Concept 07: 3D Rotations, Gimbal Lock & Quaternions

▶ Interactive Demo: Gimbal Lock & Quaternion Explorer

Drive roll, pitch and yaw on a wireframe robot, swap the order the turns are applied in, and watch the quaternion update live. Push pitch toward 90° and watch the roll and yaw axes collapse onto each other.

1. The Real-World Problem: The Field Is Not Flat

Every concept so far has needed exactly one angle: heading. A navX reports it, Concept 02 rotates a joystick command by it, Concept 04 recovers it with atan2, Concept 05 wraps it. One number, because the carpet is flat.

Now drive up a ramp. The nose lifts 20 degrees and heading does not change — it cannot, having only ever measured a turn about the vertical. Yet a camera bolted to the robot now points 20 degrees into the air.

The gap opens everywhere: a turret that both spins and elevates has two independent axes, and an AprilTag hangs on a wall at a specific attitude — which is why WPILib returns a tag’s location as a Pose3d. The obvious answer, one angle per axis, is broken in a diagnosable way.

2. Building the Math: From Three Angles to Four Numbers

Step 1: One angle per axis

In 3D there are three perpendicular axes, so the natural guess is three numbers. Fix a frame on the robot — X out the nose, Y out the left side, Z straight up — and name a turn about each.

Roll — about X, the nose axis. The robot leans onto one side’s wheels.
Pitch — about Y. The nose goes up or down; the ramp above is 20 degrees of pitch.
Yaw — about Z, the vertical. Heading, and all Concepts 01 through 05 needed.

These are the Euler angles: readable, and what every IMU dashboard shows. Nothing below says stop reading them — it says stop storing orientation in them.

Step 2: The order is part of the answer

Concept 02 ended on a warning. In 2D, rotating by α then β is one rotation by α + β, and α + β = β + α, so order cannot matter. In 3D that argument collapses: a rotation is not a number added to a number.

Try it with a real book, laid flat, cover up, top edge pointing away from you. Yaw, then pitch: spin it 90° counter-clockwise on the table, then tip it 90° away from you. Reset and swap: tip it away first, then spin it about the vertical. The two end up visibly different — track the top edge, which started along +X:

\[\begin{aligned} \text{yaw 90°, then pitch 90°:} \quad +X &\longrightarrow +Y \\[4pt] \text{pitch 90°, then yaw 90°:} \quad +X &\longrightarrow -Z \end{aligned}\]

So “roll 30, pitch 20, yaw 45” is not an orientation — it is three numbers waiting for a convention: which axis turns first, and whether the turns are about fixed world axes or the body’s own moving axes. Libraries disagree. WPILib’s Rotation3d(roll, pitch, yaw) is extrinsic — roll about fixed X, then pitch about Y, then yaw about Z — where aerospace usually means body-axis yaw, pitch, roll. Cross the two and the answer is wrong but plausible.

Math!

Write R(θ) for a rotation by θ, and a product for applying one then another. In 2D R(α)R(β) = R(β)R(α); in 3D R₁R₂ ≠ R₂R₁, which is what non-commutative means. Read R₁R₂ as “R-one composed with R-two” — and the rightmost factor is applied first.

Step 3: Gimbal lock, derived

The next problem is fatal. Picture a two-axis turret, read the aerospace way: yaw about the vertical, then pitch about the new side-to-side axis, then roll about the nose. With the barrel horizontal these are three different motions.

Now elevate the barrel through 90 degrees of pitch, until it points straight up. (A right-hand turn about +Y tips the nose down, so “straight up” is a pitch of −90°.) The nose axis is now the vertical — the line the yaw stage turns about. Roll and yaw have become the same rotation.

A roll of φ about a vertical nose is a yaw of φ, so the pair only produces yaw + roll about the vertical:

   yaw    pitch   roll     resulting orientation
   ------------------------------------------------
    0°    -90°      0°     identical
   30°    -90°    -30°     identical
   45°    -90°    -45°     identical

   30°    -90°      0°     identical
    0°    -90°     30°     identical

Three dials, two degrees of freedom. The encoding is many-to-one — infinitely many (yaw, roll) pairs name that orientation, so converting back has no unique answer. This is gimbal lock, after the three-ring mechanism whose rings become coplanar.

The damage is the neighborhood, not the exact singularity. Elevate to 89.9 degrees instead, so the barrel points 0.1 degrees off vertical, and sweep yaw through 90 degrees. The nose travels a circle of angular radius 0.1 degrees, and two points 90 degrees apart on it differ by

\[\sqrt{2} \times 0.1° = 0.141°\]

of actual direction. Read that backwards: a 0.141-degree nudge of the barrel demands a 90-degree jump in reported yaw. Millidegree IMU noise becomes yaw swings of tens of degrees per loop cycle, and a controller sees a huge error from nothing. Point an arm straight up and a naive Euler-angle controller thrashes.

Step 4: Euler’s rotation theorem

Any orientation of a rigid body about a fixed point, however you got there, is a single rotation by some angle about some single axis. This is Euler’s rotation theorem, and it is the key idea of this concept.

Test it on the book. A rotation of 120 degrees about the diagonal (1, 1, 1)/√3 sends +X → +Y, +Y → +Z, +Z → +X — what yaw-then-pitch did. The reverse order is a different single rotation: 120 degrees about (−1, 1, 1)/√3.

Why must it hold? A rotation preserves lengths and fixes the origin, so it maps the unit sphere onto itself, and such a map either turns the sphere about an axis or reflects it. Reflection is out — it would turn a right hand into a left one. What survives is a turn, and a turn has an axis: the two points that did not move.

So orientation is a direction plus an amount: an axis, 3 numbers, and an angle, 1 more. No order to get wrong, and no configuration where controls collide.

Step 5: Quaternions encode axis-angle

Axis-angle is right but awkward: composing two pairs is a mess, and the axis is undefined at zero angle. The quaternion fixes both, and is what your IMU is really computing — four numbers, from the axis n̂ and angle θ:

\[\begin{aligned} w &= \cos\!\left(\frac{\theta}{2}\right) \\[6pt] (x,\ y,\ z) &= \hat{n} \cdot \sin\!\left(\frac{\theta}{2}\right) \end{aligned}\]

Every ingredient is from Concept 01: a cosine, a sine, and a scaled unit vector.

Why the half? Because of how a quaternion is applied. A lone quaternion product does not send 3D vectors to 3D vectors while preserving lengths; what works is the two-sided sandwich q v q⁻¹. Since q appears on both sides, the angle inside it is used twice — store the full θ and you get a turn of 2θ, store θ/2 and the two applications combine into exactly θ.

Since the half-angle runs 0 to 180 degrees as θ runs 0 to 360, q and −q name the same orientation: a log will occasionally show all four components flip sign with the robot still, and nothing is wrong.

Math!

(n̂, θ) is the axis-angle representation, read “a rotation of theta about the axis n-hat”; as with Concept 02’s î and ĵ, the hat means length 1. q = (w, x, y, z) is read “the quaternion w, x, y, z”, from quaternio, Latin for a set of four — Hamilton’s 1843 extension of the complex numbers, as the complex numbers extend the reals. You need none of that algebra, only that four numbers of length 1 encode an axis and an angle.

Step 6: The constraint is Concept 01’s identity

Not every four numbers is a rotation. It has to be a unit quaternion:

\[w^2 + x^2 + y^2 + z^2 = 1\]

It falls out of the definition, using n̂’s own unit length nx² + ny² + nz² = 1:

\[\begin{aligned} w^2 + x^2 + y^2 + z^2 &= \cos^2\!\left(\tfrac{\theta}{2}\right) + \left(n_x^2 + n_y^2 + n_z^2\right)\sin^2\!\left(\tfrac{\theta}{2}\right) \\[4pt] &= \cos^2\!\left(\tfrac{\theta}{2}\right) + \sin^2\!\left(\tfrac{\theta}{2}\right) = 1 \end{aligned}\]

The last line is Concept 01’s sin² + cos² = 1 — Pythagoras on a hypotenuse of 1 — at the half-angle. It proved there that splitting a speed preserves the speed, and in Concept 02 that a 2D rotation preserves length. Same job here: the unit constraint keeps the rotation rigid, and no Euler triple offers a self-check like it.

Step 7: The book experiment, in quaternions

