feat(camera): xtrinsics and callibration support

README.md

@@ -30,6 +30,7 @@ import rcs
 from rcs import sim
 from rcs._core.sim import CameraType
 from rcs.camera.sim import SimCameraConfig, SimCameraSet
+from time import sleep
 simulation = sim.Sim(rcs.scenes["fr3_empty_world"]["mjb"])
 urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
 ik = rcs.common.RL(str(urdf_path))
@@ -42,23 +43,18 @@ gripper_cfg_sim = sim.SimGripperConfig()
 gripper_cfg_sim.add_id("0")
 gripper = sim.SimGripper(simulation, gripper_cfg_sim)
 
-# add camera to have a rendering gui
-cameras = {
-    "wrist": SimCameraConfig(
-        identifier="wrist_0",
-        type=CameraType.fixed,
-        resolution_width=640,
-        resolution_height=480,
-        frame_rate=30,
-    ),
-}
-camera_set = SimCameraSet(simulation, cameras)
+camera_set = SimCameraSet(simulation, {})
 simulation.open_gui()
+# wait for gui
+sleep(5)
+# step the robot 10 cm in x direction
 robot.set_cartesian_position(
-    robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.05, 0, 0]))
+    robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.1, 0, 0]))
 )
+# close gripper
 gripper.grasp()
 simulation.step_until_convergence()
+input("press enter to close")
 ```
 ### Gym Env Interface
 ```python
@@ -80,7 +76,7 @@ env_rel = SimEnvCreator()(
 )
 env_rel.get_wrapper_attr("sim").open_gui()
 
-for _ in range(10):
+for _ in range(100):
     obs, info = env_rel.reset()
     for _ in range(10):
         # sample random relative action and execute it
@@ -110,8 +106,8 @@ For more details real the readme file of the respective extension.
 After the required hardware extensions are installed the examples also above work on real hardware:
 Switch to hardware by setting the following flag:
 ```python
-ROBOT_INSTANCE = RobotPlatform.SIMULATION
-# ROBOT_INSTANCE = RobotPlatform.HARDWARE
+# ROBOT_INSTANCE = RobotPlatform.SIMULATION
+ROBOT_INSTANCE = RobotPlatform.HARDWARE
 ```
 
 #### Command Line Interface

extensions/rcs_realsense/pyproject.toml

@@ -9,6 +9,7 @@ description="RCS realsense module"
 dependencies = [
     "rcs==0.4.0",
     "pyrealsense2~=2.55.1",
+    "apriltag==0.0.16",
 ]
 readme = "README.md"
 maintainers = [

extensions/rcs_realsense/src/rcs_realsense/calibration.py

@@ -0,0 +1,151 @@
+import logging
+import threading
+import typing
+from time import sleep
+
+import apriltag
+import cv2
+import numpy as np
+from rcs._core import common
+from rcs.camera.hw import CalibrationStrategy
+from rcs.camera.interface import Frame
+from tqdm import tqdm
+
+logger = logging.getLogger(__name__)
+
+
+class FR3BaseArucoCalibration(CalibrationStrategy):
+    """Calibration with a 3D printed aruco marker that fits around the vention's FR3 base mounting plate."""
+
+    def __init__(self, camera_name: str):
+        # base frame to camera, world to base frame
+        self._extrinsics: np.ndarray[tuple[typing.Literal[4], typing.Literal[4]], np.dtype[np.float64]] | None = None
+        self.camera_name = camera_name
+        self.tag_to_world = common.Pose(
+            rpy_vector=np.array([np.pi, 0, -np.pi / 2]), translation=np.array([0.145, 0, 0])
+        ).pose_matrix()
+
+    def calibrate(
+        self,
+        samples: list[Frame],
+        intrinsics: np.ndarray[tuple[typing.Literal[3], typing.Literal[4]], np.dtype[np.float64]],
+        lock: threading.Lock,
+    ) -> bool:
+        logger.info("Calibrating camera %s. Position it as you wish and press enter.", self.camera_name)
+        input()
+        tries = 3
+        while len(samples) < 10 and tries > 0:
+            logger.info("not enought frames in recorded, waiting 2 seconds...")
+            tries = -1
+            sleep(2)
+        if tries == 0:
+            logger.warning("Calibration failed, not enough frames arrived.")
+            return False
+
+        frames = []
+        with lock:
+            for sample in samples:
+                frames.append(sample.camera.color.data.copy())
+        print(frames)
+
+        _, tag_to_cam = get_average_marker_pose(frames, intrinsics=intrinsics, calib_tag_id=9, show_live_window=False)
+
+        cam_to_world = self.tag_to_world @ np.linalg.inv(tag_to_cam)
+        world_to_cam = np.linalg.inv(cam_to_world)
+        self._extrinsics = world_to_cam  # type: ignore
+        return True
+
+    def get_extrinsics(self) -> np.ndarray[tuple[typing.Literal[4], typing.Literal[4]], np.dtype[np.float64]] | None:
+        return self._extrinsics
+
+
+def get_average_marker_pose(
+    samples,
+    intrinsics,
+    calib_tag_id,
+    show_live_window,
+):
+    # create detector
+    options = apriltag.DetectorOptions(families="tag25h9")
+    detector = apriltag.Detector(options=options)
+
+    # make while loop with tqdm
+    poses = []
+
+    last_frame = None
+    for frame in tqdm(samples):
+
+        # detect tags
+        marker_det, pose = get_marker_pose(calib_tag_id, detector, intrinsics, frame)
+
+        if marker_det is None:
+            continue
+
+        for corner in marker_det.corners:
+            cv2.circle(frame, tuple(corner.astype(int)), 5, (0, 0, 255), -1)
+
+        poses.append(pose)
+
+        last_frame = frame.copy()
+
+        camera_matrix = intrinsics[:3, :3]
+
+        if show_live_window:
+            cv2.drawFrameAxes(frame, camera_matrix, None, pose[:3, :3], pose[:3, 3], 0.1)  # type: ignore
+            # show frame
+            cv2.imshow("frame", frame)
+
+            # wait for key press
+            if cv2.waitKey(1) & 0xFF == ord("q"):
+                break
+    if last_frame is None:
+        msg = "No frames were processed, cannot calculate average pose. Check if the tag is visible."
+        raise ValueError(msg)
+
+    if show_live_window:
+        cv2.destroyAllWindows()
+
+    # calculate the average marker pose
+    avg_pose = np.mean(poses, axis=0)
+    logger.info(f"Average pose: {avg_pose}")
+
+    return last_frame, avg_pose
+
+
+def get_marker_pose(calib_tag_id, detector, intrinsics, frame):
+    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+    detections = detector.detect(gray)
+
+    # count detections
+    n_det = 0
+    marker_det = None
+    for det in detections:
+        if det.tag_id != calib_tag_id:
+            continue
+        n_det += 1
+        marker_det = det
+
+    if n_det > 1:
+        msg = f"Expected 1 detection of tag id {calib_tag_id}, got {n_det}."
+        raise ValueError(msg)
+
+    if marker_det is None:
+        return None, None
+
+    fx = intrinsics[0, 0]
+    fy = intrinsics[1, 1]
+    cx = intrinsics[0, 2]
+    cy = intrinsics[1, 2]
+
+    pose, _, _ = detector.detection_pose(
+        marker_det,
+        camera_params=(
+            fx,
+            fy,
+            cx,
+            cy,
+        ),
+        tag_size=0.1,
+    )
+
+    return marker_det, pose