cleaned up the URDFParser API

Author: mhubii

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,

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,37 +34,53 @@ 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
             )
         )
 
     @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,68 +125,56 @@ 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.
             end_link_name (str): End link name.
             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.
             end_link_name (str): End link name.
             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.")

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
                 )
             ),

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],

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))

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
                 )
             ),

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
                 )
             ),