413 lines
13 KiB
Markdown
413 lines
13 KiB
Markdown
# 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和错误日志返回后再调整；不得直接提高控制频率、
|
||
关闭限速或改成高跟随。
|