PingPongRealWorldRL / runtime /sim_mujoco_bridge.py
chore: publish H2 real-world RL system
d8290eb verified 3 days ago
76.9 kB
"""Isolated 1000 Hz MuJoCo owner for simulation of the H2 deployment transport.
This module is the simulated plant/bridge side only.  It never runs the policy
and never imports the hardware middleware.  The unchanged 50 Hz
``RealShmRunner`` communicates with it through a random H2BR segment using the
same current ABI version as the hardware bridge.
"""

from __future__ import annotations

from contextlib import contextmanager
from dataclasses import asdict, dataclass
import fcntl
import json
import math
from multiprocessing import shared_memory
import os
from pathlib import Path
import stat
import time
import traceback
from typing import Iterator

import numpy as np

from . import ball as ball_model
from .embodiments import load_embodiment
from .h2_contract import (
    HARDWARE_JOINT_NAMES,
    H2_DOF,
    LOWCMD_DOF,
    MODE_PR,
    command_position_bounds_hw,
)
from .h2_shm_bridge import (
    BRIDGE_SIZE,
    H2BridgeMemory,
    MAX_KD,
    MAX_KP,
    MOTION_SWITCHER_ATTESTED_FLAG,
    PYTHON_FAULT_REQUEST_FLAG,
    SHM_MAGIC,
    SHM_NAME,
    SHM_VERSION,
    SIMULATION_RUNNER_ATTESTED_FLAG,
    SIMULATOR_IDENTITY,
    SIMULATOR_OWNER_FLAG,
    SIM_SHM_PREFIX,
    TT_DATA_LEN,
)
from .motion_attestation import interface_hash
from .mathutil import yaw_of
from .planner_protocol import (
    BALL_SOURCE_MONOTONIC_INDEX,
    H2_PLANNER_PACKET_LEN,
    PROTOCOL_VERSION_INDEX,
    ROBOT_SOURCE_MONOTONIC_INDEX,
    build_h2_planner_packet,
)


BRIDGE_HZ = 1000.0
BRIDGE_DT = 1.0 / BRIDGE_HZ
# Training/reference dynamics use 200 Hz. The transport still publishes state
# and consumes commands at 1000 Hz, so these clocks are intentionally separate.
DEFAULT_PHYSICS_DT = 0.005
COMMAND_WATCHDOG_S = 0.10
HOLD_TORQUE_FRACTION = 0.90
MOCAP_HZ = 120.0
TRACE_HZ = 50.0
# Mirror the fixed production C++ LowStateFinite/CommandValid envelope.
STATE_JOINT_LIMIT_TOLERANCE_RAD = 0.05
STATE_MAX_JOINT_VELOCITY_RAD_S = 40.0
LOWSTATE_MIN_QUATERNION_NORM = 0.5
LOWSTATE_MAX_QUATERNION_NORM = 1.5
HIT_REACH_WINDOW_HALF_WIDTH_S = 0.15

FAULT_KINDS = (
    "none",
    "state_dropout",
    "invalid_lowstate",
    "non_pr_mode",
    "motor_fault",
    "planner_receipt_dropout",
    "robot_source_dropout",
    "planner_origin_mismatch",
    "planner_protocol_mismatch",
    "command_id_rollback",
    "root_jump",
    "heading_jump",
    "corrupt_shm_nan",
)


def validate_sim_shm_name(name: str) -> str:
    normalized = str(name).lstrip("/")
    if normalized == SHM_NAME or not normalized.startswith(SIM_SHM_PREFIX):
        raise ValueError(f"simulator requires a random {SIM_SHM_PREFIX}<run-id> SHM name")
    if not normalized or "/" in normalized or normalized in {".", ".."}:
        raise ValueError("invalid simulation SHM name")
    return normalized


def _h2_armature(joint_name: str) -> float:
    """Isaac Lab H2 actuator armature, copied into this isolated new project."""

    joint = joint_name.removesuffix("_joint")
    armature_5020 = 0.003609725
    armature_7520_14 = 0.010177520
    armature_7520_22 = 0.025101925
    armature_4010 = 0.00425
    if joint.endswith(("_hip_pitch", "_hip_roll", "_knee")):
        return armature_7520_22
    if joint.endswith("_hip_yaw") or joint == "waist_yaw":
        return armature_7520_14
    if joint.endswith(("_ankle_pitch", "_ankle_roll")):
        return 2.0 * armature_5020
    if joint in ("waist_roll", "waist_pitch", "head_pitch", "head_yaw"):
        return 2.0 * armature_5020
    if joint.endswith(
        ("_shoulder_pitch", "_shoulder_roll", "_shoulder_yaw", "_elbow", "_wrist_roll")
    ):
        return armature_5020
    if joint.endswith(("_wrist_pitch", "_wrist_yaw")):
        return armature_4010
    raise ValueError(f"no H2 armature mapping for {joint_name!r}")


@dataclass(frozen=True)
class SimScenario:
    shm_name: str
    mjcf_path: str
    ready_pose_path: str
    trace_path: str
    physics_dt: float = DEFAULT_PHYSICS_DT
    # Canonical simulated L1 occurs only after the 5.0 s interpolation and
    # 0.5 s frozen-pose settle gate have completed.
    arm_at_s: float = 5.55
    command_at_s: float = 6.30
    time_to_hit_s: float = 0.80
    fault_kind: str = "none"
    fault_at_s: float = 6.80
    fault_duration_s: float | None = None
    viewer: bool = False
    keep_mjcf_joint_dynamics: bool = False
    # Match commissioning: the frozen pose may be suspended, but the feet must
    # carry the robot when L1 hands control to the balance policy.
    startup_support_until_s: float = 5.55
    robot_base_origin_world: tuple[float, float, float] = (-1.87, 0.0, -0.76)
    hit_position: tuple[float, float, float] = (0.40, 0.0, 1.10)
    racket_velocity: tuple[float, float, float] = (2.0, 0.0, 0.45)
    incoming_velocity: tuple[float, float, float] = (-3.0, 0.0, 0.0)
    landing_xy: tuple[float, float] = (2.87, 0.0)
    # Simulation-only perturbation applied atomically on the synthetic L1
    # frame. It exercises G1-style direct policy entry from measured suspended
    # gravity offsets without adding a production OOD threshold.
    handover_joint_offsets_hw: tuple[float, ...] = (0.0,) * H2_DOF
    handover_offset_preload_s: float = 0.50

    def validate(self) -> None:
        validate_sim_shm_name(self.shm_name)
        if self.fault_kind not in FAULT_KINDS:
            raise ValueError(f"unknown fault {self.fault_kind!r}; expected one of {FAULT_KINDS}")
        values = np.asarray(
            [self.physics_dt, self.arm_at_s, self.command_at_s, self.time_to_hit_s,
             self.startup_support_until_s,
             self.fault_at_s, self.handover_offset_preload_s,
             *self.robot_base_origin_world, *self.hit_position,
             *self.racket_velocity, *self.incoming_velocity, *self.landing_xy],
            dtype=np.float64,
        )
        if not np.isfinite(values).all():
            raise ValueError("simulation scenario contains NaN/Inf")
        offsets = np.asarray(self.handover_joint_offsets_hw, dtype=np.float64)
        if offsets.shape != (H2_DOF,) or not np.isfinite(offsets).all():
            raise ValueError(f"handover_joint_offsets_hw must contain {H2_DOF} finite values")
        if not 0.10 <= self.handover_offset_preload_s <= 1.0:
            raise ValueError("handover_offset_preload_s must be in [0.10,1.0]s")
        if not 0.001 <= self.physics_dt <= 0.005:
            raise ValueError("physics_dt must be in [0.001,0.005]s")
        if self.arm_at_s < 0.0 or self.command_at_s <= self.arm_at_s:
            raise ValueError("command_at_s must be later than arm_at_s")
        if self.startup_support_until_s < 0.0:
            raise ValueError("startup support release time must be non-negative")
        if not 0.0 < self.time_to_hit_s <= 2.0:
            raise ValueError("time_to_hit_s must be in (0,2]s")
        if self.fault_kind != "none" and self.fault_at_s <= 0.0:
            raise ValueError("fault_at_s must be positive")
        if self.fault_duration_s is not None:
            if self.fault_kind not in ("robot_source_dropout", "planner_receipt_dropout"):
                raise ValueError("fault_duration_s is only supported for mocap/planner dropouts")
            if not math.isfinite(self.fault_duration_s) or self.fault_duration_s <= 0.0:
                raise ValueError("fault_duration_s must be finite and positive")
        if not Path(self.mjcf_path).is_file() or not Path(self.ready_pose_path).is_file():
            raise FileNotFoundError("simulation MJCF or ready-pose artifact is missing")

    def fault_active(self, elapsed_s: float) -> bool:
        return (
            self.fault_kind != "none" and elapsed_s >= self.fault_at_s
            and (self.fault_duration_s is None
                 or elapsed_s < self.fault_at_s + self.fault_duration_s)
        )


@dataclass(frozen=True)
class SimCommand:
    stamp_ns: int
    enabled: bool
    flags: int
    mode_pr: int
    q: np.ndarray
    dq: np.ndarray
    tau: np.ndarray
    kp: np.ndarray
    kd: np.ndarray
    mode: np.ndarray
    reserve: np.ndarray


