Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Quaternion Basics

Other resources

There's a great tutorial here

Components of a quaternion

ROS uses quaternions to track and apply rotations. A quaternion has 4 components (x,y,z,w). That's right, 'w' is last (but beware: some libraries like Eigen put w as the first number!). The commonly-used unit quaternion that yields no rotation about the x/y/z axes is (0,0,0,1):

(C++)

   1 #include <tf2/LinearMath/Quaternion.h>
   2 ...
   3 tf2::Quaternion myQuaternion;
   4 myQuaternion.setRPY( 0, 0, 0 );  // Create this quaternion from roll/pitch/yaw (in radians)
   5 
   6 // Print the quaternion components (0,0,0,1)
   7 ROS_INFO_STREAM("x: " << myQuaternion.getX() << " y: " << myQuaternion.getY() << 
   8                 " z: " << myQuaternion.getZ() << " w: " << myQuaternion.getW());

The magnitude of a quaternion should be one. If numerical errors cause a quaternion magnitude other than one, ROS will print warnings. To avoid these warnings, normalize the quaternion:

   1 q.normalize();

ROS uses two quaternion datatypes: msg and 'tf.' To convert between them in C++, use the methods of tf2_geometry_msgs.

(C++)

   1 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
   2 tf2::Quaternion quat_tf;
   3 geometry_msgs::Quaternion quat_msg = ...;
   4 
   5 tf2::convert(quat_msg , quat_tf);
   6 // or
   7 tf2::fromMsg(quat_msg, quat_tf);
   8 // or for the other conversion direction
   9 quat_msg = tf2::toMsg(quat_tf);

(Python)

   1 from geometry_msgs.msg import Quaternion
   2 
   3 # Create a list of floats, which is compatible with tf
   4 # quaternion methods
   5 quat_tf = [0, 1, 0, 0]
   6 
   7 quat_msg = Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3])

Think in RPY then convert to quaternion

It's easy for humans to think of rotations about axes but hard to think in terms of quaternions. A suggestion is to calculate target rotations in terms of (roll about an X-axis) / (subsequent pitch about the Y-axis) / (subsequent yaw about the Z-axis), then convert to a quaternion:

(Python)

   1 from tf.transformations import quaternion_from_euler
   2   
   3 if __name__ == '__main__':
   4 
   5   # RPY to convert: 90deg, 0, -90deg
   6   q = quaternion_from_euler(1.5707, 0, -1.5707)
   7 
   8   print "The quaternion representation is %s %s %s %s." % (q[0], q[1], q[2], q[3])

Applying a quaternion rotation

To apply the rotation of one quaternion to a pose, simply multiply the previous quaternion of the pose by the quaternion representing the desired rotation. The order of this multiplication matters.

(C++)

   1 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
   2 
   3 tf2::Quaternion q_orig, q_rot, q_new;
   4 
   5 // Get the original orientation of 'commanded_pose'
   6 tf2::convert(commanded_pose.pose.orientation , q_orig);
   7 
   8 double r=3.14159, p=0, y=0;  // Rotate the previous pose by 180* about X
   9 q_rot.setRPY(r, p, y);
  10 
  11 q_new = q_rot*q_orig;  // Calculate the new orientation
  12 q_new.normalize();
  13 
  14 // Stuff the new rotation back into the pose. This requires conversion into a msg type
  15 tf2::convert(q_new, commanded_pose.pose.orientation);

(Python)

   1 from tf.transformations import *
   2 
   3 q_orig = quaternion_from_euler(0, 0, 0)
   4 q_rot = quaternion_from_euler(pi, 0, 0)
   5 q_new = quaternion_multiply(q_rot, q_orig)
   6 print q_new

Inverting a quaternion

An easy way to invert a quaternion is to negate the w-component:

(Python)

   1 q[3] = -q[3]

Relative rotations

Say you have two quaternions from the same frame, q_1 and q_2. You want to find the relative rotation, q_r, to go from q_1 to q_2:

   1 q_2 = q_r*q_1

You can solve for q_r similarly to solving a matrix equation. Invert q_1 and right-multiply both sides. Again, the order of multiplication is important:

   1 q_r = q_2*q_1_inverse

Here's an example to get the relative rotation from the previous robot pose to the current robot pose:

(Python)

   1 q1_inv[0] = prev_pose.pose.orientation.x
   2 q1_inv[1] = prev_pose.pose.orientation.y
   3 q1_inv[2] = prev_pose.pose.orientation.z
   4 q1_inv[3] = -prev_pose.pose.orientation.w # Negate for inverse
   5 
   6 q2[0] = current_pose.pose.orientation.x
   7 q2[1] = current_pose.pose.orientation.y
   8 q2[2] = current_pose.pose.orientation.z
   9 q2[3] = current_pose.pose.orientation.w
  10 
  11 qr = tf.transformations.quaternion_multiply(q2, q1_inv)

-Dr. Andy Zelenak, University of Texas