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)