from __future__ import annotations # 新增:启动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") # 同步保存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) # 与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 # 和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)}") # 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()