feat: record OpenNeck actions in sim2real datasets

Author: BotRunner64

AGENTS.md

@@ -150,9 +150,9 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos
 - Optional LinkerHand control uses `hands.enabled=true`, `hands.driver=linkerhand_l6|linkerhand_o6`, and `hands.mode=gripper|vr_hand_pose`; default is disabled
 - Optional Pico sim2real HDF5 recording uses `--config-name sim2real_record` or `recording.enabled=true`; it requires `input.provider=pico4`, `input.video.enabled=true`, `input.video.source=realsense`, an interactive terminal, and the `recording` extra
 - Recording is manual only: terminal `R` starts an episode, `S` saves, `D` discards the active episode, and `Q` shuts down; `STANDING`, `MOCAP`, `ARMS`, and paused mocap are recordable
-- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the root-plus-joint reference consumed by the motion tracker; `action.hand(12)` is present when LinkerHand control is enabled
+- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the root-plus-joint reference consumed by the motion tracker; `action.hand(12)` is present when LinkerHand control is enabled, and `action.neck(2)` is present when OpenNeck control is enabled
 - Sim2real recording uses an editable source layout: `schema.json`, `episodes.jsonl`, per-episode HDF5 files under `recording.output_dir/data/`, and compressed MP4 files under `recording.output_dir/videos/d435i_rgb/`; task prompts live only in `episodes.jsonl`, and HDF5 files contain only frame arrays with no metadata attributes or raw RGB datasets
-- Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, FPS, and feature definitions; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported
+- Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, `neck_type=none|openneck`, FPS, and feature definitions; optional action fields are controlled directly by `hands.enabled` and `neck.enabled`; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported
 - Episodes interrupted before their `episodes.jsonl` entry is committed are discarded on the next recording-worker startup and do not consume an episode index
 - `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` for Pico grip/trigger open-close control and supports LinkerHand L6 and O6
 - `vr_hand_pose` mode reuses `Pico4InputProvider.get_hand_snapshot()` and somehand 0.2.0 public `somehand.api` for continuous Pico hand-pose retargeting; do not start a second `PicoBridge` for hand control
@@ -294,6 +294,11 @@ Critical note: align robot root orientation to the BVH human forward direction b
 
 ## Development
 
+### Simplicity Policy
+- Prefer the smallest implementation that satisfies the current requirement
+- Do not add speculative configuration switches, abstraction layers, compatibility paths, or extensibility without a concrete use case
+- Reuse existing enable flags and data flows when they already express the required behavior
+
 ### Runtime Validation Policy
 - Fail fast for logical mismatches such as observation definition vs. ONNX signature mismatch
 - Do not silently pad, trim, clip, or replace invalid data/config to "make it run"

README.md

@@ -101,12 +101,14 @@ python scripts/run/run_sim2real.py --config-name sim2real_record \
 Recording uses the terminal controls `R` start, `S` save, `D` discard, and `Q`
 shutdown. `STANDING`, `MOCAP`, `ARMS`, and paused mocap can be recorded. Saved
 episodes are written under `data/recordings/sim2real_hdf5/data/`, with compressed
-MP4 files under `videos/d435i_rgb/`. `schema.json` records the FPS, robot and
-hand types, feature shapes, names, and groups. `episodes.jsonl` maps each episode
+MP4 files under `videos/d435i_rgb/`. `schema.json` records the FPS, robot, hand,
+and neck types, plus feature shapes, names, and groups. `episodes.jsonl` maps each episode
 to its HDF5/video files and stores its editable task prompt. HDF5 contains only
 frame-aligned arrays: `observation.state(68)`, scalar `observation.mode`, and
 `action(36)` as the aligned reference qpos consumed by the motion tracker.
-`action.hand(12)` is present when LinkerHand control is enabled.
+`action.hand(12)` is present exactly when LinkerHand control is enabled, and
+`action.neck(2)` contains the latest normalized OpenNeck yaw/pitch command exactly
+when OpenNeck control is enabled.
 Recording is non-critical: an incompatible output schema stops only the
 recording worker while G1 control continues. Episodes interrupted before their
 manifest entry is committed are discarded on the next recording startup.

