# RM75 QP 收敛优化实施计划

> **执行要求：** 使用 `superpowers:executing-plans` 逐项执行。用户未授权
> subagent、独立worktree或本地分支，因此本计划只允许当前会话内联实施。所有
> 步骤使用复选框跟踪。

**目标：** 将当前每周期单步QP改为有界迭代QP，使低跟随RM75在手柄移动10 cm
后约1秒内稳定到位，并消除由近距离台阶目标造成的持续轻微晃动。

**实现方式：** 每个正常控制周期仍先用UDP实际关节角同步Placo，然后在一次
`PlacoIkSolver.solve()`内部最多迭代30次，提前达到1 mm位置误差和0.005 rad
姿态误差即返回。最终关节解继续经过现有90 Hz关节速度与加速度限幅后，以
`follow=false`发送；不增加预测状态、线程、连接、依赖或配置参数。

**技术栈：** Python 3.10、ROS2 Humble、Placo 0.9.4、NumPy、pytest、
ament/colcon。

**设计文档：**
`docs/superpowers/specs/2026-07-29-rm75-qp-convergence-design.md`

---

## 仓库与安全约束

- 构建、测试和启动命令在 `/home/robot/WS_xr` 执行。
- Git命令在 `/home/robot/WS_xr/src` 执行。
- 每次构建、测试或启动前执行 `source /opt/ros/humble/setup.bash`。
- 不自动提交、推送、创建分支或worktree。
- 不连接真机，不发送真实CANFD，不移动机械臂，不操作夹爪。
- 只通过 `arm_debug.launch.py arm:=right use_mock:=true`进行启动验证。
- 不修改三份机械臂YAML、RealMan适配器、launch、UI、依赖或公开入口。
- 保留工作空间、圆柱、TCP速度、姿态速度、关节速度、关节加速度、反馈超时、
  CANFD恢复、Grip重新使能和安全停止逻辑。

## 文件范围

- 修改 `xr_rm_teleop/test/test_placo_transforms.py`
  - 增加真实Placo 7 cm目标收敛回归测试。
- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
  - 增加固定上限、提前收敛和逐步安全校验。

不需要修改 `single_arm_velocity_teleop.py`；现有 `_solve_joint_target()` 已负责
QP异常时打印限频警告并保持上一组安全关节目标，现有
`_limit_joint_command_step()` 已负责最终90 Hz真实命令限速。

---

## 任务一：用真实Placo复现单步QP不收敛

**修改文件：**

- `xr_rm_teleop/test/test_placo_transforms.py`

- [x] **步骤1：增加测试辅助函数**

在文件顶部增加：

```python
import math
```

在现有URDF结构测试之后增加：

```python
def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]:
    pytest.importorskip("placo")
    urdf_path = (
        Path(__file__).resolve().parents[1]
        / "models"
        / "rm75_omnipicker"
        / "urdf"
        / "RM75-B_OmniPicker_fixed.urdf"
    )
    joints = [
        math.radians(value)
        for value in [
            -90.14,
            3.76,
            -86.89,
            87.89,
            -96.53,
            -79.62,
            -90.04,
        ]
    ]
    return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints
```

`importorskip()`只让没有Placo的普通系统Python跳过真模型用例；下面的RED/GREEN
命令会显式加入项目现有Placo 0.9.4路径，因此该用例必须实际执行而不能跳过。

- [x] **步骤2：增加7 cm目标收敛测试**

增加：

```python
def test_qp_solve_converges_to_reachable_tcp_target() -> None:
    solver, joints = _rm75_placo_solver()
    start_pose = solver.update_joint_state(joints)
    target_pose = start_pose.copy()
    target_pose[0, 3] += 0.07

    result = solver.solve(target_pose)
    reached_pose = solver.update_joint_state(result)
    position_error = np.linalg.norm(
        target_pose[:3, 3] - reached_pose[:3, 3]
    )
    rotation_delta = (
        target_pose[:3, :3] @ reached_pose[:3, :3].T
    )
    orientation_error = math.acos(
        float(
            np.clip(
                (np.trace(rotation_delta) - 1.0) * 0.5,
                -1.0,
                1.0,
            )
        )
    )

    assert position_error <= 1e-3
    assert orientation_error <= 5e-3
```

