lerobot/docs/SONIC_FIDELITY_AUDIT.md

SONIC fidelity audit — Python/ONNX port vs C++ deploy reference

Compares the lerobot Python/ONNX SONIC port against the original C++ deploy stack (gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp and headers).

Verdict: the port is algorithmically faithful in the core control math (the parts that determine stability and pose tracking). The gaps are concentrated in (a) update cadence, (b) the safety layer, and (c) two modes/paths present in C++ but never wired up. No silent math bug was found; divergences are deliberate or missing-feature.

Genuinely faithful (verified, no action needed)

• Action production — both do q_target = DEFAULT_ANGLES + policy_out[isaaclab→mujoco] * ACTION_SCALE (residual-to-default, not residual-to-previous). C++ g1_deploy_onnx_ref.cpp:3119-3127, Python sonic_pipeline.py:708-724.
• Joint-order remap — ISAACLAB_TO_MUJOCO / MUJOCO_TO_ISAACLAB identical arrays.
• Gains — same Kp = armature·ω², Kd = 2ζ·armature·ω, ω = 10·2π, ζ = 2, and the same 2× set {4,5,10,11,13,14} (ankles + waist roll/pitch).
• History normalization — robot q subtracts defaults, velocities raw, gravity = quat_rotate(conj(base), [0,0,-1]), oldest→newest ordering.
• 6D anchor rotation — verified element-by-element: C++ takes the first two columns of the rotation matrix flattened row-wise (.cpp:677-683); Python quat_to_6d (sonic_pipeline.py:227-240) produces the identical 6 values. Only the Python docstring wording ("rows") is misleading — the math is correct.
• Planner — replan intervals (RUN 0.1 / CRAWL 0.2 / boxing 1.0 / default 1.0 s), 8-frame slerp crossfade blend, 30→50 Hz linear+slerp resample, MOTION_LOOK_AHEAD = 2, 4-frame 30 Hz context. All match.
• SLERP / heading / FK conventions — wxyz, shortest-path slerp with 0.9995 linear fallback, calc_heading yaw extraction.

Divergences that reduce fidelity (ranked)

1. Encoder cadence: 10 Hz vs 50 Hz (biggest)

C++ runs the encoder every control tick — GatherTokenState → Encode() is unconditional inside the 50 Hz Control() loop (.cpp:1644-1684, no N-step gate). The port recomputes the token only every 5 ticks (ENCODER_UPDATE_EVERY = 5, sonic_pipeline.py:150), so the latent is up to 80 ms stale and the decoder consumes a held token for 4 of every 5 ticks. Likely a perf shortcut. For full faithfulness set ENCODER_UPDATE_EVERY = 1 (cheap on GPU).

2. SMPL root anchor disabled by default

C++ always feeds the reference root orientation into the anchor/heading. The port sets smpl_root_quat = None unless enable_smpl_root=True (sonic_whole_body.py:366), because the raw 30 Hz root caused QACC spikes. Faithful fix: slerp-resample the root 30→50 Hz like the joints, then re-enable by default. Until then, mode-2 heading steering isn't faithful. See SONIC_REPLAY_DEBUGGING.md.

3. VR 3-point teleop (encode_mode = 1) — now wired (was inert)

Originally the encoder layout for mode 1 existed but nothing set encode_mode=1 or filled vr_3point_local_target / vr_3point_local_orn_target. Now implemented end-to-end:

• Producer pico_publisher.py computes the 3 keypoints via smpl_fk.compute_3point (ported from gear_sonic _process_3pt_pose) and adds vr3_pos (9) / vr3_orn (12) to the rt/smpl message.
• SmplStream parses them (has_vr3); PicoHeadset(mode="vr3") emits vr3_pos.* / vr3_orn.* action keys.
• SonicWholeBodyController extracts them, switches to encode_mode=1, fills the controller targets, and drives locomotion from the joystick/keyboard planner (use_joystick=True); PlannerController.build_encoder_obs gained a mode-1 branch (lower body per-frame step 5 + VR targets + anchor).

Still not ported: vr_3point_compliance, and the operator calibration (ThreePointPose.apply_calibration) / physical wrist offsets — the raw tracked joint poses are used, so hand-tracking may need calibration tuning.

4. Safety layer largely absent

• Joint-velocity kill switch at >35 rad/s (.cpp:2829-2832) — missing.
• E-stop damping: C++ e-stop commands kp=0, kd=8 (active damping, .cpp:2708-2714). The port's Space/e-stop just sets playing=False + LM.IDLE and stops the cursor — it does not switch to a damped hold. Less safe.
• Motor-temperature monitor (90 °C / 85 °C hysteresis) — missing.
• Stale-/late-state watchdogs (500 ms fail, 50 ms warn, 200 ms token timeout) — not in the SONIC layer (partly covered by UnitreeG1, not equivalently).
• Per-tick delta clamp — C++ has none, so the reverted MAX_DELTA_PER_STEP was correctly removed; that part is faithful.

5. Idle readaptation blend missing

At planner IDLE at a motion end, C++ runs a double-threshold blend (0.98/0.02 toward robot-current or original target; thresholds 0.10/0.05/0.045 rad; .cpp:3303-3361). The port just holds. Minor; only matters at motion-end idle.

6. Input/streaming paths not ported

• ZMQ Protocol V1 joint streaming (encode_mode=0 from a live joint stream via StreamedMotionMerger) — not implemented; the port's streaming path is SMPL-only.
• External token injection (tokens over ZMQ/ROS2 bypassing the encoder) — not supported; the port always encodes locally. Fine for standalone.
• Gamepad — C++ has a full gamepad map (EMA smooth 0.3, deadzone 0.05); the port has joystick byte-parsing + keyboard, no gamepad manager. Functionally close.

7. Minor input-feel differences

Delta-heading is continuous (±0.02 rad/tick) in the port vs discrete steps (±π/6, ±π/12) in C++; speed/height increments differ slightly. Behavioral feel only, not correctness.

Recommendation (priority order)

1. ENCODER_UPDATE_EVERY = 1 (or a param defaulting to 1) — closes the biggest gap for near-zero cost on GPU.
2. Rate-match the SMPL root 30→50 Hz (slerp) and re-enable enable_smpl_root by default.
3. Add the safety envelope: joint-velocity kill (35 rad/s) and a proper damped e-stop (kp=0, kd≈8). Real hardware-safety consequences.
4. Wire encode_mode=1 from the pico headset (3-point targets), or document as out of scope.
5. Fix the quat_to_6d docstring wording ("rows" → "first two columns flattened row-wise").

Items 4–7 are feature-completeness; 1–3 are what to do for faithful behavior on the robot.