# Rigid-body transformations in three-dimensions

> Marcos Duarte
> Laboratory of Biomechanics and Motor Control ([http://demotu.org/](http://demotu.org/))
> Federal University of ABC, Brazil

The kinematics of a rigid body is completely described by its pose, i.e., its position and orientation in space (and the corresponding changes, translation and rotation). In a three-dimensional space, at least three coordinates and three angles are necessary to describe the pose of the rigid body, totalizing six degrees of freedom for a rigid body.

In motion analysis, to describe a translation and rotation of a rigid body with respect to a coordinate system, typically we attach another coordinate system to the rigid body and determine a transformation between these two coordinate systems.

A transformation is any function mapping a set to another set. For the description of the kinematics of rigid bodies, we are interested only in what is called rigid or Euclidean transformations (denoted as SE(3) for the three-dimensional space) because they preserve the distance between every pair of points of the body (which is considered rigid by definition). Translations and rotations are examples of rigid transformations (a reflection is also an example of rigid transformation but this changes the right-hand axis convention to a left hand, which usually is not of interest). In turn, rigid transformations are examples of [affine transformations](https://en.wikipedia.org/wiki/Affine_transformation). Examples of other affine transformations are shear and scaling transformations (which preserves angles but not lengths).

We will follow the same rationale as in the notebook [Rigid-body transformations in a plane (2D)](http://nbviewer.ipython.org/github/BMClab/bmc/blob/master/notebooks/Transformation2D.ipynb) and we will skip the fundamental concepts already covered there. So, you if haven't done yet, you should read that notebook before continuing here.

## Translation

A pure three-dimensional translation of a rigid body (or a coordinate system attached to it) in relation to other rigid body (with other coordinate system) is illustrated in the figure below.

Figure. A point in three-dimensional space represented in two coordinate systems, with one coordinate system translated.

The position of point $\mathbf{P}$ originally described in the $xyz$ (local) coordinate system but now described in the $\mathbf{XYZ}$ (Global) coordinate system in vector form is:

$$ \mathbf{P_G} = \mathbf{L_G} + \mathbf{P_l} $$

Or in terms of its components:

$$ \begin{array}{}
\mathbf{P_X} =& \mathbf{L_X} + \mathbf{P}_x \\
\mathbf{P_Y} =& \mathbf{L_Y} + \mathbf{P}_y \\
\mathbf{P_Z} =& \mathbf{L_Z} + \mathbf{P}_z
\end{array} $$

And in matrix form:

$$
\begin{bmatrix}
\mathbf{P_X} \\
\mathbf{P_Y} \\
\mathbf{P_Z}
\end{bmatrix} =
\begin{bmatrix}
\mathbf{L_X} \\
\mathbf{L_Y} \\
\mathbf{L_Z}
\end{bmatrix} +
\begin{bmatrix}
\mathbf{P}_x \\
\mathbf{P}_y \\
\mathbf{P}_z
\end{bmatrix}
$$

From classical mechanics, this is an example of [Galilean transformation](http://en.wikipedia.org/wiki/Galilean_transformation).

Let's use Python to compute some numeric examples:

```python
# Import the necessary libraries
import numpy as np
# suppress scientific notation for small numbers:
np.set_printoptions(precision=4, suppress=True)
```

For example, if the local coordinate system is translated by $\mathbf{L_G}=[1, 2, 3]$ in relation to the Global coordinate system, a point with coordinates $\mathbf{P_l}=[4, 5, 6]$ at the local coordinate system will have the position $\mathbf{P_G}=[5, 7, 9]$ at the Global coordinate system:

```python
LG = np.array([1, 2, 3])  # Numpy array
Pl = np.array([4, 5, 6])
PG = LG + Pl
PG
```

    array([5, 7, 9])

This operation also works if we have more than one point (NumPy try to guess how to handle vectors with different dimensions):

```python
Pl = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])  # 2D array with 3 rows and 2 columns
PG = LG + Pl
PG
```

    array([[ 2,  4,  6],
           [ 5,  7,  9],
           [ 8, 10, 12]])

## Rotation

A pure three-dimensional rotation of a $xyz$ (local) coordinate system in relation to other $\mathbf{XYZ}$ (Global) coordinate system and the position of a point in these two coordinate systems are illustrated in the next figure (remember that this is equivalent to describing a rotation between two rigid bodies).

A point in three-dimensional space represented in two coordinate systems, with one system rotated.

In analogy to the rotation in two dimensions, we can calculate the rotation matrix that describes the rotation of the $xyz$ (local) coordinate system in relation to the $\mathbf{XYZ}$ (Global) coordinate system using the direction cosines between the axes of the two coordinate systems:

$$ \mathbf{R_{Gl}} = \begin{bmatrix}
\cos\mathbf{X}x & \cos\mathbf{X}y & \cos\mathbf{X}z \\
\cos\mathbf{Y}x & \cos\mathbf{Y}y & \cos\mathbf{Y}z \\
\cos\mathbf{Z}x & \cos\mathbf{Z}y & \cos\mathbf{Z}z
\end{bmatrix} $$

Note however that for rotations around more than one axis, these angles will not lie in the main planes ($\mathbf{XY, YZ, ZX}$) of the $\mathbf{XYZ}$ coordinate system, as illustrated in the figure below for the direction angles of the $y$ axis only. Thus, the determination of these angles by simple inspection, as we have done for the two-dimensional case, would not be simple.

Figure. Definition of direction angles for the $y$ axis of the local coordinate system in relation to the $\mathbf{XYZ}$ Global coordinate system.

Note that the nine angles shown in the matrix above for the direction cosines are obviously redundant since only three angles are necessary to describe the orientation of a rigid body in the three-dimensional space.

An important characteristic of angles in the three-dimensional space is that angles cannot be treated as vectors: the result of a sequence of rotations of a rigid body around different axes depends on the order of the rotations, as illustrated in the next figure.

Figure. The result of a sequence of rotations around different axes of a coordinate system depends on the order of the rotations. In the first example (first row), the rotations are around a Global (fixed) coordinate system. In the second example (second row), the rotations are around a local (rotating) coordinate system.

Let's focus now on how to understand rotations in the three-dimensional space, looking at the rotations between coordinate systems (or between rigid bodies). Later we will apply what we have learned to describe the position of a point in these different coordinate systems.

### Euler angles

There are different ways to describe a three-dimensional rotation of a rigid body (or of a coordinate system). The most straightforward solution would probably be to use a [spherical coordinate system](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/ReferenceFrame.ipynb#Spherical-coordinate-system), but spherical coordinates would be difficult to give an anatomical or clinical interpretation. A solution that has been often employed in biomechanics to handle rotations in the three-dimensional space is to use Euler angles. Under certain conditions, Euler angles can have an anatomical interpretation, but this representation also has some caveats. Let's see the Euler angles now.

[Leonhard Euler](https://en.wikipedia.org/wiki/Leonhard_Euler) in the XVIII century showed that two three-dimensional coordinate systems with a common origin can be related by a sequence of up to three elemental rotations about the axes of the local coordinate system, where no two successive rotations may be about the same axis, which now are known as [Euler (or Eulerian) angles](http://en.wikipedia.org/wiki/Euler_angles).

#### Elemental rotations

First, let's see rotations around a fixed Global coordinate system as we did for the two-dimensional case. The next figure illustrates elemental rotations of the local coordinate system around each axis of the fixed Global coordinate system.

Figure. Elemental rotations of the $xyz$ coordinate system around each axis, $\mathbf{X}$, $\mathbf{Y}$, and $\mathbf{Z}$, of the fixed $\mathbf{XYZ}$ coordinate system. Note that for better clarity, the axis around where the rotation occurs is shown perpendicular to this page for each elemental rotation.

#### Rotations around the fixed coordinate system

The rotation matrices for the elemental rotations around each axis of the fixed $\mathbf{XYZ}$ coordinate system (rotations of the local coordinate system in relation to the Global coordinate system) are shown next.

Around $\mathbf{X}$ axis:

$$ \mathbf{R_{Gl,\,X}} =
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\alpha & -\sin\alpha \\
0 & \sin\alpha & \cos\alpha
\end{bmatrix} $$

Around $\mathbf{Y}$ axis:

$$ \mathbf{R_{Gl,\,Y}} =
\begin{bmatrix}
\cos\beta & 0 & \sin\beta \\
0 & 1 & 0 \\
-\sin\beta & 0 & \cos\beta
\end{bmatrix} $$

Around $\mathbf{Z}$ axis:

$$ \mathbf{R_{Gl,\,Z}} =
\begin{bmatrix}
\cos\gamma & -\sin\gamma & 0\\
\sin\gamma & \cos\gamma & 0 \\
0 & 0 & 1
\end{bmatrix} $$

These matrices are the rotation matrices for the case of two-dimensional coordinate systems plus the corresponding terms for the third axes of the local and Global coordinate systems, which are parallel.
To understand why the terms for the third axes are 1's or 0's, for instance, remember they represent the cosine directors. The cosines between $\mathbf{X}x$, $\mathbf{Y}y$, and $\mathbf{Z}z$ for the elemental rotations around respectively the $\mathbf{X}$, $\mathbf{Y}$, and $\mathbf{Z}$ axes are all 1 because $\mathbf{X}x$, $\mathbf{Y}y$, and $\mathbf{Z}z$ are parallel ($\cos 0^o$). The cosines of the other elements are zero because the axis around where each rotation occurs is perpendicular to the other axes of the coordinate systems ($\cos 90^o$).

#### Rotations around the local coordinate system

The rotation matrices for the elemental rotations this time around each axis of the $xyz$ coordinate system (rotations of the Global coordinate system in relation to the local coordinate system), similarly to the two-dimensional case, are simply the transpose of the above matrices as shown next.

Around $x$ axis:

$$ \mathbf{R}_{\mathbf{lG},\,x} =
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\alpha & \sin\alpha \\
0 & -\sin\alpha & \cos\alpha
\end{bmatrix} $$

Around $y$ axis:

$$ \mathbf{R}_{\mathbf{lG},\,y} =
\begin{bmatrix}
\cos\beta & 0 & -\sin\beta \\
0 & 1 & 0 \\
\sin\beta & 0 & \cos\beta
\end{bmatrix} $$

Around $z$ axis:

$$ \mathbf{R}_{\mathbf{lG},\,z} =
\begin{bmatrix}
\cos\gamma & \sin\gamma & 0\\
-\sin\gamma & \cos\gamma & 0 \\
0 & 0 & 1
\end{bmatrix} $$

Notice this is equivalent to instead of rotating the local coordinate system by $\alpha, \beta, \gamma$ in relation to axes of the Global coordinate system, to rotate the Global coordinate system by $-\alpha, -\beta, -\gamma$ in relation to the axes of the local coordinate system; remember that $\cos(-\:\cdot)=\cos(\cdot)$ and $\sin(-\:\cdot)=-\sin(\cdot)$.

The fact that we chose to rotate the local coordinate system by a counterclockwise (positive) angle in relation to the Global coordinate system is just a matter of convention.

#### Sequence of elemental rotations

Consider now a sequence of elemental rotations around the $\mathbf{X}$, $\mathbf{Y}$, and $\mathbf{Z}$ axes of the fixed $\mathbf{XYZ}$ coordinate system illustrated in the next figure.

Figure. Sequence of elemental rotations of the $xyz$ coordinate system around each axis, $\mathbf{X}$, $\mathbf{Y}$, $\mathbf{Z}$, of the fixed $\mathbf{XYZ}$ coordinate system.

This sequence of elemental rotations (each one of the local coordinate system with respect to the fixed Global coordinate system) is mathematically represented by a multiplication between the rotation matrices:

$$ \begin{array}{l l}
\mathbf{R_{Gl,\;XYZ}} & = \mathbf{R_{Z}} \mathbf{R_{Y}} \mathbf{R_{X}} \\
\\
& = \begin{bmatrix}
\cos\gamma & -\sin\gamma & 0\\
\sin\gamma & \cos\gamma & 0 \\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
\cos\beta & 0 & \sin\beta \\
0 & 1 & 0 \\
-\sin\beta & 0 & \cos\beta
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\alpha & -sin\alpha \\
0 & \sin\alpha & cos\alpha
\end{bmatrix} \\
\\
& =
\begin{bmatrix}
\cos\beta\:\cos\gamma \;&\;
\sin\alpha\:\sin\beta\:cos\gamma-\cos\alpha\:\sin\gamma \;&\;
\cos\alpha\:\sin\beta\:cos\gamma+\sin\alpha\:\sin\gamma \;\;\; \\
\cos\beta\:\sin\gamma \;&\;
\sin\alpha\:\sin\beta\:sin\gamma+\cos\alpha\:\cos\gamma \;&\;
\cos\alpha\:\sin\beta\:sin\gamma-\sin\alpha\:\cos\gamma \;\;\;  \\
-\sin\beta \;&\; \sin\alpha\:\cos\beta \;&\; \cos\alpha\:\cos\beta \;\;\;
\end{bmatrix}
\end{array} $$

Note that the order of the matrices.

We can check this matrix multiplication using [Sympy](http://sympy.org/en/index.html):

```python
#import the necessary libraries
from IPython.core.display import Math, display
import sympy as sym
cos, sin = sym.cos, sym.sin

a, b, g = sym.symbols('alpha, beta, gamma')

# Elemental rotation matrices of xyz in relation to XYZ:
RX = sym.Matrix([[1, 0, 0], [0, cos(a), -sin(a)], [0, sin(a), cos(a)]])
RY = sym.Matrix([[cos(b), 0, sin(b)], [0, 1, 0], [-sin(b), 0, cos(b)]])
RZ = sym.Matrix([[cos(g), -sin(g), 0], [sin(g), cos(g), 0], [0, 0, 1]])

# Rotation matrix of xyz in relation to XYZ:
RXYZ = RZ*RY*RX

display(Math(sym.latex(r'\mathbf{R_{Gl,\,XYZ}}=') + sym.latex(RXYZ, mat_str='matrix')))
```

$$\mathbf{R_{Gl,\,XYZ}}=\left[\begin{matrix}\cos{\left (\beta \right )} \cos{\left (\gamma \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 )} \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 (\alpha \right )} \sin{\left (\beta \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & - \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\beta \right )} \sin{\left (\gamma \right )} \cos{\left (\alpha \right )}\\- \sin{\left (\beta \right )} & \sin{\left (\alpha \right )} \cos{\left (\beta \right )} & \cos{\left (\alpha \right )} \cos{\left (\beta \right )}\end{matrix}\right]$$

For instance, we can calculate the numerical rotation matrix for these sequential elemental rotations by $90^o$ around $\mathbf{X,Y,Z}$:

```python
R = sym.lambdify((a, b, g), RXYZ, 'numpy')
R = R(np.pi/2, np.pi/2, np.pi/2)
display(Math(r'\mathbf{R_{Gl,\,XYZ\,}}(90^o, 90^o, 90^o) =' + \
             sym.latex(sym.Matrix(R).n(chop=True, prec=3))))
```

$$\mathbf{R_{Gl,\,XYZ\,}}(90^o, 90^o, 90^o) =\left[\begin{matrix}0 & 0 & 1.0\\0 & 1.0 & 0\\-1.0 & 0 & 0\end{matrix}\right]$$

Examining the matrix above and the correspondent previous figure, one can see they agree: the rotated $x$ axis (first column of the above matrix) has value -1 in the $\mathbf{Z}$ direction $[0,0,-1]$, the rotated $y$ axis (second column) is at the $\mathbf{Y}$ direction $[0,1,0]$, and the rotated $z$ axis (third column) is at the $\mathbf{X}$ direction $[1,0,0]$.

We also can calculate the sequence of elemental rotations around the $x$, $y$, $z$ axes of the rotating $xyz$ coordinate system illustrated in the next figure.

Figure. Sequence of elemental rotations of a second $xyz$ local coordinate system around each axis, $x$, $y$, $z$, of the rotating $xyz$ coordinate system.

Likewise, this sequence of elemental rotations (each one of the local coordinate system with respect to the rotating local coordinate system) is mathematically represented by a multiplication between the rotation matrices (which are the inverse of the matrices for the rotations around $\mathbf{X,Y,Z}$ as we saw earlier):

$$ \begin{array}{l l}
\mathbf{R}_{\mathbf{lG},\,xyz} & = \mathbf{R_{z}} \mathbf{R_{y}} \mathbf{R_{x}} \\
\\
& = \begin{bmatrix}
\cos\gamma & \sin\gamma & 0\\
-\sin\gamma & \cos\gamma & 0 \\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
\cos\beta & 0 & -\sin\beta \\
0 & 1 & 0 \\
sin\beta & 0 & \cos\beta
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\alpha & \sin\alpha \\
0 & -\sin\alpha & \cos\alpha
\end{bmatrix} \\
\\
& =
\begin{bmatrix}
\cos\beta\:\cos\gamma \;&\;
\sin\alpha\:\sin\beta\:\cos\gamma+\cos\alpha\:\sin\gamma \;&\;
\cos\alpha\:\sin\beta\:\cos\gamma-\sin\alpha\:\sin\gamma \;\;\; \\
-\cos\beta\:\sin\gamma \;&\;
-\sin\alpha\:\sin\beta\:\sin\gamma+\cos\alpha\:\cos\gamma \;&\;
\cos\alpha\:\sin\beta\:\sin\gamma+\sin\alpha\:\cos\gamma \;\;\;  \\
\sin\beta \;&\; -\sin\alpha\:\cos\beta \;&\; \cos\alpha\:\cos\beta \;\;\;
\end{bmatrix}
\end{array} $$

As before, the order of the matrices is from right to left.

Once again, we can check this matrix multiplication using [Sympy](http://sympy.org/en/index.html):

```python
a, b, g = sym.symbols('alpha, beta, gamma')
# Elemental rotation matrices of xyz (local):
Rx = sym.Matrix([[1, 0, 0], [0, cos(a), sin(a)], [0, -sin(a), cos(a)]])
Ry = sym.Matrix([[cos(b), 0, -sin(b)], [0, 1, 0], [sin(b), 0, cos(b)]])
Rz = sym.Matrix([[cos(g), sin(g), 0], [-sin(g), cos(g), 0], [0, 0, 1]])
# Rotation matrix of xyz' in relation to xyz:
Rxyz = Rz*Ry*Rx
Math(sym.latex(r'\mathbf{R}_{\mathbf{lG},\,xyz}=') + sym.latex(Rxyz, mat_str='matrix'))
```

$$\mathbf{R}_{\mathbf{lG},\,xyz}=\left[\begin{matrix}\cos{\left (\beta \right )} \cos{\left (\gamma \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 )} \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 (\alpha \right )} \sin{\left (\beta \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\beta \right )} \sin{\left (\gamma \right )} \cos{\left (\alpha \right )}\\\sin{\left (\beta \right )} & - \sin{\left (\alpha \right )} \cos{\left (\beta \right )} & \cos{\left (\alpha \right )} \cos{\left (\beta \right )}\end{matrix}\right]$$

For instance, let's calculate the numerical rotation matrix for these sequential elemental rotations by $90^o$ around $x,y,z$:

```python
R = sym.lambdify((a, b, g), Rxyz, 'numpy')
R = R(np.pi/2, np.pi/2, np.pi/2)
display(Math(r'\mathbf{R}_{\mathbf{lG},\,xyz\,}(90^o, 90^o, 90^o) =' + \
             sym.latex(sym.Matrix(R).n(chop=True, prec=3))))
```

$$\mathbf{R}_{\mathbf{lG},\,xyz\,}(90^o, 90^o, 90^o) =\left[\begin{matrix}0 & 0 & 1.0\\0 & -1.0 & 0\\1.0 & 0 & 0\end{matrix}\right]$$

Once again, let's compare the above matrix and the correspondent previous figure to see if it makes sense. But remember that this matrix is the Global-to-local rotation matrix, $\mathbf{R}_{\mathbf{lG},\,xyz}$, where the coordinates of the local basis' versors are rows, not columns, in this matrix. With this detail in mind, one can see that the previous figure and matrix also agree: the rotated $x$ axis (first row of the above matrix) is at the $\mathbf{Z}$ direction $[0,0,1]$, the rotated $y$ axis (second row) is at the $\mathbf{-Y}$ direction $[0,-1,0]$, and the rotated $z$ axis (third row) is at the $\mathbf{X}$ direction $[1,0,0]$.

In fact, this example didn't serve to distinguish versors as rows or columns because the $\mathbf{R}_{\mathbf{lG},\,xyz}$ matrix above is symmetric!
Let's look on the resultant matrix for the example above after only the first two rotations, $\mathbf{R}_{\mathbf{lG},\,xy}$ to understand this difference:

```python
Rxy = Ry*Rx
R = sym.lambdify((a, b), Rxy, 'numpy')
R = R(np.pi/2, np.pi/2)
display(Math(r'\mathbf{R}_{\mathbf{lG},\,xy\,}(90^o, 90^o) =' + \
             sym.latex(sym.Matrix(R).n(chop=True, prec=3))))
```

$$\mathbf{R}_{\mathbf{lG},\,xy\,}(90^o, 90^o) =\left[\begin{matrix}0 & 1.0 & 0\\0 & 0 & 1.0\\1.0 & 0 & 0\end{matrix}\right]$$

Comparing this matrix with the third plot in the figure, we see that the coordinates of versor $x$ in the Global coordinate system are $[0,1,0]$, i.e., local axis $x$ is aligned with Global axis $Y$, and this versor is indeed the first row, not first column, of the matrix above. Confer the other two rows.

What are then in the columns of the local-to-Global rotation matrix?
The columns are the coordinates of Global basis' versors in the local coordinate system! For example, the first column of the matrix above is the coordinates of $X$, which is aligned with $z$: $[0,0,1]$.

#### Rotations in a coordinate system is equivalent to minus rotations in the other coordinate system

Remember that we saw for the elemental rotations that it's equivalent to instead of rotating the local coordinate system, $xyz$, by $\alpha, \beta, \gamma$ in relation to axes of the Global coordinate system, to rotate the Global coordinate system, $\mathbf{XYZ}$, by $-\alpha, -\beta, -\gamma$ in relation to the axes of the local coordinate system. The same property applies to a sequence of rotations: rotations of $xyz$ in relation to $\mathbf{XYZ}$ by $\alpha, \beta, \gamma$ result in the same matrix as rotations of $\mathbf{XYZ}$ in relation to $xyz$ by $-\alpha, -\beta, -\gamma$:

$$ \begin{array}{l l}
\mathbf{R_{Gl,\,XYZ\,}}(\alpha,\beta,\gamma) & = \mathbf{R_{Gl,\,Z}}(\gamma)\, \mathbf{R_{Gl,\,Y}}(\beta)\, \mathbf{R_{Gl,\,X}}(\alpha) \\
& = \mathbf{R}_{\mathbf{lG},\,z\,}(-\gamma)\, \mathbf{R}_{\mathbf{lG},\,y\,}(-\beta)\, \mathbf{R}_{\mathbf{lG},\,x\,}(-\alpha) \\
& = \mathbf{R}_{\mathbf{lG},\,xyz\,}(-\alpha,-\beta,-\gamma)
\end{array}
$$

Confer that by examining the $\mathbf{R_{Gl,\,XYZ}}$ and $\mathbf{R}_{\mathbf{lG},\,xyz}$ matrices above.

Let's verify this property with Sympy:

```python
RXYZ = RZ*RY*RX
# Rotation matrix of xyz in relation to XYZ:
display(Math(sym.latex(r'\mathbf{R_{Gl,\,XYZ\,}}(\alpha,\beta,\gamma) =')))
display(Math(sym.latex(RXYZ, mat_str='matrix')))

# Elemental rotation matrices of XYZ in relation to xyz and negate all angles:
Rx_neg = sym.Matrix([[1, 0, 0], [0, cos(-a), -sin(-a)], [0, sin(-a), cos(-a)]]).T
Ry_neg = sym.Matrix([[cos(-b), 0, sin(-b)], [0, 1, 0], [-sin(-b), 0, cos(-b)]]).T
Rz_neg = sym.Matrix([[cos(-g), -sin(-g), 0], [sin(-g), cos(-g), 0], [0, 0, 1]]).T

# Rotation matrix of XYZ in relation to xyz:
Rxyz_neg = Rz_neg*Ry_neg*Rx_neg
display(Math(sym.latex(r'\mathbf{R}_{\mathbf{lG},\,xyz\,}(-\alpha,-\beta,-\gamma) =')))
display(Math(sym.latex(Rxyz_neg, mat_str='matrix')))

# Check that the two matrices are equal:
display(Math(sym.latex(r'\mathbf{R_{Gl,\,XYZ\,}}(\alpha,\beta,\gamma) \;==\;' + \
                       r'\mathbf{R}_{\mathbf{lG},\,xyz\,}(-\alpha,-\beta,-\gamma)')))
RXYZ == Rxyz_neg
```

$$\mathbf{R_{Gl,\,XYZ\,}}(\alpha,\beta,\gamma) =$$

$$\left[\begin{matrix}\cos{\left (\beta \right )} \cos{\left (\gamma \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 )} \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 (\alpha \right )} \sin{\left (\beta \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & - \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\beta \right )} \sin{\left (\gamma \right )} \cos{\left (\alpha \right )}\\- \sin{\left (\beta \right )} & \sin{\left (\alpha \right )} \cos{\left (\beta \right )} & \cos{\left (\alpha \right )} \cos{\left (\beta \right )}\end{matrix}\right]$$

$$\mathbf{R}_{\mathbf{lG},\,xyz\,}(-\alpha,-\beta,-\gamma) =$$

$$\left[\begin{matrix}\cos{\left (\beta \right )} \cos{\left (\gamma \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 )} \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 (\alpha \right )} \sin{\left (\beta \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & - \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\beta \right )} \sin{\left (\gamma \right )} \cos{\left (\alpha \right )}\\- \sin{\left (\beta \right )} & \sin{\left (\alpha \right )} \cos{\left (\beta \right )} & \cos{\left (\alpha \right )} \cos{\left (\beta \right )}\end{matrix}\right]$$

$$\mathbf{R_{Gl,\,XYZ\,}}(\alpha,\beta,\gamma) \;==\;\mathbf{R}_{\mathbf{lG},\,xyz\,}(-\alpha,-\beta,-\gamma)$$

    True

#### Rotations in a coordinate system is the transpose of inverse order of rotations in the other coordinate system

There is another property of the rotation matrices for the different coordinate systems: the rotation matrix, for example from the Global to the local coordinate system for the $xyz$ sequence, is just the transpose of the rotation matrix for the inverse operation (from the local to the Global coordinate system) of the inverse sequence ($\mathbf{ZYX}$) and vice-versa:

$$ \begin{array}{l l}
\mathbf{R}_{\mathbf{lG},\,xyz}(\alpha,\beta,\gamma) & = \mathbf{R}_{\mathbf{lG},\,z\,} \mathbf{R}_{\mathbf{lG},\,y\,} \mathbf{R}_{\mathbf{lG},\,x} \\
& = \mathbf{R_{Gl,\,Z\,}^{-1}} \mathbf{R_{Gl,\,Y\,}^{-1}} \mathbf{R_{Gl,\,X\,}^{-1}} \\
& = \mathbf{R_{Gl,\,Z\,}^{T}} \mathbf{R_{Gl,\,Y\,}^{T}} \mathbf{R_{Gl,\,X\,}^{T}} \\
& = (\mathbf{R_{Gl,\,X\,}} \mathbf{R_{Gl,\,Y\,}} \mathbf{R_{Gl,\,Z}})^\mathbf{T} \\
& = \mathbf{R_{Gl,\,ZYX\,}^{T}}(\gamma,\beta,\alpha)
\end{array}
$$

Where we used the properties that the inverse of the rotation matrix (which is orthonormal) is its transpose and that the transpose of a product of matrices is equal to the product of their transposes in reverse order.

Let's verify this property with Sympy:

```python
RZYX = RX*RY*RZ
Rxyz = Rz*Ry*Rx
display(Math(sym.latex(r'\mathbf{R_{Gl,\,ZYX\,}^T}=') + sym.latex(RZYX.T, mat_str='matrix')))
display(Math(sym.latex(r'\mathbf{R}_{\mathbf{lG},\,xyz\,}(\alpha,\beta,\gamma) \,==\,' + \
                       r'\mathbf{R_{Gl,\,ZYX\,}^T}(\gamma,\beta,\alpha)')))
Rxyz == RZYX.T
```

$$\mathbf{R_{Gl,\,ZYX\,}^T}=\left[\begin{matrix}\cos{\left (\beta \right )} \cos{\left (\gamma \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 )} \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 (\alpha \right )} \sin{\left (\beta \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\beta \right )} \sin{\left (\gamma \right )} \cos{\left (\alpha \right )}\\\sin{\left (\beta \right )} & - \sin{\left (\alpha \right )} \cos{\left (\beta \right )} & \cos{\left (\alpha \right )} \cos{\left (\beta \right )}\end{matrix}\right]$$

$$\mathbf{R}_{\mathbf{lG},\,xyz\,}(\alpha,\beta,\gamma) \,==\,\mathbf{R_{Gl,\,ZYX\,}^T}(\gamma,\beta,\alpha)$$

    True

#### Sequence of rotations of a Vector

We saw in the notebook [Rigid-body transformations in a plane (2D)](http://nbviewer.jupyter.org/github/BMClab/bmc/blob/master/notebooks/Transformation2D.ipynb#Rotation-of-a-Vector) that the rotation matrix can also be used to rotate a vector (in fact, a point, image, solid, etc.) by a given angle around an axis of the coordinate system. Let's investigate that for the 3D case using the example earlier where a book was rotated in different orders and around the Global and local coordinate systems.

Before any rotation, the point shown in that figure as a round black dot on the spine of the book has coordinates $\mathbf{P}=[0, 1, 2]$ (the book has thickness 0, width 1, and height 2).

After the first sequence of rotations shown in the figure (rotated around $X$ and $Y$ by $90^0$ each time), $\mathbf{P}$ has coordinates $\mathbf{P}=[1, -2, 0]$ in the global coordinate system. Let's verify that:

```python
P = np.array([[0, 1, 2]]).T

RXY = RY*RX
R = sym.lambdify((a, b), RXY, 'numpy')
R = R(np.pi/2, np.pi/2)
P1 = np.dot(R, P)
print('P1 =', P1.T)
```

    P1 = [[ 1. -2.  0.]]

As expected.
The reader is invited to deduce the position of point $\mathbf{P}$ after the inverse order of rotations, but still around the Global coordinate system.

Although we are performing vector rotation, where we don't need the concept of transformation between coordinate systems, in the example above we used the local-to-Global rotation matrix, $\mathbf{R_{Gl}}$. As we saw in the notebook for the 2D transformation, when we use this matrix, it performs a counter-clockwise (positive) rotation.
If we want to rotate the vector in the clockwise (negative) direction, we can use the very same rotation matrix entering a negative angle or we can use the inverse rotation matrix, the Global-to-local rotation matrix, $\mathbf{R_{lG}}$ and a positive (negative of negative) angle, because $\mathbf{R_{Gl}}(\alpha) = \mathbf{R_{lG}}(-\alpha)$, but bear in mind that even in this latter case we are rotating around the Global coordinate system!

Consider now that we want to deduce algebraically the position of the point $\mathbf{P}$ after the rotations around the local coordinate system as shown in the second set of examples in the figure with the sequence of book rotations. The point has the same initial position, $\mathbf{P}=[0, 1, 2]$, and after the rotations around $x$ and $y$ by $90^0$ each time, what is the position of this point?
It's implicit in this question that the new desired position is in the Global coordinate system because the local coordinate system rotates with the book and the point never changes its position in the local coordinate system. So, by inspection of the figure, the new position of the point is $\mathbf{P1}=[2, 0, 1]$.
Let's naively try to deduce this position by repeating the steps as before:

```python
Rxy = Ry*Rx
R = sym.lambdify((a, b), Rxy, 'numpy')
R = R(np.pi/2, np.pi/2)
P1 = np.dot(R, P)
print('P1 =', P1.T)
```

    P1 = [[ 1.  2. -0.]]

The wrong answer.
The problem is that we defined the rotation of a vector using the local-to-Global rotation matrix. One correction solution for this problem is to continuing using the multiplication of the Global-to-local rotation matrices, $\mathbf{R}_{xy} = \mathbf{R}_y\,\mathbf{R}_x$, transpose $\mathbf{R}_{xy}$ to get the Global-to-local coordinate system, $\mathbf{R_{XY}}=\mathbf{R^T}_{xy}$, and then rotate the vector using this matrix:

```python
Rxy = Ry*Rx
RXY = Rxy.T
R = sym.lambdify((a, b), RXY, 'numpy')
R = R(np.pi/2, np.pi/2)
P1 = np.dot(R, P)
print('P1 =', P1.T)
```

    P1 = [[ 2. -0.  1.]]

The correct answer.

Another solution is to understand that when using the Global-to-local rotation matrix, counter-clockwise rotations (as performed with the book the figure) are negative, not positive, and that when dealing with rotations with the Global-to-local rotation matrix the order of matrix multiplication is inverted, for example, it should be $\mathbf{R\_}_{xyz} = \mathbf{R}_x\,\mathbf{R}_y\,\mathbf{R}_z$ (an added underscore to remind us this is not the convention adopted here).

```python
R_xy = Rx*Ry
R = sym.lambdify((a, b), R_xy, 'numpy')
R = R(-np.pi/2, -np.pi/2)
P1 = np.dot(R, P)
print('P1 =', P1.T)
```

    P1 = [[ 2. -0.  1.]]

The correct answer.
The reader is invited to deduce the position of point $\mathbf{P}$ after the inverse order of rotations, around the local coordinate system.

In fact, you will find elsewhere texts about rotations in 3D adopting this latter convention as the standard, i.e., they introduce the Global-to-local rotation matrix and describe sequence of rotations algebraically as matrix multiplication in the direct order, $\mathbf{R\_}_{xyz} = \mathbf{R}_x\,\mathbf{R}_y\,\mathbf{R}_z$, the inverse we have done in this text. It's all a matter of convention, just that.

#### The 12 different sequences of Euler angles

The Euler angles are defined in terms of rotations around a rotating local coordinate system. As we saw for the sequence of rotations around $x, y, z$, the axes of the local rotated coordinate system are not fixed in space because after the first elemental rotation, the other two axes rotate.

Other sequences of rotations could be produced without combining axes of the two different coordinate systems (Global and local) for the definition of the rotation axes. There is a total of 12 different sequences of three elemental rotations that are valid and may be used for describing the rotation of a coordinate system with respect to another coordinate system:

$$ xyz \quad xzy \quad yzx \quad yxz \quad zxy \quad zyx  $$

$$ xyx \quad xzx \quad yzy \quad yxy \quad zxz \quad zyz  $$

The first six sequences (first row) are all around different axes, they are usually referred as Cardan or Tait–Bryan angles. The other six sequences (second row) have the first and third rotations around the same axis, but keep in mind that the axis for the third rotation is not at the same place anymore because it changed its orientation after the second rotation. The sequences with repeated axes are known as proper or classic Euler angles.

Which order to use it is a matter of convention, but because the order affects the results, it's fundamental to follow a convention and report it. In Engineering Mechanics (including Biomechanics), the $xyz$ order is more common; in Physics the $zxz$ order is more common (but the letters chosen to refer to the axes are arbitrary, what matters is the directions they represent). In Biomechanics, the order for the Cardan angles is most often based on the angle of most interest or of most reliable measurement. Accordingly, the axis of flexion/extension is typically selected as the first axis, the axis for abduction/adduction is the second, and the axis for internal/external rotation is the last one. We will see about this order later. The $zyx$ order is commonly used to describe the orientation of a ship or aircraft and the rotations are known as the nautical angles: yaw, pitch and roll, respectively (see next figure).

Figure. The principal axes of an aircraft and the names for the rotations around these axes (image from Wikipedia).

If instead of rotations around the rotating local coordinate system we perform rotations around the fixed Global coordinate system, we will have other 12 different sequences of three elemental rotations, these are called simply rotation angles. So, in total there are 24 possible different sequences of three elemental rotations, but the 24 orders are not independent; with the 12 different sequences of Euler angles at the local coordinate system we can obtain the other 12 sequences at the Global coordinate system.

The Python function `euler_rotmat.py` (code at the end of this text) determines the rotation matrix in algebraic form for any of the 24 different sequences (and sequences with only one or two axes can be inputed). This function also determines the rotation matrix in numeric form if a list of up to three angles are inputed.

For instance, the rotation matrix in algebraic form for the $zxz$ order of Euler angles at the local coordinate system and the correspondent rotation matrix in numeric form after three elemental rotations by $90^o$ each are:

```python
import sys
sys.path.insert(1, r'./../functions')
from euler_rotmat import euler_rotmat
```

```python
Ra, Rn = euler_rotmat(order='zxz', frame='local', angles=[90, 90, 90])
```

$$\mathbf{R}_{local}( z:\alpha,x:\beta,z:\gamma) =\left[\begin{matrix}- \sin{\left (\alpha \right )} \sin{\left (\gamma \right )} \cos{\left (\beta \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\gamma \right )} \cos{\left (\alpha \right )} \cos{\left (\beta \right )} & \sin{\left (\beta \right )} \sin{\left (\gamma \right )}\\- \sin{\left (\alpha \right )} \cos{\left (\beta \right )} \cos{\left (\gamma \right )} - \sin{\left (\gamma \right )} \cos{\left (\alpha \right )} & - \sin{\left (\alpha \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\beta \right )} \cos{\left (\gamma \right )} & \sin{\left (\beta \right )} \cos{\left (\gamma \right )}\\\sin{\left (\alpha \right )} \sin{\left (\beta \right )} & - \sin{\left (\beta \right )} \cos{\left (\alpha \right )} & \cos{\left (\beta \right )}\end{matrix}\right]$$

$$\mathbf{R}_{local}(z:\alpha=90^o,\;x:\beta=90^o,\;z:\gamma=90^o)=\left[\begin{matrix}0 & 0 & 1.0\\0 & -1.0 & 0\\1.0 & 0 & 0\end{matrix}\right]$$

#### Line of nodes

The second axis of rotation in the rotating coordinate system is also referred as the nodal axis or line of nodes; this axis coincides with the intersection of two perpendicular planes, one from each Global (fixed) and local (rotating) coordinate systems. The figure below shows an example of rotations and the nodal axis for the $xyz$ sequence of the Cardan angles.

Figure. First row: example of rotations for the $xyz$ sequence of the Cardan angles. The Global (fixed) $XYZ$ coordinate system is shown in green, the local (rotating) $xyz$ coordinate system is shown in blue. The nodal axis (N, shown in red) is defined by the intersection of the $YZ$ and $xy$ planes and all rotations can be described in relation to this nodal axis or to a perpendicular axis to it. Second row: starting from no rotation, the local coordinate system is rotated by $\alpha$ around the $x$ axis, then by $\beta$ around the rotated $y$ axis, and finally by $\gamma$ around the twice rotated $z$ axis. Note that the line of nodes coincides with the $y$ axis for the second rotation.

#### Determination of the Euler angles

Once a convention is adopted, the corresponding three Euler angles of rotation can be found.
For example, for the $\mathbf{R}_{xyz}$ rotation matrix:

```python
R = euler_rotmat(order='xyz', frame='local')
```

$$\mathbf{R}_{local}( x:\alpha,y:\beta,z:\gamma) =\left[\begin{matrix}\cos{\left (\beta \right )} \cos{\left (\gamma \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 )} \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 (\alpha \right )} \sin{\left (\beta \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\beta \right )} \sin{\left (\gamma \right )} \cos{\left (\alpha \right )}\\\sin{\left (\beta \right )} & - \sin{\left (\alpha \right )} \cos{\left (\beta \right )} & \cos{\left (\alpha \right )} \cos{\left (\beta \right )}\end{matrix}\right]$$

The corresponding Cardan angles for the `xyz` sequence can be given by:

$$ \begin{array}{}
\alpha = \arctan\left(\dfrac{\sin(\alpha)}{\cos(\alpha)}\right) = \arctan\left(\dfrac{-\mathbf{R}_{21}}{\;\;\;\mathbf{R}_{22}}\right) \\
\\
\beta = \arctan\left(\dfrac{\sin(\beta)}{\cos(\beta)}\right) = \arctan\left(\dfrac{\mathbf{R}_{20}}{\sqrt{\mathbf{R}_{00}^2+\mathbf{R}_{10}^2}}\right) \\
\\
\gamma = \arctan\left(\dfrac{\sin(\gamma)}{\cos(\gamma)}\right) = \arctan\left(\dfrac{-\mathbf{R}_{10}}{\;\;\;\mathbf{R}_{00}}\right)
\end{array} $$

Note that we prefer to use the mathematical function `arctan` rather than simply `arcsin` because the latter cannot for example distinguish $45^o$ from $135^o$ and also for better numerical accuracy. See the text [Angular kinematics in a plane (2D)](https://nbviewer.jupyter.org/github/BMClab/bmc/blob/master/notebooks/KinematicsAngular2D.ipynb) for more on these issues.

And here is a Python function to compute the Euler angles of rotations from the Global to the local coordinate system for the $xyz$ Cardan sequence:

```python
def euler_angles_from_rot_xyz(rot_matrix, unit='deg'):
    """ Compute Euler angles from rotation matrix in the xyz sequence."""

    import numpy as np

    R = np.array(rot_matrix, copy=False).astype(np.float64)[:3, :3]
    angles = np.zeros(3)

    angles[0] = np.arctan2(-R[2, 1], R[2, 2])
    angles[1] = np.arctan2( R[2, 0], np.sqrt(R[0, 0]**2 + R[1, 0]**2))
    angles[2] = np.arctan2(-R[1, 0], R[0, 0])

    if unit[:3].lower() == 'deg': # convert from rad to degree
        angles = np.rad2deg(angles)

    return angles
```

For instance, consider sequential rotations of 45$^o$ around $x,y,z$. The resultant rotation matrix is:

```python
Ra, Rn = euler_rotmat(order='xyz', frame='local', angles=[45, 45, 45], showA=False)
```

$$\mathbf{R}_{local}(x:\alpha=45^o,\;y:\beta=45^o,\;z:\gamma=45^o)=\left[\begin{matrix}0.5 & 0.8536 & 0.1464\\-0.5 & 0.1464 & 0.8536\\0.7071 & -0.5 & 0.5\end{matrix}\right]$$

Let's check that calculating back the Cardan angles from this rotation matrix using the `euler_angles_from_rot_xyz()` function:

```python
euler_angles_from_rot_xyz(Rn, unit='deg')
```

    array([ 45.,  45.,  45.])

We could implement a function to calculate the Euler angles for any of the 12 sequences (in fact, plus another 12 sequences if we consider all the rotations from and to the two coordinate systems), but this is tedious. There is a smarter solution using the concept of [quaternion](http://en.wikipedia.org/wiki/Quaternion), but we wont see that now.

Let's see a problem with using Euler angles known as gimbal lock.

### Gimbal lock

[Gimbal lock](http://en.wikipedia.org/wiki/Gimbal_lock) is the loss of one degree of freedom in a three-dimensional coordinate system that occurs when an axis of rotation is placed parallel with another previous axis of rotation and two of the three rotations will be around the same direction given a certain convention of the Euler angles. This "locks" the system into rotations in a degenerate two-dimensional space. The system is not really locked in the sense it can't be moved or reach the other degree of freedom, but it will need an extra rotation for that.
For instance, let's look at the $zxz$ sequence of rotations by the angles $\alpha, \beta, \gamma$:

$$ \begin{array}{l l}
\mathbf{R}_{zxz} & = \mathbf{R_{z}} \mathbf{R_{x}} \mathbf{R_{z}} \\
\\
& =
\begin{bmatrix}
\cos\gamma & \sin\gamma & 0\\
-\sin\gamma & \cos\gamma & 0 \\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\beta & \sin\beta \\
0 & -\sin\beta & \cos\beta
\end{bmatrix}
\begin{bmatrix}
\cos\alpha & \sin\alpha & 0\\
-\sin\alpha & \cos\alpha & 0 \\
0 & 0 & 1
\end{bmatrix}
\end{array} $$

Which results in:

```python
a, b, g = sym.symbols('alpha, beta, gamma')
# Elemental rotation matrices of xyz (local):
Rz = sym.Matrix([[cos(a), sin(a), 0], [-sin(a), cos(a), 0], [0, 0, 1]])
Rx = sym.Matrix([[1, 0, 0], [0, cos(b), sin(b)], [0, -sin(b), cos(b)]])
Rz2 = sym.Matrix([[cos(g), sin(g), 0], [-sin(g), cos(g), 0], [0, 0, 1]])
# Rotation matrix for the zxz sequence:
Rzxz = Rz2*Rx*Rz
Math(sym.latex(r'\mathbf{R}_{zxz}=') + sym.latex(Rzxz, mat_str='matrix'))
```

$$\mathbf{R}_{zxz}=\left[\begin{matrix}- \sin{\left (\alpha \right )} \sin{\left (\gamma \right )} \cos{\left (\beta \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\gamma \right )} \cos{\left (\alpha \right )} \cos{\left (\beta \right )} & \sin{\left (\beta \right )} \sin{\left (\gamma \right )}\\- \sin{\left (\alpha \right )} \cos{\left (\beta \right )} \cos{\left (\gamma \right )} - \sin{\left (\gamma \right )} \cos{\left (\alpha \right )} & - \sin{\left (\alpha \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\beta \right )} \cos{\left (\gamma \right )} & \sin{\left (\beta \right )} \cos{\left (\gamma \right )}\\\sin{\left (\alpha \right )} \sin{\left (\beta \right )} & - \sin{\left (\beta \right )} \cos{\left (\alpha \right )} & \cos{\left (\beta \right )}\end{matrix}\right]$$

Let's examine what happens with this rotation matrix when the rotation around the second axis ($x$) by $\beta$ is zero:

$$ \begin{array}{l l}
\mathbf{R}_{zxz}(\alpha, \beta=0, \gamma) =
\begin{bmatrix}
\cos\gamma & \sin\gamma & 0\\
-\sin\gamma & \cos\gamma & 0 \\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
\cos\alpha & \sin\alpha & 0\\
-\sin\alpha & \cos\alpha & 0 \\
0 & 0 & 1
\end{bmatrix}
\end{array} $$

The second matrix is the identity matrix and has no effect on the product of the matrices, which will be:

```python
Rzxz = Rz2*Rz
Math(sym.latex(r'\mathbf{R}_{xyz}(\alpha, \beta=0, \gamma)=') + \
     sym.latex(Rzxz, mat_str='matrix'))
```

$$\mathbf{R}_{xyz}(\alpha, \beta=0, \gamma)=\left[\begin{matrix}- \sin{\left (\alpha \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\gamma \right )} \cos{\left (\alpha \right )} & 0\\- \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} - \sin{\left (\gamma \right )} \cos{\left (\alpha \right )} & - \sin{\left (\alpha \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & 0\\0 & 0 & 1\end{matrix}\right]$$

Which simplifies to:

```python
Rzxz = sym.simplify(Rzxz)
Math(sym.latex(r'\mathbf{R}_{xyz}(\alpha, \beta=0, \gamma)=') + \
     sym.latex(Rzxz, mat_str='matrix'))
```

$$\mathbf{R}_{xyz}(\alpha, \beta=0, \gamma)=\left[\begin{matrix}\cos{\left (\alpha + \gamma \right )} & \sin{\left (\alpha + \gamma \right )} & 0\\- \sin{\left (\alpha + \gamma \right )} & \cos{\left (\alpha + \gamma \right )} & 0\\0 & 0 & 1\end{matrix}\right]$$

Despite different values of $\alpha$ and $\gamma$ the result is a single rotation around the $z$ axis given by the sum $\alpha+\gamma$. In this case, of the three degrees of freedom one was lost (the other degree of freedom was set by $\beta=0$). For movement analysis, this means for example that one angle will be undetermined because everything we know is the sum of the two angles obtained from the rotation matrix. We can set the unknown angle to zero but this is arbitrary.

In fact, we already dealt with another example of gimbal lock when we looked at the $xyz$ sequence with rotations by $90^o$. See the figure representing these rotations again and perceive that the first and third rotations were around the same axis because the second rotation was by $90^o$. Let's do the matrix multiplication replacing only the second angle by $90^o$ (and let's use the `euler_rotmat.py`:

```python
Ra, Rn = euler_rotmat(order='xyz', frame='local', angles=[None, 90., None], showA=False)
```

$$\mathbf{R}_{local}(x:\alpha,\;y:\beta=90^o,\;z:\gamma)=\left[\begin{matrix}0 & \sin{\left (\alpha + \gamma \right )} & - \cos{\left (\alpha + \gamma \right )}\\0 & \cos{\left (\alpha + \gamma \right )} & \sin{\left (\alpha + \gamma \right )}\\1.0 & 0 & 0\end{matrix}\right]$$

Once again, one degree of freedom was lost and we will not be able to uniquely determine the three angles for the given rotation matrix and sequence.

Possible solutions to avoid the gimbal lock are: choose a different sequence; do not rotate the system by the angle that puts the system in gimbal lock (in the examples above, avoid $\beta=90^o$); or add an extra fourth parameter in the description of the rotation angles.

But if we have a physical system where we measure or specify exactly three Euler angles in a fixed sequence to describe or control it, and we can't avoid the system to assume certain angles, then we might have to say "Houston, we have a problem".
A famous situation where such a problem occurred was during the Apollo 13 mission. This is an actual conversation between crew and mission control during the Apollo 13 mission (Corke, 2011):

>`Mission clock: 02 08 12 47`
**Flight**: *Go, Guidance.*
**Guido**: *He’s getting close to gimbal lock there.*
**Flight**: *Roger. CapCom, recommend he bring up C3, C4, B3, B4, C1 and C2 thrusters, and advise he’s getting close to gimbal lock.*
**CapCom**: *Roger.*

*Of note, it was not a gimbal lock that caused the accident with the the Apollo 13 mission, the problem was an oxygen tank explosion.*

## Determination of the rotation matrix

A typical way to determine the rotation matrix for a rigid body in biomechanics is to use motion analysis to measure the position of at least three non-collinear markers placed on the rigid body, and then calculate a basis with these positions, analogue to what we have described in the notebook [Rigid-body transformations in a plane (2D)](http://nbviewer.ipython.org/github/BMClab/bmc/blob/master/notebooks/Transformation2D.ipynb).

### Basis

If we have the position of three markers: **m1**, **m2**, **m3**, a basis (formed by three orthogonal versors) can be found as:

 - First axis, **v1**, the vector **m2-m1**;
 - Second axis, **v2**, the cross product between the vectors **v1** and **m3-m1**;
 - Third axis, **v3**, the cross product between the vectors **v1** and **v2**.

Then, each of these vectors are normalized resulting in three orthogonal versors.

For example, given the positions m1 = [1,0,0], m2 = [0,1,0], m3 = [0,0,1], a basis can be found:

```python
m1 = np.array([1, 0, 0])
m2 = np.array([0, 1, 0])
m3 = np.array([0, 0, 1])

v1 = m2 - m1
v2 = np.cross(v1, m3 - m1)
v3 = np.cross(v1, v2)

print('Versors:')
v1 = v1/np.linalg.norm(v1)
print('v1 =', v1)

v2 = v2/np.linalg.norm(v2)
print('v2 =', v2)

v3 = v3/np.linalg.norm(v3)
print('v3 =', v3)

print('\nNorm of each versor:\n',
      np.linalg.norm(np.cross(v1, v2)),
      np.linalg.norm(np.cross(v1, v3)),
      np.linalg.norm(np.cross(v2, v3)))
```

    Versors:
    v1 = [-0.7071  0.7071  0.    ]
    v2 = [ 0.5774  0.5774  0.5774]
    v3 = [ 0.4082  0.4082 -0.8165]

    Norm of each versor:
     1.0 1.0 1.0

Remember from the text [Rigid-body transformations in a plane (2D)](http://nbviewer.ipython.org/github/BMClab/bmc/blob/master/notebooks/Transformation2D.ipynb) that the versors of this basis are the columns of the $\mathbf{R_{Gl}}$ and the rows of the $\mathbf{R_{lG}}$ rotation matrices, for instance:

```python
RlG = np.array([v1, v2, v3])
print('Rotation matrix from Global to local coordinate system:\n', RlG)
```

    Rotation matrix from Global to local coordinate system:
     [[-0.7071  0.7071  0.    ]
     [ 0.5774  0.5774  0.5774]
     [ 0.4082  0.4082 -0.8165]]

And the corresponding angles of rotation using the $xyz$ sequence are:

```python
euler_angles_from_rot_xyz(RlG)
```

    array([-153.4349,   24.0948, -140.7685])

These angles don't mean anything now because they are angles of the axes of the arbitrary basis we computed. In biomechanics, if we want an anatomical interpretation of the coordinate system orientation, we define the versors of the basis oriented with anatomical axes (e.g., for the shoulder, one versor would be aligned with the long axis of the upper arm).
We will see how to perform this computation later. Now we will combine translation and rotation in a single transformation.

## Translation and Rotation

Consider the case where the local coordinate system is translated and rotated in relation to the Global coordinate system as illustrated in the next figure.

Figure. A point in three-dimensional space represented in two coordinate systems, with one system translated and rotated.

The position of point $\mathbf{P}$ originally described in the local coordinate system, but now described in the Global coordinate system in vector form is:

$$ \mathbf{P_G} = \mathbf{L_G} + \mathbf{R_{Gl}}\mathbf{P_l} $$

This means that we first *disrotate* the local coordinate system and then correct for the translation between the two coordinate systems. Note that we can't invert this order: the point position is expressed in the local coordinate system and we can't add this vector to another vector expressed in the Global coordinate system, first we have to convert the vectors to the same coordinate system.

If now we want to find the position of a point at the local coordinate system given its position in the Global coordinate system, the rotation matrix and the translation vector, we have to invert the expression above:

$$ \begin{array}{l l}
\mathbf{P_G} = \mathbf{L_G} + \mathbf{R_{Gl}}\mathbf{P_l} \implies \\
\\
\mathbf{R_{Gl}^{-1}}\cdot\mathbf{P_G} = \mathbf{R_{Gl}^{-1}}\left(\mathbf{L_G} + \mathbf{R_{Gl}}\mathbf{P_l}\right) \implies \\
\\
\mathbf{R_{Gl}^{-1}}\mathbf{P_G} = \mathbf{R_{Gl}^{-1}}\mathbf{L_G} + \mathbf{R_{Gl}^{-1}}\mathbf{R_{Gl}}\mathbf{P_l} \implies \\
\\
\mathbf{P_l} = \mathbf{R_{Gl}^{-1}}\left(\mathbf{P_G}-\mathbf{L_G}\right) =  \mathbf{R_{Gl}^T}\left(\mathbf{P_G}-\mathbf{L_G}\right) \;\;\;\;\; \text{or} \;\;\;\;\; \mathbf{P_l} = \mathbf{R_{lG}}\left(\mathbf{P_G}-\mathbf{L_G}\right)
\end{array} $$

The expression above indicates that to perform the inverse operation, to go from the Global to the local coordinate system, we first translate and then rotate the coordinate system.

### Transformation matrix

It is possible to combine the translation and rotation operations in only one matrix, called the transformation matrix:

$$ \begin{bmatrix}
\mathbf{P_X} \\
\mathbf{P_Y} \\
\mathbf{P_Z} \\
1
\end{bmatrix} =
\begin{bmatrix}
. & . & . & \mathbf{L_{X}} \\
. & \mathbf{R_{Gl}} & .  & \mathbf{L_{Y}} \\
. & . & .  & \mathbf{L_{Z}} \\
0 & 0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
\mathbf{P}_x \\
\mathbf{P}_y \\
\mathbf{P}_z \\
1
\end{bmatrix} $$

Or simply:

$$ \mathbf{P_G} = \mathbf{T_{Gl}}\mathbf{P_l} $$

Remember that in general the transformation matrix is not orthonormal, i.e., its inverse is not equal to its transpose.

The inverse operation, to express the position at the local coordinate system in terms of the Global reference system, is:

$$ \mathbf{P_l} = \mathbf{T_{Gl}^{-1}}\mathbf{P_G} $$

And in matrix form:

$$ \begin{bmatrix}
\mathbf{P_x} \\
\mathbf{P_y} \\
\mathbf{P_z} \\
1
\end{bmatrix} =
\begin{bmatrix}
\cdot & \cdot & \cdot & \cdot \\
\cdot & \mathbf{R^{-1}_{Gl}} & \cdot  & -\mathbf{R^{-1}_{Gl}}\:\mathbf{L_G} \\
\cdot & \cdot & \cdot  & \cdot \\
0 & 0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
\mathbf{P_X} \\
\mathbf{P_Y} \\
\mathbf{P_Z} \\
1
\end{bmatrix} $$

### Example with actual motion analysis data

*The data for this example is taken from page 183 of David Winter's book.*
Consider the following marker positions placed on a leg (described in the laboratory coordinate system with coordinates $x, y, z$ in cm, the $x$ axis points forward and the $y$ axes points upward): lateral malleolus (**lm** = [2.92, 10.10, 18.85]), medial malleolus (**mm** = [2.71, 10.22, 26.52]), fibular head (**fh** = [5.05, 41.90, 15.41]), and medial condyle (**mc** = [8.29, 41.88, 26.52]). Define the ankle joint center as the centroid between the **lm** and **mm** markers and the knee joint center as the centroid between the **fh** and **mc** markers. An anatomical coordinate system for the leg can be defined as: the quasi-vertical axis ($y$) passes through the ankle and knee joint centers; a temporary medio-lateral axis ($z$) passes through the two markers on the malleolus, an anterior-posterior as the cross product between the two former calculated orthogonal axes, and the origin at the ankle joint center.
 a) Calculate the anatomical coordinate system for the leg as described above.
 b) Calculate the rotation matrix and the translation vector for the transformation from the anatomical to the laboratory coordinate system.
 c) Calculate the position of each marker and of each joint center at the anatomical coordinate system.
 d) Calculate the Cardan angles using the $zxy$ sequence for the orientation of the leg with respect to the laboratory (but remember that the letters chosen to refer to axes are arbitrary, what matters is the directions they represent).

```python
# calculation of the joint centers
mm = np.array([2.71, 10.22, 26.52])
lm = np.array([2.92, 10.10, 18.85])
fh = np.array([5.05, 41.90, 15.41])
mc = np.array([8.29, 41.88, 26.52])
ajc = (mm + lm)/2
kjc = (fh + mc)/2
print('Poition of the ankle joint center:', ajc)
print('Poition of the knee joint center:', kjc)
```

    Poition of the ankle joint center: [  2.815  10.16   22.685]
    Poition of the knee joint center: [  6.67   41.89   20.965]

```python
# calculation of the anatomical coordinate system axes (basis)
y = kjc - ajc
x = np.cross(y, mm - lm)
z = np.cross(x, y)
print('Versors:')
x = x/np.linalg.norm(x)
y = y/np.linalg.norm(y)
z = z/np.linalg.norm(z)
print('x =', x)
print('y =', y)
print('z =', z)
Oleg = ajc
print('\nOrigin =', Oleg)
```

    Versors:
    x = [ 0.9925 -0.119   0.029 ]
    y = [ 0.1204  0.9913 -0.0537]
    z = [-0.0224  0.0568  0.9981]

    Origin = [  2.815  10.16   22.685]

```python
# Rotation matrices
RGl = np.array([x, y , z]).T
print('Rotation matrix from the anatomical to the laboratory coordinate system:\n', RGl)
RlG = RGl.T
print('\nRotation matrix from the laboratory to the anatomical coordinate system:\n', RlG)
```

    Rotation matrix from the anatomical to the laboratory coordinate system:
     [[ 0.9925  0.1204 -0.0224]
     [-0.119   0.9913  0.0568]
     [ 0.029  -0.0537  0.9981]]

    Rotation matrix from the laboratory to the anatomical coordinate system:
     [[ 0.9925 -0.119   0.029 ]
     [ 0.1204  0.9913 -0.0537]
     [-0.0224  0.0568  0.9981]]

```python
# Translational vector
OG = np.array([0, 0, 0])  # Laboratory coordinate system origin
LG = Oleg - OG
print('Translational vector from the anatomical to the laboratory coordinate system:\n', LG)
```

    Translational vector from the anatomical to the laboratory coordinate system:
     [  2.815  10.16   22.685]

To get the coordinates from the laboratory (global) coordinate system to the anatomical (local) coordinate system:

$$ \mathbf{P_l} = \mathbf{R_{lG}}\left(\mathbf{P_G}-\mathbf{L_G}\right) $$

```python
# position of each marker and of each joint center at the anatomical coordinate system
mml  = np.dot(RlG, (mm - LG))  # equivalent to the algebraic expression RlG*(mm - LG).T
lml  = np.dot(RlG, (lm - LG))
fhl = np.dot(RlG, (fh - LG))
mcl = np.dot(RlG, (mc - LG))
ajcl = np.dot(RlG, (ajc - LG))
kjcl = np.dot(RlG, (kjc - LG))
print('Coordinates of mm in the anatomical system:\n', mml)
print('Coordinates of lm in the anatomical system:\n', lml)
print('Coordinates of fh in the anatomical system:\n', fhl)
print('Coordinates of mc in the anatomical system:\n', mcl)
print('Coordinates of kjc in the anatomical system:\n', kjcl)
print('Coordinates of ajc in the anatomical system (origin):\n', ajcl)
```

    Coordinates of mm in the anatomical system:
     [-0.     -0.1592  3.8336]
    Coordinates of lm in the anatomical system:
     [-0.      0.1592 -3.8336]
    Coordinates of fh in the anatomical system:
     [ -1.7703  32.1229  -5.5078]
    Coordinates of mc in the anatomical system:
     [  1.7703  31.8963   5.5078]
    Coordinates of kjc in the anatomical system:
     [  0.      32.0096   0.    ]
    Coordinates of ajc in the anatomical system (origin):
     [ 0.  0.  0.]

## Problems

1. For the example about how the order of rotations of a rigid body affects the orientation shown in a figure above, deduce the rotation matrices for each of the 4 cases shown in the figure. For the first two cases, deduce the rotation matrices from the global to the local coordinate system and for the other two examples, deduce the rotation matrices from the local to the global coordinate system.

2. Consider the data from problem 7 in the notebook [Frame of reference](http://nbviewer.ipython.org/github/BMClab/bmc/blob/master/notebooks/ReferenceFrame.ipynb) where the following anatomical landmark positions are given (units in meters): RASIS=[0.5,0.8,0.4], LASIS=[0.55,0.78,0.1], RPSIS=[0.3,0.85,0.2], and LPSIS=[0.29,0.78,0.3]. Deduce the rotation matrices for the global to anatomical coordinate system and for the anatomical to global coordinate system.

3. For the data from the last example, calculate the Cardan angles using the $zxy$ sequence for the orientation of the leg with respect to the laboratory (but remember that the letters chosen to refer to axes are arbitrary, what matters is the directions they represent).

## References

- Corke P (2011) [Robotics, Vision and Control: Fundamental Algorithms in MATLAB](http://www.petercorke.com/RVC/). Springer-Verlag Berlin.
- Robertson G, Caldwell G, Hamill J, Kamen G (2013) [Research Methods in Biomechanics](http://books.google.com.br/books?id=gRn8AAAAQBAJ). 2nd Edition. Human Kinetics.
- [Maths - Euler Angles](http://www.euclideanspace.com/maths/geometry/rotations/euler/).
- Murray RM, Li Z, Sastry SS (1994) [A Mathematical Introduction to Robotic Manipulation](http://www.cds.caltech.edu/~murray/mlswiki/index.php/Main_Page). Boca Raton, CRC Press.
- Ruina A, Rudra P (2013) [Introduction to Statics and Dynamics](http://ruina.tam.cornell.edu/Book/index.html). Oxford University Press.
- Siciliano B, Sciavicco L, Villani L, Oriolo G (2009) [Robotics - Modelling, Planning and Control](http://books.google.com.br/books/about/Robotics.html?hl=pt-BR&id=jPCAFmE-logC). Springer-Verlag London.
- Winter DA (2009) [Biomechanics and motor control of human movement](http://books.google.com.br/books?id=_bFHL08IWfwC). 4 ed. Hoboken, USA: Wiley.
- Zatsiorsky VM (1997) [Kinematics of Human Motion](http://books.google.com.br/books/about/Kinematics_of_Human_Motion.html?id=Pql_xXdbrMcC&redir_esc=y). Champaign, Human Kinetics.

## Function `euler_rotmatrix.py`

```python
# %load ./../functions/euler_rotmat.py
#!/usr/bin/env python

"""Euler rotation matrix given sequence, frame, and angles."""

from __future__ import division, print_function

__author__ = 'Marcos Duarte, https://github.com/demotu/BMC'
__version__ = 'euler_rotmat.py v.1 2014/03/10'

def euler_rotmat(order='xyz', frame='local', angles=None, unit='deg',
                 str_symbols=None, showA=True, showN=True):
    """Euler rotation matrix given sequence, frame, and angles.

    This function calculates the algebraic rotation matrix (3x3) for a given
    sequence ('order' argument) of up to three elemental rotations of a given
    coordinate system ('frame' argument) around another coordinate system, the
    Euler (or Eulerian) angles [1]_.

    This function also calculates the numerical values of the rotation matrix
    when numerical values for the angles are inputed for each rotation axis.
    Use None as value if the rotation angle for the particular axis is unknown.

    The symbols for the angles are: alpha, beta, and gamma for the first,
    second, and third rotations, respectively.
    The matrix product is calulated from right to left and in the specified
    sequence for the Euler angles. The first letter will be the first rotation.

    The function will print and return the algebraic rotation matrix and the
    numerical rotation matrix if angles were inputed.

    Parameters
    ----------
    order  : string, optional (default = 'xyz')
             Sequence for the Euler angles, any combination of the letters
             x, y, and z with 1 to 3 letters is accepted to denote the
             elemental rotations. The first letter will be the first rotation.

    frame  : string, optional (default = 'local')
             Coordinate system for which the rotations are calculated.
             Valid values are 'local' or 'global'.

    angles : list, array, or bool, optional (default = None)
             Numeric values of the rotation angles ordered as the 'order'
             parameter. Enter None for a rotation whith unknown value.

    unit   : str, optional (default = 'deg')
             Unit of the input angles.

    str_symbols : list of strings, optional (default = None)
             New symbols for the angles, for instance, ['theta', 'phi', 'psi']

    showA  : bool, optional (default = True)
             True (1) displays the Algebraic rotation matrix in rich format.
             False (0) to not display.

    showN  : bool, optional (default = True)
             True (1) displays the Numeric rotation matrix in rich format.
             False (0) to not display.

    Returns
    -------
    R     :  Matrix Sympy object
             Rotation matrix (3x3) in algebraic format.

    Rn    :  Numpy array or Matrix Sympy object (only if angles are inputed)
             Numeric rotation matrix (if values for all angles were inputed) or
             a algebraic matrix with some of the algebraic angles substituted
             by the corresponding inputed numeric values.

    Notes
    -----
    This code uses Sympy, the Python library for symbolic mathematics, to
    calculate the algebraic rotation matrix and shows this matrix in latex form
    possibly for using with the IPython Notebook, see [1]_.

    References
    ----------
    .. [1] http://nbviewer.ipython.org/github/duartexyz/BMC/blob/master/Transformation3D.ipynb

    Examples
    --------
    >>> # import function
    >>> from euler_rotmat import euler_rotmat
    >>> # Default options: xyz sequence, local frame and show matrix
    >>> R = euler_rotmat()
    >>> # XYZ sequence (around global (fixed) coordinate system)
    >>> R = euler_rotmat(frame='global')
    >>> # Enter numeric values for all angles and show both matrices
    >>> R, Rn = euler_rotmat(angles=[90, 90, 90])
    >>> # show what is returned
    >>> euler_rotmat(angles=[90, 90, 90])
    >>> # show only the rotation matrix for the elemental rotation at x axis
    >>> R = euler_rotmat(order='x')
    >>> # zxz sequence and numeric value for only one angle
    >>> R, Rn = euler_rotmat(order='zxz', angles=[None, 0, None])
    >>> # input values in radians:
    >>> import numpy as np
    >>> R, Rn = euler_rotmat(order='zxz', angles=[None, np.pi, None], unit='rad')
    >>> # shows only the numeric matrix
    >>> R, Rn = euler_rotmat(order='zxz', angles=[90, 0, None], showA='False')
    >>> # Change the angles' symbols
    >>> R = euler_rotmat(order='zxz', str_symbols=['theta', 'phi', 'psi'])
    >>> # Negativate the angles' symbols
    >>> R = euler_rotmat(order='zxz', str_symbols=['-theta', '-phi', '-psi'])
    >>> # all algebraic matrices for all possible sequences for the local frame
    >>> s=['xyz','xzy','yzx','yxz','zxy','zyx','xyx','xzx','yzy','yxy','zxz','zyz']
    >>> for seq in s: R = euler_rotmat(order=seq)
    >>> # all algebraic matrices for all possible sequences for the global frame
    >>> for seq in s: R = euler_rotmat(order=seq, frame='global')
    """

    import numpy as np
    import sympy as sym
    try:
        from IPython.core.display import Math, display
        ipython = True
    except:
        ipython = False

    angles = np.asarray(np.atleast_1d(angles), dtype=np.float64)
    if ~np.isnan(angles).all():
        if len(order) != angles.size:
            raise ValueError("Parameters 'order' and 'angles' (when " +
                             "different from None) must have the same size.")

    x, y, z = sym.symbols('x, y, z')
    sig = [1, 1, 1]
    if str_symbols is None:
        a, b, g = sym.symbols('alpha, beta, gamma')
    else:
        s = str_symbols
        if s[0][0] == '-': s[0] = s[0][1:]; sig[0] = -1
        if s[1][0] == '-': s[1] = s[1][1:]; sig[1] = -1
        if s[2][0] == '-': s[2] = s[2][1:]; sig[2] = -1
        a, b, g = sym.symbols(s)

    var = {'x': x, 'y': y, 'z': z, 0: a, 1: b, 2: g}
    # Elemental rotation matrices for xyz (local)
    cos, sin = sym.cos, sym.sin
    Rx = sym.Matrix([[1, 0, 0], [0, cos(x), sin(x)], [0, -sin(x), cos(x)]])
    Ry = sym.Matrix([[cos(y), 0, -sin(y)], [0, 1, 0], [sin(y), 0, cos(y)]])
    Rz = sym.Matrix([[cos(z), sin(z), 0], [-sin(z), cos(z), 0], [0, 0, 1]])

    if frame.lower() == 'global':
        Rs = {'x': Rx.T, 'y': Ry.T, 'z': Rz.T}
        order = order.upper()
    else:
        Rs = {'x': Rx, 'y': Ry, 'z': Rz}
        order = order.lower()

    R = Rn = sym.Matrix(sym.Identity(3))
    str1 = r'\mathbf{R}_{%s}( ' %frame  # last space needed for order=''
    #str2 = [r'\%s'%var[0], r'\%s'%var[1], r'\%s'%var[2]]
    str2 = [1, 1, 1]
    for i in range(len(order)):
        Ri = Rs[order[i].lower()].subs(var[order[i].lower()], sig[i] * var[i])
        R = Ri * R
        if sig[i] > 0:
            str2[i] = '%s:%s' %(order[i], sym.latex(var[i]))
        else:
            str2[i] = '%s:-%s' %(order[i], sym.latex(var[i]))
        str1 = str1 + str2[i] + ','
        if ~np.isnan(angles).all() and ~np.isnan(angles[i]):
            if unit[:3].lower() == 'deg':
                angles[i] = np.deg2rad(angles[i])
            Rn = Ri.subs(var[i], angles[i]) * Rn
            #Rn = sym.lambdify(var[i], Ri, 'numpy')(angles[i]) * Rn
            str2[i] = str2[i] + '=%.0f^o' %np.around(np.rad2deg(angles[i]), 0)
        else:
            Rn = Ri * Rn

    Rn = sym.simplify(Rn)  # for trigonometric relations

    try:
        # nsimplify only works if there are symbols
        Rn2 = sym.latex(sym.nsimplify(Rn, tolerance=1e-8).n(chop=True, prec=4))
    except:
        Rn2 = sym.latex(Rn.n(chop=True, prec=4))
        # there are no symbols, pass it as Numpy array
        Rn = np.asarray(Rn)

    if showA and ipython:
        display(Math(str1[:-1] + ') =' + sym.latex(R, mat_str='matrix')))

    if showN and ~np.isnan(angles).all() and ipython:
            str2 = ',\;'.join(str2[:angles.size])
            display(Math(r'\mathbf{R}_{%s}(%s)=%s' %(frame, str2, Rn2)))

    if np.isnan(angles).all():
        return R
    else:
        return R, Rn

```


## Introduction example
Expressing the dynamics of a ship as an Ordinary Differential Equation (ODE) is a well established technique. The ODE can be solved as an initial value problem with integration of accelerations and velocities to obtain a ship's trajectory. This is commonly known as a simulation. The workflow of a simulation is to first establish a force model that can estimate the hydrodynamic forces as function of the current state. Accelerations can then be calculated from these forces together with the mass. The velocities and positions can then be determined with time integration of the acceleration.

Let's exemplify this with a simple problem, simulated a ball being dropped in air.
The forces acting on this ball will be the drag from the air, which is modelled as: $C_d \cdot \dot{x}^2$ and the gravity, which is modelled as $g \cdot m$:

```python
# %load imports.py
## Local packages:

%matplotlib inline
%load_ext autoreload
%autoreload 2
%config Completer.use_jedi = False  ## (To fix autocomplete)

## External packages:
import pandas as pd
pd.options.display.max_rows = 999
pd.options.display.max_columns = 999
pd.set_option("display.max_columns", None)

import numpy as np
import os
import matplotlib.pyplot as plt
#if os.name == 'nt':
#    plt.style.use('presentation.mplstyle')  # Windows

import plotly.express as px
import plotly.graph_objects as go

import seaborn as sns
import sympy as sp
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame,
                                      Particle, Point)
from sympy.physics.vector.printing import vpprint, vlatex
from IPython.display import display, Math, Latex, Markdown
from src.substitute_dynamic_symbols import run, lambdify

import pyro

import sklearn
import pykalman
from statsmodels.sandbox.regression.predstd import wls_prediction_std
import statsmodels.api as sm

from scipy.integrate import solve_ivp

## Local packages:
from src.data import mdl

from src.symbols import *
from src.parameters import *
import src.symbols as symbols
from src import prime_system
from src.models import regression
from src.visualization.regression import show_pred
from src.visualization.plot import track_plot
from src.equation import Equation

## Load models:
# (Uncomment these for faster loading):
import src.models.vmm_abkowitz  as vmm_abkowitz
```

```python
x = dynamicsymbols('x')
m,F,C_d,g = sp.symbols('m F C_d g')

eq_kinetics = sp.Eq(F, -m*g + C_d*x.diff()**2)
Math(vlatex(eq_kinetics))
```

The kinematics connecting the forces to motion can be described with Newtons 2nd law:

```python
eq_kinematics = sp.Eq(F, m*x.diff().diff())
Math(vlatex(eq_kinematics))
```

The acceleration can then be calculated using these two equations:

```python
eqs=[eq_kinetics,eq_kinematics]
solution = sp.solve(eqs, x.diff().diff(), m, F, dict=True)[0]
eq_acc = sp.Eq(x.diff().diff(),solution[x.diff().diff()])
Math(vlatex(eq_acc))
```

This equation can be used to simulate the motion of the ball, by doing a time step integration of this initial value problem.

```python
from scipy.integrate import solve_ivp

acceleration_lambda = lambdify(eq_acc.rhs)

inputs={
'C_d' : 0.01,
'g' : 9.81,
'm' : 1,
}

def step(t,states, inputs):

    x1d = states[1]
    x2d = acceleration_lambda(**inputs, x1d=x1d)

    dstates = [x1d, x2d]
    return dstates

t_ = np.linspace(0,10,100)
y0 = [
    0,0
]
solution = solve_ivp(fun=step, y0=y0, t_span=[t_[0],t_[-1]], t_eval=t_, args=(inputs,))

df_result = pd.DataFrame(solution.y.T, index=solution.t, columns=['x','x1d'])

fig,axes=plt.subplots(nrows=2)
ax=axes[0]
df_result.plot(y='x', label='$x$ [m]', ax=ax)

ax.set_title('Ball position')

ax=axes[1]
df_result.plot(y='x1d', label='$\dot{x}$ [m/s]', ax=axes[1])
ax.set_title('Ball velocity [m/s]')
ax.set_xlabel('time [s]')

for ax in axes:
    ax.grid(True)
plt.tight_layout()

```

This toy example is very simple, but is showing the basic idea behind time simulation of dynamic systems, which is often used to simualte the ship dynamics. The following physical parameters were used in the simulation above:

```python
for coeff in [C_d,g,m]:
    display(sp.Eq(coeff,inputs[str(coeff)]))
```

## Reversing the problem

This research is however about [Inverse dynamics](https://en.wikipedia.org/wiki/Inverse_dynamics), which is reversing the problem above. Instead of estimating a ships trajectory with a force model, we want to identify this force model, by using a measured ship trajectory. This is very useful when you want to fit a mathematical model to the measured ship motion, either obtained from ship model test or the real ship in full scale operation. The latter is something that today is becoming more and more relevant as more and more operational data is measure and recorded onboard the ships.

### Example
Let's reverse the simple ball drop example above by trying to identify the drag coefficient $C_d$ and graviational constant $g$ based on a measured motion of the ball. We will pretend that the simulated motion above is a measurement.

The equation for the forces acting on the ball can be expressed as s regression problem:

$ y = X \cdot \beta + \epsilon $

Where $X$ is a matrix with features and $\beta$ is a a vector with the parameters that should be regressed. $\epsilon$ is the prediction error. For the current problem the force equation is rewritten as a regression problem below:

```python
diff_eq = regression.DiffEqToMatrix(ode=eq_kinetics, label=F, base_features=[x])
Math(vlatex(diff_eq.acceleration_equation))
```

```python
Math(vlatex(diff_eq.eq_X))
```

```python
Math(vlatex(diff_eq.eq_beta))
```

```python
Math(vlatex(diff_eq.eq_y))
```

```python
kinetics_lambda = lambdify(eq_kinetics.rhs)
df_result['F'] = run(function=kinetics_lambda, inputs=inputs, **df_result)
```

And this regression problem can be solved using Ordinary Least Square (OLS) regression:

```python
X = diff_eq.calculate_features(data=df_result)
y = diff_eq.calculate_label(y=df_result['F'])

model = sm.OLS(y,X)
results = model.fit()

show_pred(X=X,y=y,results=results, label=r'$F$')
```

Indentifying the parameters of a mathematical model expressing the dynamics of a system as simple as a droped ball is fairly easy as seen in the examples above. The following research in this book will focus on solving the same problem but for a more complex system as a ship based on data with much more noise than in the perfect data from the simulation example above.

## So why is it convenient to have a mathematical model of your ship?

```python

```


# LDA/QDA on height/weight data

We're asked to fit a Linear Discriminant Analysis (LDA) and Quadratic Discriminant Analysis (QDA) model to the height/weight data and compute the the misclassification rate. See my implementation of these algorithms on [GitHub](https://github.com/ppham27/MLaPP-solutions/blob/master/chap04/DiscriminantAnalysis.py). The data can be found [here](https://github.com/ppham27/MLaPP-solutions/blob/master/chap04/heightWeightData.txt).

```python
%matplotlib inline

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from matplotlib.colors import ListedColormap
# benchmark sklearn implementations, these are much faster
from sklearn.discriminant_analysis import LinearDiscriminantAnalysis
from sklearn.discriminant_analysis import QuadraticDiscriminantAnalysis
# my own implementation, results are identical but runs much slower
import DiscriminantAnalysis

raw_data = pd.read_csv("heightWeightData.txt", header=None, names=["gender", "height", "weight"])
raw_data.info()
raw_data.head()
```

    <class 'pandas.core.frame.DataFrame'>
    RangeIndex: 210 entries, 0 to 209
    Data columns (total 3 columns):
    gender    210 non-null int64
    height    210 non-null int64
    weight    210 non-null int64
    dtypes: int64(3)
    memory usage: 5.0 KB

      gender
      height
      weight

      0
      1
      67
      125

      1
      2
      68
      140

      2
      2
      67
      142

      3
      2
      60
      110

      4
      2
      64
      97

Let's plot the data, coloring males blue and females red, too.

```python
plt.figure(figsize=(8,8))
labels = {1: 'Male', 2: 'Female'}
colors = {1: 'blue', 2: 'red'}
def plot_height_weight(title="Weight vs Height", ax=None):
    if ax == None:
        ax = plt.gca()
    for name, group in raw_data.groupby('gender'):
        ax.scatter(group.height, group.weight, color=colors[name], label=labels[name])
    ax.set_title(title)
    ax.set_xlabel('height')
    ax.set_ylabel('weight')
    ax.legend(loc='upper left')
    ax.grid()
plot_height_weight()
plt.show()
```

Let's define a few globals to help with the plotting and model fitting.

```python
x_min = 50
x_max = 85
y_min = 50
y_max = 300
X = raw_data[['height', 'weight']].as_matrix()
y = raw_data['gender'].as_matrix()
xx, yy = np.meshgrid(np.linspace(x_min, x_max, num=200, endpoint=True),
                     np.linspace(y_min, y_max, num=200, endpoint=True))
cmap_light = ListedColormap(['#AAAAFF','#FFAAAA'])
def plot_height_weight_mesh(xx, yy, Z, comment=None, title=None, ax=None):
    if ax == None:
        ax = plt.gca()
    ax.pcolormesh(xx, yy, Z, cmap=cmap_light)
    if title == None:
        plot_height_weight(ax=ax)
    else:
        plot_height_weight(ax=ax, title = title)
    ax.set_xlim([x_min, x_max])
    ax.set_ylim([y_min, y_max])
    if comment != None:
        ax.text(0.95, 0.05, comment, transform=ax.transAxes,
                verticalalignment="bottom", horizontalalignment="right",
                fontsize=14)
def decimal_to_percent(x, decimals=2):
    return '{0:.2f}%'.format(np.round(100*x, decimals=2))
```

Let's try the QDA model first. We follow the conventions of the `sklearn` implementation. Let $X$ be our data or design matrix and $y$ be our class label. The rows of $X$ consist of the vectors $\mathbf{x}_i$, which are the individual observations. We let $y_i \sim \mathrm{Multinomial}\left(\theta_1,\ldots,\theta_K\right)$, where $\sum_{k=1}^K \theta_k = 1$. We let $\mathbf{x}_i \mid y_i = k \sim \mathcal{N}\left(\mathbf{\mu}_k, \Sigma_k\right)$, which is a multivariate normal. We use the following estimates for the parameters:

\begin{align}
\hat{\theta}_k &= \frac{N_k}{N} \\
\hat{\mu}_k &= \frac{1}{N_k}\sum_{\{i~:~ y_i = k\}}\mathbf{x}_i \\
\hat{\Sigma}_k &= \frac{1}{N_k - 1}\sum_{\{i~:~y_i = k\}} \left(\mathbf{x}_i - \hat{\mu}_k\right)\left(\mathbf{x}_i - \hat{\mu}_k\right)^\intercal.
\end{align}

$N$ is total number of observations, and $N_k$ is the number of observations of class $k$. Thus, for each class $k$, we compute the proportion of observations that are of that class, the sample mean, and the unbiased estimate for covariance.

```python
# qda = QuadraticDiscriminantAnalysis(store_covariances=True) # sklearn implementation
qda = DiscriminantAnalysis.QDA() # my implementation
qda.fit(X, y)
qda_misclassification = 1 - qda.score(X, y)
qda_Z = qda.predict(np.c_[xx.ravel(), yy.ravel()])
qda_Z = qda_Z.reshape(xx.shape)
plt.figure(figsize=(8,8))
plot_height_weight_mesh(xx, yy, qda_Z, title="Weight vs Height: QDA",
                        comment="Misclassification rate: " + decimal_to_percent(qda_misclassification))
plt.show()
```

Let's look at LDA now. We compute $\hat{\theta}_k$ and $\hat{\mu}_k$ in the same manner. However, now we have that all covariances are equal, that is, $\hat{\Sigma} = \hat{\Sigma}_k$ for all $k$. Let $p$ be the number of features, that is, the number of columns in $X$. First, we note that

\begin{align}
\log p\left(\mathcal{D} \mid \boldsymbol\mu, \Sigma,\boldsymbol\theta\right)
&= \sum_{i=1}^N \log p\left(\mathbf{x}_i,y_i \mid \boldsymbol\mu, \Sigma,\boldsymbol\theta\right)
= \sum_{k = 1}^K \sum_{\{i~:~y_i = k\}} \log p\left(\mathbf{x}_i, y_i = k \mid \mu_k, \Sigma, \theta_k\right) \\
&= \sum_{k = 1}^K \sum_{\{i~:~y_i = k\}} \left[\log p(y_i = k) + \log p\left(\mathbf{x}_i \mid \mu_k, \Sigma, y_i=k\right)\right] \\
&= \sum_{k = 1}^K \sum_{\{i~:~y_i = k\}} \left[\log \theta_k - \frac{p}{2}\log 2\pi - \frac{1}{2}\log|\Sigma|
- \frac{1}{2}\left(\mathbf{x}_i - \mu_k\right)^\intercal\Sigma^{-1}\left(\mathbf{x}_i - \mu_k\right)\right] \\
&= -\frac{Np}{2}\log 2\pi -\frac{N}{2}\log|\Sigma| + \sum_{k = 1}^K \left(N_k\log\theta_k - \frac{1}{2}\sum_{\{i~:~y_i = k\}}\left(\mathbf{x}_i - \mu_k\right)^\intercal\Sigma^{-1}\left(\mathbf{x}_i - \mu_k\right)\right).
\end{align}

Let $\Lambda = \Sigma^{-1}$. The MLE is invariant with regard to reparameterization, so after isolating the terms that involve $\Sigma$ we can focus on maximizing

\begin{align}
l(\Lambda) &= \frac{N}{2}\log|\Lambda| - \frac{1}{2}\sum_{k = 1}^K \sum_{\{i~:~y_i = k\}}\left(\mathbf{x}_i - \mu_k\right)^\intercal\Lambda\left(\mathbf{x}_i - \mu_k\right) \\
&= \frac{N}{2}\log|\Lambda| - \frac{1}{2}\sum_{k = 1}^K \sum_{\{i~:~y_i = k\}}\operatorname{tr}\left(\left(\mathbf{x}_i - \mu_k\right)\left(\mathbf{x}_i - \mu_k\right)^\intercal\Lambda\right),
\end{align}

where we have used the fact that $\left(\mathbf{x}_i - \mu_k\right)^\intercal\Sigma^{-1}\left(\mathbf{x}_i - \mu_k\right)$ is a scalar, so we can replace it with the trace, and then, we apply the fact that the trace remains the same after cyclic permutations.

Now, we note these two identities to help us take the derivative with respect to $\Lambda$,

\begin{align}
\frac{\partial}{\partial\Lambda}\log|\Lambda| &= \left(\Lambda^{-1}\right)^\intercal \\
\frac{\partial}{\partial\Lambda}\operatorname{tr}\left(A\Lambda\right) &= A^\intercal.
\end{align}

Thus, we'll have that

\begin{align}
l^\prime(\Lambda) &= \frac{N}{2}\left(\Lambda^{-1}\right)^\intercal - \frac{1}{2}\sum_{k=1}^K\left(\sum_{\{i~:~y_i = k\}}\left(\mathbf{x}_i - \mu_k\right)\left(\mathbf{x}_i - \mu_k\right)^\intercal\right) \\
&= \frac{N}{2}\Sigma - \frac{1}{2}\sum_{k=1}^K\left(\sum_{\{i~:~y_i = k\}}\left(\mathbf{x}_i - \mu_k\right)\left(\mathbf{x}_i - \mu_k\right)^\intercal\right)
\end{align}

since $\Lambda^{-1} = \Sigma$ and $\Sigma$ is symmetric.

Setting $l^\prime(\Lambda) = 0$ and solving for $\Sigma$, we have that

\begin{equation}
\Sigma = \frac{1}{N} \sum_{k=1}^K\left(\sum_{\{i~:~y_i = k\}}\left(\mathbf{x}_i - \mu_k\right)\left(\mathbf{x}_i - \mu_k\right)^\intercal\right).
\end{equation}

In this manner our estimate is

\begin{equation}
\hat{\Sigma} = \frac{1}{N} \sum_{k=1}^K\left(\sum_{\{i~:~y_i = k\}}\left(\mathbf{x}_i - \hat{\mu}_k\right)\left(\mathbf{x}_i - \hat{\mu}_k\right)^\intercal\right).
\end{equation}

For whatever reason, `sklearn` uses the biased MLE estimate for covariance in LDA, but it uses the unbiased estimate for covariance in QDA.

```python
# lda = LinearDiscriminantAnalysis(store_covariance=True) # sklearn implementation
lda = DiscriminantAnalysis.LDA() # my implementation
lda.fit(X, y)
lda_misclassification = 1 - lda.score(X, y)
lda_Z = lda.predict(np.c_[xx.ravel(), yy.ravel()])
lda_Z = lda_Z.reshape(xx.shape)
plt.figure(figsize=(8,8))
plot_height_weight_mesh(xx, yy, lda_Z, title="Weight vs Height: LDA",
                        comment="Misclassification rate: " + decimal_to_percent(lda_misclassification))
plt.show()
```

Clearly, we see that the two methods give us different decision boundaries. However, for these data, the misclassification rate is the same.

```python
fig = plt.figure(figsize=(14,8))
ax1 = fig.add_subplot(1,2,1)
ax2 = fig.add_subplot(1,2,2)
plot_height_weight_mesh(xx, yy, qda_Z, title="Weight vs Height: QDA", ax=ax1,
                        comment="Misclassification rate: " + decimal_to_percent(qda_misclassification))
plot_height_weight_mesh(xx, yy, lda_Z, title="Weight vs Height: LDA", ax=ax2,
                        comment="Misclassification rate: " + decimal_to_percent(lda_misclassification))
fig.suptitle('Comparison of QDA and LDA', fontsize=18)
plt.show()
```


```python
from sympy import *
from sympy import pprint
from sympy.core.numbers import mod_inverse
a,b,c,d = symbols('a,b,c,d')
```

```python
G = Matrix([[2, 1, 3, 2],
            [4, 2, 0, 1]
           ])
```

```python
def mod(x,modulus):
    numer, denom = x.as_numer_denom()
    return numer*mod_inverse(denom,modulus) % modulus
```

```python
G_rref = G.rref(iszerofunc=lambda x: x % 5 == 0)
pprint(G_rref[0].applyfunc(lambda x: mod(x,5)))
```

```python
F = Matrix([[4,3,1,3],[2,4,1,3]])
F.rref(iszerofunc=lambda x: x % 5 == 0)
pprint(F_rref[0].applyfunc(lambda x: mod(x,5)))
```

```python

```