该测试验证一次公开 `solve()` 调用返回当前TCP目标对应的收敛关节解，而不是验证
内部迭代次数。

- [x] **步骤3：运行测试并确认RED**

```bash
source /opt/ros/humble/setup.bash
export RM75_PLACO_TEST_PATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
PYTHONPATH="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
  python3 -m pytest \
  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_converges_to_reachable_tcp_target \
  -v
```

预期：测试以位置误差约0.063 m大于0.001 m失败，证明当前单步QP确实不能在一次
调用内给出收敛关节目标。测试不得因导入错误或跳过而结束。

---

## 任务二：实现有界迭代QP

**修改文件：**

- `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`

- [x] **步骤1：增加固定收敛常量**

把模块说明改为：

```python
"""RM75 的 Placo 0.9.4 有界迭代 QP 逆解。"""
```

在现有常量后增加：

```python
QP_MAX_ITERATIONS = 30
QP_POSITION_TOLERANCE_M = 1e-3
QP_ORIENTATION_TOLERANCE_RAD = 5e-3
```

这些值是本次已确认的算法边界，不新增ROS参数。

- [x] **步骤2：增加任务误差读取**

在 `solve()` 前增加：

```python
def _target_errors(self) -> tuple[float, float]:
    position_task = self._frame_task.position()
    orientation_task = self._frame_task.orientation()
    position_task.update()
    orientation_task.update()
    return (
        float(position_task.error_norm()),
        float(orientation_task.error_norm()),
    )
```

Placo在 `solve(True)` 后只更新关节状态；先更新机器人运动学，再显式更新两个任务，
确保 `error_norm()`对应当前迭代后的状态而不是前一迭代。

- [x] **步骤3：把单步求解改为最多30次且提前收敛**

用以下实现替换现有 `solve()`：

```python
def solve(self, target_tool_pose: np.ndarray) -> list[float]:
    if self._actual_joints is None:
        raise RuntimeError(
            "joint state must be initialized before QP solve"
        )
    self._frame_task.T_world_frame = _validated_transform(
        target_tool_pose
    )
    result = np.asarray(
        self._robot.state.q[RM75_Q_SLICE],
        dtype=float,
    ).copy()
    position_error, orientation_error = self._target_errors()
    if (
        position_error <= QP_POSITION_TOLERANCE_M
        and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
    ):
        return result.tolist()

    for _ in range(QP_MAX_ITERATIONS):
        previous = result
        self._solver.solve(True)
        self._robot.update_kinematics()
        result = np.asarray(
            self._robot.state.q[RM75_Q_SLICE],
            dtype=float,
        ).copy()
        self._validate_result(result, previous)
        position_error, orientation_error = self._target_errors()
        if (
            position_error <= QP_POSITION_TOLERANCE_M
            and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
        ):
            return result.tolist()

    raise RuntimeError(
        "QP did not converge after "
        f"{QP_MAX_ITERATIONS} iterations: "
        f"position_error={position_error:.6f} m, "
        f"orientation_error={orientation_error:.6f} rad"
    )
```

目标已到达时直接返回当前关节角，避免静止时进行不必要的数值迭代。

- [x] **步骤4：让速度校验针对每次数值迭代**

把 `_validate_result()` 签名改为：

```python
def _validate_result(
    self,
    result: np.ndarray,
    reference: np.ndarray | None = None,
) -> None:
```

保留现有有限值和关节位置检查，把速度检查替换为：

```python
if reference is None:
    reference = self._actual_joints
if reference is None:
    raise RuntimeError("joint state has not been initialized")
reference = np.asarray(reference, dtype=float)
if reference.shape != (7,) or not np.isfinite(reference).all():
    raise ValueError("QP reference must contain 7 finite values")
max_step = self._velocity_limits * self._dt + 1e-9
if np.any(np.abs(result - reference) > max_step):
    raise ValueError(
        "QP result violates RM75 one-cycle velocity limits"
    )
```

这样每次内部数值迭代继续满足Placo的URDF关节速度边界；最终收敛解仍由节点现有
`_limit_joint_command_step()`按真实90 Hz周期限制后才发送。

