code-Psi0-yth-v21 / scripts /deploy_real_old /g1_real_state_writer.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
c4f8e4a verified
Raw History Blame Contribute Delete
7.29 kB
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())