revert(unitree_g1): drop onboard e-stop halt/watchdog; add simple 'e' stop key

Remove the halt/watchdog/safe-hold and soft-start re-arm machinery from the onboard
runner: engaging the safe-hold and then resuming teleop snapped the arms to the live
pose. Replace with a plain 'e'+Enter key that stops immediately (skips the soft-stop
arm ramp, goes straight to zero-torque and exits). Ctrl-C still does graceful shutdown.
Keeps the groot.height/rpy state publishing (used by the recorder), unrelated to e-stop.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Martino Russi
2026-07-20 12:17:56 +02:00
parent ef61724974
commit d0acaf96a4
+18 -55
View File
@@ -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