#!/usr/bin/env python3 from __future__ import annotations import argparse import json import math import shutil from pathlib import Path from typing import List import numpy as np import pandas as pd import torch import torch.nn as nn from safetensors import safe_open from psi.config.config import LaunchConfig from psi.data.seed.motion_repr import ( build_seed_textop_action, quat_inv_wxyz, quat_mul_wxyz, recover_seed_abs_from_textop_action, ) from psi.data.seed.progress import build_progress_conditions from psi.models.motion_distill import MotionDistillPsi0Model from psi.utils import parse_args_to_tyro_config, seed_everything from seed_text_to_motion import ( TARGET_FPS, build_model_states_from_abs, finite_difference, load_source_frame_count, normalize_quaternion_rows, quaternion_angular_velocity, recover_seed_abs_from_textop_action_with_joint_delta, resolve_ckpt_step, sanitize_motion_name, should_decode_textop_joint_from_delta, write_csv, write_metadata, ) def resolve_checkpoint_dir(run_dir: Path, ckpt_step: int | str) -> Path: if str(ckpt_step) in {"best", "best_ckpt"}: ckpt_dir = run_dir / "checkpoints" / "best_ckpt" else: ckpt_dir = run_dir / "checkpoints" / f"ckpt_{ckpt_step}" if not ckpt_dir.exists(): root_ckpt_dir = run_dir / f"ckpt_{ckpt_step}" if root_ckpt_dir.exists(): ckpt_dir = root_ckpt_dir if not ckpt_dir.exists(): raise FileNotFoundError(f"Checkpoint dir does not exist: {ckpt_dir}") return ckpt_dir def load_selected_checkpoint_state(ckpt_dir: Path, prefixes: tuple[str, ...]) -> dict[str, torch.Tensor]: model_path = ckpt_dir / "model.safetensors" index_path = ckpt_dir / "model.safetensors.index.json" shard_paths: list[Path] if index_path.exists(): weight_map = json.loads(index_path.read_text()).get("weight_map", {}) shard_paths = sorted( {ckpt_dir / shard for key, shard in weight_map.items() if key.startswith(prefixes)} ) elif model_path.exists(): shard_paths = [model_path] else: raise FileNotFoundError(f"No safetensors checkpoint found under {ckpt_dir}") state_dict: dict[str, torch.Tensor] = {} for shard_path in shard_paths: with safe_open(str(shard_path), framework="pt", device="cpu") as f: for key in f.keys(): if key.startswith(prefixes): state_dict[key] = f.get_tensor(key) return state_dict def build_noise_scheduler(launch_config: LaunchConfig): if launch_config.model.noise_scheduler == "ddpm": from diffusers.schedulers.scheduling_ddim import DDIMScheduler return DDIMScheduler( num_train_timesteps=launch_config.model.train_diffusion_steps, beta_start=0.0001, beta_end=0.02, beta_schedule="squaredcos_cap_v2", clip_sample=True, set_alpha_to_one=True, steps_offset=0, prediction_type="epsilon", ) if launch_config.model.noise_scheduler == "flow": from diffusers.schedulers.scheduling_flow_match_euler_discrete import FlowMatchEulerDiscreteScheduler return FlowMatchEulerDiscreteScheduler( num_train_timesteps=launch_config.model.train_diffusion_steps, ) raise ValueError(f"Unsupported noise scheduler: {launch_config.model.noise_scheduler}") def load_teacher_reconstruct_model( run_dir: Path, ckpt_step: int | str, launch_config: LaunchConfig, device: str, ) -> MotionDistillPsi0Model: ckpt_dir = resolve_checkpoint_dir(run_dir, ckpt_step) state_dict = load_selected_checkpoint_state( ckpt_dir, prefixes=("action_header.", "motion_encoder."), ) model = MotionDistillPsi0Model( model_cfg=launch_config.model, vlm_model=nn.Module(), ) action_state = { key[len("action_header.") :]: value for key, value in state_dict.items() if key.startswith("action_header.") } motion_state = { key[len("motion_encoder.") :]: value for key, value in state_dict.items() if key.startswith("motion_encoder.") } if not action_state: raise ValueError(f"No action_header weights found in {ckpt_dir}") if not motion_state: raise ValueError(f"No motion_encoder weights found in {ckpt_dir}") model.action_header.load_state_dict(action_state, strict=True) model.motion_encoder.load_state_dict(motion_state, strict=True) model.noise_scheduler = build_noise_scheduler(launch_config) model.action_horizon = int(launch_config.model.action_chunk_size) model.action_dim = int(launch_config.model.action_dim) model.device = device return model.to(device) def load_motion_abs(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) if not (len(joint) == len(body_pos) == len(body_quat)): raise ValueError(f"Motion csv length mismatch under {motion_dir}") return np.concatenate([joint, body_pos, body_quat], axis=1).astype(np.float32) def build_abs_action_chunk( motion_abs: np.ndarray, frame_id: int, horizon: int, sample_stride: int = 1, ) -> np.ndarray: total_frames = int(motion_abs.shape[0]) actions: List[np.ndarray] = [] for step in range(horizon): raw_index = frame_id + (step + 1) * sample_stride actions.append(motion_abs[min(raw_index, total_frames - 1)]) return np.stack(actions, axis=0).astype(np.float32) def build_teacher_action_features( actions_abs: np.ndarray, current_abs: np.ndarray, *, textop_actions: bool, action_dim: int, ) -> np.ndarray: if textop_actions: actions = build_seed_textop_action(actions_abs[:, :36], current_abs[:36]) else: actions = actions_abs[:, :action_dim] if actions.shape[-1] < action_dim: pad = np.zeros((actions.shape[0], action_dim - actions.shape[-1]), dtype=np.float32) actions = np.concatenate([actions, pad], axis=-1) if actions.shape[-1] != action_dim: raise ValueError(f"Teacher action dim mismatch: built {actions.shape[-1]}, expected {action_dim}") return actions.astype(np.float32) def normalize_action_only(maxmin, actions: np.ndarray) -> np.ndarray: action_min = np.asarray(maxmin.action_min, dtype=np.float32) action_max = np.asarray(maxmin.action_max, dtype=np.float32) ill_mask = np.abs(action_max - action_min) < 1e-4 * ( np.abs(action_max) + np.abs(action_min) + 1e-8 ) denom = action_max - action_min denom[ill_mask] = 1.0 actions_norm = np.where( ill_mask, actions, (actions - action_min) / denom * 2 - 1, ) if bool(getattr(maxmin, "use_norm_mask", False)): action_norm_masks = np.asarray(maxmin.action_norm_masks, dtype=bool) actions_norm = np.where(action_norm_masks, actions_norm, actions) return np.clip(actions_norm, -1.0, 1.0).astype(np.float32) def decode_action_chunk( chunk_pred: np.ndarray, current_abs: np.ndarray, *, textop_actions: bool, decode_textop_joint_from_delta: bool, ) -> tuple[np.ndarray, np.ndarray]: if textop_actions: if decode_textop_joint_from_delta: chunk_abs = recover_seed_abs_from_textop_action_with_joint_delta(chunk_pred, current_abs) else: chunk_abs = recover_seed_abs_from_textop_action(chunk_pred, current_abs) prev_abs = np.concatenate([current_abs[None, :], chunk_abs[:-1]], axis=0) joint_delta = chunk_abs[:, :29] - prev_abs[:, :29] root_pos_delta = chunk_abs[:, 29:32] - prev_abs[:, 29:32] root_quat_delta = quat_mul_wxyz(quat_inv_wxyz(prev_abs[:, 32:36]), chunk_abs[:, 32:36]) chunk_delta = np.concatenate([joint_delta, root_pos_delta, root_quat_delta], axis=1) else: chunk_abs = chunk_pred[:, :36] first_delta = chunk_abs[0] - current_abs if chunk_abs.shape[0] > 1: rest_delta = chunk_abs[1:] - chunk_abs[:-1] chunk_delta = np.concatenate([first_delta[None, :], rest_delta], axis=0) else: chunk_delta = first_delta[None, :] return normalize_quaternion_rows(chunk_abs.astype(np.float32)), chunk_delta.astype(np.float32) def reconstruct_teacher_motion( *, model: MotionDistillPsi0Model, maxmin, motion_abs: np.ndarray, odim: int, action_dim: int, device: str, normalize_state: bool, textop_actions: bool, decode_textop_joint_from_delta: bool, num_past_frames: int, use_future_progress_embedding: bool, num_inference_steps: int, target_frames: int, ) -> tuple[np.ndarray, np.ndarray, dict[str, float]]: horizon = int(model.action_horizon) if target_frames <= 0: target_frames = max(1, int(motion_abs.shape[0]) - 1) num_chunks = int(math.ceil(float(target_frames) / float(horizon))) all_abs: List[np.ndarray] = [] all_delta: List[np.ndarray] = [] remaining_frames = int(target_frames) for chunk_id in range(num_chunks): frame_id = chunk_id * horizon current_abs = motion_abs[min(frame_id, motion_abs.shape[0] - 1)].astype(np.float32) state_indices = [ max(0, frame_id - (int(num_past_frames) - t)) for t in range(int(num_past_frames) + 1) ] states = build_model_states_from_abs( [motion_abs[min(i, motion_abs.shape[0] - 1)] for i in state_indices], textop_actions=textop_actions, odim=odim, ) if normalize_state: states = maxmin.normalize_state_func(states) actions_abs = build_abs_action_chunk(motion_abs, frame_id, horizon) teacher_actions = build_teacher_action_features( actions_abs, current_abs, textop_actions=textop_actions, action_dim=action_dim, ) teacher_actions_norm = normalize_action_only(maxmin, teacher_actions) with torch.inference_mode(): states_t = torch.from_numpy(states).unsqueeze(0).to(device) teacher_actions_t = torch.from_numpy(teacher_actions_norm).unsqueeze(0).to(device) hidden_states = model.encode_motion_hidden(teacher_actions_t) future_progress_t = None if use_future_progress_embedding: _, future_progress = build_progress_conditions( frame_id=frame_id, total_frames=int(motion_abs.shape[0]), action_horizon=horizon, ) future_progress_t = torch.from_numpy(future_progress[None]).to(device) action_samples = torch.randn(1, horizon, action_dim, device=device) model.noise_scheduler.set_timesteps(num_inference_steps) for timestep in model.noise_scheduler.timesteps: batched_timestep = timestep.expand(1).to(device) model_output = model.action_from_hidden( hidden_states=hidden_states, attention_mask=None, action_samples=action_samples, states=states_t, timestep=batched_timestep, traj2ds=None, future_progress=future_progress_t, return_dict=True, ) action_samples = model.noise_scheduler.step( model_output=model_output.action, timestep=timestep, sample=action_samples, ).prev_sample pred_norm = action_samples[0].float().cpu().numpy() pred_norm_clipped = np.clip(np.asarray(pred_norm, dtype=np.float32), -1.0, 1.0) chunk_pred = np.asarray(maxmin.denormalize(pred_norm_clipped), dtype=np.float32) chunk_abs, chunk_delta = decode_action_chunk( chunk_pred, current_abs, textop_actions=textop_actions, decode_textop_joint_from_delta=decode_textop_joint_from_delta, ) take = min(horizon, remaining_frames) all_abs.append(chunk_abs[:take]) all_delta.append(chunk_delta[:take]) remaining_frames -= take pred_abs = np.concatenate(all_abs, axis=0)[:target_frames] pred_delta = np.concatenate(all_delta, axis=0)[:target_frames] pred_abs = normalize_quaternion_rows(pred_abs.astype(np.float32)) ref_abs = motion_abs[1 : 1 + pred_abs.shape[0], :36] if ref_abs.shape[0] == pred_abs.shape[0]: diff = pred_abs - ref_abs metrics = { "joint_mse": float(np.mean(diff[:, :29] ** 2)), "root_pos_mse": float(np.mean(diff[:, 29:32] ** 2)), "all_abs_mse": float(np.mean(diff ** 2)), } else: metrics = {} return pred_abs, pred_delta, metrics def write_teacher_info( path: Path, *, source_motion_dir: Path, frames: int, textop_actions: bool, num_past_frames: int, num_inference_steps: int, metrics: dict[str, float], ) -> None: lines = [ "Motion Information: motion_distill_teacher_reconstruct", "======================================================", "", f"source_motion_dir: {source_motion_dir}", f"target_fps: {TARGET_FPS}", f"num_inference_steps: {num_inference_steps}", f"trained_with_textop_actions: {textop_actions}", f"num_past_frames: {num_past_frames}", "condition: motion_encoder(ground_truth_actions)", "output_root_quaternion: wxyz", "output_joint_angles: 29D", "output_root_position: xyz", "", f"frames: {frames}", "", ] for key, value in metrics.items(): lines.append(f"{key}: {value:.8f}") path.write_text("\n".join(lines), encoding="utf-8") def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description="Motion-distill teacher motion reconstruction exporter") parser.add_argument("--run-dir", type=str, required=True) parser.add_argument("--ckpt-step", type=str, default="best", help="integer step, 'latest', or 'best'") parser.add_argument("--output-dir", type=str, required=True) parser.add_argument("--motion-dir", type=str, required=True) parser.add_argument("--num-output-frames", type=int, default=0, help="0 means source motion length minus one") parser.add_argument("--num-inference-steps", type=int, default=10) parser.add_argument("--device", type=str, default="cuda:0") parser.add_argument("--seed", type=int, default=42) parser.add_argument("--export-delta", action="store_true", default=False) return parser.parse_args() def main() -> None: args = parse_args() run_dir = Path(args.run_dir) output_dir = Path(args.output_dir) motion_dir = Path(args.motion_dir) output_dir.mkdir(parents=True, exist_ok=True) cfg_template: LaunchConfig = 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()) seed_everything(launch_config.seed or args.seed) ckpt_step = resolve_ckpt_step(run_dir, args.ckpt_step) model = load_teacher_reconstruct_model(run_dir, ckpt_step, launch_config, device=args.device) model.eval() motion_abs = load_motion_abs(motion_dir) source_frames = load_source_frame_count(motion_dir) or int(motion_abs.shape[0]) target_frames = int(args.num_output_frames) if args.num_output_frames > 0 else max(1, source_frames - 1) maxmin = launch_config.data.transform.field odim = int(launch_config.model.odim) action_dim = int(getattr(launch_config.model, "action_dim", getattr(model, "action_dim", 39))) textop_actions = bool(getattr(launch_config.data.transform.repack, "textop_actions", False)) decode_textop_joint_from_delta = textop_actions and should_decode_textop_joint_from_delta(launch_config) num_past_frames = int(getattr(launch_config.data.transform.repack, "num_past_frames", 0)) use_future_progress_embedding = bool(getattr(launch_config.model, "use_future_progress_embedding", False)) pred_abs, pred_delta, metrics = reconstruct_teacher_motion( model=model, maxmin=maxmin, motion_abs=motion_abs, odim=odim, action_dim=action_dim, device=args.device, normalize_state=bool(getattr(maxmin, "normalize_state", False)), textop_actions=textop_actions, decode_textop_joint_from_delta=decode_textop_joint_from_delta, num_past_frames=num_past_frames, use_future_progress_embedding=use_future_progress_embedding, num_inference_steps=int(args.num_inference_steps), target_frames=target_frames, ) motion_name = sanitize_motion_name(motion_dir.name) sample_dir = output_dir / motion_name sample_dir.mkdir(parents=True, exist_ok=True) joint_pos = pred_abs[:, :29] body_pos = pred_abs[:, 29:32] body_quat = pred_abs[:, 32:36] dt = 1.0 / float(TARGET_FPS) joint_vel = finite_difference(joint_pos, dt) body_lin_vel = finite_difference(body_pos, dt) body_ang_vel = quaternion_angular_velocity(body_quat, dt) write_csv(sample_dir / "joint_pos.csv", joint_pos, [f"joint_{i}" for i in range(29)]) write_csv(sample_dir / "joint_vel.csv", joint_vel, [f"joint_vel_{i}" for i in range(29)]) write_csv(sample_dir / "body_pos.csv", body_pos, ["body_0_x", "body_0_y", "body_0_z"]) write_csv(sample_dir / "body_quat.csv", body_quat, ["body_0_w", "body_0_x", "body_0_y", "body_0_z"]) write_csv(sample_dir / "body_lin_vel.csv", body_lin_vel, ["body_0_vel_x", "body_0_vel_y", "body_0_vel_z"]) write_csv(sample_dir / "body_ang_vel.csv", body_ang_vel, ["body_0_angvel_x", "body_0_angvel_y", "body_0_angvel_z"]) np.save(sample_dir / "motion.npy", pred_abs) if args.export_delta: write_csv(sample_dir / "joint_pos_delta.csv", pred_delta[:, :29], [f"joint_{i}" for i in range(29)]) write_csv(sample_dir / "body_pos_delta.csv", pred_delta[:, 29:32], ["body_0_x", "body_0_y", "body_0_z"]) write_csv(sample_dir / "body_quat_delta.csv", pred_delta[:, 32:36], ["body_0_w", "body_0_x", "body_0_y", "body_0_z"]) np.save(sample_dir / "motion_delta.npy", pred_delta) write_metadata(sample_dir / "metadata.txt", motion_name, int(pred_abs.shape[0])) write_teacher_info( sample_dir / "info.txt", source_motion_dir=motion_dir, frames=int(pred_abs.shape[0]), textop_actions=textop_actions, num_past_frames=num_past_frames, num_inference_steps=int(args.num_inference_steps), metrics=metrics, ) for src_name, dst_name in (("info.txt", "info_source.txt"), ("metadata.txt", "metadata_source.txt")): src_path = motion_dir / src_name if src_path.exists(): shutil.copy2(src_path, sample_dir / dst_name) sample_meta = { "run_dir": str(run_dir), "ckpt_step": ckpt_step, "source_motion_dir": str(motion_dir), "output_dir": str(sample_dir), "num_output_frames": int(pred_abs.shape[0]), "textop_actions": textop_actions, "decode_textop_joint_from_delta": decode_textop_joint_from_delta, "num_past_frames": int(num_past_frames), "num_inference_steps": int(args.num_inference_steps), "metrics": metrics, } (sample_dir / "infer_meta.json").write_text(json.dumps(sample_meta, ensure_ascii=False, indent=2), encoding="utf-8") (output_dir / "manifest.json").write_text( json.dumps({"samples": [sample_meta]}, ensure_ascii=False, indent=2), encoding="utf-8", ) print(f"[OK] teacher reconstruction -> {sample_dir}") if metrics: print("[METRICS] " + json.dumps(metrics, ensure_ascii=False)) if __name__ == "__main__": main()