feat(cmake): egl headers

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)

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
     ```
 

include/rcs/Kinematics.h

@@ -1,8 +1,8 @@
 #ifndef RCS_IK_H
 #define RCS_IK_H
 
-#include <eigen3/Eigen/Eigen>
-#include <eigen3/Eigen/Geometry>
+#include <Eigen/Eigen>
+#include <Eigen/Geometry>
 #include <memory>
 #include <optional>
 #include <string>

include/rcs/Pose.h

@@ -1,8 +1,8 @@
 #ifndef RCS_POSE_H
 #define RCS_POSE_H
 
-#include <eigen3/Eigen/Eigen>
-#include <eigen3/Eigen/Geometry>
+#include <Eigen/Eigen>
+#include <Eigen/Geometry>
 #include <memory>
 
 #include "utils.h"

include/rcs/utils.h

@@ -1,7 +1,7 @@
 #ifndef RCS_UTIL_H
 #define RCS_UTIL_H
 
-#include <eigen3/Eigen/Eigen>
+#include <Eigen/Eigen>
 #include <memory>
 
 namespace rcs {

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]

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,12 +102,41 @@ 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
 # register(
 #    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 {}

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
 
 

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()

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

src/sim/camera.cpp

@@ -3,10 +3,10 @@
 
 #include <math.h>
 
+#include <Eigen/Eigen>
 #include <algorithm>
 #include <cstdlib>
 #include <cstring>
-#include <eigen3/Eigen/Eigen>
 #include <memory>
 #include <set>
 #include <stdexcept>

src/sim/camera.h

@@ -4,7 +4,7 @@
 #include <rcs/Pose.h>
 #include <rcs/Robot.h>
 
-#include <eigen3/Eigen/Eigen>
+#include <Eigen/Eigen>
 #include <mutex>
 #include <optional>
 #include <set>

src/sim/sim.cpp

@@ -1,6 +1,5 @@
 #include "sim/sim.h"
 
-#include <GLFW/glfw3.h>
 #include <assert.h>
 
 #include <chrono>