docs/docs/configuration/config-reference.md

@@ -221,9 +221,12 @@ recording.output_dir/
         └── episode_000000.mp4
 ```
 
-`schema.json` contains the FPS, `robot_type`, `hand_type`, and feature
-definitions. `robot_type` comes from `robot.type`; `hand_type` is `none` when
-hands are disabled, otherwise it is the configured `hands.driver`.
+`schema.json` contains the FPS, `robot_type`, `hand_type`, `neck_type`, and
+feature definitions. `robot_type` comes from `robot.type`; `hand_type` is `none`
+when hands are disabled, otherwise it is the configured `hands.driver`.
+`neck_type` is `none` when active-neck control is disabled, otherwise it is the
+configured `neck.driver`. These enabled flags directly control whether their
+action fields are recorded; there are no separate recording switches.
 `episodes.jsonl` contains one object per saved episode with `episode_index`,
 `frames`, editable `task`, HDF5 path, and video paths. Task prompts can therefore
 be relabeled without rewriting HDF5 or MP4 data. Starting another recording run
@@ -247,6 +250,7 @@ observation.state              float32[N, 68]
 observation.mode               int8[N]
 action                         float32[N, 36]
 action.hand                    float32[N, 12]  # only when hands are enabled
+action.neck                    float32[N, 2]   # only when OpenNeck is enabled
 ```
 
 HDF5 files contain only these frame arrays and have no recording metadata root
@@ -262,6 +266,8 @@ high-level reference consumed by the motion tracker, not the tracker policy's
 raw output or the final joint targets sent to G1.
 `action.hand` is the latest LinkerHand command from the hand worker:
 `left_pose(6) + right_pose(6)`, using the SDK's 0-255 pose values.
+`action.neck` is the latest command successfully sent to OpenNeck by the neck
+worker: normalized `[yaw, pitch]`, each in `[-1, 1]`.
 
 ## Critical: `default_dof_pos`
 

docs/docs/tutorials/pico-sim2real.md

@@ -118,11 +118,13 @@ shutdown. `STANDING`, `MOCAP`, `ARMS`, and paused mocap can be recorded;
 saved episodes cannot be discarded afterward. Episodes are saved as `.h5` files
 under `data/recordings/sim2real_hdf5/data/`, with compressed MP4 files under
 `data/recordings/sim2real_hdf5/videos/d435i_rgb/`. The dataset-level
-`schema.json` records robot/hand types and feature definitions, while
+`schema.json` records robot/hand/neck types and feature definitions, while
 `episodes.jsonl` stores file mappings and the editable task prompt for every
 episode. HDF5 stores `frame_index`, `timestamp`, `observation.state(68)`, scalar
 `observation.mode`, and the 36D motion-tracker reference `action` at 30 Hz.
-When hand control is enabled, it also stores `action.hand(12)`.
+When hand control is enabled, it also stores `action.hand(12)`. When OpenNeck
+control is enabled, it stores the latest normalized yaw/pitch command as
+`action.neck(2)`. Disabled devices do not add their action fields.
 
 ## Operator Flow
 

docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md

@@ -236,9 +236,11 @@ recording.output_dir/
         └── episode_000000.mp4
 ```
 
-`schema.json` 保存 FPS、`robot_type`、`hand_type` 和 feature 定义。
+`schema.json` 保存 FPS、`robot_type`、`hand_type`、`neck_type` 和 feature 定义。
 `robot_type` 来自 `robot.type`；未启用灵巧手时 `hand_type` 为 `none`，否则为
-配置的 `hands.driver`。`episodes.jsonl` 每行对应一个已保存的 episode，包含
+配置的 `hands.driver`。未启用主动视觉颈部控制时 `neck_type` 为 `none`，否则为
+配置的 `neck.driver`。这些 enabled 标志直接决定是否录制对应的 action 字段；没有
+单独的录制开关。`episodes.jsonl` 每行对应一个已保存的 episode，包含
 `episode_index`、`frames`、可编辑的 `task`、HDF5 路径和视频路径。因此修改任务
 prompt 不需要重写 HDF5 或 MP4。使用相同 schema 再次启动录制时，会从下一个
 episode index 继续追加，并且可以使用不同的 `recording.task`。
@@ -258,6 +260,7 @@ observation.state              float32[N, 68]
 observation.mode               int8[N]
 action                         float32[N, 36]
 action.hand                    float32[N, 12]  # 仅启用灵巧手时存在
+action.neck                    float32[N, 2]   # 仅启用 OpenNeck 时存在
 ```
 
 HDF5 文件仅包含上述逐帧数组，不保存录制元数据根属性。RGB 帧保存在 MP4 中，
