Skip to content

Commit 4ef627c

Browse files
committed
default to visual meshes (#70)
1 parent 5e42e34 commit 4ef627c

File tree

10 files changed

+57
-55
lines changed

10 files changed

+57
-55
lines changed

roboreg/cli/rr_cam_swarm.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -123,9 +123,9 @@ def args_factory() -> argparse.Namespace:
123123
help="Scale the camera resolution by this factor. Reduces memory usage.",
124124
)
125125
parser.add_argument(
126-
"--visual-meshes",
126+
"--collision-meshes",
127127
action="store_true",
128-
help="If set, visual meshes will be used instead of collision meshes.",
128+
help="If set, collision meshes will be used instead of visual meshes.",
129129
)
130130
parser.add_argument(
131131
"--camera-info-file",
@@ -348,7 +348,7 @@ def main() -> None:
348348
urdf_parser=urdf_parser,
349349
root_link_name=args.root_link_name,
350350
end_link_name=args.end_link_name,
351-
visual=args.visual_meshes,
351+
collision=args.collision_meshes,
352352
batch_size=batch_size,
353353
device=device,
354354
target_reduction=args.target_reduction, # reduce mesh vertex count for memory reduction

roboreg/cli/rr_hydra.py

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -74,9 +74,9 @@ def args_factory() -> argparse.Namespace:
7474
help="End link name. If unspecified, the last link with mesh will be used, which may cause errors.",
7575
)
7676
parser.add_argument(
77-
"--visual-meshes",
77+
"--collision-meshes",
7878
action="store_true",
79-
help="If set, visual meshes will be used instead of collision meshes.",
79+
help="If set, collision meshes will be used instead of visual meshes.",
8080
)
8181
parser.add_argument(
8282
"--depth-conversion-factor",
@@ -168,16 +168,16 @@ def main():
168168
root_link_name = args.root_link_name
169169
end_link_name = args.end_link_name
170170
if root_link_name == "":
171-
root_link_name = urdf_parser.link_names_with_meshes(visual=args.visual_meshes)[
172-
0
173-
]
171+
root_link_name = urdf_parser.link_names_with_meshes(
172+
collision=args.collision_meshes
173+
)[0]
174174
rich.print(
175175
f"Root link name not provided. Using the first link with mesh: '{root_link_name}'."
176176
)
177177
if end_link_name == "":
178-
end_link_name = urdf_parser.link_names_with_meshes(visual=args.visual_meshes)[
179-
-1
180-
]
178+
end_link_name = urdf_parser.link_names_with_meshes(
179+
collision=args.collision_meshes
180+
)[-1]
181181
rich.print(
182182
f"End link name not provided. Using the last link with mesh: '{end_link_name}'."
183183
)
@@ -188,7 +188,7 @@ def main():
188188
urdf_parser=urdf_parser,
189189
root_link_name=root_link_name,
190190
end_link_name=end_link_name,
191-
visual=args.visual_meshes,
191+
collision=args.collision_meshes,
192192
batch_size=batch_size,
193193
)
194194

roboreg/cli/rr_mono_dr.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -85,9 +85,9 @@ def args_factory() -> argparse.Namespace:
8585
help="End link name. If unspecified, the last link with mesh will be used, which may cause errors.",
8686
)
8787
parser.add_argument(
88-
"--visual-meshes",
88+
"--collision-meshes",
8989
action="store_true",
90-
help="If set, visual meshes will be used instead of collision meshes.",
90+
help="If set, collision meshes will be used instead of visual meshes.",
9191
)
9292
parser.add_argument(
9393
"--camera-info-file",
@@ -177,7 +177,7 @@ def main() -> None:
177177
xacro_path=args.xacro_path,
178178
root_link_name=args.root_link_name,
179179
end_link_name=args.end_link_name,
180-
visual=args.visual_meshes,
180+
collision=args.collision_meshes,
181181
cameras=camera,
182182
device=device,
183183
)

roboreg/cli/rr_render.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -51,9 +51,9 @@ def args_factory() -> argparse.Namespace:
5151
help="End link name. If unspecified, the last link with mesh will be used, which may cause errors.",
5252
)
5353
parser.add_argument(
54-
"--visual-meshes",
54+
"--collision-meshes",
5555
action="store_true",
56-
help="If set, visual meshes will be used instead of collision meshes.",
56+
help="If set, collision meshes will be used instead of visual meshes.",
5757
)
5858
parser.add_argument(
5959
"--camera-info-file",
@@ -126,7 +126,7 @@ def main():
126126
end_link_name=args.end_link_name,
127127
cameras=camera,
128128
device=device,
129-
visual=args.visual_meshes,
129+
collision=args.collision_meshes,
130130
)
131131
dataset = MonocularDataset(
132132
images_path=args.images_path,

roboreg/cli/rr_stereo_dr.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -85,9 +85,9 @@ def args_factory() -> argparse.Namespace:
8585
help="End link name. If unspecified, the last link with mesh will be used, which may cause errors.",
8686
)
8787
parser.add_argument(
88-
"--visual-meshes",
88+
"--collision-meshes",
8989
action="store_true",
90-
help="If set, visual meshes will be used instead of collision meshes.",
90+
help="If set, collision meshes will be used instead of visual meshes.",
9191
)
9292
parser.add_argument(
9393
"--left-camera-info-file",
@@ -230,7 +230,7 @@ def main() -> None:
230230
end_link_name=args.end_link_name,
231231
cameras=cameras,
232232
device=device,
233-
visual=args.visual_meshes,
233+
collision=args.collision_meshes,
234234
)
235235

