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