2. SO(3): 3D Rotations#

The group of all rotations in the 3D Cartesian space is called \(SO(3)\) (SO: special orthogonal group). It is typically represented by 3D rotation matrices [7]. The minimum number of components that are required to describe any rotation from \(SO(3)\) is 3. However, there is no representation that is non-redundant, continuous, and free of singularities.

Rotations

Overview of the representations and the conversions between them that are available in pytransform3d.#

Not all representations support all operations directly without conversion to another representation. The following table is an overview. If the operation is not implemented in pytransform3d then it is shown in brackets.

Representation

Inverse

Rotation of vector

Concatenation

Interpolation

Renormalization

Rotation matrix \(\pmb{R}\)

Transpose

Yes

Yes

No

Required

Axis-angle \((\hat{\pmb{\omega}}, \theta)\)

Negative axis

No

No

SLERP

Not necessary

Rotation vector \(\pmb{\omega}\)

Negative

No

No

(Yes) (2)

Not required

Logarithm of rotation \(\left[\pmb{\omega}\right]\)

Negative

No

No

(Yes) (2)

Not required

Quaternion \(\pmb{q}\)

Conjugate

Yes

Yes

SLERP

Required

Rotor \(R\)

Reverse

Yes

Yes

SLERP

Required

Euler angles \((\alpha, \beta, \gamma)\)

(1)

No

No

No

Not necessary

Modified Rodrigues parameters \(\pmb{\psi}\)

Negative

No

Yes

No

Not required

Footnotes:

(1) Inversion of Euler angles in convention ABC (e.g., xyz) requires using another convention CBA (e.g., zyx), reversing the order of angles, and taking the negative of these.

(2) Linear interpolation is approximately correct for small differences.

2.1. Rotation Matrix#

One of the most practical representations of orientation is a rotation matrix

\[\begin{split}\boldsymbol R = \left( \begin{array}{ccc} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33}\\ \end{array} \right) \in SO(3).\end{split}\]

Note that this is a non-minimal representation for orientations because we have 9 values but only 3 degrees of freedom. This is because \(\boldsymbol R\) is orthonormal, which results in 6 constraints (enforced with norm_matrix() and checked with matrix_requires_renormalization() or check_matrix()):

  • column vectors must have unit norm (3 constraints)

  • and must be orthogonal to each other (3 constraints)

A more compact representation of these constraints is \(\boldsymbol R^T \boldsymbol R = \boldsymbol I \Leftrightarrow \boldsymbol R^T = \boldsymbol R^{-1}\).

In addition, \(\det(\boldsymbol R) = 1\) because we use right-handed coordinate system (\(\det(\boldsymbol R) = -1\) for left-handed coordinate systems).

Hence, the group \(SO(3)\) is defined as

\[SO(3) = \{\boldsymbol{R} \in \mathbb{R}^{3 \times 3} | \boldsymbol{R}\boldsymbol{R}^T = \boldsymbol{I}, \det(\boldsymbol{R}) = 1\}.\]

pytransform3d uses a numpy array of shape (3, 3) to represent rotation matrices and typically we use the variable name R for a rotation matrix.

Warning

There are two conventions on how to interpret rotations: active or passive rotation. The standard in pytransform3d is an active rotation.

We can use a rotation matrix \(\boldsymbol R_{BA}\) to transform a point \(_A\boldsymbol{p}\) from frame \(A\) to frame \(B\).

Warning

There are two different conventions on how to use rotation matrices to apply a rotation to a vector. We can either (pre-)multiply the rotation matrix to a column vector from the left side or we can (post-)multiply it to a row vector from the right side. We will use the pre-multiplication convention.

This means that we rotate a point \(_A\boldsymbol{p}\) by

\[_B\boldsymbol{p} = \boldsymbol{R}_{BAA} \boldsymbol{p}\]

This is called linear map.

Note that with our index notation (as explained in Frame Notation) and these conventions, the second index of the rotation matrix and the left index of the point have to be the same (\(A\) in this example). The rotation is applied incorrectly if this is not the case.

Each column of a rotation matrix \(\boldsymbol{R}_{BA}\) is a basis vector of frame \(B\) with respect to frame \(A\). We can plot the basis vectors of an orientation to visualize it. Here, we can see orientation represented by the rotation matrix

