mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-17 15:01:54 +00:00
Martino Russi
@@ -0,0 +1,103 @@
|
||||
# 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.
|
||||
@@ -0,0 +1,97 @@
|
||||
# SONIC replay instability — root cause & prevention
|
||||
|
||||
This documents the multi-day debugging of "SONIC motion replay is unstable / jitters /
|
||||
lags / dies on the floor", so we don't chase the same ghosts again.
|
||||
|
||||
## TL;DR
|
||||
|
||||
There were **two independent problems**, and they masked each other:
|
||||
|
||||
1. **Wrong conda environment (the "lag"/jitter).** The debugging env `lerobot_sonic`
|
||||
had a **CUDA-13** stack that the machine's GPU driver cannot run, so ONNX Runtime
|
||||
silently fell back to CPU and oversubscribed threads. The known-good env
|
||||
`lerobot312` has a **CUDA-12** stack matching the driver, so the encoder/decoder/
|
||||
planner run on the GPU (~12–20 ms planner inference) and the control loop holds
|
||||
~48–50 Hz.
|
||||
2. **SMPL root-motion feeding (the NaN/`unstable` crash).** Passing the per-frame SMPL
|
||||
root quaternion into the mode-2 anchor produced a root-acceleration spike
|
||||
(`Nan, Inf or huge value in QACC at DOF 0`) mid-episode. Disabling it gives clean
|
||||
tracking.
|
||||
|
||||
Neither is an algorithmic bug in the ported SONIC pipeline. A lot of earlier "fixes"
|
||||
(ORT thread caps, `MAX_DELTA_PER_STEP` clamp, planner-disable toggle, resampling)
|
||||
were chasing symptom #1 in the wrong environment and were reverted.
|
||||
|
||||
## Environment: what "good" looks like
|
||||
|
||||
Run the replay in `lerobot312` (CUDA-12), pointing at the current sonic checkout:
|
||||
|
||||
```bash
|
||||
conda activate lerobot312
|
||||
PYTHONPATH=/home/yope/Documents/sonic/lerobot/src \
|
||||
lerobot-replay \
|
||||
--robot.type=unitree_g1 --robot.controller=SonicWholeBodyController \
|
||||
--dataset.repo_id=lerobot/SMPL_samples --dataset.episode=12
|
||||
```
|
||||
|
||||
Known-good versions (`lerobot312`):
|
||||
|
||||
| package | good (`lerobot312`) | broken (`lerobot_sonic`) |
|
||||
|-----------------|---------------------|--------------------------|
|
||||
| GPU driver | CUDA 12.8 (`12080`) | same (unchanged) |
|
||||
| torch | `2.10.0+cu128` | `2.11.0+cu130` |
|
||||
| onnxruntime | `onnxruntime-gpu 1.26.0` | CPU `1.27.0` / cu13 mismatch |
|
||||
| cudnn | cu12 (bundled) | `nvidia-cudnn-cu13 9.19` |
|
||||
| mujoco | `3.8.1` | `3.10.0` |
|
||||
|
||||
### How to verify the GPU path is actually live (do this FIRST)
|
||||
|
||||
```bash
|
||||
python -c "import torch; print('cuda', torch.cuda.is_available())" # must be True
|
||||
python -c "import onnxruntime as ort; print(ort.get_available_providers())" # must list CUDAExecutionProvider
|
||||
```
|
||||
|
||||
If `torch.cuda.is_available()` is `False` or `CUDAExecutionProvider` is missing, STOP —
|
||||
you are in the wrong/broken env. Do not "optimize" anything else until this passes.
|
||||
|
||||
### Why CUDA 13 was fatal here
|
||||
|
||||
The GPU driver supports up to **CUDA 12.8**. A CUDA-13 build of torch/onnxruntime
|
||||
cannot initialize on it:
|
||||
- `torch.cuda.is_available()` returns `False` (silent CPU fallback).
|
||||
- `onnxruntime-gpu` (a CUDA-12 build) can't find a matching cuDNN because only the
|
||||
CUDA-13 cuDNN is installed → `CUDNN failure 1001: CUDNN_STATUS_NOT_INITIALIZED`.
|
||||
|
||||
Installing `onnxruntime` and `onnxruntime-gpu` **together** also breaks: they share the
|
||||
`onnxruntime` namespace and whichever installs last clobbers the other's shared libs.
|
||||
Keep only `onnxruntime-gpu` in a GPU env.
|
||||
|
||||
## Root motion: the NaN/unstable crash
|
||||
|
||||
Symptom:
|
||||
```
|
||||
WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 4.196.
|
||||
```
|
||||
`DOF 0` is the floating base/root. Feeding the per-frame SMPL root quaternion
|
||||
(`root.*` action keys) into `controller.smpl_root_quat` injected a discontinuity in the
|
||||
reference root trajectory (frame-to-frame jump and/or 30 Hz→50 Hz timing mismatch) that
|
||||
the tracker converted into an exploding base acceleration.
|
||||
|
||||
Current mitigation (in `sonic_whole_body.py`, `run_step`): the per-frame root quaternion
|
||||
is ignored (`self.controller.smpl_root_quat = None`) so the anchor stays self-driven.
|
||||
Result: clean tracking, no NaN.
|
||||
|
||||
Proper fix (follow-up, not yet done): smooth/slerp-filter the reference root trajectory
|
||||
(or resample to the control rate) before feeding it to the anchor, then re-enable.
|
||||
|
||||
## Prevention checklist
|
||||
|
||||
- **Always confirm the env before debugging behavior.** Run the two verification
|
||||
commands above. Most of the "instability" was environment, not code.
|
||||
- **Pin the GPU stack** to match the driver (CUDA 12.8): `torch ==2.10.0+cu128`,
|
||||
`onnxruntime-gpu ==1.26.0`, `mujoco ==3.8.1`. Do not let `lerobot_sonic` drift to a
|
||||
CUDA-13 stack.
|
||||
- **Never install `onnxruntime` and `onnxruntime-gpu` side by side.**
|
||||
- **Don't add band-aid clamps/thread-caps/resampling to hide a CPU-fallback**; fix the
|
||||
env instead. Those changes were reverted.
|
||||
- **Root trajectory must be continuous / rate-matched** before it drives the anchor.
|
||||
Reference in New Issue
Block a user