Yarp: Method yarp::dev::IPositionControl::setRefSpeeds produce zero reference speeds

[xEnVrE]

Describe the bug

A call to the method yarp::dev::IPositionControl::setRefSpeeds(const int n_joint, const int* joints, const double* spds) produce zero reference speeds when the actual trajectory is executed after calling yarp::dev::IPositionControl::positionMove().

To Reproduce

The following code produce the described behavior. The behavior was discovered using the robot iCubGenova01 while trying to move the right hand finger joints.

imod is of type yarp::dev::IControlMode*

ipos is of type yarp::dev::IPositionControl*

joints is of type yarp::sig::VectorOf<int> and contains the indexes of the joints

ref_speeds is of type yarp::sig::Vector and contains the desired reference speeds

des_joints is of type yarp::sig::Vector and contains the desired joints positions
```c++
for (std::size_t i = 0; i < joints.size(); i++)
imod->setControlMode(joints[i], VOCAB_CM_POSITION);

ipos->setRefSpeeds(joints.size(), joints.getFirst(), ref_speeds.data());

ipos->positionMove(joints.size(), joints.getFirst(), des_joints.data());
```

The wrong behavior is clearly visible by looking at the yarpmotorgui while executing the code. The reference velocity sliders of all the involved joints go to zero and the label change color from green to orange.

Please note that the single joint version yarp::dev::IPositionControl::setRefSpeed, instead, is working properly.

Configuration:

I report the current configuration used on the iCubGenova01 laptop:

• OS: Ubuntu 16.04
• yarp version: v. 3.1.0, link to commit
• icub-main: link to commit

Additional context

Two modules within icub-main are using the setRefSpeeds method:

• the iKinGazeCtrl (see here) in order to produce saccadic eye movements
• the cptService module (see here)

I am also sure about the fact that, during a recent demo on iCubGenova01, the movements produced by the cptService were wrong probably due to the same issue. Also in that case, the velocity sliders were going to zero in the yarpmotorgui.

@pattacini @vtikha

[pattacini]

Nice catch @xEnVrE ✨

[xEnVrE]

These https://github.com/robotology/yarp/commit/a35017a0abec8a04d443f9874b5d617e4bd55cbd#diff-1121c589014f53c4c3726705f8b996f7L182 are the changes on setReferenceSpeeds I was talking about

cc @randaz81 @vtikha

[barbalberto]

Were you using a CAN or ETH based icub? This info may be useful for debug.

[xEnVrE]

If I am not wrong, iCubGenova01 is CAN based.

[barbalberto]

Ok. It'll be great to do the same test on a Eth based iCub.
This will help us to discriminate between a YARP or can-device implementation issue.
Do you think you can manage to do it?

[randaz81]

It would be nice to do the check suggested by @barbalberto and eventually fix this bug BEFORE the imminent yarp/icub-main release...

[xEnVrE]

Tried this morning on both iCubGenova01 (CAN based) and iCubGenova02 (i.e. the purple one, ETH based).

Results:

• setRefSpeed (singular version) works on both
• setRefSpeeds (plural version) works only on the ETH based iCub.

cc @barbalberto

[barbalberto]

Thanks, I'll check

[barbalberto]

I saw the code on the iCubGenova01 is quite old actually.
The source is on the commit posted by @xEnVrE , but when I try to make it does not compile and cmake says thah un update of icub-firmware-shared (source) is required.
So the executable actually running on the iCub is older and I don't know how much.

I see an old fix done by @randaz81 that may be related to this issue, so my suggestion is to update everything on the iCub and redo the test. This may trigger a firmware update (or maybe not, I don't know) so I'd like to schedule half a day for this update and test.

I see that robot is quite busy these days, any suggestion to schedule this update?
@vtikha @xEnVrE @pattacini

[barbalberto]

I found the reason of this bug, and I think I found another one in the getTemperature function.
I'll check better tomorrow morning with @valegagge and then we'll do the PR

[xEnVrE]

Want to add that after the change made by @barbalberto, now iCubGenova01 shutdowns gracefully, i.e. no random hanging arms anymore.

[barbalberto]

PR opened https://github.com/robotology/yarp/pull/2033

Please check

[drdanz]

Fixed by 2033