# RM75 关节命令提前制动设计

## 背景

右臂真机保持 90 Hz 和 `follow: false`。有界迭代 QP 提高跟随速度后，手柄上下
移动约 10 mm 时出现整臂剧烈晃动，并且到达目标后仍持续振荡。

现场日志表明控制计算未超时，但存在少量 QP 未收敛警告。离线检查确认：

- 手柄上下移动按现有映射对应机器人 X 方向；
- 当前初始姿态的关节雅可比条件数约为 116，该方向的逆解对关节运动较敏感；
- 机器人 X 方向 10 mm 的收敛逆解可能包含最大约 17° 的关节变化；
- 现有关节命令限幅器只限制速度和加速度，没有根据剩余距离提前制动。

固定 10° 关节目标的离线复现中，现有限幅器运行 3 秒后仍处于约 10.56°、
-20°/s，证明稳定目标本身也会被反复越过。这与真机“到位后继续晃动”的现象
一致。

## 目标

- 关节目标稳定后，90 Hz 关节命令提前减速并停止在目标上；
- 不再因命令限幅器反复越过目标而持续振荡；
- 保留当前较快的有界迭代 QP；
- 保留现有关节最大速度和最大加速度限制；
- 保持 `rm_movej_canfd(..., follow=false)`；
- 不改变 UDP 反馈、超时保持、CANFD 错误恢复和安全停止行为。

## 不在本次范围

- 不修改 QP 迭代次数、收敛阈值或失败回退；
- 不修改初始姿态、XR 坐标映射或姿态控制；
- 不增加奇异点阻尼、预测器、新线程、新依赖或新 ROS 参数；
- 不修改三份机械臂 YAML、RealMan 适配器、launch 或 UI；
- 不连接真机，不由 Codex 发送运动命令。

若修复制动后机械臂运动已经平稳，但上下运动仍伴随不可接受的整臂大幅构型变化，
再单独设计奇异点处理；本次不把两个问题混在同一改动中。

## 方案

只修改 `SingleArmVelocityTeleop._limit_joint_command_step()`。

现有逻辑在目标仍位于运动方向前方时持续加速，只有到达或越过目标后才开始反向
减速。新逻辑对每个关节使用同一组现有状态：

- 上一次已发送的关节目标；
- 上一次关节命令速度；
- 当前 QP 关节目标；
- 现有关节最大速度、最大加速度和控制周期。

每周期按以下规则生成命令：

1. 计算关节剩余距离和单周期最大速度变化
   `velocity_step = max_acceleration * dt`。
2. 根据 90 Hz 离散积分规则，计算“本周期再加速一次、随后以最大允许减速度制动”
   所需的总距离。
3. 若关节正在朝目标运动，并且剩余距离已经不大于该制动距离，则本周期开始减速；
   否则继续朝目标加速，但不超过现有最大速度。
4. 使用 `velocity_step` 限制本周期速度变化，保持现有加速度上限。
5. 使用新速度积分得到本周期关节目标。
6. 当关节已经停下且剩余距离不超过一个最大加速度位移
   `max_acceleration * dt²` 时，在不违反单周期加速度限制的前提下精确落到目标，
   避免离散步长形成极小往复振荡。

该逻辑只负责命令轨迹制动，不改变 QP 输出。对于持续移动的目标，若目标突然越过
当前命令位置，控制器仍优先遵守加速度限制，以最大允许减速度反向；不会为禁止
瞬时越界而跳变速度。

## 安全行为

- 每周期命令速度绝对值不超过 `joint_max_speed`；
- 相邻周期间速度变化不超过 `joint_max_acc * dt`；
- 输出继续校验 NaN 和 Inf；
- QP 异常仍保持上一组安全目标并在终端限频打印警告；
- UDP 短暂超时仍以 90 Hz 重发最后一次已限速目标，不运行 QP；
- UDP 持续超时、CANFD 错误、Grip 重新使能和安全停止逻辑保持不变。

## 测试

先增加失败测试，再修改实现：

1. 对固定 10° 关节目标连续运行限幅器，验证命令不越过目标并最终停止；
2. 在整个序列中验证速度不超过现有上限；
3. 验证相邻命令速度变化不超过现有加速度上限；
4. 保留现有“从静止开始限制首周期加速度”测试；
5. 运行 `xr_rm_teleop` 全部测试；
6. 按项目规则运行 `colcon build --symlink-install`；
7. 使用 `arm_debug.launch.py arm:=right use_mock:=true`验证 90 Hz、低跟随和启动路径。

真机只由用户分阶段验证：先小位移、低风险姿态，确认不再到位后持续振荡，再逐步
增加位移。若仍有明显整臂构型变化但不再振荡，应停止扩大位移并转入奇异点处理，
不得通过提高速度、加速度或启用高跟随规避。

## 文件范围

- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`；
- 修改 `xr_rm_teleop/test/test_joint_control.py`；
- 新增本中文设计文档；
- 后续新增一份中文实施计划。