fix(dual-franka): align RLinf v0.4 deployment and control diagnostics (v0.1)

docs/source-en/rst_source/real_world_robots/dual_franka.rst

@@ -182,6 +182,22 @@ RPent translates this robot-focused schema into the internal two-node RLinf
 cluster and environment objects. To use a different file, pass
 ``--robot-config /path/to/robot_config.yaml``.
 
+During ``rotate_delta``, the position target stays at the starting TCP position.
+On real hardware, compliance/impedance control can allow the actual pose to
+deviate from the target due to load, contact, or limited tracking accuracy.
+
+``rotation_hold.max_drift_m`` defaults to ``0.10`` m (10 cm) and bounds the
+Euclidean distance from the TCP position at the start of this rotation. It is
+separate from the position tolerance, ``move_tolerance_m`` (0.006 m). At or below
+the drift limit, after rotation ends, ``move_delta`` returns the TCP to its
+starting position only if the position error exceeds that tolerance. The call
+reports ``ok=True`` only if compensation succeeds (when needed) and the final
+position and angle both meet their tolerances; otherwise it reports ``False``.
+Above the limit, a check during the rotation loop or at its end raises an RPC
+error, ending the call without translation compensation.
+
+The robot's own protections can still trigger independently.
+
 Start the Two-node Ray Cluster
 ------------------------------
 

docs/source-zh/rst_source/real_world_robots/dual_franka.rst

@@ -156,6 +156,12 @@ RPent 将其与所选快照中的末端位姿组合；若位姿使用 ``left_bas
 
 RPent 会将该机器人配置转换成内部双节点 RLinf cluster 和环境对象。如需使用其他文件，请传入 ``--robot-config /path/to/robot_config.yaml``。
 
+``rotate_delta`` 旋转时，位置目标保持在本次旋转开始时的 TCP 位置。真实硬件采用柔顺/阻抗控制，实际位姿可能因负载、接触或有限的跟踪精度而偏离目标。
+
+``rotation_hold.max_drift_m`` 默认为 ``0.10`` m（10 cm），限制 TCP 相对本次旋转起始位置的欧氏距离；它与位置到位容差 ``move_tolerance_m`` （0.006 m）不同。漂移小于或等于上限时，旋转结束后仅在位置误差超过到位容差时复用 ``move_delta`` 回位。只有补偿成功（如需补偿），且最终位置和角度均达到各自容差，才返回 ``ok=True`` ，否则返回 ``False`` 。漂移超过上限时，旋转循环中或结束时的检查会抛出 RPC 错误，停止本次调用，不再继续平移补偿。
+
+机器人自身的保护仍可能独立触发。
+
 启动双节点 Ray 集群
 --------------------
 

robots/dual_franka/config/example.yaml

@@ -51,6 +51,10 @@ workspace:
     - [0.8, 0.8, 0.7, 3.1416, 3.1416, 3.1416]
     - [0.8, 0.8, 0.7, 3.1416, 3.1416, 3.1416]
 
+rotation_hold:
+  # Abort through the RPC error path if TCP drift from its start exceeds this limit.
+  max_drift_m: 0.10
+
 joint_health:
   # Tune these guard rails for the mounting, reset posture, tool, and task.
   thresholds:

robots/dual_franka/perception.py

@@ -349,7 +349,7 @@ def _back_project_camera_pixel(
         camera_alias=camera,
         raw_key=str(camera_config["raw_key"]),
     )
-    intr = meta.get("color_intrinsics") or {}
+    intr = meta.get("raw_color_intrinsics") or meta.get("color_intrinsics") or {}
     fx = float(intr["fx"])
     fy = float(intr["fy"])
     cx = float(intr.get("ppx", intr.get("cx")))
