File size: 46,074 Bytes
c4f8e4a
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
from __future__ import annotations

import argparse
import json
import struct
import sys
import threading
import time
from base64 import b64decode, b64encode
from pathlib import Path

import numpy as np
import pandas as pd
import websocket
import zmq
from numpy.lib.format import descr_to_dtype, dtype_to_descr

from psi.data.seed.motion_repr import SEED_TEXTOP_ACTION_DIM, normalize_quat_wxyz, recover_seed_abs_from_textop_action
from psi.utils import parse_args_to_tyro_config


HEADER_SIZE = 1280
CTRL_HZ = 50
CTRL_DT = 1.0 / CTRL_HZ


def numpy_serialize(o):
    if isinstance(o, (np.ndarray, np.generic)):
        data = o.data if o.flags["C_CONTIGUOUS"] else o.tobytes()
        return {
            "__numpy__": b64encode(data).decode(),
            "dtype": dtype_to_descr(o.dtype),
            "shape": o.shape,
        }
    raise TypeError(f"Object of type {o.__class__.__name__} is not JSON serializable")


def numpy_deserialize(dct):
    if "__numpy__" in dct:
        array = np.frombuffer(b64decode(dct["__numpy__"]), descr_to_dtype(dct["dtype"]))
        return array.reshape(dct["shape"]) if dct["shape"] else array[0]
    return dct


def convert_numpy_in_dict(data, func):
    if isinstance(data, dict):
        if "__numpy__" in data:
            return func(data)
        return {key: convert_numpy_in_dict(value, func) for key, value in data.items()}
    if isinstance(data, list):
        return [convert_numpy_in_dict(item, func) for item in data]
    if isinstance(data, (np.ndarray, np.generic)):
        return func(data)
    return data


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser()
    parser.add_argument("--psi-host", default="localhost")
    parser.add_argument("--psi-port", type=int, default=8014)
    parser.add_argument("--sonic-zmq-host", default="*")
    parser.add_argument("--sonic-zmq-port", type=int, default=5556)
    parser.add_argument("--prompt", required=True)
    parser.add_argument("--init-motion-dir", default="")
    parser.add_argument("--inference-video-path", default="")
    parser.add_argument("--video-config-key", default="video_clip")
    parser.add_argument("--video-clip-motion-fps", type=float, default=50.0)
    parser.add_argument("--video-sample-fps", type=float, default=8.0)
    parser.add_argument("--video-size-pixels", type=int, default=50176)
    parser.add_argument("--video-decoder", default="ffmpeg")
    # <modify> 多段planner模式:不重启bridge,只在同一个RTC连接里按帧号切换prompt。
    parser.add_argument("--motion-plan-json", default="")
    parser.add_argument("--run-dir", required=True)
    parser.add_argument("--start-index", type=int, default=0)
    parser.add_argument("--init-state-npy", default="")
    parser.add_argument("--live-state-npy", default="")
    parser.add_argument("--require-live-state", action="store_true", default=False)
    parser.add_argument("--live-state-timeout", type=float, default=30.0)
    parser.add_argument("--live-state-max-age", type=float, default=0.5)
    parser.add_argument("--startup-ready-flag", default="")
    parser.add_argument("--start-gate-flag", default="")
    parser.add_argument("--start-gate-timeout", type=float, default=600.0)
    parser.add_argument("--render-timing-json", default="")
    parser.add_argument("--record-flag-path", default="")
    # <modify> 第一帧站稳预滚:先发重复第一帧并释放sim elastic band,等站稳后再开始record。
    parser.add_argument("--settle-flag-path", default="")
    parser.add_argument("--stand-pre-roll-seconds", type=float, default=1.0)
    parser.add_argument("--record-stable-min-root-z", type=float, default=0.75)
    parser.add_argument("--record-stable-seconds", type=float, default=0.6)
    parser.add_argument("--record-stable-timeout-seconds", type=float, default=20.0)
    parser.add_argument("--max-seconds", type=float, default=120.0)
    # <modify> 如果server长时间不再返回action,bridge不能一直卡着等,否则orchestrator和WBC会持续刷屏不退出。
    # 原逻辑:
    # 无action空闲超时保护
    parser.add_argument("--action-idle-timeout-seconds", type=float, default=8.0)
    # <modify> v13 future_progress需要目标动作总帧数;0表示使用server默认值。
    parser.add_argument("--target-frames", type=int, default=0)
    # <modify> RTC使用30帧预测窗口、20帧执行推进;保留10帧overlap保证chunk连接连续。
    # 原逻辑:
    # parser.add_argument("--pose-chunk-frames", type=int, default=30)
    # parser.add_argument("--pose-chunk-send-seconds", type=float, default=0.45)
    # 后续逻辑:
    # parser.add_argument("--pose-chunk-frames", type=int, default=20)
    # parser.add_argument("--pose-chunk-send-seconds", type=float, default=0.35)
    parser.add_argument("--pose-chunk-frames", type=int, default=30)
    parser.add_argument("--pose-chunk-advance-frames", type=int, default=20)
    parser.add_argument("--pose-chunk-send-seconds", type=float, default=0.0)
    parser.add_argument("--dummy-image-height", type=int, default=240)
    parser.add_argument("--dummy-image-width", type=int, default=320)
    parser.add_argument("--verbose", action="store_true", default=False)
    return parser.parse_args()


def format_motion_prompt(description: str) -> str:
    description = str(description).strip()
    return "Generate a motion sequence that matches the following description.\nDescription: " + description


