exoskeleton/code/analysis/metrics.py

637 lines
23 KiB
Python

"""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