Wrong data from gyroscopes and accelerometers

[isorrentino]

By checking data from the robot I discovered that the gyroscope and accelerometer measurements do not match the values computed from forward kinematics. In particular, all the axes have the sign inverted and the x and y axes are switched.

Here is the script I used for these plots and a dataset.

check_imu_frame.zip

The figures compare the measurements from the sensors (in blue) and the forward kinematics (in red).

While the plot on the left side shows the measurements from the sensors as they are, the one on the right shows the axes switched as:

x = -y
y = -x
z = -z

Gyroscope right foot front

Accelerometer right rear front

I did not check the other sensors.

cc @GiulioRomualdi @traversaro @Nicogene @pattacini

[traversaro]

@isorrentino I am not wrong this data was collected on the ergoCubSN000 before leaving from London and updating to the last firmware, so we are not sure which version of the firmware was running on the STRAIN2, right?

[isorrentino]

Actually, I logged the data the weekend before the robot left. So I would say the version is the same as it is now.

[traversaro]

By discussing with @isorrentino and @GiulioRomualdi, it is not 100% certain the version of firmware of STRAIN2 used in ergocub, but the most probable version is strain 2.1.0 from robotology/icub-firmware-build#77 . For some reason, it seems that the logic to rotate the gyroscope and acceleration in the FT frame implemented in:

• Strain2: remap IMU to stream wrt FT main frame robotology/icub-firmware#182
• Fix FT IMU frames conversion robotology/icub-firmware#274
• Fix FT IMU frames conversion - Stint 2 robotology/icub-firmware#275
• Fix euler angles representation for strain2 robotology/icub-firmware#286

Is not working anymore.

[pattacini]

Another relevant piece of information is:

• Fix FT-IMU frames conversion documentation#212

But how did the robot manage to walk in London?
Doesn't walking make use of IMU data? Probably not...

[traversaro]

Something that I noticed (and I do not know if it is related) is that robotology/icub-firmware#313 added a new board called strain2c . I am not sure where that board is used (a search https://github.com/search?q=repo%3Arobotology%2Frobots-configuration%20strain2c&type=code does not give any result). However, apparently that board uses a macro called STM32HAL_BOARD_STRAIN2C, but the logic to handle the different frame used by bno055 on strain2 only checks for STM32HAL_BOARD_STRAIN2, see https://github.com/robotology/icub-firmware/blob/5cf9dd1965140a5088d8fd41a41b1132a8fffe1e/emBODY/eBcode/arch-arm/embot/app/embot_app_application_theIMU.cpp#L526 .

[traversaro]

But how did the robot manage to walk in London?
Doesn't walking make use of IMU data? Probably not...

The walking mainly uses the xsens imu in the pelvis, I guess the issue is w.r.t. to an IMU mounted on an FT sensor.

@isorrentino can you share the name of the IMU that is missbehaving? Thanks!

[traversaro]

@isorrentino can you share the name of the IMU that is missbehaving? Thanks!

Found, it is the r_foot_front_ft.

[isorrentino]

@isorrentino can you share the name of the IMU that is missbehaving? Thanks!

I checked the ones mounted on the right_foot_front and the right_foot_rear ft sensors. The names should be r_foot_front_ft_acc and r_foot_rear_ft_acc. or without acc.

[GiulioRomualdi]

On the 18th of May I flashed the firmware of the 2foc as written in robotology/robots-configuration#522

I used this tag: https://github.com/robotology/icub-firmware-build/releases/tag/v1.34.1

@isorrentino collected the data on 20 may 2023 so the firmware version was already updated

[pattacini]

Something that I noticed (and I do not know if it is related) is that robotology/icub-firmware#313 added a new board called strain2c . I am not sure where that board is used (a search https://github.com/search?q=repo%3Arobotology%2Frobots-configuration%20strain2c&type=code does not give any result). However, apparently that board uses a macro called STM32HAL_BOARD_STRAIN2C, but the logic to handle the different frame used by bno055 on strain2 only checks for STM32HAL_BOARD_STRAIN2, see https://github.com/robotology/icub-firmware/blob/5cf9dd1965140a5088d8fd41a41b1132a8fffe1e/emBODY/eBcode/arch-arm/embot/app/embot_app_application_theIMU.cpp#L526 .

Probably this is the point; good catch👍🏻

cc @Nicogene @marcoaccame @davidetome

[marcoaccame]

Something that I noticed (and I do not know if it is related) is that robotology/icub-firmware#313 added a new board called strain2c . I am not sure where that board is used (a search https://github.com/search?q=repo%3Arobotology%2Frobots-configuration%20strain2c&type=code does not give any result). However, apparently that board uses a macro called STM32HAL_BOARD_STRAIN2C, but the logic to handle the different frame used by bno055 on strain2 only checks for STM32HAL_BOARD_STRAIN2, see https://github.com/robotology/icub-firmware/blob/5cf9dd1965140a5088d8fd41a41b1132a8fffe1e/emBODY/eBcode/arch-arm/embot/app/embot_app_application_theIMU.cpp#L526 .

Probably this is the point; good catch👍🏻

cc @Nicogene @marcoaccame @davidetome

i don't think that the strain2c is used yet. But yes: the rotation must be applied also if the macro STM32HAL_BOARD_STRAIN2C is defined. However, it may be even better to impose the rotation somewhere else deep inside the HW driver.