def load_motion_prompt_plan(path_text: str, fps: int = CTRL_HZ) -> list[dict]:
    if not path_text:
        return []
    motion_plan = json.loads(Path(path_text).read_text(encoding="utf-8"))
    prompt_plan = []
    for index, item in enumerate(motion_plan.get("segments", []), start=1):
        low_level = str(item.get("low_level_instruction", "")).strip()
        if not low_level:
            raise ValueError(f"motion plan segment {index} has empty low_level_instruction")
        frames = int(item.get("num_frames", 0))
        if frames <= 0:
            frames = int(round(float(item.get("duration_seconds", 0.0)) * fps))
        if frames <= 0:
            raise ValueError(f"motion plan segment {index} has invalid duration/frames")
        prompt_plan.append(
            {
                "index": index,
                "low_level_instruction": low_level,
                "prompt": format_motion_prompt(low_level),
                "frames": frames,
            }
        )
    if not prompt_plan:
        raise ValueError(f"No valid segments found in motion plan: {path_text}")
    return prompt_plan


def pack_header(fields: list[dict], version: int = 1, count: int = 1) -> bytes:
    header = {"v": version, "endian": "le", "count": count, "fields": fields}
    header_json = json.dumps(header, separators=(",", ":")).encode("utf-8")
    if len(header_json) > HEADER_SIZE:
        raise ValueError(f"ZMQ header too large: {len(header_json)} > {HEADER_SIZE}")
    return header_json.ljust(HEADER_SIZE, b"\x00")


class SonicZMQPublisher:
    def __init__(self, host: str, port: int):
        self.context = zmq.Context()
        self.socket = self.context.socket(zmq.PUB)
        self.endpoint = f"tcp://{host}:{port}"
        self.socket.bind(self.endpoint)
        time.sleep(0.5)
        print(f"[sonic] bound {self.endpoint}")

    def send_command(self, start: bool, stop: bool, planner: bool) -> None:
        fields = [
            {"name": "start", "dtype": "u8", "shape": [1]},
            {"name": "stop", "dtype": "u8", "shape": [1]},
            {"name": "planner", "dtype": "u8", "shape": [1]},
        ]
        payload = b"".join(
            [
                struct.pack("B", 1 if start else 0),
                struct.pack("B", 1 if stop else 0),
                struct.pack("B", 1 if planner else 0),
            ]
        )
        self.socket.send(b"command" + pack_header(fields) + payload)

    def send_pose(
        self,
        joint_pos: np.ndarray,
        joint_vel: np.ndarray,
        body_quat: np.ndarray,
        frame_index: np.ndarray,
        catch_up: bool = False,
    ) -> None:
        joint_pos = np.asarray(joint_pos, dtype=np.float32)
        joint_vel = np.asarray(joint_vel, dtype=np.float32)
        body_quat = np.asarray(body_quat, dtype=np.float32)
        frame_index = np.asarray(frame_index, dtype=np.int64)
        n = int(joint_pos.shape[0])
        fields = [
            {"name": "joint_pos", "dtype": "f32", "shape": [n, 29]},
            {"name": "joint_vel", "dtype": "f32", "shape": [n, 29]},
            {"name": "body_quat_w", "dtype": "f32", "shape": [n, 4]},
            {"name": "frame_index", "dtype": "i64", "shape": [n]},
            {"name": "catch_up", "dtype": "u8", "shape": [1]},
        ]
        payload = b"".join(
            [
                np.ascontiguousarray(joint_pos).tobytes(),
                np.ascontiguousarray(joint_vel).tobytes(),
                np.ascontiguousarray(body_quat).tobytes(),
                np.ascontiguousarray(frame_index).tobytes(),
                struct.pack("B", 1 if catch_up else 0),
            ]
        )
        self.socket.send(b"pose" + pack_header(fields, count=n) + payload)

    def close(self) -> None:
        self.socket.close()
        self.context.term()


def load_state_npy(path: Path) -> np.ndarray:
    state = np.load(path).astype(np.float32)
    if state.ndim == 2:
        state = state[-1]
    if state.shape != (36,):
        raise ValueError(f"Expected 36D state from {path}, got {state.shape}")
    state[32:36] = normalize_quat_wxyz(state[32:36])
    return state.astype(np.float32)


def load_state_from_motion_dir(motion_dir: Path) -> np.ndarray:
    joint = pd.read_csv(motion_dir / "joint_pos.csv").to_numpy(dtype=np.float32)
    body_pos = pd.read_csv(motion_dir / "body_pos.csv").to_numpy(dtype=np.float32)
    body_quat = pd.read_csv(motion_dir / "body_quat.csv").to_numpy(dtype=np.float32)
    state = np.concatenate([joint[0], body_pos[0], body_quat[0]], axis=0).astype(np.float32)
    state[32:36] = normalize_quat_wxyz(state[32:36])
    return state


def map_motion_dir(root_dir: Path, move_g1_path: str) -> Path:
    rel = str(move_g1_path).strip().replace("\\", "/")
    rel = rel.replace("g1/csv/", "g1/csv_sonic/")
    rel = rel.replace("/csv/", "/csv_sonic/")
    if rel.endswith(".csv"):
        rel = rel[:-4]
    if rel.startswith("dataset/seed/"):
        rel = rel[len("dataset/seed/") :]
    if rel.startswith("seed/"):
        rel = rel[len("seed/") :]
    path = Path(rel)
    return path if path.is_absolute() else root_dir / path


