This page was generated from doc/rotation-matrices.ipynb. Interactive online version: .

Converting ASDF Rotations to Rotation Matrices

To rotate objects in an ASDF scene, you can use azimuth, elevation and roll angles, for example like this:

<... rot="-30 12.5 5">

The used coordinate system conventions are shown in the section about position and orientation.

In this section we show how these angles can be converted to rotation matrices, in order to practically use those rotations in software.

There isn’t just a single way to choose rotation angles in 3D space, in fact, there are very many ways to do this, many of them leading to different rotation matrices.

Here’s a (hopefully somewhat complete) overview about the possible options and the choices taken by the ASDF:

• Right-handed vs. left-handed coordinate system: The ASDF uses a right-handed one.

• Direction of the axes: The ASDF uses the ENU (east, north, up) convention.

• Euler angles vs. Tait–Bryan angles: The ASDF uses a variation of Tait–Bryan.

• There are many possible conventions for the order of angles and which axes they rotate around: The ASDF conventions are shown in detail below.

• “intrinsic” vs. “extrinsic” = “local” vs. “global” reference system: This sounds complicated, but it’s really just about the order of transformations. See below for details.

• Rotating vectors (= “active” = “alibi”) vs. rotating the coordinate system (= “passive” = “alias”): In the following derivations we consider the active situation, but a similar derivation can be done for the passive case.

  In case you are wondering: the functions sympy.matrices.rot_axis1() etc. do the latter, therefore we cannot use them here (at least not without some further manipulations).

• Rotation matrices can be derived for pre-multiplication with column vectors vs. post-multiplication with row vectors: We are using column vectors here, but (different) matrices could be derived for use with row vectors.

Let’s get started then, shall we?

First we import SymPy, which is great for doing this kind of symbolic derivations:

import sympy as sp

We have to define our three input angles. These are often called azimuth/elevation/roll, or yaw/pitch/roll, or heading/elevation/bank.

Here we just use the greek letters \(\alpha\), \(\beta\) and \(\gamma\):

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

The ASDF uses an ENU (east, north, up) coordinate system and the reference (“forward”) direction is north, i.e. along the positive y-axis.

alpha

$\displaystyle \alpha$

The azimuth angle \(\alpha\) is:

• zero when pointing north (i.e. along the positive y-axis),

• rotating around the z-axis (which points up)

• positive when rotating towards west (right hand rule).

beta

$\displaystyle \beta$

The elevation angle \(\beta\) is:

• zero in the horizontal plane,

• rotating around the local x-axis

• positive when the nose goes up (right hand rule).

gamma

$\displaystyle \gamma$

The roll angle \(\gamma\) is:

• zero when the top of the object points to the zenith (which is just the normal “upright” orientation),

• rotating around the local y-axis

• positive when the object is leaning towards starboard (right hand rule).

The definitions above use the intrinsic way of describing the rotations (i.e. relative to local coordinate axes).

If you want to use the extrinsic way, you can use the same angles. You just have to choose the right order of global rotations: First roll, then elevation, then azimuth. We will be using the extrinsic style below.

Let’s also define the cartesian components of a vector \(a\):

a_x, a_y, a_z = sp.symbols('a_x:z')

We will need those only during the derivation, they will not appear in the final equations.

Azimuth: Rotation around the z-Axis

Writing the vector \(a\) in cylindrical coordinates \(r_z\) (radius), \(\phi_z\) (angle) and \(a_z\) (height):

r_z, phi_z = sp.symbols('r_z phi_z')

… we can get its cartesian coordinates like this:

a = sp.Matrix([
    r_z * sp.cos(phi_z),
    r_z * sp.sin(phi_z),
    a_z,
])
a

$\displaystyle \left[\begin{matrix}r_{z} \cos{\left(\phi_{z} \right)}\\r_{z} \sin{\left(\phi_{z} \right)}\\a_{z}\end{matrix}\right]$

We are using column vectors here, that means we are searching for a rotation matrix to left-multiply this vector in order to get the vector \(b\).

To get a representation of the vector \(b\), let’s rotate \(a\) by an azimuth angle \(\alpha\):

b = sp.Matrix([
    r_z * sp.cos(phi_z + alpha),
    r_z * sp.sin(phi_z + alpha),
    a_z,
])
b

