File size: 13,133 Bytes
9fc9873
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
from __future__ import annotations

# <modify> 新增:启动SONIC MuJoCo sim,在线只记录qpos,结束后离线渲染mp4。
# 这个文件放在Psi0仓库里,不修改GR00T/gear_sonic/scripts目录。

import argparse
import json
import os
import signal
import subprocess
import tempfile
import time
import xml.etree.ElementTree as ET
from pathlib import Path

os.environ.setdefault("MUJOCO_GL", "egl")
os.environ.setdefault("PYOPENGL_PLATFORM", "egl")

import mujoco
import numpy as np

from gear_sonic.utils.mujoco_sim.base_sim import BaseSimulator
from gear_sonic.utils.mujoco_sim.configs import SimLoopConfig


RUNNING = True


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser()
    parser.add_argument("--video-path", default="/tmp/psi0_sonic_jog_ff_start_180_r_tracking.mp4")
    # <modify> 同步保存MuJoCo仿真qpos,方便直接检查机器人root xyz是否真的移动。
    parser.add_argument("--qpos-path", default="/tmp/psi0_sonic_jog_ff_start_180_r_tracking_qpos.npy")
    parser.add_argument("--max-seconds", type=float, default=20.0)
    # <modify> 与GR00T/gear_sonic/scripts/2_run.sh的离线渲染参数对齐。
    parser.add_argument("--video-width", type=int, default=1440)
    parser.add_argument("--video-height", type=int, default=832)
    parser.add_argument("--video-fps", type=int, default=15)
    # 在线推理耗时会让录制墙钟时长偏离motion plan;渲染时可截到计划对应帧数。
    parser.add_argument("--render-max-frames", type=int, default=0)
    parser.add_argument("--render-timing-json", default="")
    parser.add_argument("--record-flag-path", default="")
    # 站稳预滚阶段:释放elastic band但不录像,等bridge写record flag后再正式录制。
    parser.add_argument("--settle-flag-path", default="")
    parser.add_argument("--stop-flag-path", default="")
    parser.add_argument("--live-state-npy", default="/tmp/psi0_sonic_live_state.npy")
    parser.add_argument("--live-state-period", type=float, default=0.02)
    parser.add_argument("--sim-frequency", type=int, default=200)
    parser.add_argument("--camera-distance", type=float, default=2.8)
    parser.add_argument("--camera-azimuth", type=float, default=120.0)
    parser.add_argument("--camera-elevation", type=float, default=-25.0)
    parser.add_argument("--camera-lookat", type=float, nargs=3, default=[0.0, 0.0, 0.7])
    return parser.parse_args()


def stop_handler(signum, frame) -> None:
    global RUNNING
    RUNNING = False


def prepare_scene_xml(scene_ref: str, width: int, height: int) -> tuple[str, Path | None]:
    scene_path = Path(scene_ref)
    if not scene_path.is_absolute():
        scene_path = Path("/cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/yetinghong/code/GR00T-WholeBodyControl") / scene_path

    tree = ET.parse(scene_path)
    root = tree.getroot()
    visual = root.find("visual")
    if visual is None:
        visual = ET.SubElement(root, "visual")
    global_elem = visual.find("global")
    if global_elem is None:
        global_elem = ET.SubElement(visual, "global")

    current_width = int(global_elem.get("offwidth", "640"))
    current_height = int(global_elem.get("offheight", "480"))
    target_width = max(current_width, width)
    target_height = max(current_height, height)
    if target_width == current_width and target_height == current_height:
        return str(scene_path), None

    global_elem.set("offwidth", str(target_width))
    global_elem.set("offheight", str(target_height))
    fd, tmp_path = tempfile.mkstemp(
        prefix=f"{scene_path.stem}_psi0_online_",
        suffix=".xml",
        dir=str(scene_path.parent),
    )
    os.close(fd)
    tree.write(tmp_path, encoding="unicode")
    return tmp_path, Path(tmp_path)


