Merge pull request #1166 from StanfordVL/r1pro

R1pro

Author: ChengshuLi

omnigibson/action_primitives/starter_semantic_action_primitives.py

@@ -32,7 +32,19 @@
 )
 from omnigibson.macros import create_module_macros
 from omnigibson.objects.object_base import BaseObject
-from omnigibson.robots import R1, BaseRobot, BehaviorRobot, Fetch, Freight, Husky, Locobot, Stretch, Tiago, Turtlebot
+from omnigibson.robots import (
+    R1,
+    R1Pro,
+    BaseRobot,
+    BehaviorRobot,
+    Fetch,
+    Freight,
+    Husky,
+    Locobot,
+    Stretch,
+    Tiago,
+    Turtlebot,
+)
 from omnigibson.robots.manipulation_robot import ManipulationRobot
 from omnigibson.robots.holonomic_base_robot import HolonomicBaseRobot
 from omnigibson.tasks.behavior_task import BehaviorTask
@@ -56,6 +68,7 @@
     Locobot: 1.5,
     BehaviorRobot: 0.3,
     R1: 0.3,
+    R1Pro: 0.3,
 }
 m.KP_ANGLE_VEL = {
     Tiago: 0.2,
@@ -67,6 +80,7 @@
     Locobot: 1.0,
     BehaviorRobot: 0.2,
     R1: 0.2,
+    R1Pro: 0.2,
 }
 
 m.DEFAULT_COLLISION_ACTIVATION_DISTANCE = 0.02

omnigibson/examples/robots/import_custom_robot.py

@@ -728,9 +728,14 @@ def get_joint_upper_limit(root_prim, joint_name):
         assert joint_prim is not None, f"Could not find joint prim with name {joint_name}!"
         return joint_prim.GetAttribute("physics:upperLimit").Get()
 
+    # The original format from Lula is a list of dicts, so we need to convert to a single dict
+    if isinstance(curobo_cfg.collision_spheres, list):
+        collision_spheres = {k: v for c in curobo_cfg.collision_spheres for k, v in c.to_dict().items()}
+    else:
+        collision_spheres = curobo_cfg.collision_spheres.to_dict()
+
     # Generate list of collision link names -- this is simply the list of all link names from the
     # collision spheres specification
-    collision_spheres = curobo_cfg.collision_spheres.to_dict()
     all_collision_link_names = list(collision_spheres.keys())
 
     joint_prims = find_all_joints(robot_prim, only_articulated=True)
@@ -833,7 +838,7 @@ def get_joint_upper_limit(root_prim, joint_name):
     "--config",
     required=True,
     type=click.Path(exists=True, dir_okay=False),
-    help="Absolute path to robot URDF file to import",
+    help="Absolute path to robot config yaml file to import",
 )
 def import_custom_robot(config):
     # Load config

omnigibson/robots/__init__.py

@@ -10,6 +10,7 @@
 from omnigibson.robots.locomotion_robot import LocomotionRobot
 from omnigibson.robots.manipulation_robot import ManipulationRobot
 from omnigibson.robots.r1 import R1
+from omnigibson.robots.r1pro import R1Pro
 from omnigibson.robots.robot_base import REGISTERED_ROBOTS, BaseRobot
 from omnigibson.robots.stretch import Stretch
 from omnigibson.robots.tiago import Tiago
@@ -31,6 +32,7 @@
     "LocomotionRobot",
     "ManipulationRobot",
     "R1",
+    "R1Pro",
     "REGISTERED_ROBOTS",
     "Stretch",
     "Tiago",

omnigibson/robots/r1.py

@@ -137,6 +137,18 @@ def __init__(
             **kwargs,
         )
 