Yaw 90 degrees turns about (0, 0, 1) with half-angle 45 degrees, and cos 45° = sin 45° = 0.70711:

\[q_{\text{yaw}} = (0.70711,\ 0,\ 0,\ 0.70711) \qquad q_{\text{pitch}} = (0.70711,\ 0,\ 0.70711,\ 0)\]

Multiply them in the two orders:

   yaw then pitch:   q = ( 0.5,  0.5, 0.5, 0.5)   ->  120° about ( 1, 1, 1)/sqrt(3)
   pitch then yaw:   q = ( 0.5, -0.5, 0.5, 0.5)   ->  120° about (-1, 1, 1)/sqrt(3)

Both satisfy 0.5² × 4 = 1, so both are legal rotations, and both are turns of 2 × arccos(0.5) = 120°. They differ in one sign — the difference between the nose pointing left and pointing at the floor. Concept 02’s non-commutativity, made arithmetic.

Step 8: What this buys a team

No gimbal lock, anywhere. Every unit quaternion is an ordinary point on the unit sphere in four dimensions; the barrel straight up is (0.70711, 0, −0.70711, 0), four unremarkable numbers. The singularity was never in the rotation, only in the encoding.
Composition is multiplication. “This rotation, then that one” is one quaternion product — sixteen multiplies, twelve adds, no trigonometry. Concept 03’s field → robot → camera chain is the same in 3D.
Drift is cheap to repair. Products accumulate floating-point error until the four squares stop summing to 1; the repair is one square root and four divisions. Three Euler angles have no invariant to check.
Interpolation works. Geometry Concept 03 warned that angles cannot be blended naively — halfway between 350° and 10° comes out 180°, exactly backwards — and it returns in 3D: averaging roll, pitch and yaw separately lurches through orientations neither endpoint was near. Blending quaternions along the shortest arc of the 4D unit sphere gives a smooth turn about one axis instead: SLERP.

The honest caveat. Nobody looks at (0.90984, 0.06645, −0.16043, 0.37687) and pictures a robot; Euler angles are far better for a dashboard or a log read at 2 a.m. So: store and compute in quaternions, convert to Euler angles for humans, and never convert back.

3. Solving It in Code (Java & WPILib)

First Principles (Java)

// A unit quaternion from an axis-angle pair. The axis must have length 1.
static double[] fromAxisAngle(double nx, double ny, double nz, double angleRad) {
    double len = Math.sqrt(nx * nx + ny * ny + nz * nz);
    nx /= len; ny /= len; nz /= len;              // make the axis a unit vector
    double half = angleRad / 2.0;                 // the sandwich uses q twice
    double s = Math.sin(half);
    return new double[] { Math.cos(half), nx * s, ny * s, nz * s };
}

// Compose: apply b first, then a. Sixteen multiplies, no trig.
static double[] multiply(double[] a, double[] b) {
    return new double[] {
        a[0]*b[0] - a[1]*b[1] - a[2]*b[2] - a[3]*b[3],
        a[0]*b[1] + a[1]*b[0] + a[2]*b[3] - a[3]*b[2],
        a[0]*b[2] - a[1]*b[3] + a[2]*b[0] + a[3]*b[1],
        a[0]*b[3] + a[1]*b[2] - a[2]*b[1] + a[3]*b[0]
    };
}

static double norm(double[] q) {
    return Math.sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
}

// Step 7, checked by hand.
double[] yaw90   = fromAxisAngle(0, 0, 1, Math.toRadians(90));  // (0.70711, 0, 0, 0.70711)
double[] pitch90 = fromAxisAngle(0, 1, 0, Math.toRadians(90));  // (0.70711, 0, 0.70711, 0)

double[] yawThenPitch = multiply(pitch90, yaw90);   // ( 0.5,  0.5, 0.5, 0.5)
double[] pitchThenYaw = multiply(yaw90, pitch90);   // ( 0.5, -0.5, 0.5, 0.5)

System.out.println(norm(yawThenPitch));   // 1.0  <- still a legal rotation
System.out.println(norm(pitchThenYaw));   // 1.0  <- also legal, and NOT the same one

Reading the axis and angle back is the definition in reverse: w gives the angle, and dividing the other three by sin(θ/2) gives the axis.

