Rotation Matrices

Definition of rotation matrices

  • rotmat.R() … 3D rotation matrix for rotation about a coordinate axis

Conversion Routines

  • rotmat.convert() … Convert a rotation matrix to the corresponding quaternion

  • rotmat.sequence() … Calculation of Euler, Fick/aeronautic, Helmholtz angles

  • rotmat.seq2quat() … Calculation of quaternions from Euler, Fick/aeronautic, Helmholtz angles

Symbolic matrices

  • rotmat.R_s() … symbolix matrix for rotation about a coordinate axis

For example, you can e.g. generate a Fick-matrix, with

R_Fick = R_s(2, ‘theta’) * R_s(1, ‘phi’) * R_s(0, ‘psi’)

Note: For displaying the Greek symbols for LaTeX expressions, such as ‘psi’, you may have to add the following lines to your Jupyter terminal:

>>> from sympy.interactive import printing
>>> printing.init_printing(use_latex=True)

Spatial Transformation Matrices

  • rotmat.stm() … spatial transformation matrix, for combined rotations/translations

  • rotmat.stm_s() … symbolix spatial transformation matrix

Denavit-Hartenberg Transformations

  • rotmat.dh() … Denavit-Hartenberg transformation matrix

  • rotmat.dh_s() … symbolic Denavit-Hartenberg transformation matrix

Details

Routines for working with rotation matrices

rotmat.R(axis='x', angle=90)[source]

Rotation matrix for rotation about a cardinal axis. The argument is entered in degree.

Parameters:

  • axis (int or str) – Axis of rotation, has to be 0, 1, or 2, or ‘x’, ‘y’, or ‘z’

  • angle (float) – rotation angle [deg]

Returns:

R

Return type:

rotation matrix, for rotation about the specified axis

Examples

>>> rotmat.R(axis='x', angle=45)
array([[ 1.        ,  0.        ,  0.        ],
       [ 0.        ,  0.70710678, -0.70710678],
       [ 0.        ,  0.70710678,  0.70710678]])

>>> rotmat.R(axis='x')
array([[ 1.        ,  0.        ,  0.        ],
       [ 0.        ,  0.        , -1.        ],
       [ 0.        ,  1.        ,  0.       ]])

>>> rotmat.R('y', 45)
array([[ 0.70710678,  0.        ,  0.70710678],
       [ 0.        ,  1.        ,  0.        ],
       [-0.70710678,  0.        ,  0.70710678]])

>>> rotmat.R(axis=2, angle=45)
array([[ 0.70710678, -0.70710678,  0.        ],
       [ 0.70710678,  0.70710678,  0.        ],
       [ 0.        ,  0.        ,  1.        ]])
rotmat.R_s(axis='x', angle='alpha')[source]

Symbolic rotation matrix about the given axis, by an angle with the given name

Parameters:

  • axis (int or str) – Axis of rotation, has to be 0, 1, or 2, or ‘x’, ‘y’, or ‘z’

  • alpha (string) – name of rotation angle

Returns:

R_symbolic

Return type:

symbolic matrix for rotation about the given axis

Examples

>>> R_yaw = R_s(axis=2, angle='theta')

>>> R_nautical = R_s(2) * R_s(1) * R_s(0)
rotmat.convert(rMat, to='quat')[source]

Converts a rotation matrix to the corresponding quaternion. Assumes that R has the shape (3,3), or the matrix elements in columns

Parameters:

  • rMat (array, shape (3,3) or (N,9)) – single rotation matrix, or matrix with rotation-matrix elements.

  • to (string) – Currently, only ‘quat’ is supported

Returns:

outQuat – corresponding quaternion vector(s)

Return type:

array, shape (4,) or (N,4)

Notes

