#!/usr/bin/env python3 """Issue manual L1/R1/status/quit commands to one live MuJoCo session.""" from __future__ import annotations import argparse import json import os from pathlib import Path import sys import time ROOT = Path(__file__).resolve().parents[1] if str(ROOT) not in sys.path: sys.path.insert(0, str(ROOT)) from runtime.h2_shm_bridge import H2SharedMemoryBridge, SIMULATOR_IDENTITY # noqa: E402 from runtime.configuration import load_settings # noqa: E402 from runtime.motion_attestation import interface_hash # noqa: E402 from runtime.planner_protocol import ( # noqa: E402 BALL_SOURCE_MONOTONIC_INDEX, ROBOT_SOURCE_MONOTONIC_INDEX, source_age_s, ) DEFAULT_LATEST = Path("/tmp/h2_live_sim_latest_run") def _resolve_run_dir(raw: str | None) -> Path: if raw: path = Path(raw).expanduser().resolve() else: try: path = Path(DEFAULT_LATEST.read_text(encoding="utf-8").strip()).resolve() except FileNotFoundError as error: raise RuntimeError("no live-sim run is registered") from error metadata = path.stat() if metadata.st_uid != os.getuid() or metadata.st_mode & 0o077: raise RuntimeError("live-sim run directory must be owner-only") return path def _shm_name(run_dir: Path) -> str: value = (run_dir / "sim_shm_name").read_text(encoding="utf-8").strip() if not value.startswith("h2_pingpong_sim_") or "/" in value: raise RuntimeError("run directory contains an invalid simulation SHM name") return value def _load_json(path: Path) -> dict[str, object]: try: value = json.loads(path.read_text(encoding="utf-8")) except FileNotFoundError: return {} return value if isinstance(value, dict) else {} def _settings_for_run(run_dir: Path): try: path = Path((run_dir / "config_path").read_text(encoding="utf-8").strip()) except FileNotFoundError: path = ROOT / "config.yaml" version_path = run_dir / "deployment_version" version = version_path.read_text().strip() if version_path.is_file() else None return load_settings(path, deployment_version=version) def _touch_once(path: Path) -> None: try: descriptor = os.open(path, os.O_WRONLY | os.O_CREAT | os.O_EXCL, 0o600) except FileExistsError: return try: os.write(descriptor, f"{time.monotonic_ns()}\n".encode("ascii")) os.fsync(descriptor) finally: os.close(descriptor) def _read_state(run_dir: Path): # This command is a separate process, not the process family that owns the # simulation segment. Prevent multiprocessing.resource_tracker from # unlinking the owner's still-live name when this short-lived reader exits. bridge = H2SharedMemoryBridge(_shm_name(run_dir), independent_reader=True) try: identity = bridge.read_identity() state = bridge.read_state() finally: bridge.close() if identity.interface_hash != interface_hash(SIMULATOR_IDENTITY): raise RuntimeError("H2BR is not owned by the MuJoCo simulator") return identity, state def _print_status(run_dir: Path) -> int: identity, state = _read_state(run_dir) runner = _load_json(run_dir / "runner_status.json") plant = _load_json(run_dir / "bridge_status.json") now_ns = time.monotonic_ns() state_age = (now_ns - int(state.state_stamp_ns)) * 1.0e-9 planner_age = (now_ns - int(state.tt_stamp_ns)) * 1.0e-9 now_s = time.monotonic() robot_age = source_age_s(now_s, state.tt_data[ROBOT_SOURCE_MONOTONIC_INDEX]) ball_age = source_age_s(now_s, state.tt_data[BALL_SOURCE_MONOTONIC_INDEX]) print( f"phase={runner.get('phase', 'BOOT')} shm={_shm_name(run_dir)} " f"owner_flags=0x{identity.owner_flags:02x} " f"state_age={state_age:.4f}s planner_age={planner_age:.4f}s " f"robot_source_age={robot_age:.4f}s ball_source_age={ball_age:.4f}s " f"start={int(state.start_requested)} stop={int(state.stop_requested)} " f"fault={int(state.emergency_stop)}" ) print( f"policy_ticks={runner.get('policy_ticks', 0)} " f"active_ticks={runner.get('active_reference_policy_ticks', 0)} " f"accepted_id={runner.get('accepted_command_id', 0)} " f"reference_id={runner.get('reference_command_id', 0)} " f"root_z={plant.get('pelvis_pos', [None, None, None])[2]} " f"max_tilt={plant.get('max_tilt_rad')} " f"max_dq={plant.get('max_abs_joint_velocity_rad_s')} " f"max_torque_util={plant.get('max_torque_utilization')}" ) print( f"hybrid_ball_mode={plant.get('hybrid_ball_mode', 'WAIT_INPUT')} " f"phase={plant.get('hybrid_ball_phase', 'UNKNOWN')} " f"velocity_source={plant.get('hybrid_ball_velocity_source', 'none')} " f"contacts={plant.get('hybrid_ball_contact_count', 0)} " f"sim_steps={plant.get('hybrid_ball_simulated_steps', 0)} " f"source_age={plant.get('hybrid_ball_source_age_s')}" ) if runner.get("stop_reason"): print(f"stop_reason={runner['stop_reason']}") return 0 def _require_frozen_and_fresh(run_dir: Path, state) -> None: runner = _load_json(run_dir / "runner_status.json") if runner.get("phase") != "FROZEN_WAIT": raise RuntimeError( f"L1 rejected: runner phase is {runner.get('phase', 'BOOT')}, expected FROZEN_WAIT" ) if not (run_dir / "frozen.ready").exists(): raise RuntimeError("L1 rejected: runner has not reached FROZEN_WAIT") if state.stop_requested or state.emergency_stop: raise RuntimeError("L1 rejected: R1/fault is permanently latched") if not state.state_ready or not state.tt_ready: raise RuntimeError("L1 rejected: simulated LowState/planner is not ready") settings = _settings_for_run(run_dir) now_ns = time.monotonic_ns() state_age = ( float("inf") if int(state.state_stamp_ns) <= 0 else (now_ns - int(state.state_stamp_ns)) * 1.0e-9 ) planner_age = ( float("inf") if int(state.tt_stamp_ns) <= 0 else (now_ns - int(state.tt_stamp_ns)) * 1.0e-9 ) now_s = time.monotonic() robot_age = source_age_s( now_s, state.tt_data[ROBOT_SOURCE_MONOTONIC_INDEX] ) limits = ( ("state", state_age, settings.max_control_gap_s), ("planner receipt", planner_age, 0.10), ("robot source", robot_age, settings.robot_mocap_timeout_s), ) for label, age, limit in limits: if age < -0.01 or age > limit: raise RuntimeError( f"L1 rejected: {label} age {age:.4f}s exceeds {limit:.4f}s" ) def main(argv: list[str] | None = None) -> int: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "command", choices=("status", "check-frozen", "l1", "r1", "quit") ) parser.add_argument("--run-dir", default=None) args = parser.parse_args(argv) try: run_dir = _resolve_run_dir(args.run_dir) if args.command == "status": return _print_status(run_dir) _, state = _read_state(run_dir) if args.command in ("check-frozen", "l1"): _require_frozen_and_fresh(run_dir, state) if args.command == "check-frozen": _print_status(run_dir) print("[PASS] FROZEN_WAIT state/planner/simulated robot are fresh; ball visibility is not required") return 0 _touch_once(run_dir / "control.l1") print("[SIM L1] requested; policy will start on the next runner cycle") return 0 if args.command == "r1": _touch_once(run_dir / "control.r1") print("[SIM R1] permanently latched; use status, then quit for cleanup") return 0 if not (run_dir / "control.r1").exists() and not ( state.stop_requested or state.emergency_stop ): raise RuntimeError("quit rejected: issue r1 first") _touch_once(run_dir / "control.quit") print("[SIM QUIT] requested after R1; waiting for safe teardown") return 0 except (OSError, RuntimeError, ValueError) as error: print(f"[FAIL] {error}", file=sys.stderr) return 1 if __name__ == "__main__": raise SystemExit(main())