Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ergoCub 1.0 S/N:000 – Inconsistency in the orientation data between head IMU (assumed to be correct) and feet IMUs (possibly wrong) #1649

Closed
G-Cervettini opened this issue Oct 4, 2023 · 18 comments
Closed

ergoCub 1.0 S/N:000 – Inconsistency in the orientation data between head IMU (assumed to be correct) and feet IMUs (possibly wrong) #1649

G-Cervettini opened this issue Oct 4, 2023 · 18 comments
Assignees
@davidetome @sgiraz @AntonioConsilvio
Labels
ergoCub 1.0 S/N:000 ergoCub robot (prototype)

Comments

@G-Cervettini
Copy link

G-Cervettini commented Oct 4, 2023 •
edited
Loading

Robot Name 🤖

ergoCub 1.0 S/N:000

Request/Failure description

Misalignment between the expected IMU rotation and measured one, in a similar way as shown in the issues https://github.com/icub-tech-iit/task-force-miscellanea/issues/126 and icub-tech-iit/ergocub-software#129.

Detailed context

Yesterday with @GiulioRomualdi and @DanielePucci we tested my imu-based floating base estimator on the real robot for the first time and the data stream from right foot IMUs showed an unexpected behavior.
The estimator is supposed to track the robot pose changes based on IMU data and it is working in simulation.
Here the behavior of the estimator in yesterday's test on the robot

NO CORRECTION:

2023_10_03_14_13_34_NoCorrection.mp4

Having a look at this result, it looked like Roll and Pitch were switched wile Yaw had the wrong sign. Thus, I tried to apply this correction to imu data and the result matched much better the expected behavior

CORRECTED:

2023_10_03_14_10_57_Corrected.mp4

Thus, I'm involving icub-tech because there may be something to fix in the F/T firmware regarding RPY data.
In my estimator I'm using both the right foot IMUs data and applying the iDynTree::geodesicL2MeanRotation() on them.

With @GiulioRomualdi we also tried to consider the front and rear sensors separately and they seem to behave differently from each other:

front_vs_rear.mp4

In the previous video the root link imu data are used as ground-truth and the FK-propagated right foot orientation (thick axes) is compared with raw data from front and rear foot IMUs (thin axes).
In particular the front imu seems to show a much bigger error than the rear one.
Thanks in advance for the support!

Additional context

Here the mat files of the logged data:
03-Oct-2023_Feet_IMUs_Base_Estimation.zip

How does it affect you?

I am working on a imu-based floating base estimation algorithm and I need IMUs data to be consistent for my code to behave as expected.
@github-actions github-actions bot changed the title Inconsistency in the orientation data between head IMU (assumed to be correct) and feet IMUs (possibly wrong) ergoCub 1.0 S/N:000 – Inconsistency in the orientation data between head IMU (assumed to be correct) and feet IMUs (possibly wrong) Oct 4, 2023
@github-actions github-actions bot added the ergoCub 1.0 S/N:000 ergoCub robot (prototype) label Oct 4, 2023
@DanielePucci
Copy link

CC @Nicogene @maggia80

@Nicogene
Copy link
Member

Nicogene commented Oct 5, 2023

Hi @G-Cervettini,

analyzing this issue

We discovered that when we fixed the measurement frame for the accelerometer gyro etc. changing the placement option of the chip, we forgot to consider that the euler representation we use in YARP is different from any orientation definition that imu bosch chip supports (windows or android standard).

For this reason, we need to check both the mtb4, strain2, and rfe and apply for each of them a transformation that allows us to correct the Euler angles.

The fix for the rfe should be already done by @davidetome, as soon as we receive positive feedback we will move to the other boards.

cc @pattacini @marcoaccame

@davidetome davidetome mentioned this issue Oct 11, 2023
Closed
@davidetome
Copy link

davidetome commented Oct 11, 2023 •
edited
Loading

Ciao @G-Cervettini !

after this fix, we also fixed the STRAIN2/STRAIN2C and tested on the desk; can you please flash attached binaries in the RFE and in the STRAIN2 (or STRAIN2C) and test again?

binaries.zip

We are waiting for your kind positive feedback to merge this PR

cc @Nicogene @marcoaccame @sgiraz

@G-Cervettini
Copy link
Author

Ciao @davidetome, thanks for your fix! I will test again and update you asap.
Once the PR is merged both rfe and strain2 should be ok. What about mtb4 instead, as @Nicogene was mentioning?

For this reason, we need to check both the mtb4, strain2, and rfe and apply for each of them a transformation that allows us to correct the Euler angles.

