From b587e815872bd6b3c5949ba2e1c708dd9f72b37e Mon Sep 17 00:00:00 2001 From: Martino Russi Date: Sun, 26 Jul 2026 21:36:02 +0200 Subject: [PATCH] feat(unitree_g1): onboard controller deployment for SONIC walk Run the whole-body controller (SONIC decoder / GR00T) onboard the G1 against local DDS at full rate, with the laptop shipping only high-level actions over ZMQ instead of 50Hz lowcmd via the socket bridge. - config: add onboard, dds_interface, release_motion_control, physical_remote - unitree_g1: onboard connect() branch (local DDS + MotionSwitcher release + physical wireless remote), _release_motion_control, _wireless_remote_input, controller-loop wireless priority; SDK channels when sim OR onboard - run_g1_server: port Gripper/build_gripper/parse_camera_specs; add --cameras spec supporting by-path device names (survive USB re-enumeration) + FOURCC - run_g1_onboard: onboard entry point (ZMQ actions -> send_action), with a --sonic-token-action flag for the 64-D latent-token interface - infer_sonic_g1_onboard: laptop-side sender that runs nepyope/sonic_walk (pi0.5) and PUSHes 64-D tokens to the onboard controller --- .../robots/unitree_g1/config_unitree_g1.py | 18 ++ .../unitree_g1/infer_sonic_g1_onboard.py | 187 +++++++++++++ .../robots/unitree_g1/run_g1_onboard.py | 251 ++++++++++++++++++ .../robots/unitree_g1/run_g1_server.py | 134 +++++++++- src/lerobot/robots/unitree_g1/unitree_g1.py | 99 ++++++- 5 files changed, 674 insertions(+), 15 deletions(-) create mode 100644 src/lerobot/robots/unitree_g1/infer_sonic_g1_onboard.py create mode 100644 src/lerobot/robots/unitree_g1/run_g1_onboard.py diff --git a/src/lerobot/robots/unitree_g1/config_unitree_g1.py b/src/lerobot/robots/unitree_g1/config_unitree_g1.py index 298d60e03..818cb68b7 100644 --- a/src/lerobot/robots/unitree_g1/config_unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/config_unitree_g1.py @@ -62,6 +62,24 @@ class UnitreeG1Config(RobotConfig): # Socket config for ZMQ bridge robot_ip: str = "192.168.123.164" # default G1 IP + # Run the locomotion / whole-body controller ONBOARD the robot (policy on the G1 + # itself, against local DDS at full rate) instead of on the laptop over the ZMQ + # socket bridge. In this mode the robot object uses the real Unitree SDK channels + # and expects high-level actions (arm targets + joystick axes, or 64-D SONIC + # tokens) fed via send_action -- e.g. by run_g1_onboard.py, which receives them + # from the laptop over ZMQ. 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 + # Onboard sub-flags. On a real G1 both are True: the built-in motion services + # must be released before we can write lowcmd, and locomotion axes are read from + # the physical wireless remote. Against a DDS sim neither applies (no + # MotionSwitcher, no physical remote), so set both False so the controller takes + # its locomotion axes purely from send_action (ZMQ) input. + release_motion_control: bool = True + physical_remote: bool = True + # Cameras (ZMQ-based remote cameras) cameras: dict[str, CameraConfig] = field(default_factory=dict) diff --git a/src/lerobot/robots/unitree_g1/infer_sonic_g1_onboard.py b/src/lerobot/robots/unitree_g1/infer_sonic_g1_onboard.py new file mode 100644 index 000000000..657366b40 --- /dev/null +++ b/src/lerobot/robots/unitree_g1/infer_sonic_g1_onboard.py @@ -0,0 +1,187 @@ +#!/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 sender for the SONIC whole-body walk policy, onboard deployment. + +This is the counterpart to ``run_g1_onboard.py`` (which runs the SONIC decoder on the +robot). The heavy VLA (``nepyope/sonic_walk``, a pi0.5 token policy) runs here on the +laptop GPU; only the resulting 64-D latent token is shipped to the robot over ZMQ: + + laptop: camera frame (ZMQ from robot :5555) + previous token + -> pi0.5 -> next 64-D token + -> PUSH JSON {motion_token.i.pos: ...} to robot :6004 + robot: run_g1_onboard receives the token, SonicWholeBodyController decodes it + into whole-body joint commands against local DDS at full rate. + +The policy's ``observation.state`` is the token currently being executed, so we close +the loop by feeding back the *last token we sent* (the decoder holds it until a new one +arrives). This mirrors what ``lerobot-rollout`` does via the robot's token echo, but +without a controller / DDS on the laptop. + +The policy is pi0.5 with chunk_size=50, so a full diffusion inference runs only about +once every 50 ticks; ``select_action`` pops one queued token per tick in between. + +Run ``run_g1_onboard.py --controller SonicWholeBodyController --sonic-token-action +--cameras ...`` on the robot first, then this on the laptop: + + python -m lerobot.robots.unitree_g1.infer_sonic_g1_onboard \ + --policy-path nepyope/sonic_walk --robot-ip 192.168.123.164 \ + --task "walk back and forth" +""" + +import argparse +import contextlib +import json +import logging +import signal +import time + +import numpy as np +import torch + +from lerobot.cameras.zmq import ZMQCamera, ZMQCameraConfig +from lerobot.configs.policies import PreTrainedConfig +from lerobot.policies.factory import get_policy_class, make_pre_post_processors +from lerobot.policies.utils import prepare_observation_for_inference +from lerobot.robots.unitree_g1.controllers.sonic_whole_body import TOKEN_DIM, token_action_key + +logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s", force=True) +logger = logging.getLogger("sonic_sender") + +ACTION_PORT = 6004 # matches run_g1_onboard.py --action-port +IMAGE_KEY = "observation.images.ego_view" # pi05 sonic_walk VISUAL input +STATE_KEY = "observation.state" + + +def main() -> None: + p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + p.add_argument("--policy-path", default="nepyope/sonic_walk", help="Policy repo id or local path") + p.add_argument("--robot-ip", default="192.168.123.164", help="Robot IP (camera + action ports)") + p.add_argument("--action-port", type=int, default=ACTION_PORT, help="Onboard ZMQ PULL port for actions") + p.add_argument("--camera-port", type=int, default=5555, help="Onboard ZMQ camera PUB port") + p.add_argument("--camera-name", default="head_camera", help="Camera name served by run_g1_onboard") + p.add_argument("--camera-width", type=int, default=640, help="Camera width") + p.add_argument("--camera-height", type=int, default=480, help="Camera height") + p.add_argument("--task", default="walk back and forth", help="Language prompt for the VLA") + p.add_argument("--fps", type=float, default=30.0, help="Token send rate (matches training inference)") + p.add_argument("--device", default="cuda", help="Torch device") + p.add_argument("--max-ticks", type=int, default=0, help="Stop after N ticks (0 = run forever)") + p.add_argument("--dry-run", action="store_true", help="Run inference but do not PUSH tokens to the robot") + args = p.parse_args() + + device = torch.device(args.device) + + # --- Policy + processors (normalization stats baked into the checkpoint) --- + logger.info("Loading policy from '%s'...", args.policy_path) + policy_cfg = PreTrainedConfig.from_pretrained(args.policy_path) + policy_cfg.pretrained_path = args.policy_path + policy = get_policy_class(policy_cfg.type).from_pretrained(args.policy_path, config=policy_cfg) + policy = policy.to(device) + policy.eval() + policy.reset() + + preprocessor, postprocessor = make_pre_post_processors( + policy_cfg=policy_cfg, + pretrained_path=args.policy_path, + preprocessor_overrides={"device_processor": {"device": str(device)}}, + ) + logger.info("Policy loaded (type=%s, device=%s, chunk=%s)", policy_cfg.type, device, + getattr(policy_cfg, "chunk_size", "?")) + + # --- Camera (ZMQ from the robot's onboard image server) --- + cam = ZMQCamera( + ZMQCameraConfig( + server_address=args.robot_ip, + port=args.camera_port, + camera_name=args.camera_name, + width=args.camera_width, + height=args.camera_height, + fps=int(args.fps), + ) + ) + logger.info("Connecting camera %s@%s:%d ...", args.camera_name, args.robot_ip, args.camera_port) + cam.connect() + + # --- Action PUSH socket to the onboard controller --- + import zmq + + ctx = zmq.Context.instance() + sock = ctx.socket(zmq.PUSH) + sock.setsockopt(zmq.SNDHWM, 2) + sock.setsockopt(zmq.LINGER, 0) + sock.connect(f"tcp://{args.robot_ip}:{args.action_port}") + logger.info("Sending tokens to tcp://%s:%d (dry_run=%s)", args.robot_ip, args.action_port, args.dry_run) + + stop = {"flag": False} + signal.signal(signal.SIGINT, lambda *_: stop.__setitem__("flag", True)) + signal.signal(signal.SIGTERM, lambda *_: stop.__setitem__("flag", True)) + + # observation.state = the token currently executing on the robot (last one we sent); + # start at zeros, matching the decoder's zero-seeded initial state. + prev_token = np.zeros(TOKEN_DIM, dtype=np.float32) + period = 1.0 / args.fps + n = 0 + t_infer_total = 0.0 + logger.info("Streaming tokens at %.0f Hz. Ctrl-C to stop.", args.fps) + try: + while not stop["flag"]: + t0 = time.time() + try: + frame = cam.read() # HxWxC uint8 RGB + except Exception as e: # noqa: BLE001 + logger.warning("Camera read failed: %s", e) + time.sleep(period) + continue + + raw_obs = { + IMAGE_KEY: np.ascontiguousarray(frame), + STATE_KEY: prev_token.copy(), + } + with torch.inference_mode(): + obs = prepare_observation_for_inference(raw_obs, device, args.task, "unitree_g1") + obs = preprocessor(obs) + action = policy.select_action(obs) + action = postprocessor(action) + token = action.squeeze(0).to("cpu").numpy().astype(np.float32) + prev_token = token + + if not args.dry_run: + msg = {token_action_key(i): float(token[i]) for i in range(TOKEN_DIM)} + with contextlib.suppress(zmq.Again): + sock.send_string(json.dumps(msg), zmq.NOBLOCK) + + n += 1 + t_infer_total += time.time() - t0 + if n % 30 == 0: + logger.info( + "tick %d | avg %.1f ms/tick | token[:3]=%s", + n, 1000.0 * t_infer_total / 30.0, np.round(token[:3], 3).tolist(), + ) + t_infer_total = 0.0 + + if args.max_ticks and n >= args.max_ticks: + break + time.sleep(max(0.0, period - (time.time() - t0))) + finally: + logger.info("Stopping sender after %d ticks.", n) + with contextlib.suppress(Exception): + cam.disconnect() + with contextlib.suppress(Exception): + sock.close(linger=0) + + +if __name__ == "__main__": + main() 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..bd2f99cba --- /dev/null +++ b/src/lerobot/robots/unitree_g1/run_g1_onboard.py @@ -0,0 +1,251 @@ +#!/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 / whole-body controller ONBOARD, driven by high-level actions +from a laptop. + +The controller (GR00T / Holosoma / SONIC whole-body) runs on the robot itself against +local DDS, at full control rate. The laptop ships only the resulting high-level action +(arm joint targets + joystick axes + gripper flags, or a 64-D SONIC motion token) as +JSON over ZMQ. This process applies each action via ``UnitreeG1.send_action`` while the +onboard controller thread keeps the legs balanced / decodes the token. + +This is the real-deploy counterpart to running ``lerobot-rollout`` on the laptop with +``--robot.is_simulation=false`` (the ZMQ *socket bridge*): there the 50 Hz lowcmd +crosses the network; here only compact high-level actions do, and the control loop stays +local to the robot. Pair with a laptop client that produces actions (exo teleop, or a +policy such as ``nepyope/sonic_walk`` emitting ``motion_token.{i}.pos``). + +Besides receiving actions, this process publishes ``observation.state`` (29 joint ``.q``) +on a ZMQ PUB port so a laptop policy client has proprioception. + +Safety: type ``e`` then Enter in this terminal to stop immediately (zero-torque + exit). +Ctrl-C does the normal graceful shutdown (kp ramp). + +Examples (on the robot): + + # GR00T locomotion, arm targets from the laptop: + python -m lerobot.robots.unitree_g1.run_g1_onboard --controller GrootLocomotionController + + # SONIC whole-body walk policy: laptop ships 64-D tokens, decoder runs here: + python -m lerobot.robots.unitree_g1.run_g1_onboard \ + --controller SonicWholeBodyController --sonic-token-action \ + --cameras "head_camera:/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.3-video-index0:640x480" +""" + +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="Controller class name") + p.add_argument("--dds-interface", default=None, help="DDS network interface (default: SDK default)") + p.add_argument( + "--sim", + action="store_true", + help="Attach to a DDS MuJoCo sim: skip MotionSwitcher + physical remote, default dds-interface 'lo'.", + ) + p.add_argument( + "--sonic-token-action", + action="store_true", + help="SONIC token interface: actions carry a 64-D motion_token.{i}.pos that the decoder consumes.", + ) + p.add_argument("--action-port", type=int, default=ACTION_PORT, help="ZMQ PULL port for laptop actions") + p.add_argument("--state-port", type=int, default=STATE_PORT, help="ZMQ PUB port for observation.state") + p.add_argument("--state-fps", type=float, default=30.0, help="observation.state publish rate; <=0 disables") + p.add_argument("--gravity-compensation", action="store_true", help="Enable arm gravity compensation") + # Gripper control (Damiao over CAN). + p.add_argument("--grippers", action="store_true", help="Drive Damiao grippers from action L3/R3 flags") + 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)") + p.set_defaults(gripper_fd=True) + # Optional camera streaming (ZMQ) so the laptop policy client / viewer can connect. + 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() + + dds_interface = args.dds_interface + if args.sim and dds_interface is None: + dds_interface = "lo" + + cfg = UnitreeG1Config( + is_simulation=False, + onboard=True, + controller=args.controller, + dds_interface=dds_interface, + gravity_compensation=args.gravity_compensation, + release_motion_control=not args.sim, + physical_remote=not args.sim, + sonic_token_action=args.sonic_token_action, + cameras={}, + ) + + # Optional camera server (background thread; independent of DDS/CAN). + 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, token=%s)...", args.controller, args.sonic_token_action) + robot.connect() + + 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()) + + 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 controller loop publishing + time.sleep(0.05) + 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 a laptop + # inference client can feed it to a policy. DDS stays local; only compact JSON + # state crosses the network. (For a token policy the laptop closes the loop on the + # token instead, but publishing joint state is harmless and useful for logging.) + 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} + 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)") + + 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) + + 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")} + logger.info("Applied %d actions | axes=%s", n, axes) + finally: + logger.info("Shutting down onboard controller...") + stop.set() + if state_sock is not None: + with contextlib.suppress(Exception): + state_sock.close(linger=0) + if camera_server is not None: + with contextlib.suppress(Exception): + camera_server.stop() + for g in grippers.values(): + with contextlib.suppress(Exception): + g.bus.disconnect() + 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 b5bd0baf8..f769f5d5d 100644 --- a/src/lerobot/robots/unitree_g1/run_g1_server.py +++ b/src/lerobot/robots/unitree_g1/run_g1_server.py @@ -28,9 +28,11 @@ 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 +43,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 @@ -51,6 +56,105 @@ LOWSTATE_PORT = 6001 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:6,left_wrist:0``. ``device`` may be an integer index or an explicit + device path (e.g. ``/dev/video6``), including stable ``by-path`` names like + ``/dev/v4l/by-path/platform-...:2.1:1.3-video-index0`` which survive USB + re-enumeration (unlike bare ``/dev/videoN`` indices). Because a by-path name + itself contains colons, the optional ``WxH`` and ``FOURCC`` are parsed from the + *right* so the device-path colons 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(":")] + + 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") + 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 = [] @@ -155,7 +259,11 @@ def main() -> None: """Main entry point for the robot server bridge.""" 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("--camera-device", default="4", + help="Camera device: index or /dev/video path or by-path name (default: 4)") + parser.add_argument("--cameras", default=None, + help="Multi-camera spec 'name:device[:WxH[:FOURCC]]', comma-separated. Overrides " + "--camera-device; device may be a by-path name to survive USB re-enumeration.") 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)") @@ -164,20 +272,20 @@ def main() -> None: # Optionally start camera server in background thread camera_thread = None - if args.camera: - camera_config = { - "fps": args.camera_fps, - "cameras": { - "head_camera": { - "device_id": args.camera_device, - "shape": [args.camera_height, args.camera_width], - } - }, - } + if args.camera or args.cameras: + if args.cameras: + cameras = parse_camera_specs(args.cameras, args.camera_width, args.camera_height) + else: + # Single camera; accept an int index or a device/by-path string. + dev = args.camera_device + dev = int(dev) if str(dev).lstrip("-").isdigit() else dev + cameras = {"head_camera": {"device_id": dev, "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"{n}(dev {c['device_id']})" for n, c in cameras.items()) + print(f"Camera server started on port {args.camera_port}: {cam_summary}") # initialize DDS ChannelFactoryInitialize(0) diff --git a/src/lerobot/robots/unitree_g1/unitree_g1.py b/src/lerobot/robots/unitree_g1/unitree_g1.py index 45ed147b8..a6ed98f32 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/unitree_g1.py @@ -87,6 +87,14 @@ class LocomotionController(Protocol): kTopicLowCommand_Debug = "rt/lowcmd" kTopicLowState = "rt/lowstate" +# Wireless-remote button byte layout, mapped to the positional button indices the +# locomotion controllers expect. Used in onboard mode to read the physical Unitree +# remote from lowstate (mirrors the exo teleoperator's RemoteController). +_REMOTE_BUTTON_MAP: list[str] = [ + "RB", "LB", "start", "back", "RT", "LT", "", "", + "A", "B", "X", "Y", "up", "right", "down", "left", +] + @dataclass class MotorState: @@ -130,8 +138,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 @@ -168,6 +178,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 + # Replay-camera state: keep the encoded (raw) cells per camera and decode # frames lazily as the play cursor advances, with a small frame cache, so we # don't materialize gigabytes of decoded RGB at construction time. @@ -421,6 +435,13 @@ class UnitreeG1(Robot): 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/ZMQ axes stand. + if self.config.onboard: + wl = self._wireless_remote_input(lowstate) + if wl is not None: + controller_input.update(wl) + # Run controller step controller_action = self.controller.run_step(controller_input, lowstate) @@ -443,6 +464,58 @@ 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 + (ZMQ) axes keep control; otherwise the physical remote takes priority. + """ + 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 # Initialize DDS channel and simulation environment if self.config.is_simulation: @@ -468,6 +541,28 @@ class UnitreeG1(Robot): self._env_wrapper = _normalize_hub_result(raw) # 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) + # Real robot: hand low-level control over from the built-in services. + # A DDS sim has no MotionSwitcher, so this is skipped there. + if self.config.release_motion_control: + self._release_motion_control() + # Real robot: read the physical wireless remote from lowstate for + # locomotion. A sim has no physical remote, so leave _joystick=None and + # let send_action (ZMQ) drive the locomotion axes instead. + if self.config.physical_remote: + from unitree_sdk2py.utils.joystick import Joystick + + 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)