New task: DClaw Valve Rotating

mani_skill2/agents/robots/__init__.py

@@ -1,8 +1,9 @@
 from mani_skill2.agents.robots.anymal.anymal_c import ANYmalC
-
+from .dclaw import DClaw
 from .fetch import Fetch
 from .mobile_panda import MobilePandaDualArm, MobilePandaSingleArm
 from .panda import Panda
+from .xarm import XArm7Ability
 from .xmate3 import Xmate3Robotiq
 
 ROBOTS = {
@@ -11,5 +12,9 @@
     "mobile_panda_single_arm": MobilePandaSingleArm,
     "xmate3_robotiq": Xmate3Robotiq,
     "fetch": Fetch,
+    # Dexterous Hand
+    "dclaw": DClaw,
+    "xarm7_ability": XArm7Ability,
+    # Locomotion
     "anymal-c": ANYmalC,
 }

mani_skill2/agents/robots/dclaw/__init__.py

@@ -0,0 +1 @@
+from .dclaw import DClaw

mani_skill2/agents/robots/dclaw/dclaw.py

@@ -0,0 +1,144 @@
+from copy import deepcopy
+from typing import List
+
+import sapien
+import torch
+
+from mani_skill2 import PACKAGE_ASSET_DIR
+from mani_skill2.agents.base_agent import BaseAgent
+from mani_skill2.agents.controllers import *
+from mani_skill2.agents.utils import (
+    get_active_joint_indices,
+)
+from mani_skill2.utils.sapien_utils import (
+    get_objs_by_names,
+)
+from mani_skill2.utils.structs.pose import vectorize_pose
+
+
+class DClaw(BaseAgent):
+    uid = "dclaw"
+    urdf_path = f"{PACKAGE_ASSET_DIR}/robots/dclaw/dclaw_gripper_glb.urdf"
+    urdf_config = dict(
+        _materials=dict(
+            tip=dict(static_friction=2.0, dynamic_friction=1.0, restitution=0.0)
+        ),
+        link=dict(
+            link_f1_3=dict(material="tip", patch_radius=0.1, min_patch_radius=0.1),
+            link_f2_3=dict(material="tip", patch_radius=0.1, min_patch_radius=0.1),
+            link_f3_3=dict(material="tip", patch_radius=0.1, min_patch_radius=0.1),
+        ),
+    )
+    sensor_configs = {}
+
+    def __init__(self, *args, **kwargs):
+        self.joint_names = [
+            "joint_f1_0",
+            "joint_f2_0",
+            "joint_f3_0",
+            "joint_f1_1",
+            "joint_f2_1",
+            "joint_f3_1",
+            "joint_f1_2",
+            "joint_f2_2",
+            "joint_f3_2",
+        ]
+
+        self.joint_stiffness = 1e2
+        self.joint_damping = 1e1
+        self.joint_force_limit = 2e1
+        self.tip_link_names = ["link_f1_head", "link_f2_head", "link_f3_head"]
+        self.root_joint_names = ["joint_f1_0", "joint_f2_0", "joint_f3_0"]
+
+        super().__init__(*args, **kwargs)
+        self.tip_poses = torch.zeros(1).to(self.device)
+
+    def _after_init(self):
+        self.tip_links: List[sapien.Entity] = get_objs_by_names(
+            self.robot.get_links(), self.tip_link_names
+        )
+        self.root_joints = [
+            self.robot.find_joint_by_name(n) for n in self.root_joint_names
+        ]
+        self.root_joint_indices = get_active_joint_indices(
+            self.robot, self.root_joint_names
+        )
+
+    @property
+    def controller_configs(self):
+        # -------------------------------------------------------------------------- #
+        # Arm
+        # -------------------------------------------------------------------------- #
+        joint_pos = PDJointPosControllerConfig(
+            self.joint_names,
+            None,
+            None,
+            self.joint_stiffness,
+            self.joint_damping,
+            self.joint_force_limit,
+            normalize_action=False,
+        )
+        joint_delta_pos = PDJointPosControllerConfig(
+            self.joint_names,
+            -0.1,
+            0.1,
+            self.joint_stiffness,
+            self.joint_damping,
+            self.joint_force_limit,
+            use_delta=True,
+        )
+        joint_target_delta_pos = deepcopy(joint_delta_pos)
+        joint_target_delta_pos.use_target = True
+
+        # PD joint velocity
+        pd_joint_vel = PDJointVelControllerConfig(
+            self.joint_names,
+            -1.0,
+            1.0,
+            self.joint_damping,  # this might need to be tuned separately
+            self.joint_force_limit,
+        )
+
+        # PD joint position and velocity
+        joint_pos_vel = PDJointPosVelControllerConfig(
+            self.joint_names,
+            None,
+            None,
+            self.joint_stiffness,
+            self.joint_damping,
+            self.joint_force_limit,
+            normalize_action=False,
+        )
+        joint_delta_pos_vel = PDJointPosVelControllerConfig(
+            self.joint_names,
+            -0.1,
+            0.1,
+            self.joint_stiffness,
+            self.joint_damping,
+            self.joint_force_limit,
+            use_delta=True,
+        )
+
+        controller_configs = dict(
+            pd_joint_delta_pos=dict(joint=joint_delta_pos),
+            pd_joint_pos=dict(joint=joint_pos),
+            pd_joint_target_delta_pos=dict(joint=joint_target_delta_pos),
+            # Caution to use the following controllers
+            pd_joint_vel=dict(joint=pd_joint_vel),
+            pd_joint_pos_vel=dict(joint=joint_pos_vel),
+            pd_joint_delta_pos_vel=dict(joint=joint_delta_pos_vel),
+        )
+
+        # Make a deepcopy in case users modify any config
+        return deepcopy_dict(controller_configs)
+
+    def get_proprioception(self):
+        """
+        Get the proprioceptive state of the agent.
+        """
+        obs = super().get_proprioception()
+        tip_poses = [vectorize_pose(link.pose) for link in self.tip_links]
+        self.tip_poses = torch.stack(tip_poses, dim=1)
+        obs.update({"tip_poses": self.tip_poses.view(-1, 21)})
+
+        return obs

