"""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