Basic Rotations with Quaternions

The quaternions is a very interesting mathematical tools that make possible perform rotation operations without using rotation matrices. This method is efficent and secure (avoiding the gimbal lock effect) in dynamic situations. A very interesting explanation about the mathematical foundations and capacities of quaternions can be found here.

The ROS library TF provide several functions to handle quaternions in C++ and Python. There exists many opensource libraries which provides quaternion funciontalities like Eigen, Bullet to name a couple of them.

Rotating a point
ROS works with the right hand axis convention. This axis convention is specially adequate for mobile robots. Here the robot is always looking to the axis-x infinity.

import roslib
roslib.load_manifest('tf')
import tf
from tf.transformations import *

# we want to rotate the point using the x-axis ("roll")
rot=quaternion_from_euler(-numpy.pi/4,0,0)

# the object is located in the y=1. We use the format [x,y,z,w] and w is allways 0 for vectors
vec= [0,1,0,0]

#now we apply the mathematical operation res=q*v*q'. We use this function for multiplication but internally it is just a complex multiplication operation.
result=quaternion_multiply(quaternion_multiply(rot, vec),quaternion_conjugate(rot))

In [33]: result
Out[33]: array([ 0. , 0.70710678, -0.70710678, 0. ])

Concatenate Rotations
Here I show how to concatenate multiple rotations using quaternions:

q1=quaternion_from_euler(-numpy.pi/4,0,0)
q2=quaternion_from_euler(numpy.pi/4,0,0)
q3=quaternion_from_euler(-numpy.pi/4,0,0)
q4=quaternion_from_euler(numpy.pi/4,0,numpy.pi/4)

#the full transform is q4*q3*q2*q1
rot= quaternion_multiply(q4,quaternion_multiply(q3,quaternion_multiply(q2,q1)))
result=quaternion_multiply(quaternion_multiply(rot, vec),quaternion_conjugate(rot))

In [86]: result1
Out[86]: array([ 0. , 0.70710678, -0.70710678, 0. ])

In [90]: result2
Out[90]: array([ 0., 1., 0., 0.])

In [93]: result3
Out[93]: array([ 0. , 0.70710678, -0.70710678, 0. ])

In [96]: result
Out[96]: array([-0.70710678, 0.70710678, 0. , 0. ])