[pattacini]

However, it may be even better to impose the rotation somewhere else deep inside the HW driver.

The conversion is implemented to realign the IMU frame to the FT frame so the driver shouldn't be involved in that operation, I think, as it should be rather responsible only for dealing with the IMU itself.

[Nicogene]

The robot will be back soon and we might double-check the strain2 fw version.

Before that, we might check if the forces/accelerations/gyro are expressed in the same reference system w/ an FT on the bench.

For doing it, using https://github.com/icub-tech-iit/ft-sensors-simulink could ease the test

cc @pattacini @davidetome @marcoaccame

[pattacini]

Also, we could already patch the FW by dealing with the macro STM32HAL_BOARD_STRAIN2 as well.
We should be fairly quick in view of the distro, hence other refactorings could come later.

[davidetome]

added check for strain2c macro for IMU :

• davidetome/icub-firmware@97b5b64

Tomorrow I'll test other stuff

cc @Nicogene @marcoaccame

[Nicogene]

In the meanwhile I checked on the robot the fw version of strain2 boards

And it is the latest one (2.1.0)

cc @marcoaccame @pattacini @davidetome

[pattacini]

However, it may be even better to impose the rotation somewhere else deep inside the HW driver.

The conversion is implemented to realign the IMU frame to the FT frame so the driver shouldn't be involved in that operation, I think, as it should be rather responsible only for dealing with the IMU itself.

Just for the sake of clarity, in robotology/icub-firmware#286 we moved the conversion inside the driver, which is fine for the time being.
However, when it comes to converting accelerations, we are actually neglecting non-inertial terms. Accounting for these terms in the future cannot be delegated to the device driver.

[davidetome]

Today I checked the axis map using the Simulink model, here are the results :

F/T sensor w/ the latest FW in devel

The mapping is :

x = y
y = x
z = -z

F/T sensor w/ the latest FW in devel without the remapping

Removing the macro :

#if defined(STM32HAL_BOARD_STRAIN2)
    // In strain2 we have the P6 configuration reported in bno055 datasheet
    embot::hw::bno055::write(pImpl->config.sensor, embot::hw::bno055::Register::AXIS_MAP_CONFIG, 0x21, 5*embot::core::time1millisec);
    embot::hw::bno055::write(pImpl->config.sensor, embot::hw::bno055::Register::AXIS_MAP_SIGN, 0x07, 5*embot::core::time1millisec);
#endif

Yhe mapping is again :

x = y
y = x
z = -z

F/T sensor w/ the FW included in the v1.26.0 release

Using the FW released on 31/8/22 when the remapping was implemented

The mapping is :

x = -x
y = -y
z =  z

The code inside the macro STM32HAL_BOARD_STRAIN2 does not remap the axis at this time for some reason.

ℹ️ The difference between the 2 FW is the STM32HAL lib used and some mods in the driver. We've to deeply inspect these differences

cc @Nicogene @marcoaccame @pattacini

[Nicogene]

After a brief alignment w/ @marcoaccame and @davidetome we decided to check if we request runtime of changing the reference frame the measure changes or it is ignored in order to narrow-down the problem to eventually the startup configuration

[davidetome]

Thoi morning I tested sending the CAN message to set orientation P5 :

And the orientation of the axis changes (in this case Z axis)

cc @Nicogene @marcoaccame

[Nicogene]

We should have found the bug, we already committed the fix, we should test it first:

• strain2: fix setting of orientation at startup robotology/icub-firmware#390
• strain2 v2.2.0, strain2c v2.2.0 robotology/icub-firmware-build#89

It incorporates also this

cc @marcoaccame @pattacini @davidetome @isorrentino @traversaro

[traversaro]

Great, thanks a lot!

[traversaro]

Do you have an idea of what caused the regression and which versions of the firmware are affected?

[Nicogene]

Do you have an idea of what caused the regression and which versions of the firmware are affected?

This PR:

• FAP reading for mtb4 and mtb4c + IMU configurability over CAN robotology/icub-firmware#347

Added the possibility of configuring the IMU reference frame via CAN, and it came after

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

So the versions:

• < 2.0.15
• 2.0.16
• 2.1.0

are affected by this bug.

It was due to the fact that if not specified in the imu config message, the placement P1 was set by default, instead of keeping the same set at bootstrap (that for strain2 was P6)

[traversaro]

It was due to the fact that if not specified in the imu config message, the placement P1 was set by default, instead of keeping the same set at bootstrap (that for strain2 was P6)

Thanks, this is now clear!

[davidetome]

Orientation is now ok 👍🏻

cc @Nicogene @marcoaccame

[Nicogene]

Fixed by:

• strain2 v2.2.0, strain2c v2.2.0 robotology/icub-firmware-build#89
• strain2: fix setting of orientation at startup robotology/icub-firmware#390

[DanielePucci]

Hi all, thanks for dealing with this. I was wondering: what is the plan to update the FT sensors on the ergoCubSN000? If already done, thanks also for that

[Nicogene]

Hi @DanielePucci, I am in contact w/ @AntonioConsilvio for updating the FT of the robot since it is in Rome, worst case we will update it when it will be back

[DanielePucci]

Thanks for the feedback @Nicogene, looks a nice plan

[DanielePucci]

Cc @G-Cervettini