Download src/psi/runtime/swa_rollout.py from Tinghong-Ye22/code-HFD: direct link, hf CLI and curl.
- Browser
- Download file 7.15 kB
-
https://huggingface.co/Tinghong-Ye22/code-HFD/resolve/main/src/psi/runtime/swa_rollout.py
- Command line
-
hf download hf://Tinghong-Ye22/code-HFD/src/psi/runtime/swa_rollout.py
-
curl -L -o swa_rollout.py https://huggingface.co/Tinghong-Ye22/code-HFD/resolve/main/src/psi/runtime/swa_rollout.py
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), | |
| ) | |
| 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 | |