$\displaystyle \left[\begin{matrix}r_{z} \cos{\left(\alpha + \phi_{z} \right)}\\r_{z} \sin{\left(\alpha + \phi_{z} \right)}\\a_{z}\end{matrix}\right]$

Note that \(a_z\) is not affected by the rotation.

We can use some trigonometric identities to expand this:

b = b.expand(trig=True)
b

$\displaystyle \left[\begin{matrix}- r_{z} \sin{\left(\alpha \right)} \sin{\left(\phi_{z} \right)} + r_{z} \cos{\left(\alpha \right)} \cos{\left(\phi_{z} \right)}\\r_{z} \sin{\left(\alpha \right)} \cos{\left(\phi_{z} \right)} + r_{z} \sin{\left(\phi_{z} \right)} \cos{\left(\alpha \right)}\\a_{z}\end{matrix}\right]$

… and re-write it using the (cartesian) coordinates of vector \(a\): \(a_x\), \(a_y\) and \(a_z\):

b = b.subs(list(zip(a, [a_x, a_y, a_z])))
b

$\displaystyle \left[\begin{matrix}a_{x} \cos{\left(\alpha \right)} - a_{y} \sin{\left(\alpha \right)}\\a_{x} \sin{\left(\alpha \right)} + a_{y} \cos{\left(\alpha \right)}\\a_{z}\end{matrix}\right]$

Remember, we are looking for a rotation matrix that, when \(a\) is left-multiplied by it, yields \(b\).

In other words (or rather symbols):

\begin{equation*} \begin{bmatrix} b_x\\ b_y\\ b_z \end{bmatrix} = R_z(\alpha) \begin{bmatrix} a_x\\ a_y\\ a_z \end{bmatrix} \end{equation*}

Given the components of \(b\) shown above, we can simply pick out the matrix elements.

Or we let SymPy do it:

Rz = sp.Matrix([[line.coeff(var) for var in [a_x, a_y, a_z]]
                for line in b])
Rz

$\displaystyle \left[\begin{matrix}\cos{\left(\alpha \right)} & - \sin{\left(\alpha \right)} & 0\\\sin{\left(\alpha \right)} & \cos{\left(\alpha \right)} & 0\\0 & 0 & 1\end{matrix}\right]$

That’s it!

Let’s do a little sanity check, rotating the y unit vector (i.e. “looking straight ahead”) by 90 degrees to the left:

Rz.subs(alpha, sp.pi / 2) * sp.Matrix([0, 1, 0])

$\displaystyle \left[\begin{matrix}-1\\0\\0\end{matrix}\right]$

This yields the negative x unit vector, which points westwards. That sounds about right!

Elevation: Rotation around the (local) x-Axis

Now the same thing, just using a different vector \(a\).

r_x, phi_x = sp.symbols('r_x phi_x')
a = sp.Matrix([
    a_x,
    r_x * sp.cos(phi_x),
    r_x * sp.sin(phi_x),
])
a

$\displaystyle \left[\begin{matrix}a_{x}\\r_{x} \cos{\left(\phi_{x} \right)}\\r_{x} \sin{\left(\phi_{x} \right)}\end{matrix}\right]$

Let’s rotate \(a\) by the elevation angle \(\beta\) to get a vector \(b\):

b = sp.Matrix([
    a_x,
    r_x * sp.cos(phi_x + beta),
    r_x * sp.sin(phi_x + beta),
])
b

$\displaystyle \left[\begin{matrix}a_{x}\\r_{x} \cos{\left(\beta + \phi_{x} \right)}\\r_{x} \sin{\left(\beta + \phi_{x} \right)}\end{matrix}\right]$

Again, expand using trig identities and substitute \(a\) back in:

b = b.expand(trig=True).subs(list(zip(a, [a_x, a_y, a_z])))
b

$\displaystyle \left[\begin{matrix}a_{x}\\a_{y} \cos{\left(\beta \right)} - a_{z} \sin{\left(\beta \right)}\\a_{y} \sin{\left(\beta \right)} + a_{z} \cos{\left(\beta \right)}\end{matrix}\right]$

… and obtain a matrix \(R_x(\beta)\) that transforms \(a\) into \(b\):

Rx = sp.Matrix([[line.coeff(var) for var in [a_x, a_y, a_z]]
                for line in b])
Rx

