feat: Implement absolute scheduling for feedback loop and enhance tool frame configuration

2026-07-29 09:44:51 +08:00
parent 2a12eea4d5
commit 687a0b401a
11 changed files with 876 additions and 21 deletions
@@ -0,0 +1,131 @@
+# RM75 反馈调度与跟随速度优化设计
+
+## 目标
+
+在保留 Placo 单步 QP、工作空间与圆柱限位、关节速度限制、指令超时和安全
+停止的前提下，分阶段解决 PICO 遥操机械臂跟随速度很慢的问题。
+
+每阶段只改变一个控制因素，真机验证通过后才进入下一阶段：
+
+1. 提高关节反馈的新鲜度；
+2. 启用 `rm_movej_canfd` 高跟随完全透传；
+3. 将右臂单独调试 TCP 速度提高到 `0.2 m/s`。
+
+## 真机证据
+
+右臂在 125 Hz、低跟随模式下连续四个约 5 秒窗口的结果为：
+
+- `period mean=8.000 ms`，四个窗口最大值为 `9.023–9.424 ms`；
+- `total mean=1.908–2.004 ms`，最大值不超过 `4.109 ms`；
+- `qp mean=0.230–0.245 ms`；
+- `send mean=0.122–0.127 ms`；
+- `feedback_read mean=9.101–9.630 ms`；
+- `feedback_interval mean=17.368–17.918 ms`；
+- `feedback_age mean=9.079–9.632 ms`，最差达到 `46.480 ms`。
+
+每个窗口只有 `279–288` 次新反馈，即实际反馈频率约为 `56–58 Hz`。
+`feedback_interval - feedback_read` 在四个窗口中稳定为 `8.27–8.39 ms`，
+确认当前反馈线程把一次 SDK 查询耗时和完整的 8 ms 等待串联起来：
+
+```text
+当前更新间隔 = rm_get_joint_degree 调用耗时 + 8 ms 固定等待
+```
+
+控制回调、QP 和发送均有充足余量，不是当前反馈慢的原因。
+
+## 阶段一：反馈线程绝对周期调度
+
+### 调度语义
+
+将当前“读取完成后固定等待 8 ms”改为“读取起始时间之间以 8 ms 为目标”：
+
+```text
+读取耗时 < 8 ms：只等待剩余时间
+读取耗时 ≥ 8 ms：不再额外等待，从当前时间重新建立周期基准
+```
+
+调度不补跑已经错过的历史周期。一次长阻塞结束后最多立即开始下一次读取，
+不会为了追赶多个旧截止点而密集补调用 SDK。
+
+等价的目标启动间隔为：
+
+```text
+max(8 ms, 本次反馈读取耗时)
+```
+
+当前读取平均约 9.3 ms，因此预期反馈频率接近 `100 Hz`，但不强求达到
+`125 Hz`。
+
+### 范围
+
+本阶段只修改 `RealManAdapter._feedback_loop()` 的等待计算。以下内容保持不变：
+
+- `follow=false`；
+- `canfd_trajectory_mode=2`；
+- 右臂单独调试 `max_linear_speed=0.15 m/s`；
+- 125 Hz ROS 控制定时器和 Placo QP；
+- 同一个 RealMan 连接承担反馈与发送，不新增连接；
+- 所有安全限位、超时和停止逻辑。
+
+现有 `feedback_read`、`feedback_interval` 和其他 timing 指标继续保留。
+
+### 验收
+
+右臂真机连续采集四个 timing 窗口，全部满足：
+
+- `feedback_interval mean ≤ 12 ms`；
+- `feedback_interval mean - feedback_read mean ≤ 2 ms`；
+- `feedback_age mean ≤ 7 ms`；
+- `period max < 10 ms`；
+- `total max < 8 ms`；
+- 无反馈超时、异常停止或 SDK 发送错误。
+
+若更密集的反馈查询使 `period max` 达到或超过 10 ms，或明显增加发送耗时，
+停止后续高跟随阶段，继续定位同一 SDK 连接的读写竞争。
+
+## 阶段二：高跟随完全透传
+
+只有阶段一通过后才验证：
+
+- `follow=true`；
+- `canfd_trajectory_mode=0`；
+- 右臂单独调试速度仍为 `0.15 m/s`。
+
+先通过现有 launch 参数显式启用高跟随完成右臂单机验证；验证通过后，再把
+`arm_debug.launch.py` 默认值和 `dual_arm_rm75.yaml`、`left_arm_rm75.yaml`、
+`right_arm_rm75.yaml` 同步为高跟随完全透传。
+
+验收条件：
+
+- 快速移动手柄约 10 cm 后，机械臂追赶不超过 1 秒；
+- 连续四个 timing 窗口 `period max < 10 ms`；
+- 无明显振荡、跳动、反馈超时或异常停止。
+
+若仍追赶超过 1 秒，不进入加速阶段；先增加关节目标与实测关节误差统计，
+确认慢速来自 QP 单步目标还是控制器执行。
+
+## 阶段三：右臂速度提高到 0.2 m/s
+
+只有阶段二通过后，将 `right_arm_rm75.yaml` 中右臂单独调试的
+`max_linear_speed` 从 `0.15` 提高到 `0.2 m/s`。
+
+- `dual_arm_rm75.yaml` 的左右臂已经是 `0.2 m/s`，无需修改；
+- 左臂单独调试速度保持现状；
+- 右臂真机 `max_line_speed=0.25 m/s` 安全上限保持不变。
+
+右臂 `scale=0.7` 时，手柄移动 10 cm 对应约 7 cm TCP 目标，理论限速时间约
+0.35 秒。验收追赶时间不超过 0.7 秒，并确认没有明显振荡或限位异常。
+
+## 测试与交付
+
+阶段一实现采用测试先行：
+
+- 用确定性时钟和停止事件验证短读取只等待剩余时间；
+- 验证读取超期后不额外等待，也不补跑多个历史周期；
+- 运行 `xr_rm_teleop` 全部 pytest；
+- 运行 `test_orientation_control.py`；
+- 在 `/home/robot/WS_xr` 运行 `colcon build --symlink-install`。
+
+Codex 不连接真机、不移动机械臂。每个阶段的真机验证由用户通过
+`xr_rm_bringup/launch/arm_debug.launch.py` 在右臂、小范围动作下完成，并把连续
+四个完整 timing 窗口返回后再进入下一阶段。
@@ -0,0 +1,53 @@
+# RM75 关节反馈线程计时统计设计
+
+## 目标
+
+在不改变关节反馈轮询、QP、关节指令和安全停止行为的前提下，测清当前
+RealMan 反馈线程的两个关键时间：
+
+- `rm_get_joint_degree()` 单次调用耗时；
+- 相邻两次成功写入关节反馈缓存的实际更新间隔。
+
+本轮只增加统计。高跟随、TCP 速度、反馈调度和 QP 控制方式均保持现状，待
+真机日志确认根因后再修改。
+
+## 方案
+
+`RealManAdapter` 在 `_read_joint_state_once()` 中使用单调高精度时钟记录：
+
+- `feedback_read`：从调用 `rm_get_joint_degree()` 前到调用返回后的耗时；
+- `feedback_interval`：本次成功反馈时间戳与上次成功反馈时间戳之差。
+
+两个数值随 `JointStateSnapshot` 写入现有线程安全缓存。首次成功反馈没有可靠
+的前序时间戳，因此不提供 `feedback_interval`。
+
+`SingleArmVelocityTeleop` 只在看到新的反馈时间戳时，将这两个数值各记录一次，
+避免 125 Hz 控制循环重复读取同一缓存而造成重复统计。统计加入现有约 5 秒
+timing 窗口，并输出各自的样本数、mean、P95、P99 和 max：
+
+```text
+feedback_read[n=<样本数> mean=<均值> p95=<P95> p99=<P99> max=<最大值> ms]
+feedback_interval[n=<样本数> mean=<均值> p95=<P95> p99=<P99> max=<最大值> ms]
+```
+
+读取失败不产生成功样本，继续沿用现有一次告警、反馈超时和安全停止逻辑。
+Mock 模式不伪造厂商 API 调用耗时。
+
+## 验证
+
+- 扩展现有关节控制单元测试，使用确定性快照验证新反馈只统计一次、重复缓存
+  不重复计数、首次反馈没有更新间隔。
+- 运行 `xr_rm_teleop` 相关 pytest。
+- 按项目规则在工作空间根目录运行 `colcon build --symlink-install`。
+- 真机测试仍由用户使用 `arm_debug.launch.py arm:=right use_mock:=false` 执行；
+  本轮不自动连接机械臂。
+
+## 后续决策
+
+用户提供真机 timing 日志后再判断：
+
+- 若 `feedback_interval` 主要由 `feedback_read + 8 ms` 构成，再评估绝对周期
+  调度；
+- 若 `feedback_read` 本身经常超过 8 ms，优先定位厂商查询或同一连接的读写
+  竞争；
+- 反馈问题确认前，不把高跟随或预测式 QP 与本轮统计改动混在一起。
@@ -0,0 +1,61 @@
+# RM75 工具坐标系幂等配置设计
+
+## 目标
+
+修复遥操作节点每次启动都无条件创建 RealMan 工具坐标系、忽略重复名称错误，
+随后仍误报“外设配置完成”的问题。
+
+本修复只处理控制器工具坐标系的创建、更新、切换和返回值检查，不修改夹爪
+IO、Modbus、Placo、URDF 或任何机械臂运动控制参数。
+
+## 根因
+
+右臂 `scissorgripper: 1` 选择 `peripherals_rm75.yaml` 中的 `omnipic`。
+`configure_peripheral_on_connect` 默认为 `true`，因此节点每次启动都会进入
+`peripheral_cfg()`。
+
+当前实现无条件执行：
+
+```python
+robot.rm_set_manual_tool_frame(frame=tool_frame)
+robot.rm_change_tool_frame(tool_name)
+```
+
+RealMan 控制器会持久保存工具坐标系。首次启动创建成功，后续启动因
+`omnipic` 已存在而创建失败。两个返回值均未检查，因此代码继续执行并输出
+配置成功日志；若 YAML 中的 TCP、重量或重心已变化，控制器仍可能保留旧值。
+
+## 方案
+
+在 `fun_peripheral.py` 中增加一个小型内部函数，负责单一工具坐标系的幂等
+配置：
+
+1. 调用 `rm_get_total_tool_frame()` 获取现有工具坐标系名称并检查
+   `return_code`。
+2. 若目标名称不存在，调用 `rm_set_manual_tool_frame()`。
+3. 若目标名称已存在，调用 `rm_update_tool_frame()`。
+4. 检查创建或更新返回值。
+5. 调用 `rm_change_tool_frame()` 并检查返回值。
+
+任一步失败都抛出包含 SDK 操作名称和返回码的 `RuntimeError`。异常沿现有
+节点初始化链路向上传播，因此不会继续误报“外设配置完成”。
+
+不通过“先删除再创建”实现更新，避免在切换中的控制器上产生短暂无工具
+坐标系状态。
+
+## 测试
+
+在现有外设相关测试文件中使用 FakeArm 覆盖：
+
+- 名称不存在时只调用创建，然后切换；
+- 名称存在时只调用更新，然后切换；
+- 查询、创建/更新或切换失败时抛出明确错误。
+
+随后运行：
+
+- `xr_rm_teleop` 全部 pytest；
+- `test_orientation_control.py`；
+- `/home/robot/WS_xr` 下的 `colcon build --symlink-install`。
+
+Codex 不连接真机。真机验证由用户重新启动右臂 launch，确认不再出现
+`Failed to create the tool frame system`，并能看到外设配置完成日志。