From 8f2a83efe3ead5553693d8553e72ed00d6e836a8 Mon Sep 17 00:00:00 2001 From: Martino Russi Date: Sat, 18 Jul 2026 13:33:31 +0200 Subject: [PATCH] feat(unitree_g1): soft-stop arms on disconnect Ramp the arms slowly back to the rest pose on disconnect while the locomotion controller still holds the legs, so the arms don't drop when going passive. Adds soft_stop / soft_stop_duration config and makes disconnect() idempotent (safe under GC / double-call). Co-authored-by: Cursor --- .../robots/unitree_g1/config_unitree_g1.py | 6 +++ src/lerobot/robots/unitree_g1/unitree_g1.py | 37 ++++++++++++++++++- 2 files changed, 42 insertions(+), 1 deletion(-) diff --git a/src/lerobot/robots/unitree_g1/config_unitree_g1.py b/src/lerobot/robots/unitree_g1/config_unitree_g1.py index b786c2a33..a892491c1 100644 --- a/src/lerobot/robots/unitree_g1/config_unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/config_unitree_g1.py @@ -71,3 +71,9 @@ class UnitreeG1Config(RobotConfig): # Lower-body controller class name, e.g. "GrootLocomotionController" or # "HolosomaLocomotionController". None disables it. controller: str | None = None + + # On disconnect, ramp the arms slowly back to `default_positions` (hands down) + # before going passive, instead of dropping straight to zero torque. Only + # applies on the real robot when a locomotion controller holds the legs. + soft_stop: bool = True + soft_stop_duration: float = 3.0 diff --git a/src/lerobot/robots/unitree_g1/unitree_g1.py b/src/lerobot/robots/unitree_g1/unitree_g1.py index a065d8d2f..fbbbd3e03 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/unitree_g1.py @@ -298,6 +298,7 @@ class UnitreeG1(Robot): pass def connect(self, calibrate: bool = True) -> None: # connect to DDS + self._is_disconnected = False # Initialize DDS channel and simulation environment if self.config.is_simulation: from lerobot.envs import make_env @@ -313,6 +314,7 @@ class UnitreeG1(Robot): # run_g1_server, which drives the Damiao grippers over CAN. self._gripper_sock = None self._last_gripper_cmd = None + self._warned_no_gripper_buttons = False if not self.config.is_simulation: try: import zmq @@ -385,6 +387,26 @@ class UnitreeG1(Robot): fps = int(1.0 / self.controller.control_dt) logger.info(f"Controller thread started ({fps}Hz)") + def _soft_stop(self) -> None: + """Gently ramp the arms to the default rest pose before shutdown. + + Mirror of the connect-time soft-start. Only runs on the real robot when a + locomotion controller is active (so the legs stay balanced while the arms + come down) and lowstate is available to read the current pose. + """ + if self.config.is_simulation or not self.config.soft_stop: + return + if self.controller is None: + return + with self._lowstate_lock: + if self._lowstate is None: + return + try: + logger.info("Soft-stop: ramping arms to default position...") + self._interpolate_to_default(duration=self.config.soft_stop_duration) + except Exception as e: + logger.warning(f"Soft-stop failed ({e}); continuing shutdown.") + def _send_zero_torque(self) -> None: """Send a zero-gain command to make joints passive before shutting down.""" try: @@ -400,6 +422,17 @@ class UnitreeG1(Robot): logger.warning(f"Failed to send zero-torque on disconnect: {e}") def disconnect(self): + # Idempotent: disconnect() can be called both explicitly and again via GC / + # interpreter shutdown; re-running soft-stop against already-closed cameras + # would error, so bail out if we've already torn down. + if getattr(self, "_is_disconnected", False): + return + self._is_disconnected = True + + # Soft-stop: ramp arms slowly back to the rest pose (hands down) while the + # controller still holds the legs, so they don't drop when we go passive. + self._soft_stop() + # Put robot in passive mode before stopping threads if not self.config.is_simulation: self._send_zero_torque() @@ -549,7 +582,9 @@ class UnitreeG1(Robot): l3 = action.get("remote.button.4") r3 = action.get("remote.button.0") if l3 is None and r3 is None: - logger.warning("[gripper] no remote.button.0/4 in action — teleop not emitting exo buttons") + if not self._warned_no_gripper_buttons: + logger.warning("[gripper] no remote.button.0/4 in action — teleop not emitting exo buttons") + self._warned_no_gripper_buttons = True return cmd = {"L": int(bool(l3)), "R": int(bool(r3))} if cmd == self._last_gripper_cmd: