# RM75 QP 阈值与 UDP 反馈周期修复设计

## 背景

右臂真机在 90 Hz、`follow: false` 遥操过程中频繁出现两类警告：

```text
QP did not converge after 30 iterations:
position_error=0.001245~0.001609 m

UDP关节反馈超时，保持最后安全目标。
```

现场 timing 日志同时表明：

- 控制回调最大约 5.1 ms，没有耗尽 11.111 ms 周期；
- UDP 反馈间隔均值约 25 ms，即实际约 40 Hz；
- UDP 间隔存在 37~71 ms 的明显抖动；
- UDP 短暂超时后约 44 ms 收到新帧，但现有安全状态机要求先松开 Grip。

因此，控制线程计算量不是这两类警告的原因。

## 根因

### QP 近阈值失败

当前 QP 最多迭代 30 次，并要求：

```text
位置误差 <= 1 mm
姿态误差 <= 0.005 rad
```

现场失败时姿态误差约 0.0007 rad，已经满足要求；位置误差仅比 1 mm 高
0.245~0.609 mm。30 次迭代中的每一步已经通过关节有限值、关节位置限制和单步
速度限制校验，但最终结果仍因严格的 1 mm 判定被整体丢弃。

用户已明确确认 2 mm 位置残差可接受；该数值也与现有 1 mm 手柄位置死区处于
同一量级。将位置收敛阈值改为 2 mm，可以接收现场这类安全的近收敛
结果，同时继续拒绝此前出现过的 7.5 mm 等明显未收敛结果。

### UDP 周期单位错误

项目参数 `realtime_push_cycle_ms` 的单位是毫秒，三份机械臂配置均填写 `5`。
当前适配器把这个值原样传给：

```python
rm_realtime_push_config_t(cycle, ...)
```

但睿尔曼 SDK 的 `cycle` 单位不是毫秒，而是 5 ms 的倍数。因此：

```text
当前传入 cycle=5
实际周期 = 5 × 5 ms = 25 ms
实际频率 = 40 Hz
```

这与现场 `feedback_interval mean≈25 ms` 完全一致。期望 5 ms 上报时，SDK
参数应为 `cycle=1`。

在 120 ms 反馈超时窗口内，25 ms 上报只有约 5 次发送机会；修正为 5 ms 后有
约 24 次发送机会，能显著提高对偶发丢包和调度抖动的容忍度。若网络或 SDK
回调整体停顿超过 120 ms，仍应触发现有安全超时。

## 目标

- QP 位置收敛阈值由 1 mm 调整为 2 mm；
- `realtime_push_cycle_ms: 5` 实际配置成 SDK `cycle=1`，恢复 5 ms 上报；
- UDP 超时警告打印触发时的实际反馈年龄；
- 保持控制频率 90 Hz 和 `follow: false`；
- 保持 UDP 短超时保持、持续超时重同步、Grip 重使能、CANFD 恢复和故障锁存；
- 保留工作空间、圆柱、TCP、姿态和关节安全限制。

## 不在本次范围

- 不增加 QP 最大迭代次数；
- 不修改姿态收敛阈值；
- 不放宽 `command_timeout_sec=0.12`；
- 不修改 `feedback_resync_timeout_sec=0.5`；
- 不允许短超时后自动恢复 Grip；
- 不修改三份机械臂 YAML；
- 不修改 UDP 目标 IP、端口、线程模式或增加新连接；
- 不处理网卡、交换机或控制器固件问题；
- 不修改用户当前未提交的 `right_arm_rm75.yaml` 参数调整。

## 修改方案

### QP 收敛

在 `placo_ik_solver.py` 中只修改：

```python
QP_POSITION_TOLERANCE_M = 2e-3
```

30 次迭代、逐步关节安全校验、姿态阈值和未收敛异常格式保持不变。

### UDP 周期换算

保留公开参数 `realtime_push_cycle_ms` 的毫秒语义和“正数且为 5 ms 倍数”的现有
校验。构造 SDK 配置时执行：

```python
sdk_cycle = self._realtime_push_cycle_ms // 5
```

示例：

| 项目参数 | SDK `cycle` | 实际周期 |
|---:|---:|---:|
| 5 ms | 1 | 5 ms |
| 10 ms | 2 | 10 ms |
| 25 ms | 5 | 25 ms |

启动日志仍打印毫秒值，避免把 SDK 内部单位暴露为用户配置。

### UDP 超时日志

首次进入短暂超时时打印：

```text
right_rm75 UDP关节反馈超时（age=xxx.x ms），保持最后安全目标。
```

日志只增加诊断值，不改变节流、保持目标、QP 停止和 Grip 重使能行为。

## 数据流与安全

正常路径：

```text
YAML 5 ms
→ 适配器换算 SDK cycle=1
→ 控制器约每 5 ms UDP 上报
→ 回调校验并缓存 joint_position
→ 90 Hz 控制读取最新实际反馈
→ 最多 30 次 QP，位置阈值 2 mm
→ 现有关节提前制动限幅
→ rm_movej_canfd(..., follow=false)
```

反馈年龄超过 120 ms 时仍停止生成新目标和 QP，以 90 Hz 重发最后安全目标，并
要求 Grip 松开后重新使能。达到 500 ms 时仍只尝试一次
`rm_get_joint_degree()`；失败时仍停止并锁存故障。

## 测试与验证

自动测试：

1. 增加 QP 1.5 mm 位置残差的行为测试，验证其被 2 mm 阈值接受；
2. 保留明显未收敛结果抛出异常的行为；
3. 修改 SDK 配置测试，验证项目 5 ms 参数传入 SDK 时为 `cycle=1`；
4. 增加 10 ms 到 `cycle=2` 的换算覆盖；
5. 验证 UDP 超时日志包含实际反馈年龄；
6. 运行 `xr_rm_teleop` 全部测试和姿态控制测试；
7. 运行 `colcon build --symlink-install`；
8. 使用 `arm_debug.launch.py arm:=right use_mock:=true`验证启动路径。

真机由用户验证：

- 连续 timing 窗口中 `feedback_interval mean` 从约 25 ms 降到接近 5 ms；
- 正常遥操不再频繁出现 UDP 超时；
- 位置残差小于 2 mm 时不再出现 QP 未收敛警告；
- 真正超过 120 ms 的反馈中断仍打印带 `age` 的警告并执行现有安全保持；
- 若修正后仍频繁出现超过 120 ms 的中断，再依据 `age` 和间隔数据排查网络、
  SDK 回调或控制器固件，不继续盲目放宽超时。

## 文件范围

- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`；
- 修改 `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`；
- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`；
- 修改相关现有测试；
- 新增本中文设计文档和后续中文实施计划；
- 不修改 YAML、launch、UI 或依赖。