[Feature] Add a floating panda gripper and update missing docs on new robots

docs/source/robots/dextrous_hands/index.md

@@ -16,10 +16,10 @@ Robot Description: [https://github.com/haosulab/ManiSkill/tree/main/mani_skill/a
 
 ### Right Variant
 
-```{figure} ../images/allegro_hand_left.png
+```{figure} ../images/allegro_hand_right.png
 ```
 
-Robot UID: `allegro_hand_left`
+Robot UID: `allegro_hand_right`
 
 Agent Class Code: [https://github.com/haosulab/ManiSkill/blob/main/mani_skill/agents/robots/allegro_hand/allegro.py](https://github.com/haosulab/ManiSkill/blob/main/mani_skill/agents/robots/allegro_hand/allegro.py)
 

docs/source/robots/humanoids/index.md

@@ -5,18 +5,34 @@
 ```{figure} ../images/unitree_h1.png
 ```
 
-Robot UID: `unitree_h1`
+Robot UID: `unitree_h1, unitree_h1_simplified`
 
 Agent Class Code: [https://github.com/haosulab/ManiSkill/blob/main/mani_skill/agents/robots/unitree_h1/h1.py](https://github.com/haosulab/ManiSkill/blob/main/mani_skill/agents/robots/unitree_h1/h1.py)
 
 Robot Description: [https://github.com/haosulab/ManiSkill-UnitreeH1](https://github.com/haosulab/ManiSkill-UnitreeH1)
 
+Robot `unitree_h1` comes with a complete set of collision meshes and is not simplified. `unitree_h1_simplified` is a version where most collision meshes are removed and a minimal set is kept for use and faster simulation.
+
 ### With Dextrous Hands (WIP)
 
 ### With Mounted Sensors (WIP)
 
 Main blocker here is actually its unclear where people typically mount cameras/depth sensors onto Unitree H1
 
+## Unitree G1
+
+```{figure} ../images/unitree_g1.png
+```
+
+Robot UID: `unitree_g1, unitree_g1_simplified_legs`
+
+Agent Class Code: [https://github.com/haosulab/ManiSkill/blob/main/mani_skill/agents/robots/unitree_g1/g1.py](https://github.com/haosulab/ManiSkill/blob/main/mani_skill/agents/robots/unitree_g1/g1.py)
+
+Robot Description: [https://github.com/haosulab/ManiSkill-UnitreeG1](https://github.com/haosulab/ManiSkill-UnitreeG1)
+
+Robot `unitree_g1` comes with a complete set of collision meshes and is not simplified. `unitree_g1_simplified_legs` is a version where all collision meshes except those around the legs and feet are removed, useful for fast training without manipulation.
+
+
 ## Stompy (from KScale Labs)
 
 ```{figure} ../images/stompy.png

docs/source/robots/index.md

@@ -5,7 +5,7 @@
 
 
 
-This page showcases all the already built/modelled robots ready for simulation across a number of different categories. Some of them are displayed above in an empty environment using a predefined keyframe. Note that not all of these robots are used in tasks in ManiSkill, and some are not tuned for maximum efficiency yet. You can generally assume robots that are used in existing tasks in ManiSkill are of the highest quality and already tuned.
+This sections here show the already built/modelled robots ready for simulation across a number of different categories. Some of them are displayed above in an empty environment using a predefined keyframe. Note that not all of these robots are used in tasks in ManiSkill, and some are not tuned for maximum efficiency yet or for sim2real transfer. You can generally assume robots that are used in existing tasks in ManiSkill are of the highest quality and already tuned.
 
 To learn about how to load your own custom robots see [the tutorial](../user_guide/tutorials/custom_robots.md).
 

docs/source/robots/other/index.md

@@ -0,0 +1,14 @@
+# Other Robots
+
+Robots that don't fit into other standard categories (e.g. floating grippers)
+
+## Fetch Robot
+
+```{figure} ../images/floating_panda_gripper.png
+```
+
+Robot UID: `floating_panda_gripper`
+
+Agent Class Code: [https://github.com/haosulab/ManiSkill/blob/main/mani_skill/agents/robots/floating_panda_gripper/floating_panda_gripper.py](https://github.com/haosulab/ManiSkill/blob/main/mani_skill/agents/robots/floating_panda_gripper/floating_panda_gripper.py)
+
+Robot Description: [https://github.com/haosulab/ManiSkill/blob/main/mani_skill/assets/robots/panda/panda_v2_gripper.urdf](https://github.com/haosulab/ManiSkill/blob/main/mani_skill/assets/robots/panda/panda_v2_gripper.urdf)

mani_skill/agents/base_agent.py

@@ -76,8 +76,8 @@ def __init__(
         self,
         scene: ManiSkillScene,
         control_freq: int,
-        control_mode: str = None,
-        agent_idx: int = None,
+        control_mode: Optional[str] = None,
+        agent_idx: Optional[str] = None,
     ):
         self.scene = scene
         self._control_freq = control_freq

mani_skill/agents/robots/__init__.py

@@ -2,14 +2,15 @@
 from .anymal import ANYmalC
 from .dclaw import DClaw
 from .fetch import Fetch
+from .floating_panda_gripper import FloatingPandaGripper
 from .googlerobot import *
 from .humanoid import Humanoid
 from .panda import *
 from .stompy import Stompy
 from .trifingerpro import TriFingerPro
+from .unitree_g1 import *
 from .unitree_go import *
 from .unitree_h1 import *
-from .unitree_g1 import *
 from .ur_e import UR10e
 from .widowx import *
 from .xarm import XArm7Ability

mani_skill/agents/robots/floating_panda_gripper/__init__.py

@@ -0,0 +1 @@
+from .floating_panda_gripper import FloatingPandaGripper

mani_skill/agents/robots/floating_panda_gripper/floating_panda_gripper.py

@@ -0,0 +1,145 @@
+from copy import deepcopy
+
+import numpy as np
+import sapien
+
+from mani_skill import PACKAGE_ASSET_DIR
+from mani_skill.agents.base_agent import BaseAgent, Keyframe
+from mani_skill.agents.controllers import *
+from mani_skill.agents.registration import register_agent
+from mani_skill.sensors.camera import CameraConfig
+from mani_skill.utils import sapien_utils
+
+
+@register_agent()
+class FloatingPandaGripper(BaseAgent):
+    uid = "floating_panda_gripper"
+    urdf_path = f"{PACKAGE_ASSET_DIR}/robots/panda/panda_v2_gripper.urdf"  # You can use f"{PACKAGE_ASSET_DIR}" to reference a urdf file in the mani_skill /assets package folder
+    urdf_config = dict(
+        _materials=dict(
+            gripper=dict(static_friction=2.0, dynamic_friction=2.0, restitution=0.0)
+        ),
+        link=dict(
+            panda_leftfinger=dict(
+                material="gripper", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            panda_rightfinger=dict(
+                material="gripper", patch_radius=0.1, min_patch_radius=0.1
+            ),
+        ),
+    )
+    keyframes = dict(
+        open_facing_up=Keyframe(
+            qpos=[0, 0, 0, 0, 0, 0, 0, 0], pose=sapien.Pose(p=[0, 0, 0.5])
+        ),
+        open_facing_side=Keyframe(
+            qpos=[0, 0, 0, 0, np.pi / 2, 0, 0, 0], pose=sapien.Pose(p=[0, 0, 0.5])
+        ),
+        open_facing_down=Keyframe(
+            qpos=[0, 0, 0, 0, np.pi, 0, 0, 0], pose=sapien.Pose(p=[0, 0, 0.5])
+        ),
+    )
+    root_joint_names = [
+        "root_x_axis_joint",
+        "root_y_axis_joint",
+        "root_z_axis_joint",
+        "root_x_rot_joint",
+        "root_y_rot_joint",
+        "root_z_rot_joint",
+    ]
+    gripper_joint_names = [
+        "panda_finger_joint1",
+        "panda_finger_joint2",
+    ]
+    gripper_stiffness = 1e3
+    gripper_damping = 1e2
+    gripper_force_limit = 100
+
+    ee_link_name = "panda_hand_tcp"
+
+    @property
+    def _controller_configs(self):
+        pd_ee_pose = PDEEPoseControllerConfig(
+            self.root_joint_names,
+            None,
+            None,
+            self.gripper_stiffness,
+            self.gripper_damping,
+            self.gripper_force_limit,
+            use_delta=False,
+            frame="base",
+            ee_link=self.ee_link_name,
+            urdf_path=self.urdf_path,
+            normalize_action=False,
+        )
+        pd_ee_pose_quat = deepcopy(pd_ee_pose)
+        pd_ee_pose_quat.rotation_convention = "quaternion"
+        pd_ee_delta_pose = PDEEPoseControllerConfig(
+            joint_names=self.root_joint_names,
+            pos_lower=-0.1,
+            pos_upper=0.1,
+            rot_lower=-0.1,
+            rot_upper=0.1,
+            stiffness=self.gripper_stiffness,
+            damping=self.gripper_damping,
+            force_limit=self.gripper_force_limit,
+            ee_link=self.ee_link_name,
+            urdf_path=self.urdf_path,
+        )
+        arm_pd_joint_pos = PDJointPosControllerConfig(
+            joint_names=self.root_joint_names,
+            lower=None,
+            upper=None,
+            stiffness=self.gripper_stiffness,
+            damping=self.gripper_damping,
+            force_limit=self.gripper_force_limit,
+            normalize_action=False,
+        )
+        arm_pd_joint_delta_pos = PDJointPosControllerConfig(
+            joint_names=self.root_joint_names,
+            lower=-0.1,
+            upper=0.1,
+            stiffness=self.gripper_stiffness,
+            damping=self.gripper_damping,
+            force_limit=self.gripper_force_limit,
+            use_delta=True,
+        )
+
+        gripper_pd_joint_pos = PDJointPosMimicControllerConfig(
+            self.gripper_joint_names,
+            -0.01,  # a trick to have force when the object is thin
+            0.04,
+            self.gripper_stiffness,
+            self.gripper_damping,
+            self.gripper_force_limit,
+        )
+        return dict(
+            pd_joint_delta_pos=dict(
+                root=arm_pd_joint_delta_pos, gripper=gripper_pd_joint_pos
+            ),
+            pd_joint_pos=dict(root=arm_pd_joint_pos, gripper=gripper_pd_joint_pos),
+            pd_ee_delta_pose=dict(root=pd_ee_delta_pose, gripper=gripper_pd_joint_pos),
+            pd_ee_pose=dict(root=pd_ee_pose, gripper=gripper_pd_joint_pos),
+            pd_ee_pose_quat=dict(root=pd_ee_pose_quat, gripper=gripper_pd_joint_pos),
+        )
+
+    @property
+    def _sensor_configs(self):
+        return []
+
+    def _after_init(self):
+        self.finger1_link = sapien_utils.get_obj_by_name(
+            self.robot.get_links(), "panda_leftfinger"
+        )
+        self.finger2_link = sapien_utils.get_obj_by_name(
+            self.robot.get_links(), "panda_rightfinger"
+        )
+        self.finger1pad_link = sapien_utils.get_obj_by_name(
+            self.robot.get_links(), "panda_leftfinger_pad"
+        )
+        self.finger2pad_link = sapien_utils.get_obj_by_name(
+            self.robot.get_links(), "panda_rightfinger_pad"
+        )
+        self.tcp = sapien_utils.get_obj_by_name(
+            self.robot.get_links(), self.ee_link_name
+        )

mani_skill/agents/robots/panda/panda.py

@@ -129,10 +129,6 @@ def _controller_configs(self):
         arm_pd_ee_target_delta_pose = deepcopy(arm_pd_ee_delta_pose)
         arm_pd_ee_target_delta_pose.use_target = True
 
-        # PD ee position (for human-interaction/teleoperation)
-        arm_pd_ee_delta_pose_align = deepcopy(arm_pd_ee_delta_pose)
-        arm_pd_ee_delta_pose_align.frame = "ee_align"
-
         # PD joint velocity
         arm_pd_joint_vel = PDJointVelControllerConfig(
             self.arm_joint_names,
@@ -185,9 +181,6 @@ def _controller_configs(self):
             pd_ee_delta_pose=dict(
                 arm=arm_pd_ee_delta_pose, gripper=gripper_pd_joint_pos
             ),
-            pd_ee_delta_pose_align=dict(
-                arm=arm_pd_ee_delta_pose_align, gripper=gripper_pd_joint_pos
-            ),
             # TODO(jigu): how to add boundaries for the following controllers
             pd_joint_target_delta_pos=dict(
                 arm=arm_pd_joint_target_delta_pos, gripper=gripper_pd_joint_pos

mani_skill/assets/robots/panda/panda_v2_gripper.urdf

@@ -4,6 +4,84 @@
 <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
 <!-- =================================================================================== -->
 <robot name="panda">
+  <link name="root">
+    <inertial>
+      <mass value="0"/>
+      <inertia ixx="0.00" ixy="0.00" ixz="0.00" iyy="0.00" iyz="0.00" izz="0.00"/>
+    </inertial>
+  </link>
+  <link name="root_arm_1_link_1">
+    <inertial>
+      <mass value="0"/>
+      <inertia ixx="0.00" ixy="0.00" ixz="0.00" iyy="0.00" iyz="0.00" izz="0.00"/>
+    </inertial>
+  </link>
+  <link name="root_arm_1_link_2">
+    <inertial>
+      <mass value="0"/>
+      <inertia ixx="0.00" ixy="0.00" ixz="0.00" iyy="0.00" iyz="0.00" izz="0.00"/>
+    </inertial>
+  </link>
+  <link name="root_arm_1_link_3">
+    <inertial>
+      <mass value="0"/>
+      <inertia ixx="0.00" ixy="0.00" ixz="0.00" iyy="0.00" iyz="0.00" izz="0.00"/>
+    </inertial>
+  </link>
+  <link name="root_arm_1_link_4">
+    <inertial>
+      <mass value="0"/>
+      <inertia ixx="0.00" ixy="0.00" ixz="0.00" iyy="0.00" iyz="0.00" izz="0.00"/>
+    </inertial>
+  </link>
+  <link name="root_arm_1_link_5">
+    <inertial>
+      <mass value="0"/>
+      <inertia ixx="0.00" ixy="0.00" ixz="0.00" iyy="0.00" iyz="0.00" izz="0.00"/>
+    </inertial>
+  </link>
+  <joint name="root_x_axis_joint" type="prismatic">
+    <parent link="root"/>
+    <child link="root_arm_1_link_1"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" lower="-20" upper="20" velocity="0.50"/>
+    <dynamics damping="10.0" friction="10"/>
+  </joint>
+  <joint name="root_y_axis_joint" type="prismatic">
+    <parent link="root_arm_1_link_1"/>
+    <child link="root_arm_1_link_2"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="100" lower="-20" upper="20" velocity="0.50"/>
+    <dynamics damping="10.0" friction="10"/>
+  </joint>
+  <joint name="root_z_axis_joint" type="prismatic">
+    <parent link="root_arm_1_link_2"/>
+    <child link="root_arm_1_link_3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="100" lower="-20" upper="20" velocity="0.50"/>
+    <dynamics damping="10.0" friction="10"/>
+  </joint>
+  <joint name="root_x_rot_joint" type="revolute">
+    <parent link="root_arm_1_link_3"/>
+    <child link="root_arm_1_link_4"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" lower="-20" upper="20" velocity="0.50"/>
+    <dynamics damping="10.0" friction="10"/>
+  </joint>
+  <joint name="root_y_rot_joint" type="revolute">
+    <parent link="root_arm_1_link_4"/>
+    <child link="root_arm_1_link_5"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="100" lower="-20" upper="20" velocity="0.50"/>
+    <dynamics damping="10.0" friction="10"/>
+  </joint>
+  <joint name="root_z_rot_joint" type="revolute">
+    <parent link="root_arm_1_link_5"/>
+    <child link="panda_hand"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="100" lower="-20" upper="20" velocity="0.50"/>
+    <dynamics damping="10.0" friction="10"/>
+  </joint>
   <link name="panda_hand">
     <visual>
       <geometry>

mani_skill/examples/demo_robot.py

@@ -41,22 +41,27 @@ def main():
     )
     env.reset(seed=0)
     env: BaseEnv = env.unwrapped
+    print(f"Selected robot {args.robot_uid}. Control mode: {args.control_mode}")
     print("Selected Robot has the following keyframes to view: ")
     print(env.agent.keyframes.keys())
     env.agent.robot.set_qpos(env.agent.robot.qpos * 0)
     kf = None
     if len(env.agent.keyframes) > 0:
+        kf_name = None
         if args.keyframe is not None:
-            kf = env.agent.keyframes[args.keyframe]
+            kf_name = args.keyframe
+            kf = env.agent.keyframes[kf_name]
         else:
-            for kf in env.agent.keyframes.values():
+            for kf_name, kf in env.agent.keyframes.items():
                 # keep the first keyframe we find
                 break
         if kf.qpos is not None:
             env.agent.robot.set_qpos(kf.qpos)
         if kf.qvel is not None:
             env.agent.robot.set_qvel(kf.qvel)
         env.agent.robot.set_pose(kf.pose)
+        if kf_name is not None:
+            print(f"Viewing keyframe {kf_name}")
     if env.gpu_sim_enabled:
         env.scene._gpu_apply_all()
         env.scene.px.gpu_update_articulation_kinematics()