fix: 更新规范化逆运动学实验产物

.gitignore

@@ -44,3 +44,4 @@ AMENT_IGNORE
 *.vsix
 
 .codex
+.worktrees/

AGENTS.md

@@ -288,7 +288,7 @@ test: 添加 xxx 测试
 
 ## 项目专属规则
 
-* 本项目面向 Ubuntu 22.04 和 ROS2 Humble；编译前必须先切换到工作空间根目录 `/home/robot/WS_xr`，并执行 `source /opt/ros/humble/setup.bash`。禁止在 `/home/robot/WS_xr/src` 中运行 `colcon build`，否则会在源码目录生成多余的 `build/`、`install/` 和 `log/`；测试和运行命令也应在工作空间根目录执行。
+* 本项目面向 Ubuntu 22.04 和 ROS2 Humble；构建、测试和运行命令应在工作空间根目录 `/home/robot/WS_xr` 执行，并先 `source /opt/ros/humble/setup.bash`。
 * 工作空间包含 `xr_rm_input`、`xr_rm_teleop` 两个 `ament_python` 包，以及 `xr_rm_interfaces`、`xr_rm_bringup` 两个 `ament_cmake` 包；优先使用现有 ROS2 包、节点和消息，不要另建重复入口。
 * 修改 ROS 节点、launch、消息定义或安装配置后，至少运行 `colcon build --symlink-install`；涉及遥操作姿态控制时，再运行 `pytest src/xr_rm_teleop/test/test_orientation_control.py`。
 * 遥操作统一使用 `xr_rm_bringup/launch/arm_debug.launch.py`；调试和验证默认使用 `use_mock:=true`，未经用户明确要求不得连接真机、移动机械臂或操作夹爪。

README.md

@@ -1,97 +1,44 @@
-# XR-RM75 双臂遥操作工作空间
+# XR-RM75 双臂遥操作
 
-本仓库是面向 **Ubuntu 22.04 + ROS2 Humble + PICO 4 Ultra + 睿尔曼 RM75** 的阶段一 XR 双臂遥操作项目。当前目标是先跑通一条低速、安全、可调试的闭环：
+基于 **Ubuntu 22.04、ROS2 Humble、PICO 4 Ultra 和睿尔曼 RM75** 的双臂 XR 遥操作
+工作空间，支持单臂/双臂 Mock 与真机控制，以及 MuJoCo 运动学显示。
+
+> [!WARNING]
+> 真机命令会连接并控制机械臂。首次运行必须从 Mock 和单臂低速验证开始，确保急停
+> 可用且工作区无人。当前项目没有双臂碰撞检测或避障。
+
+## 当前能力
+
+- PICO/XR 双手柄 UDP 输入，相对位姿目标与 Placo QP 七关节控制。
+- 单臂/双臂 Mock 与真机、手柄/话题夹爪控制，以及只读 MuJoCo 双臂显示。
+- 工作空间/圆柱限位、速度限制、指令超时和安全慢停。
+- 统一 launch、Tkinter 启动面板、调试话题和 Mock 输入工具。
+
+尚未完成：D405/D435 视频流、数据记录、相机标定、目标检测、双臂碰撞避障、任务级状态机，以及 PICO 与 ROS 的完整时间同步和状态回传。
+
+## 系统架构
 
 ```text
-PICO/XR 双手柄 UDP JSON
+PICO / XRoboToolkit
+  -> UDP JSON
   -> xr_rm_input/udp_controller_receiver
-  -> /xr/left_controller 与 /xr/right_controller
+  -> /xr/left_controller、/xr/right_controller
   -> xr_rm_teleop/single_arm_velocity_teleop
-  -> Placo QP 单步逆解
-  -> 左右 RM75 七关节角透传控制
-  -> /xr_rm/<arm_name>/joint_states
-  -> 可选 xr_rm_mujoco/dual_arm_simulator
-  -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
+  -> 相对 TCP 目标 + Placo QP
+  -> Mock 或 RM75 rm_movej_canfd
+  -> joint_states / 调试话题
+       -> 可选 xr_rm_mujoco/dual_arm_simulator
 ```
 
-当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP。姿态目标使用旋转矩阵和 SO(3) 最短路径完成死区、滤波与限速，不经过 RPY。每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。
+工作空间包含五个 ROS2 包：`xr_rm_input` 负责手柄输入，`xr_rm_interfaces` 定义消息，
+`xr_rm_teleop` 实现遥操作与真机适配，`xr_rm_bringup` 提供启动和配置，
+`xr_rm_mujoco` 负责只读运动学显示。
 
-## 当前范围
+`single_arm_velocity_teleop` 每个实例只控制一台机械臂；双臂模式分别启动 `left_arm_teleop` 和 `right_arm_teleop`。
 
-已完成：
+## 环境与构建
 
-- PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
-- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
-- 使用现有双臂 URDF 的 MuJoCo 运动学显示，可由 Mock 或真机反馈同步双臂姿态。
-- RM75 真机连接适配，包含关节反馈缓存、`rm_movej_canfd` 关节透传、安全速度/加速度配置、可选初始化点位移动。
-- Placo 0.9.4 RM75 QP 逆解；收到首帧有效关节反馈后才启用，求解失败时保留上一组有效关节目标。
-- 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
-- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
-- XRoboToolkit bridge 读取左右手柄 pose、Grip、Trigger、摇杆和主副按键。
-
-暂未完成：
-
-- D405/D435 视频流、数据记录、相机标定和目标检测链路。
-- 双臂碰撞模型、任务级状态机、自动采摘策略。
-- PICO 端与 ROS 端的完整时间同步和状态回传。
-
-## 项目结构
-
-```text
-src/
-├── README.md                       # 项目主文档
-├── AGENTS.md                       # Codex 项目工作流和安全规则
-├── docs/superpowers/               # Superpowers 设计与实施计划
-├── xr_rm_bringup/
-│   ├── config/
-│   │   ├── dual_arm_mujoco.yaml    # MuJoCo 显示刷新参数
-│   │   ├── dual_arm_rm75.yaml      # 双臂配置：left_arm_teleop 与 right_arm_teleop
-│   │   ├── left_arm_rm75.yaml      # 左臂单独调试配置
-│   │   ├── right_arm_rm75.yaml     # 右臂单独调试配置
-│   │   └── peripherals_rm75.yaml   # 左右臂末端外设配置
-│   ├── launch/
-│   │   └── arm_debug.launch.py     # 统一入口：单臂/双臂、Mock/真机、可选 MuJoCo
-│   └── tools/
-│       ├── launcher_ui.py          # 图形化调试启动面板
-│       └── realman_dual_arm_state_monitor.py
-├── xr_rm_input/
-│   ├── launch/
-│   │   └── udp_receiver.launch.py  # 低层 UDP 接收测试入口
-│   ├── test/
-│   │   └── test_controller_fields.py
-│   └── xr_rm_input/
-│       ├── udp_controller_receiver.py
-│       ├── xrobotoolkit_to_udp_bridge.py
-│       └── sample_udp_sender.py    # 本机扫轴/正弦模拟手柄 UDP 数据
-├── xr_rm_interfaces/
-│   └── msg/
-│       └── XrController.msg        # 手柄状态与位姿
-├── xr_rm_mujoco/
-│   └── xr_rm_mujoco/
-│       └── dual_arm_simulator.py   # 双臂 URDF 运动学映射与 MuJoCo viewer
-└── xr_rm_teleop/
-    ├── models/
-    │   ├── rm75/                   # 旧 RM75 模型资源（launch 不再选用）
-    │   ├── rm75_omnipicker/        # 旧单臂 OmniPicker 模型资源
-    │   └── dual_rm75/              # 当前左右臂统一使用的双 RM75 URDF 与网格
-    └── xr_rm_teleop/
-        ├── placo_ik_solver.py      # Placo 0.9.4 单步 QP 逆解
-        ├── single_arm_velocity_teleop.py
-        ├── realman_adapter.py
-        └── fun_peripheral.py
-```
-
-`single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。
-
-## Superpowers Git 约束
-
-使用 Superpowers 执行任务时，只允许按 skill 工作流创建本地 Git 提交。
-禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。skill 如需
-独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
-
-## 环境准备
-
-在工作空间根目录，也就是包含 `src/` 的目录执行：
+在工作空间根目录执行：
 
 ```bash
 cd /home/robot/WS_xr
@@ -102,440 +49,125 @@ colcon build --symlink-install
 source install/setup.bash
 ```
 
-真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。
+遥操作和 MuJoCo 节点固定使用 `/home/robot/miniconda3/envs/xr/bin/python`，
+其中固定 Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0；不要从
+用户或系统 Python 覆盖这些版本。
 
-遥操作和 MuJoCo 节点固定由 `/home/robot/miniconda3/envs/xr/bin/python` 启动，并复用其中的 Python 3.10、Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0。`ros2`、`colcon`、pytest 和 `udp_controller_receiver` 仍使用系统 Python。禁止通过 `pip --user`、`sudo pip` 或系统安装升级 Placo、Pinocchio、EigenPy 和 NumPy。
+真机模式另需睿尔曼 Python API2；Mock 模式不依赖厂商 SDK。
 
-只读检查 Placo 版本：
+## 快速开始
+
+以下命令均在 `/home/robot/WS_xr` 执行，并先 source ROS2 与 `install/setup.bash`。
+
+### Mock
 
 ```bash
-/home/robot/miniconda3/envs/xr/bin/python -c \
-  "import importlib.metadata; print(importlib.metadata.version('placo'))"
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
 ```
 
-输出必须为 `0.9.4`。
-
-同时检查 MuJoCo：
+另开终端发送模拟手柄数据：
 
 ```bash
-/home/robot/miniconda3/envs/xr/bin/python -c \
-  "import mujoco; print(mujoco.__version__)"
-```
-
-当前验证版本为 `3.10.0`。系统 pytest 会通过 `xr_rm_mujoco/test/conftest.py` 复用该固定 XR 环境中的 MuJoCo，因此新包可直接按 ROS2 标准方式测试：
-
-```bash
-colcon test --packages-select xr_rm_mujoco --event-handlers console_direct+
-colcon test-result --verbose
-```
-
-如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：
-
-```bash
-export XR_RM_WS=/home/robot/WS_xr
-```
-
-## 使用 launcher_ui.py 调试
-
-推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、source ROS2 与 `install/setup.bash`，并把每个命令放到独立终端中运行。
-
-源码方式启动：
-
-```bash
-cd /home/robot/WS_xr
-python3 src/xr_rm_bringup/tools/launcher_ui.py
-```
-
-构建后也可以通过 ROS2 入口启动：
-
-```bash
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 run xr_rm_bringup launcher_ui
-```
-
-面板顶部的 `Mode` 分为四类：
-
-- `Simulation`：双臂 mock、XRoboToolkit bridge、双手 sample UDP 和 controller 频率监控。
-- `MuJoCo`：双臂 Mock/真机 MuJoCo launch、XRoboToolkit bridge 和 controller 频率监控；真机命令会连接两台 RM75。
-- `Real Hardware`：左右臂网络 ping、左臂/右臂/双臂真机 launch、XRoboToolkit bridge 和左右夹爪开合。
-- `Diagnostics`：`ros2 doctor --report`、四个核心包的 `ros2 pkg prefix`、controller 位置/频率监控。
-
-常用按钮：
-
-- `Run Selected`：运行当前选中的命令。双击列表项也可以运行。
-- `Check Env`：检查 ROS2 Humble、工作空间 build、终端、四个核心 ROS 包、睿尔曼 API2。
-- `Stop All`：结束由本工作空间启动的 launch、sample sender、topic monitor、MuJoCo viewer、相关 ROS 节点和终端窗口。
-
-`Stop All` 会保留现有 PC Service；点击启动器窗口 `X` 并确认退出时会额外停止 PC Service。两条清理路径都会停止 `dual_arm_simulator`，关闭 MuJoCo viewer。
-
-每个模式都会附带基础监控入口：
-
-- `Open Controller Topic Monitor`：同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
-- `Open ROS Topic/Node List Monitor`：每秒刷新 `ros2 topic list` 和 `ros2 node list`。
-
-`Simulation` 和 `MuJoCo` 模式还提供 `Open Controller Hz Monitor`；`Diagnostics` 同时提供 controller 位置与频率监控。
-
-分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：
-
-```bash
-sudo apt install terminator wmctrl xdotool
-sudo update-alternatives --config x-terminal-emulator
-```
-
-## 推荐调试顺序
-
-第一步：检查环境。
-
-打开 `launcher_ui.py`，点击 `Check Env`。如果 `install/setup.bash` 缺失，先回工作空间根目录重新执行 `colcon build --symlink-install`。
-
-第二步：分别跑左、右臂 mock 闭环。
-
-分两个终端依次验证左臂：
-
-```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
-ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port 15000 \
+ros2 run xr_rm_input sample_udp_sender \
+  --hand both --host 127.0.0.1 --port 15000 \
   --pattern axis_sweep --seconds 30
 ```
 
-停止左臂进程后，再分别验证右臂：
+单臂调试时将 `arm` 改为 `left` 或 `right`。推荐先分别完成左右单臂
+Mock，再进入双臂或真机验证。
 
-```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
-ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port 15000 \
-  --pattern axis_sweep --seconds 30
-```
-
-`sample_udp_sender` 默认使用 `axis_sweep` 扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
-
-观察：
-
-```bash
-ros2 topic echo /xr/left_controller
-ros2 topic echo /xr/right_controller
-ros2 topic echo /xr_rm/left_rm75/target_pose
-ros2 topic echo /xr_rm/right_rm75/target_pose
-ros2 topic echo /xr_rm/left_rm75/cmd_vel
-ros2 topic echo /xr_rm/right_rm75/cmd_vel
-```
-
-第三步：单臂真机。
-
-先只上一个臂，确认网络、方向、急停和限幅：
-
-```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
-```
-
-所有 YAML 默认都不会执行 `movej(initial_joint_pose)`。只有确认安全区清空后，才可在当前使用的
-`left_arm_rm75.yaml`、`right_arm_rm75.yaml` 或 `dual_arm_rm75.yaml` 中将
-`move_to_initial_pose_on_connect` 改为 `true`。
-
-第四步：双臂真机。
-
-```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
-```
-
-双臂默认不会自动移动到初始化点；机器人地址、控制参数和初始化移动开关均从
-`dual_arm_rm75.yaml` 读取。
-
-## Launch 入口说明
-
-`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 Simulation、MuJoCo 和 Real Hardware launch 命令都调用它。
-
-常用参数：
-
-- `arm`：`left`、`right`、`both`，默认 `right`。
-- `use_mock`：`true` 不连接真机，`false` 连接 RM75。
-- `use_mujoco`：`true` 额外启动双臂 MuJoCo 显示，默认 `false`，仅支持 `arm:=both`。
-- `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
-- `udp_port`：UDP 监听端口，默认 `15000`。
-- `udp_timer_hz`：UDP receiver 轮询频率，默认 `200.0`。
-
-机器人 IP/端口、控制频率、CANFD、限速、工具和初始化位姿等行为参数只由对应 YAML
-配置，launch 不再提供同名覆盖项。
-
-## MuJoCo 双臂仿真
-
-无真机时，由 Mock 关节状态驱动 MuJoCo：
+### MuJoCo
 
 ```bash
 ros2 launch xr_rm_bringup arm_debug.launch.py \
   arm:=both use_mock:=true use_mujoco:=true
 ```
 
-连接真机时，由两台 RM75 的实际关节反馈同步 MuJoCo。下面命令会连接真机，执行前必须完成真机安全检查：
+MuJoCo 只订阅关节状态，不参与控制。真机显示可将 `use_mock` 改为 `false`，
+但该命令会同时连接两台 RM75。
+
+### PICO 输入
+
+只保留一个 UDP 输入源，然后启动 XRoboToolkit bridge：
 
 ```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py \