- [x] **步骤5：运行目标测试并确认GREEN**

```bash
source /opt/ros/humble/setup.bash
export RM75_PLACO_TEST_PATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
PYTHONPATH="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
  python3 -m pytest \
  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_converges_to_reachable_tcp_target \
  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_result_rejects_nan_position_and_velocity_violations \
  -v
```

预期：两个测试通过；真实Placo用例不被跳过。

- [x] **步骤6：运行Placo变换测试文件**

```bash
source /opt/ros/humble/setup.bash
export RM75_PLACO_TEST_PATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
PYTHONPATH="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
  python3 -m pytest \
  src/xr_rm_teleop/test/test_placo_transforms.py \
  -v
```

预期：全部通过，无失败或跳过。

---

## 任务三：回归、安全和mock验证

**验证范围：**

- `xr_rm_teleop`全部测试；
- ROS2工作空间构建；
- 统一launch的右臂mock启动；
- 最终差异与安全配置审计。

- [x] **步骤1：运行遥操作包全部测试**

```bash
source /opt/ros/humble/setup.bash
export RM75_PLACO_TEST_PATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
PYTHONPATH="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
  python3 -m pytest src/xr_rm_teleop/test -v
```

预期：全部测试通过，真实Placo收敛用例被执行。

- [x] **步骤2：按项目规则单独运行姿态控制测试**

```bash
source /opt/ros/humble/setup.bash
python3 -m pytest \
  src/xr_rm_teleop/test/test_orientation_control.py \
  -v
```

预期：全部通过。

- [x] **步骤3：构建ROS2工作空间**

```bash
source /opt/ros/humble/setup.bash
colcon build --symlink-install
```

预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`和
`xr_rm_bringup`全部构建成功。

- [x] **步骤4：通过统一入口进行右臂mock启动验证**

```bash
source /opt/ros/humble/setup.bash
source install/setup.bash
if timeout --signal=INT 10s ros2 launch \
  xr_rm_bringup arm_debug.launch.py \
  arm:=right use_mock:=true udp_port:=15123
then
  true
else
  launch_status=$?
  test "$launch_status" -eq 124
fi
```

预期：

- `udp_controller_receiver`和`single_arm_velocity_teleop`正常启动；
- 节点报告 `dt=0.0111s`、`follow=False`；
- mock关节初始化成功；
- 不导入RealMan SDK，不建立真机连接，不发送CANFD；
- 10秒后仅由 `timeout`结束。

- [x] **步骤5：最终差异和安全审计**

在 `/home/robot/WS_xr/src` 执行：

```bash
git diff --check
git status --short
git diff -- \
  xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
  xr_rm_teleop/test/test_placo_transforms.py
rg -n \
  "control_rate_hz|follow:|configure_safety_limits|move_to_initial_pose_on_connect" \
  xr_rm_bringup/config/dual_arm_rm75.yaml \
  xr_rm_bringup/config/left_arm_rm75.yaml \
  xr_rm_bringup/config/right_arm_rm75.yaml
```

预期：

- 生产代码只修改Placo求解器；
- 测试只增加真实模型收敛验证；
- 三份配置继续使用90 Hz、`follow: false`、
  `configure_safety_limits: true`和
  `move_to_initial_pose_on_connect: false`；
- 不改变此前由用户保留的 `AGENTS.md` 修改；
- 不自动提交或推送。

---

## 真机交接验收

Codex不执行本节。自动验证全部通过后，由用户在安全工作区使用：

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py \
  arm:=right use_mock:=false
```

验收步骤：

1. 急停可用、Grip松开、工作区无人后启动。
2. 按住Grip，快速移动手柄约10 cm后保持不动。
3. 机械臂应在约1秒内稳定到位，无持续肉眼可见晃动。
4. 连续观察四个5秒 timing 窗口，`total max`均低于11.111 ms。
5. 不应出现QP未收敛、反馈超时、CANFD错误或故障锁存日志。
6. 松开Grip后机械臂按现有逻辑安全停止。

若任一窗口 `total max`达到或超过11.111 ms，或机械臂出现明显振荡，立即松开
Grip并停止测试，把完整timing和错误日志返回后再调整；不得直接提高控制频率、
关闭限速或改成高跟随。