107 lines
3.7 KiB
Python
107 lines
3.7 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
|
|
audit_h4_energy,
|
|
compute_h1_composite,
|
|
compute_h2_wrench_metrics,
|
|
compute_h3_power_mismatch,
|
|
)
|
|
|
|
|
|
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)
|
|
self.assertAlmostEqual(metrics["h2_force_rmse_N"], 5.0)
|
|
self.assertAlmostEqual(metrics["h2_moment_rmse_Nm"], 2.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)
|
|
|
|
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)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|