# 手柄主键回初始位姿实施计划

> **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:** 左手 X 键和右手 A 键分别让对应机械臂安全回到配置的初始关节位姿，并同步三份机械臂配置中的新关节角。

**Architecture:** 继续使用现有左右手柄独立话题和单臂遥操作节点，不增加协调节点。遥操作节点检测自身 `XrController.primary` 的上升沿，先停止当前遥操作，再调用真机或 mock 适配器的同名回位方法并重新同步关节状态。

**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3、rclpy、pytest、ament/colcon、RealMan Python API2（仅真机运行时）。

## 全局约束

- 构建、测试和运行命令在 `/home/robot/WS_xr` 执行，并先运行 `source /opt/ros/humble/setup.bash`。
- 所有自动验证使用 mock 或假对象，不连接真机、不移动机械臂、不操作夹爪。
- 保留工作空间与圆柱限位、线速度与角速度限制、指令超时和安全停止逻辑。
- `configure_safety_limits` 保持启用；`move_to_initial_pose_on_connect` 默认值保持 `false`。
- mock 模式不得导入或依赖睿尔曼厂商 SDK，不新增 RealMan 连接。
- 只修改完成本功能所需文件，不新增依赖、节点、话题、服务或配置项。
- 左臂初始关节角（度）：`[-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]`。
- 右臂初始关节角（度）：`[-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]`。

---

### Task 1: 复用适配器初始位姿运动

**Files:**
- Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:42-82`
- Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:178-182`
- Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:454-459`
- Test: `src/xr_rm_teleop/test/test_initial_joint_pose.py:13-35`

**Interfaces:**
- Consumes: 现有 `initial_joint_pose: list[float]`（度）和 `init_move_speed: int`。
- Produces: `MockRealManAdapter.move_to_initial_pose() -> None`。
- Produces: `RealManAdapter.move_to_initial_pose() -> None`。

- [ ] **Step 1: 先写失败测试**

将真机测试改为调用公开方法，并增加 mock 恢复初始关节角的测试：

```python
def test_initial_pose_uses_joint_move_only() -> None:
    class FakeArm:
        def __init__(self) -> None:
            self.calls = []

        def rm_movej(self, *args):
            self.calls.append(args)
            return 0

    joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
    adapter = RealManAdapter(
        "127.0.0.1",
        8080,
        0,
        "127.0.0.1",
        8090,
        initial_joint_pose=joints,
    )
    adapter._arm = FakeArm()

    adapter.move_to_initial_pose()

    assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]


def test_mock_initial_pose_restores_configured_joints() -> None:
    initial_degrees = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
    adapter = MockRealManAdapter(initial_degrees)
    adapter.send_joint_target([0.0] * 7, follow=False)

    adapter.move_to_initial_pose()

    assert adapter.read_joint_state().positions == pytest.approx(
        [math.radians(value) for value in initial_degrees]
    )
```

- [ ] **Step 2: 运行测试并确认按预期失败**

Run:

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
  -k 'initial_pose_uses_joint_move_only or mock_initial_pose_restores_configured_joints' -v
```

Expected: FAIL，两个适配器都还没有公开的 `move_to_initial_pose` 方法。

- [ ] **Step 3: 写最小实现**

在 mock 中保存初始弧度值并实现恢复：

```python
self._initial_joint_positions = [
    math.radians(value) for value in initial_joint_degrees
]
self._joint_positions = list(self._initial_joint_positions)
```

```python
def move_to_initial_pose(self) -> None:
    self._joint_positions = list(self._initial_joint_positions)
    self.last_joint_target = list(self._joint_positions)
```

将真机 `_move_to_initial_pose` 改为公开方法，并保留原有阻塞式关节运动：

```python
def move_to_initial_pose(self) -> None:
    self._require_arm()
    if self._initial_joint_pose is None:
        raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")

    ret = self._arm.rm_movej(
        self._initial_joint_pose,
        self._init_move_speed,
        0,
        0,
        1,
    )
    self._check_return(ret, "rm_movej(initial_joint_pose)")
```

同时把 `connect()` 中的启动回位调用改为：

```python
if self._move_to_initial_pose_on_connect:
    self.move_to_initial_pose()
```

- [ ] **Step 4: 运行测试并确认通过**

Run:

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
  -k 'initial_pose_uses_joint_move_only or mock_initial_pose_restores_configured_joints' -v
```

Expected: PASS。

- [ ] **Step 5: 创建本地提交**

```bash
cd /home/robot/WS_xr/src
git add xr_rm_teleop/xr_rm_teleop/realman_adapter.py \
  xr_rm_teleop/test/test_initial_joint_pose.py
