feat: Implement RM75 joint feedback and fault recovery design

docs/superpowers/specs/2026-07-29-rm75-feedback-recovery-design.md

@@ -0,0 +1,151 @@
+# RM75 关节反馈与故障恢复设计
+
+## 目标
+
+将当前 RM75 QP 遥操作链路调整为：
+
+- 启动时使用 `rm_get_joint_degree()` 获取实际关节角并初始化 QP。
+- 运行时只把 RealMan UDP `joint_position` 作为连续实际关节反馈。
+- UDP 短暂超时时保持最后一次已限速的安全关节目标，不生成新运动。
+- UDP 持续超时或 CANFD 发送错误时，按明确的同步、停止和人工恢复流程处理。
+- `rm_movej_canfd()` 保持低跟随，控制频率使用 `xr_rm_teleop` 分支的 90 Hz。
+
+不改变现有工作空间/圆柱限位、TCP与关节速度和加速度限制、XR命令超时、安全停止、外设控制及双臂节点名。
+
+## 控制数据源
+
+启动初始化与运行反馈使用不同的数据源：
+
+1. `RealManAdapter` 建立现有唯一厂商连接。
+2. 节点同步调用一次 `rm_get_joint_degree()`。
+3. 查询成功后把7个角度转换为弧度，用于初始化 Placo QP、最后安全目标和关节限速历史。
+4. 查询失败时关闭适配器、打印错误并使节点启动失败，不发送 CANFD。
+5. 正常运行后，QP 的连续实际状态只来自已校验且运动状态正常的 UDP `joint_position`。
+
+同步查询只用于启动、持续反馈超时恢复和 CANFD 错误恢复，不新增连接，不进行常态轮询。
+
+## 状态与转换
+
+### 正常运行
+
+控制定时器以 90 Hz 执行。每个周期读取最新 UDP 关节快照，同步 QP，执行现有目标生成、安全限位、单步 QP、关节速度/加速度限制，然后调用：
+
+```text
+rm_movej_canfd(target_degrees, follow=false, ...)
+```
+
+只有已经通过关节限速并成功发送的目标才能成为“最后安全目标”。
+
+### UDP 短暂超时
+
+UDP 快照年龄超过现有 `command_timeout_sec=0.12` 秒、但未达到 `feedback_resync_timeout_sec=0.5` 秒时：
+
+- 不使用过期反馈同步 QP。
+- 不运行目标生成和 QP。
+- 不更新任何目标、滤波器或限速历史。
+- 若超时前正在遥操作且已有成功发送的安全目标，以 90 Hz 原样重发该目标。
+- 若超时前未在遥操作或没有成功发送的目标，保持停止，不开始 CANFD 输出。
+- 首次进入时打印节流后的警告。
+
+机械臂报警、关节掉使能、关节错误或非有限关节值不是普通超时，仍立即执行安全停止。
+
+### UDP 持续超时
+
+UDP 快照年龄达到 0.5 秒时，每次中断只同步调用一次 `rm_get_joint_degree()`：
+
+- 查询成功：用实际角度重置 QP、最后安全目标和关节限速历史；不生成新运动，继续保持并等待 UDP 恢复。
+- 查询失败：调用 slow-stop，停止 CANFD，进入锁存故障并打印错误。
+
+同一次中断不会反复查询。收到新的有效 UDP 反馈后，查询标志才复位。
+
+### UDP 恢复
+
+UDP 恢复后先持续同步实际关节状态，但不能直接恢复运动：
+
+1. 当前 Grip 必须松开。
+2. 节点清除重新使能要求。
+3. 操作者再次按下 Grip，节点以新的手柄和机械臂实际位姿建立相对控制起点。
+
+### CANFD 错误
+
+`rm_movej_canfd()` 返回错误或抛出异常时：
+
+1. 立即停止后续 CANFD 发送。
+2. 调用 slow-stop。
+3. 打印包含机械臂名称、命令名称和原始错误的终端错误日志。
+4. 调用 `rm_get_joint_degree()` 查询实际关节角。
+5. 查询成功时重置 QP和关节命令历史，但不再发送保持命令；等待有效 UDP 和 Grip 松开后重新按下。
+6. 查询失败时进入锁存故障并打印查询错误。
+
+### 锁存故障
+
+锁存故障只作用于发生错误的机械臂节点：
+
+- 控制定时器不再查询、运行 QP或发送 CANFD。
+- slow-stop 只发送一次。
+- 后续 UDP 恢复或 Grip 操作不能自动解锁。
+- 终端保留明确错误信息，但不在每个周期重复刷屏。
+- 操作者检查后必须重启对应遥操作节点才能恢复。
+
+## 代码边界
+
+### `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
+
+- 给真实与 mock 适配器增加同步关节角查询能力。
+- 复用现有厂商连接。
+- 校验返回码、数量和有限值，统一返回弧度。
+- 保留 UDP 回调作为运行时快照来源。
+
+### `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- 启动时查询并初始化 QP。
+- 增加 `feedback_resync_timeout_sec` 参数，默认 0.5 秒。
+- 校验 `feedback_resync_timeout_sec > command_timeout_sec > 0`。
+- 在现有控制周期内加入保持、一次性重新同步、等待 Grip 重使能和锁存判断。
+- 复用现有 `_safe_stop()`、Grip 重使能和关节限速逻辑，不新增状态机类。
+
+### 配置
+
+以下配置的 `control_rate_hz` 从 125 Hz 改为 90 Hz，并增加相同的 0.5 秒持续超时参数：
+
+- `xr_rm_bringup/config/dual_arm_rm75.yaml`
+- `xr_rm_bringup/config/left_arm_rm75.yaml`
+- `xr_rm_bringup/config/right_arm_rm75.yaml`
+
+三份配置继续使用 `follow: false`，双臂节点名保持 `left_arm_teleop` 和 `right_arm_teleop`。
+
+## 错误日志
+
+以下转换必须写入 ROS2 终端日志：
+
+- 启动关节查询失败：`error`。
+- 首次进入 UDP 短暂超时：`warn`。
+- 持续超时查询开始及成功：`warn`/`info`。
+- 持续超时查询失败并锁存：`error`。
+- CANFD 发送失败：`error`。
+- CANFD 后关节查询失败并锁存：`error`。
+- UDP 恢复并等待 Grip 人工重使能：`info`。
+
+日志包含机械臂名称和失败阶段；周期性路径使用状态转换或节流避免刷屏。
+
+## 测试与验证
+
+使用现有 mock 和单元测试完成，不连接真机：
+
+1. 适配器正确解析 `rm_get_joint_degree()` 成功结果，并拒绝错误码、错误数量和 NaN/Inf。
+2. 启动查询结果初始化 QP 和安全目标；查询失败时节点不能进入控制。
+3. 0.12～0.5 秒反馈超时期间不调用 QP，只重发同一安全目标。
+4. 0.5 秒持续超时只查询一次；成功后等待 UDP 与 Grip，失败后锁存。
+5. CANFD 错误后停止发送并查询；查询成功要求 Grip 重使能，查询失败锁存。
+6. 机械臂报警或掉使能仍立即停止，不能进入保持路径。
+7. 三份配置均使用 90 Hz、0.5 秒持续超时和低跟随。
+
+在工作空间根目录 `/home/robot/WS_xr` 执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_initial_joint_pose.py
+pytest src/xr_rm_teleop/test/test_joint_control.py
+pytest src/xr_rm_teleop/test/test_orientation_control.py
+colcon build --symlink-install
+```

docs/superpowers/specs/2026-07-29-rm75-qp-convergence-design.md

@@ -0,0 +1,192 @@
+# 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、依赖或公开入口。