Head IMU returns the roll and yaw value with opposite sign

[davidegorbani]

Bug description

I was doing some tests with the robot iCubGenova09 (iRonCub-Mk3) and I was testing the IMU in the head on the robot; I noticed a weird behaviour of the IMU so investigated further and I found out that the roll and yaw angles that the IMU returns have an opposite sign with respect to the actual one. In order to prove it, as suggested by @traversaro, I calculated the orientation computed by the IMU with respect to the initial frame of the IMU and compared it with the orientation obtained using the forward kinematics.
Since the IMU returns the relative orientation between the IMU frame and a fixed frame A, first I computed the rotation matrix ${^{A}}R_{IMU_0}$, that is the rotation matrix between the IMU frame at the initial time and the IMU fixed frame, and I used this to compute the rotation matrix between the frame of the IMU at each time instant and the IMU frame at the initial time as:
$${^{IMU_0}}R_{IMU} = ({^{A}}R_{IMU_0})^{T} * {^{A}}R_{IMU}$$
Then I used the forward kinematics to compute the same rotation matrix $^{IMU_0} R _{IMU} ^{FK}$ as:

$${^{IMU_0}}R_{IMU} ^{FK} = ({^{B}}R_{IMU_0})^{T} * {^{B}}R_{IMU}$$

where ${^{B}}R_{IMU}$ is the rotation matrix between the IMU frame and the root_link and the ${^{B}}R_{IMU_0}$ is the rotation matrix between the IMU frame and the root_link at the initial time.

This is the plot comparing the roll, pitch and yaw obtained using the head IMU and the forward kinematics and, as it possible to notice, the roll and yaw have different signs.

Calculating the rotation matrix ${^{A}}R_{IMU}$ changing the sign of the roll and yaw, the orientations are very similar.

CC @HosameldinMohamed @gabrielenava

Steps to reproduce

In order to reproduce the bug it is possible to use the following dataset that I collected rotating the neck joint of the robot and check the plot generated by the MATLAB script in https://gist.github.com/davidegorbani/8eb592db29d2c046d36682cc6cd889b9.

The dataset and the urdf model are:
dataset.zip

Expected behavior

No response

Example repository

No response

Additional context

No response

[traversaro]

@davidegorbani can you report the following information:

• Version of the rfe firmware loaded on the robot
• Version of icub-main
• Version of YARP

Thanks!

Furthermore, I think there are two possible source of error:

• The location of the head_imu frame in the URDF
• The value returned by the RPY values

To disambiguate what is the source of the error, we can try to plot the reading of the accelerometer, multiplied by ${^{IMU_0}}R_{IMU}$ and by ${^{IMU_0}}R_{IMU}^{FK}$ . Beside the effects of accelerations, the value of the gravity acceleration expressed in a constant frame should be constant. Plotting this should tell us what is the problem:

• If the accelerometer measurement multipled by ${^{IMU_0}}R_{IMU}$ is not constant, probably the problem are the RPY values returned by the IMU
• If the accelerometer measurement multipled by ${^{IMU_0}}R_{IMU}^{FK}$ is not constant, instead the error is probably is on the frame in the URDF model
• If both are not constant, then we can't say anything

[traversaro]

fyi @Nicogene @pattacini

[pattacini]

Thanks for reporting this @davidegorbani
Could you also provide the version of the software involved?
Most likely, we'd need the distro release running on the head and the application release of the RFE board

To get to know it, you may peruse the brand-new instructions (thanks to @davidetome):

• https://icub-tech-iit.github.io/documentation/icub_firmware/firmwareupdater/firmwareupdater/

[davidegorbani]

The version of the rfe firmware is the 1.3.0 while the version of YARP is the 3.7.

[davidegorbani]

To disambiguate what is the source of the error, we can try to plot the reading of the accelerometer, multiplied by

I did the test that you suggested and those are plots that I got:

The first one is the plot of the acceleration vector multiplied by ${^{IMU_0}}R_{IMU}$ while in the second one the acceleration vector is multiplied by ${^{IMU_0}}R_{IMU}^{FK}$.
Then I multiplied the acceleration vector by ${^{IMU_0}}R_{IMU}$ created inverting the sign of the roll and yaw and this is the result:

[traversaro]

So it seems that the frame position in the URDF is correct, while it is the roll pitch yaw value returned by the IMU that is wrong.

[davidegorbani]

