style: fix ruff SIM105/format lints in unitree_g1 scripts

Use contextlib.suppress(zmq.Again) instead of try/except/pass in the
onboard state publisher and teleop client, plus ruff-format/isort
autofixes across the unitree_g1 helpers and dataset_reader imports.

Co-authored-by: Cursor <cursoragent@cursor.com>

src/lerobot/cameras/opencv/camera_opencv.py

@@ -241,7 +241,12 @@ class OpenCVCamera(Camera):
         actual_fps = self.videocapture.get(cv2.CAP_PROP_FPS)
         # Use math.isclose for robust float comparison
         if not success or not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
-            raise RuntimeError(f"{self} failed to set fps={self.fps} ({actual_fps=}).")
+            # Some cameras only run at a fixed rate (e.g. 90 fps). Rather than abort, adopt the
+            # camera's actual fps; downstream consumers can sample frames at whatever rate they need.
+            logger.warning(
+                f"{self} failed to set fps={self.fps} ({actual_fps=}); using the camera's actual fps."
+            )
+            self.fps = actual_fps
 
     def _validate_fourcc(self) -> None:
         """Validates and sets the camera's FOURCC code."""
@@ -276,17 +281,25 @@ class OpenCVCamera(Camera):
         width_success = self.videocapture.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.capture_width))
         height_success = self.videocapture.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.capture_height))
 
+        # Trust the measured resolution: some fixed-format V4L2 cameras return False from
+        # set() even when the value is already correct. Only fail if the actual value is wrong.
         actual_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH)))
-        if not width_success or self.capture_width != actual_width:
+        if self.capture_width != actual_width:
             raise RuntimeError(
                 f"{self} failed to set capture_width={self.capture_width} ({actual_width=}, {width_success=})."
             )
+        if not width_success:
+            logger.warning(f"{self} set(CAP_PROP_FRAME_WIDTH) returned False but {actual_width=} is correct.")
 
         actual_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_HEIGHT)))
-        if not height_success or self.capture_height != actual_height:
+        if self.capture_height != actual_height:
             raise RuntimeError(
                 f"{self} failed to set capture_height={self.capture_height} ({actual_height=}, {height_success=})."
             )
+        if not height_success:
+            logger.warning(
+                f"{self} set(CAP_PROP_FRAME_HEIGHT) returned False but {actual_height=} is correct."
+            )
 
     @staticmethod
     def find_cameras() -> list[dict[str, Any]]:

src/lerobot/cameras/zmq/image_server.py

@@ -31,7 +31,7 @@ import cv2
 import numpy as np
 import zmq
 
-from ..configs import ColorMode
+from ..configs import ColorMode, Cv2Backends
 from ..opencv import OpenCVCamera, OpenCVCameraConfig
 
 logger = logging.getLogger(__name__)
@@ -92,26 +92,75 @@ class ImageServer:
     def __init__(self, config: dict, port: int = 5555):
         # fps controls the publish loop rate (how often frames are sent over ZMQ), not the camera capture rate
         self.fps = config.get("fps", 30)
+        # First-frame warmup: UVC cameras (Arducam/RealSense) can take >1s to deliver
+        # their first frame, especially with several sharing a USB bus. A tight timeout
+        # aborts the launch and leaves the device wedged (no STREAMOFF), so give it room.
+        self.warmup_s = config.get("warmup_s", 5)
+        # Flaky USB cameras (RealSense especially) intermittently fail the first
+        # open or first-frame read; retry a few times before giving up.
+        self.open_attempts = config.get("open_attempts", 5)
+        self.open_retry_delay_s = config.get("open_retry_delay_s", 2.0)
         self.cameras: dict[str, OpenCVCamera] = {}
         self.capture_threads: dict[str, CameraCaptureThread] = {}
+        self._stop = threading.Event()
 
-        for name, cfg in config.get("cameras", {}).items():
-            shape = cfg.get("shape", [480, 640])
-            cam_config = OpenCVCameraConfig(
-                index_or_path=cfg.get("device_id", 0),
-                fps=self.fps,
-                width=shape[1],
-                height=shape[0],
-                color_mode=ColorMode.RGB,
-            )
-            camera = OpenCVCamera(cam_config)
-            camera.connect()
-            self.cameras[name] = camera
-            logger.info(f"Camera {name}: {shape[1]}x{shape[0]}")
+        # If any camera fails to open, release the ones we already opened so the V4L2
+        # devices get a clean STREAMOFF instead of staying busy until the next reboot.
+        try:
+            for name, cfg in config.get("cameras", {}).items():
+                shape = cfg.get("shape", [480, 640])
+                cam_kwargs = {
+                    "index_or_path": cfg.get("device_id", 0),
+                    "fps": self.fps,
+                    "width": shape[1],
+                    "height": shape[0],
+                    "color_mode": ColorMode.RGB,
+                    "warmup_s": self.warmup_s,
+                    # Force V4L2 (Linux): the default FFMPEG backend is read-only for capture
+                    # props, so it can't set FOURCC/resolution (e.g. RealSense color nodes).
+                    "backend": Cv2Backends.V4L2,
+                }
+                # Some cameras (e.g. RealSense color nodes) won't apply a resolution unless the
+                # pixel format is forced first, so pass a FOURCC through when provided.
+                if cfg.get("fourcc"):
+                    cam_kwargs["fourcc"] = cfg["fourcc"]
+                cam_config = OpenCVCameraConfig(**cam_kwargs)
+                camera = OpenCVCamera(cam_config)
 
-            # Create capture thread for this camera
-            capture_thread = CameraCaptureThread(camera, name)
-            self.capture_threads[name] = capture_thread
+                last_err: Exception | None = None
+                for attempt in range(1, self.open_attempts + 1):
+                    try:
+                        camera.connect()
+                        last_err = None
+                        break
+                    except Exception as e:  # noqa: BLE001
+                        last_err = e
+                        logger.warning(
+                            "Camera %s open attempt %d/%d failed: %s",
+                            name,
+                            attempt,
+                            self.open_attempts,
+                            e,
+                        )
+                        with contextlib.suppress(Exception):
+                            camera.disconnect()
+                        if attempt < self.open_attempts:
+                            time.sleep(self.open_retry_delay_s)
+                if last_err is not None:
+                    raise RuntimeError(
+                        f"Camera {name} failed to open after {self.open_attempts} attempts"
+                    ) from last_err
+
+                self.cameras[name] = camera
+                logger.info(f"Camera {name}: {shape[1]}x{shape[0]}")
+
+                # Create capture thread for this camera
+                capture_thread = CameraCaptureThread(camera, name)
+                self.capture_threads[name] = capture_thread
+        except Exception:
+            logger.exception("Failed to open cameras; releasing any already-opened devices.")
+            self._release_cameras()
+            raise
 
         # ZMQ PUB socket
         self.context = zmq.Context()
@@ -122,10 +171,31 @@ class ImageServer:
 
         logger.info(f"ImageServer running on port {port}")
 
+    def stop(self) -> None:
+        """Signal the publish loop to exit so ``run()`` reaches its cleanup.
+
+        Call this from the owning process on shutdown (e.g. Ctrl-C) — otherwise a
+        daemon thread running ``run()`` is killed abruptly and the cameras never get
+        released, leaving the V4L2 devices wedged until reboot.
+        """
+        self._stop.set()
+
+    def _release_cameras(self) -> None:
+        """Stop capture threads and disconnect all cameras (safe to call twice).
+
+        Ensures each V4L2 device gets a clean STREAMOFF/release so a failed or
+        interrupted run doesn't leave devices busy until the next reboot.
+        """
+        for capture_thread in self.capture_threads.values():
+            with contextlib.suppress(Exception):
+                capture_thread.stop()
+        for cam in self.cameras.values():
+            with contextlib.suppress(Exception):
+                cam.disconnect()
+
     def run(self):
         frame_count = 0
         frame_times = deque(maxlen=60)
-        last_published_ts: dict[str, float] = {}
 
         # Start all capture threads
         for capture_thread in self.capture_threads.values():
@@ -134,26 +204,28 @@ class ImageServer:
         # Wait for first frames to be captured and encoded
         logger.info("Waiting for cameras to start capturing...")
         for name, capture_thread in self.capture_threads.items():
-            while capture_thread.get_latest()[0] is None:
+            while capture_thread.get_latest()[0] is None and not self._stop.is_set():
                 time.sleep(0.01)
             logger.info(f"Camera {name} ready (capture + encode in background)")
 
         try:
-            while True:
+            while not self._stop.is_set():
                 t0 = time.time()
 
-                # Build message
+                # Build message. Always include EVERY camera's latest frame so each message
+                # is complete: clients pick their own stream by name, and a partial message
+                # makes them fall back to another camera's image (cross-feed flicker).
                 message = {"timestamps": {}, "images": {}}
                 for name, capture_thread in self.capture_threads.items():
                     encoded, timestamp = capture_thread.get_latest()
-                    if encoded is not None and timestamp > last_published_ts.get(name, 0.0):
+                    if encoded is not None:
                         message["timestamps"][name] = timestamp
                         message["images"][name] = encoded
-                        last_published_ts[name] = timestamp
 
                 # Send as JSON string (suppress if buffer full)
-                with contextlib.suppress(zmq.Again):
-                    self.socket.send_string(json.dumps(message), zmq.NOBLOCK)
+                if message["images"]:
+                    with contextlib.suppress(zmq.Again):
+                        self.socket.send_string(json.dumps(message), zmq.NOBLOCK)
 
                 frame_count += 1
                 frame_times.append(time.time() - t0)
@@ -168,10 +240,7 @@ class ImageServer:
         except KeyboardInterrupt:
             pass
         finally:
-            for capture_thread in self.capture_threads.values():
-                capture_thread.stop()
-            for cam in self.cameras.values():
-                cam.disconnect()
+            self._release_cameras()
             self.socket.close()
             self.context.term()
 

src/lerobot/datasets/dataset_reader.py

@@ -19,9 +19,9 @@ from collections.abc import Callable
 from concurrent.futures import ThreadPoolExecutor
 from pathlib import Path
 
-import datasets
 import torch
 
