feat: Implement absolute scheduling for feedback loop and enhance tool frame configuration

docs/superpowers/plans/2026-07-28-rm75-feedback-absolute-scheduling.md

@@ -0,0 +1,109 @@
+# RM75 Feedback Absolute Scheduling Implementation Plan
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** 将关节反馈线程从“读取后固定等待 8 ms”改为无历史周期补跑的绝对起始周期调度。
+
+**Architecture:** `RealManAdapter._feedback_loop()` 保留现有读取、告警和停止结构，只把固定 `Event.wait(feedback_period)` 替换为下一截止时间计算。读取提前完成时等待剩余时间；读取超期时重置调度基准并立即进入下一周期。
+
+**Tech Stack:** Python 3.10、threading、time.monotonic、pytest、ROS2 Humble、colcon
+
+---
+
+### Task 1: 反馈绝对周期调度
+
+**Files:**
+- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
+- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
+
+- [ ] **Step 1: 写失败测试**
+
+用确定性的 FakeTime 和 FakeStopEvent 运行 `_feedback_loop()` 三次读取：
+
+- 第一次读取在 5 ms 完成，应只等待剩余 3 ms；
+- 第二次在 18 ms 完成，超过 16 ms 截止时间，应不等待并把基准重置为
+  18 ms；
+- 第三次在 23 ms 完成，应等待到新基准的 26 ms，即再次等待 3 ms。
+
+断言读取三次且 `wait()` 参数为 `[0.003, 0.003]`。该结果同时证明没有补跑
+旧的 8 ms 和 16 ms 截止点。
+
+- [ ] **Step 2: 确认测试失败**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
+```
+
+预期：当前固定等待实现记录 `[0.008, 0.008, 0.008]`，测试失败。
+
+- [ ] **Step 3: 写最小实现**
+
+把 `_feedback_loop()` 改为：
+
+```python
+def _feedback_loop(self) -> None:
+    next_read_at = time.monotonic()
+    while not self._feedback_stop.is_set():
+        try:
+            self._read_joint_state_once()
+            self._feedback_fault_logged = False
+        except Exception as exc:
+            if not self._feedback_fault_logged:
+                self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
+                self._feedback_fault_logged = True
+
+        next_read_at += self._feedback_period
+        remaining = next_read_at - time.monotonic()
+        if remaining <= 0.0:
+            next_read_at = time.monotonic()
+            continue
+        self._feedback_stop.wait(remaining)
+```
+
+- [ ] **Step 4: 确认局部测试通过**
+
+重复 Step 2 命令。预期：全部 PASS。
+
+### Task 2: 调度回归验证
+
+**Files:**
+- Verify: `xr_rm_teleop`
+- Verify: ROS2 workspace
+
+- [ ] **Step 1: 运行遥操作包测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test
+```
+
+预期：全部 PASS。
+
+- [ ] **Step 2: 运行姿态控制指定测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
+```
+
+预期：全部 PASS。
+
+- [ ] **Step 3: 构建工作空间**
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：四个包构建成功。
+
+- [ ] **Step 4: 检查最终差异**
+
+```bash
+git diff --check
+git status --short
+```
+
+预期：只包含已确认的统计、工具坐标系幂等修复、反馈绝对周期调度、对应测试
+及 Superpowers 文档。按仓库规则不自动提交。

docs/superpowers/plans/2026-07-28-rm75-feedback-thread-timing.md

@@ -0,0 +1,167 @@
+# RM75 Feedback Thread Timing Implementation Plan
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** 在不改变反馈轮询和机械臂控制行为的前提下，统计 `rm_get_joint_degree()` 调用耗时与成功反馈更新间隔。
+
+**Architecture:** `RealManAdapter` 在反馈读取边界测量时间，并把可选计时值随 `JointStateSnapshot` 放入现有缓存。`SingleArmVelocityTeleop` 复用现有 timing 窗口，只对新的反馈时间戳记录一次并输出汇总。
+
+**Tech Stack:** Python 3.10、ROS2 Humble、pytest、NumPy、colcon
+
+---
+
+### Task 1: 在反馈缓存中携带真实读取计时
+
+**Files:**
+- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
+- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
+
+- [ ] **Step 1: 写失败测试**
+
+在 `test_joint_feedback_is_cached_in_radians` 中通过 `monkeypatch` 固定
+`perf_counter_ns()` 和 `monotonic()`，连续读取两次，并验证：
+
+```python
+assert first.read_duration_ms == pytest.approx(2.0)
+assert first.update_interval_ms is None
+assert second.read_duration_ms == pytest.approx(3.0)
+assert second.update_interval_ms == pytest.approx(11.0)
+```
+
+- [ ] **Step 2: 确认测试失败**
+
+在 `/home/robot/WS_xr` 执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py::test_joint_feedback_is_cached_in_radians
+```
+
+预期：因 `JointStateSnapshot` 尚无计时字段而失败。
+
+- [ ] **Step 3: 最小实现反馈计时**
+
+给快照增加可选字段，保持现有两参数构造兼容：
+
+```python
+@dataclass(frozen=True)
+class JointStateSnapshot:
+    positions: list[float]
+    received_at: float
+    read_duration_ms: float | None = None
+    update_interval_ms: float | None = None
+```
+
+在 `_read_joint_state_once()` 中只包围 SDK 调用测量 `read_duration_ms`；数据校验
+成功后取得 `received_at`，并在缓存锁内根据上一快照计算
+`update_interval_ms`。`get_latest_joint_state()` 同步复制两个字段。Mock 使用
+字段默认值，不伪造计时。
+
+- [ ] **Step 4: 确认局部测试通过**
+
+重复 Step 2 命令。预期：PASS。
+
+### Task 2: 将唯一反馈样本加入现有 timing 汇总
+
+**Files:**
+- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+- Test: `xr_rm_teleop/test/test_joint_control.py`
+
+- [ ] **Step 1: 写失败测试**
+
+扩展 `test_timing_stats_logs_summary_and_clears_window`：在三个控制样本中传入
+“快照 A、重复快照 A、快照 B”，并断言日志包含：
+
+```python
+assert "feedback_read[n=2" in messages[0]
+assert "feedback_interval[n=1" in messages[0]
+```
+
+这样同时验证新反馈只计一次、重复缓存不重复计数、首次反馈无更新间隔。
+
+- [ ] **Step 2: 确认测试失败**
+
+在 `/home/robot/WS_xr` 执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test/test_joint_control.py::test_timing_stats_logs_summary_and_clears_window
+```
+
+预期：因 timing 样本尚不支持新字段而失败。
+
+- [ ] **Step 3: 最小实现唯一反馈统计**
+
+在节点初始化时：
+
+```python
+self._timing_samples = {
+    name: []
+    for name in (
+        "period",
+        "total",
+        "qp",
+        "send",
+        "feedback_age",
+        "feedback_read",
+        "feedback_interval",
+    )
+}
+self._last_timing_feedback_received_at: float | None = None
+```
+
+让 `_record_timing_sample()` 接收当前 `JointStateSnapshot`。仅当
+`received_at` 与 `_last_timing_feedback_received_at` 不同时，追加非 `None`
+的读取耗时和更新间隔。汇总时仅输出非空的新数组，避免 mock 模式对空数组
+求百分位数。控制循环把已有 `snapshot` 传入该函数。
+
+- [ ] **Step 4: 确认相关测试通过**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test/test_joint_control.py src/xr_rm_teleop/test/test_initial_joint_pose.py
+```
+
+预期：全部 PASS。
+
+### Task 3: 回归验证
+
+**Files:**
+- Verify: `xr_rm_teleop`
+- Verify: ROS2 workspace
+
+- [ ] **Step 1: 运行遥操作包测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test
+```
+
+预期：全部 PASS。
+
+- [ ] **Step 2: 运行姿态控制指定测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
+```
+
+预期：全部 PASS。
+
+- [ ] **Step 3: 构建工作空间**
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：四个包构建成功。
+
+- [ ] **Step 4: 检查差异**
+
+```bash
+git diff --check
+git status --short
+```
+
+预期：只包含设计、计划、反馈计时实现及相关测试。按仓库规则不自动提交。