// Recover (axis, angle) from a unit quaternion. Round-trips fromAxisAngle exactly.
static double[] toAxisAngle(double[] q) {
    double w = Math.max(-1.0, Math.min(1.0, q[0]));   // clamp against drift
    double angle = 2.0 * Math.acos(w);
    double s = Math.sqrt(1.0 - w * w);                // this is sin(angle / 2)
    if (s < 1e-9) {
        return new double[] { 0, 0, 1, 0 };           // no rotation: axis is arbitrary
    }
    return new double[] { q[1] / s, q[2] / s, q[3] / s, angle };
}

double[] back = toAxisAngle(yawThenPitch);
// axis (0.57735, 0.57735, 0.57735) = (1,1,1)/sqrt(3),  angle 2.0944 rad = 120.000 deg

That 1e-9 guard is the only special case in the file, and it is not a singularity: a zero rotation has no distinguished axis. Euler-angle code needs a branch whenever pitch nears ±90 degrees, with no correct answer inside it.

In a Robot Project (Java & WPILib)

import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.VecBuilder;

// 1. From roll, pitch and yaw -- the readable route, and what an IMU hands you.
//    WPILib documents these as EXTRINSIC: roll about fixed X, then pitch about
//    fixed Y, then yaw about fixed Z. Another library's triple may not mean this.
Rotation3d onTheRamp = new Rotation3d(
    Math.toRadians(0.0),     // roll
    Math.toRadians(-20.0),   // pitch -- nose 20 degrees UP the ramp; see Step 3
    Math.toRadians(45.0));   // yaw

// 2. From an axis and an angle -- Euler's rotation theorem, spelled out.
Rotation3d spinUp = new Rotation3d(VecBuilder.fill(0, 0, 1), Math.toRadians(90));

// 3. From a quaternion -- what a navX or Pigeon 2 reports natively.
Rotation3d fromImu = new Rotation3d(new Quaternion(0.70711, 0, 0, 0.70711));

All three constructors do the same thing internally: Rotation3d stores a quaternion and nothing else, as Concept 01’s Rotation2d stores a cosine and a sine rather than an angle. Euler angles are computed on demand, never stored.

Quaternion q = onTheRamp.getQuaternion();
System.out.printf("w %.5f  x %.5f  y %.5f  z %.5f%n",
                  q.getW(), q.getX(), q.getY(), q.getZ());
// w 0.90984  x 0.06645  y -0.16043  z 0.37687

double sumOfSquares = q.getW()*q.getW() + q.getX()*q.getX()
                    + q.getY()*q.getY() + q.getZ()*q.getZ();   // 1.00000

// Euler's theorem, from the library: one axis, one angle.
var axis         = onTheRamp.getAxis();                   // (0.1601, -0.3866, 0.9082)
double angleDeg  = Math.toDegrees(onTheRamp.getAngle());  // 49.0325

// And back to human-readable numbers, for the dashboard only.
double pitchDeg = Math.toDegrees(onTheRamp.getY());       // -20.0
double yawDeg   = Math.toDegrees(onTheRamp.getZ());       // 45.0

A pitch of −20 plus a yaw of 45 is a single rotation of 49.03 degrees about the axis (0.1601, −0.3866, 0.9082). Two turns in, one turn out, as Step 4 promised; the from-scratch tier agrees, reproducing (0.90984, 0.06645, −0.16043, 0.37687) from that axis and angle. And none of this is academic — an AprilTag’s location is a Pose3d:

// Position in three numbers, orientation in a Rotation3d that holds a quaternion.
Pose3d tagPose = new Pose3d(new Translation3d(4.20, 1.83, 1.45), onTheRamp);

// Composition is a quaternion product under the hood -- Step 8, second paragraph.
Rotation3d combined = onTheRamp.rotateBy(spinUp);

// Rotation3d.times(scalar) scales a rotation along its own single axis, which is
// what makes interpolate() a true SLERP rather than a three-angle average.
Rotation3d halfway = onTheRamp.interpolate(spinUp, 0.5);

4. Bridge to Graphics, Games & Machine Learning

