feat(g1): add PICO SONIC WebXR teleoperation by TomCC7 · Pull Request #3721 · dimensionalOS/dimos

TomCC7 · 2026-08-27T05:23:47Z

Summary

route PICO WebXR body tracking through a specialized G1 SONIC teleoperation task
keep SONIC balancing active while the WebXR deadman switches between operator and planner references
make hardware and MuJoCo control activation wait for a complete low-level command
add the runnable unitree-g1-sonic-webxr-teleop blueprint and native MuJoCo visualization path

Dependencies

SONIC whole-body controller for the G1 #3557 — SONIC whole-body controller
feat: g1 quest teleop on shared dual-arm IK #3436 — G1 Quest/WebXR teleoperation stack
feat(teleop): add PICO WebXR body tracking #3695 — PICO WebXR body tracking

This draft integrates the three prerequisite branches for end-to-end simulation testing.

Validation

400 affected tests passed; 6 optional-dependency tests skipped as expected
Ruff passed on changed source
mypy passed on the changed source modules
blueprint registry generation and blueprint-kwargs validation passed
live PICO + Motion Tracker test passed with:
uv run dimos --simulation mujoco --viewer none run unitree-g1-sonic-webxr-teleop
- G1 stood and remained balanced
- WebXR engagement drove the SONIC reference
- motion was visible in the native MuJoCo viewer

Local runtime note

ONNX Runtime falls back to CPU when the CUDA execution provider is unavailable.

GEAR-SONIC planner+encoder+decoder ported from the Matrix parity reimplementation into a ControlTask claiming all 29 joints. All 27 locomotion modes RPC-reachable via task_invoke; twist via the standard duck-typed hook; upper-body targets are encoder hints only (never post-decoder overrides). Armature-derived SONIC PD gains carried over exactly, including the doubled ankle/waist entries. Blueprint unitree-g1-sonic-wbc with the standard sim/real dual backend. Verified standalone against raw MuJoCo: 40 s continuous choreography (walk 4 m, squat, boxing, dance) with zero falls and zero NaN at 0.23 ms/step policy latency on CPU onnxruntime.

Groot-style URDF static robot + joint-state overrides, entity key matching zenoh's stripped topic naming (world/g1_joints). Guarded off under DIMOS_TRANSPORT=zenoh: callable factories in rerun_config do not survive zenoh module-deploy serialization (msgpack -> dicts -> validation reject); applies to any blueprint, works under LCM.

SONIC's native protocol as a first-class task input: wire codec for the 1280-byte packed-header format (round-trip verified against NVIDIA's own builders), command/planner/pose topic routing polled non-blocking from compute(), StreamedMotionMerger ported faithfully (sliding window, catch-up, protocol v1/v2/v3, encode-mode selection), SMPL + wrist encoder fields for mode 2, wire-order 17-DoF upper-body targets with velocities, direct planner commands overriding twist mapping, g1_debug state PUB. E2E verified in sim against the live blueprint driven ONLY by NVIDIA's sender code: planner-mode walk, then protocol-v1 streamed-motion playback of the macarena reference clip - 110 planner + 110 pose messages, zero decode errors, zero tick errors, no falls (base z 0.79 at end).

play_motion_clip/stop_motion_clip/list_motion_clips on the task: reference clips (50 Hz CSVs, IsaacLab joint order) load into a StreamedMotion and run through the exact streamed-motion path the ZMQ pose topic uses, heading re-anchored on start. Verified in the sim blueprint over RPC: macarena (1375 frames) and dance_in_da_party (497) play end to end, no falls, clean return to planner idle after stop. data/sonic ships via LFS (encoder+decoder+774MB planner, observation config, 13 reference clips); blueprint model paths now default to LfsPath("sonic") with SONIC_MODEL_DIR/SONIC_PLANNER_PATH still overriding, so the GR00T-WholeBodyControl checkout is no longer required. Also restores the LFS pointer for reloc_and_nav_to.webp that the first SONIC commit accidentally hydrated (132 B -> 1 MB blob).

End-effector teleop through SONIC's native interface: set_vr_3point RPC and the ZMQ planner-topic vr fields feed root-relative wrist/head poses (9 pos + 12 quat, C++ GatherVR3Point buffered convention - sender applies offsets and root normalization) into encoder obs [901:922]. Teleop mode builds the C++-faithful observation: zeroed buffer, mode scalar, lowerbody joint history from the planner trajectory using the mujoco-order-in-isaaclab index set, single-frame anchor orientation. Stale VR data (>0.5 s) reverts to planner obs; locomotion keeps running underneath. Verified: unit test pins every field offset against policy_parameters.hpp and observation_config.yaml; sim E2E - hands-raised targets streamed at 15 Hz swing the elbows 1.28 rad while base z stays 0.77-0.80, and the arms return to idle after the stream stops.

