Math fixes

[Micheal Parks (micheal-parks)]

Overview

I asked claude to cross reference the RDK's spatial math with our own in the visualizer. It found a few important edge cases and real bugs that I then asked it to fix.

Additionally, I've added instructions for how to handle lib/math.

The summary:

Fixes

Three ways the visualizer disagreed with the RDK:

• An all-zero orientation vector rendered as a 90° tilt instead of no rotation. Protobuf decodes absent numbers as 0, so any pose setting only a position hit this.
• axis_angles configs were read as orientation vectors — same field names, so they produced a plausible wrong rotation rather than an error. Quaternions spelled the way RDK writes them ({W,X,Y,Z}) produced a NaN pose that made the frame and its children vanish.
• Config geometries RDK accepts but the editor can't write (cylinder, point, mesh, or no type) threw and killed frame loading. Untyped ones now infer their shape like RDK; the rest are skipped with a warning.

Tests

46 new assertions. Each fix verified by reverting it and watching the tests fail.

[changeset-bot]

🦋 Changeset detected

Latest commit: 54de459

The changes in this PR will be included in the next version bump.

This PR includes changesets to release 1 package

Name	Type
@viamrobotics/motion-tools	Patch

Not sure what this means? Click here to learn what changesets are.

Click here if you're a maintainer who wants to add another changeset to this PR

[Micheal Parks (micheal-parks)]

Claude (@claude) review

[claude]

Claude finished Micheal Parks (@micheal-parks)'s task in 5m 53s —— View job

---

Review

• Read CLAUDE.md
• Read changed files and diff
• Review correctness, tests, API design, and patterns
• Post inline comments
• Post final summary

---

Summary

The fixes are correct and well-motivated. The poseFromJson/quatFromJson refactor consolidates what was four independent decoders into one authoritative path, which is exactly the right shape for this kind of wire-format problem. Tests are thorough and each new case is directly tied to a real bug.

---

Bug (test code): quatAppxEqual helper in OrientationVector.spec.ts

The comparison on lines 11–14 is asymmetric:

Math.abs(q1.x) - Math.abs(q2.x) < EPSILON

This is always true when |q2.x| > |q1.x| regardless of how large the difference is. Example: q1.x = 0, q2.x = 0.99 → -0.99 < 0.0001 passes. The correct form is Math.abs(Math.abs(q1.x) - Math.abs(q2.x)) < EPSILON. Same issue for the ov.th comparison in ovAppxEqual (line 25).

The existing test values happen to be structured so the expected has non-zero components wherever the actual should be non-zero — so real regressions get caught today. But a broken conversion that returned a too-large value in a component the expected has as zero would pass silently.

Fix this →

---

Observations (non-blocking)

ov_degrees was silently broken before — the old else branch in setFromFrame applied degToRad to the entire { x, y, z } direction vector, not just th. The new code routes ov_degrees through the direct path that only converts th. Worth noting because it's a silent behavior change for anyone who previously loaded a degrees-based config; those frames will now render correctly.

Zero-axis axis_angles warning path is untested. orientationJson.ts:71–77 has a warn+identity fallback for a zero axis vector. It's covered by documentation but no test verifies the warning fires and the result is finite. spatialJson.spec.ts would be the right home for it.

quatFromJson comment (see inline): the comment on line 43–45 attributes .normalize() to the Go source quaternionJSON.toQuaternion. The actual reason it's needed is that protobuf defaults all missing fields to 0, giving (0,0,0,0) — not a valid rotation. That motivation is clearer than the Go cross-reference.

geometryJson.ts:38 (see inline): the r > 0 guard lets a capsule with r = 0 through. Intentional per the stated design ("already through RDK's gates"), but worth having explicit.

spatialJson.spec.ts already covers euler_angles through the parameterized quarterTurnAboutX table — I initially flagged a gap in pose.spec.ts but spatialJson closes it.

---

The core math is sound. Fix the test helper and it's good to go.

[github-actions]

PR Preview Action v1.8.1
Preview removed because the pull request was closed.
2026-08-12 18:28 UTC

[Devin T. Currie (DTCurrie)]

I came across some of this in the motion plan preview stack. One things I ran into while checking this against the stack (#910-#930). had claude do a quick writeup:

geometryCenterInFrame skips the rotation when the geometry has none

src/lib/math/spatialJson.ts:70

if (quatFromJson(geoOrient, tmpQGeo)) {
	tmpQLocal.copy(tmpQInv).multiply(tmpQGeo)
	center.setFromQuaternion(tmpQLocal)
}

The docblock right above it has the convention correct: the local center is P_frame⁻¹ ∘ P_geometry, because FrameSystem.Transform skips the final transform for a GeometriesInFrame:

We don't want to apply the final transformation when that is taken care of by the geometries [...] A frame is assigned a pose and a geometry and the two are not coupled together.

The rotation half of that is R_frame⁻¹ · R_geometry. When the geometry declares no orientation, R_geometry is identity, so the answer is still R_frame⁻¹, not identity. The guard skips the composition in exactly the case where the geometry contributes nothing and the frame contributes everything, so a link geometry on a rotated frame gets drawn unrotated in the link's local space. quatFromJson already writes identity when it finds nothing, so dropping the guard costs nothing:

// Unconditional: an absent orientation means identity in the parent's frame, which is still
// R_frame⁻¹ once expressed locally. `quatFromJson` writes identity when it finds nothing.
quatFromJson(geoOrient, tmpQGeo)
tmpQLocal.copy(tmpQInv).multiply(tmpQGeo)
center.setFromQuaternion(tmpQLocal)

This came in with #936 and this PR relocates the file, so it is not a regression here, just a good moment to catch it.

undoes the frame rotation at src/lib/math/__tests__/spatialJson.spec.ts:138 is the test positioned to catch it: it passes undefined for geoOrient against a frame rotated 90°, which is precisely the broken case, but asserts only x/y/z. Adding an assertion on the resulting rotation there closes it.