Merge pull request #167 from utn-mi/juelg/osc

feat: async operational space controller

- adds Operational Space Controller support for hardware RCS
- adds async control support to RCS
- started to remove app code from RCS such as vive teleop and recorder wrapper (we might want to readd them later again but first need to get them right, they currently have large typing issues)

Author: juelg

CMakeLists.txt

@@ -53,7 +53,7 @@ find_package(MuJoCo REQUIRED)
 FetchContent_Declare(
     libfranka
     GIT_REPOSITORY https://github.com/frankaemika/libfranka.git
-    GIT_TAG 0.14.2
+    GIT_TAG 0.15.0
     GIT_PROGRESS TRUE
     EXCLUDE_FROM_ALL
 )
@@ -98,7 +98,7 @@ FetchContent_MakeAvailable(libfranka rl pybind11)
 if (${INCLUDE_UTN_MODELS})
     if (DEFINED GITLAB_MODELS_TOKEN)
         include(download_scenes)
-        set(ref bfdd70ae189ed3a925bf1c16d29162e847a21d56)
+        set(ref 055a6fdab480dde8443030236012f76d9be90918)
         FetchContent_Declare(
             scenes
             URL         "https://gitos.rrze.fau.de/api/v4/projects/1100/repository/archive?path=scenes&sha=${ref}"

pyproject.toml

@@ -22,10 +22,11 @@ dependencies = ["websockets>=11.0",
     "pillow~=10.3",
     "python-dotenv==1.0.1",
     "opencv-python~=4.10.0.84",
+    "h5py~=3.13.0",
     # NOTE: when changing the mujoco version, also change it in requirements_dev.txt
     "mujoco==3.2.6",
     # NOTE: Same for pinocchio (pin)
-    "pin==2.7.0"
+    "pin==2.7.0",
 ]
 readme = "README.md"
 maintainers = [

python/examples/fr3.py

@@ -4,7 +4,7 @@
 import numpy as np
 import rcsss
 from rcsss import sim
-from rcsss._core.hw import FR3Config, IKController
+from rcsss._core.hw import FR3Config, IKSolver
 from rcsss._core.sim import CameraType
 from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig
 from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager
@@ -82,7 +82,7 @@ def main():
             # add camera to have a rendering gui
             cameras = {
                 "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free)),
-                "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)),
+                "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)),
             }
             cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=20)
             camera_set = SimCameraSet(simulation, cam_cfg)  # noqa: F841
@@ -95,7 +95,7 @@ def main():
             robot = rcsss.hw.FR3(ROBOT_IP, ik)
             robot_cfg = FR3Config()
             robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset())
-            robot_cfg.controller = IKController.robotics_library
+            robot_cfg.ik_solver = IKSolver.rcs
             robot.set_parameters(robot_cfg)
 
             gripper_cfg_hw = rcsss.hw.FHConfig()

python/examples/grasp_demo.py

@@ -92,7 +92,7 @@ def main():
     print(env.unwrapped.robot.get_cartesian_position().translation())  # type: ignore
     # assert False
     controller = PickUpDemo(env)
-    controller.pickup("yellow_box_geom")
+    controller.pickup("box_geom")
 
 
 if __name__ == "__main__":

python/examples/grasp_demo_lab.py

@@ -89,7 +89,7 @@ def main():
     )
     env.reset()
     controller = PickUpDemo(env)
-    controller.pickup("yellow_box_geom")
+    controller.pickup("box_geom")
 
 
 if __name__ == "__main__":

python/rcsss/_core/hw/__init__.pyi

@@ -19,13 +19,14 @@ __all__ = [
     "FR3Load",
     "FR3State",
     "FrankaHand",
-    "IKController",
+    "IKSolver",
     "exceptions",
-    "internal",
-    "robotics_library",
+    "franka",
+    "rcs",
 ]
 
 class FHConfig(rcsss._core.common.GConfig):
+    async_control: bool
     epsilon_inner: float
     epsilon_outer: float
     force: float
@@ -36,8 +37,12 @@ class FHConfig(rcsss._core.common.GConfig):
 class FHState(rcsss._core.common.GState):
     def __init__(self) -> None: ...
     @property