docs/superpowers/plans/2026-07-28-rm75-idempotent-tool-frame.md

@@ -0,0 +1,107 @@
+# RM75 Idempotent Tool Frame Implementation Plan
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** 让 RealMan 工具坐标系在首次启动时创建、后续启动时更新，并检查所有相关 SDK 返回值。
+
+**Architecture:** `fun_peripheral.py` 增加一个只负责工具坐标系的内部函数，先查询名称列表，再选择创建或更新，最后切换。现有 `peripheral_cfg()` 继续负责 IO 和夹爪初始化，只把原来的两次无检查调用替换为该函数。
+
+**Tech Stack:** Python 3.10、RealMan Python API2、pytest、ROS2 Humble、colcon
+
+---
+
+### Task 1: 工具坐标系幂等配置
+
+**Files:**
+- Modify: `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`
+- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
+
+- [ ] **Step 1: 写失败测试**
+
+导入新的 `_configure_tool_frame`，用 FakeArm 分别返回包含和不包含 `omnipic`
+的名称列表。断言不存在时调用 `rm_set_manual_tool_frame`，存在时调用
+`rm_update_tool_frame`，两条路径最后都调用 `rm_change_tool_frame`。
+
+再用参数化失败返回码验证查询、创建/更新和切换失败均抛出包含 SDK 操作名称
+的 `RuntimeError`。
+
+- [ ] **Step 2: 确认测试失败**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
+```
+
+预期：因 `_configure_tool_frame` 尚不存在而在测试收集阶段失败。
+
+- [ ] **Step 3: 写最小实现**
+
+在 `fun_peripheral.py` 增加：
+
+```python
+def _check_sdk_return(result: Any, operation: str) -> None:
+    if result != 0:
+        raise RuntimeError(f"{operation} failed with code {result}: {result!r}")
+
+
+def _configure_tool_frame(robot, tool_frame, tool_name: str) -> None:
+    frames = robot.rm_get_total_tool_frame()
+    if not isinstance(frames, dict):
+        raise RuntimeError(
+            f"rm_get_total_tool_frame returned invalid data: {frames!r}"
+        )
+    _check_sdk_return(
+        frames.get("return_code"),
+        "rm_get_total_tool_frame",
+    )
+    tool_names = frames.get("tool_names")
+    if not isinstance(tool_names, (list, tuple)):
+        raise RuntimeError(
+            f"rm_get_total_tool_frame returned invalid tool_names: {tool_names!r}"
+        )
+
+    if tool_name in tool_names:
+        operation = "rm_update_tool_frame"
+        result = robot.rm_update_tool_frame(frame=tool_frame)
+    else:
+        operation = "rm_set_manual_tool_frame"
+        result = robot.rm_set_manual_tool_frame(frame=tool_frame)
+    _check_sdk_return(result, operation)
+    _check_sdk_return(
+        robot.rm_change_tool_frame(tool_name),
+        "rm_change_tool_frame",
+    )
+```
+
+在 `peripheral_cfg()` 中用
+`_configure_tool_frame(robot, tool_frame, tool_name)` 替换原来的创建和切换
+调用。
+
+- [ ] **Step 4: 确认测试通过**
+
+重复 Step 2 命令。预期：全部 PASS。
+
+### Task 2: 工具修复回归验证
+
+**Files:**
+- Verify: `xr_rm_teleop`
+- Verify: ROS2 workspace
+
+- [ ] **Step 1: 运行遥操作包测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test
+```
+
+预期：全部 PASS。
+
+- [ ] **Step 2: 构建工作空间**
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：四个包构建成功。
+