def get_text_data_paths(data_config) -> tuple[Path, Path]:
    if hasattr(data_config, "root_dir"):
        data_root = Path(data_config.root_dir)
        metadata_csv = Path(getattr(data_config, "metadata_csv", "metadata/seed_metadata_v004.csv"))
    else:
        text_config = getattr(data_config, "text", None)
        if text_config is None:
            raise AttributeError("data config has neither root_dir nor text config")
        if isinstance(text_config, dict):
            data_root = Path(text_config["root_dir"])
            metadata_csv = Path(text_config.get("metadata_csv", "metadata/seed_metadata_v004.csv"))
        else:
            data_root = Path(text_config.root_dir)
            metadata_csv = Path(getattr(text_config, "metadata_csv", "metadata/seed_metadata_v004.csv"))
    if not metadata_csv.is_absolute():
        metadata_csv = data_root / metadata_csv
    return data_root, metadata_csv


def load_initial_state(args: argparse.Namespace) -> np.ndarray:
    if args.init_state_npy:
        return load_state_npy(Path(args.init_state_npy).expanduser())
    if getattr(args, "init_motion_dir", ""):
        motion_dir = Path(args.init_motion_dir).expanduser()
        if not motion_dir.exists():
            raise FileNotFoundError(f"init motion dir does not exist: {motion_dir}")
        print(f"[state] init from motion dir: {motion_dir}")
        return load_state_from_motion_dir(motion_dir)

    run_dir = Path(args.run_dir)
    cfg_template = parse_args_to_tyro_config(run_dir / "argv.txt")  # type: ignore
    launch_config = cfg_template.model_validate_json((run_dir / "run_config.json").read_text())
    data_root, metadata_csv = get_text_data_paths(launch_config.data)
    metadata = pd.read_csv(metadata_csv)
    row_idx = min(max(0, int(args.start_index)), len(metadata) - 1)
    motion_dir = map_motion_dir(data_root, str(metadata.iloc[row_idx]["move_g1_path"]))
    print(f"[state] init from metadata index {row_idx}: {motion_dir}")
    return load_state_from_motion_dir(motion_dir)


class StateProvider:
    def __init__(self, live_state_npy: str, require_live: bool = False, max_age: float = 0.5):
        self.live_state_npy = Path(live_state_npy).expanduser() if live_state_npy else None
        self.require_live = require_live
        self.max_age = max_age
        self.last_mtime = 0.0
        self.live_state = None
        self.last_good_load_time = 0.0
        # <modify> live-state偶尔卡住时不让发送线程无声崩掉;按秒限频打印fallback提示。
        self.last_fallback_warn_time = 0.0

    def get(self, fallback: np.ndarray) -> np.ndarray:
        if self.live_state_npy is None:
            return fallback.copy()
        try:
            stat = self.live_state_npy.stat()
            if stat.st_mtime != self.last_mtime:
                self.live_state = load_state_npy(self.live_state_npy)
                self.last_mtime = stat.st_mtime
                self.last_good_load_time = time.time()
        except FileNotFoundError:
            pass
        except Exception as exc:
            print(f"[state] live state load failed: {exc}")
        if self.live_state is None:
            if self.require_live:
                raise RuntimeError(f"live state not available: {self.live_state_npy}")
            return fallback.copy()
        if self.require_live and time.time() - self.last_mtime > self.max_age:
            raise RuntimeError(
                f"live state is stale: age={time.time() - self.last_mtime:.3f}s path={self.live_state_npy}"
            )
        return self.live_state.copy()

    def get_or_fallback(self, fallback: np.ndarray) -> np.ndarray:
        # <modify> 首帧仍要求live-state就绪;运行中如果sim/live-state短暂过期,则沿用reference state继续把plan发完。
        # 原逻辑:require_live_state下live-state超过max_age会抛异常,send_obs_loop线程退出但主流程还在等action。
        try:
            return self.get(fallback)
        except RuntimeError as exc:
            now = time.time()
            if now - self.last_fallback_warn_time > 1.0:
                print(f"[state] live state fallback to reference: {exc}", flush=True)
                self.last_fallback_warn_time = now
            return fallback.copy()

    def has_live(self) -> bool:
        return self.live_state is not None

    def wait_first_live(self, fallback: np.ndarray, timeout: float) -> np.ndarray:
        deadline = time.time() + timeout
        while time.time() < deadline:
            try:
                state = self.get(fallback)
            except RuntimeError:
                state = fallback
            if self.has_live() and time.time() - self.last_mtime <= self.max_age:
                print(f"[state] live state ready: {self.live_state_npy}")
                return state
            time.sleep(0.05)
        raise TimeoutError(f"live state not ready within {timeout:.1f}s: {self.live_state_npy}")


def wait_live_state_upright(
    state_provider: StateProvider,
    fallback: np.ndarray,
    min_root_z: float,
    stable_seconds: float,
    timeout: float,
) -> tuple[float, float]:
    if min_root_z <= 0.0 or stable_seconds <= 0.0:
        return 0.0, float("nan")
    start = time.time()
    stable_start = None
    best_z = -float("inf")
    last_z = float("nan")
    while time.time() - start < timeout:
        now = time.time()
        try:
            state = state_provider.get(fallback)
            root_z = float(state[31])
            fresh = state_provider.has_live() and now - state_provider.last_mtime <= state_provider.max_age
        except RuntimeError:
            root_z = float("nan")
            fresh = False
        except Exception:
            root_z = float("nan")
            fresh = False
        if np.isfinite(root_z):
            best_z = max(best_z, root_z)
            last_z = root_z
        if fresh and root_z >= min_root_z:
            if stable_start is None:
                stable_start = now
            if now - stable_start >= stable_seconds:
                return now - start, root_z
        else:
            stable_start = None
        time.sleep(0.05)
    raise TimeoutError(
        "robot did not become upright before recording: "
        f"min_root_z={min_root_z:.3f}, stable_seconds={stable_seconds:.2f}, "
        f"timeout={timeout:.2f}, last_root_z={last_z:.3f}, best_root_z={best_z:.3f}"
    )