+    def bool_state(self) -> bool: ...
+    @property
     def is_grasped(self) -> bool: ...
     @property
+    def is_moving(self) -> bool: ...
+    @property
     def last_commanded_width(self) -> float: ...
     @property
     def max_unnormalized_width(self) -> float: ...
@@ -49,20 +54,35 @@ class FHState(rcsss._core.common.GState):
 class FR3(rcsss._core.common.Robot):
     def __init__(self, ip: str, ik: rcsss._core.common.IK | None = None) -> None: ...
     def automatic_error_recovery(self) -> None: ...
+    def controller_set_joint_position(
+        self, desired_q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]
+    ) -> None: ...
     def double_tap_robot_to_continue(self) -> None: ...
     def get_parameters(self) -> FR3Config: ...
     def get_state(self) -> FR3State: ...
+    def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcsss._core.common.Pose) -> None: ...
     def set_cartesian_position_ik(
         self, pose: rcsss._core.common.Pose, max_time: float, elbow: float | None, max_force: float | None = 5
     ) -> None: ...
     def set_cartesian_position_internal(self, pose: rcsss._core.common.Pose) -> None: ...
     def set_default_robot_behavior(self) -> None: ...
-    def set_guiding_mode(self, enabled: bool) -> None: ...
+    def set_guiding_mode(
+        self,
+        x: bool = True,
+        y: bool = True,
+        z: bool = True,
+        roll: bool = True,
+        pitch: bool = True,
+        yaw: bool = True,
+        elbow: bool = True,
+    ) -> None: ...
     def set_parameters(self, cfg: FR3Config) -> bool: ...
+    def stop_control_thread(self) -> None: ...
+    def zero_torque_guiding(self) -> None: ...
 
 class FR3Config(rcsss._core.common.RConfig):
-    controller: IKController
-    guiding_mode_enabled: bool
+    async_control: bool
+    ik_solver: IKSolver
     load_parameters: FR3Load | None
     nominal_end_effector_frame: rcsss._core.common.Pose | None
     speed_factor: float
@@ -87,20 +107,20 @@ class FrankaHand(rcsss._core.common.Gripper):
     def is_grasped(self) -> bool: ...
     def set_parameters(self, cfg: FHConfig) -> bool: ...
 
-class IKController:
+class IKSolver:
     """
     Members:
 
-      internal
+      franka
 
-      robotics_library
+      rcs
     """
 
     __members__: typing.ClassVar[
-        dict[str, IKController]
-    ]  # value = {'internal': <IKController.internal: 0>, 'robotics_library': <IKController.robotics_library: 1>}
-    internal: typing.ClassVar[IKController]  # value = <IKController.internal: 0>
-    robotics_library: typing.ClassVar[IKController]  # value = <IKController.robotics_library: 1>
+        dict[str, IKSolver]
+    ]  # value = {'franka': <IKSolver.franka: 0>, 'rcs': <IKSolver.rcs: 1>}
+    franka: typing.ClassVar[IKSolver]  # value = <IKSolver.franka: 0>
+    rcs: typing.ClassVar[IKSolver]  # value = <IKSolver.rcs: 1>
     def __eq__(self, other: typing.Any) -> bool: ...
     def __getstate__(self) -> int: ...
     def __hash__(self) -> int: ...
@@ -116,5 +136,5 @@ class IKController:
     @property
     def value(self) -> int: ...
 
-internal: IKController  # value = <IKController.internal: 0>
-robotics_library: IKController  # value = <IKController.robotics_library: 1>
+franka: IKSolver  # value = <IKSolver.franka: 0>
+rcs: IKSolver  # value = <IKSolver.rcs: 1>

python/rcsss/camera/hw.py

@@ -14,6 +14,7 @@
     BaseCameraSetConfig,
     Frame,
     FrameSet,
+    SimpleFrameRate,
 )
 
 
@@ -40,6 +41,7 @@ def __init__(self):
         self._next_ring_index = 0
         self._buffer_len = 0
         self.writer: dict[str, cv2.VideoWriter] = {}
