File size: 7,152 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
"""The SAME closed-loop body rollout for SWA deployment and held-out eval."""
from __future__ import annotations

import numpy as np
import torch


def inplace_future_progress(frame_id: int, total_frames: int, horizon: int = 30):
    """SWA blends *inside* the task, never inserts a frozen phase prefix."""
    if total_frames <= 0 or frame_id < 0:
        raise ValueError("Invalid motion progress timeline")
    p = np.minimum(np.arange(frame_id + 1, frame_id + horizon + 1), total_frames - 1) / max(total_frames - 1, 1)
    return np.stack((p, 1 - p, p, np.sin(2 * np.pi * p), np.cos(2 * np.pi * p)), -1).astype(np.float32)


class ReplyDebounce:
    """KEEP from the old paired experiment: two consecutive ready windows."""
    def __init__(self, consecutive=2):
        if consecutive < 1:
            raise ValueError("Positive debounce count required")
        self.consecutive, self.streak = consecutive, 0

    def update(self, probability):
        self.streak = self.streak + 1 if probability >= .5 else 0
        return self.streak == self.consecutive


def body_metrics(prediction, target, *, fps=50):
    """Old experiment's physical-space metrics, also reusable by audio tasks.

    Score executed frames, including seams; never teacher-force, DTW, or align
    away drift. Acceleration *error* additionally prevents rewarding freezing.
    """
    p, g = np.asarray(prediction, np.float64), np.asarray(target, np.float64)
    n = min(len(p), len(g))
    if n < 3 or p.shape[1:] != (36,) or g.shape[1:] != (36,):
        raise ValueError("Body metrics require >=3 physical 36D frames")
    p, g = p[:n], g[:n]
    if not (np.isfinite(p).all() and np.isfinite(g).all()):
        raise ValueError("Non-finite closed-loop motion")
    def rms(x):
        return float(np.sqrt(np.mean(x ** 2)))
    pq, gq = p[:, 32:36], g[:, 32:36]
    pn, gn = np.linalg.norm(pq, axis=-1), np.linalg.norm(gq, axis=-1)
    if (pn < 1e-8).any() or (gn < 1e-8).any():
        raise ValueError("Zero root quaternion")
    dot = np.clip(np.abs(np.sum(pq * gq, -1) / (pn * gn)), 0, 1)
    return dict(
        joint_rmse_rad=rms(p[:, :29] - g[:, :29]),
        root_xy_rmse_m=rms(p[:, 29:31] - g[:, 29:31]),
        root_z_rmse_m=rms(p[:, 31] - g[:, 31]),
        root_rotation_deg=float(np.mean(np.degrees(2 * np.arccos(dot)))),
        joint_velocity_error_rad_s=rms(np.diff(p[:, :29] - g[:, :29], axis=0) * fps),
        joint_acceleration_error_rad_s2=rms(np.diff(p[:, :29] - g[:, :29], n=2, axis=0) * fps ** 2),
    )


@torch.inference_mode()
def rollout_source(model, launch_config, source, *, speech=None, observer=None):
    import os
    from psi.data.seed.native_visual import source_visual_mode
    source = source_visual_mode(source, os.environ.get('HFD_VISUAL_INPUT_MODE',
        getattr(getattr(launch_config, 'data', None), 'multimodal_visual_input_mode', 'cached')))
    from psi.data.seed.swa_visual_qa_dataset import _load_body
    from psi.runtime.motion_rollout import (
        _flow_action, _stationary_future_progress, build_model_states_from_abs,
        normalize_quaternion_rows,
        clamp_generated_actions, decode_action_chunk, next_rtc_prefix,
    )
    from diffusers import FlowMatchEulerDiscreteScheduler
    if model.training:
        raise ValueError("Closed-loop evaluation must use model.eval()")
    model.device = next(model.parameters()).device
    # Training owns another scheduler; do not alter its flow noise schedule.
    model.noise_scheduler = FlowMatchEulerDiscreteScheduler(
        num_train_timesteps=int(launch_config.model.train_diffusion_steps))
    steps = [s for s in source["steps"] if s["update_semantic_cache"]]
    packets = source["swa_packets"]
    if len(steps) != len(packets) or not packets:
        raise RuntimeError("SWA action and AV packet timelines disagree")
    if "multimodal_overlay" in source:
        if not bool(getattr(launch_config.data, "dynamic_task_entry_blend", False)):
            raise ValueError("Unified overlay rollout requires the current in-place motion progress geometry")
        # Exactly the same initialization used by the ordinary action rollout;
        # subsequent states/RTC tails are generated, never read from GT.
        current = np.asarray(source["initial_abs_state"], np.float32).copy()
    else:
        body = _load_body(str(source["swa_job"]["path"]))
        current = body["initial_abs_state"].copy()
    cache = model.begin_source([source["source_id"]])
    known, old_control, progress, duration = None, None, 0, 1000
    field = launch_config.data.transform.field
    generator = torch.Generator(device=model.device).manual_seed(1911)
    trigger = ReplyDebounce()
    poses, events = [], []
    for index, (step, packet) in enumerate(zip(steps, packets)):
        model.swa_packets = [packet]
        with torch.autocast(model.device.type, dtype=torch.bfloat16, enabled=model.device.type == "cuda"):
            condition = model._encode_qwen_stream_input(
                source_ids=[source["source_id"]], cache=cache, update_semantic_cache=True,
            )
        cache = condition.kv_cache
        control = int(condition.control_logits.argmax(-1).item())
        probability = float(model.reply_logits.softmax(-1)[0, 1])
        if trigger.update(probability) and speech is not None:
            speech.submit(cache, window=index, seconds=float(packet["available_seconds"]))
        if control == 1 and old_control != 1:
            progress = 0
            duration = max(1, round(float(torch.expm1(condition.motion_duration_log1p_seconds[0])) * 50))
        future = inplace_future_progress(progress, duration) if control == 1 else _stationary_future_progress(1000)
        state = build_model_states_from_abs([current], textop_actions=True, odim=int(launch_config.model.odim))
        if bool(field.normalize_state):
            state = field.normalize_state_func(state)
        prediction = _flow_action(
            model, condition, known_prefix=known, states=torch.as_tensor(state[None], device=model.device),
            future_progress=torch.as_tensor(future[None], device=model.device),
            num_inference_steps=10, generator=generator,
        )
        prediction = clamp_generated_actions(prediction, known)
        # Carry the generated 10-frame tail even across token changes.
        normalized = prediction[0].float().cpu().numpy()
        absolute = decode_action_chunk(normalized, current, field)
        known = torch.as_tensor(next_rtc_prefix(normalized, current, absolute[19], field)[None],
                                device=model.device)
        poses.append(absolute[:20])
        current = absolute[19].copy()
        if control == 1:
            progress += 20
        old_control = control
        events.append(dict(window=index, seconds=float(packet["available_seconds"]),
                           control=control, control_gt=int(step["control_label"]),
                           reply_probability=probability, reply_gt=int(packet["reply_label"])))
        if observer is not None:
            observer(model, cache, packet)
        cache.detach()
    return np.concatenate(poses), events