code-HFD / scripts /deploy /online_psi0_sonic_bridge.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
9fc9873 verified
Raw History Blame Contribute Delete
46.1 kB
from __future__ import annotations
import argparse
import json
import struct
import sys
import threading
import time
from base64 import b64decode, b64encode
from pathlib import Path
import numpy as np
import pandas as pd
import websocket
import zmq
from numpy.lib.format import descr_to_dtype, dtype_to_descr
from psi.data.seed.motion_repr import SEED_TEXTOP_ACTION_DIM, normalize_quat_wxyz, recover_seed_abs_from_textop_action
from psi.utils import parse_args_to_tyro_config
HEADER_SIZE = 1280
CTRL_HZ = 50
CTRL_DT = 1.0 / CTRL_HZ
def numpy_serialize(o):
if isinstance(o, (np.ndarray, np.generic)):
data = o.data if o.flags["C_CONTIGUOUS"] else o.tobytes()
return {
"__numpy__": b64encode(data).decode(),
"dtype": dtype_to_descr(o.dtype),
"shape": o.shape,
}
raise TypeError(f"Object of type {o.__class__.__name__} is not JSON serializable")
def numpy_deserialize(dct):
if "__numpy__" in dct:
array = np.frombuffer(b64decode(dct["__numpy__"]), descr_to_dtype(dct["dtype"]))
return array.reshape(dct["shape"]) if dct["shape"] else array[0]
return dct
def convert_numpy_in_dict(data, func):
if isinstance(data, dict):
if "__numpy__" in data:
return func(data)
return {key: convert_numpy_in_dict(value, func) for key, value in data.items()}
if isinstance(data, list):
return [convert_numpy_in_dict(item, func) for item in data]
if isinstance(data, (np.ndarray, np.generic)):
return func(data)
return data
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser()
parser.add_argument("--psi-host", default="localhost")
parser.add_argument("--psi-port", type=int, default=8014)
parser.add_argument("--sonic-zmq-host", default="*")
parser.add_argument("--sonic-zmq-port", type=int, default=5556)
parser.add_argument("--prompt", required=True)
parser.add_argument("--init-motion-dir", default="")
parser.add_argument("--inference-video-path", default="")
parser.add_argument("--video-config-key", default="video_clip")
parser.add_argument("--video-clip-motion-fps", type=float, default=50.0)
parser.add_argument("--video-sample-fps", type=float, default=8.0)
parser.add_argument("--video-size-pixels", type=int, default=50176)
parser.add_argument("--video-decoder", default="ffmpeg")
# <modify> 多段planner模式:不重启bridge,只在同一个RTC连接里按帧号切换prompt。
parser.add_argument("--motion-plan-json", default="")
parser.add_argument("--run-dir", required=True)
parser.add_argument("--start-index", type=int, default=0)
parser.add_argument("--init-state-npy", default="")
parser.add_argument("--live-state-npy", default="")
parser.add_argument("--require-live-state", action="store_true", default=False)
parser.add_argument("--live-state-timeout", type=float, default=30.0)
parser.add_argument("--live-state-max-age", type=float, default=0.5)
parser.add_argument("--startup-ready-flag", default="")
parser.add_argument("--start-gate-flag", default="")
parser.add_argument("--start-gate-timeout", type=float, default=600.0)
parser.add_argument("--render-timing-json", default="")
parser.add_argument("--record-flag-path", default="")
# <modify> 第一帧站稳预滚:先发重复第一帧并释放sim elastic band,等站稳后再开始record。
parser.add_argument("--settle-flag-path", default="")
parser.add_argument("--stand-pre-roll-seconds", type=float, default=1.0)
parser.add_argument("--record-stable-min-root-z", type=float, default=0.75)
parser.add_argument("--record-stable-seconds", type=float, default=0.6)
parser.add_argument("--record-stable-timeout-seconds", type=float, default=20.0)
parser.add_argument("--max-seconds", type=float, default=120.0)
# <modify> 如果server长时间不再返回action,bridge不能一直卡着等,否则orchestrator和WBC会持续刷屏不退出。
# 原逻辑:
# 无action空闲超时保护
parser.add_argument("--action-idle-timeout-seconds", type=float, default=8.0)
# <modify> v13 future_progress需要目标动作总帧数;0表示使用server默认值。
parser.add_argument("--target-frames", type=int, default=0)
# <modify> RTC使用30帧预测窗口、20帧执行推进;保留10帧overlap保证chunk连接连续。
# 原逻辑:
# parser.add_argument("--pose-chunk-frames", type=int, default=30)
# parser.add_argument("--pose-chunk-send-seconds", type=float, default=0.45)
# 后续逻辑:
# parser.add_argument("--pose-chunk-frames", type=int, default=20)
# parser.add_argument("--pose-chunk-send-seconds", type=float, default=0.35)
parser.add_argument("--pose-chunk-frames", type=int, default=30)
parser.add_argument("--pose-chunk-advance-frames", type=int, default=20)
parser.add_argument("--pose-chunk-send-seconds", type=float, default=0.0)
parser.add_argument("--dummy-image-height", type=int, default=240)
parser.add_argument("--dummy-image-width", type=int, default=320)
parser.add_argument("--verbose", action="store_true", default=False)
return parser.parse_args()
def format_motion_prompt(description: str) -> str:
description = str(description).strip()
return "Generate a motion sequence that matches the following description.\nDescription: " + description
def load_motion_prompt_plan(path_text: str, fps: int = CTRL_HZ) -> list[dict]:
if not path_text:
return []
motion_plan = json.loads(Path(path_text).read_text(encoding="utf-8"))
prompt_plan = []
for index, item in enumerate(motion_plan.get("segments", []), start=1):
low_level = str(item.get("low_level_instruction", "")).strip()
if not low_level:
raise ValueError(f"motion plan segment {index} has empty low_level_instruction")
frames = int(item.get("num_frames", 0))
if frames <= 0:
frames = int(round(float(item.get("duration_seconds", 0.0)) * fps))
if frames <= 0:
raise ValueError(f"motion plan segment {index} has invalid duration/frames")
prompt_plan.append(
{
"index": index,
"low_level_instruction": low_level,
"prompt": format_motion_prompt(low_level),
"frames": frames,
}
)
if not prompt_plan:
raise ValueError(f"No valid segments found in motion plan: {path_text}")
return prompt_plan
def pack_header(fields: list[dict], version: int = 1, count: int = 1) -> bytes:
header = {"v": version, "endian": "le", "count": count, "fields": fields}
header_json = json.dumps(header, separators=(",", ":")).encode("utf-8")
if len(header_json) > HEADER_SIZE:
raise ValueError(f"ZMQ header too large: {len(header_json)} > {HEADER_SIZE}")
return header_json.ljust(HEADER_SIZE, b"\x00")
class SonicZMQPublisher:
def __init__(self, host: str, port: int):
self.context = zmq.Context()
self.socket = self.context.socket(zmq.PUB)
self.endpoint = f"tcp://{host}:{port}"
self.socket.bind(self.endpoint)
time.sleep(0.5)
print(f"[sonic] bound {self.endpoint}")
def send_command(self, start: bool, stop: bool, planner: bool) -> None:
fields = [
{"name": "start", "dtype": "u8", "shape": [1]},
{"name": "stop", "dtype": "u8", "shape": [1]},
{"name": "planner", "dtype": "u8", "shape": [1]},
]
payload = b"".join(
[
struct.pack("B", 1 if start else 0),
struct.pack("B", 1 if stop else 0),
struct.pack("B", 1 if planner else 0),
]
)
self.socket.send(b"command" + pack_header(fields) + payload)
def send_pose(
self,
joint_pos: np.ndarray,
joint_vel: np.ndarray,
body_quat: np.ndarray,
frame_index: np.ndarray,
catch_up: bool = False,
) -> None:
joint_pos = np.asarray(joint_pos, dtype=np.float32)
joint_vel = np.asarray(joint_vel, dtype=np.float32)
body_quat = np.asarray(body_quat, dtype=np.float32)
frame_index = np.asarray(frame_index, dtype=np.int64)
n = int(joint_pos.shape[0])
fields = [
{"name": "joint_pos", "dtype": "f32", "shape": [n, 29]},
{"name": "joint_vel", "dtype": "f32", "shape": [n, 29]},
{"name": "body_quat_w", "dtype": "f32", "shape": [n, 4]},
{"name": "frame_index", "dtype": "i64", "shape": [n]},
{"name": "catch_up", "dtype": "u8", "shape": [1]},
]
payload = b"".join(
[
np.ascontiguousarray(joint_pos).tobytes(),
np.ascontiguousarray(joint_vel).tobytes(),
np.ascontiguousarray(body_quat).tobytes(),
np.ascontiguousarray(frame_index).tobytes(),
struct.pack("B", 1 if catch_up else 0),
]
)
self.socket.send(b"pose" + pack_header(fields, count=n) + payload)
def close(self) -> None:
self.socket.close()
self.context.term()
def load_state_npy(path: Path) -> np.ndarray:
state = np.load(path).astype(np.float32)
if state.ndim == 2:
state = state[-1]
if state.shape != (36,):
raise ValueError(f"Expected 36D state from {path}, got {state.shape}")
state[32:36] = normalize_quat_wxyz(state[32:36])
return state.astype(np.float32)
def load_state_from_motion_dir(motion_dir: Path) -> np.ndarray:
joint = pd.read_csv(motion_dir / "joint_pos.csv").to_numpy(dtype=np.float32)
body_pos = pd.read_csv(motion_dir / "body_pos.csv").to_numpy(dtype=np.float32)
body_quat = pd.read_csv(motion_dir / "body_quat.csv").to_numpy(dtype=np.float32)
state = np.concatenate([joint[0], body_pos[0], body_quat[0]], axis=0).astype(np.float32)
state[32:36] = normalize_quat_wxyz(state[32:36])
return state
def map_motion_dir(root_dir: Path, move_g1_path: str) -> Path:
rel = str(move_g1_path).strip().replace("\\", "/")
rel = rel.replace("g1/csv/", "g1/csv_sonic/")
rel = rel.replace("/csv/", "/csv_sonic/")
if rel.endswith(".csv"):
rel = rel[:-4]
if rel.startswith("dataset/seed/"):
rel = rel[len("dataset/seed/") :]
if rel.startswith("seed/"):
rel = rel[len("seed/") :]
path = Path(rel)
return path if path.is_absolute() else root_dir / path
def get_text_data_paths(data_config) -> tuple[Path, Path]:
if hasattr(data_config, "root_dir"):
data_root = Path(data_config.root_dir)
metadata_csv = Path(getattr(data_config, "metadata_csv", "metadata/seed_metadata_v004.csv"))
else:
text_config = getattr(data_config, "text", None)
if text_config is None:
raise AttributeError("data config has neither root_dir nor text config")
if isinstance(text_config, dict):
data_root = Path(text_config["root_dir"])
metadata_csv = Path(text_config.get("metadata_csv", "metadata/seed_metadata_v004.csv"))
else:
data_root = Path(text_config.root_dir)
metadata_csv = Path(getattr(text_config, "metadata_csv", "metadata/seed_metadata_v004.csv"))
if not metadata_csv.is_absolute():
metadata_csv = data_root / metadata_csv
return data_root, metadata_csv
def load_initial_state(args: argparse.Namespace) -> np.ndarray:
if args.init_state_npy:
return load_state_npy(Path(args.init_state_npy).expanduser())
if getattr(args, "init_motion_dir", ""):
motion_dir = Path(args.init_motion_dir).expanduser()
if not motion_dir.exists():
raise FileNotFoundError(f"init motion dir does not exist: {motion_dir}")
print(f"[state] init from motion dir: {motion_dir}")
return load_state_from_motion_dir(motion_dir)
run_dir = Path(args.run_dir)
cfg_template = parse_args_to_tyro_config(run_dir / "argv.txt") # type: ignore
launch_config = cfg_template.model_validate_json((run_dir / "run_config.json").read_text())
data_root, metadata_csv = get_text_data_paths(launch_config.data)
metadata = pd.read_csv(metadata_csv)
row_idx = min(max(0, int(args.start_index)), len(metadata) - 1)
motion_dir = map_motion_dir(data_root, str(metadata.iloc[row_idx]["move_g1_path"]))
print(f"[state] init from metadata index {row_idx}: {motion_dir}")
return load_state_from_motion_dir(motion_dir)
class StateProvider:
def __init__(self, live_state_npy: str, require_live: bool = False, max_age: float = 0.5):
self.live_state_npy = Path(live_state_npy).expanduser() if live_state_npy else None
self.require_live = require_live
self.max_age = max_age
self.last_mtime = 0.0
self.live_state = None
self.last_good_load_time = 0.0
# <modify> live-state偶尔卡住时不让发送线程无声崩掉;按秒限频打印fallback提示。
self.last_fallback_warn_time = 0.0
def get(self, fallback: np.ndarray) -> np.ndarray:
if self.live_state_npy is None:
return fallback.copy()
try:
stat = self.live_state_npy.stat()
if stat.st_mtime != self.last_mtime:
self.live_state = load_state_npy(self.live_state_npy)
self.last_mtime = stat.st_mtime
self.last_good_load_time = time.time()
except FileNotFoundError:
pass
except Exception as exc:
print(f"[state] live state load failed: {exc}")
if self.live_state is None:
if self.require_live:
raise RuntimeError(f"live state not available: {self.live_state_npy}")
return fallback.copy()
if self.require_live and time.time() - self.last_mtime > self.max_age:
raise RuntimeError(
f"live state is stale: age={time.time() - self.last_mtime:.3f}s path={self.live_state_npy}"
)
return self.live_state.copy()
def get_or_fallback(self, fallback: np.ndarray) -> np.ndarray:
# <modify> 首帧仍要求live-state就绪;运行中如果sim/live-state短暂过期,则沿用reference state继续把plan发完。
# 原逻辑:require_live_state下live-state超过max_age会抛异常,send_obs_loop线程退出但主流程还在等action。
try:
return self.get(fallback)
except RuntimeError as exc:
now = time.time()
if now - self.last_fallback_warn_time > 1.0:
print(f"[state] live state fallback to reference: {exc}", flush=True)
self.last_fallback_warn_time = now
return fallback.copy()
def has_live(self) -> bool:
return self.live_state is not None
def wait_first_live(self, fallback: np.ndarray, timeout: float) -> np.ndarray:
deadline = time.time() + timeout
while time.time() < deadline:
try:
state = self.get(fallback)
except RuntimeError:
state = fallback
if self.has_live() and time.time() - self.last_mtime <= self.max_age:
print(f"[state] live state ready: {self.live_state_npy}")
return state
time.sleep(0.05)
raise TimeoutError(f"live state not ready within {timeout:.1f}s: {self.live_state_npy}")
def wait_live_state_upright(
state_provider: StateProvider,
fallback: np.ndarray,
min_root_z: float,
stable_seconds: float,
timeout: float,
) -> tuple[float, float]:
if min_root_z <= 0.0 or stable_seconds <= 0.0:
return 0.0, float("nan")
start = time.time()
stable_start = None
best_z = -float("inf")
last_z = float("nan")
while time.time() - start < timeout:
now = time.time()
try:
state = state_provider.get(fallback)
root_z = float(state[31])
fresh = state_provider.has_live() and now - state_provider.last_mtime <= state_provider.max_age
except RuntimeError:
root_z = float("nan")
fresh = False
except Exception:
root_z = float("nan")
fresh = False
if np.isfinite(root_z):
best_z = max(best_z, root_z)
last_z = root_z
if fresh and root_z >= min_root_z:
if stable_start is None:
stable_start = now
if now - stable_start >= stable_seconds:
return now - start, root_z
else:
stable_start = None
time.sleep(0.05)
raise TimeoutError(
"robot did not become upright before recording: "
f"min_root_z={min_root_z:.3f}, stable_seconds={stable_seconds:.2f}, "
f"timeout={timeout:.2f}, last_root_z={last_z:.3f}, best_root_z={best_z:.3f}"
)
def wait_start_gate(ready_flag_text: str, gate_flag_text: str, timeout: float) -> None:
if ready_flag_text:
ready_flag = Path(ready_flag_text)
ready_flag.parent.mkdir(parents=True, exist_ok=True)
ready_flag.write_text(str(time.time()), encoding="utf-8")
print(f"[bridge] startup ready: {ready_flag}", flush=True)
if not gate_flag_text:
return
gate_flag = Path(gate_flag_text)
print(f"[bridge] waiting start gate: {gate_flag}", flush=True)
deadline = time.time() + timeout
while time.time() < deadline:
if gate_flag.exists():
print(f"[bridge] start gate released: {gate_flag}", flush=True)
return
time.sleep(0.1)
raise TimeoutError(f"start gate was not released within {timeout:.1f}s: {gate_flag}")
def write_render_timing(
path_text: str,
stream_frame_offset: int,
pre_roll_elapsed_seconds: float,
startup_seconds: float,
) -> None:
if not path_text:
return
path = Path(path_text)
path.parent.mkdir(parents=True, exist_ok=True)
payload = {
"stream_frame_offset": int(stream_frame_offset),
"ctrl_hz": CTRL_HZ,
"startup_seconds": float(startup_seconds),
"pre_roll_elapsed_seconds": float(pre_roll_elapsed_seconds),
"created_at": time.time(),
}
path.write_text(json.dumps(payload, sort_keys=True), encoding="utf-8")
print(
f"[bridge] render timing: startup_seconds={payload['startup_seconds']:.3f} path={path}",
flush=True,
)
def build_request_payload(
state: np.ndarray,
prompt: str,
image: np.ndarray,
target_frames: int = 0,
segment_index: int = 0,
segment_elapsed_frames: int = 0,
video_condition: dict | None = None,
) -> str:
# <modify> 将目标动作帧数放入condition,server端据此计算progress/future_progress。
condition = {}
if int(target_frames) > 0:
condition["progress_target_frames"] = int(target_frames)
# <modify> planner多段模式下把当前段号/段内帧号也发给RTC server,用于段切换时重置progress。
if int(segment_index) > 0:
condition["segment_index"] = int(segment_index)
condition["segment_elapsed_frames"] = int(segment_elapsed_frames)
if video_condition:
condition.update(video_condition)
payload = {
"image": {"cam0": image},
"state": {"states": state.astype(np.float32)},
"gt_action": None,
"dataset_name": None,
"instruction": prompt,
"history": None,
# 原逻辑:
# "condition": None,
"condition": condition,
"timestamp": None,
}
return json.dumps(convert_numpy_in_dict(payload, numpy_serialize))
def decode_response_actions(message: str) -> np.ndarray | None:
data = convert_numpy_in_dict(json.loads(message), numpy_deserialize)
if "action" not in data or data["action"] is None:
return None
action = np.asarray(data["action"], dtype=np.float32)
if action.ndim == 1:
action = action[None, :]
elif action.ndim == 3:
action = action[0]
elif action.ndim != 2:
raise ValueError(f"Unsupported action response shape: {action.shape}")
return action.astype(np.float32)
def decode_response_message(message: str) -> tuple[np.ndarray | None, dict]:
# <modify> 同时解析server返回的chunk元信息,方便bridge侧确认chunk是否连续。
data = convert_numpy_in_dict(json.loads(message), numpy_deserialize)
if "action" not in data or data["action"] is None:
return None, data
action = np.asarray(data["action"], dtype=np.float32)
if action.ndim == 1:
action = action[None, :]
elif action.ndim == 3:
action = action[0]
elif action.ndim != 2:
raise ValueError(f"Unsupported action response shape: {action.shape}")
return action.astype(np.float32), data
def action_to_abs_state(action: np.ndarray, current_state: np.ndarray) -> np.ndarray:
# <modify> v13 TextOp action是39D;不能把39D token误当成36D绝对姿态。
# 原逻辑:
# if action.shape[0] == 70:
if action.shape[0] == SEED_TEXTOP_ACTION_DIM:
next_state = recover_seed_abs_from_textop_action(action[None, :], current_state)[0]
elif action.shape[0] == 36:
next_state = action[:36].copy()
else:
raise ValueError(f"Unsupported action shape: {action.shape}")
next_state[32:36] = normalize_quat_wxyz(next_state[32:36])
return next_state.astype(np.float32)
def actions_to_abs_states(actions: np.ndarray, current_state: np.ndarray, max_frames: int) -> np.ndarray:
# <modify> SONIC streamed-motion一次发送多帧;TextOp action chunk按当前state恢复成连续绝对姿态。
actions = np.asarray(actions, dtype=np.float32)
if max_frames > 0:
actions = actions[:max_frames]
if actions.ndim != 2 or actions.shape[0] <= 0:
raise ValueError(f"Unsupported action chunk shape: {actions.shape}")
# <modify> v13 TextOp chunk按motion_repr.SEED_TEXTOP_ACTION_DIM恢复到36D Sonic姿态。
# 原逻辑:
# if actions.shape[1] == 70:
if actions.shape[1] == SEED_TEXTOP_ACTION_DIM:
states = recover_seed_abs_from_textop_action(actions, current_state)
elif actions.shape[1] == 36:
states = actions[:, :36].copy()
else:
raise ValueError(f"Unsupported action chunk shape: {actions.shape}")
for i in range(states.shape[0]):
states[i, 32:36] = normalize_quat_wxyz(states[i, 32:36])
return states.astype(np.float32)
def compose_policy_state(live_state: np.ndarray, reference_state: np.ndarray) -> np.ndarray:
# <modify> SONIC tracking下MuJoCo root xy可能不随参考全局平移走;如果直接用live root解码,
# 每个20帧chunk都会从接近0的root重新开始。这里保留live关节/root高度,同时用已发送reference
# 的root xy和root朝向作为连续位姿种子。
state = np.asarray(live_state, dtype=np.float32).copy()
state[29:31] = reference_state[29:31]
state[32:36] = reference_state[32:36]
return state
def main() -> None:
args = parse_args()
wait_start_gate(args.startup_ready_flag, args.start_gate_flag, args.start_gate_timeout)
ws_url = f"ws://{args.psi_host}:{args.psi_port}/ws"
# <modify> 保留v13 infer模板原文大小写;server端也不再强制lower。
# 原逻辑:
# prompt = args.prompt.strip().lower()
prompt = args.prompt.strip()
# <modify> 如果传入planner motion_plan.json,则按segments逐段切换low-level instruction。
prompt_plan = load_motion_prompt_plan(args.motion_plan_json, CTRL_HZ) if args.motion_plan_json else []
if prompt_plan:
total_plan_frames = sum(int(item["frames"]) for item in prompt_plan)
if args.target_frames <= 0:
args.target_frames = total_plan_frames
print(f"[planner] loaded {len(prompt_plan)} segments / {total_plan_frames} frames")
for item in prompt_plan:
print(
f"[planner] segment {item['index']}: {item['frames']} frames - {item['low_level_instruction']}",
flush=True,
)
else:
total_plan_frames = 0
prompt_lock = threading.Lock()
video_path = Path(args.inference_video_path).expanduser() if args.inference_video_path else None
if video_path is not None and not video_path.exists():
raise FileNotFoundError(f"inference video does not exist: {video_path}")
if video_path is not None:
print(
"[video] video_clip input: "
f"path={video_path} sample_fps={args.video_sample_fps} "
f"size_pixels={args.video_size_pixels} motion_fps={args.video_clip_motion_fps}",
flush=True,
)
print(
"[stream] "
f"chunk_frames={args.pose_chunk_frames} "
f"advance_frames={args.pose_chunk_advance_frames} "
f"coverage={args.pose_chunk_advance_frames / CTRL_HZ:.3f}s "
f"send_sleep={args.pose_chunk_send_seconds:.3f}s",
flush=True,
)
prompt_state = {
"prompt": prompt_plan[0]["prompt"] if prompt_plan else prompt,
"segment_index": int(prompt_plan[0]["index"]) if prompt_plan else 0,
"segment_target_frames": int(prompt_plan[0]["frames"]) if prompt_plan else int(args.target_frames),
"segment_elapsed_frames": 0,
"video_frame_cursor": 0,
}
def update_prompt_state(sent_frames: int) -> None:
if not prompt_plan:
return
cursor = 0
selected = prompt_plan[-1]
selected_start = max(0, total_plan_frames - int(selected["frames"]))
for item in prompt_plan:
next_cursor = cursor + int(item["frames"])
if sent_frames < next_cursor:
selected = item
selected_start = cursor
break
cursor = next_cursor
segment_elapsed = min(max(0, sent_frames - selected_start), int(selected["frames"]))
with prompt_lock:
old_index = int(prompt_state["segment_index"])
prompt_state["prompt"] = selected["prompt"]
prompt_state["segment_index"] = int(selected["index"])
prompt_state["segment_target_frames"] = int(selected["frames"])
prompt_state["segment_elapsed_frames"] = int(segment_elapsed)
if old_index != int(selected["index"]):
print(
f"[planner] switch to segment {selected['index']}: {selected['low_level_instruction']}",
flush=True,
)
dummy_image = np.zeros((args.dummy_image_height, args.dummy_image_width, 3), dtype=np.uint8)
current_state = load_initial_state(args)
state_provider = StateProvider(
args.live_state_npy,
require_live=args.require_live_state,
max_age=args.live_state_max_age,
)
if args.require_live_state:
if not args.live_state_npy:
raise ValueError("--require-live-state needs --live-state-npy")
try:
current_state = state_provider.wait_first_live(current_state, args.live_state_timeout)
except TimeoutError as exc:
raise SystemExit(str(exc)) from None
state_lock = threading.Lock()
stop_event = threading.Event()
first_pose_event = threading.Event()
startup_errors: list[str] = []
# <modify> 在线桥接:通过SONIC ZMQManager的command topic切到streamed-motion模式。
sonic = SonicZMQPublisher(args.sonic_zmq_host, args.sonic_zmq_port)
for _ in range(10):
sonic.send_command(start=True, stop=False, planner=False)
time.sleep(0.1)
print("[sonic] requested STREAMED_MOTION mode")
# <modify> Psi0 sonic server首帧会初始化RTC controller,首次回复可能比单帧周期慢很多。
ws = websocket.create_connection(ws_url, timeout=30)
ws.settimeout(30)
print(f"[psi] connected {ws_url}")
# <modify> 录制和MAX_SECONDS计时都延后到第一包pose真正发给WBC时开始。
# 原逻辑:
# if args.record_flag_path:
# record_flag = Path(args.record_flag_path)
# record_flag.parent.mkdir(parents=True, exist_ok=True)
# record_flag.write_text(str(time.time()), encoding="utf-8")
# print(f"[record] enabled: {record_flag}")
record_flag = Path(args.record_flag_path) if args.record_flag_path else None
settle_flag = Path(args.settle_flag_path) if args.settle_flag_path else None
def enable_recording(label: str) -> None:
if record_flag is None or record_flag.exists():
return
record_flag.parent.mkdir(parents=True, exist_ok=True)
record_flag.write_text(str(time.time()), encoding="utf-8")
print(f"[record] enabled: {record_flag} ({label})")
def send_obs_loop() -> None:
next_tick = time.perf_counter()
while not stop_event.is_set():
try:
with state_lock:
# <modify> 发送给Psi0的observation仍使用live关节,但root xy/yaw沿用reference累积值,
# 防止每个chunk的root全局位置被固定的仿真root重置。
# 原逻辑:
# state = state_provider.get(current_state)
live_state = state_provider.get_or_fallback(current_state)
state = compose_policy_state(live_state, current_state)
# <modify> 50Hz发送最新36D state;如果配置了live_state_npy,这里会优先使用真实/仿真状态。
with prompt_lock:
active_prompt = str(prompt_state["prompt"])
active_target_frames = int(prompt_state["segment_target_frames"])
active_segment_index = int(prompt_state["segment_index"])
active_segment_elapsed_frames = int(prompt_state["segment_elapsed_frames"])
active_video_frame_cursor = int(prompt_state["video_frame_cursor"])
video_condition = None
if video_path is not None:
video_clip_start_sec = float(active_video_frame_cursor) / float(args.video_clip_motion_fps)
video_clip_duration_sec = float(args.pose_chunk_frames) / float(args.video_clip_motion_fps)
video_condition = {
"input_mode": "video_clip",
"video_path": str(video_path),
"video_config_key": str(args.video_config_key),
"video_clip_start_sec": video_clip_start_sec,
"video_clip_duration_sec": video_clip_duration_sec,
"video_sample_fps": float(args.video_sample_fps),
"video_size_pixels": int(args.video_size_pixels),
"video_decoder": str(args.video_decoder),
"video_clip_motion_fps": float(args.video_clip_motion_fps),
}
ws.send(
build_request_payload(
state,
active_prompt,
dummy_image,
active_target_frames,
active_segment_index,
active_segment_elapsed_frames,
video_condition,
)
)
except Exception as exc:
# <modify> 发送线程出错时显式停止主循环,避免WBC只收到前几包后一直刷waiting。
print(f"[psi] send stopped: {exc}", flush=True)
if not first_pose_event.is_set():
startup_errors.append(f"send failed before first pose: {exc}")
stop_event.set()
break
next_tick += CTRL_DT
sleep_time = next_tick - time.perf_counter()
if sleep_time > 0.0:
time.sleep(sleep_time)
else:
next_tick = time.perf_counter()
sender = threading.Thread(target=send_obs_loop, daemon=True)
sender.start()
latest_actions = None
latest_action_meta = {}
latest_action_version = 0
action_lock = threading.Lock()
action_event = threading.Event()
def recv_action_loop() -> None:
nonlocal latest_actions, latest_action_meta, latest_action_version
while not stop_event.is_set():
try:
message = ws.recv()
except Exception as exc:
# <modify> server主动断开或测试结束时优雅退出,避免只留下长traceback。
# 原逻辑:主循环中直接ws.recv(),连接关闭会抛出完整异常。
print(f"[psi] recv stopped: {exc}")
if not first_pose_event.is_set():
startup_errors.append(f"recv failed before first pose: {exc}")
stop_event.set()
break
if message is None or message == "":
print("[psi] recv closed")
if not first_pose_event.is_set():
startup_errors.append("recv closed before first pose")
stop_event.set()
break
try:
# <modify> 解析server侧chunk_version/controller_generated_frames,后面每包打印出来确认连续性。
# 原逻辑:
# actions = decode_response_actions(message)
actions, meta = decode_response_message(message)
except Exception as exc:
# <modify> 测试结束或服务端返回非JSON控制消息时,只停止bridge,不打印大段traceback。
# 原逻辑:decode_response_actions(message)
print(f"[psi] decode stopped: {exc}", flush=True)
if not first_pose_event.is_set():
startup_errors.append(f"decode failed before first pose: {exc}")
stop_event.set()
break
if actions is None:
continue
with action_lock:
latest_actions = actions
latest_action_meta = dict(meta)
latest_action_version += 1
action_event.set()
receiver = threading.Thread(target=recv_action_loop, daemon=True)
receiver.start()
frame_idx = 0
prev_joint = current_state[:29].copy()
# <modify> start_time=None表示还没收到第一包action;避免server warmup时间挤占有效运动时长。
# 原逻辑:
# start_time = time.time()
start_time = None
last_action_receive_time = None
last_sent_version = 0
stream_frame_offset = 0
pre_roll_done = False
stream_target_frames = total_plan_frames if total_plan_frames > 0 else max(0, int(args.target_frames))
try:
while not stop_event.is_set():
# <modify> planner模式下以motion_plan的帧数为准,避免wall-clock计时把最后几个chunk提前截断。
# 原逻辑:
# if start_time is not None and args.max_seconds > 0 and time.time() - start_time > args.max_seconds:
if (
stream_target_frames <= 0
and start_time is not None
and args.max_seconds > 0
and time.time() - start_time > args.max_seconds
):
break
if stream_target_frames > 0 and frame_idx >= stream_target_frames:
break
if not action_event.wait(timeout=1.0):
# <modify> server静默卡住时,不要让bridge无限等待并持续打印fallback/wbc waiting日志。
# 原逻辑:
# if not action_event.wait(timeout=1.0):
# continue
if (
start_time is not None
and last_action_receive_time is not None
and args.action_idle_timeout_seconds > 0
and time.time() - last_action_receive_time > args.action_idle_timeout_seconds
):
raise RuntimeError(
"No new action received from Psi0 server for "
f"{time.time() - last_action_receive_time:.2f}s"
)
continue
with action_lock:
actions = None if latest_actions is None else latest_actions.copy()
action_meta = dict(latest_action_meta)
action_version = latest_action_version
action_event.clear()
if actions is None or action_version == last_sent_version:
continue
last_action_receive_time = time.time()
last_sent_version = action_version
update_prompt_state(frame_idx)
with state_lock:
# <modify> TextOp root delta解码也用连续reference root作为种子,否则20帧chunk之间root会回零。
# 原逻辑:
# decode_state = state_provider.get(current_state)
live_state = state_provider.get_or_fallback(current_state)
decode_state = compose_policy_state(live_state, current_state)
# <modify> Psi0 server返回30帧TextOp预测窗口;这里恢复成连续SONIC绝对姿态窗口。
# <modify> planner最后一段如果剩余不足一个chunk,只发送剩余帧数,保证650帧计划不会变成640/660帧。
# 原逻辑:
# state_chunk = actions_to_abs_states(actions, decode_state, args.pose_chunk_frames)
max_chunk_frames = int(args.pose_chunk_frames)
if stream_target_frames > 0:
max_chunk_frames = min(max_chunk_frames, max(0, stream_target_frames - frame_idx))
if max_chunk_frames <= 0:
break
advance_frames = min(
int(args.pose_chunk_advance_frames),
max(1, max_chunk_frames),
)
state_chunk = actions_to_abs_states(actions, decode_state, max_chunk_frames)
advance_frames = min(advance_frames, int(state_chunk.shape[0]))
next_current_state = state_chunk[advance_frames - 1].copy()
joint_pos = state_chunk[:, :29]
joint_vel = np.empty_like(joint_pos)
# <modify> 第一包首帧不要用metadata初始姿态和模型首帧直接差分,否则WBC会看到几十rad/s的尖峰速度。
# 原逻辑:
# joint_vel[0] = (joint_pos[0] - prev_joint) / CTRL_DT
if start_time is None:
joint_vel[0] = 0.0
else:
joint_vel[0] = (joint_pos[0] - prev_joint) / CTRL_DT
if joint_pos.shape[0] > 1:
joint_vel[1:] = (joint_pos[1:] - joint_pos[:-1]) / CTRL_DT
body_quat = state_chunk[:, 32:36]
n = int(state_chunk.shape[0])
frame_indices = np.arange(stream_frame_offset + frame_idx, stream_frame_offset + frame_idx + n, dtype=np.int64)
if start_time is None:
if not pre_roll_done and args.stand_pre_roll_seconds > 0.0:
# <modify> 正式动作前先让机器人追踪第一帧站稳,避免一开始从空中掉下来。
# 做法:给WBC发送一段重复第一帧的hold reference;sim看到settle_flag后释放elastic band但不录像。
# live_state连续站稳后再打开录像,额外录一段正常站立启动,再发送正式motion。
stand_frames = max(1, int(round(args.stand_pre_roll_seconds * CTRL_HZ)))
hold_seconds = max(
args.stand_pre_roll_seconds + 1.0,
args.record_stable_timeout_seconds + args.record_stable_seconds + 1.0,
)
hold_frames = max(
stand_frames + max(1, int(args.pose_chunk_frames)),
int(round(hold_seconds * CTRL_HZ)),
)
hold_joint_pos = np.repeat(joint_pos[:1], hold_frames, axis=0)
hold_joint_vel = np.zeros_like(hold_joint_pos)
hold_body_quat = np.repeat(body_quat[:1], hold_frames, axis=0)
hold_frame_indices = np.arange(0, hold_frames, dtype=np.int64)
pre_roll_start = time.time()
sonic.send_pose(hold_joint_pos, hold_joint_vel, hold_body_quat, hold_frame_indices, catch_up=False)
if settle_flag is not None:
settle_flag.parent.mkdir(parents=True, exist_ok=True)
settle_flag.write_text(str(time.time()), encoding="utf-8")
print(
f"[bridge] stand pre-roll: hold_frames=0..{hold_frames - 1} "
f"min_root_z={args.record_stable_min_root_z:.3f} "
f"stable_seconds={args.record_stable_seconds:.2f} "
f"timeout={args.record_stable_timeout_seconds:.2f}",
flush=True,
)
if args.record_stable_min_root_z > 0.0 and args.record_stable_seconds > 0.0:
stable_elapsed, stable_root_z = wait_live_state_upright(
state_provider,
current_state,
args.record_stable_min_root_z,
args.record_stable_seconds,
args.record_stable_timeout_seconds,
)
else:
time.sleep(args.stand_pre_roll_seconds)
stable_elapsed = time.time() - pre_roll_start
stable_root_z = float("nan")
record_start_time = time.time()
enable_recording("startup hold")
startup_hold_seconds = max(0.0, float(args.stand_pre_roll_seconds))
if startup_hold_seconds > 0.0:
print(f"[bridge] startup hold recording: {startup_hold_seconds:.2f}s", flush=True)
time.sleep(startup_hold_seconds)
recorded_startup_seconds = time.time() - record_start_time
elapsed_frames = max(stand_frames, int(round((time.time() - pre_roll_start) * CTRL_HZ)))
stream_frame_offset = min(max(0, elapsed_frames), max(0, hold_frames - int(args.pose_chunk_frames)))
print(
f"[bridge] stand pre-roll done: elapsed={stable_elapsed:.2f}s "
f"root_z={stable_root_z:.3f} "
f"startup_record={recorded_startup_seconds:.2f}s "
f"plan_start_stream_frame={stream_frame_offset}",
flush=True,
)
write_render_timing(
args.render_timing_json,
stream_frame_offset,
stable_elapsed,
recorded_startup_seconds,
)
pre_roll_done = True
last_sent_version = action_version
continue
start_time = time.time()
enable_recording("first pose")
first_pose_event.set()
print("[bridge] first pose sent, start MAX_SECONDS timer")
sonic.send_pose(joint_pos, joint_vel, body_quat, frame_indices, catch_up=False)
with state_lock:
# <modify> 发送30帧预测窗口,但reference状态只推进20帧;后10帧作为overlap供下一包覆盖。
# 原逻辑:
# current_state = state_chunk[-1].copy()
current_state = next_current_state
# <modify> prev_joint跟随已推进帧,而不是预测窗口最后一帧;窗口尾部会被下一包overlap覆盖。
# 原逻辑:
# prev_joint = joint_pos[-1].copy()
prev_joint = joint_pos[advance_frames - 1].copy()
root = state_chunk[advance_frames - 1, 29:32]
tail_root = state_chunk[-1, 29:32]
print(
f"[bridge] frames={frame_idx}..{frame_idx + n - 1} "
f"advance={advance_frames} "
f"root=({root[0]:.3f},{root[1]:.3f},{root[2]:.3f}) "
f"tail=({tail_root[0]:.3f},{tail_root[1]:.3f},{tail_root[2]:.3f}) "
f"action_shape={actions.shape} "
f"server_version={action_meta.get('version')} "
f"chunk_version={action_meta.get('chunk_version')} "
f"ctrl_frames={action_meta.get('controller_generated_frames')}"
)
frame_idx += advance_frames
update_prompt_state(frame_idx)
with prompt_lock:
prompt_state["video_frame_cursor"] = int(frame_idx)
if args.pose_chunk_send_seconds > 0:
time.sleep(args.pose_chunk_send_seconds)
except KeyboardInterrupt:
print("[bridge] interrupted")
except Exception as exc:
print(f"[bridge] stopped by error: {exc}")
if not first_pose_event.is_set():
startup_errors.append(str(exc))
finally:
stop_event.set()
try:
sonic.send_command(start=False, stop=True, planner=False)
except Exception:
pass
try:
ws.close()
except Exception:
pass
sonic.close()
print("[bridge] stopped")
if not first_pose_event.is_set():
if not startup_errors:
startup_errors.append("bridge stopped before first pose")
print("[bridge] fatal: " + "; ".join(startup_errors), flush=True)
raise SystemExit(1)
if __name__ == "__main__":
main()