"""Independent H1--H4 trial metrics. These functions consume stored arrays only. They deliberately do not import the simulation or controller implementations, so paper endpoints can be reconstructed independently from raw evidence. """ from __future__ import annotations from typing import Any, Mapping import numpy as np METRIC_SCHEMA_VERSION = "1.0.0" class MetricError(ValueError): pass def _vector(value: Any, name: str, *, dtype=float) -> np.ndarray: array = np.asarray(value, dtype=dtype) if array.ndim != 1 or array.size == 0: raise MetricError(f"{name} must be a non-empty one-dimensional array") return array def _matrix(value: Any, name: str, columns: int | None = None) -> np.ndarray: array = np.asarray(value, dtype=float) if array.ndim != 2 or array.shape[0] == 0: raise MetricError(f"{name} must be a non-empty two-dimensional array") if columns is not None and array.shape[1] != columns: raise MetricError(f"{name} must have {columns} columns") return array def _same_rows(named: Mapping[str, np.ndarray]) -> int: counts = {name: value.shape[0] for name, value in named.items()} if len(set(counts.values())) != 1: raise MetricError(f"sample counts differ: {counts}") return next(iter(counts.values())) def _finite(array: np.ndarray, name: str) -> None: if not np.all(np.isfinite(array)): raise MetricError(f"{name} contains non-finite values") def _wrapped_delta(array: np.ndarray) -> np.ndarray: return (array + np.pi) % (2.0 * np.pi) - np.pi def compute_h1_composite( *, mapping_valid: Any, position_error_m: Any, orientation_error_rad: Any, q_slave: Any, swivel_angle_rad: Any, master_step_norm: Any, position_threshold_m: float, orientation_threshold_rad: float, joint_step_threshold_rad: float, swivel_step_threshold_rad: float, input_step_threshold_rad: float, accepted: Any | None = None, commanded_reset: Any | None = None, degeneracy_transition: Any | None = None, ) -> dict[str, Any]: """Compute the trajectory-level H1 ``F_r``, ``D_r``, and ``C_r``.""" valid = _vector(mapping_valid, "mapping_valid", dtype=bool) e_position = _vector(position_error_m, "position_error_m") e_orientation = _vector(orientation_error_rad, "orientation_error_rad") slave = _matrix(q_slave, "q_slave") swivel = _vector(swivel_angle_rad, "swivel_angle_rad") input_step = _vector(master_step_norm, "master_step_norm") n = _same_rows( { "mapping_valid": valid, "position_error_m": e_position, "orientation_error_rad": e_orientation, "q_slave": slave, "swivel_angle_rad": swivel, "master_step_norm": input_step, } ) accepted_array = ( np.ones(n, dtype=bool) if accepted is None else _vector(accepted, "accepted", dtype=bool) ) reset = ( np.zeros(n, dtype=bool) if commanded_reset is None else _vector(commanded_reset, "commanded_reset", dtype=bool) ) degeneracy = ( np.zeros(n, dtype=bool) if degeneracy_transition is None else _vector( degeneracy_transition, "degeneracy_transition", dtype=bool, ) ) _same_rows( { "accepted": accepted_array, "commanded_reset": reset, "degeneracy_transition": degeneracy, "mapping_valid": valid, } ) for array, name in ( (e_position, "position_error_m"), (e_orientation, "orientation_error_rad"), (slave, "q_slave"), (swivel, "swivel_angle_rad"), (input_step, "master_step_norm"), ): _finite(array, name) thresholds = { "position_threshold_m": position_threshold_m, "orientation_threshold_rad": orientation_threshold_rad, "joint_step_threshold_rad": joint_step_threshold_rad, "swivel_step_threshold_rad": swivel_step_threshold_rad, "input_step_threshold_rad": input_step_threshold_rad, } if any(not np.isfinite(value) or value < 0.0 for value in thresholds.values()): raise MetricError("H1 thresholds must be finite and non-negative") failure_mask = accepted_array & ( ~valid | (e_position > position_threshold_m) | (e_orientation > orientation_threshold_rad) ) if n > 1: joint_step = np.max( np.abs(_wrapped_delta(slave[1:] - slave[:-1])), axis=1, ) swivel_step = np.abs(_wrapped_delta(swivel[1:] - swivel[:-1])) eligible = ( accepted_array[1:] & accepted_array[:-1] & valid[1:] & valid[:-1] & (input_step[1:] <= input_step_threshold_rad) & ~reset[1:] & ~degeneracy[1:] ) discontinuity_mask = eligible & ( (joint_step > joint_step_threshold_rad) | (swivel_step > swivel_step_threshold_rad) ) else: joint_step = np.empty(0, dtype=float) swivel_step = np.empty(0, dtype=float) eligible = np.empty(0, dtype=bool) discontinuity_mask = np.empty(0, dtype=bool) F_r = int(np.any(failure_mask)) D_r = int(np.any(discontinuity_mask)) return { "h1_F_r": F_r, "h1_D_r": D_r, "h1_C_r": max(F_r, D_r), "h1_failure_sample_count": int(np.sum(failure_mask)), "h1_discontinuity_sample_count": int(np.sum(discontinuity_mask)), "h1_eligible_increment_count": int(np.sum(eligible)), "h1_mapping_valid_fraction": float(np.mean(valid[accepted_array])) if np.any(accepted_array) else 0.0, "h1_max_position_error_m": float(np.max(e_position[accepted_array])) if np.any(accepted_array) else 0.0, "h1_max_orientation_error_rad": float(np.max(e_orientation[accepted_array])) if np.any(accepted_array) else 0.0, "h1_max_joint_step_rad": float(np.max(joint_step)) if joint_step.size else 0.0, "h1_max_swivel_step_rad": float(np.max(swivel_step)) if swivel_step.size else 0.0, } def compute_h2_wrench_metrics( wrench_estimated: Any, wrench_reference: Any, *, sample_mask: Any | None = None, ) -> dict[str, Any]: estimate = _matrix(wrench_estimated, "wrench_estimated", columns=6) reference = _matrix(wrench_reference, "wrench_reference", columns=6) n = _same_rows({"wrench_estimated": estimate, "wrench_reference": reference}) _finite(estimate, "wrench_estimated") _finite(reference, "wrench_reference") mask = ( np.ones(n, dtype=bool) if sample_mask is None else _vector(sample_mask, "sample_mask", dtype=bool) ) if mask.shape[0] != n or not np.any(mask): raise MetricError("H2 sample_mask must select at least one aligned sample") error = estimate[mask] - reference[mask] force_norm = np.linalg.norm(error[:, :3], axis=1) moment_norm = np.linalg.norm(error[:, 3:], axis=1) return { "h2_sample_count": int(error.shape[0]), "h2_force_rmse_N": float(np.sqrt(np.mean(force_norm**2))), "h2_moment_rmse_Nm": float(np.sqrt(np.mean(moment_norm**2))), "h2_force_mae_N": float(np.mean(force_norm)), "h2_moment_mae_Nm": float(np.mean(moment_norm)), "h2_force_bias_xyz_N": np.mean(error[:, :3], axis=0).tolist(), "h2_moment_bias_xyz_Nm": np.mean(error[:, 3:], axis=0).tolist(), } def _dt_array(dt: Any, count: int) -> np.ndarray: array = np.asarray(dt, dtype=float) if array.ndim == 0: array = np.full(count, float(array), dtype=float) if array.shape != (count,): raise MetricError(f"dt must be scalar or have shape ({count},)") if not np.all(np.isfinite(array)) or np.any(array <= 0.0): raise MetricError("dt must contain finite positive intervals") return array def compute_h3_power_mismatch( *, tau_master_raw: Any, qd_master: Any, tau_slave_source: Any, qd_slave_source: Any, dt: Any, force_scale: float = 1.0, epsilon_energy_J: float = 1e-12, return_valid: Any | None = None, ) -> dict[str, Any]: tau_m = _matrix(tau_master_raw, "tau_master_raw") qd_m = _matrix(qd_master, "qd_master") tau_s = _matrix(tau_slave_source, "tau_slave_source") qd_s = _matrix(qd_slave_source, "qd_slave_source") n = _same_rows( { "tau_master_raw": tau_m, "qd_master": qd_m, "tau_slave_source": tau_s, "qd_slave_source": qd_s, } ) if tau_m.shape != qd_m.shape or tau_s.shape != qd_s.shape: raise MetricError("torque and velocity shapes must match at each port") for array, name in ( (tau_m, "tau_master_raw"), (qd_m, "qd_master"), (tau_s, "tau_slave_source"), (qd_s, "qd_slave_source"), ): _finite(array, name) intervals = _dt_array(dt, n) chi = ( np.ones(n, dtype=bool) if return_valid is None else _vector(return_valid, "return_valid", dtype=bool) ) if chi.shape[0] != n: raise MetricError("return_valid length mismatch") force_scale = float(force_scale) epsilon_energy_J = float(epsilon_energy_J) if not np.isfinite(force_scale) or force_scale < 0.0: raise MetricError("force_scale must be finite and non-negative") if not np.isfinite(epsilon_energy_J) or epsilon_energy_J <= 0.0: raise MetricError("epsilon_energy_J must be finite and positive") power_master = np.einsum("ij,ij->i", tau_m, qd_m) power_slave = chi.astype(float) * np.einsum("ij,ij->i", tau_s, qd_s) scaled_slave = force_scale * power_slave numerator = float(np.sum(np.abs(power_master - scaled_slave) * intervals)) denominator = float( 0.5 * np.sum((np.abs(power_master) + np.abs(scaled_slave)) * intervals) + epsilon_energy_J ) return { "h3_epsilon_P_act": numerator / denominator, "h3_power_mismatch_numerator_J": numerator, "h3_power_normalizer_J": denominator, "h3_return_valid_fraction": float(np.mean(chi)), "h3_master_raw_work_J": float(np.sum(power_master * intervals)), "h3_scaled_slave_work_J": float(np.sum(scaled_slave * intervals)), } def audit_h4_energy( *, energy_before_J: Any, energy_after_J: Any, tau_candidate: Any, tau_applied: Any, tau_accepted: Any | None = None, qd_master: Any, dt: Any, energy_min_J: float, energy_max_J: float, epsilon_torque_impulse_Nms: float = 1e-12, audit_tolerance_J: float = 1e-10, software_preclip_J: Any | None = None, ) -> dict[str, Any]: energy_before = _vector(energy_before_J, "energy_before_J") energy_after = _vector(energy_after_J, "energy_after_J") candidate = _matrix(tau_candidate, "tau_candidate") applied = _matrix(tau_applied, "tau_applied") accepted = ( applied if tau_accepted is None else _matrix(tau_accepted, "tau_accepted") ) velocity = _matrix(qd_master, "qd_master") n = _same_rows( { "energy_before_J": energy_before, "energy_after_J": energy_after, "tau_candidate": candidate, "tau_applied": applied, "tau_accepted": accepted, "qd_master": velocity, } ) if not ( candidate.shape == applied.shape == accepted.shape == velocity.shape ): raise MetricError( "candidate, projected, accepted torque, and velocity must align" ) for array, name in ( (energy_before, "energy_before_J"), (energy_after, "energy_after_J"), (candidate, "tau_candidate"), (applied, "tau_applied"), (accepted, "tau_accepted"), (velocity, "qd_master"), ): _finite(array, name) intervals = _dt_array(dt, n) energy_min_J = float(energy_min_J) energy_max_J = float(energy_max_J) tolerance = float(audit_tolerance_J) epsilon_tau = float(epsilon_torque_impulse_Nms) if not ( np.isfinite(energy_min_J) and np.isfinite(energy_max_J) and 0.0 <= energy_min_J <= energy_max_J ): raise MetricError("invalid energy bounds") if not np.isfinite(tolerance) or tolerance < 0.0: raise MetricError("audit_tolerance_J must be finite and non-negative") if not np.isfinite(epsilon_tau) or epsilon_tau <= 0.0: raise MetricError( "epsilon_torque_impulse_Nms must be finite and positive" ) # The deterministic gate is reconstructed at the actuator-accepted port. # If no drive readback exists, callers may omit tau_accepted and explicitly # declare that projected == accepted for that backend. applied_power = np.einsum("ij,ij->i", accepted, velocity) candidate_power = np.einsum("ij,ij->i", candidate, velocity) reconstructed_preclip = energy_before - applied_power * intervals reconstructed_after = np.clip( reconstructed_preclip, energy_min_J, energy_max_J, ) deficit = np.maximum(0.0, energy_min_J - reconstructed_preclip) accounting_error = np.abs(energy_after - reconstructed_after) shadow_energy = float(energy_before[0]) shadow_min = shadow_energy for power, interval in zip(candidate_power, intervals): shadow_energy = min(energy_max_J, shadow_energy - float(power * interval)) shadow_min = min(shadow_min, shadow_energy) shadow_deficit = max(0.0, energy_min_J - shadow_min) projected_deficit = float(np.max(deficit)) numerator = float( np.sum(np.linalg.norm(accepted - candidate, axis=1) * intervals) ) denominator = float( np.sum(np.linalg.norm(candidate, axis=1) * intervals) + epsilon_tau ) preclip_discrepancy = 0.0 if software_preclip_J is not None: software = _vector(software_preclip_J, "software_preclip_J") if software.shape[0] != n: raise MetricError("software_preclip_J length mismatch") _finite(software, "software_preclip_J") preclip_discrepancy = float( np.max(np.abs(software - reconstructed_preclip)) ) audit_pass = ( projected_deficit <= tolerance and float(np.max(accounting_error)) <= tolerance and preclip_discrepancy <= tolerance ) return { "h4_energy_audit_pass": bool(audit_pass), "h4_preclip_floor_deficit_max_J": projected_deficit, "h4_energy_accounting_max_error_J": float(np.max(accounting_error)), "h4_software_preclip_max_error_J": preclip_discrepancy, "h4_shadow_floor_deficit_J": float(shadow_deficit), "h4_projected_floor_deficit_J": projected_deficit, "h4_delta_B_J": float(shadow_deficit - projected_deficit), "h4_D_proj": numerator / denominator, "h4_projection_distortion_numerator_Nms": numerator, "h4_projection_distortion_normalizer_Nms": denominator, "h4_shadow_energy_min_J": float(shadow_min), "h4_downstream_modification_max_Nm": float( np.max(np.linalg.norm(accepted - applied, axis=1)) ), } def _field( samples: Mapping[str, Any], fields: Mapping[str, str], logical_name: str, default_name: str, *, optional: bool = False, ) -> Any: stored_name = fields.get(logical_name, default_name) if stored_name not in samples: if optional: return None raise MetricError( f"missing sample field {stored_name!r} for {logical_name!r}" ) return samples[stored_name] def derive_trial_metrics( samples: Mapping[str, Any], configuration: Mapping[str, Any], ) -> dict[str, Any]: """Derive configured endpoint families from one stored trial.""" enabled = configuration.get("enabled") if not isinstance(enabled, list) or not enabled: raise MetricError("metric configuration needs a non-empty enabled list") result: dict[str, Any] = {"metric_schema_version": METRIC_SCHEMA_VERSION} for family in enabled: family_config = configuration.get(family, {}) fields = family_config.get("fields", {}) if family == "h1": thresholds = family_config.get("thresholds", {}) required_thresholds = ( "position_threshold_m", "orientation_threshold_rad", "joint_step_threshold_rad", "swivel_step_threshold_rad", "input_step_threshold_rad", ) missing = [name for name in required_thresholds if name not in thresholds] if missing: raise MetricError(f"missing H1 thresholds: {missing}") pose_success = _field( samples, fields, "pose_success", "map_pose_success" ) differential_valid = _field( samples, fields, "differential_valid", "map_differential_valid", ) mapping_valid = np.asarray(pose_success, dtype=bool) & np.asarray( differential_valid, dtype=bool ) result.update( compute_h1_composite( mapping_valid=mapping_valid, position_error_m=_field( samples, fields, "position_error_m", "map_position_error_m", ), orientation_error_rad=_field( samples, fields, "orientation_error_rad", "map_orientation_error_rad", ), q_slave=_field(samples, fields, "q_slave", "map_q_slave"), swivel_angle_rad=_field( samples, fields, "swivel_angle_rad", "map_swivel_angle_rad", ), master_step_norm=_field( samples, fields, "master_step_norm", "map_master_step_norm", ), accepted=_field( samples, fields, "accepted", "map_accepted", optional=True, ), commanded_reset=_field( samples, fields, "commanded_reset", "map_commanded_reset", optional=True, ), degeneracy_transition=_field( samples, fields, "degeneracy_transition", "map_degeneracy_transition", optional=True, ), **thresholds, ) ) elif family == "h2": result.update( compute_h2_wrench_metrics( _field( samples, fields, "wrench_estimated", "wrench_estimated", ), _field( samples, fields, "wrench_reference", "wrench_reference", ), sample_mask=_field( samples, fields, "sample_mask", "wrench_sample_mask", optional=True, ), ) ) elif family == "h3": result.update( compute_h3_power_mismatch( tau_master_raw=_field( samples, fields, "tau_master_raw", "tau_master_raw" ), qd_master=_field(samples, fields, "qd_master", "qd_master"), tau_slave_source=_field( samples, fields, "tau_slave_source", "tau_slave_source", ), qd_slave_source=_field( samples, fields, "qd_slave_source", "qd_slave_source", ), dt=_field(samples, fields, "dt", "dt"), return_valid=_field( samples, fields, "return_valid", "return_valid", optional=True, ), force_scale=family_config.get("force_scale", 1.0), epsilon_energy_J=family_config.get( "epsilon_energy_J", 1e-12 ), ) ) elif family == "h4": result.update( audit_h4_energy( energy_before_J=_field( samples, fields, "energy_before_J", "energy_before_J" ), energy_after_J=_field( samples, fields, "energy_after_J", "energy_after_J" ), tau_candidate=_field( samples, fields, "tau_candidate", "tau_master_candidate" ), tau_applied=_field( samples, fields, "tau_applied", "tau_master_applied" ), tau_accepted=_field( samples, fields, "tau_accepted", "tau_master_accepted", optional=True, ), qd_master=_field(samples, fields, "qd_master", "qd_master"), dt=_field(samples, fields, "dt", "dt"), software_preclip_J=_field( samples, fields, "software_preclip_J", "energy_preclip_J", optional=True, ), energy_min_J=family_config["energy_min_J"], energy_max_J=family_config["energy_max_J"], epsilon_torque_impulse_Nms=family_config.get( "epsilon_torque_impulse_Nms", 1e-12 ), audit_tolerance_J=family_config.get( "audit_tolerance_J", 1e-10 ), ) ) else: raise MetricError(f"unknown metric family {family!r}") return result