Compare commits
13
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 YikaiFu-cart
|
e20c5a983e | ||
|
YikaiFu-cart
|
6982620041 | ||
|
YikaiFu-cart
|
6008e34b5d | ||
|
YikaiFu-cart
|
c1ea1a2816 | ||
|
YikaiFu-cart
|
40be5560ee | ||
|
YikaiFu-cart
|
1ec74f107b | ||
|
YikaiFu-cart
|
deeee076d7 | ||
|
YikaiFu-cart
|
e60d620dfe | ||
|
YikaiFu-cart
|
043d3d0533 | ||
|
YikaiFu-cart
|
7b2caf3012 | ||
|
YikaiFu-cart
|
c62d69e9ab | ||
|
YikaiFu-cart
|
f6b484d168 | ||
|
YikaiFu-cart
|
bb672e3f39 |
+1
-2
@@ -43,5 +43,4 @@ AMENT_IGNORE
|
|||||||
# Built Visual Studio Code Extensions
|
# Built Visual Studio Code Extensions
|
||||||
*.vsix
|
*.vsix
|
||||||
|
|
||||||
.codex
|
.codex
|
||||||
.worktrees/
|
|
||||||
@@ -288,7 +288,7 @@ test: 添加 xxx 测试
|
|||||||
|
|
||||||
## 项目专属规则
|
## 项目专属规则
|
||||||
|
|
||||||
* 本项目面向 Ubuntu 22.04 和 ROS2 Humble；构建、测试和运行命令应在工作空间根目录 `/home/robot/WS_xr` 执行，并先 `source /opt/ros/humble/setup.bash`。
|
* 本项目面向 Ubuntu 22.04 和 ROS2 Humble；编译前必须先切换到工作空间根目录 `/home/robot/WS_xr`，并执行 `source /opt/ros/humble/setup.bash`。禁止在 `/home/robot/WS_xr/src` 中运行 `colcon build`，否则会在源码目录生成多余的 `build/`、`install/` 和 `log/`；测试和运行命令也应在工作空间根目录执行。
|
||||||
* 工作空间包含 `xr_rm_input`、`xr_rm_teleop` 两个 `ament_python` 包，以及 `xr_rm_interfaces`、`xr_rm_bringup` 两个 `ament_cmake` 包；优先使用现有 ROS2 包、节点和消息，不要另建重复入口。
|
* 工作空间包含 `xr_rm_input`、`xr_rm_teleop` 两个 `ament_python` 包，以及 `xr_rm_interfaces`、`xr_rm_bringup` 两个 `ament_cmake` 包；优先使用现有 ROS2 包、节点和消息，不要另建重复入口。
|
||||||
* 修改 ROS 节点、launch、消息定义或安装配置后，至少运行 `colcon build --symlink-install`；涉及遥操作姿态控制时，再运行 `pytest src/xr_rm_teleop/test/test_orientation_control.py`。
|
* 修改 ROS 节点、launch、消息定义或安装配置后，至少运行 `colcon build --symlink-install`；涉及遥操作姿态控制时，再运行 `pytest src/xr_rm_teleop/test/test_orientation_control.py`。
|
||||||
* 遥操作统一使用 `xr_rm_bringup/launch/arm_debug.launch.py`；调试和验证默认使用 `use_mock:=true`，未经用户明确要求不得连接真机、移动机械臂或操作夹爪。
|
* 遥操作统一使用 `xr_rm_bringup/launch/arm_debug.launch.py`；调试和验证默认使用 `use_mock:=true`，未经用户明确要求不得连接真机、移动机械臂或操作夹爪。
|
||||||
|
|||||||
@@ -1,44 +1,97 @@
|
|||||||
# XR-RM75 双臂遥操作
|
# XR-RM75 双臂遥操作工作空间
|
||||||
|
|
||||||
基于 **Ubuntu 22.04、ROS2 Humble、PICO 4 Ultra 和睿尔曼 RM75** 的双臂 XR 遥操作
|
本仓库是面向 **Ubuntu 22.04 + ROS2 Humble + PICO 4 Ultra + 睿尔曼 RM75** 的阶段一 XR 双臂遥操作项目。当前目标是先跑通一条低速、安全、可调试的闭环：
|
||||||
工作空间，支持单臂/双臂 Mock 与真机控制，以及 MuJoCo 运动学显示。
|
|
||||||
|
|
||||||
> [!WARNING]
|
|
||||||
> 真机命令会连接并控制机械臂。首次运行必须从 Mock 和单臂低速验证开始，确保急停
|
|
||||||
> 可用且工作区无人。当前项目没有双臂碰撞检测或避障。
|
|
||||||
|
|
||||||
## 当前能力
|
|
||||||
|
|
||||||
- PICO/XR 双手柄 UDP 输入，相对位姿目标与 Placo QP 七关节控制。
|
|
||||||
- 单臂/双臂 Mock 与真机、手柄/话题夹爪控制，以及只读 MuJoCo 双臂显示。
|
|
||||||
- 工作空间/圆柱限位、速度限制、指令超时和安全慢停。
|
|
||||||
- 统一 launch、Tkinter 启动面板、调试话题和 Mock 输入工具。
|
|
||||||
|
|
||||||
尚未完成：D405/D435 视频流、数据记录、相机标定、目标检测、双臂碰撞避障、任务级状态机，以及 PICO 与 ROS 的完整时间同步和状态回传。
|
|
||||||
|
|
||||||
## 系统架构
|
|
||||||
|
|
||||||
```text
|
```text
|
||||||
PICO / XRoboToolkit
|
PICO/XR 双手柄 UDP JSON
|
||||||
-> UDP JSON
|
|
||||||
-> xr_rm_input/udp_controller_receiver
|
-> xr_rm_input/udp_controller_receiver
|
||||||
-> /xr/left_controller、/xr/right_controller
|
-> /xr/left_controller 与 /xr/right_controller
|
||||||
-> xr_rm_teleop/single_arm_velocity_teleop
|
-> xr_rm_teleop/single_arm_velocity_teleop
|
||||||
-> 相对 TCP 目标 + Placo QP
|
-> Placo QP 单步逆解
|
||||||
-> Mock 或 RM75 rm_movej_canfd
|
-> 左右 RM75 七关节角透传控制
|
||||||
-> joint_states / 调试话题
|
-> /xr_rm/<arm_name>/joint_states
|
||||||
-> 可选 xr_rm_mujoco/dual_arm_simulator
|
-> 可选 xr_rm_mujoco/dual_arm_simulator
|
||||||
|
-> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
|
||||||
```
|
```
|
||||||
|
|
||||||
工作空间包含五个 ROS2 包：`xr_rm_input` 负责手柄输入，`xr_rm_interfaces` 定义消息，
|
当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP。姿态目标使用旋转矩阵和 SO(3) 最短路径完成死区、滤波与限速，不经过 RPY。每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。
|
||||||
`xr_rm_teleop` 实现遥操作与真机适配，`xr_rm_bringup` 提供启动和配置，
|
|
||||||
`xr_rm_mujoco` 负责只读运动学显示。
|
|
||||||
|
|
||||||
`single_arm_velocity_teleop` 每个实例只控制一台机械臂；双臂模式分别启动 `left_arm_teleop` 和 `right_arm_teleop`。
|
## 当前范围
|
||||||
|
|
||||||
## 环境与构建
|
已完成：
|
||||||
|
|
||||||
在工作空间根目录执行：
|
- PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
|
||||||
|
- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
|
||||||
|
- 使用现有双臂 URDF 的 MuJoCo 运动学显示，可由 Mock 或真机反馈同步双臂姿态。
|
||||||
|
- RM75 真机连接适配，包含关节反馈缓存、`rm_movej_canfd` 关节透传、安全速度/加速度配置、可选初始化点位移动。
|
||||||
|
- Placo 0.9.4 RM75 QP 逆解；收到首帧有效关节反馈后才启用，求解失败时保留上一组有效关节目标。
|
||||||
|
- 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
|
||||||
|
- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
|
||||||
|
- XRoboToolkit bridge 读取左右手柄 pose、Grip、Trigger、摇杆和主副按键。
|
||||||
|
|
||||||
|
暂未完成：
|
||||||
|
|
||||||
|
- D405/D435 视频流、数据记录、相机标定和目标检测链路。
|
||||||
|
- 双臂碰撞模型、任务级状态机、自动采摘策略。
|
||||||
|
- PICO 端与 ROS 端的完整时间同步和状态回传。
|
||||||
|
|
||||||
|
## 项目结构
|
||||||
|
|
||||||
|
```text
|
||||||
|
src/
|
||||||
|
├── README.md # 项目主文档
|
||||||
|
├── AGENTS.md # Codex 项目工作流和安全规则
|
||||||
|
├── docs/superpowers/ # Superpowers 设计与实施计划
|
||||||
|
├── xr_rm_bringup/
|
||||||
|
│ ├── config/
|
||||||
|
│ │ ├── dual_arm_mujoco.yaml # MuJoCo 显示刷新参数
|
||||||
|
│ │ ├── dual_arm_rm75.yaml # 双臂配置：left_arm_teleop 与 right_arm_teleop
|
||||||
|
│ │ ├── left_arm_rm75.yaml # 左臂单独调试配置
|
||||||
|
│ │ ├── right_arm_rm75.yaml # 右臂单独调试配置
|
||||||
|
│ │ └── peripherals_rm75.yaml # 左右臂末端外设配置
|
||||||
|
│ ├── launch/
|
||||||
|
│ │ └── arm_debug.launch.py # 统一入口：单臂/双臂、Mock/真机、可选 MuJoCo
|
||||||
|
│ └── tools/
|
||||||
|
│ ├── launcher_ui.py # 图形化调试启动面板
|
||||||
|
│ └── realman_dual_arm_state_monitor.py
|
||||||
|
├── xr_rm_input/
|
||||||
|
│ ├── launch/
|
||||||
|
│ │ └── udp_receiver.launch.py # 低层 UDP 接收测试入口
|
||||||
|
│ ├── test/
|
||||||
|
│ │ └── test_controller_fields.py
|
||||||
|
│ └── xr_rm_input/
|
||||||
|
│ ├── udp_controller_receiver.py
|
||||||
|
│ ├── xrobotoolkit_to_udp_bridge.py
|
||||||
|
│ └── sample_udp_sender.py # 本机扫轴/正弦模拟手柄 UDP 数据
|
||||||
|
├── xr_rm_interfaces/
|
||||||
|
│ └── msg/
|
||||||
|
│ └── XrController.msg # 手柄状态与位姿
|
||||||
|
├── xr_rm_mujoco/
|
||||||
|
│ └── xr_rm_mujoco/
|
||||||
|
│ └── dual_arm_simulator.py # 双臂 URDF 运动学映射与 MuJoCo viewer
|
||||||
|
└── xr_rm_teleop/
|
||||||
|
├── models/
|
||||||
|
│ ├── rm75/ # 旧 RM75 模型资源（launch 不再选用）
|
||||||
|
│ ├── rm75_omnipicker/ # 旧单臂 OmniPicker 模型资源
|
||||||
|
│ └── dual_rm75/ # 当前左右臂统一使用的双 RM75 URDF 与网格
|
||||||
|
└── xr_rm_teleop/
|
||||||
|
├── placo_ik_solver.py # Placo 0.9.4 单步 QP 逆解
|
||||||
|
├── single_arm_velocity_teleop.py
|
||||||
|
├── realman_adapter.py
|
||||||
|
└── fun_peripheral.py
|
||||||
|
```
|
||||||
|
|
||||||
|
`single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。
|
||||||
|
|
||||||
|
## Superpowers Git 约束
|
||||||
|
|
||||||
|
使用 Superpowers 执行任务时，只允许按 skill 工作流创建本地 Git 提交。
|
||||||
|
禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。skill 如需
|
||||||
|
独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
|
||||||
|
|
||||||
|
## 环境准备
|
||||||
|
|
||||||
|
在工作空间根目录，也就是包含 `src/` 的目录执行：
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
cd /home/robot/WS_xr
|
cd /home/robot/WS_xr
|
||||||
@@ -49,125 +102,440 @@ colcon build --symlink-install
|
|||||||
source install/setup.bash
|
source install/setup.bash
|
||||||
```
|
```
|
||||||
|
|
||||||
遥操作和 MuJoCo 节点固定使用 `/home/robot/miniconda3/envs/xr/bin/python`，
|
真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。
|
||||||
其中固定 Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0；不要从
|
|
||||||
用户或系统 Python 覆盖这些版本。
|
|
||||||
|
|
||||||
真机模式另需睿尔曼 Python API2；Mock 模式不依赖厂商 SDK。
|
遥操作和 MuJoCo 节点固定由 `/home/robot/miniconda3/envs/xr/bin/python` 启动，并复用其中的 Python 3.10、Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0。`ros2`、`colcon`、pytest 和 `udp_controller_receiver` 仍使用系统 Python。禁止通过 `pip --user`、`sudo pip` 或系统安装升级 Placo、Pinocchio、EigenPy 和 NumPy。
|
||||||
|
|
||||||
## 快速开始
|
只读检查 Placo 版本：
|
||||||
|
|
||||||
以下命令均在 `/home/robot/WS_xr` 执行，并先 source ROS2 与 `install/setup.bash`。
|
|
||||||
|
|
||||||
### Mock
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
|
/home/robot/miniconda3/envs/xr/bin/python -c \
|
||||||
|
"import importlib.metadata; print(importlib.metadata.version('placo'))"
|
||||||
```
|
```
|
||||||
|
|
||||||
另开终端发送模拟手柄数据：
|
输出必须为 `0.9.4`。
|
||||||
|
|
||||||
|
同时检查 MuJoCo：
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 run xr_rm_input sample_udp_sender \
|
/home/robot/miniconda3/envs/xr/bin/python -c \
|
||||||
--hand both --host 127.0.0.1 --port 15000 \
|
"import mujoco; print(mujoco.__version__)"
|
||||||
|
```
|
||||||
|
|
||||||
|
当前验证版本为 `3.10.0`。系统 pytest 会通过 `xr_rm_mujoco/test/conftest.py` 复用该固定 XR 环境中的 MuJoCo，因此新包可直接按 ROS2 标准方式测试：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
colcon test --packages-select xr_rm_mujoco --event-handlers console_direct+
|
||||||
|
colcon test-result --verbose
|
||||||
|
```
|
||||||
|
|
||||||
|
如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
export XR_RM_WS=/home/robot/WS_xr
|
||||||
|
```
|
||||||
|
|
||||||
|
## 使用 launcher_ui.py 调试
|
||||||
|
|
||||||
|
推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、source ROS2 与 `install/setup.bash`，并把每个命令放到独立终端中运行。
|
||||||
|
|
||||||
|
源码方式启动：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
python3 src/xr_rm_bringup/tools/launcher_ui.py
|
||||||
|
```
|
||||||
|
|
||||||
|
构建后也可以通过 ROS2 入口启动：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
ros2 run xr_rm_bringup launcher_ui
|
||||||
|
```
|
||||||
|
|
||||||
|
面板顶部的 `Mode` 分为四类：
|
||||||
|
|
||||||
|
- `Simulation`：双臂 mock、XRoboToolkit bridge、双手 sample UDP 和 controller 频率监控。
|
||||||
|
- `MuJoCo`：双臂 Mock/真机 MuJoCo launch、XRoboToolkit bridge 和 controller 频率监控；真机命令会连接两台 RM75。
|
||||||
|
- `Real Hardware`：左右臂网络 ping、左臂/右臂/双臂真机 launch、XRoboToolkit bridge 和左右夹爪开合。
|
||||||
|
- `Diagnostics`：`ros2 doctor --report`、四个核心包的 `ros2 pkg prefix`、controller 位置/频率监控。
|
||||||
|
|
||||||
|
常用按钮：
|
||||||
|
|
||||||
|
- `Run Selected`：运行当前选中的命令。双击列表项也可以运行。
|
||||||
|
- `Check Env`：检查 ROS2 Humble、工作空间 build、终端、四个核心 ROS 包、睿尔曼 API2。
|
||||||
|
- `Stop All`：结束由本工作空间启动的 launch、sample sender、topic monitor、MuJoCo viewer、相关 ROS 节点和终端窗口。
|
||||||
|
|
||||||
|
`Stop All` 会保留现有 PC Service；点击启动器窗口 `X` 并确认退出时会额外停止 PC Service。两条清理路径都会停止 `dual_arm_simulator`，关闭 MuJoCo viewer。
|
||||||
|
|
||||||
|
每个模式都会附带基础监控入口：
|
||||||
|
|
||||||
|
- `Open Controller Topic Monitor`：同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
|
||||||
|
- `Open ROS Topic/Node List Monitor`：每秒刷新 `ros2 topic list` 和 `ros2 node list`。
|
||||||
|
|
||||||
|
`Simulation` 和 `MuJoCo` 模式还提供 `Open Controller Hz Monitor`；`Diagnostics` 同时提供 controller 位置与频率监控。
|
||||||
|
|
||||||
|
分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
sudo apt install terminator wmctrl xdotool
|
||||||
|
sudo update-alternatives --config x-terminal-emulator
|
||||||
|
```
|
||||||
|
|
||||||
|
## 推荐调试顺序
|
||||||
|
|
||||||
|
第一步：检查环境。
|
||||||
|
|
||||||
|
打开 `launcher_ui.py`，点击 `Check Env`。如果 `install/setup.bash` 缺失，先回工作空间根目录重新执行 `colcon build --symlink-install`。
|
||||||
|
|
||||||
|
第二步：分别跑左、右臂 mock 闭环。
|
||||||
|
|
||||||
|
分两个终端依次验证左臂：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
|
||||||
|
ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port 15000 \
|
||||||
--pattern axis_sweep --seconds 30
|
--pattern axis_sweep --seconds 30
|
||||||
```
|
```
|
||||||
|
|
||||||
单臂调试时将 `arm` 改为 `left` 或 `right`。推荐先分别完成左右单臂
|
停止左臂进程后，再分别验证右臂：
|
||||||
Mock，再进入双臂或真机验证。
|
|
||||||
|
|
||||||
### MuJoCo
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
|
||||||
arm:=both use_mock:=true use_mujoco:=true
|
ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port 15000 \
|
||||||
|
--pattern axis_sweep --seconds 30
|
||||||
```
|
```
|
||||||
|
|
||||||
MuJoCo 只订阅关节状态，不参与控制。真机显示可将 `use_mock` 改为 `false`，
|
`sample_udp_sender` 默认使用 `axis_sweep` 扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
|
||||||
但该命令会同时连接两台 RM75。
|
|
||||||
|
|
||||||
### PICO 输入
|
观察：
|
||||||
|
|
||||||
只保留一个 UDP 输入源，然后启动 XRoboToolkit bridge：
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
|
ros2 topic echo /xr/left_controller
|
||||||
--host 127.0.0.1 --port 15000 --hz 90
|
ros2 topic echo /xr/right_controller
|
||||||
|
ros2 topic echo /xr_rm/left_rm75/target_pose
|
||||||
|
ros2 topic echo /xr_rm/right_rm75/target_pose
|
||||||
|
ros2 topic echo /xr_rm/left_rm75/cmd_vel
|
||||||
|
ros2 topic echo /xr_rm/right_rm75/cmd_vel
|
||||||
```
|
```
|
||||||
|
|
||||||
确认左右 topic 持续接收数据：
|
第三步：单臂真机。
|
||||||
|
|
||||||
```bash
|
先只上一个臂，确认网络、方向、急停和限幅：
|
||||||
ros2 topic hz /xr/left_controller
|
|
||||||
ros2 topic hz /xr/right_controller
|
|
||||||
```
|
|
||||||
|
|
||||||
图形启动面板可运行 `python3 src/xr_rm_bringup/tools/launcher_ui.py`，提供
|
|
||||||
Simulation、MuJoCo、Real Hardware 和 Diagnostics 模式。
|
|
||||||
|
|
||||||
### 真机
|
|
||||||
|
|
||||||
确认对应 YAML 中 `move_to_initial_pose_on_connect: false`，再从单臂开始：
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
|
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
|
||||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
|
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
|
||||||
```
|
```
|
||||||
|
|
||||||
单臂方向、限位、急停、超时停止和夹爪均验证后，才能启动双臂：
|
所有 YAML 默认都不会执行 `movej(initial_joint_pose)`。只有确认安全区清空后，才可在当前使用的
|
||||||
|
`left_arm_rm75.yaml`、`right_arm_rm75.yaml` 或 `dual_arm_rm75.yaml` 中将
|
||||||
|
`move_to_initial_pose_on_connect` 改为 `true`。
|
||||||
|
|
||||||
|
第四步：双臂真机。
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
|
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
|
||||||
```
|
```
|
||||||
|
|
||||||
按住 `grip` 控制对应机械臂；松开后停止。点击 `trigger` 切换对应夹爪开/关。
|
双臂默认不会自动移动到初始化点；机器人地址、控制参数和初始化移动开关均从
|
||||||
左手 X、右手 A 会请求对应机械臂回到配置的初始位姿；真机使用前必须清空安全区。
|
`dual_arm_rm75.yaml` 读取。
|
||||||
|
|
||||||
## Launch 参数
|
## Launch 入口说明
|
||||||
|
|
||||||
统一入口为 `xr_rm_bringup/launch/arm_debug.launch.py`：
|
`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 Simulation、MuJoCo 和 Real Hardware launch 命令都调用它。
|
||||||
|
|
||||||
| 参数 | 默认值 | 说明 |
|
常用参数：
|
||||||
| --- | --- | --- |
|
|
||||||
| `arm` | `right` | `left`、`right` 或 `both` |
|
|
||||||
| `use_mock` | `true` | `false` 会连接真机 |
|
|
||||||
| `use_mujoco` | `false` | 仅支持 `arm:=both` |
|
|
||||||
| `udp_host` | `0.0.0.0` | UDP 监听地址 |
|
|
||||||
| `udp_port` | `15000` | UDP 监听端口 |
|
|
||||||
| `udp_timer_hz` | `200.0` | UDP receiver 轮询频率 |
|
|
||||||
|
|
||||||
## 配置
|
- `arm`：`left`、`right`、`both`，默认 `right`。
|
||||||
|
- `use_mock`：`true` 不连接真机，`false` 连接 RM75。
|
||||||
|
- `use_mujoco`：`true` 额外启动双臂 MuJoCo 显示，默认 `false`，仅支持 `arm:=both`。
|
||||||
|
- `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
|
||||||
|
- `udp_port`：UDP 监听端口，默认 `15000`。
|
||||||
|
- `udp_timer_hz`：UDP receiver 轮询频率，默认 `200.0`。
|
||||||
|
|
||||||
| 文件 | 用途 |
|
机器人 IP/端口、控制频率、CANFD、限速、工具和初始化位姿等行为参数只由对应 YAML
|
||||||
| --- | --- |
|
配置，launch 不再提供同名覆盖项。
|
||||||
| `dual_arm_rm75.yaml` | 双臂节点、网络、控制与安全参数 |
|
|
||||||
| `left_arm_rm75.yaml` | 左臂单独调试 |
|
|
||||||
| `right_arm_rm75.yaml` | 右臂单独调试 |
|
|
||||||
| `peripherals_rm75.yaml` | 工具坐标、负载和末端执行器 |
|
|
||||||
| `dual_arm_mujoco.yaml` | MuJoCo 刷新频率 |
|
|
||||||
|
|
||||||
修改某一侧控制参数时，同时检查单臂和双臂 YAML 是否需要同步。必须保留工作空间/
|
## MuJoCo 双臂仿真
|
||||||
圆柱限位、线速度与角速度限制、关节速度/加速度限制、指令超时和安全停止逻辑。
|
|
||||||
|
|
||||||
`configure_safety_limits` 不得默认关闭；
|
无真机时，由 Mock 关节状态驱动 MuJoCo：
|
||||||
`move_to_initial_pose_on_connect` 必须保持默认 `false`。
|
|
||||||
|
|
||||||
## 测试
|
|
||||||
|
|
||||||
在工作空间根目录执行：
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
|
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||||||
|
arm:=both use_mock:=true use_mujoco:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
连接真机时，由两台 RM75 的实际关节反馈同步 MuJoCo。下面命令会连接真机，执行前必须完成真机安全检查：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||||||
|
arm:=both use_mock:=false use_mujoco:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
桌面 UI 的 `MuJoCo` 模式分别提供上述 Mock 和真机命令，并明确标记会连接真机的 `Dual Arm MuJoCo Real Hardware Launch`。
|
||||||
|
|
||||||
|
MuJoCo 只订阅当前关节状态，不参与控制，也不向真机下发指令：
|
||||||
|
|
||||||
|
- `/xr_rm/left_rm75/joint_states`、`/xr_rm/right_rm75/joint_states`：当前适配器反馈；Mock 与真机模式均按控制周期约 `90 Hz` 发布。真机底层原始反馈周期仍为 `5 ms`（约 `200 Hz`），由遥操作节点按 `90 Hz` 采样发布。
|
||||||
|
- `/xr_rm/left_rm75/joint_target`、`/xr_rm/right_rm75/joint_target`：经 QP 和现有限制处理后的目标关节角，仅用于调试，MuJoCo 不订阅。
|
||||||
|
- MuJoCo viewer 默认按 `dual_arm_mujoco.yaml` 中的 `60 Hz` 刷新。其初始姿态直接来自 `dual_arm_rm75.yaml` 的 `initial_joint_pose`，不会在 MuJoCo YAML 中重复保存。
|
||||||
|
|
||||||
|
Mock 模式的遥操作目标仍经过 `dual_arm_rm75.yaml` 中的工作空间、圆柱、线速度、角速度、关节速度/加速度、超时和停止限制。左手 X、右手 A 分别立即复位对应 Mock 机械臂；Grip 保持按下时，下一控制周期会重新锚定并继续遥操作。真机复位完成后仍需松开 Grip 才能恢复遥操作，且 `move_to_initial_pose_on_connect` 保持为 `false`，连接真机不会自动移动。
|
||||||
|
|
||||||
|
## 配置文件说明
|
||||||
|
|
||||||
|
`xr_rm_bringup/config/dual_arm_rm75.yaml` 是双臂配置主文件，包含两个 ROS 节点命名空间：
|
||||||
|
|
||||||
|
- `left_arm_teleop`
|
||||||
|
- `right_arm_teleop`
|
||||||
|
|
||||||
|
`left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。
|
||||||
|
|
||||||
|
`dual_arm_mujoco.yaml` 只保存 MuJoCo viewer 刷新频率；双臂初始关节角和遥操作限制继续统一读取 `dual_arm_rm75.yaml`。
|
||||||
|
|
||||||
|
`xr_rm_bringup/config/peripherals_rm75.yaml` 保存真实控制器使用的末端工具坐标、负载和左右臂外设选择。左臂 `scissorgripper: 2` 是外设选择值，选择 `minisci`，TCP 的 Z 向偏移为 `0.165 m`；右臂 `scissorgripper: 1`，选择 `omnipic`，TCP 的 Z 向偏移为 `0.14 m`。真机连接阶段仍会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。
|
||||||
|
|
||||||
|
Placo 使用 `xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`。左右 ROS 节点分别创建独立 solver：左臂从 `scissor_base_link` 到 `scissor_scissor_tcp`，并 mask 右臂；右臂从 `omnipic_base_link` 到 `omnipic_OmniPic_tcp`，并 mask 左臂。节点目标仍在各自局部基坐标系中，现有 PICO 映射不改为公共坐标系。
|
||||||
|
|
||||||
|
重点控制参数：
|
||||||
|
|
||||||
|
- `controller_topic`：订阅的手柄话题。
|
||||||
|
- `scale`：手柄位移到 TCP 位移的比例。
|
||||||
|
- `target_filter_alpha` / `target_filter_alpha_fast`：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
|
||||||
|
- `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。
|
||||||
|
- `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
|
||||||
|
- `enable_orientation_control`：是否把手柄相对旋转映射到 TCP 姿态。
|
||||||
|
- `orientation_filter_alpha` / `orientation_deadband_rad`：按 SO(3) 最短旋转角处理的目标 TCP 姿态滤波和死区。
|
||||||
|
- `max_orientation_speed`：目标 TCP 姿态沿 SO(3) 最短路径的最大角速度，当前为 `0.5 rad/s`。
|
||||||
|
- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
|
||||||
|
- `cyl_radius_limit`：基座圆柱半径限制。
|
||||||
|
- `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
|
||||||
|
- `robot_ip` / `robot_port`：RM75 TCP 控制连接地址。
|
||||||
|
- `realtime_push_host_ip`：连接机械臂 Wi-Fi 后本机实际 IPv4；可用
|
||||||
|
`ip -4 route get 192.168.192.19` 查看输出中的 `src`，当前为 `192.168.192.148`。
|
||||||
|
- `realtime_push_port`：UDP 主动反馈端口；左臂 `8089`、右臂 `8090`，同机双臂不能重复。
|
||||||
|
- `realtime_push_cycle_ms`：UDP 主动反馈周期，当前为厂商支持的 `5 ms`。
|
||||||
|
- `follow` / `canfd_trajectory_mode`：`rm_movej_canfd` 的高跟随和轨迹模式参数。
|
||||||
|
- 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证；确认关节加速度与反馈稳定后，再单独测试高跟随。
|
||||||
|
- `initial_joint_pose`：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。
|
||||||
|
|
||||||
|
当前 `/xr/*_controller` 的坐标处理：
|
||||||
|
|
||||||
|
- XRoboToolkit bridge 原样转发 SDK 的手柄位置和四元数，不额外转换坐标轴。
|
||||||
|
- receiver 默认按 `xyzw` 解析四元数，也可通过 `quat_order:=wxyz` 切换。
|
||||||
|
- 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
|
||||||
|
- 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。
|
||||||
|
- 两侧局部 `-Y` 均指向机器人前方；局部 `+Y` 指向后方，后方工作空间仅保留 `0.10 m`。
|
||||||
|
- 左臂局部 `+X/+Y/+Z` 分别指向下/后/左外侧；右臂分别指向上/后/右外侧。
|
||||||
|
|
||||||
|
如果某个机械臂方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
|
||||||
|
|
||||||
|
## 末端工具开合
|
||||||
|
|
||||||
|
真机 launch 默认会在遥操作节点内启用工具控制。左/右手柄 `trigger` 从低于阈值按到 `>= 0.95` 时，会切换一次对应夹爪开/关状态，并保持到下一次点击。`grip` 仍只控制机械臂运动，不影响夹爪 trigger 切换。
|
||||||
|
|
||||||
|
也可以用 Bool 话题手动控制开合，`true` 表示打开，`false` 表示闭合：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
|
||||||
|
ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
|
||||||
|
|
||||||
|
ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
|
||||||
|
ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
|
||||||
|
```
|
||||||
|
|
||||||
|
桌面 UI 的 `Real Hardware` 模式提供 `Left/Right Gripper Open/Close` 命令项；运行双臂真机 launch 时也可直接通过左右手柄 `trigger` 分别切换夹爪。
|
||||||
|
|
||||||
|
## UDP 数据格式
|
||||||
|
|
||||||
|
当前 XRoboToolkit bridge 每个周期发送一个双手柄 JSON 包：
|
||||||
|
|
||||||
|
```json
|
||||||
|
{
|
||||||
|
"t": 12.345,
|
||||||
|
"source_time": 12.345,
|
||||||
|
"seq": 42,
|
||||||
|
"frame_id": "xr_world",
|
||||||
|
"controllers": {
|
||||||
|
"left": {
|
||||||
|
"hand": "left",
|
||||||
|
"grip": true,
|
||||||
|
"trigger": 0.0,
|
||||||
|
"axis": [0.2, -0.4],
|
||||||
|
"buttons": {
|
||||||
|
"primary": true,
|
||||||
|
"secondary": false
|
||||||
|
},
|
||||||
|
"pos": [-0.12, 1.05, 0.30],
|
||||||
|
"quat": [0.0, 0.0, 0.0, 1.0],
|
||||||
|
"pose_valid": true,
|
||||||
|
"pose_source": "xrobotoolkit"
|
||||||
|
},
|
||||||
|
"right": {
|
||||||
|
"hand": "right",
|
||||||
|
"grip": true,
|
||||||
|
"trigger": 1.0,
|
||||||
|
"axis": [-0.1, 0.3],
|
||||||
|
"buttons": {
|
||||||
|
"primary": false,
|
||||||
|
"secondary": true
|
||||||
|
},
|
||||||
|
"pos": [0.12, 1.05, 0.30],
|
||||||
|
"quat": [0.0, 0.0, 0.0, 1.0],
|
||||||
|
"pose_valid": true,
|
||||||
|
"pose_source": "xrobotoolkit"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
字段说明：
|
||||||
|
|
||||||
|
- `t` / `source_time`：bridge 的 PC 单调时间，用于诊断发送周期。
|
||||||
|
- `seq`：bridge 递增的 UDP 包序号，bridge 重启后重新计数。
|
||||||
|
- `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
|
||||||
|
- `grip`：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
|
||||||
|
- `trigger`：经过 bridge 滞回处理的 `0.0/1.0` 值；上升沿切换对应夹爪状态。
|
||||||
|
- `axis`：摇杆 `[x, y]`，每个分量限制在 `-1.0` 到 `1.0`。
|
||||||
|
- `buttons.primary`：左手 X 键或右手 A 键。
|
||||||
|
- `buttons.secondary`：左手 Y 键或右手 B 键。
|
||||||
|
- `pos`：手柄位置，长度 3。
|
||||||
|
- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
|
||||||
|
- `pose_valid`：姿态是否可信；`false` 时接收端强制 `grip=false`。
|
||||||
|
- `pose_source`：当前 bridge 使用 `xrobotoolkit`。
|
||||||
|
|
||||||
|
`axis`、`buttons.primary` 和 `buttons.secondary` 会进入 `XrController`；旧 UDP
|
||||||
|
包缺少这些字段时分别回退为 `[0,0]`、`false` 和 `false`。
|
||||||
|
|
||||||
|
接收端发布的消息格式为：
|
||||||
|
|
||||||
|
```text
|
||||||
|
std_msgs/Header header
|
||||||
|
string hand
|
||||||
|
|
||||||
|
bool grip
|
||||||
|
float32 trigger
|
||||||
|
bool primary
|
||||||
|
bool secondary
|
||||||
|
float32[2] axis
|
||||||
|
|
||||||
|
geometry_msgs/Pose pose
|
||||||
|
```
|
||||||
|
|
||||||
|
`udp_controller_receiver` 仍兼容调试用的单手柄包：可以直接发送带 `hand`、`pos`、
|
||||||
|
`quat` 的 JSON object，也可以用 `controllers` list、顶层 `left/right`、
|
||||||
|
`pose.position`、`position`、`p`、`q` 等常见字段。
|
||||||
|
|
||||||
|
## 官方 XRoboToolkit bridge
|
||||||
|
|
||||||
|
如果使用官方 XRoboToolkit APK 和 PC-Service，可以用 `xrobotoolkit_to_udp_bridge` 从本机 ROS Python 环境中的 `xrobotoolkit_sdk` 读取左右手柄数据，再转换成当前 `udp_controller_receiver` 支持的 UDP JSON。
|
||||||
|
|
||||||
|
正式运行时不要同时启动官方 `PXREAClientUnity` / `RobotLinuxDemo` 可视化窗口。`/opt/apps/roboticsservice/run3D.sh` 会启动这个可视化 demo，适合单独确认 PICO 与 PC-Service 已连接；bridge 遥操作链路中只需要 PC-Service。
|
||||||
|
|
||||||
|
运行前只保留一个 UDP 输入源。先清掉重复 bridge、sample sender 和官方 Unity 可视化 demo，再保留或启动 PC-Service：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
pkill -f '[x]robotoolkit_to_udp_bridge'
|
||||||
|
pkill -f '[s]ample_udp_sender'
|
||||||
|
pkill -f '[R]obotLinuxDemo.x86_64'
|
||||||
|
pkill -f '[P]XREAClientUnity'
|
||||||
|
pgrep -af RoboticsServiceProcess || /opt/apps/roboticsservice/runService.sh
|
||||||
|
```
|
||||||
|
|
||||||
|
启动 ROS mock 接收链路：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
另开终端启动 bridge：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source ~/.bashrc
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
|
||||||
|
--host 127.0.0.1 --port 15000 --hz 90
|
||||||
|
```
|
||||||
|
|
||||||
|
bridge 默认对 grip/trigger 做轻量滞回：`grip` 按下阈值 `0.90`、松开阈值 `0.75`；`trigger` 按下阈值 `0.95`、松开阈值 `0.75`。启动日志会打印 PID、UDP endpoint 和阈值，便于确认当前只运行了一个 bridge。
|
||||||
|
|
||||||
|
验证手柄数据是否进入 ROS：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ps -ef | grep -E 'xrobotoolkit_to_udp_bridge|sample_udp_sender|RobotLinuxDemo|PXREAClientUnity' | grep -v grep
|
||||||
|
ros2 topic hz /xr/left_controller
|
||||||
|
ros2 topic hz /xr/right_controller
|
||||||
|
ros2 topic echo /xr/left_controller --field pose.position
|
||||||
|
ros2 topic echo /xr/right_controller --field pose.position
|
||||||
|
ros2 topic echo /xr/left_controller --field grip
|
||||||
|
ros2 topic echo /xr/right_controller --field grip
|
||||||
|
ros2 topic echo /xr/right_controller --field trigger
|
||||||
|
```
|
||||||
|
|
||||||
|
`/xr/left_controller` 和 `/xr/right_controller` 持续刷新、位置随手柄移动变化、`grip` 随握持键切换，即表示官方 XRoboToolkit 数据已经进入当前遥操作输入层。
|
||||||
|
|
||||||
|
## 真机安全验证
|
||||||
|
|
||||||
|
第一次接真机时按这个顺序走：
|
||||||
|
|
||||||
|
1. 确认急停、网络、机械臂工作区和人员位置。
|
||||||
|
2. `launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75`。
|
||||||
|
3. 确认对应 YAML 中 `move_to_initial_pose_on_connect: false` 后单臂启动。
|
||||||
|
4. 手握急停，按住 `grip` 后只做小幅单轴移动。
|
||||||
|
5. 逐个确认上/下、前/后、左/右方向。
|
||||||
|
6. 小角度转动手柄，确认 `/xr_rm/<arm>/target_pose` 姿态和 `/xr_rm/<arm>/cmd_vel.twist.angular` 变化符合预期。
|
||||||
|
7. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
|
||||||
|
8. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
|
||||||
|
9. 左右臂都确认后，再运行 `Dual Arm RealMan Launch`。
|
||||||
|
|
||||||
|
当前项目没有双臂碰撞检测/避障。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
|
||||||
|
|
||||||
|
## 后续优化路线
|
||||||
|
|
||||||
|
为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
|
||||||
|
|
||||||
|
1. 稳定 PICO 数据链路：利用 `seq`、`source_time`、`pose_valid` 做频率、延迟、丢包和追踪状态统计，记录 `/xr/*_controller`、`/xr_rm/*/raw_target_pose`、`/xr_rm/*/target_pose`、`/xr_rm/*/target_clamped`、`/xr_rm/*/current_pose`。
|
||||||
|
2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
|
||||||
|
3. 细化末端执行器：增加夹爪状态反馈、力控比例、安全上限和现场可视化提示。
|
||||||
|
4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
|
||||||
|
5. 从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。
|
||||||
|
|
||||||
|
## 常见问题
|
||||||
|
|
||||||
|
`launcher_ui.py` 提示找不到 `install/setup.bash`：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
source /opt/ros/humble/setup.bash
|
source /opt/ros/humble/setup.bash
|
||||||
colcon build --symlink-install
|
colcon build --symlink-install
|
||||||
colcon test --event-handlers console_direct+
|
source install/setup.bash
|
||||||
colcon test-result --verbose
|
|
||||||
```
|
```
|
||||||
|
|
||||||
涉及遥操作姿态控制时，额外运行：
|
真机模式提示缺少 `Robotic_Arm`：
|
||||||
|
|
||||||
```bash
|
```text
|
||||||
pytest src/xr_rm_teleop/test/test_orientation_control.py
|
未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。
|
||||||
```
|
```
|
||||||
|
|
||||||
真机验证不属于自动测试。默认使用 `use_mock:=true`，未经现场安全确认不要连接或
|
Controller topic 没有数据：
|
||||||
移动机械臂。
|
|
||||||
|
- 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
|
||||||
|
- 确认端口是 `15000`，或 launch 与发送端端口一致。
|
||||||
|
- 用 `sample_udp_sender` 在本机验证接收链路。
|
||||||
|
- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed；SDK
|
||||||
|
读取失败时 bridge 会发送 `pose_valid=false` 的停止包。
|
||||||
|
|
||||||
|
机械臂不动：
|
||||||
|
|
||||||
|
- 确认 `grip=true`。
|
||||||
|
- 确认 `udp_controller_receiver` 终端没有持续 `pose_valid=false` 日志；该字段不会写入 `XrController` 消息，但会让接收端强制停止。
|
||||||
|
- 确认 `/xr_rm/<arm>/raw_target_pose` 与 `/xr_rm/<arm>/target_pose` 是否在变化。
|
||||||
|
- 确认 `/xr_rm/<arm>/target_clamped` 是否持续为 `true`，如果是，目标 TCP 可能被工作空间、圆柱半径或单帧步长限制夹住。
|
||||||
|
- 确认真机 SDK 连接成功，且 RM75 没有报警或急停。
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -1,406 +0,0 @@
|
|||||||
# RM75 双臂 J3 参考角仿真标定实施计划
|
|
||||||
|
|
||||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
|
||||||
|
|
||||||
**Goal:** 使用当前双臂 URDF、Placo QP 和 MuJoCo 运动学模型运行可复现的左右臂 J3 参考角粗扫与细扫，并输出评分、稳定区间和推荐角度。
|
|
||||||
|
|
||||||
**Architecture:** 新增一个仅供离线实验使用的脚本，负责生成 18 条严格六维 TCP 轨迹、建立三类 QP 试验配置、运行候选角度扫描、计算硬门槛与并列评分，并生成 CSV/JSON/Markdown 结果。生产控制器、QP 求解器和 YAML 均不修改；测试只覆盖轨迹、评分和一个真实 Placo/MuJoCo 冒烟评估。
|
|
||||||
|
|
||||||
**Tech Stack:** Python 3.10、NumPy、Placo 0.9.4、MuJoCo 3.10、pytest、ROS2 Humble 工作空间。
|
|
||||||
|
|
||||||
---
|
|
||||||
|
|
||||||
## 文件结构
|
|
||||||
|
|
||||||
- 新增 `xr_rm_teleop/test/j3_reference_calibration.py`：离线轨迹生成、QP/MuJoCo 评估、评分、结果输出和命令行入口。
|
|
||||||
- 新增 `xr_rm_teleop/test/test_j3_reference_calibration.py`：轨迹端点、严格姿态插值、百分位评分、平台选择和真实模型冒烟测试。
|
|
||||||
- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/summary.csv`：候选角度汇总。
|
|
||||||
- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/trajectories.csv`：逐轨迹指标。
|
|
||||||
- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/result.json`：机器可读结果。
|
|
||||||
- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/report.md`：左右臂推荐角度、平台区间、基线对比和最差轨迹。
|
|
||||||
|
|
||||||
### Task 1：用失败测试固定轨迹与评分行为
|
|
||||||
|
|
||||||
**Files:**
|
|
||||||
- Create: `xr_rm_teleop/test/test_j3_reference_calibration.py`
|
|
||||||
- Create: `xr_rm_teleop/test/j3_reference_calibration.py`
|
|
||||||
|
|
||||||
- [ ] **Step 1：写轨迹生成失败测试**
|
|
||||||
|
|
||||||
测试使用以下公开接口：
|
|
||||||
|
|
||||||
```python
|
|
||||||
def build_task_trajectories(
|
|
||||||
initial_world_pose: np.ndarray,
|
|
||||||
control_rate_hz: float = 90.0,
|
|
||||||
max_linear_speed: float = 0.15,
|
|
||||||
max_angular_speed: float = 0.5,
|
|
||||||
) -> list[Trajectory]:
|
|
||||||
...
|
|
||||||
```
|
|
||||||
|
|
||||||
断言：
|
|
||||||
|
|
||||||
```python
|
|
||||||
def test_build_task_trajectories_creates_nine_strict_6d_routes() -> None:
|
|
||||||
initial = np.eye(4)
|
|
||||||
initial[:3, 3] = [0.35, 0.20, 0.10]
|
|
||||||
|
|
||||||
trajectories = build_task_trajectories(initial)
|
|
||||||
|
|
||||||
assert len(trajectories) == 9
|
|
||||||
assert {(route.harvest_y, route.harvest_z) for route in trajectories} == {
|
|
||||||
(y, z)
|
|
||||||
for y in (0.30, 0.40, 0.50)
|
|
||||||
for z in (-0.30, -0.20, -0.10)
|
|
||||||
}
|
|
||||||
for route in trajectories:
|
|
||||||
assert np.allclose(route.poses[0], initial)
|
|
||||||
assert np.allclose(route.poses[-1], initial)
|
|
||||||
basket = route.waypoints[5]
|
|
||||||
assert basket[0, 3] == pytest.approx(initial[0, 3])
|
|
||||||
assert basket[1, 3] == pytest.approx(initial[1, 3])
|
|
||||||
assert basket[2, 3] == pytest.approx(initial[2, 3] - 0.40)
|
|
||||||
assert basket[:3, 2] == pytest.approx([0.0, 0.0, -1.0], abs=1e-6)
|
|
||||||
```
|
|
||||||
|
|
||||||
- [ ] **Step 2：写评分和平台选择失败测试**
|
|
||||||
|
|
||||||
公开接口：
|
|
||||||
|
|
||||||
```python
|
|
||||||
def rank_candidates(rows: list[CandidateMetrics]) -> list[CandidateMetrics]:
|
|
||||||
...
|
|
||||||
|
|
||||||
def choose_stable_platform(
|
|
||||||
ranked: list[CandidateMetrics],
|
|
||||||
scan_step_deg: float,
|
|
||||||
) -> tuple[float, tuple[float, float]]:
|
|
||||||
...
|
|
||||||
```
|
|
||||||
|
|
||||||
测试构造三个硬门槛相同的候选，断言评分严格等于：
|
|
||||||
|
|
||||||
```python
|
|
||||||
score = (
|
|
||||||
0.45 * r_sigma
|
|
||||||
+ 0.20 * r_q4
|
|
||||||
+ 0.20 * r_elbow
|
|
||||||
+ 0.10 * r_smooth
|
|
||||||
+ 0.05 * r_track
|
|
||||||
)
|
|
||||||
```
|
|
||||||
|
|
||||||
并断言连续候选均达到最高分的 98% 时返回平台中点，而不是孤立端点。
|
|
||||||
|
|
||||||
- [ ] **Step 3：运行测试并确认按预期失败**
|
|
||||||
|
|
||||||
Run:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
|
|
||||||
src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
|
|
||||||
```
|
|
||||||
|
|
||||||
Expected: FAIL，原因是 `j3_reference_calibration` 或公开函数尚不存在。
|
|
||||||
|
|
||||||
### Task 2：实现最小轨迹与评分模块
|
|
||||||
|
|
||||||
**Files:**
|
|
||||||
- Create: `xr_rm_teleop/test/j3_reference_calibration.py`
|
|
||||||
- Test: `xr_rm_teleop/test/test_j3_reference_calibration.py`
|
|
||||||
|
|
||||||
- [ ] **Step 1：实现旋转和 SE(3) 插值**
|
|
||||||
|
|
||||||
只使用 NumPy 和标准库：
|
|
||||||
|
|
||||||
```python
|
|
||||||
def rotation_angle(rotation: np.ndarray) -> float:
|
|
||||||
cosine = np.clip((np.trace(rotation) - 1.0) * 0.5, -1.0, 1.0)
|
|
||||||
return float(math.acos(cosine))
|
|
||||||
|
|
||||||
|
|
||||||
def rotation_vector(rotation: np.ndarray) -> np.ndarray:
|
|
||||||
angle = rotation_angle(rotation)
|
|
||||||
if angle <= 1e-12:
|
|
||||||
return np.zeros(3)
|
|
||||||
axis = np.array([
|
|
||||||
rotation[2, 1] - rotation[1, 2],
|
|
||||||
rotation[0, 2] - rotation[2, 0],
|
|
||||||
rotation[1, 0] - rotation[0, 1],
|
|
||||||
]) / (2.0 * math.sin(angle))
|
|
||||||
return axis * angle
|
|
||||||
|
|
||||||
|
|
||||||
def interpolate_pose(start: np.ndarray, end: np.ndarray, count: int) -> list[np.ndarray]:
|
|
||||||
relative = end[:3, :3] @ start[:3, :3].T
|
|
||||||
vector = rotation_vector(relative)
|
|
||||||
return [
|
|
||||||
make_pose(
|
|
||||||
start[:3, 3] + alpha * (end[:3, 3] - start[:3, 3]),
|
|
||||||
so3_exp(alpha * vector) @ start[:3, :3],
|
|
||||||
)
|
|
||||||
for alpha in np.linspace(0.0, 1.0, count + 1)[1:]
|
|
||||||
]
|
|
||||||
```
|
|
||||||
|
|
||||||
`rotation_vector` 对接近 180° 的情况使用特征向量兜底，避免工具朝下转换产生除零。
|
|
||||||
|
|
||||||
- [ ] **Step 2：实现九条本侧完整轨迹**
|
|
||||||
|
|
||||||
定义不可变数据类：
|
|
||||||
|
|
||||||
```python
|
|
||||||
@dataclass(frozen=True)
|
|
||||||
class Trajectory:
|
|
||||||
name: str
|
|
||||||
harvest_y: float
|
|
||||||
harvest_z: float
|
|
||||||
waypoints: tuple[np.ndarray, ...]
|
|
||||||
poses: tuple[np.ndarray, ...]
|
|
||||||
```
|
|
||||||
|
|
||||||
航点固定为：初始、预接近、采摘、预接近、筐上方、筐内、筐上方、初始。每段点数为：
|
|
||||||
|
|
||||||
```python
|
|
||||||
duration = max(
|
|
||||||
translation_distance / max_linear_speed,
|
|
||||||
rotation_distance / max_angular_speed,
|
|
||||||
)
|
|
||||||
steps = max(1, math.ceil(duration * control_rate_hz))
|
|
||||||
```
|
|
||||||
|
|
||||||
- [ ] **Step 3：实现百分位排名和并列评分**
|
|
||||||
|
|
||||||
同值获得同一百分位，单一取值获得 1.0。平滑性和跟踪排名分别定义为：
|
|
||||||
|
|
||||||
```python
|
|
||||||
r_smooth = 0.5 * rank_low(motion_cost) + 0.5 * rank_low(max_joint_speed)
|
|
||||||
r_track = 0.5 * rank_low(max_position_error) + 0.5 * rank_low(max_orientation_error)
|
|
||||||
```
|
|
||||||
|
|
||||||
硬门槛按 `(N_fail, -N_complete)` 字典序先筛选；只有满足位置误差、姿态误差、J4、
|
|
||||||
关节限位、速度和跳变条件的候选进入综合评分。
|
|
||||||
|
|
||||||
- [ ] **Step 4：运行测试确认通过**
|
|
||||||
|
|
||||||
Run:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
|
|
||||||
src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
|
|
||||||
```
|
|
||||||
|
|
||||||
Expected: 轨迹与评分测试 PASS。
|
|
||||||
|
|
||||||
### Task 3：用失败测试固定真实 Placo/MuJoCo 单轨迹评估
|
|
||||||
|
|
||||||
**Files:**
|
|
||||||
- Modify: `xr_rm_teleop/test/test_j3_reference_calibration.py`
|
|
||||||
- Modify: `xr_rm_teleop/test/j3_reference_calibration.py`
|
|
||||||
|
|
||||||
- [ ] **Step 1：写真实模型冒烟失败测试**
|
|
||||||
|
|
||||||
接口：
|
|
||||||
|
|
||||||
```python
|
|
||||||
def evaluate_trajectory(
|
|
||||||
arm: str,
|
|
||||||
trajectory: Trajectory,
|
|
||||||
variant: Variant,
|
|
||||||
urdf_path: Path,
|
|
||||||
initial_joint_degrees: tuple[float, ...],
|
|
||||||
) -> TrajectoryMetrics:
|
|
||||||
...
|
|
||||||
```
|
|
||||||
|
|
||||||
使用左臂从初始 TCP 沿公共 `+Y` 移动 1 mm 的两点轨迹，断言：
|
|
||||||
|
|
||||||
```python
|
|
||||||
assert metrics.cycles == 2
|
|
||||||
assert metrics.failures == 0
|
|
||||||
assert math.isfinite(metrics.min_sigma)
|
|
||||||
assert metrics.min_q4_margin_deg > 0.0
|
|
||||||
assert metrics.max_position_error_m <= 2e-3
|
|
||||||
assert metrics.max_orientation_error_rad <= 5e-3
|
|
||||||
```
|
|
||||||
|
|
||||||
测试还将求得的七关节状态写入 `DualArmKinematicModel` 并断言按名称读回一致。
|
|
||||||
|
|
||||||
- [ ] **Step 2：运行冒烟测试并确认按预期失败**
|
|
||||||
|
|
||||||
Run:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
|
|
||||||
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
|
|
||||||
src/xr_rm_teleop/test/test_j3_reference_calibration.py::test_evaluate_trajectory_uses_real_placo_and_mujoco -q
|
|
||||||
```
|
|
||||||
|
|
||||||
Expected: FAIL，原因是评估器尚未实现。
|
|
||||||
|
|
||||||
- [ ] **Step 3：实现三类 QP 变体**
|
|
||||||
|
|
||||||
```python
|
|
||||||
@dataclass(frozen=True)
|
|
||||||
class Variant:
|
|
||||||
name: str
|
|
||||||
q3_reference_deg: float | None
|
|
||||||
enable_manipulability: bool
|
|
||||||
q4_min_deg: float | None
|
|
||||||
```
|
|
||||||
|
|
||||||
- `original`：三个可选项均关闭；
|
|
||||||
- `manip_j4`：位置可操作度权重 `1e-4`，J4 硬下限 10°；
|
|
||||||
- `q3_<angle>`：在 `manip_j4` 基础上加入 J3 软任务，权重 `1e-5`。
|
|
||||||
|
|
||||||
J4 约束使用 Placo 0.9.4 的 `add_joint_space_half_spaces_constraint(A, b)`，构造
|
|
||||||
`-q4 <= -q4_min`。J3 使用 `add_joints_task()`；位置可操作度使用
|
|
||||||
`add_manipulability_task(tcp_frame, "position", 1.0)`。
|
|
||||||
|
|
||||||
- [ ] **Step 4：实现逐周期评估与失败保持**
|
|
||||||
|
|
||||||
每个目标点前将上一有效关节状态同步给 Placo。求解失败时：
|
|
||||||
|
|
||||||
```python
|
|
||||||
failures += 1
|
|
||||||
solver.update_joint_state(last_valid_joints)
|
|
||||||
current_joints = last_valid_joints.copy()
|
|
||||||
```
|
|
||||||
|
|
||||||
不把失败后的 Placo 内部迭代状态带到下一周期。成功状态写入 MuJoCo，并记录六维
|
|
||||||
雅可比最小奇异值、J4 余量、肘部外展量、TCP 误差、关节速度和运动代价。
|
|
||||||
|
|
||||||
- [ ] **Step 5：运行全部标定脚本测试**
|
|
||||||
|
|
||||||
Run:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
|
|
||||||
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
|
|
||||||
src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
|
|
||||||
```
|
|
||||||
|
|
||||||
Expected: 全部 PASS。
|
|
||||||
|
|
||||||
### Task 4：运行粗扫、细扫并生成结果
|
|
||||||
|
|
||||||
**Files:**
|
|
||||||
- Modify: `xr_rm_teleop/test/j3_reference_calibration.py`
|
|
||||||
- Generate: `docs/superpowers/results/2026-08-12-rm75-j3-calibration/*`
|
|
||||||
|
|
||||||
- [ ] **Step 1：实现命令行和结果输出**
|
|
||||||
|
|
||||||
命令行：
|
|
||||||
|
|
||||||
```bash
|
|
||||||
python j3_reference_calibration.py \
|
|
||||||
--urdf <path> \
|
|
||||||
--config <dual_arm_rm75.yaml> \
|
|
||||||
--output-dir <directory> \
|
|
||||||
--phase coarse|fine|all
|
|
||||||
```
|
|
||||||
|
|
||||||
粗扫结束后对每侧选择最高分候选，在其 ±10°、原扫描边界内以 2° 细扫。CSV 使用
|
|
||||||
`csv.DictWriter`，JSON 使用 `json.dump`，Markdown 报告由同一汇总对象生成，不新增依赖。
|
|
||||||
|
|
||||||
- [ ] **Step 2：运行完整仿真标定**
|
|
||||||
|
|
||||||
Run:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd /home/robot/WS_xr
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
|
|
||||||
/home/robot/miniconda3/envs/xr/bin/python \
|
|
||||||
src/xr_rm_teleop/test/j3_reference_calibration.py \
|
|
||||||
--urdf src/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf \
|
|
||||||
--config src/xr_rm_bringup/config/dual_arm_rm75.yaml \
|
|
||||||
--output-dir src/docs/superpowers/results/2026-08-12-rm75-j3-calibration \
|
|
||||||
--phase all
|
|
||||||
```
|
|
||||||
|
|
||||||
Expected: 左右臂粗扫和细扫完成；输出两个基线、全部候选、推荐角度和平台区间。
|
|
||||||
|
|
||||||
- [ ] **Step 3：检查结果完整性**
|
|
||||||
|
|
||||||
Run:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
/home/robot/miniconda3/envs/xr/bin/python - <<'PY'
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
path = Path('src/docs/superpowers/results/2026-08-12-rm75-j3-calibration/result.json')
|
|
||||||
data = json.loads(path.read_text(encoding='utf-8'))
|
|
||||||
assert set(data['arms']) == {'left', 'right'}
|
|
||||||
for arm in data['arms'].values():
|
|
||||||
assert arm['coarse_candidates']
|
|
||||||
assert arm['fine_candidates']
|
|
||||||
assert arm['recommended_reference_deg'] is not None
|
|
||||||
assert len(arm['stable_interval_deg']) == 2
|
|
||||||
print('result integrity: OK')
|
|
||||||
PY
|
|
||||||
```
|
|
||||||
|
|
||||||
Expected: `result integrity: OK`。
|
|
||||||
|
|
||||||
### Task 5：工作空间验证与结果复核
|
|
||||||
|
|
||||||
**Files:**
|
|
||||||
- Verify only.
|
|
||||||
|
|
||||||
- [ ] **Step 1：运行新增测试和相关现有测试**
|
|
||||||
|
|
||||||
Run:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd /home/robot/WS_xr
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
|
|
||||||
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
|
|
||||||
src/xr_rm_teleop/test/test_j3_reference_calibration.py \
|
|
||||||
src/xr_rm_teleop/test/test_placo_transforms.py \
|
|
||||||
src/xr_rm_mujoco/test/test_dual_arm_simulator.py -q
|
|
||||||
```
|
|
||||||
|
|
||||||
Expected: 全部 PASS。
|
|
||||||
|
|
||||||
- [ ] **Step 2：按项目规则构建工作空间**
|
|
||||||
|
|
||||||
Run:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd /home/robot/WS_xr
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
colcon build --symlink-install
|
|
||||||
```
|
|
||||||
|
|
||||||
Expected: 相关 ROS2 包构建成功。
|
|
||||||
|
|
||||||
- [ ] **Step 3：运行姿态控制回归测试**
|
|
||||||
|
|
||||||
Run:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd /home/robot/WS_xr
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
pytest src/xr_rm_teleop/test/test_orientation_control.py -q
|
|
||||||
```
|
|
||||||
|
|
||||||
Expected: 全部 PASS。
|
|
||||||
|
|
||||||
- [ ] **Step 4：人工复核结果报告**
|
|
||||||
|
|
||||||
确认：
|
|
||||||
|
|
||||||
- 每侧确有 9 条完整轨迹；
|
|
||||||
- `original`、`manip_j4` 和 J3 候选均存在；
|
|
||||||
- 推荐角来自硬门槛通过集合；
|
|
||||||
- 平台选择符合 98% 规则；
|
|
||||||
- 报告明确列出失败轨迹，且没有把失败更多的候选排到前面；
|
|
||||||
- 没有修改生产控制器和 YAML。
|
|
||||||
@@ -1,372 +0,0 @@
|
|||||||
# 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`、合并分支或连接真机。
|
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,657 @@
|
|||||||
|
# 右臂番茄采摘 ACT 数据采集适配设计
|
||||||
|
|
||||||
|
## 背景与目标
|
||||||
|
|
||||||
|
当前项目已经通过 ROS2 Humble、PICO 手柄、Placo QP 和 RealMan Python API2
|
||||||
|
完成 RM75 遥操作。右臂遥操节点以名义 `90 Hz` 读取实际关节反馈，根据 PICO
|
||||||
|
相对位姿生成 TCP 目标，经过工作空间限制、QP、关节速度与加速度限制后，通过
|
||||||
|
`rm_movej_canfd` 下发最终关节目标。
|
||||||
|
|
||||||
|
本次变更在不重写现有遥操链路的前提下，增加一个独立的 ALOHA/ACT 风格数据采集
|
||||||
|
节点。第一阶段只采集右臂番茄采摘任务，每个 episode 覆盖从初始位姿出发、抓取
|
||||||
|
番茄、搬运至 RM75 下方收集篮并释放番茄的完整过程。
|
||||||
|
|
||||||
|
核心目标如下：
|
||||||
|
|
||||||
|
- 以右臂 7 个实际关节角和夹爪逻辑状态作为 `observations/qpos`；
|
||||||
|
- 以实际成功下发或零阶保持的 7 个最终关节目标和夹爪目标作为 `action`；
|
||||||
|
- 同步采集一台全局 D455 和一台右腕 D405 的 RGB 图像；
|
||||||
|
- 以 `30 Hz` 形成同周期因果对齐的数据；
|
||||||
|
- 每个完整任务流式保存为一个 ALOHA/ACT 核心结构兼容的 HDF5 文件；
|
||||||
|
- 支持手柄开始、结束、丢弃、拒绝、质量检查、崩溃恢复和编号防覆盖；
|
||||||
|
- 保留足够的 PICO、TCP、QP、夹爪和时间戳调试数据，但不让采集节点进入机器人
|
||||||
|
控制链路。
|
||||||
|
|
||||||
|
参考实现为 ALOHA 官方仓库中的
|
||||||
|
[`record_episodes.py`](https://github.com/tonyzhaozh/aloha/blob/master/aloha_scripts/record_episodes.py)。
|
||||||
|
官方双臂数据使用 14 维状态和动作；本项目第一阶段采用右臂 `7+1=8` 维，因此只
|
||||||
|
保证 HDF5 核心组织方式兼容，不声称官方旧训练加载器可以不修改直接训练。
|
||||||
|
|
||||||
|
## 非目标
|
||||||
|
|
||||||
|
第一阶段明确不实现：
|
||||||
|
|
||||||
|
- 左臂或双臂 ACT 采集；
|
||||||
|
- ACT 训练代码、数据加载器、策略部署或自动完成判定；
|
||||||
|
- 深度图、红外图、点云、图像压缩或 ROS 图像话题；
|
||||||
|
- rosbag、中间格式、离线转换工具、GUI、声音或手柄震动反馈；
|
||||||
|
- 让 ACT 学习 A 键触发的回初始位姿运动；
|
||||||
|
- 修改现有工作空间限制、圆柱限制、速度限制、指令超时、安全停止或
|
||||||
|
`move_to_initial_pose_on_connect` 默认值；
|
||||||
|
- 新建重复的 ROS2 包或第二个 RealMan 连接。
|
||||||
|
|
||||||
|
后续训练时建议将 ACT `chunk_size` 设为 `60`，对应约 2 秒动作长度；该参数属于
|
||||||
|
训练配置，不写死在采集逻辑中。
|
||||||
|
|
||||||
|
## 现有控制链路与关键约束
|
||||||
|
|
||||||
|
现有 `single_arm_velocity_teleop` 在每个控制周期内依次完成：
|
||||||
|
|
||||||
|
```text
|
||||||
|
读取最新 RM75 反馈
|
||||||
|
→ 同步 Placo 状态
|
||||||
|
→ 读取 PICO 状态并生成 TCP 目标
|
||||||
|
→ 工作空间、位姿步长与速度限制
|
||||||
|
→ Placo QP
|
||||||
|
→ 关节速度与加速度限制
|
||||||
|
→ rm_movej_canfd 下发
|
||||||
|
→ 发布 joint_target 调试话题
|
||||||
|
```
|
||||||
|
|
||||||
|
当前 `joint_states`、TCP 调试话题和 `joint_target` 分别发布并各自取时间戳。如果
|
||||||
|
采集节点仅订阅这些分散话题，即使时间接近，也可能把第 100 个控制周期的
|
||||||
|
`q_actual` 与第 99 个控制周期的 `q_target` 拼在一起。这里的“第 100 个周期”是
|
||||||
|
控制序号，不是 `100 Hz`；现有控制频率仍是名义 `90 Hz`。
|
||||||
|
|
||||||
|
此外，A 键回位调用的是一次阻塞式 `rm_movej(initial_joint_pose)`。轨迹由 RM75
|
||||||
|
控制器内部生成，项目不能获得每个控制周期的中间目标，也不经过当前 QP 和
|
||||||
|
`rm_movej_canfd` 链路，因此不能与遥操动作混用同一种标签语义。
|
||||||
|
|
||||||
|
## 总体架构
|
||||||
|
|
||||||
|
采用一个自定义原子控制采样消息和一个独立 ACT 采集节点：
|
||||||
|
|
||||||
|
```text
|
||||||
|
PICO 输入 + RM75 反馈
|
||||||
|
↓
|
||||||
|
single_arm_velocity_teleop（90 Hz）
|
||||||
|
↓
|
||||||
|
QP → 关节限速/限加速度 → RM75 下发
|
||||||
|
↓
|
||||||
|
ActControlSample（同周期、同时间戳、同序号）
|
||||||
|
↓
|
||||||
|
act_episode_recorder（每 3 个控制周期取 1 个）
|
||||||
|
├──────────────┐
|
||||||
|
↓ ↓
|
||||||
|
全局 D455 RGB 右腕 D405 RGB
|
||||||
|
└──────┬───────┘
|
||||||
|
↓
|
||||||
|
30 Hz 流式写入临时 HDF5
|
||||||
|
↓
|
||||||
|
裁剪 → 质量检查 → 保存/拒绝/丢弃
|
||||||
|
```
|
||||||
|
|
||||||
|
“原子”表示消息是一个逻辑上不可拆分的控制周期快照。订阅者要么收到该周期完整的
|
||||||
|
反馈、求解结果、最终动作和状态，要么该周期整体缺失；不会自行拼接多个异步话题。
|
||||||
|
|
||||||
|
职责边界如下：
|
||||||
|
|
||||||
|
- 遥操节点继续唯一负责 RM75 连接、反馈、QP、限位、动作下发和安全停止；
|
||||||
|
- 遥操节点只增加原子消息发布和夹爪逻辑状态记录，不读取相机、不写 HDF5；
|
||||||
|
- 采集节点只订阅控制/PICO 数据、独占两台相机并写文件，不连接或控制 RM75；
|
||||||
|
- 采集节点异常、退出或写盘过慢不得阻塞遥操发布或改变机器人动作；
|
||||||
|
- 相机由采集节点通过 `pyrealsense2` 直接打开，不再发布和重新订阅 ROS 图像。
|
||||||
|
|
||||||
|
原子消息使用本机低延迟、非阻塞的 best-effort QoS。采集节点检测任何需要保留的
|
||||||
|
控制周期丢失并拒绝 episode，而不是让 DDS 反压影响遥操控制。
|
||||||
|
|
||||||
|
## 原子控制采样消息
|
||||||
|
|
||||||
|
在 `xr_rm_interfaces` 中新增 `ActControlSample.msg`，发布话题为：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/xr_rm/right_rm75/act_control_sample
|
||||||
|
```
|
||||||
|
|
||||||
|
消息至少表达以下内容：
|
||||||
|
|
||||||
|
| 类别 | 字段语义 |
|
||||||
|
|---|---|
|
||||||
|
| 周期标识 | ROS header、`control_seq`、控制周期单调时间戳 |
|
||||||
|
| 关节反馈 | `q_actual[7]`、当前 URDF 关节上下限、反馈接收单调时间戳、反馈年龄、反馈有效状态 |
|
||||||
|
| QP | QP 原始输出或失败时的保持目标 `q_qp_raw[7]`、是否尝试、成功状态、耗时 |
|
||||||
|
| 最终动作 | 经关节限速后的 `q_target[7]`、动作时间戳、是否当前周期成功下发 |
|
||||||
|
| TCP | 当前 TCP、PICO 映射前的原始目标 TCP、最终受限目标 TCP、命令速度 |
|
||||||
|
| PICO | 当前右手位姿、Grip、Trigger、A、B 和摇杆值 |
|
||||||
|
| 夹爪 | 请求目标、已确认逻辑状态、命令是否处理中、命令是否失败 |
|
||||||
|
| 控制状态 | `teleop_active`、`action_valid`、QP 回退、目标限位和控制故障状态 |
|
||||||
|
|
||||||
|
数值型关节字段在 ROS 消息中保持双精度，写入 HDF5 核心数据时显式转换成
|
||||||
|
`float32`。位姿使用位置加四元数，不保存完整 Placo 对象、Hessian、约束矩阵或
|
||||||
|
其他大体积求解器内部状态。
|
||||||
|
|
||||||
|
消息在每个实际执行的 90 Hz 控制回调中发布，包括 Grip 松开和安全停止状态：
|
||||||
|
|
||||||
|
- 当前周期成功发送动作时，`command_sent=true`；
|
||||||
|
- 当前周期没有发送，但此前存在成功目标时，`q_target` 零阶保持上一个成功目标，
|
||||||
|
`command_sent=false`、`action_valid=true`；
|
||||||
|
- 尚未形成任何有效目标或当前动作发送失败时，`action_valid=false`；
|
||||||
|
- QP 失败但成功重发上次有效目标时，`qp_success=false`、`action_valid=true`；
|
||||||
|
- 发送失败必须发布失败状态，并由采集节点拒绝当前 episode。
|
||||||
|
|
||||||
|
相机数据不放进该消息。相机时间戳和帧号由采集节点在同一主机的单调时钟域中补充。
|
||||||
|
|
||||||
|
## 夹爪数据语义
|
||||||
|
|
||||||
|
右臂使用 Modbus 电动夹爪。由于不同番茄尺寸会导致实际停止开度不同，而本项目只
|
||||||
|
关心抓取意图，第一阶段不读取或估算实际开度。
|
||||||
|
|
||||||
|
统一约定：
|
||||||
|
|
||||||
|
```text
|
||||||
|
0 = closed
|
||||||
|
1 = open
|
||||||
|
```
|
||||||
|
|
||||||
|
两类状态必须分开：
|
||||||
|
|
||||||
|
- `action[7]` 是目标状态，在 Trigger 产生开合请求的控制周期立即改变；
|
||||||
|
- `qpos[7]` 是已确认逻辑状态，只有 Modbus 命令正常返回后才改变；
|
||||||
|
- 命令失败时 `qpos[7]` 保持原值，并拒绝当前 episode；
|
||||||
|
- 采集前右臂夹爪必须成功初始化为完全打开 `1.0`，之后才允许把初始
|
||||||
|
`qpos[7]` 设为 `1`；
|
||||||
|
- 不再使用原先含义不明确的 `0.75 → 0.15` 初始化序列。
|
||||||
|
|
||||||
|
结束一个有效番茄采摘 episode 前，操作者应先请求打开夹爪，等待日志/状态确认
|
||||||
|
逻辑状态已经变为 `open`，再松开 Grip 并按 B。结束时夹爪命令仍在执行，保存状态
|
||||||
|
最多等待 3 秒；失败或超时则拒绝。最终裁剪后的数据若没有包含已确认的打开状态，
|
||||||
|
同样拒绝，不能把保存后的成功状态回填到更早样本中。
|
||||||
|
|
||||||
|
## 相机配置与采集
|
||||||
|
|
||||||
|
第一阶段固定使用两台已确定序列号的 RealSense：
|
||||||
|
|
||||||
|
| ACT 名称 | 型号与位置 | 序列号 |
|
||||||
|
|---|---|---|
|
||||||
|
| `cam_high` | 全局 D455 | `234222303366` |
|
||||||
|
| `cam_right_wrist` | 右臂腕部 D405 | `412622272532` |
|
||||||
|
|
||||||
|
两路图像参数统一为：
|
||||||
|
|
||||||
|
```text
|
||||||
|
分辨率：640 × 480
|
||||||
|
帧率：30 FPS
|
||||||
|
格式：RGB uint8
|
||||||
|
HDF5 形状：(T, 480, 640, 3)
|
||||||
|
```
|
||||||
|
|
||||||
|
不采集深度、红外和点云，不使用 JPEG 压缩。采集线程直接请求 RealSense RGB8，
|
||||||
|
避免为颜色通道转换引入 OpenCV 依赖。
|
||||||
|
|
||||||
|
每台相机使用独立采集线程和一个很小的 `deque` 帧缓冲。每帧保存：
|
||||||
|
|
||||||
|
- RealSense 帧号；
|
||||||
|
- RealSense 硬件时间戳；
|
||||||
|
- `wait_for_frames` 返回后立即读取的主机单调时间戳；
|
||||||
|
- RGB 数组。
|
||||||
|
|
||||||
|
两台设备的硬件时钟不能默认视为同一时钟域，因此正式对齐只使用同一主机的单调
|
||||||
|
时钟；硬件时间戳只用于发现设备重启、帧号跳变和采集异常。
|
||||||
|
|
||||||
|
采集节点独占相机。指定设备缺失、型号/序列号不匹配、流配置失败或已经被其他
|
||||||
|
进程占用时，预检失败并停留在 `IDLE`，不自动替换成其他相机。
|
||||||
|
|
||||||
|
## 30 Hz 采样与因果对齐
|
||||||
|
|
||||||
|
正式采样不使用独立的 30 Hz ROS 定时器。采集节点在首次有效 Grip 控制周期记录
|
||||||
|
`sample_origin_seq`，随后只选择：
|
||||||
|
|
||||||
|
```text
|
||||||
|
(control_seq - sample_origin_seq) % 3 == 0
|
||||||
|
```
|
||||||
|
|
||||||
|
因此 90 Hz 控制消息按 `0、3、6、9...` 的相对序号形成名义 30 Hz 数据，同时保证
|
||||||
|
第一个正式样本就是首次有效动作，而不是等待一个全局取模相位。
|
||||||
|
|
||||||
|
每个样本的定义为：
|
||||||
|
|
||||||
|
```text
|
||||||
|
observation[t]
|
||||||
|
= 当前控制周期开始时读取的 q_actual
|
||||||
|
+ 对每台相机选择主机时间戳不晚于该控制周期的最新帧
|
||||||
|
|
||||||
|
action[t]
|
||||||
|
= 同一控制周期经 QP、关节限速后成功发送的 q_target
|
||||||
|
或 Grip 暂停/QP 回退时明确定义的上次成功目标
|
||||||
|
```
|
||||||
|
|
||||||
|
采集节点收到消息时，相机缓冲中可能已经存在晚于控制周期的帧，因此不能简单取
|
||||||
|
“回调时最新帧”，必须按 `host_monotonic_ns <= control_monotonic_ns` 选择最近帧。
|
||||||
|
不存在满足条件且年龄不超过 50 ms 的帧时，当前 episode 拒绝。
|
||||||
|
|
||||||
|
不采用官方旧加载器中的 `action[t-1]` 补丁。HDF5 根属性写入：
|
||||||
|
|
||||||
|
```text
|
||||||
|
action_alignment = "same_step_causal"
|
||||||
|
```
|
||||||
|
|
||||||
|
控制、反馈、动作和图像源时间戳全部保存在 `/debug`，未来只有在真实延迟测量证明
|
||||||
|
存在稳定偏移时，才在训练加载器中调整；原始 HDF5 不进行不可逆移位。
|
||||||
|
|
||||||
|
## Episode 边界与手柄状态机
|
||||||
|
|
||||||
|
### 按键映射
|
||||||
|
|
||||||
|
- 右手 B，即右手 `secondary` 单击：开始准备或结束保存；
|
||||||
|
- 左手 Y，即左手 `secondary` 长按 1 秒：丢弃当前准备/录制；
|
||||||
|
- 右手 A，即右手 `primary`：继续保持现有右臂回初始位姿功能；
|
||||||
|
- Grip：继续只控制遥操离合，不作为“只在按下时才记录”的采集开关。
|
||||||
|
|
||||||
|
右手 B 在右手 Grip 按下时始终忽略，避免运动中误触开始或结束。左手 Y 仅在
|
||||||
|
`ARMED` 或 `RECORDING` 中长按有效，在 `IDLE` 中无作用，且永远不删除上一个已经
|
||||||
|
保存的 episode。
|
||||||
|
|
||||||
|
### 状态机
|
||||||
|
|
||||||
|
```text
|
||||||
|
IDLE
|
||||||
|
└─ Grip 松开时单击右手 B
|
||||||
|
├─ 预检失败 → IDLE
|
||||||
|
└─ 预检通过 → ARMED
|
||||||
|
|
||||||
|
ARMED
|
||||||
|
├─ 第一次 Grip 有效动作 → RECORDING
|
||||||
|
├─ 再次单击右手 B → 取消 → IDLE
|
||||||
|
├─ 长按左手 Y 1 秒 → DISCARDED → IDLE
|
||||||
|
└─ 按右手 A → 取消 → IDLE
|
||||||
|
|
||||||
|
RECORDING
|
||||||
|
├─ Grip 松开后单击右手 B → SAVING
|
||||||
|
├─ 长按左手 Y 1 秒 → DISCARDED → IDLE
|
||||||
|
├─ 按右手 A → REJECTED → IDLE
|
||||||
|
└─ 硬质量故障/60 秒上限/Ctrl+C → REJECTED → IDLE
|
||||||
|
|
||||||
|
SAVING
|
||||||
|
├─ 质量检查通过 → SAVED → IDLE
|
||||||
|
└─ 质量检查失败 → REJECTED → IDLE
|
||||||
|
```
|
||||||
|
|
||||||
|
`IDLE`、`ARMED`、`RECORDING`、`SAVING` 是运行状态；`SAVED`、`DISCARDED`、
|
||||||
|
`REJECTED` 是短暂结果状态，发布一次结果并输出日志后回到 `IDLE`。`SAVING`
|
||||||
|
期间忽略 B/Y 录制按键，A 键仍属于原有遥操逻辑，但不会再进入已经结束的数据。
|
||||||
|
|
||||||
|
状态通过 `std_msgs/msg/String` 话题 `/act/recording_status` 和终端日志报告，不增加
|
||||||
|
新状态消息、声音或震动接口。
|
||||||
|
|
||||||
|
### 连续记录与 Grip 暂停
|
||||||
|
|
||||||
|
正式时间轴从 `ARMED` 后 Grip 按下且第一次
|
||||||
|
`action_valid=true、command_sent=true` 的控制周期开始。Grip 刚按下的建基准周期
|
||||||
|
尚未向 RM75 发送新的 CANFD 目标，因此不作为第一个训练样本。
|
||||||
|
录制过程中临时松开 Grip 时仍以 30 Hz 保存图像和 `qpos`，`action` 零阶保持上次
|
||||||
|
成功目标，并记录 `teleop_active=false`：
|
||||||
|
|
||||||
|
- 松开后重新按 Grip：暂停区间保留，继续同一个 episode；
|
||||||
|
- 最后一次松开后按 B：将该次松开至 B 之间的纯操作等待数据裁掉，episode 结束在
|
||||||
|
最后一次 Grip 松开附近；
|
||||||
|
- 夹爪打开确认必须已经包含在裁剪终点之前，否则拒绝该 episode。
|
||||||
|
|
||||||
|
只在 Grip 按下时保存数据会丢失接近任务开始、暂停恢复和完整视觉上下文，因此不
|
||||||
|
采用该方案。
|
||||||
|
|
||||||
|
### Episode 是否包含 A 键回位
|
||||||
|
|
||||||
|
一个正式 episode 只包含：
|
||||||
|
|
||||||
|
```text
|
||||||
|
初始位姿、夹爪打开
|
||||||
|
→ 接近番茄
|
||||||
|
→ 闭合夹爪
|
||||||
|
→ 搬运至收集篮
|
||||||
|
→ 打开夹爪并确认成功
|
||||||
|
→ 松开 Grip
|
||||||
|
→ 按 B 结束
|
||||||
|
```
|
||||||
|
|
||||||
|
A 键的 `rm_movej(initial_joint_pose)` 必须在成功结束 episode 后执行，不写进
|
||||||
|
episode。这样 ACT 始终学习同一种逐周期 `q_target` 动作语义。未来实时推理若要
|
||||||
|
连续采摘，应由上层状态机执行：
|
||||||
|
|
||||||
|
```text
|
||||||
|
ACT 完成一次采摘 → 完成判定 → 固定 rm_movej 复位 → 下一次 ACT 采摘
|
||||||
|
```
|
||||||
|
|
||||||
|
若在 `RECORDING` 中误按 A，当前文件转入拒绝目录，原因写为：
|
||||||
|
|
||||||
|
```text
|
||||||
|
initial_pose_command_during_episode
|
||||||
|
```
|
||||||
|
|
||||||
|
拒绝数据不会拦截 A 键原有回位动作，也不会额外控制机器人。
|
||||||
|
|
||||||
|
## HDF5 核心结构
|
||||||
|
|
||||||
|
数据根目录和任务目录固定为：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/home/robot/ACT_Data
|
||||||
|
/home/robot/ACT_Data/tomato_pick
|
||||||
|
```
|
||||||
|
|
||||||
|
正式文件核心结构：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/observations/qpos float32 (T, 8)
|
||||||
|
/observations/images/cam_high uint8 (T, 480, 640, 3)
|
||||||
|
/observations/images/cam_right_wrist uint8 (T, 480, 640, 3)
|
||||||
|
/action float32 (T, 8)
|
||||||
|
/debug/...
|
||||||
|
```
|
||||||
|
|
||||||
|
`T` 是该次任务的实际样本数，不要求所有 episode 等长，不进行文件内 padding。
|
||||||
|
最短有效 episode 为 `60` 个样本，即 2 秒；最长为 `1800` 个样本，即 60 秒。
|
||||||
|
|
||||||
|
8 维字段顺序固定为：
|
||||||
|
|
||||||
|
```text
|
||||||
|
qpos[0:7] = RM75 实际反馈关节角，单位 rad
|
||||||
|
qpos[7] = 已确认夹爪逻辑状态，0 closed、1 open
|
||||||
|
|
||||||
|
action[0:7] = 最终成功下发或明确定义为保持的关节目标，单位 rad
|
||||||
|
action[7] = 夹爪请求目标，0 closed、1 open
|
||||||
|
```
|
||||||
|
|
||||||
|
根属性至少包括：
|
||||||
|
|
||||||
|
| 属性 | 值或语义 |
|
||||||
|
|---|---|
|
||||||
|
| `sim` | `false`，与 ALOHA 真实数据约定一致 |
|
||||||
|
| `task_name` | `tomato_pick` |
|
||||||
|
| `sample_rate_hz` | `30` |
|
||||||
|
| `action_alignment` | `same_step_causal` |
|
||||||
|
| `arm` | `right_rm75` |
|
||||||
|
| `episode_status` | `saved` 或 `rejected` |
|
||||||
|
| `camera_high_serial` | `234222303366` |
|
||||||
|
| `camera_right_wrist_serial` | `412622272532` |
|
||||||
|
| `joint_names` | 7 个 RM75 关节名和 `gripper` 的固定顺序 |
|
||||||
|
| `joint_lower_limits` | 来自当前 Placo/URDF 的 7 关节下限 |
|
||||||
|
| `joint_upper_limits` | 来自当前 Placo/URDF 的 7 关节上限 |
|
||||||
|
| `reject_reason` | 仅拒绝文件存在 |
|
||||||
|
| `interrupted` | 正常文件为 `false`，Ctrl+C/异常恢复为 `true` |
|
||||||
|
|
||||||
|
图像不压缩，每帧使用一个 HDF5 chunk；数值数据使用可扩展的一维时间轴并分块
|
||||||
|
写入。按两路 `640×480×3×30` 计算，图像数据约为 3.3 GB/分钟，因此不能把完整
|
||||||
|
episode 先缓存到内存再一次性保存。
|
||||||
|
|
||||||
|
不创建 `/observations/qvel`、`/observations/effort`、压缩标记或 `compress_len`；
|
||||||
|
也不使用零值、有限差分或其他伪数据填充缺失字段。后续训练加载器按存在的核心
|
||||||
|
字段读取。
|
||||||
|
|
||||||
|
## Debug 结构
|
||||||
|
|
||||||
|
自定义消息是运行时传输载体，进程退出后不会保留；HDF5 `/debug` 是永久诊断记录。
|
||||||
|
ACT 训练默认不读取该组。
|
||||||
|
|
||||||
|
建议使用以下精简结构，布尔状态以 `uint8` 保存：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/debug/timestamps/control_monotonic_ns int64 (T,)
|
||||||
|
/debug/timestamps/feedback_monotonic_ns int64 (T,)
|
||||||
|
/debug/timestamps/action_monotonic_ns int64 (T,)
|
||||||
|
/debug/timestamps/cam_high_host_monotonic_ns int64 (T,)
|
||||||
|
/debug/timestamps/cam_wrist_host_monotonic_ns int64 (T,)
|
||||||
|
/debug/timestamps/cam_high_hardware_ms float64 (T,)
|
||||||
|
/debug/timestamps/cam_wrist_hardware_ms float64 (T,)
|
||||||
|
/debug/timestamps/cam_high_age_ms float32 (T,)
|
||||||
|
/debug/timestamps/cam_wrist_age_ms float32 (T,)
|
||||||
|
/debug/timestamps/inter_camera_skew_ms float32 (T,)
|
||||||
|
|
||||||
|
/debug/cameras/cam_high_frame_number uint64 (T,)
|
||||||
|
/debug/cameras/cam_wrist_frame_number uint64 (T,)
|
||||||
|
|
||||||
|
/debug/control/control_seq uint64 (T,)
|
||||||
|
/debug/control/teleop_active uint8 (T,)
|
||||||
|
/debug/control/action_valid uint8 (T,)
|
||||||
|
/debug/control/command_sent uint8 (T,)
|
||||||
|
/debug/control/target_clamped uint8 (T,)
|
||||||
|
/debug/control/control_fault uint8 (T,)
|
||||||
|
|
||||||
|
/debug/qp/raw_target float32 (T, 7)
|
||||||
|
/debug/qp/attempted uint8 (T,)
|
||||||
|
/debug/qp/success uint8 (T,)
|
||||||
|
/debug/qp/duration_ms float32 (T,)
|
||||||
|
|
||||||
|
/debug/tcp/current_pose float32 (T, 7)
|
||||||
|
/debug/tcp/raw_target_pose float32 (T, 7)
|
||||||
|
/debug/tcp/final_target_pose float32 (T, 7)
|
||||||
|
/debug/tcp/command_velocity float32 (T, 6)
|
||||||
|
|
||||||
|
/debug/pico/right_pose float32 (T, 7)
|
||||||
|
/debug/pico/right_inputs float32 (T, 6)
|
||||||
|
/debug/pico/left_secondary uint8 (T,)
|
||||||
|
|
||||||
|
/debug/gripper/target_open uint8 (T,)
|
||||||
|
/debug/gripper/state_open uint8 (T,)
|
||||||
|
/debug/gripper/command_pending uint8 (T,)
|
||||||
|
/debug/gripper/command_failed uint8 (T,)
|
||||||
|
```
|
||||||
|
|
||||||
|
位姿顺序统一为 `[x, y, z, qx, qy, qz, qw]`，TCP 速度顺序统一为
|
||||||
|
`[vx, vy, vz, wx, wy, wz]`。`right_inputs` 顺序在文件属性中写明。当前周期没有发送
|
||||||
|
动作时，`action_monotonic_ns=-1`，并以 `command_sent=false` 消除歧义。
|
||||||
|
|
||||||
|
episode 根属性额外保存 QP 失败次数、失败占比、最长连续失败次数、目标限位次数、
|
||||||
|
相机帧率、丢帧率和最大时间偏差等汇总指标。
|
||||||
|
|
||||||
|
## 数据质量规则
|
||||||
|
|
||||||
|
### 开始前预检
|
||||||
|
|
||||||
|
Grip 松开时单击 B 后，采集节点检查：
|
||||||
|
|
||||||
|
- 输出目录存在或可以创建且可写；
|
||||||
|
- 可用空间不少于 4 GiB，约为 60 秒原始图像估算值的 1.2 倍；
|
||||||
|
- 两台指定相机均在线、已经连续预热 5 秒且当前帧率合格；
|
||||||
|
- 最近 `q_actual` 合法，反馈年龄不超过 50 ms，RM75 无掉使能或控制故障；
|
||||||
|
- 右臂夹爪初始化打开命令已经成功；
|
||||||
|
- 左右 PICO 话题均在现有手柄超时范围内保持新鲜；
|
||||||
|
- 没有第二个采集进程持有任务目录锁或相机设备；
|
||||||
|
- 启动组合是 `arm:=right use_mock:=false record_act:=true`。
|
||||||
|
|
||||||
|
任一预检失败时输出明确原因并停留在 `IDLE`，不生成空文件，也不改变机器人状态。
|
||||||
|
|
||||||
|
### 硬拒绝条件
|
||||||
|
|
||||||
|
以下任一情况使当前 episode 进入 `REJECTED`：
|
||||||
|
|
||||||
|
- 样本少于 60 或达到 60 秒上限；
|
||||||
|
- 控制周期序列缺失、有效平均采样率低于 27 Hz 或相邻样本间隔超过 100 ms；
|
||||||
|
- `qpos/action` 不是 `(T,8)`、包含 NaN/Inf、违反配置关节限制或夹爪值不是
|
||||||
|
`0/1`；
|
||||||
|
- `q_actual` 年龄超过 50 ms，反馈超时、掉使能或出现控制故障；
|
||||||
|
- 最终关节动作发送失败，或消息表示的反馈和动作不属于同一控制周期；
|
||||||
|
- 夹爪命令失败、超时，或最终裁剪数据没有包含已确认的打开状态；
|
||||||
|
- 任一路相机平均帧率低于 27 FPS；
|
||||||
|
- 任一路相机硬件帧号丢失率超过 1%，或采样后的重复/跳帧比例超过 1%；
|
||||||
|
- 任一采样图像年龄超过 50 ms，或两路图像主机时间差超过 50 ms；
|
||||||
|
- 图像形状、数据类型或 RGB 通道约定错误；
|
||||||
|
- 录制中按 A；
|
||||||
|
- HDF5 写入失败、磁盘空间不足或有界写入队列持续积压；
|
||||||
|
- Ctrl+C、采集节点异常退出或启动时恢复崩溃残留文件。
|
||||||
|
|
||||||
|
采集节点的拒绝只处理数据，不额外发送停止或运动命令。若原因来自控制故障，仍由
|
||||||
|
现有遥操安全链路执行原有安全停止。
|
||||||
|
|
||||||
|
### 允许但记录告警的情况
|
||||||
|
|
||||||
|
QP 求解偶发失败时，现有逻辑保留上次有效关节目标。只要该保持目标最终成功发送、
|
||||||
|
反馈和其他质量规则正常，就不自动拒绝 episode，而是保存：
|
||||||
|
|
||||||
|
- QP 失败样本数；
|
||||||
|
- 失败占比；
|
||||||
|
- 最长连续失败样本数；
|
||||||
|
- 每个样本的 `qp_success`。
|
||||||
|
|
||||||
|
工作空间限位、TCP 步长限制或关节速度/加速度限制生效同样只记录，不自动拒绝。
|
||||||
|
这些限制是正常安全控制的一部分。
|
||||||
|
|
||||||
|
## 文件编号、保存、拒绝与恢复
|
||||||
|
|
||||||
|
正式编号只扫描任务目录根部的 `episode_<数字>.hdf5`，取最大编号加一：
|
||||||
|
|
||||||
|
```text
|
||||||
|
已有 episode_0.hdf5 ... episode_9.hdf5
|
||||||
|
重启后下一个正式文件仍为 episode_10.hdf5
|
||||||
|
```
|
||||||
|
|
||||||
|
不填补编号空洞，绝不覆盖已有正式文件。任务目录使用标准库文件锁保证同一时刻只有
|
||||||
|
一个采集进程分配编号和写入；临时文件与目标文件位于同一文件系统，检查通过后使用
|
||||||
|
不覆盖已有目标的原子发布方式。
|
||||||
|
|
||||||
|
文件生命周期：
|
||||||
|
|
||||||
|
```text
|
||||||
|
录制中：
|
||||||
|
/home/robot/ACT_Data/tomato_pick/episode_10.partial.hdf5
|
||||||
|
|
||||||
|
检查通过：
|
||||||
|
/home/robot/ACT_Data/tomato_pick/episode_10.hdf5
|
||||||
|
|
||||||
|
检查失败：
|
||||||
|
/home/robot/ACT_Data/tomato_pick/rejected/
|
||||||
|
episode_10_<reason>_<timestamp>.hdf5
|
||||||
|
```
|
||||||
|
|
||||||
|
- 只有正式保存成功才消耗编号；
|
||||||
|
- 手动长按 Y 丢弃时关闭并删除当前临时文件，不生成拒绝文件；
|
||||||
|
- 普通质量拒绝转入 `rejected/`，不消耗正式编号；
|
||||||
|
- Ctrl+C 时尽力关闭可读 HDF5，根属性写入 `interrupted=true`，文件名原因使用
|
||||||
|
`interrupted`；
|
||||||
|
- 下次启动先处理遗留临时文件：可读文件转入 `rejected/` 并标记
|
||||||
|
`crash_recovered`，不可读文件改成带时间戳的 `.partial.hdf5` 保留；
|
||||||
|
- 拒绝原因同时写入文件名和根属性；
|
||||||
|
- 任一步出现目标文件冲突时停止保存并报警，不能覆盖或自动删除已有 episode。
|
||||||
|
|
||||||
|
写入采用单独工作线程和有界队列，ROS 回调只完成取样、对齐和入队。队列容量只需
|
||||||
|
覆盖短暂磁盘抖动，不能无限增长掩盖磁盘吞吐不足；持续积压时拒绝数据。
|
||||||
|
|
||||||
|
## 启动、配置与依赖
|
||||||
|
|
||||||
|
继续使用唯一入口 `xr_rm_bringup/launch/arm_debug.launch.py`，新增参数：
|
||||||
|
|
||||||
|
```text
|
||||||
|
record_act:=false
|
||||||
|
```
|
||||||
|
|
||||||
|
默认 `false`，现有 mock、单臂、双臂和 MuJoCo 启动行为不变。第一阶段唯一允许的
|
||||||
|
采集组合为：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||||||
|
arm:=right use_mock:=false record_act:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
`record_act:=true` 配合 `arm:=left|both` 或 `use_mock:=true` 时，在 launch 参数校验
|
||||||
|
阶段明确拒绝。ACT 采集节点退出不触发整个 launch 的 `Shutdown`，保证数据进程故障
|
||||||
|
不会终止遥操；终端必须清晰显示采集已经不可用。
|
||||||
|
|
||||||
|
新增一份专用 YAML，集中保存：
|
||||||
|
|
||||||
|
- 数据根目录和任务名；
|
||||||
|
- 两个相机序列号、分辨率和帧率；
|
||||||
|
- 30 Hz 采样率、2 秒最短时长和 60 秒最长时长；
|
||||||
|
- 相机、反馈、磁盘和时间对齐质量阈值；
|
||||||
|
- PICO 左右话题、原子采样话题和状态话题。
|
||||||
|
|
||||||
|
硬件相关阈值保留为配置项，核心 HDF5 字段顺序和 8 维语义固定，不为未来可能的
|
||||||
|
变体增加插件或通用框架。
|
||||||
|
|
||||||
|
采集节点继续由 `/home/robot/miniconda3/envs/xr/bin/python` 启动。复用该环境已有的
|
||||||
|
`pyrealsense2` 和 NumPy，只在该环境增加 HDF5 必需依赖 `h5py`；不修改系统 Python，
|
||||||
|
不新增 Conda 环境，也不增加 OpenCV 依赖。依赖缺失时节点应在打开相机或创建文件前
|
||||||
|
给出明确错误。
|
||||||
|
|
||||||
|
## 最小代码范围
|
||||||
|
|
||||||
|
预计只修改或新增以下位置：
|
||||||
|
|
||||||
|
- `xr_rm_interfaces/msg/ActControlSample.msg`：原子消息；
|
||||||
|
- `xr_rm_interfaces/CMakeLists.txt`、`package.xml`：生成并导出消息；
|
||||||
|
- `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：发布原子样本、跟踪夹爪
|
||||||
|
请求/成功状态和 A 键事件；
|
||||||
|
- `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`：把请求的初始工具状态明确改为完全
|
||||||
|
打开；
|
||||||
|
- `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：独立采集节点；
|
||||||
|
- `xr_rm_teleop/setup.py`、`package.xml`：安装入口和 ROS 运行依赖声明；
|
||||||
|
- `xr_rm_bringup/config/act_tomato_pick.yaml`：采集与硬件参数；
|
||||||
|
- `xr_rm_bringup/config/peripherals_rm75.yaml`：只为右臂启用初始化打开；
|
||||||
|
- `xr_rm_bringup/launch/arm_debug.launch.py`：默认关闭的 `record_act` 启动分支；
|
||||||
|
- 现有测试目录中的最小相关测试。
|
||||||
|
|
||||||
|
不改左臂/双臂运动参数，不改变 `left_arm_teleop`、`right_arm_teleop` 节点名，不创建
|
||||||
|
第二个相机包、训练包或数据工具包。
|
||||||
|
|
||||||
|
## 测试与验收
|
||||||
|
|
||||||
|
### 自动化验证
|
||||||
|
|
||||||
|
测试全部使用 mock、假适配器、合成相机帧和临时目录，不连接真机、不移动机械臂、
|
||||||
|
不操作真实夹爪：
|
||||||
|
|
||||||
|
- 原子消息中的 `q_actual`、QP 输出和最终 `q_target` 来自同一控制周期；
|
||||||
|
- QP 失败时记录失败并保持上次目标，不自动拒绝；
|
||||||
|
- 发送失败、夹爪失败和 A 键误触触发拒绝；
|
||||||
|
- Trigger 请求立即改变 `action[7]`，成功返回后才改变 `qpos[7]`；
|
||||||
|
- B/Y/Grip 的边沿、长按、忽略条件和所有状态转换正确；
|
||||||
|
- 从首次有效样本开始按控制序号每 3 个周期取 1 个；
|
||||||
|
- 相机只选择不晚于控制时间的最新帧，并能发现过期、偏斜和帧号异常；
|
||||||
|
- Grip 中途暂停保留，最终松开到 B 的等待段正确裁剪；
|
||||||
|
- HDF5 核心路径、形状、dtype、8 维顺序、根属性和 debug 字段正确；
|
||||||
|
- 变量长度、最短/最长限制、质量拒绝、手动丢弃和 Ctrl+C 正确；
|
||||||
|
- 编号从最大正式编号加一，拒绝不占号，已有文件不被覆盖；
|
||||||
|
- 可读和不可读崩溃残留分别按设计恢复；
|
||||||
|
- `record_act` 默认关闭，非法 arm/mock 组合被 launch 拒绝；
|
||||||
|
- mock 模式不会导入或调用 RealMan SDK、RealSense 或 HDF5 采集链路。
|
||||||
|
|
||||||
|
按照仓库要求，构建和测试从工作空间根目录执行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
colcon build --symlink-install
|
||||||
|
pytest src/xr_rm_teleop/test/test_orientation_control.py
|
||||||
|
```
|
||||||
|
|
||||||
|
实施时还应运行新增测试及受影响的现有关节控制、初始位姿和外设配置测试。未看到
|
||||||
|
实际通过输出前不得声称验证通过。
|
||||||
|
|
||||||
|
### 真机手工验收
|
||||||
|
|
||||||
|
真机验收必须由用户明确授权并在现有安全检查完成后进行，至少验证：
|
||||||
|
|
||||||
|
1. 启动后右臂夹爪完全打开，状态未在命令成功前提前标记；
|
||||||
|
2. 两台相机序列号和画面角色正确，持续 30 FPS 左右；
|
||||||
|
3. B 开始、Grip 激活、B 结束、Y 丢弃和 A 误触拒绝符合状态机；
|
||||||
|
4. 正常采摘文件包含完整抓取、搬运和释放，不包含 A 键回位；
|
||||||
|
5. `qpos/action` 是有限的 `(T,8)` `float32`，图像是两路
|
||||||
|
`(T,480,640,3)` `uint8`；
|
||||||
|
6. QP 短暂失败只增加 debug 计数，成功调整后仍可完成 episode；
|
||||||
|
7. 重启后编号继续递增，丢弃和拒绝不会覆盖或占用正式编号；
|
||||||
|
8. Ctrl+C、相机断流和磁盘不足产生带明确原因的拒绝文件；
|
||||||
|
9. ACT 采集节点退出后，遥操安全链路仍按现有行为运行。
|
||||||
|
|
||||||
|
## 后续训练与推理影响
|
||||||
|
|
||||||
|
本设计生成 ALOHA/ACT 风格的核心数据，但原始 ACT 代码通常把状态维度硬编码为
|
||||||
|
双臂 14 维，并可能假设固定 episode 长度。训练阶段需要单独适配：
|
||||||
|
|
||||||
|
- `state_dim=8`；
|
||||||
|
- 两个相机名 `cam_high`、`cam_right_wrist`；
|
||||||
|
- 变量长度 episode 的 padding 和 mask；
|
||||||
|
- `chunk_size≈60`；
|
||||||
|
- 不使用 `action[t-1]` 旧补丁；
|
||||||
|
- 忽略 `/debug`，除非用于筛选或诊断。
|
||||||
|
|
||||||
|
实时推理只负责从初始位姿执行一次采摘到释放。回初始位姿继续调用当前确定性的
|
||||||
|
`rm_movej`，由未来的上层任务状态机协调，避免让一个低层策略混合两种动作接口和
|
||||||
|
任务阶段。
|
||||||
@@ -1,245 +0,0 @@
|
|||||||
# RM75 双臂 J3 参考角仿真标定设计
|
|
||||||
|
|
||||||
## 1. 目标
|
|
||||||
|
|
||||||
在不连接真机、不修改现有生产控制参数的前提下，基于当前双臂 URDF、Placo QP
|
|
||||||
求解器和 MuJoCo 运动学模型，分别标定左臂与右臂的第三关节软引导参考角：
|
|
||||||
|
|
||||||
\[
|
|
||||||
q_{3,\mathrm{ref}}^{L,*},\qquad q_{3,\mathrm{ref}}^{R,*}。
|
|
||||||
\]
|
|
||||||
|
|
||||||
标定结果只作为当前机器人初始姿态、采摘区域、工具安装和本侧收集筐布局下的
|
|
||||||
仿真初值。后续必须通过 mock 完整控制链路和真机低速试验复验，允许根据实测结果
|
|
||||||
更新参数。
|
|
||||||
|
|
||||||
## 2. 范围
|
|
||||||
|
|
||||||
本轮只做离线参数标定：
|
|
||||||
|
|
||||||
- 左右臂分别从当前 YAML 初始关节姿态出发；
|
|
||||||
- 左臂放入左臂初始 TCP 下方约 40 cm 的本侧收集筐；
|
|
||||||
- 右臂放入右臂初始 TCP 下方约 40 cm 的本侧收集筐；
|
|
||||||
- 每个轨迹点同时指定 TCP 位置与姿态，保持严格六维跟踪；
|
|
||||||
- 第四关节下限暂定左右臂均为 10°；
|
|
||||||
- 使用现有 Placo 任务接口临时加入 J3 软任务和位置可操作度任务；
|
|
||||||
- 将每个有效关节结果同步写入现有 MuJoCo 双臂模型，检查关节映射和状态有效性；
|
|
||||||
- 输出候选角度的逐轨迹指标、汇总排名和推荐平台区间。
|
|
||||||
|
|
||||||
本轮不修改 `placo_ik_solver.py`、遥操作节点或 YAML，不测试真机，不加入任务阶段
|
|
||||||
状态机、自动姿态放松、碰撞规划或新依赖。
|
|
||||||
|
|
||||||
## 3. 坐标与姿态约定
|
|
||||||
|
|
||||||
- 双臂机器人公共坐标系 `+Y` 为正前方；
|
|
||||||
- 公共坐标系 `+Z` 为机器人垂直向上；
|
|
||||||
- 左右方向使用公共坐标系 `X`；
|
|
||||||
- 采摘点保持对应机械臂初始 TCP 的横向 `X` 位置；
|
|
||||||
- 采摘与退出阶段保持初始 TCP 姿态；
|
|
||||||
- 从退出点移动到收集筐上方时，TCP 姿态采用四元数球面插值，平滑旋转为工具工作
|
|
||||||
轴沿公共坐标系 `-Z`；
|
|
||||||
- 收集筐上方至筐内的垂直下降段保持工具朝下姿态；
|
|
||||||
- 返回初始位姿时平滑恢复初始 TCP 姿态。
|
|
||||||
|
|
||||||
“严格六维”表示每个时刻的位置和姿态目标均参与同一 QP，仿真不会因接近奇异点
|
|
||||||
而自动降低姿态权重。
|
|
||||||
|
|
||||||
## 4. 轨迹族
|
|
||||||
|
|
||||||
### 4.1 采摘点
|
|
||||||
|
|
||||||
每条机械臂使用 3 个前向距离和 3 个高度：
|
|
||||||
|
|
||||||
\[
|
|
||||||
y_h\in\{0.30,0.40,0.50\}\ \mathrm{m},
|
|
||||||
\]
|
|
||||||
|
|
||||||
\[
|
|
||||||
z_h\in\{-0.30,-0.20,-0.10\}\ \mathrm{m}。
|
|
||||||
\]
|
|
||||||
|
|
||||||
这些点位于用户给定的前方 30～50 cm、相对机械臂基座高度 ±50 cm 范围内，并且
|
|
||||||
是当前固定初始工具姿态下离线预扫描得到的主要可解高度区间。左右臂各 9 条轨迹，
|
|
||||||
共 18 条完整轨迹。
|
|
||||||
|
|
||||||
### 4.2 单条完整轨迹
|
|
||||||
|
|
||||||
每条轨迹由以下连续段组成：
|
|
||||||
|
|
||||||
1. 初始 TCP 位姿；
|
|
||||||
2. 采摘点前方 5 cm 的预接近点；
|
|
||||||
3. 沿公共 `+Y` 直线进入采摘点；
|
|
||||||
4. 沿原路径退回预接近点；
|
|
||||||
5. 移动到本侧收集筐上方 10 cm，同时平滑旋转到工具朝下；
|
|
||||||
6. 垂直下降 10 cm，到达初始 TCP 下方约 40 cm 的收集筐目标；
|
|
||||||
7. 垂直抬升 10 cm；
|
|
||||||
8. 返回初始 TCP 位姿。
|
|
||||||
|
|
||||||
轨迹按当前控制频率 90 Hz 离散，平移速度不超过 0.15 m/s，角速度不超过
|
|
||||||
0.5 rad/s。每条轨迹均从相同初始关节状态重新开始，避免上一候选角或上一轨迹的
|
|
||||||
状态污染下一次评估。
|
|
||||||
|
|
||||||
### 4.3 边界轨迹
|
|
||||||
|
|
||||||
工作区边界、不可达目标和 QP 失败恢复轨迹不参与第一轮 J3 参数排名。J3 参数确定后，
|
|
||||||
再使用这些轨迹验证失败保持和恢复逻辑，避免不可达点数量掩盖参考角本身的差异。
|
|
||||||
|
|
||||||
## 5. QP 试验配置
|
|
||||||
|
|
||||||
主任务保持现有严格六维相对位姿任务，内部等价于：
|
|
||||||
|
|
||||||
\[
|
|
||||||
\left\|J_p\Delta q-e_p\right\|^2
|
|
||||||
+\left\|J_R\Delta q-e_R\right\|^2，
|
|
||||||
\]
|
|
||||||
|
|
||||||
其中误差定义为目标减当前。仿真脚本不改变 Placo 的误差符号。
|
|
||||||
|
|
||||||
在主任务之外临时加入：
|
|
||||||
|
|
||||||
\[
|
|
||||||
w_e\left(q_3+\Delta q_3-q_{3,\mathrm{ref}}\right)^2,
|
|
||||||
\qquad w_e=10^{-5}，
|
|
||||||
\]
|
|
||||||
|
|
||||||
以及 TCP 位置可操作度任务：
|
|
||||||
|
|
||||||
\[
|
|
||||||
-w_m\nabla m_p(q)^T\Delta q,
|
|
||||||
\qquad w_m=10^{-4}。
|
|
||||||
\]
|
|
||||||
|
|
||||||
保留当前动能正则、URDF 关节位置限制和关节速度限制。第四关节临时增加：
|
|
||||||
|
|
||||||
\[
|
|
||||||
q_4\ge10^\circ。
|
|
||||||
\]
|
|
||||||
|
|
||||||
## 6. 参数扫描
|
|
||||||
|
|
||||||
### 6.1 粗扫
|
|
||||||
|
|
||||||
左臂：
|
|
||||||
|
|
||||||
\[
|
|
||||||
q_{3,\mathrm{ref}}^L\in\{0^\circ,10^\circ,\ldots,100^\circ\}。
|
|
||||||
\]
|
|
||||||
|
|
||||||
右臂：
|
|
||||||
|
|
||||||
\[
|
|
||||||
q_{3,\mathrm{ref}}^R\in\{0^\circ,-10^\circ,\ldots,-120^\circ\}。
|
|
||||||
\]
|
|
||||||
|
|
||||||
### 6.2 细扫
|
|
||||||
|
|
||||||
在粗扫最优候选附近 ±10° 内以 2° 为步长再次扫描。若多个相邻候选没有明显差异，
|
|
||||||
选择稳定平台区的中心，而不是选择孤立的单点峰值。
|
|
||||||
|
|
||||||
### 6.3 基线
|
|
||||||
|
|
||||||
同时运行两组基线：
|
|
||||||
|
|
||||||
- `original`：当前原始 QP，不含 J3 软任务、位置可操作度任务和 J4 额外下限；
|
|
||||||
- `manip_j4`：不含 J3 软任务，但加入位置可操作度任务和 J4 额外下限。
|
|
||||||
|
|
||||||
所有 J3 候选均在 `manip_j4` 基础上只改变 J3 参考角。最终结果必须同时报告：
|
|
||||||
|
|
||||||
- 相对当前原始 QP 的改善；
|
|
||||||
- 相对“只加位置可操作度”的改善；
|
|
||||||
- J3 软任务是否降低六维跟踪成功率。
|
|
||||||
|
|
||||||
## 7. 记录指标
|
|
||||||
|
|
||||||
对每个候选角度、每条轨迹记录：
|
|
||||||
|
|
||||||
- QP 求解失败周期数 `N_fail`；
|
|
||||||
- 完成全部轨迹的数量 `N_complete`；
|
|
||||||
- 整条轨迹六维雅可比的最小奇异值 `sigma_min`；
|
|
||||||
- 第四关节最小安全余量 `m_q4 = min(q4 - 10°)`；
|
|
||||||
- 肘部最小外展量 `d_elbow`；
|
|
||||||
- 最大 TCP 位置误差和姿态误差；
|
|
||||||
- 最大关节速度；
|
|
||||||
- 累计关节运动代价 `E_q`；
|
|
||||||
- 是否出现超过阈值的单周期关节构型跳变。
|
|
||||||
|
|
||||||
肘部外展量使用公共坐标系中第四连杆位置计算：
|
|
||||||
|
|
||||||
\[
|
|
||||||
d_{\mathrm{elbow}}^L=-x_{\mathrm{elbow}}^L,
|
|
||||||
\qquad
|
|
||||||
d_{\mathrm{elbow}}^R=x_{\mathrm{elbow}}^R。
|
|
||||||
\]
|
|
||||||
|
|
||||||
当前 URDF 碰撞网格对相邻连杆存在已知自碰撞警告，因此本轮不把 MuJoCo/Placo
|
|
||||||
碰撞距离加入评分，避免错误碰撞几何影响 J3 选择。
|
|
||||||
|
|
||||||
## 8. 选择规则与评分函数
|
|
||||||
|
|
||||||
### 8.1 硬门槛
|
|
||||||
|
|
||||||
候选角度首先按以下顺序筛选：
|
|
||||||
|
|
||||||
1. `N_fail` 最少；
|
|
||||||
2. `N_complete` 最多；
|
|
||||||
3. 最大位置误差不超过 2 mm；
|
|
||||||
4. 最大姿态误差不超过 0.005 rad；
|
|
||||||
5. 不违反第四关节、URDF 关节位置和速度限制；
|
|
||||||
6. 不出现超过配置阈值的单周期关节跳变。
|
|
||||||
|
|
||||||
只在通过同一组硬门槛的候选之间使用评分函数。这样不能用较高可操作度抵消更多的
|
|
||||||
QP 失败或更差的 TCP 跟踪。
|
|
||||||
|
|
||||||
### 8.2 并列候选评分函数
|
|
||||||
|
|
||||||
对通过硬门槛的候选，将各项指标在同一机械臂的候选集合内转换为 `[0,1]` 的百分位
|
|
||||||
排名。数值越大越好的指标直接排名，数值越小越好的指标反向排名：
|
|
||||||
|
|
||||||
- `r_sigma`：全轨迹最小奇异值排名；
|
|
||||||
- `r_q4`：第四关节最小安全余量排名；
|
|
||||||
- `r_elbow`：肘部最小外展量排名；
|
|
||||||
- `r_smooth`：累计关节运动代价与最大关节速度的联合反向排名；
|
|
||||||
- `r_track`：最大六维 TCP 跟踪误差的反向排名。
|
|
||||||
|
|
||||||
并列候选的综合评分为：
|
|
||||||
|
|
||||||
\[
|
|
||||||
S(q_{3,\mathrm{ref}})
|
|
||||||
=0.45r_{\sigma}
|
|
||||||
+0.20r_{q4}
|
|
||||||
+0.20r_{\mathrm{elbow}}
|
|
||||||
+0.10r_{\mathrm{smooth}}
|
|
||||||
+0.05r_{\mathrm{track}}。
|
|
||||||
\]
|
|
||||||
|
|
||||||
选择：
|
|
||||||
|
|
||||||
\[
|
|
||||||
q_{3,\mathrm{ref}}^*=\arg\max S(q_{3,\mathrm{ref}})。
|
|
||||||
\]
|
|
||||||
|
|
||||||
最小奇异值权重最高，因为本轮首要目标是降低奇异点和 QP 失败风险；第四关节余量
|
|
||||||
和肘部外展各占 0.20；平滑性和跟踪误差用于区分性能接近的候选。若评分最高点与
|
|
||||||
相邻角度差异小于 2%，取相邻稳定平台的中心角度。
|
|
||||||
|
|
||||||
### 8.3 结果报告
|
|
||||||
|
|
||||||
左右臂分别输出：
|
|
||||||
|
|
||||||
- 推荐参考角；
|
|
||||||
- 推荐稳定区间；
|
|
||||||
- 粗扫与细扫排名表；
|
|
||||||
- 与两组基线的指标对比；
|
|
||||||
- 最差轨迹及其失败位置；
|
|
||||||
- 是否建议保留左右臂统一的第四关节 10° 下限。
|
|
||||||
|
|
||||||
## 9. 实施边界与后续流程
|
|
||||||
|
|
||||||
标定完成后的顺序为：
|
|
||||||
|
|
||||||
1. 根据仿真结果形成正式 QP 修改规格；
|
|
||||||
2. 将左右臂 J3 参考角作为独立可调参数写入对应 YAML；
|
|
||||||
3. 在 `use_mock:=true` 下运行完整遥操作控制链路；
|
|
||||||
4. 加入 QP 失败时不提交笛卡尔目标历史的修复并验证恢复；
|
|
||||||
5. 经过安全评审后，在真机上以低速、小范围方式复验；
|
|
||||||
6. 根据真机日志更新 J3 参考角，但不取消工作空间、速度、超时和安全停止限制。
|
|
||||||
@@ -1,175 +0,0 @@
|
|||||||
# 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 软项，不回退失败状态修复。
|
|
||||||
@@ -1,415 +0,0 @@
|
|||||||
# RM75 三种逆运动学方法离线对比实验设计
|
|
||||||
|
|
||||||
## 1. 目标
|
|
||||||
|
|
||||||
基于现有番茄采摘 episode 的右臂目标位姿轨迹，在完全一致的机械臂模型、初始关节
|
|
||||||
状态、时间轴、收敛判据和输出安全限制下，对比以下三种七自由度逆运动学方法：
|
|
||||||
|
|
||||||
1. Jacobian Moore-Penrose 伪逆法；
|
|
||||||
2. 阻尼最小二乘法（Damped Least Squares，DLS）；
|
|
||||||
3. 当前项目中的优化 Placo QP 方法。
|
|
||||||
|
|
||||||
实验输出用于补充中期报告 2.3.4 节预留的三张图，并同时生成逐采样数据、汇总指标和
|
|
||||||
可直接粘贴到报告中的中文结果分析。实验必须由真实计算结果驱动，不预设或硬编码
|
|
||||||
“QP 更优”的结论。
|
|
||||||
|
|
||||||
## 2. 现有上下文
|
|
||||||
|
|
||||||
### 2.1 报告要求
|
|
||||||
|
|
||||||
中期报告 2.3.4 节已经确定：
|
|
||||||
|
|
||||||
- 三种方法使用同一机械臂模型、初始关节状态和末端目标轨迹；
|
|
||||||
- 统计位置 RMSE、姿态 RMSE、归一化关节安全裕度、最大关节速度、求解时间和
|
|
||||||
求解成功率；
|
|
||||||
- 章节末尾预留三张对比图。
|
|
||||||
|
|
||||||
现有图号从图 2-10 跳到图 2-14，因此本实验生成图 2-11、图 2-12 和图 2-13。
|
|
||||||
|
|
||||||
### 2.2 当前 QP 与报告文字的差异
|
|
||||||
|
|
||||||
报告 2.3.2 节主要描述六维末端软任务、动能正则化、关节位置和速度限制。当前分支的
|
|
||||||
`PlacoIkSolver` 还包含：
|
|
||||||
|
|
||||||
- J3 初始构型软引导；
|
|
||||||
- J4 硬下限和预警区软缓冲；
|
|
||||||
- 接近奇异区时动态启用的六维可操作度任务；
|
|
||||||
- QP 失败时恢复实际关节状态并保持上一安全输出。
|
|
||||||
|
|
||||||
本实验使用当前优化 QP，而不是关闭上述辅助任务的基础 QP。最终分析文件需要提供一段
|
|
||||||
方法补充文字，避免报告方法描述与对比对象不一致。
|
|
||||||
|
|
||||||
## 3. 范围与安全边界
|
|
||||||
|
|
||||||
### 3.1 本次包含
|
|
||||||
|
|
||||||
- 只读加载一个现有右臂 episode；
|
|
||||||
- 离线重采样目标位姿；
|
|
||||||
- 在同一 URDF 上运行三种逆运动学方法；
|
|
||||||
- 复用当前 QP 代码和右臂 YAML 参数；
|
|
||||||
- 对三种方法使用相同的输出端安全处理；
|
|
||||||
- 生成 SVG、300 dpi PNG、CSV、JSON 和中文 Markdown 分析。
|
|
||||||
|
|
||||||
### 3.2 本次不包含
|
|
||||||
|
|
||||||
- 不连接真机，不移动机械臂，不操作夹爪；
|
|
||||||
- 不启动新的 PICO 录制；
|
|
||||||
- 不修改生产遥操作节点、launch、YAML 默认值或公开 API；
|
|
||||||
- 不使用 episode 中已经记录的 QP 关节结果充当本次 QP 结果；
|
|
||||||
- 不模拟电机、通信和接触动力学；
|
|
||||||
- 不直接编辑用户提供的 PDF。
|
|
||||||
|
|
||||||
实验只使用当前 Conda 环境已经安装的 NumPy、h5py、Matplotlib 和 Placo，不新增项目
|
|
||||||
依赖。
|
|
||||||
|
|
||||||
因此，结果应表述为“基于真实遥操作目标轨迹的离线运动学对比”，不得表述为新的真机
|
|
||||||
在线控制对比。
|
|
||||||
|
|
||||||
## 4. 数据源与质量基线
|
|
||||||
|
|
||||||
实验固定使用：
|
|
||||||
|
|
||||||
```text
|
|
||||||
/home/robot/ACT_Data/tomato_pick/episode_0.hdf5
|
|
||||||
```
|
|
||||||
|
|
||||||
该文件的已核对属性如下：
|
|
||||||
|
|
||||||
- 机械臂：`right_rm75`；
|
|
||||||
- 样本数：484；
|
|
||||||
- 有效时长：约 16.1 s；
|
|
||||||
- 保存采样率：约 30 Hz；
|
|
||||||
- 位姿顺序：`x,y,z,qx,qy,qz,qw`；
|
|
||||||
- 所有目标位姿、当前位姿和关节状态均为有限值；
|
|
||||||
- 目标和当前四元数范数接近 1；
|
|
||||||
- 483 帧为遥操作激活且已发送命令；
|
|
||||||
- 记录时 QP 尝试 483 次并成功 483 次；
|
|
||||||
- 132 帧触发过目标限幅，轨迹本身包含足够的约束压力。
|
|
||||||
|
|
||||||
只使用满足以下条件的最长连续区间：
|
|
||||||
|
|
||||||
```text
|
|
||||||
teleop_active && action_valid && command_sent
|
|
||||||
```
|
|
||||||
|
|
||||||
共同末端目标取 `debug/tcp/final_target_pose`。该字段已通过原系统的工作空间限制、目标
|
|
||||||
平滑和单帧笛卡尔步长限制，适合作为三种逆运动学方法的共同安全输入。共同初始关节角
|
|
||||||
取有效区间第一帧的 `observations/qpos[:7]`。
|
|
||||||
|
|
||||||
episode 中后续 `observations/qpos`、`debug/qp/raw_target`、QP 成功标志和耗时只用于
|
|
||||||
数据质量核对，不替代任何方法在本实验中的离线计算结果。
|
|
||||||
|
|
||||||
## 5. 统一复放架构
|
|
||||||
|
|
||||||
数据流为：
|
|
||||||
|
|
||||||
```text
|
|
||||||
episode_0.hdf5
|
|
||||||
-> 有效区间与共同初始状态
|
|
||||||
-> 30 Hz 目标位姿重采样到 90 Hz
|
|
||||||
-> 伪逆 / DLS / 当前优化 QP 三路独立复放
|
|
||||||
-> 共同输出安全层
|
|
||||||
-> 正向运动学和逐采样指标
|
|
||||||
-> CSV / JSON / 三张图 / 中文分析
|
|
||||||
```
|
|
||||||
|
|
||||||
三种方法各自维护独立的关节状态和上一周期关节速度。每个方法的下一状态只能由该方法
|
|
||||||
本周期的安全输出推进，三路之间不共享可变状态。
|
|
||||||
|
|
||||||
离线状态推进采用理想位置跟随，即共同输出限速器给出的关节目标直接作为下一 90 Hz
|
|
||||||
周期的关节状态。这一简化隔离了逆运动学方法本身，不引入未建模的电机和网络差异。
|
|
||||||
|
|
||||||
## 6. 目标轨迹重采样
|
|
||||||
|
|
||||||
原 episode 按约 30 Hz 保存，而当前遥操作控制器使用 90 Hz。重采样使用 episode 的
|
|
||||||
`debug/timestamps/control_monotonic_ns`，目标时间轴保持原始起止时刻并以 1/90 s
|
|
||||||
采样：
|
|
||||||
|
|
||||||
- 位置使用分段线性插值；
|
|
||||||
- 姿态使用归一化四元数的最短弧 SLERP；
|
|
||||||
- 相邻四元数点积为负时先翻转后一四元数，避免绕长弧插值；
|
|
||||||
- 第一个和最后一个重采样位姿必须与原始有效区间端点一致；
|
|
||||||
- 不对目标轨迹额外放大、延长或人工加入困难片段。
|
|
||||||
|
|
||||||
## 7. 三种逆运动学方法
|
|
||||||
|
|
||||||
### 7.1 共同任务定义
|
|
||||||
|
|
||||||
当前关节状态为 `q`，正向运动学得到当前 TCP 位姿 `(p, R)`，目标位姿为
|
|
||||||
`(p_d, R_d)`。位置误差和姿态误差分别为：
|
|
||||||
|
|
||||||
```text
|
|
||||||
e_p = p_d - p
|
|
||||||
e_R = Log(R^T R_d)
|
|
||||||
```
|
|
||||||
|
|
||||||
求解时的角速度误差表达必须与所用 `local_world_aligned` Jacobian 的坐标表达一致；
|
|
||||||
姿态误差大小统一使用目标与实际旋转矩阵之间的最短夹角评价。伪逆和 DLS 使用相同的
|
|
||||||
位置、姿态反馈增益、相同 Jacobian、相同 90 Hz 步长和相同数值迭代框架。
|
|
||||||
|
|
||||||
三种方法对单个目标最多执行 30 次数值迭代。满足以下两个条件时记为收敛：
|
|
||||||
|
|
||||||
```text
|
|
||||||
位置误差 <= 0.002 m
|
|
||||||
姿态误差 <= 0.005 rad
|
|
||||||
```
|
|
||||||
|
|
||||||
### 7.2 Jacobian 伪逆法
|
|
||||||
|
|
||||||
伪逆法按报告公式计算：
|
|
||||||
|
|
||||||
```text
|
|
||||||
q_dot = pinv(J) * v_d
|
|
||||||
```
|
|
||||||
|
|
||||||
其中 `v_d` 由共同的六维位姿反馈误差生成。实现直接使用 NumPy 的 Moore-Penrose
|
|
||||||
伪逆，不增加零空间任务、阻尼或自适应奇异值阈值，以保持基线定义清楚。
|
|
||||||
|
|
||||||
### 7.3 DLS 方法
|
|
||||||
|
|
||||||
DLS 按报告公式计算：
|
|
||||||
|
|
||||||
```text
|
|
||||||
q_dot = J^T * inv(J * J^T + mu^2 * I) * v_d
|
|
||||||
```
|
|
||||||
|
|
||||||
公式保持与报告一致；数值实现使用线性方程求解，不显式计算矩阵逆。
|
|
||||||
|
|
||||||
只扫描固定阻尼系数，不实现自适应 DLS。候选值使用对数尺度的小集合：
|
|
||||||
|
|
||||||
```text
|
|
||||||
0.001, 0.003, 0.01, 0.03, 0.1, 0.3
|
|
||||||
```
|
|
||||||
|
|
||||||
每个候选均完整复放 episode，先按求解成功率从高到低选择，再在成功率相同的候选中
|
|
||||||
最小化：
|
|
||||||
|
|
||||||
```text
|
|
||||||
位置 RMSE / 0.002 + 姿态 RMSE / 0.005
|
|
||||||
```
|
|
||||||
|
|
||||||
若仍并列，选择最大关节速度更小的候选。阻尼扫描使用同一条评价轨迹，因此最终文字
|
|
||||||
必须说明该 DLS 是“在当前轨迹上选优的固定阻尼基线”；这一口径对 DLS 较有利，不能
|
|
||||||
将其解释为跨轨迹最优参数。
|
|
||||||
|
|
||||||
阻尼扫描耗时不计入三种方法的在线求解时间对比。
|
|
||||||
|
|
||||||
### 7.4 当前优化 QP
|
|
||||||
|
|
||||||
QP 直接实例化现有 `xr_rm_teleop.placo_ik_solver.PlacoIkSolver`，使用 90 Hz 步长、
|
|
||||||
当前双臂 URDF 和右臂配置中的参数：
|
|
||||||
|
|
||||||
```text
|
|
||||||
qp_j3_reference_deg: -89.57
|
|
||||||
qp_j3_weight: 0.0001
|
|
||||||
qp_j4_min_deg: 10.0
|
|
||||||
qp_j4_warn_deg: 25.0
|
|
||||||
qp_j4_weight: 0.0001
|
|
||||||
qp_manipulability_sigma_stop: 0.01
|
|
||||||
qp_manipulability_sigma_warn: 0.04
|
|
||||||
qp_manipulability_weight: 0.0001
|
|
||||||
```
|
|
||||||
|
|
||||||
保留现有六维末端软任务、`1e-6` 动能正则化、URDF 关节位置和速度限制、30 次迭代、
|
|
||||||
收敛阈值、输入变换校验、失败恢复和结果有效性检查。实验脚本不复制或重写 QP。
|
|
||||||
|
|
||||||
## 8. 共同输出安全层与失败处理
|
|
||||||
|
|
||||||
三种方法使用同一安全口径：
|
|
||||||
|
|
||||||
1. 每次数值迭代结果必须为 7 个有限关节值；
|
|
||||||
2. 数值迭代的关节状态不得超出 URDF 位置范围；
|
|
||||||
3. 相邻数值迭代的关节变化不得超过 URDF 速度上限乘以 `1/90 s`；
|
|
||||||
4. 有效求解结果继续经过生产控制器现有的关节速度/加速度限制逻辑；
|
|
||||||
5. 输出端最大关节速度为 `180 deg/s`，最大关节加速度为 `300 deg/s^2`；
|
|
||||||
6. 未在 30 次内收敛、出现非有限值或违反硬边界时，本周期记为失败并保持上一安全
|
|
||||||
关节状态；
|
|
||||||
7. 失败不会停止离线复放，时间轴继续推进，并记录失败次数和最长连续失败长度。
|
|
||||||
|
|
||||||
QP 在优化内部主动处理关节边界；伪逆和 DLS 在每次候选步之后接受同样的硬检查。
|
|
||||||
共同输出层不会消除算法差异：基线仍可能因候选步无效而失败或保持，QP 则可能在优化
|
|
||||||
过程中找到满足约束的解。
|
|
||||||
|
|
||||||
## 9. 评价指标
|
|
||||||
|
|
||||||
### 9.1 末端跟踪
|
|
||||||
|
|
||||||
- 逐采样位置误差 `||p_d - p||`，单位为 mm；
|
|
||||||
- 逐采样姿态夹角误差，单位为 degree；
|
|
||||||
- 全轨迹位置 RMSE，报告中仍以 m 给出，图中用 mm；
|
|
||||||
- 全轨迹姿态 RMSE，报告公式使用 rad，图中用 degree。
|
|
||||||
|
|
||||||
### 9.2 关节运动
|
|
||||||
|
|
||||||
- 每个采样时刻七关节绝对速度的最大值，单位为 `deg/s`；
|
|
||||||
- 全轨迹最大关节速度；
|
|
||||||
- 超过或触发共同速度/加速度限制器的周期数；
|
|
||||||
- 按报告式 (2-28) 计算的逐采样最小归一化关节安全裕度；
|
|
||||||
- 全轨迹最小归一化关节安全裕度。
|
|
||||||
|
|
||||||
速度图不再使用归一化速度。当前 URDF 中右臂七个关节的速度上限均为 `3.14 rad/s`
|
|
||||||
（约 `180 deg/s`），直接展示实际速度更直观且与报告文字一致。
|
|
||||||
|
|
||||||
### 9.3 求解性能
|
|
||||||
|
|
||||||
- 收敛成功周期数和成功率；
|
|
||||||
- 失败周期数和最长连续失败长度；
|
|
||||||
- 单周期 IK 求解平均时间和最大时间，单位为 ms。
|
|
||||||
|
|
||||||
耗时只覆盖单次 IK 求解，不包含 HDF5 读取、重采样、指标汇总和绘图。先执行一次完整
|
|
||||||
预热复放，再对选定参数的三种方法各重复 10 次。轨迹和非耗时指标必须在重复复放间
|
|
||||||
保持确定；平均和最大耗时从 10 次计时复放汇总。
|
|
||||||
|
|
||||||
## 10. 三张图设计
|
|
||||||
|
|
||||||
### 10.1 图 2-11 三种逆运动学方法末端位姿跟踪误差对比
|
|
||||||
|
|
||||||
使用上下两个共享时间轴的子图：
|
|
||||||
|
|
||||||
- `(a)` 位置误差时序，单位 mm；
|
|
||||||
- `(b)` 姿态误差时序，单位 degree。
|
|
||||||
|
|
||||||
三种方法使用固定颜色、不同线型，并在失败保持区间添加不遮挡曲线的标记。图中不绘制
|
|
||||||
episode 原始 QP 误差曲线。
|
|
||||||
|
|
||||||
### 10.2 图 2-12 三种逆运动学方法关节运动约束对比
|
|
||||||
|
|
||||||
使用两个共享时间轴的子图：
|
|
||||||
|
|
||||||
- `(a)` 每个时刻的最大关节速度，单位 `deg/s`，并绘制 `180 deg/s` 虚线；
|
|
||||||
- `(b)` 每个时刻的最小归一化关节安全裕度，数值越大表示离关节上下限越远。
|
|
||||||
|
|
||||||
### 10.3 图 2-13 三种逆运动学方法综合性能指标对比
|
|
||||||
|
|
||||||
使用 `2 x 3` 六个小型分组柱状图，避免不同量纲共用坐标轴：
|
|
||||||
|
|
||||||
1. 位置 RMSE；
|
|
||||||
2. 姿态 RMSE；
|
|
||||||
3. 最大关节速度；
|
|
||||||
4. 最小归一化关节安全裕度；
|
|
||||||
5. 平均和最大求解时间；
|
|
||||||
6. 求解成功率。
|
|
||||||
|
|
||||||
柱顶标注精确数值。最终配色需兼顾色盲识别和灰度打印，除颜色外再使用线型、标记和
|
|
||||||
图例区分方法。
|
|
||||||
|
|
||||||
## 11. 文件与产物
|
|
||||||
|
|
||||||
实验实现优先保持最小范围：
|
|
||||||
|
|
||||||
```text
|
|
||||||
xr_rm_teleop/test/ik_method_comparison.py
|
|
||||||
xr_rm_teleop/test/test_ik_method_comparison.py
|
|
||||||
```
|
|
||||||
|
|
||||||
前者包含命令行入口、HDF5 读取、重采样、三种方法复放、指标计算和绘图；后者只覆盖
|
|
||||||
无法由现有测试保护的新非平凡逻辑，不新增测试框架或通用评测抽象。
|
|
||||||
|
|
||||||
默认输出目录为：
|
|
||||||
|
|
||||||
```text
|
|
||||||
output/ik_comparison/episode_0/
|
|
||||||
```
|
|
||||||
|
|
||||||
产物包括：
|
|
||||||
|
|
||||||
```text
|
|
||||||
samples.csv
|
|
||||||
summary.json
|
|
||||||
figure_2_11_tracking_error.svg
|
|
||||||
figure_2_11_tracking_error.png
|
|
||||||
figure_2_12_joint_constraints.svg
|
|
||||||
figure_2_12_joint_constraints.png
|
|
||||||
figure_2_13_summary.svg
|
|
||||||
figure_2_13_summary.png
|
|
||||||
analysis_2.3.4.md
|
|
||||||
```
|
|
||||||
|
|
||||||
`samples.csv` 使用长表结构，每行对应“方法 + 时间点”，至少包含目标位姿、实际位姿、
|
|
||||||
位置误差、姿态误差、七关节角、七关节速度、最小安全裕度、求解耗时、成功标志和限制
|
|
||||||
触发标志。`summary.json` 保存输入路径、Git 提交、参数、选定 DLS 阻尼、指标和产物路径，
|
|
||||||
保证结果可追溯。
|
|
||||||
|
|
||||||
`analysis_2.3.4.md` 使用中文撰写，包含：
|
|
||||||
|
|
||||||
- 数据来源和离线实验口径；
|
|
||||||
- DLS 最终阻尼和选择规则；
|
|
||||||
- 三张图的建议图题与图注；
|
|
||||||
- 与式 (2-25) 至式 (2-28) 对应的数值结果；
|
|
||||||
- 对优势、代价和异常结果的客观分析；
|
|
||||||
- 当前优化 QP 相对报告 2.3.2 节的补充方法说明。
|
|
||||||
|
|
||||||
## 12. 错误处理
|
|
||||||
|
|
||||||
以下情况在生成任何正式图前立即报错：
|
|
||||||
|
|
||||||
- episode 路径不存在或不是 HDF5；
|
|
||||||
- 必需字段或属性缺失；
|
|
||||||
- 数组长度不一致；
|
|
||||||
- 找不到至少包含两个样本的连续有效遥操作区间；
|
|
||||||
- 时间戳非严格递增；
|
|
||||||
- 位姿、关节角或四元数含 NaN/Inf；
|
|
||||||
- 四元数无法正规化；
|
|
||||||
- episode 机械臂不是 `right_rm75`；
|
|
||||||
- URDF 或当前右臂配置不存在；
|
|
||||||
- Placo 版本不是项目固定的 0.9.4；
|
|
||||||
- 任一方法没有生成与统一时间轴等长的结果；
|
|
||||||
- CSV、JSON 和绘图使用的汇总数值不一致。
|
|
||||||
|
|
||||||
单个目标的逆运动学失败属于实验结果，按上一安全状态保持，不中止整条轨迹。输入数据
|
|
||||||
结构错误、模型错误和结果长度错误属于实验无效，必须中止并说明原因。
|
|
||||||
|
|
||||||
## 13. 测试与验证
|
|
||||||
|
|
||||||
### 13.1 聚焦测试
|
|
||||||
|
|
||||||
最小测试至少覆盖：
|
|
||||||
|
|
||||||
- 30 Hz 到 90 Hz 重采样保持首尾位置和姿态；
|
|
||||||
- SLERP 选择最短弧并输出单位四元数；
|
|
||||||
- 姿态夹角误差在单位旋转和已知小角度下正确；
|
|
||||||
- 关节安全裕度与式 (2-28) 一致；
|
|
||||||
- 无效候选触发失败保持而不是推进状态；
|
|
||||||
- DLS 选择规则按成功率、归一化误差和最大速度依次决策；
|
|
||||||
- 汇总指标与逐采样数据一致。
|
|
||||||
|
|
||||||
### 13.2 真实模型冒烟验证
|
|
||||||
|
|
||||||
使用当前双臂 URDF 和右臂初始关节角，对三种方法各运行一小段真实目标位姿序列，确认：
|
|
||||||
|
|
||||||
- 输出始终为有限 7 维关节值；
|
|
||||||
- 没有输出越过 URDF 关节位置边界；
|
|
||||||
- 失败时保持上一安全状态；
|
|
||||||
- 当前 QP 直接走现有 `PlacoIkSolver`，没有本地复制实现。
|
|
||||||
|
|
||||||
### 13.3 项目级验证
|
|
||||||
|
|
||||||
从工作空间根目录 `/home/robot/WS_xr` 执行，并先加载 ROS2 Humble：
|
|
||||||
|
|
||||||
```bash
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
colcon build --symlink-install
|
|
||||||
pytest src/xr_rm_teleop/test/test_orientation_control.py
|
|
||||||
```
|
|
||||||
|
|
||||||
随后使用项目固定的 Conda Python 运行聚焦测试和完整离线实验。验证完成后还需检查：
|
|
||||||
|
|
||||||
- 三张 PNG 无裁切、重叠、乱码或不可辨识曲线；
|
|
||||||
- SVG 可编辑且文字完整；
|
|
||||||
- PNG 为 300 dpi；
|
|
||||||
- 图题、坐标轴、单位和图例为中文论文风格；
|
|
||||||
- `summary.json` 与图中柱顶数值一致；
|
|
||||||
- 同一输入重复运行时，除耗时外的结果一致。
|
|
||||||
|
|
||||||
## 14. 验收标准
|
|
||||||
|
|
||||||
满足以下条件才视为完成：
|
|
||||||
|
|
||||||
1. 三种方法从完全相同的 episode 目标轨迹和初始关节状态开始；
|
|
||||||
2. 当前优化 QP 复用现有实现和右臂参数；
|
|
||||||
3. 三种方法使用同一输出安全口径，任何失败均安全保持；
|
|
||||||
4. DLS 固定阻尼选择过程和最终值可追溯；
|
|
||||||
5. 生成三张与报告公式和图号一致的正式对比图；
|
|
||||||
6. 生成完整 CSV、JSON 和中文 2.3.4 分析文字；
|
|
||||||
7. 所有实际执行的测试和构建结果如实记录；
|
|
||||||
8. 不连接真机、不修改生产控制默认值、不新增依赖和重复 QP 实现。
|
|
||||||
@@ -1,29 +0,0 @@
|
|||||||
# 2.3.4 三种逆运动学方法对比补充分析
|
|
||||||
|
|
||||||
本结果是基于真实遥操作目标轨迹的离线运动学对比，不代表真机闭环实验。数据来自
|
|
||||||
`/home/robot/ACT_Data/tomato_pick/episode_0.hdf5`，目标位姿以 90.0 Hz 重采样；三种方法使用同一初始
|
|
||||||
关节状态、同一 URDF、相同收敛阈值和共同的输出速度/加速度限制。
|
|
||||||
|
|
||||||
DLS 扫描的固定阻尼候选为 0.001, 0.003, 0.01, 0.03, 0.1, 0.3，本轨迹选定
|
|
||||||
`0.3`。该参数是在当前评价轨迹上选优，不应解释为跨轨迹最优参数。
|
|
||||||
|
|
||||||
| 方法 | 位置 RMSE (m) | 姿态 RMSE (rad) | 最大关节速度 (°/s) | 最小归一化裕度 | 平均/最大求解时间 (ms) | 成功率 |
|
|
||||||
| --- | ---: | ---: | ---: | ---: | ---: | ---: |
|
|
||||||
| Jacobian 伪逆 | 0.260542 | 0.713938 | 30.000 | 0.1509 | 0.136 / 1.061 | 1.17% |
|
|
||||||
| DLS | 0.005845 | 0.011201 | 49.999 | 0.0607 | 0.391 / 1.942 | 100.00% |
|
|
||||||
| 优化 QP | 0.004929 | 0.011075 | 46.666 | 0.0611 | 0.206 / 1.057 | 100.00% |
|
|
||||||
|
|
||||||
图 2-11 三种逆运动学方法的末端位置与姿态跟踪误差。纵轴采用对数坐标以同时显示不同
|
|
||||||
数量级的误差，叉号稀疏标记数值求解失败并保持上一安全关节状态的周期。
|
|
||||||
|
|
||||||
图 2-12 三种逆运动学方法的最大关节速度与最小归一化关节安全裕度。红色虚线表示
|
|
||||||
180°/s 输出速度上限，裕度越大表示离关节位置边界越远。
|
|
||||||
|
|
||||||
图 2-13 三种逆运动学方法的综合性能对比，包括误差、关节运动、求解时间和成功率。
|
|
||||||
|
|
||||||
按本次单轨迹数值比较，位置 RMSE 最低的方法为优化 QP，姿态 RMSE 最低的方法为
|
|
||||||
优化 QP，成功率最高的方法为DLS、优化 QP。这些结论只描述本次离线复放，
|
|
||||||
未进行统计显著性检验。
|
|
||||||
|
|
||||||
当前优化 QP 除六维末端主任务外，还保留项目中的 J3 参考软任务、J4 硬下界与软缓冲，
|
|
||||||
以及按最小奇异值动态激活的六维可操作度任务；伪逆和 DLS 基线不包含这些附加任务。
|
|
||||||
Binary file not shown.
|
Before Width: | Height: | Size: 433 KiB |
File diff suppressed because it is too large
Load Diff
|
Before Width: | Height: | Size: 194 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 271 KiB |
File diff suppressed because it is too large
Load Diff
|
Before Width: | Height: | Size: 129 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 271 KiB |
File diff suppressed because it is too large
Load Diff
|
Before Width: | Height: | Size: 179 KiB |
File diff suppressed because it is too large
Load Diff
@@ -1,50 +0,0 @@
|
|||||||
{
|
|
||||||
"source_episode": "/home/robot/ACT_Data/tomato_pick/episode_0.hdf5",
|
|
||||||
"git_commit": "f30aac547b6fe84703044c66b30b70720ee4d40a",
|
|
||||||
"sample_rate_hz": 90.00141938026849,
|
|
||||||
"selected_dls_damping": 0.3,
|
|
||||||
"methods": {
|
|
||||||
"pinv": {
|
|
||||||
"method": "pinv",
|
|
||||||
"damping": null,
|
|
||||||
"success_rate": 0.011748445058742226,
|
|
||||||
"position_rmse_m": 0.26054182896155575,
|
|
||||||
"orientation_rmse_rad": 0.713938311140316,
|
|
||||||
"max_joint_speed_deg_s": 29.999526880705357,
|
|
||||||
"min_joint_margin": 0.15094435580838983,
|
|
||||||
"mean_solve_ms": 0.13594157401520388,
|
|
||||||
"max_solve_ms": 1.061404,
|
|
||||||
"failure_count": 1430,
|
|
||||||
"longest_failure_streak": 1430,
|
|
||||||
"command_limited_count": 17
|
|
||||||
},
|
|
||||||
"dls": {
|
|
||||||
"method": "dls",
|
|
||||||
"damping": 0.3,
|
|
||||||
"success_rate": 1.0,
|
|
||||||
"position_rmse_m": 0.005845412147117041,
|
|
||||||
"orientation_rmse_rad": 0.011200514621088859,
|
|
||||||
"max_joint_speed_deg_s": 49.999211467842265,
|
|
||||||
"min_joint_margin": 0.060689206887586424,
|
|
||||||
"mean_solve_ms": 0.39115279765031097,
|
|
||||||
"max_solve_ms": 1.94196,
|
|
||||||
"failure_count": 0,
|
|
||||||
"longest_failure_streak": 0,
|
|
||||||
"command_limited_count": 1282
|
|
||||||
},
|
|
||||||
"qp": {
|
|
||||||
"method": "qp",
|
|
||||||
"damping": null,
|
|
||||||
"success_rate": 1.0,
|
|
||||||
"position_rmse_m": 0.004929450681893051,
|
|
||||||
"orientation_rmse_rad": 0.01107496419458487,
|
|
||||||
"max_joint_speed_deg_s": 46.66593070331945,
|
|
||||||
"min_joint_margin": 0.06105232712172298,
|
|
||||||
"mean_solve_ms": 0.20632571050449205,
|
|
||||||
"max_solve_ms": 1.057197,
|
|
||||||
"failure_count": 0,
|
|
||||||
"longest_failure_streak": 0,
|
|
||||||
"command_limited_count": 1267
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,33 @@
|
|||||||
|
act_episode_recorder:
|
||||||
|
ros__parameters:
|
||||||
|
output_root: /home/robot/ACT_Data
|
||||||
|
task_name: tomato_pick
|
||||||
|
control_sample_topic: /xr_rm/right_rm75/act_control_sample
|
||||||
|
right_controller_topic: /xr/right_controller
|
||||||
|
left_controller_topic: /xr/left_controller
|
||||||
|
status_topic: /act/recording_status
|
||||||
|
|
||||||
|
cam_high_serial: "234222303366"
|
||||||
|
cam_high_model: D455
|
||||||
|
cam_right_wrist_serial: "412622272532"
|
||||||
|
cam_right_wrist_model: D405
|
||||||
|
image_width: 640
|
||||||
|
image_height: 480
|
||||||
|
camera_fps: 30
|
||||||
|
camera_warmup_sec: 5.0
|
||||||
|
|
||||||
|
control_rate_hz: 90.0
|
||||||
|
sample_rate_hz: 30.0
|
||||||
|
min_samples: 60
|
||||||
|
max_samples: 1800
|
||||||
|
min_control_hz: 27.0
|
||||||
|
max_control_gap_ms: 100.0
|
||||||
|
min_camera_fps: 27.0
|
||||||
|
max_drop_ratio: 0.01
|
||||||
|
max_feedback_age_ms: 50.0
|
||||||
|
max_camera_age_ms: 50.0
|
||||||
|
max_camera_skew_ms: 50.0
|
||||||
|
min_free_space_gib: 4.0
|
||||||
|
y_hold_sec: 1.0
|
||||||
|
gripper_completion_timeout_sec: 3.0
|
||||||
|
writer_queue_size: 8
|
||||||
@@ -28,16 +28,6 @@ left_arm_teleop:
|
|||||||
orientation_deadband_rad: 0.005
|
orientation_deadband_rad: 0.005
|
||||||
orientation_filter_alpha: 0.65
|
orientation_filter_alpha: 0.65
|
||||||
max_orientation_speed: 0.5
|
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_min: [-0.70, -0.70, 0.10]
|
||||||
workspace_max: [0.70, 0.10, 0.75]
|
workspace_max: [0.70, 0.10, 0.75]
|
||||||
cyl_radius_limit: [0.10, 0.80]
|
cyl_radius_limit: [0.10, 0.80]
|
||||||
@@ -95,15 +85,6 @@ right_arm_teleop:
|
|||||||
orientation_deadband_rad: 0.005
|
orientation_deadband_rad: 0.005
|
||||||
orientation_filter_alpha: 0.65
|
orientation_filter_alpha: 0.65
|
||||||
max_orientation_speed: 0.5
|
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_min: [-0.70, -0.70, 0.10]
|
||||||
workspace_max: [0.70, 0.10, 0.75]
|
workspace_max: [0.70, 0.10, 0.75]
|
||||||
cyl_radius_limit: [0.10, 0.80]
|
cyl_radius_limit: [0.10, 0.80]
|
||||||
|
|||||||
@@ -22,16 +22,6 @@ single_arm_velocity_teleop:
|
|||||||
orientation_deadband_rad: 0.005
|
orientation_deadband_rad: 0.005
|
||||||
orientation_filter_alpha: 0.65
|
orientation_filter_alpha: 0.65
|
||||||
max_orientation_speed: 0.5
|
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_min: [-0.70, -0.70, 0.10]
|
||||||
workspace_max: [0.70, 0.10, 0.75]
|
workspace_max: [0.70, 0.10, 0.75]
|
||||||
cyl_radius_limit: [0.10, 0.80]
|
cyl_radius_limit: [0.10, 0.80]
|
||||||
|
|||||||
@@ -27,3 +27,4 @@ arms:
|
|||||||
scissorgripper: 0
|
scissorgripper: 0
|
||||||
right:
|
right:
|
||||||
scissorgripper: 1
|
scissorgripper: 1
|
||||||
|
set_initial_tool_state: true
|
||||||
|
|||||||
@@ -21,16 +21,6 @@ single_arm_velocity_teleop:
|
|||||||
orientation_deadband_rad: 0.005
|
orientation_deadband_rad: 0.005
|
||||||
orientation_filter_alpha: 0.65
|
orientation_filter_alpha: 0.65
|
||||||
max_orientation_speed: 0.5
|
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_min: [-0.70, -0.70, 0.10]
|
||||||
workspace_max: [0.70, 0.10, 0.75]
|
workspace_max: [0.70, 0.10, 0.75]
|
||||||
cyl_radius_limit: [0.10, 0.80]
|
cyl_radius_limit: [0.10, 0.80]
|
||||||
|
|||||||
@@ -80,6 +80,29 @@ def _validate_mujoco_mode(arm: str, use_mujoco: bool) -> None:
|
|||||||
raise ValueError("use_mujoco:=true requires arm:=both")
|
raise ValueError("use_mujoco:=true requires arm:=both")
|
||||||
|
|
||||||
|
|
||||||
|
def _validate_act_mode(
|
||||||
|
arm: str,
|
||||||
|
use_mock: bool,
|
||||||
|
record_act: bool,
|
||||||
|
) -> None:
|
||||||
|
if record_act and (arm != "right" or use_mock):
|
||||||
|
raise ValueError(
|
||||||
|
"record_act:=true requires arm:=right use_mock:=false"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _act_recorder_node() -> Node:
|
||||||
|
"""创建独立 ACT 数据采集节点；退出时不终止遥操作。"""
|
||||||
|
return Node(
|
||||||
|
package="xr_rm_teleop",
|
||||||
|
executable="act_episode_recorder",
|
||||||
|
name="act_episode_recorder",
|
||||||
|
output="screen",
|
||||||
|
prefix=[XR_PYTHON],
|
||||||
|
parameters=[_config_file("act_tomato_pick.yaml")],
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
def _single_arm_node(
|
def _single_arm_node(
|
||||||
arm: str,
|
arm: str,
|
||||||
use_mock: bool,
|
use_mock: bool,
|
||||||
@@ -164,10 +187,14 @@ def _launch_setup(context, *args, **kwargs):
|
|||||||
use_mujoco = _as_bool(
|
use_mujoco = _as_bool(
|
||||||
LaunchConfiguration("use_mujoco").perform(context)
|
LaunchConfiguration("use_mujoco").perform(context)
|
||||||
)
|
)
|
||||||
|
record_act = _as_bool(
|
||||||
|
LaunchConfiguration("record_act").perform(context)
|
||||||
|
)
|
||||||
|
|
||||||
if arm not in ("left", "right", "both"):
|
if arm not in ("left", "right", "both"):
|
||||||
raise ValueError("arm must be one of: left, right, both")
|
raise ValueError("arm must be one of: left, right, both")
|
||||||
_validate_mujoco_mode(arm, use_mujoco)
|
_validate_mujoco_mode(arm, use_mujoco)
|
||||||
|
_validate_act_mode(arm, use_mock, record_act)
|
||||||
|
|
||||||
nodes = [_udp_receiver_node()]
|
nodes = [_udp_receiver_node()]
|
||||||
if arm == "both":
|
if arm == "both":
|
||||||
@@ -176,6 +203,8 @@ def _launch_setup(context, *args, **kwargs):
|
|||||||
nodes.append(_single_arm_node(arm, use_mock))
|
nodes.append(_single_arm_node(arm, use_mock))
|
||||||
if use_mujoco:
|
if use_mujoco:
|
||||||
nodes.append(_mujoco_node())
|
nodes.append(_mujoco_node())
|
||||||
|
if record_act:
|
||||||
|
nodes.append(_act_recorder_node())
|
||||||
return nodes
|
return nodes
|
||||||
|
|
||||||
|
|
||||||
@@ -187,6 +216,8 @@ def generate_launch_description() -> LaunchDescription:
|
|||||||
DeclareLaunchArgument("use_mock", default_value="true"),
|
DeclareLaunchArgument("use_mock", default_value="true"),
|
||||||
# true 时额外启动只读 MuJoCo 双臂显示，默认不改变现有启动行为。
|
# true 时额外启动只读 MuJoCo 双臂显示，默认不改变现有启动行为。
|
||||||
DeclareLaunchArgument("use_mujoco", default_value="false"),
|
DeclareLaunchArgument("use_mujoco", default_value="false"),
|
||||||
|
# true 时只允许右臂真机，并启动独立 ACT 数据采集节点。
|
||||||
|
DeclareLaunchArgument("record_act", default_value="false"),
|
||||||
# UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
|
# UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
|
||||||
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
||||||
DeclareLaunchArgument("udp_port", default_value="15000"),
|
DeclareLaunchArgument("udp_port", default_value="15000"),
|
||||||
|
|||||||
@@ -0,0 +1,2 @@
|
|||||||
|
*
|
||||||
|
!.gitignore
|
||||||
@@ -41,3 +41,37 @@ def test_udp_receiver_exit_shuts_down_launch() -> None:
|
|||||||
receiver = arm_debug_launch._udp_receiver_node()
|
receiver = arm_debug_launch._udp_receiver_node()
|
||||||
|
|
||||||
assert isinstance(receiver._ExecuteLocal__on_exit, Shutdown)
|
assert isinstance(receiver._ExecuteLocal__on_exit, Shutdown)
|
||||||
|
|
||||||
|
|
||||||
|
def test_act_recording_is_disabled_by_default() -> None:
|
||||||
|
description = arm_debug_launch.generate_launch_description()
|
||||||
|
arguments = {
|
||||||
|
entity.name: entity
|
||||||
|
for entity in description.entities
|
||||||
|
if isinstance(entity, DeclareLaunchArgument)
|
||||||
|
}
|
||||||
|
|
||||||
|
assert perform_substitutions(
|
||||||
|
LaunchContext(),
|
||||||
|
arguments["record_act"].default_value,
|
||||||
|
) == "false"
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize(
|
||||||
|
("arm", "use_mock"),
|
||||||
|
[("left", False), ("both", False), ("right", True)],
|
||||||
|
)
|
||||||
|
def test_act_recording_rejects_unsupported_modes(arm, use_mock) -> None:
|
||||||
|
with pytest.raises(ValueError, match="arm:=right use_mock:=false"):
|
||||||
|
arm_debug_launch._validate_act_mode(arm, use_mock, True)
|
||||||
|
|
||||||
|
|
||||||
|
def test_act_recording_accepts_right_real_mode() -> None:
|
||||||
|
arm_debug_launch._validate_act_mode("right", False, True)
|
||||||
|
arm_debug_launch._validate_act_mode("both", True, False)
|
||||||
|
|
||||||
|
|
||||||
|
def test_act_recorder_exit_does_not_shutdown_teleoperation() -> None:
|
||||||
|
recorder = arm_debug_launch._act_recorder_node()
|
||||||
|
|
||||||
|
assert recorder._ExecuteLocal__on_exit is None
|
||||||
|
|||||||
@@ -0,0 +1,76 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import importlib.util
|
||||||
|
from pathlib import Path
|
||||||
|
import sys
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
|
||||||
|
MODULE_PATH = (
|
||||||
|
Path(__file__).resolve().parents[1]
|
||||||
|
/ "tools"
|
||||||
|
/ "realsense_multi_camera_test.py"
|
||||||
|
)
|
||||||
|
SPEC = importlib.util.spec_from_file_location("realsense_multi_camera_test", MODULE_PATH)
|
||||||
|
assert SPEC is not None and SPEC.loader is not None
|
||||||
|
camera_test = importlib.util.module_from_spec(SPEC)
|
||||||
|
sys.modules[SPEC.name] = camera_test
|
||||||
|
SPEC.loader.exec_module(camera_test)
|
||||||
|
|
||||||
|
|
||||||
|
def devices() -> list[camera_test.DeviceInfo]:
|
||||||
|
return [
|
||||||
|
camera_test.DeviceInfo("Intel RealSense D405", "D405", "412622272532", "3.2"),
|
||||||
|
camera_test.DeviceInfo("Intel RealSense D455", "D455", "234222303366", "3.2"),
|
||||||
|
camera_test.DeviceInfo("Intel RealSense D405", "D405", "260322272273", "3.2"),
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def test_assigns_camera_roles_with_and_without_left_serial() -> None:
|
||||||
|
unidentified = camera_test.assign_camera_roles(devices(), None)
|
||||||
|
assert [camera.role for camera in unidentified] == ["GLOBAL", "D405-A", "D405-B"]
|
||||||
|
assert [camera.serial for camera in unidentified[1:]] == ["260322272273", "412622272532"]
|
||||||
|
|
||||||
|
identified = camera_test.assign_camera_roles(devices(), "412622272532")
|
||||||
|
assert {camera.role: camera.serial for camera in identified} == {
|
||||||
|
"GLOBAL": "234222303366",
|
||||||
|
"LEFT": "412622272532",
|
||||||
|
"RIGHT": "260322272273",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def test_rejects_invalid_camera_selection() -> None:
|
||||||
|
with pytest.raises(ValueError, match="左臂序列号"):
|
||||||
|
camera_test.assign_camera_roles(devices(), "missing")
|
||||||
|
|
||||||
|
with pytest.raises(ValueError, match="2 台 D405 和 1 台 D455"):
|
||||||
|
camera_test.assign_camera_roles(devices()[:-1], None)
|
||||||
|
|
||||||
|
|
||||||
|
def test_counts_frame_number_gaps() -> None:
|
||||||
|
stats = camera_test.FrameStats(target_fps=30, start_time=0.0)
|
||||||
|
stats.update(10, 0.0)
|
||||||
|
stats.update(11, 1.0 / 30.0)
|
||||||
|
stats.update(14, 2.0 / 30.0)
|
||||||
|
|
||||||
|
assert stats.received == 3
|
||||||
|
assert stats.dropped == 2
|
||||||
|
assert stats.drop_rate == pytest.approx(0.4)
|
||||||
|
assert stats.average_fps(2.0 / 30.0) == pytest.approx(30.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_snapshot_names_follow_camera_roles() -> None:
|
||||||
|
identified = camera_test.assign_camera_roles(devices(), "412622272532")
|
||||||
|
assert camera_test.snapshot_filenames(identified) == {
|
||||||
|
"234222303366": "global.png",
|
||||||
|
"412622272532": "left.png",
|
||||||
|
"260322272273": "right.png",
|
||||||
|
}
|
||||||
|
|
||||||
|
unidentified = camera_test.assign_camera_roles(devices(), None)
|
||||||
|
assert camera_test.snapshot_filenames(unidentified) == {
|
||||||
|
"234222303366": "global.png",
|
||||||
|
"260322272273": "d405_260322272273.png",
|
||||||
|
"412622272532": "d405_412622272532.png",
|
||||||
|
}
|
||||||
+494
@@ -0,0 +1,494 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""同时预览并检查两台 D405 和一台 D455 的彩色画面。"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
from collections import deque
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from datetime import datetime
|
||||||
|
from pathlib import Path
|
||||||
|
import threading
|
||||||
|
import time
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
|
||||||
|
DEFAULT_WIDTH = 640
|
||||||
|
DEFAULT_HEIGHT = 480
|
||||||
|
DEFAULT_FPS = 30
|
||||||
|
DEFAULT_LEFT_SERIAL = "260322272273"
|
||||||
|
FPS_WINDOW_SECONDS = 2.0
|
||||||
|
WARMUP_SECONDS = 5.0
|
||||||
|
MAX_DROP_RATE = 0.01
|
||||||
|
MIN_FPS_RATIO = 0.9
|
||||||
|
PREVIEW_TILE_WIDTH = 640
|
||||||
|
WINDOW_NAME = "XR RM - Three RealSense Camera Test"
|
||||||
|
OUTPUT_DIR = Path(__file__).resolve().parents[1] / "test" / "camera_test_output"
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class DeviceInfo:
|
||||||
|
name: str
|
||||||
|
model: str
|
||||||
|
serial: str
|
||||||
|
usb_type: str
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CameraAssignment:
|
||||||
|
role: str
|
||||||
|
name: str
|
||||||
|
model: str
|
||||||
|
serial: str
|
||||||
|
usb_type: str
|
||||||
|
|
||||||
|
|
||||||
|
def assign_camera_roles(
|
||||||
|
devices: list[DeviceInfo], left_serial: str | None
|
||||||
|
) -> list[CameraAssignment]:
|
||||||
|
d405 = sorted(
|
||||||
|
(device for device in devices if device.model == "D405"),
|
||||||
|
key=lambda device: device.serial,
|
||||||
|
)
|
||||||
|
d455 = [device for device in devices if device.model == "D455"]
|
||||||
|
if len(d405) != 2 or len(d455) != 1 or len(devices) != 3:
|
||||||
|
raise ValueError(
|
||||||
|
f"需要连接 2 台 D405 和 1 台 D455，当前识别到 "
|
||||||
|
f"{len(d405)} 台 D405、{len(d455)} 台 D455、共 {len(devices)} 台 RealSense"
|
||||||
|
)
|
||||||
|
|
||||||
|
for device in devices:
|
||||||
|
if not device.usb_type.startswith("3"):
|
||||||
|
raise ValueError(
|
||||||
|
f"{device.model} ({device.serial}) 当前为 USB {device.usb_type}，"
|
||||||
|
"请检查扩展坞和数据线"
|
||||||
|
)
|
||||||
|
|
||||||
|
global_camera = CameraAssignment("GLOBAL", **d455[0].__dict__)
|
||||||
|
if left_serial is None:
|
||||||
|
arms = [
|
||||||
|
CameraAssignment(f"D405-{suffix}", **device.__dict__)
|
||||||
|
for suffix, device in zip(("A", "B"), d405)
|
||||||
|
]
|
||||||
|
else:
|
||||||
|
matches = [device for device in d405 if device.serial == left_serial]
|
||||||
|
if not matches:
|
||||||
|
raise ValueError(f"左臂序列号 {left_serial} 不属于当前连接的 D405")
|
||||||
|
left = matches[0]
|
||||||
|
right = next(device for device in d405 if device.serial != left_serial)
|
||||||
|
arms = [
|
||||||
|
CameraAssignment("LEFT", **left.__dict__),
|
||||||
|
CameraAssignment("RIGHT", **right.__dict__),
|
||||||
|
]
|
||||||
|
return [global_camera, *arms]
|
||||||
|
|
||||||
|
|
||||||
|
def snapshot_filenames(assignments: list[CameraAssignment]) -> dict[str, str]:
|
||||||
|
role_names = {
|
||||||
|
"GLOBAL": "global.png",
|
||||||
|
"LEFT": "left.png",
|
||||||
|
"RIGHT": "right.png",
|
||||||
|
}
|
||||||
|
return {
|
||||||
|
camera.serial: role_names.get(camera.role, f"d405_{camera.serial}.png")
|
||||||
|
for camera in assignments
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class FrameStats:
|
||||||
|
target_fps: int
|
||||||
|
start_time: float
|
||||||
|
received: int = 0
|
||||||
|
dropped: int = 0
|
||||||
|
last_frame_number: int | None = None
|
||||||
|
first_frame_time: float | None = None
|
||||||
|
recent_times: deque[float] = field(default_factory=deque)
|
||||||
|
|
||||||
|
def update(self, frame_number: int, now: float) -> None:
|
||||||
|
if self.last_frame_number is not None and frame_number > self.last_frame_number:
|
||||||
|
self.dropped += max(0, frame_number - self.last_frame_number - 1)
|
||||||
|
self.last_frame_number = frame_number
|
||||||
|
self.received += 1
|
||||||
|
if self.first_frame_time is None:
|
||||||
|
self.first_frame_time = now
|
||||||
|
self.recent_times.append(now)
|
||||||
|
cutoff = now - FPS_WINDOW_SECONDS
|
||||||
|
while self.recent_times and self.recent_times[0] < cutoff:
|
||||||
|
self.recent_times.popleft()
|
||||||
|
|
||||||
|
@property
|
||||||
|
def drop_rate(self) -> float:
|
||||||
|
expected = self.received + self.dropped
|
||||||
|
return self.dropped / expected if expected else 0.0
|
||||||
|
|
||||||
|
@property
|
||||||
|
def rolling_fps(self) -> float:
|
||||||
|
if len(self.recent_times) < 2:
|
||||||
|
return 0.0
|
||||||
|
elapsed = self.recent_times[-1] - self.recent_times[0]
|
||||||
|
return (len(self.recent_times) - 1) / elapsed if elapsed > 0 else 0.0
|
||||||
|
|
||||||
|
def average_fps(self, now: float) -> float:
|
||||||
|
if self.received < 2 or self.first_frame_time is None:
|
||||||
|
return 0.0
|
||||||
|
last_frame_time = self.recent_times[-1] if self.recent_times else now
|
||||||
|
elapsed = last_frame_time - self.first_frame_time
|
||||||
|
return (self.received - 1) / elapsed if elapsed > 0 else 0.0
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CameraState:
|
||||||
|
frame: Any | None
|
||||||
|
actual_size: tuple[int, int]
|
||||||
|
received: int
|
||||||
|
dropped: int
|
||||||
|
drop_rate: float
|
||||||
|
rolling_fps: float
|
||||||
|
average_fps: float
|
||||||
|
elapsed: float
|
||||||
|
error: str
|
||||||
|
|
||||||
|
|
||||||
|
class CameraWorker:
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
assignment: CameraAssignment,
|
||||||
|
width: int,
|
||||||
|
height: int,
|
||||||
|
fps: int,
|
||||||
|
rs: Any,
|
||||||
|
np: Any,
|
||||||
|
) -> None:
|
||||||
|
self.assignment = assignment
|
||||||
|
self.width = width
|
||||||
|
self.height = height
|
||||||
|
self.fps = fps
|
||||||
|
self._rs = rs
|
||||||
|
self._np = np
|
||||||
|
self._pipeline = rs.pipeline()
|
||||||
|
self._stop_event = threading.Event()
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
self._thread: threading.Thread | None = None
|
||||||
|
self._frame: Any | None = None
|
||||||
|
self._actual_size = (width, height)
|
||||||
|
self._stats = FrameStats(fps, time.monotonic())
|
||||||
|
self._error = ""
|
||||||
|
|
||||||
|
def start(self) -> None:
|
||||||
|
config = self._rs.config()
|
||||||
|
config.enable_device(self.assignment.serial)
|
||||||
|
config.enable_stream(
|
||||||
|
self._rs.stream.color,
|
||||||
|
self.width,
|
||||||
|
self.height,
|
||||||
|
self._rs.format.bgr8,
|
||||||
|
self.fps,
|
||||||
|
)
|
||||||
|
profile = self._pipeline.start(config)
|
||||||
|
video_profile = profile.get_stream(self._rs.stream.color).as_video_stream_profile()
|
||||||
|
with self._lock:
|
||||||
|
self._actual_size = (video_profile.width(), video_profile.height())
|
||||||
|
self._stats = FrameStats(self.fps, time.monotonic())
|
||||||
|
self._thread = threading.Thread(
|
||||||
|
target=self._capture_loop,
|
||||||
|
name=f"camera-{self.assignment.serial}",
|
||||||
|
daemon=True,
|
||||||
|
)
|
||||||
|
self._thread.start()
|
||||||
|
|
||||||
|
def _capture_loop(self) -> None:
|
||||||
|
while not self._stop_event.is_set():
|
||||||
|
try:
|
||||||
|
frames = self._pipeline.wait_for_frames(timeout_ms=1000)
|
||||||
|
except RuntimeError as exc:
|
||||||
|
if self._stop_event.is_set():
|
||||||
|
return
|
||||||
|
with self._lock:
|
||||||
|
self._error = str(exc)
|
||||||
|
return
|
||||||
|
|
||||||
|
color_frame = frames.get_color_frame()
|
||||||
|
if not color_frame:
|
||||||
|
continue
|
||||||
|
frame = self._np.asanyarray(color_frame.get_data()).copy()
|
||||||
|
now = time.monotonic()
|
||||||
|
with self._lock:
|
||||||
|
self._frame = frame
|
||||||
|
self._stats.update(color_frame.get_frame_number(), now)
|
||||||
|
|
||||||
|
def state(self, now: float) -> CameraState:
|
||||||
|
with self._lock:
|
||||||
|
return CameraState(
|
||||||
|
frame=self._frame,
|
||||||
|
actual_size=self._actual_size,
|
||||||
|
received=self._stats.received,
|
||||||
|
dropped=self._stats.dropped,
|
||||||
|
drop_rate=self._stats.drop_rate,
|
||||||
|
rolling_fps=self._stats.rolling_fps,
|
||||||
|
average_fps=self._stats.average_fps(now),
|
||||||
|
elapsed=now - self._stats.start_time,
|
||||||
|
error=self._error,
|
||||||
|
)
|
||||||
|
|
||||||
|
def stop(self) -> None:
|
||||||
|
self._stop_event.set()
|
||||||
|
if self._thread is not None:
|
||||||
|
self._thread.join(timeout=1.2)
|
||||||
|
try:
|
||||||
|
self._pipeline.stop()
|
||||||
|
except RuntimeError:
|
||||||
|
pass
|
||||||
|
if self._thread is not None and self._thread.is_alive():
|
||||||
|
self._thread.join(timeout=1.0)
|
||||||
|
|
||||||
|
|
||||||
|
def load_runtime_dependencies() -> tuple[Any, Any, Any]:
|
||||||
|
try:
|
||||||
|
import cv2
|
||||||
|
import numpy as np
|
||||||
|
import pyrealsense2 as rs
|
||||||
|
except ImportError as exc:
|
||||||
|
raise RuntimeError(
|
||||||
|
"缺少相机测试依赖。请使用 /home/robot/miniconda3/envs/xr/bin/python "
|
||||||
|
"运行，并确认 xr 环境已安装 pyrealsense2、numpy 和 opencv-python。"
|
||||||
|
) from exc
|
||||||
|
return cv2, np, rs
|
||||||
|
|
||||||
|
|
||||||
|
def enumerate_devices(rs: Any) -> list[DeviceInfo]:
|
||||||
|
devices = []
|
||||||
|
for device in rs.context().query_devices():
|
||||||
|
name = device.get_info(rs.camera_info.name)
|
||||||
|
if "D405" in name:
|
||||||
|
model = "D405"
|
||||||
|
elif "D455" in name:
|
||||||
|
model = "D455"
|
||||||
|
else:
|
||||||
|
model = name
|
||||||
|
usb_type = (
|
||||||
|
device.get_info(rs.camera_info.usb_type_descriptor)
|
||||||
|
if device.supports(rs.camera_info.usb_type_descriptor)
|
||||||
|
else "unknown"
|
||||||
|
)
|
||||||
|
devices.append(
|
||||||
|
DeviceInfo(
|
||||||
|
name=name,
|
||||||
|
model=model,
|
||||||
|
serial=device.get_info(rs.camera_info.serial_number),
|
||||||
|
usb_type=usb_type,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return devices
|
||||||
|
|
||||||
|
|
||||||
|
def camera_status(state: CameraState, target_fps: int) -> tuple[str, tuple[int, int, int]]:
|
||||||
|
if state.error:
|
||||||
|
return "ERROR", (0, 0, 255)
|
||||||
|
if state.received == 0:
|
||||||
|
return "WAITING", (0, 215, 255)
|
||||||
|
if state.elapsed < WARMUP_SECONDS:
|
||||||
|
return "WARMUP", (0, 215, 255)
|
||||||
|
if state.rolling_fps >= target_fps * MIN_FPS_RATIO and state.drop_rate <= MAX_DROP_RATE:
|
||||||
|
return "PASS", (0, 200, 0)
|
||||||
|
return "FAIL", (0, 0, 255)
|
||||||
|
|
||||||
|
|
||||||
|
def render_tile(
|
||||||
|
worker: CameraWorker,
|
||||||
|
state: CameraState,
|
||||||
|
tile_width: int,
|
||||||
|
tile_height: int,
|
||||||
|
cv2: Any,
|
||||||
|
np: Any,
|
||||||
|
) -> Any:
|
||||||
|
if state.frame is None:
|
||||||
|
tile = np.zeros((tile_height, tile_width, 3), dtype=np.uint8)
|
||||||
|
else:
|
||||||
|
tile = cv2.resize(state.frame, (tile_width, tile_height))
|
||||||
|
|
||||||
|
status, color = camera_status(state, worker.fps)
|
||||||
|
cv2.rectangle(tile, (0, 0), (tile_width, 100), (0, 0, 0), -1)
|
||||||
|
width, height = state.actual_size
|
||||||
|
lines = [
|
||||||
|
f"{worker.assignment.role} {worker.assignment.model} {worker.assignment.serial}",
|
||||||
|
f"USB {worker.assignment.usb_type} {width}x{height}@{worker.fps}",
|
||||||
|
f"FPS {state.rolling_fps:.1f} Frames {state.received} "
|
||||||
|
f"Dropped {state.dropped} ({state.drop_rate:.2%})",
|
||||||
|
status if not state.error else f"ERROR: {state.error[:70]}",
|
||||||
|
]
|
||||||
|
for index, line in enumerate(lines):
|
||||||
|
cv2.putText(
|
||||||
|
tile,
|
||||||
|
line,
|
||||||
|
(10, 22 + index * 24),
|
||||||
|
cv2.FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.55,
|
||||||
|
color if index == len(lines) - 1 else (255, 255, 255),
|
||||||
|
1,
|
||||||
|
cv2.LINE_AA,
|
||||||
|
)
|
||||||
|
return tile
|
||||||
|
|
||||||
|
|
||||||
|
def compose_preview(
|
||||||
|
workers: list[CameraWorker],
|
||||||
|
states: dict[str, CameraState],
|
||||||
|
capture_width: int,
|
||||||
|
capture_height: int,
|
||||||
|
cv2: Any,
|
||||||
|
np: Any,
|
||||||
|
) -> Any:
|
||||||
|
tile_width = min(capture_width, PREVIEW_TILE_WIDTH)
|
||||||
|
tile_height = round(tile_width * capture_height / capture_width)
|
||||||
|
tiles = {
|
||||||
|
worker.assignment.serial: render_tile(
|
||||||
|
worker,
|
||||||
|
states[worker.assignment.serial],
|
||||||
|
tile_width,
|
||||||
|
tile_height,
|
||||||
|
cv2,
|
||||||
|
np,
|
||||||
|
)
|
||||||
|
for worker in workers
|
||||||
|
}
|
||||||
|
global_worker = next(worker for worker in workers if worker.assignment.role == "GLOBAL")
|
||||||
|
arm_workers = [worker for worker in workers if worker.assignment.role != "GLOBAL"]
|
||||||
|
|
||||||
|
top = np.zeros((tile_height, tile_width * 2, 3), dtype=np.uint8)
|
||||||
|
offset = tile_width // 2
|
||||||
|
top[:, offset : offset + tile_width] = tiles[global_worker.assignment.serial]
|
||||||
|
bottom = np.hstack([tiles[worker.assignment.serial] for worker in arm_workers])
|
||||||
|
return np.vstack((top, bottom))
|
||||||
|
|
||||||
|
|
||||||
|
def save_snapshots(
|
||||||
|
assignments: list[CameraAssignment],
|
||||||
|
states: dict[str, CameraState],
|
||||||
|
cv2: Any,
|
||||||
|
) -> Path:
|
||||||
|
missing = [camera.role for camera in assignments if states[camera.serial].frame is None]
|
||||||
|
if missing:
|
||||||
|
raise RuntimeError(f"以下相机尚无有效画面，不能保存快照: {', '.join(missing)}")
|
||||||
|
|
||||||
|
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f")[:-3]
|
||||||
|
snapshot_dir = OUTPUT_DIR / timestamp
|
||||||
|
snapshot_dir.mkdir(parents=True, exist_ok=False)
|
||||||
|
filenames = snapshot_filenames(assignments)
|
||||||
|
for camera in assignments:
|
||||||
|
path = snapshot_dir / filenames[camera.serial]
|
||||||
|
if not cv2.imwrite(str(path), states[camera.serial].frame):
|
||||||
|
raise RuntimeError(f"保存快照失败: {path}")
|
||||||
|
return snapshot_dir
|
||||||
|
|
||||||
|
|
||||||
|
def print_summary(workers: list[CameraWorker]) -> bool:
|
||||||
|
now = time.monotonic()
|
||||||
|
print("\n相机测试汇总:")
|
||||||
|
passed = True
|
||||||
|
for worker in workers:
|
||||||
|
state = worker.state(now)
|
||||||
|
status, _color = camera_status(state, worker.fps)
|
||||||
|
passed = passed and status == "PASS"
|
||||||
|
print(
|
||||||
|
f" {worker.assignment.role:<7} {worker.assignment.serial}: "
|
||||||
|
f"平均 {state.average_fps:.1f} FPS, 接收 {state.received}, "
|
||||||
|
f"掉帧 {state.dropped} ({state.drop_rate:.2%}), {status}"
|
||||||
|
)
|
||||||
|
print("结论: " + ("三路链路满足当前阈值" if passed else "至少一路未满足当前阈值"))
|
||||||
|
return passed
|
||||||
|
|
||||||
|
|
||||||
|
def parse_args() -> argparse.Namespace:
|
||||||
|
parser = argparse.ArgumentParser(description=__doc__)
|
||||||
|
parser.add_argument("--width", type=int, default=DEFAULT_WIDTH, help="采集宽度")
|
||||||
|
parser.add_argument("--height", type=int, default=DEFAULT_HEIGHT, help="采集高度")
|
||||||
|
parser.add_argument("--fps", type=int, default=DEFAULT_FPS, help="目标帧率")
|
||||||
|
parser.add_argument(
|
||||||
|
"--left-serial",
|
||||||
|
default=DEFAULT_LEFT_SERIAL,
|
||||||
|
help=f"左臂 D405 的 RealSense 序列号（默认: {DEFAULT_LEFT_SERIAL}）",
|
||||||
|
)
|
||||||
|
args = parser.parse_args()
|
||||||
|
if args.width <= 0 or args.height <= 0 or args.fps <= 0:
|
||||||
|
parser.error("width、height 和 fps 必须为正数")
|
||||||
|
return args
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
args = parse_args()
|
||||||
|
try:
|
||||||
|
cv2, np, rs = load_runtime_dependencies()
|
||||||
|
assignments = assign_camera_roles(enumerate_devices(rs), args.left_serial)
|
||||||
|
except RuntimeError as exc:
|
||||||
|
print(f"错误: {exc}")
|
||||||
|
return 2
|
||||||
|
except ValueError as exc:
|
||||||
|
print(f"设备检查失败: {exc}")
|
||||||
|
return 2
|
||||||
|
|
||||||
|
print("相机分配:")
|
||||||
|
for camera in assignments:
|
||||||
|
print(
|
||||||
|
f" {camera.role:<7} {camera.model} serial={camera.serial} "
|
||||||
|
f"USB={camera.usb_type}"
|
||||||
|
)
|
||||||
|
|
||||||
|
workers: list[CameraWorker] = []
|
||||||
|
passed = False
|
||||||
|
try:
|
||||||
|
for assignment in assignments:
|
||||||
|
worker = CameraWorker(
|
||||||
|
assignment,
|
||||||
|
args.width,
|
||||||
|
args.height,
|
||||||
|
args.fps,
|
||||||
|
rs,
|
||||||
|
np,
|
||||||
|
)
|
||||||
|
workers.append(worker)
|
||||||
|
worker.start()
|
||||||
|
|
||||||
|
cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL)
|
||||||
|
print("按 S 保存三路快照，按 Q 或 Esc 退出。")
|
||||||
|
while True:
|
||||||
|
now = time.monotonic()
|
||||||
|
states = {worker.assignment.serial: worker.state(now) for worker in workers}
|
||||||
|
preview = compose_preview(
|
||||||
|
workers,
|
||||||
|
states,
|
||||||
|
args.width,
|
||||||
|
args.height,
|
||||||
|
cv2,
|
||||||
|
np,
|
||||||
|
)
|
||||||
|
cv2.imshow(WINDOW_NAME, preview)
|
||||||
|
key = cv2.waitKey(1) & 0xFF
|
||||||
|
if key in (ord("q"), ord("Q"), 27):
|
||||||
|
break
|
||||||
|
if key in (ord("s"), ord("S")):
|
||||||
|
try:
|
||||||
|
output = save_snapshots(assignments, states, cv2)
|
||||||
|
print(f"快照已保存: {output}")
|
||||||
|
except RuntimeError as exc:
|
||||||
|
print(f"快照失败: {exc}")
|
||||||
|
if cv2.getWindowProperty(WINDOW_NAME, cv2.WND_PROP_VISIBLE) < 1:
|
||||||
|
break
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("\n收到 Ctrl+C，正在停止相机。")
|
||||||
|
except Exception as exc:
|
||||||
|
print(f"相机启动或显示失败: {exc}")
|
||||||
|
finally:
|
||||||
|
for worker in reversed(workers):
|
||||||
|
worker.stop()
|
||||||
|
try:
|
||||||
|
cv2.destroyAllWindows()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
if workers:
|
||||||
|
passed = print_summary(workers)
|
||||||
|
return 0 if passed else 1
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED)
|
|||||||
find_package(std_msgs REQUIRED)
|
find_package(std_msgs REQUIRED)
|
||||||
|
|
||||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||||
|
"msg/ActControlSample.msg"
|
||||||
"msg/XrController.msg"
|
"msg/XrController.msg"
|
||||||
DEPENDENCIES geometry_msgs std_msgs
|
DEPENDENCIES geometry_msgs std_msgs
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -0,0 +1,41 @@
|
|||||||
|
std_msgs/Header header
|
||||||
|
uint64 control_seq
|
||||||
|
int64 control_monotonic_ns
|
||||||
|
int64 feedback_monotonic_ns
|
||||||
|
int64 action_monotonic_ns
|
||||||
|
|
||||||
|
float32 feedback_age_ms
|
||||||
|
float32 qp_duration_ms
|
||||||
|
|
||||||
|
float64[7] q_actual
|
||||||
|
float64[7] q_qp_raw
|
||||||
|
float64[7] q_target
|
||||||
|
float64[7] joint_lower_limits
|
||||||
|
float64[7] joint_upper_limits
|
||||||
|
|
||||||
|
geometry_msgs/Pose tcp_current
|
||||||
|
geometry_msgs/Pose tcp_raw_target
|
||||||
|
geometry_msgs/Pose tcp_target
|
||||||
|
geometry_msgs/Twist tcp_command_velocity
|
||||||
|
geometry_msgs/Pose pico_pose
|
||||||
|
|
||||||
|
bool pico_grip
|
||||||
|
float32 pico_trigger
|
||||||
|
bool pico_primary
|
||||||
|
bool pico_secondary
|
||||||
|
float32[2] pico_axis
|
||||||
|
|
||||||
|
bool gripper_target_open
|
||||||
|
bool gripper_state_open
|
||||||
|
bool gripper_state_known
|
||||||
|
bool gripper_command_pending
|
||||||
|
bool gripper_command_failed
|
||||||
|
|
||||||
|
bool teleop_active
|
||||||
|
bool feedback_valid
|
||||||
|
bool action_valid
|
||||||
|
bool command_sent
|
||||||
|
bool qp_attempted
|
||||||
|
bool qp_success
|
||||||
|
bool target_clamped
|
||||||
|
bool control_fault
|
||||||
@@ -55,7 +55,9 @@ setup(
|
|||||||
tests_require=["pytest"],
|
tests_require=["pytest"],
|
||||||
entry_points={
|
entry_points={
|
||||||
"console_scripts": [
|
"console_scripts": [
|
||||||
"single_arm_velocity_teleop = xr_rm_teleop.single_arm_velocity_teleop:main",
|
"act_episode_recorder = xr_rm_teleop.act_episode_recorder:main",
|
||||||
|
"single_arm_velocity_teleop = "
|
||||||
|
"xr_rm_teleop.single_arm_velocity_teleop:main",
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
)
|
)
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,194 @@
|
|||||||
|
from types import SimpleNamespace
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pytest
|
||||||
|
from builtin_interfaces.msg import Time as TimeMsg
|
||||||
|
|
||||||
|
from xr_rm_interfaces.msg import XrController
|
||||||
|
from xr_rm_teleop.single_arm_velocity_teleop import (
|
||||||
|
SingleArmVelocityTeleop,
|
||||||
|
_ActCycleContext,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
class FakePublisher:
|
||||||
|
def __init__(self, error=None) -> None:
|
||||||
|
self.error = error
|
||||||
|
self.messages = []
|
||||||
|
|
||||||
|
def publish(self, message) -> None:
|
||||||
|
if self.error is not None:
|
||||||
|
raise self.error
|
||||||
|
self.messages.append(message)
|
||||||
|
|
||||||
|
|
||||||
|
class FakeLogger:
|
||||||
|
def __init__(self) -> None:
|
||||||
|
self.warnings = []
|
||||||
|
|
||||||
|
def warn(self, message, **kwargs) -> None:
|
||||||
|
del kwargs
|
||||||
|
self.warnings.append(message)
|
||||||
|
|
||||||
|
|
||||||
|
def _controller(*, grip=True) -> XrController:
|
||||||
|
message = XrController()
|
||||||
|
message.hand = "right"
|
||||||
|
message.grip = grip
|
||||||
|
message.trigger = 0.25
|
||||||
|
message.primary = False
|
||||||
|
message.secondary = False
|
||||||
|
message.axis = [0.1, -0.2]
|
||||||
|
message.pose.orientation.w = 1.0
|
||||||
|
return message
|
||||||
|
|
||||||
|
|
||||||
|
def _teleop(*, last_target=None, tool_state=True):
|
||||||
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||||
|
teleop._arm_name = "right_rm75"
|
||||||
|
teleop._last_msg = _controller()
|
||||||
|
teleop._active = True
|
||||||
|
teleop._control_fault_latched = False
|
||||||
|
teleop._last_current_pose = np.eye(4)
|
||||||
|
teleop._robot_start_transform = None
|
||||||
|
teleop._last_sent_target = None
|
||||||
|
teleop._last_sent_orientation = None
|
||||||
|
teleop._last_successful_action_target = last_target
|
||||||
|
teleop._ik_solver = SimpleNamespace(
|
||||||
|
joint_position_limits=np.asarray([[-1.0, 1.0]] * 7)
|
||||||
|
)
|
||||||
|
teleop._tool_state_snapshot = lambda: (
|
||||||
|
True,
|
||||||
|
tool_state,
|
||||||
|
False,
|
||||||
|
False,
|
||||||
|
)
|
||||||
|
teleop.get_clock = lambda: SimpleNamespace(
|
||||||
|
now=lambda: SimpleNamespace(to_msg=lambda: TimeMsg())
|
||||||
|
)
|
||||||
|
teleop.get_logger = lambda: FakeLogger()
|
||||||
|
return teleop
|
||||||
|
|
||||||
|
|
||||||
|
def _cycle(**overrides) -> _ActCycleContext:
|
||||||
|
values = {
|
||||||
|
"control_seq": 100,
|
||||||
|
"control_monotonic_ns": 1_000_000_000,
|
||||||
|
"feedback_monotonic_ns": 990_000_000,
|
||||||
|
"action_monotonic_ns": 1_005_000_000,
|
||||||
|
"feedback_age_ms": 10.0,
|
||||||
|
"q_actual": [0.1] * 7,
|
||||||
|
"q_qp_raw": [0.3] * 7,
|
||||||
|
"q_target": [0.2] * 7,
|
||||||
|
"current_pose": np.eye(4),
|
||||||
|
"raw_target_pose": np.eye(4),
|
||||||
|
"target_pose": np.eye(4),
|
||||||
|
"command_velocity": [0.0] * 6,
|
||||||
|
"feedback_valid": True,
|
||||||
|
"command_sent": True,
|
||||||
|
"qp_attempted": True,
|
||||||
|
"qp_success": True,
|
||||||
|
}
|
||||||
|
values.update(overrides)
|
||||||
|
return _ActCycleContext(**values)
|
||||||
|
|
||||||
|
|
||||||
|
def test_act_sample_uses_feedback_and_limited_target_from_one_cycle() -> None:
|
||||||
|
teleop = _teleop(last_target=[0.2] * 7)
|
||||||
|
|
||||||
|
message = teleop._build_act_control_sample(_cycle())
|
||||||
|
|
||||||
|
assert message.control_seq == 100
|
||||||
|
assert message.q_actual == pytest.approx([0.1] * 7)
|
||||||
|
assert message.q_qp_raw == pytest.approx([0.3] * 7)
|
||||||
|
assert message.q_target == pytest.approx([0.2] * 7)
|
||||||
|
assert message.joint_lower_limits == pytest.approx([-1.0] * 7)
|
||||||
|
assert message.joint_upper_limits == pytest.approx([1.0] * 7)
|
||||||
|
assert message.command_sent
|
||||||
|
assert message.action_valid
|
||||||
|
assert message.qp_attempted
|
||||||
|
assert message.qp_success
|
||||||
|
|
||||||
|
|
||||||
|
def test_act_sample_marks_qp_fallback_as_valid_held_action() -> None:
|
||||||
|
teleop = _teleop(last_target=[0.2] * 7)
|
||||||
|
cycle = _cycle(
|
||||||
|
q_qp_raw=[0.2] * 7,
|
||||||
|
q_target=[0.2] * 7,
|
||||||
|
qp_success=False,
|
||||||
|
)
|
||||||
|
|
||||||
|
message = teleop._build_act_control_sample(cycle)
|
||||||
|
|
||||||
|
assert message.q_target == pytest.approx([0.2] * 7)
|
||||||
|
assert message.action_valid
|
||||||
|
assert message.qp_attempted
|
||||||
|
assert not message.qp_success
|
||||||
|
|
||||||
|
|
||||||
|
def test_act_sample_holds_last_action_while_grip_is_released() -> None:
|
||||||
|
teleop = _teleop(last_target=[0.4] * 7)
|
||||||
|
teleop._last_msg = _controller(grip=False)
|
||||||
|
teleop._active = False
|
||||||
|
cycle = _cycle(
|
||||||
|
q_qp_raw=None,
|
||||||
|
q_target=None,
|
||||||
|
command_sent=False,
|
||||||
|
qp_attempted=False,
|
||||||
|
qp_success=False,
|
||||||
|
action_monotonic_ns=-1,
|
||||||
|
)
|
||||||
|
|
||||||
|
message = teleop._build_act_control_sample(cycle)
|
||||||
|
|
||||||
|
assert message.q_target == pytest.approx([0.4] * 7)
|
||||||
|
assert message.action_valid
|
||||||
|
assert not message.command_sent
|
||||||
|
assert not message.teleop_active
|
||||||
|
|
||||||
|
|
||||||
|
def test_act_sample_marks_send_failure_invalid() -> None:
|
||||||
|
teleop = _teleop(last_target=[0.4] * 7)
|
||||||
|
|
||||||
|
message = teleop._build_act_control_sample(
|
||||||
|
_cycle(send_failed=True, command_sent=False)
|
||||||
|
)
|
||||||
|
|
||||||
|
assert not message.action_valid
|
||||||
|
|
||||||
|
|
||||||
|
def test_act_sample_marks_unknown_gripper_state() -> None:
|
||||||
|
teleop = _teleop(last_target=[0.2] * 7, tool_state=None)
|
||||||
|
|
||||||
|
message = teleop._build_act_control_sample(_cycle())
|
||||||
|
|
||||||
|
assert not message.gripper_state_known
|
||||||
|
|
||||||
|
|
||||||
|
def test_act_sample_publish_failure_does_not_escape_control_path() -> None:
|
||||||
|
teleop = _teleop(last_target=[0.2] * 7)
|
||||||
|
logger = FakeLogger()
|
||||||
|
teleop._act_sample_pub = FakePublisher(RuntimeError("dds failed"))
|
||||||
|
teleop.get_logger = lambda: logger
|
||||||
|
|
||||||
|
teleop._publish_act_control_sample(_cycle())
|
||||||
|
|
||||||
|
assert logger.warnings == [
|
||||||
|
"right_rm75 ACT原子样本发布失败：dds failed"
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def test_control_tick_wraps_one_impl_call_in_one_atomic_sample() -> None:
|
||||||
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||||
|
cycles = []
|
||||||
|
published = []
|
||||||
|
teleop._act_control_seq = 7
|
||||||
|
teleop._control_tick_impl = lambda cycle: cycles.append(cycle)
|
||||||
|
teleop._publish_act_control_sample = lambda cycle: published.append(cycle)
|
||||||
|
|
||||||
|
teleop._control_tick()
|
||||||
|
|
||||||
|
assert len(cycles) == 1
|
||||||
|
assert published == cycles
|
||||||
|
assert cycles[0].control_seq == 7
|
||||||
|
assert teleop._act_control_seq == 8
|
||||||
@@ -0,0 +1,882 @@
|
|||||||
|
import queue
|
||||||
|
import threading
|
||||||
|
from types import SimpleNamespace
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
try:
|
||||||
|
import h5py
|
||||||
|
except ImportError:
|
||||||
|
h5py = None
|
||||||
|
|
||||||
|
from xr_rm_interfaces.msg import ActControlSample, XrController
|
||||||
|
from xr_rm_teleop.act_episode_recorder import (
|
||||||
|
NO_ACTION,
|
||||||
|
ActEpisodeRecorder,
|
||||||
|
ButtonTracker,
|
||||||
|
CameraBuffer,
|
||||||
|
CameraFrame,
|
||||||
|
EpisodeMetadata,
|
||||||
|
EpisodeStore,
|
||||||
|
EpisodeWriter,
|
||||||
|
QualityError,
|
||||||
|
QualityLimits,
|
||||||
|
RecordingSession,
|
||||||
|
RecordingState,
|
||||||
|
TaskDirectoryLock,
|
||||||
|
discard_partial,
|
||||||
|
next_episode_index,
|
||||||
|
publish_without_overwrite,
|
||||||
|
recover_partial_files,
|
||||||
|
select_camera_pair,
|
||||||
|
select_frame,
|
||||||
|
validate_episode,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _recording_session(*, origin_seq=10, max_samples=1800):
|
||||||
|
session = RecordingSession(max_samples=max_samples)
|
||||||
|
session.arm()
|
||||||
|
decision = session.on_control(
|
||||||
|
origin_seq,
|
||||||
|
grip=True,
|
||||||
|
action_valid=True,
|
||||||
|
command_sent=True,
|
||||||
|
)
|
||||||
|
assert decision.record_sample
|
||||||
|
return session
|
||||||
|
|
||||||
|
|
||||||
|
def test_recording_starts_on_first_sent_grip_action_and_downsamples():
|
||||||
|
session = RecordingSession(max_samples=1800)
|
||||||
|
session.arm()
|
||||||
|
|
||||||
|
assert session.on_control(
|
||||||
|
100,
|
||||||
|
grip=False,
|
||||||
|
action_valid=True,
|
||||||
|
command_sent=False,
|
||||||
|
) == NO_ACTION
|
||||||
|
assert session.on_control(
|
||||||
|
101,
|
||||||
|
grip=True,
|
||||||
|
action_valid=True,
|
||||||
|
command_sent=False,
|
||||||
|
) == NO_ACTION
|
||||||
|
first = session.on_control(
|
||||||
|
102,
|
||||||
|
grip=True,
|
||||||
|
action_valid=True,
|
||||||
|
command_sent=True,
|
||||||
|
)
|
||||||
|
second = session.on_control(
|
||||||
|
103,
|
||||||
|
grip=True,
|
||||||
|
action_valid=True,
|
||||||
|
command_sent=True,
|
||||||
|
)
|
||||||
|
session.on_control(
|
||||||
|
104,
|
||||||
|
grip=True,
|
||||||
|
action_valid=True,
|
||||||
|
command_sent=True,
|
||||||
|
)
|
||||||
|
third = session.on_control(
|
||||||
|
105,
|
||||||
|
grip=True,
|
||||||
|
action_valid=True,
|
||||||
|
command_sent=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert first.record_sample
|
||||||
|
assert not second.record_sample
|
||||||
|
assert third.record_sample
|
||||||
|
assert session.sample_origin_seq == 102
|
||||||
|
|
||||||
|
|
||||||
|
def test_final_grip_release_marks_crop_point_but_mid_pause_is_kept():
|
||||||
|
session = _recording_session(origin_seq=10)
|
||||||
|
session.on_control(11, grip=True, action_valid=True, command_sent=True)
|
||||||
|
session.on_control(12, grip=True, action_valid=True, command_sent=True)
|
||||||
|
session.on_control(13, grip=False, action_valid=True, command_sent=False)
|
||||||
|
first_crop = session.candidate_end_count
|
||||||
|
|
||||||
|
assert first_crop == 2
|
||||||
|
|
||||||
|
session.on_control(14, grip=True, action_valid=True, command_sent=False)
|
||||||
|
assert session.candidate_end_count is None
|
||||||
|
session.on_control(15, grip=True, action_valid=True, command_sent=True)
|
||||||
|
session.on_control(16, grip=False, action_valid=True, command_sent=False)
|
||||||
|
|
||||||
|
assert session.candidate_end_count == 3
|
||||||
|
|
||||||
|
|
||||||
|
def test_finish_waits_for_control_sample_after_b_request():
|
||||||
|
session = _recording_session(origin_seq=10)
|
||||||
|
session.on_control(11, grip=False, action_valid=True, command_sent=False)
|
||||||
|
session.request_finish()
|
||||||
|
|
||||||
|
decision = session.on_control(
|
||||||
|
12,
|
||||||
|
grip=False,
|
||||||
|
action_valid=True,
|
||||||
|
command_sent=False,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert decision.finish
|
||||||
|
assert session.candidate_end_count == 1
|
||||||
|
|
||||||
|
|
||||||
|
def test_missing_control_sequence_rejects_recording():
|
||||||
|
session = _recording_session(origin_seq=10)
|
||||||
|
session.on_control(11, grip=True, action_valid=True, command_sent=True)
|
||||||
|
|
||||||
|
decision = session.on_control(
|
||||||
|
13,
|
||||||
|
grip=True,
|
||||||
|
action_valid=True,
|
||||||
|
command_sent=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert decision.reject_reason == "control_sequence_gap"
|
||||||
|
|
||||||
|
|
||||||
|
def test_max_samples_rejects_without_stopping_robot():
|
||||||
|
session = _recording_session(origin_seq=10, max_samples=2)
|
||||||
|
session.on_control(11, grip=True, action_valid=True, command_sent=True)
|
||||||
|
session.on_control(12, grip=True, action_valid=True, command_sent=True)
|
||||||
|
|
||||||
|
decision = session.on_control(
|
||||||
|
13,
|
||||||
|
grip=True,
|
||||||
|
action_valid=True,
|
||||||
|
command_sent=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert decision.reject_reason == "max_duration"
|
||||||
|
|
||||||
|
|
||||||
|
def test_right_b_is_ignored_while_grip_is_pressed():
|
||||||
|
tracker = ButtonTracker(hold_ns=1_000_000_000)
|
||||||
|
tracker.on_right(False, False, False, 0, RecordingState.IDLE)
|
||||||
|
|
||||||
|
events = tracker.on_right(
|
||||||
|
False,
|
||||||
|
True,
|
||||||
|
True,
|
||||||
|
1,
|
||||||
|
RecordingState.IDLE,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert not events.right_b
|
||||||
|
|
||||||
|
|
||||||
|
def test_left_y_requires_new_press_inside_active_recording_state():
|
||||||
|
tracker = ButtonTracker(hold_ns=1_000_000_000)
|
||||||
|
tracker.on_left(True, 0, RecordingState.IDLE)
|
||||||
|
assert not tracker.on_left(
|
||||||
|
True,
|
||||||
|
2_000_000_000,
|
||||||
|
RecordingState.RECORDING,
|
||||||
|
).discard
|
||||||
|
tracker.on_left(False, 2_100_000_000, RecordingState.RECORDING)
|
||||||
|
tracker.on_left(True, 3_000_000_000, RecordingState.RECORDING)
|
||||||
|
|
||||||
|
assert not tracker.on_left(
|
||||||
|
True,
|
||||||
|
3_999_999_999,
|
||||||
|
RecordingState.RECORDING,
|
||||||
|
).discard
|
||||||
|
assert tracker.on_left(
|
||||||
|
True,
|
||||||
|
4_000_000_000,
|
||||||
|
RecordingState.RECORDING,
|
||||||
|
).discard
|
||||||
|
assert not tracker.on_left(
|
||||||
|
True,
|
||||||
|
5_000_000_000,
|
||||||
|
RecordingState.RECORDING,
|
||||||
|
).discard
|
||||||
|
|
||||||
|
|
||||||
|
def test_right_a_reports_recording_rejection_event():
|
||||||
|
tracker = ButtonTracker(hold_ns=1_000_000_000)
|
||||||
|
tracker.on_right(False, False, False, 0, RecordingState.RECORDING)
|
||||||
|
|
||||||
|
events = tracker.on_right(
|
||||||
|
True,
|
||||||
|
False,
|
||||||
|
False,
|
||||||
|
1,
|
||||||
|
RecordingState.RECORDING,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert events.reject_reason == "initial_pose_command_during_episode"
|
||||||
|
|
||||||
|
|
||||||
|
def _image(value, *, shape=(480, 640, 3), dtype=np.uint8):
|
||||||
|
return np.full(shape, value, dtype=dtype)
|
||||||
|
|
||||||
|
|
||||||
|
def test_select_frame_returns_latest_frame_not_after_control_time():
|
||||||
|
frames = (
|
||||||
|
CameraFrame(_image(1), 10, 100.0, 900_000_000),
|
||||||
|
CameraFrame(_image(2), 11, 133.3, 933_000_000),
|
||||||
|
CameraFrame(_image(3), 12, 166.6, 1_010_000_000),
|
||||||
|
)
|
||||||
|
|
||||||
|
selected, age_ms = select_frame(frames, 1_000_000_000, 100.0)
|
||||||
|
|
||||||
|
assert selected.frame_number == 11
|
||||||
|
assert age_ms == pytest.approx(67.0, abs=0.1)
|
||||||
|
|
||||||
|
|
||||||
|
def test_select_frame_rejects_missing_old_and_invalid_images():
|
||||||
|
with pytest.raises(QualityError, match="camera_frame_missing"):
|
||||||
|
select_frame((), 1_000_000_000, 50.0)
|
||||||
|
with pytest.raises(QualityError, match="camera_frame_too_old"):
|
||||||
|
select_frame(
|
||||||
|
(CameraFrame(_image(1), 10, 100.0, 900_000_000),),
|
||||||
|
1_000_000_000,
|
||||||
|
50.0,
|
||||||
|
)
|
||||||
|
with pytest.raises(QualityError, match="camera_frame_format"):
|
||||||
|
select_frame(
|
||||||
|
(
|
||||||
|
CameraFrame(
|
||||||
|
_image(1, shape=(10, 10, 3)),
|
||||||
|
10,
|
||||||
|
100.0,
|
||||||
|
990_000_000,
|
||||||
|
),
|
||||||
|
),
|
||||||
|
1_000_000_000,
|
||||||
|
50.0,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_select_camera_pair_rejects_inter_camera_skew():
|
||||||
|
high = (CameraFrame(_image(1), 10, 100.0, 990_000_000),)
|
||||||
|
wrist = (CameraFrame(_image(2), 20, 100.0, 930_000_000),)
|
||||||
|
|
||||||
|
with pytest.raises(QualityError, match="camera_skew"):
|
||||||
|
select_camera_pair(
|
||||||
|
high,
|
||||||
|
wrist,
|
||||||
|
1_000_000_000,
|
||||||
|
max_age_ms=100.0,
|
||||||
|
max_skew_ms=50.0,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_camera_buffer_is_bounded_and_counts_dropped_frames():
|
||||||
|
buffer = CameraBuffer(maxlen=4)
|
||||||
|
for frame_number in (10, 11, 13, 14, 15):
|
||||||
|
buffer.push(
|
||||||
|
CameraFrame(
|
||||||
|
_image(frame_number),
|
||||||
|
frame_number,
|
||||||
|
float(frame_number),
|
||||||
|
frame_number * 1_000_000,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
stats = buffer.stats()
|
||||||
|
|
||||||
|
assert [frame.frame_number for frame in buffer.snapshot()] == [
|
||||||
|
11,
|
||||||
|
13,
|
||||||
|
14,
|
||||||
|
15,
|
||||||
|
]
|
||||||
|
assert stats.frame_count == 5
|
||||||
|
assert stats.dropped_frames == 1
|
||||||
|
assert stats.drop_ratio == pytest.approx(1.0 / 6.0)
|
||||||
|
assert stats.fps == pytest.approx(800.0)
|
||||||
|
|
||||||
|
|
||||||
|
requires_h5py = pytest.mark.skipif(
|
||||||
|
h5py is None,
|
||||||
|
reason="h5py is not installed",
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _metadata():
|
||||||
|
return EpisodeMetadata(
|
||||||
|
joint_names=tuple(f"joint_{index}" for index in range(1, 8))
|
||||||
|
+ ("gripper",),
|
||||||
|
joint_lower_limits=np.full(7, -3.0, dtype=np.float64),
|
||||||
|
joint_upper_limits=np.full(7, 3.0, dtype=np.float64),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _episode_sample(seq):
|
||||||
|
control_ns = seq * 11_111_111
|
||||||
|
qpos = np.zeros(8, dtype=np.float32)
|
||||||
|
qpos[-1] = 1.0
|
||||||
|
action = qpos.copy()
|
||||||
|
return {
|
||||||
|
"observations/qpos": qpos,
|
||||||
|
"action": action,
|
||||||
|
"observations/images/cam_high": _image(seq % 255),
|
||||||
|
"observations/images/cam_right_wrist": _image((seq + 1) % 255),
|
||||||
|
"debug/timestamps/control_monotonic_ns": control_ns,
|
||||||
|
"debug/timestamps/feedback_monotonic_ns": control_ns - 1_000_000,
|
||||||
|
"debug/timestamps/action_monotonic_ns": control_ns,
|
||||||
|
"debug/timestamps/cam_high_host_monotonic_ns": control_ns - 2_000_000,
|
||||||
|
"debug/timestamps/cam_wrist_host_monotonic_ns": control_ns - 3_000_000,
|
||||||
|
"debug/timestamps/cam_high_hardware_ms": float(seq),
|
||||||
|
"debug/timestamps/cam_wrist_hardware_ms": float(seq),
|
||||||
|
"debug/timestamps/cam_high_age_ms": 2.0,
|
||||||
|
"debug/timestamps/cam_wrist_age_ms": 3.0,
|
||||||
|
"debug/timestamps/inter_camera_skew_ms": 1.0,
|
||||||
|
"debug/cameras/cam_high_frame_number": seq,
|
||||||
|
"debug/cameras/cam_wrist_frame_number": seq,
|
||||||
|
"debug/control/control_seq": seq,
|
||||||
|
"debug/control/teleop_active": 1,
|
||||||
|
"debug/control/action_valid": 1,
|
||||||
|
"debug/control/command_sent": 1,
|
||||||
|
"debug/control/target_clamped": 0,
|
||||||
|
"debug/control/control_fault": 0,
|
||||||
|
"debug/qp/raw_target": np.zeros(7, dtype=np.float32),
|
||||||
|
"debug/qp/attempted": 1,
|
||||||
|
"debug/qp/success": 1,
|
||||||
|
"debug/qp/duration_ms": 1.0,
|
||||||
|
"debug/tcp/current_pose": np.zeros(7, dtype=np.float32),
|
||||||
|
"debug/tcp/raw_target_pose": np.zeros(7, dtype=np.float32),
|
||||||
|
"debug/tcp/final_target_pose": np.zeros(7, dtype=np.float32),
|
||||||
|
"debug/tcp/command_velocity": np.zeros(6, dtype=np.float32),
|
||||||
|
"debug/pico/right_pose": np.zeros(7, dtype=np.float32),
|
||||||
|
"debug/pico/right_inputs": np.zeros(6, dtype=np.float32),
|
||||||
|
"debug/pico/left_secondary": 0,
|
||||||
|
"debug/gripper/target_open": 1,
|
||||||
|
"debug/gripper/state_open": 1,
|
||||||
|
"debug/gripper/command_pending": 0,
|
||||||
|
"debug/gripper/command_failed": 0,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_episode_store_writes_act_core_schema(tmp_path):
|
||||||
|
store = EpisodeStore.create(
|
||||||
|
tmp_path / "episode_0.partial.hdf5",
|
||||||
|
_metadata(),
|
||||||
|
)
|
||||||
|
for seq in (100, 103, 106):
|
||||||
|
store.append(_episode_sample(seq))
|
||||||
|
store.close()
|
||||||
|
|
||||||
|
with h5py.File(store.path, "r") as root:
|
||||||
|
assert root.attrs["sim"] == np.bool_(False)
|
||||||
|
assert root.attrs["action_alignment"] == "same_step_causal"
|
||||||
|
assert root["observations/qpos"].shape == (3, 8)
|
||||||
|
assert root["observations/qpos"].dtype == np.float32
|
||||||
|
assert root["action"].shape == (3, 8)
|
||||||
|
assert root["action"].dtype == np.float32
|
||||||
|
assert root["observations/images/cam_high"].shape == (
|
||||||
|
3,
|
||||||
|
480,
|
||||||
|
640,
|
||||||
|
3,
|
||||||
|
)
|
||||||
|
assert root["observations/images/cam_high"].dtype == np.uint8
|
||||||
|
assert root["observations/images/cam_right_wrist"].shape == (
|
||||||
|
3,
|
||||||
|
480,
|
||||||
|
640,
|
||||||
|
3,
|
||||||
|
)
|
||||||
|
assert "observations/qvel" not in root
|
||||||
|
assert "observations/effort" not in root
|
||||||
|
assert "compress_len" not in root
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_episode_store_truncates_every_time_axis_dataset(tmp_path):
|
||||||
|
store = EpisodeStore.create(
|
||||||
|
tmp_path / "episode_0.partial.hdf5",
|
||||||
|
_metadata(),
|
||||||
|
)
|
||||||
|
for seq in (100, 103, 106):
|
||||||
|
store.append(_episode_sample(seq))
|
||||||
|
|
||||||
|
store.truncate(2)
|
||||||
|
store.close()
|
||||||
|
|
||||||
|
lengths = []
|
||||||
|
with h5py.File(store.path, "r") as root:
|
||||||
|
root.visititems(
|
||||||
|
lambda _name, item: lengths.append(item.shape[0])
|
||||||
|
if isinstance(item, h5py.Dataset)
|
||||||
|
else None
|
||||||
|
)
|
||||||
|
assert lengths
|
||||||
|
assert set(lengths) == {2}
|
||||||
|
|
||||||
|
|
||||||
|
def test_next_index_uses_max_saved_episode_and_ignores_rejected(tmp_path):
|
||||||
|
(tmp_path / "episode_2.hdf5").touch()
|
||||||
|
(tmp_path / "episode_9.hdf5").touch()
|
||||||
|
rejected = tmp_path / "rejected"
|
||||||
|
rejected.mkdir()
|
||||||
|
(rejected / "episode_20_bad_20260810.hdf5").touch()
|
||||||
|
|
||||||
|
assert next_episode_index(tmp_path) == 10
|
||||||
|
|
||||||
|
|
||||||
|
def test_publish_never_overwrites_existing_episode(tmp_path):
|
||||||
|
partial = tmp_path / "episode_1.partial.hdf5"
|
||||||
|
partial.write_bytes(b"new")
|
||||||
|
final = tmp_path / "episode_1.hdf5"
|
||||||
|
final.write_bytes(b"old")
|
||||||
|
|
||||||
|
with pytest.raises(FileExistsError):
|
||||||
|
publish_without_overwrite(partial, final)
|
||||||
|
|
||||||
|
assert final.read_bytes() == b"old"
|
||||||
|
assert partial.read_bytes() == b"new"
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_recover_marks_readable_and_preserves_unreadable_partial(tmp_path):
|
||||||
|
readable = tmp_path / "episode_1.partial.hdf5"
|
||||||
|
store = EpisodeStore.create(readable, _metadata())
|
||||||
|
store.append(_episode_sample(100))
|
||||||
|
store.close()
|
||||||
|
unreadable = tmp_path / "episode_2.partial.hdf5"
|
||||||
|
unreadable.write_bytes(b"not hdf5")
|
||||||
|
|
||||||
|
recovered = recover_partial_files(tmp_path, timestamp="20260810T120000")
|
||||||
|
|
||||||
|
assert len(recovered) == 2
|
||||||
|
rejected = tmp_path / "rejected" / (
|
||||||
|
"episode_1_crash_recovered_20260810T120000.hdf5"
|
||||||
|
)
|
||||||
|
assert rejected in recovered
|
||||||
|
with h5py.File(rejected, "r") as root:
|
||||||
|
assert root.attrs["episode_status"] == "rejected"
|
||||||
|
assert root.attrs["reject_reason"] == "crash_recovered"
|
||||||
|
assert root.attrs["interrupted"] == np.bool_(True)
|
||||||
|
assert not readable.exists()
|
||||||
|
assert any(path.name.endswith(".partial.hdf5") for path in recovered)
|
||||||
|
assert unreadable not in recovered
|
||||||
|
assert not unreadable.exists()
|
||||||
|
|
||||||
|
|
||||||
|
def test_discard_partial_removes_only_current_file(tmp_path):
|
||||||
|
current = tmp_path / "episode_1.partial.hdf5"
|
||||||
|
current.write_bytes(b"current")
|
||||||
|
saved = tmp_path / "episode_0.hdf5"
|
||||||
|
saved.write_bytes(b"saved")
|
||||||
|
|
||||||
|
discard_partial(current)
|
||||||
|
|
||||||
|
assert not current.exists()
|
||||||
|
assert saved.read_bytes() == b"saved"
|
||||||
|
assert next_episode_index(tmp_path) == 1
|
||||||
|
|
||||||
|
|
||||||
|
def test_task_directory_lock_rejects_second_recorder(tmp_path):
|
||||||
|
first = TaskDirectoryLock(tmp_path)
|
||||||
|
second = TaskDirectoryLock(tmp_path)
|
||||||
|
first.acquire()
|
||||||
|
try:
|
||||||
|
with pytest.raises(BlockingIOError):
|
||||||
|
second.acquire()
|
||||||
|
finally:
|
||||||
|
first.release()
|
||||||
|
|
||||||
|
|
||||||
|
def _quality_limits():
|
||||||
|
return QualityLimits(min_samples=3, max_samples=100)
|
||||||
|
|
||||||
|
|
||||||
|
def _valid_episode(tmp_path):
|
||||||
|
path = tmp_path / "episode_0.partial.hdf5"
|
||||||
|
store = EpisodeStore.create(path, _metadata())
|
||||||
|
for index, seq in enumerate((100, 103, 106)):
|
||||||
|
sample = _episode_sample(seq)
|
||||||
|
sample["debug/cameras/cam_high_frame_number"] = 200 + index
|
||||||
|
sample["debug/cameras/cam_wrist_frame_number"] = 300 + index
|
||||||
|
store.append(sample)
|
||||||
|
store.close()
|
||||||
|
with h5py.File(path, "r+") as root:
|
||||||
|
root.attrs["camera_high_fps"] = 30.0
|
||||||
|
root.attrs["camera_right_wrist_fps"] = 30.0
|
||||||
|
root.attrs["camera_high_drop_ratio"] = 0.0
|
||||||
|
root.attrs["camera_right_wrist_drop_ratio"] = 0.0
|
||||||
|
return path
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_validate_episode_accepts_valid_file(tmp_path):
|
||||||
|
report = validate_episode(_valid_episode(tmp_path), _quality_limits())
|
||||||
|
|
||||||
|
assert report.accepted
|
||||||
|
assert report.reason is None
|
||||||
|
assert report.metrics["control_hz"] == pytest.approx(30.0, rel=1e-5)
|
||||||
|
|
||||||
|
|
||||||
|
def _mutate_episode(path, mutation):
|
||||||
|
with h5py.File(path, "r+") as root:
|
||||||
|
if mutation == "short_episode":
|
||||||
|
root.visititems(
|
||||||
|
lambda _name, item: item.resize(2, axis=0)
|
||||||
|
if isinstance(item, h5py.Dataset)
|
||||||
|
else None
|
||||||
|
)
|
||||||
|
elif mutation == "control_seq_gap":
|
||||||
|
root["debug/control/control_seq"][1] = 104
|
||||||
|
elif mutation == "nonfinite_qpos":
|
||||||
|
root["observations/qpos"][1, 0] = np.nan
|
||||||
|
elif mutation == "joint_limit":
|
||||||
|
root["action"][1, 0] = 4.0
|
||||||
|
elif mutation == "invalid_gripper":
|
||||||
|
root["action"][1, 7] = 0.5
|
||||||
|
elif mutation == "feedback_age":
|
||||||
|
control_ns = root["debug/timestamps/control_monotonic_ns"][1]
|
||||||
|
root["debug/timestamps/feedback_monotonic_ns"][1] = (
|
||||||
|
control_ns - 60_000_000
|
||||||
|
)
|
||||||
|
elif mutation == "action_invalid":
|
||||||
|
root["debug/control/action_valid"][1] = 0
|
||||||
|
elif mutation == "control_fault":
|
||||||
|
root["debug/control/control_fault"][1] = 1
|
||||||
|
elif mutation == "camera_fps":
|
||||||
|
root.attrs["camera_high_fps"] = 20.0
|
||||||
|
elif mutation == "camera_drop":
|
||||||
|
root.attrs["camera_right_wrist_drop_ratio"] = 0.02
|
||||||
|
elif mutation == "camera_age":
|
||||||
|
root["debug/timestamps/cam_high_age_ms"][1] = 60.0
|
||||||
|
elif mutation == "camera_skew":
|
||||||
|
root["debug/timestamps/inter_camera_skew_ms"][1] = 60.0
|
||||||
|
elif mutation == "final_gripper_closed":
|
||||||
|
root["observations/qpos"][-1, 7] = 0.0
|
||||||
|
else:
|
||||||
|
raise AssertionError(f"unknown mutation: {mutation}")
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
@pytest.mark.parametrize(
|
||||||
|
("mutation", "reason"),
|
||||||
|
[
|
||||||
|
("short_episode", "too_few_samples"),
|
||||||
|
("control_seq_gap", "control_sequence_gap"),
|
||||||
|
("nonfinite_qpos", "nonfinite_qpos"),
|
||||||
|
("joint_limit", "joint_limit_violation"),
|
||||||
|
("invalid_gripper", "invalid_gripper_state"),
|
||||||
|
("feedback_age", "feedback_too_old"),
|
||||||
|
("action_invalid", "invalid_action"),
|
||||||
|
("control_fault", "control_fault"),
|
||||||
|
("camera_fps", "camera_fps"),
|
||||||
|
("camera_drop", "camera_drop_ratio"),
|
||||||
|
("camera_age", "camera_frame_too_old"),
|
||||||
|
("camera_skew", "camera_skew"),
|
||||||
|
("final_gripper_closed", "final_gripper_not_open"),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
def test_validate_episode_reports_stable_reason(
|
||||||
|
tmp_path,
|
||||||
|
mutation,
|
||||||
|
reason,
|
||||||
|
):
|
||||||
|
path = _valid_episode(tmp_path)
|
||||||
|
_mutate_episode(path, mutation)
|
||||||
|
|
||||||
|
report = validate_episode(path, _quality_limits())
|
||||||
|
|
||||||
|
assert not report.accepted
|
||||||
|
assert report.reason == reason
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_validate_episode_reports_qp_failures_without_rejecting(tmp_path):
|
||||||
|
path = _valid_episode(tmp_path)
|
||||||
|
with h5py.File(path, "r+") as root:
|
||||||
|
root["debug/qp/attempted"][:] = (1, 1, 1)
|
||||||
|
root["debug/qp/success"][:] = (0, 0, 1)
|
||||||
|
root["debug/control/target_clamped"][:] = (1, 0, 1)
|
||||||
|
|
||||||
|
report = validate_episode(path, _quality_limits())
|
||||||
|
|
||||||
|
assert report.accepted
|
||||||
|
assert report.metrics["qp_failure_count"] == 2
|
||||||
|
assert report.metrics["qp_failure_ratio"] == pytest.approx(2.0 / 3.0)
|
||||||
|
assert report.metrics["qp_longest_failure_streak"] == 2
|
||||||
|
assert report.metrics["target_clamped_count"] == 2
|
||||||
|
|
||||||
|
|
||||||
|
class _StatusPublisher:
|
||||||
|
def __init__(self):
|
||||||
|
self.messages = []
|
||||||
|
|
||||||
|
def publish(self, message):
|
||||||
|
self.messages.append(message.data)
|
||||||
|
|
||||||
|
|
||||||
|
class _Logger:
|
||||||
|
def info(self, *_args, **_kwargs):
|
||||||
|
pass
|
||||||
|
|
||||||
|
def warn(self, *_args, **_kwargs):
|
||||||
|
pass
|
||||||
|
|
||||||
|
def error(self, *_args, **_kwargs):
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
def _control_message(seq, control_ns, *, grip=True):
|
||||||
|
message = ActControlSample()
|
||||||
|
message.control_seq = seq
|
||||||
|
message.control_monotonic_ns = control_ns
|
||||||
|
message.feedback_monotonic_ns = control_ns - 1_000_000
|
||||||
|
message.action_monotonic_ns = control_ns + 1_000_000 if grip else -1
|
||||||
|
message.q_actual = [0.0] * 7
|
||||||
|
message.q_qp_raw = [0.0] * 7
|
||||||
|
message.q_target = [0.0] * 7
|
||||||
|
message.joint_lower_limits = [-3.0] * 7
|
||||||
|
message.joint_upper_limits = [3.0] * 7
|
||||||
|
message.tcp_current.orientation.w = 1.0
|
||||||
|
message.tcp_raw_target.orientation.w = 1.0
|
||||||
|
message.tcp_target.orientation.w = 1.0
|
||||||
|
message.pico_pose.orientation.w = 1.0
|
||||||
|
message.pico_grip = grip
|
||||||
|
message.gripper_target_open = True
|
||||||
|
message.gripper_state_open = True
|
||||||
|
message.gripper_state_known = True
|
||||||
|
message.teleop_active = grip
|
||||||
|
message.feedback_valid = True
|
||||||
|
message.action_valid = True
|
||||||
|
message.command_sent = grip
|
||||||
|
message.qp_attempted = grip
|
||||||
|
message.qp_success = grip
|
||||||
|
return message
|
||||||
|
|
||||||
|
|
||||||
|
def _seed_camera(start_ns, first_number):
|
||||||
|
camera = SimpleNamespace(
|
||||||
|
buffer=CameraBuffer(maxlen=4),
|
||||||
|
last_error=None,
|
||||||
|
)
|
||||||
|
image = _image(1)
|
||||||
|
for index in range(151):
|
||||||
|
camera.buffer.push(
|
||||||
|
CameraFrame(
|
||||||
|
image,
|
||||||
|
first_number + index,
|
||||||
|
index * (1000.0 / 30.0),
|
||||||
|
start_ns
|
||||||
|
- 5_020_000_000
|
||||||
|
+ round(index * 5_000_000_000 / 150),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return camera
|
||||||
|
|
||||||
|
|
||||||
|
def _recorder_for_test(tmp_path):
|
||||||
|
now_ns = 10_000_000_000
|
||||||
|
recorder = object.__new__(ActEpisodeRecorder)
|
||||||
|
recorder._task_dir = tmp_path / "tomato_pick"
|
||||||
|
recorder._task_dir.mkdir(parents=True)
|
||||||
|
recorder._quality_limits = _quality_limits()
|
||||||
|
recorder._min_free_space_bytes = 4 * 1024**3
|
||||||
|
recorder._controller_timeout_ns = 500_000_000
|
||||||
|
recorder._camera_warmup_ns = 5_000_000_000
|
||||||
|
recorder._gripper_completion_timeout_ns = 3_000_000_000
|
||||||
|
recorder._writer_queue_size = 8
|
||||||
|
recorder._max_camera_age_ms = 50.0
|
||||||
|
recorder._max_camera_skew_ms = 50.0
|
||||||
|
recorder._session = RecordingSession(max_samples=100)
|
||||||
|
recorder._button_tracker = ButtonTracker(hold_ns=1_000_000_000)
|
||||||
|
recorder._latest_control = _control_message(99, now_ns - 1_000_000)
|
||||||
|
recorder._latest_control_received_ns = now_ns - 1_000_000
|
||||||
|
recorder._right_controller_received_ns = now_ns - 1_000_000
|
||||||
|
recorder._left_controller_received_ns = now_ns - 1_000_000
|
||||||
|
recorder._left_secondary = False
|
||||||
|
recorder._high_camera = _seed_camera(now_ns, 850)
|
||||||
|
recorder._wrist_camera = _seed_camera(now_ns, 1850)
|
||||||
|
recorder._camera_start_error = None
|
||||||
|
recorder._writer = None
|
||||||
|
recorder._store = None
|
||||||
|
recorder._partial_path = None
|
||||||
|
recorder._camera_baselines = None
|
||||||
|
recorder._saving_deadline_ns = None
|
||||||
|
recorder._status_pub = _StatusPublisher()
|
||||||
|
recorder._now_ns = lambda: now_ns
|
||||||
|
recorder._disk_usage = lambda _path: SimpleNamespace(free=5 * 1024**3)
|
||||||
|
recorder.get_logger = lambda: _Logger()
|
||||||
|
return recorder
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_preflight_requires_open_gripper_fresh_inputs_and_disk_space(tmp_path):
|
||||||
|
recorder = _recorder_for_test(tmp_path)
|
||||||
|
|
||||||
|
assert recorder._run_preflight() is None
|
||||||
|
|
||||||
|
recorder._latest_control.gripper_state_open = False
|
||||||
|
assert recorder._run_preflight() == "gripper_not_open"
|
||||||
|
recorder._latest_control.gripper_state_open = True
|
||||||
|
recorder._right_controller_received_ns = 0
|
||||||
|
assert recorder._run_preflight() == "right_controller_stale"
|
||||||
|
recorder._right_controller_received_ns = recorder._now_ns()
|
||||||
|
recorder._disk_usage = lambda _path: SimpleNamespace(free=1024)
|
||||||
|
assert recorder._run_preflight() == "insufficient_disk_space"
|
||||||
|
|
||||||
|
|
||||||
|
def _push_recording_frames(recorder, control_ns, frame_number):
|
||||||
|
recorder._high_camera.buffer.push(
|
||||||
|
CameraFrame(
|
||||||
|
_image(2),
|
||||||
|
frame_number,
|
||||||
|
float(frame_number),
|
||||||
|
control_ns - 2_000_000,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
recorder._wrist_camera.buffer.push(
|
||||||
|
CameraFrame(
|
||||||
|
_image(3),
|
||||||
|
frame_number,
|
||||||
|
float(frame_number),
|
||||||
|
control_ns - 3_000_000,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _finish_fake_episode(recorder):
|
||||||
|
start_ns = recorder._now_ns()
|
||||||
|
frame_number = 1001
|
||||||
|
for offset, seq in enumerate(range(100, 107)):
|
||||||
|
control_ns = start_ns + offset * 11_111_111
|
||||||
|
if (seq - 100) % 3 == 0:
|
||||||
|
_push_recording_frames(recorder, control_ns, frame_number)
|
||||||
|
frame_number += 1
|
||||||
|
recorder._on_control_sample(_control_message(seq, control_ns))
|
||||||
|
|
||||||
|
recorder._on_control_sample(
|
||||||
|
_control_message(107, start_ns + 7 * 11_111_111, grip=False)
|
||||||
|
)
|
||||||
|
recorder._handle_right_b(grip=False)
|
||||||
|
final_ns = start_ns + 8 * 11_111_111
|
||||||
|
_push_recording_frames(recorder, final_ns, frame_number)
|
||||||
|
recorder._on_control_sample(_control_message(108, final_ns, grip=False))
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_end_to_end_fake_episode_saves_and_returns_idle(tmp_path):
|
||||||
|
recorder = _recorder_for_test(tmp_path)
|
||||||
|
recorder._handle_right_b(grip=False)
|
||||||
|
|
||||||
|
assert recorder.state is RecordingState.ARMED
|
||||||
|
|
||||||
|
_finish_fake_episode(recorder)
|
||||||
|
|
||||||
|
assert (tmp_path / "tomato_pick" / "episode_0.hdf5").is_file()
|
||||||
|
assert recorder.state is RecordingState.IDLE
|
||||||
|
assert "SAVING" in recorder._status_pub.messages
|
||||||
|
assert recorder._status_pub.messages[-2:] == ["SAVED", "IDLE"]
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_final_publish_rejects_allocated_episode_number_conflict(tmp_path):
|
||||||
|
recorder = _recorder_for_test(tmp_path)
|
||||||
|
recorder._handle_right_b(grip=False)
|
||||||
|
existing = recorder._task_dir / "episode_0.hdf5"
|
||||||
|
existing.write_bytes(b"existing")
|
||||||
|
|
||||||
|
_finish_fake_episode(recorder)
|
||||||
|
|
||||||
|
assert existing.read_bytes() == b"existing"
|
||||||
|
assert not (recorder._task_dir / "episode_1.hdf5").exists()
|
||||||
|
rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
|
||||||
|
assert len(rejected) == 1
|
||||||
|
with h5py.File(rejected[0], "r") as root:
|
||||||
|
assert root.attrs["reject_reason"] == "episode_number_conflict"
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_discard_and_interrupt_only_process_current_partial(tmp_path):
|
||||||
|
recorder = _recorder_for_test(tmp_path)
|
||||||
|
recorder._handle_right_b(grip=False)
|
||||||
|
partial = recorder._partial_path
|
||||||
|
|
||||||
|
recorder._discard_current()
|
||||||
|
|
||||||
|
assert not partial.exists()
|
||||||
|
assert recorder.state is RecordingState.IDLE
|
||||||
|
|
||||||
|
recorder._handle_right_b(grip=False)
|
||||||
|
recorder.interrupt_recording("interrupted")
|
||||||
|
rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
|
||||||
|
assert len(rejected) == 1
|
||||||
|
with h5py.File(rejected[0], "r") as root:
|
||||||
|
assert root.attrs["reject_reason"] == "interrupted"
|
||||||
|
assert root.attrs["interrupted"] == np.bool_(True)
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_a_button_and_camera_error_reject_without_robot_commands(tmp_path):
|
||||||
|
recorder = _recorder_for_test(tmp_path)
|
||||||
|
recorder._handle_right_b(grip=False)
|
||||||
|
recorder._button_tracker.on_right(
|
||||||
|
False,
|
||||||
|
False,
|
||||||
|
False,
|
||||||
|
recorder._now_ns(),
|
||||||
|
RecordingState.RECORDING,
|
||||||
|
)
|
||||||
|
recorder._session.state = RecordingState.RECORDING
|
||||||
|
controller = XrController()
|
||||||
|
controller.primary = True
|
||||||
|
|
||||||
|
recorder._on_right_controller(controller)
|
||||||
|
|
||||||
|
rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
|
||||||
|
assert len(rejected) == 1
|
||||||
|
with h5py.File(rejected[0], "r") as root:
|
||||||
|
assert root.attrs["reject_reason"] == (
|
||||||
|
"initial_pose_command_during_episode"
|
||||||
|
)
|
||||||
|
|
||||||
|
recorder._handle_right_b(grip=False)
|
||||||
|
recorder._high_camera.last_error = RuntimeError("usb")
|
||||||
|
recorder._on_control_sample(
|
||||||
|
_control_message(100, recorder._now_ns())
|
||||||
|
)
|
||||||
|
rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
|
||||||
|
assert len(rejected) == 2
|
||||||
|
camera_rejected = next(
|
||||||
|
path for path in rejected if "camera_error" in path.name
|
||||||
|
)
|
||||||
|
with h5py.File(camera_rejected, "r") as root:
|
||||||
|
assert root.attrs["reject_reason"] == "camera_error"
|
||||||
|
|
||||||
|
|
||||||
|
def test_episode_writer_reports_queue_backlog_and_write_error():
|
||||||
|
started = threading.Event()
|
||||||
|
release = threading.Event()
|
||||||
|
|
||||||
|
class BlockingStore:
|
||||||
|
def append(self, _sample):
|
||||||
|
started.set()
|
||||||
|
assert release.wait(timeout=1.0)
|
||||||
|
|
||||||
|
writer = EpisodeWriter(BlockingStore(), queue_size=1)
|
||||||
|
writer.submit({})
|
||||||
|
assert started.wait(timeout=1.0)
|
||||||
|
writer.submit({})
|
||||||
|
with pytest.raises(queue.Full):
|
||||||
|
writer.submit({})
|
||||||
|
release.set()
|
||||||
|
writer.finish()
|
||||||
|
assert writer.error is None
|
||||||
|
|
||||||
|
class FailingStore:
|
||||||
|
def append(self, _sample):
|
||||||
|
raise OSError("disk full")
|
||||||
|
|
||||||
|
writer = EpisodeWriter(FailingStore(), queue_size=1)
|
||||||
|
writer.submit({})
|
||||||
|
writer.finish()
|
||||||
|
assert isinstance(writer.error, OSError)
|
||||||
@@ -1,303 +0,0 @@
|
|||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import math
|
|
||||||
import sys
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import h5py
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
TEST_DIR = Path(__file__).resolve().parent
|
|
||||||
if str(TEST_DIR) not in sys.path:
|
|
||||||
sys.path.insert(0, str(TEST_DIR))
|
|
||||||
|
|
||||||
import ik_method_comparison as comparison
|
|
||||||
|
|
||||||
|
|
||||||
def test_slerp_uses_shortest_arc_and_returns_unit_quaternion() -> None:
|
|
||||||
start = np.asarray([0.0, 0.0, 0.0, 1.0])
|
|
||||||
end = -np.asarray([0.0, 0.0, math.sin(0.1), math.cos(0.1)])
|
|
||||||
|
|
||||||
actual = comparison._slerp_quaternion(start, end, 0.5)
|
|
||||||
|
|
||||||
assert np.linalg.norm(actual) == pytest.approx(1.0)
|
|
||||||
assert actual == pytest.approx(
|
|
||||||
[0.0, 0.0, math.sin(0.05), math.cos(0.05)]
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def test_resample_trajectory_keeps_endpoints_and_uses_requested_rate() -> None:
|
|
||||||
trajectory = comparison.EpisodeTrajectory(
|
|
||||||
source_path=Path("episode.hdf5"),
|
|
||||||
times_s=np.asarray([0.0, 0.5, 1.0]),
|
|
||||||
target_poses=np.asarray(
|
|
||||||
[
|
|
||||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
|
|
||||||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
|
|
||||||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
|
|
||||||
]
|
|
||||||
),
|
|
||||||
initial_joints=np.zeros(7),
|
|
||||||
)
|
|
||||||
|
|
||||||
actual = comparison.resample_trajectory(trajectory, 4.0)
|
|
||||||
|
|
||||||
assert actual.times_s == pytest.approx([0.0, 0.25, 0.5, 0.75, 1.0])
|
|
||||||
assert actual.target_poses[0] == pytest.approx(trajectory.target_poses[0])
|
|
||||||
assert actual.target_poses[-1] == pytest.approx(trajectory.target_poses[-1])
|
|
||||||
assert actual.target_poses[:, 0] == pytest.approx(actual.times_s)
|
|
||||||
|
|
||||||
|
|
||||||
def _write_episode(path: Path) -> None:
|
|
||||||
poses = np.asarray(
|
|
||||||
[
|
|
||||||
[0.1, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
|
|
||||||
[0.2, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
|
|
||||||
[0.3, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
|
|
||||||
[0.4, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0],
|
|
||||||
],
|
|
||||||
dtype=np.float32,
|
|
||||||
)
|
|
||||||
with h5py.File(path, "w") as handle:
|
|
||||||
handle.attrs["arm"] = "right_rm75"
|
|
||||||
handle.attrs["pose_order"] = "x,y,z,qx,qy,qz,qw"
|
|
||||||
handle.create_dataset("debug/tcp/final_target_pose", data=poses)
|
|
||||||
handle.create_dataset(
|
|
||||||
"debug/timestamps/control_monotonic_ns",
|
|
||||||
data=np.asarray([0, 33_000_000, 66_000_000, 99_000_000]),
|
|
||||||
)
|
|
||||||
handle.create_dataset(
|
|
||||||
"debug/control/teleop_active", data=[0, 1, 1, 0]
|
|
||||||
)
|
|
||||||
handle.create_dataset(
|
|
||||||
"debug/control/action_valid", data=[1, 1, 1, 1]
|
|
||||||
)
|
|
||||||
handle.create_dataset(
|
|
||||||
"debug/control/command_sent", data=[0, 1, 1, 0]
|
|
||||||
)
|
|
||||||
qpos = np.zeros((4, 8), dtype=np.float32)
|
|
||||||
qpos[1, :7] = np.arange(7) * 0.1
|
|
||||||
handle.create_dataset("observations/qpos", data=qpos)
|
|
||||||
|
|
||||||
|
|
||||||
def test_load_episode_uses_longest_valid_run_and_first_valid_qpos(
|
|
||||||
tmp_path: Path,
|
|
||||||
) -> None:
|
|
||||||
path = tmp_path / "episode.hdf5"
|
|
||||||
_write_episode(path)
|
|
||||||
|
|
||||||
actual = comparison.load_episode(path)
|
|
||||||
|
|
||||||
assert actual.times_s == pytest.approx([0.0, 0.033])
|
|
||||||
assert actual.target_poses[:, 0] == pytest.approx([0.2, 0.3])
|
|
||||||
assert actual.initial_joints == pytest.approx(np.arange(7) * 0.1)
|
|
||||||
|
|
||||||
|
|
||||||
def test_load_episode_rejects_wrong_arm(tmp_path: Path) -> None:
|
|
||||||
path = tmp_path / "episode.hdf5"
|
|
||||||
_write_episode(path)
|
|
||||||
with h5py.File(path, "r+") as handle:
|
|
||||||
handle.attrs.modify("arm", "left_rm75")
|
|
||||||
|
|
||||||
with pytest.raises(ValueError, match="right_rm75"):
|
|
||||||
comparison.load_episode(path)
|
|
||||||
|
|
||||||
|
|
||||||
def test_orientation_error_and_joint_margin_match_definitions() -> None:
|
|
||||||
identity = np.eye(3)
|
|
||||||
quarter_turn = comparison._rotation_z(math.pi / 2.0)
|
|
||||||
joints = np.asarray([0.0, -0.5])
|
|
||||||
lower = np.asarray([-1.0, -1.0])
|
|
||||||
upper = np.asarray([1.0, 3.0])
|
|
||||||
|
|
||||||
assert comparison.orientation_error_rad(identity, quarter_turn) \
|
|
||||||
== pytest.approx(math.pi / 2.0)
|
|
||||||
assert comparison.normalized_joint_margin(joints, lower, upper) \
|
|
||||||
== pytest.approx(0.125)
|
|
||||||
|
|
||||||
|
|
||||||
def test_choose_dls_damping_is_lexicographic() -> None:
|
|
||||||
candidates = [
|
|
||||||
comparison.MethodSummary("dls", 0.01, 0.90, 0.004, 0.01, 50.0),
|
|
||||||
comparison.MethodSummary("dls", 0.03, 0.95, 0.006, 0.02, 30.0),
|
|
||||||
comparison.MethodSummary("dls", 0.10, 0.95, 0.004, 0.01, 40.0),
|
|
||||||
]
|
|
||||||
|
|
||||||
assert comparison.choose_dls_damping(candidates) == pytest.approx(0.10)
|
|
||||||
|
|
||||||
|
|
||||||
def test_limit_joint_command_reuses_production_limiter() -> None:
|
|
||||||
target, velocity, limited = comparison.limit_joint_command(
|
|
||||||
target=np.full(7, 1.0),
|
|
||||||
previous_target=np.zeros(7),
|
|
||||||
previous_velocity=np.zeros(7),
|
|
||||||
max_speed=1.0,
|
|
||||||
max_acceleration=10.0,
|
|
||||||
dt=0.1,
|
|
||||||
)
|
|
||||||
|
|
||||||
assert target == pytest.approx([0.1] * 7)
|
|
||||||
assert velocity == pytest.approx([1.0] * 7)
|
|
||||||
assert limited
|
|
||||||
|
|
||||||
|
|
||||||
class _FakeSolver:
|
|
||||||
def __init__(self, fail: bool) -> None:
|
|
||||||
self.fail = fail
|
|
||||||
self.joint_limits = np.asarray([[-2.0, 2.0]] * 7)
|
|
||||||
|
|
||||||
def update_joint_state(self, joints: list[float]) -> np.ndarray:
|
|
||||||
pose = np.eye(4)
|
|
||||||
pose[0, 3] = joints[0]
|
|
||||||
return pose
|
|
||||||
|
|
||||||
def solve(self, target: np.ndarray) -> list[float]:
|
|
||||||
if self.fail:
|
|
||||||
raise RuntimeError("not converged")
|
|
||||||
return [float(target[0, 3])] + [0.0] * 6
|
|
||||||
|
|
||||||
|
|
||||||
def test_replay_holds_previous_state_on_solver_failure() -> None:
|
|
||||||
trajectory = comparison.EpisodeTrajectory(
|
|
||||||
source_path=Path("episode.hdf5"),
|
|
||||||
times_s=np.asarray([0.0, 0.1]),
|
|
||||||
target_poses=np.asarray(
|
|
||||||
[
|
|
||||||
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
|
|
||||||
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
|
|
||||||
]
|
|
||||||
),
|
|
||||||
initial_joints=np.zeros(7),
|
|
||||||
)
|
|
||||||
|
|
||||||
result = comparison.run_replay(
|
|
||||||
"fake",
|
|
||||||
_FakeSolver(fail=True),
|
|
||||||
trajectory,
|
|
||||||
max_speed=1.0,
|
|
||||||
max_acceleration=10.0,
|
|
||||||
measure_time=False,
|
|
||||||
)
|
|
||||||
|
|
||||||
assert result.joints == pytest.approx(np.zeros((2, 7)))
|
|
||||||
assert not result.success.any()
|
|
||||||
assert result.velocities == pytest.approx(np.zeros((2, 7)))
|
|
||||||
|
|
||||||
|
|
||||||
def test_real_urdf_solvers_return_finite_safe_outputs() -> None:
|
|
||||||
pytest.importorskip("placo")
|
|
||||||
urdf = TEST_DIR.parent / "models" / "dual_rm75" / "Dual_arm.urdf"
|
|
||||||
config_path = (
|
|
||||||
TEST_DIR.parents[1]
|
|
||||||
/ "xr_rm_bringup"
|
|
||||||
/ "config"
|
|
||||||
/ "right_arm_rm75.yaml"
|
|
||||||
)
|
|
||||||
joints = np.radians(
|
|
||||||
[-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
|
|
||||||
)
|
|
||||||
solvers = [
|
|
||||||
comparison.DifferentialIkSolver(urdf, 1.0 / 90.0, "pinv"),
|
|
||||||
comparison.DifferentialIkSolver(urdf, 1.0 / 90.0, "dls", 0.03),
|
|
||||||
comparison.make_qp_solver(
|
|
||||||
urdf,
|
|
||||||
1.0 / 90.0,
|
|
||||||
comparison.load_right_config(config_path),
|
|
||||||
),
|
|
||||||
]
|
|
||||||
for solver in solvers:
|
|
||||||
target = solver.update_joint_state(joints.tolist())
|
|
||||||
target = target.copy()
|
|
||||||
target[0, 3] += 0.003
|
|
||||||
result = np.asarray(solver.solve(target), dtype=float)
|
|
||||||
assert result.shape == (7,)
|
|
||||||
assert np.isfinite(result).all()
|
|
||||||
assert np.all(result >= solver.joint_limits[:, 0] - 1e-9)
|
|
||||||
assert np.all(result <= solver.joint_limits[:, 1] + 1e-9)
|
|
||||||
|
|
||||||
|
|
||||||
def test_load_right_config_returns_qp_and_command_limits() -> None:
|
|
||||||
config = (
|
|
||||||
TEST_DIR.parents[1]
|
|
||||||
/ "xr_rm_bringup"
|
|
||||||
/ "config"
|
|
||||||
/ "right_arm_rm75.yaml"
|
|
||||||
)
|
|
||||||
|
|
||||||
actual = comparison.load_right_config(config)
|
|
||||||
|
|
||||||
assert actual["qp_j3_reference_deg"] == pytest.approx(-89.57)
|
|
||||||
assert actual["qp_j4_min_deg"] == pytest.approx(10.0)
|
|
||||||
assert actual["qp_manipulability_weight"] == pytest.approx(1e-4)
|
|
||||||
assert actual["joint_max_speed"] == pytest.approx(180.0)
|
|
||||||
assert actual["joint_max_acc"] == pytest.approx(300.0)
|
|
||||||
|
|
||||||
|
|
||||||
def test_summarize_result_uses_report_metrics() -> None:
|
|
||||||
result = comparison.ReplayResult(
|
|
||||||
method="pinv",
|
|
||||||
times_s=np.asarray([0.0, 0.1]),
|
|
||||||
target_poses=np.zeros((2, 7)),
|
|
||||||
actual_poses=np.zeros((2, 7)),
|
|
||||||
joints=np.zeros((2, 7)),
|
|
||||||
velocities=np.asarray([[0.0] * 7, [math.pi] + [0.0] * 6]),
|
|
||||||
position_errors_m=np.asarray([0.003, 0.004]),
|
|
||||||
orientation_errors_rad=np.asarray([0.01, 0.02]),
|
|
||||||
joint_margins=np.asarray([0.2, 0.1]),
|
|
||||||
solve_durations_ms=np.asarray([1.0, 2.0]),
|
|
||||||
success=np.asarray([True, False]),
|
|
||||||
command_limited=np.asarray([False, True]),
|
|
||||||
)
|
|
||||||
|
|
||||||
actual = comparison.summarize_result(result)
|
|
||||||
|
|
||||||
assert actual.success_rate == pytest.approx(0.5)
|
|
||||||
assert actual.position_rmse_m == pytest.approx(0.0035355339)
|
|
||||||
assert actual.orientation_rmse_rad == pytest.approx(0.0158113883)
|
|
||||||
assert actual.max_joint_speed_deg_s == pytest.approx(180.0)
|
|
||||||
|
|
||||||
|
|
||||||
def test_write_outputs_creates_consistent_files(tmp_path: Path) -> None:
|
|
||||||
result = comparison.ReplayResult(
|
|
||||||
method="qp",
|
|
||||||
times_s=np.asarray([0.0, 0.1]),
|
|
||||||
target_poses=np.tile(
|
|
||||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (2, 1)
|
|
||||||
),
|
|
||||||
actual_poses=np.tile(
|
|
||||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (2, 1)
|
|
||||||
),
|
|
||||||
joints=np.zeros((2, 7)),
|
|
||||||
velocities=np.zeros((2, 7)),
|
|
||||||
position_errors_m=np.asarray([0.001, 0.002]),
|
|
||||||
orientation_errors_rad=np.asarray([0.001, 0.002]),
|
|
||||||
joint_margins=np.asarray([0.2, 0.2]),
|
|
||||||
solve_durations_ms=np.asarray([0.5, 0.6]),
|
|
||||||
success=np.asarray([True, True]),
|
|
||||||
command_limited=np.asarray([False, False]),
|
|
||||||
)
|
|
||||||
summary = comparison.summarize_result(result)
|
|
||||||
|
|
||||||
comparison.write_outputs(
|
|
||||||
tmp_path,
|
|
||||||
{"pinv": result, "dls": result, "qp": result},
|
|
||||||
{"pinv": summary, "dls": summary, "qp": summary},
|
|
||||||
selected_damping=0.03,
|
|
||||||
source_path=Path("episode_0.hdf5"),
|
|
||||||
git_commit="abc1234",
|
|
||||||
)
|
|
||||||
|
|
||||||
expected = {
|
|
||||||
"samples.csv",
|
|
||||||
"summary.json",
|
|
||||||
"figure_2_11_tracking_error.svg",
|
|
||||||
"figure_2_11_tracking_error.png",
|
|
||||||
"figure_2_12_joint_constraints.svg",
|
|
||||||
"figure_2_12_joint_constraints.png",
|
|
||||||
"figure_2_13_summary.svg",
|
|
||||||
"figure_2_13_summary.png",
|
|
||||||
"analysis_2.3.4.md",
|
|
||||||
}
|
|
||||||
assert expected == {path.name for path in tmp_path.iterdir()}
|
|
||||||
assert all((tmp_path / name).stat().st_size > 0 for name in expected)
|
|
||||||
@@ -6,13 +6,14 @@ from types import ModuleType, SimpleNamespace
|
|||||||
import pytest
|
import pytest
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
from xr_rm_teleop import realman_adapter
|
from xr_rm_teleop import fun_peripheral, realman_adapter
|
||||||
from xr_rm_teleop.realman_adapter import RealManAdapter
|
from xr_rm_teleop.realman_adapter import RealManAdapter
|
||||||
from xr_rm_teleop.realman_adapter import MockRealManAdapter
|
from xr_rm_teleop.realman_adapter import MockRealManAdapter
|
||||||
from xr_rm_teleop.fun_peripheral import (
|
from xr_rm_teleop.fun_peripheral import (
|
||||||
PeripheralConfig,
|
PeripheralConfig,
|
||||||
_configure_tool_frame,
|
_configure_tool_frame,
|
||||||
load_peripheral_config,
|
load_peripheral_config,
|
||||||
|
peripheral_cfg,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -82,6 +83,78 @@ def test_deployed_peripheral_config_selects_left_and_right_tools() -> None:
|
|||||||
assert right.tool_pose == [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
|
assert right.tool_pose == [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_right_tool_initializes_open_only_for_right_arm() -> None:
|
||||||
|
config_file = CONFIG_DIR / "peripherals_rm75.yaml"
|
||||||
|
left = load_peripheral_config(str(config_file), "left")
|
||||||
|
right = load_peripheral_config(str(config_file), "right")
|
||||||
|
|
||||||
|
assert not left.set_initial_tool_state
|
||||||
|
assert right.set_initial_tool_state
|
||||||
|
|
||||||
|
|
||||||
|
def test_omnipic_initial_state_opens_fully(monkeypatch) -> None:
|
||||||
|
calls = []
|
||||||
|
|
||||||
|
class FakeArm:
|
||||||
|
def rm_set_voltage(self, *args):
|
||||||
|
del args
|
||||||
|
|
||||||
|
def rm_set_io_mode(self, *args):
|
||||||
|
del args
|
||||||
|
|
||||||
|
def rm_algo_quaternion2euler(self, quaternion):
|
||||||
|
del quaternion
|
||||||
|
return [0.0, 0.0, 0.0]
|
||||||
|
|
||||||
|
def rm_get_total_tool_frame(self):
|
||||||
|
return {"return_code": 0, "tool_names": []}
|
||||||
|
|
||||||
|
def rm_set_manual_tool_frame(self, *, frame):
|
||||||
|
del frame
|
||||||
|
return 0
|
||||||
|
|
||||||
|
def rm_change_tool_frame(self, tool_name):
|
||||||
|
del tool_name
|
||||||
|
return 0
|
||||||
|
|
||||||
|
def rm_set_modbus_mode(self, **kwargs):
|
||||||
|
del kwargs
|
||||||
|
return 0
|
||||||
|
|
||||||
|
def rm_write_single_register(self, params, value):
|
||||||
|
del params, value
|
||||||
|
return 0
|
||||||
|
|
||||||
|
sdk = ModuleType("Robotic_Arm.rm_robot_interface")
|
||||||
|
sdk.rm_frame_t = lambda *args: object()
|
||||||
|
sdk.rm_peripheral_read_write_params_t = lambda *args: object()
|
||||||
|
package = ModuleType("Robotic_Arm")
|
||||||
|
package.rm_robot_interface = sdk
|
||||||
|
monkeypatch.setitem(sys.modules, "Robotic_Arm", package)
|
||||||
|
monkeypatch.setitem(sys.modules, "Robotic_Arm.rm_robot_interface", sdk)
|
||||||
|
monkeypatch.setattr(fun_peripheral.time, "sleep", lambda seconds: None)
|
||||||
|
monkeypatch.setattr(
|
||||||
|
fun_peripheral,
|
||||||
|
"set_tool_position",
|
||||||
|
lambda robot, percent, device, scissorgripper: calls.append(
|
||||||
|
(percent, device, scissorgripper)
|
||||||
|
),
|
||||||
|
)
|
||||||
|
tools = {
|
||||||
|
"scissor": [[0.0] * 7, [0.0] * 7],
|
||||||
|
"omnipic": [[0.0] * 7, [0.0] * 7],
|
||||||
|
}
|
||||||
|
|
||||||
|
peripheral_cfg(
|
||||||
|
FakeArm(),
|
||||||
|
1,
|
||||||
|
tools,
|
||||||
|
set_initial_tool_state=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert calls == [(1.0, 1, 1)]
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize(
|
@pytest.mark.parametrize(
|
||||||
("config_name", "node_names"),
|
("config_name", "node_names"),
|
||||||
[
|
[
|
||||||
@@ -100,43 +173,6 @@ def test_deployed_workspace_is_in_front_of_robot(config_name, node_names) -> Non
|
|||||||
assert parameters["workspace_max"] == [0.70, 0.10, 0.75]
|
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(
|
@pytest.mark.parametrize(
|
||||||
("existing", "expected_operation"),
|
("existing", "expected_operation"),
|
||||||
[(False, "create"), (True, "update")],
|
[(False, "create"), (True, "update")],
|
||||||
|
|||||||
@@ -1,4 +1,5 @@
|
|||||||
import math
|
import math
|
||||||
|
import threading
|
||||||
import time
|
import time
|
||||||
from types import SimpleNamespace
|
from types import SimpleNamespace
|
||||||
|
|
||||||
@@ -11,7 +12,6 @@ from xr_rm_teleop.single_arm_velocity_teleop import (
|
|||||||
SingleArmVelocityTeleop,
|
SingleArmVelocityTeleop,
|
||||||
_make_transform,
|
_make_transform,
|
||||||
_so3_exp,
|
_so3_exp,
|
||||||
_so3_log,
|
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -43,6 +43,69 @@ class FakePublisher:
|
|||||||
self.messages.append(message)
|
self.messages.append(message)
|
||||||
|
|
||||||
|
|
||||||
|
def _tool_state_teleop(*, command_error=None):
|
||||||
|
started = threading.Event()
|
||||||
|
release = threading.Event()
|
||||||
|
|
||||||
|
class Adapter:
|
||||||
|
def set_tool_enabled(self, open_tool):
|
||||||
|
del open_tool
|
||||||
|
started.set()
|
||||||
|
assert release.wait(timeout=1.0)
|
||||||
|
if command_error is not None:
|
||||||
|
raise command_error
|
||||||
|
|
||||||
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||||
|
teleop._arm_name = "right_rm75"
|
||||||
|
teleop._adapter = Adapter()
|
||||||
|
teleop._tool_command_queue = None
|
||||||
|
teleop._tool_worker_stop = threading.Event()
|
||||||
|
teleop._tool_worker_thread = None
|
||||||
|
teleop._tool_state_lock = threading.Lock()
|
||||||
|
teleop._tool_target_open = True
|
||||||
|
teleop._tool_state_open = True
|
||||||
|
teleop._tool_command_pending = False
|
||||||
|
teleop._tool_command_failed = False
|
||||||
|
teleop.get_logger = lambda: FakeLogger()
|
||||||
|
teleop._start_tool_worker()
|
||||||
|
return teleop, started, release
|
||||||
|
|
||||||
|
|
||||||
|
def test_tool_state_changes_only_after_command_succeeds() -> None:
|
||||||
|
teleop, started, release = _tool_state_teleop()
|
||||||
|
try:
|
||||||
|
teleop._enqueue_tool_command(False, "test")
|
||||||
|
assert started.wait(timeout=1.0)
|
||||||
|
|
||||||
|
assert teleop._tool_state_snapshot() == (False, True, True, False)
|
||||||
|
|
||||||
|
release.set()
|
||||||
|
assert teleop._tool_command_queue is not None
|
||||||
|
teleop._tool_command_queue.join()
|
||||||
|
|
||||||
|
assert teleop._tool_state_snapshot() == (False, False, False, False)
|
||||||
|
finally:
|
||||||
|
release.set()
|
||||||
|
teleop._shutdown_tool_worker()
|
||||||
|
|
||||||
|
|
||||||
|
def test_tool_failure_keeps_previous_state_and_is_reported() -> None:
|
||||||
|
teleop, started, release = _tool_state_teleop(
|
||||||
|
command_error=RuntimeError("modbus failed")
|
||||||
|
)
|
||||||
|
try:
|
||||||
|
teleop._enqueue_tool_command(False, "test")
|
||||||
|
assert started.wait(timeout=1.0)
|
||||||
|
release.set()
|
||||||
|
assert teleop._tool_command_queue is not None
|
||||||
|
teleop._tool_command_queue.join()
|
||||||
|
|
||||||
|
assert teleop._tool_state_snapshot() == (False, True, False, True)
|
||||||
|
finally:
|
||||||
|
release.set()
|
||||||
|
teleop._shutdown_tool_worker()
|
||||||
|
|
||||||
|
|
||||||
def _joint_publishing_teleop() -> SingleArmVelocityTeleop:
|
def _joint_publishing_teleop() -> SingleArmVelocityTeleop:
|
||||||
names = [f"omnipic_joint_{index}" for index in range(1, 8)]
|
names = [f"omnipic_joint_{index}" for index in range(1, 8)]
|
||||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||||
@@ -611,7 +674,7 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
|
|||||||
assert teleop._ik_solver.solve_calls == 0
|
assert teleop._ik_solver.solve_calls == 0
|
||||||
|
|
||||||
|
|
||||||
def test_qp_failure_returns_none_and_keeps_last_known_good_target() -> None:
|
def test_qp_failure_returns_last_known_good_target() -> None:
|
||||||
class FailingSolver:
|
class FailingSolver:
|
||||||
def solve(self, target):
|
def solve(self, target):
|
||||||
del target
|
del target
|
||||||
@@ -623,13 +686,14 @@ def test_qp_failure_returns_none_and_keeps_last_known_good_target() -> None:
|
|||||||
teleop._arm_name = "right_rm75"
|
teleop._arm_name = "right_rm75"
|
||||||
teleop.get_logger = lambda: FakeLogger()
|
teleop.get_logger = lambda: FakeLogger()
|
||||||
|
|
||||||
target = teleop._solve_joint_target(np.eye(4))
|
target, qp_success = teleop._solve_joint_target(np.eye(4))
|
||||||
|
|
||||||
assert target is None
|
assert target == pytest.approx([0.1] * 7)
|
||||||
|
assert not qp_success
|
||||||
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
|
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
|
||||||
|
|
||||||
|
|
||||||
def test_qp_success_waits_for_send_before_updating_last_known_good_target() -> None:
|
def test_qp_success_updates_last_known_good_target() -> None:
|
||||||
class SuccessfulSolver:
|
class SuccessfulSolver:
|
||||||
def solve(self, target):
|
def solve(self, target):
|
||||||
del target
|
del target
|
||||||
@@ -641,57 +705,11 @@ def test_qp_success_waits_for_send_before_updating_last_known_good_target() -> N
|
|||||||
teleop._arm_name = "left_rm75"
|
teleop._arm_name = "left_rm75"
|
||||||
teleop.get_logger = lambda: FakeLogger()
|
teleop.get_logger = lambda: FakeLogger()
|
||||||
|
|
||||||
target = teleop._solve_joint_target(np.eye(4))
|
target, qp_success = teleop._solve_joint_target(np.eye(4))
|
||||||
|
|
||||||
assert target == pytest.approx([0.2] * 7)
|
assert target == pytest.approx([0.2] * 7)
|
||||||
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
|
assert qp_success
|
||||||
|
assert teleop._last_valid_joint_target == pytest.approx([0.2] * 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:
|
def test_enter_active_control_initializes_se3_orientation_state() -> None:
|
||||||
|
|||||||
@@ -6,7 +6,6 @@ from xml.etree import ElementTree
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
from xr_rm_teleop import placo_ik_solver
|
|
||||||
from xr_rm_teleop.placo_ik_solver import (
|
from xr_rm_teleop.placo_ik_solver import (
|
||||||
QP_ORIENTATION_TOLERANCE_RAD,
|
QP_ORIENTATION_TOLERANCE_RAD,
|
||||||
QP_POSITION_TOLERANCE_M,
|
QP_POSITION_TOLERANCE_M,
|
||||||
@@ -243,7 +242,6 @@ def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
|
|||||||
solver._frame_task = SimpleNamespace(T_a_b=None)
|
solver._frame_task = SimpleNamespace(T_a_b=None)
|
||||||
solver._solver = SimpleNamespace(solve=lambda update: None)
|
solver._solver = SimpleNamespace(solve=lambda update: None)
|
||||||
solver._validate_result = lambda result, previous: None
|
solver._validate_result = lambda result, previous: None
|
||||||
solver._update_auxiliary_task_weights = lambda: None
|
|
||||||
solver._target_errors = lambda: (2.1e-3, 0.0)
|
solver._target_errors = lambda: (2.1e-3, 0.0)
|
||||||
|
|
||||||
with pytest.raises(RuntimeError, match="QP did not converge after 30"):
|
with pytest.raises(RuntimeError, match="QP did not converge after 30"):
|
||||||
@@ -274,6 +272,20 @@ def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
|
|||||||
_validated_transform(transform)
|
_validated_transform(transform)
|
||||||
|
|
||||||
|
|
||||||
|
def test_joint_position_limits_returns_a_copy() -> None:
|
||||||
|
solver = object.__new__(PlacoIkSolver)
|
||||||
|
solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
|
||||||
|
|
||||||
|
first = solver.joint_position_limits
|
||||||
|
second = solver.joint_position_limits
|
||||||
|
|
||||||
|
assert first.shape == (7, 2)
|
||||||
|
assert np.isfinite(first).all()
|
||||||
|
assert np.all(first[:, 0] < first[:, 1])
|
||||||
|
first[0, 0] = 999.0
|
||||||
|
assert second[0, 0] != 999.0
|
||||||
|
|
||||||
|
|
||||||
def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
|
def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
|
||||||
solver = object.__new__(PlacoIkSolver)
|
solver = object.__new__(PlacoIkSolver)
|
||||||
solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
|
solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
|
||||||
@@ -287,130 +299,3 @@ def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
|
|||||||
solver._validate_result(np.full(7, 2.0))
|
solver._validate_result(np.full(7, 2.0))
|
||||||
with pytest.raises(ValueError, match="velocity"):
|
with pytest.raises(ValueError, match="velocity"):
|
||||||
solver._validate_result(np.full(7, 0.2))
|
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))
|
|
||||||
]
|
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -225,9 +225,12 @@ def peripheral_cfg(
|
|||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
|
|
||||||
if set_initial_tool_state:
|
if set_initial_tool_state:
|
||||||
set_tool_position(robot, percent=0.75, device=1, scissorgripper=scissorgripper)
|
set_tool_position(
|
||||||
time.sleep(1.5)
|
robot,
|
||||||
set_tool_position(robot, percent=0.15, device=1, scissorgripper=scissorgripper)
|
percent=1.0,
|
||||||
|
device=1,
|
||||||
|
scissorgripper=scissorgripper,
|
||||||
|
)
|
||||||
|
|
||||||
elif scissorgripper == 2:
|
elif scissorgripper == 2:
|
||||||
# 小型剪刀夹爪通过控制器 DO3/DO4 控制。
|
# 小型剪刀夹爪通过控制器 DO3/DO4 控制。
|
||||||
|
|||||||
@@ -31,14 +31,6 @@ QP_POSITION_TOLERANCE_M = 2e-3
|
|||||||
QP_ORIENTATION_TOLERANCE_RAD = 5e-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:
|
def _validated_transform(transform: np.ndarray) -> np.ndarray:
|
||||||
values = np.asarray(transform, dtype=float)
|
values = np.asarray(transform, dtype=float)
|
||||||
if values.shape != (4, 4) or not np.isfinite(values).all():
|
if values.shape != (4, 4) or not np.isfinite(values).all():
|
||||||
@@ -68,15 +60,6 @@ class PlacoIkSolver:
|
|||||||
urdf_path: str,
|
urdf_path: str,
|
||||||
dt: float,
|
dt: float,
|
||||||
arm: str,
|
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:
|
) -> None:
|
||||||
if dt <= 0.0:
|
if dt <= 0.0:
|
||||||
raise ValueError("dt must be positive")
|
raise ValueError("dt must be positive")
|
||||||
@@ -151,37 +134,12 @@ class PlacoIkSolver:
|
|||||||
]
|
]
|
||||||
)
|
)
|
||||||
self._actual_joints: np.ndarray | None = None
|
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 = placo.KinematicsSolver(self._robot)
|
||||||
self._solver.dt = dt
|
self._solver.dt = dt
|
||||||
self._solver.mask_fbase(True)
|
self._solver.mask_fbase(True)
|
||||||
for name in inactive_joint_names:
|
for name in inactive_joint_names:
|
||||||
self._solver.mask_dof(name)
|
self._solver.mask_dof(name)
|
||||||
self._solver.enable_joint_limits(True)
|
|
||||||
self._solver.enable_velocity_limits(True)
|
self._solver.enable_velocity_limits(True)
|
||||||
self._frame_task = self._solver.add_relative_frame_task(
|
self._frame_task = self._solver.add_relative_frame_task(
|
||||||
self._base_frame,
|
self._base_frame,
|
||||||
@@ -191,53 +149,6 @@ class PlacoIkSolver:
|
|||||||
self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
|
self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
|
||||||
self._solver.add_kinetic_energy_regularization_task(1e-6)
|
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
|
@property
|
||||||
def joint_names(self) -> list[str]:
|
def joint_names(self) -> list[str]:
|
||||||
return list(self._joint_names)
|
return list(self._joint_names)
|
||||||
@@ -246,6 +157,10 @@ class PlacoIkSolver:
|
|||||||
def base_configuration(self) -> list[float]:
|
def base_configuration(self) -> list[float]:
|
||||||
return self._robot.state.q[:7].tolist()
|
return self._robot.state.q[:7].tolist()
|
||||||
|
|
||||||
|
@property
|
||||||
|
def joint_position_limits(self) -> np.ndarray:
|
||||||
|
return self._joint_limits.copy()
|
||||||
|
|
||||||
def update_joint_state(self, joints: list[float]) -> np.ndarray:
|
def update_joint_state(self, joints: list[float]) -> np.ndarray:
|
||||||
values = np.asarray(joints, dtype=float)
|
values = np.asarray(joints, dtype=float)
|
||||||
if values.shape != (7,) or not np.isfinite(values).all():
|
if values.shape != (7,) or not np.isfinite(values).all():
|
||||||
@@ -272,64 +187,32 @@ class PlacoIkSolver:
|
|||||||
float(orientation_task.error_norm()),
|
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]:
|
def solve(self, target_tool_pose: np.ndarray) -> list[float]:
|
||||||
if self._actual_joints is None:
|
if self._actual_joints is None:
|
||||||
raise RuntimeError("joint state must be initialized before QP solve")
|
raise RuntimeError("joint state must be initialized before QP solve")
|
||||||
try:
|
self._frame_task.T_a_b = _validated_transform(
|
||||||
self._frame_task.T_a_b = _validated_transform(
|
target_tool_pose
|
||||||
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()
|
||||||
result = np.asarray(
|
result = np.asarray(
|
||||||
self._robot.state.q[self._q_offsets],
|
self._robot.state.q[self._q_offsets],
|
||||||
dtype=float,
|
dtype=float,
|
||||||
).copy()
|
).copy()
|
||||||
|
self._validate_result(result, previous)
|
||||||
position_error, orientation_error = self._target_errors()
|
position_error, orientation_error = self._target_errors()
|
||||||
if (
|
if (
|
||||||
position_error <= QP_POSITION_TOLERANCE_M
|
position_error <= QP_POSITION_TOLERANCE_M
|
||||||
@@ -337,32 +220,12 @@ class PlacoIkSolver:
|
|||||||
):
|
):
|
||||||
return result.tolist()
|
return result.tolist()
|
||||||
|
|
||||||
for _ in range(QP_MAX_ITERATIONS):
|
raise RuntimeError(
|
||||||
previous = result
|
"QP did not converge after "
|
||||||
self._update_auxiliary_task_weights()
|
f"{QP_MAX_ITERATIONS} iterations: "
|
||||||
self._solver.solve(True)
|
f"position_error={position_error:.6f} m, "
|
||||||
self._robot.update_kinematics()
|
f"orientation_error={orientation_error:.6f} rad"
|
||||||
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(
|
def _validate_result(
|
||||||
self,
|
self,
|
||||||
|
|||||||
@@ -237,9 +237,9 @@ class RealManAdapter:
|
|||||||
)
|
)
|
||||||
|
|
||||||
def read_joint_state(self) -> JointStateSnapshot:
|
def read_joint_state(self) -> JointStateSnapshot:
|
||||||
self._require_arm()
|
arm = self._require_arm()
|
||||||
started_at = time.monotonic()
|
started_at = time.monotonic()
|
||||||
result = self._arm.rm_get_joint_degree()
|
result = arm.rm_get_joint_degree()
|
||||||
finished_at = time.monotonic()
|
finished_at = time.monotonic()
|
||||||
if not isinstance(result, tuple) or len(result) != 2:
|
if not isinstance(result, tuple) or len(result) != 2:
|
||||||
raise RuntimeError(
|
raise RuntimeError(
|
||||||
@@ -256,10 +256,10 @@ class RealManAdapter:
|
|||||||
)
|
)
|
||||||
|
|
||||||
def send_joint_target(self, joints: list[float], follow: bool) -> None:
|
def send_joint_target(self, joints: list[float], follow: bool) -> None:
|
||||||
self._require_arm()
|
arm = self._require_arm()
|
||||||
if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
|
if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
|
||||||
raise ValueError("joint target must contain 7 finite values")
|
raise ValueError("joint target must contain 7 finite values")
|
||||||
ret = self._arm.rm_movej_canfd(
|
ret = arm.rm_movej_canfd(
|
||||||
[math.degrees(value) for value in joints],
|
[math.degrees(value) for value in joints],
|
||||||
follow,
|
follow,
|
||||||
0,
|
0,
|
||||||
@@ -315,9 +315,10 @@ class RealManAdapter:
|
|||||||
self._arm = None
|
self._arm = None
|
||||||
self._realtime_callback = None
|
self._realtime_callback = None
|
||||||
|
|
||||||
def _require_arm(self) -> None:
|
def _require_arm(self) -> Any:
|
||||||
if self._arm is None:
|
if self._arm is None:
|
||||||
raise RuntimeError("睿尔曼机械臂尚未连接")
|
raise RuntimeError("睿尔曼机械臂尚未连接")
|
||||||
|
return self._arm
|
||||||
|
|
||||||
def _on_realtime_arm_state(self, data: Any) -> None:
|
def _on_realtime_arm_state(self, data: Any) -> None:
|
||||||
if not self._accept_realtime_feedback:
|
if not self._accept_realtime_feedback:
|
||||||
@@ -457,11 +458,11 @@ class RealManAdapter:
|
|||||||
self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
|
self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
|
||||||
|
|
||||||
def move_to_initial_pose(self) -> None:
|
def move_to_initial_pose(self) -> None:
|
||||||
self._require_arm()
|
arm = self._require_arm()
|
||||||
if self._initial_joint_pose is None:
|
if self._initial_joint_pose is None:
|
||||||
raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
|
raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
|
||||||
|
|
||||||
ret = self._arm.rm_movej(
|
ret = arm.rm_movej(
|
||||||
self._initial_joint_pose,
|
self._initial_joint_pose,
|
||||||
self._init_move_speed,
|
self._init_move_speed,
|
||||||
0,
|
0,
|
||||||
|
|||||||
@@ -10,17 +10,19 @@ import math
|
|||||||
import queue
|
import queue
|
||||||
import threading
|
import threading
|
||||||
import time
|
import time
|
||||||
|
from dataclasses import dataclass
|
||||||
from typing import Iterable
|
from typing import Iterable
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import rclpy
|
import rclpy
|
||||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
|
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||||
from rclpy.time import Time
|
from rclpy.time import Time
|
||||||
from sensor_msgs.msg import JointState
|
from sensor_msgs.msg import JointState
|
||||||
from std_msgs.msg import Bool
|
from std_msgs.msg import Bool
|
||||||
|
|
||||||
from xr_rm_interfaces.msg import XrController
|
from xr_rm_interfaces.msg import ActControlSample, XrController
|
||||||
|
|
||||||
from .fun_peripheral import load_peripheral_config
|
from .fun_peripheral import load_peripheral_config
|
||||||
from .placo_ik_solver import PlacoIkSolver
|
from .placo_ik_solver import PlacoIkSolver
|
||||||
@@ -31,6 +33,30 @@ from .realman_adapter import (
|
|||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class _ActCycleContext:
|
||||||
|
control_seq: int
|
||||||
|
control_monotonic_ns: int
|
||||||
|
feedback_monotonic_ns: int = -1
|
||||||
|
action_monotonic_ns: int = -1
|
||||||
|
feedback_age_ms: float = math.inf
|
||||||
|
qp_duration_ms: float = 0.0
|
||||||
|
q_actual: list[float] | None = None
|
||||||
|
q_qp_raw: list[float] | None = None
|
||||||
|
q_target: list[float] | None = None
|
||||||
|
current_pose: np.ndarray | None = None
|
||||||
|
raw_target_pose: np.ndarray | None = None
|
||||||
|
target_pose: np.ndarray | None = None
|
||||||
|
command_velocity: list[float] | None = None
|
||||||
|
feedback_valid: bool = False
|
||||||
|
command_sent: bool = False
|
||||||
|
send_failed: bool = False
|
||||||
|
qp_attempted: bool = False
|
||||||
|
qp_success: bool = False
|
||||||
|
target_clamped: bool = False
|
||||||
|
control_fault: bool = False
|
||||||
|
|
||||||
|
|
||||||
def _norm(values: Iterable[float]) -> float:
|
def _norm(values: Iterable[float]) -> float:
|
||||||
return math.sqrt(sum(value * value for value in values))
|
return math.sqrt(sum(value * value for value in values))
|
||||||
|
|
||||||
@@ -201,14 +227,6 @@ 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("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("use_mock", True)
|
||||||
self.declare_parameter("robot_urdf_path", "")
|
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_ip", "192.168.1.18")
|
||||||
self.declare_parameter("robot_port", 8080)
|
self.declare_parameter("robot_port", 8080)
|
||||||
self.declare_parameter("realtime_push_host_ip", "")
|
self.declare_parameter("realtime_push_host_ip", "")
|
||||||
@@ -268,30 +286,6 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
|
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._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
|
||||||
self._use_mock = self._bool_parameter("use_mock")
|
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._follow = self._bool_parameter("follow")
|
||||||
self._enable_tool_control = self._bool_parameter("enable_tool_control")
|
self._enable_tool_control = self._bool_parameter("enable_tool_control")
|
||||||
self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
|
self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
|
||||||
@@ -323,12 +317,19 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
self._latest_joint_positions: list[float] | None = None
|
self._latest_joint_positions: list[float] | None = None
|
||||||
self._last_joint_command_target: list[float] | None = None
|
self._last_joint_command_target: list[float] | None = None
|
||||||
self._last_joint_command_velocity: list[float] | None = None
|
self._last_joint_command_velocity: list[float] | None = None
|
||||||
|
self._last_successful_action_target: list[float] | None = None
|
||||||
|
self._act_control_seq = 0
|
||||||
self._joint_feedback_ready = False
|
self._joint_feedback_ready = False
|
||||||
self._grip_rearm_required = False
|
self._grip_rearm_required = False
|
||||||
self._feedback_resync_attempted = False
|
self._feedback_resync_attempted = False
|
||||||
self._control_fault_latched = False
|
self._control_fault_latched = False
|
||||||
self._stop_sent = True
|
self._stop_sent = True
|
||||||
self._trigger_tool_open = True
|
self._trigger_tool_open = True
|
||||||
|
self._tool_state_lock = threading.Lock()
|
||||||
|
self._tool_target_open = True
|
||||||
|
self._tool_state_open: bool | None = None
|
||||||
|
self._tool_command_pending = False
|
||||||
|
self._tool_command_failed = False
|
||||||
self._last_primary_pressed: bool | None = None
|
self._last_primary_pressed: bool | None = None
|
||||||
self._last_trigger_pressed: bool | None = None
|
self._last_trigger_pressed: bool | None = None
|
||||||
self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
|
self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
|
||||||
@@ -360,18 +361,6 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
str(self.get_parameter("robot_urdf_path").value),
|
str(self.get_parameter("robot_urdf_path").value),
|
||||||
self._dt,
|
self._dt,
|
||||||
peripheral_arm,
|
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}"
|
debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
|
||||||
self._joint_state_pub = self.create_publisher(
|
self._joint_state_pub = self.create_publisher(
|
||||||
@@ -384,6 +373,15 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
f"{debug_ns}/joint_target",
|
f"{debug_ns}/joint_target",
|
||||||
10,
|
10,
|
||||||
)
|
)
|
||||||
|
self._act_sample_pub = self.create_publisher(
|
||||||
|
ActControlSample,
|
||||||
|
f"{debug_ns}/act_control_sample",
|
||||||
|
QoSProfile(
|
||||||
|
history=HistoryPolicy.KEEP_LAST,
|
||||||
|
depth=10,
|
||||||
|
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||||
|
),
|
||||||
|
)
|
||||||
self._adapter = self._make_adapter()
|
self._adapter = self._make_adapter()
|
||||||
self._adapter.connect()
|
self._adapter.connect()
|
||||||
self._initialize_joint_state()
|
self._initialize_joint_state()
|
||||||
@@ -474,11 +472,20 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
|
|
||||||
def _setup_tool_control(self) -> None:
|
def _setup_tool_control(self) -> None:
|
||||||
peripheral_arm = self._peripheral_arm_name()
|
peripheral_arm = self._peripheral_arm_name()
|
||||||
if self._bool_parameter("configure_peripheral_on_connect"):
|
configure_on_connect = self._bool_parameter(
|
||||||
|
"configure_peripheral_on_connect"
|
||||||
|
)
|
||||||
|
if configure_on_connect:
|
||||||
self._adapter.configure_peripheral(
|
self._adapter.configure_peripheral(
|
||||||
self._peripheral_config,
|
self._peripheral_config,
|
||||||
peripheral_arm,
|
peripheral_arm,
|
||||||
)
|
)
|
||||||
|
if self._peripheral_config.set_initial_tool_state:
|
||||||
|
with self._tool_state_lock:
|
||||||
|
self._tool_target_open = True
|
||||||
|
self._tool_state_open = True
|
||||||
|
self._tool_command_pending = False
|
||||||
|
self._tool_command_failed = False
|
||||||
|
|
||||||
if not self._enable_tool_control:
|
if not self._enable_tool_control:
|
||||||
if self._enable_trigger_gripper_control:
|
if self._enable_trigger_gripper_control:
|
||||||
@@ -528,6 +535,10 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
)
|
)
|
||||||
return
|
return
|
||||||
|
|
||||||
|
with self._tool_state_lock:
|
||||||
|
self._tool_target_open = open_tool
|
||||||
|
self._tool_command_pending = True
|
||||||
|
|
||||||
item = (open_tool, source)
|
item = (open_tool, source)
|
||||||
while True:
|
while True:
|
||||||
try:
|
try:
|
||||||
@@ -556,15 +567,34 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
try:
|
try:
|
||||||
self._adapter.set_tool_enabled(open_tool)
|
self._adapter.set_tool_enabled(open_tool)
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
|
with self._tool_state_lock:
|
||||||
|
self._tool_command_failed = True
|
||||||
self.get_logger().error(
|
self.get_logger().error(
|
||||||
f"{self._arm_name} tool {action} failed from {source}: {exc}"
|
f"{self._arm_name} tool {action} failed from {source}: {exc}"
|
||||||
)
|
)
|
||||||
continue
|
continue
|
||||||
|
with self._tool_state_lock:
|
||||||
|
self._tool_state_open = open_tool
|
||||||
|
self._tool_command_failed = False
|
||||||
self.get_logger().info(
|
self.get_logger().info(
|
||||||
f"{self._arm_name} tool {action} command sent from {source}"
|
f"{self._arm_name} tool {action} command sent from {source}"
|
||||||
)
|
)
|
||||||
finally:
|
finally:
|
||||||
self._tool_command_queue.task_done()
|
self._tool_command_queue.task_done()
|
||||||
|
if self._tool_command_queue.empty():
|
||||||
|
with self._tool_state_lock:
|
||||||
|
self._tool_command_pending = False
|
||||||
|
|
||||||
|
def _tool_state_snapshot(
|
||||||
|
self,
|
||||||
|
) -> tuple[bool, bool | None, bool, bool]:
|
||||||
|
with self._tool_state_lock:
|
||||||
|
return (
|
||||||
|
self._tool_target_open,
|
||||||
|
self._tool_state_open,
|
||||||
|
self._tool_command_pending,
|
||||||
|
self._tool_command_failed,
|
||||||
|
)
|
||||||
|
|
||||||
def _peripheral_arm_name(self) -> str:
|
def _peripheral_arm_name(self) -> str:
|
||||||
configured = str(self.get_parameter("peripheral_arm").value).strip().lower()
|
configured = str(self.get_parameter("peripheral_arm").value).strip().lower()
|
||||||
@@ -626,6 +656,18 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
self._enqueue_tool_command(self._trigger_tool_open, "trigger")
|
self._enqueue_tool_command(self._trigger_tool_open, "trigger")
|
||||||
|
|
||||||
def _control_tick(self) -> None:
|
def _control_tick(self) -> None:
|
||||||
|
control_seq = getattr(self, "_act_control_seq", 0)
|
||||||
|
self._act_control_seq = control_seq + 1
|
||||||
|
cycle = _ActCycleContext(
|
||||||
|
control_seq=control_seq,
|
||||||
|
control_monotonic_ns=time.monotonic_ns(),
|
||||||
|
)
|
||||||
|
try:
|
||||||
|
self._control_tick_impl(cycle)
|
||||||
|
finally:
|
||||||
|
self._publish_act_control_sample(cycle)
|
||||||
|
|
||||||
|
def _control_tick_impl(self, cycle: _ActCycleContext) -> None:
|
||||||
tick_started_ns = time.perf_counter_ns()
|
tick_started_ns = time.perf_counter_ns()
|
||||||
last_tick_started_ns = getattr(
|
last_tick_started_ns = getattr(
|
||||||
self,
|
self,
|
||||||
@@ -640,10 +682,12 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
)
|
)
|
||||||
now = self.get_clock().now()
|
now = self.get_clock().now()
|
||||||
if self._control_fault_latched:
|
if self._control_fault_latched:
|
||||||
|
cycle.control_fault = True
|
||||||
return
|
return
|
||||||
|
|
||||||
snapshot = self._adapter.get_latest_joint_state()
|
snapshot = self._adapter.get_latest_joint_state()
|
||||||
if not self._joint_snapshot_is_motion_ready(snapshot):
|
if not self._joint_snapshot_is_motion_ready(snapshot):
|
||||||
|
cycle.control_fault = True
|
||||||
self._grip_rearm_required = True
|
self._grip_rearm_required = True
|
||||||
if self._joint_feedback_ready:
|
if self._joint_feedback_ready:
|
||||||
self.get_logger().warn(
|
self.get_logger().warn(
|
||||||
@@ -654,13 +698,18 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
self._safe_stop(reset_active=True)
|
self._safe_stop(reset_active=True)
|
||||||
return
|
return
|
||||||
assert snapshot is not None
|
assert snapshot is not None
|
||||||
|
cycle.q_actual = list(snapshot.positions)
|
||||||
|
cycle.feedback_monotonic_ns = int(snapshot.received_at * 1e9)
|
||||||
feedback_age = time.monotonic() - snapshot.received_at
|
feedback_age = time.monotonic() - snapshot.received_at
|
||||||
|
cycle.feedback_age_ms = feedback_age * 1000.0
|
||||||
if feedback_age < 0.0:
|
if feedback_age < 0.0:
|
||||||
|
cycle.control_fault = True
|
||||||
self._grip_rearm_required = True
|
self._grip_rearm_required = True
|
||||||
self._joint_feedback_ready = False
|
self._joint_feedback_ready = False
|
||||||
self._safe_stop(reset_active=True)
|
self._safe_stop(reset_active=True)
|
||||||
return
|
return
|
||||||
if feedback_age > self._command_timeout_sec:
|
if feedback_age > self._command_timeout_sec:
|
||||||
|
cycle.control_fault = True
|
||||||
self._handle_stale_joint_feedback(feedback_age)
|
self._handle_stale_joint_feedback(feedback_age)
|
||||||
return
|
return
|
||||||
|
|
||||||
@@ -668,6 +717,7 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
try:
|
try:
|
||||||
current_pose = self._sync_joint_feedback(snapshot)
|
current_pose = self._sync_joint_feedback(snapshot)
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
|
cycle.control_fault = True
|
||||||
self.get_logger().error(
|
self.get_logger().error(
|
||||||
f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}",
|
f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}",
|
||||||
throttle_duration_sec=1.0,
|
throttle_duration_sec=1.0,
|
||||||
@@ -676,6 +726,8 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
self._grip_rearm_required = True
|
self._grip_rearm_required = True
|
||||||
self._safe_stop(reset_active=True)
|
self._safe_stop(reset_active=True)
|
||||||
return
|
return
|
||||||
|
cycle.current_pose = current_pose
|
||||||
|
cycle.feedback_valid = True
|
||||||
if not self._joint_feedback_ready:
|
if not self._joint_feedback_ready:
|
||||||
if self._grip_rearm_required:
|
if self._grip_rearm_required:
|
||||||
message = (
|
message = (
|
||||||
@@ -724,6 +776,7 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
try:
|
try:
|
||||||
controller_quat = self._controller_quaternion(self._last_msg)
|
controller_quat = self._controller_quaternion(self._last_msg)
|
||||||
except ValueError as exc:
|
except ValueError as exc:
|
||||||
|
cycle.control_fault = True
|
||||||
self.get_logger().warn(
|
self.get_logger().warn(
|
||||||
f"{self._arm_name} XR 手柄姿态无效，停止输出：{exc}",
|
f"{self._arm_name} XR 手柄姿态无效，停止输出：{exc}",
|
||||||
throttle_duration_sec=1.0,
|
throttle_duration_sec=1.0,
|
||||||
@@ -768,23 +821,35 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
sent_target,
|
sent_target,
|
||||||
sent_orientation,
|
sent_orientation,
|
||||||
)
|
)
|
||||||
|
cycle.raw_target_pose = raw_target_pose
|
||||||
|
cycle.target_pose = target_pose
|
||||||
|
cycle.command_velocity = list(velocity)
|
||||||
|
cycle.target_clamped = target_clamped
|
||||||
|
|
||||||
self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
|
self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
|
||||||
|
cycle.qp_attempted = True
|
||||||
qp_started_ns = time.perf_counter_ns()
|
qp_started_ns = time.perf_counter_ns()
|
||||||
joint_target = self._solve_joint_target(target_pose)
|
joint_target, qp_success = self._solve_joint_target(target_pose)
|
||||||
qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6
|
qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6
|
||||||
|
cycle.qp_duration_ms = qp_ms
|
||||||
|
cycle.q_qp_raw = list(joint_target)
|
||||||
|
cycle.qp_success = qp_success
|
||||||
send_started_ns = time.perf_counter_ns()
|
send_started_ns = time.perf_counter_ns()
|
||||||
sent = self._send_and_commit_joint_target(
|
sent = self._send_joint_target(joint_target)
|
||||||
joint_target,
|
|
||||||
filtered_target,
|
|
||||||
filtered_orientation,
|
|
||||||
sent_target,
|
|
||||||
sent_orientation,
|
|
||||||
now,
|
|
||||||
)
|
|
||||||
send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6
|
send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6
|
||||||
if sent:
|
if sent:
|
||||||
|
assert self._last_joint_command_target is not None
|
||||||
|
final_target = list(self._last_joint_command_target)
|
||||||
|
self._last_successful_action_target = final_target
|
||||||
|
cycle.q_target = final_target
|
||||||
|
cycle.action_monotonic_ns = time.monotonic_ns()
|
||||||
|
cycle.command_sent = True
|
||||||
|
self._last_sent_target = sent_target
|
||||||
|
self._last_sent_orientation = sent_orientation.copy()
|
||||||
|
self._last_command_time = now
|
||||||
self._stop_sent = False
|
self._stop_sent = False
|
||||||
|
else:
|
||||||
|
cycle.send_failed = True
|
||||||
total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6
|
total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6
|
||||||
try:
|
try:
|
||||||
self._record_timing_sample(
|
self._record_timing_sample(
|
||||||
@@ -915,15 +980,17 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
|
|
||||||
def _filter_target(self, target: list[float]) -> list[float]:
|
def _filter_target(self, target: list[float]) -> list[float]:
|
||||||
if self._filtered_target is None:
|
if self._filtered_target is None:
|
||||||
|
self._filtered_target = list(target)
|
||||||
return list(target)
|
return list(target)
|
||||||
|
|
||||||
delta = [target[i] - self._filtered_target[i] for i in range(3)]
|
delta = [target[i] - self._filtered_target[i] for i in range(3)]
|
||||||
distance = _norm(delta)
|
distance = _norm(delta)
|
||||||
alpha = self._adaptive_filter_alpha(distance)
|
alpha = self._adaptive_filter_alpha(distance)
|
||||||
return [
|
self._filtered_target = [
|
||||||
alpha * target[i] + (1.0 - alpha) * self._filtered_target[i]
|
alpha * target[i] + (1.0 - alpha) * self._filtered_target[i]
|
||||||
for i in range(3)
|
for i in range(3)
|
||||||
]
|
]
|
||||||
|
return list(self._filtered_target)
|
||||||
|
|
||||||
def _adaptive_filter_alpha(self, distance: float) -> float:
|
def _adaptive_filter_alpha(self, distance: float) -> float:
|
||||||
if self._target_filter_fast_threshold_m <= 1e-9:
|
if self._target_filter_fast_threshold_m <= 1e-9:
|
||||||
@@ -962,15 +1029,17 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
|
|
||||||
def _filter_orientation_target(self, target_rotation: np.ndarray) -> np.ndarray:
|
def _filter_orientation_target(self, target_rotation: np.ndarray) -> np.ndarray:
|
||||||
if self._filtered_orientation_target is None:
|
if self._filtered_orientation_target is None:
|
||||||
return _project_rotation(target_rotation)
|
self._filtered_orientation_target = _project_rotation(target_rotation)
|
||||||
|
return self._filtered_orientation_target.copy()
|
||||||
|
|
||||||
error = _so3_log(
|
error = _so3_log(
|
||||||
target_rotation @ self._filtered_orientation_target.T
|
target_rotation @ self._filtered_orientation_target.T
|
||||||
)
|
)
|
||||||
return _project_rotation(
|
self._filtered_orientation_target = _project_rotation(
|
||||||
_so3_exp(self._orientation_filter_alpha * error)
|
_so3_exp(self._orientation_filter_alpha * error)
|
||||||
@ self._filtered_orientation_target
|
@ self._filtered_orientation_target
|
||||||
)
|
)
|
||||||
|
return self._filtered_orientation_target.copy()
|
||||||
|
|
||||||
def _limit_orientation_step(
|
def _limit_orientation_step(
|
||||||
self,
|
self,
|
||||||
@@ -1243,7 +1312,7 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
def _solve_joint_target(
|
def _solve_joint_target(
|
||||||
self,
|
self,
|
||||||
target_pose: np.ndarray,
|
target_pose: np.ndarray,
|
||||||
) -> list[float] | None:
|
) -> tuple[list[float], bool]:
|
||||||
if self._last_valid_joint_target is None:
|
if self._last_valid_joint_target is None:
|
||||||
raise RuntimeError("valid joint feedback has not been initialized")
|
raise RuntimeError("valid joint feedback has not been initialized")
|
||||||
try:
|
try:
|
||||||
@@ -1253,27 +1322,9 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
|
f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
|
||||||
throttle_duration_sec=1.0,
|
throttle_duration_sec=1.0,
|
||||||
)
|
)
|
||||||
return None
|
return list(self._last_valid_joint_target), False
|
||||||
return list(result)
|
self._last_valid_joint_target = list(result)
|
||||||
|
return list(result), True
|
||||||
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:
|
def _safe_stop(self, reset_active: bool) -> None:
|
||||||
if not self._stop_sent:
|
if not self._stop_sent:
|
||||||
@@ -1445,6 +1496,122 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
self._cmd_vel_pub.publish(velocity_msg)
|
self._cmd_vel_pub.publish(velocity_msg)
|
||||||
self._target_clamped_pub.publish(clamped_msg)
|
self._target_clamped_pub.publish(clamped_msg)
|
||||||
|
|
||||||
|
def _build_act_control_sample(
|
||||||
|
self,
|
||||||
|
cycle: _ActCycleContext,
|
||||||
|
) -> ActControlSample:
|
||||||
|
message = ActControlSample()
|
||||||
|
message.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
message.header.frame_id = "rm_base"
|
||||||
|
message.control_seq = cycle.control_seq
|
||||||
|
message.control_monotonic_ns = cycle.control_monotonic_ns
|
||||||
|
message.feedback_monotonic_ns = cycle.feedback_monotonic_ns
|
||||||
|
message.action_monotonic_ns = cycle.action_monotonic_ns
|
||||||
|
message.feedback_age_ms = float(cycle.feedback_age_ms)
|
||||||
|
message.qp_duration_ms = float(cycle.qp_duration_ms)
|
||||||
|
|
||||||
|
q_actual = cycle.q_actual or [0.0] * 7
|
||||||
|
held_target = (
|
||||||
|
cycle.q_target
|
||||||
|
or self._last_successful_action_target
|
||||||
|
or q_actual
|
||||||
|
)
|
||||||
|
qp_target = cycle.q_qp_raw or held_target
|
||||||
|
limits = np.asarray(
|
||||||
|
self._ik_solver.joint_position_limits,
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
if limits.shape != (7, 2) or not np.isfinite(limits).all():
|
||||||
|
raise ValueError("joint limits must have finite shape (7, 2)")
|
||||||
|
message.q_actual = [float(value) for value in q_actual]
|
||||||
|
message.q_qp_raw = [float(value) for value in qp_target]
|
||||||
|
message.q_target = [float(value) for value in held_target]
|
||||||
|
message.joint_lower_limits = limits[:, 0].tolist()
|
||||||
|
message.joint_upper_limits = limits[:, 1].tolist()
|
||||||
|
|
||||||
|
current_pose = cycle.current_pose
|
||||||
|
if current_pose is None:
|
||||||
|
current_pose = self._debug_pose_fallback()
|
||||||
|
if current_pose is None:
|
||||||
|
current_pose = np.eye(4)
|
||||||
|
raw_target_pose = cycle.raw_target_pose
|
||||||
|
if raw_target_pose is None:
|
||||||
|
raw_target_pose = current_pose
|
||||||
|
target_pose = cycle.target_pose
|
||||||
|
if target_pose is None:
|
||||||
|
target_pose = current_pose
|
||||||
|
message.tcp_current = self._pose_msg(
|
||||||
|
message.header.stamp,
|
||||||
|
current_pose,
|
||||||
|
).pose
|
||||||
|
message.tcp_raw_target = self._pose_msg(
|
||||||
|
message.header.stamp,
|
||||||
|
raw_target_pose,
|
||||||
|
).pose
|
||||||
|
message.tcp_target = self._pose_msg(
|
||||||
|
message.header.stamp,
|
||||||
|
target_pose,
|
||||||
|
).pose
|
||||||
|
velocity = cycle.command_velocity or [0.0] * 6
|
||||||
|
if len(velocity) != 6:
|
||||||
|
raise ValueError("ACT command velocity must contain 6 values")
|
||||||
|
message.tcp_command_velocity.linear.x = float(velocity[0])
|
||||||
|
message.tcp_command_velocity.linear.y = float(velocity[1])
|
||||||
|
message.tcp_command_velocity.linear.z = float(velocity[2])
|
||||||
|
message.tcp_command_velocity.angular.x = float(velocity[3])
|
||||||
|
message.tcp_command_velocity.angular.y = float(velocity[4])
|
||||||
|
message.tcp_command_velocity.angular.z = float(velocity[5])
|
||||||
|
|
||||||
|
controller = self._last_msg
|
||||||
|
if controller is not None:
|
||||||
|
message.pico_pose = controller.pose
|
||||||
|
message.pico_grip = bool(controller.grip)
|
||||||
|
message.pico_trigger = float(controller.trigger)
|
||||||
|
message.pico_primary = bool(controller.primary)
|
||||||
|
message.pico_secondary = bool(controller.secondary)
|
||||||
|
message.pico_axis = [float(value) for value in controller.axis]
|
||||||
|
|
||||||
|
tool_target, tool_state, tool_pending, tool_failed = (
|
||||||
|
self._tool_state_snapshot()
|
||||||
|
)
|
||||||
|
message.gripper_target_open = tool_target
|
||||||
|
message.gripper_state_known = tool_state is not None
|
||||||
|
message.gripper_state_open = bool(tool_state)
|
||||||
|
message.gripper_command_pending = tool_pending
|
||||||
|
message.gripper_command_failed = tool_failed
|
||||||
|
|
||||||
|
message.teleop_active = bool(self._active)
|
||||||
|
message.feedback_valid = cycle.feedback_valid
|
||||||
|
message.action_valid = bool(
|
||||||
|
self._last_successful_action_target is not None
|
||||||
|
and cycle.feedback_valid
|
||||||
|
and not cycle.send_failed
|
||||||
|
and not cycle.control_fault
|
||||||
|
)
|
||||||
|
message.command_sent = cycle.command_sent
|
||||||
|
message.qp_attempted = cycle.qp_attempted
|
||||||
|
message.qp_success = cycle.qp_success
|
||||||
|
message.target_clamped = cycle.target_clamped
|
||||||
|
message.control_fault = bool(
|
||||||
|
cycle.control_fault or self._control_fault_latched
|
||||||
|
)
|
||||||
|
return message
|
||||||
|
|
||||||
|
def _publish_act_control_sample(
|
||||||
|
self,
|
||||||
|
cycle: _ActCycleContext,
|
||||||
|
) -> None:
|
||||||
|
publisher = getattr(self, "_act_sample_pub", None)
|
||||||
|
if publisher is None:
|
||||||
|
return
|
||||||
|
try:
|
||||||
|
publisher.publish(self._build_act_control_sample(cycle))
|
||||||
|
except Exception as exc:
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"{self._arm_name} ACT原子样本发布失败：{exc}",
|
||||||
|
throttle_duration_sec=1.0,
|
||||||
|
)
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped:
|
def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped:
|
||||||
transform = _make_transform(pose[:3, 3], pose[:3, :3])
|
transform = _make_transform(pose[:3, 3], pose[:3, :3])
|
||||||
@@ -1526,35 +1693,6 @@ class SingleArmVelocityTeleop(Node):
|
|||||||
raise ValueError("joint_max_speed must be > 0")
|
raise ValueError("joint_max_speed must be > 0")
|
||||||
if self._joint_command_max_acceleration <= 0.0:
|
if self._joint_command_max_acceleration <= 0.0:
|
||||||
raise ValueError("joint_max_acc must be > 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:
|
def _shutdown_tool_worker(self) -> None:
|
||||||
if self._tool_worker_thread is None or self._tool_command_queue is None:
|
if self._tool_worker_thread is None or self._tool_command_queue is None:
|
||||||
|
|||||||
Reference in New Issue
Block a user