Kindr
Kinematics and Dynamics for Robotics
Rotations

Introduction

A rotation between two rigid bodies can be represented by using different parameterizations. The most well-known parameterizations are Euler angles, rotation matrix, angle-axis, rotation vector and unit quaternion. Depending on the application, some representations may be better suited than others. For instance, a unit quaternion does not have singularity issues like Euler angles and are therefore better suited for calculations, but Euler angles are much easier to be interpreted by a user. The library provides therefore a type for each parameterization of a rotation, which enables type safe conversions between different representations.

The type of a rotation further depends on the usage of a rotation. A rotation can be considered either as an active (alibi) or a passive (alibi) transformation. The following figure visualizes the difference:

rotation_active_passive.png

The figure shows a rotation about the z-axis with an angle $\theta$. If the rotation is considered as active, the (blue) position vector ${}_A \mathbf{r}_{O\,P}$ expressed in frame A is rotated to the (green) position vector ${}_A \mathbf{r}_{O\,Q}$, which is also expressed in frame A. If the rotation is considered as passive, the (blue) vector $ {}_A \mathbf{ r}_{O\,P}$ is not rotated actively, but its coordinates are mapped from frame A to frame B, i.e. the result is ${}_B \mathbf{r}_{O\,P}$. In other words, an active rotation rotates a vector, whereas a passive rotation rotates the coordinate frame. The result of rotation applied to a vector or column-wise to a matrix therefore depends on the usage type.

This library provides a passive implementation of different rotation parameterizations. The library depends on the C++ Eigen library, which already provides some of the representations, but does not feature the type safety and misses many features.

The following section describes the genric interface of all rotation classes, whereas the section Parameterizations shows the different parameterizations and explains how to use them.

Generic Interface

Rotation Base Class

The class kindr::RotationBase serves as an interface for a rotation between two rigid bodies. All types of representations of a rotation, such as quaternion, angle-axis, and Euler angles, are derived from this base class.

The type of a rotation is defined by

  • the kind of parameterization (angle-axis, quaternion, etc.) and
  • the primitive data type of the parameters (float/double, etc.).

Construction of a Rotation

The default constructor of a rotation always creates an identity rotation. If the library is built with debugging symbols, an assertion is thrown if a rotation is constructed with parameter values that do not represent a valid rotation.

Identity Rotation

Any rotation can be set equal to the identity rotation by the method setIdentity().

Rotation from Two Vectors

A rotation that describes the relative orientation of two vectors can be initialized by means of the method setFromVectors().

Unique Rotation

A rotation can be represented in different ways. The quaternion $Q = q_0 + q_1 i + q_2 j + q_3 k$ and the quaternion $Q = -q_0 - q_1 i - q_2 j - q_3 k$, for instance, represent the same rotation. However, sometimes a unique representation is desired.

A unique representation is obtained by

  • getUnique(): gets a copy of the presentation, which is unique, and
  • setUnique(): sets the representation to the unique representation.

Note that numerical issues may cause problems when this method is used to compare two rotations. We therefore recommend to use the method isNear() (see below).

Inverse of a Rotation

The inverse of a rotation is provided by two methods:

Composition of a Rotation

A rotation can be composed of other rotations. The multiplication operator enables to concatenate rotations as follows:

C_DA = C_DC*C_CB*C_BA;

Due to numerical issues, the result of the concatenation may become an invalid rotation. Due to speed considerations, the user is responsible to guarentee that the parameter values represent a valid rotation. The library therefore offers the method fix() to project the parameters to the admissible space. The method would, for instance, normalize the rotation quaternion.

Rotation of a Vector or a Matrix

To rotate a vector or a matrix columnwise, the following methods are provided:

  • rotate(): rotates a vector or a matrix columnwise
  • inverseRotate(): reverse rotates a vector or a matrix columnwise

Exponential Map and Logarithmic Map

The exponential map maps a vector in $\mathsf{R}^3$ describing the axis and magnitude of a three degree-of-freedom rotation to the corresponding rotation parameterization. The mapping is provided by the method setExponentialMap(). The corresponding logarithmic map is provided by the method getLogarithmicMap().

