1164 lines
44 KiB
Python
1164 lines
44 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.1.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_h1_audit_metrics(
|
|
*,
|
|
differential_valid: Any,
|
|
validity_reason_code: Any,
|
|
reach_clip_code: Any,
|
|
joint_limit_active: Any,
|
|
geometry_degenerate: Any,
|
|
low_manipulability: Any,
|
|
slave_min_singular_value: Any,
|
|
slave_manipulability: Any,
|
|
validity_reason_labels: Any | None = None,
|
|
) -> dict[str, Any]:
|
|
"""Summarize H1 stratum isolation and invalid-sample explanations."""
|
|
differential = _vector(
|
|
differential_valid, "differential_valid", dtype=bool
|
|
)
|
|
reason_raw = _vector(validity_reason_code, "validity_reason_code")
|
|
reach_raw = _vector(reach_clip_code, "reach_clip_code")
|
|
joint_limit = _vector(
|
|
joint_limit_active, "joint_limit_active", dtype=bool
|
|
)
|
|
geometry = _vector(
|
|
geometry_degenerate, "geometry_degenerate", dtype=bool
|
|
)
|
|
low_manip = _vector(
|
|
low_manipulability, "low_manipulability", dtype=bool
|
|
)
|
|
minimum_singular = _vector(
|
|
slave_min_singular_value, "slave_min_singular_value"
|
|
)
|
|
manipulability = _vector(slave_manipulability, "slave_manipulability")
|
|
_same_rows(
|
|
{
|
|
"differential_valid": differential,
|
|
"validity_reason_code": reason_raw,
|
|
"reach_clip_code": reach_raw,
|
|
"joint_limit_active": joint_limit,
|
|
"geometry_degenerate": geometry,
|
|
"low_manipulability": low_manip,
|
|
"slave_min_singular_value": minimum_singular,
|
|
"slave_manipulability": manipulability,
|
|
}
|
|
)
|
|
_finite(reason_raw, "validity_reason_code")
|
|
_finite(reach_raw, "reach_clip_code")
|
|
_finite(minimum_singular, "slave_min_singular_value")
|
|
_finite(manipulability, "slave_manipulability")
|
|
if np.any(minimum_singular < 0.0) or np.any(manipulability < 0.0):
|
|
raise MetricError(
|
|
"H1 singular-value and manipulability diagnostics must be non-negative"
|
|
)
|
|
if not np.all(reason_raw == np.floor(reason_raw)) or np.any(reason_raw < 0):
|
|
raise MetricError("validity_reason_code must contain non-negative integers")
|
|
if not np.all(reach_raw == np.floor(reach_raw)) or not set(
|
|
np.asarray(reach_raw, dtype=int).tolist()
|
|
).issubset({-1, 0, 1}):
|
|
raise MetricError("reach_clip_code must contain only -1, 0, or 1")
|
|
reason = np.asarray(reason_raw, dtype=int)
|
|
reach = np.asarray(reach_raw, dtype=int)
|
|
|
|
if validity_reason_labels is None:
|
|
labels: list[str] = []
|
|
elif (
|
|
not isinstance(validity_reason_labels, list)
|
|
or any(not isinstance(label, str) or not label for label in validity_reason_labels)
|
|
):
|
|
raise MetricError("H1 validity_reason_labels must be a list of strings")
|
|
else:
|
|
labels = list(validity_reason_labels)
|
|
maximum_code = int(np.max(reason))
|
|
if labels and maximum_code >= len(labels):
|
|
raise MetricError(
|
|
"H1 validity_reason_labels does not cover every recorded code"
|
|
)
|
|
|
|
unique_codes, counts = np.unique(reason, return_counts=True)
|
|
histogram = {
|
|
(labels[int(code)] if labels else f"code_{int(code)}"): int(count)
|
|
for code, count in zip(unique_codes, counts)
|
|
}
|
|
invalid = ~differential
|
|
invalid_codes = reason[invalid]
|
|
if invalid_codes.size:
|
|
invalid_unique, invalid_counts = np.unique(
|
|
invalid_codes, return_counts=True
|
|
)
|
|
highest = int(np.max(invalid_counts))
|
|
# Ties use the lowest stable enum code.
|
|
primary_code = int(
|
|
np.min(invalid_unique[invalid_counts == highest])
|
|
)
|
|
primary_reason = (
|
|
labels[primary_code] if labels else f"code_{primary_code}"
|
|
)
|
|
else:
|
|
primary_reason = labels[0] if labels else "code_0"
|
|
|
|
return {
|
|
"h1_reach_clip_fraction": float(np.mean(reach != 0)),
|
|
"h1_reach_clip_lower_fraction": float(np.mean(reach == -1)),
|
|
"h1_reach_clip_upper_fraction": float(np.mean(reach == 1)),
|
|
"h1_low_manipulability_fraction": float(np.mean(low_manip)),
|
|
"h1_joint_limit_active_fraction": float(np.mean(joint_limit)),
|
|
"h1_geometry_degenerate_fraction": float(np.mean(geometry)),
|
|
"h1_min_slave_min_singular_value": float(np.min(minimum_singular)),
|
|
"h1_min_slave_manipulability": float(np.min(manipulability)),
|
|
"h1_validity_reason_histogram": histogram,
|
|
"h1_primary_invalid_reason": primary_reason,
|
|
"h1_invalid_reason_sample_count": int(invalid_codes.size),
|
|
"h1_unexplained_invalid_fraction": float(
|
|
np.mean(invalid & (reason == 0))
|
|
),
|
|
}
|
|
|
|
|
|
def compute_h2_wrench_metrics(
|
|
wrench_estimated: Any,
|
|
wrench_reference: Any,
|
|
*,
|
|
sample_mask: Any | None = None,
|
|
numerical_rank_deficient: Any | None = None,
|
|
operationally_ill_conditioned: Any | None = None,
|
|
numerical_rank_threshold: Any | None = None,
|
|
operational_min_scaled_singular_threshold: Any | None = None,
|
|
scaled_singular_values: Any | None = None,
|
|
condition_number: 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)
|
|
result = {
|
|
"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(),
|
|
}
|
|
if numerical_rank_deficient is not None:
|
|
numerical_flag = _vector(
|
|
numerical_rank_deficient,
|
|
"numerical_rank_deficient",
|
|
dtype=bool,
|
|
)
|
|
if numerical_flag.shape[0] != n:
|
|
raise MetricError("H2 numerical-rank flag length mismatch")
|
|
result["h2_numerical_rank_deficient_fraction"] = float(
|
|
np.mean(numerical_flag[mask])
|
|
)
|
|
if operationally_ill_conditioned is not None:
|
|
operational_flag = _vector(
|
|
operationally_ill_conditioned,
|
|
"operationally_ill_conditioned",
|
|
dtype=bool,
|
|
)
|
|
if operational_flag.shape[0] != n:
|
|
raise MetricError("H2 operational-condition flag length mismatch")
|
|
result["h2_operationally_ill_conditioned_fraction"] = float(
|
|
np.mean(operational_flag[mask])
|
|
)
|
|
if numerical_rank_threshold is not None:
|
|
numerical_threshold = _vector(
|
|
numerical_rank_threshold,
|
|
"numerical_rank_threshold",
|
|
)
|
|
if numerical_threshold.shape[0] != n:
|
|
raise MetricError("H2 numerical-rank threshold length mismatch")
|
|
_finite(numerical_threshold, "numerical_rank_threshold")
|
|
result["h2_numerical_rank_threshold_min"] = float(
|
|
np.min(numerical_threshold[mask])
|
|
)
|
|
result["h2_numerical_rank_threshold_max"] = float(
|
|
np.max(numerical_threshold[mask])
|
|
)
|
|
if operational_min_scaled_singular_threshold is not None:
|
|
operational_threshold = _vector(
|
|
operational_min_scaled_singular_threshold,
|
|
"operational_min_scaled_singular_threshold",
|
|
)
|
|
if operational_threshold.shape[0] != n:
|
|
raise MetricError("H2 operational threshold length mismatch")
|
|
_finite(
|
|
operational_threshold,
|
|
"operational_min_scaled_singular_threshold",
|
|
)
|
|
selected_threshold = operational_threshold[mask]
|
|
if not np.allclose(
|
|
selected_threshold,
|
|
selected_threshold[0],
|
|
rtol=0.0,
|
|
atol=1e-15,
|
|
):
|
|
raise MetricError(
|
|
"H2 operational threshold must be constant within a trial"
|
|
)
|
|
result["h2_operational_min_scaled_singular_threshold"] = float(
|
|
selected_threshold[0]
|
|
)
|
|
if scaled_singular_values is not None:
|
|
singular_values = _matrix(
|
|
scaled_singular_values,
|
|
"scaled_singular_values",
|
|
)
|
|
if singular_values.shape[0] != n:
|
|
raise MetricError("H2 singular-value sample count mismatch")
|
|
_finite(singular_values, "scaled_singular_values")
|
|
result["h2_min_scaled_singular_value"] = float(
|
|
np.min(singular_values[mask, -1])
|
|
)
|
|
if condition_number is not None:
|
|
condition = _vector(condition_number, "condition_number")
|
|
if condition.shape[0] != n:
|
|
raise MetricError("H2 condition-number sample count mismatch")
|
|
if np.any(np.isnan(condition)) or np.any(condition < 0.0):
|
|
raise MetricError(
|
|
"H2 condition number must be non-negative and not NaN"
|
|
)
|
|
selected_condition = condition[mask]
|
|
finite_condition = selected_condition[np.isfinite(selected_condition)]
|
|
result["h2_infinite_condition_number_fraction"] = float(
|
|
np.mean(~np.isfinite(selected_condition))
|
|
)
|
|
result["h2_max_finite_condition_number"] = (
|
|
float(np.max(finite_condition))
|
|
if finite_condition.size
|
|
else None
|
|
)
|
|
# A rank-deficient sample has infinite condition number. JSON cannot
|
|
# represent infinity, so the explicit fraction above carries that
|
|
# condition while this field is null in that case.
|
|
result["h2_max_condition_number"] = (
|
|
float(np.max(selected_condition))
|
|
if np.all(np.isfinite(selected_condition))
|
|
else None
|
|
)
|
|
return result
|
|
|
|
|
|
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,
|
|
minimum_power_activity_J: float = 0.0,
|
|
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)
|
|
minimum_power_activity_J = float(minimum_power_activity_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")
|
|
if (
|
|
not np.isfinite(minimum_power_activity_J)
|
|
or minimum_power_activity_J < 0.0
|
|
):
|
|
raise MetricError(
|
|
"minimum_power_activity_J must be finite and non-negative"
|
|
)
|
|
|
|
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))
|
|
power_activity = float(
|
|
0.5
|
|
* np.sum((np.abs(power_master) + np.abs(scaled_slave)) * intervals)
|
|
)
|
|
denominator = power_activity + epsilon_energy_J
|
|
normalized = numerator / denominator
|
|
normalized_valid = bool(power_activity >= minimum_power_activity_J)
|
|
return {
|
|
# Kept for backward-compatible diagnostics. Confirmatory analysis must
|
|
# use the validity flag/gated value when a nonzero activity gate is set.
|
|
"h3_epsilon_P_act": normalized,
|
|
"h3_epsilon_P_act_gated": normalized if normalized_valid else None,
|
|
"h3_normalized_metric_valid": normalized_valid,
|
|
"h3_minimum_power_activity_J": minimum_power_activity_J,
|
|
"h3_power_activity_J": power_activity,
|
|
"h3_absolute_power_mismatch_J": numerator,
|
|
"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_energy_min_J": energy_min_J,
|
|
"h4_energy_max_J": energy_max_J,
|
|
"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 compute_bilateral_diagnostics(
|
|
*,
|
|
master_tracking_error_rad: Any,
|
|
slave_tracking_error_rad: Any,
|
|
feedback_torque_Nm: Any,
|
|
contact_force_N: Any,
|
|
projection_factor: Any,
|
|
projection_tolerance: float = 1e-12,
|
|
contact_force_threshold_N: float = 1e-6,
|
|
) -> dict[str, Any]:
|
|
"""Compute secondary task/transparency diagnostics from stored samples."""
|
|
master_error = _vector(
|
|
master_tracking_error_rad, "master_tracking_error_rad"
|
|
)
|
|
slave_error = _vector(
|
|
slave_tracking_error_rad, "slave_tracking_error_rad"
|
|
)
|
|
feedback = _matrix(feedback_torque_Nm, "feedback_torque_Nm")
|
|
contact = _vector(contact_force_N, "contact_force_N")
|
|
rho = _vector(projection_factor, "projection_factor")
|
|
_same_rows(
|
|
{
|
|
"master_tracking_error_rad": master_error,
|
|
"slave_tracking_error_rad": slave_error,
|
|
"feedback_torque_Nm": feedback,
|
|
"contact_force_N": contact,
|
|
"projection_factor": rho,
|
|
}
|
|
)
|
|
for array, name in (
|
|
(master_error, "master_tracking_error_rad"),
|
|
(slave_error, "slave_tracking_error_rad"),
|
|
(feedback, "feedback_torque_Nm"),
|
|
(contact, "contact_force_N"),
|
|
(rho, "projection_factor"),
|
|
):
|
|
_finite(array, name)
|
|
projection_tolerance = float(projection_tolerance)
|
|
contact_force_threshold_N = float(contact_force_threshold_N)
|
|
if not np.isfinite(projection_tolerance) or projection_tolerance < 0.0:
|
|
raise MetricError("projection_tolerance must be finite and non-negative")
|
|
if (
|
|
not np.isfinite(contact_force_threshold_N)
|
|
or contact_force_threshold_N < 0.0
|
|
):
|
|
raise MetricError(
|
|
"contact_force_threshold_N must be finite and non-negative"
|
|
)
|
|
feedback_norm = np.linalg.norm(feedback, axis=1)
|
|
return {
|
|
"bilateral_master_tracking_rmse_rad": float(
|
|
np.sqrt(np.mean(np.square(master_error)))
|
|
),
|
|
"bilateral_slave_tracking_rmse_rad": float(
|
|
np.sqrt(np.mean(np.square(slave_error)))
|
|
),
|
|
"bilateral_feedback_torque_rms_Nm": float(
|
|
np.sqrt(np.mean(np.square(feedback_norm)))
|
|
),
|
|
"bilateral_feedback_torque_peak_Nm": float(np.max(feedback_norm)),
|
|
"bilateral_contact_force_rms_N": float(
|
|
np.sqrt(np.mean(np.square(contact)))
|
|
),
|
|
"bilateral_contact_force_peak_N": float(np.max(contact)),
|
|
"bilateral_contact_fraction": float(
|
|
np.mean(contact > contact_force_threshold_N)
|
|
),
|
|
"bilateral_projection_intervention_fraction": float(
|
|
np.mean(rho < (1.0 - projection_tolerance))
|
|
),
|
|
"bilateral_projection_factor_min": float(np.min(rho)),
|
|
}
|
|
|
|
|
|
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 _constant_trial_value(value: Any, name: str) -> float:
|
|
"""Resolve a scalar or constant non-empty sample vector."""
|
|
array = np.asarray(value, dtype=float)
|
|
if array.ndim == 0:
|
|
result = float(array)
|
|
elif array.ndim == 1 and array.size > 0:
|
|
_finite(array, name)
|
|
result = float(array[0])
|
|
if not np.allclose(array, result, rtol=0.0, atol=1e-15):
|
|
raise MetricError(f"{name} must be constant within one trial")
|
|
else:
|
|
raise MetricError(f"{name} must be a scalar or non-empty vector")
|
|
if not np.isfinite(result):
|
|
raise MetricError(f"{name} must be finite")
|
|
return result
|
|
|
|
|
|
def _energy_bound_for_trial(
|
|
samples: Mapping[str, Any],
|
|
fields: Mapping[str, str],
|
|
family_config: Mapping[str, Any],
|
|
logical_name: str,
|
|
default_field: str,
|
|
) -> float:
|
|
"""Prefer stored effective bounds and reject config/sample disagreement."""
|
|
stored = _field(
|
|
samples,
|
|
fields,
|
|
logical_name,
|
|
default_field,
|
|
optional=True,
|
|
)
|
|
configured = family_config.get(logical_name)
|
|
if stored is None and configured is None:
|
|
raise MetricError(
|
|
f"H4 requires {logical_name} in samples or metric configuration"
|
|
)
|
|
if stored is None:
|
|
return _constant_trial_value(configured, logical_name)
|
|
stored_value = _constant_trial_value(stored, default_field)
|
|
if configured is not None:
|
|
configured_value = _constant_trial_value(configured, logical_name)
|
|
if not np.isclose(
|
|
stored_value, configured_value, rtol=0.0, atol=1e-15
|
|
):
|
|
raise MetricError(
|
|
f"{logical_name} disagrees with stored effective configuration"
|
|
)
|
|
return stored_value
|
|
|
|
|
|
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,
|
|
)
|
|
)
|
|
audit_fields = family_config.get("audit_fields", [])
|
|
if audit_fields:
|
|
required_audit_fields = {
|
|
"map_validity_reason_code",
|
|
"map_reach_clip_code",
|
|
"map_joint_limit_active",
|
|
"map_geometry_degenerate",
|
|
"map_slave_min_singular_value",
|
|
"map_slave_manipulability",
|
|
"map_low_manipulability",
|
|
}
|
|
if (
|
|
not isinstance(audit_fields, list)
|
|
or any(
|
|
not isinstance(name, str) or not name
|
|
for name in audit_fields
|
|
)
|
|
):
|
|
raise MetricError("H1 audit_fields must be a list of names")
|
|
configured_audit_fields = set(audit_fields)
|
|
if configured_audit_fields != required_audit_fields:
|
|
missing_audit = sorted(
|
|
required_audit_fields - configured_audit_fields
|
|
)
|
|
unknown_audit = sorted(
|
|
configured_audit_fields - required_audit_fields
|
|
)
|
|
raise MetricError(
|
|
"H1 audit_fields must declare the complete audit set; "
|
|
f"missing={missing_audit}, unknown={unknown_audit}"
|
|
)
|
|
for stored_name in sorted(required_audit_fields):
|
|
if stored_name not in samples:
|
|
raise MetricError(
|
|
f"missing configured H1 audit field {stored_name!r}"
|
|
)
|
|
result.update(
|
|
compute_h1_audit_metrics(
|
|
differential_valid=differential_valid,
|
|
validity_reason_code=samples[
|
|
"map_validity_reason_code"
|
|
],
|
|
reach_clip_code=samples["map_reach_clip_code"],
|
|
joint_limit_active=samples[
|
|
"map_joint_limit_active"
|
|
],
|
|
geometry_degenerate=samples[
|
|
"map_geometry_degenerate"
|
|
],
|
|
low_manipulability=samples[
|
|
"map_low_manipulability"
|
|
],
|
|
slave_min_singular_value=samples[
|
|
"map_slave_min_singular_value"
|
|
],
|
|
slave_manipulability=samples[
|
|
"map_slave_manipulability"
|
|
],
|
|
validity_reason_labels=family_config.get(
|
|
"validity_reason_labels"
|
|
),
|
|
)
|
|
)
|
|
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,
|
|
),
|
|
numerical_rank_deficient=_field(
|
|
samples,
|
|
fields,
|
|
"numerical_rank_deficient",
|
|
"solver_numerical_rank_deficient",
|
|
optional=True,
|
|
),
|
|
operationally_ill_conditioned=_field(
|
|
samples,
|
|
fields,
|
|
"operationally_ill_conditioned",
|
|
"solver_operationally_ill_conditioned",
|
|
optional=True,
|
|
),
|
|
numerical_rank_threshold=_field(
|
|
samples,
|
|
fields,
|
|
"numerical_rank_threshold",
|
|
"solver_numerical_rank_threshold",
|
|
optional=True,
|
|
),
|
|
operational_min_scaled_singular_threshold=_field(
|
|
samples,
|
|
fields,
|
|
"operational_min_scaled_singular_threshold",
|
|
"operational_min_scaled_singular_threshold",
|
|
optional=True,
|
|
),
|
|
scaled_singular_values=_field(
|
|
samples,
|
|
fields,
|
|
"scaled_singular_values",
|
|
"scaled_singular_values",
|
|
optional=True,
|
|
),
|
|
condition_number=_field(
|
|
samples,
|
|
fields,
|
|
"condition_number",
|
|
"solver_condition_number",
|
|
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
|
|
),
|
|
minimum_power_activity_J=family_config.get(
|
|
"minimum_power_activity_J", 0.0
|
|
),
|
|
)
|
|
)
|
|
elif family == "h4":
|
|
energy_min_J = _energy_bound_for_trial(
|
|
samples,
|
|
fields,
|
|
family_config,
|
|
"energy_min_J",
|
|
"configured_energy_min_J",
|
|
)
|
|
energy_max_J = _energy_bound_for_trial(
|
|
samples,
|
|
fields,
|
|
family_config,
|
|
"energy_max_J",
|
|
"configured_energy_max_J",
|
|
)
|
|
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=energy_min_J,
|
|
energy_max_J=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
|
|
),
|
|
)
|
|
)
|
|
elif family == "bilateral":
|
|
result.update(
|
|
compute_bilateral_diagnostics(
|
|
master_tracking_error_rad=_field(
|
|
samples,
|
|
fields,
|
|
"master_tracking_error_rad",
|
|
"master_tracking_error",
|
|
),
|
|
slave_tracking_error_rad=_field(
|
|
samples,
|
|
fields,
|
|
"slave_tracking_error_rad",
|
|
"slave_tracking_error",
|
|
),
|
|
feedback_torque_Nm=_field(
|
|
samples,
|
|
fields,
|
|
"feedback_torque_Nm",
|
|
"tau_master_applied",
|
|
),
|
|
contact_force_N=_field(
|
|
samples,
|
|
fields,
|
|
"contact_force_N",
|
|
"contact_force_norm",
|
|
),
|
|
projection_factor=_field(
|
|
samples,
|
|
fields,
|
|
"projection_factor",
|
|
"rho",
|
|
),
|
|
projection_tolerance=family_config.get(
|
|
"projection_tolerance", 1e-12
|
|
),
|
|
contact_force_threshold_N=family_config.get(
|
|
"contact_force_threshold_N", 1e-6
|
|
),
|
|
)
|
|
)
|
|
else:
|
|
raise MetricError(f"unknown metric family {family!r}")
|
|
return result
|