Download scripts/deploy/online_psi0_sonic_bridge.py from Tinghong-Ye22/code-Psi0-yth-v21: direct link, hf CLI and curl.
- Browser
- Download file 46.1 kB
-
https://huggingface.co/Tinghong-Ye22/code-Psi0-yth-v21/resolve/main/scripts/deploy/online_psi0_sonic_bridge.py
- Command line
-
hf download hf://Tinghong-Ye22/code-Psi0-yth-v21/scripts/deploy/online_psi0_sonic_bridge.py
-
curl -L -o online_psi0_sonic_bridge.py https://huggingface.co/Tinghong-Ye22/code-Psi0-yth-v21/resolve/main/scripts/deploy/online_psi0_sonic_bridge.py
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() | |