def wait_start_gate(ready_flag_text: str, gate_flag_text: str, timeout: float) -> None:
    if ready_flag_text:
        ready_flag = Path(ready_flag_text)
        ready_flag.parent.mkdir(parents=True, exist_ok=True)
        ready_flag.write_text(str(time.time()), encoding="utf-8")
        print(f"[bridge] startup ready: {ready_flag}", flush=True)
    if not gate_flag_text:
        return
    gate_flag = Path(gate_flag_text)
    print(f"[bridge] waiting start gate: {gate_flag}", flush=True)
    deadline = time.time() + timeout
    while time.time() < deadline:
        if gate_flag.exists():
            print(f"[bridge] start gate released: {gate_flag}", flush=True)
            return
        time.sleep(0.1)
    raise TimeoutError(f"start gate was not released within {timeout:.1f}s: {gate_flag}")


def write_render_timing(
    path_text: str,
    stream_frame_offset: int,
    pre_roll_elapsed_seconds: float,
    startup_seconds: float,
) -> None:
    if not path_text:
        return
    path = Path(path_text)
    path.parent.mkdir(parents=True, exist_ok=True)
    payload = {
        "stream_frame_offset": int(stream_frame_offset),
        "ctrl_hz": CTRL_HZ,
        "startup_seconds": float(startup_seconds),
        "pre_roll_elapsed_seconds": float(pre_roll_elapsed_seconds),
        "created_at": time.time(),
    }
    path.write_text(json.dumps(payload, sort_keys=True), encoding="utf-8")
    print(
        f"[bridge] render timing: startup_seconds={payload['startup_seconds']:.3f} path={path}",
        flush=True,
    )


def build_request_payload(
    state: np.ndarray,
    prompt: str,
    image: np.ndarray,
    target_frames: int = 0,
    segment_index: int = 0,
    segment_elapsed_frames: int = 0,
    video_condition: dict | None = None,
) -> str:
    # <modify> 将目标动作帧数放入condition,server端据此计算progress/future_progress。
    condition = {}
    if int(target_frames) > 0:
        condition["progress_target_frames"] = int(target_frames)
    # <modify> planner多段模式下把当前段号/段内帧号也发给RTC server,用于段切换时重置progress。
    if int(segment_index) > 0:
        condition["segment_index"] = int(segment_index)
        condition["segment_elapsed_frames"] = int(segment_elapsed_frames)
    if video_condition:
        condition.update(video_condition)
    payload = {
        "image": {"cam0": image},
        "state": {"states": state.astype(np.float32)},
        "gt_action": None,
        "dataset_name": None,
        "instruction": prompt,
        "history": None,
        # 原逻辑:
        # "condition": None,
        "condition": condition,
        "timestamp": None,
    }
    return json.dumps(convert_numpy_in_dict(payload, numpy_serialize))


def decode_response_actions(message: str) -> np.ndarray | None:
    data = convert_numpy_in_dict(json.loads(message), numpy_deserialize)
    if "action" not in data or data["action"] is None:
        return None
    action = np.asarray(data["action"], dtype=np.float32)
    if action.ndim == 1:
        action = action[None, :]
    elif action.ndim == 3:
        action = action[0]
    elif action.ndim != 2:
        raise ValueError(f"Unsupported action response shape: {action.shape}")
    return action.astype(np.float32)


def decode_response_message(message: str) -> tuple[np.ndarray | None, dict]:
    # <modify> 同时解析server返回的chunk元信息,方便bridge侧确认chunk是否连续。
    data = convert_numpy_in_dict(json.loads(message), numpy_deserialize)
    if "action" not in data or data["action"] is None:
        return None, data
    action = np.asarray(data["action"], dtype=np.float32)
    if action.ndim == 1:
        action = action[None, :]
    elif action.ndim == 3:
        action = action[0]
    elif action.ndim != 2:
        raise ValueError(f"Unsupported action response shape: {action.shape}")
    return action.astype(np.float32), data


def action_to_abs_state(action: np.ndarray, current_state: np.ndarray) -> np.ndarray:
    # <modify> v13 TextOp action是39D;不能把39D token误当成36D绝对姿态。
    # 原逻辑:
    # if action.shape[0] == 70:
    if action.shape[0] == SEED_TEXTOP_ACTION_DIM:
        next_state = recover_seed_abs_from_textop_action(action[None, :], current_state)[0]
    elif action.shape[0] == 36:
        next_state = action[:36].copy()
    else:
        raise ValueError(f"Unsupported action shape: {action.shape}")
    next_state[32:36] = normalize_quat_wxyz(next_state[32:36])
    return next_state.astype(np.float32)


