#!/usr/bin/env python3 """Independent numerical endpoint tests for H1--H4.""" from pathlib import Path import sys import unittest import numpy as np CODE_ROOT = Path(__file__).resolve().parents[1] sys.path.insert(0, str(CODE_ROOT)) from analysis.metrics import ( # noqa: E402 MetricError, audit_h4_energy, compute_h1_composite, compute_h2_wrench_metrics, compute_h3_power_mismatch, compute_bilateral_diagnostics, derive_trial_metrics, ) class IndependentExperimentMetricsTest(unittest.TestCase): def test_h1_detects_only_eligible_discontinuity(self): metrics = compute_h1_composite( mapping_valid=[True, True, True], position_error_m=[0.0, 0.0, 0.0], orientation_error_rad=[0.0, 0.0, 0.0], q_slave=np.array( [ [0.0, 0.0], [0.05, 0.0], [0.50, 0.0], ] ), swivel_angle_rad=[0.0, 0.05, 0.10], master_step_norm=[0.0, 0.05, 0.05], position_threshold_m=0.01, orientation_threshold_rad=0.1, joint_step_threshold_rad=0.2, swivel_step_threshold_rad=0.2, input_step_threshold_rad=0.1, ) self.assertEqual(metrics["h1_F_r"], 0) self.assertEqual(metrics["h1_D_r"], 1) self.assertEqual(metrics["h1_C_r"], 1) def test_h2_separates_force_and_moment_rmse(self): reference = np.zeros((3, 6)) estimate = np.tile([3.0, 4.0, 0.0, 0.0, 0.0, 2.0], (3, 1)) metrics = compute_h2_wrench_metrics( estimate, reference, numerical_rank_deficient=[False, False, True], operationally_ill_conditioned=[True, False, True], numerical_rank_threshold=[1e-9, 2e-9, 3e-9], operational_min_scaled_singular_threshold=[0.05] * 3, scaled_singular_values=np.array( [ [1.0, 0.04], [1.0, 0.06], [1.0, 0.01], ] ), condition_number=[25.0, 16.0, 100.0], ) self.assertAlmostEqual(metrics["h2_force_rmse_N"], 5.0) self.assertAlmostEqual(metrics["h2_moment_rmse_Nm"], 2.0) self.assertAlmostEqual( metrics["h2_numerical_rank_deficient_fraction"], 1.0 / 3.0 ) self.assertAlmostEqual( metrics["h2_operationally_ill_conditioned_fraction"], 2.0 / 3.0 ) self.assertAlmostEqual( metrics["h2_operational_min_scaled_singular_threshold"], 0.05 ) self.assertAlmostEqual(metrics["h2_min_scaled_singular_value"], 0.01) self.assertAlmostEqual(metrics["h2_max_condition_number"], 100.0) def test_h3_is_zero_for_identical_aligned_ports(self): torque = np.array([[1.0, 2.0], [-2.0, 1.0], [0.5, -0.5]]) velocity = np.array([[0.2, 0.1], [0.3, -0.1], [0.4, 0.2]]) metrics = compute_h3_power_mismatch( tau_master_raw=torque, qd_master=velocity, tau_slave_source=torque, qd_slave_source=velocity, dt=0.002, ) self.assertAlmostEqual(metrics["h3_epsilon_P_act"], 0.0) self.assertTrue(metrics["h3_normalized_metric_valid"]) self.assertEqual(metrics["h3_epsilon_P_act_gated"], 0.0) def test_h3_gates_low_activity_but_keeps_absolute_mismatch(self): metrics = compute_h3_power_mismatch( tau_master_raw=np.array([[2.0e-5], [1.0e-5]]), qd_master=np.ones((2, 1)), tau_slave_source=np.zeros((2, 1)), qd_slave_source=np.ones((2, 1)), dt=0.002, minimum_power_activity_J=1.0e-3, ) self.assertFalse(metrics["h3_normalized_metric_valid"]) self.assertIsNone(metrics["h3_epsilon_P_act_gated"]) self.assertGreater(metrics["h3_absolute_power_mismatch_J"], 0.0) self.assertEqual( metrics["h3_absolute_power_mismatch_J"], metrics["h3_power_mismatch_numerator_J"], ) def test_h4_reconstructs_floor_and_projection_distortion(self): metrics = audit_h4_energy( energy_before_J=[1.2, 1.1], energy_after_J=[1.1, 1.0], tau_candidate=np.array([[0.4], [0.4]]), tau_applied=np.array([[0.1], [0.1]]), qd_master=np.ones((2, 1)), dt=1.0, energy_min_J=1.0, energy_max_J=2.0, audit_tolerance_J=1e-12, ) self.assertTrue(metrics["h4_energy_audit_pass"]) self.assertAlmostEqual(metrics["h4_projected_floor_deficit_J"], 0.0) self.assertAlmostEqual(metrics["h4_shadow_floor_deficit_J"], 0.6) self.assertAlmostEqual(metrics["h4_delta_B_J"], 0.6) self.assertAlmostEqual(metrics["h4_D_proj"], 0.75, places=10) def test_h4_detects_downstream_drive_modification(self): metrics = audit_h4_energy( energy_before_J=[1.1], energy_after_J=[1.0], tau_candidate=np.array([[0.2]]), tau_applied=np.array([[0.1]]), tau_accepted=np.array([[0.15]]), qd_master=np.ones((1, 1)), dt=1.0, energy_min_J=1.0, energy_max_J=2.0, software_preclip_J=[1.0], audit_tolerance_J=1e-12, ) self.assertFalse(metrics["h4_energy_audit_pass"]) self.assertAlmostEqual( metrics["h4_downstream_modification_max_Nm"], 0.05 ) self.assertGreater(metrics["h4_software_preclip_max_error_J"], 0.0) def test_h4_uses_stored_trial_specific_energy_bounds(self): samples = { "energy_before_J": np.array([0.15]), "energy_after_J": np.array([0.14]), "tau_master_candidate": np.array([[0.1]]), "tau_master_applied": np.array([[0.1]]), "qd_master": np.array([[1.0]]), "dt": np.array([0.1]), "configured_energy_min_J": np.array([0.0]), "configured_energy_max_J": np.array([0.2]), } metrics = derive_trial_metrics( samples, { "enabled": ["h4"], "h4": {"audit_tolerance_J": 1e-12}, }, ) self.assertEqual(metrics["h4_energy_min_J"], 0.0) self.assertEqual(metrics["h4_energy_max_J"], 0.2) with self.assertRaisesRegex(MetricError, "disagrees"): derive_trial_metrics( samples, { "enabled": ["h4"], "h4": { "energy_min_J": 0.05, "energy_max_J": 0.2, }, }, ) def test_bilateral_diagnostics_expose_task_and_intervention_cost(self): metrics = compute_bilateral_diagnostics( master_tracking_error_rad=[0.1, 0.2], slave_tracking_error_rad=[0.2, 0.4], feedback_torque_Nm=np.array([[3.0, 4.0], [0.0, 0.0]]), contact_force_N=[0.0, 2.0], projection_factor=[1.0, 0.5], ) self.assertAlmostEqual( metrics["bilateral_master_tracking_rmse_rad"], np.sqrt(0.025), ) self.assertAlmostEqual( metrics["bilateral_feedback_torque_rms_Nm"], 5.0 / np.sqrt(2.0), ) self.assertEqual( metrics["bilateral_projection_intervention_fraction"], 0.5 ) self.assertEqual(metrics["bilateral_contact_fraction"], 0.5) if __name__ == "__main__": unittest.main()