\[\begin{split}\boldsymbol R = \left( \begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ \end{array} \right).\end{split}\]
from pytransform3d.rotations import plot_basis
plot_basis()
../_images/rotations-1.png

Note

When plotting basis vectors, it is a convention to use red for the x-axis, green for the y-axis and blue for the z-axis (RGB for xyz).

We can easily chain multiple rotations: we can apply the rotation defined by \(\boldsymbol R_{CB}\) after the rotation \(\boldsymbol R_{BA}\) by applying the rotation

\[\boldsymbol R_{CA} = \boldsymbol R_{CB} \boldsymbol R_{BA}.\]

Note that the indices have to align again. Otherwise rotations are not applied in the correct order.

Warning

There are two different conventions on how to concatenate rotation matrices. Suppose we have a rotation matrix \(R_1\) and another matrix \(R_2\) and we want to first rotate by \(R_1\) and then by \(R_2\). If we want to apply both rotations in global coordinates, we have to concatenate them with \(R_2 \cdot R_1\). We can also express the second rotation in terms of a local, body-fixed coordinates by \(R_1 \cdot R_2\), which means \(R_1\) defines new coordinates in which \(R_2\) is applied. Note that this applies to both passive and active rotation matrices.

The easiest way to construct rotation matrices is through rotations about the basis vectors with active_matrix_from_angle(). Multiple rotation matrices that were constructed like this can be concatenated. This will be done, for instance, to obtain rotation matrices from Euler angles (see Euler Angles).

Pros

  • Supported operations: all except interpolation.

  • Interpretation: each column is a basis vector.

  • Singularities: none.

  • Ambiguities: none that are specific for rotation matrices.

Cons

  • Representation: 9 values for 3 degrees of freedom.

  • Renormalization: expensive in comparison to quaternions.

2.2. Axis-Angle#

../_images/sphx_glr_plot_axis_angle_001.png

Each rotation can be represented by a single rotation about one axis. The axis can be represented as a three-dimensional unit vector and the angle by a scalar:

\[\begin{split}\left( \hat{\boldsymbol{\omega}}, \theta \right) = \left( \left( \begin{array}{c}\omega_x\\\omega_y\\\omega_z\end{array} \right), \theta \right)\end{split}\]

pytransform3d uses a numpy array of shape (4,) for the axis-angle representation of a rotation, where the first 3 entries correspond to the unit axis of rotation and the fourth entry to the rotation angle in radians, and typically we use the variable name a.

Note that the axis-angle representation has a singularity at \(\theta = 0\) as there is an infinite number of rotation axes that represent the identity rotation in this case. However, we can modify the representation to avoid this singularity.

2.3. Rotation Vector#

Since \(||\hat{\boldsymbol{\omega}}|| = 1\), it is possible to write this in a more compact way as a rotation vector [2]

\[\boldsymbol{\omega} = \hat{\boldsymbol{\omega}} \theta \in \mathbb{R}^3.\]

In code, we call this the compact axis-angle representation. pytransform3d uses a numpy array of shape (3,) for the compact axis-angle representation of a rotation and typically it uses the variable name a.

We can also refer to this representation as exponential coordinates of rotation [5]. We can represent angular velocity as \(\hat{\boldsymbol{\omega}} \dot{\theta}\) and angular acceleration as \(\hat{\boldsymbol{\omega}} \ddot{\theta}\) so that we can easily do component-wise integration and differentiation with this representation.

Pros

  • Representation: minimal.

  • Supported operations: interpolation; can also represent angular velocity and acceleration.

Cons

  • Ambiguities: an angle of 0 and any multiple of \(2\pi\) represent the same orientation (can be avoided with norm_compact_axis_angle(), which introduces discontinuities); when \(\theta = \pi\), the axes \(\hat{\boldsymbol{\omega}}\) and \(-\hat{\boldsymbol{\omega}}\) represent the same rotation.

  • Supported operations: concatenation and transformation of vectors requires conversion to another representation.

2.4. Logarithm of Rotation#

