diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b5c0ef2..83770a0c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,7 +33,6 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) include(FetchContent) -find_package(Eigen3 REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) find_package(MuJoCo REQUIRED) find_package(pinocchio REQUIRED) @@ -45,8 +44,22 @@ FetchContent_Declare(pybind11 GIT_PROGRESS TRUE EXCLUDE_FROM_ALL ) +FetchContent_Declare(Eigen3 + GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git + GIT_TAG 3.4.1 + GIT_PROGRESS TRUE + EXCLUDE_FROM_ALL +) +FetchContent_Declare( + egl_headers + GIT_REPOSITORY https://github.com/KhronosGroup/EGL-Registry.git + GIT_TAG main + GIT_SHALLOW TRUE +) -FetchContent_MakeAvailable(pybind11) +FetchContent_MakeAvailable(pybind11 Eigen3 egl_headers) include(compile_scenes) +# egl headers +include_directories(${egl_headers_SOURCE_DIR}/api) add_subdirectory(src) diff --git a/README.md b/README.md index e5bf2b2b..909d83a7 100644 --- a/README.md +++ b/README.md @@ -110,11 +110,11 @@ We build and test RCS on the latest Debian and on the latest Ubuntu LTS. ```shell conda create -n rcs python=3.11 conda activate rcs - pip install -r requirements.txt ``` 3. **Install RCS**: ```shell + pip install -r requirements.txt pip install -ve . --no-build-isolation ``` diff --git a/include/rcs/Kinematics.h b/include/rcs/Kinematics.h index 57d7973a..ce6b73cb 100644 --- a/include/rcs/Kinematics.h +++ b/include/rcs/Kinematics.h @@ -1,8 +1,8 @@ #ifndef RCS_IK_H #define RCS_IK_H -#include -#include +#include +#include #include #include #include diff --git a/include/rcs/Pose.h b/include/rcs/Pose.h index e2697fb3..1f0a0e36 100644 --- a/include/rcs/Pose.h +++ b/include/rcs/Pose.h @@ -1,8 +1,8 @@ #ifndef RCS_POSE_H #define RCS_POSE_H -#include -#include +#include +#include #include #include "utils.h" diff --git a/include/rcs/utils.h b/include/rcs/utils.h index 472c54de..5ebc8cce 100644 --- a/include/rcs/utils.h +++ b/include/rcs/utils.h @@ -1,7 +1,7 @@ #ifndef RCS_UTIL_H #define RCS_UTIL_H -#include +#include #include namespace rcs { diff --git a/pyproject.toml b/pyproject.toml index 4474fadf..5ccd3c38 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,5 +1,14 @@ [build-system] -requires = [] +requires = [ + "build", + "wheel", + "scikit-build-core>=0.3.3", + "pybind11", + "cmake", + "ninja", + "mujoco==3.2.6", + "pin==3.7.0", +] build-backend = "scikit_build_core.build" [project] diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index fdfee7bb..55d90a7f 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -2,14 +2,17 @@ import os import site -from dataclasses import dataclass +from dataclasses import dataclass, field from gymnasium import register +from lerobot.configs.types import PolicyFeature +from lerobot.envs.configs import EnvConfig from rcs._core import __version__, common from rcs.envs.creators import ( FR3LabDigitGripperPickUpSimEnvCreator, FR3SimplePickUpSimEnvCreator, ) +from rcs.lerobot import Pick from rcs import camera, envs, hand, sim @@ -99,8 +102,11 @@ def get_scene_urdf(scene_name: str) -> str | None: id="rcs/FR3LabDigitGripperPickUpSim-v0", entry_point=FR3LabDigitGripperPickUpSimEnvCreator(), ) +register( + id="rcs/pick", + entry_point=Pick(), +) -# Genius TODO: Add the tacto version of the SimEnvCreator # TODO: gym.make("rcs/FR3SimEnv-v0") results in a pickling error: # TypeError: cannot pickle 'rcs._core.sim.SimRobotConfig' object # cf. https://pybind11.readthedocs.io/en/stable/advanced/classes.html#deepcopy-support @@ -108,3 +114,29 @@ def get_scene_urdf(scene_name: str) -> str | None: # id="rcs/FR3SimEnv-v0", # entry_point=SimEnvCreator(), # ) + + +# lerobot env registration +@EnvConfig.register_subclass("rcs/pick") +@dataclass +class RCSEnvConfig(EnvConfig): + task: str | None = "pick" + fps: int = 30 + features: dict[str, PolicyFeature] = field(default_factory=dict) + features_map: dict[str, str] = field(default_factory=dict) + max_parallel_tasks: int = 1 + disable_env_checker: bool = True + + @property + def package_name(self) -> str: + """Package name to import if environment not found in gym registry""" + return "rcs" + + @property + def gym_id(self) -> str: + """ID string used in gym.make() to instantiate the environment""" + return f"{self.package_name}/{self.task}" + + @property + def gym_kwargs(self) -> dict: + return {} diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 589b49fd..77cdf3cb 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -65,6 +65,7 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo info["collision"] = info["collision"] or state.collision info["ik_success"] = state.ik_success info["is_sim_converged"] = self.sim.is_converged() + info["tau_ext"] = self.sim.data.qfrc_constraint[:7] # truncate episode if collision obs.update(self.unwrapped.get_obs()) return obs, 0, False, info["collision"] or not state.ik_success, info @@ -77,6 +78,7 @@ def reset( self.sim.step(1) # todo: an obs method that is recursive over wrappers would be needed obs.update(self.unwrapped.get_obs()) + info["tau_ext"] = np.array(self.sim.data.qfrc_constraint[:7]) return obs, info diff --git a/python/rcs/lerobot.py b/python/rcs/lerobot.py new file mode 100644 index 00000000..10028bca --- /dev/null +++ b/python/rcs/lerobot.py @@ -0,0 +1,154 @@ +from typing import Any + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import EnvCreator +from rcs._core.sim import CameraType, SimCameraConfig, SimConfig +from rcs.envs.base import ControlMode, RelativeActionSpace +from rcs.envs.creators import SimTaskEnvCreator +from rcs.envs.space_utils import ActObsInfoWrapper +from rcs.sim import SimGripperConfig +from rcs_tacto.tacto_wrapper import TactoSimWrapper + +import rcs + +SCENE_FILE = "beast_refiner/rcs_env/rcs_icra_scene_digit/scene.xml" +CAMERAS = [ + "wrist", + "bird_eye", + "side", + "side_right", + "side_wide", +] + + +class AlignSpaceWrapper(ActObsInfoWrapper): + + def __init__(self, env): + super().__init__(env) + self.observation_space = gym.spaces.Dict( + { + "state": gym.spaces.Box(low=-np.inf, high=np.inf, shape=(15,), dtype=np.float32), + "images.image": env.observation_space["frames"]["side"]["rgb"]["data"], + "images.image2": env.observation_space["frames"]["wrist"]["rgb"]["data"], + "images.tactile_left": gym.spaces.Box(low=0, high=255, shape=(320, 240, 3), dtype=np.uint8), + "images.tactile_right": gym.spaces.Box(low=0, high=255, shape=(320, 240, 3), dtype=np.uint8), + } + ) + # self.action_space = env.action_space["joints"] + gym.spaces.Box(low=-np.inf, high=np.inf, shape=(8,), dtype=np.float32) + + def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: + observation = { + "state": np.concat([observation["joints"], observation["gripper"], info["tau_ext"]]), + "images.image": observation["frames"]["side"]["rgb"]["data"], + "images.image2": observation["frames"]["wrist"]["rgb"]["data"], + "images.tactile_left": observation["frames"]["tactile_left_tacto_pad_0"]["rgb"]["data"], + "images.tactile_right": observation["frames"]["tactile_right_tacto_pad_0"]["rgb"]["data"], + } + # tactile and tau_ext are still unsolved + + return observation, info + + def action(self, action: np.ndarray) -> dict[str, Any]: + action = {"joints": action[:7], "gripper": action[7]} + return action + + +def get_robot_cfg(): + robot_cfg = rcs.sim.SimRobotConfig() + robot_cfg.tcp_offset = rcs.common.Pose( + translation=np.array([0.0, 0.0, 0.15]), + rotation=np.array([[0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]]), + ) + robot_cfg.robot_type = rcs.common.RobotType.FR3 + robot_cfg.add_id("0") + robot_cfg.mjcf_scene_path = str(SCENE_FILE) + robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot + return robot_cfg + + +def make_single_env(): + robot_cfg = get_robot_cfg() + random_pos_args = { + "joint_name": "box_joint", + "init_object_pose": rcs.common.Pose( + translation=np.array([0.048, 0, 0.855]), + quaternion=np.array([0.0, 0.0, 1, 0]), + ), + "include_rotation": True, + "x_scale": 0.3, + "y_scale": 0.4, + "x_offset": -0.1, + "y_offset": -0.2, + } + gripper_cfg = SimGripperConfig() + gripper_cfg.collision_geoms = [] + gripper_cfg.collision_geoms_fingers = [] + gripper_cfg.add_id("0") + + sim_cfg = SimConfig() + sim_cfg.realtime = False + sim_cfg.async_control = True + sim_cfg.frequency = 30 # in Hz + + resolution = (256, 256) + cameras = { + cam: SimCameraConfig( + identifier=cam, + type=CameraType.fixed, + resolution_height=resolution[1], + resolution_width=resolution[0], + frame_rate=0, # this means render on demand + ) + for cam in CAMERAS + } + + env_factory = SimTaskEnvCreator() + env = env_factory( + robot_cfg=robot_cfg, + control_mode=ControlMode.JOINTS, + render_mode="", # put this to 'human' to open a gui to watch the simulation + delta_actions=False, + cameras=cameras, + random_pos_args=random_pos_args, + gripper_cfg=gripper_cfg, + sim_cfg=sim_cfg, + ) + env = RelativeActionSpace(env, max_mov=np.deg2rad(90)) + env = TactoSimWrapper( + env, + tacto_sites=["left_tacto_pad_0", "right_tacto_pad_0"], + tacto_geoms=["box_geom"], + tacto_fps=30, + enable_depth=False, + visualize=False, + ) + env = AlignSpaceWrapper(env) + return env + + +def make_env(n_envs: int = 1, use_async_envs: bool = False): + """ + Create vectorized environments for your custom task. + + Args: + n_envs: Number of parallel environments + use_async_envs: Whether to use AsyncVectorEnv or SyncVectorEnv + + Returns: + gym.vector.VectorEnv or dict mapping suite names to vectorized envs + """ + + # Choose vector environment type + env_cls = gym.vector.AsyncVectorEnv if use_async_envs else gym.vector.SyncVectorEnv + + # Create vectorized environment + vec_env = env_cls([make_single_env for _ in range(n_envs)]) + + return vec_env + + +class Pick(EnvCreator): + def __call__(self) -> gym.Env: + return make_single_env() diff --git a/requirements.txt b/requirements.txt index 7474dbd7..24238735 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,8 +1,2 @@ -build -wheel -scikit-build-core>=0.3.3 -pybind11 -cmake -ninja mujoco==3.2.6 pin==3.7.0 \ No newline at end of file diff --git a/src/sim/camera.cpp b/src/sim/camera.cpp index a70f8525..cead2d83 100644 --- a/src/sim/camera.cpp +++ b/src/sim/camera.cpp @@ -3,10 +3,10 @@ #include +#include #include #include #include -#include #include #include #include diff --git a/src/sim/camera.h b/src/sim/camera.h index 095991e1..8b3daa25 100644 --- a/src/sim/camera.h +++ b/src/sim/camera.h @@ -4,7 +4,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/sim/sim.cpp b/src/sim/sim.cpp index 8059921e..032c3f41 100644 --- a/src/sim/sim.cpp +++ b/src/sim/sim.cpp @@ -1,6 +1,5 @@ #include "sim/sim.h" -#include #include #include