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,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`，并能看到外设配置完成日志。