diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 795ad7eb..d249e69f 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -24,7 +24,10 @@ def get_object_pose(self, geom_name) -> Pose: data = self.env.get_wrapper_attr("sim").data geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom_name) - return Pose(translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3)) + obj_pose_world_coordinates = Pose(translation=data.geom_xpos[geom_id], + rotation=data.geom_xmat[geom_id].reshape(3, 3)) + obj_pose_robot_coordinates = self.env.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) + return obj_pose_robot_coordinates def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: waypoints = [] @@ -36,11 +39,11 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in def step(self, action: np.ndarray) -> dict: re = self.env.step(action) s: FR3State = self.env.unwrapped.robot.get_state() - return re[0] + return re def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]: end_eff_pose = self.env.unwrapped.robot.get_cartesian_position() - + # @todo goal_pose is with respective to the robot base which is different in empty world scene and lab scene? goal_pose = self.get_object_pose(geom_name=geom_name) # goal pose is above the object and gripper coordinate must flip z-axis (end effector base rotation is [1, 0, 0, 0]) # be careful we define identity quaternion as as [0, 0, 0, 1] @@ -55,6 +58,14 @@ def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper. # calculate delta action delta_action = waypoints[i] * waypoints[i - 1].inverse() obs = self.step(self._action(delta_action, gripper)) + ik_success = obs[-1]['ik_success'] + if not obs[-1]['ik_success']: + trans_source, rot_source = waypoints[i - 1].translation(), waypoints[i - 1].rotation_rpy().as_vector() + trans_dest, rot_des = waypoints[i].translation(), waypoints[i].rotation_rpy().as_vector() + print(f"ik success: {ik_success} when attempting to move from trans: {trans_source}, rot: {rot_source}\n" + f" to trans: {trans_dest} rot: {rot_des}!") + print(f"aborting motion!") + exit(-1) return obs def approach(self, geom_name: str): @@ -83,7 +94,10 @@ def pickup(self, geom_name: str): def main(): - env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True) + """ + available envs: "rcs/SimplePickUpSim-v0", "rcs/FR3LabPickUpSimDigitHand-v0", "rcs/SimplePickUpSimDigitHand-v0 + """ + env = gym.make("rcs/SimplePickUpSimDigitHand-v0", render_mode="human", delta_actions=True) env.reset() controller = PickUpDemo(env) controller.pickup("yellow_box_geom") diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index 67dcd663..9feeecde 100644 --- a/python/rcsss/__init__.py +++ b/python/rcsss/__init__.py @@ -6,7 +6,7 @@ from gymnasium import register from rcsss import camera, control, envs, sim from rcsss._core import __version__, common, hw -from rcsss.envs.factories import FR3Real, FR3SimplePickUpSim +from rcsss.envs.factories import FR3Real, FR3SimplePickUpSim, FR3SimplePickUpSimDigitHand, FR3LabPickUpSimDigitHand # available mujoco scenes scenes = { @@ -21,6 +21,16 @@ id="rcs/SimplePickUpSim-v0", entry_point=FR3SimplePickUpSim(), ) + +register( + id="rcs/SimplePickUpSimDigitHand-v0", + entry_point=FR3SimplePickUpSimDigitHand(), +) +register( + id="rcs/FR3LabPickUpSimDigitHand-v0", + entry_point=FR3LabPickUpSimDigitHand(), +) + register( id="rcs/FR3Real-v0", entry_point=FR3Real(), diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 227a37b0..b5e1f4cf 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -29,9 +29,9 @@ FR3Sim, FR3SimplePickUpSimSuccessWrapper, RandomCubePos, - SimWrapper, + SimWrapper, RandomCubePosLab, ) - +import numpy as np logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -157,6 +157,12 @@ def default_fr3_sim_robot_cfg(): cfg.realtime = False return cfg +def digit_fr3_sim_robot_cfg(): + cfg = sim.FR3Config() + pose_offset = rcsss.common.Pose(translation=np.array([0, 0, 0.0466])) + cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) * pose_offset + cfg.realtime = False + return cfg def default_fr3_sim_gripper_cfg(): return sim.FHConfig() @@ -327,3 +333,92 @@ def __call__( # type: ignore env_rel.get_wrapper_attr("sim").open_gui() return env_rel + + +class FR3SimplePickUpSimDigitHand(EnvCreator): + def __call__( # type: ignore + self, + render_mode: str = "human", + control_mode: str = "xyzrpy", + resolution: tuple[int, int] | None = None, + frame_rate: int = 10, + delta_actions: bool = True, + ) -> gym.Env: + if resolution is None: + resolution = (256, 256) + + cameras = {"eye-in-hand_0": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed))} + + camera_cfg = SimCameraSetConfig( + cameras=cameras, + resolution_width=resolution[0], + resolution_height=resolution[1], + frame_rate=frame_rate, + physical_units=True, + ) + + env_rel = fr3_sim_env( + control_mode=( + ControlMode.CARTESIAN_TRPY + if control_mode == "xyzrpy" + else ControlMode.JOINTS if control_mode == "joints" else ControlMode.CARTESIAN_TQuart + ), + robot_cfg=digit_fr3_sim_robot_cfg(), + collision_guard=False, + gripper_cfg=default_fr3_sim_gripper_cfg(), + camera_set_cfg=camera_cfg, + max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, + relative_to=RelativeTo.LAST_STEP, + mjcf="fr3_simple_pick_up_digit_hand", + sim_wrapper=RandomCubePos, + ) + env_rel = FR3SimplePickUpSimSuccessWrapper(env_rel) + if render_mode == "human": + env_rel.get_wrapper_attr("sim").open_gui() + + return env_rel + + +class FR3LabPickUpSimDigitHand(EnvCreator): + def __call__( # type: ignore + self, + render_mode: str = "human", + control_mode: str = "xyzrpy", + resolution: tuple[int, int] | None = None, + frame_rate: int = 10, + delta_actions: bool = True, + ) -> gym.Env: + if resolution is None: + resolution = (256, 256) + + cameras = {"eye-in-hand_0": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)), + "eye-in-hand_1": SimCameraConfig(identifier="eye-in-hand_1", type=int(CameraType.fixed))} + + camera_cfg = SimCameraSetConfig( + cameras=cameras, + resolution_width=resolution[0], + resolution_height=resolution[1], + frame_rate=frame_rate, + physical_units=True, + ) + + env_rel = fr3_sim_env( + control_mode=( + ControlMode.CARTESIAN_TRPY + if control_mode == "xyzrpy" + else ControlMode.JOINTS if control_mode == "joints" else ControlMode.CARTESIAN_TQuart + ), + robot_cfg=digit_fr3_sim_robot_cfg(), + collision_guard=False, + gripper_cfg=default_fr3_sim_gripper_cfg(), + camera_set_cfg=camera_cfg, + max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, + relative_to=RelativeTo.LAST_STEP, + mjcf="lab_simple_pick_up_digit_hand", + sim_wrapper=RandomCubePosLab, + ) + env_rel = FR3SimplePickUpSimSuccessWrapper(env_rel) + if render_mode == "human": + env_rel.get_wrapper_attr("sim").open_gui() + + return env_rel \ No newline at end of file diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 31451a44..0001b761 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -188,6 +188,23 @@ def reset( return obs, info +class RandomCubePosLab(SimWrapper): + """Wrapper to randomly place cube in the FR3SimplePickUpSim environment.""" + + def reset( + self, seed: int | None = None, options: dict[str, Any] | None = None + ) -> tuple[dict[str, Any], dict[str, Any]]: + obs, info = super().reset(seed=seed, options=options) + + iso_cube = [0.0, 0.0, 0.826] + pos_x = iso_cube[0] + np.random.random() * 0.2 + 0.1 + pos_y = iso_cube[1] + np.random.random() * 0.2 + 0.1 + pos_z = 0.826 + self.sim.data.joint("yellow-box-joint").qpos[:3] = [pos_x, pos_y, pos_z] + + return obs, info + + class FR3SimplePickUpSimSuccessWrapper(gym.Wrapper): """Wrapper to check if the cube is successfully picked up in the FR3SimplePickUpSim environment."""