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()