$\displaystyle \left[\begin{matrix}1 & 0 & 0\\0 & \cos{\left(\beta \right)} & - \sin{\left(\beta \right)}\\0 & \sin{\left(\beta \right)} & \cos{\left(\beta \right)}\end{matrix}\right]$

And again a sanity check, this time using an elevation of 90 degrees:

Rx.subs(beta, sp.pi / 2) * sp.Matrix([0, 1, 0])

$\displaystyle \left[\begin{matrix}0\\0\\1\end{matrix}\right]$

The result is a vector pointing up, which is what we expected, didn’t we?

Roll: Rotation around the (local) y-Axis

Doing very similar steps as before:

r_y, phi_y = sp.symbols('r_y phi_y')
a = sp.Matrix([
    r_y * sp.sin(phi_y),
    a_y,
    r_y * sp.cos(phi_y),
])
a

$\displaystyle \left[\begin{matrix}r_{y} \sin{\left(\phi_{y} \right)}\\a_{y}\\r_{y} \cos{\left(\phi_{y} \right)}\end{matrix}\right]$

b = sp.Matrix([
    r_y * sp.sin(phi_y + gamma),
    a_y,
    r_y * sp.cos(phi_y + gamma),
])
b

$\displaystyle \left[\begin{matrix}r_{y} \sin{\left(\gamma + \phi_{y} \right)}\\a_{y}\\r_{y} \cos{\left(\gamma + \phi_{y} \right)}\end{matrix}\right]$

b = b.expand(trig=True).subs(list(zip(a, [a_x, a_y, a_z])))
b

$\displaystyle \left[\begin{matrix}a_{x} \cos{\left(\gamma \right)} + a_{z} \sin{\left(\gamma \right)}\\a_{y}\\- a_{x} \sin{\left(\gamma \right)} + a_{z} \cos{\left(\gamma \right)}\end{matrix}\right]$

Ry = sp.Matrix([[line.coeff(var) for var in [a_x, a_y, a_z]]
                for line in b])
Ry

$\displaystyle \left[\begin{matrix}\cos{\left(\gamma \right)} & 0 & \sin{\left(\gamma \right)}\\0 & 1 & 0\\- \sin{\left(\gamma \right)} & 0 & \cos{\left(\gamma \right)}\end{matrix}\right]$

Sanity check: Applying a roll angle of 90 degrees to a vector pointing up …

Ry.subs(gamma, sp.pi / 2) * sp.Matrix([0, 0, 1])

$\displaystyle \left[\begin{matrix}1\\0\\0\end{matrix}\right]$

… leads to a vector pointing east. This is what we wanted.

Combining all Axes

As mentioned above, we have to choose the right sequence of (global) rotations: first roll, then elevation, then azimuth.

Note that we start with \(R_y\) (roll) on the right, and then left-apply \(R_x\) (elevation) and then left-apply \(R_z\) (azimuth).

You should read this from right to left:

R = Rz * Rx * Ry
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]$

That’s it, that’s our rotation matrix!

Copy this to use it with SymPy (you’ll have to import Matrix, sin and cos and define alpha, beta and gamma):

print(R)

Matrix([[-sin(alpha)*sin(beta)*sin(gamma) + cos(alpha)*cos(gamma), -sin(alpha)*cos(beta), sin(alpha)*sin(beta)*cos(gamma) + sin(gamma)*cos(alpha)], [sin(alpha)*cos(gamma) + sin(beta)*sin(gamma)*cos(alpha), cos(alpha)*cos(beta), sin(alpha)*sin(gamma) - sin(beta)*cos(alpha)*cos(gamma)], [-sin(gamma)*cos(beta), sin(beta), cos(beta)*cos(gamma)]])

If you want to use it with NumPy, you can copy this (you’ll have to import numpy and define alpha, beta and gamma):

from sympy.printing.numpy import NumPyPrinter
print(NumPyPrinter().doprint(R))

numpy.array([[-numpy.sin(alpha)*numpy.sin(beta)*numpy.sin(gamma) + numpy.cos(alpha)*numpy.cos(gamma), -numpy.sin(alpha)*numpy.cos(beta), numpy.sin(alpha)*numpy.sin(beta)*numpy.cos(gamma) + numpy.sin(gamma)*numpy.cos(alpha)], [numpy.sin(alpha)*numpy.cos(gamma) + numpy.sin(beta)*numpy.sin(gamma)*numpy.cos(alpha), numpy.cos(alpha)*numpy.cos(beta), numpy.sin(alpha)*numpy.sin(gamma) - numpy.sin(beta)*numpy.cos(alpha)*numpy.cos(gamma)], [-numpy.sin(gamma)*numpy.cos(beta), numpy.sin(beta), numpy.cos(beta)*numpy.cos(gamma)]])