class SimH2BridgeOwner:
    """Exclusive creator and state writer for one random simulation H2BR."""

    def __init__(self, name: str):
        self.name = validate_sim_shm_name(name)
        self._shm = shared_memory.SharedMemory(name=self.name, create=True, size=BRIDGE_SIZE)
        path = Path("/dev/shm") / self.name
        if path.exists():
            os.chmod(path, 0o600)
            metadata = path.stat()
            if not stat.S_ISREG(metadata.st_mode) or metadata.st_uid != os.getuid():
                self._shm.unlink()
                self._shm.close()
                raise RuntimeError("simulation H2BR is not an owner-controlled regular file")
        self._shm.buf[:] = b"\x00" * BRIDGE_SIZE
        self.mem = H2BridgeMemory.from_buffer(self._shm.buf)
        self.mem.magic = SHM_MAGIC
        self.mem.version = SHM_VERSION
        self.mem.reserved_flags = SIMULATOR_OWNER_FLAG
        self.mem.bridge_interface_hash = interface_hash(SIMULATOR_IDENTITY)
        self.mem.state_mode_pr = MODE_PR
        self.mem.imu_quat_wxyz[0] = 1.0

    @contextmanager
    def locked(self, operation: int) -> Iterator[None]:
        while True:
            try:
                fcntl.flock(self._shm._fd, operation)
                break
            except InterruptedError:
                continue
        try:
            yield
        finally:
            fcntl.flock(self._shm._fd, fcntl.LOCK_UN)

    def read_command(self) -> SimCommand:
        with self.locked(fcntl.LOCK_SH):
            return SimCommand(
                stamp_ns=int(self.mem.command_stamp_ns),
                enabled=bool(self.mem.command_enabled),
                flags=int(self.mem.reserved_flags),
                mode_pr=int(self.mem.cmd_mode_pr),
                q=np.ctypeslib.as_array(self.mem.cmd_q).copy(),
                dq=np.ctypeslib.as_array(self.mem.cmd_dq).copy(),
                tau=np.ctypeslib.as_array(self.mem.cmd_tau).copy(),
                kp=np.ctypeslib.as_array(self.mem.cmd_kp).copy(),
                kd=np.ctypeslib.as_array(self.mem.cmd_kd).copy(),
                mode=np.ctypeslib.as_array(self.mem.cmd_mode).copy(),
                reserve=np.ctypeslib.as_array(self.mem.cmd_reserve).copy(),
            )

    def write_state(
        self,
        *,
        q: np.ndarray,
        dq: np.ndarray,
        quat_wxyz: np.ndarray,
        gyro: np.ndarray,
        root_xy: np.ndarray,
        quat_xyzw: np.ndarray,
        planner_packet: np.ndarray,
        start_requested: bool,
        stop_requested: bool,
        emergency_stop: bool,
        mode_pr: int,
        update_state: bool = True,
        update_planner: bool = True,
        planner_ready: bool = True,
        planner_stamp_ns: int | None = None,
    ) -> None:
        now_ns = time.monotonic_ns()
        with self.locked(fcntl.LOCK_EX):
            seq = int(self.mem.state_seq)
            if seq & 1:
                seq += 1
            if update_state:
                np.ctypeslib.as_array(self.mem.joint_pos)[:] = np.asarray(q, dtype=np.float32)
                np.ctypeslib.as_array(self.mem.joint_vel)[:] = np.asarray(dq, dtype=np.float32)
                np.ctypeslib.as_array(self.mem.imu_quat_wxyz)[:] = np.asarray(
                    quat_wxyz, dtype=np.float32
                )
                np.ctypeslib.as_array(self.mem.gyro)[:] = np.asarray(gyro, dtype=np.float32)
                np.ctypeslib.as_array(self.mem.root_current_xy)[:] = np.asarray(
                    root_xy, dtype=np.float32
                )
                np.ctypeslib.as_array(self.mem.mocap_pelvis_quat_xyzw)[:] = np.asarray(
                    quat_xyzw, dtype=np.float32
                )
                self.mem.state_stamp_ns = now_ns
                self.mem.state_ready = 1
                self.mem.robot_connected = 1
                self.mem.state_mode_pr = int(mode_pr)
            if update_planner:
                destination = np.ctypeslib.as_array(self.mem.tt_data)
                destination.fill(0.0)
                packet = np.asarray(planner_packet, dtype=np.float64).reshape(-1)
                if packet.size > TT_DATA_LEN:
                    raise ValueError(
                        f"planner packet has {packet.size} values; H2BR holds {TT_DATA_LEN}"
                    )
                destination[: packet.size] = packet
                stamp_ns = now_ns if planner_stamp_ns is None else int(planner_stamp_ns)
                if stamp_ns <= 0:
                    raise ValueError("planner receipt timestamp must be positive")
                self.mem.tt_stamp_ns = stamp_ns
                self.mem.tt_ready = int(bool(planner_ready))
            self.mem.start_requested = int(bool(start_requested))
            self.mem.stop_requested = int(bool(stop_requested))
            self.mem.emergency_stop = int(bool(emergency_stop))
            self.mem.mode_machine = 0
            self.mem.state_seq = seq + 2

    def mark_emergency(self) -> None:
        with self.locked(fcntl.LOCK_EX):
            self.mem.emergency_stop = 1

    def close(self) -> None:
        if getattr(self, "mem", None) is not None:
            self.mem = None
            self._shm.close()
        try:
            self._shm.unlink()
        except FileNotFoundError:
            pass
        lock_path = Path("/dev/shm") / f"{self.name}.python_writer.lock"
        try:
            lock_path.unlink()
        except FileNotFoundError:
            pass


