wholeBodyDynamics broken on iCubGenova04

[gabrielenava]

Together with @Yeshasvitvs we tried to run the fast yoga demo on iCubGenova04. The robot fails immediately. The measured joint torques went up to a crazy value: 2000 Nm (see the picture below).

We checked the FT measurements both using Simulink and then directly from the yarp port, but we weren't able to see any issue (see the picture below):

However, the iCubGui showed crazy random forces acting on the robot (see the attached GIF):

We then suspected that for some reason wholeBodyDynamics got broken, and therefore we checked directly a wholeBodyDynamics port (/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o) and this was the result:

There are no errors in the yarplogger, and yarp, icub-main, codyco-modules, robots-configurations are in devel branch with no local changes.

[traversaro]

There are three inputs to wbd: ft sensors, joint positions and gyro/acceleroneter of the imu. Have you checked the last two?

[gabrielenava]

I checked the joint positions and there were no spikes, but I didn't check the IMU. I will check it tomorrow.

[gabrielenava]

I checked the joints and the IMU measurements. They were fine, while in the iCubGui I saw random forces as in the gif. See attached pictures.

IMU:

Joints:

However, I noticed strange error in the logger (generic motor error?). Maybe it is also worth to open an issue in icub-support. (consider the robot was position control and floating while I tested it).

[traversaro]

We tried two things with Luca:

• Not using the imu, sending the useFixedFrameAsKinematicSource command on RPC,
• using the xsens IMU.

In both case everything was working fine, so even if there are no crazy values in the bosch IMU, the issue is related in some way to the use of the bosch IMU.

[gabrielenava]

The issue seems related to the joint acceleration estimation for the neck joints. See https://github.com/robotology/icub-support/issues/622. Closing this issue.