+import datasets
 from lerobot.configs import (
     DEFAULT_DEPTH_UNIT,
     DEPTH_METER_UNIT,

src/lerobot/robots/openarm_follower/openarm_follower.py

@@ -150,9 +150,6 @@ class OpenArmFollower(Robot):
 
         self.configure()
 
-        if self.is_calibrated:
-            self.bus.set_zero_position()
-
         self.bus.enable_torque()
 
         logger.info(f"{self} connected.")

src/lerobot/robots/unitree_g1/config_unitree_g1.py

@@ -43,6 +43,23 @@ def _build_gains() -> tuple[list[float], list[float]]:
 
 _DEFAULT_KP, _DEFAULT_KD = _build_gains()
 
+# Rest / soft-stop arm pose. The G1 elbow's mechanical zero sits ~90deg (forearm
+# pointing forward); a positive elbow angle *extends* the arm toward straight (this is
+# why holosoma uses 0.6 for a mildly-extended natural stance). We hang the arms nearly
+# straight down so that on soft-stop they're already down and don't drop as dead weight
+# when the joints go passive. If the arms curl the wrong way on your robot, flip the
+# sign of _REST_ELBOW.
+_LEFT_ELBOW_IDX = 18
+_RIGHT_ELBOW_IDX = 25
+_REST_ELBOW = 1.17  # rad, ~160deg forearm (0 rad~=90deg, 1.5 rad~=180deg straight)
+
+
+def _build_default_positions() -> list[float]:
+    pos = [0.0] * 29
+    pos[_LEFT_ELBOW_IDX] = _REST_ELBOW
+    pos[_RIGHT_ELBOW_IDX] = _REST_ELBOW
+    return pos
+
 
 @RobotConfig.register_subclass("unitree_g1")
 @dataclass
@@ -50,8 +67,8 @@ class UnitreeG1Config(RobotConfig):
     kp: list[float] = field(default_factory=lambda: _DEFAULT_KP.copy())
     kd: list[float] = field(default_factory=lambda: _DEFAULT_KD.copy())
 
-    # Default joint positions
-    default_positions: list[float] = field(default_factory=lambda: [0.0] * 29)
+    # Default joint positions (rest / soft-stop pose; arms hang straight down)
+    default_positions: list[float] = field(default_factory=_build_default_positions)
 
     # Control loop timestep
     control_dt: float = 1.0 / 250.0  # 250Hz
@@ -59,6 +76,17 @@ class UnitreeG1Config(RobotConfig):
     # Launch mujoco simulation
     is_simulation: bool = True
 
+    # Run the locomotion controller ONBOARD the robot (policy on the G1 itself,
+    # against local DDS) instead of on the laptop over the ZMQ bridge. In this mode
+    # the robot object uses the real Unitree SDK channels and expects high-level
+    # actions (arm targets + joystick axes) to be fed in via send_action (e.g. by
+    # run_g1_onboard.py, which receives them from the laptop). Mutually exclusive
+    # with is_simulation.
+    onboard: bool = False
+    # DDS network interface for onboard mode (None = SDK default, matching
+    # run_g1_server.py's ChannelFactoryInitialize(0)).
+    dds_interface: str | None = None
+
     # Socket config for ZMQ bridge
     robot_ip: str = "192.168.123.164"  # default G1 IP
 
@@ -71,3 +99,9 @@ class UnitreeG1Config(RobotConfig):
     # Lower-body controller class name, e.g. "GrootLocomotionController" or
     # "HolosomaLocomotionController". None disables it.
     controller: str | None = None
+
+    # On disconnect, ramp the arms slowly back to `default_positions` (hands down)
+    # before going passive, instead of dropping straight to zero torque. Only
+    # applies on the real robot when a locomotion controller holds the legs.
+    soft_stop: bool = True
+    soft_stop_duration: float = 3.0

src/lerobot/robots/unitree_g1/gr00t_locomotion.py

@@ -15,6 +15,7 @@
 # limitations under the License.
 
 import logging
+import os
 from collections import deque
 
 import numpy as np
@@ -39,10 +40,22 @@ GROOT_DEFAULT_ANGLES[[4, 10]] = -0.2  # Ankle pitch
 # Control parameters
 ACTION_SCALE = 0.25
 CONTROL_DT = 0.02  # 50Hz
-ANG_VEL_SCALE: float = 0.25
+ANG_VEL_SCALE: float = 0.5
 DOF_POS_SCALE: float = 1.0
 DOF_VEL_SCALE: float = 0.05
-CMD_SCALE: list[float] = [2.0, 2.0, 0.25]
+CMD_SCALE: list[float] = [2.0, 2.0, 0.5]
+
+# Waist-height control via the right stick Y axis (the only unmapped axis).
+# Rate control (m per control step at full deflection): a self-centering stick
+# returns to 0 = "hold current height". ~0.2 m/s at 50 Hz.
+HEIGHT_STICK_RATE: float = 0.004
+HEIGHT_MIN: float = 0.50
+HEIGHT_MAX: float = 1.00
+
+# Deadzone applied to all stick axes. Resting sticks drift by ~0.03-0.05, which
+# otherwise keeps cmd_magnitude above the balance/walk threshold and makes the
+# robot march in place instead of standing still on the Balance policy.
+STICK_DEADZONE: float = 0.1
 
 
 DEFAULT_GROOT_REPO_ID = "nepyope/GR00T-WholeBodyControl_g1"
@@ -51,26 +64,41 @@ DEFAULT_GROOT_REPO_ID = "nepyope/GR00T-WholeBodyControl_g1"
 def load_groot_policies(
     repo_id: str = DEFAULT_GROOT_REPO_ID,
 ) -> tuple[ort.InferenceSession, ort.InferenceSession]:
-    """Load GR00T dual-policy system (Balance + Walk) from the hub.
+    """Load GR00T dual-policy system (Balance + Walk).
+
+    If the env var ``LEROBOT_GROOT_POLICY_DIR`` is set, the two ONNX files are
+    loaded from that local directory (e.g. finetuned checkpoints) instead of the
+    Hub. Otherwise they are downloaded from ``repo_id``.
 
     Args:
         repo_id: Hugging Face Hub repository ID containing the ONNX policies.
     """
-    logger.info(f"Loading GR00T dual-policy system from the hub ({repo_id})...")
+    local_dir = os.environ.get("LEROBOT_GROOT_POLICY_DIR")
+    if local_dir:
+        balance_path = os.path.join(local_dir, "GR00T-WholeBodyControl-Balance.onnx")
+        walk_path = os.path.join(local_dir, "GR00T-WholeBodyControl-Walk.onnx")
+        logger.info(f"Loading GR00T dual-policy system from local dir ({local_dir})...")
+    else:
+        logger.info(f"Loading GR00T dual-policy system from the hub ({repo_id})...")
+        balance_path = hf_hub_download(
+            repo_id=repo_id,
+            filename="GR00T-WholeBodyControl-Balance.onnx",
+        )
+        walk_path = hf_hub_download(
+            repo_id=repo_id,
+            filename="GR00T-WholeBodyControl-Walk.onnx",
+        )
 
-    # Download ONNX policies from Hugging Face Hub
-    balance_path = hf_hub_download(
-        repo_id=repo_id,
-        filename="GR00T-WholeBodyControl-Balance.onnx",
-    )
-    walk_path = hf_hub_download(
-        repo_id=repo_id,
-        filename="GR00T-WholeBodyControl-Walk.onnx",
-    )
-
-    # Load ONNX policies
-    policy_balance = ort.InferenceSession(balance_path)
-    policy_walk = ort.InferenceSession(walk_path)
+    # Load ONNX policies. Cap onnxruntime to a single thread: these are tiny MLP
+    # policies run in the real-time control loop, and letting ORT grab one intra-op
+    # thread per core oversubscribes the CPU and starves the teleop/IK loop
+    # (teleop becomes abysmally slow). Sequential + 1 thread is fastest here.
+    so = ort.SessionOptions()
+    so.intra_op_num_threads = 1
+    so.inter_op_num_threads = 1
+    so.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
+    policy_balance = ort.InferenceSession(balance_path, sess_options=so)
+    policy_walk = ort.InferenceSession(walk_path, sess_options=so)
 
     logger.info("GR00T policies loaded successfully")
 
@@ -134,12 +162,19 @@ class GrootLocomotionController:
         buttons = [int(action.get(k, 0)) for k in REMOTE_BUTTONS]
         if buttons[0]:  # R1 - raise waist
             self.groot_height_cmd += 0.001
-            self.groot_height_cmd = np.clip(self.groot_height_cmd, 0.50, 1.00)
+            self.groot_height_cmd = np.clip(self.groot_height_cmd, HEIGHT_MIN, HEIGHT_MAX)
         if buttons[4]:  # R2 - lower waist
             self.groot_height_cmd -= 0.001
-            self.groot_height_cmd = np.clip(self.groot_height_cmd, 0.50, 1.00)
+            self.groot_height_cmd = np.clip(self.groot_height_cmd, HEIGHT_MIN, HEIGHT_MAX)
 
-        lx, ly, rx, _ry = (action.get(k, 0.0) for k in REMOTE_AXES)
+        lx, ly, rx, ry = (action.get(k, 0.0) for k in REMOTE_AXES)
+        # Deadzone every axis so resting-stick drift doesn't leak into commands.
+        lx, ly, rx, ry = (0.0 if abs(v) < STICK_DEADZONE else v for v in (lx, ly, rx, ry))
+        # Right stick Y controls waist height (rate control) — the only otherwise
+        # unmapped axis. Push up = raise, push down = lower, release = hold.
+        if ry != 0.0:
+            self.groot_height_cmd += ry * HEIGHT_STICK_RATE
+            self.groot_height_cmd = np.clip(self.groot_height_cmd, HEIGHT_MIN, HEIGHT_MAX)
         self.cmd[0] = ly  # Forward/backward
         self.cmd[1] = -lx  # Left/right (negated)
         self.cmd[2] = -rx  # Rotation rate (negated)

src/lerobot/robots/unitree_g1/holosoma_locomotion.py

@@ -78,7 +78,14 @@ def load_policy(
     logger.info(f"Loading {policy_type.upper()} policy from: {repo_id}/{filename}")
     policy_path = hf_hub_download(repo_id=repo_id, filename=filename)
 
-    policy = ort.InferenceSession(policy_path)
+    # Cap onnxruntime to a single thread: this is a tiny policy run in the
+    # real-time control loop, and letting ORT grab one intra-op thread per core
+    # oversubscribes the CPU and starves the teleop/IK loop (abysmally slow teleop).
+    so = ort.SessionOptions()
+    so.intra_op_num_threads = 1
+    so.inter_op_num_threads = 1
+    so.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
+    policy = ort.InferenceSession(policy_path, sess_options=so)
     logger.info(f"Policy loaded: {policy.get_inputs()[0].shape} → {policy.get_outputs()[0].shape}")
 
     # Extract KP/KD from ONNX metadata

src/lerobot/robots/unitree_g1/run_g1.sh

@@ -0,0 +1,38 @@
+#!/usr/bin/env bash
+# Launch the G1 ZMQ bridge server with grippers + cameras.
+# Handles conda activation and CAN bring-up so you only run one command on the robot.
+set -euo pipefail
+
+# --- conda -------------------------------------------------------------------
+CONDA_SH="${CONDA_SH:-$HOME/miniforge3/etc/profile.d/conda.sh}"
+if [[ ! -f "$CONDA_SH" ]]; then
+    echo "conda.sh not found at $CONDA_SH (set CONDA_SH=/path/to/conda.sh)" >&2
+    exit 1
+fi
+# shellcheck disable=SC1090
+source "$CONDA_SH"
+conda activate lerobot
+
+# --- CAN bring-up + test -----------------------------------------------------
+CAN_INTERFACES="${CAN_INTERFACES:-can0,can1}"
+echo "==> Setting up CAN interfaces: $CAN_INTERFACES"
+lerobot-setup-can --mode=setup --interfaces="$CAN_INTERFACES"
+
+echo "==> Testing CAN motors on: $CAN_INTERFACES"
+lerobot-setup-can --mode=test --interfaces="$CAN_INTERFACES"
+
+# --- G1 server ---------------------------------------------------------------
+CAMERAS="${CAMERAS:-head_camera:/dev/v4l/by-id/usb-Intel_R__RealSense_TM__Depth_Camera_435i_Intel_R__RealSense_TM__Depth_Camera_435i_254343063964-video-index0:640x480:YUYV,left_wrist:/dev/video2:1280x720:MJPG,right_wrist:/dev/video0:1280x720:MJPG}"
+CAMERA_FPS="${CAMERA_FPS:-30}"
+GRIPPER_PORT_LEFT="${GRIPPER_PORT_LEFT:-can1}"
+GRIPPER_PORT_RIGHT="${GRIPPER_PORT_RIGHT:-can0}"
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+
+echo "==> Starting G1 server"
+exec python "$SCRIPT_DIR/run_g1_server.py" \
+    --grippers \
+    --gripper-port-left "$GRIPPER_PORT_LEFT" \
+    --gripper-port-right "$GRIPPER_PORT_RIGHT" \
+    --camera-fps "$CAMERA_FPS" \
+    --cameras "$CAMERAS"

src/lerobot/robots/unitree_g1/run_g1_onboard.py

@@ -0,0 +1,300 @@
+#!/usr/bin/env python
+
+# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Run the G1 locomotion controller ONBOARD, driven by high-level actions from a laptop.
+
+The locomotion policy (GRoot / Holosoma) runs on the robot itself against local DDS, at
+full control rate. The laptop reads the exos, runs IK, and ships only the resulting
+action (arm joint targets + joystick axes + gripper flags) as JSON over ZMQ. This process
+applies each action via ``UnitreeG1.send_action`` while the onboard controller thread
+keeps the legs balanced.
+
+Pair with ``run_g1_teleop_client.py`` (exo teleop) or ``infer_pi05_g1_onboard.py`` (policy
+eval) on the laptop. Grippers (exo L3/R3 or policy flags) are driven directly over CAN here
+when ``--grippers`` is passed; cameras are served over ZMQ when ``--cameras`` is passed.
+
+Besides receiving actions, this process also publishes ``observation.state`` (29 joint ``.q``)
+on a ZMQ PUB port so the laptop policy client has proprioception, and applies absolute base
+height / torso orientation from the action dict (``groot.height`` / ``groot.rpy.*``) onto the
+controller (the joystick interface can only nudge height, not set it absolutely).
+
+Safety: type ``e`` then Enter in this terminal to stop immediately (skips the soft-stop arm
+ramp: goes straight to zero-torque and exits). Ctrl-C does the normal graceful shutdown.
+
+Examples (on the robot):
+
+    python -m lerobot.robots.unitree_g1.run_g1_onboard --controller GrootLocomotionController
+
+    # with grippers (bring CAN up first, e.g. lerobot-setup-can --mode=setup --interfaces=can0,can1)
+    python -m lerobot.robots.unitree_g1.run_g1_onboard --controller GrootLocomotionController \
+        --grippers --gripper-port-left can1 --gripper-port-right can0
+"""
+
+import argparse
+import contextlib
+import json
+import logging
+import os
+import signal
+import sys
+import threading
+import time
+
+import numpy as np
+import zmq
+
+from lerobot.cameras.zmq.image_server import ImageServer
+from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
+from lerobot.robots.unitree_g1.g1_utils import G1_29_JointIndex
+from lerobot.robots.unitree_g1.run_g1_server import Gripper, build_gripper, parse_camera_specs
+from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1
+
+logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s", force=True)
+logger = logging.getLogger("g1_onboard")
+
+ACTION_PORT = 6004
+STATE_PORT = 6005
+
+
+def main() -> None:
+    p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+    p.add_argument("--controller", default="GrootLocomotionController", help="Locomotion controller class")
+    p.add_argument("--dds-interface", default=None, help="DDS network interface (default: SDK default)")
+    p.add_argument("--action-port", type=int, default=ACTION_PORT, help="ZMQ port for laptop actions")
+    p.add_argument(
+        "--state-port",
+        type=int,
+        default=STATE_PORT,
+        help="ZMQ PUB port for observation.state feedback (29 joint .q) to the inference client",
+    )
+    p.add_argument(
+        "--state-fps",
+        type=float,
+        default=30.0,
+        help="observation.state publish rate (Hz); set <=0 to disable the state PUB entirely",
+    )
+    p.add_argument(
+        "--gravity-compensation",
+        action="store_true",
+        help="Enable arm gravity compensation (needs pinocchio/casadi on the robot)",
+    )
+    # Gripper control from exo L3/R3 (same wiring as run_g1_server.py).
+    p.add_argument("--grippers", action="store_true", help="Drive Damiao grippers from exo L3/R3")
+    p.add_argument("--gripper-port-left", default="can1", help="CAN interface for LEFT gripper")
+    p.add_argument("--gripper-port-right", default="can0", help="CAN interface for RIGHT gripper")
+    p.add_argument("--gripper-send-id", type=lambda x: int(x, 0), default=0x08, help="Motor send CAN id")
+    p.add_argument("--gripper-recv-id", type=lambda x: int(x, 0), default=0x18, help="Motor recv CAN id")
+    p.add_argument("--gripper-motor-type", default="dm4310", help="Damiao motor type")
+    p.add_argument("--gripper-open-deg", type=float, default=-65.0, help="Gripper OPEN position (deg)")
+    p.add_argument("--gripper-close-deg", type=float, default=0.0, help="Gripper CLOSE position (deg)")
+    p.add_argument("--gripper-kp", type=float, default=15.0, help="MIT position gain (stiffness)")
+    p.add_argument("--gripper-kd", type=float, default=0.5, help="MIT damping gain")
+    p.add_argument(
+        "--gripper-no-fd", dest="gripper_fd", action="store_false", help="Classic CAN (non-FD adapter)"
+    )
+    p.set_defaults(gripper_fd=True)
+    # Optional camera streaming (so view_cameras.py can connect). Same spec format as
+    # run_g1_server.py: 'name:device[:WxH[:FOURCC]]' comma-separated.
+    p.add_argument("--cameras", default=None, help="Camera spec 'name:device[:WxH[:FOURCC]]', comma-sep")
+    p.add_argument("--camera-fps", type=int, default=30, help="Camera FPS")
+    p.add_argument("--camera-port", type=int, default=5555, help="Camera ZMQ port")
+    p.add_argument("--camera-width", type=int, default=640, help="Default camera width")
+    p.add_argument("--camera-height", type=int, default=480, help="Default camera height")
+    args = p.parse_args()
+
+    cfg = UnitreeG1Config(
+        is_simulation=False,
+        onboard=True,
+        controller=args.controller,
+        dds_interface=args.dds_interface,
+        gravity_compensation=args.gravity_compensation,
+        cameras={},
+    )
+    # Optional camera server (for view_cameras.py). Runs in a background thread and is
+    # independent of DDS/CAN, so it coexists with the onboard controller and grippers.
+    camera_server = None
+    if args.cameras:
+        cameras = parse_camera_specs(args.cameras, args.camera_width, args.camera_height)
+        camera_server = ImageServer({"fps": args.camera_fps, "cameras": cameras}, port=args.camera_port)
+        threading.Thread(target=camera_server.run, daemon=True).start()
+        cam_summary = ", ".join(f"{name}(dev {c['device_id']})" for name, c in cameras.items())
+        logger.info("Camera server started on :%d: %s", args.camera_port, cam_summary)
+
+    robot = UnitreeG1(cfg)
+    logger.info("Connecting onboard robot (controller=%s)...", args.controller)
+    robot.connect()
+
+    # Grippers: driven directly over CAN from the exo L3/R3 flags in each action
+    # (L3 = remote.button.4 -> left, R3 = remote.button.0 -> right; pressed = close).
+    grippers: dict[str, Gripper] = {}
+    if args.grippers:
+        for side, port in (("L", args.gripper_port_left), ("R", args.gripper_port_right)):
+            grippers[side] = build_gripper(
+                side,
+                port,
+                args.gripper_send_id,
+                args.gripper_recv_id,
+                args.gripper_motor_type,
+                args.gripper_fd,
+                args.gripper_open_deg,
+                args.gripper_close_deg,
+                args.gripper_kp,
+                args.gripper_kd,
+            )
+        logger.info("Grippers enabled: L3 -> left, R3 -> right")
+
+    ctx = zmq.Context.instance()
+    sock = ctx.socket(zmq.PULL)
+    sock.setsockopt(zmq.CONFLATE, 1)  # only ever act on the freshest command
+    sock.setsockopt(zmq.RCVTIMEO, 200)  # keeps the loop responsive to the stop event
+    sock.bind(f"tcp://0.0.0.0:{args.action_port}")
+    logger.info("Onboard controller live. Waiting for laptop actions on :%d ...", args.action_port)
+    logger.info("Type 'e' then Enter to STOP immediately (or Ctrl-C for graceful shutdown).")
+
+    stop = threading.Event()
+    signal.signal(signal.SIGINT, lambda *_: stop.set())
+    signal.signal(signal.SIGTERM, lambda *_: stop.set())
+
+    # Emergency stop key: 'e' + Enter -> go passive NOW. We stop the controller loop
+    # from re-publishing, send a single zero-gain (limp) command, and hard-exit the
+    # process. This deliberately skips the graceful disconnect (soft-stop arm ramp +
+    # thread joins), which is what made it take several seconds.
+    def estop_listener() -> None:
+        for line in sys.stdin:
+            if line.strip().lower() == "e":
+                logger.warning("E-STOP ('e'): going passive NOW.")
+                try:
+                    robot._shutdown_event.set()  # stop the 50Hz controller loop publishing
+                    time.sleep(0.05)  # let it finish its current cycle
+                    robot._send_zero_torque()  # motors limp; nothing overwrites it now
+                except Exception as e:  # noqa: BLE001
+                    logger.warning("E-stop zero-torque failed: %s", e)
+                os._exit(0)  # immediate hard exit, no slow cleanup
+
+    threading.Thread(target=estop_listener, daemon=True).start()
+
+    # Proprioception feedback: publish observation.state (29 joint .q) so the
+    # laptop-side inference client can feed it to the policy. DDS stays local to
+    # the robot; only this compact JSON state crosses the network (like the
+    # camera/action ZMQ channels). get_observation() here reads only DDS lowstate
+    # (the robot is built with cameras={}), so it never touches the USB cameras.
+    # Disable with --state-fps <=0 (e.g. to reproduce the plain-teleop setup).
+    state_sock = None
+    if args.state_fps > 0:
+        state_sock = ctx.socket(zmq.PUB)
+        state_sock.setsockopt(zmq.SNDHWM, 2)
+        state_sock.setsockopt(zmq.LINGER, 0)
+        state_sock.bind(f"tcp://0.0.0.0:{args.state_port}")
+        logger.info("Publishing observation.state on :%d at %.0f Hz", args.state_port, args.state_fps)
+
+        def publish_state() -> None:
+            period = 1.0 / args.state_fps
+            joint_names = [j.name for j in G1_29_JointIndex]
+            while not stop.is_set():
+                t0 = time.time()
+                obs = robot.get_observation()
+                if obs:
+                    state = {f"{name}.q": float(obs.get(f"{name}.q", 0.0)) for name in joint_names}
+                    # Also publish the controller's *commanded* base height / torso
+                    # orientation. These live only on the robot (the joystick nudges
+                    # them here), so the laptop recorder needs them to reconstruct the
+                    # policy's absolute height/rpy action channels.
+                    if robot.controller is not None:
+                        state["groot.height"] = float(getattr(robot.controller, "groot_height_cmd", 0.0))
+                        orient = getattr(robot.controller, "groot_orientation_cmd", None)
+                        if orient is not None:
+                            state["groot.rpy.roll"] = float(orient[0])
+                            state["groot.rpy.pitch"] = float(orient[1])
+                            state["groot.rpy.yaw"] = float(orient[2])
+                    with contextlib.suppress(zmq.Again):
+                        state_sock.send_json(state, zmq.NOBLOCK)
+                time.sleep(max(0.0, period - (time.time() - t0)))
+
+        threading.Thread(target=publish_state, daemon=True).start()
+    else:
+        logger.info("observation.state PUB disabled (--state-fps<=0); inference client will get no proprio")
+
+    n = 0
+    try:
+        while not stop.is_set():
+            try:
+                payload = sock.recv()
+            except zmq.Again:
+                continue
+            except zmq.ContextTerminated:
+                break
+
+            try:
+                action = json.loads(payload.decode("utf-8"))
+            except (ValueError, UnicodeDecodeError) as e:
+                logger.warning("Dropping malformed action: %s", e)
+                continue
+
+            robot.send_action(action)
+
+            # Absolute base height / torso orientation from the policy (not
+            # expressible via the joystick interface, so set on the controller
+            # directly). Teleop omits these keys, so this is a no-op there.
+            if robot.controller is not None:
+                height = action.get("groot.height")
+                if height is not None:
+                    robot.controller.groot_height_cmd = float(height)
+                roll = action.get("groot.rpy.roll")
+                pitch = action.get("groot.rpy.pitch")
+                yaw = action.get("groot.rpy.yaw")
+                if None not in (roll, pitch, yaw):
+                    robot.controller.groot_orientation_cmd = np.array(
+                        [float(roll), float(pitch), float(yaw)], dtype=np.float32
+                    )
+
+            if grippers:
+                # L3 = remote.button.4 -> left, R3 = remote.button.0 -> right.
+                if "L" in grippers and "remote.button.4" in action:
+                    grippers["L"].apply(bool(action["remote.button.4"]))
+                if "R" in grippers and "remote.button.0" in action:
+                    grippers["R"].apply(bool(action["remote.button.0"]))
+
+            n += 1
+            if n % 60 == 0:
+                axes = {
+                    k: round(float(action.get(k, 0.0)), 3)
+                    for k in ("remote.lx", "remote.ly", "remote.rx", "remote.ry")
+                }
+                btn = {k: action.get(k) for k in ("remote.button.0", "remote.button.4") if k in action}
+                logger.info("Applied %d actions | axes=%s buttons=%s", n, axes, btn)
+    finally:
+        logger.info("Shutting down onboard controller...")
+        stop.set()
+        if state_sock is not None:
+            try:
+                state_sock.close(linger=0)
+            except Exception as e:  # noqa: BLE001
+                logger.warning("State socket close failed: %s", e)
+        if camera_server is not None:
+            try:
+                camera_server.stop()
+            except Exception as e:  # noqa: BLE001
+                logger.warning("Camera server stop failed: %s", e)
+        for g in grippers.values():
+            try:
+                g.bus.disconnect()
+            except Exception as e:  # noqa: BLE001
+                logger.warning("Gripper %s disconnect failed: %s", g.name, e)
+        robot.disconnect()
+
+
+if __name__ == "__main__":
+    main()

src/lerobot/robots/unitree_g1/run_g1_server.py

@@ -26,11 +26,12 @@ Uses JSON for secure serialization instead of pickle.
 
 import argparse
 import base64
-import contextlib
 import json
+import re
 import threading
 import time
-from typing import Any
+from dataclasses import dataclass
+from typing import TYPE_CHECKING, Any
 
 import zmq
 from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
@@ -41,6 +42,9 @@ from unitree_sdk2py.utils.crc import CRC
 
 from lerobot.cameras.zmq.image_server import ImageServer
 
+if TYPE_CHECKING:
+    from lerobot.motors.damiao.damiao import DamiaoMotorsBus
+
 # DDS topic names follow Unitree SDK naming conventions
 # ruff: noqa: N816
 kTopicLowCommand_Debug = "rt/lowcmd"  # action to robot
@@ -48,9 +52,125 @@ kTopicLowState = "rt/lowstate"  # observation from robot
 
 LOWCMD_PORT = 6000
 LOWSTATE_PORT = 6001
+# Side-channel for gripper commands sent by the teleop laptop (exo R3/L3 clicks).
+# The exo joystick buttons are only known laptop-side, so the robot object forwards
+# them here as JSON {"L": 0/1, "R": 0/1}; see UnitreeG1._send_gripper_cmd.
+GRIPPER_PORT = 6002
 NUM_MOTORS = 35
 
 
+@dataclass
+class Gripper:
+    """A single Damiao gripper that only writes to CAN when the open/close state changes."""
+
+    name: str
+    bus: "DamiaoMotorsBus"
+    open_deg: float
+    close_deg: float
+    _last_cmd: str | None = None  # "open" | "close"
+
+    def apply(self, want_close: bool) -> None:
+        want = "close" if want_close else "open"
+        if want == self._last_cmd:
+            return
+        target = self.close_deg if want_close else self.open_deg
+        self.bus.write("Goal_Position", "gripper", target)
+        self._last_cmd = want
+        print(f"[gripper] {self.name} -> {want.upper()} ({target:.1f} deg)")
+
+
+def build_gripper(
+    name: str,
+    port: str,
+    send_id: int,
+    recv_id: int,
+    motor_type: str,
+    use_can_fd: bool,
+    open_deg: float,
+    close_deg: float,
+    kp: float,
+    kd: float,
+) -> Gripper:
+    from lerobot.motors.damiao.damiao import DamiaoMotorsBus
+    from lerobot.motors.motors_bus import Motor, MotorNormMode
+
+    motors = {
+        "gripper": Motor(
+            id=send_id,
+            model=motor_type,
+            norm_mode=MotorNormMode.DEGREES,
+            motor_type_str=motor_type,
+            recv_id=recv_id,
+        )
+    }
+    bus = DamiaoMotorsBus(port=port, motors=motors, use_can_fd=use_can_fd)
+    print(f"Connecting {name} gripper on {port} (fd={use_can_fd})...")
+    bus.connect(handshake=True)
+    bus.write("Kp", "gripper", kp)
+    bus.write("Kd", "gripper", kd)
+    bus.write("Goal_Position", "gripper", open_deg)  # start open
+    print(f"  {name}: connected, torque enabled, opened.")
+    return Gripper(name, bus, open_deg, close_deg, _last_cmd="open")
+
+
+def parse_camera_specs(spec: str, default_width: int, default_height: int) -> dict[str, dict]:
+    """Parse a multi-camera spec string into an ImageServer `cameras` dict.
+
+    Format: comma-separated ``name:device[:WxH[:FOURCC]]`` entries, e.g.
+    ``head_camera:4,left_wrist:0,right_wrist:1,ego:2``. ``device`` may be an
+    integer index or an explicit device path (e.g. ``/dev/video4``); the path form
+    is more reliable when the bare integer index fails to open. The optional ``WxH``
+    overrides the default resolution (e.g. ``left_wrist:0:640x480``). The optional
+    ``FOURCC`` forces a pixel format (e.g. ``head_camera:/dev/video8:1280x720:YUYV``),
+    which some cameras (e.g. RealSense color nodes) require before the resolution
+    can be applied.
+
+    The device token may itself contain colons — notably stable ``by-path`` names
+    like ``/dev/v4l/by-path/platform-3610000.xhci-usb-0:2.2:1.0-video-index0``,
+    which survive USB re-enumeration/unplug (unlike bare ``/dev/videoN`` indices or
+    ``by-id`` names that collide when two cameras share a serial). The optional
+    ``WxH`` and ``FOURCC`` are therefore parsed from the *right* so the colons in
+    the device path are preserved.
+    """
+    wh_re = re.compile(r"\d+x\d+", re.IGNORECASE)
+    fourcc_re = re.compile(r"[A-Za-z0-9]{4}")
+
+    cameras: dict[str, dict] = {}
+    for entry in spec.split(","):
+        entry = entry.strip()
+        if not entry:
+            continue
+        if ":" not in entry:
+            raise ValueError(f"Invalid camera spec '{entry}', expected 'name:device[:WxH[:FOURCC]]'")
+        name, rest = entry.split(":", 1)
+        name = name.strip()
+        tokens = [t.strip() for t in rest.split(":")]
+
+        # Peel optional FOURCC then WxH off the right. FOURCC only appears after a
+        # WxH, so require that pairing to avoid mistaking a device-path segment for
+        # a pixel format. Real device-path tail segments (e.g. "1.0-video-index0")
+        # won't match these strict patterns.
+        fourcc = None
+        if len(tokens) >= 3 and wh_re.fullmatch(tokens[-2]) and fourcc_re.fullmatch(tokens[-1]):
+            fourcc = tokens.pop().upper()
+        width, height = default_width, default_height
+        if len(tokens) >= 2 and wh_re.fullmatch(tokens[-1]):
+            w, h = tokens.pop().lower().split("x")
+            width, height = int(w), int(h)
+
+        raw_id = ":".join(tokens).strip()
+        if not raw_id:
+            raise ValueError(f"Invalid camera spec '{entry}', missing device")
+        # Accept either an integer index or an explicit device path (e.g. /dev/video4).
+        device_id: int | str = int(raw_id) if raw_id.lstrip("-").isdigit() else raw_id
+        if name in cameras:
+            raise ValueError(f"Duplicate camera name '{name}' in --cameras")
+        cameras[name] = {"device_id": device_id, "shape": [height, width], "fourcc": fourcc}
+    if not cameras:
+        raise ValueError("No cameras parsed from --cameras spec")
+    return cameras
+
+
 def lowstate_to_dict(msg: hg_LowState) -> dict[str, Any]:
     """Convert LowState SDK message to a JSON-serializable dictionary."""
     motor_states = []
@@ -98,6 +218,34 @@ def dict_to_lowcmd(data: dict[str, Any]) -> hg_LowCmd:
     return cmd
 
 
+def gripper_cmd_loop(
+    gripper_sock: zmq.Socket,
+    grippers: dict[str, Gripper],
+    shutdown_event: threading.Event,
+) -> None:
+    """Receive gripper commands from the teleop laptop and apply them.
+
+    Payload is JSON ``{"L": 0/1, "R": 0/1}`` where 1 = close, 0 = open. Only writes
+    CAN when a gripper's state actually changes (handled by Gripper.apply).
+    """
+    while not shutdown_event.is_set():
+        try:
+            payload = gripper_sock.recv()
+        except zmq.ContextTerminated:
+            break
+        except zmq.Again:
+            continue
+        try:
+            cmd = json.loads(payload.decode("utf-8"))
+        except (json.JSONDecodeError, UnicodeDecodeError):
+            continue
+        print(f"[gripper] recv {cmd}")
+        if "L" in grippers and "L" in cmd:
+            grippers["L"].apply(bool(cmd["L"]))
+        if "R" in grippers and "R" in cmd:
+            grippers["R"].apply(bool(cmd["R"]))
+
+
 def state_forward_loop(
     lowstate_sub: ChannelSubscriber,
     lowstate_sock: zmq.Socket,
@@ -119,9 +267,14 @@ def state_forward_loop(
             # Convert to dict and serialize with JSON
             state_dict = lowstate_to_dict(msg)
             payload = json.dumps({"topic": kTopicLowState, "data": state_dict}).encode("utf-8")
-            # if no subscribers / tx buffer full, just drop
-            with contextlib.suppress(zmq.Again):
+            try:
+                # if no subscribers / tx buffer full, just drop
                 lowstate_sock.send(payload, zmq.NOBLOCK)
+            except zmq.Again:
+                pass
+            except zmq.ContextTerminated:
+                # Context torn down during shutdown; exit the loop quietly.
+                break
             last_state_time = now
 
 
@@ -156,28 +309,59 @@ def main() -> None:
     parser = argparse.ArgumentParser(description="DDS-to-ZMQ bridge server for Unitree G1")
     parser.add_argument("--camera", action="store_true", help="Also launch camera server")
     parser.add_argument("--camera-device", type=int, default=4, help="Camera device ID (default: 4)")
+    parser.add_argument(
+        "--cameras",
+        type=str,
+        default=None,
+        help=(
+            "Multi-camera spec 'name:device_id[:WxH]' comma-separated, e.g. "
+            "'head_camera:4,left_wrist:0,right_wrist:1,ego:2'. Overrides --camera-device "
+            "and implies --camera. Per-camera resolution optional (defaults to "
+            "--camera-width/--camera-height)."
+        ),
+    )
     parser.add_argument("--camera-fps", type=int, default=30, help="Camera FPS (default: 30)")
     parser.add_argument("--camera-width", type=int, default=640, help="Camera width (default: 640)")
     parser.add_argument("--camera-height", type=int, default=480, help="Camera height (default: 480)")
     parser.add_argument("--camera-port", type=int, default=5555, help="Camera ZMQ port (default: 5555)")
+    # Gripper control from wireless-remote R3/L3
+    parser.add_argument(
+        "--grippers", action="store_true", help="Enable Damiao gripper control from wireless remote R3/L3"
+    )
+    parser.add_argument("--gripper-port-left", default="can1", help="CAN interface for LEFT gripper")
+    parser.add_argument("--gripper-port-right", default="can0", help="CAN interface for RIGHT gripper")
+    parser.add_argument("--gripper-send-id", type=lambda x: int(x, 0), default=0x08, help="Motor send CAN id")
+    parser.add_argument("--gripper-recv-id", type=lambda x: int(x, 0), default=0x18, help="Motor recv CAN id")
+    parser.add_argument("--gripper-motor-type", default="dm4310", help="Damiao motor type")
+    parser.add_argument("--gripper-open-deg", type=float, default=-65.0, help="Gripper OPEN position (deg)")
+    parser.add_argument("--gripper-close-deg", type=float, default=0.0, help="Gripper CLOSE position (deg)")
+    parser.add_argument("--gripper-kp", type=float, default=15.0, help="MIT position gain (stiffness)")
+    parser.add_argument("--gripper-kd", type=float, default=0.5, help="MIT damping gain")
+    parser.add_argument(
+        "--gripper-no-fd", dest="gripper_fd", action="store_false", help="Classic CAN (non-FD adapter)"
+    )
+    parser.set_defaults(gripper_fd=True)
     args = parser.parse_args()
 
     # Optionally start camera server in background thread
     camera_thread = None
-    if args.camera:
-        camera_config = {
-            "fps": args.camera_fps,
-            "cameras": {
+    camera_server = None
+    if args.camera or args.cameras:
+        if args.cameras:
+            cameras = parse_camera_specs(args.cameras, args.camera_width, args.camera_height)
+        else:
+            cameras = {
                 "head_camera": {
                     "device_id": args.camera_device,
                     "shape": [args.camera_height, args.camera_width],
                 }
-            },
-        }
+            }
+        camera_config = {"fps": args.camera_fps, "cameras": cameras}
         camera_server = ImageServer(camera_config, port=args.camera_port)
         camera_thread = threading.Thread(target=camera_server.run, daemon=True)
         camera_thread.start()
-        print(f"Camera server started on port {args.camera_port} (device {args.camera_device})")
+        cam_summary = ", ".join(f"{name}(dev {c['device_id']})" for name, c in cameras.items())
+        print(f"Camera server started on port {args.camera_port}: {cam_summary}")
 
     # initialize DDS
     ChannelFactoryInitialize(0)
@@ -214,6 +398,39 @@ def main() -> None:
     lowstate_sock = ctx.socket(zmq.PUB)
     lowstate_sock.bind(f"tcp://0.0.0.0:{LOWSTATE_PORT}")
 
+    # Optionally connect Damiao grippers driven by exo R3/L3 (forwarded from the laptop)
+    grippers: dict[str, Gripper] = {}
+    gripper_sock = None
+    if args.grippers:
+        try:
+            grippers["L"] = build_gripper(
+                "L",
+                args.gripper_port_left,
+                args.gripper_send_id,
+                args.gripper_recv_id,
+                args.gripper_motor_type,
+                args.gripper_fd,
+                args.gripper_open_deg,
+                args.gripper_close_deg,
+                args.gripper_kp,
+                args.gripper_kd,
+            )
+            grippers["R"] = build_gripper(
+                "R",
+                args.gripper_port_right,
+                args.gripper_send_id,
+                args.gripper_recv_id,
+                args.gripper_motor_type,
+                args.gripper_fd,
+                args.gripper_open_deg,
+                args.gripper_close_deg,
+                args.gripper_kp,
+                args.gripper_kd,
+            )
+        except Exception as e:  # noqa: BLE001
+            print(f"WARNING: gripper setup failed ({e}); continuing without grippers.")
+            grippers = {}
+
     state_period = 0.002  # ~500 hz
     shutdown_event = threading.Event()
 
@@ -224,6 +441,18 @@ def main() -> None:
     )
     t_state.start()
 
+    # start gripper command listener (commands come from the teleop laptop)
+    t_gripper = None
+    if grippers:
+        gripper_sock = ctx.socket(zmq.PULL)
+        gripper_sock.bind(f"tcp://0.0.0.0:{GRIPPER_PORT}")
+        t_gripper = threading.Thread(
+            target=gripper_cmd_loop,
+            args=(gripper_sock, grippers, shutdown_event),
+        )
+        t_gripper.start()
+        print(f"Grippers enabled: listening for R3/L3 commands on port {GRIPPER_PORT}")
+
     print("bridge running (lowstate -> zmq, lowcmd -> dds)")
 
     # run command forwarding in main thread
@@ -233,10 +462,21 @@ def main() -> None:
         print("shutting down bridge...")
     finally:
         shutdown_event.set()
+        # Stop the camera server first so it releases the V4L2 devices cleanly;
+        # otherwise the daemon thread is killed on exit and the cameras stay wedged.
+        if camera_server is not None:
+            camera_server.stop()
         ctx.term()  # terminates blocking zmq.recv() calls
         t_state.join(timeout=2.0)
+        if t_gripper is not None:
+            t_gripper.join(timeout=2.0)
         if camera_thread is not None:
-            camera_thread.join(timeout=2.0)
+            camera_thread.join(timeout=3.0)
+        for g in grippers.values():
+            try:
+                g.bus.disconnect(disable_torque=True)
+            except Exception as exc:  # noqa: BLE001
+                print(f"  {g.name} gripper disconnect error: {exc}")
 
 
 if __name__ == "__main__":

src/lerobot/robots/unitree_g1/run_g1_teleop_client.py

@@ -0,0 +1,136 @@
+#!/usr/bin/env python
+
+# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Laptop-side thin client: read the exos, run IK, ship the action to the onboard G1.
+
+This is the counterpart to ``run_g1_onboard.py``. The heavy IK (pinocchio/casadi) stays
+on the laptop where it's set up; only the resulting action (arm joint targets + joystick
+axes + gripper flags) is sent as JSON over ZMQ. The locomotion policy runs on the robot,
+so nothing latency-critical crosses the network.
+
+Example (on the laptop):
+
+    export LD_PRELOAD="$HOME/Documents/miniconda3/envs/lerobot312/lib/libstdc++.so.6"
+    export OMP_NUM_THREADS=1 OPENBLAS_NUM_THREADS=1
+    python -m lerobot.robots.unitree_g1.run_g1_teleop_client \
+        --robot-ip 172.18.130.111 \
+        --left-arm-port /dev/ttyACM1 --right-arm-port /dev/ttyACM0 \
+        --teleop-id asdasd
+"""
+
+import argparse
+import contextlib
+import logging
+import time
+
+import numpy as np
+import zmq
+
+from lerobot.teleoperators.unitree_g1.config_unitree_g1 import (
+    ExoskeletonArmPortConfig,
+    UnitreeG1TeleoperatorConfig,
+)
+from lerobot.teleoperators.unitree_g1.unitree_g1 import UnitreeG1Teleoperator
+
+logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s", force=True)
+logger = logging.getLogger("g1_teleop_client")
+
+ACTION_PORT = 6004
+
+
+def _to_jsonable(action: dict) -> dict:
+    """Cast numpy scalars to plain Python so json.dumps accepts the action."""
+    out: dict = {}
+    for k, v in action.items():
+        if isinstance(v, (np.generic,)):
+            out[k] = v.item()
+        else:
+            out[k] = v
+    return out
+
+
+def main() -> None:
+    p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+    p.add_argument("--robot-ip", required=True, help="IP of the robot running run_g1_onboard.py")
+    p.add_argument("--action-port", type=int, default=ACTION_PORT)
+    p.add_argument("--left-arm-port", required=True, help="Serial port for the LEFT exo arm")
+    p.add_argument("--right-arm-port", required=True, help="Serial port for the RIGHT exo arm")
+    p.add_argument("--teleop-id", default="exo", help="Teleoperator id (for calibration files)")
+    p.add_argument("--frozen-joints", default="", help="Comma-separated joints to freeze in IK")
+    p.add_argument("--fps", type=float, default=60.0, help="Max action send rate")
+    args = p.parse_args()
+
+    cfg = UnitreeG1TeleoperatorConfig(
+        id=args.teleop_id,
+        left_arm_config=ExoskeletonArmPortConfig(port=args.left_arm_port),
+        right_arm_config=ExoskeletonArmPortConfig(port=args.right_arm_port),
+        frozen_joints=args.frozen_joints,
+    )
+    teleop = UnitreeG1Teleoperator(cfg)
+    logger.info("Connecting exo teleoperator (L=%s, R=%s)...", args.left_arm_port, args.right_arm_port)
+    teleop.connect()
+
+    ctx = zmq.Context.instance()
+    sock = ctx.socket(zmq.PUSH)
+    sock.setsockopt(zmq.SNDHWM, 2)
+    sock.setsockopt(zmq.CONFLATE, 1)  # drop stale actions rather than queue them
+    sock.setsockopt(zmq.LINGER, 0)
+    sock.connect(f"tcp://{args.robot_ip}:{args.action_port}")
+    logger.info("Sending actions to %s:%d. Ctrl-C to stop.", args.robot_ip, args.action_port)
+
+    # The normal lerobot-teleoperate loop calls teleop.send_feedback(robot_obs) each
+    # iteration, which resets the RemoteController axes from the robot's (usually idle)
+    # wireless-remote bytes. That reset is what keeps `wireless_active` False so the exo
+    # thumb-sticks (set_from_exo) are read every frame. We have no robot feedback here,
+    # so without this reset the first non-zero exo axis latches `wireless_active` True
+    # and the sticks freeze. Reset the latched axes ourselves before every get_action.
+    rc = teleop.remote_controller
+
+    period = 1.0 / args.fps if args.fps > 0 else 0.0
+    n = 0
+    try:
+        while True:
+            t0 = time.time()
+            rc.lx = rc.ly = rc.rx = rc.ry = 0.0
+            rc.button = [0] * 16
+            action = teleop.get_action()
+            with contextlib.suppress(zmq.Again):
+                sock.send_json(_to_jsonable(action), zmq.NOBLOCK)
+            n += 1
+            if n % 60 == 0:
+                axes = {
+                    k: round(float(action.get(k, 0.0)), 3)
+                    for k in ("remote.lx", "remote.ly", "remote.rx", "remote.ry")
+                }
+                logger.info("Sent %d actions | axes=%s", n, axes)
+            if period:
+                time.sleep(max(0.0, period - (time.time() - t0)))
+    except KeyboardInterrupt:
+        logger.info("Stopping teleop client...")
+    finally:
+        with_suppress_disconnect(teleop)
+        sock.close(linger=0)
+
+
+def with_suppress_disconnect(teleop) -> None:
+    try:
+        teleop.disconnect()
+    except Exception as e:  # noqa: BLE001
+        logger.warning("teleop.disconnect() failed: %s", e)
+
+
+if __name__ == "__main__":
+    main()

src/lerobot/robots/unitree_g1/unitree_g1.py

@@ -53,6 +53,7 @@ if TYPE_CHECKING or _unitree_sdk_available:
         LowState_ as hg_LowState,
     )
     from unitree_sdk2py.utils.crc import CRC
+    from unitree_sdk2py.utils.joystick import Joystick
 else:
     _SDKChannelFactoryInitialize = None
     _SDKChannelPublisher = None
@@ -61,6 +62,31 @@ else:
     hg_LowCmd = None
     hg_LowState = None
     CRC = None
+    Joystick = None
+
+
+# Wireless-remote button byte layout (little-endian uint16 from bytes 2-3), mapped to
+# the positional button indices the locomotion controllers expect. Mirrors the exo
+# teleoperator's RemoteController so the onboard path reads the physical Unitree remote
+# identically.
+_REMOTE_BUTTON_MAP: list[str] = [
+    "RB",
+    "LB",
+    "start",
+    "back",
+    "RT",
+    "LT",
+    "",
+    "",
+    "A",
+    "B",
+    "X",
+    "Y",
+    "up",
+    "right",
+    "down",
+    "left",
+]
 
 logger = logging.getLogger(__name__)
 
@@ -79,6 +105,10 @@ class LocomotionController(Protocol):
 kTopicLowCommand_Debug = "rt/lowcmd"
 kTopicLowState = "rt/lowstate"
 
+# Side-channel port on the robot for forwarding exo R3/L3 gripper commands
+# (see run_g1_server.gripper_cmd_loop). Real-robot only.
+GRIPPER_CMD_PORT = 6002
+
 
 @dataclass
 class MotorState:
@@ -122,8 +152,10 @@ class UnitreeG1(Robot):
         # Initialize cameras config (ZMQ-based) - actual connection in connect()
         self._cameras = make_cameras_from_configs(config.cameras)
 
-        # Import channel classes based on mode
-        if config.is_simulation:
+        # Import channel classes based on mode. Simulation and onboard both talk to a
+        # real (local) DDS via the Unitree SDK; only the laptop-side bridge client uses
+        # the ZMQ socket shim.
+        if config.is_simulation or config.onboard:
             self._ChannelFactoryInitialize = _SDKChannelFactoryInitialize
             self._ChannelPublisher = _SDKChannelPublisher
             self._ChannelSubscriber = _SDKChannelSubscriber
@@ -157,6 +189,10 @@ class UnitreeG1(Robot):
         self.controller_input = default_remote_input()
         self.controller_output = {}
 
+        # Onboard-only: parser for the physical Unitree wireless remote (read straight
+        # from local lowstate so joystick locomotion works without a laptop round-trip).
+        self._joystick = None
+
     def _subscribe_lowstate(self):  # polls robot state @ 250Hz
         while not self._shutdown_event.is_set():
             start_time = time.time()
@@ -260,17 +296,28 @@ class UnitreeG1(Robot):
 
             if lowstate is not None and self.controller is not None:
                 loop_count += 1
-                if time.time() - last_log_time >= 5.0:  # Log every 5 seconds
-                    actual_hz = loop_count / (time.time() - last_log_time)
-                    logger.info(
-                        f"Controller actual rate: {actual_hz:.1f}Hz (target: {1.0 / control_dt:.1f}Hz)"
-                    )
-                    loop_count = 0
-                    last_log_time = time.time()
+
                 # Read controller input snapshot
                 with self._controller_action_lock:
                     controller_input = dict(self.controller_input)
 
+                # Onboard: the physical Unitree remote (in local lowstate) takes
+                # priority for locomotion when active; otherwise laptop/exo axes stand.
+                wl = None
+                if self.config.onboard:
+                    wl = self._wireless_remote_input(lowstate)
+                    if wl is not None:
+                        controller_input.update(wl)
+
+                if time.time() - last_log_time >= 5.0:  # Log every 5 seconds
+                    actual_hz = loop_count / (time.time() - last_log_time)
+                    eff = {k: round(float(controller_input.get(k, 0.0)), 3) for k in REMOTE_AXES}
+                    logger.info(
+                        f"Controller {actual_hz:.1f}Hz | eff_axes={eff} wireless={'ACTIVE' if wl else 'idle'}"
+                    )
+                    loop_count = 0
+                    last_log_time = time.time()
+
                 # Run controller step
                 controller_action = self.controller.run_step(controller_input, lowstate)
 
@@ -293,7 +340,62 @@ class UnitreeG1(Robot):
     def configure(self) -> None:
         pass
 
+    def _wireless_remote_input(self, lowstate) -> dict | None:
+        """Parse the physical Unitree remote from lowstate into controller inputs.
+
+        Onboard only. Returns None when the remote is idle so the laptop-provided
+        (exo) axes keep control; otherwise the physical remote takes priority — the
+        same precedence the exo teleoperator applies laptop-side.
+        """
+        js = self._joystick
+        if js is None:
+            return None
+        wr = getattr(lowstate, "wireless_remote", None)
+        if not wr or len(wr) < 24:
+            return None
+        try:
+            js.extract(wr)
+        except Exception:  # noqa: BLE001
+            return None
+
+        axes = {
+            "remote.lx": float(js.lx.data),
+            "remote.ly": float(js.ly.data),
+            "remote.rx": float(js.rx.data),
+            "remote.ry": float(js.ry.data),
+        }
+        active = any(abs(v) > 1e-2 for v in axes.values())
+        out = dict(axes)
+        for i, name in enumerate(_REMOTE_BUTTON_MAP):
+            if name:
+                val = float(getattr(js, name).data)
+                out[f"remote.button.{i}"] = val
+                if val:
+                    active = True
+        return out if active else None
+
+    def _release_motion_control(self) -> None:
+        """Release the robot's built-in motion services so we can send raw lowcmd.
+
+        Onboard-only. Mirrors run_g1_server.py: on the real robot the factory
+        locomotion/hand services must relinquish control before our controller can
+        write to ``rt/lowcmd``, otherwise commands are ignored or fought.
+        """
+        from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+
+        msc = MotionSwitcherClient()
+        msc.SetTimeout(5.0)
+        msc.Init()
+        _, result = msc.CheckMode()
+        while result is not None and "name" in result and result["name"]:
+            logger.info("[UnitreeG1] Releasing built-in mode '%s'...", result["name"])
+            msc.ReleaseMode()
+            _, result = msc.CheckMode()
+            time.sleep(1.0)
+
     def connect(self, calibrate: bool = True) -> None:  # connect to DDS
+        self._is_disconnected = False
+        self._pending_arm_softstart = False
         # Initialize DDS channel and simulation environment
         if self.config.is_simulation:
             from lerobot.envs import make_env
@@ -302,9 +404,41 @@ class UnitreeG1(Robot):
             self._env_wrapper = make_env("lerobot/unitree-g1-mujoco", trust_remote_code=True)
             # Extract the actual gym env from the dict structure
             self.sim_env = self._env_wrapper["hub_env"][0].envs[0]
+        elif self.config.onboard:
+            # Real robot, controller running onboard against local DDS. Initialize the
+            # real SDK channel factory on the robot's DDS interface and take low-level
+            # control from the built-in services before we start writing lowcmd.
+            if self.config.dds_interface:
+                self._ChannelFactoryInitialize(0, self.config.dds_interface)
+            else:
+                self._ChannelFactoryInitialize(0)
+            self._release_motion_control()
+            # Read the physical wireless remote directly from lowstate for locomotion.
+            self._joystick = Joystick()
+            for axis in (self._joystick.lx, self._joystick.ly, self._joystick.rx, self._joystick.ry):
+                axis.smooth = 1.0
+                axis.deadzone = 0.0
         else:
             self._ChannelFactoryInitialize(0, config=self.config)
 
+        # Gripper command side-channel (real robot only): forwards exo R3/L3 clicks to
+        # run_g1_server, which drives the Damiao grippers over CAN.
+        self._gripper_sock = None
+        self._last_gripper_cmd = None
+        self._warned_no_gripper_buttons = False
+        if not self.config.is_simulation and not self.config.onboard:
+            try:
+                import zmq
+
+                sock = zmq.Context.instance().socket(zmq.PUSH)
+                sock.setsockopt(zmq.SNDHWM, 2)
+                sock.setsockopt(zmq.LINGER, 0)
+                sock.connect(f"tcp://{self.config.robot_ip}:{GRIPPER_CMD_PORT}")
+                self._gripper_sock = sock
+            except Exception as e:  # noqa: BLE001
+                logger.warning(f"Gripper command channel setup failed ({e}); grippers disabled.")
+                self._gripper_sock = None
+
         # Initialize direct motor control interface
         self.lowcmd_publisher = self._ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd)
         self.lowcmd_publisher.Init()
@@ -352,11 +486,37 @@ class UnitreeG1(Robot):
 
         # Start controller thread if enabled
         if self.controller is not None:
+            # Defer the arm soft-start to the first teleop action so we ramp to the
+            # exo's actual starting pose instead of the fixed default (avoids a snap
+            # when the operator's arms aren't at the default). Arms hold their current
+            # pose (set above) until then; the controller owns the legs meanwhile.
+            self._pending_arm_softstart = True
+
             self._controller_thread = threading.Thread(target=self._controller_loop, daemon=True)
             self._controller_thread.start()
             fps = int(1.0 / self.controller.control_dt)
             logger.info(f"Controller thread started ({fps}Hz)")
 
+    def _soft_stop(self) -> None:
+        """Gently ramp the arms to the default rest pose before shutdown.
+
+        Mirror of the connect-time soft-start. Only runs on the real robot when a
+        locomotion controller is active (so the legs stay balanced while the arms
+        come down) and lowstate is available to read the current pose.
+        """
+        if self.config.is_simulation or not self.config.soft_stop:
+            return
+        if self.controller is None:
+            return
+        with self._lowstate_lock:
+            if self._lowstate is None:
+                return
+        try:
+            logger.info("Soft-stop: ramping arms to default position...")
+            self._interpolate_to_default(duration=self.config.soft_stop_duration)
+        except Exception as e:
+            logger.warning(f"Soft-stop failed ({e}); continuing shutdown.")
+
     def _send_zero_torque(self) -> None:
         """Send a zero-gain command to make joints passive before shutting down."""
         try:
@@ -372,6 +532,17 @@ class UnitreeG1(Robot):
             logger.warning(f"Failed to send zero-torque on disconnect: {e}")
 
     def disconnect(self):
+        # Idempotent: disconnect() can be called both explicitly and again via GC /
+        # interpreter shutdown; re-running soft-stop against already-closed cameras
+        # would error, so bail out if we've already torn down.
+        if getattr(self, "_is_disconnected", False):
+            return
+        self._is_disconnected = True
+
+        # Soft-stop: ramp arms slowly back to the rest pose (hands down) while the
+        # controller still holds the legs, so they don't drop when we go passive.
+        self._soft_stop()
+
         # Put robot in passive mode before stopping threads
         if not self.config.is_simulation:
             self._send_zero_torque()
@@ -412,6 +583,12 @@ class UnitreeG1(Robot):
             self.sim_env = None
             self._env_wrapper = None
 
+        # Close gripper command channel
+        sock = getattr(self, "_gripper_sock", None)
+        if sock is not None:
+            sock.close(linger=0)
+            self._gripper_sock = None
+
         # Disconnect cameras
         for cam in self._cameras.values():
             cam.disconnect()
@@ -471,6 +648,13 @@ class UnitreeG1(Robot):
         return obs
 
     def send_action(self, action: RobotAction) -> RobotAction:
+        # One-time soft-start: ramp the arms to the exo's first commanded pose so
+        # they don't snap when teleop starts. Flag is cleared before ramping so the
+        # ramp's own send_action calls don't re-enter this branch.
+        if self.controller is not None and getattr(self, "_pending_arm_softstart", False):
+            self._pending_arm_softstart = False
+            self._softstart_arms_to_action(action)
+
         action_to_publish = action
         if self.controller is not None:
             # Controller thread owns legs/waist. Here we only update joystick inputs
@@ -500,8 +684,61 @@ class UnitreeG1(Robot):
                 tau[joint.value] = arm_tau[local_idx]
 
         self.publish_lowcmd(action_to_publish, tau=tau)
+        self._send_gripper_cmd(action)
         return action
 
+    def _softstart_arms_to_action(self, action: RobotAction) -> None:
+        """Ramp arms from their current pose to the exo's first commanded pose.
+
+        Runs once, on the first teleop action after connect, so the arms move
+        smoothly to wherever the operator is holding the exoskeleton instead of
+        snapping. Legs stay under the controller throughout (send_action filters
+        to arm joints when a controller is active).
+        """
+        target = np.array(self.config.default_positions, dtype=np.float32).copy()
+        have_arm_target = False
+        for joint in G1_29_JointArmIndex:
+            key = f"{joint.name}.q"
+            if key in action:
+                target[joint.value] = float(action[key])
+                have_arm_target = True
+        if not have_arm_target:
+            return
+        logger.info("Soft-start: ramping arms to exo's first commanded pose...")
+        try:
+            self._interpolate_to_default(duration=3.0, default_positions=target)
+        except Exception as e:
+            logger.warning(f"Arm soft-start to exo pose failed ({e}); continuing.")
+
+    def _send_gripper_cmd(self, action: RobotAction) -> None:
+        """Forward exo R3/L3 button flags to run_g1_server to open/close the grippers.
+
+        L3 (left stick, button.4) -> left gripper, R3 (right stick, button.0) -> right.
+        Only sends when the state changes to avoid flooding the channel.
+        """
+        sock = getattr(self, "_gripper_sock", None)
+        if sock is None:
+            return
+        l3 = action.get("remote.button.4")
+        r3 = action.get("remote.button.0")
+        if l3 is None and r3 is None:
+            if not self._warned_no_gripper_buttons:
+                logger.warning("[gripper] no remote.button.0/4 in action — teleop not emitting exo buttons")
+                self._warned_no_gripper_buttons = True
+            return
+        cmd = {"L": int(bool(l3)), "R": int(bool(r3))}
+        if cmd == self._last_gripper_cmd:
+            return
+        self._last_gripper_cmd = cmd
+
+        import zmq
+
+        try:
+            sock.send_json(cmd, zmq.NOBLOCK)
+            logger.info(f"[gripper] sent {cmd} to {self.config.robot_ip}:{GRIPPER_CMD_PORT}")
+        except zmq.ZMQError as e:
+            logger.warning(f"[gripper] send failed ({e})")
+
     def _update_controller_action(self, action: RobotAction) -> None:
         """Update controller input state from incoming teleop action."""
         with self._controller_action_lock:
@@ -527,6 +764,48 @@ class UnitreeG1(Robot):
     def cameras(self) -> dict:
         return self._cameras
 
+    def _interpolate_to_default(
+        self,
+        duration: float = 3.0,
+        control_dt: float | None = None,
+        default_positions: np.ndarray | list[float] | None = None,
+    ) -> None:
+        """Smoothly ramp joints from their current pose to the default pose (real robot).
+
+        When a locomotion controller owns the legs, ``send_action`` filters to the arm
+        joints, so this effectively ramps only the arms — enough to avoid a startup snap.
+        """
+        if control_dt is None:
+            control_dt = self.config.control_dt
+        if default_positions is None:
+            default_positions = np.array(self.config.default_positions, dtype=np.float32)
+
+        num_steps = max(1, int(duration / control_dt))
+
+        # record current positions
+        obs = self.get_observation()
+        init_dof_pos = np.zeros(29, dtype=np.float32)
+        for motor in G1_29_JointIndex:
+            init_dof_pos[motor.value] = obs[f"{motor.name}.q"]
+
+        # Interpolate to default position
+        for step in range(num_steps):
+            start_time = time.time()
+
+            alpha = step / num_steps
+            action_dict = {}
+            for motor in G1_29_JointIndex:
+                target_pos = default_positions[motor.value]
+                interp_pos = init_dof_pos[motor.value] * (1 - alpha) + target_pos * alpha
+                action_dict[f"{motor.name}.q"] = float(interp_pos)
+
+            self.send_action(action_dict)
+
+            # Maintain constant control rate
+            elapsed = time.time() - start_time
+            sleep_time = max(0, control_dt - elapsed)
+            time.sleep(sleep_time)
+
     def reset(
         self,
         control_dt: float | None = None,
@@ -543,34 +822,9 @@ class UnitreeG1(Robot):
                 {f"{motor.name}.q": float(default_positions[motor.value]) for motor in G1_29_JointIndex}
             )
         else:
-            total_time = 3.0
-            num_steps = int(total_time / control_dt)
-
-            # get current state
-            obs = self.get_observation()
-
-            # record current positions
-            init_dof_pos = np.zeros(29, dtype=np.float32)
-            for motor in G1_29_JointIndex:
-                init_dof_pos[motor.value] = obs[f"{motor.name}.q"]
-
-            # Interpolate to default position
-            for step in range(num_steps):
-                start_time = time.time()
-
-                alpha = step / num_steps
-                action_dict = {}
-                for motor in G1_29_JointIndex:
-                    target_pos = default_positions[motor.value]
-                    interp_pos = init_dof_pos[motor.value] * (1 - alpha) + target_pos * alpha
-                    action_dict[f"{motor.name}.q"] = float(interp_pos)
-
-                self.send_action(action_dict)
-
-                # Maintain constant control rate
-                elapsed = time.time() - start_time
-                sleep_time = max(0, control_dt - elapsed)
-                time.sleep(sleep_time)
+            self._interpolate_to_default(
+                duration=3.0, control_dt=control_dt, default_positions=default_positions
+            )
 
         # Reset controller internal state (gait phase, obs history, etc.)
         if self.controller is not None and hasattr(self.controller, "reset"):

src/lerobot/robots/unitree_g1/view_cameras.py

@@ -0,0 +1,124 @@
+#!/usr/bin/env python
+
+# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Lightweight side-by-side viewer for the G1 robot's ZMQ camera streams.
+
+A fast cv2 alternative to ``--display_data`` (rerun), for eyeballing exactly what
+the robot is streaming. Subscribes to the same ZMQ PUB server as teleop (PUB/SUB,
+so it runs concurrently), and shows head + wrists in one window.
+
+Example (run alongside teleop, no ``--display_data`` needed):
+
+    python -m lerobot.robots.unitree_g1.view_cameras --server-address 172.18.130.111
+
+Notes:
+    - Needs a GUI-capable OpenCV build (``pip install opencv-python``, not the
+      ``-headless`` variant) since it uses ``cv2.imshow``.
+    - Camera names/resolutions default to what ``run_g1.sh`` streams.
+"""
+
+import argparse
+import contextlib
+import logging
+
+import cv2
+import numpy as np
+
+from lerobot.cameras.zmq import ZMQCamera, ZMQCameraConfig
+
+logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s", force=True)
+logger = logging.getLogger("g1_view_cameras")
+
+# name -> (native width, native height) as served by run_g1.sh
+DEFAULT_CAMERAS: dict[str, tuple[int, int]] = {
+    "head_camera": (640, 480),
+    "left_wrist": (1280, 720),
+    "right_wrist": (1280, 720),
+}
+
+
+def _placeholder(name: str, pane_h: int) -> np.ndarray:
+    img = np.zeros((pane_h, int(pane_h * 4 / 3), 3), dtype=np.uint8)
+    cv2.putText(img, f"{name}: no frame", (10, pane_h // 2), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
+    return img
+
+
+def main() -> None:
+    p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+    p.add_argument("--server-address", required=True, help="Robot IP running the ZMQ camera server")
+    p.add_argument("--port", type=int, default=5555)
+    p.add_argument("--fps", type=int, default=30)
+    p.add_argument("--pane-height", type=int, default=360, help="Display height per camera pane (px)")
+    p.add_argument(
+        "--cameras",
+        default=",".join(DEFAULT_CAMERAS),
+        help="Comma-separated camera names to show (must match the server).",
+    )
+    args = p.parse_args()
+
+    names = [c.strip() for c in args.cameras.split(",") if c.strip()]
+    cams: dict[str, ZMQCamera] = {}
+    for name in names:
+        w, h = DEFAULT_CAMERAS.get(name, (None, None))
+        cfg = ZMQCameraConfig(
+            server_address=args.server_address,
+            port=args.port,
+            camera_name=name,
+            width=w,
+            height=h,
+            fps=args.fps,
+            warmup_s=5,
+        )
+        cam = ZMQCamera(cfg)
+        logger.info("Connecting to %s @ %s:%d ...", name, args.server_address, args.port)
+        cam.connect()
+        cams[name] = cam
+    logger.info("All cameras connected. Press 'q' or ESC in the window to quit.")
+
+    win = "G1 cameras (q/ESC to quit)"
+    cv2.namedWindow(win, cv2.WINDOW_NORMAL)
+    try:
+        while True:
+            panes = []
+            for name, cam in cams.items():
+                frame = None
+                with contextlib.suppress(Exception):
+                    frame = cam.read_latest(max_age_ms=2000)
+                if frame is None:
+                    panes.append(_placeholder(name, args.pane_height))
+                    continue
+                bgr = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
+                scale = args.pane_height / bgr.shape[0]
+                disp = cv2.resize(bgr, (int(bgr.shape[1] * scale), args.pane_height))
+                cv2.putText(disp, name, (8, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
+                panes.append(disp)
+
+            if panes:
+                cv2.imshow(win, cv2.hconcat(panes))
+            key = cv2.waitKey(1) & 0xFF
+            if key in (ord("q"), 27):
+                break
+    except KeyboardInterrupt:
+        pass
+    finally:
+        cv2.destroyAllWindows()
+        for cam in cams.values():
+            with contextlib.suppress(Exception):
+                cam.disconnect()
+
+
+if __name__ == "__main__":
+    main()

src/lerobot/teleoperators/unitree_g1/exo_serial.py

@@ -71,12 +71,16 @@ class ExoskeletonArm:
     calibration_fpath: Path
     side: str
     baud_rate: int = 115200
+    ema_window: int = 10
 
     _ser: serial.Serial | None = None
     calibration: ExoskeletonCalibration | None = None
 
     def __post_init__(self):
         require_package("pyserial", extra="unitree_g1", import_name="serial")
+        # EMA smoothing on joint angles to reduce jitter (mirrors homunculus glove).
+        self._ema_alpha = 2 / (self.ema_window + 1)
+        self._ema: dict[str, float] = {}
         if self.calibration_fpath.is_file():
             self._load_calibration()
 
@@ -124,8 +128,30 @@ class ExoskeletonArm:
     def get_angles(self) -> dict[str, float]:
         if not self.calibration:
             raise RuntimeError("exoskeleton not calibrated")
-        raw = self.read_raw()
-        return {} if raw is None else exo_raw_to_angles(raw, self.calibration)
+        return self.angles_from_raw(self.read_raw())
+
+    def angles_from_raw(self, raw: list[int] | None) -> dict[str, float]:
+        """Convert an already-read raw frame to EMA-smoothed joint angles.
+
+        Lets callers read the raw frame once (e.g. to also inspect the joystick
+        button channel) without consuming a second serial sample.
+        """
+        if not self.calibration:
+            raise RuntimeError("exoskeleton not calibrated")
+        if raw is None:
+            return {}
+        return self._apply_ema(exo_raw_to_angles(raw, self.calibration))
+
+    def _apply_ema(self, angles: dict[str, float]) -> dict[str, float]:
+        """Exponential moving average per joint angle; lazily initialised on first read."""
+        smoothed: dict[str, float] = {}
+        for joint, value in angles.items():
+            prev = self._ema.get(joint)
+            self._ema[joint] = (
+                value if prev is None else self._ema_alpha * value + (1 - self._ema_alpha) * prev
+            )
+            smoothed[joint] = self._ema[joint]
+        return smoothed
 
     def calibrate(self) -> None:
         if not self.is_connected:

src/lerobot/teleoperators/unitree_g1/unitree_g1.py

@@ -209,10 +209,12 @@ class UnitreeG1Teleoperator(Teleoperator):
     @cached_property
     def action_features(self) -> dict[str, type]:
         remote_features = dict.fromkeys(self.remote_controller.remote_action, float)
+        # Exo joystick clicks (R3/L3) surfaced as button flags for gripper control.
+        gripper_features = {"remote.button.0": float, "remote.button.4": float}
         if not self._arm_control_enabled:
-            return remote_features
+            return {**remote_features, **gripper_features}
         joint_features = {f"{name}.q": float for name in self._g1_arm_joint_names}
-        return {**joint_features, **remote_features}
+        return {**joint_features, **remote_features, **gripper_features}
 
     @cached_property
     def feedback_features(self) -> dict[str, type]:
@@ -276,8 +278,8 @@ class UnitreeG1Teleoperator(Teleoperator):
             left_raw = self.left_arm.read_raw()
             right_raw = self.right_arm.read_raw()
 
-            left_angles = self.left_arm.get_angles()
-            right_angles = self.right_arm.get_angles()
+            left_angles = self.left_arm.angles_from_raw(left_raw)
+            right_angles = self.right_arm.angles_from_raw(right_raw)
             joint_action = self.ik_helper.compute_g1_joints_from_exo(left_angles, right_angles)
 
         # Wireless remote has priority when non-zero; otherwise, use exo joystick.
@@ -290,7 +292,25 @@ class UnitreeG1Teleoperator(Teleoperator):
             rc.set_from_exo(right_raw, "right")
 
         rc._sync_remote_action()
-        return {**joint_action, **rc.remote_action}
+        gripper_buttons = self._exo_gripper_buttons(left_raw, right_raw)
+        return {**joint_action, **rc.remote_action, **gripper_buttons}
+
+    def _exo_gripper_buttons(
+        self, left_raw: list[int] | None, right_raw: list[int] | None
+    ) -> dict[str, float]:
+        """Exo joystick clicks as button flags: L3 (left stick) -> button.4, R3 (right) -> button.0.
+
+        Reads the raw joystick-button ADC channel directly (pressed pulls it below mid-scale),
+        so it is independent of the wireless-remote priority logic above. UnitreeG1.send_action
+        forwards these to the robot server to open/close the grippers.
+        """
+        rc = self.remote_controller
+        idx = rc.JOYSTICK_BTN_IDX
+
+        def pressed(raw: list[int] | None) -> float:
+            return 1.0 if (raw is not None and len(raw) > idx and raw[idx] < rc.ADC_HALF) else 0.0
+
+        return {"remote.button.4": pressed(left_raw), "remote.button.0": pressed(right_raw)}
 
     def send_feedback(self, feedback: dict[str, Any]) -> None:
         wireless_remote = feedback.get("wireless_remote")