Merge pull request #1937 from StanfordVL/fix/curobo-holonomic-joint-limit

wensi-ai · web-flow · commit 41d2ef044a1b · 2026-02-13T15:46:00.000-08:00
Fix curobo holonomic joint limit
diff --git a/OmniGibson/omnigibson/action_primitives/curobo.py b/OmniGibson/omnigibson/action_primitives/curobo.py
@@ -115,6 +115,27 @@ def __init__(
             robot_cfg_path_dict = {
                 CuRoboEmbodimentSelection.DEFAULT: robot_cfg_path_dict[CuRoboEmbodimentSelection.DEFAULT]
             }
+        print("!" * 100)
+        # TODO [Wensi]: Check whether this is still true for future releases.
+        print(
+            """
+            NOTE: Currently (v3.8.0), for cuda architecture 12.0 (e.g. RTX 50-series), using Default embodiment for Tiago or non-DEFAULT embodiment for R1Pro
+                will raise CUDA illegal memory access error during mg.warmup() due to cuRobo compatibility issues. 
+                Therefore, we automatically exclude these incompatible embodiments when we detect such GPU is being used. 
+            """
+        )
+        if th.cuda.get_device_capability(device) == (12, 0):
+            if robot.model == "tiago":
+                print("Detected you are using Tiago with cuda architecture 12.0 GPU: excluding non-DEFAULT embodiment.")
+                robot_cfg_path_dict = {
+                    CuRoboEmbodimentSelection.DEFAULT: robot_cfg_path_dict[CuRoboEmbodimentSelection.DEFAULT]
+                }
+            elif robot.model == "r1pro":
+                print("Detected you are using R1Pro with cuda architecture 12.0 GPU: excluding DEFAULT embodiment.")
+                robot_cfg_path_dict = {
+                    k: v for k, v in robot_cfg_path_dict.items() if k != CuRoboEmbodimentSelection.DEFAULT
+                }
+        print("!" * 100)
         robot_usd_path = robot.usd_path if robot_usd_path is None else robot_usd_path
 
         # This will be shared across all MotionGen instances
diff --git a/OmniGibson/omnigibson/action_primitives/starter_semantic_action_primitives.py b/OmniGibson/omnigibson/action_primitives/starter_semantic_action_primitives.py
@@ -25,7 +25,10 @@
     ActionPrimitiveErrorGroup,
     BaseActionPrimitiveSet,
 )
