Skip to content

Commit 1628fc7

Browse files
ft-labft-lab
authored
Ghost link skipping process for leaf links (#103)
* Enhance error checking for Mimic Joint * Updated benchmarks.md
1 parent 57b16e0 commit 1628fc7

13 files changed

Lines changed: 1383 additions & 407 deletions

‎benchmarks.md‎

Lines changed: 396 additions & 396 deletions
Large diffs are not rendered by default.

‎docs/concept_mapping.md‎

Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -231,6 +231,98 @@ The advantage of this approach is compatibility throughout the USD Ecosystem, pa
231231

232232
The disadvantage is that the kinematic hierarchy remains obfuscated (though it is in URDF as well) and that the child body frame needs to be computed into world (robot) space (in addition to the unit transformation).
233233

234+
#### A link with no elements
235+
236+
A URDF link may omit [inertial](#linkinertial), [visual](#linkvisual), and [collision](#linkcollision) elements entirely.
237+
We call a link that has no inertial, visual, or collision child elements a Ghost Link.
238+
URDF does not assign a special type name to such links; the term is used here for clarity.
239+
240+
In URDF, a Ghost Link looks like this:
241+
242+
```xml
243+
<link name="GhostLink" />
244+
```
245+
246+
##### Physics simplification (Ghost Links on Fixed chains)
247+
248+
When a subtree satisfies the rules below, ghost links in that subtree need not act as separate rigid bodies for simulation.
249+
The converter can omit assigning a rigid body to those links while still keeping their place in the kinematic hierarchy, and it can skip authoring physics joints whose child side would not be a rigid body (including joints that only connect ghost segments on such chains).
250+
251+
That simplification applies only to maximal subtrees for which every link meets all of the following:
252+
253+
1. The link is a Ghost Link (as defined above).
254+
2. The joint from its parent link to this link is Fixed.
255+
3. Every child link satisfies the same three conditions recursively; if a link has several children, each branch is checked separately.
256+
257+
If any child is not a Ghost Link, connects with a non-Fixed joint, or heads into a subtree that fails these rules, links on the path above that point keep rigid bodies and the corresponding physics joints are still emitted.
258+
A different child branch that does satisfy the rules end-to-end may still be simplified on its own, omitting rigid bodies and internal Fixed joints along that branch—for example, a side branch of only Ghost Links on Fixed joints to the tips while another branch continues to articulated, non-Ghost links.
259+
260+
The nested Geometry Xform hierarchy is not removed; only rigid-body assignment and redundant physics joints change.
261+
262+
263+
Consider the following URDF:
264+
265+
```
266+
<robot>
267+
<link name="BaseLink">
268+
...
269+
</link>
270+
<link name="BoxLink">
271+
...
272+
</link>
273+
<link name="GhostLink1" />
274+
<link name="GhostLink2" />
275+
276+
<joint name="joint1" type="fixed">
277+
<origin ... />
278+
<parent link="BaseLink"/>
279+
<child link="BoxLink"/>
280+
</joint>
281+
<joint name="joint2" type="fixed">
282+
<origin ... />
283+
<parent link="BoxLink"/>
284+
<child link="GhostLink1"/>
285+
</joint>
286+
<joint name="joint3" type="fixed">
287+
<origin ... />
288+
<parent link="GhostLink1"/>
289+
<child link="GhostLink2"/>
290+
</joint>
291+
</robot>
292+
```
293+
294+
Converting this to USD using [Nested Bodies](#nested-bodies) might initially produce a layout like the following:
295+
296+
```
297+
/Robot (Xform)
298+
/Geometry (Scope)
299+
/BaseLink (Xform) (with rigid body)
300+
/BoxLink (Xform) (with rigid body)
301+
/GhostLink1 (Xform) (with rigid body)
302+
/GhostLink2 (Xform) (with rigid body)
303+
/Physics (Scope)
304+
/Joint1 (Joint) (parent=BaseLink, child=BoxLink)
305+
/Joint2 (Joint) (parent=BoxLink, child=GhostLink1)
306+
/Joint3 (Joint) (parent=GhostLink1, child=GhostLink2)
307+
```
308+
309+
Because `GhostLink1` and `GhostLink2` are Ghost Links and the chain from `BoxLink` to the leaves is only Fixed joints between Ghost Links, rigid bodies need not be assigned to `GhostLink1` or `GhostLink2`.
310+
The Fixed joints whose child is a Ghost Link without a rigid body (`joint2`, `joint3`) are not authored in Physics.
311+
The USD content can then be simplified to:
312+
313+
```
314+
/Robot (Xform)
315+
/Geometry (Scope)
316+
/BaseLink (Xform) (with rigid body)
317+
/BoxLink (Xform) (with rigid body)
318+
/GhostLink1 (Xform)
319+
/GhostLink2 (Xform)
320+
/Physics (Scope)
321+
/Joint1 (Joint) (parent=BaseLink, child=BoxLink)
322+
```
323+
324+
In Physics, only the joint with `parent=BaseLink` and `child=BoxLink` remains (in addition to any world/root joint policy used elsewhere in this converter).
325+
234326
### link/inertial
235327

236328
The inertial element within a link defines the link’s mass, center of mass, and its central inertia properties. When not defined, it indicates zero mass and zero inertia.

‎tests/data/error_incorrect_mimic_joint.urdf‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<robot name="error_incorrect_mimic_joint">
2+
<link name="BaseLink">
3+
<visual>
4+
<origin rpy="0 0 0" xyz="-2 0 0.5"/>
5+
<geometry>
6+
<box size="1 1 1"/>
7+
</geometry>
8+
</visual>
9+
</link>
10+
11+
<link name="link2">
12+
<visual>
13+
<origin rpy="0 0 0" xyz="0 0 0.5"/>
14+
<geometry>
15+
<sphere radius="0.5"/>
16+
</geometry>
17+
</visual>
18+
</link>
19+
20+
<joint name="JointA" type="fixed">
21+
<parent link="BaseLink"/>
22+
<child link="link2" />
23+
24+
<!-- This is a deliberate mistake. -->
25+
<!-- Specify a non-existent joint. -->
26+
<mimic joint="foo" multiplier="2.0" offset="1.0"/>
27+
</joint>
28+
</robot>

‎tests/data/link_skip_ghost_link.urdf‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
<?xml version="1.0"?>
2+
<robot name="link_skip_ghost_link">
3+
<!-- There is a `Ghost Link` at the end of the link chain. -->
4+
<!-- In this case, the rigid body of the ghost link is deleted, and the joint referencing it is also deleted. -->
5+
<link name="BaseLink">
6+
<visual>
7+
<geometry>
8+
<box size="1.0 1.0 0.2"/>
9+
</geometry>
10+
</visual>
11+
<collision>
12+
<geometry>
13+
<box size="1.0 1.0 0.2"/>
14+
</geometry>
15+
</collision>
16+
</link>
17+
<link name="ghost_link" />
18+
19+
<joint name="joint1" type="fixed">
20+
<origin rpy="0 0 0" xyz="0 0 0.2"/>
21+
<parent link="BaseLink"/>
22+
<child link="ghost_link"/>
23+
</joint>
24+
</robot>

‎tests/data/link_skip_ghost_link_chain.urdf‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
<?xml version="1.0"?>
2+
<robot name="link_skip_ghost_link_chain">
3+
<!-- There is a `Ghost Link` among the chain of links. -->
4+
<!-- In this case, it will be converted with the existing structure intact. -->
5+
<link name="BaseLink">
6+
<visual>
7+
<geometry>
8+
<box size="1.0 1.0 0.2"/>
9+
</geometry>
10+
</visual>
11+
<collision>
12+
<geometry>
13+
<box size="1.0 1.0 0.2"/>
14+
</geometry>
15+
</collision>
16+
</link>
17+
<link name="ghost_link" />
18+
<link name="ghost_link_2" />
19+
<link name="ghost_link_3" />
20+
<link name="link_box">
21+
<visual>
22+
<geometry>
23+
<box size="0.5 0.5 0.5"/>
24+
</geometry>
25+
</visual>
26+
<collision>
27+
<geometry>
28+
<box size="0.5 0.5 0.5"/>
29+
</geometry>
30+
</collision>
31+
</link>
32+
33+
<joint name="joint1" type="fixed">
34+
<origin rpy="0 0 0" xyz="0 0 0.1"/>
35+
<parent link="BaseLink"/>
36+
<child link="ghost_link"/>
37+
</joint>
38+
<joint name="joint2" type="fixed">
39+
<origin rpy="0 0 0" xyz="0 0 0.1"/>
40+
<parent link="ghost_link"/>
41+
<child link="ghost_link_2"/>
42+
</joint>
43+
<joint name="joint3" type="fixed">
44+
<origin rpy="0 0 0" xyz="0 0 0.1"/>
45+
<parent link="ghost_link_2"/>
46+
<child link="ghost_link_3"/>
47+
</joint>
48+
<joint name="joint_box" type="revolute">
49+
<origin rpy="0 0 0" xyz="0 0 0.2"/>
50+
<axis xyz="0 1 0"/>
51+
<limit lower="-1.0" upper="1.0"/>
52+
<parent link="ghost_link_3"/>
53+
<child link="link_box"/>
54+
</joint>
55+
</robot>

‎tests/data/link_skip_ghost_link_chain_branch.urdf‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
<?xml version="1.0"?>
2+
<robot name="link_skip_ghost_link_chain_branch">
3+
<!-- There are `Ghost Links` from the middle to the end of the link chain. -->
4+
<!-- Also, in this URDF, the joints are branched. -->
5+
<!-- In this case, the rigid body of the ghost link is deleted, and the joint referencing it is also deleted. -->
6+
<link name="BaseLink">
7+
<visual>
8+
<geometry>
9+
<box size="1.0 1.0 0.2"/>
10+
</geometry>
11+
</visual>
12+
<collision>
13+
<geometry>
14+
<box size="1.0 1.0 0.2"/>
15+
</geometry>
16+
</collision>
17+
</link>
18+
<link name="ghost_link" />
19+
<link name="ghost_link_2" />
20+
<link name="ghost_link_3" />
21+
<link name="link_box">
22+
<visual>
23+
<geometry>
24+
<box size="0.5 0.5 0.5"/>
25+
</geometry>
26+
</visual>
27+
<collision>
28+
<geometry>
29+
<box size="0.5 0.5 0.5"/>
30+
</geometry>
31+
</collision>
32+
</link>
33+
<link name="ghost_link_4" />
34+
<link name="ghost_link_5" />
35+
36+
<joint name="joint1" type="fixed">
37+
<origin rpy="0 0 0" xyz="0 0 0.1"/>
38+
<parent link="BaseLink"/>
39+
<child link="ghost_link"/>
40+
</joint>
41+
<joint name="joint2" type="fixed">
42+
<origin rpy="0 0 0" xyz="0 0 0.1"/>
43+
<parent link="ghost_link"/>
44+
<child link="ghost_link_2"/>
45+
</joint>
46+
<joint name="joint3" type="fixed">
47+
<origin rpy="0 0 0" xyz="0 0 0.1"/>
48+
<parent link="ghost_link_2"/>
49+
<child link="ghost_link_3"/>
50+
</joint>
51+
<joint name="joint_box" type="revolute">
52+
<origin rpy="0 0 0" xyz="0 0 0.2"/>
53+
<axis xyz="0 1 0"/>
54+
<limit lower="-1.0" upper="1.0"/>
55+
<parent link="ghost_link_3"/>
56+
<child link="link_box"/>
57+
</joint>
58+
<joint name="joint4" type="fixed">
59+
<origin rpy="0 0 0" xyz="0.5 0 0.1"/>
60+
<parent link="ghost_link_2"/>
61+
<child link="ghost_link_4"/>
62+
</joint>
63+
<joint name="joint5" type="fixed">
64+
<origin rpy="0 0 0" xyz="0 0 0.1"/>
65+
<parent link="ghost_link_4"/>
66+
<child link="ghost_link_5"/>
67+
</joint>
68+
</robot>

‎tests/data/link_skip_ghost_link_chain_end.urdf‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
<?xml version="1.0"?>
2+
<robot name="link_skip_ghost_link_chain_end">
3+
<!-- There are `Ghost Links` from the middle to the end of the link chain. -->
4+
<!-- In this case, the rigid body of the ghost link is deleted, and the joint referencing it is also deleted. -->
5+
<link name="BaseLink">
6+
<visual>
7+
<geometry>
8+
<box size="1.0 1.0 0.2"/>
9+
</geometry>
10+
</visual>
11+
<collision>
12+
<geometry>
13+
<box size="1.0 1.0 0.2"/>
14+
</geometry>
15+
</collision>
16+
</link>
17+
<link name="ghost_link" />
18+
<link name="ghost_link_2" />
19+
<link name="ghost_link_3" />
20+
21+
<joint name="joint1" type="fixed">
22+
<origin rpy="0 0 0" xyz="0 0 0.1"/>
23+
<parent link="BaseLink"/>
24+
<child link="ghost_link"/>
25+
</joint>
26+
<joint name="joint2" type="fixed">
27+
<origin rpy="0 0 0" xyz="0 0 0.1"/>
28+
<parent link="ghost_link"/>
29+
<child link="ghost_link_2"/>
30+
</joint>
31+
<joint name="joint3" type="fixed">
32+
<origin rpy="0 0 0" xyz="0 0 0.1"/>
33+
<parent link="ghost_link_2"/>
34+
<child link="ghost_link_3"/>
35+
</joint>
36+
</robot>

‎tests/data/link_skip_ghost_link_with_mimic.urdf‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
<?xml version="1.0"?>
2+
<robot name="link_skip_ghost_link_with_mimic">
3+
<link name="BaseLinkA">
4+
<visual>
5+
<origin rpy="0 0 0" xyz="-0.12 0 0"/>
6+
<geometry>
7+
<box size="0.2 0.2 0.2"/>
8+
</geometry>
9+
</visual>
10+
</link>
11+
<link name="BaseLinkB">
12+
<visual>
13+
<origin rpy="0 0 0" xyz="-0.12 0 0"/>
14+
<geometry>
15+
<box size="0.2 0.2 0.2"/>
16+
</geometry>
17+
</visual>
18+
</link>
19+
20+
<!-- A link referenced by a mimic joint must not be a candidate for ghost link removal. -->
21+
<link name="linkA"/>
22+
<link name="linkB">
23+
<visual>
24+
<origin rpy="0 0 0" xyz="0.27 0 0"/>
25+
<geometry>
26+
<box size="0.5 0.2 0.2"/>
27+
</geometry>
28+
</visual>
29+
</link>
30+
31+
<joint name="joint_base" type="fixed">
32+
<origin rpy="0 0 0" xyz="0 0.4 0"/>
33+
<parent link="BaseLinkA"/>
34+
<child link="BaseLinkB"/>
35+
</joint>
36+
<joint name="jointA" type="fixed">
37+
<origin rpy="0 0 0" xyz="0 0 0"/>
38+
<parent link="BaseLinkA"/>
39+
<child link="linkA"/>
40+
</joint>
41+
<joint name="jointB" type="revolute">
42+
<origin rpy="0 0 0" xyz="0 0 0"/>
43+
<parent link="BaseLinkB"/>
44+
<child link="linkB"/>
45+
<axis xyz="0 1 0"/>
46+
<limit lower="-0.7" upper="0.5"/>
47+
<mimic joint="jointA" multiplier="2.0" offset="0.0"/>
48+
</joint>
49+
</robot>
50+

0 commit comments

Comments
 (0)