NVIDIA's SONIC v1.1 (HF nvidia/GEAR-SONIC sonic_v1_1) is now the shipped checkpoint: heading-normalized target orientations + wrist-pose augmentation, the intended model for whole-body teleoperation. The pipeline selects the obs layout by the encoder's input width (1762 original / 1751 v1.1 - root_z dropped, anchor fields reordered, every downstream offset shifted) and switches the anchor-orientation left quat to robot heading only (C++ orientation_mode 1). The original release still runs via SONIC_MODEL_DIR; data/sonic and the LFS archive carry v1.1. Fixes a heading bug v1.1 exposed (latent under the original checkpoint): motion-source switches kept the previous source's heading delta, so post-clip planner trajectories stayed anchored to the clip's mocap heading and the policy turned instead of walking. Heading now re-anchors on every source switch (C++ reinitialize_heading_ semantics). PICO 5-sensor teleop path (full-body SMPL from headset + controllers + foot trackers, pose-topic protocol v3): accept the pico server's body_quat_w field name, consume vr_position/vr_orientation from the pose topic, and apply heading_increment (joystick yaw) into the heading delta. Verified on v1.1 in sim: walk 4.28 m; macarena tracks end to end; VR 3-point elbows 1.34 rad with base z 0.786-0.807; clip->VR->walk regression 4.19 m; pico-faithful SMPL stream (NVIDIA's pack_pose_message, exact field set) runs encoder mode 2 with stable stance and +0.9 rad heading_increment yaws the robot +0.72 rad. Obs layouts for both variants unit-pinned against the C++ registry and both observation_config.yaml files.

Drop the checkpoint-variant layout table: we ship exactly one policy (SONIC v1.1 in the LFS archive), so the obs layout is flat module constants again and the anchor math is unconditionally heading-normalized. A checkpoint with a different encoder width now fails at load with a clear error instead of being silently supported. Smoke on the stripped build: clip -> walk 4.21 m, base z 0.788.

…on to self-hosted Both from PR review: set_velocity_command was reachable only via the coordinator's undeclared reflective-dispatch fallback (warning, no signature validation); and the blueprint stringifies LfsPath model paths at import, which on the plain Ubuntu runner means a git-lfs failure or an 836MB pull during blueprint validation - same reason unitree-g1-nav-sim is self-hosted.

Conflict in test_all_blueprints.py: main reorganized SELF_HOSTED_BLUEPRINTS (openarm added, nav blueprints removed with the asset-manager migration); kept main's list plus unitree-g1-sonic-wbc, which still stringifies LfsPath model paths at import. Post-merge hardening found by the runtime smoke: pyzmq was never a declared dependency (the task's ZMQ wire rode a manually installed package) - added to project deps. And a missing pyzmq used to make _zmq_start retry the import and log a warning on every 50 Hz compute tick, which starved the control loop until the robot collapsed; the wire now disables itself permanently after one failed start, one warning. Smoke on merged main: arm + walk 3.56 m, base z 0.787.

Two mechanisms from the C++ gamepad manager that the port lacked, found by chasing a kneel-entry crash (instant walking-context -> kneel switch made the planner emit a violent drop): - set_mode now stages floor postures exactly like gamepad_manager.hpp: crawling is entered through kneel, elbow crawling through crawling, exits reverse the ladder, one rung per 2 s dwell, advanced from step(). - Forced modes apply the C++ applySpeedAndHeight table: kneel/squat/crawl send the planner height command (0.4 / 0.4 / 0.3) - without it the planner emits a floor-collapse descent instead of a supported kneel - and the crawl/boxing families their canonical 0.7 speed. Also applies command-topic delta_heading (incremental yaw, was decoded and only logged). Verified in the standalone harness at real-time planner pacing: kneel holds at z 0.41 and stands back up; single kneel likewise; crawl -> kneel -> stand recovery works; full 27-mode + 13-clip take with a live chained floor section, zero falls. Genuine policy limitation left: lying face down has no get-up (the C++ transition logic never chains out of lying either).

…feat/g1-sonic-wbc

Review feedback: nothing points NVIDIA's ZMQ tooling at dimos, and the teleop work built on this task talks to the pipeline directly (zenoh), so the socket layer was dead code. Removes zmq_wire.py, the task's socket/poll/publish plumbing, and the pyzmq dependency. The streamed motion merger and the pose-fields contract stay; that is what teleop consumes.

Review feedback: the version is a property of the sender, not of each chunk. Establish it from the first chunk, hold it until reset(), and keep the per-chunk field check purely as a mid-stream sender-change guard (the C++ endpoint errors on that too).

PICO WebXR omits its platform-reserved menu button, so controller packets contain six buttons. Treat the trailing menu slot as optional instead of rejecting otherwise valid joystick input.

codecov · 2026-09-10T20:58:25Z

❌ 27 Tests Failed:

Tests completed	Failed	Passed	Skipped
5280	27	5253	186

View the top 3 failed test(s) by shortest run time

::dimos.simulation.mujoco.test_model

Stack Traces | 0s run time

ImportError while importing test module '.../simulation/mujoco/test_model.py'.
Hint: make sure your test modules/packages have valid Python names.
Traceback:
.../simulation/mujoco/test_model.py:18: in <module>
    from dimos.simulation.mujoco.model import get_assets
.../simulation/mujoco/model.py:23: in <module>
    from mujoco_playground._src import mjx_env
E   ModuleNotFoundError: No module named 'mujoco_playground'

dimos.control.tasks.g1_sonic_wbc_task.test_sonic_onnx_runtime::test_prepare_rejects_cpu_only_runtime_before_loading_models