Search code examples
c++roswebots

Quaternion calculation in "RosInertialUnit.cpp" of Webots ROS default controller

today I was taking a closer look at the quaternion calculation used in the "RosInertialUnit.cpp" file as part of the default ROS controller.

I wanted to try out the InterialUnit using the "keyboard_teleop.wbt" - world and added the sensor to the Pioneer robot.

I was then comparing the robot's rotation values given in the scene tree (in axis + angle format) with the output of the sensor in ROS (orientation converterd to a quaternion). You can see both in the screenshots below: enter image description here enter image description here

In my mind the quaternion output doesn't match the values given in the scene tree. When using MATLAB's function "quat = axang2quat(axang)" I would obtain the following for the example above: 

quat = 0.7936    0.0131   -0.6082    0.0104  % w x y z

which when comparing with the ROS message shows that y and z are switched. I'm not quite sure if this is on purpose (maybe a different convention?). I didn't want to start a pull request right away but wanted to discuss the issue here before.

I was testing the following implementation in a changed version of "RosInertialUnit.cpp", which gives me the expected results (same results as calculated in MATLAB).

  double halfRoll = mInertialUnit->getRollPitchYaw()[0] * 0.5;   // turning around x
  double halfPitch = mInertialUnit->getRollPitchYaw()[2] * 0.5;  // turning around y
  double halfYaw = mInertialUnit->getRollPitchYaw()[1] * 0.5;    // turning around z
  double cosYaw = cos(halfYaw);
  double sinYaw = sin(halfYaw);
  double cosPitch = cos(halfPitch);
  double sinPitch = sin(halfPitch);
  double cosRoll = cos(halfRoll);
  double sinRoll = sin(halfRoll);

  value.orientation.x = cosYaw * cosPitch * sinRoll - sinYaw * sinPitch * cosRoll;
  value.orientation.y = sinYaw * cosPitch * sinRoll + cosYaw * sinPitch * cosRoll;
  value.orientation.z = sinYaw * cosPitch * cosRoll - cosYaw * sinPitch * sinRoll;
  value.orientation.w = cosYaw * cosPitch * cosRoll + sinYaw * sinPitch * sinRoll;

This is the same implementation as used in this wikipedia article.

Solution

  • This inversion is due to the fact that Webots and ROS coordinate systems are not equivalent.

    In Webots:

    • X: left
    • Y: up
    • Z: forward

    Which leads to: (https://cyberbotics.com/doc/reference/inertialunit#field-summary)

    • roll: left (Webots X)
    • pitch: forward (Webots Z)
    • yaw: up (Webots Y)

    In ROS: (https://www.ros.org/reps/rep-0103.html#axis-orientation)

    • X: forward
    • Y: left
    • Z: up

    Which leads to: (https://www.ros.org/reps/rep-0103.html#rotation-representation)

    • roll: forward (ROS X)
    • pitch: left (ROS Y)
    • yaw: up (ROS Z)

    As you can see the roll and pitch axes are switched, this is why they are switched in the code too.