mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-24 18:26:11 +00:00
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:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user