From 20be5ed463f7cea9aed73d78e03d619e01a7f86d Mon Sep 17 00:00:00 2001 From: harshdeepsingh Date: Tue, 4 Nov 2025 12:35:39 +0100 Subject: [PATCH 1/2] feat: expose rotation axis info in UrdfJoint --- src/urdf/UrdfJoint.ts | 21 +++++++++++++++-- src/urdf/UrdfTypes.ts | 1 + test/urdfjoint.test.js | 51 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 71 insertions(+), 2 deletions(-) create mode 100644 test/urdfjoint.test.js diff --git a/src/urdf/UrdfJoint.ts b/src/urdf/UrdfJoint.ts index 3b5c2bca3..44d84e570 100644 --- a/src/urdf/UrdfJoint.ts +++ b/src/urdf/UrdfJoint.ts @@ -4,7 +4,7 @@ */ import { UrdfAttrs, type UrdfDefaultOptions } from './UrdfTypes.js'; -import { Pose } from '../math/index.js'; +import { Pose, Vector3 } from '../math/index.js'; import { parseUrdfOrigin } from './UrdfUtils.js'; import type { Nullable } from '../types/interface-types.js'; @@ -20,7 +20,11 @@ export default class UrdfJoint { minval = NaN; maxval = NaN; origin: Pose = new Pose(); - + axis: Vector3 = new Vector3({ + x: 1, + y: 0, + z: 0 + }); constructor({xml}: UrdfDefaultOptions) { this.name = xml.getAttribute(UrdfAttrs.Name) ?? 'unknown_name'; @@ -47,5 +51,18 @@ export default class UrdfJoint { if (origins.length > 0) { this.origin = parseUrdfOrigin(origins[0]); } + + const axis = xml.getElementsByTagName(UrdfAttrs.Axis); + if (axis.length > 0) { + const xyzValue = axis[0].getAttribute(UrdfAttrs.Xyz)?.split(' '); + if (xyzValue && xyzValue.length === 3) { + const [x, y, z] = xyzValue.map(parseFloat); + this.axis = new Vector3({ + x, + y, + z, + }); + } + } } } diff --git a/src/urdf/UrdfTypes.ts b/src/urdf/UrdfTypes.ts index 20738650b..40106ff6b 100644 --- a/src/urdf/UrdfTypes.ts +++ b/src/urdf/UrdfTypes.ts @@ -30,6 +30,7 @@ export enum UrdfAttrs { Geometry = 'geometry', Material = 'material', Scale = 'scale', + Axis = 'axis', } export interface UrdfDefaultOptions { diff --git a/test/urdfjoint.test.js b/test/urdfjoint.test.js new file mode 100644 index 000000000..8590005e5 --- /dev/null +++ b/test/urdfjoint.test.js @@ -0,0 +1,51 @@ +import { describe, it, expect } from 'vitest'; +import { DOMParser } from '@xmldom/xmldom'; +import UrdfJoint from '../src/urdf/UrdfJoint'; + +describe('UrdfJoint', () => { + it('should parse axis correctly from URDF', () => { + const jointWithAxisUrdf = ` + + + + + `; + const parser = new DOMParser(); + const xml = parser.parseFromString(jointWithAxisUrdf, 'text/xml').documentElement; + const joint = new UrdfJoint({ xml }); + expect(joint.axis.x).toBe(0); + expect(joint.axis.y).toBe(1); + expect(joint.axis.z).toBe(0); + }); + + it('should default axis to (1,0,0) if not present', () => { + const jointNoAxisUrdf = ` + + + + + `; + const parser = new DOMParser(); + const xml = parser.parseFromString(jointNoAxisUrdf, 'text/xml').documentElement; + const joint = new UrdfJoint({ xml }); + expect(joint.axis.x).toBe(1); + expect(joint.axis.y).toBe(0); + expect(joint.axis.z).toBe(0); + }); + + it('should default axis to (1,0,0) if axis xyz is malformed', () => { + const jointMalformedAxisUrdf = ` + + + + + + `; + const parser = new DOMParser(); + const xml = parser.parseFromString(jointMalformedAxisUrdf, 'text/xml').documentElement; + const joint = new UrdfJoint({ xml }); + expect(joint.axis.x).toBe(1); + expect(joint.axis.y).toBe(0); + expect(joint.axis.z).toBe(0); + }); +}); From ba81f0a7cbf534c7aaca4fc840a160327b4af307 Mon Sep 17 00:00:00 2001 From: Ezra Brooks Date: Mon, 10 Nov 2025 12:28:40 -0700 Subject: [PATCH 2/2] Fail hard on invalid axis --- src/urdf/UrdfJoint.ts | 17 ++++++++++------- test/urdfjoint.test.js | 18 +++++++++++++----- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/src/urdf/UrdfJoint.ts b/src/urdf/UrdfJoint.ts index 351774485..6dea24638 100644 --- a/src/urdf/UrdfJoint.ts +++ b/src/urdf/UrdfJoint.ts @@ -58,14 +58,17 @@ export default class UrdfJoint { const axis = xml.getElementsByTagName(UrdfAttrs.Axis); if (axis.length > 0) { const xyzValue = axis[0].getAttribute(UrdfAttrs.Xyz)?.split(" "); - if (xyzValue && xyzValue.length === 3) { - const [x, y, z] = xyzValue.map(parseFloat); - this.axis = new Vector3({ - x, - y, - z, - }); + if (!xyzValue || xyzValue.length !== 3) { + throw new Error( + "If specified, axis must have an xyz value composed of three numbers", + ); } + const [x, y, z] = xyzValue.map(parseFloat); + this.axis = new Vector3({ + x, + y, + z, + }); } } } diff --git a/test/urdfjoint.test.js b/test/urdfjoint.test.js index e5d24f8a4..afdcbe9f9 100644 --- a/test/urdfjoint.test.js +++ b/test/urdfjoint.test.js @@ -15,6 +15,9 @@ describe("UrdfJoint", () => { jointWithAxisUrdf, "text/xml", ).documentElement; + if (!xml) { + throw new Error("Failed to parse XML"); + } const joint = new UrdfJoint({ xml }); expect(joint.axis.x).toBe(0); expect(joint.axis.y).toBe(1); @@ -33,13 +36,16 @@ describe("UrdfJoint", () => { jointNoAxisUrdf, "text/xml", ).documentElement; + if (!xml) { + throw new Error("Failed to parse XML"); + } const joint = new UrdfJoint({ xml }); expect(joint.axis.x).toBe(1); expect(joint.axis.y).toBe(0); expect(joint.axis.z).toBe(0); }); - it("should default axis to (1,0,0) if axis xyz is malformed", () => { + it("should throw if axis xyz is malformed", () => { const jointMalformedAxisUrdf = ` @@ -52,9 +58,11 @@ describe("UrdfJoint", () => { jointMalformedAxisUrdf, "text/xml", ).documentElement; - const joint = new UrdfJoint({ xml }); - expect(joint.axis.x).toBe(1); - expect(joint.axis.y).toBe(0); - expect(joint.axis.z).toBe(0); + if (!xml) { + throw new Error("Failed to parse XML"); + } + expect(() => new UrdfJoint({ xml })).toThrowError( + "If specified, axis must have an xyz value composed of three numbers", + ); }); });