In addition, we can represent \(\hat{\boldsymbol{\omega}} \theta\) by the cross-product matrix (cross_product_matrix())

\[\begin{split}\left[\hat{\boldsymbol{\omega}}\right] \theta = \left( \begin{matrix} 0 & -\omega_3 & \omega_2\\ \omega_3 & 0 & -\omega_1\\ -\omega_2 & \omega_1 & 0\\ \end{matrix} \right) \theta \in so(3) \subset \mathbb{R}^{3 \times 3},\end{split}\]

where \(\left[\hat{\boldsymbol{\omega}}\right] \theta\) is the matrix logarithm of a rotation matrix and \(so(3)\) is the Lie algebra of the Lie group \(SO(3)\).

2.5. Quaternions#

Quaternions are represented by a scalar / real part \(w\) and an vector / imaginary part \(x \boldsymbol{i} + y \boldsymbol{j} + z \boldsymbol{k}\).

\[\boldsymbol{q} = w + x \boldsymbol{i} + y \boldsymbol{j} + z \boldsymbol{k} \in \mathbb{H}\]

Warning

There are two different quaternion conventions: Hamilton’s convention defines \(ijk = -1\) and the Shuster or JPL convention (from NASA’s Jet Propulsion Laboratory, JPL) defines \(ijk = 1\) [1]. These two conventions result in different multiplication operations and conversions to other representations. We use Hamilton’s convention.

Read this paper for details about the two conventions and why Hamilton’s convention should be used. Section VI A gives further useful hints to identify which convention is used.

The unit quaternion space \(S^3\) can be used to represent orientations with an encoding based on the rotation axis and angle. A rotation quaternion is a four-dimensional unit vector (versor) \(\boldsymbol{\hat{q}}\). The following equation describes its relation to axis-angle notation.

\[\begin{split}\boldsymbol{\hat{q}} = \left( \begin{array}{c} w\\ x\\ y\\ z\\ \end{array} \right) = \left( \begin{array}{c} \cos \frac{\theta}{2}\\ \omega_x \sin \frac{\theta}{2}\\ \omega_y \sin \frac{\theta}{2}\\ \omega_z \sin \frac{\theta}{2}\\ \end{array} \right) = \left( \begin{array}{c} \cos \frac{\theta}{2}\\ \hat{\boldsymbol{\omega}} \sin \frac{\theta}{2}\\ \end{array} \right)\end{split}\]

pytransform3d uses a numpy array of shape (4,) for quaternions and typically we use the variable name q.

Warning

The scalar component \(w\) of a quaternion is sometimes the first element and sometimes the last element of the versor. We will use the first element to store the scalar component.

Since the other convention is also used often, pytransform3d provides the functions quaternion_wxyz_from_xyzw() and quaternion_xyzw_from_wxyz() for conversion.

Warning

The antipodal unit quaternions \(\boldsymbol{\hat{q}}\) and \(-\boldsymbol{\hat{q}}\) (quaternion_double()) represent the same rotation (double cover). This must be considered during operations like interpolation, distance calculation, or (approximate) equality checks.

The quaternion product (concatenate_quaternions()) can be used to concatenate rotations like the matrix product can be used to concatenate rotations represented by rotation matrices.

The inverse of the rotation represented by the unit quaternion \(\boldsymbol{q}\) is represented by the conjugate \(\boldsymbol{q}^*\) (q_conj()).

We can rotate a vector by representing it as a pure quaternion (i.e., with a scalar part of 0) and then left-multiplying the quaternion and right-multiplying its conjugate \(\boldsymbol{q}\boldsymbol{v}\boldsymbol{q}^*\) with the quaternion product (q_prod_vector()).

Pros

  • Representation: compact.

  • Renormalization: checked with quaternion_requires_renormalization(); cheap in comparison to rotation matrices (); less susceptible to round-off errors than matrix representation.

  • Discontinuities: none.

  • Computational efficiency: the quaternion product is cheaper than the matrix product.

  • Singularities: none.

Cons

  • Interpretation: not straightforward.

  • Ambiguities: double cover.

2.6. Euler Angles#

