Marcos Duarte, Renato Naville Watanabe
Laboratory of Biomechanics and Motor Control
Federal University of ABC, Brazil
euler_rotmatrix.py
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. 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) 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.
# Import the necessary libraries
import numpy as np
# suppress scientific notation for small numbers:
np.set_printoptions(precision=4, suppress=True)
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.
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:
Or in terms of its components:
\begin{equation} \begin{array}{ll} \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} \end{equation}And in matrix form:
\begin{equation} \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} \end{equation}From classical mechanics, this is an example of Galilean transformation.
Let's use Python to compute some numeric examples:
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:
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):
Pl = np.array([[1, 2, 3],
[4, 5, 6],
[7, 8, 9]]) # 2D array with 3 rows and 3 columns
PG = LG + Pl
PG
array([[ 2, 4, 6], [ 5, 7, 9], [ 8, 10, 12]])
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).
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.
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:
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.
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.
Analogue to what we have described in the notebook Rigid-body transformations in a plane (2D), the columns (or rows) of the 3D rotation matrix $\mathbf{R_{Gl}}$ (or $\mathbf{R_{lG}}$) between two coordinate systems are the versors of the basis of the rotated coordinate system.
For example, consider that we have measured the position of at least three non-collinear markers placed on the rigid body, let's calculate a basis with these positions and then the rotation matrix.
If we have the position of three markers: m1, m2, m3, a basis (formed by three orthogonal versors) can be found as:
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:
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)))
RGl = np.array([v1, v2, v3]).T # or np.vstack((v1, v2, v3)).T
print('\nRotation matrix RGl:\n', RGl)
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.0000000000000002 Rotation matrix RGl: [[-0.7071 0.5774 0.4082] [ 0.7071 0.5774 0.4082] [ 0. 0.5774 -0.8165]]
However, we have the same problem as with the rotation matrix deduced from the direction cosines; there is no simple way to determine/interpret the angles of rotation from this matrix!
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, 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 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.
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.
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:
\begin{equation} \mathbf{R_{Gl,\,X}} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{bmatrix} \end{equation}Around $\mathbf{Y}$ axis:
\begin{equation} \mathbf{R_{Gl,\,Y}} = \begin{bmatrix} \cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & \cos\beta \end{bmatrix} \end{equation}Around $\mathbf{Z}$ axis:
\begin{equation} \mathbf{R_{Gl,\,Z}} = \begin{bmatrix} \cos\gamma & -\sin\gamma & 0\\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{equation}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$).
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:
\begin{equation} \mathbf{R}_{\mathbf{lG},\,x} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & \sin\alpha \\ 0 & -\sin\alpha & \cos\alpha \end{bmatrix} \end{equation}Around $y$ axis:
\begin{equation} \mathbf{R}_{\mathbf{lG},\,y} = \begin{bmatrix} \cos\beta & 0 & -\sin\beta \\ 0 & 1 & 0 \\ \sin\beta & 0 & \cos\beta \end{bmatrix} \end{equation}Around $z$ axis:
\begin{equation} \mathbf{R}_{\mathbf{lG},\,z} = \begin{bmatrix} \cos\gamma & \sin\gamma & 0\\ -\sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{equation}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)$.
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.
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:
Note the order of the matrices.
We can check this matrix multiplication using Sympy:
#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(r'\mathbf{R_{Gl,\,XYZ}}=' + sym.latex(RXYZ,
mat_str='matrix')))
For instance, we can calculate the numerical rotation matrix for these sequential elemental rotations by $90^o$ around $\mathbf{X,Y,Z}$:
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(3, chop=True))))
Below you can test any sequence of rotation around the global coordinates. Just change the matrix R, and the angles of the variables $\alpha$, $\beta$ and $\gamma$. In the example below is the rotation around the global basis, in the sequence x,y,z, with the angles $\alpha=\pi/3$ rad, $\beta=\pi/4$ rad and $\gamma=\pi/6$ rad.
import sys
sys.path.insert(1, r'./../functions') # add to pythonpath
%matplotlib notebook
import matplotlib.pyplot as plt
from CCS import CCS
R = RZ*RY*RX
R = sym.lambdify((a, b, g), R, 'numpy')
alpha = np.pi/2
beta = np.pi/2
gamma = np.pi/2
R = R(alpha, beta, gamma)
e1 = np.array([[1, 0, 0]])
e2 = np.array([[0, 1, 0]])
e3 = np.array([[0, 0, 1]])
basis = np.vstack((e1, e2, e3))
basisRot = R @ basis
CCS(Oijk=np.array([0, 0, 0]), Oxyz=np.array([0, 0, 0]),
ijk=basis.T, xyz=basisRot.T, vector=False)
plt.show()
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.
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):
As before, the order of the matrices is from right to left.
Once again, we can check this matrix multiplication using Sympy:
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(r'\mathbf{R}_{\mathbf{lG},\,xyz}=' + sym.latex(Rxyz, mat_str='matrix'))
For instance, let's calculate the numerical rotation matrix for these sequential elemental rotations by $90^o$ around $x,y,z$:
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(3, chop=True))))
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:
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(3, chop=True))))
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]$.
Below you can test any sequence of rotation, around the local coordinates. Just change the matrix R, and the angles of the variables $\alpha$, $\beta$ and $\gamma$. In the example below is the rotation around the local basis, in the sequence x,y,z, with the angles $\alpha=\pi/3$ rad, $\beta=\pi/4$ rad and $\gamma=\pi/2$ rad.
R = Rz*Ry*Rx
R = sym.lambdify((a, b, g), R, 'numpy')
alpha = np.pi/3
beta = np.pi/4
gamma = np.pi/6
R = R(alpha, beta, gamma)
e1 = np.array([[1, 0, 0]])
e2 = np.array([[0, 1, 0]])
e3 = np.array([[0, 0, 1]])
basis = np.vstack((e1, e2, e3))
basisRot = R @ basis
CCS(Oijk=np.array([0, 0, 0]), Oxyz=np.array([0, 0, 0]),
ijk=basisRot.T, xyz=basis.T, vector=False)
plt.show()
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$:
Confer that by examining the $\mathbf{R_{Gl,\,XYZ}}$ and $\mathbf{R}_{\mathbf{lG},\,xyz}$ matrices above.
Let's verify this property with Sympy:
RXYZ = RZ*RY*RX
# Rotation matrix of xyz in relation to XYZ:
display(Math(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(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(r'\mathbf{R_{Gl,\,XYZ\,}}(\alpha,\beta,\gamma) \;==\;' + \
r'\mathbf{R}_{\mathbf{lG},\,xyz\,}(-\alpha,-\beta,-\gamma)'))
RXYZ == Rxyz_neg
True
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:
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:
RZYX = RX * RY * RZ
Rxyz = Rz * Ry * Rx
display(Math(r'\mathbf{R_{Gl,\,ZYX\,}^T}=' + sym.latex(RZYX.T, mat_str='matrix')))
display(Math(r'\mathbf{R}_{\mathbf{lG},\,xyz\,}(\alpha,\beta,\gamma) \,==\,' + \
r'\mathbf{R_{Gl,\,ZYX\,}^T}(\gamma,\beta,\alpha)'))
Rxyz == RZYX.T
True
We saw in the notebook Rigid-body transformations in a plane (2D) 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:
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:
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:
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).
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 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:
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).
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:
from euler_rotmat import euler_rotmat
Ra, Rn = euler_rotmat(order='zxz', frame='local', angles=[90, 90, 90])
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.
Once a convention is adopted, the corresponding three Euler angles of rotation can be found.
For example, for the $\mathbf{R}_{xyz}$ rotation matrix:
R = euler_rotmat(order='xyz', frame='local')
The corresponding Cardan angles for the xyz
sequence can be given by:
Note that we prefer to use the mathematical function arctan2
rather than simply arcsin
, arccos
or arctan
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) 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:
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:
Ra, Rn = euler_rotmat(order='xyz', frame='local',
angles=[45, 45, 45], showA=False)
Let's check that calculating back the Cardan angles from this rotation matrix using the euler_angles_from_rot_xyz()
function:
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, but we will not see that now.
Let's see a problem with using Euler angles known as gimbal lock.
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$:
Which results in:
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(r'\mathbf{R}_{zxz}=' + sym.latex(Rzxz, mat_str='matrix'))
Let's examine what happens with this rotation matrix when the rotation around the second axis ($x$) by $\beta$ is zero:
\begin{equation} \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} \end{equation}The second matrix is the identity matrix and has no effect on the product of the matrices, which will be:
Rzxz = Rz2*Rz
Math(r'\mathbf{R}_{xyz}(\alpha, \beta=0, \gamma)=' + \
sym.latex(Rzxz, mat_str='matrix'))
Which simplifies to:
Rzxz = sym.simplify(Rzxz)
Math(r'\mathbf{R}_{xyz}(\alpha, \beta=0, \gamma)=' + \
sym.latex(Rzxz, mat_str='matrix'))
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
:
Ra, Rn = euler_rotmat(order='xyz', frame='local',
angles=[None, 90., None], showA=False)
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.
Let's revise the example where we determined the rotation matrix by building a basis given at least three non-collinear points, but now we will find the Euler angles of rotation.
Given the positions m1 = [1,0,0], m2 = [0,1,0], m3 = [0,0,1], a basis can be found:
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)
Versors: v1 = [-0.7071 0.7071 0. ] v2 = [0.5774 0.5774 0.5774] v3 = [ 0.4082 0.4082 -0.8165]
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:
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) as seen in this notebook about reference frames.
Similarly to the bidimensional case, to compute the rotation matrix between two local coordinate systems we can use the rotation matrices of both coordinate systems:
\begin{equation} R_{l_1l_2} = R_{Gl_1}^TR_{Gl_2} \end{equation}After this, the Euler angles between both coordinate systems can be found using the arctan2
function as shown previously.
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.
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:
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:
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.
It is possible to combine the translation and rotation operations in only one matrix, called the transformation matrix:
Or simply:
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:
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).
# 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]
# 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]
# 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]]
# 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:
# 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.]
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.
Consider the data from problem 7 in the notebook Frame of reference 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.
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).
Write down 4*4 matrices for each of the following (from http://www.eecs.qmul.ac.uk/~sgg/cg/Exers/transformations_ex.html):
To translate by the vector (1, 2, 3)
To scale with respect to the origin by the amount (2, 4, 6)
To rotate around the z-axis by 45 degrees (note sin 45 = cos 45 = 1/sqrt(2))
To rotate around the x-axis by 45 degrees.
Solve the first two problems from https://rrg.utk.edu/resources/BME473/assignments/BME473_homework_3.pdf.
euler_rotmatrix.py
¶# %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
Using Pandas, to load a .trc file, we must specify the parameters:
import numpy as np
import pandas as pd
data = pd.read_csv('./../data/walk.trc', sep='\t', header=None, skiprows=6)
data
0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | ... | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | |
---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
0 | 1 | 0.000 | 516.54236 | 966.88000 | -306.10416 | 531.67438 | 981.34631 | -560.16077 | 315.74045 | 977.08398 | ... | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN |
1 | 2 | 0.007 | 523.92200 | 967.96594 | -308.23773 | 539.83044 | 982.78345 | -561.77612 | 323.29425 | 977.64166 | ... | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN |
2 | 3 | 0.013 | 531.20807 | 968.92493 | -310.12112 | 547.60663 | 984.00653 | -563.42725 | 330.56866 | 978.15283 | ... | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN |
3 | 4 | 0.020 | 538.24219 | 969.77612 | -311.72064 | 555.40649 | 985.09637 | -564.85162 | 337.63867 | 978.81207 | ... | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN |
4 | 5 | 0.027 | 545.11420 | 970.81128 | -313.07266 | 563.14301 | 986.00916 | -566.06659 | 344.50589 | 979.21619 | ... | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN |
... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... |
179 | 180 | 1.193 | 1875.04834 | 958.35535 | -290.67004 | 1897.06970 | 960.27222 | -546.90094 | 1678.10901 | 977.31805 | ... | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN |
180 | 181 | 1.200 | 1885.37988 | 958.09222 | -291.67429 | 1907.52881 | 960.81549 | -547.97144 | 1688.67249 | 977.12646 | ... | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN |
181 | 182 | 1.207 | 1895.50452 | 957.80798 | -292.71875 | 1917.97290 | 961.49707 | -548.99799 | 1699.17065 | 977.02045 | ... | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN |
182 | 183 | 1.213 | 1905.40540 | 957.61029 | -293.84250 | 1928.34631 | 962.31494 | -550.08704 | 1709.65186 | 976.93237 | ... | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN |
183 | 184 | 1.220 | 1916.91516 | 958.57813 | -294.85443 | 1938.59363 | 963.29272 | -551.26306 | 1720.05396 | 976.89636 | ... | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN | NaN |
184 rows × 168 columns
But now the columns of the pandas dataframe don't have names and it will be easier if the columns have as names the marker's name (line 4 of the .trc file) and its direction (line 5).
The solution is to first read only the header of the .trc file to get the markers' names and directions and read a second time only to get the numeric data.
We wrote a function to do that, named 'read_trc.py' and it is stored in the functions directory of the BMC repository.
Here is how to use this function:
import sys
sys.path.insert(1, r'./../functions') # add to pythonpath
from read_trc import read_trc
h, data = read_trc('./../data/walk.trc', fname2='', dropna=False, na=0.0, fmt='uni')
data
Opening file "./../data/walk.trc" ... Number of markers changed from 28 to 55. done.
Frame# | Time | R.ASISx | R.ASISy | R.ASISz | L.ASISx | L.ASISy | L.ASISz | R.PSISx | R.PSISy | ... | V_R.TT_KJCz | V_L.TT_KJCx | V_L.TT_KJCy | V_L.TT_KJCz | V_R.MT2x | V_R.MT2y | V_R.MT2z | V_L.MT2x | V_L.MT2y | V_L.MT2z | |
---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
0 | 1 | 0.000 | 516.54236 | 966.88000 | -306.10416 | 531.67438 | 981.34631 | -560.16077 | 315.74045 | 977.08398 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
1 | 2 | 0.007 | 523.92200 | 967.96594 | -308.23773 | 539.83044 | 982.78345 | -561.77612 | 323.29425 | 977.64166 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
2 | 3 | 0.013 | 531.20807 | 968.92493 | -310.12112 | 547.60663 | 984.00653 | -563.42725 | 330.56866 | 978.15283 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
3 | 4 | 0.020 | 538.24219 | 969.77612 | -311.72064 | 555.40649 | 985.09637 | -564.85162 | 337.63867 | 978.81207 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
4 | 5 | 0.027 | 545.11420 | 970.81128 | -313.07266 | 563.14301 | 986.00916 | -566.06659 | 344.50589 | 979.21619 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... |
179 | 180 | 1.193 | 1875.04834 | 958.35535 | -290.67004 | 1897.06970 | 960.27222 | -546.90094 | 1678.10901 | 977.31805 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
180 | 181 | 1.200 | 1885.37988 | 958.09222 | -291.67429 | 1907.52881 | 960.81549 | -547.97144 | 1688.67249 | 977.12646 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
181 | 182 | 1.207 | 1895.50452 | 957.80798 | -292.71875 | 1917.97290 | 961.49707 | -548.99799 | 1699.17065 | 977.02045 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
182 | 183 | 1.213 | 1905.40540 | 957.61029 | -293.84250 | 1928.34631 | 962.31494 | -550.08704 | 1709.65186 | 976.93237 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
183 | 184 | 1.220 | 1916.91516 | 958.57813 | -294.85443 | 1938.59363 | 963.29272 | -551.26306 | 1720.05396 | 976.89636 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
184 rows × 167 columns
h, data = read_trc('./../data/walk.trc', fname2='', dropna=False, na=0.0, fmt='multi')
data
Opening file "./../data/walk.trc" ... Number of markers changed from 28 to 55. done.
Marker | R_ASIS | L_ASIS | R_PSIS | L_PSIS | ... | V_R_TT_KJC | V_L_TT_KJC | V_R_MT2 | V_L_MT2 | ||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
Coordinate | X | Y | Z | X | Y | Z | X | Y | Z | X | ... | Z | X | Y | Z | X | Y | Z | X | Y | Z |
XYZ | X1 | Y1 | Z1 | X2 | Y2 | Z2 | X3 | Y3 | Z3 | X4 | ... | Z52 | X53 | Y53 | Z53 | X54 | Y54 | Z54 | X55 | Y55 | Z55 |
Time | |||||||||||||||||||||
0.000 | 516.54236 | 966.88000 | -306.10416 | 531.67438 | 981.34631 | -560.16077 | 315.74045 | 977.08398 | -388.89532 | 312.81592 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
0.007 | 523.92200 | 967.96594 | -308.23773 | 539.83044 | 982.78345 | -561.77612 | 323.29425 | 977.64166 | -391.11392 | 320.88770 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
0.013 | 531.20807 | 968.92493 | -310.12112 | 547.60663 | 984.00653 | -563.42725 | 330.56866 | 978.15283 | -393.34290 | 328.17276 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
0.020 | 538.24219 | 969.77612 | -311.72064 | 555.40649 | 985.09637 | -564.85162 | 337.63867 | 978.81207 | -395.60764 | 335.90649 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
0.027 | 545.11420 | 970.81128 | -313.07266 | 563.14301 | 986.00916 | -566.06659 | 344.50589 | 979.21619 | -397.85052 | 343.31287 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... | ... |
1.193 | 1875.04834 | 958.35535 | -290.67004 | 1897.06970 | 960.27222 | -546.90094 | 1678.10901 | 977.31805 | -385.06229 | 1678.84143 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
1.200 | 1885.37988 | 958.09222 | -291.67429 | 1907.52881 | 960.81549 | -547.97144 | 1688.67249 | 977.12646 | -386.23868 | 1689.18640 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
1.207 | 1895.50452 | 957.80798 | -292.71875 | 1917.97290 | 961.49707 | -548.99799 | 1699.17065 | 977.02045 | -387.41364 | 1699.72668 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
1.213 | 1905.40540 | 957.61029 | -293.84250 | 1928.34631 | 962.31494 | -550.08704 | 1709.65186 | 976.93237 | -388.50803 | 1710.00281 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
1.220 | 1916.91516 | 958.57813 | -294.85443 | 1938.59363 | 963.29272 | -551.26306 | 1720.05396 | 976.89636 | -389.46796 | 1720.32813 | ... | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
184 rows × 165 columns