每周期单步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和错误日志返回后再调整；不得直接提高控制频率、
+关闭限速或改成高跟随。
@@ -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. 若不再振荡但仍有不可接受的整臂大幅构型变化，停止扩大位移，转入独立的奇异点
+   处理设计。
@@ -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`；
+- 新增本中文设计文档；
+- 后续新增一份中文实施计划。
@@ -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]
@@ -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")