remove transform_imu_data

- Use JSON + base64 serialization for secure communication instead of pickle
- Add documentation section
- Rename robot_server to run_g1_server
- Add dependecies to pyproject.toml

.github/workflows/fast_tests.yml

@@ -60,19 +60,12 @@ jobs:
     runs-on: ubuntu-latest
     env:
       MUJOCO_GL: egl
-      HF_HOME: /mnt/cache/.cache/huggingface
-      HF_LEROBOT_HOME: /mnt/cache/.cache/huggingface/lerobot
     steps:
       - uses: actions/checkout@v4
         with:
           persist-credentials: false
           lfs: true
 
-      # NOTE(Steven): Mount to `/mnt` to avoid the limited storage on `/home`. Consider cleaning default SDKs or using self-hosted runners for more space.
-      # (As of 2024-06-10, the runner's `/home` has only 6.2 GB free—8% of its 72 GB total.)
-      - name: Setup /mnt storage
-        run: sudo chown -R $USER:$USER /mnt
-
       # TODO(Steven): Evaluate the need of these dependencies
       - name: Install apt dependencies
         run: |

.github/workflows/full_tests.yml

@@ -58,19 +58,12 @@ jobs:
       github.event_name == 'workflow_dispatch'
     env:
       MUJOCO_GL: egl
-      HF_HOME: /mnt/cache/.cache/huggingface
-      HF_LEROBOT_HOME: /mnt/cache/.cache/huggingface/lerobot
     steps:
       - uses: actions/checkout@v4
         with:
           lfs: true
           persist-credentials: false
 
-      # NOTE(Steven): Mount to `/mnt` to avoid the limited storage on `/home`. Consider cleaning default SDKs or using self-hosted runners for more space.
-      # (As of 2024-06-10, the runner's `/home` has only 6.2 GB free—8% of its 72 GB total.)
-      - name: Setup /mnt storage
-        run: sudo chown -R $USER:$USER /mnt
-
       - name: Install apt dependencies
         run: |
           sudo apt-get update && sudo apt-get install -y build-essential \

.github/workflows/unbound_deps_tests.yml

@@ -45,19 +45,12 @@ jobs:
     runs-on: ubuntu-latest
     env:
       MUJOCO_GL: egl
-      HF_HOME: /mnt/cache/.cache/huggingface
-      HF_LEROBOT_HOME: /mnt/cache/.cache/huggingface/lerobot
     steps:
       - uses: actions/checkout@v4
         with:
           lfs: true
           persist-credentials: false
 
-      # NOTE(Steven): Mount to `/mnt` to avoid the limited storage on `/home`. Consider cleaning default SDKs or using self-hosted runners for more space.
-      # (As of 2024-06-10, the runner's `/home` has only 6.2 GB free—8% of its 72 GB total.)
-      - name: Setup /mnt storage
-        run: sudo chown -R $USER:$USER /mnt
-
       - name: Install apt dependencies
         run: |
           sudo apt-get update && sudo apt-get install -y build-essential \

benchmarks/video/benchmark.py

@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+
+# Copyright 2024 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.
+import threading
+import time
+from contextlib import ContextDecorator
+
+
+class TimeBenchmark(ContextDecorator):
+    """
+    Measures execution time using a context manager or decorator.
+
+    This class supports both context manager and decorator usage, and is thread-safe for multithreaded
+    environments.
+
+    Args:
+        print: If True, prints the elapsed time upon exiting the context or completing the function. Defaults
+        to False.
+
+    Examples:
+
+        Using as a context manager:
+
+        >>> benchmark = TimeBenchmark()
+        >>> with benchmark:
+        ...     time.sleep(1)
+        >>> print(f"Block took {benchmark.result:.4f} seconds")
+        Block took approximately 1.0000 seconds
+
+        Using with multithreading:
+
+        ```python
+        import threading
+
+        benchmark = TimeBenchmark()
+
+
+        def context_manager_example():
+            with benchmark:
+                time.sleep(0.01)
+            print(f"Block took {benchmark.result_ms:.2f} milliseconds")
+
+
+        threads = []
+        for _ in range(3):
+            t1 = threading.Thread(target=context_manager_example)
+            threads.append(t1)
+
+        for t in threads:
+            t.start()
+
+        for t in threads:
+            t.join()
+        ```
+        Expected output:
+        Block took approximately 10.00 milliseconds
+        Block took approximately 10.00 milliseconds
+        Block took approximately 10.00 milliseconds
+    """
+
+    def __init__(self, print=False):
+        self.local = threading.local()
+        self.print_time = print
+
+    def __enter__(self):
+        self.local.start_time = time.perf_counter()
+        return self
+
+    def __exit__(self, *exc):
+        self.local.end_time = time.perf_counter()
+        self.local.elapsed_time = self.local.end_time - self.local.start_time
+        if self.print_time:
+            print(f"Elapsed time: {self.local.elapsed_time:.4f} seconds")
+        return False
+
+    @property
+    def result(self):
+        return getattr(self.local, "elapsed_time", None)
+
+    @property
+    def result_ms(self):
+        return self.result * 1e3

benchmarks/video/capture_camera_feed.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python
+
+# Copyright 2024 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.
+"""Capture video feed from a camera as raw images."""
+
+import argparse
+import datetime as dt
+import os
+import time
+from pathlib import Path
+
+import cv2
+import rerun as rr
+
+# see https://rerun.io/docs/howto/visualization/limit-ram
+RERUN_MEMORY_LIMIT = os.getenv("LEROBOT_RERUN_MEMORY_LIMIT", "5%")
+
+
+def display_and_save_video_stream(output_dir: Path, fps: int, width: int, height: int, duration: int):
+    rr.init("lerobot_capture_camera_feed")
+    rr.spawn(memory_limit=RERUN_MEMORY_LIMIT)
+
+    now = dt.datetime.now()
+    capture_dir = output_dir / f"{now:%Y-%m-%d}" / f"{now:%H-%M-%S}"
+    if not capture_dir.exists():
+        capture_dir.mkdir(parents=True, exist_ok=True)
+
+    # Opens the default webcam
+    cap = cv2.VideoCapture(0)
+    if not cap.isOpened():
+        print("Error: Could not open video stream.")
+        return
+
+    cap.set(cv2.CAP_PROP_FPS, fps)
+    cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
+    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
+
+    frame_index = 0
+    start_time = time.time()
+    while time.time() - start_time < duration:
+        ret, frame = cap.read()
+
+        if not ret:
+            print("Error: Could not read frame.")
+            break
+        rr.log("video/stream", rr.Image(frame), static=True)
+        cv2.imwrite(str(capture_dir / f"frame_{frame_index:06d}.png"), frame)
+        frame_index += 1
+
+    # Release the capture
+    cap.release()
+
+    # TODO(Steven): Add a graceful shutdown via a close() method for the Viewer context, though not currently supported in the Rerun API.
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+
+    parser.add_argument(
+        "--output-dir",
+        type=Path,
+        default=Path("outputs/cam_capture/"),
+        help="Directory where the capture images are written. A subfolder named with the current date & time will be created inside it for each capture.",
+    )
+    parser.add_argument(
+        "--fps",
+        type=int,
+        default=30,
+        help="Frames Per Second of the capture.",
+    )
+    parser.add_argument(
+        "--width",
+        type=int,
+        default=1280,
+        help="Width of the captured images.",
+    )
+    parser.add_argument(
+        "--height",
+        type=int,
+        default=720,
+        help="Height of the captured images.",
+    )
+    parser.add_argument(
+        "--duration",
+        type=int,
+        default=20,
+        help="Duration in seconds for which the video stream should be captured.",
+    )
+    args = parser.parse_args()
+    display_and_save_video_stream(**vars(args))

benchmarks/video/run_video_benchmark.py

