File size: 7,285 Bytes
c4f8e4a | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 | from __future__ import annotations
import argparse
import os
import signal
import time
from pathlib import Path
from threading import Lock
import numpy as np
LOWSTATE_TOPIC = "rt/lowstate"
ODOM_TOPIC = "rt/odommodestate"
NUM_G1_JOINTS = 29
RUNNING = True
class LatestBuffer:
def __init__(self):
self._data = None
self._lock = Lock()
def set(self, data) -> None:
with self._lock:
self._data = data
def get(self):
with self._lock:
return self._data
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser()
parser.add_argument("--live-state-npy", default="/tmp/psi0_sonic_live_state.npy")
parser.add_argument("--period", type=float, default=0.02)
parser.add_argument("--root-z-fallback", type=float, default=0.78)
parser.add_argument("--quat-order", choices=["wxyz", "xyzw"], default="wxyz")
parser.add_argument("--require-odom", action="store_true", default=False)
parser.add_argument("--status-period", type=float, default=2.0)
return parser.parse_args()
def stop_handler(signum, frame) -> None:
global RUNNING
RUNNING = False
def normalize_quat_wxyz(quat: np.ndarray) -> np.ndarray:
quat = np.asarray(quat, dtype=np.float32)
norm = float(np.linalg.norm(quat))
if not np.isfinite(norm) or norm < 1e-6:
return np.asarray([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
return (quat / norm).astype(np.float32)
def convert_quat(quat, quat_order: str) -> np.ndarray:
quat = np.asarray(quat, dtype=np.float32).reshape(-1)
if quat.shape[0] != 4:
return np.asarray([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
if quat_order == "xyzw":
quat = quat[[3, 0, 1, 2]]
return normalize_quat_wxyz(quat)
def get_lowstate_joint_q(lowstate) -> np.ndarray | None:
if lowstate is None or not hasattr(lowstate, "motor_state"):
return None
try:
q = np.asarray([lowstate.motor_state[i].q for i in range(NUM_G1_JOINTS)], dtype=np.float32)
except Exception:
return None
if q.shape != (NUM_G1_JOINTS,) or not np.all(np.isfinite(q)):
return None
return q
def get_lowstate_quat(lowstate, quat_order: str) -> np.ndarray | None:
imu = getattr(lowstate, "imu_state", None)
if imu is None or not hasattr(imu, "quaternion"):
return None
quat = convert_quat(imu.quaternion, quat_order)
if np.all(np.isfinite(quat)):
return quat
return None
def get_root_from_odom(odom, quat_order: str) -> tuple[np.ndarray | None, np.ndarray | None]:
if odom is None:
return None, None
root_pos = None
if hasattr(odom, "position"):
pos = np.asarray(odom.position, dtype=np.float32).reshape(-1)
if pos.shape[0] >= 3 and np.all(np.isfinite(pos[:3])):
root_pos = pos[:3].copy()
root_quat = None
imu = getattr(odom, "imu_state", None)
if imu is not None and hasattr(imu, "quaternion"):
root_quat = convert_quat(imu.quaternion, quat_order)
return root_pos, root_quat
def write_state_atomic(path: Path, state: np.ndarray) -> None:
tmp_path = Path(str(path) + ".tmp")
with tmp_path.open("wb") as f:
np.save(f, state.astype(np.float32))
os.replace(tmp_path, path)
def main() -> int:
args = parse_args()
signal.signal(signal.SIGINT, stop_handler)
signal.signal(signal.SIGTERM, stop_handler)
try:
from unitree_sdk2py.core.channel import ChannelFactoryInitialize, ChannelSubscriber
from unitree_sdk2py.idl.unitree_go.msg.dds_._SportModeState_ import SportModeState_
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
except Exception as exc:
raise SystemExit(f"[g1-state] failed to import Unitree SDK2 python package: {exc}") from exc
live_state_path = Path(args.live_state_npy)
live_state_path.parent.mkdir(parents=True, exist_ok=True)
odom_buffer = LatestBuffer()
print("[g1-state] initializing Unitree DDS")
ChannelFactoryInitialize(0)
lowstate_sub = ChannelSubscriber(LOWSTATE_TOPIC, LowState_)
lowstate_sub.Init()
odom_sub = ChannelSubscriber(ODOM_TOPIC, SportModeState_)
odom_sub.Init(lambda msg: odom_buffer.set(msg), 1)
print(f"[g1-state] lowstate topic: {LOWSTATE_TOPIC}")
print(f"[g1-state] odom topic: {ODOM_TOPIC}")
print(f"[g1-state] live_state: {live_state_path}")
print(f"[g1-state] period: {args.period:.3f}s")
next_tick = time.perf_counter()
next_status = 0.0
wrote_once = False
warned_odom = False
warned_root_z = False
while RUNNING:
lowstate = lowstate_sub.Read()
q = get_lowstate_joint_q(lowstate)
if q is None:
if time.time() >= next_status:
print("[g1-state] waiting for valid lowstate motor q")
next_status = time.time() + args.status_period
time.sleep(0.01)
continue
odom = odom_buffer.get()
odom_pos, odom_quat = get_root_from_odom(odom, args.quat_order)
lowstate_quat = get_lowstate_quat(lowstate, args.quat_order)
if odom is None and args.require_odom:
if time.time() >= next_status:
print("[g1-state] waiting for odom because --require-odom is set")
next_status = time.time() + args.status_period
time.sleep(0.01)
continue
if odom is None and not warned_odom:
print("[g1-state] odom not ready; root xy stays zero and root quat uses lowstate IMU if available")
warned_odom = True
root_pos = np.zeros(3, dtype=np.float32) if odom_pos is None else odom_pos.astype(np.float32)
root_quat = odom_quat if odom_quat is not None else lowstate_quat
if root_quat is None:
root_quat = np.asarray([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
# Unitree odom z can be unavailable or reported as zero on some stacks.
# Psi0/Sonic 36D state still needs a plausible pelvis height; 0.78m is
# the standing-height scale observed in the existing Seed/Sonic data.
if not np.isfinite(root_pos[2]) or root_pos[2] < 0.2 or root_pos[2] > 1.5:
root_pos[2] = np.float32(args.root_z_fallback)
if not warned_root_z:
print(f"[g1-state] using root z fallback: {args.root_z_fallback:.3f} m")
warned_root_z = True
state = np.zeros(36, dtype=np.float32)
state[:29] = q
state[29:32] = root_pos
state[32:36] = normalize_quat_wxyz(root_quat)
write_state_atomic(live_state_path, state)
if not wrote_once or time.time() >= next_status:
root = state[29:32]
print(
"[g1-state] wrote live_state "
f"q0={state[0]:.3f} root=({root[0]:.3f},{root[1]:.3f},{root[2]:.3f})"
)
wrote_once = True
next_status = time.time() + args.status_period
next_tick += args.period
sleep_time = next_tick - time.perf_counter()
if sleep_time > 0.0:
time.sleep(sleep_time)
else:
next_tick = time.perf_counter()
print("[g1-state] stopped")
return 0
if __name__ == "__main__":
raise SystemExit(main())
|