feat: expose rotation axis info in UrdfJoint

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

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,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 = `
+    <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;
+    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;
+        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 = `
+        <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;
+        const joint = new UrdfJoint({ xml });
+        expect(joint.axis.x).toBe(1);
+        expect(joint.axis.y).toBe(0);
+        expect(joint.axis.z).toBe(0);
+    });
+});