# 手柄主键回初始位姿设计

## 背景与目标

有线连接已基本解决 UDP 超时和逆解失败问题。本次只增加一个明确操作：
点击当前机械臂对应手柄的主键，使该机械臂按已配置的关节角回到初始位姿。

- 右臂模式：右手 A 键控制右臂，左手 X 键无效。
- 左臂模式：左手 X 键控制左臂，右手 A 键无效。
- 双臂模式：右手 A 键控制右臂，左手 X 键控制左臂。

## 最小方案

`XrController.primary` 已表示左手 X 键或右手 A 键。每个遥操作节点继续只订阅
自身的手柄话题，并在 `primary` 上升沿调用适配器的初始位姿运动：

```text
左手 X → /xr/left_controller  → left_arm_teleop  → 左臂初始位姿
右手 A → /xr/right_controller → right_arm_teleop → 右臂初始位姿
```

单臂模式只启动对应节点，因此另一只手柄天然无效；双臂模式下两个节点独立处理，
无需新增协调节点、话题、服务或配置项。

## 运动与安全行为

- 只在按键从未按下变为按下时触发，持续按住不重复执行。
- 回位前调用现有安全停止逻辑，退出当前相对位姿遥操作。
- 使用更新后的 `initial_joint_pose` 和现有 `init_move_speed`。
- 复用现有 `rm_movej(..., block=1)` 阻塞运动，完成后重新同步关节状态和 QP 状态。
- 回位后必须先松开 Grip，才能重新进入遥操作。
- 回位失败时记录错误并保持停止，不自动重试。
- mock 模式只更新模拟关节状态，不导入或调用厂商 SDK。

## 初始位姿配置

关节角单位沿用现有 YAML，均为度：

- 左臂：`[-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]`

`left_arm_rm75.yaml` 使用左臂值，`right_arm_rm75.yaml` 使用右臂值；
`dual_arm_rm75.yaml` 中左右节点分别使用对应值。三个文件中的 IP、坐标映射、
工作空间、安全限制及其他控制参数保持不变。

## 代码范围

- 在 `realman_adapter.py` 为真机和 mock 提供同名的公开回位方法，真机实现复用现有
  私有初始位姿运动代码。
- 在 `single_arm_velocity_teleop.py` 的现有手柄回调中增加主键上升沿处理。
- 在现有测试文件中增加最小的适配器回位和按键边沿测试。
- 同步 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和 `dual_arm_rm75.yaml` 中的
  `initial_joint_pose`。

不修改消息定义、UDP 输入节点、launch、三个 YAML 中的其他参数、安全限位或
夹爪控制。

## 验证

- 测试主键首次按下触发一次，持续按住不重复触发，松开后可以再次触发。
- 测试 mock 回位后恢复配置的初始关节角。
- 测试真机适配器仍只调用既有的关节运动命令；测试使用假 SDK 对象，不连接真机。
- 检查单臂和双臂配置中的左右初始位姿与上述值一致。
- 在 `/home/robot/WS_xr` 执行：

  ```bash
  source /opt/ros/humble/setup.bash
  python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py
  python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py
  colcon build --symlink-install
  ```

全部验证使用 mock 或假对象，不连接真机、不移动机械臂、不操作夹爪。