254 lines
11 KiB
Python
254 lines
11 KiB
Python
#!/usr/bin/env python3
|
|
"""Regenerate the r1 and go2 fleet agents from the canonical g1 agent.
|
|
|
|
g1 (agents/g1/sanad_api_g1.py) is the SOURCE OF TRUTH for all shared features
|
|
(telemetry, map sync, logs, alerts, project-logs, remote, control, software,
|
|
firmware, timing). r1 is a near-exact subset of g1 (same unitree_hg DDS; only
|
|
naming + FSM ids differ). go2 differs in the DDS family (unitree_go: battery is
|
|
nested in LowState.bms_state), so its DDSReader is swapped wholesale.
|
|
|
|
Run this after ANY change to g1 to keep the three agents consistent:
|
|
python3 tools/gen_agents.py
|
|
"""
|
|
import re
|
|
import sys
|
|
from pathlib import Path
|
|
|
|
ROOT = Path(__file__).resolve().parent.parent
|
|
G1 = (ROOT / "agents/g1/sanad_api_g1.py").read_text()
|
|
|
|
|
|
# ─────────────────────────── r1 (unitree_hg subset) ───────────────────────────
|
|
def make_r1(src: str) -> str:
|
|
reps = [
|
|
("sanad_api_g1", "sanad_api_r1"),
|
|
("— G1 fleet agent", "— R1 fleet agent"),
|
|
("The single G1 agent", "The single R1 agent"),
|
|
("Unitree G1, unitree_hg DDS", "Unitree R1 EDU, unitree_hg DDS — same family as the G1"),
|
|
('description="G1 fleet agent: telemetry + map sync"',
|
|
'description="R1 fleet agent: telemetry + map sync"'),
|
|
('_env("SN", "g1_0000")', '_env("SN", "r1_0000")'),
|
|
('"ROBOT_MODEL", "g1"', '"ROBOT_MODEL", "r1"'),
|
|
('"G1_READ_FSM"', '"R1_READ_FSM"'),
|
|
('_env("G1_POSITION_SOURCE", "odom")', '_env("R1_POSITION_SOURCE", "none")'),
|
|
('"fsm": 200 if moving else 4', '"fsm": 811 if moving else 4'),
|
|
("(G1 ids: 200 walk-ready / 4 stand / 2 squat / 702 lie2stand)",
|
|
"(R1 ids: 0 zero-torque / 1 damping / 4 standing / 811 gait-running)"),
|
|
("e.g. g1_58", "e.g. r1_82"),
|
|
("DDS_INTERFACE (eth0)", "DDS_INTERFACE (eth10)"),
|
|
# FSM + control-mode maps (R1 ids)
|
|
('# G1 FSM ids (differ from the R1\'s): 200 balance/walk-ready, 4 StandUp, 2 Squat, 702 Lie2Stand.\n'
|
|
'_FSM_STATUS = {200: "ready", 4: "standing", 2: "squat", 702: "lie2stand"}\n'
|
|
'# Control-panel mode labels + the switchable set (fsm_id -> friendly mode).\n'
|
|
'_CONTROL_MODES = {200: "running", 4: "lock", 2: "squat", 702: "lie2stand", 0: "zero_torque", 1: "damp"}',
|
|
'# R1 FSM ids (official R1 sport doc): 0 ZeroTorque, 1 Damping, 4 Locked-Standing, 811 Gait-Running.\n'
|
|
'_FSM_STATUS = {0: "zero_torque", 1: "damping", 4: "locked_standing", 811: "running"}\n'
|
|
'# Control-panel mode labels + the switchable set (fsm_id -> friendly mode).\n'
|
|
'_CONTROL_MODES = {0: "zero_torque", 1: "damp", 4: "lock", 811: "running"}'),
|
|
]
|
|
out = src
|
|
for a, b in reps:
|
|
if a not in out:
|
|
sys.exit(f"[r1] anchor missing: {a!r}")
|
|
out = out.replace(a, b)
|
|
return out
|
|
|
|
|
|
# ─────────────────────────── go2 (unitree_go DDS) ─────────────────────────────
|
|
GO2_DDS_READER = '''class DDSReader:
|
|
"""Subscribes rt/lowstate (unitree_go LowState_); battery is NESTED in
|
|
LowState_.bms_state (Go2 has no separate rt/lf/bmsstate). Optional position
|
|
from rt/lf/sportmodestate. Passive reads only; the only RPC ever issued is
|
|
GET_FSM_ID (Go2 has no loco FSM RPC, so it stays off)."""
|
|
|
|
def __init__(self, cfg: Config):
|
|
self.cfg = cfg
|
|
self._lock = threading.Lock()
|
|
self._bms: Optional[Dict[str, Any]] = None
|
|
self._bms_ts = 0.0
|
|
self._low_ts = 0.0
|
|
self._temps: List[float] = []
|
|
self._max_dq = 0.0
|
|
self._xy: Optional[Dict[str, float]] = None
|
|
self._fw: Dict[str, Any] = {}
|
|
self._loco = None
|
|
self.ok = False
|
|
self._start()
|
|
|
|
def _start(self) -> None:
|
|
try:
|
|
from unitree_sdk2py.core.channel import (
|
|
ChannelFactoryInitialize, ChannelSubscriber)
|
|
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
|
|
SportModeState_ = None
|
|
if self.cfg.position_source in ("sportmode", "odom"):
|
|
try:
|
|
from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
|
|
except Exception:
|
|
SportModeState_ = None
|
|
except Exception as e:
|
|
log.warning("unitree_sdk2py unavailable (%s) — telemetry runs in heartbeat mode", e)
|
|
return
|
|
try:
|
|
ChannelFactoryInitialize(self.cfg.dds_domain, self.cfg.dds_interface)
|
|
self._low_sub = ChannelSubscriber("rt/lowstate", LowState_)
|
|
self._low_sub.Init(self._on_low, 10)
|
|
if SportModeState_ is not None:
|
|
self._sport_sub = ChannelSubscriber("rt/lf/sportmodestate", SportModeState_)
|
|
self._sport_sub.Init(self._on_odom, 10)
|
|
self.ok = True
|
|
log.info("DDS up: domain=%d iface=%s (rt/lowstate; battery from bms_state%s)",
|
|
self.cfg.dds_domain, self.cfg.dds_interface,
|
|
" + sportmode" if SportModeState_ is not None else "")
|
|
except Exception as e:
|
|
log.warning("DDS init failed (%s) — heartbeat mode", e)
|
|
|
|
def _init_loco(self) -> None:
|
|
return # Go2 has no r1/g1-style loco FSM RPC
|
|
|
|
def _on_low(self, msg) -> None:
|
|
try:
|
|
# battery from the nested BMS state
|
|
bms = getattr(msg, "bms_state", None) or getattr(msg, "bms", None)
|
|
if bms is not None:
|
|
soc = int(getattr(bms, "soc", 0) or 0)
|
|
cur = int(getattr(bms, "current", 0) or 0) # mA
|
|
volt_v = None
|
|
try:
|
|
pv = getattr(msg, "power_v", None)
|
|
if pv:
|
|
volt_v = round(float(pv), 1)
|
|
except Exception:
|
|
volt_v = None
|
|
temp_c = None
|
|
try:
|
|
ntc = []
|
|
for attr in ("bq_ntc", "mcu_ntc"):
|
|
nt = getattr(bms, attr, None)
|
|
if nt is not None:
|
|
vals = [int(x) for x in nt] if hasattr(nt, "__iter__") else [int(nt)]
|
|
ntc.extend(v for v in vals if -40 <= v <= 150)
|
|
if ntc:
|
|
temp_c = max(ntc)
|
|
except Exception:
|
|
temp_c = None
|
|
try:
|
|
vh, vl = getattr(bms, "version_high", None), getattr(bms, "version_low", None)
|
|
if vh is not None:
|
|
self._fw["bms"] = f"{int(vh)}.{int(vl or 0)}"
|
|
except Exception:
|
|
pass
|
|
with self._lock:
|
|
self._bms = {
|
|
"soc": max(0, min(100, soc)),
|
|
"current_a": round(cur / 1000.0, 2),
|
|
"voltage_v": volt_v,
|
|
"temp_c": temp_c,
|
|
"soh": int(getattr(bms, "soh", 0) or 0),
|
|
"cycles": int(getattr(bms, "cycle", 0) or 0),
|
|
}
|
|
self._bms_ts = time.monotonic()
|
|
temps: List[float] = []
|
|
max_dq = 0.0
|
|
for m in (getattr(msg, "motor_state", None) or []):
|
|
t = getattr(m, "temperature", None)
|
|
if t is not None:
|
|
try:
|
|
vals = [float(x) for x in t] if hasattr(t, "__iter__") else [float(t)]
|
|
# 0 = slot not reporting (unpopulated motor), not a real temp
|
|
temps.extend(v for v in vals if 0 < v <= 200)
|
|
except Exception:
|
|
pass
|
|
dq = getattr(m, "dq", None)
|
|
if dq is not None:
|
|
try:
|
|
max_dq = max(max_dq, abs(float(dq)))
|
|
except Exception:
|
|
pass
|
|
with self._lock:
|
|
self._low_ts = time.monotonic()
|
|
self._temps = temps
|
|
self._max_dq = max_dq
|
|
except Exception:
|
|
pass
|
|
|
|
def _on_odom(self, msg) -> None:
|
|
try:
|
|
pos = getattr(msg, "position", None)
|
|
if pos is not None and len(pos) >= 2:
|
|
with self._lock:
|
|
self._xy = {"x": round(float(pos[0]), 3), "y": round(float(pos[1]), 3)}
|
|
except Exception:
|
|
pass
|
|
|
|
def snapshot(self) -> Dict[str, Any]:
|
|
with self._lock:
|
|
now = time.monotonic()
|
|
return {
|
|
"bms": dict(self._bms) if self._bms else None,
|
|
"low_age": (now - self._low_ts) if self._low_ts else None,
|
|
"temps": list(self._temps),
|
|
"max_dq": self._max_dq,
|
|
"xy": dict(self._xy) if self._xy else None,
|
|
"fw": dict(self._fw),
|
|
}
|
|
|
|
def fsm_id(self) -> Optional[int]:
|
|
return None # Go2 has no loco FSM id RPC
|
|
|
|
|
|
'''
|
|
|
|
GO2_CONTROL = ('# Go2 has no r1/g1 FSM-id scheme (SportClient modes). Placeholders — UNVERIFIED.\n'
|
|
'_FSM_STATUS = {}\n'
|
|
'# Control-panel mode labels + the switchable set.\n'
|
|
'_CONTROL_MODES = {0: "idle", 1: "stand", 2: "walk"}\n'
|
|
'_CONTROL_SWITCHABLE = ["damp", "stand", "walk"]')
|
|
|
|
|
|
def make_go2(src: str) -> str:
|
|
reps = [
|
|
("sanad_api_g1", "sanad_api_go2"),
|
|
("— G1 fleet agent", "— Go2 fleet agent"),
|
|
("The single G1 agent", "The single Go2 agent"),
|
|
("Unitree G1, unitree_hg DDS", "Unitree Go2, unitree_go DDS (⚠ UNVERIFIED on hardware)"),
|
|
('description="G1 fleet agent: telemetry + map sync"',
|
|
'description="Go2 fleet agent: telemetry + map sync"'),
|
|
('_env("SN", "g1_0000")', '_env("SN", "go2_0000")'),
|
|
('"ROBOT_MODEL", "g1"', '"ROBOT_MODEL", "go2"'),
|
|
('_env("ROBOT_TYPE", "humanoid")', '_env("ROBOT_TYPE", "dog")'),
|
|
('"G1_READ_FSM"', '"GO2_READ_FSM"'),
|
|
('_env("G1_POSITION_SOURCE", "odom")', '_env("GO2_POSITION_SOURCE", "none")'),
|
|
("e.g. g1_58", "e.g. go2_77"),
|
|
("DDS_INTERFACE (eth0)", "DDS_INTERFACE (eth0)"),
|
|
]
|
|
out = src
|
|
for a, b in reps:
|
|
if a not in out:
|
|
sys.exit(f"[go2] anchor missing: {a!r}")
|
|
out = out.replace(a, b)
|
|
# swap DDSReader class (from 'class DDSReader:' up to the FSM/control block)
|
|
start = out.index("class DDSReader:")
|
|
ctrl_start = out.index('# G1 FSM ids (differ from the R1', start)
|
|
out = out[:start] + GO2_DDS_READER + out[ctrl_start:]
|
|
# swap the FSM/control-modes block
|
|
ctrl_a = out.index('# G1 FSM ids (differ from the R1')
|
|
ctrl_b = out.index('_CONTROL_SWITCHABLE = ["zero_torque", "damp", "lock", "running"]')
|
|
ctrl_b = out.index("\n", ctrl_b)
|
|
out = out[:ctrl_a] + GO2_CONTROL + out[ctrl_b:]
|
|
return out
|
|
|
|
|
|
def main() -> int:
|
|
r1 = make_r1(G1)
|
|
go2 = make_go2(G1)
|
|
(ROOT / "agents/r1/sanad_api_r1.py").write_text(r1)
|
|
(ROOT / "agents/go2/sanad_api_go2.py").write_text(go2)
|
|
print(f"generated r1 ({len(r1.splitlines())} lines)")
|
|
print(f"generated go2 ({len(go2.splitlines())} lines)")
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
sys.exit(main())
|