[Documentation] [TitleIndex] [WordIndex

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

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. The unit quaternion (meaning no rotation about the x/y/z axes) is (0,0,0,1):

   1 tf::Quaternion q = tf::createQuaternionFromRPY(0, 0, 0);  // Create this quaternion from roll/pitch/yaw (in radians)
   2 ROS_INFO_STREAM(q);  // Print the quaternion components (0,0,0,1)
   3 

The Euclidean 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 2 quaternion datatypes: msg and 'tf.' To convert between them, use the methods of tf/transform_datatypes:

   1 #include <tf/transform_datatypes.h>
   2 ...
   3 quaternionMsgToTF(quat_msg , quat_tf);

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. A Python example of the conversion:

   1 import rospy
   2 from tf.msg import tfMessage
   3 from tf.transformations import quaternion_from_euler
   4   
   5 if __name__ == '__main__':
   6 
   7   # RPY to convert: 90deg, 0, -90deg
   8   q = quaternion_from_euler(1.5707, 0, -1.5707)
   9 
  10   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:

   1 tf::Quaternion q_orig, q_rot;
   2 double r=3.14159, p=0, y=0;  // Rotate the previous pose by 180* about X
   3 q_rot = tf::createQuaternionFromRPY(r, p, y);
   4 
   5 quaternionMsgToTF(commanded_pose.pose.orientation , q_orig);  // Get the original orientation of 'commanded_pose'
   6 
   7 q_orig *= q_rot;  // Calculate the new orientation
   8 q_orig.normalize();
   9 quaternionTFToMsg(q_orig, commanded_pose.pose.orientation);  // Stuff the new rotation back into the pose. This requires conversion into a msg type
  10 


2017-10-21 12:26