def open_ffmpeg(video_path: Path, width: int, height: int, fps: int) -> subprocess.Popen:
    video_path.parent.mkdir(parents=True, exist_ok=True)
    cmd = [
        "ffmpeg",
        "-y",
        "-loglevel",
        "error",
        "-f",
        "rawvideo",
        "-vcodec",
        "rawvideo",
        "-s",
        f"{width}x{height}",
        "-pix_fmt",
        "rgb24",
        "-r",
        str(fps),
        "-i",
        "-",
        "-an",
        "-vcodec",
        "libx264",
        "-preset",
        "veryfast",
        "-crf",
        "18",
        "-pix_fmt",
        "yuv420p",
        str(video_path),
    ]
    return subprocess.Popen(cmd, stdin=subprocess.PIPE, stdout=subprocess.DEVNULL, stderr=subprocess.PIPE)


def build_simulator(args: argparse.Namespace) -> tuple[BaseSimulator, mujoco.MjvCamera, Path | None]:
    sim_cfg = SimLoopConfig(
        enable_onscreen=False,
        enable_offscreen=True,
        sim_frequency=args.sim_frequency,
        verbose=False,
        interface="sim",
    )
    wbc_config = sim_cfg.load_wbc_yaml()
    wbc_config["ENV_NAME"] = sim_cfg.env_name
    wbc_config["ENABLE_ONSCREEN"] = False
    wbc_config["ENABLE_OFFSCREEN"] = True
    # <modify> 和2_run.sh保持一致:warmup阶段保留ElasticBand,正式录制时再释放。
    wbc_config["ENABLE_ELASTIC_BAND"] = True
    scene_ref, temp_path = prepare_scene_xml(
        wbc_config["ROBOT_SCENE"],
        args.video_width,
        args.video_height,
    )
    wbc_config["ROBOT_SCENE"] = scene_ref

    camera_configs = {
        "recording": {
            "height": args.video_height,
            "width": args.video_width,
        }
    }
    simulator = BaseSimulator(
        config=wbc_config,
        env_name=sim_cfg.env_name,
        onscreen=False,
        offscreen=True,
        camera_configs=camera_configs,
    )

    camera = mujoco.MjvCamera()
    mujoco.mjv_defaultCamera(camera)
    camera.type = mujoco.mjtCamera.mjCAMERA_TRACKING
    camera.trackbodyid = simulator.sim_env.mj_model.body("pelvis").id
    camera.distance = args.camera_distance
    camera.azimuth = args.camera_azimuth
    camera.elevation = args.camera_elevation
    camera.lookat[:] = np.asarray(args.camera_lookat, dtype=float)
    return simulator, camera, temp_path


def resolve_render_max_frames(base_frames: int, timing_json: str, video_fps: int) -> int:
    if base_frames <= 0:
        return 0
    if not timing_json:
        return base_frames
    path = Path(timing_json)
    if not path.exists():
        print(f"[record-sim] render timing not found, using base frames: {base_frames}")
        return base_frames
    try:
        timing = json.loads(path.read_text(encoding="utf-8"))
        startup_seconds = max(0.0, float(timing.get("startup_seconds", 0.0)))
    except Exception as exc:
        print(f"[record-sim] render timing read failed: {exc}; using base frames: {base_frames}")
        return base_frames
    total_frames = base_frames + int(np.ceil(startup_seconds * float(video_fps)))
    print(
        f"[record-sim] render timing: base_frames={base_frames} "
        f"startup_seconds={startup_seconds:.3f} total_frames={total_frames}"
    )
    return total_frames


