From a785b00fdac17605cf411c48aedb476f31bdea7f Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 12 Dec 2024 18:00:19 +0100 Subject: [PATCH 1/4] feat(gym-env): created a new env for simple pick up with digit hand --- python/examples/grasp_demo.py | 3 +- python/rcsss/__init__.py | 8 +++++- python/rcsss/envs/factories.py | 52 +++++++++++++++++++++++++++++++++- 3 files changed, 60 insertions(+), 3 deletions(-) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 795ad7eb..224274a1 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -83,7 +83,8 @@ def pickup(self, geom_name: str): def main(): - env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True) + # env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True) + 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..2b42242c 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 # available mujoco scenes scenes = { @@ -21,6 +21,12 @@ id="rcs/SimplePickUpSim-v0", entry_point=FR3SimplePickUpSim(), ) + +register( + id="rcs/SimplePickUpSimDigitHand-v0", + entry_point=FR3SimplePickUpSimDigitHand(), +) + register( id="rcs/FR3Real-v0", entry_point=FR3Real(), diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 227a37b0..62fd979b 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -31,7 +31,7 @@ RandomCubePos, SimWrapper, ) - +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,47 @@ 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 From 8fc299144b378ace1f4c324a7ad550c4b16cf57f Mon Sep 17 00:00:00 2001 From: suman1209 Date: Wed, 18 Dec 2024 17:55:28 +0100 Subject: [PATCH 2/4] feature(added lab scene for pickup):added lab pick up scene to the gym.make --- python/examples/grasp_demo.py | 6 ++--- python/rcsss/__init__.py | 6 ++++- python/rcsss/envs/factories.py | 47 +++++++++++++++++++++++++++++++++- python/rcsss/envs/sim.py | 17 ++++++++++++ 4 files changed, 71 insertions(+), 5 deletions(-) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 224274a1..089ca91c 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -40,7 +40,7 @@ def step(self, action: np.ndarray) -> dict: 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] @@ -83,8 +83,8 @@ def pickup(self, geom_name: str): def main(): - # env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True) - env = gym.make("rcs/SimplePickUpSimDigitHand-v0", render_mode="human", delta_actions=True) + # available envs: "rcs/SimplePickUpSim-v0", "rcs/FR3LabPickUpSimDigitHand-v0", "SimplePickUpSimDigitHand-v0" + env = gym.make("rcs/FR3LabPickUpSimDigitHand-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 2b42242c..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, FR3SimplePickUpSimDigitHand +from rcsss.envs.factories import FR3Real, FR3SimplePickUpSim, FR3SimplePickUpSimDigitHand, FR3LabPickUpSimDigitHand # available mujoco scenes scenes = { @@ -26,6 +26,10 @@ id="rcs/SimplePickUpSimDigitHand-v0", entry_point=FR3SimplePickUpSimDigitHand(), ) +register( + id="rcs/FR3LabPickUpSimDigitHand-v0", + entry_point=FR3LabPickUpSimDigitHand(), +) register( id="rcs/FR3Real-v0", diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 62fd979b..b5e1f4cf 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -29,7 +29,7 @@ FR3Sim, FR3SimplePickUpSimSuccessWrapper, RandomCubePos, - SimWrapper, + SimWrapper, RandomCubePosLab, ) import numpy as np logger = logging.getLogger(__name__) @@ -377,3 +377,48 @@ def __call__( # type: ignore 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.""" From b31699ea558c98eb49364d57903e09cf8abacf77 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 19 Dec 2024 14:56:57 +0100 Subject: [PATCH 3/4] fix(added lab scene for pickup):converting geom pose to robot_base frame and detecting an ik failure --- python/examples/grasp_demo.py | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 089ca91c..e6b8bed9 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,7 +39,7 @@ 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() @@ -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): @@ -84,7 +95,7 @@ def pickup(self, geom_name: str): def main(): # available envs: "rcs/SimplePickUpSim-v0", "rcs/FR3LabPickUpSimDigitHand-v0", "SimplePickUpSimDigitHand-v0" - env = gym.make("rcs/FR3LabPickUpSimDigitHand-v0", render_mode="human", delta_actions=True) + env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True) env.reset() controller = PickUpDemo(env) controller.pickup("yellow_box_geom") From ecb1ccfda7fbb620765804f45979258845b5847c Mon Sep 17 00:00:00 2001 From: suman1209 Date: Wed, 8 Jan 2025 13:19:33 +0100 Subject: [PATCH 4/4] refactor(grasp_demo): available env strings refactored --- python/examples/grasp_demo.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index e6b8bed9..d249e69f 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -94,8 +94,10 @@ def pickup(self, geom_name: str): def main(): - # available envs: "rcs/SimplePickUpSim-v0", "rcs/FR3LabPickUpSimDigitHand-v0", "SimplePickUpSimDigitHand-v0" - 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")