From 2c128c1f54654b8dd661b1ecfefb28077d962f77 Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Thu, 30 Jul 2026 09:49:39 +0800 Subject: [PATCH] =?UTF-8?q?=E6=AF=8F=E5=91=A8=E6=9C=9F=E5=8D=95=E6=AD=A5QP?= =?UTF-8?q?=E6=94=B9=E4=B8=BA=E6=9C=89=E7=95=8C=E8=BF=AD=E4=BB=A3QP?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../plans/2026-07-29-rm75-qp-convergence.md | 412 ++++++++++++++++++ .../2026-07-30-rm75-joint-command-braking.md | 309 +++++++++++++ ...07-30-rm75-joint-command-braking-design.md | 99 +++++ xr_rm_teleop/test/test_placo_transforms.py | 53 +++ xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py | 66 ++- 5 files changed, 932 insertions(+), 7 deletions(-) create mode 100644 docs/superpowers/plans/2026-07-29-rm75-qp-convergence.md create mode 100644 docs/superpowers/plans/2026-07-30-rm75-joint-command-braking.md create mode 100644 docs/superpowers/specs/2026-07-30-rm75-joint-command-braking-design.md diff --git a/docs/superpowers/plans/2026-07-29-rm75-qp-convergence.md b/docs/superpowers/plans/2026-07-29-rm75-qp-convergence.md new file mode 100644 index 0000000..8efff06 --- /dev/null +++ b/docs/superpowers/plans/2026-07-29-rm75-qp-convergence.md @@ -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和错误日志返回后再调整；不得直接提高控制频率、 +关闭限速或改成高跟随。 diff --git a/docs/superpowers/plans/2026-07-30-rm75-joint-command-braking.md b/docs/superpowers/plans/2026-07-30-rm75-joint-command-braking.md new file mode 100644 index 0000000..a903354 --- /dev/null +++ b/docs/superpowers/plans/2026-07-30-rm75-joint-command-braking.md @@ -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. 若不再振荡但仍有不可接受的整臂大幅构型变化，停止扩大位移，转入独立的奇异点 + 处理设计。 diff --git a/docs/superpowers/specs/2026-07-30-rm75-joint-command-braking-design.md b/docs/superpowers/specs/2026-07-30-rm75-joint-command-braking-design.md new file mode 100644 index 0000000..bb9ba71 --- /dev/null +++ b/docs/superpowers/specs/2026-07-30-rm75-joint-command-braking-design.md @@ -0,0 +1,99 @@ +# RM75 关节命令提前制动设计 + +## 背景 + +右臂真机保持 90 Hz 和 `follow: false`。有界迭代 QP 提高跟随速度后，手柄上下 +移动约 10 mm 时出现整臂剧烈晃动，并且到达目标后仍持续振荡。 + +现场日志表明控制计算未超时，但存在少量 QP 未收敛警告。离线检查确认： + +- 手柄上下移动按现有映射对应机器人 X 方向； +- 当前初始姿态的关节雅可比条件数约为 116，该方向的逆解对关节运动较敏感； +- 机器人 X 方向 10 mm 的收敛逆解可能包含最大约 17° 的关节变化； +- 现有关节命令限幅器只限制速度和加速度，没有根据剩余距离提前制动。 + +固定 10° 关节目标的离线复现中，现有限幅器运行 3 秒后仍处于约 10.56°、 +-20°/s，证明稳定目标本身也会被反复越过。这与真机“到位后继续晃动”的现象 +一致。 + +## 目标 + +- 关节目标稳定后，90 Hz 关节命令提前减速并停止在目标上； +- 不再因命令限幅器反复越过目标而持续振荡； +- 保留当前较快的有界迭代 QP； +- 保留现有关节最大速度和最大加速度限制； +- 保持 `rm_movej_canfd(..., follow=false)`； +- 不改变 UDP 反馈、超时保持、CANFD 错误恢复和安全停止行为。 + +## 不在本次范围 + +- 不修改 QP 迭代次数、收敛阈值或失败回退； +- 不修改初始姿态、XR 坐标映射或姿态控制； +- 不增加奇异点阻尼、预测器、新线程、新依赖或新 ROS 参数； +- 不修改三份机械臂 YAML、RealMan 适配器、launch 或 UI； +- 不连接真机，不由 Codex 发送运动命令。 + +若修复制动后机械臂运动已经平稳，但上下运动仍伴随不可接受的整臂大幅构型变化， +再单独设计奇异点处理；本次不把两个问题混在同一改动中。 + +## 方案 + +只修改 `SingleArmVelocityTeleop._limit_joint_command_step()`。 + +现有逻辑在目标仍位于运动方向前方时持续加速，只有到达或越过目标后才开始反向 +减速。新逻辑对每个关节使用同一组现有状态： + +- 上一次已发送的关节目标； +- 上一次关节命令速度； +- 当前 QP 关节目标； +- 现有关节最大速度、最大加速度和控制周期。 + +每周期按以下规则生成命令： + +1. 计算关节剩余距离和单周期最大速度变化 + `velocity_step = max_acceleration * dt`。 +2. 根据 90 Hz 离散积分规则，计算“本周期再加速一次、随后以最大允许减速度制动” + 所需的总距离。 +3. 若关节正在朝目标运动，并且剩余距离已经不大于该制动距离，则本周期开始减速； + 否则继续朝目标加速，但不超过现有最大速度。 +4. 使用 `velocity_step` 限制本周期速度变化，保持现有加速度上限。 +5. 使用新速度积分得到本周期关节目标。 +6. 当关节已经停下且剩余距离不超过一个最大加速度位移 + `max_acceleration * dt²` 时，在不违反单周期加速度限制的前提下精确落到目标， + 避免离散步长形成极小往复振荡。 + +该逻辑只负责命令轨迹制动，不改变 QP 输出。对于持续移动的目标，若目标突然越过 +当前命令位置，控制器仍优先遵守加速度限制，以最大允许减速度反向；不会为禁止 +瞬时越界而跳变速度。 + +## 安全行为 + +- 每周期命令速度绝对值不超过 `joint_max_speed`； +- 相邻周期间速度变化不超过 `joint_max_acc * dt`； +- 输出继续校验 NaN 和 Inf； +- QP 异常仍保持上一组安全目标并在终端限频打印警告； +- UDP 短暂超时仍以 90 Hz 重发最后一次已限速目标，不运行 QP； +- UDP 持续超时、CANFD 错误、Grip 重新使能和安全停止逻辑保持不变。 + +## 测试 + +先增加失败测试，再修改实现： + +1. 对固定 10° 关节目标连续运行限幅器，验证命令不越过目标并最终停止； +2. 在整个序列中验证速度不超过现有上限； +3. 验证相邻命令速度变化不超过现有加速度上限； +4. 保留现有“从静止开始限制首周期加速度”测试； +5. 运行 `xr_rm_teleop` 全部测试； +6. 按项目规则运行 `colcon build --symlink-install`； +7. 使用 `arm_debug.launch.py arm:=right use_mock:=true`验证 90 Hz、低跟随和启动路径。 + +真机只由用户分阶段验证：先小位移、低风险姿态，确认不再到位后持续振荡，再逐步 +增加位移。若仍有明显整臂构型变化但不再振荡，应停止扩大位移并转入奇异点处理， +不得通过提高速度、加速度或启用高跟随规避。 + +## 文件范围 + +- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`； +- 修改 `xr_rm_teleop/test/test_joint_control.py`； +- 新增本中文设计文档； +- 后续新增一份中文实施计划。 diff --git a/xr_rm_teleop/test/test_placo_transforms.py b/xr_rm_teleop/test/test_placo_transforms.py index 8d1ec34..ae8da9a 100644 --- a/xr_rm_teleop/test/test_placo_transforms.py +++ b/xr_rm_teleop/test/test_placo_transforms.py @@ -1,3 +1,4 @@ +import math from pathlib import Path from xml.etree import ElementTree @@ -45,6 +46,58 @@ def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None: assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0" +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 + + +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 + + def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None: transform = np.eye(4) transform[:3, 3] = [0.3, -0.1, 0.2] diff --git a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py index 4d4ce79..f9f17cb 100644 --- a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py +++ b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py @@ -1,4 +1,4 @@ -"""RM75 的 Placo 0.9.4 单步 QP 逆解。""" +"""RM75 的 Placo 0.9.4 有界迭代 QP 逆解。""" from __future__ import annotations @@ -10,6 +10,9 @@ import numpy as np EXPECTED_PLACO_VERSION = "0.9.4" RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)] RM75_Q_SLICE = slice(7, 14) +QP_MAX_ITERATIONS = 30 +QP_POSITION_TOLERANCE_M = 1e-3 +QP_ORIENTATION_TOLERANCE_RAD = 5e-3 def _validated_transform(transform: np.ndarray) -> np.ndarray: @@ -118,25 +121,74 @@ class PlacoIkSolver: self._frame_task.T_world_frame = base_to_tool.copy() return base_to_tool.copy() + 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()), + ) + 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) - self._solver.solve(True) + self._frame_task.T_world_frame = _validated_transform( + target_tool_pose + ) result = np.asarray( self._robot.state.q[RM75_Q_SLICE], dtype=float, ).copy() - self._validate_result(result) - return result.tolist() + position_error, orientation_error = self._target_errors() + if ( + position_error <= QP_POSITION_TOLERANCE_M + and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD + ): + return result.tolist() - def _validate_result(self, result: np.ndarray) -> None: + 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" + ) + + def _validate_result( + self, + result: np.ndarray, + reference: np.ndarray | None = None, + ) -> None: if result.shape != (7,) or not np.isfinite(result).all(): raise ValueError("QP result must contain 7 finite values") lower = self._joint_limits[:, 0] upper = self._joint_limits[:, 1] if np.any(result < lower - 1e-9) or np.any(result > upper + 1e-9): raise ValueError("QP result violates RM75 joint position limits") + 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 - self._actual_joints) > max_step): + if np.any(np.abs(result - reference) > max_step): raise ValueError("QP result violates RM75 one-cycle velocity limits")