@@ -656,7 +656,9 @@ def _camera_meta(
         aliases.append("extra_0")
     for key in aliases:
         value = meta.get(key)
-        if isinstance(value, dict) and value.get("color_intrinsics"):
+        if isinstance(value, dict) and (
+            value.get("raw_color_intrinsics") or value.get("color_intrinsics")
+        ):
             return value
     raise ValueError(f"{camera_alias} RealSense color intrinsics not found")
 
@@ -864,7 +866,7 @@ def _mask_to_camera_world(
         camera_alias=camera,
         raw_key=str(camera_config["raw_key"]),
     )
-    intr = meta.get("color_intrinsics") or {}
+    intr = meta.get("raw_color_intrinsics") or meta.get("color_intrinsics") or {}
     fx = float(intr["fx"])
     fy = float(intr["fy"])
     cx = float(intr.get("ppx", intr.get("cx")))

robots/dual_franka/rpent_env.py

@@ -0,0 +1,206 @@
+# Copyright 2026 The RPent Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""RPent observations on top of the official RLinf dual-Franka environment."""
+
+from __future__ import annotations
+
+import copy
+import queue
+from dataclasses import replace
+from typing import Any
+
+import cv2
+import gymnasium as gym
+import numpy as np
+from gymnasium.envs.registration import register
+from rlinf.envs.real.franka.dual_franka_tcp import DualFrankaTCPEnv
+from rlinf.envs.real.wrappers import build_stack
+from rlinf.robotics.parts.cameras import Camera, CameraInfo, RealSenseCamera
+
+from robots.franka.rpent_env import realsense_color_intrinsics
+
+_CAMERA_FRAME_TIMEOUT_S = 0.5
+_REALSENSE_BACKEND = "rpent_realsense"
+
+
+@Camera.register(_REALSENSE_BACKEND)
+class RPentRealSenseCamera(RealSenseCamera):
+    """Expose projection metadata from the camera's existing SDK connection."""
+
+    def get_color_intrinsics(self) -> dict[str, Any]:
+        """Read serializable intrinsics on the node that owns the camera."""
+        return realsense_color_intrinsics(self)
+
+
+class RPentDualFrankaEnv(DualFrankaTCPEnv):
+    """Preserve policy observations and retain their uncropped RGB-D for RPent."""
+
+    def __init__(self, *args: Any, **kwargs: Any) -> None:
+        self._rpent_camera_snapshot: dict[str, dict[str, np.ndarray]] | None = None
+        super().__init__(*args, **kwargs)
+
+    def _camera_infos(self) -> list[CameraInfo]:
+        # A public method on this driver is also forwarded by RLinf's remote
+        # camera handle; SDK profile objects themselves cannot cross that boundary.
+        return [
+            replace(info, camera_type=_REALSENSE_BACKEND)
+            if info.camera_type == "realsense"
+            else info
+            for info in super()._camera_infos()
+        ]
+
+    def _get_camera_observation(
+        self,
+    ) -> tuple[dict[str, np.ndarray], dict[str, np.ndarray]]:
+        """Collect once for policy and perception, invalidating stale raw data."""
+        self._rpent_camera_snapshot = None
+        frames: dict[str, np.ndarray] = {}
+        depths: dict[str, np.ndarray] = {}
+        raw_frames: dict[str, np.ndarray] = {}
+        raw_depths: dict[str, np.ndarray] = {}
+        display_frames: dict[str, np.ndarray] = {}
+        all_fresh = True
+
+        for name, camera in self._cameras.items():
+            try:
+                reading = camera.get_observation(
+                    timeout=_CAMERA_FRAME_TIMEOUT_S,
+                    max_age=_CAMERA_FRAME_TIMEOUT_S,
+                )
+            except queue.Empty:
+                # Preserve RLinf's policy fallback during camera recovery, but
+                # never present its old image as a fresh projection observation.
+                cached = self._last_camera_frame.get(name)
+                if cached is None:
+                    raise RuntimeError(
+                        f"Camera {name} stalled with no cached frame to fall back to."
+                    ) from None
+                self._logger.error("Camera %s stalled; using the last frame.", name)
+                reading = cached
+                all_fresh = False
+
+            raw_frames[name] = reading["frame"][..., ::-1].copy()
+            reshape_size = self.observation_space["frames"][name].shape[:2][::-1]
+            cropped, resized = self._crop_frame(reading["frame"], reshape_size)
+            frames[name] = resized[..., ::-1]
+            display_frames[name] = resized
+            display_frames[f"{name}_full"] = cropped
+            if "depth" in reading:
+                raw_depths[name] = reading["depth"].copy()
+                _, depths[name] = self._crop_frame(
+                    reading["depth"], reshape_size, interpolation=cv2.INTER_NEAREST
+                )
+            self._last_camera_frame[name] = reading
+
+        self.camera_player.put_frame(display_frames)
+        if all_fresh:
+            self._rpent_camera_snapshot = {
+                "raw_frames": raw_frames,
+                "raw_depths": raw_depths,
+            }
+        return frames, depths
+
+    def get_raw_camera_metadata(self) -> dict[str, dict[str, Any]]:
+        """Describe the complete, uncropped snapshot and supported calibration."""
+        if self.config.is_dummy:
+            return {}
+        snapshot = self._rpent_camera_snapshot
+        if snapshot is None:
+            raise RuntimeError(
+                "No fresh complete camera snapshot; acquire an observation first."
+            )
+        metadata = {}
+        for name, camera in self._cameras.items():
+            info = camera.camera_info
+            depth = snapshot["raw_depths"].get(name)
+            intrinsics = (
+                camera.get_color_intrinsics()
+                if isinstance(camera, RPentRealSenseCamera)
+                else None
+            )
+            frame = snapshot["raw_frames"][name]
+            height, width = frame.shape[:2]
+            metadata[name] = {
+                "name": name,
+                "serial_number": info.serial_number,
+                "camera_type": "realsense"
+                if info.camera_type == _REALSENSE_BACKEND
+                else info.camera_type,
+                "raw_resolution": [width, height],
+                "output_resolution": [width, height],
+                "crop_bounds_xyxy": [0, 0, width, height],
+                "crop_region": None,
+                "depth_scale": float(camera.depth_scale),
+                "depth_unit": "m" if depth is not None else None,
+                "depth_aligned_to_color": bool(info.enable_depth),
+                "extrinsic_cam2base": None,
+                "extrinsic_cam2ee": None,
+                "raw_color_intrinsics": intrinsics,
+                "intrinsic_K": (
+                    [
+                        [intrinsics["fx"], 0.0, intrinsics["ppx"]],
+                        [0.0, intrinsics["fy"], intrinsics["ppy"]],
+                        [0.0, 0.0, 1.0],
+                    ]
+                    if intrinsics is not None
+                    else None
+                ),
+                "depth_enabled": bool(info.enable_depth),
+                "depth_available": depth is not None,
+                "projection_supported": depth is not None and intrinsics is not None,
+                "rgb_shape": list(frame.shape),
+                "depth_shape": None if depth is None else list(depth.shape),
+            }
+        return metadata
+
+    def get_raw_camera_snapshot(self) -> dict[str, Any]:
+        """Copy the preceding acquisition, or return empty physical views in dummy mode."""
+        if self.config.is_dummy:
+            return {"raw_frames": {}, "raw_depths": {}, "camera_meta": {}}
+        metadata = self.get_raw_camera_metadata()
+        return {
+            **copy.deepcopy(self._rpent_camera_snapshot),
+            "camera_meta": metadata,
+        }
+
+
+def create_rpent_dual_franka_env(
+    override_cfg: dict,
+    worker_info: object,
+    robot_info: object,
+    env_idx: int,
+    env_cfg: dict,
+) -> gym.Env:
+    """Create the dual-Franka adapter with RLinf's standard wrapper stack."""
+    env = RPentDualFrankaEnv(
+        override_cfg=override_cfg,
+        worker_info=worker_info,
+        robot_info=robot_info,
+        env_idx=env_idx,
+    )
+    try:
+        return build_stack(env, env_cfg)
+    except BaseException:
+        env.close()
+        raise
+
+
+def register_rpent_dual_franka_env() -> None:
+    """Register the RPent dual-Franka adapter with Gymnasium."""
+    if "RPentDualFrankaTCPEnv-v1" not in gym.registry:
+        register(
+            id="RPentDualFrankaTCPEnv-v1",
+            entry_point="robots.dual_franka.rpent_env:create_rpent_dual_franka_env",
+        )

robots/dual_franka/runtime_config.py

@@ -191,6 +191,7 @@ def load_runtime_config(
             "left_gripper_connection": left_gripper.get("connection"),
             "right_gripper_connection": right_gripper.get("connection"),
             "realtime_config": robot.get("realtime_config"),
+            "compliance": robot.get("compliance"),
             "left_controller_node_rank": LEFT_CONTROLLER_NODE,
             "right_controller_node_rank": RIGHT_CONTROLLER_NODE,
             "node_rank": HARDWARE_NODE,
@@ -241,13 +242,17 @@ def load_runtime_config(
                     "keyboard_reward_wrapper": None,
                     "use_relative_frame": False,
                     "video_cfg": {},
-                    "init_params": {"id": "DualFrankaTCPEnv-v1"},
+                    "init_params": {"id": "RPentDualFrankaTCPEnv-v1"},
                     "override_cfg": override_cfg,
                 }
             },
         }
     )
     controller = flatten_control(CONTROL)
+    rotation_hold = _require_mapping(raw.get("rotation_hold", {}), "rotation_hold")
+    # max_drift_m must be finite and > 0 (m); exceeding it aborts rotation.
+    max_drift = float(rotation_hold.get("max_drift_m", 0.10))
+    controller["rotate_max_drift_m"] = max_drift
     controller["robot_config_path"] = str(
         Path(path or DEFAULT_CONFIG).expanduser().resolve()
     )