File size: 17,264 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
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
#!/usr/bin/env python3
"""CPU-only SWA review assets: exact microphone, causal camera, generated body.

No model/checkpoint and no answer TTS. Original generated poses are immutable;
the separate display CSVs hold the initial state until the first packet arrives.
"""
from __future__ import annotations

import argparse
import hashlib
import json
from pathlib import Path
import subprocess
import textwrap

import numpy as np

FPS, SR = 50, 16000


def read(path):
    return json.loads(Path(path).read_text(encoding="utf-8"))


def write(path, data):
    path.write_text(json.dumps(data, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")


def load_cpu(path):
    import torch
    torch.set_num_threads(1)
    return torch.load(path, map_location="cpu", weights_only=False, mmap=True)


def read_wave(path):
    from scipy.io import wavfile
    sr, wave = wavfile.read(path)
    if sr != SR or wave.ndim != 1:
        raise ValueError(f"SWA microphone must be mono 16 kHz: {path}")
    if wave.dtype == np.int16:
        wave = wave.astype(np.float32) / 32768
    elif wave.dtype.kind != "f":
        raise ValueError(f"Unsupported SWA waveform encoding: {wave.dtype}")
    return np.asarray(wave, np.float32)


def source_row(run, metrics):
    row = metrics.get("render_source")
    if row is None:
        # Old reports lack provenance. Resolve by immutable ID, not today's
        # dataset index, which may move when a source plan grows.
        plan = read(run / "run_config.json")["data"]["swa_source_plan"]
        matches = [r for r in read(plan) if r["id"] == metrics["id"]]
        if len(matches) != 1:
            raise ValueError(f"Cannot uniquely recover SWA source {metrics['id']}")
        row = matches[0]
    if row["id"] != metrics["id"] or row["question"]["question"] != metrics["question"]:
        raise ValueError("Saved SWA identity/question differs from render source")
    return row


def microphone(row, body):
    root = Path(row["frontend_root"])
    timeline_path = root / "data/timeline" / f"{row['id']}.pt"
    if timeline_path.is_file():
        data = load_cpu(timeline_path)
        timeline = data["timeline"]
        audio = np.asarray(data["mixed_audio"], np.float32).copy()
        expected = timeline.get("audio_sha256")
        if expected and hashlib.sha256(audio.tobytes()).hexdigest() != expected:
            raise ValueError("SWA stored microphone hash mismatch")
    else:
        frontend = load_cpu(root / "data/frontend" / f"{row['id']}.pt")
        if frontend.get("format") != "static_factorized_v1":
            raise ValueError("Missing original SWA microphone; refusing guessed audio")
        timeline = frontend["timeline"]
        audio = np.asarray(body["base_audio"], np.float32).copy()
        if row.get("swa_has_question", True):
            voice = Path(timeline["question_voice"])
            expected = row.get("asr_review", {}).get("wave_sha256")
            if expected and hashlib.sha256(voice.read_bytes()).hexdigest() != expected:
                raise ValueError("SWA question WAV differs from screened input")
            question = read_wave(voice).copy()
            question *= float(timeline["question_gain"])
            fade = min(160, len(question) // 4)
            if fade:
                question[:fade] *= np.linspace(0, 1, fade)
                question[-fade:] *= np.linspace(1, 0, fade)
            start = round(float(timeline["question_start"]) * SR)
            end = start + len(question)
            if start < 0 or end > len(audio):
                raise ValueError("SWA question is outside source audio")
            if abs(end / SR - float(timeline["question_end"])) > 1 / SR:
                raise ValueError("SWA question timeline differs from input encoding")
            audio[start:end] += question
    if not row.get("swa_has_question", True):
        audio = np.asarray(body["base_audio"], np.float32).copy()
    if audio.ndim != 1 or not np.isfinite(audio).all() or np.max(np.abs(audio)) >= 1:
        raise ValueError("Invalid/clipped SWA microphone")
    return audio, timeline


def display_motion(initial, generated, events):
    if (generated.ndim != 2 or generated.shape[1] != 36
            or len(generated) != 20 * len(events) or not len(events)
            or not np.isfinite(generated).all()):
        raise ValueError("SWA generated poses/events do not match 20-frame execution")
    starts = np.rint(np.array([e["seconds"] for e in events]) * FPS).astype(int)
    if starts[0] < 0 or np.any(np.diff(starts) != 20):
        raise ValueError("SWA event arrivals do not follow the recorded 0.4s cadence")
    initial = np.asarray(initial, np.float32)
    if initial.shape != (36,) or not np.isfinite(initial).all():
        raise ValueError("Invalid initial robot state")
    poses = np.repeat(initial[None], starts[-1] + 20, axis=0)
    for i, start in enumerate(starts):
        poses[start:start + 20] = generated[i * 20:(i + 1) * 20]
    return poses, starts.tolist()


def answer_audio(output, metrics):
    """Read only actual generated PCM and its measured FIFO schedule, never GT."""
    from scipy.io import wavfile
    from psi.runtime.swa_speech import schedule_speech
    segments = schedule_speech(metrics['answers'])
    if segments != metrics['speech_timeline']:
        raise ValueError('Saved speech schedule differs from measured PCM releases')
    wave = np.zeros(max([0] + [s['end'] for s in segments]), np.float32)
    loaded = {}
    for segment in segments:
        name = segment['path']
        path = (output / name).resolve()
        if not path.is_relative_to(output.resolve()):
            raise ValueError('Answer PCM must be inside its result directory')
        if name not in loaded:
            sr, audio = wavfile.read(path)
            if sr != 24000 or audio.ndim != 1 or audio.dtype.kind != 'f' or not np.isfinite(audio).all():
                raise ValueError('Invalid native answer WAV')
            loaded[name] = audio
        audio = loaded[name][segment['source_start']:segment['source_end']]
        if len(audio) != segment['end'] - segment['start']:
            raise ValueError('Answer WAV is incomplete')
        wave[segment['start']:segment['end']] = audio
    return wave, segments


def prepare(run, output):
    from scipy.io import wavfile
    metrics = read(output / "metrics.json")
    row = source_row(run, metrics)
    body = load_cpu(row["path"])
    generated = np.load(output / "generated_abs.npy", allow_pickle=False)
    poses, starts = display_motion(body["initial_abs_state"], generated, metrics["events"])
    motion_frames = len(poses)
    audio, timeline = microphone(row, body)
    native = metrics.get('native_speech_version') == 1
    speaker, segments = answer_audio(output, metrics) if native else (np.zeros(0, np.float32), [])
    required_frames = max(len(poses), int(np.ceil(len(speaker) / 480)))
    if required_frames > len(poses):
        poses = np.concatenate((poses, np.repeat(poses[-1:], required_frames - len(poses), axis=0)))
    samples = len(poses) * (SR // FPS)
    if len(audio) > samples:
        raise ValueError("Review would truncate the SWA microphone")
    audio = np.pad(audio, (0, samples - len(audio)))
    motion_dir = output / "render_motion"
    motion_dir.mkdir(exist_ok=True)
    for name, values in (("joint_pos", poses[:, :29]), ("body_pos", poses[:, 29:32]),
                         ("body_quat", poses[:, 32:36])):
        np.savetxt(motion_dir / f"{name}.csv", values, delimiter=",", fmt="%.9g")
    # IEEE float WAV preserves the exact model-input samples.
    wavfile.write(output / "assembled_input_audio_16k.wav", SR, audio)
    mix_gain = 1.
    if native:
        from scipy.signal import resample_poly
        mic24 = resample_poly(audio, 3, 2).astype(np.float32)
        speaker = np.pad(speaker, (0, len(mic24) - len(speaker)))
        mixed = mic24 + speaker
        mix_gain = min(1., .98 / max(float(np.max(np.abs(mixed))), 1e-8))
        wavfile.write(output / 'assembled_answer_audio_24k.wav', 24000, speaker)
        wavfile.write(output / 'assembled_review_audio_24k.wav', 24000, mixed * mix_gain)
    write(output / "infer_meta.json", {
        "swa_render_version": 2 if native else 1, "source_id": row["id"], "fps": FPS,
        "total_frames": len(poses), "generated_frames": len(generated),
        "action_display_start_frames": starts, "initial_hold_frames": starts[0],
        "timeline": timeline, "render_source": row,
        "audio_mode": "original_microphone_plus_native_generated_speech" if native else "original_microphone_only_no_generated_speech",
        "speech_segments": segments, "audio_mix_gain": mix_gain,
        "motion_end_frame": motion_frames, "speech_tail_hold_frames": len(poses) - motion_frames,
        "audio_file": "assembled_review_audio_24k.wav" if native else "assembled_input_audio_16k.wav",
        "answer_mode": "model_text_at_native_speech_playback" if native else "model_text_at_logical_packet_time_not_wall_clock_latency",
        "original_generated_motion_unchanged": True,
    })
    # Migrate only the old misleading stage marker, not any result tensors.
    if (output / ".complete").exists():
        (output / ".complete").unlink()
    (output / ".inferred").touch()
    print(f"[swa-render] prepared {row['id']}: {len(poses)} frames, native_answers={len(segments)} PCM chunks", flush=True)


def camera_frames(path, deadlines):
    """Same PTS<=deadline rule as the frozen frontend camera protocol."""
    import av
    from PIL import ImageOps
    av.logging.restore_default_callback()
    images, pts, index = [], [], 0
    previous = previous_pts = None
    with av.open(str(path)) as container:
        stream = container.streams.video[0]
        stream.thread_type = "AUTO"
        stream.codec_context.thread_count = 2
        container.seek(max(0, int((deadlines[0] - 2) / float(stream.time_base))),
                       stream=stream, backward=True)
        for frame in container.decode(stream):
            if frame.pts is None:
                continue
            now = float(frame.pts * stream.time_base)
            while index < len(deadlines) and now > deadlines[index] + 1e-8:
                if previous is None:
                    raise ValueError("No camera frame before recorded arrival")
                images.append(previous)
                pts.append(previous_pts)
                index += 1
            if index == len(deadlines):
                break
            previous = ImageOps.pad(frame.to_image(), (224, 224), color=(0, 0, 0))
            previous_pts = now
        while index < len(deadlines):
            if previous is None:
                raise ValueError("Empty source camera")
            images.append(previous)
            pts.append(previous_pts)
            index += 1
    if np.max(np.asarray(deadlines) - pts) >= .2:
        raise ValueError("Camera recording ended before the SWA timeline")
    return images, np.asarray(pts)


def compose(output):
    import cv2
    from PIL import Image, ImageDraw, ImageFont, ImageOps
    cv2.setNumThreads(1)
    metadata = read(output / "infer_meta.json")
    metrics = read(output / "metrics.json")
    row, timeline = metadata["render_source"], metadata["timeline"]
    total = int(metadata["total_frames"])
    if row.get("camera_domain") == "static_image":
        captures = np.array([0.])
        with Image.open(row["question"]["image_path"]) as image:
            cameras = [ImageOps.pad(image.convert("RGB"), (224, 224), color=(0, 0, 0))]
    else:
        captures = np.asarray(timeline["camera_capture_seconds"])
        cameras, pts = camera_frames(row["question"]["video_path"],
                                     captures + float(timeline["video_offset"]))
        if not np.allclose(pts, timeline["camera_pts"], atol=1e-7, rtol=0):
            raise ValueError("Review camera PTS differ from actual encoded frames")
    cameras = [image.resize((384, 384)) for image in cameras]
    font = ImageFont.truetype("/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf", 18)
    robot = cv2.VideoCapture(str(output / "render_motion/reference_motion.mp4"))
    if (not robot.isOpened() or abs(robot.get(cv2.CAP_PROP_FPS) - FPS) > .01
            or int(robot.get(cv2.CAP_PROP_FRAME_COUNT)) != total):
        robot.release()
        raise ValueError("SWA robot render frame count/FPS mismatch")
    target = output / "reference_motion_with_audio.mp4"
    temporary = output / ".swa_review.partial.mp4"
    command = ["ffmpeg", "-v", "error", "-y", "-f", "rawvideo", "-pixel_format", "rgb24",
               "-video_size", "960x560", "-framerate", str(FPS), "-i", "pipe:0",
               "-i", str(output / metadata.get('audio_file', 'assembled_input_audio_16k.wav')),
               "-map", "0:v:0", "-map", "1:a:0", "-c:v", "libx264", "-preset", "veryfast",
               "-threads", "2", "-crf", "21", "-pix_fmt", "yuv420p", "-c:a", "aac",
               "-t", str(total / FPS), "-movflags", "+faststart", str(temporary)]
    child = subprocess.Popen(command, stdin=subprocess.PIPE)
    try:
        for frame in range(total):
            ok, bgr = robot.read()
            if not ok:
                raise ValueError("SWA robot video ended early")
            t = frame / FPS
            canvas = Image.new("RGB", (960, 560), (20, 20, 20))
            camera_index = np.searchsorted(captures, t + 1e-8, side="right") - 1
            if camera_index >= 0:
                canvas.paste(cameras[min(camera_index, len(cameras) - 1)], (0, 36))
            body = Image.fromarray(cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB))
            canvas.paste(ImageOps.pad(body, (576, 384), color=(0, 0, 0)), (384, 36))
            draw = ImageDraw.Draw(canvas)
            draw.text((8, 7), f"SWA camera input          |          Generated robot     {t:.2f}s",
                      font=font, fill="white")
            native = metadata['swa_render_version'] >= 2
            notice = "Audio: microphone + native model voice (offline causal replay)." if native else "Audio: original microphone. Model answer: text only (no generated voice)."
            if frame >= metadata.get('motion_end_frame', total):
                notice = "Answer audio tail: holding last generated robot pose."
            draw.text((8, 424), notice,
                      font=font, fill="white")
            question = metrics["question"] if t >= float(timeline["question_start"]) and row.get("swa_has_question", True) else "(not spoken yet)"
            def answer_time(answer):
                matching = [s['start'] / 24000 for s in metadata.get('speech_segments', []) if s['path'] == answer.get('audio_path')]
                return min(matching) if matching else float(answer['seconds'])
            available = [a for a in metrics["answers"] if answer_time(a) <= t + 1e-8]
            answer = available[-1]["text"] if available else "(no answer yet)"
            lines = (textwrap.wrap("Question: " + question, 97)[:2]
                     + textwrap.wrap("Model: " + answer, 97)[:3])
            for index, line in enumerate(lines):
                draw.text((8, 447 + index * 21), line, font=font, fill="white")
            child.stdin.write(np.asarray(canvas, np.uint8).tobytes())
        child.stdin.close()
        if child.wait() != 0:
            raise RuntimeError("SWA video/audio encoding failed")
    finally:
        robot.release()
        if not child.stdin.closed:
            try:
                child.stdin.close()
            except BrokenPipeError:
                pass
        if child.poll() is None:
            child.kill()
        child.wait()
    probe = json.loads(subprocess.check_output(
        ["ffprobe", "-v", "error", "-show_streams", "-of", "json", str(temporary)]))
    video = next(s for s in probe["streams"] if s["codec_type"] == "video")
    audio = next(s for s in probe["streams"] if s["codec_type"] == "audio")
    if (int(video["nb_frames"]) != total or video["avg_frame_rate"] != "50/1"
            or abs(float(audio.get("start_time", 0))) > .021):
        raise ValueError("SWA composed video lost frames or shifted microphone")
    temporary.replace(target)
    if metadata['swa_render_version'] >= 2:
        metadata['answer_mode'] = 'model_text_at_native_speech_playback'
    metadata["swa_review"] = {"video": str(target), "camera_pts_verified": True,
                              "frames": total, "fps": FPS,
                              "generated_speech": metadata['swa_render_version'] >= 2 and bool(metrics['answers'])}
    write(output / "infer_meta.json", metadata)
    print(f"[done] SWA synchronized review: {target}", flush=True)


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("mode", choices=("prepare", "compose"))
    parser.add_argument("--run-dir", type=Path, required=True)
    parser.add_argument("--output-dir", type=Path, required=True)
    args = parser.parse_args()
    if not args.output_dir.resolve().is_relative_to(args.run_dir.resolve()):
        parser.error("SWA output must be inside the selected run")
    if args.mode == "prepare":
        prepare(args.run_dir, args.output_dir)
    else:
        compose(args.output_dir)


if __name__ == "__main__":
    main()