code-HFD / src /psi /runtime /swa_rollout.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
06f9561 verified
Raw History Blame Contribute Delete
7.15 kB
"""The SAME closed-loop body rollout for SWA deployment and held-out eval."""
from __future__ import annotations
import numpy as np
import torch
def inplace_future_progress(frame_id: int, total_frames: int, horizon: int = 30):
"""SWA blends *inside* the task, never inserts a frozen phase prefix."""
if total_frames <= 0 or frame_id < 0:
raise ValueError("Invalid motion progress timeline")
p = np.minimum(np.arange(frame_id + 1, frame_id + horizon + 1), total_frames - 1) / max(total_frames - 1, 1)
return np.stack((p, 1 - p, p, np.sin(2 * np.pi * p), np.cos(2 * np.pi * p)), -1).astype(np.float32)
class ReplyDebounce:
"""KEEP from the old paired experiment: two consecutive ready windows."""
def __init__(self, consecutive=2):
if consecutive < 1:
raise ValueError("Positive debounce count required")
self.consecutive, self.streak = consecutive, 0
def update(self, probability):
self.streak = self.streak + 1 if probability >= .5 else 0
return self.streak == self.consecutive
def body_metrics(prediction, target, *, fps=50):
"""Old experiment's physical-space metrics, also reusable by audio tasks.
Score executed frames, including seams; never teacher-force, DTW, or align
away drift. Acceleration *error* additionally prevents rewarding freezing.
"""
p, g = np.asarray(prediction, np.float64), np.asarray(target, np.float64)
n = min(len(p), len(g))
if n < 3 or p.shape[1:] != (36,) or g.shape[1:] != (36,):
raise ValueError("Body metrics require >=3 physical 36D frames")
p, g = p[:n], g[:n]
if not (np.isfinite(p).all() and np.isfinite(g).all()):
raise ValueError("Non-finite closed-loop motion")
def rms(x):
return float(np.sqrt(np.mean(x ** 2)))
pq, gq = p[:, 32:36], g[:, 32:36]
pn, gn = np.linalg.norm(pq, axis=-1), np.linalg.norm(gq, axis=-1)
if (pn < 1e-8).any() or (gn < 1e-8).any():
raise ValueError("Zero root quaternion")
dot = np.clip(np.abs(np.sum(pq * gq, -1) / (pn * gn)), 0, 1)
return dict(
joint_rmse_rad=rms(p[:, :29] - g[:, :29]),
root_xy_rmse_m=rms(p[:, 29:31] - g[:, 29:31]),
root_z_rmse_m=rms(p[:, 31] - g[:, 31]),
root_rotation_deg=float(np.mean(np.degrees(2 * np.arccos(dot)))),
joint_velocity_error_rad_s=rms(np.diff(p[:, :29] - g[:, :29], axis=0) * fps),
joint_acceleration_error_rad_s2=rms(np.diff(p[:, :29] - g[:, :29], n=2, axis=0) * fps ** 2),
)
@torch.inference_mode()
def rollout_source(model, launch_config, source, *, speech=None, observer=None):
import os
from psi.data.seed.native_visual import source_visual_mode
source = source_visual_mode(source, os.environ.get('HFD_VISUAL_INPUT_MODE',
getattr(getattr(launch_config, 'data', None), 'multimodal_visual_input_mode', 'cached')))
from psi.data.seed.swa_visual_qa_dataset import _load_body
from psi.runtime.motion_rollout import (
_flow_action, _stationary_future_progress, build_model_states_from_abs,
normalize_quaternion_rows,
clamp_generated_actions, decode_action_chunk, next_rtc_prefix,
)
from diffusers import FlowMatchEulerDiscreteScheduler
if model.training:
raise ValueError("Closed-loop evaluation must use model.eval()")
model.device = next(model.parameters()).device
# Training owns another scheduler; do not alter its flow noise schedule.
model.noise_scheduler = FlowMatchEulerDiscreteScheduler(
num_train_timesteps=int(launch_config.model.train_diffusion_steps))
steps = [s for s in source["steps"] if s["update_semantic_cache"]]
packets = source["swa_packets"]
if len(steps) != len(packets) or not packets:
raise RuntimeError("SWA action and AV packet timelines disagree")
if "multimodal_overlay" in source:
if not bool(getattr(launch_config.data, "dynamic_task_entry_blend", False)):
raise ValueError("Unified overlay rollout requires the current in-place motion progress geometry")
# Exactly the same initialization used by the ordinary action rollout;
# subsequent states/RTC tails are generated, never read from GT.
current = np.asarray(source["initial_abs_state"], np.float32).copy()
else:
body = _load_body(str(source["swa_job"]["path"]))
current = body["initial_abs_state"].copy()
cache = model.begin_source([source["source_id"]])
known, old_control, progress, duration = None, None, 0, 1000
field = launch_config.data.transform.field
generator = torch.Generator(device=model.device).manual_seed(1911)
trigger = ReplyDebounce()
poses, events = [], []
for index, (step, packet) in enumerate(zip(steps, packets)):
model.swa_packets = [packet]
with torch.autocast(model.device.type, dtype=torch.bfloat16, enabled=model.device.type == "cuda"):
condition = model._encode_qwen_stream_input(
source_ids=[source["source_id"]], cache=cache, update_semantic_cache=True,
)
cache = condition.kv_cache
control = int(condition.control_logits.argmax(-1).item())
probability = float(model.reply_logits.softmax(-1)[0, 1])
if trigger.update(probability) and speech is not None:
speech.submit(cache, window=index, seconds=float(packet["available_seconds"]))
if control == 1 and old_control != 1:
progress = 0
duration = max(1, round(float(torch.expm1(condition.motion_duration_log1p_seconds[0])) * 50))
future = inplace_future_progress(progress, duration) if control == 1 else _stationary_future_progress(1000)
state = build_model_states_from_abs([current], textop_actions=True, odim=int(launch_config.model.odim))
if bool(field.normalize_state):
state = field.normalize_state_func(state)
prediction = _flow_action(
model, condition, known_prefix=known, states=torch.as_tensor(state[None], device=model.device),
future_progress=torch.as_tensor(future[None], device=model.device),
num_inference_steps=10, generator=generator,
)
prediction = clamp_generated_actions(prediction, known)
# Carry the generated 10-frame tail even across token changes.
normalized = prediction[0].float().cpu().numpy()
absolute = decode_action_chunk(normalized, current, field)
known = torch.as_tensor(next_rtc_prefix(normalized, current, absolute[19], field)[None],
device=model.device)
poses.append(absolute[:20])
current = absolute[19].copy()
if control == 1:
progress += 20
old_control = control
events.append(dict(window=index, seconds=float(packet["available_seconds"]),
control=control, control_gt=int(step["control_label"]),
reply_probability=probability, reply_gt=int(packet["reply_label"])))
if observer is not None:
observer(model, cache, packet)
cache.detach()
return np.concatenate(poses), events