feat: Implement RM75 joint feedback and fault recovery design

2026-07-29 19:39:12 +08:00
parent f795c06d44
commit 6d22d5600a
12 changed files with 1800 additions and 108 deletions
@@ -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
+```