Skip to content

Add joint limit and inertial link data #299

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
dkozma wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Add joint limit and inertial link data #299

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
dedf65d
Added missing joint limit and link inertial data
dkozma Nov 13, 2024
9858447
Add `meshPath` to track mesh loading, update UMD files
dkozma Nov 21, 2024
5bee231
Add tests
dkozma Nov 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 12 additions & 1 deletion javascript/src/URDFClasses.d.ts
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,20 +4,31 @@ export interface URDFCollider extends Object3D {

isURDFCollider: true;
urdfNode: Element | null;
meshPath: string | null;

}

export interface URDFVisual extends Object3D {

isURDFVisual: true;
urdfNode: Element | null;
meshPath: string | null;

}

interface URDFInertial {

origin: { xyz: Vector3 | null, rpy: Vector3 | null } | null;
mass: Number | null;
inertia: { ixx: Number, ixy: Number, ixz: Number, iyy: Number, iyz: Number, izz: Number } | null;

}

export interface URDFLink extends Object3D {

isURDFLink: true;
urdfNode: Element | null;
inertial: URDFInertial | null;

}

Expand All @@ -30,7 +41,7 @@ export interface URDFJoint extends Object3D {
jointType: 'fixed' | 'continuous' | 'revolute' | 'planar' | 'prismatic' | 'floating';
angle: Number;
jointValue: Number[];
limit: { lower: Number, upper: Number }; // TODO: add more
limit: { lower: Number, upper: Number, effort: Number | null, velocity: Number | null };
ignoreLimits: Boolean;
mimicJoints: URDFMimicJoint[];

Expand Down
8 changes: 7 additions & 1 deletion javascript/src/URDFClasses.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ class URDFCollider extends URDFBase {
super(...args);
this.isURDFCollider = true;
this.type = 'URDFCollider';
this.meshPath = null;

}

Expand All @@ -50,6 +51,7 @@ class URDFVisual extends URDFBase {
super(...args);
this.isURDFVisual = true;
this.type = 'URDFVisual';
this.meshPath = null;

}

Expand All @@ -63,6 +65,8 @@ class URDFLink extends URDFBase {
this.isURDFLink = true;
this.type = 'URDFLink';

this.inertial = null;

}

}
Expand Down Expand Up @@ -122,7 +126,7 @@ class URDFJoint extends URDFBase {
this.jointValue = null;
this.jointType = 'fixed';
this.axis = new Vector3(1, 0, 0);
this.limit = { lower: 0, upper: 0 };
this.limit = { lower: 0, upper: 0, effort: null, velocity: null };
this.ignoreLimits = false;

this.origPosition = null;
Expand All @@ -141,6 +145,8 @@ class URDFJoint extends URDFBase {
this.axis = source.axis.clone();
this.limit.lower = source.limit.lower;
this.limit.upper = source.limit.upper;
this.limit.effort = source.limit.effort;
this.limit.velocity = source.limit.velocity;
this.ignoreLimits = false;

this.jointValue = [...source.jointValue];
Expand Down
45 changes: 43 additions & 2 deletions javascript/src/URDFLoader.js
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
import * as THREE from 'three';
import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
import { ColladaLoader } from 'three/examples/jsm/loaders/ColladaLoader.js';
import { URDFRobot, URDFJoint, URDFLink, URDFCollider, URDFVisual, URDFMimicJoint } from './URDFClasses.js';
import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
import { URDFCollider, URDFJoint, URDFLink, URDFMimicJoint, URDFRobot, URDFVisual } from './URDFClasses.js';

/*
Reference coordinate frames for THREE.js and ROS.
Expand Down Expand Up @@ -37,6 +37,14 @@ function processTuple(val) {

}

// take a vector "x y z" and process it into
// a THREE.Vector3
function processVector(val) {

return new THREE.Vector3(...processTuple(val));

}

// applies a rotation a threejs object in URDF order
function applyRotation(obj, rpy, additive = false) {

Expand Down Expand Up @@ -371,6 +379,8 @@ class URDFLoader {

obj.limit.lower = parseFloat(n.getAttribute('lower') || obj.limit.lower);
obj.limit.upper = parseFloat(n.getAttribute('upper') || obj.limit.upper);
obj.limit.effort = parseFloat(n.getAttribute('effort') || obj.limit.effort);
obj.limit.velocity = parseFloat(n.getAttribute('velocity') || obj.limit.velocity);

}
});
Expand Down Expand Up @@ -410,6 +420,36 @@ class URDFLoader {
target.urdfName = target.name;
target.urdfNode = link;

// Extract the attributes
children.forEach(n => {

const type = n.nodeName.toLowerCase();
if (type === 'inertial') {
const subNodes = [ ...n.children ];
const origin = subNodes.find(sn => sn.nodeName.toLowerCase() === 'origin');
const xyz = origin ? origin.getAttribute('xyz') : null;
const rpy = origin ? origin.getAttribute('rpy') : null;
const mass = subNodes.find(sn => sn.nodeName.toLowerCase() === 'mass');
const inertia = subNodes.find(sn => sn.nodeName.toLowerCase() === 'inertia');
target.inertial = origin || mass || inertia ? {
origin: xyz || rpy ? {
xyz: xyz ? processVector(xyz) : null,
rpy: rpy ? processVector(rpy) : null,
} : null,
mass: mass ? parseFloat(mass.getAttribute('value') || 0) : null,
inertia: inertia ? {
ixx: parseFloat(inertia.getAttribute('ixx') || 0),
ixy: parseFloat(inertia.getAttribute('ixy') || 0),
ixz: parseFloat(inertia.getAttribute('ixz') || 0),
iyy: parseFloat(inertia.getAttribute('iyy') || 0),
iyz: parseFloat(inertia.getAttribute('iyz') || 0),
izz: parseFloat(inertia.getAttribute('izz') || 0),
} : null,
} : null;

}
});

if (parseVisual) {

const visualNodes = children.filter(n => n.nodeName.toLowerCase() === 'visual');
Expand Down Expand Up @@ -544,6 +584,7 @@ class URDFLoader {
// file path is null if a package directory is not provided.
if (filePath !== null) {

group.meshPath = filePath;
const scaleAttr = n.children[0].getAttribute('scale');
if (scaleAttr) {

Expand Down
72 changes: 72 additions & 0 deletions javascript/test/URDFRobot.test.js
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
import { JSDOM } from 'jsdom';
import { Vector3 } from 'three';
import URDFLoader from '../src/URDFLoader.js';

const jsdom = new JSDOM();
Expand Down Expand Up @@ -35,6 +36,77 @@ describe('URDFRobot', () => {
expect(robot.joints.JOINT2.angle).toEqual(2);
});

it('should correctly parse joint efforts and velocities.', () => {
const loader = new URDFLoader();
const robot = loader.parse(`
<robot name="TEST">
<link name="LINK1"/>
<link name="LINK2"/>
<link name="LINK3"/>
<joint name="JOINT1" type="continuous">
<axis xyz="0 0 -1" />
<parent link="LINK1"/>
<child link="LINK2"/>
<limit effort="150" lower="-3.14" upper="3.14" velocity="5.20" />
</joint>
<joint name="JOINT2" type="continuous">
<axis xyz="0 0 -1" />
<parent link="LINK2"/>
<child link="LINK3"/>
</joint>
</robot>
`);

expect(robot.joints.JOINT1.limit.effort).toEqual(150);
expect(robot.joints.JOINT1.limit.lower).toEqual(-3.14);
expect(robot.joints.JOINT1.limit.upper).toEqual(3.14);
expect(robot.joints.JOINT1.limit.velocity).toEqual(5.20);

expect(robot.joints.JOINT2.limit.effort).toEqual(null);
expect(robot.joints.JOINT2.limit.lower).toEqual(0);
expect(robot.joints.JOINT2.limit.upper).toEqual(0);
expect(robot.joints.JOINT2.limit.velocity).toEqual(null);
});

it('should correctly parse joint inertial data.', () => {
const loader = new URDFLoader();
const robot = loader.parse(`
<robot name="TEST">
<link name="LINK1">
<inertial>
<origin rpy="0 0 -1.5707963267948966" xyz="0.14635000035763 0 0"/>
<mass value="2.5076"/>
<inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072" />
</inertial>
</link>
<link name="LINK2"/>
<link name="LINK3"/>
<joint name="JOINT1" type="continuous">
<axis xyz="0 0 -1" />
<parent link="LINK1"/>
<child link="LINK2"/>
</joint>
<joint name="JOINT2" type="continuous">
<axis xyz="0 0 -1" />
<parent link="LINK2"/>
<child link="LINK3"/>
</joint>
</robot>
`);

expect(robot.links.LINK1.inertial.origin.rpy).toEqual(new Vector3(0, 0, -1.5707963267948966));
expect(robot.links.LINK1.inertial.origin.xyz).toEqual(new Vector3(0.14635000035763, 0, 0));
expect(robot.links.LINK1.inertial.mass).toEqual(2.5076);
expect(robot.links.LINK1.inertial.inertia.ixx).toEqual(0.00443333156);
expect(robot.links.LINK1.inertial.inertia.iyy).toEqual(0.00443333156);
expect(robot.links.LINK1.inertial.inertia.izz).toEqual(0.0072);
expect(robot.links.LINK1.inertial.inertia.ixy).toEqual(0);
expect(robot.links.LINK1.inertial.inertia.ixz).toEqual(0);
expect(robot.links.LINK1.inertial.inertia.iyz).toEqual(0);

expect(robot.links.LINK2.inertial).toEqual(null);
});

it('should parse material colors and name.', () => {
const loader = new URDFLoader();
const res = loader.parse(`
Expand Down
Loading