Skip to content

Commit 757eeda

Browse files
ferrolhodkozma
ferrolho
and
dkozma
authored
feat: parse <inertial> elements and joint effort/velocity limits (#330)
* Added missing joint limit and link inertial data * Add tests * refactor: address PR review feedback from maintainer --------- Co-authored-by: Dave Kozma <dkozma@radical-ai.com>
1 parent 74c8f75 commit 757eeda

5 files changed

Lines changed: 157 additions & 4 deletions

File tree

‎javascript/README.md‎

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -266,10 +266,10 @@ The type of joint. Can only be the URDF types of joints.
266266
### .limit
267267

268268
```js
269-
.limit : { lower : number, upper : number }
269+
.limit : { lower : number, upper : number, effort : number, velocity : number }
270270
```
271271

272-
An object containing the `lower` and `upper` constraints for the joint.
272+
An object containing the `lower` and `upper` position constraints, as well as the `effort` and `velocity` limits for the joint. All fields default to zero if not specified in the URDF.
273273

274274
### .axis
275275

@@ -357,6 +357,14 @@ name : string
357357

358358
The name of the link.
359359

360+
### .inertial
361+
362+
```js
363+
inertial : { mass : number, origin : { xyz : number[], rpy : number[] }, inertia : { ixx, ixy, ixz, iyy, iyz, izz : number } }
364+
```
365+
366+
The inertial properties of the link parsed from the `<inertial>` element. All fields default to zero if not specified in the URDF.
367+
360368
## URDFRobot
361369

362370
_extends [URDFLink](#URDFLink)_

‎javascript/src/URDFClasses.d.ts‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,21 @@ export class URDFVisual extends URDFBase {
1919

2020
}
2121

22+
export interface URDFInertial {
23+
24+
mass: number;
25+
origin: { xyz: number[], rpy: number[] };
26+
inertia: {
27+
ixx: number; ixy: number; ixz: number;
28+
iyy: number; iyz: number; izz: number;
29+
};
30+
31+
}
32+
2233
export class URDFLink extends URDFBase {
2334

2435
isURDFLink: true;
36+
inertial: URDFInertial;
2537

2638
}
2739

@@ -34,7 +46,7 @@ export class URDFJoint extends URDFBase {
3446
jointType: 'fixed' | 'continuous' | 'revolute' | 'planar' | 'prismatic' | 'floating';
3547
angle: number;
3648
jointValue: number[];
37-
limit: { lower: number, upper: number }; // TODO: add more
49+
limit: { lower: number, upper: number, effort: number, velocity: number };
3850
ignoreLimits: boolean;
3951
mimicJoints: URDFMimicJoint[];
4052

‎javascript/src/URDFClasses.js‎

Lines changed: 26 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,29 @@ class URDFLink extends URDFBase {
6363
this.isURDFLink = true;
6464
this.type = 'URDFLink';
6565

66+
this.inertial = {
67+
mass: 0,
68+
origin: { xyz: [0, 0, 0], rpy: [0, 0, 0] },
69+
inertia: { ixx: 0, ixy: 0, ixz: 0, iyy: 0, iyz: 0, izz: 0 },
70+
};
71+
72+
}
73+
74+
copy(source, recursive) {
75+
76+
super.copy(source, recursive);
77+
78+
this.inertial = {
79+
mass: source.inertial.mass,
80+
origin: {
81+
xyz: [...source.inertial.origin.xyz],
82+
rpy: [...source.inertial.origin.rpy],
83+
},
84+
inertia: { ...source.inertial.inertia },
85+
};
86+
87+
return this;
88+
6689
}
6790

6891
}
@@ -122,7 +145,7 @@ class URDFJoint extends URDFBase {
122145
this.jointValue = null;
123146
this.jointType = 'fixed';
124147
this.axis = new Vector3(1, 0, 0);
125-
this.limit = { lower: 0, upper: 0 };
148+
this.limit = { lower: 0, upper: 0, effort: 0, velocity: 0 };
126149
this.ignoreLimits = false;
127150

128151
this.origPosition = null;
@@ -141,6 +164,8 @@ class URDFJoint extends URDFBase {
141164
this.axis = source.axis.clone();
142165
this.limit.lower = source.limit.lower;
143166
this.limit.upper = source.limit.upper;
167+
this.limit.effort = source.limit.effort;
168+
this.limit.velocity = source.limit.velocity;
144169
this.ignoreLimits = false;
145170

146171
this.jointValue = [...source.jointValue];

‎javascript/src/URDFLoader.js‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -364,6 +364,8 @@ class URDFLoader {
364364

365365
obj.limit.lower = parseFloat(n.getAttribute('lower') || obj.limit.lower);
366366
obj.limit.upper = parseFloat(n.getAttribute('upper') || obj.limit.upper);
367+
obj.limit.effort = parseFloat(n.getAttribute('effort') || obj.limit.effort);
368+
obj.limit.velocity = parseFloat(n.getAttribute('velocity') || obj.limit.velocity);
367369

368370
}
369371
});
@@ -403,6 +405,37 @@ class URDFLoader {
403405
target.urdfName = target.name;
404406
target.urdfNode = link;
405407

408+
// Parse inertial properties
409+
const inertialNode = children.find(n => n.nodeName.toLowerCase() === 'inertial');
410+
if (inertialNode) {
411+
412+
[ ...inertialNode.children ].forEach(n => {
413+
414+
const type = n.nodeName.toLowerCase();
415+
if (type === 'origin') {
416+
417+
target.inertial.origin.xyz = processTuple(n.getAttribute('xyz'));
418+
target.inertial.origin.rpy = processTuple(n.getAttribute('rpy'));
419+
420+
} else if (type === 'mass') {
421+
422+
target.inertial.mass = parseFloat(n.getAttribute('value')) || 0;
423+
424+
} else if (type === 'inertia') {
425+
426+
target.inertial.inertia.ixx = parseFloat(n.getAttribute('ixx')) || 0;
427+
target.inertial.inertia.ixy = parseFloat(n.getAttribute('ixy')) || 0;
428+
target.inertial.inertia.ixz = parseFloat(n.getAttribute('ixz')) || 0;
429+
target.inertial.inertia.iyy = parseFloat(n.getAttribute('iyy')) || 0;
430+
target.inertial.inertia.iyz = parseFloat(n.getAttribute('iyz')) || 0;
431+
target.inertial.inertia.izz = parseFloat(n.getAttribute('izz')) || 0;
432+
433+
}
434+
435+
});
436+
437+
}
438+
406439
if (parseVisual) {
407440

408441
const visualNodes = children.filter(n => n.nodeName.toLowerCase() === 'visual');

‎javascript/test/URDFRobot.test.js‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,81 @@ describe('URDFRobot', () => {
3535
expect(robot.joints.JOINT2.angle).toEqual(2);
3636
});
3737

38+
it('should correctly parse joint efforts and velocities.', () => {
39+
const loader = new URDFLoader();
40+
const robot = loader.parse(`
41+
<robot name="TEST">
42+
<link name="LINK1"/>
43+
<link name="LINK2"/>
44+
<link name="LINK3"/>
45+
<joint name="JOINT1" type="continuous">
46+
<axis xyz="0 0 -1" />
47+
<parent link="LINK1"/>
48+
<child link="LINK2"/>
49+
<limit effort="150" lower="-3.14" upper="3.14" velocity="5.20" />
50+
</joint>
51+
<joint name="JOINT2" type="continuous">
52+
<axis xyz="0 0 -1" />
53+
<parent link="LINK2"/>
54+
<child link="LINK3"/>
55+
</joint>
56+
</robot>
57+
`);
58+
59+
expect(robot.joints.JOINT1.limit.effort).toEqual(150);
60+
expect(robot.joints.JOINT1.limit.lower).toEqual(-3.14);
61+
expect(robot.joints.JOINT1.limit.upper).toEqual(3.14);
62+
expect(robot.joints.JOINT1.limit.velocity).toEqual(5.20);
63+
64+
expect(robot.joints.JOINT2.limit.effort).toEqual(0);
65+
expect(robot.joints.JOINT2.limit.lower).toEqual(0);
66+
expect(robot.joints.JOINT2.limit.upper).toEqual(0);
67+
expect(robot.joints.JOINT2.limit.velocity).toEqual(0);
68+
});
69+
70+
it('should correctly parse joint inertial data.', () => {
71+
const loader = new URDFLoader();
72+
const robot = loader.parse(`
73+
<robot name="TEST">
74+
<link name="LINK1">
75+
<inertial>
76+
<origin rpy="0 0 -1.5707963267948966" xyz="0.14635000035763 0 0"/>
77+
<mass value="2.5076"/>
78+
<inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072" />
79+
</inertial>
80+
</link>
81+
<link name="LINK2"/>
82+
<link name="LINK3"/>
83+
<joint name="JOINT1" type="continuous">
84+
<axis xyz="0 0 -1" />
85+
<parent link="LINK1"/>
86+
<child link="LINK2"/>
87+
</joint>
88+
<joint name="JOINT2" type="continuous">
89+
<axis xyz="0 0 -1" />
90+
<parent link="LINK2"/>
91+
<child link="LINK3"/>
92+
</joint>
93+
</robot>
94+
`);
95+
96+
expect(robot.links.LINK1.inertial.origin.rpy).toEqual([0, 0, -1.5707963267948966]);
97+
expect(robot.links.LINK1.inertial.origin.xyz).toEqual([0.14635000035763, 0, 0]);
98+
expect(robot.links.LINK1.inertial.mass).toEqual(2.5076);
99+
expect(robot.links.LINK1.inertial.inertia.ixx).toEqual(0.00443333156);
100+
expect(robot.links.LINK1.inertial.inertia.iyy).toEqual(0.00443333156);
101+
expect(robot.links.LINK1.inertial.inertia.izz).toEqual(0.0072);
102+
expect(robot.links.LINK1.inertial.inertia.ixy).toEqual(0);
103+
expect(robot.links.LINK1.inertial.inertia.ixz).toEqual(0);
104+
expect(robot.links.LINK1.inertial.inertia.iyz).toEqual(0);
105+
106+
// LINK2 has no <inertial> tag — should still have default values
107+
expect(robot.links.LINK2.inertial.mass).toEqual(0);
108+
expect(robot.links.LINK2.inertial.origin.xyz).toEqual([0, 0, 0]);
109+
expect(robot.links.LINK2.inertial.origin.rpy).toEqual([0, 0, 0]);
110+
expect(robot.links.LINK2.inertial.inertia.ixx).toEqual(0);
111+
});
112+
38113
it('should parse material colors and name.', () => {
39114
const loader = new URDFLoader();
40115
const res = loader.parse(`

0 commit comments

Comments
 (0)