Fix NaN poses from orientation JSON that omits its zero fields

.changeset/olive-pugs-shave.md

@@ -0,0 +1,5 @@
+---
+'@viamrobotics/motion-tools': patch
+---
+
+Fix NaN poses from orientation JSON that omits its zero fields

src/lib/math/__tests__/pose.spec.ts

@@ -423,6 +423,26 @@ describe('setFromFrame', () => {
 		expect(pose.toQuaternion().angleTo(quarterTurnAboutX)).toBeCloseTo(0, 6)
 	})
 
+	/**
+	 * A hand-written config leaves its zero fields out, and Go's unmarshal reads
+	 * them back as zero. A NaN here reaches `toMatrix4`, which drops the frame and
+	 * everything parented to it out of the scene.
+	 */
+	it.each([
+		['euler_angles', { roll: Math.PI / 2 }],
+		['axis_angles', { x: 1, th: Math.PI / 2 }],
+		['ov_degrees', { y: -1, th: 90 }],
+		['ov_radians', { y: -1, th: Math.PI / 2 }],
+		['quaternion', { X: Math.SQRT1_2, W: Math.SQRT1_2 }],
+	])('reads a %s value that omits its zero fields', (type, value) => {
+		const orientation = { type, value } as Frame['orientation']
+		const pose = new Pose().setFromFrame({ orientation })
+
+		expect(pose.isFinite()).toBe(true)
+		expect(pose.toQuaternion().angleTo(quarterTurnAboutX)).toBeCloseTo(0, 6)
+		expect(pose.toMatrix4().elements.every((element) => Number.isFinite(element))).toBe(true)
+	})
+
 	/**
 	 * `R4AA` tags its fields `th/x/y/z`, the names both orientation-vector
 	 * encodings use, so a misread yields a plausible wrong rotation. Hence

src/lib/math/__tests__/spatialJson.spec.ts

@@ -100,6 +100,41 @@ describe('poseFromJson', () => {
 		expect(warn).not.toHaveBeenCalled()
 	})
 
+	/**
+	 * The same quarter turn again, with every zero-valued field left out. Go's
+	 * unmarshal fills those with zero, so RDK reads each of these as the rotation
+	 * above — a hand-written machine config spells them this way.
+	 */
+	const partialQuarterTurnAboutX: [string, RawOrientation][] = [
+		['ov_degrees', { type: 'ov_degrees', value: { y: -1, th: 90 } }],
+		['ov_radians', { type: 'ov_radians', value: { y: -1, th: MathUtils.degToRad(90) } }],
+		['quaternion', { type: 'quaternion', value: { X: Math.SQRT1_2, W: Math.SQRT1_2 } }],
+		['euler_angles', { type: 'euler_angles', value: { roll: Math.PI / 2 } }],
+		['axis_angles', { type: 'axis_angles', value: { x: 1, th: Math.PI / 2 } }],
+	]
+
+	it.each(partialQuarterTurnAboutX)(
+		'reads the same turn from %s with its zero fields omitted',
+		(_label, orientation) => {
+			const pose = poseFromJson(undefined, orientation)
+
+			expect(pose.isFinite()).toBe(true)
+			expectVectorClose(rotates(pose, new Vector3(0, 1, 0)), [0, 0, 1])
+			expect(warn).not.toHaveBeenCalled()
+		}
+	)
+
+	/** A value with no fields at all is RDK's zero orientation, not a NaN one. */
+	it.each(['ov_degrees', 'ov_radians', 'quaternion', 'euler_angles'])(
+		'reads an empty %s value as identity',
+		(type) => {
+			const pose = poseFromJson({ X: 1, Y: 2, Z: 3 }, { type, value: {} })
+
+			expect(pose.isFinite()).toBe(true)
+			expectVectorClose(rotates(pose, new Vector3(0, 1, 0)), [0, 1, 0])
+		}
+	)
+
 	it('agrees with three.js on a quaternion round trip', () => {
 		const source = new Quaternion().setFromAxisAngle(new Vector3(1, 2, 3).normalize(), 0.7)
 		const pose = poseFromJson(undefined, {

src/lib/math/orientationJson.ts

@@ -20,8 +20,9 @@ const tmpOv = new OrientationVector()
  * frame editor writes `{ w, x, y, z }`, and Go's unmarshal accepts both.
  */
 type QuatJson = Partial<Record<'W' | 'X' | 'Y' | 'Z' | 'w' | 'x' | 'y' | 'z', number>>
-type EulerJson = { roll: number; pitch: number; yaw: number }
-type OvJson = { x: number; y: number; z: number; th: number }
+/** Partial like `QuatJson`: Go's unmarshal leaves an absent field at its zero value. */
+type EulerJson = Partial<Record<'roll' | 'pitch' | 'yaw', number>>
+type OvJson = Partial<Record<'x' | 'y' | 'z' | 'th', number>>
 
 /**
  * Writes `out` and reports whether it holds a real rotation; false leaves it identity. Callers that
@@ -49,7 +50,7 @@ export const quatFromJson = (orientation: RawOrientation | undefined, out: Quate
 			case 'euler_angles': {
 				const v = value as EulerJson
 				// RDK uses Tait–Bryan Z-Y′-X″; Three.js defaults to 'XYZ'.
-				out.setFromEuler(tmpE.set(v.roll, v.pitch, v.yaw, 'ZYX'))
+				out.setFromEuler(tmpE.set(v.roll ?? 0, v.pitch ?? 0, v.yaw ?? 0, 'ZYX'))
 				return true
 			}
 			case 'ov_radians': {
@@ -67,7 +68,7 @@ export const quatFromJson = (orientation: RawOrientation | undefined, out: Quate
 				const v = value as OvJson
 				// RDK normalizes inside `R4AA.ToQuat`, and `setFromAxisAngle` assumes a
 				// unit axis. RDK panics on a zero one rather than defining it.
-				tmpAxis.set(v.x, v.y, v.z)
+				tmpAxis.set(v.x ?? 0, v.y ?? 0, v.z ?? 0)
 				if (tmpAxis.lengthSq() > 0) {
 					out.setFromAxisAngle(tmpAxis.normalize(), v.th ?? 0)
 					return true