{{{#!wiki tip
Using ROS 2? Check out the [[https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Tf2-Main.html|ROS 2 tf2 tutorials]].
}}}

## page was renamed from Tutorials/Quaternions
####################################
##FILL ME IN
####################################
## descriptive title for the tutorial
## title= Quaternion Basics
## multi-line description to be displayed in search 
## description = The basics of quaternion usage in ROS.
## what level user is this tutorial for 
## level= IntermediateCategory
## keywords = quaternion, rotation
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== Other resources ==

There's a great tutorial [[https://eater.net/quaternions|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++)

{{{
#!cplusplus
#include <tf2/LinearMath/Quaternion.h>
...
tf2::Quaternion myQuaternion;
myQuaternion.setRPY( 0, 0, 0 );  // Create this quaternion from roll/pitch/yaw (in radians)

// Print the quaternion components (0,0,0,1)
ROS_INFO_STREAM("x: " << myQuaternion.getX() << " y: " << myQuaternion.getY() << 
                " 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:
{{{
#!cplusplus
q.normalize();
}}}

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

(C++)

{{{
#!cplusplus 
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion quat_tf;
geometry_msgs::Quaternion quat_msg = ...;

tf2::convert(quat_msg , quat_tf);
// or
tf2::fromMsg(quat_msg, quat_tf);
// or for the other conversion direction
quat_msg = tf2::toMsg(quat_tf);
}}}

(Python)
{{{
#!python
from geometry_msgs.msg import Quaternion

# Create a list of floats, which is compatible with tf
# quaternion methods
quat_tf = [0, 1, 0, 0]

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)

{{{
#!python
from tf.transformations import quaternion_from_euler
  
if __name__ == '__main__':

  # RPY to convert: 90deg, 0, -90deg
  q = quaternion_from_euler(1.5707, 0, -1.5707)

  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++)

{{{
#!cplusplus 
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

tf2::Quaternion q_orig, q_rot, q_new;

// Get the original orientation of 'commanded_pose'
tf2::convert(commanded_pose.pose.orientation , q_orig);

double r=3.14159, p=0, y=0;  // Rotate the previous pose by 180* about X
q_rot.setRPY(r, p, y);

q_new = q_rot*q_orig;  // Calculate the new orientation
q_new.normalize();

// Stuff the new rotation back into the pose. This requires conversion into a msg type
tf2::convert(q_new, commanded_pose.pose.orientation);
}}}

(Python)

{{{
#!python
from tf.transformations import *

q_orig = quaternion_from_euler(0, 0, 0)
q_rot = quaternion_from_euler(pi, 0, 0)
q_new = quaternion_multiply(q_rot, q_orig)
print q_new
}}}

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

(Python)

{{{
#!python
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''':

{{{
#!python 
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:

{{{
#!python
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)

{{{
#!python
q1_inv[0] = prev_pose.pose.orientation.x
q1_inv[1] = prev_pose.pose.orientation.y
q1_inv[2] = prev_pose.pose.orientation.z
q1_inv[3] = -prev_pose.pose.orientation.w # Negate for inverse

q2[0] = current_pose.pose.orientation.x
q2[1] = current_pose.pose.orientation.y
q2[2] = current_pose.pose.orientation.z
q2[3] = current_pose.pose.orientation.w

qr = tf.transformations.quaternion_multiply(q2, q1_inv)
}}}


''-Dr. Andy Zelenak, University of Texas''


## AUTOGENERATED DO NOT DELETE
## TutorialCategory 
## FILL IN THE STACK TUTORIAL CATEGORY HERE
## DebugCategory