This page was generated from doc/quaternions.ipynb. Interactive online version: .

Converting ASDF Rotations to Quaternions

This notebook shows the same thing as the notebook about rotation matrices, just using quaternions instead of rotation matrices. For more detailed explanations, have a look over there.

You might be tempted to use the equations from Wikipedia, but those use different conventions for axes and angles! The resulting equations will have a similar structure but will not be quite identical.

With the code below, any convention can be calculated by adapting

• the pairing of angles with their corresponding axes

• the sign of angles (or direction of axes) according to handedness

• the order of combining the individual axis/angle quaternions

import sympy as sp

from sympy.algebras import Quaternion

alpha, beta, gamma = sp.symbols('alpha beta gamma')

Azimuth: Rotation around the z-Axis

q_z = Quaternion.from_axis_angle((0, 0, 1), alpha)
q_z

$\displaystyle \cos{\left(\frac{\alpha}{2} \right)} + 0 i + 0 j + \sin{\left(\frac{\alpha}{2} \right)} k$

Example: Rotating the y unit vector (i.e. “looking north”) by 90 degrees to the left:

Quaternion.rotate_point((0, 1, 0), q_z.subs(alpha, sp.pi / 2))

(-1, 0, 0)

As expected, this yields the negative x unit vector, which points westwards.

Elevation: Rotation around the (local) x-Axis

q_x = Quaternion.from_axis_angle((1, 0, 0), beta)
q_x

$\displaystyle \cos{\left(\frac{\beta}{2} \right)} + \sin{\left(\frac{\beta}{2} \right)} i + 0 j + 0 k$

Example: Applying 90 degrees of elevation to the y unit vector:

Quaternion.rotate_point((0, 1, 0), q_x.subs(beta, sp.pi / 2))

(0, 0, 1)

As expected, this yields a vector pointing up.

Roll: Rotation around the (local) y-Axis

q_y = Quaternion.from_axis_angle((0, 1, 0), gamma)
q_y

$\displaystyle \cos{\left(\frac{\gamma}{2} \right)} + 0 i + \sin{\left(\frac{\gamma}{2} \right)} j + 0 k$

Example: Applying a roll angle of 90 degrees to a vector pointing up:

Quaternion.rotate_point((0, 0, 1), q_y.subs(gamma, sp.pi / 2))

(1, 0, 0)

As expected, this yields a vector pointing east.

Combining all Axes

This is easy, we only have to make sure to use the right order. As with rotation matrices, you should read this from right to left (first roll, then elevation, then azimuth):

q = q_z * q_x * q_y
q

$\displaystyle \left(- \sin{\left(\frac{\alpha}{2} \right)} \sin{\left(\frac{\beta}{2} \right)} \sin{\left(\frac{\gamma}{2} \right)} + \cos{\left(\frac{\alpha}{2} \right)} \cos{\left(\frac{\beta}{2} \right)} \cos{\left(\frac{\gamma}{2} \right)}\right) + \left(- \sin{\left(\frac{\alpha}{2} \right)} \sin{\left(\frac{\gamma}{2} \right)} \cos{\left(\frac{\beta}{2} \right)} + \sin{\left(\frac{\beta}{2} \right)} \cos{\left(\frac{\alpha}{2} \right)} \cos{\left(\frac{\gamma}{2} \right)}\right) i + \left(\sin{\left(\frac{\alpha}{2} \right)} \sin{\left(\frac{\beta}{2} \right)} \cos{\left(\frac{\gamma}{2} \right)} + \sin{\left(\frac{\gamma}{2} \right)} \cos{\left(\frac{\alpha}{2} \right)} \cos{\left(\frac{\beta}{2} \right)}\right) j + \left(\sin{\left(\frac{\alpha}{2} \right)} \cos{\left(\frac{\beta}{2} \right)} \cos{\left(\frac{\gamma}{2} \right)} + \sin{\left(\frac{\beta}{2} \right)} \sin{\left(\frac{\gamma}{2} \right)} \cos{\left(\frac{\alpha}{2} \right)}\right) k$

If you want to copy-paste this:

print(q)