def actions_to_abs_states(actions: np.ndarray, current_state: np.ndarray, max_frames: int) -> np.ndarray:
    # <modify> SONIC streamed-motion一次发送多帧;TextOp action chunk按当前state恢复成连续绝对姿态。
    actions = np.asarray(actions, dtype=np.float32)
    if max_frames > 0:
        actions = actions[:max_frames]
    if actions.ndim != 2 or actions.shape[0] <= 0:
        raise ValueError(f"Unsupported action chunk shape: {actions.shape}")
    # <modify> v13 TextOp chunk按motion_repr.SEED_TEXTOP_ACTION_DIM恢复到36D Sonic姿态。
    # 原逻辑:
    # if actions.shape[1] == 70:
    if actions.shape[1] == SEED_TEXTOP_ACTION_DIM:
        states = recover_seed_abs_from_textop_action(actions, current_state)
    elif actions.shape[1] == 36:
        states = actions[:, :36].copy()
    else:
        raise ValueError(f"Unsupported action chunk shape: {actions.shape}")
    for i in range(states.shape[0]):
        states[i, 32:36] = normalize_quat_wxyz(states[i, 32:36])
    return states.astype(np.float32)


def compose_policy_state(live_state: np.ndarray, reference_state: np.ndarray) -> np.ndarray:
    # <modify> SONIC tracking下MuJoCo root xy可能不随参考全局平移走;如果直接用live root解码,
    # 每个20帧chunk都会从接近0的root重新开始。这里保留live关节/root高度,同时用已发送reference
    # 的root xy和root朝向作为连续位姿种子。
    state = np.asarray(live_state, dtype=np.float32).copy()
    state[29:31] = reference_state[29:31]
    state[32:36] = reference_state[32:36]
    return state