git commit -m "feat: 复用适配器初始位姿运动"
```

### Task 2: 在遥操作节点处理主键上升沿

**Files:**
- Modify: `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:276-299`
- Modify: `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:514-534`
- Test: `src/xr_rm_teleop/test/test_joint_control.py:16-114`

**Interfaces:**
- Consumes: `XrController.primary: bool`。
- Consumes: Task 1 的 `adapter.move_to_initial_pose() -> None`。
- Produces: `SingleArmVelocityTeleop._handle_initial_pose_button(msg: XrController) -> None`。

- [ ] **Step 1: 先写主键边沿失败测试**

在 `test_joint_control.py` 增加测试辅助函数和成功路径测试：

```python
def _primary_button_teleop(*, move_error=None):
    events = []
    errors = []
    snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())

    class Adapter:
        def move_to_initial_pose(self):
            events.append("move")
            if move_error is not None:
                raise move_error

        def read_joint_state(self):
            events.append("read")
            return snapshot

    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._arm_name = "right_rm75"
    teleop._adapter = Adapter()
    teleop._last_primary_pressed = None
    teleop._grip_rearm_required = False
    teleop._safe_stop = lambda reset_active: events.append(
        ("stop", reset_active)
    )
    teleop._reset_joint_state = lambda value: events.append(("sync", value))
    teleop._handle_trigger_gripper = lambda msg: None
    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
    teleop.get_logger = lambda: SimpleNamespace(
        info=lambda message: None,
        error=lambda message: errors.append(message),
    )
    return teleop, events, errors, snapshot


def test_primary_button_rising_edge_moves_once_and_resyncs() -> None:
    teleop, events, _, snapshot = _primary_button_teleop()
    released = SimpleNamespace(primary=False)
    pressed = SimpleNamespace(primary=True)

    teleop._on_controller(released)
    teleop._on_controller(pressed)
    teleop._on_controller(pressed)
    teleop._on_controller(released)
    teleop._on_controller(pressed)

    expected_once = [
        ("stop", True),
        "move",
        "read",
        ("sync", snapshot),
    ]
    assert events == expected_once * 2
    assert teleop._grip_rearm_required
```

- [ ] **Step 2: 运行测试并确认按预期失败**

Run:

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
  -k primary_button_rising_edge_moves_once_and_resyncs -v
```

Expected: FAIL，因为 `_on_controller` 尚未处理 `primary`。

- [ ] **Step 3: 写最小成功实现**

在节点状态中增加与现有 trigger 相同的首次采样保护：

```python
self._last_primary_pressed: bool | None = None
```

在现有回调中接入主键处理：

```python
def _on_controller(self, msg: XrController) -> None:
    self._last_msg = msg
    self._last_msg_time = self.get_clock().now()
    self._handle_initial_pose_button(msg)
    self._handle_trigger_gripper(msg)
```

增加主键上升沿处理；首次采样只建立状态，避免节点启动时按键已经按住而意外运动：

```python
def _handle_initial_pose_button(self, msg: XrController) -> None:
    if self._last_primary_pressed is None:
        self._last_primary_pressed = msg.primary
        return

    rising_edge = msg.primary and not self._last_primary_pressed
    self._last_primary_pressed = msg.primary
    if not rising_edge:
        return

    self._grip_rearm_required = True
    self._safe_stop(reset_active=True)
    self._adapter.move_to_initial_pose()
    self._reset_joint_state(self._adapter.read_joint_state())
    self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
```

- [ ] **Step 4: 运行测试并确认通过**

Run:

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
  -k primary_button_rising_edge_moves_once_and_resyncs -v
```

Expected: PASS。

- [ ] **Step 5: 先写失败路径测试**

```python
def test_primary_button_move_failure_logs_and_stays_stopped() -> None:
    failure = RuntimeError("rm_movej failed")
    teleop, events, errors, _ = _primary_button_teleop(
        move_error=failure
    )

    teleop._on_controller(SimpleNamespace(primary=False))
    teleop._on_controller(SimpleNamespace(primary=True))

    assert events == [("stop", True), "move"]
    assert teleop._grip_rearm_required
    assert errors == [
        "right_rm75 回初始位姿失败：rm_movej failed"
    ]
```

- [ ] **Step 6: 运行失败路径测试并确认按预期失败**

Run:

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
  -k primary_button_move_failure_logs_and_stays_stopped -v
```

Expected: FAIL，并抛出 `RuntimeError: rm_movej failed`。

- [ ] **Step 7: 增加最小异常处理**

