From 550109a244ad6b3de419ac87d5570638101ce2f8 Mon Sep 17 00:00:00 2001 From: Stone Tao Date: Wed, 6 Dec 2023 13:13:29 -0800 Subject: [PATCH] refactor xmate3 --- mani_skill2/agents/robots/__init__.py | 5 +- mani_skill2/agents/robots/xmate3/__init__.py | 1 - mani_skill2/agents/robots/xmate3/configs.py | 146 ----------------- mani_skill2/agents/robots/xmate3/xmate3.py | 155 ++++++++++++++++++- 4 files changed, 151 insertions(+), 156 deletions(-) delete mode 100644 mani_skill2/agents/robots/xmate3/configs.py diff --git a/mani_skill2/agents/robots/__init__.py b/mani_skill2/agents/robots/__init__.py index c77a131b..dc29e92c 100644 --- a/mani_skill2/agents/robots/__init__.py +++ b/mani_skill2/agents/robots/__init__.py @@ -1,11 +1,10 @@ from .mobile_panda import MobilePandaDualArm, MobilePandaSingleArm from .panda import Panda - -# from .xmate3 import Xmate3Robotiq +from .xmate3 import Xmate3Robotiq ROBOTS = { "panda": Panda, "mobile_panda_dual_arm": MobilePandaDualArm, "mobile_panda_single_arm": MobilePandaSingleArm, - # "xmate3_robotiq": Xmate3Robotiq, + "xmate3_robotiq": Xmate3Robotiq, } diff --git a/mani_skill2/agents/robots/xmate3/__init__.py b/mani_skill2/agents/robots/xmate3/__init__.py index c7a52e34..f42133e0 100644 --- a/mani_skill2/agents/robots/xmate3/__init__.py +++ b/mani_skill2/agents/robots/xmate3/__init__.py @@ -1,2 +1 @@ -from .configs import Xmate3RobotiqDefaultConfig from .xmate3 import Xmate3Robotiq diff --git a/mani_skill2/agents/robots/xmate3/configs.py b/mani_skill2/agents/robots/xmate3/configs.py deleted file mode 100644 index d88ca466..00000000 --- a/mani_skill2/agents/robots/xmate3/configs.py +++ /dev/null @@ -1,146 +0,0 @@ -from mani_skill2 import ASSET_DIR -from mani_skill2.agents.controllers import * -from mani_skill2.sensors.camera import CameraConfig - - -class Xmate3RobotiqDefaultConfig: - def __init__(self) -> None: - self.urdf_path = f"{ASSET_DIR}/robots/xmate3_robotiq/xmate3_robotiq.urdf" - self.urdf_config = dict( - _materials=dict( - gripper=dict(static_friction=2.0, dynamic_friction=2.0, restitution=0.0) - ), - link=dict( - left_inner_finger_pad=dict( - material="gripper", patch_radius=0.1, min_patch_radius=0.1 - ), - right_inner_finger_pad=dict( - material="gripper", patch_radius=0.1, min_patch_radius=0.1 - ), - ), - ) - - self.arm_joint_names = [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - "joint7", - ] - self.arm_stiffness = 1e3 - self.arm_damping = 1e2 - self.arm_force_limit = 100 - - self.gripper_joint_names = [ - "robotiq_2f_140_left_driver_joint", - "robotiq_2f_140_right_driver_joint", - ] - self.gripper_stiffness = 1e3 - self.gripper_damping = 1e2 - self.gripper_force_limit = 100 - - self.ee_link_name = "grasp_convenient_link" - - @property - def controllers(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, - ) - - # PD ee position - arm_pd_ee_delta_pos = PDEEPosControllerConfig( - self.arm_joint_names, - -0.1, - 0.1, - self.arm_stiffness, - self.arm_damping, - self.arm_force_limit, - ee_link=self.ee_link_name, - ) - 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, - ) - - # -------------------------------------------------------------------------- # - # Gripper - # -------------------------------------------------------------------------- # - # NOTE(jigu): IssacGym uses large P and D but with force limit - # However, tune a good force limit to have a good mimic behavior - gripper_pd_joint_pos = PDJointPosMimicControllerConfig( - self.gripper_joint_names, - 0, - 0.068 + 0.01, - self.gripper_stiffness, - self.gripper_damping, - self.gripper_force_limit, - ) - - controller_configs = dict( - pd_joint_pos=dict(arm=arm_pd_joint_pos, gripper=gripper_pd_joint_pos), - arm_pd_joint_delta_pos=dict( - arm=arm_pd_joint_delta_pos, gripper=gripper_pd_joint_pos - ), - pd_ee_delta_pos=dict(arm=arm_pd_ee_delta_pos, gripper=gripper_pd_joint_pos), - pd_ee_delta_pose=dict( - arm=arm_pd_ee_delta_pose, gripper=gripper_pd_joint_pos - ), - ) - - # Make a deepcopy in case users modify any config - return deepcopy_dict(controller_configs) - - @property - def sensors(self): - return [ - CameraConfig( - uid="base_camera", - p=[0.0, 0.0, 0.0], - q=[1, 0, 0, 0], - width=128, - height=128, - fov=1.5707, - near=0.01, - far=10, - entity_uid="camera_base_link", - hide_link=False, - ), - CameraConfig( - uid="hand_camera", - p=[0.0, 0.0, 0.0], - q=[1, 0, 0, 0], - width=128, - height=128, - fov=1.5707, - near=0.01, - far=10, - entity_uid="camera_hand_link", - hide_link=False, - ), - ] diff --git a/mani_skill2/agents/robots/xmate3/xmate3.py b/mani_skill2/agents/robots/xmate3/xmate3.py index 57aa74ac..7f981e5b 100644 --- a/mani_skill2/agents/robots/xmate3/xmate3.py +++ b/mani_skill2/agents/robots/xmate3/xmate3.py @@ -2,8 +2,10 @@ import sapien import sapien.physx as physx +from mani_skill2 import ASSET_DIR from mani_skill2.agents.base_agent import BaseAgent -from mani_skill2.agents.robots.xmate3.configs import Xmate3RobotiqDefaultConfig +from mani_skill2.agents.controllers import * +from mani_skill2.sensors.camera import CameraConfig from mani_skill2.utils.common import compute_angle_between from mani_skill2.utils.sapien_utils import ( compute_total_impulse, @@ -14,11 +16,52 @@ class Xmate3Robotiq(BaseAgent): - config: Xmate3RobotiqDefaultConfig + uid = "xmate3_robotiq" + urdf_path = f"{ASSET_DIR}/robots/xmate3_robotiq/xmate3_robotiq.urdf" + urdf_config = dict( + _materials=dict( + gripper=dict(static_friction=2.0, dynamic_friction=2.0, restitution=0.0) + ), + link=dict( + left_inner_finger_pad=dict( + material="gripper", patch_radius=0.1, min_patch_radius=0.1 + ), + right_inner_finger_pad=dict( + material="gripper", patch_radius=0.1, min_patch_radius=0.1 + ), + ), + ) - @classmethod - def get_default_config(cls): - return Xmate3RobotiqDefaultConfig() + def __init__( + self, + scene: sapien.Scene, + control_freq: int, + control_mode: str = None, + fix_root_link=True, + ): + self.arm_joint_names = [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ] + self.arm_stiffness = 1e3 + self.arm_damping = 1e2 + self.arm_force_limit = 100 + + self.gripper_joint_names = [ + "robotiq_2f_140_left_driver_joint", + "robotiq_2f_140_right_driver_joint", + ] + self.gripper_stiffness = 1e3 + self.gripper_damping = 1e2 + self.gripper_force_limit = 100 + + self.ee_link_name = "grasp_convenient_link" + super().__init__(scene, control_freq, control_mode, fix_root_link) def _after_init(self): self.finger1_link: sapien.Entity = get_obj_by_name( @@ -28,8 +71,108 @@ def _after_init(self): self.robot.get_links(), "right_inner_finger_pad" ).entity self.tcp: physx.PhysxArticulationLinkComponent = get_obj_by_name( - self.robot.get_links(), self.config.ee_link_name + self.robot.get_links(), self.ee_link_name + ) + + @property + def controllers(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, + ) + + # PD ee position + arm_pd_ee_delta_pos = PDEEPosControllerConfig( + self.arm_joint_names, + -0.1, + 0.1, + self.arm_stiffness, + self.arm_damping, + self.arm_force_limit, + ee_link=self.ee_link_name, ) + 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, + ) + + # -------------------------------------------------------------------------- # + # Gripper + # -------------------------------------------------------------------------- # + # NOTE(jigu): IssacGym uses large P and D but with force limit + # However, tune a good force limit to have a good mimic behavior + gripper_pd_joint_pos = PDJointPosMimicControllerConfig( + self.gripper_joint_names, + 0, + 0.068 + 0.01, + self.gripper_stiffness, + self.gripper_damping, + self.gripper_force_limit, + ) + + controller_configs = dict( + pd_joint_pos=dict(arm=arm_pd_joint_pos, gripper=gripper_pd_joint_pos), + arm_pd_joint_delta_pos=dict( + arm=arm_pd_joint_delta_pos, gripper=gripper_pd_joint_pos + ), + pd_ee_delta_pos=dict(arm=arm_pd_ee_delta_pos, gripper=gripper_pd_joint_pos), + pd_ee_delta_pose=dict( + arm=arm_pd_ee_delta_pose, gripper=gripper_pd_joint_pos + ), + ) + + # Make a deepcopy in case users modify any config + return deepcopy_dict(controller_configs) + + sensor_configs = [ + CameraConfig( + uid="base_camera", + p=[0.0, 0.0, 0.0], + q=[1, 0, 0, 0], + width=128, + height=128, + fov=1.5707, + near=0.01, + far=10, + entity_uid="camera_base_link", + hide_link=False, + ), + CameraConfig( + uid="hand_camera", + p=[0.0, 0.0, 0.0], + q=[1, 0, 0, 0], + width=128, + height=128, + fov=1.5707, + near=0.01, + far=10, + entity_uid="camera_hand_link", + hide_link=False, + ), + ] def is_grasping(self, object: sapien.Entity = None, min_impulse=1e-6, max_angle=85): contacts = self.scene.get_contacts()