refactor ArticulatedTrunkRobot, MobileManipulationRobot, UntuckedArmP…

…oseRobot

omnigibson/controllers/controller_base.py

@@ -133,8 +133,8 @@ def __init__(
         )
         command_output_limits = (
             (
-                th.tensor(self._control_limits[self.control_type][0])[self.dof_idx],
-                th.tensor(self._control_limits[self.control_type][1])[self.dof_idx],
+                self._control_limits[self.control_type][0][self.dof_idx],
+                self._control_limits[self.control_type][1][self.dof_idx],
             )
             if type(command_output_limits) == str and command_output_limits == "default"
             else command_output_limits

omnigibson/robots/a1.py

@@ -42,8 +42,7 @@ def __init__(
         """
         Args:
             name (str): Name for the object. Names need to be unique per scene
-            prim_path (None or str): global path in the stage to this object. If not specified, will automatically be
-                created at /World/<name>
+            relative_prim_path (str): Scene-local prim path of the Prim to encapsulate or create.
             scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale
                 for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a
                 3-array specifies per-axis scaling.
@@ -166,7 +165,7 @@ def _default_joint_pos(self):
 
     @property
     def finger_lengths(self):
-        return {self.default_arm: 0.1}
+        return {self.default_arm: 0.087}
 
     @property
     def arm_link_names(self):

omnigibson/robots/fetch.py

@@ -6,30 +6,19 @@
 from omnigibson.controllers import ControlType
 from omnigibson.macros import gm
 from omnigibson.robots.active_camera_robot import ActiveCameraRobot
-from omnigibson.robots.manipulation_robot import GraspingPoint, ManipulationRobot
+from omnigibson.robots.articulated_trunk_robot import ArticulatedTrunkRobot
+from omnigibson.robots.manipulation_robot import GraspingPoint
 from omnigibson.robots.two_wheel_robot import TwoWheelRobot
+from omnigibson.robots.untucked_arm_pose_robot import UntuckedArmPoseRobot
 from omnigibson.utils.python_utils import assert_valid_key
 from omnigibson.utils.transform_utils import euler2quat
 from omnigibson.utils.ui_utils import create_module_logger
 from omnigibson.utils.usd_utils import ControllableObjectViewAPI, JointType
 
 log = create_module_logger(module_name=__name__)
 
-DEFAULT_ARM_POSES = {
-    "vertical",
-    "diagonal15",
-    "diagonal30",
-    "diagonal45",
-    "horizontal",
-}
 
-RESET_JOINT_OPTIONS = {
-    "tuck",
-    "untuck",
-}
-
-
-class Fetch(ManipulationRobot, TwoWheelRobot, ActiveCameraRobot):
+class Fetch(TwoWheelRobot, ArticulatedTrunkRobot, UntuckedArmPoseRobot, ActiveCameraRobot):
     """
     Fetch Robot
     Reference: https://fetchrobotics.com/robotics-platforms/fetch-mobile-manipulator/
@@ -61,18 +50,19 @@ def __init__(
         # Unique to ManipulationRobot
         grasping_mode="physical",
         disable_grasp_handling=False,
-        # Unique to Fetch
+        # Unique to ArticulatedTrunkRobot
         rigid_trunk=False,
         default_trunk_offset=0.2,
+        # Unique to MobileManipulationRobot
         default_reset_mode="untuck",
+        # Unique to UntuckedArmPoseRobot
         default_arm_pose="vertical",
         **kwargs,
     ):
         """
         Args:
             name (str): Name for the object. Names need to be unique per scene
-            prim_path (None or str): global path in the stage to this object. If not specified, will automatically be
-                created at /World/<name>
+            relative_prim_path (str): Scene-local prim path of the Prim to encapsulate or create.
             scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale
                 for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a
                 3-array specifies per-axis scaling.
@@ -81,6 +71,7 @@ def __init__(
             self_collisions (bool): Whether to enable self collisions for this object
             load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
                 loading this prim at runtime.
+            fixed_base (bool): whether to fix the base of this object or not
             abilities (None or dict): If specified, manually adds specific object states to this object. It should be
                 a dict in the form of {ability: {param: value}} containing object abilities and parameters to pass to
                 the object state instance constructor.
@@ -111,25 +102,17 @@ def __init__(
                 If "sticky", will magnetize any object touching the gripper's fingers.
             disable_grasp_handling (bool): If True, will disable all grasp handling for this object. This means that
                 sticky and assisted grasp modes will not work unless the connection/release methodsare manually called.
-            rigid_trunk (bool) if True, will prevent the trunk from moving during execution.
-            default_trunk_offset (float): sets the default height of the robot's trunk
+            rigid_trunk (bool): If True, will prevent the trunk from moving during execution.
+            default_trunk_offset (float): The default height of the robot's trunk
             default_reset_mode (str): Default reset mode for the robot. Should be one of: {"tuck", "untuck"}
-            If reset_joint_pos is not None, this will be ignored (since _default_joint_pos won't be used during initialization).
+                If reset_joint_pos is not None, this will be ignored (since _default_joint_pos won't be used during initialization).
             default_arm_pose (str): Default pose for the robot arm. Should be one of:
                 {"vertical", "diagonal15", "diagonal30", "diagonal45", "horizontal"}
                 If either reset_joint_pos is not None or default_reset_mode is "tuck", this will be ignored.
                 Otherwise the reset_joint_pos will be initialized to the precomputed joint positions that represents default_arm_pose.
             kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing
                 for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject).
         """
-        # Store args
-        self.rigid_trunk = rigid_trunk
-        self.default_trunk_offset = default_trunk_offset
-        assert_valid_key(key=default_reset_mode, valid_keys=RESET_JOINT_OPTIONS, name="default_reset_mode")
-        self.default_reset_mode = default_reset_mode
-        assert_valid_key(key=default_arm_pose, valid_keys=DEFAULT_ARM_POSES, name="default_arm_pose")
-        self.default_arm_pose = default_arm_pose
-
         # Run super init
         super().__init__(
             relative_prim_path=relative_prim_path,
@@ -151,6 +134,10 @@ def __init__(
             sensor_config=sensor_config,
             grasping_mode=grasping_mode,
             disable_grasp_handling=disable_grasp_handling,
+            rigid_trunk=rigid_trunk,
+            default_trunk_offset=default_trunk_offset,
+            default_reset_mode=default_reset_mode,
+            default_arm_pose=default_arm_pose,
             **kwargs,
         )
 
@@ -177,37 +164,23 @@ def tucked_default_joint_pos(self):
 
     @property
     def untucked_default_joint_pos(self):
-        pos = th.zeros(self.n_joints)
+        pos = super().untucked_default_joint_pos
         pos[self.base_control_idx] = 0.0
         pos[self.trunk_control_idx] = 0.02 + self.default_trunk_offset
         pos[self.camera_control_idx] = th.tensor([0.0, 0.45])
         pos[self.gripper_control_idx[self.default_arm]] = th.tensor([0.05, 0.05])  # open gripper
-
-        # Choose arm based on setting
-        if self.default_arm_pose == "vertical":
-            pos[self.arm_control_idx[self.default_arm]] = th.tensor(
-                [-0.94121, -0.64134, 1.55186, 1.65672, -0.93218, 1.53416, 2.14474]
-            )
-        elif self.default_arm_pose == "diagonal15":
-            pos[self.arm_control_idx[self.default_arm]] = th.tensor(
-                [-0.95587, -0.34778, 1.46388, 1.47821, -0.93813, 1.4587, 1.9939]
-            )
-        elif self.default_arm_pose == "diagonal30":
-            pos[self.arm_control_idx[self.default_arm]] = th.tensor(
-                [-1.06595, -0.22184, 1.53448, 1.46076, -0.84995, 1.36904, 1.90996]
-            )
-        elif self.default_arm_pose == "diagonal45":
-            pos[self.arm_control_idx[self.default_arm]] = th.tensor(
-                [-1.11479, -0.0685, 1.5696, 1.37304, -0.74273, 1.3983, 1.79618]
-            )
-        elif self.default_arm_pose == "horizontal":
-            pos[self.arm_control_idx[self.default_arm]] = th.tensor(
-                [-1.43016, 0.20965, 1.86816, 1.77576, -0.27289, 1.31715, 2.01226]
-            )
-        else:
-            raise ValueError("Unknown default arm pose: {}".format(self.default_arm_pose))
         return pos
 
+    @property
+    def default_arm_poses(self):
+        return {
+            "vertical": th.tensor([-0.94121, -0.64134, 1.55186, 1.65672, -0.93218, 1.53416, 2.14474]),
+            "diagonal15": th.tensor([-0.95587, -0.34778, 1.46388, 1.47821, -0.93813, 1.4587, 1.9939]),
+            "diagonal30": th.tensor([-1.06595, -0.22184, 1.53448, 1.46076, -0.84995, 1.36904, 1.90996]),
+            "diagonal45": th.tensor([-1.11479, -0.0685, 1.5696, 1.37304, -0.74273, 1.3983, 1.79618]),
+            "horizontal": th.tensor([-1.43016, 0.20965, 1.86816, 1.77576, -0.27289, 1.31715, 2.01226]),
+        }
+
     def _post_load(self):
         super()._post_load()
 
@@ -233,50 +206,6 @@ def discrete_action_list(self):
     def _create_discrete_action_space(self):
         raise ValueError("Fetch does not support discrete actions!")
 
-    def tuck(self):
-        """
-        Immediately set this robot's configuration to be in tucked mode
-        """
-        self.set_joint_positions(self.tucked_default_joint_pos)
-
-    def untuck(self):
-        """
-        Immediately set this robot's configuration to be in untucked mode
-        """
-        self.set_joint_positions(self.untucked_default_joint_pos)
-
-    def _initialize(self):
-        # Run super method first
-        super()._initialize()
-
-    def _postprocess_control(self, control, control_type):
-        # Run super method first
-        u_vec, u_type_vec = super()._postprocess_control(control=control, control_type=control_type)
-
-        # Override trunk value if we're keeping the trunk rigid
-        if self.rigid_trunk:
-            u_vec[self.trunk_control_idx] = self.untucked_default_joint_pos[self.trunk_control_idx]
-            u_type_vec[self.trunk_control_idx] = ControlType.POSITION
-
-        # Return control
-        return u_vec, u_type_vec
-
-    def _get_proprioception_dict(self):
-        dic = super()._get_proprioception_dict()
-
-        # Add trunk info
-        joint_positions = ControllableObjectViewAPI.get_joint_positions(self.articulation_root_path)
-        joint_velocities = ControllableObjectViewAPI.get_joint_velocities(self.articulation_root_path)
-        dic["trunk_qpos"] = joint_positions[self.trunk_control_idx]
-        dic["trunk_qvel"] = joint_velocities[self.trunk_control_idx]
-
-        return dic
-
-    @property
-    def default_proprio_obs(self):
-        obs_keys = super().default_proprio_obs
-        return obs_keys + ["trunk_qpos"]
-
     @property
     def controller_order(self):
         # Ordered by general robot kinematics chain
@@ -295,35 +224,6 @@ def _default_controllers(self):
 
         return controllers
 
-    @property
-    def _default_controller_config(self):
-        # Grab defaults from super method first
-        cfg = super()._default_controller_config
-
-        # Need to override joint idx being controlled to include trunk in default arm controller configs
-        for arm_cfg in cfg[f"arm_{self.default_arm}"].values():
-            arm_control_idx = th.cat([self.trunk_control_idx, self.arm_control_idx[self.default_arm]])
-            arm_cfg["dof_idx"] = arm_control_idx
-
-            # Need to modify the default joint positions also if this is a null joint controller
-            if arm_cfg["name"] == "NullJointController":
-                arm_cfg["default_command"] = self.reset_joint_pos[arm_control_idx]
-
-            # If using rigid trunk, we also clamp its limits
-            if self.rigid_trunk:
-                arm_cfg["control_limits"]["position"][0][self.trunk_control_idx] = self.untucked_default_joint_pos[
-                    self.trunk_control_idx
-                ]
-                arm_cfg["control_limits"]["position"][1][self.trunk_control_idx] = self.untucked_default_joint_pos[
-                    self.trunk_control_idx
-                ]
-
-        return cfg
-
-    @property
-    def _default_joint_pos(self):
-        return self.tucked_default_joint_pos if self.default_reset_mode == "tuck" else self.untucked_default_joint_pos
-
     @property
     def wheel_radius(self):
         return 0.0613
@@ -354,14 +254,6 @@ def assisted_grasp_end_points(self):
             ]
         }
 
-    @property
-    def trunk_control_idx(self):
-        """
-        Returns:
-            n-array: Indices in low-level control vector corresponding to trunk joints.
-        """
-        return th.tensor([list(self.joints.keys()).index(name) for name in self.trunk_joint_names])
-
     @property
     def disabled_collision_pairs(self):
         return [

omnigibson/robots/franka.py

@@ -44,8 +44,7 @@ def __init__(
         """
         Args:
             name (str): Name for the object. Names need to be unique per scene
-            prim_path (None or str): global path in the stage to this object. If not specified, will automatically be
-                created at /World/<name>
+            relative_prim_path (str): Scene-local prim path of the Prim to encapsulate or create.
             scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale
                 for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a
                 3-array specifies per-axis scaling.

omnigibson/robots/manipulation_robot.py

@@ -92,8 +92,7 @@ def __init__(
         """
         Args:
             name (str): Name for the object. Names need to be unique per scene
-            prim_path (None or str): global path in the stage to this object. If not specified, will automatically be
-                created at /World/<name>
+            relative_prim_path (str): Scene-local prim path of the Prim to encapsulate or create.
             scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale
                 for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a
                 3-array specifies per-axis scaling.