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

2026-07-29 09:44:51 +08:00
parent 2a12eea4d5
commit 687a0b401a
11 changed files with 876 additions and 21 deletions
@@ -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
+```
+
+预期：四个包构建成功。
+