diff --git a/src/urdf/UrdfJoint.ts b/src/urdf/UrdfJoint.ts index 6ac60bc36..6dea24638 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"; @@ -19,6 +19,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"; @@ -49,5 +54,21 @@ 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) { + 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/src/urdf/UrdfTypes.ts b/src/urdf/UrdfTypes.ts index 22a0ad95e..ce3aedc7a 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..afdcbe9f9 --- /dev/null +++ b/test/urdfjoint.test.js @@ -0,0 +1,68 @@ +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; + 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); + 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; + 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 throw if axis xyz is malformed", () => { + const jointMalformedAxisUrdf = ` + + + + + + `; + const parser = new DOMParser(); + const xml = parser.parseFromString( + jointMalformedAxisUrdf, + "text/xml", + ).documentElement; + 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", + ); + }); +});