Box Plus and Box Minus Operations

The box plus operation defined as $\mathcal{R}_2 = \mathcal{R}_1\boxplus\mathbf{v} = \mathcal{R}(e^{\hat{\mathbf{v}}}) \cdot \mathcal{R}_1 $, where $\mathcal{R}$ is a rotation, $\mathbf{v}$ is a vector in $\mathsf{R}^3$ and $\hat{}$ maps a vector to the skew-symmetric matrix corresponding to the cross product, is provided by the method boxPlus().

The box minus operation defined as $\mathbf{v} = \mathcal{R}_1\boxminus\mathcal{R}_2 = \ln{(\mathcal{R}_1\cdot\mathcal{R}_2^{-1})}\check{}$ is provided by the method boxMinus().

rotations_interfaces_comparison Comparison of Rotations To compare two rotations, the equal operator (operator==) can be used to check if a rotation is equal to another rotation.

Small numeric errors may however lead to a wrong result. Therefore, the function isNear() compares two rotations using a measure.

The difference between two rotations can be obtained by the method getDisparityAngle().

Conversion between Rotations

There are two kinds of conversions:

  • Conversion between different representations, i.e. between different parameterizations (e.g. rotation quaternion - angle-axis)
  • Conversion between different primitive data types (e.g. float - double)

Conversion between Representations

The following methods allow to convert between different representations of a rotation:

AngleAxisD angleAxis;
RotationQuaternionD quaternion(angleAxis); // constructor
angleAxis = quaternion; // assignment operator
quaternion(angleAxis); // parenthesis operator

Conversion between Primitive Data Types

The conversion between different primitive data types (double/float) works the same way as the conversion between different representations.

Parameterizations

The following representations are currently provided by the library:

Angle-Axis

The class AngleAxis provides an angle-axis representation of a rotation:

$\boxed{(\theta, \mathbf{n}), \quad \lVert \mathbf{n} \rVert = 1,}$

where $\theta \in \mathbb{R}$ is the rotation angle and $\mathbf{n} \in \mathbb{R}^3$ is the rotation axis with unit length.

The angle and rotation axis ared stored in Eigen::AngleAxis.

The following typedefs are provided for convenience:

Create an Angle-Axis

double angle = 0.2;
double n_1 = 1.0;
double n_2 = 0.0;
double n_3 = 0.0;
Eigen::Vector3d axis(n_1, n_2, n_3);
AngleAxisD angleAxis1; // identity rotation: angle = 0.0 axis = (1.0, 0.0, 0.0)
AngleAxisD angleAxis2(angle, axis);
AngleAxisD angleAxis3(angle, n_1, n_2, n_3);
AngleAxisD angleAxis4(angle, 1.0, 1.0, 1.0); // will create an error in debug mode

Use an Angle-Axis

double angle = angleAxis.angle(); // get rotation angle
Eigen::Vector3d axis = angleAxis.axis(); // get rotation axis
Eigen::AngleAxisd angleAxisEigen = angleAxis.toImplementation(); // get Eigen angle-axis
Eigen::Vector3d vector_rotated = angleAxis.rotate(Eigen::Vector3d(1.0,2.0,3.0)); // rotate a vector
Eigen::Vector3d vector_invrotated = angleAxis.inverseRotate(Eigen::Vector3d(1.0,2.0,3.0)); // reverse rotate a vector
angleAxis.setUnique(); // Modifies the angle such that it is in [0,PI)
std::cout << "Angle-Axis: " << angleAxis << std::endl: // output angle-axis
if (angleAxis == AngleAxisD(M_PI/2.0, 1.0, 0.0, 0.0)) // compare angle-axis
std::cout << "Rotation is about x-axis with angle M_PI/2.0\n";

Rotation Vector

The class kindr::RotationVector implements a rotation vector representation of a rotation:

$\boxed{\mathbf{\phi}\in\mathbb{R}^3}$