Rotation Matrix to Angles

You may ask: how can we get back from the rotation matrix to our angles?

If you look at the matrix \(R\) above, you see that one component only depends on one variable. Namely, the component in the last row, middle column:

R[2, 1]

$\displaystyle \sin{\left(\beta \right)}$

Therefore, we can get the value of \(\beta\) simply by taking the arc-sine of this matrix element. In a numeric calculation, this would probably look something like:

beta = asin(R[2, 1])

Note:

The argument of the asin() function has to be in the domain [-1.0; 1.0] (see https://en.cppreference.com/w/c/numeric/math/asin).

Due to rounding errors, the value might be slightly outside this range, which would lead to a return value of NaN.

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

The rest of the matrix components depend on more than one variable, but there are a few elements that depend only on two variables.

If we divide the top middle component (multiplied by \(-1\)) by the one below:

-R[0, 1] / R[1, 1]

$\displaystyle \frac{\sin{\left(\alpha \right)}}{\cos{\left(\alpha \right)}}$

… we get an expression that only depends on \(\alpha\).

We can simplify this expression:

_.simplify()

$\displaystyle \tan{\left(\alpha \right)}$

Therefore, to get the angle \(\alpha\), we only have to calculate \(\frac{-R_{0, 1}}{R_{1, 1}}\) and take the arc-tangent of the result.

To get the appropriate quadrant of the result, we will use the function atan2() in numeric calculations:

alpha = atan2(-R[0, 1], R[1, 1])

We can do a similar thing to get \(\gamma\):

-R[2, 0] / R[2, 2]

$\displaystyle \frac{\sin{\left(\gamma \right)}}{\cos{\left(\gamma \right)}}$

_.simplify()

$\displaystyle \tan{\left(\gamma \right)}$

Similar to the above, we take the arc-tangent of \(\frac{-R_{2, 0}}{R_{2, 2}}\) to get the angle \(\gamma\).

gamma = atan2(-R[2, 0], R[2, 2])

Gimbal Lock

But wait a second, we might have a problem: the dreaded gimbal lock!

Let’s consider the case where \(\beta = 90\) degrees:

R1 = R.subs(beta, sp.pi/2)
R1

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

If we try to calculate \(\alpha\) and \(\gamma\) like above, we end up calculating

atan2(0, 0)

Sadly, that is not defined:

sp.atan2(0, 0)

$\displaystyle \text{NaN}$

Note:

If the implementation supports IEEE floating-point arithmetic (IEC 60559), no NaN is returned (except if one of the inputs is NaN), see https://en.cppreference.com/w/c/numeric/math/atan2.

In this case, atan2() will return \(\pm 0\) or \(\pm \pi\) (which is generally not correct).

Depending on your use case, however, this might be good enough. If not, keep reading below!

We can try to find alternative equations for \(\alpha\) and \(\gamma\) from the hitherto unused matrix elements (but let’s simplify the matrix first):

R1 = sp.trigsimp(R1)
R1

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

sp.simplify(R1[1, 0] / R1[0, 0])

$\displaystyle \tan{\left(\alpha + \gamma \right)}$

sp.simplify(R1[0, 2] / -R1[1, 2])

$\displaystyle \tan{\left(\alpha + \gamma \right)}$

There is no unique solution to these equations. You can freely choose either \(\alpha\) or \(\gamma\) and use that to calculate the other angle.

A very similar thing happens for \(\beta = -90\) degrees:

R2 = R.subs(beta, -sp.pi/2)
R2

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

R2 = sp.trigsimp(R2)
R2

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

sp.simplify(R2[1, 0] / R2[0, 0])

$\displaystyle \tan{\left(\alpha - \gamma \right)}$

sp.simplify(-R2[0, 2] / R2[1, 2])

$\displaystyle \tan{\left(\alpha - \gamma \right)}$

Again, there is no unique solution. You can freely choose one of the angles and then calculate the other one.

The easiest way to avoid this whole gimbal lock problem, is simply to never convert rotation matrices to angles.