diff --git a/examples/taxim/grasp_digit_demo.py b/examples/taxim/grasp_digit_demo.py index c1f29ca7..0424be1d 100644 --- a/examples/taxim/grasp_digit_demo.py +++ b/examples/taxim/grasp_digit_demo.py @@ -23,11 +23,11 @@ def _progress(iterable): class PickUpDemo: def __init__(self, env: gym.Env): self.env = env - self._robot = cast(SimRobot, self.env.get_wrapper_attr("robot")["robot"]) + self._robot = cast(SimRobot, self.env.get_wrapper_attr("robot")["right"]) self.home_pose = self._robot.get_cartesian_position() def _action(self, pose: Pose, gripper: list[float]) -> dict[str, Any]: - return {"robot": {"xyzrpy": pose.xyzrpy(), "gripper": gripper}} + return {"right": {"xyzrpy": pose.xyzrpy(), "gripper": gripper}} def get_object_pose(self, geom_name: str) -> Pose: model = self.env.get_wrapper_attr("sim").model diff --git a/extensions/rcs_fr3/src/rcs_fr3/configs.py b/extensions/rcs_fr3/src/rcs_fr3/configs.py index f55ff42a..2a25d056 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/configs.py +++ b/extensions/rcs_fr3/src/rcs_fr3/configs.py @@ -119,7 +119,7 @@ def config(self) -> FR3MultiHardwareEnvCreatorConfig: cfg = base.config() cfg.robot_cfg.async_control = True cfg.robot_cfg.ip = self.robot_ip - cfg.robot_cfg.tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")] + cfg.robot_cfg.tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")] cfg.robot_cfg.q_home = rcs.HOME_POSITIONS["FR3_DROID"] return FR3MultiHardwareEnvCreatorConfig( @@ -209,8 +209,8 @@ def config(self) -> FR3MultiHardwareEnvCreatorConfig: cfg = super().config() cfg.camera_cfgs = None - cfg.robot_cfgs["left"].tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")] - cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")] + cfg.robot_cfgs["left"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")] + cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")] cfg.robot_cfgs["left"].q_home = rcs.HOME_POSITIONS["FR3_DUO_LEFT"] cfg.robot_cfgs["right"].q_home = rcs.HOME_POSITIONS["FR3_DUO_RIGHT"] cfg.gripper_cfgs = { diff --git a/extensions/rcs_panda/src/rcs_panda/configs.py b/extensions/rcs_panda/src/rcs_panda/configs.py index 368b5d98..d90453e5 100644 --- a/extensions/rcs_panda/src/rcs_panda/configs.py +++ b/extensions/rcs_panda/src/rcs_panda/configs.py @@ -60,7 +60,7 @@ def config(self) -> PandaMultiHardwareEnvCreatorConfig: cfg = base.config() cfg.robot_cfg.async_control = True cfg.robot_cfg.ip = self.robot_ip - cfg.robot_cfg.tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")] + cfg.robot_cfg.tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")] cfg.robot_cfg.q_home = rcs.ROBOTS[RobotType.Panda].q_home return PandaMultiHardwareEnvCreatorConfig( diff --git a/extensions/rcs_taxim/src/rcs_taxim/creators.py b/extensions/rcs_taxim/src/rcs_taxim/creators.py index 3ebd54f0..5fdf2e62 100644 --- a/extensions/rcs_taxim/src/rcs_taxim/creators.py +++ b/extensions/rcs_taxim/src/rcs_taxim/creators.py @@ -84,6 +84,7 @@ def __call__( scene = EmptyWorldFR3() cfg = scene.config() + cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[rcs.common.GripperType("Robotiq2F85")] cfg.control_mode = control_mode cfg.headless = render_mode != "human" cfg.sim_cfg.realtime = render_mode == "human" @@ -97,8 +98,8 @@ def __call__( cfg.relative_to = RelativeTo.LAST_STEP if delta_actions else RelativeTo.NONE if not delta_actions: cfg.max_relative_movement = None - cfg.gripper_cfgs = {"robot": _taxim_gripper_cfg()} - cfg.gripper_offsets = None + cfg.gripper_cfgs = {"right": _taxim_gripper_cfg()} + cfg.gripper_offsets = {"right": rcs.GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType("Robotiq2F85")]} cfg.root_frame_objects = { "": ( rcs.OBJECT_PATHS["green_cube"], diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 428ee4c3..730cb8a0 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -190,11 +190,20 @@ class RobotMetaConfig: common.GripperType("Robotiq2F85"): "assets/grippers/robotiq_2f85/robotiq_2f85.xml", } -GRIPPER_OFFSETS: dict[common.GripperType, common.Pose] = { +GRIPPER_TCP_OFFSETS: dict[common.GripperType, common.Pose] = { common.GripperType.FrankaHand: common.Pose(pose_matrix=common.FrankaHandTCPOffset()), common.GripperType("Robotiq2F85"): common.Pose(translation=np.array([0, 0.0, 0.1493])), } +GRIPPER_MOUNT_OFFSETS: dict[common.GripperType, common.Pose] = { + common.GripperType.FrankaHand: common.Pose( + rotation=common.FrankaHandTCPOffset()[:3, :3], translation=np.array([0.0, 0.0, 0.0]) + ), + common.GripperType("Robotiq2F85"): common.Pose( + translation=np.array([0.0, 0.0, 0.0]), quaternion=np.array([0.0, 0.0, 0.7071068, 0.7071068]) + ), +} + SCENE_PATHS: dict[str, str] = {"empty_world": "assets/scenes/empty_world/scene.xml"} OBJECT_PATHS: dict[str, str] = { @@ -212,9 +221,6 @@ class RobotMetaConfig: TASKS: dict[str, Any] = {} DEFAULT_TRANSFORMS = { - "FR3_ROBOTIQ_GRIPPER": common.Pose( - translation=np.array([0.0, 0.0, 0.0]), quaternion=np.array([0.0, 0.0, 0.7071068, 0.7071068]) - ), "FR3_ROBOTIQ_WRIST_D405_MOUNT": common.Pose( translation=np.array([0.0, 0.0, 0.0]), quaternion=np.array([0.0, 0.0, 0.7071068, 0.7071068]) ), diff --git a/python/rcs/envs/configs.py b/python/rcs/envs/configs.py index 2139c7d0..d9f23d95 100644 --- a/python/rcs/envs/configs.py +++ b/python/rcs/envs/configs.py @@ -4,7 +4,7 @@ import gymnasium as gym import numpy as np -from rcs._core.common import FrankaHandTCPOffset, GripperType, RobotType +from rcs._core.common import GripperType, RobotType from rcs._core.sim import ( CameraType, SimCameraConfig, @@ -24,7 +24,8 @@ from rcs import ( CAMERA_PATHS, DEFAULT_TRANSFORMS, - GRIPPER_OFFSETS, + GRIPPER_MOUNT_OFFSETS, + GRIPPER_TCP_OFFSETS, OBJECT_PATHS, SCENE_PATHS, ) @@ -39,7 +40,7 @@ def config(self) -> SimEnvCreatorConfig: q_home[-1] = np.pi / 4 robot_cfg: SimRobotConfig[Literal[7]] = SimRobotConfig( robot_type=RobotType.FR3, - tcp_offset=GRIPPER_OFFSETS[rcs.common.GripperType.FrankaHand], + tcp_offset=GRIPPER_TCP_OFFSETS[rcs.common.GripperType.FrankaHand], attachment_site=rcs.ROBOTS[RobotType.FR3].attachment_site, kinematic_model_path=rcs.ROBOTS[RobotType.FR3].mjcf_model_path, joint_rotational_tolerance=0.05 * (np.pi / 180.0), @@ -153,9 +154,7 @@ def config(self) -> SimEnvCreatorConfig: ), } gripper_offsets: dict[str, rcs.common.Pose] | None = { - self.robot_prefix_template: rcs.common.Pose( - rotation=FrankaHandTCPOffset()[:3, :3], translation=np.array([0.0, 0.0, 0.0]) - ) + self.robot_prefix_template: GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType.FrankaHand] } return SimEnvCreatorConfig( robot_cfgs=robot_cfgs, @@ -186,7 +185,7 @@ class EmptyWorldFR3Duo(SimEnvCreator): def config(self) -> SimEnvCreatorConfig: robot_cfg: SimRobotConfig[Literal[7]] = SimRobotConfig( - tcp_offset=GRIPPER_OFFSETS[rcs.common.GripperType("Robotiq2F85")], + tcp_offset=GRIPPER_TCP_OFFSETS[rcs.common.GripperType("Robotiq2F85")], robot_type=RobotType.FR3, attachment_site=rcs.ROBOTS[RobotType.FR3].attachment_site, kinematic_model_path=rcs.ROBOTS[RobotType.FR3].mjcf_model_path, @@ -327,9 +326,7 @@ def config(self) -> SimEnvCreatorConfig: robot_name="right", ), } - gripper_offset = rcs.common.Pose( - quaternion=np.array(self.gripper_mesh_quaternion_offset), translation=np.array([0.0, 0.0, 0.0]) - ) + gripper_offset = GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType("Robotiq2F85")] return SimEnvCreatorConfig( robot_cfgs=robot_cfgs, sim_cfg=sim_cfg, @@ -363,7 +360,7 @@ def config(self) -> SimEnvCreatorConfig: lead_robot_name = self.lead_robot_name(cfg) robot_cfg = cfg.robot_cfgs[lead_robot_name] - robot_cfg.tcp_offset = GRIPPER_OFFSETS[rcs.common.GripperType("Robotiq2F85")] + robot_cfg.tcp_offset = GRIPPER_TCP_OFFSETS[rcs.common.GripperType("Robotiq2F85")] robot_cfg.attachment_site = rcs.ROBOTS[rt].attachment_site robot_cfg.kinematic_model_path = rcs.ROBOTS[rt].mjcf_model_path robot_cfg.arm_collision_geoms = [] diff --git a/python/rcs/lerobot_joint_converter.py b/python/rcs/lerobot_joint_converter.py index f9efaeae..2e57a06d 100644 --- a/python/rcs/lerobot_joint_converter.py +++ b/python/rcs/lerobot_joint_converter.py @@ -120,7 +120,7 @@ def __init__( self.source_sql = self._build_source_sql(self.dataset_paths) self.video_encoding = video_encoding - self.tcp_offset = rcs.GRIPPER_OFFSETS[self.gripper_type] + self.tcp_offset = rcs.GRIPPER_TCP_OFFSETS[self.gripper_type] self.ik = rcs.common.Pin( rcs.ROBOTS[robot_type].mjcf_model_path, rcs.ROBOTS[robot_type].attachment_site,