docs/superpowers/specs/2026-07-28-rm75-feedback-scheduling-follow-speed-design.md

@@ -0,0 +1,131 @@
+# RM75 反馈调度与跟随速度优化设计
+
+## 目标
+
+在保留 Placo 单步 QP、工作空间与圆柱限位、关节速度限制、指令超时和安全
+停止的前提下，分阶段解决 PICO 遥操机械臂跟随速度很慢的问题。
+
+每阶段只改变一个控制因素，真机验证通过后才进入下一阶段：
+
+1. 提高关节反馈的新鲜度；
+2. 启用 `rm_movej_canfd` 高跟随完全透传；
+3. 将右臂单独调试 TCP 速度提高到 `0.2 m/s`。
+
+## 真机证据
+
+右臂在 125 Hz、低跟随模式下连续四个约 5 秒窗口的结果为：
+
+- `period mean=8.000 ms`，四个窗口最大值为 `9.023–9.424 ms`；
+- `total mean=1.908–2.004 ms`，最大值不超过 `4.109 ms`；
+- `qp mean=0.230–0.245 ms`；
+- `send mean=0.122–0.127 ms`；
+- `feedback_read mean=9.101–9.630 ms`；
+- `feedback_interval mean=17.368–17.918 ms`；
+- `feedback_age mean=9.079–9.632 ms`，最差达到 `46.480 ms`。
+
+每个窗口只有 `279–288` 次新反馈，即实际反馈频率约为 `56–58 Hz`。
+`feedback_interval - feedback_read` 在四个窗口中稳定为 `8.27–8.39 ms`，
+确认当前反馈线程把一次 SDK 查询耗时和完整的 8 ms 等待串联起来：
+
+```text
+当前更新间隔 = rm_get_joint_degree 调用耗时 + 8 ms 固定等待
+```
+
+控制回调、QP 和发送均有充足余量，不是当前反馈慢的原因。
+
+## 阶段一：反馈线程绝对周期调度
+
+### 调度语义
+
+将当前“读取完成后固定等待 8 ms”改为“读取起始时间之间以 8 ms 为目标”：
+
+```text
+读取耗时 < 8 ms：只等待剩余时间
+读取耗时 ≥ 8 ms：不再额外等待，从当前时间重新建立周期基准
+```
+
+调度不补跑已经错过的历史周期。一次长阻塞结束后最多立即开始下一次读取，
+不会为了追赶多个旧截止点而密集补调用 SDK。
+
+等价的目标启动间隔为：
+
+```text
+max(8 ms, 本次反馈读取耗时)
+```
+
+当前读取平均约 9.3 ms，因此预期反馈频率接近 `100 Hz`，但不强求达到
+`125 Hz`。
+
+### 范围
+
+本阶段只修改 `RealManAdapter._feedback_loop()` 的等待计算。以下内容保持不变：
+
+- `follow=false`；
+- `canfd_trajectory_mode=2`；
+- 右臂单独调试 `max_linear_speed=0.15 m/s`；
+- 125 Hz ROS 控制定时器和 Placo QP；
+- 同一个 RealMan 连接承担反馈与发送，不新增连接；
+- 所有安全限位、超时和停止逻辑。
+
+现有 `feedback_read`、`feedback_interval` 和其他 timing 指标继续保留。
+
+### 验收
+
+右臂真机连续采集四个 timing 窗口，全部满足：
+
+- `feedback_interval mean ≤ 12 ms`；
+- `feedback_interval mean - feedback_read mean ≤ 2 ms`；
+- `feedback_age mean ≤ 7 ms`；
+- `period max < 10 ms`；
+- `total max < 8 ms`；
+- 无反馈超时、异常停止或 SDK 发送错误。
+
+若更密集的反馈查询使 `period max` 达到或超过 10 ms，或明显增加发送耗时，
+停止后续高跟随阶段，继续定位同一 SDK 连接的读写竞争。
+
+## 阶段二：高跟随完全透传
+
+只有阶段一通过后才验证：
+
+- `follow=true`；
+- `canfd_trajectory_mode=0`；
+- 右臂单独调试速度仍为 `0.15 m/s`。
+
+先通过现有 launch 参数显式启用高跟随完成右臂单机验证；验证通过后，再把
+`arm_debug.launch.py` 默认值和 `dual_arm_rm75.yaml`、`left_arm_rm75.yaml`、
+`right_arm_rm75.yaml` 同步为高跟随完全透传。
+
+验收条件：
+
+- 快速移动手柄约 10 cm 后，机械臂追赶不超过 1 秒；
+- 连续四个 timing 窗口 `period max < 10 ms`；
+- 无明显振荡、跳动、反馈超时或异常停止。
+
+若仍追赶超过 1 秒，不进入加速阶段；先增加关节目标与实测关节误差统计，
+确认慢速来自 QP 单步目标还是控制器执行。
+
+## 阶段三：右臂速度提高到 0.2 m/s
+
+只有阶段二通过后，将 `right_arm_rm75.yaml` 中右臂单独调试的
+`max_linear_speed` 从 `0.15` 提高到 `0.2 m/s`。
+
+- `dual_arm_rm75.yaml` 的左右臂已经是 `0.2 m/s`，无需修改；
+- 左臂单独调试速度保持现状；
+- 右臂真机 `max_line_speed=0.25 m/s` 安全上限保持不变。
+
+右臂 `scale=0.7` 时，手柄移动 10 cm 对应约 7 cm TCP 目标，理论限速时间约
+0.35 秒。验收追赶时间不超过 0.7 秒，并确认没有明显振荡或限位异常。
+
+## 测试与交付
+
+阶段一实现采用测试先行：
+
+- 用确定性时钟和停止事件验证短读取只等待剩余时间；
+- 验证读取超期后不额外等待，也不补跑多个历史周期；
+- 运行 `xr_rm_teleop` 全部 pytest；
+- 运行 `test_orientation_control.py`；
+- 在 `/home/robot/WS_xr` 运行 `colcon build --symlink-install`。
+
+Codex 不连接真机、不移动机械臂。每个阶段的真机验证由用户通过
+`xr_rm_bringup/launch/arm_debug.launch.py` 在右臂、小范围动作下完成，并把连续
+四个完整 timing 窗口返回后再进入下一阶段。