class H2MujocoPlant:
    """Floating-base H2 plant with all indices resolved from joint names."""

    def __init__(
        self,
        mjcf_path: str,
        ready_pose_path: str,
        *,
        physics_dt: float,
        keep_mjcf_joint_dynamics: bool,
    ):
        import mujoco

        self.mujoco = mujoco
        self.model = mujoco.MjModel.from_xml_path(str(mjcf_path))
        self.data = mujoco.MjData(self.model)
        if self.model.nu != H2_DOF:
            raise RuntimeError(f"H2 MJCF must have {H2_DOF} actuators, got {self.model.nu}")
        self.model.opt.timestep = float(physics_dt)

        qpos_addr: list[int] = []
        qvel_addr: list[int] = []
        actuator_id: list[int] = []
        joint_lo: list[float] = []
        joint_hi: list[float] = []
        for name in HARDWARE_JOINT_NAMES:
            jid = int(mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name))
            if jid < 0:
                raise RuntimeError(f"H2 MJCF is missing joint {name!r}")
            matches = np.flatnonzero(self.model.actuator_trnid[:, 0] == jid)
            if matches.size != 1:
                raise RuntimeError(f"joint {name!r} must have exactly one actuator, got {matches.tolist()}")
            qpos_addr.append(int(self.model.jnt_qposadr[jid]))
            qvel_addr.append(int(self.model.jnt_dofadr[jid]))
            actuator_id.append(int(matches[0]))
            joint_lo.append(float(self.model.jnt_range[jid, 0]))
            joint_hi.append(float(self.model.jnt_range[jid, 1]))

        self.qpos_addr_hw = np.asarray(qpos_addr, dtype=np.int32)
        self.qvel_addr_hw = np.asarray(qvel_addr, dtype=np.int32)
        self.actuator_id_hw = np.asarray(actuator_id, dtype=np.int32)
        if len(set(self.actuator_id_hw.tolist())) != H2_DOF:
            raise RuntimeError("H2 HW-to-MuJoCo actuator mapping is not bijective")
        self.joint_lo_hw = np.asarray(joint_lo, dtype=np.float64)
        self.joint_hi_hw = np.asarray(joint_hi, dtype=np.float64)
        self.torque_lo_hw = self.model.actuator_ctrlrange[self.actuator_id_hw, 0].astype(np.float64)
        self.torque_hi_hw = self.model.actuator_ctrlrange[self.actuator_id_hw, 1].astype(np.float64)
        if not np.all(self.torque_lo_hw < 0.0) or not np.all(self.torque_hi_hw > 0.0):
            raise RuntimeError("H2 actuator torque ranges must straddle zero")

        if not keep_mjcf_joint_dynamics:
            self.model.dof_armature[self.qvel_addr_hw] = np.asarray(
                [_h2_armature(name) for name in HARDWARE_JOINT_NAMES], dtype=np.float64
            )
            self.model.dof_damping[self.qvel_addr_hw] = 0.0
            self.model.dof_frictionloss[self.qvel_addr_hw] = 0.0

        with np.load(str(ready_pose_path), allow_pickle=True) as ready:
            ready_names = [str(value) for value in np.asarray(ready["joint_names"]).ravel()]
            ready_values = np.asarray(ready["joint_pos"], dtype=np.float64)[0]
            ready_by_name = dict(zip(ready_names, ready_values))
            missing = [name for name in HARDWARE_JOINT_NAMES if name not in ready_by_name]
            if missing:
                raise RuntimeError(f"ready pose is missing H2 joints: {missing}")
            self.ready_q_hw = np.asarray(
                [ready_by_name[name] for name in HARDWARE_JOINT_NAMES], dtype=np.float64
            )
            body_names = [str(value) for value in np.asarray(ready["body_names"]).ravel()]
            pelvis_index = body_names.index("pelvis")
            root_pos = np.asarray(ready["body_pos_w"], dtype=np.float64)[0, pelvis_index]
            root_quat = np.asarray(ready["body_quat_w"], dtype=np.float64)[0, pelvis_index]

        self.data.qpos[:] = 0.0
        self.data.qvel[:] = 0.0
        self.data.qpos[:3] = root_pos
        self.data.qpos[3:7] = root_quat
        self.data.qpos[self.qpos_addr_hw] = self.ready_q_hw
        self.initial_root_pos = root_pos.copy()
        self.initial_root_quat = root_quat.copy()
        mujoco.mj_forward(self.model, self.data)
        self.pelvis_id = int(mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "pelvis"))
        if self.pelvis_id < 0:
            raise RuntimeError("H2 MJCF has no pelvis body")
        self.paddle_center_site_id = int(
            mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "paddle_center")
        )
        if self.paddle_center_site_id < 0:
            raise RuntimeError("H2 MJCF has no paddle_center site")
        h2_spec = load_embodiment("h2")
        self.virtual_paddle_body_id = int(
            mujoco.mj_name2id(
                self.model,
                mujoco.mjtObj.mjOBJ_BODY,
                h2_spec.paddle_body_name,
            )
        )
        if self.virtual_paddle_body_id < 0:
            raise RuntimeError(
                f"H2 MJCF has no virtual-paddle body {h2_spec.paddle_body_name!r}"
            )
        self.virtual_paddle_body_name = h2_spec.paddle_body_name
        self.virtual_paddle_offset_xyz = np.asarray(
            h2_spec.paddle_offset_xyz, dtype=np.float64
        )
        self.virtual_paddle_offset_rpy = np.asarray(
            h2_spec.paddle_offset_rpy, dtype=np.float64
        )
        self.virtual_paddle_half_width = float(h2_spec.paddle_half_width)
        self.virtual_paddle_half_height = float(h2_spec.paddle_half_height)
        self.stroke_split_relative_y = float(h2_spec.stroke_split_relative_y)
        self.mocap_ids = {}
        for name in ("tt_ball", "tt_target_marker", "tt_landing_marker"):
            bid = int(mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, name))
            self.mocap_ids[name] = int(self.model.body_mocapid[bid]) if bid >= 0 else -1
        self.last_tau_hw = np.zeros(H2_DOF, dtype=np.float64)
        self.last_target_hw = self.ready_q_hw.copy()
        self.last_marker_positions: dict[str, np.ndarray] = {}

    def state(self) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
        q = self.data.qpos[self.qpos_addr_hw].astype(np.float64).copy()
        dq = self.data.qvel[self.qvel_addr_hw].astype(np.float64).copy()
        quat = self.data.qpos[3:7].astype(np.float64).copy()
        norm = float(np.linalg.norm(quat))
        if norm > 0.0 and np.isfinite(norm):
            quat /= norm
        # MuJoCo free-joint angular qvel is the body-frame angular velocity used
        # by the H2 training/sim2sim observation.  mjOBJ_BODY local velocity is
        # expressed through the inertial-body frame and can also lag after the
        # explicit support reset, which changes the policy input materially.
        gyro_body = self.data.qvel[3:6].astype(np.float64).copy()
        return q, dq, quat, gyro_body

    def apply(
        self,
        command: SimCommand | None,
        *,
        hold_q: np.ndarray | None = None,
        root_supported: bool = False,
    ) -> np.ndarray:
        q, dq, _, _ = self.state()
        if command is None:
            tau = np.zeros(H2_DOF, dtype=np.float64)
        else:
            target = command.q[:H2_DOF].astype(np.float64)
            if hold_q is not None:
                target = np.asarray(hold_q, dtype=np.float64).reshape(H2_DOF)
            tau = (
                command.tau[:H2_DOF]
                + command.kp[:H2_DOF] * (target - q)
                + command.kd[:H2_DOF] * (command.dq[:H2_DOF] - dq)
            )
            self.last_target_hw = target.copy()
        tau = np.clip(tau, self.torque_lo_hw, self.torque_hi_hw)
        self.data.ctrl[:] = 0.0
        self.data.ctrl[self.actuator_id_hw] = tau
        self.last_tau_hw = tau.copy()
        self.mujoco.mj_step(self.model, self.data)
        if root_supported:
            # Explicit startup harness: the real commissioning procedure also
            # requires suspension for the first map/control validation.  The
            # support is removed before the canonical strike.
            self.data.qpos[:3] = self.initial_root_pos
            self.data.qpos[3:7] = self.initial_root_quat
            self.data.qvel[:6] = 0.0
            self.mujoco.mj_forward(self.model, self.data)
        return tau

    def update_markers(self, *, elapsed_s: float, scenario: SimScenario) -> None:
        hidden = np.array([0.0, 0.0, -1.0], dtype=np.float64)
        deadline = scenario.command_at_s + scenario.time_to_hit_s
        if elapsed_s < scenario.command_at_s:
            ball = hidden
        elif elapsed_s <= deadline:
            remaining = deadline - elapsed_s
            ball = np.asarray(scenario.hit_position) - np.asarray(scenario.incoming_velocity) * remaining
        else:
            flight = min(1.0, (elapsed_s - deadline) / 0.65)
            landing = np.array([scenario.landing_xy[0], scenario.landing_xy[1], 0.78])
            ball = (1.0 - flight) * np.asarray(scenario.hit_position) + flight * landing
            ball[2] += 0.45 * 4.0 * flight * (1.0 - flight)
        marker_positions = {
            "tt_ball": ball,
            "tt_target_marker": np.asarray(scenario.hit_position),
            "tt_landing_marker": np.array([scenario.landing_xy[0], scenario.landing_xy[1], 0.765]),
        }
        for name, position in marker_positions.items():
            mocap_id = self.mocap_ids.get(name, -1)
            if mocap_id >= 0:
                self.data.mocap_pos[mocap_id] = position
            self.last_marker_positions[name] = np.asarray(position, dtype=np.float64).copy()

    def update_markers_from_planner(
        self,
        packet: np.ndarray,
        *,
        ready: bool,
        ball_position_robot_m: np.ndarray | None,
    ) -> None:
        """Render planner targets plus the independent hybrid-ball position.
        Planner-v3 intentionally has no instantaneous ball position.  The ball
        marker therefore comes only from the adapter's real ZMQ observation or
        the canonical post-contact kinematic simulation; missing ball data is
        rendered as hidden rather than reconstructed from TTH.
        """

        hidden = np.array([0.0, 0.0, -1.0], dtype=np.float64)
        values = np.asarray(packet, dtype=np.float64).reshape(-1)
        active = bool(
            ready
            and values.size >= H2_PLANNER_PACKET_LEN
            and np.isfinite(values[:H2_PLANNER_PACKET_LEN]).all()
            and values[14] > 0.0
            and values[7] > 0.0
        )
        if active:
            hit = values[8:11].copy()
            landing = np.array([values[18], values[19], 0.765], dtype=np.float64)
        else:
            hit = hidden
            landing = hidden
        if ball_position_robot_m is None:
            ball = hidden
        else:
            ball = np.asarray(ball_position_robot_m, dtype=np.float64)
            if ball.shape != (3,) or not np.isfinite(ball).all():
                raise ValueError("hybrid ball marker must contain three finite values")
        marker_positions = {
            "tt_ball": ball,
            "tt_target_marker": hit,
            "tt_landing_marker": landing,
        }
        for name, position in marker_positions.items():
            mocap_id = self.mocap_ids.get(name, -1)
            if mocap_id >= 0:
                self.data.mocap_pos[mocap_id] = position
            self.last_marker_positions[name] = np.asarray(position, dtype=np.float64).copy()

    def paddle_center_world(self) -> np.ndarray:
        return self.data.site_xpos[self.paddle_center_site_id].astype(np.float64).copy()

    def virtual_paddle_state(self) -> ball_model.PaddleState:
        """Return the exact training virtual paddle, not the MJCF display site."""

        # ``mj_step`` integrates qpos/qvel after computing derived body
        # velocities.  Refresh them before sampling a new 120 Hz ball frame so
        # contact uses the current wrist state rather than the preceding step.
        self.mujoco.mj_forward(self.model, self.data)
        velocity = np.zeros(6, dtype=np.float64)
        self.mujoco.mj_objectVelocity(
            self.model,
            self.data,
            self.mujoco.mjtObj.mjOBJ_BODY,
            self.virtual_paddle_body_id,
            velocity,
            0,
        )
        return ball_model.virtual_paddle(
            self.data.xpos[self.virtual_paddle_body_id].astype(np.float64),
            self.data.xquat[self.virtual_paddle_body_id].astype(np.float64),
            velocity[3:6],
            velocity[:3],
            self.virtual_paddle_offset_xyz,
            self.virtual_paddle_offset_rpy,
        )