@@ -21,13 +21,11 @@ See the provided README.md or run `python benchmark/video/run_video_benchmark.py
 
 import argparse
 import datetime as dt
-import itertools
 import random
 import shutil
 from collections import OrderedDict
 from concurrent.futures import ThreadPoolExecutor, as_completed
 from pathlib import Path
-from threading import Lock
 
 import einops
 import numpy as np
@@ -37,13 +35,13 @@ import torch
 from skimage.metrics import mean_squared_error, peak_signal_noise_ratio, structural_similarity
 from tqdm import tqdm
 
+from benchmarks.video.benchmark import TimeBenchmark
 from lerobot.datasets.lerobot_dataset import LeRobotDataset
 from lerobot.datasets.video_utils import (
-    decode_video_frames,
+    decode_video_frames_torchvision,
     encode_video_frames,
 )
 from lerobot.utils.constants import OBS_IMAGE
-from lerobot.utils.utils import TimerManager
 
 BASE_ENCODING = OrderedDict(
     [
@@ -88,7 +86,7 @@ def load_original_frames(imgs_dir: Path, timestamps: list[float], fps: int) -> t
     frames = []
     for ts in timestamps:
         idx = int(ts * fps)
-        frame = PIL.Image.open(imgs_dir / f"frame-{idx:06d}.png")
+        frame = PIL.Image.open(imgs_dir / f"frame_{idx:06d}.png")
         frame = torch.from_numpy(np.array(frame))
         frame = frame.type(torch.float32) / 255
         frame = einops.rearrange(frame, "h w c -> c h w")
@@ -99,21 +97,21 @@ def load_original_frames(imgs_dir: Path, timestamps: list[float], fps: int) -> t
 def save_decoded_frames(
     imgs_dir: Path, save_dir: Path, frames: torch.Tensor, timestamps: list[float], fps: int
 ) -> None:
-    if save_dir.exists() and len(list(save_dir.glob("frame-*.png"))) == len(timestamps):
+    if save_dir.exists() and len(list(save_dir.glob("frame_*.png"))) == len(timestamps):
         return
 
     save_dir.mkdir(parents=True, exist_ok=True)
     for i, ts in enumerate(timestamps):
         idx = int(ts * fps)
         frame_hwc = (frames[i].permute((1, 2, 0)) * 255).type(torch.uint8).cpu().numpy()
-        PIL.Image.fromarray(frame_hwc).save(save_dir / f"frame-{idx:06d}_decoded.png")
-        shutil.copyfile(imgs_dir / f"frame-{idx:06d}.png", save_dir / f"frame-{idx:06d}_original.png")
+        PIL.Image.fromarray(frame_hwc).save(save_dir / f"frame_{idx:06d}_decoded.png")
+        shutil.copyfile(imgs_dir / f"frame_{idx:06d}.png", save_dir / f"frame_{idx:06d}_original.png")
 
 
 def save_first_episode(imgs_dir: Path, dataset: LeRobotDataset) -> None:
     episode_index = 0
     ep_num_images = dataset.meta.episodes["length"][episode_index]
-    if imgs_dir.exists() and len(list(imgs_dir.glob("frame-*.png"))) == ep_num_images:
+    if imgs_dir.exists() and len(list(imgs_dir.glob("frame_*.png"))) == ep_num_images:
         return
 
     imgs_dir.mkdir(parents=True, exist_ok=True)
@@ -127,7 +125,7 @@ def save_first_episode(imgs_dir: Path, dataset: LeRobotDataset) -> None:
         tqdm(imgs_dataset, desc=f"saving {dataset.repo_id} first episode images", leave=False)
     ):
         img = item[img_keys[0]]
-        img.save(str(imgs_dir / f"frame-{i:06d}.png"), quality=100)
+        img.save(str(imgs_dir / f"frame_{i:06d}.png"), quality=100)
 
         if i >= ep_num_images - 1:
             break
@@ -151,6 +149,18 @@ def sample_timestamps(timestamps_mode: str, ep_num_images: int, fps: int) -> lis
     return [idx / fps for idx in frame_indexes]
 
 
+def decode_video_frames(
+    video_path: str,
+    timestamps: list[float],
+    tolerance_s: float,
+    backend: str,
+) -> torch.Tensor:
+    if backend in ["pyav", "video_reader"]:
+        return decode_video_frames_torchvision(video_path, timestamps, tolerance_s, backend)
+    else:
+        raise NotImplementedError(backend)
+
+
 def benchmark_decoding(
     imgs_dir: Path,
     video_path: Path,
@@ -162,8 +172,8 @@ def benchmark_decoding(
     num_workers: int = 4,
     save_frames: bool = False,
 ) -> dict:
-    def process_sample(sample: int, lock: Lock):
-        time_benchmark = TimerManager(log=False)
+    def process_sample(sample: int):
+        time_benchmark = TimeBenchmark()
         timestamps = sample_timestamps(timestamps_mode, ep_num_images, fps)
         num_frames = len(timestamps)
         result = {
@@ -172,13 +182,13 @@ def benchmark_decoding(
             "mse_values": [],
         }
 
-        with time_benchmark, lock:
+        with time_benchmark:
             frames = decode_video_frames(video_path, timestamps=timestamps, tolerance_s=5e-1, backend=backend)
-        result["load_time_video_ms"] = (time_benchmark.last * 1000) / num_frames
+        result["load_time_video_ms"] = time_benchmark.result_ms / num_frames
 
         with time_benchmark:
             original_frames = load_original_frames(imgs_dir, timestamps, fps)
-        result["load_time_images_ms"] = (time_benchmark.last * 1000) / num_frames
+        result["load_time_images_ms"] = time_benchmark.result_ms / num_frames
 
         frames_np, original_frames_np = frames.numpy(), original_frames.numpy()
         for i in range(num_frames):
@@ -205,10 +215,8 @@ def benchmark_decoding(
     # A sample is a single set of decoded frames specified by timestamps_mode (e.g. a single frame, 2 frames, etc.).
     # For each sample, we record metrics (loading time and quality metrics) which are then averaged over all samples.
     # As these samples are independent, we run them in parallel threads to speed up the benchmark.
-    # Use a single shared lock for all worker threads
-    shared_lock = Lock()
     with ThreadPoolExecutor(max_workers=num_workers) as executor:
-        futures = [executor.submit(process_sample, i, shared_lock) for i in range(num_samples)]
+        futures = [executor.submit(process_sample, i) for i in range(num_samples)]
         for future in tqdm(as_completed(futures), total=num_samples, desc="samples", leave=False):
             result = future.result()
             load_times_video_ms.append(result["load_time_video_ms"])
@@ -350,27 +358,24 @@ def main(
                 imgs_dir = output_dir / "images" / dataset.repo_id.replace("/", "_")
                 # We only use the first episode
                 save_first_episode(imgs_dir, dataset)
-                for duet in [
-                    dict(zip(encoding_benchmarks.keys(), unique_combination, strict=False))
-                    for unique_combination in itertools.product(*encoding_benchmarks.values())
-                ]:
-                    encoding_cfg = BASE_ENCODING.copy()
-                    encoding_cfg["vcodec"] = video_codec
-                    encoding_cfg["pix_fmt"] = pixel_format
-                    for key, value in duet.items():
+                for key, values in tqdm(encoding_benchmarks.items(), desc="encodings (g, crf)", leave=False):
+                    for value in tqdm(values, desc=f"encodings ({key})", leave=False):
+                        encoding_cfg = BASE_ENCODING.copy()
+                        encoding_cfg["vcodec"] = video_codec
+                        encoding_cfg["pix_fmt"] = pixel_format
                         encoding_cfg[key] = value
-                    args_path = Path("_".join(str(value) for value in encoding_cfg.values()))
-                    video_path = output_dir / "videos" / args_path / f"{repo_id.replace('/', '_')}.mp4"
-                    benchmark_table += benchmark_encoding_decoding(
-                        dataset,
-                        video_path,
-                        imgs_dir,
-                        encoding_cfg,
-                        decoding_benchmarks,
-                        num_samples,
-                        num_workers,
-                        save_frames,
-                    )
+                        args_path = Path("_".join(str(value) for value in encoding_cfg.values()))
+                        video_path = output_dir / "videos" / args_path / f"{repo_id.replace('/', '_')}.mp4"
+                        benchmark_table += benchmark_encoding_decoding(
+                            dataset,
+                            video_path,
+                            imgs_dir,
+                            encoding_cfg,
+                            decoding_benchmarks,
+                            num_samples,
+                            num_workers,
+                            save_frames,
+                        )
 
             # Save intermediate results
             benchmark_df = pd.DataFrame(benchmark_table, columns=headers)
@@ -404,9 +409,9 @@ if __name__ == "__main__":
         nargs="*",
         default=[
             "lerobot/pusht_image",
-            "lerobot/aloha_mobile_shrimp_image",
-            "lerobot/paris_street",
-            "lerobot/kitchen",
+            "aliberts/aloha_mobile_shrimp_image",
+            "aliberts/paris_street",
+            "aliberts/kitchen",
         ],
         help="Datasets repo-ids to test against. First episodes only are used. Must be images.",
     )
@@ -414,7 +419,7 @@ if __name__ == "__main__":
         "--vcodec",
         type=str,
         nargs="*",
-        default=["h264", "hevc", "libsvtav1"],
+        default=["libx264", "hevc", "libsvtav1"],
         help="Video codecs to be tested",
     )
     parser.add_argument(
@@ -463,7 +468,7 @@ if __name__ == "__main__":
         "--backends",
         type=str,
         nargs="*",
-        default=["torchcodec", "pyav"],
+        default=["pyav", "video_reader"],
         help="Torchvision decoding backend to be tested.",
     )
     parser.add_argument(

docs/source/envhub_leisaac.mdx

@@ -139,7 +139,7 @@ from lerobot.teleoperators import (  # noqa: F401
     make_teleoperator_from_config,
     so101_leader,
 )
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import init_logging
 from lerobot.envs.factory import make_env
 
@@ -196,7 +196,7 @@ def teleop_loop(teleop: Teleoperator, env: gym.Env, fps: int):
             obs, info = env.reset()
 
         dt_s = time.perf_counter() - loop_start
-        precise_sleep(1 / fps - dt_s)
+        busy_wait(1 / fps - dt_s)
         loop_s = time.perf_counter() - loop_start
         print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
 

docs/source/il_robots.mdx

@@ -393,7 +393,7 @@ import time
 from lerobot.datasets.lerobot_dataset import LeRobotDataset
 from lerobot.robots.so100_follower.config_so100_follower import SO100FollowerConfig
 from lerobot.robots.so100_follower.so100_follower import SO100Follower
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import log_say
 
 episode_idx = 0
@@ -415,7 +415,7 @@ for idx in range(dataset.num_frames):
     }
     robot.send_action(action)
 
-    precise_sleep(1.0 / dataset.fps - (time.perf_counter() - t0))
+    busy_wait(1.0 / dataset.fps - (time.perf_counter() - t0))
 
 robot.disconnect()
 ```

docs/source/unitree_g1.mdx

@@ -1,14 +1,14 @@
 # Unitree G1 Robot Setup and Control
 
-This guide covers the complete setup process for the Unitree G1 humanoid, from initial connection to running gr00t_wbc locomotion.
+This guide covers the complete setup process for the Unitree G1 humanoid robot, from initial connection to running locomotion policies.
 
-## About the Unitree G1
+## 🤖 About the Unitree G1
 
-We offer support for both 29 and 23 DOF G1. In this first PR we introduce:
+The Unitree G1 humanoid comes in two flavors: 29-DOF and 23-DOF humanoid robot capable of whole-body control, manipulation, and locomotion. In this first PR we introduce:
 
-- **`unitree g1` robot class, handling low level communication with the humanoid**
+- **Low-level motor control** via DDS (Data Distribution Service)
 - **ZMQ socket bridge** for remote communication over WiFi, allowing one to deploy policies remotely instead of over ethernet or directly on the Orin
-- **GR00T locomotion policy** for bipedal walking and balance
+- **GR00T locomotion policiey** for bipedal walking and balance
 
 ---
 
@@ -25,7 +25,7 @@ sudo ip addr add 192.168.123.200/24 dev enp131s0
 sudo ip link set enp131s0 up
 ```
 
-**Note**: The robot's Ethernet IP is fixed at `192.168.123.164`. Your computer must use `192.168.123.x` where x ≠ 164.
+> **Note**: The robot's Ethernet IP is fixed at `192.168.123.164`. Your computer must use `192.168.123.x` where x ≠ 164.
 
 ### Step 2: SSH into the Robot
 
@@ -103,63 +103,76 @@ ip a show wlan0
 
 ### Step 4: SSH Over WiFi
 
-Once connected to WiFi, note the robot's IP address and disconnect the Ethernet cable. You can now SSH over WiFi:
+Once connected to WiFi, note the robot's IP address (e.g., `172.18.129.215`) and disconnect the Ethernet cable. You can now SSH over WiFi:
 
 ```bash
-ssh unitree@<YOUR_ROBOT_IP>
+ssh unitree@172.18.129.215
 # Password: 123
 ```
 
-Replace `<YOUR_ROBOT_IP>` with your robot's actual WiFi IP address (e.g., `172.18.129.215`).
-
 ---
 
 ## Part 3: Robot Server Setup
 
-### Step 1: Install LeRobot on the Orin
+The robot server introduced here acts as a DDS-to-ZMQ bridge, allowing your one to control the robot wirelessly.
 
-SSH into the robot and install LeRobot:
+### Step 1: Copy Server Script to Robot
+
+From your laptop, copy the robot server script:
 
 ```bash
-ssh unitree@<YOUR_ROBOT_IP>
-
-conda create -y -n lerobot python=3.10
-conda activate lerobot
-git clone https://github.com/huggingface/lerobot.git
-cd lerobot
-pip install -e '.[unitree_g1]'
-git clone https://github.com/unitreerobotics/unitree_sdk2_python.git
-cd unitree_sdk2_python  && pip install -e .
+# Copy the server script and its dependencies
+scp src/lerobot/robots/unitree_g1/run_g1_server.py unitree@172.18.129.215:~/run_g1_server.py
+scp src/lerobot/robots/unitree_g1/g1_utils.py unitree@172.18.129.215:~/g1_utils.py
 ```
 
-**Note**: The Unitree SDK requires CycloneDDS v0.10.2 to be installed. See the [Unitree SDK documentation](https://github.com/unitreerobotics/unitree_sdk2_python) for details.
+### Step 2: Install Dependencies on Robot
 
-### Step 2: Run the Robot Server
+SSH into the robot and install required packages:
+
+```bash
+ssh unitree@172.18.129.215
+
+# Install build tools and Python dependencies
+sudo apt update
+sudo apt install -y build-essential python3-dev python3-pip
+
+# Install Python packages (pyzmq and Unitree SDK)
+pip3 install pyzmq
+pip3 install git+https://github.com/unitreerobotics/unitree_sdk2_python.git
+```
+
+> **Note**: The Unitree SDK requires CycloneDDS v0.10.2 to be installed. See the [Unitree SDK documentation](https://github.com/unitreerobotics/unitree_sdk2_python) for details.
+
+### Step 3: Run the Robot Server
 
 On the robot:
 
 ```bash
-python src/lerobot/robots/unitree_g1/run_g1_server.py
+python3 ~/run_g1_server.py
 ```
 
-**Important**: Keep this terminal running. The server must be active for remote control.
+You should see output like:
+
+```
+Robot server listening on:
+  Commands:  tcp://*:6000 (PULL)
+  State:     tcp://*:6001 (PUB)
+DDS initialized, forwarding started...
+```
+
+> **Important**: Keep this terminal running. The server must be active for remote control.
 
 ---
 
-## Part 4: Running GR00T Locomotion
+## 🚶 Part 4: Running GR00T Locomotion
 
 With the robot server running, you can now control the robot from your laptop.
 
-### Step 1: Install LeRobot on your machine
+### Step 1: Install LeRobot with Unitree G1 Support (on your laptop)
 
 ```bash
-conda create -y -n lerobot python=3.10
-conda activate lerobot
-git clone https://github.com/huggingface/lerobot.git
-cd lerobot
 pip install -e '.[unitree_g1]'
-git clone https://github.com/unitreerobotics/unitree_sdk2_python.git
-cd unitree_sdk2_python  && pip install -e .
 ```
 
 ### Step 2: Update Robot IP in Config
@@ -168,16 +181,40 @@ Edit the config file to match your robot's WiFi IP:
 
 ```python
 # In src/lerobot/robots/unitree_g1/config_unitree_g1.py