用 `try/except` 包住回位和状态同步，失败时记录错误并保持已经设置的停止与 Grip
重新使能状态：

```python
try:
    self._adapter.move_to_initial_pose()
    self._reset_joint_state(self._adapter.read_joint_state())
except Exception as exc:
    self.get_logger().error(
        f"{self._arm_name} 回初始位姿失败：{exc}"
    )
    return

self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
```

- [ ] **Step 8: 运行两条主键测试并确认通过**

Run:

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -k primary_button -v
```

Expected: PASS。

- [ ] **Step 9: 创建本地提交**

```bash
cd /home/robot/WS_xr/src
git add xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
  xr_rm_teleop/test/test_joint_control.py
git commit -m "feat: 添加手柄主键回初始位姿"
```

### Task 3: 同步三份初始位姿配置

**Files:**
- Modify: `src/xr_rm_bringup/config/left_arm_rm75.yaml:57`
- Modify: `src/xr_rm_bringup/config/right_arm_rm75.yaml:57`
- Modify: `src/xr_rm_bringup/config/dual_arm_rm75.yaml:64`
- Modify: `src/xr_rm_bringup/config/dual_arm_rm75.yaml:121`

**Interfaces:**
- Consumes: 用户确认的左右臂 7 个关节角，单位为度。
- Produces: 单臂和双臂模式一致的对应臂 `initial_joint_pose`。

- [ ] **Step 1: 只替换四处初始位姿**

```yaml
# left_arm_rm75.yaml
initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]

# right_arm_rm75.yaml
initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]

# dual_arm_rm75.yaml / left_arm_teleop
initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]

# dual_arm_rm75.yaml / right_arm_teleop
initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
```

- [ ] **Step 2: 解析 YAML 并验证四处值**

Run:

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 - <<'PY'
from pathlib import Path
import yaml

config_dir = Path("src/xr_rm_bringup/config")
left = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
right = [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]

left_single = yaml.safe_load((config_dir / "left_arm_rm75.yaml").read_text())
right_single = yaml.safe_load((config_dir / "right_arm_rm75.yaml").read_text())
dual = yaml.safe_load((config_dir / "dual_arm_rm75.yaml").read_text())

assert left_single["single_arm_velocity_teleop"]["ros__parameters"]["initial_joint_pose"] == left
assert right_single["single_arm_velocity_teleop"]["ros__parameters"]["initial_joint_pose"] == right
assert dual["left_arm_teleop"]["ros__parameters"]["initial_joint_pose"] == left
assert dual["right_arm_teleop"]["ros__parameters"]["initial_joint_pose"] == right
PY
```

Expected: exit code 0，无输出。

- [ ] **Step 3: 确认没有改动其他 YAML 参数**

Run:

```bash
cd /home/robot/WS_xr/src
git diff --word-diff=plain -- \
  xr_rm_bringup/config/left_arm_rm75.yaml \
  xr_rm_bringup/config/right_arm_rm75.yaml \
  xr_rm_bringup/config/dual_arm_rm75.yaml
```

Expected: 只有四个 `initial_joint_pose` 列表发生变化。

- [ ] **Step 4: 创建本地提交**

```bash
cd /home/robot/WS_xr/src
git add xr_rm_bringup/config/left_arm_rm75.yaml \
  xr_rm_bringup/config/right_arm_rm75.yaml \
  xr_rm_bringup/config/dual_arm_rm75.yaml
git commit -m "config: 更新左右臂初始位姿"
```

### Task 4: 完整验证

**Files:**
- Verify: `src/xr_rm_teleop/test/test_initial_joint_pose.py`
- Verify: `src/xr_rm_teleop/test/test_joint_control.py`
- Verify: `src/xr_rm_teleop/test/test_orientation_control.py`
- Verify: 全部四个 ROS2 包

**Interfaces:**
- Consumes: Tasks 1–3 的本地提交。
- Produces: mock 测试与 ROS2 构建通过的可验证结果。

- [ ] **Step 1: 运行遥操作相关测试**

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
  src/xr_rm_teleop/test/test_joint_control.py \
  src/xr_rm_teleop/test/test_orientation_control.py
```

Expected: PASS，无 error 或 warning。

- [ ] **Step 2: 构建工作空间**

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install
```

Expected: `xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop` 和 `xr_rm_bringup`
构建完成，无失败包。

- [ ] **Step 3: 检查最终范围**

```bash
cd /home/robot/WS_xr/src
git status --short
git log -6 --oneline
```

Expected: 工作区干净；只有设计、计划、适配器、遥操作节点、两份测试和三份 YAML
配置的相关本地提交，不存在远程写操作。