File size: 9,143 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 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 | """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)
|