209 lines
7.5 KiB
Python
209 lines
7.5 KiB
Python
#!/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()
|