658 lines
28 KiB
Markdown
658 lines
28 KiB
Markdown
# 右臂番茄采摘 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`，由未来的上层任务状态机协调，避免让一个低层策略混合两种动作接口和
|
||
任务阶段。
|