This is a jupyter notebook. Jupyter Notebooks allows you to combine notes, code, and output into a single document. You can even export your document as a presentation or presentation.
In this worksheet, we will use the python3 SymPy package to derive expressions for converting between euler angles and matrices.
If you would like more information about jupyter notebook features:
For this assignment, you do not need to run this notebook. It has been compiled for you and saved as a webpage. However, if you would like to play with it, start by running all the cells (from the menu: goto 'Cell' -> 'Run All').
# python3
from sympy import *
init_printing(use_latex='mathjax')
import math
# Define symbols
cx,sx = symbols('cx sx')
cy,sy = symbols('cy sy')
cz,sz = symbols('cz sz')
Rx = Matrix([
[1, 0, 0],
[0, cx,-sx],
[0, sx, cx]])
Ry = Matrix([
[ cy, 0, sy],
[ 0, 1, 0],
[-sy, 0, cy]])
Rz = Matrix([
[cz, -sz, 0],
[sz, cz, 0],
[0, 0, 1]])
We can compute the matrix Rzyx by multiplying matrixes corresponding to each consecutive rotation, e.g.
$$ R_{zyx}(\theta_x, \theta_y, \theta_z) = R_z(\theta_z) * R_y(\theta_y) * R_x(\theta_x) $$In this file, we will use the SymPy to compute algebraic expressions for euler angle matrices. Using these expressions, we will be able to derive formulas for converting from matrices to euler angles.
In the following example, let
Rzyx = Rz * Ry * Rx
pprint(Rzyx)
Now that we have a matrix expression for the ZYX euler angles, we have formulas which describe how matrices and euler angles relate to each. Specifically, suppose we have a 3x3 rotation matrix R with the following elements
$$ R = \begin{bmatrix} r_{00} & r_{01} & r_{02} \\ r_{10} & r_{11} & r_{12} \\ r_{20} & r_{21} & r_{22} \\ \end{bmatrix} $$where each $r_{ij}$ represents a scalar value in $\mathbb{R}$. Usually math texts will use indexing at 1, but here let's use 0-based indexing so that it will be easier to use implement these formulas later.
Now suppose we wish to extract the euler angles from this matrix. We can get the Y rotation back from the term $r_{20}$.
$$ r_{20} = -\sin(\theta_y) \\ => \theta_y = \text{asin}(-r_{20}) $$What about the rotations around X and Z? We can obtain these similarly using the terms from the first column and last row. A robust method involves using the fact that
$$ \tan(\theta) = \frac{\sin(\theta)}{\cos(\theta)} $$to form the following expression for obtaining $\theta_x$
$$ \frac{r_{21}}{r_{22}} = \frac{\sin(\theta_x)}{\cos(\theta_x)} = \tan(\theta_x) \\ => \theta_x = \text{atan2}(r_{21}, r_{22}) $$The expression for $\theta_z$ can be obtained similarly
$$ \frac{r_{10}}{r_{00}} = \frac{\sin(\theta_z)}{\cos(\theta_z)} = \tan(\theta_z) \\ => \theta_z = \text{atan2}(r_{10}, r_{00}) $$Using atan2 makes it easier to handle the cases when $\theta$ is near 0, 90, or 180 degrees, which makes sine and cosine close to zero and 1. Be careful when using acos and asin because values even slightly out of the range [-1,1] can lead to NaNs. The computer will not tolerate nansense!
When the middle euler angle is 90 degrees, we need to look to the non-zero terms to values for the first and last angles. For example, for ZYX euler angles, we need to handle the case when Y is either positive or negative 90 degrees.
# Compute Rzyx when y is +90
Ry90 = Matrix([
[ 0, 0, 1],
[ 0, 1, 0],
[-1, 0, 0]])
Rzyx = Rz * Ry90 * Rx
pprint(Rzyx)
So now we have the above expression. We know that the Y rotation is 90 but what about the X and Z rotations? We need to look at the upper part of the matrix to figure these out.
Let's apply the sine and cosine addition rules
$$ \sin(z + x) = \sin(z) \cos(x) + \cos(z) \sin(x) \\ \sin(z - x) = \sin(z) \cos(x) - \cos(z) \sin(x) \\ \cos(z + x) = \cos(z) \cos(x) - \sin(z) \sin(x) \\ \cos(z - x) = \cos(z) \cos(x) + \sin(z) \sin(x) \\ $$Another useful property of sine and cosine is the following
$$ \sin(-\theta) = -\sin(\theta) \\ \cos(-\theta) = \cos(\theta) $$Let's try to simplify the above matrix using these rules. For example, the term in position $r_{01}$ has two terms containing both sine and cosine, so it corresponds to one of the sine rules. It also has a negative, so its the difference between two angles X and Z.
$$ \begin{bmatrix} 0 & s(x-z) & c(x-z) \\ 0 & c(x-z) & s(z-x) \\ -1 & 0 & 0 \end{bmatrix} $$which can be rewritten so every term has angle $x-z$ $$ \begin{bmatrix} 0 & s(x-z) & c(x-z) \\ 0 & c(x-z) & -s(x-z) \\ -1 & 0 & 0 \end{bmatrix} $$
Therefore, we can use atan2($r_{01}$, $r_{02}$) to get the $\theta$ angle corresponding to the difference between $x-z$. Many values for X and Z could combine to be $\theta$. Let's choose one of X or Z to be zero and then the other can be $\theta$.
# Compute Rzyx when y is -90
Ry90_Minus = Ry90.T
Rzyx = Rz * Ry90_Minus * Rx
pprint(Rzyx)
The other five euler angle combinations can be derived similarly.
print("Rxyz")
pprint(Rx * Ry * Rz)
print()
print()
print("Y = 90")
pprint(Rx * Ry90 * Rz)
print()
print()
print("Y = -90")
pprint(Rx * Ry90_Minus * Rz)
print()
print()
Y = 90 $$ \begin{bmatrix} 0 & 0 & 1 \\ s(x+z) & c(x+z) & 0\\ -c(x+z) & s(x+z) & 0 \\ \end{bmatrix} $$
Y = -90 $$ \begin{bmatrix} 0 & 0 & -1 \\ s(z-x) & c(z-x) & 0 \\ c(z-x) & -s(z-x) & 0 \\ \end{bmatrix} $$
print("Ryxz")
pprint(Ry * Rx * Rz)
print()
print()
Rx90 = Matrix([
[1, 0, 0],
[0, 0,-1],
[0, 1, 0]])
print("+90")
pprint(Ry * Rx90 * Rz)
print()
print()
Rx90_Minus = Rx90.T
print("-90")
pprint(Ry * Rx90.T * Rz)
print()
print()
X = 90 $$ \begin{bmatrix} c(y-z) & s(y-z) & 0\\ 0 & 0 & -1 \\ -s(y-z) & c(y-z) & 0 \\ \end{bmatrix} $$
X = -90 $$ \begin{bmatrix} c(y+z) & -s(y+z) & 0 \\ 0 & 0 & 1 \\ -s(y+z) & -c(y+z) & 0 \\ \end{bmatrix} $$
print("Rzxy")
pprint(Rz * Rx * Ry)
print()
print()
print("+90")
pprint(Rz * Rx90 * Ry)
print()
print()
print("-90")
pprint(Rz * Rx90.T * Ry)
print()
print()
X = 90 $$ \begin{bmatrix} c(y+z) & 0 & s(y+z) \\ s(y+z) & 0 & -c(y+z) \\ 0 & 1 & 0 \\ \end{bmatrix} $$
X = -90 $$ \begin{bmatrix} c(y-z) & 0 & s(y-z) \\ -s(y-z) & 0 & c(y-z) \\ 0 & -1 & 0 \\ \end{bmatrix} $$
print("Rxzy")
pprint(Rx * Rz * Ry)
print()
print()
Rz90 = Matrix([
[0, -1, 0],
[1, 0, 0],
[0, 0, 1]])
print("+90")
pprint(Rx * Rz90 * Ry)
print()
print()
print("-90")
pprint(Rx * Rz90.T * Ry)
print()
print()
Z = 90 $$ \begin{bmatrix} 0 & -1 & 0 \\ c(x-y) & 0 & -s(x-y) \\ s(x-y) & 0 & c(x-y) \\ \end{bmatrix} $$
Z = -90 $$ \begin{bmatrix} 0 & 1 & 0 \\ -c(x+y) & 0 & -s(x+y) \\ -s(x+y) & 0 & c(x+y) \\ \end{bmatrix} $$
print("Ryzx")
pprint(Ry * Rz * Rx)
print()
print()
print("+90")
pprint(Ry * Rz90 * Rx)
print()
print()
print("-90")
pprint(Ry * Rz90.T * Rx)
print()
print()
Z = 90 $$ \begin{bmatrix} 0 & -c(x+y) & s(x+y) \\ 1 & 0 & 0 \\ 0 & s(x+y) & c(x+y) \\ \end{bmatrix} $$
Z = -90 $$ \begin{bmatrix} 0 & c(y-x) & s(y-x) \\ -1 & 0 & 0 \\ 0 & -s(y-x) & c(y-x) \\ \end{bmatrix} $$