+    def _post_load(self):
+        super()._post_load()
+
+        # R1 and R1Pro's URDFs still use the mesh type for the collision meshes of the wheels (see the source URDFs)
+        # as opposed to sphere primitives. As a result, even though import robot script changes to sphere approximation,
+        # GeomPrim will change it back to convex hull approximation during post load. We need to manually set it back to sphere.
+        # TODO: replace the mesh collision mesh with sphere primitives in the import robot script if use_sphere_wheels=True.
+        for wheel_name in self.floor_touching_base_link_names:
+            wheel_link = self.links[wheel_name]
+            assert set(wheel_link.collision_meshes) == {"collisions"}, "Wheel link should only have 1 collision!"
+            wheel_link.collision_meshes["collisions"].set_collision_approximation("boundingSphere")
+
     # Name of the actual root link that we are interested in. Note that this is different from self.root_link_name,
     # which is "base_footprint_x", corresponding to the first of the 6 1DoF joints to control the base.
     @property
@@ -175,7 +187,7 @@ def tucked_default_joint_pos(self):
         # Keep the current joint positions for the base joints
         pos[self.base_idx] = self.get_joint_positions()[self.base_idx]
         for arm in self.arm_names:
-            pos[self.gripper_control_idx[arm]] = th.tensor([0.03, 0.03])  # open gripper
+            pos[self.gripper_control_idx[arm]] = th.tensor([0.05, 0.05])  # open gripper
         return pos
 
     @property
@@ -184,7 +196,7 @@ def untucked_default_joint_pos(self):
         # Keep the current joint positions for the base joints
         pos[self.base_idx] = self.get_joint_positions()[self.base_idx]
         for arm in self.arm_names:
-            pos[self.gripper_control_idx[arm]] = th.tensor([0.03, 0.03])  # open gripper
+            pos[self.gripper_control_idx[arm]] = th.tensor([0.05, 0.05])  # open gripper
             pos[self.arm_control_idx[arm]] = th.tensor([0.0, 1.906, -0.991, 1.571, 0.915, -1.571])
         return pos
 

omnigibson/robots/r1pro.py

@@ -0,0 +1,75 @@
+from functools import cached_property
+
+import torch as th
+
+from omnigibson.robots.r1 import R1
+
+
+class R1Pro(R1):
+    """
+    R1 Pro Robot
+    """
+
+    @property
+    def tucked_default_joint_pos(self):
+        pos = th.zeros(self.n_dof)
+        # Keep the current joint positions for the base joints
+        pos[self.base_idx] = self.get_joint_positions()[self.base_idx]
+        for arm in self.arm_names:
+            pos[self.gripper_control_idx[arm]] = th.tensor([0.05, 0.05])  # open gripper
+        return pos
+
+    @property
+    def untucked_default_joint_pos(self):
+        pos = th.zeros(self.n_dof)
+        # Keep the current joint positions for the base joints
+        pos[self.base_idx] = self.get_joint_positions()[self.base_idx]
+        for arm in self.arm_names:
+            pos[self.gripper_control_idx[arm]] = th.tensor([0.05, 0.05])  # open gripper
+        pos[self.arm_control_idx["left"]] = th.tensor([0.0, 1.57, 0.0, -1.57, 1.57, 0.0, 0.0])
+        pos[self.arm_control_idx["right"]] = th.tensor([0.0, -1.57, 0.0, -1.57, -1.57, 0.0, 0.0])
+        return pos
+
+    @cached_property
+    def floor_touching_base_link_names(self):
+        return ["wheel_motor_link1", "wheel_motor_link2", "wheel_motor_link3"]
+
+    @cached_property
+    def arm_link_names(self):
+        return {arm: [f"{arm}_arm_link{i}" for i in range(1, 8)] for arm in self.arm_names}
+
+    @cached_property
+    def arm_joint_names(self):
+        return {arm: [f"{arm}_arm_joint{i}" for i in range(1, 8)] for arm in self.arm_names}
+
+    @cached_property
+    def finger_link_names(self):
+        return {arm: [f"{arm}_gripper_finger_link{i}" for i in range(1, 3)] for arm in self.arm_names}
+
+    @cached_property
+    def finger_joint_names(self):
+        return {arm: [f"{arm}_gripper_finger_joint{i}" for i in range(1, 3)] for arm in self.arm_names}
+
+    @property
+    def arm_workspace_range(self):
+        return {arm: th.deg2rad(th.tensor([-45, 45], dtype=th.float32)) for arm in self.arm_names}
+
+    @property
+    def disabled_collision_pairs(self):
+        # badly modeled gripper collision meshes
+        return [
+            ["left_arm_link1", "torso_link4"],
+            ["left_arm_link2", "torso_link4"],
+            ["right_arm_link1", "torso_link4"],
+            ["right_arm_link2", "torso_link4"],
+            ["left_arm_link5", "left_arm_link7"],
+            ["right_arm_link5", "right_arm_link7"],
+            ["left_gripper_finger_link1", "left_realsense_link"],
+            ["right_gripper_finger_link1", "right_realsense_link"],
+            ["left_gripper_finger_link1", "left_gripper_finger_link2"],
+            ["right_gripper_finger_link1", "right_gripper_finger_link2"],
+            ["base_link", "wheel_motor_link1"],
+            ["base_link", "wheel_motor_link2"],
+            ["base_link", "wheel_motor_link3"],
+            ["torso_link2", "torso_link4"],
+        ]

