ik failed: max iteration reached (error = XXXXX > 0.000100)

[torydebra]

@lucarossini-iit, digging out I found out that the problem is about how the error is computed:

bool PositionCartesianSolver::solve()
{
    // allocate variables
    Eigen::VectorXd q, qcurr, dq, error;

    // initial q
    _model->getJointPosition(qcurr);

    // initial cost
    getError(error);
    ...

Indeed, the q of the PositionCartesianSolver::_model is correctly in the final position (as shown by the yellow nspg marker). But, getError looks among the error of the cartesian tasks inside the _task_map:

void PositionCartesianSolver::getError(Eigen::VectorXd& error) const
{
    error.setZero(_n_task);

    int error_idx = 0;
    for(auto pair : _task_map)
    {
        auto t = pair.second;
        t->update(*_model); // tbd: optimize

        error.segment(error_idx, t->size) = t->error;
        error_idx += t->size;
    }
}

This _task_map is the problem: in the t->update(*_model); // tbd: optimize:

void PositionCartesianSolver::CartesianTaskData::update(XBot::ModelInterface& model)
{
    J.setZero(size, model.getJointNum());
    error.setZero(size);

    /* If task was disabled, error and jacobian are zero */
    auto active = task->getActivationState();

    if(active == ActivationState::Disabled)
    {
        return;
    }

    /* Error computation */
    Eigen::Affine3d T, Tdes;
    ctask->getCurrentPose(T);
    ctask->getPoseReference(Tdes);

    std::cout << "CartesianTaskData current pose " << T.translation().transpose() << std::endl;
    std::cout << "CartesianTaskData desired pose " << Tdes.translation().transpose() << std::endl;
    ...

there is this ctask->getCurrentPose(T) that takes the pose of the real robot (which still needs to move), instead of the pose of the nspg-yellow robot, e.g.:

CartesianTaskData current pose  0.571033 0.0688122  0.118524
CartesianTaskData desired pose 0.379009 0.455405 0.132772

Indeed the error XXXXX of the issue title is exactly the error between the tcp of the real robot and the given goal.

So, it seems that the CartesianTaskData inside the _task_map are not using the correct model of the ci, but instead the real robot.

[lucarossini-iit]

This is weird because the only ModelInterface::Ptr in the PostitionCartesianSolver comes with the ci given as argument in the constructor and the NSPG uses the same ModelInterface. Also, in the case you only have CartesianTasks in the first layer of the Cartesio stack, _task_map and _ctask_map should be exactly the same as you see in the constructor:

for(auto t : ci->getIkProblem().getTask(0))
    {
        if(auto cart = std::dynamic_pointer_cast<Cartesian::CartesianTask>(t))
        {
            auto tdata = std::make_shared<CartesianTaskData>(cart);

            _n_task += tdata->size;

            _task_map[cart->getName()] = tdata;
            _ctask_map[cart->getName()] = tdata;

            printf("[PositionCartesianSolver] adding cartesian task '%s' to '%s', size is %d \n",
                   cart->getBaseLink().c_str(),
                   cart->getDistalLink().c_str(),
                   tdata->size);
        }
    }

meaning that you should have the same behavior computing the error both from _ctask_map and _task_map.

[torydebra]

meaning that you should have the same behavior computing the error both from _ctask_map and _task_map.

Yes sorry I was wrong about this, the behaviour is the same (indeed I deleted that part on my comment).

---

About the problem, it could be that, since CartesianInterface is involved, in some parts CartesianInterface reads robot state from xbotcore and/or rostopic instead of using the model?

[lucarossini-iit]

Unless you have a callback that updates the model from the xbotcore ros topics I don't think this can be the case. Also, if I remeber correctly, you created a different model for the ValidityCheckContext and the PositionCartesianSolver that should not be updated by the RobotInterface. Is it correct?

[torydebra]

Is it correct?

Exactly:

ValidityCheckContext creation:

_model_validity_pred = XBot::ModelInterface::getModel(cfg);
_model_validity_pred->syncFrom(*_robot);
_vc_context = std::make_unique<XBot::Cartesian::Planning::ValidityCheckContext>(cartesio_planner_config, _model_validity_pred, *_nh);
auto validity_predicate = [this](const Eigen::VectorXd& q)
{
  _model_validity_pred->chain("arm").setJointPosition(q);
  _model_validity_pred->update();
  return _vc_context->vc_aggregate.checkAll();
};

_planner->setStateValidityPredicate(validity_predicate);

PositionCartesianSolver:

auto ctx_planner = std::make_shared<XBot::Cartesian::Context>(std::make_shared<XBot::Cartesian::Parameters>(_period),_model_validity_pred);
_ci_solver_position_cart = XBot::Cartesian::CartesianInterfaceImpl::MakeInstance("OpenSot",pb,ctx_planner);
_ik_solver = std::make_shared<XBot::Cartesian::Planning::PositionCartesianSolver>(_ci_solver_position_cart);
_ik_solver->setMinStepSize(0.6);
_nspg = std::make_shared<XBot::Cartesian::Planning::NSPG>(_ik_solver, *_vc_context);

The _model_validity_pred is updated with the real robot state before solving the ik:

//probably these two are the same, I did both just to be sure
_ik_solver->getModel()->setJointPosition(_js[0]);
_model_validity_pred->setJointPosition(_js[0]);

_ik_solver->getModel()->update();
_ik_solver->setDesiredPose("ArmCartesian", _target_pose);

...

Eigen::VectorXd planning_qgoal;
_nspg->sample(3)

and after solving, but probably these are useless, since from the _ik_solver is using _model_validity_pred right?

_ik_solver->getModel()->getJointPosition(planning_qgoal);

//probably these are useless, since from the _ik_solver is using _model_validity_pred right?
_model_validity_pred->setJointPosition(planning_qgoal);
_model_validity_pred->update();

[lucarossini-iit]

Yes, the point is that someone is resetting the joint position of the _model_validity_pred to the start position (or the current robot position) before the PositionCartesianSolver checks the Cartesian error. We should investigate this finding the issue.

[torydebra]

Since this problem:

there is this ctask->getCurrentPose(T) that takes the pose of the real robot (which still needs to move), instead of the pose of the nspg-yellow robot, e.g.:

For now I am solving getting the current pose from the given model directly:

//ctask->getCurrentPose(T);
model.getPose(ctask->getDistalLink(), ctask->getBaseLink(), T);