iCubGenova09 – Jumps on the estimated yaw angle provided by the robot IMU

[GiulioRomualdi]

Device name 🤖

iCubGenova09

Request/Failure description

I collected the orientation data coming from the robot imu mounted on the FTs and I noticed some jumps on the yaw angle. This may be related to #1485.

Detailed context

We collected the data as follows:

1. we put the robot on the pole
2. We collected the data using the multianalogsensor interface

Let us now define ${}^0 R_{imu}$ the orientation coming from the IMU w.r.t. the IMU inertial frame (denoted with 0). Let us now introduce $I$ the inertial frame considered by the kinematics. We would like to compare ${}^I R _ {imu}$ with $\prescript{I}{}{R} _ {expected}$ that is computed by the forward kinematics. ${} ^ I R_{imu}$ is given by $${}^I R_{imu} = {}^I R_{0} {}^0 R _{imu}$$
where ${}^I R _{0}$ is computed using the first readout of the imu as
$${}^I R _{0} = {}^I R _{imu}(0) {}^0 R _{imu}(0)^\top$$

We collected two datasets:

1. The joints move from the homing configuration to a configuration where all leg joint positions are equal to 0. The associated dataset is called 2023_01_12_13_16_07.mat
2. I set the leg joints configuration to 0 then I put the hip roll and pitch joints in idle so I can manually move the leg. This experiment (suggested by @traversaro) allows us to have the pitch and roll angle of the IMU in the legs equal to the one on the foot. The associated dataset is called 2023_01_18_16_39_51.mat

Both the datasets can be found here: datasets.zip

What follows are the plots associated with the experiment (1) (dataset 2023_01_12_13_16_07.mat)

r_leg	r_foot_front	r_foot_rear
l_leg	l_foot_front	l_foot_rear

As you can notice in the left leg the yaw angle jumps several times, as far as I can notice in the following plot (please notice that is the same plot as the l_leg plot in the above table with some labels) the jumps are not due the wrapping of the angle between (-180 +180 or 0 360). On the other hand, the roll angle jumps between +180 to -180 (and this is expected)

Moreover as far as I know at the instant in which the yaw jumps we are not in a gimbal lock situation.

Since the strange jumps seem to appear only on the yaw axis we decide to substitute the yaw angle with the one provided by the forward kinematics. So I defined
$${}^I R_{imu, fk} = R_{z}^{fk} R_{y}^{imu} R_{x}^{imu}$$ then I compared ${}^I R _ {imu}$ and ${}^I R_{imu, fk}$ with $\prescript{I}{}{R} _ {expected}$, as you can notice in the following videos when the yaw angle is forced to be the one provided by the forward kinematics the two frames almost coincide.

Videos of the dataset 2023_01_12_13_16_07.mat

	IMU	IMU w/ yaw correction
r_leg	out.mp4	out.mp4
r_foot_front	out.mp4	out.mp4
r_foot_rear	out.mp4	out.mp4
l_leg	out.mp4	out.mp4

Videos of the dataset 2023_01_18_16_39_51.mat

	IMU	IMU w/ yaw correction
r_leg	out.mp4	out.mp4
r_foot_front	out.mp4	out.mp4
r_foot_rear	out.mp4	out.mp4

⚠️ I was wondering if this behavior is connected to #1485 given the fact that the magnetometer value is mostly used to detect the yaw.

Additional context

Similarly to #1485 I opened the issue in tech-support but I don't think it is an issue specifically related to the iCubGenova09.

cc @pattacini @traversaro @G-Cervettini

How does it affect you?

No response

[traversaro]

Apparently the issue is happening just on FT58, and not on FT45 . Did you checked which version of firmware is loaded on these devices?

[traversaro]

Another thing that I think could be interesting is to plot in the same figure the yaw from the sensor and the one from the kinematics.

[GiulioRomualdi]

Another thing that I think could be interesting is to plot in the same figure the yaw from the sensor and the one from the kinematics.

Hi @traversaro as you suggested I show you the plots of the rpy angle provided by the IK and the rpy extracted by ${} ^ I R_{imu}$
⚠️ ⚠️ As discussed in #1487 (comment) ${}^I R _ {imu}$ is not directly provided by the imu but is given by
$${}^I R_{imu} = {}^I R_{0} {}^0 R _{imu}$$
where ${}^I R _{0}$ gives us the possibility to align the orientation provided by the IMU to the FK orientation at the initial time instant.

Here are the plots, as you can notice the roll and pitch angles of the IMU follow the one evaluated by the FK. On the other hand, there is a mismatch regarding the yaw. In details on the leg imus (FT58), there are some jumps, while on the feet imus (FT45) the signal seems to be smooth but once the $yaw_{FK}$ increases the $yaw_{IMU}$ decreases and vice-versa.

r_leg	r_foot_front	r_foot_rear
l_leg	l_foot_front	l_foot_rear

[traversaro]

Thanks, interestingly, also the differences in the yaw of foot sensors are quite strange. How are you going from homogenous matrices to RPY in the case of the forward kinematics?

[traversaro]

By the way, sorry for the random suggestsions, but if the joints after the leg FT sensor and before the foot are constant, another thing that it would be interested to check is to plot {}^{l_foot_front} R_{l_foot_rear} and {}^{l_foot_front} R_{l_leg} . They should be constant, but if they are not constant something wrong is going on.

[AntonioConsilvio]

Hi @traversaro @GiulioRomualdi, we are organizing to address this issue in early February.

cc @maggia80 @Fabrizio69 @pattacini

[pattacini]

/remind February 06

[octo-reminder]

⏰ Reminder
Monday, February 6, 2023 10:00 AM (GMT+01:00)

[octo-reminder]

🔔 @pattacini

[pattacini]

Just planned for this sprint!

[Nicogene]

We noticed similar jumps on yaw also inspecting the eul data with reading from the simulink model

Looking here it seems that it is due to magnetometer data not reliable, then is is somehow related to #1485

[Nicogene]

Alongside @mfussi66, we managed to use ft-sensor-simulink for reading imu data from the rfe board. The magnetometer does not stream all zeros, and on the yaw of the Euler angle, we do not have unexpected jumps.

Here is the plot of the Euler angles:

[Nicogene]

As we supposed fixing the magnetometer has fixed also the jumps in the Euler angles, see the plots above:

Thanks @marcoaccame!

Euler angles

strain2

mtb4

cc @GiulioRomualdi

[GiulioRomualdi]

That's wonderful! This activity was pivotal for enabling the usage of the feet imu in the walking infrastructure! Thank you, really!

Cc @G-Cervettini @DanielePucci

[DanielePucci]

Super cool, thanks @Nicogene @marcoaccame

[Nicogene]

The fix has been delivered also in terms of binaries:

• strain2 v2.1.0, strain2c v2.1.0, mtb4 v1.21.0, mtb4c v2.1.0 icub-firmware-build#77

I think we can close the issue 🚀

cc @GiulioRomualdi