\[\begin{split}\vec q = 0.5*copysign\left( {\begin{array}{*{20}{c}} {\sqrt {1 + {R_{xx}} - {R_{yy}} - {R_{zz}}} ,}\\ {\sqrt {1 - {R_{xx}} + {R_{yy}} - {R_{zz}}} ,}\\ {\sqrt {1 - {R_{xx}} - {R_{yy}} + {R_{zz}}} ,} \end{array}\begin{array}{*{20}{c}} {{R_{zy}} - {R_{yz}}}\\ {{R_{xz}} - {R_{zx}}}\\ {{R_{yx}} - {R_{xy}}} \end{array}} \right)\end{split}\]

More info under http://en.wikipedia.org/wiki/Quaternion

Examples

>>> rotMat = array([[cos(alpha), -sin(alpha), 0],
>>>    [sin(alpha), cos(alpha), 0],
>>>    [0, 0, 1]])
>>> rotmat.convert(rotMat, 'quat')
array([[ 0.99500417,  0.        ,  0.        ,  0.09983342]])
rotmat.dh(theta=0, d=0, r=0, alpha=0)[source]

Denavit Hartenberg transformation and rotation matrix.

\[T_n^{n - 1}= {Trans}_{z_{n - 1}}(d_n) \cdot {Rot}_{z_{n - 1}}(\theta_n) \cdot {Trans}_{x_n}(r_n) \cdot {Rot}_{x_n}(\alpha_n)\]
\[\begin{split}T_n=\left[\begin{array}{ccc|c} \cos\theta_n & -\sin\theta_n \cos\alpha_n & \sin\theta_n \sin\alpha_n & r_n\cos\theta_n \\ \sin\theta_n & \cos\theta_n \cos\alpha_n & -\cos\theta_n \sin\alpha_n & r_n \sin\theta_n \\ 0 & \sin\alpha_n & \cos\alpha_n & d_n \\ \hline 0 & 0 & 0 & 1 \end{array} \right] =\left[\begin{array}{ccc|c} & & & \\ & R & & T \\ & & & \\ \hline 0 & 0 & 0 & 1 \end{array}\right]\end{split}\]

Examples

