Async Operational Space Controller

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(