每周期单步QP改为有界迭代QP

2026-07-30 09:49:39 +08:00
parent 6d22d5600a
commit 2c128c1f54
5 changed files with 932 additions and 7 deletions
@@ -0,0 +1,309 @@
+# RM75 关节命令提前制动实施计划
+
+> **供智能体执行者：** 必须使用 `superpowers:subagent-driven-development`
+>（推荐）或 `superpowers:executing-plans` 逐项实施；所有步骤使用复选框跟踪。
+
+**目标：** 修复 90 Hz 关节命令在稳定目标附近反复越界的问题，使 RM75 在保留
+现有速度、加速度限制和低跟随模式的前提下提前制动并稳定停止。
+
+**架构：** 保留当前 QP、反馈和故障恢复链路，只替换
+`SingleArmVelocityTeleop._limit_joint_command_step()` 内部的关节命令生成规则。
+每个关节根据离散制动距离决定继续加速或开始减速，最终命令仍由现有
+`_send_joint_target()` 发送。
+
+**技术栈：** Python 3.10、ROS2 Humble、NumPy、pytest、ament/colcon。
+
+---
+
+## 执行约束
+
+- 设计文档：
+  `docs/superpowers/specs/2026-07-30-rm75-joint-command-braking-design.md`。
+- 构建、测试和启动命令在 `/home/robot/WS_xr` 执行，并先运行
+  `source /opt/ros/humble/setup.bash`。
+- 不连接真机，不发送真实 CANFD，不移动机械臂，不操作夹爪。
+- 启动验证只使用
+  `xr_rm_bringup/launch/arm_debug.launch.py arm:=right use_mock:=true`。
+- 不修改 QP、YAML、RealMan 适配器、launch、UI、依赖和公开 API。
+- 保留工作空间、圆柱、TCP 速度、姿态速度、关节速度、关节加速度、超时保持、
+  CANFD 恢复、Grip 重新使能和安全停止逻辑。
+- 用户未要求 Git 提交，因此本计划不执行 `git commit` 或 `git push`。
+- 保留工作区中已有的其他修改，不回退、不覆盖：
+  `placo_ik_solver.py`、`test_placo_transforms.py` 及现有 Superpowers 文档。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/test/test_joint_control.py`
+  - 增加固定目标提前制动回归测试。
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+  - 在现有关节限幅入口实现离散制动距离判断。
+- 不创建新的运行时代码文件或配置项。
+
+### 任务一：增加持续振荡回归测试
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_joint_control.py:206`
+- 测试：`xr_rm_teleop/test/test_joint_control.py`
+
+- [ ] **步骤 1：在现有首周期加速度测试后增加固定目标测试**
+
+增加以下测试：
+
+```python
+def test_joint_command_step_brakes_before_fixed_target_without_overshoot() -> None:
+    dt = 1.0 / 90.0
+    max_speed = math.radians(180.0)
+    max_acceleration = math.radians(300.0)
+    target = np.radians(
+        [10.0, -10.0, 3.0, -3.0, 1.0, -1.0, 0.1]
+    ).tolist()
+    command = [0.0] * 7
+    velocity = [0.0] * 7
+
+    for _ in range(180):
+        previous_velocity = list(velocity)
+        command, velocity = (
+            SingleArmVelocityTeleop._limit_joint_command_step(
+                target=target,
+                previous_target=command,
+                previous_velocity=velocity,
+                max_speed=max_speed,
+                max_acceleration=max_acceleration,
+                dt=dt,
+            )
+        )
+
+        for index in range(7):
+            assert min(0.0, target[index]) - 1e-12 <= command[index]
+            assert command[index] <= max(0.0, target[index]) + 1e-12
+            assert abs(velocity[index]) <= max_speed + 1e-12
+            assert (
+                abs(velocity[index] - previous_velocity[index])
+                <= max_acceleration * dt + 1e-12
+            )
+
+    assert command == pytest.approx(target, abs=1e-12)
+    assert velocity == pytest.approx([0.0] * 7, abs=1e-12)
+```
+
+该测试同时覆盖正负方向、不同目标距离、最大速度、最大加速度、禁止越过固定目标
+和最终停止。
+
+- [ ] **步骤 2：运行新增测试并确认失败**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_joint_control.py::test_joint_command_step_brakes_before_fixed_target_without_overshoot \
+  -v
+```
+
+预期：`FAIL`，现有实现会让至少一个关节命令越过固定目标。失败原因必须来自新增
+越界断言，不能是导入或环境错误。
+
+### 任务二：实现离散提前制动
+
+**文件：**
+
+- 修改：
+  `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:1232-1263`
+- 测试：`xr_rm_teleop/test/test_joint_control.py`
+
+- [ ] **步骤 1：用离散制动逻辑替换现有限幅计算**
+
+保留方法签名和现有长度、参数校验，将
+`desired_velocity = np.clip(...)` 到返回值的部分替换为：
+
+```python
+        values = np.asarray(
+            [target, previous_target, previous_velocity],
+            dtype=float,
+        )
+        if not np.isfinite(values).all():
+            raise ValueError("joint command contains NaN/Inf")
+
+        velocity_step = max_acceleration * dt
+        arrival_distance = velocity_step * dt
+        limited_target = []
+        limited_velocity = []
+        for desired_target, last_target, last_velocity in zip(
+            target,
+            previous_target,
+            previous_velocity,
+        ):
+            error = desired_target - last_target
+            if (
+                abs(last_velocity) <= 1e-12
+                and abs(error) <= arrival_distance
+            ):
+                velocity = error / dt
+                position = desired_target
+            else:
+                direction = (
+                    math.copysign(1.0, error)
+                    if abs(error) > 1e-12
+                    else 0.0
+                )
+                accelerated_speed = min(
+                    abs(last_velocity) + velocity_step,
+                    max_speed,
+                )
+                braking_steps = max(
+                    0,
+                    math.ceil(accelerated_speed / velocity_step) - 1,
+                )
+                braking_distance = accelerated_speed * dt + dt * (
+                    braking_steps * accelerated_speed
+                    - velocity_step
+                    * braking_steps
+                    * (braking_steps + 1)
+                    / 2.0
+                )
+                desired_velocity = direction * max_speed
+                if (
+                    last_velocity * error > 0.0
+                    and abs(error) <= braking_distance
+                ):
+                    desired_velocity = 0.0
+                velocity = _clamp(
+                    desired_velocity,
+                    last_velocity - velocity_step,
+                    last_velocity + velocity_step,
+                )
+                velocity = _clamp(velocity, -max_speed, max_speed)
+                position = last_target + velocity * dt
+
+            limited_target.append(position)
+            limited_velocity.append(velocity)
+
+        if not np.isfinite(limited_target).all():
+            raise ValueError("joint command contains NaN/Inf")
+        return limited_target, limited_velocity
+```
+
+不要增加 ROS 参数或辅助类。制动距离直接使用当前方法已有的
+`max_acceleration`、`max_speed` 和 `dt`。
+
+- [ ] **步骤 2：运行新增测试并确认通过**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_joint_control.py::test_joint_command_step_brakes_before_fixed_target_without_overshoot \
+  -v
+```
+
+预期：`PASS`。
+
+- [ ] **步骤 3：运行关节控制测试文件**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v
+```
+
+预期：全部通过；现有
+`test_joint_command_step_limits_acceleration_from_rest` 继续通过，证明首周期
+加速度行为没有回归。
+
+### 任务三：完整验证
+
+**文件：**
+
+- 不修改文件。
+
+- [ ] **步骤 1：运行 `xr_rm_teleop` 全部测试**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_TEST_PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
+PYTHONPATH="${RM75_TEST_PYTHONPATH}:${PYTHONPATH:-}" \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test -v
+```
+
+预期：全部测试通过，无失败；真实 Placo 回归测试必须执行，不能因缺少模块而跳过。
+
+- [ ] **步骤 2：单独运行姿态控制测试**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_orientation_control.py -v
+```
+
+预期：全部通过。
+
+- [ ] **步骤 3：构建工作空间**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：`xr_rm_input`、`xr_rm_interfaces`、`xr_rm_teleop` 和 `xr_rm_bringup`
+全部构建成功。
+
+- [ ] **步骤 4：使用 mock 启动右臂统一 launch**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+timeout 10s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=true
+```
+
+预期：
+
+- 节点日志显示控制周期约 `dt=0.0111s`；
+- 日志显示 `follow=False`；
+- 不连接厂商 SDK，不发送真实 CANFD；
+- 除 `timeout` 主动结束产生的退出状态外，没有 Python 异常或 ROS 错误。
+
+- [ ] **步骤 5：检查最终差异**
+
+运行：
+
+```bash
+git -C /home/robot/WS_xr/src diff --check
+git -C /home/robot/WS_xr/src status --short
+git -C /home/robot/WS_xr/src diff -- \
+  xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  xr_rm_teleop/test/test_joint_control.py
+```
+
+预期：
+
+- `diff --check` 无输出；
+- 本次运行时代码改动只涉及上述两个文件；
+- 原有工作区修改仍保留；
+- 不存在提交或远程推送。
+
+## 用户真机验证边界
+
+自动验证完成后，只提供手动验证步骤，不由 Codex 操作真机：
+
+1. 保持低跟随，从安全姿态和小于 5 mm 的上下位移开始；
+2. 手柄停止后观察机械臂是否立即减振并稳定；
+3. 确认无持续 QP、UDP、CANFD 或故障锁存错误后，再测试 10 mm；
+4. 若不再振荡但仍有不可接受的整臂大幅构型变化，停止扩大位移，转入独立的奇异点
+   处理设计。