added factory pattern to URDFParser

Author: mhubii

cli/rr_cam_swarm.py

@@ -301,8 +301,9 @@ def main() -> None:
         device=device,
     )
 
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro(ros_package=args.ros_package, xacro_path=args.xacro_path)
+    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,
         root_link_name=args.root_link_name,

cli/rr_hydra.py

@@ -162,8 +162,9 @@ def main():
     height, width, intrinsics = parse_camera_info(args.camera_info_file)
 
     # instantiate kinematics
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro(ros_package=args.ros_package, xacro_path=args.xacro_path)
+    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 == "":
@@ -225,7 +226,9 @@ def main():
     mesh_normals = []
     for i in range(batch_size):
         mesh_normals.append(
-            compute_vertex_normals(vertices=mesh_vertices[i], faces=robot.faces)
+            compute_vertex_normals(
+                vertices=mesh_vertices[i], faces=robot.mesh_container.faces
+            )
         )
 
     # clean observed vertices and turn into tensor

roboreg/io/parsers.py

@@ -12,33 +12,44 @@
 class URDFParser:
     __slots__ = ["_urdf", "_robot"]
 
-    def __init__(self) -> None:
-        self._urdf = None
-        self._robot = None
+    def __init__(self, urdf: str) -> None:
+        self._urdf = urdf
+        self._robot = urdf_parser_py.urdf.Robot.from_xml_string(urdf)
 
-    def from_urdf(self, urdf: str) -> None:
+    @classmethod
+    def from_file(cls, path: Union[Path, str]) -> None:
         r"""Instantiate URDF parser from URDF string.
 
         Args:
-            urdf (str): URDF string.
+            path (Union[Path, str]): Path to URDF file.
         """
-        self._urdf = urdf
-        self._robot = urdf_parser_py.urdf.Robot.from_xml_string(urdf)
+        path = Path(path)
+        if not path.exists():
+            raise FileNotFoundError(f"URDF file {path} does not exist.")
+        if not path.suffix == ".urdf":
+            raise ValueError(f"URDF file {path} must have .urdf extension.")
 
-    def from_ros_xacro(self, ros_package: str, xacro_path: str) -> None:
+        with open(path, "r") as f:
+            urdf = f.read()
+
+        return cls(urdf=urdf)
+
+    @classmethod
+    def from_ros_xacro(cls, ros_package: str, xacro_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.
         """
-        self.from_urdf(
-            urdf=self.urdf_from_ros_xacro(
+        return cls(
+            urdf=cls._urdf_from_ros_xacro(
                 ros_package=ros_package, xacro_path=xacro_path
             )
         )
 
-    def urdf_from_ros_xacro(self, ros_package: str, xacro_path: str) -> str:
+    @staticmethod
+    def _urdf_from_ros_xacro(ros_package: str, xacro_path: str) -> str:
         r"""Convert ROS xacro file to URDF.
 
         Args:
@@ -52,10 +63,9 @@ def urdf_from_ros_xacro(self, ros_package: str, xacro_path: str) -> str:
         import xacro
         from ament_index_python import get_package_share_directory
 
-        self._urdf = xacro.process(
+        return xacro.process(
             os.path.join(get_package_share_directory(ros_package), xacro_path)
         )
-        return self._urdf
 
     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.

roboreg/util/factories.py

@@ -42,8 +42,9 @@ def create_robot_scene(
     target_reduction: float = 0.0,
 ) -> RobotScene:
     # create URDF parser
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro(ros_package=ros_package, xacro_path=xacro_path)
+    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(

test/differentiable/test_kinematics.py

@@ -19,8 +19,9 @@ def test_torch_kinematics(
     root_link_name: str = "lbr_link_0",
     end_link_name: str = "lbr_link_7",
 ) -> None:
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro(ros_package=ros_package, xacro_path=xacro_path)
+    urdf_parser = URDFParser.from_ros_xacro(
+        ros_package=ros_package, xacro_path=xacro_path
+    )
     device = "cuda" if torch.cuda.is_available() else "cpu"
     kinematics = TorchKinematics(
         urdf=urdf_parser.urdf,
@@ -43,8 +44,9 @@ def test_torch_kinematics_on_mesh(
     root_link_name: str = "lbr_link_0",
     end_link_name: str = "lbr_link_7",
 ) -> None:
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro(ros_package=ros_package, xacro_path=xacro_path)
+    urdf_parser = URDFParser.from_ros_xacro(
+        ros_package=ros_package, xacro_path=xacro_path
+    )
     device = "cuda" if torch.cuda.is_available() else "cpu"
     kinematics = TorchKinematics(
         urdf=urdf_parser.urdf,
@@ -103,8 +105,7 @@ def test_display_xyz(vertices: torch.Tensor) -> None:
 
 @pytest.mark.skip(reason="To be fixed.")
 def test_diff_kinematics() -> None:
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro(
+    urdf_parser = URDFParser.from_ros_xacro(
         ros_package="lbr_description", xacro_path="urdf/med7/med7.xacro"
     )
     device = "cuda" if torch.cuda.is_available() else "cpu"

test/differentiable/test_rendering.py

@@ -62,8 +62,7 @@ def __init__(
             )
 
         # instantiate URDF parser
-        self.urdf_parser = URDFParser()
-        self.urdf_parser.from_ros_xacro(
+        self.urdf_parser = URDFParser.from_ros_xacro(
             ros_package="lbr_description", xacro_path="urdf/med7/med7.xacro"
         )
 

test/differentiable/test_robot.py

@@ -7,8 +7,7 @@
 
 @pytest.mark.skip(reason="To be fixed.")
 def test_robot() -> None:
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro(
+    urdf_parser = URDFParser.from_ros_xacro(
         ros_package="lbr_description", xacro_path="urdf/med7/med7.xacro"
     )
     collision = True

test/io/test_parsers.py

@@ -15,8 +15,7 @@
 
 @pytest.mark.skip(reason="To be fixed.")
 def test_urdf_parser() -> None:
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro("lbr_description", "urdf/med7/med7.xacro")
+    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"))

test/test_hydra_icp.py

@@ -128,8 +128,9 @@ def test_hydra_icp():
     )
 
     # instantiate kinematics
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro(ros_package=ros_package, xacro_path=xacro_path)
+    urdf_parser = URDFParser.from_ros_xacro(
+        ros_package=ros_package, xacro_path=xacro_path
+    )
     kinematics = TorchKinematics(
         urdf=urdf_parser.urdf,
         root_link_name=root_link_name,
@@ -258,8 +259,9 @@ def test_hydra_robust_icp() -> None:
     )
 
     # instantiate kinematics
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro(ros_package=ros_package, xacro_path=xacro_path)
+    urdf_parser = URDFParser.from_ros_xacro(
+        ros_package=ros_package, xacro_path=xacro_path
+    )
     kinematics = TorchKinematics(
         urdf=urdf_parser.urdf,
         root_link_name=root_link_name,

test/util/test_viz.py

@@ -87,11 +87,7 @@ def test_visualize_robot():
     end_link_name = "lbr_link_7"
 
     # parse URDF
-    urdf_parser = URDFParser()
-    urdf_parser.from_ros_xacro("lbr_description", "urdf/med7/med7.xacro")
-    paths = urdf_parser.ros_package_mesh_paths(
-        root_link_name=root_link_name, end_link_name=end_link_name
-    )
+    urdf_parser = URDFParser.from_ros_xacro("lbr_description", "urdf/med7/med7.xacro")
 
     # load meshes
     meshes = rrd.TorchMeshContainer(