Merge pull request #209 from RobotControlStack/feat/calibration-cache

feat: calibration cache for realsense

Author: juelg

extensions/rcs_realsense/pyproject.toml

@@ -10,6 +10,7 @@ dependencies = [
     "rcs==0.4.0",
     "pyrealsense2~=2.55.1",
     "apriltag==0.0.16",
+    "diskcache",
 ]
 readme = "README.md"
 maintainers = [

extensions/rcs_realsense/src/rcs_realsense/calibration.py

@@ -1,10 +1,12 @@
 import logging
 import threading
 import typing
+from pathlib import Path
 from time import sleep
 
 import apriltag
 import cv2
+import diskcache as dc
 import numpy as np
 from rcs._core import common
 from rcs.camera.hw import CalibrationStrategy
@@ -19,7 +21,10 @@ class FR3BaseArucoCalibration(CalibrationStrategy):
 
     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._cache = dc.Cache(Path.home() / ".cache" / "rcs")
+        self._extrinsics: np.ndarray[tuple[typing.Literal[4], typing.Literal[4]], np.dtype[np.float64]] | None = (
+            self._cache.get(f"{camera_name}_extrinsics")
+        )  # 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])
@@ -53,6 +58,7 @@ def calibrate(
         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
+        self._cache.set(f"{self.camera_name}_extrinsics", world_to_cam, expire=3600)
         return True
 
     def get_extrinsics(self) -> np.ndarray[tuple[typing.Literal[4], typing.Literal[4]], np.dtype[np.float64]] | None:

python/rcs/envs/base.py

@@ -693,15 +693,15 @@ def __init__(self, env, gripper: common.Gripper, binary: bool = True, open_on_re
         self.action_space: gym.spaces.Dict
         self.action_space.spaces.update(get_space(GripperDictType).spaces)
         self.gripper_key = get_space_keys(GripperDictType)[0]
-        self._gripper = gripper
+        self.gripper = gripper
         self.binary = binary
         self._last_gripper_cmd = None
         self.open_on_reset = open_on_reset
 
     def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]:
         if self.open_on_reset:
             # resetting opens the gripper
-            self._gripper.reset()
+            self.gripper.reset()
         self._last_gripper_cmd = None
         return super().reset(**kwargs)
 
@@ -712,7 +712,7 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl
                 self._last_gripper_cmd if self._last_gripper_cmd is not None else self.BINARY_GRIPPER_OPEN
             )
         else:
-            observation[self.gripper_key] = self._gripper.get_normalized_width()
+            observation[self.gripper_key] = self.gripper.get_normalized_width()
 
         return observation, info
 
@@ -725,9 +725,9 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
         gripper_action = np.clip(gripper_action, 0.0, 1.0)
 
         if self.binary:
-            self._gripper.grasp() if gripper_action == self.BINARY_GRIPPER_CLOSED else self._gripper.open()
+            self.gripper.grasp() if gripper_action == self.BINARY_GRIPPER_CLOSED else self.gripper.open()
         else:
-            self._gripper.set_normalized_width(gripper_action)
+            self.gripper.set_normalized_width(gripper_action)
         self._last_gripper_cmd = gripper_action
         del action[self.gripper_key]
         return action

python/rcs/envs/utils.py

@@ -94,8 +94,8 @@ def get_tcp_offset(mjcf: str | Path) -> rcs.common.Pose:
         tcp_offset_translation = np.array(model.numeric("tcp_offset_translation").data)
         tcp_offset_rotation_matrix = np.array(model.numeric("tcp_offset_rotation_matrix").data)
         return rcs.common.Pose(
-            translation=tcp_offset_translation, rotation=tcp_offset_rotation_matrix.reshape((3, 3))
-        )  #  type: ignore
+            translation=tcp_offset_translation, rotation=tcp_offset_rotation_matrix.reshape((3, 3))  #  type: ignore
+        )
     except KeyError:
         msg = "No tcp offset found in the model. Using the default tcp offset."
         logging.info(msg)