docs/superpowers/specs/2026-07-28-rm75-feedback-thread-timing-design.md

@@ -0,0 +1,53 @@
+# RM75 关节反馈线程计时统计设计
+
+## 目标
+
+在不改变关节反馈轮询、QP、关节指令和安全停止行为的前提下，测清当前
+RealMan 反馈线程的两个关键时间：
+
+- `rm_get_joint_degree()` 单次调用耗时；
+- 相邻两次成功写入关节反馈缓存的实际更新间隔。
+
+本轮只增加统计。高跟随、TCP 速度、反馈调度和 QP 控制方式均保持现状，待
+真机日志确认根因后再修改。
+
+## 方案
+
+`RealManAdapter` 在 `_read_joint_state_once()` 中使用单调高精度时钟记录：
+
+- `feedback_read`：从调用 `rm_get_joint_degree()` 前到调用返回后的耗时；
+- `feedback_interval`：本次成功反馈时间戳与上次成功反馈时间戳之差。
+
+两个数值随 `JointStateSnapshot` 写入现有线程安全缓存。首次成功反馈没有可靠
+的前序时间戳，因此不提供 `feedback_interval`。
+
+`SingleArmVelocityTeleop` 只在看到新的反馈时间戳时，将这两个数值各记录一次，
+避免 125 Hz 控制循环重复读取同一缓存而造成重复统计。统计加入现有约 5 秒
+timing 窗口，并输出各自的样本数、mean、P95、P99 和 max：
+
+```text
+feedback_read[n=<样本数> mean=<均值> p95=<P95> p99=<P99> max=<最大值> ms]
+feedback_interval[n=<样本数> mean=<均值> p95=<P95> p99=<P99> max=<最大值> ms]
+```
+
+读取失败不产生成功样本，继续沿用现有一次告警、反馈超时和安全停止逻辑。
+Mock 模式不伪造厂商 API 调用耗时。
+
+## 验证
+
+- 扩展现有关节控制单元测试，使用确定性快照验证新反馈只统计一次、重复缓存
+  不重复计数、首次反馈没有更新间隔。
+- 运行 `xr_rm_teleop` 相关 pytest。
+- 按项目规则在工作空间根目录运行 `colcon build --symlink-install`。
+- 真机测试仍由用户使用 `arm_debug.launch.py arm:=right use_mock:=false` 执行；
+  本轮不自动连接机械臂。
+
+## 后续决策
+
+用户提供真机 timing 日志后再判断：
+
+- 若 `feedback_interval` 主要由 `feedback_read + 8 ms` 构成，再评估绝对周期
+  调度；
+- 若 `feedback_read` 本身经常超过 8 ms，优先定位厂商查询或同一连接的读写
+  竞争；
+- 反馈问题确认前，不把高跟随或预测式 QP 与本轮统计改动混在一起。

