Strange interaction among motor interfaces for multi-joints control

[pattacini]

Hi @barbalberto

I and @randaz81 spotted today a very strange interaction among motor interfaces for multi-joints control.

The following code used by the reaching controller

iposd[0]->setPositions(jointsIndexes[0].size(),jointsIndexes[0].getFirst(),&ref[0]);
iposd[1]->setPositions(jointsIndexes[1].size(),jointsIndexes[1].getFirst(),&ref[3]);
iposd[2]->setPositions(jointsIndexes[2].size(),jointsIndexes[2].getFirst(),&ref[4]);
iposd[3]->setPositions(jointsIndexes[3].size(),jointsIndexes[3].getFirst(),&ref[9]);

sends references to the following devices:

• 0 :arrow_right: torso_tripod
• 1 :arrow_right: torso (just the yaw)
• 2 :arrow_right: [left|right]_arm (only the first 5 joints)
• 3 :arrow_right: [left|right]_wrist_tripod

In this configuration all the joints but those of the wrist_tripod reach their set-points. Instead, the latter joints remain stuck to supposedly hard-coded values.

By contrast, if we rely on the modified snippet below, where basically we replace the multi-joints control with subsequent single-joint calls, everything works as expected:

iposd[0]->setPositions(jointsIndexes[0].size(),jointsIndexes[0].getFirst(),&ref[0]);
iposd[1]->setPositions(jointsIndexes[1].size(),jointsIndexes[1].getFirst(),&ref[3]);
for (size_t i=0; i<jointsIndexes[2].size(); i++)
    iposd[2]->setPosition(i,ref[4+i]);
iposd[3]->setPositions(jointsIndexes[3].size(),jointsIndexes[3].getFirst(),&ref[9]);

[pattacini]

@barbalberto fixed this via 2d5ecd0

[barbalberto]

The error was that inverse kinematic was computed (and therefore command sent to the robot) even if setPositions was called with number of joints 0. Commit fixed this behaviour also for positionMove and relativeMove.