diff --git a/.changeset/olive-pugs-shave.md b/.changeset/olive-pugs-shave.md new file mode 100644 index 000000000..92272ed05 --- /dev/null +++ b/.changeset/olive-pugs-shave.md @@ -0,0 +1,5 @@ +--- +'@viamrobotics/motion-tools': patch +--- + +Fix NaN poses from orientation JSON that omits its zero fields diff --git a/src/lib/math/__tests__/pose.spec.ts b/src/lib/math/__tests__/pose.spec.ts index d5afc0e34..dee39aa21 100644 --- a/src/lib/math/__tests__/pose.spec.ts +++ b/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 diff --git a/src/lib/math/__tests__/spatialJson.spec.ts b/src/lib/math/__tests__/spatialJson.spec.ts index ef81dff44..53b93bbca 100644 --- a/src/lib/math/__tests__/spatialJson.spec.ts +++ b/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, { diff --git a/src/lib/math/orientationJson.ts b/src/lib/math/orientationJson.ts index 3e8e46643..8136ed80f 100644 --- a/src/lib/math/orientationJson.ts +++ b/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> -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> +type OvJson = Partial> /** * 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