152 lines
6.3 KiB
Markdown
152 lines
6.3 KiB
Markdown
# 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
|
||
```
|