diff --git a/src/lerobot/robots/unitree_g1/config_unitree_g1.py b/src/lerobot/robots/unitree_g1/config_unitree_g1.py index a41bd2a61..917f90dfa 100644 --- a/src/lerobot/robots/unitree_g1/config_unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/config_unitree_g1.py @@ -86,11 +86,6 @@ class UnitreeG1Config(RobotConfig): # Compensates for gravity on the unitree's arms using the arm ik solver gravity_compensation: bool = False - # When False, connect() does not start the background controller thread, so a - # caller can drive the controller synchronously (one decode per fed action), - # reproducing the deploy's single 50Hz control clock for faithful replay. - run_controller_thread: bool = True - # Token-output VLA interface for the SONIC decoder. When True (and the controller # is ``SonicWholeBodyController``), the robot advertises a 64-D latent-token action # space (``motion_token.{i}.pos``) and exposes the last commanded token as a 64-D @@ -103,8 +98,3 @@ class UnitreeG1Config(RobotConfig): # Locomotion controller class name, e.g. "GrootLocomotionController", # "HolosomaLocomotionController", or "SonicWholeBodyController". None disables it. controller: str | None = None - - # On disconnect (e.g. Ctrl-C), seconds to hold the current pose while ramping joint - # stiffness (kp) to zero — a soft, damped settle instead of an instant limp / - # free-fall. 0 disables it (immediate zero-torque). Real robot only. - graceful_stop_s: float = 1.5 diff --git a/src/lerobot/robots/unitree_g1/run_g1_server.py b/src/lerobot/robots/unitree_g1/run_g1_server.py index 95afeb172..1f134bcfa 100644 --- a/src/lerobot/robots/unitree_g1/run_g1_server.py +++ b/src/lerobot/robots/unitree_g1/run_g1_server.py @@ -43,7 +43,6 @@ import argparse import base64 import contextlib import json -import re import signal import threading import time @@ -237,8 +236,7 @@ def serve_onboard_controller( cameras={}, ) - # Optional camera server (background thread; independent of DDS). - camera_server = None + # 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() @@ -315,57 +313,9 @@ def serve_onboard_controller( 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() robot.disconnect() -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 = [] @@ -470,11 +420,7 @@ 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", 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-device", type=int, default=4, help="Camera device ID (default: 4)") 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)") @@ -542,13 +488,13 @@ def main() -> None: print(f"[handshake] running controller ONBOARD: {agreed['controller']} " f"(sonic_token_action={agreed['sonic_token_action']})") cameras = None - if args.camera or args.cameras: - if args.cameras: - cameras = parse_camera_specs(args.cameras, args.camera_width, args.camera_height) - else: - 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]}} + 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"]), @@ -561,20 +507,20 @@ def main() -> None: # Optionally start camera server in background thread camera_thread = None - 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} + if args.camera: + camera_config = { + "fps": args.camera_fps, + "cameras": { + "head_camera": { + "device_id": args.camera_device, + "shape": [args.camera_height, args.camera_width], + } + }, + } camera_server = ImageServer(camera_config, port=args.camera_port) camera_thread = threading.Thread(target=camera_server.run, daemon=True) camera_thread.start() - 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}") + print(f"Camera server started on port {args.camera_port} (device {args.camera_device})") # 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 52cd4ff7d..7b3d34df2 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/unitree_g1.py @@ -650,16 +650,13 @@ class UnitreeG1(Robot): self.msg.motor_cmd[joint].kd = self.kd[joint.value] self.msg.motor_cmd[joint].q = lowstate.motor_state[joint.value].q - # Start controller thread if enabled. Skipped when run_controller_thread is - # False so a caller can step the controller synchronously (faithful replay). - if self.controller is not None and self.config.run_controller_thread: + # Start the 50 Hz controller thread (runs the locomotion/whole-body policy and + # publishes low commands to DDS). + if self.controller is not None: self._controller_thread = threading.Thread(target=self._controller_loop, daemon=True) self._controller_thread.start() fps = int(1.0 / self.controller.control_dt) logger.info(f"Controller thread started ({fps}Hz)") - elif self.controller is not None: - logger.info("Controller thread disabled (run_controller_thread=False); " - "caller must drive controller.run_step synchronously.") def _send_zero_torque(self) -> None: """Send a zero-gain command to make joints passive before shutting down.""" @@ -675,35 +672,6 @@ class UnitreeG1(Robot): except Exception as e: logger.warning(f"Failed to send zero-torque on disconnect: {e}") - def _graceful_stop(self) -> None: - """Soft shutdown: hold the current pose and ramp joint stiffness (kp) to zero - over ``graceful_stop_s`` while keeping damping (kd), then go passive. - - Prevents the robot from collapsing the instant control ends (a bare - zero-torque command is kp=kd=0 ≈ free-fall). Must run after the controller - loop has stopped so the two aren't publishing at once. - """ - if self.config.graceful_stop_s <= 0: - self._send_zero_torque() - return - with self._lowstate_lock: - lowstate = self._lowstate - if lowstate is None: - self._send_zero_torque() - return - q_hold = {f"{motor.name}.q": lowstate.motor_state[motor.value].q for motor in G1_29_JointIndex} - kp = np.array(self.kp, dtype=np.float32) - kd = np.array(self.kd, dtype=np.float32) - zeros = np.zeros(29, dtype=np.float32) - dt = self.controller.control_dt if self.controller is not None else self.config.control_dt - steps = max(1, int(self.config.graceful_stop_s / dt)) - logger.info("Graceful stop: damping down over %.1fs", self.config.graceful_stop_s) - for i in range(steps): - ratio = (i + 1) / steps - self.publish_lowcmd(q_hold, kp=kp * (1.0 - ratio), kd=kd, tau=zeros) - time.sleep(dt) - self._send_zero_torque() - def disconnect(self): if self._client: self._disconnect_client() @@ -724,11 +692,12 @@ class UnitreeG1(Robot): "concurrent low commands (fail-safe: joints keep last command until exit)" ) - # Soft, damped settle instead of an instant limp (real robot only; the - # subscribe thread is still alive here to supply the current pose). Only ramp - # once the controller thread has definitely exited. + # Put the robot in passive mode (zero-torque) before stopping the rest (real + # robot only; the subscribe thread is still alive here to supply the current + # pose). Only publish once the controller thread has definitely exited so the + # two aren't publishing at once. if not self.config.is_simulation and controller_stopped: - self._graceful_stop() + self._send_zero_torque() if self.controller is not None and hasattr(self.controller, "shutdown"): self.controller.shutdown()