Reference initial orientation to world frame in IMU plugin

[PeterBowman]

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:

• Add ability to set IMU initial Orientation on boot gazebosim/gazebo-classic#1959
• Measure IMU orientation with respect to world ros-simulation/gazebo_ros_pkgs#1051
• Wrong data streamed by the accelerometer on the iCub3 head #592 (comment)

[traversaro]

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]

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]

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]

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

[isorrentino]

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

I agree!

[gabrielenava]

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:

nevertheless I agree with the modification!

cc @HosameldinMohamed

[traversaro]

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]

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

Ok also for me!

[prashanthr05]

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]

@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]

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.

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]

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]

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]

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]

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]

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]

Thanks @PeterBowman !