acRealman_xr/docs/superpowers/specs/2026-07-30-rm75-joint-command-braking-design.md

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；
• 新增本中文设计文档；
• 后续新增一份中文实施计划。