-  arm:=both use_mock:=false use_mujoco:=true
-```
-
-桌面 UI 的 `MuJoCo` 模式分别提供上述 Mock 和真机命令，并明确标记会连接真机的 `Dual Arm MuJoCo Real Hardware Launch`。
-
-MuJoCo 只订阅当前关节状态，不参与控制，也不向真机下发指令：
-
-- `/xr_rm/left_rm75/joint_states`、`/xr_rm/right_rm75/joint_states`：当前适配器反馈；Mock 与真机模式均按控制周期约 `90 Hz` 发布。真机底层原始反馈周期仍为 `5 ms`（约 `200 Hz`），由遥操作节点按 `90 Hz` 采样发布。
-- `/xr_rm/left_rm75/joint_target`、`/xr_rm/right_rm75/joint_target`：经 QP 和现有限制处理后的目标关节角，仅用于调试，MuJoCo 不订阅。
-- MuJoCo viewer 默认按 `dual_arm_mujoco.yaml` 中的 `60 Hz` 刷新。其初始姿态直接来自 `dual_arm_rm75.yaml` 的 `initial_joint_pose`，不会在 MuJoCo YAML 中重复保存。
-
-Mock 模式的遥操作目标仍经过 `dual_arm_rm75.yaml` 中的工作空间、圆柱、线速度、角速度、关节速度/加速度、超时和停止限制。左手 X、右手 A 分别立即复位对应 Mock 机械臂；Grip 保持按下时，下一控制周期会重新锚定并继续遥操作。真机复位完成后仍需松开 Grip 才能恢复遥操作，且 `move_to_initial_pose_on_connect` 保持为 `false`，连接真机不会自动移动。
-
-## 配置文件说明
-
-`xr_rm_bringup/config/dual_arm_rm75.yaml` 是双臂配置主文件，包含两个 ROS 节点命名空间：
-
-- `left_arm_teleop`
-- `right_arm_teleop`
-
-`left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。
-
-`dual_arm_mujoco.yaml` 只保存 MuJoCo viewer 刷新频率；双臂初始关节角和遥操作限制继续统一读取 `dual_arm_rm75.yaml`。
-
-`xr_rm_bringup/config/peripherals_rm75.yaml` 保存真实控制器使用的末端工具坐标、负载和左右臂外设选择。左臂 `scissorgripper: 2` 是外设选择值，选择 `minisci`，TCP 的 Z 向偏移为 `0.165 m`；右臂 `scissorgripper: 1`，选择 `omnipic`，TCP 的 Z 向偏移为 `0.14 m`。真机连接阶段仍会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。
-
-Placo 使用 `xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`。左右 ROS 节点分别创建独立 solver：左臂从 `scissor_base_link` 到 `scissor_scissor_tcp`，并 mask 右臂；右臂从 `omnipic_base_link` 到 `omnipic_OmniPic_tcp`，并 mask 左臂。节点目标仍在各自局部基坐标系中，现有 PICO 映射不改为公共坐标系。
-
-重点控制参数：
-
-- `controller_topic`：订阅的手柄话题。
-- `scale`：手柄位移到 TCP 位移的比例。
-- `target_filter_alpha` / `target_filter_alpha_fast`：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
-- `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。
-- `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
-- `enable_orientation_control`：是否把手柄相对旋转映射到 TCP 姿态。
-- `orientation_filter_alpha` / `orientation_deadband_rad`：按 SO(3) 最短旋转角处理的目标 TCP 姿态滤波和死区。
-- `max_orientation_speed`：目标 TCP 姿态沿 SO(3) 最短路径的最大角速度，当前为 `0.5 rad/s`。
-- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
-- `cyl_radius_limit`：基座圆柱半径限制。
-- `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
-- `robot_ip` / `robot_port`：RM75 TCP 控制连接地址。
-- `realtime_push_host_ip`：连接机械臂 Wi-Fi 后本机实际 IPv4；可用
-  `ip -4 route get 192.168.192.19` 查看输出中的 `src`，当前为 `192.168.192.148`。
-- `realtime_push_port`：UDP 主动反馈端口；左臂 `8089`、右臂 `8090`，同机双臂不能重复。
-- `realtime_push_cycle_ms`：UDP 主动反馈周期，当前为厂商支持的 `5 ms`。
-- `follow` / `canfd_trajectory_mode`：`rm_movej_canfd` 的高跟随和轨迹模式参数。
-- 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证；确认关节加速度与反馈稳定后，再单独测试高跟随。
-- `initial_joint_pose`：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。
-
-当前 `/xr/*_controller` 的坐标处理：
-
-- XRoboToolkit bridge 原样转发 SDK 的手柄位置和四元数，不额外转换坐标轴。
-- receiver 默认按 `xyzw` 解析四元数，也可通过 `quat_order:=wxyz` 切换。
-- 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
-- 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。
-- 两侧局部 `-Y` 均指向机器人前方；局部 `+Y` 指向后方，后方工作空间仅保留 `0.10 m`。
-- 左臂局部 `+X/+Y/+Z` 分别指向下/后/左外侧；右臂分别指向上/后/右外侧。
-
-如果某个机械臂方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
-
-## 末端工具开合
-
-真机 launch 默认会在遥操作节点内启用工具控制。左/右手柄 `trigger` 从低于阈值按到 `>= 0.95` 时，会切换一次对应夹爪开/关状态，并保持到下一次点击。`grip` 仍只控制机械臂运动，不影响夹爪 trigger 切换。
-
-也可以用 Bool 话题手动控制开合，`true` 表示打开，`false` 表示闭合：
-
-```bash
-ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
-ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
-
-ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
-ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
-```
-
-桌面 UI 的 `Real Hardware` 模式提供 `Left/Right Gripper Open/Close` 命令项；运行双臂真机 launch 时也可直接通过左右手柄 `trigger` 分别切换夹爪。
-
-## UDP 数据格式
-
-当前 XRoboToolkit bridge 每个周期发送一个双手柄 JSON 包：
-
-```json
-{
-  "t": 12.345,
-  "source_time": 12.345,
-  "seq": 42,
-  "frame_id": "xr_world",
-  "controllers": {
-    "left": {
-      "hand": "left",
-      "grip": true,
-      "trigger": 0.0,
-      "axis": [0.2, -0.4],
-      "buttons": {
-        "primary": true,
-        "secondary": false
-      },
-      "pos": [-0.12, 1.05, 0.30],
-      "quat": [0.0, 0.0, 0.0, 1.0],
-      "pose_valid": true,
-      "pose_source": "xrobotoolkit"
-    },
-    "right": {
-      "hand": "right",
-      "grip": true,
-      "trigger": 1.0,
-      "axis": [-0.1, 0.3],
-      "buttons": {
-        "primary": false,
-        "secondary": true
-      },
-      "pos": [0.12, 1.05, 0.30],
-      "quat": [0.0, 0.0, 0.0, 1.0],
-      "pose_valid": true,
-      "pose_source": "xrobotoolkit"
-    }
-  }
-}
-```
-
-字段说明：
-
-- `t` / `source_time`：bridge 的 PC 单调时间，用于诊断发送周期。
-- `seq`：bridge 递增的 UDP 包序号，bridge 重启后重新计数。
-- `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
-- `grip`：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
-- `trigger`：经过 bridge 滞回处理的 `0.0/1.0` 值；上升沿切换对应夹爪状态。
-- `axis`：摇杆 `[x, y]`，每个分量限制在 `-1.0` 到 `1.0`。
-- `buttons.primary`：左手 X 键或右手 A 键。
-- `buttons.secondary`：左手 Y 键或右手 B 键。
-- `pos`：手柄位置，长度 3。
-- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
-- `pose_valid`：姿态是否可信；`false` 时接收端强制 `grip=false`。
-- `pose_source`：当前 bridge 使用 `xrobotoolkit`。
-
-`axis`、`buttons.primary` 和 `buttons.secondary` 会进入 `XrController`；旧 UDP
-包缺少这些字段时分别回退为 `[0,0]`、`false` 和 `false`。
-
-接收端发布的消息格式为：
-
-```text
-std_msgs/Header header
-string hand
-
-bool grip
-float32 trigger
-bool primary
-bool secondary
-float32[2] axis
-
-geometry_msgs/Pose pose
-```
-
-`udp_controller_receiver` 仍兼容调试用的单手柄包：可以直接发送带 `hand`、`pos`、
-`quat` 的 JSON object，也可以用 `controllers` list、顶层 `left/right`、
-`pose.position`、`position`、`p`、`q` 等常见字段。
-
-## 官方 XRoboToolkit bridge
-
-如果使用官方 XRoboToolkit APK 和 PC-Service，可以用 `xrobotoolkit_to_udp_bridge` 从本机 ROS Python 环境中的 `xrobotoolkit_sdk` 读取左右手柄数据，再转换成当前 `udp_controller_receiver` 支持的 UDP JSON。
-
-正式运行时不要同时启动官方 `PXREAClientUnity` / `RobotLinuxDemo` 可视化窗口。`/opt/apps/roboticsservice/run3D.sh` 会启动这个可视化 demo，适合单独确认 PICO 与 PC-Service 已连接；bridge 遥操作链路中只需要 PC-Service。
-
-运行前只保留一个 UDP 输入源。先清掉重复 bridge、sample sender 和官方 Unity 可视化 demo，再保留或启动 PC-Service：
-
-```bash
-pkill -f '[x]robotoolkit_to_udp_bridge'
-pkill -f '[s]ample_udp_sender'
-pkill -f '[R]obotLinuxDemo.x86_64'
-pkill -f '[P]XREAClientUnity'
-pgrep -af RoboticsServiceProcess || /opt/apps/roboticsservice/runService.sh
-```
-
-启动 ROS mock 接收链路：
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
-```
-
-另开终端启动 bridge：
-
-```bash
-cd /home/robot/WS_xr
-source ~/.bashrc
-source /opt/ros/humble/setup.bash
-source install/setup.bash
 ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
   --host 127.0.0.1 --port 15000 --hz 90
 ```
 
-bridge 默认对 grip/trigger 做轻量滞回：`grip` 按下阈值 `0.90`、松开阈值 `0.75`；`trigger` 按下阈值 `0.95`、松开阈值 `0.75`。启动日志会打印 PID、UDP endpoint 和阈值，便于确认当前只运行了一个 bridge。
-
-验证手柄数据是否进入 ROS：
+确认左右 topic 持续接收数据：
 
 ```bash
-ps -ef | grep -E 'xrobotoolkit_to_udp_bridge|sample_udp_sender|RobotLinuxDemo|PXREAClientUnity' | grep -v grep
 ros2 topic hz /xr/left_controller
 ros2 topic hz /xr/right_controller
-ros2 topic echo /xr/left_controller --field pose.position
-ros2 topic echo /xr/right_controller --field pose.position
-ros2 topic echo /xr/left_controller --field grip
-ros2 topic echo /xr/right_controller --field grip
-ros2 topic echo /xr/right_controller --field trigger
 ```
 
-`/xr/left_controller` 和 `/xr/right_controller` 持续刷新、位置随手柄移动变化、`grip` 随握持键切换，即表示官方 XRoboToolkit 数据已经进入当前遥操作输入层。
+图形启动面板可运行 `python3 src/xr_rm_bringup/tools/launcher_ui.py`，提供
+Simulation、MuJoCo、Real Hardware 和 Diagnostics 模式。
 
-## 真机安全验证
+### 真机
 
-第一次接真机时按这个顺序走：
+确认对应 YAML 中 `move_to_initial_pose_on_connect: false`，再从单臂开始：
 
-1. 确认急停、网络、机械臂工作区和人员位置。
-2. `launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75`。
-3. 确认对应 YAML 中 `move_to_initial_pose_on_connect: false` 后单臂启动。
-4. 手握急停，按住 `grip` 后只做小幅单轴移动。
-5. 逐个确认上/下、前/后、左/右方向。
-6. 小角度转动手柄，确认 `/xr_rm/<arm>/target_pose` 姿态和 `/xr_rm/<arm>/cmd_vel.twist.angular` 变化符合预期。
-7. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
-8. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
-9. 左右臂都确认后，再运行 `Dual Arm RealMan Launch`。
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
+```
 
-当前项目没有双臂碰撞检测/避障。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
+单臂方向、限位、急停、超时停止和夹爪均验证后，才能启动双臂：
 
-## 后续优化路线
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
+```
 
-为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
+按住 `grip` 控制对应机械臂；松开后停止。点击 `trigger` 切换对应夹爪开/关。
+左手 X、右手 A 会请求对应机械臂回到配置的初始位姿；真机使用前必须清空安全区。
 
-1. 稳定 PICO 数据链路：利用 `seq`、`source_time`、`pose_valid` 做频率、延迟、丢包和追踪状态统计，记录 `/xr/*_controller`、`/xr_rm/*/raw_target_pose`、`/xr_rm/*/target_pose`、`/xr_rm/*/target_clamped`、`/xr_rm/*/current_pose`。
-2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
-3. 细化末端执行器：增加夹爪状态反馈、力控比例、安全上限和现场可视化提示。
-4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
-5. 从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。
+## Launch 参数
 
-## 常见问题
+统一入口为 `xr_rm_bringup/launch/arm_debug.launch.py`：
 
-`launcher_ui.py` 提示找不到 `install/setup.bash`：
+| 参数 | 默认值 | 说明 |
+| --- | --- | --- |
+| `arm` | `right` | `left`、`right` 或 `both` |
+| `use_mock` | `true` | `false` 会连接真机 |
+| `use_mujoco` | `false` | 仅支持 `arm:=both` |
+| `udp_host` | `0.0.0.0` | UDP 监听地址 |
+| `udp_port` | `15000` | UDP 监听端口 |
+| `udp_timer_hz` | `200.0` | UDP receiver 轮询频率 |
+
+## 配置
+
+| 文件 | 用途 |
+| --- | --- |
+| `dual_arm_rm75.yaml` | 双臂节点、网络、控制与安全参数 |
+| `left_arm_rm75.yaml` | 左臂单独调试 |
+| `right_arm_rm75.yaml` | 右臂单独调试 |
+| `peripherals_rm75.yaml` | 工具坐标、负载和末端执行器 |
+| `dual_arm_mujoco.yaml` | MuJoCo 刷新频率 |
+
+修改某一侧控制参数时，同时检查单臂和双臂 YAML 是否需要同步。必须保留工作空间/
+圆柱限位、线速度与角速度限制、关节速度/加速度限制、指令超时和安全停止逻辑。
+
+`configure_safety_limits` 不得默认关闭；
+`move_to_initial_pose_on_connect` 必须保持默认 `false`。
+
+## 测试
+
+在工作空间根目录执行：
 
 ```bash
-cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 colcon build --symlink-install
-source install/setup.bash
+colcon test --event-handlers console_direct+
+colcon test-result --verbose
 ```
 
-真机模式提示缺少 `Robotic_Arm`：
+涉及遥操作姿态控制时，额外运行：
 
-```text
-未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。
+```bash
+pytest src/xr_rm_teleop/test/test_orientation_control.py
 ```
 
-Controller topic 没有数据：
-
-- 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
-- 确认端口是 `15000`，或 launch 与发送端端口一致。
-- 用 `sample_udp_sender` 在本机验证接收链路。
-- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed；SDK
-  读取失败时 bridge 会发送 `pose_valid=false` 的停止包。
-
-机械臂不动：
-
-- 确认 `grip=true`。
-- 确认 `udp_controller_receiver` 终端没有持续 `pose_valid=false` 日志；该字段不会写入 `XrController` 消息，但会让接收端强制停止。
-- 确认 `/xr_rm/<arm>/raw_target_pose` 与 `/xr_rm/<arm>/target_pose` 是否在变化。
-- 确认 `/xr_rm/<arm>/target_clamped` 是否持续为 `true`，如果是，目标 TCP 可能被工作空间、圆柱半径或单帧步长限制夹住。
-- 确认真机 SDK 连接成功，且 RM75 没有报警或急停。
+真机验证不属于自动测试。默认使用 `use_mock:=true`，未经现场安全确认不要连接或
+移动机械臂。

docs/superpowers/plans/2026-08-12-rm75-j3-simulation-calibration.md

@@ -0,0 +1,406 @@
+# RM75 双臂 J3 参考角仿真标定实施计划
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** 使用当前双臂 URDF、Placo QP 和 MuJoCo 运动学模型运行可复现的左右臂 J3 参考角粗扫与细扫，并输出评分、稳定区间和推荐角度。
+
+**Architecture:** 新增一个仅供离线实验使用的脚本，负责生成 18 条严格六维 TCP 轨迹、建立三类 QP 试验配置、运行候选角度扫描、计算硬门槛与并列评分，并生成 CSV/JSON/Markdown 结果。生产控制器、QP 求解器和 YAML 均不修改；测试只覆盖轨迹、评分和一个真实 Placo/MuJoCo 冒烟评估。
+
+**Tech Stack:** Python 3.10、NumPy、Placo 0.9.4、MuJoCo 3.10、pytest、ROS2 Humble 工作空间。
+
+---
+
+## 文件结构
+
+- 新增 `xr_rm_teleop/test/j3_reference_calibration.py`：离线轨迹生成、QP/MuJoCo 评估、评分、结果输出和命令行入口。
+- 新增 `xr_rm_teleop/test/test_j3_reference_calibration.py`：轨迹端点、严格姿态插值、百分位评分、平台选择和真实模型冒烟测试。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/summary.csv`：候选角度汇总。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/trajectories.csv`：逐轨迹指标。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/result.json`：机器可读结果。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/report.md`：左右臂推荐角度、平台区间、基线对比和最差轨迹。
+
+### Task 1：用失败测试固定轨迹与评分行为
+
+**Files:**
+- Create: `xr_rm_teleop/test/test_j3_reference_calibration.py`
+- Create: `xr_rm_teleop/test/j3_reference_calibration.py`
+
+- [ ] **Step 1：写轨迹生成失败测试**
+
+测试使用以下公开接口：
+
+```python
+def build_task_trajectories(
+    initial_world_pose: np.ndarray,
+    control_rate_hz: float = 90.0,
+    max_linear_speed: float = 0.15,
+    max_angular_speed: float = 0.5,
+) -> list[Trajectory]:
+    ...
+```
+
+断言：
+
+```python
+def test_build_task_trajectories_creates_nine_strict_6d_routes() -> None:
+    initial = np.eye(4)
+    initial[:3, 3] = [0.35, 0.20, 0.10]
+
+    trajectories = build_task_trajectories(initial)
+
+    assert len(trajectories) == 9
+    assert {(route.harvest_y, route.harvest_z) for route in trajectories} == {
+        (y, z)
+        for y in (0.30, 0.40, 0.50)
+        for z in (-0.30, -0.20, -0.10)
+    }
+    for route in trajectories:
+        assert np.allclose(route.poses[0], initial)
+        assert np.allclose(route.poses[-1], initial)
+        basket = route.waypoints[5]
+        assert basket[0, 3] == pytest.approx(initial[0, 3])
+        assert basket[1, 3] == pytest.approx(initial[1, 3])
+        assert basket[2, 3] == pytest.approx(initial[2, 3] - 0.40)
+        assert basket[:3, 2] == pytest.approx([0.0, 0.0, -1.0], abs=1e-6)
+```
+
+- [ ] **Step 2：写评分和平台选择失败测试**
+
+公开接口：
+
+```python
+def rank_candidates(rows: list[CandidateMetrics]) -> list[CandidateMetrics]:
+    ...
+
+def choose_stable_platform(
+    ranked: list[CandidateMetrics],
+    scan_step_deg: float,
+) -> tuple[float, tuple[float, float]]:
+    ...
+```
+
+测试构造三个硬门槛相同的候选，断言评分严格等于：
+
+```python
+score = (
+    0.45 * r_sigma
+    + 0.20 * r_q4
+    + 0.20 * r_elbow
+    + 0.10 * r_smooth
+    + 0.05 * r_track
+)
+```
+
+并断言连续候选均达到最高分的 98% 时返回平台中点，而不是孤立端点。
+
+- [ ] **Step 3：运行测试并确认按预期失败**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
+```
+
+Expected: FAIL，原因是 `j3_reference_calibration` 或公开函数尚不存在。
+
+### Task 2：实现最小轨迹与评分模块
+
+**Files:**
+- Create: `xr_rm_teleop/test/j3_reference_calibration.py`
+- Test: `xr_rm_teleop/test/test_j3_reference_calibration.py`
+
+- [ ] **Step 1：实现旋转和 SE(3) 插值**
+
+只使用 NumPy 和标准库：
+
+```python
+def rotation_angle(rotation: np.ndarray) -> float:
+    cosine = np.clip((np.trace(rotation) - 1.0) * 0.5, -1.0, 1.0)
+    return float(math.acos(cosine))
+
+
+def rotation_vector(rotation: np.ndarray) -> np.ndarray:
+    angle = rotation_angle(rotation)
+    if angle <= 1e-12:
+        return np.zeros(3)
+    axis = np.array([
+        rotation[2, 1] - rotation[1, 2],
+        rotation[0, 2] - rotation[2, 0],
+        rotation[1, 0] - rotation[0, 1],
+    ]) / (2.0 * math.sin(angle))
+    return axis * angle
+
+
+def interpolate_pose(start: np.ndarray, end: np.ndarray, count: int) -> list[np.ndarray]:
+    relative = end[:3, :3] @ start[:3, :3].T
+    vector = rotation_vector(relative)
+    return [
+        make_pose(
+            start[:3, 3] + alpha * (end[:3, 3] - start[:3, 3]),
+            so3_exp(alpha * vector) @ start[:3, :3],
+        )
+        for alpha in np.linspace(0.0, 1.0, count + 1)[1:]
+    ]
+```
+
+`rotation_vector` 对接近 180° 的情况使用特征向量兜底，避免工具朝下转换产生除零。
+
+- [ ] **Step 2：实现九条本侧完整轨迹**
+
+定义不可变数据类：
+
+```python
+@dataclass(frozen=True)
+class Trajectory:
+    name: str
+    harvest_y: float
+    harvest_z: float
+    waypoints: tuple[np.ndarray, ...]
+    poses: tuple[np.ndarray, ...]
+```
+
+航点固定为：初始、预接近、采摘、预接近、筐上方、筐内、筐上方、初始。每段点数为：
+
+```python
+duration = max(
+    translation_distance / max_linear_speed,
+    rotation_distance / max_angular_speed,
+)
+steps = max(1, math.ceil(duration * control_rate_hz))
+```
+
+- [ ] **Step 3：实现百分位排名和并列评分**
+
+同值获得同一百分位，单一取值获得 1.0。平滑性和跟踪排名分别定义为：
+
+```python
+r_smooth = 0.5 * rank_low(motion_cost) + 0.5 * rank_low(max_joint_speed)
+r_track = 0.5 * rank_low(max_position_error) + 0.5 * rank_low(max_orientation_error)
+```
+
+硬门槛按 `(N_fail, -N_complete)` 字典序先筛选；只有满足位置误差、姿态误差、J4、
+关节限位、速度和跳变条件的候选进入综合评分。
+
+- [ ] **Step 4：运行测试确认通过**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
+```
+
+Expected: 轨迹与评分测试 PASS。
+
+### Task 3：用失败测试固定真实 Placo/MuJoCo 单轨迹评估
+
+**Files:**
+- Modify: `xr_rm_teleop/test/test_j3_reference_calibration.py`
+- Modify: `xr_rm_teleop/test/j3_reference_calibration.py`
+
+- [ ] **Step 1：写真实模型冒烟失败测试**
+
+接口：
+
+```python
+def evaluate_trajectory(
+    arm: str,
+    trajectory: Trajectory,
+    variant: Variant,
+    urdf_path: Path,
+    initial_joint_degrees: tuple[float, ...],
+) -> TrajectoryMetrics:
+    ...
+```
+
+使用左臂从初始 TCP 沿公共 `+Y` 移动 1 mm 的两点轨迹，断言：
+
+```python
+assert metrics.cycles == 2
+assert metrics.failures == 0
+assert math.isfinite(metrics.min_sigma)
+assert metrics.min_q4_margin_deg > 0.0
+assert metrics.max_position_error_m <= 2e-3
+assert metrics.max_orientation_error_rad <= 5e-3
+```
+
+测试还将求得的七关节状态写入 `DualArmKinematicModel` 并断言按名称读回一致。
+
+- [ ] **Step 2：运行冒烟测试并确认按预期失败**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py::test_evaluate_trajectory_uses_real_placo_and_mujoco -q
+```
+
+Expected: FAIL，原因是评估器尚未实现。
+
+- [ ] **Step 3：实现三类 QP 变体**
+
+```python
+@dataclass(frozen=True)
+class Variant:
+    name: str
+    q3_reference_deg: float | None
+    enable_manipulability: bool
+    q4_min_deg: float | None
+```
+
+- `original`：三个可选项均关闭；
+- `manip_j4`：位置可操作度权重 `1e-4`，J4 硬下限 10°；
+- `q3_<angle>`：在 `manip_j4` 基础上加入 J3 软任务，权重 `1e-5`。
+
+J4 约束使用 Placo 0.9.4 的 `add_joint_space_half_spaces_constraint(A, b)`，构造
+`-q4 <= -q4_min`。J3 使用 `add_joints_task()`；位置可操作度使用
+`add_manipulability_task(tcp_frame, "position", 1.0)`。
+
+- [ ] **Step 4：实现逐周期评估与失败保持**
+
+每个目标点前将上一有效关节状态同步给 Placo。求解失败时：
+
+```python
+failures += 1
+solver.update_joint_state(last_valid_joints)
+current_joints = last_valid_joints.copy()
+```
+
+不把失败后的 Placo 内部迭代状态带到下一周期。成功状态写入 MuJoCo，并记录六维
+雅可比最小奇异值、J4 余量、肘部外展量、TCP 误差、关节速度和运动代价。
+
+- [ ] **Step 5：运行全部标定脚本测试**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
+```
+
+Expected: 全部 PASS。
+
+### Task 4：运行粗扫、细扫并生成结果
+
+**Files:**
+- Modify: `xr_rm_teleop/test/j3_reference_calibration.py`
+- Generate: `docs/superpowers/results/2026-08-12-rm75-j3-calibration/*`
+
+- [ ] **Step 1：实现命令行和结果输出**
+
+命令行：
+
+```bash
+python j3_reference_calibration.py \
+  --urdf <path> \
+  --config <dual_arm_rm75.yaml> \
+  --output-dir <directory> \
+  --phase coarse|fine|all
+```
+
+粗扫结束后对每侧选择最高分候选，在其 ±10°、原扫描边界内以 2° 细扫。CSV 使用
+`csv.DictWriter`，JSON 使用 `json.dump`，Markdown 报告由同一汇总对象生成，不新增依赖。
+
+- [ ] **Step 2：运行完整仿真标定**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python \
+  src/xr_rm_teleop/test/j3_reference_calibration.py \
+  --urdf src/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf \
+  --config src/xr_rm_bringup/config/dual_arm_rm75.yaml \
+  --output-dir src/docs/superpowers/results/2026-08-12-rm75-j3-calibration \
+  --phase all
+```
+
+Expected: 左右臂粗扫和细扫完成；输出两个基线、全部候选、推荐角度和平台区间。
+
+- [ ] **Step 3：检查结果完整性**
+
+Run:
+
+```bash
+/home/robot/miniconda3/envs/xr/bin/python - <<'PY'
+import json
+from pathlib import Path
+
+path = Path('src/docs/superpowers/results/2026-08-12-rm75-j3-calibration/result.json')
+data = json.loads(path.read_text(encoding='utf-8'))
+assert set(data['arms']) == {'left', 'right'}
+for arm in data['arms'].values():
+    assert arm['coarse_candidates']
+    assert arm['fine_candidates']
+    assert arm['recommended_reference_deg'] is not None
+    assert len(arm['stable_interval_deg']) == 2
+print('result integrity: OK')
+PY
+```
+
+Expected: `result integrity: OK`。
+
+### Task 5：工作空间验证与结果复核
+
+**Files:**
+- Verify only.
+
+- [ ] **Step 1：运行新增测试和相关现有测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py \
+  src/xr_rm_teleop/test/test_placo_transforms.py \
+  src/xr_rm_mujoco/test/test_dual_arm_simulator.py -q
+```
+
+Expected: 全部 PASS。
+
+- [ ] **Step 2：按项目规则构建工作空间**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+Expected: 相关 ROS2 包构建成功。
+
+- [ ] **Step 3：运行姿态控制回归测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_orientation_control.py -q
+```
+
+Expected: 全部 PASS。
+
+- [ ] **Step 4：人工复核结果报告**
+
+确认：
+
+- 每侧确有 9 条完整轨迹；
+- `original`、`manip_j4` 和 J3 候选均存在；
+- 推荐角来自硬门槛通过集合；
+- 平台选择符合 98% 规则；
+- 报告明确列出失败轨迹，且没有把失败更多的候选排到前面；
+- 没有修改生产控制器和 YAML。

docs/superpowers/plans/2026-08-13-rm75-qp-robustness.md

@@ -0,0 +1,372 @@
+# RM75 双臂采摘 QP 稳健性优化实施计划
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** 在当前双臂严格六维遥操作链路中实现 QP 失败参考状态保持、J3 初始姿态软引导、J4 硬下限与软缓冲，以及按六维奇异值动态启用的可操作度任务。
+
+**Architecture:** 保留 Placo 相对六维位姿主任务和下游关节速度/加速度限制。遥操作层将滤波结果作为候选值，只有 QP 求解和关节发送都成功后才提交；QP 求解器复用 Placo 现有 joints、half-space 和 manipulability 任务，不新增求解框架或依赖。
+
+**Tech Stack:** Python 3.10、ROS2 Humble、Placo 0.9.4、NumPy、pytest、ament/colcon。
+
+---
+
+## 文件结构
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：QP 失败状态和笛卡尔参考状态提交。
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`：J3、J4、动态六维可操作度和失败状态恢复。
+- 修改 `xr_rm_teleop/test/test_joint_control.py`：失败不发送、不提交和发送失败保持测试。
+- 修改 `xr_rm_teleop/test/test_placo_transforms.py`：辅助任务参数、激活函数和真实模型测试。
+- 修改 `xr_rm_teleop/test/test_initial_joint_pose.py`：三份 YAML 的 QP 参数一致性测试。
+- 修改 `xr_rm_bringup/config/dual_arm_rm75.yaml`：左右臂独立 QP 参数。
+- 修改 `xr_rm_bringup/config/left_arm_rm75.yaml`：左臂 QP 参数。
+- 修改 `xr_rm_bringup/config/right_arm_rm75.yaml`：右臂 QP 参数。
+
+### Task 1：QP 失败时不提交笛卡尔参考状态
+
+**Files:**
+- Modify: `xr_rm_teleop/test/test_joint_control.py`
+- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- [ ] **Step 1：修改 QP 失败测试并增加候选滤波测试**
+
+把现有失败测试改为要求 `_solve_joint_target()` 返回 `None`，同时增加位置和姿态滤波只计算候选、不直接修改已提交状态的断言：
+
+```python
+def test_qp_failure_returns_none_and_keeps_last_known_good_target() -> None:
+    ...
+    target = teleop._solve_joint_target(np.eye(4))
+    assert target is None
+    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
+
+
+def test_target_filters_do_not_commit_candidate_state() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._filtered_target = [0.0, 0.0, 0.0]
+    teleop._filtered_orientation_target = np.eye(3)
+    teleop._target_filter_alpha = 0.5
+    teleop._target_filter_alpha_fast = 0.5
+    teleop._target_filter_fast_threshold_m = 1.0
+    teleop._orientation_filter_alpha = 0.5
+
+    position = teleop._filter_target([0.2, 0.0, 0.0])
+    orientation = teleop._filter_orientation_target(
+        _so3_exp(np.asarray([0.0, 0.0, 0.2]))
+    )
+
+    assert position == pytest.approx([0.1, 0.0, 0.0])
+    assert teleop._filtered_target == pytest.approx([0.0, 0.0, 0.0])
+    assert teleop._filtered_orientation_target == pytest.approx(np.eye(3))
+    assert np.linalg.norm(_so3_log(orientation)) == pytest.approx(0.1)
+```
+
+- [ ] **Step 2：运行新测试并确认按预期失败**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'qp_failure or target_filters_do_not_commit' -q
+```
+
+Expected: FAIL；当前失败路径仍返回旧关节数组，滤波函数会立即修改成员状态。
+
+- [ ] **Step 3：实现最小失败保持逻辑**
+
+修改 `_filter_target()` 和 `_filter_orientation_target()` 只返回候选值，不直接写成员。
+修改 `_solve_joint_target()` 在异常时返回 `None`，成功时也不提前更新
+`_last_valid_joint_target`。控制周期只在结果非空时发送，并在发送成功后统一提交：
+
+```python
+joint_target = self._solve_joint_target(target_pose)
+sent = (
+    joint_target is not None
+    and self._send_joint_target(joint_target)
+)
+if sent:
+    self._last_valid_joint_target = list(joint_target)
+    self._filtered_target = list(filtered_target)
+    self._filtered_orientation_target = filtered_orientation.copy()
+    self._last_sent_target = sent_target
+    self._last_sent_orientation = sent_orientation.copy()
+    self._last_command_time = now
+    self._stop_sent = False
+```
+
+失败时不调用 `_send_joint_target()`，因此不会把旧关节保持动作伪装成新 QP 成功；已
+存在的指令超时和反馈故障保持逻辑不改变。
+
+- [ ] **Step 4：运行关节控制测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_joint_control.py -q
+```
+
+Expected: PASS。
+
+### Task 2：J3、J4 与动态六维可操作度
+
+**Files:**
+- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
+- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
+
+- [ ] **Step 1：写辅助任务激活和参数失败测试**
+
+增加纯激活函数测试：
+
+```python
+def test_lower_margin_activation_is_clamped_and_linear() -> None:
+    assert _lower_margin_activation(0.05, 0.01, 0.04) == 0.0
+    assert _lower_margin_activation(0.025, 0.01, 0.04) == pytest.approx(0.5)
+    assert _lower_margin_activation(0.005, 0.01, 0.04) == 1.0
+```
+
+增加真实左右臂求解器测试，构造时传入：
+
+```python
+solver = PlacoIkSolver(
+    str(DUAL_URDF_PATH),
+    1.0 / 90.0,
+    arm,
+    j3_reference_deg=j3_reference_deg,
+    j3_weight=1e-5,
+    j4_min_deg=10.0,
+    j4_warn_deg=25.0,
+    j4_weight=1e-4,
+    manipulability_sigma_stop=0.01,
+    manipulability_sigma_warn=0.04,
+    manipulability_weight=1e-4,
+)
+```
+
+断言 J3 任务目标等于该侧参考角、J4 half-space 为 `-q4 <= -10°`，六维雅可比为
+`6x7` 且奇异值有限。
+
+- [ ] **Step 2：运行新测试并确认按预期失败**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py \
+  -k 'lower_margin_activation or auxiliary_qp_tasks' -q
+```
+
+Expected: FAIL；激活函数和构造参数尚不存在。
+
+- [ ] **Step 3：实现 Placo 辅助任务**
+
+新增 `_lower_margin_activation(value, stop, warn)`，并在构造器中验证有限参数及
+`j4_warn > j4_min`、`sigma_warn > sigma_stop > 0`。复用 Placo 原生接口：
+
+```python
+self._j3_task = self._solver.add_joints_task()
+self._j3_task.set_joints({self._joint_names[2]: np.deg2rad(j3_reference_deg)})
+self._j3_task.configure("j3_reference", "soft", j3_weight)
+
+self._j4_task = self._solver.add_joints_task()
+self._j4_task.set_joints({self._joint_names[3]: np.deg2rad(j4_warn_deg)})
+
+matrix = np.zeros((1, self._robot.state.q.size))
+matrix[0, self._q_offsets[3]] = -1.0
+self._j4_constraint = self._solver.add_joint_space_half_spaces_constraint(
+    matrix,
+    np.asarray([-np.deg2rad(j4_min_deg)]),
+)
+self._j4_constraint.configure("j4_lower_bound", "hard")
+
+self._manipulability_task = self._solver.add_manipulability_task(
+    self._tcp_frame,
+    "both",
+    1.0,
+)
+```
+
+每次数值迭代前，从 `frame_jacobian(..., "local_world_aligned")` 的当前臂 `6x7`
+雅可比计算 `sigma_min`。J4 和可操作度任务分别使用线性夹紧激活系数重新配置软权重；
+J3 权重使用节点传入的左右臂独立配置。启用 Placo 原生关节限位，保留现有速度限位
+和结果校验。
+
+- [ ] **Step 4：失败时恢复 Placo 到实际关节反馈**
+
+在 `solve()` 入口保存实际关节状态；任何求解异常或 30 次未收敛时，将活动臂关节
+恢复到 `_actual_joints` 并更新运动学后重新抛出异常。测试制造不收敛，断言内部活动
+关节未停留在失败迭代结果。
+
+- [ ] **Step 5：运行 Placo 测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py -q
+```
+
+Expected: PASS。
+
+### Task 3：同步节点和三份控制配置
+
+**Files:**
+- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
+- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
+- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
+- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
+
+- [ ] **Step 1：写三份 YAML 一致性失败测试**
+
+扩展现有 YAML 参数化测试，断言左右臂分别为：
+
+```python
+expected = {
+    "left": {
+        "qp_j3_reference_deg": 67.96,
+        "qp_j3_weight": 1e-5,
+    },
+    "right": {
+        "qp_j3_reference_deg": -89.57,
+        "qp_j3_weight": 1e-4,
+    },
+}
+shared = {
+    "qp_j4_min_deg": 10.0,
+    "qp_j4_warn_deg": 25.0,
+    "qp_j4_weight": 1e-4,
+    "qp_manipulability_sigma_stop": 0.01,
+    "qp_manipulability_sigma_warn": 0.04,
+    "qp_manipulability_weight": 1e-4,
+}
+```
+
+同时断言单臂 YAML 与双臂同侧节点值一致。
+
+- [ ] **Step 2：运行配置测试并确认按预期失败**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_initial_joint_pose.py -q
+```
+
+Expected: FAIL；QP 参数尚未写入 YAML。
+
+- [ ] **Step 3：声明、读取并传入 QP 参数**
+
+节点声明上述八个 `qp_*` 参数，进行有限性和大小关系验证，并作为关键字参数传入
+`PlacoIkSolver`。三份 YAML 同步写入相同共享参数，J3 只按左右臂设置不同参考角；
+不修改 `configure_safety_limits` 和 `move_to_initial_pose_on_connect`。
+
+- [ ] **Step 4：运行配置和遥操作姿态测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_initial_joint_pose.py \
+  src/xr_rm_teleop/test/test_orientation_control.py -q
+```
+
+Expected: PASS。
+
+### Task 4：完整验证和本地提交
+
+**Files:**
+- Verify all modified files.
+
+- [ ] **Step 1：运行遥操作包测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test -q
+```
+
+Expected: 全部 PASS，无失败。
+
+- [ ] **Step 2：运行真实 URDF 左右臂 QP 冒烟测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH /home/robot/miniconda3/envs/xr/bin/python \
+  src/xr_rm_teleop/test/placo_ik_smoke.py \
+  src/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf
+```
+
+Expected: 左右臂保持位姿漂移和 1 cm 六维 QP 冒烟断言均通过。
+
+- [ ] **Step 3：构建 ROS2 工作空间**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+Expected: 所有工作空间包构建成功。
+
+- [ ] **Step 4：检查差异与安全配置**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr/src
+git diff --check
+git diff --stat
+rg -n "configure_safety_limits: true|move_to_initial_pose_on_connect: false" \
+  xr_rm_bringup/config/{dual_arm_rm75,left_arm_rm75,right_arm_rm75}.yaml
+```
+
+Expected: 无空白错误，三份配置继续保留安全设置。
+
+- [ ] **Step 5：创建本地提交**
+
+规格文档和实施计划必须在同一个本地提交中；实现与测试一并纳入该提交，避免文档和
+代码版本不一致：
+
+```bash
+git add \
+  docs/superpowers/specs/2026-08-13-rm75-qp-robustness-design.md \
+  docs/superpowers/plans/2026-08-13-rm75-qp-robustness.md \
+  xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
+  xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  xr_rm_teleop/test/test_joint_control.py \
+  xr_rm_teleop/test/test_placo_transforms.py \
+  xr_rm_teleop/test/test_initial_joint_pose.py \
+  xr_rm_bringup/config/dual_arm_rm75.yaml \
+  xr_rm_bringup/config/left_arm_rm75.yaml \
+  xr_rm_bringup/config/right_arm_rm75.yaml
+git commit -m "feat: 优化双臂采摘QP稳健性"
+```
+
+禁止 `git push`、合并分支或连接真机。

docs/superpowers/specs/2026-08-12-rm75-j3-simulation-calibration-design.md

@@ -0,0 +1,245 @@
+# RM75 双臂 J3 参考角仿真标定设计
+
+## 1. 目标
+
+在不连接真机、不修改现有生产控制参数的前提下，基于当前双臂 URDF、Placo QP
+求解器和 MuJoCo 运动学模型，分别标定左臂与右臂的第三关节软引导参考角：
+
+\[
+q_{3,\mathrm{ref}}^{L,*},\qquad q_{3,\mathrm{ref}}^{R,*}。
+\]
+
+标定结果只作为当前机器人初始姿态、采摘区域、工具安装和本侧收集筐布局下的
+仿真初值。后续必须通过 mock 完整控制链路和真机低速试验复验，允许根据实测结果
+更新参数。
+
+## 2. 范围
+
+本轮只做离线参数标定：
+
+- 左右臂分别从当前 YAML 初始关节姿态出发；
+- 左臂放入左臂初始 TCP 下方约 40 cm 的本侧收集筐；
+- 右臂放入右臂初始 TCP 下方约 40 cm 的本侧收集筐；
+- 每个轨迹点同时指定 TCP 位置与姿态，保持严格六维跟踪；
+- 第四关节下限暂定左右臂均为 10°；
+- 使用现有 Placo 任务接口临时加入 J3 软任务和位置可操作度任务；
+- 将每个有效关节结果同步写入现有 MuJoCo 双臂模型，检查关节映射和状态有效性；
+- 输出候选角度的逐轨迹指标、汇总排名和推荐平台区间。
+
+本轮不修改 `placo_ik_solver.py`、遥操作节点或 YAML，不测试真机，不加入任务阶段
+状态机、自动姿态放松、碰撞规划或新依赖。
+
+## 3. 坐标与姿态约定
+
+- 双臂机器人公共坐标系 `+Y` 为正前方；
+- 公共坐标系 `+Z` 为机器人垂直向上；
+- 左右方向使用公共坐标系 `X`；
+- 采摘点保持对应机械臂初始 TCP 的横向 `X` 位置；
+- 采摘与退出阶段保持初始 TCP 姿态；
+- 从退出点移动到收集筐上方时，TCP 姿态采用四元数球面插值，平滑旋转为工具工作
+  轴沿公共坐标系 `-Z`；
+- 收集筐上方至筐内的垂直下降段保持工具朝下姿态；
+- 返回初始位姿时平滑恢复初始 TCP 姿态。
+
+“严格六维”表示每个时刻的位置和姿态目标均参与同一 QP，仿真不会因接近奇异点
+而自动降低姿态权重。
+
+## 4. 轨迹族
+
+### 4.1 采摘点
+
+每条机械臂使用 3 个前向距离和 3 个高度：
+
+\[
+y_h\in\{0.30,0.40,0.50\}\ \mathrm{m},
+\]
+
+\[
+z_h\in\{-0.30,-0.20,-0.10\}\ \mathrm{m}。
+\]
+
+这些点位于用户给定的前方 30～50 cm、相对机械臂基座高度 ±50 cm 范围内，并且
+是当前固定初始工具姿态下离线预扫描得到的主要可解高度区间。左右臂各 9 条轨迹，
+共 18 条完整轨迹。
+
+### 4.2 单条完整轨迹
+
+每条轨迹由以下连续段组成：
+
+1. 初始 TCP 位姿；
+2. 采摘点前方 5 cm 的预接近点；
+3. 沿公共 `+Y` 直线进入采摘点；
+4. 沿原路径退回预接近点；
+5. 移动到本侧收集筐上方 10 cm，同时平滑旋转到工具朝下；
+6. 垂直下降 10 cm，到达初始 TCP 下方约 40 cm 的收集筐目标；
+7. 垂直抬升 10 cm；
+8. 返回初始 TCP 位姿。
+
+轨迹按当前控制频率 90 Hz 离散，平移速度不超过 0.15 m/s，角速度不超过
+0.5 rad/s。每条轨迹均从相同初始关节状态重新开始，避免上一候选角或上一轨迹的
+状态污染下一次评估。
+
+### 4.3 边界轨迹
+
+工作区边界、不可达目标和 QP 失败恢复轨迹不参与第一轮 J3 参数排名。J3 参数确定后，
+再使用这些轨迹验证失败保持和恢复逻辑，避免不可达点数量掩盖参考角本身的差异。
+
+## 5. QP 试验配置
+
+主任务保持现有严格六维相对位姿任务，内部等价于：
+
+\[
+\left\|J_p\Delta q-e_p\right\|^2
++\left\|J_R\Delta q-e_R\right\|^2，
+\]
+
+其中误差定义为目标减当前。仿真脚本不改变 Placo 的误差符号。
+
+在主任务之外临时加入：
+
+\[
+w_e\left(q_3+\Delta q_3-q_{3,\mathrm{ref}}\right)^2,
+\qquad w_e=10^{-5}，
+\]
+
+以及 TCP 位置可操作度任务：
+
+\[
+-w_m\nabla m_p(q)^T\Delta q,
+\qquad w_m=10^{-4}。
+\]
+
+保留当前动能正则、URDF 关节位置限制和关节速度限制。第四关节临时增加：
+
+\[
+q_4\ge10^\circ。
+\]
+
+## 6. 参数扫描
+
+### 6.1 粗扫
+
+左臂：
+
+\[
+q_{3,\mathrm{ref}}^L\in\{0^\circ,10^\circ,\ldots,100^\circ\}。
+\]
+
+右臂：
+
+\[
+q_{3,\mathrm{ref}}^R\in\{0^\circ,-10^\circ,\ldots,-120^\circ\}。
+\]
+
+### 6.2 细扫
+
+在粗扫最优候选附近 ±10° 内以 2° 为步长再次扫描。若多个相邻候选没有明显差异，
+选择稳定平台区的中心，而不是选择孤立的单点峰值。
+
+### 6.3 基线
+
+同时运行两组基线：
+
+- `original`：当前原始 QP，不含 J3 软任务、位置可操作度任务和 J4 额外下限；
+- `manip_j4`：不含 J3 软任务，但加入位置可操作度任务和 J4 额外下限。
+
+所有 J3 候选均在 `manip_j4` 基础上只改变 J3 参考角。最终结果必须同时报告：
+
+- 相对当前原始 QP 的改善；
+- 相对“只加位置可操作度”的改善；
+- J3 软任务是否降低六维跟踪成功率。
+
+## 7. 记录指标
+
+对每个候选角度、每条轨迹记录：
+
+- QP 求解失败周期数 `N_fail`；
+- 完成全部轨迹的数量 `N_complete`；
+- 整条轨迹六维雅可比的最小奇异值 `sigma_min`；
+- 第四关节最小安全余量 `m_q4 = min(q4 - 10°)`；
+- 肘部最小外展量 `d_elbow`；
+- 最大 TCP 位置误差和姿态误差；
+- 最大关节速度；
+- 累计关节运动代价 `E_q`；
+- 是否出现超过阈值的单周期关节构型跳变。
+
+肘部外展量使用公共坐标系中第四连杆位置计算：
+
+\[
+d_{\mathrm{elbow}}^L=-x_{\mathrm{elbow}}^L,
+\qquad
+d_{\mathrm{elbow}}^R=x_{\mathrm{elbow}}^R。
+\]
+
+当前 URDF 碰撞网格对相邻连杆存在已知自碰撞警告，因此本轮不把 MuJoCo/Placo
+碰撞距离加入评分，避免错误碰撞几何影响 J3 选择。
+
+## 8. 选择规则与评分函数
+
+### 8.1 硬门槛
+
+候选角度首先按以下顺序筛选：
+
+1. `N_fail` 最少；
+2. `N_complete` 最多；
+3. 最大位置误差不超过 2 mm；
+4. 最大姿态误差不超过 0.005 rad；
+5. 不违反第四关节、URDF 关节位置和速度限制；
+6. 不出现超过配置阈值的单周期关节跳变。
+
+只在通过同一组硬门槛的候选之间使用评分函数。这样不能用较高可操作度抵消更多的
+QP 失败或更差的 TCP 跟踪。
+
+### 8.2 并列候选评分函数
+
+对通过硬门槛的候选，将各项指标在同一机械臂的候选集合内转换为 `[0,1]` 的百分位
+排名。数值越大越好的指标直接排名，数值越小越好的指标反向排名：
+
+- `r_sigma`：全轨迹最小奇异值排名；
+- `r_q4`：第四关节最小安全余量排名；
+- `r_elbow`：肘部最小外展量排名；
+- `r_smooth`：累计关节运动代价与最大关节速度的联合反向排名；
+- `r_track`：最大六维 TCP 跟踪误差的反向排名。
+
+并列候选的综合评分为：
+
+\[
+S(q_{3,\mathrm{ref}})
+=0.45r_{\sigma}
++0.20r_{q4}
++0.20r_{\mathrm{elbow}}
++0.10r_{\mathrm{smooth}}
++0.05r_{\mathrm{track}}。
+\]
+
+选择：
+
+\[
+q_{3,\mathrm{ref}}^*=\arg\max S(q_{3,\mathrm{ref}})。
+\]
+
+最小奇异值权重最高，因为本轮首要目标是降低奇异点和 QP 失败风险；第四关节余量
+和肘部外展各占 0.20；平滑性和跟踪误差用于区分性能接近的候选。若评分最高点与
+相邻角度差异小于 2%，取相邻稳定平台的中心角度。
+
+### 8.3 结果报告
+
+左右臂分别输出：
+
+- 推荐参考角；
+- 推荐稳定区间；
+- 粗扫与细扫排名表；
+- 与两组基线的指标对比；
+- 最差轨迹及其失败位置；
+- 是否建议保留左右臂统一的第四关节 10° 下限。
+
+## 9. 实施边界与后续流程
+
+标定完成后的顺序为：
+
+1. 根据仿真结果形成正式 QP 修改规格；
+2. 将左右臂 J3 参考角作为独立可调参数写入对应 YAML；
+3. 在 `use_mock:=true` 下运行完整遥操作控制链路；
+4. 加入 QP 失败时不提交笛卡尔目标历史的修复并验证恢复；
+5. 经过安全评审后，在真机上以低速、小范围方式复验；
+6. 根据真机日志更新 J3 参考角，但不取消工作空间、速度、超时和安全停止限制。

docs/superpowers/specs/2026-08-13-rm75-qp-robustness-design.md

@@ -0,0 +1,175 @@
+# RM75 双臂采摘 QP 稳健性优化方案概述
+
+## 1. 目标与边界
+
+本方案面向当前双臂机器人从初始位姿向机器人公共坐标系 `+Y` 前方采摘，再移动到
+本侧机械臂初始 TCP 正下方约 40 cm、位于底盘车上的收集筐这一流程。首要目标是：
+
+- 保持手柄给出的 TCP 位置和姿态严格参与六维逆解；
+- 减少奇异点附近的构型恶化、QP 不收敛和连续失败；
+- QP 失败时保持上一安全关节解，并且不提交本周期笛卡尔参考状态；
+- 保留现有工作空间、速度、加速度、指令超时和安全停止限制。
+
+本轮不加入自动采摘状态机、自动放松姿态或真机自动运动，不取消现有安全限制。
+
+此前离线扫描中所有候选均未通过完整轨迹硬门槛，因此不能把扫描得到的左臂 34°、
+右臂 0°写成“最优 J3”。仿真只能说明：两臂 `q4 >= 10°` 均保持正余量，J4 的 10°
+硬下限不是当次 QP 失败的直接原因。
+
+## 2. 更新后的 QP 目标
+
+主任务和辅助任务写为：
+
+\[
+\begin{aligned}
+\min_{\Delta q}\quad
+&\left\|J_p\Delta q-e_p\right\|_{W_p}^2
++\left\|J_R\Delta q-e_R\right\|_{W_R}^2 \\
+&+\lambda\left\|\Delta q\right\|^2
+-w_m\alpha_m(\sigma)\nabla m_6(q)^T\Delta q \\
+&+w_3\left(q_3+\Delta q_3-q_{3,\mathrm{ref}}\right)^2 \\
+&+w_4\alpha_4(q_4)
+  \left[q_{4,\mathrm{warn}}-(q_4+\Delta q_4)\right]_+^2，
+\end{aligned}
+\]
+
+其中：
+
+- `e = 目标位姿 - 当前位姿`，因此主任务使用 `JΔq - e`。如果误差定义相反，公式
+  才写成加号；当前 Placo 代码不翻转误差符号。
+- 前两项是严格六维 TCP 位置和姿态任务，始终保持最高权重。
+- `λ||Δq||²` 是现有动能正则，用于抑制过大的关节增量和数值抖动。
+- `m6` 使用 Placo 支持的 `both` 类型六维可操作度；`αm` 只在完整六维雅可比的
+  最小奇异值进入预警区时逐渐激活，正常区域为零。
+- J3 是低权重软引导，不属于可行性硬门槛。
+- `[x]+ = max(0, x)`；J4 软项只在进入预警区后产生作用，提前远离 10° 硬下限。
+
+辅助项不能通过提高权重来抵消六维 TCP 跟踪。第一版复用 Placo 现有任务接口，不
+引入新的分层 QP 框架或外部依赖。
+
+## 3. 四处修改
+
+### 3.1 六维主任务、可操作度与数值迭代
+
+严格六维 TCP 主任务保持不变，可操作度由“全程恒定启用的位置任务”改为“接近奇异
+区才启用的六维任务”：
+
+- `sigma_min >= sigma_warn`：`αm = 0`，不干扰正常遥操作；
+- `sigma_stop < sigma_min < sigma_warn`：`αm` 从 0 平滑增加到 1；
+- `sigma_min <= sigma_stop`：保持最大辅助权重，但仍不降低六维 TCP 权重。
+
+`sigma_warn`、`sigma_stop` 和最大辅助权重先保留为仿真可调参数，根据现有完整轨迹
+日志确定；不直接沿用此前效果不明显的恒定位置可操作度权重。
+
+当前 30 次求解是同一目标的数值迭代，不是 30 个真实控制周期。生产控制链路仍由
+现有关节速度和加速度限制器约束实际运动，因此不把“单个物理周期到达完整目标”作为
+收敛要求。求解器逐次检查六维误差；只有达到当前位置和姿态阈值的结果才允许发送。
+30 次内未收敛则恢复到本周期实际关节反馈，不发送未收敛的中间结果。
+
+### 3.2 J3 初始姿态软参考
+
+取消继续扫描 J3 最优角，使用当前 YAML 初始姿态作为第一版参考：
+
+\[
+q_{3,\mathrm{ref}}^L=67.96^\circ，\qquad
+q_{3,\mathrm{ref}}^R=-89.57^\circ。
+\]
+
+J3 只使用低权重软任务，不设置 J3 硬限位，不因追踪参考角而放松 TCP 位姿任务。
+代表性严格六维 mock 路径显示：左臂使用 `1e-5` 可完成路径，提高到 `1e-4` 会提前
+触及关节限位；右臂使用 `1e-5` 时 J6 到达 URDF 下限，提高到 `1e-4` 后保留约
+23° J6 余量并完成前伸段。因此第一版分别取：
+
+\[
+w_3^L=10^{-5}，\qquad w_3^R=10^{-4}。
+\]
+
+左右臂参数分别配置，后续只在完整 mock 轨迹明显改善时再调整，不把参考角本身当作
+成功保证。
+
+### 3.3 J4 硬下限与软缓冲区
+
+左右臂暂时保持相同硬约束：
+
+\[
+q_4\geq q_{4,\min}=10^\circ。
+\]
+
+在硬下限上方增加预警区，第一版取 `q4_warn = 25°`：
+
+- `q4 >= 25°`：J4 软项关闭；
+- `10° < q4 < 25°`：软项随接近 10°逐渐增强；
+- `q4 <= 10°`：由硬约束禁止继续向下。
+
+这样保留收集筐下降阶段所需的可达空间，同时避免 QP 到达 10°附近才突然遇到约束
+边界。`25°` 是待仿真验证的缓冲起点，不是新的硬下限；左右臂允许分别调整预警角，
+但除非轨迹数据证明有必要，不增加更多参数。
+
+### 3.4 QP 失败恢复与笛卡尔参考状态提交
+
+这是除目标函数外最关键的修复。当前风险流程为：
+
+```text
+QP 失败
+→ 关节指令保持不动
+→ 笛卡尔目标历史仍向前更新
+→ 下一周期误差进一步增大
+→ 连续失败或恢复时突跳
+```
+
+修改后，QP 求解结果、关节目标和笛卡尔参考状态按同一周期提交。
+
+QP 成功时：
+
+```text
+QP 成功
+→ 发送新关节目标
+→ 关节目标发送成功
+→ 提交新的笛卡尔参考状态
+```
+
+QP 失败或关节目标发送失败时：
+
+```text
+QP 失败
+→ 丢弃失败后的 Placo 内部迭代结果
+→ 保持上一有效关节目标
+→ 不提交本周期笛卡尔参考状态
+→ 操作者把手柄移回可行区域后继续求解
+```
+
+“不提交笛卡尔参考状态”包括不更新本周期候选的滤波状态、
+`_last_sent_target`、`_last_sent_orientation` 和命令时间。下一周期仍从上一已提交的
+笛卡尔参考状态以及实际关节反馈出发计算，防止 QP 误差在机械臂不动时继续累积。
+
+手柄原始输入仍正常接收，不会被程序改写，也不会自动改变操作者给出的末端姿态。
+失败时机械臂不会为了恢复而自行移动；操作者主动将手柄移回可行区域后，QP 使用新的
+手柄输入重新求解。收集筐到达和松开夹爪仍以实际 TCP 反馈及位置、姿态容差为判据。
+
+## 4. 保留约束
+
+QP 和下游控制继续保留：
+
+- URDF 关节位置限制和 J4 的 10°额外硬下限；
+- 现有关节速度、关节加速度、TCP 线速度和角速度限制；
+- 工作空间/圆柱限位、指令超时和安全停止；
+- `configure_safety_limits` 默认启用；
+- `move_to_initial_pose_on_connect` 默认关闭；
+- mock 模式不依赖睿尔曼真机 SDK。
+
+## 5. 验证顺序与通过标准
+
+实施按以下顺序进行：
+
+1. 先实现 QP 失败保持，以及关节目标与笛卡尔参考状态的成功后统一提交；
+2. 加入 J4 的 10°硬下限与 25°软缓冲区；
+3. 加入左右臂 J3 初始姿态软参考；
+4. 加入按六维最小奇异值激活的 `both` 可操作度任务；
+5. 在 `use_mock:=true` 下运行初始位姿、前方 30～50 cm 采摘、本侧下方 40 cm 收集
+   筐和返回初始位姿的完整严格六维轨迹。
+
+至少记录并比较修改前后的：QP 成功/失败周期数、连续失败长度、失败周期参考状态是否
+保持不变、恢复时的关节跳变量、完整轨迹成功数、
+六维最小奇异值、最大位置/姿态误差、J4 最小余量、最大关节速度以及目标历史与实际
+TCP 的偏差。只有失败周期下降、完整轨迹成功率不降低、严格六维误差和全部安全约束
+仍满足时，辅助项才保留；否则首先回退可操作度或 J4 软项，不回退失败状态修复。

docs/superpowers/specs/2026-08-24-rm75-ik-method-comparison-design.md

@@ -0,0 +1,415 @@
+# RM75 三种逆运动学方法离线对比实验设计
+
+## 1. 目标
+
+基于现有番茄采摘 episode 的右臂目标位姿轨迹，在完全一致的机械臂模型、初始关节
+状态、时间轴、收敛判据和输出安全限制下，对比以下三种七自由度逆运动学方法：
+
+1. Jacobian Moore-Penrose 伪逆法；
+2. 阻尼最小二乘法（Damped Least Squares，DLS）；
+3. 当前项目中的优化 Placo QP 方法。
+
+实验输出用于补充中期报告 2.3.4 节预留的三张图，并同时生成逐采样数据、汇总指标和
+可直接粘贴到报告中的中文结果分析。实验必须由真实计算结果驱动，不预设或硬编码
+“QP 更优”的结论。
+
+## 2. 现有上下文
+
+### 2.1 报告要求
+
+中期报告 2.3.4 节已经确定：
+
+- 三种方法使用同一机械臂模型、初始关节状态和末端目标轨迹；
+- 统计位置 RMSE、姿态 RMSE、归一化关节安全裕度、最大关节速度、求解时间和
+  求解成功率；
+- 章节末尾预留三张对比图。
+
+现有图号从图 2-10 跳到图 2-14，因此本实验生成图 2-11、图 2-12 和图 2-13。
+
+### 2.2 当前 QP 与报告文字的差异
+
+报告 2.3.2 节主要描述六维末端软任务、动能正则化、关节位置和速度限制。当前分支的
+`PlacoIkSolver` 还包含：
+
+- J3 初始构型软引导；
+- J4 硬下限和预警区软缓冲；
+- 接近奇异区时动态启用的六维可操作度任务；
+- QP 失败时恢复实际关节状态并保持上一安全输出。
+
+本实验使用当前优化 QP，而不是关闭上述辅助任务的基础 QP。最终分析文件需要提供一段
+方法补充文字，避免报告方法描述与对比对象不一致。
+
+## 3. 范围与安全边界
+
+### 3.1 本次包含
+
+- 只读加载一个现有右臂 episode；
+- 离线重采样目标位姿；
+- 在同一 URDF 上运行三种逆运动学方法；
+- 复用当前 QP 代码和右臂 YAML 参数；
+- 对三种方法使用相同的输出端安全处理；
+- 生成 SVG、300 dpi PNG、CSV、JSON 和中文 Markdown 分析。
+
+### 3.2 本次不包含
+
+- 不连接真机，不移动机械臂，不操作夹爪；
+- 不启动新的 PICO 录制；
+- 不修改生产遥操作节点、launch、YAML 默认值或公开 API；
+- 不使用 episode 中已经记录的 QP 关节结果充当本次 QP 结果；
+- 不模拟电机、通信和接触动力学；
+- 不直接编辑用户提供的 PDF。
+
+实验只使用当前 Conda 环境已经安装的 NumPy、h5py、Matplotlib 和 Placo，不新增项目
+依赖。
+
+因此，结果应表述为“基于真实遥操作目标轨迹的离线运动学对比”，不得表述为新的真机
+在线控制对比。
+
+## 4. 数据源与质量基线
+
+实验固定使用：
+
+```text
+/home/robot/ACT_Data/tomato_pick/episode_0.hdf5
+```
+
+该文件的已核对属性如下：
+
+- 机械臂：`right_rm75`；
+- 样本数：484；
+- 有效时长：约 16.1 s；
+- 保存采样率：约 30 Hz；
+- 位姿顺序：`x,y,z,qx,qy,qz,qw`；
+- 所有目标位姿、当前位姿和关节状态均为有限值；
+- 目标和当前四元数范数接近 1；
+- 483 帧为遥操作激活且已发送命令；
+- 记录时 QP 尝试 483 次并成功 483 次；
+- 132 帧触发过目标限幅，轨迹本身包含足够的约束压力。
+
+只使用满足以下条件的最长连续区间：
+
+```text
+teleop_active && action_valid && command_sent
+```
+
+共同末端目标取 `debug/tcp/final_target_pose`。该字段已通过原系统的工作空间限制、目标
+平滑和单帧笛卡尔步长限制，适合作为三种逆运动学方法的共同安全输入。共同初始关节角
+取有效区间第一帧的 `observations/qpos[:7]`。
+
+episode 中后续 `observations/qpos`、`debug/qp/raw_target`、QP 成功标志和耗时只用于
+数据质量核对，不替代任何方法在本实验中的离线计算结果。
+
+## 5. 统一复放架构
+
+数据流为：
+
+```text
+episode_0.hdf5
+  -> 有效区间与共同初始状态
+  -> 30 Hz 目标位姿重采样到 90 Hz
+  -> 伪逆 / DLS / 当前优化 QP 三路独立复放
+  -> 共同输出安全层
+  -> 正向运动学和逐采样指标
+  -> CSV / JSON / 三张图 / 中文分析
+```
+
+三种方法各自维护独立的关节状态和上一周期关节速度。每个方法的下一状态只能由该方法
+本周期的安全输出推进，三路之间不共享可变状态。
+
+离线状态推进采用理想位置跟随，即共同输出限速器给出的关节目标直接作为下一 90 Hz
+周期的关节状态。这一简化隔离了逆运动学方法本身，不引入未建模的电机和网络差异。
+
+## 6. 目标轨迹重采样
+
+原 episode 按约 30 Hz 保存，而当前遥操作控制器使用 90 Hz。重采样使用 episode 的
+`debug/timestamps/control_monotonic_ns`，目标时间轴保持原始起止时刻并以 1/90 s
+采样：
+
+- 位置使用分段线性插值；
+- 姿态使用归一化四元数的最短弧 SLERP；
+- 相邻四元数点积为负时先翻转后一四元数，避免绕长弧插值；
+- 第一个和最后一个重采样位姿必须与原始有效区间端点一致；
+- 不对目标轨迹额外放大、延长或人工加入困难片段。
+
+## 7. 三种逆运动学方法
+
+### 7.1 共同任务定义
+
+当前关节状态为 `q`，正向运动学得到当前 TCP 位姿 `(p, R)`，目标位姿为
+`(p_d, R_d)`。位置误差和姿态误差分别为：
+
+```text
+e_p = p_d - p
+e_R = Log(R^T R_d)
+```
+
+求解时的角速度误差表达必须与所用 `local_world_aligned` Jacobian 的坐标表达一致；
+姿态误差大小统一使用目标与实际旋转矩阵之间的最短夹角评价。伪逆和 DLS 使用相同的
+位置、姿态反馈增益、相同 Jacobian、相同 90 Hz 步长和相同数值迭代框架。
+
+三种方法对单个目标最多执行 30 次数值迭代。满足以下两个条件时记为收敛：
+
+```text
+位置误差 <= 0.002 m
+姿态误差 <= 0.005 rad
+```
+
+### 7.2 Jacobian 伪逆法
+
+伪逆法按报告公式计算：
+
+```text
+q_dot = pinv(J) * v_d
+```
+
+其中 `v_d` 由共同的六维位姿反馈误差生成。实现直接使用 NumPy 的 Moore-Penrose
+伪逆，不增加零空间任务、阻尼或自适应奇异值阈值，以保持基线定义清楚。
+
+### 7.3 DLS 方法
+
+DLS 按报告公式计算：
+
+```text
+q_dot = J^T * inv(J * J^T + mu^2 * I) * v_d
+```
+
+公式保持与报告一致；数值实现使用线性方程求解，不显式计算矩阵逆。
+
+只扫描固定阻尼系数，不实现自适应 DLS。候选值使用对数尺度的小集合：
+
+```text
+0.001, 0.003, 0.01, 0.03, 0.1, 0.3
+```
+
+每个候选均完整复放 episode，先按求解成功率从高到低选择，再在成功率相同的候选中
+最小化：
+
+```text
+位置 RMSE / 0.002 + 姿态 RMSE / 0.005
+```
+
+若仍并列，选择最大关节速度更小的候选。阻尼扫描使用同一条评价轨迹，因此最终文字
+必须说明该 DLS 是“在当前轨迹上选优的固定阻尼基线”；这一口径对 DLS 较有利，不能
+将其解释为跨轨迹最优参数。
+
+阻尼扫描耗时不计入三种方法的在线求解时间对比。
+
+### 7.4 当前优化 QP
+
+QP 直接实例化现有 `xr_rm_teleop.placo_ik_solver.PlacoIkSolver`，使用 90 Hz 步长、
+当前双臂 URDF 和右臂配置中的参数：
+
+```text
+qp_j3_reference_deg: -89.57
+qp_j3_weight: 0.0001
+qp_j4_min_deg: 10.0
+qp_j4_warn_deg: 25.0
+qp_j4_weight: 0.0001
+qp_manipulability_sigma_stop: 0.01
+qp_manipulability_sigma_warn: 0.04
+qp_manipulability_weight: 0.0001
+```
+
+保留现有六维末端软任务、`1e-6` 动能正则化、URDF 关节位置和速度限制、30 次迭代、
+收敛阈值、输入变换校验、失败恢复和结果有效性检查。实验脚本不复制或重写 QP。
+
+## 8. 共同输出安全层与失败处理
+
+三种方法使用同一安全口径：
+
+1. 每次数值迭代结果必须为 7 个有限关节值；
+2. 数值迭代的关节状态不得超出 URDF 位置范围；
+3. 相邻数值迭代的关节变化不得超过 URDF 速度上限乘以 `1/90 s`；
+4. 有效求解结果继续经过生产控制器现有的关节速度/加速度限制逻辑；
+5. 输出端最大关节速度为 `180 deg/s`，最大关节加速度为 `300 deg/s^2`；
+6. 未在 30 次内收敛、出现非有限值或违反硬边界时，本周期记为失败并保持上一安全
+   关节状态；
+7. 失败不会停止离线复放，时间轴继续推进，并记录失败次数和最长连续失败长度。
+
+QP 在优化内部主动处理关节边界；伪逆和 DLS 在每次候选步之后接受同样的硬检查。
+共同输出层不会消除算法差异：基线仍可能因候选步无效而失败或保持，QP 则可能在优化
+过程中找到满足约束的解。
+
+## 9. 评价指标
+
+### 9.1 末端跟踪
+
+- 逐采样位置误差 `||p_d - p||`，单位为 mm；
+- 逐采样姿态夹角误差，单位为 degree；
+- 全轨迹位置 RMSE，报告中仍以 m 给出，图中用 mm；
+- 全轨迹姿态 RMSE，报告公式使用 rad，图中用 degree。
+
+### 9.2 关节运动
+
+- 每个采样时刻七关节绝对速度的最大值，单位为 `deg/s`；
+- 全轨迹最大关节速度；
+- 超过或触发共同速度/加速度限制器的周期数；
+- 按报告式 (2-28) 计算的逐采样最小归一化关节安全裕度；
+- 全轨迹最小归一化关节安全裕度。
+
+速度图不再使用归一化速度。当前 URDF 中右臂七个关节的速度上限均为 `3.14 rad/s`
+（约 `180 deg/s`），直接展示实际速度更直观且与报告文字一致。
+
+### 9.3 求解性能
+
+- 收敛成功周期数和成功率；
+- 失败周期数和最长连续失败长度；
+- 单周期 IK 求解平均时间和最大时间，单位为 ms。
+
+耗时只覆盖单次 IK 求解，不包含 HDF5 读取、重采样、指标汇总和绘图。先执行一次完整
+预热复放，再对选定参数的三种方法各重复 10 次。轨迹和非耗时指标必须在重复复放间
+保持确定；平均和最大耗时从 10 次计时复放汇总。
+
+## 10. 三张图设计
+
+### 10.1 图 2-11 三种逆运动学方法末端位姿跟踪误差对比
+
+使用上下两个共享时间轴的子图：
+
+- `(a)` 位置误差时序，单位 mm；
+- `(b)` 姿态误差时序，单位 degree。
+
+三种方法使用固定颜色、不同线型，并在失败保持区间添加不遮挡曲线的标记。图中不绘制
+episode 原始 QP 误差曲线。
+
+### 10.2 图 2-12 三种逆运动学方法关节运动约束对比
+
+使用两个共享时间轴的子图：
+
+- `(a)` 每个时刻的最大关节速度，单位 `deg/s`，并绘制 `180 deg/s` 虚线；
+- `(b)` 每个时刻的最小归一化关节安全裕度，数值越大表示离关节上下限越远。
+
+### 10.3 图 2-13 三种逆运动学方法综合性能指标对比
+
+使用 `2 x 3` 六个小型分组柱状图，避免不同量纲共用坐标轴：
+
+1. 位置 RMSE；
+2. 姿态 RMSE；
+3. 最大关节速度；
+4. 最小归一化关节安全裕度；
+5. 平均和最大求解时间；
+6. 求解成功率。
+
+柱顶标注精确数值。最终配色需兼顾色盲识别和灰度打印，除颜色外再使用线型、标记和
+图例区分方法。
+
+## 11. 文件与产物
+
+实验实现优先保持最小范围：
+
+```text
+xr_rm_teleop/test/ik_method_comparison.py
+xr_rm_teleop/test/test_ik_method_comparison.py
+```
+
+前者包含命令行入口、HDF5 读取、重采样、三种方法复放、指标计算和绘图；后者只覆盖
+无法由现有测试保护的新非平凡逻辑，不新增测试框架或通用评测抽象。
+
+默认输出目录为：
+
+```text
+output/ik_comparison/episode_0/
+```
+
+产物包括：
+
+```text
+samples.csv
+summary.json
+figure_2_11_tracking_error.svg
+figure_2_11_tracking_error.png
+figure_2_12_joint_constraints.svg
+figure_2_12_joint_constraints.png
+figure_2_13_summary.svg
+figure_2_13_summary.png
+analysis_2.3.4.md
+```
+
+`samples.csv` 使用长表结构，每行对应“方法 + 时间点”，至少包含目标位姿、实际位姿、
+位置误差、姿态误差、七关节角、七关节速度、最小安全裕度、求解耗时、成功标志和限制
+触发标志。`summary.json` 保存输入路径、Git 提交、参数、选定 DLS 阻尼、指标和产物路径，
+保证结果可追溯。
+
+`analysis_2.3.4.md` 使用中文撰写，包含：
+
+- 数据来源和离线实验口径；
+- DLS 最终阻尼和选择规则；
+- 三张图的建议图题与图注；
+- 与式 (2-25) 至式 (2-28) 对应的数值结果；
+- 对优势、代价和异常结果的客观分析；
+- 当前优化 QP 相对报告 2.3.2 节的补充方法说明。
+
+## 12. 错误处理
+
+以下情况在生成任何正式图前立即报错：
+
+- episode 路径不存在或不是 HDF5；
+- 必需字段或属性缺失；
+- 数组长度不一致；
+- 找不到至少包含两个样本的连续有效遥操作区间；
+- 时间戳非严格递增；
+- 位姿、关节角或四元数含 NaN/Inf；
+- 四元数无法正规化；
+- episode 机械臂不是 `right_rm75`；
+- URDF 或当前右臂配置不存在；
+- Placo 版本不是项目固定的 0.9.4；
+- 任一方法没有生成与统一时间轴等长的结果；
+- CSV、JSON 和绘图使用的汇总数值不一致。
+
+单个目标的逆运动学失败属于实验结果，按上一安全状态保持，不中止整条轨迹。输入数据
+结构错误、模型错误和结果长度错误属于实验无效，必须中止并说明原因。
+
+## 13. 测试与验证
+
+### 13.1 聚焦测试
+
+最小测试至少覆盖：
+
+- 30 Hz 到 90 Hz 重采样保持首尾位置和姿态；
+- SLERP 选择最短弧并输出单位四元数；
+- 姿态夹角误差在单位旋转和已知小角度下正确；
+- 关节安全裕度与式 (2-28) 一致；
+- 无效候选触发失败保持而不是推进状态；
+- DLS 选择规则按成功率、归一化误差和最大速度依次决策；
+- 汇总指标与逐采样数据一致。
+
+### 13.2 真实模型冒烟验证
+
+使用当前双臂 URDF 和右臂初始关节角，对三种方法各运行一小段真实目标位姿序列，确认：
+
+- 输出始终为有限 7 维关节值；
+- 没有输出越过 URDF 关节位置边界；
+- 失败时保持上一安全状态；
+- 当前 QP 直接走现有 `PlacoIkSolver`，没有本地复制实现。
+
+### 13.3 项目级验证
+
+从工作空间根目录 `/home/robot/WS_xr` 执行，并先加载 ROS2 Humble：
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+pytest src/xr_rm_teleop/test/test_orientation_control.py
+```
+
+随后使用项目固定的 Conda Python 运行聚焦测试和完整离线实验。验证完成后还需检查：
+
+- 三张 PNG 无裁切、重叠、乱码或不可辨识曲线；
+- SVG 可编辑且文字完整；
+- PNG 为 300 dpi；
+- 图题、坐标轴、单位和图例为中文论文风格；
+- `summary.json` 与图中柱顶数值一致；
+- 同一输入重复运行时，除耗时外的结果一致。
+
+## 14. 验收标准
+
+满足以下条件才视为完成：
+
+1. 三种方法从完全相同的 episode 目标轨迹和初始关节状态开始；
+2. 当前优化 QP 复用现有实现和右臂参数；
+3. 三种方法使用同一输出安全口径，任何失败均安全保持；
+4. DLS 固定阻尼选择过程和最终值可追溯；
+5. 生成三张与报告公式和图号一致的正式对比图；
+6. 生成完整 CSV、JSON 和中文 2.3.4 分析文字；
+7. 所有实际执行的测试和构建结果如实记录；
+8. 不连接真机、不修改生产控制默认值、不新增依赖和重复 QP 实现。

output/ik_comparison/episode_0/analysis_2.3.4.md

@@ -0,0 +1,29 @@
+# 2.3.4 三种逆运动学方法对比补充分析
+
+本结果是基于真实遥操作目标轨迹的离线运动学对比，不代表真机闭环实验。数据来自
+`/home/robot/ACT_Data/tomato_pick/episode_0.hdf5`，目标位姿以 90.0 Hz 重采样；三种方法使用同一初始
+关节状态、同一 URDF、相同收敛阈值和共同的输出速度/加速度限制。
+
+DLS 扫描的固定阻尼候选为 0.001, 0.003, 0.01, 0.03, 0.1, 0.3，本轨迹选定
+`0.3`。该参数是在当前评价轨迹上选优，不应解释为跨轨迹最优参数。
+
+| 方法 | 位置 RMSE (m) | 姿态 RMSE (rad) | 最大关节速度 (°/s) | 最小归一化裕度 | 平均/最大求解时间 (ms) | 成功率 |
+| --- | ---: | ---: | ---: | ---: | ---: | ---: |
+| Jacobian 伪逆 | 0.260542 | 0.713938 | 30.000 | 0.1509 | 0.136 / 1.061 | 1.17% |
+| DLS | 0.005845 | 0.011201 | 49.999 | 0.0607 | 0.391 / 1.942 | 100.00% |
+| 优化 QP | 0.004929 | 0.011075 | 46.666 | 0.0611 | 0.206 / 1.057 | 100.00% |
+
+图 2-11 三种逆运动学方法的末端位置与姿态跟踪误差。纵轴采用对数坐标以同时显示不同
+数量级的误差，叉号稀疏标记数值求解失败并保持上一安全关节状态的周期。
+
+图 2-12 三种逆运动学方法的最大关节速度与最小归一化关节安全裕度。红色虚线表示
+180°/s 输出速度上限，裕度越大表示离关节位置边界越远。
+
+图 2-13 三种逆运动学方法的综合性能对比，包括误差、关节运动、求解时间和成功率。
+
+按本次单轨迹数值比较，位置 RMSE 最低的方法为优化 QP，姿态 RMSE 最低的方法为
+优化 QP，成功率最高的方法为DLS、优化 QP。这些结论只描述本次离线复放，
+未进行统计显著性检验。
+
+当前优化 QP 除六维末端主任务外，还保留项目中的 J3 参考软任务、J4 硬下界与软缓冲，
+以及按最小奇异值动态激活的六维可操作度任务；伪逆和 DLS 基线不包含这些附加任务。

output/ik_comparison/episode_0/summary.json

@@ -0,0 +1,50 @@
+{
+  "source_episode": "/home/robot/ACT_Data/tomato_pick/episode_0.hdf5",
+  "git_commit": "f30aac547b6fe84703044c66b30b70720ee4d40a",
+  "sample_rate_hz": 90.00141938026849,
+  "selected_dls_damping": 0.3,
+  "methods": {
+    "pinv": {
+      "method": "pinv",
+      "damping": null,
+      "success_rate": 0.011748445058742226,
+      "position_rmse_m": 0.26054182896155575,
+      "orientation_rmse_rad": 0.713938311140316,
+      "max_joint_speed_deg_s": 29.999526880705357,
+      "min_joint_margin": 0.15094435580838983,
+      "mean_solve_ms": 0.13594157401520388,
+      "max_solve_ms": 1.061404,
+      "failure_count": 1430,
+      "longest_failure_streak": 1430,
+      "command_limited_count": 17
+    },
+    "dls": {
+      "method": "dls",
+      "damping": 0.3,
+      "success_rate": 1.0,
+      "position_rmse_m": 0.005845412147117041,
+      "orientation_rmse_rad": 0.011200514621088859,
+      "max_joint_speed_deg_s": 49.999211467842265,
+      "min_joint_margin": 0.060689206887586424,
+      "mean_solve_ms": 0.39115279765031097,
+      "max_solve_ms": 1.94196,
+      "failure_count": 0,
+      "longest_failure_streak": 0,
+      "command_limited_count": 1282
+    },
+    "qp": {
+      "method": "qp",
+      "damping": null,
+      "success_rate": 1.0,
+      "position_rmse_m": 0.004929450681893051,
+      "orientation_rmse_rad": 0.01107496419458487,
+      "max_joint_speed_deg_s": 46.66593070331945,
+      "min_joint_margin": 0.06105232712172298,
+      "mean_solve_ms": 0.20632571050449205,
+      "max_solve_ms": 1.057197,
+      "failure_count": 0,
+      "longest_failure_streak": 0,
+      "command_limited_count": 1267
+    }
+  }
+}

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -28,6 +28,16 @@ left_arm_teleop:
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
+
+    # QP 辅助任务：严格六维 TCP 主任务保持最高权重。
+    qp_j3_reference_deg: 67.96
+    qp_j3_weight: 0.00001
+    qp_j4_min_deg: 10.0
+    qp_j4_warn_deg: 25.0
+    qp_j4_weight: 0.0001
+    qp_manipulability_sigma_stop: 0.01
+    qp_manipulability_sigma_warn: 0.04
+    qp_manipulability_weight: 0.0001
     workspace_min: [-0.70, -0.70, 0.10]
     workspace_max: [0.70, 0.10, 0.75]
     cyl_radius_limit: [0.10, 0.80]
@@ -85,6 +95,15 @@ right_arm_teleop:
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
+
+    qp_j3_reference_deg: -89.57
+    qp_j3_weight: 0.0001
+    qp_j4_min_deg: 10.0
+    qp_j4_warn_deg: 25.0
+    qp_j4_weight: 0.0001
+    qp_manipulability_sigma_stop: 0.01
+    qp_manipulability_sigma_warn: 0.04
+    qp_manipulability_weight: 0.0001
     workspace_min: [-0.70, -0.70, 0.10]
     workspace_max: [0.70, 0.10, 0.75]
     cyl_radius_limit: [0.10, 0.80]

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -22,6 +22,16 @@ single_arm_velocity_teleop:
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
+
+    # QP 辅助任务：严格六维 TCP 主任务保持最高权重。
+    qp_j3_reference_deg: 67.96
+    qp_j3_weight: 0.00001
+    qp_j4_min_deg: 10.0
+    qp_j4_warn_deg: 25.0
+    qp_j4_weight: 0.0001
+    qp_manipulability_sigma_stop: 0.01
+    qp_manipulability_sigma_warn: 0.04
+    qp_manipulability_weight: 0.0001
     workspace_min: [-0.70, -0.70, 0.10]
     workspace_max: [0.70, 0.10, 0.75]
     cyl_radius_limit: [0.10, 0.80]

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -21,6 +21,16 @@ single_arm_velocity_teleop:
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
+
+    # QP 辅助任务：严格六维 TCP 主任务保持最高权重。
+    qp_j3_reference_deg: -89.57
+    qp_j3_weight: 0.0001
+    qp_j4_min_deg: 10.0
+    qp_j4_warn_deg: 25.0
+    qp_j4_weight: 0.0001
+    qp_manipulability_sigma_stop: 0.01
+    qp_manipulability_sigma_warn: 0.04
+    qp_manipulability_weight: 0.0001
     workspace_min: [-0.70, -0.70, 0.10]
     workspace_max: [0.70, 0.10, 0.75]
     cyl_radius_limit: [0.10, 0.80]

xr_rm_bringup/test/camera_test_output/.gitignore

@@ -1,2 +0,0 @@
-*
-!.gitignore

xr_rm_bringup/test/test_realsense_multi_camera.py

@@ -1,76 +0,0 @@
-from __future__ import annotations
-
-import importlib.util
-from pathlib import Path
-import sys
-
-import pytest
-
-
-MODULE_PATH = (
-    Path(__file__).resolve().parents[1]
-    / "tools"
-    / "realsense_multi_camera_test.py"
-)
-SPEC = importlib.util.spec_from_file_location("realsense_multi_camera_test", MODULE_PATH)
-assert SPEC is not None and SPEC.loader is not None
-camera_test = importlib.util.module_from_spec(SPEC)
-sys.modules[SPEC.name] = camera_test
-SPEC.loader.exec_module(camera_test)
-
-
-def devices() -> list[camera_test.DeviceInfo]:
-    return [
-        camera_test.DeviceInfo("Intel RealSense D405", "D405", "412622272532", "3.2"),
-        camera_test.DeviceInfo("Intel RealSense D455", "D455", "234222303366", "3.2"),
-        camera_test.DeviceInfo("Intel RealSense D405", "D405", "260322272273", "3.2"),
-    ]
-
-
-def test_assigns_camera_roles_with_and_without_left_serial() -> None:
-    unidentified = camera_test.assign_camera_roles(devices(), None)
-    assert [camera.role for camera in unidentified] == ["GLOBAL", "D405-A", "D405-B"]
-    assert [camera.serial for camera in unidentified[1:]] == ["260322272273", "412622272532"]
-
-    identified = camera_test.assign_camera_roles(devices(), "412622272532")
-    assert {camera.role: camera.serial for camera in identified} == {
-        "GLOBAL": "234222303366",
-        "LEFT": "412622272532",
-        "RIGHT": "260322272273",
-    }
-
-
-def test_rejects_invalid_camera_selection() -> None:
-    with pytest.raises(ValueError, match="左臂序列号"):
-        camera_test.assign_camera_roles(devices(), "missing")
-
-    with pytest.raises(ValueError, match="2 台 D405 和 1 台 D455"):
-        camera_test.assign_camera_roles(devices()[:-1], None)
-
-
-def test_counts_frame_number_gaps() -> None:
-    stats = camera_test.FrameStats(target_fps=30, start_time=0.0)
-    stats.update(10, 0.0)
-    stats.update(11, 1.0 / 30.0)
-    stats.update(14, 2.0 / 30.0)
-
-    assert stats.received == 3
-    assert stats.dropped == 2
-    assert stats.drop_rate == pytest.approx(0.4)
-    assert stats.average_fps(2.0 / 30.0) == pytest.approx(30.0)
-
-
-def test_snapshot_names_follow_camera_roles() -> None:
-    identified = camera_test.assign_camera_roles(devices(), "412622272532")
-    assert camera_test.snapshot_filenames(identified) == {
-        "234222303366": "global.png",
-        "412622272532": "left.png",
-        "260322272273": "right.png",
-    }
-
-    unidentified = camera_test.assign_camera_roles(devices(), None)
-    assert camera_test.snapshot_filenames(unidentified) == {
-        "234222303366": "global.png",
-        "260322272273": "d405_260322272273.png",
-        "412622272532": "d405_412622272532.png",
-    }

xr_rm_bringup/tools/realsense_multi_camera_test.py

@@ -1,494 +0,0 @@
-#!/usr/bin/env python3
-"""同时预览并检查两台 D405 和一台 D455 的彩色画面。"""
-
-from __future__ import annotations
-
-import argparse
-from collections import deque
-from dataclasses import dataclass, field
-from datetime import datetime
-from pathlib import Path
-import threading
-import time
-from typing import Any
-
-
-DEFAULT_WIDTH = 640
-DEFAULT_HEIGHT = 480
-DEFAULT_FPS = 30
-DEFAULT_LEFT_SERIAL = "260322272273"
-FPS_WINDOW_SECONDS = 2.0
-WARMUP_SECONDS = 5.0
-MAX_DROP_RATE = 0.01
-MIN_FPS_RATIO = 0.9
-PREVIEW_TILE_WIDTH = 640
-WINDOW_NAME = "XR RM - Three RealSense Camera Test"
-OUTPUT_DIR = Path(__file__).resolve().parents[1] / "test" / "camera_test_output"
-
-
-@dataclass(frozen=True)
-class DeviceInfo:
-    name: str
-    model: str
-    serial: str
-    usb_type: str
-
-
-@dataclass(frozen=True)
-class CameraAssignment:
-    role: str
-    name: str
-    model: str
-    serial: str
-    usb_type: str
-
-
-def assign_camera_roles(
-    devices: list[DeviceInfo], left_serial: str | None
-) -> list[CameraAssignment]:
-    d405 = sorted(
-        (device for device in devices if device.model == "D405"),
-        key=lambda device: device.serial,
-    )
-    d455 = [device for device in devices if device.model == "D455"]
-    if len(d405) != 2 or len(d455) != 1 or len(devices) != 3:
-        raise ValueError(
-            f"需要连接 2 台 D405 和 1 台 D455，当前识别到 "
-            f"{len(d405)} 台 D405、{len(d455)} 台 D455、共 {len(devices)} 台 RealSense"
-        )
-
-    for device in devices:
-        if not device.usb_type.startswith("3"):
-            raise ValueError(
-                f"{device.model} ({device.serial}) 当前为 USB {device.usb_type}，"
-                "请检查扩展坞和数据线"
-            )
-
-    global_camera = CameraAssignment("GLOBAL", **d455[0].__dict__)
-    if left_serial is None:
-        arms = [
-            CameraAssignment(f"D405-{suffix}", **device.__dict__)
-            for suffix, device in zip(("A", "B"), d405)
-        ]
-    else:
-        matches = [device for device in d405 if device.serial == left_serial]
-        if not matches:
-            raise ValueError(f"左臂序列号 {left_serial} 不属于当前连接的 D405")
-        left = matches[0]
-        right = next(device for device in d405 if device.serial != left_serial)
-        arms = [
-            CameraAssignment("LEFT", **left.__dict__),
-            CameraAssignment("RIGHT", **right.__dict__),
-        ]
-    return [global_camera, *arms]
-
-
-def snapshot_filenames(assignments: list[CameraAssignment]) -> dict[str, str]:
-    role_names = {
-        "GLOBAL": "global.png",
-        "LEFT": "left.png",
-        "RIGHT": "right.png",
-    }
-    return {
-        camera.serial: role_names.get(camera.role, f"d405_{camera.serial}.png")
-        for camera in assignments
-    }
-
-
-@dataclass
-class FrameStats:
-    target_fps: int
-    start_time: float
-    received: int = 0
-    dropped: int = 0
-    last_frame_number: int | None = None
-    first_frame_time: float | None = None
-    recent_times: deque[float] = field(default_factory=deque)
-
-    def update(self, frame_number: int, now: float) -> None:
-        if self.last_frame_number is not None and frame_number > self.last_frame_number:
-            self.dropped += max(0, frame_number - self.last_frame_number - 1)
-        self.last_frame_number = frame_number
-        self.received += 1
-        if self.first_frame_time is None:
-            self.first_frame_time = now
-        self.recent_times.append(now)
-        cutoff = now - FPS_WINDOW_SECONDS
-        while self.recent_times and self.recent_times[0] < cutoff:
-            self.recent_times.popleft()
-
-    @property
-    def drop_rate(self) -> float:
-        expected = self.received + self.dropped
-        return self.dropped / expected if expected else 0.0
-
-    @property
-    def rolling_fps(self) -> float:
-        if len(self.recent_times) < 2:
-            return 0.0
-        elapsed = self.recent_times[-1] - self.recent_times[0]
-        return (len(self.recent_times) - 1) / elapsed if elapsed > 0 else 0.0
-
-    def average_fps(self, now: float) -> float:
-        if self.received < 2 or self.first_frame_time is None:
-            return 0.0
-        last_frame_time = self.recent_times[-1] if self.recent_times else now
-        elapsed = last_frame_time - self.first_frame_time
-        return (self.received - 1) / elapsed if elapsed > 0 else 0.0
-
-
-@dataclass(frozen=True)
-class CameraState:
-    frame: Any | None
-    actual_size: tuple[int, int]
-    received: int
-    dropped: int
-    drop_rate: float
-    rolling_fps: float
-    average_fps: float
-    elapsed: float
-    error: str
-
-
-class CameraWorker:
-    def __init__(
-        self,
-        assignment: CameraAssignment,
-        width: int,
-        height: int,
-        fps: int,
-        rs: Any,
-        np: Any,
-    ) -> None:
-        self.assignment = assignment
-        self.width = width
-        self.height = height
-        self.fps = fps
-        self._rs = rs
-        self._np = np
-        self._pipeline = rs.pipeline()
-        self._stop_event = threading.Event()
-        self._lock = threading.Lock()
-        self._thread: threading.Thread | None = None
-        self._frame: Any | None = None
-        self._actual_size = (width, height)
-        self._stats = FrameStats(fps, time.monotonic())
-        self._error = ""
-
-    def start(self) -> None:
-        config = self._rs.config()
-        config.enable_device(self.assignment.serial)
-        config.enable_stream(
-            self._rs.stream.color,
-            self.width,
-            self.height,
-            self._rs.format.bgr8,
-            self.fps,
-        )
-        profile = self._pipeline.start(config)
-        video_profile = profile.get_stream(self._rs.stream.color).as_video_stream_profile()
-        with self._lock:
-            self._actual_size = (video_profile.width(), video_profile.height())
-            self._stats = FrameStats(self.fps, time.monotonic())
-        self._thread = threading.Thread(
-            target=self._capture_loop,
-            name=f"camera-{self.assignment.serial}",
-            daemon=True,
-        )
-        self._thread.start()
-
-    def _capture_loop(self) -> None:
-        while not self._stop_event.is_set():
-            try:
-                frames = self._pipeline.wait_for_frames(timeout_ms=1000)
-            except RuntimeError as exc:
-                if self._stop_event.is_set():
-                    return
-                with self._lock:
-                    self._error = str(exc)
-                return
-
-            color_frame = frames.get_color_frame()
-            if not color_frame:
-                continue
-            frame = self._np.asanyarray(color_frame.get_data()).copy()
-            now = time.monotonic()
-            with self._lock:
-                self._frame = frame
-                self._stats.update(color_frame.get_frame_number(), now)
-
-    def state(self, now: float) -> CameraState:
-        with self._lock:
-            return CameraState(
-                frame=self._frame,
-                actual_size=self._actual_size,
-                received=self._stats.received,
-                dropped=self._stats.dropped,
-                drop_rate=self._stats.drop_rate,
-                rolling_fps=self._stats.rolling_fps,
-                average_fps=self._stats.average_fps(now),
-                elapsed=now - self._stats.start_time,
-                error=self._error,
-            )
-
-    def stop(self) -> None:
-        self._stop_event.set()
-        if self._thread is not None:
-            self._thread.join(timeout=1.2)
-        try:
-            self._pipeline.stop()
-        except RuntimeError:
-            pass
-        if self._thread is not None and self._thread.is_alive():
-            self._thread.join(timeout=1.0)
-
-
-def load_runtime_dependencies() -> tuple[Any, Any, Any]:
-    try:
-        import cv2
-        import numpy as np
-        import pyrealsense2 as rs
-    except ImportError as exc:
-        raise RuntimeError(
-            "缺少相机测试依赖。请使用 /home/robot/miniconda3/envs/xr/bin/python "
-            "运行，并确认 xr 环境已安装 pyrealsense2、numpy 和 opencv-python。"
-        ) from exc
-    return cv2, np, rs
-
-
-def enumerate_devices(rs: Any) -> list[DeviceInfo]:
-    devices = []
-    for device in rs.context().query_devices():
-        name = device.get_info(rs.camera_info.name)
-        if "D405" in name:
-            model = "D405"
-        elif "D455" in name:
-            model = "D455"
-        else:
-            model = name
-        usb_type = (
-            device.get_info(rs.camera_info.usb_type_descriptor)
-            if device.supports(rs.camera_info.usb_type_descriptor)
-            else "unknown"
-        )
-        devices.append(
-            DeviceInfo(
-                name=name,
-                model=model,
-                serial=device.get_info(rs.camera_info.serial_number),
-                usb_type=usb_type,
-            )
-        )
-    return devices
-
-
-def camera_status(state: CameraState, target_fps: int) -> tuple[str, tuple[int, int, int]]:
-    if state.error:
-        return "ERROR", (0, 0, 255)
-    if state.received == 0:
-        return "WAITING", (0, 215, 255)
-    if state.elapsed < WARMUP_SECONDS:
-        return "WARMUP", (0, 215, 255)
-    if state.rolling_fps >= target_fps * MIN_FPS_RATIO and state.drop_rate <= MAX_DROP_RATE:
-        return "PASS", (0, 200, 0)
-    return "FAIL", (0, 0, 255)
-
-
-def render_tile(
-    worker: CameraWorker,
-    state: CameraState,
-    tile_width: int,
-    tile_height: int,
-    cv2: Any,
-    np: Any,
-) -> Any:
-    if state.frame is None:
-        tile = np.zeros((tile_height, tile_width, 3), dtype=np.uint8)
-    else:
-        tile = cv2.resize(state.frame, (tile_width, tile_height))
-
-    status, color = camera_status(state, worker.fps)
-    cv2.rectangle(tile, (0, 0), (tile_width, 100), (0, 0, 0), -1)
-    width, height = state.actual_size
-    lines = [
-        f"{worker.assignment.role}  {worker.assignment.model}  {worker.assignment.serial}",
-        f"USB {worker.assignment.usb_type}  {width}x{height}@{worker.fps}",
-        f"FPS {state.rolling_fps:.1f}  Frames {state.received}  "
-        f"Dropped {state.dropped} ({state.drop_rate:.2%})",
-        status if not state.error else f"ERROR: {state.error[:70]}",
-    ]
-    for index, line in enumerate(lines):
-        cv2.putText(
-            tile,
-            line,
-            (10, 22 + index * 24),
-            cv2.FONT_HERSHEY_SIMPLEX,
-            0.55,
-            color if index == len(lines) - 1 else (255, 255, 255),
-            1,
-            cv2.LINE_AA,
-        )
-    return tile
-
-
-def compose_preview(
-    workers: list[CameraWorker],
-    states: dict[str, CameraState],
-    capture_width: int,
-    capture_height: int,
-    cv2: Any,
-    np: Any,
-) -> Any:
-    tile_width = min(capture_width, PREVIEW_TILE_WIDTH)
-    tile_height = round(tile_width * capture_height / capture_width)
-    tiles = {
-        worker.assignment.serial: render_tile(
-            worker,
-            states[worker.assignment.serial],
-            tile_width,
-            tile_height,
-            cv2,
-            np,
-        )
-        for worker in workers
-    }
-    global_worker = next(worker for worker in workers if worker.assignment.role == "GLOBAL")
-    arm_workers = [worker for worker in workers if worker.assignment.role != "GLOBAL"]
-
-    top = np.zeros((tile_height, tile_width * 2, 3), dtype=np.uint8)
-    offset = tile_width // 2
-    top[:, offset : offset + tile_width] = tiles[global_worker.assignment.serial]
-    bottom = np.hstack([tiles[worker.assignment.serial] for worker in arm_workers])
-    return np.vstack((top, bottom))
-
-
-def save_snapshots(
-    assignments: list[CameraAssignment],
-    states: dict[str, CameraState],
-    cv2: Any,
-) -> Path:
-    missing = [camera.role for camera in assignments if states[camera.serial].frame is None]
-    if missing:
-        raise RuntimeError(f"以下相机尚无有效画面，不能保存快照: {', '.join(missing)}")
-
-    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f")[:-3]
-    snapshot_dir = OUTPUT_DIR / timestamp
-    snapshot_dir.mkdir(parents=True, exist_ok=False)
-    filenames = snapshot_filenames(assignments)
-    for camera in assignments:
-        path = snapshot_dir / filenames[camera.serial]
-        if not cv2.imwrite(str(path), states[camera.serial].frame):
-            raise RuntimeError(f"保存快照失败: {path}")
-    return snapshot_dir
-
-
-def print_summary(workers: list[CameraWorker]) -> bool:
-    now = time.monotonic()
-    print("\n相机测试汇总:")
-    passed = True
-    for worker in workers:
-        state = worker.state(now)
-        status, _color = camera_status(state, worker.fps)
-        passed = passed and status == "PASS"
-        print(
-            f"  {worker.assignment.role:<7} {worker.assignment.serial}: "
-            f"平均 {state.average_fps:.1f} FPS, 接收 {state.received}, "
-            f"掉帧 {state.dropped} ({state.drop_rate:.2%}), {status}"
-        )
-    print("结论: " + ("三路链路满足当前阈值" if passed else "至少一路未满足当前阈值"))
-    return passed
-
-
-def parse_args() -> argparse.Namespace:
-    parser = argparse.ArgumentParser(description=__doc__)
-    parser.add_argument("--width", type=int, default=DEFAULT_WIDTH, help="采集宽度")
-    parser.add_argument("--height", type=int, default=DEFAULT_HEIGHT, help="采集高度")
-    parser.add_argument("--fps", type=int, default=DEFAULT_FPS, help="目标帧率")
-    parser.add_argument(
-        "--left-serial",
-        default=DEFAULT_LEFT_SERIAL,
-        help=f"左臂 D405 的 RealSense 序列号（默认: {DEFAULT_LEFT_SERIAL}）",
-    )
-    args = parser.parse_args()
-    if args.width <= 0 or args.height <= 0 or args.fps <= 0:
-        parser.error("width、height 和 fps 必须为正数")
-    return args
-
-
-def main() -> int:
-    args = parse_args()
-    try:
-        cv2, np, rs = load_runtime_dependencies()
-        assignments = assign_camera_roles(enumerate_devices(rs), args.left_serial)
-    except RuntimeError as exc:
-        print(f"错误: {exc}")
-        return 2
-    except ValueError as exc:
-        print(f"设备检查失败: {exc}")
-        return 2
-
-    print("相机分配:")
-    for camera in assignments:
-        print(
-            f"  {camera.role:<7} {camera.model}  serial={camera.serial}  "
-            f"USB={camera.usb_type}"
-        )
-
-    workers: list[CameraWorker] = []
-    passed = False
-    try:
-        for assignment in assignments:
-            worker = CameraWorker(
-                assignment,
-                args.width,
-                args.height,
-                args.fps,
-                rs,
-                np,
-            )
-            workers.append(worker)
-            worker.start()
-
-        cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL)
-        print("按 S 保存三路快照，按 Q 或 Esc 退出。")
-        while True:
-            now = time.monotonic()
-            states = {worker.assignment.serial: worker.state(now) for worker in workers}
-            preview = compose_preview(
-                workers,
-                states,
-                args.width,
-                args.height,
-                cv2,
-                np,
-            )
-            cv2.imshow(WINDOW_NAME, preview)
-            key = cv2.waitKey(1) & 0xFF
-            if key in (ord("q"), ord("Q"), 27):
-                break
-            if key in (ord("s"), ord("S")):
-                try:
-                    output = save_snapshots(assignments, states, cv2)
-                    print(f"快照已保存: {output}")
-                except RuntimeError as exc:
-                    print(f"快照失败: {exc}")
-            if cv2.getWindowProperty(WINDOW_NAME, cv2.WND_PROP_VISIBLE) < 1:
-                break
-    except KeyboardInterrupt:
-        print("\n收到 Ctrl+C，正在停止相机。")
-    except Exception as exc:
-        print(f"相机启动或显示失败: {exc}")
-    finally:
-        for worker in reversed(workers):
-            worker.stop()
-        try:
-            cv2.destroyAllWindows()
-        except Exception:
-            pass
-        if workers:
-            passed = print_summary(workers)
-    return 0 if passed else 1
-
-
-if __name__ == "__main__":
-    raise SystemExit(main())

xr_rm_teleop/test/test_ik_method_comparison.py

@@ -0,0 +1,303 @@
+from __future__ import annotations
+
+import math
+import sys
+from pathlib import Path
+
+import h5py
+import numpy as np
+import pytest
+
+TEST_DIR = Path(__file__).resolve().parent
+if str(TEST_DIR) not in sys.path:
+    sys.path.insert(0, str(TEST_DIR))
+
+import ik_method_comparison as comparison
+
+
+def test_slerp_uses_shortest_arc_and_returns_unit_quaternion() -> None:
+    start = np.asarray([0.0, 0.0, 0.0, 1.0])
+    end = -np.asarray([0.0, 0.0, math.sin(0.1), math.cos(0.1)])
+
+    actual = comparison._slerp_quaternion(start, end, 0.5)
+
+    assert np.linalg.norm(actual) == pytest.approx(1.0)
+    assert actual == pytest.approx(
+        [0.0, 0.0, math.sin(0.05), math.cos(0.05)]
+    )
+
+
+def test_resample_trajectory_keeps_endpoints_and_uses_requested_rate() -> None:
+    trajectory = comparison.EpisodeTrajectory(
+        source_path=Path("episode.hdf5"),
+        times_s=np.asarray([0.0, 0.5, 1.0]),
+        target_poses=np.asarray(
+            [
+                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+                [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+                [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+            ]
+        ),
+        initial_joints=np.zeros(7),
+    )
+
+    actual = comparison.resample_trajectory(trajectory, 4.0)
+
+    assert actual.times_s == pytest.approx([0.0, 0.25, 0.5, 0.75, 1.0])
+    assert actual.target_poses[0] == pytest.approx(trajectory.target_poses[0])
+    assert actual.target_poses[-1] == pytest.approx(trajectory.target_poses[-1])
+    assert actual.target_poses[:, 0] == pytest.approx(actual.times_s)
+
+
+def _write_episode(path: Path) -> None:
+    poses = np.asarray(
+        [
+            [0.1, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
+            [0.2, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
+            [0.3, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
+            [0.4, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
+        ],
+        dtype=np.float32,
+    )
+    with h5py.File(path, "w") as handle:
+        handle.attrs["arm"] = "right_rm75"
+        handle.attrs["pose_order"] = "x,y,z,qx,qy,qz,qw"
+        handle.create_dataset("debug/tcp/final_target_pose", data=poses)
+        handle.create_dataset(
+            "debug/timestamps/control_monotonic_ns",
+            data=np.asarray([0, 33_000_000, 66_000_000, 99_000_000]),
+        )
+        handle.create_dataset(
+            "debug/control/teleop_active", data=[0, 1, 1, 0]
+        )
+        handle.create_dataset(
+            "debug/control/action_valid", data=[1, 1, 1, 1]
+        )
+        handle.create_dataset(
+            "debug/control/command_sent", data=[0, 1, 1, 0]
+        )
+        qpos = np.zeros((4, 8), dtype=np.float32)
+        qpos[1, :7] = np.arange(7) * 0.1
+        handle.create_dataset("observations/qpos", data=qpos)
+
+
+def test_load_episode_uses_longest_valid_run_and_first_valid_qpos(
+    tmp_path: Path,
+) -> None:
+    path = tmp_path / "episode.hdf5"
+    _write_episode(path)
+
+    actual = comparison.load_episode(path)
+
+    assert actual.times_s == pytest.approx([0.0, 0.033])
+    assert actual.target_poses[:, 0] == pytest.approx([0.2, 0.3])
+    assert actual.initial_joints == pytest.approx(np.arange(7) * 0.1)
+
+
+def test_load_episode_rejects_wrong_arm(tmp_path: Path) -> None:
+    path = tmp_path / "episode.hdf5"
+    _write_episode(path)
+    with h5py.File(path, "r+") as handle:
+        handle.attrs.modify("arm", "left_rm75")
+
+    with pytest.raises(ValueError, match="right_rm75"):
+        comparison.load_episode(path)
+
+
+def test_orientation_error_and_joint_margin_match_definitions() -> None:
+    identity = np.eye(3)
+    quarter_turn = comparison._rotation_z(math.pi / 2.0)
+    joints = np.asarray([0.0, -0.5])
+    lower = np.asarray([-1.0, -1.0])
+    upper = np.asarray([1.0, 3.0])
+
+    assert comparison.orientation_error_rad(identity, quarter_turn) \
+        == pytest.approx(math.pi / 2.0)
+    assert comparison.normalized_joint_margin(joints, lower, upper) \
+        == pytest.approx(0.125)
+
+
+def test_choose_dls_damping_is_lexicographic() -> None:
+    candidates = [
+        comparison.MethodSummary("dls", 0.01, 0.90, 0.004, 0.01, 50.0),
+        comparison.MethodSummary("dls", 0.03, 0.95, 0.006, 0.02, 30.0),
+        comparison.MethodSummary("dls", 0.10, 0.95, 0.004, 0.01, 40.0),
+    ]
+
+    assert comparison.choose_dls_damping(candidates) == pytest.approx(0.10)
+
+
+def test_limit_joint_command_reuses_production_limiter() -> None:
+    target, velocity, limited = comparison.limit_joint_command(
+        target=np.full(7, 1.0),
+        previous_target=np.zeros(7),
+        previous_velocity=np.zeros(7),
+        max_speed=1.0,
+        max_acceleration=10.0,
+        dt=0.1,
+    )
+
+    assert target == pytest.approx([0.1] * 7)
+    assert velocity == pytest.approx([1.0] * 7)
+    assert limited
+
+
+class _FakeSolver:
+    def __init__(self, fail: bool) -> None:
+        self.fail = fail
+        self.joint_limits = np.asarray([[-2.0, 2.0]] * 7)
+
+    def update_joint_state(self, joints: list[float]) -> np.ndarray:
+        pose = np.eye(4)
+        pose[0, 3] = joints[0]
+        return pose
+
+    def solve(self, target: np.ndarray) -> list[float]:
+        if self.fail:
+            raise RuntimeError("not converged")
+        return [float(target[0, 3])] + [0.0] * 6
+
+
+def test_replay_holds_previous_state_on_solver_failure() -> None:
+    trajectory = comparison.EpisodeTrajectory(
+        source_path=Path("episode.hdf5"),
+        times_s=np.asarray([0.0, 0.1]),
+        target_poses=np.asarray(
+            [
+                [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+                [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+            ]
+        ),
+        initial_joints=np.zeros(7),
+    )
+
+    result = comparison.run_replay(
+        "fake",
+        _FakeSolver(fail=True),
+        trajectory,
+        max_speed=1.0,
+        max_acceleration=10.0,
+        measure_time=False,
+    )
+
+    assert result.joints == pytest.approx(np.zeros((2, 7)))
+    assert not result.success.any()
+    assert result.velocities == pytest.approx(np.zeros((2, 7)))
+
+
+def test_real_urdf_solvers_return_finite_safe_outputs() -> None:
+    pytest.importorskip("placo")
+    urdf = TEST_DIR.parent / "models" / "dual_rm75" / "Dual_arm.urdf"
+    config_path = (
+        TEST_DIR.parents[1]
+        / "xr_rm_bringup"
+        / "config"
+        / "right_arm_rm75.yaml"
+    )
+    joints = np.radians(
+        [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
+    )
+    solvers = [
+        comparison.DifferentialIkSolver(urdf, 1.0 / 90.0, "pinv"),
+        comparison.DifferentialIkSolver(urdf, 1.0 / 90.0, "dls", 0.03),
+        comparison.make_qp_solver(
+            urdf,
+            1.0 / 90.0,
+            comparison.load_right_config(config_path),
+        ),
+    ]
+    for solver in solvers:
+        target = solver.update_joint_state(joints.tolist())
+        target = target.copy()
+        target[0, 3] += 0.003
+        result = np.asarray(solver.solve(target), dtype=float)
+        assert result.shape == (7,)
+        assert np.isfinite(result).all()
+        assert np.all(result >= solver.joint_limits[:, 0] - 1e-9)
+        assert np.all(result <= solver.joint_limits[:, 1] + 1e-9)
+
+
+def test_load_right_config_returns_qp_and_command_limits() -> None:
+    config = (
+        TEST_DIR.parents[1]
+        / "xr_rm_bringup"
+        / "config"
+        / "right_arm_rm75.yaml"
+    )
+
+    actual = comparison.load_right_config(config)
+
+    assert actual["qp_j3_reference_deg"] == pytest.approx(-89.57)
+    assert actual["qp_j4_min_deg"] == pytest.approx(10.0)
+    assert actual["qp_manipulability_weight"] == pytest.approx(1e-4)
+    assert actual["joint_max_speed"] == pytest.approx(180.0)
+    assert actual["joint_max_acc"] == pytest.approx(300.0)
+
+
+def test_summarize_result_uses_report_metrics() -> None:
+    result = comparison.ReplayResult(
+        method="pinv",
+        times_s=np.asarray([0.0, 0.1]),
+        target_poses=np.zeros((2, 7)),
+        actual_poses=np.zeros((2, 7)),
+        joints=np.zeros((2, 7)),
+        velocities=np.asarray([[0.0] * 7, [math.pi] + [0.0] * 6]),
+        position_errors_m=np.asarray([0.003, 0.004]),
+        orientation_errors_rad=np.asarray([0.01, 0.02]),
+        joint_margins=np.asarray([0.2, 0.1]),
+        solve_durations_ms=np.asarray([1.0, 2.0]),
+        success=np.asarray([True, False]),
+        command_limited=np.asarray([False, True]),
+    )
+
+    actual = comparison.summarize_result(result)
+
+    assert actual.success_rate == pytest.approx(0.5)
+    assert actual.position_rmse_m == pytest.approx(0.0035355339)
+    assert actual.orientation_rmse_rad == pytest.approx(0.0158113883)
+    assert actual.max_joint_speed_deg_s == pytest.approx(180.0)
+
+
+def test_write_outputs_creates_consistent_files(tmp_path: Path) -> None:
+    result = comparison.ReplayResult(
+        method="qp",
+        times_s=np.asarray([0.0, 0.1]),
+        target_poses=np.tile(
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (2, 1)
+        ),
+        actual_poses=np.tile(
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (2, 1)
+        ),
+        joints=np.zeros((2, 7)),
+        velocities=np.zeros((2, 7)),
+        position_errors_m=np.asarray([0.001, 0.002]),
+        orientation_errors_rad=np.asarray([0.001, 0.002]),
+        joint_margins=np.asarray([0.2, 0.2]),
+        solve_durations_ms=np.asarray([0.5, 0.6]),
+        success=np.asarray([True, True]),
+        command_limited=np.asarray([False, False]),
+    )
+    summary = comparison.summarize_result(result)
+
+    comparison.write_outputs(
+        tmp_path,
+        {"pinv": result, "dls": result, "qp": result},
+        {"pinv": summary, "dls": summary, "qp": summary},
+        selected_damping=0.03,
+        source_path=Path("episode_0.hdf5"),
+        git_commit="abc1234",
+    )
+
+    expected = {
+        "samples.csv",
+        "summary.json",
+        "figure_2_11_tracking_error.svg",
+        "figure_2_11_tracking_error.png",
+        "figure_2_12_joint_constraints.svg",
+        "figure_2_12_joint_constraints.png",
+        "figure_2_13_summary.svg",
+        "figure_2_13_summary.png",
+        "analysis_2.3.4.md",
+    }
+    assert expected == {path.name for path in tmp_path.iterdir()}
+    assert all((tmp_path / name).stat().st_size > 0 for name in expected)

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -100,6 +100,43 @@ def test_deployed_workspace_is_in_front_of_robot(config_name, node_names) -> Non
         assert parameters["workspace_max"] == [0.70, 0.10, 0.75]
 
 
+@pytest.mark.parametrize(
+    "arm,single_config,dual_node,j3_reference_deg,j3_weight",
+    [
+        ("left", "left_arm_rm75.yaml", "left_arm_teleop", 67.96, 1e-5),
+        ("right", "right_arm_rm75.yaml", "right_arm_teleop", -89.57, 1e-4),
+    ],
+)
+def test_qp_optimization_parameters_match_single_and_dual_configs(
+    arm,
+    single_config,
+    dual_node,
+    j3_reference_deg,
+    j3_weight,
+) -> None:
+    del arm
+    with (CONFIG_DIR / single_config).open(encoding="utf-8") as stream:
+        single = yaml.safe_load(stream)["single_arm_velocity_teleop"][
+            "ros__parameters"
+        ]
+    with (CONFIG_DIR / "dual_arm_rm75.yaml").open(encoding="utf-8") as stream:
+        dual = yaml.safe_load(stream)[dual_node]["ros__parameters"]
+
+    expected = {
+        "qp_j3_reference_deg": j3_reference_deg,
+        "qp_j3_weight": j3_weight,
+        "qp_j4_min_deg": 10.0,
+        "qp_j4_warn_deg": 25.0,
+        "qp_j4_weight": 1e-4,
+        "qp_manipulability_sigma_stop": 0.01,
+        "qp_manipulability_sigma_warn": 0.04,
+        "qp_manipulability_weight": 1e-4,
+    }
+    for name, value in expected.items():
+        assert single[name] == pytest.approx(value)
+        assert dual[name] == pytest.approx(value)
+
+
 @pytest.mark.parametrize(
     ("existing", "expected_operation"),
     [(False, "create"), (True, "update")],

xr_rm_teleop/test/test_joint_control.py

@@ -11,6 +11,7 @@ from xr_rm_teleop.single_arm_velocity_teleop import (
     SingleArmVelocityTeleop,
     _make_transform,
     _so3_exp,
+    _so3_log,
 )
 
 
@@ -610,7 +611,7 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
     assert teleop._ik_solver.solve_calls == 0
 
 
-def test_qp_failure_returns_last_known_good_target() -> None:
+def test_qp_failure_returns_none_and_keeps_last_known_good_target() -> None:
     class FailingSolver:
         def solve(self, target):
             del target
@@ -624,11 +625,11 @@ def test_qp_failure_returns_last_known_good_target() -> None:
 
     target = teleop._solve_joint_target(np.eye(4))
 
-    assert target == pytest.approx([0.1] * 7)
+    assert target is None
     assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
 
 
-def test_qp_success_updates_last_known_good_target() -> None:
+def test_qp_success_waits_for_send_before_updating_last_known_good_target() -> None:
     class SuccessfulSolver:
         def solve(self, target):
             del target
@@ -643,7 +644,54 @@ def test_qp_success_updates_last_known_good_target() -> None:
     target = teleop._solve_joint_target(np.eye(4))
 
     assert target == pytest.approx([0.2] * 7)
-    assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
+    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
+
+
+def test_target_filters_do_not_commit_candidate_state() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._filtered_target = [0.0, 0.0, 0.0]
+    teleop._filtered_orientation_target = np.eye(3)
+    teleop._target_filter_alpha = 0.5
+    teleop._target_filter_alpha_fast = 0.5
+    teleop._target_filter_fast_threshold_m = 1.0
+    teleop._orientation_filter_alpha = 0.5
+
+    position = teleop._filter_target([0.2, 0.0, 0.0])
+    orientation = teleop._filter_orientation_target(
+        _so3_exp(np.asarray([0.0, 0.0, 0.2]))
+    )
+
+    assert position == pytest.approx([0.1, 0.0, 0.0])
+    assert teleop._filtered_target == pytest.approx([0.0, 0.0, 0.0])
+    assert teleop._filtered_orientation_target == pytest.approx(np.eye(3))
+    assert np.linalg.norm(_so3_log(orientation)) == pytest.approx(0.1)
+
+
+def test_failed_send_does_not_commit_cartesian_reference_state() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._filtered_target = [0.2, 0.0, 0.0]
+    teleop._filtered_orientation_target = np.eye(3)
+    teleop._last_sent_target = [0.2, 0.0, 0.0]
+    teleop._last_sent_orientation = np.eye(3)
+    teleop._last_command_time = FakeTime()
+    teleop._send_joint_target = lambda joints: False
+
+    sent = teleop._send_and_commit_joint_target(
+        [0.3] * 7,
+        [0.3, 0.0, 0.0],
+        _so3_exp(np.asarray([0.0, 0.0, 0.1])),
+        [0.3, 0.0, 0.0],
+        _so3_exp(np.asarray([0.0, 0.0, 0.1])),
+        FakeTime(),
+    )
+
+    assert not sent
+    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
+    assert teleop._filtered_target == pytest.approx([0.2, 0.0, 0.0])
+    assert teleop._filtered_orientation_target == pytest.approx(np.eye(3))
+    assert teleop._last_sent_target == pytest.approx([0.2, 0.0, 0.0])
+    assert teleop._last_sent_orientation == pytest.approx(np.eye(3))
 
 
 def test_enter_active_control_initializes_se3_orientation_state() -> None:

xr_rm_teleop/test/test_placo_transforms.py

@@ -6,6 +6,7 @@ from xml.etree import ElementTree
 import numpy as np
 import pytest
 
+from xr_rm_teleop import placo_ik_solver
 from xr_rm_teleop.placo_ik_solver import (
     QP_ORIENTATION_TOLERANCE_RAD,
     QP_POSITION_TOLERANCE_M,
@@ -242,6 +243,7 @@ def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
     solver._frame_task = SimpleNamespace(T_a_b=None)
     solver._solver = SimpleNamespace(solve=lambda update: None)
     solver._validate_result = lambda result, previous: None
+    solver._update_auxiliary_task_weights = lambda: None
     solver._target_errors = lambda: (2.1e-3, 0.0)
 
     with pytest.raises(RuntimeError, match="QP did not converge after 30"):
@@ -285,3 +287,130 @@ def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
         solver._validate_result(np.full(7, 2.0))
     with pytest.raises(ValueError, match="velocity"):
         solver._validate_result(np.full(7, 0.2))
+
+
+def test_lower_margin_activation_is_clamped_and_linear() -> None:
+    activation = placo_ik_solver._lower_margin_activation
+
+    assert activation(0.05, 0.01, 0.04) == 0.0
+    assert activation(0.025, 0.01, 0.04) == pytest.approx(0.5)
+    assert activation(0.005, 0.01, 0.04) == 1.0
+
+
+@pytest.mark.parametrize(
+    "arm,joint_degrees,j3_reference_deg",
+    [
+        ("left", ARM_CASES[0][1], 67.96),
+        ("right", ARM_CASES[1][1], -89.57),
+    ],
+)
+def test_solver_configures_auxiliary_qp_tasks(
+    arm: str,
+    joint_degrees: list[float],
+    j3_reference_deg: float,
+) -> None:
+    pytest.importorskip("placo")
+    solver = PlacoIkSolver(
+        str(DUAL_URDF_PATH),
+        1.0 / 90.0,
+        arm,
+        j3_reference_deg=j3_reference_deg,
+        j3_weight=1e-5,
+        j4_min_deg=10.0,
+        j4_warn_deg=25.0,
+        j4_weight=1e-4,
+        manipulability_sigma_stop=0.01,
+        manipulability_sigma_warn=0.04,
+        manipulability_weight=1e-4,
+    )
+    joints = np.radians(joint_degrees).tolist()
+    solver.update_joint_state(joints)
+
+    assert solver._j3_task.get_joint(
+        solver._joint_names[2]
+    ) == pytest.approx(math.radians(j3_reference_deg))
+    assert np.asarray(solver._j4_constraint.A)[
+        solver._q_offsets[3]
+    ] == pytest.approx(-1.0)
+    assert np.asarray(solver._j4_constraint.b) == pytest.approx(
+        [-math.radians(10.0)]
+    )
+    assert solver._j4_constraint.priority == "hard"
+    jacobian = solver._active_tcp_jacobian()
+    assert jacobian.shape == (6, 7)
+    assert np.isfinite(jacobian).all()
+    assert np.linalg.svd(jacobian, compute_uv=False)[-1] > 0.0
+
+
+def test_failed_qp_restores_internal_state_to_actual_feedback() -> None:
+    solver, joints = _dual_placo_solver("left", ARM_CASES[0][1])
+    current_pose = solver.update_joint_state(joints)
+    unreachable = current_pose.copy()
+    unreachable[2, 3] += 10.0
+
+    with pytest.raises((RuntimeError, ValueError)):
+        solver.solve(unreachable)
+
+    assert solver._robot.state.q[solver._q_offsets] == pytest.approx(joints)
+
+
+def test_solver_rejects_non_positive_manipulability_threshold() -> None:
+    pytest.importorskip("placo")
+
+    with pytest.raises(ValueError, match="manipulability thresholds"):
+        PlacoIkSolver(
+            str(DUAL_URDF_PATH),
+            1.0 / 90.0,
+            "left",
+            manipulability_sigma_stop=0.0,
+            manipulability_sigma_warn=0.04,
+        )
+
+
+@pytest.mark.parametrize(
+    "q4_deg,sigma_min,expected_activation",
+    [
+        (25.0, 0.04, 0.0),
+        (17.5, 0.025, 0.5),
+        (10.0, 0.01, 1.0),
+    ],
+)
+def test_auxiliary_weights_activate_only_inside_warning_margins(
+    q4_deg: float,
+    sigma_min: float,
+    expected_activation: float,
+) -> None:
+    class TaskSpy:
+        def __init__(self) -> None:
+            self.calls = []
+
+        def configure(self, name, priority, weight) -> None:
+            self.calls.append((name, priority, weight))
+
+    solver = object.__new__(PlacoIkSolver)
+    solver._q_offsets = np.arange(7, 14)
+    solver._robot = SimpleNamespace(
+        state=SimpleNamespace(q=np.zeros(21))
+    )
+    solver._robot.state.q[solver._q_offsets[3]] = math.radians(q4_deg)
+    solver._j4_task = TaskSpy()
+    solver._j4_min = math.radians(10.0)
+    solver._j4_warn = math.radians(25.0)
+    solver._j4_weight = 1e-4
+    solver._manipulability_task = TaskSpy()
+    solver._manipulability_sigma_stop = 0.01
+    solver._manipulability_sigma_warn = 0.04
+    solver._manipulability_weight = 1e-4
+    jacobian = np.zeros((6, 7))
+    jacobian[:, :6] = np.diag([1.0] * 5 + [sigma_min])
+    solver._active_tcp_jacobian = lambda: jacobian
+
+    solver._update_auxiliary_task_weights()
+
+    expected_weight = 1e-4 * expected_activation
+    assert solver._j4_task.calls == [
+        ("j4_soft_buffer", "soft", pytest.approx(expected_weight))
+    ]
+    assert solver._manipulability_task.calls == [
+        ("tcp_6d_manipulability", "soft", pytest.approx(expected_weight))
+    ]

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -31,6 +31,14 @@ QP_POSITION_TOLERANCE_M = 2e-3
 QP_ORIENTATION_TOLERANCE_RAD = 5e-3
 
 
+def _lower_margin_activation(value: float, stop: float, warn: float) -> float:
+    if not all(np.isfinite(item) for item in (value, stop, warn)):
+        raise ValueError("activation values must be finite")
+    if stop >= warn:
+        raise ValueError("activation stop must be smaller than warn")
+    return float(np.clip((warn - value) / (warn - stop), 0.0, 1.0))
+
+
 def _validated_transform(transform: np.ndarray) -> np.ndarray:
     values = np.asarray(transform, dtype=float)
     if values.shape != (4, 4) or not np.isfinite(values).all():
@@ -60,6 +68,15 @@ class PlacoIkSolver:
         urdf_path: str,
         dt: float,
         arm: str,
+        *,
+        j3_reference_deg: float | None = None,
+        j3_weight: float = 1e-5,
+        j4_min_deg: float | None = None,
+        j4_warn_deg: float | None = None,
+        j4_weight: float = 1e-4,
+        manipulability_sigma_stop: float = 0.01,
+        manipulability_sigma_warn: float = 0.04,
+        manipulability_weight: float = 0.0,
     ) -> None:
         if dt <= 0.0:
             raise ValueError("dt must be positive")
@@ -134,12 +151,37 @@ class PlacoIkSolver:
             ]
         )
         self._actual_joints: np.ndarray | None = None
+        weights = (j3_weight, j4_weight, manipulability_weight)
+        if not all(np.isfinite(value) and value >= 0.0 for value in weights):
+            raise ValueError("QP auxiliary weights must be finite and non-negative")
+        if j3_reference_deg is not None and not np.isfinite(j3_reference_deg):
+            raise ValueError("J3 reference must be finite")
+        if (j4_min_deg is None) != (j4_warn_deg is None):
+            raise ValueError("J4 minimum and warning angles must be configured together")
+        if j4_min_deg is not None:
+            if not all(np.isfinite(value) for value in (j4_min_deg, j4_warn_deg)):
+                raise ValueError("J4 angles must be finite")
+            if j4_warn_deg <= j4_min_deg:
+                raise ValueError("J4 warning angle must exceed its minimum")
+            j4_limits_deg = np.degrees(self._joint_limits[3])
+            if j4_min_deg < j4_limits_deg[0] or j4_warn_deg > j4_limits_deg[1]:
+                raise ValueError("J4 safety angles must stay within URDF limits")
+        if not (
+            np.isfinite(manipulability_sigma_stop)
+            and np.isfinite(manipulability_sigma_warn)
+            and 0.0 < manipulability_sigma_stop
+            < manipulability_sigma_warn
+        ):
+            raise ValueError(
+                "manipulability thresholds must satisfy 0 < stop < warn"
+            )
 
         self._solver = placo.KinematicsSolver(self._robot)
         self._solver.dt = dt
         self._solver.mask_fbase(True)
         for name in inactive_joint_names:
             self._solver.mask_dof(name)
+        self._solver.enable_joint_limits(True)
         self._solver.enable_velocity_limits(True)
         self._frame_task = self._solver.add_relative_frame_task(
             self._base_frame,
@@ -149,6 +191,53 @@ class PlacoIkSolver:
         self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
         self._solver.add_kinetic_energy_regularization_task(1e-6)
 
+        self._j3_task = None
+        if j3_reference_deg is not None and j3_weight > 0.0:
+            self._j3_task = self._solver.add_joints_task()
+            self._j3_task.set_joints(
+                {self._joint_names[2]: np.deg2rad(j3_reference_deg)}
+            )
+            self._j3_task.configure("j3_reference", "soft", j3_weight)
+
+        self._j4_task = None
+        self._j4_constraint = None
+        self._j4_min = None
+        self._j4_warn = None
+        self._j4_weight = j4_weight
+        if j4_min_deg is not None:
+            self._j4_min = float(np.deg2rad(j4_min_deg))
+            self._j4_warn = float(np.deg2rad(j4_warn_deg))
+            self._j4_task = self._solver.add_joints_task()
+            self._j4_task.set_joints(
+                {self._joint_names[3]: self._j4_warn}
+            )
+            self._j4_task.configure("j4_soft_buffer", "soft", 0.0)
+            matrix = np.zeros((1, self._robot.state.q.size))
+            matrix[0, self._q_offsets[3]] = -1.0
+            self._j4_constraint = (
+                self._solver.add_joint_space_half_spaces_constraint(
+                    matrix,
+                    np.asarray([-self._j4_min]),
+                )
+            )
+            self._j4_constraint.configure("j4_lower_bound", "hard")
+
+        self._manipulability_task = None
+        self._manipulability_sigma_stop = manipulability_sigma_stop
+        self._manipulability_sigma_warn = manipulability_sigma_warn
+        self._manipulability_weight = manipulability_weight
+        if manipulability_weight > 0.0:
+            self._manipulability_task = self._solver.add_manipulability_task(
+                self._tcp_frame,
+                "both",
+                1.0,
+            )
+            self._manipulability_task.configure(
+                "tcp_6d_manipulability",
+                "soft",
+                0.0,
+            )
+
     @property
     def joint_names(self) -> list[str]:
         return list(self._joint_names)
@@ -183,32 +272,64 @@ class PlacoIkSolver:
             float(orientation_task.error_norm()),
         )
 
+    def _active_tcp_jacobian(self) -> np.ndarray:
+        jacobian = np.asarray(
+            self._robot.frame_jacobian(
+                self._tcp_frame,
+                "local_world_aligned",
+            ),
+            dtype=float,
+        )[:, self._v_offsets]
+        if jacobian.shape != (6, 7) or not np.isfinite(jacobian).all():
+            raise ValueError("TCP Jacobian must be a finite 6x7 matrix")
+        return jacobian
+
+    def _update_auxiliary_task_weights(self) -> None:
+        if self._j4_task is not None:
+            q4 = float(self._robot.state.q[self._q_offsets[3]])
+            activation = _lower_margin_activation(
+                q4,
+                self._j4_min,
+                self._j4_warn,
+            )
+            self._j4_task.configure(
+                "j4_soft_buffer",
+                "soft",
+                self._j4_weight * activation,
+            )
+        if self._manipulability_task is not None:
+            sigma_min = float(
+                np.linalg.svd(
+                    self._active_tcp_jacobian(),
+                    compute_uv=False,
+                )[-1]
+            )
+            activation = _lower_margin_activation(
+                sigma_min,
+                self._manipulability_sigma_stop,
+                self._manipulability_sigma_warn,
+            )
+            self._manipulability_task.configure(
+                "tcp_6d_manipulability",
+                "soft",
+                self._manipulability_weight * activation,
+            )
+
+    def _restore_actual_joint_state(self) -> None:
+        self._robot.state.q[self._q_offsets] = self._actual_joints
+        self._robot.update_kinematics()
+
     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_a_b = _validated_transform(
-            target_tool_pose
-        )
-        result = np.asarray(
-            self._robot.state.q[self._q_offsets],
-            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()
+        try:
+            self._frame_task.T_a_b = _validated_transform(
+                target_tool_pose
+            )
             result = np.asarray(
                 self._robot.state.q[self._q_offsets],
                 dtype=float,
             ).copy()
-            self._validate_result(result, previous)
             position_error, orientation_error = self._target_errors()
             if (
                 position_error <= QP_POSITION_TOLERANCE_M
@@ -216,12 +337,32 @@ class PlacoIkSolver:
             ):
                 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"
-        )
+            for _ in range(QP_MAX_ITERATIONS):
+                previous = result
+                self._update_auxiliary_task_weights()
+                self._solver.solve(True)
+                self._robot.update_kinematics()
+                result = np.asarray(
+                    self._robot.state.q[self._q_offsets],
+                    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"
+            )
+        except Exception:
+            self._restore_actual_joint_state()
+            raise
 
     def _validate_result(
         self,

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -237,9 +237,9 @@ class RealManAdapter:
             )
 
     def read_joint_state(self) -> JointStateSnapshot:
-        arm = self._require_arm()
+        self._require_arm()
         started_at = time.monotonic()
-        result = arm.rm_get_joint_degree()
+        result = self._arm.rm_get_joint_degree()
         finished_at = time.monotonic()
         if not isinstance(result, tuple) or len(result) != 2:
             raise RuntimeError(
@@ -256,10 +256,10 @@ class RealManAdapter:
         )
 
     def send_joint_target(self, joints: list[float], follow: bool) -> None:
-        arm = self._require_arm()
+        self._require_arm()
         if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
             raise ValueError("joint target must contain 7 finite values")
-        ret = arm.rm_movej_canfd(
+        ret = self._arm.rm_movej_canfd(
             [math.degrees(value) for value in joints],
             follow,
             0,
@@ -315,10 +315,9 @@ class RealManAdapter:
             self._arm = None
             self._realtime_callback = None
 
-    def _require_arm(self) -> Any:
+    def _require_arm(self) -> None:
         if self._arm is None:
             raise RuntimeError("睿尔曼机械臂尚未连接")
-        return self._arm
 
     def _on_realtime_arm_state(self, data: Any) -> None:
         if not self._accept_realtime_feedback:
@@ -458,11 +457,11 @@ class RealManAdapter:
             self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
 
     def move_to_initial_pose(self) -> None:
-        arm = self._require_arm()
+        self._require_arm()
         if self._initial_joint_pose is None:
             raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
 
-        ret = arm.rm_movej(
+        ret = self._arm.rm_movej(
             self._initial_joint_pose,
             self._init_move_speed,
             0,

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -201,6 +201,14 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0])
         self.declare_parameter("use_mock", True)
         self.declare_parameter("robot_urdf_path", "")
+        self.declare_parameter("qp_j3_reference_deg", 0.0)
+        self.declare_parameter("qp_j3_weight", 1e-5)
+        self.declare_parameter("qp_j4_min_deg", 10.0)
+        self.declare_parameter("qp_j4_warn_deg", 25.0)
+        self.declare_parameter("qp_j4_weight", 1e-4)
+        self.declare_parameter("qp_manipulability_sigma_stop", 0.01)
+        self.declare_parameter("qp_manipulability_sigma_warn", 0.04)
+        self.declare_parameter("qp_manipulability_weight", 1e-4)
         self.declare_parameter("robot_ip", "192.168.1.18")
         self.declare_parameter("robot_port", 8080)
         self.declare_parameter("realtime_push_host_ip", "")
@@ -260,6 +268,30 @@ class SingleArmVelocityTeleop(Node):
         self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
         self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
         self._use_mock = self._bool_parameter("use_mock")
+        self._qp_j3_reference_deg = float(
+            self.get_parameter("qp_j3_reference_deg").value
+        )
+        self._qp_j3_weight = float(
+            self.get_parameter("qp_j3_weight").value
+        )
+        self._qp_j4_min_deg = float(
+            self.get_parameter("qp_j4_min_deg").value
+        )
+        self._qp_j4_warn_deg = float(
+            self.get_parameter("qp_j4_warn_deg").value
+        )
+        self._qp_j4_weight = float(
+            self.get_parameter("qp_j4_weight").value
+        )
+        self._qp_manipulability_sigma_stop = float(
+            self.get_parameter("qp_manipulability_sigma_stop").value
+        )
+        self._qp_manipulability_sigma_warn = float(
+            self.get_parameter("qp_manipulability_sigma_warn").value
+        )
+        self._qp_manipulability_weight = float(
+            self.get_parameter("qp_manipulability_weight").value
+        )
         self._follow = self._bool_parameter("follow")
         self._enable_tool_control = self._bool_parameter("enable_tool_control")
         self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
@@ -328,6 +360,18 @@ class SingleArmVelocityTeleop(Node):
             str(self.get_parameter("robot_urdf_path").value),
             self._dt,
             peripheral_arm,
+            j3_reference_deg=self._qp_j3_reference_deg,
+            j3_weight=self._qp_j3_weight,
+            j4_min_deg=self._qp_j4_min_deg,
+            j4_warn_deg=self._qp_j4_warn_deg,
+            j4_weight=self._qp_j4_weight,
+            manipulability_sigma_stop=(
+                self._qp_manipulability_sigma_stop
+            ),
+            manipulability_sigma_warn=(
+                self._qp_manipulability_sigma_warn
+            ),
+            manipulability_weight=self._qp_manipulability_weight,
         )
         debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
         self._joint_state_pub = self.create_publisher(
@@ -730,12 +774,16 @@ class SingleArmVelocityTeleop(Node):
         joint_target = self._solve_joint_target(target_pose)
         qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6
         send_started_ns = time.perf_counter_ns()
-        sent = self._send_joint_target(joint_target)
+        sent = self._send_and_commit_joint_target(
+            joint_target,
+            filtered_target,
+            filtered_orientation,
+            sent_target,
+            sent_orientation,
+            now,
+        )
         send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6
         if sent:
-            self._last_sent_target = sent_target
-            self._last_sent_orientation = sent_orientation.copy()
-            self._last_command_time = now
             self._stop_sent = False
         total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6
         try:
@@ -867,17 +915,15 @@ class SingleArmVelocityTeleop(Node):
 
     def _filter_target(self, target: list[float]) -> list[float]:
         if self._filtered_target is None:
-            self._filtered_target = list(target)
             return list(target)
 
         delta = [target[i] - self._filtered_target[i] for i in range(3)]
         distance = _norm(delta)
         alpha = self._adaptive_filter_alpha(distance)
-        self._filtered_target = [
+        return [
             alpha * target[i] + (1.0 - alpha) * self._filtered_target[i]
             for i in range(3)
         ]
-        return list(self._filtered_target)
 
     def _adaptive_filter_alpha(self, distance: float) -> float:
         if self._target_filter_fast_threshold_m <= 1e-9:
@@ -916,17 +962,15 @@ class SingleArmVelocityTeleop(Node):
 
     def _filter_orientation_target(self, target_rotation: np.ndarray) -> np.ndarray:
         if self._filtered_orientation_target is None:
-            self._filtered_orientation_target = _project_rotation(target_rotation)
-            return self._filtered_orientation_target.copy()
+            return _project_rotation(target_rotation)
 
         error = _so3_log(
             target_rotation @ self._filtered_orientation_target.T
         )
-        self._filtered_orientation_target = _project_rotation(
+        return _project_rotation(
             _so3_exp(self._orientation_filter_alpha * error)
             @ self._filtered_orientation_target
         )
-        return self._filtered_orientation_target.copy()
 
     def _limit_orientation_step(
         self,
@@ -1196,7 +1240,10 @@ class SingleArmVelocityTeleop(Node):
             self._last_valid_joint_target = list(snapshot.positions)
         return current_pose
 
-    def _solve_joint_target(self, target_pose: np.ndarray) -> list[float]:
+    def _solve_joint_target(
+        self,
+        target_pose: np.ndarray,
+    ) -> list[float] | None:
         if self._last_valid_joint_target is None:
             raise RuntimeError("valid joint feedback has not been initialized")
         try:
@@ -1206,10 +1253,28 @@ class SingleArmVelocityTeleop(Node):
                 f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
                 throttle_duration_sec=1.0,
             )
-            return list(self._last_valid_joint_target)
-        self._last_valid_joint_target = list(result)
+            return None
         return list(result)
 
+    def _send_and_commit_joint_target(
+        self,
+        joint_target: list[float] | None,
+        filtered_target: list[float],
+        filtered_orientation: np.ndarray,
+        sent_target: list[float],
+        sent_orientation: np.ndarray,
+        now: Time,
+    ) -> bool:
+        if joint_target is None or not self._send_joint_target(joint_target):
+            return False
+        self._last_valid_joint_target = list(joint_target)
+        self._filtered_target = list(filtered_target)
+        self._filtered_orientation_target = filtered_orientation.copy()
+        self._last_sent_target = list(sent_target)
+        self._last_sent_orientation = sent_orientation.copy()
+        self._last_command_time = now
+        return True
+
     def _safe_stop(self, reset_active: bool) -> None:
         if not self._stop_sent:
             self._send_stop_once()
@@ -1461,6 +1526,35 @@ class SingleArmVelocityTeleop(Node):
             raise ValueError("joint_max_speed must be > 0")
         if self._joint_command_max_acceleration <= 0.0:
             raise ValueError("joint_max_acc must be > 0")
+        qp_weights = (
+            self._qp_j3_weight,
+            self._qp_j4_weight,
+            self._qp_manipulability_weight,
+        )
+        if not all(
+            math.isfinite(value) and value >= 0.0
+            for value in qp_weights
+        ):
+            raise ValueError("QP auxiliary weights must be finite and non-negative")
+        if not math.isfinite(self._qp_j3_reference_deg):
+            raise ValueError("qp_j3_reference_deg must be finite")
+        if not all(
+            math.isfinite(value)
+            for value in (self._qp_j4_min_deg, self._qp_j4_warn_deg)
+        ):
+            raise ValueError("QP J4 angles must be finite")
+        if self._qp_j4_warn_deg <= self._qp_j4_min_deg:
+            raise ValueError("qp_j4_warn_deg must exceed qp_j4_min_deg")
+        if not (
+            math.isfinite(self._qp_manipulability_sigma_stop)
+            and math.isfinite(self._qp_manipulability_sigma_warn)
+            and 0.0 < self._qp_manipulability_sigma_stop
+            < self._qp_manipulability_sigma_warn
+        ):
+            raise ValueError(
+                "QP manipulability sigma thresholds must satisfy "
+                "0 < stop < warn"
+            )
 
     def _shutdown_tool_worker(self) -> None:
         if self._tool_worker_thread is None or self._tool_command_queue is None: