Generate IK URDF on-the-fly (#108)

* Implement get_latest_urdf() with error handling

* Implement get_joint_limits()

* Implement make_joints_rigid()

* Implement merge_arm()

* Implement add_virtual_rotary_joint()

* Implement clip_joint_limits()

* Implement generate_ik_urdfs()

* Implement generate_urdf_from_robot()

* Generate Web Teleop's URDF on-the-fly

* Remove preparation script

* Switch to using Stretch URDF

* Add Stretch URDF dependency

* CMake doesnt need to install urdf/ anymore

* Run prettier

* Run prettier

---------

Co-authored-by: hello-vinitha <[email protected]>

CMakeLists.txt

@@ -58,10 +58,6 @@ install(DIRECTORY nodes
   DESTINATION share/${PROJECT_NAME}
 )
 
-install(DIRECTORY urdf
-  DESTINATION share/${PROJECT_NAME}
-)
-
 install(DIRECTORY config
   DESTINATION share/${PROJECT_NAME}
 )

launch/web_interface.launch.py

@@ -196,23 +196,6 @@ def generate_launch_description():
         stretch_has_nav_head_cam,
     )
 
-    # If the stretch tool has the configuration(s) that require the specialized URDFs
-    # (e.g., move-to-pregrasp, enabled with the dex wrist, requires the specialized URDFs),
-    # check if the specialized URDFs exist and raise an exception if they don't.
-    if stretch_tool == "eoa_wrist_dw3_tool_sg3":
-        specialized_urdf_path = os.path.join(
-            teleop_interface_package, "urdf", "stretch_base_rotation_ik.urdf"
-        )
-        if not os.path.exists(specialized_urdf_path):
-            raise FileNotFoundError(
-                "Could not find the specialized URDF, which is required for move-to-pregrasp, "
-                f"at {specialized_urdf_path}. Run the following:\n"
-                "    1. `colcon_cd stretch_web_teleop`\n"
-                "    2. `python3 prepare_specialized_urdf.py`\n"
-                "    3. `cd ~/ament_ws`\n"
-                "    4. `colcon build`"
-            )
-
     # Declare launch arguments
     params_file = DeclareLaunchArgument(
         "params",

nodes/move_to_pregrasp.py

@@ -1,19 +1,19 @@
 #!/usr/bin/env python3
 
 # Standard Imports
-import os
 import sys
 import threading
 import traceback
 from typing import Dict, Generator, List, Optional, Tuple
 
-# Third-Party Imports
 import numpy as np
 import numpy.typing as npt
 import rclpy
+
+# Third-Party Imports
+import stretch_urdf.urdf_utils as uu
 import tf2_ros
 import yaml
-from ament_index_python import get_package_share_directory
 from cv_bridge import CvBridge
 from geometry_msgs.msg import Point, Quaternion, Transform, TransformStamped, Vector3
 from rclpy.action import ActionServer, CancelResponse, GoalResponse
@@ -100,14 +100,12 @@ def __init__(
         self.wrist_offset: Optional[Tuple[float, float]] = None
 
         # Create the inverse jacobian controller to execute motions
-        urdf_abs_path = os.path.join(
-            get_package_share_directory("stretch_web_teleop"),
-            "urdf/stretch_base_rotation_ik.urdf",
-        )
+        urdf_fpaths = uu.generate_ik_urdfs("stretch_web_teleop", rigid_wrist_urdf=False)
+        urdf_fpath = urdf_fpaths[0]
         self.controller = StretchIKControl(
             self,
             tf_buffer=self.tf_buffer,
-            urdf_path=urdf_abs_path,
+            urdf_path=urdf_fpath,
             static_transform_broadcaster=self.static_transform_broadcaster,
         )
 

requirements.txt

@@ -1,4 +1,5 @@
 gTTS
+hello-robot-stretch-urdf>=0.1.0
 loguru
 pin
 PyAudio==0.2.14