where $\lVert \mathbf{\phi} \rVert $ is the rotation angle and $\frac{\mathbf{\phi}}{\lVert \mathbf{\phi} \rVert}$ is the rotation axis.

It stores the rotation vector in Eigen::Matrix<Scalar, 3, 1>.

The following typedefs are provided for convenience:

Create a Rotation Vector

double v_1 = 2.0;
double v_2 = 0.0;
double v_3 = 0.0;
Eigen::Vector3d vector(v_1, v_2, v_3);
RotationVectorD rotationVector1; // identity rotation: vector = (0.0, 0.0, 0.0)
RotationVectorD rotationVector2(vector);
RotationVectorD rotationVector3(v_1, v_2, v_3);

Use a Rotation Vector

Eigen::Vector3d vector = rotationVector.vector(); // get Eigen vector
Eigen::Vector3d vector_rotated = rotationVector.rotate(Eigen::Vector3d(1.0,2.0,3.0)); // rotate a vector
Eigen::Vector3d vector_invrotated = rotationVector.inverseRotate(Eigen::Vector3d(1.0,2.0,3.0)); // reverse rotate a vector
rotationVector.setUnique(); // Modifies the angle such that it is in [0,PI)
std::cout << "Rotation Vector: " << rotationVector << std::endl: // output angle-axis
if (rotationVector == RotationVectorD(M_PI/2.0, 0.0, 0.0)) // compare rotation vector
std::cout << "Rotation is about x-axis with angle M_PI/2.0\n";

Rotation Quaternion

The class kindr::RotationQuaternion implements a Hamiltonian unit quaternion representation of a rotation:

$\boxed{\begin{aligned}Q &= q_0 + q_1 i + q_2 j + q_3 k \in \mathbb{H}, \quad q_i \in \mathbb{R} \\ i^2 &= j^2=k^2 = ijk = -1, \quad \lVert Q \rVert= \sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2} = 1 \\ \end{aligned}}$

It stores the coefficients according to the kindr::UnitQuaternion. See also Chapter Quternions for more information.

The following typedefs are provided for convenience:

Create a Rotation Quaternion

double p_0 = 1.0;
double p_1 = 0.0;
double p_2 = 0.0;
double p_3 = 0.0;
RotationQuaternionD rquat1; // identity rotation: (1.0, 0.0, 0.0, 0.0)
RotationQuaternionD rquat2(p_0, p_1, p_2, p_3); // create with four scalars
RotationQuaternionD rquat3(Eigen::Quaterniond(p_0, p_1, p_2, p_3)); // create with Eigen quaternion
RotationQuaternionD rquat5(UnitQuaternion(p_0, p_1, p_2, p_3)); // create with unit quaternion
RotationQuaternionD rquat5(Quaternion(p_0, p_1, p_2, p_3)); // create with quaternion
RotationQuaternionD rquat4(1.0, 1.0, 1.0, 1.0); // will create an error in debug mode

Use a Quaternion

double p_0 = rquat.w();
double p_1 = rquat.x();
double p_2 = rquat.y();
double p_3 = rquat.z();
Eigen::Quaterniond quatEigen = rquat.toImplementation(); // get Eigen quaternion
UnitQuaternionD uquat = rquat.toUnitQuaternion(); // get unit quaternion
Eigen::Vector3d vector_rotated = rquat.rotate(Eigen::Vector3d(1.0,2.0,3.0)); // rotate a vector
Eigen::Vector3d vector_invrotated = rquat.inverseRotate(Eigen::Vector3d(1.0,2.0,3.0)); // reverse rotate a vector
rquat.setUnique(); // Modifies the quaternion such that p_0 >= 0
std::cout << "Quaternion: " << rquat << std::endl: // output quaternion
if (rquat == RotationQuaternionD()) // compare
std::cout << "Rotation is identity\n";

Rotation Matrix

The class kindr::RotationMatrix implements a 3x3 rotation matrix representation of a rotation:

$\boxed{R = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \in SO(3)}$