-from omnigibson.action_primitives.curobo import CuRoboEmbodimentSelection, CuRoboMotionGenerator
+from omnigibson.action_primitives.curobo import (
+    CuRoboEmbodimentSelection,
+    CuRoboMotionGenerator,
+)
 from omnigibson.controllers import (
     InverseKinematicsController,
     HolonomicBaseJointController,
@@ -941,8 +944,6 @@ def _plan_joint_motion(
         # Grab the first successful trajectory if found
         success_idx = th.where(successes)[0].cpu()
         if len(success_idx) == 0:
-            # print("motion planning fails")
-            # breakpoint()
             raise ActionPrimitiveError(
                 ActionPrimitiveError.Reason.PLANNING_ERROR,
                 "There is no accessible path from where you are to the desired pose. Try again",
@@ -967,8 +968,7 @@ def _execute_motion_plan(
         low_precision=False,
         ignore_physics=False,
     ):
-        for i, joint_pos in enumerate(q_traj):
-            # indented_print(f"Executing motion plan step {i + 1}/{len(q_traj)}")
+        for joint_pos in q_traj:
             if ignore_physics:
                 self.robot.set_joint_positions(joint_pos)
                 og.sim.step()
@@ -983,9 +983,7 @@ def _execute_motion_plan(
                 for arm in self.robot.arm_names:
                     articulation_control_idx.append(self.robot.arm_control_idx[arm])
                 articulation_control_idx = th.cat(articulation_control_idx)
-                for j in range(m.MAX_STEPS_FOR_JOINT_MOTION):
-                    # indented_print(f"Step {j + 1}/{m.MAX_STEPS_FOR_JOINT_MOTION}")
-
+                for _ in range(m.MAX_STEPS_FOR_JOINT_MOTION):
                     # We need to call @q_to_action for every step because as the robot base moves, the same base joint_pos will be
                     # converted to different actions, since HolonomicBaseJointController accepts an action in the robot local frame.
                     action = self.robot.q_to_action(joint_pos)
@@ -1535,9 +1533,6 @@ def _navigate_to_pose(self, pose_2d, skip_obstacle_update=False):
             self.debug_visual_marker.set_position_orientation(*pose_3d)
         target_pos = {self.robot.base_footprint_link_name: pose_3d[0]}
         target_quat = {self.robot.base_footprint_link_name: pose_3d[1]}
-
-        # print("base motion planning")
-        # breakpoint()
         q_traj = self._plan_joint_motion(
             target_pos,
             target_quat,
diff --git a/OmniGibson/omnigibson/configs/r1pro_primitives.yaml b/OmniGibson/omnigibson/configs/r1pro_primitives.yaml
@@ -0,0 +1,117 @@
+env:
+  action_frequency: 30                  # (int): environment executes action at the action_frequency rate
+  physics_frequency: 120                # (int): physics frequency (1 / physics_timestep for physx)
+  device: null                          # (None or str): specifies the device to be used if running on the gpu with torch backend
+  automatic_reset: false                # (bool): whether to automatic reset after an episode finishes
+  flatten_action_space: false           # (bool): whether to flatten the action space as a sinle 1D-array
+  flatten_obs_space: false              # (bool): whether the observation space should be flattened when generated
+  use_external_obs: false               # (bool): Whether to use external observations or not
+  external_sensors: null                # (None or list): If specified, list of sensor configurations for external sensors to add. Should specify sensor "type" and any additional kwargs to instantiate the sensor. Each entry should be the kwargs passed to @create_sensor, in addition to position, orientation
+
+render:
+  viewer_width: 1280
+  viewer_height: 720
+
+scene:
+  type: InteractiveTraversableScene
+  scene_model: Rs_int
+  trav_map_resolution: 0.1
+  default_erosion_radius: 0.0
+  trav_map_with_objects: true
+  num_waypoints: 1
+  waypoint_resolution: 0.2
+  load_object_categories: null
+  not_load_object_categories: null
+  load_room_types: ["living_room"]
+  load_room_instances: null
+  load_task_relevant_only: false
+  seg_map_resolution: 1.0
+  scene_source: OG
+  include_robots: false
+
+robots:
+  - type: R1Pro
+    obs_modalities: [rgb]
+    include_sensor_names: null
+    exclude_sensor_names: null
+    scale: 1.0
+    self_collisions: true
+    action_normalize: false
+    action_type: continuous
+    grasping_mode: sticky
+    reset_joint_pos: [
+      0.0000,
+      -0.0000,
+      0.0247,
+      0.0009,
+      0.0004,
+      -0.0000,
+      0.0000,
+      0.0000,
+      0.0000,
+      0.0000,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0,
+      0.0300,
+      0.0300,
+      0.0300,
+      0.0300,
+    ]
+    sensor_config:
+      VisionSensor:
+        sensor_kwargs:
+          image_height: 128
+          image_width: 128
+    controller_config:
+      base:
+        name: HolonomicBaseJointController
+        motor_type: position
+        command_input_limits: null
+        use_impedances: false
+      trunk:
+        name: JointController
+        motor_type: position
+        command_input_limits: null
+        use_delta_commands: false
+        use_impedances: false
+      arm_left:
+        name: JointController
+        motor_type: position
+        command_input_limits: null
+        use_delta_commands: false
+        use_impedances: false
+      arm_right:
+        name: JointController
+        motor_type: position
+        command_input_limits: null
+        use_delta_commands: false
+        use_impedances: false
+      gripper_left:
+        name: JointController
+        motor_type: position
+        command_input_limits: null
+        use_delta_commands: false
+        use_impedances: false
+      gripper_right:
+        name: JointController
+        motor_type: position
+        command_input_limits: null
+        use_delta_commands: false
+        use_impedances: false
+
+objects: []
+
+task:
+  type: DummyTask
diff --git a/OmniGibson/omnigibson/examples/action_primitives/rs_int_example.py b/OmniGibson/omnigibson/examples/action_primitives/rs_int_example.py
@@ -22,7 +22,7 @@ def main():
 
     It loads Rs_int with a robot, and the robot picks and places an apple.
     """
-    robot_options = ["R1", "Tiago"]
+    robot_options = ["R1", "R1Pro", "Tiago"]
     robot_type = choose_from_options(options=robot_options, name="robot options", random_selection=False)
 
     # Load the config
diff --git a/OmniGibson/omnigibson/robots/robot.py b/OmniGibson/omnigibson/robots/robot.py
@@ -3452,7 +3452,7 @@ def curobo_path(self):
             if eef_def and eef_def.curobo_path:
                 return os.path.join(get_dataset_path("omnigibson-robot-assets"), eef_def.curobo_path)
             else:
-                assert False, f"Robot not supported for curobo."
+                assert False, "Robot not supported for curobo."
 
         # Import here to avoid circular imports
         from omnigibson.action_primitives.curobo import CuRoboEmbodimentSelection
@@ -3479,7 +3479,7 @@ def curobo_attached_object_link_names(self):
         ):
             assert (
                 self.end_effector in self._definition.manipulation.eef_support_curobo_attached_object_link_names
-            ), f"Robot not supported for curobo."
+            ), "Robot not supported for curobo."
 
         assert self.is_manipulation
         # By default, sets the standardized path
@@ -4110,15 +4110,19 @@ def q_to_action(self, q):
             ), f"Controller [{name}] should be a JointController/HolonomicBaseJointController with use_delta_commands=False!"
             command = q[controller.dof_idx]
             if isinstance(controller, HolonomicBaseJointController):
-                # For a holonomic base joint controller, the command should be in the robot local frame
-                # For orientation, we need to convert the command to a delta angle
-                cur_rz_joint_pos = self.get_joint_positions()[self.base_idx][5]
+                # Holonomnic base controller expects delta (x, y, rz) in robot base footprint link frame
+                # However, q actions are in absolute (x, y, rz) in robot root frame, so we need to convert them before feeding to the controller
+                base_joint_pos = self.get_joint_positions()[self.base_idx]
+                cur_rz_joint_pos = base_joint_pos[5]
                 delta_q = wrap_angle(command[2] - cur_rz_joint_pos)
 
                 # For translation, we need to convert the command to the robot local frame
-                body_pose = self.get_position_orientation()
-                canonical_pos = th.tensor([command[0], command[1], body_pose[0][2]], dtype=th.float32)
-                local_pos = T.relative_pose_transform(canonical_pos, th.tensor([0.0, 0.0, 0.0, 1.0]), *body_pose)[0]
+                body_pos = base_joint_pos[:3]
+                body_quat = T.mat2quat(T.euler_intrinsic2mat(base_joint_pos[3:6]))
+                canonical_pos = th.tensor([command[0], command[1], body_pos[2]], dtype=th.float32)
+                local_pos = T.relative_pose_transform(
+                    canonical_pos, th.tensor([0.0, 0.0, 0.0, 1.0]), body_pos, body_quat
+                )[0]
                 command = th.tensor([local_pos[0], local_pos[1], delta_q])
             action.append(controller._reverse_preprocess_command(command))
         action = th.cat(action, dim=0)
diff --git a/OmniGibson/tests/test_curobo.py b/OmniGibson/tests/test_curobo.py
@@ -244,6 +244,16 @@ def test_curobo():
         },
     ]
     for robot_cfg in robot_cfgs:
+        if th.cuda.is_available() and th.cuda.get_device_capability(0) == (12, 0):
+            # TODO [Wensi]: Check whether this is still true for future releases.
+            # Currently (v3.8.0), for cuda architecture 12.0 (e.g. RTX 50-series), using Default embodiment for Tiago or non-DEFAULT embodiment for R1Pro
+            #     will raise CUDA illegal memory access error during mg.warmup() due to cuRobo compatibility issues.
+            # Therefore, we remove R1Pro for testing if we detect such GPU is being used.
+            if robot_cfg["model"] == "r1pro":
+                print(
+                    f"Skipping testing for {robot_cfg['model']} on cuda architecture 12.0 GPU due to cuRobo embodiment compatibility issues."
+                )
+                continue
         cfg["robots"] = [robot_cfg]
 
         env = og.Environment(configs=cfg)
@@ -284,7 +294,7 @@ def test_curobo():
             debug=False,
             use_cuda_graph=True,
             collision_activation_distance=0.075,  # Use larger activation distance for better reproducibility
-            use_default_embodiment_only=True,
+            use_default_embodiment_only=False,
         )
 
         # Sample values for robot
