"""Shared deployment action solver/state conversion; used by audio, SWA and eval.""" from __future__ import annotations import contextlib from typing import TYPE_CHECKING, List import numpy as np import torch from psi.data.seed.full_duplex_timing import ACTION_CHUNK_FRAMES, RTC_PREFIX_FRAMES from psi.data.seed.motion_repr import SEED_ABS_DIM, build_seed_textop_state if TYPE_CHECKING: from psi.models.full_duplex_psi0 import FullDuplexPsi0Model, FullDuplexStreamCondition def _autocast(device): return torch.autocast("cuda", dtype=torch.bfloat16) if device.type == "cuda" else contextlib.nullcontext() def _model_device(model) -> torch.device: """Resolve a model device without requiring an ad-hoc ``model.device`` field.""" if isinstance(model, torch.nn.Module): # A manually attached attribute can be stale after model.to(...). # Runtime tensors must follow the actual parameters. try: return next(model.parameters()).device except StopIteration: try: return next(model.buffers()).device except StopIteration: pass device = getattr(model, "device", None) if device is not None: return torch.device(device) raise RuntimeError("Action rollout model has no parameters/buffers or explicit device") def _flow_action( model: FullDuplexPsi0Model, condition: FullDuplexStreamCondition, *, known_prefix: torch.Tensor | None, states: torch.Tensor, future_progress: torch.Tensor, num_inference_steps: int, generator: torch.Generator, prefix_present: torch.Tensor | None = None, scheduler=None, ) -> torch.Tensor: # v26 keeps one complete 30-frame action timeline in every solver call. # With RTC enabled the first 10 positions are overwritten with the # previous chunk's clean tail at *every* solver iteration and receive # timestep zero; the remaining 20 positions are the newly solved postfix. # Splitting the call into a 10-frame context plus a 20-frame model input is # a different objective and does not match the v26 training path. sample_horizon = ACTION_CHUNK_FRAMES batch = states.shape[0] device = _model_device(model) scheduler = model.noise_scheduler if scheduler is None else scheduler samples = torch.randn( (batch, sample_horizon, int(model.action_dim)), device=device, dtype=torch.float32, generator=generator, ) prefix_mask: torch.Tensor | None = None prefix = None if known_prefix is not None: prefix = known_prefix.to(device=device, dtype=samples.dtype) expected_prefix_shape = (batch, RTC_PREFIX_FRAMES, int(model.action_dim)) if tuple(prefix.shape) != expected_prefix_shape: raise ValueError( "RTC prefix must contain exactly the deployment overlap: " f"got={tuple(prefix.shape)} expected={expected_prefix_shape}" ) prefix_mask = torch.zeros( (batch, ACTION_CHUNK_FRAMES), device=device, dtype=torch.bool, ) prefix_mask[:, :RTC_PREFIX_FRAMES] = True if prefix_present is not None: if tuple(prefix_present.shape) != (batch,): raise ValueError("prefix_present must have one flag per source") prefix_mask &= prefix_present.to(device=device, dtype=torch.bool)[:, None] elif prefix_present is not None: raise ValueError("prefix_present requires a prefix tensor") scheduler.set_timesteps(int(num_inference_steps), device=device) for timestep in scheduler.timesteps: # Training always supplies one diffusion time per action frame. Keep # that exact (B,30) shape even for the first source window, where no # RTC prefix exists and every frame has the same time value. batched_timestep = timestep.expand(batch, ACTION_CHUNK_FRAMES).to( device ) if prefix_mask is not None and prefix is not None: batched_timestep = torch.where( prefix_mask, torch.zeros_like(batched_timestep), batched_timestep, ) # ``prefix`` contains only the 10 overlapping frames, whereas the # solver sample contains the complete 30-frame RTC timeline. Copy # it into the leading slice instead of broadcasting incompatible # 10- and 30-frame tensors. fixed = torch.cat( [prefix, samples[:, RTC_PREFIX_FRAMES:]], dim=1, ) samples = torch.where(prefix_mask[:, :, None], fixed, samples) with _autocast(device): prediction = model.forward_action_conditioned( condition=condition, action_samples=samples, known_action_prefix=None, states=states, timestep=batched_timestep, future_progress=future_progress, ) samples = scheduler.step( model_output=prediction, timestep=timestep, sample=samples, ).prev_sample if prefix_mask is not None and prefix is not None: fixed = torch.cat( [prefix, samples[:, RTC_PREFIX_FRAMES:]], dim=1, ) samples = torch.where(prefix_mask[:, :, None], fixed, samples) return samples.float() def _stationary_future_progress(total_frames: int) -> np.ndarray: if int(total_frames) <= 0: raise ValueError("Stationary progress requires a positive task length") row = np.asarray([0.0, 1.0, 0.0, 0.0, 1.0], dtype=np.float32) return np.repeat(row[None, :], ACTION_CHUNK_FRAMES, axis=0) def normalize_quaternion_rows(motion_abs: np.ndarray) -> np.ndarray: if motion_abs.shape[1] < 36: return motion_abs q = motion_abs[:, 32:36] norm = np.linalg.norm(q, axis=1, keepdims=True) norm = np.where(norm < 1e-8, 1.0, norm) motion_abs[:, 32:36] = q / norm return motion_abs def build_model_states_from_abs( state_history_abs: List[np.ndarray], *, textop_actions: bool, odim: int, ) -> np.ndarray: states_abs = np.stack(state_history_abs, axis=0).astype(np.float32) if textop_actions and odim == 40: from psi.data.seed.motion_repr40 import build_root40_state states = build_root40_state(states_abs[:, :SEED_ABS_DIM]) else: states = build_seed_textop_state(states_abs[:, :SEED_ABS_DIM]) if textop_actions else states_abs if states.shape[-1] < odim: pad = np.zeros((states.shape[0], odim - states.shape[-1]), dtype=np.float32) states = np.concatenate([states, pad], axis=-1) if states.shape[-1] != odim: raise ValueError(f"Infer state dim mismatch: built {states.shape[-1]}D, model odim={odim}") return states.astype(np.float32) def clamp_generated_actions(prediction, known_prefix=None): """Bound newly solved frames, NEVER distort a re-anchored RTC prefix.""" bounded = prediction.clamp(-1, 1) if known_prefix is not None: bounded = bounded.clone() bounded[:, :known_prefix.shape[1]] = known_prefix return bounded def decode_action_chunk(normalized, current_abs, field): """Decode either persisted representation; both return 36D render poses.""" from psi.data.seed.motion_repr import recover_seed_abs_from_textop_action raw = np.asarray(field.denormalize(normalized), np.float32) if raw.shape[-1] == 40: from psi.data.seed.motion_repr40 import recover_seed_abs_from_root40_action result = recover_seed_abs_from_root40_action(raw, current_abs) elif raw.shape[-1] == 39: result = recover_seed_abs_from_textop_action(raw, current_abs) else: raise ValueError(f"Unsupported action dimension: {raw.shape[-1]}") return normalize_quaternion_rows(result) def next_rtc_prefix(normalized, old_current_abs, executed_abs, field, *, executed_frames=20): """Rebase the unexecuted tail using the actually observed next state.""" if normalized.shape[-1] == 39: return normalized[executed_frames:executed_frames+RTC_PREFIX_FRAMES].copy() if normalized.shape[-1] != 40: raise ValueError("RTC requires a known action representation") from psi.data.seed.motion_repr40 import choose_heading_anchor, reencode_root40_prefix raw = np.asarray(field.denormalize(normalized), np.float32) rebased = reencode_root40_prefix( raw, old_current_abs, choose_heading_anchor(old_current_abs), executed_abs, choose_heading_anchor(executed_abs), start_index=executed_frames, length=RTC_PREFIX_FRAMES, ) lo = np.asarray(field.action_min, np.float32) hi = np.asarray(field.action_max, np.float32) if np.any(hi <= lo): raise ValueError("Root40 normalization must have positive ranges") # No clipping: rotating an XY vector may move a component outside the # training quantile box. Clipping would change the physical RTC trajectory. return ((rebased - lo) / (hi - lo) * 2 - 1).astype(np.float32)