+        self.rate = SimpleFrameRate()
 
     def buffer_size(self) -> int:
         return len(self._buffer) - self._buffer.count(None)
@@ -91,33 +93,35 @@ def start(self, warm_up: bool = True):
         self._thread = threading.Thread(target=self.polling_thread, args=(warm_up,))
         self._thread.start()
 
-    def record_video(self, path: Path, episode: int):
+    def record_video(self, path: Path, str_id: str):
+        if self.recording_ongoing():
+            return
+        self.clear_buffer()
         for camera in self.camera_names:
             self.writer[camera] = cv2.VideoWriter(
-                str(path / f"episode_{episode}_{camera}.mp4"),
+                str(path / f"episode_{str_id}_{camera}.mp4"),
                 # migh require to install ffmpeg
                 cv2.VideoWriter_fourcc(*"mp4v"),  # type: ignore
                 self.config.frame_rate,
                 (self.config.resolution_width, self.config.resolution_height),
             )
 
+    def recording_ongoing(self) -> bool:
+        with self._buffer_lock:
+            return len(self.writer) > 0
+
     def stop_video(self):
         if len(self.writer) > 0:
-            for camera_key, writer in self.writer.items():
-                for i in range(self._next_ring_index):
-                    frameset = self._buffer[i]
-                    assert frameset is not None
-                    # rgb to bgr as expected by opencv
-                    writer.write(frameset.frames[camera_key].camera.color.data[:, :, ::-1])
-            for camera in self.camera_names:
-                self.writer[camera].release()
-            self.writer = {}
+            with self._buffer_lock:
+                for camera in self.camera_names:
+                    self.writer[camera].release()
+                self.writer = {}
 
     def warm_up(self):
         for _ in range(self.config.warm_up_disposal_frames):
             for camera_name in self.camera_names:
                 self._poll_frame(camera_name)
-            sleep(1 / self.config.frame_rate)
+            self.rate(self.config.frame_rate)
 
     def polling_thread(self, warm_up: bool = True):
         if warm_up:
@@ -126,16 +130,14 @@ def polling_thread(self, warm_up: bool = True):
             frame_set = self.poll_frame_set()
             with self._buffer_lock:
                 self._buffer[self._next_ring_index] = frame_set
+                # copy the buffer to the record path
+                for camera_key, writer in self.writer.items():
+                    frameset = self._buffer[self._next_ring_index]
+                    assert frameset is not None
+                    writer.write(frameset.frames[camera_key].camera.color.data[:, :, ::-1])
                 self._next_ring_index = (self._next_ring_index + 1) % self.config.max_buffer_frames
                 self._buffer_len = max(self._buffer_len + 1, self.config.max_buffer_frames)
-                if self._next_ring_index == 0:
-                    # copy the buffer to the record path
-                    for camera_key, writer in self.writer.items():
-                        for i in range(self.config.max_buffer_frames):
-                            frameset = self._buffer[i]
-                            assert frameset is not None
-                            writer.write(frameset.frames[camera_key].camera.color.data[:, :, ::-1])
-            sleep(1 / self.config.frame_rate)
+            self.rate(self.config.frame_rate)
 
     def poll_frame_set(self) -> FrameSet:
         """Gather frames over all available cameras."""

python/rcsss/camera/interface.py

@@ -1,9 +1,40 @@
+import logging
 from dataclasses import dataclass
 from datetime import datetime
+from time import sleep, time
 from typing import Any, Protocol
 
 from pydantic import BaseModel, Field
 
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+
+class SimpleFrameRate:
+    def __init__(self):
+        self.t = None
+        self._last_print = None
+
+    def reset(self):
+        self.t = None
+
+    def __call__(self, frame_rate: int | float):
+        if self.t is None:
+            self.t = time()
+            self._last_print = self.t
+            sleep(1 / frame_rate if isinstance(frame_rate, int) else frame_rate)
+            return
+        sleep_time = (
+            1 / frame_rate - (time() - self.t) if isinstance(frame_rate, int) else frame_rate - (time() - self.t)
+        )
+        if sleep_time > 0:
+            sleep(sleep_time)
+        if self._last_print is None or time() - self._last_print > 10:
+            self._last_print = time()
+            logger.info(f"FPS: {1 / (time() - self.t)}")
+
+        self.t = time()
+
 
 class BaseCameraConfig(BaseModel):
     identifier: str

