Issues with Cartesian interface

[paliasgh]

Hello,

I have a question regarding sending the robot's arms into a given position. Let's say a code has these lines in a function in order to set the end-effector position of the robot to x and orientation to o:

        drvArmL.view(iarm);
        iarm->getDOF(curDOF);
        newDOF.resize(3);
        newDOF[0] = 0;
        newDOF[1] = 0;
        newDOF[2] = 0;
        iarm->setDOF(newDOF, curDOF);
        iarm->setPosePriority("orientation");
        iarm->goToPoseSync(x,o);
        iarm->setTrajTime(1.5);

What happens when I run this function (called in response to rpc service request) is that the robot tries to reach the position, but it will not fully get there. It gets close though. If I run the function again, it gets closer. In this video, I am running the function a number of times so that the arm finally goes into the desired position:

1.3.mov

What should I do to make goToPoseSync() try its best, even if it takes time, to reach the desired position accurately?

I am also configuring this at the beginning:

iarm->setInTargetTol(0.001);

Thank you for your help!

[pattacini]

Hi @paliasgh

Here's my suggestions:

drvArmL.view(iarm);

// better off complying with the proper size of DOF
Vector curDOF;
iarm->getDOF(curDOF);
auto newDOF = curDOF;
newDOF = 1;  // ensure that all DOFs are enabled to then tune them up below

// iCub has a narrow reachable space because it's a baby; hence, let's use the torso too
newDOF[0] = 1;  // enable torso pitch
newDOF[1] = 0;  // torso roll is usually not required
newDOF[2] = 1;  // enable torso yaw
iarm->setDOF(newDOF, curDOF);

// reaching with prioritized position (default) is always the best option for us, especially when grasping is considered
// the solver will strive to comply with the target position and then will adapt to reduce the orientation error
// the other way around makes sense only in very specific conditions
iarm->setPosePriority("position");

// set the reactivity of the controller before employing it
iarm->setTrajTime(1.5);

iarm->goToPoseSync(x,o);

Somewhat related discussion:

• About the orientation in cartesian space control #665

[paliasgh]

Hello @pattacini ,

Thanks for the advise! With just setting setPosePriority("position") the system now works much better.