# 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
```

预期：四个包构建成功。