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 BasicsDescription: The basics of quaternion usage in ROS.
Keywords: quaternion, rotation
Tutorial Level: INTERMEDIATE
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):
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:
ROS uses two quaternion datatypes: msg and 'tf.' To convert between them in C++, use the methods of tf2_geometry_msgs.
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:
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.
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);
Inverting a quaternion
An easy way to invert a quaternion is to negate the w-component:
1 q = -q
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:
1 q1_inv = prev_pose.pose.orientation.x 2 q1_inv = prev_pose.pose.orientation.y 3 q1_inv = prev_pose.pose.orientation.z 4 q1_inv = -prev_pose.pose.orientation.w # Negate for inverse 5 6 q2 = current_pose.pose.orientation.x 7 q2 = current_pose.pose.orientation.y 8 q2 = current_pose.pose.orientation.z 9 q2 = current_pose.pose.orientation.w 10 11 qr = tf.transformations.quaternion_multiply(q2, q1_inv)
-Dr. Andy Zelenak, University of Texas