Skip to content

Fix 3D rotation composition with quaternions - #1247

Merged
rickwierenga merged 3 commits into
PyLabRobot:mainfrom
fly1d:codex/fix-rotation-composition
Sep 10, 2026
Merged

Fix 3D rotation composition with quaternions#1247
rickwierenga merged 3 commits into
PyLabRobot:mainfrom
fly1d:codex/fix-rotation-composition

Conversation

@fly1d

@fly1d fly1d commented Sep 9, 2026

Copy link
Copy Markdown
Contributor

Fixes #1165.

Summary

  • Store a normalized quaternion inside Rotation while keeping the public x, y, and z accessors and serialized shape.
  • Compose Rotation values with the Hamilton product instead of adding Euler components.
  • Apply Resource.rotate() increments in the parent-coordinate frame while preserving the existing Rotation instance and [0, 360) component normalization.
  • Cover non-commuting rotations, parent-child composition, component assignment, gimbal-lock representations, and serialization round trips.

Why

Adding Euler components does not compose general 3D rotations. For example, applying an x-axis increment after a 90-degree z rotation produces a different matrix when the angles are added. Quaternion multiplication preserves the requested order without introducing a matrix-to-Euler conversion into each composition.

Euler angles remain part of the public and serialized API. Equivalent Euler representations are therefore selected close to the previous component-wise values, with an explicit singular case at pitch = +/-90 degrees.

Verification

  • .venv/bin/python -m pytest -q: 2983 passed, 36 skipped, 759 subtests passed
  • BIN=.venv/bin/ make lint
  • BIN=.venv/bin/ make format-check
  • BIN=.venv/bin/ make typecheck: 808 source files checked
  • 10,000 deterministic composition and serialization checks, including near-singular angles: maximum matrix errors 1.110e-15 and 2.753e-12

No hardware was connected or driven.

AI assistance: OpenAI Codex assisted with implementation and test validation.

@BioCam

BioCam commented Sep 9, 2026

Copy link
Copy Markdown
Collaborator

@fly1d nice fix - this is the right way to do it. A heads-up on how it interacts with #1248, which I just merged.

This makes Rotation.get_rotation_matrix about 17x faster (5.0 → 0.30 µs) but Rotation.__add__ about 24x slower (0.16 → 3.88 µs).
Before #1248 the slowdown outweighed the speed-up, because get_absolute_location called __add__ a quadratic number of times, so the whole thing got slower.
Those calls are gone now, so against current main it comes out roughly neutral: 3-4% slower on an unrotated deck, no measurable difference on rotated chains.

The small slowdown is .x/.y/.z becoming properties (27 → 49 ns per read) - get_absolute_location reads all three once per chain level to decide whether a matrix is needed at all.
Comparing the stored quaternion against the identity instead would claw it back.

Comment thread pylabrobot/resources/rotation.py
Store a normalized quaternion behind the existing Euler accessors so ordered rotation composition no longer adds Euler components. Apply Resource.rotate increments in the parent coordinate frame while preserving the serialized x/y/z shape.

Add regressions for non-commuting rotations, hierarchy composition, normalization, gimbal-lock representations, and serialization.

Assisted-by: OpenAI Codex
Use the stored identity quaternion in the absolute-location hot path and document why the matrix helper remains as an independent test reference.

Assisted-by: OpenAI Codex
@fly1d
fly1d force-pushed the codex/fix-rotation-composition branch from 1d8bd1c to 6330eb5 Compare September 10, 2026 01:55
@fly1d

fly1d commented Sep 10, 2026

Copy link
Copy Markdown
Contributor Author

Thanks. I rebased onto current main and applied the suggested hot-path fix in 6330eb50: get_absolute_location now compares the stored quaternion directly with the identity quaternion. The 88 focused tests, Ruff, formatting, and mypy on the changed modules pass.

@rickwierenga
rickwierenga merged commit ee70bef into PyLabRobot:main Sep 10, 2026
22 checks passed
BioCam added a commit to BioCam/pylabrobot that referenced this pull request Sep 10, 2026
PyLabRobot#1247 replaced `rotate`'s three Euler additions with `rotation._prepend`, which composes by
quaternion product rather than by adding components. This branch wraps those same three lines with
the pivot, so the two met head on.

Resolved by keeping the pivot and taking upstream's composition: the reference point's travel is
measured from the rotation matrices before and after, so it does not care how the orientation was
arrived at - and under a real composition the "after" matrix is now correct where Euler addition
would have been wrong for a turn about anything but one axis.

`rotate_to` also composes rather than assigns, so it was worth checking it still lands where it is
told. Swept 108 starting orientations against four targets: every one arrives exactly.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
BioCam added a commit to BioCam/pylabrobot that referenced this pull request Sep 10, 2026
`rotate_to` computed a per-axis Euler delta and handed it to `rotate`, which composes by quaternion
since PyLabRobot#1247. Those are not the same operation. Only Z survived it, and by accident of the
convention: Euler is Rz*Ry*Rx and `_prepend` pre-multiplies, so a pure-Z delta adds cleanly where X
and Y have rotations applied after them. 73 of 108 cases landed off target on X, 42 on Y.

`_turn` takes the orientation a caller wants and leaves `reference` where it was, so `rotate`
composes and `rotate_to` assigns, and neither carries a copy of the pivot arithmetic. It writes the
angles in place rather than binding a new `Rotation`, keeping the instance and the normalisation
`_prepend` established.

Tests: `rotate_to` lands on every axis from every starting orientation, angles stay in [0, 360) on
all three axes, and a pivot inside a turned parent holds - the last of those covering the frame
correction, which nothing reached before. Checked by mutation: five mutations of these lines, five
caught, where two survived beforehand.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
BioCam added a commit to BioCam/pylabrobot that referenced this pull request Sep 10, 2026
Takes PyLabRobot#1247 and resolves `rotate` the way the primitives branch does: the pivot stays and the
composition is upstream's. `resource.py` and `resource_tests.py` are that branch's, so the two do
not drift while it is in review.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Resource.rotate implementation is wrong in general

3 participants