Skip to content

Commit ecc341c

Browse files
committed
fix: handle URDF world-anchor joints and link/joint name collisions
Two failures that fell out of testing against a wider URDF corpus (Bullet's minitaur, the so101 from onshape-to-robot, drake's acrobot): - Many URDFs anchor the base link to the world frame with a fixed joint whose parent is a synthetic `world` link that's never defined. BB has no concept of a world frame — the topology root is the robot. Drop such joints in a pre-pass, leaving the child unparented so the root-finder picks it up. A warning records what was dropped. - URDF has separate namespaces for link and joint names, but BB requires global uniqueness across both. Several real URDFs use the same string for a link and a joint (so101's `gripper`, minitaur's `motor_back_rightL_link`). Rename colliding joints with a `_joint` suffix (incrementing if that's also taken) and rewrite `<mimic>` source references to match. With these fixes, eleven public URDFs now compile cleanly: Allegro Hand, ANYmal B, Bullet cartpole, Drake acrobot/pendulum, KUKA iiwa, Bullet minitaur, Franka Panda, SO-101, UR5, and WidowX (xacro-free portions).
1 parent 3159a56 commit ecc341c

6 files changed

Lines changed: 140 additions & 1 deletion

File tree

lib/bb/urdf/importer.ex

Lines changed: 76 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,12 @@ if Code.ensure_loaded?(Sourceror) do
5050
@spec to_quoted(Parser.robot(), module) ::
5151
{:ok, Macro.t(), [String.t()]} | {:error, term}
5252
def to_quoted(robot, module_name) when is_atom(module_name) do
53-
robot = dedupe_material_names(robot)
53+
robot =
54+
robot
55+
|> drop_world_anchor_joints()
56+
|> dedupe_joint_names()
57+
|> dedupe_material_names()
58+
5459
links_by_name = Map.new(robot.links, &{&1.name, &1})
5560
joints_by_parent = Enum.group_by(robot.joints, & &1.parent)
5661
child_links = MapSet.new(robot.joints, & &1.child)
@@ -76,6 +81,76 @@ if Code.ensure_loaded?(Sourceror) do
7681
end
7782
end
7883

84+
# URDF commonly anchors a robot to the world with a fixed joint whose
85+
# parent is a synthetic link (`world`, `map`, etc.) that's never defined
86+
# in the file. BB has no concept of a world frame — the topology root is
87+
# the robot. Drop joints whose parent link isn't defined; their child
88+
# then becomes an unparented link, which the root-finder picks up
89+
# naturally.
90+
defp drop_world_anchor_joints(robot) do
91+
link_names = MapSet.new(robot.links, & &1.name)
92+
93+
{joints, dropped} =
94+
Enum.split_with(robot.joints, &MapSet.member?(link_names, &1.parent))
95+
96+
warnings =
97+
Enum.map(dropped, fn joint ->
98+
"dropped joint #{inspect(joint.name)}: parent link #{inspect(joint.parent)} is not defined (URDF world anchor?)"
99+
end)
100+
101+
%{robot | joints: joints, warnings: robot.warnings ++ warnings}
102+
end
103+
104+
# URDF has separate namespaces for link and joint names; BB requires
105+
# global uniqueness across both. Rename any joint whose name collides
106+
# with a link (or with another joint) and rewrite `<mimic>` source
107+
# references to match.
108+
defp dedupe_joint_names(robot) do
109+
link_names = MapSet.new(robot.links, & &1.name)
110+
{joints, _used, renames} = rename_colliding_joints(robot.joints, link_names)
111+
joints = rewrite_mimic_sources(joints, renames)
112+
%{robot | joints: joints}
113+
end
114+
115+
defp rename_colliding_joints(joints, link_names) do
116+
Enum.reduce(joints, {[], link_names, %{}}, fn joint, {acc, used, renames} ->
117+
if MapSet.member?(used, joint.name) do
118+
new_name = unique_name(joint.name <> "_joint", used)
119+
120+
{[%{joint | name: new_name} | acc], MapSet.put(used, new_name),
121+
Map.put(renames, joint.name, new_name)}
122+
else
123+
{[joint | acc], MapSet.put(used, joint.name), renames}
124+
end
125+
end)
126+
|> then(fn {joints, used, renames} -> {Enum.reverse(joints), used, renames} end)
127+
end
128+
129+
defp unique_name(base, used) do
130+
if MapSet.member?(used, base) do
131+
Enum.find_value(Stream.iterate(2, &(&1 + 1)), &numbered_candidate(base, &1, used))
132+
else
133+
base
134+
end
135+
end
136+
137+
defp numbered_candidate(base, n, used) do
138+
candidate = "#{base}_#{n}"
139+
if MapSet.member?(used, candidate), do: nil, else: candidate
140+
end
141+
142+
defp rewrite_mimic_sources(joints, renames) when map_size(renames) == 0, do: joints
143+
144+
defp rewrite_mimic_sources(joints, renames) do
145+
Enum.map(joints, fn
146+
%{mimic: %{joint: source} = mimic} = joint ->
147+
%{joint | mimic: %{mimic | joint: Map.get(renames, source, source)}}
148+
149+
joint ->
150+
joint
151+
end)
152+
end
153+
79154
# URDF lets many visuals reference a single named material; BB's DSL
80155
# requires globally-unique entity names. Keep the URDF name on the first
81156
# visual that uses each material and strip it on later occurrences (BB

