Download scripts/deploy/run_sonic_sim_record_online.py from Tinghong-Ye22/code-HFD: direct link, hf CLI and curl.
- Browser
- Download file 13.1 kB
-
https://huggingface.co/Tinghong-Ye22/code-HFD/resolve/main/scripts/deploy/run_sonic_sim_record_online.py
- Command line
-
hf download hf://Tinghong-Ye22/code-HFD/scripts/deploy/run_sonic_sim_record_online.py
-
curl -L -o run_sonic_sim_record_online.py https://huggingface.co/Tinghong-Ye22/code-HFD/resolve/main/scripts/deploy/run_sonic_sim_record_online.py
13.1 kB
| 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() | |