File size: 13,133 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 | 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()
|