A complete rotation can be split into three rotations around basis vectors. pytransform3d uses a numpy array of shape (3,) for Euler angles, where each entry corresponds to a rotation angle in radians around one basis vector. The basis vector that will be used and the order of rotation is defined by the convention that we use. See Euler Angles for more information.

Warning

There are 24 different conventions for defining euler angles. There are 12 different valid ways to sequence rotation axes that can be interpreted as extrinsic or intrinsic rotations: XZX, XYX, YXY, YZY, ZYZ, ZXZ, XZY, XYZ, YXZ, YZX, ZYX, and ZXY.

Pros

  • Representation: minimal.

  • Interpretation: easy to interpret for users (when the convention is clearly defined) in comparison to axis-angle or quaternions.

Cons

  • Ambiguities: 24 different conventions, infinitely many Euler angles represent the same rotation without proper limits for the angles.

  • Singularity: gimbal lock.

  • Supported operations: all operations require conversion to another representation.

2.7. Rotors#

../_images/sphx_glr_plot_bivector_001.png

Rotors and quaternions are very similar concepts in 3D. However, rotors are more general as they can be extended to more dimensions [3] [4].

The concept of a quaternion builds on the axis-angle representation, in which we rotate by an angle about a rotation axis (see black arrow in the illustration above). The axis can be computed from the cross product of two vectors (gray arrows). A rotor builds on a plane-angle representation, in which we rotate with a given direction by an angle in a plane (indicated by gray area). The plane can be computed from the wedge product \(a \wedge b\) (see wedge()) of two vectors \(a\) and \(b\), which is a so-called bivector. Although both approaches might seem different, in 3D they operate with exactly the same numbers in exactly the same way.

Warning

The rotors \(R\) and \(-R\) represent exactly the same rotation.

A rotor is a unit multivector

\[R = (a, b_{yz}, b_{zx}, b_{xy})\]

that consists of a scalar \(a\) and a bivector \((b_{yz}, b_{zx}, b_{xy})\). The components of a bivector constructed by the wedge product of two vectors can be interpreted as the area of the parallelogram formed by the two vectors projected on the three basis planes yz, zx, and xy (see illustration above). These values also correspond to the x-, y-, and z-components of the cross product of the two vectors, which allows two different interpretations of the same numbers from which we can then derive quaternions on the one hand and rotors on the other hand.

Warning

In pytransform3d our convention is that we organize the components of a rotor in exactly the same way as we organize the components of the equivalent quaternion. There are other conventions. It is not just possible to change the order of the scalar and the bivector (similar to a quaterion), but also to change the order of bivector components.

Pros and cons for rotors are the same as for quaternions as they have the same representation in 3D.

2.8. Modified Rodrigues Parameters#

Another minimal representation of rotation are modified Rodrigues parameters (MRP) [6] [8]

\[\boldsymbol{\psi} = \tan \left(\frac{\theta}{4}\right) \hat{\boldsymbol{\omega}}\]

This representation is similar to the compact axis-angle representation. However, the angle of rotation is converted to \(\tan(\frac{\theta}{4})\). Hence, there is an easy conversion from unit quaternions to MRP (mrp_from_quaternion()):

\[\begin{split}\boldsymbol{\psi} = \frac{ \left( \begin{array}{c} x\\ y\\ z\\ \end{array} \right)}{1 + w}\end{split}\]

given some quaternion with a scalar \(w\) and a vector \(\left(x, y, z \right)^T\).

pytransform3d uses a numpy array of shape (3,) for modified Rodrigues parameters.

Warning

MRPs have a singuarity at \(2 \pi\) (see mrp_near_singularity()) which we can avoid by ensuring the angle of rotation does not exceed \(\pi\) (with norm_mrp()).

Warning

MRPs have two representations for the same rotation: \(\boldsymbol{\psi}\) and \(-\frac{1}{||\boldsymbol{\psi}||^2} \boldsymbol{\psi}\) (mrp_double()) represent the same rotation and correspond to two antipodal unit quaternions [8].

Pros

  • Representation: minimal.

Cons

  • Interpretation: not straightforward.

  • Singularity: at \(\theta = 2 \pi\).

  • Ambiguity: double cover.

  • Supported operations: transformation of vectors requires conversion to another representation.

2.9. References#