mani_skill2/agents/robots/xarm/__init__.py

@@ -0,0 +1 @@
+from .xarm7_ability import XArm7Ability

mani_skill2/agents/robots/xarm/xarm7_ability.py

@@ -0,0 +1,176 @@
+from copy import deepcopy
+from typing import Dict, Tuple
+
+import sapien.physx as physx
+
+from mani_skill2 import PACKAGE_ASSET_DIR
+from mani_skill2.agents.base_agent import BaseAgent
+from mani_skill2.agents.controllers import *
+from mani_skill2.utils.sapien_utils import (
+    get_obj_by_name,
+    get_objs_by_names,
+)
+
+
+class XArm7Ability(BaseAgent):
+    uid = "xarm7_ability"
+    urdf_path = f"{PACKAGE_ASSET_DIR}/robots/xarm7/xarm7_ability_right_hand.urdf"
+    urdf_config = dict(
+        _materials=dict(
+            front_finger=dict(
+                static_friction=2.0, dynamic_friction=1.5, restitution=0.0
+            )
+        ),
+        link=dict(
+            thumnb_L2=dict(
+                material="front_finger", patch_radius=0.05, min_patch_radius=0.04
+            ),
+            index_L2=dict(
+                material="front_finger", patch_radius=0.05, min_patch_radius=0.04
+            ),
+            middle_L2=dict(
+                material="front_finger", patch_radius=0.05, min_patch_radius=0.04
+            ),
+            ring_L2=dict(
+                material="front_finger", patch_radius=0.05, min_patch_radius=0.04
+            ),
+            pinky_L2=dict(
+                material="front_finger", patch_radius=0.05, min_patch_radius=0.04
+            ),
+        ),
+    )
+
+    def __init__(self, *args, **kwargs):
+        self.arm_joint_names = [
+            "joint1",
+            "joint2",
+            "joint3",
+            "joint4",
+            "joint5",
+            "joint6",
+            "joint7",
+        ]
+        self.arm_stiffness = 1e4
+        self.arm_damping = 1e3
+        self.arm_force_limit = 500
+
+        self.hand_joint_names = [
+            "thumb_q1",
+            "index_q1",
+            "middle_q1",
+            "ring_q1",
+            "pinky_q1",
+            "thumb_q2",
+            "index_q2",
+            "middle_q2",
+            "ring_q2",
+            "pinky_q2",
+        ]
+        self.hand_stiffness = 1e3
+        self.hand_damping = 1e2
+        self.hand_force_limit = 50
+
+        self.ee_link_name = "base"
+
+        super().__init__(*args, **kwargs)
+
+    @property
+    def controller_configs(self):
+        # -------------------------------------------------------------------------- #
+        # Arm
+        # -------------------------------------------------------------------------- #
+        arm_pd_joint_pos = PDJointPosControllerConfig(
+            self.arm_joint_names,
+            None,
+            None,
+            self.arm_stiffness,
+            self.arm_damping,
+            self.arm_force_limit,
+            normalize_action=False,
+        )
+        arm_pd_joint_delta_pos = PDJointPosControllerConfig(
+            self.arm_joint_names,
+            -0.1,
+            0.1,
+            self.arm_stiffness,
+            self.arm_damping,
+            self.arm_force_limit,
+            use_delta=True,
+        )
+        arm_pd_joint_target_delta_pos = deepcopy(arm_pd_joint_delta_pos)
+        arm_pd_joint_target_delta_pos.use_target = True
+
+        # PD ee position
+        arm_pd_ee_delta_pose = PDEEPoseControllerConfig(
+            self.arm_joint_names,
+            -0.1,
+            0.1,
+            0.1,
+            self.arm_stiffness,
+            self.arm_damping,
+            self.arm_force_limit,
+            ee_link=self.ee_link_name,
+            urdf_path=self.urdf_path,
+        )
+
+        arm_pd_ee_target_delta_pose = deepcopy(arm_pd_ee_delta_pose)
+        arm_pd_ee_target_delta_pose.use_target = True
+
+        # -------------------------------------------------------------------------- #
+        # Hand
+        # -------------------------------------------------------------------------- #
+        hand_target_delta_pos = PDJointPosControllerConfig(
+            self.hand_joint_names,
+            -0.1,
+            0.1,
+            self.hand_stiffness,
+            self.hand_damping,
+            self.hand_force_limit,
+            use_delta=True,
+        )
+        hand_target_delta_pos.use_target = True
+
+        controller_configs = dict(
+            pd_joint_delta_pos=dict(
+                arm=arm_pd_joint_delta_pos, gripper=hand_target_delta_pos
+            ),
+            pd_joint_pos=dict(arm=arm_pd_joint_pos, gripper=hand_target_delta_pos),
+            pd_ee_delta_pose=dict(
+                arm=arm_pd_ee_delta_pose, gripper=hand_target_delta_pos
+            ),
+            pd_ee_target_delta_pose=dict(
+                arm=arm_pd_ee_target_delta_pose, gripper=hand_target_delta_pos
+            ),
+        )
+
+        # Make a deepcopy in case users modify any config
+        return deepcopy_dict(controller_configs)
+
+    sensor_configs = []
+
+    def _after_init(self):
+        hand_front_link_names = [
+            "thumb_L2",
+            "index_L2",
+            "middle_L2",
+            "ring_L2",
+            "pinky_L2",
+        ]
+        self.hand_front_links = get_objs_by_names(
+            self.robot.get_links(), hand_front_link_names
+        )
+
+        finger_tip_link_names = [
+            "thumb_tip",
+            "index_tip",
+            "middle_tip",
+            "ring_tip",
+            "pinky_tip",
+        ]
+        self.finger_tip_links = get_objs_by_names(
+            self.robot.get_links(), finger_tip_link_names
+        )
+
+        self.tcp = get_obj_by_name(self.robot.get_links(), self.ee_link_name)
+
+        self.queries: Dict[str, Tuple[physx.PhysxGpuContactQuery, Tuple[int]]] = dict()