diff --git a/.gitignore b/.gitignore
index 33034add..0be09449 100644
--- a/.gitignore
+++ b/.gitignore
@@ -74,6 +74,7 @@ outputs/
 benchmark_results/
 logs/
 ckpt/
+ckpt_restore/
 wandb/
 *.log
 # Training - downloaded reference assets (re-downloadable)
diff --git a/AGENTS.md b/AGENTS.md
index 5024575d..9f307b5a 100644
--- a/AGENTS.md
+++ b/AGENTS.md
@@ -12,9 +12,14 @@ Config: Hydra/OmegaConf YAML files in `teleopit/configs/`
 
 ```
 InputProvider (BVH file / Pico4 VR) → Retargeter (GMR) → ObservationBuilder (167D) → Controller (dual-input TemporalCNN ONNX) → Robot (MuJoCo + PD / Unitree SDK)
+
+Host policy service → onboard policy client/scheduler → 36D reference → same ObservationBuilder/Controller → Unitree SDK
 ```
 
-Module-internal isolation: all modules run in-process and communicate via `InProcessBus` (zero-copy). Core interfaces are defined as `typing.Protocol` in `teleopit/interfaces.py`.
+Offline core modules communicate through `InProcessBus` (zero-copy). Sim2real
+workers use localhost ZMQ plus shared-memory video rings, while the external
+host-policy boundary uses strict msgpack/ZeroMQ without pickle. Core interfaces
+are defined as `typing.Protocol` in `teleopit/interfaces.py`.
 
 ## Supported Surface
 
@@ -22,6 +27,7 @@ Module-internal isolation: all modules run in-process and communicate via `InPro
 - Inference observation: `velcmd_history` (167D, dual-input ONNX with `obs` + `obs_history`)
 - TemporalCNN actor/critic with scaled dims (2048,1024,512,256,128)
 - Realtime inference uses a retargeted-reference timeline before observation build; `reference_steps=[0]` is the default production path
+- Host high-level-policy deployment uses an independent script/environment boundary; its network structure is defined by the current client/server code and protocol tests, and LeRobot is not a Teleopit dependency
 
 ## Directory Structure
 
@@ -56,13 +62,17 @@ teleopit/                 # Core inference package
 │   └── loop.py           # SimulationLoop — PD control at 200Hz, policy at 50Hz
 ├── sim2real/
 │   ├── mp/               # Process-isolated sim2real runtime and IPC
-│   └── hands/            # Optional LinkerHand driver/mapper plugins
+│   ├── hands/            # Optional LinkerHand driver/mapper plugins
+│   └── neck/             # Optional OpenNeck active-vision gimbal control
+├── high_level_policy/    # Host-policy protocol, strict client, frame transform, and action scheduler
 └── recording/            # Pico motion NPZ recording helpers
 scripts/
 ├── run/run_sim.py        # Offline sim2sim pipeline
 ├── run/run_sim2real.py   # G1 sim2real control; supports offline BVH playback and Pico4
+├── run/run_high_level_policy_sim2real.py # Independent host-policy deployment runtime
 ├── run/record_pico_motion.py # Interactive Pico recording → G1 motion NPZ clips
 ├── render/render_sim.py  # Render single BVH → 3 MuJoCo videos (mocap input, retarget, sim2sim)
+├── view/view_recording.py # Read-only synchronized sim2real recording reviewer
 └── dev/compute_ik_offsets.py # Compute IK quaternion offsets for new BVH formats
 train_mimic/              # Training package
 ├── app.py                # Shared app helpers for train/play/benchmark
@@ -78,7 +88,7 @@ train_mimic/              # Training package
 └── scripts/
     ├── train.py          # Training entry point
     ├── play.py           # Checkpoint playback
-    ├── benchmark.py      # Policy evaluation with tracking errors
+    ├── benchmark.py      # OmniXtreme-style policy benchmark
     └── save_onnx.py      # Export TemporalCNN ONNX
 ```
 
@@ -87,7 +97,7 @@ train_mimic/              # Training package
 ### Sim2Sim Pipeline
 - Policy runs at 50Hz, PD control at 200Hz (`decimation=4`, `sim_dt=0.005`)
 - Action flow: `compute_action()` returns raw action → `get_target_dof_pos()` applies clip `[-10, 10]`, scale, and `default_dof_pos`
-- Must use `assets/robots/unitree_g1/g1_29dof.xml` for training, sim2sim, dataset FK, and retargeting; it is the canonical G1 XML entry point
+- `assets/robots/unitree_g1/g1_29dof.xml` is the default G1 XML, not a model allowlist; training can select another task-compatible model with `--robot_xml`, and each workflow should keep its robot joint/body definitions consistent
 
 ### Multi-Viewer Support
 `SimulationLoop` supports multiple simultaneous viewer windows controlled by the `viewers` config:
@@ -134,6 +144,8 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos
 - The pico-bridge receiver runs on the Teleopit host, which can be a workstation PC or robot onboard computer; do not maintain a separate onboard Pico input mode
 - pico-bridge 0.2.1 is the supported runtime; camera preview uses `PicoBridge(video="frames").push_video_frame(rgb_uint8)`
 - Pico video preview is optional and disabled by default; sim2sim uses the MuJoCo `d435i_rgb` camera and sim2real uses RealSense when `input.video.enabled=true`
+- RealSense frame timeouts and disconnects are non-critical in sim2real: the video producer rebuilds the capture pipeline in the background, and video start/tick/stop failures must never exit `pico_input` or stop G1 control
+- The supervisor treats `pico_input` as non-critical; if that process exits, `robot_control` remains active, stale mocap references hold the last command, and the Unitree remote remains available for `STANDING` or manual `DAMPING`
 - Bone naming follows `pico_bridge_to_g1.json`
 - The provider applies an input-space transform to match the current retarget config
 - Do not hardcode that transform as a public coordinate-system contract; validate against actual retarget/sim2sim behavior when SDK or firmware changes
@@ -141,7 +153,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos
 - Pico sim2sim supports a keyboard-driven top-level mode state machine: `STANDING → MOCAP ↔ ARMS`, `X` returns to `STANDING`
 - Default Pico sim2sim keyboard mappings are `Y` → `MOCAP`, `A` → pause/resume mocap, `B` → toggle `MOCAP`/`ARMS`, `X` → back to `STANDING`, `Q` → quit
 - Pico4 sim2real pause/resume is handled as a mocap-session control event (`toggle_pause`), not as a mode switch to `STANDING`
-- Default Pico pause button is `A`; resume resets policy/reference state and yaw/XY root-offset alignment while the process-isolated realtime reference worker continues its live input timeline
+- Default Pico/controller pause button is `A`; Unitree remote `B` also pauses/resumes Pico sim2real. Resume resets policy/reference state and yaw/XY root-offset alignment while the process-isolated realtime reference worker continues its live input timeline
 - Pico4 sim2real arms the process-isolated reference worker only when entering `MOCAP`; `STANDING` and `DAMPING` disarm it so cold startup frames do not warm-start GMR before mocap entry
 - Pico4 sim2sim/sim2real support `ARMS` mode toggled from `MOCAP` with Pico/controller `B`; retargeting continues, while the control loop sends the motion tracker a composed reference with stand-pose body/legs/waist and live retargeted arms
 - `ARMS` entering/exiting/resume resets policy/reference alignment and uses Kp ramp; offline BVH sim2real does not use `ARMS`, and Unitree remote `B` remains BVH replay
@@ -149,16 +161,43 @@ 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)`, `observation.mode(1)`, `action(36)`, and `action.hand(12)`; RealSense capture lives in `pico_input` through the normal `input.video` path
-- HDF5 recording writes compressed MP4 sidecar videos under `recording.output_dir/videos/<camera_key>/` while HDF5 episodes store `frame_index`, `timestamp`, low-dimensional data, and video sync attributes; raw RGB image datasets are not supported
-- `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
+- Recording requires a fresh RealSense frame before `R` can start an episode; an active episode is discarded after one second without a fresh camera frame while Pico input and G1 control continue, and recording does not restart automatically when video recovers
+- 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; when LinkerHand control is enabled, `observation.state.hand(12)` stores the left/right hardware joint readback and `action.hand(12)` stores the target; when OpenNeck control is enabled, `observation.state.neck(2)` stores the servo `[yaw_deg, pitch_deg]` readback and `action.neck(2)` stores the mechanically clamped target
+- 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`, `neck_type=none|openneck`, FPS, and feature definitions; optional hand/neck state and 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
+- Review saved sim2real recordings with `scripts/view/view_recording.py`; it validates manifest/HDF5/MP4 alignment and synchronizes camera video, an observed-vs-reference MuJoCo overlay, joint/mode plots, and optional hand/neck signals; because measured root XYZ is not recorded, the observed robot is anchored to the reference root position
+- `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` and supports LinkerHand L6 and O6; the side grip trigger is a deadman enable, the index trigger controls closure while it is held, and releasing the side grip opens that hand
+- `vr_hand_pose` mode reuses `Pico4InputProvider.get_hand_snapshot()` and somehand 0.3.0 public `somehand.api` for continuous Pico hand-pose retargeting; do not start a second `PicoBridge` for hand control
 - Teleopit owns Pico 26-joint hand-state to 21-landmark conversion; do not import `somehand.pico_input`
-- LinkerHand O6 supports only `hands.mode=gripper`; its default `close_pose` is `[86, 73, 118, 111, 110, 111]`
-- L6 `gripper` mode uses the configured `hands.linkerhand_l6.speed` (default `[50]*6`); O6 `gripper` mode uses `hands.linkerhand_o6.speed` (default `[255]*6`); `vr_hand_pose` always sets LinkerHand L6 speed to `[255]*6`
+- LinkerHand O6 supports `hands.mode=gripper|vr_hand_pose`; its default `close_pose` is `[86, 73, 118, 111, 110, 111]`
+- L6 `gripper` mode uses the configured `hands.linkerhand_l6.speed` (default `[50]*6`); O6 `gripper` mode uses `hands.linkerhand_o6.speed` (default `[255]*6`); `vr_hand_pose` always sets LinkerHand L6/O6 speed to `[255]*6`
 - `vr_hand_pose` defaults to a low-latency somehand path: `hands.somehand.rate_hz=60`, `max_iterations=12`, `temporal_filter_alpha=1.0`, and `output_alpha=1.0`; this prioritizes response speed over smoothing
 - LinkerHand control is active in all sim2real modes when `hands.enabled=true`; shutdown and hand-runtime failure must send the configured open pose
 - In `vr_hand_pose` mode, missing/inactive hand pose holds the last commanded pose for that side instead of opening the hand
+- Optional OpenNeck active-vision gimbal control uses `neck.enabled=true` and `neck.driver=openneck`; it requires `input.provider=pico4`, reuses the existing Pico receiver, and must not start a second `PicoBridge`
+- OpenNeck is integrated as a non-critical sim2real `neck_worker`; failures should not stop the G1 control loop, and no OpenNeck state is added to the 167D policy observation
+
+### Host High-Level Policy
+- `scripts/run/run_high_level_policy_sim2real.py` is independent from the Pico `run_sim2real.py` runtime; it must not start PicoBridge, GMR, or the realtime retarget reference worker
+- The host LeRobot/ReplayPolicy service runs in the separate `lerobot-teleopit` repository and environment; Teleopit must not depend on LeRobot, Transformers, or host policy classes
+- The current client/server code and protocol tests define the ZeroMQ request/response structure. During active development, Teleopit and `lerobot-teleopit` must update that structure together; no legacy network envelope is supported
+- The only shared data file is `hand_calibration.json`, which contains the LinkerHand O6 open/close calibration and must stay identical in both repositories
+- The host boundary uses ZeroMQ REQ/REP with msgpack and non-pickle float32 arrays. Deployment is asynchronous and receding-horizon: the isolated client worker keeps at most one request in flight, submits the latest eligible observation every `high_level_policy.replan_steps` 30 Hz source frames, and leaves the current plan executing while inference runs. The onboard scheduler uses the echoed monotonic observation timestamp to skip elapsed source frames and replace the active plan when a newer response arrives; process isolation keeps the 50 Hz robot loop running
+- Policy `get_action` input is RGB JPEG plus G1 joint positions `float32[29]`, raw measured left/right LinkerHand O6 readback `float32[12]`, measured OpenNeck yaw/pitch degrees `float32[2]`, and the observation-time active reference root pose `float32[7]` (`xyz + quaternion wxyz`) in the session-local frame. The first three arrays form the 43D model state; the source reference pose is used only to reconstruct source-relative root output and is not a model input
+- The scheduler keeps a short history of the rate-limited active session-local reference and queries/interpolates it at each camera timestamp. Session reset seeds this history from the initial active reference; the source anchor must never be reconstructed from the robot's measured root pose
+- Canonical action is `float32[T,50]` with protocol horizon `T` in `[1,50]`: local root `xyz(3)` + local root quaternion `wxyz(4)` + G1 joint reference `29` + left/right O6 closure `12` + OpenNeck yaw/pitch degrees `2`
+- The body reference `[0:36]` is yaw/XY-delocalized once and sent through the existing motion tracker. It is never sent directly as a G1 motor command and must not pass through the mocap alignment a second time
+- High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, Teleopit creates exactly one host session and remains in `STANDING` only while waiting for its first valid chunk; that chunk enters `POLICY` directly. There is no candidate-reference alignment, entry Kp ramp, second session/reset, or `POLICY_STARTING` mode. The 50 Hz output limiter starts from the measured robot reference captured when the session begins, and an entry failure remains in `STANDING`
+- Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING`
+- Policy pause freezes the scheduler/body reference and holds the latest hand/neck command. Leaving `POLICY` opens LinkerHand and centers OpenNeck
+- The onboard scheduler clips G1 joint references to `real_robot.joint_pos_lower/upper` when the required correction is at most `high_level_policy.safety.max_joint_projection_rad` (default `0.1` rad), and clips OpenNeck yaw/pitch commands to their configured degree ranges. It rejects whole chunks on excessive joint correction, shape/finiteness, session/sequence, quaternion, absolute root height, hand closure, or staleness failures. It accepts temporal root, yaw, and joint-reference discontinuities at entry, inside chunks, and across chunks because recorded pause/resume transitions may be discontinuous; accepted root translation, yaw, and joint output is rate-limited at 50 Hz. Never pad or trim malformed host output, and do not clip other out-of-range fields into validity
+- A newer timestamp-aligned chunk normally replaces the active plan before its horizon ends. If inference runs longer, the scheduler holds the plan's final reference for `high_level_policy.hold_s`; exhausting that grace period triggers the action watchdog. A request timeout, host/network failure, watchdog expiry, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`; an invalid result is rejected while the last valid plan remains available. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING`
+- Initial production hardware support requires two LinkerHand O6 hands and OpenNeck because all 50 canonical action dimensions are active
+- OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported
+- OpenNeck maps the independent HMD `PicoFrame.head.rotation` relative to the same-frame full-body `Body.Spine3` orientation; it must never use the full-body `Body.Head` skeleton joint for neck control, and HMD pose updates must remain independent of duplicate-body-frame filtering
+- OpenNeck uses a fixed identity neutral pose and no neck-side EMA; it must not capture the first live frame as a runtime zero pose, so tracking can start while the operator's head is turned
+- After the raw relative-angle dead zone, `neck.pitch_gain` (default `1.4`) scales pitch before `move_deg()` while yaw remains one-to-one; OpenNeck remains responsible for final mechanical clipping; positive yaw turns left and positive pitch looks up
 
 ### SimulationLoop Runtime Behavior
 - `realtime=true` enforces wall-clock pacing even without a viewer
@@ -169,7 +208,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos
 - Realtime inferred `motion_joint_vel`, anchor linear velocity, and anchor angular velocity can be EMA-smoothed via `reference_velocity_smoothing_alpha` and `reference_anchor_velocity_smoothing_alpha`
 - Sim2real Pico pause/resume uses mocap-session states `ACTIVE ↔ PAUSED`; resume clears policy/reference state, rebuilds yaw/XY root alignment, and does not interpolate retarget qpos from the paused pose
 - Realtime sim2sim with Pico control events uses the same mocap-session pause/resume semantics and rebuilds the realtime reference path on resume, including the configured warmup
-- Realtime sim2sim `STANDING ↔ MOCAP` transitions rebuild the realtime reference path on entry; Pico sim2real `STANDING -> MOCAP` additionally rearms and resets the process-isolated reference worker before accepting fresh references
+- Realtime Pico sim2sim `STANDING -> MOCAP` resets GMR, seeds its floating root from the current live pelvis target, and rebuilds the realtime reference path before accepting references; Pico sim2real performs the same GMR cold start through its rearmed process-isolated reference worker
 - Realtime Pico sim2sim can start directly in `STANDING` with keyboard mode control enabled via top-level `keyboard.enabled`
 
 ### Inference Observation
@@ -203,7 +242,7 @@ The single supported training task is `General-Tracking-G1` (experiment name: `g
 - Training env uses `sampling_mode="rewind"`
 - Tracking rewards include root position/orientation/linear velocity/angular velocity, body pose/velocity, joint position/velocity, survival, action-rate, joint-limit, self-collision, and ankle acceleration terms
 - Supported motion sampling modes are `uniform`, `start`, and `rewind`; `rewind` restarts failed environments from the same clip after stepping back `rewind_min_steps..rewind_max_steps` with probability `rewind_prob`, otherwise it falls back to uniform sampling
-- Playback/benchmark use `play=True`, which switches motion sampling to `start`
+- Playback and benchmark use `play=True`, which switches motion sampling to `start`; benchmark pins exact clip ids/start times, disables clip-end resampling, and reports `MPJPE(m)`, `root_pos_error(m)`, `root_rot_error(rad)`, `root_vel_error(m/s)`, and `success_rate(%)`
 - `window_steps=[0]`
 - `save_onnx.py` exports dual-input TemporalCNN ONNX
 
@@ -239,9 +278,11 @@ python train_mimic/scripts/save_onnx.py --checkpoint logs/rsl_rl/g1_general_trac
 
 ### External Assets
 - Do not commit robot meshes, datasets, checkpoints, or demo media to Git; use `scripts/setup/download_assets.py`
-- `assets/robots/unitree_g1/g1_29dof.xml` and its meshes are the canonical G1 robot model assets; they are downloaded from the `robots` asset group and are not tracked in Git
+- G1 XML variants and their meshes are downloaded under `assets/robots/unitree_g1/` by the `robots` asset group and are not tracked in Git; `g1_29dof.xml` is the default
+- The neck-and-O6 runtime variant is `assets/robots/unitree_g1/g1_29dof_neck_o6.xml`
+- Released tracking assets download under `ckpt/` as the matching `track_g1.{pt,onnx}` and `track_g1_neck_o6.{pt,onnx}` pairs
 - `teleopit/retargeting/gmr/assets/` is gitignored; downloaded at runtime
-- `train_mimic/assets/` is no longer tracked; FK tooling reuses `assets/robots/unitree_g1/g1_29dof.xml`
+- `train_mimic/assets/` is no longer tracked; FK tooling uses the robot assets under `assets/robots/`, with `assets/robots/unitree_g1/g1_29dof.xml` as the default G1 model
 - `third_party/linkerhand-python-sdk` and `third_party/somehand` support optional LinkerHand sim2real control
 - Run `python scripts/dev/check_large_tracked_files.py` before pushing
 
@@ -263,7 +304,7 @@ python scripts/setup/prepare_modelscope_assets.py --only data
 
 # 2. Upload to each repo
 modelscope upload --repo-type model BingqianWu/Teleopit-models \
-  data/modelscope_upload/checkpoints checkpoints
+  data/modelscope_upload/checkpoints checkpoints --sync
 modelscope upload --repo-type model BingqianWu/Teleopit-models \
   data/modelscope_upload/archives archives
 modelscope upload --repo-type dataset BingqianWu/Teleopit-datasets \
@@ -289,6 +330,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"
@@ -309,4 +355,4 @@ pytest tests/ -v
 ## Known Issues
 
 1. `lafan1-resolved` retargeting is still broken because it uses a different BVH skeleton layout.
-2. Legacy downloaded GMR XMLs under `teleopit/retargeting/gmr/assets/unitree_g1/` are not the project entry point; use `assets/robots/unitree_g1/g1_29dof.xml`.
+2. Legacy downloaded GMR XMLs under `teleopit/retargeting/gmr/assets/unitree_g1/` are separate retargeting assets, not replacements for the runtime robot bundle under `assets/robots/unitree_g1/`; `g1_29dof.xml` is the default runtime G1 model.
diff --git a/CHANGELOG.md b/CHANGELOG.md
index e72a1b9e..cabeab9e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,5 +1,19 @@
 # Changelog
 
+## [0.5.0] - 2026-08-03
+
+- 新增独立的 host high-level-policy sim2real 运行时：使用严格的 msgpack/ZeroMQ 协议、异步 receding-horizon replanning、时间戳对齐调度，以及 50 Hz 输出安全校验和限速。
+- 扩展 G1 外设支持：加入 OpenNeck 0.2.0 物理角度控制、Pico HMD 主动视觉映射、LinkerHand O6 somehand 0.3.0 手势控制，以及手部和颈部状态回读。
+- 更新 sim2real 录制与审阅流程：采用 `schema.json`、`episodes.jsonl`、逐 episode HDF5 和压缩 MP4 布局，记录可选手部/颈部状态与动作，并新增同步 recording viewer。
+- 新增匹配的 G1 模型/策略组合：默认 `g1_29dof.xml` 配合 `ckpt/track_g1.{pt,onnx}`，neck-and-O6 版本配合 `g1_29dof_neck_o6.xml` 和 `ckpt/track_g1_neck_o6.{pt,onnx}`。
+- 更新 OmniXtreme-style benchmark，并增强 Pico/RealSense 故障恢复、GMR mocap-entry cold start、high-level-policy watchdog 和引用安全处理。
+
+### 迁移说明
+
+- v0.4 的根目录 `track.{pt,onnx}` 路径已替换为 `ckpt/track_g1.{pt,onnx}`；neck-and-O6 运行时必须使用对应的模型和策略组合。
+- 旧的 attribute-based sim2real HDF5 格式不再支持；录制、转换和审阅工具使用当前 manifest-based source layout。
+- Host-policy 网络协议不提供旧 envelope 兼容，OpenNeck 旧 normalized API 也不再支持；Teleopit 与 companion runtime 必须使用匹配版本。
+
 ## [0.4.0] - 2026-06-25
 
 - 改进 Pico 实时控制：支持 pico-bridge 0.2.1、`ARMS` 模式，以及保留 retargeter warm-start 的模式切换/暂停恢复。
diff --git a/README.md b/README.md
index b2990a7f..08f9350f 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,5 @@
 <p align="center">
-  <img src="assets/teleopit_logo.jpg" width="80" alt="Teleopit">
+  <img src="assets/teleopit.png" width="720" alt="Teleopit whole-body teleoperation demo">
 </p>
 
 <h1 align="center">Teleopit</h1>
@@ -11,11 +11,9 @@
 </p>
 
 <p align="center">
+  <a href="https://botrunner64.github.io/teleopit-page/">Project Homepage</a> &bull;
   <a href="https://BotRunner64.github.io/Teleopit/">Documentation</a> &bull;
-  <a href="https://BotRunner64.github.io/Teleopit/zh-Hans/">中文文档</a> &bull;
-  <a href="https://BotRunner64.github.io/Teleopit/tutorials/pico-sim2sim">Pico Sim2Sim</a> &bull;
-  <a href="https://BotRunner64.github.io/Teleopit/tutorials/pico-sim2real">Pico Sim2Real</a> &bull;
-  <a href="https://BotRunner64.github.io/Teleopit/tutorials/training">Training</a>
+  <a href="https://BotRunner64.github.io/Teleopit/zh-Hans/">中文文档</a>
 </p>
 
 ---
@@ -35,15 +33,18 @@ pip install modelscope
 python scripts/setup/download_assets.py --only robots gmr ckpt bvh
 ```
 
-The canonical Unitree G1 robot model is downloaded to
-`assets/robots/unitree_g1/g1_29dof.xml`. Training, sim2sim, retargeting, and FK
-validation all use this same XML.
+The default Unitree G1 robot model is downloaded to
+`assets/robots/unitree_g1/g1_29dof.xml`, with additional model variants in the
+same directory. Training can select a task-compatible XML with `--robot_xml`;
+the quick-start command below uses the default model and its matching
+`ckpt/track_g1.onnx` policy. The neck-and-O6 variant uses
+`g1_29dof_neck_o6.xml` with `ckpt/track_g1_neck_o6.onnx`.
 
 **3. Run**
 
 ```bash
 python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
+    controller.policy_path=ckpt/track_g1.onnx \
     input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
 ```
 
@@ -53,7 +54,7 @@ To show the simulated D435i RGB camera view, add the explicit `camera` viewer:
 
 ```bash
 python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
+    controller.policy_path=ckpt/track_g1.onnx \
     input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
     'viewers=[sim2sim,camera]'
 ```
@@ -61,63 +62,30 @@ python scripts/run/run_sim.py \
 For sim2real, viewers are disabled by default. Add `viewers=retarget` to show
 the retargeted reference in an optional MuJoCo window.
 
-## Pico Motion Recording
-
-Record many Pico clips as training-ready G1 motion NPZ files:
-
-```bash
-pip install -e '.[pico4]'
-python scripts/run/record_pico_motion.py
-```
-
-The recorder starts the Pico receiver and live Retarget viewer before waiting
-for clip names, so preview keeps running while the terminal is idle. Enter a
-semantic clip name, then use `R` to start, `S` to save, `D` to discard, `N` for
-a new name, and `Q` to quit. Saved clips are written to
-`data/pico_motion/clips/` using the semantic label in the filename, with no
-sidecar JSON.
-
-Merge recorded clips into the standard HDF5 shard dataset:
-
-```bash
-python train_mimic/scripts/data/build_dataset.py \
-    --spec data/pico_motion/pico_recorded.yaml --force
-```
-
-## Sim2Real HDF5 Recording
+## Documentation
 
-Pico sim2real can also record manual HDF5 episodes from the real G1:
+Full docs at **[BotRunner64.github.io/Teleopit](https://BotRunner64.github.io/Teleopit/)**, covering installation profiles, all tutorials, configuration reference, and architecture.
 
-```bash
-pip install -e '.[recording]'
-# If you use RealSense video, install pyrealsense2 manually for your platform.
-# On Arm machines, prefer conda-forge:
-# conda install -c conda-forge pyrealsense2
-python scripts/run/run_sim2real.py --config-name sim2real_record \
-    controller.policy_path=track.onnx \
-    recording.task="walk forward"
-```
+## Changelog
 
-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 as `.h5` files under `data/recordings/sim2real_hdf5/episodes/`.
-`sim2real_record.yaml` stores camera frames as compressed MP4 sidecar files under
-`data/recordings/sim2real_hdf5/videos/` and keeps `frame_index` / `timestamp`
-sync metadata in the HDF5 episode. The low-dimensional HDF5 schema records
-`observation.state(68)`, `observation.mode(1)`, `action(36)` as the aligned
-reference qpos sent to the policy path, and `action.hand(12)` as the latest
-LinkerHand left/right 6D pose commands.
+### v0.5.0 (2026-08-03)
 
-## Documentation
+- Added an independent host high-level-policy sim2real runtime with a strict msgpack/ZeroMQ protocol, asynchronous receding-horizon replanning, timestamp-aligned scheduling, and validated, rate-limited 50 Hz output.
+- Extended G1 peripheral support with OpenNeck 0.2.0 physical-angle control, Pico HMD active-vision mapping, LinkerHand O6 hand-pose control through somehand 0.3.0, and hand/neck state readback.
+- Updated sim2real recording and review around `schema.json`, `episodes.jsonl`, per-episode HDF5 files, compressed MP4 video, optional hand/neck state and action fields, and a synchronized recording viewer.
+- Added matched G1 model/policy pairs: `g1_29dof.xml` with `ckpt/track_g1.{pt,onnx}`, and `g1_29dof_neck_o6.xml` with `ckpt/track_g1_neck_o6.{pt,onnx}`.
+- Updated the OmniXtreme-style benchmark and hardened Pico/RealSense recovery, GMR mocap-entry cold start, the high-level-policy watchdog, and reference safety handling.
 
-Full docs at **[BotRunner64.github.io/Teleopit](https://BotRunner64.github.io/Teleopit/)**, covering installation profiles, all tutorials, configuration reference, and architecture.
+#### Migration notes
 
-## Changelog
+- The v0.4 root-level `track.{pt,onnx}` paths are replaced by `ckpt/track_g1.{pt,onnx}`; the neck-and-O6 runtime requires its matching robot model and policy.
+- The old attribute-based sim2real HDF5 format is unsupported; recording, conversion, and review use the current manifest-based source layout.
+- The host-policy protocol has no legacy envelope compatibility, and the old normalized OpenNeck API is unsupported; Teleopit and its companion runtimes must use matching versions.
 
 ### v0.4.0 (2026-06-25)
 
 - Improved Pico realtime control with pico-bridge 0.2.1, `ARMS` mode, armed sim2real mocap entry, and retargeter-preserving pause/arms resets.
-- Added optional LinkerHand L6/O6 sim2real control, including Pico gripper input and low-latency L6 `vr_hand_pose`.
+- Added optional LinkerHand L6/O6 sim2real control, including Pico gripper input and low-latency L6/O6 `vr_hand_pose`.
 - Added manual Pico sim2real HDF5 recording and an interactive Pico motion recorder for training NPZ clips.
 - Refined the training data path with minimal HDF5 shards, explicit precompute, rewind sampling, and updated tracking rewards.
 
diff --git a/assets/teleopit.png b/assets/teleopit.png
new file mode 100644
index 00000000..271b166d
Binary files /dev/null and b/assets/teleopit.png differ
diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md
deleted file mode 100644
index f6bf965f..00000000
--- a/docs/docs/configuration/config-reference.md
+++ /dev/null
@@ -1,223 +0,0 @@
----
-sidebar_position: 2
----
-
-# Config Reference
-
-Complete reference for all configurable fields.
-
-## Top-Level Fields
-
-| Field | Description | Default |
-|-------|-------------|---------|
-| `policy_hz` | Policy inference frequency | `50` |
-| `pd_hz` | PD control frequency (simulation only) | `200` |
-| `viewers` | Viewer set: `mocap`, `retarget`, `sim2sim`, `camera`, `all`, `none`. `all` opens `mocap`, `retarget`, and `sim2sim`; add `camera` explicitly. | `sim2sim` |
-| `realtime` | Rate-limit to wall clock | `false` |
-| `num_steps` | Number of steps; `0` = infinite | `0` |
-| `keyboard.enabled` | Enable realtime keyboard mode control for sim2sim | `false` |
-| `playback.pause_on_end` | Pause at last frame when offline motion ends | `false` |
-| `playback.keyboard.enabled` | Enable keyboard control for offline playback | `false` |
-
-## Robot
-
-| Field | Description | Default |
-|-------|-------------|---------|
-| `robot.num_actions` | Joint action dimension | `29` |
-| `robot.xml_path` | MuJoCo XML path | - |
-| `d435i_rgb` | Fixed RGB camera in the G1 MJCF; use `viewers=[sim2sim,camera]` to display it | - |
-| `robot.kps` / `robot.kds` | PD gains | - |
-| `robot.default_angles` | Default standing pose | - |
-| `robot.torque_limits` | Joint torque limits | - |
-
-## Controller
-
-| Field | Description | Default |
-|-------|-------------|---------|
-| `controller.policy_path` | **Required.** Path to ONNX policy file | - |
-| `controller.device` | Inference device: `cpu` / `auto` / `cuda:N` | `cpu` |
-| `controller.action_scale` | Action scaling factor | - |
-| `controller.clip_range` | Action clipping range | - |
-| `controller.default_dof_pos` | Joint angle offset base | - |
-
-## Input
-
-### Offline BVH
-
-| Field | Description |
-|-------|-------------|
-| `input.bvh_file` | **Required.** Path to BVH file |
-| `input.bvh_format` | `lafan1` / `hc_mocap` |
-| `input.human_format` | Human skeleton format |
-
-> BVH input does not set `input.provider` — it is inferred from the config group name.
-
-### Pico 4
-
-| Field | Description | Default |
-|-------|-------------|---------|
-| `input.provider` | `pico4` | `pico4` |
-| `input.human_format` | Retarget skeleton format | `pico_bridge` |
-| `input.pico4_timeout` | Wait timeout in seconds | `60` |
-| `input.pico4_buffer_size` | Frame buffer size | `60` |
-| `input.pause_button` | Button for pause/resume | `A` |
-| `input.pause_debounce_s` | Debounce time for pause button | `0.25` |
-| `input.arms_button` | Button for Pico `MOCAP` / `ARMS` toggle | `B` |
-| `input.arms_debounce_s` | Debounce time for arms-mode button | `0.25` |
-| `input.bridge_host` | Teleopit host receiver bind host | `0.0.0.0` |
-| `input.bridge_port` | Teleopit host receiver TCP/UDP port | `63901` |
-| `input.bridge_discovery` | Enable pico-bridge discovery advertising | `true` |
-| `input.bridge_advertise_ip` | Optional advertised host IP override | `null` |
-| `input.bridge_start_timeout` | Timeout while starting the bridge | `10.0` |
-| `input.bridge_history_size` | Pico frame history retained by the bridge | `120` |
-| `input.video.enabled` | Stream host camera preview back to Pico through pico-bridge 0.2.1 | `false` |
-| `input.video.source` | Video source: `mujoco`, `realsense`, or `test-pattern` | `null` |
-| `input.video.width` / `height` / `fps` | Video capture/render settings | `1280` / `720` / `30` |
-| `input.video.device` | Optional RealSense serial | `null` |
-| `input.video.fail_on_error` | Fail startup instead of disabling video on error | `false` |
-
-### Realtime
-
-| Field | Description |
-|-------|-------------|
-| `retarget_buffer_enabled` | Enable retarget buffering |
-| `retarget_buffer_window_s` | Buffer window size |
-| `retarget_buffer_delay_s` | Buffer delay |
-| `reference_steps` | Reference window steps |
-| `realtime_buffer_warmup_steps` | Warmup before playback |
-| `reference_velocity_smoothing_alpha` | Velocity smoothing |
-| `reference_anchor_velocity_smoothing_alpha` | Anchor velocity smoothing |
-
-## Sim2Real
-
-Fields used by sim2real configs (`sim2real.yaml`, `pico4_sim2real.yaml`).
-
-Sim2real defaults to `viewers=none`. Set `viewers=retarget` to open an optional
-MuJoCo window showing the retargeted reference; `sim2sim`, `mocap`, `camera`,
-and `all` are simulation-only viewer modes.
-
-### Safety
-
-| Field | Description | Default |
-|-------|-------------|---------|
-| `startup_ramp_duration` | Kp ramp duration after entering `STANDING`; gradually increases PD gains without changing policy targets | `2.0` |
-| `joint_vel_limit` | Joint velocity limit (rad/s); triggers emergency damping if exceeded | `10.0` |
-| `mocap_switch.check_frames` | Consecutive valid frames required before switching to MOCAP | `10` |
-| `arm_mocap.controlled_joint_indices` | G1 joints driven by live retargeting in Pico `ARMS` mode | `[15..28]` |
-
-### Real Robot
-
-| Field | Description | Default |
-|-------|-------------|---------|
-| `real_robot.network_interface` | Network interface for Unitree DDS communication. For wired PC-to-G1 control, find the cable interface with `ifconfig` and set that name, for example `enp130s0`; for onboard robot execution, `eth0` is usually correct. | `eth0` |
-| `real_robot.kp_real` | Real-robot proportional gains (per joint) | - |
-| `real_robot.kd_real` | Real-robot derivative gains (per joint) | - |
-| `real_robot.kd_damping` | Damping mode kd | `8.0` |
-| `real_robot.control_mode` | Ankle control mode (`PR` = Pitch-Roll) | `PR` |
-| `real_robot.joint_pos_lower` | Joint position lower limits (rad) | - |
-| `real_robot.joint_pos_upper` | Joint position upper limits (rad) | - |
-
-### Pause/Resume (Pico sim2real)
-
-Realtime Pico resume re-centers heading and ground-plane position before tracking continues. Operators should keep still and stay as close as practical to the paused pose to reduce sudden reference changes.
-
-### Dexterous Hand (Pico sim2real)
-
-`hands.enabled=true` requires `input.provider=pico4` plus local editable
-installs of `third_party/linkerhand-python-sdk` and `third_party/somehand`.
-When enabled, hand control remains active in all sim2real modes.
-`gripper` supports `linkerhand_l6` and `linkerhand_o6` by interpolating Pico
-trigger input between the configured open and close poses. `vr_hand_pose` is
-L6-only: missing hand pose holds the last command for that side, L6 speed is
-set to the maximum, and Teleopit converts Pico hand state to 21 landmarks before
-calling somehand 0.2.0 through `somehand.api` only.
-
-| Field | Description | Default |
-|-------|-------------|---------|
-| `hands.enabled` | Enable optional hand worker | `false` |
-| `hands.driver` | Hand driver plugin: `linkerhand_l6` or `linkerhand_o6` | `linkerhand_l6` |
-| `hands.mode` | `gripper` or `vr_hand_pose` | `gripper` |
-| `hands.sides` | Controlled sides | `[left, right]` |
-| `hands.rate_hz` | Maximum gripper command rate in Hz | `30.0` |
-| `hands.frame_timeout_s` | Controller or hand-pose staleness threshold | `0.3` |
-| `hands.linkerhand_l6.left_can` / `right_can` | CAN channels for each hand | `can0` / `can1` |
-| `hands.linkerhand_l6.speed` | L6 speed used by `gripper`; `vr_hand_pose` overrides this to maximum speed | see config |
-| `hands.linkerhand_l6.open_pose` / `close_pose` | Six-value L6 open/closed poses | see config |
-| `hands.linkerhand_o6.left_can` / `right_can` | CAN channels for each O6 hand | `can0` / `can1` |
-| `hands.linkerhand_o6.speed` | O6 speed used by `gripper` | see config |
-| `hands.linkerhand_o6.open_pose` / `close_pose` | Six-value O6 open/closed poses | see config |
-| `hands.somehand.config_path` | Official somehand 0.2.0 bi-hand L6 config used by `vr_hand_pose` | see config |
-| `hands.somehand.rate_hz` | Low-latency `vr_hand_pose` command rate in Hz | `60.0` |
-| `hands.somehand.max_iterations` | somehand solver iteration cap for `vr_hand_pose` | `12` |
-| `hands.somehand.temporal_filter_alpha` | somehand input landmark smoothing alpha; `1.0` disables smoothing delay | `1.0` |
-| `hands.somehand.output_alpha` | somehand qpos output smoothing alpha; `1.0` disables smoothing delay | `1.0` |
-
-### HDF5 Recording (Pico sim2real)
-
-`recording.enabled=true` is supported only with `input.provider=pico4`,
-`input.video.enabled=true`, `input.video.source=realsense`, and an interactive
-terminal. The recorder is manual: `R` starts an episode, `S` saves the active
-episode, `D` discards the active episode, and `Q` shuts down. `STANDING`,
-`MOCAP`, `ARMS`, and paused mocap can be recorded.
-
-`sim2real_record.yaml` enables both recording and the required RealSense
-`input.video` path. Recording does not open a second camera; it consumes the
-same frames produced by `pico_input`.
-
-| Field | Description | Default |
-|-------|-------------|---------|
-| `recording.enabled` | Enable manual HDF5 recording | `false` |
-| `recording.output_dir` | Dataset root directory | `data/recordings/sim2real_hdf5` |
-| `recording.task` | Task string stored with frames | `demo` |
-| `recording.fps` | Recording/video clock rate | `30` |
-| `recording.min_episode_seconds` | Discard saved episodes shorter than this duration | `1.0` |
-| `recording.record_modes` | Modes that allow recording start and frame writes | `[standing, mocap, arms, pause]` |
-| `recording.camera.key` | RGB image dataset key | `observation.images.d435i_rgb` |
-| `recording.camera.width` / `height` / `fps` | RealSense RGB capture settings | `640` / `480` / `30` |
-| `recording.camera.device` | Optional RealSense serial | `null` |
-| `recording.video.codec` / `quality` / `pixelformat` | MP4 sidecar encoder settings | `libx264` / `8` / `yuv420p` |
-
-Camera failure behavior is controlled by `input.video.fail_on_error`.
-
-Each saved episode has one `.h5` file under `recording.output_dir/episodes/`
-and one compressed MP4 sidecar under
-`recording.output_dir/videos/<camera_key>/`. The HDF5 episode stores
-`frame_index` and `timestamp` arrays, plus `video_path`, `video_fps`, and
-`video_frames` root attributes for synchronization. Raw RGB image datasets are
-not written.
-
-HDF5 datasets:
-
-```text
-frame_index                    int64[N]
-timestamp                      float64[N]
-observation.state              float32[68]
-observation.mode               float32[1]
-action                         float32[36]
-action.hand                    float32[12]
-```
-
-The root attributes include the Teleopit HDF5 recording format, schema version,
-task, fps, frame count, and video sync metadata.
-
-`observation.state` is ordered as `joint_pos(29)`, `joint_vel(29)`,
-`base_quat_wxyz(4)`, `base_ang_vel(3)`, and `projected_gravity(3)`.
-`observation.mode` is a numeric categorical: `standing=0`, `mocap=1`,
-`arms=2`, and `pause=3`. `action` is the current reference qpos:
-`root_pos(3) + root_quat_wxyz(4) + joint_pos(29)`.
-`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.
-
-## Critical: `default_dof_pos`
-
-The RL policy outputs action **offsets** relative to the default standing pose, not absolute joint angles:
-
-```text
-target_dof_pos = clip(action, low, high) * action_scale + default_dof_pos
-```
-
-Therefore:
-- `default_dof_pos` must align with `robot.default_angles`
-- Missing this offset causes the robot to fall immediately
-
-`TeleopPipeline` automatically passes `robot.default_angles` to the controller's `default_dof_pos` during initialization. Understanding this chain is important when writing custom entry points or tests.
diff --git a/docs/docs/configuration/faq.md b/docs/docs/configuration/faq.md
deleted file mode 100644
index ccc28537..00000000
--- a/docs/docs/configuration/faq.md
+++ /dev/null
@@ -1,29 +0,0 @@
----
-sidebar_position: 3
----
-
-# Configuration FAQ
-
-## Why does it fail even though I set `policy_path`?
-
-1. Verify the file exists
-2. Confirm the input dimension is `167` with dual inputs (`obs` + `obs_history`)
-
-## Why must I specify `input.bvh_file` explicitly?
-
-`input/bvh.yaml` no longer provides machine-specific default paths. Always specify explicitly:
-
-```bash
-python scripts/run/run_sim.py \
-    controller.policy_path=policy.onnx \
-    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
-```
-
-## Why doesn't `viewer=true` work?
-
-The legacy `viewer` alias has been removed. Use `viewers` (plural):
-
-```bash
-python scripts/run/run_sim.py controller.policy_path=policy.onnx viewers=sim2sim
-python scripts/run/run_sim.py controller.policy_path=policy.onnx viewers=none
-```
diff --git a/docs/docs/getting-started/download-assets.md b/docs/docs/getting-started/download-assets.md
deleted file mode 100644
index 81854a30..00000000
--- a/docs/docs/getting-started/download-assets.md
+++ /dev/null
@@ -1,49 +0,0 @@
----
-sidebar_position: 2
----
-
-# Download Assets
-
-Robot models, datasets, and checkpoints are hosted on ModelScope and must be downloaded before use.
-
-## One-Click Download
-
-Download all assets (models, data, GMR retargeting assets):
-
-```bash
-pip install modelscope
-python scripts/setup/download_assets.py
-```
-
-## Selective Download
-
-Download only what you need for inference:
-
-```bash
-python scripts/setup/download_assets.py --only robots gmr ckpt bvh
-```
-
-## Asset Inventory
-
-Downloaded file sizes change as checkpoints, datasets, and asset bundles are updated. Use the repository paths below as the stable contract.
-
-| Local Path | Purpose |
-|------------|---------|
-| `track.onnx` | ONNX inference model |
-| `track.pt` | PyTorch checkpoint for resume training |
-| `data/datasets/<dataset>/shard_*.h5` | Minimal motion datasets; run precompute before training |
-| `data/sample_bvh/*.bvh` | Sample motion files |
-| `assets/robots/unitree_g1/` | Canonical G1 XML and meshes used by training, sim2sim, retargeting, and FK validation |
-| `teleopit/retargeting/gmr/assets/` | GMR retargeting assets, IK configs, and non-canonical robot descriptions |
-
-## Asset Groups
-
-| Group | ModelScope Repo | Contents |
-|-------|----------------|----------|
-| `ckpt` | `BingqianWu/Teleopit-models` | `track.onnx`, `track.pt` |
-| `robots` | `BingqianWu/Teleopit-models` | Canonical robot XML/meshes |
-| `gmr` | `BingqianWu/Teleopit-models` | GMR retargeting assets |
-| `bvh` | `BingqianWu/Teleopit-models` | Sample BVH motion files |
-| `data` | `BingqianWu/Teleopit-datasets` | Minimal shards for `lafan1`, `pico_record`, `seed`, and `twist2` |
-
-For asset management details (uploading, versioning), see [Asset Management](../reference/assets).
diff --git a/docs/docs/getting-started/installation.md b/docs/docs/getting-started/installation.md
index ed9daf6c..16b72638 100644
--- a/docs/docs/getting-started/installation.md
+++ b/docs/docs/getting-started/installation.md
@@ -2,101 +2,173 @@
 sidebar_position: 1
 ---
 
-# Installation
+# Install Teleopit
 
-Teleopit supports multiple installation profiles depending on your use case.
+Install only the parts you need. All commands below run from the repository
+root and require Python 3.10 or newer.
 
-## Prerequisites
-
-- Python 3.10+
-- [Conda](https://docs.conda.io/) (recommended)
+## 1. Get the Code
 
 ```bash
-conda create -n teleopit python=3.10
-conda activate teleopit
+git clone https://github.com/BotRunner64/Teleopit.git
+cd Teleopit
 ```
 
-## Install Profiles
+You only need Git submodules for a physical G1 or optional LinkerHand control;
+those steps appear later on this page.
+
+## 2. Create a Python Environment
+
+Choose one environment tool. Do not run all three sections.
 
-### Inference Only (sim2sim)
+### uv
 
 ```bash
-pip install -e .
+uv venv --python 3.10
+source .venv/bin/activate
 ```
 
-This is sufficient for offline BVH playback and MuJoCo simulation.
+When this page shows `pip install`, you may use `uv pip install` instead.
+
+### pip and venv
+
+```bash
+python3.10 -m venv .venv
+source .venv/bin/activate
+python -m pip install --upgrade pip
+```
 
-### Training
+### Conda
 
 ```bash
-pip install -e '.[train]'
+conda create -n teleopit python=3.10
+conda activate teleopit
 ```
 
-Adds `rsl-rl-lib`, `mjlab`, `wandb`, `swanlab`, and training dependencies.
+Conda creates the environment; use `pip install` inside that environment to
+install Teleopit.
+
+## 3. Install the Profile You Need
+
+Each extra includes the base Teleopit package. Start with the row matching your
+goal; you can install another extra later in the same environment.
+
+| Goal | Install command | What it adds |
+|------|-----------------|--------------|
+| Run a motion controller in MuJoCo | `pip install -e .` | Core inference, GMR, MuJoCo and ONNX Runtime |
+| Use Pico in simulation or on G1 | `pip install -e '.[pico4]'` | Pico receiver plus the sim2real runtime |
+| Replay BVH on a physical G1 without Pico | `pip install -e '.[sim2real]'` | G1 runtime and OpenCV |
+| Train a controller | `pip install -e '.[train]'` | mjlab, RSL-RL and experiment loggers |
+| Record Pico sim2real episodes | `pip install -e '.[recording]'` | Pico runtime and MP4 writing |
+| Review saved recordings | `pip install -e '.[review]'` | OpenCV and the MuJoCo/Viser reviewer |
+| Use OpenNeck with Pico | `pip install -e '.[openneck]'` | Pico runtime and the OpenNeck driver |
+| Run the test suite | `pip install -e '.[dev]'` | pytest and coverage tools |
 
-### Sim2Real (Hardware Deployment)
+## 4. Download the Matching Assets
+
+The Python package does not contain robot meshes, policies or motion datasets.
+Install the default ModelScope downloader once:
 
 ```bash
-pip install -e '.[sim2real]'
+pip install modelscope
 ```
 
-Adds `opencv-python`. You also need to initialize submodules and build/install the C++ `g1_bridge_sdk` bridge:
+Then download the bundle for your goal:
+
+| Goal | Command |
+|------|---------|
+| Simulation, Pico VR or G1 inference | `python scripts/setup/download_assets.py --only robots gmr ckpt bvh` |
+| Training from the distributed datasets | `python scripts/setup/download_assets.py --only robots data` |
+| Everything | `python scripts/setup/download_assets.py` |
+
+Use HuggingFace instead of ModelScope when needed:
 
 ```bash
-git submodule update --init --recursive
-bash scripts/setup/setup_g1_bridge.sh
+python scripts/setup/download_assets.py \
+    --source huggingface \
+    --only robots gmr ckpt bvh
 ```
 
-See [G1 Bridge SDK](../reference/g1-bridge-sdk) for details.
+The inference bundle creates the `track_g1` and `track_g1_neck_o6` ONNX/checkpoint
+pairs under `ckpt/`, plus the G1 model files, GMR files and a sample BVH under
+their expected project paths. See
+[Assets](../reference/resources/assets) for the complete inventory
+and asset group mapping.
+
+## 5. Additional Setup for a Physical G1
 
-### Pico 4 VR
+Build the C++ DDS bridge on the computer that will run Teleopit:
 
 ```bash
-pip install -e '.[pico4]'
+git submodule update --init --recursive
+bash scripts/setup/setup_g1_bridge.sh
 ```
 
-Teleopit uses the in-process `pico_bridge.PicoBridge` receiver for Pico tracking.
-Teleopit targets pico-bridge 0.2.1 and its `pico_native` tracking semantics.
-The receiver can run on a workstation PC or the robot onboard computer.
-See [Pico Sim2Sim](../tutorials/pico-sim2sim) and
-[Pico Sim2Real](../tutorials/pico-sim2real) for the full setup guides.
+The bridge is required for both Pico and BVH control on a real G1. See
+[Companion Projects](../reference/companion-projects#g1-bridge-sdk) if the
+build or robot connection fails.
+
+## 6. Optional Hardware
 
-Optional LinkerHand control for Pico sim2real uses local third-party packages.
-Install those packages directly after initializing the submodules:
+### LinkerHand L6 or O6
+
+Only install these local packages when `hands.enabled=true`:
 
 ```bash
 git submodule update --init --recursive
 pip install -e third_party/linkerhand-python-sdk
 pip install -e third_party/somehand
-scripts/setup/download_somehand_l6_assets.sh
+bash scripts/setup/download_somehand_assets.sh
 ```
 
-These packages are only required when `hands.enabled=true`.
+### OpenNeck
 
-### Sim2Real Recording
+The `openneck` extra already includes the Pico profile. Calibrate the device
+before enabling it:
 
 ```bash
-pip install -e '.[recording]'
+pip install -e '.[openneck]'
+openneck calibrate
 ```
 
-Adds the Pico sim2real stack plus the video dependencies used by
-`sim2real_record.yaml`. RealSense Python bindings are platform-specific: install
-`pyrealsense2` manually in the active environment when using
-`input.video.source=realsense`. On Arm machines, use conda-forge rather than the
-pip package:
+Teleopit uses the OpenNeck angle API. Old normalized calibration fields are not
+supported.
+
+### RealSense Recording or Preview
+
+Install `pyrealsense2` separately when a RealSense camera is enabled. On Arm
+machines, use conda-forge:
 
 ```bash
 conda install -c conda-forge pyrealsense2
 ```
 
-## Verify Installation
+Pico body tracking itself does not require RealSense.
+
+## 7. Verify the Environment
+
+Run the core import check:
 
 ```bash
 python -c "import teleopit; print('teleopit OK')"
-python -c "import train_mimic.tasks; print('training OK')"  # if training installed
 ```
 
-## Next Steps
+If you installed Pico or training dependencies, run the matching check:
+
+```bash
+python -c "from pico_bridge import PicoBridge; print('Pico OK')"
+python -c "import train_mimic.tasks; print('training OK')"
+```
+
+For an inference profile with the `robots gmr ckpt bvh` assets, finish with one
+sample simulation:
+
+```bash
+python scripts/run/run_sim.py \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
+```
 
-- [Download Assets](download-assets) - Download models and data
-- [Quick Start](quick-start) - Run your first simulation
+The installation is ready when a MuJoCo window opens and the simulated G1
+follows the sample motion. Close the window to stop, then continue with one of
+the four task-based tutorials.
diff --git a/docs/docs/getting-started/quick-start.md b/docs/docs/getting-started/quick-start.md
deleted file mode 100644
index bede79a1..00000000
--- a/docs/docs/getting-started/quick-start.md
+++ /dev/null
@@ -1,63 +0,0 @@
----
-sidebar_position: 3
----
-
-# Quick Start
-
-This guide walks you through running your first sim2sim playback in under 5 minutes.
-
-## Prerequisites
-
-1. [Install Teleopit](installation) (inference profile)
-2. [Download assets](download-assets) (`--only robots gmr ckpt bvh`)
-
-## Run Offline Sim2Sim
-
-```bash
-python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
-    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
-```
-
-You should see MuJoCo viewer windows showing the robot tracking the BVH motion.
-
-## Keyboard Controls
-
-When running with `playback.keyboard.enabled=true`:
-
-| Key | Action |
-|-----|--------|
-| `Space` / `P` | Pause / Resume |
-| `R` | Replay from start |
-| `Q` | Stop |
-
-```bash
-python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
-    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
-    playback.keyboard.enabled=true
-```
-
-## Viewer Modes
-
-Control which viewers are displayed:
-
-```bash
-# All viewers (mocap + retarget + sim2sim)
-python scripts/run/run_sim.py controller.policy_path=track.onnx viewers=all
-
-# No viewer (headless)
-python scripts/run/run_sim.py controller.policy_path=track.onnx viewers=none
-
-# Specific viewers
-python scripts/run/run_sim.py controller.policy_path=track.onnx 'viewers=[retarget,sim2sim]'
-```
-
-## What's Next
-
-- [Offline Sim2Sim Tutorial](../tutorials/offline-sim2sim) - Full guide with rendering
-- [Pico Sim2Sim](../tutorials/pico-sim2sim) - Verify Pico tracking in MuJoCo
-- [Standalone Standing](../tutorials/standalone-standing) - Check G1 bridge, network, and policy standing
-- [Pico Sim2Real](../tutorials/pico-sim2real) - Deploy Pico teleoperation to Unitree G1
-- [BVH Sim2Real](../tutorials/bvh-sim2real) - Replay offline BVH motions on Unitree G1
-- [Training](../tutorials/training) - Train your own policy
diff --git a/docs/docs/intro.md b/docs/docs/intro.md
index 8555ed00..6eb4111b 100644
--- a/docs/docs/intro.md
+++ b/docs/docs/intro.md
@@ -3,45 +3,43 @@ sidebar_position: 1
 slug: /
 ---
 
-# Introduction
+# Teleopit
 
 > **Looking for Chinese docs?** [中文文档点此进入](https://BotRunner64.github.io/Teleopit/zh-Hans/)
 
-**Teleopit** is a lightweight, extensible whole-body teleoperation framework for humanoid robots. It provides real-time motion retargeting from human operators to Unitree G1 robots, supporting both MuJoCo simulation and real hardware deployment.
+Teleopit is a **full-embodiment humanoid teleoperation system for the Unitree
+G1**. With a supported Pico headset, an operator can drive the robot's
+whole-body motion in real time. In onboard deployments, optional LinkerHand
+hands reproduce hand gestures, and an optional OpenNeck gimbal turns head
+motion into active camera control.
 
-## Key Features
+The same motion controller runs in MuJoCo first, so you can check tracking and
+controls before connecting a physical robot.
 
-- **Offline sim2sim**: Play back BVH motion capture files through RL policy in MuJoCo
-- **VR teleoperation**: Real-time whole-body control via Pico 4 / Pico 4 Ultra full body tracking
-- **Sim2real deployment**: Deploy to Unitree G1 hardware with the same pipeline
-- **Training pipeline**: End-to-end RL training with General-Tracking-G1 task
-- **Extensible design**: Protocol-based components (InputProvider, Retargeter, Controller, Robot)
+## Start Here
 
-## Pipeline Overview
+If this is your first time using Teleopit:
 
-```text
-InputProvider (BVH / Pico4 VR)
-    -> Retargeter (GMR)
-    -> ObservationBuilder (167D)
-    -> Controller (dual-input TemporalCNN ONNX)
-    -> Robot (MuJoCo sim or Unitree G1)
-```
+1. [Install Teleopit](getting-started/installation) for the job you want to do
+   and complete the check at the end of that page.
+2. Continue with one of the four guides below.
 
-## Technical Specs
+| I want to... | Follow this guide |
+|--------------|-------------------|
+| Check a motion controller in MuJoCo | [Run a Motion Controller in Simulation](tutorials/offline-sim2sim) |
+| Try Pico VR control without a real robot | [VR Teleoperation in Simulation](tutorials/pico-sim2sim) |
+| Control a physical G1 with Pico VR | [VR Teleoperation on Unitree G1](tutorials/pico-sim2real) |
+| Train and export my own controller | [Train a Motion Controller](tutorials/training) |
 
-| Spec | Value |
-|------|-------|
-| Policy frequency | 50 Hz |
-| PD control frequency | 200 Hz |
-| Observation dimension | 167D |
-| Action dimension | 29D (G1 joints) |
-| ONNX model | Dual-input TemporalCNN |
-| Retargeting | GMR (General Motion Retargeting) |
-| Simulator | MuJoCo |
-| Hardware | Unitree G1 (29 DOF) |
+:::warning Before using a real robot
+Make the Pico workflow work in simulation first. Keep the Unitree remote in
+hand during hardware operation; `L1+R1` is the emergency path to `DAMPING`.
+:::
 
-## What's Next
+## Looking for Implementation Details?
 
-- [Installation](getting-started/installation) - Set up your environment
-- [Quick Start](getting-started/quick-start) - Run your first sim2sim
-- [Tutorials](tutorials/offline-sim2sim) - Step-by-step guides for each use case
+The user guides intentionally keep internals out of the main flow. See
+[Architecture](reference/architecture) for the runtime pipeline and technical
+specifications, [Assets](reference/resources/assets) for every
+downloaded file, or
+[Configuration](reference/configuration/overview) for Hydra options.
diff --git a/docs/docs/reference/architecture.md b/docs/docs/reference/architecture.md
index f979d687..66d3719c 100644
--- a/docs/docs/reference/architecture.md
+++ b/docs/docs/reference/architecture.md
@@ -1,79 +1,161 @@
 ---
-sidebar_position: 1
+sidebar_position: 2
 ---
 
 # Architecture
 
-System internals and technical constraints for developers.
+This page defines Teleopit's runtime pipelines, repository layout, supported
+technical surface, and public entry points.
 
 ## Pipeline
 
-```text
-InputProvider (BVH file / Pico4)
-    -> Retargeter (GMR)
-    -> ObservationBuilder (167D)
-    -> Controller (dual-input TemporalCNN ONNX)
-    -> Robot (MuJoCo sim or Unitree G1)
-```
-
-Offline/online inference is assembled by `teleopit/runtime/` and `teleopit/pipeline.py`. The hardware state machine runs through the process-isolated runtime in `teleopit/sim2real/mp/`. Training is provided by `train_mimic/`.
-
-## Code Structure
+![Teleopit runtime pipelines](/img/diagrams/architecture-pipeline.svg)
+
+The main tracking path converts BVH or live PICO body motion into a time-aligned
+G1 reference. `VelCmdObservationBuilder` combines that reference with robot
+state, and the dual-input TemporalCNN ONNX controller produces 29 joint offsets.
+The same observation and controller path drives MuJoCo and the real G1.
+
+Pico hand and active-vision paths are optional process-isolated workers. They
+reuse the same in-process `PicoBridge` receiver and never add fields to the 167D
+tracking-policy observation. A hand or neck failure must not stop G1 body
+control. These optional hardware paths are supported by onboard deployment;
+external-host Pico deployment supports whole-body control only.
+
+Host-policy deployment is independent from the Pico runtime. A separate host
+environment receives JPEG RGB, measured G1 joint positions, raw measured O6
+readback, measured OpenNeck angles, and an observation-time source reference
+root pose. The body/hand/neck arrays form the 43D model observation; the
+session-local source pose only anchors reconstruction of source-relative root
+output. The host returns canonical `float32[T,50]` action chunks over strict
+ZeroMQ/msgpack messages. The onboard validator and scheduler convert the body
+portion into a 36D reference for the existing motion tracker; host output never
+bypasses that tracker or becomes a direct motor command.
+
+The Teleopit and host environments share semantic data and one identical
+`hand_calibration.json`, but do not import each other's Python packages. The
+current client/server code and protocol tests define the network structure, so
+both repositories must change together when that protocol changes.
+
+## Runtime Boundaries
+
+- Offline core components communicate through `InProcessBus` without copying
+  array payloads.
+- Sim2real robot control, reference generation, camera, recording, hand, neck,
+  and host-policy client work are process-isolated where blocking or hardware
+  failure could disturb the 50 Hz control loop.
+- Local sim2real workers use localhost ZeroMQ and shared-memory video rings.
+- The external host-policy boundary uses msgpack and non-pickle float32 arrays.
+- Shared component contracts are `typing.Protocol` definitions in
+  `teleopit/interfaces.py`.
+
+## Repository Layout
 
 ```text
-configs / scripts
-    -> runtime
-    -> interfaces + pipeline state machines
-    -> adapters (inputs / retargeting / controller / robot / recording)
-
-train_mimic/scripts
-    -> train_mimic/app.py
-    -> single task registry / env builder / runner cfg
-    -> mjlab / rsl_rl
-
-train_mimic/scripts/data
-    -> train_mimic/data/dataset_builder.py
-    -> dataset_lib / motion_fk / convert_pkl_to_npz
+teleopit/                              — Core inference and deployment package
+├── interfaces.py                     — Robot, controller, input and retargeting protocols
+├── pipeline.py                       — Thin offline simulation facade
+├── runtime/                          — Config/path resolution, factories and CLI validation
+├── configs/                          — Hydra runtime configuration
+├── bus/                              — In-process zero-copy publish/subscribe
+├── inputs/                           — BVH, PICO and realtime input adapters
+├── retargeting/gmr/                  — Self-contained whole-body GMR implementation
+├── controllers/                      — Observation builder and ONNX policy controller
+├── robots/                           — MuJoCo robot adapter
+├── sim/                              — 200 Hz PD / 50 Hz policy simulation loop
+├── sim2real/
+│   ├── mp/                           — Process supervisor, IPC and robot-control state machine
+│   ├── hands/                        — Optional LinkerHand drivers and input mapping
+│   └── neck/                         — Optional OpenNeck mapping and worker
+├── high_level_policy/                — Host protocol, frame transforms and action scheduler
+└── recording/                        — Sim2real dataset schema and recording workers
+
+train_mimic/                          — Training package
+├── app.py                            — Shared train/play/benchmark assembly
+├── tasks/tracking/                   — General-Tracking-G1 task and TemporalCNN model
+├── data/                             — Dataset construction and motion loading
+└── scripts/                          — Training, playback, benchmark and ONNX export
+
+scripts/                              — User-facing runtime and maintenance entry points
+├── run/                              — Simulation, sim2real and recording commands
+├── setup/                            — Asset download and hardware setup
+├── render/                           — Offline video rendering
+├── view/                             — Recording review
+└── dev/                              — Validation and calibration utilities
+
+third_party/                          — Optional hardware SDKs and somehand
+tests/                                — Unit, protocol and integration tests
 ```
 
-## Core Boundaries
-
-| Module | Role |
-|--------|------|
-| `teleopit/interfaces.py` | Stable protocols: InputProvider, Retargeter, Controller, Robot, ObservationBuilder |
-| `teleopit/runtime/` | Config parsing, path normalization, component assembly, CLI validation |
-| `teleopit/pipeline.py` | Lightweight facade for offline sim |
-| `teleopit/sim2real/mp/` | Process-isolated sim2real state machine, IPC, and robot-control loop |
-| `teleopit/controllers/observation.py` | ObservationBuilder |
-| `teleopit/controllers/rl_policy.py` | Accepts dual-input ONNX whose observation dimension matches the runtime builder |
-| `train_mimic/app.py` | Shared train/play/benchmark assembly |
-| `train_mimic/tasks/tracking/config/` | Single task registration (`General-Tracking-G1`) |
-| `train_mimic/data/dataset_builder.py` | Sole official dataset construction entry |
-
 ## Technical Specifications
 
-| Spec | Value |
-|------|-------|
+| Specification | Supported value |
+|---------------|-----------------|
+| Robot | Unitree G1 with 29 actuated joints |
+| Simulator | MuJoCo |
+| Whole-body retargeting | GMR (General Motion Retargeting) |
+| Policy / PD rates | 50 Hz / 200 Hz |
 | Training task | `General-Tracking-G1` |
 | Inference observation | `velcmd_history` (167D) |
-| ONNX signature | Dual-input `obs` (167D) + `obs_history` |
-| Actor/Critic | TemporalCNN (2048, 1024, 512, 256, 128) |
-| Training sampling | Default `rewind`; also supports `uniform`; playback/benchmark use `start` |
-| Training `window_steps` | `[0]` |
-| Data format | Minimal recursive HDF5 shards (`shard_*.h5`) |
+| ONNX signature | Dual input: `obs` (167D) + `obs_history` |
+| Policy action | 29D joint offsets from `default_dof_pos` |
+| Actor / critic | TemporalCNN (2048, 1024, 512, 256, 128) |
+| Training sampling | `rewind` by default; `uniform` supported; playback uses `start`; benchmark pins exact clips and disables clip-end resampling |
+| Training window | `window_steps=[0]` |
+| Distributed motion data | Minimal recursive HDF5 `shard_*.h5` files |
+| Optional hands | LinkerHand L6/O6 with gripper or PICO hand-pose input |
+| Optional active vision | OpenNeck yaw/pitch in physical degrees |
+| Host-policy observation | JPEG RGB + G1 joint position (29D) + raw O6 readback (12D) + OpenNeck degrees (2D); request also carries the camera-time active reference root pose (7D) |
+| Host-policy action | `float32[T,50]`, 30 Hz source horizon, `T` in `[1,50]` |
+| Host-policy body control | 36D root/joint reference through the existing 50 Hz motion tracker |
 
 ## Constraints
 
-- `controller.policy_path` must be explicitly provided and the file must exist
-- Offline BVH runs require explicit `input.bvh_file`
-- `viewers` is the sole viewer configuration entry
-- Observation/ONNX dimension mismatch causes immediate startup error
-- sim2real also requires a dual-input ONNX whose observation dimension matches the runtime builder
-
-## Public Surface
-
-**Stable run modes:** offline sim2sim, offline sim2real playback, Pico4 sim2sim, G1 sim2real
-
-**Stable training entry points:** `train.py`, `play.py`, `benchmark.py`, `save_onnx.py`
-
-**Stable data entry points:** `build_dataset.py`, `precompute_dataset.py`
+- `controller.policy_path` must be explicit and point to an existing file.
+- Offline BVH runs require an explicit, existing `input.bvh_file`.
+- `viewers` is the only viewer configuration key.
+- Observation definitions and ONNX signatures must match exactly; startup fails
+  instead of padding or trimming data.
+- `default_dof_pos` must come from the selected robot's default standing angles.
+- Sim2real requires the same dual-input observation contract used in simulation.
+- Host message-envelope or schema mismatches are rejected while the robot
+  remains in `STANDING`. Shape, finiteness, session, sequence, quaternion,
+  staleness, and safety violations reject the whole action chunk.
+- Host actions are validated, scheduled, and rate-limited onboard. The host
+  cannot bypass the motion tracker or send G1 motor commands.
+- Policy entry remains an internal `STANDING` flow while one host session waits
+  for its first valid chunk. That chunk enters `POLICY` directly, with no
+  candidate alignment, entry Kp ramp, or second session/reset. The 50 Hz limiter
+  starts from the measured robot reference captured at session start.
+- Temporal root, yaw, and joint-reference discontinuities are accepted at chunk
+  boundaries and inside chunks, then rate-limited at the 50 Hz scheduler output
+  so recorded pause/resume transitions remain usable.
+- PICO input, RealSense preview, recording, hand, and neck failures are
+  non-critical; the Unitree remote and robot-control loop remain available.
+
+## Public Entry Points
+
+Supported run modes are offline sim2sim, offline sim2real playback, PICO
+sim2sim, PICO G1 sim2real, and independent host-policy G1 sim2real.
+
+Runtime commands:
+
+- `scripts/run/run_sim.py` — offline BVH and live PICO sim2sim
+- `scripts/run/run_sim2real.py` — BVH or PICO G1 sim2real
+- `scripts/run/run_high_level_policy_sim2real.py` — independent host-policy G1 deployment
+- `scripts/run/record_pico_motion.py` — record retargeted motion clips from PICO
+- `scripts/render/render_sim.py` — render mocap, retargeting, and sim2sim videos
+- `scripts/view/view_recording.py` — review synchronized sim2real recordings
+
+Training and data commands:
+
+- `train_mimic/scripts/train.py`, `play.py`, `benchmark.py`, `save_onnx.py`
+- `train_mimic/scripts/data/build_dataset.py`
+- `train_mimic/scripts/data/precompute_dataset.py`
+
+Public Python surfaces:
+
+- Protocols in `teleopit/interfaces.py`
+- `TeleopPipeline`
+- `VelCmdObservationBuilder`
+- `RLPolicyController`
diff --git a/docs/docs/reference/companion-projects.md b/docs/docs/reference/companion-projects.md
new file mode 100644
index 00000000..a43d26aa
--- /dev/null
+++ b/docs/docs/reference/companion-projects.md
@@ -0,0 +1,88 @@
+---
+sidebar_position: 4
+---
+
+# Companion Projects
+
+Teleopit integrates four focused components for robot communication, hand
+retargeting, active vision, and PICO transport. They are kept outside the
+`teleopit` Python package so each project can own its hardware protocol and
+public API.
+
+| Component | Source | Function | Use in Teleopit |
+|-----------|--------|----------|-----------------|
+| G1 Bridge SDK | [Teleopit source tree](https://github.com/BotRunner64/Teleopit/tree/master/third_party/g1_bridge_sdk) | Native C++/pybind11 bridge over Unitree SDK2 and Cyclone DDS | Real-time G1 state, remote input, mode selection, and 200 Hz low-level commands |
+| somehand | [GitHub](https://github.com/BotRunner64/somehand) | Dexterous-hand retargeting library | Maps live Pico hand landmarks to LinkerHand L6/O6 targets |
+| OpenNeck | [GitHub](https://github.com/BotRunner64/OpenNeck) | Calibrated two-axis neck driver | Converts physical yaw/pitch degrees to safe servo commands |
+| PICO Bridge | [GitHub](https://github.com/BotRunner64/pico-bridge) | Headset app and Python receiver for PICO tracking and video | Supplies body, controller, hand, and HMD frames and optionally returns RGB video |
+
+## G1 Bridge SDK
+
+G1 Bridge SDK is maintained directly in Teleopit under
+`third_party/g1_bridge_sdk`; it is not a separate repository. Its setup script
+downloads [Unitree SDK2](https://github.com/unitreerobotics/unitree_sdk2), then
+builds and installs the local pybind11 extension:
+
+```bash
+bash scripts/setup/setup_g1_bridge.sh
+```
+
+All DDS publish/subscribe work runs on native C++ threads. Teleopit's
+`UnitreeG1` adapter reads joint state, base orientation, angular velocity, and
+wireless-remote input through the bridge, and sends 29-joint position targets
+with per-joint PD gains. This is the hardware boundary used by sim2real
+teleoperation, the standalone standing check, and host-policy deployment.
+
+## somehand
+
+somehand provides configurable human-to-robot hand retargeting. Teleopit pins
+the compatible source as the `third_party/somehand` Git submodule and uses its
+0.3.0 public `somehand.api` surface.
+
+In `hands.mode=vr_hand_pose`, Teleopit converts PICO's 26-joint hand state to
+21 landmarks, calls somehand for continuous retargeting, and sends the result
+to LinkerHand L6 or O6. Teleopit owns the live Pico receiver and the landmark
+conversion; it does not start somehand's standalone Pico input path.
+
+Install the dexterous-hand dependencies with:
+
+```bash
+git submodule update --init --recursive
+pip install -e third_party/linkerhand-python-sdk
+pip install -e third_party/somehand
+```
+
+## OpenNeck
+
+OpenNeck owns serial communication, degree-to-servo-step conversion, and
+calibrated mechanical limits for the two-axis active-vision gimbal. Teleopit
+supports the OpenNeck 0.2.0 physical-angle API and calls `move_deg()`; removed
+normalized control fields are not compatible.
+
+For Pico teleoperation, Teleopit computes HMD rotation relative to the
+same-frame `Body.Spine3` orientation, applies the configured dead zone and
+pitch gain, and sends yaw/pitch degrees from a non-critical neck worker. Host
+policy deployment sends the validated neck fields from its canonical action.
+
+```bash
+pip install -e '.[openneck]'
+openneck calibrate
+```
+
+## PICO Bridge
+
+PICO Bridge contains both the headset application and the importable Python PC
+receiver. Teleopit supports release 0.2.1, installed by the `pico4` extra:
+
+```bash
+pip install -e '.[pico4]'
+```
+
+One in-process `PicoBridge` instance supplies full-body, controller, hand, and
+independent HMD data to Teleopit. Whole-body retargeting, hand control, and
+OpenNeck all reuse that receiver. When video is enabled, Teleopit can also push
+MuJoCo or RealSense RGB frames back to the headset through
+`push_video_frame()`.
+
+Download the headset APK from the
+[PICO Bridge releases](https://github.com/BotRunner64/pico-bridge/releases).
diff --git a/docs/docs/reference/configuration/fields.md b/docs/docs/reference/configuration/fields.md
new file mode 100644
index 00000000..b2831724
--- /dev/null
+++ b/docs/docs/reference/configuration/fields.md
@@ -0,0 +1,368 @@
+---
+sidebar_position: 2
+---
+
+# Configuration Fields
+
+Complete reference for Teleopit's Hydra configuration fields.
+
+## Top-Level Fields
+
+| Field | Description | Default |
+|-------|-------------|---------|
+| `policy_hz` | Policy inference frequency | `50` |
+| `pd_hz` | PD control frequency (simulation only) | `200` |
+| `viewers` | Viewer set: `mocap`, `retarget`, `sim2sim`, `camera`, `all`, `none`. `all` opens `mocap`, `retarget`, and `sim2sim`; add `camera` explicitly. | `sim2sim` |
+| `realtime` | Rate-limit to wall clock | `false` |
+| `num_steps` | Number of steps; `0` = infinite | `0` |
+| `keyboard.enabled` | Enable realtime keyboard mode control for sim2sim | `false` |
+| `playback.pause_on_end` | Pause at last frame when offline motion ends | `false` |
+| `playback.keyboard.enabled` | Enable keyboard control for offline playback | `false` |
+
+## Robot
+
+| Field | Description | Default |
+|-------|-------------|---------|
+| `robot.type` | Stable robot type written into recording schemas | `unitree_g1_29dof` |
+| `robot.num_actions` | Joint action dimension | `29` |
+| `robot.xml_path` | MuJoCo XML path | - |
+| `d435i_rgb` | Fixed RGB camera in the G1 MJCF; use `viewers=[sim2sim,camera]` to display it | - |
+| `robot.kps` / `robot.kds` | PD gains | - |
+| `robot.default_angles` | Default standing pose | - |
+| `robot.torque_limits` | Joint torque limits | - |
+
+## Controller
+
+| Field | Description | Default |
+|-------|-------------|---------|
+| `controller.policy_path` | **Required.** Path to ONNX policy file | - |
+| `controller.device` | Inference device: `cpu` / `auto` / `cuda:N` | `cpu` |
+| `controller.action_scale` | Action scaling factor | - |
+| `controller.clip_range` | Action clipping range | - |
+| `controller.default_dof_pos` | Joint angle offset base | - |
+
+## Input
+
+### Offline BVH
+
+| Field | Description |
+|-------|-------------|
+| `input.bvh_file` | **Required.** Path to BVH file |
+| `input.bvh_format` | `lafan1` / `hc_mocap` |
+| `input.human_format` | Human skeleton format |
+
+> BVH input does not set `input.provider` — it is inferred from the config group name.
+
+### Pico 4
+
+| Field | Description | Default |
+|-------|-------------|---------|
+| `input.provider` | `pico4` | `pico4` |
+| `input.human_format` | Retarget skeleton format | `pico_bridge` |
+| `input.pico4_timeout` | Wait timeout in seconds | `60` |
+| `input.pico4_buffer_size` | Frame buffer size | `60` |
+| `input.pause_button` | Button for pause/resume | `A` |
+| `input.pause_debounce_s` | Debounce time for pause button | `0.25` |
+| `input.arms_button` | Button for Pico `MOCAP` / `ARMS` toggle | `B` |
+| `input.arms_debounce_s` | Debounce time for arms-mode button | `0.25` |
+| `input.bridge_host` | Teleopit host receiver bind host | `0.0.0.0` |
+| `input.bridge_port` | Teleopit host receiver TCP/UDP port | `63901` |
+| `input.bridge_discovery` | Enable pico-bridge discovery advertising | `true` |
+| `input.bridge_advertise_ip` | Optional advertised host IP override | `null` |
+| `input.bridge_start_timeout` | Timeout while starting the bridge | `10.0` |
+| `input.bridge_history_size` | Pico frame history retained by the bridge | `120` |
+| `input.video.enabled` | Stream host camera preview back to Pico through pico-bridge 0.2.1 | `false` |
+| `input.video.source` | Video source: `mujoco`, `realsense`, or `test-pattern` | `null` |
+| `input.video.width` / `height` / `fps` | Video capture/render settings | `1280` / `720` / `30` |
+| `input.video.device` | Optional RealSense serial | `null` |
+
+### Realtime
+
+| Field | Description |
+|-------|-------------|
+| `retarget_buffer_enabled` | Enable retarget buffering |
+| `retarget_buffer_window_s` | Buffer window size |
+| `retarget_buffer_delay_s` | Buffer delay |
+| `reference_steps` | Reference window steps |
+| `realtime_buffer_warmup_steps` | Warmup before playback |
+| `reference_velocity_smoothing_alpha` | Velocity smoothing |
+| `reference_anchor_velocity_smoothing_alpha` | Anchor velocity smoothing |
+
+## Sim2Real
+
+Fields used by sim2real configs (`sim2real.yaml`, `pico4_sim2real.yaml`).
+
+Sim2real defaults to `viewers=none`. Set `viewers=retarget` to open an optional
+MuJoCo window showing the retargeted reference; `sim2sim`, `mocap`, `camera`,
+and `all` are simulation-only viewer modes.
+
+### Safety
+
+| Field | Description | Default |
+|-------|-------------|---------|
+| `startup_ramp_duration` | Kp ramp duration after entering `STANDING`; gradually increases PD gains without changing policy targets | `2.0` |
+| `joint_vel_limit` | Joint velocity limit (rad/s); triggers emergency damping if exceeded | `10.0` |
+| `mocap_switch.check_frames` | Consecutive valid frames required before switching to MOCAP | `10` |
+| `arm_mocap.controlled_joint_indices` | G1 joints driven by live retargeting in Pico `ARMS` mode | `[15..28]` |
+
+### Host High-Level Policy (independent sim2real)
+
+`high_level_policy_sim2real.yaml` is used only by
+`scripts/run/run_high_level_policy_sim2real.py`. It starts camera, network
+client, robot-control, LinkerHand O6, and OpenNeck workers. It does not start
+PicoBridge, GMR, or a retarget reference worker. The host LeRobot environment
+remains separate and must track the current client/server message structure and
+protocol tests. The only shared data file is `hand_calibration.json`.
+
+| Field | Description | Default |
+|-------|-------------|---------|
+| `camera.source` | Onboard policy camera: `realsense` or integration-only `test-pattern` | `realsense` |
+| `camera.width` / `height` / `fps` | Exact policy image contract | `640` / `480` / `30` |
+| `camera.device` | Optional RealSense serial | `null` |
+| `standing_return_ramp_duration` | Kp-ramp duration when returning from active control to `STANDING` | `2.0` |
+| `high_level_policy.endpoint` | Host policy ZeroMQ TCP endpoint | `tcp://127.0.0.1:5555` |
+| `high_level_policy.task` | Non-empty task prompt sent on reset and every observation | `demo` |
+| `high_level_policy.timeout_s` | Per-request network deadline; expiry pauses `POLICY` | `1.0` |
+| `high_level_policy.reconnect_backoff_s` | Retry delay while establishing a new session | `1.0` |
+| `high_level_policy.replan_steps` | Minimum interval between requests in 30 Hz source frames; must not exceed the horizon reported by the host | `3` |
+| `high_level_policy.jpeg_quality` | JPEG quality for the 640x480 RGB frame | `90` |
+| `high_level_policy.max_observation_age_s` | Maximum camera/observation age before a request is skipped | `0.15` |
+| `high_level_policy.max_result_age_s` | Maximum local IPC age before a received result is rejected | `0.1` |
+| `high_level_policy.entry_timeout_s` | Maximum time to establish the entry session and receive its first valid chunk, and maximum fresh-chunk wait on resume | `5.0` |
+| `high_level_policy.hold_s` | Final-reference grace period after the active plan horizon before the action watchdog pauses `POLICY` | `3.0` |
+| `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | Accepted absolute root-height range | `0.55` / `1.05` |
+| `high_level_policy.safety.max_root_xy_speed_m_s` | Root XY speed limit applied to the 50 Hz scheduler output | `2.5` |
+| `high_level_policy.safety.max_root_displacement_m` | Source-frame-equivalent 3D root step used by the 50 Hz output limiter | `0.1` |
+| `high_level_policy.safety.max_yaw_rate_rad_s` | Root yaw-rate limit applied to the 50 Hz scheduler output | `2.5` |
+| `high_level_policy.safety.max_joint_rate_rad_s` | Per-joint rate limit applied to the 50 Hz scheduler output | `10.0` |
+| `high_level_policy.safety.max_joint_projection_rad` | Maximum correction allowed when clipping a G1 joint reference to its position limit | `0.1` |
+| `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | OpenNeck yaw clipping range | `-45` / `45` |
+| `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | OpenNeck pitch clipping range | `-40` / `40` |
+
+The request loop is asynchronous and receding-horizon. The isolated client has
+at most one ZeroMQ request in flight, selects the latest eligible observation
+at the configured source-frame stride, and leaves the current action plan
+running during host inference. A newer response replaces that plan according
+to its echoed onboard monotonic observation timestamp.
+
+G1 reference joint positions are clipped to
+`real_robot.joint_pos_lower/upper` when the required correction does not exceed
+`high_level_policy.safety.max_joint_projection_rad`; larger corrections reject
+the chunk. OpenNeck yaw/pitch values are clipped to their configured ranges and
+do not reject a chunk solely because of neck overshoot. The initial runtime
+requires
+`hands.driver=linkerhand_o6`, both hand sides, and `neck.driver=openneck` because
+all canonical 50D action fields are active. OpenNeck policy values go directly
+to `move_deg(yaw, pitch)` after onboard clipping and chunk validation; Pico
+dead-zone and pitch-gain mapping are not applied.
+
+### Real Robot
+
+| Field | Description | Default |
+|-------|-------------|---------|
+| `real_robot.network_interface` | Network interface for Unitree DDS communication. For wired PC-to-G1 control, find the cable interface with `ifconfig` and set that name, for example `enp130s0`; for onboard robot execution, `eth0` is usually correct. | `eth0` |
+| `real_robot.kp_real` | Real-robot proportional gains (per joint) | - |
+| `real_robot.kd_real` | Real-robot derivative gains (per joint) | - |
+| `real_robot.kd_damping` | Damping mode kd | `8.0` |
+| `real_robot.control_mode` | Ankle control mode (`PR` = Pitch-Roll) | `PR` |
+| `real_robot.joint_pos_lower` | Joint position lower limits (rad) | - |
+| `real_robot.joint_pos_upper` | Joint position upper limits (rad) | - |
+
+### Pause/Resume (Pico sim2real)
+
+Realtime Pico resume re-centers heading and ground-plane position before tracking continues. Operators should keep still and stay as close as practical to the paused pose to reduce sudden reference changes.
+
+### Dexterous Hand (Pico sim2real)
+
+`hands.enabled=true` requires `input.provider=pico4` plus local editable
+installs of `third_party/linkerhand-python-sdk` and `third_party/somehand`.
+When enabled, hand control remains active in all sim2real modes.
+`gripper` supports `linkerhand_l6` and `linkerhand_o6`. The corresponding
+controller's side grip trigger is a deadman enable: while it is held, the index
+trigger interpolates between the configured open and close poses; releasing
+the side grip trigger commands that hand to open. `vr_hand_pose` is supported
+by `linkerhand_l6` and `linkerhand_o6`: missing hand pose holds the last command
+for that side, the selected hand speed is set to the maximum, and Teleopit
+converts Pico hand state to 21 landmarks before calling somehand 0.3.0 through
+`somehand.api` only.
+
+| Field | Description | Default |
+|-------|-------------|---------|
+| `hands.enabled` | Enable optional hand worker | `false` |
+| `hands.driver` | Hand driver plugin: `linkerhand_l6` or `linkerhand_o6` | `linkerhand_l6` |
+| `hands.mode` | `gripper` or `vr_hand_pose` | `gripper` |
+| `hands.sides` | Controlled sides | `[left, right]` |
+| `hands.rate_hz` | Maximum gripper command rate in Hz | `30.0` |
+| `hands.frame_timeout_s` | Controller or hand-pose staleness threshold | `0.3` |
+| `hands.linkerhand_l6.left_can` / `right_can` | CAN channels for each hand | `can0` / `can1` |
+| `hands.linkerhand_l6.speed` | L6 speed used by `gripper`; `vr_hand_pose` overrides this to maximum speed | see config |
+| `hands.linkerhand_l6.open_pose` / `close_pose` | Six-value L6 open/closed poses | see config |
+| `hands.linkerhand_o6.left_can` / `right_can` | CAN channels for each O6 hand | `can0` / `can1` |
+| `hands.linkerhand_o6.speed` | O6 speed used by `gripper`; `vr_hand_pose` overrides this to maximum speed | see config |
+| `hands.linkerhand_o6.open_pose` / `close_pose` | Six-value O6 open/closed poses | see config |
+| `hands.somehand.l6_config_path` | Official somehand 0.3.0 bi-hand L6 config used by L6 `vr_hand_pose` | see config |
+| `hands.somehand.o6_config_path` | Official somehand 0.3.0 bi-hand O6 config used by O6 `vr_hand_pose` | see config |
+| `hands.somehand.rate_hz` | Low-latency `vr_hand_pose` command rate in Hz | `60.0` |
+| `hands.somehand.max_iterations` | somehand solver iteration cap for `vr_hand_pose` | `12` |
+| `hands.somehand.temporal_filter_alpha` | somehand input landmark smoothing alpha; `1.0` disables smoothing delay | `1.0` |
+| `hands.somehand.output_alpha` | somehand qpos output smoothing alpha; `1.0` disables smoothing delay | `1.0` |
+
+### OpenNeck Active Vision (Pico sim2real)
+
+`neck.enabled=true` requires `input.provider=pico4` and the `openneck` extra. The
+neck worker reuses Teleopit's existing Pico receiver and does not start
+a second `PicoBridge` or RealSense pipeline. OpenNeck runs as a non-critical
+sim2real worker and does not change the policy observation. Head motion comes
+from the independent HMD `PicoFrame.head.rotation`, mapped relative to
+`Body.Spine3` from the same source frame. The neck path never reads the
+full-body tracker's `Body.Head` skeleton joint, whose model constraints can
+under-report extreme head pitch. HMD updates remain independent of duplicate
+body-frame filtering. The mapper uses the fixed PICO neutral orientation and
+no neck-side EMA; startup does not capture the operator's first pose as a new
+zero pose, so the operator does not need to face straight when tracking starts.
+Teleopit converts the supported PICO convention to OpenNeck's physical
+convention—positive yaw turns left and positive pitch looks up. After applying
+`neck.dead_zone_deg` to the raw relative angles, it multiplies pitch by
+`neck.pitch_gain` (default `1.4`) while leaving yaw one-to-one. The resulting
+physical angles are sent through OpenNeck 0.2.0 `move_deg()`. OpenNeck performs
+the direct-drive degree-to-step conversion and clips each target to the
+mechanical step limits in its calibration file.
+
+OpenNeck 0.2.0 calibration files use angle-control fields such as
+`yaw_center_step`, `yaw_min_step`, `yaw_max_step`, and `yaw_step_sign` (and the
+corresponding pitch fields). The previous normalized OpenNeck configuration is
+unsupported; run `openneck calibrate` to create a current file. Teleopit's
+removed `neck.yaw_range_deg`, `neck.pitch_range_deg`, and `neck.invert_*` keys
+are rejected rather than ignored.
+
+| Field | Description | Default |
+|-------|-------------|---------|
+| `neck.enabled` | Enable optional OpenNeck worker | `false` |
+| `neck.driver` | Neck driver plugin; currently `openneck` | `openneck` |
+| `neck.config_path` | Optional OpenNeck 0.2.0 angle-calibration config path | `null` |
+| `neck.port` | Optional serial port override, for example `/dev/ttyACM0` | `null` |
+| `neck.rate_hz` | Maximum neck command rate in Hz | `60.0` |
+| `neck.frame_timeout_s` | Pico HMD/Spine3 pose staleness threshold | `0.2` |
+| `neck.active_modes` | Sim2real modes that allow neck motion | `[standing, mocap, arms, pause]` |
+| `neck.dead_zone_deg` | Yaw/pitch dead zone in degrees | `0.5` |
+| `neck.pitch_gain` | Gain applied to relative HMD pitch after the dead zone | `1.4` |
+| `neck.center_on_start` / `center_on_shutdown` | Center the gimbal at worker startup/shutdown | `true` / `false` |
+| `neck.release_on_shutdown` | Release servo torque after shutdown when supported | `false` |
+| `neck.dry_run` | Compute commands without opening OpenNeck hardware | `false` |
+
+### HDF5 Recording (Pico sim2real)
+
+`recording.enabled=true` is supported only with `input.provider=pico4`,
+`input.video.enabled=true`, `input.video.source=realsense`, and an interactive
+terminal. The recorder is manual: `R` starts an episode, `S` saves the active
+episode, `D` discards the active episode, and `Q` shuts down. `STANDING`,
+`MOCAP`, `ARMS`, and paused mocap can be recorded.
+
+`sim2real_record.yaml` enables both recording and the required RealSense
+`input.video` path. Recording does not open a second camera; it consumes the
+same frames produced by `pico_input`.
+
+| Field | Description | Default |
+|-------|-------------|---------|
+| `recording.enabled` | Enable manual HDF5 recording | `false` |
+| `recording.output_dir` | Dataset root directory | `data/recordings/sim2real_hdf5` |
+| `recording.task` | Episode task prompt written to `episodes.jsonl` | `demo` |
+| `recording.fps` | Recording/video clock rate | `30` |
+| `recording.min_episode_seconds` | Discard saved episodes shorter than this duration | `1.0` |
+| `recording.record_modes` | Modes that allow recording start and frame writes | `[standing, mocap, arms, pause]` |
+| `recording.camera.key` | RGB image dataset key | `observation.images.d435i_rgb` |
+| `recording.camera.width` / `height` / `fps` | RealSense RGB capture settings | `640` / `480` / `30` |
+| `recording.camera.device` | Optional RealSense serial | `null` |
+| `recording.video.codec` / `quality` / `pixelformat` | MP4 sidecar encoder settings | `libx264` / `8` / `yuv420p` |
+
+RealSense frame timeouts and disconnects rebuild the capture pipeline in the
+background and never stop Pico input or G1 control. Recording requires a fresh
+camera frame before accepting `R`. An active episode is discarded after one
+second without a fresh frame, and recording remains idle after the camera
+recovers until the operator presses `R` again. If the entire `pico_input`
+worker exits, `robot_control` remains active and holds the latest command; the
+Unitree remote remains available for returning to `STANDING` or requesting
+`DAMPING`.
+
+The recorder creates an editable source dataset:
+
+```text
+recording.output_dir/
+├── schema.json
+├── episodes.jsonl
+├── data/
+│   └── episode_000000.h5
+└── videos/
+    └── d435i_rgb/
+        └── episode_000000.mp4
+```
+
+`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
+state and 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
+with the same schema resumes at the next episode index and may use a different
+`recording.task`.
+
+The format is intentionally not compatible with the earlier attribute-based
+HDF5 layout. Use an empty `recording.output_dir`; when an existing schema does
+not match, the recording worker rejects the dataset and exits without writing
+episodes. Recording is non-critical, so the main sim2real control runtime
+continues and reports the worker failure. An episode interrupted before its
+`episodes.jsonl` entry is committed is discarded on the next recording-worker
+startup and does not consume an episode index.
+
+HDF5 datasets:
+
+```text
+frame_index                    int64[N]
+timestamp                      float64[N]
+observation.state              float32[N, 68]
+observation.state.hand         float32[N, 12]  # only when hands are enabled
+observation.state.neck         float32[N, 2]   # only when OpenNeck is enabled
+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
+attributes. RGB frames remain in MP4 and are associated through
+`episodes.jsonl`; raw RGB HDF5 datasets are not written.
+
+`observation.state` is ordered as `joint_pos(29)`, `joint_vel(29)`,
+`base_quat_wxyz(4)`, `base_ang_vel(3)`, and `projected_gravity(3)`.
+`observation.state.hand` is the latest LinkerHand hardware readback:
+`left_state(6) + right_state(6)`, using the SDK's 0-255 joint values.
+`observation.state.neck` is the latest OpenNeck servo position returned by
+`read_deg()`: `[yaw_deg, pitch_deg]` in degrees.
+`observation.mode` is a numeric categorical: `standing=0`, `mocap=1`,
+`arms=2`, and `pause=3`. `action` is the current reference qpos:
+`root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`. It is the
+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 mechanically clamped target returned by OpenNeck
+after a successful command: `[yaw_deg, pitch_deg]` in degrees. Positive yaw
+turns left and positive pitch looks up. The reachable range comes from the
+OpenNeck calibration file and is therefore not fixed in the recording schema.
+
+## Critical: `default_dof_pos`
+
+The RL policy outputs action **offsets** relative to the default standing pose, not absolute joint angles:
+
+```text
+target_dof_pos = clip(action, low, high) * action_scale + default_dof_pos
+```
+
+Therefore:
+- `default_dof_pos` must align with `robot.default_angles`
+- Missing this offset causes the robot to fall immediately
+
+`TeleopPipeline` automatically passes `robot.default_angles` to the controller's `default_dof_pos` during initialization. Understanding this chain is important when writing custom entry points or tests.
diff --git a/docs/docs/configuration/overview.md b/docs/docs/reference/configuration/overview.md
similarity index 93%
rename from docs/docs/configuration/overview.md
rename to docs/docs/reference/configuration/overview.md
index 6d967dbf..722e5345 100644
--- a/docs/docs/configuration/overview.md
+++ b/docs/docs/reference/configuration/overview.md
@@ -16,6 +16,7 @@ Runtime assembly is centralized in `teleopit/runtime/`. Scripts, `TeleopPipeline
 | `teleopit/configs/pico4_sim.yaml` | Pico 4 VR sim2sim |
 | `teleopit/configs/sim2real.yaml` | BVH sim2real on Unitree G1 |
 | `teleopit/configs/pico4_sim2real.yaml` | Pico 4 VR sim2real on Unitree G1 |
+| `teleopit/configs/high_level_policy_sim2real.yaml` | Independent host-policy sim2real on Unitree G1 |
 
 These compose sub-configs:
 
@@ -75,4 +76,4 @@ Teleopit does not silently fix misconfigurations:
 
 When you encounter a configuration error, look for **which two components have inconsistent definitions**.
 
-For the complete field reference, see [Config Reference](config-reference).
+For the complete field reference, see [Configuration Fields](fields).
diff --git a/docs/docs/reference/g1-bridge-sdk.md b/docs/docs/reference/g1-bridge-sdk.md
deleted file mode 100644
index 2337df3c..00000000
--- a/docs/docs/reference/g1-bridge-sdk.md
+++ /dev/null
@@ -1,59 +0,0 @@
----
-sidebar_position: 4
----
-
-# G1 Bridge SDK
-
-C++ DDS bridge library wrapping unitree_sdk2 with pybind11, providing near-zero latency (< 0.5 ms) access to Unitree G1's real-time communication interface.
-
-All DDS publish/subscribe runs on native C++ threads. The Python side only calls simple get/set methods.
-
-## Dependencies
-
-- CMake >= 3.10
-- GCC >= 9.4 (C++17 support)
-- pybind11 >= 2.6
-- Unitree SDK2 (bundled in `third_party/g1_bridge_sdk/thirdparty/unitree_sdk2/`, no manual install needed)
-- Cyclone DDS (unitree_sdk2 dependency)
-
-## Installation
-
-```bash
-bash scripts/setup/setup_g1_bridge.sh
-```
-
-The script clones `unitree_sdk2`, installs `pybind11`, and builds the C++ bridge automatically.
-
-## Python API
-
-```python
-import g1_bridge_sdk
-
-bridge = g1_bridge_sdk.G1Bridge(
-    network_interface="enp130s0",  # PC Ethernet interface connected to G1
-    publish_hz=200              # Command publish rate (default 200 Hz)
-)
-```
-
-For wired PC-to-G1 control, run `ifconfig` on the PC and use the interface name for the G1 cable connection. When running onboard on the robot computer, `eth0` is usually the correct interface.
-
-| Method | Description |
-|--------|-------------|
-| `wait_for_state(timeout_sec=5.0)` | Block until first LowState frame; returns False on timeout |
-| `get_state()` | Returns `(qpos[29], qvel[29], quat[4], ang_vel[3])` numpy arrays |
-| `get_state_counter()` | Returns cumulative LowState frame count |
-| `get_wireless_remote()` | Returns 40-byte wireless remote data |
-| `get_mode_machine()` | Returns current mode_machine value |
-| `set_target(target, kp, kd)` | Set target joint positions and PD gains (29 elements each) |
-| `lock_joints()` | Lock current joint positions |
-| `set_damping()` | Switch to damping mode (for emergency stop) |
-| `start_publish()` | Start command publish thread |
-| `stop_publish()` | Stop command publish thread |
-| `check_mode()` | Query current motion mode, returns `(code, name)` |
-| `select_mode(name)` | Switch motion mode (e.g., `"ai"`, `"normal"`) |
-| `release_mode()` | Release current mode, enter low-level control |
-
-## Usage
-
-- **Pico4 hardware teleoperation**: `scripts/run/run_sim2real.py`
-- **Standalone standing test**: `scripts/run/standalone_standing.py`
diff --git a/docs/docs/reference/assets.md b/docs/docs/reference/resources/assets.md
similarity index 59%
rename from docs/docs/reference/assets.md
rename to docs/docs/reference/resources/assets.md
index daf811a3..2a8315f3 100644
--- a/docs/docs/reference/assets.md
+++ b/docs/docs/reference/resources/assets.md
@@ -1,18 +1,45 @@
 ---
-sidebar_position: 2
+sidebar_position: 1
 ---
 
-# Asset Management
+# Assets
 
-Datasets, checkpoints, robot models, and demo media are not tracked in Git. They are distributed via ModelScope and HuggingFace. The canonical Unitree G1 model is downloaded to `assets/robots/unitree_g1/g1_29dof.xml`.
+Teleopit's Git repository contains code, not large robot meshes, policies or
+motion data. [Installation](../../getting-started/installation) shows the shortest
+download command for each user workflow; this page is the complete inventory
+and maintainer reference.
 
 ## What's Not in Git
 
 - `assets/robots/` - Canonical robot XML/meshes
 - `teleopit/retargeting/gmr/assets/` - GMR retargeting assets, IK configs, and non-canonical robot descriptions
-- `data/`, checkpoints, caches
+- `data/`, `ckpt/`, checkpoints, caches
 - Demo media (`assets/demo.gif`, `assets/demo.mp4`)
 
+## Asset Inventory
+
+| Group | Local result | Used for |
+|-------|--------------|----------|
+| `ckpt` | `ckpt/track_g1.{onnx,pt}`, `ckpt/track_g1_neck_o6.{onnx,pt}` | Ready-to-run inference models and matching PyTorch checkpoints |
+| `robots` | Robot XML variants and meshes under `assets/robots/` | Training, MuJoCo inference, GMR and dataset FK |
+| `gmr` | `teleopit/retargeting/gmr/assets/` | Retargeting models and IK configuration |
+| `bvh` | `data/sample_bvh/*.bvh` | Sample motions used by the installation check and simulation tutorial |
+| `data` | `data/datasets/<dataset>/shard_*.h5` | Minimal distributed motion datasets; precompute before training |
+
+The current G1 robot bundle includes:
+
+| Model XML | Setup |
+|-----------|-------|
+| `assets/robots/unitree_g1/g1_29dof.xml` | Base G1 model and the default |
+| `assets/robots/unitree_g1/g1_29dof_dex3.xml` | G1 with Dex3 hand geometry and inertial properties |
+| `assets/robots/unitree_g1/g1_29dof_neck_o6.xml` | G1 with neck active vision and O6 hand models |
+
+The default is not a model allowlist. Training can select another
+task-compatible XML with `--robot_xml`. XML files in the GMR asset directory
+belong to their retargeting configurations and are separate from the runtime
+robot bundle. Use the `track_g1` policy pair with the base model and the
+`track_g1_neck_o6` pair with the neck-and-O6 model.
+
 ## Repositories
 
 ### ModelScope (default download source)
@@ -29,17 +56,17 @@ Datasets, checkpoints, robot models, and demo media are not tracked in Git. They
 | `12e21/Teleopit-models` | model | Checkpoints, GMR retargeting assets, sample BVH |
 | `12e21/Teleopit-datasets` | dataset | Training/validation datasets |
 
-### Asset Group Mapping
+### Asset Group and Repository Mapping
 
 | Group | Repository | Remote Path |
 |-------|-----------|-------------|
-| `ckpt` | Teleopit-models | `checkpoints/track.onnx`, `checkpoints/track.pt` |
+| `ckpt` | Teleopit-models | `checkpoints/track_g1.{onnx,pt}`, `checkpoints/track_g1_neck_o6.{onnx,pt}` |
 | `robots` | Teleopit-models | `archives/robot_assets.tar.gz` |
 | `gmr` | Teleopit-models | `archives/gmr_assets.tar.gz` |
 | `bvh` | Teleopit-models | `archives/sample_bvh.tar.gz` |
 | `data` | Teleopit-datasets | `data/datasets/*/*.h5` (`lafan1`, `pico_record`, `seed`, `twist2`) |
 
-## Download
+## Download Behavior
 
 Use the project download script (defaults to ModelScope):
 
@@ -61,8 +88,10 @@ Local paths after download:
 
 | Remote | Local |
 |--------|-------|
-| `checkpoints/track.onnx` | `track.onnx` |
-| `checkpoints/track.pt` | `track.pt` |
+| `checkpoints/track_g1.onnx` | `ckpt/track_g1.onnx` |
+| `checkpoints/track_g1.pt` | `ckpt/track_g1.pt` |
+| `checkpoints/track_g1_neck_o6.onnx` | `ckpt/track_g1_neck_o6.onnx` |
+| `checkpoints/track_g1_neck_o6.pt` | `ckpt/track_g1_neck_o6.pt` |
 | `archives/robot_assets.tar.gz` | `assets/robots/` (extracted) |
 | `archives/gmr_assets.tar.gz` | `teleopit/retargeting/gmr/assets/` (extracted) |
 | `archives/sample_bvh.tar.gz` | `data/sample_bvh/` (extracted) |
@@ -84,7 +113,7 @@ Output goes to `data/modelscope_upload/`.
 ```bash
 # Model repo
 modelscope upload --repo-type model BingqianWu/Teleopit-models \
-    data/modelscope_upload/checkpoints checkpoints
+    data/modelscope_upload/checkpoints checkpoints --sync
 modelscope upload --repo-type model BingqianWu/Teleopit-models \
     data/modelscope_upload/archives archives
 
@@ -93,6 +122,11 @@ modelscope upload --repo-type dataset BingqianWu/Teleopit-datasets \
     data/modelscope_upload/data data
 ```
 
+The checkpoint upload intentionally uses `--sync`. Its deletion scope is the
+remote `checkpoints/` directory, so obsolete policy names are removed without
+touching `archives/`. Do not add `--sync` to the archive upload unless the local
+staging directory contains every remote archive that must be retained.
+
 ### Step 3: Tag Version
 
 Only the model repo supports tags (dataset repo does not).
diff --git a/docs/docs/reference/dataset.md b/docs/docs/reference/resources/motion-datasets.md
similarity index 94%
rename from docs/docs/reference/dataset.md
rename to docs/docs/reference/resources/motion-datasets.md
index 08e9178c..185f6085 100644
--- a/docs/docs/reference/dataset.md
+++ b/docs/docs/reference/resources/motion-datasets.md
@@ -1,8 +1,13 @@
 ---
-sidebar_position: 3
+sidebar_position: 2
 ---
 
-# Dataset
+# Motion Datasets
+
+Motion datasets provide reference motion for controller training. The
+distributable format and the precomputed training format are separate; training
+accepts only the latter. For synchronized robot, reference and camera
+recordings, see [Teleoperation Datasets](teleoperation-datasets).
 
 ## Download Pre-Built Dataset (Recommended)
 
diff --git a/docs/docs/reference/resources/teleoperation-datasets.md b/docs/docs/reference/resources/teleoperation-datasets.md
new file mode 100644
index 00000000..7fca39c5
--- /dev/null
+++ b/docs/docs/reference/resources/teleoperation-datasets.md
@@ -0,0 +1,104 @@
+---
+sidebar_position: 3
+---
+
+# Teleoperation Datasets
+
+Teleoperation datasets are manually recorded sim2real episodes. They synchronize
+the G1 state, the reference consumed by the motion tracker, optional hand and
+neck commands, and RealSense RGB video. This format is intended for review and
+external policy work; it is not a motion dataset for controller training.
+
+## Record Episodes
+
+Recording is available only for onboard Pico sim2real deployment with an
+interactive terminal and a fresh RealSense stream:
+
+```bash
+pip install -e '.[recording]'
+python scripts/run/run_sim2real.py --config-name sim2real_record \
+    controller.policy_path=policy.onnx
+```
+
+The equivalent manual configuration requires `recording.enabled=true`,
+`input.provider=pico4`, `input.video.enabled=true`, and
+`input.video.source=realsense`.
+
+Use `R` to start an episode, `S` to save it, `D` to discard it, and `Q` to shut
+down. `STANDING`, `MOCAP`, `ARMS`, and paused mocap are recordable. Recording
+does not start without a fresh camera frame. If the camera stays stale for one
+second during an episode, that episode is discarded; Pico input and G1 control
+continue, and recording does not restart automatically when video recovers.
+
+## Dataset Layout
+
+The recording runtime writes an editable dataset rather than one self-contained
+HDF5 file:
+
+```text
+data/recordings/sim2real_hdf5/
+├── schema.json
+├── episodes.jsonl
+├── data/
+│   └── episode_000000.h5
+└── videos/
+    └── d435i_rgb/
+        └── episode_000000.mp4
+```
+
+`schema.json` defines the dataset FPS, `robot_type`, `hand_type`, `neck_type`,
+and every feature's shape, dtype, names, and groups. Hardware types must match
+the active runtime configuration.
+
+`episodes.jsonl` is the editable episode manifest. Each line maps one episode
+to its HDF5 and MP4 files and stores the task prompt. Task text is not copied
+into HDF5 attributes, so it can be edited without rewriting frame data.
+
+## Frame Fields
+
+Each HDF5 file contains only frame-aligned arrays:
+
+| Field | Shape | Meaning |
+|-------|-------|---------|
+| `frame_index` | scalar | Camera/action frame index |
+| `timestamp` | scalar | Monotonic timestamp in seconds |
+| `observation.state` | `(68,)` | G1 joint state, base orientation/angular velocity, and projected gravity |
+| `observation.state.hand` | `(12,)`, optional | Left/right LinkerHand hardware joint readback |
+| `observation.state.neck` | `(2,)`, optional | OpenNeck servo yaw/pitch readback in degrees |
+| `observation.mode` | scalar | `STANDING`, `MOCAP`, `ARMS`, or paused mocap code |
+| `action` | `(36,)` | Root pose plus 29-joint reference consumed by the motion tracker |
+| `action.hand` | `(12,)`, optional | Left/right LinkerHand target when hand control is enabled |
+| `action.neck` | `(2,)`, optional | Mechanically clamped OpenNeck yaw/pitch target in degrees |
+
+`observation.state` is ordered as `joint_pos(29)`, `joint_vel(29)`,
+`base_quat_wxyz(4)`, `base_ang_vel(3)`, and `projected_gravity(3)`.
+`observation.state.hand` uses the LinkerHand SDK's 0-255 joint values, ordered
+as six left-hand channels followed by six right-hand channels.
+`observation.state.neck` is `[yaw_deg, pitch_deg]` returned by OpenNeck
+`read_deg()`.
+`observation.mode` uses `standing=0`, `mocap=1`, `arms=2`, and `pause=3`.
+`action` is `root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`.
+
+Camera RGB is stored only in the MP4 sidecar; HDF5 does not duplicate raw image
+frames. Optional state and action fields appear exactly when the corresponding
+hardware is enabled.
+
+## Commit and Recovery Rules
+
+The recorder commits HDF5 and video files before appending the manifest entry.
+An interrupted, uncommitted episode is removed on the next recording-worker
+startup and does not consume an episode index. An incompatible existing
+`schema.json` stops only the non-critical recording worker, while G1 control
+continues.
+
+## Review Recordings
+
+```bash
+python scripts/view/view_recording.py \
+    --recording data/recordings/sim2real_hdf5
+```
+
+The reviewer validates manifest paths, HDF5 shapes, dtypes, finite values, and
+MP4 alignment before playback. Measured root XYZ is not recorded, so the
+observed robot is anchored to the reference root position; global root
+translation cannot be evaluated from this format.
diff --git a/docs/docs/reference/training-troubleshooting.md b/docs/docs/reference/training-troubleshooting.md
deleted file mode 100644
index b683c6c4..00000000
--- a/docs/docs/reference/training-troubleshooting.md
+++ /dev/null
@@ -1,173 +0,0 @@
----
-sidebar_position: 5
----
-
-# Training Troubleshooting
-
-Common training issues and solutions.
-
-:::info
-For training workflow, see [Training Tutorial](../tutorials/training). For data preparation, see [Dataset Reference](dataset).
-:::
-
----
-
-## Issue 1: Mean Episode Length = 1.00 (Robot Terminates on First Step)
-
-### Symptoms
-
-- `Mean episode length: 1.00`
-- `Episode_Termination/anchor_pos` near total parallel environment count
-- `Metrics/motion/error_anchor_pos` > 0.5 m
-- `Metrics/motion/error_body_rot` very large (close to pi)
-
-### Root Cause
-
-Usually not a PPO hyperparameter issue, but **motion NPZ labels inconsistent with MuJoCo FK**:
-
-1. **Body position coordinate error**: Local coordinates used as world coordinates
-2. **Body order error**: Using PKL's 38-body order instead of mjlab G1's 30-body order
-3. **Body orientation/angular velocity error**: All bodies approximated to root orientation
-
-The current `convert_pkl_to_npz.py` fixes these issues.
-
-### Quick Diagnosis
-
-```bash
-python train_mimic/scripts/data/check_motion_npz_fk.py \
-    --npz data/lafan1_clips/lafan1/<clip>.npz
-```
-
-Expected thresholds: `pos_max < 1e-3 m`, `quat_mean < 0.05 rad`, `quat_p95 < 0.10 rad`.
-
-If check fails, regenerate data and run a smoke test:
-
-```bash
-python train_mimic/scripts/train.py \
-    --num_envs 64 --max_iterations 100 \
-    --motion_file data/datasets/<dataset>_precomputed
-```
-
-Expected: `Mean episode length` significantly > 1, `error_anchor_pos` starts decreasing.
-
----
-
-## Issue 2: Episode Length Not Growing
-
-### Symptoms
-
-After 1000+ iterations, `Mean episode length` stays low (< 3) with no upward trend.
-
-### Possible Causes
-
-1. Poor retargeting quality (unreachable target poses)
-2. Tracking reward weight too low vs regularization
-3. Learning rate too high/low, clip_param mismatch
-4. Termination thresholds too strict
-
-### Diagnosis Steps
-
-1. Visualize reference motion with `play.py`
-2. Check reward distribution - tracking reward should dominate
-3. Temporarily increase `bad_anchor_pos` threshold (0.25m -> 0.5m)
-4. Compare with mjlab's built-in G1 tracking task
-
----
-
-## Issue 3: Slow Training
-
-### Symptoms
-
-Training speed < 1000 steps/s (expected 1500-2000 on RTX 4090).
-
-### Solutions
-
-1. Increase `--num_envs` to 4096 (needs 24 GB VRAM)
-2. Disable `--video` during training
-3. Use TensorBoard instead of W&B (default)
-
----
-
-## Issue 4: `nefc overflow - please increase njmax`
-
-### Symptoms
-
-```text
-nefc overflow - please increase njmax to 257
-```
-
-### Root Cause
-
-MuJoCo constraint buffer insufficient. When the robot falls or has many contacts, active constraints exceed `njmax`. The `mjlab` training default is `sim.njmax=250`.
-
-### Solution
-
-Already fixed in the repository. The env builder in `train_mimic/tasks/tracking/config/env.py` overrides:
-
-```python
-self.sim.njmax = 500
-self.sim.nconmax = 150_000
-```
-
-If warnings persist at higher values, increase to `njmax = 800`.
-
-:::note
-Only modifying the robot XML is insufficient - the simulation-level `njmax` in mjlab takes precedence.
-:::
-
----
-
-## Issue 5: Benchmark Video Problems
-
-### Video has only 1 frame
-
-Ensure `num_eval_steps >= video_length`:
-
-```bash
-python train_mimic/scripts/benchmark.py \
-    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
-    --motion_file data/datasets/<dataset>_precomputed \
-    --num_envs 1 --num_eval_steps 2000 \
-    --video --video_length 600
-```
-
-### EGL/OpenGL errors
-
-Install OpenGL/EGL dependencies:
-
-```bash
-conda install -c conda-forge libopengl libglx libegl libglvnd pyopengl
-```
-
-If GPU EGL is unavailable, try CPU rendering:
-
-```bash
-MUJOCO_GL=osmesa PYOPENGL_PLATFORM=osmesa \
-    python train_mimic/scripts/benchmark.py ... --video
-```
-
----
-
-## Issue 6: Foot Sliding in Sim2Sim (Benchmark OK but ONNX Inference Slides)
-
-### Root Cause
-
-Sim2sim configuration parameters mismatch with training environment:
-
-1. **`default_angles` mismatch (critical)**: Different joint defaults cause action offset and observation errors
-2. **Missing joint armature**: Training environment has non-zero armature; zero armature causes overshoot
-3. **condim mismatch**: Different collision parameters between training and sim2sim
-
-### Diagnosis
-
-```python
-from mjlab.asset_zoo.robots import get_g1_robot_cfg
-cfg = get_g1_robot_cfg()
-print(cfg.init_state.joint_pos)  # Must match g1.yaml default_angles
-```
-
-### Solution
-
-Update `teleopit/configs/robot/g1.yaml` and `assets/robots/unitree_g1/g1_29dof.xml` to match training environment values (default angles, armature, condim).
-
-This fix also affects the sim2real path since `default_angles` is shared by `rl_policy.py` and `observation.py`.
diff --git a/docs/docs/tutorials/bvh-sim2real.md b/docs/docs/tutorials/bvh-sim2real.md
index 75c9a83b..55548494 100644
--- a/docs/docs/tutorials/bvh-sim2real.md
+++ b/docs/docs/tutorials/bvh-sim2real.md
@@ -36,7 +36,7 @@ For onboard deployment, `eth0` is usually correct.
 
 ```bash
 python scripts/run/run_sim2real.py \
-    controller.policy_path=track.onnx \
+    controller.policy_path=ckpt/track_g1.onnx \
     real_robot.network_interface=enp130s0 \
     input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
 ```
diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md
new file mode 100644
index 00000000..e7b03b41
--- /dev/null
+++ b/docs/docs/tutorials/high-level-policy-sim2real.md
@@ -0,0 +1,234 @@
+---
+sidebar_position: 4
+---
+
+# From Teleoperation Data to Imitation Learning / VLA Deployment
+
+This guide connects the complete workflow: record Pico demonstrations with
+Teleopit, train an ACT or GR00T N1.7 policy in `lerobot-teleopit`, and run the
+result on a physical Unitree G1.
+
+```text
+Pico demonstration
+  -> Teleopit v4 recording
+  -> LeRobot Dataset
+  -> ACT or GR00T checkpoint
+  -> host policy server
+  -> Teleopit onboard motion tracker
+  -> G1 + LinkerHand O6 + OpenNeck
+```
+
+Teleopit owns recording and real-time robot control.
+[`lerobot-teleopit`](https://github.com/BotRunner64/lerobot-teleopit) owns
+dataset conversion, model training and the host policy server. Keep their
+Python environments separate; the host sends reference motion, not G1 motor
+commands.
+
+## Before You Start
+
+- [VR Teleoperation on Unitree G1](pico-sim2real) works reliably.
+- The onboard setup has two LinkerHand O6 hands, OpenNeck and a RealSense RGB
+  camera. The current training and deployment path requires all of them.
+- The onboard computer is prepared through
+  [Installation](../getting-started/installation) with recording, OpenNeck,
+  LinkerHand and somehand support, plus `ckpt/track_g1_neck_o6.onnx`.
+- The [Standalone Standing Test](standalone-standing) is stable with the same
+  G1 network interface and low-level tracking policy.
+- The host workstation is prepared separately through the
+  [`lerobot-teleopit` installation guide](https://github.com/BotRunner64/lerobot-teleopit#installation).
+
+:::danger Keep the Unitree remote in your hand
+Use `L1+R1` to enter `DAMPING` whenever motion is unexpected. Keep clear space
+around the robot, have another person ready to support or stop it, and never
+run two programs that can command the G1 at the same time.
+:::
+
+## 1. Record and Review Demonstrations
+
+Run the recording configuration on the G1 onboard computer. This example uses
+Pico hand-pose retargeting; use `hands.mode=gripper` when demonstrations should
+use the controller triggers instead.
+
+```bash
+python scripts/run/run_sim2real.py \
+  --config-name sim2real_record \
+  controller.policy_path=ckpt/track_g1_neck_o6.onnx \
+  real_robot.network_interface=eth0 \
+  hands.enabled=true \
+  hands.driver=linkerhand_o6 \
+  hands.mode=vr_hand_pose \
+  neck.enabled=true \
+  recording.output_dir=data/recordings/my_task \
+  recording.task="pick up the object"
+```
+
+Use the G1 remote to enter `MOCAP` or `ARMS`, then use the recording terminal:
+
+| Key | Action |
+|-----|--------|
+| `R` | Start an episode after a fresh RealSense frame is available |
+| `S` | Save the active episode |
+| `D` | Discard the active episode |
+| `Q` | Shut down the runtime |
+
+Record one task per dataset and keep `recording.task` consistent. Save only
+successful demonstrations, while varying useful factors such as starting pose,
+object position and execution speed.
+
+Review the synchronized video, measured state and reference before training:
+
+```bash
+python scripts/view/view_recording.py \
+  --recording data/recordings/my_task
+```
+
+Discard episodes with tracking loss, camera interruption or unsafe references.
+For the recording schema and recovery rules, see
+[Teleoperation Datasets](../reference/resources/teleoperation-datasets).
+
+## 2. Hand the Dataset to `lerobot-teleopit`
+
+Copy the complete recording directory to the host without flattening or
+renaming its contents. A typical source directory is:
+
+```text
+lerobot-teleopit/data/raw/my_task/
+├── schema.json
+├── episodes.jsonl
+├── data/
+└── videos/d435i_rgb/
+```
+
+The current converter requires a Teleopit v4 dataset with LinkerHand O6 and
+OpenNeck state/action fields. Missing fields are rejected rather than padded.
+If the same task was recorded in several directories, use the host repository's
+`merge_raw_datasets.py` tool before conversion.
+
+Run all remaining host commands inside the independent `lerobot-teleopit`
+environment. Its
+[Dataset Conversion and Training guide](https://github.com/BotRunner64/lerobot-teleopit/blob/main/docs/training-entrypoint.md)
+covers dependencies, merge options, training scales, multi-GPU settings and
+logging.
+
+## 3. Convert and Train on the Host
+
+The shortest conversion command is:
+
+```bash
+python scripts/convert_dataset.py \
+  --source data/raw/my_task \
+  --output data/lerobot/my_task \
+  --repo-id local/my_task \
+  --workers 4
+```
+
+Choose one training command. For ACT:
+
+```bash
+python scripts/train_policy.py \
+  --policy act \
+  --dataset-root data/lerobot/my_task \
+  --devices 0
+```
+
+For GR00T N1.7:
+
+```bash
+python scripts/train_policy.py \
+  --policy groot \
+  --dataset-root data/lerobot/my_task \
+  --devices 0,1,2,3
+```
+
+Append `--dry-run` to verify the resolved launch without starting training.
+Unless `--output-dir` is set, runs are created under `outputs/train/`. The
+deployable artifact is the run's `checkpoints/last/pretrained_model/`
+directory.
+
+## 4. Validate the Robot Path with ReplayPolicy
+
+Before loading a learned checkpoint, replay a recorded episode from the host:
+
+```bash
+python scripts/run_policy_server.py \
+  --backend replay \
+  --dataset-root data/lerobot/my_task \
+  --repo-id local/my_task \
+  --episode 0 \
+  --start-frame 0 \
+  --chunk-size 15 \
+  --bind tcp://0.0.0.0:5555
+```
+
+Bind to `0.0.0.0` only on the trusted robot network. The service has no
+authentication and must not be exposed to the public internet.
+
+On the G1 onboard computer, start Teleopit's dedicated runtime. Replace
+`HOST_IP` with the workstation address and use the same task wording as the
+dataset:
+
+```bash
+python scripts/run/run_high_level_policy_sim2real.py \
+  controller.policy_path=ckpt/track_g1_neck_o6.onnx \
+  high_level_policy.endpoint=tcp://HOST_IP:5555 \
+  high_level_policy.task="pick up the object" \
+  real_robot.network_interface=eth0
+```
+
+Starting the process leaves the robot in `IDLE`. Use the Unitree remote:
+
+| Control | Action |
+|---------|--------|
+| `Start` | Enter `STANDING` |
+| `Y` | Start a policy session; the first valid chunk enters `POLICY` |
+| `B` | Pause or resume after a fresh chunk is available |
+| `X` | End the session and return to `STANDING` |
+| `L1+R1` | Immediately enter `DAMPING` |
+
+ReplayPolicy should reproduce the recorded reference closely enough to verify
+the network, action convention and onboard execution path. Stop here if it
+does not. A learned policy cannot fix a recording, conversion, coordinate or
+low-level tracking problem.
+
+## 5. Deploy the Trained Policy
+
+Press `X` to return the G1 to `STANDING`, then stop ReplayPolicy. On the host,
+start the learned-policy server with the `pretrained_model` directory itself:
+
+```bash
+python scripts/run_policy_server.py \
+  --backend lerobot \
+  --checkpoint outputs/train/<run>/checkpoints/last/pretrained_model \
+  --device cuda \
+  --bind tcp://0.0.0.0:5555
+```
+
+ACT and GR00T use the same server command. Press `Y` on the Unitree remote to
+create a new policy session. Begin with a familiar scene from the training
+distribution and small, recoverable motions.
+
+To record the observations and actions exchanged during a run, add this host
+option:
+
+```bash
+--record-dir outputs/policy-recordings
+```
+
+Teleopit validates and rate-limits each returned plan before the 50 Hz motion
+tracker consumes it. Malformed output is rejected rather than padded or
+trimmed. A host, network, camera or action-watchdog fault pauses the session
+and holds the latest commands; it does not automatically enter `STANDING`.
+Restore the failed path and press `B` to resume, or use `X` or `L1+R1` as
+appropriate.
+
+## Common Problems
+
+| Symptom | What to check |
+|---------|---------------|
+| Pressing `Y` never enters `POLICY` | Host IP and firewall, server logs, a fresh RealSense frame, matching code versions and identical `hand_calibration.json` files |
+| `POLICY` becomes paused | Host inference latency, request timeout, stale camera/result, action watchdog or a required worker exit |
+
+For model action coordinates and host-side behavior, see the
+[`lerobot-teleopit` Action Space guide](https://github.com/BotRunner64/lerobot-teleopit/blob/main/docs/planar-relative-root-actions.md).
+For onboard timing and safety settings, see
+[Configuration Fields](../reference/configuration/fields#host-high-level-policy-independent-sim2real).
diff --git a/docs/docs/tutorials/offline-sim2sim.md b/docs/docs/tutorials/offline-sim2sim.md
index 57455374..99cf69b0 100644
--- a/docs/docs/tutorials/offline-sim2sim.md
+++ b/docs/docs/tutorials/offline-sim2sim.md
@@ -2,89 +2,130 @@
 sidebar_position: 1
 ---
 
-# Offline Sim2Sim
+# Run a Motion Controller in Simulation
 
-Run BVH motion capture files through the RL policy in MuJoCo simulation.
+Use this guide to watch a trained controller reproduce a motion in MuJoCo. This
+is the quickest way to answer two basic questions before adding VR or a real
+robot:
 
-## Basic Playback
+- Does the policy load and keep the G1 stable?
+- Does the retargeted motion look like the source motion?
+
+## Before You Start
+
+Complete [Installation](../getting-started/installation) with the base profile
+and the `robots gmr ckpt bvh` asset bundle.
+
+## 1. Run the Sample Motion
 
 ```bash
 python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
-    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
+    playback.keyboard.enabled=true
 ```
 
-### Using hc_mocap Format
+The `sim2sim` window is the result that matters: it shows the G1 produced by
+physics and the policy, not just a kinematic target.
+
+| Key | Action |
+|-----|--------|
+| `Space` or `P` | Pause or resume |
+| `R` | Replay from the first frame |
+| `Q` | Stop |
+
+The run is healthy when the robot remains stable and follows the overall timing
+and pose of the clip. Small tracking error is normal; falling, frozen joints or
+a clearly wrong facing direction is not.
+
+## 2. Compare the Three Views
+
+Open all views when you need to find where a bad result starts:
 
 ```bash
 python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
-    input.bvh_file=data/hc_mocap/walk.bvh \
-    input.bvh_format=hc_mocap
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
+    viewers=all
 ```
 
-## Keyboard Playback
+| View | What you are looking at |
+|------|-------------------------|
+| `mocap` | The human skeleton read from the BVH file |
+| `retarget` | The kinematic G1 pose produced by GMR |
+| `sim2sim` | The G1 after policy inference and MuJoCo physics |
+
+If `mocap` is wrong, check the BVH format. If `mocap` looks right but
+`retarget` does not, inspect the retargeting setup. If only `sim2sim` is wrong,
+check the policy and observation configuration.
 
-Enable interactive control for offline BVH playback:
+You can also select views explicitly:
 
 ```bash
+# Only the physics result
 python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
+    controller.policy_path=ckpt/track_g1.onnx \
     input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
-    playback.keyboard.enabled=true
-```
+    viewers=sim2sim
 
-| Key | Action |
-|-----|--------|
-| `Space` / `P` | Pause / Resume |
-| `R` | Replay from start |
-| `Q` | Stop |
+# No windows; useful for a server or timing test
+python scripts/run/run_sim.py \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
+    viewers=none
+```
 
-Additional options:
+Closing every active viewer ends the simulation.
 
-```bash
-# Pause at end of motion
-playback.pause_on_end=true
+## 3. Try Your Own BVH
 
-# Limit number of steps (0 = infinite)
-num_steps=300
+For a LAFAN1-style file:
 
-# Wall-clock rate limiting (even without viewer)
-realtime=true
+```bash
+python scripts/run/run_sim.py \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=/path/to/motion.bvh \
+    input.bvh_format=lafan1
 ```
 
-## Viewer Modes
-
-Viewers run in separate subprocesses. Use shell quotes for list overrides.
+For an `hc_mocap` file:
 
 ```bash
-viewers=sim2sim          # Default
-viewers=all              # mocap + retarget + sim2sim
-viewers=none             # Headless
-'viewers=[retarget,sim2sim]'  # Specific combination
+python scripts/run/run_sim.py \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=/path/to/motion.bvh \
+    input.bvh_format=hc_mocap
 ```
 
-:::note
-When all active viewer windows are closed, the simulation ends automatically.
-:::
+Teleopit does not guess an unknown skeleton layout. A file can be valid BVH and
+still need an adapter before it matches a supported format.
 
-## Offline Rendering
+## 4. Save a Video
 
-Render simulation to video (headless):
+Use the renderer when you want repeatable output instead of interactive
+windows:
 
 ```bash
 MUJOCO_GL=egl python scripts/render/render_sim.py \
     --bvh data/sample_bvh/aiming1_subject1.bvh \
-    --policy track.onnx
+    --policy ckpt/track_g1.onnx
 ```
 
-For hc_mocap format:
+Add `--format hc_mocap` for that input format. The renderer writes synchronized
+`mocap`, `retarget` and `sim2sim` videos.
+
+## Useful Playback Options
 
 ```bash
-MUJOCO_GL=egl python scripts/render/render_sim.py \
-    --bvh data/hc_mocap/wander.bvh \
-    --format hc_mocap \
-    --policy track.onnx
+# Hold the final pose instead of exiting
+playback.pause_on_end=true
+
+# Stop after 300 simulation steps; 0 means no step limit
+num_steps=300
+
+# Keep wall-clock timing even with no viewer
+realtime=true
 ```
 
-The render pipeline outputs three views (mocap input, retarget, sim2sim), all using MuJoCo rendering.
+For every available field, see
+[Configuration](../reference/configuration/overview).
diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md
index ff9395bc..e8a9feb0 100644
--- a/docs/docs/tutorials/pico-sim2real.md
+++ b/docs/docs/tutorials/pico-sim2real.md
@@ -1,316 +1,244 @@
 ---
-sidebar_position: 4
+sidebar_position: 3
 ---
 
-# Pico 4 VR Teleoperation on Unitree G1
+# VR Teleoperation on Unitree G1
 
-Use this tutorial after [Pico Sim2Sim](pico-sim2sim) is working. It deploys the
-same realtime Pico input path to a physical Unitree G1.
+This guide moves the Pico workflow from MuJoCo to a physical Unitree G1. First
+choose where Teleopit will run, then verify standing control before handing the
+robot over to live body tracking.
 
-```text
-Pico headset -> Teleopit host -> retarget -> RL policy -> g1_bridge_sdk -> G1
-```
-
-There are two deployment styles:
-
-| Deployment | Where Teleopit Runs | Main Difference |
-|------------|---------------------|-----------------|
-| Wired PC-to-G1 | External workstation or laptop | Set `real_robot.network_interface` to the PC Ethernet interface connected to G1 |
-| Onboard | G1 onboard computer | Install Teleopit on the onboard computer; `eth0` is usually correct |
+:::danger Keep the Unitree remote in your hand
+Use `L1+R1` to enter `DAMPING` whenever motion is unexpected. Keep clear space
+around the robot and have another person ready to support or stop it.
+:::
 
-Both styles use `Pico4InputProvider` and the in-process pico-bridge receiver.
-There is no separate onboard Pico input mode.
+## Choose a Deployment
 
-Teleopit targets pico-bridge 0.2.1 and its `pico_native` tracking semantics.
+### External host: whole-body tracking only
 
-## 1. Install Runtime Dependencies
+Run Teleopit on a workstation or laptop connected to G1 by Ethernet. The Pico
+headset must be able to reach this computer over the network.
 
-Install Pico and sim2real dependencies on the machine that will run Teleopit:
+This deployment is for G1 whole-body control only. Keep LinkerHand, OpenNeck,
+RealSense preview and recording disabled. Find the wired interface connected
+to G1:
 
 ```bash
-pip install -e '.[pico4]'
-git submodule update --init --recursive
-bash scripts/setup/setup_g1_bridge.sh
+ifconfig
 ```
 
-Verify Pico receiver import:
+Use that interface name in the commands below. The examples use `enp130s0`.
 
-```bash
-python -c "from pico_bridge import PicoBridge; print('OK')"
-```
+### Onboard computer: full embodiment
 
-## 2. Choose The Network Interface
+Run Teleopit directly on the G1 onboard computer when you also need LinkerHand,
+OpenNeck, RealSense preview or data collection. The Pico headset must be able to
+reach the onboard computer.
 
-`real_robot.network_interface` is the Linux interface used for Unitree DDS
-communication.
+When the onboard setup includes both O6 hands and OpenNeck, set the low-level
+tracking policy to
+`controller.policy_path=ckpt/track_g1_neck_o6.onnx`.
 
-For wired PC-to-G1 deployment:
+The G1 DDS interface is `eth0` by default. Apart from the network interface and
+the optional onboard hardware settings, the body-control configuration and
+launch command are the same as for an external host.
 
-1. Connect the PC to the G1 by Ethernet.
-2. Run `ifconfig` on the PC.
-3. Use the Ethernet interface connected to the robot, for example `enp130s0`.
-4. Keep the Pico headset on a network that can reach the PC running Teleopit.
+## Before You Start
 
-For onboard deployment:
+Do not continue until all of these are true:
 
-1. Run Teleopit on the robot onboard computer.
-2. Keep the Pico headset on a network that can reach the onboard computer.
-3. Use `real_robot.network_interface=eth0` unless your robot network differs.
-4. Set `input.bridge_advertise_ip=<host-ip>` if Pico discovery advertises the
-   wrong address.
+- [VR Teleoperation in Simulation](pico-sim2sim) works reliably.
+- You installed the `pico4` profile and built `g1_bridge_sdk` as described in
+  [Installation](../getting-started/installation).
+- `ckpt/track_g1.onnx`, the robot files and GMR assets are present.
+- The machine running Teleopit has a wired DDS connection to G1.
+- No other program is commanding the robot.
 
-### Onboard RealSense On Arm
+## 1. Check Standing Control
 
-The pico-bridge PC receiver supports Arm machines when the required Python
-dependencies are available. On Arm onboard computers that need RealSense preview,
-install `pyrealsense2` from conda-forge in the active Conda environment instead
-of relying on the pip package:
+Check state reception and policy timing without sending motor commands. On an
+external host, replace `enp130s0` with the interface reported by `ifconfig`:
 
 ```bash
-pip uninstall pyrealsense2
-conda install -c conda-forge pyrealsense2
+python scripts/run/standalone_standing.py \
+    --policy ckpt/track_g1.onnx \
+    --network-interface enp130s0 \
+    --dry-run
 ```
 
-This only matters when using the optional RealSense preview path
-(`input.video.enabled=true`). Pico tracking and robot control do not require
-RealSense.
-
-## 3. Run The Controller
+On the onboard computer, use `--network-interface eth0`.
 
-Wired PC example:
+If the dry run succeeds, repeat the command without `--dry-run` in a safe
+hardware setup:
 
 ```bash
-python scripts/run/run_sim2real.py \
-    --config-name pico4_sim2real \
-    controller.policy_path=track.onnx \
-    real_robot.network_interface=enp130s0
+python scripts/run/standalone_standing.py \
+    --policy ckpt/track_g1.onnx \
+    --network-interface enp130s0
 ```
 
-Onboard example:
+Stop here if standing control is not stable. Follow the
+[Standalone Standing Test](standalone-standing) before adding Pico input.
 
-```bash
-python scripts/run/run_sim2real.py \
-    --config-name pico4_sim2real \
-    controller.policy_path=track.onnx \
-    real_robot.network_interface=eth0
-```
-
-## Optional HDF5 Recording
+## 2. Start Pico Sim2Real
 
-Install the recording extra on the machine that owns Pico input and RealSense:
+External-host example:
 
 ```bash
-pip install -e '.[recording]'
+python scripts/run/run_sim2real.py \
+    --config-name pico4_sim2real \
+    controller.policy_path=ckpt/track_g1.onnx \
+    real_robot.network_interface=enp130s0
 ```
 
-Run the recording config:
+Onboard-computer example:
 
 ```bash
 python scripts/run/run_sim2real.py \
-    --config-name sim2real_record \
-    controller.policy_path=track.onnx \
-    real_robot.network_interface=enp130s0 \
-    recording.task="walk forward"
+    --config-name pico4_sim2real \
+    controller.policy_path=ckpt/track_g1.onnx \
+    real_robot.network_interface=eth0
 ```
 
-Terminal controls are `R` start episode, `S` save, `D` discard, and `Q`
-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/episodes/`, with compressed MP4 sidecar
-videos under `data/recordings/sim2real_hdf5/videos/`. The HDF5 episode stores
-`frame_index` and `timestamp` sync arrays plus `observation.state(68)`,
-`observation.mode(1)`, `action(36)`, and `action.hand(12)` at 30 Hz.
+Starting the program does not immediately give Pico control of the robot.
 
-## Operator Flow
+## 3. Use the G1 State Machine
 
-Keep the Unitree remote in hand. `L1+R1` is the emergency stop path into
-`DAMPING`.
+![Pico G1 control state machine](/img/diagrams/pico-g1-state-machine.svg)
 
-| Control | Action |
-|---------|--------|
-| Unitree remote `Start` | Enter `STANDING` |
-| Unitree remote `Y` | Enter `MOCAP` |
-| Pico/controller `A` | Pause / resume live mocap |
-| Pico/controller `B` | Toggle `MOCAP` / `ARMS` |
-| Unitree remote `X` | Return to `STANDING` |
-| Unitree remote `L1+R1` | Emergency stop (`DAMPING`) |
+Labels beginning with **G1 remote** refer to the Unitree remote. Labels
+beginning with **Pico controller** refer to the VR controllers. The computer
+keyboard does not switch robot modes.
 
-Enter `MOCAP` only after Pico tracking is stable. Teleopit validates consecutive
-mocap frames before switching; if validation fails, the robot stays in
-`STANDING`.
-
-## Runtime Behavior
-
-Pico sim2real uses the shared realtime reference timeline:
-
-```text
-Pico body frames -> retarget -> reference buffer -> observation -> policy -> G1 joints
-```
+Press **G1 remote** `Start` to enter `STANDING`. Wait until the robot is stable,
+stand in a neutral pose and make sure Pico tracking is valid. Then press
+**G1 remote** `Y` to enter `MOCAP`, and begin with small, slow movements. Press
+**G1 remote** `X` when you want to end the VR session and return to `STANDING`.
 
-When entering `STANDING`, Teleopit releases active Unitree modes, enters
-debug/low-level control, locks the current joints briefly, resets policy state,
-and ramps Kp without changing policy targets.
+`MOCAP` follows the whole body. `ARMS` keeps the body, waist and legs in the
+standing pose while both arms continue to follow. `PAUSED` holds the current
+reference; resuming returns to the previous `MOCAP` or `ARMS` state.
 
-When entering `MOCAP`, Teleopit rearms the process-isolated reference worker,
-resets its GMR state and realtime reference buffer, then waits for fresh
-validated references before tracking the live mocap command. `STANDING` and
-`DAMPING` keep the reference worker disarmed so cold startup frames cannot
-warm-start retargeting before mocap entry.
+Teleopit checks several consecutive Pico frames before entering `MOCAP`. If
+that check fails, the robot stays in `STANDING`.
 
-`ARMS` keeps the same live retargeting timeline running, but sends the motion
-tracker a composed reference: body, waist, and legs stay at the standing pose
-while both arms follow the live retargeted result. Entering or leaving `ARMS`
-resets policy/reference alignment and uses the same Kp ramp safety path.
-
-## Pause / Resume
-
-Pico pause/resume is a mocap-session control event.
-
-- `ACTIVE`: the pause button freezes the current reference pose.
-- `PAUSED`: pressing it again clears policy/reference state, warms the realtime
-  buffer, re-centers yaw/XY alignment, and resumes from live mocap.
-
-:::warning
-Resume while standing still and close to the paused pose. This reduces sudden
-reference changes when live tracking resumes.
+:::tip Pause and resume
+G1 remote `B` or Pico controller `A` pauses and resumes the current session.
+Resume while standing still and close to the held pose. Use G1 remote `X`
+instead when you want to end the session.
 :::
 
-## Optional LinkerHand Control
-
-Pico sim2real can drive LinkerHand hands from Pico input:
+If Pico input stops, body control holds the last reference and the G1 remote
+remains available. Use `X` to return to `STANDING`, or `L1+R1` to enter
+`DAMPING`; do not wait for an automatic mode change.
 
-- `gripper`: hold the matching side grip as a deadman switch; the matching
-  trigger closes that hand. This mode supports `hands.driver=linkerhand_l6` and
-  `hands.driver=linkerhand_o6`; speed and open/close poses come from the matching
-  driver config.
-- `vr_hand_pose`: L6-only mode that retargets Pico hand pose through somehand and
-  commands the continuous L6 hand target. If a hand pose disappears, that side
-  keeps its last commanded pose. This mode uses Teleopit's Pico landmark adapter
-  and the public `somehand.api` from somehand 0.2.0. It always sets L6 speed to
-  the maximum.
-
-When `hands.enabled=true`, hand control remains active in all sim2real modes.
-Shutdown and hand-runtime failure send the configured open pose.
-
-Install the local hand-control packages first if they were not installed with
-the main Pico profile:
-
-```bash
-git submodule update --init --recursive
-pip install -e third_party/linkerhand-python-sdk
-pip install -e third_party/somehand
-scripts/setup/download_somehand_l6_assets.sh
-```
+## Onboard Only: LinkerHand
 
-Bring up the CAN interfaces before testing or running hand control:
+Skip this section unless LinkerHand hardware is connected to the onboard
+computer. Install the hand packages from
+[Installation](../getting-started/installation), then bring up both CAN
+interfaces:
 
 ```bash
 sudo /usr/sbin/ip link set can0 up type can bitrate 1000000
 sudo /usr/sbin/ip link set can1 up type can bitrate 1000000
 ```
 
-Before enabling full sim2real, verify the hand connection with a standalone
-open/close test. The test runs until Ctrl-C:
+Test both hands before starting G1 control:
 
 ```bash
-python scripts/dev/test_linkerhand_l6.py \
+python scripts/dev/test_linkerhand.py \
+    --driver linkerhand_o6 \
     --hand-type both \
     --left-can can0 \
     --right-can can1
 ```
 
-For an O6 standalone open/close test, add the O6 driver:
+Enable O6 hand-pose control by adding these overrides to the sim2real command:
 
-```bash
-python scripts/dev/test_linkerhand_l6.py \
-    --driver linkerhand_o6 \
-    --hand-type both \
-    --left-can can0 \
-    --right-can can1
+```text
+hands.enabled=true
+hands.driver=linkerhand_o6
+hands.mode=vr_hand_pose
+hands.linkerhand_o6.left_can=can0
+hands.linkerhand_o6.right_can=can1
 ```
 
-To test O6 with live Pico gripper input, add `--mode gripper`.
+With `hands.mode=gripper`, hold the controller's side grip trigger to enable
+that hand, then use the index trigger to control how far it closes. Releasing
+the side grip trigger commands that hand to open. LinkerHand L6 is also
+supported through the matching `hands.linkerhand_l6.*` settings.
 
-Then enable L6 gripper control in Pico sim2real:
+## Onboard Only: OpenNeck
+
+Install and calibrate OpenNeck:
 
 ```bash
-hands.enabled=true
-hands.driver=linkerhand_l6
-hands.mode=gripper
-hands.linkerhand_l6.left_can=can0
-hands.linkerhand_l6.right_can=can1
+pip install -e '.[openneck]'
+openneck calibrate
 ```
 
-For O6 gripper control, use:
+Then add these overrides to the sim2real command:
 
-```bash
-hands.enabled=true
-hands.driver=linkerhand_o6
-hands.mode=gripper
-hands.linkerhand_o6.left_can=can0
-hands.linkerhand_o6.right_can=can1
+```text
+neck.enabled=true
+neck.port=/dev/ttyACM0
 ```
 
-For continuous VR hand-pose control, use:
+OpenNeck follows the Pico HMD relative to the operator's upper body. It reuses
+the existing Pico receiver.
 
-```bash
-hands.enabled=true
-hands.driver=linkerhand_l6
-hands.mode=vr_hand_pose
-hands.linkerhand_l6.left_can=can0
-hands.linkerhand_l6.right_can=can1
+## Onboard Only: RealSense Preview
+
+Install `pyrealsense2`, then add:
+
+```text
+input.video.enabled=true
+input.video.device=<optional-realsense-serial>
 ```
 
-## Optional RealSense Preview
+The camera view is sent to the headset. A timeout restarts the camera in the
+background without stopping Pico tracking or G1 control.
+
+## Onboard Only: Record and Review Data
 
-Stream the G1 RealSense color camera back to the Pico headset:
+Recording requires a fresh RealSense RGB frame:
 
 ```bash
 python scripts/run/run_sim2real.py \
-    --config-name pico4_sim2real \
-    controller.policy_path=track.onnx \
-    real_robot.network_interface=enp130s0 \
-    input.video.enabled=true \
-    input.video.device=<optional-realsense-serial>
+    --config-name sim2real_record \
+    controller.policy_path=ckpt/track_g1.onnx \
+    real_robot.network_interface=eth0 \
+    recording.task="walk forward"
 ```
 
-If video fails, control continues unless `input.video.fail_on_error=true`.
+Use terminal `R` to start an episode, `S` to save it, `D` to discard it and
+`Q` to shut down. If no fresh camera frame arrives for one second, the active
+episode is discarded while robot control continues. Start a new episode
+manually after video recovers.
 
-## Common Parameters
+Review the saved recording with:
 
 ```bash
-# Real G1 DDS interface
-real_robot.network_interface=enp130s0
-
-# Pico timeout
-input.pico4_timeout=30
-
-# Override advertised Pico discovery IP
-input.bridge_advertise_ip=192.168.1.20
+pip install -e '.[review]'
+python scripts/view/view_recording.py \
+    --recording data/recordings/sim2real_hdf5
+```
 
-# Consecutive valid mocap frames required before MOCAP
-mocap_switch.check_frames=10
+The viewer synchronizes camera video, measured and reference G1 poses, and
+optional hand and neck signals. See
+[Teleoperation Datasets](../reference/resources/teleoperation-datasets)
+for the stored fields.
 
-# Change Pico pause button
-input.pause_button=right_axis_click
+## Common Problems
 
-# Enable LinkerHand gripper control
-hands.enabled=true
-hands.driver=linkerhand_l6
-hands.mode=gripper
+| Problem | Solution |
+|---------|----------|
+| RealSense does not work on Arm | Remove the PyPI wheel with `pip uninstall pyrealsense2`, then install the conda-forge Arm build with `conda install -c conda-forge pyrealsense2` |
 
-# Enable headset video preview
-input.video.enabled=true
-```
+## Other G1 Workflows
 
-## Troubleshooting
-
-| Symptom | Likely Cause | Fix |
-|---------|--------------|-----|
-| No LowState received | Wrong interface or G1 network not connected | Check Ethernet wiring and `real_robot.network_interface` |
-| `TimeoutError: No Pico4 body data` | Headset is not connected or tracking is inactive | Check headset app, network, and `input.pico4_timeout` |
-| Cannot enter debug mode | Unitree mode release failed | Stop other robot modes and press `Start` again |
-| Robot enters `STANDING` but not `MOCAP` | Mocap validation failed | Keep tracking active and stable; check `mocap_switch.check_frames` logs |
-| Pico pause does not return to `STANDING` | Expected behavior | Pico pause freezes mocap; press remote `X` for `STANDING` |
-| LinkerHand does not move | `hands.enabled=false`, gripper deadman released, SDK/assets not installed, or CAN channel wrong | Enable `hands.enabled`, set `hands.mode`, run `scripts/dev/test_linkerhand_l6.py`, and check the selected driver's `left_can` / `right_can` |
-| Video preview is unavailable | RealSense or video source failed | Check camera permissions, `input.video.source`, and logs |
+- [Standalone Standing Test](standalone-standing)
+- [BVH Playback on Unitree G1](bvh-sim2real)
+- [From Teleoperation Data to Imitation Learning / VLA Deployment](high-level-policy-sim2real)
diff --git a/docs/docs/tutorials/pico-sim2sim.md b/docs/docs/tutorials/pico-sim2sim.md
index ff354dd9..461035f5 100644
--- a/docs/docs/tutorials/pico-sim2sim.md
+++ b/docs/docs/tutorials/pico-sim2sim.md
@@ -2,149 +2,160 @@
 sidebar_position: 2
 ---
 
-# Pico 4 VR Teleoperation in Simulation
+# VR Teleoperation in Simulation
 
-Use this tutorial to verify Pico 4 / Pico 4 Ultra full-body tracking in MuJoCo
-before running on real Unitree G1 hardware.
+Use Pico tracking to control a simulated G1 before connecting a physical robot.
+Do not skip this step: it lets you fix headset, network and body-tracking
+problems without putting hardware at risk.
 
-```text
-Pico headset -> pico-bridge receiver -> retarget -> RL policy -> MuJoCo G1
-```
-
-After this works, continue with [Pico Sim2Real](pico-sim2real).
-
-## Supported Devices
+## Supported Headsets
 
 - Pico 4
 - Pico 4 Ultra
+- Pico 4 Ultra Enterprise
+- Pico 4 Pro
+
+All headsets must have full-body tracking enabled and run a Pico system version
+that supports the current body-tracking interface.
+
+## Before You Start
+
+You need:
+
+- the headset and the computer running Teleopit on the same network,
+- the `pico4` install profile and `robots gmr ckpt bvh` assets, and
+- a working result from
+  [Run a Motion Controller in Simulation](offline-sim2sim).
 
-## 1. Set Up The Headset
+## 1. Prepare the Headset
+
+1. Download the headset APK from
+   [pico-bridge Releases](https://github.com/BotRunner64/pico-bridge/releases).
+2. Install it:
 
-1. Download the headset APK from [pico-bridge Releases](https://github.com/BotRunner64/pico-bridge/releases).
-2. Install it with adb:
    ```bash
    adb install pico-bridge.apk
    ```
-3. Launch the pico-bridge headset client.
-4. Enable full-body tracking.
-5. Keep the headset and Teleopit host on the same network.
 
-## 2. Install The Pico Host Extra
+3. Open the pico-bridge app in the headset.
+4. Turn on full-body tracking.
 
-On the machine that will run Teleopit:
+Teleopit uses pico-bridge 0.2.1. The receiver runs inside Teleopit on your
+computer; there is no second relay program to start.
 
-```bash
-pip install -e '.[pico4]'
-```
+## 2. Check That the Computer Receives Pico Data
 
-Verify the receiver package:
+This diagnostic prints body-frame and connection information without starting
+the robot controller:
 
 ```bash
-python -c "from pico_bridge import PicoBridge; print('OK')"
+python scripts/dev/test_pico_bridge.py --no-video
 ```
 
-Teleopit starts `pico_bridge.PicoBridge` in-process through
-`Pico4InputProvider`. The same Pico input path is used later for wired and
-onboard sim2real deployment.
-
-Teleopit targets pico-bridge 0.2.1 and its `pico_native` tracking semantics.
+Move slightly and confirm that new valid frames continue to arrive. Press
+`Ctrl+C` to stop the diagnostic.
 
-## 3. Download Assets
+If discovery chooses the wrong network address, pass the address that the
+headset can reach:
 
 ```bash
-pip install modelscope
-python scripts/setup/download_assets.py --only robots gmr ckpt bvh
+python scripts/dev/test_pico_bridge.py \
+    --no-video \
+    --bridge-advertise-ip=192.168.1.20
 ```
 
-## 4. Run Pico Sim2Sim
+## 3. Start the Simulation
 
 ```bash
 python scripts/run/run_sim.py \
     --config-name pico4_sim \
-    controller.policy_path=track.onnx
+    controller.policy_path=ckpt/track_g1.onnx
 ```
 
-The simulation starts in `STANDING`. Wait until Pico tracking is active, then
-enter `MOCAP`.
+The robot intentionally starts in `STANDING`; live body tracking does not take
+control until you ask for it.
 
-| Keyboard | Action |
-|----------|--------|
-| `Y` | Enter `MOCAP` |
-| `A` | Pause / resume live mocap |
-| `B` | Toggle `MOCAP` / `ARMS` |
-| `X` | Return to `STANDING` |
-| `Q` | Quit |
+## 4. Use the Simulation State Machine
 
-`pico4_sim.yaml` defaults to `viewers=all`, which opens mocap, retarget, and
-sim2sim viewers. Use `viewers=sim2sim` or `viewers=none` when you want fewer
-windows.
+![Pico simulation control state machine](/img/diagrams/pico-sim-state-machine.svg)
 
-## Pause / Resume
+Labels beginning with **Keyboard** refer to the computer keyboard. Labels
+beginning with **Pico controller** refer to the VR controllers. The Unitree G1
+remote is not used in simulation.
 
-Pico pause/resume freezes the mocap session; it is not a switch back to
-`STANDING`.
+Stand in a comfortable neutral pose and wait for stable tracking before using
+**Keyboard** `Y` to enter `MOCAP`. Move slowly at first. Use **Keyboard** `X` to
+end the VR session and return to `STANDING`; **Keyboard** `Q` quits the
+simulation from any state.
 
-- Press keyboard `A` or the Pico/controller pause button to freeze the current
-  reference pose.
-- Press it again to rebuild the realtime reference path, re-center yaw and
-  ground-plane position, and continue from the current live tracking stream.
+`MOCAP` follows the whole body. `ARMS` keeps the body, waist and legs in the
+standing pose while both arms continue to follow. `PAUSED` holds the current
+reference and returns to the previous `MOCAP` or `ARMS` state when resumed.
 
-The default Pico pause button is `A`. Supported overrides include `B`, `X`, `Y`,
-`left_axis_click`, `right_axis_click`, `left_menu_button`, and
-`right_menu_button`.
+Each new `STANDING -> MOCAP` session recalibrates the live root pose. You may
+turn to a new heading while standing, then enter `MOCAP` again.
 
-The default Pico arms-mode button is `B`. `ARMS` keeps body, waist, and legs at
-the standing pose while both arms follow the live retargeted result.
+:::tip Pausing is not the same as stopping VR control
+Keyboard or Pico controller `A` freezes and resumes the current mocap pose.
+Use Keyboard `X` when you want to end the session and return to `STANDING`.
+:::
 
-## Optional Headset Video Preview
+## Choose the Viewer Layout
 
-pico-bridge 0.2.1 can show a host-side camera stream in the headset. In
-simulation, Teleopit can stream the MuJoCo `d435i_rgb` camera:
+Pico simulation opens the mocap, retarget and physics views by default. Use a
+smaller layout when you no longer need all three:
 
 ```bash
+# Physics result only
 python scripts/run/run_sim.py \
     --config-name pico4_sim \
-    controller.policy_path=track.onnx \
-    input.video.enabled=true
+    controller.policy_path=ckpt/track_g1.onnx \
+    viewers=sim2sim
+
+# Headless
+python scripts/run/run_sim.py \
+    --config-name pico4_sim \
+    controller.policy_path=ckpt/track_g1.onnx \
+    viewers=none
 ```
 
-Use `input.video.source=test-pattern` for a receiver-side video sanity check. If
-video startup fails, Teleopit logs the error, disables video, and keeps tracking
-and control running. Set `input.video.fail_on_error=true` to fail startup
-instead.
+## Optional Headset Video
 
-## Common Parameters
+To send the simulated `d435i_rgb` camera view back to the headset:
 
 ```bash
-# Pico wait timeout for the first body frame
-input.pico4_timeout=30
+python scripts/run/run_sim.py \
+    --config-name pico4_sim \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.video.enabled=true
+```
 
-# Override the IP advertised to the headset during discovery
-input.bridge_advertise_ip=192.168.1.20
+Use `input.video.source=test-pattern` to check only the video connection.
+Video failure disables the preview but does not stop tracking or control.
 
-# Disable discovery and bind explicitly
-input.bridge_discovery=false input.bridge_host=0.0.0.0 input.bridge_port=63901
+## Network Overrides
 
-# Change the Pico pause button
-input.pause_button=right_axis_click
+Most setups only need automatic discovery. Use these overrides when the
+diagnostic shows a network problem:
 
-# Disable keyboard mode control
-keyboard.enabled=false
+```bash
+# Advertise a specific host address to the headset
+input.bridge_advertise_ip=192.168.1.20
 
-# Change policy frequency
-policy_hz=30
+# Disable discovery and bind explicitly
+input.bridge_discovery=false
+input.bridge_host=0.0.0.0
+input.bridge_port=63901
 
-# Enable headset video preview
-input.video.enabled=true
+# Wait longer for the first body frame
+input.pico4_timeout=30
 ```
 
-## Troubleshooting
+## Common Problems
+
+| Problem | Solution |
+|---------|----------|
+| No body frames arrive | Upgrade the Pico headset to the latest available system version, restart it, enable full-body tracking again, and rerun `scripts/dev/test_pico_bridge.py --no-video` |
 
-| Symptom | Likely Cause | Fix |
-|---------|--------------|-----|
-| `ImportError: pico_bridge` | Pico extra not installed | Run `pip install -e '.[pico4]'` |
-| Startup says pico-bridge is too old | Installed receiver does not support the required API or tracking semantics | Reinstall the Pico extra so pico-bridge 0.2.1 is used |
-| `TimeoutError: No Pico4 body data` | Headset is not connected or body tracking is inactive | Check the headset app, network, and `input.pico4_timeout` |
-| Discovery cannot find the host | Wrong advertised IP or blocked UDP | Set `input.bridge_advertise_ip=<host-ip>` and confirm UDP port `63901` is reachable |
-| Sim robot does not follow | Loop is still in `STANDING` | Press `Y` after tracking is ready |
-| Pico video is black or disabled | Video source failed or camera access is unavailable | Check `input.video.source` and logs |
+Once this workflow is reliable, continue with
+[VR Teleoperation on Unitree G1](pico-sim2real).
diff --git a/docs/docs/tutorials/standalone-standing.md b/docs/docs/tutorials/standalone-standing.md
index 772b0e91..73e730cc 100644
--- a/docs/docs/tutorials/standalone-standing.md
+++ b/docs/docs/tutorials/standalone-standing.md
@@ -36,7 +36,7 @@ Use `--dry-run` first for timing checks without sending motor commands:
 
 ```bash
 python scripts/run/standalone_standing.py \
-    --policy track.onnx \
+    --policy ckpt/track_g1.onnx \
     --network-interface enp130s0 \
     --dry-run
 ```
@@ -47,7 +47,7 @@ Run the standing controller after confirming the network interface:
 
 ```bash
 python scripts/run/standalone_standing.py \
-    --policy track.onnx \
+    --policy ckpt/track_g1.onnx \
     --network-interface enp130s0
 ```
 
@@ -55,7 +55,7 @@ For onboard deployment, the interface is usually `eth0`:
 
 ```bash
 python scripts/run/standalone_standing.py \
-    --policy track.onnx \
+    --policy ckpt/track_g1.onnx \
     --network-interface eth0
 ```
 
@@ -67,7 +67,7 @@ this startup behavior:
 
 ```bash
 python scripts/run/standalone_standing.py \
-    --policy track.onnx \
+    --policy ckpt/track_g1.onnx \
     --network-interface eth0 \
     --kp-ramp-duration 2.0 \
     --kp-ramp-floor-ratio 0.1
diff --git a/docs/docs/tutorials/training.md b/docs/docs/tutorials/training.md
index fcf5cc2a..852cd273 100644
--- a/docs/docs/tutorials/training.md
+++ b/docs/docs/tutorials/training.md
@@ -1,40 +1,78 @@
 ---
-sidebar_position: 5
+sidebar_position: 4
 ---
 
-# Training
+# Train a Motion Controller
 
-Train a whole-body tracking policy and export it as ONNX for inference.
+This guide starts with downloaded motion data and ends with an ONNX controller
+that Teleopit can run in simulation or on a G1.
 
-:::info
-For data preparation, see [Dataset Reference](../reference/dataset). For common training issues, see [Training Troubleshooting](../reference/training-troubleshooting).
-:::
+The normal training path assumes an NVIDIA GPU. Motion data is loaded into
+memory at startup, so larger combined datasets also need enough system and GPU
+memory.
 
-## Setup
+## Before You Start
 
-```bash
-conda create -n teleopit python=3.10
-conda activate teleopit
-pip install -e '.[train]'
-```
+Follow [Installation](../getting-started/installation) with:
+
+- the `train` profile, and
+- the `robots data` asset bundle.
+
+Verify the training package:
 
-Verify:
 ```bash
 python -c "import train_mimic.tasks; print('training OK')"
 ```
 
-Download the distributed minimal datasets and generate the combined precomputed
-training dataset:
+## 1. Prepare the Downloaded Dataset
+
+Downloaded datasets are compact distribution files. Training uses a second
+directory with joint velocities and body kinematics precomputed:
 
 ```bash
-python scripts/setup/download_assets.py --only robots data
 python train_mimic/scripts/data/precompute_dataset.py \
-    data/datasets --outdir data/datasets_precomputed --jobs 8
+    data/datasets \
+    --outdir data/datasets_precomputed \
+    --jobs 8
+```
+
+Use `data/datasets_precomputed` for every training, playback and benchmark
+command below. Pointing training at the original `data/datasets` directory is
+an error, not a supported shortcut.
+
+For custom BVH, PKL, NPZ or Pico-recorded data, see
+[Motion Datasets](../reference/resources/motion-datasets).
+
+## 2. Choose the Robot Model
+
+Use `--robot_xml` to select the MuJoCo model used by training. If the argument
+is omitted, it defaults to:
+
+```text
+assets/robots/unitree_g1/g1_29dof.xml
 ```
 
-## Training
+The current `robots` asset bundle includes these ready-to-use examples:
+
+| Model XML | Setup |
+|-----------|-------|
+| `assets/robots/unitree_g1/g1_29dof.xml` | Base G1 model and the default |
+| `assets/robots/unitree_g1/g1_29dof_dex3.xml` | G1 with Dex3 hand geometry and inertial properties |
+| `assets/robots/unitree_g1/g1_29dof_neck_o6.xml` | G1 with neck active vision and O6 hand models |
+
+This table describes the models shipped in the current asset bundle; it is not
+a hard-coded model allowlist. Another XML can be passed when its joint and body
+definitions are compatible with the selected task configuration and dataset.
 
-### Smoke Test
+The full-training command below explicitly selects the base model as a copyable
+example. Replace that path with the model you want to train. The other commands
+do not repeat this option; playback and benchmark load the robot from the
+selected task configuration.
+
+## 3. Run a Short Smoke Test
+
+Before starting a long job, verify that the dataset, simulator and logger work
+together:
 
 ```bash
 python train_mimic/scripts/train.py \
@@ -43,103 +81,114 @@ python train_mimic/scripts/train.py \
     --motion_file data/datasets_precomputed
 ```
 
-### Full Training
+The test is successful when environments step, losses are reported and a run
+directory appears under `logs/rsl_rl/g1_general_tracking/`.
+
+## 4. Start a Full Run
 
 ```bash
 python train_mimic/scripts/train.py \
+    --robot_xml assets/robots/unitree_g1/g1_29dof.xml \
     --num_envs 4096 \
     --max_iterations 30000 \
     --motion_file data/datasets_precomputed
 ```
 
-### Multi-GPU
+Reduce `--num_envs` if GPU memory is insufficient. The default logger is
+TensorBoard; choose `--logger wandb` or `--logger swanlab` when required.
+
+`--max_iterations` means additional iterations. For example, resuming
+`model_12000.pt` with `--max_iterations 18000` continues to iteration 30000.
+
+## 5. Watch the Checkpoint in Simulation
 
 ```bash
-python train_mimic/scripts/train.py \
-    --gpu_ids 0 1 2 3 \
-    --num_envs 1024 \
-    --max_iterations 30000 \
+python train_mimic/scripts/play.py \
+    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
     --motion_file data/datasets_precomputed
 ```
 
-### Multi-Node Multi-GPU
+Playback starts clips from their beginning and removes training noise. Use it
+to catch an obviously unstable policy before exporting.
 
-Use `torchrun` directly when training across multiple machines:
+## 6. Run the Benchmark
 
 ```bash
-torchrun \
-    --nnodes=$PET_NNODES \
-    --nproc_per_node=$PET_NPROC_PER_NODE \
-    --node_rank=$PET_NODE_RANK \
-    --master_addr=$PET_MASTER_ADDR \
-    --master_port=$PET_MASTER_PORT \
-    train_mimic/scripts/train.py \
-    --num_envs 1024 \
-    --max_iterations 1000 \
-    --motion_file data/datasets_precomputed
+python train_mimic/scripts/benchmark.py \
+    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
+    --motion_file data/datasets_precomputed \
+    --num_envs 32
 ```
 
-**Notes:**
-- `--num_envs` is per-GPU in multi-GPU mode
-- `--num_envs` is also per-process in multi-node mode, so total environments scale with `world_size`
-- Default logger is TensorBoard. Use `--logger wandb` or `--logger swanlab` to select W&B or SwanLab; the project name defaults to `experiment_name`
-- `--motion_file` accepts a precomputed training dataset root directory or a single precomputed `.h5` shard; shard discovery is recursive
-- If you only have the minimal distributed shards, first run `python train_mimic/scripts/data/precompute_dataset.py <minimal_dataset> --outdir <precomputed_dataset>` and pass the precomputed output to training.
-- Training loads all discovered precomputed motion windows into memory at startup.
-- `--max_iterations` means additional iterations; resuming from `model_12000.pt` with `--max_iterations 18000` trains to `model_30000.pt`
+The benchmark evaluates one deterministic 10-second rollout for every eligible
+clip. It reports:
+
+- mean per-joint position error (`MPJPE`),
+- root position, rotation and velocity error, and
+- rollout success rate.
+
+Results are written as a text summary, JSON, per-clip CSV and per-rollout CSV.
 
-## Export ONNX
+## 7. Export ONNX
 
 ```bash
 python train_mimic/scripts/save_onnx.py \
     --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
-    --output track.onnx \
+    --output ckpt/track_g1.onnx \
     --history_length 10
 ```
 
-The exported model is a dual-input ONNX (`obs` + `obs_history`). The inference side expects a 167D dual-input ONNX policy matching the current `velcmd_history` observation.
+The result must be a dual-input TemporalCNN with `obs` and `obs_history`.
+Teleopit validates the 167D observation signature at startup and rejects an
+incompatible export.
 
-## Evaluation
-
-### Playback
+Test the export in the normal runtime:
 
 ```bash
-python train_mimic/scripts/play.py \
-    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
-    --motion_file data/datasets_precomputed
+python scripts/run/run_sim.py \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
 ```
 
-### Benchmark
+## Scale to Multiple GPUs
+
+For one machine:
 
 ```bash
-python train_mimic/scripts/benchmark.py \
-    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
-    --motion_file data/datasets_precomputed \
-    --num_envs 1
+python train_mimic/scripts/train.py \
+    --gpu_ids 0 1 2 3 \
+    --num_envs 1024 \
+    --max_iterations 30000 \
+    --motion_file data/datasets_precomputed
 ```
 
-### Benchmark with Video
+`--num_envs` is per GPU.
+
+For multiple machines, launch the same script with `torchrun`:
 
 ```bash
-python train_mimic/scripts/benchmark.py \
-    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
-    --motion_file data/datasets_precomputed \
-    --num_envs 1 \
-    --video \
-    --video_length 600
+torchrun \
+    --nnodes=$PET_NNODES \
+    --nproc_per_node=$PET_NPROC_PER_NODE \
+    --node_rank=$PET_NODE_RANK \
+    --master_addr=$PET_MASTER_ADDR \
+    --master_port=$PET_MASTER_PORT \
+    train_mimic/scripts/train.py \
+    --num_envs 1024 \
+    --max_iterations 1000 \
+    --motion_file data/datasets_precomputed
 ```
 
-## Training Architecture
+Here `--num_envs` is per process, so the total scales with the world size.
 
-```text
-train_mimic/scripts
-    -> train_mimic/app.py
-    -> single task registry / env builder / runner cfg
-    -> mjlab + rsl_rl
-```
+## Common Problems
+
+| Symptom | What to check |
+|---------|---------------|
+| Loader says the dataset is minimal | Run `precompute_dataset.py` and use its output directory |
+| Out of GPU memory | Lower `--num_envs` or train with fewer precomputed data shards |
+| Out of system memory during startup | Train on fewer precomputed shards or add RAM |
+| Training is unexpectedly slow | Check that PyTorch detects CUDA and that the training device is a CUDA GPU |
 
-Key files:
-- `train_mimic/app.py` - Shared entry point for train/play/benchmark
-- `train_mimic/tasks/tracking/config/env.py` - General-Tracking-G1 env builder
-- `train_mimic/tasks/tracking/config/rl.py` - TemporalCNN PPO config
-- `train_mimic/tasks/tracking/mdp/commands.py` - Supports `uniform`, `start`, and `rewind` sampling modes. Training defaults to `rewind`; playback/benchmark use `start`.
+For task internals and model dimensions, see
+[Architecture](../reference/architecture).
diff --git a/docs/docusaurus.config.ts b/docs/docusaurus.config.ts
index 8119bf29..78dd3cd4 100644
--- a/docs/docusaurus.config.ts
+++ b/docs/docusaurus.config.ts
@@ -4,7 +4,7 @@ import type * as Preset from '@docusaurus/preset-classic';
 
 const config: Config = {
   title: 'Teleopit',
-  tagline: 'Lightweight, extensible whole-body teleoperation framework for humanoid robots',
+  tagline: 'Full-embodiment teleoperation for humanoid robots',
   favicon: 'img/favicon.ico',
 
   url: 'https://BotRunner64.github.io',
@@ -72,7 +72,7 @@ const config: Config = {
           items: [
             {label: 'Getting Started', to: '/getting-started/installation'},
             {label: 'Tutorials', to: '/tutorials/offline-sim2sim'},
-            {label: 'Configuration', to: '/configuration/overview'},
+            {label: 'Reference', to: '/reference/configuration/overview'},
           ],
         },
         {
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json
new file mode 100644
index 00000000..8a845705
--- /dev/null
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json
@@ -0,0 +1,26 @@
+{
+  "version.label": {
+    "message": "当前版本",
+    "description": "The label for version current"
+  },
+  "sidebar.docsSidebar.category.Getting Started": {
+    "message": "入门",
+    "description": "The label for category 'Getting Started' in sidebar 'docsSidebar'"
+  },
+  "sidebar.docsSidebar.category.Tutorials": {
+    "message": "教程",
+    "description": "The label for category 'Tutorials' in sidebar 'docsSidebar'"
+  },
+  "sidebar.docsSidebar.category.Configuration": {
+    "message": "配置",
+    "description": "The label for category 'Configuration' in sidebar 'docsSidebar'"
+  },
+  "sidebar.docsSidebar.category.Reference": {
+    "message": "参考资料",
+    "description": "The label for category 'Reference' in sidebar 'docsSidebar'"
+  },
+  "sidebar.docsSidebar.category.Resources": {
+    "message": "资源",
+    "description": "The label for category 'Resources' in sidebar 'docsSidebar'"
+  }
+}
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md
deleted file mode 100644
index 0366815c..00000000
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md
+++ /dev/null
@@ -1,224 +0,0 @@
----
-sidebar_position: 2
----
-
-# 配置参考
-
-本页列出 Teleopit 所有可配置字段及其含义。
-
-## 顶层字段
-
-| 字段 | 类型 | 默认值 | 说明 |
-|---|---|---|---|
-| `policy_hz` | int | — | 策略推理频率（Hz） |
-| `pd_hz` | int | `200` | PD 控制器频率（Hz，仅仿真），通常高于 `policy_hz` |
-| `viewers` | str/list | `sim2sim` | 可视化窗口集合：`mocap`、`retarget`、`sim2sim`、`camera`、`all`、`none`。`all` 打开 `mocap`、`retarget` 和 `sim2sim`；如需相机画面需显式加入 `camera` |
-| `realtime` | bool | `false` | 是否启用实时模式（实机部署时需开启） |
-| `num_steps` | int | — | 仿真总步数；设为 `-1` 表示无限运行 |
-| `keyboard.enabled` | bool | `false` | 是否启用 sim2sim 实时键盘模式控制 |
-| `playback.pause_on_end` | bool | `false` | 回放结束后是否暂停（而非退出） |
-| `playback.keyboard.enabled` | bool | `false` | 是否启用键盘控制回放进度 |
-
-## Robot 字段
-
-机器人相关配置位于 `robot/` 子目录。以 `robot/g1.yaml` 为例：
-
-| 字段 | 类型 | 说明 |
-|---|---|---|
-| `num_actions` | int | 策略输出的动作维度（即受控关节数） |
-| `xml_path` | str | MuJoCo MJCF 模型文件路径 |
-| `d435i_rgb` | camera | G1 MJCF 中的固定 RGB 相机；配合 `viewers=[sim2sim,camera]` 显示画面 |
-| `kps` | list[float] | 各关节的比例增益（P 增益） |
-| `kds` | list[float] | 各关节的微分增益（D 增益） |
-| `default_angles` | list[float] | 默认关节角度（弧度），也是策略动作的零点 |
-| `torque_limits` | list[float] | 各关节的力矩上限 |
-
-## Controller 字段
-
-控制器配置位于 `controller/` 子目录。
-
-| 字段 | 类型 | 说明 |
-|---|---|---|
-| `policy_path` | str | **必填。** 策略模型文件路径（ONNX 格式） |
-| `device` | str | 推理设备，如 `"cpu"` 或 `"cuda:0"` |
-| `action_scale` | float | 动作缩放系数 |
-| `clip_range` | list[float] | 动作裁剪范围，格式为 `[min, max]` |
-| `default_dof_pos` | list[float] | 默认关节位置，用于计算控制目标 |
-
-### 关键说明：`default_dof_pos` 与动作计算
-
-策略输出的 action 是相对于 `default_dof_pos` 的**偏移量**，最终的关节控制目标按如下公式计算：
-
-```
-target = clip(action, clip_range) * action_scale + default_dof_pos
-```
-
-因此，`default_dof_pos` 决定了策略输出的"零点"。如果该值与训练时使用的不一致，策略的行为将完全偏离预期。
-
-## Input 字段
-
-输入源配置位于 `input/` 子目录，不同输入源的字段各异。
-
-### BVH 输入（`input/bvh.yaml`）
-
-| 字段 | 类型 | 说明 |
-|---|---|---|
-| `bvh_file` | str | BVH 文件路径 |
-| `bvh_format` | str | BVH 骨骼格式标识 |
-| `human_format` | str | 人体骨架格式 |
-
-> BVH 输入不设置 `input.provider` — 由配置组名自动推断。
-
-### Pico 4 输入（`input/pico4.yaml`）
-
-| 字段 | 类型 | 默认值 | 说明 |
-|---|---|---|---|
-| `provider` | str | `pico4` | 输入源类型 |
-| `human_format` | str | `pico_bridge` | 重定向骨架格式 |
-| `pico4_timeout` | float | `60` | 等待设备连接的超时时间（秒） |
-| `pico4_buffer_size` | int | `60` | 帧缓冲区大小 |
-| `pause_button` | str | `A` | 用于暂停/恢复的手柄按钮名称 |
-| `pause_debounce_s` | float | `0.25` | 暂停按钮防抖时间 |
-| `arms_button` | str | `B` | Pico 中用于切换 `MOCAP` / `ARMS` 的按钮 |
-| `arms_debounce_s` | float | `0.25` | 双臂模式按钮防抖时间 |
-| `bridge_host` | str | `0.0.0.0` | Teleopit host receiver 绑定地址 |
-| `bridge_port` | int | `63901` | Teleopit host receiver TCP/UDP 端口 |
-| `bridge_discovery` | bool | `true` | 是否启用 pico-bridge 发现广播 |
-| `bridge_advertise_ip` | str/null | `null` | 可选的 host 广播 IP 覆盖 |
-| `bridge_start_timeout` | float | `10.0` | 启动 bridge 的超时时间 |
-| `bridge_history_size` | int | `120` | bridge 保留的 Pico 帧历史长度 |
-| `video.enabled` | bool | `false` | 通过 pico-bridge 0.2.1 将 host 相机预览发送回 Pico |
-| `video.source` | str/null | `null` | 视频源：`mujoco`、`realsense` 或 `test-pattern` |
-| `video.width` / `height` / `fps` | int | `1280` / `720` / `30` | 视频采集/渲染设置 |
-| `video.device` | str/null | `null` | 可选的 RealSense 序列号 |
-| `video.fail_on_error` | bool | `false` | 视频失败时是否让启动失败，而不是关闭视频后继续 |
-
-## Realtime 字段
-
-实时模式相关字段，仅在 `realtime=true` 时生效。
-
-| 字段 | 说明 |
-|---|---|
-| `retarget_buffer_enabled` | 是否启用重定向缓冲 |
-| `retarget_buffer_window_s` | 缓冲窗口大小 |
-| `retarget_buffer_delay_s` | 缓冲延迟 |
-| `reference_steps` | 参考轨迹窗口步数 |
-| `realtime_buffer_warmup_steps` | 播放前预热帧数 |
-| `reference_velocity_smoothing_alpha` | 速度平滑系数 |
-| `reference_anchor_velocity_smoothing_alpha` | 锚点速度平滑系数 |
-
-## Sim2Real 字段
-
-以下字段用于 sim2real 配置（`sim2real.yaml`、`pico4_sim2real.yaml`）。
-
-sim2real 默认使用 `viewers=none`。设置 `viewers=retarget` 可打开一个可选的
-MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all`
-仅用于仿真 viewer。
-
-### 安全相关
-
-| 字段 | 说明 | 默认值 |
-|---|---|---|
-| `startup_ramp_duration` | 进入 `STANDING` 后的 Kp ramp 时长；逐步提高 PD 增益，不改变 policy target | `2.0` |
-| `joint_vel_limit` | 关节速度限制（rad/s），超过时触发急停 | `10.0` |
-| `mocap_switch.check_frames` | 切换到 MOCAP 前所需的连续有效帧数 | `10` |
-| `arm_mocap.controlled_joint_indices` | Pico `ARMS` 模式下由实时 retargeting 驱动的 G1 关节 | `[15..28]` |
-
-### 真机 SDK
-
-| 字段 | 说明 | 默认值 |
-|---|---|---|
-| `real_robot.network_interface` | Unitree DDS 通信网络接口。PC 通过网线连接 G1 控制时，用 `ifconfig` 找到这根网线对应的接口名并填写，例如 `enp130s0`；在机器人 onboard 计算机上运行时通常使用 `eth0` | `eth0` |
-| `real_robot.kp_real` | 真机比例增益（各关节） | — |
-| `real_robot.kd_real` | 真机微分增益（各关节） | — |
-| `real_robot.kd_damping` | 阻尼模式 kd | `8.0` |
-| `real_robot.control_mode` | 踝关节控制模式（`PR` = Pitch-Roll） | `PR` |
-| `real_robot.joint_pos_lower` | 关节位置下限（rad） | — |
-| `real_robot.joint_pos_upper` | 关节位置上限（rad） | — |
-
-### 暂停/恢复（Pico sim2real）
-
-实时 Pico 恢复追踪时会先重新居中航向和地面平面位置。操作者应保持静止，并尽量贴近暂停时的姿态，以减少参考突变。
-
-### 灵巧手（Pico sim2real）
-
-`hands.enabled=true` 要求 `input.provider=pico4`，并以本地 editable 方式安装
-`third_party/linkerhand-python-sdk` 和 `third_party/somehand`。启用后，手控会在所有 sim2real 模式中保持生效。
-`gripper` 支持 `linkerhand_l6` 和 `linkerhand_o6`，会用 Pico trigger 在配置的张开和闭合姿态之间插值。
-`vr_hand_pose` 只支持 L6：手部 pose 消失时，对应侧会保持上一条命令；L6 速度会设为最大值；
-Teleopit 会先将 Pico 手部状态转成 21 个 landmarks，再只通过 somehand 0.2.0 公开的 `somehand.api` 调用。
-
-| 字段 | 说明 | 默认值 |
-|---|---|---|
-| `hands.enabled` | 启用可选手部运行时 | `false` |
-| `hands.mode` | `gripper` 或 `vr_hand_pose` | `gripper` |
-| `hands.driver` | 手部设备驱动：`linkerhand_l6` 或 `linkerhand_o6` | `linkerhand_l6` |
-| `hands.sides` | 控制侧 | `[left, right]` |
-| `hands.rate_hz` | gripper 最大命令频率（Hz） | `30.0` |
-| `hands.frame_timeout_s` | 手柄或手部 pose 过期阈值 | `0.3` |
-| `hands.linkerhand_l6.left_can` / `right_can` | 左右手 CAN 通道 | `can0` / `can1` |
-| `hands.linkerhand_l6.speed` | `gripper` 使用的 L6 速度；`vr_hand_pose` 会覆盖为最大速度 | 见配置 |
-| `hands.linkerhand_l6.deadman_threshold` | 启用单侧控制所需的最小 grip 值 | `0.5` |
-| `hands.linkerhand_l6.trigger_deadzone` | trigger 两端死区 | `0.05` |
-| `hands.linkerhand_l6.open_pose` / `close_pose` | L6 的 6 维张开/闭合姿态 | 见配置 |
-| `hands.linkerhand_o6.left_can` / `right_can` | 左右 O6 手 CAN 通道 | `can0` / `can1` |
-| `hands.linkerhand_o6.speed` | `gripper` 使用的 O6 速度 | 见配置 |
-| `hands.linkerhand_o6.open_pose` / `close_pose` | O6 的 6 维张开/闭合姿态 | 见配置 |
-| `hands.somehand.config_path` | `vr_hand_pose` 使用的 somehand 双手 L6 配置 | 见配置 |
-| `hands.somehand.rate_hz` | 低延时 `vr_hand_pose` 命令频率（Hz） | `60.0` |
-| `hands.somehand.max_iterations` | `vr_hand_pose` 的 somehand solver 迭代上限 | `12` |
-| `hands.somehand.temporal_filter_alpha` | somehand 输入 landmarks 平滑 alpha；`1.0` 表示关闭平滑延时 | `1.0` |
-| `hands.somehand.output_alpha` | somehand qpos 输出平滑 alpha；`1.0` 表示关闭平滑延时 | `1.0` |
-
-### HDF5 录制（Pico sim2real）
-
-`recording.enabled=true` 只支持 `input.provider=pico4`、
-`input.video.enabled=true`、`input.video.source=realsense`，并且需要交互式终端。
-录制是手动控制：`R` 开始 episode，`S` 保存当前 episode，`D` 丢弃当前 episode，
-`Q` 关闭。可以录制 `STANDING`、`MOCAP`、`ARMS` 和暂停状态的 mocap。
-
-`sim2real_record.yaml` 会同时启用录制和必需的 RealSense `input.video`
-路径。录制不会打开第二路相机，而是消费 `pico_input` 已经产生的同一批帧。
-
-| 字段 | 说明 | 默认值 |
-|---|---|---|
-| `recording.enabled` | 启用手动 HDF5 录制 | `false` |
-| `recording.output_dir` | 数据集根目录 | `data/recordings/sim2real_hdf5` |
-| `recording.task` | 写入 frame 的任务字符串 | `demo` |
-| `recording.fps` | 录制/视频主时钟频率 | `30` |
-| `recording.min_episode_seconds` | 保存时短于该时长的 episode 会被丢弃 | `1.0` |
-| `recording.record_modes` | 允许开始录制和写帧的模式 | `[standing, mocap, arms, pause]` |
-| `recording.camera.key` | RGB 图像数据集 key | `observation.images.d435i_rgb` |
-| `recording.camera.width` / `height` / `fps` | RealSense RGB 采集设置 | `640` / `480` / `30` |
-| `recording.camera.device` | 可选 RealSense 序列号 | `null` |
-| `recording.video.codec` / `quality` / `pixelformat` | MP4 sidecar 编码设置 | `libx264` / `8` / `yuv420p` |
-
-相机失败时的行为由 `input.video.fail_on_error` 控制。
-
-每个保存的 episode 会在 `recording.output_dir/episodes/` 下写入一个 `.h5`
-文件，并在 `recording.output_dir/videos/<camera_key>/` 下写入一个压缩 MP4
-sidecar。HDF5 episode 保存 `frame_index` 和 `timestamp` 数组，并在根属性中
-记录 `video_path`、`video_fps` 和 `video_frames` 用于同步。录制不会写入原始
-RGB 图像 dataset。
-
-HDF5 datasets：
-
-```text
-frame_index                    int64[N]
-timestamp                      float64[N]
-observation.state              float32[68]
-observation.mode               float32[1]
-action                         float32[36]
-action.hand                    float32[12]
-```
-
-根属性包含 Teleopit HDF5 recording format、schema version、task、fps、
-frame count 和视频同步元数据。
-
-`observation.state` 的顺序是 `joint_pos(29)`、`joint_vel(29)`、
-`base_quat_wxyz(4)`、`base_ang_vel(3)` 和 `projected_gravity(3)`。
-`observation.mode` 是数值类别：`standing=0`、`mocap=1`、
-`arms=2`、`pause=3`。`action` 是当前 reference qpos：
-`root_pos(3) + root_quat_wxyz(4) + joint_pos(29)`。
-`action.hand` 是手部 worker 最新的 LinkerHand 命令：
-`left_pose(6) + right_pose(6)`，使用 SDK 的 0-255 pose 数值。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/faq.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/faq.md
deleted file mode 100644
index 65856f4e..00000000
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/faq.md
+++ /dev/null
@@ -1,29 +0,0 @@
----
-sidebar_position: 3
----
-
-# 配置常见问题
-
-## 为什么设置了 `policy_path` 还是启动不了？
-
-1. 确认文件存在
-2. 确认输入维度是 `167`，且为双输入 ONNX（`obs` + `obs_history`）
-
-## 为什么必须显式指定 `input.bvh_file`？
-
-`input/bvh.yaml` 已不再提供机器相关的默认路径。始终在命令行显式指定：
-
-```bash
-python scripts/run/run_sim.py \
-    controller.policy_path=policy.onnx \
-    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
-```
-
-## 为什么 `viewer=true` 不起作用？
-
-旧的 `viewer` 别名已移除。请使用 `viewers`（复数）：
-
-```bash
-python scripts/run/run_sim.py controller.policy_path=policy.onnx viewers=sim2sim
-python scripts/run/run_sim.py controller.policy_path=policy.onnx viewers=none
-```
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/download-assets.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/download-assets.md
deleted file mode 100644
index 9c6463ac..00000000
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/download-assets.md
+++ /dev/null
@@ -1,49 +0,0 @@
----
-sidebar_position: 2
----
-
-# 下载资源
-
-机器人模型、数据集和检查点托管在 ModelScope 上，使用前需要先下载。
-
-## 一键下载
-
-下载全部资源（模型、数据、GMR 重定向资源）：
-
-```bash
-pip install modelscope
-python scripts/setup/download_assets.py
-```
-
-## 按需下载
-
-只下载推理所需的资源：
-
-```bash
-python scripts/setup/download_assets.py --only robots gmr ckpt bvh
-```
-
-## 资源清单
-
-checkpoint、数据集和资源包更新后，下载文件大小会变化。下表中的仓库路径才是稳定约定。
-
-| 本地路径 | 用途 |
-|----------|------|
-| `track.onnx` | ONNX 推理模型 |
-| `track.pt` | 用于恢复训练的 PyTorch checkpoint |
-| `data/datasets/<dataset>/shard_*.h5` | 最小运动数据集；训练前需先预计算 |
-| `data/sample_bvh/*.bvh` | 示例动捕文件 |
-| `assets/robots/unitree_g1/` | 训练、sim2sim、重定向和 FK 校验共用的 G1 canonical XML 与 mesh |
-| `teleopit/retargeting/gmr/assets/` | GMR 重定向资源、IK 配置和非 canonical 机器人描述 |
-
-## 资源分组
-
-| 分组 | ModelScope 仓库 | 包含内容 |
-|------|-----------------|----------|
-| `ckpt` | `BingqianWu/Teleopit-models` | `track.onnx`、`track.pt` |
-| `robots` | `BingqianWu/Teleopit-models` | Canonical 机器人 XML/mesh |
-| `gmr` | `BingqianWu/Teleopit-models` | GMR 重定向资源 |
-| `bvh` | `BingqianWu/Teleopit-models` | 示例 BVH 动捕文件 |
-| `data` | `BingqianWu/Teleopit-datasets` | `lafan1`、`pico_record`、`seed`、`twist2` 的最小 shard |
-
-资源管理的更多细节（上传、版本控制等）请参阅 [资源管理](../reference/assets)。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md
index 4f3028c0..29238b23 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md
@@ -2,100 +2,164 @@
 sidebar_position: 1
 ---
 
-# 安装
+# 安装 Teleopit
 
-Teleopit 提供多种安装配置，你可以根据实际使用场景选择对应的安装方式。
+只安装你真正需要的部分。下面的命令都在仓库根目录执行，并要求 Python 3.10
+或更高版本。
 
-## 前置条件
-
-- Python 3.10+
-- [Conda](https://docs.conda.io/)（推荐）
+## 1. 获取代码
 
 ```bash
-conda create -n teleopit python=3.10
-conda activate teleopit
+git clone https://github.com/BotRunner64/Teleopit.git
+cd Teleopit
 ```
 
-## 安装配置
+只有连接真实 G1 或使用 LinkerHand 时才需要 Git 子模块，相关命令放在本页后面。
+
+## 2. 创建 Python 环境
+
+下面三种方式任选一种，不要全部执行。
 
-### 仅推理（sim2sim）
+### uv
 
 ```bash
-pip install -e .
+uv venv --python 3.10
+source .venv/bin/activate
 ```
 
-该配置已足够进行离线 BVH 回放和 MuJoCo 仿真。
+本页后续出现 `pip install` 时，也可以替换为 `uv pip install`。
+
+### pip 和 venv
+
+```bash
+python3.10 -m venv .venv
+source .venv/bin/activate
+python -m pip install --upgrade pip
+```
 
-### 训练
+### Conda
 
 ```bash
-pip install -e '.[train]'
+conda create -n teleopit python=3.10
+conda activate teleopit
 ```
 
-额外安装 `rsl-rl-lib`、`mjlab`、`wandb`、`swanlab` 等训练相关依赖。
+Conda 负责创建环境；进入环境后，仍使用 `pip install` 安装 Teleopit。
+
+## 3. 根据目标安装依赖
+
+每个 extra 都包含 Teleopit 基础包。先安装与你当前目标对应的一项；以后可以在同一个
+环境中继续安装其他 extra。
+
+| 目标 | 安装命令 | 增加的内容 |
+|------|----------|------------|
+| 在 MuJoCo 中运行运控 | `pip install -e .` | 基础推理、GMR、MuJoCo 和 ONNX Runtime |
+| 在仿真或 G1 上使用 Pico | `pip install -e '.[pico4]'` | Pico 接收与真机运行环境 |
+| 不使用 Pico，在真实 G1 上回放 BVH | `pip install -e '.[sim2real]'` | G1 运行环境和 OpenCV |
+| 训练运控策略 | `pip install -e '.[train]'` | mjlab、RSL-RL 和实验记录工具 |
+| 录制 Pico 真机数据 | `pip install -e '.[recording]'` | Pico 运行环境和 MP4 写入依赖 |
+| 查看已录制的数据 | `pip install -e '.[review]'` | OpenCV 和 MuJoCo/Viser 查看工具 |
+| 使用 OpenNeck | `pip install -e '.[openneck]'` | Pico 运行环境和 OpenNeck 驱动 |
+| 运行测试 | `pip install -e '.[dev]'` | pytest 和覆盖率工具 |
 
-### Sim2Real（硬件部署）
+## 4. 下载对应资源
+
+Python 包里不包含机器人 mesh、运控模型和动作数据。先安装默认的 ModelScope
+下载工具：
 
 ```bash
-pip install -e '.[sim2real]'
+pip install modelscope
 ```
 
-额外安装 `opencv-python`。此外还需要初始化子模块并编译/安装 C++ `g1_bridge_sdk` 桥接库：
+再根据目标下载：
+
+| 目标 | 命令 |
+|------|------|
+| 仿真、Pico VR 或 G1 推理 | `python scripts/setup/download_assets.py --only robots gmr ckpt bvh` |
+| 使用已发布数据集训练 | `python scripts/setup/download_assets.py --only robots data` |
+| 下载全部资源 | `python scripts/setup/download_assets.py` |
+
+需要从 HuggingFace 下载时：
 
 ```bash
-git submodule update --init --recursive
-bash scripts/setup/setup_g1_bridge.sh
+python scripts/setup/download_assets.py \
+    --source huggingface \
+    --only robots gmr ckpt bvh
 ```
 
-详见 [G1 Bridge SDK](../reference/g1-bridge-sdk)。
+推理资源包会把 `track_g1` 和 `track_g1_neck_o6` 两组 ONNX/checkpoint 放到 `ckpt/`，
+并把 G1 模型文件、GMR 文件和示例 BVH 放到代码默认查找的位置。完整文件清单和资源分组
+见[资产](../reference/resources/assets)。
+
+## 5. 连接真实 G1 前的额外安装
 
-### Pico 4 VR
+在实际运行 Teleopit 的电脑上编译 C++ DDS bridge：
 
 ```bash
-pip install -e '.[pico4]'
+git submodule update --init --recursive
+bash scripts/setup/setup_g1_bridge.sh
 ```
 
-Teleopit 使用进程内的 `pico_bridge.PicoBridge` receiver 接收 Pico 追踪数据。
-Teleopit 面向 pico-bridge 0.2.1 及其 `pico_native` tracking 语义。
-receiver 可以运行在工作站 PC，也可以运行在机器人 onboard 计算机。
-完整设置流程详见 [Pico Sim2Sim](../tutorials/pico-sim2sim) 和
-[Pico Sim2Real](../tutorials/pico-sim2real)。
+无论使用 Pico 还是真机 BVH 回放，都需要这个 bridge。如果编译失败或收不到机器人
+状态，请查看[配套项目](../reference/companion-projects#g1-bridge-sdk)。
+
+## 6. 可选硬件
 
-Pico sim2real 可选的 LinkerHand 控制使用本地 third-party 包。初始化
-submodule 后，直接安装这些包：
+### LinkerHand L6 或 O6
+
+只有设置 `hands.enabled=true` 时才需要安装：
 
 ```bash
 git submodule update --init --recursive
 pip install -e third_party/linkerhand-python-sdk
 pip install -e third_party/somehand
-scripts/setup/download_somehand_l6_assets.sh
+bash scripts/setup/download_somehand_assets.sh
 ```
 
-只有在 `hands.enabled=true` 时才需要安装这些包。
+### OpenNeck
 
-### Sim2Real 录制
+`openneck` extra 已经包含 Pico 依赖。启用前先完成标定：
 
 ```bash
-pip install -e '.[recording]'
+pip install -e '.[openneck]'
+openneck calibrate
 ```
 
-该配置包含 Pico sim2real 栈，以及 `sim2real_record.yaml` 使用的视频依赖。
-RealSense Python 绑定与平台相关；使用 `input.video.source=realsense` 时，
-需要在当前环境中手动安装 `pyrealsense2`。在 Arm 机器上，请使用
-conda-forge，而不是 pip 包：
+Teleopit 使用 OpenNeck 的角度接口，不支持旧版归一化标定字段。
+
+### RealSense 录制或视频预览
+
+启用 RealSense 时还需要单独安装 `pyrealsense2`。Arm 设备建议使用
+conda-forge：
 
 ```bash
 conda install -c conda-forge pyrealsense2
 ```
 
-## 验证安装
+Pico 身体追踪本身不依赖 RealSense。
+
+## 7. 检查安装结果
+
+先检查 Teleopit 基础包：
 
 ```bash
 python -c "import teleopit; print('teleopit OK')"
-python -c "import train_mimic.tasks; print('training OK')"  # 仅在安装了训练配置时适用
 ```
 
-## 下一步
+如果安装了 Pico 或训练依赖，再运行对应检查：
+
+```bash
+python -c "from pico_bridge import PicoBridge; print('Pico OK')"
+python -c "import train_mimic.tasks; print('training OK')"
+```
+
+如果安装的是推理环境，并已经下载 `robots gmr ckpt bvh` 资源，最后运行一次示例仿真：
+
+```bash
+python scripts/run/run_sim.py \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
+```
 
-- [下载资源](download-assets) - 下载模型和数据
-- [快速上手](quick-start) - 运行你的第一个仿真
+MuJoCo 窗口能够打开，仿真 G1 能跟随示例动作，就说明安装完成。关闭窗口即可停止，
+然后进入四条任务教程之一。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md
deleted file mode 100644
index 1fee2d36..00000000
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md
+++ /dev/null
@@ -1,63 +0,0 @@
----
-sidebar_position: 3
----
-
-# 快速上手
-
-本指南带你在 5 分钟内完成第一次 sim2sim 回放。
-
-## 前置条件
-
-1. [安装 Teleopit](installation)（推理配置）
-2. [下载资源](download-assets)（`--only robots gmr ckpt bvh`）
-
-## 运行离线 Sim2Sim
-
-```bash
-python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
-    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
-```
-
-运行后你应该能看到 MuJoCo 查看器窗口，展示机器人跟踪 BVH 动作的过程。
-
-## 键盘控制
-
-在启用 `playback.keyboard.enabled=true` 时可使用以下快捷键：
-
-| 按键 | 功能 |
-|------|------|
-| `Space` / `P` | 暂停 / 继续 |
-| `R` | 从头重播 |
-| `Q` | 停止 |
-
-```bash
-python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
-    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
-    playback.keyboard.enabled=true
-```
-
-## 查看器模式
-
-控制显示哪些查看器窗口：
-
-```bash
-# 显示全部查看器（动捕 + 重定向 + sim2sim）
-python scripts/run/run_sim.py controller.policy_path=track.onnx viewers=all
-
-# 无查看器（无头模式）
-python scripts/run/run_sim.py controller.policy_path=track.onnx viewers=none
-
-# 指定查看器
-python scripts/run/run_sim.py controller.policy_path=track.onnx 'viewers=[retarget,sim2sim]'
-```
-
-## 下一步
-
-- [离线 Sim2Sim 教程](../tutorials/offline-sim2sim) - 包含渲染的完整指南
-- [Pico Sim2Sim](../tutorials/pico-sim2sim) - 在 MuJoCo 中验证 Pico 追踪
-- [独立站立测试](../tutorials/standalone-standing) - 检查 G1 bridge、网络和 policy 站立
-- [Pico Sim2Real](../tutorials/pico-sim2real) - 将 Pico 遥操作部署到 Unitree G1
-- [BVH Sim2Real](../tutorials/bvh-sim2real) - 在 Unitree G1 上回放离线 BVH 动作
-- [训练](../tutorials/training) - 训练你自己的策略
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md
index 30fa4e3c..8174e7d0 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md
@@ -3,43 +3,38 @@ sidebar_position: 1
 slug: /
 ---
 
-# 简介
-
-**Teleopit** 是一个轻量、可扩展的人形机器人全身遥操作框架。它能够将人类操作者的动作实时映射到 Unitree G1 机器人上，同时支持 MuJoCo 仿真和实物硬件部署。
-
-## 核心特性
-
-- **离线 sim2sim**：在 MuJoCo 中回放 BVH 动捕文件，通过 RL 策略驱动机器人
-- **VR 遥操作**：基于 Pico 4 / Pico 4 Ultra 全身追踪的实时全身控制
-- **Sim2Real 部署**：使用同一套流程直接部署到 Unitree G1 实物
-- **训练流程**：基于 General-Tracking-G1 任务的端到端强化学习训练
-- **可扩展设计**：基于协议的组件体系（InputProvider、Retargeter、Controller、Robot）
-
-## 流程概览
-
-```text
-InputProvider (BVH / Pico4 VR)
-    -> Retargeter (GMR)
-    -> ObservationBuilder (167D)
-    -> Controller (双输入 TemporalCNN ONNX)
-    -> Robot (MuJoCo 仿真 或 Unitree G1)
-```
-
-## 技术规格
-
-| 项目 | 参数 |
-|------|------|
-| 策略频率 | 50 Hz |
-| PD 控制频率 | 200 Hz |
-| 观测维度 | 167D |
-| 动作维度 | 29D（G1 关节） |
-| ONNX 模型 | 双输入 TemporalCNN |
-| 运动重定向 | GMR（General Motion Retargeting） |
-| 仿真器 | MuJoCo |
-| 硬件平台 | Unitree G1（29 自由度） |
-
-## 下一步
-
-- [安装指南](getting-started/installation) - 搭建开发环境
-- [快速上手](getting-started/quick-start) - 运行你的第一个 sim2sim 示例
-- [教程](tutorials/offline-sim2sim) - 各使用场景的详细步骤指引
+# Teleopit
+
+Teleopit 是一套面向 Unitree G1 的**全具身人形机器人遥操作系统**。操作者戴上支持的
+Pico 头显后，可以实时控制机器人的全身动作。机载部署还可以接入可选的 LinkerHand
+控制手势，并通过可选的 OpenNeck 把头部动作转换为机器人相机朝向。
+
+同一套运控策略会先在 MuJoCo 中运行。你可以先在仿真里确认动作和控制方式，再连接
+真实机器人。
+
+## 从这里开始
+
+第一次使用 Teleopit 时，建议按这个顺序：
+
+1. 根据自己的目标[安装 Teleopit](getting-started/installation)，并完成该页面最后的
+   安装检查。
+2. 从下面四条路径中选择一条继续。
+
+| 我想做什么 | 对应教程 |
+|------------|----------|
+| 在 MuJoCo 中检查运控策略 | [在仿真中运行运控](tutorials/offline-sim2sim) |
+| 不连接真机，先尝试 Pico VR 遥操 | [在仿真中进行 VR 遥操](tutorials/pico-sim2sim) |
+| 使用 Pico VR 控制真实 G1 | [用 VR 遥操真实 G1](tutorials/pico-sim2real) |
+| 训练并导出自己的运控策略 | [训练运控策略](tutorials/training) |
+
+:::warning 连接真机之前
+请先把 Pico 仿真遥操跑通。真机运行时始终把 Unitree 遥控器拿在手里；
+`L1+R1` 是进入 `DAMPING` 的紧急停止方式。
+:::
+
+## 想了解实现细节？
+
+主线教程只保留完成任务所需的内容。运行流程和技术规格见
+[系统架构](reference/architecture)，下载文件与资源分组见
+[资产](reference/resources/assets)，Hydra 参数见
+[配置说明](reference/configuration/overview)。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md
index 5baef6ff..ee24882f 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md
@@ -1,79 +1,148 @@
 ---
-sidebar_position: 1
+sidebar_position: 2
 ---
 
-# 架构
+# 系统架构
 
-面向开发者的系统内部结构和技术约束。
+本页定义 Teleopit 的运行时流程、仓库布局、支持的技术范围和公共入口。
 
 ## Pipeline
 
-```text
-InputProvider（BVH 文件 / Pico4）
-    -> Retargeter（GMR）
-    -> ObservationBuilder（167D）
-    -> Controller（双输入 TemporalCNN ONNX）
-    -> Robot（MuJoCo 仿真或 Unitree G1）
-```
+![Teleopit 运行时流程](/img/diagrams/architecture-pipeline-zh.svg)
 
-离线/在线推理由 `teleopit/runtime/` 和 `teleopit/pipeline.py` 装配。硬件状态机通过 `teleopit/sim2real/mp/` 中的进程隔离运行时执行。训练由 `train_mimic/` 提供。
+全身运控主流程把 BVH 或 PICO 实时身体动作转换为时间对齐的 G1 参考。
+`VelCmdObservationBuilder` 把参考动作与机器人状态组合起来，双输入 TemporalCNN
+ONNX 运控器再输出 29 维关节偏移。同一套观测和运控器路径同时用于 MuJoCo 和真机 G1。
 
-## 代码结构
+Pico 手部和主动视觉路径是可选的进程隔离 worker。它们复用同一个进程内
+`PicoBridge` 接收器，不会向 167 维运控策略观测增加字段。手部或颈部故障不能停止
+G1 身体控制。这些可选硬件路径只支持机载部署；外部主机 Pico 部署只支持全身控制。
 
-```text
-configs / scripts
-    -> runtime
-    -> interfaces + pipeline state machines
-    -> adapters（inputs / retargeting / controller / robot / recording）
-
-train_mimic/scripts
-    -> train_mimic/app.py
-    -> single task registry / env builder / runner cfg
-    -> mjlab / rsl_rl
-
-train_mimic/scripts/data
-    -> train_mimic/data/dataset_builder.py
-    -> dataset_lib / motion_fk / convert_pkl_to_npz
-```
+主机高层策略部署与 Pico 运行时彼此独立。单独的主机环境接收 JPEG RGB、G1 实测关节
+位置、O6 原始实测 readback、OpenNeck 实测角度，以及 observation 时刻的 source
+reference root pose。身体、手部和颈部数组组成 43 维模型观测；session-local source
+pose 只用于重建 source-relative root 输出。主机再通过严格的 ZeroMQ/msgpack 消息返回
+canonical `float32[T,50]` action chunk。机载校验器和调度器把其中的身体部分转换为
+36 维参考，交给现有 motion tracker；主机输出不能绕过 tracker，也不能直接成为电机
+命令。
 
-## 核心模块边界
+Teleopit 和主机环境共享语义数据和一份完全相同的 `hand_calibration.json`，但不会导入
+对方的 Python 包。当前 client/server 代码和协议测试定义网络结构，因此协议变化时
+两个仓库必须同步修改。
 
-| 模块 | 职责 |
-|------|------|
-| `teleopit/interfaces.py` | 稳定协议：InputProvider、Retargeter、Controller、Robot、ObservationBuilder |
-| `teleopit/runtime/` | 配置解析、路径规范化、组件装配、CLI 校验 |
-| `teleopit/pipeline.py` | 离线仿真的轻量 facade |
-| `teleopit/sim2real/mp/` | 进程隔离的 sim2real 状态机、IPC 和机器人控制循环 |
-| `teleopit/controllers/observation.py` | ObservationBuilder |
-| `teleopit/controllers/rl_policy.py` | 接受观测维度与运行时 builder 匹配的双输入 ONNX |
-| `train_mimic/app.py` | 共享的训练/播放/benchmark 装配 |
-| `train_mimic/tasks/tracking/config/` | 单一任务注册（`General-Tracking-G1`） |
-| `train_mimic/data/dataset_builder.py` | 唯一官方数据集构建入口 |
+## 运行时边界
 
-## 技术规格
+- 离线核心组件通过 `InProcessBus` 通信，不复制数组 payload。
+- 真机机器人控制、参考生成、相机、录制、手部、颈部和高层策略客户端在可能阻塞或
+  硬件故障影响 50 Hz 控制循环时使用进程隔离。
+- 本地 sim2real worker 使用 localhost ZeroMQ 和共享内存视频环。
+- 外部主机策略边界使用 msgpack 和非 pickle 的 float32 数组。
+- 共享组件契约是在 `teleopit/interfaces.py` 中定义的 `typing.Protocol`。
 
-| 项目 | 规格 |
-|---|---|
-| 训练任务 | `General-Tracking-G1` |
-| 推理观测 | `velcmd_history`（167D） |
-| ONNX 签名 | 双输入 `obs`（167D）+ `obs_history` |
-| Actor/Critic | TemporalCNN（2048、1024、512、256、128） |
-| 训练采样 | 默认 `rewind`；也支持 `uniform`；播放/评估使用 `start` |
-| 训练 `window_steps` | `[0]` |
-| 数据格式 | 可递归发现的最小 HDF5 shard（`shard_*.h5`） |
+## 仓库布局
 
-## 约束
-
-- 必须显式提供 `controller.policy_path`，且文件必须存在
-- 离线 BVH 运行必须显式提供 `input.bvh_file`
-- `viewers` 是唯一的 viewer 配置入口
-- 观测/ONNX 维度不匹配会在启动时立即报错
-- sim2real 也要求双输入 ONNX，且观测维度必须与运行时 builder 匹配
+```text
+teleopit/                              — 核心推理和部署包
+├── interfaces.py                     — 机器人、运控器、输入和重定向协议
+├── pipeline.py                       — 轻量离线仿真 facade
+├── runtime/                          — 配置/路径解析、工厂和 CLI 校验
+├── configs/                          — Hydra 运行时配置
+├── bus/                              — 进程内零拷贝发布/订阅
+├── inputs/                           — BVH、PICO 和实时输入适配器
+├── retargeting/gmr/                  — 自包含的全身 GMR 实现
+├── controllers/                      — 观测构建器和 ONNX 策略运控器
+├── robots/                           — MuJoCo 机器人适配器
+├── sim/                              — 200 Hz PD / 50 Hz 策略仿真循环
+├── sim2real/
+│   ├── mp/                           — 进程 supervisor、IPC 和机器人控制状态机
+│   ├── hands/                        — 可选 LinkerHand 驱动和输入映射
+│   └── neck/                         — 可选 OpenNeck 映射和 worker
+├── high_level_policy/                — 主机协议、坐标变换和 action 调度器
+└── recording/                        — Sim2real 数据 schema 和录制 worker
+
+train_mimic/                          — 训练包
+├── app.py                            — 共享的训练/播放/benchmark 装配
+├── tasks/tracking/                   — General-Tracking-G1 任务和 TemporalCNN 模型
+├── data/                             — 数据集构建和动作加载
+└── scripts/                          — 训练、播放、benchmark 和 ONNX 导出
+
+scripts/                              — 面向用户的运行和维护入口
+├── run/                              — 仿真、sim2real 和录制命令
+├── setup/                            — 资源下载和硬件设置
+├── render/                           — 离线视频渲染
+├── view/                             — 录制数据检查
+└── dev/                              — 校验和标定工具
+
+third_party/                          — 可选硬件 SDK 和 somehand
+tests/                                — 单元、协议和集成测试
+```
 
-## 公共接口
+## 技术规格
 
-**稳定运行模式：** 离线 sim2sim、离线 sim2real playback、Pico4 sim2sim、G1 sim2real
+| 规格 | 支持值 |
+|------|--------|
+| 机器人 | 29 个执行关节的 Unitree G1 |
+| 仿真器 | MuJoCo |
+| 全身重定向 | GMR（General Motion Retargeting） |
+| 策略 / PD 频率 | 50 Hz / 200 Hz |
+| 训练任务 | `General-Tracking-G1` |
+| 推理观测 | `velcmd_history`（167 维） |
+| ONNX 签名 | 双输入：`obs`（167 维）+ `obs_history` |
+| 策略动作 | 相对 `default_dof_pos` 的 29 维关节偏移 |
+| Actor / critic | TemporalCNN（2048、1024、512、256、128） |
+| 训练采样 | 默认 `rewind`；支持 `uniform`；播放使用 `start`；benchmark 固定精确 clip 并禁用 clip 末尾重采样 |
+| 训练窗口 | `window_steps=[0]` |
+| 分发动作数据 | 递归 minimal HDF5 `shard_*.h5` 文件 |
+| 可选手部 | LinkerHand L6/O6，支持 gripper 或 PICO 手部姿态输入 |
+| 可选主动视觉 | 使用物理角度的 OpenNeck yaw/pitch |
+| 主机策略观测 | JPEG RGB + G1 关节位置（29 维）+ O6 原始 readback（12 维）+ OpenNeck 角度（2 维）；请求还携带相机时刻的 active reference root pose（7 维） |
+| 主机策略动作 | `float32[T,50]`，30 Hz 源时间线，`T` 在 `[1,50]` 内 |
+| 主机策略身体控制 | 36 维根部/关节参考，通过现有 50 Hz motion tracker |
 
-**稳定训练入口：** `train.py`、`play.py`、`benchmark.py`、`save_onnx.py`
+## 约束
 
-**稳定数据入口：** `build_dataset.py`、`precompute_dataset.py`
+- `controller.policy_path` 必须显式提供，并指向现有文件。
+- 离线 BVH 运行必须显式提供现有的 `input.bvh_file`。
+- `viewers` 是唯一的 viewer 配置键。
+- 观测定义必须与 ONNX 签名完全一致；启动时会直接失败，不会 pad 或 trim 数据。
+- `default_dof_pos` 必须来自所选机器人的默认站立角度。
+- sim2real 使用与仿真相同的双输入观测契约。
+- 主机消息 envelope 或 schema 不匹配时会被拒绝，机器人保持在 `STANDING`。shape、
+  有限值、session、sequence、四元数、时效性或安全检查失败时，会拒绝整个 action
+  chunk。
+- 主机 action 在机载侧校验、调度并限速；主机不能绕过 motion tracker 或发送 G1
+  电机命令。
+- 策略 entry 在一个主机 session 等待第一份有效 chunk 时保持为 `STANDING` 内部流程。
+  该 chunk 会直接进入 `POLICY`，不执行候选参考对齐、entry Kp ramp 或第二次
+  session/reset。50 Hz limiter 从 session 开始时捕获的机器人实测参考起步。
+- chunk 边界和内部的根部、yaw 与关节参考时间跳变都会被接受，再由 50 Hz scheduler
+  输出限速，从而保留录制的 pause/resume 转换。
+- PICO 输入、RealSense 预览、录制、手部和颈部故障都是非关键故障；Unitree 遥控器
+  和机器人控制循环仍然可用。
+
+## 公共入口
+
+支持的运行模式包括离线 sim2sim、离线 sim2real 回放、PICO sim2sim、PICO G1
+sim2real，以及独立主机高层策略 G1 sim2real。
+
+运行命令：
+
+- `scripts/run/run_sim.py` — 离线 BVH 和 PICO 实时 sim2sim
+- `scripts/run/run_sim2real.py` — BVH 或 PICO G1 sim2real
+- `scripts/run/run_high_level_policy_sim2real.py` — 独立主机高层策略 G1 部署
+- `scripts/run/record_pico_motion.py` — 从 PICO 录制重定向动作 clip
+- `scripts/render/render_sim.py` — 渲染 mocap、重定向和 sim2sim 视频
+- `scripts/view/view_recording.py` — 检查同步的 sim2real 录制数据
+
+训练和数据命令：
+
+- `train_mimic/scripts/train.py`、`play.py`、`benchmark.py`、`save_onnx.py`
+- `train_mimic/scripts/data/build_dataset.py`
+- `train_mimic/scripts/data/precompute_dataset.py`
+
+公共 Python 接口：
+
+- `teleopit/interfaces.py` 中的协议
+- `TeleopPipeline`
+- `VelCmdObservationBuilder`
+- `RLPolicyController`
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md
new file mode 100644
index 00000000..3f2bae22
--- /dev/null
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md
@@ -0,0 +1,77 @@
+---
+sidebar_position: 4
+---
+
+# 配套项目
+
+Teleopit 集成了四个职责明确的组件，分别负责机器人通信、灵巧手重定向、主动视觉和
+PICO 数据传输。它们位于 `teleopit` Python 包之外，各自维护硬件协议和公共 API。
+
+| 组件 | 源码地址 | 功能 | 在 Teleopit 中的用途 |
+|------|----------|------|-----------------------|
+| G1 Bridge SDK | [Teleopit 源码目录](https://github.com/BotRunner64/Teleopit/tree/master/third_party/g1_bridge_sdk) | 基于 Unitree SDK2、Cyclone DDS 和 pybind11 的原生 C++ bridge | 获取 G1 实时状态和遥控器输入、切换模式，并发送 200 Hz 底层命令 |
+| somehand | [GitHub](https://github.com/BotRunner64/somehand) | 灵巧手动作重定向库 | 把 Pico 实时手部 landmark 映射为 LinkerHand L6/O6 目标 |
+| OpenNeck | [GitHub](https://github.com/BotRunner64/OpenNeck) | 带标定的双轴颈部驱动 | 把物理 yaw/pitch 角度转换为安全的舵机命令 |
+| PICO Bridge | [GitHub](https://github.com/BotRunner64/pico-bridge) | 传输 PICO 追踪和视频的头显应用与 Python 接收器 | 提供身体、手柄、手部和 HMD 帧，并可回传 RGB 视频 |
+
+## G1 Bridge SDK
+
+G1 Bridge SDK 直接维护在 Teleopit 的 `third_party/g1_bridge_sdk` 中，不是单独的
+仓库。安装脚本会下载 [Unitree SDK2](https://github.com/unitreerobotics/unitree_sdk2)，
+然后构建并安装本地 pybind11 扩展：
+
+```bash
+bash scripts/setup/setup_g1_bridge.sh
+```
+
+所有 DDS 发布和订阅都运行在原生 C++ 线程中。Teleopit 的 `UnitreeG1` 适配器通过
+bridge 读取关节状态、基座方向、角速度和无线遥控器输入，并发送带逐关节 PD 增益的
+29 关节位置目标。真机遥操、独立站立测试和主机高层策略部署都使用这个硬件边界。
+
+## somehand
+
+somehand 提供可配置的人手到机器人手部动作重定向。Teleopit 把兼容源码固定为
+`third_party/somehand` Git submodule，并使用 0.3.0 的公共 `somehand.api`。
+
+在 `hands.mode=vr_hand_pose` 下，Teleopit 把 PICO 的 26 关节手部状态转换为 21 个
+landmark，调用 somehand 连续重定向，再把结果发送给 LinkerHand L6 或 O6。Pico
+实时接收和 landmark 转换由 Teleopit 负责，不会启动 somehand 自带的 Pico 输入路径。
+
+安装灵巧手依赖：
+
+```bash
+git submodule update --init --recursive
+pip install -e third_party/linkerhand-python-sdk
+pip install -e third_party/somehand
+```
+
+## OpenNeck
+
+OpenNeck 负责双轴主动视觉云台的串口通信、角度到舵机 step 的转换和标定机械限位。
+Teleopit 支持 OpenNeck 0.2.0 的物理角度 API，并调用 `move_deg()`；已经移除的
+normalized 控制字段不兼容。
+
+在 Pico 遥操中，Teleopit 计算 HMD 相对同帧 `Body.Spine3` 的旋转，应用配置的死区和
+俯仰增益，再由非关键 neck worker 发送 yaw/pitch 角度。主机高层策略部署则发送
+canonical action 中经过校验的颈部字段。
+
+```bash
+pip install -e '.[openneck]'
+openneck calibrate
+```
+
+## PICO Bridge
+
+PICO Bridge 同时包含头显应用和可导入的 Python PC 接收器。Teleopit 支持 0.2.1
+版本，由 `pico4` extra 安装：
+
+```bash
+pip install -e '.[pico4]'
+```
+
+一个进程内 `PicoBridge` 实例为 Teleopit 提供全身、手柄、手部和独立 HMD 数据。
+全身重定向、手部控制和 OpenNeck 共用这个接收器。启用视频后，Teleopit 还可以通过
+`push_video_frame()` 把 MuJoCo 或 RealSense RGB 帧推回头显。
+
+头显 APK 从 [PICO Bridge Releases](https://github.com/BotRunner64/pico-bridge/releases)
+下载。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md
new file mode 100644
index 00000000..98a8f937
--- /dev/null
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md
@@ -0,0 +1,347 @@
+---
+sidebar_position: 2
+---
+
+# 配置字段
+
+本页列出 Teleopit 的全部 Hydra 配置字段。
+
+## 顶层字段
+
+| 字段 | 类型 | 默认值 | 说明 |
+|---|---|---|---|
+| `policy_hz` | int | — | 策略推理频率（Hz） |
+| `pd_hz` | int | `200` | PD 控制器频率（Hz，仅仿真），通常高于 `policy_hz` |
+| `viewers` | str/list | `sim2sim` | 可视化窗口集合：`mocap`、`retarget`、`sim2sim`、`camera`、`all`、`none`。`all` 打开 `mocap`、`retarget` 和 `sim2sim`；如需相机画面需显式加入 `camera` |
+| `realtime` | bool | `false` | 是否启用实时模式（实机部署时需开启） |
+| `num_steps` | int | — | 仿真总步数；设为 `-1` 表示无限运行 |
+| `keyboard.enabled` | bool | `false` | 是否启用 sim2sim 实时键盘模式控制 |
+| `playback.pause_on_end` | bool | `false` | 回放结束后是否暂停（而非退出） |
+| `playback.keyboard.enabled` | bool | `false` | 是否启用键盘控制回放进度 |
+
+## Robot 字段
+
+机器人相关配置位于 `robot/` 子目录。以 `robot/g1.yaml` 为例：
+
+| 字段 | 类型 | 说明 |
+|---|---|---|
+| `type` | str | 写入录制 schema 的稳定机器人类型，G1 为 `unitree_g1_29dof` |
+| `num_actions` | int | 策略输出的动作维度（即受控关节数） |
+| `xml_path` | str | MuJoCo MJCF 模型文件路径 |
+| `d435i_rgb` | camera | G1 MJCF 中的固定 RGB 相机；配合 `viewers=[sim2sim,camera]` 显示画面 |
+| `kps` | list[float] | 各关节的比例增益（P 增益） |
+| `kds` | list[float] | 各关节的微分增益（D 增益） |
+| `default_angles` | list[float] | 默认关节角度（弧度），也是策略动作的零点 |
+| `torque_limits` | list[float] | 各关节的力矩上限 |
+
+## Controller 字段
+
+控制器配置位于 `controller/` 子目录。
+
+| 字段 | 类型 | 说明 |
+|---|---|---|
+| `policy_path` | str | **必填。** 策略模型文件路径（ONNX 格式） |
+| `device` | str | 推理设备，如 `"cpu"` 或 `"cuda:0"` |
+| `action_scale` | float | 动作缩放系数 |
+| `clip_range` | list[float] | 动作裁剪范围，格式为 `[min, max]` |
+| `default_dof_pos` | list[float] | 默认关节位置，用于计算控制目标 |
+
+### 关键说明：`default_dof_pos` 与动作计算
+
+策略输出的 action 是相对于 `default_dof_pos` 的**偏移量**，最终的关节控制目标按如下公式计算：
+
+```
+target = clip(action, clip_range) * action_scale + default_dof_pos
+```
+
+因此，`default_dof_pos` 决定了策略输出的"零点"。如果该值与训练时使用的不一致，策略的行为将完全偏离预期。
+
+## Input 字段
+
+输入源配置位于 `input/` 子目录，不同输入源的字段各异。
+
+### BVH 输入（`input/bvh.yaml`）
+
+| 字段 | 类型 | 说明 |
+|---|---|---|
+| `bvh_file` | str | BVH 文件路径 |
+| `bvh_format` | str | BVH 骨骼格式标识 |
+| `human_format` | str | 人体骨架格式 |
+
+> BVH 输入不设置 `input.provider` — 由配置组名自动推断。
+
+### Pico 4 输入（`input/pico4.yaml`）
+
+| 字段 | 类型 | 默认值 | 说明 |
+|---|---|---|---|
+| `provider` | str | `pico4` | 输入源类型 |
+| `human_format` | str | `pico_bridge` | 重定向骨架格式 |
+| `pico4_timeout` | float | `60` | 等待设备连接的超时时间（秒） |
+| `pico4_buffer_size` | int | `60` | 帧缓冲区大小 |
+| `pause_button` | str | `A` | 用于暂停/恢复的手柄按钮名称 |
+| `pause_debounce_s` | float | `0.25` | 暂停按钮防抖时间 |
+| `arms_button` | str | `B` | Pico 中用于切换 `MOCAP` / `ARMS` 的按钮 |
+| `arms_debounce_s` | float | `0.25` | 双臂模式按钮防抖时间 |
+| `bridge_host` | str | `0.0.0.0` | Teleopit host receiver 绑定地址 |
+| `bridge_port` | int | `63901` | Teleopit host receiver TCP/UDP 端口 |
+| `bridge_discovery` | bool | `true` | 是否启用 pico-bridge 发现广播 |
+| `bridge_advertise_ip` | str/null | `null` | 可选的 host 广播 IP 覆盖 |
+| `bridge_start_timeout` | float | `10.0` | 启动 bridge 的超时时间 |
+| `bridge_history_size` | int | `120` | bridge 保留的 Pico 帧历史长度 |
+| `video.enabled` | bool | `false` | 通过 pico-bridge 0.2.1 将 host 相机预览发送回 Pico |
+| `video.source` | str/null | `null` | 视频源：`mujoco`、`realsense` 或 `test-pattern` |
+| `video.width` / `height` / `fps` | int | `1280` / `720` / `30` | 视频采集/渲染设置 |
+| `video.device` | str/null | `null` | 可选的 RealSense 序列号 |
+
+## Realtime 字段
+
+实时模式相关字段，仅在 `realtime=true` 时生效。
+
+| 字段 | 说明 |
+|---|---|
+| `retarget_buffer_enabled` | 是否启用重定向缓冲 |
+| `retarget_buffer_window_s` | 缓冲窗口大小 |
+| `retarget_buffer_delay_s` | 缓冲延迟 |
+| `reference_steps` | 参考轨迹窗口步数 |
+| `realtime_buffer_warmup_steps` | 播放前预热帧数 |
+| `reference_velocity_smoothing_alpha` | 速度平滑系数 |
+| `reference_anchor_velocity_smoothing_alpha` | 锚点速度平滑系数 |
+
+## Sim2Real 字段
+
+以下字段用于 sim2real 配置（`sim2real.yaml`、`pico4_sim2real.yaml`）。
+
+sim2real 默认使用 `viewers=none`。设置 `viewers=retarget` 可打开一个可选的
+MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all`
+仅用于仿真 viewer。
+
+### 安全相关
+
+| 字段 | 说明 | 默认值 |
+|---|---|---|
+| `startup_ramp_duration` | 进入 `STANDING` 后的 Kp ramp 时长；逐步提高 PD 增益，不改变 policy target | `2.0` |
+| `joint_vel_limit` | 关节速度限制（rad/s），超过时触发急停 | `10.0` |
+| `mocap_switch.check_frames` | 切换到 MOCAP 前所需的连续有效帧数 | `10` |
+| `arm_mocap.controlled_joint_indices` | Pico `ARMS` 模式下由实时 retargeting 驱动的 G1 关节 | `[15..28]` |
+
+### 主机 High-Level Policy（独立 sim2real）
+
+`high_level_policy_sim2real.yaml` 只供
+`scripts/run/run_high_level_policy_sim2real.py` 使用。它会启动 camera、network
+client、robot-control、LinkerHand O6 和 OpenNeck worker；不会启动 PicoBridge、GMR
+或 retarget reference worker。主机 LeRobot 环境保持独立，并且必须跟随当前
+client/server 消息结构与协议测试。唯一共享的数据文件是 `hand_calibration.json`。
+
+| 字段 | 说明 | 默认值 |
+|------|------|--------|
+| `camera.source` | Onboard 策略相机：`realsense`，或仅供集成测试的 `test-pattern` | `realsense` |
+| `camera.width` / `height` / `fps` | 精确的策略图像契约 | `640` / `480` / `30` |
+| `camera.device` | 可选 RealSense 序列号 | `null` |
+| `standing_return_ramp_duration` | 从主动控制返回 `STANDING` 时的 Kp ramp 时长 | `2.0` |
+| `high_level_policy.endpoint` | 主机策略 ZeroMQ TCP endpoint | `tcp://127.0.0.1:5555` |
+| `high_level_policy.task` | reset 和每个 observation 都会发送的非空任务 prompt | `demo` |
+| `high_level_policy.timeout_s` | 单次网络请求 deadline；超时会暂停 `POLICY` | `1.0` |
+| `high_level_policy.reconnect_backoff_s` | 建立新 session 时的重试间隔 | `1.0` |
+| `high_level_policy.replan_steps` | 两次请求之间的最小 30 Hz source-frame 间隔；不得超过主机报告的 horizon | `3` |
+| `high_level_policy.jpeg_quality` | 640x480 RGB 帧的 JPEG 质量 | `90` |
+| `high_level_policy.max_observation_age_s` | 跳过请求前允许的最大 camera/observation age | `0.15` |
+| `high_level_policy.max_result_age_s` | 拒绝已接收结果前允许的最大本地 IPC age | `0.1` |
+| `high_level_policy.entry_timeout_s` | 建立 entry session 并收到其第一份有效 chunk 的最长时间，以及恢复时等待新鲜 chunk 的最长时间 | `5.0` |
+| `high_level_policy.hold_s` | active plan horizon 结束后、action watchdog 暂停 `POLICY` 前保持最终 reference 的 grace period | `3.0` |
+| `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | 可接受的绝对 root 高度范围 | `0.55` / `1.05` |
+| `high_level_policy.safety.max_root_xy_speed_m_s` | 应用于 50 Hz scheduler 输出的 root XY 速度限制 | `2.5` |
+| `high_level_policy.safety.max_root_displacement_m` | 50 Hz 输出 rate limiter 使用的 source-frame 等效 3D root 步长 | `0.1` |
+| `high_level_policy.safety.max_yaw_rate_rad_s` | 应用于 50 Hz scheduler 输出的 root yaw rate 限制 | `2.5` |
+| `high_level_policy.safety.max_joint_rate_rad_s` | 应用于 50 Hz scheduler 输出的单关节 rate 限制 | `10.0` |
+| `high_level_policy.safety.max_joint_projection_rad` | 将 G1 关节 reference 裁剪到位置限位时允许的最大修正量 | `0.1` |
+| `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | OpenNeck yaw 裁剪范围 | `-45` / `45` |
+| `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | OpenNeck pitch 裁剪范围 | `-40` / `40` |
+
+请求循环采用异步 receding-horizon。隔离的 client 最多只有一个 ZeroMQ 请求在途，
+按照配置的 source-frame stride 选择最新的合格 observation，并在主机推理期间继续执行
+当前 action plan。较新的 response 会依据其中回显的 onboard 单调 observation 时间戳
+替换该计划。
+
+当所需修正量不超过 `high_level_policy.safety.max_joint_projection_rad` 时，G1 reference
+joint position 会裁剪到 `real_robot.joint_pos_lower/upper`；更大的修正量会导致 chunk 被拒绝。
+OpenNeck yaw/pitch 会裁剪到配置范围，单纯的 neck 越界不会导致 chunk 被拒绝。由于 canonical
+50D action 的所有字段都处于启用状态，初始运行时要求
+`hands.driver=linkerhand_o6`、左右两只手以及 `neck.driver=openneck`。OpenNeck 策略值
+在 onboard 裁剪并完成 chunk 验证后直接发送给 `move_deg(yaw, pitch)`；不会应用 Pico
+dead-zone 或 pitch-gain 映射。
+
+### 真机 SDK
+
+| 字段 | 说明 | 默认值 |
+|---|---|---|
+| `real_robot.network_interface` | Unitree DDS 通信网络接口。PC 通过网线连接 G1 控制时，用 `ifconfig` 找到这根网线对应的接口名并填写，例如 `enp130s0`；在机器人 onboard 计算机上运行时通常使用 `eth0` | `eth0` |
+| `real_robot.kp_real` | 真机比例增益（各关节） | — |
+| `real_robot.kd_real` | 真机微分增益（各关节） | — |
+| `real_robot.kd_damping` | 阻尼模式 kd | `8.0` |
+| `real_robot.control_mode` | 踝关节控制模式（`PR` = Pitch-Roll） | `PR` |
+| `real_robot.joint_pos_lower` | 关节位置下限（rad） | — |
+| `real_robot.joint_pos_upper` | 关节位置上限（rad） | — |
+
+### 暂停/恢复（Pico sim2real）
+
+实时 Pico 恢复追踪时会先重新居中航向和地面平面位置。操作者应保持静止，并尽量贴近暂停时的姿态，以减少参考突变。
+
+### 灵巧手（Pico sim2real）
+
+`hands.enabled=true` 要求 `input.provider=pico4`，并以本地 editable 方式安装
+`third_party/linkerhand-python-sdk` 和 `third_party/somehand`。启用后，手控会在所有 sim2real 模式中保持生效。
+`gripper` 支持 `linkerhand_l6` 和 `linkerhand_o6`。对应手柄侧面的握持扳机键（grip）
+是安全使能键：保持按住时，食指扳机键（trigger）会在配置的张开和闭合姿态之间插值；
+松开侧面握持扳机键会让该侧手张开。
+`vr_hand_pose` 支持 `linkerhand_l6` 和 `linkerhand_o6`：手部 pose 消失时，对应侧会保持上一条命令；
+所选手的速度会设为最大值；Teleopit 会先将 Pico 手部状态转成 21 个 landmarks，
+再只通过 somehand 0.3.0 公开的 `somehand.api` 调用。
+
+| 字段 | 说明 | 默认值 |
+|---|---|---|
+| `hands.enabled` | 启用可选手部运行时 | `false` |
+| `hands.mode` | `gripper` 或 `vr_hand_pose` | `gripper` |
+| `hands.driver` | 手部设备驱动：`linkerhand_l6` 或 `linkerhand_o6` | `linkerhand_l6` |
+| `hands.sides` | 控制侧 | `[left, right]` |
+| `hands.rate_hz` | gripper 最大命令频率（Hz） | `30.0` |
+| `hands.frame_timeout_s` | 手柄或手部 pose 过期阈值 | `0.3` |
+| `hands.linkerhand_l6.left_can` / `right_can` | 左右手 CAN 通道 | `can0` / `can1` |
+| `hands.linkerhand_l6.speed` | `gripper` 使用的 L6 速度；`vr_hand_pose` 会覆盖为最大速度 | 见配置 |
+| `hands.linkerhand_l6.deadman_threshold` | 启用单侧控制所需的最小 grip 值 | `0.5` |
+| `hands.linkerhand_l6.trigger_deadzone` | trigger 两端死区 | `0.05` |
+| `hands.linkerhand_l6.open_pose` / `close_pose` | L6 的 6 维张开/闭合姿态 | 见配置 |
+| `hands.linkerhand_o6.left_can` / `right_can` | 左右 O6 手 CAN 通道 | `can0` / `can1` |
+| `hands.linkerhand_o6.speed` | `gripper` 使用的 O6 速度；`vr_hand_pose` 会覆盖为最大速度 | 见配置 |
+| `hands.linkerhand_o6.open_pose` / `close_pose` | O6 的 6 维张开/闭合姿态 | 见配置 |
+| `hands.somehand.l6_config_path` | L6 `vr_hand_pose` 使用的 somehand 0.3.0 官方双手 L6 配置 | 见配置 |
+| `hands.somehand.o6_config_path` | O6 `vr_hand_pose` 使用的 somehand 0.3.0 官方双手 O6 配置 | 见配置 |
+| `hands.somehand.rate_hz` | 低延时 `vr_hand_pose` 命令频率（Hz） | `60.0` |
+| `hands.somehand.max_iterations` | `vr_hand_pose` 的 somehand solver 迭代上限 | `12` |
+| `hands.somehand.temporal_filter_alpha` | somehand 输入 landmarks 平滑 alpha；`1.0` 表示关闭平滑延时 | `1.0` |
+| `hands.somehand.output_alpha` | somehand qpos 输出平滑 alpha；`1.0` 表示关闭平滑延时 | `1.0` |
+
+### OpenNeck 主动视觉（Pico sim2real）
+
+`neck.enabled=true` 要求 `input.provider=pico4` 和 `openneck` extra。neck worker
+复用 Teleopit 已有的 Pico receiver，不会启动第二个 `PicoBridge` 或 RealSense 管线。
+OpenNeck 作为非关键 sim2real worker 运行，不会改变策略观测。头部运动来自独立的头显
+`PicoFrame.head.rotation`，并相对于同一个源帧中的 `Body.Spine3` 进行映射。neck 路径
+绝不读取全身动捕的 `Body.Head` 骨架关节；人体模型约束可能使该关节低估极端低头角度。
+头显姿态更新不受 body 重复帧过滤影响。mapper 使用固定的 PICO 中立姿态且不进行颈部侧
+EMA；启动时不会把操作者的第一帧姿态采集为新的零位，因此开始追踪时操作者不需要保持头部
+朝正前方。Teleopit 将受支持的 PICO 约定转换为 OpenNeck 的物理约定——正 yaw 向左转，
+正 pitch 向上看。对原始相对角度应用 `neck.dead_zone_deg` 后，Teleopit 将 pitch 乘以
+`neck.pitch_gain`（默认 `1.4`），而 yaw 仍保持一比一。随后通过 OpenNeck 0.2.0 的
+`move_deg()` 发送得到的物理角度。OpenNeck 负责直驱角度到舵机步数的转换，并将每个目标
+裁剪到标定文件中的机械步数限位。
+
+OpenNeck 0.2.0 标定文件使用 `yaw_center_step`、`yaw_min_step`、
+`yaw_max_step` 和 `yaw_step_sign` 等角度控制字段（pitch 使用对应字段）。不支持以前的
+OpenNeck 归一化配置；运行 `openneck calibrate` 创建当前格式的文件。Teleopit 已移除的
+`neck.yaw_range_deg`、`neck.pitch_range_deg` 和 `neck.invert_*` 键会被拒绝，
+而不是被忽略。
+
+| 字段 | 说明 | 默认值 |
+|---|---|---|
+| `neck.enabled` | 启用可选 OpenNeck worker | `false` |
+| `neck.driver` | 头颈设备驱动插件；当前为 `openneck` | `openneck` |
+| `neck.config_path` | 可选 OpenNeck 0.2.0 角度标定配置路径 | `null` |
+| `neck.port` | 可选串口覆盖，例如 `/dev/ttyACM0` | `null` |
+| `neck.rate_hz` | 最大头颈命令频率（Hz） | `60.0` |
+| `neck.frame_timeout_s` | Pico 头显/Spine3 姿态过期阈值 | `0.2` |
+| `neck.active_modes` | 允许头颈运动的 sim2real 模式 | `[standing, mocap, arms, pause]` |
+| `neck.dead_zone_deg` | yaw/pitch 死区（度） | `0.5` |
+| `neck.pitch_gain` | 死区后应用于头显相对 pitch 的增益 | `1.4` |
+| `neck.center_on_start` / `center_on_shutdown` | worker 启动/关闭时回中云台 | `true` / `false` |
+| `neck.release_on_shutdown` | 关闭后在支持时释放舵机扭矩 | `false` |
+| `neck.dry_run` | 只计算命令，不打开 OpenNeck 硬件 | `false` |
+
+### HDF5 录制（Pico sim2real）
+
+`recording.enabled=true` 只支持 `input.provider=pico4`、
+`input.video.enabled=true`、`input.video.source=realsense`，并且需要交互式终端。
+录制是手动控制：`R` 开始 episode，`S` 保存当前 episode，`D` 丢弃当前 episode，
+`Q` 关闭。可以录制 `STANDING`、`MOCAP`、`ARMS` 和暂停状态的 mocap。
+
+`sim2real_record.yaml` 会同时启用录制和必需的 RealSense `input.video`
+路径。录制不会打开第二路相机，而是消费 `pico_input` 已经产生的同一批帧。
+
+| 字段 | 说明 | 默认值 |
+|---|---|---|
+| `recording.enabled` | 启用手动 HDF5 录制 | `false` |
+| `recording.output_dir` | 数据集根目录 | `data/recordings/sim2real_hdf5` |
+| `recording.task` | 写入 `episodes.jsonl` 的 episode 任务 prompt | `demo` |
+| `recording.fps` | 录制/视频主时钟频率 | `30` |
+| `recording.min_episode_seconds` | 保存时短于该时长的 episode 会被丢弃 | `1.0` |
+| `recording.record_modes` | 允许开始录制和写帧的模式 | `[standing, mocap, arms, pause]` |
+| `recording.camera.key` | RGB 图像数据集 key | `observation.images.d435i_rgb` |
+| `recording.camera.width` / `height` / `fps` | RealSense RGB 采集设置 | `640` / `480` / `30` |
+| `recording.camera.device` | 可选 RealSense 序列号 | `null` |
+| `recording.video.codec` / `quality` / `pixelformat` | MP4 sidecar 编码设置 | `libx264` / `8` / `yuv420p` |
+
+RealSense 帧超时或断连时会在后台重建采集 pipeline，绝不会停止 Pico 输入或 G1
+控制。按 `R` 开始录制前必须存在新鲜相机帧。录制期间一秒内没有新鲜帧时，当前
+episode 会被丢弃；相机恢复后录制仍保持空闲，直到操作员再次按 `R`。
+如果整个 `pico_input` worker 退出，`robot_control` 会继续运行并保持最新命令；
+Unitree 遥控器仍可用于返回 `STANDING` 或请求 `DAMPING`。
+
+录制器会创建一份便于编辑的源数据集：
+
+```text
+recording.output_dir/
+├── schema.json
+├── episodes.jsonl
+├── data/
+│   └── episode_000000.h5
+└── videos/
+    └── d435i_rgb/
+        └── episode_000000.mp4
+```
+
+`schema.json` 保存 FPS、`robot_type`、`hand_type`、`neck_type` 和 feature 定义。
+`robot_type` 来自 `robot.type`；未启用灵巧手时 `hand_type` 为 `none`，否则为
+配置的 `hands.driver`。未启用主动视觉颈部控制时 `neck_type` 为 `none`，否则为
+配置的 `neck.driver`。这些 enabled 标志直接决定是否录制对应的 state 和 action
+字段；没有单独的录制开关。`episodes.jsonl` 每行对应一个已保存的 episode，包含
+`episode_index`、`frames`、可编辑的 `task`、HDF5 路径和视频路径。因此修改任务
+prompt 不需要重写 HDF5 或 MP4。使用相同 schema 再次启动录制时，会从下一个
+episode index 继续追加，并且可以使用不同的 `recording.task`。
+
+该格式有意不兼容之前依赖 HDF5 根属性的布局。请使用空的
+`recording.output_dir`；如果已有 schema 不匹配，录制 worker 会拒绝该数据集并
+退出，不写入 episode。录制属于非关键进程，因此 sim2real 主控制运行时会继续
+运行并报告 worker 故障。在 `episodes.jsonl` 条目提交前中断的 episode 会在下次
+录制 worker 启动时被丢弃，并且不会占用 episode index。
+
+HDF5 datasets：
+
+```text
+frame_index                    int64[N]
+timestamp                      float64[N]
+observation.state              float32[N, 68]
+observation.state.hand         float32[N, 12]  # 仅启用灵巧手时存在
+observation.state.neck         float32[N, 2]   # 仅启用 OpenNeck 时存在
+observation.mode               int8[N]
+action                         float32[N, 36]
+action.hand                    float32[N, 12]  # 仅启用灵巧手时存在
+action.neck                    float32[N, 2]   # 仅启用 OpenNeck 时存在
+```
+
+HDF5 文件仅包含上述逐帧数组，不保存录制元数据根属性。RGB 帧保存在 MP4 中，
+并通过 `episodes.jsonl` 与 episode 关联；不会写入原始 RGB HDF5 dataset。
+
+`observation.state` 的顺序是 `joint_pos(29)`、`joint_vel(29)`、
+`base_quat_wxyz(4)`、`base_ang_vel(3)` 和 `projected_gravity(3)`。
+`observation.state.hand` 是最新的 LinkerHand 硬件回读：
+`left_state(6) + right_state(6)`，使用 SDK 的 0-255 关节数值。
+`observation.state.neck` 是 OpenNeck `read_deg()` 返回的最新舵机位置：
+以度为单位的 `[yaw_deg, pitch_deg]`。
+`observation.mode` 是数值类别：`standing=0`、`mocap=1`、
+`arms=2`、`pause=3`。`action` 是当前 reference qpos：
+`root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`。它是 motion tracker
+消费的高层参考，不是 tracker policy 的原始输出，也不是最终下发给 G1 的关节目标。
+`action.hand` 是手部 worker 最新的 LinkerHand 命令：
+`left_pose(6) + right_pose(6)`，使用 SDK 的 0-255 pose 数值。
+`action.neck` 是 OpenNeck 成功执行命令后返回的最新机械限位裁剪目标：以度为单位的
+`[yaw_deg, pitch_deg]`。正 yaw 向左转，正 pitch 向上看。可达范围来自 OpenNeck
+标定文件，因此录制 schema 中没有固定范围。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/overview.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/overview.md
similarity index 93%
rename from docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/overview.md
rename to docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/overview.md
index 301c7b0c..3ffacf9c 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/overview.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/overview.md
@@ -16,6 +16,7 @@ Teleopit 使用 [Hydra](https://hydra.cc/) 组合配置。大多数运行入口
 | `teleopit/configs/pico4_sim.yaml` | Pico 4 VR sim2sim |
 | `teleopit/configs/sim2real.yaml` | BVH sim2real（Unitree G1 真机） |
 | `teleopit/configs/pico4_sim2real.yaml` | Pico 4 VR sim2real（Unitree G1 真机） |
+| `teleopit/configs/high_level_policy_sim2real.yaml` | 独立主机策略 sim2real（Unitree G1 真机） |
 
 它们会组合以下子配置：
 
@@ -75,4 +76,4 @@ Teleopit 不会静默修补配置错误：
 
 当你遇到配置错误时，应该查找**哪两个组件的定义不一致**。
 
-完整字段参考请查看 [配置参考](config-reference)。
+完整字段参考请查看[配置字段](fields)。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/g1-bridge-sdk.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/g1-bridge-sdk.md
deleted file mode 100644
index 2d2024ab..00000000
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/g1-bridge-sdk.md
+++ /dev/null
@@ -1,59 +0,0 @@
----
-sidebar_position: 4
----
-
-# G1 Bridge SDK
-
-C++ DDS 桥接库，用 pybind11 封装 unitree_sdk2，让 Python 以接近零延迟（< 0.5 ms）访问 Unitree G1 的实时通信接口。
-
-所有 DDS 发布/订阅运行在原生 C++ 线程中，Python 侧只需调用简单的 get/set 方法。
-
-## 依赖
-
-- CMake >= 3.10
-- GCC >= 9.4（支持 C++17）
-- pybind11 >= 2.6
-- Unitree SDK2（已内置于 `third_party/g1_bridge_sdk/thirdparty/unitree_sdk2/`，无需手动安装）
-- Cyclone DDS（unitree_sdk2 依赖）
-
-## 安装
-
-```bash
-bash scripts/setup/setup_g1_bridge.sh
-```
-
-脚本会自动克隆 `unitree_sdk2`、安装 `pybind11` 并编译 C++ 桥接库。
-
-## Python API
-
-```python
-import g1_bridge_sdk
-
-bridge = g1_bridge_sdk.G1Bridge(
-    network_interface="enp130s0",  # PC 上连接 G1 的以太网接口
-    publish_hz=200              # 指令发布频率（默认 200 Hz）
-)
-```
-
-PC 通过网线连接 G1 控制时，先在 PC 上运行 `ifconfig`，填写这根 G1 网线对应的接口名。在机器人 onboard 计算机上运行时，`eth0` 通常就是正确接口。
-
-| 方法 | 说明 |
-|------|------|
-| `wait_for_state(timeout_sec=5.0)` | 阻塞等待第一帧 LowState，超时返回 False |
-| `get_state()` | 返回 `(qpos[29], qvel[29], quat[4], ang_vel[3])` numpy 数组 |
-| `get_state_counter()` | 返回累计收到的 LowState 帧数 |
-| `get_wireless_remote()` | 返回 40 字节无线遥控数据 |
-| `get_mode_machine()` | 返回当前 mode_machine 值 |
-| `set_target(target, kp, kd)` | 设置目标关节位置和 PD 增益（各 29 元素） |
-| `lock_joints()` | 锁定当前关节位置 |
-| `set_damping()` | 切换为阻尼模式（急停用） |
-| `start_publish()` | 启动指令发布线程 |
-| `stop_publish()` | 停止指令发布线程 |
-| `check_mode()` | 查询当前运动模式，返回 `(code, name)` |
-| `select_mode(name)` | 切换运动模式（如 `"ai"`、`"normal"`） |
-| `release_mode()` | 释放当前模式，进入低级控制 |
-
-## 使用场景
-
-- **Pico4 真机遥操作**：`scripts/run/run_sim2real.py`
-- **独立站立测试**：`scripts/run/standalone_standing.py`
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/assets.md
similarity index 63%
rename from docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md
rename to docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/assets.md
index 98ba4a86..a5de0ab2 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/assets.md
@@ -1,18 +1,42 @@
 ---
-sidebar_position: 2
+sidebar_position: 1
 ---
 
-# 资源管理
+# 资产
 
-数据集、checkpoint、机器人模型和演示媒体不进 Git 历史，统一走外部下载。Unitree G1 的 canonical 模型下载到 `assets/robots/unitree_g1/g1_29dof.xml`。
+Teleopit 的 Git 仓库只保存代码，不保存大型机器人 mesh、运控模型和动作数据。
+[安装说明](../../getting-started/installation)给出了每种用户场景最短的下载命令；本页提供
+完整文件清单和维护者说明。
 
 ## 不入库的内容
 
 - `assets/robots/` — canonical 机器人 XML/mesh
 - `teleopit/retargeting/gmr/assets/` — GMR 重定向资源、IK 配置和非 canonical 机器人描述
-- `data/`、checkpoint、缓存等生成产物
+- `data/`、`ckpt/`、checkpoint、缓存等生成产物
 - 演示媒体（`assets/demo.gif`、`assets/demo.mp4`）
 
+## 资源清单
+
+| 资源组 | 下载后的路径 | 用途 |
+|--------|--------------|------|
+| `ckpt` | `ckpt/track_g1.{onnx,pt}`、`ckpt/track_g1_neck_o6.{onnx,pt}` | 可直接运行的推理模型和对应 PyTorch checkpoint |
+| `robots` | `assets/robots/` 下的机器人 XML 变体与 mesh | 训练、MuJoCo 推理、GMR 和数据集 FK |
+| `gmr` | `teleopit/retargeting/gmr/assets/` | 动作重定向模型和 IK 配置 |
+| `bvh` | `data/sample_bvh/*.bvh` | 安装检查和仿真教程使用的示例动作 |
+| `data` | `data/datasets/<dataset>/shard_*.h5` | 用于分发的精简动作数据；训练前需要预计算 |
+
+当前 G1 机器人资源包包括：
+
+| 模型 XML | 配置 |
+|----------|------|
+| `assets/robots/unitree_g1/g1_29dof.xml` | 基础 G1 模型，也是默认值 |
+| `assets/robots/unitree_g1/g1_29dof_dex3.xml` | 带 Dex3 手部几何和惯性参数的 G1 |
+| `assets/robots/unitree_g1/g1_29dof_neck_o6.xml` | 带颈部主动视觉和 O6 手部模型的 G1 |
+
+默认值不是模型白名单。训练可以通过 `--robot_xml` 选择其他与任务兼容的 XML。GMR
+资源目录中的 XML 属于对应的重定向配置，与运行时机器人资源包是两套不同资源。基础
+模型使用 `track_g1` 模型对，颈部加 O6 模型使用 `track_g1_neck_o6` 模型对。
+
 ## 远程仓库
 
 ### ModelScope（默认下载源）
@@ -29,17 +53,17 @@ sidebar_position: 2
 | `12e21/Teleopit-models` | model | checkpoint、GMR retargeting 资源、示例 BVH |
 | `12e21/Teleopit-datasets` | dataset | 训练/验证数据集 |
 
-### 资源组与仓库的对应关系
+### 资源组与仓库对应关系
 
 | 组 | 仓库 | 远端路径 |
 |----|------|---------|
-| `ckpt` | Teleopit-models | `checkpoints/track.onnx`、`checkpoints/track.pt` |
+| `ckpt` | Teleopit-models | `checkpoints/track_g1.{onnx,pt}`、`checkpoints/track_g1_neck_o6.{onnx,pt}` |
 | `robots` | Teleopit-models | `archives/robot_assets.tar.gz` |
 | `gmr` | Teleopit-models | `archives/gmr_assets.tar.gz` |
 | `bvh` | Teleopit-models | `archives/sample_bvh.tar.gz` |
 | `data` | Teleopit-datasets | `data/datasets/*/*.h5`（`lafan1`、`pico_record`、`seed`、`twist2`） |
 
-## 下载
+## 下载行为
 
 使用项目自带的下载脚本（默认从 ModelScope 下载）：
 
@@ -61,8 +85,10 @@ python scripts/setup/download_assets.py --source huggingface
 
 | 远端路径 | 本地路径 |
 |---------|---------|
-| `checkpoints/track.onnx` | `track.onnx` |
-| `checkpoints/track.pt` | `track.pt` |
+| `checkpoints/track_g1.onnx` | `ckpt/track_g1.onnx` |
+| `checkpoints/track_g1.pt` | `ckpt/track_g1.pt` |
+| `checkpoints/track_g1_neck_o6.onnx` | `ckpt/track_g1_neck_o6.onnx` |
+| `checkpoints/track_g1_neck_o6.pt` | `ckpt/track_g1_neck_o6.pt` |
 | `archives/robot_assets.tar.gz` | `assets/robots/`（自动解压） |
 | `archives/gmr_assets.tar.gz` | `teleopit/retargeting/gmr/assets/`（自动解压） |
 | `archives/sample_bvh.tar.gz` | `data/sample_bvh/`（自动解压） |
@@ -84,7 +110,7 @@ python scripts/setup/prepare_modelscope_assets.py --only data
 ```bash
 # 模型仓库
 modelscope upload --repo-type model BingqianWu/Teleopit-models \
-    data/modelscope_upload/checkpoints checkpoints
+    data/modelscope_upload/checkpoints checkpoints --sync
 modelscope upload --repo-type model BingqianWu/Teleopit-models \
     data/modelscope_upload/archives archives
 
@@ -93,6 +119,10 @@ modelscope upload --repo-type dataset BingqianWu/Teleopit-datasets \
     data/modelscope_upload/data data
 ```
 
+checkpoint 上传有意使用 `--sync`。它只会在远端 `checkpoints/` 目录内删除本地不存在的
+旧模型名，不会影响 `archives/`。除非本地 staging 中包含所有需要保留的远端归档，否则
+不要给归档上传命令添加 `--sync`。
+
 ### 第三步：打版本 tag
 
 ModelScope 仅模型仓库支持 tag，数据集仓库不支持。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/dataset.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/motion-datasets.md
similarity index 95%
rename from docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/dataset.md
rename to docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/motion-datasets.md
index 93fb2f21..5fb94e24 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/dataset.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/motion-datasets.md
@@ -1,8 +1,12 @@
 ---
-sidebar_position: 3
+sidebar_position: 2
 ---
 
-# 数据集
+# 动作数据集
+
+动作数据集为运控训练提供参考动作。用于分发的 minimal 格式与预计算训练格式彼此
+独立，训练只能读取后者。同步的机器人状态、参考动作和相机录制见
+[遥操数据集](teleoperation-datasets)。
 
 ## 下载预构建数据集（推荐）
 
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md
new file mode 100644
index 00000000..f215c378
--- /dev/null
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md
@@ -0,0 +1,93 @@
+---
+sidebar_position: 3
+---
+
+# 遥操数据集
+
+遥操数据集是手动录制的 sim2real episode。它同步保存 G1 状态、motion tracker
+消费的参考动作、可选的手部和颈部命令，以及 RealSense RGB 视频。这个格式用于数据
+检查和外部策略开发，不是运控训练使用的动作数据集。
+
+## 录制 Episode
+
+录制只支持带交互终端和新鲜 RealSense 画面的 Pico 机载 sim2real 部署：
+
+```bash
+pip install -e '.[recording]'
+python scripts/run/run_sim2real.py --config-name sim2real_record \
+    controller.policy_path=policy.onnx
+```
+
+手动配置的等价条件是 `recording.enabled=true`、`input.provider=pico4`、
+`input.video.enabled=true` 和 `input.video.source=realsense`。
+
+终端按 `R` 开始一条 episode，按 `S` 保存，按 `D` 丢弃，按 `Q` 关闭运行时。
+`STANDING`、`MOCAP`、`ARMS` 和动捕暂停状态都可以录制。没有新鲜相机帧时不能开始
+录制；录制过程中相机画面超过一秒未更新时，当前 episode 会被丢弃，但 Pico 输入和
+G1 控制继续运行。视频恢复后不会自动重新开始录制。
+
+## 数据集目录
+
+录制程序写出的是一个可编辑数据集，而不是单个包含所有内容的 HDF5：
+
+```text
+data/recordings/sim2real_hdf5/
+├── schema.json
+├── episodes.jsonl
+├── data/
+│   └── episode_000000.h5
+└── videos/
+    └── d435i_rgb/
+        └── episode_000000.mp4
+```
+
+`schema.json` 定义数据集 FPS、`robot_type`、`hand_type`、`neck_type`，以及每个字段的
+shape、dtype、名称和分组。硬件类型必须与当前运行配置一致。
+
+`episodes.jsonl` 是可编辑的 episode 清单。每一行把一条 episode 映射到对应 HDF5
+和 MP4，并保存任务描述。任务文本不会写入 HDF5 attribute，因此修改任务描述不需要
+重写帧数据。
+
+## 帧字段
+
+每个 HDF5 只包含按帧对齐的数组：
+
+| 字段 | Shape | 含义 |
+|------|-------|------|
+| `frame_index` | scalar | 相机/动作帧序号 |
+| `timestamp` | scalar | 单调时钟时间戳，单位为秒 |
+| `observation.state` | `(68,)` | G1 关节状态、基座方向/角速度和投影重力 |
+| `observation.state.hand` | `(12,)`，可选 | 左右 LinkerHand 硬件关节回读 |
+| `observation.state.neck` | `(2,)`，可选 | 以度为单位的 OpenNeck 舵机 yaw/pitch 回读 |
+| `observation.mode` | scalar | `STANDING`、`MOCAP`、`ARMS` 或动捕暂停状态码 |
+| `action` | `(36,)` | motion tracker 使用的根部姿态和 29 关节参考 |
+| `action.hand` | `(12,)`，可选 | 启用手部控制时的左右 LinkerHand 目标 |
+| `action.neck` | `(2,)`，可选 | 经过机械限位后的 OpenNeck yaw/pitch 角度 |
+
+`observation.state` 的顺序为 `joint_pos(29)`、`joint_vel(29)`、
+`base_quat_wxyz(4)`、`base_ang_vel(3)` 和 `projected_gravity(3)`。
+`observation.state.hand` 使用 LinkerHand SDK 的 0-255 关节数值，顺序是左手六个
+通道，然后是右手六个通道。`observation.state.neck` 是 OpenNeck `read_deg()`
+返回的 `[yaw_deg, pitch_deg]`。
+`observation.mode` 使用 `standing=0`、`mocap=1`、`arms=2` 和 `pause=3`。
+`action` 的结构是 `root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`。
+
+相机 RGB 只保存在 MP4 sidecar 中，HDF5 不重复保存 raw image。只有启用对应硬件时，
+才会出现可选 state 和 action 字段。
+
+## 提交与恢复规则
+
+录制器会先提交 HDF5 和视频文件，再向清单追加记录。进程中断后，未提交的 episode
+会在下次录制进程启动时删除，也不会占用 episode 序号。已有 `schema.json` 与当前
+配置不兼容时，只会停止非关键的录制进程，G1 控制会继续运行。
+
+## 检查录制数据
+
+```bash
+python scripts/view/view_recording.py \
+    --recording data/recordings/sim2real_hdf5
+```
+
+播放前，查看器会检查清单路径、HDF5 shape、dtype、有限值和 MP4 对齐。录制数据
+不包含实测根部 XYZ，因此实测机器人会锚定到参考根部位置；这个格式无法评估全局
+根部平移。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md
deleted file mode 100644
index 14a8500a..00000000
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md
+++ /dev/null
@@ -1,173 +0,0 @@
----
-sidebar_position: 5
----
-
-# 训练问题排查
-
-常见训练问题及解决方案。
-
-:::info
-训练流程见[训练教程](../tutorials/training)，数据准备见[数据集参考](dataset)。
-:::
-
----
-
-## 问题 1：Mean Episode Length = 1.00（机器人第一步就终止）
-
-### 现象
-
-- `Mean episode length: 1.00`
-- `Episode_Termination/anchor_pos` 接近并行环境总数
-- `Metrics/motion/error_anchor_pos` > 0.5 m
-- `Metrics/motion/error_body_rot` 很大（接近 pi）
-
-### 根本原因
-
-通常不是 PPO 超参数问题，而是 **motion NPZ 的监督标签与 MuJoCo FK 不一致**：
-
-1. **body 位置坐标系错误**：把局部坐标当世界坐标使用
-2. **body 顺序错误**：使用 PKL 的 38-body 顺序而非 mjlab G1 的 30-body 顺序
-3. **body 朝向/角速度标签错误**：所有 body 近似为 root 朝向
-
-当前版本的 `convert_pkl_to_npz.py` 已修复上述问题。
-
-### 快速排查
-
-```bash
-python train_mimic/scripts/data/check_motion_npz_fk.py \
-    --npz data/lafan1_clips/lafan1/<clip>.npz
-```
-
-推荐判据：`pos_max < 1e-3 m`、`quat_mean < 0.05 rad`、`quat_p95 < 0.10 rad`。
-
-如果检查失败，重新生成数据并做一次 smoke test：
-
-```bash
-python train_mimic/scripts/train.py \
-    --num_envs 64 --max_iterations 100 \
-    --motion_file data/datasets/<dataset>_precomputed
-```
-
-预期：`Mean episode length` 明显大于 1，`error_anchor_pos` 开始下降。
-
----
-
-## 问题 2：Episode Length 不增长
-
-### 现象
-
-训练 1000+ 轮后，`Mean episode length` 仍然很低（< 3），无上升趋势。
-
-### 可能原因
-
-1. Retargeting 质量差（目标姿态不可达）
-2. Tracking reward 权重过低，正则化权重过高
-3. 学习率过大/过小，clip_param 不匹配
-4. 终止条件过严
-
-### 排查步骤
-
-1. 用 `play.py` 可视化参考运动
-2. 检查奖励分布——tracking reward 应占主导
-3. 临时增大 `bad_anchor_pos` 阈值（0.25m → 0.5m）
-4. 对比 mjlab 内置的 G1 tracking task
-
----
-
-## 问题 3：训练速度慢
-
-### 现象
-
-训练速度 < 1000 steps/s（RTX 4090 预期 1500-2000 steps/s）。
-
-### 解决方案
-
-1. 增加 `--num_envs` 到 4096（需要 24 GB 显存）
-2. 训练时关闭 `--video`
-3. 使用 TensorBoard 替代 W&B（默认即 TensorBoard）
-
----
-
-## 问题 4：`nefc overflow - please increase njmax`
-
-### 现象
-
-```text
-nefc overflow - please increase njmax to 257
-```
-
-### 根本原因
-
-MuJoCo 约束缓冲区不足。机器人跌倒或大量接触时，活跃约束数超出 `njmax`。`mjlab` 训练默认 `sim.njmax=250`。
-
-### 解决方案
-
-仓库中已修复。`train_mimic/tasks/tracking/config/env.py` 的 env builder 覆盖了训练仿真参数：
-
-```python
-self.sim.njmax = 500
-self.sim.nconmax = 150_000
-```
-
-如果警告仍出现在更高数值，增加到 `njmax = 800`。
-
-:::note
-仅修改机器人 XML 不够——`mjlab` 的仿真层 `njmax` 才是实际生效的参数。
-:::
-
----
-
-## 问题 5：Benchmark 视频问题
-
-### 视频只有 1 帧
-
-确保 `num_eval_steps >= video_length`：
-
-```bash
-python train_mimic/scripts/benchmark.py \
-    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
-    --motion_file data/datasets/<dataset>_precomputed \
-    --num_envs 1 --num_eval_steps 2000 \
-    --video --video_length 600
-```
-
-### EGL/OpenGL 错误
-
-安装 OpenGL/EGL 依赖：
-
-```bash
-conda install -c conda-forge libopengl libglx libegl libglvnd pyopengl
-```
-
-如果 GPU EGL 不可用，尝试 CPU 渲染：
-
-```bash
-MUJOCO_GL=osmesa PYOPENGL_PLATFORM=osmesa \
-    python train_mimic/scripts/benchmark.py ... --video
-```
-
----
-
-## 问题 6：Sim2Sim 脚滑（Benchmark 正常但 ONNX 推理脚打滑）
-
-### 根本原因
-
-sim2sim 配置参数与训练环境不一致：
-
-1. **`default_angles` 不匹配（最关键）**：不同的关节默认值导致动作偏移和观测误差
-2. **缺少 joint armature**：训练环境有非零 armature，零 armature 导致过冲
-3. **condim 不一致**：训练和 sim2sim 之间碰撞参数不同
-
-### 诊断方法
-
-```python
-from mjlab.asset_zoo.robots import get_g1_robot_cfg
-cfg = get_g1_robot_cfg()
-print(cfg.init_state.joint_pos)  # 必须与 g1.yaml default_angles 一致
-```
-
-### 解决方案
-
-更新 `teleopit/configs/robot/g1.yaml` 和 `assets/robots/unitree_g1/g1_29dof.xml`，使其与训练环境的值一致（default angles、armature、condim）。
-
-此修复同时影响 sim2real 路径，因为 `default_angles` 被 `rl_policy.py` 和 `observation.py` 共用。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/bvh-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/bvh-sim2real.md
index 59acadaa..7ad1a507 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/bvh-sim2real.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/bvh-sim2real.md
@@ -34,7 +34,7 @@ real_robot.network_interface=enp130s0
 
 ```bash
 python scripts/run/run_sim2real.py \
-    controller.policy_path=track.onnx \
+    controller.policy_path=ckpt/track_g1.onnx \
     real_robot.network_interface=enp130s0 \
     input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
 ```
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md
new file mode 100644
index 00000000..0e700042
--- /dev/null
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md
@@ -0,0 +1,217 @@
+---
+sidebar_position: 4
+---
+
+# 从遥操数据到模仿学习 / VLA 真机部署
+
+本教程串联完整工作流：使用 Teleopit 录制 Pico 示教，在 `lerobot-teleopit` 中训练
+ACT 或 GR00T N1.7 策略，再把训练结果运行到 Unitree G1 真机上。
+
+```text
+Pico 示教
+  -> Teleopit v4 录制数据
+  -> LeRobot Dataset
+  -> ACT 或 GR00T checkpoint
+  -> 主机策略服务
+  -> Teleopit onboard motion tracker
+  -> G1 + LinkerHand O6 + OpenNeck
+```
+
+Teleopit 负责录制和实时机器人控制；
+[`lerobot-teleopit`](https://github.com/BotRunner64/lerobot-teleopit)
+负责数据转换、模型训练和主机策略服务。两个仓库使用相互独立的 Python 环境；主机发送
+reference motion，而不是 G1 电机命令。
+
+## 开始之前
+
+- [Unitree G1 VR 遥操作](pico-sim2real)已经可靠运行。
+- Onboard 配置包含两只 LinkerHand O6、OpenNeck 和一台 RealSense RGB 相机。
+  当前训练与部署路径要求这些硬件全部存在。
+- Onboard 计算机已经按照[安装指南](../getting-started/installation)安装 recording、
+  OpenNeck、LinkerHand 和 somehand 支持，并且存在
+  `ckpt/track_g1_neck_o6.onnx`。
+- 使用相同 G1 网络接口和底层 tracking policy 时，
+  [独立站立测试](standalone-standing)已经稳定运行。
+- 主机工作站已经单独按照
+  [`lerobot-teleopit` 安装指南](https://github.com/BotRunner64/lerobot-teleopit#installation)
+  准备完成。
+
+:::danger 始终把 Unitree 遥控器拿在手中
+动作异常时立即按 `L1+R1` 进入 `DAMPING`。确保机器人周围有足够的安全空间，
+安排另一人随时准备扶住或停止机器人，并且不要同时运行两个可能向 G1 发送命令的程序。
+:::
+
+## 1. 录制并检查示教
+
+在 G1 onboard 计算机上运行录制配置。下面的示例使用 Pico 手部姿态重定向；
+如果示教需要使用手柄扳机，请改用 `hands.mode=gripper`。
+
+```bash
+python scripts/run/run_sim2real.py \
+  --config-name sim2real_record \
+  controller.policy_path=ckpt/track_g1_neck_o6.onnx \
+  real_robot.network_interface=eth0 \
+  hands.enabled=true \
+  hands.driver=linkerhand_o6 \
+  hands.mode=vr_hand_pose \
+  neck.enabled=true \
+  recording.output_dir=data/recordings/my_task \
+  recording.task="pick up the object"
+```
+
+使用 G1 遥控器进入 `MOCAP` 或 `ARMS`，然后操作录制终端：
+
+| 按键 | 动作 |
+|------|------|
+| `R` | RealSense 有新鲜画面后，开始一条 episode |
+| `S` | 保存当前 episode |
+| `D` | 丢弃当前 episode |
+| `Q` | 关闭运行时 |
+
+每个 dataset 只录制一项任务，并保持 `recording.task` 一致。只保存成功示教，
+同时覆盖有意义的初始姿态、物体位置和执行速度变化。
+
+训练前检查同步视频、实测状态和 reference：
+
+```bash
+python scripts/view/view_recording.py \
+  --recording data/recordings/my_task
+```
+
+出现追踪丢失、相机中断或不安全 reference 时，应丢弃对应 episode。录制 schema
+和恢复规则见[遥操数据集](../reference/resources/teleoperation-datasets)。
+
+## 2. 将 Dataset 交给 `lerobot-teleopit`
+
+把完整录制目录复制到主机，不要展开目录层级或修改其中的名称。典型 source 目录为：
+
+```text
+lerobot-teleopit/data/raw/my_task/
+├── schema.json
+├── episodes.jsonl
+├── data/
+└── videos/d435i_rgb/
+```
+
+当前转换器要求 Teleopit v4 dataset 包含 LinkerHand O6 和 OpenNeck 的 state/action
+字段；缺失字段会被拒绝，不会自动补齐。如果同一任务录制在多个目录中，请在转换前使用
+主机仓库的 `merge_raw_datasets.py` 工具合并。
+
+后续所有主机命令都应在独立的 `lerobot-teleopit` 环境中运行。其
+[数据转换与训练指南](https://github.com/BotRunner64/lerobot-teleopit/blob/main/docs/training-entrypoint.zh-CN.md)
+包含依赖、合并选项、训练规模、多 GPU 设置和实验日志说明。
+
+## 3. 在主机上转换并训练
+
+最短的数据转换命令为：
+
+```bash
+python scripts/convert_dataset.py \
+  --source data/raw/my_task \
+  --output data/lerobot/my_task \
+  --repo-id local/my_task \
+  --workers 4
+```
+
+选择一种训练命令。训练 ACT：
+
+```bash
+python scripts/train_policy.py \
+  --policy act \
+  --dataset-root data/lerobot/my_task \
+  --devices 0
+```
+
+训练 GR00T N1.7：
+
+```bash
+python scripts/train_policy.py \
+  --policy groot \
+  --dataset-root data/lerobot/my_task \
+  --devices 0,1,2,3
+```
+
+追加 `--dry-run` 可以检查最终启动配置，而不实际开始训练。如果没有设置
+`--output-dir`，训练结果会写入 `outputs/train/`。可部署产物是对应 run 下的
+`checkpoints/last/pretrained_model/` 目录。
+
+## 4. 使用 ReplayPolicy 验证真机链路
+
+加载 learned checkpoint 前，先从主机回放一条已经录制的 episode：
+
+```bash
+python scripts/run_policy_server.py \
+  --backend replay \
+  --dataset-root data/lerobot/my_task \
+  --repo-id local/my_task \
+  --episode 0 \
+  --start-frame 0 \
+  --chunk-size 15 \
+  --bind tcp://0.0.0.0:5555
+```
+
+只在可信的机器人网络上绑定 `0.0.0.0`。该服务没有身份验证，不得暴露到公网。
+
+在 G1 onboard 计算机上启动 Teleopit 专用运行时。将 `HOST_IP` 替换为工作站地址，
+并使用与 dataset 一致的任务描述：
+
+```bash
+python scripts/run/run_high_level_policy_sim2real.py \
+  controller.policy_path=ckpt/track_g1_neck_o6.onnx \
+  high_level_policy.endpoint=tcp://HOST_IP:5555 \
+  high_level_policy.task="pick up the object" \
+  real_robot.network_interface=eth0
+```
+
+进程启动后，机器人保持 `IDLE`。使用 Unitree 遥控器操作：
+
+| 操作 | 动作 |
+|------|------|
+| `Start` | 进入 `STANDING` |
+| `Y` | 创建策略 session；第一份有效 chunk 会进入 `POLICY` |
+| `B` | 暂停，或在新鲜 chunk 可用后恢复 |
+| `X` | 结束 session 并返回 `STANDING` |
+| `L1+R1` | 立即进入 `DAMPING` |
+
+ReplayPolicy 应当足够准确地重现录制 reference，以验证网络、action 约定和 onboard
+执行链路。如果回放不正确，请在这里停止。Learned policy 无法修复录制、转换、坐标或
+底层 tracking 问题。
+
+## 5. 部署训练好的策略
+
+按 `X` 让 G1 返回 `STANDING`，然后停止 ReplayPolicy。在主机上使用
+`pretrained_model` 目录本身启动 learned-policy server：
+
+```bash
+python scripts/run_policy_server.py \
+  --backend lerobot \
+  --checkpoint outputs/train/<run>/checkpoints/last/pretrained_model \
+  --device cuda \
+  --bind tcp://0.0.0.0:5555
+```
+
+ACT 和 GR00T 使用相同的 server 命令。按 Unitree 遥控器 `Y` 创建新的策略 session。
+开始时使用训练分布内熟悉的场景，并只允许幅度小、容易恢复的动作。
+
+如果需要记录一次运行中交换的 observation 和 action，增加以下主机参数：
+
+```bash
+--record-dir outputs/policy-recordings
+```
+
+Teleopit 会在 50 Hz motion tracker 使用之前校验并限速每一份 plan。格式错误的输出
+会被拒绝，不会被补齐或删减。主机、网络、相机或 action watchdog 故障会暂停 session
+并保持最后一条命令，不会自动进入 `STANDING`。恢复故障链路后按 `B` 继续，或根据情况
+使用 `X` 或 `L1+R1`。
+
+## 常见问题
+
+| 现象 | 检查项 |
+|------|--------|
+| 按 `Y` 后始终不进入 `POLICY` | 主机 IP 和防火墙、server 日志、新鲜的 RealSense 画面、两边匹配的代码版本，以及完全一致的 `hand_calibration.json` |
+| `POLICY` 进入暂停 | 主机推理延迟、请求超时、相机/结果过期、action watchdog，或必要 worker 退出 |
+
+模型 action 坐标和主机端行为见
+[`lerobot-teleopit` Action Space 指南](https://github.com/BotRunner64/lerobot-teleopit/blob/main/docs/planar-relative-root-actions.zh-CN.md)。
+Onboard 时序和安全设置见
+[配置字段](../reference/configuration/fields#主机-high-level-policy独立-sim2real)。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md
index b5731659..b33ef0ff 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md
@@ -2,89 +2,125 @@
 sidebar_position: 1
 ---
 
-# 离线 Sim2Sim
+# 在仿真中运行运控
 
-在 MuJoCo 仿真环境中，使用 BVH 动捕文件驱动 RL 策略进行全身运动复现。
+本教程让训练好的运控策略在 MuJoCo 中复现一段动作。在接入 VR 或真实机器人之前，
+先用它确认两个最基本的问题：
 
-## 基本播放
+- 运控模型能否正常加载，并让 G1 保持稳定？
+- 重定向后的机器人动作是否与原始动作一致？
+
+## 开始之前
+
+按照[安装说明](../getting-started/installation)安装基础依赖，并下载
+`robots gmr ckpt bvh` 资源包。
+
+## 1. 运行示例动作
 
 ```bash
 python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
-    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
+    playback.keyboard.enabled=true
 ```
 
-### 使用 hc_mocap 格式
+最重要的是 `sim2sim` 窗口：它显示的是运控策略和物理仿真共同产生的 G1 动作，而不是
+单纯的运动学目标。
+
+| 按键 | 作用 |
+|------|------|
+| `Space` 或 `P` | 暂停或继续 |
+| `R` | 从第一帧重新播放 |
+| `Q` | 停止 |
+
+机器人能够保持稳定，并大致跟上动作的节奏和姿态，就说明运行正常。少量跟踪误差是正常
+的；摔倒、关节不动或朝向明显错误则不是。
+
+## 2. 对比三个视图
+
+动作异常时，打开全部视图可以判断问题从哪一步开始：
 
 ```bash
 python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
-    input.bvh_file=data/hc_mocap/walk.bvh \
-    input.bvh_format=hc_mocap
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
+    viewers=all
 ```
 
-## 键盘交互重播
+| 视图 | 显示内容 |
+|------|----------|
+| `mocap` | 从 BVH 中读取的人体骨架 |
+| `retarget` | GMR 生成的 G1 运动学目标 |
+| `sim2sim` | 经过运控推理和 MuJoCo 物理后的 G1 |
 
-为离线 BVH 播放启用键盘交互控制：
+如果 `mocap` 就不对，先检查 BVH 格式；如果 `mocap` 正常但 `retarget` 不对，检查动作
+重定向；如果只有 `sim2sim` 不对，检查运控模型和观测配置。
+
+也可以只打开需要的视图：
 
 ```bash
+# 只看物理仿真结果
 python scripts/run/run_sim.py \
-    controller.policy_path=track.onnx \
+    controller.policy_path=ckpt/track_g1.onnx \
     input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
-    playback.keyboard.enabled=true
-```
+    viewers=sim2sim
 
-| 按键 | 功能 |
-|------|------|
-| `Space` / `P` | 暂停 / 继续 |
-| `R` | 从头重播 |
-| `Q` | 停止 |
+# 不打开窗口，适合服务器或时序测试
+python scripts/run/run_sim.py \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \
+    viewers=none
+```
 
-其他可选参数：
+关闭所有已打开的 Viewer 后，仿真会自动结束。
 
-```bash
-# 动作播放结束后自动暂停
-playback.pause_on_end=true
+## 3. 使用自己的 BVH
 
-# 限制仿真步数（0 = 无限）
-num_steps=300
+LAFAN1 格式：
 
-# 按真实时间速率播放（即使无 Viewer 窗口也生效）
-realtime=true
+```bash
+python scripts/run/run_sim.py \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=/path/to/motion.bvh \
+    input.bvh_format=lafan1
 ```
 
-## Viewer 模式
-
-Viewer 以独立子进程运行。使用 shell 引号传递列表参数。
+`hc_mocap` 格式：
 
 ```bash
-viewers=sim2sim          # 默认模式
-viewers=all              # mocap + retarget + sim2sim 三视图
-viewers=none             # 无头模式（不显示窗口）
-'viewers=[retarget,sim2sim]'  # 自定义组合
+python scripts/run/run_sim.py \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=/path/to/motion.bvh \
+    input.bvh_format=hc_mocap
 ```
 
-:::note
-当所有 Viewer 窗口被关闭后，仿真会自动结束。
-:::
+Teleopit 不会猜测未知的骨架布局。一个文件即使是合法 BVH，也可能需要先写适配器才能
+作为支持的格式使用。
 
-## 离线渲染
+## 4. 保存视频
 
-在无头模式下将仿真渲染为视频：
+需要可重复的视频结果而不是交互窗口时：
 
 ```bash
 MUJOCO_GL=egl python scripts/render/render_sim.py \
     --bvh data/sample_bvh/aiming1_subject1.bvh \
-    --policy track.onnx
+    --policy ckpt/track_g1.onnx
 ```
 
-使用 hc_mocap 格式时：
+`hc_mocap` 输入需要再加 `--format hc_mocap`。渲染脚本会输出同步的 `mocap`、
+`retarget` 和 `sim2sim` 视频。
+
+## 常用播放参数
 
 ```bash
-MUJOCO_GL=egl python scripts/render/render_sim.py \
-    --bvh data/hc_mocap/wander.bvh \
-    --format hc_mocap \
-    --policy track.onnx
+# 动作结束后保持最后姿态
+playback.pause_on_end=true
+
+# 运行 300 个仿真 step；0 表示不限制
+num_steps=300
+
+# 即使不打开 Viewer，也按照真实时间运行
+realtime=true
 ```
 
-渲染管线输出三个视角（动捕输入、重定向、sim2sim），均通过 MuJoCo 渲染。
+完整字段见[配置说明](../reference/configuration/overview)。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md
index de155bcb..e5bd2d81 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md
@@ -1,302 +1,226 @@
 ---
-sidebar_position: 4
+sidebar_position: 3
 ---
 
-# Pico 4 VR 真机遥操作
+# 用 VR 遥操真实 G1
 
-在 [Pico Sim2Sim](pico-sim2sim) 跑通后，使用本教程把同一条实时 Pico 输入路径部署到
-真实 Unitree G1。
+本教程会把已经在 MuJoCo 中验证过的 Pico 流程迁移到真实 Unitree G1。先确定 Teleopit
+运行在哪里，再验证站立运控，最后才把机器人交给实时身体追踪。
 
-```text
-Pico 头显 -> Teleopit host -> retarget -> RL policy -> g1_bridge_sdk -> G1
-```
-
-有两种部署方式：
-
-| 部署方式 | Teleopit 运行位置 | 主要区别 |
-|----------|-------------------|----------|
-| Wired PC-to-G1 | 外部工作站或笔记本 | 将 `real_robot.network_interface` 设置为 PC 上连接 G1 的以太网接口 |
-| Onboard | G1 onboard 计算机 | 在 onboard 计算机安装 Teleopit；通常使用 `eth0` |
+:::danger 始终把 Unitree 遥控器拿在手里
+动作异常时立即按 `L1+R1` 进入 `DAMPING`。清空机器人周围空间，并安排另一名人员
+随时扶住或停止机器人。
+:::
 
-两种方式都使用 `Pico4InputProvider` 和进程内 pico-bridge receiver。不存在单独的
-onboard Pico 输入模式。
+## 选择部署方式
 
-Teleopit 面向 pico-bridge 0.2.1 及其 `pico_native` tracking 语义。
+### 外部主机部署：仅全身追踪
 
-## 1. 安装运行时依赖
+Teleopit 运行在工作站或笔记本电脑上，电脑通过网线连接 G1；Pico 头显需要能够通过
+网络访问这台电脑。
 
-在运行 Teleopit 的机器上安装 Pico 和 sim2real 依赖：
+这种部署方式只用于 G1 全身控制，请保持 LinkerHand、OpenNeck、RealSense 画面和
+数据录制关闭。先查看连接 G1 的有线网卡：
 
 ```bash
-pip install -e '.[pico4]'
-git submodule update --init --recursive
-bash scripts/setup/setup_g1_bridge.sh
+ifconfig
 ```
 
-验证 Pico receiver 导入：
+在后面的命令中填写这块网卡的名称。本文以 `enp130s0` 为例。
 
-```bash
-python -c "from pico_bridge import PicoBridge; print('OK')"
-```
+### 机载电脑部署：完整具身能力
 
-## 2. 选择网络接口
+如果还需要 LinkerHand、OpenNeck、RealSense 画面或数据采集，请直接在 G1 机载电脑
+上运行 Teleopit。Pico 头显需要能够访问机载电脑。
 
-`real_robot.network_interface` 是用于 Unitree DDS 通信的 Linux 网卡接口。
+机载配置同时使用 O6 双手和 OpenNeck 时，请将底层运控策略设为
+`controller.policy_path=ckpt/track_g1_neck_o6.onnx`。
 
-对于 wired PC-to-G1 部署：
+G1 DDS 默认使用 `eth0`。除了网络接口和可选机载硬件配置之外，全身控制的配置和启动
+命令与外部主机部署相同。
 
-1. 用网线连接 PC 和 G1。
-2. 在 PC 上运行 `ifconfig`。
-3. 使用连接到机器人的以太网接口，例如 `enp130s0`。
-4. 确保 Pico 头显所在网络可以访问运行 Teleopit 的 PC。
+## 开始之前
 
-对于 onboard 部署：
+请确认以下条件全部满足：
 
-1. 在机器人 onboard 计算机上运行 Teleopit。
-2. 确保 Pico 头显所在网络可以访问 onboard 计算机。
-3. 除非机器人网络不同，否则使用 `real_robot.network_interface=eth0`。
-4. 如果 Pico discovery 广播了错误地址，设置 `input.bridge_advertise_ip=<host-ip>`。
+- [在仿真中进行 VR 遥操](pico-sim2sim)已经稳定运行；
+- 已按照[安装](../getting-started/installation)安装 `pico4` 依赖并编译
+  `g1_bridge_sdk`；
+- 已准备好 `ckpt/track_g1.onnx`、机器人文件和 GMR 资源；
+- 运行 Teleopit 的设备已经通过有线 DDS 网络连接 G1；
+- 没有其他程序正在控制机器人。
 
-### Arm Onboard 的 RealSense 配置
+## 1. 检查站立运控
 
-pico-bridge PC receiver 在所需 Python 依赖可用时支持 Arm 机器。对于需要 RealSense
-预览的 Arm onboard 计算机，应在当前 Conda 环境中从 conda-forge 安装 `pyrealsense2`，
-不要依赖 pip 包：
+先只检查机器人状态接收和策略频率，不发送电机命令。外部主机需要把 `enp130s0`
+替换为 `ifconfig` 查到的有线网卡：
 
 ```bash
-pip uninstall pyrealsense2
-conda install -c conda-forge pyrealsense2
+python scripts/run/standalone_standing.py \
+    --policy ckpt/track_g1.onnx \
+    --network-interface enp130s0 \
+    --dry-run
 ```
 
-这只影响可选的 RealSense 预览路径（`input.video.enabled=true`）。Pico 追踪和机器人控制
-本身不需要 RealSense。
-
-## 3. 运行控制器
+在机载电脑上运行时，使用 `--network-interface eth0`。
 
-Wired PC 示例：
+Dry run 成功后，在确保硬件安全的情况下去掉 `--dry-run` 再运行一次：
 
 ```bash
-python scripts/run/run_sim2real.py \
-    --config-name pico4_sim2real \
-    controller.policy_path=track.onnx \
-    real_robot.network_interface=enp130s0
+python scripts/run/standalone_standing.py \
+    --policy ckpt/track_g1.onnx \
+    --network-interface enp130s0
 ```
 
-Onboard 示例：
+站立运控不稳定时不要继续接入 Pico，请先按照
+[单独测试站立运控](standalone-standing)排查。
 
-```bash
-python scripts/run/run_sim2real.py \
-    --config-name pico4_sim2real \
-    controller.policy_path=track.onnx \
-    real_robot.network_interface=eth0
-```
-
-## 可选 HDF5 录制
+## 2. 启动 Pico 真机遥操
 
-在负责 Pico 输入和 RealSense 的机器上安装 recording extra：
+外部主机示例：
 
 ```bash
-pip install -e '.[recording]'
+python scripts/run/run_sim2real.py \
+    --config-name pico4_sim2real \
+    controller.policy_path=ckpt/track_g1.onnx \
+    real_robot.network_interface=enp130s0
 ```
 
-运行录制配置：
+机载电脑示例：
 
 ```bash
 python scripts/run/run_sim2real.py \
-    --config-name sim2real_record \
-    controller.policy_path=track.onnx \
-    real_robot.network_interface=enp130s0 \
-    recording.task="walk forward"
+    --config-name pico4_sim2real \
+    controller.policy_path=ckpt/track_g1.onnx \
+    real_robot.network_interface=eth0
 ```
 
-终端控制为：`R` 开始 episode，`S` 保存，`D` 丢弃，`Q` 关闭。可以录制
-`STANDING`、`MOCAP`、`ARMS` 和暂停状态的 mocap；已经保存的 episode 不支持再丢弃。
-episode 会保存为 `data/recordings/sim2real_hdf5/episodes/` 下的 `.h5` 文件，
-压缩 MP4 sidecar 视频保存在 `data/recordings/sim2real_hdf5/videos/` 下。
-HDF5 episode 以 30 Hz 保存 `frame_index` 和 `timestamp` 同步数组，以及
-`observation.state(68)`、`observation.mode(1)`、`action(36)` 和
-`action.hand(12)`。
-
-## 操作流程
-
-始终把 Unitree 遥控器拿在手里。`L1+R1` 是进入 `DAMPING` 的急停路径。
-
-| 控制 | 动作 |
-|------|------|
-| Unitree remote `Start` | 进入 `STANDING` |
-| Unitree remote `Y` | 进入 `MOCAP` |
-| Pico/controller `A` | 暂停 / 恢复实时动捕 |
-| Pico/controller `B` | 在 `MOCAP` / `ARMS` 之间切换 |
-| Unitree remote `X` | 返回 `STANDING` |
-| Unitree remote `L1+R1` | 急停（`DAMPING`） |
-
-只在 Pico 追踪稳定后进入 `MOCAP`。Teleopit 会在切换前验证连续动捕帧；验证失败时，
-机器人会保持在 `STANDING`。
-
-## 运行时行为
+程序启动后不会立即让 Pico 接管机器人。
 
-Pico sim2real 使用共享的实时参考时间线：
+## 3. 按 G1 状态机操作
 
-```text
-Pico body frames -> retarget -> reference buffer -> observation -> policy -> G1 joints
-```
-
-进入 `STANDING` 时，Teleopit 会释放当前 Unitree 模式，进入 debug/low-level 控制，
-短暂锁住当前关节，重置 policy 状态，并在不改变 policy target 的情况下执行 Kp ramp。
+![Pico G1 控制状态机](/img/diagrams/pico-g1-state-machine-zh.svg)
 
-进入 `MOCAP` 时，Teleopit 会重新 arm 进程隔离的 reference worker，重置其中的 GMR 状态
-和实时 reference buffer，然后等待新的已验证 reference，再开始跟踪实时 mocap 命令。
-`STANDING` 和 `DAMPING` 会让 reference worker 保持 disarmed，避免冷启动帧在进入 mocap
-之前 warm-start retargeting。
+图中的 **G1 遥控器**表示 Unitree 遥控器，**Pico 手柄**表示 VR 手柄。电脑键盘不负责
+切换真机状态。
 
-`ARMS` 会保持同一条实时 retargeting 时间线继续运行，但发送给 motion tracker 的参考会被组合：
-身体、腰部和腿部保持站立姿态，双臂跟随实时 retarget 结果。进入或离开 `ARMS` 时会重置
-policy/reference 对齐，并使用同一套 Kp ramp 安全路径。
+先按 **G1 遥控器** `Start` 进入 `STANDING`。等机器人站稳，以中立姿态站好，并确认
+Pico 追踪有效。然后按 **G1 遥控器** `Y` 进入 `MOCAP`，从缓慢的小幅动作开始。需要
+结束 VR 会话时，按 **G1 遥控器** `X` 返回 `STANDING`。
 
-## 暂停 / 恢复
+`MOCAP` 控制全身。`ARMS` 会让身体、腰和腿保持站立，只有双臂继续跟随。
+`PAUSED` 保持当前参考姿态，恢复后回到暂停前的 `MOCAP` 或 `ARMS`。
 
-Pico 暂停/恢复是 mocap-session control event。
+进入 `MOCAP` 前，Teleopit 会连续检查多帧 Pico 数据。检查没有通过时，机器人会继续
+停留在 `STANDING`。
 
-- `ACTIVE`：暂停键冻结当前参考姿态。
-- `PAUSED`：再次按下会清空 policy/reference 状态，预热实时 buffer，重新居中 yaw/XY 对齐，
-  并从实时 mocap 恢复。
-
-:::warning
-恢复时请保持静止，并尽量接近暂停时的姿态。这样可以减少实时追踪恢复时的参考突变。
+:::tip 暂停和恢复
+G1 遥控器 `B` 或 Pico 手柄 `A` 会暂停、恢复当前会话。恢复时请保持静止，并尽量接近
+暂停时的姿态。需要结束会话时，请使用 G1 遥控器 `X`。
 :::
 
-## 可选 LinkerHand 控制
-
-Pico sim2real 可以用 Pico 输入控制 LinkerHand：
+如果 Pico 输入中断，全身运控会保持最后一个参考，G1 遥控器仍然可用。按 `X` 返回
+`STANDING`，或按 `L1+R1` 进入 `DAMPING`；不要等待系统自动切换状态。
 
-- `gripper`：按住同侧 grip 作为 deadman，同侧 trigger 控制对应手闭合。
-  该模式支持 `hands.driver=linkerhand_l6` 和 `hands.driver=linkerhand_o6`；
-  速度和张开/闭合姿态来自对应 driver 配置。
-- `vr_hand_pose`：只支持 L6，通过 somehand 重定向 Pico 手部 pose，并下发连续 L6 手部目标。
-  如果某侧手部 pose 消失，该侧会保持上一条手势命令。这个模式使用 Teleopit 的
-  Pico landmark 适配器和 somehand 0.2.0 公开的 `somehand.api`，并始终将 L6
-  速度设为最大值。默认配置使用 60 Hz 的低延时 somehand 路径并减少平滑，所以响应会更快，
-  但可能比标准 somehand 设置更抖。
-
-`hands.enabled=true` 时，手控会在所有 sim2real 模式中保持生效。退出和手控运行时失败会发送配置的张开姿态。
-
-如果主 Pico profile 没有包含手控支持，先安装本地手控包：
-
-```bash
-git submodule update --init --recursive
-pip install -e third_party/linkerhand-python-sdk
-pip install -e third_party/somehand
-scripts/setup/download_somehand_l6_assets.sh
-```
+## 仅机载：LinkerHand
 
-测试或运行手控前，先开启 CAN 接口：
+只有 LinkerHand 已连接到机载电脑时才需要本节。先按照
+[安装](../getting-started/installation)安装手部依赖，再启用两路 CAN：
 
 ```bash
 sudo /usr/sbin/ip link set can0 up type can bitrate 1000000
 sudo /usr/sbin/ip link set can1 up type can bitrate 1000000
 ```
 
-启用完整 sim2real 前，先用独立开合测试验证灵巧手连接。测试默认一直运行到 Ctrl-C：
+启动 G1 运控前，先单独测试双手：
 
 ```bash
-python scripts/dev/test_linkerhand_l6.py \
+python scripts/dev/test_linkerhand.py \
+    --driver linkerhand_o6 \
     --hand-type both \
     --left-can can0 \
     --right-can can1
 ```
 
-O6 独立开合测试需要加上 O6 driver：
+在真机启动命令后追加以下参数，即可启用 O6 手部姿态控制：
 
-```bash
-python scripts/dev/test_linkerhand_l6.py \
-    --driver linkerhand_o6 \
-    --hand-type both \
-    --left-can can0 \
-    --right-can can1
+```text
+hands.enabled=true
+hands.driver=linkerhand_o6
+hands.mode=vr_hand_pose
+hands.linkerhand_o6.left_can=can0
+hands.linkerhand_o6.right_can=can1
 ```
 
-如果要用实时 Pico gripper 输入测试 O6，再加 `--mode gripper`。
+使用 `hands.mode=gripper` 时，需要按住对应手柄侧面的握持扳机键（grip）才会启用
+该侧手部控制；保持按住后，再用食指扳机键（trigger）控制闭合程度。松开侧面握持
+扳机键会让该侧手张开。LinkerHand L6 也受支持，对应参数为
+`hands.linkerhand_l6.*`。
 
-然后在 Pico sim2real 中启用 L6 gripper 控制：
+## 仅机载：OpenNeck
+
+安装并校准 OpenNeck：
 
 ```bash
-hands.enabled=true
-hands.driver=linkerhand_l6
-hands.mode=gripper
-hands.linkerhand_l6.left_can=can0
-hands.linkerhand_l6.right_can=can1
+pip install -e '.[openneck]'
+openneck calibrate
 ```
 
-O6 gripper 控制使用：
+然后在真机启动命令后追加：
 
-```bash
-hands.enabled=true
-hands.driver=linkerhand_o6
-hands.mode=gripper
-hands.linkerhand_o6.left_can=can0
-hands.linkerhand_o6.right_can=can1
+```text
+neck.enabled=true
+neck.port=/dev/ttyACM0
 ```
 
-连续 VR 手部 pose 控制使用：
+OpenNeck 会根据 Pico 头显相对操作者上身的运动转动，并复用已有的 Pico 接收程序。
 
-```bash
-hands.enabled=true
-hands.driver=linkerhand_l6
-hands.mode=vr_hand_pose
-hands.linkerhand_l6.left_can=can0
-hands.linkerhand_l6.right_can=can1
+## 仅机载：RealSense 画面
+
+安装 `pyrealsense2`，再追加：
+
+```text
+input.video.enabled=true
+input.video.device=<可选的-realsense-序列号>
 ```
 
-## 可选 RealSense 预览
+相机画面会发送到头显。相机超时后会在后台重连，不会停止 Pico 追踪或 G1 运控。
 
-将 G1 RealSense 彩色相机推送回 Pico 头显：
+## 仅机载：录制和查看数据
+
+录制前必须能够收到新的 RealSense RGB 帧：
 
 ```bash
 python scripts/run/run_sim2real.py \
-    --config-name pico4_sim2real \
-    controller.policy_path=track.onnx \
-    real_robot.network_interface=enp130s0 \
-    input.video.enabled=true \
-    input.video.device=<optional-realsense-serial>
+    --config-name sim2real_record \
+    controller.policy_path=ckpt/track_g1.onnx \
+    real_robot.network_interface=eth0 \
+    recording.task="向前走"
 ```
 
-如果视频失败，控制会继续运行，除非设置了 `input.video.fail_on_error=true`。
+在终端按 `R` 开始一个 episode，按 `S` 保存，按 `D` 丢弃，按 `Q` 关闭程序。如果
+一秒内没有收到新的相机帧，当前 episode 会被丢弃，但机器人运控会继续。视频恢复后
+需要手动重新开始录制。
 
-## 常用参数
+查看已保存的数据：
 
 ```bash
-# G1 DDS 网卡接口
-real_robot.network_interface=enp130s0
-
-# Pico 超时时间
-input.pico4_timeout=30
-
-# 覆盖 Pico discovery 广播 IP
-input.bridge_advertise_ip=192.168.1.20
+pip install -e '.[review]'
+python scripts/view/view_recording.py \
+    --recording data/recordings/sim2real_hdf5
+```
 
-# 进入 MOCAP 前要求的连续有效动捕帧数
-mocap_switch.check_frames=10
+查看器会同步显示相机视频、G1 实测与参考姿态，以及可选的手部和头部信号。字段说明
+见[遥操数据集](../reference/resources/teleoperation-datasets)。
 
-# 更换 Pico 暂停键
-input.pause_button=right_axis_click
+## 常见问题
 
-# 开启 LinkerHand gripper 控制
-hands.enabled=true
-hands.driver=linkerhand_l6
-hands.mode=gripper
+| 问题 | 解决方法 |
+|------|----------|
+| Arm 设备上 RealSense 无法使用 | 先运行 `pip uninstall pyrealsense2` 删除 PyPI wheel，再运行 `conda install -c conda-forge pyrealsense2` 安装 conda-forge 提供的 Arm 构建 |
 
-# 开启头显视频预览
-input.video.enabled=true
-```
+## 其他 G1 工作流
 
-## 故障排查
-
-| 现象 | 可能原因 | 解决方法 |
-|------|----------|----------|
-| 没有收到 LowState | 网卡错误或 G1 网络未连接 | 检查网线和 `real_robot.network_interface` |
-| `TimeoutError: No Pico4 body data` | 头显未连接或追踪未激活 | 检查头显 app、网络和 `input.pico4_timeout` |
-| 无法进入 debug mode | Unitree mode 释放失败 | 停止其他机器人模式后再次按 `Start` |
-| 机器人进入 `STANDING` 但不进入 `MOCAP` | 动捕验证失败 | 保持追踪稳定，查看 `mocap_switch.check_frames` 日志 |
-| Pico 暂停没有返回 `STANDING` | 这是预期行为 | Pico 暂停只冻结 mocap；按遥控器 `X` 返回 `STANDING` |
-| LinkerHand 不动 | `hands.enabled=false`、gripper deadman 未按住、SDK/资产未安装，或 CAN 通道错误 | 设置 `hands.enabled=true` 和 `hands.mode`，运行 `scripts/dev/test_linkerhand_l6.py`，并检查所选 driver 的 `left_can` / `right_can` |
-| 视频预览不可用 | RealSense 或视频源失败 | 检查相机权限、`input.video.source` 和日志 |
+- [单独测试站立运控](standalone-standing)
+- [在 Unitree G1 上回放 BVH](bvh-sim2real)
+- [从遥操数据到模仿学习 / VLA 真机部署](high-level-policy-sim2real)
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md
index 02eb6143..ce3f6276 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md
@@ -2,141 +2,147 @@
 sidebar_position: 2
 ---
 
-# Pico 4 VR 仿真遥操作
+# 在仿真中进行 VR 遥操
 
-使用本教程在接入真实 Unitree G1 之前，先在 MuJoCo 中验证 Pico 4 / Pico 4 Ultra
-全身追踪。
+连接真实机器人之前，先用 Pico 控制仿真 G1。不要跳过这一步：头显、网络和身体追踪
+问题都可以在这里解决，不会给硬件带来风险。
 
-```text
-Pico 头显 -> pico-bridge receiver -> retarget -> RL policy -> MuJoCo G1
-```
-
-此流程跑通后，再继续阅读 [Pico Sim2Real](pico-sim2real)。
-
-## 支持设备
+## 支持的头显
 
 - Pico 4
 - Pico 4 Ultra
+- Pico 4 Ultra Enterprise
+- Pico 4 Pro
+
+所有设备都需要开启全身追踪，并使用支持当前身体追踪接口的 Pico 系统版本。
+
+## 开始之前
+
+你需要：
+
+- 头显和运行 Teleopit 的电脑处于同一网络；
+- 已安装 `pico4` 依赖并下载 `robots gmr ckpt bvh` 资源；
+- [在仿真中运行运控](offline-sim2sim)已经正常。
 
-## 1. 设置头显
+## 1. 准备头显
+
+1. 从 [pico-bridge Releases](https://github.com/BotRunner64/pico-bridge/releases)
+   下载头显 APK。
+2. 安装 APK：
 
-1. 从 [pico-bridge Releases](https://github.com/BotRunner64/pico-bridge/releases) 下载头显 APK。
-2. 使用 adb 安装：
    ```bash
    adb install pico-bridge.apk
    ```
-3. 启动 pico-bridge 头显 client。
+
+3. 在头显中打开 pico-bridge。
 4. 开启全身追踪。
-5. 确保头显和 Teleopit host 在同一网络。
 
-## 2. 安装 Pico Host Extra
+Teleopit 使用 pico-bridge 0.2.1。接收程序会直接运行在 Teleopit 进程中，不需要再
+启动一个单独的转发程序。
 
-在运行 Teleopit 的机器上执行：
+## 2. 检查电脑是否收到 Pico 数据
 
-```bash
-pip install -e '.[pico4]'
-```
-
-验证 receiver 包：
+下面的诊断只打印身体帧和连接状态，不会启动机器人运控：
 
 ```bash
-python -c "from pico_bridge import PicoBridge; print('OK')"
+python scripts/dev/test_pico_bridge.py --no-video
 ```
 
-Teleopit 会通过 `Pico4InputProvider` 在进程内启动 `pico_bridge.PicoBridge`。
-后续 wired 和 onboard sim2real 部署也使用同一条 Pico 输入路径。
-
-Teleopit 面向 pico-bridge 0.2.1 及其 `pico_native` tracking 语义。
+轻微移动身体，确认终端持续收到新的有效帧。按 `Ctrl+C` 结束诊断。
 
-## 3. 下载资源
+如果自动发现选择了错误的网卡地址，显式指定头显能够访问的地址：
 
 ```bash
-pip install modelscope
-python scripts/setup/download_assets.py --only robots gmr ckpt bvh
+python scripts/dev/test_pico_bridge.py \
+    --no-video \
+    --bridge-advertise-ip=192.168.1.20
 ```
 
-## 4. 运行 Pico Sim2Sim
+## 3. 启动仿真
 
 ```bash
 python scripts/run/run_sim.py \
     --config-name pico4_sim \
-    controller.policy_path=track.onnx
+    controller.policy_path=ckpt/track_g1.onnx
 ```
 
-仿真从 `STANDING` 开始。等待 Pico 追踪激活后，再进入 `MOCAP`。
+机器人会有意从 `STANDING` 开始；只有操作者主动切换后，实时身体追踪才会接管。
 
-| 键盘 | 动作 |
-|------|------|
-| `Y` | 进入 `MOCAP` |
-| `A` | 暂停 / 恢复实时动捕 |
-| `B` | 在 `MOCAP` / `ARMS` 之间切换 |
-| `X` | 返回 `STANDING` |
-| `Q` | 退出 |
+## 4. 按状态机操作
 
-`pico4_sim.yaml` 默认使用 `viewers=all`，会打开 mocap、retarget 和 sim2sim
-三个 viewer。需要更少窗口时，可使用 `viewers=sim2sim` 或 `viewers=none`。
+![Pico 仿真控制状态机](/img/diagrams/pico-sim-state-machine-zh.svg)
 
-## 暂停 / 恢复
+图中的**键盘**表示运行 Teleopit 的电脑键盘，**Pico 手柄**表示 VR 手柄。仿真过程
+不使用 Unitree G1 遥控器。
 
-Pico 暂停/恢复会冻结 mocap session；它不是切回 `STANDING`。
+以舒适的中立姿态站好，等待追踪稳定，再按**键盘** `Y` 进入 `MOCAP`。先从小幅慢动作
+开始。按**键盘** `X` 结束 VR 会话并返回 `STANDING`；在任意状态按**键盘** `Q`
+退出仿真。
 
-- 按键盘 `A` 或 Pico/controller 暂停键，冻结当前参考姿态。
-- 再按一次会重建实时参考路径，重新居中 yaw 和地面平面位置，然后从当前实时追踪流继续。
+`MOCAP` 控制全身。`ARMS` 会让身体、腰和腿保持站立，只有双臂继续跟随。
+`PAUSED` 保持当前参考姿态，恢复后返回之前的 `MOCAP` 或 `ARMS`。
 
-默认 Pico 暂停键是 `A`。支持的覆盖值包括 `B`、`X`、`Y`、`left_axis_click`、
-`right_axis_click`、`left_menu_button` 和 `right_menu_button`。
+每次重新从 `STANDING` 进入 `MOCAP` 时，系统都会重新对齐实时根部姿态。操作者可以
+在站立状态改变朝向，再重新进入 `MOCAP`。
 
-默认 Pico 双臂模式按钮是 `B`。`ARMS` 会让身体、腰部和腿部保持站立姿态，同时双臂跟随
-实时 retarget 结果。
+:::tip 暂停不等于结束 VR 控制
+键盘或 Pico 手柄 `A` 只会冻结并恢复当前动捕姿态。需要结束会话并回到站立时，
+请按键盘 `X`。
+:::
 
-## 可选头显视频预览
+## 选择 Viewer 布局
 
-pico-bridge 0.2.1 可以在头显中显示 host 侧视频流。在仿真中，Teleopit 可以推送
-MuJoCo `d435i_rgb` 相机：
+Pico 仿真默认会打开动捕、重定向和物理仿真三个视图。不再需要全部视图时，可以减少窗口：
 
 ```bash
+# 只看物理仿真结果
 python scripts/run/run_sim.py \
     --config-name pico4_sim \
-    controller.policy_path=track.onnx \
-    input.video.enabled=true
+    controller.policy_path=ckpt/track_g1.onnx \
+    viewers=sim2sim
+
+# 不打开窗口
+python scripts/run/run_sim.py \
+    --config-name pico4_sim \
+    controller.policy_path=ckpt/track_g1.onnx \
+    viewers=none
 ```
 
-使用 `input.video.source=test-pattern` 可以做 receiver 侧视频 sanity check。如果视频启动失败，
-Teleopit 会记录错误、关闭视频，并继续运行追踪和控制。设置
-`input.video.fail_on_error=true` 可改为启动失败。
+## 可选：头显视频
 
-## 常用参数
+把仿真的 `d435i_rgb` 相机画面发送回头显：
 
 ```bash
-# 等待第一帧 Pico body 数据的超时时间
-input.pico4_timeout=30
+python scripts/run/run_sim.py \
+    --config-name pico4_sim \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.video.enabled=true
+```
 
-# 覆盖 discovery 广播给头显的 IP
-input.bridge_advertise_ip=192.168.1.20
+使用 `input.video.source=test-pattern` 可以只检查视频链路。视频失败时预览会关闭，但
+身体追踪和运控会继续运行。
 
-# 关闭 discovery 并显式绑定
-input.bridge_discovery=false input.bridge_host=0.0.0.0 input.bridge_port=63901
+## 网络参数
 
-# 更换 Pico 暂停键
-input.pause_button=right_axis_click
+大部分网络只需要自动发现。诊断显示网络有问题时再使用这些参数：
 
-# 关闭键盘模式控制
-keyboard.enabled=false
+```bash
+# 向头显广播指定的电脑地址
+input.bridge_advertise_ip=192.168.1.20
 
-# 修改策略频率
-policy_hz=30
+# 关闭自动发现并显式绑定
+input.bridge_discovery=false
+input.bridge_host=0.0.0.0
+input.bridge_port=63901
 
-# 开启头显视频预览
-input.video.enabled=true
+# 延长等待第一帧身体数据的时间
+input.pico4_timeout=30
 ```
 
-## 故障排查
+## 常见问题
+
+| 问题 | 解决方法 |
+|------|----------|
+| 收不到身体帧 | 把 Pico 头显升级到最新可用的系统版本，重启头显，重新开启全身追踪，然后再次运行 `scripts/dev/test_pico_bridge.py --no-video` |
 
-| 现象 | 可能原因 | 解决方法 |
-|------|----------|----------|
-| `ImportError: pico_bridge` | 未安装 Pico extra | 执行 `pip install -e '.[pico4]'` |
-| 启动提示 pico-bridge 太旧 | 已安装 receiver 不支持所需 API 或 tracking 语义 | 重新安装 Pico extra，确保使用 pico-bridge 0.2.1 |
-| `TimeoutError: No Pico4 body data` | 头显未连接或 body tracking 未激活 | 检查头显 app、网络和 `input.pico4_timeout` |
-| discovery 找不到 host | 广播 IP 不对或 UDP 被阻断 | 设置 `input.bridge_advertise_ip=<host-ip>`，确认 UDP 端口 `63901` 可达 |
-| 仿真机器人不跟随 | 循环仍在 `STANDING` | 追踪准备好后按 `Y` |
-| Pico 视频黑屏或被关闭 | 视频源失败或相机不可访问 | 检查 `input.video.source` 和日志 |
+这条流程稳定后，再继续[用 VR 遥操真实 G1](pico-sim2real)。
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/standalone-standing.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/standalone-standing.md
index 54bb06e6..02ba0151 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/standalone-standing.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/standalone-standing.md
@@ -34,7 +34,7 @@ bash scripts/setup/setup_g1_bridge.sh
 
 ```bash
 python scripts/run/standalone_standing.py \
-    --policy track.onnx \
+    --policy ckpt/track_g1.onnx \
     --network-interface enp130s0 \
     --dry-run
 ```
@@ -45,7 +45,7 @@ python scripts/run/standalone_standing.py \
 
 ```bash
 python scripts/run/standalone_standing.py \
-    --policy track.onnx \
+    --policy ckpt/track_g1.onnx \
     --network-interface enp130s0
 ```
 
@@ -53,7 +53,7 @@ python scripts/run/standalone_standing.py \
 
 ```bash
 python scripts/run/standalone_standing.py \
-    --policy track.onnx \
+    --policy ckpt/track_g1.onnx \
     --network-interface eth0
 ```
 
@@ -64,7 +64,7 @@ standalone standing 复用 sim2real standing 组件：`UnitreeG1Robot`、
 
 ```bash
 python scripts/run/standalone_standing.py \
-    --policy track.onnx \
+    --policy ckpt/track_g1.onnx \
     --network-interface eth0 \
     --kp-ramp-duration 2.0 \
     --kp-ramp-floor-ratio 0.1
diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md
index 1de8d8e1..98c44861 100644
--- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md
+++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md
@@ -1,39 +1,72 @@
 ---
-sidebar_position: 5
+sidebar_position: 4
 ---
 
-# 训练
+# 训练运控策略
 
-训练全身追踪策略，并导出为 ONNX 格式用于推理部署。
+本教程从已下载的动作数据开始，最终得到可以在 Teleopit 仿真和真实 G1 上运行的
+ONNX 运控模型。
 
-:::info
-数据准备请参阅 [数据集参考](../reference/dataset)。常见训练问题请参阅 [训练故障排查](../reference/training-troubleshooting)。
-:::
+常规训练流程默认使用 NVIDIA GPU。动作数据会在启动时全部加载到内存，因此合并数据集
+越大，需要的内存和显存也越多。
 
-## 环境安装
+## 开始之前
 
-```bash
-conda create -n teleopit python=3.10
-conda activate teleopit
-pip install -e '.[train]'
-```
+按照[安装说明](../getting-started/installation)完成：
+
+- `train` 依赖；
+- `robots data` 资源包。
+
+检查训练包：
 
-验证安装：
 ```bash
 python -c "import train_mimic.tasks; print('training OK')"
 ```
 
-下载分发的最小数据集，并生成合并后的预计算训练数据集：
+## 1. 预处理已下载的数据集
+
+下载的数据是便于分发的精简版本。训练需要另一个目录，其中提前计算好了关节速度和
+身体运动学信息：
 
 ```bash
-python scripts/setup/download_assets.py --only robots data
 python train_mimic/scripts/data/precompute_dataset.py \
-    data/datasets --outdir data/datasets_precomputed --jobs 8
+    data/datasets \
+    --outdir data/datasets_precomputed \
+    --jobs 8
+```
+
+下面所有训练、回放和 benchmark 命令都使用 `data/datasets_precomputed`。把原始
+`data/datasets` 直接传给训练会报错，这不是支持的快捷方式。
+
+自定义 BVH、PKL、NPZ 或 Pico 录制数据的处理方法见
+[动作数据集](../reference/resources/motion-datasets)。
+
+## 2. 选择机器人模型
+
+使用 `--robot_xml` 指定训练所用的 MuJoCo 模型。如果省略该参数，默认使用：
+
+```text
+assets/robots/unitree_g1/g1_29dof.xml
 ```
 
-## 训练
+当前 `robots` 资源包提供了以下可以直接使用的示例：
+
+| 模型 XML | 配置 |
+|----------|------|
+| `assets/robots/unitree_g1/g1_29dof.xml` | 基础 G1 模型，也是默认值 |
+| `assets/robots/unitree_g1/g1_29dof_dex3.xml` | 带 Dex3 手部几何和惯性参数的 G1 |
+| `assets/robots/unitree_g1/g1_29dof_neck_o6.xml` | 带颈部主动视觉和 O6 手部模型的 G1 |
+
+这个表只是当前资源包随附的模型示例，不是写死的模型白名单。只要关节和刚体定义与所选
+训练任务配置及数据集兼容，也可以传入其他模型 XML。
 
-### 冒烟测试
+下面“开始完整训练”的主命令会显式选择基础模型，便于直接复制。训练其他模型时，
+替换 `--robot_xml` 后的路径即可。其他命令不再重复该参数；回放和 benchmark 会从
+所选任务配置中加载机器人。
+
+## 3. 先做短时间冒烟测试
+
+开始长时间训练前，先确认数据集、仿真器和日志工具能够一起工作：
 
 ```bash
 python train_mimic/scripts/train.py \
@@ -42,103 +75,110 @@ python train_mimic/scripts/train.py \
     --motion_file data/datasets_precomputed
 ```
 
-### 完整训练
+只要环境能够持续 step、终端输出 loss，并且
+`logs/rsl_rl/g1_general_tracking/` 下生成新的运行目录，这项检查就通过了。
+
+## 4. 开始完整训练
 
 ```bash
 python train_mimic/scripts/train.py \
+    --robot_xml assets/robots/unitree_g1/g1_29dof.xml \
     --num_envs 4096 \
     --max_iterations 30000 \
     --motion_file data/datasets_precomputed
 ```
 
-### 多卡训练
+显存不足时降低 `--num_envs`。默认日志工具是 TensorBoard；需要时可使用
+`--logger wandb` 或 `--logger swanlab`。
+
+`--max_iterations` 表示继续训练多少次。例如从 `model_12000.pt` 恢复并设置
+`--max_iterations 18000`，最终会训练到第 30000 次。
+
+## 5. 在仿真中查看 checkpoint
 
 ```bash
-python train_mimic/scripts/train.py \
-    --gpu_ids 0 1 2 3 \
-    --num_envs 1024 \
-    --max_iterations 30000 \
+python train_mimic/scripts/play.py \
+    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
     --motion_file data/datasets_precomputed
 ```
 
-### 多机多卡训练
+回放会从每段动作开头开始，并关闭训练噪声。导出前先用它排除明显不稳定的模型。
 
-跨多台机器训练时，直接使用 `torchrun`：
+## 6. 运行 Benchmark
 
 ```bash
-torchrun \
-    --nnodes=$PET_NNODES \
-    --nproc_per_node=$PET_NPROC_PER_NODE \
-    --node_rank=$PET_NODE_RANK \
-    --master_addr=$PET_MASTER_ADDR \
-    --master_port=$PET_MASTER_PORT \
-    train_mimic/scripts/train.py \
-    --num_envs 1024 \
-    --max_iterations 1000 \
-    --motion_file data/datasets_precomputed
+python train_mimic/scripts/benchmark.py \
+    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
+    --motion_file data/datasets_precomputed \
+    --num_envs 32
 ```
 
-**注意事项：**
-- 多卡模式下 `--num_envs` 为每张 GPU 的环境数量
-- 多机模式下 `--num_envs` 也按每个进程计算，因此总环境数会随 `world_size` 线性增长
-- 默认日志工具为 TensorBoard。使用 `--logger wandb` 或 `--logger swanlab` 可选择 W&B 或 SwanLab；项目名默认使用 `experiment_name`
-- `--motion_file` 接受预计算训练数据集根目录或单个预计算 `.h5` shard；shard 会递归发现
-- 如果只有最小分发 shard，先运行 `python train_mimic/scripts/data/precompute_dataset.py <minimal_dataset> --outdir <precomputed_dataset>`，再把预计算输出传给训练。
-- 训练会在启动时把所有发现的预计算 motion window 全量加载到内存中。
-- `--max_iterations` 表示追加迭代次数；例如从 `model_12000.pt` 恢复训练并设置 `--max_iterations 18000`，最终将训练到 `model_30000.pt`
+Benchmark 会对每个长度足够的 clip 执行一次确定性的 10 秒 rollout，并报告：
+
+- 平均关节位置误差（`MPJPE`）；
+- 根部位置、旋转和速度误差；
+- rollout 成功率。
+
+结果会保存为文本摘要、JSON、逐 clip CSV 和逐 rollout CSV。
 
-## 导出 ONNX
+## 7. 导出 ONNX
 
 ```bash
 python train_mimic/scripts/save_onnx.py \
     --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
-    --output track.onnx \
+    --output ckpt/track_g1.onnx \
     --history_length 10
 ```
 
-导出的模型为双输入 ONNX（`obs` + `obs_history`）。推理端需要与当前 `velcmd_history` 观测匹配的 167D 双输入 ONNX 策略。
+输出必须是包含 `obs` 和 `obs_history` 的双输入 TemporalCNN。Teleopit 会在启动时
+检查 167D 观测签名，不兼容的导出文件会直接报错。
 
-## 评估
-
-### 播放验证
+使用正常运行入口检查导出结果：
 
 ```bash
-python train_mimic/scripts/play.py \
-    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
-    --motion_file data/datasets_precomputed
+python scripts/run/run_sim.py \
+    controller.policy_path=ckpt/track_g1.onnx \
+    input.bvh_file=data/sample_bvh/aiming1_subject1.bvh
 ```
 
-### 定量评估
+## 扩展到多张 GPU
+
+单机多卡：
 
 ```bash
-python train_mimic/scripts/benchmark.py \
-    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
-    --motion_file data/datasets_precomputed \
-    --num_envs 1
+python train_mimic/scripts/train.py \
+    --gpu_ids 0 1 2 3 \
+    --num_envs 1024 \
+    --max_iterations 30000 \
+    --motion_file data/datasets_precomputed
 ```
 
-### 带视频的定量评估
+这里的 `--num_envs` 是每张 GPU 的环境数量。
+
+多机训练使用 `torchrun`：
 
 ```bash
-python train_mimic/scripts/benchmark.py \
-    --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
-    --motion_file data/datasets_precomputed \
-    --num_envs 1 \
-    --video \
-    --video_length 600
+torchrun \
+    --nnodes=$PET_NNODES \
+    --nproc_per_node=$PET_NPROC_PER_NODE \
+    --node_rank=$PET_NODE_RANK \
+    --master_addr=$PET_MASTER_ADDR \
+    --master_port=$PET_MASTER_PORT \
+    train_mimic/scripts/train.py \
+    --num_envs 1024 \
+    --max_iterations 1000 \
+    --motion_file data/datasets_precomputed
 ```
 
-## 训练架构
+这里的 `--num_envs` 是每个进程的环境数量，总数会随 world size 增长。
 
-```text
-train_mimic/scripts
-    -> train_mimic/app.py
-    -> single task registry / env builder / runner cfg
-    -> mjlab + rsl_rl
-```
+## 常见问题
+
+| 现象 | 检查内容 |
+|------|----------|
+| Loader 提示数据集是 minimal 格式 | 运行 `precompute_dataset.py`，并使用它的输出目录 |
+| 显存不足 | 降低 `--num_envs`，或使用更少的预计算数据 shard |
+| 启动加载时内存不足 | 减少参与训练的 precomputed shard，或增加内存 |
+| 训练速度异常缓慢 | 检查 PyTorch 是否识别 CUDA，并确认训练设备实际使用 CUDA GPU |
 
-关键文件：
-- `train_mimic/app.py` - 训练/播放/评估的统一入口
-- `train_mimic/tasks/tracking/config/env.py` - General-Tracking-G1 环境构建器
-- `train_mimic/tasks/tracking/config/rl.py` - TemporalCNN PPO 配置
-- `train_mimic/tasks/tracking/mdp/commands.py` - 支持 `uniform`、`start` 和 `rewind` 采样模式。训练默认使用 `rewind`；播放/评估使用 `start`。
+任务内部结构和模型维度见[系统架构](../reference/architecture)。
diff --git a/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json b/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json
index 607d7fa0..abfe9792 100644
--- a/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json
+++ b/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json
@@ -8,16 +8,16 @@
     "description": "The title of the footer links column with title=More in the footer"
   },
   "link.item.label.Getting Started": {
-    "message": "快速上手",
+    "message": "安装",
     "description": "The label of footer link with label=Getting Started"
   },
   "link.item.label.Tutorials": {
     "message": "教程",
     "description": "The label of footer link with label=Tutorials"
   },
-  "link.item.label.Configuration": {
-    "message": "配置",
-    "description": "The label of footer link with label=Configuration"
+  "link.item.label.Reference": {
+    "message": "参考资料",
+    "description": "The label of footer link with label=Reference"
   },
   "link.item.label.GitHub": {
     "message": "GitHub",
diff --git a/docs/sidebars.ts b/docs/sidebars.ts
index 5fa0eba5..756bfad2 100644
--- a/docs/sidebars.ts
+++ b/docs/sidebars.ts
@@ -8,8 +8,6 @@ const sidebars: SidebarsConfig = {
       label: 'Getting Started',
       items: [
         'getting-started/installation',
-        'getting-started/download-assets',
-        'getting-started/quick-start',
       ],
     },
     {
@@ -18,33 +16,37 @@ const sidebars: SidebarsConfig = {
       items: [
         'tutorials/offline-sim2sim',
         'tutorials/pico-sim2sim',
-        'tutorials/standalone-standing',
         'tutorials/pico-sim2real',
-        'tutorials/bvh-sim2real',
+        'tutorials/high-level-policy-sim2real',
         'tutorials/training',
       ],
     },
-    {
-      type: 'category',
-      label: 'Configuration',
-      items: [
-        'configuration/overview',
-        'configuration/config-reference',
-        'configuration/faq',
-      ],
-    },
     {
       type: 'category',
       label: 'Reference',
       items: [
+        {
+          type: 'category',
+          label: 'Configuration',
+          items: [
+            'reference/configuration/overview',
+            'reference/configuration/fields',
+          ],
+        },
         'reference/architecture',
-        'reference/assets',
-        'reference/dataset',
-        'reference/g1-bridge-sdk',
-        'reference/training-troubleshooting',
+        {
+          type: 'category',
+          label: 'Resources',
+          items: [
+            'reference/resources/assets',
+            'reference/resources/motion-datasets',
+            'reference/resources/teleoperation-datasets',
+          ],
+        },
+        'reference/companion-projects',
+        'contributing',
       ],
     },
-    'contributing',
   ],
 };
 
diff --git a/docs/static/img/diagrams/architecture-pipeline-zh.svg b/docs/static/img/diagrams/architecture-pipeline-zh.svg
new file mode 100644
index 00000000..0b916ca6
--- /dev/null
+++ b/docs/static/img/diagrams/architecture-pipeline-zh.svg
@@ -0,0 +1,107 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1400" height="760" viewBox="0 0 1400 760" role="img" aria-labelledby="title desc">
+  <title id="title">Teleopit 运行时流程</title>
+  <desc id="desc">全身运控主流程从 BVH 或 PICO 输入开始，依次经过 GMR、参考时间线、观测构建、ONNX 运控和 MuJoCo 或 Unitree G1。主机高层策略可以注入经过校验的参考，PICO 可选路径负责灵巧手和 OpenNeck。</desc>
+  <defs>
+    <marker id="arrow" markerWidth="10" markerHeight="10" refX="8" refY="5" orient="auto">
+      <path d="M0,0 L10,5 L0,10 Z" fill="#475569"/>
+    </marker>
+    <style>
+      text { font-family: "Noto Sans CJK SC", "Microsoft YaHei", Inter, Arial, sans-serif; }
+      .title { font-size: 30px; font-weight: 700; fill: #0f172a; }
+      .lane { font-size: 15px; font-weight: 700; fill: #475569; letter-spacing: .08em; }
+      .box-title { font-size: 18px; font-weight: 700; fill: #0f172a; }
+      .copy { font-size: 14px; fill: #475569; }
+      .note { font-size: 14px; font-weight: 600; fill: #334155; }
+      .arrow { fill: none; stroke: #475569; stroke-width: 2.5; marker-end: url(#arrow); }
+      .branch { fill: none; stroke: #64748b; stroke-width: 2.5; stroke-dasharray: 7 6; marker-end: url(#arrow); }
+    </style>
+  </defs>
+
+  <rect width="1400" height="760" rx="22" fill="#f8fafc"/>
+  <text x="48" y="50" class="title">Teleopit 运行时流程</text>
+
+  <text x="48" y="94" class="lane">全身运控</text>
+  <rect x="48" y="112" width="180" height="100" rx="16" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <text x="138" y="151" class="box-title" text-anchor="middle">输入提供器</text>
+  <text x="138" y="178" class="copy" text-anchor="middle">BVH · PICO 身体</text>
+
+  <rect x="272" y="112" width="180" height="100" rx="16" fill="#ede9fe" stroke="#7c3aed" stroke-width="2.5"/>
+  <text x="362" y="151" class="box-title" text-anchor="middle">GMR 重定向</text>
+  <text x="362" y="178" class="copy" text-anchor="middle">人体动作 → G1 动作</text>
+
+  <rect x="496" y="112" width="180" height="100" rx="16" fill="#fef3c7" stroke="#d97706" stroke-width="2.5"/>
+  <text x="586" y="145" class="box-title" text-anchor="middle">参考时间线</text>
+  <text x="586" y="172" class="copy" text-anchor="middle">根部 + 29 关节</text>
+  <text x="586" y="193" class="copy" text-anchor="middle">时间对齐 · 平滑</text>
+
+  <rect x="720" y="112" width="180" height="100" rx="16" fill="#dcfce7" stroke="#16a34a" stroke-width="2.5"/>
+  <text x="810" y="145" class="box-title" text-anchor="middle">观测构建器</text>
+  <text x="810" y="172" class="copy" text-anchor="middle">velcmd_history</text>
+  <text x="810" y="193" class="copy" text-anchor="middle">167 维</text>
+
+  <rect x="944" y="112" width="180" height="100" rx="16" fill="#fee2e2" stroke="#dc2626" stroke-width="2.5"/>
+  <text x="1034" y="145" class="box-title" text-anchor="middle">ONNX 运控器</text>
+  <text x="1034" y="172" class="copy" text-anchor="middle">TemporalCNN</text>
+  <text x="1034" y="193" class="copy" text-anchor="middle">29 维偏移</text>
+
+  <rect x="1168" y="112" width="180" height="100" rx="16" fill="#e2e8f0" stroke="#475569" stroke-width="2.5"/>
+  <text x="1258" y="151" class="box-title" text-anchor="middle">机器人</text>
+  <text x="1258" y="178" class="copy" text-anchor="middle">MuJoCo · Unitree G1</text>
+
+  <path d="M228 162 L262 162" class="arrow"/>
+  <path d="M452 162 L486 162" class="arrow"/>
+  <path d="M676 162 L710 162" class="arrow"/>
+  <path d="M900 162 L934 162" class="arrow"/>
+  <path d="M1124 162 L1158 162" class="arrow"/>
+
+  <text x="48" y="302" class="lane">独立主机高层策略部署</text>
+  <rect x="48" y="322" width="220" height="96" rx="16" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <text x="158" y="353" class="box-title" text-anchor="middle">机载观测</text>
+  <text x="158" y="380" class="copy" text-anchor="middle">JPEG + 43 维实测状态</text>
+  <text x="158" y="401" class="copy" text-anchor="middle">+ 相机时刻参考根部</text>
+
+  <rect x="340" y="322" width="220" height="96" rx="16" fill="#ede9fe" stroke="#7c3aed" stroke-width="2.5"/>
+  <text x="450" y="359" class="box-title" text-anchor="middle">主机策略服务</text>
+  <text x="450" y="386" class="copy" text-anchor="middle">ZeroMQ · msgpack</text>
+
+  <rect x="632" y="322" width="260" height="96" rx="16" fill="#fef3c7" stroke="#d97706" stroke-width="2.5"/>
+  <text x="762" y="353" class="box-title" text-anchor="middle">校验器 + 调度器</text>
+  <text x="762" y="380" class="copy" text-anchor="middle">时间戳对齐 · 安全检查</text>
+  <text x="762" y="401" class="copy" text-anchor="middle">36 维身体参考</text>
+
+  <path d="M268 370 L330 370" class="arrow"/>
+  <path d="M560 370 L622 370" class="arrow"/>
+  <path d="M762 322 C762 270 586 278 586 222" class="branch"/>
+  <text x="842" y="276" class="note">接入现有 motion tracker</text>
+
+  <text x="48" y="510" class="lane">可选 PICO 机载路径 · 共用一个 PICOBRIDGE 接收器</text>
+  <rect x="48" y="532" width="220" height="96" rx="16" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <text x="158" y="568" class="box-title" text-anchor="middle">手部 / 手柄</text>
+  <text x="158" y="595" class="copy" text-anchor="middle">PICO 追踪帧</text>
+
+  <rect x="340" y="532" width="220" height="96" rx="16" fill="#ede9fe" stroke="#7c3aed" stroke-width="2.5"/>
+  <text x="450" y="568" class="box-title" text-anchor="middle">手部映射</text>
+  <text x="450" y="595" class="copy" text-anchor="middle">somehand · gripper</text>
+
+  <rect x="632" y="532" width="220" height="96" rx="16" fill="#dcfce7" stroke="#16a34a" stroke-width="2.5"/>
+  <text x="742" y="568" class="box-title" text-anchor="middle">LinkerHand</text>
+  <text x="742" y="595" class="copy" text-anchor="middle">L6 · O6</text>
+
+  <path d="M268 580 L330 580" class="arrow"/>
+  <path d="M560 580 L622 580" class="arrow"/>
+
+  <rect x="48" y="650" width="220" height="72" rx="16" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <text x="158" y="680" class="box-title" text-anchor="middle">HMD + Spine3</text>
+  <text x="158" y="705" class="copy" text-anchor="middle">同帧旋转</text>
+
+  <rect x="340" y="650" width="220" height="72" rx="16" fill="#ede9fe" stroke="#7c3aed" stroke-width="2.5"/>
+  <text x="450" y="680" class="box-title" text-anchor="middle">偏航 / 俯仰映射</text>
+  <text x="450" y="705" class="copy" text-anchor="middle">死区 · 俯仰增益</text>
+
+  <rect x="632" y="650" width="220" height="72" rx="16" fill="#dcfce7" stroke="#16a34a" stroke-width="2.5"/>
+  <text x="742" y="680" class="box-title" text-anchor="middle">OpenNeck</text>
+  <text x="742" y="705" class="copy" text-anchor="middle">物理角度</text>
+
+  <path d="M268 686 L330 686" class="arrow"/>
+  <path d="M560 686 L622 686" class="arrow"/>
+</svg>
diff --git a/docs/static/img/diagrams/architecture-pipeline.svg b/docs/static/img/diagrams/architecture-pipeline.svg
new file mode 100644
index 00000000..15c01875
--- /dev/null
+++ b/docs/static/img/diagrams/architecture-pipeline.svg
@@ -0,0 +1,107 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1400" height="760" viewBox="0 0 1400 760" role="img" aria-labelledby="title desc">
+  <title id="title">Teleopit runtime pipelines</title>
+  <desc id="desc">The main whole-body tracking pipeline runs from BVH or PICO input through GMR, a reference timeline, observation building, ONNX control, and a MuJoCo or Unitree G1 robot. A host policy can inject validated references, while optional PICO paths control hands and OpenNeck.</desc>
+  <defs>
+    <marker id="arrow" markerWidth="10" markerHeight="10" refX="8" refY="5" orient="auto">
+      <path d="M0,0 L10,5 L0,10 Z" fill="#475569"/>
+    </marker>
+    <style>
+      text { font-family: Inter, Arial, sans-serif; }
+      .title { font-size: 30px; font-weight: 700; fill: #0f172a; }
+      .lane { font-size: 15px; font-weight: 700; fill: #475569; letter-spacing: .08em; }
+      .box-title { font-size: 18px; font-weight: 700; fill: #0f172a; }
+      .copy { font-size: 14px; fill: #475569; }
+      .note { font-size: 14px; font-weight: 600; fill: #334155; }
+      .arrow { fill: none; stroke: #475569; stroke-width: 2.5; marker-end: url(#arrow); }
+      .branch { fill: none; stroke: #64748b; stroke-width: 2.5; stroke-dasharray: 7 6; marker-end: url(#arrow); }
+    </style>
+  </defs>
+
+  <rect width="1400" height="760" rx="22" fill="#f8fafc"/>
+  <text x="48" y="50" class="title">Teleopit runtime pipelines</text>
+
+  <text x="48" y="94" class="lane">WHOLE-BODY TRACKING</text>
+  <rect x="48" y="112" width="180" height="100" rx="16" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <text x="138" y="151" class="box-title" text-anchor="middle">InputProvider</text>
+  <text x="138" y="178" class="copy" text-anchor="middle">BVH · PICO body</text>
+
+  <rect x="272" y="112" width="180" height="100" rx="16" fill="#ede9fe" stroke="#7c3aed" stroke-width="2.5"/>
+  <text x="362" y="151" class="box-title" text-anchor="middle">GMR Retargeter</text>
+  <text x="362" y="178" class="copy" text-anchor="middle">Human → G1 motion</text>
+
+  <rect x="496" y="112" width="180" height="100" rx="16" fill="#fef3c7" stroke="#d97706" stroke-width="2.5"/>
+  <text x="586" y="145" class="box-title" text-anchor="middle">Reference timeline</text>
+  <text x="586" y="172" class="copy" text-anchor="middle">Root + 29 joints</text>
+  <text x="586" y="193" class="copy" text-anchor="middle">time-aligned · smoothed</text>
+
+  <rect x="720" y="112" width="180" height="100" rx="16" fill="#dcfce7" stroke="#16a34a" stroke-width="2.5"/>
+  <text x="810" y="145" class="box-title" text-anchor="middle">ObservationBuilder</text>
+  <text x="810" y="172" class="copy" text-anchor="middle">velcmd_history</text>
+  <text x="810" y="193" class="copy" text-anchor="middle">167D</text>
+
+  <rect x="944" y="112" width="180" height="100" rx="16" fill="#fee2e2" stroke="#dc2626" stroke-width="2.5"/>
+  <text x="1034" y="145" class="box-title" text-anchor="middle">ONNX Controller</text>
+  <text x="1034" y="172" class="copy" text-anchor="middle">TemporalCNN</text>
+  <text x="1034" y="193" class="copy" text-anchor="middle">29D offsets</text>
+
+  <rect x="1168" y="112" width="180" height="100" rx="16" fill="#e2e8f0" stroke="#475569" stroke-width="2.5"/>
+  <text x="1258" y="151" class="box-title" text-anchor="middle">Robot</text>
+  <text x="1258" y="178" class="copy" text-anchor="middle">MuJoCo · Unitree G1</text>
+
+  <path d="M228 162 L262 162" class="arrow"/>
+  <path d="M452 162 L486 162" class="arrow"/>
+  <path d="M676 162 L710 162" class="arrow"/>
+  <path d="M900 162 L934 162" class="arrow"/>
+  <path d="M1124 162 L1158 162" class="arrow"/>
+
+  <text x="48" y="302" class="lane">INDEPENDENT HOST-POLICY DEPLOYMENT</text>
+  <rect x="48" y="322" width="220" height="96" rx="16" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <text x="158" y="353" class="box-title" text-anchor="middle">Onboard observation</text>
+  <text x="158" y="380" class="copy" text-anchor="middle">JPEG + 43D measured state</text>
+  <text x="158" y="401" class="copy" text-anchor="middle">+ camera-time reference root</text>
+
+  <rect x="340" y="322" width="220" height="96" rx="16" fill="#ede9fe" stroke="#7c3aed" stroke-width="2.5"/>
+  <text x="450" y="359" class="box-title" text-anchor="middle">Host policy server</text>
+  <text x="450" y="386" class="copy" text-anchor="middle">ZeroMQ · msgpack</text>
+
+  <rect x="632" y="322" width="260" height="96" rx="16" fill="#fef3c7" stroke="#d97706" stroke-width="2.5"/>
+  <text x="762" y="353" class="box-title" text-anchor="middle">Validator + scheduler</text>
+  <text x="762" y="380" class="copy" text-anchor="middle">Timestamp alignment · safety</text>
+  <text x="762" y="401" class="copy" text-anchor="middle">36D body reference</text>
+
+  <path d="M268 370 L330 370" class="arrow"/>
+  <path d="M560 370 L622 370" class="arrow"/>
+  <path d="M762 322 C762 270 586 278 586 222" class="branch"/>
+  <text x="842" y="276" class="note">Feeds the existing motion tracker</text>
+
+  <text x="48" y="510" class="lane">OPTIONAL PICO-BASED ONBOARD PATHS · ONE SHARED PICOBRIDGE RECEIVER</text>
+  <rect x="48" y="532" width="220" height="96" rx="16" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <text x="158" y="568" class="box-title" text-anchor="middle">Hands / controllers</text>
+  <text x="158" y="595" class="copy" text-anchor="middle">PICO tracking frame</text>
+
+  <rect x="340" y="532" width="220" height="96" rx="16" fill="#ede9fe" stroke="#7c3aed" stroke-width="2.5"/>
+  <text x="450" y="568" class="box-title" text-anchor="middle">Hand mapping</text>
+  <text x="450" y="595" class="copy" text-anchor="middle">somehand · gripper</text>
+
+  <rect x="632" y="532" width="220" height="96" rx="16" fill="#dcfce7" stroke="#16a34a" stroke-width="2.5"/>
+  <text x="742" y="568" class="box-title" text-anchor="middle">LinkerHand</text>
+  <text x="742" y="595" class="copy" text-anchor="middle">L6 · O6</text>
+
+  <path d="M268 580 L330 580" class="arrow"/>
+  <path d="M560 580 L622 580" class="arrow"/>
+
+  <rect x="48" y="650" width="220" height="72" rx="16" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <text x="158" y="680" class="box-title" text-anchor="middle">HMD + Spine3</text>
+  <text x="158" y="705" class="copy" text-anchor="middle">Same-frame rotations</text>
+
+  <rect x="340" y="650" width="220" height="72" rx="16" fill="#ede9fe" stroke="#7c3aed" stroke-width="2.5"/>
+  <text x="450" y="680" class="box-title" text-anchor="middle">Yaw / pitch mapping</text>
+  <text x="450" y="705" class="copy" text-anchor="middle">Dead zone · pitch gain</text>
+
+  <rect x="632" y="650" width="220" height="72" rx="16" fill="#dcfce7" stroke="#16a34a" stroke-width="2.5"/>
+  <text x="742" y="680" class="box-title" text-anchor="middle">OpenNeck</text>
+  <text x="742" y="705" class="copy" text-anchor="middle">Physical degrees</text>
+
+  <path d="M268 686 L330 686" class="arrow"/>
+  <path d="M560 686 L622 686" class="arrow"/>
+</svg>
diff --git a/docs/static/img/diagrams/pico-g1-state-machine-zh.svg b/docs/static/img/diagrams/pico-g1-state-machine-zh.svg
new file mode 100644
index 00000000..a7dcc23f
--- /dev/null
+++ b/docs/static/img/diagrams/pico-g1-state-machine-zh.svg
@@ -0,0 +1,88 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1200" height="650" viewBox="0 0 1200 650" role="img" aria-labelledby="title desc">
+  <title id="title">Pico 遥操 Unitree G1 状态机</title>
+  <desc id="desc">G1 遥控器进入站立并开始或结束全身追踪，Pico 手柄切换仅手臂模式或暂停，任意状态按 G1 遥控器 L1 加 R1 进入阻尼。</desc>
+  <defs>
+    <marker id="arrow" markerWidth="10" markerHeight="10" refX="8" refY="5" orient="auto-start-reverse">
+      <path d="M0,0 L10,5 L0,10 Z" fill="#475569"/>
+    </marker>
+    <marker id="danger-arrow" markerWidth="10" markerHeight="10" refX="8" refY="5" orient="auto-start-reverse">
+      <path d="M0,0 L10,5 L0,10 Z" fill="#dc2626"/>
+    </marker>
+    <style>
+      text { font-family: "Noto Sans CJK SC", "Microsoft YaHei", Arial, sans-serif; }
+      .title { font-size: 28px; font-weight: 700; fill: #0f172a; }
+      .group-label { font-size: 15px; font-weight: 700; fill: #475569; letter-spacing: .05em; }
+      .state-title { font-size: 20px; font-weight: 700; fill: #0f172a; }
+      .state-copy { font-size: 14px; fill: #475569; }
+      .label { font-size: 14px; font-weight: 600; fill: #334155; }
+      .danger-label { font-size: 15px; font-weight: 700; fill: #991b1b; }
+      .arrow { fill: none; stroke: #475569; stroke-width: 2.5; marker-end: url(#arrow); }
+      .both { fill: none; stroke: #475569; stroke-width: 2.5; marker-start: url(#arrow); marker-end: url(#arrow); }
+      .branch { fill: none; stroke: #94a3b8; stroke-width: 2.5; }
+      .danger-arrow { fill: none; stroke: #dc2626; stroke-width: 2.5; stroke-dasharray: 8 6; marker-end: url(#danger-arrow); }
+    </style>
+  </defs>
+
+  <rect width="1200" height="650" rx="20" fill="#f8fafc"/>
+  <text x="60" y="52" class="title">真实 G1 控制</text>
+
+  <rect x="60" y="72" width="125" height="30" rx="15" fill="#dbeafe"/>
+  <text x="122" y="92" class="label" text-anchor="middle">G1 遥控器</text>
+  <rect x="197" y="72" width="130" height="30" rx="15" fill="#ede9fe"/>
+  <text x="262" y="92" class="label" text-anchor="middle">Pico 手柄</text>
+
+  <rect x="646" y="78" width="494" height="470" rx="22" fill="#ffffff" stroke="#94a3b8" stroke-width="2" stroke-dasharray="8 7"/>
+  <text x="672" y="111" class="group-label">实时 VR 会话</text>
+
+  <rect x="54" y="222" width="220" height="104" rx="18" fill="#fee2e2" stroke="#dc2626" stroke-width="2.5"/>
+  <circle cx="83" cy="248" r="8" fill="#dc2626"/>
+  <text x="164" y="265" class="state-title" text-anchor="middle">DAMPING</text>
+  <text x="164" y="293" class="state-copy" text-anchor="middle">电机阻尼</text>
+
+  <rect x="346" y="222" width="220" height="104" rx="18" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <circle cx="375" cy="248" r="8" fill="#0284c7"/>
+  <text x="456" y="265" class="state-title" text-anchor="middle">STANDING</text>
+  <text x="456" y="293" class="state-copy" text-anchor="middle">机器人站立 · 追踪未接管</text>
+
+  <rect x="684" y="158" width="190" height="98" rx="18" fill="#dcfce7" stroke="#16a34a" stroke-width="2.5"/>
+  <circle cx="713" cy="184" r="8" fill="#16a34a"/>
+  <text x="779" y="200" class="state-title" text-anchor="middle">MOCAP</text>
+  <text x="779" y="228" class="state-copy" text-anchor="middle">全身追踪</text>
+
+  <rect x="912" y="158" width="190" height="98" rx="18" fill="#fef3c7" stroke="#d97706" stroke-width="2.5"/>
+  <circle cx="941" cy="184" r="8" fill="#d97706"/>
+  <text x="1007" y="200" class="state-title" text-anchor="middle">ARMS</text>
+  <text x="1007" y="228" class="state-copy" text-anchor="middle">仅双臂追踪</text>
+
+  <rect x="798" y="412" width="190" height="98" rx="18" fill="#f1f5f9" stroke="#64748b" stroke-width="2.5"/>
+  <circle cx="827" cy="438" r="8" fill="#64748b"/>
+  <text x="893" y="454" class="state-title" text-anchor="middle">PAUSED</text>
+  <text x="893" y="482" class="state-copy" text-anchor="middle">保持当前参考姿态</text>
+
+  <path d="M274 274 L346 274" class="arrow"/>
+  <rect x="260" y="181" width="100" height="28" rx="14" fill="#dbeafe"/>
+  <text x="310" y="200" class="label" text-anchor="middle">Start</text>
+
+  <path d="M566 248 C606 248 629 207 684 207" class="arrow"/>
+  <rect x="562" y="174" width="108" height="28" rx="14" fill="#dbeafe"/>
+  <text x="616" y="193" class="label" text-anchor="middle">遥控器 Y</text>
+
+  <path d="M646 320 C616 320 601 300 566 300" class="arrow"/>
+  <rect x="565" y="339" width="150" height="28" rx="14" fill="#dbeafe"/>
+  <text x="640" y="358" class="label" text-anchor="middle">遥控器 X · 结束</text>
+
+  <path d="M874 207 L912 207" class="both"/>
+  <rect x="826" y="122" width="134" height="28" rx="14" fill="#ede9fe"/>
+  <text x="893" y="141" class="label" text-anchor="middle">Pico 手柄 B</text>
+
+  <path d="M779 256 C779 313 846 310 880 344" class="branch"/>
+  <path d="M1007 256 C1007 313 940 310 906 344" class="branch"/>
+  <circle cx="893" cy="345" r="5" fill="#64748b"/>
+  <path d="M893 350 L893 412" class="both"/>
+  <rect x="784" y="367" width="218" height="28" rx="14" fill="#eef2ff"/>
+  <text x="893" y="386" class="label" text-anchor="middle">遥控器 B · Pico 手柄 A</text>
+
+  <path d="M1090 604 L164 604 L164 326" class="danger-arrow"/>
+  <rect x="421" y="575" width="436" height="38" rx="19" fill="#fee2e2"/>
+  <text x="639" y="599" class="danger-label" text-anchor="middle">任意状态 · 遥控器 L1+R1 → DAMPING</text>
+</svg>
diff --git a/docs/static/img/diagrams/pico-g1-state-machine.svg b/docs/static/img/diagrams/pico-g1-state-machine.svg
new file mode 100644
index 00000000..bffecdcb
--- /dev/null
+++ b/docs/static/img/diagrams/pico-g1-state-machine.svg
@@ -0,0 +1,88 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1200" height="650" viewBox="0 0 1200 650" role="img" aria-labelledby="title desc">
+  <title id="title">Pico teleoperation state machine on Unitree G1</title>
+  <desc id="desc">The G1 remote enters standing and starts or ends body tracking. The Pico controller switches arms mode or pauses. G1 remote L1 plus R1 enters damping from any state.</desc>
+  <defs>
+    <marker id="arrow" markerWidth="10" markerHeight="10" refX="8" refY="5" orient="auto-start-reverse">
+      <path d="M0,0 L10,5 L0,10 Z" fill="#475569"/>
+    </marker>
+    <marker id="danger-arrow" markerWidth="10" markerHeight="10" refX="8" refY="5" orient="auto-start-reverse">
+      <path d="M0,0 L10,5 L0,10 Z" fill="#dc2626"/>
+    </marker>
+    <style>
+      text { font-family: Inter, Arial, sans-serif; }
+      .title { font-size: 28px; font-weight: 700; fill: #0f172a; }
+      .group-label { font-size: 15px; font-weight: 700; fill: #475569; letter-spacing: .05em; }
+      .state-title { font-size: 20px; font-weight: 700; fill: #0f172a; }
+      .state-copy { font-size: 14px; fill: #475569; }
+      .label { font-size: 14px; font-weight: 600; fill: #334155; }
+      .danger-label { font-size: 15px; font-weight: 700; fill: #991b1b; }
+      .arrow { fill: none; stroke: #475569; stroke-width: 2.5; marker-end: url(#arrow); }
+      .both { fill: none; stroke: #475569; stroke-width: 2.5; marker-start: url(#arrow); marker-end: url(#arrow); }
+      .branch { fill: none; stroke: #94a3b8; stroke-width: 2.5; }
+      .danger-arrow { fill: none; stroke: #dc2626; stroke-width: 2.5; stroke-dasharray: 8 6; marker-end: url(#danger-arrow); }
+    </style>
+  </defs>
+
+  <rect width="1200" height="650" rx="20" fill="#f8fafc"/>
+  <text x="60" y="52" class="title">Physical G1 control</text>
+
+  <rect x="60" y="72" width="126" height="30" rx="15" fill="#dbeafe"/>
+  <text x="123" y="92" class="label" text-anchor="middle">G1 remote</text>
+  <rect x="198" y="72" width="158" height="30" rx="15" fill="#ede9fe"/>
+  <text x="277" y="92" class="label" text-anchor="middle">Pico controller</text>
+
+  <rect x="646" y="78" width="494" height="470" rx="22" fill="#ffffff" stroke="#94a3b8" stroke-width="2" stroke-dasharray="8 7"/>
+  <text x="672" y="111" class="group-label">LIVE VR SESSION</text>
+
+  <rect x="54" y="222" width="220" height="104" rx="18" fill="#fee2e2" stroke="#dc2626" stroke-width="2.5"/>
+  <circle cx="83" cy="248" r="8" fill="#dc2626"/>
+  <text x="164" y="265" class="state-title" text-anchor="middle">DAMPING</text>
+  <text x="164" y="293" class="state-copy" text-anchor="middle">Motors damped</text>
+
+  <rect x="346" y="222" width="220" height="104" rx="18" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <circle cx="375" cy="248" r="8" fill="#0284c7"/>
+  <text x="456" y="265" class="state-title" text-anchor="middle">STANDING</text>
+  <text x="456" y="293" class="state-copy" text-anchor="middle">Robot standing · tracking idle</text>
+
+  <rect x="684" y="158" width="190" height="98" rx="18" fill="#dcfce7" stroke="#16a34a" stroke-width="2.5"/>
+  <circle cx="713" cy="184" r="8" fill="#16a34a"/>
+  <text x="779" y="200" class="state-title" text-anchor="middle">MOCAP</text>
+  <text x="779" y="228" class="state-copy" text-anchor="middle">Whole-body tracking</text>
+
+  <rect x="912" y="158" width="190" height="98" rx="18" fill="#fef3c7" stroke="#d97706" stroke-width="2.5"/>
+  <circle cx="941" cy="184" r="8" fill="#d97706"/>
+  <text x="1007" y="200" class="state-title" text-anchor="middle">ARMS</text>
+  <text x="1007" y="228" class="state-copy" text-anchor="middle">Arms-only tracking</text>
+
+  <rect x="798" y="412" width="190" height="98" rx="18" fill="#f1f5f9" stroke="#64748b" stroke-width="2.5"/>
+  <circle cx="827" cy="438" r="8" fill="#64748b"/>
+  <text x="893" y="454" class="state-title" text-anchor="middle">PAUSED</text>
+  <text x="893" y="482" class="state-copy" text-anchor="middle">Hold current reference</text>
+
+  <path d="M274 274 L346 274" class="arrow"/>
+  <rect x="260" y="181" width="100" height="28" rx="14" fill="#dbeafe"/>
+  <text x="310" y="200" class="label" text-anchor="middle">Start</text>
+
+  <path d="M566 248 C606 248 629 207 684 207" class="arrow"/>
+  <rect x="562" y="174" width="108" height="28" rx="14" fill="#dbeafe"/>
+  <text x="616" y="193" class="label" text-anchor="middle">G1 remote Y</text>
+
+  <path d="M646 320 C616 320 601 300 566 300" class="arrow"/>
+  <rect x="565" y="339" width="159" height="28" rx="14" fill="#dbeafe"/>
+  <text x="644" y="358" class="label" text-anchor="middle">G1 remote X · end</text>
+
+  <path d="M874 207 L912 207" class="both"/>
+  <rect x="815" y="122" width="156" height="28" rx="14" fill="#ede9fe"/>
+  <text x="893" y="141" class="label" text-anchor="middle">Pico controller B</text>
+
+  <path d="M779 256 C779 313 846 310 880 344" class="branch"/>
+  <path d="M1007 256 C1007 313 940 310 906 344" class="branch"/>
+  <circle cx="893" cy="345" r="5" fill="#64748b"/>
+  <path d="M893 350 L893 412" class="both"/>
+  <rect x="766" y="367" width="254" height="28" rx="14" fill="#eef2ff"/>
+  <text x="893" y="386" class="label" text-anchor="middle">G1 remote B · Pico controller A</text>
+
+  <path d="M1090 604 L164 604 L164 326" class="danger-arrow"/>
+  <rect x="394" y="575" width="490" height="38" rx="19" fill="#fee2e2"/>
+  <text x="639" y="599" class="danger-label" text-anchor="middle">Any state · G1 remote L1+R1 → DAMPING</text>
+</svg>
diff --git a/docs/static/img/diagrams/pico-sim-state-machine-zh.svg b/docs/static/img/diagrams/pico-sim-state-machine-zh.svg
new file mode 100644
index 00000000..81c91bca
--- /dev/null
+++ b/docs/static/img/diagrams/pico-sim-state-machine-zh.svg
@@ -0,0 +1,74 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1200" height="620" viewBox="0 0 1200 620" role="img" aria-labelledby="title desc">
+  <title id="title">Pico 仿真控制状态机</title>
+  <desc id="desc">键盘 Y 开始实时会话，键盘或 Pico 手柄 B 切换全身与仅手臂控制，键盘或 Pico 手柄 A 暂停，键盘 X 返回站立，键盘 Q 退出。</desc>
+  <defs>
+    <marker id="arrow" markerWidth="10" markerHeight="10" refX="8" refY="5" orient="auto-start-reverse">
+      <path d="M0,0 L10,5 L0,10 Z" fill="#475569"/>
+    </marker>
+    <style>
+      text { font-family: "Noto Sans CJK SC", "Microsoft YaHei", Arial, sans-serif; }
+      .title { font-size: 28px; font-weight: 700; fill: #0f172a; }
+      .group-label { font-size: 15px; font-weight: 700; fill: #475569; letter-spacing: .05em; }
+      .state-title { font-size: 21px; font-weight: 700; fill: #0f172a; }
+      .state-copy { font-size: 14px; fill: #475569; }
+      .label { font-size: 14px; font-weight: 600; fill: #334155; }
+      .hint { font-size: 15px; font-weight: 600; fill: #334155; }
+      .arrow { fill: none; stroke: #475569; stroke-width: 2.5; marker-end: url(#arrow); }
+      .both { fill: none; stroke: #475569; stroke-width: 2.5; marker-start: url(#arrow); marker-end: url(#arrow); }
+      .branch { fill: none; stroke: #94a3b8; stroke-width: 2.5; }
+    </style>
+  </defs>
+
+  <rect width="1200" height="620" rx="20" fill="#f8fafc"/>
+  <text x="60" y="52" class="title">仿真控制</text>
+
+  <rect x="60" y="72" width="110" height="30" rx="15" fill="#dbeafe"/>
+  <text x="115" y="92" class="label" text-anchor="middle">电脑键盘</text>
+  <rect x="182" y="72" width="130" height="30" rx="15" fill="#ede9fe"/>
+  <text x="247" y="92" class="label" text-anchor="middle">Pico 手柄</text>
+
+  <rect x="390" y="78" width="750" height="474" rx="22" fill="#ffffff" stroke="#94a3b8" stroke-width="2" stroke-dasharray="8 7"/>
+  <text x="416" y="111" class="group-label">实时 VR 会话</text>
+
+  <rect x="62" y="220" width="238" height="104" rx="18" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <circle cx="91" cy="246" r="8" fill="#0284c7"/>
+  <text x="181" y="263" class="state-title" text-anchor="middle">STANDING</text>
+  <text x="181" y="291" class="state-copy" text-anchor="middle">站立姿态 · 实时追踪未接管</text>
+
+  <rect x="430" y="158" width="230" height="98" rx="18" fill="#dcfce7" stroke="#16a34a" stroke-width="2.5"/>
+  <circle cx="459" cy="184" r="8" fill="#16a34a"/>
+  <text x="545" y="200" class="state-title" text-anchor="middle">MOCAP</text>
+  <text x="545" y="228" class="state-copy" text-anchor="middle">全身追踪</text>
+
+  <rect x="850" y="158" width="230" height="98" rx="18" fill="#fef3c7" stroke="#d97706" stroke-width="2.5"/>
+  <circle cx="879" cy="184" r="8" fill="#d97706"/>
+  <text x="965" y="200" class="state-title" text-anchor="middle">ARMS</text>
+  <text x="965" y="228" class="state-copy" text-anchor="middle">双臂跟随 · 身体保持站立</text>
+
+  <rect x="640" y="414" width="250" height="100" rx="18" fill="#f1f5f9" stroke="#64748b" stroke-width="2.5"/>
+  <circle cx="669" cy="440" r="8" fill="#64748b"/>
+  <text x="765" y="456" class="state-title" text-anchor="middle">PAUSED</text>
+  <text x="765" y="484" class="state-copy" text-anchor="middle">保持当前参考姿态</text>
+
+  <path d="M300 248 C345 248 370 207 430 207" class="arrow"/>
+  <rect x="292" y="172" width="115" height="28" rx="14" fill="#dbeafe"/>
+  <text x="349" y="191" class="label" text-anchor="middle">键盘 Y</text>
+
+  <path d="M390 311 C354 311 337 292 300 292" class="arrow"/>
+  <rect x="301" y="329" width="153" height="28" rx="14" fill="#dbeafe"/>
+  <text x="377" y="348" class="label" text-anchor="middle">键盘 X · 结束</text>
+
+  <path d="M660 207 L850 207" class="both"/>
+  <rect x="679" y="122" width="172" height="28" rx="14" fill="#eef2ff"/>
+  <text x="765" y="141" class="label" text-anchor="middle">键盘 B · Pico B</text>
+
+  <path d="M545 256 C545 307 670 302 750 344" class="branch"/>
+  <path d="M965 256 C965 307 860 302 780 344" class="branch"/>
+  <circle cx="765" cy="345" r="5" fill="#64748b"/>
+  <path d="M765 350 L765 414" class="both"/>
+  <rect x="822" y="342" width="185" height="28" rx="14" fill="#eef2ff"/>
+  <text x="914" y="361" class="label" text-anchor="middle">键盘 A · Pico A</text>
+
+  <rect x="60" y="570" width="1080" height="34" rx="17" fill="#e2e8f0"/>
+  <text x="600" y="592" class="hint" text-anchor="middle">任意状态按键盘 Q · 退出仿真</text>
+</svg>
diff --git a/docs/static/img/diagrams/pico-sim-state-machine.svg b/docs/static/img/diagrams/pico-sim-state-machine.svg
new file mode 100644
index 00000000..d56a4fe9
--- /dev/null
+++ b/docs/static/img/diagrams/pico-sim-state-machine.svg
@@ -0,0 +1,74 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1200" height="620" viewBox="0 0 1200 620" role="img" aria-labelledby="title desc">
+  <title id="title">Pico simulation control state machine</title>
+  <desc id="desc">Keyboard Y starts a live session, keyboard or Pico controller B switches between whole-body and arms-only control, keyboard or Pico controller A pauses, keyboard X returns to standing, and keyboard Q quits.</desc>
+  <defs>
+    <marker id="arrow" markerWidth="10" markerHeight="10" refX="8" refY="5" orient="auto-start-reverse">
+      <path d="M0,0 L10,5 L0,10 Z" fill="#475569"/>
+    </marker>
+    <style>
+      text { font-family: Inter, Arial, sans-serif; }
+      .title { font-size: 28px; font-weight: 700; fill: #0f172a; }
+      .group-label { font-size: 15px; font-weight: 700; fill: #475569; letter-spacing: .05em; }
+      .state-title { font-size: 21px; font-weight: 700; fill: #0f172a; }
+      .state-copy { font-size: 14px; fill: #475569; }
+      .label { font-size: 14px; font-weight: 600; fill: #334155; }
+      .hint { font-size: 15px; font-weight: 600; fill: #334155; }
+      .arrow { fill: none; stroke: #475569; stroke-width: 2.5; marker-end: url(#arrow); }
+      .both { fill: none; stroke: #475569; stroke-width: 2.5; marker-start: url(#arrow); marker-end: url(#arrow); }
+      .branch { fill: none; stroke: #94a3b8; stroke-width: 2.5; }
+    </style>
+  </defs>
+
+  <rect width="1200" height="620" rx="20" fill="#f8fafc"/>
+  <text x="60" y="52" class="title">Simulation control</text>
+
+  <rect x="60" y="72" width="130" height="30" rx="15" fill="#dbeafe"/>
+  <text x="125" y="92" class="label" text-anchor="middle">Keyboard</text>
+  <rect x="202" y="72" width="158" height="30" rx="15" fill="#ede9fe"/>
+  <text x="281" y="92" class="label" text-anchor="middle">Pico controller</text>
+
+  <rect x="390" y="78" width="750" height="474" rx="22" fill="#ffffff" stroke="#94a3b8" stroke-width="2" stroke-dasharray="8 7"/>
+  <text x="416" y="111" class="group-label">LIVE VR SESSION</text>
+
+  <rect x="62" y="220" width="238" height="104" rx="18" fill="#e0f2fe" stroke="#0284c7" stroke-width="2.5"/>
+  <circle cx="91" cy="246" r="8" fill="#0284c7"/>
+  <text x="181" y="263" class="state-title" text-anchor="middle">STANDING</text>
+  <text x="181" y="291" class="state-copy" text-anchor="middle">Standing pose · tracking inactive</text>
+
+  <rect x="430" y="158" width="230" height="98" rx="18" fill="#dcfce7" stroke="#16a34a" stroke-width="2.5"/>
+  <circle cx="459" cy="184" r="8" fill="#16a34a"/>
+  <text x="545" y="200" class="state-title" text-anchor="middle">MOCAP</text>
+  <text x="545" y="228" class="state-copy" text-anchor="middle">Whole-body tracking</text>
+
+  <rect x="850" y="158" width="230" height="98" rx="18" fill="#fef3c7" stroke="#d97706" stroke-width="2.5"/>
+  <circle cx="879" cy="184" r="8" fill="#d97706"/>
+  <text x="965" y="200" class="state-title" text-anchor="middle">ARMS</text>
+  <text x="965" y="228" class="state-copy" text-anchor="middle">Live arms · standing body</text>
+
+  <rect x="640" y="414" width="250" height="100" rx="18" fill="#f1f5f9" stroke="#64748b" stroke-width="2.5"/>
+  <circle cx="669" cy="440" r="8" fill="#64748b"/>
+  <text x="765" y="456" class="state-title" text-anchor="middle">PAUSED</text>
+  <text x="765" y="484" class="state-copy" text-anchor="middle">Hold the current reference</text>
+
+  <path d="M300 248 C345 248 370 207 430 207" class="arrow"/>
+  <rect x="292" y="172" width="121" height="28" rx="14" fill="#dbeafe"/>
+  <text x="352" y="191" class="label" text-anchor="middle">Keyboard Y</text>
+
+  <path d="M390 311 C354 311 337 292 300 292" class="arrow"/>
+  <rect x="301" y="329" width="168" height="28" rx="14" fill="#dbeafe"/>
+  <text x="385" y="348" class="label" text-anchor="middle">Keyboard X · end</text>
+
+  <path d="M660 207 L850 207" class="both"/>
+  <rect x="667" y="122" width="196" height="28" rx="14" fill="#eef2ff"/>
+  <text x="765" y="141" class="label" text-anchor="middle">Keyboard B · Pico B</text>
+
+  <path d="M545 256 C545 307 670 302 750 344" class="branch"/>
+  <path d="M965 256 C965 307 860 302 780 344" class="branch"/>
+  <circle cx="765" cy="345" r="5" fill="#64748b"/>
+  <path d="M765 350 L765 414" class="both"/>
+  <rect x="806" y="342" width="217" height="28" rx="14" fill="#eef2ff"/>
+  <text x="914" y="361" class="label" text-anchor="middle">Keyboard A · Pico A</text>
+
+  <rect x="60" y="570" width="1080" height="34" rx="17" fill="#e2e8f0"/>
+  <text x="600" y="592" class="hint" text-anchor="middle">Keyboard Q · Quit the simulation from any state</text>
+</svg>
diff --git a/pyproject.toml b/pyproject.toml
index 4bb6ce39..db70cc8a 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "teleopit"
-version = "0.4.0"
+version = "0.5.0"
 description = "Teleoperation framework for humanoid robots with motion retargeting"
 authors = [
     {name = "Teleopit Team"}
@@ -24,6 +24,7 @@ dependencies = [
     "h5py",
     "onnxruntime",
     "pyzmq",
+    "msgpack",
     "rich",
     "loop-rate-limiters",
     "imageio",
@@ -50,16 +51,27 @@ train = [
     "tqdm>=4.65.0",
 ]
 pico4 = [
-    "pico-bridge[camera] @ https://github.com/BotRunner64/pico-bridge/releases/download/v0.2.1/pico_bridge-0.2.1-py3-none-any.whl",
+    "pico-bridge @ https://github.com/BotRunner64/pico-bridge/releases/download/v0.2.1/pico_bridge-0.2.1-py3-none-any.whl",
     "teleopit[sim2real]",
 ]
+openneck = [
+    "openneck @ git+https://github.com/BotRunner64/OpenNeck.git",
+    "teleopit[pico4]",
+]
 recording = [
     "teleopit[pico4]",
     "opencv-python",
     "imageio[ffmpeg]",
 ]
+review = [
+    "opencv-python",
+    "mjviser>=0.0.14",
+]
 dexhand = []
 
 [tool.setuptools.packages.find]
 where = ["."]
 include = ["teleopit*", "train_mimic*"]
+
+[tool.setuptools.package-data]
+"teleopit.high_level_policy" = ["hand_calibration.json"]
diff --git a/scripts/dev/bench_policy_onnx.py b/scripts/dev/bench_policy_onnx.py
index b40b7c2a..9a018c95 100644
--- a/scripts/dev/bench_policy_onnx.py
+++ b/scripts/dev/bench_policy_onnx.py
@@ -4,9 +4,9 @@
 It does not require MuJoCo, robot hardware, GMR assets, or Pico input.
 
 Examples:
-    python scripts/dev/bench_policy_onnx.py --policy track.onnx
-    python scripts/dev/bench_policy_onnx.py --policy track.onnx --runs 20000 --device cpu
-    python scripts/dev/bench_policy_onnx.py --policy track.onnx --mode direct
+    python scripts/dev/bench_policy_onnx.py --policy ckpt/track_g1.onnx
+    python scripts/dev/bench_policy_onnx.py --policy ckpt/track_g1.onnx --runs 20000 --device cpu
+    python scripts/dev/bench_policy_onnx.py --policy ckpt/track_g1.onnx --mode direct
 """
 
 from __future__ import annotations
diff --git a/scripts/dev/test_bridge_state.py b/scripts/dev/test_g1_bridge.py
similarity index 97%
rename from scripts/dev/test_bridge_state.py
rename to scripts/dev/test_g1_bridge.py
index 7cae1df2..b7d05e98 100644
--- a/scripts/dev/test_bridge_state.py
+++ b/scripts/dev/test_g1_bridge.py
@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-"""Diagnostic: compare C++ bridge state vs Python SDK state."""
+"""Compare G1 C++ bridge state with Unitree Python SDK state."""
 import sys
 import time
 import copy
diff --git a/scripts/dev/test_linkerhand_l6.py b/scripts/dev/test_linkerhand.py
similarity index 94%
rename from scripts/dev/test_linkerhand_l6.py
rename to scripts/dev/test_linkerhand.py
index 3b40d58c..4a854d58 100644
--- a/scripts/dev/test_linkerhand_l6.py
+++ b/scripts/dev/test_linkerhand.py
@@ -34,7 +34,8 @@
 O6_OPEN_POSE = [250, 250, 250, 250, 250, 250]
 O6_CLOSE_POSE = [86, 73, 118, 111, 110, 111]
 O6_DEFAULT_SPEED = [255, 255, 255, 255, 255, 255]
-DEFAULT_SOMEHAND_CONFIG_PATH = "third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml"
+DEFAULT_L6_SOMEHAND_CONFIG_PATH = "third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml"
+DEFAULT_O6_SOMEHAND_CONFIG_PATH = "third_party/somehand/configs/retargeting/bihand/linkerhand_o6_bihand.yaml"
 OPEN_CLOSE_HOLD_S = 1.0
 GRIPPER_RATE_HZ = 30.0
 VR_HAND_POSE_RATE_HZ = 60.0
@@ -56,7 +57,7 @@ def parse_args() -> argparse.Namespace:
         "--driver",
         choices=["linkerhand_l6", "linkerhand_o6"],
         default="linkerhand_l6",
-        help="Hand driver to test. O6 currently supports open_close and gripper only.",
+        help="Hand driver to test.",
     )
     parser.add_argument(
         "--mode",
@@ -77,8 +78,6 @@ def parse_args() -> argparse.Namespace:
         help='RS485 serial port such as /dev/ttyUSB0; "None" uses CAN',
     )
     args = parser.parse_args()
-    if args.driver == "linkerhand_o6" and args.mode == "vr_hand_pose":
-        raise SystemExit("hands.driver=linkerhand_o6 supports only --mode open_close or gripper")
     args.speed = list(O6_DEFAULT_SPEED if args.driver == "linkerhand_o6" else DEFAULT_SPEED)
     args.open_pose = list(O6_OPEN_POSE if args.driver == "linkerhand_o6" else OPEN_POSE)
     args.close_pose = list(O6_CLOSE_POSE if args.driver == "linkerhand_o6" else CLOSE_POSE)
@@ -113,7 +112,8 @@ def make_config(args: argparse.Namespace, *, mode: str) -> dict[str, object]:
             "frame_timeout_s": FRAME_TIMEOUT_S,
             driver_section: driver_cfg,
             "somehand": {
-                "config_path": DEFAULT_SOMEHAND_CONFIG_PATH,
+                "l6_config_path": DEFAULT_L6_SOMEHAND_CONFIG_PATH,
+                "o6_config_path": DEFAULT_O6_SOMEHAND_CONFIG_PATH,
                 "rate_hz": VR_HAND_POSE_RATE_HZ,
                 "max_iterations": 12,
                 "temporal_filter_alpha": 1.0,
@@ -260,9 +260,6 @@ def run_gripper(args: argparse.Namespace) -> None:
 
 
 def run_vr_hand_pose(args: argparse.Namespace) -> None:
-    if args.hand_type != "both":
-        raise SystemExit("hands.mode=vr_hand_pose currently requires --hand-type both")
-
     config = make_config(args, mode="vr_hand_pose")
     provider = make_pico_provider()
     device, mapper = build_driver_runtime(config, driver=args.driver)
diff --git a/scripts/dev/test_openneck.py b/scripts/dev/test_openneck.py
new file mode 100644
index 00000000..24f7a04b
--- /dev/null
+++ b/scripts/dev/test_openneck.py
@@ -0,0 +1,218 @@
+#!/usr/bin/env python3
+"""Exercise optional OpenNeck active-vision control."""
+
+from __future__ import annotations
+
+import argparse
+import logging
+import math
+from pathlib import Path
+import sys
+import time
+
+
+REPO_ROOT = Path(__file__).resolve().parents[2]
+sys.path.insert(0, str(REPO_ROOT))
+
+from teleopit.inputs.pico4_provider import Pico4InputProvider  # noqa: E402
+from teleopit.sim2real.neck.config import NeckConfig  # noqa: E402
+from teleopit.sim2real.neck.openneck import build_neck_device  # noqa: E402
+from teleopit.sim2real.neck.worker import NeckRuntime  # noqa: E402
+
+
+DEFAULT_RATE_HZ = 60.0
+DEFAULT_FRAME_TIMEOUT_S = 0.3
+DEFAULT_PITCH_GAIN = 1.4
+DEFAULT_TEST_ANGLE_DEG = 5.0
+DEFAULT_HOLD_S = 0.8
+DEFAULT_PICO_TIMEOUT_S = 60.0
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="Test OpenNeck active-vision control")
+    parser.add_argument(
+        "--mode",
+        choices=["direct", "pico"],
+        default="direct",
+        help=(
+            "direct sends a conservative fixed motion pattern to OpenNeck; "
+            "pico drives OpenNeck from live Pico HMD rotation relative to Spine3."
+        ),
+    )
+    parser.add_argument("--port", default=None, help="Optional OpenNeck serial port, for example /dev/ttyACM0")
+    parser.add_argument("--config", dest="config_path", default=None, help="Optional OpenNeck calibration config path")
+    parser.add_argument("--dry-run", action="store_true", help="Compute/log commands without opening OpenNeck hardware")
+    parser.add_argument("--rate-hz", type=float, default=DEFAULT_RATE_HZ)
+    parser.add_argument("--frame-timeout-s", type=float, default=DEFAULT_FRAME_TIMEOUT_S)
+    parser.add_argument(
+        "--angle-deg",
+        type=float,
+        default=DEFAULT_TEST_ANGLE_DEG,
+        help="Direct-test angle magnitude in degrees. Keep this conservative.",
+    )
+    parser.add_argument("--hold-s", type=float, default=DEFAULT_HOLD_S, help="Seconds to hold each direct-test command")
+    parser.add_argument("--duration-s", type=float, default=0.0, help="Pico mode duration; 0 means until Ctrl-C")
+    parser.add_argument("--no-center-on-start", action="store_true")
+    parser.add_argument("--no-center-on-shutdown", action="store_true")
+    parser.add_argument("--release-on-shutdown", action="store_true")
+    parser.add_argument("--dead-zone-deg", type=float, default=0.5)
+    parser.add_argument("--pitch-gain", type=float, default=DEFAULT_PITCH_GAIN)
+    parser.add_argument("--bridge-host", default="0.0.0.0")
+    parser.add_argument("--bridge-port", type=int, default=63901)
+    parser.add_argument("--bridge-discovery", action=argparse.BooleanOptionalAction, default=True)
+    parser.add_argument("--bridge-advertise-ip", default=None)
+    args = parser.parse_args()
+    if args.rate_hz <= 0:
+        raise SystemExit("--rate-hz must be > 0")
+    if args.frame_timeout_s <= 0:
+        raise SystemExit("--frame-timeout-s must be > 0")
+    if args.hold_s <= 0:
+        raise SystemExit("--hold-s must be > 0")
+    if args.duration_s < 0:
+        raise SystemExit("--duration-s must be >= 0")
+    if args.angle_deg <= 0.0:
+        raise SystemExit("--angle-deg must be > 0")
+    if not math.isfinite(args.pitch_gain) or args.pitch_gain <= 0.0:
+        raise SystemExit("--pitch-gain must be finite and > 0")
+    return args
+
+
+def make_neck_config(args: argparse.Namespace) -> NeckConfig:
+    return NeckConfig(
+        enabled=True,
+        driver="openneck",
+        config_path=args.config_path,
+        port=args.port,
+        rate_hz=args.rate_hz,
+        frame_timeout_s=args.frame_timeout_s,
+        active_modes=("mocap",),
+        dead_zone_deg=args.dead_zone_deg,
+        pitch_gain=args.pitch_gain,
+        center_on_start=not bool(args.no_center_on_start),
+        center_on_shutdown=not bool(args.no_center_on_shutdown),
+        release_on_shutdown=bool(args.release_on_shutdown),
+        dry_run=bool(args.dry_run),
+    )
+
+
+def make_pico_provider(args: argparse.Namespace) -> Pico4InputProvider:
+    return Pico4InputProvider(
+        timeout=DEFAULT_PICO_TIMEOUT_S,
+        pause_button=None,
+        arms_button=None,
+        bridge_host=args.bridge_host,
+        bridge_port=args.bridge_port,
+        bridge_discovery=bool(args.bridge_discovery),
+        bridge_advertise_ip=args.bridge_advertise_ip,
+        bridge_video=None,
+        bridge_video_enabled=False,
+    )
+
+
+def run_direct(args: argparse.Namespace) -> None:
+    cfg = make_neck_config(args)
+    device = build_neck_device(cfg)
+    angle_deg = float(args.angle_deg)
+    pattern = [
+        ("center", 0.0, 0.0),
+        ("yaw left", angle_deg, 0.0),
+        ("center", 0.0, 0.0),
+        ("yaw right", -angle_deg, 0.0),
+        ("center", 0.0, 0.0),
+        ("pitch up", 0.0, angle_deg),
+        ("center", 0.0, 0.0),
+        ("pitch down", 0.0, -angle_deg),
+        ("center", 0.0, 0.0),
+    ]
+
+    print(
+        f"Testing OpenNeck direct pattern | port={args.port} dry_run={args.dry_run} "
+        f"angle={angle_deg:.2f}deg",
+        flush=True,
+    )
+    try:
+        device.connect()
+        if cfg.center_on_start:
+            device.center()
+        for label, yaw_deg, pitch_deg in pattern:
+            print(f"{label}: yaw={yaw_deg:.2f}deg pitch={pitch_deg:.2f}deg", flush=True)
+            device.move_deg(yaw_deg, pitch_deg)
+            time.sleep(float(args.hold_s))
+    except KeyboardInterrupt:
+        print("Interrupted; shutting down OpenNeck", flush=True)
+    finally:
+        try:
+            if cfg.center_on_shutdown:
+                device.center()
+            if cfg.release_on_shutdown:
+                device.release_torque()
+        finally:
+            device.close()
+
+
+def run_pico(args: argparse.Namespace) -> None:
+    cfg = make_neck_config(args)
+    provider = make_pico_provider(args)
+    runtime = NeckRuntime(cfg)
+    sleep_s = 1.0 / max(float(args.rate_hz), 1.0)
+    deadline = time.monotonic() + float(args.duration_s) if args.duration_s > 0.0 else None
+    last_seq = -1
+    command_count = 0
+
+    print(
+        "Testing OpenNeck active vision from the live Pico HMD rotation relative to Spine3; "
+        "press Ctrl-C to stop.",
+        flush=True,
+    )
+    try:
+        runtime.start()
+        while deadline is None or time.monotonic() < deadline:
+            now_s = time.monotonic()
+            snapshot = provider.get_head_pose_snapshot()
+            if snapshot is not None and int(snapshot.seq) != last_seq:
+                command = runtime.tick(
+                    hmd_rotation_wxyz=snapshot.hmd_rotation_wxyz,
+                    spine3_rotation_wxyz=snapshot.spine3_rotation_wxyz,
+                    pose_timestamp_s=snapshot.timestamp_s,
+                    active=True,
+                    now_s=now_s,
+                )
+                moved = command is not None
+                if moved:
+                    command_count += 1
+                last_seq = int(snapshot.seq)
+                age_ms = max((now_s - float(snapshot.timestamp_s)) * 1000.0, 0.0)
+                print(
+                    f"pico seq={snapshot.seq} age={age_ms:.1f}ms "
+                    f"moved={moved} commands={command_count}",
+                    flush=True,
+                )
+            elif snapshot is None:
+                runtime.tick(
+                    hmd_rotation_wxyz=None,
+                    spine3_rotation_wxyz=None,
+                    pose_timestamp_s=None,
+                    active=True,
+                    now_s=now_s,
+                )
+            time.sleep(sleep_s)
+    except KeyboardInterrupt:
+        print("Interrupted; shutting down OpenNeck", flush=True)
+    finally:
+        runtime.close()
+        provider.close()
+
+
+def main() -> None:
+    logging.basicConfig(level=logging.INFO, format="%(levelname)s:%(name)s:%(message)s")
+    args = parse_args()
+    if args.mode == "direct":
+        run_direct(args)
+    elif args.mode == "pico":
+        run_pico(args)
+    else:
+        raise AssertionError(f"Unhandled mode: {args.mode}")
+
+
+if __name__ == "__main__":
+    main()
diff --git a/scripts/run/check_pico_signal.py b/scripts/dev/test_pico_bridge.py
similarity index 65%
rename from scripts/run/check_pico_signal.py
rename to scripts/dev/test_pico_bridge.py
index 44eac250..2d7f25a6 100644
--- a/scripts/run/check_pico_signal.py
+++ b/scripts/dev/test_pico_bridge.py
@@ -1,26 +1,47 @@
-"""Pico mocap/video signal diagnostic entry point."""
+#!/usr/bin/env python3
+"""Pico Bridge mocap/video diagnostic entry point."""
 
 from __future__ import annotations
 
+import argparse
 from collections import Counter
 import logging
 import os
+from pathlib import Path
 import signal
+import sys
 import time
 import threading
 from typing import Any
 
-import hydra
 import numpy as np
-from omegaconf import DictConfig
 
-from teleopit.inputs.human_frame_validation import HumanFrameValidationResult, validate_human_frame
-from teleopit.inputs.pico4_provider import Pico4InputProvider
-from teleopit.inputs.pico_video import PicoVideoRuntime, bridge_video_source, parse_pico_video_config
-from teleopit.runtime.common import cfg_get
 
+REPO_ROOT = Path(__file__).resolve().parents[2]
+sys.path.insert(0, str(REPO_ROOT))
 
-logger = logging.getLogger("teleopit.tools.check_pico_signal")
+from teleopit.inputs.human_frame_validation import (  # noqa: E402
+    HumanFrameValidationResult,
+    validate_human_frame,
+)
+from teleopit.inputs.pico4_provider import Pico4InputProvider  # noqa: E402
+from teleopit.inputs.pico_video import (  # noqa: E402
+    PicoVideoConfig,
+    PicoVideoRuntime,
+    bridge_video_source,
+)
+
+
+logger = logging.getLogger("teleopit.tools.test_pico_bridge")
+
+DEFAULT_BRIDGE_HOST = "0.0.0.0"
+DEFAULT_BRIDGE_PORT = 63901
+DEFAULT_VIDEO_SOURCE = "realsense"
+DEFAULT_VIDEO_WIDTH = 1280
+DEFAULT_VIDEO_HEIGHT = 720
+DEFAULT_VIDEO_FPS = 30
+DEFAULT_POLL_HZ = 120.0
+DEFAULT_SUMMARY_INTERVAL_S = 1.0
 
 
 def _fmt_vec(values: tuple[float, ...] | None) -> str:
@@ -94,7 +115,7 @@ def _log_summary(
 ) -> None:
     if total <= 0:
         logger.info(
-            "Pico signal summary | window=%.1fs samples=0 provider_fps=%.1f "
+            "Pico Bridge summary | window=%.1fs samples=0 provider_fps=%.1f "
             "last_seq=%s video_frames=%d",
             window_s,
             provider_fps,
@@ -110,7 +131,7 @@ def _log_summary(
     min_pos = last_stats.get("min_pos")
     max_pos = last_stats.get("max_pos")
     logger.info(
-        "Pico signal summary | window=%.1fs samples=%d valid=%d invalid=%d reasons=%s "
+        "Pico Bridge summary | window=%.1fs samples=%d valid=%d invalid=%d reasons=%s "
         "provider_fps=%.1f last_seq=%s last_age_ms=%s video_frames=%d "
         "max_abs_pos=%s pelvis=%s extent=%s min=%s max=%s quat_norm=[%s,%s]",
         window_s,
@@ -145,24 +166,72 @@ def _fmt_float(value: Any) -> str:
     return f"{float(value):.4f}"
 
 
-def _build_provider(cfg: DictConfig, video_enabled: bool) -> Pico4InputProvider:
-    input_cfg = cfg_get(cfg, "input", {}) or {}
-    video_cfg = parse_pico_video_config(input_cfg)
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="Test Pico Bridge body tracking and RealSense video streaming",
+    )
+    parser.add_argument("--bridge-host", default=DEFAULT_BRIDGE_HOST)
+    parser.add_argument("--bridge-port", type=int, default=DEFAULT_BRIDGE_PORT)
+    parser.add_argument(
+        "--bridge-discovery",
+        action=argparse.BooleanOptionalAction,
+        default=True,
+    )
+    parser.add_argument("--bridge-advertise-ip", default=None)
+    parser.add_argument(
+        "--video",
+        action=argparse.BooleanOptionalAction,
+        default=True,
+        help="Stream video to Pico; enabled by default. Use --no-video to disable it.",
+    )
+    parser.add_argument(
+        "--video-source",
+        choices=["realsense", "test-pattern"],
+        default=DEFAULT_VIDEO_SOURCE,
+    )
+    parser.add_argument("--video-width", type=int, default=DEFAULT_VIDEO_WIDTH)
+    parser.add_argument("--video-height", type=int, default=DEFAULT_VIDEO_HEIGHT)
+    parser.add_argument("--video-fps", type=int, default=DEFAULT_VIDEO_FPS)
+    parser.add_argument("--video-device", default=None)
+    parser.add_argument(
+        "--duration-s",
+        type=float,
+        default=0.0,
+        help="Diagnostic duration; 0 means until Ctrl-C.",
+    )
+    args = parser.parse_args()
+
+    if not 1 <= args.bridge_port <= 65535:
+        parser.error("--bridge-port must be in [1, 65535]")
+    if args.video and (args.video_width <= 0 or args.video_height <= 0 or args.video_fps <= 0):
+        parser.error("--video-width, --video-height, and --video-fps must be > 0")
+    if args.duration_s < 0.0:
+        parser.error("--duration-s must be >= 0")
+    return args
+
+
+def _make_video_config(args: argparse.Namespace) -> PicoVideoConfig:
+    return PicoVideoConfig(
+        enabled=bool(args.video),
+        source=str(args.video_source) if args.video else None,
+        width=int(args.video_width),
+        height=int(args.video_height),
+        fps=int(args.video_fps),
+        device=None if args.video_device in (None, "", "null") else str(args.video_device),
+    )
+
+
+def _build_provider(args: argparse.Namespace, video_cfg: PicoVideoConfig) -> Pico4InputProvider:
     return Pico4InputProvider(
-        human_format=str(cfg_get(input_cfg, "human_format", "pico_bridge")),
-        timeout=float(cfg_get(input_cfg, "pico4_timeout", 60.0)),
-        buffer_size=int(cfg_get(input_cfg, "pico4_buffer_size", 60)),
-        timestamp_gap_reset_s=float(cfg_get(input_cfg, "pico4_timestamp_gap_reset_s", 0.15)),
-        pause_button=cfg_get(input_cfg, "pause_button", "A"),
-        pause_debounce_s=float(cfg_get(input_cfg, "pause_debounce_s", 0.25)),
-        bridge_host=str(cfg_get(input_cfg, "bridge_host", "0.0.0.0")),
-        bridge_port=int(cfg_get(input_cfg, "bridge_port", 63901)),
-        bridge_discovery=bool(cfg_get(input_cfg, "bridge_discovery", True)),
-        bridge_advertise_ip=cfg_get(input_cfg, "bridge_advertise_ip", None),
+        human_format="pico_bridge",
+        pause_button=None,
+        arms_button=None,
+        bridge_host=str(args.bridge_host),
+        bridge_port=int(args.bridge_port),
+        bridge_discovery=bool(args.bridge_discovery),
+        bridge_advertise_ip=args.bridge_advertise_ip,
         bridge_video=bridge_video_source(video_cfg),
-        bridge_video_enabled=video_enabled,
-        bridge_start_timeout=float(cfg_get(input_cfg, "bridge_start_timeout", 10.0)),
-        bridge_history_size=int(cfg_get(input_cfg, "bridge_history_size", 120)),
+        bridge_video_enabled=video_cfg.enabled,
     )
 
 
@@ -191,38 +260,33 @@ def _run() -> None:
     return done
 
 
-@hydra.main(version_base=None, config_path="../../teleopit/configs", config_name="pico4_sim2real")
-def main(cfg: DictConfig) -> None:
+def main() -> None:
     logging.basicConfig(level=logging.INFO, format="%(levelname)s:%(name)s:%(message)s")
-    input_cfg = cfg_get(cfg, "input", {}) or {}
-    video_cfg = parse_pico_video_config(input_cfg)
-    diag_cfg = cfg_get(cfg, "diagnostic", {}) or {}
-    poll_hz = float(cfg_get(diag_cfg, "poll_hz", cfg_get(cfg_get(cfg, "runtime", {}) or {}, "pico_input_hz", 120.0)))
-    summary_interval_s = float(cfg_get(diag_cfg, "summary_interval_s", 1.0))
-    duration_s = float(cfg_get(diag_cfg, "duration_s", 0.0))
-
-    logger.info("Starting Pico signal diagnostic")
+    args = parse_args()
+    video_cfg = _make_video_config(args)
+
+    logger.info("Starting Pico Bridge diagnostic")
     logger.info(
         "Pico bridge | host=%s port=%s discovery=%s advertise_ip=%s",
-        cfg_get(input_cfg, "bridge_host", "0.0.0.0"),
-        cfg_get(input_cfg, "bridge_port", 63901),
-        cfg_get(input_cfg, "bridge_discovery", True),
-        cfg_get(input_cfg, "bridge_advertise_ip", None),
+        args.bridge_host,
+        args.bridge_port,
+        args.bridge_discovery,
+        args.bridge_advertise_ip,
     )
     logger.info(
         "Signal check | validation=finite_values poll_hz=%.1f summary_interval_s=%.1f "
         "duration_s=%s video_enabled=%s video_source=%s",
-        poll_hz,
-        summary_interval_s,
-        f"{duration_s:.1f}" if duration_s > 0.0 else "until Ctrl-C",
+        DEFAULT_POLL_HZ,
+        DEFAULT_SUMMARY_INTERVAL_S,
+        f"{args.duration_s:.1f}" if args.duration_s > 0.0 else "until Ctrl-C",
         video_cfg.enabled,
         video_cfg.source,
     )
 
     stop_event = threading.Event()
     _install_signal_handlers(stop_event)
-    provider = _build_provider(cfg, video_cfg.enabled)
-    video_runtime = PicoVideoRuntime(provider=provider, config=video_cfg, mode="sim2real")
+    provider = _build_provider(args, video_cfg)
+    video_runtime = PicoVideoRuntime(provider=provider, config=video_cfg)
     total = 0
     valid = 0
     invalid_reasons: Counter[str] = Counter()
@@ -231,7 +295,7 @@ def main(cfg: DictConfig) -> None:
     last_stats: dict[str, Any] = {}
     window_start_s = time.monotonic()
     start_s = window_start_s
-    sleep_s = 1.0 / max(poll_hz, 1.0)
+    sleep_s = 1.0 / DEFAULT_POLL_HZ
     video_start_done: threading.Event | None = None
 
     try:
@@ -240,7 +304,7 @@ def main(cfg: DictConfig) -> None:
             video_start_done = _start_video_runtime_async(video_runtime)
         while not stop_event.is_set():
             now = time.monotonic()
-            if duration_s > 0.0 and now - start_s >= duration_s:
+            if args.duration_s > 0.0 and now - start_s >= args.duration_s:
                 break
 
             video_runtime.tick()
@@ -264,7 +328,7 @@ def main(cfg: DictConfig) -> None:
                             _log_invalid(seq, last_age_ms, result)
 
             now = time.monotonic()
-            if now - window_start_s >= summary_interval_s:
+            if now - window_start_s >= DEFAULT_SUMMARY_INTERVAL_S:
                 _log_summary(
                     window_s=now - window_start_s,
                     total=total,
@@ -286,7 +350,7 @@ def main(cfg: DictConfig) -> None:
                 video_start_done = None
             stop_event.wait(timeout=sleep_s)
     except KeyboardInterrupt:
-        logger.info("KeyboardInterrupt -- stopping Pico signal diagnostic")
+        logger.info("KeyboardInterrupt -- stopping Pico Bridge diagnostic")
     finally:
         video_runtime.stop()
         provider.close()
diff --git a/scripts/run/run_high_level_policy_sim2real.py b/scripts/run/run_high_level_policy_sim2real.py
new file mode 100644
index 00000000..acc664c7
--- /dev/null
+++ b/scripts/run/run_high_level_policy_sim2real.py
@@ -0,0 +1,59 @@
+"""Run host high-level-policy control through Teleopit's onboard motion tracker."""
+
+from __future__ import annotations
+
+import inspect
+
+import hydra
+from omegaconf import DictConfig
+
+from teleopit.high_level_policy.config import parse_high_level_policy_config
+from teleopit.runtime.cli import validate_policy_path
+from teleopit.runtime.console import (
+    PlainConsole,
+    configure_runtime_logging,
+    high_level_policy_operator_controls,
+)
+from teleopit.sim2real.mp import HighLevelPolicySim2RealRuntime
+
+
+@hydra.main(
+    version_base=None,
+    config_path="../../teleopit/configs",
+    config_name="high_level_policy_sim2real",
+)
+def main(cfg: DictConfig) -> None:
+    _run_high_level_policy_sim2real(cfg)
+
+
+def _run_high_level_policy_sim2real(cfg: DictConfig) -> None:
+    configure_runtime_logging(cfg, force=True)
+    validate_policy_path(cfg, "run_high_level_policy_sim2real.py")
+    policy_cfg = parse_high_level_policy_config(cfg)
+    console = PlainConsole(title="Teleopit high-level policy sim2real")
+    runtime_params = inspect.signature(HighLevelPolicySim2RealRuntime).parameters
+    runtime = (
+        HighLevelPolicySim2RealRuntime(cfg, console=console)
+        if "console" in runtime_params
+        else HighLevelPolicySim2RealRuntime(cfg)
+    )
+    console.start(
+        status=(
+            ("State", "IDLE"),
+            ("Runtime", "high-level policy"),
+            ("Host", policy_cfg.endpoint),
+            ("Task", policy_cfg.task),
+        ),
+        controls=high_level_policy_operator_controls(),
+        events=("Start enters STANDING; Remote Y requests host-policy takeover",),
+        control_section="Controls",
+        show_help_key=False,
+    )
+    try:
+        runtime.run()
+    finally:
+        runtime.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/scripts/setup/download_somehand_l6_assets.sh b/scripts/setup/download_somehand_assets.sh
similarity index 100%
rename from scripts/setup/download_somehand_l6_assets.sh
rename to scripts/setup/download_somehand_assets.sh
diff --git a/scripts/view/view_recording.py b/scripts/view/view_recording.py
new file mode 100644
index 00000000..e8dbc9ed
--- /dev/null
+++ b/scripts/view/view_recording.py
@@ -0,0 +1,1207 @@
+#!/usr/bin/env python3
+"""Read-only synchronized reviewer for Teleopit sim2real recordings."""
+
+from __future__ import annotations
+
+import argparse
+from dataclasses import dataclass
+import html
+import json
+from pathlib import Path
+import sys
+import time
+from typing import Any
+
+import h5py
+import numpy as np
+
+PROJECT_ROOT = Path(__file__).resolve().parents[2]
+if str(PROJECT_ROOT) not in sys.path:
+    sys.path.insert(0, str(PROJECT_ROOT))
+
+from teleopit.constants import FULL_QPOS_DIM, NUM_JOINTS
+from teleopit.recording.hdf5 import (
+    ACTION_KEY,
+    FRAME_INDEX_KEY,
+    HAND_ACTION_KEY,
+    HAND_STATE_KEY,
+    HDF5_RECORDING_FORMAT,
+    HDF5_RECORDING_VERSION,
+    MODE_KEY,
+    NECK_ACTION_KEY,
+    NECK_STATE_KEY,
+    STATE_KEY,
+    TIMESTAMP_KEY,
+)
+from teleopit.runtime.assets import UNITREE_G1_XML, missing_gmr_assets_message
+
+
+DEFAULT_RECORDING_ROOT = PROJECT_ROOT / "data" / "recordings" / "sim2real_hdf5"
+DEFAULT_XML = UNITREE_G1_XML
+
+JOINT_GROUPS: tuple[tuple[str, slice], ...] = (
+    ("left leg", slice(0, 6)),
+    ("right leg", slice(6, 12)),
+    ("waist", slice(12, 15)),
+    ("left arm", slice(15, 22)),
+    ("right arm", slice(22, 29)),
+)
+GROUP_COLORS = ("#3b82f6", "#06b6d4", "#f59e0b", "#ec4899", "#8b5cf6")
+
+
+@dataclass(frozen=True)
+class RecordingEpisode:
+    episode_index: int
+    frames: int
+    task: str
+    data_path: Path
+    video_path: Path
+
+    def label(self, fps: int) -> str:
+        duration_s = self.frames / fps
+        return f"#{self.episode_index:06d} · {self.task} · {duration_s:.1f}s"
+
+
+@dataclass(frozen=True)
+class RecordingDataset:
+    root: Path
+    schema: dict[str, Any]
+    features: dict[str, Any]
+    fps: int
+    image_key: str
+    image_shape: tuple[int, int, int]
+    mode_names: dict[int, str]
+    joint_names: tuple[str, ...]
+    hand_names: tuple[str, ...]
+    has_hand_action: bool
+    has_neck_action: bool
+    episodes: tuple[RecordingEpisode, ...]
+
+
+@dataclass(frozen=True)
+class EpisodeReviewData:
+    episode: RecordingEpisode
+    frame_index: np.ndarray
+    timestamps: np.ndarray
+    state: np.ndarray
+    mode: np.ndarray
+    action: np.ndarray
+    hand_state: np.ndarray | None
+    hand_action: np.ndarray | None
+    neck_state: np.ndarray | None
+    neck_action: np.ndarray | None
+    joint_error: np.ndarray
+    group_error: dict[str, np.ndarray]
+    root_orientation_error_rad: np.ndarray
+    joint_rmse_rad: float
+    root_orientation_rmse_rad: float
+    max_joint_error_rad: float
+    max_joint_error_frame: int
+    max_joint_error_name: str
+
+
+def _read_json_object(path: Path, *, label: str) -> dict[str, Any]:
+    try:
+        payload = json.loads(path.read_text(encoding="utf-8"))
+    except FileNotFoundError as exc:
+        raise ValueError(f"Recording {label} not found: {path}") from exc
+    except (OSError, json.JSONDecodeError) as exc:
+        raise ValueError(f"Invalid recording {label}: {path}") from exc
+    if not isinstance(payload, dict):
+        raise ValueError(f"Recording {label} must contain a JSON object: {path}")
+    return payload
+
+
+def _feature_shape(features: dict[str, Any], key: str) -> tuple[int, ...]:
+    feature = features.get(key)
+    if not isinstance(feature, dict):
+        raise ValueError(f"Recording schema is missing feature {key!r}")
+    shape = feature.get("shape")
+    if not isinstance(shape, list) or not all(isinstance(value, int) for value in shape):
+        raise ValueError(f"Recording schema feature {key!r} has invalid shape {shape!r}")
+    return tuple(shape)
+
+
+def _feature_dtype(features: dict[str, Any], key: str) -> np.dtype:
+    feature = features.get(key)
+    raw_dtype = feature.get("dtype") if isinstance(feature, dict) else None
+    if not isinstance(raw_dtype, str):
+        raise ValueError(
+            f"Recording schema feature {key!r} has invalid dtype {raw_dtype!r}"
+        )
+    try:
+        return np.dtype(raw_dtype)
+    except TypeError as exc:
+        raise ValueError(
+            f"Recording schema feature {key!r} has invalid dtype {raw_dtype!r}"
+        ) from exc
+
+
+def _feature_names(features: dict[str, Any], key: str, expected: int) -> tuple[str, ...]:
+    feature = features.get(key)
+    names = feature.get("names") if isinstance(feature, dict) else None
+    if not isinstance(names, list) or len(names) != expected:
+        raise ValueError(
+            f"Recording schema feature {key!r} must define {expected} names, got {names!r}"
+        )
+    return tuple(str(name) for name in names)
+
+
+def _resolve_recording_path(root: Path, value: object, *, label: str) -> Path:
+    if not isinstance(value, str) or not value.strip():
+        raise ValueError(f"Recording manifest {label} must be a non-empty relative path")
+    relative = Path(value)
+    if relative.is_absolute():
+        raise ValueError(f"Recording manifest {label} must be relative to {root}: {value!r}")
+    resolved = (root / relative).resolve()
+    try:
+        resolved.relative_to(root)
+    except ValueError as exc:
+        raise ValueError(f"Recording manifest {label} escapes dataset root: {value!r}") from exc
+    if not resolved.is_file():
+        raise ValueError(f"Recording manifest {label} not found: {resolved}")
+    return resolved
+
+
+def _validate_hdf5_episode(
+    data_path: Path,
+    *,
+    frames: int,
+    features: dict[str, Any],
+    keys: tuple[str, ...],
+) -> None:
+    try:
+        with h5py.File(data_path, "r") as h5:
+            for key in keys:
+                if key not in h5:
+                    raise ValueError(f"Recording episode {data_path} is missing HDF5 dataset {key!r}")
+                expected_shape = (frames, *_feature_shape(features, key))
+                actual_shape = tuple(h5[key].shape)
+                if actual_shape != expected_shape:
+                    raise ValueError(
+                        f"Recording episode {data_path} dataset {key!r} shape {actual_shape} "
+                        f"!= manifest/schema shape {expected_shape}"
+                    )
+                expected_dtype = _feature_dtype(features, key)
+                actual_dtype = h5[key].dtype
+                if actual_dtype != expected_dtype:
+                    raise ValueError(
+                        f"Recording episode {data_path} dataset {key!r} dtype {actual_dtype} "
+                        f"!= schema dtype {expected_dtype}"
+                    )
+    except OSError as exc:
+        raise ValueError(f"Cannot open recording episode HDF5: {data_path}") from exc
+
+
+def load_recording_dataset(recording_root: str | Path) -> RecordingDataset:
+    """Load and validate the dataset-level schema and episode manifest."""
+
+    root = Path(recording_root).expanduser().resolve()
+    if not root.is_dir():
+        raise ValueError(f"Recording root not found: {root}")
+
+    schema = _read_json_object(root / "schema.json", label="schema.json")
+    if schema.get("format") != HDF5_RECORDING_FORMAT:
+        raise ValueError(
+            f"Unsupported recording format {schema.get('format')!r}; expected {HDF5_RECORDING_FORMAT!r}"
+        )
+    if schema.get("version") != HDF5_RECORDING_VERSION:
+        raise ValueError(
+            f"Unsupported recording version {schema.get('version')!r}; expected {HDF5_RECORDING_VERSION}"
+        )
+
+    fps = schema.get("fps")
+    if not isinstance(fps, int) or fps <= 0:
+        raise ValueError(f"Recording schema fps must be a positive integer, got {fps!r}")
+    features = schema.get("features")
+    if not isinstance(features, dict):
+        raise ValueError("Recording schema features must be an object")
+
+    expected_shapes = {
+        FRAME_INDEX_KEY: (),
+        TIMESTAMP_KEY: (),
+        STATE_KEY: (68,),
+        MODE_KEY: (),
+        ACTION_KEY: (FULL_QPOS_DIM,),
+    }
+    for key, expected_shape in expected_shapes.items():
+        actual_shape = _feature_shape(features, key)
+        if actual_shape != expected_shape:
+            raise ValueError(
+                f"Recording schema feature {key!r} shape {actual_shape} != {expected_shape}"
+            )
+
+    video_keys = [
+        str(key)
+        for key, feature in features.items()
+        if isinstance(feature, dict) and feature.get("dtype") == "video"
+    ]
+    if len(video_keys) != 1:
+        raise ValueError(
+            f"Recording reviewer requires exactly one video feature, found {video_keys}"
+        )
+    image_key = video_keys[0]
+    image_shape = _feature_shape(features, image_key)
+    if len(image_shape) != 3 or image_shape[2] != 3 or min(image_shape) <= 0:
+        raise ValueError(
+            f"Recording video feature {image_key!r} must have [height, width, 3] shape, got {image_shape}"
+        )
+
+    mode_feature = features[MODE_KEY]
+    raw_mode_values = mode_feature.get("values") if isinstance(mode_feature, dict) else None
+    if not isinstance(raw_mode_values, dict) or not raw_mode_values:
+        raise ValueError(f"Recording schema feature {MODE_KEY!r} must define mode values")
+    mode_names: dict[int, str] = {}
+    for name, value in raw_mode_values.items():
+        if not isinstance(value, int) or value in mode_names:
+            raise ValueError(f"Recording schema has invalid or duplicate mode code {value!r}")
+        mode_names[value] = str(name)
+
+    action_names = _feature_names(features, ACTION_KEY, FULL_QPOS_DIM)
+    joint_names = action_names[7:]
+    if len(joint_names) != NUM_JOINTS:
+        raise ValueError(
+            f"Recording action names must contain {NUM_JOINTS} reference joints, got {len(joint_names)}"
+        )
+
+    hand_type = str(schema.get("hand_type", "none")).strip().lower()
+    neck_type = str(schema.get("neck_type", "none")).strip().lower()
+    has_hand_action = hand_type != "none"
+    has_neck_action = neck_type != "none"
+    hand_names: tuple[str, ...] = ()
+    if has_hand_action:
+        if _feature_shape(features, HAND_STATE_KEY) != (12,):
+            raise ValueError(f"Recording schema feature {HAND_STATE_KEY!r} must be 12D")
+        _feature_names(features, HAND_STATE_KEY, 12)
+        if _feature_shape(features, HAND_ACTION_KEY) != (12,):
+            raise ValueError(f"Recording schema feature {HAND_ACTION_KEY!r} must be 12D")
+        hand_names = _feature_names(features, HAND_ACTION_KEY, 12)
+    elif HAND_STATE_KEY in features or HAND_ACTION_KEY in features:
+        raise ValueError(f"Recording schema hand_type={hand_type!r} must not define hand features")
+    if has_neck_action:
+        if _feature_shape(features, NECK_STATE_KEY) != (2,):
+            raise ValueError(f"Recording schema feature {NECK_STATE_KEY!r} must be 2D")
+        _feature_names(features, NECK_STATE_KEY, 2)
+        if _feature_shape(features, NECK_ACTION_KEY) != (2,):
+            raise ValueError(f"Recording schema feature {NECK_ACTION_KEY!r} must be 2D")
+        _feature_names(features, NECK_ACTION_KEY, 2)
+    elif NECK_STATE_KEY in features or NECK_ACTION_KEY in features:
+        raise ValueError(f"Recording schema neck_type={neck_type!r} must not define neck features")
+
+    hdf5_keys = [FRAME_INDEX_KEY, TIMESTAMP_KEY, STATE_KEY, MODE_KEY, ACTION_KEY]
+    if has_hand_action:
+        hdf5_keys.extend((HAND_STATE_KEY, HAND_ACTION_KEY))
+    if has_neck_action:
+        hdf5_keys.extend((NECK_STATE_KEY, NECK_ACTION_KEY))
+
+    manifest_path = root / "episodes.jsonl"
+    try:
+        lines = manifest_path.read_text(encoding="utf-8").splitlines()
+    except OSError as exc:
+        raise ValueError(f"Recording manifest not found or unreadable: {manifest_path}") from exc
+
+    episodes: list[RecordingEpisode] = []
+    for line_number, raw_line in enumerate(lines, start=1):
+        if not raw_line.strip():
+            continue
+        try:
+            entry = json.loads(raw_line)
+        except json.JSONDecodeError as exc:
+            raise ValueError(f"Invalid JSON in {manifest_path}:{line_number}") from exc
+        if not isinstance(entry, dict):
+            raise ValueError(f"Recording manifest entry at line {line_number} must be an object")
+
+        expected_index = len(episodes)
+        episode_index = entry.get("episode_index")
+        if episode_index != expected_index:
+            raise ValueError(
+                f"Recording episode indices must be contiguous from 0; line {line_number} "
+                f"expected {expected_index}, got {episode_index!r}"
+            )
+        frames = entry.get("frames")
+        if not isinstance(frames, int) or frames <= 0:
+            raise ValueError(
+                f"Recording manifest entry {episode_index} frames must be positive, got {frames!r}"
+            )
+        task = entry.get("task")
+        if not isinstance(task, str) or not task.strip():
+            raise ValueError(f"Recording manifest entry {episode_index} task must not be empty")
+
+        videos = entry.get("videos")
+        if not isinstance(videos, dict) or image_key not in videos:
+            raise ValueError(
+                f"Recording manifest entry {episode_index} is missing video path for {image_key!r}"
+            )
+        data_path = _resolve_recording_path(
+            root,
+            entry.get("data"),
+            label=f"episode {episode_index} data",
+        )
+        video_path = _resolve_recording_path(
+            root,
+            videos[image_key],
+            label=f"episode {episode_index} video {image_key}",
+        )
+        _validate_hdf5_episode(
+            data_path,
+            frames=frames,
+            features=features,
+            keys=tuple(hdf5_keys),
+        )
+        episodes.append(
+            RecordingEpisode(
+                episode_index=episode_index,
+                frames=frames,
+                task=task.strip(),
+                data_path=data_path,
+                video_path=video_path,
+            )
+        )
+
+    if not episodes:
+        raise ValueError(f"Recording manifest has no saved episodes: {manifest_path}")
+
+    return RecordingDataset(
+        root=root,
+        schema=schema,
+        features=features,
+        fps=fps,
+        image_key=image_key,
+        image_shape=image_shape,
+        mode_names=mode_names,
+        joint_names=joint_names,
+        hand_names=hand_names,
+        has_hand_action=has_hand_action,
+        has_neck_action=has_neck_action,
+        episodes=tuple(episodes),
+    )
+
+
+def _normalized_quaternions(values: np.ndarray, *, label: str) -> np.ndarray:
+    norms = np.linalg.norm(values, axis=1, keepdims=True)
+    if np.any(norms < 1e-6):
+        bad_frame = int(np.flatnonzero(norms[:, 0] < 1e-6)[0])
+        raise ValueError(f"{label} contains a zero quaternion at frame {bad_frame}")
+    return values / norms
+
+
+def load_episode_review_data(
+    dataset: RecordingDataset,
+    episode: RecordingEpisode,
+) -> EpisodeReviewData:
+    """Load one episode and compute synchronized tracking metrics."""
+
+    keys = [FRAME_INDEX_KEY, TIMESTAMP_KEY, STATE_KEY, MODE_KEY, ACTION_KEY]
+    if dataset.has_hand_action:
+        keys.extend((HAND_STATE_KEY, HAND_ACTION_KEY))
+    if dataset.has_neck_action:
+        keys.extend((NECK_STATE_KEY, NECK_ACTION_KEY))
+    with h5py.File(episode.data_path, "r") as h5:
+        arrays = {key: np.asarray(h5[key]) for key in keys}
+
+    frame_index = arrays[FRAME_INDEX_KEY]
+    timestamps = arrays[TIMESTAMP_KEY].astype(np.float64, copy=False)
+    state = arrays[STATE_KEY].astype(np.float64, copy=False)
+    mode = arrays[MODE_KEY].astype(np.int64, copy=False)
+    action = arrays[ACTION_KEY].astype(np.float64, copy=False)
+    hand_state = (
+        arrays[HAND_STATE_KEY].astype(np.float64, copy=False)
+        if dataset.has_hand_action
+        else None
+    )
+    hand_action = (
+        arrays[HAND_ACTION_KEY].astype(np.float64, copy=False)
+        if dataset.has_hand_action
+        else None
+    )
+    neck_state = (
+        arrays[NECK_STATE_KEY].astype(np.float64, copy=False)
+        if dataset.has_neck_action
+        else None
+    )
+    neck_action = (
+        arrays[NECK_ACTION_KEY].astype(np.float64, copy=False)
+        if dataset.has_neck_action
+        else None
+    )
+
+    expected_frame_index = np.arange(episode.frames, dtype=frame_index.dtype)
+    if not np.array_equal(frame_index, expected_frame_index):
+        raise ValueError(
+            f"Recording episode {episode.episode_index} frame_index must be contiguous from 0"
+        )
+    numeric_arrays = {
+        TIMESTAMP_KEY: timestamps,
+        STATE_KEY: state,
+        ACTION_KEY: action,
+    }
+    if hand_state is not None:
+        numeric_arrays[HAND_STATE_KEY] = hand_state
+    if hand_action is not None:
+        numeric_arrays[HAND_ACTION_KEY] = hand_action
+    if neck_state is not None:
+        numeric_arrays[NECK_STATE_KEY] = neck_state
+    if neck_action is not None:
+        numeric_arrays[NECK_ACTION_KEY] = neck_action
+    for key, values in numeric_arrays.items():
+        if not np.isfinite(values).all():
+            raise ValueError(
+                f"Recording episode {episode.episode_index} dataset {key!r} contains NaN or Inf"
+            )
+    if episode.frames > 1 and np.any(np.diff(timestamps) <= 0.0):
+        raise ValueError(
+            f"Recording episode {episode.episode_index} timestamps must be strictly increasing"
+        )
+    invalid_modes = sorted(set(int(value) for value in np.unique(mode)) - set(dataset.mode_names))
+    if invalid_modes:
+        raise ValueError(
+            f"Recording episode {episode.episode_index} contains unknown mode codes {invalid_modes}"
+        )
+
+    actual_joint_pos = state[:, :NUM_JOINTS]
+    reference_joint_pos = action[:, 7:]
+    joint_error = actual_joint_pos - reference_joint_pos
+    group_error = {
+        name: np.sqrt(np.mean(np.square(joint_error[:, indices]), axis=1))
+        for name, indices in JOINT_GROUPS
+    }
+
+    actual_quat = _normalized_quaternions(
+        state[:, 58:62],
+        label=f"episode {episode.episode_index} observation base quaternion",
+    )
+    reference_quat = _normalized_quaternions(
+        action[:, 3:7],
+        label=f"episode {episode.episode_index} reference root quaternion",
+    )
+    quat_dot = np.abs(np.sum(actual_quat * reference_quat, axis=1))
+    root_orientation_error_rad = 2.0 * np.arccos(np.clip(quat_dot, 0.0, 1.0))
+
+    max_frame, max_joint = np.unravel_index(
+        int(np.argmax(np.abs(joint_error))),
+        joint_error.shape,
+    )
+    return EpisodeReviewData(
+        episode=episode,
+        frame_index=frame_index,
+        timestamps=timestamps,
+        state=state,
+        mode=mode,
+        action=action,
+        hand_state=hand_state,
+        hand_action=hand_action,
+        neck_state=neck_state,
+        neck_action=neck_action,
+        joint_error=joint_error,
+        group_error=group_error,
+        root_orientation_error_rad=root_orientation_error_rad,
+        joint_rmse_rad=float(np.sqrt(np.mean(np.square(joint_error)))),
+        root_orientation_rmse_rad=float(
+            np.sqrt(np.mean(np.square(root_orientation_error_rad)))
+        ),
+        max_joint_error_rad=float(abs(joint_error[max_frame, max_joint])),
+        max_joint_error_frame=int(max_frame),
+        max_joint_error_name=dataset.joint_names[int(max_joint)],
+    )
+
+
+def aligned_qpos_pair(data: EpisodeReviewData, frame: int) -> tuple[np.ndarray, np.ndarray]:
+    """Return actual/reference qpos with actual root position aligned to reference."""
+
+    if frame < 0 or frame >= data.episode.frames:
+        raise IndexError(f"Frame {frame} outside episode range [0, {data.episode.frames - 1}]")
+    reference_qpos = data.action[frame].copy()
+    reference_qpos[3:7] = _normalized_quaternions(
+        reference_qpos[None, 3:7],
+        label="reference root quaternion",
+    )[0]
+    actual_qpos = reference_qpos.copy()
+    actual_qpos[3:7] = _normalized_quaternions(
+        data.state[frame : frame + 1, 58:62],
+        label="observation base quaternion",
+    )[0]
+    actual_qpos[7:] = data.state[frame, :NUM_JOINTS]
+    return actual_qpos, reference_qpos
+
+
+class RecordingVideoReader:
+    def __init__(self, dataset: RecordingDataset, episode: RecordingEpisode) -> None:
+        try:
+            import cv2
+        except ImportError as exc:
+            raise RuntimeError(
+                "Recording review requires OpenCV; install with pip install -e '.[review]'"
+            ) from exc
+        self._cv2 = cv2
+        self._episode = episode
+        self._expected_shape = dataset.image_shape
+        self._capture = cv2.VideoCapture(str(episode.video_path))
+        if not self._capture.isOpened():
+            raise RuntimeError(f"Cannot open recording video: {episode.video_path}")
+        reported_frames = int(round(self._capture.get(cv2.CAP_PROP_FRAME_COUNT)))
+        reported_fps = float(self._capture.get(cv2.CAP_PROP_FPS))
+        if reported_frames != episode.frames:
+            self.close()
+            raise ValueError(
+                f"Recording episode {episode.episode_index} MP4 frame count {reported_frames} "
+                f"!= manifest/HDF5 frame count {episode.frames}"
+            )
+        if abs(reported_fps - dataset.fps) > 0.1:
+            self.close()
+            raise ValueError(
+                f"Recording episode {episode.episode_index} MP4 fps {reported_fps:g} "
+                f"!= schema fps {dataset.fps}"
+            )
+        self._next_frame = 0
+
+    def read_frame(self, frame: int) -> np.ndarray:
+        if frame != self._next_frame:
+            self._capture.set(self._cv2.CAP_PROP_POS_FRAMES, frame)
+        ok, bgr = self._capture.read()
+        if not ok or bgr is None:
+            raise RuntimeError(
+                f"Failed to decode episode {self._episode.episode_index} video frame {frame}"
+            )
+        self._next_frame = frame + 1
+        rgb = self._cv2.cvtColor(bgr, self._cv2.COLOR_BGR2RGB)
+        if tuple(rgb.shape) != self._expected_shape:
+            raise ValueError(
+                f"Episode {self._episode.episode_index} video frame shape {rgb.shape} "
+                f"!= schema shape {self._expected_shape}"
+            )
+        return rgb
+
+    def close(self) -> None:
+        if getattr(self, "_capture", None) is not None:
+            self._capture.release()
+
+
+class _PrefixedSceneApi:
+    """Prefix Viser node names so two MuJoCo scenes can share one server."""
+
+    def __init__(self, scene: Any, prefix: str) -> None:
+        self._scene = scene
+        self._prefix = prefix
+
+    def __getattr__(self, name: str) -> Any:
+        attribute = getattr(self._scene, name)
+        if callable(attribute) and name.startswith("add_"):
+            return lambda path, *args, **kwargs: attribute(
+                self._prefix + str(path),
+                *args,
+                **kwargs,
+            )
+        return attribute
+
+
+class _PrefixedViserServer:
+    def __init__(self, server: Any, prefix: str) -> None:
+        self._server = server
+        self.scene = _PrefixedSceneApi(server.scene, prefix)
+
+    def __getattr__(self, name: str) -> Any:
+        return getattr(self._server, name)
+
+
+class RobotOverlayScene:
+    """Show observed G1 geometry with a translucent green reference overlay."""
+
+    def __init__(self, server: Any, xml_path: Path) -> None:
+        try:
+            import mujoco
+            from mjviser import ViserMujocoScene
+        except ImportError as exc:
+            raise RuntimeError(
+                "Recording review requires mjviser; install with pip install -e '.[review]'"
+            ) from exc
+
+        self._mujoco = mujoco
+        self._actual_model = mujoco.MjModel.from_xml_path(str(xml_path))
+        self._reference_model = mujoco.MjModel.from_xml_path(str(xml_path))
+        for model in (self._actual_model, self._reference_model):
+            if model.nq != FULL_QPOS_DIM:
+                raise ValueError(
+                    f"Recording reviewer robot XML nq={model.nq} != action dim "
+                    f"{FULL_QPOS_DIM}: {xml_path}"
+                )
+
+        reference_color = np.array([0.1, 0.95, 0.2], dtype=np.float32)
+        self._reference_model.geom_rgba[:, :3] = reference_color
+        visible_geoms = self._reference_model.geom_rgba[:, 3] > 0.0
+        self._reference_model.geom_rgba[visible_geoms, 3] = 0.38
+        world_geoms = self._reference_model.geom_bodyid == 0
+        self._reference_model.geom_rgba[world_geoms, 3] = 0.0
+        if self._reference_model.nmat > 0:
+            self._reference_model.mat_rgba[:, :3] = reference_color
+            visible_materials = self._reference_model.mat_rgba[:, 3] > 0.0
+            self._reference_model.mat_rgba[visible_materials, 3] = 0.38
+            world_materials = np.unique(self._reference_model.geom_matid[world_geoms])
+            world_materials = world_materials[world_materials >= 0]
+            self._reference_model.mat_rgba[world_materials, 3] = 0.0
+
+        self._actual_root = server.scene.add_frame("/actual", show_axes=False)
+        self._reference_root = server.scene.add_frame("/reference", show_axes=False)
+        self._actual_scene = ViserMujocoScene(
+            _PrefixedViserServer(server, "/actual"),
+            self._actual_model,
+            num_envs=1,
+        )
+        self._reference_scene = ViserMujocoScene(
+            _PrefixedViserServer(server, "/reference"),
+            self._reference_model,
+            num_envs=1,
+        )
+        self._actual_data = mujoco.MjData(self._actual_model)
+        self._reference_data = mujoco.MjData(self._reference_model)
+
+    def update(
+        self,
+        actual_qpos: np.ndarray,
+        reference_qpos: np.ndarray,
+        *,
+        show_reference: bool,
+    ) -> None:
+        self._actual_data.qpos[:] = actual_qpos
+        self._reference_data.qpos[:] = reference_qpos
+        self._mujoco.mj_forward(self._actual_model, self._actual_data)
+        self._mujoco.mj_forward(self._reference_model, self._reference_data)
+        self._actual_scene.update_from_mjdata(self._actual_data)
+        self._reference_root.visible = show_reference
+        if show_reference:
+            self._reference_scene.update_from_mjdata(self._reference_data)
+
+    def close(self) -> None:
+        self._actual_root.remove()
+        self._reference_root.remove()
+
+
+class RecordingReviewerApp:
+    def __init__(
+        self,
+        *,
+        dataset: RecordingDataset,
+        xml_path: Path,
+        port: int,
+        initial_episode: int,
+    ) -> None:
+        try:
+            import viser
+        except ImportError as exc:
+            raise RuntimeError(
+                "Recording review requires Viser; install with pip install -e '.[review]'"
+            ) from exc
+
+        self._dataset = dataset
+        self._episode_position = initial_episode
+        self._data = load_episode_review_data(dataset, dataset.episodes[initial_episode])
+        self._video = RecordingVideoReader(dataset, self._data.episode)
+        self._server = viser.ViserServer(port=port, label="Recording Reviewer")
+        self._server.gui.configure_theme(
+            control_layout="fixed",
+            control_width="large",
+            dark_mode=True,
+            show_share_button=False,
+            brand_color=(34, 197, 94),
+        )
+        self._server.scene.world_axes.visible = False
+        self._server.initial_camera.position = (2.4, -2.4, 1.8)
+        self._server.initial_camera.look_at = (0.0, 0.0, 0.8)
+        self._server.initial_camera.up = (0.0, 0.0, 1.0)
+        self._server.initial_camera.fov = 45.0
+        try:
+            self._robot_scene = RobotOverlayScene(self._server, xml_path)
+        except Exception:
+            self._video.close()
+            self._server.stop()
+            raise
+
+        self._playing = False
+        self._speed = 1.0
+        self._frame_accumulator = 0.0
+        self._current_frame = -1
+        self._show_reference = True
+        self._pending_actions: list[str] = []
+        self._pending_episode: int | None = None
+        self._pending_scrub: int | None = None
+        self._pending_joint: str | None = None
+        self._pending_hand: str | None = None
+
+        self._joint_chart: Any | None = None
+        self._group_chart: Any | None = None
+        self._mode_chart: Any | None = None
+        self._hand_chart: Any | None = None
+        self._neck_chart: Any | None = None
+        self._setup_gui(self._video.read_frame(0))
+        self._set_frame(0, force=True)
+
+    def _setup_gui(self, first_camera_frame: np.ndarray) -> None:
+        gui = self._server.gui
+        episode_labels = [episode.label(self._dataset.fps) for episode in self._dataset.episodes]
+
+        with gui.add_folder("Camera", order=0):
+            self._camera_image = gui.add_image(
+                first_camera_frame,
+                label="D435i RGB",
+                format="jpeg",
+                jpeg_quality=82,
+            )
+            gui.add_markdown(
+                "The main view is interactive 3D: the recorded robot state uses its normal "
+                "appearance and the motion-tracker reference is translucent green."
+            )
+            self._current_html = gui.add_html("")
+
+        with gui.add_folder("Episode", order=1):
+            self._episode_dropdown = gui.add_dropdown(
+                "Episode",
+                options=episode_labels,
+                initial_value=episode_labels[self._episode_position],
+            )
+            self._summary_html = gui.add_html("")
+
+            @self._episode_dropdown.on_update
+            def _(_) -> None:
+                selected = episode_labels.index(self._episode_dropdown.value)
+                if selected != self._episode_position:
+                    self._pending_episode = selected
+
+        with gui.add_folder("Playback", order=2):
+            self._play_button = gui.add_button("Play", color="green")
+            self._frame_slider = gui.add_slider(
+                "Frame",
+                min=0,
+                max=max(0, self._data.episode.frames - 1),
+                step=1,
+                initial_value=0,
+            )
+            self._speed_group = gui.add_button_group(
+                "Speed",
+                options=["0.25x", "0.5x", "1x", "2x"],
+            )
+            self._speed_group.value = "1x"
+            self._restart_button = gui.add_button("Restart")
+            self._prev_button = gui.add_button("Previous episode")
+            self._next_button = gui.add_button("Next episode")
+            self._reference_checkbox = gui.add_checkbox(
+                "Show green reference",
+                initial_value=True,
+            )
+            @self._play_button.on_click
+            def _(_) -> None:
+                self._pending_actions.append("toggle_play")
+
+            @self._frame_slider.on_update
+            def _(_) -> None:
+                requested = int(self._frame_slider.value)
+                if requested != self._current_frame:
+                    self._pending_scrub = requested
+
+            @self._speed_group.on_click
+            def _(event) -> None:
+                self._speed = {
+                    "0.25x": 0.25,
+                    "0.5x": 0.5,
+                    "1x": 1.0,
+                    "2x": 2.0,
+                }.get(str(event.target.value), 1.0)
+
+            @self._restart_button.on_click
+            def _(_) -> None:
+                self._pending_actions.append("restart")
+
+            @self._prev_button.on_click
+            def _(_) -> None:
+                self._pending_actions.append("previous")
+
+            @self._next_button.on_click
+            def _(_) -> None:
+                self._pending_actions.append("next")
+
+            @self._reference_checkbox.on_update
+            def _(_) -> None:
+                self._show_reference = bool(self._reference_checkbox.value)
+                self._pending_actions.append("refresh")
+
+        self._tracking_folder = gui.add_folder("Tracking", order=3)
+        with self._tracking_folder:
+            self._joint_dropdown = gui.add_dropdown(
+                "Joint",
+                options=list(self._dataset.joint_names),
+                initial_value=self._dataset.joint_names[0],
+            )
+            gui.add_markdown(
+                "The observed robot uses the reference root position because the recording "
+                "does not contain measured root XYZ. Joint and root-orientation comparisons remain valid."
+            )
+
+            @self._joint_dropdown.on_update
+            def _(_) -> None:
+                self._pending_joint = str(self._joint_dropdown.value)
+
+        self._signals_folder = gui.add_folder("Recorded signals", order=4, expand_by_default=False)
+        with self._signals_folder:
+            gui.add_markdown("Mode: `0 standing`, `1 mocap`, `2 arms`, `3 pause`.")
+            if self._dataset.has_hand_action:
+                self._hand_dropdown = gui.add_dropdown(
+                    "Hand channel",
+                    options=list(self._dataset.hand_names),
+                    initial_value=self._dataset.hand_names[0],
+                )
+
+                @self._hand_dropdown.on_update
+                def _(_) -> None:
+                    self._pending_hand = str(self._hand_dropdown.value)
+            else:
+                self._hand_dropdown = None
+                gui.add_markdown("This dataset does not contain `action.hand`.")
+            if not self._dataset.has_neck_action:
+                gui.add_markdown("This dataset does not contain `action.neck`.")
+
+        self._refresh_summary()
+        self._refresh_charts()
+
+    @staticmethod
+    def _chart_axes(y_label: str) -> tuple[dict[str, Any], dict[str, Any]]:
+        return (
+            {"label": "time (s)", "stroke": "#9ca3af"},
+            {"label": y_label, "stroke": "#9ca3af"},
+        )
+
+    def _add_chart(
+        self,
+        folder: Any,
+        *,
+        data: tuple[np.ndarray, ...],
+        series: tuple[dict[str, Any], ...],
+        title: str,
+        y_label: str,
+        order: float,
+    ) -> Any:
+        with folder:
+            return self._server.gui.add_uplot(
+                data=data,
+                series=series,
+                title=title,
+                axes=self._chart_axes(y_label),
+                legend={"show": True, "live": True},
+                cursor={"show": True, "x": True, "y": True},
+                height=220,
+                order=order,
+            )
+
+    def _refresh_charts(self) -> None:
+        for handle in (
+            self._joint_chart,
+            self._group_chart,
+            self._mode_chart,
+            self._hand_chart,
+            self._neck_chart,
+        ):
+            if handle is not None:
+                handle.remove()
+
+        timestamps = self._data.timestamps.astype(np.float64, copy=False)
+        selected_joint = str(self._joint_dropdown.value)
+        joint_index = self._dataset.joint_names.index(selected_joint)
+        actual = self._data.state[:, joint_index]
+        reference = self._data.action[:, 7 + joint_index]
+        error = self._data.joint_error[:, joint_index]
+        self._joint_chart = self._add_chart(
+            self._tracking_folder,
+            data=(timestamps, actual, reference, error),
+            series=(
+                {"label": "time"},
+                {"label": "actual", "stroke": "#3b82f6", "width": 2.0},
+                {"label": "reference", "stroke": "#22c55e", "width": 2.0},
+                {"label": "error", "stroke": "#ef4444", "width": 1.5},
+            ),
+            title=f"Joint: {selected_joint}",
+            y_label="rad",
+            order=1,
+        )
+        group_values = tuple(self._data.group_error[name] for name, _ in JOINT_GROUPS)
+        group_series: tuple[dict[str, Any], ...] = (
+            {"label": "time"},
+            *tuple(
+                {"label": name, "stroke": color, "width": 1.6}
+                for (name, _), color in zip(JOINT_GROUPS, GROUP_COLORS, strict=True)
+            ),
+        )
+        self._group_chart = self._add_chart(
+            self._tracking_folder,
+            data=(timestamps, *group_values),
+            series=group_series,
+            title="Instantaneous group joint RMSE",
+            y_label="rad",
+            order=2,
+        )
+        self._mode_chart = self._add_chart(
+            self._signals_folder,
+            data=(timestamps, self._data.mode.astype(np.float64)),
+            series=(
+                {"label": "time"},
+                {"label": "mode", "stroke": "#f59e0b", "width": 2.0},
+            ),
+            title="Mode timeline",
+            y_label="mode code",
+            order=1,
+        )
+
+        if (
+            self._data.hand_state is not None
+            and self._data.hand_action is not None
+            and self._hand_dropdown is not None
+        ):
+            selected_hand = str(self._hand_dropdown.value)
+            hand_index = self._dataset.hand_names.index(selected_hand)
+            self._hand_chart = self._add_chart(
+                self._signals_folder,
+                data=(
+                    timestamps,
+                    self._data.hand_state[:, hand_index],
+                    self._data.hand_action[:, hand_index],
+                ),
+                series=(
+                    {"label": "time"},
+                    {"label": "state", "stroke": "#3b82f6", "width": 2.0},
+                    {"label": "target", "stroke": "#8b5cf6", "width": 2.0},
+                ),
+                title=f"LinkerHand state vs target: {selected_hand}",
+                y_label="SDK pose",
+                order=2,
+            )
+        else:
+            self._hand_chart = None
+
+        if self._data.neck_state is not None and self._data.neck_action is not None:
+            self._neck_chart = self._add_chart(
+                self._signals_folder,
+                data=(
+                    timestamps,
+                    self._data.neck_state[:, 0],
+                    self._data.neck_action[:, 0],
+                    self._data.neck_state[:, 1],
+                    self._data.neck_action[:, 1],
+                ),
+                series=(
+                    {"label": "time"},
+                    {"label": "yaw state", "stroke": "#3b82f6", "width": 2.0},
+                    {"label": "yaw target", "stroke": "#06b6d4", "width": 2.0},
+                    {"label": "pitch state", "stroke": "#f59e0b", "width": 2.0},
+                    {"label": "pitch target", "stroke": "#ec4899", "width": 2.0},
+                ),
+                title="OpenNeck state vs target",
+                y_label="degrees",
+                order=3,
+            )
+        else:
+            self._neck_chart = None
+
+    def _mode_name(self, frame: int) -> str:
+        return self._dataset.mode_names[int(self._data.mode[frame])]
+
+    def _set_frame(self, frame: int, *, force: bool = False) -> None:
+        frame = max(0, min(int(frame), self._data.episode.frames - 1))
+        if not force and frame == self._current_frame:
+            return
+        self._camera_image.image = self._video.read_frame(frame)
+        actual_qpos, reference_qpos = aligned_qpos_pair(self._data, frame)
+        self._robot_scene.update(
+            actual_qpos,
+            reference_qpos,
+            show_reference=self._show_reference,
+        )
+        self._current_frame = frame
+        self._frame_slider.value = frame
+        instant_rmse = float(np.sqrt(np.mean(np.square(self._data.joint_error[frame]))))
+        selected_joint = str(self._joint_dropdown.value)
+        joint_index = self._dataset.joint_names.index(selected_joint)
+        selected_error = float(self._data.joint_error[frame, joint_index])
+        self._current_html.content = (
+            "<div style='line-height:1.45'>"
+            f"<strong>Frame:</strong> {frame}/{self._data.episode.frames - 1}<br/>"
+            f"<strong>Time:</strong> {self._data.timestamps[frame]:.2f}s<br/>"
+            f"<strong>Mode:</strong> {html.escape(self._mode_name(frame))}<br/>"
+            f"<strong>Instant joint RMSE:</strong> {instant_rmse:.3f} rad<br/>"
+            f"<strong>{html.escape(selected_joint)} error:</strong> {selected_error:+.3f} rad"
+            "</div>"
+        )
+
+    def _refresh_summary(self) -> None:
+        episode = self._data.episode
+        mode_counts = [
+            f"{html.escape(name)}={int(np.count_nonzero(self._data.mode == code))}"
+            for code, name in sorted(self._dataset.mode_names.items())
+            if np.any(self._data.mode == code)
+        ]
+        self._summary_html.content = (
+            "<div style='line-height:1.45'>"
+            f"<strong>Task:</strong> {html.escape(episode.task)}<br/>"
+            f"<strong>Frames:</strong> {episode.frames} @ {self._dataset.fps} FPS "
+            f"({episode.frames / self._dataset.fps:.2f}s)<br/>"
+            f"<strong>Modes:</strong> {', '.join(mode_counts)}<br/>"
+            f"<strong>Joint RMSE:</strong> {self._data.joint_rmse_rad:.3f} rad "
+            f"({np.degrees(self._data.joint_rmse_rad):.2f}°)<br/>"
+            f"<strong>Root orientation RMSE:</strong> "
+            f"{np.degrees(self._data.root_orientation_rmse_rad):.2f}°<br/>"
+            f"<strong>Max joint error:</strong> {self._data.max_joint_error_rad:.3f} rad "
+            f"at frame {self._data.max_joint_error_frame} "
+            f"({html.escape(self._data.max_joint_error_name)})"
+            "</div>"
+        )
+
+    def _set_playing(self, playing: bool) -> None:
+        self._playing = playing
+        self._frame_accumulator = 0.0
+        self._play_button.label = "Pause" if playing else "Play"
+        self._play_button.color = "red" if playing else "green"
+
+    def _load_episode(self, position: int) -> None:
+        position = max(0, min(position, len(self._dataset.episodes) - 1))
+        if position == self._episode_position:
+            return
+        self._set_playing(False)
+        new_data = load_episode_review_data(
+            self._dataset,
+            self._dataset.episodes[position],
+        )
+        new_video = RecordingVideoReader(self._dataset, new_data.episode)
+        old_video = self._video
+        self._data = new_data
+        self._video = new_video
+        self._episode_position = position
+        old_video.close()
+        self._current_frame = -1
+        self._frame_slider.max = max(0, new_data.episode.frames - 1)
+        self._episode_dropdown.value = new_data.episode.label(self._dataset.fps)
+        self._refresh_summary()
+        self._refresh_charts()
+        self._set_frame(0, force=True)
+
+    def _process_pending(self) -> None:
+        if self._pending_episode is not None:
+            position = self._pending_episode
+            self._pending_episode = None
+            self._load_episode(position)
+
+        if self._pending_joint is not None:
+            self._pending_joint = None
+            self._refresh_charts()
+            self._set_frame(self._current_frame, force=True)
+        if self._pending_hand is not None:
+            self._pending_hand = None
+            self._refresh_charts()
+
+        if self._pending_scrub is not None:
+            frame = self._pending_scrub
+            self._pending_scrub = None
+            self._set_playing(False)
+            self._set_frame(frame)
+
+        while self._pending_actions:
+            action = self._pending_actions.pop(0)
+            if action == "toggle_play":
+                if self._current_frame >= self._data.episode.frames - 1:
+                    self._set_frame(0)
+                self._set_playing(not self._playing)
+            elif action == "restart":
+                self._set_playing(False)
+                self._set_frame(0, force=True)
+            elif action == "previous":
+                self._load_episode(self._episode_position - 1)
+            elif action == "next":
+                self._load_episode(self._episode_position + 1)
+            elif action == "refresh":
+                self._set_frame(self._current_frame, force=True)
+
+    def run(self) -> None:
+        print(f"\nRecording reviewer ready at http://localhost:{self._server.get_port()}")
+        print(f"Dataset: {self._dataset.root}")
+        print("Press Ctrl+C to exit.\n")
+        previous_time = time.monotonic()
+        try:
+            while True:
+                now = time.monotonic()
+                elapsed = now - previous_time
+                previous_time = now
+                self._process_pending()
+                if self._playing:
+                    self._frame_accumulator += elapsed * self._dataset.fps * self._speed
+                    advance = int(self._frame_accumulator)
+                    if advance > 0:
+                        self._frame_accumulator -= advance
+                        next_frame = self._current_frame + advance
+                        if next_frame >= self._data.episode.frames - 1:
+                            self._set_frame(self._data.episode.frames - 1)
+                            self._set_playing(False)
+                        else:
+                            self._set_frame(next_frame)
+                time.sleep(1.0 / 60.0)
+        except KeyboardInterrupt:
+            print("\nShutting down...")
+        finally:
+            self.close()
+
+    def close(self) -> None:
+        self._video.close()
+        self._robot_scene.close()
+        self._server.stop()
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description="Read-only synchronized reviewer for Teleopit sim2real recordings"
+    )
+    parser.add_argument(
+        "--recording",
+        type=str,
+        default=str(DEFAULT_RECORDING_ROOT),
+        help="Recording dataset root containing schema.json and episodes.jsonl",
+    )
+    parser.add_argument("--xml", type=str, default=None, help="Canonical G1 MuJoCo XML path")
+    parser.add_argument("--episode", type=int, default=0, help="Initial episode index")
+    parser.add_argument("--port", type=int, default=8013, help="Viser server port")
+    args = parser.parse_args()
+
+    recording_root = Path(args.recording)
+    if not recording_root.is_absolute():
+        recording_root = (PROJECT_ROOT / recording_root).resolve()
+    try:
+        dataset = load_recording_dataset(recording_root)
+    except ValueError as exc:
+        print(f"ERROR: {exc}", file=sys.stderr)
+        raise SystemExit(1) from exc
+
+    episode_positions = {
+        episode.episode_index: position for position, episode in enumerate(dataset.episodes)
+    }
+    if args.episode not in episode_positions:
+        valid = [episode.episode_index for episode in dataset.episodes]
+        print(f"ERROR: episode {args.episode} not found; available indices: {valid}", file=sys.stderr)
+        raise SystemExit(1)
+
+    xml_path = Path(args.xml).expanduser() if args.xml else DEFAULT_XML
+    if not xml_path.is_absolute():
+        xml_path = (PROJECT_ROOT / xml_path).resolve()
+    if not xml_path.is_file():
+        print(
+            "ERROR: " + missing_gmr_assets_message(xml_path, label="Robot XML"),
+            file=sys.stderr,
+        )
+        raise SystemExit(1)
+
+    try:
+        app = RecordingReviewerApp(
+            dataset=dataset,
+            xml_path=xml_path,
+            port=args.port,
+            initial_episode=episode_positions[args.episode],
+        )
+    except (ImportError, RuntimeError, ValueError) as exc:
+        print(f"ERROR: {exc}", file=sys.stderr)
+        raise SystemExit(1) from exc
+    app.run()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml
new file mode 100644
index 00000000..4f7f51d1
--- /dev/null
+++ b/teleopit/configs/high_level_policy_sim2real.yaml
@@ -0,0 +1,55 @@
+defaults:
+  - sim2real
+  - _self_
+
+# This configuration is launched only by run_high_level_policy_sim2real.py.
+# It does not start PicoBridge, GMR, or the teleoperation reference worker.
+input:
+  provider: high_level_policy
+
+# Smooth an explicit return from POLICY to STANDING.
+standing_return_ramp_duration: 2.0
+
+camera:
+  source: realsense  # realsense | test-pattern
+  width: 640
+  height: 480
+  fps: 30
+  device: null
+
+high_level_policy:
+  enabled: true
+  endpoint: tcp://127.0.0.1:5555
+  task: demo
+  timeout_s: 1.0
+  reconnect_backoff_s: 1.0
+  replan_steps: 3  # Submit a fresh observation every three 30 Hz action frames.
+  jpeg_quality: 90
+  max_observation_age_s: 0.15
+  max_result_age_s: 0.1
+  entry_timeout_s: 5.0
+  hold_s: 3.0
+  safety:
+    root_height_min_m: 0.55
+    root_height_max_m: 1.05
+    max_root_xy_speed_m_s: 2.5
+    max_root_displacement_m: 0.1
+    max_yaw_rate_rad_s: 2.5
+    max_joint_rate_rad_s: 10.0
+    max_joint_projection_rad: 0.1
+    neck_yaw_min_deg: -45.0
+    neck_yaw_max_deg: 45.0
+    neck_pitch_min_deg: -40.0
+    neck_pitch_max_deg: 40.0
+
+recording:
+  enabled: false
+
+hands:
+  enabled: true
+  driver: linkerhand_o6
+  sides: [left, right]
+
+neck:
+  enabled: true
+  driver: openneck
diff --git a/teleopit/configs/input/pico4.yaml b/teleopit/configs/input/pico4.yaml
index 8aa4d7dc..2861f955 100644
--- a/teleopit/configs/input/pico4.yaml
+++ b/teleopit/configs/input/pico4.yaml
@@ -22,4 +22,3 @@ video:
   height: 720
   fps: 30
   device: null
-  fail_on_error: false
diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml
index 9b335e58..3887185e 100644
--- a/teleopit/configs/pico4_sim2real.yaml
+++ b/teleopit/configs/pico4_sim2real.yaml
@@ -97,13 +97,30 @@ hands:
     close_pose: [86, 73, 118, 111, 110, 111]
     print_input: false
   somehand:
-    config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml
+    l6_config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml
+    o6_config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_o6_bihand.yaml
     # Low-latency vr_hand_pose path. This favors response speed over smoothing.
     rate_hz: 60.0
     max_iterations: 12
     temporal_filter_alpha: 1.0
     output_alpha: 1.0
 
+# Optional OpenNeck active-vision gimbal control.
+neck:
+  enabled: false
+  driver: openneck
+  config_path: null
+  port: null
+  rate_hz: 60.0
+  frame_timeout_s: 0.2
+  active_modes: [standing, mocap, arms, pause]
+  dead_zone_deg: 0.5
+  pitch_gain: 1.4
+  center_on_start: true
+  center_on_shutdown: false
+  release_on_shutdown: false
+  dry_run: false
+
 # Physical robot SDK configuration
 real_robot:
   network_interface: "eth0"
diff --git a/teleopit/configs/robot/g1.yaml b/teleopit/configs/robot/g1.yaml
index 014e84f1..4553fd73 100644
--- a/teleopit/configs/robot/g1.yaml
+++ b/teleopit/configs/robot/g1.yaml
@@ -2,6 +2,7 @@
 # Action scale computed as 0.25 * effort_limit / stiffness per actuator.
 # Do NOT copy from deploy.yaml (contains known waist_yaw scale error).
 
+type: unitree_g1_29dof
 num_actions: 29
 # Inference uses the 167D velcmd_history observation path (General-Tracking-G1).
 
diff --git a/teleopit/configs/sim2real.yaml b/teleopit/configs/sim2real.yaml
index f2dbe24f..4adc276c 100644
--- a/teleopit/configs/sim2real.yaml
+++ b/teleopit/configs/sim2real.yaml
@@ -98,13 +98,30 @@ hands:
     close_pose: [86, 73, 118, 111, 110, 111]
     print_input: false
   somehand:
-    config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml
+    l6_config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml
+    o6_config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_o6_bihand.yaml
     # Low-latency vr_hand_pose path. This favors response speed over smoothing.
     rate_hz: 60.0
     max_iterations: 12
     temporal_filter_alpha: 1.0
     output_alpha: 1.0
 
+# Optional OpenNeck active-vision gimbal control. Use only with input.provider=pico4.
+neck:
+  enabled: false
+  driver: openneck
+  config_path: null
+  port: null
+  rate_hz: 60.0
+  frame_timeout_s: 0.2
+  active_modes: [standing, mocap, arms, pause]
+  dead_zone_deg: 0.5
+  pitch_gain: 1.4
+  center_on_start: true
+  center_on_shutdown: false
+  release_on_shutdown: false
+  dry_run: false
+
 # Physical robot SDK configuration
 real_robot:
   network_interface: "eth0"
diff --git a/teleopit/configs/sim2real_record.yaml b/teleopit/configs/sim2real_record.yaml
index 2fda8bf4..c598ab40 100644
--- a/teleopit/configs/sim2real_record.yaml
+++ b/teleopit/configs/sim2real_record.yaml
@@ -13,4 +13,3 @@ input:
     height: 480
     fps: 30
     device: null
-    fail_on_error: true
diff --git a/teleopit/constants.py b/teleopit/constants.py
index 5a4269f8..2f724dd7 100644
--- a/teleopit/constants.py
+++ b/teleopit/constants.py
@@ -5,3 +5,37 @@
 ROOT_DIM = ROOT_POS_DIM + ROOT_QUAT_DIM  # 7: pos(3) + quat_wxyz(4)
 NUM_JOINTS = 29  # G1 actuated joints
 FULL_QPOS_DIM = ROOT_DIM + NUM_JOINTS  # 36: root + joints
+
+# Canonical actuator order used by the downloaded g1_29dof.xml, policy output,
+# Unitree command path, and sim2real recording schema.
+G1_JOINT_NAMES = (
+    "left_hip_pitch_joint",
+    "left_hip_roll_joint",
+    "left_hip_yaw_joint",
+    "left_knee_joint",
+    "left_ankle_pitch_joint",
+    "left_ankle_roll_joint",
+    "right_hip_pitch_joint",
+    "right_hip_roll_joint",
+    "right_hip_yaw_joint",
+    "right_knee_joint",
+    "right_ankle_pitch_joint",
+    "right_ankle_roll_joint",
+    "waist_yaw_joint",
+    "waist_roll_joint",
+    "waist_pitch_joint",
+    "left_shoulder_pitch_joint",
+    "left_shoulder_roll_joint",
+    "left_shoulder_yaw_joint",
+    "left_elbow_joint",
+    "left_wrist_roll_joint",
+    "left_wrist_pitch_joint",
+    "left_wrist_yaw_joint",
+    "right_shoulder_pitch_joint",
+    "right_shoulder_roll_joint",
+    "right_shoulder_yaw_joint",
+    "right_elbow_joint",
+    "right_wrist_roll_joint",
+    "right_wrist_pitch_joint",
+    "right_wrist_yaw_joint",
+)
diff --git a/teleopit/high_level_policy/__init__.py b/teleopit/high_level_policy/__init__.py
new file mode 100644
index 00000000..107bb1bb
--- /dev/null
+++ b/teleopit/high_level_policy/__init__.py
@@ -0,0 +1,21 @@
+"""Lightweight onboard client and scheduler for host high-level policies."""
+
+from teleopit.high_level_policy.client import (
+    HighLevelPolicyClient,
+    PolicyActionChunk,
+    PolicyDescription,
+)
+from teleopit.high_level_policy.hand_calibration import HandCalibration
+from teleopit.high_level_policy.scheduler import (
+    HighLevelPolicyScheduler,
+    PolicyFrameTransform,
+)
+
+__all__ = [
+    "HandCalibration",
+    "HighLevelPolicyClient",
+    "HighLevelPolicyScheduler",
+    "PolicyActionChunk",
+    "PolicyDescription",
+    "PolicyFrameTransform",
+]
diff --git a/teleopit/high_level_policy/client.py b/teleopit/high_level_policy/client.py
new file mode 100644
index 00000000..66ef776c
--- /dev/null
+++ b/teleopit/high_level_policy/client.py
@@ -0,0 +1,303 @@
+"""Synchronous ZeroMQ client used only from the isolated onboard worker."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any, Mapping
+
+import numpy as np
+import zmq
+
+from teleopit.high_level_policy.protocol import (
+    ENDPOINTS,
+    MAX_ACTION_HORIZON,
+    MAX_IMAGE_BYTES,
+    MAX_REQUEST_BYTES,
+    MAX_RESPONSE_BYTES,
+    PolicyProtocolError,
+    decode_float32_array,
+    encode_float32_array,
+    pack_message,
+    unpack_message,
+)
+
+
+class PolicyTransportError(RuntimeError):
+    """The host did not complete a REQ/REP exchange before the deadline."""
+
+
+@dataclass(frozen=True)
+class PolicyDescription:
+    policy_type: str
+    policy_id: str
+    dataset_fps: int
+    max_action_horizon: int
+
+
+@dataclass(frozen=True)
+class PolicyActionChunk:
+    session_id: str
+    source_sequence_id: int
+    source_onboard_monotonic_timestamp_ns: int
+    action_fps: int
+    actions: np.ndarray
+    policy_id: str
+    server_inference_ms: float
+
+
+class HighLevelPolicyClient:
+    def __init__(
+        self,
+        endpoint: str,
+        *,
+        timeout_s: float,
+        context: zmq.Context[Any] | None = None,
+    ) -> None:
+        if not str(endpoint).startswith(("tcp://", "inproc://")):
+            raise ValueError("High-level policy endpoint must use tcp:// (or inproc:// in tests)")
+        if not np.isfinite(timeout_s) or timeout_s <= 0.0:
+            raise ValueError("High-level policy timeout_s must be finite and > 0")
+        self.endpoint = str(endpoint)
+        self.timeout_s = float(timeout_s)
+        self._own_context = context is None
+        self._context = zmq.Context() if context is None else context
+        self._socket: zmq.Socket[Any] | None = None
+        self._open_socket()
+
+    def close(self) -> None:
+        self._close_socket()
+        if self._own_context:
+            self._context.term()
+
+    def ping(self) -> bool:
+        data = self._request("ping", {})
+        if set(data) != {"ready"} or not isinstance(data["ready"], bool):
+            raise PolicyProtocolError("invalid_response", "ping data must contain exactly boolean ready")
+        return bool(data["ready"])
+
+    def describe(self) -> PolicyDescription:
+        data = self._request("describe", {})
+        expected_fields = {
+            "observation_schema",
+            "observation_dim",
+            "action_schema",
+            "action_dim",
+            "dataset_fps",
+            "max_action_horizon",
+            "policy_type",
+            "policy_id",
+            "ready",
+        }
+        if set(data) != expected_fields:
+            raise PolicyProtocolError("invalid_response", "describe data contains unexpected fields")
+        expected_schema = {
+            "observation_schema": "teleopit-g1-joint-pos-dex-neck-state",
+            "observation_dim": 43,
+            "action_schema": "teleopit-g1-reference",
+            "action_dim": 50,
+        }
+        for name, value in expected_schema.items():
+            if data[name] != value:
+                raise PolicyProtocolError(
+                    "schema_mismatch", f"describe {name} must be {value!r}, got {data[name]!r}"
+                )
+        if data["dataset_fps"] != 30:
+            raise PolicyProtocolError(
+                "invalid_response", f"describe dataset_fps must be 30, got {data['dataset_fps']!r}"
+            )
+        if data["ready"] is not True:
+            raise PolicyProtocolError("policy_not_ready", "Host policy reported ready=false")
+        if not isinstance(data["policy_type"], str) or not data["policy_type"]:
+            raise PolicyProtocolError("invalid_response", "describe policy_type must be non-empty")
+        if not isinstance(data["policy_id"], str) or not data["policy_id"]:
+            raise PolicyProtocolError("invalid_response", "describe policy_id must be non-empty")
+        horizon = _int64(data["max_action_horizon"], name="max_action_horizon")
+        if not 1 <= horizon <= MAX_ACTION_HORIZON:
+            raise PolicyProtocolError("invalid_response", "describe max_action_horizon is outside limits")
+        return PolicyDescription(
+            policy_type=data["policy_type"],
+            policy_id=data["policy_id"],
+            dataset_fps=30,
+            max_action_horizon=horizon,
+        )
+
+    def reset(self, session_id: str, task: str) -> None:
+        data = self._request("reset", {"session_id": session_id, "task": task})
+        if set(data) != {"session_id", "reset"} or data["session_id"] != session_id or data["reset"] is not True:
+            raise PolicyProtocolError("invalid_response", "reset acknowledgement does not match the requested session")
+
+    def get_action(
+        self,
+        *,
+        session_id: str,
+        sequence_id: int,
+        onboard_monotonic_timestamp_ns: int,
+        task: str,
+        jpeg_image: bytes,
+        body_joint_positions: object,
+        dex_state: object,
+        neck_state: object,
+        source_reference_root_pose: object,
+    ) -> PolicyActionChunk:
+        if not isinstance(jpeg_image, bytes):
+            raise PolicyProtocolError("invalid_image", "jpeg_image must be bytes")
+        if not 4 <= len(jpeg_image) <= MAX_IMAGE_BYTES:
+            raise PolicyProtocolError("invalid_image", "jpeg_image size is outside limits")
+        if not jpeg_image.startswith(b"\xff\xd8") or not jpeg_image.endswith(b"\xff\xd9"):
+            raise PolicyProtocolError("invalid_image", "jpeg_image is missing JPEG start/end markers")
+        body_array = _finite_float32_vector(
+            body_joint_positions,
+            name="body_joint_positions",
+            size=29,
+        )
+        dex_array = _finite_float32_vector(dex_state, name="dex_state", size=12)
+        neck_array = _finite_float32_vector(neck_state, name="neck_state", size=2)
+        source_pose_array = _finite_float32_vector(
+            source_reference_root_pose,
+            name="source_reference_root_pose",
+            size=7,
+        )
+        quaternion_norm = float(np.linalg.norm(source_pose_array[3:7]))
+        if abs(quaternion_norm - 1.0) > 1e-3:
+            raise PolicyProtocolError(
+                "invalid_reference_pose",
+                "source_reference_root_pose quaternion norm must be near 1, "
+                f"got {quaternion_norm:.6g}",
+            )
+        request_data = {
+            "session_id": session_id,
+            "sequence_id": _int64(sequence_id, name="sequence_id"),
+            "onboard_monotonic_timestamp_ns": _int64(
+                onboard_monotonic_timestamp_ns,
+                name="onboard_monotonic_timestamp_ns",
+            ),
+            "task": task,
+            "image_encoding": "jpeg",
+            "image": jpeg_image,
+            "body_joint_positions": encode_float32_array(body_array),
+            "dex_state": encode_float32_array(dex_array),
+            "neck_state": encode_float32_array(neck_array),
+            "source_reference_root_pose": encode_float32_array(source_pose_array),
+        }
+        data = self._request("get_action", request_data)
+        expected_fields = {
+            "session_id",
+            "source_sequence_id",
+            "source_onboard_monotonic_timestamp_ns",
+            "action_fps",
+            "actions",
+            "policy_id",
+            "server_inference_ms",
+        }
+        if set(data) != expected_fields:
+            raise PolicyProtocolError("invalid_response", "get_action data contains unexpected fields")
+        if data["session_id"] != session_id:
+            raise PolicyProtocolError("session_mismatch", "get_action response session_id does not match request")
+        source_sequence = _int64(data["source_sequence_id"], name="source_sequence_id")
+        source_timestamp = _int64(
+            data["source_onboard_monotonic_timestamp_ns"],
+            name="source_onboard_monotonic_timestamp_ns",
+        )
+        if source_sequence != sequence_id or source_timestamp != onboard_monotonic_timestamp_ns:
+            raise PolicyProtocolError("stale_response", "get_action response does not echo the source observation")
+        action_fps = _int64(data["action_fps"], name="action_fps")
+        if action_fps != 30:
+            raise PolicyProtocolError("invalid_response", f"action_fps must be 30, got {action_fps}")
+        actions = decode_float32_array(data["actions"], name="actions", expected_shape=(None, 50))
+        if not 1 <= len(actions) <= MAX_ACTION_HORIZON:
+            raise PolicyProtocolError("invalid_response", f"actions horizon is invalid: {len(actions)}")
+        if not isinstance(data["policy_id"], str) or not data["policy_id"]:
+            raise PolicyProtocolError("invalid_response", "policy_id must be non-empty")
+        inference_ms = float(data["server_inference_ms"])
+        if not np.isfinite(inference_ms) or inference_ms < 0.0:
+            raise PolicyProtocolError("invalid_response", "server_inference_ms must be finite and >= 0")
+        return PolicyActionChunk(
+            session_id=session_id,
+            source_sequence_id=source_sequence,
+            source_onboard_monotonic_timestamp_ns=source_timestamp,
+            action_fps=action_fps,
+            actions=actions,
+            policy_id=data["policy_id"],
+            server_inference_ms=inference_ms,
+        )
+
+    def _request(self, endpoint: str, data: dict[str, Any]) -> dict[str, Any]:
+        if endpoint not in ENDPOINTS:
+            raise PolicyProtocolError("unknown_endpoint", f"Unsupported endpoint {endpoint!r}")
+        request = {"endpoint": endpoint, "data": data}
+        payload = pack_message(request, max_bytes=MAX_REQUEST_BYTES)
+        socket = self._socket
+        if socket is None:
+            raise PolicyTransportError("High-level policy client is closed")
+        try:
+            socket.send(payload)
+            reply_payload = socket.recv()
+        except zmq.Again as exc:
+            self._recreate_socket()
+            raise PolicyTransportError(
+                f"High-level policy {endpoint} timed out after {self.timeout_s:.3f}s"
+            ) from exc
+        except zmq.ZMQError as exc:
+            self._recreate_socket()
+            raise PolicyTransportError(
+                f"High-level policy {endpoint} transport failed: {exc}"
+            ) from exc
+        reply = unpack_message(reply_payload, max_bytes=MAX_RESPONSE_BYTES)
+        return self._parse_reply(reply, endpoint=endpoint)
+
+    def _parse_reply(self, reply: Mapping[str, Any], *, endpoint: str) -> dict[str, Any]:
+        base = {"endpoint", "ok"}
+        if not base.issubset(reply):
+            raise PolicyProtocolError("invalid_response", "Response is missing envelope fields")
+        if reply["endpoint"] != endpoint:
+            raise PolicyProtocolError("invalid_response", f"Response endpoint {reply['endpoint']!r} != {endpoint!r}")
+        if not isinstance(reply["ok"], bool):
+            raise PolicyProtocolError("invalid_response", "Response ok must be boolean")
+        if reply["ok"]:
+            if set(reply) != base | {"data"} or not isinstance(reply["data"], dict):
+                raise PolicyProtocolError("invalid_response", "Successful response must contain exactly data")
+            return dict(reply["data"])
+        if set(reply) != base | {"error"} or not isinstance(reply["error"], Mapping):
+            raise PolicyProtocolError("invalid_response", "Failed response must contain exactly error")
+        error = reply["error"]
+        if set(error) != {"code", "message"} or not all(isinstance(error[key], str) for key in error):
+            raise PolicyProtocolError("invalid_response", "Response error must contain string code/message")
+        raise PolicyProtocolError(error["code"], error["message"])
+
+    def _open_socket(self) -> None:
+        socket = self._context.socket(zmq.REQ)
+        timeout_ms = max(1, int(round(self.timeout_s * 1000.0)))
+        socket.setsockopt(zmq.LINGER, 0)
+        socket.setsockopt(zmq.RCVHWM, 1)
+        socket.setsockopt(zmq.SNDHWM, 1)
+        socket.setsockopt(zmq.RCVTIMEO, timeout_ms)
+        socket.setsockopt(zmq.SNDTIMEO, timeout_ms)
+        socket.setsockopt(zmq.MAXMSGSIZE, MAX_RESPONSE_BYTES)
+        socket.connect(self.endpoint)
+        self._socket = socket
+
+    def _close_socket(self) -> None:
+        socket = self._socket
+        self._socket = None
+        if socket is not None:
+            socket.close(linger=0)
+
+    def _recreate_socket(self) -> None:
+        self._close_socket()
+        self._open_socket()
+
+
+def _int64(value: object, *, name: str) -> int:
+    if not isinstance(value, int) or isinstance(value, bool) or not 0 <= value <= 2**63 - 1:
+        raise PolicyProtocolError("invalid_value", f"{name} must be an int64 in [0, 2^63-1]")
+    return int(value)
+
+
+def _finite_float32_vector(value: object, *, name: str, size: int) -> np.ndarray:
+    array = np.asarray(value, dtype=np.float32)
+    if array.shape != (size,) or not np.all(np.isfinite(array)):
+        raise PolicyProtocolError(
+            "invalid_observation",
+            f"{name} must be finite float32[{size}], got {array.shape}",
+        )
+    return np.ascontiguousarray(array, dtype=np.float32)
diff --git a/teleopit/high_level_policy/config.py b/teleopit/high_level_policy/config.py
new file mode 100644
index 00000000..79c89fcd
--- /dev/null
+++ b/teleopit/high_level_policy/config.py
@@ -0,0 +1,232 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+import math
+from typing import Any
+
+import numpy as np
+
+from teleopit.high_level_policy.protocol import MAX_ACTION_HORIZON
+from teleopit.runtime.common import cfg_get
+
+
+@dataclass(frozen=True)
+class HighLevelPolicyConfig:
+    endpoint: str
+    task: str
+    timeout_s: float
+    reconnect_backoff_s: float
+    replan_steps: int
+    jpeg_quality: int
+    max_observation_age_s: float
+    max_result_age_s: float
+    entry_timeout_s: float
+    hold_s: float
+
+
+@dataclass(frozen=True)
+class HighLevelPolicyCameraConfig:
+    source: str
+    width: int
+    height: int
+    fps: int
+    device: str | None
+
+
+@dataclass(frozen=True)
+class HighLevelPolicySafetyConfig:
+    root_height_min_m: float
+    root_height_max_m: float
+    max_root_xy_speed_m_s: float
+    max_root_displacement_m: float
+    max_yaw_rate_rad_s: float
+    max_joint_rate_rad_s: float
+    max_joint_projection_rad: float
+    joint_pos_lower: tuple[float, ...]
+    joint_pos_upper: tuple[float, ...]
+    neck_yaw_min_deg: float
+    neck_yaw_max_deg: float
+    neck_pitch_min_deg: float
+    neck_pitch_max_deg: float
+
+
+def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig:
+    policy_cfg = cfg_get(cfg, "high_level_policy", {}) or {}
+    endpoint = str(cfg_get(policy_cfg, "endpoint", "tcp://127.0.0.1:5555")).strip()
+    if not endpoint.startswith("tcp://"):
+        raise ValueError("high_level_policy.endpoint must be a tcp:// endpoint")
+    task = str(cfg_get(policy_cfg, "task", "")).strip()
+    if not task:
+        raise ValueError("high_level_policy.task must be a non-empty prompt")
+    if len(task.encode("utf-8")) > 1024:
+        raise ValueError("high_level_policy.task exceeds the protocol 1024-byte UTF-8 limit")
+    timeout_s = _positive_float(cfg_get(policy_cfg, "timeout_s", 1.0), "timeout_s")
+    reconnect_backoff_s = _positive_float(
+        cfg_get(policy_cfg, "reconnect_backoff_s", 1.0), "reconnect_backoff_s"
+    )
+    replan_steps = int(cfg_get(policy_cfg, "replan_steps", 3))
+    if not 1 <= replan_steps <= MAX_ACTION_HORIZON:
+        raise ValueError(
+            "high_level_policy.replan_steps must be in "
+            f"[1, {MAX_ACTION_HORIZON}]"
+        )
+    jpeg_quality = int(cfg_get(policy_cfg, "jpeg_quality", 90))
+    if not 1 <= jpeg_quality <= 100:
+        raise ValueError("high_level_policy.jpeg_quality must be in [1, 100]")
+    max_observation_age_s = _positive_float(
+        cfg_get(policy_cfg, "max_observation_age_s", 0.15), "max_observation_age_s"
+    )
+    max_result_age_s = _positive_float(
+        cfg_get(policy_cfg, "max_result_age_s", 0.1), "max_result_age_s"
+    )
+    entry_timeout_s = _positive_float(
+        cfg_get(policy_cfg, "entry_timeout_s", 5.0), "entry_timeout_s"
+    )
+    hold_s = float(cfg_get(policy_cfg, "hold_s", 3.0))
+    if not math.isfinite(hold_s) or hold_s < 0.0:
+        raise ValueError("high_level_policy.hold_s must be finite and >= 0")
+    return HighLevelPolicyConfig(
+        endpoint=endpoint,
+        task=task,
+        timeout_s=timeout_s,
+        reconnect_backoff_s=reconnect_backoff_s,
+        replan_steps=replan_steps,
+        jpeg_quality=jpeg_quality,
+        max_observation_age_s=max_observation_age_s,
+        max_result_age_s=max_result_age_s,
+        entry_timeout_s=entry_timeout_s,
+        hold_s=hold_s,
+    )
+
+
+def parse_high_level_policy_camera_config(cfg: Any) -> HighLevelPolicyCameraConfig:
+    camera_cfg = cfg_get(cfg, "camera", {}) or {}
+    source = str(cfg_get(camera_cfg, "source", "realsense")).strip().lower()
+    if source not in ("realsense", "test-pattern"):
+        raise ValueError("camera.source must be realsense or test-pattern")
+    width = int(cfg_get(camera_cfg, "width", 640))
+    height = int(cfg_get(camera_cfg, "height", 480))
+    fps = int(cfg_get(camera_cfg, "fps", 30))
+    if (width, height, fps) != (640, 480, 30):
+        raise ValueError(
+            "High-level policy camera must be exactly width=640, height=480, fps=30"
+        )
+    device = cfg_get(camera_cfg, "device", None)
+    return HighLevelPolicyCameraConfig(
+        source=source,
+        width=width,
+        height=height,
+        fps=fps,
+        device=None if device in (None, "", "null") else str(device),
+    )
+
+
+def parse_high_level_policy_safety_config(cfg: Any) -> HighLevelPolicySafetyConfig:
+    policy_cfg = cfg_get(cfg, "high_level_policy", {}) or {}
+    safety_cfg = cfg_get(policy_cfg, "safety", {}) or {}
+    real_cfg = cfg_get(cfg, "real_robot", {}) or {}
+
+    root_height_min_m = _finite_float(
+        cfg_get(safety_cfg, "root_height_min_m", 0.55),
+        "safety.root_height_min_m",
+    )
+    root_height_max_m = _finite_float(
+        cfg_get(safety_cfg, "root_height_max_m", 1.05),
+        "safety.root_height_max_m",
+    )
+    if root_height_min_m >= root_height_max_m:
+        raise ValueError(
+            "high_level_policy.safety.root_height_min_m must be less than root_height_max_m"
+        )
+
+    joint_pos_lower = _joint_limit_vector(
+        cfg_get(real_cfg, "joint_pos_lower", None),
+        "real_robot.joint_pos_lower",
+    )
+    joint_pos_upper = _joint_limit_vector(
+        cfg_get(real_cfg, "joint_pos_upper", None),
+        "real_robot.joint_pos_upper",
+    )
+    if np.any(np.asarray(joint_pos_lower) >= np.asarray(joint_pos_upper)):
+        raise ValueError("real_robot joint position lower limits must be below upper limits")
+
+    neck_yaw_min_deg = _finite_float(
+        cfg_get(safety_cfg, "neck_yaw_min_deg", -45.0),
+        "safety.neck_yaw_min_deg",
+    )
+    neck_yaw_max_deg = _finite_float(
+        cfg_get(safety_cfg, "neck_yaw_max_deg", 45.0),
+        "safety.neck_yaw_max_deg",
+    )
+    neck_pitch_min_deg = _finite_float(
+        cfg_get(safety_cfg, "neck_pitch_min_deg", -40.0),
+        "safety.neck_pitch_min_deg",
+    )
+    neck_pitch_max_deg = _finite_float(
+        cfg_get(safety_cfg, "neck_pitch_max_deg", 40.0),
+        "safety.neck_pitch_max_deg",
+    )
+    if neck_yaw_min_deg >= neck_yaw_max_deg:
+        raise ValueError(
+            "high_level_policy.safety.neck_yaw_min_deg must be less than neck_yaw_max_deg"
+        )
+    if neck_pitch_min_deg >= neck_pitch_max_deg:
+        raise ValueError(
+            "high_level_policy.safety.neck_pitch_min_deg must be less than neck_pitch_max_deg"
+        )
+
+    return HighLevelPolicySafetyConfig(
+        root_height_min_m=root_height_min_m,
+        root_height_max_m=root_height_max_m,
+        max_root_xy_speed_m_s=_positive_float(
+            cfg_get(safety_cfg, "max_root_xy_speed_m_s", 2.5),
+            "safety.max_root_xy_speed_m_s",
+        ),
+        max_root_displacement_m=_positive_float(
+            cfg_get(safety_cfg, "max_root_displacement_m", 0.1),
+            "safety.max_root_displacement_m",
+        ),
+        max_yaw_rate_rad_s=_positive_float(
+            cfg_get(safety_cfg, "max_yaw_rate_rad_s", 2.5),
+            "safety.max_yaw_rate_rad_s",
+        ),
+        max_joint_rate_rad_s=_positive_float(
+            cfg_get(safety_cfg, "max_joint_rate_rad_s", 10.0),
+            "safety.max_joint_rate_rad_s",
+        ),
+        max_joint_projection_rad=_positive_float(
+            cfg_get(safety_cfg, "max_joint_projection_rad", 0.1),
+            "safety.max_joint_projection_rad",
+        ),
+        joint_pos_lower=joint_pos_lower,
+        joint_pos_upper=joint_pos_upper,
+        neck_yaw_min_deg=neck_yaw_min_deg,
+        neck_yaw_max_deg=neck_yaw_max_deg,
+        neck_pitch_min_deg=neck_pitch_min_deg,
+        neck_pitch_max_deg=neck_pitch_max_deg,
+    )
+
+
+def _positive_float(value: object, name: str) -> float:
+    parsed = float(value)
+    if not math.isfinite(parsed) or parsed <= 0.0:
+        raise ValueError(f"high_level_policy.{name} must be finite and > 0")
+    return parsed
+
+
+def _finite_float(value: object, name: str) -> float:
+    parsed = float(value)
+    if not math.isfinite(parsed):
+        raise ValueError(f"high_level_policy.{name} must be finite")
+    return parsed
+
+
+def _joint_limit_vector(value: object, name: str) -> tuple[float, ...]:
+    if value is None:
+        raise ValueError(
+            f"{name} is required for high-level-policy action validation"
+        )
+    array = np.asarray(value, dtype=np.float64).reshape(-1)
+    if array.shape != (29,) or not np.all(np.isfinite(array)):
+        raise ValueError(f"{name} must contain 29 finite values")
+    return tuple(float(item) for item in array)
diff --git a/teleopit/high_level_policy/hand_calibration.json b/teleopit/high_level_policy/hand_calibration.json
new file mode 100644
index 00000000..2779413f
--- /dev/null
+++ b/teleopit/high_level_policy/hand_calibration.json
@@ -0,0 +1,5 @@
+{
+  "open_raw": [250, 250, 250, 250, 250, 250],
+  "close_raw": [86, 73, 118, 111, 110, 111],
+  "range_tolerance": 0.0001
+}
diff --git a/teleopit/high_level_policy/hand_calibration.py b/teleopit/high_level_policy/hand_calibration.py
new file mode 100644
index 00000000..10cc7ecd
--- /dev/null
+++ b/teleopit/high_level_policy/hand_calibration.py
@@ -0,0 +1,39 @@
+"""Load the LinkerHand calibration shared with the host policy service."""
+
+from __future__ import annotations
+
+import json
+import math
+from dataclasses import dataclass
+from importlib.resources import files
+
+
+@dataclass(frozen=True)
+class HandCalibration:
+    open_raw: tuple[float, ...]
+    close_raw: tuple[float, ...]
+    range_tolerance: float
+
+    @classmethod
+    def load(cls) -> "HandCalibration":
+        path = files("teleopit.high_level_policy").joinpath("hand_calibration.json")
+        try:
+            document = json.loads(path.read_bytes())
+            opened = tuple(float(value) for value in document["open_raw"])
+            closed = tuple(float(value) for value in document["close_raw"])
+            range_tolerance = float(document["range_tolerance"])
+        except (OSError, json.JSONDecodeError, KeyError, TypeError, ValueError) as exc:
+            raise ValueError(f"Invalid hand_calibration.json: {exc}") from exc
+
+        if len(opened) != 6 or len(closed) != 6:
+            raise ValueError("hand_calibration.json must define six open_raw and close_raw values")
+        if not all(math.isfinite(value) for value in (*opened, *closed, range_tolerance)):
+            raise ValueError("hand_calibration.json values must be finite")
+        if any(opened_value == closed_value for opened_value, closed_value in zip(opened, closed, strict=True)):
+            raise ValueError("Each hand_calibration.json open/close pair must differ")
+
+        return cls(
+            open_raw=opened,
+            close_raw=closed,
+            range_tolerance=range_tolerance,
+        )
diff --git a/teleopit/high_level_policy/protocol.py b/teleopit/high_level_policy/protocol.py
new file mode 100644
index 00000000..32801787
--- /dev/null
+++ b/teleopit/high_level_policy/protocol.py
@@ -0,0 +1,105 @@
+"""Serialization helpers and fixed limits for high-level-policy messages."""
+
+from __future__ import annotations
+
+from typing import Any, Mapping
+
+import msgpack
+import numpy as np
+
+
+ENDPOINTS = frozenset({"ping", "describe", "reset", "get_action"})
+MAX_REQUEST_BYTES = 2_097_152
+MAX_RESPONSE_BYTES = 262_144
+MAX_IMAGE_BYTES = 1_572_864
+MAX_TASK_UTF8_BYTES = 1_024
+MAX_SESSION_ID_UTF8_BYTES = 128
+MAX_ACTION_HORIZON = 50
+
+
+class PolicyProtocolError(ValueError):
+    def __init__(self, code: str, message: str) -> None:
+        super().__init__(message)
+        self.code = str(code)
+        self.message = str(message)
+
+
+def encode_float32_array(values: object) -> dict[str, object]:
+    array = np.asarray(values)
+    if not np.issubdtype(array.dtype, np.number):
+        raise PolicyProtocolError("invalid_array", f"Array dtype must be numeric, got {array.dtype}")
+    encoded = np.ascontiguousarray(array, dtype="<f4")
+    if not np.all(np.isfinite(encoded)):
+        raise PolicyProtocolError("nonfinite_array", "Array contains non-finite values")
+    return {"dtype": "<f4", "shape": list(encoded.shape), "data": encoded.tobytes(order="C")}
+
+
+def decode_float32_array(
+    value: object,
+    *,
+    name: str,
+    expected_shape: tuple[int | None, ...],
+) -> np.ndarray:
+    if not isinstance(value, Mapping) or set(value) != {"dtype", "shape", "data"}:
+        raise PolicyProtocolError("invalid_array", f"{name} must contain exactly dtype, shape, and data")
+    if value["dtype"] != "<f4":
+        raise PolicyProtocolError("invalid_array", f"{name}.dtype must be '<f4'")
+    shape = value["shape"]
+    if not isinstance(shape, list) or len(shape) != len(expected_shape):
+        raise PolicyProtocolError("invalid_array", f"{name}.shape must have rank {len(expected_shape)}")
+    parsed: list[int] = []
+    for index, (actual, expected) in enumerate(zip(shape, expected_shape, strict=True)):
+        if not isinstance(actual, int) or isinstance(actual, bool) or actual < 0:
+            raise PolicyProtocolError("invalid_array", f"{name}.shape[{index}] must be a non-negative int")
+        if expected is not None and actual != expected:
+            raise PolicyProtocolError(
+                "invalid_array", f"{name}.shape[{index}] must be {expected}, got {actual}"
+            )
+        parsed.append(actual)
+    raw = value["data"]
+    if not isinstance(raw, bytes):
+        raise PolicyProtocolError("invalid_array", f"{name}.data must be msgpack binary")
+    expected_bytes = int(np.prod(parsed, dtype=np.int64)) * np.dtype("<f4").itemsize
+    if len(raw) != expected_bytes:
+        raise PolicyProtocolError(
+            "invalid_array", f"{name}.data has {len(raw)} bytes, expected {expected_bytes}"
+        )
+    decoded = np.frombuffer(raw, dtype="<f4").reshape(tuple(parsed)).copy()
+    if not np.all(np.isfinite(decoded)):
+        raise PolicyProtocolError("nonfinite_array", f"{name} contains non-finite values")
+    return decoded
+
+
+def pack_message(message: Mapping[str, Any], *, max_bytes: int) -> bytes:
+    try:
+        payload = msgpack.packb(dict(message), use_bin_type=True, strict_types=True)
+    except (TypeError, ValueError) as exc:
+        raise PolicyProtocolError("serialization_error", f"Cannot serialize message: {exc}") from exc
+    if len(payload) > max_bytes:
+        raise PolicyProtocolError(
+            "message_too_large", f"Serialized message has {len(payload)} bytes; limit is {max_bytes}"
+        )
+    return payload
+
+
+def unpack_message(payload: object, *, max_bytes: int) -> dict[str, Any]:
+    if not isinstance(payload, bytes):
+        raise PolicyProtocolError("invalid_message", "Protocol payload must be bytes")
+    if len(payload) > max_bytes:
+        raise PolicyProtocolError("message_too_large", f"Message has {len(payload)} bytes; limit is {max_bytes}")
+    try:
+        message = msgpack.unpackb(
+            payload,
+            raw=False,
+            strict_map_key=True,
+            max_bin_len=max_bytes,
+            max_str_len=max_bytes,
+            max_array_len=128,
+            max_map_len=64,
+            max_ext_len=0,
+        )
+    except (msgpack.ExtraData, msgpack.FormatError, msgpack.StackError, ValueError) as exc:
+        raise PolicyProtocolError("invalid_msgpack", f"Cannot decode message: {exc}") from exc
+    if not isinstance(message, dict):
+        raise PolicyProtocolError("invalid_message", "Protocol message must be a map")
+    return message
diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py
new file mode 100644
index 00000000..205c0115
--- /dev/null
+++ b/teleopit/high_level_policy/scheduler.py
@@ -0,0 +1,527 @@
+"""Session-local frame conversion and latency-aware 30 Hz action scheduling."""
+
+from __future__ import annotations
+
+from bisect import bisect_right
+from collections import deque
+from dataclasses import dataclass
+import math
+
+import numpy as np
+
+from teleopit.high_level_policy.client import PolicyActionChunk
+from teleopit.high_level_policy.config import HighLevelPolicySafetyConfig
+from teleopit.high_level_policy.hand_calibration import HandCalibration
+from teleopit.high_level_policy.protocol import MAX_ACTION_HORIZON
+from teleopit.math_utils import quat_inv_np, quat_mul_np
+from teleopit.sim.reference_motion import interpolate_retarget_qpos
+
+
+ACTION_DIM = 50
+BODY_ACTION_DIM = 36
+ROOT_QUATERNION = slice(3, 7)
+REFERENCE_HISTORY_SIZE = 256
+MAX_REFERENCE_INTERPOLATION_GAP_PERIODS = 2.5
+
+
+def _normalized_quaternion(value: object, *, name: str) -> np.ndarray:
+    quaternion = np.asarray(value, dtype=np.float32).reshape(-1)
+    if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
+        raise ValueError(f"{name} must be a finite wxyz quaternion")
+    norm = float(np.linalg.norm(quaternion))
+    if norm < 1e-8:
+        raise ValueError(f"{name} has a near-zero norm")
+    if abs(norm - 1.0) > 1e-3:
+        raise ValueError(f"{name} norm must be near 1, got {norm:.6g}")
+    return quaternion / np.float32(norm)
+
+
+def _yaw_from_quaternion(value: object) -> float:
+    w, x, y, z = _normalized_quaternion(value, name="anchor quaternion")
+    return float(math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)))
+
+
+def _yaw_quaternion(yaw_rad: float) -> np.ndarray:
+    half = 0.5 * float(yaw_rad)
+    return np.array([math.cos(half), 0.0, 0.0, math.sin(half)], dtype=np.float32)
+
+
+@dataclass(frozen=True)
+class PolicyFrameTransform:
+    origin_xy: tuple[float, float]
+    yaw_rad: float
+
+    @classmethod
+    def from_robot_pose(cls, root_xy: object, quaternion_wxyz: object) -> "PolicyFrameTransform":
+        xy = np.asarray(root_xy, dtype=np.float64).reshape(-1)
+        if xy.shape[0] < 2 or not np.all(np.isfinite(xy[:2])):
+            raise ValueError("Policy session root_xy must contain two finite values")
+        return cls(
+            origin_xy=(float(xy[0]), float(xy[1])),
+            yaw_rad=_yaw_from_quaternion(quaternion_wxyz),
+        )
+
+    def localize_body_action(self, action: object) -> np.ndarray:
+        body = np.asarray(action, dtype=np.float32).reshape(-1).copy()
+        if body.shape != (BODY_ACTION_DIM,) or not np.all(np.isfinite(body)):
+            raise ValueError(f"High-level body action must be finite float32[{BODY_ACTION_DIM}]")
+        world_delta = body[:2].astype(np.float64) - np.asarray(self.origin_xy, dtype=np.float64)
+        cosine = math.cos(self.yaw_rad)
+        sine = math.sin(self.yaw_rad)
+        body[0] = cosine * world_delta[0] + sine * world_delta[1]
+        body[1] = -sine * world_delta[0] + cosine * world_delta[1]
+        world_quaternion = _normalized_quaternion(body[ROOT_QUATERNION], name="action root quaternion")
+        local_quaternion = quat_mul_np(quat_inv_np(_yaw_quaternion(self.yaw_rad)), world_quaternion)
+        body[ROOT_QUATERNION] = _normalized_quaternion(
+            local_quaternion, name="localized action root quaternion"
+        )
+        return body
+
+    def delocalize_body_action(self, action: object) -> np.ndarray:
+        body = np.asarray(action, dtype=np.float32).reshape(-1).copy()
+        if body.shape != (BODY_ACTION_DIM,) or not np.all(np.isfinite(body)):
+            raise ValueError(f"High-level body action must be finite float32[{BODY_ACTION_DIM}]")
+        local_xy = body[:2].astype(np.float64)
+        cosine = math.cos(self.yaw_rad)
+        sine = math.sin(self.yaw_rad)
+        body[0] = cosine * local_xy[0] - sine * local_xy[1] + self.origin_xy[0]
+        body[1] = sine * local_xy[0] + cosine * local_xy[1] + self.origin_xy[1]
+        local_quaternion = _normalized_quaternion(body[ROOT_QUATERNION], name="action root quaternion")
+        world_quaternion = quat_mul_np(_yaw_quaternion(self.yaw_rad), local_quaternion)
+        body[ROOT_QUATERNION] = _normalized_quaternion(
+            world_quaternion, name="delocalized action root quaternion"
+        )
+        return body
+
+
+class HighLevelPolicyScheduler:
+    def __init__(
+        self,
+        *,
+        hold_s: float = 3.0,
+        safety: HighLevelPolicySafetyConfig | None = None,
+        output_hz: float = 50.0,
+    ) -> None:
+        if not np.isfinite(hold_s) or hold_s < 0.0:
+            raise ValueError("high_level_policy.hold_s must be finite and >= 0")
+        if not np.isfinite(output_hz) or output_hz <= 0.0:
+            raise ValueError("High-level policy scheduler output_hz must be finite and > 0")
+        self.hold_s = float(hold_s)
+        self.safety = safety
+        self.output_hz = float(output_hz)
+        self._session_id: str | None = None
+        self._chunk: PolicyActionChunk | None = None
+        self._last_source_sequence_id = -1
+        self._last_source_timestamp_ns = -1
+        self._paused_at_s: float | None = None
+        self._timestamp_shift_s = 0.0
+        self._last_output_action: np.ndarray | None = None
+        self._reference_history: deque[tuple[float, np.ndarray]] = deque(
+            maxlen=REFERENCE_HISTORY_SIZE
+        )
+
+    @property
+    def session_id(self) -> str | None:
+        return self._session_id
+
+    @property
+    def has_chunk(self) -> bool:
+        return self._chunk is not None
+
+    @property
+    def paused(self) -> bool:
+        return self._paused_at_s is not None
+
+    def reset(
+        self,
+        session_id: str,
+        *,
+        initial_action: object | None = None,
+        initial_reference: object | None = None,
+        initial_timestamp_s: float | None = None,
+    ) -> None:
+        if not isinstance(session_id, str) or not session_id:
+            raise ValueError("High-level policy session_id must be non-empty")
+        if (initial_reference is None) != (initial_timestamp_s is None):
+            raise ValueError(
+                "High-level policy initial reference and initial_timestamp_s must be provided together"
+            )
+        self._session_id = session_id
+        self._chunk = None
+        self._last_source_sequence_id = -1
+        self._last_source_timestamp_ns = -1
+        self._paused_at_s = None
+        self._timestamp_shift_s = 0.0
+        initial_output = (
+            None
+            if initial_action is None
+            else self._validate_single_action(initial_action, name="initial_action")
+        )
+        self._last_output_action = (
+            None if initial_output is None else initial_output.copy()
+        )
+        self._reference_history.clear()
+        if initial_reference is not None:
+            assert initial_timestamp_s is not None
+            self._record_reference(initial_timestamp_s, initial_reference)
+
+    def clear(self) -> None:
+        self._session_id = None
+        self._chunk = None
+        self._last_source_sequence_id = -1
+        self._last_source_timestamp_ns = -1
+        self._paused_at_s = None
+        self._timestamp_shift_s = 0.0
+        self._last_output_action = None
+        self._reference_history.clear()
+
+    def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None:
+        self._accept(chunk, now_s=now_s)
+
+    def _accept(
+        self,
+        chunk: PolicyActionChunk,
+        *,
+        now_s: float,
+    ) -> None:
+        if not np.isfinite(now_s):
+            raise ValueError("High-level policy scheduler now_s must be finite")
+        if self._session_id is None or chunk.session_id != self._session_id:
+            raise ValueError(
+                f"High-level policy action session mismatch: active={self._session_id!r}, "
+                f"received={chunk.session_id!r}"
+            )
+        if (
+            not isinstance(chunk.source_sequence_id, int)
+            or isinstance(chunk.source_sequence_id, bool)
+            or chunk.source_sequence_id < 0
+        ):
+            raise ValueError("High-level policy source sequence must be a non-negative integer")
+        if chunk.source_sequence_id <= self._last_source_sequence_id:
+            raise ValueError(
+                "High-level policy source sequence must increase: "
+                f"last={self._last_source_sequence_id}, received={chunk.source_sequence_id}"
+            )
+        if not isinstance(chunk.action_fps, int) or isinstance(chunk.action_fps, bool) or chunk.action_fps != 30:
+            raise ValueError(f"High-level policy action_fps must be 30, got {chunk.action_fps}")
+        if (
+            not isinstance(chunk.source_onboard_monotonic_timestamp_ns, int)
+            or isinstance(chunk.source_onboard_monotonic_timestamp_ns, bool)
+            or not 0 <= chunk.source_onboard_monotonic_timestamp_ns <= 2**63 - 1
+        ):
+            raise ValueError("High-level policy source timestamp must be a non-negative int64")
+        if chunk.source_onboard_monotonic_timestamp_ns <= self._last_source_timestamp_ns:
+            raise ValueError(
+                "High-level policy source timestamp must increase: "
+                f"last={self._last_source_timestamp_ns}, "
+                f"received={chunk.source_onboard_monotonic_timestamp_ns}"
+            )
+        if not isinstance(chunk.policy_id, str) or not chunk.policy_id:
+            raise ValueError("High-level policy policy_id must be non-empty")
+        if not np.isfinite(chunk.server_inference_ms) or chunk.server_inference_ms < 0.0:
+            raise ValueError("High-level policy server_inference_ms must be finite and >= 0")
+        source_s = chunk.source_onboard_monotonic_timestamp_ns * 1e-9
+        if source_s > float(now_s) + 0.001:
+            raise ValueError(
+                "High-level policy source timestamp is in the future: "
+                f"source={source_s:.9f}s now={float(now_s):.9f}s"
+            )
+        actions = self._validate_actions(chunk.actions)
+        valid_until_s = source_s + len(actions) / float(chunk.action_fps) + self.hold_s
+        if float(now_s) > valid_until_s:
+            raise ValueError(
+                "High-level policy action chunk is already expired: "
+                f"age={float(now_s) - source_s:.3f}s horizon={len(actions) / chunk.action_fps:.3f}s"
+            )
+        self._chunk = PolicyActionChunk(
+            session_id=chunk.session_id,
+            source_sequence_id=chunk.source_sequence_id,
+            source_onboard_monotonic_timestamp_ns=chunk.source_onboard_monotonic_timestamp_ns,
+            action_fps=chunk.action_fps,
+            actions=actions,
+            policy_id=chunk.policy_id,
+            server_inference_ms=chunk.server_inference_ms,
+        )
+        self._last_source_sequence_id = chunk.source_sequence_id
+        self._last_source_timestamp_ns = chunk.source_onboard_monotonic_timestamp_ns
+        self._timestamp_shift_s = 0.0
+        if self._paused_at_s is not None:
+            self._paused_at_s = float(now_s)
+
+    def pause(self, now_s: float) -> None:
+        if self._paused_at_s is None:
+            timestamp_s = self._validated_timestamp(now_s, name="pause now_s")
+            self._paused_at_s = timestamp_s
+            self._record_last_output_reference(timestamp_s)
+
+    def resume(self, now_s: float) -> None:
+        if self._paused_at_s is None:
+            return
+        timestamp_s = self._validated_timestamp(now_s, name="resume now_s")
+        self._timestamp_shift_s += max(0.0, timestamp_s - self._paused_at_s)
+        self._paused_at_s = None
+        self._record_last_output_reference(timestamp_s)
+
+    def sample(self, now_s: float) -> np.ndarray | None:
+        timestamp_s = self._validated_timestamp(now_s, name="sample now_s")
+        desired = self._sample_unlimited(timestamp_s)
+        if desired is None:
+            return None
+        previous = self._last_output_action
+        safety = self.safety
+        if previous is not None and safety is not None:
+            desired = self._rate_limit_output(previous, desired, safety=safety)
+        self._last_output_action = desired.copy()
+        self._record_reference(timestamp_s, desired)
+        return desired
+
+    def reference_root_pose_at(self, timestamp_s: float) -> np.ndarray | None:
+        """Return the active session-local reference root at a camera timestamp."""
+
+        query_s = self._validated_timestamp(timestamp_s, name="reference timestamp_s")
+        history = tuple(self._reference_history)
+        if not history:
+            return None
+        if query_s <= history[0][0]:
+            return history[0][1].copy()
+        if query_s >= history[-1][0]:
+            return history[-1][1].copy()
+        timestamps = [sample[0] for sample in history]
+        right_index = bisect_right(timestamps, query_s)
+        left_timestamp, left_pose = history[right_index - 1]
+        right_timestamp, right_pose = history[right_index]
+        if (
+            right_timestamp - left_timestamp
+            > MAX_REFERENCE_INTERPOLATION_GAP_PERIODS / self.output_hz
+        ):
+            return left_pose.copy()
+        alpha = (query_s - left_timestamp) / (right_timestamp - left_timestamp)
+        return np.asarray(
+            interpolate_retarget_qpos(left_pose, right_pose, alpha),
+            dtype=np.float32,
+        )
+
+    def _record_last_output_reference(self, timestamp_s: float) -> None:
+        if self._last_output_action is not None:
+            self._record_reference(timestamp_s, self._last_output_action)
+
+    def _record_reference(self, timestamp_s: float, action: object) -> None:
+        timestamp = self._validated_timestamp(
+            timestamp_s,
+            name="reference history timestamp_s",
+        )
+        validated = self._validate_single_action(action, name="reference history action")
+        root_pose = validated[:7].copy()
+        if self._reference_history:
+            latest_timestamp = self._reference_history[-1][0]
+            if timestamp < latest_timestamp:
+                raise ValueError(
+                    "High-level policy reference history timestamp must not decrease: "
+                    f"last={latest_timestamp:.9f}s received={timestamp:.9f}s"
+                )
+            if timestamp == latest_timestamp:
+                self._reference_history[-1] = (timestamp, root_pose)
+                return
+        self._reference_history.append((timestamp, root_pose))
+
+    @staticmethod
+    def _validated_timestamp(value: object, *, name: str) -> float:
+        timestamp = float(value)
+        if not np.isfinite(timestamp) or timestamp < 0.0:
+            raise ValueError(f"High-level policy {name} must be finite and >= 0")
+        return timestamp
+
+    def _sample_unlimited(self, now_s: float) -> np.ndarray | None:
+        chunk = self._chunk
+        if chunk is None:
+            return None
+        effective_now_s = self._paused_at_s if self._paused_at_s is not None else float(now_s)
+        source_s = chunk.source_onboard_monotonic_timestamp_ns * 1e-9 + self._timestamp_shift_s
+        frame_f = (effective_now_s - source_s) * float(chunk.action_fps)
+        if frame_f <= 0.0:
+            return chunk.actions[0].copy()
+        last_index = len(chunk.actions) - 1
+        if frame_f >= float(last_index):
+            valid_until_s = source_s + len(chunk.actions) / float(chunk.action_fps) + self.hold_s
+            if effective_now_s > valid_until_s:
+                return None
+            return chunk.actions[last_index].copy()
+        index0 = int(math.floor(frame_f))
+        index1 = min(index0 + 1, last_index)
+        alpha = float(frame_f - index0)
+        interpolated = interpolate_retarget_qpos(
+            np.asarray(chunk.actions[index0], dtype=np.float64),
+            np.asarray(chunk.actions[index1], dtype=np.float64),
+            alpha,
+        )
+        return np.asarray(interpolated, dtype=np.float32)
+
+    def _rate_limit_output(
+        self,
+        previous: np.ndarray,
+        desired: np.ndarray,
+        *,
+        safety: HighLevelPolicySafetyConfig,
+    ) -> np.ndarray:
+        output = np.asarray(desired, dtype=np.float32).copy()
+        previous = self._validate_single_action(previous, name="previous output action")
+
+        root_delta = output[0:3].astype(np.float64) - previous[0:3].astype(np.float64)
+        max_root_delta = safety.max_root_displacement_m * 30.0 / self.output_hz
+        root_distance = float(np.linalg.norm(root_delta))
+        if root_distance > max_root_delta:
+            root_delta *= max_root_delta / root_distance
+        max_xy_delta = safety.max_root_xy_speed_m_s / self.output_hz
+        xy_distance = float(np.linalg.norm(root_delta[:2]))
+        if xy_distance > max_xy_delta:
+            root_delta[:2] *= max_xy_delta / xy_distance
+        output[0:3] = previous[0:3] + root_delta.astype(np.float32)
+
+        previous_yaw = _yaw_from_quaternion(previous[ROOT_QUATERNION])
+        desired_quaternion = _normalized_quaternion(
+            output[ROOT_QUATERNION], name="desired output root quaternion"
+        )
+        desired_yaw = _yaw_from_quaternion(desired_quaternion)
+        yaw_delta = math.atan2(
+            math.sin(desired_yaw - previous_yaw),
+            math.cos(desired_yaw - previous_yaw),
+        )
+        max_yaw_delta = safety.max_yaw_rate_rad_s / self.output_hz
+        limited_yaw = previous_yaw + float(np.clip(yaw_delta, -max_yaw_delta, max_yaw_delta))
+        desired_tilt = quat_mul_np(
+            quat_inv_np(_yaw_quaternion(desired_yaw)),
+            desired_quaternion,
+        )
+        limited_quaternion = _normalized_quaternion(
+            quat_mul_np(_yaw_quaternion(limited_yaw), desired_tilt),
+            name="rate-limited output root quaternion",
+        )
+        if float(np.dot(previous[ROOT_QUATERNION], limited_quaternion)) < 0.0:
+            limited_quaternion = -limited_quaternion
+        output[ROOT_QUATERNION] = limited_quaternion
+
+        max_joint_delta = safety.max_joint_rate_rad_s / self.output_hz
+        output[7:36] = previous[7:36] + np.clip(
+            output[7:36] - previous[7:36],
+            -max_joint_delta,
+            max_joint_delta,
+        )
+        return output
+
+    def _validate_actions(
+        self,
+        values: object,
+    ) -> np.ndarray:
+        actions = np.asarray(values)
+        if (
+            actions.ndim != 2
+            or actions.shape[1] != ACTION_DIM
+            or not 1 <= len(actions) <= MAX_ACTION_HORIZON
+        ):
+            raise ValueError(
+                f"High-level policy actions must have shape [T, {ACTION_DIM}] "
+                f"with T in [1, {MAX_ACTION_HORIZON}]"
+            )
+        if not np.issubdtype(actions.dtype, np.number) or not np.all(np.isfinite(actions)):
+            raise ValueError("High-level policy actions must be finite numeric values")
+        validated = np.ascontiguousarray(actions, dtype=np.float32)
+        previous: np.ndarray | None = None
+        for index in range(len(validated)):
+            quaternion = _normalized_quaternion(
+                validated[index, ROOT_QUATERNION], name=f"action[{index}] root quaternion"
+            )
+            if previous is not None and float(np.dot(previous, quaternion)) < 0.0:
+                quaternion = -quaternion
+            validated[index, ROOT_QUATERNION] = quaternion
+            previous = quaternion
+        hand = validated[:, 36:48]
+        if float(np.min(hand)) < 0.0 or float(np.max(hand)) > 1.0:
+            raise ValueError("High-level policy LinkerHand closure must be within [0, 1]")
+        safety = self.safety
+        if safety is not None:
+            joints = validated[:, 7:36]
+            projected = np.clip(
+                joints,
+                np.asarray(safety.joint_pos_lower, dtype=np.float32),
+                np.asarray(safety.joint_pos_upper, dtype=np.float32),
+            )
+            correction = np.abs(projected - joints)
+            violations = np.argwhere(correction > safety.max_joint_projection_rad)
+            if len(violations):
+                frame, joint = (int(value) for value in violations[0])
+                raise ValueError(
+                    "High-level policy joint projection correction exceeds "
+                    f"{safety.max_joint_projection_rad:.6g} rad: "
+                    f"action[{frame}, {7 + joint}] correction="
+                    f"{float(correction[frame, joint]):.6g} rad"
+                )
+            validated[:, 7:36] = projected
+            validated[:, 48] = np.clip(
+                validated[:, 48],
+                safety.neck_yaw_min_deg,
+                safety.neck_yaw_max_deg,
+            )
+            validated[:, 49] = np.clip(
+                validated[:, 49],
+                safety.neck_pitch_min_deg,
+                safety.neck_pitch_max_deg,
+            )
+            self._validate_safety_limits(validated, safety=safety)
+        return validated
+
+    @staticmethod
+    def _validate_single_action(values: object, *, name: str) -> np.ndarray:
+        action = np.asarray(values)
+        if action.shape != (ACTION_DIM,) or not np.issubdtype(action.dtype, np.number):
+            raise ValueError(f"{name} must be a numeric float32[{ACTION_DIM}]")
+        action = np.ascontiguousarray(action, dtype=np.float32)
+        if not np.all(np.isfinite(action)):
+            raise ValueError(f"{name} must contain only finite values")
+        action[ROOT_QUATERNION] = _normalized_quaternion(
+            action[ROOT_QUATERNION], name=f"{name} root quaternion"
+        )
+        return action
+
+    @staticmethod
+    def _validate_safety_limits(
+        actions: np.ndarray,
+        *,
+        safety: HighLevelPolicySafetyConfig,
+    ) -> None:
+        root_height = actions[:, 2]
+        if (
+            float(np.min(root_height)) < safety.root_height_min_m
+            or float(np.max(root_height)) > safety.root_height_max_m
+        ):
+            raise ValueError(
+                "High-level policy root height is outside "
+                f"[{safety.root_height_min_m}, {safety.root_height_max_m}] m"
+            )
+
+        joints = actions[:, 7:36]
+        lower = np.asarray(safety.joint_pos_lower, dtype=np.float32)
+        upper = np.asarray(safety.joint_pos_upper, dtype=np.float32)
+        violations = np.argwhere((joints < lower[None, :]) | (joints > upper[None, :]))
+        if len(violations):
+            frame, joint = (int(value) for value in violations[0])
+            raise ValueError(
+                "High-level policy joint position exceeds real_robot limits: "
+                f"action[{frame}, {7 + joint}]={float(joints[frame, joint]):.6g}, "
+                f"range=[{float(lower[joint]):.6g}, {float(upper[joint]):.6g}]"
+            )
+
+
+def closure_to_o6_pose(
+    closure: object,
+    calibration: HandCalibration | None = None,
+) -> tuple[int, ...]:
+    calibration = calibration or HandCalibration.load()
+    values = np.asarray(closure, dtype=np.float32).reshape(-1)
+    if values.shape != (6,) or not np.all(np.isfinite(values)):
+        raise ValueError("LinkerHand O6 closure must contain six finite values")
+    if float(np.min(values)) < 0.0 or float(np.max(values)) > 1.0:
+        raise ValueError("LinkerHand O6 closure must be within [0, 1]")
+    opened = np.asarray(calibration.open_raw, dtype=np.float32)
+    closed = np.asarray(calibration.close_raw, dtype=np.float32)
+    raw = np.rint(opened - values * (opened - closed)).astype(np.int64)
+    return tuple(int(value) for value in raw)
diff --git a/teleopit/inputs/pico4_provider.py b/teleopit/inputs/pico4_provider.py
index 1780b03f..52b184f5 100644
--- a/teleopit/inputs/pico4_provider.py
+++ b/teleopit/inputs/pico4_provider.py
@@ -94,6 +94,16 @@ class PicoHandSnapshot:
     seq: int
 
 
+@dataclass(frozen=True)
+class PicoHeadPoseSnapshot:
+    """Synchronized HMD and torso orientations for active-neck control."""
+
+    hmd_rotation_wxyz: NDArray[np.float64] | None
+    spine3_rotation_wxyz: NDArray[np.float64] | None
+    timestamp_s: float
+    seq: int
+
+
 _PAUSE_BUTTON_MAP: dict[str, tuple[str, str]] = {
     "A": ("right", "primaryButton"),
     "B": ("right", "secondaryButton"),
@@ -173,6 +183,32 @@ def _coordinate_transform_input(body_pose_dict: dict[str, list]) -> dict[str, li
     return body_pose_dict
 
 
+def _transform_pico_native_rotation(rotation_xyzw: Any) -> NDArray[np.float64] | None:
+    """Convert one PICO-native xyzw orientation into Teleopit wxyz coordinates."""
+    try:
+        rotation = np.asarray(rotation_xyzw, dtype=np.float64).reshape(-1)
+    except (TypeError, ValueError):
+        return None
+    if rotation.shape != (4,) or not np.all(np.isfinite(rotation)):
+        return None
+    quat_wxyz = np.array(
+        [rotation[3], rotation[0], rotation[1], rotation[2]],
+        dtype=np.float64,
+    )
+    norm = float(np.linalg.norm(quat_wxyz))
+    if norm <= 1e-9:
+        return None
+    transformed = quat_mul_np(
+        _INPUT_TO_TELEOPIT_QUAT,
+        quat_wxyz / norm,
+        scalar_first=True,
+    )
+    transformed_norm = float(np.linalg.norm(transformed))
+    if transformed_norm <= 1e-9 or not np.all(np.isfinite(transformed)):
+        return None
+    return np.asarray(transformed / transformed_norm, dtype=np.float64)
+
+
 class Pico4InputProvider(RealtimeInputProvider):
     """Realtime input provider backed by the ``pico_bridge`` receiver."""
 
@@ -240,6 +276,7 @@ def __init__(
         self._last_source_seq: int | None = None
         self._controller_snapshot: PicoControllerSnapshot | None = None
         self._hand_snapshot: PicoHandSnapshot | None = None
+        self._head_pose_snapshot: PicoHeadPoseSnapshot | None = None
         self._ground_alignment_offset: float | None = None
         self._bridge = bridge_cls(
             host=bridge_host,
@@ -336,6 +373,11 @@ def get_hand_snapshot(self) -> PicoHandSnapshot | None:
         with self._lock:
             return self._hand_snapshot
 
+    def get_head_pose_snapshot(self) -> PicoHeadPoseSnapshot | None:
+        """Return the latest synchronized HMD/Spine3 orientation snapshot."""
+        with self._lock:
+            return self._head_pose_snapshot
+
     def push_video_frame(self, frame: NDArray[np.uint8]) -> int:
         """Push one RGB camera frame to pico-bridge 0.2.1 video output."""
         push_video_frame = getattr(self._bridge, "push_video_frame", None)
@@ -400,6 +442,7 @@ def _poll_loop(self) -> None:
 
     def _accept_pico_frame(self, frame: Any) -> bool:
         timestamp = float(getattr(frame, "receive_time_s", time.monotonic()))
+        self._accept_head_pose_snapshot(frame, timestamp=timestamp)
         self._accept_controller_snapshot(frame, timestamp=timestamp)
         self._accept_hand_snapshot(frame, timestamp=timestamp)
         self._poll_control_events(frame, timestamp=timestamp)
@@ -467,6 +510,35 @@ def _accept_hand_snapshot(self, frame: Any, *, timestamp: float) -> None:
         with self._lock:
             self._hand_snapshot = snapshot
 
+    def _accept_head_pose_snapshot(self, frame: Any, *, timestamp: float) -> None:
+        """Capture HMD and Spine3 rotations from the same pico_bridge frame."""
+        seq = int(getattr(frame, "seq", self._last_source_seq or -1))
+
+        head = getattr(frame, "head", None)
+        hmd_rotation = _transform_pico_native_rotation(
+            None if head is None else getattr(head, "rotation", None)
+        )
+
+        spine3_rotation: NDArray[np.float64] | None = None
+        body = getattr(frame, "body", None)
+        if body is not None and bool(getattr(body, "active", False)):
+            try:
+                body_joints = np.asarray(getattr(body, "joints"), dtype=np.float64)
+            except (AttributeError, TypeError, ValueError):
+                body_joints = np.empty((0, 0), dtype=np.float64)
+            if body_joints.shape == (len(BODY_JOINT_NAMES), 7):
+                spine3 = body_joints[BODY_JOINT_NAMES.index("Spine3")]
+                spine3_rotation = _transform_pico_native_rotation(spine3[[3, 4, 5, 6]])
+
+        snapshot = PicoHeadPoseSnapshot(
+            hmd_rotation_wxyz=hmd_rotation,
+            spine3_rotation_wxyz=spine3_rotation,
+            timestamp_s=float(timestamp),
+            seq=seq,
+        )
+        with self._lock:
+            self._head_pose_snapshot = snapshot
+
     def _poll_control_events(self, frame: Any, *, timestamp: float) -> bool:
         emitted = False
         emitted = self._poll_button_control_event(
diff --git a/teleopit/inputs/pico_video.py b/teleopit/inputs/pico_video.py
index fd1ed25e..ae24d3f9 100644
--- a/teleopit/inputs/pico_video.py
+++ b/teleopit/inputs/pico_video.py
@@ -23,7 +23,6 @@ class PicoVideoConfig:
     height: int = 720
     fps: int = 30
     device: str | None = None
-    fail_on_error: bool = False
 
 
 def parse_pico_video_config(input_cfg: Any) -> PicoVideoConfig:
@@ -48,7 +47,6 @@ def parse_pico_video_config(input_cfg: Any) -> PicoVideoConfig:
         height=height,
         fps=fps,
         device=None if device in (None, "", "null") else str(device),
-        fail_on_error=bool(cfg_get(video_cfg, "fail_on_error", False)),
     )
 
 
@@ -68,13 +66,11 @@ def __init__(
         *,
         provider: Any,
         config: PicoVideoConfig,
-        mode: str,
         robot: Any | None = None,
         frame_callback: Callable[[np.ndarray, float], None] | None = None,
     ) -> None:
         self._provider = provider
         self._config = config
-        self._mode = mode
         self._robot = robot
         self._frame_callback = frame_callback
         self._producer: _VideoProducer | None = None
@@ -140,9 +136,7 @@ def stop(self) -> None:
             self._producer = None
 
     def _handle_error(self, exc: Exception) -> None:
-        if self._config.fail_on_error:
-            raise RuntimeError("Pico video pipeline failed") from exc
-        logger.warning("Pico video disabled after error: %s", exc)
+        logger.warning("Pico video disabled after error; tracking and control continue: %s", exc)
 
 
 class _VideoProducer:
@@ -154,6 +148,10 @@ def stop(self) -> None: ...
 
 
 class _RealSenseVideoProducer(_VideoProducer):
+    _STARTUP_WAIT_S = 5.0
+    _FRAME_TIMEOUT_MS = 1000
+    _RECONNECT_DELAY_S = 1.0
+
     def __init__(
         self,
         provider: Any,
@@ -175,11 +173,14 @@ def pushed_frames(self) -> int:
 
     def start(self) -> None:
         self._thread.start()
-        self._ready_event.wait(timeout=5.0)
+        self._ready_event.wait(timeout=self._STARTUP_WAIT_S)
         if self._error is not None:
             raise RuntimeError("failed to start RealSense video producer") from self._error
         if not self._ready_event.is_set():
-            raise TimeoutError("RealSense video producer did not become ready within 5s")
+            logger.warning(
+                "RealSense video producer is not ready after %.1fs; reconnecting in background",
+                self._STARTUP_WAIT_S,
+            )
 
     def tick(self) -> None:
         if self._error is not None:
@@ -193,36 +194,66 @@ def stop(self) -> None:
     def _run(self) -> None:
         try:
             import pyrealsense2 as rs
-
-            pipeline = rs.pipeline()
-            config = rs.config()
-            if self._config.device is not None:
-                config.enable_device(self._config.device)
-            config.enable_stream(
-                rs.stream.color,
-                self._config.width,
-                self._config.height,
-                rs.format.rgb8,
-                self._config.fps,
-            )
-            pipeline.start(config)
+        except BaseException as exc:
+            self._error = exc
             self._ready_event.set()
-            try:
-                while not self._stop_event.is_set():
-                    frames = pipeline.wait_for_frames()
-                    color_frame = frames.get_color_frame()
-                    if not color_frame:
-                        continue
-                    rgb = np.ascontiguousarray(np.asanyarray(color_frame.get_data()), dtype=np.uint8)
-                    timestamp_s = time.monotonic()
-                    if self._frame_callback is not None:
-                        self._frame_callback(rgb, timestamp_s)
-                    if callable(getattr(self._provider, "push_video_frame", None)):
-                        self._pushed_frames = int(self._provider.push_video_frame(rgb))
-                    else:
-                        self._pushed_frames += 1
-            finally:
-                pipeline.stop()
+            logger.exception("RealSense Pico video producer could not load pyrealsense2")
+            return
+
+        reconnecting = False
+        try:
+            while not self._stop_event.is_set():
+                pipeline = None
+                pipeline_started = False
+                try:
+                    pipeline = rs.pipeline()
+                    config = rs.config()
+                    if self._config.device is not None:
+                        config.enable_device(self._config.device)
+                    config.enable_stream(
+                        rs.stream.color,
+                        self._config.width,
+                        self._config.height,
+                        rs.format.rgb8,
+                        self._config.fps,
+                    )
+                    pipeline.start(config)
+                    pipeline_started = True
+                    self._ready_event.set()
+                    if reconnecting:
+                        logger.info("RealSense Pico video stream reconnected")
+                    reconnecting = False
+                    while not self._stop_event.is_set():
+                        frames = pipeline.wait_for_frames(self._FRAME_TIMEOUT_MS)
+                        color_frame = frames.get_color_frame()
+                        if not color_frame:
+                            continue
+                        rgb = np.ascontiguousarray(np.asanyarray(color_frame.get_data()), dtype=np.uint8)
+                        timestamp_s = time.monotonic()
+                        if self._frame_callback is not None:
+                            self._frame_callback(rgb, timestamp_s)
+                        if callable(getattr(self._provider, "push_video_frame", None)):
+                            self._pushed_frames = int(self._provider.push_video_frame(rgb))
+                        else:
+                            self._pushed_frames += 1
+                except Exception as exc:
+                    if self._stop_event.is_set():
+                        break
+                    reconnecting = True
+                    logger.warning(
+                        "RealSense Pico video stream lost; reconnecting in %.1fs: %s",
+                        self._RECONNECT_DELAY_S,
+                        exc,
+                    )
+                finally:
+                    if pipeline_started and pipeline is not None:
+                        try:
+                            pipeline.stop()
+                        except RuntimeError as exc:
+                            logger.warning("Failed to stop RealSense pipeline during reconnect: %s", exc)
+
+                if not self._stop_event.is_set():
+                    self._stop_event.wait(self._RECONNECT_DELAY_S)
         except BaseException as exc:
             self._error = exc
             self._ready_event.set()
diff --git a/teleopit/pipeline.py b/teleopit/pipeline.py
index 181a1912..5a9e0b63 100644
--- a/teleopit/pipeline.py
+++ b/teleopit/pipeline.py
@@ -43,7 +43,6 @@ def __init__(self, cfg: DictConfig | dict[str, Any], *, console: PlainConsole |
         self.video_runtime = PicoVideoRuntime(
             provider=self.input_provider,
             config=parse_pico_video_config(input_cfg),
-            mode="sim2sim",
             robot=self.robot,
         )
         self.loop = SimulationLoop(
diff --git a/teleopit/recording/hdf5.py b/teleopit/recording/hdf5.py
index 36849179..723157fb 100644
--- a/teleopit/recording/hdf5.py
+++ b/teleopit/recording/hdf5.py
@@ -1,60 +1,91 @@
-"""HDF5 recorder and schema helpers for Teleopit sim2real recording."""
+"""Editable HDF5 dataset writer for Teleopit sim2real recording."""
 
 from __future__ import annotations
 
 from dataclasses import dataclass
 import json
 import logging
+import os
 from pathlib import Path
 import re
-import time
 from typing import Any
 
 import h5py
 import numpy as np
 
-from teleopit.constants import FULL_QPOS_DIM, NUM_JOINTS
+from teleopit.constants import FULL_QPOS_DIM, G1_JOINT_NAMES, NUM_JOINTS
 from teleopit.controllers.observation import _quat_rotate_np
 from teleopit.math_utils import quat_inv_np
 from teleopit.runtime.common import cfg_get
+from teleopit.sim2real.hands.linkerhand_l6 import L6_SDK_JOINT_ORDER
+from teleopit.sim2real.hands.linkerhand_o6 import O6_SDK_JOINT_ORDER
 
 
 logger = logging.getLogger(__name__)
 
 IMAGE_KEY = "observation.images.d435i_rgb"
 STATE_KEY = "observation.state"
+HAND_STATE_KEY = "observation.state.hand"
+NECK_STATE_KEY = "observation.state.neck"
 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
-MODE_DIM = 1
+HAND_STATE_DIM = 12
+NECK_STATE_DIM = 2
 ACTION_DIM = FULL_QPOS_DIM
 HAND_ACTION_DIM = 12
+NECK_ACTION_DIM = 2
 DEFAULT_IMAGE_SHAPE = (480, 640, 3)
-HDF5_RECORDING_FORMAT = "teleopit_sim2real_recording_hdf5"
-HDF5_RECORDING_VERSION = 1
+HDF5_RECORDING_FORMAT = "teleopit_hdf5"
+HDF5_RECORDING_VERSION = 4
+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,
     "arms": 2,
     "pause": 3,
 }
+_EPISODE_HDF5_FILENAME = re.compile(r"episode_\d{6,}\.h5")
+_EPISODE_MP4_FILENAME = re.compile(r"episode_\d{6,}\.mp4")
 
 
 @dataclass(frozen=True)
 class RecordingSchema:
+    fps: int
+    robot_type: str
+    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
+    hand_state_key: str = HAND_STATE_KEY
+    hand_state_dim: int = HAND_STATE_DIM
+    neck_state_key: str = NECK_STATE_KEY
+    neck_state_dim: int = NECK_STATE_DIM
     mode_key: str = MODE_KEY
-    mode_dim: int = MODE_DIM
     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)
@@ -64,13 +95,47 @@ class MP4VideoConfig:
     pixelformat: str = "yuv420p"
 
 
-def build_recording_schema(camera_cfg: Any) -> RecordingSchema:
-    key = str(cfg_get(camera_cfg, "key", IMAGE_KEY))
+def build_recording_schema(
+    camera_cfg: Any,
+    *,
+    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:
         raise ValueError("recording.camera.width and recording.camera.height must be positive")
-    return RecordingSchema(image_key=key, image_shape=(height, width, 3))
+    if parsed_fps <= 0:
+        raise ValueError("recording.fps must be positive")
+    if parsed_robot_type != DEFAULT_ROBOT_TYPE:
+        raise ValueError(
+            f"Unsupported recording robot_type={parsed_robot_type!r}; expected {DEFAULT_ROBOT_TYPE!r}"
+        )
+    if parsed_hand_type not in SUPPORTED_HAND_TYPES:
+        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),
+    )
 
 
 def build_mp4_video_config(video_cfg: Any) -> MP4VideoConfig:
@@ -84,77 +149,90 @@ def build_mp4_video_config(video_cfg: Any) -> MP4VideoConfig:
     )
 
 
-def hdf5_schema(
-    schema: RecordingSchema,
-    *,
-    video_config: MP4VideoConfig | None = None,
-) -> dict[str, object]:
-    video_cfg = video_config or MP4VideoConfig()
-    return {
-        "format": HDF5_RECORDING_FORMAT,
-        "version": HDF5_RECORDING_VERSION,
-        "features": {
-            schema.image_key: {
-                "type": "video",
-                "format": "mp4",
-                "codec": video_cfg.codec,
-                "shape": list(schema.image_shape),
-                "dtype": "uint8",
-                "sync": {
-                    "frame_index": FRAME_INDEX_KEY,
-                    "timestamp": TIMESTAMP_KEY,
-                },
-            },
-            FRAME_INDEX_KEY: {
-                "type": "index",
-                "shape": [],
-                "dtype": "int64",
-            },
-            TIMESTAMP_KEY: {
-                "type": "timestamp",
-                "shape": [],
-                "dtype": "float64",
-                "units": "seconds",
-            },
-            schema.state_key: {
-                "type": "low_dim",
-                "shape": [schema.state_dim],
-                "dtype": "float32",
-                "slices": {
-                    "joint_pos": [0, 29],
-                    "joint_vel": [29, 58],
-                    "base_quat_wxyz": [58, 62],
-                    "base_ang_vel": [62, 65],
-                    "projected_gravity": [65, 68],
-                },
+def hdf5_schema(schema: RecordingSchema) -> dict[str, object]:
+    features: dict[str, object] = {
+        FRAME_INDEX_KEY: {
+            "dtype": "int64",
+            "shape": [],
+        },
+        TIMESTAMP_KEY: {
+            "dtype": "float64",
+            "shape": [],
+            "units": "seconds",
+        },
+        schema.state_key: {
+            "dtype": "float32",
+            "shape": [schema.state_dim],
+            "names": _state_names(),
+            "groups": {
+                "joint_pos": [0, 29],
+                "joint_vel": [29, 58],
+                "base_quat_wxyz": [58, 62],
+                "base_ang_vel": [62, 65],
+                "projected_gravity": [65, 68],
             },
-            schema.mode_key: {
-                "type": "categorical",
-                "shape": [schema.mode_dim],
-                "dtype": "float32",
-                "codes": MODE_CODES,
+        },
+        schema.mode_key: {
+            "dtype": "int8",
+            "shape": [],
+            "values": MODE_CODES,
+        },
+        schema.action_key: {
+            "dtype": "float32",
+            "shape": [schema.action_dim],
+            "names": _reference_action_names(),
+            "groups": {
+                "root_pos": [0, 3],
+                "root_quat_wxyz": [3, 7],
+                "reference_joint_pos": [7, 36],
             },
-            schema.action_key: {
-                "type": "low_dim",
-                "shape": [schema.action_dim],
-                "dtype": "float32",
-                "slices": {
-                    "root_pos": [0, 3],
-                    "root_quat_wxyz": [3, 7],
-                    "joint_pos": [7, 36],
-                },
+        },
+    }
+    if schema.has_hand_action:
+        features[schema.hand_state_key] = {
+            "dtype": "float32",
+            "shape": [schema.hand_state_dim],
+            "names": _hand_action_names(schema.hand_type),
+            "groups": {
+                "left_hand_state": [0, 6],
+                "right_hand_state": [6, 12],
             },
-            schema.hand_action_key: {
-                "type": "low_dim",
-                "shape": [schema.hand_action_dim],
-                "dtype": "float32",
-                "units": "linkerhand_uint8_pose",
-                "slices": {
-                    "left_pose": [0, 6],
-                    "right_pose": [6, 12],
-                },
+        }
+        features[schema.hand_action_key] = {
+            "dtype": "float32",
+            "shape": [schema.hand_action_dim],
+            "names": _hand_action_names(schema.hand_type),
+            "groups": {
+                "left_hand_target": [0, 6],
+                "right_hand_target": [6, 12],
             },
-        },
+        }
+    if schema.has_neck_action:
+        features[schema.neck_state_key] = {
+            "dtype": "float32",
+            "shape": [schema.neck_state_dim],
+            "names": ["yaw_deg", "pitch_deg"],
+            "units": "degrees",
+        }
+        features[schema.neck_action_key] = {
+            "dtype": "float32",
+            "shape": [schema.neck_action_dim],
+            "names": ["yaw_deg", "pitch_deg"],
+            "units": "degrees",
+        }
+    features[schema.image_key] = {
+        "dtype": "video",
+        "shape": list(schema.image_shape),
+        "names": ["height", "width", "channel"],
+    }
+    return {
+        "format": HDF5_RECORDING_FORMAT,
+        "version": HDF5_RECORDING_VERSION,
+        "fps": schema.fps,
+        "robot_type": schema.robot_type,
+        "hand_type": schema.hand_type,
+        "neck_type": schema.neck_type,
+        "features": features,
     }
 
 
@@ -183,7 +261,7 @@ def build_observation_state(robot_state: object) -> np.ndarray:
 
 
 def normalize_action_reference_qpos(reference_qpos: object) -> np.ndarray:
-    action = np.asarray(reference_qpos, dtype=np.float32).reshape(-1)[:ACTION_DIM]
+    action = np.asarray(reference_qpos, dtype=np.float32).reshape(-1)
     if action.shape[0] != ACTION_DIM:
         raise ValueError(f"recording action reference qpos must be {ACTION_DIM}D, got {action.shape[0]}")
     return action
@@ -202,41 +280,51 @@ def normalize_hand_action(left_pose: object, right_pose: object) -> np.ndarray:
     return action
 
 
-def build_mode_observation(mode: str) -> np.ndarray:
+def build_neck_action(yaw_deg: object, pitch_deg: object) -> np.ndarray:
+    action = np.asarray([yaw_deg, pitch_deg], 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:
         raise ValueError(f"Unsupported recording mode {mode!r}; expected one of {sorted(MODE_CODES)}")
-    return np.array([MODE_CODES[normalized]], dtype=np.float32)
+    return np.int8(MODE_CODES[normalized])
 
 
 class TeleopitHDF5Recorder:
-    """Writes one HDF5 file per saved sim2real recording episode."""
+    """Writes editable per-episode HDF5 and MP4 files plus a JSONL manifest."""
 
     def __init__(
         self,
         *,
         output_dir: Path,
         task: str,
-        fps: int,
         schema: RecordingSchema,
         video_config: MP4VideoConfig | None = None,
     ) -> None:
         self._output_dir = output_dir
-        self._task = str(task)
-        self._fps = int(fps)
+        self._task = str(task).strip()
         self._schema = schema
+        self._fps = schema.fps
         self._video_config = video_config or MP4VideoConfig()
         self._active = False
         self._frames_in_episode = 0
-        self._episode_index = 0
+        self._next_episode_index = 0
+        self._active_episode_index: int | None = None
         self._h5: h5py.File | None = None
         self._tmp_path: Path | None = None
         self._episode_path: Path | None = None
         self._tmp_video_path: Path | None = None
         self._episode_video_path: Path | None = None
+        self._data_rel_path: str | None = None
         self._video_rel_path: str | None = None
         self._video_writer: Any | None = None
         self._datasets: dict[str, h5py.Dataset] = {}
+        if not self._task:
+            raise ValueError("recording.task must not be empty")
 
     @classmethod
     def create(
@@ -244,7 +332,6 @@ def create(
         *,
         output_dir: str | Path,
         task: str,
-        fps: int,
         schema: RecordingSchema,
         video_config: MP4VideoConfig | None = None,
     ) -> "TeleopitHDF5Recorder":
@@ -253,36 +340,39 @@ def create(
         recorder = cls(
             output_dir=root,
             task=task,
-            fps=fps,
             schema=schema,
             video_config=video_config,
         )
-        recorder._write_schema_sidecar()
+        recorder._initialize_dataset()
         return recorder
 
     def start_episode(self) -> None:
         if self._active:
             raise RuntimeError("Cannot start a new recording episode while one is active")
-        self._episode_index += 1
-        timestamp = time.strftime("%Y%m%d_%H%M%S")
-        stem = f"episode_{timestamp}_{time.time_ns()}_{self._episode_index:06d}"
-        tmp_dir = self._output_dir / ".tmp"
-        episodes_dir = self._output_dir / "episodes"
-        tmp_dir.mkdir(parents=True, exist_ok=True)
-        episodes_dir.mkdir(parents=True, exist_ok=True)
-        self._tmp_path = tmp_dir / f"{stem}.h5"
-        self._episode_path = episodes_dir / f"{stem}.h5"
-        self._h5 = h5py.File(self._tmp_path, "w")
-        video_dir = self._output_dir / "videos" / _safe_path_component(self._schema.image_key)
-        tmp_video_dir = tmp_dir / "videos" / _safe_path_component(self._schema.image_key)
-        video_dir.mkdir(parents=True, exist_ok=True)
-        tmp_video_dir.mkdir(parents=True, exist_ok=True)
+        episode_index = self._next_episode_index
+        stem = f"episode_{episode_index:06d}"
+        data_dir = self._output_dir / "data"
+        video_storage_key = _video_storage_key(self._schema.image_key)
+        video_dir = self._output_dir / "videos" / video_storage_key
+        tmp_data_dir = self._output_dir / ".tmp" / "data"
+        tmp_video_dir = self._output_dir / ".tmp" / "videos" / video_storage_key
+        for path in (data_dir, video_dir, tmp_data_dir, tmp_video_dir):
+            path.mkdir(parents=True, exist_ok=True)
+
+        self._tmp_path = tmp_data_dir / f"{stem}.h5"
+        self._episode_path = data_dir / f"{stem}.h5"
         self._tmp_video_path = tmp_video_dir / f"{stem}.mp4"
         self._episode_video_path = video_dir / f"{stem}.mp4"
+        self._data_rel_path = self._episode_path.relative_to(self._output_dir).as_posix()
         self._video_rel_path = self._episode_video_path.relative_to(self._output_dir).as_posix()
+        self._active_episode_index = episode_index
+        if self._episode_path.exists() or self._episode_video_path.exists():
+            self._reset_episode()
+            raise FileExistsError(f"Recording episode {stem} already exists")
+
         try:
+            self._h5 = h5py.File(self._tmp_path, "w")
             self._video_writer = self._create_video_writer(self._tmp_video_path)
-            self._write_episode_header(self._h5)
             self._datasets = self._create_datasets(self._h5)
             self._active = True
             self._frames_in_episode = 0
@@ -295,10 +385,12 @@ def add_frame(
         *,
         image: np.ndarray,
         state: np.ndarray,
-        mode: np.ndarray,
+        mode: object,
         action: np.ndarray,
-        hand_action: np.ndarray,
-        task: str,
+        hand_state: np.ndarray | None = None,
+        neck_state: np.ndarray | None = None,
+        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")
@@ -306,9 +398,21 @@ def add_frame(
         if tuple(image_arr.shape) != self._schema.image_shape:
             raise ValueError(f"{self._schema.image_key} frame shape {image_arr.shape} != {self._schema.image_shape}")
         state_arr = self._validate_vector(state, self._schema.state_key, self._schema.state_dim)
-        mode_arr = self._validate_vector(mode, self._schema.mode_key, self._schema.mode_dim)
+        mode_value = self._validate_mode(mode)
         action_arr = self._validate_vector(action, self._schema.action_key, self._schema.action_dim)
-        hand_action_arr = self._validate_vector(hand_action, self._schema.hand_action_key, self._schema.hand_action_dim)
+        optional_vectors: dict[str, np.ndarray] = {}
+        for enabled, value, key, dim in (
+            (self._schema.has_hand_action, hand_state, self._schema.hand_state_key, self._schema.hand_state_dim),
+            (self._schema.has_hand_action, hand_action, self._schema.hand_action_key, self._schema.hand_action_dim),
+            (self._schema.has_neck_action, neck_state, self._schema.neck_state_key, self._schema.neck_state_dim),
+            (self._schema.has_neck_action, neck_action, self._schema.neck_action_key, self._schema.neck_action_dim),
+        ):
+            if enabled and value is None:
+                raise ValueError(f"{key} is required when its device is enabled")
+            if not enabled and value is not None:
+                raise ValueError(f"{key} must be omitted when its device is disabled")
+            if value is not None:
+                optional_vectors[key] = self._validate_vector(value, key, dim)
 
         row = self._frames_in_episode
         for dataset in self._datasets.values():
@@ -319,12 +423,11 @@ def add_frame(
         self._datasets[FRAME_INDEX_KEY][row] = row
         self._datasets[TIMESTAMP_KEY][row] = float(row) / float(self._fps)
         self._datasets[self._schema.state_key][row] = state_arr
-        self._datasets[self._schema.mode_key][row] = mode_arr
+        self._datasets[self._schema.mode_key][row] = mode_value
         self._datasets[self._schema.action_key][row] = action_arr
-        self._datasets[self._schema.hand_action_key][row] = hand_action_arr
+        for key, value in optional_vectors.items():
+            self._datasets[key][row] = value
         self._frames_in_episode += 1
-        self._h5.attrs["frames"] = self._frames_in_episode
-        self._h5.attrs["task"] = str(task)
 
     def save_episode(self) -> None:
         if not self._active:
@@ -333,11 +436,38 @@ def save_episode(self) -> None:
         episode_path = self._require_episode_path()
         tmp_video_path = self._tmp_video_path
         episode_video_path = self._episode_video_path
+        episode_index = self._active_episode_index
+        data_rel_path = self._data_rel_path
+        video_rel_path = self._video_rel_path
+        frames = self._frames_in_episode
         self._close_active_outputs()
-        if tmp_video_path is None or episode_video_path is None:
-            raise RuntimeError("recording episode has no video output path")
-        tmp_video_path.replace(episode_video_path)
-        tmp_path.replace(episode_path)
+        if (
+            tmp_video_path is None
+            or episode_video_path is None
+            or episode_index is None
+            or data_rel_path is None
+            or video_rel_path is None
+        ):
+            raise RuntimeError("recording episode paths are incomplete")
+        try:
+            tmp_video_path.replace(episode_video_path)
+            tmp_path.replace(episode_path)
+            self._append_manifest_entry(
+                {
+                    "episode_index": episode_index,
+                    "frames": frames,
+                    "task": self._task,
+                    "data": data_rel_path,
+                    "videos": {self._schema.image_key: video_rel_path},
+                }
+            )
+        except Exception:
+            for path in (tmp_path, tmp_video_path, episode_path, episode_video_path):
+                if path.exists():
+                    path.unlink()
+            self._reset_episode()
+            raise
+        self._next_episode_index += 1
         self._reset_episode()
 
     def discard_episode(self) -> None:
@@ -346,39 +476,176 @@ def discard_episode(self) -> None:
         tmp_path = self._tmp_path
         tmp_video_path = self._tmp_video_path
         self._close_active_outputs()
-        if tmp_path is not None and tmp_path.exists():
-            tmp_path.unlink()
-        if tmp_video_path is not None and tmp_video_path.exists():
-            tmp_video_path.unlink()
+        for path in (tmp_path, tmp_video_path):
+            if path is not None and path.exists():
+                path.unlink()
         self._reset_episode()
 
     def finalize(self) -> None:
         if self._active:
             self.discard_episode()
 
-    def _write_schema_sidecar(self) -> None:
-        path = self._output_dir / "schema.json"
-        path.write_text(json.dumps(self._schema_dict(), indent=2) + "\n", encoding="utf-8")
-
-    def _write_episode_header(self, h5: h5py.File) -> None:
-        h5.attrs["format"] = HDF5_RECORDING_FORMAT
-        h5.attrs["version"] = HDF5_RECORDING_VERSION
-        h5.attrs["task"] = self._task
-        h5.attrs["fps"] = self._fps
-        h5.attrs["frames"] = 0
-        h5.attrs["schema_json"] = json.dumps(self._schema_dict(), sort_keys=True)
-        h5.attrs["video_key"] = self._schema.image_key
-        h5.attrs["video_path"] = self._video_rel_path or ""
-        h5.attrs["video_format"] = "mp4"
-        h5.attrs["video_codec"] = self._video_config.codec
-        h5.attrs["video_pixelformat"] = self._video_config.pixelformat
-        h5.attrs["video_fps"] = self._fps
-        h5.attrs["video_frames"] = 0
-        h5.attrs["video_from_timestamp_s"] = 0.0
-        h5.attrs["video_to_timestamp_s"] = 0.0
+    def _initialize_dataset(self) -> None:
+        schema_path = self._output_dir / "schema.json"
+        expected_schema = self._schema_dict()
+        if schema_path.exists():
+            try:
+                existing_schema = json.loads(schema_path.read_text(encoding="utf-8"))
+            except (OSError, json.JSONDecodeError) as exc:
+                raise ValueError(f"Invalid recording schema: {schema_path}") from exc
+            if existing_schema != expected_schema:
+                raise ValueError(
+                    f"Recording schema mismatch at {schema_path}; use an empty output_dir for the new dataset"
+                )
+        else:
+            if self._has_recorded_payload():
+                raise ValueError(
+                    f"Recording output {self._output_dir} contains data without schema.json; use an empty output_dir"
+                )
+            schema_path.write_text(
+                json.dumps(expected_schema, indent=2, ensure_ascii=False) + "\n",
+                encoding="utf-8",
+            )
+
+        manifest_path = self._manifest_path
+        if not manifest_path.exists():
+            if self._has_recorded_payload():
+                raise ValueError(
+                    f"Recording output {self._output_dir} contains data without episodes.jsonl; use an empty output_dir"
+                )
+            manifest_path.touch()
+        entries = self._read_manifest_entries()
+        self._discard_uncommitted_episode_files(entries)
+        self._next_episode_index = len(entries)
+
+    @property
+    def _manifest_path(self) -> Path:
+        return self._output_dir / "episodes.jsonl"
+
+    def _has_recorded_payload(self) -> bool:
+        for dirname in ("data", "videos", "episodes"):
+            path = self._output_dir / dirname
+            if path.exists() and any(item.is_file() for item in path.rglob("*")):
+                return True
+        return False
+
+    def _read_manifest_entries(self) -> list[dict[str, object]]:
+        entries: list[dict[str, object]] = []
+        for line_number, raw_line in enumerate(
+            self._manifest_path.read_text(encoding="utf-8").splitlines(),
+            start=1,
+        ):
+            line = raw_line.strip()
+            if not line:
+                continue
+            try:
+                entry = json.loads(line)
+            except json.JSONDecodeError as exc:
+                raise ValueError(f"Invalid JSON in {self._manifest_path}:{line_number}") from exc
+            if not isinstance(entry, dict):
+                raise ValueError(f"Episode entry in {self._manifest_path}:{line_number} must be an object")
+            expected_index = len(entries)
+            if entry.get("episode_index") != expected_index:
+                raise ValueError(
+                    f"Episode indices in {self._manifest_path} must be contiguous from 0; "
+                    f"line {line_number} expected {expected_index}, got {entry.get('episode_index')!r}"
+                )
+            data_path = entry.get("data")
+            videos = entry.get("videos")
+            if not isinstance(data_path, str) or not isinstance(videos, dict):
+                raise ValueError(f"Episode entry in {self._manifest_path}:{line_number} has invalid paths")
+            referenced_paths = [data_path, *[str(path) for path in videos.values()]]
+            for relative_path in referenced_paths:
+                if not (self._output_dir / relative_path).is_file():
+                    raise ValueError(
+                        f"Episode entry in {self._manifest_path}:{line_number} references missing file {relative_path!r}"
+                    )
+            entries.append(entry)
+        return entries
+
+    def _append_manifest_entry(self, entry: dict[str, object]) -> None:
+        current = self._manifest_path.read_bytes()
+        if current and not current.endswith(b"\n"):
+            current += b"\n"
+        encoded_entry = (
+            json.dumps(entry, ensure_ascii=False, separators=(",", ":")) + "\n"
+        ).encode("utf-8")
+        tmp_path = self._output_dir / ".tmp" / "episodes.jsonl"
+        tmp_path.parent.mkdir(parents=True, exist_ok=True)
+        try:
+            with tmp_path.open("wb") as handle:
+                handle.write(current)
+                handle.write(encoded_entry)
+                handle.flush()
+                os.fsync(handle.fileno())
+            tmp_path.replace(self._manifest_path)
+        except Exception:
+            if tmp_path.exists():
+                tmp_path.unlink()
+            raise
+
+    def _discard_uncommitted_episode_files(self, entries: list[dict[str, object]]) -> None:
+        committed_paths: set[Path] = set()
+        for entry in entries:
+            data_path = entry.get("data")
+            videos = entry.get("videos")
+            if isinstance(data_path, str):
+                committed_paths.add((self._output_dir / data_path).resolve())
+            if isinstance(videos, dict):
+                committed_paths.update(
+                    (self._output_dir / str(relative_path)).resolve()
+                    for relative_path in videos.values()
+                )
+
+        video_storage_key = _video_storage_key(self._schema.image_key)
+        final_candidates = [
+            *self._matching_episode_files(
+                self._output_dir / "data",
+                _EPISODE_HDF5_FILENAME,
+            ),
+            *self._matching_episode_files(
+                self._output_dir / "videos" / video_storage_key,
+                _EPISODE_MP4_FILENAME,
+            ),
+        ]
+        tmp_candidates = [
+            *self._matching_episode_files(
+                self._output_dir / ".tmp" / "data",
+                _EPISODE_HDF5_FILENAME,
+            ),
+            *self._matching_episode_files(
+                self._output_dir / ".tmp" / "videos" / video_storage_key,
+                _EPISODE_MP4_FILENAME,
+            ),
+            self._output_dir / ".tmp" / "episodes.jsonl",
+        ]
+        for path in final_candidates:
+            if path.resolve() not in committed_paths:
+                self._discard_interrupted_artifact(path)
+        for path in tmp_candidates:
+            if path.is_file():
+                self._discard_interrupted_artifact(path)
+
+    @staticmethod
+    def _matching_episode_files(directory: Path, pattern: re.Pattern[str]) -> list[Path]:
+        if not directory.is_dir():
+            return []
+        return [
+            path
+            for path in directory.iterdir()
+            if path.is_file() and pattern.fullmatch(path.name)
+        ]
+
+    @staticmethod
+    def _discard_interrupted_artifact(path: Path) -> None:
+        try:
+            path.unlink()
+        except OSError as exc:
+            raise RuntimeError(f"Failed to discard interrupted recording artifact: {path}") from exc
+        logger.warning("Discarded interrupted recording artifact: %s", path)
 
     def _create_datasets(self, h5: h5py.File) -> dict[str, h5py.Dataset]:
-        return {
+        datasets = {
             FRAME_INDEX_KEY: h5.create_dataset(
                 FRAME_INDEX_KEY,
                 shape=(0,),
@@ -393,13 +660,33 @@ def _create_datasets(self, h5: h5py.File) -> dict[str, h5py.Dataset]:
                 chunks=(1024,),
                 dtype=np.float64,
             ),
-            self._schema.state_key: self._create_vector_dataset(h5, self._schema.state_key, self._schema.state_dim),
-            self._schema.mode_key: self._create_vector_dataset(h5, self._schema.mode_key, self._schema.mode_dim),
-            self._schema.action_key: self._create_vector_dataset(h5, self._schema.action_key, self._schema.action_dim),
-            self._schema.hand_action_key: self._create_vector_dataset(
-                h5, self._schema.hand_action_key, self._schema.hand_action_dim
+            self._schema.state_key: self._create_vector_dataset(
+                h5,
+                self._schema.state_key,
+                self._schema.state_dim,
+            ),
+            self._schema.mode_key: h5.create_dataset(
+                self._schema.mode_key,
+                shape=(0,),
+                maxshape=(None,),
+                chunks=(1024,),
+                dtype=np.int8,
+            ),
+            self._schema.action_key: self._create_vector_dataset(
+                h5,
+                self._schema.action_key,
+                self._schema.action_dim,
             ),
         }
+        for enabled, key, dim in (
+            (self._schema.has_hand_action, self._schema.hand_state_key, self._schema.hand_state_dim),
+            (self._schema.has_hand_action, self._schema.hand_action_key, self._schema.hand_action_dim),
+            (self._schema.has_neck_action, self._schema.neck_state_key, self._schema.neck_state_dim),
+            (self._schema.has_neck_action, self._schema.neck_action_key, self._schema.neck_action_dim),
+        ):
+            if enabled:
+                datasets[key] = self._create_vector_dataset(h5, key, dim)
+        return datasets
 
     @staticmethod
     def _create_vector_dataset(h5: h5py.File, key: str, dim: int) -> h5py.Dataset:
@@ -418,11 +705,18 @@ def _validate_vector(value: object, key: str, dim: int) -> np.ndarray:
             raise ValueError(f"{key} must be {dim}D")
         return arr
 
+    @staticmethod
+    def _validate_mode(value: object) -> np.int8:
+        arr = np.asarray(value).reshape(-1)
+        if arr.shape[0] != 1:
+            raise ValueError(f"{MODE_KEY} must be scalar")
+        parsed = int(arr[0])
+        if parsed not in MODE_CODES.values():
+            raise ValueError(f"{MODE_KEY} must be one of {sorted(MODE_CODES.values())}, got {parsed}")
+        return np.int8(parsed)
+
     def _schema_dict(self) -> dict[str, object]:
-        return hdf5_schema(
-            self._schema,
-            video_config=self._video_config,
-        )
+        return hdf5_schema(self._schema)
 
     def _create_video_writer(self, path: Path) -> Any:
         try:
@@ -443,26 +737,23 @@ def _close_active_outputs(self) -> None:
             self._video_writer.close()
             self._video_writer = None
         if self._h5 is not None:
-            self._h5.attrs["video_frames"] = self._frames_in_episode
-            self._h5.attrs["video_to_timestamp_s"] = (
-                float(max(self._frames_in_episode - 1, 0)) / float(self._fps)
-                if self._frames_in_episode > 0
-                else 0.0
-            )
-            self._h5.attrs["video_path"] = self._video_rel_path or ""
             self._h5.close()
-        self._h5 = None
+            self._h5 = None
         self._datasets = {}
 
     def _reset_episode(self) -> None:
         self._active = False
         self._frames_in_episode = 0
+        self._active_episode_index = None
+        self._h5 = None
         self._tmp_path = None
         self._episode_path = None
         self._tmp_video_path = None
         self._episode_video_path = None
+        self._data_rel_path = None
         self._video_rel_path = None
         self._video_writer = None
+        self._datasets = {}
 
     def _cleanup_partial_episode(self) -> None:
         if self._video_writer is not None:
@@ -477,19 +768,12 @@ def _cleanup_partial_episode(self) -> None:
             except Exception:
                 logger.exception("Failed to close partial HDF5 recording file")
             self._h5 = None
-        tmp_path = self._tmp_path
-        tmp_video_path = self._tmp_video_path
-        if tmp_path is not None and tmp_path.exists():
-            try:
-                tmp_path.unlink()
-            except Exception:
-                logger.exception("Failed to remove partial HDF5 recording file: %s", tmp_path)
-        if tmp_video_path is not None and tmp_video_path.exists():
-            try:
-                tmp_video_path.unlink()
-            except Exception:
-                logger.exception("Failed to remove partial MP4 recording file: %s", tmp_video_path)
-        self._datasets = {}
+        for path in (self._tmp_path, self._tmp_video_path):
+            if path is not None and path.exists():
+                try:
+                    path.unlink()
+                except Exception:
+                    logger.exception("Failed to remove partial recording file: %s", path)
         self._reset_episode()
 
     def _require_tmp_path(self) -> Path:
@@ -503,6 +787,50 @@ def _require_episode_path(self) -> Path:
         return self._episode_path
 
 
-def _safe_path_component(value: str) -> str:
-    safe = re.sub(r"[^A-Za-z0-9._-]+", "_", value).strip("._")
+def _state_names() -> list[str]:
+    return [
+        *[f"{name}.position" for name in G1_JOINT_NAMES],
+        *[f"{name}.velocity" for name in G1_JOINT_NAMES],
+        "base_quat.w",
+        "base_quat.x",
+        "base_quat.y",
+        "base_quat.z",
+        "base_ang_vel.x",
+        "base_ang_vel.y",
+        "base_ang_vel.z",
+        "projected_gravity.x",
+        "projected_gravity.y",
+        "projected_gravity.z",
+    ]
+
+
+def _reference_action_names() -> list[str]:
+    return [
+        "root_pos.x",
+        "root_pos.y",
+        "root_pos.z",
+        "root_quat.w",
+        "root_quat.x",
+        "root_quat.y",
+        "root_quat.z",
+        *G1_JOINT_NAMES,
+    ]
+
+
+def _hand_action_names(hand_type: str) -> list[str]:
+    if hand_type == "linkerhand_l6":
+        joint_order = L6_SDK_JOINT_ORDER
+    elif hand_type == "linkerhand_o6":
+        joint_order = O6_SDK_JOINT_ORDER
+    else:
+        raise ValueError(f"hand action names are unavailable for hand_type={hand_type!r}")
+    return [
+        *[f"left_{name}" for name in joint_order],
+        *[f"right_{name}" for name in joint_order],
+    ]
+
+
+def _video_storage_key(value: str) -> str:
+    leaf = value.rsplit(".", 1)[-1]
+    safe = re.sub(r"[^A-Za-z0-9._-]+", "_", leaf).strip("._")
     return safe or "camera"
diff --git a/teleopit/retargeting/gmr/motion_retarget.py b/teleopit/retargeting/gmr/motion_retarget.py
index c97400f9..2958e0b3 100644
--- a/teleopit/retargeting/gmr/motion_retarget.py
+++ b/teleopit/retargeting/gmr/motion_retarget.py
@@ -165,13 +165,57 @@ def reset_configuration(self):
         pause/resume) so the warm-start IK solver does not get stuck in a
         local minimum far from the new target.
 
-        The next ``retarget()`` call will use many more iterations so the
-        solver can converge from the default pose to the (potentially distant)
-        new target.
+        The next ``retarget()`` call will seed the floating root from the live
+        target and use many more iterations so the articulated joints can
+        converge from their default pose.
         """
         self.configuration.update(q=self.model.qpos0.copy())
         self._warmup_needed = True
 
+    def _seed_warmup_root_from_target(self):
+        """Seed a floating root from the current human-root target.
+
+        Starting every reset from the model's fixed world heading can make the
+        nonlinear IK solve converge to a different joint branch when the live
+        subject faces some directions.  The root target is already known after
+        ``update_targets()``, so initialize only the floating root from it and
+        leave all articulated joints at their default values.
+        """
+        root_body_id = mj.mj_name2id(
+            self.model,
+            mj.mjtObj.mjOBJ_BODY,
+            self.robot_root_name,
+        )
+        if root_body_id < 0:
+            raise ValueError(f"Robot root body '{self.robot_root_name}' was not found")
+
+        free_joint_id = None
+        joint_start = int(self.model.body_jntadr[root_body_id])
+        joint_count = int(self.model.body_jntnum[root_body_id])
+        for joint_id in range(joint_start, joint_start + joint_count):
+            if self.model.jnt_type[joint_id] == mj.mjtJoint.mjJNT_FREE:
+                free_joint_id = joint_id
+                break
+        if free_joint_id is None:
+            return
+
+        root_pos, root_quat = self.scaled_human_data[self.human_root_name]
+        root_pos = np.asarray(root_pos, dtype=np.float64).reshape(-1)
+        root_quat = np.asarray(root_quat, dtype=np.float64).reshape(-1)
+        if root_pos.shape != (3,) or not np.all(np.isfinite(root_pos)):
+            raise ValueError(f"Human root position must be finite 3D, got {root_pos}")
+        if root_quat.shape != (4,) or not np.all(np.isfinite(root_quat)):
+            raise ValueError(f"Human root quaternion must be finite wxyz, got {root_quat}")
+        quat_norm = float(np.linalg.norm(root_quat))
+        if quat_norm <= 1e-9:
+            raise ValueError("Human root quaternion norm must be positive")
+
+        q_seed = self.model.qpos0.copy()
+        qpos_adr = int(self.model.jnt_qposadr[free_joint_id])
+        q_seed[qpos_adr:qpos_adr + 3] = root_pos
+        q_seed[qpos_adr + 3:qpos_adr + 7] = root_quat / quat_norm
+        self.configuration.update(q=q_seed)
+
     def setup_retarget_configuration(self):
         self.configuration = mink.Configuration(self.model)
     
@@ -244,10 +288,11 @@ def retarget(self, human_data, offset_to_ground=False):
         # Update the task targets
         self.update_targets(human_data, offset_to_ground)
 
-        # After a reset, use a large dt and more iterations so the solver
-        # can converge from the default pose to a potentially distant target.
+        # After a reset, seed the floating root and use a large dt plus more
+        # iterations so the articulated joints can converge from defaults.
         warmup = self._warmup_needed
         if warmup:
+            self._seed_warmup_root_from_target()
             self._warmup_needed = False
         iter_limit = self._warmup_max_iter if warmup else self.max_iter
         dt = self._warmup_dt if warmup else self.configuration.model.opt.timestep
diff --git a/teleopit/retargeting/gmr/params.py b/teleopit/retargeting/gmr/params.py
index 1110eef0..1be0b883 100644
--- a/teleopit/retargeting/gmr/params.py
+++ b/teleopit/retargeting/gmr/params.py
@@ -1,8 +1,8 @@
 from pathlib import Path
 
 from teleopit.runtime.assets import (
-    UNITREE_G1_AVP_O6_XML,
     UNITREE_G1_DEX3_XML,
+    UNITREE_G1_NECK_O6_XML,
     UNITREE_G1_XML,
 )
 
@@ -19,7 +19,7 @@ def _resolve_path(relative_path):
 ROBOT_XML_DICT = {
     "unitree_g1": UNITREE_G1_XML,
     "unitree_g1_with_hands": UNITREE_G1_DEX3_XML,
-    "unitree_g1_avp_o6": UNITREE_G1_AVP_O6_XML,
+    "unitree_g1_neck_o6": UNITREE_G1_NECK_O6_XML,
     "unitree_h1": _resolve_path("assets/unitree_h1/h1.xml"),
     "unitree_h1_2": _resolve_path("assets/unitree_h1_2/h1_2_handless.xml"),
     "booster_t1": _resolve_path("assets/booster_t1/T1_serial.xml"),
@@ -43,7 +43,7 @@ def _resolve_path(relative_path):
     "smplx": {
         "unitree_g1": _resolve_path("ik_configs/smplx_to_g1.json"),
         "unitree_g1_with_hands": _resolve_path("ik_configs/smplx_to_g1.json"),
-        "unitree_g1_avp_o6": _resolve_path("ik_configs/smplx_to_g1.json"),
+        "unitree_g1_neck_o6": _resolve_path("ik_configs/smplx_to_g1.json"),
         "unitree_h1": _resolve_path("ik_configs/smplx_to_h1.json"),
         "unitree_h1_2": _resolve_path("ik_configs/smplx_to_h1_2.json"),
         "booster_t1": _resolve_path("ik_configs/smplx_to_t1.json"),
@@ -63,7 +63,7 @@ def _resolve_path(relative_path):
     "bvh_lafan1": {
         "unitree_g1": _resolve_path("ik_configs/bvh_lafan1_to_g1.json"),
         "unitree_g1_with_hands": _resolve_path("ik_configs/bvh_lafan1_to_g1.json"),
-        "unitree_g1_avp_o6": _resolve_path("ik_configs/bvh_lafan1_to_g1.json"),
+        "unitree_g1_neck_o6": _resolve_path("ik_configs/bvh_lafan1_to_g1.json"),
         "booster_t1_29dof": _resolve_path("ik_configs/bvh_lafan1_to_t1_29dof.json"),
         "fourier_n1": _resolve_path("ik_configs/bvh_lafan1_to_n1.json"),
         "stanford_toddy": _resolve_path("ik_configs/bvh_lafan1_to_toddy.json"),
@@ -83,7 +83,7 @@ def _resolve_path(relative_path):
     "fbx": {
         "unitree_g1": _resolve_path("ik_configs/fbx_to_g1.json"),
         "unitree_g1_with_hands": _resolve_path("ik_configs/fbx_to_g1.json"),
-        "unitree_g1_avp_o6": _resolve_path("ik_configs/fbx_to_g1.json"),
+        "unitree_g1_neck_o6": _resolve_path("ik_configs/fbx_to_g1.json"),
     },
     "fbx_offline": {
         "unitree_g1": _resolve_path("ik_configs/fbx_offline_to_g1.json"),
@@ -97,7 +97,7 @@ def _resolve_path(relative_path):
 ROBOT_BASE_DICT = {
     "unitree_g1": "pelvis",
     "unitree_g1_with_hands": "pelvis",
-    "unitree_g1_avp_o6": "pelvis",
+    "unitree_g1_neck_o6": "pelvis",
     "unitree_h1": "pelvis",
     "unitree_h1_2": "pelvis",
     "booster_t1": "Waist",
@@ -119,7 +119,7 @@ def _resolve_path(relative_path):
 VIEWER_CAM_DISTANCE_DICT = {
     "unitree_g1": 2.0,
     "unitree_g1_with_hands": 2.0,
-    "unitree_g1_avp_o6": 2.0,
+    "unitree_g1_neck_o6": 2.0,
     "unitree_h1": 3.0,
     "unitree_h1_2": 3.0,
     "booster_t1": 2.0,
diff --git a/teleopit/runtime/assets.py b/teleopit/runtime/assets.py
index 00d2e8e7..66041dbb 100644
--- a/teleopit/runtime/assets.py
+++ b/teleopit/runtime/assets.py
@@ -8,7 +8,7 @@
 GMR_ASSETS_ROOT = PROJECT_ROOT / "teleopit" / "retargeting" / "gmr" / "assets"
 UNITREE_G1_XML = ROBOT_ASSETS_ROOT / "unitree_g1" / "g1_29dof.xml"
 UNITREE_G1_DEX3_XML = ROBOT_ASSETS_ROOT / "unitree_g1" / "g1_29dof_dex3.xml"
-UNITREE_G1_AVP_O6_XML = ROBOT_ASSETS_ROOT / "unitree_g1" / "g1_29dof_avp_o6.xml"
+UNITREE_G1_NECK_O6_XML = ROBOT_ASSETS_ROOT / "unitree_g1" / "g1_29dof_neck_o6.xml"
 UNITREE_G1_MJLAB_XML = UNITREE_G1_XML
 
 
diff --git a/teleopit/runtime/console.py b/teleopit/runtime/console.py
index 2b6d7b12..07b2d894 100644
--- a/teleopit/runtime/console.py
+++ b/teleopit/runtime/console.py
@@ -200,6 +200,7 @@ def _highlight_text(self, text: str) -> str:
             "MOCAP": GREEN + BOLD,
             "STANDING": GREEN + BOLD,
             "ARMS": MAGENTA + BOLD,
+            "POLICY": CYAN + BOLD,
         }
         for word, code in replacements.items():
             highlighted = highlighted.replace(word, f"{code}{word}{RESET}")
@@ -256,6 +257,7 @@ def sim2real_operator_controls(cfg: Any) -> tuple[KeyboardControl, ...]:
     if provider == "pico4":
         controls.extend(
             [
+                KeyboardControl("Remote B", "pause/resume"),
                 KeyboardControl("Pico/Controller A", "pause/resume"),
                 KeyboardControl("Pico/Controller B", "arms"),
             ]
@@ -269,3 +271,13 @@ def sim2real_operator_controls(cfg: Any) -> tuple[KeyboardControl, ...]:
         )
     controls.extend(sim2real_keyboard_controls(cfg))
     return tuple(controls)
+
+
+def high_level_policy_operator_controls() -> tuple[KeyboardControl, ...]:
+    return (
+        KeyboardControl("Remote Start", "standing"),
+        KeyboardControl("Remote Y", "policy takeover"),
+        KeyboardControl("Remote B", "pause/resume"),
+        KeyboardControl("Remote X", "standing"),
+        KeyboardControl("Remote L1+R1", "damping / estop"),
+    )
diff --git a/teleopit/runtime/external_assets.py b/teleopit/runtime/external_assets.py
index 1f63c929..52c97f4d 100644
--- a/teleopit/runtime/external_assets.py
+++ b/teleopit/runtime/external_assets.py
@@ -21,8 +21,18 @@ class AssetEntry:
 
 ASSET_GROUPS: dict[str, list[AssetEntry]] = {
     "ckpt": [
-        AssetEntry("checkpoints/track.onnx", "track.onnx", repo="model"),
-        AssetEntry("checkpoints/track.pt", "track.pt", repo="model"),
+        AssetEntry("checkpoints/track_g1.onnx", "ckpt/track_g1.onnx", repo="model"),
+        AssetEntry("checkpoints/track_g1.pt", "ckpt/track_g1.pt", repo="model"),
+        AssetEntry(
+            "checkpoints/track_g1_neck_o6.onnx",
+            "ckpt/track_g1_neck_o6.onnx",
+            repo="model",
+        ),
+        AssetEntry(
+            "checkpoints/track_g1_neck_o6.pt",
+            "ckpt/track_g1_neck_o6.pt",
+            repo="model",
+        ),
     ],
     "gmr": [
         AssetEntry(
diff --git a/teleopit/sim/session.py b/teleopit/sim/session.py
index c404200c..7f740235 100644
--- a/teleopit/sim/session.py
+++ b/teleopit/sim/session.py
@@ -252,6 +252,10 @@ def enter_mocap_mode(self) -> bool:
             return False
         state = loop.robot.get_state()
         start_qpos = loop._resolve_hold_qpos(None, None, None, state)
+        # STANDING does not run retargeting, so the live subject may have
+        # changed pose or heading discontinuously since the previous MOCAP
+        # session. Cold-start GMR from the current live root on the next frame.
+        self._retargeter.reset()
         self.reset_policy_reference_state()
         self._step_runner.last_retarget_qpos = start_qpos.copy()
         self.last_commanded_motion_qpos = start_qpos.copy()
diff --git a/teleopit/sim2real/__init__.py b/teleopit/sim2real/__init__.py
index 2c22fb9c..bb7a6c39 100644
--- a/teleopit/sim2real/__init__.py
+++ b/teleopit/sim2real/__init__.py
@@ -1,5 +1,6 @@
 __all__ = [
     "Sim2RealRuntime",
+    "HighLevelPolicySim2RealRuntime",
     "UnitreeG1Robot",
     "UnitreeRemote",
     "Button",
@@ -11,6 +12,10 @@ def __getattr__(name: str):
         from teleopit.sim2real.mp import Sim2RealRuntime
 
         return Sim2RealRuntime
+    if name == "HighLevelPolicySim2RealRuntime":
+        from teleopit.sim2real.mp import HighLevelPolicySim2RealRuntime
+
+        return HighLevelPolicySim2RealRuntime
     if name == "UnitreeG1Robot":
         from teleopit.sim2real.unitree_g1 import UnitreeG1Robot
 
diff --git a/teleopit/sim2real/hands/base.py b/teleopit/sim2real/hands/base.py
index 76257789..b8506444 100644
--- a/teleopit/sim2real/hands/base.py
+++ b/teleopit/sim2real/hands/base.py
@@ -18,6 +18,8 @@ class HandPoseCommand:
 class HandDevice(Protocol):
     def connect(self) -> None: ...
 
+    def get_state(self, side: str) -> tuple[float, ...]: ...
+
     def send_pose(self, side: str, pose: Sequence[int], *, force: bool = False, reason: str = "") -> None: ...
 
     def open_all(self, *, force: bool = False, reason: str = "") -> None: ...
diff --git a/teleopit/sim2real/hands/linkerhand_l6.py b/teleopit/sim2real/hands/linkerhand_l6.py
index d7ed00be..44f5624c 100644
--- a/teleopit/sim2real/hands/linkerhand_l6.py
+++ b/teleopit/sim2real/hands/linkerhand_l6.py
@@ -89,7 +89,9 @@ def parse_linkerhand_l6_config(cfg: Any) -> LinkerHandL6Config:
         close_pose=tuple(close_pose),
         fixed_thumb_yaw=thumb_yaw,
         print_input=bool(cfg_get(l6_cfg, "print_input", False)),
-        somehand_config_path=str(cfg_get(somehand_cfg, "config_path", DEFAULT_SOMEHAND_CONFIG)),
+        somehand_config_path=str(
+            cfg_get(somehand_cfg, "l6_config_path", cfg_get(somehand_cfg, "config_path", DEFAULT_SOMEHAND_CONFIG))
+        ),
         somehand_rate_hz=_positive_float(cfg_get(somehand_cfg, "rate_hz", cfg_get(somehand_cfg, "rate", 60.0)), "somehand.rate_hz"),
         somehand_max_iterations=_optional_positive_int(cfg_get(somehand_cfg, "max_iterations", None), "somehand.max_iterations"),
         somehand_temporal_filter_alpha=_optional_alpha(cfg_get(somehand_cfg, "temporal_filter_alpha", None), "somehand.temporal_filter_alpha"),
@@ -128,6 +130,17 @@ def connect(self) -> None:
             raise
         self.open_all(force=True, reason="startup")
 
+    def get_state(self, side: str) -> tuple[float, ...]:
+        if side not in self.config.sides:
+            raise ValueError(f"LinkerHand L6 side is not configured: {side!r}")
+        hand = self._hands.get(side)
+        if hand is None:
+            raise RuntimeError(f"LinkerHand L6 {side} is not connected")
+        state = tuple(float(value) for value in hand.get_state())
+        if len(state) != 6:
+            raise RuntimeError(f"LinkerHand L6 {side} state must contain 6 values, got {len(state)}")
+        return state
+
     def send_pose(self, side: str, pose: Sequence[int], *, force: bool = False, reason: str = "") -> None:
         del reason
         next_pose = tuple(_uint8(value, f"{side}.pose") for value in pose)
@@ -204,33 +217,51 @@ def close(self) -> None:
         pass
 
 
-class SomehandL6Mapper(HandInputMapper):
-    def __init__(self, config: LinkerHandL6Config):
+class SomehandRetargetMapper(HandInputMapper):
+    def __init__(
+        self,
+        config: Any,
+        *,
+        family: str,
+        joint_order: Sequence[str],
+        config_path: str,
+        config_label: str,
+    ):
         self.config = config
-        self._engine: Any | None = None
+        self.family = family.upper()
+        self.joint_order = tuple(joint_order)
+        self.config_path = config_path
+        self.config_label = config_label
+        self._engine: dict[str, Any] = {}
         self._hand_frame_cls: Any | None = None
-        self._bihand_frame_cls: Any | None = None
-        self._mappers: dict[str, L6RetargetPoseMapper] = {}
+        self._mappers: dict[str, RetargetPoseMapper] = {}
         self._next_tick_s = 0.0
         self._active = False
 
     def start(self) -> None:
-        _require_somehand_020()
+        _require_somehand_030()
         from somehand.api import HandFrame, RetargetingEngine, load_bihand_config, load_retargeting_config
 
-        config_path = _resolve_project_path(self.config.somehand_config_path)
+        config_path = _resolve_project_path(self.config_path)
         if not config_path.exists():
-            raise FileNotFoundError(f"somehand L6 config not found: {config_path}")
+            raise FileNotFoundError(f"{self.config_label} not found: {config_path}")
         bihand_config = load_bihand_config(str(config_path))
         self._engine = {}
-        for side, path in (("left", bihand_config.left_config_path), ("right", bihand_config.right_config_path)):
+        self._mappers = {}
+        config_paths = {"left": bihand_config.left_config_path, "right": bihand_config.right_config_path}
+        for side in self.config.sides:
+            path = config_paths[side]
             retarget_cfg = load_retargeting_config(path)
             self._apply_low_latency_overrides(retarget_cfg)
             self._engine[side] = RetargetingEngine(retarget_cfg)
         self._hand_frame_cls = HandFrame
         for side, engine in self._engine.items():
-            if side in self.config.sides:
-                self._mappers[side] = L6RetargetPoseMapper(getattr(engine, "hand_model", None), side=side)
+            self._mappers[side] = RetargetPoseMapper(
+                getattr(engine, "hand_model", None),
+                side=side,
+                family=self.family,
+                joint_order=self.joint_order,
+            )
 
     def map(self, *, controller_snapshot: object | None, hand_snapshot: object | None, active: bool, now_s: float) -> tuple[HandPoseCommand, ...]:
         del controller_snapshot
@@ -277,21 +308,42 @@ def _apply_low_latency_overrides(self, cfg: object) -> None:
             cfg.preprocess.temporal_filter_alpha = float(self.config.somehand_temporal_filter_alpha)
 
 
+class SomehandL6Mapper(SomehandRetargetMapper):
+    def __init__(self, config: LinkerHandL6Config):
+        super().__init__(
+            config,
+            family="L6",
+            joint_order=L6_SDK_JOINT_ORDER,
+            config_path=config.somehand_config_path,
+            config_label="somehand L6 config",
+        )
+
+
 class L6RetargetPoseMapper:
     def __init__(self, hand_model: Any | None, *, side: str):
+        self._delegate = RetargetPoseMapper(hand_model, side=side, family="L6", joint_order=L6_SDK_JOINT_ORDER)
+
+    def qpos_to_pose(self, qpos: object) -> list[int]:
+        return self._delegate.qpos_to_pose(qpos)
+
+
+class RetargetPoseMapper:
+    def __init__(self, hand_model: Any | None, *, side: str, family: str, joint_order: Sequence[str]):
         if hand_model is None:
-            raise ValueError("somehand L6 hand model is missing")
+            raise ValueError(f"somehand {family} hand model is missing")
         get_index = getattr(hand_model, "get_joint_name_to_qpos_index", None)
         if not callable(get_index):
-            raise ValueError("somehand L6 hand model does not expose get_joint_name_to_qpos_index()")
+            raise ValueError(f"somehand {family} hand model does not expose get_joint_name_to_qpos_index()")
         joint_index = get_index()
-        self._indices = np.asarray([_resolve_l6_joint_index(joint_index, name, side=side) for name in L6_SDK_JOINT_ORDER], dtype=np.int64)
+        self.family = family.upper()
+        self._indices = np.asarray([_resolve_joint_index(joint_index, name, side=side, family=self.family) for name in joint_order], dtype=np.int64)
         mapping = _load_linkerhand_mapping_module()
         side_key = "l" if side == "left" else "r"
         self._mapping = mapping
-        self._arc_min = np.asarray(getattr(mapping, f"l6_{side_key}_min"), dtype=np.float64)
-        self._arc_max = np.asarray(getattr(mapping, f"l6_{side_key}_max"), dtype=np.float64)
-        self._direction = np.asarray(getattr(mapping, f"l6_{side_key}_derict"), dtype=np.int8)
+        mapping_prefix = self.family.lower()
+        self._arc_min = np.asarray(getattr(mapping, f"{mapping_prefix}_{side_key}_min"), dtype=np.float64)
+        self._arc_max = np.asarray(getattr(mapping, f"{mapping_prefix}_{side_key}_max"), dtype=np.float64)
+        self._direction = np.asarray(getattr(mapping, f"{mapping_prefix}_{side_key}_derict"), dtype=np.int8)
 
     def qpos_to_pose(self, qpos: object) -> list[int]:
         values = np.asarray(qpos, dtype=np.float64).reshape(-1)
@@ -303,7 +355,7 @@ def qpos_to_pose(self, qpos: object) -> list[int]:
                 scaled = self._mapping.scale_value(arc, float(self._arc_min[index]), float(self._arc_max[index]), 255.0, 0.0)
             else:
                 scaled = self._mapping.scale_value(arc, float(self._arc_min[index]), float(self._arc_max[index]), 0.0, 255.0)
-            pose.append(_uint8(round(float(scaled)), "somehand.pose"))
+            pose.append(_retarget_uint8(round(float(scaled)), "somehand.pose"))
         return pose
 
 
@@ -332,36 +384,52 @@ def trigger_to_pose(
     return pose
 
 
-def _require_somehand_020() -> None:
+def _require_somehand_030() -> None:
     try:
         installed = version("somehand")
     except PackageNotFoundError as exc:
-        raise ImportError("somehand==0.2.0 is required for hands.mode=vr_hand_pose") from exc
-    if installed != "0.2.0":
-        raise ImportError(f"somehand==0.2.0 is required for hands.mode=vr_hand_pose, found {installed}")
+        raise ImportError("somehand==0.3.0 is required for hands.mode=vr_hand_pose") from exc
+    if installed != "0.3.0":
+        raise ImportError(f"somehand==0.3.0 is required for hands.mode=vr_hand_pose, found {installed}")
 
 
 def _resolve_l6_joint_index(joint_index: dict[str, int], semantic_name: str, *, side: str) -> int:
-    for candidate in _l6_joint_candidates(semantic_name, side=side):
+    return _resolve_joint_index(joint_index, semantic_name, side=side, family="L6")
+
+
+def _resolve_joint_index(joint_index: dict[str, int], semantic_name: str, *, side: str, family: str) -> int:
+    for candidate in _joint_candidates(semantic_name, side=side, family=family):
         if candidate in joint_index:
             return int(joint_index[candidate])
-    suffixes = tuple(f"_{alias}" for alias in _l6_aliases(semantic_name))
+    aliases = _joint_aliases(semantic_name, family=family)
+    suffixes = tuple(f"_{alias}" for alias in aliases)
     for name, index in joint_index.items():
-        if name in _l6_aliases(semantic_name) or any(name.endswith(suffix) for suffix in suffixes):
+        if name in aliases or any(name.endswith(suffix) for suffix in suffixes):
             return int(index)
-    raise ValueError(f"Cannot resolve LinkerHand L6 SDK joint {semantic_name!r} in somehand hand model")
+    raise ValueError(f"Cannot resolve LinkerHand {family} SDK joint {semantic_name!r} in somehand hand model")
 
 
 def _l6_joint_candidates(semantic_name: str, *, side: str) -> tuple[str, ...]:
+    return _joint_candidates(semantic_name, side=side, family="L6")
+
+
+def _joint_candidates(semantic_name: str, *, side: str, family: str) -> tuple[str, ...]:
     prefixes = ("", f"{side}_", f"{side[0]}_", f"{side[0].upper()}_", f"{'lh' if side == 'left' else 'rh'}_")
-    return tuple(f"{prefix}{alias}" for alias in _l6_aliases(semantic_name) for prefix in prefixes)
+    return tuple(f"{prefix}{alias}" for alias in _joint_aliases(semantic_name, family=family) for prefix in prefixes)
 
 
 def _l6_aliases(semantic_name: str) -> tuple[str, ...]:
+    return _joint_aliases(semantic_name, family="L6")
+
+
+def _joint_aliases(semantic_name: str, *, family: str) -> tuple[str, ...]:
+    del family
     if semantic_name == "thumb_cmc_pitch":
         return ("thumb_cmc_pitch", "thumb_pitch")
     if semantic_name == "thumb_cmc_roll":
         return ("thumb_cmc_roll", "thumb_roll")
+    if semantic_name == "thumb_cmc_yaw":
+        return ("thumb_cmc_yaw", "thumb_yaw")
     aliases = [semantic_name]
     if semantic_name.endswith("_mcp_pitch"):
         finger = semantic_name[: -len("_mcp_pitch")]
@@ -407,6 +475,13 @@ def _uint8(value: object, field_name: str) -> int:
     return parsed
 
 
+def _retarget_uint8(value: object, field_name: str) -> int:
+    parsed = int(value)
+    if parsed < 0 or parsed > 255:
+        raise ValueError(f"{field_name} must be in 0-255, got {value!r}")
+    return parsed
+
+
 def _pose_values(value: object, field_name: str) -> list[int]:
     parsed = [_uint8(item, field_name) for item in value]  # type: ignore[union-attr]
     if len(parsed) != 6:
diff --git a/teleopit/sim2real/hands/linkerhand_o6.py b/teleopit/sim2real/hands/linkerhand_o6.py
index 6d4de349..7f593113 100644
--- a/teleopit/sim2real/hands/linkerhand_o6.py
+++ b/teleopit/sim2real/hands/linkerhand_o6.py
@@ -6,13 +6,28 @@
 
 from teleopit.runtime.common import cfg_get
 from teleopit.sim2real.hands.base import HAND_SIDES, HandDevice, HandInputMapper
-from teleopit.sim2real.hands.linkerhand_l6 import GripperMapper
+from teleopit.sim2real.hands.linkerhand_l6 import (
+    GripperMapper,
+    SomehandRetargetMapper,
+    _optional_alpha,
+    _optional_positive_int,
+)
 
 logger = logging.getLogger(__name__)
 
+DEFAULT_SOMEHAND_CONFIG = "third_party/somehand/configs/retargeting/bihand/linkerhand_o6_bihand.yaml"
 OPEN_POSE = (250, 250, 250, 250, 250, 250)
 CLOSE_POSE = (86, 73, 118, 111, 110, 111)
 DEFAULT_SPEED = (255, 255, 255, 255, 255, 255)
+VR_HAND_POSE_SPEED = (255, 255, 255, 255, 255, 255)
+O6_SDK_JOINT_ORDER = (
+    "thumb_cmc_pitch",
+    "thumb_cmc_yaw",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+)
 
 
 @dataclass(frozen=True)
@@ -31,17 +46,28 @@ class LinkerHandO6Config:
     close_pose: tuple[int, ...]
     fixed_thumb_yaw: int | None
     print_input: bool
+    somehand_config_path: str
+    somehand_rate_hz: float
+    somehand_max_iterations: int | None
+    somehand_temporal_filter_alpha: float | None
+    somehand_output_alpha: float | None
 
 
 def parse_linkerhand_o6_config(cfg: Any) -> LinkerHandO6Config:
     hands_cfg = cfg_get(cfg, "hands", {}) or {}
     o6_cfg = cfg_get(hands_cfg, "linkerhand_o6", {}) or {}
+    somehand_cfg = cfg_get(hands_cfg, "somehand", {}) or {}
     mode = str(cfg_get(hands_cfg, "mode", "gripper")).strip().lower()
-    if mode != "gripper":
-        raise ValueError(f"hands.driver=linkerhand_o6 supports only hands.mode=gripper, got {mode!r}")
+    if mode not in ("gripper", "vr_hand_pose"):
+        raise ValueError(f"hands.mode must be gripper or vr_hand_pose, got {mode!r}")
     sides = tuple(str(side).strip().lower() for side in cfg_get(hands_cfg, "sides", HAND_SIDES))
     if not sides or any(side not in HAND_SIDES for side in sides):
         raise ValueError("hands.sides must contain left, right, or both sides")
+    speed = (
+        VR_HAND_POSE_SPEED
+        if mode == "vr_hand_pose"
+        else tuple(_pose_values(cfg_get(o6_cfg, "speed", DEFAULT_SPEED), "speed"))
+    )
     return LinkerHandO6Config(
         mode=mode,
         sides=sides,
@@ -52,11 +78,25 @@ def parse_linkerhand_o6_config(cfg: Any) -> LinkerHandO6Config:
         frame_timeout_s=_positive_float(cfg_get(hands_cfg, "frame_timeout_s", 0.3), "frame_timeout_s"),
         trigger_deadzone=_deadzone(cfg_get(o6_cfg, "trigger_deadzone", 0.05)),
         deadman_threshold=_threshold(cfg_get(o6_cfg, "deadman_threshold", 0.5)),
-        speed=tuple(_pose_values(cfg_get(o6_cfg, "speed", DEFAULT_SPEED), "speed")),
+        speed=tuple(speed),
         open_pose=tuple(_pose_values(cfg_get(o6_cfg, "open_pose", OPEN_POSE), "open_pose")),
         close_pose=tuple(_pose_values(cfg_get(o6_cfg, "close_pose", CLOSE_POSE), "close_pose")),
         fixed_thumb_yaw=None,
         print_input=bool(cfg_get(o6_cfg, "print_input", False)),
+        somehand_config_path=str(cfg_get(somehand_cfg, "o6_config_path", DEFAULT_SOMEHAND_CONFIG)),
+        somehand_rate_hz=_positive_float(
+            cfg_get(somehand_cfg, "rate_hz", cfg_get(somehand_cfg, "rate", 60.0)),
+            "somehand.rate_hz",
+        ),
+        somehand_max_iterations=_optional_positive_int(
+            cfg_get(somehand_cfg, "max_iterations", None),
+            "somehand.max_iterations",
+        ),
+        somehand_temporal_filter_alpha=_optional_alpha(
+            cfg_get(somehand_cfg, "temporal_filter_alpha", None),
+            "somehand.temporal_filter_alpha",
+        ),
+        somehand_output_alpha=_optional_alpha(cfg_get(somehand_cfg, "output_alpha", None), "somehand.output_alpha"),
     )
 
 
@@ -91,6 +131,17 @@ def connect(self) -> None:
             raise
         self.open_all(force=True, reason="startup")
 
+    def get_state(self, side: str) -> tuple[float, ...]:
+        if side not in self.config.sides:
+            raise ValueError(f"LinkerHand O6 side is not configured: {side!r}")
+        hand = self._hands.get(side)
+        if hand is None:
+            raise RuntimeError(f"LinkerHand O6 {side} is not connected")
+        state = tuple(float(value) for value in hand.get_state())
+        if len(state) != 6:
+            raise RuntimeError(f"LinkerHand O6 {side} state must contain 6 values, got {len(state)}")
+        return state
+
     def send_pose(self, side: str, pose: Sequence[int], *, force: bool = False, reason: str = "") -> None:
         del reason
         next_pose = tuple(_uint8(value, f"{side}.pose") for value in pose)
@@ -125,7 +176,19 @@ def close(self) -> None:
 
 def build_linkerhand_o6(cfg: Any) -> tuple[HandDevice, HandInputMapper]:
     config = parse_linkerhand_o6_config(cfg)
-    return LinkerHandO6Device(config), GripperMapper(config)
+    mapper: HandInputMapper = SomehandO6Mapper(config) if config.mode == "vr_hand_pose" else GripperMapper(config)
+    return LinkerHandO6Device(config), mapper
+
+
+class SomehandO6Mapper(SomehandRetargetMapper):
+    def __init__(self, config: LinkerHandO6Config):
+        super().__init__(
+            config,
+            family="O6",
+            joint_order=O6_SDK_JOINT_ORDER,
+            config_path=config.somehand_config_path,
+            config_label="somehand O6 config",
+        )
 
 
 def _uint8(value: object, field_name: str) -> int:
diff --git a/teleopit/sim2real/hands/worker.py b/teleopit/sim2real/hands/worker.py
index e9fe323a..27d9e01f 100644
--- a/teleopit/sim2real/hands/worker.py
+++ b/teleopit/sim2real/hands/worker.py
@@ -37,6 +37,9 @@ def start(self) -> tuple[HandPoseCommand, ...]:
             finally:
                 raise
 
+    def get_state(self, side: str) -> tuple[float, ...]:
+        return self._device.get_state(side)
+
     def tick(
         self,
         *,
@@ -90,6 +93,10 @@ class DisabledHandRuntime:
     def start(self) -> tuple[HandPoseCommand, ...]:
         return ()
 
+    def get_state(self, side: str) -> tuple[float, ...]:
+        del side
+        raise RuntimeError("Dexterous hand control is disabled")
+
     def tick(
         self,
         *,
diff --git a/teleopit/sim2real/mp/__init__.py b/teleopit/sim2real/mp/__init__.py
index f32f55cd..3f55ff45 100644
--- a/teleopit/sim2real/mp/__init__.py
+++ b/teleopit/sim2real/mp/__init__.py
@@ -3,7 +3,9 @@
 from teleopit.sim2real.mp.runtime import (
     Sim2RealRuntime,
 )
+from teleopit.sim2real.mp.high_level_policy_runtime import HighLevelPolicySim2RealRuntime
 
 __all__ = [
     "Sim2RealRuntime",
+    "HighLevelPolicySim2RealRuntime",
 ]
diff --git a/teleopit/sim2real/mp/high_level_policy_runtime.py b/teleopit/sim2real/mp/high_level_policy_runtime.py
new file mode 100644
index 00000000..5788107b
--- /dev/null
+++ b/teleopit/sim2real/mp/high_level_policy_runtime.py
@@ -0,0 +1,638 @@
+"""Independent high-level-policy sim2real process assembly.
+
+This runtime deliberately does not start PicoBridge, GMR, or a reference
+worker. It owns one RealSense stream and sends host-policy body references
+through Teleopit's existing motion tracker.
+"""
+
+from __future__ import annotations
+
+import logging
+import multiprocessing as mp
+from multiprocessing.synchronize import Event as MpEvent
+import time
+from typing import Any, Callable
+
+import numpy as np
+
+from teleopit.high_level_policy.config import (
+    parse_high_level_policy_camera_config,
+    parse_high_level_policy_config,
+    parse_high_level_policy_safety_config,
+)
+from teleopit.high_level_policy.hand_calibration import HandCalibration
+from teleopit.high_level_policy.scheduler import closure_to_o6_pose
+from teleopit.runtime.common import cfg_get
+from teleopit.runtime.console import OPERATOR_LOGGER_NAME, PlainConsole
+from teleopit.sim2real.hands.linkerhand_o6 import (
+    LinkerHandO6Device,
+    parse_linkerhand_o6_config,
+)
+from teleopit.sim2real.mp.high_level_policy_worker import HighLevelPolicyWorker
+from teleopit.sim2real.mp.ipc import (
+    COMMAND_TOPIC,
+    HAND_COMMAND_TOPIC,
+    HIGH_LEVEL_POLICY_TARGET_TOPIC,
+    MODE_TOPIC,
+    NECK_COMMAND_TOPIC,
+    VIDEO_TOPIC,
+    LatestSubscriber,
+    Sim2RealIpcEndpoints,
+    ZmqPublisher,
+    default_endpoints,
+)
+from teleopit.sim2real.mp.messages import (
+    CommandPacket,
+    HandCommandPacket,
+    HighLevelPolicyTargetPacket,
+    ModeStatePacket,
+    NeckCommandPacket,
+)
+from teleopit.sim2real.mp.runtime import (
+    HIGH_LEVEL_POLICY_FAULT_COMMAND,
+    _mp_cfg,
+    _plain_cfg,
+    _run_robot_control_worker,
+    _worker_loop,
+)
+from teleopit.sim2real.mp.shm import SharedFrameRingWriter
+from teleopit.sim2real.neck.config import parse_neck_config
+from teleopit.sim2real.neck.openneck import build_neck_device
+
+
+logger = logging.getLogger(__name__)
+operator_logger = logging.getLogger(OPERATOR_LOGGER_NAME)
+
+
+class HighLevelPolicySim2RealRuntime:
+    def __init__(self, cfg: Any, *, console: PlainConsole | None = None) -> None:
+        self.cfg = _plain_cfg(cfg)
+        _validate_high_level_policy_runtime_config(self.cfg)
+        runtime_cfg = _mp_cfg(self.cfg)
+        self._ctx = mp.get_context(str(cfg_get(runtime_cfg, "start_method", "spawn")))
+        self._stop_event = self._ctx.Event()
+        self._processes: list[mp.Process] = []
+        self._shutdown_timeout_s = float(cfg_get(runtime_cfg, "shutdown_timeout_s", 3.0))
+        self._endpoints = default_endpoints(
+            host=str(cfg_get(runtime_cfg, "host", "127.0.0.1")),
+            base_port=int(cfg_get(runtime_cfg, "base_port", 39700)),
+        )
+        self._command_pub: ZmqPublisher | None = None
+        self._console = console or PlainConsole(title="Teleopit high-level policy", enabled=False)
+
+    def run(self) -> None:
+        operator_logger.info("high-level policy runtime starting")
+        try:
+            self._start_processes()
+            self._command_pub = ZmqPublisher(self._endpoints.command_pub)
+            reported_dead: set[str] = set()
+            while not self._stop_event.is_set():
+                time.sleep(0.2)
+                critical_dead = [
+                    process.name
+                    for process in self._processes
+                    if process.name == "robot_control"
+                    and not process.is_alive()
+                    and process.exitcode not in (None, 0)
+                ]
+                if critical_dead:
+                    operator_logger.error("critical worker exited: %s", ", ".join(critical_dead))
+                    self._stop_event.set()
+                    break
+                required_input_dead = [
+                    process.name
+                    for process in self._processes
+                    if process.name in {"camera", "high_level_policy"}
+                    and not process.is_alive()
+                    and process.exitcode is not None
+                ]
+                if required_input_dead and self._command_pub is not None:
+                    detail = (
+                        "required high-level-policy input worker exited: "
+                        + ", ".join(required_input_dead)
+                    )
+                    self._command_pub.publish(
+                        COMMAND_TOPIC,
+                        CommandPacket(
+                            command=HIGH_LEVEL_POLICY_FAULT_COMMAND,
+                            timestamp_s=time.monotonic(),
+                            payload={"detail": detail},
+                        ),
+                    )
+                noncritical_dead = [
+                    process.name
+                    for process in self._processes
+                    if process.name != "robot_control"
+                    and not process.is_alive()
+                    and process.exitcode is not None
+                    and process.name not in reported_dead
+                ]
+                if noncritical_dead:
+                    operator_logger.warning(
+                        "non-critical worker exited: %s; G1 remains under local control",
+                        ", ".join(noncritical_dead),
+                    )
+                    reported_dead.update(noncritical_dead)
+        except KeyboardInterrupt:
+            operator_logger.info("keyboard interrupt -> shutting down")
+            self._stop_event.set()
+        finally:
+            self.shutdown()
+
+    def shutdown(self) -> None:
+        self._stop_event.set()
+        if self._command_pub is not None:
+            self._command_pub.publish(
+                COMMAND_TOPIC,
+                CommandPacket(command="shutdown", timestamp_s=time.monotonic()),
+            )
+        for process in self._processes:
+            process.join(timeout=self._shutdown_timeout_s)
+        for process in self._processes:
+            if process.is_alive():
+                operator_logger.warning("terminating worker %s", process.name)
+                process.terminate()
+                process.join(timeout=1.0)
+        self._processes.clear()
+        if self._command_pub is not None:
+            self._command_pub.close()
+            self._command_pub = None
+
+    def _start_processes(self) -> None:
+        if self._processes:
+            return
+        specs: list[tuple[str, Callable[..., None]]] = [
+            ("camera", _run_high_level_policy_camera_worker),
+            ("high_level_policy", _run_high_level_policy_client_worker),
+            ("robot_control", _run_robot_control_worker),
+        ]
+        hands_cfg = cfg_get(self.cfg, "hands", {}) or {}
+        if bool(cfg_get(hands_cfg, "enabled", False)):
+            specs.append(("policy_hand", _run_high_level_policy_hand_worker))
+        neck_cfg = parse_neck_config(self.cfg)
+        if neck_cfg.enabled:
+            specs.append(("policy_neck", _run_high_level_policy_neck_worker))
+        for name, target in specs:
+            process = self._ctx.Process(
+                name=name,
+                target=target,
+                args=(self.cfg, self._endpoints, self._stop_event),
+            )
+            process.start()
+            self._processes.append(process)
+
+
+def _validate_high_level_policy_runtime_config(cfg: dict[str, Any]) -> None:
+    input_cfg = cfg_get(cfg, "input", {}) or {}
+    if str(cfg_get(input_cfg, "provider", "")).strip().lower() != "high_level_policy":
+        raise ValueError(
+            "HighLevelPolicySim2RealRuntime requires input.provider=high_level_policy"
+        )
+    policy_cfg = cfg_get(cfg, "high_level_policy", {}) or {}
+    if not bool(cfg_get(policy_cfg, "enabled", False)):
+        raise ValueError("HighLevelPolicySim2RealRuntime requires high_level_policy.enabled=true")
+    parse_high_level_policy_config(cfg)
+    parse_high_level_policy_camera_config(cfg)
+    parse_high_level_policy_safety_config(cfg)
+    reference_steps = tuple(int(value) for value in cfg_get(cfg, "reference_steps", [0]))
+    if reference_steps != (0,):
+        raise ValueError("High-level policy sim2real requires reference_steps=[0]")
+    recording_cfg = cfg_get(cfg, "recording", {}) or {}
+    if bool(cfg_get(recording_cfg, "enabled", False)):
+        raise ValueError("High-level policy recording is not supported in the initial runtime")
+
+    calibration = HandCalibration.load()
+    hands_cfg = cfg_get(cfg, "hands", {}) or {}
+    if not bool(cfg_get(hands_cfg, "enabled", False)):
+        raise ValueError("High-level policy action[36:48] requires hands.enabled=true")
+    if str(cfg_get(hands_cfg, "driver", "")).strip().lower() != "linkerhand_o6":
+        raise ValueError("High-level policy requires hands.driver=linkerhand_o6")
+    hand_config = parse_linkerhand_o6_config(cfg)
+    if len(hand_config.sides) != 2 or set(hand_config.sides) != {"left", "right"}:
+        raise ValueError("High-level policy requires hands.sides=[left, right]")
+    if tuple(float(value) for value in hand_config.open_pose) != calibration.open_raw:
+        raise ValueError("LinkerHand O6 open_pose does not match hand_calibration.json")
+    if tuple(float(value) for value in hand_config.close_pose) != calibration.close_raw:
+        raise ValueError("LinkerHand O6 close_pose does not match hand_calibration.json")
+
+    neck_cfg = parse_neck_config(cfg)
+    if not neck_cfg.enabled or neck_cfg.driver != "openneck":
+        raise ValueError("High-level policy action[48:50] requires neck.enabled=true and driver=openneck")
+    if neck_cfg.dry_run:
+        raise ValueError("High-level policy neck_state requires neck.dry_run=false")
+
+
+def _run_high_level_policy_client_worker(
+    cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent
+) -> None:
+    def _main() -> None:
+        HighLevelPolicyWorker(cfg, endpoints, stop_event).run()
+
+    _worker_loop("high_level_policy", cfg, _main)
+
+
+def _stop_and_hardware_reset_realsense(pipeline: Any, device: Any) -> None:
+    try:
+        pipeline.stop()
+    except RuntimeError as exc:
+        logger.warning("Failed to stop stalled high-level policy RealSense pipeline: %s", exc)
+    try:
+        device.hardware_reset()
+    except RuntimeError as exc:
+        logger.warning("Failed to hardware-reset high-level policy RealSense: %s", exc)
+
+
+def _run_high_level_policy_camera_worker(
+    cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent
+) -> None:
+    def _main() -> None:
+        camera_cfg = parse_high_level_policy_camera_config(cfg)
+        runtime_cfg = _mp_cfg(cfg)
+        publisher = ZmqPublisher(endpoints.video_pub)
+        command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC)
+        writer = SharedFrameRingWriter(
+            shape=(camera_cfg.height, camera_cfg.width, 3),
+            dtype=np.uint8,
+            slots=int(cfg_get(runtime_cfg, "video_slots", 3)),
+        )
+        rs: Any | None = None
+        rs_context: Any | None = None
+        pipeline: Any | None = None
+        pipeline_profile: Any | None = None
+        device: Any | None = None
+        device_serial = camera_cfg.device
+        try:
+            if camera_cfg.source == "realsense":
+                try:
+                    import pyrealsense2 as rs
+                except ImportError as exc:
+                    raise RuntimeError(
+                        "RealSense high-level-policy camera requires pyrealsense2"
+                    ) from exc
+                rs_context = rs.context()
+            period_s = 1.0 / float(camera_cfg.fps)
+            test_frame_index = 0
+            camera_stalled = False
+            while not stop_event.is_set():
+                command = command_sub.recv_latest()
+                if isinstance(command, CommandPacket) and command.command == "shutdown":
+                    break
+                started_s = time.monotonic()
+                if camera_cfg.source != "realsense":
+                    frame = _test_pattern(
+                        camera_cfg.height,
+                        camera_cfg.width,
+                        test_frame_index,
+                    )
+                    test_frame_index += 1
+                else:
+                    assert rs is not None
+                    if pipeline is None:
+                        try:
+                            pipeline = rs.pipeline(rs_context)
+                            rs_config = rs.config()
+                            if device_serial is not None:
+                                rs_config.enable_device(device_serial)
+                            rs_config.enable_stream(
+                                rs.stream.color,
+                                camera_cfg.width,
+                                camera_cfg.height,
+                                rs.format.rgb8,
+                                camera_cfg.fps,
+                            )
+                            pipeline_profile = pipeline.start(rs_config)
+                            device = pipeline_profile.get_device()
+                            if device_serial is None:
+                                device_serial = device.get_info(rs.camera_info.serial_number)
+                        except Exception as exc:
+                            pipeline = None
+                            pipeline_profile = None
+                            device = None
+                            if not camera_stalled:
+                                operator_logger.warning(
+                                    "High-level policy RealSense unavailable; "
+                                    "retrying in 1.0s: %s",
+                                    exc,
+                                )
+                            camera_stalled = True
+                            stop_event.wait(1.0)
+                            continue
+                    try:
+                        frames = pipeline.wait_for_frames(timeout_ms=1000)
+                        color = frames.get_color_frame()
+                        if not color:
+                            raise RuntimeError("frameset contains no color frame")
+                    except Exception as exc:
+                        if not camera_stalled:
+                            operator_logger.warning(
+                                "High-level policy RealSense stalled; "
+                                "hardware-resetting and reconnecting: %s",
+                                exc,
+                            )
+                        camera_stalled = True
+                        frames = None
+                        color = None
+                        assert device is not None
+                        _stop_and_hardware_reset_realsense(pipeline, device)
+                        pipeline = None
+                        pipeline_profile = None
+                        device = None
+                        stop_event.wait(1.0)
+                        continue
+                    if camera_stalled:
+                        operator_logger.info(
+                            "High-level policy RealSense reconnected"
+                        )
+                    camera_stalled = False
+                    frame = np.ascontiguousarray(
+                        np.asanyarray(color.get_data()),
+                        dtype=np.uint8,
+                    )
+                timestamp_s = time.monotonic()
+                descriptor = writer.write(frame, timestamp_s=timestamp_s)
+                publisher.publish(VIDEO_TOPIC, descriptor)
+                if camera_cfg.source == "realsense":
+                    frame = color = frames = None
+                else:
+                    elapsed_s = time.monotonic() - started_s
+                    if elapsed_s < period_s:
+                        time.sleep(period_s - elapsed_s)
+        finally:
+            if pipeline is not None:
+                try:
+                    pipeline.stop()
+                except RuntimeError as exc:
+                    logger.warning(
+                        "Failed to stop high-level policy RealSense pipeline: %s",
+                        exc,
+                    )
+            writer.close(unlink=True)
+            command_sub.close()
+            publisher.close()
+
+    _worker_loop("camera", cfg, _main)
+
+
+def _test_pattern(height: int, width: int, frame_index: int) -> np.ndarray:
+    x = np.linspace(0, 255, width, dtype=np.uint8)
+    y = np.linspace(0, 255, height, dtype=np.uint8)[:, None]
+    frame = np.empty((height, width, 3), dtype=np.uint8)
+    frame[:, :, 0] = x[None, :]
+    frame[:, :, 1] = y
+    frame[:, :, 2] = np.uint8(frame_index % 256)
+    return frame
+
+
+def _policy_target_action(target: HighLevelPolicyTargetPacket) -> np.ndarray:
+    action = np.asarray(target.action, dtype=np.float32).reshape(-1)
+    if action.shape != (50,) or not np.all(np.isfinite(action)):
+        raise ValueError("High-level policy hardware worker received an invalid 50D target")
+    return action
+
+
+def _policy_target_is_current(
+    target: object,
+    mode: ModeStatePacket | None,
+    *,
+    last_target_seq: int,
+    max_age_s: float,
+    now_s: float | None = None,
+) -> bool:
+    if not isinstance(target, HighLevelPolicyTargetPacket):
+        return False
+    if mode is None or mode.mode != "policy" or mode.policy_paused:
+        return False
+    if mode.policy_session_id is None or target.session_id != mode.policy_session_id:
+        return False
+    if (
+        not isinstance(target.seq, int)
+        or isinstance(target.seq, bool)
+        or target.seq <= last_target_seq
+    ):
+        return False
+    current_s = time.monotonic() if now_s is None else float(now_s)
+    age_s = current_s - float(target.timestamp_s)
+    return bool(np.isfinite(age_s) and 0.0 <= age_s <= float(max_age_s))
+
+
+def _apply_policy_hand_target(
+    device: LinkerHandO6Device,
+    target: HighLevelPolicyTargetPacket,
+    calibration: HandCalibration,
+) -> tuple[np.ndarray, np.ndarray]:
+    action = _policy_target_action(target)
+    left_pose = closure_to_o6_pose(action[36:42], calibration)
+    right_pose = closure_to_o6_pose(action[42:48], calibration)
+    device.send_pose(
+        "left",
+        left_pose,
+        reason="policy",
+    )
+    device.send_pose(
+        "right",
+        right_pose,
+        reason="policy",
+    )
+    return (
+        np.asarray(left_pose, dtype=np.float32),
+        np.asarray(right_pose, dtype=np.float32),
+    )
+
+
+def _apply_policy_neck_target(
+    device: Any,
+    target: HighLevelPolicyTargetPacket,
+) -> tuple[float, float]:
+    action = _policy_target_action(target)
+    yaw_deg = float(action[48])
+    pitch_deg = float(action[49])
+    device.move_deg(yaw_deg, pitch_deg)
+    return yaw_deg, pitch_deg
+
+
+def _run_high_level_policy_hand_worker(
+    cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent
+) -> None:
+    def _main() -> None:
+        config = parse_linkerhand_o6_config(cfg)
+        device = LinkerHandO6Device(config)
+        calibration = HandCalibration.load()
+        target_sub = LatestSubscriber(
+            endpoints.high_level_policy_control_pub,
+            HIGH_LEVEL_POLICY_TARGET_TOPIC,
+        )
+        mode_sub = LatestSubscriber(endpoints.mode_pub, MODE_TOPIC)
+        command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC)
+        state_pub = ZmqPublisher(endpoints.hand_command_pub)
+        latest_mode: ModeStatePacket | None = None
+        last_target_seq = -1
+        was_in_policy = False
+        state_seq = 0
+        last_state_s = float("-inf")
+        state_period_s = 1.0 / 30.0
+        left_pose = np.asarray(config.open_pose, dtype=np.float32)
+        right_pose = np.asarray(config.open_pose, dtype=np.float32)
+        sleep_s = 1.0 / max(float(cfg_get(_mp_cfg(cfg), "hand_worker_hz", 120.0)), 1.0)
+
+        def _publish_state(*, active: bool, timestamp_s: float) -> None:
+            nonlocal state_seq
+            try:
+                left_state = np.asarray(device.get_state("left"), dtype=np.float32)
+                right_state = np.asarray(device.get_state("right"), dtype=np.float32)
+            except Exception as exc:
+                logger.warning("LinkerHand O6 policy state read failed: %s", exc)
+                left_state = right_state = None
+            state_seq += 1
+            state_pub.publish(
+                HAND_COMMAND_TOPIC,
+                HandCommandPacket(
+                    timestamp_s=float(timestamp_s),
+                    driver="linkerhand_o6",
+                    mode="policy",
+                    active=bool(active),
+                    left_pose=left_pose.copy(),
+                    right_pose=right_pose.copy(),
+                    seq=state_seq,
+                    left_state=(
+                        None if left_state is None else left_state.copy()
+                    ),
+                    right_state=(
+                        None if right_state is None else right_state.copy()
+                    ),
+                ),
+            )
+
+        try:
+            device.connect()
+            while not stop_event.is_set():
+                command = command_sub.recv_latest()
+                if isinstance(command, CommandPacket) and command.command == "shutdown":
+                    break
+                mode = mode_sub.recv_latest()
+                if isinstance(mode, ModeStatePacket):
+                    latest_mode = mode
+                in_policy = bool(latest_mode is not None and latest_mode.mode == "policy")
+                if was_in_policy and not in_policy:
+                    device.open_all(force=True, reason="policy-inactive")
+                    left_pose = np.asarray(config.open_pose, dtype=np.float32)
+                    right_pose = np.asarray(config.open_pose, dtype=np.float32)
+                was_in_policy = in_policy
+                target = target_sub.recv_latest()
+                if _policy_target_is_current(
+                    target,
+                    latest_mode,
+                    last_target_seq=last_target_seq,
+                    max_age_s=config.frame_timeout_s,
+                ):
+                    left_pose, right_pose = _apply_policy_hand_target(
+                        device,
+                        target,
+                        calibration,
+                    )
+                    last_target_seq = int(target.seq)
+                now_s = time.monotonic()
+                if now_s - last_state_s >= state_period_s:
+                    _publish_state(active=in_policy, timestamp_s=now_s)
+                    last_state_s = now_s
+                time.sleep(sleep_s)
+        finally:
+            try:
+                device.close()
+            finally:
+                target_sub.close()
+                mode_sub.close()
+                command_sub.close()
+                state_pub.close()
+
+    _worker_loop("policy_hand", cfg, _main)
+
+
+def _run_high_level_policy_neck_worker(
+    cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent
+) -> None:
+    def _main() -> None:
+        config = parse_neck_config(cfg)
+        device = build_neck_device(config)
+        target_sub = LatestSubscriber(
+            endpoints.high_level_policy_control_pub,
+            HIGH_LEVEL_POLICY_TARGET_TOPIC,
+        )
+        mode_sub = LatestSubscriber(endpoints.mode_pub, MODE_TOPIC)
+        command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC)
+        state_pub = ZmqPublisher(endpoints.neck_command_pub)
+        latest_mode: ModeStatePacket | None = None
+        last_target_seq = -1
+        was_in_policy = False
+        state_seq = 0
+        last_state_s = float("-inf")
+        state_period_s = 1.0 / 30.0
+        yaw_deg = 0.0
+        pitch_deg = 0.0
+        sleep_s = 1.0 / max(config.rate_hz, 1.0)
+
+        def _publish_state(*, active: bool, timestamp_s: float) -> None:
+            nonlocal state_seq
+            try:
+                state_yaw_deg, state_pitch_deg = device.read_deg()
+            except Exception as exc:
+                logger.warning("OpenNeck policy state read failed: %s", exc)
+                state_yaw_deg = state_pitch_deg = None
+            state_seq += 1
+            state_pub.publish(
+                NECK_COMMAND_TOPIC,
+                NeckCommandPacket(
+                    timestamp_s=float(timestamp_s),
+                    driver="openneck",
+                    active=bool(active),
+                    yaw_deg=float(yaw_deg),
+                    pitch_deg=float(pitch_deg),
+                    seq=state_seq,
+                    state_yaw_deg=state_yaw_deg,
+                    state_pitch_deg=state_pitch_deg,
+                ),
+            )
+
+        try:
+            device.connect()
+            if config.center_on_start:
+                device.center()
+            while not stop_event.is_set():
+                command = command_sub.recv_latest()
+                if isinstance(command, CommandPacket) and command.command == "shutdown":
+                    break
+                mode = mode_sub.recv_latest()
+                if isinstance(mode, ModeStatePacket):
+                    latest_mode = mode
+                in_policy = bool(latest_mode is not None and latest_mode.mode == "policy")
+                if was_in_policy and not in_policy:
+                    device.center()
+                    yaw_deg = 0.0
+                    pitch_deg = 0.0
+                was_in_policy = in_policy
+                target = target_sub.recv_latest()
+                if _policy_target_is_current(
+                    target,
+                    latest_mode,
+                    last_target_seq=last_target_seq,
+                    max_age_s=config.frame_timeout_s,
+                ):
+                    yaw_deg, pitch_deg = _apply_policy_neck_target(device, target)
+                    last_target_seq = int(target.seq)
+                now_s = time.monotonic()
+                if now_s - last_state_s >= state_period_s:
+                    _publish_state(active=in_policy, timestamp_s=now_s)
+                    last_state_s = now_s
+                time.sleep(sleep_s)
+        finally:
+            try:
+                device.center()
+                if config.release_on_shutdown:
+                    device.release_torque()
+            finally:
+                device.close()
+                target_sub.close()
+                mode_sub.close()
+                command_sub.close()
+                state_pub.close()
+
+    _worker_loop("policy_neck", cfg, _main)
diff --git a/teleopit/sim2real/mp/high_level_policy_worker.py b/teleopit/sim2real/mp/high_level_policy_worker.py
new file mode 100644
index 00000000..cdacf06c
--- /dev/null
+++ b/teleopit/sim2real/mp/high_level_policy_worker.py
@@ -0,0 +1,288 @@
+"""Isolated client worker for asynchronous receding-horizon policy inference."""
+
+from __future__ import annotations
+
+import logging
+import time
+from typing import Any, Callable
+
+import numpy as np
+
+from teleopit.high_level_policy.client import (
+    HighLevelPolicyClient,
+    PolicyTransportError,
+)
+from teleopit.high_level_policy.config import parse_high_level_policy_config
+from teleopit.high_level_policy.protocol import PolicyProtocolError
+from teleopit.sim2real.mp.ipc import (
+    COMMAND_TOPIC,
+    HIGH_LEVEL_POLICY_ACTION_TOPIC,
+    HIGH_LEVEL_POLICY_OBSERVATION_TOPIC,
+    HIGH_LEVEL_POLICY_SESSION_TOPIC,
+    HIGH_LEVEL_POLICY_STATUS_TOPIC,
+    LatestSubscriber,
+    Sim2RealIpcEndpoints,
+    ZmqPublisher,
+)
+from teleopit.sim2real.mp.messages import (
+    CommandPacket,
+    HighLevelPolicyActionPacket,
+    HighLevelPolicyObservationPacket,
+    HighLevelPolicySessionPacket,
+    HighLevelPolicyStatusPacket,
+)
+from teleopit.sim2real.mp.shm import SharedFrameRingReader
+
+
+logger = logging.getLogger(__name__)
+
+
+def encode_policy_jpeg(frame: object, *, quality: int) -> bytes:
+    try:
+        import cv2
+    except ImportError as exc:
+        raise RuntimeError(
+            "High-level policy image encoding requires OpenCV; install teleopit[sim2real]"
+        ) from exc
+    rgb = np.asarray(frame)
+    if rgb.shape != (480, 640, 3) or rgb.dtype != np.uint8:
+        raise ValueError(
+            f"High-level policy camera frame must be uint8[480,640,3], got {rgb.dtype}{rgb.shape}"
+        )
+    bgr = cv2.cvtColor(np.ascontiguousarray(rgb), cv2.COLOR_RGB2BGR)
+    ok, encoded = cv2.imencode(".jpg", bgr, [cv2.IMWRITE_JPEG_QUALITY, int(quality)])
+    if not ok:
+        raise RuntimeError("OpenCV failed to encode the high-level policy JPEG")
+    payload = encoded.tobytes()
+    if not payload.startswith(b"\xff\xd8") or not payload.endswith(b"\xff\xd9"):
+        raise RuntimeError("OpenCV returned an invalid JPEG payload")
+    return payload
+
+
+class HighLevelPolicyWorker:
+    def __init__(
+        self,
+        cfg: dict[str, Any],
+        endpoints: Sim2RealIpcEndpoints,
+        stop_event: Any,
+        *,
+        client_factory: Callable[..., HighLevelPolicyClient] = HighLevelPolicyClient,
+        frame_reader: SharedFrameRingReader | None = None,
+    ) -> None:
+        self.cfg = cfg
+        self.endpoints = endpoints
+        self.stop_event = stop_event
+        self.policy_cfg = parse_high_level_policy_config(cfg)
+        self._client_factory = client_factory
+        self._frame_reader = frame_reader or SharedFrameRingReader()
+        self._session_sub = LatestSubscriber(
+            endpoints.high_level_policy_control_pub,
+            HIGH_LEVEL_POLICY_SESSION_TOPIC,
+        )
+        self._observation_sub = LatestSubscriber(
+            endpoints.high_level_policy_control_pub,
+            HIGH_LEVEL_POLICY_OBSERVATION_TOPIC,
+        )
+        self._command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC)
+        self._result_pub = ZmqPublisher(endpoints.high_level_policy_result_pub)
+        self._client: HighLevelPolicyClient | None = None
+        self._active_session: HighLevelPolicySessionPacket | None = None
+        self._ready = False
+        self._paused = False
+        self._last_session_seq = -1
+        self._last_observation_seq = -1
+        self._last_request_timestamp_ns: int | None = None
+        self._next_connect_time_s = 0.0
+        self._status_seq = 0
+        self._policy_type: str | None = None
+        self._policy_id: str | None = None
+        self._new_session_required = False
+
+    def run(self) -> None:
+        try:
+            while not self.stop_event.is_set():
+                command = self._command_sub.recv_latest()
+                if isinstance(command, CommandPacket) and command.command == "shutdown":
+                    break
+                session = self._session_sub.recv_latest()
+                if isinstance(session, HighLevelPolicySessionPacket):
+                    self._handle_session(session)
+                if self._active_session is not None and not self._ready:
+                    self._connect_if_due()
+                observation = self._observation_sub.recv_latest()
+                if isinstance(observation, HighLevelPolicyObservationPacket):
+                    self._handle_observation(observation)
+                time.sleep(0.001)
+        finally:
+            self.close()
+
+    def close(self) -> None:
+        client = self._client
+        self._client = None
+        if client is not None:
+            client.close()
+        self._frame_reader.close()
+        self._session_sub.close()
+        self._observation_sub.close()
+        self._command_sub.close()
+        self._result_pub.close()
+
+    def _handle_session(self, packet: HighLevelPolicySessionPacket) -> None:
+        if int(packet.seq) <= self._last_session_seq:
+            return
+        self._last_session_seq = int(packet.seq)
+        command = str(packet.command).strip().lower()
+        if command == "start":
+            if self._active_session is None or packet.session_id != self._active_session.session_id:
+                self._active_session = packet
+                self._ready = False
+                self._paused = False
+                self._last_observation_seq = -1
+                self._last_request_timestamp_ns = None
+                self._next_connect_time_s = 0.0
+                self._new_session_required = False
+                self._policy_type = None
+                self._policy_id = None
+                self._publish_status("connecting", "connecting to host policy")
+            return
+        if self._active_session is None or packet.session_id != self._active_session.session_id:
+            return
+        if command == "pause":
+            if not self._paused:
+                self._paused = True
+                self._publish_status("paused", "policy requests paused")
+        elif command == "resume":
+            if self._paused:
+                self._paused = False
+                self._last_request_timestamp_ns = None
+                if self._ready:
+                    self._publish_status("ready", "policy requests resumed")
+                else:
+                    self._new_session_required = False
+                    self._next_connect_time_s = 0.0
+                    self._policy_type = None
+                    self._policy_id = None
+                    self._publish_status(
+                        "connecting",
+                        "reconnecting the paused policy session",
+                    )
+        elif command == "stop":
+            self._publish_status("stopped", "policy session stopped")
+            self._active_session = None
+            self._ready = False
+            self._paused = False
+
+    def _connect_if_due(self) -> None:
+        session = self._active_session
+        now_s = time.monotonic()
+        if session is None or self._new_session_required or now_s < self._next_connect_time_s:
+            return
+        try:
+            if self._client is None:
+                self._client = self._client_factory(
+                    self.policy_cfg.endpoint,
+                    timeout_s=self.policy_cfg.timeout_s,
+                )
+            description = self._client.describe()
+            if self.policy_cfg.replan_steps > description.max_action_horizon:
+                raise ValueError(
+                    "high_level_policy.replan_steps exceeds host max_action_horizon: "
+                    f"{self.policy_cfg.replan_steps} > {description.max_action_horizon}"
+                )
+            self._client.reset(session.session_id, session.task)
+            self._policy_type = description.policy_type
+            self._policy_id = description.policy_id
+            self._ready = True
+            self._publish_status("ready", "host policy session reset")
+        except (PolicyProtocolError, PolicyTransportError, ValueError, RuntimeError) as exc:
+            self._ready = False
+            self._next_connect_time_s = now_s + self.policy_cfg.reconnect_backoff_s
+            self._publish_status("unavailable", str(exc))
+
+    def _handle_observation(self, packet: HighLevelPolicyObservationPacket) -> None:
+        session = self._active_session
+        if session is None or not self._ready or self._paused:
+            return
+        if packet.session_id != session.session_id or packet.sequence_id <= self._last_observation_seq:
+            return
+        now_s = time.monotonic()
+        if now_s - float(packet.timestamp_s) > self.policy_cfg.max_observation_age_s:
+            return
+        minimum_interval_ns = int(round(self.policy_cfg.replan_steps / 30.0 * 1e9))
+        if (
+            self._last_request_timestamp_ns is not None
+            and packet.onboard_monotonic_timestamp_ns - self._last_request_timestamp_ns
+            < minimum_interval_ns
+        ):
+            return
+        client = self._client
+        if client is None:
+            return
+        try:
+            frame = self._frame_reader.read(packet.frame, copy=True)
+            jpeg = encode_policy_jpeg(frame, quality=self.policy_cfg.jpeg_quality)
+            chunk = client.get_action(
+                session_id=session.session_id,
+                sequence_id=int(packet.sequence_id),
+                onboard_monotonic_timestamp_ns=int(packet.onboard_monotonic_timestamp_ns),
+                task=session.task,
+                jpeg_image=jpeg,
+                body_joint_positions=packet.body_joint_positions,
+                dex_state=packet.dex_state,
+                neck_state=packet.neck_state,
+                source_reference_root_pose=packet.source_reference_root_pose,
+            )
+            if self._policy_id is None or chunk.policy_id != self._policy_id:
+                raise PolicyProtocolError(
+                    "policy_mismatch",
+                    "get_action policy_id does not match the preceding describe response",
+                )
+            self._result_pub.publish(
+                HIGH_LEVEL_POLICY_ACTION_TOPIC,
+                HighLevelPolicyActionPacket(
+                    session_id=chunk.session_id,
+                    source_sequence_id=chunk.source_sequence_id,
+                    source_onboard_monotonic_timestamp_ns=chunk.source_onboard_monotonic_timestamp_ns,
+                    action_fps=chunk.action_fps,
+                    actions=np.asarray(chunk.actions, dtype=np.float32).copy(),
+                    policy_id=chunk.policy_id,
+                    server_inference_ms=chunk.server_inference_ms,
+                    received_timestamp_s=time.monotonic(),
+                ),
+            )
+            self._last_observation_seq = int(packet.sequence_id)
+            self._last_request_timestamp_ns = int(packet.onboard_monotonic_timestamp_ns)
+        except (PolicyProtocolError, PolicyTransportError, ValueError, RuntimeError) as exc:
+            logger.warning("High-level policy request failed: %s", exc)
+            self._ready = False
+            self._paused = True
+            self._new_session_required = True
+            self._publish_status(
+                "fault",
+                f"{exc}; POLICY paused and can be resumed with B after recovery",
+            )
+
+    def _publish_status(self, status: str, detail: str) -> None:
+        self._status_seq += 1
+        session_id = None if self._active_session is None else self._active_session.session_id
+        self._result_pub.publish(
+            HIGH_LEVEL_POLICY_STATUS_TOPIC,
+            HighLevelPolicyStatusPacket(
+                session_id=session_id,
+                status=str(status),
+                detail=str(detail),
+                timestamp_s=time.monotonic(),
+                seq=self._status_seq,
+                policy_type=self._policy_type,
+                policy_id=self._policy_id,
+            ),
+        )
+
+
+def run_high_level_policy_worker(
+    cfg: dict[str, Any],
+    endpoints: Sim2RealIpcEndpoints,
+    stop_event: Any,
+) -> None:
+    worker = HighLevelPolicyWorker(cfg, endpoints, stop_event)
+    worker.run()
diff --git a/teleopit/sim2real/mp/ipc.py b/teleopit/sim2real/mp/ipc.py
index e0c3b1c9..6d3c59b0 100644
--- a/teleopit/sim2real/mp/ipc.py
+++ b/teleopit/sim2real/mp/ipc.py
@@ -11,8 +11,10 @@
 
 
 BODY_TOPIC = "body"
+HEAD_POSE_TOPIC = "head_pose"
 HAND_TOPIC = "hand"
 HAND_COMMAND_TOPIC = "hand_command"
+NECK_COMMAND_TOPIC = "neck_command"
 CONTROLLER_TOPIC = "controller"
 CONTROL_EVENTS_TOPIC = "control_events"
 REFERENCE_TOPIC = "reference"
@@ -21,13 +23,20 @@
 RECORD_TOPIC = "record"
 HEALTH_TOPIC = "health"
 COMMAND_TOPIC = "command"
+HIGH_LEVEL_POLICY_SESSION_TOPIC = "high_level_policy_session"
+HIGH_LEVEL_POLICY_OBSERVATION_TOPIC = "high_level_policy_observation"
+HIGH_LEVEL_POLICY_ACTION_TOPIC = "high_level_policy_action"
+HIGH_LEVEL_POLICY_STATUS_TOPIC = "high_level_policy_status"
+HIGH_LEVEL_POLICY_TARGET_TOPIC = "high_level_policy_target"
 
 
 @dataclass(frozen=True)
 class Sim2RealIpcEndpoints:
     body_pub: str
+    head_pose_pub: str
     hand_pub: str
     hand_command_pub: str
+    neck_command_pub: str
     controller_pub: str
     control_events_pub: str
     reference_pub: str
@@ -37,6 +46,8 @@ class Sim2RealIpcEndpoints:
     health_pub: str
     command_pub: str
     reference_command_pub: str
+    high_level_policy_control_pub: str
+    high_level_policy_result_pub: str
 
 
 def default_endpoints(*, host: str = "127.0.0.1", base_port: int = 39700) -> Sim2RealIpcEndpoints:
@@ -44,8 +55,10 @@ def default_endpoints(*, host: str = "127.0.0.1", base_port: int = 39700) -> Sim
     prefix = f"tcp://{host}:"
     return Sim2RealIpcEndpoints(
         body_pub=f"{prefix}{base_port}",
+        head_pose_pub=f"{prefix}{base_port + 13}",
         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}",
@@ -55,6 +68,8 @@ def default_endpoints(*, host: str = "127.0.0.1", base_port: int = 39700) -> Sim
         health_pub=f"{prefix}{base_port + 9}",
         command_pub=f"{prefix}{base_port + 10}",
         reference_command_pub=f"{prefix}{base_port + 11}",
+        high_level_policy_control_pub=f"{prefix}{base_port + 14}",
+        high_level_policy_result_pub=f"{prefix}{base_port + 15}",
     )
 
 
diff --git a/teleopit/sim2real/mp/messages.py b/teleopit/sim2real/mp/messages.py
index a56bb1e7..41ee11ab 100644
--- a/teleopit/sim2real/mp/messages.py
+++ b/teleopit/sim2real/mp/messages.py
@@ -13,6 +13,7 @@
 
 
 Float64Array = NDArray[np.float64]
+Float32Array = NDArray[np.float32]
 
 
 @dataclass(frozen=True)
@@ -57,6 +58,8 @@ class ModeStatePacket:
     mocap_paused: bool
     timestamp_s: float
     seq: int
+    policy_paused: bool = False
+    policy_session_id: str | None = None
 
 
 @dataclass(frozen=True)
@@ -66,7 +69,7 @@ class RecordStepPacket:
     mocap_active: bool
     recordable: bool
     observation_state: Float64Array
-    observation_mode: Float64Array
+    observation_mode: int
     action_reference_qpos: Float64Array
     seq: int
 
@@ -80,6 +83,20 @@ class HandCommandPacket:
     left_pose: Float64Array
     right_pose: Float64Array
     seq: int
+    left_state: Float64Array | None = None
+    right_state: Float64Array | None = None
+
+
+@dataclass(frozen=True)
+class NeckCommandPacket:
+    timestamp_s: float
+    driver: str
+    active: bool
+    yaw_deg: float
+    pitch_deg: float
+    seq: int
+    state_yaw_deg: float | None = None
+    state_pitch_deg: float | None = None
 
 
 @dataclass(frozen=True)
@@ -106,3 +123,56 @@ class SharedFrameDescriptor:
     shape: tuple[int, ...]
     dtype: str
     slots: int
+
+
+@dataclass(frozen=True)
+class HighLevelPolicySessionPacket:
+    session_id: str
+    task: str
+    command: str
+    timestamp_s: float
+    seq: int
+
+
+@dataclass(frozen=True)
+class HighLevelPolicyObservationPacket:
+    session_id: str
+    sequence_id: int
+    onboard_monotonic_timestamp_ns: int
+    body_joint_positions: Float32Array
+    dex_state: Float32Array
+    neck_state: Float32Array
+    source_reference_root_pose: Float32Array
+    frame: SharedFrameDescriptor
+    timestamp_s: float
+
+
+@dataclass(frozen=True)
+class HighLevelPolicyActionPacket:
+    session_id: str
+    source_sequence_id: int
+    source_onboard_monotonic_timestamp_ns: int
+    action_fps: int
+    actions: Float32Array
+    policy_id: str
+    server_inference_ms: float
+    received_timestamp_s: float
+
+
+@dataclass(frozen=True)
+class HighLevelPolicyStatusPacket:
+    session_id: str | None
+    status: str
+    detail: str
+    timestamp_s: float
+    seq: int
+    policy_type: str | None = None
+    policy_id: str | None = None
+
+
+@dataclass(frozen=True)
+class HighLevelPolicyTargetPacket:
+    session_id: str
+    action: Float32Array
+    timestamp_s: float
+    seq: int
diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py
index f600a5bb..ead22db8 100644
--- a/teleopit/sim2real/mp/runtime.py
+++ b/teleopit/sim2real/mp/runtime.py
@@ -9,12 +9,19 @@
 from pathlib import Path
 import sys
 import time
+import uuid
 from typing import Any, Callable
 
 import numpy as np
 from numpy.typing import NDArray
 
 from teleopit.constants import FULL_QPOS_DIM, NUM_JOINTS, ROOT_DIM
+from teleopit.high_level_policy.client import PolicyActionChunk
+from teleopit.high_level_policy.config import (
+    parse_high_level_policy_config,
+    parse_high_level_policy_safety_config,
+)
+from teleopit.high_level_policy.scheduler import HighLevelPolicyScheduler, PolicyFrameTransform
 from teleopit.controllers.observation import VelCmdObservationBuilder, align_motion_qpos_yaw
 from teleopit.controllers.rl_policy import RLPolicyController
 from teleopit.inputs.bvh_provider import BVHInputProvider
@@ -43,11 +50,16 @@
 from teleopit.runtime.reference_config import parse_reference_config
 from teleopit.runtime.terminal_keyboard import TerminalKeyboardReader
 from teleopit.recording.hdf5 import (
+    DEFAULT_ROBOT_TYPE,
+    NO_HAND_TYPE,
+    NO_NECK_TYPE,
     build_mode_observation,
     build_mp4_video_config,
     build_observation_state,
-    normalize_hand_action,
+    build_recording_schema,
     normalize_action_reference_qpos,
+    normalize_hand_action,
+    build_neck_action,
 )
 from teleopit.sim.reference_motion import OfflineReferenceMotion
 from teleopit.sim.reference_timeline import ReferenceTimeline, ReferenceWindow, ReferenceWindowBuilder
@@ -62,15 +74,24 @@
 from teleopit.sim2real.hands.base import HandPoseCommand
 from teleopit.sim2real.hands.linkerhand_l6 import parse_linkerhand_l6_config
 from teleopit.sim2real.hands.linkerhand_o6 import parse_linkerhand_o6_config
+from teleopit.sim2real.neck.config import parse_neck_config
+from teleopit.sim2real.neck.worker import build_neck_runtime, head_pose_packet, mode_packet_active
 from teleopit.sim2real.mp.ipc import (
     BODY_TOPIC,
     COMMAND_TOPIC,
     CONTROL_EVENTS_TOPIC,
     CONTROLLER_TOPIC,
     HAND_COMMAND_TOPIC,
+    HEAD_POSE_TOPIC,
     HAND_TOPIC,
     HEALTH_TOPIC,
+    HIGH_LEVEL_POLICY_ACTION_TOPIC,
+    HIGH_LEVEL_POLICY_OBSERVATION_TOPIC,
+    HIGH_LEVEL_POLICY_SESSION_TOPIC,
+    HIGH_LEVEL_POLICY_STATUS_TOPIC,
+    HIGH_LEVEL_POLICY_TARGET_TOPIC,
     MODE_TOPIC,
+    NECK_COMMAND_TOPIC,
     RECORD_TOPIC,
     REFERENCE_TOPIC,
     VIDEO_TOPIC,
@@ -85,7 +106,13 @@
     ControlEventsPacket,
     HandCommandPacket,
     HealthPacket,
+    HighLevelPolicyActionPacket,
+    HighLevelPolicyObservationPacket,
+    HighLevelPolicySessionPacket,
+    HighLevelPolicyStatusPacket,
+    HighLevelPolicyTargetPacket,
     ModeStatePacket,
+    NeckCommandPacket,
     ReferencePacket,
     RecordStepPacket,
     SnapshotPacket,
@@ -111,6 +138,7 @@
 PROJECT_ROOT = Path(__file__).resolve().parents[3]
 ARM_MOCAP_REFERENCE_COMMAND = "arm_mocap_reference"
 DISARM_MOCAP_REFERENCE_COMMAND = "disarm_mocap_reference"
+HIGH_LEVEL_POLICY_FAULT_COMMAND = "high_level_policy_fault"
 
 
 class RobotMode(Enum):
@@ -118,6 +146,7 @@ class RobotMode(Enum):
     STANDING = "standing"
     MOCAP = "mocap"
     ARMS = "arms"
+    POLICY = "policy"
     DAMPING = "damping"
 
 
@@ -269,6 +298,11 @@ def _input_provider_kind(cfg: Any) -> str:
     return str(cfg_get(cfg_get(cfg, "input", {}) or {}, "provider", "bvh")).strip().lower()
 
 
+def _high_level_policy_enabled(cfg: Any) -> bool:
+    policy_cfg = cfg_get(cfg, "high_level_policy", {}) or {}
+    return bool(cfg_get(policy_cfg, "enabled", False))
+
+
 def _recording_cfg(cfg: Any) -> Any:
     return cfg_get(cfg, "recording", {}) or {}
 
@@ -281,6 +315,24 @@ def _recording_camera_cfg(cfg: Any) -> Any:
     return cfg_get(_recording_cfg(cfg), "camera", {}) or {}
 
 
+def _recording_hardware_types(cfg: Any) -> tuple[str, str, str]:
+    robot_cfg = cfg_get(cfg, "robot", {}) or {}
+    robot_type = str(cfg_get(robot_cfg, "type", DEFAULT_ROBOT_TYPE)).strip().lower()
+    hands_cfg = cfg_get(cfg, "hands", {}) or {}
+    hand_type = (
+        str(cfg_get(hands_cfg, "driver", "linkerhand_l6")).strip().lower()
+        if bool(cfg_get(hands_cfg, "enabled", False))
+        else NO_HAND_TYPE
+    )
+    neck_cfg = cfg_get(cfg, "neck", {}) or {}
+    neck_type = (
+        str(cfg_get(neck_cfg, "driver", "openneck")).strip().lower()
+        if bool(cfg_get(neck_cfg, "enabled", False))
+        else NO_NECK_TYPE
+    )
+    return robot_type, hand_type, neck_type
+
+
 def _configured_open_hand_pose(cfg: Any) -> tuple[np.ndarray, np.ndarray]:
     hands_cfg = cfg_get(cfg, "hands", {}) or {}
     driver = str(cfg_get(hands_cfg, "driver", "linkerhand_l6")).strip().lower()
@@ -308,15 +360,32 @@ def _validate_new_runtime_config(cfg: Any) -> None:
             "Legacy sim2real config keys are no longer supported: "
             f"{', '.join(legacy_keys)}. Use input.provider, runtime, and hands instead."
         )
+    if _high_level_policy_enabled(cfg):
+        raise ValueError(
+            "high_level_policy.enabled=true requires the independent "
+            "scripts/run/run_high_level_policy_sim2real.py entry point"
+        )
     provider = _input_provider_kind(cfg)
     if provider not in ("pico4", "bvh"):
         raise ValueError(f"sim2real input.provider must be pico4 or bvh, got {provider!r}")
     hands_cfg = cfg_get(cfg, "hands", {}) or {}
     if bool(cfg_get(hands_cfg, "enabled", False)) and provider != "pico4":
         raise ValueError("hands.enabled=true requires input.provider=pico4")
+    neck_cfg = parse_neck_config(cfg)
+    if neck_cfg.enabled:
+        if provider != "pico4":
+            raise ValueError("neck.enabled=true requires input.provider=pico4")
+        if neck_cfg.driver != "openneck":
+            raise ValueError(f"Unsupported neck.driver={neck_cfg.driver!r}; supported drivers: openneck")
     if _recording_enabled(cfg):
         if provider != "pico4":
             raise ValueError("recording.enabled=true requires input.provider=pico4")
+        if bool(cfg_get(hands_cfg, "enabled", False)):
+            hand_sides = {str(side).strip().lower() for side in cfg_get(hands_cfg, "sides", ("left", "right"))}
+            if hand_sides != {"left", "right"}:
+                raise ValueError("hand-state recording requires hands.sides=[left, right]")
+        if neck_cfg.enabled and neck_cfg.dry_run:
+            raise ValueError("neck-state recording requires neck.dry_run=false")
         rec_cfg = _recording_cfg(cfg)
         if str(cfg_get(rec_cfg, "format", "hdf5")) != "hdf5":
             raise ValueError("Only recording.format=hdf5 is supported")
@@ -330,6 +399,14 @@ def _validate_new_runtime_config(cfg: Any) -> None:
             raise ValueError("recording.camera.source must be realsense")
         if int(cfg_get(rec_cfg, "fps", 30)) != int(cfg_get(camera_cfg, "fps", 30)):
             raise ValueError("recording.fps must match recording.camera.fps")
+        robot_type, hand_type, neck_type = _recording_hardware_types(cfg)
+        build_recording_schema(
+            camera_cfg,
+            fps=int(cfg_get(rec_cfg, "fps", 30)),
+            robot_type=robot_type,
+            hand_type=hand_type,
+            neck_type=neck_type,
+        )
         input_video = parse_pico_video_config(cfg_get(cfg, "input", {}) or {})
         if not input_video.enabled:
             raise ValueError("recording.enabled=true requires input.video.enabled=true")
@@ -412,12 +489,11 @@ def run(self) -> None:
                 self._command_pub = ZmqPublisher(self._endpoints.command_pub)
                 self._keyboard = TerminalKeyboardReader()
                 operator_logger.info("keyboard recording controls active: R start, S save, D discard, Q shutdown, H help")
+            reported_noncritical_dead: set[str] = set()
             while not self._stop_event.is_set():
                 self._poll_terminal_recording_controls()
                 time.sleep(0.2)
                 critical_names = {"robot_control", "reference"}
-                if _input_provider_kind(self.cfg) == "pico4":
-                    critical_names.add("pico_input")
                 critical_dead = [
                     process.name
                     for process in self._processes
@@ -435,9 +511,14 @@ def run(self) -> None:
                     if not process.is_alive()
                     and process.exitcode not in (None, 0)
                     and process.name not in critical_names
+                    and process.name not in reported_noncritical_dead
                 ]
                 if noncritical_dead:
-                    operator_logger.warning("non-critical worker exited: %s", ", ".join(noncritical_dead))
+                    operator_logger.warning(
+                        "non-critical worker exited: %s; G1 control remains active",
+                        ", ".join(noncritical_dead),
+                    )
+                    reported_noncritical_dead.update(noncritical_dead)
         except KeyboardInterrupt:
             operator_logger.info("keyboard interrupt -> shutting down")
             self._stop_event.set()
@@ -479,6 +560,9 @@ def _start_processes(self) -> None:
         hands_cfg = cfg_get(self.cfg, "hands", {}) or {}
         if bool(cfg_get(hands_cfg, "enabled", False)):
             specs.append(("hand_worker", _run_hand_worker))
+        neck_cfg = parse_neck_config(self.cfg)
+        if neck_cfg.enabled:
+            specs.append(("neck_worker", _run_neck_worker))
         if _recording_enabled(self.cfg):
             specs.append(("recording_worker", _run_recording_worker))
         video_cfg = parse_pico_video_config(cfg_get(self.cfg, "input", {}))
@@ -567,6 +651,28 @@ def _main() -> None:
         )
 
         body_pub = ZmqPublisher(endpoints.body_pub)
+        head_pose_pub: ZmqPublisher | None = None
+
+        def _disable_head_pose_publisher(message: str) -> None:
+            nonlocal head_pose_pub
+            logger.exception(message)
+            failed_publisher = head_pose_pub
+            head_pose_pub = None
+            if failed_publisher is None:
+                return
+            try:
+                failed_publisher.close()
+            except Exception:
+                logger.exception("Failed to close disabled OpenNeck head-pose publisher")
+
+        if parse_neck_config(cfg).enabled:
+            try:
+                head_pose_pub = ZmqPublisher(endpoints.head_pose_pub)
+            except Exception:
+                _disable_head_pose_publisher(
+                    "OpenNeck head-pose IPC setup failed; neck control is disabled "
+                    "while pico_input continues"
+                )
         hand_pub = ZmqPublisher(endpoints.hand_pub)
         controller_pub = ZmqPublisher(endpoints.controller_pub)
         events_pub = ZmqPublisher(endpoints.control_events_pub)
@@ -591,27 +697,59 @@ def _publish_recording_frame(frame: NDArray[np.generic], timestamp_s: float) ->
         video_runtime = PicoVideoRuntime(
             provider=provider,
             config=video_cfg,
-            mode="sim2real",
             frame_callback=_publish_recording_frame if _recording_enabled(cfg) else None,
         )
 
         hz = float(cfg_get(_mp_cfg(cfg), "pico_input_hz", 120.0))
         sleep_s = 1.0 / max(hz, 1.0)
         last_body_seq = -1
+        last_head_pose_seq = -1
         last_hand_seq = -1
         last_controller_seq = -1
         last_video_seq = -1
         last_health_s = 0.0
         try:
-            video_runtime.start()
+            try:
+                video_runtime.start()
+            except Exception:
+                logger.exception(
+                    "Pico video startup failed; video is disabled while pico_input and robot control continue"
+                )
             while not stop_event.is_set():
-                video_runtime.tick()
+                try:
+                    video_runtime.tick()
+                except Exception:
+                    logger.exception(
+                        "Pico video runtime failed; video is disabled while pico_input and robot control continue"
+                    )
                 command = command_sub.recv_latest()
                 if isinstance(command, CommandPacket) and command.command == "shutdown":
                     stop_event.set()
                     break
 
                 now = time.monotonic()
+                if head_pose_pub is not None:
+                    try:
+                        head_pose_snapshot = provider.get_head_pose_snapshot()
+                        if (
+                            head_pose_snapshot is not None
+                            and int(head_pose_snapshot.seq) != last_head_pose_seq
+                        ):
+                            head_pose_pub.publish(
+                                HEAD_POSE_TOPIC,
+                                SnapshotPacket(
+                                    snapshot=head_pose_snapshot,
+                                    timestamp_s=float(head_pose_snapshot.timestamp_s),
+                                    seq=int(head_pose_snapshot.seq),
+                                ),
+                            )
+                            last_head_pose_seq = int(head_pose_snapshot.seq)
+                    except Exception:
+                        _disable_head_pose_publisher(
+                            "OpenNeck head-pose stream failed; neck control is disabled "
+                            "while pico_input continues"
+                        )
+
                 if callable(getattr(provider, "has_frame", None)) and provider.has_frame():
                     try:
                         frame, timestamp_s, seq = provider.get_frame_packet()
@@ -668,6 +806,7 @@ def _publish_recording_frame(frame: NDArray[np.generic], timestamp_s: float) ->
                             metrics={
                                 "body_seq": last_body_seq,
                                 "body_fps": float(provider.fps),
+                                "head_pose_seq": last_head_pose_seq,
                                 "hand_seq": last_hand_seq,
                                 "controller_seq": last_controller_seq,
                                 "video_seq": last_video_seq,
@@ -677,11 +816,26 @@ def _publish_recording_frame(frame: NDArray[np.generic], timestamp_s: float) ->
                     last_health_s = now
                 time.sleep(sleep_s)
         finally:
-            video_runtime.stop()
+            try:
+                video_runtime.stop()
+            except Exception:
+                logger.exception("Failed to stop Pico video runtime during pico_input cleanup")
             if frame_writer is not None:
                 frame_writer.close(unlink=True)
             command_sub.close()
-            for publisher in (body_pub, hand_pub, controller_pub, events_pub, health_pub, video_pub):
+            if head_pose_pub is not None:
+                try:
+                    head_pose_pub.close()
+                except Exception:
+                    logger.exception("Failed to close OpenNeck head-pose publisher")
+            for publisher in (
+                body_pub,
+                hand_pub,
+                controller_pub,
+                events_pub,
+                health_pub,
+                video_pub,
+            ):
                 if publisher is not None:
                     publisher.close()
             provider.close()
@@ -1050,6 +1204,7 @@ def __init__(
         self.endpoints = endpoints
         self.stop_event = stop_event
         self.provider_kind = _input_provider_kind(cfg)
+        self.high_level_policy_enabled = _high_level_policy_enabled(cfg)
         self.mode = RobotMode.IDLE
         self.policy_hz = float(cfg_get(cfg, "policy_hz", 50.0))
         self.dt = 1.0 / self.policy_hz
@@ -1079,6 +1234,8 @@ def __init__(
             policy_dt_s=self.dt,
             reference_steps=cfg_get(cfg, "reference_steps", [0]),
         )
+        if self.high_level_policy_enabled and self._reference_window_builder.reference_steps != (0,):
+            raise ValueError("High-level policy sim2real currently requires reference_steps=[0]")
         self._ref_proc = Sim2RealReferenceProcessor(
             obs_builder=self.obs_builder,
             policy=self.policy,
@@ -1102,6 +1259,41 @@ def __init__(
         self._mocap_reference_arm_retry_s = float(cfg_get(_mp_cfg(cfg), "mocap_reference_arm_retry_s", 0.1))
         self._mocap_session = MocapSessionManager()
 
+        self._high_level_policy_cfg = (
+            parse_high_level_policy_config(cfg) if self.high_level_policy_enabled else None
+        )
+        self._high_level_policy_safety_cfg = (
+            parse_high_level_policy_safety_config(cfg)
+            if self.high_level_policy_enabled
+            else None
+        )
+        self._high_level_policy_scheduler = (
+            HighLevelPolicyScheduler(
+                hold_s=self._high_level_policy_cfg.hold_s,
+                safety=self._high_level_policy_safety_cfg,
+                output_hz=self.policy_hz,
+            )
+            if self._high_level_policy_cfg is not None
+            else None
+        )
+        self._policy_entry_pending = False
+        self._policy_entry_deadline_s: float | None = None
+        self._policy_session_id: str | None = None
+        self._policy_frame_transform: PolicyFrameTransform | None = None
+        self._policy_paused = False
+        self._policy_resume_pending = False
+        self._policy_resume_deadline_s: float | None = None
+        self._policy_resume_source_timestamp_ns: int | None = None
+        self._policy_hold_qpos: Float64Array | None = None
+        self._policy_session_seq = 0
+        self._policy_observation_seq = 0
+        self._policy_target_seq = 0
+        self._last_policy_session_publish_s = 0.0
+        self._last_policy_video_seq = -1
+        self._latest_policy_video: SharedFrameDescriptor | None = None
+        self._latest_policy_status: HighLevelPolicyStatusPacket | None = None
+        self._last_policy_status_seq = -1
+
         self._latest_reference: ReferencePacket | None = None
         mp_cfg = _mp_cfg(cfg)
         self._max_reference_age_s = float(cfg_get(mp_cfg, "max_reference_age_s", 0.25))
@@ -1111,12 +1303,56 @@ def __init__(
         self._last_reference_seq = -1
         self._consecutive_valid_references = 0
 
-        self._reference_sub = LatestSubscriber(endpoints.reference_pub, REFERENCE_TOPIC)
-        self._events_sub = LatestSubscriber(endpoints.control_events_pub, CONTROL_EVENTS_TOPIC)
+        self._reference_sub = (
+            None
+            if self.high_level_policy_enabled
+            else LatestSubscriber(endpoints.reference_pub, REFERENCE_TOPIC)
+        )
+        self._events_sub = (
+            None
+            if self.high_level_policy_enabled
+            else LatestSubscriber(endpoints.control_events_pub, CONTROL_EVENTS_TOPIC)
+        )
         self._command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC)
-        self._reference_command_pub = ZmqPublisher(endpoints.reference_command_pub)
+        self._reference_command_pub = (
+            None
+            if self.high_level_policy_enabled
+            else ZmqPublisher(endpoints.reference_command_pub)
+        )
+        self._policy_video_sub = (
+            LatestSubscriber(endpoints.video_pub, VIDEO_TOPIC)
+            if self.high_level_policy_enabled
+            else None
+        )
+        self._policy_hand_state_sub = (
+            LatestSubscriber(endpoints.hand_command_pub, HAND_COMMAND_TOPIC)
+            if self.high_level_policy_enabled
+            else None
+        )
+        self._policy_neck_state_sub = (
+            LatestSubscriber(endpoints.neck_command_pub, NECK_COMMAND_TOPIC)
+            if self.high_level_policy_enabled
+            else None
+        )
+        self._policy_action_sub = (
+            LatestSubscriber(endpoints.high_level_policy_result_pub, HIGH_LEVEL_POLICY_ACTION_TOPIC)
+            if self.high_level_policy_enabled
+            else None
+        )
+        self._policy_status_sub = (
+            LatestSubscriber(endpoints.high_level_policy_result_pub, HIGH_LEVEL_POLICY_STATUS_TOPIC)
+            if self.high_level_policy_enabled
+            else None
+        )
+        self._policy_control_pub = (
+            ZmqPublisher(endpoints.high_level_policy_control_pub)
+            if self.high_level_policy_enabled
+            else None
+        )
         self._mode_pub = ZmqPublisher(endpoints.mode_pub)
         self._record_pub = ZmqPublisher(endpoints.record_pub) if _recording_enabled(cfg) else None
+        self._latest_policy_hand_state: HandCommandPacket | None = None
+        self._latest_policy_neck_state: NeckCommandPacket | None = None
 
         viewers = _parse_sim2real_viewers(cfg)
         self._retarget_viewer = _Sim2RealRetargetViewer(
@@ -1150,6 +1386,8 @@ def run(self) -> None:
                         self._standing_step()
                     elif self.mode in (RobotMode.MOCAP, RobotMode.ARMS):
                         self._mocap_step()
+                    elif self.mode == RobotMode.POLICY:
+                        self._high_level_policy_step()
 
                 self._publish_mode_state()
                 work_elapsed_s = time.monotonic() - t0
@@ -1158,13 +1396,15 @@ def run(self) -> None:
                     loop_start_s=t0,
                     work_elapsed_s=work_elapsed_s,
                     cycle_elapsed_s=cycle_elapsed_s,
-                    pico_age_s=self._reference_age_s(),
+                    pico_age_s=None if self.high_level_policy_enabled else self._reference_age_s(),
                 )
         finally:
             self.shutdown()
 
     def shutdown(self) -> None:
-        if self.mode in (RobotMode.STANDING, RobotMode.MOCAP, RobotMode.ARMS):
+        if self.high_level_policy_enabled and self._policy_session_id is not None:
+            self._stop_high_level_policy_session()
+        if self.mode in (RobotMode.STANDING, RobotMode.MOCAP, RobotMode.ARMS, RobotMode.POLICY):
             try:
                 self.robot.set_damping()
                 time.sleep(0.5)
@@ -1175,10 +1415,22 @@ def shutdown(self) -> None:
             except Exception:
                 logger.exception("Failed to exit debug mode during robot_control shutdown")
         self._retarget_viewer.shutdown()
-        self._reference_sub.close()
-        self._events_sub.close()
+        for subscriber in (
+            self._reference_sub,
+            self._events_sub,
+            self._policy_video_sub,
+            self._policy_hand_state_sub,
+            self._policy_neck_state_sub,
+            self._policy_action_sub,
+            self._policy_status_sub,
+        ):
+            if subscriber is not None:
+                subscriber.close()
         self._command_sub.close()
-        self._reference_command_pub.close()
+        if self._reference_command_pub is not None:
+            self._reference_command_pub.close()
+        if self._policy_control_pub is not None:
+            self._policy_control_pub.close()
         self._mode_pub.close()
         if self._record_pub is not None:
             self._record_pub.close()
@@ -1204,6 +1456,20 @@ def _drain_ipc(self) -> None:
         if isinstance(command, CommandPacket) and command.command == "shutdown":
             self.stop_event.set()
             return
+        if isinstance(command, CommandPacket) and command.command == HIGH_LEVEL_POLICY_FAULT_COMMAND:
+            detail = str(
+                command.payload.get(
+                    "detail",
+                    "required high-level-policy input worker exited",
+                )
+            )
+            self._handle_high_level_policy_fault(detail)
+            return
+        if bool(getattr(self, "high_level_policy_enabled", False)):
+            self._drain_high_level_policy_ipc()
+            return
+        if self._reference_sub is None or self._events_sub is None:
+            raise RuntimeError("Teleoperation robot worker is missing reference/event subscribers")
         reference = self._reference_sub.recv_latest()
         if isinstance(reference, ReferencePacket):
             self._note_reference_packet(reference)
@@ -1212,6 +1478,9 @@ def _drain_ipc(self) -> None:
             self._handle_mocap_control_events(events.events)
 
     def _handle_transitions(self) -> None:
+        if bool(getattr(self, "high_level_policy_enabled", False)):
+            self._handle_high_level_policy_transitions()
+            return
         if self.mode == RobotMode.IDLE:
             if self.remote.start.on_pressed:
                 operator_logger.info("Start -> STANDING")
@@ -1233,13 +1502,17 @@ def _handle_transitions(self) -> None:
                 self._send_reference_command("replay_mocap")
                 self._resume_paused_mocap_if_needed()
                 return
-            if self.remote.A.on_pressed:
+            pause_pressed = (
+                self.remote.B.on_pressed if self.provider_kind == "pico4" else self.remote.A.on_pressed
+            )
+            if pause_pressed:
+                button = "B" if self.provider_kind == "pico4" else "A"
                 if self._mocap_session.state == MocapSessionState.PAUSED:
-                    operator_logger.info("A -> resume playback")
+                    operator_logger.info("%s -> resume playback", button)
                     self._send_reference_command("resume_mocap")
                     self._resume_paused_mocap()
                 else:
-                    operator_logger.info("A -> pause playback")
+                    operator_logger.info("%s -> pause playback", button)
                     self._send_reference_command("pause_mocap")
                     self._pause_active_mocap()
                 return
@@ -1251,6 +1524,448 @@ def _handle_transitions(self) -> None:
                 operator_logger.info("Start -> STANDING")
                 self._enter_standing()
 
+    def _drain_high_level_policy_ipc(self) -> None:
+        if self._policy_video_sub is None or self._policy_action_sub is None or self._policy_status_sub is None:
+            raise RuntimeError("High-level policy robot worker is missing IPC subscribers")
+        video = self._policy_video_sub.recv_latest()
+        if isinstance(video, SharedFrameDescriptor) and int(video.seq) > self._last_policy_video_seq:
+            self._latest_policy_video = video
+        status = self._policy_status_sub.recv_latest()
+        if isinstance(status, HighLevelPolicyStatusPacket) and int(status.seq) > self._last_policy_status_seq:
+            self._latest_policy_status = status
+            self._last_policy_status_seq = int(status.seq)
+            if status.status in ("fault", "unavailable"):
+                logger.warning("High-level policy host status=%s: %s", status.status, status.detail)
+            current_session = status.session_id == self._policy_session_id
+            terminal_fault = status.status == "fault" or (
+                status.status == "unavailable" and self.mode == RobotMode.POLICY
+            )
+            if current_session and terminal_fault:
+                self._handle_high_level_policy_fault(status.detail)
+                return
+        packet = self._policy_action_sub.recv_latest()
+        if not isinstance(packet, HighLevelPolicyActionPacket):
+            return
+        if packet.session_id != self._policy_session_id:
+            logger.debug(
+                "Discarded high-level policy action for inactive session: active=%r received=%r",
+                self._policy_session_id,
+                packet.session_id,
+            )
+            return
+        # A request may already be in flight when the operator pauses. Drain
+        # its result without replacing the reference frozen at the B press.
+        if self._policy_paused and not self._policy_resume_pending:
+            return
+        scheduler = self._high_level_policy_scheduler
+        policy_cfg = self._high_level_policy_cfg
+        if scheduler is None or policy_cfg is None:
+            return
+        now_s = time.monotonic()
+        if self.mode == RobotMode.STANDING and self._policy_entry_pending:
+            deadline_s = self._policy_entry_deadline_s
+            if deadline_s is not None and now_s > deadline_s:
+                operator_logger.warning(
+                    "High-level policy entry timed out; remaining in STANDING"
+                )
+                self._enter_standing()
+                return
+        if self.mode == RobotMode.POLICY and self._policy_resume_pending:
+            deadline_s = self._policy_resume_deadline_s
+            if deadline_s is not None and now_s > deadline_s:
+                self._handle_high_level_policy_fault(
+                    "resume timed out waiting for a fresh action chunk"
+                )
+                return
+        result_age_s = now_s - float(packet.received_timestamp_s)
+        if (
+            not np.isfinite(result_age_s)
+            or result_age_s < 0.0
+            or result_age_s > policy_cfg.max_result_age_s
+        ):
+            logger.warning(
+                "Rejected stale high-level policy result: age=%.3fs limit=%.3fs",
+                result_age_s,
+                policy_cfg.max_result_age_s,
+            )
+            if self.mode == RobotMode.STANDING and self._policy_entry_pending:
+                operator_logger.warning(
+                    "High-level policy entry failed; received a stale action result"
+                )
+                self._enter_standing()
+            return
+        minimum_source_timestamp_ns = self._policy_resume_source_timestamp_ns
+        if (
+            self._policy_resume_pending
+            and minimum_source_timestamp_ns is not None
+            and int(packet.source_onboard_monotonic_timestamp_ns)
+            < minimum_source_timestamp_ns
+        ):
+            logger.warning("Discarded pre-resume high-level policy action chunk")
+            return
+        if self.mode == RobotMode.STANDING and not self._policy_entry_pending:
+            return
+        chunk = PolicyActionChunk(
+            session_id=packet.session_id,
+            source_sequence_id=int(packet.source_sequence_id),
+            source_onboard_monotonic_timestamp_ns=int(
+                packet.source_onboard_monotonic_timestamp_ns
+            ),
+            action_fps=int(packet.action_fps),
+            actions=np.asarray(packet.actions, dtype=np.float32),
+            policy_id=str(packet.policy_id),
+            server_inference_ms=float(packet.server_inference_ms),
+        )
+        if self.mode == RobotMode.STANDING:
+            try:
+                scheduler.accept(chunk, now_s=now_s)
+            except ValueError as exc:
+                logger.warning("Rejected high-level policy entry chunk: %s", exc)
+                operator_logger.warning(
+                    "High-level policy entry failed; remaining in STANDING"
+                )
+                self._enter_standing()
+                return
+            self._transition_to_high_level_policy()
+            return
+        try:
+            scheduler.accept(chunk, now_s=now_s)
+        except ValueError as exc:
+            logger.warning("Rejected high-level policy action chunk: %s", exc)
+            return
+        if self._policy_resume_pending:
+            scheduler.resume(now_s)
+            self._policy_paused = False
+            self._policy_resume_pending = False
+            self._policy_resume_deadline_s = None
+            self._policy_resume_source_timestamp_ns = None
+            operator_logger.info("fresh action chunk -> resume POLICY")
+
+    def _handle_high_level_policy_transitions(self) -> None:
+        if self.mode == RobotMode.IDLE:
+            if self.remote.start.on_pressed:
+                operator_logger.info("Start -> STANDING")
+                self._enter_standing()
+            return
+        if self.mode == RobotMode.STANDING:
+            if self.remote.X.on_pressed and self._policy_entry_pending:
+                operator_logger.info("X -> cancel high-level policy entry")
+                self._enter_standing()
+                return
+            if self.remote.Y.on_pressed and not self._policy_entry_pending:
+                operator_logger.info("Y -> request high-level policy")
+                self._begin_high_level_policy_entry()
+            if self._policy_entry_pending:
+                self._publish_high_level_policy_session(
+                    "start",
+                    repeat=True,
+                )
+                deadline_s = self._policy_entry_deadline_s
+                if deadline_s is not None and time.monotonic() > deadline_s:
+                    operator_logger.warning("High-level policy entry timed out; remaining in STANDING")
+                    self._enter_standing()
+            return
+        if self.mode == RobotMode.POLICY:
+            if self.remote.X.on_pressed:
+                operator_logger.info("X -> STANDING")
+                self._enter_standing()
+                return
+            if self.remote.B.on_pressed:
+                self._toggle_high_level_policy_pause()
+                return
+            self._publish_high_level_policy_session(
+                "resume"
+                if self._policy_resume_pending or not self._policy_paused
+                else "pause",
+                repeat=True,
+            )
+            return
+        if self.mode == RobotMode.DAMPING and self.remote.start.on_pressed:
+            operator_logger.info("Start -> STANDING")
+            self._enter_standing()
+
+    def _begin_high_level_policy_entry(self) -> None:
+        self._start_high_level_policy_entry_session()
+
+    def _build_high_level_policy_boundary_action(self, state: object) -> np.ndarray:
+        transform = self._policy_frame_transform
+        if transform is None:
+            raise RuntimeError("High-level policy entry is missing its frame transform")
+        initial_action = np.zeros(50, dtype=np.float32)
+        initial_action[:36] = transform.localize_body_action(
+            self._build_robot_state_qpos(state)
+        )
+        return initial_action
+
+    def _build_high_level_policy_reference_action(self, reference_qpos: object) -> np.ndarray:
+        transform = self._policy_frame_transform
+        if transform is None:
+            raise RuntimeError("High-level policy entry is missing its frame transform")
+        initial_reference = np.zeros(50, dtype=np.float32)
+        initial_reference[:36] = transform.localize_body_action(reference_qpos)
+        return initial_reference
+
+    def _start_high_level_policy_entry_session(self) -> None:
+        policy_cfg = self._high_level_policy_cfg
+        scheduler = self._high_level_policy_scheduler
+        if policy_cfg is None or scheduler is None:
+            raise RuntimeError("High-level policy runtime is not configured")
+        if self._policy_session_id is not None:
+            self._publish_high_level_policy_session("stop")
+        state = self.robot.get_state()
+        root_pos = self._resolve_base_pos(state)
+        self._policy_frame_transform = PolicyFrameTransform.from_robot_pose(
+            root_pos[:2],
+            getattr(state, "quat"),
+        )
+        active_reference = (
+            np.asarray(self._last_commanded_motion_qpos, dtype=np.float64).copy()
+            if self._last_commanded_motion_qpos is not None
+            else self._standing_qpos.copy()
+        )
+        session_started_s = time.monotonic()
+        self._policy_session_id = uuid.uuid4().hex
+        scheduler.reset(
+            self._policy_session_id,
+            initial_action=self._build_high_level_policy_boundary_action(state),
+            initial_reference=self._build_high_level_policy_reference_action(
+                active_reference
+            ),
+            initial_timestamp_s=session_started_s,
+        )
+        self._policy_entry_pending = True
+        self._policy_entry_deadline_s = session_started_s + policy_cfg.entry_timeout_s
+        self._policy_paused = False
+        self._policy_resume_pending = False
+        self._policy_resume_deadline_s = None
+        self._policy_resume_source_timestamp_ns = None
+        self._policy_hold_qpos = active_reference.copy()
+        self._policy_observation_seq = 0
+        self._last_policy_video_seq = (
+            -1
+            if self._latest_policy_video is None
+            else int(self._latest_policy_video.seq)
+        )
+        self._last_policy_session_publish_s = 0.0
+        self._publish_high_level_policy_session("start", repeat=False)
+
+    def _publish_high_level_policy_session(self, command: str, *, repeat: bool = False) -> None:
+        publisher = self._policy_control_pub
+        session_id = self._policy_session_id
+        policy_cfg = self._high_level_policy_cfg
+        if publisher is None or session_id is None or policy_cfg is None:
+            return
+        now_s = time.monotonic()
+        if repeat and now_s - self._last_policy_session_publish_s < 0.2:
+            return
+        self._policy_session_seq += 1
+        publisher.publish(
+            HIGH_LEVEL_POLICY_SESSION_TOPIC,
+            HighLevelPolicySessionPacket(
+                session_id=session_id,
+                task=policy_cfg.task,
+                command=str(command),
+                timestamp_s=now_s,
+                seq=self._policy_session_seq,
+            ),
+        )
+        self._last_policy_session_publish_s = now_s
+
+    def _publish_high_level_policy_observation(self, robot_state: object) -> None:
+        if not (self._policy_entry_pending or self.mode == RobotMode.POLICY):
+            return
+        if self._policy_paused and not self._policy_resume_pending:
+            return
+        publisher = self._policy_control_pub
+        frame = self._latest_policy_video
+        scheduler = self._high_level_policy_scheduler
+        session_id = self._policy_session_id
+        policy_cfg = self._high_level_policy_cfg
+        if (
+            publisher is None
+            or frame is None
+            or scheduler is None
+            or session_id is None
+            or policy_cfg is None
+        ):
+            return
+        if int(frame.seq) <= self._last_policy_video_seq:
+            return
+        now_s = time.monotonic()
+        if abs(now_s - float(frame.timestamp_s)) > policy_cfg.max_observation_age_s:
+            return
+        self._drain_high_level_policy_hardware_state()
+        hardware_state = self._high_level_policy_hardware_state(
+            now_s=now_s,
+            max_age_s=policy_cfg.max_observation_age_s,
+        )
+        if hardware_state is None:
+            return
+        dex_state, neck_state = hardware_state
+        source_reference_root_pose = scheduler.reference_root_pose_at(
+            float(frame.timestamp_s)
+        )
+        if source_reference_root_pose is None:
+            return
+        body_joint_positions = build_observation_state(robot_state)[:NUM_JOINTS]
+        sequence_id = self._policy_observation_seq
+        publisher.publish(
+            HIGH_LEVEL_POLICY_OBSERVATION_TOPIC,
+            HighLevelPolicyObservationPacket(
+                session_id=session_id,
+                sequence_id=sequence_id,
+                onboard_monotonic_timestamp_ns=int(round(float(frame.timestamp_s) * 1e9)),
+                body_joint_positions=body_joint_positions.astype(
+                    np.float32,
+                    copy=True,
+                ),
+                dex_state=dex_state,
+                neck_state=neck_state,
+                source_reference_root_pose=source_reference_root_pose,
+                frame=frame,
+                timestamp_s=now_s,
+            ),
+        )
+        self._policy_observation_seq += 1
+        self._last_policy_video_seq = int(frame.seq)
+
+    def _drain_high_level_policy_hardware_state(self) -> None:
+        hand_subscriber = self._policy_hand_state_sub
+        neck_subscriber = self._policy_neck_state_sub
+        if hand_subscriber is None or neck_subscriber is None:
+            return
+        hand_state = hand_subscriber.recv_latest()
+        if isinstance(hand_state, HandCommandPacket):
+            self._latest_policy_hand_state = hand_state
+        neck_state = neck_subscriber.recv_latest()
+        if isinstance(neck_state, NeckCommandPacket):
+            self._latest_policy_neck_state = neck_state
+
+    def _high_level_policy_hardware_state(
+        self,
+        *,
+        now_s: float,
+        max_age_s: float,
+    ) -> tuple[np.ndarray, np.ndarray] | None:
+        hand = self._latest_policy_hand_state
+        neck = self._latest_policy_neck_state
+        if (
+            hand is None
+            or hand.left_state is None
+            or hand.right_state is None
+            or neck is None
+            or neck.state_yaw_deg is None
+            or neck.state_pitch_deg is None
+        ):
+            return None
+        hand_age_s = float(now_s) - float(hand.timestamp_s)
+        neck_age_s = float(now_s) - float(neck.timestamp_s)
+        if not (
+            np.isfinite(hand_age_s)
+            and 0.0 <= hand_age_s <= float(max_age_s)
+            and np.isfinite(neck_age_s)
+            and 0.0 <= neck_age_s <= float(max_age_s)
+        ):
+            return None
+        left_state = np.asarray(hand.left_state, dtype=np.float32).reshape(-1)
+        right_state = np.asarray(hand.right_state, dtype=np.float32).reshape(-1)
+        dex_state = np.concatenate((left_state, right_state), dtype=np.float32)
+        neck_state = np.asarray(
+            [neck.state_yaw_deg, neck.state_pitch_deg],
+            dtype=np.float32,
+        )
+        if dex_state.shape != (12,) or not np.all(np.isfinite(dex_state)):
+            return None
+        if neck_state.shape != (2,) or not np.all(np.isfinite(neck_state)):
+            return None
+        return dex_state.copy(), neck_state.copy()
+
+    def _transition_to_high_level_policy(self) -> None:
+        state = self.robot.get_state()
+        resume_qpos = self._build_robot_state_qpos(state)
+        self._reset_policy_state()
+        self._last_retarget_qpos = None
+        self._last_commanded_motion_qpos = resume_qpos.copy()
+        self._policy_hold_qpos = resume_qpos.copy()
+        self._policy_entry_pending = False
+        self._policy_entry_deadline_s = None
+        self._policy_paused = False
+        self._policy_resume_pending = False
+        self._policy_resume_deadline_s = None
+        self._policy_resume_source_timestamp_ns = None
+        self.mode = RobotMode.POLICY
+        operator_logger.info("mode -> POLICY")
+
+    def _toggle_high_level_policy_pause(self) -> None:
+        scheduler = self._high_level_policy_scheduler
+        if scheduler is None:
+            return
+        now_s = time.monotonic()
+        if self._policy_paused:
+            if self._policy_resume_pending:
+                return
+            policy_cfg = self._high_level_policy_cfg
+            if policy_cfg is None:
+                return
+            self._policy_resume_pending = True
+            self._policy_resume_deadline_s = now_s + policy_cfg.entry_timeout_s
+            self._policy_resume_source_timestamp_ns = int(round(now_s * 1e9))
+            self._publish_high_level_policy_session("resume")
+            operator_logger.info("B -> resume POLICY; waiting for a fresh action chunk")
+        else:
+            scheduler.pause(now_s)
+            self._policy_paused = True
+            self._policy_resume_pending = False
+            self._policy_resume_deadline_s = None
+            self._policy_resume_source_timestamp_ns = None
+            self._policy_hold_qpos = self._resolve_mocap_hold_qpos()
+            self._publish_high_level_policy_session("pause")
+            operator_logger.info("B -> pause POLICY")
+
+    def _stop_high_level_policy_session(self) -> None:
+        if self._policy_session_id is not None:
+            self._publish_high_level_policy_session("stop")
+        scheduler = self._high_level_policy_scheduler
+        if scheduler is not None:
+            scheduler.clear()
+        self._policy_entry_pending = False
+        self._policy_entry_deadline_s = None
+        self._policy_session_id = None
+        self._policy_frame_transform = None
+        self._policy_paused = False
+        self._policy_resume_pending = False
+        self._policy_resume_deadline_s = None
+        self._policy_resume_source_timestamp_ns = None
+        self._policy_hold_qpos = None
+        self._latest_policy_status = None
+
+    def _handle_high_level_policy_fault(self, detail: str) -> None:
+        if not bool(getattr(self, "high_level_policy_enabled", False)):
+            return
+        if self.mode == RobotMode.POLICY:
+            if self._policy_paused and not self._policy_resume_pending:
+                return
+            scheduler = self._high_level_policy_scheduler
+            if scheduler is not None:
+                scheduler.pause(time.monotonic())
+            self._policy_paused = True
+            self._policy_resume_pending = False
+            self._policy_resume_deadline_s = None
+            self._policy_resume_source_timestamp_ns = None
+            self._policy_hold_qpos = self._resolve_mocap_hold_qpos()
+            self._publish_high_level_policy_session("pause")
+            operator_logger.warning(
+                "High-level policy fault -> pause POLICY: %s",
+                detail,
+            )
+            return
+        if self._policy_entry_pending:
+            operator_logger.warning(
+                "High-level policy entry failed; remaining in STANDING: %s",
+                detail,
+            )
+            self._enter_standing()
+
     def _standing_step(self) -> None:
         robot_state = self.robot.get_state()
         qpos = self._standing_qpos.copy()
@@ -1275,6 +1990,7 @@ def _standing_step(self) -> None:
         self._last_action = np.asarray(action, dtype=np.float32).reshape(-1)
         self._last_retarget_qpos = qpos.copy()
         self._last_commanded_motion_qpos = qpos.copy()
+        self._publish_high_level_policy_observation(robot_state)
         self._publish_record_step(robot_state=robot_state, reference_qpos=qpos)
         self._write_retarget_viewer(qpos)
 
@@ -1301,15 +2017,106 @@ def _mocap_step(self) -> None:
         robot_state = self.robot.get_state()
         self._execute_mocap_pipeline(reference.qpos, robot_state, reference.reference_window)
 
+    def _high_level_policy_step(self) -> None:
+        scheduler = self._high_level_policy_scheduler
+        transform = self._policy_frame_transform
+        session_id = self._policy_session_id
+        if self._policy_resume_pending:
+            robot_state = self.robot.get_state()
+            self._publish_high_level_policy_observation(robot_state)
+            deadline_s = self._policy_resume_deadline_s
+            if deadline_s is not None and time.monotonic() > deadline_s:
+                self._handle_high_level_policy_fault("resume timed out waiting for a fresh action chunk")
+            hold_qpos = self._policy_hold_qpos
+            if hold_qpos is None:
+                hold_qpos = self._resolve_mocap_hold_qpos()
+                self._policy_hold_qpos = hold_qpos.copy()
+            self._run_static_mocap_step(hold_qpos)
+            return
+        if self._policy_paused:
+            hold_qpos = self._policy_hold_qpos
+            if hold_qpos is None:
+                hold_qpos = self._resolve_mocap_hold_qpos()
+                self._policy_hold_qpos = hold_qpos.copy()
+            self._run_static_mocap_step(hold_qpos)
+            return
+        if scheduler is None or transform is None or session_id is None:
+            detail = "POLICY mode is missing its scheduler/session transform"
+            logger.error(detail)
+            self._handle_high_level_policy_fault(detail)
+            hold_qpos = self._policy_hold_qpos
+            if hold_qpos is None:
+                hold_qpos = self._resolve_mocap_hold_qpos()
+                self._policy_hold_qpos = hold_qpos.copy()
+            self._run_static_mocap_step(hold_qpos)
+            return
+
+        robot_state = self.robot.get_state()
+        self._publish_high_level_policy_observation(robot_state)
+        scheduled = scheduler.sample(time.monotonic())
+        if scheduled is None:
+            self._handle_high_level_policy_fault("action watchdog expired")
+            hold_qpos = self._policy_hold_qpos
+            if hold_qpos is None:
+                hold_qpos = self._resolve_mocap_hold_qpos()
+                self._policy_hold_qpos = hold_qpos.copy()
+            self._run_static_mocap_step(hold_qpos)
+            return
+        reference_qpos = transform.delocalize_body_action(scheduled[:36]).astype(np.float64)
+        self._execute_reference_pipeline(
+            reference_qpos,
+            robot_state,
+            reference_window=None,
+            align_reference=False,
+            compose_arms=False,
+        )
+        self._policy_hold_qpos = reference_qpos.copy()
+        publisher = self._policy_control_pub
+        if publisher is not None:
+            self._policy_target_seq += 1
+            publisher.publish(
+                HIGH_LEVEL_POLICY_TARGET_TOPIC,
+                HighLevelPolicyTargetPacket(
+                    session_id=session_id,
+                    action=np.asarray(scheduled, dtype=np.float32).copy(),
+                    timestamp_s=time.monotonic(),
+                    seq=self._policy_target_seq,
+                ),
+            )
+
     def _execute_mocap_pipeline(
         self,
         reference_qpos: Float64Array,
         robot_state: object,
         reference_window: ReferenceWindow | None,
+    ) -> None:
+        self._execute_reference_pipeline(
+            reference_qpos,
+            robot_state,
+            reference_window=reference_window,
+            align_reference=True,
+            compose_arms=self.mode == RobotMode.ARMS,
+        )
+
+    def _execute_reference_pipeline(
+        self,
+        reference_qpos: Float64Array,
+        robot_state: object,
+        *,
+        reference_window: ReferenceWindow | None,
+        align_reference: bool,
+        compose_arms: bool,
     ) -> None:
         reference_window_aligned = False
-        reference_qpos = self._ref_proc.align_reference_yaw(reference_qpos, robot_state=robot_state)
-        if self.mode == RobotMode.ARMS:
+        if align_reference:
+            reference_qpos = self._ref_proc.align_reference_yaw(
+                reference_qpos,
+                robot_state=robot_state,
+            )
+        else:
+            reference_qpos = np.asarray(reference_qpos, dtype=np.float64).copy()
+            reference_window_aligned = True
+        if compose_arms:
             reference_qpos = self._compose_arm_reference(reference_qpos)
             aligned_window = self._ref_proc.align_reference_window(reference_window, robot_state)
             reference_window = self._compose_arm_reference_window(aligned_window)
@@ -1372,10 +2179,21 @@ def _compose_arm_reference_window(self, reference_window: ReferenceWindow | None
 
     def _enter_standing(self) -> None:
         prev_mode = self.mode
+        if bool(getattr(self, "high_level_policy_enabled", False)) and (
+            prev_mode == RobotMode.POLICY or self._policy_entry_pending
+        ):
+            self._stop_high_level_policy_session()
         self._disarm_mocap_reference_if_needed()
         self._clear_reference_gate()
         self._mocap_entry_requested = False
-        already_in_debug = self.mode in (RobotMode.STANDING, RobotMode.MOCAP, RobotMode.ARMS)
+        if prev_mode == RobotMode.STANDING:
+            return
+        already_in_debug = self.mode in (
+            RobotMode.STANDING,
+            RobotMode.MOCAP,
+            RobotMode.ARMS,
+            RobotMode.POLICY,
+        )
         if not already_in_debug:
             logger.info("Entering debug mode...")
             ok = self.robot.enter_debug_mode()
@@ -1385,7 +2203,12 @@ def _enter_standing(self) -> None:
             time.sleep(0.5)
 
         state = self.robot.get_state()
-        if prev_mode not in (RobotMode.MOCAP, RobotMode.ARMS):
+        if prev_mode not in (
+            RobotMode.STANDING,
+            RobotMode.MOCAP,
+            RobotMode.ARMS,
+            RobotMode.POLICY,
+        ):
             logger.info("Locking joints to current position...")
             self.robot.lock_all_joints()
             time.sleep(0.3)
@@ -1397,7 +2220,11 @@ def _enter_standing(self) -> None:
         self._last_commanded_motion_qpos = None
         self._set_default_standing_reference(state)
         self._reset_policy_state()
-        if prev_mode in (RobotMode.MOCAP, RobotMode.ARMS):
+        if prev_mode in (
+            RobotMode.MOCAP,
+            RobotMode.ARMS,
+            RobotMode.POLICY,
+        ):
             self._safety.start_kp_ramp(
                 duration_s=self._standing_return_ramp_duration,
                 floor_ratio=self._standing_return_kp_ramp_floor_ratio,
@@ -1473,10 +2300,14 @@ def _resume_paused_mocap_if_needed(self) -> None:
             self._resume_paused_mocap()
 
     def _enter_damping(self) -> None:
+        if bool(getattr(self, "high_level_policy_enabled", False)) and (
+            self.mode == RobotMode.POLICY or self._policy_entry_pending
+        ):
+            self._stop_high_level_policy_session()
         self._disarm_mocap_reference_if_needed()
         self._clear_reference_gate()
         self._mocap_entry_requested = False
-        if self.mode in (RobotMode.STANDING, RobotMode.MOCAP, RobotMode.ARMS):
+        if self.mode in (RobotMode.STANDING, RobotMode.MOCAP, RobotMode.ARMS, RobotMode.POLICY):
             logger.info("DAMPING: sending LowCmd damping...")
             self.robot.set_damping()
             time.sleep(0.5)
@@ -1587,6 +2418,8 @@ def _resume_paused_mocap(self) -> None:
         logger.info("Mocap session -> ACTIVE (multiprocess episode-reset + reference realignment)")
 
     def _send_reference_command(self, command: str) -> None:
+        if self._reference_command_pub is None:
+            return
         self._reference_command_pub.publish(
             COMMAND_TOPIC,
             CommandPacket(command=command, timestamp_s=time.monotonic()),
@@ -1638,7 +2471,7 @@ def _paused_mocap_step(self) -> None:
             raise RuntimeError("Paused mocap session is missing a hold_qpos")
         self._run_static_mocap_step(hold_qpos)
 
-    def _run_static_mocap_step(self, hold_qpos: Float64Array) -> None:
+    def _run_static_mocap_step(self, hold_qpos: Float64Array) -> object:
         robot_state = self.robot.get_state()
         qpos = np.asarray(hold_qpos, dtype=np.float64).copy()
         motion_joint_vel = np.zeros(self.num_actions, dtype=np.float32)
@@ -1665,6 +2498,7 @@ def _run_static_mocap_step(self, hold_qpos: Float64Array) -> None:
         self._last_commanded_motion_qpos = qpos.copy()
         self._publish_record_step(robot_state=robot_state, reference_qpos=qpos)
         self._write_retarget_viewer(qpos)
+        return robot_state
 
     def _hold_mocap_reference(self, reason: str, *, detail: str | None = None) -> None:
         if self._last_mocap_hold_reason != reason:
@@ -1687,6 +2521,10 @@ def _publish_mode_state(self) -> None:
                 mocap_paused=paused,
                 timestamp_s=time.monotonic(),
                 seq=self._mode_seq,
+                policy_paused=self.mode == RobotMode.POLICY and self._policy_paused,
+                policy_session_id=(
+                    self._policy_session_id if self.mode == RobotMode.POLICY else None
+                ),
             ),
         )
 
@@ -1706,7 +2544,7 @@ def _publish_record_step(self, *, robot_state: object, reference_qpos: Float64Ar
                     mocap_active=active,
                     recordable=recordable,
                     observation_state=build_observation_state(robot_state).astype(np.float32, copy=True),
-                    observation_mode=build_mode_observation(record_mode).astype(np.float32, copy=True),
+                    observation_mode=int(build_mode_observation(record_mode)),
                     action_reference_qpos=normalize_action_reference_qpos(reference_qpos).astype(np.float32, copy=True),
                     seq=self._mode_seq,
                 ),
@@ -1728,7 +2566,7 @@ def _publish_damping_record_step(self) -> None:
                     mocap_active=False,
                     recordable=False,
                     observation_state=build_observation_state(robot_state).astype(np.float32, copy=True),
-                    observation_mode=np.array([-1.0], dtype=np.float32),
+                    observation_mode=-1,
                     action_reference_qpos=normalize_action_reference_qpos(reference_qpos).astype(np.float32, copy=True),
                     seq=self._mode_seq,
                 ),
@@ -1802,6 +2640,8 @@ def _main() -> None:
 
 
 class _RecordingWorker:
+    _CAMERA_TIMEOUT_S = 1.0
+
     def __init__(
         self,
         cfg: dict[str, Any],
@@ -1827,6 +2667,7 @@ def __init__(
         self._record_sub = LatestSubscriber(endpoints.record_pub, RECORD_TOPIC)
         self._video_sub = LatestSubscriber(endpoints.video_pub, VIDEO_TOPIC)
         self._hand_command_sub = LatestSubscriber(endpoints.hand_command_pub, HAND_COMMAND_TOPIC)
+        self._neck_command_sub = LatestSubscriber(endpoints.neck_command_pub, NECK_COMMAND_TOPIC)
         self._command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC)
         self._frame_reader = frame_reader or SharedFrameRingReader()
         self._latest_record: RecordStepPacket | None = None
@@ -1840,23 +2681,35 @@ def __init__(
             right_pose=right_open.astype(np.float32, copy=True),
             seq=0,
         )
+        self._latest_neck_command = NeckCommandPacket(
+            timestamp_s=0.0,
+            driver=str(cfg_get(cfg_get(cfg, "neck", {}) or {}, "driver", "openneck")).strip().lower(),
+            active=False,
+            yaw_deg=0.0,
+            pitch_deg=0.0,
+            seq=0,
+        )
         self._latest_video_seq = -1
+        self._latest_video_received_s: float | None = None
         self._active = False
         self._episode_started_s = 0.0
         self._episode_frames = 0
 
-        from teleopit.recording.hdf5 import (
-            TeleopitHDF5Recorder,
-            build_recording_schema,
-        )
+        from teleopit.recording.hdf5 import TeleopitHDF5Recorder
 
-        self._schema = build_recording_schema(self.camera_cfg)
+        robot_type, hand_type, neck_type = _recording_hardware_types(cfg)
+        self._schema = build_recording_schema(
+            self.camera_cfg,
+            fps=self.fps,
+            robot_type=robot_type,
+            hand_type=hand_type,
+            neck_type=neck_type,
+        )
         self._video_config = build_mp4_video_config(cfg_get(self.rec_cfg, "video", {}) or {})
         factory = recorder_factory or TeleopitHDF5Recorder.create
         self._recorder = factory(
             output_dir=cfg_get(self.rec_cfg, "output_dir", "data/recordings/sim2real_hdf5"),
             task=self.task,
-            fps=self.fps,
             schema=self._schema,
             video_config=self._video_config,
         )
@@ -1879,9 +2732,14 @@ def run(self) -> None:
                 if isinstance(hand_command, HandCommandPacket):
                     self._latest_hand_command = hand_command
 
+                neck_command = self._neck_command_sub.recv_latest()
+                if isinstance(neck_command, NeckCommandPacket):
+                    self._latest_neck_command = neck_command
+
                 video = self._video_sub.recv_latest()
                 if isinstance(video, SharedFrameDescriptor):
                     self._handle_video(video)
+                self._discard_if_camera_stale()
 
                 time.sleep(idle_sleep_s)
         finally:
@@ -1896,6 +2754,7 @@ def run(self) -> None:
                 self._record_sub.close()
                 self._video_sub.close()
                 self._hand_command_sub.close()
+                self._neck_command_sub.close()
                 self._command_sub.close()
                 self._frame_reader.close()
 
@@ -1928,6 +2787,9 @@ def _start_episode(self) -> None:
                 record.recordable,
             )
             return
+        if not self._camera_is_fresh():
+            operator_logger.warning("cannot start recording: no fresh RealSense frame")
+            return
         self._recorder.start_episode()
         self._active = True
         self._episode_started_s = time.monotonic()
@@ -1938,6 +2800,9 @@ def _save_episode(self) -> None:
         if not self._active:
             operator_logger.info("no active recording episode to save")
             return
+        if not self._camera_is_fresh():
+            self._discard_episode("camera stream timeout")
+            return
         duration_s = time.monotonic() - self._episode_started_s
         if self._episode_frames <= 0:
             self._discard_episode("empty episode")
@@ -1963,6 +2828,7 @@ def _handle_video(self, descriptor: SharedFrameDescriptor) -> None:
         if int(descriptor.seq) == self._latest_video_seq:
             return
         self._latest_video_seq = int(descriptor.seq)
+        self._latest_video_received_s = time.monotonic()
         if not self._active:
             return
         record = self._latest_record
@@ -1973,20 +2839,77 @@ def _handle_video(self, descriptor: SharedFrameDescriptor) -> None:
             logger.warning("Recording stopped because mode is no longer recordable: %s", record.mode)
             self._discard_episode("mode not recordable")
             return
+        if self._schema.has_hand_action and (
+            self._latest_hand_command.left_state is None
+            or self._latest_hand_command.right_state is None
+        ):
+            return
+        if self._schema.has_neck_action and (
+            self._latest_neck_command.state_yaw_deg is None
+            or self._latest_neck_command.state_pitch_deg is None
+        ):
+            return
         image = self._frame_reader.read(descriptor, copy=True)
-        self._recorder.add_frame(
-            image=np.asarray(image, dtype=np.uint8),
-            state=np.asarray(record.observation_state, dtype=np.float32),
-            mode=np.asarray(record.observation_mode, dtype=np.float32),
-            action=np.asarray(record.action_reference_qpos, dtype=np.float32),
-            hand_action=normalize_hand_action(
+        hand_state = (
+            normalize_hand_action(
+                self._latest_hand_command.left_state,
+                self._latest_hand_command.right_state,
+            )
+            if self._schema.has_hand_action
+            else None
+        )
+        hand_action = (
+            normalize_hand_action(
                 self._latest_hand_command.left_pose,
                 self._latest_hand_command.right_pose,
-            ),
-            task=self.task,
+            )
+            if self._schema.has_hand_action
+            else None
+        )
+        neck_state = (
+            build_neck_action(
+                self._latest_neck_command.state_yaw_deg,
+                self._latest_neck_command.state_pitch_deg,
+            )
+            if self._schema.has_neck_action
+            else None
         )
+        neck_action = (
+            build_neck_action(
+                self._latest_neck_command.yaw_deg,
+                self._latest_neck_command.pitch_deg,
+            )
+            if self._schema.has_neck_action
+            else None
+        )
+        frame_kwargs = {
+            "image": np.asarray(image, dtype=np.uint8),
+            "state": np.asarray(record.observation_state, dtype=np.float32),
+            "mode": record.observation_mode,
+            "action": np.asarray(record.action_reference_qpos, dtype=np.float32),
+            "hand_state": hand_state,
+            "hand_action": hand_action,
+        }
+        if neck_state is not None:
+            frame_kwargs["neck_state"] = neck_state
+        if neck_action is not None:
+            frame_kwargs["neck_action"] = neck_action
+        self._recorder.add_frame(**frame_kwargs)
         self._episode_frames += 1
 
+    def _camera_is_fresh(self, *, now_s: float | None = None) -> bool:
+        if self._latest_video_received_s is None:
+            return False
+        now = time.monotonic() if now_s is None else float(now_s)
+        return now - self._latest_video_received_s <= self._CAMERA_TIMEOUT_S
+
+    def _discard_if_camera_stale(self, *, now_s: float | None = None) -> bool:
+        if not self._active or self._camera_is_fresh(now_s=now_s):
+            return False
+        self._discard_episode("camera stream timeout")
+        return True
+
+
 def _run_recording_worker(
     cfg: dict[str, Any],
     endpoints: Sim2RealIpcEndpoints,
@@ -1999,6 +2922,128 @@ def _main() -> None:
     _worker_loop("recording_worker", cfg, _main)
 
 
+def _run_neck_worker(
+    cfg: dict[str, Any],
+    endpoints: Sim2RealIpcEndpoints,
+    stop_event: MpEvent,
+) -> None:
+    def _main() -> None:
+        neck_cfg = parse_neck_config(cfg)
+        runtime = build_neck_runtime(neck_cfg)
+        head_pose_sub = LatestSubscriber(endpoints.head_pose_pub, HEAD_POSE_TOPIC)
+        mode_sub = LatestSubscriber(endpoints.mode_pub, MODE_TOPIC)
+        command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC)
+        neck_command_pub = (
+            ZmqPublisher(endpoints.neck_command_pub)
+            if _recording_enabled(cfg)
+            else None
+        )
+        latest_hmd_rotation: Float64Array | None = None
+        latest_spine3_rotation: Float64Array | None = None
+        latest_pose_timestamp_s: float | None = None
+        latest_pose_seq = -1
+        latest_mode: ModeStatePacket | None = None
+        command_count = 0
+        command_seq = 0
+        sleep_s = 1.0 / max(float(neck_cfg.rate_hz), 1.0)
+        last_status_s = 0.0
+
+        def _publish_neck_command(
+            *,
+            timestamp_s: float,
+            active: bool,
+            yaw_deg: float,
+            pitch_deg: float,
+        ) -> None:
+            nonlocal command_seq
+            if neck_command_pub is None:
+                return
+            try:
+                state_yaw_deg, state_pitch_deg = runtime.read_deg()
+            except Exception:
+                logger.exception("OpenNeck state read failed")
+                state_yaw_deg = state_pitch_deg = None
+            command_seq += 1
+            neck_command_pub.publish(
+                NECK_COMMAND_TOPIC,
+                NeckCommandPacket(
+                    timestamp_s=float(timestamp_s),
+                    driver=neck_cfg.driver,
+                    active=bool(active),
+                    yaw_deg=float(yaw_deg),
+                    pitch_deg=float(pitch_deg),
+                    seq=command_seq,
+                    state_yaw_deg=state_yaw_deg,
+                    state_pitch_deg=state_pitch_deg,
+                ),
+            )
+
+        try:
+            runtime.start()
+            if neck_cfg.center_on_start or neck_command_pub is not None:
+                _publish_neck_command(
+                    timestamp_s=time.monotonic(),
+                    active=False,
+                    yaw_deg=0.0,
+                    pitch_deg=0.0,
+                )
+            while not stop_event.is_set():
+                runtime_command = command_sub.recv_latest()
+                if isinstance(runtime_command, CommandPacket) and runtime_command.command == "shutdown":
+                    stop_event.set()
+                    break
+                pose_packet = head_pose_sub.recv_latest()
+                hmd_rotation, spine3_rotation, pose_timestamp_s, pose_seq = head_pose_packet(pose_packet)
+                if pose_seq >= 0:
+                    latest_hmd_rotation = hmd_rotation
+                    latest_spine3_rotation = spine3_rotation
+                    latest_pose_timestamp_s = pose_timestamp_s
+                    latest_pose_seq = pose_seq
+                mode_packet = mode_sub.recv_latest()
+                if isinstance(mode_packet, ModeStatePacket):
+                    latest_mode = mode_packet
+                now_s = time.monotonic()
+                active = mode_packet_active(latest_mode, neck_cfg)
+                try:
+                    neck_command = runtime.tick(
+                        hmd_rotation_wxyz=latest_hmd_rotation,
+                        spine3_rotation_wxyz=latest_spine3_rotation,
+                        pose_timestamp_s=latest_pose_timestamp_s,
+                        active=active,
+                        now_s=now_s,
+                    )
+                    if neck_command is not None:
+                        command_count += 1
+                        _publish_neck_command(
+                            timestamp_s=now_s,
+                            active=active,
+                            yaw_deg=neck_command.yaw_deg,
+                            pitch_deg=neck_command.pitch_deg,
+                        )
+                except Exception:
+                    logger.exception("OpenNeck worker tick failed; neck control continues")
+                if now_s - last_status_s >= 5.0:
+                    logger.debug(
+                        "OpenNeck worker status | head_pose_seq=%s commands=%s active=%s",
+                        latest_pose_seq,
+                        command_count,
+                        active,
+                    )
+                    last_status_s = now_s
+                time.sleep(sleep_s)
+        finally:
+            try:
+                runtime.close()
+            finally:
+                head_pose_sub.close()
+                mode_sub.close()
+                command_sub.close()
+                if neck_command_pub is not None:
+                    neck_command_pub.close()
+
+    _worker_loop("neck_worker", cfg, _main)
+
+
 class _HandSnapshotProxy:
     def __init__(self) -> None:
         self.hand_snapshot: Any | None = None
@@ -2037,6 +3082,11 @@ def _main() -> None:
         hand_mode = str(cfg_get(hands_cfg, "mode", "gripper")).strip().lower()
         left_pose, right_pose = _configured_open_hand_pose(cfg)
         command_seq = 0
+        recording_enabled = _recording_enabled(cfg)
+        state_interval_s = (
+            1.0 / float(cfg_get(_recording_cfg(cfg), "fps", 30))
+            if recording_enabled else 0.0
+        )
 
         def _apply_hand_commands(commands: tuple[HandPoseCommand, ...]) -> bool:
             nonlocal left_pose, right_pose
@@ -2056,8 +3106,20 @@ def _apply_hand_commands(commands: tuple[HandPoseCommand, ...]) -> bool:
                     logger.warning("Ignoring hand command with unsupported side %r", hand_command.side)
             return changed
 
-        def _publish_hand_command(*, timestamp_s: float, active_state: bool) -> None:
+        def _publish_hand_command(
+            *,
+            timestamp_s: float,
+            active_state: bool,
+            read_state: bool = True,
+        ) -> None:
             nonlocal command_seq
+            left_state = right_state = None
+            if recording_enabled and read_state:
+                try:
+                    left_state = np.asarray(runtime.get_state("left"), dtype=np.float32)
+                    right_state = np.asarray(runtime.get_state("right"), dtype=np.float32)
+                except Exception:
+                    logger.exception("LinkerHand state read failed")
             command_seq += 1
             hand_command_pub.publish(
                 HAND_COMMAND_TOPIC,
@@ -2069,6 +3131,8 @@ def _publish_hand_command(*, timestamp_s: float, active_state: bool) -> None:
                     left_pose=np.asarray(left_pose, dtype=np.float32).copy(),
                     right_pose=np.asarray(right_pose, dtype=np.float32).copy(),
                     seq=command_seq,
+                    left_state=None if left_state is None else left_state.copy(),
+                    right_state=None if right_state is None else right_state.copy(),
                 ),
             )
 
@@ -2077,6 +3141,7 @@ def _publish_hand_command(*, timestamp_s: float, active_state: bool) -> None:
             startup_s = time.monotonic()
             _apply_hand_commands(startup_commands)
             _publish_hand_command(timestamp_s=startup_s, active_state=False)
+            last_state_s = startup_s
             while not stop_event.is_set():
                 command = command_sub.recv_latest()
                 if isinstance(command, CommandPacket) and command.command == "shutdown":
@@ -2099,9 +3164,11 @@ def _publish_hand_command(*, timestamp_s: float, active_state: bool) -> None:
                         active=active,
                         now_s=now_s,
                     )
-                    if commands:
-                        if _apply_hand_commands(commands):
-                            _publish_hand_command(timestamp_s=now_s, active_state=active)
+                    commands_changed = bool(commands) and _apply_hand_commands(commands)
+                    state_due = recording_enabled and now_s - last_state_s >= state_interval_s
+                    if commands_changed or state_due:
+                        _publish_hand_command(timestamp_s=now_s, active_state=active)
+                        last_state_s = now_s
                 except Exception:
                     logger.exception("Dexterous hand worker tick failed; hand control continues")
                 time.sleep(sleep_s)
@@ -2110,7 +3177,7 @@ def _publish_hand_command(*, timestamp_s: float, active_state: bool) -> None:
                 shutdown_commands = runtime.close()
                 shutdown_s = time.monotonic()
                 if _apply_hand_commands(shutdown_commands):
-                    _publish_hand_command(timestamp_s=shutdown_s, active_state=False)
+                    _publish_hand_command(timestamp_s=shutdown_s, active_state=False, read_state=False)
             finally:
                 hand_sub.close()
                 controller_sub.close()
diff --git a/teleopit/sim2real/neck/__init__.py b/teleopit/sim2real/neck/__init__.py
new file mode 100644
index 00000000..29c09f94
--- /dev/null
+++ b/teleopit/sim2real/neck/__init__.py
@@ -0,0 +1,12 @@
+"""Optional active-neck runtimes for sim2real."""
+
+from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config
+from teleopit.sim2real.neck.mapper import HmdPoseMapper
+from teleopit.sim2real.neck.worker import build_neck_runtime
+
+__all__ = [
+    "HmdPoseMapper",
+    "NeckConfig",
+    "build_neck_runtime",
+    "parse_neck_config",
+]
diff --git a/teleopit/sim2real/neck/config.py b/teleopit/sim2real/neck/config.py
new file mode 100644
index 00000000..fbdca070
--- /dev/null
+++ b/teleopit/sim2real/neck/config.py
@@ -0,0 +1,100 @@
+from __future__ import annotations
+
+from collections.abc import Iterable
+from dataclasses import dataclass
+import math
+from pathlib import Path
+from typing import Any
+
+from teleopit.runtime.common import cfg_get
+
+VALID_NECK_ACTIVE_MODES = frozenset(("standing", "mocap", "arms", "pause"))
+REMOVED_NECK_CONFIG_KEYS = (
+    "yaw_range_deg",
+    "pitch_range_deg",
+    "invert_yaw",
+    "invert_pitch",
+)
+
+
+@dataclass(frozen=True)
+class NeckConfig:
+    enabled: bool = False
+    driver: str = "openneck"
+    config_path: str | None = None
+    port: str | None = None
+    rate_hz: float = 60.0
+    frame_timeout_s: float = 0.2
+    active_modes: tuple[str, ...] = ("standing", "mocap", "arms", "pause")
+    dead_zone_deg: float = 0.5
+    pitch_gain: float = 1.4
+    center_on_start: bool = True
+    center_on_shutdown: bool = False
+    release_on_shutdown: bool = False
+    dry_run: bool = False
+
+
+def parse_neck_config(cfg: Any) -> NeckConfig:
+    neck_cfg = cfg_get(cfg, "neck", {}) or {}
+    removed = [key for key in REMOVED_NECK_CONFIG_KEYS if cfg_get(neck_cfg, key, None) is not None]
+    if removed:
+        raise ValueError(
+            "Removed normalized OpenNeck config key(s): "
+            f"{', '.join(removed)}. Teleopit now sends head angles in degrees; "
+            "configure motor direction and mechanical limits in the OpenNeck calibration file."
+        )
+    active_modes = _parse_active_modes(cfg_get(neck_cfg, "active_modes", ["standing", "mocap", "arms", "pause"]))
+    rate_hz = float(cfg_get(neck_cfg, "rate_hz", 60.0))
+    if rate_hz <= 0:
+        raise ValueError("neck.rate_hz must be > 0")
+    frame_timeout_s = float(cfg_get(neck_cfg, "frame_timeout_s", 0.2))
+    if frame_timeout_s <= 0:
+        raise ValueError("neck.frame_timeout_s must be > 0")
+    dead_zone_deg = float(cfg_get(neck_cfg, "dead_zone_deg", 0.5))
+    if dead_zone_deg < 0:
+        raise ValueError("neck.dead_zone_deg must be >= 0")
+    pitch_gain = float(cfg_get(neck_cfg, "pitch_gain", 1.4))
+    if not math.isfinite(pitch_gain) or pitch_gain <= 0:
+        raise ValueError("neck.pitch_gain must be finite and > 0")
+    config_path = cfg_get(neck_cfg, "config_path", None)
+    if config_path in ("", "null"):
+        config_path = None
+    elif config_path is not None:
+        config_path = str(Path(str(config_path)).expanduser())
+    port = cfg_get(neck_cfg, "port", None)
+    if port in ("", "null"):
+        port = None
+    return NeckConfig(
+        enabled=bool(cfg_get(neck_cfg, "enabled", False)),
+        driver=str(cfg_get(neck_cfg, "driver", "openneck")).strip().lower(),
+        config_path=config_path,
+        port=None if port is None else str(port),
+        rate_hz=rate_hz,
+        frame_timeout_s=frame_timeout_s,
+        active_modes=active_modes,
+        dead_zone_deg=dead_zone_deg,
+        pitch_gain=pitch_gain,
+        center_on_start=bool(cfg_get(neck_cfg, "center_on_start", True)),
+        center_on_shutdown=bool(cfg_get(neck_cfg, "center_on_shutdown", False)),
+        release_on_shutdown=bool(cfg_get(neck_cfg, "release_on_shutdown", False)),
+        dry_run=bool(cfg_get(neck_cfg, "dry_run", False)),
+    )
+
+
+def _parse_active_modes(value: Any) -> tuple[str, ...]:
+    if isinstance(value, str):
+        modes = (value.strip().lower(),)
+    elif isinstance(value, Iterable):
+        modes = tuple(str(mode).strip().lower() for mode in value)
+    else:
+        raise ValueError("neck.active_modes must be a mode string or a list of modes")
+    modes = tuple(mode for mode in modes if mode)
+    if not modes:
+        raise ValueError("neck.active_modes must contain at least one mode")
+    unsupported = sorted(set(modes).difference(VALID_NECK_ACTIVE_MODES))
+    if unsupported:
+        raise ValueError(
+            "neck.active_modes contains unsupported modes "
+            f"{unsupported}; supported modes: {sorted(VALID_NECK_ACTIVE_MODES)}"
+        )
+    return modes
diff --git a/teleopit/sim2real/neck/mapper.py b/teleopit/sim2real/neck/mapper.py
new file mode 100644
index 00000000..f5737ba8
--- /dev/null
+++ b/teleopit/sim2real/neck/mapper.py
@@ -0,0 +1,99 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+import math
+
+import numpy as np
+from numpy.typing import NDArray
+
+from teleopit.sim2real.neck.config import NeckConfig
+
+
+FloatArray = NDArray[np.float64]
+
+
+@dataclass(frozen=True)
+class NeckCommand:
+    yaw_deg: float
+    pitch_deg: float
+    roll_deg: float
+
+
+class HmdPoseMapper:
+    """Map synchronized Pico HMD/Spine3 orientations to OpenNeck angles."""
+
+    def __init__(self, config: NeckConfig) -> None:
+        self._cfg = config
+
+    def map_pose(
+        self,
+        *,
+        hmd_rotation_wxyz: FloatArray | None,
+        spine3_rotation_wxyz: FloatArray | None,
+    ) -> NeckCommand | None:
+        q_hmd = _normalized_quat(hmd_rotation_wxyz)
+        if q_hmd is None:
+            return None
+        q_body = _normalized_quat(spine3_rotation_wxyz)
+        if q_body is None:
+            return None
+        # The HMD and Spine3 share the same neutral orientation in the supported
+        # PICO convention, so their relative identity is the fixed zero pose.
+        # Deliberately do not read the full-body tracker's skeleton Head joint:
+        # its model constraints can under-report extreme head pitch.
+        q_cmd = _qmul(_qconj(q_body), q_hmd)
+        yaw_deg, pitch_deg, roll_deg = _openneck_yaw_pitch_roll_deg(q_cmd)
+        # Convert the supported PICO convention to OpenNeck's physical command
+        # convention: positive yaw turns left and positive pitch looks up.
+        yaw_deg = -yaw_deg
+        pitch_deg = -pitch_deg
+        if abs(yaw_deg) < self._cfg.dead_zone_deg:
+            yaw_deg = 0.0
+        if abs(pitch_deg) < self._cfg.dead_zone_deg:
+            pitch_deg = 0.0
+        else:
+            pitch_deg *= self._cfg.pitch_gain
+
+        return NeckCommand(
+            yaw_deg=float(yaw_deg),
+            pitch_deg=float(pitch_deg),
+            roll_deg=float(roll_deg),
+        )
+
+
+def _normalized_quat(value: FloatArray | None) -> FloatArray | None:
+    if value is None:
+        return None
+    quat = np.asarray(value, dtype=np.float64).reshape(-1)
+    if quat.shape != (4,) or not np.all(np.isfinite(quat)):
+        return None
+    norm = float(np.linalg.norm(quat))
+    if norm <= 1e-9:
+        return None
+    return quat / norm
+
+
+def _qconj(q: FloatArray) -> FloatArray:
+    return np.array([q[0], -q[1], -q[2], -q[3]], dtype=np.float64)
+
+
+def _qmul(a: FloatArray, b: FloatArray) -> FloatArray:
+    w1, x1, y1, z1 = a
+    w2, x2, y2, z2 = b
+    return np.array(
+        [
+            w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
+            w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
+            w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
+            w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2,
+        ],
+        dtype=np.float64,
+    )
+
+
+def _openneck_yaw_pitch_roll_deg(q_wxyz: FloatArray) -> tuple[float, float, float]:
+    w, x, y, z = q_wxyz
+    yaw = math.degrees(math.atan2(2.0 * (x * z + w * y), 1.0 - 2.0 * (y * y + z * z)))
+    pitch = math.degrees(math.asin(float(np.clip(-2.0 * (y * z - w * x), -1.0, 1.0))))
+    roll = math.degrees(math.atan2(2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z)))
+    return yaw, pitch, roll
diff --git a/teleopit/sim2real/neck/openneck.py b/teleopit/sim2real/neck/openneck.py
new file mode 100644
index 00000000..a02c6b1e
--- /dev/null
+++ b/teleopit/sim2real/neck/openneck.py
@@ -0,0 +1,135 @@
+from __future__ import annotations
+
+import logging
+from typing import Protocol
+
+from teleopit.sim2real.neck.config import NeckConfig
+
+logger = logging.getLogger(__name__)
+
+
+def _load_openneck_controller() -> type:
+    try:
+        from openneck import OpenNeckController
+    except ModuleNotFoundError as exc:
+        raise ImportError(
+            "OpenNeck 0.2.0 is required for neck.driver=openneck. "
+            "Install with: pip install -e '.[openneck]'"
+        ) from exc
+    if not all(callable(getattr(OpenNeckController, name, None)) for name in ("move_deg", "read_deg")):
+        raise ImportError(
+            "OpenNeck 0.2.0 move_deg/read_deg angle API is required; reinstall with: "
+            "pip install --force-reinstall --no-deps "
+            "'openneck @ git+https://github.com/BotRunner64/OpenNeck.git'"
+        )
+    return OpenNeckController
+
+
+class NeckDevice(Protocol):
+    def connect(self) -> None: ...
+
+    def center(self) -> None: ...
+
+    def release_torque(self) -> None: ...
+
+    def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]: ...
+
+    def read_deg(self) -> tuple[float, float]: ...
+
+    def close(self) -> None: ...
+
+
+class OpenNeckDevice:
+    def __init__(self, config: NeckConfig) -> None:
+        self._cfg = config
+        self._controller = None
+
+    def connect(self) -> None:
+        OpenNeckController = _load_openneck_controller()
+        controller = OpenNeckController(
+            config=self._cfg.config_path,
+            port=self._cfg.port,
+        )
+        controller.connect()
+        self._controller = controller
+        logger.info("OpenNeck connected on port %s", getattr(self._controller, "port", self._cfg.port))
+
+    def center(self) -> None:
+        if self._controller is not None:
+            self._controller.center()
+
+    def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]:
+        if self._controller is None:
+            raise RuntimeError("OpenNeck is not connected")
+        applied = self._controller.move_deg(float(yaw_deg), float(pitch_deg))
+        return float(applied.yaw_deg), float(applied.pitch_deg)
+
+    def read_deg(self) -> tuple[float, float]:
+        if self._controller is None:
+            raise RuntimeError("OpenNeck is not connected")
+        state = self._controller.read_deg()
+        return float(state.yaw_deg), float(state.pitch_deg)
+
+    def release_torque(self) -> None:
+        if self._controller is not None:
+            self._controller.release_torque()
+
+    def close(self) -> None:
+        controller = self._controller
+        self._controller = None
+        if controller is not None:
+            controller.close()
+
+
+class DryRunNeckDevice:
+    def __init__(self, config: NeckConfig) -> None:
+        self._cfg = config
+        self._controller = None
+
+    def connect(self) -> None:
+        OpenNeckController = _load_openneck_controller()
+        self._controller = OpenNeckController(
+            config=self._cfg.config_path,
+            port=self._cfg.port,
+        )
+        logger.info("OpenNeck dry-run device active")
+
+    def center(self) -> None:
+        logger.info("OpenNeck dry-run center")
+
+    def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]:
+        controller = self._controller
+        if controller is None:
+            raise RuntimeError("OpenNeck dry-run device is not connected")
+        # Reuse OpenNeck's calibration conversion without writing to
+        # its servo driver, so dry-run reports the same clamped target.
+        yaw_step = controller._angle_to_step("yaw", float(yaw_deg))
+        pitch_step = controller._angle_to_step("pitch", float(pitch_deg))
+        applied_yaw_deg = float(controller._step_to_angle("yaw", yaw_step))
+        applied_pitch_deg = float(controller._step_to_angle("pitch", pitch_step))
+        logger.debug(
+            "OpenNeck dry-run command yaw=%.3fdeg pitch=%.3fdeg applied_yaw=%.3fdeg applied_pitch=%.3fdeg",
+            yaw_deg,
+            pitch_deg,
+            applied_yaw_deg,
+            applied_pitch_deg,
+        )
+        return applied_yaw_deg, applied_pitch_deg
+
+    def read_deg(self) -> tuple[float, float]:
+        raise RuntimeError("OpenNeck dry-run has no hardware state to read")
+
+    def release_torque(self) -> None:
+        logger.info("OpenNeck dry-run release")
+
+    def close(self) -> None:
+        self._controller = None
+        logger.info("OpenNeck dry-run closed")
+
+
+def build_neck_device(config: NeckConfig) -> NeckDevice:
+    if config.driver != "openneck":
+        raise ValueError("Unsupported neck.driver={!r}; supported drivers: openneck".format(config.driver))
+    if config.dry_run:
+        return DryRunNeckDevice(config)
+    return OpenNeckDevice(config)
diff --git a/teleopit/sim2real/neck/worker.py b/teleopit/sim2real/neck/worker.py
new file mode 100644
index 00000000..7e63b504
--- /dev/null
+++ b/teleopit/sim2real/neck/worker.py
@@ -0,0 +1,144 @@
+from __future__ import annotations
+
+import logging
+import time
+from typing import Any
+
+import numpy as np
+from numpy.typing import NDArray
+
+from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config
+from teleopit.sim2real.neck.mapper import HmdPoseMapper, NeckCommand
+from teleopit.sim2real.neck.openneck import NeckDevice, build_neck_device
+
+logger = logging.getLogger(__name__)
+FloatArray = NDArray[np.float64]
+
+
+class NeckRuntime:
+    def __init__(self, config: NeckConfig, device: NeckDevice | None = None) -> None:
+        self._cfg = config
+        self._device = device or build_neck_device(config)
+        self._mapper = HmdPoseMapper(config)
+
+    def start(self) -> None:
+        self._device.connect()
+        if self._cfg.center_on_start:
+            self._device.center()
+
+    def read_deg(self) -> tuple[float, float]:
+        return self._device.read_deg()
+
+    def tick(
+        self,
+        *,
+        hmd_rotation_wxyz: FloatArray | None,
+        spine3_rotation_wxyz: FloatArray | None,
+        pose_timestamp_s: float | None,
+        active: bool,
+        now_s: float | None = None,
+    ) -> NeckCommand | None:
+        now = time.monotonic() if now_s is None else float(now_s)
+        if not active or pose_timestamp_s is None:
+            return None
+        if now - float(pose_timestamp_s) > self._cfg.frame_timeout_s:
+            return None
+        command = self._mapper.map_pose(
+            hmd_rotation_wxyz=hmd_rotation_wxyz,
+            spine3_rotation_wxyz=spine3_rotation_wxyz,
+        )
+        if command is None:
+            return None
+        applied_yaw_deg, applied_pitch_deg = self._device.move_deg(
+            command.yaw_deg,
+            command.pitch_deg,
+        )
+        return NeckCommand(
+            yaw_deg=applied_yaw_deg,
+            pitch_deg=applied_pitch_deg,
+            roll_deg=command.roll_deg,
+        )
+
+    def close(self) -> None:
+        try:
+            if self._cfg.center_on_shutdown:
+                try:
+                    self._device.center()
+                except Exception:
+                    logger.exception("Failed to center OpenNeck on shutdown; closing device")
+            if self._cfg.release_on_shutdown:
+                try:
+                    self._device.release_torque()
+                except Exception:
+                    logger.exception("Failed to release OpenNeck torque on shutdown; closing device")
+        finally:
+            self._device.close()
+
+
+class DisabledNeckRuntime:
+    def start(self) -> None:
+        return None
+
+    def read_deg(self) -> tuple[float, float]:
+        raise RuntimeError("OpenNeck control is disabled")
+
+    def tick(
+        self,
+        *,
+        hmd_rotation_wxyz: FloatArray | None,
+        spine3_rotation_wxyz: FloatArray | None,
+        pose_timestamp_s: float | None,
+        active: bool,
+        now_s: float | None = None,
+    ) -> None:
+        del hmd_rotation_wxyz, spine3_rotation_wxyz, pose_timestamp_s, active, now_s
+        return None
+
+    def close(self) -> None:
+        return None
+
+
+def build_neck_runtime(cfg: Any | NeckConfig, device: NeckDevice | None = None) -> NeckRuntime | DisabledNeckRuntime:
+    neck_cfg = cfg if isinstance(cfg, NeckConfig) else parse_neck_config(cfg)
+    if not neck_cfg.enabled:
+        return DisabledNeckRuntime()
+    return NeckRuntime(neck_cfg, device=device)
+
+
+def mode_packet_active(mode_packet: object | None, config: NeckConfig) -> bool:
+    if mode_packet is None:
+        return False
+    mode = "pause" if bool(getattr(mode_packet, "mocap_paused", False)) else str(getattr(mode_packet, "mode", "")).strip().lower()
+    return mode in config.active_modes
+
+
+def head_pose_packet(
+    packet: object | None,
+) -> tuple[FloatArray | None, FloatArray | None, float | None, int]:
+    if packet is None or not all(hasattr(packet, attr) for attr in ("snapshot", "timestamp_s", "seq")):
+        return None, None, None, -1
+    snapshot = getattr(packet, "snapshot")
+    if snapshot is None or not all(
+        hasattr(snapshot, attr)
+        for attr in ("hmd_rotation_wxyz", "spine3_rotation_wxyz", "timestamp_s", "seq")
+    ):
+        return None, None, None, -1
+    try:
+        timestamp_s = float(getattr(packet, "timestamp_s"))
+        seq = int(getattr(packet, "seq"))
+        if int(getattr(snapshot, "seq")) != seq:
+            return None, None, None, -1
+        hmd_rotation = _optional_quat(getattr(snapshot, "hmd_rotation_wxyz"))
+        spine3_rotation = _optional_quat(getattr(snapshot, "spine3_rotation_wxyz"))
+        return hmd_rotation, spine3_rotation, timestamp_s, seq
+    except (TypeError, ValueError):
+        return None, None, None, -1
+
+
+def _optional_quat(value: object | None) -> FloatArray | None:
+    if value is None:
+        return None
+    try:
+        return np.asarray(value, dtype=np.float64).reshape(-1)
+    except (TypeError, ValueError):
+        return None
diff --git a/teleopit/sim2real/safety.py b/teleopit/sim2real/safety.py
index 6ce08ebe..57a5e5db 100644
--- a/teleopit/sim2real/safety.py
+++ b/teleopit/sim2real/safety.py
@@ -56,6 +56,10 @@ def __init__(
             self._joint_pos_lower = None
             self._joint_pos_upper = None
 
+    @property
+    def kp_ramp_active(self) -> bool:
+        return self._kp_ramp_active
+
     def compute_kp_ramp_gains(self) -> tuple[Float32Array, Float32Array] | None:
         """Return (kp, kd) for current Kp-ramp step, or None if ramp inactive."""
         if not self._kp_ramp_active:
diff --git a/tests/test_active_neck.py b/tests/test_active_neck.py
new file mode 100644
index 00000000..f7f52b2c
--- /dev/null
+++ b/tests/test_active_neck.py
@@ -0,0 +1,408 @@
+from __future__ import annotations
+
+import math
+from types import ModuleType, SimpleNamespace
+
+import numpy as np
+
+from teleopit.inputs.pico4_provider import PicoHeadPoseSnapshot
+from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config
+from teleopit.sim2real.neck.mapper import HmdPoseMapper
+from teleopit.sim2real.neck.openneck import DryRunNeckDevice, OpenNeckDevice
+from teleopit.sim2real.neck.worker import NeckRuntime, head_pose_packet
+from teleopit.sim2real.mp.messages import SnapshotPacket
+
+
+def _quat_y(deg: float) -> np.ndarray:
+    rad = math.radians(deg)
+    return np.array([math.cos(rad / 2.0), 0.0, math.sin(rad / 2.0), 0.0], dtype=np.float64)
+
+
+def _quat_x(deg: float) -> np.ndarray:
+    rad = math.radians(deg)
+    return np.array([math.cos(rad / 2.0), math.sin(rad / 2.0), 0.0, 0.0], dtype=np.float64)
+
+
+class FakeDevice:
+    def __init__(self) -> None:
+        self.moves: list[tuple[float, float]] = []
+        self.center_calls = 0
+        self.released = False
+        self.closed = False
+
+    def connect(self) -> None:
+        return None
+
+    def center(self) -> None:
+        self.center_calls += 1
+
+    def release_torque(self) -> None:
+        self.released = True
+
+    def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]:
+        self.moves.append((yaw_deg, pitch_deg))
+        return max(-20.0, min(20.0, yaw_deg)), max(-10.0, min(10.0, pitch_deg))
+
+    def read_deg(self) -> tuple[float, float]:
+        return -18.5, 9.5
+
+    def close(self) -> None:
+        self.closed = True
+
+
+def test_hmd_pose_mapper_applies_pitch_gain_to_openneck_degrees() -> None:
+    mapper = HmdPoseMapper(
+        NeckConfig(enabled=True, dead_zone_deg=0.0, pitch_gain=1.4)
+    )
+
+    command = mapper.map_pose(
+        hmd_rotation_wxyz=_quat_y(30.0),
+        spine3_rotation_wxyz=_quat_y(0.0),
+    )
+    assert command is not None
+    assert command.yaw_deg == pytest_approx(-30.0)
+
+    command = mapper.map_pose(
+        hmd_rotation_wxyz=_quat_y(0.0),
+        spine3_rotation_wxyz=_quat_y(0.0),
+    )
+    assert command is not None
+    assert command.yaw_deg == pytest_approx(0.0)
+
+    command = mapper.map_pose(
+        hmd_rotation_wxyz=_quat_x(15.0),
+        spine3_rotation_wxyz=_quat_x(0.0),
+    )
+    assert command is not None
+    assert command.pitch_deg == pytest_approx(-21.0)
+
+
+def test_hmd_pose_mapper_applies_dead_zone_before_pitch_gain() -> None:
+    mapper = HmdPoseMapper(
+        NeckConfig(enabled=True, dead_zone_deg=0.5, pitch_gain=2.0)
+    )
+
+    inside_dead_zone = mapper.map_pose(
+        hmd_rotation_wxyz=_quat_x(0.4),
+        spine3_rotation_wxyz=_quat_x(0.0),
+    )
+    outside_dead_zone = mapper.map_pose(
+        hmd_rotation_wxyz=_quat_x(10.0),
+        spine3_rotation_wxyz=_quat_x(0.0),
+    )
+
+    assert inside_dead_zone is not None
+    assert inside_dead_zone.pitch_deg == pytest_approx(0.0)
+    assert outside_dead_zone is not None
+    assert outside_dead_zone.pitch_deg == pytest_approx(-20.0)
+
+
+def test_hmd_pose_mapper_uses_body_relative_orientation() -> None:
+    mapper = HmdPoseMapper(NeckConfig(enabled=True, dead_zone_deg=0.0))
+
+    command = mapper.map_pose(
+        hmd_rotation_wxyz=_quat_y(40.0),
+        spine3_rotation_wxyz=_quat_y(10.0),
+    )
+
+    assert command is not None
+    assert command.yaw_deg == pytest_approx(-30.0)
+
+
+def test_hmd_pose_mapper_requires_hmd_and_spine3_orientations() -> None:
+    mapper = HmdPoseMapper(NeckConfig(enabled=True))
+
+    assert mapper.map_pose(
+        hmd_rotation_wxyz=_quat_y(30.0),
+        spine3_rotation_wxyz=None,
+    ) is None
+    assert mapper.map_pose(
+        hmd_rotation_wxyz=None,
+        spine3_rotation_wxyz=_quat_y(0.0),
+    ) is None
+
+
+def test_neck_runtime_sends_degrees_and_returns_applied_target() -> None:
+    device = FakeDevice()
+    cfg = NeckConfig(
+        enabled=True,
+        dead_zone_deg=0.0,
+        center_on_start=True,
+        center_on_shutdown=True,
+    )
+    runtime = NeckRuntime(cfg, device=device)
+
+    runtime.start()
+    assert runtime.read_deg() == (-18.5, 9.5)
+    command = runtime.tick(
+        hmd_rotation_wxyz=_quat_y(30.0),
+        spine3_rotation_wxyz=_quat_y(0.0),
+        pose_timestamp_s=1.0,
+        active=True,
+        now_s=1.01,
+    )
+    neutral_command = runtime.tick(
+        hmd_rotation_wxyz=_quat_y(0.0),
+        spine3_rotation_wxyz=_quat_y(0.0),
+        pose_timestamp_s=1.02,
+        active=True,
+        now_s=1.03,
+    )
+    runtime.close()
+
+    assert command is not None
+    assert command.yaw_deg == pytest_approx(-20.0)
+    assert command.pitch_deg == pytest_approx(0.0)
+    assert neutral_command is not None
+    assert neutral_command.yaw_deg == pytest_approx(0.0)
+    np.testing.assert_allclose(device.moves, [(-30.0, 0.0), (0.0, 0.0)], atol=1e-6)
+    assert device.center_calls == 2
+    assert device.closed is True
+
+
+def test_neck_runtime_releases_torque_on_shutdown_when_enabled() -> None:
+    device = FakeDevice()
+    runtime = NeckRuntime(
+        NeckConfig(enabled=True, center_on_start=False, release_on_shutdown=True),
+        device=device,
+    )
+
+    runtime.close()
+
+    assert device.released is True
+    assert device.closed is True
+
+
+def test_neck_shutdown_defaults_to_close_only() -> None:
+    device = FakeDevice()
+    runtime = NeckRuntime(NeckConfig(enabled=True, center_on_start=False), device=device)
+
+    runtime.close()
+
+    assert device.center_calls == 0
+    assert device.released is False
+    assert device.closed is True
+
+
+def test_neck_runtime_closes_after_shutdown_center_failure() -> None:
+    class CenterFailingDevice(FakeDevice):
+        def center(self) -> None:
+            raise RuntimeError("neck center failed")
+
+    device = CenterFailingDevice()
+    runtime = NeckRuntime(
+        NeckConfig(enabled=True, center_on_start=False, center_on_shutdown=True),
+        device=device,
+    )
+
+    runtime.close()
+
+    assert device.closed is True
+
+
+def test_head_pose_packet_extracts_synchronized_snapshot() -> None:
+    snapshot = PicoHeadPoseSnapshot(
+        hmd_rotation_wxyz=_quat_y(20.0),
+        spine3_rotation_wxyz=_quat_y(5.0),
+        timestamp_s=1.0,
+        seq=4,
+    )
+
+    hmd_rotation, spine3_rotation, timestamp_s, seq = head_pose_packet(
+        SnapshotPacket(snapshot=snapshot, timestamp_s=1.0, seq=4)
+    )
+
+    np.testing.assert_allclose(hmd_rotation, _quat_y(20.0))
+    np.testing.assert_allclose(spine3_rotation, _quat_y(5.0))
+    assert timestamp_s == 1.0
+    assert seq == 4
+
+
+def test_head_pose_packet_ignores_incomplete_or_mismatched_packets() -> None:
+    assert head_pose_packet(None) == (None, None, None, -1)
+    assert head_pose_packet(SimpleNamespace(snapshot=object(), timestamp_s=1.0, seq=1)) == (
+        None,
+        None,
+        None,
+        -1,
+    )
+    snapshot = PicoHeadPoseSnapshot(
+        hmd_rotation_wxyz=_quat_y(0.0),
+        spine3_rotation_wxyz=_quat_y(0.0),
+        timestamp_s=1.0,
+        seq=2,
+    )
+    assert head_pose_packet(SnapshotPacket(snapshot=snapshot, timestamp_s=1.0, seq=3)) == (
+        None,
+        None,
+        None,
+        -1,
+    )
+
+
+def test_openneck_device_uses_angle_api_and_returns_applied_target(monkeypatch) -> None:
+    calls: list[str] = []
+
+    class FakeOpenNeckController:
+        port = "/dev/fake"
+
+        def __init__(self, *, config: object, port: object) -> None:
+            calls.append(f"init-{config}-{port}")
+
+        def connect(self) -> None:
+            calls.append("connect")
+
+        def center(self) -> None:
+            calls.append("center")
+
+        def move_deg(self, yaw_deg: float, pitch_deg: float) -> SimpleNamespace:
+            calls.append(f"move-{yaw_deg}-{pitch_deg}")
+            return SimpleNamespace(yaw_deg=-20.0, pitch_deg=10.0)
+
+        def read_deg(self) -> SimpleNamespace:
+            calls.append("read")
+            return SimpleNamespace(yaw_deg=-18.5, pitch_deg=9.5)
+
+        def release_torque(self) -> None:
+            calls.append("release-torque")
+
+        def close(self) -> None:
+            calls.append("close")
+
+    module = ModuleType("openneck")
+    module.OpenNeckController = FakeOpenNeckController  # type: ignore[attr-defined]
+    monkeypatch.setitem(__import__("sys").modules, "openneck", module)
+
+    device = OpenNeckDevice(
+        NeckConfig(enabled=True, config_path="neck.json", port="/dev/ttyACM0")
+    )
+    device.connect()
+    device.center()
+    applied = device.move_deg(-25.0, 15.0)
+    state = device.read_deg()
+    device.release_torque()
+    device.close()
+
+    assert applied == (-20.0, 10.0)
+    assert state == (-18.5, 9.5)
+    assert calls == [
+        "init-neck.json-/dev/ttyACM0",
+        "connect",
+        "center",
+        "move--25.0-15.0",
+        "read",
+        "release-torque",
+        "close",
+    ]
+
+
+def test_dry_run_neck_device_reuses_openneck_calibration_clamp(monkeypatch) -> None:
+    calls: list[str] = []
+
+    class FakeOpenNeckController:
+        def __init__(self, *, config: object, port: object) -> None:
+            calls.append(f"init-{config}-{port}")
+
+        def move_deg(self, yaw_deg: float, pitch_deg: float) -> None:
+            del yaw_deg, pitch_deg
+            raise AssertionError("dry-run must not send a hardware command")
+
+        def read_deg(self) -> None:
+            raise AssertionError("dry-run must not read hardware state")
+
+        def _angle_to_step(self, axis: str, angle_deg: float) -> int:
+            calls.append(f"angle-to-step-{axis}-{angle_deg}")
+            low, high = (-20.0, 20.0) if axis == "yaw" else (-10.0, 10.0)
+            return round(max(low, min(high, angle_deg)))
+
+        def _step_to_angle(self, axis: str, step: int) -> float:
+            calls.append(f"step-to-angle-{axis}-{step}")
+            return float(step)
+
+    module = ModuleType("openneck")
+    module.OpenNeckController = FakeOpenNeckController  # type: ignore[attr-defined]
+    monkeypatch.setitem(__import__("sys").modules, "openneck", module)
+
+    device = DryRunNeckDevice(
+        NeckConfig(
+            enabled=True,
+            config_path="neck.json",
+            port="/dev/ttyACM0",
+            dry_run=True,
+        )
+    )
+    device.connect()
+    applied = device.move_deg(25.0, -15.0)
+    try:
+        device.read_deg()
+    except RuntimeError as exc:
+        assert "no hardware state" in str(exc)
+    else:
+        raise AssertionError("expected dry-run state read to fail")
+    device.close()
+
+    assert applied == (20.0, -10.0)
+    assert calls == [
+        "init-neck.json-/dev/ttyACM0",
+        "angle-to-step-yaw-25.0",
+        "angle-to-step-pitch--15.0",
+        "step-to-angle-yaw-20",
+        "step-to-angle-pitch--10",
+    ]
+
+
+def test_parse_neck_config_validates_rate() -> None:
+    try:
+        parse_neck_config({"neck": {"enabled": True, "rate_hz": 0}})
+    except ValueError as exc:
+        assert "neck.rate_hz" in str(exc)
+    else:
+        raise AssertionError("expected ValueError")
+
+
+def test_parse_neck_config_accepts_scalar_active_mode() -> None:
+    cfg = parse_neck_config({"neck": {"enabled": True, "active_modes": "mocap"}})
+
+    assert cfg.active_modes == ("mocap",)
+
+
+def test_parse_neck_config_accepts_pitch_gain() -> None:
+    cfg = parse_neck_config({"neck": {"enabled": True, "pitch_gain": 1.6}})
+
+    assert cfg.pitch_gain == pytest_approx(1.6)
+
+
+def test_parse_neck_config_rejects_invalid_pitch_gain() -> None:
+    for value in (0.0, -1.0, float("nan"), float("inf")):
+        try:
+            parse_neck_config({"neck": {"enabled": True, "pitch_gain": value}})
+        except ValueError as exc:
+            assert "neck.pitch_gain" in str(exc)
+        else:
+            raise AssertionError(f"expected ValueError for pitch_gain={value!r}")
+
+
+def test_parse_neck_config_rejects_unknown_active_mode() -> None:
+    try:
+        parse_neck_config({"neck": {"enabled": True, "active_modes": ["mocap", "idle"]}})
+    except ValueError as exc:
+        assert "neck.active_modes" in str(exc)
+        assert "idle" in str(exc)
+    else:
+        raise AssertionError("expected ValueError")
+
+
+def test_parse_neck_config_rejects_removed_normalized_fields() -> None:
+    try:
+        parse_neck_config({"neck": {"enabled": True, "yaw_range_deg": 90.0}})
+    except ValueError as exc:
+        assert "Removed normalized OpenNeck config" in str(exc)
+        assert "angles in degrees" in str(exc)
+    else:
+        raise AssertionError("expected ValueError")
+
+
+def pytest_approx(value: float):
+    import pytest
+
+    return pytest.approx(value, abs=1e-6)
diff --git a/tests/test_benchmark_omnixtreme.py b/tests/test_benchmark_omnixtreme.py
new file mode 100644
index 00000000..dffdf770
--- /dev/null
+++ b/tests/test_benchmark_omnixtreme.py
@@ -0,0 +1,444 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from types import SimpleNamespace
+
+import pytest
+import numpy as np
+import torch
+
+from train_mimic.benchmarking import (
+    BenchmarkJob,
+    ClipSpec,
+    RolloutResult,
+    build_benchmark_plan,
+    compute_tracking_metrics,
+    summarize_rollouts,
+    write_benchmark_outputs,
+)
+from train_mimic.tasks.tracking.mdp.commands import MotionCommand
+from train_mimic.scripts.benchmark import _configure_benchmark_env_cfg, _run_batch, parse_args
+
+
+@dataclass
+class _FakeAgentCfg:
+    clip_actions: float | None = None
+
+
+def _clip(clip_id: int, duration_s: float) -> ClipSpec:
+    return ClipSpec(
+        clip_id=clip_id,
+        shard_path="shard.h5",
+        shard_clip_index=clip_id,
+        frame_offset=clip_id * 1000,
+        num_frames=int(duration_s * 30) + 1,
+        fps=30.0,
+        sample_start_s=0.0,
+        sample_end_s=duration_s,
+    )
+
+
+def test_build_benchmark_plan_uses_all_eligible_clips() -> None:
+    plan = build_benchmark_plan(
+        [_clip(0, 10.0), _clip(1, 9.9), _clip(2, 12.0)],
+        clip_seconds=10.0,
+        step_dt=0.02,
+    )
+
+    assert plan.control_steps == 500
+    assert [clip.clip_id for clip in plan.eligible_clips] == [0, 2]
+    assert [clip.clip_id for clip in plan.skipped_short_clips] == [1]
+    assert plan.jobs == (
+        BenchmarkJob(job_id=0, clip_id=0, rollout_index=0, start_time_s=0.0),
+        BenchmarkJob(job_id=1, clip_id=2, rollout_index=0, start_time_s=0.0),
+    )
+
+
+def test_build_benchmark_plan_requires_integer_control_steps() -> None:
+    with pytest.raises(ValueError, match="integer number of control steps"):
+        build_benchmark_plan(
+            [_clip(0, 10.0)],
+            clip_seconds=10.0,
+            step_dt=0.03,
+        )
+
+
+def test_compute_tracking_metrics() -> None:
+    ref = np.array(
+        [
+            [[0.0, 0.0, 0.0]],
+            [[1.0, 0.0, 0.0]],
+            [[3.0, 0.0, 0.0]],
+        ],
+        dtype=np.float32,
+    )
+    robot = np.array(
+        [
+            [[0.0, 0.0, 0.0]],
+            [[1.1, 0.0, 0.0]],
+            [[3.4, 0.0, 0.0]],
+        ],
+        dtype=np.float32,
+    )
+
+    metrics = compute_tracking_metrics(
+        ref,
+        robot,
+        root_pos_error_m=np.array([0.1, 0.2, 0.3], dtype=np.float32),
+        root_rot_error_rad=np.array([0.01, 0.02, 0.03], dtype=np.float32),
+        root_vel_error_m_s=np.array([1.0, 2.0, 3.0], dtype=np.float32),
+    )
+
+    assert metrics["mpjpe_m"] == pytest.approx((0.0 + 0.1 + 0.4) / 3.0)
+    assert metrics["root_pos_error_m"] == pytest.approx(0.2)
+    assert metrics["root_rot_error_rad"] == pytest.approx(0.02)
+    assert metrics["root_vel_error_m_s"] == pytest.approx(2.0)
+
+
+def test_summarize_rollouts_aggregates_success_and_metrics() -> None:
+    results = [
+        RolloutResult(0, 0, 0, True, 500, None, None, 0.01, 0.1, 0.01, 1.0),
+        RolloutResult(
+            1,
+            0,
+            1,
+            False,
+            120,
+            120,
+            "anchor_pos",
+            float("nan"),
+            float("nan"),
+            float("nan"),
+            float("nan"),
+        ),
+        RolloutResult(2, 1, 0, True, 500, None, None, 0.03, 0.3, 0.03, 3.0),
+    ]
+
+    summary = summarize_rollouts(results)
+
+    assert summary["global"]["success_rate"] == pytest.approx(200.0 / 3.0)
+    assert summary["global"]["mpjpe_m"] == pytest.approx(0.02)
+    assert summary["global"]["root_pos_error_m"] == pytest.approx(0.2)
+    assert summary["global"]["root_rot_error_rad"] == pytest.approx(0.02)
+    assert summary["global"]["root_vel_error_m_s"] == pytest.approx(2.0)
+    assert summary["per_clip"][0]["success_rate"] == pytest.approx(50.0)
+    assert summary["per_clip"][0]["mpjpe_m"] == pytest.approx(0.01)
+    assert summary["per_clip"][1]["success_rate"] == pytest.approx(100.0)
+
+
+def test_parse_args_rejects_removed_legacy_flags() -> None:
+    with pytest.raises(SystemExit):
+        parse_args(
+            [
+                "--checkpoint",
+                "model.pt",
+                "--motion_file",
+                "data/datasets_precomputed",
+                "--num_eval_steps",
+                "2000",
+            ]
+        )
+
+
+def test_parse_args_rejects_rollouts_per_clip_flag() -> None:
+    with pytest.raises(SystemExit):
+        parse_args(
+            [
+                "--checkpoint",
+                "model.pt",
+                "--motion_file",
+                "data/datasets_precomputed",
+                "--rollouts_per_clip",
+                "2",
+            ]
+        )
+
+
+def test_benchmark_env_cfg_disables_noise_events_and_clip_resample() -> None:
+    motion = SimpleNamespace(
+        motion_file="old",
+        sampling_mode="rewind",
+        resample_on_clip_end=True,
+        pose_range={"x": (-1.0, 1.0)},
+        velocity_range={"x": (-1.0, 1.0)},
+        joint_position_range=(-0.1, 0.1),
+    )
+    cfg = SimpleNamespace(
+        commands={"motion": motion},
+        events={"base_com": object(), "add_joint_default_pos": object()},
+        episode_length_s=1.0,
+        auto_reset=True,
+    )
+
+    out = _configure_benchmark_env_cfg(
+        cfg,
+        motion_file="data/datasets_precomputed",
+        clip_seconds=10.0,
+    )
+
+    assert out.commands["motion"].motion_file == "data/datasets_precomputed"
+    assert out.commands["motion"].sampling_mode == "start"
+    assert out.commands["motion"].resample_on_clip_end is False
+    assert out.commands["motion"].pose_range == {}
+    assert out.commands["motion"].velocity_range == {}
+    assert out.commands["motion"].joint_position_range == (0.0, 0.0)
+    assert out.events == {}
+    assert out.episode_length_s == 10.0
+    assert out.auto_reset is False
+
+
+def test_reset_to_motion_rejects_sample_end_time() -> None:
+    cmd = SimpleNamespace()
+    cmd.device = "cpu"
+    cmd.motion_times = torch.zeros(1, dtype=torch.float32)
+    cmd.motion_ids = torch.zeros(1, dtype=torch.long)
+    cmd.time_left = torch.zeros(1, dtype=torch.float32)
+    cmd.motion = SimpleNamespace(
+        num_clips=1,
+        clip_sample_start_s=torch.tensor([0.0], dtype=torch.float32),
+        clip_sample_end_s=torch.tensor([10.0], dtype=torch.float32),
+    )
+
+    with pytest.raises(ValueError, match=r"range=\[0\.000000, 10\.000000\)"):
+        MotionCommand.reset_to_motion(
+            cmd,
+            torch.tensor([0]),
+            torch.tensor([0]),
+            torch.tensor([10.0]),
+        )
+
+
+def test_write_benchmark_outputs_serializes_failed_metrics_as_null(tmp_path) -> None:
+    plan = build_benchmark_plan(
+        [_clip(0, 10.0)],
+        clip_seconds=10.0,
+        step_dt=0.02,
+    )
+    result = RolloutResult(
+        job_id=0,
+        clip_id=0,
+        rollout_index=0,
+        success=False,
+        steps=120,
+        failure_step=120,
+        failure_reason="anchor_pos",
+        mpjpe_m=float("nan"),
+        root_pos_error_m=float("nan"),
+        root_rot_error_rad=float("nan"),
+        root_vel_error_m_s=float("nan"),
+    )
+
+    paths = write_benchmark_outputs(
+        tmp_path,
+        text_stem="benchmark",
+        metadata={
+            "task": "General-Tracking-G1",
+            "checkpoint": "model.pt",
+            "motion_file": "dataset",
+        },
+        plan=plan,
+        results=[result],
+    )
+
+    data = paths["summary_json"].read_text()
+    assert "NaN" not in data
+    report = __import__("json").loads(data)
+    assert report["global"]["mpjpe_m"] is None
+    assert report["global"]["root_pos_error_m"] is None
+    assert report["per_rollout"][0]["mpjpe_m"] is None
+
+
+def test_run_batch_resets_inactive_done_envs_and_excludes_failed_metrics(monkeypatch) -> None:
+    import train_mimic.scripts.benchmark as benchmark_script
+
+    class FakeTensor:
+        def __init__(self, values):
+            self.values = np.asarray(values)
+
+        def __or__(self, other):
+            return FakeTensor(self.values | other.values)
+
+        def detach(self):
+            return self
+
+        def cpu(self):
+            return self
+
+        def numpy(self):
+            return self.values
+
+        def item(self):
+            return bool(self.values)
+
+    class FakeTorch:
+        long = "long"
+        float32 = "float32"
+
+        class no_grad:
+            def __enter__(self):
+                return None
+
+            def __exit__(self, exc_type, exc, tb):
+                return False
+
+        @staticmethod
+        def tensor(values, dtype=None, device=None):
+            return list(values)
+
+        @staticmethod
+        def arange(n, dtype=None, device=None):
+            return list(range(n))
+
+    class FakeCmd:
+        def reset_to_motion(self, env_ids, motion_ids, motion_times):
+            return None
+
+    class FakeCommandManager:
+        def __init__(self):
+            self.cmd = FakeCmd()
+
+        def get_term(self, name):
+            return self.cmd
+
+        def compute(self, dt):
+            return None
+
+    class FakeScene:
+        def write_data_to_sim(self):
+            return None
+
+    class FakeSim:
+        def forward(self):
+            return None
+
+        def sense(self):
+            return None
+
+    class FakeObservationManager:
+        def __init__(self):
+            self.reset_calls = []
+
+        def reset(self, env_ids):
+            self.reset_calls.append(list(env_ids))
+            return {}
+
+        def compute(self, update_history):
+            return {"actor": np.zeros((2, 1), dtype=np.float32)}
+
+    class FakeTermCfg:
+        time_out = False
+
+    class FakeTerminationManager:
+        active_terms = ("failure",)
+
+        def get_term_cfg(self, term_name):
+            return FakeTermCfg()
+
+        def get_term(self, term_name):
+            return [FakeTensor(False), FakeTensor(True)]
+
+    class FakeEnv:
+        instances = []
+
+        def __init__(self, cfg, device, render_mode):
+            self.cfg = cfg
+            self.device = device
+            self.scene = FakeScene()
+            self.sim = FakeSim()
+            self.command_manager = FakeCommandManager()
+            self.observation_manager = FakeObservationManager()
+            self.termination_manager = FakeTerminationManager()
+            self.reset_calls = []
+            self.step_index = 0
+            FakeEnv.instances.append(self)
+
+        def reset(self, env_ids=None):
+            self.reset_calls.append(None if env_ids is None else list(env_ids))
+            return {"actor": np.zeros((2, 1), dtype=np.float32)}, {}
+
+        def step(self, actions):
+            self.step_index += 1
+            if self.step_index == 1:
+                terminated = FakeTensor([False, True])
+                truncated = FakeTensor([False, False])
+            else:
+                terminated = FakeTensor([False, True])
+                truncated = FakeTensor([True, False])
+            return (
+                {"actor": np.zeros((2, 1), dtype=np.float32)},
+                None,
+                terminated,
+                truncated,
+                {},
+            )
+
+        def close(self):
+            return None
+
+    class FakeWrapper:
+        def __init__(self, env, clip_actions):
+            self.env = env
+
+    class FakeRunner:
+        def __init__(self, wrapped_env, agent_dict, log_dir, device):
+            return None
+
+        def load(self, checkpoint, map_location):
+            return None
+
+        def get_inference_policy(self, device):
+            return lambda obs: np.zeros((2, 1), dtype=np.float32)
+
+    def fake_aligned(_cmd):
+        ref = np.zeros((2, 1, 3), dtype=np.float32)
+        robot = np.zeros((2, 1, 3), dtype=np.float32)
+        return ref, robot
+
+    def fake_root_errors(_cmd):
+        return (
+            np.zeros(2, dtype=np.float32),
+            np.zeros(2, dtype=np.float32),
+            np.zeros(2, dtype=np.float32),
+        )
+
+    monkeypatch.setattr(benchmark_script, "_aligned_keybody_positions", fake_aligned)
+    monkeypatch.setattr(benchmark_script, "_root_tracking_errors", fake_root_errors)
+
+    motion = SimpleNamespace(motion_file="dataset")
+    base_env_cfg = SimpleNamespace(
+        commands={"motion": motion},
+        events={},
+        episode_length_s=1.0,
+        auto_reset=True,
+        scene=SimpleNamespace(num_envs=0),
+    )
+    agent_cfg = _FakeAgentCfg()
+    jobs = [
+        BenchmarkJob(0, 0, 0, 0.0),
+        BenchmarkJob(1, 1, 0, 0.0),
+    ]
+
+    results = _run_batch(
+        batch_index=0,
+        jobs=jobs,
+        base_env_cfg=base_env_cfg,
+        agent_cfg=agent_cfg,
+        runner_cls=FakeRunner,
+        fallback_runner_cls=FakeRunner,
+        checkpoint="model.pt",
+        log_dir="logs",
+        device="cpu",
+        torch_module=FakeTorch,
+        ManagerBasedRlEnv=FakeEnv,
+        RslRlVecEnvWrapper=FakeWrapper,
+        clip_seconds=10.0,
+        control_steps=2,
+        seed=42,
+    )
+
+    env = FakeEnv.instances[-1]
+    assert env.reset_calls == [None, [1]]
+    assert env.observation_manager.reset_calls == [[0, 1]]
+    assert results[0].success is True
+    assert results[1].success is False
+    assert np.isnan(results[1].mpjpe_m)
diff --git a/tests/test_dexterous_hand.py b/tests/test_dexterous_hand.py
index 53aad28a..c3c3b321 100644
--- a/tests/test_dexterous_hand.py
+++ b/tests/test_dexterous_hand.py
@@ -1,7 +1,7 @@
 from __future__ import annotations
 
 import sys
-from types import SimpleNamespace
+from types import ModuleType, SimpleNamespace
 
 import numpy as np
 import pytest
@@ -10,6 +10,7 @@
 from teleopit.sim2real.hands.linkerhand_l6 import (
     GripperMapper,
     LinkerHandL6Device,
+    RetargetPoseMapper,
     SomehandL6Mapper,
     parse_linkerhand_l6_config,
     trigger_to_pose,
@@ -17,7 +18,10 @@
 from teleopit.sim2real.hands.base import HandPoseCommand
 from teleopit.sim2real.hands.linkerhand_o6 import (
     CLOSE_POSE as O6_CLOSE_POSE,
+    DEFAULT_SOMEHAND_CONFIG as O6_DEFAULT_SOMEHAND_CONFIG,
     LinkerHandO6Device,
+    O6_SDK_JOINT_ORDER,
+    SomehandO6Mapper,
     parse_linkerhand_o6_config,
 )
 from teleopit.sim2real.hands.pico_landmarks import pico_hand_to_landmarks
@@ -47,6 +51,7 @@ def __init__(self, *, hand_joint: str, hand_type: str, modbus: str, can: str) ->
         self.hand = FakeInnerHand()
         self.speed: list[int] | None = None
         self.poses: list[list[int]] = []
+        self.state = [1, 2, 3, 4, 5, 6] if hand_type == "left" else [11, 12, 13, 14, 15, 16]
         self.close_can_calls = 0
         FakeLinkerHandApi.instances.append(self)
 
@@ -56,6 +61,9 @@ def set_speed(self, speed: list[int]) -> None:
     def finger_move(self, pose: list[int]) -> None:
         self.poses.append(list(pose))
 
+    def get_state(self) -> list[int]:
+        return list(self.state)
+
     def close_can(self) -> None:
         self.close_can_calls += 1
 
@@ -106,6 +114,12 @@ def _o6_cfg(mode: str = "gripper") -> dict[str, object]:
                 "trigger_deadzone": 0.05,
                 "deadman_threshold": 0.5,
             },
+            "somehand": {
+                "rate_hz": 60.0,
+                "max_iterations": 12,
+                "temporal_filter_alpha": 1.0,
+                "output_alpha": 1.0,
+            },
         },
     }
 
@@ -196,8 +210,10 @@ def test_linkerhand_l6_device_starts_sdk(monkeypatch) -> None:
 
     device.connect()
     device.send_pose("left", cfg.close_pose)
+    state = device.get_state("left")
     device.close()
 
+    assert state == (1.0, 2.0, 3.0, 4.0, 5.0, 6.0)
     assert [hand.can for hand in FakeLinkerHandApi.instances] == ["can0", "can1"]
     assert FakeLinkerHandApi.instances[0].speed == [50, 50, 50, 50, 50, 50]
     assert FakeLinkerHandApi.instances[0].poses[-2] == list(cfg.close_pose)
@@ -233,8 +249,10 @@ def test_linkerhand_o6_device_starts_sdk(monkeypatch) -> None:
 
     device.connect()
     device.send_pose("left", cfg.close_pose)
+    state = device.get_state("right")
     device.close()
 
+    assert state == (11.0, 12.0, 13.0, 14.0, 15.0, 16.0)
     assert [hand.hand_joint for hand in FakeLinkerHandApi.instances] == ["O6", "O6"]
     assert [hand.can for hand in FakeLinkerHandApi.instances] == ["can0", "can1"]
     assert FakeLinkerHandApi.instances[0].speed == [255, 255, 255, 255, 255, 255]
@@ -244,9 +262,104 @@ def test_linkerhand_o6_device_starts_sdk(monkeypatch) -> None:
     assert [hand.hand.close_calls for hand in FakeLinkerHandApi.instances] == [0, 0]
 
 
-def test_linkerhand_o6_rejects_vr_hand_pose() -> None:
-    with pytest.raises(ValueError, match="supports only hands.mode=gripper"):
-        parse_linkerhand_o6_config(_o6_cfg(mode="vr_hand_pose"))
+def test_linkerhand_o6_accepts_vr_hand_pose() -> None:
+    cfg = parse_linkerhand_o6_config(_o6_cfg(mode="vr_hand_pose"))
+
+    assert cfg.mode == "vr_hand_pose"
+    assert cfg.speed == (255, 255, 255, 255, 255, 255)
+    assert cfg.somehand_config_path == O6_DEFAULT_SOMEHAND_CONFIG
+
+    mapper = SomehandO6Mapper(cfg)
+    assert mapper.map(controller_snapshot=None, hand_snapshot=None, active=False, now_s=10.0) == ()
+    mapper._active = True
+    first_inactive = mapper.map(controller_snapshot=None, hand_snapshot=None, active=False, now_s=10.1)
+    assert [command.force for command in first_inactive] == [True, True]
+
+
+def test_somehand_mapper_loads_only_configured_side(monkeypatch) -> None:
+    class FakeHandModel:
+        def get_joint_name_to_qpos_index(self) -> dict[str, int]:
+            return {
+                "lh_thumb_cmc_pitch": 0,
+                "lh_thumb_cmc_roll": 1,
+                "lh_index_mcp_pitch": 2,
+                "lh_middle_mcp_pitch": 3,
+                "lh_ring_mcp_pitch": 4,
+                "lh_pinky_mcp_pitch": 5,
+            }
+
+    class FakeRetargetingEngine:
+        def __init__(self, cfg: object) -> None:
+            self.cfg = cfg
+            self.hand_model = FakeHandModel()
+
+    loaded_paths: list[str] = []
+    somehand_api = ModuleType("somehand.api")
+    somehand_api.HandFrame = object
+    somehand_api.RetargetingEngine = FakeRetargetingEngine
+    somehand_api.load_bihand_config = lambda path: SimpleNamespace(
+        left_config_path="left-only.yaml",
+        right_config_path="right-should-not-load.yaml",
+    )
+
+    def load_retargeting_config(path: str):
+        loaded_paths.append(path)
+        if path != "left-only.yaml":
+            raise AssertionError(f"unexpected path loaded: {path}")
+        return SimpleNamespace(
+            solver=SimpleNamespace(max_iterations=30, output_alpha=0.7),
+            preprocess=SimpleNamespace(temporal_filter_alpha=0.35),
+        )
+
+    somehand_api.load_retargeting_config = load_retargeting_config
+    somehand_pkg = ModuleType("somehand")
+    somehand_pkg.__path__ = []
+    monkeypatch.setitem(sys.modules, "somehand", somehand_pkg)
+    monkeypatch.setitem(sys.modules, "somehand.api", somehand_api)
+    monkeypatch.setattr("teleopit.sim2real.hands.linkerhand_l6.version", lambda name: "0.3.0")
+    monkeypatch.setattr("teleopit.sim2real.hands.linkerhand_l6._resolve_project_path", lambda path: SimpleNamespace(exists=lambda: True))
+    monkeypatch.setattr(
+        "teleopit.sim2real.hands.linkerhand_l6._load_linkerhand_mapping_module",
+        lambda: SimpleNamespace(
+            l6_l_min=[0.0, -0.087266, 0.0, 0.0, 0.0, 0.0],
+            l6_l_max=[0.837758, 1.256637, 1.134464, 1.134464, 1.134464, 1.134464],
+            l6_l_derict=[-1, -1, -1, -1, -1, -1],
+        ),
+    )
+    config_dict = _cfg(mode="vr_hand_pose")
+    config_dict["hands"]["sides"] = ["left"]  # type: ignore[index]
+    mapper = SomehandL6Mapper(parse_linkerhand_l6_config(config_dict))
+
+    mapper.start()
+
+    assert loaded_paths == ["left-only.yaml"]
+
+
+def test_o6_retarget_pose_mapper_uses_o6_thumb_yaw_and_mapping(monkeypatch) -> None:
+    class FakeHandModel:
+        def get_joint_name_to_qpos_index(self) -> dict[str, int]:
+            return {
+                "lh_thumb_cmc_pitch": 0,
+                "lh_thumb_cmc_yaw": 1,
+                "lh_index_mcp_pitch": 2,
+                "lh_middle_mcp_pitch": 3,
+                "lh_ring_mcp_pitch": 4,
+                "lh_pinky_mcp_pitch": 5,
+            }
+
+    mapping = SimpleNamespace(
+        o6_l_min=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        o6_l_max=[0.58, 1.36, 1.6, 1.6, 1.6, 1.6],
+        o6_l_derict=[-1, -1, -1, -1, -1, -1],
+        is_within_range=lambda value, lower, upper: max(lower, min(upper, value)),
+        scale_value=lambda value, in_min, in_max, out_min, out_max: out_min
+        + (value - in_min) * (out_max - out_min) / (in_max - in_min),
+    )
+    monkeypatch.setattr("teleopit.sim2real.hands.linkerhand_l6._load_linkerhand_mapping_module", lambda: mapping)
+
+    mapper = RetargetPoseMapper(FakeHandModel(), side="left", family="O6", joint_order=O6_SDK_JOINT_ORDER)
+
+    assert mapper.qpos_to_pose(np.asarray([0.58, 0.0, 0.8, 1.6, 0.0, 1.6])) == [0, 255, 128, 0, 255, 0]
 
 
 def test_hand_runtime_closes_device_when_mapper_start_fails() -> None:
@@ -295,6 +408,10 @@ class FakeDevice:
         def connect(self) -> None:
             calls.append(("connect", None, None))
 
+        def get_state(self, side: str) -> tuple[float, ...]:
+            start = 1.0 if side == "left" else 11.0
+            return tuple(start + index for index in range(6))
+
         def send_pose(self, side, pose, *, force=False, reason="") -> None:
             calls.append((side, tuple(pose), reason))
 
@@ -323,6 +440,7 @@ def close(self) -> None:
     runtime = HandRuntime(FakeDevice(), mapper, open_commands=open_commands)
 
     startup = runtime.start()
+    assert runtime.get_state("right") == (11.0, 12.0, 13.0, 14.0, 15.0, 16.0)
     ticked = runtime.tick(controller_snapshot=None, hand_snapshot=None, active=True, now_s=1.0)
     mapper.fail = True
     failure = runtime.tick(controller_snapshot=None, hand_snapshot=None, active=True, now_s=2.0)
diff --git a/tests/test_download_assets.py b/tests/test_download_assets.py
index 9b7a0f26..b7f48782 100644
--- a/tests/test_download_assets.py
+++ b/tests/test_download_assets.py
@@ -56,6 +56,20 @@ def test_robot_asset_group_uses_archive_layout() -> None:
     assert entries[0].mode == "extract"
 
 
+def test_checkpoint_asset_group_uses_named_ckpt_directory() -> None:
+    entries = ASSET_GROUPS["ckpt"]
+
+    assert [(entry.remote_path, entry.local_path) for entry in entries] == [
+        ("checkpoints/track_g1.onnx", "ckpt/track_g1.onnx"),
+        ("checkpoints/track_g1.pt", "ckpt/track_g1.pt"),
+        (
+            "checkpoints/track_g1_neck_o6.onnx",
+            "ckpt/track_g1_neck_o6.onnx",
+        ),
+        ("checkpoints/track_g1_neck_o6.pt", "ckpt/track_g1_neck_o6.pt"),
+    ]
+
+
 def test_data_asset_group_downloads_only_hdf5_shards() -> None:
     entries = ASSET_GROUPS["data"]
 
diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py
new file mode 100644
index 00000000..4a53cf9e
--- /dev/null
+++ b/tests/test_high_level_policy.py
@@ -0,0 +1,1490 @@
+from __future__ import annotations
+
+import math
+import threading
+import time
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+import zmq
+
+from teleopit.high_level_policy.client import HighLevelPolicyClient, PolicyActionChunk
+from teleopit.high_level_policy.config import (
+    HighLevelPolicySafetyConfig,
+    parse_high_level_policy_config,
+)
+from teleopit.high_level_policy.hand_calibration import HandCalibration
+from teleopit.high_level_policy.protocol import (
+    MAX_ACTION_HORIZON,
+    MAX_REQUEST_BYTES,
+    MAX_RESPONSE_BYTES,
+    PolicyProtocolError,
+    decode_float32_array,
+    encode_float32_array,
+    pack_message,
+    unpack_message,
+)
+from teleopit.high_level_policy.scheduler import (
+    HighLevelPolicyScheduler,
+    PolicyFrameTransform,
+    closure_to_o6_pose,
+)
+from teleopit.sim2real.mp.high_level_policy_runtime import (
+    HighLevelPolicySim2RealRuntime,
+    _apply_policy_neck_target,
+    _policy_target_is_current,
+    _stop_and_hardware_reset_realsense,
+    _test_pattern,
+    _validate_high_level_policy_runtime_config,
+)
+from teleopit.sim2real.mp.high_level_policy_worker import HighLevelPolicyWorker
+from teleopit.sim2real.mp.messages import (
+    HandCommandPacket,
+    HighLevelPolicyActionPacket,
+    HighLevelPolicyObservationPacket,
+    HighLevelPolicySessionPacket,
+    HighLevelPolicyStatusPacket,
+    HighLevelPolicyTargetPacket,
+    ModeStatePacket,
+    NeckCommandPacket,
+    SharedFrameDescriptor,
+)
+from teleopit.sim2real.mp.runtime import (
+    RobotMode,
+    Sim2RealRuntime,
+    _RobotControlWorker,
+)
+from teleopit.runtime.mocap_session import MocapSessionState
+
+
+def _chunk(*, source_s: float, sequence: int = 0, frames: int = 3) -> PolicyActionChunk:
+    actions = np.zeros((frames, 50), dtype=np.float32)
+    actions[:, 2] = 0.78
+    actions[:, 3] = 1.0
+    actions[:, 0] = np.arange(frames, dtype=np.float32)
+    actions[:, 36:48] = 0.5
+    actions[:, 48] = np.arange(frames, dtype=np.float32) * 10.0
+    return PolicyActionChunk(
+        session_id="session-1",
+        source_sequence_id=sequence,
+        source_onboard_monotonic_timestamp_ns=int(round(source_s * 1e9)),
+        action_fps=30,
+        actions=actions,
+        policy_id="test",
+        server_inference_ms=1.0,
+    )
+
+
+def _safe_actions(frames: int = 3) -> np.ndarray:
+    actions = np.zeros((frames, 50), dtype=np.float32)
+    actions[:, 0] = np.arange(frames, dtype=np.float32) * 0.02
+    actions[:, 2] = 0.76
+    actions[:, 3] = 1.0
+    actions[:, 36:48] = 0.5
+    return actions
+
+
+def _safety_config() -> HighLevelPolicySafetyConfig:
+    return HighLevelPolicySafetyConfig(
+        root_height_min_m=0.55,
+        root_height_max_m=1.05,
+        max_root_xy_speed_m_s=2.5,
+        max_root_displacement_m=0.1,
+        max_yaw_rate_rad_s=2.5,
+        max_joint_rate_rad_s=10.0,
+        max_joint_projection_rad=0.1,
+        joint_pos_lower=(-3.0,) * 29,
+        joint_pos_upper=(3.0,) * 29,
+        neck_yaw_min_deg=-45.0,
+        neck_yaw_max_deg=45.0,
+        neck_pitch_min_deg=-40.0,
+        neck_pitch_max_deg=40.0,
+    )
+
+
+def _safe_chunk(actions: np.ndarray, *, source_s: float = 1.0, sequence: int = 0) -> PolicyActionChunk:
+    return PolicyActionChunk(
+        session_id="session-1",
+        source_sequence_id=sequence,
+        source_onboard_monotonic_timestamp_ns=int(round(source_s * 1e9)),
+        action_fps=30,
+        actions=actions,
+        policy_id="test",
+        server_inference_ms=1.0,
+    )
+
+
+def test_high_level_policy_default_hold_covers_inference_and_transport_jitter() -> None:
+    config = parse_high_level_policy_config({"high_level_policy": {"task": "demo"}})
+
+    assert config.hold_s == pytest.approx(3.0)
+
+
+def test_high_level_policy_replan_steps_uses_protocol_horizon_limit() -> None:
+    config = parse_high_level_policy_config(
+        {
+            "high_level_policy": {
+                "task": "demo",
+                "replan_steps": MAX_ACTION_HORIZON,
+            }
+        }
+    )
+
+    assert config.replan_steps == MAX_ACTION_HORIZON
+
+    with pytest.raises(ValueError, match=rf"\[1, {MAX_ACTION_HORIZON}\]"):
+        parse_high_level_policy_config(
+            {
+                "high_level_policy": {
+                    "task": "demo",
+                    "replan_steps": MAX_ACTION_HORIZON + 1,
+                }
+            }
+        )
+
+
+def test_packaged_hand_calibration_loads() -> None:
+    calibration = HandCalibration.load()
+
+    assert calibration.open_raw == (250.0, 250.0, 250.0, 250.0, 250.0, 250.0)
+    assert calibration.close_raw == (86.0, 73.0, 118.0, 111.0, 110.0, 111.0)
+    assert calibration.range_tolerance == pytest.approx(0.0001)
+
+
+def test_msgpack_float32_array_roundtrip_is_little_endian() -> None:
+    values = np.arange(12, dtype=np.float64).reshape(3, 4)
+    message = {"array": encode_float32_array(values)}
+    payload = pack_message(message, max_bytes=4096)
+    decoded_message = unpack_message(payload, max_bytes=4096)
+    decoded = decode_float32_array(
+        decoded_message["array"],
+        name="array",
+        expected_shape=(3, 4),
+    )
+
+    assert decoded.dtype == np.dtype("float32")
+    np.testing.assert_allclose(decoded, values.astype(np.float32))
+
+
+def test_policy_frame_transform_localizes_and_delocalizes_action() -> None:
+    yaw = math.pi / 2.0
+    yaw_quaternion = np.array([math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)], dtype=np.float32)
+    transform = PolicyFrameTransform.from_robot_pose([2.0, 3.0], yaw_quaternion)
+
+    body = np.zeros(36, dtype=np.float32)
+    body[0] = 1.0
+    body[2] = 0.78
+    body[3] = 1.0
+    world = transform.delocalize_body_action(body)
+    np.testing.assert_allclose(world[:3], [2.0, 4.0, 0.78], atol=1e-6)
+    np.testing.assert_allclose(world[3:7], yaw_quaternion, atol=1e-6)
+    np.testing.assert_allclose(transform.localize_body_action(world), body, atol=1e-6)
+
+
+def test_scheduler_uses_source_timestamp_and_interpolates_at_30hz() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1)
+    scheduler.reset("session-1")
+    scheduler.accept(_chunk(source_s=10.0), now_s=10.01)
+
+    halfway = scheduler.sample(10.0 + 0.5 / 30.0)
+    assert halfway is not None
+    assert halfway[0] == pytest.approx(0.5)
+    assert halfway[48] == pytest.approx(5.0)
+
+
+def test_scheduler_accepts_protocol_max_action_horizon() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1)
+    scheduler.reset("session-1")
+    scheduler.accept(_chunk(source_s=10.0, frames=MAX_ACTION_HORIZON), now_s=10.01)
+
+    assert scheduler.has_chunk
+
+
+def test_scheduler_replaces_active_plan_using_new_source_timestamp() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1)
+    scheduler.reset("session-1")
+    scheduler.accept(_chunk(source_s=20.0), now_s=20.0)
+
+    replacement = _chunk(source_s=20.05, sequence=1)
+    replacement.actions[:, 0] += 10.0
+    scheduler.accept(replacement, now_s=20.06)
+
+    scheduled = scheduler.sample(20.05 + 0.5 / 30.0)
+    assert scheduled is not None
+    assert scheduled[0] == pytest.approx(10.5)
+
+
+def test_scheduler_pause_freezes_and_resume_shifts_plan_time() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1)
+    scheduler.reset("session-1")
+    scheduler.accept(_chunk(source_s=20.0), now_s=20.0)
+    scheduler.pause(20.02)
+
+    paused = scheduler.sample(25.0)
+    assert paused is not None
+    scheduler.resume(25.0)
+    resumed = scheduler.sample(25.0)
+    assert resumed is not None
+    np.testing.assert_allclose(resumed, paused)
+
+
+def test_scheduler_interpolates_active_reference_history_at_camera_timestamp() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1)
+    initial = _safe_actions(1)[0]
+    scheduler.reset("session-1", initial_action=initial)
+    assert scheduler.reference_root_pose_at(1.0) is None
+    scheduler.reset(
+        "session-1",
+        initial_reference=initial,
+        initial_timestamp_s=1.0,
+    )
+    actions = _safe_actions(2)
+    actions[1, 0] = 1.0
+    yaw = math.pi / 2.0
+    actions[1, 3:7] = [
+        math.cos(yaw / 2.0),
+        0.0,
+        0.0,
+        math.sin(yaw / 2.0),
+    ]
+    scheduler.accept(_safe_chunk(actions), now_s=1.0)
+    scheduler.sample(1.0 + 1.0 / 30.0)
+
+    source_pose = scheduler.reference_root_pose_at(1.0 + 0.5 / 30.0)
+
+    assert source_pose is not None
+    assert source_pose[0] == pytest.approx(0.5)
+    source_yaw = 2.0 * math.atan2(float(source_pose[6]), float(source_pose[3]))
+    assert source_yaw == pytest.approx(math.pi / 4.0)
+
+
+def test_scheduler_rejects_wrong_session_and_expired_chunk() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.0)
+    scheduler.reset("other")
+    with pytest.raises(ValueError, match="session mismatch"):
+        scheduler.accept(_chunk(source_s=1.0), now_s=1.0)
+
+    scheduler.reset("session-1")
+    with pytest.raises(ValueError, match="already expired"):
+        scheduler.accept(_chunk(source_s=1.0), now_s=2.0)
+
+    with pytest.raises(ValueError, match="in the future"):
+        scheduler.accept(_chunk(source_s=3.0), now_s=2.0)
+
+
+def test_scheduler_rejects_nonincreasing_source_timestamp() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1)
+    scheduler.reset("session-1")
+    scheduler.accept(_chunk(source_s=1.0), now_s=1.0)
+
+    with pytest.raises(ValueError, match="source timestamp must increase"):
+        scheduler.accept(_chunk(source_s=1.0, sequence=1), now_s=1.01)
+
+
+def test_linkerhand_closure_uses_hand_calibration() -> None:
+    assert closure_to_o6_pose(np.zeros(6, dtype=np.float32)) == (250, 250, 250, 250, 250, 250)
+    assert closure_to_o6_pose(np.ones(6, dtype=np.float32)) == (86, 73, 118, 111, 110, 111)
+
+
+def test_scheduler_validates_complete_chunk_against_onboard_safety_limits() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config())
+    initial = _safe_actions(1)[0]
+    scheduler.reset("session-1", initial_action=initial)
+    scheduler.accept(_safe_chunk(_safe_actions()), now_s=1.01)
+
+    assert scheduler.has_chunk
+
+
+def test_scheduler_accepts_internal_reference_discontinuities() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config())
+    initial = _safe_actions(1)[0]
+    initial[7] = 0.8
+    actions = _safe_actions()
+    actions[0, 0] = 0.2
+    actions[0, 7] = -0.08
+    actions[1, 0] = -0.2
+    actions[1, 7] = 0.5
+    yaw = 0.2
+    actions[1, 3:7] = [math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)]
+    scheduler.reset("session-1", initial_action=initial)
+
+    scheduler.accept(_safe_chunk(actions), now_s=1.01)
+
+    assert scheduler.has_chunk
+
+
+def test_scheduler_clips_joint_positions_to_onboard_limits() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config())
+    scheduler.reset(
+        "session-1",
+        initial_action=_safe_actions(1)[0],
+    )
+    actions = _safe_actions(1)
+    actions[0, 7] = -3.08
+    actions[0, 8] = 3.08
+
+    scheduler.accept(_safe_chunk(actions), now_s=1.01)
+    scheduled = None
+    for _ in range(20):
+        scheduled = scheduler.sample(1.0)
+
+    assert scheduled is not None
+    assert scheduled[7] == pytest.approx(-3.0)
+    assert scheduled[8] == pytest.approx(3.0)
+
+
+def test_scheduler_clips_openneck_angles_to_onboard_limits() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config())
+    scheduler.reset(
+        "session-1",
+        initial_action=_safe_actions(1)[0],
+    )
+    actions = _safe_actions()
+    actions[:, 48] = [-46.0, 0.0, 46.0]
+    actions[:, 49] = [41.0, 0.0, -41.0]
+
+    scheduler.accept(_safe_chunk(actions), now_s=1.01)
+    first_action = scheduler.sample(1.0)
+    final_action = scheduler.sample(1.0 + 2.0 / 30.0)
+
+    assert first_action is not None
+    assert first_action[48] == pytest.approx(-45.0)
+    assert first_action[49] == pytest.approx(40.0)
+    assert final_action is not None
+    assert final_action[48] == pytest.approx(45.0)
+    assert final_action[49] == pytest.approx(-40.0)
+
+
+def test_scheduler_rejects_joint_projection_above_limit() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config())
+    scheduler.reset(
+        "session-1",
+        initial_action=_safe_actions(1)[0],
+    )
+    actions = _safe_actions(1)
+    actions[0, 7] = -3.11
+
+    with pytest.raises(ValueError, match="joint projection correction exceeds"):
+        scheduler.accept(_safe_chunk(actions), now_s=1.01)
+    assert not scheduler.has_chunk
+
+
+def test_scheduler_rejects_entire_unsafe_non_joint_chunk() -> None:
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config())
+    scheduler.reset(
+        "session-1",
+        initial_action=_safe_actions(1)[0],
+    )
+    actions = _safe_actions()
+    actions[1, 2] = 0.4
+
+    with pytest.raises(ValueError, match="root height"):
+        scheduler.accept(_safe_chunk(actions), now_s=1.01)
+    assert not scheduler.has_chunk
+
+
+def test_scheduler_accepts_discontinuous_plan_and_rate_limits_output_at_50hz() -> None:
+    scheduler = HighLevelPolicyScheduler(
+        hold_s=0.1,
+        safety=_safety_config(),
+        output_hz=50.0,
+    )
+    initial = _safe_actions(1)[0]
+    scheduler.reset("session-1", initial_action=initial)
+    actions = _safe_actions(2)
+    actions[1, 0] = 0.2
+    actions[1, 7] = 0.5
+    yaw = 0.2
+    actions[1, 3:7] = [math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)]
+    scheduler.accept(_safe_chunk(actions), now_s=1.01)
+
+    output = scheduler.sample(1.0 + 1.0 / 30.0)
+
+    assert output is not None
+    assert output[0] == pytest.approx(2.5 / 50.0)
+    assert output[7] == pytest.approx(10.0 / 50.0)
+    output_yaw = 2.0 * math.atan2(float(output[6]), float(output[3]))
+    assert output_yaw == pytest.approx(2.5 / 50.0, abs=1e-6)
+
+
+def test_policy_client_roundtrip_matches_current_messages() -> None:
+    context = zmq.Context()
+    endpoint = "inproc://teleopit-policy-client-roundtrip"
+    server = context.socket(zmq.REP)
+    server.bind(endpoint)
+    requests: list[dict[str, object]] = []
+
+    def serve() -> None:
+        for _ in range(3):
+            request = unpack_message(server.recv(), max_bytes=MAX_REQUEST_BYTES)
+            requests.append(request)
+            name = request["endpoint"]
+            if name == "describe":
+                data = {
+                    "observation_schema": "teleopit-g1-joint-pos-dex-neck-state",
+                    "observation_dim": 43,
+                    "action_schema": "teleopit-g1-reference",
+                    "action_dim": 50,
+                    "dataset_fps": 30,
+                    "max_action_horizon": 3,
+                    "policy_type": "replay",
+                    "policy_id": "test-policy",
+                    "ready": True,
+                }
+            elif name == "reset":
+                data = {"session_id": "session-1", "reset": True}
+            else:
+                observation = request["data"]
+                data = {
+                    "session_id": "session-1",
+                    "source_sequence_id": observation["sequence_id"],
+                    "source_onboard_monotonic_timestamp_ns": observation[
+                        "onboard_monotonic_timestamp_ns"
+                    ],
+                    "action_fps": 30,
+                    "actions": encode_float32_array(_safe_actions()),
+                    "policy_id": "test-policy",
+                    "server_inference_ms": 1.0,
+                }
+            server.send(
+                pack_message(
+                    {
+                        "endpoint": name,
+                        "ok": True,
+                        "data": data,
+                    },
+                    max_bytes=MAX_RESPONSE_BYTES,
+                )
+            )
+
+    thread = threading.Thread(target=serve)
+    thread.start()
+    client = HighLevelPolicyClient(endpoint, timeout_s=0.2, context=context)
+    try:
+        description = client.describe()
+        client.reset("session-1", "demo")
+        body_joint_positions = np.linspace(-0.2, 0.2, 29, dtype=np.float32)
+        dex_state = np.arange(12, dtype=np.float32) + 100.0
+        neck_state = np.array([5.0, -7.0], dtype=np.float32)
+        source_reference_root_pose = np.array(
+            [1.0, 2.0, 0.76, 0.9995, 0.0, 0.0, 0.0],
+            dtype=np.float32,
+        )
+        chunk = client.get_action(
+            session_id="session-1",
+            sequence_id=4,
+            onboard_monotonic_timestamp_ns=123,
+            task="demo",
+            jpeg_image=b"\xff\xd8test\xff\xd9",
+            body_joint_positions=body_joint_positions,
+            dex_state=dex_state,
+            neck_state=neck_state,
+            source_reference_root_pose=source_reference_root_pose,
+        )
+        assert description.policy_id == "test-policy"
+        np.testing.assert_allclose(chunk.actions, _safe_actions())
+        assert all(set(request) == {"endpoint", "data"} for request in requests)
+        get_action_data = requests[2]["data"]
+        assert isinstance(get_action_data, dict)
+        assert set(get_action_data) == {
+            "session_id",
+            "sequence_id",
+            "onboard_monotonic_timestamp_ns",
+            "task",
+            "image_encoding",
+            "image",
+            "body_joint_positions",
+            "dex_state",
+            "neck_state",
+            "source_reference_root_pose",
+        }
+        np.testing.assert_array_equal(
+            decode_float32_array(
+                get_action_data["body_joint_positions"],
+                name="body_joint_positions",
+                expected_shape=(29,),
+            ),
+            body_joint_positions,
+        )
+        np.testing.assert_array_equal(
+            decode_float32_array(
+                get_action_data["dex_state"],
+                name="dex_state",
+                expected_shape=(12,),
+            ),
+            dex_state,
+        )
+        np.testing.assert_array_equal(
+            decode_float32_array(
+                get_action_data["neck_state"],
+                name="neck_state",
+                expected_shape=(2,),
+            ),
+            neck_state,
+        )
+        np.testing.assert_array_equal(
+            decode_float32_array(
+                get_action_data["source_reference_root_pose"],
+                name="source_reference_root_pose",
+                expected_shape=(7,),
+            ),
+            source_reference_root_pose,
+        )
+    finally:
+        client.close()
+        thread.join(timeout=1.0)
+        server.close(linger=0)
+        context.term()
+
+
+def test_policy_client_rejects_extra_response_envelope_fields() -> None:
+    client = object.__new__(HighLevelPolicyClient)
+
+    with pytest.raises(PolicyProtocolError, match="exactly data"):
+        client._parse_reply(
+            {
+                "endpoint": "describe",
+                "ok": True,
+                "data": {},
+                "extra": "not allowed",
+            },
+            endpoint="describe",
+        )
+
+
+def test_high_level_policy_runtime_is_independent_from_pico_and_gmr() -> None:
+    started: list[str] = []
+
+    class FakeProcess:
+        def __init__(self, *, name: str, target, args) -> None:  # type: ignore[no-untyped-def]
+            del target, args
+            self.name = name
+
+        def start(self) -> None:
+            started.append(self.name)
+
+    runtime = object.__new__(HighLevelPolicySim2RealRuntime)
+    runtime.cfg = {
+        "hands": {"enabled": True},
+        "neck": {"enabled": True, "driver": "openneck"},
+    }
+    runtime._ctx = SimpleNamespace(Process=FakeProcess)
+    runtime._endpoints = SimpleNamespace()
+    runtime._stop_event = SimpleNamespace()
+    runtime._processes = []
+
+    runtime._start_processes()
+
+    assert started == ["camera", "high_level_policy", "robot_control", "policy_hand", "policy_neck"]
+    assert all("pico" not in name and "reference" not in name and "retarget" not in name for name in started)
+
+
+def test_high_level_policy_runtime_config_requires_safety_joint_limits() -> None:
+    cfg = {
+        "input": {"provider": "high_level_policy"},
+        "camera": {"source": "test-pattern", "width": 640, "height": 480, "fps": 30},
+        "high_level_policy": {"enabled": True, "task": "demo"},
+        "reference_steps": [0],
+        "recording": {"enabled": False},
+        "hands": {"enabled": True, "driver": "linkerhand_o6", "sides": ["left", "right"]},
+        "neck": {"enabled": True, "driver": "openneck"},
+        "real_robot": {},
+    }
+
+    with pytest.raises(ValueError, match="joint_pos_lower"):
+        _validate_high_level_policy_runtime_config(cfg)
+
+
+def test_standard_pico_runtime_rejects_high_level_policy_flag() -> None:
+    cfg = {
+        "input": {"provider": "pico4"},
+        "high_level_policy": {"enabled": True},
+    }
+
+    with pytest.raises(ValueError, match="independent.*run_high_level_policy_sim2real.py"):
+        Sim2RealRuntime(cfg)
+
+
+def test_high_level_policy_test_camera_is_exact_protocol_shape() -> None:
+    frame = _test_pattern(480, 640, 7)
+    assert frame.shape == (480, 640, 3)
+    assert frame.dtype == np.uint8
+    assert np.all(frame[:, :, 2] == 7)
+
+
+def test_realsense_recovery_stops_pipeline_before_hardware_reset() -> None:
+    calls: list[str] = []
+    pipeline = SimpleNamespace(stop=lambda: calls.append("stop"))
+    device = SimpleNamespace(hardware_reset=lambda: calls.append("hardware_reset"))
+
+    _stop_and_hardware_reset_realsense(pipeline, device)
+
+    assert calls == ["stop", "hardware_reset"]
+
+
+def test_openneck_policy_target_is_sent_directly_in_physical_degrees() -> None:
+    calls: list[tuple[float, float]] = []
+    device = SimpleNamespace(move_deg=lambda yaw, pitch: calls.append((yaw, pitch)))
+    action = _safe_actions(1)[0]
+    action[48:50] = [12.5, -7.25]
+    target = HighLevelPolicyTargetPacket(
+        session_id="session-1",
+        action=action,
+        timestamp_s=1.0,
+        seq=1,
+    )
+
+    _apply_policy_neck_target(device, target)
+
+    assert calls == [(12.5, -7.25)]
+
+
+def test_policy_hardware_target_requires_current_session_and_timestamp() -> None:
+    action = _safe_actions(1)[0]
+    target = HighLevelPolicyTargetPacket(
+        session_id="session-1",
+        action=action,
+        timestamp_s=10.0,
+        seq=5,
+    )
+    mode = ModeStatePacket(
+        mode="policy",
+        mocap_active=False,
+        mocap_paused=False,
+        timestamp_s=10.0,
+        seq=1,
+        policy_session_id="session-1",
+    )
+
+    assert _policy_target_is_current(
+        target,
+        mode,
+        last_target_seq=4,
+        max_age_s=0.2,
+        now_s=10.1,
+    )
+    assert not _policy_target_is_current(
+        target,
+        mode,
+        last_target_seq=5,
+        max_age_s=0.2,
+        now_s=10.1,
+    )
+    assert not _policy_target_is_current(
+        HighLevelPolicyTargetPacket(
+            session_id="old-session",
+            action=action,
+            timestamp_s=10.0,
+            seq=6,
+        ),
+        mode,
+        last_target_seq=4,
+        max_age_s=0.2,
+        now_s=10.1,
+    )
+    assert not _policy_target_is_current(
+        target,
+        mode,
+        last_target_seq=4,
+        max_age_s=0.2,
+        now_s=10.3,
+    )
+    assert not _policy_target_is_current(
+        target,
+        ModeStatePacket(
+            mode="policy",
+            mocap_active=False,
+            mocap_paused=False,
+            timestamp_s=10.0,
+            seq=2,
+            policy_paused=True,
+            policy_session_id="session-1",
+        ),
+        last_target_seq=4,
+        max_age_s=0.2,
+        now_s=10.1,
+    )
+
+
+def _remote(*, a: bool = False, b: bool = False, x: bool = False, y: bool = False):  # type: ignore[no-untyped-def]
+    button = lambda pressed=False: SimpleNamespace(on_pressed=pressed, pressed=pressed)
+    return SimpleNamespace(
+        A=button(a),
+        B=button(b),
+        X=button(x),
+        Y=button(y),
+        start=button(False),
+    )
+
+
+def test_high_level_policy_y_requests_takeover_without_starting_mode_state() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    worker.mode = RobotMode.STANDING
+    worker.remote = _remote(y=True)
+    worker._policy_entry_pending = False
+    worker._policy_paused = False
+    requests: list[str] = []
+
+    def begin() -> None:
+        requests.append("begin")
+        worker._policy_entry_pending = True
+
+    worker._begin_high_level_policy_entry = begin
+    worker._publish_high_level_policy_session = lambda *_args, **_kwargs: None
+    worker._policy_entry_deadline_s = None
+
+    worker._handle_high_level_policy_transitions()
+
+    assert requests == ["begin"]
+    assert worker.mode == RobotMode.STANDING
+
+
+def test_policy_transition_after_first_chunk_does_not_start_kp_ramp() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    worker.mode = RobotMode.STANDING
+    worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace())
+    resume_qpos = np.zeros(36, dtype=np.float64)
+    resume_qpos[3] = 1.0
+    worker._build_robot_state_qpos = lambda _state: resume_qpos.copy()
+    resets: list[str] = []
+    worker._reset_policy_state = lambda: resets.append("reset")
+    worker._last_retarget_qpos = np.ones(36, dtype=np.float64)
+    worker._last_commanded_motion_qpos = None
+    worker._policy_hold_qpos = None
+    worker._policy_entry_pending = True
+    worker._policy_entry_deadline_s = 2.0
+    worker._policy_paused = True
+    worker._policy_resume_pending = False
+    worker._policy_resume_deadline_s = None
+    worker._policy_resume_source_timestamp_ns = None
+    worker._standing_return_ramp_duration = 0.5
+    worker._standing_return_kp_ramp_floor_ratio = 0.5
+    worker._safety = SimpleNamespace(
+        start_kp_ramp=lambda **_kwargs: pytest.fail(
+            "POLICY transition must not start an entry Kp ramp"
+        )
+    )
+
+    worker._transition_to_high_level_policy()
+
+    assert worker.mode == RobotMode.POLICY
+    assert resets == ["reset"]
+    assert not worker._policy_entry_pending
+    assert not worker._policy_paused
+    np.testing.assert_array_equal(worker._policy_hold_qpos, resume_qpos)
+
+
+def test_policy_entry_rejects_action_received_after_deadline() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    now_s = time.monotonic()
+    worker.mode = RobotMode.STANDING
+    worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_action_sub = SimpleNamespace(
+        recv_latest=lambda: HighLevelPolicyActionPacket(
+            session_id="session-1",
+            source_sequence_id=1,
+            source_onboard_monotonic_timestamp_ns=int(round(now_s * 1e9)),
+            action_fps=30,
+            actions=_safe_actions(1),
+            policy_id="test",
+            server_inference_ms=1.0,
+            received_timestamp_s=now_s,
+        )
+    )
+    worker._last_policy_video_seq = -1
+    worker._last_policy_status_seq = -1
+    worker._policy_session_id = "session-1"
+    worker._policy_paused = False
+    worker._policy_resume_pending = False
+    worker._policy_resume_source_timestamp_ns = None
+    worker._policy_entry_pending = True
+    worker._policy_entry_deadline_s = now_s - 0.01
+    accepted: list[object] = []
+    worker._high_level_policy_scheduler = SimpleNamespace(
+        accept=lambda *args, **kwargs: accepted.append((args, kwargs))
+    )
+    worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0)
+    standing: list[str] = []
+    worker._enter_standing = lambda: standing.append("standing")
+    worker._transition_to_high_level_policy = lambda: pytest.fail(
+        "expired entry action must not enter POLICY"
+    )
+
+    worker._drain_high_level_policy_ipc()
+
+    assert standing == ["standing"]
+    assert accepted == []
+
+
+def test_policy_entry_first_chunk_uses_measured_reference_boundary() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    now_s = time.monotonic()
+    worker.mode = RobotMode.STANDING
+    worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_action_sub = SimpleNamespace(
+        recv_latest=lambda: HighLevelPolicyActionPacket(
+            session_id="session-1",
+            source_sequence_id=1,
+            source_onboard_monotonic_timestamp_ns=int(round(now_s * 1e9)),
+            action_fps=30,
+            actions=_safe_actions(1),
+            policy_id="test",
+            server_inference_ms=1.0,
+            received_timestamp_s=now_s,
+        )
+    )
+    worker._last_policy_video_seq = -1
+    worker._last_policy_status_seq = -1
+    worker._policy_session_id = "session-1"
+    worker._policy_paused = False
+    worker._policy_resume_pending = False
+    worker._policy_resume_source_timestamp_ns = None
+    worker._policy_entry_pending = True
+    worker._policy_entry_deadline_s = now_s + 1.0
+    worker._policy_frame_transform = PolicyFrameTransform.from_robot_pose(
+        [0.0, 0.0],
+        [1.0, 0.0, 0.0, 0.0],
+    )
+    current_qpos = np.zeros(36, dtype=np.float64)
+    current_qpos[2] = 0.76
+    current_qpos[3] = 1.0
+    current_qpos[7] = 0.8
+    state = SimpleNamespace()
+    worker.robot = SimpleNamespace(get_state=lambda: state)
+    worker._build_robot_state_qpos = lambda _state: current_qpos.copy()
+    scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config())
+    boundary_action = worker._build_high_level_policy_boundary_action(state)
+    scheduler.reset(
+        "session-1",
+        initial_action=boundary_action,
+    )
+    worker._high_level_policy_scheduler = scheduler
+    worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0)
+    worker._enter_standing = lambda: pytest.fail(
+        "valid first chunk must enter POLICY"
+    )
+    transitions: list[str] = []
+    worker._transition_to_high_level_policy = lambda: transitions.append("policy")
+
+    worker._drain_high_level_policy_ipc()
+
+    scheduled = scheduler.sample(now_s)
+    assert boundary_action[7] == pytest.approx(0.8)
+    assert current_qpos[7] == pytest.approx(0.8)
+    assert transitions == ["policy"]
+    assert scheduler.has_chunk
+    assert scheduled is not None
+    assert scheduled[7] == pytest.approx(0.6)
+
+
+def test_policy_session_seeds_source_history_from_active_reference() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    state = SimpleNamespace(
+        qpos=np.linspace(-0.2, 0.2, 29, dtype=np.float32),
+        quat=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32),
+        base_pos=np.array([10.0, 20.0, 0.76], dtype=np.float64),
+    )
+    worker.robot = SimpleNamespace(get_state=lambda: state)
+    worker.num_actions = 29
+    worker._default_root_pos = np.array([0.0, 0.0, 0.76], dtype=np.float64)
+    worker._high_level_policy_cfg = SimpleNamespace(entry_timeout_s=5.0)
+    scheduler = HighLevelPolicyScheduler()
+    worker._high_level_policy_scheduler = scheduler
+    worker._policy_session_id = None
+    worker._latest_policy_video = None
+    worker._last_commanded_motion_qpos = np.zeros(36, dtype=np.float64)
+    worker._last_commanded_motion_qpos[:7] = [
+        12.0,
+        24.0,
+        0.82,
+        1.0,
+        0.0,
+        0.0,
+        0.0,
+    ]
+    worker._standing_qpos = np.zeros(36, dtype=np.float64)
+    worker._standing_qpos[3] = 1.0
+    worker._publish_high_level_policy_session = lambda *_args, **_kwargs: None
+
+    worker._start_high_level_policy_entry_session()
+
+    source_pose = scheduler.reference_root_pose_at(time.monotonic())
+    assert source_pose is not None
+    np.testing.assert_allclose(source_pose[:3], [2.0, 4.0, 0.82], atol=1e-6)
+    np.testing.assert_array_equal(
+        worker._policy_hold_qpos,
+        worker._last_commanded_motion_qpos,
+    )
+
+
+def test_policy_observation_uses_active_reference_and_measured_hardware_state() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    now_s = time.monotonic()
+    frame = SharedFrameDescriptor(
+        shm_name="camera",
+        slot=0,
+        seq=7,
+        timestamp_s=now_s,
+        shape=(480, 640, 3),
+        dtype="uint8",
+        slots=3,
+    )
+    worker._policy_entry_pending = True
+    worker.mode = RobotMode.STANDING
+    worker._policy_paused = False
+    worker._policy_resume_pending = False
+    worker._policy_session_id = "session-1"
+    worker._high_level_policy_cfg = SimpleNamespace(max_observation_age_s=0.15)
+    worker._latest_policy_video = frame
+    worker._last_policy_video_seq = -1
+    worker._policy_observation_seq = 0
+    worker._policy_hand_state_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_neck_state_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._latest_policy_hand_state = HandCommandPacket(
+        timestamp_s=now_s,
+        driver="linkerhand_o6",
+        mode="policy",
+        active=False,
+        left_pose=np.full(6, 250.0, dtype=np.float32),
+        right_pose=np.full(6, 250.0, dtype=np.float32),
+        seq=1,
+        left_state=np.arange(6, dtype=np.float32) + 10.0,
+        right_state=np.arange(6, dtype=np.float32) + 20.0,
+    )
+    worker._latest_policy_neck_state = NeckCommandPacket(
+        timestamp_s=now_s,
+        driver="openneck",
+        active=False,
+        yaw_deg=0.0,
+        pitch_deg=0.0,
+        seq=1,
+        state_yaw_deg=12.0,
+        state_pitch_deg=-8.0,
+    )
+    scheduler = HighLevelPolicyScheduler()
+    initial_action = _safe_actions(1)[0]
+    active_reference = initial_action.copy()
+    active_reference[:7] = [4.0, 5.0, 0.82, 1.0, 0.0, 0.0, 0.0]
+    scheduler.reset(
+        "session-1",
+        initial_action=initial_action,
+        initial_reference=active_reference,
+        initial_timestamp_s=now_s,
+    )
+    worker._high_level_policy_scheduler = scheduler
+    published: list[tuple[str, object]] = []
+    worker._policy_control_pub = SimpleNamespace(
+        publish=lambda topic, packet: published.append((topic, packet))
+    )
+    robot_state = SimpleNamespace(
+        qpos=np.linspace(-0.2, 0.2, 29, dtype=np.float32),
+        qvel=np.zeros(29, dtype=np.float32),
+        quat=np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32),
+        ang_vel=np.zeros(3, dtype=np.float32),
+        base_pos=np.array([99.0, 98.0, 97.0], dtype=np.float32),
+    )
+
+    worker._publish_high_level_policy_observation(robot_state)
+
+    assert len(published) == 1
+    packet = published[0][1]
+    assert isinstance(packet, HighLevelPolicyObservationPacket)
+    np.testing.assert_array_equal(packet.body_joint_positions, robot_state.qpos)
+    np.testing.assert_array_equal(
+        packet.dex_state,
+        np.concatenate(
+            (
+                worker._latest_policy_hand_state.left_state,
+                worker._latest_policy_hand_state.right_state,
+            )
+        ),
+    )
+    np.testing.assert_array_equal(packet.neck_state, [12.0, -8.0])
+    np.testing.assert_array_equal(
+        packet.source_reference_root_pose,
+        active_reference[:7],
+    )
+    assert packet.source_reference_root_pose[0] != robot_state.base_pos[0]
+
+
+def test_policy_entry_stale_result_aborts_current_session() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    now_s = time.monotonic()
+    worker.mode = RobotMode.STANDING
+    worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_action_sub = SimpleNamespace(
+        recv_latest=lambda: HighLevelPolicyActionPacket(
+            session_id="session-1",
+            source_sequence_id=1,
+            source_onboard_monotonic_timestamp_ns=int(round(now_s * 1e9)),
+            action_fps=30,
+            actions=_safe_actions(1),
+            policy_id="test",
+            server_inference_ms=1.0,
+            received_timestamp_s=now_s - 1.0,
+        )
+    )
+    worker._last_policy_video_seq = -1
+    worker._last_policy_status_seq = -1
+    worker._policy_session_id = "session-1"
+    worker._policy_paused = False
+    worker._policy_resume_pending = False
+    worker._policy_resume_source_timestamp_ns = None
+    worker._policy_entry_pending = True
+    worker._policy_entry_deadline_s = now_s + 1.0
+    worker._high_level_policy_scheduler = SimpleNamespace()
+    worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=0.1)
+    standing: list[str] = []
+    worker._enter_standing = lambda: standing.append("standing")
+
+    worker._drain_high_level_policy_ipc()
+
+    assert standing == ["standing"]
+
+
+def test_policy_entry_cancel_from_standing_only_stops_session() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    worker.high_level_policy_enabled = True
+    worker.mode = RobotMode.STANDING
+    worker._policy_entry_pending = True
+    worker._mocap_entry_requested = False
+    stops: list[str] = []
+
+    def stop_session() -> None:
+        stops.append("stop")
+        worker._policy_entry_pending = False
+
+    worker._stop_high_level_policy_session = stop_session
+    worker._disarm_mocap_reference_if_needed = lambda: None
+    worker._clear_reference_gate = lambda: None
+    worker.robot = SimpleNamespace(
+        get_state=lambda: pytest.fail("entry cancel must not rebuild STANDING"),
+        lock_all_joints=lambda: pytest.fail(
+            "entry cancel must not lock joints or block the control loop"
+        ),
+    )
+
+    worker._enter_standing()
+
+    assert stops == ["stop"]
+    assert worker.mode == RobotMode.STANDING
+
+
+def test_high_level_policy_body_action_uses_existing_tracker_without_second_alignment() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    action = _safe_actions(1)[0]
+    worker._high_level_policy_scheduler = SimpleNamespace(sample=lambda _now: action.copy())
+    worker._policy_frame_transform = SimpleNamespace(
+        delocalize_body_action=lambda body: np.asarray(body, dtype=np.float32) + np.float32(1.0)
+    )
+    worker._policy_session_id = "session-1"
+    worker._policy_paused = False
+    worker._policy_resume_pending = False
+    worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace())
+    worker._publish_high_level_policy_observation = lambda _state: None
+    worker._policy_control_pub = None
+    worker._policy_hold_qpos = None
+    calls: list[tuple[np.ndarray, dict[str, object]]] = []
+
+    def execute(reference, _state, **kwargs) -> None:  # type: ignore[no-untyped-def]
+        calls.append((np.asarray(reference), kwargs))
+
+    worker._execute_reference_pipeline = execute
+
+    worker._high_level_policy_step()
+
+    assert len(calls) == 1
+    np.testing.assert_allclose(calls[0][0], action[:36] + 1.0)
+    assert calls[0][1] == {
+        "reference_window": None,
+        "align_reference": False,
+        "compose_arms": False,
+    }
+
+
+def test_policy_and_pico_remote_b_both_toggle_pause() -> None:
+    policy_worker = object.__new__(_RobotControlWorker)
+    policy_worker.mode = RobotMode.POLICY
+    policy_worker.remote = _remote(b=True)
+    policy_toggles: list[str] = []
+    policy_worker._toggle_high_level_policy_pause = lambda: policy_toggles.append("policy")
+    policy_worker._handle_high_level_policy_transitions()
+
+    pico_worker = object.__new__(_RobotControlWorker)
+    pico_worker.mode = RobotMode.MOCAP
+    pico_worker.provider_kind = "pico4"
+    pico_worker.remote = _remote(b=True)
+    pico_worker._mocap_session = SimpleNamespace(state=MocapSessionState.ACTIVE)
+    pico_commands: list[str] = []
+    pico_worker._send_reference_command = pico_commands.append
+    pico_worker._pause_active_mocap = lambda: pico_commands.append("paused")
+    pico_worker._handle_transitions()
+
+    assert policy_toggles == ["policy"]
+    assert pico_commands == ["pause_mocap", "paused"]
+
+
+def test_policy_worker_pause_resume_retransmission_is_idempotent() -> None:
+    worker = object.__new__(HighLevelPolicyWorker)
+    worker._last_session_seq = -1
+    worker._active_session = None
+    worker._ready = False
+    worker._paused = False
+    worker._last_observation_seq = -1
+    worker._last_request_timestamp_ns = None
+    worker._next_connect_time_s = 0.0
+    worker._new_session_required = False
+    statuses: list[str] = []
+    worker._publish_status = lambda status, _detail: statuses.append(status)
+
+    def packet(command: str, seq: int) -> HighLevelPolicySessionPacket:
+        return HighLevelPolicySessionPacket(
+            session_id="session-1",
+            task="demo",
+            command=command,
+            timestamp_s=1.0,
+            seq=seq,
+        )
+
+    worker._handle_session(packet("start", 1))
+    worker._ready = True
+    worker._handle_session(packet("pause", 2))
+    worker._handle_session(packet("pause", 3))
+    worker._handle_session(packet("resume", 4))
+    worker._handle_session(packet("resume", 5))
+
+    assert statuses == ["connecting", "paused", "ready"]
+
+
+def test_policy_worker_resume_reconnects_faulted_current_session() -> None:
+    worker = object.__new__(HighLevelPolicyWorker)
+    worker._last_session_seq = -1
+    worker._active_session = None
+    worker._ready = False
+    worker._paused = False
+    worker._last_observation_seq = -1
+    worker._last_request_timestamp_ns = None
+    worker._next_connect_time_s = 0.0
+    worker._new_session_required = False
+    statuses: list[str] = []
+    worker._publish_status = lambda status, _detail: statuses.append(status)
+
+    def packet(command: str, seq: int) -> HighLevelPolicySessionPacket:
+        return HighLevelPolicySessionPacket(
+            session_id="session-1",
+            task="demo",
+            command=command,
+            timestamp_s=1.0,
+            seq=seq,
+        )
+
+    worker._handle_session(packet("start", 1))
+    worker._new_session_required = True
+    worker._handle_session(packet("pause", 2))
+    worker._handle_session(packet("resume", 3))
+
+    assert statuses == ["connecting", "paused", "connecting"]
+    assert not worker._paused
+    assert not worker._new_session_required
+
+
+def test_policy_worker_replans_on_configured_source_frame_stride(monkeypatch) -> None:
+    worker = object.__new__(HighLevelPolicyWorker)
+    worker._active_session = HighLevelPolicySessionPacket(
+        session_id="session-1",
+        task="demo",
+        command="start",
+        timestamp_s=time.monotonic(),
+        seq=1,
+    )
+    worker._ready = True
+    worker._paused = False
+    worker._last_observation_seq = -1
+    worker._last_request_timestamp_ns = 1_000_000_000
+    worker.policy_cfg = SimpleNamespace(
+        replan_steps=3,
+        max_observation_age_s=0.15,
+        jpeg_quality=90,
+    )
+    requests: list[dict[str, object]] = []
+
+    def get_action(**kwargs):  # type: ignore[no-untyped-def]
+        requests.append(kwargs)
+        return _chunk(
+            source_s=int(kwargs["onboard_monotonic_timestamp_ns"]) * 1e-9,
+            sequence=int(kwargs["sequence_id"]),
+        )
+
+    worker._client = SimpleNamespace(get_action=get_action)
+    worker._policy_id = "test"
+    worker._frame_reader = SimpleNamespace(
+        read=lambda _descriptor, copy: np.zeros((480, 640, 3), dtype=np.uint8)
+    )
+    published: list[object] = []
+    worker._result_pub = SimpleNamespace(
+        publish=lambda _topic, packet: published.append(packet)
+    )
+    monkeypatch.setattr(
+        "teleopit.sim2real.mp.high_level_policy_worker.encode_policy_jpeg",
+        lambda _frame, quality: b"jpeg",
+    )
+
+    def observation(sequence_id: int, timestamp_ns: int) -> HighLevelPolicyObservationPacket:
+        return HighLevelPolicyObservationPacket(
+            session_id="session-1",
+            sequence_id=sequence_id,
+            onboard_monotonic_timestamp_ns=timestamp_ns,
+            body_joint_positions=np.arange(29, dtype=np.float32),
+            dex_state=np.arange(12, dtype=np.float32) + 100.0,
+            neck_state=np.array([5.0, -2.0], dtype=np.float32),
+            source_reference_root_pose=np.array(
+                [1.0, 2.0, 0.76, 1.0, 0.0, 0.0, 0.0],
+                dtype=np.float32,
+            ),
+            frame=object(),  # type: ignore[arg-type]
+            timestamp_s=time.monotonic(),
+        )
+
+    worker._handle_observation(observation(1, 1_099_999_999))
+    worker._handle_observation(observation(2, 1_100_000_000))
+
+    assert len(requests) == 1
+    assert requests[0]["sequence_id"] == 2
+    np.testing.assert_array_equal(
+        requests[0]["body_joint_positions"],
+        np.arange(29, dtype=np.float32),
+    )
+    np.testing.assert_array_equal(
+        requests[0]["dex_state"],
+        np.arange(12, dtype=np.float32) + 100.0,
+    )
+    np.testing.assert_array_equal(
+        requests[0]["neck_state"],
+        np.array([5.0, -2.0], dtype=np.float32),
+    )
+    np.testing.assert_array_equal(
+        requests[0]["source_reference_root_pose"],
+        np.array([1.0, 2.0, 0.76, 1.0, 0.0, 0.0, 0.0], dtype=np.float32),
+    )
+    assert worker._last_request_timestamp_ns == 1_100_000_000
+    assert len(published) == 1
+    assert published[0].source_onboard_monotonic_timestamp_ns == 1_100_000_000
+
+
+def test_policy_fault_uses_normal_pause_without_entering_standing() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    worker.high_level_policy_enabled = True
+    worker.mode = RobotMode.POLICY
+    worker._policy_paused = False
+    worker._policy_resume_pending = False
+    worker._policy_resume_deadline_s = None
+    worker._policy_resume_source_timestamp_ns = None
+    paused: list[float] = []
+    worker._high_level_policy_scheduler = SimpleNamespace(
+        pause=lambda now_s: paused.append(float(now_s))
+    )
+    hold_qpos = np.arange(36, dtype=np.float64)
+    worker._resolve_mocap_hold_qpos = lambda: hold_qpos.copy()
+    published: list[str] = []
+    worker._publish_high_level_policy_session = published.append
+    worker._enter_standing = lambda: pytest.fail("fault must not enter STANDING")
+
+    worker._handle_high_level_policy_fault("network timeout")
+
+    assert worker.mode == RobotMode.POLICY
+    assert worker._policy_paused
+    assert not worker._policy_resume_pending
+    assert len(paused) == 1
+    assert published == ["pause"]
+    np.testing.assert_array_equal(worker._policy_hold_qpos, hold_qpos)
+
+
+def test_policy_watchdog_pauses_and_holds_last_reference() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    worker.high_level_policy_enabled = True
+    worker.mode = RobotMode.POLICY
+    worker._policy_paused = False
+    worker._policy_resume_pending = False
+    worker._policy_resume_deadline_s = None
+    worker._policy_resume_source_timestamp_ns = None
+    paused: list[float] = []
+    worker._high_level_policy_scheduler = SimpleNamespace(
+        sample=lambda _now_s: None,
+        pause=lambda now_s: paused.append(float(now_s)),
+    )
+    worker._policy_frame_transform = SimpleNamespace()
+    worker._policy_session_id = "session-1"
+    worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace())
+    worker._publish_high_level_policy_observation = lambda _state: None
+    hold_qpos = np.arange(36, dtype=np.float64)
+    worker._last_commanded_motion_qpos = hold_qpos.copy()
+    worker._last_retarget_qpos = None
+    worker._publish_high_level_policy_session = lambda _command: None
+    held: list[np.ndarray] = []
+    worker._run_static_mocap_step = lambda qpos: held.append(np.asarray(qpos).copy())
+
+    worker._high_level_policy_step()
+
+    assert worker.mode == RobotMode.POLICY
+    assert worker._policy_paused
+    assert not worker._policy_resume_pending
+    assert len(paused) == 1
+    assert len(held) == 1
+    np.testing.assert_array_equal(held[0], hold_qpos)
+
+
+def test_current_policy_fault_status_is_handled_even_while_paused() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_action_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_status_sub = SimpleNamespace(
+        recv_latest=lambda: HighLevelPolicyStatusPacket(
+            session_id="session-1",
+            status="fault",
+            detail="network timeout",
+            timestamp_s=1.0,
+            seq=1,
+        )
+    )
+    worker._last_policy_video_seq = -1
+    worker._last_policy_status_seq = -1
+    worker._policy_session_id = "session-1"
+    worker._policy_paused = True
+    worker.mode = RobotMode.POLICY
+    handled: list[str] = []
+    worker._handle_high_level_policy_fault = handled.append
+
+    worker._drain_high_level_policy_ipc()
+
+    assert handled == ["network timeout"]
+
+
+def test_policy_pause_resume_waits_for_fresh_chunk_then_resumes() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    worker.mode = RobotMode.POLICY
+    worker._policy_session_id = "session-1"
+    worker._policy_paused = True
+    worker._policy_resume_pending = False
+    worker._policy_resume_deadline_s = None
+    worker._policy_resume_source_timestamp_ns = None
+    resumed: list[float] = []
+    accepted: list[object] = []
+    worker._high_level_policy_scheduler = SimpleNamespace(
+        accept=lambda *args, **kwargs: accepted.append((args, kwargs)),
+        resume=lambda now_s: resumed.append(float(now_s)),
+    )
+    worker._high_level_policy_cfg = SimpleNamespace(
+        entry_timeout_s=1.0,
+        max_result_age_s=0.1,
+    )
+    session_commands: list[str] = []
+    worker._publish_high_level_policy_session = session_commands.append
+
+    worker._toggle_high_level_policy_pause()
+
+    assert worker._policy_paused
+    assert worker._policy_resume_pending
+    assert session_commands == ["resume"]
+
+    source_timestamp_ns = worker._policy_resume_source_timestamp_ns
+    assert source_timestamp_ns is not None
+    worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_action_sub = SimpleNamespace(
+        recv_latest=lambda: HighLevelPolicyActionPacket(
+            session_id="session-1",
+            source_sequence_id=1,
+            source_onboard_monotonic_timestamp_ns=source_timestamp_ns,
+            action_fps=30,
+            actions=_safe_actions(1),
+            policy_id="test",
+            server_inference_ms=1.0,
+            received_timestamp_s=time.monotonic(),
+        )
+    )
+    worker._last_policy_video_seq = -1
+    worker._last_policy_status_seq = -1
+
+    worker._drain_high_level_policy_ipc()
+
+    assert len(accepted) == 1
+    assert len(resumed) == 1
+    assert not worker._policy_paused
+    assert not worker._policy_resume_pending
+
+
+def test_policy_resume_rejects_action_received_after_deadline() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    now_s = time.monotonic()
+    worker.mode = RobotMode.POLICY
+    worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_action_sub = SimpleNamespace(
+        recv_latest=lambda: HighLevelPolicyActionPacket(
+            session_id="session-1",
+            source_sequence_id=1,
+            source_onboard_monotonic_timestamp_ns=int(round(now_s * 1e9)),
+            action_fps=30,
+            actions=_safe_actions(1),
+            policy_id="test",
+            server_inference_ms=1.0,
+            received_timestamp_s=now_s,
+        )
+    )
+    worker._last_policy_video_seq = -1
+    worker._last_policy_status_seq = -1
+    worker._policy_session_id = "session-1"
+    worker._policy_paused = True
+    worker._policy_resume_pending = True
+    worker._policy_resume_deadline_s = now_s - 0.01
+    worker._policy_resume_source_timestamp_ns = int(round((now_s - 0.1) * 1e9))
+    accepted: list[object] = []
+    worker._high_level_policy_scheduler = SimpleNamespace(
+        accept=lambda *args, **kwargs: accepted.append((args, kwargs)),
+        resume=lambda _now_s: pytest.fail("expired resume must not resume scheduler"),
+    )
+    worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0)
+    faults: list[str] = []
+    worker._handle_high_level_policy_fault = faults.append
+
+    worker._drain_high_level_policy_ipc()
+
+    assert faults == ["resume timed out waiting for a fresh action chunk"]
+    assert accepted == []
+
+
+def test_paused_robot_worker_discards_inflight_policy_result() -> None:
+    worker = object.__new__(_RobotControlWorker)
+    worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None)
+    worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None)
+    action = _safe_actions(1)
+    worker._policy_action_sub = SimpleNamespace(
+        recv_latest=lambda: HighLevelPolicyActionPacket(
+            session_id="session-1",
+            source_sequence_id=1,
+            source_onboard_monotonic_timestamp_ns=1,
+            action_fps=30,
+            actions=action,
+            policy_id="test",
+            server_inference_ms=1.0,
+            received_timestamp_s=1.0,
+        )
+    )
+    worker._last_policy_video_seq = -1
+    worker._last_policy_status_seq = -1
+    worker._policy_session_id = "session-1"
+    worker._policy_paused = True
+    worker._policy_resume_pending = False
+    accepted: list[object] = []
+    worker._high_level_policy_scheduler = SimpleNamespace(
+        accept=lambda *args, **kwargs: accepted.append((args, kwargs))
+    )
+    worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=0.1)
+
+    worker._drain_high_level_policy_ipc()
+
+    assert accepted == []
diff --git a/tests/test_pico4_provider.py b/tests/test_pico4_provider.py
index e785aaae..ae98f7a9 100644
--- a/tests/test_pico4_provider.py
+++ b/tests/test_pico4_provider.py
@@ -28,10 +28,16 @@ def _pico_frame(
     body_active: bool = True,
     right_primary: bool = False,
     right_secondary: bool = False,
+    head_rotation_xyzw: np.ndarray | None = None,
 ) -> SimpleNamespace:
     return SimpleNamespace(
         seq=seq,
         receive_time_s=timestamp,
+        head=(
+            None
+            if head_rotation_xyzw is None
+            else SimpleNamespace(rotation=np.asarray(head_rotation_xyzw, dtype=np.float64))
+        ),
         body=SimpleNamespace(active=body_active, joints=body_poses),
         controllers=SimpleNamespace(
             left=SimpleNamespace(buttons={}),
@@ -70,6 +76,7 @@ def _make_provider() -> Pico4InputProvider:
     provider._ground_alignment_offset = None
     provider._controller_snapshot = None
     provider._hand_snapshot = None
+    provider._head_pose_snapshot = None
     provider._closed = False
     return provider
 
@@ -345,3 +352,93 @@ def test_pico4_provider_exposes_hand_snapshot_when_body_inactive() -> None:
     assert snapshot.right.present is True
     assert snapshot.right.active is False
     np.testing.assert_allclose(snapshot.left.joints[:, 0:3], 1.5)
+
+
+def test_pico4_provider_exposes_hmd_rotation_separately_from_skeleton_head() -> None:
+    provider = _make_provider()
+    body_poses = _body_poses(1.0)
+    angle = np.deg2rad(30.0)
+    hmd_rotation_xyzw = np.array(
+        [0.0, np.sin(angle / 2.0), 0.0, np.cos(angle / 2.0)],
+        dtype=np.float64,
+    )
+
+    assert provider._accept_pico_frame(
+        _pico_frame(
+            body_poses,
+            seq=7,
+            timestamp=3.0,
+            head_rotation_xyzw=hmd_rotation_xyzw,
+        )
+    ) is True
+
+    snapshot = provider.get_head_pose_snapshot()
+    assert snapshot is not None
+    assert snapshot.seq == 7
+    assert snapshot.timestamp_s == pytest.approx(3.0)
+    expected_body = body_poses.copy()
+    expected_body[BODY_JOINT_NAMES.index("Head"), 3:7] = hmd_rotation_xyzw
+    expected_frame = Pico4InputProvider._convert_body_joints_to_frame(expected_body)
+    np.testing.assert_allclose(
+        snapshot.hmd_rotation_wxyz,
+        expected_frame["Head"][1],
+        atol=1e-6,
+    )
+    np.testing.assert_allclose(
+        snapshot.spine3_rotation_wxyz,
+        expected_frame["Spine3"][1],
+        atol=1e-6,
+    )
+    skeleton_frame = Pico4InputProvider._convert_body_joints_to_frame(body_poses)
+    assert not np.allclose(snapshot.hmd_rotation_wxyz, skeleton_frame["Head"][1])
+
+
+def test_pico4_provider_updates_hmd_snapshot_when_duplicate_body_is_dropped() -> None:
+    provider = _make_provider()
+    body_poses = _body_poses(1.0)
+
+    assert provider._accept_pico_frame(
+        _pico_frame(
+            body_poses,
+            seq=1,
+            timestamp=1.0,
+            head_rotation_xyzw=np.array([0.0, 0.0, 0.0, 1.0]),
+        )
+    ) is True
+    first = provider.get_head_pose_snapshot()
+    assert first is not None
+
+    angle = np.deg2rad(20.0)
+    assert provider._accept_pico_frame(
+        _pico_frame(
+            body_poses.copy(),
+            seq=2,
+            timestamp=1.01,
+            head_rotation_xyzw=np.array([np.sin(angle / 2.0), 0.0, 0.0, np.cos(angle / 2.0)]),
+        )
+    ) is False
+    second = provider.get_head_pose_snapshot()
+
+    assert second is not None
+    assert second.seq == 2
+    assert second.timestamp_s == pytest.approx(1.01)
+    assert not np.allclose(second.hmd_rotation_wxyz, first.hmd_rotation_wxyz)
+
+
+def test_pico4_provider_invalidates_spine3_when_body_tracking_is_inactive() -> None:
+    provider = _make_provider()
+
+    assert provider._accept_pico_frame(
+        _pico_frame(
+            _body_poses(1.0),
+            seq=4,
+            timestamp=2.0,
+            body_active=False,
+            head_rotation_xyzw=np.array([0.0, 0.0, 0.0, 1.0]),
+        )
+    ) is False
+
+    snapshot = provider.get_head_pose_snapshot()
+    assert snapshot is not None
+    assert snapshot.hmd_rotation_wxyz is not None
+    assert snapshot.spine3_rotation_wxyz is None
diff --git a/tests/test_pico_video.py b/tests/test_pico_video.py
index 6e0d3cb7..0748f352 100644
--- a/tests/test_pico_video.py
+++ b/tests/test_pico_video.py
@@ -39,7 +39,7 @@ def test_pico_video_config_rejects_enabled_unknown_source() -> None:
         parse_pico_video_config({"video": {"enabled": True, "source": "webcam"}})
 
 
-def test_realsense_video_runtime_pushes_rgb_frames(monkeypatch: pytest.MonkeyPatch) -> None:
+def test_realsense_video_runtime_reconnects_after_frame_timeout(monkeypatch: pytest.MonkeyPatch) -> None:
     fake_rs = ModuleType("pyrealsense2")
     fake_rs.stream = SimpleNamespace(color="color")
     fake_rs.format = SimpleNamespace(rgb8="rgb8")
@@ -59,12 +59,23 @@ class FakeFrames:
         def get_color_frame(self) -> FakeColorFrame:
             return FakeColorFrame()
 
+    pipeline_instances = 0
+    wait_calls = 0
+
     class FakePipeline:
+        def __init__(self) -> None:
+            nonlocal pipeline_instances
+            pipeline_instances += 1
+
         def start(self, _config: object) -> None:
             pass
 
-        def wait_for_frames(self) -> FakeFrames:
+        def wait_for_frames(self, _timeout_ms: int) -> FakeFrames:
+            nonlocal wait_calls
+            wait_calls += 1
             time.sleep(0.005)
+            if wait_calls == 1:
+                raise RuntimeError("Frame didn't arrive within 5000")
             return FakeFrames()
 
         def stop(self) -> None:
@@ -73,18 +84,31 @@ def stop(self) -> None:
     fake_rs.config = FakeConfig
     fake_rs.pipeline = FakePipeline
     monkeypatch.setitem(sys.modules, "pyrealsense2", fake_rs)
+    monkeypatch.setattr(pico_video._RealSenseVideoProducer, "_RECONNECT_DELAY_S", 0.005)
 
     sink = _FrameSink()
-    config = parse_pico_video_config({"video": {"enabled": True, "source": "realsense", "width": 3, "height": 2}})
-    runtime = PicoVideoRuntime(provider=sink, config=config, mode="sim2real")
+    config = parse_pico_video_config(
+        {
+            "video": {
+                "enabled": True,
+                "source": "realsense",
+                "width": 3,
+                "height": 2,
+            }
+        }
+    )
+    runtime = PicoVideoRuntime(provider=sink, config=config)
 
     runtime.start()
-    time.sleep(0.03)
+    deadline = time.monotonic() + 0.5
+    while not sink.frames and time.monotonic() < deadline:
+        time.sleep(0.005)
     runtime.stop()
 
     assert sink.frames
     assert sink.frames[-1].shape == (2, 3, 3)
     assert sink.frames[-1].dtype == np.uint8
+    assert pipeline_instances >= 2
 
 
 def test_realsense_video_runtime_invokes_frame_callback(monkeypatch: pytest.MonkeyPatch) -> None:
@@ -108,7 +132,7 @@ class FakePipeline:
         def start(self, _config: object) -> None:
             pass
 
-        def wait_for_frames(self) -> FakeFrames:
+        def wait_for_frames(self, _timeout_ms: int) -> FakeFrames:
             time.sleep(0.005)
             return FakeFrames()
 
@@ -125,7 +149,6 @@ def stop(self) -> None:
     runtime = PicoVideoRuntime(
         provider=sink,
         config=config,
-        mode="sim2real",
         frame_callback=lambda frame, _timestamp_s: callback_frames.append(frame.copy()),
     )
 
@@ -159,7 +182,7 @@ def stop(self) -> None:
 
     sink = _FrameSink()
     config = parse_pico_video_config({"video": {"enabled": True, "source": "realsense"}})
-    runtime = PicoVideoRuntime(provider=sink, config=config, mode="sim2real")
+    runtime = PicoVideoRuntime(provider=sink, config=config)
 
     runtime.start()
 
@@ -167,7 +190,7 @@ def stop(self) -> None:
     assert sink.frames == []
 
 
-def test_video_runtime_stops_producer_before_reraising_tick_error(monkeypatch: pytest.MonkeyPatch) -> None:
+def test_video_runtime_stops_producer_and_isolates_tick_error(monkeypatch: pytest.MonkeyPatch) -> None:
     stopped = False
 
     class FailingProducer:
@@ -187,18 +210,49 @@ def stop(self) -> None:
     monkeypatch.setattr(pico_video, "_RealSenseVideoProducer", FailingProducer)
 
     sink = _FrameSink()
-    config = parse_pico_video_config(
-        {"video": {"enabled": True, "source": "realsense", "fail_on_error": True}}
-    )
-    runtime = PicoVideoRuntime(provider=sink, config=config, mode="sim2real")
+    config = parse_pico_video_config({"video": {"enabled": True, "source": "realsense"}})
+    runtime = PicoVideoRuntime(provider=sink, config=config)
 
     runtime.start()
-    with pytest.raises(RuntimeError, match="Pico video pipeline failed"):
-        runtime.tick()
+    runtime.tick()
 
     assert stopped is True
 
 
+def test_realsense_video_start_failure_does_not_stop_unstarted_pipeline(monkeypatch: pytest.MonkeyPatch) -> None:
+    fake_rs = ModuleType("pyrealsense2")
+    fake_rs.stream = SimpleNamespace(color="color")
+    fake_rs.format = SimpleNamespace(rgb8="rgb8")
+    stop_calls = 0
+
+    class FakeConfig:
+        def enable_stream(self, *_args: object) -> None:
+            pass
+
+    class FakePipeline:
+        def start(self, _config: object) -> None:
+            raise RuntimeError("no device connected")
+
+        def stop(self) -> None:
+            nonlocal stop_calls
+            stop_calls += 1
+
+    fake_rs.config = FakeConfig
+    fake_rs.pipeline = FakePipeline
+    monkeypatch.setitem(sys.modules, "pyrealsense2", fake_rs)
+    monkeypatch.setattr(pico_video._RealSenseVideoProducer, "_STARTUP_WAIT_S", 0.02)
+    monkeypatch.setattr(pico_video._RealSenseVideoProducer, "_RECONNECT_DELAY_S", 0.005)
+
+    sink = _FrameSink()
+    config = parse_pico_video_config({"video": {"enabled": True, "source": "realsense"}})
+    runtime = PicoVideoRuntime(provider=sink, config=config)
+
+    runtime.start()
+    runtime.stop()
+
+    assert stop_calls == 0
+
+
 def test_mujoco_video_runtime_renders_camera_frame(monkeypatch: pytest.MonkeyPatch) -> None:
     fake_mujoco = ModuleType("mujoco")
     fake_mujoco.mjtObj = SimpleNamespace(mjOBJ_CAMERA="camera")
@@ -223,7 +277,7 @@ def close(self) -> None:
     sink = _FrameSink()
     robot = SimpleNamespace(model=object(), data=object())
     config = parse_pico_video_config({"video": {"enabled": True, "source": "mujoco", "width": 4, "height": 3}})
-    runtime = PicoVideoRuntime(provider=sink, config=config, mode="sim2sim", robot=robot)
+    runtime = PicoVideoRuntime(provider=sink, config=config, robot=robot)
 
     runtime.start()
     runtime.tick()
diff --git a/tests/test_recording_viewer.py b/tests/test_recording_viewer.py
new file mode 100644
index 00000000..6e802264
--- /dev/null
+++ b/tests/test_recording_viewer.py
@@ -0,0 +1,170 @@
+from __future__ import annotations
+
+import json
+from pathlib import Path
+import sys
+
+import h5py
+import numpy as np
+import pytest
+
+_PROJECT_ROOT = str(Path(__file__).resolve().parents[1])
+if _PROJECT_ROOT not in sys.path:
+    sys.path.insert(0, _PROJECT_ROOT)
+
+from scripts.view.view_recording import (
+    aligned_qpos_pair,
+    load_episode_review_data,
+    load_recording_dataset,
+)
+from teleopit.recording.hdf5 import (
+    ACTION_KEY,
+    FRAME_INDEX_KEY,
+    HAND_ACTION_KEY,
+    HAND_STATE_KEY,
+    MODE_KEY,
+    NECK_ACTION_KEY,
+    NECK_STATE_KEY,
+    RecordingSchema,
+    STATE_KEY,
+    TIMESTAMP_KEY,
+    hdf5_schema,
+)
+
+
+def _write_recording(
+    root: Path,
+    *,
+    frames: int = 4,
+    manifest_frames: int | None = None,
+    hand_type: str = "none",
+    neck_type: str = "none",
+) -> None:
+    root.mkdir(parents=True, exist_ok=True)
+    schema = RecordingSchema(
+        fps=30,
+        robot_type="unitree_g1_29dof",
+        hand_type=hand_type,
+        neck_type=neck_type,
+        image_key="observation.images.d435i_rgb",
+        image_shape=(4, 6, 3),
+    )
+    (root / "schema.json").write_text(
+        json.dumps(hdf5_schema(schema)),
+        encoding="utf-8",
+    )
+    data_path = root / "data" / "episode_000000.h5"
+    data_path.parent.mkdir()
+    state = np.zeros((frames, 68), dtype=np.float32)
+    state[:, 58] = 1.0
+    action = np.zeros((frames, 36), dtype=np.float32)
+    action[:, :3] = np.array([1.0, 2.0, 0.8], dtype=np.float32)
+    action[:, 3] = 1.0
+    action[:, 7:] = 0.1
+    with h5py.File(data_path, "w") as h5:
+        h5.create_dataset(FRAME_INDEX_KEY, data=np.arange(frames, dtype=np.int64))
+        h5.create_dataset(TIMESTAMP_KEY, data=np.arange(frames, dtype=np.float64) / 30.0)
+        h5.create_dataset(STATE_KEY, data=state)
+        h5.create_dataset(MODE_KEY, data=np.ones(frames, dtype=np.int8))
+        h5.create_dataset(ACTION_KEY, data=action)
+        if hand_type != "none":
+            h5.create_dataset(HAND_STATE_KEY, data=np.full((frames, 12), 20.0, dtype=np.float32))
+            h5.create_dataset(HAND_ACTION_KEY, data=np.zeros((frames, 12), dtype=np.float32))
+        if neck_type != "none":
+            h5.create_dataset(
+                NECK_STATE_KEY,
+                data=np.tile(np.array([11.5, -7.5], dtype=np.float32), (frames, 1)),
+            )
+            h5.create_dataset(
+                NECK_ACTION_KEY,
+                data=np.tile(np.array([12.5, -8.0], dtype=np.float32), (frames, 1)),
+            )
+
+    video_path = root / "videos" / "d435i_rgb" / "episode_000000.mp4"
+    video_path.parent.mkdir(parents=True)
+    video_path.touch()
+    manifest = {
+        "episode_index": 0,
+        "frames": frames if manifest_frames is None else manifest_frames,
+        "task": "test task",
+        "data": "data/episode_000000.h5",
+        "videos": {
+            "observation.images.d435i_rgb": "videos/d435i_rgb/episode_000000.mp4"
+        },
+    }
+    (root / "episodes.jsonl").write_text(json.dumps(manifest) + "\n", encoding="utf-8")
+
+
+def test_recording_viewer_loads_schema_episode_and_tracking_metrics(tmp_path: Path) -> None:
+    _write_recording(
+        tmp_path,
+        hand_type="linkerhand_o6",
+        neck_type="openneck",
+    )
+
+    dataset = load_recording_dataset(tmp_path)
+    data = load_episode_review_data(dataset, dataset.episodes[0])
+
+    assert dataset.fps == 30
+    assert dataset.image_shape == (4, 6, 3)
+    assert dataset.has_hand_action is True
+    assert dataset.has_neck_action is True
+    assert data.hand_state is not None
+    assert data.hand_action is not None
+    assert data.neck_state is not None
+    assert data.neck_action is not None
+    np.testing.assert_allclose(data.hand_state[0], 20.0)
+    np.testing.assert_allclose(data.neck_state[0], [11.5, -7.5])
+    assert data.joint_rmse_rad == pytest.approx(0.1)
+    assert data.root_orientation_rmse_rad == pytest.approx(0.0)
+    assert data.max_joint_error_rad == pytest.approx(0.1)
+    assert set(data.group_error) == {
+        "left leg",
+        "right leg",
+        "waist",
+        "left arm",
+        "right arm",
+    }
+    assert all(np.allclose(values, 0.1) for values in data.group_error.values())
+
+
+def test_recording_viewer_aligns_observed_root_position_to_reference(tmp_path: Path) -> None:
+    _write_recording(tmp_path)
+    dataset = load_recording_dataset(tmp_path)
+    data = load_episode_review_data(dataset, dataset.episodes[0])
+
+    actual_qpos, reference_qpos = aligned_qpos_pair(data, 0)
+
+    np.testing.assert_allclose(actual_qpos[:3], [1.0, 2.0, 0.8])
+    np.testing.assert_allclose(reference_qpos[:3], [1.0, 2.0, 0.8])
+    np.testing.assert_allclose(actual_qpos[7:], 0.0)
+    np.testing.assert_allclose(reference_qpos[7:], 0.1)
+
+
+def test_recording_viewer_rejects_manifest_hdf5_frame_mismatch(tmp_path: Path) -> None:
+    _write_recording(tmp_path, frames=4, manifest_frames=5)
+
+    with pytest.raises(ValueError, match="shape .* != manifest/schema shape"):
+        load_recording_dataset(tmp_path)
+
+
+def test_recording_viewer_rejects_non_finite_episode_data(tmp_path: Path) -> None:
+    _write_recording(tmp_path)
+    data_path = tmp_path / "data" / "episode_000000.h5"
+    with h5py.File(data_path, "r+") as h5:
+        h5[STATE_KEY][2, 0] = np.nan
+
+    dataset = load_recording_dataset(tmp_path)
+    with pytest.raises(ValueError, match="contains NaN or Inf"):
+        load_episode_review_data(dataset, dataset.episodes[0])
+
+
+def test_recording_viewer_rejects_hdf5_dtype_mismatch(tmp_path: Path) -> None:
+    _write_recording(tmp_path)
+    data_path = tmp_path / "data" / "episode_000000.h5"
+    with h5py.File(data_path, "r+") as h5:
+        del h5[MODE_KEY]
+        h5.create_dataset(MODE_KEY, data=np.full(4, 1.9, dtype=np.float32))
+
+    with pytest.raises(ValueError, match="dtype float32 != schema dtype int8"):
+        load_recording_dataset(tmp_path)
diff --git a/tests/test_retargeting.py b/tests/test_retargeting.py
index 3d0cd8a1..c2c5fb64 100644
--- a/tests/test_retargeting.py
+++ b/tests/test_retargeting.py
@@ -7,6 +7,7 @@
 """
 import json
 from pathlib import Path
+from types import SimpleNamespace
 from unittest.mock import MagicMock
 
 import numpy as np
@@ -170,3 +171,61 @@ def test_pico_bridge_g1_foot_ik_uses_canonical_foot_sites(self):
             assert ik_config[table_name]["left_foot"][0] == "Left_Foot"
             assert ik_config[table_name]["right_foot"][-1] == "site"
             assert ik_config[table_name]["right_foot"][0] == "Right_Foot"
+
+
+@requires_mujoco
+@requires_mink
+class TestGmrResetWarmup:
+    def test_warmup_seeds_floating_root_from_current_target(self, monkeypatch):
+        import mujoco
+
+        from teleopit.retargeting.gmr.motion_retarget import GeneralMotionRetargeting
+
+        gmr = object.__new__(GeneralMotionRetargeting)
+        gmr.robot_root_name = "pelvis"
+        gmr.human_root_name = "Pelvis"
+        gmr.scaled_human_data = {
+            "Pelvis": (
+                np.array([1.0, -2.0, 0.83], dtype=np.float64),
+                np.array([0.0, 0.0, 0.0, 2.0], dtype=np.float64),
+            )
+        }
+        gmr.model = SimpleNamespace(
+            qpos0=np.arange(36, dtype=np.float64),
+            body_jntadr=np.array([0, 0], dtype=np.int32),
+            body_jntnum=np.array([0, 1], dtype=np.int32),
+            jnt_type=np.array([mujoco.mjtJoint.mjJNT_FREE], dtype=np.int32),
+            jnt_qposadr=np.array([0], dtype=np.int32),
+        )
+        gmr.configuration = SimpleNamespace(update=MagicMock())
+        monkeypatch.setattr(mujoco, "mj_name2id", lambda *_args: 1)
+
+        gmr._seed_warmup_root_from_target()
+
+        seeded = gmr.configuration.update.call_args.kwargs["q"]
+        np.testing.assert_allclose(seeded[:3], [1.0, -2.0, 0.83])
+        np.testing.assert_allclose(seeded[3:7], [0.0, 0.0, 0.0, 1.0])
+        np.testing.assert_allclose(seeded[7:], gmr.model.qpos0[7:])
+
+    def test_retarget_applies_root_seed_only_on_reset_warmup(self):
+        from teleopit.retargeting.gmr.motion_retarget import GeneralMotionRetargeting
+
+        gmr = object.__new__(GeneralMotionRetargeting)
+        gmr.update_targets = MagicMock()
+        gmr._seed_warmup_root_from_target = MagicMock()
+        gmr._warmup_needed = True
+        gmr._warmup_max_iter = 200
+        gmr._warmup_dt = 0.1
+        gmr.max_iter = 10
+        gmr.use_ik_match_table1 = False
+        gmr.use_ik_match_table2 = False
+        gmr.configuration = SimpleNamespace(
+            data=SimpleNamespace(qpos=np.zeros(36, dtype=np.float64)),
+            model=SimpleNamespace(opt=SimpleNamespace(timestep=0.005)),
+        )
+
+        gmr.retarget({})
+        gmr.retarget({})
+
+        gmr._seed_warmup_root_from_target.assert_called_once_with()
+        assert gmr._warmup_needed is False
diff --git a/tests/test_sim2real_multiprocess.py b/tests/test_sim2real_multiprocess.py
index 72a7d933..6436108d 100644
--- a/tests/test_sim2real_multiprocess.py
+++ b/tests/test_sim2real_multiprocess.py
@@ -1,8 +1,11 @@
 from __future__ import annotations
 
 import importlib.util
+import json
 import logging
 from pathlib import Path
+import shutil
+import time
 from types import SimpleNamespace
 
 import h5py
@@ -16,9 +19,13 @@
     ACTION_KEY,
     FRAME_INDEX_KEY,
     HAND_ACTION_KEY,
+    HAND_STATE_KEY,
     HDF5_RECORDING_FORMAT,
+    HDF5_RECORDING_VERSION,
     IMAGE_KEY,
     MODE_KEY,
+    NECK_ACTION_KEY,
+    NECK_STATE_KEY,
     STATE_KEY,
     TIMESTAMP_KEY,
     build_mode_observation,
@@ -26,8 +33,15 @@
     build_recording_schema,
     hdf5_schema,
 )
-from teleopit.sim2real.mp.ipc import HEALTH_TOPIC, LatestSubscriber, ZmqPublisher
-from teleopit.sim2real.mp.messages import HandCommandPacket, ModeStatePacket, RecordStepPacket, ReferencePacket, SharedFrameDescriptor
+from teleopit.sim2real.mp.ipc import HEALTH_TOPIC, LatestSubscriber, ZmqPublisher, default_endpoints
+from teleopit.sim2real.mp.messages import (
+    HandCommandPacket,
+    ModeStatePacket,
+    NeckCommandPacket,
+    RecordStepPacket,
+    ReferencePacket,
+    SharedFrameDescriptor,
+)
 from teleopit.sim.reference_timeline import ReferenceSample, ReferenceWindow
 from teleopit.sim2real.mp.runtime import (
     ARM_MOCAP_REFERENCE_COMMAND,
@@ -41,6 +55,9 @@
     _configured_open_hand_pose,
     _hand_worker_active_for_mode,
     _human_frame_is_valid,
+    _recording_hardware_types,
+    _run_neck_worker,
+    _run_pico_io_worker,
 )
 from teleopit.sim2real.mp.shm import SharedFrameRingReader, SharedFrameRingWriter
 
@@ -81,6 +98,16 @@ def test_sim2real_runtime_rejects_hands_without_pico_provider() -> None:
         Sim2RealRuntime(cfg)
 
 
+def test_sim2real_runtime_rejects_neck_without_pico_provider() -> None:
+    cfg = {
+        "input": {"provider": "bvh"},
+        "runtime": {"shutdown_timeout_s": 0.01},
+        "neck": {"enabled": True, "driver": "openneck"},
+    }
+    with pytest.raises(ValueError, match="neck.enabled=true requires input.provider=pico4"):
+        Sim2RealRuntime(cfg)
+
+
 def test_sim2real_runtime_rejects_recording_without_pico_provider() -> None:
     cfg = {
         "input": {"provider": "bvh"},
@@ -101,6 +128,26 @@ def test_sim2real_runtime_rejects_recording_without_input_video() -> None:
         Sim2RealRuntime(cfg)
 
 
+@pytest.mark.parametrize(
+    ("device_cfg", "message"),
+    [
+        ({"hands": {"enabled": True, "sides": ["left"]}}, "hands.sides"),
+        ({"neck": {"enabled": True, "dry_run": True}}, "neck.dry_run"),
+    ],
+)
+def test_sim2real_runtime_rejects_recording_without_device_readback(
+    device_cfg: dict[str, object],
+    message: str,
+) -> None:
+    cfg = {
+        "input": {"provider": "pico4"},
+        "recording": {"enabled": True},
+        **device_cfg,
+    }
+    with pytest.raises(ValueError, match=message):
+        Sim2RealRuntime(cfg)
+
+
 def test_shared_frame_ring_roundtrip() -> None:
     writer = SharedFrameRingWriter(shape=(2, 3, 1), dtype=np.uint8, slots=2)
     reader = SharedFrameRingReader()
@@ -291,6 +338,257 @@ def Process(self, *, name: str, target: object, args: tuple[object, ...]) -> Fak
     assert started_names == ["pico_input", "reference", "robot_control", "recording_worker"]
 
 
+def test_neck_enabled_adds_neck_worker() -> None:
+    started_names: list[str] = []
+
+    class FakeProcess:
+        def __init__(self, *, name: str, target: object, args: tuple[object, ...]) -> None:
+            del target, args
+            self.name = name
+            self.exitcode = 0
+
+        def start(self) -> None:
+            started_names.append(self.name)
+
+    class FakeContext:
+        def Event(self) -> object:
+            return SimpleNamespace(set=lambda: None, is_set=lambda: False)
+
+        def Process(self, *, name: str, target: object, args: tuple[object, ...]) -> FakeProcess:
+            return FakeProcess(name=name, target=target, args=args)
+
+    cfg = {
+        "input": {"provider": "pico4"},
+        "runtime": {"shutdown_timeout_s": 0.01},
+        "neck": {"enabled": True, "driver": "openneck", "dry_run": True},
+    }
+    runtime = Sim2RealRuntime(cfg)
+    runtime._ctx = FakeContext()  # type: ignore[assignment]
+
+    runtime._start_processes()
+
+    assert started_names == ["pico_input", "reference", "robot_control", "neck_worker"]
+
+
+@pytest.mark.parametrize(
+    "failure_stage",
+    ["setup", "snapshot", "publish", "close", "video_start", "video_tick", "video_stop"],
+)
+def test_pico_auxiliary_failure_does_not_stop_pico_input(monkeypatch, failure_stage: str) -> None:
+    endpoints = default_endpoints(base_port=39890)
+    closed_publishers: list[str] = []
+    provider_closed = False
+
+    class FakeStopEvent:
+        def __init__(self) -> None:
+            self.polls = 0
+
+        def is_set(self) -> bool:
+            self.polls += 1
+            return self.polls > 1
+
+        def set(self) -> None:
+            self.polls = 2
+
+    class FakeProvider:
+        fps = 0.0
+
+        def __init__(self, **_kwargs: object) -> None:
+            return None
+
+        def get_head_pose_snapshot(self) -> SimpleNamespace:
+            if failure_stage == "snapshot":
+                raise RuntimeError("head-pose snapshot failed")
+            return SimpleNamespace(
+                hmd_rotation_wxyz=np.array([1.0, 0.0, 0.0, 0.0]),
+                spine3_rotation_wxyz=np.array([1.0, 0.0, 0.0, 0.0]),
+                timestamp_s=1.0,
+                seq=1,
+            )
+
+        def has_frame(self) -> bool:
+            return False
+
+        def pop_control_events(self) -> tuple[object, ...]:
+            return ()
+
+        def get_controller_snapshot(self) -> None:
+            return None
+
+        def get_hand_snapshot(self) -> None:
+            return None
+
+        def close(self) -> None:
+            nonlocal provider_closed
+            provider_closed = True
+
+    class FakeVideoRuntime:
+        pushed_frames = 0
+
+        def __init__(self, **_kwargs: object) -> None:
+            return None
+
+        def start(self) -> None:
+            if failure_stage == "video_start":
+                raise RuntimeError("video startup failed")
+            return None
+
+        def tick(self) -> None:
+            if failure_stage == "video_tick":
+                raise RuntimeError("video tick failed")
+            return None
+
+        def stop(self) -> None:
+            if failure_stage == "video_stop":
+                raise RuntimeError("video stop failed")
+            return None
+
+    class FakeSubscriber:
+        def __init__(self, _endpoint: str, _topic: str) -> None:
+            return None
+
+        def recv_latest(self) -> None:
+            return None
+
+        def close(self) -> None:
+            return None
+
+    class FakePublisher:
+        def __init__(self, endpoint: str) -> None:
+            self.endpoint = endpoint
+            if endpoint == endpoints.head_pose_pub and failure_stage == "setup":
+                raise RuntimeError("head-pose bind failed")
+
+        def publish(self, _topic: str, _payload: object) -> None:
+            if self.endpoint == endpoints.head_pose_pub and failure_stage == "publish":
+                raise RuntimeError("head-pose publish failed")
+
+        def close(self) -> None:
+            closed_publishers.append(self.endpoint)
+            if self.endpoint == endpoints.head_pose_pub and failure_stage == "close":
+                raise RuntimeError("head-pose close failed")
+
+    monkeypatch.setattr("teleopit.sim2real.mp.runtime.Pico4InputProvider", FakeProvider)
+    monkeypatch.setattr("teleopit.sim2real.mp.runtime.PicoVideoRuntime", FakeVideoRuntime)
+    monkeypatch.setattr("teleopit.sim2real.mp.runtime.LatestSubscriber", FakeSubscriber)
+    monkeypatch.setattr("teleopit.sim2real.mp.runtime.ZmqPublisher", FakePublisher)
+
+    _run_pico_io_worker(
+        {"input": {"provider": "pico4"}, "neck": {"enabled": True}},
+        endpoints,
+        FakeStopEvent(),  # type: ignore[arg-type]
+    )
+
+    assert provider_closed is True
+    assert endpoints.body_pub in closed_publishers
+    if failure_stage == "setup":
+        assert endpoints.head_pose_pub not in closed_publishers
+    else:
+        assert closed_publishers.count(endpoints.head_pose_pub) == 1
+
+
+@pytest.mark.parametrize("recording_enabled", [False, True])
+def test_neck_command_publisher_is_only_created_for_recording(monkeypatch, recording_enabled: bool) -> None:
+    publisher_endpoints: list[str] = []
+    published_topics: list[str] = []
+    published_packets: list[object] = []
+
+    class FakeRuntime:
+        def start(self) -> None:
+            return None
+
+        def read_deg(self) -> tuple[float, float]:
+            return 1.0, -2.0
+
+        def close(self) -> None:
+            return None
+
+    class FakeSubscriber:
+        def __init__(self, endpoint: str, topic: str) -> None:
+            del endpoint, topic
+
+        def close(self) -> None:
+            return None
+
+    class FakePublisher:
+        def __init__(self, endpoint: str) -> None:
+            publisher_endpoints.append(endpoint)
+
+        def publish(self, topic: str, payload: object) -> None:
+            published_topics.append(topic)
+            published_packets.append(payload)
+
+        def close(self) -> None:
+            return None
+
+    monkeypatch.setattr("teleopit.sim2real.mp.runtime.build_neck_runtime", lambda _cfg: FakeRuntime())
+    monkeypatch.setattr("teleopit.sim2real.mp.runtime.LatestSubscriber", FakeSubscriber)
+    monkeypatch.setattr("teleopit.sim2real.mp.runtime.ZmqPublisher", FakePublisher)
+    endpoints = default_endpoints(base_port=39870)
+    stop_event = SimpleNamespace(is_set=lambda: True, set=lambda: None)
+
+    _run_neck_worker(
+        {
+            "neck": {"enabled": True, "driver": "openneck"},
+            "recording": {"enabled": recording_enabled},
+        },
+        endpoints,
+        stop_event,  # type: ignore[arg-type]
+    )
+
+    assert publisher_endpoints == ([endpoints.neck_command_pub] if recording_enabled else [])
+    assert published_topics == (["neck_command"] if recording_enabled else [])
+    if recording_enabled:
+        packet = published_packets[0]
+        assert isinstance(packet, NeckCommandPacket)
+        assert packet.state_yaw_deg == 1.0
+        assert packet.state_pitch_deg == -2.0
+
+
+@pytest.mark.parametrize("worker_name", ["neck_worker", "pico_input"])
+def test_noncritical_worker_exit_warning_is_not_repeated(monkeypatch, caplog, worker_name: str) -> None:
+    class FakeStopEvent:
+        def __init__(self) -> None:
+            self.polls = 0
+            self.stopped = False
+
+        def is_set(self) -> bool:
+            self.polls += 1
+            if self.polls >= 4:
+                self.stopped = True
+            return self.stopped
+
+        def set(self) -> None:
+            self.stopped = True
+
+    class FakeProcess:
+        exitcode = 1
+
+        def __init__(self) -> None:
+            self.name = worker_name
+
+        def is_alive(self) -> bool:
+            return False
+
+        def join(self, timeout: float | None = None) -> None:
+            del timeout
+
+    cfg = {
+        "input": {"provider": "pico4"},
+        "runtime": {"shutdown_timeout_s": 0.01},
+        "neck": {"enabled": True, "driver": "openneck", "dry_run": True},
+    }
+    runtime = Sim2RealRuntime(cfg)
+    runtime._stop_event = FakeStopEvent()  # type: ignore[assignment]
+    monkeypatch.setattr(runtime, "_start_processes", lambda: runtime._processes.append(FakeProcess()))  # type: ignore[arg-type]
+
+    with caplog.at_level(logging.WARNING, logger="teleopit.operator"):
+        runtime.run()
+
+    warnings = [message for message in caplog.messages if "non-critical worker exited" in message]
+    assert warnings == [f"non-critical worker exited: {worker_name}; G1 control remains active"]
+
+
 def test_recording_key_mapping() -> None:
     assert map_recording_key_to_command("R") == "record_start"
     assert map_recording_key_to_command("s") == "record_save"
@@ -299,37 +597,119 @@ def test_recording_key_mapping() -> None:
     assert map_recording_key_to_command("x") is None
 
 
+@pytest.mark.parametrize(
+    ("hands_enabled", "neck_enabled", "hand_type", "neck_type"),
+    [
+        (False, False, "none", "none"),
+        (True, False, "linkerhand_l6", "none"),
+        (False, True, "none", "openneck"),
+        (True, True, "linkerhand_l6", "openneck"),
+    ],
+)
+def test_recording_optional_hardware_types_follow_enabled_flags(
+    hands_enabled: bool,
+    neck_enabled: bool,
+    hand_type: str,
+    neck_type: str,
+) -> None:
+    assert _recording_hardware_types(
+        {
+            "robot": {"type": "unitree_g1_29dof"},
+            "hands": {"enabled": hands_enabled, "driver": "linkerhand_l6"},
+            "neck": {"enabled": neck_enabled, "driver": "openneck"},
+        }
+    ) == ("unitree_g1_29dof", hand_type, neck_type)
+
+
 def test_hdf5_recording_schema() -> None:
-    schema = build_recording_schema({"width": 640, "height": 480, "key": IMAGE_KEY})
+    schema = build_recording_schema(
+        {"width": 640, "height": 480, "key": IMAGE_KEY},
+        fps=30,
+        robot_type="unitree_g1_29dof",
+        hand_type="linkerhand_o6",
+        neck_type="openneck",
+    )
     sidecar = hdf5_schema(schema)
     features = sidecar["features"]
 
     assert sidecar["format"] == HDF5_RECORDING_FORMAT
-    assert features[IMAGE_KEY]["type"] == "video"
-    assert features[IMAGE_KEY]["format"] == "mp4"
+    assert sidecar["version"] == HDF5_RECORDING_VERSION == 4
+    assert sidecar["fps"] == 30
+    assert sidecar["robot_type"] == "unitree_g1_29dof"
+    assert sidecar["hand_type"] == "linkerhand_o6"
+    assert sidecar["neck_type"] == "openneck"
+    assert features[IMAGE_KEY]["dtype"] == "video"
     assert features[IMAGE_KEY]["shape"] == [480, 640, 3]
     assert features[FRAME_INDEX_KEY]["dtype"] == "int64"
     assert features[TIMESTAMP_KEY]["dtype"] == "float64"
     assert features[STATE_KEY]["shape"] == [68]
-    assert features[MODE_KEY]["shape"] == [1]
+    assert features[MODE_KEY]["shape"] == []
+    assert features[MODE_KEY]["dtype"] == "int8"
     assert features[ACTION_KEY]["shape"] == [36]
+    assert features[HAND_STATE_KEY]["shape"] == [12]
     assert features[HAND_ACTION_KEY]["shape"] == [12]
-    assert sidecar["features"][STATE_KEY]["slices"]["joint_pos"] == [0, 29]
-    assert sidecar["features"][STATE_KEY]["slices"]["projected_gravity"] == [65, 68]
-    assert sidecar["features"][MODE_KEY]["codes"]["pause"] == 3
-    assert sidecar["features"][ACTION_KEY]["slices"]["joint_pos"] == [7, 36]
-    assert sidecar["features"][HAND_ACTION_KEY]["slices"]["left_pose"] == [0, 6]
-    assert sidecar["features"][HAND_ACTION_KEY]["slices"]["right_pose"] == [6, 12]
+    assert features[NECK_STATE_KEY]["shape"] == [2]
+    assert features[NECK_ACTION_KEY]["shape"] == [2]
+    assert features[STATE_KEY]["groups"]["joint_pos"] == [0, 29]
+    assert features[STATE_KEY]["groups"]["projected_gravity"] == [65, 68]
+    assert features[MODE_KEY]["values"]["pause"] == 3
+    assert features[ACTION_KEY]["groups"]["reference_joint_pos"] == [7, 36]
+    assert features[HAND_STATE_KEY]["groups"]["left_hand_state"] == [0, 6]
+    assert features[HAND_STATE_KEY]["groups"]["right_hand_state"] == [6, 12]
+    assert features[HAND_ACTION_KEY]["groups"]["left_hand_target"] == [0, 6]
+    assert features[HAND_ACTION_KEY]["groups"]["right_hand_target"] == [6, 12]
+    assert features[NECK_ACTION_KEY]["names"] == ["yaw_deg", "pitch_deg"]
+    assert features[NECK_STATE_KEY]["names"] == ["yaw_deg", "pitch_deg"]
+    assert features[NECK_STATE_KEY]["units"] == "degrees"
+    assert features[NECK_ACTION_KEY]["units"] == "degrees"
+    assert "range" not in features[NECK_ACTION_KEY]
+    assert len(features[STATE_KEY]["names"]) == 68
+    assert len(features[ACTION_KEY]["names"]) == 36
+    assert len(features[HAND_STATE_KEY]["names"]) == 12
+    assert len(features[HAND_ACTION_KEY]["names"]) == 12
+
+
+@pytest.mark.parametrize(
+    ("hand_type", "neck_type", "has_hand_action", "has_neck_action"),
+    [
+        ("none", "none", False, False),
+        ("linkerhand_l6", "none", True, False),
+        ("none", "openneck", False, True),
+        ("linkerhand_o6", "openneck", True, True),
+    ],
+)
+def test_hdf5_recording_schema_optional_action_combinations(
+    hand_type: str,
+    neck_type: str,
+    has_hand_action: bool,
+    has_neck_action: bool,
+) -> None:
+    schema = build_recording_schema(
+        {"width": 640, "height": 480, "key": IMAGE_KEY},
+        hand_type=hand_type,
+        neck_type=neck_type,
+    )
+    features = hdf5_schema(schema)["features"]
+
+    assert schema.has_hand_action is has_hand_action
+    assert schema.has_neck_action is has_neck_action
+    assert (HAND_STATE_KEY in features) is has_hand_action
+    assert (HAND_ACTION_KEY in features) is has_hand_action
+    assert (NECK_STATE_KEY in features) is has_neck_action
+    assert (NECK_ACTION_KEY in features) is has_neck_action
 
 
 def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None:
     from teleopit.recording.hdf5 import MP4VideoConfig, TeleopitHDF5Recorder
 
-    schema = build_recording_schema({"width": 2, "height": 2, "key": IMAGE_KEY})
+    schema = build_recording_schema(
+        {"width": 2, "height": 2, "key": IMAGE_KEY},
+        hand_type="linkerhand_l6",
+        neck_type="openneck",
+    )
     recorder = TeleopitHDF5Recorder.create(
         output_dir=tmp_path,
         task="walk",
-        fps=30,
         schema=schema,
         video_config=MP4VideoConfig(quality=5),
     )
@@ -341,39 +721,156 @@ def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None:
             state=np.arange(68, dtype=np.float32),
             mode=build_mode_observation("mocap"),
             action=np.arange(36, dtype=np.float32),
+            hand_state=np.arange(12, dtype=np.float32) + 20.0,
             hand_action=np.arange(12, dtype=np.float32),
-            task="walk",
+            neck_state=np.array([11.5, -7.5], dtype=np.float32),
+            neck_action=np.array([12.5, -8.0], dtype=np.float32),
         )
     recorder.save_episode()
     recorder.finalize()
 
-    episodes = sorted((tmp_path / "episodes").glob("*.h5"))
-    videos = sorted((tmp_path / "videos" / "observation.images.d435i_rgb").glob("*.mp4"))
+    episodes = sorted((tmp_path / "data").glob("*.h5"))
+    videos = sorted((tmp_path / "videos" / "d435i_rgb").glob("*.mp4"))
     assert len(episodes) == 1
     assert len(videos) == 1
     assert videos[0].stat().st_size > 0
     assert (tmp_path / "schema.json").exists()
-    assert not list((tmp_path / ".tmp").glob("*.h5"))
+    assert (tmp_path / "episodes.jsonl").exists()
+    assert not list((tmp_path / ".tmp").rglob("*.h5"))
+
+    manifest = [json.loads(line) for line in (tmp_path / "episodes.jsonl").read_text().splitlines()]
+    assert manifest == [
+        {
+            "episode_index": 0,
+            "frames": 2,
+            "task": "walk",
+            "data": "data/episode_000000.h5",
+            "videos": {IMAGE_KEY: "videos/d435i_rgb/episode_000000.mp4"},
+        }
+    ]
 
     with h5py.File(episodes[0], "r") as h5:
-        assert h5.attrs["format"] == HDF5_RECORDING_FORMAT
-        assert h5.attrs["version"] == 1
-        assert h5.attrs["task"] == "walk"
-        assert h5.attrs["fps"] == 30
-        assert h5.attrs["frames"] == 2
-        assert h5.attrs["video_path"] == videos[0].relative_to(tmp_path).as_posix()
-        assert h5.attrs["video_key"] == IMAGE_KEY
-        assert h5.attrs["video_frames"] == 2
-        assert h5.attrs["video_fps"] == 30
+        assert dict(h5.attrs) == {}
         assert IMAGE_KEY not in h5
         assert h5[FRAME_INDEX_KEY].shape == (2,)
         assert h5[TIMESTAMP_KEY].shape == (2,)
         np.testing.assert_array_equal(h5[FRAME_INDEX_KEY][...], np.array([0, 1], dtype=np.int64))
         np.testing.assert_allclose(h5[TIMESTAMP_KEY][...], np.array([0.0, 1.0 / 30.0], dtype=np.float64))
         assert h5[STATE_KEY].shape == (2, 68)
-        assert h5[MODE_KEY].shape == (2, 1)
+        assert h5[MODE_KEY].shape == (2,)
+        assert h5[MODE_KEY].dtype == np.dtype(np.int8)
         assert h5[ACTION_KEY].shape == (2, 36)
+        assert h5[HAND_STATE_KEY].shape == (2, 12)
         assert h5[HAND_ACTION_KEY].shape == (2, 12)
+        assert h5[NECK_STATE_KEY].shape == (2, 2)
+        assert h5[NECK_ACTION_KEY].shape == (2, 2)
+        np.testing.assert_allclose(
+            h5[NECK_STATE_KEY][...],
+            np.array([[11.5, -7.5], [11.5, -7.5]], dtype=np.float32),
+        )
+        np.testing.assert_allclose(
+            h5[NECK_ACTION_KEY][...],
+            np.array([[12.5, -8.0], [12.5, -8.0]], dtype=np.float32),
+        )
+
+
+def test_hdf5_recorder_resumes_and_keeps_tasks_in_editable_manifest(tmp_path: Path) -> None:
+    from teleopit.recording.hdf5 import TeleopitHDF5Recorder
+
+    schema = build_recording_schema({"width": 2, "height": 2, "key": IMAGE_KEY})
+
+    def write_episode(task: str, value: int) -> None:
+        recorder = TeleopitHDF5Recorder.create(output_dir=tmp_path, task=task, schema=schema)
+        recorder.start_episode()
+        recorder.add_frame(
+            image=np.full((2, 2, 3), value, dtype=np.uint8),
+            state=np.full(68, value, dtype=np.float32),
+            mode=build_mode_observation("mocap"),
+            action=np.full(36, value, dtype=np.float32),
+        )
+        recorder.save_episode()
+        recorder.finalize()
+
+    write_episode("pick up the box", 1)
+    first_entry = json.loads((tmp_path / "episodes.jsonl").read_text().strip())
+    first_entry["task"] = "pick up the red box"
+    (tmp_path / "episodes.jsonl").write_text(
+        json.dumps(first_entry, ensure_ascii=False, separators=(",", ":")),
+        encoding="utf-8",
+    )
+
+    write_episode("把盒子放到桌上", 2)
+
+    entries = [json.loads(line) for line in (tmp_path / "episodes.jsonl").read_text().splitlines()]
+    assert [entry["episode_index"] for entry in entries] == [0, 1]
+    assert [entry["task"] for entry in entries] == ["pick up the red box", "把盒子放到桌上"]
+    assert sorted(path.name for path in (tmp_path / "data").glob("*.h5")) == [
+        "episode_000000.h5",
+        "episode_000001.h5",
+    ]
+    with h5py.File(tmp_path / "data" / "episode_000001.h5", "r") as h5:
+        assert HAND_STATE_KEY not in h5
+        assert HAND_ACTION_KEY not in h5
+        assert NECK_STATE_KEY not in h5
+        assert NECK_ACTION_KEY not in h5
+
+
+def test_hdf5_recorder_discards_uncommitted_episode_files_on_resume(tmp_path: Path) -> None:
+    from teleopit.recording.hdf5 import TeleopitHDF5Recorder
+
+    schema = build_recording_schema({"width": 2, "height": 2, "key": IMAGE_KEY})
+    recorder = TeleopitHDF5Recorder.create(output_dir=tmp_path, task="first", schema=schema)
+    recorder.start_episode()
+    recorder.add_frame(
+        image=np.zeros((2, 2, 3), dtype=np.uint8),
+        state=np.zeros(68, dtype=np.float32),
+        mode=build_mode_observation("mocap"),
+        action=np.zeros(36, dtype=np.float32),
+    )
+    recorder.save_episode()
+
+    orphan_data = tmp_path / "data" / "episode_000001.h5"
+    orphan_video = tmp_path / "videos" / "d435i_rgb" / "episode_000001.mp4"
+    shutil.copyfile(tmp_path / "data" / "episode_000000.h5", orphan_data)
+    shutil.copyfile(tmp_path / "videos" / "d435i_rgb" / "episode_000000.mp4", orphan_video)
+    tmp_data = tmp_path / ".tmp" / "data" / "episode_000001.h5"
+    tmp_video = tmp_path / ".tmp" / "videos" / "d435i_rgb" / "episode_000001.mp4"
+    tmp_data.parent.mkdir(parents=True, exist_ok=True)
+    tmp_video.parent.mkdir(parents=True, exist_ok=True)
+    shutil.copyfile(orphan_data, tmp_data)
+    shutil.copyfile(orphan_video, tmp_video)
+
+    resumed = TeleopitHDF5Recorder.create(output_dir=tmp_path, task="second", schema=schema)
+
+    assert not orphan_data.exists()
+    assert not orphan_video.exists()
+    assert not tmp_data.exists()
+    assert not tmp_video.exists()
+    resumed.start_episode()
+    resumed.add_frame(
+        image=np.ones((2, 2, 3), dtype=np.uint8),
+        state=np.ones(68, dtype=np.float32),
+        mode=build_mode_observation("mocap"),
+        action=np.ones(36, dtype=np.float32),
+    )
+    resumed.save_episode()
+
+    entries = [json.loads(line) for line in (tmp_path / "episodes.jsonl").read_text().splitlines()]
+    assert [entry["episode_index"] for entry in entries] == [0, 1]
+
+
+def test_hdf5_recorder_rejects_existing_incompatible_schema(tmp_path: Path) -> None:
+    from teleopit.recording.hdf5 import TeleopitHDF5Recorder
+
+    no_hands = build_recording_schema({"width": 2, "height": 2}, hand_type="none")
+    TeleopitHDF5Recorder.create(output_dir=tmp_path, task="demo", schema=no_hands).finalize()
+    with_hands = build_recording_schema(
+        {"width": 2, "height": 2},
+        hand_type="linkerhand_l6",
+    )
+
+    with pytest.raises(ValueError, match="schema mismatch"):
+        TeleopitHDF5Recorder.create(output_dir=tmp_path, task="demo", schema=with_hands)
 
 
 def test_hdf5_recorder_cleans_partial_episode_when_video_writer_fails(tmp_path: Path) -> None:
@@ -385,16 +882,16 @@ def _create_video_writer(self, path: Path) -> object:
             raise RuntimeError("writer failed")
 
     schema = build_recording_schema({"width": 2, "height": 2, "key": IMAGE_KEY})
-    recorder = FailingVideoRecorder.create(output_dir=tmp_path, task="walk", fps=30, schema=schema)
+    recorder = FailingVideoRecorder.create(output_dir=tmp_path, task="walk", schema=schema)
 
     with pytest.raises(RuntimeError, match="writer failed"):
         recorder.start_episode()
 
     recorder.finalize()
-    assert not list((tmp_path / ".tmp").glob("*.h5"))
-    assert not list((tmp_path / ".tmp" / "videos" / "observation.images.d435i_rgb").glob("*.mp4"))
-    assert not list((tmp_path / "episodes").glob("*.h5"))
-    assert not list((tmp_path / "videos" / "observation.images.d435i_rgb").glob("*.mp4"))
+    assert not list((tmp_path / ".tmp").rglob("*.h5"))
+    assert not list((tmp_path / ".tmp" / "videos" / "d435i_rgb").glob("*.mp4"))
+    assert not list((tmp_path / "data").glob("*.h5"))
+    assert not list((tmp_path / "videos" / "d435i_rgb").glob("*.mp4"))
 
 
 def test_hdf5_recorder_keeps_startup_error_when_partial_cleanup_fails(tmp_path: Path) -> None:
@@ -412,13 +909,13 @@ def _create_datasets(self, h5: h5py.File) -> dict[str, h5py.Dataset]:
             raise RuntimeError("startup failed")
 
     schema = build_recording_schema({"width": 2, "height": 2, "key": IMAGE_KEY})
-    recorder = FailingDatasetRecorder.create(output_dir=tmp_path, task="walk", fps=30, schema=schema)
+    recorder = FailingDatasetRecorder.create(output_dir=tmp_path, task="walk", schema=schema)
 
     with pytest.raises(RuntimeError, match="startup failed"):
         recorder.start_episode()
 
     recorder.finalize()
-    assert not list((tmp_path / ".tmp").glob("*.h5"))
+    assert not list((tmp_path / ".tmp").rglob("*.h5"))
 
 
 def test_configured_open_hand_pose_matches_linkerhand_l6_parser() -> None:
@@ -958,9 +1455,8 @@ def test_robot_worker_publish_record_step() -> None:
     assert packet.mocap_active is True
     assert packet.recordable is True
     assert packet.observation_state.shape == (68,)
-    assert packet.observation_mode.shape == (1,)
+    assert packet.observation_mode == int(build_mode_observation("arms"))
     assert packet.action_reference_qpos.shape == (36,)
-    np.testing.assert_allclose(packet.observation_mode, build_mode_observation("arms"))
     np.testing.assert_allclose(packet.action_reference_qpos, reference_qpos.astype(np.float32))
 
 
@@ -999,7 +1495,7 @@ def test_robot_worker_enter_damping_publishes_non_recordable_packet() -> None:
     assert packet.mode == "damping"
     assert packet.recordable is False
     assert packet.mocap_active is False
-    np.testing.assert_allclose(packet.observation_mode, np.array([-1.0], dtype=np.float32))
+    assert packet.observation_mode == -1
 
 
 def test_recording_worker_start_save_discard_with_fake_adapter() -> None:
@@ -1017,19 +1513,28 @@ def add_frame(
             *,
             image: np.ndarray,
             state: np.ndarray,
-            mode: np.ndarray,
+            mode: object,
             action: np.ndarray,
-            hand_action: np.ndarray,
-            task: str,
+            hand_state: np.ndarray | None = None,
+            neck_state: np.ndarray | None = None,
+            hand_action: np.ndarray | None = None,
+            neck_action: np.ndarray | None = None,
         ) -> None:
-            calls.append(f"frame:{task}")
+            calls.append("frame")
+            assert hand_state is not None
+            assert neck_state is not None
+            assert hand_action is not None
+            assert neck_action is not None
             frames.append(
                 {
                     "image": image.copy(),
                     "state": state.copy(),
-                    "mode": mode.copy(),
+                    "mode": np.asarray(mode).copy(),
                     "action": action.copy(),
+                    "hand_state": hand_state.copy(),
+                    "neck_state": neck_state.copy(),
                     "hand_action": hand_action.copy(),
+                    "neck_action": neck_action.copy(),
                 }
             )
 
@@ -1055,7 +1560,9 @@ def fake_factory(**_kwargs: object) -> FakeRecorder:
                 "fps": 30,
                 "min_episode_seconds": 0.0,
                 "camera": {"width": 2, "height": 2, "key": IMAGE_KEY},
-            }
+            },
+            "hands": {"enabled": True, "driver": "linkerhand_l6"},
+            "neck": {"enabled": True, "driver": "openneck"},
         },
         endpoints,
         stop_event,  # type: ignore[arg-type]
@@ -1069,7 +1576,7 @@ def fake_factory(**_kwargs: object) -> FakeRecorder:
             mocap_active=False,
             recordable=False,
             observation_state=np.ones(68, dtype=np.float32),
-            observation_mode=build_mode_observation("standing"),
+            observation_mode=int(build_mode_observation("standing")),
             action_reference_qpos=np.ones(36, dtype=np.float32),
             seq=1,
         )
@@ -1082,15 +1589,19 @@ def fake_factory(**_kwargs: object) -> FakeRecorder:
             mocap_active=False,
             recordable=True,
             observation_state=np.arange(68, dtype=np.float32),
-            observation_mode=build_mode_observation("standing"),
+            observation_mode=int(build_mode_observation("standing")),
             action_reference_qpos=np.arange(36, dtype=np.float32),
             seq=2,
         )
         worker._start_episode()
+        assert calls == []
+
+        ready_desc = writer.write(np.full((2, 2, 3), 4, dtype=np.uint8), timestamp_s=2.0)
+        worker._handle_video(ready_desc)
+        worker._start_episode()
         worker._save_episode()
         assert calls == ["start", "discard"]
 
-        worker._start_episode()
         worker._latest_hand_command = HandCommandPacket(
             timestamp_s=2.05,
             driver="linkerhand_l6",
@@ -1099,17 +1610,33 @@ def fake_factory(**_kwargs: object) -> FakeRecorder:
             left_pose=np.arange(6, dtype=np.float32),
             right_pose=np.arange(6, 12, dtype=np.float32),
             seq=1,
+            left_state=np.arange(20, 26, dtype=np.float32),
+            right_state=np.arange(26, 32, dtype=np.float32),
         )
+        worker._latest_neck_command = NeckCommandPacket(
+            timestamp_s=2.06,
+            driver="openneck",
+            active=True,
+            yaw_deg=12.5,
+            pitch_deg=-8.0,
+            seq=1,
+            state_yaw_deg=11.5,
+            state_pitch_deg=-7.5,
+        )
+        worker._start_episode()
         desc = writer.write(np.full((2, 2, 3), 5, dtype=np.uint8), timestamp_s=2.1)
         worker._handle_video(desc)
         worker._save_episode()
 
-        assert calls == ["start", "discard", "start", "frame:walk", "save"]
+        assert calls == ["start", "discard", "start", "frame", "save"]
         assert frames[0]["image"].shape == (2, 2, 3)
         np.testing.assert_allclose(frames[0]["state"], np.arange(68, dtype=np.float32))
-        np.testing.assert_allclose(frames[0]["mode"], build_mode_observation("standing"))
+        assert int(frames[0]["mode"]) == int(build_mode_observation("standing"))
         np.testing.assert_allclose(frames[0]["action"], np.arange(36, dtype=np.float32))
+        np.testing.assert_allclose(frames[0]["hand_state"], np.arange(20, 32, dtype=np.float32))
         np.testing.assert_allclose(frames[0]["hand_action"], np.arange(12, dtype=np.float32))
+        np.testing.assert_allclose(frames[0]["neck_state"], np.array([11.5, -7.5], dtype=np.float32))
+        np.testing.assert_allclose(frames[0]["neck_action"], np.array([12.5, -8.0], dtype=np.float32))
 
         worker._latest_record = RecordStepPacket(
             timestamp_s=3.0,
@@ -1117,17 +1644,29 @@ def fake_factory(**_kwargs: object) -> FakeRecorder:
             mocap_active=False,
             recordable=True,
             observation_state=np.zeros(68, dtype=np.float32),
-            observation_mode=build_mode_observation("pause"),
+            observation_mode=int(build_mode_observation("pause")),
             action_reference_qpos=np.zeros(36, dtype=np.float32),
             seq=3,
         )
         worker._start_episode()
         worker._discard_episode("test")
         assert calls[-2:] == ["start", "discard"]
+
+        worker._start_episode()
+        worker._latest_video_received_s = time.monotonic() - worker._CAMERA_TIMEOUT_S - 0.1
+        assert worker._discard_if_camera_stale() is True
+        assert calls[-2:] == ["start", "discard"]
+
+        worker._latest_video_received_s = time.monotonic()
+        worker._start_episode()
+        worker._latest_video_received_s = time.monotonic() - worker._CAMERA_TIMEOUT_S - 0.1
+        worker._save_episode()
+        assert calls[-2:] == ["start", "discard"]
     finally:
         writer.close(unlink=True)
         worker._record_sub.close()
         worker._video_sub.close()
         worker._hand_command_sub.close()
+        worker._neck_command_sub.close()
         worker._command_sub.close()
         worker._frame_reader.close()
diff --git a/tests/test_sim_loop.py b/tests/test_sim_loop.py
index 11cdebd8..3f1521d7 100644
--- a/tests/test_sim_loop.py
+++ b/tests/test_sim_loop.py
@@ -604,6 +604,7 @@ def __init__(self) -> None:
                 (),
                 (TerminalKeyEvent("y"),),
                 (TerminalKeyEvent("x"),),
+                (TerminalKeyEvent("y"),),
             ]
             self._idx = 0
 
@@ -642,16 +643,19 @@ def close(self) -> None:
         viewers=set(),
     )
 
+    retargeter = _DummyRetargeter()
     result = loop.run(
         input_provider=_RealtimeInputProvider(),
-        retargeter=_DummyRetargeter(),
-        num_steps=3,
+        retargeter=retargeter,
+        num_steps=4,
     )
 
-    assert result["steps"] == 3
+    assert result["steps"] == 4
     np.testing.assert_allclose(obs_builder.mimic_obs_calls[0], np.array([0.0], dtype=np.float32), atol=1e-6)
     np.testing.assert_allclose(obs_builder.mimic_obs_calls[1], np.array([0.3], dtype=np.float32), atol=1e-6)
     np.testing.assert_allclose(obs_builder.mimic_obs_calls[2], np.array([0.0], dtype=np.float32), atol=1e-6)
+    np.testing.assert_allclose(obs_builder.mimic_obs_calls[3], np.array([0.6], dtype=np.float32), atol=1e-6)
+    assert retargeter.reset_calls == 2
 
 
 @requires_mujoco
@@ -716,6 +720,9 @@ def get_realtime_input_packet(self):
             return packet
 
     class _Retargeter:
+        def __init__(self) -> None:
+            self.reset_calls = 0
+
         def retarget(self, frame: object) -> np.ndarray:
             pelvis = np.asarray(frame["Pelvis"][0], dtype=np.float64)
             qpos = np.zeros(36, dtype=np.float64)
@@ -725,6 +732,9 @@ def retarget(self, frame: object) -> np.ndarray:
             qpos[8] = pelvis[0] + 10.0
             return qpos
 
+        def reset(self) -> None:
+            self.reset_calls += 1
+
     class _KeyboardReader:
         def __init__(self) -> None:
             self._polls = [
@@ -770,9 +780,10 @@ def close(self) -> None:
         viewers=set(),
     )
 
+    retargeter = _Retargeter()
     result = loop.run(
         input_provider=_RealtimeInputProvider(),
-        retargeter=_Retargeter(),
+        retargeter=retargeter,
         num_steps=4,
     )
 
@@ -784,6 +795,7 @@ def close(self) -> None:
     # Step 3 toggles back to full-body MOCAP; root XY is reanchored, while non-arm joints follow retarget again.
     np.testing.assert_allclose(obs_builder.motion_qpos_calls[3][0], 0.0, atol=1e-6)
     np.testing.assert_allclose(obs_builder.motion_qpos_calls[3][7], 1.2, atol=1e-6)
+    assert retargeter.reset_calls == 1
 
 
 @requires_mujoco
diff --git a/third_party/somehand b/third_party/somehand
index 0e9adba4..f0a6b42e 160000
--- a/third_party/somehand
+++ b/third_party/somehand
@@ -1 +1 @@
-Subproject commit 0e9adba4e193540279f8e5803a9339a49666499a
+Subproject commit f0a6b42e151ca10a6eec3e24c24c10cd13c40314
diff --git a/train_mimic/benchmarking.py b/train_mimic/benchmarking.py
new file mode 100644
index 00000000..cfb854ce
--- /dev/null
+++ b/train_mimic/benchmarking.py
@@ -0,0 +1,361 @@
+"""OmniXtreme-style benchmark helpers for motion tracking policies."""
+
+from __future__ import annotations
+
+import csv
+import json
+from dataclasses import asdict, dataclass
+from pathlib import Path
+from typing import Any, Iterable, Sequence
+
+import h5py
+import numpy as np
+
+from train_mimic.data.dataset_lib import (
+    compute_clip_sample_ranges,
+    find_precomputed_motion_shards,
+    parse_window_steps,
+)
+
+
+@dataclass(frozen=True)
+class ClipSpec:
+    clip_id: int
+    shard_path: str
+    shard_clip_index: int
+    frame_offset: int
+    num_frames: int
+    fps: float
+    sample_start_s: float
+    sample_end_s: float
+
+
+@dataclass(frozen=True)
+class BenchmarkJob:
+    job_id: int
+    clip_id: int
+    rollout_index: int
+    start_time_s: float
+
+
+@dataclass(frozen=True)
+class BenchmarkPlan:
+    clip_seconds: float
+    control_steps: int
+    step_dt: float
+    eligible_clips: tuple[ClipSpec, ...]
+    skipped_short_clips: tuple[ClipSpec, ...]
+    jobs: tuple[BenchmarkJob, ...]
+
+
+@dataclass(frozen=True)
+class RolloutResult:
+    job_id: int
+    clip_id: int
+    rollout_index: int
+    success: bool
+    steps: int
+    failure_step: int | None
+    failure_reason: str | None
+    mpjpe_m: float
+    root_pos_error_m: float
+    root_rot_error_rad: float
+    root_vel_error_m_s: float
+
+
+def _json_safe(value: Any) -> Any:
+    if isinstance(value, float):
+        return value if np.isfinite(value) else None
+    if isinstance(value, dict):
+        return {key: _json_safe(item) for key, item in value.items()}
+    if isinstance(value, list):
+        return [_json_safe(item) for item in value]
+    return value
+
+
+def load_clip_specs(
+    motion_file: str | Path,
+    *,
+    window_steps: Sequence[int] = (0,),
+) -> tuple[ClipSpec, ...]:
+    """Load benchmark clip specs in the same order MotionLib assigns clip ids."""
+    specs: list[ClipSpec] = []
+    next_clip_id = 0
+    steps = parse_window_steps(window_steps)
+    max_future = max((step for step in steps if step > 0), default=0)
+    max_history = -min((step for step in steps if step < 0), default=0)
+    min_clip_length = max_history + 1 + max_future + 1
+    for shard_path in find_precomputed_motion_shards(Path(motion_file)):
+        with h5py.File(shard_path, "r") as h5:
+            starts = np.asarray(h5["clip_starts"], dtype=np.int64)
+            lengths = np.asarray(h5["clip_lengths"], dtype=np.int64)
+            fps = np.asarray(h5["clip_fps"], dtype=np.float32)
+            valid_mask = lengths >= min_clip_length
+            if not np.any(valid_mask):
+                continue
+            valid_starts = starts[valid_mask]
+            valid_lengths = lengths[valid_mask]
+            valid_fps = fps[valid_mask]
+            valid_shard_clip_indices = np.nonzero(valid_mask)[0]
+            sample_starts, sample_ends = compute_clip_sample_ranges(
+                valid_lengths,
+                window_steps=steps,
+            )
+            for shard_clip_index, start, length, cur_fps, sample_start, sample_end in zip(
+                valid_shard_clip_indices,
+                valid_starts,
+                valid_lengths,
+                valid_fps,
+                sample_starts,
+                sample_ends,
+                strict=True,
+            ):
+                specs.append(
+                    ClipSpec(
+                        clip_id=next_clip_id,
+                        shard_path=str(shard_path),
+                        shard_clip_index=int(shard_clip_index),
+                        frame_offset=int(start),
+                        num_frames=int(length),
+                        fps=float(cur_fps),
+                        sample_start_s=float(sample_start) / float(cur_fps),
+                        sample_end_s=float(sample_end) / float(cur_fps),
+                    )
+                )
+                next_clip_id += 1
+    return tuple(specs)
+
+
+def build_benchmark_plan(
+    clips: Sequence[ClipSpec],
+    *,
+    clip_seconds: float,
+    step_dt: float,
+) -> BenchmarkPlan:
+    if clip_seconds <= 0.0:
+        raise ValueError(f"clip_seconds must be > 0, got {clip_seconds}")
+    if step_dt <= 0.0:
+        raise ValueError(f"step_dt must be > 0, got {step_dt}")
+
+    control_steps_f = clip_seconds / step_dt
+    control_steps = int(round(control_steps_f))
+    if not np.isclose(control_steps_f, control_steps, atol=1e-6):
+        raise ValueError(
+            f"clip_seconds={clip_seconds} is not an integer number of control steps "
+            f"for step_dt={step_dt}"
+        )
+
+    eligible: list[ClipSpec] = []
+    skipped: list[ClipSpec] = []
+    for clip in clips:
+        duration_s = clip.sample_end_s - clip.sample_start_s
+        if duration_s + 1e-9 >= clip_seconds:
+            eligible.append(clip)
+        else:
+            skipped.append(clip)
+    if not eligible:
+        raise ValueError(
+            f"No clips are at least {clip_seconds:.3f}s long after applying valid sample ranges."
+        )
+
+    jobs: list[BenchmarkJob] = []
+    for clip in eligible:
+        jobs.append(
+            BenchmarkJob(
+                job_id=len(jobs),
+                clip_id=clip.clip_id,
+                rollout_index=0,
+                start_time_s=clip.sample_start_s,
+            )
+        )
+    return BenchmarkPlan(
+        clip_seconds=clip_seconds,
+        control_steps=control_steps,
+        step_dt=step_dt,
+        eligible_clips=tuple(eligible),
+        skipped_short_clips=tuple(skipped),
+        jobs=tuple(jobs),
+    )
+
+
+def compute_tracking_metrics(
+    aligned_ref_pos: np.ndarray,
+    aligned_robot_pos: np.ndarray,
+    root_pos_error_m: np.ndarray,
+    root_rot_error_rad: np.ndarray,
+    root_vel_error_m_s: np.ndarray,
+) -> dict[str, float]:
+    """Compute benchmark tracking metrics from aligned key bodies and root errors.
+
+    Key-body inputs are ``(T, B, 3)`` arrays in root/anchor coordinates. Root
+    error inputs are per-frame values computed with the same anchor metrics used
+    by ``MotionCommand``.
+    """
+    ref = np.asarray(aligned_ref_pos, dtype=np.float64)
+    robot = np.asarray(aligned_robot_pos, dtype=np.float64)
+    if ref.shape != robot.shape:
+        raise ValueError(f"aligned position shape mismatch: {ref.shape} vs {robot.shape}")
+    if ref.ndim != 3 or ref.shape[-1] != 3:
+        raise ValueError(f"aligned positions must be (T,B,3), got {ref.shape}")
+    if ref.shape[0] == 0 or ref.shape[1] == 0:
+        raise ValueError(
+            f"aligned positions must have non-empty T and B dimensions, got {ref.shape}"
+        )
+
+    root_pos = np.asarray(root_pos_error_m, dtype=np.float64)
+    root_rot = np.asarray(root_rot_error_rad, dtype=np.float64)
+    root_vel = np.asarray(root_vel_error_m_s, dtype=np.float64)
+    for name, values in (
+        ("root_pos_error_m", root_pos),
+        ("root_rot_error_rad", root_rot),
+        ("root_vel_error_m_s", root_vel),
+    ):
+        if values.size == 0:
+            raise ValueError(f"{name} must be non-empty")
+
+    pos_error = np.linalg.norm(ref - robot, axis=-1)
+    mpjpe = float(pos_error.mean())
+
+    return {
+        "mpjpe_m": mpjpe,
+        "root_pos_error_m": float(root_pos.mean()),
+        "root_rot_error_rad": float(root_rot.mean()),
+        "root_vel_error_m_s": float(root_vel.mean()),
+    }
+
+
+def summarize_rollouts(results: Sequence[RolloutResult]) -> dict[str, Any]:
+    if not results:
+        raise ValueError("Cannot summarize an empty benchmark result set")
+
+    def finite_mean(values: Iterable[float]) -> float:
+        arr = np.asarray(list(values), dtype=np.float64)
+        arr = arr[np.isfinite(arr)]
+        if arr.size == 0:
+            return float("nan")
+        return float(arr.mean())
+
+    clip_ids = sorted({result.clip_id for result in results})
+    per_clip: list[dict[str, Any]] = []
+    for clip_id in clip_ids:
+        clip_results = [result for result in results if result.clip_id == clip_id]
+        per_clip.append(
+            {
+                "clip_id": clip_id,
+                "rollouts": len(clip_results),
+                "success_rate": 100.0
+                * sum(1 for result in clip_results if result.success)
+                / len(clip_results),
+                "mpjpe_m": finite_mean(result.mpjpe_m for result in clip_results),
+                "root_pos_error_m": finite_mean(
+                    result.root_pos_error_m for result in clip_results
+                ),
+                "root_rot_error_rad": finite_mean(
+                    result.root_rot_error_rad for result in clip_results
+                ),
+                "root_vel_error_m_s": finite_mean(
+                    result.root_vel_error_m_s for result in clip_results
+                ),
+            }
+        )
+
+    return {
+        "global": {
+            "clips": len(clip_ids),
+            "rollouts": len(results),
+            "success_rate": 100.0
+            * sum(1 for result in results if result.success)
+            / len(results),
+            "mpjpe_m": finite_mean(result.mpjpe_m for result in results),
+            "root_pos_error_m": finite_mean(
+                result.root_pos_error_m for result in results
+            ),
+            "root_rot_error_rad": finite_mean(
+                result.root_rot_error_rad for result in results
+            ),
+            "root_vel_error_m_s": finite_mean(
+                result.root_vel_error_m_s for result in results
+            ),
+        },
+        "per_clip": per_clip,
+    }
+
+
+def write_benchmark_outputs(
+    output_dir: str | Path,
+    *,
+    text_stem: str,
+    metadata: dict[str, Any],
+    plan: BenchmarkPlan,
+    results: Sequence[RolloutResult],
+) -> dict[str, Path]:
+    output_path = Path(output_dir)
+    output_path.mkdir(parents=True, exist_ok=True)
+    summary = summarize_rollouts(results)
+
+    txt_path = output_path / f"{text_stem}.txt"
+    json_path = output_path / f"{text_stem}.json"
+    per_clip_path = output_path / f"{text_stem}-per_clip.csv"
+    per_rollout_path = output_path / f"{text_stem}-per_rollout.csv"
+
+    global_summary = summary["global"]
+    lines = [
+        "OmniXtreme-style Benchmark Results",
+        f"checkpoint: {metadata['checkpoint']}",
+        f"motion_file: {metadata['motion_file']}",
+        f"clip_seconds: {plan.clip_seconds:.6f}",
+        f"control_steps: {plan.control_steps}",
+        f"eligible_clips: {len(plan.eligible_clips)}",
+        f"skipped_short_clips: {len(plan.skipped_short_clips)}",
+        "",
+        f"MPJPE(m): {global_summary['mpjpe_m']:.6f}",
+        f"root_pos_error(m): {global_summary['root_pos_error_m']:.6f}",
+        f"root_rot_error(rad): {global_summary['root_rot_error_rad']:.6f}",
+        f"root_vel_error(m/s): {global_summary['root_vel_error_m_s']:.6f}",
+        f"success_rate(%): {global_summary['success_rate']:.6f}",
+    ]
+    txt_path.write_text("\n".join(lines) + "\n")
+
+    report = {
+        "metadata": metadata,
+        "protocol": {
+            "clip_seconds": plan.clip_seconds,
+            "control_steps": plan.control_steps,
+            "step_dt": plan.step_dt,
+        },
+        "global": global_summary,
+        "per_clip": summary["per_clip"],
+        "per_rollout": [asdict(result) for result in results],
+        "eligible_clips": [asdict(clip) for clip in plan.eligible_clips],
+        "skipped_short_clips": [asdict(clip) for clip in plan.skipped_short_clips],
+    }
+    json_path.write_text(json.dumps(_json_safe(report), indent=2, allow_nan=False))
+
+    with per_clip_path.open("w", newline="") as f:
+        writer = csv.DictWriter(
+            f,
+            fieldnames=[
+                "clip_id",
+                "rollouts",
+                "success_rate",
+                "mpjpe_m",
+                "root_pos_error_m",
+                "root_rot_error_rad",
+                "root_vel_error_m_s",
+            ],
+        )
+        writer.writeheader()
+        writer.writerows(summary["per_clip"])
+
+    with per_rollout_path.open("w", newline="") as f:
+        writer = csv.DictWriter(f, fieldnames=list(asdict(results[0]).keys()))
+        writer.writeheader()
+        for result in results:
+            writer.writerow(asdict(result))
+
+    return {
+        "summary_txt": txt_path,
+        "summary_json": json_path,
+        "per_clip_csv": per_clip_path,
+        "per_rollout_csv": per_rollout_path,
+    }
diff --git a/train_mimic/scripts/benchmark.py b/train_mimic/scripts/benchmark.py
index 7fdcb632..85393d28 100644
--- a/train_mimic/scripts/benchmark.py
+++ b/train_mimic/scripts/benchmark.py
@@ -1,35 +1,33 @@
 #!/usr/bin/env python3
-"""Benchmark a trained tracking policy on motion clips.
+"""Benchmark a G1 motion tracking policy with an OmniXtreme-style protocol.
 
-Runs policy rollout for a fixed number of evaluation steps and reports
-distribution statistics for motion-tracking errors.
-
-Can optionally render and save benchmark videos for qualitative inspection.
+Default protocol:
+    * 10 second clips at the policy control rate (500 steps at 50 Hz)
+    * One deterministic rollout per eligible motion clip
+    * MPJPE, root tracking errors, and success rate
 
 Usage:
-    # Benchmark only (no video)
     python train_mimic/scripts/benchmark.py \
-        --checkpoint logs/rsl_rl/g1_tracking/.../model_30000.pt \
+        --checkpoint logs/rsl_rl/g1_general_tracking/<run>/model_30000.pt \
         --motion_file data/datasets_precomputed \
-        --num_envs 1
-
-    # Single video (one continuous clip)
-    python train_mimic/scripts/benchmark.py ... --video --video_length 500
-
-    # Multiple separate clip videos
-    python train_mimic/scripts/benchmark.py ... --video --num_clips 10 --video_length 250
+        --num_envs 32
 """
 
 from __future__ import annotations
 
 import argparse
-import json
+import copy
 import os
+import sys
 from pathlib import Path
+from typing import Sequence
 
-import h5py
 import numpy as np
-from tensordict import TensorDictBase
+from tensordict import TensorDict
+
+REPO_ROOT = Path(__file__).resolve().parents[2]
+if str(REPO_ROOT) not in sys.path:
+    sys.path.insert(0, str(REPO_ROOT))
 
 from train_mimic.app import (
     DEFAULT_TASK,
@@ -40,207 +38,307 @@
     validate_checkpoint_path,
     validate_motion_file,
 )
-from train_mimic.data.dataset_lib import find_precomputed_motion_shards
-from teleopit.debug.rollout_trace import RolloutTraceWriter
-
-
-def _render_frame(unwrapped: object, split: bool = False, _cmd: object = None) -> np.ndarray:
-    """Render a frame using the environment's offline renderer (ghost included).
-
-    Args:
-        unwrapped: The unwrapped ManagerBasedRlEnv.
-        split: If True, render split-screen with camera lookat on ref (left)
-               and robot (right). Same scene, different camera targets.
-        _cmd: MotionCommand term (required when split=True).
-    """
-    if not split:
-        frame = unwrapped.render()
-        if frame is None:
-            raise RuntimeError("render() returned None; ensure render_mode='rgb_array'")
-        return frame
-
-    import mujoco
-
-    renderer = unwrapped._offline_renderer
-    cam = renderer._cam
-    env_idx = max(0, min(int(renderer._cfg.env_idx), int(unwrapped.sim.data.nworld) - 1))
-
-    # Full scene update (robot + ghost via debug vis).
-    debug_callback = (
-        unwrapped.update_visualizers if hasattr(unwrapped, "update_visualizers") else None
+from train_mimic.benchmarking import (
+    BenchmarkJob,
+    RolloutResult,
+    build_benchmark_plan,
+    compute_tracking_metrics,
+    load_clip_specs,
+    summarize_rollouts,
+    write_benchmark_outputs,
+)
+
+
+def parse_args(argv: Sequence[str] | None = None) -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="OmniXtreme-style benchmark for the G1 tracking policy."
     )
-    renderer.update(unwrapped.sim.data, debug_vis_callback=debug_callback)
-
-    # Save original camera state.
-    orig_type = cam.type
-    orig_trackbodyid = cam.trackbodyid
-    orig_lookat = cam.lookat.copy()
-
-    # --- Left: camera follows ref pose ---
-    ref_pos = _cmd.body_pos_w[env_idx, 0].cpu().numpy()
-    cam.type = mujoco.mjtCamera.mjCAMERA_FREE.value
-    cam.trackbodyid = -1
-    cam.lookat[:] = [ref_pos[0], ref_pos[1], 0.8]
-    renderer._renderer.update_scene(renderer._data, camera=cam)
-    # Re-apply ghost geoms after update_scene reset.
-    if debug_callback is not None:
-        from mjlab.viewer.native.visualizer import MujocoNativeDebugVisualizer
-        vis = MujocoNativeDebugVisualizer(
-            renderer._renderer.scene, renderer._model, env_idx=renderer._cfg.env_idx
-        )
-        debug_callback(vis)
-    frame_ref = renderer._renderer.render()
-
-    # --- Right: camera follows robot ---
-    robot_pos = unwrapped.sim.data.qpos[env_idx, :3].cpu().numpy()
-    cam.lookat[:] = [robot_pos[0], robot_pos[1], 0.8]
-    renderer._renderer.update_scene(renderer._data, camera=cam)
-    if debug_callback is not None:
-        vis = MujocoNativeDebugVisualizer(
-            renderer._renderer.scene, renderer._model, env_idx=renderer._cfg.env_idx
-        )
-        debug_callback(vis)
-    frame_robot = renderer._renderer.render()
-
-    # Restore camera.
-    cam.type = orig_type
-    cam.trackbodyid = orig_trackbodyid
-    cam.lookat[:] = orig_lookat
-
-    return np.concatenate([frame_ref, frame_robot], axis=1)
-
-
-def _to_float(value: object, torch_module: object) -> float:
-    if isinstance(value, torch_module.Tensor):
-        if value.numel() == 0:
-            return 0.0
-        return float(value.float().mean().item())
-    if isinstance(value, (float, int)):
-        return float(value)
-    raise TypeError(f"Unsupported value type: {type(value)}")
-
-
-def _stats(values: list[float]) -> dict[str, float]:
-    if not values:
-        return {
-            "mean": float("nan"),
-            "std": float("nan"),
-            "p50": float("nan"),
-            "p95": float("nan"),
-            "min": float("nan"),
-            "max": float("nan"),
-        }
-    arr = np.asarray(values, dtype=np.float64)
-    return {
-        "mean": float(arr.mean()),
-        "std": float(arr.std()),
-        "p50": float(np.percentile(arr, 50)),
-        "p95": float(np.percentile(arr, 95)),
-        "min": float(arr.min()),
-        "max": float(arr.max()),
-    }
-
-
-def parse_args() -> argparse.Namespace:
-    parser = argparse.ArgumentParser(description="Benchmark G1 tracking policy.")
     parser.add_argument("--checkpoint", type=str, required=True)
-    parser.add_argument("--motion_file", type=str, required=True, help="Path to precomputed training dataset root containing Teleopit shard_*.h5 files")
-    parser.add_argument("--num_envs", type=int, default=1)
-    parser.add_argument("--num_eval_steps", type=int, default=2000,
-                        help="Number of rollout steps for evaluation (default: 2000)")
-    parser.add_argument("--warmup_steps", type=int, default=100,
-                        help="Warmup steps ignored from metric aggregation (default: 100)")
+    parser.add_argument(
+        "--motion_file",
+        type=str,
+        required=True,
+        help="Precomputed training dataset root or shard produced by precompute_dataset.py",
+    )
+    parser.add_argument("--num_envs", type=int, default=32)
     parser.add_argument("--seed", type=int, default=42)
-    parser.add_argument("--video", action="store_true",
-                        help="Record benchmark video(s)")
-    parser.add_argument("--num_clips", type=int, default=1,
-                        help="Number of separate video clips to render (default: 1)")
-    parser.add_argument("--video_length", type=int, default=None,
-                        help="Steps per video clip (default: longest clip in motion file)")
-    parser.add_argument("--video_folder", type=str, default=None,
-                        help="Output folder for benchmark video(s)")
-    parser.add_argument("--split", action="store_true",
-                        help="Render split-screen video with two camera angles")
+    parser.add_argument("--clip_seconds", type=float, default=10.0)
+    parser.add_argument("--output_dir", type=str, default="benchmark_results")
     parser.add_argument("--device", type=str, default=None)
-    parser.add_argument("--task", type=str, default=DEFAULT_TASK,
-                        help="Task id to benchmark (default: %(default)s)")
     parser.add_argument(
-        "--debug_trace",
+        "--task",
         type=str,
-        default=None,
-        help="Optional .npz path to dump per-step benchmark trace for comparison",
+        default=DEFAULT_TASK,
+        help="Task id to benchmark (default: %(default)s)",
+    )
+    return parser.parse_args(argv)
+
+
+def _chunked(seq: Sequence[BenchmarkJob], size: int) -> list[Sequence[BenchmarkJob]]:
+    return [seq[i : i + size] for i in range(0, len(seq), size)]
+
+
+def _obs_tensordict(obs_dict: object, num_envs: int) -> TensorDict:
+    return TensorDict(obs_dict, batch_size=[num_envs])
+
+
+def _configure_benchmark_env_cfg(
+    base_cfg: object,
+    *,
+    motion_file: str,
+    clip_seconds: float,
+) -> object:
+    cfg = copy.deepcopy(base_cfg)
+    cfg.commands["motion"].motion_file = motion_file
+    cfg.commands["motion"].sampling_mode = "start"
+    cfg.commands["motion"].resample_on_clip_end = False
+    cfg.commands["motion"].pose_range = {}
+    cfg.commands["motion"].velocity_range = {}
+    cfg.commands["motion"].joint_position_range = (0.0, 0.0)
+    cfg.events = {}
+    cfg.episode_length_s = clip_seconds
+    cfg.auto_reset = False
+    return cfg
+
+
+def _reset_to_jobs(
+    env: object,
+    jobs: Sequence[BenchmarkJob],
+    torch_module: object,
+) -> TensorDict:
+    obs_dict, _extras = env.reset()
+    cmd = env.command_manager.get_term("motion")
+    env_ids = torch_module.arange(len(jobs), dtype=torch_module.long, device=env.device)
+    motion_ids = torch_module.tensor(
+        [job.clip_id for job in jobs],
+        dtype=torch_module.long,
+        device=env.device,
+    )
+    motion_times = torch_module.tensor(
+        [job.start_time_s for job in jobs],
+        dtype=torch_module.float32,
+        device=env.device,
+    )
+    cmd.reset_to_motion(env_ids, motion_ids, motion_times)
+    env.observation_manager.reset(env_ids)
+    env.scene.write_data_to_sim()
+    env.sim.forward()
+    env.command_manager.compute(dt=0.0)
+    env.sim.sense()
+    obs_dict = env.observation_manager.compute(update_history=True)
+    env.obs_buf = obs_dict
+    return _obs_tensordict(obs_dict, len(jobs))
+
+
+def _aligned_keybody_positions(cmd: object) -> tuple[np.ndarray, np.ndarray]:
+    from mjlab.utils.lab_api.math import quat_apply, quat_inv
+
+    ref_anchor_pos = cmd.anchor_pos_w[:, None, :]
+    robot_anchor_pos = cmd.robot_anchor_pos_w[:, None, :]
+    num_bodies = cmd.body_pos_w.shape[1]
+
+    ref_anchor_inv = quat_inv(cmd.anchor_quat_w)[:, None, :].expand(-1, num_bodies, -1)
+    robot_anchor_inv = quat_inv(cmd.robot_anchor_quat_w)[:, None, :].expand(
+        -1, num_bodies, -1
+    )
+    ref_aligned = quat_apply(ref_anchor_inv, cmd.body_pos_w - ref_anchor_pos)
+    robot_aligned = quat_apply(robot_anchor_inv, cmd.robot_body_pos_w - robot_anchor_pos)
+    return (
+        ref_aligned.detach().cpu().numpy().astype(np.float32, copy=False),
+        robot_aligned.detach().cpu().numpy().astype(np.float32, copy=False),
+    )
+
+
+def _root_tracking_errors(cmd: object) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+    from mjlab.utils.lab_api.math import quat_error_magnitude
+    import torch
+
+    root_pos_error = torch.norm(cmd.anchor_pos_w - cmd.robot_anchor_pos_w, dim=-1)
+    root_rot_error = quat_error_magnitude(cmd.anchor_quat_w, cmd.robot_anchor_quat_w)
+    root_vel_error = torch.norm(
+        cmd.anchor_lin_vel_w - cmd.robot_anchor_lin_vel_w,
+        dim=-1,
+    )
+    return (
+        root_pos_error.detach().cpu().numpy().astype(np.float32, copy=False),
+        root_rot_error.detach().cpu().numpy().astype(np.float32, copy=False),
+        root_vel_error.detach().cpu().numpy().astype(np.float32, copy=False),
     )
-    return parser.parse_args()
-
-
-def _load_motion_dir_video_metadata(motion_dir: str) -> tuple[float, int]:
-    clip_fps: float | None = None
-    max_clip_frames = 0
-    for shard_path in find_precomputed_motion_shards(motion_dir):
-        with h5py.File(shard_path, "r") as h5:
-            fps_arr = np.asarray(h5["clip_fps"], dtype=np.float32)
-            if fps_arr.size == 0:
-                continue
-            cur_fps = float(fps_arr[0])
-            if np.any(fps_arr != cur_fps):
-                raise ValueError(f"inconsistent fps within HDF5 shard: {shard_path}")
-            if clip_fps is None:
-                clip_fps = cur_fps
-            elif clip_fps != cur_fps:
-                raise ValueError(
-                    f"inconsistent fps across shards: {shard_path} has {cur_fps}, expected {clip_fps}"
-                )
-            max_clip_frames = max(max_clip_frames, int(np.asarray(h5["clip_lengths"]).max()))
-    if clip_fps is None:
-        raise ValueError(f"failed reading HDF5 shard metadata from {motion_dir}")
-    return clip_fps, max_clip_frames
 
 
-def _tensor_to_numpy(value: object, torch_module: object) -> np.ndarray:
-    if isinstance(value, torch_module.Tensor):
-        return value.detach().cpu().numpy()
-    return np.asarray(value)
+def _failure_reason(env: object, env_index: int) -> str:
+    manager = env.termination_manager
+    for term_name in manager.active_terms:
+        term_cfg = manager.get_term_cfg(term_name)
+        if term_cfg.time_out:
+            continue
+        if bool(manager.get_term(term_name)[env_index].item()):
+            return term_name
+    for term_name in manager.active_terms:
+        term_cfg = manager.get_term_cfg(term_name)
+        if term_cfg.time_out and bool(manager.get_term(term_name)[env_index].item()):
+            return term_name
+    return "done"
+
+
+def _run_batch(
+    *,
+    batch_index: int,
+    jobs: Sequence[BenchmarkJob],
+    base_env_cfg: object,
+    agent_cfg: object,
+    runner_cls: object,
+    fallback_runner_cls: object,
+    checkpoint: str,
+    log_dir: str,
+    device: str,
+    torch_module: object,
+    ManagerBasedRlEnv: object,
+    RslRlVecEnvWrapper: object,
+    clip_seconds: float,
+    control_steps: int,
+    seed: int,
+) -> list[RolloutResult]:
+    env_cfg = _configure_benchmark_env_cfg(
+        base_env_cfg,
+        motion_file=base_env_cfg.commands["motion"].motion_file,
+        clip_seconds=clip_seconds,
+    )
+    env_cfg.seed = seed + batch_index
+    env_cfg.scene.num_envs = len(jobs)
 
+    env = ManagerBasedRlEnv(cfg=env_cfg, device=device, render_mode=None)
+    wrapped_env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions)
+    agent_dict = build_runner_cfg_dict(agent_cfg, force_tensorboard=True)
+    RunnerCls = runner_cls or fallback_runner_cls
+    runner = RunnerCls(wrapped_env, agent_dict, log_dir=log_dir, device=device)
+    runner.load(checkpoint, map_location=device)
+    policy = runner.get_inference_policy(device=device)
 
-def _first_env_numpy(value: object, torch_module: object) -> np.ndarray:
-    array = _tensor_to_numpy(value, torch_module)
-    if array.ndim == 0:
-        return array.reshape(1)
-    return array[0].copy()
+    aligned_ref_by_env: list[list[np.ndarray]] = [[] for _ in jobs]
+    aligned_robot_by_env: list[list[np.ndarray]] = [[] for _ in jobs]
+    root_pos_error_by_env: list[list[float]] = [[] for _ in jobs]
+    root_rot_error_by_env: list[list[float]] = [[] for _ in jobs]
+    root_vel_error_by_env: list[list[float]] = [[] for _ in jobs]
+    active = np.ones(len(jobs), dtype=bool)
+    finished: dict[int, tuple[bool, int, int | None, str | None]] = {}
 
+    try:
+        obs = _reset_to_jobs(env, jobs, torch_module)
+        cmd = env.command_manager.get_term("motion")
+        for step in range(control_steps):
+            ref_aligned, robot_aligned = _aligned_keybody_positions(cmd)
+            root_pos_error, root_rot_error, root_vel_error = _root_tracking_errors(cmd)
+            for env_index, is_active in enumerate(active):
+                if is_active:
+                    aligned_ref_by_env[env_index].append(ref_aligned[env_index])
+                    aligned_robot_by_env[env_index].append(robot_aligned[env_index])
+                    root_pos_error_by_env[env_index].append(
+                        float(root_pos_error[env_index])
+                    )
+                    root_rot_error_by_env[env_index].append(
+                        float(root_rot_error[env_index])
+                    )
+                    root_vel_error_by_env[env_index].append(
+                        float(root_vel_error[env_index])
+                    )
+
+            with torch_module.no_grad():
+                actions = policy(obs)
+            if agent_cfg.clip_actions is not None:
+                actions = torch_module.clamp(
+                    actions,
+                    -agent_cfg.clip_actions,
+                    agent_cfg.clip_actions,
+                )
 
-def _extract_obs_for_trace(obs: object, torch_module: object) -> tuple[np.ndarray, np.ndarray | None]:
-    if isinstance(obs, TensorDictBase):
-        actor = _first_env_numpy(obs["actor"], torch_module).astype(np.float32, copy=False)
-        actor_history = None
-        if "actor_history" in obs.keys():
-            actor_history = _first_env_numpy(obs["actor_history"], torch_module).astype(
-                np.float32, copy=False
+            obs_dict, _rewards, terminated, truncated, _extras = env.step(actions)
+            obs = _obs_tensordict(obs_dict, len(jobs))
+
+            done = (terminated | truncated).detach().cpu().numpy().astype(bool)
+            terminated_np = terminated.detach().cpu().numpy().astype(bool)
+            truncated_np = truncated.detach().cpu().numpy().astype(bool)
+            done_envs = [int(env_index) for env_index, is_done in enumerate(done) if is_done]
+            for env_index, is_done in enumerate(done):
+                if not active[env_index] or not is_done:
+                    continue
+                reached_horizon = step + 1 >= control_steps
+                success = bool(
+                    truncated_np[env_index]
+                    and reached_horizon
+                    and not terminated_np[env_index]
+                )
+                failure_step = None if success else step + 1
+                failure_reason = None if success else _failure_reason(env, env_index)
+                finished[env_index] = (success, step + 1, failure_step, failure_reason)
+                active[env_index] = False
+
+            if step + 1 < control_steps and done_envs:
+                env_ids = torch_module.tensor(
+                    done_envs,
+                    dtype=torch_module.long,
+                    device=env.device,
+                )
+                obs_dict, _extras = env.reset(env_ids=env_ids)
+                obs = _obs_tensordict(obs_dict, len(jobs))
+
+            if not active.any():
+                break
+    finally:
+        env.close()
+
+    results: list[RolloutResult] = []
+    for env_index, job in enumerate(jobs):
+        success, steps, failure_step, failure_reason = finished.get(
+            env_index,
+            (True, control_steps, None, None),
+        )
+        metrics = compute_tracking_metrics(
+            np.stack(aligned_ref_by_env[env_index], axis=0),
+            np.stack(aligned_robot_by_env[env_index], axis=0),
+            np.asarray(root_pos_error_by_env[env_index], dtype=np.float32),
+            np.asarray(root_rot_error_by_env[env_index], dtype=np.float32),
+            np.asarray(root_vel_error_by_env[env_index], dtype=np.float32),
+        )
+        if not success:
+            metrics = {
+                "mpjpe_m": float("nan"),
+                "root_pos_error_m": float("nan"),
+                "root_rot_error_rad": float("nan"),
+                "root_vel_error_m_s": float("nan"),
+            }
+        results.append(
+            RolloutResult(
+                job_id=job.job_id,
+                clip_id=job.clip_id,
+                rollout_index=job.rollout_index,
+                success=success,
+                steps=steps,
+                failure_step=failure_step,
+                failure_reason=failure_reason,
+                mpjpe_m=metrics["mpjpe_m"],
+                root_pos_error_m=metrics["root_pos_error_m"],
+                root_rot_error_rad=metrics["root_rot_error_rad"],
+                root_vel_error_m_s=metrics["root_vel_error_m_s"],
             )
-        return actor, actor_history
-    raise TypeError(f"Unsupported observation container for debug trace: {type(obs)}")
+        )
+    return results
 
 
-def main() -> int:
-    args = parse_args()
+def main(argv: Sequence[str] | None = None) -> int:
+    args = parse_args(argv)
 
-    if args.warmup_steps < 0:
-        raise ValueError("--warmup_steps must be >= 0")
-    if not args.video and args.num_eval_steps <= args.warmup_steps:
-        raise ValueError("--num_eval_steps must be greater than --warmup_steps")
-    if args.video and args.num_envs != 1:
-        raise ValueError("--video currently requires --num_envs 1")
-    if args.debug_trace is not None and args.num_envs != 1:
-        raise ValueError("--debug_trace currently requires --num_envs 1")
-    validate_motion_file(args.motion_file)
+    if args.num_envs <= 0:
+        raise ValueError("--num_envs must be > 0")
+    if args.clip_seconds <= 0.0:
+        raise ValueError("--clip_seconds must be > 0")
 
-    # Set render backend before importing modules that may initialize MuJoCo/GL.
-    if args.video and "MUJOCO_GL" not in os.environ:
-        os.environ["MUJOCO_GL"] = "egl"
-        print("[INFO] --video enabled, MUJOCO_GL not set. Defaulting to MUJOCO_GL=egl.")
-    if args.video and "PYOPENGL_PLATFORM" not in os.environ:
-        os.environ["PYOPENGL_PLATFORM"] = "egl"
-        print("[INFO] --video enabled, PYOPENGL_PLATFORM not set. Defaulting to PYOPENGL_PLATFORM=egl.")
+    validate_motion_file(args.motion_file)
+    try:
+        validate_checkpoint_path(args.checkpoint)
+    except FileNotFoundError as exc:
+        print(f"Error: {exc}")
+        return 1
 
     (
         torch,
@@ -252,328 +350,99 @@ def main() -> int:
         _load_runner_cls,
         configure_torch_backends,
     ) = import_training_stack()
-
-    try:
-        validate_checkpoint_path(args.checkpoint)
-    except FileNotFoundError as exc:
-        print(f"Error: {exc}")
-        return 1
-
     configure_torch_backends()
 
-    # Load configs (play=True disables corruption, push_robot, etc.)
-    task_name, env_cfg, agent_cfg, runner_cls = load_task_components(
+    task_name, base_env_cfg, agent_cfg, runner_cls = load_task_components(
         args.task,
         play=True,
         load_env_cfg=_load_env_cfg,
         load_rl_cfg=_load_rl_cfg,
         load_runner_cls=_load_runner_cls,
     )
-
-    # Configure for benchmark
-    env_cfg.seed = args.seed
-    env_cfg.scene.num_envs = args.num_envs
-    env_cfg.commands["motion"].motion_file = args.motion_file
-    env_cfg.commands["motion"].pose_range = {}
-    env_cfg.commands["motion"].velocity_range = {}
-
-    # Use uniform sampling so each reset picks a different motion segment.
-    if args.video and args.num_clips > 1:
-        env_cfg.commands["motion"].sampling_mode = "uniform"
-
-    step_dt = float(env_cfg.decimation) * float(env_cfg.sim.mujoco.timestep)
-    required_episode_s = args.num_eval_steps * step_dt + 1.0
-    if float(env_cfg.episode_length_s) < required_episode_s:
-        env_cfg.episode_length_s = required_episode_s
-    if args.video:
-        env_cfg.terminations.pop("time_out", None)
-        env_cfg.terminations.pop("anchor_pos", None)
-        env_cfg.terminations.pop("anchor_ori", None)
-        env_cfg.terminations.pop("ee_body_pos", None)
-        env_cfg.terminations.pop("body_z_tracking_failure", None)
-        env_cfg.terminations.pop("gravity_tracking_failure", None)
+    base_env_cfg.commands["motion"].motion_file = args.motion_file
+    benchmark_env_cfg = _configure_benchmark_env_cfg(
+        base_env_cfg,
+        motion_file=args.motion_file,
+        clip_seconds=args.clip_seconds,
+    )
+    step_dt = float(benchmark_env_cfg.decimation) * float(
+        benchmark_env_cfg.sim.mujoco.timestep
+    )
+    clips = load_clip_specs(
+        args.motion_file,
+        window_steps=benchmark_env_cfg.commands["motion"].window_steps,
+    )
+    plan = build_benchmark_plan(
+        clips,
+        clip_seconds=args.clip_seconds,
+        step_dt=step_dt,
+    )
 
     device = resolve_device(args.device, torch)
-
-    # Create env
-    render_mode = "rgb_array" if args.video else None
-    try:
-        env = ManagerBasedRlEnv(cfg=env_cfg, device=device, render_mode=render_mode)
-    except Exception as exc:
-        if args.video:
-            raise RuntimeError(
-                "Video renderer initialization failed. "
-                "Try setting MUJOCO_GL=egl (or osmesa) and make sure the corresponding "
-                "OpenGL backend libraries are available on this machine."
-            ) from exc
-        else:
-            raise
-
-    env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions)
-
-    # Auto-resolve video_length from motion file if not specified.
-    if args.video and args.video_length is None:
-        clip_fps, max_clip_frames = _load_motion_dir_video_metadata(args.motion_file)
-        step_dt = env.unwrapped.step_dt
-        args.video_length = int(max_clip_frames / clip_fps / step_dt)
-        print(f"[INFO] Auto video_length={args.video_length} steps "
-              f"({max_clip_frames} frames / {clip_fps} fps / {step_dt} step_dt = "
-              f"{args.video_length * step_dt:.1f}s)")
-    elif args.video_length is None:
-        args.video_length = 600
-
-    # Auto-adjust num_eval_steps to cover all video clips.
-    if args.video:
-        min_steps = args.num_clips * args.video_length + args.warmup_steps
-        if args.num_eval_steps < min_steps:
-            print(f"[INFO] Increasing --num_eval_steps from {args.num_eval_steps} to {min_steps} "
-                  f"to cover {args.num_clips} clips x {args.video_length} steps + {args.warmup_steps} warmup.")
-            args.num_eval_steps = min_steps
-
+    batches = _chunked(plan.jobs, args.num_envs)
     log_dir = os.path.dirname(args.checkpoint)
-    agent_dict = build_runner_cfg_dict(agent_cfg, force_tensorboard=True)
-    RunnerCls = runner_cls or MjlabOnPolicyRunner
-    runner = RunnerCls(env, agent_dict, log_dir=log_dir, device=device)
-    runner.load(args.checkpoint, map_location=device)
-    policy = runner.get_inference_policy(device=device)
+    results: list[RolloutResult] = []
 
-    # --- Video recording setup ---
-    video_writer = None
-    video_folder: Path | None = None
-    video_paths: list[Path] = []
-    clip_step_counter = 0
-    clip_idx = 0
-
-    if args.video:
-        import imageio.v2 as imageio
-
-        video_folder = Path(args.video_folder or "benchmark_results/videos")
-        video_folder.mkdir(parents=True, exist_ok=True)
-        video_fps = max(1, int(round(1.0 / env.unwrapped.step_dt)))
-
-        def _open_clip_writer(idx: int):
-            nonlocal video_writer, clip_step_counter
-            if args.num_clips == 1:
-                path = video_folder / "benchmark.mp4"
-            else:
-                path = video_folder / f"clip_{idx:03d}.mp4"
-            video_paths.append(path)
-            video_writer = imageio.get_writer(str(path), fps=video_fps, quality=8)
-            clip_step_counter = 0
-            print(f"[INFO] Recording clip {idx + 1}/{args.num_clips}: {path}")
-
-        _open_clip_writer(0)
-
-    # --- Benchmark loop ---
-    obs = env.get_observations()
-    unwrapped = env.unwrapped
-    cmd = unwrapped.command_manager.get_term("motion")
-    metric_keys = sorted(cmd.metrics.keys())
-    metric_series: dict[str, list[float]] = {k: [] for k in metric_keys}
-    reward_series: list[float] = []
-    reset_log_series: dict[str, list[float]] = {}
-    trace_writer: RolloutTraceWriter | None = None
-    if args.debug_trace is not None:
-        trace_writer = RolloutTraceWriter(
-            args.debug_trace,
-            metadata={
-                "source": "benchmark",
-                "task": args.task,
-                "checkpoint": args.checkpoint,
-                "motion_file": args.motion_file,
-                "num_envs": args.num_envs,
-                "step_dt": float(env.unwrapped.step_dt),
-            },
+    print(
+        "Running OmniXtreme-style benchmark: "
+        f"{len(plan.eligible_clips)} clips, {len(plan.jobs)} rollouts, "
+        f"{plan.control_steps} steps/rollout, batch size {args.num_envs}."
+    )
+    if plan.skipped_short_clips:
+        print(
+            f"Skipping {len(plan.skipped_short_clips)} clips shorter than "
+            f"{args.clip_seconds:.2f}s."
         )
 
-    done_events = 0
-    timeout_events = 0
-    ep_len_buf = torch.zeros(args.num_envs, dtype=torch.long, device=device)
-    completed_episode_lengths: list[int] = []
-
-    try:
-        for step in range(args.num_eval_steps):
-            actor_obs, actor_history = _extract_obs_for_trace(obs, torch)
-            with torch.no_grad():
-                actions = policy(obs)
-            obs, rewards, dones, extras = env.step(actions)
-            ep_len_buf += 1
-
-            # Record video frame.
-            if video_writer is not None and clip_idx < args.num_clips:
-                frame = _render_frame(env.unwrapped, split=args.split, _cmd=cmd)
-                video_writer.append_data(frame)
-                clip_step_counter += 1
-
-                # Close current clip and open next one.
-                if clip_step_counter >= args.video_length:
-                    video_writer.close()
-                    video_writer = None
-                    clip_idx += 1
-                    if clip_idx < args.num_clips:
-                        # Reset env to sample a new motion segment.
-                        obs, _ = env.reset()
-                        ep_len_buf[:] = 0
-                        _open_clip_writer(clip_idx)
-
-            done_mask = dones > 0
-            num_done = int(done_mask.sum().item())
-            if num_done > 0:
-                done_events += num_done
-                completed_episode_lengths.extend(ep_len_buf[done_mask].detach().cpu().tolist())
-                ep_len_buf[done_mask] = 0
-                extras_log = extras.get("log", {}) if isinstance(extras, dict) else {}
-                if isinstance(extras_log, dict):
-                    for key, value in extras_log.items():
-                        if key.startswith(("Episode_Reward/", "Episode_Termination/", "Metrics/motion/")):
-                            reset_log_series.setdefault(key, []).append(_to_float(value, torch))
-
-            if step < args.warmup_steps:
-                continue
-
-            reward_series.append(_to_float(rewards, torch))
-            for key in metric_keys:
-                metric_series[key].append(_to_float(cmd.metrics[key], torch))
-
-            if isinstance(extras, dict) and "time_outs" in extras and isinstance(extras["time_outs"], torch.Tensor):
-                timeout_events += int(extras["time_outs"].sum().item())
-
-            if trace_writer is not None:
-                trace_writer.add_step(
-                    step=np.int64(step),
-                    policy_time=np.float64(step * env.unwrapped.step_dt),
-                    obs=actor_obs,
-                    obs_history=actor_history,
-                    action=_first_env_numpy(actions, torch).astype(np.float32, copy=False),
-                    reward=np.asarray(_to_float(rewards, torch), dtype=np.float32),
-                    motion_joint_pos=_first_env_numpy(cmd.joint_pos, torch).astype(np.float32, copy=False),
-                    motion_joint_vel=_first_env_numpy(cmd.joint_vel, torch).astype(np.float32, copy=False),
-                    motion_anchor_pos_w=_first_env_numpy(cmd.anchor_pos_w, torch).astype(np.float32, copy=False),
-                    motion_anchor_quat_w=_first_env_numpy(cmd.anchor_quat_w, torch).astype(np.float32, copy=False),
-                    motion_anchor_lin_vel_w=_first_env_numpy(cmd.anchor_lin_vel_w, torch).astype(np.float32, copy=False),
-                    motion_anchor_ang_vel_w=_first_env_numpy(cmd.anchor_ang_vel_w, torch).astype(np.float32, copy=False),
-                    robot_joint_pos=_first_env_numpy(cmd.robot_joint_pos, torch).astype(np.float32, copy=False),
-                    robot_joint_vel=_first_env_numpy(cmd.robot_joint_vel, torch).astype(np.float32, copy=False),
-                    robot_anchor_pos_w=_first_env_numpy(cmd.robot_anchor_pos_w, torch).astype(np.float32, copy=False),
-                    robot_anchor_quat_w=_first_env_numpy(cmd.robot_anchor_quat_w, torch).astype(np.float32, copy=False),
-                    done=np.asarray(bool(dones[0].item()), dtype=np.bool_),
-                )
-    finally:
-        if video_writer is not None:
-            video_writer.close()
-        if trace_writer is not None:
-            trace_writer.save()
-        env.close()
-
-    # --- Report ---
-    effective_steps = args.num_eval_steps - args.warmup_steps
-    if effective_steps <= 0:
-        raise RuntimeError("No effective evaluation steps. Increase --num_eval_steps or decrease --warmup_steps.")
-
-    metric_stats = {key: _stats(vals) for key, vals in metric_series.items()}
-    reward_stats = _stats(reward_series)
-    reset_log_stats = {key: _stats(vals) for key, vals in reset_log_series.items()}
-
-    anchor_pos = metric_stats.get("error_anchor_pos", {}).get("mean", float("nan"))
-    anchor_rot = metric_stats.get("error_anchor_rot", {}).get("mean", float("nan"))
-    body_pos = metric_stats.get("error_body_pos", {}).get("mean", float("nan"))
-    total = anchor_pos + anchor_rot + body_pos
-
-    eval_transitions = args.num_eval_steps * args.num_envs
-    done_rate = done_events / max(eval_transitions, 1)
-    timeout_rate = timeout_events / max(eval_transitions, 1)
-    ep_len_stats = _stats([float(v) for v in completed_episode_lengths])
-
-    print(f"\nBenchmark Results ({effective_steps} effective steps, warmup {args.warmup_steps}):")
-    print(f"  total_error(anchor_pos+anchor_rot+body_pos): {total:.4f}")
-    print(f"  error_anchor_pos: {anchor_pos:.4f}")
-    print(f"  error_anchor_rot: {anchor_rot:.4f}")
-    print(f"  error_body_pos: {body_pos:.4f}")
-    print(f"  mean_step_reward: {reward_stats['mean']:.4f}")
-    print(f"  done_rate: {done_rate:.4f}")
-    print(f"  timeout_rate: {timeout_rate:.4f}")
-    print(f"  completed_episodes: {len(completed_episode_lengths)}")
-    print(f"  mean_episode_length: {ep_len_stats['mean']:.2f}")
-
-    print("\nMetric distributions (mean / p50 / p95):")
-    for key in (
-        "error_anchor_pos",
-        "error_anchor_rot",
-        "error_anchor_lin_vel",
-        "error_anchor_ang_vel",
-        "error_body_pos",
-        "error_body_rot",
-        "error_body_lin_vel",
-        "error_body_ang_vel",
-        "error_joint_pos",
-        "error_joint_vel",
-    ):
-        if key not in metric_stats:
-            continue
-        stats = metric_stats[key]
-        print(f"  {key}: {stats['mean']:.4f} / {stats['p50']:.4f} / {stats['p95']:.4f}")
-
-    Path("benchmark_results").mkdir(exist_ok=True)
-    output_path = Path("benchmark_results") / f"{args.task}-{Path(args.checkpoint).stem}.txt"
-    json_path = Path("benchmark_results") / f"{args.task}-{Path(args.checkpoint).stem}.json"
-
-    lines = [
-        f"checkpoint: {args.checkpoint}",
-        f"motion_file: {args.motion_file}",
-        f"num_envs: {args.num_envs}",
-        f"num_eval_steps: {args.num_eval_steps}",
-        f"warmup_steps: {args.warmup_steps}",
-        f"effective_steps: {effective_steps}",
-        "",
-        f"total_error(anchor_pos+anchor_rot+body_pos): {total:.6f}",
-        f"error_anchor_pos_mean: {anchor_pos:.6f}",
-        f"error_anchor_rot_mean: {anchor_rot:.6f}",
-        f"error_body_pos_mean: {body_pos:.6f}",
-        f"mean_step_reward: {reward_stats['mean']:.6f}",
-        f"done_rate: {done_rate:.6f}",
-        f"timeout_rate: {timeout_rate:.6f}",
-        f"completed_episodes: {len(completed_episode_lengths)}",
-        f"mean_episode_length: {ep_len_stats['mean']:.6f}",
-        "",
-        "metric_stats(mean,std,p50,p95,min,max):",
-    ]
-    for key in sorted(metric_stats.keys()):
-        s = metric_stats[key]
-        lines.append(
-            f"{key}: {s['mean']:.6f}, {s['std']:.6f}, {s['p50']:.6f}, {s['p95']:.6f}, {s['min']:.6f}, {s['max']:.6f}"
+    for batch_index, batch_jobs in enumerate(batches):
+        print(
+            f"[{batch_index + 1}/{len(batches)}] "
+            f"rollouts {batch_jobs[0].job_id}..{batch_jobs[-1].job_id}"
         )
-    if reset_log_stats:
-        lines.append("")
-        lines.append("reset_log_stats(mean,std,p50,p95,min,max):")
-        for key in sorted(reset_log_stats.keys()):
-            s = reset_log_stats[key]
-            lines.append(
-                f"{key}: {s['mean']:.6f}, {s['std']:.6f}, {s['p50']:.6f}, {s['p95']:.6f}, {s['min']:.6f}, {s['max']:.6f}"
+        results.extend(
+            _run_batch(
+                batch_index=batch_index,
+                jobs=batch_jobs,
+                base_env_cfg=benchmark_env_cfg,
+                agent_cfg=agent_cfg,
+                runner_cls=runner_cls,
+                fallback_runner_cls=MjlabOnPolicyRunner,
+                checkpoint=args.checkpoint,
+                log_dir=log_dir,
+                device=device,
+                torch_module=torch,
+                ManagerBasedRlEnv=ManagerBasedRlEnv,
+                RslRlVecEnvWrapper=RslRlVecEnvWrapper,
+                clip_seconds=args.clip_seconds,
+                control_steps=plan.control_steps,
+                seed=args.seed,
             )
-    output_path.write_text("\n".join(lines) + "\n")
-
-    report = {
-        "checkpoint": args.checkpoint,
-        "motion_file": args.motion_file,
-        "num_envs": args.num_envs,
-        "num_eval_steps": args.num_eval_steps,
-        "warmup_steps": args.warmup_steps,
-        "effective_steps": effective_steps,
-        "total_error": total,
-        "mean_step_reward": reward_stats["mean"],
-        "done_rate": done_rate,
-        "timeout_rate": timeout_rate,
-        "completed_episodes": len(completed_episode_lengths),
-        "mean_episode_length": ep_len_stats["mean"],
-        "metric_stats": metric_stats,
-        "reset_log_stats": reset_log_stats,
-    }
-    json_path.write_text(json.dumps(report, indent=2))
-
-    print(f"\nSaved summary to: {output_path}")
-    print(f"Saved detailed json to: {json_path}")
-    for vp in video_paths:
-        print(f"Saved video: {vp}")
+        )
+
+    output_stem = f"{task_name}-{Path(args.checkpoint).stem}-omnixtreme"
+    paths = write_benchmark_outputs(
+        args.output_dir,
+        text_stem=output_stem,
+        metadata={
+            "task": task_name,
+            "checkpoint": args.checkpoint,
+            "motion_file": args.motion_file,
+            "seed": args.seed,
+            "num_envs": args.num_envs,
+        },
+        plan=plan,
+        results=results,
+    )
+
+    summary = summarize_rollouts(results)["global"]
+    print("\nBenchmark Results:")
+    print(f"  MPJPE(m): {summary['mpjpe_m']:.4f}")
+    print(f"  root_pos_error(m): {summary['root_pos_error_m']:.4f}")
+    print(f"  root_rot_error(rad): {summary['root_rot_error_rad']:.4f}")
+    print(f"  root_vel_error(m/s): {summary['root_vel_error_m_s']:.4f}")
+    print(f"  success_rate(%): {summary['success_rate']:.2f}")
+    for label, path in paths.items():
+        print(f"Saved {label}: {path}")
     return 0
 
 
diff --git a/train_mimic/tasks/tracking/mdp/commands.py b/train_mimic/tasks/tracking/mdp/commands.py
index add39de0..9648bf6e 100644
--- a/train_mimic/tasks/tracking/mdp/commands.py
+++ b/train_mimic/tasks/tracking/mdp/commands.py
@@ -826,6 +826,60 @@ def _resample_command(self, env_ids: torch.Tensor):
 
         self._reset_envs_to_current_reference(env_ids)
 
+    def reset_to_motion(
+        self,
+        env_ids: torch.Tensor,
+        motion_ids: torch.Tensor,
+        motion_times: torch.Tensor,
+    ) -> None:
+        """Reset selected environments to exact motion clips/times.
+
+        This is intended for deterministic benchmark rollouts. Normal training
+        and playback should continue using the configured sampling mode.
+        """
+        if env_ids.ndim != 1:
+            raise ValueError(f"env_ids must be 1-D, got {tuple(env_ids.shape)}")
+        if motion_ids.ndim != 1 or motion_times.ndim != 1:
+            raise ValueError(
+                "motion_ids and motion_times must be 1-D, got "
+                f"{tuple(motion_ids.shape)} and {tuple(motion_times.shape)}"
+            )
+        if not (len(env_ids) == len(motion_ids) == len(motion_times)):
+            raise ValueError(
+                "env_ids, motion_ids, and motion_times must have matching lengths, got "
+                f"{len(env_ids)}, {len(motion_ids)}, {len(motion_times)}"
+            )
+        if len(env_ids) == 0:
+            return
+
+        env_ids = env_ids.to(device=self.device, dtype=torch.long)
+        motion_ids = motion_ids.to(device=self.device, dtype=torch.long)
+        motion_times = motion_times.to(device=self.device, dtype=self.motion_times.dtype)
+        if torch.any(motion_ids < 0) or torch.any(motion_ids >= self.motion.num_clips):
+            raise ValueError(
+                f"motion_ids out of range [0, {self.motion.num_clips}): "
+                f"{motion_ids.detach().cpu().tolist()}"
+            )
+
+        sample_starts = self.motion.clip_sample_start_s[motion_ids]
+        sample_ends = self.motion.clip_sample_end_s[motion_ids]
+        invalid_times = (motion_times < sample_starts) | (motion_times >= sample_ends)
+        if torch.any(invalid_times):
+            bad = torch.where(invalid_times)[0]
+            first = int(bad[0].item())
+            raise ValueError(
+                "motion_times must be inside each clip's valid sample range; "
+                f"motion_id={int(motion_ids[first].item())}, "
+                f"time={float(motion_times[first].item()):.6f}, "
+                f"range=[{float(sample_starts[first].item()):.6f}, "
+                f"{float(sample_ends[first].item()):.6f})"
+            )
+
+        self.motion_ids[env_ids] = motion_ids
+        self.motion_times[env_ids] = motion_times
+        self.time_left[env_ids] = float(self.cfg.resampling_time_range[1])
+        self._reset_envs_to_current_reference(env_ids)
+
     def _reset_envs_to_current_reference(self, env_ids: torch.Tensor) -> None:
         if env_ids.numel() == 0:
             return
@@ -920,8 +974,13 @@ def _update_command(self):
         exceeded = self.motion_times >= end_times
 
         env_ids = torch.where(exceeded)[0]
-        if env_ids.numel() > 0:
+        if env_ids.numel() > 0 and self.cfg.resample_on_clip_end:
             self._resample_command(env_ids)
+        elif env_ids.numel() > 0:
+            self.motion_times[env_ids] = torch.nextafter(
+                end_times[env_ids],
+                self.motion.clip_sample_start_s[self.motion_ids[env_ids]],
+            )
 
         self._refresh_frame_cache()
 
@@ -1037,6 +1096,7 @@ class MotionCommandCfg(CommandTermCfg):
     velocity_range: dict[str, tuple[float, float]] = field(default_factory=dict)
     joint_position_range: tuple[float, float] = (-0.52, 0.52)
     sampling_mode: Literal["uniform", "start", "rewind"] = "rewind"
+    resample_on_clip_end: bool = True
     window_steps: tuple[int, ...] = (0,)
     rewind_prob: float = 0.8
     rewind_min_steps: int = 25
diff --git a/train_mimic/tasks/tracking/tracking_env_cfg.py b/train_mimic/tasks/tracking/tracking_env_cfg.py
index ece6329a..eb1faaae 100644
--- a/train_mimic/tasks/tracking/tracking_env_cfg.py
+++ b/train_mimic/tasks/tracking/tracking_env_cfg.py
@@ -269,7 +269,7 @@ def make_tracking_env_cfg() -> ManagerBasedRlEnvCfg:
             params={"command_name": "motion", "std": 3.0},
         ),
         "survival": RewardTermCfg(func=mdp.survival, weight=3.0),
-        "action_rate_l2": RewardTermCfg(func=mdp.action_rate_l2, weight=-0.3),
+        "action_rate_l2": RewardTermCfg(func=mdp.action_rate_l2, weight=-0.5),
         "joint_limit": RewardTermCfg(
             func=mdp.joint_pos_limits,
             weight=-10.0,