class SyntheticPlanner:
    """Deterministic v3 producer with independent receipt and source clocks."""

    def __init__(self, scenario: SimScenario):
        self.scenario = scenario
        now = time.monotonic()
        self.robot_source_stamp = now
        self.ball_source_stamp = now
        self.next_robot_sample = now
        self.next_ball_sample = now
        self.cached_root = np.zeros(3, dtype=np.float64)
        self.cached_quat_xyzw = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float64)

    def packet(self, *, now: float, elapsed_s: float, root_local: np.ndarray, quat_wxyz: np.ndarray) -> np.ndarray:
        fault_active = self.scenario.fault_active(elapsed_s)
        robot_stale = fault_active and self.scenario.fault_kind == "robot_source_dropout"
        if now >= self.next_robot_sample and not robot_stale:
            self.cached_root = np.asarray(root_local, dtype=np.float64).copy()
            self.cached_quat_xyzw = np.asarray(quat_wxyz, dtype=np.float64)[[1, 2, 3, 0]]
            self.robot_source_stamp = now
            self.next_robot_sample = now + 1.0 / MOCAP_HZ
        if now >= self.next_ball_sample:
            self.ball_source_stamp = now
            self.next_ball_sample = now + 1.0 / MOCAP_HZ

        origin = np.asarray(self.scenario.robot_base_origin_world, dtype=np.float64).copy()
        root = self.cached_root.copy()
        quat = self.cached_quat_xyzw.copy()
        if fault_active and self.scenario.fault_kind == "planner_origin_mismatch":
            origin[0] += 0.50
        if fault_active and self.scenario.fault_kind == "root_jump":
            root[1] += 1.0
        if fault_active and self.scenario.fault_kind == "heading_jump":
            yaw = 1.2
            quat = np.array([0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0)])

        command_id = 1 if elapsed_s >= self.scenario.command_at_s else 0
        if fault_active and self.scenario.fault_kind == "command_id_rollback":
            command_id = 0
        deadline = self.scenario.command_at_s + self.scenario.time_to_hit_s
        tth = max(-0.59, deadline - elapsed_s) if command_id > 0 else -0.59
        packet = build_h2_planner_packet(
            robot_pos_world=origin + root,
            robot_quat_xyzw=quat,
            time_to_hit_s=tth,
            hit_pos_robot=self.scenario.hit_position,
            racket_velocity_robot=self.scenario.racket_velocity,
            command_id=command_id,
            incoming_velocity_robot=self.scenario.incoming_velocity,
            landing_xy_robot=self.scenario.landing_xy,
            robot_source_monotonic_s=self.robot_source_stamp,
            ball_source_monotonic_s=self.ball_source_stamp,
            robot_base_origin_world=origin,
        )
        if fault_active and self.scenario.fault_kind == "planner_protocol_mismatch":
            packet[PROTOCOL_VERSION_INDEX] = 99.0
        return packet


class SimMujocoBridge:
    def __init__(
        self,
        scenario: SimScenario,
        *,
        planner=None,
        live_mode: bool = False,
        trace_hz: float = TRACE_HZ,
        trace_max_bytes: int | None = None,
        live_status_path: str = "",
        live_ball_source=None,
        live_ball_controller=None,
        live_ball_endpoint: str = "",
    ):
        scenario.validate()
        self.scenario = scenario
        self.owner = SimH2BridgeOwner(scenario.shm_name)
        try:
            self.plant = H2MujocoPlant(
                scenario.mjcf_path,
                scenario.ready_pose_path,
                physics_dt=scenario.physics_dt,
                keep_mjcf_joint_dynamics=scenario.keep_mjcf_joint_dynamics,
            )
        except BaseException:
            self.owner.close()
            raise
        self.planner = SyntheticPlanner(scenario) if planner is None else planner
        self.live_mode = bool(live_mode)
        if self.live_mode and (live_ball_source is None or live_ball_controller is None):
            self.owner.close()
            raise ValueError("live MuJoCo requires both ZMQ ball source and hybrid controller")
        if not self.live_mode and (live_ball_source is not None or live_ball_controller is not None):
            self.owner.close()
            raise ValueError("hybrid ball input is available only in live MuJoCo mode")
        self.live_ball_source = live_ball_source
        self.live_ball_controller = live_ball_controller
        self.live_ball_endpoint = str(live_ball_endpoint)
        self.live_ball_snapshot = (
            live_ball_controller.snapshot() if self.live_mode else None
        )
        self._live_forehand = True
        self._live_ball_last_wall_s: float | None = None
        self.trace_hz = float(trace_hz)
        self.trace_max_bytes = (
            None if trace_max_bytes is None else int(trace_max_bytes)
        )
        self.live_status_path = str(live_status_path)
        if not np.isfinite(self.trace_hz) or self.trace_hz < 0.0 or self.trace_hz > TRACE_HZ:
            raise ValueError(f"trace_hz must be finite and in [0,{TRACE_HZ:.0f}]")
        if self.trace_max_bytes is not None and self.trace_max_bytes < 4096:
            raise ValueError("trace_max_bytes must be at least 4096 when enabled")
        self.fault_latched = False
        self.fault_reason = ""
        self.fault_wall_s: float | None = None
        self.output_inhibited = False
        self.hold_q: np.ndarray | None = None
        self.last_valid_command: SimCommand | None = None
        self.first_command_s: float | None = None
        self.state_updates = 0
        self.command_updates = 0
        self.actuation_steps = 0
        self.max_abs_tau = 0.0
        self.max_torque_utilization = 0.0
        self.max_abs_joint_velocity = 0.0
        self.min_root_z = float(self.plant.data.qpos[2])
        self.max_tilt_rad = 0.0
        self.max_loop_lag_s = 0.0
        self._last_command_stamp = 0
        self._last_state_publish_wall = time.monotonic()
        self._fault_injected = False
        self._handover_offset_injected = False
        self.startup_support_released_s: float | None = None
        self.command_target_clamps = 0
        self.max_unconstrained_effort_utilization = 0.0
        self.max_target_projection_rad = 0.0
        self.target_projection_ticks = 0
        self.plant_metric_samples = 0
        self.hit_window_samples = 0
        self.min_paddle_to_ball_distance_m = float("inf")
        self.min_paddle_to_ball_elapsed_s: float | None = None
        self.min_paddle_to_hit_point_distance_m = float("inf")
        self.min_paddle_to_hit_point_elapsed_s: float | None = None
        self.last_paddle_center = self.plant.paddle_center_world()
        self.last_ball_position = np.array([0.0, 0.0, -1.0], dtype=np.float64)

    def _live_planner_context(
        self, packet: np.ndarray, *, planner_ready: bool
    ) -> tuple[bool, int]:
        """Resolve stroke side and command id using the deployed planner packet."""

        values = np.asarray(packet, dtype=np.float64).reshape(-1)
        command_id = 0
        if (
            planner_ready
            and values.size >= H2_PLANNER_PACKET_LEN
            and np.isfinite(values[:H2_PLANNER_PACKET_LEN]).all()
        ):
            raw_command_id = float(values[14])
            rounded_command_id = int(round(raw_command_id))
            if rounded_command_id >= 0 and abs(raw_command_id - rounded_command_id) <= 1.0e-6:
                command_id = rounded_command_id
            if command_id > 0:
                pelvis = self.plant.data.qpos[:3].astype(np.float64)
                relative_hit = values[8:11] - pelvis
                yaw = yaw_of(self.plant.data.qpos[3:7])
                lateral = (
                    -math.sin(yaw) * float(relative_hit[0])
                    + math.cos(yaw) * float(relative_hit[1])
                )
                self._live_forehand = bool(
                    lateral <= self.plant.stroke_split_relative_y
                )
        return self._live_forehand, command_id

    def _update_live_hybrid_ball(
        self,
        packet: np.ndarray,
        *,
        planner_ready: bool,
        wall_now_s: float,
    ) -> None:
        """Poll one latest real frame, then render the current hybrid state."""

        if not self.live_mode:
            raise RuntimeError("hybrid ball update is live-mode only")
        previous_wall_s = self._live_ball_last_wall_s
        elapsed_s = 0.0 if previous_wall_s is None else max(0.0, wall_now_s - previous_wall_s)
        self._live_ball_last_wall_s = float(wall_now_s)
        snapshot = self.live_ball_controller.advance_simulation(elapsed_s)
        if snapshot.landed:
            landing_xy = self.live_ball_controller.ball.landing_xy
            print(
                "[HYBRID BALL LANDING] "
                f"xy_m={None if landing_xy is None else landing_xy.tolist()}",
                flush=True,
            )
        sample = self.live_ball_source.poll_latest()
        if sample is not None:
            forehand, command_id = self._live_planner_context(
                packet, planner_ready=planner_ready
            )
            snapshot = self.live_ball_controller.observe_real_sample(
                sample,
                paddle=self.plant.virtual_paddle_state(),
                forehand=forehand,
                command_id=command_id,
            )
            if snapshot.contact:
                velocity = np.asarray(snapshot.marker_velocity_mps, dtype=np.float64)
                print(
                    "[HYBRID BALL CONTACT] real->sim "
                    f"command_id={command_id} outgoing_mps={velocity.tolist()}",
                    flush=True,
                )
            elif snapshot.new_rally:
                print(
                    f"[HYBRID BALL NEW RALLY] source_stamp={sample.source_stamp_s:.6f}",
                    flush=True,
                )
        self.live_ball_snapshot = snapshot
        self.plant.update_markers_from_planner(
            packet,
            ready=planner_ready,
            ball_position_robot_m=snapshot.marker_position_robot_m,
        )

    def _hybrid_ball_telemetry(self) -> dict[str, object]:
        if not self.live_mode or self.live_ball_snapshot is None:
            return {}
        snapshot = self.live_ball_snapshot
        receipt_age_s = (
            None
            if snapshot.source_receipt_monotonic_ns <= 0
            else max(
                0.0,
                (time.monotonic_ns() - snapshot.source_receipt_monotonic_ns) * 1.0e-9,
            )
        )
        landing_xy = self.live_ball_controller.ball.landing_xy
        outgoing_velocity = self.live_ball_controller.ball.outgoing_vel
        return {
            "hybrid_ball_mode": snapshot.mode,
            "hybrid_ball_phase": snapshot.phase,
            "hybrid_ball_velocity_source": snapshot.velocity_source,
            "hybrid_ball_source_stamp_s": snapshot.source_stamp_s,
            "hybrid_ball_source_age_s": receipt_age_s,
            "hybrid_ball_contact_count": snapshot.contact_count,
            "hybrid_ball_simulated_steps": snapshot.simulated_steps,
            "hybrid_ball_marker_velocity_mps": (
                None
                if snapshot.marker_velocity_mps is None
                else snapshot.marker_velocity_mps.tolist()
            ),
            "hybrid_ball_outgoing_velocity_mps": (
                outgoing_velocity.tolist()
                if snapshot.contact_count > 0
                else None
            ),
            "hybrid_ball_landing_xy": (
                None if landing_xy is None else landing_xy.tolist()
            ),
        }

    def _latch_fault(self, reason: str, elapsed_s: float, *, inhibit: bool = False) -> None:
        if not self.fault_latched:
            self.fault_latched = True
            self.fault_reason = str(reason)
            self.fault_wall_s = float(elapsed_s)
            q, _, _, _ = self.plant.state()
            self.hold_q = q
            # Production LatchFault synchronizes the emergency flag even when
            # the triggering LowState is rejected and never copied into SHM.
            self.owner.mark_emergency()
        self.output_inhibited = self.output_inhibited or bool(inhibit)

    def _latched_hold_command(self, basis: SimCommand) -> SimCommand:
        """Recreate the production C++ captured-pose fault hold.
        A non-inhibiting fault keeps publishing a hold captured at the first
        fault edge.  The hold target is a mechanical position (unlike a learned
        virtual PD target), and its gains are only reduced as needed to reserve
        the same 90 percent effort envelope as ``LatchedHold`` in the C++
        bridge.  The returned command is still projected from the latest state
        immediately before each simulated actuator tick.
        """

        q, dq, _, _ = self.plant.state()
        captured = q if self.hold_q is None else np.asarray(self.hold_q, dtype=np.float64)
        hold_lo, hold_hi = command_position_bounds_hw(
            self.plant.joint_lo_hw,
            self.plant.joint_hi_hw,
        )
        target = np.zeros(LOWCMD_DOF, dtype=np.float32)
        target[:H2_DOF] = np.clip(captured, hold_lo, hold_hi).astype(np.float32)
        desired_velocity = np.zeros(LOWCMD_DOF, dtype=np.float32)
        feedforward = np.zeros(LOWCMD_DOF, dtype=np.float32)
        kp = np.asarray(basis.kp, dtype=np.float32).copy()
        kd = np.asarray(basis.kd, dtype=np.float32).copy()
        torque_budget = HOLD_TORQUE_FRACTION * np.minimum(
            np.abs(self.plant.torque_lo_hw),
            np.abs(self.plant.torque_hi_hw),
        )
        for index in range(H2_DOF):
            position_error = float(target[index]) - float(q[index])
            abs_error = abs(position_error)
            if abs_error > 1.0e-6 and float(kp[index]) * abs_error > torque_budget[index]:
                kp[index] = np.float32(torque_budget[index] / abs_error)
            position_effort = abs(float(kp[index]) * position_error)
            damping_budget = max(0.0, float(torque_budget[index]) - position_effort)
            abs_velocity = abs(float(dq[index]))
            if (
                abs_velocity > 1.0e-6
                and float(kd[index]) * abs_velocity > damping_budget
            ):
                kd[index] = np.float32(damping_budget / abs_velocity)
        kp[H2_DOF:] = 0.0
        kd[H2_DOF:] = 0.0
        mode = np.zeros(LOWCMD_DOF, dtype=np.uint8)
        mode[:H2_DOF] = 1
        return SimCommand(
            stamp_ns=basis.stamp_ns,
            enabled=basis.enabled,
            flags=basis.flags,
            mode_pr=MODE_PR,
            q=target,
            dq=desired_velocity,
            tau=feedforward,
            kp=kp,
            kd=kd,
            mode=mode,
            reserve=np.zeros(LOWCMD_DOF, dtype=np.uint32),
        )

    def _validate_command(self, command: SimCommand, now_ns: int) -> str:
        required_flags = SIMULATOR_OWNER_FLAG | SIMULATION_RUNNER_ATTESTED_FLAG
        if command.flags & required_flags != required_flags:
            return "simulation runner attestation is missing"
        if command.flags & MOTION_SWITCHER_ATTESTED_FLAG:
            return "simulator received the real MotionSwitcher-attestation bit"
        if command.flags & PYTHON_FAULT_REQUEST_FLAG:
            return "Python runner requested the permanent fault latch"
        if not command.enabled:
            return ""
        age_s = (now_ns - command.stamp_ns) * 1.0e-9
        if command.stamp_ns <= 0 or age_s < -0.01:
            return "command timestamp is invalid"
        if age_s > COMMAND_WATCHDOG_S:
            return f"command watchdog expired ({age_s:.3f}s)"
        arrays = np.concatenate([command.q, command.dq, command.tau, command.kp, command.kd])
        if not np.isfinite(arrays).all():
            return "command contains NaN/Inf"
        if command.mode_pr != MODE_PR:
            return f"command mode_pr={command.mode_pr}, expected PR(0)"
        if np.any(command.kp < 0.0) or np.any(command.kp > MAX_KP):
            return "command Kp is outside the ABI bounds"
        if np.any(command.kd < 0.0) or np.any(command.kd > MAX_KD):
            return "command Kd is outside the ABI bounds"
        if np.any(command.mode[:H2_DOF] != 1) or np.any(command.mode[H2_DOF:] != 0):
            return "command motor-mode slots are invalid"
        if np.any(command.reserve != 0):
            return "command reserve slots are non-zero"
        if any(
            np.any(arr[H2_DOF:] != 0.0)
            for arr in (command.q, command.dq, command.tau, command.kp, command.kd)
        ):
            return "inactive H2 slots 31..34 are non-zero"
        if np.any(command.q[29:31] != 0.0):
            return "raw learned head targets must be exactly zero"
        return ""

    def _constrain_for_latest_state(self, command: SimCommand) -> tuple[SimCommand | None, str]:
        """Project only actuator effort from the immutable raw virtual target.
        This is the H2 equivalent of the working G1/sim2sim actuator path:
        compute PD effort from the newest state, saturate that effort at the
        actuator ``ctrlrange``, then encode the saturated result as a local
        command for this tick.  A learned ``q_des`` is therefore deliberately
        not intersected with a mechanical joint-target interval.  In
        particular, an exact-zero raw head target may project away from zero
        when compensating velocity would otherwise exceed the torque limit.
        """

        q, dq, _, _ = self.plant.state()
        # Keep the v7 raw learned target immutable.  The next 1000 Hz tick must
        # start from it again and use that tick's newest measured q/dq.
        target = command.q.copy().astype(np.float64)
        velocity_offset = command.kd[:H2_DOF] * (command.dq[:H2_DOF] - dq)
        raw_effort = (
            command.tau[:H2_DOF]
            + command.kp[:H2_DOF] * (target[:H2_DOF] - q)
            + velocity_offset
        )
        torque_scale = np.maximum(
            np.abs(self.plant.torque_lo_hw), np.abs(self.plant.torque_hi_hw)
        )
        self.max_unconstrained_effort_utilization = max(
            self.max_unconstrained_effort_utilization,
            float(np.max(np.abs(raw_effort) / torque_scale)),
        )
        safe_effort = np.clip(
            raw_effort,
            self.plant.torque_lo_hw,
            self.plant.torque_hi_hw,
        )
        saturated = np.abs(safe_effort - raw_effort) > 1.0e-7
        zero_kp = command.kp[:H2_DOF] <= 1.0e-8
        offset = command.tau[:H2_DOF] + velocity_offset
        zero_kp_infeasible = zero_kp & (
            (offset < self.plant.torque_lo_hw - 1.0e-3)
            | (offset > self.plant.torque_hi_hw + 1.0e-3)
        )
        if np.any(zero_kp_infeasible):
            index = int(np.flatnonzero(zero_kp_infeasible)[0])
            return None, f"zero-Kp effort is infeasible for {HARDWARE_JOINT_NAMES[index]}"
        projected = saturated & ~zero_kp
        for index in np.flatnonzero(projected):
            kp = float(command.kp[index])
            target[index] = q[index] + (
                safe_effort[index]
                - float(command.tau[index])
                - float(velocity_offset[index])
            ) / kp
        self.command_target_clamps += int(np.count_nonzero(projected))
        projection = float(
            np.max(np.abs(target[:H2_DOF] - np.asarray(command.q[:H2_DOF], dtype=np.float64)))
        )
        self.max_target_projection_rad = max(self.max_target_projection_rad, projection)
        if np.any(projected):
            self.target_projection_ticks += 1
        return SimCommand(
            stamp_ns=command.stamp_ns,
            enabled=command.enabled,
            flags=command.flags,
            mode_pr=command.mode_pr,
            q=target.astype(np.float32),
            dq=command.dq,
            tau=command.tau,
            kp=command.kp,
            kd=command.kd,
            mode=command.mode,
            reserve=command.reserve,
        ), ""

    def _observe_and_validate_plant_state(
        self,
        q: np.ndarray,
        dq: np.ndarray,
        quat_wxyz: np.ndarray,
        gyro: np.ndarray,
    ) -> str:
        """Update physics-rate peaks and mirror production LowStateFinite gates."""

        self.plant_metric_samples += 1
        root_z = float(self.plant.data.qpos[2])
        if math.isfinite(root_z):
            self.min_root_z = min(self.min_root_z, root_z)
        if np.isfinite(dq).all():
            self.max_abs_joint_velocity = max(
                self.max_abs_joint_velocity, float(np.max(np.abs(dq)))
            )
        raw_quat = self.plant.data.qpos[3:7].astype(np.float64).copy()
        values = np.concatenate([q, dq, raw_quat, gyro, [root_z]])
        if not np.isfinite(values).all():
            return "simulated LowState contains NaN/Inf"
        quat_norm = float(np.linalg.norm(raw_quat))
        if not LOWSTATE_MIN_QUATERNION_NORM <= quat_norm <= LOWSTATE_MAX_QUATERNION_NORM:
            return f"simulated LowState quaternion norm {quat_norm:.3f} is invalid"
        up_z = 1.0 - 2.0 * (
            float(quat_wxyz[1]) * float(quat_wxyz[1])
            + float(quat_wxyz[2]) * float(quat_wxyz[2])
        )
        tilt = float(math.acos(float(np.clip(up_z, -1.0, 1.0))))
        self.max_tilt_rad = max(self.max_tilt_rad, tilt)
        bad_position = np.flatnonzero(
            (q < self.plant.joint_lo_hw - STATE_JOINT_LIMIT_TOLERANCE_RAD)
            | (q > self.plant.joint_hi_hw + STATE_JOINT_LIMIT_TOLERANCE_RAD)
        )
        if bad_position.size:
            index = int(bad_position[0])
            return (
                "simulated LowState joint position left production domain: "
                f"{HARDWARE_JOINT_NAMES[index]}={q[index]:.4f}rad"
            )
        bad_velocity = np.flatnonzero(np.abs(dq) > STATE_MAX_JOINT_VELOCITY_RAD_S)
        if bad_velocity.size:
            index = int(bad_velocity[0])
            return (
                "simulated LowState joint velocity left production domain: "
                f"{HARDWARE_JOINT_NAMES[index]}={dq[index]:.3f}rad/s"
            )
        return ""

    def _observe_hit_reach(self, elapsed_s: float) -> None:
        deadline = self.scenario.command_at_s + self.scenario.time_to_hit_s
        if abs(float(elapsed_s) - deadline) > HIT_REACH_WINDOW_HALF_WIDTH_S:
            return
        paddle = self.plant.paddle_center_world()
        ball = self.plant.last_marker_positions.get("tt_ball")
        hit_point = np.asarray(self.scenario.hit_position, dtype=np.float64)
        if ball is None or not np.isfinite(np.concatenate([paddle, ball, hit_point])).all():
            return
        self.hit_window_samples += 1
        self.last_paddle_center = paddle.copy()
        self.last_ball_position = ball.copy()
        ball_distance = float(np.linalg.norm(paddle - ball))
        hit_distance = float(np.linalg.norm(paddle - hit_point))
        if ball_distance < self.min_paddle_to_ball_distance_m:
            self.min_paddle_to_ball_distance_m = ball_distance
            self.min_paddle_to_ball_elapsed_s = float(elapsed_s)
        if hit_distance < self.min_paddle_to_hit_point_distance_m:
            self.min_paddle_to_hit_point_distance_m = hit_distance
            self.min_paddle_to_hit_point_elapsed_s = float(elapsed_s)

    def _telemetry(self, elapsed_s: float, command: SimCommand | None) -> dict[str, object]:
        q, dq, quat, _ = self.plant.state()
        up_z = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
        tilt = float(math.acos(float(np.clip(up_z, -1.0, 1.0))))
        paddle = self.plant.paddle_center_world()
        ball = self.plant.last_marker_positions.get("tt_ball", self.last_ball_position)
        telemetry = {
            "elapsed_s": round(float(elapsed_s), 6),
            "q_hw": q.tolist(),
            "dq_hw": dq.tolist(),
            "q_target_hw": self.plant.last_target_hw.tolist(),
            "q_desired_hw": (
                np.asarray(command.q[:H2_DOF], dtype=np.float64).tolist()
                if command is not None and command.enabled
                else self.plant.last_target_hw.tolist()
            ),
            "tau_hw": self.plant.last_tau_hw.tolist(),
            "pelvis_pos": self.plant.data.qpos[:3].astype(float).tolist(),
            "pelvis_quat_wxyz": quat.tolist(),
            "tilt_rad": tilt,
            "paddle_center_world": paddle.tolist(),
            "ball_mocap_world": np.asarray(ball, dtype=np.float64).tolist(),
            "paddle_to_ball_distance_m": float(np.linalg.norm(paddle - ball)),
            "paddle_to_hit_point_distance_m": float(
                np.linalg.norm(paddle - np.asarray(self.scenario.hit_position, dtype=np.float64))
            ),
            "command_stamp_ns": int(command.stamp_ns) if command is not None else 0,
            "fault_latched": self.fault_latched,
            "fault_reason": self.fault_reason,
        }
        telemetry.update(self._hybrid_ball_telemetry())
        return telemetry

    def _planner_packet(
        self,
        *,
        now: float,
        elapsed_s: float,
        quat_wxyz: np.ndarray,
    ) -> tuple[np.ndarray, bool, bool, int | None]:
        packet = self.planner.packet(
            now=now,
            elapsed_s=elapsed_s,
            root_local=self.plant.data.qpos[:3],
            quat_wxyz=quat_wxyz,
        )
        if not self.live_mode:
            return np.asarray(packet, dtype=np.float64), True, True, None
        return (
            np.asarray(packet, dtype=np.float64),
            bool(getattr(self.planner, "last_packet_was_updated", False)),
            bool(getattr(self.planner, "last_packet_ready", False)),
            int(getattr(self.planner, "last_receipt_stamp_ns", 0)) or None,
        )

    @staticmethod
    def _rotate_trace(path: Path, trace_file, max_bytes: int | None):
        if max_bytes is None or trace_file.tell() < max_bytes:
            return trace_file
        trace_file.close()
        oldest = path.with_name(path.name + ".3")
        try:
            oldest.unlink()
        except FileNotFoundError:
            pass
        for source_suffix, destination_suffix in ((".2", ".3"), (".1", ".2"), ("", ".1")):
            source = path.with_name(path.name + source_suffix)
            destination = path.with_name(path.name + destination_suffix)
            try:
                source.replace(destination)
            except FileNotFoundError:
                pass
        return path.open("w", encoding="utf-8")

    def _write_live_status(
        self,
        *,
        elapsed_s: float,
        command: SimCommand | None,
        planner_ready: bool,
        start_requested: bool,
        stop_requested: bool,
    ) -> None:
        if not self.live_status_path:
            return
        status = self._telemetry(elapsed_s, command)
        status.update(
            {
                "schema": "h2-live-mujoco-status-v1",
                "planner_ready": bool(planner_ready),
                "start_requested": bool(start_requested),
                "stop_requested": bool(stop_requested),
                "state_updates": self.state_updates,
                "command_updates": self.command_updates,
                "actuation_steps": self.actuation_steps,
                "max_torque_utilization": self.max_torque_utilization,
                "max_abs_joint_velocity_rad_s": self.max_abs_joint_velocity,
                "min_root_z_m": self.min_root_z,
                "max_tilt_rad": self.max_tilt_rad,
                "max_loop_lag_s": self.max_loop_lag_s,
            }
        )
        path = Path(self.live_status_path)
        temporary = path.with_name(f".{path.name}.{os.getpid()}.tmp")
        try:
            temporary.write_text(
                json.dumps(status, sort_keys=True) + "\n", encoding="utf-8"
            )
            os.replace(temporary, path)
        except OSError:
            try:
                temporary.unlink()
            except OSError:
                pass

    def run(
        self,
        stop_event,
        scenario_start_event,
        status_queue,
        *,
        start_request_event=None,
        stop_request_event=None,
        policy_active_event=None,
    ) -> dict[str, object]:
        view = None
        trace_file = None
        scenario_wall_start: float | None = None
        scenario_elapsed_s = 0.0
        tick = 0
        physics_accumulator = 0.0
        next_tick = time.perf_counter()
        try:
            trace_path = Path(self.scenario.trace_path) if self.scenario.trace_path else None
            if trace_path is not None and self.trace_hz > 0.0:
                trace_path.parent.mkdir(parents=True, exist_ok=True)
                trace_file = trace_path.open("w", encoding="utf-8")
            if self.scenario.viewer:
                import mujoco.viewer

                view = mujoco.viewer.launch_passive(self.plant.model, self.plant.data)
                # A close rear three-quarter view keeps both feet, ankles and
                # the upper body visible without letting the table hide a
                # balance failure.  Users can still orbit/zoom normally.
                view.cam.lookat[:] = self.plant.data.qpos[:3]
                view.cam.distance = 2.8
                view.cam.azimuth = 55.0
                view.cam.elevation = -10.0

            q, dq, quat, gyro = self.plant.state()
            initial_state_reason = self._observe_and_validate_plant_state(q, dq, quat, gyro)
            if initial_state_reason:
                raise RuntimeError(initial_state_reason)
            packet, update_planner, planner_ready, planner_stamp_ns = self._planner_packet(
                now=time.monotonic(), elapsed_s=0.0, quat_wxyz=quat,
            )
            if self.live_mode:
                self._update_live_hybrid_ball(
                    packet,
                    planner_ready=planner_ready,
                    wall_now_s=time.monotonic(),
                )
            else:
                self.plant.update_markers(elapsed_s=0.0, scenario=self.scenario)
            self.owner.write_state(
                q=q, dq=dq, quat_wxyz=quat, gyro=gyro,
                root_xy=self.plant.data.qpos[:2], quat_xyzw=quat[[1, 2, 3, 0]],
                planner_packet=packet, start_requested=False, stop_requested=False,
                emergency_stop=False, mode_pr=MODE_PR,
                update_planner=update_planner,
                planner_ready=planner_ready,
                planner_stamp_ns=planner_stamp_ns,
            )
            self.state_updates += 1
            ready_status = {
                "event": "ready",
                "shm_name": self.scenario.shm_name,
                "actuator_id_by_hw": self.plant.actuator_id_hw.tolist(),
                "qpos_addr_by_hw": self.plant.qpos_addr_hw.tolist(),
                "qvel_addr_by_hw": self.plant.qvel_addr_hw.tolist(),
            }
            if self.live_mode:
                ready_status["mocap_zmq_endpoint"] = self.live_ball_endpoint
            status_queue.put(ready_status)

            while not stop_event.is_set():
                wall_now = time.monotonic()
                if scenario_start_event.is_set() and scenario_wall_start is None:
                    scenario_wall_start = wall_now
                    # Artifact/model loading can keep the transport owner alive
                    # for seconds before the runner starts.  Do not inherit that
                    # arbitrary pre-scenario remainder: align the 200 Hz plant to
                    # the fifth 1000 Hz transport tick for reproducible real-loop
                    # simulation without changing either deployed clock.
                    physics_accumulator = 0.0
                elapsed_s = 0.0 if scenario_wall_start is None else wall_now - scenario_wall_start
                scenario_elapsed_s = float(elapsed_s)
                start_requested = bool(
                    self.live_mode
                    and start_request_event is not None
                    and start_request_event.is_set()
                )
                stop_requested = bool(
                    self.live_mode
                    and stop_request_event is not None
                    and stop_request_event.is_set()
                )
                if stop_requested and not self.fault_latched:
                    self._latch_fault("simulated R1 stop request", elapsed_s)
                fault_active = (
                    not self.live_mode
                    and
                    scenario_wall_start is not None
                    and self.scenario.fault_active(elapsed_s)
                )

                command = self.owner.read_command()
                if command.stamp_ns != self._last_command_stamp:
                    self.command_updates += 1
                    self._last_command_stamp = command.stamp_ns
                reason = self._validate_command(command, time.monotonic_ns())
                if reason:
                    if reason == "Python runner requested the permanent fault latch":
                        self._latch_fault(reason, elapsed_s)
                    elif command.enabled or self.last_valid_command is not None:
                        self._latch_fault(reason, elapsed_s)
                elif command.enabled:
                    first_valid_command = self.last_valid_command is None
                    self.last_valid_command = command
                    if first_valid_command:
                        # The plant is intentionally stationary during model
                        # loading.  Start its 200 Hz phase from the first valid
                        # 50 Hz command, not from an arbitrary transport tick
                        # that happened while the runner was still loading.
                        physics_accumulator = 0.0
                    if self.first_command_s is None and scenario_wall_start is not None:
                        self.first_command_s = elapsed_s

                if fault_active and not self._fault_injected:
                    self._fault_injected = True
                    if self.scenario.fault_kind == "invalid_lowstate":
                        self._latch_fault("injected invalid LowState", elapsed_s, inhibit=True)
                    elif self.scenario.fault_kind == "non_pr_mode":
                        self._latch_fault(
                            "LowState left PR(0) mode (mode_pr=1)", elapsed_s, inhibit=True
                        )
                    elif self.scenario.fault_kind == "motor_fault":
                        self._latch_fault("injected motor fault", elapsed_s, inhibit=True)

                state_age = wall_now - self._last_state_publish_wall
                if self.scenario.fault_kind == "state_dropout" and fault_active and state_age > COMMAND_WATCHDOG_S:
                    self._latch_fault("simulated LowState watchdog expired", elapsed_s, inhibit=True)

                root_supported = bool(
                    scenario_wall_start is not None
                    and not self.output_inhibited
                    and (
                        (
                            self.live_mode
                            and not (
                                policy_active_event is not None
                                and policy_active_event.is_set()
                            )
                        )
                        or (
                            not self.live_mode
                            and elapsed_s < self.scenario.startup_support_until_s
                        )
                    )
                )
                if (
                    scenario_wall_start is not None
                    and not root_supported
                    and self.startup_support_released_s is None
                ):
                    self.startup_support_released_s = elapsed_s
                physics_accumulator += BRIDGE_DT
                step_physics = physics_accumulator + 1.0e-12 >= self.scenario.physics_dt
                if step_physics:
                    physics_accumulator -= self.scenario.physics_dt
                if self.last_valid_command is None:
                    # Model an already-powered stationary robot while policy and
                    # CLoSD artifacts load. Physics begins on the first valid command.
                    self.plant.mujoco.mj_forward(self.plant.model, self.plant.data)
                elif not step_physics:
                    pass
                elif self.output_inhibited:
                    self.plant.apply(None, root_supported=False)
                else:
                    basis = self.last_valid_command
                    if self.fault_latched:
                        basis = self._latched_hold_command(basis)
                    constrained, constrain_reason = self._constrain_for_latest_state(basis)
                    if constrained is None:
                        # Match production: a projection failure latches and
                        # continues with the captured hold.  It must never turn
                        # a finite PR command into a zero-effort/free-fall tick.
                        self._latch_fault(constrain_reason, elapsed_s)
                        hold = self._latched_hold_command(self.last_valid_command)
                        self.plant.apply(hold, root_supported=root_supported)
                        self.actuation_steps += 1
                    else:
                        self.plant.apply(constrained, root_supported=root_supported)
                        self.actuation_steps += 1

                offsets = np.asarray(self.scenario.handover_joint_offsets_hw, dtype=np.float64)
                preload_start_s = (
                    self.scenario.arm_at_s - self.scenario.handover_offset_preload_s
                )
                if (
                    not self.live_mode
                    and scenario_wall_start is not None
                    and np.any(offsets != 0.0)
                    and preload_start_s <= elapsed_s < self.scenario.arm_at_s
                ):
                    # Model the tether/external load as a stationary constraint
                    # that establishes the measured gravity offsets before L1.
                    # Releasing this constraint at L1 avoids an instantaneous
                    # joint/contact teleport on the policy's first frame.
                    self.plant.data.qpos[self.plant.qpos_addr_hw] = (
                        self.plant.ready_q_hw + offsets
                    )
                    self.plant.data.qvel[self.plant.qvel_addr_hw] = 0.0
                    self.plant.mujoco.mj_forward(self.plant.model, self.plant.data)
                    self._handover_offset_injected = True

                q, dq, quat, gyro = self.plant.state()
                state_reason = self._observe_and_validate_plant_state(q, dq, quat, gyro)
                if state_reason:
                    self._latch_fault(state_reason, elapsed_s, inhibit=True)
                self.max_abs_tau = max(self.max_abs_tau, float(np.max(np.abs(self.plant.last_tau_hw))))
                limits = np.maximum(np.abs(self.plant.torque_lo_hw), np.abs(self.plant.torque_hi_hw))
                self.max_torque_utilization = max(
                    self.max_torque_utilization,
                    float(np.max(np.abs(self.plant.last_tau_hw) / limits)),
                )

                packet, planner_was_updated, planner_ready, planner_stamp_ns = self._planner_packet(
                    now=wall_now,
                    elapsed_s=elapsed_s,
                    quat_wxyz=quat,
                )
                if self.live_mode:
                    self._update_live_hybrid_ball(
                        packet,
                        planner_ready=planner_ready,
                        wall_now_s=wall_now,
                    )
                else:
                    self.plant.update_markers(elapsed_s=elapsed_s, scenario=self.scenario)
                if step_physics and not self.live_mode:
                    self._observe_hit_reach(elapsed_s)
                update_planner = planner_was_updated and not (
                    fault_active and self.scenario.fault_kind == "planner_receipt_dropout"
                )
                if not self.live_mode:
                    planner_ready = not (
                        fault_active and self.scenario.fault_kind == "planner_protocol_mismatch"
                    )
                emergency = self.fault_latched
                write_q = q.copy()
                if fault_active and self.scenario.fault_kind == "corrupt_shm_nan":
                    write_q[0] = np.nan
                reject_lowstate = bool(state_reason) or (
                    fault_active
                    and self.scenario.fault_kind in (
                        "state_dropout", "invalid_lowstate", "non_pr_mode",
                    )
                )
                if not reject_lowstate:
                    self.owner.write_state(
                        q=write_q,
                        dq=dq,
                        quat_wxyz=quat,
                        gyro=gyro,
                        root_xy=(packet[:2] if self.live_mode and planner_ready else self.plant.data.qpos[:2]),
                        quat_xyzw=(packet[3:7] if self.live_mode and planner_ready else quat[[1, 2, 3, 0]]),
                        planner_packet=packet,
                        start_requested=(
                            start_requested
                            if self.live_mode
                            else scenario_wall_start is not None and elapsed_s >= self.scenario.arm_at_s
                        ),
                        stop_requested=stop_requested,
                        emergency_stop=emergency,
                        mode_pr=MODE_PR,
                        update_state=True,
                        update_planner=update_planner,
                        planner_ready=planner_ready,
                        planner_stamp_ns=planner_stamp_ns,
                    )
                    self._last_state_publish_wall = wall_now
                    self.state_updates += 1

                trace_interval = (
                    None
                    if self.trace_hz <= 0.0
                    else max(1, int(round(BRIDGE_HZ / self.trace_hz)))
                )
                if trace_file is not None and trace_interval is not None and tick % trace_interval == 0:
                    trace_file.write(
                        json.dumps(self._telemetry(elapsed_s, command), separators=(",", ":"))
                        + "\n"
                    )
                    trace_file.flush()
                    trace_file = self._rotate_trace(
                        trace_path, trace_file, self.trace_max_bytes
                    )
                if self.live_mode and tick % max(1, int(BRIDGE_HZ // 2)) == 0:
                    self._write_live_status(
                        elapsed_s=elapsed_s,
                        command=command,
                        planner_ready=planner_ready,
                        start_requested=start_requested,
                        stop_requested=stop_requested,
                    )
                if view is not None:
                    if not view.is_running():
                        stop_event.set()
                    elif tick % 33 == 0:
                        view.sync()
                tick += 1
                next_tick += BRIDGE_DT
                delay = next_tick - time.perf_counter()
                if delay > 0.0:
                    time.sleep(delay)
                else:
                    self.max_loop_lag_s = max(self.max_loop_lag_s, -delay)
                    next_tick = time.perf_counter()
        finally:
            if view is not None:
                view.close()
            if trace_file is not None:
                trace_file.close()
            self.owner.close()

        report = {
            "schema": "h2-mujoco-h2br-summary-v1",
            "live_mode": self.live_mode,
            "scenario": asdict(self.scenario),
            "physics_hz": 1.0 / self.scenario.physics_dt,
            "bridge_hz": BRIDGE_HZ,
            "scenario_elapsed_s": scenario_elapsed_s,
            "state_updates": self.state_updates,
            "command_updates": self.command_updates,
            "actuation_steps": self.actuation_steps,
            "first_command_s": self.first_command_s,
            "fault_injected": self._fault_injected,
            "fault_latched": self.fault_latched,
            "fault_reason": self.fault_reason,
            "fault_wall_s": self.fault_wall_s,
            "output_inhibited": self.output_inhibited,
            "max_abs_tau_nm": self.max_abs_tau,
            "max_torque_utilization": self.max_torque_utilization,
            "max_unconstrained_effort_utilization": self.max_unconstrained_effort_utilization,
            "command_target_clamps": self.command_target_clamps,
            "max_target_projection_rad": self.max_target_projection_rad,
            "target_projection_ticks": self.target_projection_ticks,
            "plant_metric_samples": self.plant_metric_samples,
            "max_abs_joint_velocity_rad_s": self.max_abs_joint_velocity,
            "min_root_z_m": self.min_root_z,
            "initial_root_z_m": float(self.plant.initial_root_pos[2]),
            "max_tilt_rad": self.max_tilt_rad,
            "hit_deadline_s": (
                None
                if self.live_mode
                else self.scenario.command_at_s + self.scenario.time_to_hit_s
            ),
            "hit_reach_window_half_width_s": HIT_REACH_WINDOW_HALF_WIDTH_S,
            "hit_window_samples": self.hit_window_samples,
            "min_paddle_to_ball_distance_m": (
                self.min_paddle_to_ball_distance_m
                if math.isfinite(self.min_paddle_to_ball_distance_m)
                else None
            ),
            "min_paddle_to_ball_elapsed_s": self.min_paddle_to_ball_elapsed_s,
            "min_paddle_to_hit_point_distance_m": (
                self.min_paddle_to_hit_point_distance_m
                if math.isfinite(self.min_paddle_to_hit_point_distance_m)
                else None
            ),
            "min_paddle_to_hit_point_elapsed_s": self.min_paddle_to_hit_point_elapsed_s,
            "max_loop_lag_s": self.max_loop_lag_s,
            "startup_support_until_s": self.scenario.startup_support_until_s,
            "startup_support_released_s": self.startup_support_released_s,
            "handover_offset_injected": self._handover_offset_injected,
            "handover_joint_offsets_hw": list(self.scenario.handover_joint_offsets_hw),
            "handover_offset_preload_s": self.scenario.handover_offset_preload_s,
            "actuator_id_by_hardware_id": self.plant.actuator_id_hw.tolist(),
            "qpos_addr_by_hardware_id": self.plant.qpos_addr_hw.tolist(),
            "qvel_addr_by_hardware_id": self.plant.qvel_addr_hw.tolist(),
        }
        if self.live_mode:
            report["hybrid_ball"] = {
                "endpoint": self.live_ball_endpoint,
                **self._hybrid_ball_telemetry(),
            }
        return report


def simulation_bridge_process(scenario_dict, stop_event, scenario_start_event, status_queue) -> None:
    """Spawn-safe bridge target; all errors are reported before process exit."""

    try:
        scenario = SimScenario(**scenario_dict)
        bridge = SimMujocoBridge(scenario)
        report = bridge.run(stop_event, scenario_start_event, status_queue)
        status_queue.put({"event": "finished", "report": report})
    except BaseException as error:
        status_queue.put({
            "event": "error",
            "error": f"{type(error).__name__}: {error}",
            "traceback": traceback.format_exc(),
        })
        raise


def live_simulation_bridge_process(
    scenario_dict,
    stop_event,
    scenario_start_event,
    status_queue,
    planner_handles,
    start_request_event,
    stop_request_event,
    policy_active_event,
    trace_hz: float,
    trace_max_bytes: int,
    live_status_path: str,
    mocap_zmq_endpoint: str = "tcp://127.0.0.1:5556",
) -> None:
    """Spawn-safe live HIL target with no ROS or hardware imports."""

    ball_source = None
    try:
        from .embodiments import load_embodiment
        from .live_hybrid_ball import (
            DEFAULT_BALL_CONTROL_DT,
            HybridBallController,
            LatestZmqBallSource,
        )
        from .ros_planner_ingress import FusedMujocoPlannerSource, SharedPlannerMailbox

        scenario = SimScenario(**scenario_dict)
        h2_spec = load_embodiment("h2")
        ball_source = LatestZmqBallSource(
            scenario.robot_base_origin_world,
            endpoint=mocap_zmq_endpoint,
        )
        ball_controller = HybridBallController(
            paddle_half_width=h2_spec.paddle_half_width,
            paddle_half_height=h2_spec.paddle_half_height,
            hit_plane_x=h2_spec.hit_plane_x,
            ball_control_dt=DEFAULT_BALL_CONTROL_DT,
        )
        planner = FusedMujocoPlannerSource(
            SharedPlannerMailbox(planner_handles),
            scenario.robot_base_origin_world,
        )
        bridge = SimMujocoBridge(
            scenario,
            planner=planner,
            live_mode=True,
            trace_hz=trace_hz,
            trace_max_bytes=trace_max_bytes,
            live_status_path=live_status_path,
            live_ball_source=ball_source,
            live_ball_controller=ball_controller,
            live_ball_endpoint=mocap_zmq_endpoint,
        )
        report = bridge.run(
            stop_event,
            scenario_start_event,
            status_queue,
            start_request_event=start_request_event,
            stop_request_event=stop_request_event,
            policy_active_event=policy_active_event,
        )
        status_queue.put({"event": "finished", "report": report})
    except BaseException as error:
        status_queue.put(
            {
                "event": "error",
                "error": f"{type(error).__name__}: {error}",
                "traceback": traceback.format_exc(),
            }
        )
        raise
    finally:
        if ball_source is not None:
            ball_source.close()


__all__ = [
    "BRIDGE_HZ",
    "BRIDGE_DT",
    "COMMAND_WATCHDOG_S",
    "DEFAULT_PHYSICS_DT",
    "FAULT_KINDS",
    "H2MujocoPlant",
    "SimH2BridgeOwner",
    "SimMujocoBridge",
    "SimScenario",
    "SyntheticPlanner",
    "live_simulation_bridge_process",
    "simulation_bridge_process",
    "validate_sim_shm_name",
]