diff --git a/src/lerobot/robots/unitree_g1/config_unitree_g1.py b/src/lerobot/robots/unitree_g1/config_unitree_g1.py index a892491c1..5fc4075a8 100644 --- a/src/lerobot/robots/unitree_g1/config_unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/config_unitree_g1.py @@ -59,6 +59,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 diff --git a/src/lerobot/robots/unitree_g1/gr00t_locomotion.py b/src/lerobot/robots/unitree_g1/gr00t_locomotion.py index c3182a9ed..51a873e71 100644 --- a/src/lerobot/robots/unitree_g1/gr00t_locomotion.py +++ b/src/lerobot/robots/unitree_g1/gr00t_locomotion.py @@ -44,6 +44,18 @@ DOF_POS_SCALE: float = 1.0 DOF_VEL_SCALE: float = 0.05 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" @@ -68,9 +80,16 @@ def load_groot_policies( 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 +153,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) diff --git a/src/lerobot/robots/unitree_g1/holosoma_locomotion.py b/src/lerobot/robots/unitree_g1/holosoma_locomotion.py index 3d3bccbdc..a6b3db66f 100644 --- a/src/lerobot/robots/unitree_g1/holosoma_locomotion.py +++ b/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 diff --git a/src/lerobot/robots/unitree_g1/run_g1.sh b/src/lerobot/robots/unitree_g1/run_g1.sh new file mode 100755 index 000000000..a0971eb4e --- /dev/null +++ b/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" diff --git a/src/lerobot/robots/unitree_g1/run_g1_onboard.py b/src/lerobot/robots/unitree_g1/run_g1_onboard.py new file mode 100644 index 000000000..d6baa333d --- /dev/null +++ b/src/lerobot/robots/unitree_g1/run_g1_onboard.py @@ -0,0 +1,111 @@ +#!/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`` on the laptop. Grippers/cameras are handled +separately by ``run_g1_server.py`` and are intentionally out of scope here. + +Example (on the robot): + + python -m lerobot.robots.unitree_g1.run_g1_onboard --controller GrootLocomotionController +""" + +import argparse +import json +import logging +import signal +import threading + +import zmq + +from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config +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 + + +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( + "--gravity-compensation", + action="store_true", + help="Enable arm gravity compensation (needs pinocchio/casadi on the robot)", + ) + 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={}, + ) + robot = UnitreeG1(cfg) + logger.info("Connecting onboard robot (controller=%s)...", args.controller) + robot.connect() + + 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, 1000) + 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) + + stop = threading.Event() + signal.signal(signal.SIGINT, lambda *_: stop.set()) + signal.signal(signal.SIGTERM, lambda *_: stop.set()) + + 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) + n += 1 + if n % 200 == 0: + logger.info("Applied %d actions", n) + finally: + logger.info("Shutting down onboard controller...") + robot.disconnect() + + +if __name__ == "__main__": + main() diff --git a/src/lerobot/robots/unitree_g1/run_g1_server.py b/src/lerobot/robots/unitree_g1/run_g1_server.py index efe716d44..108520035 100644 --- a/src/lerobot/robots/unitree_g1/run_g1_server.py +++ b/src/lerobot/robots/unitree_g1/run_g1_server.py @@ -26,7 +26,6 @@ Uses JSON for secure serialization instead of pickle. import argparse import base64 -import contextlib import json import threading import time @@ -248,9 +247,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 diff --git a/src/lerobot/robots/unitree_g1/run_g1_teleop_client.py b/src/lerobot/robots/unitree_g1/run_g1_teleop_client.py new file mode 100644 index 000000000..a02f302eb --- /dev/null +++ b/src/lerobot/robots/unitree_g1/run_g1_teleop_client.py @@ -0,0 +1,123 @@ +#!/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 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) + + period = 1.0 / args.fps if args.fps > 0 else 0.0 + n = 0 + try: + while True: + t0 = time.time() + action = teleop.get_action() + try: + sock.send_json(_to_jsonable(action), zmq.NOBLOCK) + except zmq.Again: + pass + n += 1 + if n % 200 == 0: + logger.info("Sent %d actions", n) + 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() diff --git a/src/lerobot/robots/unitree_g1/unitree_g1.py b/src/lerobot/robots/unitree_g1/unitree_g1.py index fbbbd3e03..45209853a 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/unitree_g1.py @@ -126,8 +126,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 @@ -297,8 +299,28 @@ class UnitreeG1(Robot): def configure(self) -> None: pass + 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 @@ -307,6 +329,15 @@ 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() else: self._ChannelFactoryInitialize(0, config=self.config) @@ -315,7 +346,7 @@ class UnitreeG1(Robot): self._gripper_sock = None self._last_gripper_cmd = None self._warned_no_gripper_buttons = False - if not self.config.is_simulation: + if not self.config.is_simulation and not self.config.onboard: try: import zmq @@ -375,12 +406,11 @@ class UnitreeG1(Robot): # Start controller thread if enabled if self.controller is not None: - # Soft-start: ramp the arms from their current pose to the default position - # before the locomotion policy takes over, avoiding a jump at startup. The - # controller owns the legs, so send_action only moves the arms here. Runs in - # both sim and on the real robot so the ramp is visible in MuJoCo too. - logger.info("Soft-start: ramping arms to default position...") - self._interpolate_to_default(duration=3.0) + # 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() @@ -538,6 +568,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 @@ -570,6 +607,29 @@ class UnitreeG1(Robot): 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. diff --git a/src/lerobot/robots/unitree_g1/view_cameras.py b/src/lerobot/robots/unitree_g1/view_cameras.py new file mode 100644 index 000000000..c56c24110 --- /dev/null +++ b/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()