Implementing Rotations: A Robotics Field Guide

Why?

My aim here is to elucidate the complex machinery that constitutes the hard part of working with transforms and frames in robotics: rotations and rotational representations.

Even when working with a pre-existing software library that provides rotational representations for you, there are so many different conventions (and the implications of those conventions, often mixed together, so ingrained in the math) that without thorough documentation on the part of the library (good luck), you're bound to be banging your head against the wall at some point. Sometimes, even understanding exactly what the functions are giving you can give you pause.

This guide is meant to be a one-stop-shop for concisely clarifying the possibilities and helping you recognize which ones you're working with and their implications. Some convenient calculators that conform to your chosen conventions are also provided.

Note: The FIXME keywords indicate missing information that I'll be filling in as my free time allows.

I've implemented many of these concepts in a C++ library with corresponding Python bindings. There's also a Python script that implements the checks laid out in this guide for deducing the rotational conventions used by a particular library.

Introduction: Conventions

Often ignored or omitted from documentation are the hidden conventions associated with a rotation representation implementation--particularly implementations that allow for converting between different representations. But conventions are very important to get right in order to ensure consistency and correctness, as well as prevent needless hours of debugging. This guide attempts to aggregate most, if not all, possible conventions for the various representations in one place. Here are the types of conventions relevant to rotational representations:

• Ordering: Pure semantics--in what order are the components stored in memory and notationally?
• Handedness: This convention is a catch-all for intrinsic properties that determine the geometry of compositions.
• Function: Does the rotation serve to change the reference frame of a vector (Passive) or move the vector (Active) by its action? Note that in computer graphics, active functions are more common, whereas in robotics rotations almost always are meant to be passive. The one nuance is when library definitions associate quaternions with rotation matrices in such a way that it looks like the quaternion is acting as an active counterpart to its corresponding passive rotation matrix--more on that later.
• Directionality: A rotation is relative--the rotation of frame \(A\) relative to frame \(B\). Directionality determines which of \(A\) or \(B\) is the frame being rotated from and to. In robotics, the canonical \(A\) and \(B\) frames are often labeled as \(W\) (the "World" frame) and \(B\) (the "Body" frame). The “World” and “Body” frames are only semi-arbitrary. Regardless of conventions, it is natural to think of a rotation intuitively as going from some “static (World)” frame to some “transformed (Body)” frame.
• Perturbation: Only relevant for representations that have defined addition \(\oplus\) and subtraction \(\ominus\) operators and thus tangent-space vector aliases. Perturbation convention determines which tangent space (or "frame") the vector belongs to. The convention is largely up to your preference, and isn't specifically tied to the other conventions used--you just have to be consistent within your algorithm!

The table below gives most, if not all, of the possible convention combinations for the rotational representations used in this guide.

\(^{*}\) There are really \(3^3\) possible orderings of Euler Angles, though a good portion of those are redundant. The three chosen conventions in the table were chosen as (1) NASA standard airplane, (2) NASA standard aerospace, and (3) historically significant.

Two very popular convention groups for quaternions are called the Hamilton and Shuster/JPL conventions. This table will also include the conventions used by some members of my lab:

See Table 1 of the Flipped Quaternion Paper for an overview of literature and software that use the Hamilton and Shuster / JPL conventions.

Introduction: Notions of Distance

A distance metric \(\text{dist}(a,b)\) must satisfy the following properties:

• non-negativity: \(\text{dist}(a,b) \geq 0\)
• identity: \(\text{dist}(a,b)=0 \iff a=b\)
• symmetry: \(\text{dist}(a,b) \geq \text{dist}(b,a)\)
• triangle inequality: \(\text{dist}(a,c) \leq \text{dist}(a,b) + \text{dist}(b,c)\)

There are many possible choices depending on analytical/computational convenience, particularly for rotations. A good review of metrics can be found in R. Hartley, J. Trumpf, Y. Dai, and H. Li. Rotation averaging. IJCV, 103(3):267-305, 2013..

Rotation Matrix

Construction Techniques

From Frame Axes \(A\) & \(B\)

F = Passive:

$$\mathbf{R}_A^B=\begin{bmatrix}^B\mathbf{x}_A & ^B\mathbf{y}_A & ^B\mathbf{z}_A\end{bmatrix}$$

F = Active:

See F = passive, where \(A\) is the source frame and \(B\) is the destination frame.

From Rotation \(\theta\) about n-Axis from World to Body

