From f2bea3b1d9ab6d6cd8f3e87241213dc0829acf05 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Mon, 13 Jan 2025 18:26:13 +0100 Subject: [PATCH 01/24] feature(sim): added lab empty world and lab sim env with digit hand --- python/examples/grasp_demo.py | 32 ++++++++-- python/rcsss/__init__.py | 12 +++- python/rcsss/envs/factories.py | 105 ++++++++++++++++++++++++++++++++- python/rcsss/envs/sim.py | 70 +++++++++++++++++++++- 4 files changed, 210 insertions(+), 9 deletions(-) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index baef831f..3ae94a91 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -5,6 +5,7 @@ import mujoco import numpy as np from rcsss._core.common import Pose +from rcsss._core.sim import FR3State from rcsss.envs.base import FR3Env, GripperWrapper logger = logging.getLogger(__name__) @@ -25,7 +26,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 = [] @@ -34,10 +38,12 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in waypoints.append(start_pose.interpolate(end_pose, t)) return waypoints - def step(self, action: dict) -> dict: - return self.env.step(action)[0] + def step(self, action: np.ndarray) -> dict: + re = self.env.step(action) + s: FR3State = self.env.unwrapped.robot.get_state() + return re - def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]: + def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: end_eff_pose = self.unwrapped.robot.get_cartesian_position() goal_pose = self.get_object_pose(geom_name=geom_name) @@ -53,6 +59,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): @@ -81,8 +95,14 @@ def pickup(self, geom_name: str): def main(): - env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True) - env.reset() + """ + available envs: "rcs/SimplePickUpSim-v0", + "rcs/FR3LabPickUpSimDigitHand-v0", + "rcs/SimplePickUpSimDigitHand-v0" + """ + env = gym.make("rcs/FR3LabPickUpSimDigitHand-v0", render_mode="human", delta_actions=True, + robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465]) + obs = 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 b5c85388..49560dc5 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -1,3 +1,4 @@ +import json import logging from os import PathLike from typing import Type @@ -27,7 +28,7 @@ FR3Sim, FR3SimplePickUpSimSuccessWrapper, RandomCubePos, - SimWrapper, + SimWrapper, RandomCubePosLab, ) logger = logging.getLogger(__name__) @@ -155,6 +156,17 @@ def default_fr3_sim_robot_cfg(): cfg.realtime = False return cfg +def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): + if tcp_path is None: + raise ValueError("No tcp_path was provided.") + with open(tcp_path, "r") as f: + data = json.load(f) + tcp_offset = data["offset_translation"] + cfg = sim.FR3Config() + pose_offset = rcsss.common.Pose(translation=np.array(tcp_offset)) + 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() @@ -319,3 +331,94 @@ 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, + robot2_cam_pose: list[int] | None = None + ) -> 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, robot2_cam_pose=robot2_cam_pose) + sim = env_rel.get_wrapper_attr("sim") + if render_mode == "human": + 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 08a9e6a4..0b1f7780 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -1,4 +1,6 @@ +import json import logging +from os import PathLike from typing import Any, SupportsFloat, Type, cast import gymnasium as gym @@ -7,6 +9,39 @@ from rcsss import sim from rcsss.envs.base import ControlMode, FR3Env, GripperWrapper +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +""" + @todo there is a cyclic import issue when getting the below two functions from factories, currently I am duplicating this, but + need to find a separate place to put this function into +""" + +def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: + if urdf_path is None and "lab" in rcsss.scenes: + urdf_path = rcsss.scenes["lab"].parent / "fr3.urdf" + assert urdf_path.exists(), "Automatic deduced urdf path does not exist. Corrupted models directory." + logger.info("Using automatic found urdf.") + elif urdf_path is None and not allow_none_if_not_found: + msg = "This pip package was not built with the UTN lab models, please pass the urdf and mjcf path to use simulation or collision guard." + raise ValueError(msg) + elif urdf_path is None: + logger.warning("No urdf path was found. Proceeding, but set_cartesian methods will result in errors.") + return str(urdf_path) if urdf_path is not None else None + + +def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): + if tcp_path is None: + raise ValueError("No tcp_path was provided.") + with open(tcp_path, "r") as f: + data = json.load(f) + tcp_offset = data["offset_translation"] + cfg = sim.FR3Config() + pose_offset = rcsss.common.Pose(translation=np.array(tcp_offset)) + cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) * pose_offset + cfg.realtime = False + return cfg + class SimWrapper(gym.Wrapper): def __init__(self, env: gym.Env, simulation: sim.Sim): @@ -188,13 +223,31 @@ 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.""" EE_HOME = np.array([0.34169773, 0.00047028, 0.4309004]) - def __init__(self, env): + def __init__(self, env, robot2_cam_pose:list[int] | None = None): super().__init__(env) + self.robot2_cam_pose = robot2_cam_pose self.unwrapped: FR3Env assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." self.sim = env.get_wrapper_attr("sim") @@ -214,3 +267,18 @@ def step(self, action: dict[str, Any]): reward = -diff_cube_home - diff_ee_cube return obs, reward, success, truncated, info + + def reset(self): + out = super().reset() + if self.robot2_cam_pose is not None: + urdf_path = get_urdf_path(None, allow_none_if_not_found=False) + assert urdf_path is not None + # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup + ik2 = rcsss.common.IK(urdf_path) + robot2 = rcsss.sim.FR3(self.sim, "1", ik2) + cfg2 = digit_fr3_sim_robot_cfg() + cfg2.realtime = False + robot2.set_parameters(cfg2) + robot2.set_joint_position(np.array(self.robot2_cam_pose)) + self.sim.step_until_convergence() + return out From a58b1a0549a01ed3cd8c553807056f23775897e8 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Mon, 13 Jan 2025 18:41:57 +0100 Subject: [PATCH 02/24] refactor(grasp_demo): refactored to correct linting issues --- python/examples/grasp_demo.py | 23 +++++++++++------- python/rcsss/__init__.py | 7 +++++- python/rcsss/envs/factories.py | 44 +++++++++++++++++++--------------- python/rcsss/envs/sim.py | 6 +++-- 4 files changed, 50 insertions(+), 30 deletions(-) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 3ae94a91..b9ca95ee 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -26,8 +26,9 @@ 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) - obj_pose_world_coordinates = 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 @@ -59,12 +60,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']: + 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"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 @@ -100,8 +103,12 @@ def main(): "rcs/FR3LabPickUpSimDigitHand-v0", "rcs/SimplePickUpSimDigitHand-v0" """ - env = gym.make("rcs/FR3LabPickUpSimDigitHand-v0", render_mode="human", delta_actions=True, - robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465]) + env = gym.make( + "rcs/FR3LabPickUpSimDigitHand-v0", + render_mode="human", + delta_actions=True, + robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465], + ) obs = env.reset() controller = PickUpDemo(env) controller.pickup("yellow_box_geom") diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index 9feeecde..c9b22acc 100644 --- a/python/rcsss/__init__.py +++ b/python/rcsss/__init__.py @@ -6,7 +6,12 @@ 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, FR3LabPickUpSimDigitHand +from rcsss.envs.factories import ( + FR3LabPickUpSimDigitHand, + FR3Real, + FR3SimplePickUpSim, + FR3SimplePickUpSimDigitHand, +) # available mujoco scenes scenes = { diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 49560dc5..5676aad4 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -28,7 +28,8 @@ FR3Sim, FR3SimplePickUpSimSuccessWrapper, RandomCubePos, - SimWrapper, RandomCubePosLab, + RandomCubePosLab, + SimWrapper, ) logger = logging.getLogger(__name__) @@ -156,9 +157,11 @@ def default_fr3_sim_robot_cfg(): cfg.realtime = False return cfg -def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): + +def digit_fr3_sim_robot_cfg(tcp_path: str | None = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): if tcp_path is None: - raise ValueError("No tcp_path was provided.") + msg = "No tcp_path was provided." + raise Exception(msg) with open(tcp_path, "r") as f: data = json.load(f) tcp_offset = data["offset_translation"] @@ -168,6 +171,7 @@ def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up cfg.realtime = False return cfg + def default_fr3_sim_gripper_cfg(): return sim.FHConfig() @@ -335,12 +339,12 @@ def __call__( # type: ignore 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, + 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) @@ -379,19 +383,21 @@ def __call__( # type: ignore 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, - robot2_cam_pose: list[int] | None = None + self, + render_mode: str = "human", + control_mode: str = "xyzrpy", + resolution: tuple[int, int] | None = None, + frame_rate: int = 10, + delta_actions: bool = True, + robot2_cam_pose: list[int] | None = None, ) -> 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))} + 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, @@ -421,4 +427,4 @@ def __call__( # type: ignore if render_mode == "human": sim.open_gui() - return env_rel \ No newline at end of file + return env_rel diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 0b1f7780..a7bce517 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -17,6 +17,7 @@ need to find a separate place to put this function into """ + def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: if urdf_path is None and "lab" in rcsss.scenes: urdf_path = rcsss.scenes["lab"].parent / "fr3.urdf" @@ -32,7 +33,8 @@ def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: boo def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): if tcp_path is None: - raise ValueError("No tcp_path was provided.") + msg = "No tcp_path was provided." + raise Exception(msg) with open(tcp_path, "r") as f: data = json.load(f) tcp_offset = data["offset_translation"] @@ -245,7 +247,7 @@ class FR3SimplePickUpSimSuccessWrapper(gym.Wrapper): EE_HOME = np.array([0.34169773, 0.00047028, 0.4309004]) - def __init__(self, env, robot2_cam_pose:list[int] | None = None): + def __init__(self, env, robot2_cam_pose: list[int] | None = None): super().__init__(env) self.robot2_cam_pose = robot2_cam_pose self.unwrapped: FR3Env From be73b5d39e04b05638336fae5aacbe2c7f326ad2 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Mon, 13 Jan 2025 18:41:57 +0100 Subject: [PATCH 03/24] refactor(grasp_demo): refactored to correct linting issues --- python/examples/grasp_demo.py | 38 ++++++++++++++++------------- python/rcsss/__init__.py | 7 +++++- python/rcsss/envs/factories.py | 44 +++++++++++++++++++--------------- python/rcsss/envs/sim.py | 6 +++-- 4 files changed, 56 insertions(+), 39 deletions(-) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 3ae94a91..7903811b 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -1,11 +1,11 @@ import logging +import sys from typing import Any, cast import gymnasium as gym import mujoco import numpy as np from rcsss._core.common import Pose -from rcsss._core.sim import FR3State from rcsss.envs.base import FR3Env, GripperWrapper logger = logging.getLogger(__name__) @@ -26,10 +26,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) - 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 + obj_pose_world_coordinates = Pose( + translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3) + ) + return self.env.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: waypoints = [] @@ -39,9 +39,7 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in return waypoints def step(self, action: np.ndarray) -> dict: - re = self.env.step(action) - s: FR3State = self.env.unwrapped.robot.get_state() - return re + return self.env.step(action) def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: end_eff_pose = self.unwrapped.robot.get_cartesian_position() @@ -59,14 +57,16 @@ 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']: + 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) + 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("aborting motion!") + sys.exit(-1) return obs def approach(self, geom_name: str): @@ -100,9 +100,13 @@ def main(): "rcs/FR3LabPickUpSimDigitHand-v0", "rcs/SimplePickUpSimDigitHand-v0" """ - env = gym.make("rcs/FR3LabPickUpSimDigitHand-v0", render_mode="human", delta_actions=True, - robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465]) - obs = env.reset() + env = gym.make( + "rcs/FR3LabPickUpSimDigitHand-v0", + render_mode="human", + delta_actions=True, + robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465], + ) + env.reset() controller = PickUpDemo(env) controller.pickup("yellow_box_geom") diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index 9feeecde..c9b22acc 100644 --- a/python/rcsss/__init__.py +++ b/python/rcsss/__init__.py @@ -6,7 +6,12 @@ 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, FR3LabPickUpSimDigitHand +from rcsss.envs.factories import ( + FR3LabPickUpSimDigitHand, + FR3Real, + FR3SimplePickUpSim, + FR3SimplePickUpSimDigitHand, +) # available mujoco scenes scenes = { diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 49560dc5..5676aad4 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -28,7 +28,8 @@ FR3Sim, FR3SimplePickUpSimSuccessWrapper, RandomCubePos, - SimWrapper, RandomCubePosLab, + RandomCubePosLab, + SimWrapper, ) logger = logging.getLogger(__name__) @@ -156,9 +157,11 @@ def default_fr3_sim_robot_cfg(): cfg.realtime = False return cfg -def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): + +def digit_fr3_sim_robot_cfg(tcp_path: str | None = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): if tcp_path is None: - raise ValueError("No tcp_path was provided.") + msg = "No tcp_path was provided." + raise Exception(msg) with open(tcp_path, "r") as f: data = json.load(f) tcp_offset = data["offset_translation"] @@ -168,6 +171,7 @@ def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up cfg.realtime = False return cfg + def default_fr3_sim_gripper_cfg(): return sim.FHConfig() @@ -335,12 +339,12 @@ def __call__( # type: ignore 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, + 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) @@ -379,19 +383,21 @@ def __call__( # type: ignore 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, - robot2_cam_pose: list[int] | None = None + self, + render_mode: str = "human", + control_mode: str = "xyzrpy", + resolution: tuple[int, int] | None = None, + frame_rate: int = 10, + delta_actions: bool = True, + robot2_cam_pose: list[int] | None = None, ) -> 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))} + 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, @@ -421,4 +427,4 @@ def __call__( # type: ignore if render_mode == "human": sim.open_gui() - return env_rel \ No newline at end of file + return env_rel diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 0b1f7780..a7bce517 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -17,6 +17,7 @@ need to find a separate place to put this function into """ + def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: if urdf_path is None and "lab" in rcsss.scenes: urdf_path = rcsss.scenes["lab"].parent / "fr3.urdf" @@ -32,7 +33,8 @@ def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: boo def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): if tcp_path is None: - raise ValueError("No tcp_path was provided.") + msg = "No tcp_path was provided." + raise Exception(msg) with open(tcp_path, "r") as f: data = json.load(f) tcp_offset = data["offset_translation"] @@ -245,7 +247,7 @@ class FR3SimplePickUpSimSuccessWrapper(gym.Wrapper): EE_HOME = np.array([0.34169773, 0.00047028, 0.4309004]) - def __init__(self, env, robot2_cam_pose:list[int] | None = None): + def __init__(self, env, robot2_cam_pose: list[int] | None = None): super().__init__(env) self.robot2_cam_pose = robot2_cam_pose self.unwrapped: FR3Env From a28e192bc44f10c2f515c923201834cdcbd99f22 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Mon, 13 Jan 2025 18:26:13 +0100 Subject: [PATCH 04/24] feature(sim): added lab empty world and lab sim env with digit hand --- python/examples/grasp_demo.py | 32 ++++++++-- python/rcsss/__init__.py | 12 +++- python/rcsss/envs/factories.py | 105 ++++++++++++++++++++++++++++++++- python/rcsss/envs/sim.py | 70 +++++++++++++++++++++- 4 files changed, 210 insertions(+), 9 deletions(-) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index baef831f..3ae94a91 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -5,6 +5,7 @@ import mujoco import numpy as np from rcsss._core.common import Pose +from rcsss._core.sim import FR3State from rcsss.envs.base import FR3Env, GripperWrapper logger = logging.getLogger(__name__) @@ -25,7 +26,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 = [] @@ -34,10 +38,12 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in waypoints.append(start_pose.interpolate(end_pose, t)) return waypoints - def step(self, action: dict) -> dict: - return self.env.step(action)[0] + def step(self, action: np.ndarray) -> dict: + re = self.env.step(action) + s: FR3State = self.env.unwrapped.robot.get_state() + return re - def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]: + def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: end_eff_pose = self.unwrapped.robot.get_cartesian_position() goal_pose = self.get_object_pose(geom_name=geom_name) @@ -53,6 +59,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): @@ -81,8 +95,14 @@ def pickup(self, geom_name: str): def main(): - env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True) - env.reset() + """ + available envs: "rcs/SimplePickUpSim-v0", + "rcs/FR3LabPickUpSimDigitHand-v0", + "rcs/SimplePickUpSimDigitHand-v0" + """ + env = gym.make("rcs/FR3LabPickUpSimDigitHand-v0", render_mode="human", delta_actions=True, + robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465]) + obs = 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 b5c85388..49560dc5 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -1,3 +1,4 @@ +import json import logging from os import PathLike from typing import Type @@ -27,7 +28,7 @@ FR3Sim, FR3SimplePickUpSimSuccessWrapper, RandomCubePos, - SimWrapper, + SimWrapper, RandomCubePosLab, ) logger = logging.getLogger(__name__) @@ -155,6 +156,17 @@ def default_fr3_sim_robot_cfg(): cfg.realtime = False return cfg +def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): + if tcp_path is None: + raise ValueError("No tcp_path was provided.") + with open(tcp_path, "r") as f: + data = json.load(f) + tcp_offset = data["offset_translation"] + cfg = sim.FR3Config() + pose_offset = rcsss.common.Pose(translation=np.array(tcp_offset)) + 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() @@ -319,3 +331,94 @@ 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, + robot2_cam_pose: list[int] | None = None + ) -> 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, robot2_cam_pose=robot2_cam_pose) + sim = env_rel.get_wrapper_attr("sim") + if render_mode == "human": + 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 08a9e6a4..0b1f7780 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -1,4 +1,6 @@ +import json import logging +from os import PathLike from typing import Any, SupportsFloat, Type, cast import gymnasium as gym @@ -7,6 +9,39 @@ from rcsss import sim from rcsss.envs.base import ControlMode, FR3Env, GripperWrapper +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +""" + @todo there is a cyclic import issue when getting the below two functions from factories, currently I am duplicating this, but + need to find a separate place to put this function into +""" + +def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: + if urdf_path is None and "lab" in rcsss.scenes: + urdf_path = rcsss.scenes["lab"].parent / "fr3.urdf" + assert urdf_path.exists(), "Automatic deduced urdf path does not exist. Corrupted models directory." + logger.info("Using automatic found urdf.") + elif urdf_path is None and not allow_none_if_not_found: + msg = "This pip package was not built with the UTN lab models, please pass the urdf and mjcf path to use simulation or collision guard." + raise ValueError(msg) + elif urdf_path is None: + logger.warning("No urdf path was found. Proceeding, but set_cartesian methods will result in errors.") + return str(urdf_path) if urdf_path is not None else None + + +def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): + if tcp_path is None: + raise ValueError("No tcp_path was provided.") + with open(tcp_path, "r") as f: + data = json.load(f) + tcp_offset = data["offset_translation"] + cfg = sim.FR3Config() + pose_offset = rcsss.common.Pose(translation=np.array(tcp_offset)) + cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) * pose_offset + cfg.realtime = False + return cfg + class SimWrapper(gym.Wrapper): def __init__(self, env: gym.Env, simulation: sim.Sim): @@ -188,13 +223,31 @@ 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.""" EE_HOME = np.array([0.34169773, 0.00047028, 0.4309004]) - def __init__(self, env): + def __init__(self, env, robot2_cam_pose:list[int] | None = None): super().__init__(env) + self.robot2_cam_pose = robot2_cam_pose self.unwrapped: FR3Env assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." self.sim = env.get_wrapper_attr("sim") @@ -214,3 +267,18 @@ def step(self, action: dict[str, Any]): reward = -diff_cube_home - diff_ee_cube return obs, reward, success, truncated, info + + def reset(self): + out = super().reset() + if self.robot2_cam_pose is not None: + urdf_path = get_urdf_path(None, allow_none_if_not_found=False) + assert urdf_path is not None + # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup + ik2 = rcsss.common.IK(urdf_path) + robot2 = rcsss.sim.FR3(self.sim, "1", ik2) + cfg2 = digit_fr3_sim_robot_cfg() + cfg2.realtime = False + robot2.set_parameters(cfg2) + robot2.set_joint_position(np.array(self.robot2_cam_pose)) + self.sim.step_until_convergence() + return out From bf64b7375442a8cf2b3f9e8dfe46fb025fd87640 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Mon, 13 Jan 2025 18:41:57 +0100 Subject: [PATCH 05/24] refactor(grasp_demo): refactored to correct linting issues --- python/examples/grasp_demo.py | 38 ++++++++++++++++------------- python/rcsss/__init__.py | 7 +++++- python/rcsss/envs/factories.py | 44 +++++++++++++++++++--------------- python/rcsss/envs/sim.py | 6 +++-- 4 files changed, 56 insertions(+), 39 deletions(-) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 3ae94a91..7903811b 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -1,11 +1,11 @@ import logging +import sys from typing import Any, cast import gymnasium as gym import mujoco import numpy as np from rcsss._core.common import Pose -from rcsss._core.sim import FR3State from rcsss.envs.base import FR3Env, GripperWrapper logger = logging.getLogger(__name__) @@ -26,10 +26,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) - 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 + obj_pose_world_coordinates = Pose( + translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3) + ) + return self.env.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: waypoints = [] @@ -39,9 +39,7 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in return waypoints def step(self, action: np.ndarray) -> dict: - re = self.env.step(action) - s: FR3State = self.env.unwrapped.robot.get_state() - return re + return self.env.step(action) def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: end_eff_pose = self.unwrapped.robot.get_cartesian_position() @@ -59,14 +57,16 @@ 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']: + 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) + 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("aborting motion!") + sys.exit(-1) return obs def approach(self, geom_name: str): @@ -100,9 +100,13 @@ def main(): "rcs/FR3LabPickUpSimDigitHand-v0", "rcs/SimplePickUpSimDigitHand-v0" """ - env = gym.make("rcs/FR3LabPickUpSimDigitHand-v0", render_mode="human", delta_actions=True, - robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465]) - obs = env.reset() + env = gym.make( + "rcs/FR3LabPickUpSimDigitHand-v0", + render_mode="human", + delta_actions=True, + robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465], + ) + env.reset() controller = PickUpDemo(env) controller.pickup("yellow_box_geom") diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index 9feeecde..c9b22acc 100644 --- a/python/rcsss/__init__.py +++ b/python/rcsss/__init__.py @@ -6,7 +6,12 @@ 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, FR3LabPickUpSimDigitHand +from rcsss.envs.factories import ( + FR3LabPickUpSimDigitHand, + FR3Real, + FR3SimplePickUpSim, + FR3SimplePickUpSimDigitHand, +) # available mujoco scenes scenes = { diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 49560dc5..5676aad4 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -28,7 +28,8 @@ FR3Sim, FR3SimplePickUpSimSuccessWrapper, RandomCubePos, - SimWrapper, RandomCubePosLab, + RandomCubePosLab, + SimWrapper, ) logger = logging.getLogger(__name__) @@ -156,9 +157,11 @@ def default_fr3_sim_robot_cfg(): cfg.realtime = False return cfg -def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): + +def digit_fr3_sim_robot_cfg(tcp_path: str | None = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): if tcp_path is None: - raise ValueError("No tcp_path was provided.") + msg = "No tcp_path was provided." + raise Exception(msg) with open(tcp_path, "r") as f: data = json.load(f) tcp_offset = data["offset_translation"] @@ -168,6 +171,7 @@ def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up cfg.realtime = False return cfg + def default_fr3_sim_gripper_cfg(): return sim.FHConfig() @@ -335,12 +339,12 @@ def __call__( # type: ignore 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, + 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) @@ -379,19 +383,21 @@ def __call__( # type: ignore 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, - robot2_cam_pose: list[int] | None = None + self, + render_mode: str = "human", + control_mode: str = "xyzrpy", + resolution: tuple[int, int] | None = None, + frame_rate: int = 10, + delta_actions: bool = True, + robot2_cam_pose: list[int] | None = None, ) -> 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))} + 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, @@ -421,4 +427,4 @@ def __call__( # type: ignore if render_mode == "human": sim.open_gui() - return env_rel \ No newline at end of file + return env_rel diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 0b1f7780..a7bce517 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -17,6 +17,7 @@ need to find a separate place to put this function into """ + def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: if urdf_path is None and "lab" in rcsss.scenes: urdf_path = rcsss.scenes["lab"].parent / "fr3.urdf" @@ -32,7 +33,8 @@ def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: boo def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): if tcp_path is None: - raise ValueError("No tcp_path was provided.") + msg = "No tcp_path was provided." + raise Exception(msg) with open(tcp_path, "r") as f: data = json.load(f) tcp_offset = data["offset_translation"] @@ -245,7 +247,7 @@ class FR3SimplePickUpSimSuccessWrapper(gym.Wrapper): EE_HOME = np.array([0.34169773, 0.00047028, 0.4309004]) - def __init__(self, env, robot2_cam_pose:list[int] | None = None): + def __init__(self, env, robot2_cam_pose: list[int] | None = None): super().__init__(env) self.robot2_cam_pose = robot2_cam_pose self.unwrapped: FR3Env From 34021cde7f67d99e47ff4984b588f6031d1c3ced Mon Sep 17 00:00:00 2001 From: suman1209 Date: Mon, 20 Jan 2025 18:40:11 +0100 Subject: [PATCH 06/24] feat(sim): separated grasp_demo script from the grasp_demo_lab script --- CMakeLists.txt | 2 +- python/examples/grasp_demo.py | 34 ++------- python/examples/grasp_demo_lab.py | 113 ++++++++++++++++++++++++++++++ python/rcsss/envs/factories.py | 46 ++---------- python/rcsss/envs/sim.py | 36 ++-------- python/rcsss/envs/utils.py | 39 +++++++++++ 6 files changed, 171 insertions(+), 99 deletions(-) create mode 100644 python/examples/grasp_demo_lab.py create mode 100644 python/rcsss/envs/utils.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c8896ff..54c5e782 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,7 +98,7 @@ FetchContent_MakeAvailable(libfranka rl pybind11) if (${INCLUDE_UTN_MODELS}) if (DEFINED GITLAB_MODELS_TOKEN) include(download_scenes) - set(ref b8234983a5e35e222c955f9145ac4cd129827a8e) + set(ref 377dffded50e7815fa65e84ab4b0eb3198065881) FetchContent_Declare( scenes URL "https://gitos.rrze.fau.de/api/v4/projects/1100/repository/archive?path=scenes&sha=${ref}" diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 7903811b..baef831f 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -1,5 +1,4 @@ import logging -import sys from typing import Any, cast import gymnasium as gym @@ -26,10 +25,7 @@ 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) - obj_pose_world_coordinates = Pose( - translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3) - ) - return self.env.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) + return Pose(translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3)) def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: waypoints = [] @@ -38,10 +34,10 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in waypoints.append(start_pose.interpolate(end_pose, t)) return waypoints - def step(self, action: np.ndarray) -> dict: - return self.env.step(action) + def step(self, action: dict) -> dict: + return self.env.step(action)[0] - def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: + def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]: end_eff_pose = self.unwrapped.robot.get_cartesian_position() goal_pose = self.get_object_pose(geom_name=geom_name) @@ -57,16 +53,6 @@ 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("aborting motion!") - sys.exit(-1) return obs def approach(self, geom_name: str): @@ -95,17 +81,7 @@ def pickup(self, geom_name: str): def main(): - """ - available envs: "rcs/SimplePickUpSim-v0", - "rcs/FR3LabPickUpSimDigitHand-v0", - "rcs/SimplePickUpSimDigitHand-v0" - """ - env = gym.make( - "rcs/FR3LabPickUpSimDigitHand-v0", - render_mode="human", - delta_actions=True, - robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465], - ) + env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True) env.reset() controller = PickUpDemo(env) controller.pickup("yellow_box_geom") diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py new file mode 100644 index 00000000..1c9a021c --- /dev/null +++ b/python/examples/grasp_demo_lab.py @@ -0,0 +1,113 @@ +import logging +import sys +from typing import Any, cast + +import gymnasium as gym +import mujoco +import numpy as np +from rcsss._core.common import Pose +from rcsss.envs.base import FR3Env, GripperWrapper + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +class PickUpDemo: + def __init__(self, env: gym.Env): + self.env = env + self.unwrapped: FR3Env = cast(FR3Env, self.env.unwrapped) + self.home_pose = self.unwrapped.robot.get_cartesian_position() + + def _action(self, pose: Pose, gripper: float) -> dict[str, Any]: + return {"xyzrpy": pose.xyzrpy(), "gripper": gripper} + + def get_object_pose(self, geom_name) -> Pose: + model = self.env.get_wrapper_attr("sim").model + data = self.env.get_wrapper_attr("sim").data + + geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom_name) + obj_pose_world_coordinates = Pose( + translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3) + ) + return self.env.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) + + def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: + waypoints = [] + for i in range(num_waypoints + 1): + t = i / (num_waypoints + 1) + waypoints.append(start_pose.interpolate(end_pose, t)) + return waypoints + + def step(self, action: np.ndarray) -> dict: + return self.env.step(action) + + def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: + end_eff_pose = self.unwrapped.robot.get_cartesian_position() + + 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] + # this does not work if the object is flipped + goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) + + return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) + + def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: + for i in range(1, len(waypoints)): + # 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() + msg = (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}!") + logger.warning(msg) + return obs + + def approach(self, geom_name: str): + waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=5) + self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN) + + def grasp(self, geom_name: str): + + waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0, num_waypoints=15) + self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN) + + self.step(self._action(Pose(), GripperWrapper.BINARY_GRIPPER_CLOSED)) + + waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=20) + self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) + + def move_home(self): + end_eff_pose = self.unwrapped.robot.get_cartesian_position() + waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=10) + self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) + + def pickup(self, geom_name: str): + self.approach(geom_name) + self.grasp(geom_name) + self.move_home() + + +def main(): + # env = gym.make( + # "rcs/SimplePickUpSimDigitHand-v0", + # render_mode="human", + # delta_actions=True + # ) + + env = gym.make( + "rcs/FR3LabPickUpSimDigitHand-v0", + render_mode="human", + delta_actions=True, + robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465], + ) + env.reset() + controller = PickUpDemo(env) + controller.pickup("yellow_box_geom") + + +if __name__ == "__main__": + main() diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 5676aad4..172dd6fb 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -1,4 +1,3 @@ -import json import logging from os import PathLike from typing import Type @@ -31,6 +30,7 @@ RandomCubePosLab, SimWrapper, ) +from rcsss.envs.utils import get_urdf_path, set_tcp_offset, default_fr3_sim_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -68,19 +68,6 @@ def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | No return RealSenseCameraSet(cam_cfg) -def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: - if urdf_path is None and "lab" in rcsss.scenes: - urdf_path = rcsss.scenes["lab"].parent / "fr3.urdf" - assert urdf_path.exists(), "Automatic deduced urdf path does not exist. Corrupted models directory." - logger.info("Using automatic found urdf.") - elif urdf_path is None and not allow_none_if_not_found: - msg = "This pip package was not built with the UTN lab models, please pass the urdf and mjcf path to use simulation or collision guard." - raise ValueError(msg) - elif urdf_path is None: - logger.warning("No urdf path was found. Proceeding, but set_cartesian methods will result in errors.") - return str(urdf_path) if urdf_path is not None else None - - def fr3_hw_env( ip: str, control_mode: ControlMode, @@ -151,27 +138,6 @@ def fr3_hw_env( return env -def default_fr3_sim_robot_cfg(): - cfg = sim.FR3Config() - cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) - cfg.realtime = False - return cfg - - -def digit_fr3_sim_robot_cfg(tcp_path: str | None = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): - if tcp_path is None: - msg = "No tcp_path was provided." - raise Exception(msg) - with open(tcp_path, "r") as f: - data = json.load(f) - tcp_offset = data["offset_translation"] - cfg = sim.FR3Config() - pose_offset = rcsss.common.Pose(translation=np.array(tcp_offset)) - 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() @@ -227,8 +193,10 @@ def fr3_sim_env( assert urdf_path is not None if mjcf not in rcsss.scenes: logger.warning("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml") - - simulation = sim.Sim(rcsss.scenes.get(str(mjcf), mjcf)) + mjb_file = rcsss.scenes.get(str(mjcf), mjcf) + simulation = sim.Sim(mjb_file) + # setting the tcp offset + set_tcp_offset(robot_cfg, simulation) ik = rcsss.common.IK(urdf_path) robot = rcsss.sim.FR3(simulation, "0", ik) robot.set_parameters(robot_cfg) @@ -365,7 +333,7 @@ def __call__( # type: ignore if control_mode == "xyzrpy" else ControlMode.JOINTS if control_mode == "joints" else ControlMode.CARTESIAN_TQuart ), - robot_cfg=digit_fr3_sim_robot_cfg(), + robot_cfg=default_fr3_sim_robot_cfg(), collision_guard=False, gripper_cfg=default_fr3_sim_gripper_cfg(), camera_set_cfg=camera_cfg, @@ -413,7 +381,7 @@ def __call__( # type: ignore if control_mode == "xyzrpy" else ControlMode.JOINTS if control_mode == "joints" else ControlMode.CARTESIAN_TQuart ), - robot_cfg=digit_fr3_sim_robot_cfg(), + robot_cfg=default_fr3_sim_robot_cfg(), collision_guard=False, gripper_cfg=default_fr3_sim_gripper_cfg(), camera_set_cfg=camera_cfg, diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index a7bce517..0dd5ae22 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -1,6 +1,4 @@ -import json import logging -from os import PathLike from typing import Any, SupportsFloat, Type, cast import gymnasium as gym @@ -8,6 +6,7 @@ import rcsss from rcsss import sim from rcsss.envs.base import ControlMode, FR3Env, GripperWrapper +from rcsss.envs.utils import get_urdf_path, set_tcp_offset, default_fr3_sim_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -18,31 +17,7 @@ """ -def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: - if urdf_path is None and "lab" in rcsss.scenes: - urdf_path = rcsss.scenes["lab"].parent / "fr3.urdf" - assert urdf_path.exists(), "Automatic deduced urdf path does not exist. Corrupted models directory." - logger.info("Using automatic found urdf.") - elif urdf_path is None and not allow_none_if_not_found: - msg = "This pip package was not built with the UTN lab models, please pass the urdf and mjcf path to use simulation or collision guard." - raise ValueError(msg) - elif urdf_path is None: - logger.warning("No urdf path was found. Proceeding, but set_cartesian methods will result in errors.") - return str(urdf_path) if urdf_path is not None else None - - -def digit_fr3_sim_robot_cfg(tcp_path: str = "../models/scenes/fr3_simple_pick_up_digit_hand/tcp_offset.json"): - if tcp_path is None: - msg = "No tcp_path was provided." - raise Exception(msg) - with open(tcp_path, "r") as f: - data = json.load(f) - tcp_offset = data["offset_translation"] - cfg = sim.FR3Config() - pose_offset = rcsss.common.Pose(translation=np.array(tcp_offset)) - cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) * pose_offset - cfg.realtime = False - return cfg + class SimWrapper(gym.Wrapper): @@ -278,9 +253,10 @@ def reset(self): # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup ik2 = rcsss.common.IK(urdf_path) robot2 = rcsss.sim.FR3(self.sim, "1", ik2) - cfg2 = digit_fr3_sim_robot_cfg() - cfg2.realtime = False - robot2.set_parameters(cfg2) + # setting the tcp offset + robot_cfg = default_fr3_sim_robot_cfg() + set_tcp_offset(robot_cfg, self.sim) + robot2.set_parameters(robot_cfg) robot2.set_joint_position(np.array(self.robot2_cam_pose)) self.sim.step_until_convergence() return out diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py new file mode 100644 index 00000000..e6195a09 --- /dev/null +++ b/python/rcsss/envs/utils.py @@ -0,0 +1,39 @@ +from os import PathLike +import logging +import rcsss +from rcsss import sim +import numpy as np +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +def default_fr3_sim_robot_cfg(): + cfg = sim.FR3Config() + cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) + cfg.realtime = False + return cfg + + +def set_tcp_offset(cfg: sim.FR3Config, sim: sim.Sim): + tcp_offset = np.array([0, 0, 0]) + try: + tcp_offset = list(sim.model.numeric("tcp_offset").data) + except KeyError: + print("Using the default tcp offset") + if tcp_offset is not None: + pose_offset = rcsss.common.Pose(translation=tcp_offset) + cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) * pose_offset + return cfg + + +def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: + if urdf_path is None and "lab" in rcsss.scenes: + urdf_path = rcsss.scenes["lab"].parent / "fr3.urdf" + assert urdf_path.exists(), "Automatic deduced urdf path does not exist. Corrupted models directory." + logger.info("Using automatic found urdf.") + elif urdf_path is None and not allow_none_if_not_found: + msg = "This pip package was not built with the UTN lab models, please pass the urdf and mjcf path to use simulation or collision guard." + raise ValueError(msg) + elif urdf_path is None: + logger.warning("No urdf path was found. Proceeding, but set_cartesian methods will result in errors.") + return str(urdf_path) if urdf_path is not None else None \ No newline at end of file From 3dfb4a5f64ac1f76b2ddd19418ae8d6392a197a1 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Fri, 24 Jan 2025 17:14:35 +0100 Subject: [PATCH 07/24] feat(grasp): pickup script for grasp added --- python/examples/grasp_demo.py | 37 +++++--------------- python/examples/grasp_demo_lab.py | 25 ++++++++------ python/rcsss/envs/factories.py | 4 +-- python/rcsss/envs/sim.py | 56 ++++++++++++++++++++++++------- 4 files changed, 68 insertions(+), 54 deletions(-) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 7903811b..8319893f 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -1,5 +1,4 @@ import logging -import sys from typing import Any, cast import gymnasium as gym @@ -26,22 +25,20 @@ 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) - obj_pose_world_coordinates = Pose( - translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3) - ) - return self.env.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) + return Pose(translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3)) def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: waypoints = [] for i in range(num_waypoints + 1): t = i / (num_waypoints + 1) waypoints.append(start_pose.interpolate(end_pose, t)) + waypoints.append(end_pose) return waypoints - def step(self, action: np.ndarray) -> dict: - return self.env.step(action) + def step(self, action: dict) -> dict: + return self.env.step(action)[0] - def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: + def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]: end_eff_pose = self.unwrapped.robot.get_cartesian_position() goal_pose = self.get_object_pose(geom_name=geom_name) @@ -57,16 +54,6 @@ 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("aborting motion!") - sys.exit(-1) return obs def approach(self, geom_name: str): @@ -95,18 +82,10 @@ def pickup(self, geom_name: str): def main(): - """ - available envs: "rcs/SimplePickUpSim-v0", - "rcs/FR3LabPickUpSimDigitHand-v0", - "rcs/SimplePickUpSimDigitHand-v0" - """ - env = gym.make( - "rcs/FR3LabPickUpSimDigitHand-v0", - render_mode="human", - delta_actions=True, - robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465], - ) + env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True) env.reset() + print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore + # assert False controller = PickUpDemo(env) controller.pickup("yellow_box_geom") diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py index 1c9a021c..284b33d9 100644 --- a/python/examples/grasp_demo_lab.py +++ b/python/examples/grasp_demo_lab.py @@ -6,6 +6,7 @@ import mujoco import numpy as np from rcsss._core.common import Pose +import rcsss.envs.base from rcsss.envs.base import FR3Env, GripperWrapper logger = logging.getLogger(__name__) @@ -43,20 +44,21 @@ def step(self, action: np.ndarray) -> dict: def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: end_eff_pose = self.unwrapped.robot.get_cartesian_position() - 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] - # this does not work if the object is flipped goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) - return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: for i in range(1, len(waypoints)): # calculate delta action - delta_action = waypoints[i] * waypoints[i - 1].inverse() - obs = self.step(self._action(delta_action, gripper)) + # delta_action = waypoints[i] * waypoints[i - 1].inverse() + pose = rcsss.common.Pose(translation=waypoints[i].translation(), + rpy_vector=np.array([-3.14159265e+00, 1.57009246e-16, 0])) + + # act = self._action(delta_action, gripper) + act = self._action(pose, gripper) + + obs = self.step(act) 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() @@ -64,20 +66,21 @@ def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper. msg = (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}!") logger.warning(msg) + assert False, msg return obs def approach(self, geom_name: str): - waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=5) + waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=50) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN) def grasp(self, geom_name: str): - waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0, num_waypoints=15) + waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0, num_waypoints=50) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN) self.step(self._action(Pose(), GripperWrapper.BINARY_GRIPPER_CLOSED)) - waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=20) + waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=50) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) def move_home(self): @@ -101,7 +104,7 @@ def main(): env = gym.make( "rcs/FR3LabPickUpSimDigitHand-v0", render_mode="human", - delta_actions=True, + delta_actions=False, robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465], ) env.reset() diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 172dd6fb..cde653c5 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -28,7 +28,7 @@ FR3SimplePickUpSimSuccessWrapper, RandomCubePos, RandomCubePosLab, - SimWrapper, + SimWrapper, FR3LabPickUpSimSuccessWrapper, ) from rcsss.envs.utils import get_urdf_path, set_tcp_offset, default_fr3_sim_robot_cfg @@ -390,7 +390,7 @@ def __call__( # type: ignore mjcf="lab_simple_pick_up_digit_hand", sim_wrapper=RandomCubePosLab, ) - env_rel = FR3SimplePickUpSimSuccessWrapper(env_rel, robot2_cam_pose=robot2_cam_pose) + env_rel = FR3LabPickUpSimSuccessWrapper(env_rel, robot2_cam_pose=robot2_cam_pose) sim = env_rel.get_wrapper_attr("sim") if render_mode == "human": sim.open_gui() diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 0dd5ae22..7dd5ad17 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -245,18 +245,50 @@ def step(self, action: dict[str, Any]): return obs, reward, success, truncated, info + +class FR3LabPickUpSimSuccessWrapper(gym.Wrapper): + """ + Wrapper to check if the cube is successfully picked up in the LabPickupSim environments. + This is also used to handle the reset of the second robot. + """ + + EE_HOME = np.array([0.34169773, 0.00047028, 0.4309004]) + + def __init__(self, env, robot2_cam_pose: list[int] | None = None): + super().__init__(env) + self.robot2_cam_pose = robot2_cam_pose + self.unwrapped: FR3Env + assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." + self.sim = env.get_wrapper_attr("sim") + + def step(self, action: dict[str, Any]): + obs, reward, done, truncated, info = super().step(action) + + success = ( + self.sim.data.joint("yellow-box-joint").qpos[2] > 0.3 + and obs["gripper"] == GripperWrapper.BINARY_GRIPPER_CLOSED + ) + diff_ee_cube = np.linalg.norm( + self.sim.data.joint("yellow-box-joint").qpos[:3] + - self.unwrapped.robot.get_cartesian_position().translation() + ) + diff_cube_home = np.linalg.norm(self.sim.data.joint("yellow-box-joint").qpos[:3] - self.EE_HOME) + reward = -diff_cube_home - diff_ee_cube + + return obs, reward, success, truncated, info + def reset(self): out = super().reset() - if self.robot2_cam_pose is not None: - urdf_path = get_urdf_path(None, allow_none_if_not_found=False) - assert urdf_path is not None - # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup - ik2 = rcsss.common.IK(urdf_path) - robot2 = rcsss.sim.FR3(self.sim, "1", ik2) - # setting the tcp offset - robot_cfg = default_fr3_sim_robot_cfg() - set_tcp_offset(robot_cfg, self.sim) - robot2.set_parameters(robot_cfg) - robot2.set_joint_position(np.array(self.robot2_cam_pose)) - self.sim.step_until_convergence() + # if self.robot2_cam_pose is not None: + # urdf_path = get_urdf_path(None, allow_none_if_not_found=False) + # assert urdf_path is not None + # # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup + # ik2 = rcsss.common.IK(urdf_path) + # robot2 = rcsss.sim.FR3(self.sim, "1", ik2) + # # setting the tcp offset + # robot_cfg = default_fr3_sim_robot_cfg() + # set_tcp_offset(robot_cfg, self.sim) + # robot2.set_parameters(robot_cfg) + # robot2.set_joint_position(np.array(self.robot2_cam_pose)) + # self.sim.step_until_convergence() return out From 7efb32923f5c68aa68942e0eeab037dcd1ac8e49 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Wed, 29 Jan 2025 15:25:37 +0100 Subject: [PATCH 08/24] feat(grasp_demo_lab): unified cube pose randomizer and separated the pose setter for the second robot --- python/examples/grasp_demo_lab.py | 2 +- python/rcsss/envs/factories.py | 8 +-- python/rcsss/envs/sim.py | 99 +++++++++++++++---------------- 3 files changed, 53 insertions(+), 56 deletions(-) diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py index 284b33d9..971cb693 100644 --- a/python/examples/grasp_demo_lab.py +++ b/python/examples/grasp_demo_lab.py @@ -85,7 +85,7 @@ def grasp(self, geom_name: str): def move_home(self): end_eff_pose = self.unwrapped.robot.get_cartesian_position() - waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=10) + waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=50) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) def pickup(self, geom_name: str): diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index cde653c5..923d5e57 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -27,8 +27,7 @@ FR3Sim, FR3SimplePickUpSimSuccessWrapper, RandomCubePos, - RandomCubePosLab, - SimWrapper, FR3LabPickUpSimSuccessWrapper, + SimWrapper, FR3LabPickUpSimSuccessWrapper, Robot2Wrapper ) from rcsss.envs.utils import get_urdf_path, set_tcp_offset, default_fr3_sim_robot_cfg @@ -388,9 +387,10 @@ def __call__( # type: ignore 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, + sim_wrapper=RandomCubePos, ) - env_rel = FR3LabPickUpSimSuccessWrapper(env_rel, robot2_cam_pose=robot2_cam_pose) + env_rel = FR3LabPickUpSimSuccessWrapper(env_rel) + env_rel = Robot2Wrapper(env_rel, robot2_cam_pose) sim = env_rel.get_wrapper_attr("sim") if render_mode == "human": sim.open_gui() diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 7dd5ad17..350725de 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -11,14 +11,6 @@ logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) -""" - @todo there is a cyclic import issue when getting the below two functions from factories, currently I am duplicating this, but - need to find a separate place to put this function into -""" - - - - class SimWrapper(gym.Wrapper): def __init__(self, env: gym.Env, simulation: sim.Sim): @@ -187,34 +179,57 @@ class RandomCubePos(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]]: + 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.498, 0.0, 0.226] - 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.03 - self.sim.data.joint("yellow-box-joint").qpos[:3] = [pos_x, pos_y, pos_z] + scene_type = self.get_scene_type() + if scene_type == "empty_world": + pos_z = 0.03 + elif scene_type == "lab": + # iso_cube = [0.498, 0.0, 0.226] + pos_z = 0.826 + pos_x = iso_cube[0] + 0.2 #np.random.random() * 2 - 0.1 + pos_y = iso_cube[1] + 0.2 #np.random.random() * 2 - 0.1 - return obs, info - - -class RandomCubePosLab(SimWrapper): - """Wrapper to randomly place cube in the FR3SimplePickUpSim environment.""" + self.sim.data.joint("yellow-box-joint").qpos[:3] = [0, 0, 0] - 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) + return obs, info - 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] + def get_scene_type(self): + # @todo this gives me [0, 0, 0] + # robot_base_z = self.unwrapped.robot.get_base_pose_in_world_coordinates().translation()[2] + # if robot_base_z >= 0.8: + # scene_type = "lab" + # elif 0 < robot_base_z < 0.8: + # scene_type = "empty_world" + # else: + # raise Exception(f"Invalid robot base z {robot_base_z}!") + scene_type = "lab" + return scene_type + + +class Robot2Wrapper(gym.Wrapper): + def __init__(self, env, robot2_pose: list[int] | None = None): + super().__init__(env) + self.robot2_pose = robot2_pose + self.unwrapped: FR3Env + assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." + self.sim = env.get_wrapper_attr("sim") - return obs, info + def reset(self): + out = super().reset() + if self.robot2_pose is not None: + urdf_path = get_urdf_path(None, allow_none_if_not_found=False) + assert urdf_path is not None + # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup + ik2 = rcsss.common.IK(urdf_path) + robot2 = rcsss.sim.FR3(self.sim, "1", ik2) + # setting the tcp offset + robot_cfg = default_fr3_sim_robot_cfg() + set_tcp_offset(robot_cfg, self.sim) + robot2.set_parameters(robot_cfg) + robot2.set_joint_position(np.array(self.robot2_pose)) + self.sim.step_until_convergence() class FR3SimplePickUpSimSuccessWrapper(gym.Wrapper): @@ -222,9 +237,8 @@ class FR3SimplePickUpSimSuccessWrapper(gym.Wrapper): EE_HOME = np.array([0.34169773, 0.00047028, 0.4309004]) - def __init__(self, env, robot2_cam_pose: list[int] | None = None): + def __init__(self, env): super().__init__(env) - self.robot2_cam_pose = robot2_cam_pose self.unwrapped: FR3Env assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." self.sim = env.get_wrapper_attr("sim") @@ -254,9 +268,8 @@ class FR3LabPickUpSimSuccessWrapper(gym.Wrapper): EE_HOME = np.array([0.34169773, 0.00047028, 0.4309004]) - def __init__(self, env, robot2_cam_pose: list[int] | None = None): + def __init__(self, env): super().__init__(env) - self.robot2_cam_pose = robot2_cam_pose self.unwrapped: FR3Env assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." self.sim = env.get_wrapper_attr("sim") @@ -275,20 +288,4 @@ def step(self, action: dict[str, Any]): diff_cube_home = np.linalg.norm(self.sim.data.joint("yellow-box-joint").qpos[:3] - self.EE_HOME) reward = -diff_cube_home - diff_ee_cube - return obs, reward, success, truncated, info - - def reset(self): - out = super().reset() - # if self.robot2_cam_pose is not None: - # urdf_path = get_urdf_path(None, allow_none_if_not_found=False) - # assert urdf_path is not None - # # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup - # ik2 = rcsss.common.IK(urdf_path) - # robot2 = rcsss.sim.FR3(self.sim, "1", ik2) - # # setting the tcp offset - # robot_cfg = default_fr3_sim_robot_cfg() - # set_tcp_offset(robot_cfg, self.sim) - # robot2.set_parameters(robot_cfg) - # robot2.set_joint_position(np.array(self.robot2_cam_pose)) - # self.sim.step_until_convergence() - return out + return obs, reward, success, truncated, info \ No newline at end of file From 4a181c62fff6e571b91eee7aa48387fe7ed8e238 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Wed, 29 Jan 2025 19:14:37 +0100 Subject: [PATCH 09/24] fix(sim): random cube placement in lab scene fixed --- python/rcsss/envs/factories.py | 11 +++-- python/rcsss/envs/sim.py | 82 +++++++++++++++++++--------------- 2 files changed, 53 insertions(+), 40 deletions(-) diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 923d5e57..e6b93f4c 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -27,7 +27,7 @@ FR3Sim, FR3SimplePickUpSimSuccessWrapper, RandomCubePos, - SimWrapper, FR3LabPickUpSimSuccessWrapper, Robot2Wrapper + SimWrapper, FR3LabPickUpSimSuccessWrapper, CamRobotWrapper ) from rcsss.envs.utils import get_urdf_path, set_tcp_offset, default_fr3_sim_robot_cfg @@ -194,6 +194,11 @@ def fr3_sim_env( logger.warning("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml") mjb_file = rcsss.scenes.get(str(mjcf), mjcf) simulation = sim.Sim(mjb_file) + """ + todo, without a simulation step, sim->d is not populated with the correct values in the robot. + check robot base without this to confirm. + """ + simulation.step(1) # setting the tcp offset set_tcp_offset(robot_cfg, simulation) ik = rcsss.common.IK(urdf_path) @@ -387,10 +392,10 @@ def __call__( # type: ignore 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=RandomCubePos, + sim_wrapper=RandomCubePos ) env_rel = FR3LabPickUpSimSuccessWrapper(env_rel) - env_rel = Robot2Wrapper(env_rel, robot2_cam_pose) + env_rel = CamRobotWrapper(env_rel, robot2_cam_pose) sim = env_rel.get_wrapper_attr("sim") if render_mode == "human": sim.open_gui() diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 350725de..20478aa7 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -20,6 +20,34 @@ def __init__(self, env: gym.Env, simulation: sim.Sim): self.sim = simulation +class CamRobotWrapper(gym.Wrapper): + def __init__(self, env, cam_robot_pose: list[int] | None = None): + super().__init__(env) + self.cam_robot_pose = cam_robot_pose + self.unwrapped: FR3Env + assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." + self.sim = env.get_wrapper_attr("sim") + # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup + urdf_path = get_urdf_path(None, allow_none_if_not_found=False) + assert urdf_path is not None + ik2 = rcsss.common.IK(urdf_path) + self.robot2 = rcsss.sim.FR3(self.sim, "1", ik2) + # setting the tcp offset + robot_cfg = default_fr3_sim_robot_cfg() + set_tcp_offset(robot_cfg, self.sim) + self.robot2.set_parameters(robot_cfg) + self.robot2.reset() + + def reset(self, seed: int | None = None, options: dict[str, Any] | None = None): + # self.robot2.set_joint_position(np.array(self.cam_robot_pose)) + obs, info = super().reset(seed=seed, options=options) + + if self.cam_robot_pose is not None: + self.robot2.set_joint_position(np.array(self.cam_robot_pose)) + self.sim.step_until_convergence() + return obs, info + + class FR3Sim(gym.Wrapper): def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | None = None): if sim_wrapper is not None: @@ -181,57 +209,37 @@ class RandomCubePos(SimWrapper): 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.498, 0.0, 0.226] + self.sim.step(1) + scene_type = self.get_scene_type() + iso_cube = [0.498, 0.0, 0.226] if scene_type == "empty_world": pos_z = 0.03 elif scene_type == "lab": - # iso_cube = [0.498, 0.0, 0.226] + iso_cube_pose = rcsss.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) + iso_cube = self.unwrapped.robot.to_pose_in_world_coordinates(iso_cube_pose).translation() pos_z = 0.826 - pos_x = iso_cube[0] + 0.2 #np.random.random() * 2 - 0.1 - pos_y = iso_cube[1] + 0.2 #np.random.random() * 2 - 0.1 - self.sim.data.joint("yellow-box-joint").qpos[:3] = [0, 0, 0] + pos_x = iso_cube[0] + np.random.random() * 0.2 - 0.1 + pos_y = iso_cube[1] + np.random.random() * 0.2 - 0.1 + + + self.sim.data.joint("yellow-box-joint").qpos[:3] = [pos_x, pos_y, pos_z] return obs, info def get_scene_type(self): # @todo this gives me [0, 0, 0] - # robot_base_z = self.unwrapped.robot.get_base_pose_in_world_coordinates().translation()[2] - # if robot_base_z >= 0.8: - # scene_type = "lab" - # elif 0 < robot_base_z < 0.8: - # scene_type = "empty_world" - # else: - # raise Exception(f"Invalid robot base z {robot_base_z}!") - scene_type = "lab" + robot_base_z = self.unwrapped.robot.get_base_pose_in_world_coordinates().translation()[2] + if robot_base_z >= 0.8: + scene_type = "lab" + elif 0 < robot_base_z < 0.8: + scene_type = "empty_world" + else: + raise Exception(f"Invalid robot base z {robot_base_z}!") return scene_type -class Robot2Wrapper(gym.Wrapper): - def __init__(self, env, robot2_pose: list[int] | None = None): - super().__init__(env) - self.robot2_pose = robot2_pose - self.unwrapped: FR3Env - assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." - self.sim = env.get_wrapper_attr("sim") - - def reset(self): - out = super().reset() - if self.robot2_pose is not None: - urdf_path = get_urdf_path(None, allow_none_if_not_found=False) - assert urdf_path is not None - # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup - ik2 = rcsss.common.IK(urdf_path) - robot2 = rcsss.sim.FR3(self.sim, "1", ik2) - # setting the tcp offset - robot_cfg = default_fr3_sim_robot_cfg() - set_tcp_offset(robot_cfg, self.sim) - robot2.set_parameters(robot_cfg) - robot2.set_joint_position(np.array(self.robot2_pose)) - self.sim.step_until_convergence() - - class FR3SimplePickUpSimSuccessWrapper(gym.Wrapper): """Wrapper to check if the cube is successfully picked up in the FR3SimplePickUpSim environment.""" From 0f1f266e92d027bb3cda1f095d6def4bd5015b0b Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 30 Jan 2025 16:01:44 +0100 Subject: [PATCH 10/24] feat(grasp_lab_demo): added second robot cam pose setter --- python/rcsss/envs/factories.py | 10 ++--- python/rcsss/envs/sim.py | 67 ++++++++++++++++++++++++++-------- 2 files changed, 56 insertions(+), 21 deletions(-) diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index e6b93f4c..c666c405 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -1,4 +1,5 @@ import logging +from functools import partial from os import PathLike from typing import Type @@ -164,7 +165,7 @@ def fr3_sim_env( relative_to: RelativeTo = RelativeTo.LAST_STEP, urdf_path: str | PathLike | None = None, mjcf: str | PathLike = "fr3_empty_world", - sim_wrapper: Type[SimWrapper] | None = None, + sim_wrapper: list[Type[SimWrapper]] | None = None, ) -> gym.Env: """ Creates a simulation environment for the FR3 robot. @@ -196,7 +197,7 @@ def fr3_sim_env( simulation = sim.Sim(mjb_file) """ todo, without a simulation step, sim->d is not populated with the correct values in the robot. - check robot base without this to confirm. + check robot base world coordinates without this line to confirm. """ simulation.step(1) # setting the tcp offset @@ -391,11 +392,10 @@ def __call__( # type: ignore 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=RandomCubePos + mjcf="lab", + sim_wrapper=[RandomCubePos, partial(CamRobotWrapper, cam_robot_pose=robot2_cam_pose)], ) env_rel = FR3LabPickUpSimSuccessWrapper(env_rel) - env_rel = CamRobotWrapper(env_rel, robot2_cam_pose) sim = env_rel.get_wrapper_attr("sim") if render_mode == "human": sim.open_gui() diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 20478aa7..ab5c1415 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -20,13 +20,12 @@ def __init__(self, env: gym.Env, simulation: sim.Sim): self.sim = simulation -class CamRobotWrapper(gym.Wrapper): - def __init__(self, env, cam_robot_pose: list[int] | None = None): - super().__init__(env) +class CamRobotWrapper(SimWrapper): + def __init__(self, env: gym.Env, simulation: sim.Sim, cam_robot_pose: list[int] | None = None): + super().__init__(env, simulation) self.cam_robot_pose = cam_robot_pose self.unwrapped: FR3Env assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." - self.sim = env.get_wrapper_attr("sim") # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup urdf_path = get_urdf_path(None, allow_none_if_not_found=False) assert urdf_path is not None @@ -34,24 +33,57 @@ def __init__(self, env, cam_robot_pose: list[int] | None = None): self.robot2 = rcsss.sim.FR3(self.sim, "1", ik2) # setting the tcp offset robot_cfg = default_fr3_sim_robot_cfg() - set_tcp_offset(robot_cfg, self.sim) + # set_tcp_offset(cfg, self.sim) self.robot2.set_parameters(robot_cfg) - self.robot2.reset() - - def reset(self, seed: int | None = None, options: dict[str, Any] | None = None): - # self.robot2.set_joint_position(np.array(self.cam_robot_pose)) - obs, info = super().reset(seed=seed, options=options) - if self.cam_robot_pose is not None: - self.robot2.set_joint_position(np.array(self.cam_robot_pose)) - self.sim.step_until_convergence() - return obs, info + # ik = rcsss.common.IK(urdf_path) + # robot = rcsss.sim.FR3(simulation, "0", ik) + # cfg = sim.FR3Config() + # cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) + # cfg.realtime = False + # robot.set_parameters(cfg) + + # self.robot2.reset() + # self.set_pose() + + def set_pose(self): + # to_state = np.array(self.cam_robot_pose) + # from_state = np.array(self.robot2.get_joint_position()) + # angle_step = 0.01 + # max_dist = np.max(np.abs(to_state - from_state)) + # num_steps = math.ceil(max_dist / angle_step) + # timesteps = np.array([0, 1]) + # if num_steps > 1: + # timesteps = np.linspace(0, 1, num_steps) + # traj = [to_state * t + from_state * (1 - t) for t in timesteps] + # for joint_pos in traj: + # self.robot2.set_joint_position(np.array(joint_pos)) + # self.sim.step_until_convergence() + self.robot2.set_joint_position(np.array(self.cam_robot_pose)) + self.sim.step_until_convergence() + print(f"{self.robot2.get_joint_position() = }") + + # def reset(self, seed: int | None = None, options: dict[str, Any] | None = None): + # # self.robot2.set_joint_position(np.array(self.cam_robot_pose)) + # obs, info = super().reset(seed=seed, options=options) + # + # if self.cam_robot_pose is not None: + # self.robot2.set_joint_position(np.array(self.cam_robot_pose)) + # self.sim.step_until_convergence() + # print(f"joint angles: {self.robot2.get_joint_position()}") + # print(f"IK success: {self.robot2.get_state().ik_success}") # type: ignore + # print(f"sim converged: {self.sim.is_converged()}") + # print(f"collision: {self.robot2.get_state().collision}") + # print(f"{self.robot2.get_joint_position() = }") + # return obs, info class FR3Sim(gym.Wrapper): - def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | None = None): + def __init__(self, env, simulation: sim.Sim, sim_wrapper: list[Type[SimWrapper]] | None = None): + self.sim_wrapper = sim_wrapper if sim_wrapper is not None: - env = sim_wrapper(env, simulation) + for wrapper_i in sim_wrapper: + env = wrapper_i(env, simulation) super().__init__(env) self.unwrapped: FR3Env assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." @@ -74,6 +106,9 @@ def reset( ) -> tuple[dict[str, Any], dict[str, Any]]: self.sim.reset() _, info = super().reset(seed=seed, options=options) + # reset robot 2 here + self.sim_wrapper[1](self.env, self.sim).set_pose() + # self.unwrapped.robot.move_home() self.sim.step(1) obs = cast(dict, self.unwrapped.get_obs()) return obs, info From f2d292544521a0035e643cf3a25096267f4f0273 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 20 Mar 2025 12:06:47 +0100 Subject: [PATCH 11/24] fix(grasp_demo_lab): removed side camera feature and refactored --- python/examples/grasp_demo_lab.py | 22 +++--- python/rcsss/envs/factories.py | 12 ++-- python/rcsss/envs/sim.py | 112 +++++++----------------------- python/rcsss/envs/utils.py | 8 ++- 4 files changed, 45 insertions(+), 109 deletions(-) diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py index 971cb693..3d3b144b 100644 --- a/python/examples/grasp_demo_lab.py +++ b/python/examples/grasp_demo_lab.py @@ -1,12 +1,11 @@ import logging -import sys from typing import Any, cast import gymnasium as gym import mujoco import numpy as np -from rcsss._core.common import Pose import rcsss.envs.base +from rcsss._core.common import Pose from rcsss.envs.base import FR3Env, GripperWrapper logger = logging.getLogger(__name__) @@ -52,8 +51,9 @@ def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper. for i in range(1, len(waypoints)): # calculate delta action # delta_action = waypoints[i] * waypoints[i - 1].inverse() - pose = rcsss.common.Pose(translation=waypoints[i].translation(), - rpy_vector=np.array([-3.14159265e+00, 1.57009246e-16, 0])) + pose = rcsss.common.Pose( + translation=waypoints[i].translation(), rpy_vector=np.array([-3.14159265e00, 1.57009246e-16, 0]) + ) # act = self._action(delta_action, gripper) act = self._action(pose, gripper) @@ -63,10 +63,11 @@ def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper. 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() - msg = (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}!") + msg = ( + 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}!" + ) logger.warning(msg) - assert False, msg return obs def approach(self, geom_name: str): @@ -95,17 +96,10 @@ def pickup(self, geom_name: str): def main(): - # env = gym.make( - # "rcs/SimplePickUpSimDigitHand-v0", - # render_mode="human", - # delta_actions=True - # ) - env = gym.make( "rcs/FR3LabPickUpSimDigitHand-v0", render_mode="human", delta_actions=False, - robot2_cam_pose=[0.1243549, -1.4711298, 1.2246249, -1.9944441, 0.0872650, 1.3396115, 2.1275465], ) env.reset() controller = PickUpDemo(env) diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index c666c405..c514a90e 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -1,5 +1,4 @@ import logging -from functools import partial from os import PathLike from typing import Type @@ -25,12 +24,14 @@ from rcsss.envs.hw import FR3HW from rcsss.envs.sim import ( CollisionGuard, + FR3LabPickUpSimSuccessWrapper, FR3Sim, FR3SimplePickUpSimSuccessWrapper, RandomCubePos, - SimWrapper, FR3LabPickUpSimSuccessWrapper, CamRobotWrapper + RandomCubePosLab, + SimWrapper, ) -from rcsss.envs.utils import get_urdf_path, set_tcp_offset, default_fr3_sim_robot_cfg +from rcsss.envs.utils import default_fr3_sim_robot_cfg, get_urdf_path, set_tcp_offset logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -362,7 +363,6 @@ def __call__( # type: ignore resolution: tuple[int, int] | None = None, frame_rate: int = 10, delta_actions: bool = True, - robot2_cam_pose: list[int] | None = None, ) -> gym.Env: if resolution is None: resolution = (256, 256) @@ -392,8 +392,8 @@ def __call__( # type: ignore 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", - sim_wrapper=[RandomCubePos, partial(CamRobotWrapper, cam_robot_pose=robot2_cam_pose)], + mjcf="lab_simple_pick_up_digit_hand", + sim_wrapper=RandomCubePosLab, ) env_rel = FR3LabPickUpSimSuccessWrapper(env_rel) sim = env_rel.get_wrapper_attr("sim") diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index ab5c1415..d373a6fc 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -6,7 +6,6 @@ import rcsss from rcsss import sim from rcsss.envs.base import ControlMode, FR3Env, GripperWrapper -from rcsss.envs.utils import get_urdf_path, set_tcp_offset, default_fr3_sim_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -20,70 +19,11 @@ def __init__(self, env: gym.Env, simulation: sim.Sim): self.sim = simulation -class CamRobotWrapper(SimWrapper): - def __init__(self, env: gym.Env, simulation: sim.Sim, cam_robot_pose: list[int] | None = None): - super().__init__(env, simulation) - self.cam_robot_pose = cam_robot_pose - self.unwrapped: FR3Env - assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." - # In this experiment, we use the second robot as a camera stand for the main robot performing the Pickup - urdf_path = get_urdf_path(None, allow_none_if_not_found=False) - assert urdf_path is not None - ik2 = rcsss.common.IK(urdf_path) - self.robot2 = rcsss.sim.FR3(self.sim, "1", ik2) - # setting the tcp offset - robot_cfg = default_fr3_sim_robot_cfg() - # set_tcp_offset(cfg, self.sim) - self.robot2.set_parameters(robot_cfg) - - # ik = rcsss.common.IK(urdf_path) - # robot = rcsss.sim.FR3(simulation, "0", ik) - # cfg = sim.FR3Config() - # cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) - # cfg.realtime = False - # robot.set_parameters(cfg) - - # self.robot2.reset() - # self.set_pose() - - def set_pose(self): - # to_state = np.array(self.cam_robot_pose) - # from_state = np.array(self.robot2.get_joint_position()) - # angle_step = 0.01 - # max_dist = np.max(np.abs(to_state - from_state)) - # num_steps = math.ceil(max_dist / angle_step) - # timesteps = np.array([0, 1]) - # if num_steps > 1: - # timesteps = np.linspace(0, 1, num_steps) - # traj = [to_state * t + from_state * (1 - t) for t in timesteps] - # for joint_pos in traj: - # self.robot2.set_joint_position(np.array(joint_pos)) - # self.sim.step_until_convergence() - self.robot2.set_joint_position(np.array(self.cam_robot_pose)) - self.sim.step_until_convergence() - print(f"{self.robot2.get_joint_position() = }") - - # def reset(self, seed: int | None = None, options: dict[str, Any] | None = None): - # # self.robot2.set_joint_position(np.array(self.cam_robot_pose)) - # obs, info = super().reset(seed=seed, options=options) - # - # if self.cam_robot_pose is not None: - # self.robot2.set_joint_position(np.array(self.cam_robot_pose)) - # self.sim.step_until_convergence() - # print(f"joint angles: {self.robot2.get_joint_position()}") - # print(f"IK success: {self.robot2.get_state().ik_success}") # type: ignore - # print(f"sim converged: {self.sim.is_converged()}") - # print(f"collision: {self.robot2.get_state().collision}") - # print(f"{self.robot2.get_joint_position() = }") - # return obs, info - - class FR3Sim(gym.Wrapper): def __init__(self, env, simulation: sim.Sim, sim_wrapper: list[Type[SimWrapper]] | None = None): self.sim_wrapper = sim_wrapper if sim_wrapper is not None: - for wrapper_i in sim_wrapper: - env = wrapper_i(env, simulation) + env = sim_wrapper(env, simulation) super().__init__(env) self.unwrapped: FR3Env assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." @@ -106,8 +46,6 @@ def reset( ) -> tuple[dict[str, Any], dict[str, Any]]: self.sim.reset() _, info = super().reset(seed=seed, options=options) - # reset robot 2 here - self.sim_wrapper[1](self.env, self.sim).set_pose() # self.unwrapped.robot.move_home() self.sim.step(1) obs = cast(dict, self.unwrapped.get_obs()) @@ -238,41 +176,43 @@ def env_from_xml_paths( ) -class RandomCubePos(SimWrapper): - """Wrapper to randomly place cube in the FR3SimplePickUpSim environment.""" +class RandomCubePosLab(SimWrapper): + """Wrapper to randomly place cube in the lab environments.""" def reset( - self, seed: int | None = None, options: dict[str, Any] | None = None) -> tuple[dict[str, Any], dict[str, Any]]: + 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) self.sim.step(1) - scene_type = self.get_scene_type() iso_cube = [0.498, 0.0, 0.226] - if scene_type == "empty_world": - pos_z = 0.03 - elif scene_type == "lab": - iso_cube_pose = rcsss.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) - iso_cube = self.unwrapped.robot.to_pose_in_world_coordinates(iso_cube_pose).translation() - pos_z = 0.826 - + iso_cube_pose = rcsss.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) + iso_cube = self.unwrapped.robot.to_pose_in_world_coordinates(iso_cube_pose).translation() + pos_z = 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 - self.sim.data.joint("yellow-box-joint").qpos[:3] = [pos_x, pos_y, pos_z] return obs, info - def get_scene_type(self): - # @todo this gives me [0, 0, 0] - robot_base_z = self.unwrapped.robot.get_base_pose_in_world_coordinates().translation()[2] - if robot_base_z >= 0.8: - scene_type = "lab" - elif 0 < robot_base_z < 0.8: - scene_type = "empty_world" - else: - raise Exception(f"Invalid robot base z {robot_base_z}!") - return scene_type + +class RandomCubePos(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) + self.sim.step(1) + iso_cube = [0.498, 0.0, 0.226] + pos_z = 0.03 + pos_x = iso_cube[0] + np.random.random() * 0.2 - 0.1 + pos_y = iso_cube[1] + np.random.random() * 0.2 - 0.1 + + self.sim.data.joint("yellow-box-joint").qpos[:3] = [pos_x, pos_y, pos_z] + + return obs, info class FR3SimplePickUpSimSuccessWrapper(gym.Wrapper): @@ -331,4 +271,4 @@ def step(self, action: dict[str, Any]): diff_cube_home = np.linalg.norm(self.sim.data.joint("yellow-box-joint").qpos[:3] - self.EE_HOME) reward = -diff_cube_home - diff_ee_cube - return obs, reward, success, truncated, info \ No newline at end of file + return obs, reward, success, truncated, info diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index e6195a09..c1d5b69a 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -1,8 +1,10 @@ -from os import PathLike import logging +from os import PathLike + +import numpy as np import rcsss from rcsss import sim -import numpy as np + logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -36,4 +38,4 @@ def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: boo raise ValueError(msg) elif urdf_path is None: logger.warning("No urdf path was found. Proceeding, but set_cartesian methods will result in errors.") - return str(urdf_path) if urdf_path is not None else None \ No newline at end of file + return str(urdf_path) if urdf_path is not None else None From 35fe651709ced0d9c765ec923878819d894d38be Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 20 Mar 2025 12:37:05 +0100 Subject: [PATCH 12/24] refactor(grasp_demo_lab): resolved pylint issues --- python/examples/grasp_demo_lab.py | 10 +++++----- python/rcsss/envs/factories.py | 2 +- python/rcsss/envs/sim.py | 4 ++-- python/rcsss/envs/utils.py | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py index 3d3b144b..da775b12 100644 --- a/python/examples/grasp_demo_lab.py +++ b/python/examples/grasp_demo_lab.py @@ -29,7 +29,7 @@ def get_object_pose(self, geom_name) -> Pose: obj_pose_world_coordinates = Pose( translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3) ) - return self.env.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) + return self.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: waypoints = [] @@ -38,8 +38,8 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in waypoints.append(start_pose.interpolate(end_pose, t)) return waypoints - def step(self, action: np.ndarray) -> dict: - return self.env.step(action) + def step(self, action: dict) -> dict: + return self.env.step(action)[-1] def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: end_eff_pose = self.unwrapped.robot.get_cartesian_position() @@ -59,8 +59,8 @@ def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper. act = self._action(pose, gripper) obs = self.step(act) - ik_success = obs[-1]["ik_success"] - if not obs[-1]["ik_success"]: + ik_success = obs["ik_success"] + if not obs["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() msg = ( diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index c514a90e..6c410d62 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -166,7 +166,7 @@ def fr3_sim_env( relative_to: RelativeTo = RelativeTo.LAST_STEP, urdf_path: str | PathLike | None = None, mjcf: str | PathLike = "fr3_empty_world", - sim_wrapper: list[Type[SimWrapper]] | None = None, + sim_wrapper: Type[SimWrapper] | None = None, ) -> gym.Env: """ Creates a simulation environment for the FR3 robot. diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index d373a6fc..45053f0e 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -20,7 +20,7 @@ def __init__(self, env: gym.Env, simulation: sim.Sim): class FR3Sim(gym.Wrapper): - def __init__(self, env, simulation: sim.Sim, sim_wrapper: list[Type[SimWrapper]] | None = None): + def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | None = None): self.sim_wrapper = sim_wrapper if sim_wrapper is not None: env = sim_wrapper(env, simulation) @@ -185,7 +185,7 @@ def reset( obs, info = super().reset(seed=seed, options=options) self.sim.step(1) - iso_cube = [0.498, 0.0, 0.226] + iso_cube = np.array([0.498, 0.0, 0.226]) iso_cube_pose = rcsss.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) iso_cube = self.unwrapped.robot.to_pose_in_world_coordinates(iso_cube_pose).translation() pos_z = 0.826 diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index c1d5b69a..c2c0e720 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -19,7 +19,7 @@ def default_fr3_sim_robot_cfg(): def set_tcp_offset(cfg: sim.FR3Config, sim: sim.Sim): tcp_offset = np.array([0, 0, 0]) try: - tcp_offset = list(sim.model.numeric("tcp_offset").data) + tcp_offset = np.array(sim.model.numeric("tcp_offset").data) except KeyError: print("Using the default tcp offset") if tcp_offset is not None: From 1021a272e9a9c84f0befc61b4dfcd467e7d0f234 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 24 Mar 2025 08:35:55 +0100 Subject: [PATCH 13/24] chore: fix naming consistency between envs --- python/examples/grasp_demo_lab.py | 2 +- python/rcsss/__init__.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py index da775b12..5ded5ddd 100644 --- a/python/examples/grasp_demo_lab.py +++ b/python/examples/grasp_demo_lab.py @@ -97,7 +97,7 @@ def pickup(self, geom_name: str): def main(): env = gym.make( - "rcs/FR3LabPickUpSimDigitHand-v0", + "rcs/LabPickUpSimDigitHand-v0", render_mode="human", delta_actions=False, ) diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index c9b22acc..63d4cf6f 100644 --- a/python/rcsss/__init__.py +++ b/python/rcsss/__init__.py @@ -32,7 +32,7 @@ entry_point=FR3SimplePickUpSimDigitHand(), ) register( - id="rcs/FR3LabPickUpSimDigitHand-v0", + id="rcs/LabPickUpSimDigitHand-v0", entry_point=FR3LabPickUpSimDigitHand(), ) From a7d21ec77e0c74d2d32e13b8b4494ddd2677ed06 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 24 Mar 2025 08:46:53 +0100 Subject: [PATCH 14/24] refactor(envs): move default configs to utils --- python/examples/env_cartesian_control.py | 5 +- python/examples/env_joint_control.py | 5 +- python/rcsss/control/keyboard_control.py | 7 +-- python/rcsss/control/vive.py | 5 +- python/rcsss/envs/factories.py | 61 ++++-------------------- python/rcsss/envs/utils.py | 60 ++++++++++++++++++++--- python/tests/test_sim_envs.py | 4 +- 7 files changed, 71 insertions(+), 76 deletions(-) diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index ff0ef334..d8e9c5f7 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -3,14 +3,13 @@ from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager from rcsss.control.utils import load_creds_fr3_desk from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance -from rcsss.envs.factories import ( +from rcsss.envs.factories import fr3_hw_env, fr3_sim_env +from rcsss.envs.utils import ( default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, - fr3_hw_env, - fr3_sim_env, ) logger = logging.getLogger(__name__) diff --git a/python/examples/env_joint_control.py b/python/examples/env_joint_control.py index 1bb48538..4c69e3e2 100644 --- a/python/examples/env_joint_control.py +++ b/python/examples/env_joint_control.py @@ -4,14 +4,13 @@ from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager from rcsss.control.utils import load_creds_fr3_desk from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance -from rcsss.envs.factories import ( +from rcsss.envs.factories import fr3_hw_env, fr3_sim_env +from rcsss.envs.utils import ( default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, - fr3_hw_env, - fr3_sim_env, ) logger = logging.getLogger(__name__) diff --git a/python/rcsss/control/keyboard_control.py b/python/rcsss/control/keyboard_control.py index 2c8e11fc..11d8d049 100644 --- a/python/rcsss/control/keyboard_control.py +++ b/python/rcsss/control/keyboard_control.py @@ -7,11 +7,8 @@ from rcsss.control.fr3_desk import FCI, Desk from rcsss.control.utils import load_creds_fr3_desk from rcsss.envs.base import ControlMode, RelativeTo -from rcsss.envs.factories import ( - default_fr3_hw_gripper_cfg, - default_fr3_hw_robot_cfg, - fr3_hw_env, -) +from rcsss.envs.factories import fr3_hw_env +from rcsss.envs.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/python/rcsss/control/vive.py b/python/rcsss/control/vive.py index 549b17a7..dbd3cc8e 100644 --- a/python/rcsss/control/vive.py +++ b/python/rcsss/control/vive.py @@ -15,14 +15,13 @@ RelativeTo, RobotInstance, ) -from rcsss.envs.factories import ( +from rcsss.envs.factories import fr3_hw_env, fr3_sim_env +from rcsss.envs.utils import ( default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, - fr3_hw_env, - fr3_sim_env, ) # import matplotlib.pyplot as plt diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 6c410d62..d0609038 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -7,11 +7,8 @@ import rcsss from gymnasium.envs.registration import EnvCreator from rcsss import sim -from rcsss._core.hw import FR3Config, IKController from rcsss._core.sim import CameraType from rcsss.camera.hw import BaseHardwareCameraSet -from rcsss.camera.interface import BaseCameraConfig -from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig from rcsss.envs.base import ( CameraSetWrapper, @@ -31,44 +28,19 @@ RandomCubePosLab, SimWrapper, ) -from rcsss.envs.utils import default_fr3_sim_robot_cfg, get_urdf_path, set_tcp_offset +from rcsss.envs.utils import ( + default_fr3_hw_gripper_cfg, + default_fr3_hw_robot_cfg, + default_fr3_sim_gripper_cfg, + default_fr3_sim_robot_cfg, + default_realsense, + get_urdf_path, +) logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) -def default_fr3_hw_robot_cfg(): - robot_cfg = FR3Config() - robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) - robot_cfg.speed_factor = 0.1 - robot_cfg.controller = IKController.robotics_library - return robot_cfg - - -def default_fr3_hw_gripper_cfg(): - gripper_cfg = rcsss.hw.FHConfig() - gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 - gripper_cfg.speed = 0.1 - gripper_cfg.force = 30 - return gripper_cfg - - -def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: - if name2id is None: - return None - cameras = {name: BaseCameraConfig(identifier=id) for name, id in name2id.items()} - cam_cfg = RealSenseSetConfig( - cameras=cameras, - resolution_width=1280, - resolution_height=720, - frame_rate=15, - enable_imu=False, # does not work with imu, why? - enable_ir=True, - enable_ir_emitter=False, - ) - return RealSenseCameraSet(cam_cfg) - - def fr3_hw_env( ip: str, control_mode: ControlMode, @@ -139,23 +111,6 @@ def fr3_hw_env( return env -def default_fr3_sim_gripper_cfg(): - return sim.FHConfig() - - -def default_mujoco_cameraset_cfg(): - - cameras = { - "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)), - "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free)), - # "bird_eye": SimCameraConfig(identifier="bird-eye-cam", type=int(CameraType.fixed)), - } - # 256x256 needed for VLAs - return SimCameraSetConfig( - cameras=cameras, resolution_width=256, resolution_height=256, frame_rate=10, physical_units=True - ) - - def fr3_sim_env( control_mode: ControlMode, robot_cfg: rcsss.sim.FR3Config, diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index c2c0e720..aed626c2 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -1,9 +1,15 @@ import logging from os import PathLike +import mujoco as mj import numpy as np import rcsss from rcsss import sim +from rcsss._core.hw import FR3Config, IKController +from rcsss._core.sim import CameraType, SimCameraSetConfig +from rcsss.camera.interface import BaseCameraConfig +from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig +from rcsss.camera.sim import SimCameraConfig logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -16,13 +22,53 @@ def default_fr3_sim_robot_cfg(): return cfg -def set_tcp_offset(cfg: sim.FR3Config, sim: sim.Sim): - tcp_offset = np.array([0, 0, 0]) - try: - tcp_offset = np.array(sim.model.numeric("tcp_offset").data) - except KeyError: - print("Using the default tcp offset") - if tcp_offset is not None: +def default_fr3_hw_robot_cfg(): + robot_cfg = FR3Config() + robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) + robot_cfg.speed_factor = 0.1 + robot_cfg.controller = IKController.robotics_library + return robot_cfg + + +def default_fr3_hw_gripper_cfg(): + gripper_cfg = rcsss.hw.FHConfig() + gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 + gripper_cfg.speed = 0.1 + gripper_cfg.force = 30 + return gripper_cfg + + +def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: + if name2id is None: + return None + cameras = {name: BaseCameraConfig(identifier=id) for name, id in name2id.items()} + cam_cfg = RealSenseSetConfig( + cameras=cameras, + resolution_width=1280, + resolution_height=720, + frame_rate=15, + enable_imu=False, # does not work with imu, why? + enable_ir=True, + enable_ir_emitter=False, + ) + return RealSenseCameraSet(cam_cfg) + + +def default_fr3_sim_gripper_cfg(): + return sim.FHConfig() + + +def default_mujoco_cameraset_cfg(): + + cameras = { + "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)), + "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free)), + # "bird_eye": SimCameraConfig(identifier="bird-eye-cam", type=int(CameraType.fixed)), + } + # 256x256 needed for VLAs + return SimCameraSetConfig( + cameras=cameras, resolution_width=256, resolution_height=256, frame_rate=10, physical_units=True + ) pose_offset = rcsss.common.Pose(translation=tcp_offset) cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) * pose_offset return cfg diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 11fedca2..7d919135 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -11,11 +11,11 @@ TQuartDictType, TRPYDictType, ) -from rcsss.envs.factories import ( +from rcsss.envs.factories import fr3_sim_env +from rcsss.envs.utils import ( default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, - fr3_sim_env, ) From 14875ed3effebb65b34388483e7ad2c817d4e86c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 24 Mar 2025 08:52:47 +0100 Subject: [PATCH 15/24] refactor(envs): tcp extraction before env creation --- python/rcsss/envs/factories.py | 9 ++++----- python/rcsss/envs/utils.py | 32 +++++++++++++++++++++++++++----- 2 files changed, 31 insertions(+), 10 deletions(-) diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index d0609038..7884fd70 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -156,8 +156,7 @@ def fr3_sim_env( check robot base world coordinates without this line to confirm. """ simulation.step(1) - # setting the tcp offset - set_tcp_offset(robot_cfg, simulation) + ik = rcsss.common.IK(urdf_path) robot = rcsss.sim.FR3(simulation, "0", ik) robot.set_parameters(robot_cfg) @@ -278,7 +277,7 @@ def __call__( # type: ignore if resolution is None: resolution = (256, 256) - cameras = {"eye-in-hand_0": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed))} + cameras = {"wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed))} camera_cfg = SimCameraSetConfig( cameras=cameras, @@ -294,7 +293,7 @@ def __call__( # type: ignore if control_mode == "xyzrpy" else ControlMode.JOINTS if control_mode == "joints" else ControlMode.CARTESIAN_TQuart ), - robot_cfg=default_fr3_sim_robot_cfg(), + robot_cfg=default_fr3_sim_robot_cfg("fr3_simple_pick_up_digit_hand"), collision_guard=False, gripper_cfg=default_fr3_sim_gripper_cfg(), camera_set_cfg=camera_cfg, @@ -341,7 +340,7 @@ def __call__( # type: ignore if control_mode == "xyzrpy" else ControlMode.JOINTS if control_mode == "joints" else ControlMode.CARTESIAN_TQuart ), - robot_cfg=default_fr3_sim_robot_cfg(), + robot_cfg=default_fr3_sim_robot_cfg("lab_simple_pick_up_digit_hand"), collision_guard=False, gripper_cfg=default_fr3_sim_gripper_cfg(), camera_set_cfg=camera_cfg, diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index aed626c2..6ff7787f 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -15,9 +15,9 @@ logger.setLevel(logging.INFO) -def default_fr3_sim_robot_cfg(): +def default_fr3_sim_robot_cfg(mjcf: str | PathLike = "fr3_empty_world") -> sim.FR3Config: cfg = sim.FR3Config() - cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) + cfg.tcp_offset = get_tcp_offset(mjcf) cfg.realtime = False return cfg @@ -59,7 +59,6 @@ def default_fr3_sim_gripper_cfg(): def default_mujoco_cameraset_cfg(): - cameras = { "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)), "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free)), @@ -69,9 +68,32 @@ def default_mujoco_cameraset_cfg(): return SimCameraSetConfig( cameras=cameras, resolution_width=256, resolution_height=256, frame_rate=10, physical_units=True ) + + +def get_tcp_offset(mjcf: str | PathLike): + """Reads out tcp offset set in mjcf file. + + Convention: The tcp offset is stored in the model as a numeric attribute named "tcp_offset". + + Args: + mjcf (str | PathLike): Path to the mjcf file. + + Returns: + rcsss.common.Pose: The tcp offset. + """ + mjmdl = rcsss.scenes.get(str(mjcf), mjcf) + if mjmdl.suffix == ".xml": + model = mj.MjModel.from_xml_path(str(mjmdl)) + elif mjmdl.suffix == ".mjb": + model = mj.MjModel.from_binary_path(str(mjmdl)) + try: + tcp_offset = np.array(model.numeric("tcp_offset").data) pose_offset = rcsss.common.Pose(translation=tcp_offset) - cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) * pose_offset - return cfg + return rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) * pose_offset + except KeyError: + msg = "No tcp offset found in the model. Using the default tcp offset." + logging.warning(msg) + return rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: From 76cdd0627d57cd78bafb966c40ea68608c457f07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 24 Mar 2025 11:00:48 +0100 Subject: [PATCH 16/24] fix(control): joint reset also sets controllers --- src/sim/FR3.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/sim/FR3.cpp b/src/sim/FR3.cpp index 14081036..2bdf25fe 100644 --- a/src/sim/FR3.cpp +++ b/src/sim/FR3.cpp @@ -207,6 +207,7 @@ void FR3::set_joints_hard(const common::Vector7d& q) { size_t jnt_id = this->ids.joints[i]; size_t jnt_qposadr = this->sim->m->jnt_qposadr[jnt_id]; this->sim->d->qpos[jnt_qposadr] = q[i]; + this->sim->d->ctrl[this->ids.actuators[i]] = q[i]; } } From feaa1cacc7745dacf030b99cde24abf512009afa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 24 Mar 2025 11:06:18 +0100 Subject: [PATCH 17/24] feat(env): camera robot - added new model version with curtain and fixed cube rotation - fixed delta action problem in lab pick up - added cam robot wrapper which sets the joints position of the camera robot --- CMakeLists.txt | 2 +- python/examples/grasp_demo.py | 2 +- python/examples/grasp_demo_lab.py | 27 ++++++----------------- python/rcsss/envs/factories.py | 36 +++++++++++++++++++++++-------- 4 files changed, 35 insertions(+), 32 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 54c5e782..c6f6e994 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,7 +98,7 @@ FetchContent_MakeAvailable(libfranka rl pybind11) if (${INCLUDE_UTN_MODELS}) if (DEFINED GITLAB_MODELS_TOKEN) include(download_scenes) - set(ref 377dffded50e7815fa65e84ab4b0eb3198065881) + set(ref 48c88f05aa34730acbe80bf4b50e7d5a789b927a) FetchContent_Declare( scenes URL "https://gitos.rrze.fau.de/api/v4/projects/1100/repository/archive?path=scenes&sha=${ref}" diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 8319893f..ee2b1925 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -82,7 +82,7 @@ def pickup(self, geom_name: str): def main(): - env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True) + # compatilbe with rcs/SimplePickUpSimDigitHand-v0 and rcs/SimplePickUpSim-v0 env.reset() print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore # assert False diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py index 5ded5ddd..a100c1a0 100644 --- a/python/examples/grasp_demo_lab.py +++ b/python/examples/grasp_demo_lab.py @@ -39,7 +39,7 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in return waypoints def step(self, action: dict) -> dict: - return self.env.step(action)[-1] + return self.env.step(action)[0] def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: end_eff_pose = self.unwrapped.robot.get_cartesian_position() @@ -50,25 +50,10 @@ def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: for i in range(1, len(waypoints)): # calculate delta action - # delta_action = waypoints[i] * waypoints[i - 1].inverse() - pose = rcsss.common.Pose( - translation=waypoints[i].translation(), rpy_vector=np.array([-3.14159265e00, 1.57009246e-16, 0]) - ) - - # act = self._action(delta_action, gripper) - act = self._action(pose, gripper) - - obs = self.step(act) - ik_success = obs["ik_success"] - if not obs["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() - msg = ( - 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}!" - ) - logger.warning(msg) - return obs + delta_action = waypoints[i] * waypoints[i - 1].inverse() + + act = self._action(delta_action, gripper) + self.step(act) def approach(self, geom_name: str): waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=50) @@ -99,7 +84,7 @@ def main(): env = gym.make( "rcs/LabPickUpSimDigitHand-v0", render_mode="human", - delta_actions=False, + delta_actions=True, ) env.reset() controller = PickUpDemo(env) diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 7884fd70..540c37cd 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -151,11 +151,6 @@ def fr3_sim_env( logger.warning("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml") mjb_file = rcsss.scenes.get(str(mjcf), mjcf) simulation = sim.Sim(mjb_file) - """ - todo, without a simulation step, sim->d is not populated with the correct values in the robot. - check robot base world coordinates without this line to confirm. - """ - simulation.step(1) ik = rcsss.common.IK(urdf_path) robot = rcsss.sim.FR3(simulation, "0", ik) @@ -309,6 +304,29 @@ def __call__( # type: ignore return env_rel +class CamRobot(gym.Wrapper): + + def __init__(self, env): + super().__init__(env) + self.unwrapped: FR3Env + assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." + self.sim = env.get_wrapper_attr("sim") + self.cam_robot = rcsss.sim.FR3(self.sim, "1", env.unwrapped.robot.get_ik()) + self.cam_robot.set_parameters(default_fr3_sim_robot_cfg("lab_simple_pick_up_digit_hand")) + + def step(self, action: dict): + self.cam_robot.set_joints_hard( + [-0.78452318, -1.18096017, 1.75158399, -1.0718541, -0.11207275, 1.01050546, 2.47343638] + ) + obs, reward, done, truncated, info = super().step(action) + return obs, reward, done, truncated, info + + def reset(self, *, seed=None, options=None): + re = super().reset(seed=seed, options=options) + self.cam_robot.reset() + return re + + class FR3LabPickUpSimDigitHand(EnvCreator): def __call__( # type: ignore self, @@ -322,8 +340,8 @@ def __call__( # type: ignore 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)), + "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)), + "side": SimCameraConfig(identifier="eye-in-hand_1", type=int(CameraType.fixed)), } camera_cfg = SimCameraSetConfig( @@ -350,8 +368,8 @@ def __call__( # type: ignore sim_wrapper=RandomCubePosLab, ) env_rel = FR3LabPickUpSimSuccessWrapper(env_rel) - sim = env_rel.get_wrapper_attr("sim") + env_rel = CamRobot(env_rel) if render_mode == "human": + sim = env_rel.get_wrapper_attr("sim") sim.open_gui() - return env_rel From 70607c44e05e045536c3d6689b0393b225851ef9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 24 Mar 2025 11:13:52 +0100 Subject: [PATCH 18/24] refactor(lab env): flexible camera robot joint position --- python/examples/grasp_demo_lab.py | 3 +++ python/rcsss/envs/factories.py | 11 ++++++----- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py index a100c1a0..c2425864 100644 --- a/python/examples/grasp_demo_lab.py +++ b/python/examples/grasp_demo_lab.py @@ -85,6 +85,9 @@ def main(): "rcs/LabPickUpSimDigitHand-v0", render_mode="human", delta_actions=True, + cam_robot_joints=np.array( + [-0.78452318, -1.18096017, 1.75158399, -1.0718541, -0.11207275, 1.01050546, 2.47343638] + ), ) env.reset() controller = PickUpDemo(env) diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 540c37cd..d14c6cb1 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -28,6 +28,7 @@ RandomCubePosLab, SimWrapper, ) +from rcsss.envs.space_utils import Vec7Type from rcsss.envs.utils import ( default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, @@ -306,18 +307,17 @@ def __call__( # type: ignore class CamRobot(gym.Wrapper): - def __init__(self, env): + def __init__(self, env, cam_robot_joints: Vec7Type): super().__init__(env) self.unwrapped: FR3Env assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." self.sim = env.get_wrapper_attr("sim") self.cam_robot = rcsss.sim.FR3(self.sim, "1", env.unwrapped.robot.get_ik()) self.cam_robot.set_parameters(default_fr3_sim_robot_cfg("lab_simple_pick_up_digit_hand")) + self.cam_robot_joints = cam_robot_joints def step(self, action: dict): - self.cam_robot.set_joints_hard( - [-0.78452318, -1.18096017, 1.75158399, -1.0718541, -0.11207275, 1.01050546, 2.47343638] - ) + self.cam_robot.set_joints_hard(self.cam_robot_joints) obs, reward, done, truncated, info = super().step(action) return obs, reward, done, truncated, info @@ -330,6 +330,7 @@ def reset(self, *, seed=None, options=None): class FR3LabPickUpSimDigitHand(EnvCreator): def __call__( # type: ignore self, + cam_robot_joints: Vec7Type, render_mode: str = "human", control_mode: str = "xyzrpy", resolution: tuple[int, int] | None = None, @@ -368,7 +369,7 @@ def __call__( # type: ignore sim_wrapper=RandomCubePosLab, ) env_rel = FR3LabPickUpSimSuccessWrapper(env_rel) - env_rel = CamRobot(env_rel) + env_rel = CamRobot(env_rel, cam_robot_joints) if render_mode == "human": sim = env_rel.get_wrapper_attr("sim") sim.open_gui() From 80e2c7cde9167955259b6d12114895eb161a1770 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 10 Apr 2025 10:47:03 +0200 Subject: [PATCH 19/24] feat(sim fr3): make convergence callback registration optional --- python/rcsss/_core/sim.pyi | 4 +++- src/pybind/rcsss.cpp | 5 +++-- src/sim/FR3.cpp | 16 +++++++++------- src/sim/FR3.h | 2 +- 4 files changed, 16 insertions(+), 11 deletions(-) diff --git a/python/rcsss/_core/sim.pyi b/python/rcsss/_core/sim.pyi index 1bab6b51..9357b0a3 100644 --- a/python/rcsss/_core/sim.pyi +++ b/python/rcsss/_core/sim.pyi @@ -86,7 +86,9 @@ class FHState(rcsss._core.common.GState): def max_unnormalized_width(self) -> float: ... class FR3(rcsss._core.common.Robot): - def __init__(self, sim: Sim, id: str, ik: rcsss._core.common.IK) -> None: ... + def __init__( + self, sim: Sim, id: str, ik: rcsss._core.common.IK, register_convergence_callback: bool = True + ) -> None: ... def get_parameters(self) -> FR3Config: ... def get_state(self) -> FR3State: ... def set_joints_hard(self, q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]) -> None: ... diff --git a/src/pybind/rcsss.cpp b/src/pybind/rcsss.cpp index 3753f738..a4d996f8 100644 --- a/src/pybind/rcsss.cpp +++ b/src/pybind/rcsss.cpp @@ -466,8 +466,9 @@ PYBIND11_MODULE(_core, m) { py::class_>( sim, "FR3") .def(py::init, const std::string &, - std::shared_ptr>(), - py::arg("sim"), py::arg("id"), py::arg("ik")) + std::shared_ptr, bool>(), + py::arg("sim"), py::arg("id"), py::arg("ik"), + py::arg("register_convergence_callback") = true) .def("get_parameters", &rcs::sim::FR3::get_parameters) .def("set_parameters", &rcs::sim::FR3::set_parameters, py::arg("cfg")) .def("set_joints_hard", &rcs::sim::FR3::set_joints_hard, py::arg("q")) diff --git a/src/sim/FR3.cpp b/src/sim/FR3.cpp index 2bdf25fe..25747af9 100644 --- a/src/sim/FR3.cpp +++ b/src/sim/FR3.cpp @@ -43,15 +43,17 @@ namespace sim { // TODO: use C++11 feature to call one constructor from another FR3::FR3(std::shared_ptr sim, const std::string& id, - std::shared_ptr ik) + std::shared_ptr ik, bool register_convergence_callback) : sim{sim}, id{id}, cfg{}, state{}, m_ik(ik) { this->init_ids(); - this->sim->register_cb(std::bind(&FR3::is_arrived_callback, this), - this->cfg.seconds_between_callbacks); - this->sim->register_cb(std::bind(&FR3::is_moving_callback, this), - this->cfg.seconds_between_callbacks); - this->sim->register_all_cb(std::bind(&FR3::convergence_callback, this), - this->cfg.seconds_between_callbacks); + if (register_convergence_callback) { + this->sim->register_cb(std::bind(&FR3::is_arrived_callback, this), + this->cfg.seconds_between_callbacks); + this->sim->register_cb(std::bind(&FR3::is_moving_callback, this), + this->cfg.seconds_between_callbacks); + this->sim->register_all_cb(std::bind(&FR3::convergence_callback, this), + this->cfg.seconds_between_callbacks); + } this->sim->register_any_cb(std::bind(&FR3::collision_callback, this), this->cfg.seconds_between_callbacks); this->m_reset(); diff --git a/src/sim/FR3.h b/src/sim/FR3.h index 52acf9dc..43b722e2 100644 --- a/src/sim/FR3.h +++ b/src/sim/FR3.h @@ -37,7 +37,7 @@ struct FR3State : common::RState { class FR3 : public common::Robot { public: FR3(std::shared_ptr sim, const std::string &id, - std::shared_ptr ik); + std::shared_ptr ik, bool register_convergence_callback); ~FR3() override; bool set_parameters(const FR3Config &cfg); FR3Config *get_parameters() override; From 2d905eb91416c6e40e16f7e0715b11e1c279ebe1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 10 Apr 2025 10:49:01 +0200 Subject: [PATCH 20/24] fix(sim): allow string path in tcp offset getter --- python/examples/grasp_demo.py | 5 +++++ python/rcsss/envs/utils.py | 3 +++ 2 files changed, 8 insertions(+) diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index ee2b1925..7c3ff1ee 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -83,6 +83,11 @@ def pickup(self, geom_name: str): def main(): # compatilbe with rcs/SimplePickUpSimDigitHand-v0 and rcs/SimplePickUpSim-v0 + env = gym.make( + "rcs/SimplePickUpSimDigitHand-v0", + render_mode="human", + delta_actions=True, + ) env.reset() print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore # assert False diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index 6ff7787f..8c39289c 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -1,5 +1,6 @@ import logging from os import PathLike +from pathlib import Path import mujoco as mj import numpy as np @@ -81,6 +82,8 @@ def get_tcp_offset(mjcf: str | PathLike): Returns: rcsss.common.Pose: The tcp offset. """ + if type(mjcf) is not str: + mjcf = Path(mjcf) mjmdl = rcsss.scenes.get(str(mjcf), mjcf) if mjmdl.suffix == ".xml": model = mj.MjModel.from_xml_path(str(mjmdl)) From 977c04ebd2b2f391fcc0e91c125403c1aaa3575f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 10 Apr 2025 10:54:57 +0200 Subject: [PATCH 21/24] refactor(sim envs): simple and lab pick up share more code - update models - refactored random cube pos lab wrapper into random cube pos wrapper - refactored fr3 lab pick up sim success wrapper into pick cube success wrapper - refactored sim env creation for pick up tasks into own method - fix cam robot: does not register itself to convergence callback anymore --- .gitignore | 2 +- CMakeLists.txt | 2 +- python/rcsss/envs/factories.py | 109 ++++++++++++--------------------- python/rcsss/envs/sim.py | 53 +--------------- 4 files changed, 44 insertions(+), 122 deletions(-) diff --git a/.gitignore b/.gitignore index fff58551..362217ad 100644 --- a/.gitignore +++ b/.gitignore @@ -8,7 +8,7 @@ __pycache__ .ruff_cache .mypy_cache dist -.venv +*venv* config.yaml MUJOCO_LOG.TXT src/pybind/mujoco diff --git a/CMakeLists.txt b/CMakeLists.txt index c6f6e994..696107c0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,7 +98,7 @@ FetchContent_MakeAvailable(libfranka rl pybind11) if (${INCLUDE_UTN_MODELS}) if (DEFINED GITLAB_MODELS_TOKEN) include(download_scenes) - set(ref 48c88f05aa34730acbe80bf4b50e7d5a789b927a) + set(ref 00869d446940260ec233469182a1ed2290cc60e3) FetchContent_Declare( scenes URL "https://gitos.rrze.fau.de/api/v4/projects/1100/repository/archive?path=scenes&sha=${ref}" diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index d14c6cb1..f5482c97 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -21,11 +21,9 @@ from rcsss.envs.hw import FR3HW from rcsss.envs.sim import ( CollisionGuard, - FR3LabPickUpSimSuccessWrapper, FR3Sim, - FR3SimplePickUpSimSuccessWrapper, + PickCubeSuccessWrapper, RandomCubePos, - RandomCubePosLab, SimWrapper, ) from rcsss.envs.space_utils import Vec7Type @@ -211,11 +209,37 @@ def __call__( # type: ignore ) +def make_sim_task_envs( + mjcf: str, + render_mode: str = "human", + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, + delta_actions: bool = True, + camera_cfg: SimCameraConfig | None = None, +) -> gym.Env: + + env_rel = fr3_sim_env( + control_mode=control_mode, + robot_cfg=default_fr3_sim_robot_cfg(mjcf), + 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=mjcf, + sim_wrapper=RandomCubePos, + ) + env_rel = PickCubeSuccessWrapper(env_rel) + if render_mode == "human": + env_rel.get_wrapper_attr("sim").open_gui() + + return env_rel + + class FR3SimplePickUpSim(EnvCreator): def __call__( # type: ignore self, render_mode: str = "human", - control_mode: str = "xyzrpy", + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, resolution: tuple[int, int] | None = None, frame_rate: int = 10, delta_actions: bool = True, @@ -239,33 +263,14 @@ def __call__( # type: ignore 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=default_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", - sim_wrapper=RandomCubePos, - ) - env_rel = FR3SimplePickUpSimSuccessWrapper(env_rel) - if render_mode == "human": - env_rel.get_wrapper_attr("sim").open_gui() - - return env_rel + return make_sim_task_envs("simple_pick_up", render_mode, control_mode, delta_actions, camera_cfg) class FR3SimplePickUpSimDigitHand(EnvCreator): def __call__( # type: ignore self, render_mode: str = "human", - control_mode: str = "xyzrpy", + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, resolution: tuple[int, int] | None = None, frame_rate: int = 10, delta_actions: bool = True, @@ -282,27 +287,7 @@ def __call__( # type: ignore 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=default_fr3_sim_robot_cfg("fr3_simple_pick_up_digit_hand"), - 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 + return make_sim_task_envs("fr3_simple_pick_up_digit_hand", render_mode, control_mode, delta_actions, camera_cfg) class CamRobot(gym.Wrapper): @@ -312,7 +297,7 @@ def __init__(self, env, cam_robot_joints: Vec7Type): self.unwrapped: FR3Env assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." self.sim = env.get_wrapper_attr("sim") - self.cam_robot = rcsss.sim.FR3(self.sim, "1", env.unwrapped.robot.get_ik()) + self.cam_robot = rcsss.sim.FR3(self.sim, "1", env.unwrapped.robot.get_ik(), register_convergence_callback=False) self.cam_robot.set_parameters(default_fr3_sim_robot_cfg("lab_simple_pick_up_digit_hand")) self.cam_robot_joints = cam_robot_joints @@ -332,7 +317,7 @@ def __call__( # type: ignore self, cam_robot_joints: Vec7Type, render_mode: str = "human", - control_mode: str = "xyzrpy", + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, resolution: tuple[int, int] | None = None, frame_rate: int = 10, delta_actions: bool = True, @@ -352,25 +337,11 @@ def __call__( # type: ignore 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=default_fr3_sim_robot_cfg("lab_simple_pick_up_digit_hand"), - 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 = make_sim_task_envs( + "lab_simple_pick_up_digit_hand", + render_mode, + control_mode, + delta_actions, + camera_cfg, ) - env_rel = FR3LabPickUpSimSuccessWrapper(env_rel) - env_rel = CamRobot(env_rel, cam_robot_joints) - if render_mode == "human": - sim = env_rel.get_wrapper_attr("sim") - sim.open_gui() - return env_rel + return CamRobot(env_rel, cam_robot_joints) diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 45053f0e..3ac7f498 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -176,7 +176,7 @@ def env_from_xml_paths( ) -class RandomCubePosLab(SimWrapper): +class RandomCubePos(SimWrapper): """Wrapper to randomly place cube in the lab environments.""" def reset( @@ -197,25 +197,7 @@ def reset( return obs, info -class RandomCubePos(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) - self.sim.step(1) - iso_cube = [0.498, 0.0, 0.226] - pos_z = 0.03 - pos_x = iso_cube[0] + np.random.random() * 0.2 - 0.1 - pos_y = iso_cube[1] + np.random.random() * 0.2 - 0.1 - - self.sim.data.joint("yellow-box-joint").qpos[:3] = [pos_x, pos_y, pos_z] - - return obs, info - - -class FR3SimplePickUpSimSuccessWrapper(gym.Wrapper): +class PickCubeSuccessWrapper(gym.Wrapper): """Wrapper to check if the cube is successfully picked up in the FR3SimplePickUpSim environment.""" EE_HOME = np.array([0.34169773, 0.00047028, 0.4309004]) @@ -241,34 +223,3 @@ def step(self, action: dict[str, Any]): reward = -diff_cube_home - diff_ee_cube return obs, reward, success, truncated, info - - -class FR3LabPickUpSimSuccessWrapper(gym.Wrapper): - """ - Wrapper to check if the cube is successfully picked up in the LabPickupSim environments. - This is also used to handle the reset of the second robot. - """ - - EE_HOME = np.array([0.34169773, 0.00047028, 0.4309004]) - - def __init__(self, env): - super().__init__(env) - self.unwrapped: FR3Env - assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." - self.sim = env.get_wrapper_attr("sim") - - def step(self, action: dict[str, Any]): - obs, reward, done, truncated, info = super().step(action) - - success = ( - self.sim.data.joint("yellow-box-joint").qpos[2] > 0.3 - and obs["gripper"] == GripperWrapper.BINARY_GRIPPER_CLOSED - ) - diff_ee_cube = np.linalg.norm( - self.sim.data.joint("yellow-box-joint").qpos[:3] - - self.unwrapped.robot.get_cartesian_position().translation() - ) - diff_cube_home = np.linalg.norm(self.sim.data.joint("yellow-box-joint").qpos[:3] - self.EE_HOME) - reward = -diff_cube_home - diff_ee_cube - - return obs, reward, success, truncated, info From b1491e7c18363a956383a5575f1e78db2c64f665 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 10 Apr 2025 16:55:29 +0200 Subject: [PATCH 22/24] fix(py lint): type annotations and imports --- CMakeLists.txt | 2 +- python/examples/grasp_demo_lab.py | 6 ++---- python/rcsss/envs/factories.py | 2 +- python/rcsss/envs/utils.py | 10 +++++----- 4 files changed, 9 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 696107c0..636ecf15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,7 +98,7 @@ FetchContent_MakeAvailable(libfranka rl pybind11) if (${INCLUDE_UTN_MODELS}) if (DEFINED GITLAB_MODELS_TOKEN) include(download_scenes) - set(ref 00869d446940260ec233469182a1ed2290cc60e3) + set(ref bfdd70ae189ed3a925bf1c16d29162e847a21d56) FetchContent_Declare( scenes URL "https://gitos.rrze.fau.de/api/v4/projects/1100/repository/archive?path=scenes&sha=${ref}" diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py index c2425864..d3bb6c62 100644 --- a/python/examples/grasp_demo_lab.py +++ b/python/examples/grasp_demo_lab.py @@ -4,7 +4,6 @@ import gymnasium as gym import mujoco import numpy as np -import rcsss.envs.base from rcsss._core.common import Pose from rcsss.envs.base import FR3Env, GripperWrapper @@ -51,9 +50,8 @@ def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper. for i in range(1, len(waypoints)): # calculate delta action delta_action = waypoints[i] * waypoints[i - 1].inverse() - - act = self._action(delta_action, gripper) - self.step(act) + obs = self.step(self._action(delta_action, gripper)) + return obs def approach(self, geom_name: str): waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=50) diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index f5482c97..865f0dcb 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -214,7 +214,7 @@ def make_sim_task_envs( render_mode: str = "human", control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, delta_actions: bool = True, - camera_cfg: SimCameraConfig | None = None, + camera_cfg: SimCameraSetConfig | None = None, ) -> gym.Env: env_rel = fr3_sim_env( diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index 8c39289c..9b9590e2 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -7,16 +7,16 @@ import rcsss from rcsss import sim from rcsss._core.hw import FR3Config, IKController -from rcsss._core.sim import CameraType, SimCameraSetConfig +from rcsss._core.sim import CameraType from rcsss.camera.interface import BaseCameraConfig from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig -from rcsss.camera.sim import SimCameraConfig +from rcsss.camera.sim import SimCameraConfig, SimCameraSetConfig logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) -def default_fr3_sim_robot_cfg(mjcf: str | PathLike = "fr3_empty_world") -> sim.FR3Config: +def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world") -> sim.FR3Config: cfg = sim.FR3Config() cfg.tcp_offset = get_tcp_offset(mjcf) cfg.realtime = False @@ -71,7 +71,7 @@ def default_mujoco_cameraset_cfg(): ) -def get_tcp_offset(mjcf: str | PathLike): +def get_tcp_offset(mjcf: str | Path): """Reads out tcp offset set in mjcf file. Convention: The tcp offset is stored in the model as a numeric attribute named "tcp_offset". @@ -82,7 +82,7 @@ def get_tcp_offset(mjcf: str | PathLike): Returns: rcsss.common.Pose: The tcp offset. """ - if type(mjcf) is not str: + if isinstance(mjcf, str): mjcf = Path(mjcf) mjmdl = rcsss.scenes.get(str(mjcf), mjcf) if mjmdl.suffix == ".xml": From 7014c6034c25fda501a35c74077c115bd9a24dec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 10 Apr 2025 17:29:22 +0200 Subject: [PATCH 23/24] fix(hw fr3): default register convergence callback --- src/sim/FR3.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sim/FR3.h b/src/sim/FR3.h index 43b722e2..52964f71 100644 --- a/src/sim/FR3.h +++ b/src/sim/FR3.h @@ -37,7 +37,8 @@ struct FR3State : common::RState { class FR3 : public common::Robot { public: FR3(std::shared_ptr sim, const std::string &id, - std::shared_ptr ik, bool register_convergence_callback); + std::shared_ptr ik, + bool register_convergence_callback = true); ~FR3() override; bool set_parameters(const FR3Config &cfg); FR3Config *get_parameters() override; From 2dda99574b3613d290906a3070402f6b39df2366 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 10 Apr 2025 18:20:16 +0200 Subject: [PATCH 24/24] tests(env): fixed collision tests --- python/tests/test_sim_envs.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 7d919135..83baa811 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -145,8 +145,8 @@ def test_collision_trpy(self, cfg, gripper_cfg, cam_cfg): ) obs, _ = env.reset() # an obvious below ground collision action - obs["xyzrpy"][0] = 0.3 - obs["xyzrpy"][2] = -0.2 + obs["xyzrpy"][0] = 0.4 + obs["xyzrpy"][2] = -0.05 collision_action = TRPYDictType(xyzrpy=obs["xyzrpy"]) collision_action.update(GripperDictType(gripper=0)) obs, _, _, _, info = env.step(collision_action) @@ -163,14 +163,14 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): gripper_cfg=gripper_cfg, collision_guard=True, camera_set_cfg=cam_cfg, - max_relative_movement=0.5, + max_relative_movement=None, ) obs, _ = env.reset() unwrapped = cast(FR3Env, env.unwrapped) p1 = unwrapped.robot.get_joint_position() # an obvious below ground collision action - obs["xyzrpy"][0] = 0.3 - obs["xyzrpy"][2] = -0.2 + obs["xyzrpy"][0] = 0.4 + obs["xyzrpy"][2] = -0.05 collision_action = TRPYDictType(xyzrpy=obs["xyzrpy"]) collision_action.update(GripperDictType(gripper=0)) _, _, _, _, info = env.step(collision_action) @@ -265,8 +265,8 @@ def test_collision_tquart(self, cfg, gripper_cfg, cam_cfg): ) obs, _ = env.reset() # an obvious below ground collision action - obs["tquart"][0] = 0.3 - obs["tquart"][2] = -0.2 + obs["tquart"][0] = 0.4 + obs["tquart"][2] = -0.05 collision_action = TQuartDictType(tquart=obs["tquart"]) collision_action.update(GripperDictType(gripper=0)) _, _, _, _, info = env.step(collision_action) @@ -289,8 +289,8 @@ def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): unwrapped = cast(FR3Env, env.unwrapped) p1 = unwrapped.robot.get_joint_position() # an obvious below ground collision action - obs["tquart"][0] = 0.3 - obs["tquart"][2] = -0.2 + obs["tquart"][0] = 0.4 + obs["tquart"][2] = -0.05 collision_action = TQuartDictType(tquart=obs["tquart"]) collision_action.update(GripperDictType(gripper=0)) _, _, _, _, info = env.step(collision_action)