Python Quaternion Calculation Function
I will translate this article to Japanese sooner or later.
- Using tf library for handling Quaternion in python
- My Python Functions
- Comparison Tests
- coding tech memo: zip in python
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)]