Joint velocities published on yarp port (stateExt:o) are always 0

[nunoguedelha]

Description of the failure

In position or torque control, while we are moving any joints, we read the joint velocities (joint encoders and motor encoders) published on the yarp port stateExt:o: they happen to be always set to 0.

Detailed conditions and logs of the failure

The issue has been observed on iCubGenova02 and iCubGenova04, on both legs, arms and torso, but not on the head. The head joint velocities look correct. Example log on iCubGenova04 :

23513 1487696887.784260 
(78.574411 29.685131 0.725099 -79.76643 0.197754 19.720507) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] 
(10617.213421 -2735.602382 85.693569 -6920.304981 -112.610138 -1974.797302) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] 
...

The oldest log (checked for now) displaying the issue was taken in february on iCubGenova04. The most recent log showing correct joint velocities was taken last year on the iCub Lumix.

[traversaro]

cc @valegagge

[jeljaik]

Hello guys (@valegagge)

Me and @rlober will be in IIT until tomorrow when we head back to Paris and we would greatly benefit from this data for our experiments. Is it possible to get any urgent assistance on this? In particular on iCubGenova02.

Thanks!

[jeljaik]

This is what I get on iCubGenova02 for the head from /icub/head/stateExt:o

(0.406495 0.175782 0.087891 0.0 0.0 0.0) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] (29.185252 -52.179693 -8.239766 58.090352 0.274659 0.0) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] (0.349231 0.025443 0.000149 0.0 0.0 0.0) [ok] (-45.0 110.0 -42.0 189.0 -317.0 61.0) [ok] (7565168 7565168 7565168 7565168 7565168 7565168) [ok] (1718187123 1718187123 1718187123 1718187123 1718187123 1718187123) [ok]

Torso:

(-0.186768 -0.461427 -0.780031) [ok] (0.0 0.0 0.0) [ok] (0.0 0.0 0.0) [ok] (-17.578168 -2136.296719 1671.573905) [ok] (0.0 0.0 0.0) [ok] (0.0 0.0 0.0) [ok] (0.052504 -0.299326 -13.483865) [ok] (664.0 -256.0 1134.0) [ok] (7565168 7565168 7565168) [ok] (1718187123 1718187123 1718187123) [ok]

And left leg:

(83.446859 12.612335 0.005493 -99.212278 -14.69425 -0.142823) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] (-12505.218519 860.231592 3.295906 10062.402496 -1439.212498 -2.746589) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] (0.0 0.0 0.0 0.0 0.0 0.0) [ok] (8.131706 0.091345 -0.563777 -1.492777 -0.137983 0.061434) [ok] (-1288.0 -6.0 510.0 685.0 316.0 205.0) [ok] (7565168 7565168 7565168 7565168 7565168 7565168) [ok] (1718187123 1718187123 1718187123 1718187123 1718187123 1718187123) [ok]

[marcoaccame]

I have done some tests on iCubGenova04: even if we move a joint w/ yarpmotorgui the ETH board reports 0 velocity (and yes, it reports the changed position).

That confirms what written and is a starting point for solving the issue.

Details: I print the value of the velocity (and position) of joint 0 of right arm as soon as the UDP packet received from the ETH board is processed inside embObjMotionControl::update() which is directly called by the thread receiving the UDP packet.

Next: @ale-git will try a test FW of the EMS which regularly sends a debug message with the measured velocity.

[barbalberto]

Just made a quick test with Gazebo and this is the output, so apparently it works wih the simulator

(-0.013599 0.005353 0.004668 0.002404 0.000629 0.000235 0.001266 0.000671 -0.00022) [ok] 0.083966 0.034503 -0.055049 0.201073 -0.017514 -0.001577 -0.020586 -0.023794 -0.038042) [ok] (0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0) [ok]  ...

What it looks strange is that both the head (which is CAN) and leg (which is ETH) don't work.

A critical point we may investigate is the "ImplementEncodersTimed" which is used by all can/eth devices, but not by the simulator.

I'd put a print in the getEncoderSpeed (and maybe also getEncoderSpeedWS) in this file and see what's going on before and after conversion to degrees.

[jeljaik]

Yes!! On simulation it works! Which is exactly why I wasn't worried about this until I tried on the robot 😅 THANKS! 🙏🏽

[traversaro]

If I remember correctly for CAN the default behaviour is not publishing the encoder velocities, unless a specific parameter is added to the configuration files.

[marcoaccame]

In the ETH robot the status of the joints and motors is always transmitted from the ETH board to the PC104. And we have verified that the EMS board sends up zero values for the velocities of both joint and motor despite the joint moves. We are now looking at the FW code to understand what is wrong.

[ale-git]

The s_eo_motioncontrol_updatePositionFromEncoder(uint8_t index, eOencoderreader_valueInfo_t *encoder) function was calling MController_update_motor_pos_fbk(index, (int32_t)encoder->value[0]); also for 2FOC based control board, but this is wrong because the data from 2FOC boards are overwritten in this way. I've put a check in the Motor_update_pos_fbk(Motor* o, int32_t position_raw) so that it does nothing when called for a 2FOC based control board . Maybe a more elegant way should be adopted later.

https://github.com/robotology/icub-firmware/commit/883d1b8c2fb097d3c260600839291b98dc866d9d

[jeljaik]

Grazie @ale-git and @marcoaccame! you guys rock.

[marcoaccame]

Well done @ale-git. And good experiments for you now, @jeljaik

[traversaro]

What is the status of this issue? It affects only the robots in IIT that are using the software in the devel branch or also external robots?

@S-Dafarra

[S-Dafarra]

If I'm not wrong, @valegagge should have solved this issue definitely (at least on IIT robots), but I would ask for a confirmation from her.

[valegagge]

Yes, I did, but since no robots were available I could not verify it entirely.

[valegagge]

I verified that new fw releases fix the bug. see valegagge/icub-firmware-build@73fe2c1dc2fdd86fcfb4f17a0a2ed4d9bfe2e228

[traversaro]

Hi @valegagge, so the bug is not affecting the robot using the the master branch of icub-main/icub-firmware, right?

[valegagge]

Yes, it shouldn't. @traversaro Did you experice of it ?