tests/test_curobo.py

@@ -193,6 +193,57 @@ def test_curobo():
                 },
             },
         },
+        {
+            "type": "R1Pro",
+            "obs_modalities": "rgb",
+            "position": [0.7, -0.75, 0.056],
+            "orientation": [0, 0, 0.707, 0.707],
+            "self_collisions": True,
+            "action_normalize": False,
+            "controller_config": {
+                "base": {
+                    "name": "HolonomicBaseJointController",
+                    "motor_type": "position",
+                    "command_input_limits": None,
+                    "use_impedances": False,
+                },
+                "trunk": {
+                    "name": "JointController",
+                    "motor_type": "position",
+                    "command_input_limits": None,
+                    "use_delta_commands": False,
+                    "use_impedances": False,
+                },
+                "arm_left": {
+                    "name": "JointController",
+                    "motor_type": "position",
+                    "command_input_limits": None,
+                    "use_delta_commands": False,
+                    "use_impedances": False,
+                },
+                "arm_right": {
+                    "name": "JointController",
+                    "motor_type": "position",
+                    "command_input_limits": None,
+                    "use_delta_commands": False,
+                    "use_impedances": False,
+                },
+                "gripper_left": {
+                    "name": "JointController",
+                    "motor_type": "position",
+                    "command_input_limits": None,
+                    "use_delta_commands": False,
+                    "use_impedances": False,
+                },
+                "gripper_right": {
+                    "name": "JointController",
+                    "motor_type": "position",
+                    "command_input_limits": None,
+                    "use_delta_commands": False,
+                    "use_impedances": False,
+                },
+            },
+        },
     ]
     for robot_cfg in robot_cfgs:
         cfg["robots"] = [robot_cfg]
@@ -226,6 +277,8 @@ def test_curobo():
         # Create CuRobo instance
         batch_size = 2
         n_samples = 20
+        max_false_positive_rate = 0.05
+        max_false_negative_rate = 0.0
 
         cmg = CuRoboMotionGenerator(
             robot=robot,
@@ -326,11 +379,11 @@ def test_curobo():
             f"Collision checking false positive: {false_positive / n_samples}, false negative: {false_negative / n_samples}."
         )
         assert (
-            false_positive / n_samples == 0.0
-        ), f"Collision checking false positive rate: {false_positive / n_samples}, should be == 0.0."
+            false_positive / n_samples <= max_false_positive_rate
+        ), f"Collision checking false positive rate: {false_positive / n_samples} > {max_false_positive_rate}"
         assert (
-            false_negative / n_samples == 0.0
-        ), f"Collision checking false negative rate: {false_negative / n_samples}, should be == 0.0."
+            false_negative / n_samples <= max_false_negative_rate
+        ), f"Collision checking false negative rate: {false_negative / n_samples} > {max_false_negative_rate}"
 
         env.scene.reset()