@@ -271,3 +274,5 @@ HDF5 文件仅包含上述逐帧数组，不保存录制元数据根属性。RGB
 消费的高层参考，不是 tracker policy 的原始输出，也不是最终下发给 G1 的关节目标。
 `action.hand` 是手部 worker 最新的 LinkerHand 命令：
 `left_pose(6) + right_pose(6)`，使用 SDK 的 0-255 pose 数值。
+`action.neck` 是颈部 worker 最近一次成功发送给 OpenNeck 的命令：归一化的
+`[yaw, pitch]`，两个值的范围均为 `[-1, 1]`。

docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md

@@ -113,11 +113,12 @@ python scripts/run/run_sim2real.py \
 `STANDING`、`MOCAP`、`ARMS` 和暂停状态的 mocap；已经保存的 episode 不支持再丢弃。
 episode 会保存为 `data/recordings/sim2real_hdf5/data/` 下的 `.h5` 文件，
 压缩 MP4 视频保存在 `data/recordings/sim2real_hdf5/videos/d435i_rgb/` 下。
-数据集级 `schema.json` 保存机器人/灵巧手类型和 feature 定义，
+数据集级 `schema.json` 保存机器人/灵巧手/颈部类型和 feature 定义，
 `episodes.jsonl` 保存每个 episode 的文件映射与可编辑任务 prompt。HDF5 以
 30 Hz 保存 `frame_index`、`timestamp`、`observation.state(68)`、标量
 `observation.mode` 和作为 motion-tracker reference 的 36D `action`。启用灵巧手
-控制时还会保存 `action.hand(12)`。
+控制时还会保存 `action.hand(12)`。启用 OpenNeck 控制时，会把最新的归一化
+yaw/pitch 命令保存为 `action.neck(2)`。未启用的设备不会添加对应的 action 字段。
 
 ## 操作流程
 

scripts/dev/test_openneck.py

@@ -181,12 +181,13 @@ def run_pico(args: argparse.Namespace) -> None:
             if provider.has_frame():
                 frame, timestamp_s, seq = provider.get_frame_packet()
                 if int(seq) != last_seq:
-                    moved = runtime.tick(
+                    command = runtime.tick(
                         frame=frame,
                         frame_timestamp_s=timestamp_s,
                         active=True,
                         now_s=now_s,
                     )
+                    moved = command is not None
                     if moved:
                         command_count += 1
                     last_seq = int(seq)

teleopit/recording/hdf5.py

@@ -28,17 +28,21 @@
 MODE_KEY = "observation.mode"
 ACTION_KEY = "action"
 HAND_ACTION_KEY = "action.hand"
+NECK_ACTION_KEY = "action.neck"
 FRAME_INDEX_KEY = "frame_index"
 TIMESTAMP_KEY = "timestamp"
 STATE_DIM = 68
 ACTION_DIM = FULL_QPOS_DIM
 HAND_ACTION_DIM = 12
+NECK_ACTION_DIM = 2
 DEFAULT_IMAGE_SHAPE = (480, 640, 3)
 HDF5_RECORDING_FORMAT = "teleopit_hdf5"
-HDF5_RECORDING_VERSION = 1
+HDF5_RECORDING_VERSION = 2
 DEFAULT_ROBOT_TYPE = "unitree_g1_29dof"
 NO_HAND_TYPE = "none"
 SUPPORTED_HAND_TYPES = (NO_HAND_TYPE, "linkerhand_l6", "linkerhand_o6")
+NO_NECK_TYPE = "none"
+SUPPORTED_NECK_TYPES = (NO_NECK_TYPE, "openneck")
 MODE_CODES = {
     "standing": 0,
     "mocap": 1,
@@ -56,18 +60,25 @@ class RecordingSchema:
     hand_type: str
     image_key: str
     image_shape: tuple[int, int, int]
+    neck_type: str = NO_NECK_TYPE
     state_key: str = STATE_KEY
     state_dim: int = STATE_DIM
     mode_key: str = MODE_KEY
     action_key: str = ACTION_KEY
     action_dim: int = ACTION_DIM
     hand_action_key: str = HAND_ACTION_KEY
     hand_action_dim: int = HAND_ACTION_DIM
+    neck_action_key: str = NECK_ACTION_KEY
+    neck_action_dim: int = NECK_ACTION_DIM
 
     @property
     def has_hand_action(self) -> bool:
         return self.hand_type != NO_HAND_TYPE
 
+    @property
+    def has_neck_action(self) -> bool:
+        return self.neck_type != NO_NECK_TYPE
+
 
 @dataclass(frozen=True)
 class MP4VideoConfig:
@@ -82,13 +93,15 @@ def build_recording_schema(
     fps: int = 30,
     robot_type: str = DEFAULT_ROBOT_TYPE,
     hand_type: str = NO_HAND_TYPE,
+    neck_type: str = NO_NECK_TYPE,
 ) -> RecordingSchema:
     key = str(cfg_get(camera_cfg, "key", IMAGE_KEY)).strip()
     width = int(cfg_get(camera_cfg, "width", DEFAULT_IMAGE_SHAPE[1]))
     height = int(cfg_get(camera_cfg, "height", DEFAULT_IMAGE_SHAPE[0]))
     parsed_fps = int(fps)
     parsed_robot_type = str(robot_type).strip().lower()
     parsed_hand_type = str(hand_type).strip().lower()
+    parsed_neck_type = str(neck_type).strip().lower()
     if not key:
         raise ValueError("recording.camera.key must not be empty")
     if width <= 0 or height <= 0:
@@ -103,10 +116,15 @@ def build_recording_schema(
         raise ValueError(
             f"Unsupported recording hand_type={parsed_hand_type!r}; expected one of {SUPPORTED_HAND_TYPES}"
         )
+    if parsed_neck_type not in SUPPORTED_NECK_TYPES:
+        raise ValueError(
+            f"Unsupported recording neck_type={parsed_neck_type!r}; expected one of {SUPPORTED_NECK_TYPES}"
+        )
     return RecordingSchema(
         fps=parsed_fps,
         robot_type=parsed_robot_type,
         hand_type=parsed_hand_type,
+        neck_type=parsed_neck_type,
         image_key=key,
         image_shape=(height, width, 3),
     )
@@ -172,6 +190,14 @@ def hdf5_schema(schema: RecordingSchema) -> dict[str, object]:
                 "right_hand_target": [6, 12],
             },
         }
+    if schema.has_neck_action:
+        features[schema.neck_action_key] = {
+            "dtype": "float32",
+            "shape": [schema.neck_action_dim],
+            "names": ["yaw", "pitch"],
+            "units": "normalized",
+            "range": [-1.0, 1.0],
+        }
     features[schema.image_key] = {
         "dtype": "video",
         "shape": list(schema.image_shape),
@@ -183,6 +209,7 @@ def hdf5_schema(schema: RecordingSchema) -> dict[str, object]:
         "fps": schema.fps,
         "robot_type": schema.robot_type,
         "hand_type": schema.hand_type,
+        "neck_type": schema.neck_type,
         "features": features,
     }
 
@@ -231,6 +258,13 @@ def normalize_hand_action(left_pose: object, right_pose: object) -> np.ndarray:
     return action
 
 
+def normalize_neck_action(yaw: object, pitch: object) -> np.ndarray:
+    action = np.asarray([yaw, pitch], dtype=np.float32).reshape(-1)
+    if action.shape[0] != NECK_ACTION_DIM:
+        raise ValueError(f"recording action.neck must be {NECK_ACTION_DIM}D, got {action.shape[0]}")
+    return action
+
+
 def build_mode_observation(mode: str) -> np.int8:
     normalized = str(mode).strip().lower()
     if normalized not in MODE_CODES:
@@ -332,6 +366,7 @@ def add_frame(
         mode: object,
         action: np.ndarray,
         hand_action: np.ndarray | None = None,
+        neck_action: np.ndarray | None = None,
     ) -> None:
         if not self._active or self._h5 is None:
             raise RuntimeError("Cannot add a recording frame without an active episode")
@@ -352,6 +387,17 @@ def add_frame(
             )
         elif hand_action is not None:
             raise ValueError(f"{self._schema.hand_action_key} must be omitted for hand_type={NO_HAND_TYPE}")
+        neck_action_arr: np.ndarray | None = None
+        if self._schema.has_neck_action:
+            if neck_action is None:
+                raise ValueError(f"{self._schema.neck_action_key} is required for neck_type={self._schema.neck_type}")
+            neck_action_arr = self._validate_vector(
+                neck_action,
+                self._schema.neck_action_key,
+                self._schema.neck_action_dim,
+            )
+        elif neck_action is not None:
+            raise ValueError(f"{self._schema.neck_action_key} must be omitted for neck_type={NO_NECK_TYPE}")
 
         row = self._frames_in_episode
         for dataset in self._datasets.values():
@@ -366,6 +412,8 @@ def add_frame(
         self._datasets[self._schema.action_key][row] = action_arr
         if hand_action_arr is not None:
             self._datasets[self._schema.hand_action_key][row] = hand_action_arr
+        if neck_action_arr is not None:
+            self._datasets[self._schema.neck_action_key][row] = neck_action_arr
         self._frames_in_episode += 1
 
     def save_episode(self) -> None:
@@ -623,6 +671,12 @@ def _create_datasets(self, h5: h5py.File) -> dict[str, h5py.Dataset]:
                 self._schema.hand_action_key,
                 self._schema.hand_action_dim,
             )
+        if self._schema.has_neck_action:
+            datasets[self._schema.neck_action_key] = self._create_vector_dataset(
+                h5,
+                self._schema.neck_action_key,
+                self._schema.neck_action_dim,
+            )
         return datasets
 
     @staticmethod

teleopit/sim2real/mp/ipc.py

@@ -13,6 +13,7 @@
 BODY_TOPIC = "body"
 HAND_TOPIC = "hand"
 HAND_COMMAND_TOPIC = "hand_command"
+NECK_COMMAND_TOPIC = "neck_command"
 CONTROLLER_TOPIC = "controller"
 CONTROL_EVENTS_TOPIC = "control_events"
 REFERENCE_TOPIC = "reference"
@@ -28,6 +29,7 @@ class Sim2RealIpcEndpoints:
     body_pub: str
     hand_pub: str
     hand_command_pub: str
+    neck_command_pub: str
     controller_pub: str
     control_events_pub: str
     reference_pub: str
@@ -46,6 +48,7 @@ def default_endpoints(*, host: str = "127.0.0.1", base_port: int = 39700) -> Sim
         body_pub=f"{prefix}{base_port}",
         hand_pub=f"{prefix}{base_port + 1}",
         hand_command_pub=f"{prefix}{base_port + 2}",
+        neck_command_pub=f"{prefix}{base_port + 12}",
         controller_pub=f"{prefix}{base_port + 3}",
         control_events_pub=f"{prefix}{base_port + 4}",
         reference_pub=f"{prefix}{base_port + 5}",

teleopit/sim2real/mp/messages.py

@@ -82,6 +82,16 @@ class HandCommandPacket:
     seq: int
 
 
+@dataclass(frozen=True)
+class NeckCommandPacket:
+    timestamp_s: float
+    driver: str
+    active: bool
+    yaw: float
+    pitch: float
+    seq: int
+
+
 @dataclass(frozen=True)
 class HealthPacket:
     worker: str