feat: expose rotation axis info in UrdfJoint (#985)

* feat: expose rotation axis info in UrdfJoint

* Fail hard on invalid axis

---------

Co-authored-by: Ezra Brooks <ezra@brooks.cx>

Author: harsh-pal-robotics

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

src/urdf/UrdfTypes.ts

@@ -30,6 +30,7 @@ export enum UrdfAttrs {
   Geometry = "geometry",
   Material = "material",
   Scale = "scale",
+  Axis = "axis",
 }
 
 export interface UrdfDefaultOptions {

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 = `
+    <joint name="test_joint" type="revolute">
+        <parent link="link1"/>
+        <child link="link2"/>
+        <axis xyz="0 1 0"/>
+    </joint>`;
+    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 = `
+        <joint name="test_joint" type="revolute">
+            <parent link="link1"/>
+            <child link="link2"/>
+        </joint>
+        `;
+    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 = `
+        <joint name="test_joint" type="revolute">
+            <parent link="link1"/>
+            <child link="link2"/>
+            <axis xyz="malformed data"/>
+        </joint>
+        `;
+    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",
+    );
+  });
+});