Section 4.3: Rotations: matrices, Euler angles, axis-angle, quaternions; pitfalls

"A robot without frames has many coordinates and no agreement."

A Meticulous Mapping Agent

Embodied systems move cameras, wrists, wheels, feet, and object frames through orientation space. A rotation can be logged as yaw-pitch-roll for a human, stored as a quaternion for interpolation, optimized as a rotation vector, and applied as a matrix to points and vectors.

The builder skill is not memorizing one representation. It is choosing the representation that fits the operation, then checking invariants so a convention mistake does not reach hardware.

Theory

Rotation matrices are direct operators on vectors and compose by multiplication. Every valid rotation $R \in SO(3)$ satisfies two invariants that double as runtime checks:

$$R^\top R = I, \qquad \det(R) = +1.$$

The first says the columns are orthonormal; the second rules out reflections (a determinant of $-1$ is a left-handed mirror, not a rotation). Euler angles build a rotation from three elementary rotations about the coordinate axes. Under the ZYX convention with yaw $\psi$, pitch $\theta$, and roll $\phi$,

$$R = R_z(\psi)\,R_y(\theta)\,R_x(\phi).$$

This factorization is compact and readable, but it has a singularity: when $\theta = \pm 90^\circ$ the first and third axes align and one rotational degree of freedom is lost. This is gimbal lock, and it is why controllers and estimators store orientation as a quaternion. A unit quaternion $q = [w, x, y, z]$ with $\lVert q \rVert = 1$ represents a rotation, its conjugate $q^{*} = [w, -x, -y, -z]$ represents the inverse rotation, and quaternions interpolate smoothly along the shortest arc via SLERP:

$$\text{slerp}(q_0, q_1; u) = \frac{\sin((1-u)\Omega)}{\sin\Omega}\,q_0 + \frac{\sin(u\Omega)}{\sin\Omega}\,q_1, \qquad \cos\Omega = q_0 \cdot q_1.$$

The danger is rarely that a rotation representation is mathematically invalid. The danger is that it is valid under a different convention: xyzw instead of wxyz, degrees instead of radians, zyx instead of xyz, active instead of passive, or camera optical axes instead of body axes.

In robotics, the danger is rarely that a rotation representation is mathematically invalid. The danger is that it is valid under a different convention: xyzw instead of wxyz, degrees instead of radians, zyx instead of xyz, active instead of passive, or camera optical axes instead of body axes.

Worked Example

This fragment walks the whole representation chain. It builds a composed rotation $R_z(45^\circ)R_y(30^\circ)R_x(0^\circ)$ from elementary matrices and verifies the two invariants, demonstrates gimbal lock at pitch $\theta = 90^\circ$, forms a quaternion and its conjugate, and SLERPs between two orientations.

# Rotation representations end to end: matrices, invariants, gimbal lock,
# quaternion conjugate, and SLERP. All angles in degrees unless noted.
import numpy as np
from scipy.spatial.transform import Rotation as Rot, Slerp

def Rx(d): c,s=np.cos(np.radians(d)),np.sin(np.radians(d)); return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def Ry(d): c,s=np.cos(np.radians(d)),np.sin(np.radians(d)); return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def Rz(d): c,s=np.cos(np.radians(d)),np.sin(np.radians(d)); return np.array([[c,-s,0],[s,c,0],[0,0,1]])

# 1. Composed rotation and its invariants.
R = Rz(45) @ Ry(30) @ Rx(0)
print("R.T @ R = I? ", np.allclose(R.T @ R, np.eye(3)))
print("det(R) =     ", round(float(np.linalg.det(R)), 6))

# 2. Gimbal lock: at pitch = 90 deg, yaw and roll act on the same axis.
locked = Rot.from_euler("ZYX", [40.0, 90.0,  0.0], degrees=True)
same   = Rot.from_euler("ZYX", [ 0.0, 90.0, -40.0], degrees=True)  # different angles
print("gimbal lock (two angle sets, one rotation)?",
      np.allclose(locked.as_matrix(), same.as_matrix()))

# 3. Quaternion [w,x,y,z] and its conjugate (inverse rotation).
q = Rot.from_matrix(R).as_quat()                 # scipy returns [x,y,z,w]
q_wxyz = np.array([q[3], q[0], q[1], q[2]])
q_conj = np.array([q_wxyz[0], -q_wxyz[1], -q_wxyz[2], -q_wxyz[3]])
print("q      [w,x,y,z]:", q_wxyz.round(4).tolist())
print("q_conj [w,x,y,z]:", q_conj.round(4).tolist())

# 4. SLERP halfway between two orientations.
key = Rot.from_euler("ZYX", [[0,0,0],[90,0,0]], degrees=True)
mid = Slerp([0.0, 1.0], key)([0.5])
print("SLERP midpoint yaw (deg):",
      round(float(mid.as_euler("ZYX", degrees=True)[0,0]), 3))

Builder Recipe

1. Pick one internal rotation representation per subsystem.
2. Document Euler axis order, quaternion component order, handedness, and units.
3. Check matrix orthogonality, determinant, and quaternion norm in logs.
4. Round-trip through every serialization format used by the robot.
5. Visualize at least one known pose in simulation before running hardware.

Production Pattern

Rotations: matrices, Euler angles, axis-angle, quaternions; pitfalls sits inside the Part II robotics contract: geometry defines where things are, kinematics defines what motion is possible, dynamics defines what motion costs, control defines how errors are corrected, and sensing defines what the agent can know on time.

Use quaternions or rotation matrices for computation, and treat Euler angles as a display or UI format. This makes the section useful to students, builders, and researchers at the same time: the idea has an intuitive role, a formal interface, a runnable check, and a failure mode that can be reproduced.

Use this recipe when turning Rotations: matrices, Euler angles, axis-angle, quaternions; pitfalls into code, a simulator experiment, or a robot diagnostic. The point is not to use every library. The point is to keep the hand-built baseline and the maintained-tool path comparable.

1. Name every frame with a parent, child, unit convention, and timestamp policy.
2. Write one hand-checked transform chain and verify identity, inverse, and composition tests.
3. Run the same transform through ROS 2 tf2 or SciPy Rotation, then compare one point and one direction vector.
4. Record a frame audit with source sensor, latency, and expected sign convention.
5. Debug failed behavior by replaying the transform tree before changing policy or controller code.

Most rotation bugs come from order conventions, degrees versus radians, intrinsic versus extrinsic rotations, quaternion ordering, or normalization. A one-vector rotation test catches them early.