• 1 and 0's on the n-dimension
• Cosines on the diagonal
• Sines everywhere else...

D = B2W:

• ...Negative sine underneath the 1

D = W2B, F = Passive:

• ...Negative sine above the 1

D = W2B, F = Active:

• ...Negative sine underneath the 1

Conversions (To...)

Euler Angles

FIXME

Rodrigues

i.e., the SO(3) logarithmic map.

D = B2W:

$$\theta=\cos^{-1}\left(\frac{\text{trace}(\mathbf{R})-1}{2}\right)$$

if \(\theta\neq 0\):

$$\theta\mathbf{u} = Log(\mathbf{R}) = \frac{\theta(\mathbf{R}-\mathbf{R}^T)^\vee}{2\sin(\theta)}$$

else:

$$\theta\mathbf{u} = Log(\mathbf{R}) = \mathbf{0}$$

Alternatively, \(\mathbf{u}\) can be thought of as the eigenvector of \(\mathbf{R}\) that corresponds to the eigenvalue $1$.

D = W2B, F = Passive:

FIXME

D = W2B, F = Active:

FIXME

Quaternion

D = B2W:

\(\delta=\text{trace}(\boldsymbol{R})\)

if \(\delta>0\) then

\(s=2\sqrt{\delta+1}\)

\(q_w=\frac{s}{4}\)

\(q_x=\frac{1}{s}(R_{32}-R_{23})\)

\(q_y=\frac{1}{s}(R_{13}-R_{31})\)

\(q_z=\frac{1}{s}(R_{21}-R_{12})\)

else if \(R_{11}>R_{22}\) and \(R_{11}>R_{33}\) then

\(s=2\sqrt{1+R_{11}-R_{22}-R_{33}}\)

\(q_w=\frac{1}{s}(R_{32}-R_{23})\)

\(q_x=\frac{s}{4}\)

\(q_y=\frac{1}{s}(R_{21}+R_{12})\)

\(q_z=\frac{1}{s}(R_{31}+R_{13})\)

else if \(R_{22}>R_{33}\) then

\(s=2\sqrt{1+R_{22}-R_{11}-R_{33}}\)

\(q_w=\frac{1}{s}(R_{13}-R_{31})\)

\(q_x=\frac{1}{s}(R_{21}+R_{12})\)

\(q_y=\frac{s}{4}\)

\(q_z=\frac{1}{s}(R_{32}+R_{23})\)

else

\(s=2\sqrt{1+R_{33}-R_{11}-R_{22}}\)

\(q_w=\frac{1}{s}(R_{21}-R_{12})\)

\(q_x=\frac{1}{s}(R_{31}+R_{13})\)

\(q_y=\frac{1}{s}(R_{32}+R_{23})\)

\(q_z=\frac{s}{4}\)

D = W2B, F = Passive:

FIXME

D = W2B, F = Active:

$\delta=\text{trace}(\boldsymbol{R})$

if $\delta>0$ then

$s=2\sqrt{\delta+1}$

$q_w=\frac{s}{4}$

$q_x=\frac{1}{s}(R_{23}-R_{32})$

$q_y=\frac{1}{s}(R_{31}-R_{13})$

$q_z=\frac{1}{s}(R_{12}-R_{21})$

else if $R_{11}>R_{22}$ and $R_{11}>R_{33}$ then

$s=2\sqrt{1+R_{11}-R_{22}-R_{33}}$

$q_w=\frac{1}{s}(R_{23}-R_{32})$

$q_x=\frac{s}{4}$

$q_y=\frac{1}{s}(R_{21}+R_{12})$

$q_z=\frac{1}{s}(R_{31}+R_{13})$

else if $R_{22}>R_{33}$ then

$s=2\sqrt{1+R_{22}-R_{11}-R_{33}}$

$q_w=\frac{1}{s}(R_{31}-R_{13})$

$q_x=\frac{1}{s}(R_{21}+R_{12})$

$q_y=\frac{s}{4}$

$q_z=\frac{1}{s}(R_{32}+R_{23})$

else

$s=2\sqrt{1+R_{33}-R_{11}-R_{22}}$

$q_w=\frac{1}{s}(R_{12}-R_{21})$

$q_x=\frac{1}{s}(R_{31}+R_{13})$

$q_y=\frac{1}{s}(R_{32}+R_{23})$

$q_z=\frac{s}{4}$

Action