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",
+ );
+ });
+});