Using ROS 2? Check out the ROS 2 tf2 tutorials.

(!) 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

Description: The basics of quaternion usage in ROS.

Keywords: quaternion, rotation

Tutorial Level: INTERMEDIATE

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

Wiki: tf2/Tutorials/Quaternions (last edited 2022-10-06 16:52:54 by ShaneLoretz)