#!/usr/bin/env python3 """ZelPi -- Unitree G1/H1 humanoid bridge (runs on the robot's companion computer). Wraps the OFFICIAL unitree_sdk2py G1 LocoClient (a DDS-based high-level locomotion API) and speaks a tiny newline-delimited JSON protocol over stdio to server/createUnitreeDriver.mjs, which adapts it to ZelPi's HAL RobotDriver interface (see docs/PROTOCOL.md and server/robot-agent.mjs). STATUS -- READ BEFORE USE: implemented against unitree_sdk2py's documented example API (example/g1/loco/g1_loco_client_example.py in Unitree's own sdk2_python repo). This has NEVER been run against real G1/H1 hardware -- no unit was available to verify it against. Every SDK call is wrapped in try/except so a mismatched SDK version degrades to a clear fault message instead of silently doing the wrong thing or crashing the bridge, but the method names/FSM semantics themselves are unverified until tested live. H1 support (--robot h1) assumes the SAME LocoClient/FSM surface as G1, since newer H1 firmware shares the unitree_hg IDL with G1. Older H1 units still on the legacy unitree_legged_sdk are NOT covered by this bridge. Install (once, on the companion computer): pip install unitree_sdk2py Run: python3 unitree_bridge.py --robot g1 --iface eth0 Protocol -- newline-delimited JSON, both directions: stdin (Node -> bridge): {"cmd":"move", "vx":0.3, "vy":0.0, "vyaw":0.1} {"cmd":"skill", "skill":"balanceStand"|"zeroTorque"|"damp", "reqId":"..."} {"cmd":"estop"} {"cmd":"release"} {"cmd":"shutdown"} stdout (bridge -> Node): {"type":"ready"} {"type":"telemetry","x":,"y":,"theta":,"battery":,"joints":[...],"ts":} {"type":"skillResult","reqId":,"status":"done"|"failed","error":?} {"type":"fault","message":"..."} x/y are NOT real odometry -- the loco client's FSM state carries no absolute pose, only commanded velocity and IMU orientation. theta comes from the real IMU yaw when the LowState subscription is available; x/y are left at zero (better an honest zero than a fabricated position). Use `zelpi slam attach` against this robot's own localization/SLAM stack for a real pose estimate. """ import argparse import json import sys import threading import time def emit(obj): sys.stdout.write(json.dumps(obj) + "\n") sys.stdout.flush() def main(): ap = argparse.ArgumentParser() ap.add_argument("--robot", choices=["g1", "h1"], default="g1") ap.add_argument("--iface", default="eth0", help="network interface for CycloneDDS discovery") args = ap.parse_args() try: from unitree_sdk2py.core.channel import ChannelFactoryInitialize from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient except ImportError as e: emit({"type": "fault", "message": f"unitree_sdk2py not installed (pip install unitree_sdk2py): {e}"}) sys.exit(1) try: ChannelFactoryInitialize(0, args.iface) client = LocoClient() client.Init() except Exception as e: emit({"type": "fault", "message": f"DDS/robot init failed on iface '{args.iface}': {e}"}) sys.exit(1) state = {"theta": 0.0, "battery": None, "joints": None} state_lock = threading.Lock() estopped = {"v": False} # Best-effort low-level telemetry. Deliberately isolated behind its own # try/except: if this SDK version's LowState/IDL layout differs, the # bridge stays fully usable for control -- it just runs without # telemetry rather than crashing outright. def on_low_state(msg): try: with state_lock: imu = getattr(msg, "imu_state", None) if imu is not None and hasattr(imu, "rpy"): state["theta"] = float(imu.rpy[2]) # yaw bms = getattr(msg, "bms_state", None) if bms is not None and hasattr(bms, "soc"): state["battery"] = float(bms.soc) motors = getattr(msg, "motor_state", None) if motors is not None: state["joints"] = [float(m.q) for m in motors if hasattr(m, "q")] except Exception: pass try: from unitree_sdk2py.core.channel import ChannelSubscriber from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ sub = ChannelSubscriber("rt/lowstate", LowState_) sub.Init(on_low_state, 10) except Exception as e: emit({"type": "fault", "message": f"low-state telemetry unavailable, running control-only: {e}"}) def telemetry_loop(): while True: with state_lock: emit({ "type": "telemetry", "x": 0.0, "y": 0.0, "theta": state["theta"], "battery": state["battery"], "joints": state["joints"], "ts": time.time() * 1000, }) time.sleep(0.05) # 20 Hz, matches robot-agent.mjs's TELEMETRY_HZ threading.Thread(target=telemetry_loop, daemon=True).start() # Skill name -> SDK call. Kept to the small, well-attested subset of the # example API rather than guessing at less-certain methods (Sit/crouch # poses vary by firmware and aren't consistently named across G1/H1). skills = { "balanceStand": lambda: client.BalanceStand(), "zeroTorque": lambda: client.ZeroTorque(), "damp": lambda: client.Damp(), } emit({"type": "ready"}) for line in sys.stdin: line = line.strip() if not line: continue try: m = json.loads(line) except Exception: continue # malformed input dropped, never crashes -- same boundary discipline as lib/hal/protocol.mjs cmd = m.get("cmd") try: if cmd == "move": if estopped["v"]: continue client.Move(float(m.get("vx", 0)), float(m.get("vy", 0)), float(m.get("vyaw", 0))) elif cmd == "skill": req_id = m.get("reqId") fn = skills.get(m.get("skill")) if fn is None: emit({"type": "skillResult", "reqId": req_id, "status": "failed", "error": f"unsupported skill: {m.get('skill')}"}) continue if estopped["v"] and m.get("skill") != "damp": emit({"type": "skillResult", "reqId": req_id, "status": "failed", "error": "e-stopped"}) continue fn() emit({"type": "skillResult", "reqId": req_id, "status": "done"}) elif cmd == "estop": estopped["v"] = True try: client.Damp() # Unitree's own documented immediate safe-stop except Exception as e: emit({"type": "fault", "message": f"estop Damp() call failed: {e}"}) elif cmd == "release": # Deliberately does NOT re-stand the robot -- clears the local # latch only. A biped standing back up unattended is a real # safety hazard; the operator must send an explicit # balanceStand skill afterward. estopped["v"] = False elif cmd == "shutdown": break except Exception as e: req_id = m.get("reqId") if req_id: emit({"type": "skillResult", "reqId": req_id, "status": "failed", "error": str(e)}) else: emit({"type": "fault", "message": f"command handling error: {e}"}) if __name__ == "__main__": main()