(-sin(alpha/2)*sin(beta/2)*sin(gamma/2) + cos(alpha/2)*cos(beta/2)*cos(gamma/2)) + (-sin(alpha/2)*sin(gamma/2)*cos(beta/2) + sin(beta/2)*cos(alpha/2)*cos(gamma/2))*i + (sin(alpha/2)*sin(beta/2)*cos(gamma/2) + sin(gamma/2)*cos(alpha/2)*cos(beta/2))*j + (sin(alpha/2)*cos(beta/2)*cos(gamma/2) + sin(beta/2)*sin(gamma/2)*cos(alpha/2))*k

But you should probably pre-calculate the used terms in order to avoid repeated evaluation of the same functions. You could try something like this, for example:

q.subs([
    (sp.sin(alpha/2), sp.symbols('s_alpha')),
    (sp.sin(beta/2), sp.symbols('s_beta')),
    (sp.sin(gamma/2), sp.symbols('s_gamma')),
    (sp.cos(alpha/2), sp.symbols('c_alpha')),
    (sp.cos(beta/2), sp.symbols('c_beta')),
    (sp.cos(gamma/2), sp.symbols('c_gamma')),
])

$\displaystyle \left(c_{\alpha} c_{\beta} c_{\gamma} - s_{\alpha} s_{\beta} s_{\gamma}\right) + \left(c_{\alpha} c_{\gamma} s_{\beta} - c_{\beta} s_{\alpha} s_{\gamma}\right) i + \left(c_{\alpha} c_{\beta} s_{\gamma} + c_{\gamma} s_{\alpha} s_{\beta}\right) j + \left(c_{\alpha} s_{\beta} s_{\gamma} + c_{\beta} c_{\gamma} s_{\alpha}\right) k$

print(_)

(c_alpha*c_beta*c_gamma - s_alpha*s_beta*s_gamma) + (c_alpha*c_gamma*s_beta - c_beta*s_alpha*s_gamma)*i + (c_alpha*c_beta*s_gamma + c_gamma*s_alpha*s_beta)*j + (c_alpha*s_beta*s_gamma + c_beta*c_gamma*s_alpha)*k

Quaternion to Rotation Matrix

Just to make sure the result is the same as in the notebook about rotation matrices, let’s calculate the rotation matrix from our quaternion.

For some reason, SymPy seems to need two simplification steps for this …

R = sp.trigsimp(sp.trigsimp(q.to_rotation_matrix()))
R

$\displaystyle \left[\begin{matrix}- \sin{\left(\alpha \right)} \sin{\left(\beta \right)} \sin{\left(\gamma \right)} + \cos{\left(\alpha \right)} \cos{\left(\gamma \right)} & - \sin{\left(\alpha \right)} \cos{\left(\beta \right)} & \sin{\left(\alpha \right)} \sin{\left(\beta \right)} \cos{\left(\gamma \right)} + \sin{\left(\gamma \right)} \cos{\left(\alpha \right)}\\\sin{\left(\alpha \right)} \cos{\left(\gamma \right)} + \sin{\left(\beta \right)} \sin{\left(\gamma \right)} \cos{\left(\alpha \right)} & \cos{\left(\alpha \right)} \cos{\left(\beta \right)} & \sin{\left(\alpha \right)} \sin{\left(\gamma \right)} - \sin{\left(\beta \right)} \cos{\left(\alpha \right)} \cos{\left(\gamma \right)}\\- \sin{\left(\gamma \right)} \cos{\left(\beta \right)} & \sin{\left(\beta \right)} & \cos{\left(\beta \right)} \cos{\left(\gamma \right)}\end{matrix}\right]$

Quaternion to ASDF rotations

Again, please note that the equations from Wikipedia use different conventions for axes and angles.

We already know how to convert a rotation matrix to ASDF angles, and we know how to convert a quaternion to a rotation matrix, so let’s try that:

a, b, c, d = sp.symbols('a:d')

sp.simplify(sp.Quaternion(a, b, c, d).to_rotation_matrix())

$\displaystyle \left[\begin{matrix}\frac{a^{2} + b^{2} - c^{2} - d^{2}}{a^{2} + b^{2} + c^{2} + d^{2}} & \frac{2 \left(- a d + b c\right)}{a^{2} + b^{2} + c^{2} + d^{2}} & \frac{2 \left(a c + b d\right)}{a^{2} + b^{2} + c^{2} + d^{2}}\\\frac{2 \left(a d + b c\right)}{a^{2} + b^{2} + c^{2} + d^{2}} & \frac{a^{2} - b^{2} + c^{2} - d^{2}}{a^{2} + b^{2} + c^{2} + d^{2}} & \frac{2 \left(- a b + c d\right)}{a^{2} + b^{2} + c^{2} + d^{2}}\\\frac{2 \left(- a c + b d\right)}{a^{2} + b^{2} + c^{2} + d^{2}} & \frac{2 \left(a b + c d\right)}{a^{2} + b^{2} + c^{2} + d^{2}} & \frac{a^{2} - b^{2} - c^{2} + d^{2}}{a^{2} + b^{2} + c^{2} + d^{2}}\end{matrix}\right]$