Every 3D engine runs on this. Unity stores a rotation only as a Quaternion; transform.eulerAngles is a view computed on demand, and Unity’s own documentation warns against using it as storage. Unreal, Godot, Blender and glTF all store orientation as four numbers. Character animation is the reason: blending two keyframed poses is a SLERP, sixty times a second, on every joint of every skeleton.

Orientation is a live problem in machine learning too, and the representation is the difficulty. A network predicting an object’s pose from an image must emit a rotation, and three Euler angles train badly. The cause is the discontinuity Concept 05 fixed for a single angle, now unavoidable: the map to Euler triples has points where a tiny change in the true orientation forces a large change in the target numbers, and near gimbal lock the target is not unique. A regression loss cannot fit a function with a jump in it, so the network hedges toward the average of both sides.

Predicting a quaternion is what most pose-estimation networks did instead: four outputs normalized to length 1, Step 6’s constraint imposed as a network layer. It is still imperfect — q and −q are the same rotation, so the target is ambiguous unless a sign is chosen consistently — and the standard fix now is a 6D representation, predicting two vectors and orthogonalizing them into a frame.

5. Checkpoints & Exploration Prompts

Checkpoint 1

A robot is level and facing straight down-field: no rotation at all. Write its quaternion. Then write the one for a robot yawed 180 degrees, and confirm from the numbers alone that it is a half turn about the vertical.

Solution:

No rotation is θ = 0. The half-angle is 0, so w = cos 0° = 1 and sin 0° = 0 kills the axis: the identity quaternion is (1, 0, 0, 0), with 1² = 1. ✓ Its axis is undefined, correctly — the 1e-9 branch in toAxisAngle.
Yawed 180 degrees is θ = 180° about n̂ = (0, 0, 1), half-angle 90°, so w = cos 90° = 0 and (x, y, z) = (0, 0, 1) × sin 90°. That is (0, 0, 0, 1), and 1² = 1. ✓
Read it back. θ = 2 × arccos(0) = 180° and sin(θ/2) = 1, so the axis is (0, 0, 1) — straight up.

Checkpoint 2

A shooter’s barrel points straight up: pitch −90 degrees. Software commands yaw 40, roll 0; a moment later, yaw 0, roll 40. The mechanism does not move. Explain why, and say what a controller comparing the two yaw readings would do.

Solution:

The axes. At −90 degrees of pitch the nose has been tipped onto the vertical, so the roll axis and the yaw axis are the same line.
So the two commands are the same rotation. A roll of 40 about a vertical nose is a yaw of 40, and only the sum yaw + roll reaches the hardware — Step 3’s table shows the same collapse for (30, −30).
What the controller sees. Yaw feedback dropped from 40 to 0 while the mechanism sat still — a 40-degree error from nothing. A proportional controller answers with a large output, slamming a stationary mechanism.
Why reality is worse. Near 90 degrees the encoding is violently sensitive rather than merely degenerate: at 89.9 degrees, 0.141° of real motion swings yaw through 90 degrees, and noise alone produces that. The fix is not a harder filter — it is to stop storing orientation as three angles.

Deep Dive 1

Step 5 asserted the half-angle without proving it. Check the simplest case by hand: let q be a rotation of θ about Z, so q = (cos(θ/2), 0, 0, sin(θ/2)), and let v be (1, 0, 0) written as (0, 1, 0, 0). Work out q v q⁻¹ with the multiplication rule from the code section, remembering that a unit quaternion’s inverse negates x, y and z. The result is (0, cos θ, sin θ, 0) — Concept 02’s 2D rotation applied to î. Say where the two half-angles combined, and predict what the full θ would have given.

Deep Dive 2

Step 8 claimed SLERP blends orientations correctly and averaging Euler angles does not. Build the counterexample: A is yaw 170 degrees, B is yaw −170 degrees, roll and pitch zero. Average each angle at t = 0.25, 0.5 and 0.75, describe the path the nose takes, and compare it with Geometry Concept 03’s answer for lerp(350, 10, 0.5). Then convert both to quaternions and work out what goes wrong if a SLERP implementation does not check the sign of their dot product first, recalling that q and −q are the same orientation.

← Concept 06: Law of Cosines

Module 2 Overview

Module 3: Linear Algebra →