IMU readings reported wrongly in different Roll/Pitch

[osrf-migration]

Original report (archived issue) by Hashir Zahir (Bitbucket: hzahir).

---

This is a related issue to Issue 188 (#188), which has already been resolved in pull request 216. However, I notice some strange behavior.

Using revolute joints fixed the issue (mostly) or so I thought as I have discovered a bug. Rotations in yaw for the IMU plugin work perfectly with this new fix (the fixed joint method reports inaccurate readings), however, rotations in pitch or roll result in incorrect readings from the IMU.

For example, using <xacro:wamv_imu name="imu_wamv" y="-0.2" R="3.14159"/> where the roll is set to 180 degrees instead of yaw, the raw readings reported by the IMU plugin is (r,p,y) = (0,0, 2.76) when the correct readings are supposed to be (r,p,y) = (0,0,-2.76) (notice the yaw has been flipped/inverted). This does not make sense since physical IMU has been flipped 180 degrees. Setting an arbitary value of rpy in the macro results in even more disastrous readings.

Example: <xacro:wamv_imu name="imu_wamv" y="-0.2" R="1" P="0" Y="0"/>

results in rpy = (1.966, -0.319, -2.928), when the actual reading should have been rpy = (1,0,-2.76).

Can someone help to verify this behavior? And also any ideas on a possible fix? This is more for teams whose real life IMU provide NED data and hence the IMU link is provided with rpy = (3.14,0,0) but unfortunately the plugin doesnt seem to respond well to roll or pitch. Thanks.

[osrf-migration]

Original comment by Hashir Zahir (Bitbucket: hzahir).

---

• Edited issue description

[osrf-migration]

Original comment by Hashir Zahir (Bitbucket: hzahir).

---

• Edited issue description

[osrf-migration]

Original comment by Hashir Zahir (Bitbucket: hzahir).

---

Or is this behavior expected in the sense that the Gazebo ROS IMU plugin does not get its yaw from “true north” or “true east” and hence flipping the IMU upside down inverts the yaw? Also, what would be the recommended way to get the “actual” rpy of the vehicle from the base_link frame. I had applied an inverse transform from base_link to imu_link to the raw readings of the imu, but as mentioned above, they only work when the IMU is rotated about the yaw axis but if its rotated about the pitch axis the inverse transform does not recover the actual vehicle heading.

[osrf-migration]

Original comment by Hashir Zahir (Bitbucket: hzahir).

---

• changed priority from "major" to "minor"

[huiyulhy]

The RPY values obtained from the gazebo_ros_imu plugin is the transformation from the world2imu frames, meaning it's the rotation matrix from the world to imu frame. However, the actual values that you would want the RPY values to be giving, is the transformation from the base_link2world frame, as that will give you the actual orientation of the ASV. These 2 transformations are the same only if the imu and base_link frames are aligned. Otherwise, you would need to perform an additional step of rotating your quaternion.

However, in the event that they are not, you will need another static transform publisher to give you the transformation matrix for the imu2base_link.
the transformation matrix world2base_link = world2imu * imu2base_link
Hence, you will need to apply an additional quaternion rotation to the world2imu values that you are getting, using the imu2base_link quaternion, in order to obtain the actual orientation of your base_link frame.

You may use this function from tf2 to apply the quaternion rotations:
http://wiki.ros.org/tf2/Tutorials/Quaternions

[scpeters]

I believe there may be a workaround if you use a new tag in the gazebo_ros_imu_sensor plugin that was added in ros-simulation/gazebo_ros_pkgs#1051

[caguero]

@bsb808 and I did some tests trying to reproduce your issue but we couldn't.

Initial conditions
We launched the WAM-V at the initial pose (158, 108, 0.1, 0, 0, -2.76) with the IMU rpy offset (pi, 0, 0). Something like this: <xacro:wamv_imu name="imu_wamv" y="-0.2" R="3.14159" />

Testing
We then looked at the IMU topic and we got this message:

---
header: 
  seq: 64392
  stamp: 
    secs: 4293
    nsecs:  67000000
  frame_id: "wamv/imu_wamv_link"
orientation: 
  x: 0.191238682034
  y: -0.981495948297
  z: 0.00142908466715
  w: 0.00956178265746
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00194777826959
  y: -0.00617401651607
  z: -2.04936915664e-05
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.207396840421
  y: 0.00962789491218
  z: -9.78155006999
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Converting the quaternion from the orientation field into an Euler angle, it gives us:

>>> euler = tf.transformations.euler_from_quaternion([0.191238682034,-0.981495948297,0,0])
>>> euler
(3.141592653589793, -0.0, -2.7567265104517493)

Conclusion
As far as I understand this is the expected value. The expected value should be the pose of the IMU w.r.t. to the Gazebo origin (https://www.ros.org/reps/rep-0145.html#id19). The pi observed in roll corresponds to the starting IMU roll value and the -2.76 in yaw corresponds to the initial yaw of the WAM-V. One extra thing to take into account is that Gazebo works in ENU coordinates, so that's the convention used here. We're not applying any extra transformation to produce data in NED.

Could you double-check on your side please?

[bsb808]

All,

Not necessarily on topic for this issue, but we've seen an increase in usage of (or at least reported issues) with this project. I'm curious what project (or projects) folks are working on - are you working on RobotX 2020, VRX 2021 or something else entirely?

[HashirZahir]

All,

Not necessarily on topic for this issue, but we've seen an increase in usage of (or at least reported issues) with this project. I'm curious what project (or projects) folks are working on - are you working on RobotX 2020, VRX 2021 or something else entirely?

Hi Brian, yes most of our team is utilizing this simulator to speed up development in preparation for RobotX in Dec 2020 (assuming it doesn't get cancelled) since live testing is almost impossible given the current global situation.

[HashirZahir]

@bsb808 and I did some tests trying to reproduce your issue but we couldn't.

Initial conditions
We launched the WAM-V at the initial pose (158, 108, 0.1, 0, 0, -2.76) with the IMU rpy offset (pi, 0, 0). Something like this: <xacro:wamv_imu name="imu_wamv" y="-0.2" R="3.14159" />

Testing
We then looked at the IMU topic and we got this message:

---
header: 
  seq: 64392
  stamp: 
    secs: 4293
    nsecs:  67000000
  frame_id: "wamv/imu_wamv_link"
orientation: 
  x: 0.191238682034
  y: -0.981495948297
  z: 0.00142908466715
  w: 0.00956178265746
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00194777826959
  y: -0.00617401651607
  z: -2.04936915664e-05
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.207396840421
  y: 0.00962789491218
  z: -9.78155006999
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Converting the quaternion from the orientation field into an Euler angle, it gives us:

>>> euler = tf.transformations.euler_from_quaternion([0.191238682034,-0.981495948297,0,0])
>>> euler
(3.141592653589793, -0.0, -2.7567265104517493)

Conclusion
As far as I understand this is the expected value. The expected value should be the pose of the IMU w.r.t. to the Gazebo origin (https://www.ros.org/reps/rep-0145.html#id19). The pi observed in roll corresponds to the starting IMU roll value and the -2.76 in yaw corresponds to the initial yaw of the WAM-V. One extra thing to take into account is that Gazebo works in ENU coordinates, so that's the convention used here. We're not applying any extra transformation to produce data in NED.

Could you double-check on your side please?

Okay I will double check this thanks @caguero @bsb808

[HashirZahir]

Hi, actually we are developing a custom URDF based off your default URDF to better model our real life vehicle. In that URDF, we tried to follow as closely to the example URDF provided using the YAML files. So our IMU link looked something like this:

<joint
    name="imu_joint"
    type="revolute">
    <origin
      xyz="-0.5 0.5 1.0"
      rpy="3.142 0 0" />
    <parent
      link="base_link" />
    <child
      link="imu_link" />
    <axis
      xyz="0 0 1" />
    <limit effort="1000.0" lower="0.0" upper="0" velocity="0"/>
</joint>

Perhaps there was something wrong with the way we implemented our IMU plugin? We can confirm using the above joint results in (3.14, 0, 2.76) while you received (3.14, 0, -2.76) using your URDF with similar RPY joint positions. Would appreciate any help/advice on this matter. Thanks.

[HashirZahir]

This was eventually resolved as we mounted the IMU to rpy = (0,0,0) in our custom urdf instead of the previous (pi,0,0) , which might have caused some unexpected issues.

The issue can be closed as we no longer face IMU reading issues.