| |
| """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 |
| from runtime.configuration import load_settings |
| from runtime.motion_attestation import interface_hash |
| from runtime.planner_protocol import ( |
| 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): |
| |
| |
| |
| 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()) |
|
|
