diff --git a/docs/superpowers/plans/2026-08-13-rm75-qp-robustness.md b/docs/superpowers/plans/2026-08-13-rm75-qp-robustness.md new file mode 100644 index 0000000..7d9577d --- /dev/null +++ b/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`、合并分支或连接真机。 diff --git a/docs/superpowers/specs/2026-08-13-rm75-qp-robustness-design.md b/docs/superpowers/specs/2026-08-13-rm75-qp-robustness-design.md new file mode 100644 index 0000000..0dc8a38 --- /dev/null +++ b/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 软项，不回退失败状态修复。 diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml index 60b85ff..d52420a 100755 --- a/xr_rm_bringup/config/dual_arm_rm75.yaml +++ b/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] diff --git a/xr_rm_bringup/config/left_arm_rm75.yaml b/xr_rm_bringup/config/left_arm_rm75.yaml index 12de26d..a1559a4 100644 --- a/xr_rm_bringup/config/left_arm_rm75.yaml +++ b/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] diff --git a/xr_rm_bringup/config/right_arm_rm75.yaml b/xr_rm_bringup/config/right_arm_rm75.yaml index 789480b..4526729 100644 --- a/xr_rm_bringup/config/right_arm_rm75.yaml +++ b/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] diff --git a/xr_rm_teleop/test/test_initial_joint_pose.py b/xr_rm_teleop/test/test_initial_joint_pose.py index 95ec063..70a7760 100644 --- a/xr_rm_teleop/test/test_initial_joint_pose.py +++ b/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")], diff --git a/xr_rm_teleop/test/test_joint_control.py b/xr_rm_teleop/test/test_joint_control.py index ae5d924..e01ec58 100644 --- a/xr_rm_teleop/test/test_joint_control.py +++ b/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: diff --git a/xr_rm_teleop/test/test_placo_transforms.py b/xr_rm_teleop/test/test_placo_transforms.py index ae40541..6885d18 100644 --- a/xr_rm_teleop/test/test_placo_transforms.py +++ b/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)) + ] diff --git a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py index 098f9bf..aae7734 100644 --- a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py +++ b/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, diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py index 1531331..55de731 100755 --- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py +++ b/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: