Compare commits
11
Commits
f6b484d168
...
e20c5a983e
| 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 |
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`，由未来的上层任务状态机协调，避免让一个低层策略混合两种动作接口和
|
||||
任务阶段。
|
||||
@@ -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
|
||||
@@ -27,3 +27,4 @@ arms:
|
||||
scissorgripper: 0
|
||||
right:
|
||||
scissorgripper: 1
|
||||
set_initial_tool_state: true
|
||||
|
||||
@@ -80,6 +80,29 @@ def _validate_mujoco_mode(arm: str, use_mujoco: bool) -> None:
|
||||
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(
|
||||
arm: str,
|
||||
use_mock: bool,
|
||||
@@ -164,10 +187,14 @@ def _launch_setup(context, *args, **kwargs):
|
||||
use_mujoco = _as_bool(
|
||||
LaunchConfiguration("use_mujoco").perform(context)
|
||||
)
|
||||
record_act = _as_bool(
|
||||
LaunchConfiguration("record_act").perform(context)
|
||||
)
|
||||
|
||||
if arm not in ("left", "right", "both"):
|
||||
raise ValueError("arm must be one of: left, right, both")
|
||||
_validate_mujoco_mode(arm, use_mujoco)
|
||||
_validate_act_mode(arm, use_mock, record_act)
|
||||
|
||||
nodes = [_udp_receiver_node()]
|
||||
if arm == "both":
|
||||
@@ -176,6 +203,8 @@ def _launch_setup(context, *args, **kwargs):
|
||||
nodes.append(_single_arm_node(arm, use_mock))
|
||||
if use_mujoco:
|
||||
nodes.append(_mujoco_node())
|
||||
if record_act:
|
||||
nodes.append(_act_recorder_node())
|
||||
return nodes
|
||||
|
||||
|
||||
@@ -187,6 +216,8 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument("use_mock", default_value="true"),
|
||||
# true 时额外启动只读 MuJoCo 双臂显示，默认不改变现有启动行为。
|
||||
DeclareLaunchArgument("use_mujoco", default_value="false"),
|
||||
# true 时只允许右臂真机，并启动独立 ACT 数据采集节点。
|
||||
DeclareLaunchArgument("record_act", default_value="false"),
|
||||
# UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
|
||||
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
||||
DeclareLaunchArgument("udp_port", default_value="15000"),
|
||||
|
||||
@@ -41,3 +41,37 @@ def test_udp_receiver_exit_shuts_down_launch() -> None:
|
||||
receiver = arm_debug_launch._udp_receiver_node()
|
||||
|
||||
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
|
||||
|
||||
@@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/ActControlSample.msg"
|
||||
"msg/XrController.msg"
|
||||
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"],
|
||||
entry_points={
|
||||
"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",
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -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)
|
||||
@@ -6,13 +6,14 @@ from types import ModuleType, SimpleNamespace
|
||||
import pytest
|
||||
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 MockRealManAdapter
|
||||
from xr_rm_teleop.fun_peripheral import (
|
||||
PeripheralConfig,
|
||||
_configure_tool_frame,
|
||||
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]
|
||||
|
||||
|
||||
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(
|
||||
("config_name", "node_names"),
|
||||
[
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
|
||||
@@ -42,6 +43,69 @@ class FakePublisher:
|
||||
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:
|
||||
names = [f"omnipic_joint_{index}" for index in range(1, 8)]
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
@@ -622,9 +686,10 @@ def test_qp_failure_returns_last_known_good_target() -> None:
|
||||
teleop._arm_name = "right_rm75"
|
||||
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.1] * 7)
|
||||
assert not qp_success
|
||||
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
|
||||
|
||||
|
||||
@@ -640,9 +705,10 @@ def test_qp_success_updates_last_known_good_target() -> None:
|
||||
teleop._arm_name = "left_rm75"
|
||||
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 qp_success
|
||||
assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
|
||||
|
||||
|
||||
|
||||
@@ -272,6 +272,20 @@ def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
|
||||
_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:
|
||||
solver = object.__new__(PlacoIkSolver)
|
||||
solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -225,9 +225,12 @@ def peripheral_cfg(
|
||||
time.sleep(0.5)
|
||||
|
||||
if set_initial_tool_state:
|
||||
set_tool_position(robot, percent=0.75, device=1, scissorgripper=scissorgripper)
|
||||
time.sleep(1.5)
|
||||
set_tool_position(robot, percent=0.15, device=1, scissorgripper=scissorgripper)
|
||||
set_tool_position(
|
||||
robot,
|
||||
percent=1.0,
|
||||
device=1,
|
||||
scissorgripper=scissorgripper,
|
||||
)
|
||||
|
||||
elif scissorgripper == 2:
|
||||
# 小型剪刀夹爪通过控制器 DO3/DO4 控制。
|
||||
|
||||
@@ -157,6 +157,10 @@ class PlacoIkSolver:
|
||||
def base_configuration(self) -> list[float]:
|
||||
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:
|
||||
values = np.asarray(joints, dtype=float)
|
||||
if values.shape != (7,) or not np.isfinite(values).all():
|
||||
|
||||
@@ -10,17 +10,19 @@ import math
|
||||
import queue
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from typing import Iterable
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
from rclpy.time import Time
|
||||
from sensor_msgs.msg import JointState
|
||||
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 .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:
|
||||
return math.sqrt(sum(value * value for value in values))
|
||||
|
||||
@@ -291,12 +317,19 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._latest_joint_positions: list[float] | None = None
|
||||
self._last_joint_command_target: 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._grip_rearm_required = False
|
||||
self._feedback_resync_attempted = False
|
||||
self._control_fault_latched = False
|
||||
self._stop_sent = 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_trigger_pressed: bool | None = None
|
||||
self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
|
||||
@@ -340,6 +373,15 @@ class SingleArmVelocityTeleop(Node):
|
||||
f"{debug_ns}/joint_target",
|
||||
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.connect()
|
||||
self._initialize_joint_state()
|
||||
@@ -430,11 +472,20 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
def _setup_tool_control(self) -> None:
|
||||
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._peripheral_config,
|
||||
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 self._enable_trigger_gripper_control:
|
||||
@@ -484,6 +535,10 @@ class SingleArmVelocityTeleop(Node):
|
||||
)
|
||||
return
|
||||
|
||||
with self._tool_state_lock:
|
||||
self._tool_target_open = open_tool
|
||||
self._tool_command_pending = True
|
||||
|
||||
item = (open_tool, source)
|
||||
while True:
|
||||
try:
|
||||
@@ -512,15 +567,34 @@ class SingleArmVelocityTeleop(Node):
|
||||
try:
|
||||
self._adapter.set_tool_enabled(open_tool)
|
||||
except Exception as exc:
|
||||
with self._tool_state_lock:
|
||||
self._tool_command_failed = True
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} tool {action} failed from {source}: {exc}"
|
||||
)
|
||||
continue
|
||||
with self._tool_state_lock:
|
||||
self._tool_state_open = open_tool
|
||||
self._tool_command_failed = False
|
||||
self.get_logger().info(
|
||||
f"{self._arm_name} tool {action} command sent from {source}"
|
||||
)
|
||||
finally:
|
||||
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:
|
||||
configured = str(self.get_parameter("peripheral_arm").value).strip().lower()
|
||||
@@ -582,6 +656,18 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._enqueue_tool_command(self._trigger_tool_open, "trigger")
|
||||
|
||||
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()
|
||||
last_tick_started_ns = getattr(
|
||||
self,
|
||||
@@ -596,10 +682,12 @@ class SingleArmVelocityTeleop(Node):
|
||||
)
|
||||
now = self.get_clock().now()
|
||||
if self._control_fault_latched:
|
||||
cycle.control_fault = True
|
||||
return
|
||||
|
||||
snapshot = self._adapter.get_latest_joint_state()
|
||||
if not self._joint_snapshot_is_motion_ready(snapshot):
|
||||
cycle.control_fault = True
|
||||
self._grip_rearm_required = True
|
||||
if self._joint_feedback_ready:
|
||||
self.get_logger().warn(
|
||||
@@ -610,13 +698,18 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
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
|
||||
cycle.feedback_age_ms = feedback_age * 1000.0
|
||||
if feedback_age < 0.0:
|
||||
cycle.control_fault = True
|
||||
self._grip_rearm_required = True
|
||||
self._joint_feedback_ready = False
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
if feedback_age > self._command_timeout_sec:
|
||||
cycle.control_fault = True
|
||||
self._handle_stale_joint_feedback(feedback_age)
|
||||
return
|
||||
|
||||
@@ -624,6 +717,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
try:
|
||||
current_pose = self._sync_joint_feedback(snapshot)
|
||||
except Exception as exc:
|
||||
cycle.control_fault = True
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}",
|
||||
throttle_duration_sec=1.0,
|
||||
@@ -632,6 +726,8 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._grip_rearm_required = True
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
cycle.current_pose = current_pose
|
||||
cycle.feedback_valid = True
|
||||
if not self._joint_feedback_ready:
|
||||
if self._grip_rearm_required:
|
||||
message = (
|
||||
@@ -680,6 +776,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
try:
|
||||
controller_quat = self._controller_quaternion(self._last_msg)
|
||||
except ValueError as exc:
|
||||
cycle.control_fault = True
|
||||
self.get_logger().warn(
|
||||
f"{self._arm_name} XR 手柄姿态无效，停止输出：{exc}",
|
||||
throttle_duration_sec=1.0,
|
||||
@@ -724,19 +821,35 @@ class SingleArmVelocityTeleop(Node):
|
||||
sent_target,
|
||||
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)
|
||||
cycle.qp_attempted = True
|
||||
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
|
||||
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()
|
||||
sent = self._send_joint_target(joint_target)
|
||||
send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6
|
||||
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
|
||||
else:
|
||||
cycle.send_failed = True
|
||||
total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6
|
||||
try:
|
||||
self._record_timing_sample(
|
||||
@@ -1196,7 +1309,10 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._last_valid_joint_target = list(snapshot.positions)
|
||||
return current_pose
|
||||
|
||||
def _solve_joint_target(self, target_pose: np.ndarray) -> list[float]:
|
||||
def _solve_joint_target(
|
||||
self,
|
||||
target_pose: np.ndarray,
|
||||
) -> tuple[list[float], bool]:
|
||||
if self._last_valid_joint_target is None:
|
||||
raise RuntimeError("valid joint feedback has not been initialized")
|
||||
try:
|
||||
@@ -1206,9 +1322,9 @@ class SingleArmVelocityTeleop(Node):
|
||||
f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
return list(self._last_valid_joint_target)
|
||||
return list(self._last_valid_joint_target), False
|
||||
self._last_valid_joint_target = list(result)
|
||||
return list(result)
|
||||
return list(result), True
|
||||
|
||||
def _safe_stop(self, reset_active: bool) -> None:
|
||||
if not self._stop_sent:
|
||||
@@ -1380,6 +1496,122 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._cmd_vel_pub.publish(velocity_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
|
||||
def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped:
|
||||
transform = _make_transform(pose[:3, 3], pose[:3, :3])
|
||||
|
||||
Reference in New Issue
Block a user