docs/superpowers/specs/2026-07-28-rm75-idempotent-tool-frame-design.md

@@ -0,0 +1,61 @@
+# RM75 工具坐标系幂等配置设计
+
+## 目标
+
+修复遥操作节点每次启动都无条件创建 RealMan 工具坐标系、忽略重复名称错误，
+随后仍误报“外设配置完成”的问题。
+
+本修复只处理控制器工具坐标系的创建、更新、切换和返回值检查，不修改夹爪
+IO、Modbus、Placo、URDF 或任何机械臂运动控制参数。
+
+## 根因
+
+右臂 `scissorgripper: 1` 选择 `peripherals_rm75.yaml` 中的 `omnipic`。
+`configure_peripheral_on_connect` 默认为 `true`，因此节点每次启动都会进入
+`peripheral_cfg()`。
+
+当前实现无条件执行：
+
+```python
+robot.rm_set_manual_tool_frame(frame=tool_frame)
+robot.rm_change_tool_frame(tool_name)
+```
+
+RealMan 控制器会持久保存工具坐标系。首次启动创建成功，后续启动因
+`omnipic` 已存在而创建失败。两个返回值均未检查，因此代码继续执行并输出
+配置成功日志；若 YAML 中的 TCP、重量或重心已变化，控制器仍可能保留旧值。
+
+## 方案
+
+在 `fun_peripheral.py` 中增加一个小型内部函数，负责单一工具坐标系的幂等
+配置：
+
+1. 调用 `rm_get_total_tool_frame()` 获取现有工具坐标系名称并检查
+   `return_code`。
+2. 若目标名称不存在，调用 `rm_set_manual_tool_frame()`。
+3. 若目标名称已存在，调用 `rm_update_tool_frame()`。
+4. 检查创建或更新返回值。
+5. 调用 `rm_change_tool_frame()` 并检查返回值。
+
+任一步失败都抛出包含 SDK 操作名称和返回码的 `RuntimeError`。异常沿现有
+节点初始化链路向上传播，因此不会继续误报“外设配置完成”。
+
+不通过“先删除再创建”实现更新，避免在切换中的控制器上产生短暂无工具
+坐标系状态。
+
+## 测试
+
+在现有外设相关测试文件中使用 FakeArm 覆盖：
+
+- 名称不存在时只调用创建，然后切换；
+- 名称存在时只调用更新，然后切换；
+- 查询、创建/更新或切换失败时抛出明确错误。
+
+随后运行：
+
+- `xr_rm_teleop` 全部 pytest；
+- `test_orientation_control.py`；
+- `/home/robot/WS_xr` 下的 `colcon build --symlink-install`。
+
+Codex 不连接真机。真机验证由用户重新启动右臂 launch，确认不再出现
+`Failed to create the tool frame system`，并能看到外设配置完成日志。

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -2,9 +2,10 @@ import math
 
 import pytest
 
+from xr_rm_teleop import realman_adapter
 from xr_rm_teleop.realman_adapter import RealManAdapter
 from xr_rm_teleop.realman_adapter import MockRealManAdapter
-from xr_rm_teleop.fun_peripheral import PeripheralConfig
+from xr_rm_teleop.fun_peripheral import PeripheralConfig, _configure_tool_frame
 
 
 def test_initial_pose_uses_joint_move_only() -> None:
@@ -38,21 +39,152 @@ def test_peripheral_config_exposes_selected_tool() -> None:
     assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
 
 
-def test_joint_feedback_is_cached_in_radians() -> None:
+@pytest.mark.parametrize(
+    ("existing", "expected_operation"),
+    [(False, "create"), (True, "update")],
+)
+def test_tool_frame_is_created_or_updated(existing, expected_operation) -> None:
+    class FakeArm:
+        def __init__(self) -> None:
+            self.calls = []
+
+        def rm_get_total_tool_frame(self):
+            self.calls.append(("get",))
+            names = ["omnipic"] if existing else []
+            return {"return_code": 0, "tool_names": names}
+
+        def rm_set_manual_tool_frame(self, *, frame):
+            self.calls.append(("create", frame))
+            return 0
+
+        def rm_update_tool_frame(self, *, frame):
+            self.calls.append(("update", frame))
+            return 0
+
+        def rm_change_tool_frame(self, tool_name):
+            self.calls.append(("change", tool_name))
+            return 0
+
+    arm = FakeArm()
+    frame = object()
+
+    _configure_tool_frame(arm, frame, "omnipic")
+
+    assert arm.calls == [
+        ("get",),
+        (expected_operation, frame),
+        ("change", "omnipic"),
+    ]
+
+
+@pytest.mark.parametrize(
+    ("existing", "failure", "operation"),
+    [
+        (False, "query", "rm_get_total_tool_frame"),
+        (False, "create", "rm_set_manual_tool_frame"),
+        (True, "update", "rm_update_tool_frame"),
+        (False, "change", "rm_change_tool_frame"),
+    ],
+)
+def test_tool_frame_sdk_failures_are_reported(existing, failure, operation) -> None:
+    class FakeArm:
+        def rm_get_total_tool_frame(self):
+            names = ["omnipic"] if existing else []
+            return {
+                "return_code": 1 if failure == "query" else 0,
+                "tool_names": names,
+            }
+
+        def rm_set_manual_tool_frame(self, *, frame):
+            del frame
+            return 1 if failure == "create" else 0
+
+        def rm_update_tool_frame(self, *, frame):
+            del frame
+            return 1 if failure == "update" else 0
+
+        def rm_change_tool_frame(self, tool_name):
+            del tool_name
+            return 1 if failure == "change" else 0
+
+    with pytest.raises(RuntimeError, match=operation):
+        _configure_tool_frame(FakeArm(), object(), "omnipic")
+
+
+def test_joint_feedback_is_cached_in_radians(monkeypatch) -> None:
     class FakeArm:
         def rm_get_joint_degree(self):
             return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
 
+    perf_counter_ns = iter(
+        [1_000_000_000, 1_002_000_000, 2_000_000_000, 2_003_000_000]
+    )
+    monotonic = iter([10.0, 10.011])
+    monkeypatch.setattr(
+        realman_adapter,
+        "time",
+        type(
+            "FakeTime",
+            (),
+            {
+                "perf_counter_ns": staticmethod(lambda: next(perf_counter_ns)),
+                "monotonic": staticmethod(lambda: next(monotonic)),
+            },
+        ),
+    )
     adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
     adapter._arm = FakeArm()
 
     adapter._read_joint_state_once()
-    snapshot = adapter.get_latest_joint_state()
+    first = adapter.get_latest_joint_state()
+    adapter._read_joint_state_once()
+    second = adapter.get_latest_joint_state()
 
-    assert snapshot is not None
-    assert snapshot.positions == pytest.approx(
+    assert first is not None
+    assert first.positions == pytest.approx(
         [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
     )
+    assert first.read_duration_ms == pytest.approx(2.0)
+    assert first.update_interval_ms is None
+    assert second is not None
+    assert second.read_duration_ms == pytest.approx(3.0)
+    assert second.update_interval_ms == pytest.approx(11.0)
+
+
+def test_feedback_loop_uses_absolute_schedule_without_catch_up(monkeypatch) -> None:
+    class FakeStopEvent:
+        def __init__(self) -> None:
+            self.checks = 0
+            self.waits = []
+
+        def is_set(self):
+            self.checks += 1
+            return self.checks > 3
+
+        def wait(self, timeout):
+            self.waits.append(timeout)
+            return False
+
+    monotonic = iter([0.0, 0.005, 0.018, 0.018, 0.023])
+    monkeypatch.setattr(
+        realman_adapter,
+        "time",
+        type(
+            "FakeTime",
+            (),
+            {"monotonic": staticmethod(lambda: next(monotonic))},
+        ),
+    )
+    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.008)
+    stop_event = FakeStopEvent()
+    reads = []
+    adapter._feedback_stop = stop_event
+    adapter._read_joint_state_once = lambda: reads.append(None)
+
+    adapter._feedback_loop()
+
+    assert len(reads) == 3
+    assert stop_event.waits == pytest.approx([0.003, 0.003])
 
 
 def test_joint_target_uses_movej_canfd_in_degrees() -> None:

xr_rm_teleop/test/test_joint_control.py

@@ -194,33 +194,47 @@ def test_timing_stats_logs_summary_and_clears_window() -> None:
     teleop = object.__new__(SingleArmVelocityTeleop)
     teleop._arm_name = "right_rm75"
     teleop._dt = 0.008
-    teleop._timing_stats_window = 2
+    teleop._timing_stats_window = 3
     teleop._timing_samples = {
         name: []
-        for name in ("period", "total", "qp", "send", "feedback_age")
+        for name in (
+            "period",
+            "total",
+            "qp",
+            "send",
+            "feedback_age",
+            "feedback_read",
+            "feedback_interval",
+        )
     }
+    teleop._last_timing_feedback_received_at = None
     teleop.get_logger = lambda: SimpleNamespace(
         info=lambda message: messages.append(message)
     )
+    first_feedback = JointStateSnapshot([0.0] * 7, 10.0, 2.0, None)
+    second_feedback = JointStateSnapshot([0.0] * 7, 10.011, 3.0, 11.0)
 
-    teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
+    teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0, first_feedback)
+    teleop._record_timing_sample(8.0, 8.0, 1.5, 0.6, 3.5, first_feedback)
     assert messages == []
 
