From bedd56eed92ce76c0648c8901c3c55b7682da931 Mon Sep 17 00:00:00 2001 From: Martino Russi Date: Thu, 16 Jul 2026 14:40:32 +0200 Subject: [PATCH] Remove g1_sonic_slider, examples/onnx, and SONIC debugging docs --- docs/SONIC_FIDELITY_AUDIT.md | 103 ---- docs/SONIC_REPLAY_DEBUGGING.md | 97 ---- examples/onnx/PR_DESCRIPTION.md | 79 --- src/lerobot/robots/unitree_g1/README.md | 27 +- .../teleoperators/g1_sonic_slider/__init__.py | 20 - .../g1_sonic_slider/config_g1_sonic_slider.py | 35 -- .../g1_sonic_slider/g1_sonic_slider.py | 458 ------------------ .../g1_sonic_slider/joint_limits.py | 64 --- 8 files changed, 16 insertions(+), 867 deletions(-) delete mode 100644 docs/SONIC_FIDELITY_AUDIT.md delete mode 100644 docs/SONIC_REPLAY_DEBUGGING.md delete mode 100644 examples/onnx/PR_DESCRIPTION.md delete mode 100644 src/lerobot/teleoperators/g1_sonic_slider/__init__.py delete mode 100644 src/lerobot/teleoperators/g1_sonic_slider/config_g1_sonic_slider.py delete mode 100644 src/lerobot/teleoperators/g1_sonic_slider/g1_sonic_slider.py delete mode 100644 src/lerobot/teleoperators/g1_sonic_slider/joint_limits.py diff --git a/docs/SONIC_FIDELITY_AUDIT.md b/docs/SONIC_FIDELITY_AUDIT.md deleted file mode 100644 index 5548e5062..000000000 --- a/docs/SONIC_FIDELITY_AUDIT.md +++ /dev/null @@ -1,103 +0,0 @@ -# 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. diff --git a/docs/SONIC_REPLAY_DEBUGGING.md b/docs/SONIC_REPLAY_DEBUGGING.md deleted file mode 100644 index af6735d7f..000000000 --- a/docs/SONIC_REPLAY_DEBUGGING.md +++ /dev/null @@ -1,97 +0,0 @@ -# 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. diff --git a/examples/onnx/PR_DESCRIPTION.md b/examples/onnx/PR_DESCRIPTION.md deleted file mode 100644 index ba4f40e68..000000000 --- a/examples/onnx/PR_DESCRIPTION.md +++ /dev/null @@ -1,79 +0,0 @@ -# feat: ONNX inference support (ACT) - -## Summary - -This PR introduces a first, end-to-end path for **ONNX-based policy inference** in LeRobot, currently scoped to the **ACT** policy. The goal is to standardize how we export and run policies through ONNX Runtime so that the same workflow can later cover other policies (including the Unitree G1 whole-body / locomotion policies) and so policies can run on **edge devices** without a full PyTorch stack. - -> ⚠️ **Scope:** today this only works for **ACT**. ACT is the natural starting point because inference is a single deterministic forward pass (ResNet backbone + transformer enc/dec + action head) with a zeros VAE latent — no denoising loop, no KV cache. Other architectures (e.g. PI0.5) need more work before they can be exported the same way. - -## Motivation - -- **Standardize ONNX inference** across LeRobot policies behind one export + run convention, instead of one-off conversion scripts. -- **Run on edge devices**: ONNX Runtime has a much smaller footprint than PyTorch and ships CPU / CUDA / TensorRT / mobile execution providers, which is what we want for deploying policies (incl. Unitree G1 policies) on-robot. -- Keep normalization and control logic in Python (the LeRobot processor pipeline + action queue), and export **only the neural network** as a portable graph. - -## What's included - -All new files live under `examples/onnx/` (no changes to `src/lerobot/...`): - -- **`export_act.py`** — exports `policy.model` to ONNX as a pure function `(state, images) -> action_chunk`, then runs a numerical parity check (PyTorch vs ONNX Runtime). -- **`eval_act_onnx.py`** — evaluates ACT in sim with either the PyTorch or the ONNX backend. It swaps **only** `policy.model` with an ONNX Runtime session (wrapped as an `nn.Module`), so processors, action queue and the gym env are identical and any delta is attributable to the backend alone. -- **`convert_legacy_checkpoint.py`** — helper for older hub checkpoints that bake normalization into weights and lack `policy_preprocessor.json` / `policy_postprocessor.json`. - -## Design notes - -- Only the network is exported. At inference, ACT's `predict_action_chunk` is effectively `self.model(batch)[0]` with a zeros latent, so the graph is deterministic in `(state, images)`. -- **Normalization stays outside ONNX**, in the LeRobot processor pipeline. The ONNX graph consumes already-normalized inputs and emits normalized actions. -- torch 2.9+ defaults to the dynamo exporter (requires `onnxscript`); the exporter uses the legacy TorchScript path (`dynamo=False`) since ACT's graph is fixed-shape. - -## Results - -**Numerical parity** (PyTorch vs ONNX Runtime): - -``` -max_abs_diff = 1.073e-06 mean_abs_diff = 1.790e-07 -> PASS -``` - -**In-sim eval**, `AlohaTransferCube-v0`, identical seed: - -| backend | n_episodes | pc_success | -|---------|-----------:|-----------:| -| torch | 10 | 70.0% | -| onnx | 10 | 70.0% | - -Identical success rate; sub-1e-6 per-step parity. (Run on CPU here; both backends behave the same on CUDA.) - -## How to run - -```bash -export PYTHONPATH=src - -# export once (also runs the parity check) -python examples/onnx/export_act.py \ - --policy-path=lerobot/act_aloha_sim_transfer_cube_human \ - --output=outputs/onnx/act_transfer_cube.onnx - -# compare backends in sim -python examples/onnx/eval_act_onnx.py \ - --policy-path=lerobot/act_aloha_sim_transfer_cube_human \ - --task=AlohaTransferCube-v0 \ - --backend=torch --n-episodes=50 --batch-size=10 --device=cuda - -python examples/onnx/eval_act_onnx.py \ - --policy-path=lerobot/act_aloha_sim_transfer_cube_human \ - --task=AlohaTransferCube-v0 \ - --onnx=outputs/onnx/act_transfer_cube.onnx \ - --backend=onnx --n-episodes=50 --batch-size=10 --device=cuda -``` - -## Follow-ups (out of scope for this PR) - -- Generalize the export convention beyond ACT (PI0.5 denoising loop + KV cache, diffusion policies, etc.). -- Cover the **Unitree G1** policies so they can be deployed via ONNX Runtime on-robot. -- Provide an edge-device runner / packaging story (CPU / TensorRT / mobile execution providers) and a latency benchmark. - -## Test plan - -- [x] ONNX export succeeds for ACT and passes the parity check (`max_abs_diff < 1e-3`). -- [x] In-sim eval matches the PyTorch backend at the same seed. -- [ ] Full 50-episode eval on CUDA (torch vs onnx) reproduces the baseline success rate. diff --git a/src/lerobot/robots/unitree_g1/README.md b/src/lerobot/robots/unitree_g1/README.md index a5fb52345..f90e3846b 100644 --- a/src/lerobot/robots/unitree_g1/README.md +++ b/src/lerobot/robots/unitree_g1/README.md @@ -10,11 +10,11 @@ deploy stack (no `gear_sonic`/torch dependency). Selected with `--robot.controller=`: -| Controller | Purpose | -|---|---| -| `SonicWholeBodyController` | SONIC whole-body: locomotion, keyboard, and SMPL imitation (mode 2) | -| `GrootLocomotionController` | GR00T locomotion policy | -| `HolosomaLocomotionController` | Holosoma locomotion policy | +| Controller | Purpose | +| ------------------------------ | ------------------------------------------------------------------- | +| `SonicWholeBodyController` | SONIC whole-body: locomotion, keyboard, and SMPL imitation (mode 2) | +| `GrootLocomotionController` | GR00T locomotion policy | +| `HolosomaLocomotionController` | Holosoma locomotion policy | ## Requirements @@ -32,6 +32,7 @@ Selected with `--robot.controller=`: ## Running **Replay an SMPL dataset (motion imitation):** + ```bash lerobot-replay \ --robot.type=unitree_g1 --robot.controller=SonicWholeBodyController \ @@ -39,20 +40,24 @@ lerobot-replay \ ``` **Keyboard teleop** (drives locomotion via the native keyboard teleoperator): + ```bash lerobot-teleoperate \ --robot.type=unitree_g1 --robot.controller=SonicWholeBodyController \ --teleop.type=keyboard ``` + Controls: `WASD` move · `Q`/`E` turn · `1`–`8` mode · `9`/`0` speed · `-`/`=` height · `R` replan · `Space` emergency-stop. **PICO headset teleop** (live SMPL whole-body): + ```bash lerobot-teleoperate \ --robot.type=unitree_g1 --robot.controller=SonicWholeBodyController \ --teleop.type=pico_headset ``` + This requires the XRoboToolkit stack — see below. ## PICO headset / XRoboToolkit install @@ -84,12 +89,12 @@ Summary: ### Platform support -| Platform | Live headset teleop | Notes | -|---|---|---| -| Linux x86_64 | ✅ | Guided `install_pico.sh` (SONIC repo) | -| Linux aarch64 (Jetson Orin) | ✅ | `setup_orin.sh` builds the native lib | -| Windows x64 | ✅ (manual) | `setup_windows.bat`; no one-shot env script | -| macOS | ❌ | No PC Service / SDK build for Darwin | +| Platform | Live headset teleop | Notes | +| --------------------------- | ------------------- | ------------------------------------------- | +| Linux x86_64 | ✅ | Guided `install_pico.sh` (SONIC repo) | +| Linux aarch64 (Jetson Orin) | ✅ | `setup_orin.sh` builds the native lib | +| Windows x64 | ✅ (manual) | `setup_windows.bat`; no one-shot env script | +| macOS | ❌ | No PC Service / SDK build for Darwin | ### No hardware required (any platform, incl. macOS/Windows) diff --git a/src/lerobot/teleoperators/g1_sonic_slider/__init__.py b/src/lerobot/teleoperators/g1_sonic_slider/__init__.py deleted file mode 100644 index 1367c57b4..000000000 --- a/src/lerobot/teleoperators/g1_sonic_slider/__init__.py +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env python - -# Copyright 2026 The HuggingFace Inc. team. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from .config_g1_sonic_slider import G1SonicSliderTeleopConfig -from .g1_sonic_slider import G1SonicSliderTeleop - -__all__ = ["G1SonicSliderTeleop", "G1SonicSliderTeleopConfig"] diff --git a/src/lerobot/teleoperators/g1_sonic_slider/config_g1_sonic_slider.py b/src/lerobot/teleoperators/g1_sonic_slider/config_g1_sonic_slider.py deleted file mode 100644 index f89e7a391..000000000 --- a/src/lerobot/teleoperators/g1_sonic_slider/config_g1_sonic_slider.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python - -# Copyright 2026 The HuggingFace Inc. team. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from dataclasses import dataclass - -from ..config import TeleoperatorConfig - - -@TeleoperatorConfig.register_subclass("g1_sonic_slider") -@dataclass -class G1SonicSliderTeleopConfig(TeleoperatorConfig): - """Pygame sliders for 29-DOF G1 poses (SONIC encoder mode 0 reference).""" - - window_width: int = 780 - window_height: int = 720 - slider_width: int = 200 - row_height: int = 22 - scroll_step: int = 40 - foot_panel_width: int = 248 - use_leg_ik: bool = True - foot_xyz_margin: tuple[float, float, float] = (0.22, 0.18, 0.18) - """Per-axis slider half-range (m) around standing foot FK position in pelvis frame.""" diff --git a/src/lerobot/teleoperators/g1_sonic_slider/g1_sonic_slider.py b/src/lerobot/teleoperators/g1_sonic_slider/g1_sonic_slider.py deleted file mode 100644 index de8c99a01..000000000 --- a/src/lerobot/teleoperators/g1_sonic_slider/g1_sonic_slider.py +++ /dev/null @@ -1,458 +0,0 @@ -#!/usr/bin/env python - -# Copyright 2026 The HuggingFace Inc. team. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Pygame SONIC test UI: foot xyz (leg IK) + waist/arm joint sliders.""" - -from __future__ import annotations - -import logging -import multiprocessing as mp -import queue -from functools import cached_property - -import numpy as np - -from lerobot.robots.unitree_g1.controllers.sonic_pipeline import DEFAULT_ANGLES -from lerobot.utils.import_utils import require_package - -from ..teleoperator import Teleoperator -from .config_g1_sonic_slider import G1SonicSliderTeleopConfig -from .joint_limits import JOINT_HI, JOINT_LO, JOINT_NAMES - -logger = logging.getLogger(__name__) - -NUM_JOINTS = 29 -LEG_JOINT_COUNT = 12 -UPPER_BODY_INDICES = list(range(LEG_JOINT_COUNT, NUM_JOINTS)) -NUM_FOOT_SLIDERS = 6 -FOOT_LABELS = ("L foot X", "L foot Y", "L foot Z", "R foot X", "R foot Y", "R foot Z") - -# Pelvis-frame standing foot centers (m) if Pinocchio FK is unavailable at startup. -_FALLBACK_LEFT_FOOT = np.array([0.02, 0.12, -0.76], dtype=np.float32) -_FALLBACK_RIGHT_FOOT = np.array([0.02, -0.12, -0.76], dtype=np.float32) - -HEADER_H = 56 -LABEL_W = 148 -MARGIN = 10 -KNOB_W = 10 - - -def _leg_ik_process(target_q: mp.Queue, result_q: mp.Queue, stop_evt) -> None: - """Child process: build the leg IK once, then solve for the latest foot target. - - The IPOPT/CasADi solve holds the GIL, so it must run in a separate *process* - (not a thread) to keep the teleop UI loop responsive. - """ - import numpy as _np - - from lerobot.robots.unitree_g1.controllers.sonic_pipeline import DEFAULT_ANGLES as _DEFAULTS - from lerobot.robots.unitree_g1.g1_kinematics import G1_29_LegIK - - try: - ik = G1_29_LegIK() - q_legs = _DEFAULTS[:LEG_JOINT_COUNT].astype(_np.float64) - ik.cache_default_orientation(q_legs) - left_pos, right_pos = ik.foot_positions(q_legs) - except Exception as e: # noqa: BLE001 - result_q.put(("error", str(e))) - return - - current = q_legs.copy() - result_q.put(("ready", _np.concatenate([left_pos, right_pos]).astype(_np.float64))) - - while not stop_evt.is_set(): - try: - target = target_q.get(timeout=0.1) - except queue.Empty: - continue - if target is None: - break - # Drain to the most recent target so we never solve stale slider positions. - while True: - try: - newer = target_q.get_nowait() - except queue.Empty: - break - if newer is None: - target = None - break - target = newer - if target is None: - break - target = _np.asarray(target, dtype=_np.float64) - # Fast damped-least-squares IK: sub-ms per step, warm-started from the last - # solution so the legs track the sliders in real time. - leg_q = ik.solve_ik_dls(target[:3], target[3:], current_leg_q_g1=current) - current = _np.asarray(leg_q, dtype=_np.float64) - result_q.put(("q", current.copy())) - - -class G1SonicSliderTeleop(Teleoperator): - """Foot xyz + waist/arm sliders feeding SONIC encoder mode 0.""" - - config_class = G1SonicSliderTeleopConfig - name = "g1_sonic_slider" - - def __init__(self, config: G1SonicSliderTeleopConfig): - super().__init__(config) - self.config = config - self._values = DEFAULT_ANGLES.astype(np.float32).copy() - self._foot_xyz = np.zeros(6, dtype=np.float32) - self._foot_lo = np.full(6, -0.5, dtype=np.float32) - self._foot_hi = np.full(6, 0.5, dtype=np.float32) - self._scroll_y = 0 - self._drag_joint: int | None = None - self._drag_foot: int | None = None - self._connected = False - self._pygame = None - self._screen = None - self._font = None - self._small_font = None - self._clock = None - # Leg IK runs in a separate process: the IPOPT/CasADi solve holds the GIL, - # so a thread would still stall the UI loop. We publish the latest foot - # target and read back the newest solution over queues, never blocking. - self._ik_proc: mp.process.BaseProcess | None = None - self._ik_target_q: mp.Queue | None = None - self._ik_result_q: mp.Queue | None = None - self._ik_stop_evt = None - self._ik_ready = False - self._standing_foot_xyz: np.ndarray | None = None - self._leg_ik_ok = False - self._leg_ik_error: str | None = None - self._foot_divider_x = config.foot_panel_width - - @cached_property - def action_features(self) -> dict[str, type]: - return {f"{name}.q": float for name in JOINT_NAMES} - - @cached_property - def feedback_features(self) -> dict[str, type]: - return {} - - @property - def is_connected(self) -> bool: - return self._connected - - @property - def is_calibrated(self) -> bool: - return True - - def _set_foot_limits_from_positions(self, left_pos: np.ndarray, right_pos: np.ndarray) -> None: - """Slider range = FK foot position at standing pose ± foot_xyz_margin (meters, pelvis frame).""" - margin = np.array(self.config.foot_xyz_margin, dtype=np.float32) - for i, pos in enumerate((left_pos, right_pos)): - base = i * 3 - self._foot_xyz[base : base + 3] = pos.astype(np.float32) - self._foot_lo[base : base + 3] = pos.astype(np.float32) - margin - self._foot_hi[base : base + 3] = pos.astype(np.float32) + margin - - def _set_fallback_foot_limits(self) -> None: - self._set_foot_limits_from_positions(_FALLBACK_LEFT_FOOT, _FALLBACK_RIGHT_FOOT) - - def _init_leg_ik(self) -> None: - if not self.config.use_leg_ik: - return - # Fallback limits until the solver process reports standing FK foot positions. - self._set_fallback_foot_limits() - try: - # Metadata name for the Pinocchio distribution is "pin" (conda-forge/PyPI), - # not "pinocchio", which is only the importable module name. - require_package("pin", extra="unitree_g1", import_name="pinocchio") - require_package("casadi", extra="unitree_g1", import_name="casadi") - except Exception as e: - self._leg_ik_ok = False - self._leg_ik_error = str(e) - logger.warning("Leg IK unavailable (%s); foot sliders shown but legs use joint values", e) - return - - # Use "spawn": the parent already holds CUDA/pygame/threads and forking that - # state into the solver is unsafe. - ctx = mp.get_context("spawn") - self._ik_target_q = ctx.Queue(maxsize=2) - self._ik_result_q = ctx.Queue() - self._ik_stop_evt = ctx.Event() - self._ik_proc = ctx.Process( - target=_leg_ik_process, - args=(self._ik_target_q, self._ik_result_q, self._ik_stop_evt), - name="g1-leg-ik", - daemon=True, - ) - self._ik_proc.start() - self._leg_ik_ok = True - self._leg_ik_error = None - logger.info("Leg IK solver process starting (foot limits update once standing FK is ready)...") - - def _publish_ik_target(self) -> None: - if self._ik_target_q is None: - return - # Keep only the newest target; drop if the child is momentarily behind. - try: - self._ik_target_q.put_nowait(self._foot_xyz.copy()) - except queue.Full: - pass - - def _pump_ik_results(self) -> None: - if self._ik_result_q is None: - return - while True: - try: - kind, payload = self._ik_result_q.get_nowait() - except queue.Empty: - break - if kind == "q": - self._values[:LEG_JOINT_COUNT] = np.asarray(payload, dtype=np.float32) - elif kind == "ready": - payload = np.asarray(payload, dtype=np.float32) - self._standing_foot_xyz = payload.copy() - self._set_foot_limits_from_positions(payload[:3], payload[3:]) - self._ik_ready = True - logger.info( - "Leg IK ready — foot slider limits from standing FK ± %s m (pelvis frame)", - self.config.foot_xyz_margin, - ) - elif kind == "error": - self._leg_ik_ok = False - self._leg_ik_error = str(payload) - logger.warning( - "Leg IK unavailable (%s); foot sliders shown but legs use joint values", payload - ) - - def _stop_ik_process(self) -> None: - if self._ik_proc is None: - return - try: - if self._ik_stop_evt is not None: - self._ik_stop_evt.set() - if self._ik_target_q is not None: - try: - self._ik_target_q.put_nowait(None) - except queue.Full: - pass - self._ik_proc.join(timeout=2.0) - if self._ik_proc.is_alive(): - self._ik_proc.terminate() - finally: - self._ik_proc = None - - def connect(self, calibrate: bool = True) -> None: - require_package("pygame", extra="pygame-dep", import_name="pygame") - import pygame - - self._foot_divider_x = self.config.foot_panel_width - self._pygame = pygame - pygame.init() - pygame.display.set_caption("G1 SONIC — foot IK + upper-body sliders") - self._screen = pygame.display.set_mode((self.config.window_width, self.config.window_height)) - self._font = pygame.font.SysFont("dejavusans", 15) - self._small_font = pygame.font.SysFont("dejavusans", 12) - self._clock = pygame.time.Clock() - self._init_leg_ik() - self._connected = True - logger.info("G1 sonic slider UI ready (R=reset, wheel=scroll, Esc=quit)") - - def configure(self) -> None: - pass - - def calibrate(self) -> None: - pass - - def _reset_pose(self) -> None: - self._values[:] = DEFAULT_ANGLES - if self._leg_ik_ok and self._standing_foot_xyz is not None: - self._foot_xyz[:] = self._standing_foot_xyz - self._publish_ik_target() - - def _foot_row_rect(self, foot_idx: int) -> tuple[int, int, int, int]: - y = HEADER_H + foot_idx * self.config.row_height - track_x = MARGIN + 88 - track_w = self.config.foot_panel_width - track_x - MARGIN - return track_x, y, track_w, self.config.row_height - - def _joint_row_rect(self, ui_idx: int) -> tuple[int, int, int, int]: - joint_idx = UPPER_BODY_INDICES[ui_idx] if self._leg_ik_ok else ui_idx - row = ui_idx - y = HEADER_H + row * self.config.row_height - self._scroll_y - track_x = self._foot_divider_x + MARGIN + LABEL_W - track_w = self.config.slider_width - return track_x, y, track_w, self.config.row_height, joint_idx - - def _value_from_track(self, lo: float, hi: float, mouse_x: int, track_x: int, track_w: int) -> float: - t = (mouse_x - track_x) / max(track_w, 1) - t = float(np.clip(t, 0.0, 1.0)) - return lo + t * (hi - lo) - - def _knob_x(self, val: float, lo: float, hi: float, track_x: int, track_w: int) -> int: - span = hi - lo if hi > lo else 1.0 - t = (val - lo) / span - return int(track_x + t * track_w) - - def _handle_events(self) -> bool: - pygame = self._pygame - num_joint_rows = len(UPPER_BODY_INDICES) if self._leg_ik_ok else NUM_JOINTS - max_scroll = max(0, num_joint_rows * self.config.row_height - self.config.window_height + HEADER_H) - - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return False - if event.type == pygame.KEYDOWN: - if event.key == pygame.K_ESCAPE: - return False - if event.key == pygame.K_r: - self._reset_pose() - if event.type == pygame.MOUSEWHEEL: - self._scroll_y = int(np.clip(self._scroll_y - event.y * self.config.scroll_step, 0, max_scroll)) - if event.type == pygame.MOUSEBUTTONDOWN and event.button == 1: - mx, my = event.pos - if self.config.use_leg_ik and mx < self._foot_divider_x: - for fi in range(NUM_FOOT_SLIDERS): - rx, ry, rw, rh = self._foot_row_rect(fi) - if ry + 4 <= my <= ry + rh - 4 and rx <= mx <= rx + rw: - self._drag_foot = fi - self._foot_xyz[fi] = self._value_from_track( - float(self._foot_lo[fi]), - float(self._foot_hi[fi]), - mx, - rx, - rw, - ) - break - else: - for ui in range(num_joint_rows): - rx, ry, rw, rh, ji = self._joint_row_rect(ui) - if ry + rh < HEADER_H or ry > self.config.window_height: - continue - if ry + 4 <= my <= ry + rh - 4 and rx <= mx <= rx + rw: - self._drag_joint = ji - self._values[ji] = self._value_from_track( - float(JOINT_LO[ji]), float(JOINT_HI[ji]), mx, rx, rw - ) - break - if event.type == pygame.MOUSEBUTTONUP and event.button == 1: - self._drag_foot = None - self._drag_joint = None - if event.type == pygame.MOUSEMOTION: - mx, my = event.pos - if self._drag_foot is not None: - rx, _, rw, _ = self._foot_row_rect(self._drag_foot) - self._foot_xyz[self._drag_foot] = self._value_from_track( - float(self._foot_lo[self._drag_foot]), - float(self._foot_hi[self._drag_foot]), - mx, - rx, - rw, - ) - elif self._drag_joint is not None: - for ui in range(num_joint_rows): - rx, ry, rw, rh, ji = self._joint_row_rect(ui) - if ji == self._drag_joint: - self._values[ji] = self._value_from_track( - float(JOINT_LO[ji]), float(JOINT_HI[ji]), mx, rx, rw - ) - break - return True - - def _draw_foot_panel(self) -> None: - pygame = self._pygame - screen = self._screen - panel_title = self._small_font.render("Foot IK (pelvis)", True, (180, 200, 255)) - screen.blit(panel_title, (MARGIN, 38)) - if self._leg_ik_error: - err = self._leg_ik_error if len(self._leg_ik_error) < 42 else self._leg_ik_error[:39] + "..." - screen.blit(self._small_font.render(f"IK off: {err}", True, (255, 120, 120)), (MARGIN, 50)) - pygame.draw.line( - screen, - (60, 60, 70), - (self._foot_divider_x - 1, HEADER_H - 4), - (self._foot_divider_x - 1, self.config.window_height), - 1, - ) - - for fi, label in enumerate(FOOT_LABELS): - rx, ry, rw, rh = self._foot_row_rect(fi) - lo, hi = float(self._foot_lo[fi]), float(self._foot_hi[fi]) - txt = self._small_font.render(label, True, (190, 210, 230)) - screen.blit(txt, (MARGIN, ry + 4)) - track_y = ry + rh // 2 - 2 - pygame.draw.rect(screen, (45, 55, 70), (rx, track_y, rw, 4), border_radius=2) - kx = self._knob_x(float(self._foot_xyz[fi]), lo, hi, rx, rw) - pygame.draw.rect(screen, (70, 140, 220), (rx, track_y, max(0, kx - rx), 4), border_radius=2) - pygame.draw.rect(screen, (200, 225, 255), (kx - KNOB_W // 2, track_y - 5, KNOB_W, 14), border_radius=3) - val_txt = self._small_font.render(f"{self._foot_xyz[fi]:+.3f}", True, (150, 200, 220)) - screen.blit(val_txt, (rx + rw + 4, ry + 4)) - - def _draw_joint_panel(self) -> None: - pygame = self._pygame - screen = self._screen - num_joint_rows = len(UPPER_BODY_INDICES) if self._leg_ik_ok else NUM_JOINTS - joint_title = "Waist + arms" if self._leg_ik_ok else "All joints" - screen.blit( - self._small_font.render(joint_title, True, (180, 180, 190)), - (self._foot_divider_x + MARGIN, 38), - ) - - for ui in range(num_joint_rows): - rx, ry, rw, rh, ji = self._joint_row_rect(ui) - if ry + rh < HEADER_H or ry > self.config.window_height: - continue - short = JOINT_NAMES[ji].removeprefix("k") - label = self._small_font.render(f"{ji:02d} {short}", True, (200, 200, 210)) - screen.blit(label, (self._foot_divider_x + MARGIN, ry + 4)) - track_y = ry + rh // 2 - 2 - lo, hi = float(JOINT_LO[ji]), float(JOINT_HI[ji]) - pygame.draw.rect(screen, (55, 55, 65), (rx, track_y, rw, 4), border_radius=2) - kx = self._knob_x(float(self._values[ji]), lo, hi, rx, rw) - pygame.draw.rect(screen, (80, 160, 255), (rx, track_y, max(0, kx - rx), 4), border_radius=2) - pygame.draw.rect(screen, (220, 235, 255), (kx - KNOB_W // 2, track_y - 5, KNOB_W, 14), border_radius=3) - val_txt = self._small_font.render(f"{self._values[ji]:+.3f}", True, (170, 220, 170)) - screen.blit(val_txt, (rx + rw + 8, ry + 4)) - - def _draw(self) -> None: - pygame = self._pygame - screen = self._screen - screen.fill((28, 28, 32)) - title = self._font.render("G1 reference → SONIC encoder mode 0", True, (230, 230, 235)) - hint = self._small_font.render("Foot xyz (left) · waist/arms (right) · R reset · Esc quit", True, (150, 150, 160)) - screen.blit(title, (MARGIN, 10)) - screen.blit(hint, (MARGIN, 28)) - if self.config.use_leg_ik: - self._draw_foot_panel() - self._draw_joint_panel() - pygame.display.flip() - - def get_action(self) -> dict[str, float]: - if not self._connected: - return {f"{name}.q": float(self._values[i]) for i, name in enumerate(JOINT_NAMES)} - if not self._handle_events(): - raise KeyboardInterrupt("G1 sonic slider window closed") - if self._leg_ik_ok: - # Read the newest solution from the solver process and hand it the latest - # foot target — never block the teleop loop on the IPOPT solve. - self._pump_ik_results() - self._publish_ik_target() - self._draw() - self._clock.tick(60) - return {f"{name}.q": float(self._values[i]) for i, name in enumerate(JOINT_NAMES)} - - def send_feedback(self, feedback: dict) -> None: - del feedback - - def disconnect(self) -> None: - self._stop_ik_process() - if self._pygame is not None: - self._pygame.quit() - self._connected = False - self._screen = None diff --git a/src/lerobot/teleoperators/g1_sonic_slider/joint_limits.py b/src/lerobot/teleoperators/g1_sonic_slider/joint_limits.py deleted file mode 100644 index 16d293c12..000000000 --- a/src/lerobot/teleoperators/g1_sonic_slider/joint_limits.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python - -# Copyright 2026 The HuggingFace Inc. team. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""G1 29-DOF joint limits (rad) in MuJoCo / G1_29_JointIndex order — from g1_29dof.xml. - -SONIC encoder mode 0 expects Isaac Lab order; the whole-body controller remaps on ingest. -""" - -import numpy as np - -from lerobot.robots.unitree_g1.g1_utils import G1_29_JointIndex - -# (low, high) per joint index 0..28 -_G1_LIMITS = np.array( - [ - (-2.5307, 2.8798), # kLeftHipPitch - (-0.5236, 2.9671), # kLeftHipRoll - (-2.7576, 2.7576), # kLeftHipYaw - (-0.087267, 2.8798), # kLeftKnee - (-0.87267, 0.5236), # kLeftAnklePitch - (-0.2618, 0.2618), # kLeftAnkleRoll - (-2.5307, 2.8798), # kRightHipPitch - (-2.9671, 0.5236), # kRightHipRoll - (-2.7576, 2.7576), # kRightHipYaw - (-0.087267, 2.8798), # kRightKnee - (-0.87267, 0.5236), # kRightAnklePitch - (-0.2618, 0.2618), # kRightAnkleRoll - (-2.618, 2.618), # kWaistYaw - (-0.52, 0.52), # kWaistRoll - (-0.52, 0.52), # kWaistPitch - (-3.0892, 2.6704), # kLeftShoulderPitch - (-1.5882, 2.2515), # kLeftShoulderRoll - (-2.618, 2.618), # kLeftShoulderYaw - (-1.0472, 2.0944), # kLeftElbow - (-1.97222, 1.97222), # kLeftWristRoll - (-1.61443, 1.61443), # kLeftWristPitch - (-1.61443, 1.61443), # kLeftWristYaw - (-3.0892, 2.6704), # kRightShoulderPitch - (-2.2515, 1.5882), # kRightShoulderRoll - (-2.618, 2.618), # kRightShoulderYaw - (-1.0472, 2.0944), # kRightElbow - (-1.97222, 1.97222), # kRightWristRoll - (-1.61443, 1.61443), # kRightWristPitch - (-1.61443, 1.61443), # kRightWristYaw - ], - dtype=np.float32, -) - -JOINT_NAMES = [m.name for m in G1_29_JointIndex] -JOINT_LO = _G1_LIMITS[:, 0] -JOINT_HI = _G1_LIMITS[:, 1]