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 (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 ROS_INFO_STREAM(q);  // Print the quaternion components (0,0,0,1)
6
```

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 # tf.transformations alternative is not yet available in tf2
2 from tf.transformations import quaternion_from_euler
3
4 if __name__ == '__main__':
5
6   # RPY to convert: 90deg, 0, -90deg
7   q = quaternion_from_euler(1.5707, 0, -1.5707)
8
9   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 2019-02-12 13:26:47 by Martin Pecka)