python/rcsss/cli/main.py

@@ -1,16 +1,13 @@
 import logging
 from contextlib import ExitStack
-from pathlib import Path
 from time import sleep
 from typing import Annotated, Optional
 
 import pyrealsense2 as rs
 import rcsss
 import rcsss.control.fr3_desk
 import typer
-from PIL import Image
 from rcsss.camera.realsense import RealSenseCameraSet
-from rcsss.config import create_sample_config_yaml, read_config_yaml
 from rcsss.control.record import PoseList
 from rcsss.control.utils import load_creds_fr3_desk
 from rcsss.envs.factories import get_urdf_path
@@ -21,16 +18,6 @@
 main_app = typer.Typer(help="CLI tool for the Robot Control Stack (RCS).")
 
 
-@main_app.command()
-def sample_config(
-    path: Annotated[
-        str, typer.Option("-p", help="Path to where the default config file should be saved")
-    ] = "config.yaml",
-):
-    """Creates a sample yaml config file"""
-    create_sample_config_yaml(path)
-
-
 # REALSENSE CLI
 realsense_app = typer.Typer()
 main_app.add_typer(
@@ -53,52 +40,6 @@ def serials():
         logger.info("  %s: %s", device.product_line, device.serial)
 
 
-@realsense_app.command()
-def test(
-    path: Annotated[str, typer.Argument(help="Path to the config file")],
-):
-    """Tests all configured and connected realsense by saving the current picture."""
-    cfg = read_config_yaml(path)
-    assert cfg.hw.camera_type == "realsense", "Only realsense cameras are supported for this test."
-    assert cfg.hw.camera_config is not None, "Camera config is not set."
-    assert cfg.hw.camera_config.realsense_config is not None, "Realsense config is not set."
-    cs = RealSenseCameraSet(cfg.hw.camera_config.realsense_config)
-    cs.warm_up()
-    testdir = Path(cfg.hw.camera_config.realsense_config.record_path)
-    frame_set = cs.poll_frame_set()
-    for device_str in cfg.hw.camera_config.realsense_config.name_to_identifier:
-        assert device_str in frame_set.frames
-        frame = frame_set.frames[device_str]
-        if frame.camera.color is not None:
-            im = Image.fromarray(frame.camera.color.data, mode="RGB")
-            im.save(testdir / f"test_img_{device_str}.png")
-        if frame.camera.depth is not None:
-            im = Image.fromarray(frame.camera.depth.data)
-            im.save(testdir / f"test_depth_{device_str}.png")
-        if frame.camera.ir is not None:
-            im = Image.fromarray(frame.camera.ir.data)
-            im.save(testdir / f"test_ir_{device_str}.png")
-        if frame.imu is not None:
-            logger.info("IMU data: %s %s", frame.imu.accel, frame.imu.gyro)
-
-
-@realsense_app.command()
-def test_record(
-    path: Annotated[str, typer.Argument(help="Path to the config file")],
-    n_frames: Annotated[int, typer.Argument(help="Name of the camera that should be tested")] = 100,
-):
-    """Tests all configured and connected realsense by saving the current picture."""
-    cfg = read_config_yaml(path)
-    assert cfg.hw.camera_type == "realsense", "Only realsense cameras are supported for this test."
-    assert cfg.hw.camera_config is not None, "Camera config is not set."
-    assert cfg.hw.camera_config.realsense_config is not None, "Realsense config is not set."
-    cs = RealSenseCameraSet(cfg.hw.camera_config.realsense_config)
-    cs.start(warm_up=True)
-    while cs.buffer_size() < n_frames:
-        sleep(1 / cfg.hw.camera_config.realsense_config.frame_rate)
-    cs.stop()
-
-
 # FR3 CLI
 fr3_app = typer.Typer()
 main_app.add_typer(