def main() -> None:
    args = parse_args()
    wait_start_gate(args.startup_ready_flag, args.start_gate_flag, args.start_gate_timeout)
    ws_url = f"ws://{args.psi_host}:{args.psi_port}/ws"
    # <modify> 保留v13 infer模板原文大小写;server端也不再强制lower。
    # 原逻辑:
    # prompt = args.prompt.strip().lower()
    prompt = args.prompt.strip()
    # <modify> 如果传入planner motion_plan.json,则按segments逐段切换low-level instruction。
    prompt_plan = load_motion_prompt_plan(args.motion_plan_json, CTRL_HZ) if args.motion_plan_json else []
    if prompt_plan:
        total_plan_frames = sum(int(item["frames"]) for item in prompt_plan)
        if args.target_frames <= 0:
            args.target_frames = total_plan_frames
        print(f"[planner] loaded {len(prompt_plan)} segments / {total_plan_frames} frames")
        for item in prompt_plan:
            print(
                f"[planner] segment {item['index']}: {item['frames']} frames - {item['low_level_instruction']}",
                flush=True,
            )
    else:
        total_plan_frames = 0
    prompt_lock = threading.Lock()
    video_path = Path(args.inference_video_path).expanduser() if args.inference_video_path else None
    if video_path is not None and not video_path.exists():
        raise FileNotFoundError(f"inference video does not exist: {video_path}")
    if video_path is not None:
        print(
            "[video] video_clip input: "
            f"path={video_path} sample_fps={args.video_sample_fps} "
            f"size_pixels={args.video_size_pixels} motion_fps={args.video_clip_motion_fps}",
            flush=True,
        )
    print(
        "[stream] "
        f"chunk_frames={args.pose_chunk_frames} "
        f"advance_frames={args.pose_chunk_advance_frames} "
        f"coverage={args.pose_chunk_advance_frames / CTRL_HZ:.3f}s "
        f"send_sleep={args.pose_chunk_send_seconds:.3f}s",
        flush=True,
    )
    prompt_state = {
        "prompt": prompt_plan[0]["prompt"] if prompt_plan else prompt,
        "segment_index": int(prompt_plan[0]["index"]) if prompt_plan else 0,
        "segment_target_frames": int(prompt_plan[0]["frames"]) if prompt_plan else int(args.target_frames),
        "segment_elapsed_frames": 0,
        "video_frame_cursor": 0,
    }

    def update_prompt_state(sent_frames: int) -> None:
        if not prompt_plan:
            return
        cursor = 0
        selected = prompt_plan[-1]
        selected_start = max(0, total_plan_frames - int(selected["frames"]))
        for item in prompt_plan:
            next_cursor = cursor + int(item["frames"])
            if sent_frames < next_cursor:
                selected = item
                selected_start = cursor
                break
            cursor = next_cursor
        segment_elapsed = min(max(0, sent_frames - selected_start), int(selected["frames"]))
        with prompt_lock:
            old_index = int(prompt_state["segment_index"])
            prompt_state["prompt"] = selected["prompt"]
            prompt_state["segment_index"] = int(selected["index"])
            prompt_state["segment_target_frames"] = int(selected["frames"])
            prompt_state["segment_elapsed_frames"] = int(segment_elapsed)
        if old_index != int(selected["index"]):
            print(
                f"[planner] switch to segment {selected['index']}: {selected['low_level_instruction']}",
                flush=True,
            )
    dummy_image = np.zeros((args.dummy_image_height, args.dummy_image_width, 3), dtype=np.uint8)

    current_state = load_initial_state(args)
    state_provider = StateProvider(
        args.live_state_npy,
        require_live=args.require_live_state,
        max_age=args.live_state_max_age,
    )
    if args.require_live_state:
        if not args.live_state_npy:
            raise ValueError("--require-live-state needs --live-state-npy")
        try:
            current_state = state_provider.wait_first_live(current_state, args.live_state_timeout)
        except TimeoutError as exc:
            raise SystemExit(str(exc)) from None
    state_lock = threading.Lock()
    stop_event = threading.Event()
    first_pose_event = threading.Event()
    startup_errors: list[str] = []

    # <modify> 在线桥接:通过SONIC ZMQManager的command topic切到streamed-motion模式。
    sonic = SonicZMQPublisher(args.sonic_zmq_host, args.sonic_zmq_port)
    for _ in range(10):
        sonic.send_command(start=True, stop=False, planner=False)
        time.sleep(0.1)
    print("[sonic] requested STREAMED_MOTION mode")

    # <modify> Psi0 sonic server首帧会初始化RTC controller,首次回复可能比单帧周期慢很多。
    ws = websocket.create_connection(ws_url, timeout=30)
    ws.settimeout(30)
    print(f"[psi] connected {ws_url}")
    # <modify> 录制和MAX_SECONDS计时都延后到第一包pose真正发给WBC时开始。
    # 原逻辑:
    # if args.record_flag_path:
    #     record_flag = Path(args.record_flag_path)
    #     record_flag.parent.mkdir(parents=True, exist_ok=True)
    #     record_flag.write_text(str(time.time()), encoding="utf-8")
    #     print(f"[record] enabled: {record_flag}")
    record_flag = Path(args.record_flag_path) if args.record_flag_path else None
    settle_flag = Path(args.settle_flag_path) if args.settle_flag_path else None

    def enable_recording(label: str) -> None:
        if record_flag is None or record_flag.exists():
            return
        record_flag.parent.mkdir(parents=True, exist_ok=True)
        record_flag.write_text(str(time.time()), encoding="utf-8")
        print(f"[record] enabled: {record_flag} ({label})")

    def send_obs_loop() -> None:
        next_tick = time.perf_counter()
        while not stop_event.is_set():
            try:
                with state_lock:
                    # <modify> 发送给Psi0的observation仍使用live关节,但root xy/yaw沿用reference累积值,
                    # 防止每个chunk的root全局位置被固定的仿真root重置。
                    # 原逻辑:
                    # state = state_provider.get(current_state)
                    live_state = state_provider.get_or_fallback(current_state)
                    state = compose_policy_state(live_state, current_state)
                # <modify> 50Hz发送最新36D state;如果配置了live_state_npy,这里会优先使用真实/仿真状态。
                with prompt_lock:
                    active_prompt = str(prompt_state["prompt"])
                    active_target_frames = int(prompt_state["segment_target_frames"])
                    active_segment_index = int(prompt_state["segment_index"])
                    active_segment_elapsed_frames = int(prompt_state["segment_elapsed_frames"])
                    active_video_frame_cursor = int(prompt_state["video_frame_cursor"])
                video_condition = None
                if video_path is not None:
                    video_clip_start_sec = float(active_video_frame_cursor) / float(args.video_clip_motion_fps)
                    video_clip_duration_sec = float(args.pose_chunk_frames) / float(args.video_clip_motion_fps)
                    video_condition = {
                        "input_mode": "video_clip",
                        "video_path": str(video_path),
                        "video_config_key": str(args.video_config_key),
                        "video_clip_start_sec": video_clip_start_sec,
                        "video_clip_duration_sec": video_clip_duration_sec,
                        "video_sample_fps": float(args.video_sample_fps),
                        "video_size_pixels": int(args.video_size_pixels),
                        "video_decoder": str(args.video_decoder),
                        "video_clip_motion_fps": float(args.video_clip_motion_fps),
                    }
                ws.send(
                    build_request_payload(
                        state,
                        active_prompt,
                        dummy_image,
                        active_target_frames,
                        active_segment_index,
                        active_segment_elapsed_frames,
                        video_condition,
                    )
                )
            except Exception as exc:
                # <modify> 发送线程出错时显式停止主循环,避免WBC只收到前几包后一直刷waiting。
                print(f"[psi] send stopped: {exc}", flush=True)
                if not first_pose_event.is_set():
                    startup_errors.append(f"send failed before first pose: {exc}")
                stop_event.set()
                break
            next_tick += CTRL_DT
            sleep_time = next_tick - time.perf_counter()
            if sleep_time > 0.0:
                time.sleep(sleep_time)
            else:
                next_tick = time.perf_counter()

    sender = threading.Thread(target=send_obs_loop, daemon=True)
    sender.start()

    latest_actions = None
    latest_action_meta = {}
    latest_action_version = 0
    action_lock = threading.Lock()
    action_event = threading.Event()

    def recv_action_loop() -> None:
        nonlocal latest_actions, latest_action_meta, latest_action_version
        while not stop_event.is_set():
            try:
                message = ws.recv()
            except Exception as exc:
                # <modify> server主动断开或测试结束时优雅退出,避免只留下长traceback。
                # 原逻辑:主循环中直接ws.recv(),连接关闭会抛出完整异常。
                print(f"[psi] recv stopped: {exc}")
                if not first_pose_event.is_set():
                    startup_errors.append(f"recv failed before first pose: {exc}")
                stop_event.set()
                break
            if message is None or message == "":
                print("[psi] recv closed")
                if not first_pose_event.is_set():
                    startup_errors.append("recv closed before first pose")
                stop_event.set()
                break
            try:
                # <modify> 解析server侧chunk_version/controller_generated_frames,后面每包打印出来确认连续性。
                # 原逻辑:
                # actions = decode_response_actions(message)
                actions, meta = decode_response_message(message)
            except Exception as exc:
                # <modify> 测试结束或服务端返回非JSON控制消息时,只停止bridge,不打印大段traceback。
                # 原逻辑:decode_response_actions(message)
                print(f"[psi] decode stopped: {exc}", flush=True)
                if not first_pose_event.is_set():
                    startup_errors.append(f"decode failed before first pose: {exc}")
                stop_event.set()
                break
            if actions is None:
                continue
            with action_lock:
                latest_actions = actions
                latest_action_meta = dict(meta)
                latest_action_version += 1
            action_event.set()

    receiver = threading.Thread(target=recv_action_loop, daemon=True)
    receiver.start()

    frame_idx = 0
    prev_joint = current_state[:29].copy()
    # <modify> start_time=None表示还没收到第一包action;避免server warmup时间挤占有效运动时长。
    # 原逻辑:
    # start_time = time.time()
    start_time = None
    last_action_receive_time = None
    last_sent_version = 0
    stream_frame_offset = 0
    pre_roll_done = False
    stream_target_frames = total_plan_frames if total_plan_frames > 0 else max(0, int(args.target_frames))
    try:
        while not stop_event.is_set():
            # <modify> planner模式下以motion_plan的帧数为准,避免wall-clock计时把最后几个chunk提前截断。
            # 原逻辑:
            # if start_time is not None and args.max_seconds > 0 and time.time() - start_time > args.max_seconds:
            if (
                stream_target_frames <= 0
                and start_time is not None
                and args.max_seconds > 0
                and time.time() - start_time > args.max_seconds
            ):
                break
            if stream_target_frames > 0 and frame_idx >= stream_target_frames:
                break
            if not action_event.wait(timeout=1.0):
                # <modify> server静默卡住时,不要让bridge无限等待并持续打印fallback/wbc waiting日志。
                # 原逻辑:
                # if not action_event.wait(timeout=1.0):
                #     continue
                if (
                    start_time is not None
                    and last_action_receive_time is not None
                    and args.action_idle_timeout_seconds > 0
                    and time.time() - last_action_receive_time > args.action_idle_timeout_seconds
                ):
                    raise RuntimeError(
                        "No new action received from Psi0 server for "
                        f"{time.time() - last_action_receive_time:.2f}s"
                    )
                continue
            with action_lock:
                actions = None if latest_actions is None else latest_actions.copy()
                action_meta = dict(latest_action_meta)
                action_version = latest_action_version
                action_event.clear()
            if actions is None or action_version == last_sent_version:
                continue
            last_action_receive_time = time.time()
            last_sent_version = action_version
            update_prompt_state(frame_idx)

            with state_lock:
                # <modify> TextOp root delta解码也用连续reference root作为种子,否则20帧chunk之间root会回零。
                # 原逻辑:
                # decode_state = state_provider.get(current_state)
                live_state = state_provider.get_or_fallback(current_state)
                decode_state = compose_policy_state(live_state, current_state)
                # <modify> Psi0 server返回30帧TextOp预测窗口;这里恢复成连续SONIC绝对姿态窗口。
                # <modify> planner最后一段如果剩余不足一个chunk,只发送剩余帧数,保证650帧计划不会变成640/660帧。
                # 原逻辑:
                # state_chunk = actions_to_abs_states(actions, decode_state, args.pose_chunk_frames)
                max_chunk_frames = int(args.pose_chunk_frames)
                if stream_target_frames > 0:
                    max_chunk_frames = min(max_chunk_frames, max(0, stream_target_frames - frame_idx))
                if max_chunk_frames <= 0:
                    break
                advance_frames = min(
                    int(args.pose_chunk_advance_frames),
                    max(1, max_chunk_frames),
                )
                state_chunk = actions_to_abs_states(actions, decode_state, max_chunk_frames)
                advance_frames = min(advance_frames, int(state_chunk.shape[0]))
                next_current_state = state_chunk[advance_frames - 1].copy()

            joint_pos = state_chunk[:, :29]
            joint_vel = np.empty_like(joint_pos)
            # <modify> 第一包首帧不要用metadata初始姿态和模型首帧直接差分,否则WBC会看到几十rad/s的尖峰速度。
            # 原逻辑:
            # joint_vel[0] = (joint_pos[0] - prev_joint) / CTRL_DT
            if start_time is None:
                joint_vel[0] = 0.0
            else:
                joint_vel[0] = (joint_pos[0] - prev_joint) / CTRL_DT
            if joint_pos.shape[0] > 1:
                joint_vel[1:] = (joint_pos[1:] - joint_pos[:-1]) / CTRL_DT
            body_quat = state_chunk[:, 32:36]
            n = int(state_chunk.shape[0])
            frame_indices = np.arange(stream_frame_offset + frame_idx, stream_frame_offset + frame_idx + n, dtype=np.int64)
            if start_time is None:
                if not pre_roll_done and args.stand_pre_roll_seconds > 0.0:
                    # <modify> 正式动作前先让机器人追踪第一帧站稳,避免一开始从空中掉下来。
                    # 做法:给WBC发送一段重复第一帧的hold reference;sim看到settle_flag后释放elastic band但不录像。
                    # live_state连续站稳后再打开录像,额外录一段正常站立启动,再发送正式motion。
                    stand_frames = max(1, int(round(args.stand_pre_roll_seconds * CTRL_HZ)))
                    hold_seconds = max(
                        args.stand_pre_roll_seconds + 1.0,
                        args.record_stable_timeout_seconds + args.record_stable_seconds + 1.0,
                    )
                    hold_frames = max(
                        stand_frames + max(1, int(args.pose_chunk_frames)),
                        int(round(hold_seconds * CTRL_HZ)),
                    )
                    hold_joint_pos = np.repeat(joint_pos[:1], hold_frames, axis=0)
                    hold_joint_vel = np.zeros_like(hold_joint_pos)
                    hold_body_quat = np.repeat(body_quat[:1], hold_frames, axis=0)
                    hold_frame_indices = np.arange(0, hold_frames, dtype=np.int64)
                    pre_roll_start = time.time()
                    sonic.send_pose(hold_joint_pos, hold_joint_vel, hold_body_quat, hold_frame_indices, catch_up=False)
                    if settle_flag is not None:
                        settle_flag.parent.mkdir(parents=True, exist_ok=True)
                        settle_flag.write_text(str(time.time()), encoding="utf-8")
                    print(
                        f"[bridge] stand pre-roll: hold_frames=0..{hold_frames - 1} "
                        f"min_root_z={args.record_stable_min_root_z:.3f} "
                        f"stable_seconds={args.record_stable_seconds:.2f} "
                        f"timeout={args.record_stable_timeout_seconds:.2f}",
                        flush=True,
                    )
                    if args.record_stable_min_root_z > 0.0 and args.record_stable_seconds > 0.0:
                        stable_elapsed, stable_root_z = wait_live_state_upright(
                            state_provider,
                            current_state,
                            args.record_stable_min_root_z,
                            args.record_stable_seconds,
                            args.record_stable_timeout_seconds,
                        )
                    else:
                        time.sleep(args.stand_pre_roll_seconds)
                        stable_elapsed = time.time() - pre_roll_start
                        stable_root_z = float("nan")
                    record_start_time = time.time()
                    enable_recording("startup hold")
                    startup_hold_seconds = max(0.0, float(args.stand_pre_roll_seconds))
                    if startup_hold_seconds > 0.0:
                        print(f"[bridge] startup hold recording: {startup_hold_seconds:.2f}s", flush=True)
                        time.sleep(startup_hold_seconds)
                    recorded_startup_seconds = time.time() - record_start_time
                    elapsed_frames = max(stand_frames, int(round((time.time() - pre_roll_start) * CTRL_HZ)))
                    stream_frame_offset = min(max(0, elapsed_frames), max(0, hold_frames - int(args.pose_chunk_frames)))
                    print(
                        f"[bridge] stand pre-roll done: elapsed={stable_elapsed:.2f}s "
                        f"root_z={stable_root_z:.3f} "
                        f"startup_record={recorded_startup_seconds:.2f}s "
                        f"plan_start_stream_frame={stream_frame_offset}",
                        flush=True,
                    )
                    write_render_timing(
                        args.render_timing_json,
                        stream_frame_offset,
                        stable_elapsed,
                        recorded_startup_seconds,
                    )
                    pre_roll_done = True
                    last_sent_version = action_version
                    continue
                start_time = time.time()
                enable_recording("first pose")
                first_pose_event.set()
                print("[bridge] first pose sent, start MAX_SECONDS timer")
            sonic.send_pose(joint_pos, joint_vel, body_quat, frame_indices, catch_up=False)

            with state_lock:
                # <modify> 发送30帧预测窗口,但reference状态只推进20帧;后10帧作为overlap供下一包覆盖。
                # 原逻辑:
                # current_state = state_chunk[-1].copy()
                current_state = next_current_state

            # <modify> prev_joint跟随已推进帧,而不是预测窗口最后一帧;窗口尾部会被下一包overlap覆盖。
            # 原逻辑:
            # prev_joint = joint_pos[-1].copy()
            prev_joint = joint_pos[advance_frames - 1].copy()
            root = state_chunk[advance_frames - 1, 29:32]
            tail_root = state_chunk[-1, 29:32]
            print(
                f"[bridge] frames={frame_idx}..{frame_idx + n - 1} "
                f"advance={advance_frames} "
                f"root=({root[0]:.3f},{root[1]:.3f},{root[2]:.3f}) "
                f"tail=({tail_root[0]:.3f},{tail_root[1]:.3f},{tail_root[2]:.3f}) "
                f"action_shape={actions.shape} "
                f"server_version={action_meta.get('version')} "
                f"chunk_version={action_meta.get('chunk_version')} "
                f"ctrl_frames={action_meta.get('controller_generated_frames')}"
            )
            frame_idx += advance_frames
            update_prompt_state(frame_idx)
            with prompt_lock:
                prompt_state["video_frame_cursor"] = int(frame_idx)
            if args.pose_chunk_send_seconds > 0:
                time.sleep(args.pose_chunk_send_seconds)
    except KeyboardInterrupt:
        print("[bridge] interrupted")
    except Exception as exc:
        print(f"[bridge] stopped by error: {exc}")
        if not first_pose_event.is_set():
            startup_errors.append(str(exc))
    finally:
        stop_event.set()
        try:
            sonic.send_command(start=False, stop=True, planner=False)
        except Exception:
            pass
        try:
            ws.close()
        except Exception:
            pass
        sonic.close()
        print("[bridge] stopped")
    if not first_pose_event.is_set():
        if not startup_errors:
            startup_errors.append("bridge stopped before first pose")
        print("[bridge] fatal: " + "; ".join(startup_errors), flush=True)
        raise SystemExit(1)


if __name__ == "__main__":
    main()