-    teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)
+    teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0, second_feedback)
 
     assert len(messages) == 1
-    assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
+    assert "right_rm75 timing n=3 deadline=8.000 ms" in messages[0]
     assert (
-        "period[n=2 mean=8.000 p95=8.900 p99=8.980 "
+        "period[n=3 mean=8.000 p95=8.900 p99=8.980 "
         "max=9.000 ms overruns=1]"
     ) in messages[0]
     assert (
-        "total[n=2 mean=8.000 p95=9.800 p99=9.960 "
+        "total[n=3 mean=8.000 p95=9.800 p99=9.960 "
         "max=10.000 ms overruns=1]"
     ) in messages[0]
-    assert "qp[n=2" in messages[0]
-    assert "send[n=2" in messages[0]
-    assert "feedback_age[n=2" in messages[0]
+    assert "qp[n=3" in messages[0]
+    assert "send[n=3" in messages[0]
+    assert "feedback_age[n=3" in messages[0]
+    assert "feedback_read[n=2" in messages[0]
+    assert "feedback_interval[n=1" in messages[0]
     assert all(not samples for samples in teleop._timing_samples.values())
 
 

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

@@ -102,6 +102,40 @@ def _tool_name_for_index(tools_in_ee: dict[str, list[list[float]]], scissorgripp
     return list(tools_in_ee.keys())[scissorgripper]
 
 
+def _check_sdk_return(result: Any, operation: str) -> None:
+    if result != 0:
+        raise RuntimeError(f"{operation} failed with code {result}: {result!r}")
+
+
+def _configure_tool_frame(robot, tool_frame, tool_name: str) -> None:
+    frames = robot.rm_get_total_tool_frame()
+    if not isinstance(frames, dict):
+        raise RuntimeError(
+            f"rm_get_total_tool_frame returned invalid data: {frames!r}"
+        )
+    _check_sdk_return(
+        frames.get("return_code"),
+        "rm_get_total_tool_frame",
+    )
+    tool_names = frames.get("tool_names")
+    if not isinstance(tool_names, (list, tuple)):
+        raise RuntimeError(
+            f"rm_get_total_tool_frame returned invalid tool_names: {tool_names!r}"
+        )
+
+    if tool_name in tool_names:
+        operation = "rm_update_tool_frame"
+        result = robot.rm_update_tool_frame(frame=tool_frame)
+    else:
+        operation = "rm_set_manual_tool_frame"
+        result = robot.rm_set_manual_tool_frame(frame=tool_frame)
+    _check_sdk_return(result, operation)
+    _check_sdk_return(
+        robot.rm_change_tool_frame(tool_name),
+        "rm_change_tool_frame",
+    )
+
+
 def cal_tool_frame(handle, scissorgripper, tools_in_ee):
     """根据末端工具配置生成 RealMan 工具坐标系。"""
     from Robotic_Arm.rm_robot_interface import rm_frame_t
@@ -160,8 +194,7 @@ def peripheral_cfg(
 
     time.sleep(0.2)
     tool_frame, tool_name = cal_tool_frame(robot, scissorgripper, tools_in_ee)
-    robot.rm_set_manual_tool_frame(frame=tool_frame)
-    robot.rm_change_tool_frame(tool_name)
+    _configure_tool_frame(robot, tool_frame, tool_name)
 
     if scissorgripper == 0:
         # 剪刀夹爪通过工具板数字输出控制。

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -30,6 +30,8 @@ class ArmPose:
 class JointStateSnapshot:
     positions: list[float]
     received_at: float
+    read_duration_ms: float | None = None
+    update_interval_ms: float | None = None
 
 
 class MockRealManAdapter:
@@ -161,6 +163,8 @@ class RealManAdapter:
             return JointStateSnapshot(
                 list(self._latest_joint_state.positions),
                 self._latest_joint_state.received_at,
+                self._latest_joint_state.read_duration_ms,
+                self._latest_joint_state.update_interval_ms,
             )
 
     def send_joint_target(self, joints: list[float], follow: bool) -> None:
@@ -232,6 +236,7 @@ class RealManAdapter:
             raise RuntimeError("睿尔曼机械臂尚未连接")
 
     def _feedback_loop(self) -> None:
+        next_read_at = time.monotonic()
         while not self._feedback_stop.is_set():
             try:
                 self._read_joint_state_once()
@@ -240,11 +245,18 @@ class RealManAdapter:
                 if not self._feedback_fault_logged:
                     self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
                     self._feedback_fault_logged = True
-            self._feedback_stop.wait(self._feedback_period)
+            next_read_at += self._feedback_period
+            remaining = next_read_at - time.monotonic()
+            if remaining <= 0.0:
+                next_read_at = time.monotonic()
+                continue
+            self._feedback_stop.wait(remaining)
 
     def _read_joint_state_once(self) -> None:
         self._require_arm()
+        read_started_ns = time.perf_counter_ns()
         result = self._arm.rm_get_joint_degree()
+        read_duration_ms = (time.perf_counter_ns() - read_started_ns) * 1e-6
         self._check_return(result, "rm_get_joint_degree")
         if not isinstance(result, tuple) or len(result) < 2:
             raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}")
@@ -258,9 +270,19 @@ class RealManAdapter:
         positions = [math.radians(float(value)) for value in degrees]
         if not all(math.isfinite(value) for value in positions):
             raise RuntimeError("RM75 关节反馈包含 NaN/Inf")
-        snapshot = JointStateSnapshot(positions, time.monotonic())
+        received_at = time.monotonic()
         with self._joint_state_lock:
-            self._latest_joint_state = snapshot
+            update_interval_ms = (
+                None
+                if self._latest_joint_state is None
+                else (received_at - self._latest_joint_state.received_at) * 1000.0
+            )
+            self._latest_joint_state = JointStateSnapshot(
+                positions,
+                received_at,
+                read_duration_ms,
+                update_interval_ms,
+            )
 
     def _apply_safety_limits(self) -> None:
         # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -283,9 +283,18 @@ class SingleArmVelocityTeleop(Node):
         self._timing_stats_window = max(1, int(round(5.0 / self._dt)))
         self._timing_samples = {
             name: []
-            for name in ("period", "total", "qp", "send", "feedback_age")
+            for name in (
+                "period",
+                "total",
+                "qp",
+                "send",
+                "feedback_age",
+                "feedback_read",
+                "feedback_interval",
+            )
         }
         self._last_control_tick_started_ns: int | None = None
+        self._last_timing_feedback_received_at: float | None = None
 
         peripheral_arm = self._peripheral_arm_name()
         config_file = str(self.get_parameter("peripheral_config_file").value)
@@ -602,6 +611,7 @@ class SingleArmVelocityTeleop(Node):
                 qp_ms,
                 send_ms,
                 feedback_age_ms,
+                snapshot,
             )
         except Exception as exc:
             self.get_logger().warn(
@@ -882,6 +892,7 @@ class SingleArmVelocityTeleop(Node):
         qp_ms: float,
         send_ms: float,
         feedback_age_ms: float,
+        snapshot: JointStateSnapshot,
     ) -> None:
         if period_ms is not None:
             self._timing_samples["period"].append(period_ms)
@@ -889,6 +900,16 @@ class SingleArmVelocityTeleop(Node):
         self._timing_samples["qp"].append(qp_ms)
         self._timing_samples["send"].append(send_ms)
         self._timing_samples["feedback_age"].append(feedback_age_ms)
+        if snapshot.received_at != self._last_timing_feedback_received_at:
+            self._last_timing_feedback_received_at = snapshot.received_at
+            if snapshot.read_duration_ms is not None:
+                self._timing_samples["feedback_read"].append(
+                    snapshot.read_duration_ms
+                )
+            if snapshot.update_interval_ms is not None:
+                self._timing_samples["feedback_interval"].append(
+                    snapshot.update_interval_ms
+                )
         if len(self._timing_samples["total"]) < self._timing_stats_window:
             return
 
@@ -912,6 +933,11 @@ class SingleArmVelocityTeleop(Node):
                 self._timing_samples["feedback_age"],
             ),
         ]
+        for name in ("feedback_read", "feedback_interval"):
+            if self._timing_samples[name]:
+                summaries.append(
+                    self._timing_summary(name, self._timing_samples[name])
+                )
         message = (
             f"{self._arm_name} timing n={sample_count} "
             f"deadline={deadline_ms:.3f} ms | "