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