code-HFD / scripts /deploy /run_sonic_sim_record_online.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
9fc9873 verified
Raw History Blame Contribute Delete
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()