>>> theta_1=90.0
>>> theta_2=90.0
>>> theta_3=0.
>>> dh(theta_1,60,0,0)*dh(0,88,71,90)*dh(theta_2,15,0,0)*dh(0,0,174,-180)*dh(theta_3,15,0,0)
[[-6.12323400e-17 -6.12323400e-17 -1.00000000e+00 -7.10542736e-15],
[ 6.12323400e-17  1.00000000e+00 -6.12323400e-17  7.10000000e+01],
[  1.00000000e+00 -6.12323400e-17 -6.12323400e-17  3.22000000e+02],
[ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
Parameters:

  • theta (float) – rotation angle z axis [deg]

  • d (float) – transformation along the z-axis

  • alpha (float) – rotation angle x axis [deg]

  • r (float) – transformation along the x-axis

Returns:

dh – Denavit Hartenberg transformation matrix.

Return type:

ndarray(4x4)

rotmat.dh_s(theta=0, d=0, r=0, alpha=0)[source]

Symbolic Denavit Hartenberg transformation and rotation matrix.

>>> dh_s('theta_1',60,0,0)*dh_s(0,88,71,90)*dh_s('theta_2',15,0,0)*dh_s(0,0,174,-180)*dh_s('theta_3',15,0,0)
Parameters:

  • theta (float) – rotation angle z axis [deg]

  • d (float) – transformation along the z-axis

  • alpha (float) – rotation angle x axis [deg]

  • r (float) – transformation along the x-axis

Returns:

R

Return type:

Symbolic rotation and transformation matrix 4x4

rotmat.seq2quat(rot_angles, seq='nautical')[source]

This function takes a angles from sequenctial rotations and calculates the corresponding quaternions.

Parameters:

  • rot_angles (ndarray, nx3) – sequential rotation angles [deg]

  • seq (string) –

    Has to be one the following:

    • Euler … Rz * Rx * Rz

    • Fick … Rz * Ry * Rx

    • nautical … same as “Fick”

    • Helmholtz … Ry * Rz * Rx

Returns:

quats – corresponding quaternions

Return type:

ndarray, nx4

Examples

>>> skin.rotmat.seq2quat([90, 23.074, -90], seq='Euler')
array([[ 0.97979575,  0.        ,  0.2000007 ,  0.        ]])

Notes

The equations are longish, and can be found in 3D-Kinematics, 4.1.5 “Relation to Sequential Rotations”

rotmat.sequence(R, to='Euler')[source]

This function takes a rotation matrix, and calculates the corresponding angles for sequential rotations.

R_Euler = R3(gamma) * R1(beta) * R3(alpha)

Parameters:

  • R (ndarray, 3x3) – rotation matrix

  • to (string) –

    Has to be one the following:

    • Euler … Rz * Rx * Rz

    • Fick … Rz * Ry * Rx

    • nautical … same as “Fick”

    • Helmholtz … Ry * Rz * Rx

Returns:

  • gamma (third rotation (left-most matrix))

  • beta (second rotation)

  • alpha (first rotation(right-most matrix))

Notes

The following formulas are used:

Euler:

\[ \begin{align}\begin{aligned}\beta = -arcsin(\sqrt{ R_{xz}^2 + R_{yz}^2 }) * sign(R_{yz})\\\gamma = arcsin(\frac{R_{xz}}{sin\beta})\\\alpha = arcsin(\frac{R_{zx}}{sin\beta})\end{aligned}\end{align} \]

nautical / Fick:

\[ \begin{align}\begin{aligned} \theta = arctan(\frac{R_{yx}}{R_{xx}})\\\phi = arcsin(R_{zx})\\ \psi = arctan(\frac{R_{zy}}{R_{zz}})\end{aligned}\end{align} \]

Note that it is assumed that psi < pi !

Helmholtz:

\[ \begin{align}\begin{aligned}\theta = arcsin(R_{yx})\\\phi = -arcsin(\frac{R_{zx}}{cos\theta})\\\psi = -arcsin(\frac{R_{yz}}{cos\theta})\end{aligned}\end{align} \]

Note that it is assumed that psi < pi

rotmat.stm(axis='x', angle=0, translation=[0, 0, 0])[source]

Spatial Transformation Matrix

Parameters:

  • axis (int or str) – Axis of rotation, has to be 0, 1, or 2, or ‘x’, ‘y’, or ‘z’

  • angle (float) – rotation angle [deg]

  • translation (3x1 ndarray) – 3D-translation vector

Returns:

STM – spatial transformation matrix, for rotation about the specified axis, and translation by the given vector

Return type:

4x4 ndarray

Examples

>>> rotmat.stm(axis='x', angle=45, translation=[1,2,3.3])
array([[ 1.        ,  0.        ,  0.        ,  1.        ],
       [ 0.        ,  0.70710678, -0.70710678,  2.        ],
       [ 0.        ,  0.70710678,  0.70710678,  3.3       ],
       [ 0.        ,  0.        ,  0.        ,  1.        ]])
>>> R_z = rotmat.stm(axis='z', angle=30)
>>> T_y = rotmat.stm(translation=[0, 10, 0])
rotmat.stm_s(axis='x', angle='0', transl='0,0,0')[source]

Symbolic spatial transformation matrix about the given axis, by an angle with the given name, and translation by the given distances.

Parameters:

  • axis (int or str) – Axis of rotation, has to be 0, 1, or 2, or ‘x’, ‘y’, or ‘z’

  • angle (string) – Name of rotation angle, or ‘0’ for no rotation, ‘alpha’, ‘theta’, etc. for a symbolic rotation.

  • transl (string) – Has to contain three names, for the three translation distances. ‘0,0,0’ would correspond to no translation, and ‘x,y,z’ to an arbitrary translation.

Returns:

STM_symbolic

Return type:

corresponding symbolic spatial transformation matrix

Examples

>>> Rz_s = STM_s(axis='z', angle='theta', transl='0,0,0')

>>> Tz_s = STM_s(axis=0, angle='0', transl='0,0,z')