reverse rotation in IMU linear acceleration calculation

[gerkey]

I'm working with a Gazebo model in which the link to which I'm attaching a hector IMU is rotated to have Z down. In that situation, I find that the linear acceleration data from this plugin becomes pretty wrong. It seems to be that it's no longer in a body-fixed frame (e.g., with only diff-drive commands, I get non-zero Y accelerations depending on which direction the vehicle is pointing in the world).

After looking at the code with @scpeters, it seems that this rotation is going the wrong direction. At least, after reversing it as I've done here, I get sensible accel data irrespective of how I mount the IMU on the model and which direction the vehicle is oriented in the world.

I can appreciate that this is a pretty fundamental change and I'm not sure how to best go about testing it, but I'm happy to help.

[meyerj]

Thanks, I will check it next week, but I am quite sure that the current implementation is correct according to our conventions, because we also use the simulated sensor output in fusion algorithms like with hector_localization.

There has been some confusion about this topic in the past: #8, #11

I assume that the inverted sign of the acceleration vector output compared to other implementations is what caused the confusion about the rotation. There are pros and cons, but actually the way it is now is more consistent with the sign of the angular velocity, which also describes how the body moves within the inertial frame and not how a probe mass would move within the body/IMU frame. So indeed, if the gravity pulls the robot downwards the accelerometer measures and acceleration upwards, because it cannot distinguish these two cases. It simply depends on the point of view.

If the sign of the linear acceleration vector does not comply with some conventions, I am willing to change it and adapt other code using it, but at first sight it seems to be consistent with upcoming REP-145, too.

I will verify that an IMU mounted upside down has no negative impact, but to me rot.RotateVectorReverse() appears to be correct in order to rotate a vector from world frame to body frame if rot describes the orientation of the body in the world, no?

math::Vector3 gravity = world->GetPhysicsEngine()->GetGravity();
math::Vector3 temp = link->GetWorldLinearVel(); // get velocity in world frame
if (dt > 0.0) accel = rot.RotateVectorReverse((temp - velocity) / dt - gravity);

                                              \---------\|/--------/
                                      Acceleration of the body in world frame

                                              \-------------\|/--------------/
                                        Subtract the gravity vector in world frame
                                     (gravity downwards == body acceleration upwards)

                      \-----------------------\|/----------------------------/
                        Rotate the resulting vector from world to body frame

rot describes the rotation of the IMU body itself relative to the world origin, so to rotate a vector expressed in world frame to body frame we have to apply the reverse rotation.

[gerkey]

Yeah, I was wondering whether this might be a matter of convention rather than a bug. To be more specific, the problematic behavior that we've noticed is with ekf_localization. If we configure that node to have as its only input the angular velocities and linear accelerations from an upside-down IMU, then the pose estimates that are produced don't make sense, with the behavior dependent on the orientation of the robot in the world. Perhaps the fix here instead belongs in our configuration of ekf_localization to correctly interpret the IMU data. Have you ever connected this plugin's output with that node?

[gerkey]

Is it possible that this plugin works properly only when the link associated with the IMU plugin is not rotated with respect to its parent link?

I've boiled my problem down to a fairly simply case. Here's some URDF:

<robot name="imu-box">
  <link name="box">
    <visual>
      <geometry>
        <box size="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1"/>
      <inertia ixx="0.167"
               ixy="0"
               ixz="0"
               iyy="0.167"
               iyz="0"
               izz="0.167"/>
    </inertial>
  </link>

  <link name="imu_link"/>
  <joint name="imu_joint" type="fixed">
    <!-- Right side up -->
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <!-- Upside down -->
    <!-- origin xyz="0 0 0" rpy="3.14159 0 0"/ -->
    <parent link="box"/>
    <child link="imu_link"/>
  </joint>

  <gazebo>
    <plugin name="imu" filename="libhector_gazebo_ros_imu.so">
      <serviceName>/imu/calibrate</serviceName>
      <updateRate>60.0</updateRate>
      <bodyName>imu_link</bodyName>
      <frameId>imu_link</frameId>
      <topicName>/imu</topicName>
    </plugin>
  </gazebo>
</robot>

Call that file imu-box.urdf. Then, using Kinetic and Gazebo 7 (I'm on 7.8.1) from debs:

roscore &
rosrun gazebo_ros gazebo &
rosrun gazebo_ros spawn_model -urdf -file imu-box.urdf -model imu-box
rqt_plot /imu/linear_acceleration/x /imu/linear_acceleration/y

Now you have a box with an IMU attached and are plotting the X and Y linear acceleration values.

We want to perturb the box. Right-click it in the Gazebo UI and select "Apply Force/Torque". In "Force->X", fill in a large value, like 1000N. Then click "Apply Force" a few times. The box will move each time, and you should see a response in the X acceleration in rqt_plot. That's as expected.

Now rotate and try again: select the rotation tool in the Gazebo UI and rotate the box by 45 degrees. Then apply the 1000N force in X again. You should see the same response in X acceleration. That's also as expected.

Delete the box in the Gazebo UI.

Now edit the URDF to roll the IMU by pi, then spawn it again. Now repeat the experiment described above. In this situation, I observe that:

• When the box is aligned with the world X axis, applying a force along the box's own X axis produces the right X acceleration response.
• But when the box is rotated by 45 degrees in the world, then applying the same force along the box's X axis produces a Y acceleration response instead.

Can you comment on what's happening here?

[gerkey]

After further experimentation with my simple example, I believe that:

• My proposed change in this PR is not correct, in that it doesn't always produce the right result.
• The current code does not work properly if the IMU is attached to a link that is rotated via a fixed joint with respect to its parent link.

I wonder whether this behavior is related to the fixed joint lumping that happens when Gazebo parses URDF.

For now we can get by with not rotating our IMU, but it would be nice to understand what is happening here, and eventually to be able to mount an IMU at an arbitrary location on a vehicle.

[scpeters]

The urdf parser in sdformat plays a role in this when there is a fixed joint reduction, as it changes the <bodyName> element and creates or modifies <xyzOffset> and <rpyOffset> variables. I've summarized this for your example in the following gist:

• https://gist.github.com/scpeters/7aaf7b0e3c6775bd1dc1f1f109c02d76

[meyerj]

I have not looked at @gerkey's example yet, but in general hector IMU should respect the <rpyOffset> parameter as passed by SDF and fixed joint lumping should not be a problem. Note that <xyzOffset> is ignored at the moment, which causes errors in the linear acceleration reading in presence of rotational motions proportional to the length of the offset vector.

There was a bug for angular velocities (#23) which has been fixed in d5fcb4a. The reporter mentions that linear_acceleration values are visualized correctly for an IMU mounted upside down.

Indeed it is strange that the Hector plugin uses rot.RotateVectorReverse(...) while the gazebo_ros_imu plugin in ros-simulation has rot.RotateVector(...) at the corresponding line (gazebo_ros_imu.cpp:280). In both cases the argument is a vector in the world frame and the result is expected to be in body frame. See also ros-simulation/gazebo_ros_pkgs#344, which would confirm that the rotations in ros-simulation are actually reversed. The rotation of a body relative to the world frame is inverse to the rotation required to transform a vector from world to body frame. So rot.RotateVectorReverse(...) would be correct to transform the acceleration vector from world to body frame if rot is the rotation of the body itself.

At the moment I would assume that the output values are correct if there is either no rotation or the IMU is mounted upside down as in #23, but the rotation offset is applied wrongly in all other cases. My best guess is that the multiplication this->offset_.rot * pose.rot in gazebo_ros_imu.cpp:251 need to be reversed, too, in comparison to the original version in ros-simulation.