每周期单步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,412 @@
+# RM75 QP 收敛优化实施计划
+
+> **执行要求：** 使用 `superpowers:executing-plans` 逐项执行。用户未授权
+> subagent、独立worktree或本地分支，因此本计划只允许当前会话内联实施。所有
+> 步骤使用复选框跟踪。
+
+**目标：** 将当前每周期单步QP改为有界迭代QP，使低跟随RM75在手柄移动10 cm
+后约1秒内稳定到位，并消除由近距离台阶目标造成的持续轻微晃动。
+
+**实现方式：** 每个正常控制周期仍先用UDP实际关节角同步Placo，然后在一次
+`PlacoIkSolver.solve()`内部最多迭代30次，提前达到1 mm位置误差和0.005 rad
+姿态误差即返回。最终关节解继续经过现有90 Hz关节速度与加速度限幅后，以
+`follow=false`发送；不增加预测状态、线程、连接、依赖或配置参数。
+
+**技术栈：** Python 3.10、ROS2 Humble、Placo 0.9.4、NumPy、pytest、
+ament/colcon。
+
+**设计文档：**
+`docs/superpowers/specs/2026-07-29-rm75-qp-convergence-design.md`
+
+---
+
+## 仓库与安全约束
+
+- 构建、测试和启动命令在 `/home/robot/WS_xr` 执行。
+- Git命令在 `/home/robot/WS_xr/src` 执行。
+- 每次构建、测试或启动前执行 `source /opt/ros/humble/setup.bash`。
+- 不自动提交、推送、创建分支或worktree。
+- 不连接真机，不发送真实CANFD，不移动机械臂，不操作夹爪。
+- 只通过 `arm_debug.launch.py arm:=right use_mock:=true`进行启动验证。
+- 不修改三份机械臂YAML、RealMan适配器、launch、UI、依赖或公开入口。
+- 保留工作空间、圆柱、TCP速度、姿态速度、关节速度、关节加速度、反馈超时、
+  CANFD恢复、Grip重新使能和安全停止逻辑。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/test/test_placo_transforms.py`
+  - 增加真实Placo 7 cm目标收敛回归测试。
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
+  - 增加固定上限、提前收敛和逐步安全校验。
+
+不需要修改 `single_arm_velocity_teleop.py`；现有 `_solve_joint_target()` 已负责
+QP异常时打印限频警告并保持上一组安全关节目标，现有
+`_limit_joint_command_step()` 已负责最终90 Hz真实命令限速。
+
+---
+
+## 任务一：用真实Placo复现单步QP不收敛
+
+**修改文件：**
+
+- `xr_rm_teleop/test/test_placo_transforms.py`
+
+- [x] **步骤1：增加测试辅助函数**
+
+在文件顶部增加：
+
+```python
+import math
+```
+
+在现有URDF结构测试之后增加：
+
+```python
+def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]:
+    pytest.importorskip("placo")
+    urdf_path = (
+        Path(__file__).resolve().parents[1]
+        / "models"
+        / "rm75_omnipicker"
+        / "urdf"
+        / "RM75-B_OmniPicker_fixed.urdf"
+    )
+    joints = [
+        math.radians(value)
+        for value in [
+            -90.14,
+            3.76,
+            -86.89,
+            87.89,
+            -96.53,
+            -79.62,
+            -90.04,
+        ]
+    ]
+    return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints
+```
+
+`importorskip()`只让没有Placo的普通系统Python跳过真模型用例；下面的RED/GREEN
+命令会显式加入项目现有Placo 0.9.4路径，因此该用例必须实际执行而不能跳过。
+
+- [x] **步骤2：增加7 cm目标收敛测试**
+
+增加：
+
+```python
+def test_qp_solve_converges_to_reachable_tcp_target() -> None:
+    solver, joints = _rm75_placo_solver()
+    start_pose = solver.update_joint_state(joints)
+    target_pose = start_pose.copy()
+    target_pose[0, 3] += 0.07
+
+    result = solver.solve(target_pose)
+    reached_pose = solver.update_joint_state(result)
+    position_error = np.linalg.norm(
+        target_pose[:3, 3] - reached_pose[:3, 3]
+    )
+    rotation_delta = (
+        target_pose[:3, :3] @ reached_pose[:3, :3].T
+    )
+    orientation_error = math.acos(
+        float(
+            np.clip(
+                (np.trace(rotation_delta) - 1.0) * 0.5,
+                -1.0,
+                1.0,
+            )
+        )
+    )
+
+    assert position_error <= 1e-3
+    assert orientation_error <= 5e-3
+```
+
+该测试验证一次公开 `solve()` 调用返回当前TCP目标对应的收敛关节解，而不是验证
+内部迭代次数。
+
+- [x] **步骤3：运行测试并确认RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/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_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_converges_to_reachable_tcp_target \
+  -v
+```
+
+预期：测试以位置误差约0.063 m大于0.001 m失败，证明当前单步QP确实不能在一次
+调用内给出收敛关节目标。测试不得因导入错误或跳过而结束。
+
+---
+
+## 任务二：实现有界迭代QP
+
+**修改文件：**
+
+- `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
+
+- [x] **步骤1：增加固定收敛常量**
+
+把模块说明改为：
+
+```python
+"""RM75 的 Placo 0.9.4 有界迭代 QP 逆解。"""
+```
+
+在现有常量后增加：
+
+```python
+QP_MAX_ITERATIONS = 30
+QP_POSITION_TOLERANCE_M = 1e-3
+QP_ORIENTATION_TOLERANCE_RAD = 5e-3
+```
+
+这些值是本次已确认的算法边界，不新增ROS参数。
+
+- [x] **步骤2：增加任务误差读取**
+
+在 `solve()` 前增加：
+
+```python
+def _target_errors(self) -> tuple[float, float]:
+    position_task = self._frame_task.position()
+    orientation_task = self._frame_task.orientation()
+    position_task.update()
+    orientation_task.update()
+    return (
+        float(position_task.error_norm()),
+        float(orientation_task.error_norm()),
+    )
+```
+
+Placo在 `solve(True)` 后只更新关节状态；先更新机器人运动学，再显式更新两个任务，
+确保 `error_norm()`对应当前迭代后的状态而不是前一迭代。
+
+- [x] **步骤3：把单步求解改为最多30次且提前收敛**
+
+用以下实现替换现有 `solve()`：
+
+```python
+def solve(self, target_tool_pose: np.ndarray) -> list[float]:
+    if self._actual_joints is None:
+        raise RuntimeError(
+            "joint state must be initialized before QP solve"
+        )
+    self._frame_task.T_world_frame = _validated_transform(
+        target_tool_pose
+    )
+    result = np.asarray(
+        self._robot.state.q[RM75_Q_SLICE],
+        dtype=float,
+    ).copy()
+    position_error, orientation_error = self._target_errors()
+    if (
+        position_error <= QP_POSITION_TOLERANCE_M
+        and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+    ):
+        return result.tolist()
+
+    for _ in range(QP_MAX_ITERATIONS):
+        previous = result
+        self._solver.solve(True)
+        self._robot.update_kinematics()
+        result = np.asarray(
+            self._robot.state.q[RM75_Q_SLICE],
+            dtype=float,
+        ).copy()
+        self._validate_result(result, previous)
+        position_error, orientation_error = self._target_errors()
+        if (
+            position_error <= QP_POSITION_TOLERANCE_M
+            and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+        ):
+            return result.tolist()
+
+    raise RuntimeError(
+        "QP did not converge after "
+        f"{QP_MAX_ITERATIONS} iterations: "
+        f"position_error={position_error:.6f} m, "
+        f"orientation_error={orientation_error:.6f} rad"
+    )
+```
+
+目标已到达时直接返回当前关节角，避免静止时进行不必要的数值迭代。
+
+- [x] **步骤4：让速度校验针对每次数值迭代**
+
+把 `_validate_result()` 签名改为：
+
+```python
+def _validate_result(
+    self,
+    result: np.ndarray,
+    reference: np.ndarray | None = None,
+) -> None:
+```
+
+保留现有有限值和关节位置检查，把速度检查替换为：
+
+```python
+if reference is None:
+    reference = self._actual_joints
+if reference is None:
+    raise RuntimeError("joint state has not been initialized")
+reference = np.asarray(reference, dtype=float)
+if reference.shape != (7,) or not np.isfinite(reference).all():
+    raise ValueError("QP reference must contain 7 finite values")
+max_step = self._velocity_limits * self._dt + 1e-9
+if np.any(np.abs(result - reference) > max_step):
+    raise ValueError(
+        "QP result violates RM75 one-cycle velocity limits"
+    )
+```
+
+这样每次内部数值迭代继续满足Placo的URDF关节速度边界；最终收敛解仍由节点现有
+`_limit_joint_command_step()`按真实90 Hz周期限制后才发送。
+
+- [x] **步骤5：运行目标测试并确认GREEN**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/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_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_converges_to_reachable_tcp_target \
+  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_result_rejects_nan_position_and_velocity_violations \
+  -v
+```
+
+预期：两个测试通过；真实Placo用例不被跳过。
+
+- [x] **步骤6：运行Placo变换测试文件**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/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_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py \
+  -v
+```
+
+预期：全部通过，无失败或跳过。
+
+---
+
+## 任务三：回归、安全和mock验证
+
+**验证范围：**
+
+- `xr_rm_teleop`全部测试；
+- ROS2工作空间构建；
+- 统一launch的右臂mock启动；
+- 最终差异与安全配置审计。
+
+- [x] **步骤1：运行遥操作包全部测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/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_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest src/xr_rm_teleop/test -v
+```
+
+预期：全部测试通过，真实Placo收敛用例被执行。
+
+- [x] **步骤2：按项目规则单独运行姿态控制测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest \
+  src/xr_rm_teleop/test/test_orientation_control.py \
+  -v
+```
+
+预期：全部通过。
+
+- [x] **步骤3：构建ROS2工作空间**
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`和
+`xr_rm_bringup`全部构建成功。
+
+- [x] **步骤4：通过统一入口进行右臂mock启动验证**
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+if timeout --signal=INT 10s ros2 launch \
+  xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=true udp_port:=15123
+then
+  true
+else
+  launch_status=$?
+  test "$launch_status" -eq 124
+fi
+```
+
+预期：
+
+- `udp_controller_receiver`和`single_arm_velocity_teleop`正常启动；
+- 节点报告 `dt=0.0111s`、`follow=False`；
+- mock关节初始化成功；
+- 不导入RealMan SDK，不建立真机连接，不发送CANFD；
+- 10秒后仅由 `timeout`结束。
+
+- [x] **步骤5：最终差异和安全审计**
+
+在 `/home/robot/WS_xr/src` 执行：
+
+```bash
+git diff --check
+git status --short
+git diff -- \
+  xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
+  xr_rm_teleop/test/test_placo_transforms.py
+rg -n \
+  "control_rate_hz|follow:|configure_safety_limits|move_to_initial_pose_on_connect" \
+  xr_rm_bringup/config/dual_arm_rm75.yaml \
+  xr_rm_bringup/config/left_arm_rm75.yaml \
+  xr_rm_bringup/config/right_arm_rm75.yaml
+```
+
+预期：
+
+- 生产代码只修改Placo求解器；
+- 测试只增加真实模型收敛验证；
+- 三份配置继续使用90 Hz、`follow: false`、
+  `configure_safety_limits: true`和
+  `move_to_initial_pose_on_connect: false`；
+- 不改变此前由用户保留的 `AGENTS.md` 修改；
+- 不自动提交或推送。
+
+---
+
+## 真机交接验收
+
+Codex不执行本节。自动验证全部通过后，由用户在安全工作区使用：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=false
+```
+
+验收步骤：
+
+1. 急停可用、Grip松开、工作区无人后启动。
+2. 按住Grip，快速移动手柄约10 cm后保持不动。
+3. 机械臂应在约1秒内稳定到位，无持续肉眼可见晃动。
+4. 连续观察四个5秒 timing 窗口，`total max`均低于11.111 ms。
+5. 不应出现QP未收敛、反馈超时、CANFD错误或故障锁存日志。
+6. 松开Grip后机械臂按现有逻辑安全停止。
+
+若任一窗口 `total max`达到或超过11.111 ms，或机械臂出现明显振荡，立即松开
+Grip并停止测试，把完整timing和错误日志返回后再调整；不得直接提高控制频率、
+关闭限速或改成高跟随。