@davidetome
Copy link

davidetome commented Oct 11, 2023 •
edited
Loading

Ciao @davidetome, thanks for your fix! I will test again and update you asap. Once the PR is merged both rfe and strain2 should be ok. What about mtb4 instead, as @Nicogene was mentioning?

For this reason, we need to check both the mtb4, strain2, and rfe and apply for each of them a transformation that allows us to correct the Euler angles.

MTB4 has the same chip placement as the RFE and then has the same Euler angles conversion.

@davidetome
Copy link

davidetome commented Oct 11, 2023 •
edited
Loading

We are waiting for your kind positive feedback to merge this robotology/icub-firmware#425

The robot got broken, we've to wait for the fix

@sgiraz
Copy link
Contributor

sgiraz commented Oct 12, 2023

Hi @davidetome, afik the robot has been fixed.

cc @G-Cervettini @Nicogene

@GiulioRomualdi
Copy link
Member

Hi @sgiraz and @davidetome tomorrow morning we are going to flash the firmware and and test it

@GiulioRomualdi
Copy link
Member

One question before flashing. Which version of yarp, icub-main and icub-firmware-shared is required to have the firmware up and running?

@davidetome
Copy link

davidetome commented Oct 13, 2023 •
edited
Loading

One question before flashing. Which version of yarp, icub-main and icub-firmware-shared is required to have the firmware up and running?

Ciao @GiulioRomualdi , sorry for the late reply, I missed your comment, the FW modified is based on the last in icub-firmware devel

  • RFE v1.3
  • STRAIN2 v2.2

@GiulioRomualdi
Copy link
Member

Ciao @davidetome, this morning we tested the firmware on the strain2 as follows:

  1. we moved the robot on the pole

  2. I moved manually the left leg as in the following video

    motion.mp4

  3. We collected the data of the IMUs on the strains and we compared them with the expected values of the forward kinematics by using https://github.com/ami-iit/xcub-sensors-check/tree/main

We flashed the firmware you gave us in #1649 (comment) and we repeated the experiment again

Original firmware strain2 v2.2.0

L_FOOT_FRONT_ORIENTATION L_FOOT_REAR_ORIENTATION
L_FOOT_FRONT_ORIENTATION L_FOOT_REAR_ORIENTATION

New firmware strain2 v2.3.0

L_FOOT_FRONT_ORIENTATION L_FOOT_REAR_ORIENTATION
L_FOOT_FRONT_ORIENTATION L_FOOT_REAR_ORIENTATION

So now I think there is a problem in the pitch

You can find here the associated datasets: strain_imu_test.zip

@Nicogene
Copy link
Member

Nicogene commented Oct 13, 2023 •
edited
Loading

So now I think there is a problem in the pitch

In fact @davidetome and I had some doubt about the pitch sign that was the only one different from the rfe. This means that we can get rid of the #ifdef !

See

@davidetome
Copy link

fix done , see :

robotology/icub-main#901 (comment)

@pattacini
Copy link
Member

I would keep it open until the final user test is passed.

@pattacini pattacini reopened this Oct 13, 2023
@davidetome
Copy link

Ciao @GiulioRomualdi! New binaries are available in devel branch after this merge :

Could you please test them as suggested by @pattacini? Then we can close the issue 👍🏻

cc @Nicogene @marcoaccame

@GiulioRomualdi
Copy link
Member

Hi @davidetome, thank you for the effort. I will try to test it this week if I find a free slot on ergoCub. if this is not possible, a possibility is to perform the test on ergoCubSN001 as soon as it is available

@GiulioRomualdi
Copy link
Member

Hi all! I found a free slot on the robot and I tested the new firmware taken from robotology/icub-firmware-build@a910398

L_FOOT_FRONT_ORIENTATION L_FOOT_REAR_ORIENTATION
L_FOOT_FRONT_ORIENTATION L_FOOT_REAR_ORIENTATION

Everything is working as expected 🎉 Thank you all for the help!

⚠️ I reverted the old firmware (v2.2.0) to comply with the Distro release.

@pattacini
Copy link
Member

That's great!
Thanks for testing.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees

@davidetome davidetome

@sgiraz sgiraz

@AntonioConsilvio AntonioConsilvio

Labels
ergoCub 1.0 S/N:000 ergoCub robot (prototype)
Projects
iCub Tech Support
Status: Done
Milestone
No milestone
Development

No branches or pull requests
8 participants
@pattacini @DanielePucci @davidetome @GiulioRomualdi @sgiraz @Nicogene @G-Cervettini @AntonioConsilvio