# Python Quaternion Calculation Function

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
pitch = euler
yaw = euler
```

#### 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, e, e, '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)]
```