From 437248d823eb949864ce31d121d690871d356f98 Mon Sep 17 00:00:00 2001 From: Martino Russi Date: Sun, 19 Jul 2026 12:39:28 +0200 Subject: [PATCH] feat(unitree_g1): make onboard state PUB toggleable + lower default rate Lower observation.state publish rate to 30 Hz and allow disabling it with --state-fps <=0, so the onboard runner can reproduce the plain-teleop setup (no state thread) for A/B isolation of camera-stability issues. Co-authored-by: Cursor --- .../robots/unitree_g1/run_g1_onboard.py | 69 +++++++++++-------- 1 file changed, 40 insertions(+), 29 deletions(-) diff --git a/src/lerobot/robots/unitree_g1/run_g1_onboard.py b/src/lerobot/robots/unitree_g1/run_g1_onboard.py index 3472fbe61..8d8bb48e0 100644 --- a/src/lerobot/robots/unitree_g1/run_g1_onboard.py +++ b/src/lerobot/robots/unitree_g1/run_g1_onboard.py @@ -74,7 +74,12 @@ def main() -> None: default=STATE_PORT, help="ZMQ PUB port for observation.state feedback (29 joint .q) to the inference client", ) - p.add_argument("--state-fps", type=float, default=60.0, help="observation.state publish rate (Hz)") + p.add_argument( + "--state-fps", + type=float, + default=30.0, + help="observation.state publish rate (Hz); set <=0 to disable the state PUB entirely", + ) p.add_argument( "--gravity-compensation", action="store_true", @@ -152,36 +157,41 @@ def main() -> None: 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) - # 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 - # camera/action ZMQ channels). - state_sock = ctx.socket(zmq.PUB) - state_sock.setsockopt(zmq.SNDHWM, 2) - state_sock.setsockopt(zmq.LINGER, 0) - state_sock.bind(f"tcp://0.0.0.0:{args.state_port}") - logger.info("Publishing observation.state on :%d at %.0f Hz", args.state_port, args.state_fps) - stop = threading.Event() signal.signal(signal.SIGINT, lambda *_: stop.set()) signal.signal(signal.SIGTERM, lambda *_: stop.set()) - def publish_state() -> None: - period = 1.0 / args.state_fps if args.state_fps > 0 else 0.0 - joint_names = [j.name for j in G1_29_JointIndex] - while not stop.is_set(): - t0 = time.time() - obs = robot.get_observation() - if obs: - state = {f"{name}.q": float(obs.get(f"{name}.q", 0.0)) for name in joint_names} - try: - state_sock.send_json(state, zmq.NOBLOCK) - except zmq.Again: - pass - if period: + # 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 + # camera/action ZMQ channels). get_observation() here reads only DDS lowstate + # (the robot is built with cameras={}), so it never touches the USB cameras. + # Disable with --state-fps <=0 (e.g. to reproduce the plain-teleop setup). + state_sock = None + if args.state_fps > 0: + state_sock = ctx.socket(zmq.PUB) + state_sock.setsockopt(zmq.SNDHWM, 2) + state_sock.setsockopt(zmq.LINGER, 0) + state_sock.bind(f"tcp://0.0.0.0:{args.state_port}") + logger.info("Publishing observation.state on :%d at %.0f Hz", args.state_port, args.state_fps) + + def publish_state() -> None: + period = 1.0 / args.state_fps + joint_names = [j.name for j in G1_29_JointIndex] + while not stop.is_set(): + t0 = time.time() + obs = robot.get_observation() + if obs: + state = {f"{name}.q": float(obs.get(f"{name}.q", 0.0)) for name in joint_names} + try: + state_sock.send_json(state, zmq.NOBLOCK) + except zmq.Again: + pass time.sleep(max(0.0, period - (time.time() - t0))) - threading.Thread(target=publish_state, daemon=True).start() + threading.Thread(target=publish_state, daemon=True).start() + else: + logger.info("observation.state PUB disabled (--state-fps<=0); inference client will get no proprio") n = 0 try: @@ -234,10 +244,11 @@ def main() -> None: finally: logger.info("Shutting down onboard controller...") stop.set() - try: - state_sock.close(linger=0) - except Exception as e: # noqa: BLE001 - logger.warning("State socket close failed: %s", e) + if state_sock is not None: + try: + state_sock.close(linger=0) + except Exception as e: # noqa: BLE001 + logger.warning("State socket close failed: %s", e) if camera_server is not None: try: camera_server.stop()