code-HFD / src /psi /runtime /motion_rollout.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
06f9561 verified
Raw History Blame Contribute Delete
9.14 kB
"""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)