Remove outdated design documents for RM75 control and feedback systems

2026-07-29 16:24:07 +08:00
parent 08996434e5
commit f795c06d44
16 changed files with 3 additions and 4120 deletions
@@ -1,284 +0,0 @@
-# RM75 Placo 单步 QP 逆解设计
-
-日期：2026-07-27
-状态：已批准，等待实施
-
-## 1. 目标
-
-将 `single_arm_velocity_teleop` 当前通过 `rm_movep_canfd` 调用睿尔曼控制器内部逆解的链路，替换为独立的 Placo QP 逆解：
-
-1. 每个 90 Hz 控制周期读取最新实际关节角。
-2. 将本周期工具 TCP 目标交给 Placo。
-3. 每周期只调用一次 `solver.solve(True)`。
-4. 得到 7 个目标关节角后，通过 `rm_movej_canfd(..., follow=False)` 控制 RM75。
-
-实现借鉴 XRoboToolkit 真机示例的控制理念，但不依赖或复制 XRoboToolkit 项目代码。首轮分别独立验证左臂和右臂 RM75；实现本身继续支持现有左臂、右臂和双臂启动方式。
-
-## 2. 不在本次范围内
-
- 不采用上传示例中的 Pinocchio + OSQP 多轮迭代逆解实现。
- 不新增独立 ROS2 QP 求解节点。
- 不改变 XR 输入协议、控制器话题或左右臂节点名。
- 不关闭现有工作空间、圆柱、速度、超时或安全停止逻辑。
- 不由 Codex 连接或移动真实机械臂、夹爪。
- 不在本轮验证左右臂同时运行的双臂真机模式。
-
-## 3. 选定方案
-
-在 `xr_rm_teleop` 中新增轻量 `PlacoIkSolver`，由每个 `single_arm_velocity_teleop` 进程持有一个实例：
-
- 遥操作节点继续负责 XR 相对位姿、滤波、死区、工作空间和速度限制。
- `PlacoIkSolver` 负责 RM75 模型、工具 TCP/法兰变换以及单步 QP。
- `RealManAdapter` 负责唯一的厂商 SDK 连接、关节反馈缓存、关节目标下发、安全停止和工具控制。
- `MockRealManAdapter` 提供同一套关节反馈和关节目标接口，不导入厂商 SDK。
-
-没有选择以下方案：
-
- 将 Placo 逻辑继续堆入已有的大型遥操作节点：改动集中，但职责更混乱且难以独立测试。
- 增加独立 QP ROS2 节点：隔离更强，但引入额外话题、时序和状态同步，对当前单机 90 Hz 控制没有必要。
-
-## 4. 启动与连接生命周期
-
-`launcher_ui.py` 不直接创建 Adapter。启动链路为：
-
-```text
-launcher_ui.py
-  -> arm_debug.launch.py (arm:=left/right/both)
-  -> 对应 single_arm_velocity_teleop 节点
-  -> 节点内部创建 PlacoIkSolver 和 Adapter
-```
-
-具体行为：
-
- `arm:=left use_mock:=false`：一个左臂节点、一个求解器、一个左臂 RealMan 连接。
- `arm:=right use_mock:=false`：一个右臂节点、一个求解器、一个右臂 RealMan 连接。
- `arm:=both use_mock:=false`：左右节点各自持有一个求解器，并各自连接对应 IP。
- `use_mock:=true`：创建 `MockRealManAdapter`，不加载厂商 SDK，不建立真机连接。
-
-每个单臂节点只调用一次 `rm_create_robot_arm`。关节反馈、`rm_movej_canfd`、慢停止和工具控制复用同一个 SDK 句柄，不为反馈建立第二条连接，也不让一条连接控制两台机械臂。
-
-## 5. RM75 模型与关节约束
-
-将上传文件中的 `RM75-B.urdf` 及其网格作为 `xr_rm_teleop` 包资源安装，不携带上传示例的 Pinocchio、OSQP 或仿真控制代码。
-
-模型约定：
-
- 固定基座：`base_link`。
- 运动关节：按 `joint_1` 到 `joint_7` 顺序映射 SDK 的 7 个关节角。
- 末端法兰帧：`link_7`。
- 节点和 Placo 内部统一使用弧度；Adapter 在 SDK 反馈/指令边界完成度与弧度转换。
-
-Placo 启用 URDF 关节位置和速度限制。上传 URDF 中的位置范围与睿尔曼官方 RM75-B 范围一致：
-
-```text
-J1 ±178°, J2 ±130°, J3 ±178°, J4 ±135°,
-J5 ±178°, J6 ±128°, J7 ±360°
-```
-
-旧的、当前未被调用的 `fun_peripheral.alg_init()` 自定义限位不作为 QP 限位来源。控制器侧现有 `configure_safety_limits`、关节最大速度和最大加速度设置继续保留。
-
-参考：
-
- [睿尔曼 RM75-B 本体参数](https://develop.realman-robotics.com/robot/robotParameter/RM75OntologyParameters/)
- [XRoboToolkit DualArmURController](https://github.com/XR-Robotics/XRoboToolkit-Teleop-Sample-Python/blob/main/xrobotoolkit_teleop/hardware/dual_arm_ur_controller.py)
-
-## 6. 工具 TCP 处理
-
-URDF 只描述到 `link_7`，实际工具来自 `peripherals_rm75.yaml`。同一份工具配置有两个使用者：
-
-```text
-peripherals_rm75.yaml
-  ├─ RealManAdapter：设置真实控制器工具坐标系和负载
-  └─ PlacoIkSolver：构造法兰到工具 TCP 的固定变换
-```
-
-当前选择为：
-
- 左臂 `scissorgripper: 2`：`minisci`，局部 Z 偏移 `+0.19 m`。
- 右臂 `scissorgripper: 1`：`omnipic`，局部 Z 偏移 `+0.16 m`。
-
-实现读取完整的 `[x, y, z, qx, qy, qz, qw]`，不硬编码为世界坐标 Z 偏移。设：
-
- `B_T_F(q)`：Placo 由关节角计算的基座到法兰变换。
- `F_T_T`：YAML 给出的法兰到工具 TCP 固定变换。
- `B_T_T_target`：经过现有安全和速度限制后的目标工具 TCP。
-
-正解和目标换算为：
-
-```text
-B_T_T(q)       = B_T_F(q) * F_T_T
-B_T_F_target   = B_T_T_target * inverse(F_T_T)
-```
-
-`F_T_T` 及其逆矩阵在启动时预计算。每周期只执行少量固定尺寸矩阵运算，不重新读取 YAML、求逆或加载 URDF。工作空间、圆柱限制、调试位姿和误差验收均以工具 TCP 为准；只有 Placo frame task 使用换算后的法兰目标。
-
-## 7. Placo 求解器
-
-每个求解器包含：
-
- 一个 `placo.RobotWrapper`。Placo 0.9.4 会为模型加入 7 个虚拟浮动基座状态，
-  因此 `robot.state.q` 长度为 14，真实 RM75 关节固定映射为
-  `robot.state.q[7:14]`。
- 一个 `placo.KinematicsSolver`，`dt = 1 / control_rate_hz`。
- 一个作用于 `link_7` 的软约束完整位姿任务。
- 一个可操作度任务。
- 一个动能正则项。
- 启用的关节位置与速度限制。
-
-RM75 基座实际固定，创建求解器后必须调用 `solver.mask_fbase(True)`，禁止 QP
-通过移动虚拟基座减小末端误差。所有状态同步和结果提取只读写
-`robot.state.q[7:14]`。
-
-初始权重沿用 XR 真机示例的最小配置：
-
-```text
-frame task:                 soft, 1.0
-manipulability task:        soft, 5e-2
-kinetic energy regularizer:       1e-6
-```
-
-每个周期先用实际关节反馈覆盖 Placo 状态并更新运动学，再设置法兰目标，最后只调用一次 `solver.solve(True)`。这里的“一步”指一次外层 Placo 求解调用；QP 求解器完成该次优化所需的内部数值迭代不算额外控制周期。
-
-Placo 0.9.4 在 RM75 全零 neutral 位形下会出现 QP `NaN`；左右臂现有实际
-初始关节角的一步求解均能得到 7 个有限结果。因此全零位形不作为启动状态或
-健康检查，必须等待首帧实际关节反馈后才能启用 QP。
-
-## 8. 90 Hz 数据流
-
-```text
-XR 相对位姿
-  -> 现有死区、滤波、工作空间/圆柱限制
-  -> 现有线速度和角速度单周期限制
-  -> 目标工具 TCP
-  -> 换算目标法兰位姿
-  -> 读取 Adapter 最新实际关节角
-  -> 同步 Placo 状态
-  -> solver.solve(True) 一次
-  -> 校验 7 个目标关节角
-  -> rad 转 deg
-  -> rm_movej_canfd(..., follow=False)
-```
-
-`RealManAdapter` 连接后在后台连续调用 `rm_get_joint_degree()`，把最新 7 关节角和单调时钟时间戳存入线程安全缓存。控制定时器只复制缓存，不在 90 Hz 回调中等待关节查询。缓存锁只保护内存数据，不包围网络调用。
-
-第一次有效反馈到达前不调用 `solver.solve(True)`，也不发送运动命令。首帧必须
-包含 7 个有限关节角且未过期；收到后将度转换为弧度写入
-`robot.state.q[7:14]`，更新运动学，把当前工具 TCP 设为初始目标，并以实际
-关节角初始化 `last_valid_joint_target`。Mock 模式使用现有
-`initial_joint_pose` 初始化 7 关节状态并立即提供同样的首帧有效反馈，再通过
-同一 Placo 正解计算工具 TCP；原先仅用于笛卡尔 mock 的
-`mock_initial_pose` 随旧控制链路移除。
-
-原 `rm_movep_canfd` 不再位于遥操作运动链路中。
-
-## 9. 异常与停止策略
-
-启动时先校验 Placo、URDF、关节顺序和工具配置，成功后才连接真机。运行时分为两类异常。
-
-### 9.1 沿用 XR 的 last-known-good 策略
-
-第一帧有效关节反馈到达后，用实际关节角初始化 `last_valid_joint_target`。
-
- QP 成功且输出通过校验：更新并发送新的 `last_valid_joint_target`。
- QP 抛出异常、返回错误维数、`NaN/Inf`，或输出违反关节位置/单周期速度限制：不更新目标，继续发送上一组有效关节目标。
- 下一周期 QP 恢复：自动恢复目标更新，不要求重新按 Grip。
- 求解失败日志限频，避免日志影响控制周期。
-
-不可达目标本身不视为求解异常；软约束任务继续在约束内每周期靠近一步。
-
-### 9.2 输入、反馈或通信不可信时慢停止
-
-以下情况不使用旧关节目标，沿用现有只发送一次慢停止并重置激活状态的逻辑：
-
- XR 指令超过现有 `command_timeout_sec`。
- Grip 松开。
- 真实关节反馈没有首帧、过期、维数错误或包含 `NaN/Inf`。
- SDK 关节指令发送失败。
- 四元数非法。
- 节点关闭。
-
-关节反馈时效先复用现有 `command_timeout_sec=0.12`，避免增加含义相近的参数。若真机测量证明正常反馈无法稳定满足该阈值，再单独拆分反馈超时参数。
-
-`configure_safety_limits` 保持启用；`move_to_initial_pose_on_connect` 的启动默认值保持 `false`。
-
-## 10. 依赖、Python 环境与配置
-
- 复用现有 `/home/robot/miniconda3/envs/xr` 环境及其中已经验证的
-  Placo `0.9.4`、Pin `3.7.0` 和 NumPy `2.2.6`，不新增 XRoboToolkit
-  项目依赖。
- `arm_debug.launch.py` 明确使用
-  `/home/robot/miniconda3/envs/xr/bin/python` 启动
-  `single_arm_velocity_teleop`；ROS2 launch 和 `colcon` 仍使用系统
-  `/usr/bin/python3`。
- 禁止升级 Placo，禁止向系统 Python、`pip --user` 或其他全局位置安装
-  Placo、Pinocchio、EigenPy 或 NumPy。构建不改用 Conda Python。
- launch 启动前校验 XR Python 路径存在；不存在时直接报错，不回退到可能
-  缺少 Placo 或版本不同的系统 Python。
- 真机模式继续按需导入睿尔曼 Python API2。
- Mock 模式依赖 Placo 和 RM75 模型，但不得导入或要求安装睿尔曼 SDK。
- 工具选择继续只由 `peripherals_rm75.yaml` 和现有 `peripheral_arm` 决定。
- 左、右、双臂 YAML 中与 QP 相关的共同配置保持一致；左右现有空间、映射和初始关节角保持各自配置。
- 将左右单臂 YAML 的 `move_to_initial_pose_on_connect` 默认值统一为 `false`，并同步 README；需要自动回初始位姿时必须由用户显式传 `true`。
- 不新增“为以后准备”的插件接口、求解器工厂或额外 ROS 消息。
-
-## 11. 验证与验收
-
-### 11.1 自动验证
-
- 工具 TCP/法兰变换可往返，包含末端旋转后的局部 Z 偏移。
- RM75 URDF 能加载，且映射顺序严格为 `joint_1` 到 `joint_7`。
- 使用 Placo 0.9.4 时固定虚拟基座，真实关节只映射
-  `robot.state.q[7:14]`。
- 没有首帧有效关节反馈时不调用 QP、不发送关节目标；首帧到达后用实际关节角
-  初始化状态和 `last_valid_joint_target`。
- 一次 QP 求解输出 7 个有限关节角并满足位置、单周期速度限制。
- 强制 QP 失败时继续使用上一组有效关节目标。
- 强制反馈过期时执行慢停止。
- Mock 模式不导入睿尔曼 SDK。
- 运行现有姿态控制测试：
-
-  ```bash
-  pytest src/xr_rm_teleop/test/test_orientation_control.py
-  ```
-
- 从工作空间根目录构建：
-
-  ```bash
-  source /opt/ros/humble/setup.bash
-  colcon build --symlink-install
-  ```
-
-### 11.2 左右臂单独 Mock 验收
-
-```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
-```
-
-左臂和右臂必须分别独立启动并完成相同验收。每个单臂目标停止变化并保持 `0.5 s` 后：
-
- 工具 TCP 位置误差不超过 `5 mm`。
- 工具 TCP 姿态误差不超过 `2°`。
- 记录 Placo 单次求解耗时和控制周期超时情况。
- 左臂使用 `minisci +0.19 m` 工具变换，右臂使用 `omnipic +0.16 m` 工具变换。
-
-90 Hz 的周期预算约为 `11.1 ms`。性能数据作为验证报告输出，不把易受机器负载影响的耗时阈值写成单元测试硬断言。
-
-### 11.3 左右臂单独真机验收
-
-Codex 分别提供 `launcher_ui.py` 左臂、右臂启动步骤和检查清单，不执行真机连接、运动或夹爪操作。用户在确认急停、障碍物、低速和初始姿态后，先只启动一侧完成验证，停止该侧节点后再验证另一侧。本轮不以 `arm:=both` 进行真机验收。两侧真机首次启动都必须保持 `move_to_initial_pose_on_connect:=false`。
-
-## 12. 完成标准
-
-满足以下条件才视为实现完成：
-
-1. 遥操作运动链路不再调用 `rm_movep_canfd`。
-2. 每个有效控制周期只有一次 Placo `solve(True)`。
-3. 目标通过 7 个关节角和 `rm_movej_canfd` 下发。
-4. 同一机械臂始终只有一个 RealMan SDK 连接。
-5. 工具 TCP 偏移参与目标换算、正解和误差验收。
-6. QP 失败使用上一组有效目标，输入/反馈/通信失败执行慢停止。
-7. 指定构建、测试以及左臂、右臂各自的 mock 验收通过。
-8. 分别提供左臂、右臂真机人工验证步骤，但不代替用户执行。
-9. 遥操作节点由 launch 显式使用 XR Python 和 Placo 0.9.4，未升级或全局安装
-   数值依赖。
@@ -1,30 +0,0 @@
-# RM75 控制周期统计设计
-
-## 目标
-
-在不改变控制、QP、安全停止和真机通信行为的前提下，确认激活遥操作时的
-125 Hz 控制链路是否满足 `8 ms` 周期。
-
-## 方案
-
-在 `SingleArmVelocityTeleop` 内使用 `time.perf_counter_ns()` 采样，仅在
-Grip 激活并执行关节命令的周期记录：
-
- 相邻控制回调的实际周期；
- 控制回调总执行时间；
- Placo QP 求解时间；
- `send_joint_target()` 调用时间；
- 当前关节反馈年龄。
-
-每累计约 5 秒激活样本，通过现有 ROS logger 输出一次汇总并清空窗口。每项
-输出样本数、mean、P95、P99 和 max；实际周期与总执行时间额外输出超过
-`self._dt` 的次数。首个激活周期没有可靠的相邻周期值，因此不记录周期。
-
-统计只输出日志，不新增 ROS 消息、话题、参数、依赖或后台线程。计时与日志
-异常不得影响控制路径。
-
-## 验证
-
-先添加一个小单元测试，使用确定性样本验证百分位数、超限计数、日志输出和
-窗口清空。随后运行相关 pytest、姿态控制测试和
-`colcon build --symlink-install`。
@@ -1,131 +0,0 @@
-# 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 窗口返回后再进入下一阶段。
@@ -1,53 +0,0 @@
-# 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 与本轮统计改动混在一起。
@@ -1,61 +0,0 @@
-# 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`，并能看到外设配置完成日志。
@@ -1,241 +0,0 @@
-# RM75 SO(3) 姿态跟随与 OmniPicker 一体化模型设计
-
-日期：2026-07-28  
-状态：已确认
-
-## 1. 背景与目标
-
-当前遥操作链路已经使用 Placo QP 将 TCP 目标转换为 RM75 七关节目标，但姿态目标在进入 QP 前会转换为 RPY，并按三个欧拉角分量进行滤波和限速。RM75 当前姿态接近俯仰角 `±90°` 时，同一个物理姿态可能切换到另一组等价 RPY，导致控制器把很小的手柄旋转解释为大角度路径，出现机械臂绕行和跟随时间过长。
-
-本设计是
-`2026-07-27-rm75-placo-qp-ik-design.md`
-的增量修改；它覆盖旧设计中的模型、TCP 变换、姿态表示、控制频率和相关验收
-内容。现有的关节反馈、单步 QP、`rm_movej_canfd`、last-known-good 和停止链路
-继续保留。
-
-本次修改的目标是：
-
-1. 姿态控制全程使用旋转矩阵和 SE(3)，按 SO(3) 最短旋转路径滤波和限速。
-2. 左右臂统一使用用户提供的 `RM75-B_OmniPicker_fixed.urdf`。
-3. 在 URDF 内定义 `omnipicker_tcp`，取消 Placo 中重复的运行时末端变换。
-4. 首轮真机测试继续使用低跟随，将控制频率设为 `125 Hz`，TCP 目标角速度上限统一为 `0.5 rad/s`。
-5. 保留现有工作空间、圆柱、关节速度、指令超时和安全停止行为。
-
-## 2. 本次不处理的内容
-
- 不开启高跟随，`follow` 继续为 `false`。
- 不改变现有 Wi-Fi/有线混合网络拓扑。
- 不新增高跟随周期看门狗。
- 不修改 `avoid_singularity`，不新增奇异点检测或降速策略。
- 首轮不删除或调整现有可操作度优化任务。
- 不修改 `peripherals_rm75.yaml` 中的夹爪选择、工具位姿或负载。
- 不新增 ROS2 节点、消息、求解器工厂或第三方依赖。
- Codex 不连接或移动真实机械臂和夹爪。
-
-## 3. 姿态数据模型
-
-控制路径中的机器人位姿统一为有限的 `4×4` NumPy SE(3) 矩阵：
-
-```text
-T = [ R  p ]
-    [ 0  1 ]
-```
-
-其中 `R` 为 `3×3` 旋转矩阵，`p` 为 TCP 在机器人基坐标系下的位置。Placo 正解直接返回该矩阵，QP 目标也直接接收该矩阵。现有仅保存 `x/y/z/rx/ry/rz` 的 `ArmPose` 不再作为控制路径接口，避免在遥操作节点与 Placo 之间发生 RPY 往返转换。
-
-ROS 调试边界按消息类型转换：
-
- `PoseStamped`：旋转矩阵转换为四元数后发布。
- `TwistStamped.angular`：发布相邻两个目标旋转之间、在机器人基坐标系表达的 SO(3) 旋转向量速度。
-
-## 4. XR 姿态到机器人姿态
-
-Grip 按下时保存 XR 初始四元数和当前 `omnipicker_tcp` 初始旋转矩阵。每周期计算：
-
-```text
-R_xr_delta    = R_xr_now * transpose(R_xr_start)
-R_robot_delta = M * R_xr_delta * transpose(M)
-R_raw_target  = R_robot_delta * R_robot_start
-```
-
-`M` 为现有 `xr_to_robot_matrix`，其左右臂映射保持不变。`enable_orientation_axes` 继续保留；被关闭的机器人基坐标轴通过将对应 SO(3) 相对旋转向量分量置零实现，不再通过拼接 RPY 分量实现。
-
-姿态处理统一使用基坐标系下的左乘增量：
-
-```text
-R_error = R_target * transpose(R_current)
-r       = Log(R_error)
-R_next  = Exp(scale * r) * R_current
-```
-
-处理顺序为：
-
-1. 以 `norm(Log(R_target * R_lastᵀ))` 判断 `orientation_deadband_rad`。
-2. 以 `orientation_filter_alpha` 缩放从滤波状态到目标的最短旋转向量。
-3. 将从上一发送姿态到滤波姿态的旋转角限制在
-   `max_orientation_speed / control_rate_hz` 以内。
-4. 将限速后的旋转和已通过现有工作空间限制的位置合成为 SE(3)。
-
-该路径不使用 RPY 展开、分量插值或分量限速。相对旋转接近 `π` 时也必须选择物理最短路径；四元数 `q` 与 `-q` 必须得到相同目标。
-
-位姿误差采用与 Placo `FrameTask` 一致的解耦 `3+3` 形式：
-
-```text
-e_position    = p_target - p_current
-e_orientation = Log(R_target * transpose(R_current))
-e_pose        = [e_position; e_orientation]
-```
-
-其中 `e_pose` 可视为六维任务误差，但不是
-`Log_SE3(T_target * inverse(T_current))`。本次不引入完整 SE(3) 对数映射中的
-平移—旋转耦合。
-
-## 5. URDF 与 TCP
-
-用户上传的
-`/home/robot/下载/Models/RM75-B_OmniPicker_Pinocchio.zip`
-作为模型来源。将 fixed URDF 和被引用的 RM75、OmniPicker mesh 放入
-`xr_rm_teleop/models/rm75_omnipicker`，由现有 `xr_rm_teleop` 包安装。
-
-在 `RM75-B_OmniPicker_fixed.urdf` 中增加：
-
-```xml
-<link name="omnipicker_tcp"/>
-
-<joint name="omnipicker_tcp_joint" type="fixed">
-  <parent link="omnipicker_base_link"/>
-  <child link="omnipicker_tcp"/>
-  <origin xyz="0 0 0.16" rpy="0 0 0"/>
-</joint>
-```
-
-该变换表示 TCP 相对 RM75 法兰坐标系沿 `+Z` 方向平移 `0.16 m`、坐标轴方向不变。上传模型中 `rm75_flange` 到 `omnipicker_base_link` 为零固定变换，因此上述定义与已确认的法兰到 TCP 变换一致。
-
-`arm_debug.launch.py` 的左臂、右臂和双臂节点统一加载这一份 fixed URDF。模型仍只有 `joint_1` 至 `joint_7` 七个运动关节，OmniPicker 关节均保持 fixed。
-
-## 6. Placo QP
-
-`PlacoIkSolver` 改为：
-
-```text
-构造参数：URDF 路径、dt
-当前位姿：get_T_world_frame("omnipicker_tcp")
-目标任务：add_frame_task("omnipicker_tcp", target_se3)
-```
-
-删除仅供 Placo 使用的 `tool_pose` 构造参数、`_tool_transform`、
-`_tool_inverse` 和对应的法兰/TCP换算函数。`peripherals_rm75.yaml` 保持原状，
-仍供 `RealManAdapter` 配置真实控制器的工具坐标、负载和末端外设；其中的
-`pose` 不再传入 Placo，因此不会在 QP 中重复叠加末端偏移。
-
-QP 配置首轮保持：
-
-```text
-frame task:                 soft, 1.0
-manipulability task:        soft, 5e-2
-kinetic energy regularizer:       1e-6
-```
-
-可操作度任务已知会造成目标静止时的关节姿态变化，但按用户决定本轮保留，
-待确认 RPY 绕行消失后再单独评估。关节位置和单周期速度校验继续使用 URDF
-限制，固定虚拟基座和 `q[7:14]` 的七关节映射保持不变。
-
-## 7. 参数与启动行为
-
-以下配置在 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和
-`dual_arm_rm75.yaml` 对应节点中同步：
-
-```yaml
-control_rate_hz: 125.0
-orientation_deadband_rad: 0.005
-orientation_filter_alpha: 0.65
-max_orientation_speed: 0.5
-follow: false
-```
-
-`arm_debug.launch.py` 的 `control_rate_hz` 默认值同步为 `125.0`，`follow`
-默认值继续为 `false`。`single_arm_velocity_teleop` 内部参数默认值同步，
-避免绕过 YAML 启动时回到旧频率或旧角速度。
-
-`right_arm_rm75.yaml` 的 `move_to_initial_pose_on_connect` 从 `True` 改为
-`false`，与左臂、双臂以及 launch 默认安全行为一致。其他左右臂空间范围、
-线速度、关节速度、加速度、初始关节角和外设配置不变。
-
-## 8. 异常与停止
-
-现有异常策略保持：
-
- 非法 XR 四元数、Grip 松开、XR 超时、关节反馈过期或通信失败时执行现有慢停止并重置激活状态。
- QP 失败或输出违反关节位置/速度限制时继续使用上一组有效关节目标。
- 第一次有效关节反馈前不发送运动命令。
- `configure_safety_limits` 保持启用。
- Mock 模式不导入睿尔曼 SDK。
-
-新增 SO(3) 运算必须拒绝非有限矩阵和零四元数。旋转矩阵若满足
-`norm(RᵀR-I) <= 1e-3` 且行列式为正，则使用 `3×3` SVD 投影到最近的合法旋转；
-超出该范围时停止输出，不能把明显无效的输入静默修正成运动目标。
-
-## 9. 验证
-
-### 9.1 自动测试
-
-扩展 `test_orientation_control.py`，至少覆盖：
-
- 初始姿态俯仰接近 `+90°` 和 `-90°` 时，小手柄旋转仍产生相同量级的最短物理旋转。
- 目标跨越原 RPY 表示分支时，不产生接近 `π` 的错误路径。
- `q` 与 `-q` 产生相同旋转矩阵。
- SO(3) 死区使用整体旋转角。
- 每周期姿态步长不超过 `0.5 / 125 rad`。
- SO(3) 滤波沿最短路径收敛。
- 调试四元数有限且归一化。
-
-更新 Placo 变换测试和 smoke test，验证：
-
- fixed URDF 可加载，运动关节仍严格为七个且顺序正确。
- `omnipicker_tcp` 相对法兰的变换为 `[0, 0, 0.16]` 和单位旋转。
- 当前/原始目标/发送目标均表示 `omnipicker_tcp`。
- 一次 QP 输出七个有限关节角并满足位置与单周期速度限制。
- 目标静止两秒时记录左右臂最大关节变化，但本轮不以 `≤0.5°` 作为通过条件。
- 目标停止后 TCP 位置误差不超过 `5 mm`、姿态误差不超过 `2°`。
-
-### 9.2 命令
-
-所有命令从 `/home/robot/WS_xr` 执行：
-
-```bash
-source /opt/ros/humble/setup.bash
-PYTHONPATH=src/xr_rm_teleop pytest -q \
-  src/xr_rm_teleop/test/test_orientation_control.py
-
-source /opt/ros/humble/setup.bash
-colcon build --symlink-install
-
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
-```
-
-Placo smoke test继续使用已验证的
-`/home/robot/miniconda3/envs/xr/bin/python`，周期改为 `1/125 s`。
-
-### 9.3 真机人工验收
-
-Codex 只提供步骤，不执行真机操作。用户应分别测试左右臂：
-
-1. 确认急停可用、周围无障碍物且 `move_to_initial_pose_on_connect=false`。
-2. 先保持手柄和目标姿态不变，记录 TCP 与关节变化。
-3. 仅改变手柄姿态，重点跨越原俯仰 `±90°` 附近的 RPY 分支。
-4. 确认 TCP 以最短物理旋转跟随，没有绕一大圈。
-5. 确认 `omnipicker_tcp` 位置保持在允许误差内。
-
-## 10. 完成标准
-
-1. 控制路径中的目标生成、滤波、限速和 QP 接口不再使用 RPY。
-2. 左右臂都由同一 fixed URDF 的 `omnipicker_tcp` 作为控制帧。
-3. Placo 不再使用 `peripherals_rm75.yaml` 的工具位姿进行矩阵换算。
-4. 可操作度任务保持现状，漂移数据被记录但不作为首轮阻断项。
-5. 三份配置使用 `125 Hz`、`0.5 rad/s` 和低跟随。
-6. 右臂连接时不再自动移动到初始关节姿态。
-7. 指定测试、构建和左右臂 mock 启动验证通过。
-8. 未改变工作空间、圆柱、关节限制、指令超时、安全停止或奇异点设置。
@@ -1,198 +0,0 @@
-# RM75 CANFD UDP 主动反馈设计
-
-> 真机验证修正：右臂高跟随首轮测试触发掉使能。离线复现发现静止目标存在
-> `23.582°` 空空间漂移，第一周期约 `3315°/s²`。当前实现已移除
-> manipulability 自运动、增加软件关节加速度限幅和故障 Grip 锁存，并将三份
-> YAML 恢复为 `follow: false` 安全基线；高跟随须在基线验证后单独测试。
-
-## 目标
-
-按睿尔曼 MovejCANFD 示例，将真机关节反馈从同一 TCP 控制连接上的
-`rm_get_joint_degree()` 周期轮询，替换为控制器 UDP 主动状态推送。
-
-控制命令继续通过现有单个 `RoboticArm(RM_TRIPLE_MODE_E)` 句柄发送，不新增
-RealMan 连接，不修改 Placo QP、工作空间/圆柱限位、速度限制、指令超时或安全
-停止条件。
-
-## 根因与证据
-
-低跟随模式下，125 Hz CANFD 发送与绝对周期 TCP 反馈轮询能够同时工作：
-
- `feedback_interval mean=10.041–10.389 ms`；
- `feedback_age mean=5.730–6.138 ms`；
- 控制周期最大值不超过 `8.979 ms`。
-
-启用高跟随后，即使 `canfd_trajectory_mode=2`，控制发送仍正常：
-
- `period max=9.275 ms`；
- `send max=0.235 ms`。
-
-但同步反馈退化为：
-
- `feedback_read mean=11.278 ms`、`max=80.320 ms`；
- `feedback_age max=117.502 ms`。
-
-反馈年龄逼近现有 `command_timeout_sec=0.12 s`，触发“关节反馈缺失或过期”
-安全停止，造成 Grip 按住期间控制反复退出和重新锁定。增大超时只会允许 QP
-继续使用更旧的关节状态，不解决 TCP 反馈阻塞。
-
-睿尔曼 MovejCANFD 示例使用三线程模式、`rm_set_realtime_push()` 和
-`rm_realtime_arm_state_call_back()`，通过 UDP 回调获取关节状态，而不是在
-CANFD 透传期间同步轮询关节角。
-
-## 数据流
-
-```text
-PICO -> ROS 125 Hz 控制回调 -> Placo QP -> TCP rm_movej_canfd
-
-RM75 控制器 -> UDP 5 ms 主动推送 -> SDK 第三线程回调
-             -> JointStateSnapshot 缓存 -> ROS 125 Hz 控制回调
-```
-
-TCP 仍承担 CANFD、慢停、安全配置和末端工具命令。UDP 只承担状态反馈，两条
-传输路径共用同一个机械臂句柄。
-
-## SDK 连接与反馈生命周期
-
-`RealManAdapter.connect()` 保持三线程模式和单次 `rm_create_robot_arm()`：
-
-1. 创建并检查机械臂句柄。
-2. 下发已有安全参数和可选初始位姿。
-3. 创建并保存 `rm_realtime_arm_state_callback_ptr`，避免 Python 回调被垃圾
-   回收。
-4. 使用 `rm_realtime_push_config_t` 配置 5 ms UDP 主动上报。
-5. 注册 `rm_realtime_arm_state_call_back()`。
-6. 等待第一帧有效 UDP 反馈，最长 2 秒。
-
-若配置接口返回非零，或 2 秒内没有有效反馈，连接初始化失败并删除已创建的
-机械臂句柄；不静默回退到 TCP 轮询。
-
-连接成功后不再创建反馈线程，也不再调用 `rm_get_joint_degree()`。
-`close()` 先停止接受回调更新，再执行现有慢停和句柄删除。控制器的 UDP 配置
-由下一次启动重新覆盖，不额外增加关闭阶段配置命令。
-
-## UDP 回调与缓存
-
-回调只执行有界、非阻塞工作：
-
-1. 检查回调对象、`errCode` 和来源机械臂 IP。
-2. 读取 7 个 `joint_position`，检查数量、数值类型及 NaN/Inf。
-3. 将厂商反馈的角度转换为弧度。
-4. 使用 `time.monotonic()` 记录接收时刻，并计算与上一帧的更新间隔。
-5. 在现有 `_joint_state_lock` 下替换 `JointStateSnapshot`。
-6. 第一帧有效数据唤醒连接初始化等待。
-
-无效 UDP 帧不覆盖上一帧缓存。若后续持续丢包，现有 120 ms 新鲜度检查自然
-触发安全停止。
-
-UDP 回调不执行 QP、ROS 发布、停止命令或其他 SDK 调用，避免阻塞 SDK 接收
-线程。
-
-## 参数与三份 YAML
-
-新增真机参数：
-
- `realtime_push_host_ip`：机械臂可直接访问的上位机地址；
- `realtime_push_port`：单臂 UDP 接收端口；
- `realtime_push_cycle_ms`：主动上报周期，默认并配置为 `5`。
-
-本次现场配置：
-
-| 配置 | 节点 | host | port |
-|---|---|---|---:|
-| `right_arm_rm75.yaml` | 右臂 | `192.168.192.148` | 8090 |
-| `left_arm_rm75.yaml` | 左臂 | `192.168.192.148` | 8089 |
-| `dual_arm_rm75.yaml` | 左臂 | `192.168.192.148` | 8089 |
-| `dual_arm_rm75.yaml` | 右臂 | `192.168.192.148` | 8090 |
-
-左右臂端口必须不同。更换上位机或网络后，只需同步修改 YAML 中的
-`realtime_push_host_ip`。
-
-Mock 模式不导入厂商 SDK，也不要求 UDP 参数有效。
-
-## YAML 与 launch 参数所有权
-
-此前 `arm_debug.launch.py` 会用 launch 默认值覆盖 YAML 中的机械臂参数，
-导致 YAML 无法单独控制 `follow` 等行为。
-
-用户选择由 YAML 作为机械臂行为和硬件参数的唯一默认来源。
-
-以下参数只由 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和
-`dual_arm_rm75.yaml` 管理，launch 不再声明或覆盖：
-
- `robot_ip`、`robot_port`；
- `avoid_singularity`；
- `control_rate_hz`；
- `follow`、`canfd_trajectory_mode`、`canfd_radio`；
- `configure_safety_limits`；
- `move_to_initial_pose_on_connect`；
- `enable_tool_control`、`enable_trigger_gripper_control`；
- `trigger_close_threshold`；
- `configure_peripheral_on_connect`；
- 本设计新增的 UDP 主动反馈参数。
-
-三份 YAML 补齐工具控制参数；删除其中不再生效的 `use_mock`，避免出现两个
-配置来源。
-
-`arm_debug.launch.py` 只保留：
-
- `arm=left|right|both`，选择启动拓扑；
- `use_mock=true|false`，作为显式安全运行模式，默认仍为 `true`；
- PICO 输入节点的 `udp_host`、`udp_port`、`udp_timer_hz`；
- launch 根据安装路径和左右臂生成的 `robot_urdf_path`、
-  `peripheral_config_file`、`peripheral_arm` 和 `tool_command_topic`。
-
-`launcher_ui.py` 和 README 中的启动命令同步删除机械臂 IP、初始化移动等已
-移交 YAML 的 launch 参数，只保留 `arm`、`use_mock` 和 PICO 输入覆盖。
-
-控制模式保持分阶段范围：
-
- 三份 YAML 均使用 `follow: false`、`canfd_trajectory_mode: 2` 完成安全基线；
- 基线验证通过前不启用高跟随或模式 0，不提高 `max_linear_speed`。
-
-## Timing 日志
-
-保留：
-
- `period`；
- `total`；
- `qp`；
- `send`；
- `feedback_age`；
- `feedback_interval`。
-
-`feedback_read` 表示同步 SDK 查询耗时；UDP 架构不存在该查询，因此该字段
-不再产生样本，现有条件日志逻辑会自动省略它，不新增同义统计项。
-
-## 测试
-
-使用 FakeArm 和伪造 SDK 模块覆盖：
-
- 连接时使用正确 host、port、5 ms 周期配置 UDP 并注册回调；
- 第一帧有效回调转换 7 个关节角为弧度并解除启动等待；
- 连续回调正确记录 `feedback_interval`；
- 错误码、来源 IP、长度或 NaN/Inf 无效帧不覆盖缓存；
- UDP 配置失败或首帧超时会清理句柄并抛出明确异常；
- 真机适配器不再启动轮询线程或调用 `rm_get_joint_degree()`；
- Mock 模式无需厂商 SDK。
- launch 不再覆盖 YAML 的机械臂行为和硬件参数；
- `use_mock` 仍由 launch 默认设为 `true`；
- `launcher_ui.py` 不再传递已经删除的 launch 参数。
-
-随后运行：
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-python3 -m pytest -q src/xr_rm_teleop/test
-python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
-colcon build --symlink-install --executor sequential
-```
-
-Codex 不连接真机。用户在右臂小范围测试中确认：
-
- 启动日志显示收到 UDP 首帧；
- 按住 Grip 不再出现反馈过期或 SDK `-2`；
- 连续四个 timing 窗口 `period max < 10 ms`、`total max < 8 ms`；
- `feedback_interval mean` 接近 5 ms；
- `feedback_age mean < 5 ms`，且最大值不触发 120 ms 安全停止。