mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-17 06:51:48 +00:00
Remove g1_sonic_slider, examples/onnx, and SONIC debugging docs
This commit is contained in:
Martino Russi
@@ -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.
|
||||
@@ -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.
|
||||
@@ -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.
|
||||
@@ -10,11 +10,11 @@ deploy stack (no `gear_sonic`/torch dependency).
|
||||
|
||||
Selected with `--robot.controller=<ClassName>`:
|
||||
|
||||
| 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=<ClassName>`:
|
||||
## 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)
|
||||
|
||||
|
||||
@@ -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"]
|
||||
@@ -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."""
|
||||
@@ -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
|
||||
@@ -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]
|
||||
Reference in New Issue
Block a user