def main() -> None:
    args = parse_args()
    signal.signal(signal.SIGINT, stop_handler)
    signal.signal(signal.SIGTERM, stop_handler)

    video_path = Path(args.video_path)
    simulator = None
    encoder = None
    scene_temp_path = None
    qpos_frames = []
    saved_video = False

    try:
        simulator, camera, scene_temp_path = build_simulator(args)
        renderer = simulator.sim_env.renderers["recording"]

        print(f"[record-sim] video: {video_path}")
        print("[record-sim] online sim started")
        start_time = time.time()
        next_record_time = start_time
        next_live_state_time = start_time
        was_recording = False
        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
        stop_flag = Path(args.stop_flag_path) if args.stop_flag_path else None
        live_state_path = Path(args.live_state_npy) if args.live_state_npy else None
        if live_state_path is not None:
            live_state_path.parent.mkdir(parents=True, exist_ok=True)
        sim_count = 0
        elastic_released = False

        while RUNNING and time.time() - start_time < args.max_seconds:
            step_start = time.monotonic()
            simulator.sim_env.sim_step()

            now = time.time()
            if stop_flag is not None and stop_flag.exists():
                print("[record-sim] stop flag received")
                break

            if live_state_path is not None and now >= next_live_state_time:
                joint_pos = np.asarray(
                    simulator.sim_env.mj_data.qpos[simulator.sim_env.body_joint_index + 7 - 1],
                    dtype=np.float32,
                )
                mujoco_to_isaaclab = np.asarray(
                    [0, 6, 12, 1, 7, 13, 2, 8, 14, 3, 9, 15, 22, 4, 10, 16, 23, 5, 11, 17, 24, 18, 25, 19, 26, 20, 27, 21, 28],
                    dtype=np.int64,
                )
                state = np.zeros(36, dtype=np.float32)
                state[:29] = joint_pos[mujoco_to_isaaclab]
                state[29:36] = simulator.sim_env.mj_data.qpos[:7].astype(np.float32)
                with open(str(live_state_path) + ".tmp", "wb") as f:
                    np.save(f, state)
                os.replace(str(live_state_path) + ".tmp", live_state_path)
                next_live_state_time += args.live_state_period

            settle_enabled = settle_flag is not None and settle_flag.exists()
            record_enabled = record_flag is None or record_flag.exists()
            if (settle_enabled or record_enabled) and not elastic_released:
                elastic_band = getattr(simulator.sim_env, "elastic_band", None)
                if elastic_band is not None:
                    elastic_band.enable = False
                    print("[record-sim] elastic band released")
                elastic_released = True
                if settle_enabled and not record_enabled:
                    print("[record-sim] settling before recording")
            if record_enabled and not was_recording:
                next_record_time = now
                was_recording = True
                print("[record-sim] recording enabled")
            if record_enabled and now >= next_record_time:
                qpos_frames.append(np.asarray(simulator.sim_env.mj_data.qpos, dtype=np.float64).copy())
                next_record_time += 1.0 / args.video_fps

            if sim_count % int(simulator.reward_dt / simulator.sim_dt) == 0:
                simulator.sim_env.update_reward()

            elapsed = time.monotonic() - step_start
            sleep_time = simulator.sim_dt - elapsed
            if sleep_time > 0.0:
                time.sleep(sleep_time)
            sim_count += 1

        if not qpos_frames:
            raise RuntimeError("no MuJoCo qpos frames were recorded")
        print(f"[record-sim] recorded qpos frames={len(qpos_frames)}")
        # 只影响最终保存/渲染帧数,不提前停止仿真和live_state更新。
        render_max_frames = resolve_render_max_frames(
            args.render_max_frames,
            args.render_timing_json,
            args.video_fps,
        )
        if render_max_frames > 0 and len(qpos_frames) > render_max_frames:
            qpos_frames = qpos_frames[:render_max_frames]
            print(f"[record-sim] trimmed render qpos frames={len(qpos_frames)}")
        # <modify> qpos[:, :3]就是仿真环境里的floating base xyz。
        qpos_path = Path(args.qpos_path)
        qpos_path.parent.mkdir(parents=True, exist_ok=True)
        np.save(qpos_path, np.stack(qpos_frames, axis=0))
        print(f"[record-sim] saved qpos: {qpos_path}")

        encoder = open_ffmpeg(video_path, args.video_width, args.video_height, args.video_fps)
        if encoder.stdin is None:
            raise RuntimeError("ffmpeg stdin is not available")
        simulator.sim_env.mj_data.qvel[:] = 0
        for qpos in qpos_frames:
            simulator.sim_env.mj_data.qpos[:] = qpos
            mujoco.mj_forward(simulator.sim_env.mj_model, simulator.sim_env.mj_data)
            renderer.update_scene(simulator.sim_env.mj_data, camera=camera)
            frame = renderer.render()
            encoder.stdin.write(frame.tobytes())
    finally:
        if encoder is not None:
            if encoder.stdin is not None and not encoder.stdin.closed:
                encoder.stdin.close()
            stderr = b""
            if encoder.stderr is not None:
                stderr = encoder.stderr.read()
            code = encoder.wait()
            if code != 0:
                print(stderr.decode("utf-8", errors="replace"))
                raise RuntimeError(f"ffmpeg failed with code {code}")
            saved_video = True
        if simulator is not None:
            simulator.close()
        if scene_temp_path is not None and scene_temp_path.exists():
            scene_temp_path.unlink(missing_ok=True)
        if saved_video:
            print(f"[record-sim] saved: {video_path}")


if __name__ == "__main__":
    main()