diff --git a/src/lerobot/robots/unitree_g1/run_g1_onboard.py b/src/lerobot/robots/unitree_g1/run_g1_onboard.py index 2db8832cf..a60cb921d 100644 --- a/src/lerobot/robots/unitree_g1/run_g1_onboard.py +++ b/src/lerobot/robots/unitree_g1/run_g1_onboard.py @@ -31,11 +31,8 @@ on a ZMQ PUB port so the laptop policy client has proprioception, and applies ab height / torso orientation from the action dict (``groot.height`` / ``groot.rpy.*``) onto the controller (the joystick interface can only nudge height, not set it absolutely). -Safety: an ``{"halt": true}`` action (sent by the laptop client on Ctrl-C) engages an instant -safe-hold — zero locomotion velocity and arms frozen at their current pose, with no soft-stop -ramp. A watchdog does the same automatically if no action arrives within ``ACTION_TIMEOUT_S`` -(so a client crash or network drop also stops the robot). The robot stays standing/balanced; -Ctrl-C this process to fully power down. +Safety: type ``e`` then Enter in this terminal to stop immediately (skips the soft-stop arm +ramp: goes straight to zero-torque and exits). Ctrl-C does the normal graceful shutdown. Examples (on the robot): @@ -50,6 +47,7 @@ import argparse import json import logging import signal +import sys import threading import time @@ -58,7 +56,7 @@ 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_JointArmIndex, G1_29_JointIndex +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 @@ -68,28 +66,6 @@ logger = logging.getLogger("g1_onboard") ACTION_PORT = 6004 STATE_PORT = 6005 -# E-stop watchdog: if no fresh laptop action arrives within this window (Ctrl-C on -# the client, a crash, or a network drop), engage a safe-hold so the robot never -# keeps coasting on a stale velocity command. -ACTION_TIMEOUT_S = 0.4 - - -def _safe_hold_cmd(robot: UnitreeG1) -> dict: - """Instant e-stop action: zero locomotion velocity + freeze arms in place. - - Zeroing ``remote.*`` stops the GRoot controller from walking (it keeps the - robot balanced and standing), and echoing each arm joint's *current measured* - position back as its target holds the arms exactly where they are instead of - coasting toward a stale target or snapping to default. - """ - cmd: dict[str, float] = {"remote.lx": 0.0, "remote.ly": 0.0, "remote.rx": 0.0, "remote.ry": 0.0} - obs = robot.get_observation() or {} - for joint in G1_29_JointArmIndex: - key = f"{joint.name}.q" - if key in obs: - cmd[key] = float(obs[key]) - return cmd - def main() -> None: p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) @@ -181,14 +157,27 @@ def main() -> None: 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) # short, so the e-stop watchdog stays responsive + sock.setsockopt(zmq.RCVTIMEO, 200) # keeps the loop responsive to the stop event sock.bind(f"tcp://0.0.0.0:{args.action_port}") logger.info("Onboard controller live. Waiting for laptop actions on :%d ...", args.action_port) + logger.info("Type 'e' then Enter to STOP immediately (or Ctrl-C for graceful shutdown).") stop = threading.Event() signal.signal(signal.SIGINT, lambda *_: stop.set()) signal.signal(signal.SIGTERM, lambda *_: stop.set()) + # Emergency stop key: 'e' + Enter -> skip the soft-stop arm ramp (make it a no-op), + # then trigger the normal shutdown, which goes straight to zero-torque and exits. + def estop_listener() -> None: + for line in sys.stdin: + if line.strip().lower() == "e": + logger.warning("E-STOP ('e'): stopping immediately (no arm ramp).") + robot._soft_stop = lambda: None # skip the slow ramp-to-default + stop.set() + break + + threading.Thread(target=estop_listener, daemon=True).start() + # Proprioception feedback: publish observation.state (29 joint .q) so the # laptop-side inference client can feed it to the policy. DDS stays local to # the robot; only this compact JSON state crosses the network (like the @@ -233,24 +222,11 @@ def main() -> None: logger.info("observation.state PUB disabled (--state-fps<=0); inference client will get no proprio") n = 0 - last_action_ts = time.time() - halted = False try: while not stop.is_set(): try: payload = sock.recv() except zmq.Again: - # E-stop watchdog: laptop went quiet (Ctrl-C, crash, or network - # drop). Engage a safe-hold once so the robot doesn't keep coasting - # on the last velocity command. - if not halted and (time.time() - last_action_ts) > ACTION_TIMEOUT_S: - robot.send_action(_safe_hold_cmd(robot)) - halted = True - logger.warning( - "No actions for >%.2fs — safe-hold engaged (zero velocity, arms frozen). " - "Ctrl-C this process to fully shut down.", - ACTION_TIMEOUT_S, - ) continue except zmq.ContextTerminated: break @@ -261,19 +237,6 @@ def main() -> None: logger.warning("Dropping malformed action: %s", e) continue - # Explicit e-stop from the laptop (Ctrl-C on the client): stop NOW, - # without the slow soft-stop ramp. - if action.get("halt"): - robot.send_action(_safe_hold_cmd(robot)) - last_action_ts = time.time() - if not halted: - halted = True - logger.warning("HALT received from laptop — safe-hold engaged (zero velocity, arms frozen).") - continue - - last_action_ts = time.time() - halted = False - robot.send_action(action) # Absolute base height / torso orientation from the policy (not