diff --git a/src/lerobot/robots/unitree_g1/config_unitree_g1.py b/src/lerobot/robots/unitree_g1/config_unitree_g1.py index 3fbeb37fe..697271a6b 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_server's serve_onboard_controller, + # 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/run_g1_server.py b/src/lerobot/robots/unitree_g1/run_g1_server.py index b5bd0baf8..7227fa2a5 100644 --- a/src/lerobot/robots/unitree_g1/run_g1_server.py +++ b/src/lerobot/robots/unitree_g1/run_g1_server.py @@ -22,16 +22,33 @@ This server runs on the robot and forwards: - Robot commands (LowCmd) from ZMQ to DDS (from remote clients) Uses JSON for secure serialization instead of pickle. + +Controller-negotiation handshake +-------------------------------- +The first message from a client agrees on which controller the server will run onboard +(``serve_onboard_controller``); the controller NEVER runs on the laptop client. +Test the handshake in isolation (no DDS, runs on a laptop) in two terminals:: + + # terminal A: handshake-only server + python -m lerobot.robots.unitree_g1.run_g1_server --handshake-only + + # terminal B: client proposes a controller + python -m lerobot.robots.unitree_g1.run_g1_server \\ + --handshake-client SonicWholeBodyController --sonic-token-action --server-ip 127.0.0.1 + +On the real robot, add ``--handshake`` to the normal bridge to require agreement first. """ import argparse import base64 import contextlib import json +import signal import threading import time from typing import Any +import numpy as np import zmq from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient from unitree_sdk2py.core.channel import ChannelFactoryInitialize, ChannelPublisher, ChannelSubscriber @@ -50,6 +67,257 @@ LOWCMD_PORT = 6000 LOWSTATE_PORT = 6001 NUM_MOTORS = 35 +# Onboard high-level channels (serve_onboard_controller): compact actions in, state out. +ACTION_PORT = 6004 +STATE_PORT = 6005 + +# Controller-negotiation handshake (REQ/REP). The client's first message agrees on +# which controller the server will run before any control data flows. +HANDSHAKE_PORT = 6002 +PROTOCOL_VERSION = 1 + +# Controllers that can run ONBOARD (must match g1_utils.make_locomotion_controller). +# ``None`` (a.k.a. "bridge") means no onboard controller: the laptop owns control and +# streams raw lowcmd over the ZMQ DDS bridge (the legacy run_g1_server behavior). +VALID_CONTROLLERS = ( + "GrootLocomotionController", + "HolosomaLocomotionController", + "SonicWholeBodyController", +) +# SONIC latent-token dimensionality (mirrors sonic_whole_body.TOKEN_DIM; kept local so +# the handshake can run without importing the heavy controller / onnxruntime). +TOKEN_DIM = 64 +_BRIDGE_ALIASES = {"", "none", "null", "bridge", "raw"} + + +def _normalize_controller(name: str | None) -> str | None: + """Map a requested controller name to a canonical value (or None for raw bridge).""" + if name is None: + return None + low = str(name).strip().lower() + if low in _BRIDGE_ALIASES: + return None + for c in VALID_CONTROLLERS: + if c.lower() == low: + return c + raise ValueError(f"Unknown controller {name!r}. Available: {list(VALID_CONTROLLERS)} or 'bridge'") + + +def _capabilities(controller: str | None, sonic_token_action: bool) -> dict[str, Any]: + """The interface the server advertises for an agreed controller.""" + caps: dict[str, Any] = { + "controller": controller, + "sonic_token_action": bool(sonic_token_action), + "protocol": PROTOCOL_VERSION, + } + if controller is None: + # Raw DDS bridge: the laptop runs the controller and streams lowcmd. + caps["mode"] = "bridge" + caps["lowcmd_port"] = LOWCMD_PORT + caps["lowstate_port"] = LOWSTATE_PORT + else: + # Onboard: the controller runs here; the laptop ships compact high-level actions. + caps["mode"] = "onboard" + caps["action_port"] = ACTION_PORT + caps["state_port"] = STATE_PORT + if sonic_token_action: + caps["action_space"] = "motion_token" + caps["action_dim"] = TOKEN_DIM + return caps + + +def negotiate_controller(sock: zmq.Socket, shutdown_event: threading.Event) -> dict[str, Any]: + """Server side of the handshake: block on one REP socket until a client sends a + valid ``hello``, then reply with the negotiated capabilities and return them. + + Rejects malformed / unknown-controller requests with an error reply and keeps + waiting (a rejected client can retry). Honors ``shutdown_event`` so Ctrl-C works. + """ + poller = zmq.Poller() + poller.register(sock, zmq.POLLIN) + while not shutdown_event.is_set(): + if not dict(poller.poll(timeout=200)): + continue + raw = sock.recv() + try: + hello = json.loads(raw.decode("utf-8")) + except (ValueError, UnicodeDecodeError) as e: + sock.send_json({"type": "error", "ok": False, "error": f"bad hello: {e}"}) + continue + try: + controller = _normalize_controller(hello.get("controller")) + except ValueError as e: + sock.send_json( + {"type": "error", "ok": False, "error": str(e), "available": list(VALID_CONTROLLERS)} + ) + continue + reply = { + "type": "welcome", + "ok": True, + **_capabilities(controller, hello.get("sonic_token_action", False)), + } + sock.send_json(reply) + return reply + raise KeyboardInterrupt + + +def request_controller( + server_ip: str, + controller: str | None, + *, + sonic_token_action: bool = False, + port: int = HANDSHAKE_PORT, + timeout_s: float = 5.0, +) -> dict[str, Any]: + """Client side of the handshake: propose a controller, return the server's agreed + capabilities (or raise on rejection / timeout).""" + ctx = zmq.Context.instance() + sock = ctx.socket(zmq.REQ) + sock.setsockopt(zmq.LINGER, 0) + sock.setsockopt(zmq.RCVTIMEO, int(timeout_s * 1000)) + sock.setsockopt(zmq.SNDTIMEO, int(timeout_s * 1000)) + sock.connect(f"tcp://{server_ip}:{port}") + hello = { + "type": "hello", + "controller": controller, + "sonic_token_action": bool(sonic_token_action), + "protocol": PROTOCOL_VERSION, + } + try: + sock.send_json(hello) + reply = sock.recv_json() + except zmq.Again as e: + raise TimeoutError(f"no handshake reply from {server_ip}:{port} within {timeout_s}s") from e + finally: + sock.close(linger=0) + if not reply.get("ok"): + raise RuntimeError(f"handshake rejected: {reply.get('error')} (available: {reply.get('available')})") + return reply + + +def serve_onboard_controller( + *, + controller: str, + sonic_token_action: bool, + dds_interface: str | None = None, + sim: bool = False, + cameras: dict | None = None, + camera_fps: int = 30, + camera_port: int = 5555, + action_port: int = ACTION_PORT, + state_port: int = STATE_PORT, + state_fps: float = 30.0, + stop: threading.Event | None = None, +) -> None: + """Run the negotiated controller ONBOARD -- the single control path on the robot. + + Builds ``UnitreeG1(onboard=True, controller=...)`` so the controller/balance loop runs + locally against DDS at full rate (the 50 Hz ``_controller_loop`` thread lives in + UnitreeG1), then receives compact high-level actions from the laptop over ZMQ + (:action_port), decodes them via the controller, publishes ``observation.state`` + (:state_port), and optionally serves the ego camera. The controller NEVER runs on the + laptop; the laptop (lerobot-rollout thin-client) only ships tokens/axes and reads back + state + camera frames. + """ + # Imported lazily: UnitreeG1 imports request_controller from this module, so a + # top-level import here would be circular. + from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config + from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1 + + if stop is None: + stop = threading.Event() + signal.signal(signal.SIGINT, lambda *_: stop.set()) + signal.signal(signal.SIGTERM, lambda *_: stop.set()) + + cfg = UnitreeG1Config( + is_simulation=False, + onboard=True, + controller=controller, + dds_interface=dds_interface, + release_motion_control=not sim, + physical_remote=not sim, + cameras={}, + ) + + # Optional camera server (background daemon thread; independent of DDS). + if cameras: + camera_server = ImageServer({"fps": camera_fps, "cameras": cameras}, port=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()) + print(f"Camera server started on :{camera_port}: {cam_summary}") + + robot = UnitreeG1(cfg) + print(f"Connecting onboard robot (controller={controller}, token={sonic_token_action})...") + 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, 200) # keeps the loop responsive to the stop event + sock.bind(f"tcp://0.0.0.0:{action_port}") + print(f"Onboard controller live. Waiting for laptop actions on :{action_port} ...") + print("Ctrl-C for graceful shutdown.") + + state_sock = None + if 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:{state_port}") + print(f"Publishing observation.state on :{state_port} at {state_fps:.0f} Hz") + + def publish_state() -> None: + period = 1.0 / state_fps + while not stop.is_set(): + t0 = time.time() + obs = robot.get_observation() + if obs: + # Forward every scalar proprio key the robot exposes (29 joint .q, IMU, + # and the SONIC token echo: 64-D motion_token_state.*). Camera arrays are + # streamed separately by the ImageServer, so drop ndarrays here. This + # makes the laptop thin-client a pure relay. + state = { + k: float(v) + for k, v in obs.items() + if isinstance(v, (bool, int, float, np.floating, np.integer)) + } + 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: + print("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: + print(f"Dropping malformed action: {e}") + continue + + robot.send_action(action) + + n += 1 + if n % 60 == 0: + print(f"Applied {n} actions") + finally: + print("Shutting down onboard controller...") + stop.set() + if state_sock is not None: + with contextlib.suppress(Exception): + state_sock.close(linger=0) + robot.disconnect() + def lowstate_to_dict(msg: hg_LowState) -> dict[str, Any]: """Convert LowState SDK message to a JSON-serializable dictionary.""" @@ -160,8 +428,111 @@ def main() -> None: 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)") + # Controller-negotiation handshake (first message agrees on the controller). + parser.add_argument( + "--handshake", + action="store_true", + help="Wait for a client to negotiate the controller before bridging", + ) + parser.add_argument( + "--handshake-port", + type=int, + default=HANDSHAKE_PORT, + help=f"Handshake REQ/REP port (default: {HANDSHAKE_PORT})", + ) + parser.add_argument( + "--handshake-only", + action="store_true", + help="Run ONLY the handshake server (no DDS/cameras) to test negotiation", + ) + parser.add_argument( + "--handshake-client", + default=None, + metavar="CONTROLLER", + help="Act as a client: propose CONTROLLER (or 'bridge') to --server-ip and print the reply", + ) + parser.add_argument("--server-ip", default="127.0.0.1", help="[--handshake-client] server IP") + parser.add_argument( + "--sonic-token-action", + action="store_true", + help="[handshake] negotiate the 64-D SONIC token action interface", + ) args = parser.parse_args() + # --- Isolated handshake test paths (no DDS, safe to run on a laptop) --- + if args.handshake_client is not None: + controller = ( + None if args.handshake_client.strip().lower() in _BRIDGE_ALIASES else args.handshake_client + ) + reply = request_controller( + args.server_ip, + controller, + sonic_token_action=args.sonic_token_action, + port=args.handshake_port, + ) + print(json.dumps(reply, indent=2)) + return + + if args.handshake_only: + ctx = zmq.Context.instance() + rep = ctx.socket(zmq.REP) + rep.bind(f"tcp://0.0.0.0:{args.handshake_port}") + print(f"[handshake] server listening on :{args.handshake_port} (no DDS). Ctrl-C to stop.") + shutdown = threading.Event() + try: + while True: + reply = negotiate_controller(rep, shutdown) + print( + f"[handshake] agreed: controller={reply['controller']} mode={reply['mode']} " + f"sonic_token_action={reply['sonic_token_action']}" + ) + except KeyboardInterrupt: + print("\n[handshake] stopping") + finally: + rep.close(linger=0) + ctx.term() + return + + # Controller-negotiation handshake: the client's first message agrees on the + # controller, which we then run ONBOARD (the controller NEVER runs on the laptop). + # Bridge/None falls through to the legacy raw DDS forward (deprecated laptop control). + if args.handshake: + ctx = zmq.Context.instance() + hs = ctx.socket(zmq.REP) + hs.bind(f"tcp://0.0.0.0:{args.handshake_port}") + print(f"[handshake] waiting for client controller agreement on :{args.handshake_port} ...") + shutdown = threading.Event() + try: + agreed = negotiate_controller(hs, shutdown) + except KeyboardInterrupt: + print("[handshake] interrupted before agreement; exiting") + hs.close(linger=0) + ctx.term() + return + hs.close(linger=0) + if agreed["controller"] is not None: + print( + f"[handshake] running controller ONBOARD: {agreed['controller']} " + f"(sonic_token_action={agreed['sonic_token_action']})" + ) + cameras = None + if args.camera: + cameras = { + "head_camera": { + "device_id": args.camera_device, + "shape": [args.camera_height, args.camera_width], + } + } + serve_onboard_controller( + controller=agreed["controller"], + sonic_token_action=bool(agreed["sonic_token_action"]), + cameras=cameras, + camera_fps=args.camera_fps, + camera_port=args.camera_port, + ) + return + print("[handshake] client selected raw DDS bridge (laptop owns control) -> legacy forward.") + # Optionally start camera server in background thread camera_thread = None if args.camera: @@ -205,6 +576,7 @@ def main() -> None: # initialize ZMQ ctx = zmq.Context.instance() + shutdown_event = threading.Event() # receive commands from remote client lowcmd_sock = ctx.socket(zmq.PULL) @@ -215,7 +587,6 @@ def main() -> None: lowstate_sock.bind(f"tcp://0.0.0.0:{LOWSTATE_PORT}") state_period = 0.002 # ~500 hz - shutdown_event = threading.Event() # start observation forwarding in background thread t_state = threading.Thread( diff --git a/src/lerobot/robots/unitree_g1/unitree_g1.py b/src/lerobot/robots/unitree_g1/unitree_g1.py index 58a9085ef..728d3b46d 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/unitree_g1.py @@ -16,6 +16,8 @@ from __future__ import annotations +import contextlib +import json import logging import threading import time @@ -27,6 +29,7 @@ import numpy as np from lerobot.cameras import make_cameras_from_configs from lerobot.types import RobotAction, RobotObservation +from lerobot.utils.errors import DeviceNotConnectedError from lerobot.utils.import_utils import _unitree_sdk_available, require_package from ..robot import Robot @@ -78,6 +81,28 @@ 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: @@ -118,24 +143,34 @@ class UnitreeG1(Robot): self.config = config self.control_dt = config.control_dt + # Three mutually-exclusive roles: + # * simulation : local DDS + controller run in-process against a MuJoCo world. + # * onboard : local DDS + controller run in-process on the robot NX. + # * client : thin laptop client. No DDS, no controller. It negotiates a + # controller with ``run_g1_server`` (which runs it onboard), + # PUSHes high-level actions and reads back state + cameras over + # ZMQ. The controller *always* runs on the robot, never here. + self._client = not config.is_simulation and not config.onboard + # 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: + # DDS channels are only needed by the in-process control roles (sim / onboard), + # which both drive the real Unitree SDK. The thin client never touches DDS. + if config.is_simulation or config.onboard: self._ChannelFactoryInitialize = _SDKChannelFactoryInitialize self._ChannelPublisher = _SDKChannelPublisher self._ChannelSubscriber = _SDKChannelSubscriber else: - from .unitree_sdk2_socket import ( - ChannelFactoryInitialize, - ChannelPublisher, - ChannelSubscriber, - ) + self._ChannelFactoryInitialize = None + self._ChannelPublisher = None + self._ChannelSubscriber = None - self._ChannelFactoryInitialize = ChannelFactoryInitialize - self._ChannelPublisher = ChannelPublisher - self._ChannelSubscriber = ChannelSubscriber + # Client-side ZMQ handles / negotiated capabilities (populated in connect()). + self._client_action_sock = None + self._client_state_sock = None + self._client_state_latest: dict[str, float] = {} + self._client_caps: dict | None = None # Initialize state variables self.sim_env = None @@ -147,14 +182,33 @@ class UnitreeG1(Robot): self.arm_ik = G1_29_ArmIK() if config.gravity_compensation else None - # Controller loaded dynamically - self.controller: LocomotionController | None = make_locomotion_controller(config.controller) + # Controller loaded dynamically. GUARDRAIL: the controller must never be built or + # run on the laptop client -- it always runs onboard (or in sim). + if self._client: + self.controller: LocomotionController | None = None + else: + self.controller = make_locomotion_controller(config.controller) # Controller thread state self._controller_thread = None self._controller_action_lock = threading.Lock() 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 + + @property + def _sonic_token(self) -> bool: + """Whether the SONIC whole-body decoder is active. + + A SONIC controller consumes a 64-D latent motion token as its action and echoes + the last commanded token as ``observation.state``. Keyed purely off the selected + controller so the token interface is implicit -- no separate config flag, and the + thin client (which has no controller instance) can still advertise the schema. + """ + return self.config.controller == "SonicWholeBodyController" + def _subscribe_lowstate(self): # polls robot state @ 250Hz while not self._shutdown_event.is_set(): start_time = time.time() @@ -232,19 +286,33 @@ class UnitreeG1(Robot): @cached_property def observation_features(self) -> dict[str, type | tuple]: # Controllers may contribute their own proprio features (e.g. SONIC's token state). + # The thin client has no controller instance, so mirror the onboard token schema + # by controller name (SONIC echoes its last token as observation.state). controller_ft = getattr(self.controller, "observation_ft", {}) + if self._client and self._sonic_token: + from .controllers.sonic_whole_body import TOKEN_DIM, TOKEN_STATE_PREFIX + + controller_ft = {f"{TOKEN_STATE_PREFIX}.{i}.pos": float for i in range(TOKEN_DIM)} return {**self._motors_ft, **controller_ft, **self._cameras_ft} @cached_property def action_features(self) -> dict[str, type]: + # Role-agnostic: the schema is a pure function of the configured controller name, + # so the thin client advertises the same action space as the onboard robot. + # No controller configured at all: raw 29-DoF joint teleop. - if self.controller is None: + if self.config.controller is None: return {f"{G1_29_JointIndex(motor).name}.q": float for motor in G1_29_JointIndex} - # Whole-body controllers (SONIC): 64-D latent token. + # Whole-body controllers (SONIC): 64-D latent token. On the thin client there is + # no controller instance, so advertise the same token schema by controller name. controller_ft = getattr(self.controller, "action_ft", None) if controller_ft is not None: return dict(controller_ft) + if self._client and self._sonic_token: + from .controllers.sonic_whole_body import TOKEN_ACTION_PREFIX, TOKEN_DIM + + return {f"{TOKEN_ACTION_PREFIX}.{i}.pos": float for i in range(TOKEN_DIM)} # Locomotion controllers (GR00T / Holosoma): arm joint targets + joystick axes. # TODO: have GR00T/Holosoma advertise their own action_features too, so every @@ -280,6 +348,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) @@ -302,7 +377,170 @@ 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) + + # ------------------------------------------------------------------ # + # Thin-client role (laptop): no DDS, no controller. Talks to run_g1_server + # over ZMQ. The controller ALWAYS runs onboard; we only relay high-level + # actions and read back the state echo + camera frames. + # ------------------------------------------------------------------ # + def _connect_client(self) -> None: + import zmq + + from .run_g1_server import ACTION_PORT, HANDSHAKE_PORT, STATE_PORT, request_controller + + server_ip = self.config.robot_ip + if not server_ip: + raise ValueError("client mode requires config.robot_ip (the G1 running run_g1_server)") + + # 1) Handshake: agree with the server on which controller it will run onboard. + logger.info( + "[client] handshaking with %s:%d (controller=%s, token=%s)...", + server_ip, + HANDSHAKE_PORT, + self.config.controller, + self._sonic_token, + ) + self._client_caps = request_controller( + server_ip, + self.config.controller, + sonic_token_action=self._sonic_token, + port=HANDSHAKE_PORT, + ) + logger.info("[client] server agreed: %s", self._client_caps) + + ctx = zmq.Context.instance() + + # 2) Action PUSH: ship compact high-level actions to the onboard controller. + self._client_action_sock = ctx.socket(zmq.PUSH) + self._client_action_sock.setsockopt(zmq.SNDHWM, 2) + self._client_action_sock.setsockopt(zmq.LINGER, 0) + self._client_action_sock.connect(f"tcp://{server_ip}:{ACTION_PORT}") + + # 3) State SUB: read the onboard observation.state echo (last token / joints). + self._client_state_sock = ctx.socket(zmq.SUB) + self._client_state_sock.setsockopt(zmq.CONFLATE, 1) + self._client_state_sock.setsockopt_string(zmq.SUBSCRIBE, "") + self._client_state_sock.connect(f"tcp://{server_ip}:{STATE_PORT}") + + # 4) Cameras (ZMQ ImageServer served by run_g1_server) - same as any client. + for cam in self._cameras.values(): + if not cam.is_connected: + cam.connect() + logger.info( + "[client] connected: actions ->:%d, state <-:%d, %d camera(s).", + ACTION_PORT, + STATE_PORT, + len(self._cameras), + ) + + def _recv_client_state(self) -> None: + """Drain the state SUB (CONFLATE keeps only the freshest) into the latest cache.""" + import zmq + + if self._client_state_sock is None: + return + while True: + try: + state = self._client_state_sock.recv_json(flags=zmq.NOBLOCK) + except zmq.Again: + break + except (ValueError, zmq.ZMQError): + break + if isinstance(state, dict): + self._client_state_latest = {k: float(v) for k, v in state.items()} + + def _get_observation_client(self) -> RobotObservation: + self._recv_client_state() + obs: dict = dict(self._client_state_latest) + for cam_name, cam in self._cameras.items(): + if getattr(cam, "use_rgb", True): + obs[cam_name] = cam.read_latest() + if getattr(cam, "use_depth", False): + obs[f"{cam_name}_depth"] = cam.read_latest_depth() + return obs + + def _send_action_client(self, action: RobotAction) -> RobotAction: + """Relay the raw action straight to the onboard controller. NO processing here: + the controller negotiated in the handshake interprets it (token / wb / arm).""" + import zmq + + if self._client_action_sock is None: + raise DeviceNotConnectedError("UnitreeG1 client is not connected") + payload = json.dumps({k: float(v) for k, v in action.items()}).encode("utf-8") + with contextlib.suppress(zmq.Again): + self._client_action_sock.send(payload, zmq.NOBLOCK) + return action + + def _disconnect_client(self) -> None: + for sock in (self._client_action_sock, self._client_state_sock): + if sock is not None: + with contextlib.suppress(Exception): + sock.close(linger=0) + self._client_action_sock = None + self._client_state_sock = None + for cam in self._cameras.values(): + with contextlib.suppress(Exception): + cam.disconnect() + def connect(self, calibrate: bool = True) -> None: # connect to DDS + # Thin-client role: no DDS, no controller. Negotiate the controller with + # run_g1_server (which runs it onboard), then open the high-level ZMQ links: + # PUSH actions on :ACTION_PORT, SUB state echo on :STATE_PORT, cameras via ZMQ. + if self._client: + self._connect_client() + return + # Initialize DDS channel and simulation environment if self.config.is_simulation: from lerobot.envs import make_env @@ -311,8 +549,28 @@ 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] - else: - self._ChannelFactoryInitialize(0, config=self.config) + 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 # Initialize direct motor control interface self.lowcmd_publisher = self._ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) @@ -392,6 +650,10 @@ class UnitreeG1(Robot): logger.warning(f"Failed to send zero-torque on disconnect: {e}") def disconnect(self): + if self._client: + self._disconnect_client() + return + # Put robot in passive mode before stopping threads if not self.config.is_simulation: self._send_zero_torque() @@ -437,6 +699,9 @@ class UnitreeG1(Robot): cam.disconnect() def get_observation(self) -> RobotObservation: + if self._client: + return self._get_observation_client() + with self._lowstate_lock: lowstate = self._lowstate if lowstate is None: @@ -496,6 +761,9 @@ class UnitreeG1(Robot): return obs def send_action(self, action: RobotAction) -> RobotAction: + if self._client: + return self._send_action_client(action) + action_to_publish = action if self.controller is not None: # Controller thread owns legs/waist. Here we only update joystick inputs @@ -541,6 +809,8 @@ class UnitreeG1(Robot): @property def is_connected(self) -> bool: + if self._client: + return self._client_action_sock is not None with self._lowstate_lock: return self._lowstate is not None