#!/usr/bin/env python3 """Run the real H2 policy state machine against an isolated MuJoCo H2BR plant.""" from __future__ import annotations import argparse from dataclasses import asdict from datetime import datetime import json import math import multiprocessing import os from pathlib import Path import queue import secrets import time import numpy as np from runtime.configuration import load_settings from runtime.deployment_versions import DEPLOYMENTS from runtime.embodiments import load_embodiment from runtime.h2_contract import H2_DOF from runtime.h2_shm_bridge import SHM_NAME, SHM_VERSION, SIMULATION_ACK, SIM_SHM_PREFIX from runtime.sim_mujoco_bridge import ( FAULT_KINDS, HIT_REACH_WINDOW_HALF_WIDTH_S, SimScenario, simulation_bridge_process, ) ROOT = Path(__file__).resolve().parent ACTIVE_RECOVERY_WINDOW_S = 0.50 MAX_SIM_ROOT_DROP_FROM_NOMINAL_M = 0.20 H2_MIN_ROOT_Z_M = load_embodiment("h2").nominal_root_z - MAX_SIM_ROOT_DROP_FROM_NOMINAL_M PADDLE_REACH_DISTANCE_M = 0.10 def _production_shm_snapshot() -> tuple[int, int, int] | None: path = Path("/dev/shm") / SHM_NAME try: metadata = path.stat() except FileNotFoundError: return None return metadata.st_ino, metadata.st_size, metadata.st_mtime_ns def _wait_message(status_queue, event: str, timeout_s: float): deadline = time.monotonic() + timeout_s deferred = [] while time.monotonic() < deadline: try: item = status_queue.get(timeout=min(0.25, max(0.01, deadline - time.monotonic()))) except queue.Empty: continue if item.get("event") == "error": raise RuntimeError(item.get("traceback") or item.get("error")) if item.get("event") == event: return item, deferred deferred.append(item) raise TimeoutError(f"timed out waiting for simulation bridge event {event!r}") def _validate_run_timing( *, duration_s: float, command_at_s: float, time_to_hit_s: float, fault_kind: str, fault_at_s: float, fault_duration_s: float | None = None, ) -> None: if not 1.0 <= duration_s <= 300.0: raise ValueError("--duration-s must be in [1,300]") minimum_healthy_duration_s = command_at_s + time_to_hit_s + ACTIVE_RECOVERY_WINDOW_S if fault_kind == "none" and duration_s < minimum_healthy_duration_s: raise ValueError( "healthy run must cover command_at_s + time_to_hit_s + " f"{ACTIVE_RECOVERY_WINDOW_S:.2f}s recovery " f"(requires >= {minimum_healthy_duration_s:.3f}s)" ) if fault_kind != "none" and duration_s <= fault_at_s + 0.20: raise ValueError("fault run must continue at least 0.20s past --fault-at-s") if fault_duration_s is not None and duration_s <= fault_at_s + fault_duration_s + 0.20: raise ValueError("dropout recovery run must include at least 0.20s after recovery") _DIRECT_INHIBITING_FAULTS = { "invalid_lowstate": "injected invalid LowState", "non_pr_mode": "LowState left PR(0) mode (mode_pr=1)", "motor_fault": "injected motor fault", } _MOCAP_BALANCE_DROPOUTS = {"robot_source_dropout", "planner_receipt_dropout"} _RUNNER_FAULT_TOKENS = { "planner_origin_mismatch": ("origin mismatch",), # Production OnPlanner rejects protocol!=3 by clearing tt_ready; it never # exposes a ready protocol-99 packet to the runner. "planner_protocol_mismatch": ("planner bridge has not received", "planner bridge is not ready"), "command_id_rollback": ("command id moved backwards",), # The detailed discontinuity is printed by _parse_planner; after parsing # marks mocap invalid, the permanent runner stop uses the generic reason. "root_jump": ("root jump", "root left runtime domain", "invalid robot mocap payload"), "heading_jump": ( "heading jump", "heading left runtime domain", "invalid robot mocap payload", ), } def _finite_float(value: object) -> float | None: try: result = float(value) except (TypeError, ValueError): return None return result if math.isfinite(result) else None def _healthy_acceptance_checks( *, runner_error: str, runner_summary: dict[str, object], bridge_report: dict[str, object], max_joint_velocity_rad_s: float, max_tilt_rad: float, ) -> dict[str, bool]: """Deterministic deployment-chain gates for a no-fault simulation run.""" bridge_fault_reason = str(bridge_report.get("fault_reason", "")) torque_utilization = _finite_float(bridge_report.get("max_torque_utilization")) joint_velocity = _finite_float(bridge_report.get("max_abs_joint_velocity_rad_s")) tilt = _finite_float(bridge_report.get("max_tilt_rad")) min_root_z = _finite_float(bridge_report.get("min_root_z_m")) paddle_ball_distance = _finite_float( bridge_report.get("min_paddle_to_ball_distance_m") ) paddle_hit_distance = _finite_float( bridge_report.get("min_paddle_to_hit_point_distance_m") ) paddle_ball_time = _finite_float(bridge_report.get("min_paddle_to_ball_elapsed_s")) paddle_hit_time = _finite_float( bridge_report.get("min_paddle_to_hit_point_elapsed_s") ) support_released_s = _finite_float(bridge_report.get("startup_support_released_s")) runner_active_elapsed_s = _finite_float( runner_summary.get("runner_active_elapsed_s") ) scenario = bridge_report.get("scenario", {}) command_at_s = _finite_float( scenario.get("command_at_s") if isinstance(scenario, dict) else None ) time_to_hit_s = _finite_float( scenario.get("time_to_hit_s") if isinstance(scenario, dict) else None ) hit_deadline_s = ( command_at_s + time_to_hit_s if command_at_s is not None and time_to_hit_s is not None else None ) accepted_command_id = int(runner_summary.get("accepted_command_id", 0)) reference_command_id = int(runner_summary.get("reference_command_id", 0)) return { "runner_completed_without_exception": not runner_error, "ready_completed": bool(runner_summary.get("ready_done")), "policy_ran": bool(runner_summary.get("policy_has_run")), "runner_not_stopped": not bool(runner_summary.get("stopped")), "policy_ticks_recorded": int(runner_summary.get("policy_ticks", 0)) > 0, "canonical_command_accepted": accepted_command_id >= 1, "active_reference_id_matches_command": ( reference_command_id >= 1 and reference_command_id == accepted_command_id ), "active_reference_policy_ticks_recorded": ( int(runner_summary.get("active_reference_policy_ticks", 0)) > 0 ), "active_cycle_and_recovery_completed": ( hit_deadline_s is not None and runner_active_elapsed_s is not None and runner_active_elapsed_s + 1.0e-9 >= hit_deadline_s + ACTIVE_RECOVERY_WINDOW_S ), "plant_actuated": int(bridge_report.get("actuation_steps", 0)) > 0, "virtual_support_released_before_active_command": ( support_released_s is not None and command_at_s is not None and support_released_s <= command_at_s ), "no_fault_was_injected": not bool(bridge_report.get("fault_injected")), "only_controlled_shutdown_latch": bridge_fault_reason in ( "", "Python runner requested the permanent fault latch", ), "plant_output_not_inhibited": not bool(bridge_report.get("output_inhibited")), "applied_effort_within_limits": ( torque_utilization is not None and torque_utilization <= 1.0 + 1.0e-6 ), "joint_velocity_within_runner_gate": ( joint_velocity is not None and joint_velocity <= float(max_joint_velocity_rad_s) + 1.0e-6 ), "tilt_within_runner_gate": ( tilt is not None and tilt <= float(max_tilt_rad) + 1.0e-6 ), "root_height_above_h2_safety_floor": ( min_root_z is not None and min_root_z >= H2_MIN_ROOT_Z_M ), "hit_window_sampled": int(bridge_report.get("hit_window_samples", 0)) > 0, "paddle_center_reached_ball_proxy": ( paddle_ball_distance is not None and paddle_ball_distance <= PADDLE_REACH_DISTANCE_M and paddle_ball_time is not None and hit_deadline_s is not None and abs(paddle_ball_time - hit_deadline_s) <= HIT_REACH_WINDOW_HALF_WIDTH_S + 1.0e-6 ), "paddle_center_reached_planned_hit_point": ( paddle_hit_distance is not None and paddle_hit_distance <= PADDLE_REACH_DISTANCE_M and paddle_hit_time is not None and hit_deadline_s is not None and abs(paddle_hit_time - hit_deadline_s) <= HIT_REACH_WINDOW_HALF_WIDTH_S + 1.0e-6 ), } def _fault_acceptance_checks( *, fault_kind: str, fault_at_s: float, runner_error: str, runner_summary: dict[str, object], bridge_report: dict[str, object], ) -> dict[str, bool]: """Prove the requested fault, rather than an unrelated early failure, won.""" fault_wall_s = _finite_float(bridge_report.get("fault_wall_s")) stop_reason = str(runner_summary.get("stop_reason", "")) response_text = f"{runner_error}\n{stop_reason}".lower() accepted_command_id = int(runner_summary.get("accepted_command_id", 0)) reference_command_id = int(runner_summary.get("reference_command_id", 0)) checks = { # All default fault cases are active-stroke tests. These preconditions # stop an early ready/fall failure from being counted as fault success. "ready_completed_before_fault": bool(runner_summary.get("ready_done")), "policy_ran_before_fault": bool(runner_summary.get("policy_has_run")), "canonical_command_accepted_before_fault": accepted_command_id >= 1, "active_reference_id_matched_before_fault": ( reference_command_id >= 1 and reference_command_id == accepted_command_id ), "active_reference_policy_ran_before_fault": ( int(runner_summary.get("active_reference_policy_ticks", 0)) > 0 ), "requested_fault_was_injected": bool(bridge_report.get("fault_injected")), "bridge_fault_latched": bool(bridge_report.get("fault_latched")), "response_not_before_injection": ( fault_wall_s is not None and fault_wall_s >= float(fault_at_s) - 0.03 ), "response_within_bounded_latency": ( fault_wall_s is not None and fault_wall_s <= float(fault_at_s) + 0.75 ), } bridge_reason = str(bridge_report.get("fault_reason", "")) if fault_kind in _DIRECT_INHIBITING_FAULTS: checks["expected_bridge_fault_reason"] = ( bridge_reason == _DIRECT_INHIBITING_FAULTS[fault_kind] ) checks["plant_output_inhibited"] = bool(bridge_report.get("output_inhibited")) elif fault_kind == "state_dropout": # The 50 Hz runner's 60 ms stale-state gate intentionally fires before # the simulated bridge's independent 100 ms watchdog. Whichever latch # wins the race, both the stale-state rejection and inhibited plant are # required; accepting only a generic runner exception would be too weak. checks["runner_rejected_stale_lowstate"] = "lowstate is stale" in response_text checks["bridge_reported_expected_dropout_path"] = bridge_reason in ( "Python runner requested the permanent fault latch", "simulated LowState watchdog expired", ) checks["plant_output_inhibited"] = bool(bridge_report.get("output_inhibited")) elif fault_kind == "corrupt_shm_nan": checks["runner_rejected_corrupt_lowstate"] = ( "lowstate contains nan/inf" in response_text ) checks["runner_requested_bridge_latch"] = ( bridge_reason == "Python runner requested the permanent fault latch" ) else: tokens = _RUNNER_FAULT_TOKENS.get(fault_kind, ()) checks["runner_stopped_fail_closed"] = bool(runner_summary.get("stopped")) checks["runner_reported_expected_fault_class"] = bool(tokens) and any( token in response_text for token in tokens ) checks["runner_requested_bridge_latch"] = ( bridge_reason == "Python runner requested the permanent fault latch" ) return checks def _mocap_balance_acceptance_checks( *, runner_error: str, runner_summary: dict[str, object], bridge_report: dict[str, object], duration_s: float, max_joint_velocity_rad_s: float, max_tilt_rad: float, ) -> dict[str, bool]: """A missing upstream pose must not remove the learned balance controller.""" healthy = _healthy_acceptance_checks( runner_error=runner_error, runner_summary=runner_summary, bridge_report=bridge_report, max_joint_velocity_rad_s=max_joint_velocity_rad_s, max_tilt_rad=max_tilt_rad, ) # A cancelled stroke is not required to reach the ball. Reuse the existing # unsupported-plant/finite-dynamics checks, not the healthy strike score. keep = ( "runner_completed_without_exception", "ready_completed", "policy_ran", "runner_not_stopped", "policy_ticks_recorded", "plant_actuated", "virtual_support_released_before_active_command", "only_controlled_shutdown_latch", "plant_output_not_inhibited", "applied_effort_within_limits", "joint_velocity_within_runner_gate", "tilt_within_runner_gate", "root_height_above_h2_safety_floor", ) checks = {key: healthy[key] for key in keep} elapsed = _finite_float(runner_summary.get("runner_active_elapsed_s")) fault_wall = _finite_float(bridge_report.get("fault_wall_s")) recovering = bridge_report.get("scenario", {}).get("fault_duration_s") is not None checks.update({ "requested_dropout_was_injected": bool(bridge_report.get("fault_injected")), "balance_policy_continued": ( int(runner_summary.get("mocap_balance_entries", 0)) >= 1 and int(runner_summary.get("mocap_balance_ticks", 0)) >= 2 ), "requested_control_duration_completed": elapsed is not None and elapsed >= duration_s, # run() retains its existing final fault request on deliberate teardown. "no_latch_before_controlled_shutdown": ( not bridge_report.get("fault_latched") or (fault_wall is not None and fault_wall >= duration_s - 0.03) ), "expected_recovery_state": runner_summary.get("mocap_balance_active") is (not recovering), }) return checks def _parser() -> argparse.ArgumentParser: parser = argparse.ArgumentParser( description=( f"Run the production 182-D/31-DoF runner over an isolated H2BR v{SHM_VERSION} segment " "whose plant is MuJoCo; no hardware middleware is used." ) ) parser.add_argument("--config", default=str(ROOT / "config.yaml")) parser.add_argument("--deployment-version", "--upstream-version", choices=DEPLOYMENTS) parser.add_argument("--duration-s", type=float, default=9.0) parser.add_argument("--standup-duration-s", type=float, default=5.0) parser.add_argument( "--arm-at-s", type=float, default=5.55, help="Simulated L1 edge; canonical value occurs after FROZEN_WAIT.", ) parser.add_argument("--command-at-s", type=float, default=6.30) parser.add_argument("--time-to-hit-s", type=float, default=0.80) parser.add_argument( "--physics-dt", type=float, default=0.005, help="MuJoCo integration step; H2BR transport remains fixed at 1000 Hz.", ) parser.add_argument("--viewer", action="store_true") parser.add_argument("--keep-mjcf-joint-dynamics", action="store_true") parser.add_argument( "--startup-support-until-s", type=float, default=None, help=( "Virtual suspension release time. The canonical default is exactly the " "simulated L1 edge; a later explicit value is a " "suspended diagnostic and cannot pass active-control acceptance." ), ) parser.add_argument("--fault", choices=FAULT_KINDS, default="none") parser.add_argument("--fault-at-s", type=float, default=6.80) parser.add_argument("--fault-duration-s", type=float, default=None, help="Mocap/planner dropout duration; default is no recovery.") parser.add_argument("--hit-position", type=float, nargs=3, default=(0.40, 0.0, 1.10)) parser.add_argument("--racket-velocity", type=float, nargs=3, default=(2.0, 0.0, 0.45)) parser.add_argument("--incoming-velocity", type=float, nargs=3, default=(-3.0, 0.0, 0.0)) parser.add_argument("--landing-xy", type=float, nargs=2, default=(2.87, 0.0)) parser.add_argument( "--handover-joint-offset", type=float, nargs=2, action="append", default=[], metavar=("HW_ID", "RAD"), help="Simulation-only joint offset injected on the L1 frame; may be repeated.", ) parser.add_argument( "--robot-base-origin-world", type=float, nargs=3, default=(-1.87, 0.0, -0.76), ) parser.add_argument("--report", type=Path, default=None) parser.add_argument( "--ack-simulation-only", required=True, help=f"Exact acknowledgement that this run is MuJoCo-only; expected {SIMULATION_ACK}.", ) return parser def main() -> None: args = _parser().parse_args() if args.ack_simulation_only != SIMULATION_ACK: raise SystemExit(f"--ack-simulation-only must equal {SIMULATION_ACK}") try: _validate_run_timing( duration_s=args.duration_s, command_at_s=args.command_at_s, time_to_hit_s=args.time_to_hit_s, fault_kind=args.fault, fault_at_s=args.fault_at_s, fault_duration_s=args.fault_duration_s, ) except ValueError as error: raise SystemExit(str(error)) from error settings = load_settings(args.config, deployment_version=args.deployment_version) handover_offsets = np.zeros(H2_DOF, dtype=np.float64) seen_handover_ids: set[int] = set() for raw_id, value in args.handover_joint_offset: joint_id = int(raw_id) if float(joint_id) != raw_id or not 0 <= joint_id < H2_DOF: raise SystemExit(f"handover HW_ID must be an integer in [0,{H2_DOF - 1}]") if joint_id in seen_handover_ids: raise SystemExit(f"duplicate handover HW_ID {joint_id}") if not np.isfinite(value): raise SystemExit("handover joint offsets must be finite") seen_handover_ids.add(joint_id) handover_offsets[joint_id] = value run_id = f"{os.getpid()}_{secrets.token_hex(6)}" shm_name = f"{SIM_SHM_PREFIX}{run_id}" stamp = datetime.now().strftime("%Y%m%d_%H%M%S") run_dir = ROOT / "sim_runs" / f"{stamp}_{run_id}" report_path = args.report.expanduser().resolve() if args.report else run_dir / "summary.json" trace_path = run_dir / "telemetry.jsonl" report_path.parent.mkdir(parents=True, exist_ok=True) run_dir.mkdir(parents=True, exist_ok=True) scenario = SimScenario( shm_name=shm_name, mjcf_path=str(ROOT / "assets/robot_description/h2/mjcf/h2_pingpong.xml"), ready_pose_path=str(ROOT / "ready_pose/H2/frozen_closd_ready_pose_toeout_lowload.npz"), trace_path=str(trace_path), physics_dt=args.physics_dt, arm_at_s=args.arm_at_s, command_at_s=args.command_at_s, time_to_hit_s=args.time_to_hit_s, fault_kind=args.fault, fault_at_s=args.fault_at_s, fault_duration_s=args.fault_duration_s, viewer=args.viewer, keep_mjcf_joint_dynamics=args.keep_mjcf_joint_dynamics, startup_support_until_s=( args.arm_at_s if args.startup_support_until_s is None else args.startup_support_until_s ), robot_base_origin_world=tuple(args.robot_base_origin_world), hit_position=tuple(args.hit_position), racket_velocity=tuple(args.racket_velocity), incoming_velocity=tuple(args.incoming_velocity), landing_xy=tuple(args.landing_xy), handover_joint_offsets_hw=tuple(handover_offsets.tolist()), ) scenario.validate() production_before = _production_shm_snapshot() context = multiprocessing.get_context("spawn") stop_event = context.Event() scenario_start_event = context.Event() status_queue = context.Queue() process = context.Process( target=simulation_bridge_process, args=(asdict(scenario), stop_event, scenario_start_event, status_queue), name=f"h2-mujoco-bridge-{run_id}", ) runner = None runner_summary: dict[str, object] = {} runner_active_elapsed_s: float | None = None runner_error = "" bridge_report: dict[str, object] = {} bridge_ready: dict[str, object] = {} process.start() try: bridge_ready, _ = _wait_message(status_queue, "ready", 30.0) print( f"[SIM] isolated H2BR ready name={shm_name!r}; " "production H2BR was not opened" ) # Import the heavy policy/CLoSD stack only after the spawn child owns # the plant. This avoids inheriting a CUDA context into MuJoCo. from runtime.real_shm_runner import RealShmRunner from runtime.runner_startup import SimulationRunnerStartup runner = RealShmRunner( args.config, deployment_version=settings.deployment_version, shm_name=shm_name, start_immediately=False, state_timeout_s=0.06, planner_timeout_s=0.10, standup_duration_s=args.standup_duration_s, robot_base_origin_world=tuple(args.robot_base_origin_world), startup_backend=SimulationRunnerStartup(args.ack_simulation_only), ) scenario_start_event.set() runner_active_started_s = time.monotonic() try: runner_summary = runner.run(max_duration_s=args.duration_s) except BaseException as error: runner_error = f"{type(error).__name__}: {error}" runner_summary = runner.run_summary() print(f"[SIM RUNNER EXIT] {runner_error}") finally: runner_active_elapsed_s = time.monotonic() - runner_active_started_s runner_summary = dict(runner_summary) runner_summary["runner_active_elapsed_s"] = runner_active_elapsed_s # Give the plant one bridge-watchdog window to observe the runner's # final fault request before controlled teardown. time.sleep(0.12) finally: stop_event.set() process.join(timeout=10.0) if process.is_alive(): process.terminate() process.join(timeout=3.0) if not runner_error: runner_error = "simulation bridge did not stop within 10s" try: while True: item = status_queue.get_nowait() if item.get("event") == "finished": bridge_report = item["report"] elif item.get("event") == "error" and not runner_error: runner_error = item.get("error", "simulation bridge error") bridge_report["bridge_traceback"] = item.get("traceback", "") except queue.Empty: pass status_queue.close() status_queue.join_thread() production_after = _production_shm_snapshot() production_untouched = production_before == production_after sim_path_removed = not (Path("/dev/shm") / shm_name).exists() writer_lock_removed = not (Path("/dev/shm") / f"{shm_name}.python_writer.lock").exists() if args.fault == "none": acceptance_checks = _healthy_acceptance_checks( runner_error=runner_error, runner_summary=runner_summary, bridge_report=bridge_report, max_joint_velocity_rad_s=settings.max_joint_velocity_rad_s, max_tilt_rad=settings.max_tilt_rad, ) expected = "ready + simulated L1 + canonical command + policy actuation" elif args.fault in _MOCAP_BALANCE_DROPOUTS: acceptance_checks = _mocap_balance_acceptance_checks( runner_error=runner_error, runner_summary=runner_summary, bridge_report=bridge_report, duration_s=args.duration_s, max_joint_velocity_rad_s=settings.max_joint_velocity_rad_s, max_tilt_rad=settings.max_tilt_rad, ) expected = f"frozen-reference balance policy through {args.fault}" else: acceptance_checks = _fault_acceptance_checks( fault_kind=args.fault, fault_at_s=args.fault_at_s, runner_error=runner_error, runner_summary=runner_summary, bridge_report=bridge_report, ) expected = f"fail-closed response to injected {args.fault}" acceptance_checks.update({ "production_shm_untouched": production_untouched, "simulation_shm_removed": sim_path_removed, "writer_lock_removed": writer_lock_removed, }) passed = all(acceptance_checks.values()) result = { "schema": "h2-simulated-real-deployment-report-v1", "passed": passed, "expected": expected, "acceptance_thresholds": { "active_recovery_window_s": ACTIVE_RECOVERY_WINDOW_S, "min_root_z_m": H2_MIN_ROOT_Z_M, "paddle_reach_distance_m": PADDLE_REACH_DISTANCE_M, "hit_reach_window_half_width_s": HIT_REACH_WINDOW_HALF_WIDTH_S, }, "acceptance_checks": acceptance_checks, "runner_error": runner_error, "runner_active_elapsed_s": runner_active_elapsed_s, "runner": runner_summary, "bridge": bridge_report, "bridge_ready": bridge_ready, "isolation": { "simulation_shm_name": shm_name, "production_shm_before": production_before, "production_shm_after": production_after, "production_shm_untouched": production_untouched, "simulation_shm_removed": sim_path_removed, "writer_lock_removed": writer_lock_removed, }, "telemetry_path": str(trace_path), } report_path.write_text(json.dumps(result, indent=2, sort_keys=True) + "\n", encoding="utf-8") print( f"[SIM {'PASS' if passed else 'FAIL'}] {expected}; " f"policy_ticks={runner_summary.get('policy_ticks', 0)} " f"command_id={runner_summary.get('accepted_command_id', 0)}" ) print(f"[SIM REPORT] {report_path}") print(f"[SIM TRACE] {trace_path}") if not passed: raise SystemExit(2) if __name__ == "__main__": main()