Exponential Coordinates and Matrix Logarithms
Exponential Coordinates and Matrix Logarithms
The exponential map is the bridge between rotations you can calculate with (small angular velocities, which are vectors) and rotations you can see (finite turns of an actual object). It converts “spin at this rate for this long” into the resulting orientation. If the cross product is the engine of rotation, the exponential map is the road that takes you from the engine to the destination.
The Lie Algebra Mapping to Lie Groups
In Chapters 3 and 4, we established that physical orientations live in the Special Orthogonal Group \(\SO\), and complete configurations live in the Special Euclidean Group \(\SE\).
But how does a system actually move between two positions? If a robotic arm starts at \(R_1\) and needs to rotate to \(R_2\), it doesn’t instantly teleport. It sweeps through a continuous arc.
To govern this continuous motion, mathematicians map the curved space of Rotation Matrices into a flat, tangent space where standard calculus operations (like derivatives and velocities) work perfectly. This flat space is the Lie Algebra, denoted as \(\so\) for rotations and \(\se\) for twists.
Exponential Coordinates and Axis-Angle
Let us revisit the concept of rotating around a single fixed unit axis \(\hat{\bm{\omega}}\) by an angle \(\theta\).
If we multiply the physical axis \(\hat{\bm{\omega}} \in \Reals^3\) by the angle \(\theta \in \Reals\), we get a new 3-dimensional vector: \[\begin{equation} \bm{r} = \hat{\bm{\omega}}\theta \end{equation}\]
This incredibly compact vector \(\bm{r} \in \Reals^3\) contains all the information needed to describe any 3D rotation. It is known as the Exponential Coordinate representation of a rotation.
In basic calculus, the solution to the differential equation governing continuous continuous growth (\(\dot{x} = kx\)) is the exponential function: \(x(t) = e^{kt}\).
Similarly, the differential equation governing a rigid body continuously spinning around a fixed angular velocity axis (\(\hat{\bm{\omega}}\)) at speed \(\dot{\theta}\) is solved using exactly the same mathematical structure: the Matrix Exponential.
The Matrix Exponential
To convert our flat, 3-element Exponential Coordinate \(\hat{\bm{\omega}}\theta\) into the true, curved \(3 \times 3\) Rotation Matrix \(R \in \SO\), we must use the Matrix Exponential.
First, we expand the 3-element vector \(\hat{\bm{\omega}}\) into a \(3 \times 3\) skew-symmetric matrix, denoted as \([\hat{\bm{\omega}}]\): \[\begin{equation} [\hat{\bm{\omega}}] = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} \in \so \end{equation}\]
This skew-symmetric matrix is exactly the Lie Algebra \(\so\). It represents the flat tangent space attached to the identity rotation. The Matrix Exponential physically wraps this flat tangent vector around the curved surface of \(\SO\) to produce our final Rotation Matrix:
\[\begin{equation} R = e^{[\hat{\bm{\omega}}]\theta} = I + \sin\theta [\hat{\bm{\omega}}] + (1 - \cos\theta)[\hat{\bm{\omega}}]^2 \end{equation}\]
This formula is known as Rodrigues’ Formula. It bridges the vector world (where things are just numbers) and the geometric world (where things are rotation matrices).
The Exponential Map on SE(3)
Rodrigues’ formula handles rotations alone. A body in free space can also translate while it spins; the full exponential map takes a screw axis in \(\se\) to a rigid transform in \(\SE\).
Let \(\screw = [\hat{\bm{\omega}};\, \bm{v}] \in \se\) with \(\|\hat{\bm{\omega}}\| = 1\) and rotation angle \(\theta\). The closed-form SE(3) exponential is
\[\begin{equation} e^{[\screw]\theta} = \begin{bmatrix} e^{[\hat{\bm{\omega}}]\theta} & G(\hat{\bm{\omega}}, \theta)\,\bm{v} \\ \bm{0} & 1 \end{bmatrix}, \end{equation}\]
where \(e^{[\hat{\bm{\omega}}]\theta}\) is Rodrigues’ formula above and
\[\begin{equation} G(\hat{\bm{\omega}}, \theta) = I\,\theta + (1 - \cos\theta)\,[\hat{\bm{\omega}}] + (\theta - \sin\theta)\,[\hat{\bm{\omega}}]^2. \end{equation}\]
Three special cases are worth committing to memory:
- Pure rotation (\(\bm{v} = \bm{0}\)): the translation block vanishes; the formula reduces to Rodrigues’ rotation alone.
- Pure translation (\(\hat{\bm{\omega}} = \bm{0}\)): \(e^{[\hat{\bm{\omega}}]\theta} = I\) and \(G = I \theta\), so the transform is simply \(T = \bigl[\, I \;|\; \bm{v}\theta \,\bigr]\).
- Pure screw with finite pitch \(h\): \(\bm{v} = -\hat{\bm{\omega}} \times \bm{q} + h\hat{\bm{\omega}}\) for some point \(\bm{q}\) on the screw axis. The body rotates by \(\theta\) about the axis while sliding by \(h\theta\) along it.
See Murray, Li, & Sastry (1994) §2.3 and Lynch & Park (Lynch and Park 2017) §3.3 for derivations.
The Matrix Logarithm
What if we already have a Rotation Matrix \(R\), perhaps from a camera sensor tracking a drone, and we need to know what axis it spun around, and by how much?
We must perform the exact inverse operation: the Matrix Logarithm. The Matrix Logarithm takes a curved element from the Lie Group \(R \in \SO\) and perfectly flattens it back onto the tangent space to extract the axis-angle representation.
\[\begin{equation} [\hat{\bm{\omega}}]\theta = \log(R) \end{equation}\]
The Matrix Logarithm is computationally critical in optimal control. If your system is at orientation \(R_{current}\) and your goal is \(R_{target}\), you cannot simply compute an “error” by subtracting them \((R_{target} - R_{current})\) because matrices do not live in flat space.
The true geometric error—the precise arc of the rotation needed to correct the deviation—is found using the Logarithm of their relative transform: \[ \text{Error} = \log(R_{target} R_{current}^T) \]
Numerical Computation of log(R)
A usable implementation of the matrix log must branch around two degenerate cases, \(\theta \to 0\) and \(\theta \to \pi\), where naïve formulas divide by zero or lose all significant digits. The canonical three-branch algorithm:
import numpy as np
def matrix_log_SO3(R, eps=1e-8):
"""
Return the rotation vector omega*theta in R^3 such that
expm(skew(omega*theta)) == R, with stable handling of both
near-identity (theta->0) and near-pi branches.
"""
tr = np.trace(R)
# Branch 1: near identity --- first-order from skew part of R - I.
if tr > 3.0 - eps:
return np.array([R[2, 1] - R[1, 2],
R[0, 2] - R[2, 0],
R[1, 0] - R[0, 1]]) * 0.5
# Branch 2: near pi --- Shepperd's method, extract axis from diag of (R + I)/2.
if tr < -1.0 + eps:
M = 0.5 * (R + np.eye(3))
# pick the largest diagonal entry to avoid sqrt of a small number
k = int(np.argmax(np.diag(M)))
axis = M[:, k] / np.sqrt(M[k, k])
# Resolve the global sign from the skew part of R: for theta = pi the
# off-diagonal differences R[i,j]-R[j,i] vanish in exact arithmetic, so
# use the largest available skew component to orient the axis. At exactly
# theta = pi the rotation is its own inverse and +axis/-axis are the SAME
# rotation (antipodal ambiguity), so either sign is correct.
skew = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
j = int(np.argmax(np.abs(skew)))
if abs(skew[j]) > eps and np.sign(skew[j]) != np.sign(axis[j]):
axis = -axis
return np.pi * axis
# Branch 3: generic.
theta = np.arccos((tr - 1.0) * 0.5)
omega_hat = (1.0 / (2.0 * np.sin(theta))) * np.array([
R[2, 1] - R[1, 2],
R[0, 2] - R[2, 0],
R[1, 0] - R[0, 1],
])
return omega_hat * thetaThis routine round-trips with Rodrigues’ formula to high accuracy on a test suite of angles including \(0\), \(\pi/6\), \(\pi/2\), and \(\pi - \varepsilon\). At exactly \(\theta = \pi\) the axis sign is inherently ambiguous (\(\exp([\pi\,\hat\omega]) = \exp([-\pi\,\hat\omega])\)), so the recovered \(\pm\hat\omega\) are both valid logarithms — the round-trip is exact even though the sign is not unique. References: Shoemake 1985 / Shepperd 1978 for numerical stability at \(\theta \approx \pi\); Park & Lynch (Lynch and Park 2017) §3.3 for the generic branch.
Non-Commutativity and the BCH Formula
A fact that looks innocent but underpins everything in the next three chapters: the exponential map does not distribute over addition when the arguments do not commute. For two matrices \(A, B\) with \([A, B] = AB - BA \neq 0\),
\[ \log\bigl(e^A\, e^B\bigr) = A + B + \tfrac{1}{2}[A, B] + \tfrac{1}{12}\bigl[A, [A, B]\bigr] - \tfrac{1}{12}\bigl[B, [A, B]\bigr] + \cdots \]
This is the Baker–Campbell–Hausdorff (BCH) series. In \(\se\), the matrices \([\screw_i]\) of distinct joints generically do not commute (pitches differ, axes are not parallel), so composing rotations by summing their Lie-algebra coordinates is wrong at second order and higher. This is exactly why Chapter 9’s Product of Exponentials formula multiplies \(e^{[\screw_1]\theta_1}\, e^{[\screw_2]\theta_2}\,\cdots\) in order rather than taking \(\exp\) of a sum. For a first-order check in a small neighborhood of the identity, \(A + B\) is a usable approximation; anywhere else, trust the product form.
The Quaternion Connection
In Chapter 3, we briefly introduced Unit Quaternions \(q \in \Reals^4\) as a blazing-fast mapping of \(\SO\). Their speed and lack of singularities come directly from their deep connection to Exponential Coordinates.
Recall the Quaternion definition: \[\begin{equation} q = \begin{bmatrix} \cos(\theta/2) \\ \hat{\bm{\omega}} \sin(\theta/2) \end{bmatrix} \end{equation}\]
A quaternion is quite literally the Exponential Coordinate vector (\(\hat{\bm{\omega}}\theta\)) passed through a sine/cosine wave so that it algebraically bounds itself to a unit hypersphere. Quaternions explicitly preserve the pure geometric intention of the axis \(\hat{\bm{\omega}}\) and the continuous rotation angle \(\theta\).
Because Quaternions map rotation so cleanly via this exponential structure, interpolating halfway between two orientations \(q_1\) and \(q_2\) (called Slerp - Spherical Linear Interpolation) traces the exact, perfect geodesic arc across rotation space, whereas interpolating Euler Angles creates chaotic, unpredictable wobbling.