373 lines
12 KiB
Markdown
373 lines
12 KiB
Markdown
# 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`、合并分支或连接真机。
|