File size: 7,152 Bytes
06f9561 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 | """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
|