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