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 3c8896ff..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 b8234983a5e35e222c955f9145ac4cd129827a8e) + 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/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/examples/grasp_demo.py b/python/examples/grasp_demo.py index baef831f..7c3ff1ee 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -32,6 +32,7 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in 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: dict) -> dict: @@ -81,8 +82,15 @@ 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 = 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 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..d3bb6c62 --- /dev/null +++ b/python/examples/grasp_demo_lab.py @@ -0,0 +1,96 @@ +import logging +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.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: 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]: + end_eff_pose = self.unwrapped.robot.get_cartesian_position() + goal_pose = self.get_object_pose(geom_name=geom_name) + 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)) + return obs + + def approach(self, geom_name: str): + 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=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=50) + 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=50) + 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/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) + controller.pickup("yellow_box_geom") + + +if __name__ == "__main__": + main() diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index 67dcd663..63d4cf6f 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 +from rcsss.envs.factories import ( + FR3LabPickUpSimDigitHand, + FR3Real, + FR3SimplePickUpSim, + FR3SimplePickUpSimDigitHand, +) # available mujoco scenes scenes = { @@ -21,6 +26,16 @@ id="rcs/SimplePickUpSim-v0", entry_point=FR3SimplePickUpSim(), ) + +register( + id="rcs/SimplePickUpSimDigitHand-v0", + entry_point=FR3SimplePickUpSimDigitHand(), +) +register( + id="rcs/LabPickUpSimDigitHand-v0", + entry_point=FR3LabPickUpSimDigitHand(), +) + register( id="rcs/FR3Real-v0", entry_point=FR3Real(), 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/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 b5c85388..865f0dcb 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, @@ -25,60 +22,24 @@ from rcsss.envs.sim import ( CollisionGuard, FR3Sim, - FR3SimplePickUpSimSuccessWrapper, + PickCubeSuccessWrapper, RandomCubePos, SimWrapper, ) +from rcsss.envs.space_utils import Vec7Type +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 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, @@ -149,30 +110,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 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, @@ -211,8 +148,9 @@ 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") + mjb_file = rcsss.scenes.get(str(mjcf), mjcf) + simulation = sim.Sim(mjb_file) - simulation = sim.Sim(rcsss.scenes.get(str(mjcf), mjcf)) ik = rcsss.common.IK(urdf_path) robot = rcsss.sim.FR3(simulation, "0", ik) robot.set_parameters(robot_cfg) @@ -271,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: SimCameraSetConfig | 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, @@ -299,23 +263,85 @@ 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, + 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: ControlMode = ControlMode.CARTESIAN_TRPY, + resolution: tuple[int, int] | None = None, + frame_rate: int = 10, + delta_actions: bool = True, + ) -> gym.Env: + if resolution is None: + resolution = (256, 256) + + cameras = {"wrist": 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 = FR3SimplePickUpSimSuccessWrapper(env_rel) - if render_mode == "human": - env_rel.get_wrapper_attr("sim").open_gui() + return make_sim_task_envs("fr3_simple_pick_up_digit_hand", render_mode, control_mode, delta_actions, camera_cfg) + - return env_rel +class CamRobot(gym.Wrapper): + + 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(), 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 + + def step(self, action: dict): + self.cam_robot.set_joints_hard(self.cam_robot_joints) + 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, + cam_robot_joints: Vec7Type, + render_mode: str = "human", + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, + resolution: tuple[int, int] | None = None, + frame_rate: int = 10, + delta_actions: bool = True, + ) -> gym.Env: + if resolution is None: + resolution = (256, 256) + + cameras = { + "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( + cameras=cameras, + resolution_width=resolution[0], + resolution_height=resolution[1], + frame_rate=frame_rate, + physical_units=True, + ) + env_rel = make_sim_task_envs( + "lab_simple_pick_up_digit_hand", + render_mode, + control_mode, + delta_actions, + camera_cfg, + ) + return CamRobot(env_rel, cam_robot_joints) diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 08a9e6a4..3ac7f498 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -7,6 +7,9 @@ from rcsss import sim from rcsss.envs.base import ControlMode, FR3Env, GripperWrapper +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + class SimWrapper(gym.Wrapper): def __init__(self, env: gym.Env, simulation: sim.Sim): @@ -18,6 +21,7 @@ def __init__(self, env: gym.Env, simulation: sim.Sim): class FR3Sim(gym.Wrapper): 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) super().__init__(env) @@ -42,6 +46,7 @@ def reset( ) -> tuple[dict[str, Any], dict[str, Any]]: self.sim.reset() _, info = super().reset(seed=seed, options=options) + # self.unwrapped.robot.move_home() self.sim.step(1) obs = cast(dict, self.unwrapped.get_obs()) return obs, info @@ -172,23 +177,27 @@ def env_from_xml_paths( class RandomCubePos(SimWrapper): - """Wrapper to randomly place cube in the FR3SimplePickUpSim environment.""" + """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]]: 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 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] 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]) diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py new file mode 100644 index 00000000..9b9590e2 --- /dev/null +++ b/python/rcsss/envs/utils.py @@ -0,0 +1,112 @@ +import logging +from os import PathLike +from pathlib import Path + +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 +from rcsss.camera.interface import BaseCameraConfig +from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig +from rcsss.camera.sim import SimCameraConfig, SimCameraSetConfig + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +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 + return cfg + + +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 + ) + + +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". + + Args: + mjcf (str | PathLike): Path to the mjcf file. + + Returns: + rcsss.common.Pose: The tcp offset. + """ + if isinstance(mjcf, str): + mjcf = Path(mjcf) + 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) + 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: + 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 diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 11fedca2..83baa811 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, ) @@ -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) 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 14081036..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(); @@ -207,6 +209,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]; } } diff --git a/src/sim/FR3.h b/src/sim/FR3.h index 52acf9dc..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); + std::shared_ptr ik, + bool register_convergence_callback = true); ~FR3() override; bool set_parameters(const FR3Config &cfg); FR3Config *get_parameters() override;