Python Quaternion Calculation Function

I will translate this article to Japanese sooner or later.

This is just for my study and I recommend you to use other well-done python libraries.

Using tf library for handling Quaternion in python

There are already "tf" library to do some kind of these job.
tf (Python) — tf 0.1.0 documentation

Quaternion to Euler angle

For example, you can get euler angle from quaternion q = [x,y,z,w]

import tf

euler = tf.transformations.euler_from_quaternion(q)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]

Euler angle to DCM( Direct Cosine Matrix )

transformations — tf 0.1.0 documentation

Also, you can get 3D rotational matrix from euler angle.

Re = tf.transformations.euler_matrix(roll, pitch, yaw, 'rxyz')

Now ,you can combine upper transformation to get DCM from quaternion!

Test

Let's check this DCM with a simple example.
90 degree rotation around x axis is like below:

q = [1.0/math.sqrt(2) ,0 ,0 , 1.0/math.sqrt]

Then we can get Rotation matrix:

e = tf.transformations.euler_from_quaternion(q)
Re = tf.transformations.euler_matrix(e[0], e[1], e[2], 'rxyz')

Results in,

matrix([[ 1.,  0.,  0.],
        [ 0.,  0., -1.],
        [ 0.,  1.,  0.]])

It convert z axis to -y and y axis to z axis.

My Python Functions

I just need more compact one and write myself for my study.
I use right handed coordinate and scalar part of quaternion is in the 4th element of quaternion.
Like: q = (qx, qy, qz, qw).

I used math class to calculate some sin and cos.

import math

Basic quaternion calculations here: (this is not so compact...)

#### ----- Quaternion Calculation ----- ####

def q_mult(q1, q2):
    x1, y1, z1 ,w1 = q1
    x2, y2, z2 ,w2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
    return x, y, z ,w

def q_inv(q):
    x, y, z ,w = q
    n = x*x + y*y +z*z + w*w
    return [-x/n, -y/n, -z/n, w/n]

# inner product
def q_iprod(q1,q2):
    q = [x*y for (x,y) in zip(q1,q2)] 
    return sum(q)
    
def q_conjugate(q):
    x, y, z ,w = q
    return [-x, -y, -z, w]

def q_normalize(q):
    x, y, z ,w = q
    n = math.sqrt(x*x + y*y +z*z + w*w)
    return [x/n, y/n, z/n, w/n]

# Linear interpolate with rate: [t,(1-t)] (with normalization)     
def q_LERP(q1,q2,t):
    q = [x*(1.0-t)+y*t for (x,y) in zip(q1,q2)]    
    return q_normalize(q)
    
# Sphere interpolate with rate: [t,(1-t)]   
def q_SLERP(q1,q2,t):
    w = math.acos(q_iprod(q1,q2))
    a = math.sin((1.0-t)*w)/ math.sin(w)
    b = math.sin(t*w)/ math.sin(w)
    q = [x*a+y*b for (x,y) in zip(q1,q2)]    
    return q

# Rotate vector using quaternion        
def qv_mult(q1, v1):
    q2 =  v1 + [0.0]
    return q_mult(q_mult(q1, q2), q_conjugate(q1))[:3]

#### ---------------------------------- ####

Remember the scalar part is the 4th element in this definition.
You can just put returning scalar:w to be a first to convert the other formulation.

Quaternion to DCM

DCM can be directly get from quaternion.
Here shows the function:

def q2Rmat(q):
    x,y,z,w = q
    Rmat = np.matrix([ [x*x-y*y-z*z+w*w, 2.0*(x*y-w*z), 2.0*(x*z+w*y) ],\
              [2.0*(x*y+w*z), y*y+w*w-x*x-z*z, 2.0*(y*z-w*x)],\
              [2.0*(x*z-w*y),  2.0*(y*z+w*x), z*z+w*w-x*x-y*y]] )
    return Rmat

Remember the rotational matrix is different depending on vector or coordinate you want to rotate.
This equation is tend to rotate vector.

Test

Let's check this DCM with a simple example.
90 degree rotation around x axis is like below:

q = [1.0/math.sqrt(2) ,0 ,0 , 1.0/math.sqrt]

Then we can get Rotation matrix:

q2Rmat(q)

Results in,

matrix([[ 1.,  0.,  0.],
        [ 0.,  0., -1.],
        [ 0.,  1.,  0.]])

It convert z axis to -y and y axis to z axis.

Comparison Tests

Consider you have orientation of some robot in a quaternion form:

q = [math.sqrt(3)/2 * 0.5 , 0.5*0.5,0,math.sqrt(3)/2] 

then convert to rpy

rpy = [-0.061428016242791844, 0.9815194409145511, -0.08729595308817747]

Using tf.transformation function

array([[ 0.875     ,  0.21650635,  0.4330127 ,  0.        ],
       [ 0.21650635,  0.625     , -0.75      ,  0.        ],
       [-0.4330127 ,  0.75      ,  0.5       ,  0.        ],
       [ 0.        ,  0.        ,  0.        ,  1.        ]])

This output 4 times 4 homogeneous matrix.

Also try my sample,

matrix([[ 0.875     ,  0.21650635,  0.4330127 ],
        [ 0.21650635,  0.625     , -0.75      ],
        [-0.4330127 ,  0.75      ,  0.5       ]])

(for vector rotation)

It is almost the same result, but I think my function is more precise because it directly convert the quaternion.

coding tech memo: zip in python

I thought this kind of description is cool:

q = [x*(1.0-t)+y*t for (x,y) in zip(q1,q2)]