Since we assume a unit quaternion, all the denominators are actually 1.

Rq = sp.simplify(sp.Quaternion(a, b, c, d).to_rotation_matrix().subs(a**2 + b**2 + c**2 + d**2, 1))
Rq

$\displaystyle \left[\begin{matrix}a^{2} + b^{2} - c^{2} - d^{2} & - 2 a d + 2 b c & 2 a c + 2 b d\\2 a d + 2 b c & a^{2} - b^{2} + c^{2} - d^{2} & - 2 a b + 2 c d\\- 2 a c + 2 b d & 2 a b + 2 c d & a^{2} - b^{2} - c^{2} + d^{2}\end{matrix}\right]$

The notebook about rotation matrices shows how to obtain \(\alpha\), \(\beta\) and \(\gamma\) from this matrix.

We can get \(\alpha\) from the top middle and the central element:

sp.atan2(-Rq[0, 1], Rq[1, 1])

$\displaystyle \operatorname{atan}_{2}{\left(2 a d - 2 b c,a^{2} - b^{2} + c^{2} - d^{2} \right)}$

print(_)

atan2(2*a*d - 2*b*c, a**2 - b**2 + c**2 - d**2)

The bottom middle element provides \(\beta\):

sp.asin(Rq[2, 1])

$\displaystyle \operatorname{asin}{\left(2 a b + 2 c d \right)}$

print(_)

asin(2*a*b + 2*c*d)

Note:

As mentioned in the notebook about rotation matrices, the argument of the asin() function has to be in the domain [-1.0; 1.0].

Make sure to handle this case, e.g. by re-normalizing the quaternion.

Finally, \(\gamma\) can be obtained from the bottom left and right elements:

sp.atan2(-Rq[2, 0], Rq[2, 2])

$\displaystyle \operatorname{atan}_{2}{\left(2 a c - 2 b d,a^{2} - b^{2} - c^{2} + d^{2} \right)}$

print(_)

atan2(2*a*c - 2*b*d, a**2 - b**2 - c**2 + d**2)

Gimbal Lock

As shown in the notebook about rotation matrices, there is a problem when \(\beta = \pm 90\) degrees.

For \(\beta = 90\) degrees (which means \(2ab+2cd = 1\)), we can obtain a value for \(\alpha + \gamma\):

sp.atan2(Rq[0, 2], -Rq[1, 2])

$\displaystyle \operatorname{atan}_{2}{\left(2 a c + 2 b d,2 a b - 2 c d \right)}$

print(_)

atan2(2*a*c + 2*b*d, 2*a*b - 2*c*d)

If we for example choose this value to be \(\alpha\), this will result in \(\gamma = 0\).

Alternatively, we can use this expression:

sp.atan2(Rq[1, 0], Rq[0, 0])

$\displaystyle \operatorname{atan}_{2}{\left(2 a d + 2 b c,a^{2} + b^{2} - c^{2} - d^{2} \right)}$

print(_)

atan2(2*a*d + 2*b*c, a**2 + b**2 - c**2 - d**2)

For \(\beta = -90\) degrees (which means \(2ab+2cd = -1\)), we can use the following expression for \(\alpha + \gamma\):

sp.atan2(-Rq[0, 2], Rq[1, 2])

$\displaystyle \operatorname{atan}_{2}{\left(- 2 a c - 2 b d,- 2 a b + 2 c d \right)}$

print(_)

atan2(-2*a*c - 2*b*d, -2*a*b + 2*c*d)

Again, if we for example choose this value to be \(\alpha\), this will result in \(\gamma = 0\).

Alternatively, we can use this expression:

sp.atan2(Rq[1, 0], Rq[0, 0])

$\displaystyle \operatorname{atan}_{2}{\left(2 a d + 2 b c,a^{2} + b^{2} - c^{2} - d^{2} \right)}$

print(_)

atan2(2*a*d + 2*b*c, a**2 + b**2 - c**2 - d**2)