The problem
In chapter 0.1 you crossed a quaternion seam without stopping to look at it. One line in the rerun logging said the quiet part out loud:
MuJoCo stores quaternions wxyz; rerun wants xyzw — hence the reindex.
You reindexed four numbers and moved on. That worked because someone had already figured out which convention each library used and written the swap for you. The moment you're the one composing a block pose with a gripper pose — the moment the transform is yours — that borrowed knowledge runs out, and a wrong answer doesn't announce itself. There's no exception, no NaN, no stack trace. The block just ends up pointing the wrong way, or a centimeter off, and your policy trains on quietly corrupted coordinates for six hours before you notice.
The reason this is dangerous is that rotation math has no natural error signal. Add two positions in the wrong order and you'll often see something obviously broken. Compose two rotations in the wrong order and you get a perfectly valid rotation — just not the one you wanted. So the only defense is an answer key you trust absolutely. We have one: MuJoCo ships the same operations as C functions (mju_mulQuat, mju_quat2Mat, mju_rotVecQuat), and it's the library that drives every simulation in this book. So that's what we build: the rotation toolkit from scratch, in numpy, checked line by line against MuJoCo until the two agree to the fifteenth decimal — and then, deliberately, each of the three classic bugs, so you can measure exactly what each one costs.
Build
The file is about 260 lines, most of that the comments that walk through each operation — the code itself is a little over 160 — and there's no physics in it at all, no mj_step, no mjData. It's five short regions: the setup, then the two quaternion operations everything else is built from, then the two ways to spend a quaternion on a vector, then the Frame, then a demo that proves the whole thing against MuJoCo and draws it in rerun.
Setup
One thing to look for: the only randomness in the file is the generator that draws test quaternions, and it feeds exactly one thing — the answer-key comparison against MuJoCo.
import argparseimport jsonimport sysfrom pathlib import Path import mujocoimport numpy as npimport rerun as rr # Chapter artifacts run as loose scripts from the repo root; put the root on# sys.path so `curriculum.common` resolves (same pattern as ch0.1). device.py# keeps its torch import lazy, so this torch-free chapter stays torch-free.sys.path.insert(0, str(Path(__file__).resolve().parents[3]))from curriculum.common.device import banner # noqa: E402 parser = argparse.ArgumentParser(description=__doc__)parser.add_argument("--seed", type=int, default=0, help="seeds the random quaternions/points the checks run on")parser.add_argument("--samples", type=int, default=512, help="how many random (quat, point) pairs to check against mju_*")parser.add_argument("--smoke", action="store_true", help="fixed 512-sample run for CI; two runs must match byte-for-byte")parser.add_argument("--out", type=Path, default=Path("outputs/ch0.3-transforms"))parser.add_argument("--rerun", dest="rerun", action="store_true", default=True) # recording is the default; opt OUT, not inparser.add_argument("--no-rerun", dest="rerun", action="store_false", help="skip the .rrd recording (CI smoke)")# The three bugs everyone writes, each selectable so you can watch it diverge:parser.add_argument("--break", dest="bug", default="none", choices=["none", "quat-convention", "compose-order", "point-vs-frame"], help="inject a classic bug and measure the error it introduces")args = parser.parse_args() banner("ch0.3-transforms") # startup contract: tier + measured wall-clock to stdout, not metrics.jsonnum_samples = 512 if args.smoke else args.samples # smoke count is FIXED so CI can diff runs exactlyargs.out.mkdir(parents=True, exist_ok=True)rng = np.random.default_rng(args.seed) # PCG64 — the only source of randomness in this fileThe flags match every chapter's shape — --seed, --smoke, --out, --no-rerun — plus one that's specific to this chapter: --break, which takes the name of a bug to inject. Its default is none, and when it's none the code is correct and agrees with MuJoCo; the whole Break It section is just this one flag set to something else. The convention decision that governs the entire chapter is stated in the docstring and never revisited: a quaternion is [w, x, y, z], scalar first, the MuJoCo order. Every function below assumes it, and bug number one is what happens when a caller doesn't.
Quaternions
Two operations generate every rotation you'll ever compute. Here they are.
# A unit quaternion [w, x, y, z] encodes a 3D rotation in four numbers. Two# operations generate everything else: multiplying two of them composes their# rotations, and conjugating one inverts it. Multiplication is the Hamilton# product — NOT componentwise, and NOT commutative (q1*q2 rotates differently# from q2*q1, which is Break It #2). Write it out once, by hand, and never again.def quat_multiply(q1, q2): w1, x1, y1, z1 = q1 w2, x2, y2, z2 = q2 return np.array([ w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, # w: the scalar part w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, # x w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, # y w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, # z ]) def quat_conjugate(q): # For a UNIT quaternion, the conjugate is the inverse rotation: flip the # vector part, keep the scalar. (Non-unit quaternions also need a /|q|^2, # but every rotation quaternion is unit, so we never pay for that here.) w, x, y, z = q return np.array([w, -x, -y, -z])quat_multiply is the Hamilton product, written out term by term, and it is worth reading all four lines even though you will never read them again. Two facts hide in it. First, it is not componentwise — you cannot multiply quaternions the way you'd multiply two arrays, and numpy will happily let you try and hand back garbage. Second, it does not commute: quat_multiply(a, b) is a different rotation from quat_multiply(b, a), in general, and that asymmetry is bug number two waiting to happen. quat_conjugate is almost too simple to notice — flip the sign of the vector part — but for a unit quaternion it is the inverse rotation, which is the fact the Frame's inverse leans on later. The comment earns its place here: it's only the inverse because the quaternion is unit-length, and every rotation quaternion is, so we never pay for the general case.
Rotations
There are two ways to actually apply a quaternion to the world, and you'll want both.
# Two ways to spend a quaternion. First, bake it into a 3x3 rotation matrix —# handy when you'll rotate many vectors, and the form MuJoCo hands back from# mju_quat2Mat. Every term is quadratic in the quaternion components; this is# the exact expansion of q*(0,v)*conj(q) collected into a matrix.def quat_to_matrix(q): w, x, y, z = q return np.array([ [1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)], [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)], [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)], ]) # Second, rotate a single vector directly: promote v to a pure quaternion# (0, v), sandwich it as q * (0, v) * conj(q), and read off the vector part.# This is what mju_rotVecQuat computes, and feeding it a WRONG-order quaternion# is Break It #1 — the bug is invisible in the code and loud in the result.def rotate_vector(q, v): pure = np.array([0.0, v[0], v[1], v[2]]) # a vector as a quaternion with zero scalar part rotated = quat_multiply(quat_multiply(q, pure), quat_conjugate(q)) return rotated[1:] # drop the (now ~0) scalar part; the vector part is the answerquat_to_matrix bakes the rotation into a 3×3 matrix — the form MuJoCo hands back from mju_quat2Mat, and the one you want when you're about to rotate a hundred vectors and don't want to pay for the quaternion sandwich each time. Every entry is quadratic in the components; it's the sandwich product collected into a matrix once, algebraically.
rotate_vector is the sandwich itself: promote the vector v to a quaternion with a zero scalar part, compute q * (0, v) * conj(q), and read off the vector part of the result. That's it — that's what "rotate a vector by a quaternion" is, and it's exactly what mju_rotVecQuat computes. It's also the single most bug-prone line in robotics, because if the q you pass in is in the wrong component order, this function runs without complaint and returns a wrong vector. Hold that thought for four paragraphs.
Frames
A rotation alone isn't enough — the block isn't just turned, it's turned and somewhere. A Frame is the pair.
# A Frame is a rigid transform: a rotation (as a quaternion) plus a translation.# Read it as "world_from_tee" — it takes a point written in the tee's local# coordinates and returns the same point in world coordinates. That naming is# not decoration: it's how you catch Break It #3, because frame_a.compose(# frame_b) only type-checks in your head when a's "from" matches b's "to".class Frame: def __init__(self, rotation, translation): self.rotation = np.asarray(rotation, dtype=float) # unit quaternion [w, x, y, z] self.translation = np.asarray(translation, dtype=float) # [x, y, z] in the parent frame def transform_point(self, point): # Rotate the point out of local coordinates, THEN shift by translation. # Order matters: translating first would rotate the offset too. return rotate_vector(self.rotation, point) + self.translation def compose(self, other): # self is world_from_a, other is a_from_b -> result is world_from_b. # Rotations multiply; the child's origin rides through the parent transform. return Frame(quat_multiply(self.rotation, other.rotation), self.transform_point(other.translation)) def inverse(self): # world_from_tee -> tee_from_world. The inverse rotation is the # conjugate; the inverse translation is where the world origin sits # once you've undone the rotation. inverse_rotation = quat_conjugate(self.rotation) return Frame(inverse_rotation, -rotate_vector(inverse_rotation, self.translation)) def yaw_quaternion(yaw): # A planar rotation about +z, the only rotation a PushT block ever makes. return np.array([np.cos(yaw / 2.0), 0.0, 0.0, np.sin(yaw / 2.0)]) def random_unit_quaternion(generator): q = generator.standard_normal(4) # a Gaussian in 4D, normalized, is uniform over rotations return q / np.linalg.norm(q)Read the name world_from_tee left to right as a machine: it takes a point written in the tee's coordinates and returns that point in the world's. That naming convention is the single most useful habit in this chapter, because it makes composition checkable by eye. frame_a.compose(frame_b) is only meaningful when the "from" of a matches the "to" of b — world_from_tee.compose(tee_from_pusher) reads cleanly (the tees meet in the middle); tee_from_pusher.compose(world_from_tee) does not, and that mismatch is bug number three, visible in the names before you ever run anything.
Three methods carry it. transform_point rotates first and translates second, and the comment says why order matters: translate first and you'd rotate the offset along with the point. compose multiplies the rotations and sends the child's origin through the parent transform. inverse conjugates the rotation and works out where the parent's origin lands once the rotation is undone — the -rotate_vector(inverse_rotation, translation) line is the part people get wrong, because it is not just negating the translation; the translation has to be rotated into the new frame first.
Demo
Now we prove it and draw it. The proof comes first.
# The answer key: run every from-scratch op on `num_samples` random quaternions# and points, and record the worst disagreement with MuJoCo's mju_* functions.# Correct code lands at ~1e-15 (machine epsilon); that number IS the proof.def max_error_against_mujoco(generator, count): err_multiply = err_matrix = err_rotate = err_roundtrip = 0.0 reference = np.zeros(4) reference_matrix = np.zeros(9) reference_vector = np.zeros(3) for _ in range(count): q1 = random_unit_quaternion(generator) q2 = random_unit_quaternion(generator) v = generator.standard_normal(3) mujoco.mju_mulQuat(reference, q1, q2) err_multiply = max(err_multiply, np.max(np.abs(quat_multiply(q1, q2) - reference))) mujoco.mju_quat2Mat(reference_matrix, q1) err_matrix = max(err_matrix, np.max(np.abs(quat_to_matrix(q1).reshape(9) - reference_matrix))) mujoco.mju_rotVecQuat(reference_vector, v, q1) err_rotate = max(err_rotate, np.max(np.abs(rotate_vector(q1, v) - reference_vector))) # A Frame and its inverse must return any point exactly where it started. frame = Frame(q1, generator.standard_normal(3)) roundtrip = frame.inverse().transform_point(frame.transform_point(v)) err_roundtrip = max(err_roundtrip, np.max(np.abs(roundtrip - v))) return err_multiply, err_matrix, err_rotate, err_roundtrip # The concrete question this chapter exists to answer: the PushT block sits at# some pose; the pusher is somewhere in the world. Where is the pusher AS THE# BLOCK SEES IT — in the block's own frame? That is one inverse and one# transform_point, the exact move a reward function or a policy input makes.tee_yaw = 0.9 # radians the block is turnedworld_from_tee = Frame(yaw_quaternion(tee_yaw), np.array([0.12, -0.05, 0.0]))pusher_world = np.array([0.20, 0.10, 0.0]) # pusher, written in world coordinatespusher_in_tee = world_from_tee.inverse().transform_point(pusher_world)pusher_recovered = world_from_tee.transform_point(pusher_in_tee) # compose back: must return pusher_world # Break It: the three bugs everyone writes, each measured against ground truth.# #1 quat-convention: hand rotate_vector an [x,y,z,w] quaternion where it wants# [w,x,y,z] — the exact seam pusht_env documents. #2 compose-order: build# world_from_pusher in the wrong order. #3 point-vs-frame: read a world point# "in" a frame using the frame itself instead of its inverse. All three are# silent — no error, no exception — and all three are large.tee_long_axis = np.array([0.06, 0.0, 0.0]) # the block's long bar, in its own frameaxis_in_world = np.zeros(3) # ground truth: MuJoCo rotates the bar into worldmujoco.mju_rotVecQuat(axis_in_world, tee_long_axis, world_from_tee.rotation)if args.bug == "quat-convention": wrong_order = world_from_tee.rotation[[1, 2, 3, 0]] # wxyz -> xyzw: the classic silent swap bug_error = float(np.max(np.abs(rotate_vector(wrong_order, tee_long_axis) - axis_in_world)))elif args.bug == "compose-order": tee_from_pusher = Frame(yaw_quaternion(0.7), pusher_in_tee) correct = world_from_tee.compose(tee_from_pusher).translation # world_from_tee . tee_from_pusher swapped = tee_from_pusher.compose(world_from_tee).translation # the reversed, wrong order bug_error = float(np.max(np.abs(correct - swapped)))elif args.bug == "point-vs-frame": # to express a WORLD point in the tee's frame you apply the INVERSE frame; # using world_from_tee forward (transform_point) is the silent direction swap wrong_in_tee = world_from_tee.transform_point(pusher_world) bug_error = float(np.max(np.abs(wrong_in_tee - pusher_in_tee)))else: # --break none: the correct rotation residual, ~machine epsilon bug_error = float(np.max(np.abs(rotate_vector(world_from_tee.rotation, tee_long_axis) - axis_in_world))) err_multiply, err_matrix, err_rotate, err_roundtrip = max_error_against_mujoco(rng, num_samples) if args.rerun: rr.init("zero2robot/ch0.3-transforms", spawn=False) rr.save(str(args.out / "transforms.rrd")) # A frame is legible as its three basis arrows (x red, y green, z blue) at # its origin. Log the world frame (identity) and the tee frame so you can # SEE the rotation compose. Under --break the tee axes point wrong. shown_rotation = world_from_tee.rotation[[1, 2, 3, 0]] if args.bug == "quat-convention" else world_from_tee.rotation for name, frame_rotation, origin in [ ("world/frames/world", np.array([1.0, 0.0, 0.0, 0.0]), np.zeros(3)), ("world/frames/tee", shown_rotation, world_from_tee.translation), ]: axes = quat_to_matrix(frame_rotation).T * 0.1 # rows = rotated x/y/z basis, scaled to scene rr.log(name, rr.Arrows3D(origins=np.tile(origin, (3, 1)), vectors=axes, colors=[[229, 76, 64], [76, 178, 76], [76, 102, 229]])) rr.log("world/objects/pusher", rr.Points3D([pusher_world], colors=[[64, 115, 217]], radii=0.012)) # "Watch compositions": sweep an extra yaw onto the tee frame and trace where # its long-axis tip lands in the world — the tip draws an arc along the timeline. running = world_from_tee for step in range(24): rr.set_time("compose_step", sequence=step) tip_world = running.transform_point(tee_long_axis) rr.log("world/objects/tee_tip", rr.Points3D([tip_world], colors=[[217, 76, 64]], radii=0.01)) running = running.compose(Frame(yaw_quaternion(np.pi / 12.0), np.zeros(3))) metrics = { "seed": args.seed, "samples": num_samples, "break_mode": args.bug, # These four are diagnostic residuals near machine epsilon (~1e-15). They are # rounded like every other metric so the smoke JSON is byte-identical across # machines/BLAS (a raw ~1e-15 float is not portable). The precise values print # to stdout above, where a running learner reads them; rounded, an agreeing op # reads 0.0 and a real disagreement (>1e-6) would still survive the round. "quat_multiply_max_err": round(float(err_multiply), 6), "quat_to_matrix_max_err": round(float(err_matrix), 6), "rotate_vector_max_err": round(float(err_rotate), 6), "frame_roundtrip_max_err": round(float(err_roundtrip), 6), "break_max_err": round(bug_error, 6), # 0 when correct, ~0.08 when a bug is injected "pusher_in_tee": [round(float(v), 6) for v in pusher_in_tee], "pusher_recovered": [round(float(v), 6) for v in pusher_recovered],}(args.out / "metrics.json").write_text(json.dumps(metrics, indent=2, sort_keys=True) + "\n") print(f"checked {num_samples} random quats/points against mju_* — worst disagreement:")print(f" quat_multiply {err_multiply:.2e} quat_to_matrix {err_matrix:.2e} rotate_vector {err_rotate:.2e}")print(f" frame round-trip {err_roundtrip:.2e} (all ~machine epsilon => the from-scratch math is correct)")print(f"pusher in the tee's frame: {np.round(pusher_in_tee, 4)} (recovered world: {np.round(pusher_recovered, 4)})")if args.bug == "none": print(f"break: none — the convention check agrees with MuJoCo to {bug_error:.2e}")else: print(f"break: '{args.bug}' introduces {bug_error:.4f} of error — a wrong answer with no error message")print(f"metrics: {args.out / 'metrics.json'}")if args.rerun: print(f"recording: {args.out / 'transforms.rrd'} — open it with: rerun {args.out / 'transforms.rrd'}")max_error_against_mujoco runs every from-scratch operation on 512 random quaternions and points and records the worst disagreement with the matching mju_* function. The result is the whole argument of the chapter in four numbers: quaternion multiply agrees to 2.2e-16, quat-to-matrix to 5.0e-16, rotate-vector to 1.3e-15, and a full Frame round-trip (transform a point out and back) to 1.8e-15. Those are all machine epsilon — the distance between adjacent doubles. It's not that our code is close to MuJoCo; it's that the two are computing bit-for-bit the same thing, and the tiny residue is just floating-point noise. That's what "you can trust this" means, quantitatively.
Then the concrete question, the one this chapter exists to answer: the block sits at some pose, the pusher is somewhere in the world, and you want the pusher's position as the block sees it — in the block's own frame. That's one inverse and one transform_point, and it's the exact move a reward function makes when it measures error in the block's frame, or a policy makes when its input is relative. With seed 0 the pusher at world [0.20, 0.10, 0] lands at [0.167, 0.031, 0] in the tee frame, and composing straight back recovers [0.20, 0.10, 0] to the last decimal. The round-trip is the point: frames are only useful if going there and back is exact.
The rerun logging draws each frame as its three basis arrows and then, along a short timeline, sweeps an extra 15° yaw onto the tee frame twenty-four times, tracing where the block's long-axis tip lands after each composition. The tip walks an arc — that's "watch compositions" made literal, one compose per frame of the timeline.
Run it
python transforms.py
| cpu-laptop | expected wall-clock on cpu-laptop: ~0.02 min (measured) | measured |
| mps | wall-clock on mps: not yet measured | pending |
| t4 | expected wall-clock on t4: ~0.14 min (measured) | measured |
| 4090 | wall-clock on 4090: not yet measured | pending |
There is no training here and no GPU to wait on — the whole run is pure numpy against MuJoCo's C functions, and it finishes in about a second on a laptop. It prints the four agreement numbers, the pusher's coordinates in the tee frame, and drops a recording at outputs/ch0.3-transforms/transforms.rrd. Open it:
rerun outputs/ch0.3-transforms/transforms.rrd
You're looking at two coordinate frames — world/frames/world at the origin and world/frames/tee turned and offset — each as a red/green/blue arrow triple, plus the pusher as a single dot. Scrub the compose_step timeline and the tee's long-axis tip traces its arc. What healthy looks like: the world frame's arrows lie exactly along the grid axes, and the tee's are rigidly turned by the same angle you set in the code, all three still at right angles to each other. If any pair of arrows stops looking perpendicular, a rotation has stopped being a rotation — that's your first thing to check, and it's exactly what the next section breaks on purpose.
Break it
The three bugs everyone writes are all one flag away. Start with the one chapter 0.1 warned you about:
python transforms.py --break quat-convention --no-rerun
This takes the block's orientation quaternion — correct, in [w, x, y, z] order — and reindexes it to [x, y, z, w] before handing it to rotate_vector, which still reads it as [w, x, y, z]. Exactly the wxyz/xyzw seam from 0.1, except now nobody wrote the swap for you. The four agreement numbers are unchanged — the toolkit is still correct — but the convention check that read 0 a moment ago now reads 0.097. That's the signature to memorize: about ten centimeters of error on a bar only six centimeters long, so the rotated tip misses by more than the bar's own length — a vector that should have turned cleanly now points somewhere else entirely. In the recording (drop the --no-rerun) the tee's arrows visibly point the wrong way while the world frame's stay put. No exception, no warning — a confident wrong answer, which is the most expensive kind.
The other two live behind the same flag. --break compose-order composes world_from_tee and tee_from_pusher in the reversed order and measures the gap: 0.091. The subtle part, and the reason this bug survives code review, is that the two rotations here are both yaws about z, so they commute exactly — the resulting orientations are bit-for-bit identical. It's only the translation that moves, because composition sends the child's origin through the parent's rotation and doing that in the wrong order puts it in the wrong place. "The rotations match, so the transforms match" is a lie your intuition will tell you, and the 9 cm gap is the price. The third bug — point-versus-frame, transforming a point by a frame when you meant to express it in that frame — has its own flag, --break point-vs-frame: it reads the pusher's world position through world_from_tee forward instead of through its inverse, and the pusher lands 14 cm from where it actually is. Same demo, three flags, three silent wrong answers with three different signatures.
Exercises
Three ways to convince yourself you actually hold the math — predict before you run, hunt the convention bug, and write the rotation yourself.
What's next
You can now put a point in any frame and get it back out, exactly — but every pose in this chapter was a number you typed. Real poses come from somewhere: a joystick, a SpaceMouse, your own hand dragging a simulated gripper while MuJoCo records where it went. Next chapter you wire up teleoperation, and the first thing you'll do with the stream of poses coming off the controller is exactly what you built here — compose them into the robot's frame — except now they're arriving thirty times a second and they're yours.