YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 优化双臂采摘QP稳健性

Browse Source
This commit is contained in:
YikaiFu-cart
2026-08-13 10:23:35 +08:00
parent 807374c9fd
commit 7a5c27d6b9
10 changed files with 1078 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/superpowers/plans/2026-08-13-rm75-qp-robustness.md
+372
View File
@@ -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-13-rm75-qp-robustness-design.md
+175
View File
@@ -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 软项，不回退失败状态修复。
xr_rm_bringup/config/dual_arm_rm75.yaml
+19
View File
@@ -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
+10
View File
@@ -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
+10
View File
@@ -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_teleop/test/test_initial_joint_pose.py
+37
View File
@@ -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
+52 -4
View File
@@ -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
+129
View File
@@ -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
+166 -25
View File
@@ -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/single_arm_velocity_teleop.py
+108 -14
View File
@@ -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: