Blame for runtime/test_simulation_contract.py

from __future__ import annotations

import ast
import multiprocessing
import os
from pathlib import Path
import time
import unittest
import uuid

import numpy as np

from runtime.configuration import PROJECT_ROOT
from run_sim_shm import (
    H2_MIN_ROOT_Z_M,
    PADDLE_REACH_DISTANCE_M,
    _fault_acceptance_checks,
    _healthy_acceptance_checks,
    _validate_run_timing,
)
from runtime.h2_contract import (
    COMMAND_JOINT_MARGIN_RAD,
    H2_DOF,
    LOWCMD_DOF,
    MODE_PR,
    command_position_bounds_hw,
)
from runtime.h2_shm_bridge import (
    H2SharedMemoryBridge,
    MOTION_SWITCHER_ATTESTED_FLAG,
    SHM_NAME,
    SIMULATION_ACK,
    SIMULATION_RUNNER_ATTESTED_FLAG,
    SIMULATOR_OWNER_FLAG,
    SIM_SHM_PREFIX,
    SHM_VERSION,
)
from runtime.planner_protocol import (
    BALL_SOURCE_MONOTONIC_INDEX,
    FRAME_CONVENTION_INDEX,
    FRAME_CONVENTION_WORLD_X,
    H2_PLANNER_PACKET_LEN,
    H2_PLANNER_PROTOCOL_VERSION,
    PROTOCOL_VERSION_INDEX,
    ROBOT_BASE_ORIGIN_START_INDEX,
    ROBOT_SOURCE_MONOTONIC_INDEX,
    build_h2_planner_packet,
)
from runtime.runner_startup import SimulationRunnerStartup
from runtime.sim_mujoco_bridge import (
    BRIDGE_HZ,
    H2MujocoPlant,
    STATE_MAX_JOINT_VELOCITY_RAD_S,
    SimCommand,
    SimH2BridgeOwner,
    SimMujocoBridge,
    SimScenario,
    validate_sim_shm_name,
)


def _simulation_owner_process(name: str, ready, stop) -> None:
    owner = SimH2BridgeOwner(name)
    try:
        ready.set()
        stop.wait(10.0)
    finally:
        owner.close()


class PlannerPacketBuilderTests(unittest.TestCase):
    def test_exact_v3_layout(self):
        packet = build_h2_planner_packet(
            robot_pos_world=(-1.8, 0.1, 0.27),
            robot_quat_xyzw=(0.0, 0.0, 0.0, 1.0),
            time_to_hit_s=0.8,
            hit_pos_robot=(0.4, -0.1, 1.1),
            racket_velocity_robot=(2.0, 0.0, 0.4),
            command_id=7,
            incoming_velocity_robot=(-3.0, 0.1, -0.2),
            landing_xy_robot=(2.87, 0.0),
            robot_source_monotonic_s=100.0,
            ball_source_monotonic_s=100.01,
            robot_base_origin_world=(-1.87, 0.0, -0.76),
        )
        self.assertEqual(packet.shape, (H2_PLANNER_PACKET_LEN,))
        self.assertEqual(packet[PROTOCOL_VERSION_INDEX], H2_PLANNER_PROTOCOL_VERSION)
        self.assertEqual(packet[ROBOT_SOURCE_MONOTONIC_INDEX], 100.0)
        self.assertEqual(packet[BALL_SOURCE_MONOTONIC_INDEX], 100.01)
        np.testing.assert_array_equal(
            packet[ROBOT_BASE_ORIGIN_START_INDEX : ROBOT_BASE_ORIGIN_START_INDEX + 3],
            [-1.87, 0.0, -0.76],
        )
        self.assertEqual(packet[FRAME_CONVENTION_INDEX], FRAME_CONVENTION_WORLD_X)
        self.assertEqual(packet[14], 7.0)

    def test_nonfinite_input_and_noninteger_id_are_rejected(self):
        kwargs = dict(
            robot_pos_world=(0.0, 0.0, 1.0), robot_quat_xyzw=(0.0, 0.0, 0.0, 1.0),
            time_to_hit_s=0.8, hit_pos_robot=(0.4, 0.0, 1.1),
            racket_velocity_robot=(2.0, 0.0, 0.4), command_id=1,
            incoming_velocity_robot=(-3.0, 0.0, 0.0), landing_xy_robot=(2.87, 0.0),
            robot_source_monotonic_s=1.0, ball_source_monotonic_s=1.0,
            robot_base_origin_world=(-1.87, 0.0, -0.76),
        )
        with self.assertRaises(ValueError):
            build_h2_planner_packet(**{**kwargs, "time_to_hit_s": float("nan")})
        with self.assertRaises(ValueError):
            build_h2_planner_packet(**{**kwargs, "command_id": 1.5})


class SimulationIsolationTests(unittest.TestCase):
    def test_transport_matches_measured_h2_unique_lowstate_rate(self):
        self.assertEqual(BRIDGE_HZ, 1000.0)

    def test_scenario_start_resets_preload_physics_phase(self):
        source = (PROJECT_ROOT / "runtime/sim_mujoco_bridge.py").read_text(encoding="utf-8")
        start = source.index("if scenario_start_event.is_set() and scenario_wall_start is None:")
        reset = source.index("physics_accumulator = 0.0", start)
        elapsed = source.index("elapsed_s =", start)
        self.assertLess(reset, elapsed)

    def test_simulation_uses_position_only_h2br_v5(self):
        self.assertEqual(SHM_VERSION, 5)

    def test_production_shm_name_is_rejected(self):
        with self.assertRaises(ValueError):
            validate_sim_shm_name(SHM_NAME)
        with self.assertRaisesRegex(RuntimeError, "random"):
            SimulationRunnerStartup(SIMULATION_ACK).validate_before_model_load(
                shm_name=SHM_NAME,
                config_revision="unitree_sdk2_python@65691c8",
            )

    def test_simulation_ack_is_exact(self):
        with self.assertRaisesRegex(RuntimeError, "ack-simulation-only"):
            SimulationRunnerStartup("wrong").validate_before_model_load(
                shm_name=f"{SIM_SHM_PREFIX}test",
                config_revision="unitree_sdk2_python@65691c8",
            )

    def test_simulation_modules_have_no_hardware_middleware_imports(self):
        paths = [PROJECT_ROOT / "run_sim_shm.py", PROJECT_ROOT / "runtime/sim_mujoco_bridge.py"]
        forbidden_roots = {"rclpy", "unitree_hg", "unitree_sdk2py"}
        for path in paths:
            tree = ast.parse(path.read_text(encoding="utf-8"), filename=str(path))
            imported = set()
            for node in ast.walk(tree):
                if isinstance(node, ast.Import):
                    imported.update(alias.name.split(".")[0] for alias in node.names)
                elif isinstance(node, ast.ImportFrom) and node.module:
                    imported.add(node.module.split(".")[0])
            self.assertTrue(forbidden_roots.isdisjoint(imported), f"{path}: {imported}")

    def test_canonical_scenario_releases_startup_support_before_command(self):
        scenario = SimScenario(
            shm_name=f"{SIM_SHM_PREFIX}scenario_contract",
            mjcf_path=str(PROJECT_ROOT / "assets/robot_description/h2/mjcf/h2_pingpong.xml"),
            ready_pose_path=str(
                PROJECT_ROOT / "ready_pose/H2/frozen_closd_ready_pose_toeout_lowload.npz"
            ),
            trace_path=str(PROJECT_ROOT / "sim_runs/test-never-created.jsonl"),
            command_at_s=6.3,
        )
        scenario.validate()
        self.assertLess(scenario.startup_support_until_s, scenario.command_at_s)


class SimulationAcceptanceTests(unittest.TestCase):
    @staticmethod
    def _active_fault_runner(stop_reason: str) -> dict[str, object]:
        return {
            "ready_done": True,
            "policy_has_run": True,
            "accepted_command_id": 1,
            "reference_command_id": 1,
            "active_reference_policy_ticks": 12,
            "stopped": True,
            "stop_reason": stop_reason,
        }

    @staticmethod
    def _healthy_runner() -> dict[str, object]:
        return {
            "ready_done": True,
            "policy_has_run": True,
            "stopped": False,
            "policy_ticks": 100,
            "active_reference_policy_ticks": 50,
            "runner_active_elapsed_s": 9.0,
            "accepted_command_id": 1,
            "reference_command_id": 1,
        }

    @staticmethod
    def _healthy_bridge() -> dict[str, object]:
        return {
            "actuation_steps": 100,
            "scenario": {"command_at_s": 6.3, "time_to_hit_s": 0.8},
            "scenario_elapsed_s": 9.0,
            "startup_support_released_s": 5.8,
            "fault_injected": False,
            "fault_reason": "Python runner requested the permanent fault latch",
            "output_inhibited": False,
            "max_torque_utilization": 0.5,
            "max_abs_joint_velocity_rad_s": 8.0,
            "max_tilt_rad": 0.2,
            "min_root_z_m": 0.93,
            "hit_window_samples": 60,
            "min_paddle_to_ball_distance_m": 0.05,
            "min_paddle_to_ball_elapsed_s": 7.10,
            "min_paddle_to_hit_point_distance_m": 0.02,
            "min_paddle_to_hit_point_elapsed_s": 7.10,
        }

    def test_expected_active_fault_passes_with_correlated_reason_and_time(self):
        checks = _fault_acceptance_checks(
            fault_kind="ball_source_dropout",
            fault_at_s=6.8,
            runner_error="",
            runner_summary=self._active_fault_runner(
                "ball mocap source became stale during an active stroke (0.130s)"
            ),
            bridge_report={
                "fault_injected": True,
                "fault_latched": True,
                "fault_wall_s": 6.94,
                "fault_reason": "Python runner requested the permanent fault latch",
            },
        )
        self.assertTrue(all(checks.values()), checks)

    def test_unrelated_early_ankle_failure_cannot_pass_fault_test(self):
        checks = _fault_acceptance_checks(
            fault_kind="ball_source_dropout",
            fault_at_s=6.8,
            runner_error="",
            runner_summary=self._active_fault_runner(
                "joint limit violation: left_ankle_roll_joint=-0.4903rad"
            ),
            bridge_report={
                "fault_injected": False,
                "fault_latched": True,
                "fault_wall_s": 6.64,
                "fault_reason": "empty position/effort target intersection for left_ankle_roll_joint",
            },
        )
        self.assertFalse(all(checks.values()), checks)
        self.assertFalse(checks["requested_fault_was_injected"])
        self.assertFalse(checks["response_not_before_injection"])
        self.assertFalse(checks["runner_reported_expected_fault_class"])

    def test_direct_invalid_lowstate_requires_exact_inhibiting_bridge_fault(self):
        checks = _fault_acceptance_checks(
            fault_kind="invalid_lowstate",
            fault_at_s=6.8,
            runner_error="RuntimeError: H2 LowState is stale (0.061s)",
            runner_summary=self._active_fault_runner("bridge motor-fault latch"),
            bridge_report={
                "fault_injected": True,
                "fault_latched": True,
                "fault_wall_s": 6.801,
                "fault_reason": "injected invalid LowState",
                "output_inhibited": True,
            },
        )
        self.assertTrue(all(checks.values()), checks)

    def test_state_dropout_accepts_runner_gate_winning_before_bridge_watchdog(self):
        checks = _fault_acceptance_checks(
            fault_kind="state_dropout",
            fault_at_s=6.8,
            runner_error="RuntimeError: H2 LowState is stale (0.078s)",
            runner_summary={
                **self._active_fault_runner(""),
                "stopped": False,
            },
            bridge_report={
                "fault_injected": True,
                "fault_latched": True,
                "fault_wall_s": 6.88,
                "fault_reason": "Python runner requested the permanent fault latch",
                "output_inhibited": True,
            },
        )
        self.assertTrue(all(checks.values()), checks)

    def test_protocol_mismatch_expects_production_tt_not_ready_path(self):
        checks = _fault_acceptance_checks(
            fault_kind="planner_protocol_mismatch",
            fault_at_s=6.8,
            runner_error="",
            runner_summary=self._active_fault_runner(
                "planner bridge has not received an H2 v3 packet"
            ),
            bridge_report={
                "fault_injected": True,
                "fault_latched": True,
                "fault_wall_s": 6.82,
                "fault_reason": "Python runner requested the permanent fault latch",
            },
        )
        self.assertTrue(all(checks.values()), checks)

    def test_planner_receipt_dropout_accepts_any_frozen_clock_winning(self):
        reasons = (
            "planner receipt is stale (0.101s)",
            "robot mocap source is stale (0.101s)",
            "ball mocap source is stale (0.121s)",
            "ball mocap source became stale during an active stroke (0.121s)",
        )
        for reason in reasons:
            with self.subTest(reason=reason):
                checks = _fault_acceptance_checks(
                    fault_kind="planner_receipt_dropout",
                    fault_at_s=6.8,
                    runner_error="",
                    runner_summary=self._active_fault_runner(reason),
                    bridge_report={
                        "fault_injected": True,
                        "fault_latched": True,
                        "fault_wall_s": 6.92,
                        "fault_reason": "Python runner requested the permanent fault latch",
                    },
                )
                self.assertTrue(all(checks.values()), checks)

    def test_non_pr_fault_does_not_require_runner_to_read_invalid_mode(self):
        checks = _fault_acceptance_checks(
            fault_kind="non_pr_mode",
            fault_at_s=6.8,
            runner_error="RuntimeError: H2 LowState is stale (0.080s)",
            runner_summary=self._active_fault_runner("bridge motor-fault latch"),
            bridge_report={
                "fault_injected": True,
                "fault_latched": True,
                "fault_wall_s": 6.801,
                "fault_reason": "LowState left PR(0) mode (mode_pr=1)",
                "output_inhibited": True,
            },
        )
        self.assertTrue(all(checks.values()), checks)

    def test_healthy_metrics_must_remain_inside_real_runner_gates(self):
        runner = self._healthy_runner()
        bridge = self._healthy_bridge()
        bridge["max_torque_utilization"] = 1.0
        bridge["max_abs_joint_velocity_rad_s"] = 40.01
        checks = _healthy_acceptance_checks(
            runner_error="",
            runner_summary=runner,
            bridge_report=bridge,
            max_joint_velocity_rad_s=40.0,
            max_tilt_rad=0.8,
        )
        self.assertFalse(checks["joint_velocity_within_runner_gate"])
        bridge["max_abs_joint_velocity_rad_s"] = 40.0
        checks = _healthy_acceptance_checks(
            runner_error="",
            runner_summary=runner,
            bridge_report=bridge,
            max_joint_velocity_rad_s=40.0,
            max_tilt_rad=0.8,
        )
        self.assertTrue(all(checks.values()), checks)

    def test_full_run_virtual_support_cannot_pass_active_acceptance(self):
        bridge = self._healthy_bridge()
        bridge["startup_support_released_s"] = None
        checks = _healthy_acceptance_checks(
            runner_error="",
            runner_summary=self._healthy_runner(),
            bridge_report=bridge,
            max_joint_velocity_rad_s=40.0,
            max_tilt_rad=0.8,
        )
        self.assertFalse(checks["virtual_support_released_before_active_command"])
        self.assertFalse(all(checks.values()), checks)

    def test_forged_frozen_only_policy_summary_cannot_pass(self):
        runner = self._healthy_runner()
        runner["reference_command_id"] = 0
        runner["active_reference_policy_ticks"] = 0
        checks = _healthy_acceptance_checks(
            runner_error="",
            runner_summary=runner,
            bridge_report=self._healthy_bridge(),
            max_joint_velocity_rad_s=40.0,
            max_tilt_rad=0.8,
        )
        self.assertFalse(checks["active_reference_id_matches_command"])
        self.assertFalse(checks["active_reference_policy_ticks_recorded"])
        self.assertFalse(all(checks.values()), checks)

    def test_forged_fault_without_matching_active_reference_cannot_pass(self):
        runner = self._active_fault_runner("ball mocap source became stale")
        runner["reference_command_id"] = 0
        runner["active_reference_policy_ticks"] = 0
        checks = _fault_acceptance_checks(
            fault_kind="ball_source_dropout",
            fault_at_s=6.8,
            runner_error="",
            runner_summary=runner,
            bridge_report={
                "fault_injected": True,
                "fault_latched": True,
                "fault_wall_s": 6.93,
                "fault_reason": "Python runner requested the permanent fault latch",
            },
        )
        self.assertFalse(checks["active_reference_id_matched_before_fault"])
        self.assertFalse(checks["active_reference_policy_ran_before_fault"])

    def test_forged_collapsed_root_or_missing_root_metric_cannot_pass(self):
        for root_value in (0.20, None):
            with self.subTest(root_value=root_value):
                bridge = self._healthy_bridge()
                bridge["min_root_z_m"] = root_value
                checks = _healthy_acceptance_checks(
                    runner_error="",
                    runner_summary=self._healthy_runner(),
                    bridge_report=bridge,
                    max_joint_velocity_rad_s=40.0,
                    max_tilt_rad=0.8,
                )
                self.assertFalse(checks["root_height_above_h2_safety_floor"])
                self.assertGreater(H2_MIN_ROOT_Z_M, 0.20)

    def test_forged_missed_paddle_reach_cannot_pass(self):
        bridge = self._healthy_bridge()
        bridge["min_paddle_to_ball_distance_m"] = PADDLE_REACH_DISTANCE_M + 0.001
        checks = _healthy_acceptance_checks(
            runner_error="",
            runner_summary=self._healthy_runner(),
            bridge_report=bridge,
            max_joint_velocity_rad_s=40.0,
            max_tilt_rad=0.8,
        )
        self.assertFalse(checks["paddle_center_reached_ball_proxy"])

    def test_long_tth_must_cover_deadline_and_recovery(self):
        with self.assertRaisesRegex(ValueError, "time_to_hit_s"):
            _validate_run_timing(
                duration_s=7.4,
                command_at_s=6.3,
                time_to_hit_s=2.0,
                fault_kind="none",
                fault_at_s=6.8,
            )
        bridge = self._healthy_bridge()
        bridge["scenario"] = {"command_at_s": 6.3, "time_to_hit_s": 2.0}
        bridge["scenario_elapsed_s"] = 7.4
        runner = self._healthy_runner()
        runner["runner_active_elapsed_s"] = 7.4
        checks = _healthy_acceptance_checks(
            runner_error="",
            runner_summary=runner,
            bridge_report=bridge,
            max_joint_velocity_rad_s=40.0,
            max_tilt_rad=0.8,
        )
        self.assertFalse(checks["active_cycle_and_recovery_completed"])

    def test_requested_nine_seconds_cannot_hide_early_actual_exit(self):
        bridge = self._healthy_bridge()
        bridge["scenario_elapsed_s"] = 7.2
        runner = self._healthy_runner()
        runner["runner_active_elapsed_s"] = 7.2
        checks = _healthy_acceptance_checks(
            runner_error="",
            runner_summary=runner,
            bridge_report=bridge,
            max_joint_velocity_rad_s=40.0,
            max_tilt_rad=0.8,
        )
        self.assertFalse(checks["active_cycle_and_recovery_completed"])

        runner.pop("runner_active_elapsed_s")
        checks = _healthy_acceptance_checks(
            runner_error="",
            runner_summary=runner,
            bridge_report=bridge,
            max_joint_velocity_rad_s=40.0,
            max_tilt_rad=0.8,
        )
        self.assertFalse(checks["active_cycle_and_recovery_completed"])

    def test_bridge_teardown_grace_cannot_pad_runner_recovery_window(self):
        bridge = self._healthy_bridge()
        # The bridge lives for a 0.12s watchdog grace after runner.run exits.
        # Old acceptance would trust 7.61s and pass the 7.60s deadline; the
        # actual runner controlled for only 7.49s and must fail closed.
        bridge["scenario_elapsed_s"] = 7.61
        runner = self._healthy_runner()
        runner["runner_active_elapsed_s"] = 7.49
        checks = _healthy_acceptance_checks(
            runner_error="",
            runner_summary=runner,
            bridge_report=bridge,
            max_joint_velocity_rad_s=40.0,
            max_tilt_rad=0.8,
        )
        self.assertFalse(checks["active_cycle_and_recovery_completed"])


@unittest.skipUnless(Path("/dev/shm").is_dir(), "Linux POSIX shm test")
class SimulationSharedMemoryTests(unittest.TestCase):
    @staticmethod
    def _write_owner_state(
        owner: SimH2BridgeOwner,
        packet: np.ndarray,
        *,
        planner_ready: bool = True,
    ) -> None:
        owner.write_state(
            q=np.zeros(H2_DOF),
            dq=np.zeros(H2_DOF),
            quat_wxyz=np.array([1.0, 0.0, 0.0, 0.0]),
            gyro=np.zeros(3),
            root_xy=np.zeros(2),
            quat_xyzw=np.array([0.0, 0.0, 0.0, 1.0]),
            planner_packet=packet,
            start_requested=False,
            stop_requested=False,
            emergency_stop=False,
            mode_pr=MODE_PR,
            planner_ready=planner_ready,
        )

    @staticmethod
    def _packet() -> np.ndarray:
        return build_h2_planner_packet(
            robot_pos_world=(-1.87, 0.0, 0.28),
            robot_quat_xyzw=(0.0, 0.0, 0.0, 1.0),
            time_to_hit_s=0.8,
            hit_pos_robot=(0.4, 0.0, 1.1),
            racket_velocity_robot=(2.0, 0.0, 0.4),
            command_id=1,
            incoming_velocity_robot=(-3.0, 0.0, 0.0),
            landing_xy_robot=(2.87, 0.0),
            robot_source_monotonic_s=100.0,
            ball_source_monotonic_s=100.0,
            robot_base_origin_world=(-1.87, 0.0, -0.76),
        )

    def test_protocol_mismatch_preserves_payload_but_clears_tt_ready(self):
        name = f"{SIM_SHM_PREFIX}{os.getpid()}_{uuid.uuid4().hex}"
        owner = SimH2BridgeOwner(name)
        try:
            packet = self._packet()
            packet[PROTOCOL_VERSION_INDEX] = 99.0
            self._write_owner_state(owner, packet, planner_ready=False)
            self.assertEqual(int(owner.mem.tt_ready), 0)
            self.assertEqual(float(owner.mem.tt_data[PROTOCOL_VERSION_INDEX]), 99.0)
        finally:
            owner.close()

    def test_rejected_non_pr_sample_keeps_last_valid_state_fields(self):
        name = f"{SIM_SHM_PREFIX}{os.getpid()}_{uuid.uuid4().hex}"
        owner = SimH2BridgeOwner(name)
        try:
            self._write_owner_state(owner, self._packet())
            prior_stamp = int(owner.mem.state_stamp_ns)
            prior_mode = int(owner.mem.state_mode_pr)
            prior_q = np.ctypeslib.as_array(owner.mem.joint_pos).copy()
            owner.mark_emergency()
            self.assertEqual(int(owner.mem.emergency_stop), 1)
            self.assertEqual(int(owner.mem.state_stamp_ns), prior_stamp)
            self.assertEqual(int(owner.mem.state_mode_pr), prior_mode)
            np.testing.assert_array_equal(np.ctypeslib.as_array(owner.mem.joint_pos), prior_q)
        finally:
            owner.close()

    def test_owner_identity_and_distinct_attestation(self):
        name = f"{SIM_SHM_PREFIX}{os.getpid()}_{uuid.uuid4().hex}"
        context = multiprocessing.get_context("spawn")
        ready = context.Event()
        stop = context.Event()
        owner_process = context.Process(target=_simulation_owner_process, args=(name, ready, stop))
        owner_process.start()
        bridge = None
        try:
            self.assertTrue(ready.wait(3.0))
            bridge = H2SharedMemoryBridge(name, claim_writer=True)
            identity = bridge.read_identity()
            self.assertTrue(identity.owner_flags & SIMULATOR_OWNER_FLAG)
            self.assertFalse(identity.owner_flags & MOTION_SWITCHER_ATTESTED_FLAG)
            with self.assertRaisesRegex(RuntimeError, "simulator-owned"):
                bridge.attest_motion_switcher_released("mujoco-h2br-v1")
            bridge.attest_simulation_runner()
            identity = bridge.read_identity()
            self.assertTrue(identity.owner_flags & SIMULATION_RUNNER_ATTESTED_FLAG)
            self.assertFalse(identity.owner_flags & MOTION_SWITCHER_ATTESTED_FLAG)
        finally:
            if bridge is not None:
                bridge.close()
            stop.set()
            owner_process.join(timeout=3.0)
            if owner_process.is_alive():
                owner_process.terminate()
                owner_process.join(timeout=1.0)
            self.assertEqual(owner_process.exitcode, 0)


class MujocoNameMappingTests(unittest.TestCase):
    @classmethod
    def setUpClass(cls):
        try:
            import mujoco  # noqa: F401
        except ImportError as error:
            raise unittest.SkipTest(f"MuJoCo unavailable: {error}")
        cls.plant = H2MujocoPlant(
            str(PROJECT_ROOT / "assets/robot_description/h2/mjcf/h2_pingpong.xml"),
            str(PROJECT_ROOT / "ready_pose/H2/frozen_closd_ready_pose_toeout_lowload.npz"),
            physics_dt=0.002,
            keep_mjcf_joint_dynamics=False,
        )
        cls._initial_qpos = cls.plant.data.qpos.copy()
        cls._initial_qvel = cls.plant.data.qvel.copy()

    def setUp(self):
        self.plant.data.qpos[:] = self._initial_qpos
        self.plant.data.qvel[:] = self._initial_qvel
        self.plant.data.ctrl[:] = 0.0
        self.plant.last_tau_hw.fill(0.0)
        self.plant.last_target_hw[:] = self.plant.ready_q_hw
        self.plant.mujoco.mj_forward(self.plant.model, self.plant.data)

    def _command(self) -> SimCommand:
        q = np.zeros(LOWCMD_DOF, dtype=np.float32)
        q[:H2_DOF] = self.plant.ready_q_hw
        # Production locks both heads to exact zero once ready has settled.
        q[29:31] = 0.0
        kp = np.zeros(LOWCMD_DOF, dtype=np.float32)
        kp[:H2_DOF] = 28.501
        kd = np.zeros(LOWCMD_DOF, dtype=np.float32)
        mode = np.zeros(LOWCMD_DOF, dtype=np.uint8)
        mode[:H2_DOF] = 1
        return SimCommand(
            stamp_ns=1,
            enabled=True,
            flags=0,
            mode_pr=MODE_PR,
            q=q,
            dq=np.zeros(LOWCMD_DOF, dtype=np.float32),
            tau=np.zeros(LOWCMD_DOF, dtype=np.float32),
            kp=kp,
            kd=kd,
            mode=mode,
            reserve=np.zeros(LOWCMD_DOF, dtype=np.uint32),
        )

    def _projection_bridge(self) -> SimMujocoBridge:
        bridge = SimMujocoBridge.__new__(SimMujocoBridge)
        bridge.plant = self.plant
        bridge.command_target_clamps = 0
        bridge.max_unconstrained_effort_utilization = 0.0
        bridge.max_target_projection_rad = 0.0
        bridge.target_projection_ticks = 0
        bridge.plant_metric_samples = 0
        bridge.min_root_z = float(self.plant.data.qpos[2])
        bridge.max_abs_joint_velocity = 0.0
        bridge.max_tilt_rad = 0.0
        return bridge

    def test_hardware_ids_resolve_by_name_not_actuator_position(self):
        plant = self.plant
        self.assertEqual(plant.actuator_id_hw.shape, (H2_DOF,))
        self.assertEqual(len(set(plant.actuator_id_hw.tolist())), H2_DOF)
        # Waist HW roll/pitch/yaw -> MJCF actuators 13/14/12.
        self.assertEqual(plant.actuator_id_hw[12:15].tolist(), [13, 14, 12])
        # Wrist HW yaw/pitch/roll -> MJCF roll/pitch/yaw actuator layout.
        self.assertEqual(plant.actuator_id_hw[19:22].tolist(), [23, 22, 21])
        self.assertEqual(plant.actuator_id_hw[26:29].tolist(), [30, 29, 28])
        # Head is HW 29/30 but MJCF actuator 15/16.
        self.assertEqual(plant.actuator_id_hw[29:31].tolist(), [15, 16])
        q, dq, quat, gyro = plant.state()
        self.assertTrue(np.isfinite(np.concatenate([q, dq, quat, gyro])).all())
        np.testing.assert_allclose(q, plant.ready_q_hw, atol=1e-7)

    def test_gyro_is_free_joint_body_frame_qvel(self):
        expected = np.array([0.12, -0.23, 0.34], dtype=np.float64)
        self.plant.data.qvel[3:6] = expected
        _, _, _, gyro = self.plant.state()
        np.testing.assert_array_equal(gyro, expected)

    def test_v5_projects_each_tick_without_losing_raw_desired_target(self):
        index = 4  # left_ankle_roll_joint
        command = self._command()
        raw_desired = -0.15
        command.q[index] = raw_desired
        raw_command = command.q.copy()
        command.kd[index] = 1.814
        self.plant.data.qpos[self.plant.qpos_addr_hw[index]] = 0.0
        self.plant.data.qvel[self.plant.qvel_addr_hw[index]] = 10.0
        bridge = self._projection_bridge()

        projected, reason = bridge._constrain_for_latest_state(command)
        self.assertEqual(reason, "")
        self.assertIsNotNone(projected)
        assert projected is not None
        np.testing.assert_array_equal(command.q, raw_command)
        self.assertGreater(float(projected.q[index]), raw_desired)
        effort = (
            float(command.kp[index]) * float(projected.q[index])
            - float(command.kd[index]) * 10.0
        )
        self.assertGreaterEqual(effort, self.plant.torque_lo_hw[index] - 1.0e-5)
        self.assertLessEqual(effort, self.plant.torque_hi_hw[index] + 1.0e-5)

        # A later state makes the original request feasible.  The simulator
        # must reproject the immutable raw desired, not yesterday's projection.
        self.plant.data.qvel[self.plant.qvel_addr_hw[index]] = 0.0
        projected_again, reason = bridge._constrain_for_latest_state(command)
        self.assertEqual(reason, "")
        self.assertIsNotNone(projected_again)
        assert projected_again is not None
        self.assertAlmostEqual(float(projected_again.q[index]), raw_desired, places=6)
        self.assertEqual(bridge.target_projection_ticks, 1)
        self.assertGreater(bridge.max_target_projection_rad, 0.0)

    def test_v5_zero_kp_infeasible_effort_fails_closed(self):
        index = 4  # left_ankle_roll_joint, +/-19 Nm
        command = self._command()
        command.kp[index] = 0.0
        command.kd[index] = 1.0
        self.plant.data.qvel[self.plant.qvel_addr_hw[index]] = 20.0
        bridge = self._projection_bridge()

        projected, reason = bridge._constrain_for_latest_state(command)
        self.assertIsNone(projected)
        self.assertIn("zero-Kp effort is infeasible", reason)
        self.assertIn("left_ankle_roll_joint", reason)

    def test_v5_exact_head_zero_infeasible_fails_closed(self):
        index = 29  # head_pitch_joint, +/-50 Nm
        command = self._command()
        command.q[index] = 0.0
        command.kp[index] = 100.0
        command.kd[index] = 10.0
        self.plant.data.qpos[self.plant.qpos_addr_hw[index]] = 0.0
        self.plant.data.qvel[self.plant.qvel_addr_hw[index]] = 6.0
        bridge = self._projection_bridge()

        projected, reason = bridge._constrain_for_latest_state(command)
        self.assertIsNone(projected)
        self.assertEqual(
            reason,
            "exact head-zero target became infeasible for head_pitch_joint",
        )

    def test_sim_command_rejects_target_inside_cpp_joint_margin(self):
        index = 4
        command = self._command()
        command = SimCommand(
            stamp_ns=time.monotonic_ns(),
            enabled=True,
            flags=SIMULATOR_OWNER_FLAG | SIMULATION_RUNNER_ATTESTED_FLAG,
            mode_pr=command.mode_pr,
            q=command.q,
            dq=command.dq,
            tau=command.tau,
            kp=command.kp,
            kd=command.kd,
            mode=command.mode,
            reserve=command.reserve,
        )
        command.q[index] = np.float32(
            self.plant.joint_lo_hw[index] + 0.5 * COMMAND_JOINT_MARGIN_RAD
        )
        bridge = self._projection_bridge()
        reason = bridge._validate_command(command, time.monotonic_ns())
        self.assertIn("0.01rad margin", reason)

    def test_sim_command_accepts_exact_cpp_float32_margin_boundary(self):
        index = 3  # float32(-0.08725F + 0.01F) differs from float64 addition.
        command = self._command()
        command = SimCommand(
            stamp_ns=time.monotonic_ns(),
            enabled=True,
            flags=SIMULATOR_OWNER_FLAG | SIMULATION_RUNNER_ATTESTED_FLAG,
            mode_pr=command.mode_pr,
            q=command.q,
            dq=command.dq,
            tau=command.tau,
            kp=command.kp,
            kd=command.kd,
            mode=command.mode,
            reserve=command.reserve,
        )
        command_lo, _ = command_position_bounds_hw(
            self.plant.joint_lo_hw, self.plant.joint_hi_hw
        )
        command.q[index] = command_lo[index]
        bridge = self._projection_bridge()
        self.assertEqual(bridge._validate_command(command, time.monotonic_ns()), "")

    def test_physics_rate_velocity_spike_latches_before_lowstate_write(self):
        index = 4
        self.plant.data.qvel[self.plant.qvel_addr_hw[index]] = (
            STATE_MAX_JOINT_VELOCITY_RAD_S + 0.1
        )
        q, dq, quat, gyro = self.plant.state()
        bridge = self._projection_bridge()
        reason = bridge._observe_and_validate_plant_state(q, dq, quat, gyro)
        self.assertIn("joint velocity left production domain", reason)
        self.assertGreater(
            bridge.max_abs_joint_velocity, STATE_MAX_JOINT_VELOCITY_RAD_S
        )

        class OwnerProbe:
            emergency = False

            def mark_emergency(self):
                self.emergency = True

        bridge.owner = OwnerProbe()
        bridge.fault_latched = False
        bridge.fault_reason = ""
        bridge.fault_wall_s = None
        bridge.output_inhibited = False
        bridge.hold_q = None
        bridge._latch_fault(reason, 1.001, inhibit=True)
        self.assertTrue(bridge.fault_latched)
        self.assertTrue(bridge.output_inhibited)
        self.assertTrue(bridge.owner.emergency)
        self.assertEqual(bridge.fault_wall_s, 1.001)


if __name__ == "__main__":
    unittest.main()