From 7bb512f61b0229a5f342aa68e3869efd479d6dda Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 31 Jan 2026 14:40:51 +0000 Subject: [PATCH 1/9] cleaned up the URDFParser API --- roboreg/differentiable/robot.py | 2 +- roboreg/io/parsers.py | 111 +++++++++++++++---------- test/differentiable/test_kinematics.py | 4 +- test/differentiable/test_rendering.py | 6 +- test/io/test_parsers.py | 2 +- test/test_hydra_icp.py | 4 +- test/util/test_viz.py | 2 +- 7 files changed, 77 insertions(+), 54 deletions(-) diff --git a/roboreg/differentiable/robot.py b/roboreg/differentiable/robot.py index 29ec277..06ce7d4 100644 --- a/roboreg/differentiable/robot.py +++ b/roboreg/differentiable/robot.py @@ -37,7 +37,7 @@ def from_urdf_parser( from roboreg.io import apply_mesh_origins, load_meshes, simplify_meshes # parse data from URDF - mesh_paths = urdf_parser.ros_package_mesh_paths( + mesh_paths = urdf_parser.mesh_paths_from_ros_registry( root_link_name=root_link_name, end_link_name=end_link_name, collision=collision, diff --git a/roboreg/io/parsers.py b/roboreg/io/parsers.py index dbce4a1..b915ac4 100644 --- a/roboreg/io/parsers.py +++ b/roboreg/io/parsers.py @@ -1,4 +1,3 @@ -import os from pathlib import Path from typing import Dict, List, Tuple, Union @@ -17,8 +16,8 @@ def __init__(self, urdf: str) -> None: self._robot = urdf_parser_py.urdf.Robot.from_xml_string(urdf) @classmethod - def from_file(cls, path: Union[Path, str]) -> None: - r"""Instantiate URDF parser path to URDF file. + def from_urdf_file(cls, path: Union[Path, str]) -> None: + r"""Instantiate URDF parser via path to URDF file. Args: path (Union[Path, str]): Path to URDF file. @@ -35,13 +34,16 @@ def from_file(cls, path: Union[Path, str]) -> None: return cls(urdf=urdf) @classmethod - def from_ros_xacro(cls, ros_package: str, xacro_path: str) -> None: + def from_ros_xacro(cls, ros_package: str, xacro_path: Union[Path, str]) -> None: r"""Instantiate URDF parser from ROS xacro file. Args: ros_package (str): Internally finds the path to ros_package. - xacro_path (str): Path to xacro file relative to ros_package. + xacro_path (Union[Path,str]): Path to xacro file relative to ros_package. """ + xacro_path = Path(xacro_path) + if not xacro_path.suffix == ".xacro": + raise ValueError(f"Xacro file {xacro_path} must have .xacro extension.") return cls( urdf=cls._urdf_from_ros_xacro( ros_package=ros_package, xacro_path=xacro_path @@ -49,23 +51,36 @@ def from_ros_xacro(cls, ros_package: str, xacro_path: str) -> None: ) @staticmethod - def _urdf_from_ros_xacro(ros_package: str, xacro_path: str) -> str: - r"""Convert ROS xacro file to URDF. + def resolve_uris_via_ros_registry(uris: Dict[str, str]) -> Dict[str, Path]: + r"""Resolve the URI-style package:// prefix using the ament_index_python ROS registry. Args: - ros_package (str): Internally finds the path to ros_package. - xacro_path (str): Path to xacro file relative to ros_package. + uris (Dict[str,str]): Dictionary of link names and URI-style mesh paths, prefixed with package://. Returns: - str: URDF string. + Dict[str,Path]: Dictionary of link names and absolute mesh paths. """ - - import xacro from ament_index_python import get_package_share_directory - return xacro.process( - os.path.join(get_package_share_directory(ros_package), xacro_path) - ) + mesh_paths = {} + for link_name in uris.keys(): + uri = uris[link_name] + if uri.startswith("package://"): + mesh_path = Path(uri.removeprefix("package://")) + if len(mesh_path.parts) < 2: + raise ValueError( + f"Invalid package path {mesh_path} for link {link_name}." + ) + mesh_paths[link_name] = Path( + get_package_share_directory(mesh_path.parts[0]) + ) / Path(*mesh_path.parts[1:]) + else: + raise ValueError("Case unhandled.") + if len(mesh_paths) != len(uris): + raise RuntimeError("Some mesh paths could not be resolved.") + if not all([path.exists() for path in mesh_paths.values()]): + raise FileNotFoundError("Some resolved mesh paths do not exist.") + return mesh_paths def chain_link_names(self, root_link_name: str, end_link_name: str) -> List[str]: r"""Get link names in chain from root to end link. @@ -110,10 +125,10 @@ def link_names_with_meshes(self, collision: bool = False) -> List[str]: links.remove(link) return links - def raw_mesh_paths( + def mesh_uris( self, root_link_name: str, end_link_name: str, collision: bool = False ) -> Dict[str, str]: - r"""Get the raw mesh paths as specified in URDF. + r"""Get the mesh paths as specified in URDF. These paths may be relative or have a package:// prefix. Args: root_link_name (str): Root link name. @@ -121,29 +136,29 @@ def raw_mesh_paths( collision (bool): If True, get collision mesh paths, else visual mesh paths. Returns: - Dict[str,str]: Dictionary of link names and raw mesh paths. + Dict[str,str]: Dictionary of link names and mesh URIs. """ link_names = self.chain_link_names( root_link_name=root_link_name, end_link_name=end_link_name ) - raw_mesh_paths = {} + paths = {} # lookup paths for link_name in link_names: link: urdf_parser_py.urdf.Link = self._robot.link_map[link_name] if collision: if link.collision is None: continue - raw_mesh_paths[link_name] = link.collision.geometry.filename + paths[link_name] = Path(link.collision.geometry.filename) else: if link.visual is None: continue - raw_mesh_paths[link_name] = link.visual.geometry.filename - return raw_mesh_paths + paths[link_name] = Path(link.visual.geometry.filename) + return paths - def ros_package_mesh_paths( + def mesh_paths_from_ros_registry( self, root_link_name: str, end_link_name: str, collision: bool = False - ) -> Dict[str, str]: - r"""Get the absolute mesh paths by resolving package within ROS. + ) -> Dict[str, Path]: + r"""Get the absolute mesh paths by resolving the package:// prefix using ROS ament_index_python. Args: root_link_name (str): Root link name. @@ -151,27 +166,15 @@ def ros_package_mesh_paths( collision (bool): If True, get collision mesh paths, else visual mesh paths. Returns: - Dict[str,str]: Dictionary of link names and absolute mesh paths. + Dict[str,Path]: Dictionary of link names and absolute mesh paths. """ - raw_mesh_paths = self.raw_mesh_paths( - root_link_name=root_link_name, - end_link_name=end_link_name, - collision=collision, + return URDFParser.resolve_uris_via_ros_registry( + uris=self.mesh_uris( + root_link_name=root_link_name, + end_link_name=end_link_name, + collision=collision, + ) ) - from ament_index_python import get_package_share_directory - - ros_package_mesh_paths = {} - for link_name in raw_mesh_paths.keys(): - raw_mesh_path = raw_mesh_paths[link_name] - if raw_mesh_path.startswith("package://"): - raw_mesh_path = raw_mesh_path.replace("package://", "") - package, relative_mesh_path = raw_mesh_path.split("/", 1) - ros_package_mesh_paths[link_name] = os.path.join( - get_package_share_directory(package), relative_mesh_path - ) - else: - raise ValueError("Case unhandled.") - return ros_package_mesh_paths def mesh_origins( self, root_link_name: str, end_link_name: str, collision: bool = False @@ -209,6 +212,26 @@ def mesh_origins( mesh_origins[link_name] = origin return mesh_origins + @staticmethod + def _urdf_from_ros_xacro(ros_package: str, xacro_path: Path) -> str: + r"""Convert ROS xacro file to URDF. + + Args: + ros_package (str): Internally finds the path to ros_package. + xacro_path (Path): Path to xacro file relative to ros_package. + + Returns: + str: URDF string. + """ + + import xacro + from ament_index_python import get_package_share_directory + + path = Path(get_package_share_directory(ros_package)) / xacro_path + if not path.exists(): + raise FileNotFoundError(f"Xacro file {path} does not exist.") + return xacro.process(path) + def _verify_links_in_chain(self, root_link_name: str, end_link_name: str) -> None: if not self._robot: raise RuntimeError("Robot not initialized.") diff --git a/test/differentiable/test_kinematics.py b/test/differentiable/test_kinematics.py index 60d70f9..183f341 100644 --- a/test/differentiable/test_kinematics.py +++ b/test/differentiable/test_kinematics.py @@ -57,7 +57,7 @@ def test_torch_kinematics_on_mesh( meshes = TorchMeshContainer( meshes=apply_mesh_origins( meshes=load_meshes( - urdf_parser.ros_package_mesh_paths( + urdf_parser.mesh_paths_from_ros_registry( root_link_name=root_link_name, end_link_name=end_link_name ) ), @@ -120,7 +120,7 @@ def test_diff_kinematics() -> None: meshes = TorchMeshContainer( meshes=apply_mesh_origins( meshes=load_meshes( - urdf_parser.ros_package_mesh_paths( + urdf_parser.mesh_paths_from_ros_registry( root_link_name=root_link_name, end_link_name=end_link_name ) ), diff --git a/test/differentiable/test_rendering.py b/test/differentiable/test_rendering.py index 963682e..c0aad10 100644 --- a/test/differentiable/test_rendering.py +++ b/test/differentiable/test_rendering.py @@ -68,7 +68,7 @@ def __init__( # instantiate meshes self.meshes = rrd.TorchMeshContainer( - mesh_paths=self.urdf_parser.ros_package_mesh_paths( + mesh_paths=self.urdf_parser.mesh_paths_from_ros_registry( self.root_link_name, self.end_link_name ), batch_size=self.batch_size, @@ -269,7 +269,7 @@ def test_multi_config_single_view_rendering() -> None: # overwrite meshes with batch size test_rendering.meshes = rrd.TorchMeshContainer( - mesh_paths=test_rendering.urdf_parser.ros_package_mesh_paths( + mesh_paths=test_rendering.urdf_parser.mesh_paths_from_ros_registry( test_rendering.root_link_name, test_rendering.end_link_name ), batch_size=q.shape[0], @@ -331,7 +331,7 @@ def test_multi_config_single_view_pose_optimization() -> None: # overwrite meshes with batch size test_rendering.meshes = rrd.TorchMeshContainer( - mesh_paths=test_rendering.urdf_parser.ros_package_mesh_paths( + mesh_paths=test_rendering.urdf_parser.mesh_paths_from_ros_registry( test_rendering.root_link_name, test_rendering.end_link_name ), batch_size=q.shape[0], diff --git a/test/io/test_parsers.py b/test/io/test_parsers.py index 80cf01a..ac8de9c 100644 --- a/test/io/test_parsers.py +++ b/test/io/test_parsers.py @@ -18,7 +18,7 @@ def test_urdf_parser() -> None: urdf_parser = URDFParser.from_ros_xacro("lbr_description", "urdf/med7/med7.xacro") print(urdf_parser.chain_link_names("lbr_link_0", "lbr_link_ee")) print(urdf_parser.raw_mesh_paths("lbr_link_0", "lbr_link_ee")) - print(urdf_parser.ros_package_mesh_paths("lbr_link_0", "lbr_link_ee")) + print(urdf_parser.mesh_paths_from_ros_registry("lbr_link_0", "lbr_link_ee")) print(urdf_parser.mesh_origins("lbr_link_0", "lbr_link_ee")) print(urdf_parser.link_names_with_meshes(collision=True)) print(urdf_parser.link_names_with_meshes(collision=False)) diff --git a/test/test_hydra_icp.py b/test/test_hydra_icp.py index 3fdee58..e4b4ffb 100644 --- a/test/test_hydra_icp.py +++ b/test/test_hydra_icp.py @@ -143,7 +143,7 @@ def test_hydra_icp(): meshes = TorchMeshContainer( meshes=apply_mesh_origins( meshes=load_meshes( - urdf_parser.ros_package_mesh_paths( + urdf_parser.mesh_paths_from_ros_registry( root_link_name=root_link_name, end_link_name=end_link_name ) ), @@ -274,7 +274,7 @@ def test_hydra_robust_icp() -> None: meshes = TorchMeshContainer( meshes=apply_mesh_origins( meshes=load_meshes( - urdf_parser.ros_package_mesh_paths( + urdf_parser.mesh_paths_from_ros_registry( root_link_name=root_link_name, end_link_name=end_link_name ) ), diff --git a/test/util/test_viz.py b/test/util/test_viz.py index 1ff0955..2e34b65 100644 --- a/test/util/test_viz.py +++ b/test/util/test_viz.py @@ -93,7 +93,7 @@ def test_visualize_robot(): meshes = rrd.TorchMeshContainer( meshes=apply_mesh_origins( meshes=load_meshes( - urdf_parser.ros_package_mesh_paths( + urdf_parser.mesh_paths_from_ros_registry( root_link_name=root_link_name, end_link_name=end_link_name ) ), From bfb90e926a1c3839ff71b77299118eb76eaceff7 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 1 Feb 2026 13:42:10 +0000 Subject: [PATCH 2/9] added ros-free urdf to sample data --- test/assets/lbr_med7/mesh/link_0.stl | 3 --- test/assets/lbr_med7/mesh/link_1.stl | 3 --- test/assets/lbr_med7/mesh/link_2.stl | 3 --- test/assets/lbr_med7/mesh/link_3.stl | 3 --- test/assets/lbr_med7/mesh/link_4.stl | 3 --- test/assets/lbr_med7/mesh/link_5.stl | 3 --- test/assets/lbr_med7/mesh/link_6.stl | 3 --- test/assets/lbr_med7/mesh/link_7.stl | 3 --- test/assets/lbr_med7_r800/description/med7_r800.urdf | 3 +++ .../lbr_med7_r800/description/meshes/collision/link_0.stl | 3 +++ .../lbr_med7_r800/description/meshes/collision/link_1.stl | 3 +++ .../lbr_med7_r800/description/meshes/collision/link_2.stl | 3 +++ .../lbr_med7_r800/description/meshes/collision/link_3.stl | 3 +++ .../lbr_med7_r800/description/meshes/collision/link_4.stl | 3 +++ .../lbr_med7_r800/description/meshes/collision/link_5.stl | 3 +++ .../lbr_med7_r800/description/meshes/collision/link_6.stl | 3 +++ .../lbr_med7_r800/description/meshes/collision/link_7.stl | 3 +++ .../description/meshes/visual}/link_0.dae | 0 .../description/meshes/visual}/link_1.dae | 0 .../description/meshes/visual}/link_2.dae | 0 .../description/meshes/visual}/link_3.dae | 0 .../description/meshes/visual}/link_4.dae | 0 .../description/meshes/visual}/link_5.dae | 0 .../description/meshes/visual}/link_6.dae | 0 .../description/meshes/visual}/link_7.dae | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_cam_swarm.npy | 0 .../assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_dr.npy | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_hydra.npy | 0 .../zed2i => lbr_med7_r800/samples}/HT_hydra_robust.npy | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_left_dr.npy | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_right_dr.npy | 0 .../zed2i => lbr_med7_r800/samples}/HT_right_to_left.npy | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/depth_0.npy | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/depth_1.npy | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/depth_2.npy | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/depth_3.npy | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/depth_4.npy | 0 .../zed2i => lbr_med7_r800/samples}/joint_states_0.npy | 0 .../zed2i => lbr_med7_r800/samples}/joint_states_1.npy | 0 .../zed2i => lbr_med7_r800/samples}/joint_states_2.npy | 0 .../zed2i => lbr_med7_r800/samples}/joint_states_3.npy | 0 .../zed2i => lbr_med7_r800/samples}/joint_states_4.npy | 0 .../zed2i => lbr_med7_r800/samples}/left_camera_info.yaml | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_0.png | 0 .../zed2i => lbr_med7_r800/samples}/left_image_0_samples.csv | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_1.png | 0 .../zed2i => lbr_med7_r800/samples}/left_image_1_samples.csv | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_2.png | 0 .../zed2i => lbr_med7_r800/samples}/left_image_2_samples.csv | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_3.png | 0 .../zed2i => lbr_med7_r800/samples}/left_image_3_samples.csv | 0 .../{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_4.png | 0 .../zed2i => lbr_med7_r800/samples}/left_image_4_samples.csv | 0 .../zed2i => lbr_med7_r800/samples}/mask_sam2_left_image_0.png | 0 .../zed2i => lbr_med7_r800/samples}/mask_sam2_left_image_1.png | 0 .../zed2i => lbr_med7_r800/samples}/mask_sam2_left_image_2.png | 0 .../zed2i => lbr_med7_r800/samples}/mask_sam2_left_image_3.png | 0 .../zed2i => lbr_med7_r800/samples}/mask_sam2_left_image_4.png | 0 .../samples}/mask_sam2_right_image_0.png | 0 .../samples}/mask_sam2_right_image_1.png | 0 .../samples}/mask_sam2_right_image_2.png | 0 .../samples}/mask_sam2_right_image_3.png | 0 .../samples}/mask_sam2_right_image_4.png | 0 .../zed2i => lbr_med7_r800/samples}/right_camera_info.yaml | 0 .../zed2i => lbr_med7_r800/samples}/right_image_0.png | 0 .../zed2i => lbr_med7_r800/samples}/right_image_0_samples.csv | 0 .../zed2i => lbr_med7_r800/samples}/right_image_1.png | 0 .../zed2i => lbr_med7_r800/samples}/right_image_1_samples.csv | 0 .../zed2i => lbr_med7_r800/samples}/right_image_2.png | 0 .../zed2i => lbr_med7_r800/samples}/right_image_2_samples.csv | 0 .../zed2i => lbr_med7_r800/samples}/right_image_3.png | 0 .../zed2i => lbr_med7_r800/samples}/right_image_3_samples.csv | 0 .../zed2i => lbr_med7_r800/samples}/right_image_4.png | 0 .../zed2i => lbr_med7_r800/samples}/right_image_4_samples.csv | 0 test/assets/{xarm/mesh => xarm_7/description}/link_base.STL | 0 .../{xarm/realsense => xarm_7/samples}/HT_hydra_robust.npy | 0 .../assets/{xarm/realsense => xarm_7/samples}/camera_info.yaml | 0 test/assets/{xarm/realsense => xarm_7/samples}/depth_0.npy | 0 test/assets/{xarm/realsense => xarm_7/samples}/depth_1.npy | 0 test/assets/{xarm/realsense => xarm_7/samples}/depth_2.npy | 0 test/assets/{xarm/realsense => xarm_7/samples}/depth_3.npy | 0 test/assets/{xarm/realsense => xarm_7/samples}/depth_4.npy | 0 test/assets/{xarm/realsense => xarm_7/samples}/depth_5.npy | 0 test/assets/{xarm/realsense => xarm_7/samples}/depth_6.npy | 0 test/assets/{xarm/realsense => xarm_7/samples}/depth_7.npy | 0 test/assets/{xarm/realsense => xarm_7/samples}/depth_8.npy | 0 test/assets/{xarm/realsense => xarm_7/samples}/depth_9.npy | 0 test/assets/{xarm/realsense => xarm_7/samples}/img_0.png | 0 test/assets/{xarm/realsense => xarm_7/samples}/img_1.png | 0 test/assets/{xarm/realsense => xarm_7/samples}/img_2.png | 0 test/assets/{xarm/realsense => xarm_7/samples}/img_3.png | 0 test/assets/{xarm/realsense => xarm_7/samples}/img_4.png | 0 test/assets/{xarm/realsense => xarm_7/samples}/img_5.png | 0 test/assets/{xarm/realsense => xarm_7/samples}/img_6.png | 0 test/assets/{xarm/realsense => xarm_7/samples}/img_7.png | 0 test/assets/{xarm/realsense => xarm_7/samples}/img_8.png | 0 test/assets/{xarm/realsense => xarm_7/samples}/img_9.png | 0 .../{xarm/realsense => xarm_7/samples}/joint_state_0.npy | 0 .../{xarm/realsense => xarm_7/samples}/joint_state_1.npy | 0 .../{xarm/realsense => xarm_7/samples}/joint_state_2.npy | 0 .../{xarm/realsense => xarm_7/samples}/joint_state_3.npy | 0 .../{xarm/realsense => xarm_7/samples}/joint_state_4.npy | 0 .../{xarm/realsense => xarm_7/samples}/joint_state_5.npy | 0 .../{xarm/realsense => xarm_7/samples}/joint_state_6.npy | 0 .../{xarm/realsense => xarm_7/samples}/joint_state_7.npy | 0 .../{xarm/realsense => xarm_7/samples}/joint_state_8.npy | 0 .../{xarm/realsense => xarm_7/samples}/joint_state_9.npy | 0 .../{xarm/realsense => xarm_7/samples}/mask_sam2_img_0.png | 0 .../{xarm/realsense => xarm_7/samples}/mask_sam2_img_1.png | 0 .../{xarm/realsense => xarm_7/samples}/mask_sam2_img_2.png | 0 .../{xarm/realsense => xarm_7/samples}/mask_sam2_img_3.png | 0 .../{xarm/realsense => xarm_7/samples}/mask_sam2_img_4.png | 0 .../{xarm/realsense => xarm_7/samples}/mask_sam2_img_5.png | 0 .../{xarm/realsense => xarm_7/samples}/mask_sam2_img_6.png | 0 .../{xarm/realsense => xarm_7/samples}/mask_sam2_img_7.png | 0 .../{xarm/realsense => xarm_7/samples}/mask_sam2_img_8.png | 0 .../{xarm/realsense => xarm_7/samples}/mask_sam2_img_9.png | 0 .../{xarm/realsense => xarm_7/samples}/overlay_img_0.png | 0 .../{xarm/realsense => xarm_7/samples}/overlay_img_1.png | 0 .../{xarm/realsense => xarm_7/samples}/overlay_img_2.png | 0 .../{xarm/realsense => xarm_7/samples}/overlay_img_3.png | 0 .../{xarm/realsense => xarm_7/samples}/overlay_img_4.png | 0 .../{xarm/realsense => xarm_7/samples}/overlay_img_5.png | 0 .../{xarm/realsense => xarm_7/samples}/overlay_img_6.png | 0 .../{xarm/realsense => xarm_7/samples}/overlay_img_7.png | 0 .../{xarm/realsense => xarm_7/samples}/overlay_img_8.png | 0 .../{xarm/realsense => xarm_7/samples}/overlay_img_9.png | 0 127 files changed, 27 insertions(+), 24 deletions(-) delete mode 100644 test/assets/lbr_med7/mesh/link_0.stl delete mode 100644 test/assets/lbr_med7/mesh/link_1.stl delete mode 100644 test/assets/lbr_med7/mesh/link_2.stl delete mode 100644 test/assets/lbr_med7/mesh/link_3.stl delete mode 100644 test/assets/lbr_med7/mesh/link_4.stl delete mode 100644 test/assets/lbr_med7/mesh/link_5.stl delete mode 100644 test/assets/lbr_med7/mesh/link_6.stl delete mode 100644 test/assets/lbr_med7/mesh/link_7.stl create mode 100644 test/assets/lbr_med7_r800/description/med7_r800.urdf create mode 100644 test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl create mode 100644 test/assets/lbr_med7_r800/description/meshes/collision/link_1.stl create mode 100644 test/assets/lbr_med7_r800/description/meshes/collision/link_2.stl create mode 100644 test/assets/lbr_med7_r800/description/meshes/collision/link_3.stl create mode 100644 test/assets/lbr_med7_r800/description/meshes/collision/link_4.stl create mode 100644 test/assets/lbr_med7_r800/description/meshes/collision/link_5.stl create mode 100644 test/assets/lbr_med7_r800/description/meshes/collision/link_6.stl create mode 100644 test/assets/lbr_med7_r800/description/meshes/collision/link_7.stl rename test/assets/{lbr_med7/mesh => lbr_med7_r800/description/meshes/visual}/link_0.dae (100%) rename test/assets/{lbr_med7/mesh => lbr_med7_r800/description/meshes/visual}/link_1.dae (100%) rename test/assets/{lbr_med7/mesh => lbr_med7_r800/description/meshes/visual}/link_2.dae (100%) rename test/assets/{lbr_med7/mesh => lbr_med7_r800/description/meshes/visual}/link_3.dae (100%) rename test/assets/{lbr_med7/mesh => lbr_med7_r800/description/meshes/visual}/link_4.dae (100%) rename test/assets/{lbr_med7/mesh => lbr_med7_r800/description/meshes/visual}/link_5.dae (100%) rename test/assets/{lbr_med7/mesh => lbr_med7_r800/description/meshes/visual}/link_6.dae (100%) rename test/assets/{lbr_med7/mesh => lbr_med7_r800/description/meshes/visual}/link_7.dae (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_cam_swarm.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_dr.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_hydra.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_hydra_robust.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_left_dr.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_right_dr.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/HT_right_to_left.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/depth_0.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/depth_1.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/depth_2.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/depth_3.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/depth_4.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/joint_states_0.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/joint_states_1.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/joint_states_2.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/joint_states_3.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/joint_states_4.npy (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_camera_info.yaml (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_0.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_0_samples.csv (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_1.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_1_samples.csv (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_2.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_2_samples.csv (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_3.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_3_samples.csv (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_4.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/left_image_4_samples.csv (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/mask_sam2_left_image_0.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/mask_sam2_left_image_1.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/mask_sam2_left_image_2.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/mask_sam2_left_image_3.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/mask_sam2_left_image_4.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/mask_sam2_right_image_0.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/mask_sam2_right_image_1.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/mask_sam2_right_image_2.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/mask_sam2_right_image_3.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/mask_sam2_right_image_4.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_camera_info.yaml (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_image_0.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_image_0_samples.csv (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_image_1.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_image_1_samples.csv (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_image_2.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_image_2_samples.csv (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_image_3.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_image_3_samples.csv (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_image_4.png (100%) rename test/assets/{lbr_med7/zed2i => lbr_med7_r800/samples}/right_image_4_samples.csv (100%) rename test/assets/{xarm/mesh => xarm_7/description}/link_base.STL (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/HT_hydra_robust.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/camera_info.yaml (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/depth_0.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/depth_1.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/depth_2.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/depth_3.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/depth_4.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/depth_5.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/depth_6.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/depth_7.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/depth_8.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/depth_9.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/img_0.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/img_1.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/img_2.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/img_3.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/img_4.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/img_5.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/img_6.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/img_7.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/img_8.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/img_9.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/joint_state_0.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/joint_state_1.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/joint_state_2.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/joint_state_3.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/joint_state_4.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/joint_state_5.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/joint_state_6.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/joint_state_7.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/joint_state_8.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/joint_state_9.npy (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/mask_sam2_img_0.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/mask_sam2_img_1.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/mask_sam2_img_2.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/mask_sam2_img_3.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/mask_sam2_img_4.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/mask_sam2_img_5.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/mask_sam2_img_6.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/mask_sam2_img_7.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/mask_sam2_img_8.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/mask_sam2_img_9.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/overlay_img_0.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/overlay_img_1.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/overlay_img_2.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/overlay_img_3.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/overlay_img_4.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/overlay_img_5.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/overlay_img_6.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/overlay_img_7.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/overlay_img_8.png (100%) rename test/assets/{xarm/realsense => xarm_7/samples}/overlay_img_9.png (100%) diff --git a/test/assets/lbr_med7/mesh/link_0.stl b/test/assets/lbr_med7/mesh/link_0.stl deleted file mode 100644 index 1d916fa..0000000 --- a/test/assets/lbr_med7/mesh/link_0.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d07556c4c5c00ef4a0f2ef26aabd58efd705d7b6fe34724fe5a26c6425fe7977 -size 201984 diff --git a/test/assets/lbr_med7/mesh/link_1.stl b/test/assets/lbr_med7/mesh/link_1.stl deleted file mode 100644 index 3d47c2a..0000000 --- a/test/assets/lbr_med7/mesh/link_1.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a9a5475c6bb0b3f3133b0b4c092ebd8f719ccd2d5aa524a163f115284a58ea64 -size 367584 diff --git a/test/assets/lbr_med7/mesh/link_2.stl b/test/assets/lbr_med7/mesh/link_2.stl deleted file mode 100644 index 9d8f72d..0000000 --- a/test/assets/lbr_med7/mesh/link_2.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a5cd44b1d1197367e665d798eaecc69cb83b91d0979bc92366d3dbbe0a0e1013 -size 699884 diff --git a/test/assets/lbr_med7/mesh/link_3.stl b/test/assets/lbr_med7/mesh/link_3.stl deleted file mode 100644 index 37e908d..0000000 --- a/test/assets/lbr_med7/mesh/link_3.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9871fb3a1c81be0113fbe19e2b92eae9ba55ea099a82255921b76115bb4d198f -size 367584 diff --git a/test/assets/lbr_med7/mesh/link_4.stl b/test/assets/lbr_med7/mesh/link_4.stl deleted file mode 100644 index 743eb7c..0000000 --- a/test/assets/lbr_med7/mesh/link_4.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:408df2c5ffd2d0e16e475745466435eb8768211c08ba9ca0e4afd15938f6698d -size 699884 diff --git a/test/assets/lbr_med7/mesh/link_5.stl b/test/assets/lbr_med7/mesh/link_5.stl deleted file mode 100644 index 6363888..0000000 --- a/test/assets/lbr_med7/mesh/link_5.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8b591e6053028ffda1ebd8be9ef461fe4abfffabefa3b0847281500af9292479 -size 417384 diff --git a/test/assets/lbr_med7/mesh/link_6.stl b/test/assets/lbr_med7/mesh/link_6.stl deleted file mode 100644 index f119a6f..0000000 --- a/test/assets/lbr_med7/mesh/link_6.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6a708672e6049e27fd9427f4cf511eddf6fe81db95fa2e791af0e18876ee53b8 -size 688584 diff --git a/test/assets/lbr_med7/mesh/link_7.stl b/test/assets/lbr_med7/mesh/link_7.stl deleted file mode 100644 index b605b6b..0000000 --- a/test/assets/lbr_med7/mesh/link_7.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3b9ba0b421ac5f73eed9df5817161850267e49d1a4dcdaeadd8d36f26505387b -size 208784 diff --git a/test/assets/lbr_med7_r800/description/med7_r800.urdf b/test/assets/lbr_med7_r800/description/med7_r800.urdf new file mode 100644 index 0000000..4636b05 --- /dev/null +++ b/test/assets/lbr_med7_r800/description/med7_r800.urdf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:33a2372c4b8b178b33beff64e26756805fa0fe6205dc7d0366d0e1d13b5b84a5 +size 8818 diff --git a/test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl b/test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl new file mode 100644 index 0000000..ec2cf63 --- /dev/null +++ b/test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a992dbc53a98479fd70ecd4e235ae3f68795e9591a8fc2bc922dbb78a3e92f2b +size 6084 diff --git a/test/assets/lbr_med7_r800/description/meshes/collision/link_1.stl b/test/assets/lbr_med7_r800/description/meshes/collision/link_1.stl new file mode 100644 index 0000000..851763f --- /dev/null +++ b/test/assets/lbr_med7_r800/description/meshes/collision/link_1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d1ea3d3fbec591595a3d1c3a41cf743182ad2bb8de7be67d126f212a3d74f2f0 +size 11084 diff --git a/test/assets/lbr_med7_r800/description/meshes/collision/link_2.stl b/test/assets/lbr_med7_r800/description/meshes/collision/link_2.stl new file mode 100644 index 0000000..6217742 --- /dev/null +++ b/test/assets/lbr_med7_r800/description/meshes/collision/link_2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:251b422fd3149803a5dd94605fdb7d855f60406171b81c78b3af2109d26a96d8 +size 20984 diff --git a/test/assets/lbr_med7_r800/description/meshes/collision/link_3.stl b/test/assets/lbr_med7_r800/description/meshes/collision/link_3.stl new file mode 100644 index 0000000..8828ee9 --- /dev/null +++ b/test/assets/lbr_med7_r800/description/meshes/collision/link_3.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1145eef3a673e8608cfbeb3e56eb756bc57a9960bebb9278d76de61a8d4c281b +size 11084 diff --git a/test/assets/lbr_med7_r800/description/meshes/collision/link_4.stl b/test/assets/lbr_med7_r800/description/meshes/collision/link_4.stl new file mode 100644 index 0000000..7dcbd9c --- /dev/null +++ b/test/assets/lbr_med7_r800/description/meshes/collision/link_4.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:906e5f8a9d72e155917c4f57b7e9f4ac0226e9c0b68376efa66736ee0387ee8f +size 20984 diff --git a/test/assets/lbr_med7_r800/description/meshes/collision/link_5.stl b/test/assets/lbr_med7_r800/description/meshes/collision/link_5.stl new file mode 100644 index 0000000..3c60a1f --- /dev/null +++ b/test/assets/lbr_med7_r800/description/meshes/collision/link_5.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:43baa392451fc6d780061e7b0d8b7c085c7e4fdc828ab629ffb884d1986e8400 +size 12584 diff --git a/test/assets/lbr_med7_r800/description/meshes/collision/link_6.stl b/test/assets/lbr_med7_r800/description/meshes/collision/link_6.stl new file mode 100644 index 0000000..f3d2c73 --- /dev/null +++ b/test/assets/lbr_med7_r800/description/meshes/collision/link_6.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:da8f7a2ec6f69f935718f71dac97e411dd209a8da269c0057f5af93797dedf58 +size 20734 diff --git a/test/assets/lbr_med7_r800/description/meshes/collision/link_7.stl b/test/assets/lbr_med7_r800/description/meshes/collision/link_7.stl new file mode 100644 index 0000000..a52916d --- /dev/null +++ b/test/assets/lbr_med7_r800/description/meshes/collision/link_7.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9ce457ba4a2acf817d116a8d323e875468e5ce92fd27ae5d1864b6a78ce80d8d +size 6334 diff --git a/test/assets/lbr_med7/mesh/link_0.dae b/test/assets/lbr_med7_r800/description/meshes/visual/link_0.dae similarity index 100% rename from test/assets/lbr_med7/mesh/link_0.dae rename to test/assets/lbr_med7_r800/description/meshes/visual/link_0.dae diff --git a/test/assets/lbr_med7/mesh/link_1.dae b/test/assets/lbr_med7_r800/description/meshes/visual/link_1.dae similarity index 100% rename from test/assets/lbr_med7/mesh/link_1.dae rename to test/assets/lbr_med7_r800/description/meshes/visual/link_1.dae diff --git a/test/assets/lbr_med7/mesh/link_2.dae b/test/assets/lbr_med7_r800/description/meshes/visual/link_2.dae similarity index 100% rename from test/assets/lbr_med7/mesh/link_2.dae rename to test/assets/lbr_med7_r800/description/meshes/visual/link_2.dae diff --git a/test/assets/lbr_med7/mesh/link_3.dae b/test/assets/lbr_med7_r800/description/meshes/visual/link_3.dae similarity index 100% rename from test/assets/lbr_med7/mesh/link_3.dae rename to test/assets/lbr_med7_r800/description/meshes/visual/link_3.dae diff --git a/test/assets/lbr_med7/mesh/link_4.dae b/test/assets/lbr_med7_r800/description/meshes/visual/link_4.dae similarity index 100% rename from test/assets/lbr_med7/mesh/link_4.dae rename to test/assets/lbr_med7_r800/description/meshes/visual/link_4.dae diff --git a/test/assets/lbr_med7/mesh/link_5.dae b/test/assets/lbr_med7_r800/description/meshes/visual/link_5.dae similarity index 100% rename from test/assets/lbr_med7/mesh/link_5.dae rename to test/assets/lbr_med7_r800/description/meshes/visual/link_5.dae diff --git a/test/assets/lbr_med7/mesh/link_6.dae b/test/assets/lbr_med7_r800/description/meshes/visual/link_6.dae similarity index 100% rename from test/assets/lbr_med7/mesh/link_6.dae rename to test/assets/lbr_med7_r800/description/meshes/visual/link_6.dae diff --git a/test/assets/lbr_med7/mesh/link_7.dae b/test/assets/lbr_med7_r800/description/meshes/visual/link_7.dae similarity index 100% rename from test/assets/lbr_med7/mesh/link_7.dae rename to test/assets/lbr_med7_r800/description/meshes/visual/link_7.dae diff --git a/test/assets/lbr_med7/zed2i/HT_cam_swarm.npy b/test/assets/lbr_med7_r800/samples/HT_cam_swarm.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/HT_cam_swarm.npy rename to test/assets/lbr_med7_r800/samples/HT_cam_swarm.npy diff --git a/test/assets/lbr_med7/zed2i/HT_dr.npy b/test/assets/lbr_med7_r800/samples/HT_dr.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/HT_dr.npy rename to test/assets/lbr_med7_r800/samples/HT_dr.npy diff --git a/test/assets/lbr_med7/zed2i/HT_hydra.npy b/test/assets/lbr_med7_r800/samples/HT_hydra.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/HT_hydra.npy rename to test/assets/lbr_med7_r800/samples/HT_hydra.npy diff --git a/test/assets/lbr_med7/zed2i/HT_hydra_robust.npy b/test/assets/lbr_med7_r800/samples/HT_hydra_robust.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/HT_hydra_robust.npy rename to test/assets/lbr_med7_r800/samples/HT_hydra_robust.npy diff --git a/test/assets/lbr_med7/zed2i/HT_left_dr.npy b/test/assets/lbr_med7_r800/samples/HT_left_dr.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/HT_left_dr.npy rename to test/assets/lbr_med7_r800/samples/HT_left_dr.npy diff --git a/test/assets/lbr_med7/zed2i/HT_right_dr.npy b/test/assets/lbr_med7_r800/samples/HT_right_dr.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/HT_right_dr.npy rename to test/assets/lbr_med7_r800/samples/HT_right_dr.npy diff --git a/test/assets/lbr_med7/zed2i/HT_right_to_left.npy b/test/assets/lbr_med7_r800/samples/HT_right_to_left.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/HT_right_to_left.npy rename to test/assets/lbr_med7_r800/samples/HT_right_to_left.npy diff --git a/test/assets/lbr_med7/zed2i/depth_0.npy b/test/assets/lbr_med7_r800/samples/depth_0.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/depth_0.npy rename to test/assets/lbr_med7_r800/samples/depth_0.npy diff --git a/test/assets/lbr_med7/zed2i/depth_1.npy b/test/assets/lbr_med7_r800/samples/depth_1.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/depth_1.npy rename to test/assets/lbr_med7_r800/samples/depth_1.npy diff --git a/test/assets/lbr_med7/zed2i/depth_2.npy b/test/assets/lbr_med7_r800/samples/depth_2.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/depth_2.npy rename to test/assets/lbr_med7_r800/samples/depth_2.npy diff --git a/test/assets/lbr_med7/zed2i/depth_3.npy b/test/assets/lbr_med7_r800/samples/depth_3.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/depth_3.npy rename to test/assets/lbr_med7_r800/samples/depth_3.npy diff --git a/test/assets/lbr_med7/zed2i/depth_4.npy b/test/assets/lbr_med7_r800/samples/depth_4.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/depth_4.npy rename to test/assets/lbr_med7_r800/samples/depth_4.npy diff --git a/test/assets/lbr_med7/zed2i/joint_states_0.npy b/test/assets/lbr_med7_r800/samples/joint_states_0.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/joint_states_0.npy rename to test/assets/lbr_med7_r800/samples/joint_states_0.npy diff --git a/test/assets/lbr_med7/zed2i/joint_states_1.npy b/test/assets/lbr_med7_r800/samples/joint_states_1.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/joint_states_1.npy rename to test/assets/lbr_med7_r800/samples/joint_states_1.npy diff --git a/test/assets/lbr_med7/zed2i/joint_states_2.npy b/test/assets/lbr_med7_r800/samples/joint_states_2.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/joint_states_2.npy rename to test/assets/lbr_med7_r800/samples/joint_states_2.npy diff --git a/test/assets/lbr_med7/zed2i/joint_states_3.npy b/test/assets/lbr_med7_r800/samples/joint_states_3.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/joint_states_3.npy rename to test/assets/lbr_med7_r800/samples/joint_states_3.npy diff --git a/test/assets/lbr_med7/zed2i/joint_states_4.npy b/test/assets/lbr_med7_r800/samples/joint_states_4.npy similarity index 100% rename from test/assets/lbr_med7/zed2i/joint_states_4.npy rename to test/assets/lbr_med7_r800/samples/joint_states_4.npy diff --git a/test/assets/lbr_med7/zed2i/left_camera_info.yaml b/test/assets/lbr_med7_r800/samples/left_camera_info.yaml similarity index 100% rename from test/assets/lbr_med7/zed2i/left_camera_info.yaml rename to test/assets/lbr_med7_r800/samples/left_camera_info.yaml diff --git a/test/assets/lbr_med7/zed2i/left_image_0.png b/test/assets/lbr_med7_r800/samples/left_image_0.png similarity index 100% rename from test/assets/lbr_med7/zed2i/left_image_0.png rename to test/assets/lbr_med7_r800/samples/left_image_0.png diff --git a/test/assets/lbr_med7/zed2i/left_image_0_samples.csv b/test/assets/lbr_med7_r800/samples/left_image_0_samples.csv similarity index 100% rename from test/assets/lbr_med7/zed2i/left_image_0_samples.csv rename to test/assets/lbr_med7_r800/samples/left_image_0_samples.csv diff --git a/test/assets/lbr_med7/zed2i/left_image_1.png b/test/assets/lbr_med7_r800/samples/left_image_1.png similarity index 100% rename from test/assets/lbr_med7/zed2i/left_image_1.png rename to test/assets/lbr_med7_r800/samples/left_image_1.png diff --git a/test/assets/lbr_med7/zed2i/left_image_1_samples.csv b/test/assets/lbr_med7_r800/samples/left_image_1_samples.csv similarity index 100% rename from test/assets/lbr_med7/zed2i/left_image_1_samples.csv rename to test/assets/lbr_med7_r800/samples/left_image_1_samples.csv diff --git a/test/assets/lbr_med7/zed2i/left_image_2.png b/test/assets/lbr_med7_r800/samples/left_image_2.png similarity index 100% rename from test/assets/lbr_med7/zed2i/left_image_2.png rename to test/assets/lbr_med7_r800/samples/left_image_2.png diff --git a/test/assets/lbr_med7/zed2i/left_image_2_samples.csv b/test/assets/lbr_med7_r800/samples/left_image_2_samples.csv similarity index 100% rename from test/assets/lbr_med7/zed2i/left_image_2_samples.csv rename to test/assets/lbr_med7_r800/samples/left_image_2_samples.csv diff --git a/test/assets/lbr_med7/zed2i/left_image_3.png b/test/assets/lbr_med7_r800/samples/left_image_3.png similarity index 100% rename from test/assets/lbr_med7/zed2i/left_image_3.png rename to test/assets/lbr_med7_r800/samples/left_image_3.png diff --git a/test/assets/lbr_med7/zed2i/left_image_3_samples.csv b/test/assets/lbr_med7_r800/samples/left_image_3_samples.csv similarity index 100% rename from test/assets/lbr_med7/zed2i/left_image_3_samples.csv rename to test/assets/lbr_med7_r800/samples/left_image_3_samples.csv diff --git a/test/assets/lbr_med7/zed2i/left_image_4.png b/test/assets/lbr_med7_r800/samples/left_image_4.png similarity index 100% rename from test/assets/lbr_med7/zed2i/left_image_4.png rename to test/assets/lbr_med7_r800/samples/left_image_4.png diff --git a/test/assets/lbr_med7/zed2i/left_image_4_samples.csv b/test/assets/lbr_med7_r800/samples/left_image_4_samples.csv similarity index 100% rename from test/assets/lbr_med7/zed2i/left_image_4_samples.csv rename to test/assets/lbr_med7_r800/samples/left_image_4_samples.csv diff --git a/test/assets/lbr_med7/zed2i/mask_sam2_left_image_0.png b/test/assets/lbr_med7_r800/samples/mask_sam2_left_image_0.png similarity index 100% rename from test/assets/lbr_med7/zed2i/mask_sam2_left_image_0.png rename to test/assets/lbr_med7_r800/samples/mask_sam2_left_image_0.png diff --git a/test/assets/lbr_med7/zed2i/mask_sam2_left_image_1.png b/test/assets/lbr_med7_r800/samples/mask_sam2_left_image_1.png similarity index 100% rename from test/assets/lbr_med7/zed2i/mask_sam2_left_image_1.png rename to test/assets/lbr_med7_r800/samples/mask_sam2_left_image_1.png diff --git a/test/assets/lbr_med7/zed2i/mask_sam2_left_image_2.png b/test/assets/lbr_med7_r800/samples/mask_sam2_left_image_2.png similarity index 100% rename from test/assets/lbr_med7/zed2i/mask_sam2_left_image_2.png rename to test/assets/lbr_med7_r800/samples/mask_sam2_left_image_2.png diff --git a/test/assets/lbr_med7/zed2i/mask_sam2_left_image_3.png b/test/assets/lbr_med7_r800/samples/mask_sam2_left_image_3.png similarity index 100% rename from test/assets/lbr_med7/zed2i/mask_sam2_left_image_3.png rename to test/assets/lbr_med7_r800/samples/mask_sam2_left_image_3.png diff --git a/test/assets/lbr_med7/zed2i/mask_sam2_left_image_4.png b/test/assets/lbr_med7_r800/samples/mask_sam2_left_image_4.png similarity index 100% rename from test/assets/lbr_med7/zed2i/mask_sam2_left_image_4.png rename to test/assets/lbr_med7_r800/samples/mask_sam2_left_image_4.png diff --git a/test/assets/lbr_med7/zed2i/mask_sam2_right_image_0.png b/test/assets/lbr_med7_r800/samples/mask_sam2_right_image_0.png similarity index 100% rename from test/assets/lbr_med7/zed2i/mask_sam2_right_image_0.png rename to test/assets/lbr_med7_r800/samples/mask_sam2_right_image_0.png diff --git a/test/assets/lbr_med7/zed2i/mask_sam2_right_image_1.png b/test/assets/lbr_med7_r800/samples/mask_sam2_right_image_1.png similarity index 100% rename from test/assets/lbr_med7/zed2i/mask_sam2_right_image_1.png rename to test/assets/lbr_med7_r800/samples/mask_sam2_right_image_1.png diff --git a/test/assets/lbr_med7/zed2i/mask_sam2_right_image_2.png b/test/assets/lbr_med7_r800/samples/mask_sam2_right_image_2.png similarity index 100% rename from test/assets/lbr_med7/zed2i/mask_sam2_right_image_2.png rename to test/assets/lbr_med7_r800/samples/mask_sam2_right_image_2.png diff --git a/test/assets/lbr_med7/zed2i/mask_sam2_right_image_3.png b/test/assets/lbr_med7_r800/samples/mask_sam2_right_image_3.png similarity index 100% rename from test/assets/lbr_med7/zed2i/mask_sam2_right_image_3.png rename to test/assets/lbr_med7_r800/samples/mask_sam2_right_image_3.png diff --git a/test/assets/lbr_med7/zed2i/mask_sam2_right_image_4.png b/test/assets/lbr_med7_r800/samples/mask_sam2_right_image_4.png similarity index 100% rename from test/assets/lbr_med7/zed2i/mask_sam2_right_image_4.png rename to test/assets/lbr_med7_r800/samples/mask_sam2_right_image_4.png diff --git a/test/assets/lbr_med7/zed2i/right_camera_info.yaml b/test/assets/lbr_med7_r800/samples/right_camera_info.yaml similarity index 100% rename from test/assets/lbr_med7/zed2i/right_camera_info.yaml rename to test/assets/lbr_med7_r800/samples/right_camera_info.yaml diff --git a/test/assets/lbr_med7/zed2i/right_image_0.png b/test/assets/lbr_med7_r800/samples/right_image_0.png similarity index 100% rename from test/assets/lbr_med7/zed2i/right_image_0.png rename to test/assets/lbr_med7_r800/samples/right_image_0.png diff --git a/test/assets/lbr_med7/zed2i/right_image_0_samples.csv b/test/assets/lbr_med7_r800/samples/right_image_0_samples.csv similarity index 100% rename from test/assets/lbr_med7/zed2i/right_image_0_samples.csv rename to test/assets/lbr_med7_r800/samples/right_image_0_samples.csv diff --git a/test/assets/lbr_med7/zed2i/right_image_1.png b/test/assets/lbr_med7_r800/samples/right_image_1.png similarity index 100% rename from test/assets/lbr_med7/zed2i/right_image_1.png rename to test/assets/lbr_med7_r800/samples/right_image_1.png diff --git a/test/assets/lbr_med7/zed2i/right_image_1_samples.csv b/test/assets/lbr_med7_r800/samples/right_image_1_samples.csv similarity index 100% rename from test/assets/lbr_med7/zed2i/right_image_1_samples.csv rename to test/assets/lbr_med7_r800/samples/right_image_1_samples.csv diff --git a/test/assets/lbr_med7/zed2i/right_image_2.png b/test/assets/lbr_med7_r800/samples/right_image_2.png similarity index 100% rename from test/assets/lbr_med7/zed2i/right_image_2.png rename to test/assets/lbr_med7_r800/samples/right_image_2.png diff --git a/test/assets/lbr_med7/zed2i/right_image_2_samples.csv b/test/assets/lbr_med7_r800/samples/right_image_2_samples.csv similarity index 100% rename from test/assets/lbr_med7/zed2i/right_image_2_samples.csv rename to test/assets/lbr_med7_r800/samples/right_image_2_samples.csv diff --git a/test/assets/lbr_med7/zed2i/right_image_3.png b/test/assets/lbr_med7_r800/samples/right_image_3.png similarity index 100% rename from test/assets/lbr_med7/zed2i/right_image_3.png rename to test/assets/lbr_med7_r800/samples/right_image_3.png diff --git a/test/assets/lbr_med7/zed2i/right_image_3_samples.csv b/test/assets/lbr_med7_r800/samples/right_image_3_samples.csv similarity index 100% rename from test/assets/lbr_med7/zed2i/right_image_3_samples.csv rename to test/assets/lbr_med7_r800/samples/right_image_3_samples.csv diff --git a/test/assets/lbr_med7/zed2i/right_image_4.png b/test/assets/lbr_med7_r800/samples/right_image_4.png similarity index 100% rename from test/assets/lbr_med7/zed2i/right_image_4.png rename to test/assets/lbr_med7_r800/samples/right_image_4.png diff --git a/test/assets/lbr_med7/zed2i/right_image_4_samples.csv b/test/assets/lbr_med7_r800/samples/right_image_4_samples.csv similarity index 100% rename from test/assets/lbr_med7/zed2i/right_image_4_samples.csv rename to test/assets/lbr_med7_r800/samples/right_image_4_samples.csv diff --git a/test/assets/xarm/mesh/link_base.STL b/test/assets/xarm_7/description/link_base.STL similarity index 100% rename from test/assets/xarm/mesh/link_base.STL rename to test/assets/xarm_7/description/link_base.STL diff --git a/test/assets/xarm/realsense/HT_hydra_robust.npy b/test/assets/xarm_7/samples/HT_hydra_robust.npy similarity index 100% rename from test/assets/xarm/realsense/HT_hydra_robust.npy rename to test/assets/xarm_7/samples/HT_hydra_robust.npy diff --git a/test/assets/xarm/realsense/camera_info.yaml b/test/assets/xarm_7/samples/camera_info.yaml similarity index 100% rename from test/assets/xarm/realsense/camera_info.yaml rename to test/assets/xarm_7/samples/camera_info.yaml diff --git a/test/assets/xarm/realsense/depth_0.npy b/test/assets/xarm_7/samples/depth_0.npy similarity index 100% rename from test/assets/xarm/realsense/depth_0.npy rename to test/assets/xarm_7/samples/depth_0.npy diff --git a/test/assets/xarm/realsense/depth_1.npy b/test/assets/xarm_7/samples/depth_1.npy similarity index 100% rename from test/assets/xarm/realsense/depth_1.npy rename to test/assets/xarm_7/samples/depth_1.npy diff --git a/test/assets/xarm/realsense/depth_2.npy b/test/assets/xarm_7/samples/depth_2.npy similarity index 100% rename from test/assets/xarm/realsense/depth_2.npy rename to test/assets/xarm_7/samples/depth_2.npy diff --git a/test/assets/xarm/realsense/depth_3.npy b/test/assets/xarm_7/samples/depth_3.npy similarity index 100% rename from test/assets/xarm/realsense/depth_3.npy rename to test/assets/xarm_7/samples/depth_3.npy diff --git a/test/assets/xarm/realsense/depth_4.npy b/test/assets/xarm_7/samples/depth_4.npy similarity index 100% rename from test/assets/xarm/realsense/depth_4.npy rename to test/assets/xarm_7/samples/depth_4.npy diff --git a/test/assets/xarm/realsense/depth_5.npy b/test/assets/xarm_7/samples/depth_5.npy similarity index 100% rename from test/assets/xarm/realsense/depth_5.npy rename to test/assets/xarm_7/samples/depth_5.npy diff --git a/test/assets/xarm/realsense/depth_6.npy b/test/assets/xarm_7/samples/depth_6.npy similarity index 100% rename from test/assets/xarm/realsense/depth_6.npy rename to test/assets/xarm_7/samples/depth_6.npy diff --git a/test/assets/xarm/realsense/depth_7.npy b/test/assets/xarm_7/samples/depth_7.npy similarity index 100% rename from test/assets/xarm/realsense/depth_7.npy rename to test/assets/xarm_7/samples/depth_7.npy diff --git a/test/assets/xarm/realsense/depth_8.npy b/test/assets/xarm_7/samples/depth_8.npy similarity index 100% rename from test/assets/xarm/realsense/depth_8.npy rename to test/assets/xarm_7/samples/depth_8.npy diff --git a/test/assets/xarm/realsense/depth_9.npy b/test/assets/xarm_7/samples/depth_9.npy similarity index 100% rename from test/assets/xarm/realsense/depth_9.npy rename to test/assets/xarm_7/samples/depth_9.npy diff --git a/test/assets/xarm/realsense/img_0.png b/test/assets/xarm_7/samples/img_0.png similarity index 100% rename from test/assets/xarm/realsense/img_0.png rename to test/assets/xarm_7/samples/img_0.png diff --git a/test/assets/xarm/realsense/img_1.png b/test/assets/xarm_7/samples/img_1.png similarity index 100% rename from test/assets/xarm/realsense/img_1.png rename to test/assets/xarm_7/samples/img_1.png diff --git a/test/assets/xarm/realsense/img_2.png b/test/assets/xarm_7/samples/img_2.png similarity index 100% rename from test/assets/xarm/realsense/img_2.png rename to test/assets/xarm_7/samples/img_2.png diff --git a/test/assets/xarm/realsense/img_3.png b/test/assets/xarm_7/samples/img_3.png similarity index 100% rename from test/assets/xarm/realsense/img_3.png rename to test/assets/xarm_7/samples/img_3.png diff --git a/test/assets/xarm/realsense/img_4.png b/test/assets/xarm_7/samples/img_4.png similarity index 100% rename from test/assets/xarm/realsense/img_4.png rename to test/assets/xarm_7/samples/img_4.png diff --git a/test/assets/xarm/realsense/img_5.png b/test/assets/xarm_7/samples/img_5.png similarity index 100% rename from test/assets/xarm/realsense/img_5.png rename to test/assets/xarm_7/samples/img_5.png diff --git a/test/assets/xarm/realsense/img_6.png b/test/assets/xarm_7/samples/img_6.png similarity index 100% rename from test/assets/xarm/realsense/img_6.png rename to test/assets/xarm_7/samples/img_6.png diff --git a/test/assets/xarm/realsense/img_7.png b/test/assets/xarm_7/samples/img_7.png similarity index 100% rename from test/assets/xarm/realsense/img_7.png rename to test/assets/xarm_7/samples/img_7.png diff --git a/test/assets/xarm/realsense/img_8.png b/test/assets/xarm_7/samples/img_8.png similarity index 100% rename from test/assets/xarm/realsense/img_8.png rename to test/assets/xarm_7/samples/img_8.png diff --git a/test/assets/xarm/realsense/img_9.png b/test/assets/xarm_7/samples/img_9.png similarity index 100% rename from test/assets/xarm/realsense/img_9.png rename to test/assets/xarm_7/samples/img_9.png diff --git a/test/assets/xarm/realsense/joint_state_0.npy b/test/assets/xarm_7/samples/joint_state_0.npy similarity index 100% rename from test/assets/xarm/realsense/joint_state_0.npy rename to test/assets/xarm_7/samples/joint_state_0.npy diff --git a/test/assets/xarm/realsense/joint_state_1.npy b/test/assets/xarm_7/samples/joint_state_1.npy similarity index 100% rename from test/assets/xarm/realsense/joint_state_1.npy rename to test/assets/xarm_7/samples/joint_state_1.npy diff --git a/test/assets/xarm/realsense/joint_state_2.npy b/test/assets/xarm_7/samples/joint_state_2.npy similarity index 100% rename from test/assets/xarm/realsense/joint_state_2.npy rename to test/assets/xarm_7/samples/joint_state_2.npy diff --git a/test/assets/xarm/realsense/joint_state_3.npy b/test/assets/xarm_7/samples/joint_state_3.npy similarity index 100% rename from test/assets/xarm/realsense/joint_state_3.npy rename to test/assets/xarm_7/samples/joint_state_3.npy diff --git a/test/assets/xarm/realsense/joint_state_4.npy b/test/assets/xarm_7/samples/joint_state_4.npy similarity index 100% rename from test/assets/xarm/realsense/joint_state_4.npy rename to test/assets/xarm_7/samples/joint_state_4.npy diff --git a/test/assets/xarm/realsense/joint_state_5.npy b/test/assets/xarm_7/samples/joint_state_5.npy similarity index 100% rename from test/assets/xarm/realsense/joint_state_5.npy rename to test/assets/xarm_7/samples/joint_state_5.npy diff --git a/test/assets/xarm/realsense/joint_state_6.npy b/test/assets/xarm_7/samples/joint_state_6.npy similarity index 100% rename from test/assets/xarm/realsense/joint_state_6.npy rename to test/assets/xarm_7/samples/joint_state_6.npy diff --git a/test/assets/xarm/realsense/joint_state_7.npy b/test/assets/xarm_7/samples/joint_state_7.npy similarity index 100% rename from test/assets/xarm/realsense/joint_state_7.npy rename to test/assets/xarm_7/samples/joint_state_7.npy diff --git a/test/assets/xarm/realsense/joint_state_8.npy b/test/assets/xarm_7/samples/joint_state_8.npy similarity index 100% rename from test/assets/xarm/realsense/joint_state_8.npy rename to test/assets/xarm_7/samples/joint_state_8.npy diff --git a/test/assets/xarm/realsense/joint_state_9.npy b/test/assets/xarm_7/samples/joint_state_9.npy similarity index 100% rename from test/assets/xarm/realsense/joint_state_9.npy rename to test/assets/xarm_7/samples/joint_state_9.npy diff --git a/test/assets/xarm/realsense/mask_sam2_img_0.png b/test/assets/xarm_7/samples/mask_sam2_img_0.png similarity index 100% rename from test/assets/xarm/realsense/mask_sam2_img_0.png rename to test/assets/xarm_7/samples/mask_sam2_img_0.png diff --git a/test/assets/xarm/realsense/mask_sam2_img_1.png b/test/assets/xarm_7/samples/mask_sam2_img_1.png similarity index 100% rename from test/assets/xarm/realsense/mask_sam2_img_1.png rename to test/assets/xarm_7/samples/mask_sam2_img_1.png diff --git a/test/assets/xarm/realsense/mask_sam2_img_2.png b/test/assets/xarm_7/samples/mask_sam2_img_2.png similarity index 100% rename from test/assets/xarm/realsense/mask_sam2_img_2.png rename to test/assets/xarm_7/samples/mask_sam2_img_2.png diff --git a/test/assets/xarm/realsense/mask_sam2_img_3.png b/test/assets/xarm_7/samples/mask_sam2_img_3.png similarity index 100% rename from test/assets/xarm/realsense/mask_sam2_img_3.png rename to test/assets/xarm_7/samples/mask_sam2_img_3.png diff --git a/test/assets/xarm/realsense/mask_sam2_img_4.png b/test/assets/xarm_7/samples/mask_sam2_img_4.png similarity index 100% rename from test/assets/xarm/realsense/mask_sam2_img_4.png rename to test/assets/xarm_7/samples/mask_sam2_img_4.png diff --git a/test/assets/xarm/realsense/mask_sam2_img_5.png b/test/assets/xarm_7/samples/mask_sam2_img_5.png similarity index 100% rename from test/assets/xarm/realsense/mask_sam2_img_5.png rename to test/assets/xarm_7/samples/mask_sam2_img_5.png diff --git a/test/assets/xarm/realsense/mask_sam2_img_6.png b/test/assets/xarm_7/samples/mask_sam2_img_6.png similarity index 100% rename from test/assets/xarm/realsense/mask_sam2_img_6.png rename to test/assets/xarm_7/samples/mask_sam2_img_6.png diff --git a/test/assets/xarm/realsense/mask_sam2_img_7.png b/test/assets/xarm_7/samples/mask_sam2_img_7.png similarity index 100% rename from test/assets/xarm/realsense/mask_sam2_img_7.png rename to test/assets/xarm_7/samples/mask_sam2_img_7.png diff --git a/test/assets/xarm/realsense/mask_sam2_img_8.png b/test/assets/xarm_7/samples/mask_sam2_img_8.png similarity index 100% rename from test/assets/xarm/realsense/mask_sam2_img_8.png rename to test/assets/xarm_7/samples/mask_sam2_img_8.png diff --git a/test/assets/xarm/realsense/mask_sam2_img_9.png b/test/assets/xarm_7/samples/mask_sam2_img_9.png similarity index 100% rename from test/assets/xarm/realsense/mask_sam2_img_9.png rename to test/assets/xarm_7/samples/mask_sam2_img_9.png diff --git a/test/assets/xarm/realsense/overlay_img_0.png b/test/assets/xarm_7/samples/overlay_img_0.png similarity index 100% rename from test/assets/xarm/realsense/overlay_img_0.png rename to test/assets/xarm_7/samples/overlay_img_0.png diff --git a/test/assets/xarm/realsense/overlay_img_1.png b/test/assets/xarm_7/samples/overlay_img_1.png similarity index 100% rename from test/assets/xarm/realsense/overlay_img_1.png rename to test/assets/xarm_7/samples/overlay_img_1.png diff --git a/test/assets/xarm/realsense/overlay_img_2.png b/test/assets/xarm_7/samples/overlay_img_2.png similarity index 100% rename from test/assets/xarm/realsense/overlay_img_2.png rename to test/assets/xarm_7/samples/overlay_img_2.png diff --git a/test/assets/xarm/realsense/overlay_img_3.png b/test/assets/xarm_7/samples/overlay_img_3.png similarity index 100% rename from test/assets/xarm/realsense/overlay_img_3.png rename to test/assets/xarm_7/samples/overlay_img_3.png diff --git a/test/assets/xarm/realsense/overlay_img_4.png b/test/assets/xarm_7/samples/overlay_img_4.png similarity index 100% rename from test/assets/xarm/realsense/overlay_img_4.png rename to test/assets/xarm_7/samples/overlay_img_4.png diff --git a/test/assets/xarm/realsense/overlay_img_5.png b/test/assets/xarm_7/samples/overlay_img_5.png similarity index 100% rename from test/assets/xarm/realsense/overlay_img_5.png rename to test/assets/xarm_7/samples/overlay_img_5.png diff --git a/test/assets/xarm/realsense/overlay_img_6.png b/test/assets/xarm_7/samples/overlay_img_6.png similarity index 100% rename from test/assets/xarm/realsense/overlay_img_6.png rename to test/assets/xarm_7/samples/overlay_img_6.png diff --git a/test/assets/xarm/realsense/overlay_img_7.png b/test/assets/xarm_7/samples/overlay_img_7.png similarity index 100% rename from test/assets/xarm/realsense/overlay_img_7.png rename to test/assets/xarm_7/samples/overlay_img_7.png diff --git a/test/assets/xarm/realsense/overlay_img_8.png b/test/assets/xarm_7/samples/overlay_img_8.png similarity index 100% rename from test/assets/xarm/realsense/overlay_img_8.png rename to test/assets/xarm_7/samples/overlay_img_8.png diff --git a/test/assets/xarm/realsense/overlay_img_9.png b/test/assets/xarm_7/samples/overlay_img_9.png similarity index 100% rename from test/assets/xarm/realsense/overlay_img_9.png rename to test/assets/xarm_7/samples/overlay_img_9.png From d57082b5ef8cdd5fe9f83232adb48cd2992add01 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 1 Feb 2026 13:49:10 +0000 Subject: [PATCH 3/9] updated test data paths --- README.md | 36 +++++++++++++-------------- test/differentiable/test_rendering.py | 2 +- test/differentiable/test_scene.py | 4 +-- test/differentiable/test_structs.py | 4 +-- test/io/test_meshes.py | 8 +++--- test/io/test_parsers.py | 10 ++++---- test/test_hydra_icp.py | 4 +-- test/test_segmentor.py | 2 +- test/util/test_mask.py | 16 ++++++------ test/util/test_viz.py | 10 +++++--- 10 files changed, 50 insertions(+), 46 deletions(-) diff --git a/README.md b/README.md index 17a4fac..2e37848 100644 --- a/README.md +++ b/README.md @@ -136,7 +136,7 @@ This is a required step to generate robot masks. ```shell rr-sam2 \ - --path test/assets/lbr_med7/zed2i \ + --path test/assets/lbr_med7_r800/samples \ --pattern "left_image_*.png" \ --n-positive-samples 5 \ --n-negative-samples 5 \ @@ -148,8 +148,8 @@ The Hydra robust ICP implements a point-to-plane ICP registration on a Lie algeb ```shell rr-hydra \ - --camera-info-file test/assets/lbr_med7/zed2i/left_camera_info.yaml \ - --path test/assets/lbr_med7/zed2i \ + --camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \ + --path test/assets/lbr_med7_r800/samples \ --mask-pattern mask_sam2_left_image_*.png \ --depth-pattern depth_*.npy \ --joint-states-pattern joint_states_*.npy \ @@ -188,8 +188,8 @@ rr-cam-swarm \ --target-reduction 0.8 \ --scale 0.1 \ --n-samples 1 \ - --camera-info-file test/assets/lbr_med7/zed2i/left_camera_info.yaml \ - --path test/assets/lbr_med7/zed2i \ + --camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \ + --path test/assets/lbr_med7_r800/samples \ --image-pattern left_image_*.png \ --joint-states-pattern joint_states_*.npy \ --mask-pattern mask_sam2_left_image_*.png \ @@ -214,9 +214,9 @@ rr-mono-dr \ --xacro-path urdf/med7/med7.xacro \ --root-link-name lbr_link_0 \ --end-link-name lbr_link_7 \ - --camera-info-file test/assets/lbr_med7/zed2i/left_camera_info.yaml \ - --extrinsics-file test/assets/lbr_med7/zed2i/HT_hydra_robust.npy \ - --path test/assets/lbr_med7/zed2i \ + --camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \ + --extrinsics-file test/assets/lbr_med7_r800/samples/HT_hydra_robust.npy \ + --path test/assets/lbr_med7_r800/samples \ --image-pattern left_image_*.png \ --joint-states-pattern joint_states_*.npy \ --mask-pattern mask_sam2_left_image_*.png \ @@ -241,11 +241,11 @@ rr-stereo-dr \ --xacro-path urdf/med7/med7.xacro \ --root-link-name lbr_link_0 \ --end-link-name lbr_link_7 \ - --left-camera-info-file test/assets/lbr_med7/zed2i/left_camera_info.yaml \ - --right-camera-info-file test/assets/lbr_med7/zed2i/right_camera_info.yaml \ - --left-extrinsics-file test/assets/lbr_med7/zed2i/HT_hydra_robust.npy \ - --right-extrinsics-file test/assets/lbr_med7/zed2i/HT_right_to_left.npy \ - --path test/assets/lbr_med7/zed2i \ + --left-camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \ + --right-camera-info-file test/assets/lbr_med7_r800/samples/right_camera_info.yaml \ + --left-extrinsics-file test/assets/lbr_med7_r800/samples/HT_hydra_robust.npy \ + --right-extrinsics-file test/assets/lbr_med7_r800/samples/HT_right_to_left.npy \ + --path test/assets/lbr_med7_r800/samples \ --left-image-pattern left_image_*.png \ --right-image-pattern right_image_*.png \ --joint-states-pattern joint_states_*.npy \ @@ -271,13 +271,13 @@ rr-render \ --xacro-path urdf/med7/med7.xacro \ --root-link-name lbr_link_0 \ --end-link-name lbr_link_7 \ - --camera-info-file test/assets/lbr_med7/zed2i/left_camera_info.yaml \ - --extrinsics-file test/assets/lbr_med7/zed2i/HT_left_dr.npy \ - --images-path test/assets/lbr_med7/zed2i \ - --joint-states-path test/assets/lbr_med7/zed2i \ + --camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \ + --extrinsics-file test/assets/lbr_med7_r800/samples/HT_left_dr.npy \ + --images-path test/assets/lbr_med7_r800/samples \ + --joint-states-path test/assets/lbr_med7_r800/samples \ --image-pattern left_image_*.png \ --joint-states-pattern joint_states_*.npy \ - --output-path test/assets/lbr_med7/zed2i + --output-path test/assets/lbr_med7_r800/samples ``` ## Testing diff --git a/test/differentiable/test_rendering.py b/test/differentiable/test_rendering.py index c0aad10..5ee3f06 100644 --- a/test/differentiable/test_rendering.py +++ b/test/differentiable/test_rendering.py @@ -407,7 +407,7 @@ def test_multi_camera_pose_rendering() -> None: # load a sample mesh meshes = rrd.TorchMeshContainer( - {"link_0": "test/assets/lbr_med7/mesh/link_0.stl"}, + {"link_0": "test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl"}, batch_size=batch_size, device=device, ) diff --git a/test/differentiable/test_scene.py b/test/differentiable/test_scene.py index 406add0..9692436 100644 --- a/test/differentiable/test_scene.py +++ b/test/differentiable/test_scene.py @@ -236,8 +236,8 @@ def test_single_camera_multiple_poses() -> None: camera_name = "camera" camera = { camera_name: create_virtual_camera( - camera_info_file="test/assets/lbr_med7/zed2i/left_camera_info.yaml", - extrinsics_file="test/assets/lbr_med7/zed2i/HT_hydra_robust.npy", + camera_info_file="test/assets/lbr_med7_r800/samples/left_camera_info.yaml", + extrinsics_file="test/assets/lbr_med7_r800/samples/HT_hydra_robust.npy", device=device, ) } diff --git a/test/differentiable/test_structs.py b/test/differentiable/test_structs.py index ec82e40..8db5208 100644 --- a/test/differentiable/test_structs.py +++ b/test/differentiable/test_structs.py @@ -6,8 +6,8 @@ def test_torch_mesh_container() -> None: paths = { - "link_0": "test/assets/lbr_med7/mesh/link_0.stl", - "link_1": "test/assets/lbr_med7/mesh/link_1.stl", + "link_0": "test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl", + "link_1": "test/assets/lbr_med7_r800/description/meshes/collision/link_1.stl", } device = "cpu" container = TorchMeshContainer( diff --git a/test/io/test_meshes.py b/test/io/test_meshes.py index 5c97b2c..d018f70 100644 --- a/test/io/test_meshes.py +++ b/test/io/test_meshes.py @@ -4,14 +4,14 @@ def test_load_mesh() -> None: - path = "test/assets/lbr_med7/mesh/link_0.dae" + path = "test/assets/lbr_med7_r800/description/meshes/visual/link_0.dae" mesh = load_mesh(path=path) assert mesh.vertices.shape[-1] == 3, "Expected vertices of shape Nx3." assert mesh.faces.shape[-1] == 3, "Expected faces of shape Nx3." assert type(mesh.vertices) == np.ndarray, "Expected vertices to be numpy ndarray." assert type(mesh.faces) == np.ndarray, "Expected faces to be numpy ndarray." - path = "test/assets/lbr_med7/mesh/link_0.stl" + path = "test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl" mesh = load_mesh(path=path) assert mesh.vertices.shape[-1] == 3, "Expected vertices of shape Nx3." assert mesh.faces.shape[-1] == 3, "Expected faces of shape Nx3." @@ -27,8 +27,8 @@ def test_load_mesh() -> None: def test_load_meshes() -> None: paths = { - "link_0": "test/assets/lbr_med7/mesh/link_0.stl", - "link_1": "test/assets/lbr_med7/mesh/link_1.stl", + "link_0": "test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl", + "link_1": "test/assets/lbr_med7_r800/description/meshes/collision/link_1.stl", } meshes = load_meshes(paths=paths) assert paths.keys() == meshes.keys(), "Expected same keys." diff --git a/test/io/test_parsers.py b/test/io/test_parsers.py index ac8de9c..b1597cb 100644 --- a/test/io/test_parsers.py +++ b/test/io/test_parsers.py @@ -25,7 +25,7 @@ def test_urdf_parser() -> None: def test_find_files() -> None: - path = "test/assets/lbr_med7/zed2i" + path = "test/assets/lbr_med7_r800/samples" mask_files = find_files(path, "mask_sam2_left_*.png") assert len(mask_files) > 0, "Should find at least one mask file." @@ -37,7 +37,7 @@ def test_find_files() -> None: def test_parse_camera_info() -> None: - path = Path("test/assets/lbr_med7/zed2i") + path = Path("test/assets/lbr_med7_r800/samples") file = "left_camera_info.yaml" height, width, intrinsic_matrix = parse_camera_info(path / file) @@ -50,7 +50,7 @@ def test_parse_camera_info() -> None: def test_parse_hydra_data() -> None: - path = "test/assets/lbr_med7/zed2i" + path = "test/assets/lbr_med7_r800/samples" joint_states, masks, depths = parse_hydra_data( joint_states_files=find_files(path, "joint_states_*.npy"), mask_files=find_files(path, "mask_sam2_left_*.png"), @@ -70,7 +70,7 @@ def test_parse_hydra_data() -> None: def test_parse_mono_data() -> None: - path = "test/assets/lbr_med7/zed2i" + path = "test/assets/lbr_med7_r800/samples" images, joint_states, masks = parse_mono_data( image_files=find_files(path, "left_image_*.png"), joint_states_files=find_files(path, "joint_states_*.npy"), @@ -94,7 +94,7 @@ def test_parse_mono_data() -> None: def test_parse_stereo_data() -> None: - path = "test/assets/lbr_med7/zed2i" + path = "test/assets/lbr_med7_r800/samples" left_images, right_images, joint_states, left_masks, right_masks = ( parse_stereo_data( left_image_files=find_files(path, "left_image_*.png"), diff --git a/test/test_hydra_icp.py b/test/test_hydra_icp.py index e4b4ffb..3250a6f 100644 --- a/test/test_hydra_icp.py +++ b/test/test_hydra_icp.py @@ -111,7 +111,7 @@ def test_hydra_icp(): xacro_path = "urdf/med7/med7.xacro" root_link_name = "lbr_link_0" end_link_name = "lbr_link_7" - path = Path("test/assets/lbr_med7/zed2i") + path = Path("test/assets/lbr_med7_r800/samples") camera_info_file = "left_camera_info.yaml" joint_states_pattern = "joint_states_*.npy" mask_pattern = "mask_sam2_left_*.png" @@ -242,7 +242,7 @@ def test_hydra_robust_icp() -> None: xacro_path = "urdf/med7/med7.xacro" root_link_name = "lbr_link_0" end_link_name = "lbr_link_7" - path = Path("test/assets/lbr_med7/zed2i") + path = Path("test/assets/lbr_med7_r800/samples") camera_info_file = "left_camera_info.yaml" joint_states_pattern = "joint_states_*.npy" mask_pattern = "mask_sam2_left_*.png" diff --git a/test/test_segmentor.py b/test/test_segmentor.py index 7392ca7..70be24d 100644 --- a/test/test_segmentor.py +++ b/test/test_segmentor.py @@ -9,7 +9,7 @@ @pytest.mark.skip(reason="To be fixed.") def test_sam2_segmentor() -> None: - img = cv2.imread("test/assets/lbr_med7/zed2i/high_res/image_1.png") + img = cv2.imread("test/assets/lbr_med7_r800/samples/left_image_1.png") # detect detector = OpenCVDetector(n_positive_samples=5) # number of detected samples diff --git a/test/util/test_mask.py b/test/util/test_mask.py index 3e738b9..651573c 100644 --- a/test/util/test_mask.py +++ b/test/util/test_mask.py @@ -22,7 +22,7 @@ def test_dilate_with_kernel() -> None: idx = 1 mask = cv2.imread( - f"test/assets/lbr_med7/zed2i/mask_sam2_left_image_{idx}.png", + f"test/assets/lbr_med7_r800/samples/mask_sam2_left_image_{idx}.png", cv2.IMREAD_GRAYSCALE, ) dilated_mask = mask_dilate_with_kernel(mask) @@ -36,7 +36,7 @@ def test_dilate_with_kernel() -> None: def test_distance_transform() -> None: idx = 1 mask = cv2.imread( - f"test/assets/lbr_med7/zed2i/mask_sam2_left_image_{idx}.png", + f"test/assets/lbr_med7_r800/samples/mask_sam2_left_image_{idx}.png", cv2.IMREAD_GRAYSCALE, ) @@ -68,7 +68,7 @@ def test_distance_transform() -> None: def test_erode_with_kernel() -> None: idx = 1 mask = cv2.imread( - f"test/assets/lbr_med7/zed2i/mask_sam2_left_image_{idx}.png", + f"test/assets/lbr_med7_r800/samples/mask_sam2_left_image_{idx}.png", cv2.IMREAD_GRAYSCALE, ) eroded_mask = mask_erode_with_kernel(mask) @@ -82,7 +82,7 @@ def test_erode_with_kernel() -> None: def test_exponential_decay() -> None: idx = 1 mask = cv2.imread( - f"test/assets/lbr_med7/zed2i/mask_sam2_left_image_{idx}.png", + f"test/assets/lbr_med7_r800/samples/mask_sam2_left_image_{idx}.png", cv2.IMREAD_GRAYSCALE, ) exponential_decay = mask_exponential_decay(mask) @@ -95,9 +95,9 @@ def test_exponential_decay() -> None: @pytest.mark.skip(reason="To be fixed.") def test_extract_boundary() -> None: idx = 1 - img = cv2.imread(f"test/assets/lbr_med7/zed2i/left_image_{idx}.png") + img = cv2.imread(f"test/assets/lbr_med7_r800/samples/left_image_{idx}.png") mask = cv2.imread( - f"test/assets/lbr_med7/zed2i/mask_sam2_left_image_{idx}.png", + f"test/assets/lbr_med7_r800/samples/mask_sam2_left_image_{idx}.png", cv2.IMREAD_GRAYSCALE, ) boundary_mask = mask_extract_boundary(mask) @@ -112,9 +112,9 @@ def test_extract_boundary() -> None: @pytest.mark.skip(reason="To be fixed.") def test_extract_extended_boundary() -> None: idx = 1 - img = cv2.imread(f"test/assets/lbr_med7/zed2i/left_image_{idx}.png") + img = cv2.imread(f"test/assets/lbr_med7_r800/samples/left_image_{idx}.png") mask = cv2.imread( - f"test/assets/lbr_med7/zed2i/mask_sam2_left_image_{idx}.png", + f"test/assets/lbr_med7_r800/samples/mask_sam2_left_image_{idx}.png", cv2.IMREAD_GRAYSCALE, ) extended_boundary_mask = mask_extract_extended_boundary( diff --git a/test/util/test_viz.py b/test/util/test_viz.py index 2e34b65..cc5163f 100644 --- a/test/util/test_viz.py +++ b/test/util/test_viz.py @@ -10,7 +10,11 @@ @pytest.mark.skip(reason="To be fixed.") def test_visualize_point_cloud(): meshes = rrd.TorchMeshContainer( - meshes=load_meshes({"link_0": "test/assets/lbr_med7/mesh/link_0.stl"}), + meshes=load_meshes( + { + "link_0": "test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl" + } + ), device="cpu", ) @@ -32,8 +36,8 @@ def test_visalize_multi_color_point_cloud(): meshes = rrd.TorchMeshContainer( meshes=load_meshes( { - "link_0": "test/assets/lbr_med7/mesh/link_0.stl", - "link_1": "test/assets/lbr_med7/mesh/link_1.stl", + "link_0": "test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl", + "link_1": "test/assets/lbr_med7_r800/description/meshes/collision/link_1.stl", } ), device="cpu", From 571c7ef3d568a97ff4f07f1cabdde560a298ce23 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 1 Feb 2026 14:19:54 +0000 Subject: [PATCH 4/9] fixed paths type --- roboreg/io/parsers.py | 4 ++-- test/io/test_parsers.py | 7 ++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/roboreg/io/parsers.py b/roboreg/io/parsers.py index b915ac4..fe881f6 100644 --- a/roboreg/io/parsers.py +++ b/roboreg/io/parsers.py @@ -148,11 +148,11 @@ def mesh_uris( if collision: if link.collision is None: continue - paths[link_name] = Path(link.collision.geometry.filename) + paths[link_name] = link.collision.geometry.filename else: if link.visual is None: continue - paths[link_name] = Path(link.visual.geometry.filename) + paths[link_name] = link.visual.geometry.filename return paths def mesh_paths_from_ros_registry( diff --git a/test/io/test_parsers.py b/test/io/test_parsers.py index b1597cb..e88b534 100644 --- a/test/io/test_parsers.py +++ b/test/io/test_parsers.py @@ -17,7 +17,12 @@ def test_urdf_parser() -> None: urdf_parser = URDFParser.from_ros_xacro("lbr_description", "urdf/med7/med7.xacro") print(urdf_parser.chain_link_names("lbr_link_0", "lbr_link_ee")) - print(urdf_parser.raw_mesh_paths("lbr_link_0", "lbr_link_ee")) + print(urdf_parser.mesh_uris("lbr_link_0", "lbr_link_ee")) + print( + URDFParser.resolve_uris_via_ros_registry( + urdf_parser.mesh_uris("lbr_link_0", "lbr_link_ee") + ) + ) print(urdf_parser.mesh_paths_from_ros_registry("lbr_link_0", "lbr_link_ee")) print(urdf_parser.mesh_origins("lbr_link_0", "lbr_link_ee")) print(urdf_parser.link_names_with_meshes(collision=True)) From ab9c2413b2948558ebfef3760153efc832868e4b Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 2 Feb 2026 12:03:56 +0000 Subject: [PATCH 5/9] added relative uri resolve --- roboreg/io/parsers.py | 49 ++++++++++++++++--- .../{med7_r800.urdf => lbr_med7_r800.urdf} | 0 test/io/test_parsers.py | 48 ++++++++++++++++-- 3 files changed, 86 insertions(+), 11 deletions(-) rename test/assets/lbr_med7_r800/description/{med7_r800.urdf => lbr_med7_r800.urdf} (100%) diff --git a/roboreg/io/parsers.py b/roboreg/io/parsers.py index fe881f6..50705ab 100644 --- a/roboreg/io/parsers.py +++ b/roboreg/io/parsers.py @@ -16,11 +16,14 @@ def __init__(self, urdf: str) -> None: self._robot = urdf_parser_py.urdf.Robot.from_xml_string(urdf) @classmethod - def from_urdf_file(cls, path: Union[Path, str]) -> None: + def from_urdf_file(cls, path: Union[Path, str]) -> "URDFParser": r"""Instantiate URDF parser via path to URDF file. Args: path (Union[Path, str]): Path to URDF file. + + Returns: + URDFParser: A URDFParser instance. """ path = Path(path) if not path.exists(): @@ -34,12 +37,17 @@ def from_urdf_file(cls, path: Union[Path, str]) -> None: return cls(urdf=urdf) @classmethod - def from_ros_xacro(cls, ros_package: str, xacro_path: Union[Path, str]) -> None: + def from_ros_xacro( + cls, ros_package: str, xacro_path: Union[Path, str] + ) -> "URDFParser": r"""Instantiate URDF parser from ROS xacro file. Args: ros_package (str): Internally finds the path to ros_package. xacro_path (Union[Path,str]): Path to xacro file relative to ros_package. + + Returns: + URDFParser: A URDFParser instance. """ xacro_path = Path(xacro_path) if not xacro_path.suffix == ".xacro": @@ -51,7 +59,31 @@ def from_ros_xacro(cls, ros_package: str, xacro_path: Union[Path, str]) -> None: ) @staticmethod - def resolve_uris_via_ros_registry(uris: Dict[str, str]) -> Dict[str, Path]: + def resolve_relative_uris( + uris: Dict[str, str], base_path: Union[Path, str] + ) -> Dict[str, Path]: + r"""Resolve relative URIs using a base path. + + Args: + uris (Dict[str,str]): Dictionary of link names and relative mesh paths. + base_path (Union[Path,str]): Base path to resolve relative paths. + + Returns: + Dict[str,Path]: Dictionary of link names and absolute mesh paths. + """ + base_path = Path(base_path) + mesh_paths = {} + for link_name in uris.keys(): + uri = uris[link_name] + mesh_paths[link_name] = (base_path / Path(uri)).resolve() + if len(mesh_paths) != len(uris): + raise RuntimeError("Some mesh paths could not be resolved.") + if not all([path.exists() for path in mesh_paths.values()]): + raise FileNotFoundError("Some resolved mesh paths do not exist.") + return mesh_paths + + @staticmethod + def resolve_ros_registry_uris(uris: Dict[str, str]) -> Dict[str, Path]: r"""Resolve the URI-style package:// prefix using the ament_index_python ROS registry. Args: @@ -71,11 +103,12 @@ def resolve_uris_via_ros_registry(uris: Dict[str, str]) -> Dict[str, Path]: raise ValueError( f"Invalid package path {mesh_path} for link {link_name}." ) - mesh_paths[link_name] = Path( - get_package_share_directory(mesh_path.parts[0]) - ) / Path(*mesh_path.parts[1:]) + mesh_paths[link_name] = ( + Path(get_package_share_directory(mesh_path.parts[0])) + / Path(*mesh_path.parts[1:]) + ).resolve() else: - raise ValueError("Case unhandled.") + raise ValueError("Expected a package:// prefix.") if len(mesh_paths) != len(uris): raise RuntimeError("Some mesh paths could not be resolved.") if not all([path.exists() for path in mesh_paths.values()]): @@ -168,7 +201,7 @@ def mesh_paths_from_ros_registry( Returns: Dict[str,Path]: Dictionary of link names and absolute mesh paths. """ - return URDFParser.resolve_uris_via_ros_registry( + return URDFParser.resolve_ros_registry_uris( uris=self.mesh_uris( root_link_name=root_link_name, end_link_name=end_link_name, diff --git a/test/assets/lbr_med7_r800/description/med7_r800.urdf b/test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf similarity index 100% rename from test/assets/lbr_med7_r800/description/med7_r800.urdf rename to test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf diff --git a/test/io/test_parsers.py b/test/io/test_parsers.py index e88b534..b3a13ea 100644 --- a/test/io/test_parsers.py +++ b/test/io/test_parsers.py @@ -13,13 +13,54 @@ ) +def test_urdf_parser_from_urdf_file() -> None: + urdf_path = Path("test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf") + urdf_parser = URDFParser.from_urdf_file(path=urdf_path) + root_link_name = "lbr_link_0" + end_link_name = "lbr_link_ee" + chain_link_names = urdf_parser.chain_link_names( + root_link_name=root_link_name, end_link_name=end_link_name + ) + assert ( + chain_link_names[0] == root_link_name + ), f"Expected {root_link_name} root link, got {chain_link_names[0]}." + assert ( + chain_link_names[-1] == end_link_name + ), f"Expected {end_link_name} end link, got {chain_link_names[-1]}." + + # try resolve paths to meshes + mesh_uris = urdf_parser.mesh_uris( + root_link_name=root_link_name, end_link_name=end_link_name + ) + mesh_paths = URDFParser.resolve_relative_uris( + uris=mesh_uris, base_path=urdf_path.parent + ) + assert all( + [mesh_paths[link].exists() for link in mesh_paths] + ), "Expected all mesh paths to exist." + + # check origins + mesh_origins = urdf_parser.mesh_origins( + root_link_name=root_link_name, end_link_name=end_link_name + ) + assert all( + mesh_origins[link].shape == (4, 4) for link in mesh_origins + ), "Expected a 4x4 shape for each mesh origin." + for link in mesh_origins: + print(mesh_origins[link][3, :]) + assert all( + (mesh_origins[link][3, :] == [0.0, 0.0, 0.0, 1.0]).all() + for link in mesh_origins + ), "Expected homogeneous transforms for mesh origins." + + @pytest.mark.skip(reason="To be fixed.") -def test_urdf_parser() -> None: +def test_urdf_parser_from_ros_xacro() -> None: urdf_parser = URDFParser.from_ros_xacro("lbr_description", "urdf/med7/med7.xacro") print(urdf_parser.chain_link_names("lbr_link_0", "lbr_link_ee")) print(urdf_parser.mesh_uris("lbr_link_0", "lbr_link_ee")) print( - URDFParser.resolve_uris_via_ros_registry( + URDFParser.resolve_ros_registry_uris( urdf_parser.mesh_uris("lbr_link_0", "lbr_link_ee") ) ) @@ -155,7 +196,8 @@ def test_parse_stereo_data() -> None: os.environ["QT_QPA_PLATFORM"] = "offscreen" sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) - test_urdf_parser() + test_urdf_parser_from_urdf_file() + test_urdf_parser_from_ros_xacro() test_find_files() test_parse_camera_info() test_parse_hydra_data() From e91ab3661cb5bbc8106a8b5c9ef2f5401488f07d Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 2 Feb 2026 14:50:18 +0000 Subject: [PATCH 6/9] added a factory to VirtualCamera --- cli/rr_mono_dr.py | 5 +++-- cli/rr_render.py | 5 +++-- cli/rr_stereo_dr.py | 7 ++++--- roboreg/differentiable/scene.py | 30 +++++++++++++++--------------- roboreg/differentiable/structs.py | 24 ++++++++++++++++++++++++ roboreg/util/factories.py | 25 ++----------------------- test/differentiable/test_scene.py | 7 ++++--- 7 files changed, 55 insertions(+), 48 deletions(-) diff --git a/cli/rr_mono_dr.py b/cli/rr_mono_dr.py index 26b6301..47ca2af 100644 --- a/cli/rr_mono_dr.py +++ b/cli/rr_mono_dr.py @@ -10,10 +10,11 @@ import rich.progress import torch +from roboreg.differentiable import VirtualCamera from roboreg.io import find_files, parse_mono_data from roboreg.losses import soft_dice_loss from roboreg.util import mask_distance_transform, mask_exponential_decay, overlay_mask -from roboreg.util.factories import create_robot_scene, create_virtual_camera +from roboreg.util.factories import create_robot_scene class REGISTRATION_MODE(Enum): @@ -174,7 +175,7 @@ def main() -> None: # instantiate camera with default identity extrinsics because we optimize for robot pose instead camera = { - "camera": create_virtual_camera( + "camera": VirtualCamera.from_camera_configs( camera_info_file=args.camera_info_file, device=device, ) diff --git a/cli/rr_render.py b/cli/rr_render.py index 3f78e44..cc15f37 100644 --- a/cli/rr_render.py +++ b/cli/rr_render.py @@ -8,9 +8,10 @@ from rich import progress from torch.utils.data import DataLoader +from roboreg.differentiable import VirtualCamera from roboreg.io import MonocularDataset from roboreg.util import overlay_mask -from roboreg.util.factories import create_robot_scene, create_virtual_camera +from roboreg.util.factories import create_robot_scene def args_factory() -> argparse.Namespace: @@ -112,7 +113,7 @@ def main(): device = "cuda" if torch.cuda.is_available() else "cpu" os.environ["MAX_JOBS"] = str(args.max_jobs) # limit number of concurrent jobs camera = { - "camera": create_virtual_camera( + "camera": VirtualCamera.from_camera_configs( camera_info_file=args.camera_info_file, extrinsics_file=args.extrinsics_file, device=device, diff --git a/cli/rr_stereo_dr.py b/cli/rr_stereo_dr.py index 442046b..ef69c49 100644 --- a/cli/rr_stereo_dr.py +++ b/cli/rr_stereo_dr.py @@ -10,10 +10,11 @@ import rich.progress import torch +from roboreg.differentiable import VirtualCamera from roboreg.io import find_files, parse_stereo_data from roboreg.losses import soft_dice_loss from roboreg.util import mask_distance_transform, mask_exponential_decay, overlay_mask -from roboreg.util.factories import create_robot_scene, create_virtual_camera +from roboreg.util.factories import create_robot_scene class REGISTRATION_MODE(Enum): @@ -217,11 +218,11 @@ def main() -> None: # - left camera with default identity extrinsics because we optimize for robot pose instead # - right camera with transformation to left camera frame cameras = { - "left": create_virtual_camera( + "left": VirtualCamera.from_camera_configs( camera_info_file=args.left_camera_info_file, device=device, ), - "right": create_virtual_camera( + "right": VirtualCamera.from_camera_configs( camera_info_file=args.right_camera_info_file, extrinsics_file=args.right_extrinsics_file, device=device, diff --git a/roboreg/differentiable/scene.py b/roboreg/differentiable/scene.py index cfbbce6..a5238f9 100644 --- a/roboreg/differentiable/scene.py +++ b/roboreg/differentiable/scene.py @@ -25,21 +25,6 @@ def __init__( self._renderer = renderer self._verify_devices() - def _verify_devices(self) -> None: - for camera_name in self._cameras.keys(): - if not all( - [ - self._cameras[camera_name].device == self._robot.device, - self._robot.device == self._renderer.device, - ] - ): - raise ValueError( - "All devices must be the same. Got:\n" - f"Camera '{camera_name}' on: {self._cameras[camera_name].device}\n" - f"Robot on: {self._robot.device}\n" - f"Renderer on: {self._renderer.device}" - ) - def observe_from( self, camera_name: str, reference_transform: torch.FloatTensor = None ) -> torch.Tensor: @@ -70,6 +55,21 @@ def observe_from( self._cameras[camera_name].resolution, ) + def _verify_devices(self) -> None: + for camera_name in self._cameras.keys(): + if not all( + [ + self._cameras[camera_name].device == self._robot.device, + self._robot.device == self._renderer.device, + ] + ): + raise ValueError( + "All devices must be the same. Got:\n" + f"Camera '{camera_name}' on: {self._cameras[camera_name].device}\n" + f"Robot on: {self._robot.device}\n" + f"Renderer on: {self._renderer.device}" + ) + @property def cameras(self) -> Dict[str, VirtualCamera]: return self._cameras diff --git a/roboreg/differentiable/structs.py b/roboreg/differentiable/structs.py index 1eaf0ec..cc95da1 100644 --- a/roboreg/differentiable/structs.py +++ b/roboreg/differentiable/structs.py @@ -1,5 +1,6 @@ import abc from collections import OrderedDict +from pathlib import Path from typing import Dict, List, Optional, Tuple, Union import numpy as np @@ -347,6 +348,29 @@ def __init__( self._perspective_projection[..., 2, 3] = 2.0 * zmax * zmin / (zmin - zmax) self._perspective_projection[..., 3, 2] = 1.0 + @classmethod + def from_camera_configs( + cls, + camera_info_file: Union[Path, str], + extrinsics_file: Optional[Union[Path, str]] = None, + device: Union[torch.device, str] = "cuda", + ) -> "VirtualCamera": + from roboreg.io.parsers import parse_camera_info + + camera_info_file = Path(camera_info_file) + + height, width, intrinsics = parse_camera_info(camera_info_file=camera_info_file) + extrinsics = None + if extrinsics_file is not None: + extrinsics_file = Path(extrinsics_file) + extrinsics = np.load(extrinsics_file) + return cls( + resolution=[height, width], + intrinsics=intrinsics, + extrinsics=extrinsics, + device=device, + ) + def to(self, device: Union[torch.device, str]) -> None: self._perspective_projection = self._perspective_projection.to(device=device) super().to(device=device) diff --git a/roboreg/util/factories.py b/roboreg/util/factories.py index cf56fe4..159f110 100644 --- a/roboreg/util/factories.py +++ b/roboreg/util/factories.py @@ -1,31 +1,10 @@ -from typing import Dict, Optional +from typing import Dict -import numpy as np import rich import torch from roboreg.differentiable import NVDiffRastRenderer, Robot, RobotScene, VirtualCamera -from roboreg.io import URDFParser, parse_camera_info - - -def create_virtual_camera( - camera_info_file: str, - extrinsics_file: Optional[str] = None, - device: torch.device = ( - torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu") - ), -) -> VirtualCamera: - height, width, intrinsics = parse_camera_info(camera_info_file=camera_info_file) - extrinsics = None - if extrinsics_file is not None: - extrinsics = np.load(extrinsics_file) - camera = VirtualCamera( - resolution=[height, width], - intrinsics=intrinsics, - extrinsics=extrinsics, - device=device, - ) - return camera +from roboreg.io import URDFParser def create_robot_scene( diff --git a/test/differentiable/test_scene.py b/test/differentiable/test_scene.py index 9692436..b1a67da 100644 --- a/test/differentiable/test_scene.py +++ b/test/differentiable/test_scene.py @@ -11,9 +11,10 @@ import torch from tqdm import tqdm +from roboreg.differentiable import VirtualCamera from roboreg.io import find_files from roboreg.util import overlay_mask -from roboreg.util.factories import create_robot_scene, create_virtual_camera +from roboreg.util.factories import create_robot_scene class TestScene: @@ -90,7 +91,7 @@ def __init__( # instantiate cameras cameras = { - camera_name: create_virtual_camera( + camera_name: VirtualCamera.from_camera_configs( camera_info_file=camera_info_files[camera_name], extrinsics_file=extrinsics_files[camera_name], device=device, @@ -235,7 +236,7 @@ def test_single_camera_multiple_poses() -> None: batch_size = 4 camera_name = "camera" camera = { - camera_name: create_virtual_camera( + camera_name: VirtualCamera.from_camera_configs( camera_info_file="test/assets/lbr_med7_r800/samples/left_camera_info.yaml", extrinsics_file="test/assets/lbr_med7_r800/samples/HT_hydra_robust.npy", device=device, From 1b79b73ec936989e92201025ad514a7c66e0726c Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 3 Feb 2026 15:43:27 +0000 Subject: [PATCH 7/9] robot/scene instantiation refactor (#110) --- cli/rr_cam_swarm.py | 48 +++-- cli/rr_hydra.py | 56 +++--- cli/rr_mono_dr.py | 39 +++- cli/rr_render.py | 37 +++- cli/rr_stereo_dr.py | 41 ++++- roboreg/{differentiable => core}/__init__.py | 0 .../{differentiable => core}/kinematics.py | 0 roboreg/{differentiable => core}/rendering.py | 0 roboreg/{differentiable => core}/robot.py | 59 +----- roboreg/{differentiable => core}/scene.py | 0 roboreg/{differentiable => core}/structs.py | 0 roboreg/io/__init__.py | 1 + roboreg/io/robot_data.py | 156 ++++++++++++++++ roboreg/util/factories.py | 57 ------ .../test_kinematics.py | 6 +- .../test_rendering.py | 33 ++-- test/core/test_robot.py | 51 +++++ test/{differentiable => core}/test_scene.py | 174 ++++++++++-------- test/{differentiable => core}/test_structs.py | 2 +- test/differentiable/test_robot.py | 38 ---- test/test_hydra_icp.py | 2 +- test/util/test_viz.py | 12 +- 22 files changed, 493 insertions(+), 319 deletions(-) rename roboreg/{differentiable => core}/__init__.py (100%) rename roboreg/{differentiable => core}/kinematics.py (100%) rename roboreg/{differentiable => core}/rendering.py (100%) rename roboreg/{differentiable => core}/robot.py (61%) rename roboreg/{differentiable => core}/scene.py (100%) rename roboreg/{differentiable => core}/structs.py (100%) create mode 100644 roboreg/io/robot_data.py delete mode 100644 roboreg/util/factories.py rename test/{differentiable => core}/test_kinematics.py (96%) rename test/{differentiable => core}/test_rendering.py (95%) create mode 100644 test/core/test_robot.py rename test/{differentiable => core}/test_scene.py (67%) rename test/{differentiable => core}/test_structs.py (98%) delete mode 100644 test/differentiable/test_robot.py diff --git a/cli/rr_cam_swarm.py b/cli/rr_cam_swarm.py index aa47e86..bfbdcf6 100644 --- a/cli/rr_cam_swarm.py +++ b/cli/rr_cam_swarm.py @@ -6,8 +6,20 @@ import numpy as np import torch -from roboreg import differentiable as rrd -from roboreg.io import URDFParser, find_files, parse_camera_info, parse_mono_data +from roboreg.core import ( + NVDiffRastRenderer, + Robot, + RobotScene, + TorchKinematics, + TorchMeshContainer, + VirtualCamera, +) +from roboreg.io import ( + find_files, + load_robot_data_from_ros_xacro, + parse_camera_info, + parse_mono_data, +) from roboreg.losses import soft_dice_loss from roboreg.optim import LinearParticleSwarm, ParticleSwarmOptimizer from roboreg.util import ( @@ -295,31 +307,43 @@ def main() -> None: n_joint_states * args.n_cameras ) # (each camera observes n_joint_states joint states) camera_name = "camera" - camera = rrd.VirtualCamera( + camera = VirtualCamera( resolution=(height, width), intrinsics=intrinsics, extrinsics=torch.eye(4, device=device).unsqueeze(0).expand(batch_size, -1, -1), device=device, ) - urdf_parser = URDFParser.from_ros_xacro( - ros_package=args.ros_package, xacro_path=args.xacro_path - ) - robot = rrd.Robot.from_urdf_parser( - urdf_parser=urdf_parser, + # instantiate robot + robot_data = load_robot_data_from_ros_xacro( + ros_package=args.ros_package, + xacro_path=args.xacro_path, root_link_name=args.root_link_name, end_link_name=args.end_link_name, collision=args.collision_meshes, + target_reduction=args.target_reduction, + ) + mesh_container = TorchMeshContainer( + meshes=robot_data.meshes, batch_size=batch_size, device=device, - target_reduction=args.target_reduction, # reduce mesh vertex count for memory reduction + ) + kinematics = TorchKinematics( + urdf=robot_data.urdf, + root_link_name=robot_data.root_link_name, + end_link_name=robot_data.end_link_name, + device=device, + ) + robot = Robot( + mesh_container=mesh_container, + kinematics=kinematics, ) - renderer = rrd.NVDiffRastRenderer(device=device) - scene = rrd.RobotScene( + # instantiate scene + scene = RobotScene( cameras={camera_name: camera}, robot=robot, - renderer=renderer, + renderer=NVDiffRastRenderer(device=device), ) # repeat joint states and masks for each camera diff --git a/cli/rr_hydra.py b/cli/rr_hydra.py index d0e1611..4561167 100644 --- a/cli/rr_hydra.py +++ b/cli/rr_hydra.py @@ -2,12 +2,16 @@ import os import numpy as np -import rich import torch -from roboreg.differentiable import Robot +from roboreg.core import Robot, TorchKinematics, TorchMeshContainer from roboreg.hydra_icp import hydra_centroid_alignment, hydra_robust_icp -from roboreg.io import URDFParser, find_files, parse_camera_info, parse_hydra_data +from roboreg.io import ( + find_files, + load_robot_data_from_ros_xacro, + parse_camera_info, + parse_hydra_data, +) from roboreg.util import ( RegistrationVisualizer, clean_xyz, @@ -161,35 +165,29 @@ def main(): ) height, width, intrinsics = parse_camera_info(args.camera_info_file) - # instantiate kinematics - urdf_parser = URDFParser.from_ros_xacro( - ros_package=args.ros_package, xacro_path=args.xacro_path - ) - root_link_name = args.root_link_name - end_link_name = args.end_link_name - if root_link_name == "": - root_link_name = urdf_parser.link_names_with_meshes( - collision=args.collision_meshes - )[0] - rich.print( - f"Root link name not provided. Using the first link with mesh: '{root_link_name}'." - ) - if end_link_name == "": - end_link_name = urdf_parser.link_names_with_meshes( - collision=args.collision_meshes - )[-1] - rich.print( - f"End link name not provided. Using the last link with mesh: '{end_link_name}'." - ) - # instantiate robot batch_size = len(joint_states) - robot = Robot.from_urdf_parser( - urdf_parser=urdf_parser, - root_link_name=root_link_name, - end_link_name=end_link_name, + robot_data = load_robot_data_from_ros_xacro( + ros_package=args.ros_package, + xacro_path=args.xacro_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, collision=args.collision_meshes, - batch_size=batch_size, + ) + mesh_container = TorchMeshContainer( + meshes=robot_data.meshes, + batch_size=len(joint_states), + device=device, + ) + kinematics = TorchKinematics( + urdf=robot_data.urdf, + root_link_name=robot_data.root_link_name, + end_link_name=robot_data.end_link_name, + device=device, + ) + robot = Robot( + mesh_container=mesh_container, + kinematics=kinematics, ) # perform forward kinematics diff --git a/cli/rr_mono_dr.py b/cli/rr_mono_dr.py index 47ca2af..e758ef9 100644 --- a/cli/rr_mono_dr.py +++ b/cli/rr_mono_dr.py @@ -10,11 +10,17 @@ import rich.progress import torch -from roboreg.differentiable import VirtualCamera -from roboreg.io import find_files, parse_mono_data +from roboreg.core import ( + NVDiffRastRenderer, + Robot, + RobotScene, + TorchKinematics, + TorchMeshContainer, + VirtualCamera, +) +from roboreg.io import find_files, load_robot_data_from_ros_xacro, parse_mono_data from roboreg.losses import soft_dice_loss from roboreg.util import mask_distance_transform, mask_exponential_decay, overlay_mask -from roboreg.util.factories import create_robot_scene class REGISTRATION_MODE(Enum): @@ -181,17 +187,36 @@ def main() -> None: ) } - # instantiate robot scene - scene = create_robot_scene( - batch_size=joint_states.shape[0], + # instantiate robot + robot_data = load_robot_data_from_ros_xacro( ros_package=args.ros_package, xacro_path=args.xacro_path, root_link_name=args.root_link_name, end_link_name=args.end_link_name, collision=args.collision_meshes, - cameras=camera, + ) + mesh_container = TorchMeshContainer( + meshes=robot_data.meshes, + batch_size=joint_states.shape[0], + device=device, + ) + kinematics = TorchKinematics( + urdf=robot_data.urdf, + root_link_name=robot_data.root_link_name, + end_link_name=robot_data.end_link_name, device=device, ) + robot = Robot( + mesh_container=mesh_container, + kinematics=kinematics, + ) + + # instantiate scene + scene = RobotScene( + cameras=camera, + robot=robot, + renderer=NVDiffRastRenderer(device=device), + ) # load extrinsics estimate extrinsics = torch.tensor( diff --git a/cli/rr_render.py b/cli/rr_render.py index cc15f37..a5dc77b 100644 --- a/cli/rr_render.py +++ b/cli/rr_render.py @@ -8,10 +8,16 @@ from rich import progress from torch.utils.data import DataLoader -from roboreg.differentiable import VirtualCamera -from roboreg.io import MonocularDataset +from roboreg.core import ( + NVDiffRastRenderer, + Robot, + RobotScene, + TorchKinematics, + TorchMeshContainer, + VirtualCamera, +) +from roboreg.io import MonocularDataset, load_robot_data_from_ros_xacro from roboreg.util import overlay_mask -from roboreg.util.factories import create_robot_scene def args_factory() -> argparse.Namespace: @@ -119,16 +125,33 @@ def main(): device=device, ) } - scene = create_robot_scene( - batch_size=args.batch_size, + robot_data = load_robot_data_from_ros_xacro( ros_package=args.ros_package, xacro_path=args.xacro_path, root_link_name=args.root_link_name, end_link_name=args.end_link_name, - cameras=camera, - device=device, collision=args.collision_meshes, ) + mesh_container = TorchMeshContainer( + meshes=robot_data.meshes, + batch_size=args.batch_size, + device=device, + ) + kinematics = TorchKinematics( + urdf=robot_data.urdf, + root_link_name=robot_data.root_link_name, + end_link_name=robot_data.end_link_name, + device=device, + ) + robot = Robot( + mesh_container=mesh_container, + kinematics=kinematics, + ) + scene = RobotScene( + cameras=camera, + robot=robot, + renderer=NVDiffRastRenderer(device=device), + ) dataset = MonocularDataset( images_path=args.images_path, image_pattern=args.image_pattern, diff --git a/cli/rr_stereo_dr.py b/cli/rr_stereo_dr.py index ef69c49..8b032f3 100644 --- a/cli/rr_stereo_dr.py +++ b/cli/rr_stereo_dr.py @@ -10,11 +10,17 @@ import rich.progress import torch -from roboreg.differentiable import VirtualCamera -from roboreg.io import find_files, parse_stereo_data +from roboreg.core import ( + NVDiffRastRenderer, + Robot, + RobotScene, + TorchKinematics, + TorchMeshContainer, + VirtualCamera, +) +from roboreg.io import find_files, load_robot_data_from_ros_xacro, parse_stereo_data from roboreg.losses import soft_dice_loss from roboreg.util import mask_distance_transform, mask_exponential_decay, overlay_mask -from roboreg.util.factories import create_robot_scene class REGISTRATION_MODE(Enum): @@ -229,17 +235,36 @@ def main() -> None: ), } - # instantiate robot scene - scene = create_robot_scene( - batch_size=joint_states.shape[0], + # instantiate robot + robot_data = load_robot_data_from_ros_xacro( ros_package=args.ros_package, xacro_path=args.xacro_path, root_link_name=args.root_link_name, end_link_name=args.end_link_name, - cameras=cameras, - device=device, collision=args.collision_meshes, ) + mesh_container = TorchMeshContainer( + meshes=robot_data.meshes, + batch_size=joint_states.shape[0], + device=device, + ) + kinematics = TorchKinematics( + urdf=robot_data.urdf, + root_link_name=robot_data.root_link_name, + end_link_name=robot_data.end_link_name, + device=device, + ) + robot = Robot( + mesh_container=mesh_container, + kinematics=kinematics, + ) + + # instantiate scene + scene = RobotScene( + cameras=cameras, + robot=robot, + renderer=NVDiffRastRenderer(device=device), + ) # load extrinscis estimate...... left_extrinsics = torch.tensor( diff --git a/roboreg/differentiable/__init__.py b/roboreg/core/__init__.py similarity index 100% rename from roboreg/differentiable/__init__.py rename to roboreg/core/__init__.py diff --git a/roboreg/differentiable/kinematics.py b/roboreg/core/kinematics.py similarity index 100% rename from roboreg/differentiable/kinematics.py rename to roboreg/core/kinematics.py diff --git a/roboreg/differentiable/rendering.py b/roboreg/core/rendering.py similarity index 100% rename from roboreg/differentiable/rendering.py rename to roboreg/core/rendering.py diff --git a/roboreg/differentiable/robot.py b/roboreg/core/robot.py similarity index 61% rename from roboreg/differentiable/robot.py rename to roboreg/core/robot.py index 06ce7d4..b2ab4ad 100644 --- a/roboreg/differentiable/robot.py +++ b/roboreg/core/robot.py @@ -2,8 +2,6 @@ import torch -from roboreg.io import URDFParser - from .kinematics import TorchKinematics from .structs import TorchMeshContainer @@ -15,62 +13,15 @@ def __init__( self, mesh_container: TorchMeshContainer, kinematics: TorchKinematics, - device: Union[torch.device, str] = "cuda", ) -> None: self._mesh_container = mesh_container self._kinematics = kinematics self._configured_vertices = self.mesh_container.vertices.clone() - self._device = torch.device(device) if isinstance(device, str) else device - self.to(device=self._device) - - @classmethod - def from_urdf_parser( - cls, - urdf_parser: URDFParser, - root_link_name: str, - end_link_name: str, - collision: bool = False, - batch_size: int = 1, - device: Union[torch.device, str] = "cuda", - target_reduction: float = 0.0, - ) -> "Robot": - from roboreg.io import apply_mesh_origins, load_meshes, simplify_meshes - - # parse data from URDF - mesh_paths = urdf_parser.mesh_paths_from_ros_registry( - root_link_name=root_link_name, - end_link_name=end_link_name, - collision=collision, - ) - mesh_origins = urdf_parser.mesh_origins( - root_link_name=root_link_name, - end_link_name=end_link_name, - collision=collision, - ) - - # load and preprocess meshes - meshes = load_meshes(paths=mesh_paths) - meshes = simplify_meshes( - meshes=meshes, - target_reduction=target_reduction, - ) - meshes = apply_mesh_origins(meshes=meshes, origins=mesh_origins) - - # configure this robot - mesh_container = TorchMeshContainer( - meshes=meshes, - batch_size=batch_size, - device=device, - ) - - kinematics = TorchKinematics( - urdf=urdf_parser.urdf, - root_link_name=root_link_name, - end_link_name=end_link_name, - device=device, - ) - - return cls(mesh_container=mesh_container, kinematics=kinematics, device=device) + if mesh_container.device != kinematics.device: + raise ValueError( + "Mesh container and kinematics must be on the same device." + ) + self._device = mesh_container.device def configure( self, q: torch.FloatTensor, ht_root: torch.FloatTensor = None diff --git a/roboreg/differentiable/scene.py b/roboreg/core/scene.py similarity index 100% rename from roboreg/differentiable/scene.py rename to roboreg/core/scene.py diff --git a/roboreg/differentiable/structs.py b/roboreg/core/structs.py similarity index 100% rename from roboreg/differentiable/structs.py rename to roboreg/core/structs.py diff --git a/roboreg/io/__init__.py b/roboreg/io/__init__.py index e5088be..b198eae 100644 --- a/roboreg/io/__init__.py +++ b/roboreg/io/__init__.py @@ -2,3 +2,4 @@ from .filesystem import * from .meshes import * from .parsers import * +from .robot_data import * diff --git a/roboreg/io/robot_data.py b/roboreg/io/robot_data.py new file mode 100644 index 0000000..6e49c84 --- /dev/null +++ b/roboreg/io/robot_data.py @@ -0,0 +1,156 @@ +from dataclasses import dataclass +from pathlib import Path +from typing import Dict, Union + +import rich + +from roboreg.io import ( + Mesh, + URDFParser, + apply_mesh_origins, + load_meshes, + simplify_meshes, +) + + +@dataclass +class RobotData: + r"""Data needed to construct a Robot.""" + + meshes: Dict[str, Mesh] + urdf: str + root_link_name: str + end_link_name: str + + +def load_robot_data_from_ros_xacro( + ros_package: str, + xacro_path: Union[Path, str], + root_link_name: str = "", + end_link_name: str = "", + collision: bool = False, + target_reduction: float = 0.0, +) -> RobotData: + r"""Load data to construct a robot from a ROS xacro file. + + Args: + ros_package (str): ROS package containing the xacro file. + xacro_path (Union[Path, str]): The xacro path relative to the ros_package. + root_link_name (str): The root link name of the robot Defaults to the first link with a mesh. + end_link_name (str): The end link name of the robot Defaults to the last link with a mesh. + collision (bool): Whether to load collision meshes. Defaults to False. + target_reduction (float): Mesh simplification in [0, 1]. Defaults to 0.0 (no simplification). + + Returns: + RobotData: Data for constructing a Robot. + """ + # create a URDF parser + urdf_parser = URDFParser.from_ros_xacro( + ros_package=ros_package, xacro_path=xacro_path + ) + + if root_link_name == "": + root_link_name = urdf_parser.link_names_with_meshes(collision=collision)[0] + rich.print( + f"Root link name not provided. Using the first link with mesh: '{root_link_name}'." + ) + if end_link_name == "": + end_link_name = urdf_parser.link_names_with_meshes(collision=collision)[-1] + rich.print( + f"End link name not provided. Using the last link with mesh: '{end_link_name}'." + ) + + # parse data from URDF + mesh_paths = urdf_parser.mesh_paths_from_ros_registry( + root_link_name=root_link_name, + end_link_name=end_link_name, + collision=collision, + ) + + mesh_origins = urdf_parser.mesh_origins( + root_link_name=root_link_name, + end_link_name=end_link_name, + collision=collision, + ) + + # load and preprocess meshes + meshes = load_meshes(paths=mesh_paths) + meshes = simplify_meshes( + meshes=meshes, + target_reduction=target_reduction, + ) + meshes = apply_mesh_origins(meshes=meshes, origins=mesh_origins) + + return RobotData( + meshes=meshes, + urdf=urdf_parser.urdf, + root_link_name=root_link_name, + end_link_name=end_link_name, + ) + + +def load_robot_data_from_urdf_file( + urdf_path: Union[Path, str], + root_link_name: str = "", + end_link_name: str = "", + collision: bool = False, + target_reduction: float = 0.0, +) -> RobotData: + r"""Load data to construct a robot from a URDF file. + + Args: + urdf_path (Union[Path, str]): The path to the URDF file. Meshes are resolved relative to this path. + root_link_name (str): The root link name of the robot Defaults to the first link with a mesh. + end_link_name (str): The end link name of the robot Defaults to the last link with a mesh. + collision (bool): Whether to load collision meshes. Defaults to False. + target_reduction (float): Mesh simplification in [0, 1]. Defaults to 0.0 (no simplification). + + Returns: + RobotData: Data for constructing a Robot. + """ + urdf_path = Path(urdf_path) + + # create a URDF parser + urdf_parser = URDFParser.from_urdf_file(path=urdf_path) + + if root_link_name == "": + root_link_name = urdf_parser.link_names_with_meshes(collision=collision)[0] + rich.print( + f"Root link name not provided. Using the first link with mesh: '{root_link_name}'." + ) + if end_link_name == "": + end_link_name = urdf_parser.link_names_with_meshes(collision=collision)[-1] + rich.print( + f"End link name not provided. Using the last link with mesh: '{end_link_name}'." + ) + + # parse data from URDF + mesh_uris = urdf_parser.mesh_uris( + root_link_name=root_link_name, + end_link_name=end_link_name, + collision=collision, + ) + mesh_paths = urdf_parser.resolve_relative_uris( + uris=mesh_uris, base_path=urdf_path.parent + ) + + mesh_origins = urdf_parser.mesh_origins( + root_link_name=root_link_name, + end_link_name=end_link_name, + collision=collision, + ) + + # load and preprocess meshes + meshes = load_meshes(paths=mesh_paths) + meshes = simplify_meshes( + meshes=meshes, + target_reduction=target_reduction, + ) + meshes = apply_mesh_origins(meshes=meshes, origins=mesh_origins) + + return RobotData( + meshes=meshes, + urdf=urdf_parser.urdf, + root_link_name=root_link_name, + end_link_name=end_link_name, + ) diff --git a/roboreg/util/factories.py b/roboreg/util/factories.py deleted file mode 100644 index 159f110..0000000 --- a/roboreg/util/factories.py +++ /dev/null @@ -1,57 +0,0 @@ -from typing import Dict - -import rich -import torch - -from roboreg.differentiable import NVDiffRastRenderer, Robot, RobotScene, VirtualCamera -from roboreg.io import URDFParser - - -def create_robot_scene( - batch_size: int, - ros_package: str, - xacro_path: str, - root_link_name: str, - end_link_name: str, - cameras: Dict[str, VirtualCamera], - device: torch.device = ( - torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu") - ), - collision: bool = False, - target_reduction: float = 0.0, -) -> RobotScene: - # create URDF parser - urdf_parser = URDFParser.from_ros_xacro( - ros_package=ros_package, xacro_path=xacro_path - ) - if root_link_name == "": - root_link_name = urdf_parser.link_names_with_meshes(collision=collision)[0] - rich.print( - f"Root link name not provided. Using the first link with mesh: '{root_link_name}'." - ) - if end_link_name == "": - end_link_name = urdf_parser.link_names_with_meshes(collision=collision)[-1] - rich.print( - f"End link name not provided. Using the last link with mesh: '{end_link_name}'." - ) - - # instantiate robot - robot = Robot.from_urdf_parser( - urdf_parser=urdf_parser, - root_link_name=root_link_name, - end_link_name=end_link_name, - collision=collision, - batch_size=batch_size, - device=device, - target_reduction=target_reduction, - ) - - # instantiate renderer - renderer = NVDiffRastRenderer(device=device) - - # instantiate and return scene - return RobotScene( - cameras=cameras, - robot=robot, - renderer=renderer, - ) diff --git a/test/differentiable/test_kinematics.py b/test/core/test_kinematics.py similarity index 96% rename from test/differentiable/test_kinematics.py rename to test/core/test_kinematics.py index 183f341..2e00d75 100644 --- a/test/differentiable/test_kinematics.py +++ b/test/core/test_kinematics.py @@ -5,10 +5,8 @@ import transformations as tf from tqdm import tqdm -from roboreg.differentiable.kinematics import TorchKinematics -from roboreg.differentiable.rendering import NVDiffRastRenderer -from roboreg.differentiable.structs import TorchMeshContainer -from roboreg.io import URDFParser, load_meshes, apply_mesh_origins +from roboreg.core import NVDiffRastRenderer, TorchKinematics, TorchMeshContainer +from roboreg.io import URDFParser, apply_mesh_origins, load_meshes from roboreg.util import from_homogeneous diff --git a/test/differentiable/test_rendering.py b/test/core/test_rendering.py similarity index 95% rename from test/differentiable/test_rendering.py rename to test/core/test_rendering.py index 5ee3f06..30d1b1f 100644 --- a/test/differentiable/test_rendering.py +++ b/test/core/test_rendering.py @@ -13,7 +13,12 @@ import transformations as tf from tqdm import tqdm -from roboreg import differentiable as rrd +from roboreg.core import ( + NVDiffRastRenderer, + TorchKinematics, + TorchMeshContainer, + VirtualCamera, +) from roboreg.io import URDFParser, find_files, parse_camera_info from roboreg.util import overlay_mask @@ -67,7 +72,7 @@ def __init__( ) # instantiate meshes - self.meshes = rrd.TorchMeshContainer( + self.meshes = TorchMeshContainer( mesh_paths=self.urdf_parser.mesh_paths_from_ros_registry( self.root_link_name, self.end_link_name ), @@ -76,7 +81,7 @@ def __init__( ) # instantiante kinematics - self.kinematics = rrd.TorchKinematics( + self.kinematics = TorchKinematics( urdf_parser=self.urdf_parser, root_link_name=self.root_link_name, end_link_name=self.end_link_name, @@ -90,7 +95,7 @@ def __init__( # instantiate camera and renderer self.ht_base_cam = np.load(os.path.join(prefix, "HT_hydra_robust.npy")) - self.renderer = rrd.NVDiffRastRenderer(device=self.device) + self.renderer = NVDiffRastRenderer(device=self.device) @pytest.mark.skip(reason="To be fixed.") @@ -109,7 +114,7 @@ def test_nvdiffrast_unit() -> None: ) faces = torch.tensor([[0, 1, 2]], dtype=torch.int32, device=device) - renderer = rrd.NVDiffRastRenderer() + renderer = NVDiffRastRenderer() render = renderer.constant_color(vertices, faces, [256, 256]) cv2.imshow("render", render.cpu().numpy().squeeze()) @@ -151,7 +156,7 @@ def test_single_view_rendering() -> None: ) # create a virtual camera - camera = rrd.VirtualCamera( + camera = VirtualCamera( resolution=[test_rendering.height, test_rendering.width], intrinsics=test_rendering.intrinsics, extrinsics=test_rendering.ht_base_cam, @@ -200,7 +205,7 @@ def test_single_config_single_view_pose_optimization() -> None: test_rendering.meshes.transform_mesh(ht, link_name) # create differentiable camera and initialize extrinsics - camera = rrd.VirtualCamera( + camera = VirtualCamera( resolution=[test_rendering.height, test_rendering.width], intrinsics=test_rendering.intrinsics, extrinsics=torch.tensor( @@ -268,7 +273,7 @@ def test_multi_config_single_view_rendering() -> None: ) # overwrite meshes with batch size - test_rendering.meshes = rrd.TorchMeshContainer( + test_rendering.meshes = TorchMeshContainer( mesh_paths=test_rendering.urdf_parser.mesh_paths_from_ros_registry( test_rendering.root_link_name, test_rendering.end_link_name ), @@ -284,7 +289,7 @@ def test_multi_config_single_view_rendering() -> None: test_rendering.meshes.transform_mesh(ht, link_name) # create a virtual camera - camera = rrd.VirtualCamera( + camera = VirtualCamera( resolution=[test_rendering.height, test_rendering.width], intrinsics=test_rendering.intrinsics, extrinsics=test_rendering.ht_base_cam, @@ -330,7 +335,7 @@ def test_multi_config_single_view_pose_optimization() -> None: ) # overwrite meshes with batch size - test_rendering.meshes = rrd.TorchMeshContainer( + test_rendering.meshes = TorchMeshContainer( mesh_paths=test_rendering.urdf_parser.mesh_paths_from_ros_registry( test_rendering.root_link_name, test_rendering.end_link_name ), @@ -345,7 +350,7 @@ def test_multi_config_single_view_pose_optimization() -> None: test_rendering.meshes.transform_mesh(ht, link_name) # create differentiable camera and initialize extrinsics - camera = rrd.VirtualCamera( + camera = VirtualCamera( resolution=[test_rendering.height, test_rendering.width], intrinsics=test_rendering.intrinsics, extrinsics=torch.tensor( @@ -406,7 +411,7 @@ def test_multi_camera_pose_rendering() -> None: batch_size = 2 # render 2 cameras at once # load a sample mesh - meshes = rrd.TorchMeshContainer( + meshes = TorchMeshContainer( {"link_0": "test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl"}, batch_size=batch_size, device=device, @@ -437,12 +442,12 @@ def test_multi_camera_pose_rendering() -> None: print(extrinsics[0]) - batched_camera = rrd.VirtualCamera( + batched_camera = VirtualCamera( resolution=resolution, intrinsics=intrinsics, extrinsics=extrinsics ) # renderer - renderer = rrd.NVDiffRastRenderer(device=device) + renderer = NVDiffRastRenderer(device=device) # project meshes (knowing that intrinsics are identity) vertices = meshes.vertices.clone() diff --git a/test/core/test_robot.py b/test/core/test_robot.py new file mode 100644 index 0000000..4a4c4e0 --- /dev/null +++ b/test/core/test_robot.py @@ -0,0 +1,51 @@ +import torch + +from roboreg.core import Robot, TorchKinematics, TorchMeshContainer +from roboreg.io import load_robot_data_from_urdf_file + + +def test_robot() -> None: + batch_size = 2 + device = "cuda" if torch.cuda.is_available() else "cpu" + robot_data = load_robot_data_from_urdf_file( + urdf_path="test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf", + collision=True, + ) + + mesh_container = TorchMeshContainer( + meshes=robot_data.meshes, + batch_size=batch_size, + device=device, + ) + kinematics = TorchKinematics( + urdf=robot_data.urdf, + root_link_name=robot_data.root_link_name, + end_link_name=robot_data.end_link_name, + device=device, + ) + robot = Robot( + mesh_container=mesh_container, + kinematics=kinematics, + ) + + assert robot.device == torch.device(device), "Robot device mismatch." + assert ( + robot.configured_vertices.shape == robot.mesh_container.vertices.shape + ), "Configured vertices shape mismatch." + q = torch.zeros(batch_size, robot.kinematics.chain.n_joints, device=device) + robot.configure(q=q) + try: + q = torch.zeros(batch_size - 1, robot.kinematics.chain.n_joints, device=device) + robot.configure(q=q) + except ValueError: + pass + + +if __name__ == "__main__": + import os + import sys + + os.environ["QT_QPA_PLATFORM"] = "offscreen" + sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + + test_robot() diff --git a/test/differentiable/test_scene.py b/test/core/test_scene.py similarity index 67% rename from test/differentiable/test_scene.py rename to test/core/test_scene.py index b1a67da..f0e6e90 100644 --- a/test/differentiable/test_scene.py +++ b/test/core/test_scene.py @@ -1,9 +1,4 @@ -import os -import sys - -sys.path.append( - os.path.dirname((os.path.dirname(os.path.dirname(os.path.abspath(__file__))))) -) +from pathlib import Path import cv2 import numpy as np @@ -11,10 +6,15 @@ import torch from tqdm import tqdm -from roboreg.differentiable import VirtualCamera -from roboreg.io import find_files -from roboreg.util import overlay_mask -from roboreg.util.factories import create_robot_scene +from roboreg.core import ( + NVDiffRastRenderer, + Robot, + RobotScene, + TorchKinematics, + TorchMeshContainer, + VirtualCamera, +) +from roboreg.io import find_files, load_robot_data_from_urdf_file class TestScene: @@ -24,10 +24,10 @@ def __init__( root_link_name: str = "lbr_link_0", end_link_name: str = "lbr_link_7", camera_requires_grad: bool = False, - data_prefix: str = "test/assets/lbr_med7", - recording_prefix: str = "zed2i", + data_path: Path = Path("test/assets/lbr_med7_r800"), + samples_folder: Path = Path("samples"), ) -> None: - prefix = os.path.join(data_prefix, recording_prefix) + samples_path = data_path / samples_folder # set camera names self.camera_names = ["left", "right"] @@ -37,19 +37,20 @@ def __init__( self.images = {} for camera_name in self.camera_names: self.masks[camera_name] = [ - cv2.imread(os.path.join(prefix, file), cv2.IMREAD_GRAYSCALE) - for file in find_files(prefix, f"mask_sam2_{camera_name}_image_*.png") + cv2.imread(file, cv2.IMREAD_GRAYSCALE) + for file in find_files( + samples_path, f"mask_sam2_{camera_name}_image_*.png" + ) ] self.images[camera_name] = [ - cv2.imread(os.path.join(prefix, file)) - for file in find_files(prefix, f"{camera_name}_image_*.png") + cv2.imread(file) + for file in find_files(samples_path, f"{camera_name}_image_*.png") ] # load joint states self.joint_states = [ - np.load(os.path.join(prefix, file)) - for file in find_files(prefix, "joint_states_*.npy") + np.load(file) for file in find_files(samples_path, "joint_states_*.npy") ] # test for equal length @@ -78,15 +79,12 @@ def __init__( # instantiates camera info and extrinsics files camera_info_files = { - camera_name: os.path.join(prefix, f"{camera_name}_camera_info.yaml") + camera_name: samples_path / f"{camera_name}_camera_info.yaml" for camera_name in self.camera_names } extrinsics_files = { - "left": os.path.join(prefix, "HT_hydra_robust.npy"), - "right": os.path.join( - prefix, - "HT_right_to_left.npy", - ), + "left": samples_path / "HT_hydra_robust.npy", + "right": samples_path / "HT_right_to_left.npy", } # instantiate cameras @@ -99,22 +97,39 @@ def __init__( for camera_name in self.camera_names } - # instantiate scene - self.scene = create_robot_scene( - batch_size=self.joint_states.shape[0], - ros_package="lbr_description", - xacro_path="urdf/med7/med7.xacro", + # instantiate robot + robot_data = load_robot_data_from_urdf_file( + urdf_path="test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf", root_link_name=root_link_name, end_link_name=end_link_name, - cameras=cameras, + ) + mesh_container = TorchMeshContainer( + meshes=robot_data.meshes, + batch_size=self.joint_states.shape[0], device=device, ) + kinematics = TorchKinematics( + urdf=robot_data.urdf, + root_link_name=robot_data.root_link_name, + end_link_name=robot_data.end_link_name, + device=device, + ) + robot = Robot( + mesh_container=mesh_container, + kinematics=kinematics, + ) + + # instantiate scene + self.scene = RobotScene( + cameras=cameras, + robot=robot, + renderer=NVDiffRastRenderer(device=device), + ) # enable gradient tracking self.scene.cameras["left"].extrinsics.requires_grad = camera_requires_grad -@pytest.mark.skip(reason="To be fixed.") def test_multi_config_stereo_view() -> None: test_scene = TestScene(camera_requires_grad=False) @@ -122,29 +137,28 @@ def test_multi_config_stereo_view() -> None: test_scene.scene.robot.configure(q=test_scene.joint_states) # render all camera views + left_camera_name = "left" + right_camera_name = "right" all_renders = { - "left": test_scene.scene.observe_from("left"), - "right": test_scene.scene.observe_from( - "right", reference_transform=test_scene.scene.cameras["left"].extrinsics + left_camera_name: test_scene.scene.observe_from(left_camera_name), + right_camera_name: test_scene.scene.observe_from( + right_camera_name, + reference_transform=test_scene.scene.cameras[left_camera_name].extrinsics, ), } - # show overlays - for camera_name, renders in all_renders.items(): - for render, image in zip(renders, test_scene.images[camera_name]): - render = render.squeeze().cpu().numpy() - overlay = overlay_mask( - image, - (render * 255.0).astype(np.uint8), - scale=1.0, - ) + # expect renders to match in shape and type + for camera_name in all_renders.keys(): + assert all_renders[camera_name].shape == test_scene.masks[camera_name].shape + assert all_renders[camera_name].dtype == test_scene.masks[camera_name].dtype - cv2.imshow(camera_name, overlay) - cv2.waitKey(0) - cv2.destroyAllWindows() + # expect left / right mismatch + difference = torch.abs( + all_renders[left_camera_name] - all_renders[right_camera_name] + ).mean() + assert difference > 0.01, "Left and right renders are unexpectedly similar." -@pytest.mark.skip(reason="To be fixed.") def test_multi_config_stereo_view_pose_optimization() -> None: test_scene = TestScene(camera_requires_grad=True) @@ -181,20 +195,7 @@ def test_multi_config_stereo_view_pose_optimization() -> None: loss.backward() optimizer.step() - # show an overlay for a camera - overlays = [] - for camera_name in test_scene.scene.cameras.keys(): - render = all_renders[camera_name][0].squeeze().detach().cpu().numpy() - image = test_scene.images[camera_name][0] - overlays.append( - overlay_mask( - image, - (render * 255.0).astype(np.uint8), - scale=1.0, - ) - ) - cv2.imshow("overlays", cv2.resize(np.hstack(overlays), (0, 0), fx=0.5, fy=0.5)) - cv2.waitKey(1) + assert best_loss < float("inf"), "Optimization did not improve loss." # expect right intrinsics to be un-changed if not torch.allclose( @@ -215,20 +216,6 @@ def test_multi_config_stereo_view_pose_optimization() -> None: ), } - # show overlays - for camera_name, renders in all_renders.items(): - for render, image in zip(renders, test_scene.images[camera_name]): - render = render.squeeze().detach().cpu().numpy() - overlay = overlay_mask( - image, - (render * 255.0).astype(np.uint8), - scale=1.0, - ) - - cv2.imshow(camera_name, overlay) - cv2.waitKey(0) - cv2.destroyAllWindows() - @pytest.mark.skip(reason="To be fixed.") def test_single_camera_multiple_poses() -> None: @@ -243,15 +230,34 @@ def test_single_camera_multiple_poses() -> None: ) } - scene = create_robot_scene( - batch_size=batch_size, - ros_package="lbr_description", - xacro_path="urdf/med7/med7.xacro", + # instantiate robot + robot_data = load_robot_data_from_urdf_file( + urdf_path="test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf", root_link_name="lbr_link_0", end_link_name="lbr_link_7", - cameras=camera, + ) + mesh_container = TorchMeshContainer( + meshes=robot_data.meshes, + batch_size=batch_size, + device=device, + ) + kinematics = TorchKinematics( + urdf=robot_data.urdf, + root_link_name=robot_data.root_link_name, + end_link_name=robot_data.end_link_name, device=device, ) + robot = Robot( + mesh_container=mesh_container, + kinematics=kinematics, + ) + + # instantiate scene + scene = RobotScene( + cameras=camera, + robot=robot, + renderer=NVDiffRastRenderer(device=device), + ) # for each batch element, configure a unique camera pose... scene.cameras[camera_name].extrinsics = scene.cameras[ @@ -294,6 +300,12 @@ def test_single_camera_multiple_poses() -> None: if __name__ == "__main__": + import os + import sys + + os.environ["QT_QPA_PLATFORM"] = "offscreen" + sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + test_multi_config_stereo_view() test_multi_config_stereo_view_pose_optimization() test_single_camera_multiple_poses() diff --git a/test/differentiable/test_structs.py b/test/core/test_structs.py similarity index 98% rename from test/differentiable/test_structs.py rename to test/core/test_structs.py index 8db5208..2735677 100644 --- a/test/differentiable/test_structs.py +++ b/test/core/test_structs.py @@ -1,6 +1,6 @@ import torch -from roboreg.differentiable.structs import Camera, TorchMeshContainer, VirtualCamera +from roboreg.core import Camera, TorchMeshContainer, VirtualCamera from roboreg.io import load_meshes diff --git a/test/differentiable/test_robot.py b/test/differentiable/test_robot.py deleted file mode 100644 index b477e2f..0000000 --- a/test/differentiable/test_robot.py +++ /dev/null @@ -1,38 +0,0 @@ -import pytest -import torch - -from roboreg.differentiable import Robot -from roboreg.io import URDFParser - - -@pytest.mark.skip(reason="To be fixed.") -def test_robot() -> None: - urdf_parser = URDFParser.from_ros_xacro( - ros_package="lbr_description", xacro_path="urdf/med7/med7.xacro" - ) - collision = True - batch_size = 2 - device = "cuda" if torch.cuda.is_available() else "cpu" - robot = Robot.from_urdf_parser( - urdf_parser=urdf_parser, - root_link_name=urdf_parser.link_names_with_meshes(collision=collision)[0], - end_link_name=urdf_parser.link_names_with_meshes(collision=collision)[-1], - collision=collision, - batch_size=batch_size, - device=device, - target_reduction=0.0, - ) - - q = torch.zeros(batch_size, robot.kinematics.chain.n_joints, device=device) - robot.configure(q=q) - print(robot.configured_vertices) - - -if __name__ == "__main__": - import os - import sys - - os.environ["QT_QPA_PLATFORM"] = "offscreen" - sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) - - test_robot() diff --git a/test/test_hydra_icp.py b/test/test_hydra_icp.py index 3250a6f..97c9cd4 100644 --- a/test/test_hydra_icp.py +++ b/test/test_hydra_icp.py @@ -5,7 +5,7 @@ import torch import transformations as tf -from roboreg.differentiable import TorchKinematics, TorchMeshContainer +from roboreg.core import TorchKinematics, TorchMeshContainer from roboreg.hydra_icp import ( hydra_centroid_alignment, hydra_correspondence_indices, diff --git a/test/util/test_viz.py b/test/util/test_viz.py index cc5163f..c577fb2 100644 --- a/test/util/test_viz.py +++ b/test/util/test_viz.py @@ -2,14 +2,14 @@ import pyvista as pv import torch -from roboreg import differentiable as rrd -from roboreg.io import URDFParser, load_meshes, apply_mesh_origins +from roboreg.core import TorchKinematics, TorchMeshContainer +from roboreg.io import URDFParser, apply_mesh_origins, load_meshes from roboreg.util import RegistrationVisualizer, from_homogeneous @pytest.mark.skip(reason="To be fixed.") def test_visualize_point_cloud(): - meshes = rrd.TorchMeshContainer( + meshes = TorchMeshContainer( meshes=load_meshes( { "link_0": "test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl" @@ -33,7 +33,7 @@ def test_visualize_point_cloud(): @pytest.mark.skip(reason="To be fixed.") def test_visalize_multi_color_point_cloud(): - meshes = rrd.TorchMeshContainer( + meshes = TorchMeshContainer( meshes=load_meshes( { "link_0": "test/assets/lbr_med7_r800/description/meshes/collision/link_0.stl", @@ -94,7 +94,7 @@ def test_visualize_robot(): urdf_parser = URDFParser.from_ros_xacro("lbr_description", "urdf/med7/med7.xacro") # load meshes - meshes = rrd.TorchMeshContainer( + meshes = TorchMeshContainer( meshes=apply_mesh_origins( meshes=load_meshes( urdf_parser.mesh_paths_from_ros_registry( @@ -109,7 +109,7 @@ def test_visualize_robot(): ) # instantiate kinematics - kinematics = rrd.TorchKinematics( + kinematics = TorchKinematics( urdf=urdf_parser.urdf, root_link_name=root_link_name, end_link_name=end_link_name, From 2f7b7ca583cccc4812aa72f2c6b260d3f78ff6e8 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 3 Feb 2026 16:08:49 +0000 Subject: [PATCH 8/9] added ROS-free URDF parsing to CLI (#110) --- README.md | 24 ++++++++++---------- cli/rr_cam_swarm.py | 46 ++++++++++++++++++++++++++++---------- cli/rr_hydra.py | 43 +++++++++++++++++++++++++++--------- cli/rr_mono_dr.py | 49 +++++++++++++++++++++++++++++++---------- cli/rr_render.py | 48 ++++++++++++++++++++++++++++++---------- cli/rr_stereo_dr.py | 49 +++++++++++++++++++++++++++++++---------- cli/util/__init__.py | 0 cli/util/validate.py | 36 ++++++++++++++++++++++++++++++ test/core/test_scene.py | 2 ++ 9 files changed, 226 insertions(+), 71 deletions(-) create mode 100644 cli/util/__init__.py create mode 100644 cli/util/validate.py diff --git a/README.md b/README.md index 2e37848..a6205c2 100644 --- a/README.md +++ b/README.md @@ -128,8 +128,13 @@ Next: ``` ## Command Line Interface -> [!NOTE] -> In these examples, the [lbr_fri_ros2_stack](https://github.com/lbr-stack/lbr_fri_ros2_stack/) is used. Make sure to follow [Quick Start](https://github.com/lbr-stack/lbr_fri_ros2_stack/#quick-start) first. However, you can also use your own robot description files. +> [!TIP] +> Examples use sample data under [test/assets/lbr_med7_r800](test/assets/lbr_med7_r800). Data is stored via Git Large File Storage (LFS). Data is cloned automatically when `git-lfs` is installed. To clone in retrospect: +> ```shell +> sudo apt install git-lfs +> git lfs fetch --all +> git lfs checkout +> ``` ### Segment This is a required step to generate robot masks. @@ -153,8 +158,7 @@ rr-hydra \ --mask-pattern mask_sam2_left_image_*.png \ --depth-pattern depth_*.npy \ --joint-states-pattern joint_states_*.npy \ - --ros-package lbr_description \ - --xacro-path urdf/med7/med7.xacro \ + --urdf-path test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf \ --root-link-name lbr_link_0 \ --end-link-name lbr_link_7 \ --number-of-points 5000 \ @@ -181,8 +185,7 @@ rr-cam-swarm \ --c2 1.5 \ --max-iterations 100 \ --display-progress \ - --ros-package lbr_description \ - --xacro-path urdf/med7/med7.xacro \ + --urdf-path test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf \ --root-link-name lbr_link_0 \ --end-link-name lbr_link_7 \ --target-reduction 0.8 \ @@ -210,8 +213,7 @@ rr-mono-dr \ --lr 0.01 \ --max-iterations 100 \ --display-progress \ - --ros-package lbr_description \ - --xacro-path urdf/med7/med7.xacro \ + --urdf-path test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf \ --root-link-name lbr_link_0 \ --end-link-name lbr_link_7 \ --camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \ @@ -237,8 +239,7 @@ rr-stereo-dr \ --lr 0.01 \ --max-iterations 100 \ --display-progress \ - --ros-package lbr_description \ - --xacro-path urdf/med7/med7.xacro \ + --urdf-path test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf \ --root-link-name lbr_link_0 \ --end-link-name lbr_link_7 \ --left-camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \ @@ -267,8 +268,7 @@ Generate renders using the obtained extrinsics: rr-render \ --batch-size 1 \ --num-workers 0 \ - --ros-package lbr_description \ - --xacro-path urdf/med7/med7.xacro \ + --urdf-path test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf \ --root-link-name lbr_link_0 \ --end-link-name lbr_link_7 \ --camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \ diff --git a/cli/rr_cam_swarm.py b/cli/rr_cam_swarm.py index bfbdcf6..1a52e93 100644 --- a/cli/rr_cam_swarm.py +++ b/cli/rr_cam_swarm.py @@ -17,6 +17,7 @@ from roboreg.io import ( find_files, load_robot_data_from_ros_xacro, + load_robot_data_from_urdf_file, parse_camera_info, parse_mono_data, ) @@ -29,6 +30,8 @@ random_fov_eye_space_coordinates, ) +from .util.validate import validate_urdf_source + def args_factory() -> argparse.Namespace: parser = argparse.ArgumentParser( @@ -96,17 +99,26 @@ def args_factory() -> argparse.Namespace: action="store_true", help="Display optimization progress.", ) + parser.add_argument( + "--urdf-path", + type=str, + default="test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf", + help="Path to URDF file. Meshes resolved relative to this file. " + "Mutually exclusive with --ros-package/--xacro-path.", + ) parser.add_argument( "--ros-package", type=str, - default="lbr_description", - help="Package where the URDF is located.", + default=None, + help="ROS package containing robot description. " + "Requires --xacro-path. Mutually exclusive with --urdf-path.", ) parser.add_argument( "--xacro-path", type=str, - default="urdf/med7/med7.xacro", - help="Path to the xacro file, relative to --ros-package.", + default=None, + help="Path to xacro file relative to --ros-package. " + "Requires --ros-package. Mutually exclusive with --urdf-path.", ) parser.add_argument( "--root-link-name", @@ -180,6 +192,7 @@ def args_factory() -> argparse.Namespace: default=2, help="Number of concurrent compilation jobs for nvdiffrast. Only relevant on first run.", ) + validate_urdf_source(parser, parser.parse_args()) return parser.parse_args() @@ -315,14 +328,23 @@ def main() -> None: ) # instantiate robot - robot_data = load_robot_data_from_ros_xacro( - ros_package=args.ros_package, - xacro_path=args.xacro_path, - root_link_name=args.root_link_name, - end_link_name=args.end_link_name, - collision=args.collision_meshes, - target_reduction=args.target_reduction, - ) + if args.urdf_path is not None: + robot_data = load_robot_data_from_urdf_file( + urdf_path=args.urdf_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, + collision=args.collision_meshes, + target_reduction=args.target_reduction, + ) + else: + robot_data = load_robot_data_from_ros_xacro( + ros_package=args.ros_package, + xacro_path=args.xacro_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, + collision=args.collision_meshes, + target_reduction=args.target_reduction, + ) mesh_container = TorchMeshContainer( meshes=robot_data.meshes, batch_size=batch_size, diff --git a/cli/rr_hydra.py b/cli/rr_hydra.py index 4561167..b873407 100644 --- a/cli/rr_hydra.py +++ b/cli/rr_hydra.py @@ -9,6 +9,7 @@ from roboreg.io import ( find_files, load_robot_data_from_ros_xacro, + load_robot_data_from_urdf_file, parse_camera_info, parse_hydra_data, ) @@ -23,6 +24,8 @@ to_homogeneous, ) +from .util.validate import validate_urdf_source + def args_factory() -> argparse.Namespace: parser = argparse.ArgumentParser( @@ -53,17 +56,26 @@ def args_factory() -> argparse.Namespace: default="joint_states_*.npy", help="Joint state file pattern.", ) + parser.add_argument( + "--urdf-path", + type=str, + default="test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf", + help="Path to URDF file. Meshes resolved relative to this file. " + "Mutually exclusive with --ros-package/--xacro-path.", + ) parser.add_argument( "--ros-package", type=str, - default="lbr_description", - help="Package where the URDF is located.", + default=None, + help="ROS package containing robot description. " + "Requires --xacro-path. Mutually exclusive with --urdf-path.", ) parser.add_argument( "--xacro-path", type=str, - default="urdf/med7/med7.xacro", - help="Path to the xacro file, relative to --ros-package.", + default=None, + help="Path to xacro file relative to --ros-package. " + "Requires --ros-package. Mutually exclusive with --urdf-path.", ) parser.add_argument( "--root-link-name", @@ -147,6 +159,7 @@ def args_factory() -> argparse.Namespace: default=10, help="Erosion kernel size for mask boundary. Larger value will result in larger boundary. The closer the robot, the larger the recommended kernel size.", ) + validate_urdf_source(parser, parser.parse_args()) return parser.parse_args() @@ -167,13 +180,21 @@ def main(): # instantiate robot batch_size = len(joint_states) - robot_data = load_robot_data_from_ros_xacro( - ros_package=args.ros_package, - xacro_path=args.xacro_path, - root_link_name=args.root_link_name, - end_link_name=args.end_link_name, - collision=args.collision_meshes, - ) + if args.urdf_path is not None: + robot_data = load_robot_data_from_urdf_file( + urdf_path=args.urdf_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, + collision=args.collision_meshes, + ) + else: + robot_data = load_robot_data_from_ros_xacro( + ros_package=args.ros_package, + xacro_path=args.xacro_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, + collision=args.collision_meshes, + ) mesh_container = TorchMeshContainer( meshes=robot_data.meshes, batch_size=len(joint_states), diff --git a/cli/rr_mono_dr.py b/cli/rr_mono_dr.py index e758ef9..9d6dc73 100644 --- a/cli/rr_mono_dr.py +++ b/cli/rr_mono_dr.py @@ -18,10 +18,17 @@ TorchMeshContainer, VirtualCamera, ) -from roboreg.io import find_files, load_robot_data_from_ros_xacro, parse_mono_data +from roboreg.io import ( + find_files, + load_robot_data_from_ros_xacro, + load_robot_data_from_urdf_file, + parse_mono_data, +) from roboreg.losses import soft_dice_loss from roboreg.util import mask_distance_transform, mask_exponential_decay, overlay_mask +from .util.validate import validate_urdf_source + class REGISTRATION_MODE(Enum): DISTANCE_FUNCTION = "distance-function" @@ -74,17 +81,26 @@ def args_factory() -> argparse.Namespace: action="store_true", help="Display optimization progress.", ) + parser.add_argument( + "--urdf-path", + type=str, + default="test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf", + help="Path to URDF file. Meshes resolved relative to this file. " + "Mutually exclusive with --ros-package/--xacro-path.", + ) parser.add_argument( "--ros-package", type=str, - default="lbr_description", - help="Package where the URDF is located.", + default=None, + help="ROS package containing robot description. " + "Requires --xacro-path. Mutually exclusive with --urdf-path.", ) parser.add_argument( "--xacro-path", type=str, - default="urdf/med7/med7.xacro", - help="Path to the xacro file, relative to --ros-package.", + default=None, + help="Path to xacro file relative to --ros-package. " + "Requires --ros-package. Mutually exclusive with --urdf-path.", ) parser.add_argument( "--root-link-name", @@ -146,6 +162,7 @@ def args_factory() -> argparse.Namespace: default=2, help="Number of concurrent compilation jobs for nvdiffrast. Only relevant on first run.", ) + validate_urdf_source(parser, parser.parse_args()) return parser.parse_args() @@ -188,13 +205,21 @@ def main() -> None: } # instantiate robot - robot_data = load_robot_data_from_ros_xacro( - ros_package=args.ros_package, - xacro_path=args.xacro_path, - root_link_name=args.root_link_name, - end_link_name=args.end_link_name, - collision=args.collision_meshes, - ) + if args.urdf_path is not None: + robot_data = load_robot_data_from_urdf_file( + urdf_path=args.urdf_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, + collision=args.collision_meshes, + ) + else: + robot_data = load_robot_data_from_ros_xacro( + ros_package=args.ros_package, + xacro_path=args.xacro_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, + collision=args.collision_meshes, + ) mesh_container = TorchMeshContainer( meshes=robot_data.meshes, batch_size=joint_states.shape[0], diff --git a/cli/rr_render.py b/cli/rr_render.py index a5dc77b..65be955 100644 --- a/cli/rr_render.py +++ b/cli/rr_render.py @@ -16,9 +16,15 @@ TorchMeshContainer, VirtualCamera, ) -from roboreg.io import MonocularDataset, load_robot_data_from_ros_xacro +from roboreg.io import ( + MonocularDataset, + load_robot_data_from_ros_xacro, + load_robot_data_from_urdf_file, +) from roboreg.util import overlay_mask +from .util.validate import validate_urdf_source + def args_factory() -> argparse.Namespace: parser = argparse.ArgumentParser( @@ -33,17 +39,26 @@ def args_factory() -> argparse.Namespace: parser.add_argument( "--num-workers", type=int, default=0, help="Number of workers for data loading." ) + parser.add_argument( + "--urdf-path", + type=str, + default="test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf", + help="Path to URDF file. Meshes resolved relative to this file. " + "Mutually exclusive with --ros-package/--xacro-path.", + ) parser.add_argument( "--ros-package", type=str, - default="lbr_description", - help="Package where the URDF is located.", + default=None, + help="ROS package containing robot description. " + "Requires --xacro-path. Mutually exclusive with --urdf-path.", ) parser.add_argument( "--xacro-path", type=str, - default="urdf/med7/med7.xacro", - help="Path to the xacro file, relative to --ros-package.", + default=None, + help="Path to xacro file relative to --ros-package. " + "Requires --ros-package. Mutually exclusive with --urdf-path.", ) parser.add_argument( "--root-link-name", @@ -111,6 +126,7 @@ def args_factory() -> argparse.Namespace: default=2, help="Number of concurrent compilation jobs for nvdiffrast. Only relevant on first run.", ) + validate_urdf_source(parser, parser.parse_args()) return parser.parse_args() @@ -125,13 +141,21 @@ def main(): device=device, ) } - robot_data = load_robot_data_from_ros_xacro( - ros_package=args.ros_package, - xacro_path=args.xacro_path, - root_link_name=args.root_link_name, - end_link_name=args.end_link_name, - collision=args.collision_meshes, - ) + if args.urdf_path is not None: + robot_data = load_robot_data_from_urdf_file( + urdf_path=args.urdf_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, + collision=args.collision_meshes, + ) + else: + robot_data = load_robot_data_from_ros_xacro( + ros_package=args.ros_package, + xacro_path=args.xacro_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, + collision=args.collision_meshes, + ) mesh_container = TorchMeshContainer( meshes=robot_data.meshes, batch_size=args.batch_size, diff --git a/cli/rr_stereo_dr.py b/cli/rr_stereo_dr.py index 8b032f3..bb158ed 100644 --- a/cli/rr_stereo_dr.py +++ b/cli/rr_stereo_dr.py @@ -18,10 +18,17 @@ TorchMeshContainer, VirtualCamera, ) -from roboreg.io import find_files, load_robot_data_from_ros_xacro, parse_stereo_data +from roboreg.io import ( + find_files, + load_robot_data_from_ros_xacro, + load_robot_data_from_urdf_file, + parse_stereo_data, +) from roboreg.losses import soft_dice_loss from roboreg.util import mask_distance_transform, mask_exponential_decay, overlay_mask +from .util.validate import validate_urdf_source + class REGISTRATION_MODE(Enum): DISTANCE_FUNCTION = "distance-function" @@ -74,17 +81,26 @@ def args_factory() -> argparse.Namespace: action="store_true", help="Display optimization progress.", ) + parser.add_argument( + "--urdf-path", + type=str, + default="test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf", + help="Path to URDF file. Meshes resolved relative to this file. " + "Mutually exclusive with --ros-package/--xacro-path.", + ) parser.add_argument( "--ros-package", type=str, - default="lbr_description", - help="Package where the URDF is located.", + default=None, + help="ROS package containing robot description. " + "Requires --xacro-path. Mutually exclusive with --urdf-path.", ) parser.add_argument( "--xacro-path", type=str, - default="urdf/med7/med7.xacro", - help="Path to the xacro file, relative to --ros-package.", + default=None, + help="Path to xacro file relative to --ros-package. " + "Requires --ros-package. Mutually exclusive with --urdf-path.", ) parser.add_argument( "--root-link-name", @@ -176,6 +192,7 @@ def args_factory() -> argparse.Namespace: default=2, help="Number of concurrent compilation jobs for nvdiffrast. Only relevant on first run.", ) + validate_urdf_source(parser, parser.parse_args()) return parser.parse_args() @@ -236,13 +253,21 @@ def main() -> None: } # instantiate robot - robot_data = load_robot_data_from_ros_xacro( - ros_package=args.ros_package, - xacro_path=args.xacro_path, - root_link_name=args.root_link_name, - end_link_name=args.end_link_name, - collision=args.collision_meshes, - ) + if args.urdf_path is not None: + robot_data = load_robot_data_from_urdf_file( + urdf_path=args.urdf_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, + collision=args.collision_meshes, + ) + else: + robot_data = load_robot_data_from_ros_xacro( + ros_package=args.ros_package, + xacro_path=args.xacro_path, + root_link_name=args.root_link_name, + end_link_name=args.end_link_name, + collision=args.collision_meshes, + ) mesh_container = TorchMeshContainer( meshes=robot_data.meshes, batch_size=joint_states.shape[0], diff --git a/cli/util/__init__.py b/cli/util/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/cli/util/validate.py b/cli/util/validate.py new file mode 100644 index 0000000..03477af --- /dev/null +++ b/cli/util/validate.py @@ -0,0 +1,36 @@ +import argparse +from pathlib import Path + + +def validate_urdf_source(parser: argparse.ArgumentParser, args: argparse.Namespace): + r"""Validate mutually exclusive URDF source options.""" + urdf_provided = args.urdf_path is not None + ros_provided = args.ros_package is not None + xacro_provided = args.xacro_path is not None + + # check if both methods provided + if urdf_provided and (ros_provided or xacro_provided): + parser.error( + "Cannot specify --urdf-path together with --ros-package or --xacro-path. " + "Use either --urdf-path OR (--ros-package + --xacro-path)." + ) + + # check if ROS method incomplete + if ros_provided and not xacro_provided: + parser.error("--ros-package requires --xacro-path") + + if xacro_provided and not ros_provided: + parser.error("--xacro-path requires --ros-package") + + # check if nothing provided + if not urdf_provided and not ros_provided: + parser.error( + "Must specify URDF source: either --urdf-path OR " + "(--ros-package + --xacro-path)" + ) + + # validate file exists if using urdf-path + if urdf_provided: + urdf_path = Path(args.urdf_path) + if not urdf_path.exists(): + parser.error(f"URDF file not found: {urdf_path}") diff --git a/test/core/test_scene.py b/test/core/test_scene.py index f0e6e90..88e3b0a 100644 --- a/test/core/test_scene.py +++ b/test/core/test_scene.py @@ -130,6 +130,7 @@ def __init__( self.scene.cameras["left"].extrinsics.requires_grad = camera_requires_grad +@pytest.mark.skip(reason="No GPU support in CI.") def test_multi_config_stereo_view() -> None: test_scene = TestScene(camera_requires_grad=False) @@ -159,6 +160,7 @@ def test_multi_config_stereo_view() -> None: assert difference > 0.01, "Left and right renders are unexpectedly similar." +@pytest.mark.skip(reason="No GPU support in CI.") def test_multi_config_stereo_view_pose_optimization() -> None: test_scene = TestScene(camera_requires_grad=True) From 6301e693d28785af2dd46a38773765383bc49f1a Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 3 Feb 2026 16:21:33 +0000 Subject: [PATCH 9/9] added xarm 7 URDF description --- README.md | 22 +++++++++---------- .../description/meshes/visual/link1.stl | 3 +++ .../description/meshes/visual/link2.stl | 3 +++ .../description/meshes/visual/link3.stl | 3 +++ .../description/meshes/visual/link4.stl | 3 +++ .../description/meshes/visual/link5.stl | 3 +++ .../description/meshes/visual/link6.stl | 3 +++ .../description/meshes/visual/link7.stl | 3 +++ .../visual/link_base.stl} | 0 test/assets/xarm_7/description/xarm_7.urdf | 3 +++ 10 files changed, 34 insertions(+), 12 deletions(-) create mode 100755 test/assets/xarm_7/description/meshes/visual/link1.stl create mode 100755 test/assets/xarm_7/description/meshes/visual/link2.stl create mode 100755 test/assets/xarm_7/description/meshes/visual/link3.stl create mode 100755 test/assets/xarm_7/description/meshes/visual/link4.stl create mode 100755 test/assets/xarm_7/description/meshes/visual/link5.stl create mode 100755 test/assets/xarm_7/description/meshes/visual/link6.stl create mode 100755 test/assets/xarm_7/description/meshes/visual/link7.stl rename test/assets/xarm_7/description/{link_base.STL => meshes/visual/link_base.stl} (100%) mode change 100644 => 100755 create mode 100644 test/assets/xarm_7/description/xarm_7.urdf diff --git a/README.md b/README.md index a6205c2..cfe8747 100644 --- a/README.md +++ b/README.md @@ -277,7 +277,7 @@ rr-render \ --joint-states-path test/assets/lbr_med7_r800/samples \ --image-pattern left_image_*.png \ --joint-states-pattern joint_states_*.npy \ - --output-path test/assets/lbr_med7_r800/samples + --output-path /tmp/renders/lbr_med7_r800 ``` ## Testing @@ -288,13 +288,12 @@ To run Hydra robust ICP on provided `xarm` and `realsense` data, run ```shell rr-hydra \ - --camera-info-file test/assets/xarm/realsense/camera_info.yaml \ - --path test/assets/xarm/realsense \ + --camera-info-file test/assets/xarm_7/samples/camera_info.yaml \ + --path test/assets/xarm_7/samples \ --mask-pattern mask_*.png \ --depth-pattern depth_*.npy \ --joint-states-pattern joint_state_*.npy \ - --ros-package xarm_description \ - --xacro-path urdf/xarm_device.urdf.xacro \ + --urdf-path test/assets/xarm_7/description/xarm_7.urdf \ --root-link-name link_base \ --end-link-name link7 \ --number-of-points 5000 \ @@ -308,17 +307,16 @@ Generate renders using the obtained extrinsics: rr-render \ --batch-size 1 \ --num-workers 0 \ - --ros-package xarm_description \ - --xacro-path urdf/xarm_device.urdf.xacro \ + --urdf-path test/assets/xarm_7/description/xarm_7.urdf \ --root-link-name link_base \ --end-link-name link7 \ - --camera-info-file test/assets/xarm/realsense/camera_info.yaml \ - --extrinsics-file test/assets/xarm/realsense/HT_hydra_robust.npy \ - --images-path test/assets/xarm/realsense \ - --joint-states-path test/assets/xarm/realsense \ + --camera-info-file test/assets/xarm_7/samples/camera_info.yaml \ + --extrinsics-file test/assets/xarm_7/samples/HT_hydra_robust.npy \ + --images-path test/assets/xarm_7/samples \ + --joint-states-path test/assets/xarm_7/samples \ --image-pattern img_*.png \ --joint-states-pattern joint_state_*.npy \ - --output-path test/assets/xarm/realsense + --output-path /tmp/renders/xarm_7 ``` ## Acknowledgements diff --git a/test/assets/xarm_7/description/meshes/visual/link1.stl b/test/assets/xarm_7/description/meshes/visual/link1.stl new file mode 100755 index 0000000..f2b676f --- /dev/null +++ b/test/assets/xarm_7/description/meshes/visual/link1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:34b541122df84d2ef5fcb91b715eb19659dc15ad8d44a191dde481f780265636 +size 184184 diff --git a/test/assets/xarm_7/description/meshes/visual/link2.stl b/test/assets/xarm_7/description/meshes/visual/link2.stl new file mode 100755 index 0000000..bf93580 --- /dev/null +++ b/test/assets/xarm_7/description/meshes/visual/link2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:61e641cd47c169ecef779683332e00e4914db729bf02dfb61bfbe69351827455 +size 225584 diff --git a/test/assets/xarm_7/description/meshes/visual/link3.stl b/test/assets/xarm_7/description/meshes/visual/link3.stl new file mode 100755 index 0000000..d316d23 --- /dev/null +++ b/test/assets/xarm_7/description/meshes/visual/link3.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9e2798e7946dd70046c95455d5ba96392d0b54a6069caba91dc4ca66e1379b42 +size 237084 diff --git a/test/assets/xarm_7/description/meshes/visual/link4.stl b/test/assets/xarm_7/description/meshes/visual/link4.stl new file mode 100755 index 0000000..f6d5fe9 --- /dev/null +++ b/test/assets/xarm_7/description/meshes/visual/link4.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c757fee95f873191a0633c355c07a360032960771cabbd7593a6cdb0f1ffb089 +size 243684 diff --git a/test/assets/xarm_7/description/meshes/visual/link5.stl b/test/assets/xarm_7/description/meshes/visual/link5.stl new file mode 100755 index 0000000..e037b8b --- /dev/null +++ b/test/assets/xarm_7/description/meshes/visual/link5.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:715ad5787c5dab57589937fd47289882707b5e1eb997e340d567785b02f4ec90 +size 229084 diff --git a/test/assets/xarm_7/description/meshes/visual/link6.stl b/test/assets/xarm_7/description/meshes/visual/link6.stl new file mode 100755 index 0000000..198c530 --- /dev/null +++ b/test/assets/xarm_7/description/meshes/visual/link6.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:85b320aa420497827223d16d492bba8de091173374e361396fc7a5dad7bdb0cb +size 399384 diff --git a/test/assets/xarm_7/description/meshes/visual/link7.stl b/test/assets/xarm_7/description/meshes/visual/link7.stl new file mode 100755 index 0000000..ce9a39a --- /dev/null +++ b/test/assets/xarm_7/description/meshes/visual/link7.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:97115d848fbf802cb770cd9be639ae2af993103b9d9bbb0c50c943c738a36f18 +size 231684 diff --git a/test/assets/xarm_7/description/link_base.STL b/test/assets/xarm_7/description/meshes/visual/link_base.stl old mode 100644 new mode 100755 similarity index 100% rename from test/assets/xarm_7/description/link_base.STL rename to test/assets/xarm_7/description/meshes/visual/link_base.stl diff --git a/test/assets/xarm_7/description/xarm_7.urdf b/test/assets/xarm_7/description/xarm_7.urdf new file mode 100644 index 0000000..92b072e --- /dev/null +++ b/test/assets/xarm_7/description/xarm_7.urdf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:65943ebd6a0d3fa59373f8c15ad8cbcb37448c53bc1dd21e9a115863d979b0e4 +size 8041