exoskeleton/code/test/test_experiment_metrics.py

#!/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()