236236
# load extrinscis estimate......

roboreg/differentiable/robot.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ def __init__(
1414
urdf_parser: URDFParser,
1515
root_link_name: str,
1616
end_link_name: str,
17-
visual: bool = False,
17+
collision: bool = False,
1818
batch_size: int = 1,
1919
device: torch.device = "cuda",
2020
target_reduction: float = 0.0,
@@ -23,7 +23,7 @@ def __init__(
2323
mesh_paths=urdf_parser.ros_package_mesh_paths(
2424
root_link_name=root_link_name,
2525
end_link_name=end_link_name,
26-
visual=visual,
26+
collision=collision,
2727
),
2828
batch_size=batch_size,
2929
device=device,

roboreg/io.py

Lines changed: 24 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -82,34 +82,34 @@ def chain_link_names(self, root_link_name: str, end_link_name: str) -> List[str]
8282
link_names.append(child_link_name)
8383
return link_names
8484

85-
def link_names_with_meshes(self, visual: bool = False) -> List[str]:
85+
def link_names_with_meshes(self, collision: bool = False) -> List[str]:
8686
r"""Get link names that have meshes.
8787
8888
Args:
89-
visual (bool): If True, get visual meshes, else collision meshes.
89+
collision (bool): If True, get collision meshes, else visual meshes.
9090
9191
Returns:
9292
List[str]: List of link names with meshes.
9393
"""
9494
links = [link.name for link in self._robot.links]
9595
for link in links:
96-
if visual:
97-
if not self._robot.link_map[link].visual:
96+
if collision:
97+
if not self._robot.link_map[link].collision:
9898
links.remove(link)
9999
else:
100-
if not self._robot.link_map[link].collision:
100+
if not self._robot.link_map[link].visual:
101101
links.remove(link)
102102
return links
103103

104104
def raw_mesh_paths(
105-
self, root_link_name: str, end_link_name: str, visual: bool = False
105+
self, root_link_name: str, end_link_name: str, collision: bool = False
106106
) -> Dict[str, str]:
107107
r"""Get the raw mesh paths as specified in URDF.
108108
109109
Args:
110110
root_link_name (str): Root link name.
111111
end_link_name (str): End link name.
112-
visual (bool): If True, get visual mesh paths, else collision mesh paths.
112+
collision (bool): If True, get collision mesh paths, else visual mesh paths.
113113
114114
Returns:
115115
Dict[str,str]: Dictionary of link names and raw mesh paths.
@@ -121,31 +121,33 @@ def raw_mesh_paths(
121121
# lookup paths
122122
for link_name in link_names:
123123
link: urdf_parser_py.urdf.Link = self._robot.link_map[link_name]
124-
if visual:
125-
if link.visual is None:
126-
continue
127-
raw_mesh_paths[link_name] = link.visual.geometry.filename
128-
else:
124+
if collision:
129125
if link.collision is None:
130126
continue
131127
raw_mesh_paths[link_name] = link.collision.geometry.filename
128+
else:
129+
if link.visual is None:
130+
continue
131+
raw_mesh_paths[link_name] = link.visual.geometry.filename
132132
return raw_mesh_paths
133133

134134
def ros_package_mesh_paths(
135-
self, root_link_name: str, end_link_name: str, visual: bool = False
135+
self, root_link_name: str, end_link_name: str, collision: bool = False
136136
) -> Dict[str, str]:
137137
r"""Get the absolute mesh paths by resolving package within ROS.
138138
139139
Args:
140140
root_link_name (str): Root link name.
141141
end_link_name (str): End link name.
142-
visual (bool): If True, get visual mesh paths, else collision mesh paths.
142+
collision (bool): If True, get collision mesh paths, else visual mesh paths.
143143
144144
Returns:
145145
Dict[str,str]: Dictionary of link names and absolute mesh paths.
146146
"""
147147
raw_mesh_paths = self.raw_mesh_paths(
148-
root_link_name=root_link_name, end_link_name=end_link_name, visual=visual
148+
root_link_name=root_link_name,
149+
end_link_name=end_link_name,
150+
collision=collision,
149151
)
150152
from ament_index_python import get_package_share_directory
151153

@@ -163,14 +165,14 @@ def ros_package_mesh_paths(
163165
return ros_package_mesh_paths
164166

165167
def mesh_origins(
166-
self, root_link_name: str, end_link_name: str, visual: bool = False
168+
self, root_link_name: str, end_link_name: str, collision: bool = False
167169
) -> Dict[str, np.ndarray]:
168170
r"""Get mesh origins.
169171
170172
Args:
171173
root_link_name (str): Root link name.
172174
end_link_name (str): End link name.
173-
visual (bool): If True, get visual mesh origins, else collision mesh origins.
175+
collision (bool): If True, get collision mesh origins, else visual mesh origins.
174176
175177
Returns:
176178
Dict[str,np.ndarray]: Dictionary of link names and mesh origins.
@@ -183,14 +185,14 @@ def mesh_origins(
183185
mesh_origins = {}
184186
for link_name in link_names:
185187
link: urdf_parser_py.urdf.Link = self._robot.link_map[link_name]
186-
if visual:
187-
if link.visual is None:
188-
continue
189-
link_origin = link.visual.origin
190-
else:
188+
if collision:
191189
if link.collision is None:
192190
continue
193191
link_origin = link.collision.origin
192+
else:
193+
if link.visual is None:
194+
continue
195+
link_origin = link.visual.origin
194196
origin = transformations.euler_matrix(
195197
link_origin.rpy[0], link_origin.rpy[1], link_origin.rpy[2], "sxyz"
196198
)

roboreg/util/factories.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -38,19 +38,19 @@ def create_robot_scene(
3838
device: torch.device = (
3939
torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu")
4040
),
41-
visual: bool = False,
41+
collision: bool = False,
4242
target_reduction: float = 0.0,
4343
) -> RobotScene:
4444
# create URDF parser
4545
urdf_parser = URDFParser()
4646
urdf_parser.from_ros_xacro(ros_package=ros_package, xacro_path=xacro_path)
4747
if root_link_name == "":
48-
root_link_name = urdf_parser.link_names_with_meshes(visual=visual)[0]
48+
root_link_name = urdf_parser.link_names_with_meshes(collision=collision)[0]
4949
rich.print(
5050
f"Root link name not provided. Using the first link with mesh: '{root_link_name}'."
5151
)
5252
if end_link_name == "":
53-
end_link_name = urdf_parser.link_names_with_meshes(visual=visual)[-1]
53+
end_link_name = urdf_parser.link_names_with_meshes(collision=collision)[-1]
5454
rich.print(
5555
f"End link name not provided. Using the last link with mesh: '{end_link_name}'."
5656
)
@@ -60,7 +60,7 @@ def create_robot_scene(
6060
urdf_parser=urdf_parser,
6161
root_link_name=root_link_name,
6262
end_link_name=end_link_name,
63-
visual=visual,
63+
collision=collision,
6464
batch_size=batch_size,
6565
device=device,
6666
target_reduction=target_reduction,

test/differentiable/test_robot.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,14 +16,14 @@ def test_robot() -> None:
1616
urdf_parser.from_ros_xacro(
1717
ros_package="lbr_description", xacro_path="urdf/med7/med7.xacro"
1818
)
19-
visual = False
19+
collision = True
2020
batch_size = 2
2121
device = "cuda" if torch.cuda.is_available() else "cpu"
2222
robot = Robot(
2323
urdf_parser=urdf_parser,
24-
root_link_name=urdf_parser.link_names_with_meshes(visual=visual)[0],
25-
end_link_name=urdf_parser.link_names_with_meshes(visual=visual)[-1],
26-
visual=visual,
24+
root_link_name=urdf_parser.link_names_with_meshes(collision=collision)[0],
25+
end_link_name=urdf_parser.link_names_with_meshes(collision=collision)[-1],
26+
collision=collision,
2727
batch_size=batch_size,
2828
device=device,
2929
target_reduction=0.0,

test/test_io.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -20,8 +20,8 @@ def test_urdf_parser() -> None:
2020
print(urdf_parser.raw_mesh_paths("lbr_link_0", "lbr_link_ee"))
2121
print(urdf_parser.ros_package_mesh_paths("lbr_link_0", "lbr_link_ee"))
2222
print(urdf_parser.mesh_origins("lbr_link_0", "lbr_link_ee"))
23-
print(urdf_parser.link_names_with_meshes(visual=False))
24-
print(urdf_parser.link_names_with_meshes(visual=True))
23+
print(urdf_parser.link_names_with_meshes(collision=True))
24+
print(urdf_parser.link_names_with_meshes(collision=False))
2525

2626

2727
def test_find_files() -> None:

0 commit comments

Comments
 (0)