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.

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

Reference initial orientation to world frame in IMU plugin #639

Merged
merged 2 commits into from
Mar 10, 2023

Conversation

PeterBowman
Copy link
Member

Problem:
The IMU on our robot is mounted in such a way that the sensor frame does not match the inertial/world frame. Adjustments can be made in the SDF model file so that simulated angular velocities and linear accelerations are expressed in the local sensor frame, as expected. However, orientation measurements will always be set to zero by Gazebo at initialization, whereas the real IMU accounts for the frame mismatch and actually returns non-zero values (according to the influence of gravity).

Proposed solution:
The sensor can be configured to measure orientations in terms of the world frame via ImuSensor::SetWorldToReferenceOrientation. I adapted the solution from the ros-simulation repo linked below. A new configuration option, "useWorldReferenceOrientation", enables this opt-in behavior.

As a side note, we are using an Xsens MTi-28A53G35 sensor, wrapped by the "xsensmtx" YARP device located in icub-main.

See also:

@traversaro
Copy link
Member

Sorry for the late review @PeterBowman !

The IMU on our robot is mounted in such a way that the sensor frame does not match the inertial/world frame.

Just for me to understand, in this context you mean that th sensor frame in the initial pose does not match the inertial/world frame, right? Because in general I guess the sensor frame it can't always be aligned with the world frame, as it it moves attached to the link to the sensor is attached.

@PeterBowman
Copy link
Member Author

Hi, @traversaro! IMU data (generated by the Gazebo-YARP plugin) is inconditionally offset from the world frame, i.e. the sensor frame is initially aligned with it regardless of the orientation of the link it is attached to at start. Proof: if I configure a different initial orientation of the robot (in the .world file, e.g. initially bent or rotated), sensor measurements will still output (0, 0, 0) before moving any joint, even though we know that the orientation of the part the sensor is attached to is different. Increments in the orientation would be right, but the returned absolute values would not.

Even if we assumed that our robot would always initially start in the same pose, due to the orientation of the sensor frame (Z-down and pointing X-backwards, configured so to match the real robot), the measurements we are getting are reversed in sign. This should be doable via .sdf configuration, but I didn't manage to get satisfactory results and it also didn't solve the main issue (forced alignment with the world frame).

@traversaro
Copy link
Member

Sorry (again) for the late reply. Thanks a lot for the clear description, this is quite subtle and problematic behaviour. I think most of the users actually expects the IMU to return the ${}^{world} H_{sensor}$ (as enabled by your option), and not ${}^{sensorInitialPosition} H_{sensor}$, as that match what most real-world IMU actually use. So, even if it is a breaking change, do you think it could make sense to make the ${}^{world} H_{sensor}$ the default behaviour, and instead have an option to to keep the legacy behaviour, so that people that really need the ${}^{sensorInitialPosition} H_{sensor}$ can select it? I think we can name this opt-in flag useInitialSensorOrientationAsReference .

@traversaro
Copy link
Member

traversaro commented Mar 6, 2023

What do you think of what I proposed in the previous comment? @prashanthr05 @isorrentino @GiulioRomualdi @FabioBergonti @gabrielenava @Nicogene

@isorrentino
Copy link

What do you think of what I proposed in the previous comment?

I agree!

@gabrielenava
Copy link
Contributor

gabrielenava commented Mar 6, 2023

What do you think of what I proposed in the previous comment?

at the moment in iRonCub we use the baseState plugin and not the IMU in simulation.

However I think in older implementations we were accounting for the problem, if I understood correctly this routine:

image

nevertheless I agree with the modification!

cc @HosameldinMohamed

@traversaro
Copy link
Member

Cool @gabrielenava, I think that block is necessary nevertheless to handle the difference between the controller world and the IMU world, but anyhow the high level behaviour should be exactly the same before and after this modification.

@Nicogene
Copy link
Member

Nicogene commented Mar 6, 2023

What do you think of what I proposed in the previous comment? @prashanthr05 @isorrentino @GiulioRomualdi @FabioBergonti @gabrielenava @Nicogene

Ok also for me!

@prashanthr05
Copy link
Contributor

What do you think of what I proposed in the previous comment? @prashanthr05 @isorrentino @GiulioRomualdi @FabioBergonti @gabrielenava @Nicogene

I agree with your proposed solution. It bypasses the cognitive overload of remembering to explicitly set the reference transformation. Also, this is a hard-to-debug problem, in my experience. So it is better to document this in detail.

@traversaro
Copy link
Member

@PeterBowman do you like to make the modifications to have the old behaviour as opt-in, or do you prefer that I do it? Thanks again!

@PeterBowman
Copy link
Member Author

PeterBowman commented Mar 8, 2023

Hi, @traversaro! I have added a config option for restoring the old behavior as opt-in, please feel free to further tweak it if you see fit.

Let me provide a few pics to better illustrate what was going on and how this change will impact the sensor's output.

default_gzclient_camera(1)-2023-03-08T20_09_44 760424

In the above image, our robot (TEO) is facing the X axis (Z is the vertical one). The IMU sensor is located in the chest. It is mounted upside-down: its local X axis should point in the opposite direction of global X, similarly the local Z is opposing global Z.

  • Old orientation measurements: [0 0 0]
  • New orientation measurements: [180 0 -180]

default_gzclient_camera(1)-2023-03-08T20_32_30 234390

Now, when we rotate the chest (with the IMU) +90 degrees wrt. global Z:

  • Old orientation measurements: [0 0 -90]
  • New orientation measurements: [180 0 -90]

default_gzclient_camera(1)-2023-03-08T20_11_00 704190

If TEO begins facing the Y axis instead (it has been rotated +90 degrees in global Z), but still in its initial pose:

  • Old orientation measurements: [0 0 0]
  • New orientation measurements: [180 0 -90]

The -X/-Z orientation issue (making the virtual sensor emulate how the real one is actually mounted) might/should be solved via proper .sdf configuration, but I tried several combinations of <orientation_reference_frame> values and never managed to get it right. However, the real issue targetted here is the nonsense we get when TEO is oriented differently at start.

@traversaro
Copy link
Member

However, the real issue targetted here is the nonsense we get when TEO is oriented differently at start.

Yes, that is the real "problem".

@PeterBowman
Copy link
Member Author

Yes, that is the real "problem".

Hi! I'm terribly sorry, some meanings are difficult to catch via written word 😳. Is there some change I may have gotten wrong and should make to the current PR?

@traversaro
Copy link
Member

Yes, that is the real "problem".

Hi! I'm terribly sorry, some meanings are difficult to catch via written word 😳. Is there some change I may have gotten wrong and should make to the current PR?

No, sorry it was just a comment on my side, nothing preventing the merge of the PR.

@traversaro traversaro merged commit 4b35f02 into robotology:master Mar 10, 2023
@traversaro
Copy link
Member

Thanks @PeterBowman !

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

6 participants