-robot_ip: str = "<YOUR_ROBOT_IP>"  # Replace with your robot's WiFi IP.
+robot_ip: str = "172.18.129.215"  # Your robot's WiFi IP
 ```
 
-**Note**: When running directly on the G1 (not remotely), set `robot_ip: str = "127.0.0.1"` instead.
-
 ### Step 3: Run the Locomotion Policy
 
 ```bash
-# Run GR00T locomotion controller
+# Run GR00T locomotion controller (downloads policies from HuggingFace)
 python examples/unitree_g1/gr00t_locomotion.py --repo-id "nepyope/GR00T-WholeBodyControl_g1"
+
+# Or use the default repo (same as above):
+python examples/unitree_g1/gr00t_locomotion.py
+```
+
+The script will:
+
+1. Download Balance and Walk policies from the Hub (cached locally after first run)
+2. Connect to the robot server over WiFi/ZMQ
+3. Initialize the robot and locomotion controller
+4. Gradually move legs to default standing position (3 seconds)
+5. Start locomotion control loop at 50Hz in background thread
+6. Accept commands from the wireless remote controller
+
+**Expected output:**
+
+```
+INFO - Loading GR00T Balance policy...
+INFO - Loading GR00T Walk policy...
+INFO - [UnitreeG1] Initialize UnitreeG1...
+INFO - [UnitreeG1] Connected to robot.
+INFO - Reached default position (legs only)
+INFO - Locomotion control thread started!
+INFO - Robot initialized with GR00T locomotion policies
+INFO - Locomotion controller running in background thread
+INFO - Press Ctrl+C to stop
 ```
 
 ### Step 4: Control with Remote
@@ -187,7 +224,7 @@ python examples/unitree_g1/gr00t_locomotion.py --repo-id "nepyope/GR00T-WholeBod
 - **R1 button**: Raise waist height
 - **R2 button**: Lower waist height
 
-Press `Ctrl+C` to stop the policy.
+To stop, press `Ctrl+C` in the terminal.
 
 ---
 
@@ -200,4 +237,4 @@ Press `Ctrl+C` to stop the policy.
 
 ---
 
-_Last updated: December 2025_
+_Last updated: November 2025_

examples/backward_compatibility/replay.py

@@ -45,7 +45,7 @@ from lerobot.robots import (  # noqa: F401
     so101_follower,
 )
 from lerobot.utils.constants import ACTION
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import (
     init_logging,
     log_say,
@@ -97,7 +97,7 @@ def replay(cfg: ReplayConfig):
         robot.send_action(action)
 
         dt_s = time.perf_counter() - start_episode_t
-        precise_sleep(1 / dataset.fps - dt_s)
+        busy_wait(1 / dataset.fps - dt_s)
 
     robot.disconnect()
 

examples/lekiwi/replay.py

@@ -20,7 +20,7 @@ from lerobot.datasets.lerobot_dataset import LeRobotDataset
 from lerobot.robots.lekiwi.config_lekiwi import LeKiwiClientConfig
 from lerobot.robots.lekiwi.lekiwi_client import LeKiwiClient
 from lerobot.utils.constants import ACTION
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import log_say
 
 EPISODE_IDX = 0
@@ -58,7 +58,7 @@ def main():
         # Send action to robot
         _ = robot.send_action(action)
 
-        precise_sleep(max(1.0 / dataset.fps - (time.perf_counter() - t0), 0.0))
+        busy_wait(max(1.0 / dataset.fps - (time.perf_counter() - t0), 0.0))
 
     robot.disconnect()
 

examples/lekiwi/teleoperate.py

@@ -19,7 +19,7 @@ import time
 from lerobot.robots.lekiwi import LeKiwiClient, LeKiwiClientConfig
 from lerobot.teleoperators.keyboard.teleop_keyboard import KeyboardTeleop, KeyboardTeleopConfig
 from lerobot.teleoperators.so100_leader import SO100Leader, SO100LeaderConfig
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.visualization_utils import init_rerun, log_rerun_data
 
 FPS = 30
@@ -71,7 +71,7 @@ def main():
         # Visualize
         log_rerun_data(observation=observation, action=action)
 
-        precise_sleep(max(1.0 / FPS - (time.perf_counter() - t0), 0.0))
+        busy_wait(max(1.0 / FPS - (time.perf_counter() - t0), 0.0))
 
 
 if __name__ == "__main__":

examples/phone_to_so100/replay.py

@@ -29,7 +29,7 @@ from lerobot.robots.so100_follower.robot_kinematic_processor import (
 )
 from lerobot.robots.so100_follower.so100_follower import SO100Follower
 from lerobot.utils.constants import ACTION
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import log_say
 
 EPISODE_IDX = 0
@@ -96,7 +96,7 @@ def main():
         # Send action to robot
         _ = robot.send_action(joint_action)
 
-        precise_sleep(1.0 / dataset.fps - (time.perf_counter() - t0))
+        busy_wait(1.0 / dataset.fps - (time.perf_counter() - t0))
 
     # Clean up
     robot.disconnect()

examples/phone_to_so100/teleoperate.py

@@ -32,7 +32,7 @@ from lerobot.robots.so100_follower.so100_follower import SO100Follower
 from lerobot.teleoperators.phone.config_phone import PhoneConfig, PhoneOS
 from lerobot.teleoperators.phone.phone_processor import MapPhoneActionToRobotAction
 from lerobot.teleoperators.phone.teleop_phone import Phone
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.visualization_utils import init_rerun, log_rerun_data
 
 FPS = 30
@@ -114,7 +114,7 @@ def main():
         # Visualize
         log_rerun_data(observation=phone_obs, action=joint_action)
 
-        precise_sleep(max(1.0 / FPS - (time.perf_counter() - t0), 0.0))
+        busy_wait(max(1.0 / FPS - (time.perf_counter() - t0), 0.0))
 
 
 if __name__ == "__main__":

examples/so100_to_so100_EE/replay.py

@@ -30,7 +30,7 @@ from lerobot.robots.so100_follower.robot_kinematic_processor import (
 )
 from lerobot.robots.so100_follower.so100_follower import SO100Follower
 from lerobot.utils.constants import ACTION
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import log_say
 
 EPISODE_IDX = 0
@@ -97,7 +97,7 @@ def main():
         # Send action to robot
         _ = robot.send_action(joint_action)
 
-        precise_sleep(1.0 / dataset.fps - (time.perf_counter() - t0))
+        busy_wait(1.0 / dataset.fps - (time.perf_counter() - t0))
 
     # Clean up
     robot.disconnect()

examples/so100_to_so100_EE/teleoperate.py

@@ -32,7 +32,7 @@ from lerobot.robots.so100_follower.robot_kinematic_processor import (
 from lerobot.robots.so100_follower.so100_follower import SO100Follower
 from lerobot.teleoperators.so100_leader.config_so100_leader import SO100LeaderConfig
 from lerobot.teleoperators.so100_leader.so100_leader import SO100Leader
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.visualization_utils import init_rerun, log_rerun_data
 
 FPS = 30
@@ -120,7 +120,7 @@ def main():
         # Visualize
         log_rerun_data(observation=leader_ee_act, action=follower_joints_act)
 
-        precise_sleep(max(1.0 / FPS - (time.perf_counter() - t0), 0.0))
+        busy_wait(max(1.0 / FPS - (time.perf_counter() - t0), 0.0))
 
 
 if __name__ == "__main__":

examples/unitree_g1/gr00t_locomotion.py

@@ -1,18 +1,5 @@
 #!/usr/bin/env python
 
-# Copyright 2025 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.
 """
 Example: GR00T Locomotion with Pre-loaded Policies
 
@@ -28,6 +15,7 @@ from collections import deque
 
 import numpy as np
 import onnxruntime as ort
+import torch
 from huggingface_hub import hf_hub_download
 
 from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
@@ -35,15 +23,46 @@ from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1
 
 logger = logging.getLogger(__name__)
 
-GROOT_DEFAULT_ANGLES = np.zeros(29, dtype=np.float32)
-GROOT_DEFAULT_ANGLES[[0, 6]] = -0.1  # hip pitch
-GROOT_DEFAULT_ANGLES[[3, 9]] = 0.3  # knee
-GROOT_DEFAULT_ANGLES[[4, 10]] = -0.2  # ankle pitch
+GROOT_DEFAULT_ANGLES = np.array(
+    [
+        -0.1,
+        0.0,
+        0.0,
+        0.3,
+        -0.2,
+        0.0,  # left leg
+        -0.1,
+        0.0,
+        0.0,
+        0.3,
+        -0.2,
+        0.0,  # right leg
+        0.0,
+        0.0,
+        0.0,  # waist
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,  # left arm
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,  # right arm
+    ],
+    dtype=np.float32,
+)
 
-MISSING_JOINTS = []
-G1_MODEL = "g1_23"  # or "g1_29"
+G1_MODEL = "g1_23"
 if G1_MODEL == "g1_23":
     MISSING_JOINTS = [12, 14, 20, 21, 27, 28]  # waist yaw/pitch, wrist pitch/yaw
+elif G1_MODEL == "g1_29":
+    MISSING_JOINTS = []  # waist yaw/pitch, wrist pitch/yaw
 
 LOCOMOTION_ACTION_SCALE = 0.25
 
@@ -195,6 +214,7 @@ class GrootLocomotionController:
             self.groot_obs_stacked[start_idx:end_idx] = obs_frame
 
         # Run policy inference (ONNX) with 516D stacked observation
+        obs_tensor = torch.from_numpy(self.groot_obs_stacked).unsqueeze(0)
 
         cmd_magnitude = np.linalg.norm(self.locomotion_cmd)
 
@@ -203,7 +223,7 @@ class GrootLocomotionController:
         )  # balance/standing policy for small commands, walking policy for movement commands
 
         # run policy inference
-        ort_inputs = {selected_policy.get_inputs()[0].name: np.expand_dims(self.groot_obs_stacked, axis=0)}
+        ort_inputs = {selected_policy.get_inputs()[0].name: obs_tensor.cpu().numpy()}
         ort_outs = selected_policy.run(None, ort_inputs)
         self.groot_action = ort_outs[0].squeeze()
 

examples/unitree_g1/holosoma_locomotion.py

@@ -1,479 +0,0 @@
-#!/usr/bin/env python
-
-# Copyright 2025 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.
-"""
-Example: Holosoma Whole-Body Locomotion (23-DOF and 29-DOF)
-
-This example demonstrates loading Holosoma whole-body locomotion policies
-and running them on the Unitree G1 robot.
-
-Supports both:
-- 23-DOF native policies (82D observations, 23D actions)
-- 29-DOF policies (100D observations, 29D actions)
-"""
-
-import argparse
-import logging
-import threading
-import time
-
-import numpy as np
-import onnxruntime as ort
-from huggingface_hub import hf_hub_download
-
-from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
-from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1
-
-logging.basicConfig(level=logging.INFO)
-logger = logging.getLogger(__name__)
-
-# =============================================================================
-# 29-DOF Configuration
-# =============================================================================
-# fmt: off
-HOLOSOMA_29DOF_DEFAULT_ANGLES = np.array([
-    -0.312, 0.0, 0.0, 0.669, -0.363, 0.0,  # left leg
-    -0.312, 0.0, 0.0, 0.669, -0.363, 0.0,  # right leg
-    0.0, 0.0, 0.0,                          # waist (yaw, roll, pitch)
-    0.2, 0.2, 0.0, 0.6, 0.0, 0.0, 0.0,     # left arm
-    0.2, -0.2, 0.0, 0.6, 0.0, 0.0, 0.0,    # right arm
-], dtype=np.float32)
-
-HOLOSOMA_29DOF_KP = np.array([
-    40.179238471, 99.098427777, 40.179238471, 99.098427777, 28.501246196, 28.501246196,  # left leg
-    40.179238471, 99.098427777, 40.179238471, 99.098427777, 28.501246196, 28.501246196,  # right leg
-    40.179238471, 28.501246196, 28.501246196,  # waist
-    14.250623098, 14.250623098, 14.250623098, 14.250623098, 14.250623098, 16.778327481, 16.778327481,  # left arm
-    14.250623098, 14.250623098, 14.250623098, 14.250623098, 14.250623098, 16.778327481, 16.778327481,  # right arm
-], dtype=np.float32)
-
-HOLOSOMA_29DOF_KD = np.array([
-    2.557889765, 6.308801854, 2.557889765, 6.308801854, 1.814445687, 1.814445687,  # left leg
-    2.557889765, 6.308801854, 2.557889765, 6.308801854, 1.814445687, 1.814445687,  # right leg
-    2.557889765, 1.814445687, 1.814445687,  # waist
-    0.907222843, 0.907222843, 0.907222843, 0.907222843, 0.907222843, 1.068141502, 1.068141502,  # left arm
-    0.907222843, 0.907222843, 0.907222843, 0.907222843, 0.907222843, 1.068141502, 1.068141502,  # right arm
-], dtype=np.float32)
-
-# =============================================================================
-# 23-DOF Configuration (native G1-23: no waist_roll/pitch, no wrist_pitch/yaw)
-# Derived from 29-DOF Holosoma values
-# =============================================================================
-# Joint order: 6 left leg, 6 right leg, 1 waist_yaw, 5 left arm, 5 right arm
-HOLOSOMA_23DOF_DEFAULT_ANGLES = np.array([
-    -0.312, 0.0, 0.0, 0.669, -0.363, 0.0,  # left leg (from 29-DOF)
-    -0.312, 0.0, 0.0, 0.669, -0.363, 0.0,  # right leg (from 29-DOF)
-    0.0,                                    # waist_yaw only (from 29-DOF)
-    0.2, 0.2, 0.0, 0.6, 0.0,               # left arm first 5 joints (from 29-DOF)
-    0.2, -0.2, 0.0, 0.6, 0.0,              # right arm first 5 joints (from 29-DOF)
-], dtype=np.float32)
-
-HOLOSOMA_23DOF_KP = np.array([
-    40.179238471, 99.098427777, 40.179238471, 99.098427777, 28.501246196, 28.501246196,  # left leg
-    40.179238471, 99.098427777, 40.179238471, 99.098427777, 28.501246196, 28.501246196,  # right leg
-    40.179238471,                                                                         # waist_yaw
-    14.250623098, 14.250623098, 14.250623098, 14.250623098, 14.250623098,                 # left arm
-    14.250623098, 14.250623098, 14.250623098, 14.250623098, 14.250623098,                 # right arm
-], dtype=np.float32)
-
-HOLOSOMA_23DOF_KD = np.array([
-    2.557889765, 6.308801854, 2.557889765, 6.308801854, 1.814445687, 1.814445687,  # left leg
-    2.557889765, 6.308801854, 2.557889765, 6.308801854, 1.814445687, 1.814445687,  # right leg
-    2.557889765,                                                                    # waist_yaw
-    0.907222843, 0.907222843, 0.907222843, 0.907222843, 0.907222843,               # left arm
-    0.907222843, 0.907222843, 0.907222843, 0.907222843, 0.907222843,               # right arm
-], dtype=np.float32)
-
-# Maps 23-DOF policy index → 29-DOF motor index
-# 23-DOF: legs(0-11), waist_yaw(12), L_arm(13-17), R_arm(18-22)
-# 29-DOF: legs(0-11), waist(12-14), L_arm(15-21), R_arm(22-28)
-DOF_23_TO_MOTOR_MAP = [
-    0, 1, 2, 3, 4, 5,       # left leg → motor 0-5
-    6, 7, 8, 9, 10, 11,     # right leg → motor 6-11
-    12,                      # waist_yaw → motor 12
-    15, 16, 17, 18, 19,     # left arm (skip wrist_pitch/yaw) → motor 15-19
-    22, 23, 24, 25, 26,     # right arm (skip wrist_pitch/yaw) → motor 22-26
-]
-# fmt: on
-
-# Control parameters
-LOCOMOTION_CONTROL_DT = 0.02  # 50Hz
-LOCOMOTION_ACTION_SCALE = 0.25
-ANG_VEL_SCALE = 0.25
-DOF_POS_SCALE = 1.0
-DOF_VEL_SCALE = 0.05
-GAIT_PERIOD = 1.0
-
-DEFAULT_HOLOSOMA_REPO_ID = "nepyope/holosoma_locomotion"
-
-
-def load_holosoma_policy(
-    repo_id: str = DEFAULT_HOLOSOMA_REPO_ID,
-    policy_name: str = "fastsac",
-    local_path: str | None = None,
-) -> tuple[ort.InferenceSession, int]:
-    """Load Holosoma policy and detect observation dimension.
-
-    Returns:
-        (policy, obs_dim) tuple where obs_dim is 82 (23-DOF) or 100 (29-DOF)
-    """
-    if local_path is not None:
-        logger.info(f"Loading policy from local path: {local_path}")
-        policy_path = local_path
-    else:
-        logger.info(f"Loading policy from Hugging Face Hub: {repo_id}")
-        policy_path = hf_hub_download(repo_id=repo_id, filename=f"{policy_name}_g1_29dof.onnx")
-
-    policy = ort.InferenceSession(policy_path)
-
-    # Detect observation dimension from model input shape
-    input_shape = policy.get_inputs()[0].shape
-    obs_dim = input_shape[1] if len(input_shape) > 1 else input_shape[0]
-
-    logger.info(f"Policy loaded successfully")
-    logger.info(f"  Input: {policy.get_inputs()[0].name}, shape: {input_shape} → obs_dim={obs_dim}")
-    logger.info(f"  Output: {policy.get_outputs()[0].name}, shape: {policy.get_outputs()[0].shape}")
-
-    return policy, obs_dim
-
-
-class HolosomaLocomotionController:
-    """
-    Handles Holosoma whole-body locomotion for Unitree G1.
-    Supports both 23-DOF (82D obs) and 29-DOF (100D obs) policies.
-    """
-
-    def __init__(self, policy, robot, config, obs_dim: int = 100):
-        self.policy = policy
-        self.robot = robot
-        self.config = config
-        self.obs_dim = obs_dim
-
-        # Detect policy type from observation dimension
-        self.is_23dof = (obs_dim == 82)
-        self.num_dof = 23 if self.is_23dof else 29
-
-        # Velocity commands
-        self.locomotion_cmd = np.array([0.0, 0.0, 0.0], dtype=np.float32)
-
-        # State variables sized for policy type
-        self.qj = np.zeros(self.num_dof, dtype=np.float32)
-        self.dqj = np.zeros(self.num_dof, dtype=np.float32)
-        self.locomotion_action = np.zeros(self.num_dof, dtype=np.float32)
-        self.locomotion_obs = np.zeros(obs_dim, dtype=np.float32)
-        self.last_unscaled_action = np.zeros(self.num_dof, dtype=np.float32)
-
-        # Select config based on DOF
-        if self.is_23dof:
-            self.default_angles = HOLOSOMA_23DOF_DEFAULT_ANGLES
-            self.kp = HOLOSOMA_23DOF_KP
-            self.kd = HOLOSOMA_23DOF_KD
-            self.motor_map = DOF_23_TO_MOTOR_MAP
-        else:
-            self.default_angles = HOLOSOMA_29DOF_DEFAULT_ANGLES
-            self.kp = HOLOSOMA_29DOF_KP
-            self.kd = HOLOSOMA_29DOF_KD
-            self.motor_map = list(range(29))  # Identity map for 29-DOF
-
-        # Phase state for gait
-        self.phase = np.zeros((1, 2), dtype=np.float32)
-        self.phase[0, 0] = 0.0
-        self.phase[0, 1] = np.pi
-        self.phase_dt = 2 * np.pi / (50.0 * GAIT_PERIOD)
-        self.is_standing = False
-
-        self.counter = 0
-        self.locomotion_running = False
-        self.locomotion_thread = None
-
-        logger.info(f"HolosomaLocomotionController initialized")
-        logger.info(f"  Mode: {'23-DOF (82D obs)' if self.is_23dof else '29-DOF (100D obs)'}")
-        logger.info(f"  Action dim: {self.num_dof}")
-
-    def holosoma_locomotion_run(self):
-        """Main locomotion loop - handles both 23-DOF and 29-DOF."""
-        self.counter += 1
-
-        if self.counter == 1:
-            print("\n" + "=" * 60)
-            print(f"🚀 RUNNING HOLOSOMA {self.num_dof}-DOF LOCOMOTION POLICY")
-            print(f"   {self.obs_dim}D observations → {self.num_dof}D actions")
-            print("=" * 60 + "\n")
-
-        robot_state = self.robot.get_observation()
-        if robot_state is None:
-            return
-
-        # Remote controller
-        if robot_state.wireless_remote is not None:
-            self.robot.remote_controller.set(robot_state.wireless_remote)
-        else:
-            self.robot.remote_controller.lx = 0.0
-            self.robot.remote_controller.ly = 0.0
-            self.robot.remote_controller.rx = 0.0
-            self.robot.remote_controller.ry = 0.0
-
-        # Deadzone
-        ly = self.robot.remote_controller.ly if abs(self.robot.remote_controller.ly) > 0.1 else 0.0
-        lx = self.robot.remote_controller.lx if abs(self.robot.remote_controller.lx) > 0.1 else 0.0
-        rx = self.robot.remote_controller.rx if abs(self.robot.remote_controller.rx) > 0.1 else 0.0
-
-        self.locomotion_cmd[0] = ly
-        self.locomotion_cmd[1] = -lx
-        self.locomotion_cmd[2] = -rx
-
-        # Read joint states using motor map
-        for i in range(self.num_dof):
-            motor_idx = self.motor_map[i]
-            self.qj[i] = robot_state.motor_state[motor_idx].q
-            self.dqj[i] = robot_state.motor_state[motor_idx].dq
-
-        # IMU
-        quat = robot_state.imu_state.quaternion
-        ang_vel = np.array(robot_state.imu_state.gyroscope, dtype=np.float32)
-        gravity_orientation = self.robot.get_gravity_orientation(quat)
-
-        # Scale observations
-        qj_obs = (self.qj - self.default_angles) * DOF_POS_SCALE
-        dqj_obs = self.dqj * DOF_VEL_SCALE
-        ang_vel_scaled = ang_vel * ANG_VEL_SCALE
-
-        # Phase update
-        cmd_norm = np.linalg.norm(self.locomotion_cmd[:2])
-        ang_cmd_norm = np.abs(self.locomotion_cmd[2])
-
-        if cmd_norm < 0.01 and ang_cmd_norm < 0.01:
-            self.phase[0, :] = np.pi * np.ones(2)
-            self.is_standing = True
-        elif self.is_standing:
-            self.phase = np.array([[0.0, np.pi]], dtype=np.float32)
-            self.is_standing = False
-        else:
-            phase_tp1 = self.phase + self.phase_dt
-            self.phase = np.fmod(phase_tp1 + np.pi, 2 * np.pi) - np.pi
-
-        sin_phase = np.sin(self.phase[0, :])
-        cos_phase = np.cos(self.phase[0, :])
-
-        # Build observation (format depends on DOF)
-        if self.is_23dof:
-            # 82D: [23 actions, 3 ang_vel, 1 cmd_yaw, 2 cmd_lin, 2 cos, 23 pos, 23 vel, 3 grav, 2 sin]
-            self.locomotion_obs[0:23] = self.last_unscaled_action
-            self.locomotion_obs[23:26] = ang_vel_scaled
-            self.locomotion_obs[26] = self.locomotion_cmd[2]
-            self.locomotion_obs[27:29] = self.locomotion_cmd[:2]
-            self.locomotion_obs[29:31] = cos_phase
-            self.locomotion_obs[31:54] = qj_obs
-            self.locomotion_obs[54:77] = dqj_obs
-            self.locomotion_obs[77:80] = gravity_orientation
-            self.locomotion_obs[80:82] = sin_phase
-        else:
-            # 100D: [29 actions, 3 ang_vel, 1 cmd_yaw, 2 cmd_lin, 2 cos, 29 pos, 29 vel, 3 grav, 2 sin]
-            self.locomotion_obs[0:29] = self.last_unscaled_action
-            self.locomotion_obs[29:32] = ang_vel_scaled
-            self.locomotion_obs[32] = self.locomotion_cmd[2]
-            self.locomotion_obs[33:35] = self.locomotion_cmd[:2]
-            self.locomotion_obs[35:37] = cos_phase
-            self.locomotion_obs[37:66] = qj_obs
-            self.locomotion_obs[66:95] = dqj_obs
-            self.locomotion_obs[95:98] = gravity_orientation
-            self.locomotion_obs[98:100] = sin_phase
-
-        # Policy inference
-        obs_input = self.locomotion_obs.reshape(1, -1).astype(np.float32)
-        ort_inputs = {self.policy.get_inputs()[0].name: obs_input}
-        ort_outs = self.policy.run(None, ort_inputs)
-
-        raw_action = ort_outs[0].squeeze()
-        clipped_action = np.clip(raw_action, -100.0, 100.0)
-
-        self.last_unscaled_action = clipped_action.copy()
-        self.locomotion_action = clipped_action * LOCOMOTION_ACTION_SCALE
-
-        # Debug
-        if self.counter <= 3:
-            print(f"\n[Holosoma Debug #{self.counter}]")
-            print(f"  Phase: ({self.phase[0, 0]:.3f}, {self.phase[0, 1]:.3f})")
-            print(f"  Cmd: ({self.locomotion_cmd[0]:.2f}, {self.locomotion_cmd[1]:.2f}, {self.locomotion_cmd[2]:.2f})")
-            print(f"  Action range: [{raw_action.min():.3f}, {raw_action.max():.3f}]")
-
-        # Compute target positions
-        target_dof_pos = self.default_angles + self.locomotion_action
-
-        # Send commands to motors via motor map
-        for i in range(self.num_dof):
-            motor_idx = self.motor_map[i]
-            self.robot.msg.motor_cmd[motor_idx].q = target_dof_pos[i]
-            self.robot.msg.motor_cmd[motor_idx].qd = 0
-            self.robot.msg.motor_cmd[motor_idx].kp = self.kp[i]
-            self.robot.msg.motor_cmd[motor_idx].kd = self.kd[i]
-            self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-        # For 23-DOF: zero out missing joints (waist_roll/pitch, wrist_pitch/yaw)
-        if self.is_23dof:
-            missing_motors = [13, 14, 20, 21, 27, 28]  # waist_roll, waist_pitch, wrist_pitch/yaw
-            for motor_idx in missing_motors:
-                self.robot.msg.motor_cmd[motor_idx].q = 0.0
-                self.robot.msg.motor_cmd[motor_idx].qd = 0
-                self.robot.msg.motor_cmd[motor_idx].kp = 40.0
-                self.robot.msg.motor_cmd[motor_idx].kd = 2.0
-                self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-        self.robot.send_action(self.robot.msg)
-
-    def _locomotion_thread_loop(self):
-        logger.info("Locomotion thread started")
-        while self.locomotion_running:
-            start_time = time.time()
-            try:
-                self.holosoma_locomotion_run()
-            except Exception as e:
-                logger.error(f"Error in locomotion loop: {e}")
-                import traceback
-                traceback.print_exc()
-
-            elapsed = time.time() - start_time
-            sleep_time = max(0, LOCOMOTION_CONTROL_DT - elapsed)
-            time.sleep(sleep_time)
-        logger.info("Locomotion thread stopped")
-
-    def start_locomotion_thread(self):
-        if self.locomotion_running:
-            logger.warning("Locomotion thread already running")
-            return
-        logger.info("Starting locomotion control thread...")
-        self.locomotion_running = True
-        self.locomotion_thread = threading.Thread(target=self._locomotion_thread_loop, daemon=True)
-        self.locomotion_thread.start()
-        logger.info("Locomotion control thread started!")
-
-    def stop_locomotion_thread(self):
-        if not self.locomotion_running:
-            return
-        logger.info("Stopping locomotion control thread...")
-        self.locomotion_running = False
-        if self.locomotion_thread:
-            self.locomotion_thread.join(timeout=2.0)
-        logger.info("Locomotion control thread stopped")
-
-    def reset_robot(self):
-        """Move joints to default position."""
-        logger.info(f"Moving {self.num_dof} joints to default position...")
-
-        total_time = 3.0
-        num_step = int(total_time / self.robot.control_dt)
-
-        robot_state = self.robot.get_observation()
-
-        # Record current positions
-        init_dof_pos = np.zeros(self.num_dof, dtype=np.float32)
-        for i in range(self.num_dof):
-            motor_idx = self.motor_map[i]
-            init_dof_pos[i] = robot_state.motor_state[motor_idx].q
-
-        # Interpolate to target
-        for step in range(num_step):
-            alpha = step / num_step
-            for i in range(self.num_dof):
-                motor_idx = self.motor_map[i]
-                target = self.default_angles[i]
-                self.robot.msg.motor_cmd[motor_idx].q = init_dof_pos[i] * (1 - alpha) + target * alpha
-                self.robot.msg.motor_cmd[motor_idx].qd = 0
-                self.robot.msg.motor_cmd[motor_idx].kp = self.kp[i]
-                self.robot.msg.motor_cmd[motor_idx].kd = self.kd[i]
-                self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-            # Zero missing joints for 23-DOF
-            if self.is_23dof:
-                for motor_idx in [13, 14, 20, 21, 27, 28]:
-                    self.robot.msg.motor_cmd[motor_idx].q = 0.0
-                    self.robot.msg.motor_cmd[motor_idx].qd = 0
-                    self.robot.msg.motor_cmd[motor_idx].kp = 40.0
-                    self.robot.msg.motor_cmd[motor_idx].kd = 2.0
-                    self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-            self.robot.msg.crc = self.robot.crc.Crc(self.robot.msg)
-            self.robot.lowcmd_publisher.Write(self.robot.msg)
-            time.sleep(self.robot.control_dt)
-
-        logger.info(f"Reached default position ({self.num_dof} joints)")
-
-        # Hold for 2 seconds
-        logger.info("Holding default position for 2 seconds...")
-        hold_steps = int(2.0 / self.robot.control_dt)
-        for _ in range(hold_steps):
-            for i in range(self.num_dof):
-                motor_idx = self.motor_map[i]
-                self.robot.msg.motor_cmd[motor_idx].q = self.default_angles[i]
-                self.robot.msg.motor_cmd[motor_idx].qd = 0
-                self.robot.msg.motor_cmd[motor_idx].kp = self.kp[i]
-                self.robot.msg.motor_cmd[motor_idx].kd = self.kd[i]
-                self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-            if self.is_23dof:
-                for motor_idx in [13, 14, 20, 21, 27, 28]:
-                    self.robot.msg.motor_cmd[motor_idx].q = 0.0
-                    self.robot.msg.motor_cmd[motor_idx].qd = 0
-                    self.robot.msg.motor_cmd[motor_idx].kp = 40.0
-                    self.robot.msg.motor_cmd[motor_idx].kd = 2.0
-                    self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-            self.robot.msg.crc = self.robot.crc.Crc(self.robot.msg)
-            self.robot.lowcmd_publisher.Write(self.robot.msg)
-            time.sleep(self.robot.control_dt)
-
-        logger.info("Ready to start locomotion!")
-
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser(description="Holosoma Locomotion Controller for Unitree G1")
-    parser.add_argument("--repo-id", type=str, default=DEFAULT_HOLOSOMA_REPO_ID)
-    parser.add_argument("--policy", type=str, default="fastsac", choices=["fastsac", "ppo"])
-    parser.add_argument("--local-path", type=str, default=None, help="Path to local ONNX file")
-    args = parser.parse_args()
-
-    # Load policy and detect dimensions
-    policy, obs_dim = load_holosoma_policy(
-        repo_id=args.repo_id,
-        policy_name=args.policy,
-        local_path=args.local_path,
-    )
-
-    # Initialize robot
-    config = UnitreeG1Config()
-    robot = UnitreeG1(config)
-
-    # Initialize controller with detected obs_dim
-    controller = HolosomaLocomotionController(
-        policy=policy,
-        robot=robot,
-        config=config,
-        obs_dim=obs_dim,
-    )
-
-    try:
-        controller.reset_robot()
-        controller.start_locomotion_thread()
-
-        logger.info(f"Robot initialized with Holosoma {'23-DOF' if obs_dim == 82 else '29-DOF'} policy")
-        logger.info("Use remote controller: LY=fwd/back, LX=left/right, RX=rotate")
-        logger.info("Press Ctrl+C to stop")
-
-        while True:
-            time.sleep(1.0)
-    except KeyboardInterrupt:
-        print("\nStopping locomotion...")
-        controller.stop_locomotion_thread()
-        print("Done!")

examples/unitree_g1/unitree_rl_locomotion.py

@@ -1,447 +0,0 @@
-#!/usr/bin/env python
-
-# Copyright 2025 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.
-"""
-Example: Unitree RL 12-DOF Legs-Only Locomotion (TorchScript)
-
-This example demonstrates loading a 12-DOF legs-only locomotion policy
-(TorchScript .pt format) and running it on the Unitree G1 robot.
-
-Key characteristics:
-- Single TorchScript policy (.pt)
-- 47D observations, 12D actions (legs only)
-- Phase-based gait timing
-- Arms and waist held at fixed positions
-"""
-
-import argparse
-import logging
-import threading
-import time
-
-import numpy as np
-import torch
-from huggingface_hub import hf_hub_download
-from scipy.spatial.transform import Rotation as R
-
-from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
-from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1
-
-logging.basicConfig(level=logging.INFO)
-logger = logging.getLogger(__name__)
-
-# 12-DOF leg joint configuration
-# Joint order: [L_hip_pitch, L_hip_roll, L_hip_yaw, L_knee, L_ankle_pitch, L_ankle_roll,
-#               R_hip_pitch, R_hip_roll, R_hip_yaw, R_knee, R_ankle_pitch, R_ankle_roll]
-LEG_JOINT_INDICES = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
-
-# Default leg angles for standing
-DEFAULT_LEG_ANGLES = np.array([
-    -0.1, 0.0, 0.0, 0.3, -0.2, 0.0,   # left leg
-    -0.1, 0.0, 0.0, 0.3, -0.2, 0.0,   # right leg
-], dtype=np.float32)
-
-# KP/KD for leg joints
-LEG_KPS = np.array([150, 150, 150, 300, 40, 40, 150, 150, 150, 300, 40, 40], dtype=np.float32)
-LEG_KDS = np.array([6, 6, 6, 4, 2, 2, 6, 6, 6, 4, 2, 2], dtype=np.float32)
-
-# Waist configuration (held at zero)
-WAIST_JOINT_INDICES = [12, 13, 14]  # yaw, roll, pitch
-WAIST_KPS = np.array([250, 250, 250], dtype=np.float32)
-WAIST_KDS = np.array([5, 5, 5], dtype=np.float32)
-
-# Arm configuration (indices 15-28, held at initial position)
-ARM_JOINT_INDICES = list(range(15, 29))
-ARM_KPS = np.array([80, 80, 80, 80, 40, 40, 40,   # left arm (shoulder + wrist)
-                   80, 80, 80, 80, 40, 40, 40], dtype=np.float32)  # right arm
-ARM_KDS = np.array([3, 3, 3, 3, 1.5, 1.5, 1.5,
-                   3, 3, 3, 3, 1.5, 1.5, 1.5], dtype=np.float32)
-
-# Control parameters
-LOCOMOTION_CONTROL_DT = 0.02  # 50Hz control rate
-LOCOMOTION_ACTION_SCALE = 0.25
-ANG_VEL_SCALE = 0.25
-DOF_POS_SCALE = 1.0
-DOF_VEL_SCALE = 0.05
-CMD_SCALE = np.array([2.0, 2.0, 0.25], dtype=np.float32)
-MAX_CMD = np.array([0.8, 0.5, 1.57], dtype=np.float32)  # max vx, vy, yaw_rate
-
-# Gait parameters
-GAIT_PERIOD = 0.8  # seconds
-
-DEFAULT_REPO_ID = "nepyope/unitree_rl_locomotion"
-
-
-def load_torchscript_policy(
-    repo_id: str = DEFAULT_REPO_ID,
-    filename: str = "motion.pt",
-) -> torch.jit.ScriptModule:
-    """Load TorchScript locomotion policy from Hugging Face Hub.
-
-    Args:
-        repo_id: Hugging Face Hub repository ID containing the policy.
-        filename: Policy filename (default: motion.pt).
-    """
-    logger.info(f"Loading TorchScript policy from Hugging Face Hub ({repo_id}/{filename})...")
-
-    policy_path = hf_hub_download(
-        repo_id=repo_id,
-        filename=filename,
-    )
-
-    policy = torch.jit.load(policy_path)
-    policy.eval()
-
-    logger.info("TorchScript policy loaded successfully")
-
-    return policy
-
-
-class UnitreeRLLocomotionController:
-    """
-    Handles 12-DOF legs-only locomotion control for the Unitree G1 robot.
-
-    This controller manages:
-    - Single TorchScript policy
-    - 47D observations (single frame)
-    - 12D action output (legs only)
-    - Arms and waist held at fixed positions
-    - Phase-based gait timing
-    """
-
-    def __init__(self, policy, robot, config):
-        self.policy = policy
-        self.robot = robot
-        self.config = config
-
-        # Velocity commands (vx, vy, yaw_rate)
-        self.locomotion_cmd = np.array([0.0, 0.0, 0.0], dtype=np.float32)
-
-        # State variables (12 DOF legs)
-        self.qj = np.zeros(12, dtype=np.float32)
-        self.dqj = np.zeros(12, dtype=np.float32)
-        self.locomotion_action = np.zeros(12, dtype=np.float32)
-        self.locomotion_obs = np.zeros(47, dtype=np.float32)
-
-        # Initial arm positions (captured on reset)
-        self.initial_arm_positions = np.zeros(14, dtype=np.float32)
-
-        # Counter for phase calculation
-        self.counter = 0
-
-        # Thread management
-        self.locomotion_running = False
-        self.locomotion_thread = None
-
-        logger.info("UnitreeRLLocomotionController initialized")
-        logger.info("  Observation dim: 47, Action dim: 12 (legs only)")
-
-    def locomotion_run(self):
-        """12-DOF legs-only locomotion policy loop."""
-        self.counter += 1
-
-        if self.counter == 1:
-            print("\n" + "=" * 60)
-            print("🚀 RUNNING UNITREE RL 12-DOF LOCOMOTION POLICY")
-            print("   47D observations → 12D actions (legs only)")
-            print("   Arms and waist held at fixed positions")
-            print("=" * 60 + "\n")
-
-        # Get current observation
-        robot_state = self.robot.get_observation()
-        if robot_state is None:
-            return
-
-        # Get command from remote controller
-        if robot_state.wireless_remote is not None:
-            self.robot.remote_controller.set(robot_state.wireless_remote)
-        else:
-            self.robot.remote_controller.lx = 0.0
-            self.robot.remote_controller.ly = 0.0
-            self.robot.remote_controller.rx = 0.0
-            self.robot.remote_controller.ry = 0.0
-
-        self.locomotion_cmd[0] = self.robot.remote_controller.ly       # forward/backward
-        self.locomotion_cmd[1] = self.robot.remote_controller.lx * -1  # left/right (inverted)
-        self.locomotion_cmd[2] = self.robot.remote_controller.rx * -1  # yaw (inverted)
-
-        # Get leg joint positions and velocities (12 DOF)
-        for i, motor_idx in enumerate(LEG_JOINT_INDICES):
-            self.qj[i] = robot_state.motor_state[motor_idx].q
-            self.dqj[i] = robot_state.motor_state[motor_idx].dq
-
-        # Get IMU data
-        quat = robot_state.imu_state.quaternion
-        ang_vel = np.array(robot_state.imu_state.gyroscope, dtype=np.float32)
-
-        # Scale observations
-        gravity_orientation = self.robot.get_gravity_orientation(quat)
-        qj_obs = (self.qj - DEFAULT_LEG_ANGLES) * DOF_POS_SCALE
-        dqj_obs = self.dqj * DOF_VEL_SCALE
-        ang_vel_scaled = ang_vel * ANG_VEL_SCALE
-
-        # Calculate phase
-        count = self.counter * LOCOMOTION_CONTROL_DT
-        phase = (count % GAIT_PERIOD) / GAIT_PERIOD
-        sin_phase = np.sin(2 * np.pi * phase)
-        cos_phase = np.cos(2 * np.pi * phase)
-
-        # Build 47D observation vector
-        # [0:3]   - angular velocity (scaled)
-        # [3:6]   - gravity orientation
-        # [6:9]   - velocity command (scaled)
-        # [9:21]  - joint positions (12D, relative to default)
-        # [21:33] - joint velocities (12D, scaled)
-        # [33:45] - previous actions (12D)
-        # [45]    - sin_phase
-        # [46]    - cos_phase
-        self.locomotion_obs[0:3] = ang_vel_scaled
-        self.locomotion_obs[3:6] = gravity_orientation
-        self.locomotion_obs[6:9] = self.locomotion_cmd * CMD_SCALE * MAX_CMD
-        self.locomotion_obs[9:21] = qj_obs
-        self.locomotion_obs[21:33] = dqj_obs
-        self.locomotion_obs[33:45] = self.locomotion_action
-        self.locomotion_obs[45] = sin_phase
-        self.locomotion_obs[46] = cos_phase
-
-        # Run policy inference (TorchScript)
-        obs_tensor = torch.from_numpy(self.locomotion_obs).unsqueeze(0).float()
-        with torch.no_grad():
-            action_tensor = self.policy(obs_tensor)
-        self.locomotion_action = action_tensor.squeeze().numpy()
-
-        # Transform action to target joint positions
-        target_leg_pos = DEFAULT_LEG_ANGLES + self.locomotion_action * LOCOMOTION_ACTION_SCALE
-
-        # Debug logging (first 3 iterations)
-        if self.counter <= 3:
-            print(f"\n[Unitree RL Debug #{self.counter}]")
-            print(f"  Phase: {phase:.3f} (sin={sin_phase:.3f}, cos={cos_phase:.3f})")
-            print(f"  Cmd (vx, vy, yaw): ({self.locomotion_cmd[0]:.2f}, {self.locomotion_cmd[1]:.2f}, {self.locomotion_cmd[2]:.2f})")
-            print(f"  Action range: [{self.locomotion_action.min():.3f}, {self.locomotion_action.max():.3f}]")
-
-        # Send commands to LEG motors (0-11)
-        for i, motor_idx in enumerate(LEG_JOINT_INDICES):
-            self.robot.msg.motor_cmd[motor_idx].q = target_leg_pos[i]
-            self.robot.msg.motor_cmd[motor_idx].qd = 0
-            self.robot.msg.motor_cmd[motor_idx].kp = LEG_KPS[i]
-            self.robot.msg.motor_cmd[motor_idx].kd = LEG_KDS[i]
-            self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-        # Hold WAIST motors at zero (12, 13, 14)
-        for i, motor_idx in enumerate(WAIST_JOINT_INDICES):
-            self.robot.msg.motor_cmd[motor_idx].q = 0.0
-            self.robot.msg.motor_cmd[motor_idx].qd = 0
-            self.robot.msg.motor_cmd[motor_idx].kp = WAIST_KPS[i]
-            self.robot.msg.motor_cmd[motor_idx].kd = WAIST_KDS[i]
-            self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-        # Hold ARM motors at initial position (15-28)
-        for i, motor_idx in enumerate(ARM_JOINT_INDICES):
-            self.robot.msg.motor_cmd[motor_idx].q = self.initial_arm_positions[i]
-            self.robot.msg.motor_cmd[motor_idx].qd = 0
-            self.robot.msg.motor_cmd[motor_idx].kp = ARM_KPS[i]
-            self.robot.msg.motor_cmd[motor_idx].kd = ARM_KDS[i]
-            self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-        # Send command
-        self.robot.send_action(self.robot.msg)
-
-    def _locomotion_thread_loop(self):
-        """Background thread that runs the locomotion policy at specified rate."""
-        logger.info("Locomotion thread started")
-        while self.locomotion_running:
-            start_time = time.time()
-            try:
-                self.locomotion_run()
-            except Exception as e:
-                logger.error(f"Error in locomotion loop: {e}")
-                import traceback
-                traceback.print_exc()
-
-            # Sleep to maintain control rate
-            elapsed = time.time() - start_time
-            sleep_time = max(0, LOCOMOTION_CONTROL_DT - elapsed)
-            time.sleep(sleep_time)
-        logger.info("Locomotion thread stopped")
-
-    def start_locomotion_thread(self):
-        if self.locomotion_running:
-            logger.warning("Locomotion thread already running")
-            return
-
-        logger.info("Starting locomotion control thread...")
-        self.locomotion_running = True
-        self.locomotion_thread = threading.Thread(target=self._locomotion_thread_loop, daemon=True)
-        self.locomotion_thread.start()
-
-        logger.info("Locomotion control thread started!")
-
-    def stop_locomotion_thread(self):
-        if not self.locomotion_running:
-            return
-
-        logger.info("Stopping locomotion control thread...")
-        self.locomotion_running = False
-        if self.locomotion_thread:
-            self.locomotion_thread.join(timeout=2.0)
-        logger.info("Locomotion control thread stopped")
-
-    def reset_robot(self):
-        """Move legs to default standing position over 2 seconds (arms are captured and held)."""
-        logger.info("Moving legs to default position...")
-
-        total_time = 2.0
-        num_step = int(total_time / self.robot.control_dt)
-
-        # Get current state
-        robot_state = self.robot.get_observation()
-
-        # Capture initial arm positions (to hold during locomotion)
-        for i, motor_idx in enumerate(ARM_JOINT_INDICES):
-            self.initial_arm_positions[i] = robot_state.motor_state[motor_idx].q
-        logger.info(f"Captured initial arm positions: {self.initial_arm_positions[:4]}...")
-
-        # Record current leg positions
-        init_leg_pos = np.zeros(12, dtype=np.float32)
-        for i, motor_idx in enumerate(LEG_JOINT_INDICES):
-            init_leg_pos[i] = robot_state.motor_state[motor_idx].q
-
-        # Interpolate legs to default position
-        for step in range(num_step):
-            alpha = step / num_step
-
-            # Interpolate leg positions
-            for i, motor_idx in enumerate(LEG_JOINT_INDICES):
-                target_pos = DEFAULT_LEG_ANGLES[i]
-                self.robot.msg.motor_cmd[motor_idx].q = (
-                    init_leg_pos[i] * (1 - alpha) + target_pos * alpha
-                )
-                self.robot.msg.motor_cmd[motor_idx].qd = 0
-                self.robot.msg.motor_cmd[motor_idx].kp = LEG_KPS[i]
-                self.robot.msg.motor_cmd[motor_idx].kd = LEG_KDS[i]
-                self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-            # Hold waist at zero
-            for i, motor_idx in enumerate(WAIST_JOINT_INDICES):
-                self.robot.msg.motor_cmd[motor_idx].q = 0.0
-                self.robot.msg.motor_cmd[motor_idx].qd = 0
-                self.robot.msg.motor_cmd[motor_idx].kp = WAIST_KPS[i]
-                self.robot.msg.motor_cmd[motor_idx].kd = WAIST_KDS[i]
-                self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-            # Hold arms at initial position
-            for i, motor_idx in enumerate(ARM_JOINT_INDICES):
-                self.robot.msg.motor_cmd[motor_idx].q = self.initial_arm_positions[i]
-                self.robot.msg.motor_cmd[motor_idx].qd = 0
-                self.robot.msg.motor_cmd[motor_idx].kp = ARM_KPS[i]
-                self.robot.msg.motor_cmd[motor_idx].kd = ARM_KDS[i]
-                self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-            self.robot.msg.crc = self.robot.crc.Crc(self.robot.msg)
-            self.robot.lowcmd_publisher.Write(self.robot.msg)
-            time.sleep(self.robot.control_dt)
-
-        logger.info("Reached default leg position")
-
-        # Hold position for 2 seconds
-        logger.info("Holding default position for 2 seconds...")
-        hold_time = 2.0
-        num_hold_steps = int(hold_time / self.robot.control_dt)
-
-        for _ in range(num_hold_steps):
-            # Hold legs at default
-            for i, motor_idx in enumerate(LEG_JOINT_INDICES):
-                self.robot.msg.motor_cmd[motor_idx].q = DEFAULT_LEG_ANGLES[i]
-                self.robot.msg.motor_cmd[motor_idx].qd = 0
-                self.robot.msg.motor_cmd[motor_idx].kp = LEG_KPS[i]
-                self.robot.msg.motor_cmd[motor_idx].kd = LEG_KDS[i]
-                self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-            # Hold waist at zero
-            for i, motor_idx in enumerate(WAIST_JOINT_INDICES):
-                self.robot.msg.motor_cmd[motor_idx].q = 0.0
-                self.robot.msg.motor_cmd[motor_idx].qd = 0
-                self.robot.msg.motor_cmd[motor_idx].kp = WAIST_KPS[i]
-                self.robot.msg.motor_cmd[motor_idx].kd = WAIST_KDS[i]
-                self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-            # Hold arms at initial position
-            for i, motor_idx in enumerate(ARM_JOINT_INDICES):
-                self.robot.msg.motor_cmd[motor_idx].q = self.initial_arm_positions[i]
-                self.robot.msg.motor_cmd[motor_idx].qd = 0
-                self.robot.msg.motor_cmd[motor_idx].kp = ARM_KPS[i]
-                self.robot.msg.motor_cmd[motor_idx].kd = ARM_KDS[i]
-                self.robot.msg.motor_cmd[motor_idx].tau = 0
-
-            self.robot.msg.crc = self.robot.crc.Crc(self.robot.msg)
-            self.robot.lowcmd_publisher.Write(self.robot.msg)
-            time.sleep(self.robot.control_dt)
-
-        logger.info("Ready to start locomotion!")
-
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser(description="Unitree RL 12-DOF Locomotion Controller for Unitree G1")
-    parser.add_argument(
-        "--repo-id",
-        type=str,
-        default=DEFAULT_REPO_ID,
-        help=f"Hugging Face Hub repo ID for policy (default: {DEFAULT_REPO_ID})",
-    )
-    parser.add_argument(
-        "--filename",
-        type=str,
-        default="motion.pt",
-        help="Policy filename (default: motion.pt)",
-    )
-    args = parser.parse_args()
-
-    # Load policy
-    policy = load_torchscript_policy(repo_id=args.repo_id, filename=args.filename)
-
-    # Initialize robot
-    config = UnitreeG1Config()
-    robot = UnitreeG1(config)
-
-    # Initialize locomotion controller
-    locomotion_controller = UnitreeRLLocomotionController(
-        policy=policy,
-        robot=robot,
-        config=config,
-    )
-
-    # Reset robot and start locomotion thread
-    try:
-        locomotion_controller.reset_robot()
-        locomotion_controller.start_locomotion_thread()
-
-        # Log status
-        logger.info("Robot initialized with Unitree RL locomotion policy")
-        logger.info("Locomotion controller running in background thread")
-        logger.info("Use remote controller to command velocity:")
-        logger.info("  Left stick Y: forward/backward")
-        logger.info("  Left stick X: left/right")
-        logger.info("  Right stick X: rotate")
-        logger.info("Press Ctrl+C to stop")
-
-        # Keep robot alive
-        while True:
-            time.sleep(1.0)
-    except KeyboardInterrupt:
-        print("\nStopping locomotion...")
-        locomotion_controller.stop_locomotion_thread()
-        print("Done!")
-

pyproject.toml

@@ -25,7 +25,7 @@ discord = "https://discord.gg/s3KuuzsPFb"
 
 [project]
 name = "lerobot"
-version = "0.4.3"
+version = "0.4.2"
 description = "🤗 LeRobot: State-of-the-art Machine Learning for Real-World Robotics in Pytorch"
 readme = "README.md"
 license = { text = "Apache-2.0" }
@@ -109,7 +109,7 @@ hopejr = ["lerobot[feetech]", "lerobot[pygame-dep]"]
 lekiwi = ["lerobot[feetech]", "pyzmq>=26.2.1,<28.0.0"]
 unitree_g1 = [
     "pyzmq>=26.2.1,<28.0.0",
-    "onnxruntime>=1.16.0"
+    "unitree_sdk2py @ git+https://github.com/unitreerobotics/unitree_sdk2_python.git",
 ]
 reachy2 = ["reachy2_sdk>=1.0.14,<1.1.0"]
 kinematics = ["lerobot[placo-dep]"]

src/lerobot/policies/pi05/modeling_pi05.py

@@ -538,8 +538,6 @@ class PI05Pytorch(nn.Module):  # see openpi `PI0Pytorch`
         if config.compile_model:
             torch.set_float32_matmul_precision("high")
             self.sample_actions = torch.compile(self.sample_actions, mode=config.compile_mode)
-            # Also compile the main forward pass used during training
-            self.forward = torch.compile(self.forward, mode=config.compile_mode)
 
         msg = """An incorrect transformer version is used, please create an issue on https://github.com/huggingface/lerobot/issues"""
 

src/lerobot/rl/actor.py

@@ -78,7 +78,7 @@ from lerobot.transport.utils import (
     transitions_to_bytes,
 )
 from lerobot.utils.random_utils import set_seed
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.transition import (
     Transition,
     move_state_dict_to_device,
@@ -398,7 +398,7 @@ def act_with_policy(
 
         if cfg.env.fps is not None:
             dt_time = time.perf_counter() - start_time
-            precise_sleep(1 / cfg.env.fps - dt_time)
+            busy_wait(1 / cfg.env.fps - dt_time)
 
 
 #  Communication Functions - Group all gRPC/messaging functions

src/lerobot/rl/gym_manipulator.py

@@ -74,7 +74,7 @@ from lerobot.teleoperators import (
 from lerobot.teleoperators.teleoperator import Teleoperator
 from lerobot.teleoperators.utils import TeleopEvents
 from lerobot.utils.constants import ACTION, DONE, OBS_IMAGES, OBS_STATE, REWARD
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import log_say
 
 logging.basicConfig(level=logging.INFO)
@@ -114,7 +114,7 @@ def reset_follower_position(robot_arm: Robot, target_position: np.ndarray) -> No
     for pose in trajectory:
         action_dict = dict(zip(current_position_dict, pose, strict=False))
         robot_arm.bus.sync_write("Goal_Position", action_dict)
-        precise_sleep(0.015)
+        busy_wait(0.015)
 
 
 class RobotEnv(gym.Env):
@@ -238,7 +238,7 @@ class RobotEnv(gym.Env):
             reset_follower_position(self.robot, np.array(self.reset_pose))
             log_say("Reset the environment done.", play_sounds=True)
 
-        precise_sleep(self.reset_time_s - (time.perf_counter() - start_time))
+        busy_wait(self.reset_time_s - (time.perf_counter() - start_time))
 
         super().reset(seed=seed, options=options)
 
@@ -713,7 +713,7 @@ def control_loop(
             transition = env_processor(transition)
 
         # Maintain fps timing
-        precise_sleep(dt - (time.perf_counter() - step_start_time))
+        busy_wait(dt - (time.perf_counter() - step_start_time))
 
     if dataset is not None and cfg.dataset.push_to_hub:
         logging.info("Pushing dataset to hub")
@@ -745,7 +745,7 @@ def replay_trajectory(
         )
         transition = action_processor(transition)
         env.step(transition[TransitionKey.ACTION])
-        precise_sleep(1 / cfg.env.fps - (time.perf_counter() - start_time))
+        busy_wait(1 / cfg.env.fps - (time.perf_counter() - start_time))
 
 
 @parser.wrap()

src/lerobot/robots/unitree_g1/config_unitree_g1.py

@@ -18,38 +18,93 @@ from dataclasses import dataclass, field
 
 from ..config import RobotConfig
 
-_GAINS: dict[str, dict[str, list[float]]] = {
-    "left_leg": {
-        "kp": [150, 150, 150, 300, 40, 40],
-        "kd": [2, 2, 2, 4, 2, 2],
-    },  # pitch, roll, yaw, knee, ankle_pitch, ankle_roll
-    "right_leg": {"kp": [150, 150, 150, 300, 40, 40], "kd": [2, 2, 2, 4, 2, 2]},
-    "waist": {"kp": [250, 250, 250], "kd": [5, 5, 5]},  # yaw, roll, pitch
-    "left_arm": {"kp": [80, 80, 80, 80], "kd": [3, 3, 3, 3]},  # shoulder_pitch/roll/yaw, elbow
-    "left_wrist": {"kp": [40, 40, 40], "kd": [1.5, 1.5, 1.5]},  # roll, pitch, yaw
-    "right_arm": {"kp": [80, 80, 80, 80], "kd": [3, 3, 3, 3]},
-    "right_wrist": {"kp": [40, 40, 40], "kd": [1.5, 1.5, 1.5]},
-    "other": {"kp": [80, 80, 80, 80, 80, 80], "kd": [3, 3, 3, 3, 3, 3]},
-}
-
-
-def _build_gains() -> tuple[list[float], list[float]]:
-    """Build kp and kd lists from body-part groupings."""
-    kp = [v for g in _GAINS.values() for v in g["kp"]]
-    kd = [v for g in _GAINS.values() for v in g["kd"]]
-    return kp, kd
-
-
-_DEFAULT_KP, _DEFAULT_KD = _build_gains()
-
 
 @RobotConfig.register_subclass("unitree_g1")
 @dataclass
 class UnitreeG1Config(RobotConfig):
-    kp: list[float] = field(default_factory=lambda: _DEFAULT_KP.copy())
-    kd: list[float] = field(default_factory=lambda: _DEFAULT_KD.copy())
+    # id: str = "unitree_g1"
 
-    control_dt: float = 1.0 / 250.0  # 250Hz
+    kp: list = field(
+        default_factory=lambda: [
+            150,
+            150,
+            150,
+            300,
+            40,
+            40,  # Left leg pitch, roll, yaw, knee, ankle pitch, ankle roll
+            150,
+            150,
+            150,
+            300,
+            40,
+            40,  # Right leg pitch, roll, yaw, knee, ankle pitch, ankle roll
+            250,
+            250,
+            250,  # Waist yaw, roll, pitch
+            80,
+            80,
+            80,
+            80,  # Left shoulder pitch, roll, yaw, elbow (kp_low)
+            40,
+            40,
+            40,  # Left wrist roll, pitch, yaw (kp_wrist)
+            80,
+            80,
+            80,
+            80,  # Right shoulder pitch, roll, yaw, elbow (kp_low)
+            40,
+            40,
+            40,  # Right wrist roll, pitch, yaw (kp_wrist)
+            80,
+            80,
+            80,
+            80,
+            80,
+            80,  # Other
+        ]
+    )
+
+    kd: list = field(
+        default_factory=lambda: [
+            2,
+            2,
+            2,
+            4,
+            2,
+            2,  # Left leg pitch, roll, yaw, knee, ankle pitch, ankle roll
+            2,
+            2,
+            2,
+            4,
+            2,
+            2,  # Right leg pitch, roll, yaw, knee, ankle pitch, ankle roll
+            5,
+            5,
+            5,  # Waist yaw, roll, pitch
+            3,
+            3,
+            3,
+            3,  # Left shoulder pitch, roll, yaw, elbow (kd_low)
+            1.5,
+            1.5,
+            1.5,  # Left wrist roll, pitch, yaw (kd_wrist)
+            3,
+            3,
+            3,
+            3,  # Right shoulder pitch, roll, yaw, elbow (kd_low)
+            1.5,
+            1.5,
+            1.5,  # Right wrist roll, pitch, yaw (kd_wrist)
+            3,
+            3,
+            3,
+            3,
+            3,
+            3,  # Other
+        ]
+    )
+
+    control_dt = 1.0 / 250.0  # 250Hz
 
     # socket config for ZMQ bridge
     robot_ip: str = "172.18.129.215"

src/lerobot/robots/unitree_g1/g1_utils.py

@@ -1,19 +1,3 @@
-#!/usr/bin/env python
-
-# Copyright 2025 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 enum import IntEnum
 
 # ruff: noqa: N801, N815

src/lerobot/robots/unitree_g1/run_g1_server.py

@@ -99,12 +99,11 @@ def state_forward_loop(
     lowstate_sub: ChannelSubscriber,
     lowstate_sock: zmq.Socket,
     state_period: float,
-    shutdown_event: threading.Event,
 ) -> None:
     """Read observation from DDS and forward to ZMQ clients."""
     last_state_time = 0.0
 
-    while not shutdown_event.is_set():
+    while True:
         # read from DDS
         msg = lowstate_sub.Read()
         if msg is None:
@@ -129,10 +128,7 @@ def cmd_forward_loop(
 ) -> None:
     """Receive commands from ZMQ and forward to DDS."""
     while True:
-        try:
-            payload = lowcmd_sock.recv()
-        except zmq.ContextTerminated:
-            break
+        payload = lowcmd_sock.recv()
         msg_dict = json.loads(payload.decode("utf-8"))
 
         topic = msg_dict.get("topic", "")
@@ -186,26 +182,30 @@ def main() -> None:
     lowstate_sock.bind(f"tcp://0.0.0.0:{LOWSTATE_PORT}")
 
     state_period = 0.002  # ~500 hz
-    shutdown_event = threading.Event()
 
-    # start observation forwarding in background thread
+    # start observation forwarding thread
     t_state = threading.Thread(
         target=state_forward_loop,
-        args=(lowstate_sub, lowstate_sock, state_period, shutdown_event),
+        args=(lowstate_sub, lowstate_sock, state_period),
+        daemon=True,
     )
     t_state.start()
 
-    print("bridge running (lowstate -> zmq, lowcmd -> dds)")
+    # start action forwarding thread
+    t_cmd = threading.Thread(
+        target=cmd_forward_loop,
+        args=(lowcmd_sock, lowcmd_pub_debug, crc),
+        daemon=True,
+    )
+    t_cmd.start()
 
-    # run command forwarding in main thread
+    print("bridge running (lowstate -> zmq, lowcmd -> dds)")
+    # keep main thread alive so daemon threads don't exit
     try:
-        cmd_forward_loop(lowcmd_sock, lowcmd_pub_debug, crc)
+        while True:
+            time.sleep(1.0)
     except KeyboardInterrupt:
         print("shutting down bridge...")
-    finally:
-        shutdown_event.set()
-        ctx.term()  # terminates blocking zmq.recv() calls
-        t_state.join(timeout=2.0)
 
 
 if __name__ == "__main__":

src/lerobot/robots/unitree_g1/unitree_g1.py

@@ -73,9 +73,7 @@ class IMUState:
 # g1 observation class
 @dataclass
 class G1_29_LowState:  # noqa: N801
-    motor_state: list[MotorState] = field(
-        default_factory=lambda: [MotorState() for _ in range(G1_29_Num_Motors)]
-    )
+    motor_state: list[MotorState] = field(default_factory=lambda: [MotorState() for _ in range(G1_29_Num_Motors)])
     imu_state: IMUState = field(default_factory=IMUState)
     wireless_remote: Any = None  # Raw wireless remote data
     mode_machine: int = 0  # Robot mode
@@ -138,8 +136,8 @@ class UnitreeG1(Robot):
         self.lowstate_buffer = DataBuffer()
 
         # initialize subscribe thread to read robot state
-        self._shutdown_event = threading.Event()
         self.subscribe_thread = threading.Thread(target=self._subscribe_motor_state)
+        self.subscribe_thread.daemon = True
         self.subscribe_thread.start()
 
         while not self.is_connected:
@@ -149,7 +147,7 @@ class UnitreeG1(Robot):
         self.crc = CRC()
         self.msg = unitree_hg_msg_dds__LowCmd_()
         self.msg.mode_pr = 0
-
+        
         # Wait for first state message to arrive
         lowstate = None
         while lowstate is None:
@@ -174,7 +172,7 @@ class UnitreeG1(Robot):
         self.remote_controller = self.RemoteController()
 
     def _subscribe_motor_state(self):  # polls robot state @ 250Hz
-        while not self._shutdown_event.is_set():
+        while True:
             start_time = time.time()
             msg = self.lowstate_subscriber.Read()
             if msg is not None:
@@ -196,7 +194,7 @@ class UnitreeG1(Robot):
 
                 # Capture wireless remote data
                 lowstate.wireless_remote = msg.wireless_remote
-
+                
                 # Capture mode_machine
                 lowstate.mode_machine = msg.mode_machine
 
@@ -204,7 +202,7 @@ class UnitreeG1(Robot):
 
             current_time = time.time()
             all_t_elapsed = current_time - start_time
-            sleep_time = max(0, (self.control_dt - all_t_elapsed))  # maintain constant control dt
+            sleep_time = max(0, (self.control_dt - all_t_elapsed))  # maintina constant control dt
             time.sleep(sleep_time)
 
     @cached_property
@@ -221,8 +219,7 @@ class UnitreeG1(Robot):
         ChannelFactoryInitialize(0)
 
     def disconnect(self):
-        self._shutdown_event.set()
-        self.subscribe_thread.join(timeout=2.0)
+        pass
 
     def get_observation(self) -> dict[str, Any]:
         return self.lowstate_buffer.get_data()
@@ -252,7 +249,6 @@ class UnitreeG1(Robot):
     def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
         self.msg.crc = self.crc.Crc(action)
         self.lowcmd_publisher.Write(action)
-        return action
 
     def get_gravity_orientation(self, quaternion):  # get gravity orientation from quaternion
         """Get gravity orientation from quaternion."""

src/lerobot/robots/unitree_g1/unitree_sdk2_socket.py

@@ -1,18 +1,12 @@
-#!/usr/bin/env python
+"""
+ZMQ socket wrapper that mimics the Unitree SDK Channel interface.
 
-# Copyright 2025 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.
+This module provides a drop-in replacement for the Unitree SDK's DDS-based
+ChannelPublisher and ChannelSubscriber, using ZMQ sockets instead. This allows
+remote communication with the robot over WiFi via the robot_server bridge.
+
+Uses JSON for secure serialization instead of pickle.
+"""
 
 import base64
 import json

src/lerobot/scripts/lerobot_dataset_viz.py

@@ -65,6 +65,7 @@ import argparse
 import gc
 import logging
 import time
+from collections.abc import Iterator
 from pathlib import Path
 
 import numpy as np
@@ -77,6 +78,19 @@ from lerobot.datasets.lerobot_dataset import LeRobotDataset
 from lerobot.utils.constants import ACTION, DONE, OBS_STATE, REWARD
 
 
+class EpisodeSampler(torch.utils.data.Sampler):
+    def __init__(self, dataset: LeRobotDataset, episode_index: int):
+        from_idx = dataset.meta.episodes["dataset_from_index"][episode_index]
+        to_idx = dataset.meta.episodes["dataset_to_index"][episode_index]
+        self.frame_ids = range(from_idx, to_idx)
+
+    def __iter__(self) -> Iterator:
+        return iter(self.frame_ids)
+
+    def __len__(self) -> int:
+        return len(self.frame_ids)
+
+
 def to_hwc_uint8_numpy(chw_float32_torch: torch.Tensor) -> np.ndarray:
     assert chw_float32_torch.dtype == torch.float32
     assert chw_float32_torch.ndim == 3
@@ -105,10 +119,12 @@ def visualize_dataset(
     repo_id = dataset.repo_id
 
     logging.info("Loading dataloader")
+    episode_sampler = EpisodeSampler(dataset, episode_index)
     dataloader = torch.utils.data.DataLoader(
         dataset,
         num_workers=num_workers,
         batch_size=batch_size,
+        sampler=episode_sampler,
     )
 
     logging.info("Starting Rerun")

src/lerobot/scripts/lerobot_find_joint_limits.py

@@ -50,7 +50,7 @@ from lerobot.teleoperators import (  # noqa: F401
     make_teleoperator_from_config,
     so100_leader,
 )
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 
 
 @dataclass
@@ -114,7 +114,7 @@ def find_joint_and_ee_bounds(cfg: FindJointLimitsConfig):
             print(f"Min joint pos position {np.round(min_pos, 4).tolist()}")
             break
 
-        precise_sleep(0.01)
+        busy_wait(0.01)
 
 
 def main():

src/lerobot/scripts/lerobot_record.py

@@ -119,7 +119,7 @@ from lerobot.utils.control_utils import (
     sanity_check_dataset_robot_compatibility,
 )
 from lerobot.utils.import_utils import register_third_party_devices
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import (
     get_safe_torch_device,
     init_logging,
@@ -364,7 +364,7 @@ def record_loop(
             log_rerun_data(observation=obs_processed, action=action_values)
 
         dt_s = time.perf_counter() - start_loop_t
-        precise_sleep(1 / fps - dt_s)
+        busy_wait(1 / fps - dt_s)
 
         timestamp = time.perf_counter() - start_episode_t
 

src/lerobot/scripts/lerobot_replay.py

@@ -62,7 +62,7 @@ from lerobot.robots import (  # noqa: F401
 )
 from lerobot.utils.constants import ACTION
 from lerobot.utils.import_utils import register_third_party_devices
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import (
     init_logging,
     log_say,
@@ -121,7 +121,7 @@ def replay(cfg: ReplayConfig):
         _ = robot.send_action(processed_action)
 
         dt_s = time.perf_counter() - start_episode_t
-        precise_sleep(1 / dataset.fps - dt_s)
+        busy_wait(1 / dataset.fps - dt_s)
 
     robot.disconnect()
 

src/lerobot/scripts/lerobot_teleoperate.py

@@ -89,7 +89,7 @@ from lerobot.teleoperators import (  # noqa: F401
     so101_leader,
 )
 from lerobot.utils.import_utils import register_third_party_devices
-from lerobot.utils.robot_utils import precise_sleep
+from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import init_logging, move_cursor_up
 from lerobot.utils.visualization_utils import init_rerun, log_rerun_data
 
@@ -170,13 +170,12 @@ def teleop_loop(
             # Display the final robot action that was sent
             for motor, value in robot_action_to_send.items():
                 print(f"{motor:<{display_len}} | {value:>7.2f}")
-            move_cursor_up(len(robot_action_to_send) + 3)
+            move_cursor_up(len(robot_action_to_send) + 5)
 
         dt_s = time.perf_counter() - loop_start
-        precise_sleep(1 / fps - dt_s)
+        busy_wait(1 / fps - dt_s)
         loop_s = time.perf_counter() - loop_start
-        print(f"Teleop loop time: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
-        move_cursor_up(1)
+        print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
 
         if duration is not None and time.perf_counter() - start >= duration:
             return

src/lerobot/utils/robot_utils.py

@@ -16,40 +16,14 @@ import platform
 import time
 
 
-def precise_sleep(seconds: float, spin_threshold: float = 0.010, sleep_margin: float = 0.003):
-    """
-    Wait for `seconds` with better precision than time.sleep alone at the expense of more CPU usage.
-
-    Parameters:
-      - seconds: duration to wait
-      - spin_threshold: if remaining <= spin_threshold -> spin; otherwise sleep (seconds). Default 10ms
-      - sleep_margin: when sleeping leave this much time before deadline to avoid oversleep. Default 3ms
-
-    Note:
-        The default parameters are chosen to prioritize timing accuracy over CPU usage for the common 30 FPS use case.
-    """
-    if seconds <= 0:
-        return
-
-    system = platform.system()
-    # On macOS and Windows the scheduler / sleep granularity can make
-    # short sleeps inaccurate. Instead of burning CPU for the whole
-    # duration, sleep for most of the time and spin for the final few
-    # milliseconds to achieve good accuracy with much lower CPU usage.
-    if system in ("Darwin", "Windows"):
+def busy_wait(seconds):
+    if platform.system() == "Darwin" or platform.system() == "Windows":
+        # On Mac and Windows, `time.sleep` is not accurate and we need to use this while loop trick,
+        # but it consumes CPU cycles.
         end_time = time.perf_counter() + seconds
-        while True:
-            remaining = end_time - time.perf_counter()
-            if remaining <= 0:
-                break
-            # If there's more than a couple milliseconds left, sleep most
-            # of the remaining time and leave a small margin for the final spin.
-            if remaining > spin_threshold:
-                # Sleep but avoid sleeping past the end by leaving a small margin.
-                time.sleep(max(remaining - sleep_margin, 0))
-            else:
-                # Final short spin to hit precise timing without long sleeps.
-                pass
+        while time.perf_counter() < end_time:
+            pass
     else:
-        # On Linux time.sleep is accurate enough for most uses
-        time.sleep(seconds)
+        # On Linux time.sleep is accurate
+        if seconds > 0:
+            time.sleep(seconds)