It stores the coefficients in Eigen::Matrix.

The following typedefs are provided for convenience:

Create a Rotation Matrix

RotationMatrixD rmat1; // identity rotation: diag(1.0, 1.0, 1.0)
RotationMatrixD rmat2(r11, r12, r13, // rotation matrix from coefficients
r21 ,r22, r23,
r31, r32, r33);
Eigen::Matrix3d mat; // rotation matrix from Eigen matrix
mat << r11, r12, r13,
r21 ,r22, r23,
r31, r32, r33;
RotationMatrixD rmat3(mat); // using the constructor
RotationMatrixD rmat4; // using a setter
rmat4.setMatrix(mat);

Use a Rotation Matrix

Eigen::Matrix3d mat = rmat.matrix(); // get Eigen matrix (avoid using toImplementation())
Eigen::Vector3d vector_rotated = rmat.rotate(Eigen::Vector3d(1.0,2.0,3.0)); // rotate a vector
Eigen::Vector3d vector_invrotated = rmat.inverseRotate(Eigen::Vector3d(1.0,2.0,3.0)); // reverse rotate a vector
double determinant = rmat.determinant(); // get the determinant of the matrix
std::cout << "Rotation matrix: " << rmat << std::endl: // output rotation matrix
if (rmat == RotationMatrixD()) // compare
std::cout << "Rotation is identity\n";

Note that the rotation matrix is stored internally differently than created by the constructor! Therefore, you should access the matrix using the method matrix() instead of toImplementation().

Euler Angles ZYX

The class kindr::EulerAnglesZyx implements the Euler Angles using the Z-Y-X convention:

$\boxed{(z, y, x),}$

where $z$ is the yaw angle, $y$ is the pitch angle, and $x$ is the roll angle. The consecutive rotations are shown in the following picture:

EulerAnglesZyx.png

The following typedefs are provided for convenience:

Create Euler Angles ZYX

double yaw = 0.3; // angle around z-axis
double pitch = 0.2; // angle around new y-axis
double roll = 0.1; // angle around new x-axis
EulerAnglesZyxD angles1; // identity rotation (0, 0, 0)
EulerAnglesZyxD angles2(yaw, pitch, roll);
EulerAnglesZyxD angles3(Eigen::Vector3d(yaw, pitch, roll));

Use Euler Angles ZYX

double roll = angles.roll();
double pitch = angles.pitch();
double yaw = angles.yaw();
roll = angles.x();
pitch = angles.y();
yaw = angles.z();
Eigen::Vector3d angleVector = angles.vector(); // [roll; pitch; yaw]
angles.setUnique() // Modifies the angles, such that yaw in [-pi,pi), pitch in [-pi/2,pi/2), roll in [-pi,pi)

Euler Angles XYZ

The class kindr::EulerAnglesXyz implements the Euler Angles using the X-Y-Z convention:

$\boxed{(x, y, z ),}$

where $z$ is the yaw angle, $y$ is the pitch angle, and $x$ is the roll angle. The consecutive rotations are shown in the following picture:

EulerAnglesXyz.png

It stores the coefficients in Eigen::Vector3.

The following four typedefs are provided for convenience:

Create Euler Angles XYZ

double roll = 0.1; // angle around x-axis
double pitch = 0.2; // angle around new y-axis
double yaw = 0.3; // angle around new z-axis
EulerAnglesXyzD angles1; // identity rotation (0, 0, 0)
EulerAnglesXyzD angles2(roll, pitch, yaw);
EulerAnglesXyzD angles3(Eigen::Vector3d(roll, pitch, yaw));

Use Euler Angles XYZ

double roll = angles.roll();
double pitch = angles.pitch();
double yaw = angles.yaw();
roll = angles.x();
pitch = angles.y();
yaw = angles.z();
Eigen::Vector3d angleVector = angles.vector(); // [roll; pitch; yaw]
angles.setUnique() // Modifies the angles, such that roll in [-pi,pi), pitch in [-pi/2,pi/2), yaw in [-pi,pi)