fix(unitree_g1): make 'e' stop instant (zero-torque + hard exit)

The 'e' key previously went through the graceful disconnect (soft-stop arm ramp +
thread joins), which took several seconds while the 50Hz controller loop kept
publishing over the single zero-torque. Now 'e' stops the controller loop, sends
one zero-gain (limp) command, and os._exit immediately -> robot goes passive in ~50ms.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Martino Russi
2026-07-20 12:26:11 +02:00
parent d0acaf96a4
commit e84e56e235
@@ -46,6 +46,7 @@ Examples (on the robot):
import argparse import argparse
import json import json
import logging import logging
import os
import signal import signal
import sys import sys
import threading import threading
@@ -166,15 +167,21 @@ def main() -> None:
signal.signal(signal.SIGINT, lambda *_: stop.set()) signal.signal(signal.SIGINT, lambda *_: stop.set())
signal.signal(signal.SIGTERM, 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), # Emergency stop key: 'e' + Enter -> go passive NOW. We stop the controller loop
# then trigger the normal shutdown, which goes straight to zero-torque and exits. # from re-publishing, send a single zero-gain (limp) command, and hard-exit the
# process. This deliberately skips the graceful disconnect (soft-stop arm ramp +
# thread joins), which is what made it take several seconds.
def estop_listener() -> None: def estop_listener() -> None:
for line in sys.stdin: for line in sys.stdin:
if line.strip().lower() == "e": if line.strip().lower() == "e":
logger.warning("E-STOP ('e'): stopping immediately (no arm ramp).") logger.warning("E-STOP ('e'): going passive NOW.")
robot._soft_stop = lambda: None # skip the slow ramp-to-default try:
stop.set() robot._shutdown_event.set() # stop the 50Hz controller loop publishing
break time.sleep(0.05) # let it finish its current cycle
robot._send_zero_torque() # motors limp; nothing overwrites it now
except Exception as e: # noqa: BLE001
logger.warning("E-stop zero-torque failed: %s", e)
os._exit(0) # immediate hard exit, no slow cleanup
threading.Thread(target=estop_listener, daemon=True).start() threading.Thread(target=estop_listener, daemon=True).start()