diff --git a/extensions/rcs_realsense/pyproject.toml b/extensions/rcs_realsense/pyproject.toml index e44eba50..73dc1e98 100644 --- a/extensions/rcs_realsense/pyproject.toml +++ b/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 = [ diff --git a/extensions/rcs_realsense/src/rcs_realsense/calibration.py b/extensions/rcs_realsense/src/rcs_realsense/calibration.py index 937450c5..f532a2ba 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/calibration.py +++ b/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: diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 4eee1be9..50c17c91 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -693,7 +693,7 @@ 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 @@ -701,7 +701,7 @@ def __init__(self, env, gripper: common.Gripper, binary: bool = True, open_on_re 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 diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index eea965c9..0287e3ae 100644 --- a/python/rcs/envs/utils.py +++ b/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)