I would like to add that before updating the firmware version of the rfe, using the 1.2.0 version, I did the same test and in that case, the IMU was returning the yaw instead of the roll with the wrong sign and vice versa.

[davidetome]

As a starting point, I checked the accelerometer measurements by placing an RFE_MASTER like it's mounted on the robot (thanks @Mick3Lozzo )

(The MPU9250 IMU is not used)

The readings on the 3 axes are coherent (-9.6 on axes pointing down)

As suggested by @Nicogene There is the possibility to select the convention Android or Windows (default) but it affects only the pitch as documented in the BNO055 datasheet

And Windows is default as stated in this FW comment but in YARP we have another and so it's remapped

cc @marcoaccame @Nicogene @pattacini

[davidetome]

Today I got aligned with/ @Nicogene and we have reconstruted history.

Starting from these issues :

• The embObjIMU device output the RPY data in the wrong order #701
• Strain2: remap IMU to stream wrt FT main frame icub-firmware#182

there were problems w/ the signs in the Euler and acc on the strain (the RFE was not used)

It was fixed, setting the right placement and the manual remapping was removed :

• Fix euler angles representation for strain2 icub-firmware#286

So now we have to proceed by fixing the actual Euler conversion for the RFE and by setting the one for the STRAIN2/STRAIN2C putting an ifdef in the FW.

To do that we need to understand which is the right Euler conversion on the SRAIN2/C and the putting them in the FW.

cc @Nicogene @pattacini @marcoaccame

[davidetome]

Today I made some tests to compare the STRAIN2 and RFE.

I removed the transform for the FT sensor/STRAIN2 alignment and compared the behaviour of the IMU on the two boards with respect to the Datasheet

The behaviour is identical and correct in both (@Nicogene we can test together as double check)

22.09.2023_17.14.36_REC.mp4

Moving the axes as described above the readings increase

So I modified the FW in order to fix the Euler conversion in the RFE :

• robotology/icub-firmware@devel...davidetome:icub-firmware:devel

@davidegorbani , It may be worth if you can test the mod and give feedback 👍🏻

So please, find attached binary to flash on the RFE before testing :

rfe.v183.zip

[davidegorbani]

Now the robot is broken, but I will test it as soon as it is fixed; thank you!

[davidetome]

Ciao @davidegorbani ! Did you have the chance to test it?

[davidegorbani]

Hello,
unfortunately, the robot is not fixed so I could not test it but as soon it is available I will test it.

[davidetome]

Hello,
unfortunately, the robot is not fixed so I could not test it but as soon it is available I will test it.

Thank you for the update! 👍🏻

[davidegorbani]

Hello @davidetome, the robot is now fixed but today we have to do experiments so I will not be able to test the new firmware; I will test it next week.

[davidegorbani]

Hi @davidetome, this morning I tested the new firmware and here are the plots of the test that I did:

From the comparison between the roll, pitch and yaw angles estimated using the head IMU and the FK we can see that they are the same, apart from a jump on the roll and yaw that I think is due to the conversion from rotation matrix to Euler angles; in fact, from the plot of the test proposed here; the values of the acceleration along the x, y and z components are constant.
Thank you for your support!

CC @DanielePucci

[Nicogene]

That's great @davidegorbani!
We will check then if this patch is okay for the strain2 firmware or (more probably) we will add a #ifdef and will deliver the PRs in icub-firmware and icub-firmware-build

cc @pattacini @davidetome

[davidetome]

Thank you @davidegorbani for your feedback!
Today w/ @Nicogene we checked the head frame in Gazebo to check the Euler angles behaviour concerning the axes' rotation

We deduced that readings increase with the following :

Axis	Rotation
Roll (x)	anticlockwise
Pitch (y)	clockwise
Yaw (z)	anticlockwise

This is coherent with the fix I made in the FW for the RFE/MTB4/MTB4C, then we applied the same convention to the STRAIN2/STRAIN2C, this is the related draft PR:

• fix euler angles icub-firmware#425

PR will be merged and new binaries produced when we get positive feedback from this issue

cc @Nicogene @pattacini @marcoaccame @sgiraz

[davidetome]

The fix is done w/ these PRs :

• fix euler angles icub-firmware#425
• rfe v1.4.0 , mtb4 v1.22.0 , mtb4c v2.2.0 , strain2 v2.3.0 , strain2c v2.3.0 icub-firmware-build#108

cc @Nicogene @pattacini @marco-monforte @davidegorbani @GiulioRomualdi @G-Cervettini