# RM75 QP 收敛与低跟随稳定性优化设计

## 背景

右臂真机以90 Hz、`follow: false`运行时，用户观察到：

- 手柄移动约10 cm后，`target_pose`很快稳定；
- `current_pose`仍需约3秒缓慢追赶；
- 运动过程中机械臂存在肉眼可见的轻微晃动。

现场 timing 日志同时表明：

- 控制周期约11.111 ms；
- 控制回调平均约2.6 ms，最大约6.0 ms；
- QP平均约0.39 ms；
- CANFD发送平均约0.21 ms；
- UDP实际关节反馈平均约25 ms一帧，即约40 Hz。

因此，控制线程、QP单次计算和CANFD调用本身没有耗尽90 Hz周期；慢速发生在
`target_pose`生成之后。

## 根因

当前 `PlacoIkSolver.solve()` 每次只调用一次：

```python
self._solver.solve(True)
```

该调用把一次QP增量应用为 `q + Δq`。与此同时，90 Hz控制循环每次都会先用
最新实际关节反馈重置Placo模型。由于实际反馈约40 Hz，同一帧反馈通常会被重复
使用两到三次。

结果是每次下发的关节目标只位于实际关节角前方一小步，而不是当前TCP目标对应的
收敛关节解。低跟随控制器持续追逐这个短距离移动点，表现为：

- 对稳定TCP目标呈缓慢的渐近追赶；
- 实际反馈每约25 ms更新一次时，关节目标随反馈发生台阶式修正；
- 低跟随内部平滑与台阶式关节目标叠加，形成轻微晃动。

本地RM75模型对照结果支持该判断：从右臂初始姿态求解7 cm平移目标时，单次QP
只产生约7.6 mm TCP位移；在同一次逆解中连续迭代30次后，目标误差可降至接近
零，计算耗时约3.56 ms。

## 目标

保持现有安全基线并实现：

- 手柄移动10 cm后，机械臂约1秒内稳定到位；
- 运动和到位后无持续肉眼可见晃动；
- 控制频率保持90 Hz；
- `rm_movej_canfd()`保持低跟随；
- 运行时仍以UDP `joint_position`作为实际关节反馈；
- 保留工作空间、圆柱、TCP速度、姿态速度、关节速度与关节加速度限制；
- 保留反馈超时、CANFD错误恢复、Grip重新使能和安全停止逻辑。

## 不在本次范围

- 不启用高跟随；
- 不提高TCP或关节安全上限；
- 不修改XR手柄滤波和坐标映射；
- 不修改UDP反馈周期或增加反馈预测器；
- 不新增线程、RealMan连接、依赖或状态机；
- 不处理双臂碰撞检测。

## 方案比较

### 方案一：有限次数迭代QP

每个控制周期仍从实际关节角开始，但在一次 `solve()` 调用内部迭代QP，直到TCP
目标收敛或达到固定迭代上限。得到的完整关节目标继续经过现有关节速度与加速度
限幅后才发送。

优点：

- 直接修复单步QP只生成近距离移动点的根因；
- 不需要预测状态，不会在反馈中断时继续外推；
- 复用现有限速、错误回退和CANFD发送路径；
- 本地测量表明计算量可放入90 Hz周期。

缺点：

- 单周期QP耗时会高于当前单步求解；
- 不可达目标需要明确的未收敛处理。

### 方案二：反馈帧之间维护预测关节状态

仅在新UDP反馈到达时校正模型，其余90 Hz周期从上一条关节命令继续积分QP。

优点：

- 每周期仍只求解一次QP；
- 可避免同一反馈帧反复重置模型。

缺点：

- 引入预测状态、反馈校正和漂移处理；
- 反馈与预测偏差可能在校正时产生新的关节跳动；
- 超时与恢复逻辑需要同时管理实际状态和预测状态。

### 方案三：只调整滤波、速度或高跟随参数

`target_pose`已经快速稳定，继续提高 `max_linear_speed` 或减小目标滤波不能解决
下游渐近追赶。启用高跟随则违反本次低跟随约束。

## 决策

采用方案一。它在不引入预测状态的情况下直接修复根因，改动范围只涉及Placo
求解器及其测试。

## 控制数据流

正常运行时的数据流调整为：

```text
UDP实际关节反馈
→ 更新Placo实际关节状态和current_pose
→ 现有XR相对位姿、工作空间、圆柱、滤波和TCP限速
→ 有限次数迭代QP，得到收敛关节目标
→ 现有关节速度与加速度限幅
→ rm_movej_canfd(..., follow=false)
```

反馈短暂超时仍只以90 Hz重发最后一次已通过限速的关节目标，不运行QP。反馈持续
超时和CANFD错误仍沿用现有同步、停止与故障锁存逻辑。

## QP迭代规则

`PlacoIkSolver.solve()`按以下规则执行：

1. 校验目标变换。
2. 记录本次内部迭代前的关节状态。
3. 调用一次 `self._solver.solve(True)`。
4. 更新Placo运动学。
5. 校验本次候选关节状态：
   - 7个有限数值；
   - 不违反RM75关节位置限制；
   - 本次数值迭代步长不超过Placo按 `dt=1/90` 应用的URDF关节速度限制。
6. 使用位置任务与姿态任务的 `error_norm()`检查收敛：
   - 位置误差不超过1 mm；
   - 姿态误差不超过0.005 rad。
7. 未收敛则继续迭代，最多30次。

30次后仍未收敛，或任一迭代产生非法结果时，抛出异常。节点复用现有
`_solve_joint_target()`错误路径，在终端限频打印QP失败原因，并保持上一组安全
关节目标。

内部迭代得到的是逆解目标，不会直接绕过发送限速。最终下发仍必须经过
`_limit_joint_command_step()`，因此每个90 Hz真实命令继续满足现有
`joint_max_speed` 和 `joint_max_acc`。

## 晃动抑制

本次不再叠加新的低通滤波器。晃动通过两层现有机制抑制：

1. QP先收敛到当前TCP目标对应的关节解，避免关节目标随40 Hz反馈只前进一小步；
2. 最终关节目标由现有关节速度与加速度限幅器生成连续90 Hz命令。

若真机仍存在晃动，再根据“目标关节角与实际关节角误差”追加诊断；本次不预先
引入预测器或额外滤波。

## 性能与安全验收

自动验证：

- 7 cm可达TCP平移目标在一次 `solve()` 后位置误差不超过1 mm；
- 姿态误差满足0.005 rad阈值；
- 非法结果和未收敛目标继续触发现有安全回退；
- 关节命令速度与加速度限幅测试继续通过；
- `xr_rm_teleop`全部pytest通过；
- `colcon build --symlink-install`通过；
- `arm_debug.launch.py arm:=right use_mock:=true`正常启动。

真机由用户验证：

- 手柄快速移动10 cm并保持不动，机械臂约1秒内稳定；
- 无持续肉眼可见晃动；
- 连续四个5秒 timing 窗口中 `total` 最大值低于11.111 ms；
- 无QP失败、反馈超时、CANFD错误或故障锁存日志；
- 松开Grip后仍立即退出遥操作并执行安全停止。

若单周期 `total` 达到或超过11.111 ms，停止真机运动并降低最大QP迭代次数，
不得通过提高控制频率或关闭安全检查规避计算超时。

## 文件范围

- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`。
- 修改 `xr_rm_teleop/test/test_placo_transforms.py`。
- 如现有QP失败测试需要补充未收敛原因断言，只精确修改
  `xr_rm_teleop/test/test_joint_control.py`。

不修改三份机械臂YAML、RealMan适配器、launch、UI、依赖或公开入口。