test/bb/urdf/importer_test.exs

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -155,6 +155,26 @@ defmodule BB.Urdf.ImporterTest do
155155
assert source =~ ~r/mesh do\s*filename "package:\/\/meshes\/arm\.stl"\s*scale 1\.0/
156156
end
157157

158+
test "drops joints whose parent link is undefined (URDF world anchor)" do
159+
{source, warnings} = import_fixture("world_anchor.urdf", MyApp.WorldGen)
160+
161+
refute source =~ "joint :world_anchor"
162+
assert source =~ "joint :shoulder"
163+
assert Enum.any?(warnings, &(&1 =~ "dropped joint \"world_anchor\""))
164+
assert [{MyApp.WorldGen, _}] = Code.compile_string(source)
165+
end
166+
167+
test "renames joints that collide with link names and rewrites mimic refs" do
168+
{source, _warnings} = import_fixture("name_collision.urdf", MyApp.CollideGen)
169+
170+
# `gripper` is a link; the homonymous joint gets renamed.
171+
assert source =~ "link :gripper"
172+
assert source =~ "joint :gripper_joint"
173+
# The mimic source on `finger_joint` points to the renamed joint.
174+
assert source =~ "source: :gripper_joint"
175+
assert [{MyApp.CollideGen, _}] = Code.compile_string(source)
176+
end
177+
158178
test "errors out on multiple root links" do
159179
xml = """
160180
<?xml version="1.0"?>
Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version="1.0"?>
2+
<robot name="collision_bot">
3+
<link name="base"/>
4+
<link name="gripper"/>
5+
<link name="finger"/>
6+
7+
<joint name="gripper" type="revolute">
8+
<parent link="base"/>
9+
<child link="gripper"/>
10+
<axis xyz="0 0 1"/>
11+
<limit lower="-1" upper="1" effort="1" velocity="1"/>
12+
</joint>
13+
14+
<joint name="finger_joint" type="prismatic">
15+
<parent link="gripper"/>
16+
<child link="finger"/>
17+
<axis xyz="1 0 0"/>
18+
<limit lower="0" upper="0.05" effort="1" velocity="0.1"/>
19+
<mimic joint="gripper" multiplier="0.5"/>
20+
</joint>
21+
</robot>
Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
SPDX-FileCopyrightText: 2026 James Harton
2+
3+
SPDX-License-Identifier: Apache-2.0
Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
<?xml version="1.0"?>
2+
<robot name="anchored_bot">
3+
<link name="base_link"/>
4+
<link name="arm"/>
5+
6+
<joint name="world_anchor" type="fixed">
7+
<parent link="world"/>
8+
<child link="base_link"/>
9+
</joint>
10+
11+
<joint name="shoulder" type="revolute">
12+
<parent link="base_link"/>
13+
<child link="arm"/>
14+
<axis xyz="0 0 1"/>
15+
<limit lower="-1" upper="1" effort="1" velocity="1"/>
16+
</joint>
17+
</robot>
Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
SPDX-FileCopyrightText: 2026 James Harton
2+
3+
SPDX-License-Identifier: Apache-2.0

0 commit comments

Comments
 (0)