SmartBench/exoskeleton
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implement auditable calibration v2 protocols

Browse Source
This commit is contained in:
xtkuang 2026-07-27 12:56:08 +08:00
parent 2effd7b88d
commit 4503a12bf1
16 changed files with 2128 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
code/analysis/__init__.py
View File

@ -2,16 +2,20 @@
from .metrics import ( from .metrics import (
audit_h4_energy, audit_h4_energy,
compute_h1_audit_metrics,
compute_h1_composite, compute_h1_composite,
compute_h2_wrench_metrics, compute_h2_wrench_metrics,
compute_h3_power_mismatch, compute_h3_power_mismatch,
compute_bilateral_diagnostics,
derive_trial_metrics, derive_trial_metrics,
) )
__all__ = [ __all__ = [
"audit_h4_energy", "audit_h4_energy",
"compute_h1_audit_metrics",
"compute_h1_composite", "compute_h1_composite",
"compute_h2_wrench_metrics", "compute_h2_wrench_metrics",
"compute_h3_power_mismatch", "compute_h3_power_mismatch",
"compute_bilateral_diagnostics",
"derive_trial_metrics", "derive_trial_metrics",
] ]

41
code/analysis/make_paper_artifacts.py
View File

@ -80,7 +80,10 @@ def _scalar_cell(value: Any) -> Any:
) )
def _identity_row(trial: Mapping[str, Any]) -> dict[str, Any]: def _identity_row(
trial: Mapping[str, Any],
trial_metadata: Mapping[str, Any] | None = None,
) -> dict[str, Any]:
trajectory = trial["trajectory"] trajectory = trial["trajectory"]
method = trial["method"] method = trial["method"]
row = { row = {
@ -95,6 +98,11 @@ def _identity_row(trial: Mapping[str, Any]) -> dict[str, Any]:
} }
for name, value in sorted(trial.get("factors", {}).items()): for name, value in sorted(trial.get("factors", {}).items()):
row[f"factor_{name}"] = _scalar_cell(value) row[f"factor_{name}"] = _scalar_cell(value)
metadata = {} if trial_metadata is None else trial_metadata
for name in ("h2_data_group_id", "h2_data_seed_record_hash"):
value = metadata.get(name)
if value is not None:
row[name] = _scalar_cell(value)
return row return row
@ -109,6 +117,8 @@ def _atomic_write_csv(path: Path, rows: Sequence[Mapping[str, Any]]) -> None:
"trajectory_id", "trajectory_id",
"trajectory_family", "trajectory_family",
"replicate", "replicate",
"h2_data_group_id",
"h2_data_seed_record_hash",
] ]
all_fields = {key for row in rows for key in row} all_fields = {key for row in rows for key in row}
fieldnames = [name for name in identity_order if name in all_fields] fieldnames = [name for name in identity_order if name in all_fields]
@ -154,7 +164,17 @@ def generate_paper_source_data(
for trial in plan["trials"]: for trial in plan["trials"]:
trial_dir = batch_dir / "raw" / trial["trial_id"] trial_dir = batch_dir / "raw" / trial["trial_id"]
sample_path = trial_dir / "samples.npz" sample_path = trial_dir / "samples.npz"
trial_manifest_path = trial_dir / "trial_manifest.json"
input_files[str(sample_path.relative_to(batch_dir))] = file_sha256(sample_path) input_files[str(sample_path.relative_to(batch_dir))] = file_sha256(sample_path)
input_files[
str(trial_manifest_path.relative_to(batch_dir))
] = file_sha256(trial_manifest_path)
trial_manifest = load_document(trial_manifest_path)
trial_metadata = trial_manifest.get("metadata", {})
if not isinstance(trial_metadata, Mapping):
raise ValueError(
f"{trial_manifest_path} metadata must be a mapping"
)
with np.load(sample_path, allow_pickle=False) as archive: with np.load(sample_path, allow_pickle=False) as archive:
samples = {name: archive[name] for name in archive.files} samples = {name: archive[name] for name in archive.files}
method_id = trial["method"]["method_id"] method_id = trial["method"]["method_id"]
@ -167,7 +187,12 @@ def generate_paper_source_data(
if trial_configuration is None if trial_configuration is None
else derive_trial_metrics(samples, trial_configuration) else derive_trial_metrics(samples, trial_configuration)
) )
rows.append({**_identity_row(trial), **metrics}) rows.append(
{
**_identity_row(trial, trial_metadata),
**metrics,
}
)
metric_path = derived_dir / "trial_metrics.jsonl" metric_path = derived_dir / "trial_metrics.jsonl"
atomic_write_jsonl(metric_path, rows) atomic_write_jsonl(metric_path, rows)
@ -177,7 +202,15 @@ def generate_paper_source_data(
family_rows: list[dict[str, Any]] = [] family_rows: list[dict[str, Any]] = []
prefix = f"{family}_" prefix = f"{family}_"
for row in rows: for row in rows:
if not any(key.startswith(prefix) for key in row): provenance_identity_fields = {
"h2_data_group_id",
"h2_data_seed_record_hash",
}
if not any(
key.startswith(prefix)
and key not in provenance_identity_fields
for key in row
):
continue continue
selected = { selected = {
key: value key: value
@ -191,6 +224,8 @@ def generate_paper_source_data(
"trajectory_id", "trajectory_id",
"trajectory_family", "trajectory_family",
"replicate", "replicate",
"h2_data_group_id",
"h2_data_seed_record_hash",
} }
or key.startswith("factor_") or key.startswith("factor_")
or key.startswith(prefix) or key.startswith(prefix)

541
code/analysis/metrics.py
View File

@ -12,7 +12,7 @@ from typing import Any, Mapping
import numpy as np import numpy as np
METRIC_SCHEMA_VERSION = "1.0.0" METRIC_SCHEMA_VERSION = "1.1.0"
class MetricError(ValueError): class MetricError(ValueError):
@ -187,11 +187,132 @@ def compute_h1_composite(
} }
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( def compute_h2_wrench_metrics(
wrench_estimated: Any, wrench_estimated: Any,
wrench_reference: Any, wrench_reference: Any,
*, *,
sample_mask: Any | None = None, 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]: ) -> dict[str, Any]:
estimate = _matrix(wrench_estimated, "wrench_estimated", columns=6) estimate = _matrix(wrench_estimated, "wrench_estimated", columns=6)
reference = _matrix(wrench_reference, "wrench_reference", columns=6) reference = _matrix(wrench_reference, "wrench_reference", columns=6)
@ -208,7 +329,7 @@ def compute_h2_wrench_metrics(
error = estimate[mask] - reference[mask] error = estimate[mask] - reference[mask]
force_norm = np.linalg.norm(error[:, :3], axis=1) force_norm = np.linalg.norm(error[:, :3], axis=1)
moment_norm = np.linalg.norm(error[:, 3:], axis=1) moment_norm = np.linalg.norm(error[:, 3:], axis=1)
return { result = {
"h2_sample_count": int(error.shape[0]), "h2_sample_count": int(error.shape[0]),
"h2_force_rmse_N": float(np.sqrt(np.mean(force_norm**2))), "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_moment_rmse_Nm": float(np.sqrt(np.mean(moment_norm**2))),
@ -217,6 +338,104 @@ def compute_h2_wrench_metrics(
"h2_force_bias_xyz_N": np.mean(error[:, :3], axis=0).tolist(), "h2_force_bias_xyz_N": np.mean(error[:, :3], axis=0).tolist(),
"h2_moment_bias_xyz_Nm": 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: def _dt_array(dt: Any, count: int) -> np.ndarray:
@ -239,6 +458,7 @@ def compute_h3_power_mismatch(
dt: Any, dt: Any,
force_scale: float = 1.0, force_scale: float = 1.0,
epsilon_energy_J: float = 1e-12, epsilon_energy_J: float = 1e-12,
minimum_power_activity_J: float = 0.0,
return_valid: Any | None = None, return_valid: Any | None = None,
) -> dict[str, Any]: ) -> dict[str, Any]:
tau_m = _matrix(tau_master_raw, "tau_master_raw") tau_m = _matrix(tau_master_raw, "tau_master_raw")
@ -272,22 +492,39 @@ def compute_h3_power_mismatch(
raise MetricError("return_valid length mismatch") raise MetricError("return_valid length mismatch")
force_scale = float(force_scale) force_scale = float(force_scale)
epsilon_energy_J = float(epsilon_energy_J) 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: if not np.isfinite(force_scale) or force_scale < 0.0:
raise MetricError("force_scale must be finite and non-negative") raise MetricError("force_scale must be finite and non-negative")
if not np.isfinite(epsilon_energy_J) or epsilon_energy_J <= 0.0: if not np.isfinite(epsilon_energy_J) or epsilon_energy_J <= 0.0:
raise MetricError("epsilon_energy_J must be finite and positive") 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_master = np.einsum("ij,ij->i", tau_m, qd_m)
power_slave = chi.astype(float) * np.einsum("ij,ij->i", tau_s, qd_s) power_slave = chi.astype(float) * np.einsum("ij,ij->i", tau_s, qd_s)
scaled_slave = force_scale * power_slave scaled_slave = force_scale * power_slave
numerator = float(np.sum(np.abs(power_master - scaled_slave) * intervals)) numerator = float(np.sum(np.abs(power_master - scaled_slave) * intervals))
denominator = float( power_activity = float(
0.5 0.5
* np.sum((np.abs(power_master) + np.abs(scaled_slave)) * intervals) * np.sum((np.abs(power_master) + np.abs(scaled_slave)) * intervals)
+ epsilon_energy_J
) )
denominator = power_activity + epsilon_energy_J
normalized = numerator / denominator
normalized_valid = bool(power_activity >= minimum_power_activity_J)
return { return {
"h3_epsilon_P_act": numerator / denominator, # 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_mismatch_numerator_J": numerator,
"h3_power_normalizer_J": denominator, "h3_power_normalizer_J": denominator,
"h3_return_valid_fraction": float(np.mean(chi)), "h3_return_valid_fraction": float(np.mean(chi)),
@ -418,6 +655,8 @@ def audit_h4_energy(
"h4_projected_floor_deficit_J": projected_deficit, "h4_projected_floor_deficit_J": projected_deficit,
"h4_delta_B_J": float(shadow_deficit - projected_deficit), "h4_delta_B_J": float(shadow_deficit - projected_deficit),
"h4_D_proj": numerator / denominator, "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_numerator_Nms": numerator,
"h4_projection_distortion_normalizer_Nms": denominator, "h4_projection_distortion_normalizer_Nms": denominator,
"h4_shadow_energy_min_J": float(shadow_min), "h4_shadow_energy_min_J": float(shadow_min),
@ -427,6 +666,80 @@ def audit_h4_energy(
} }
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( def _field(
samples: Mapping[str, Any], samples: Mapping[str, Any],
fields: Mapping[str, str], fields: Mapping[str, str],
@ -445,6 +758,57 @@ def _field(
return samples[stored_name] 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( def derive_trial_metrics(
samples: Mapping[str, Any], samples: Mapping[str, Any],
configuration: Mapping[str, Any], configuration: Mapping[str, Any],
@ -533,6 +897,69 @@ def derive_trial_metrics(
**thresholds, **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": elif family == "h2":
result.update( result.update(
compute_h2_wrench_metrics( compute_h2_wrench_metrics(
@ -555,6 +982,48 @@ def derive_trial_metrics(
"wrench_sample_mask", "wrench_sample_mask",
optional=True, 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": elif family == "h3":
@ -588,9 +1057,26 @@ def derive_trial_metrics(
epsilon_energy_J=family_config.get( epsilon_energy_J=family_config.get(
"epsilon_energy_J", 1e-12 "epsilon_energy_J", 1e-12
), ),
minimum_power_activity_J=family_config.get(
"minimum_power_activity_J", 0.0
),
) )
) )
elif family == "h4": 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( result.update(
audit_h4_energy( audit_h4_energy(
energy_before_J=_field( energy_before_J=_field(
@ -621,8 +1107,8 @@ def derive_trial_metrics(
"energy_preclip_J", "energy_preclip_J",
optional=True, optional=True,
), ),
energy_min_J=family_config["energy_min_J"], energy_min_J=energy_min_J,
energy_max_J=family_config["energy_max_J"], energy_max_J=energy_max_J,
epsilon_torque_impulse_Nms=family_config.get( epsilon_torque_impulse_Nms=family_config.get(
"epsilon_torque_impulse_Nms", 1e-12 "epsilon_torque_impulse_Nms", 1e-12
), ),
@ -631,6 +1117,47 @@ def derive_trial_metrics(
), ),
) )
) )
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: else:
raise MetricError(f"unknown metric family {family!r}") raise MetricError(f"unknown metric family {family!r}")
return result return result

52
code/config/experiments/bilateral_calibration_v2_energy.json Normal file
View File

@ -0,0 +1,52 @@
{
"study_id": "g0c_bilateral_calibration_v2_energy",
"split": "calibration",
"root_seed": 2026072721,
"replicates": 2,
"methods": [
"proposed_energy"
],
"trajectories": [
{
"id": "contact_energy_tradeoff",
"family": "contact_roundtrip",
"duration_s": 1.5,
"contact_probe_fraction": 0.03
}
],
"factors": {
"map_policy": [
"source_stamped"
],
"return_delay_s": [
0.08
],
"forward_delay_s": [
0.0
],
"wall_damping": [
30.0
],
"wall_stiffness": [
400.0,
800.0
],
"feedback_strength": [
0.2,
0.35,
0.5
],
"energy_min": [
0.05
],
"energy_max": [
0.1
],
"energy_initial": [
0.055,
0.07,
0.09
]
},
"status": "Stage A energy-challenging calibration: 0.005, 0.020, and 0.040 J initial headroom bracket the approximately 0.017 J v1 contact shadow deficit"
}

106
code/config/experiments/bilateral_calibration_v2_network.json Normal file
View File

@ -0,0 +1,106 @@
{
"study_id": "g0c_bilateral_calibration_v2_network",
"split": "calibration",
"root_seed": 2026072722,
"replicates": 1,
"methods": [
"proposed_energy",
"direct_energy",
"matched_wrench_energy"
],
"trajectories": [
{
"id": "free_space_network_screen",
"family": "free_space",
"duration_s": 1.2,
"contact_probe_fraction": 0.0
},
{
"id": "contact_network_screen",
"family": "contact_roundtrip",
"duration_s": 1.2,
"contact_probe_fraction": 0.03
}
],
"factors": {
"map_policy": [
"source_stamped"
],
"network_profile": [
{
"profile_id": "nominal",
"forward_delay_s": 0.0,
"return_delay_s": 0.0,
"forward_jitter_s": 0.0,
"return_jitter_s": 0.0,
"forward_packet_loss": 0.0,
"return_packet_loss": 0.0
},
{
"profile_id": "return_delay_80ms",
"forward_delay_s": 0.0,
"return_delay_s": 0.08,
"forward_jitter_s": 0.0,
"return_jitter_s": 0.0,
"forward_packet_loss": 0.0,
"return_packet_loss": 0.0
},
{
"profile_id": "forward_delay_40ms",
"forward_delay_s": 0.04,
"return_delay_s": 0.0,
"forward_jitter_s": 0.0,
"return_jitter_s": 0.0,
"forward_packet_loss": 0.0,
"return_packet_loss": 0.0
},
{
"profile_id": "asymmetric_delay",
"forward_delay_s": 0.04,
"return_delay_s": 0.08,
"forward_jitter_s": 0.0,
"return_jitter_s": 0.0,
"forward_packet_loss": 0.0,
"return_packet_loss": 0.0
},
{
"profile_id": "asymmetric_delay_plus_jitter",
"forward_delay_s": 0.04,
"return_delay_s": 0.08,
"forward_jitter_s": 0.004,
"return_jitter_s": 0.004,
"forward_packet_loss": 0.0,
"return_packet_loss": 0.0
},
{
"profile_id": "asymmetric_delay_plus_loss",
"forward_delay_s": 0.04,
"return_delay_s": 0.08,
"forward_jitter_s": 0.0,
"return_jitter_s": 0.0,
"forward_packet_loss": 0.02,
"return_packet_loss": 0.02
}
],
"wall_damping": [
30.0
],
"wall_stiffness": [
800.0
],
"feedback_strength": [
0.35
],
"energy_min": [
0.05
],
"energy_max": [
0.1
],
"energy_initial": [
0.07
]
},
"requires_stage_a_selection": true,
"status": "Stage B template only: feedback_strength and all three energy values are placeholders from the middle Stage A cell and must be replaced by the selected Stage A profile before execution"
}

68
code/config/experiments/h1_calibration_v2.json Normal file
View File

@ -0,0 +1,68 @@
{
"study_id": "g0c_sew_calibration_v2",
"split": "calibration",
"root_seed": 2026072721,
"replicates": 3,
"methods": [
"sew",
"scaled_joint_space",
"bounded_dls_ik",
"task_priority_ik"
],
"trajectories": [
{
"id": "nominal_seeded_excursion",
"family": "nominal",
"sample_count": 81,
"instance_variation": {
"center_std_rad": 0.006,
"delta_scale_range": [0.90, 1.10],
"harmonic_weight_range": [-0.10, 0.10]
}
},
{
"id": "wide_valid_seeded_excursion",
"family": "reach_boundary",
"sample_count": 81,
"instance_variation": {
"center_std_rad": 0.005,
"delta_scale_range": [0.92, 1.08],
"harmonic_weight_range": [-0.08, 0.08]
}
},
{
"id": "valid_near_singularity",
"family": "low_manipulability_valid",
"sample_count": 81,
"low_manipulability_min_singular_threshold": 0.05,
"instance_variation": {
"center_std_rad": 0.004,
"center_jitter_mask": [1, 1, 1, 0, 1, 1, 1],
"delta_scale_range": [0.94, 1.06],
"harmonic_weight_range": [-0.06, 0.06]
}
},
{
"id": "isolated_upper_reach_clip",
"family": "reach_clip_upper",
"sample_count": 81,
"instance_variation": {
"center_std_rad": 0.004,
"center_jitter_mask": [1, 1, 1, 0, 1, 1, 1],
"delta_scale_range": [0.94, 1.06],
"harmonic_weight_range": [-0.06, 0.06]
}
},
{
"id": "master_joint_limit_neighborhood",
"family": "joint_limit",
"sample_count": 81,
"instance_variation": {
"center_std_rad": 0.004,
"center_jitter_mask": [0, 1, 1, 1, 1, 1, 1],
"delta_scale_range": [0.94, 1.06],
"harmonic_weight_range": [-0.06, 0.06]
}
}
]
}

52
code/config/experiments/h2_calibration_v2.json Normal file
View File

@ -0,0 +1,52 @@
{
"study_id": "g0c_estimator_sensitivity_calibration_v2",
"split": "calibration",
"root_seed": 2026072703,
"replicates": 2,
"methods": [
"scaled_dls",
"undamped_svd",
"no_bias",
"no_friction"
],
"trajectories": [
{
"id": "dynamic_wrench_sweep_v2",
"family": "synthetic_dynamic",
"sample_count": 192
}
],
"factors": {
"characteristic_length_m": [
0.2,
0.3,
0.4
],
"damping": [
0.02,
0.03,
0.05
],
"h2_data_root_seed": [
2026072703
],
"min_scaled_singular": [
0.02,
0.1
],
"model_error_std": [
0.0,
0.02
],
"operational_min_scaled_singular": [
0.05
],
"torque_noise_std_Nm": [
0.002,
0.02
],
"truth_characteristic_length_m": [
0.3
]
}
}

31
code/config/experiments/metrics_bilateral_v2.json Normal file
View File

@ -0,0 +1,31 @@
{
"enabled": [
"h3",
"h4",
"bilateral"
],
"h3": {
"force_scale": 1.0,
"epsilon_energy_J": 1e-12,
"minimum_power_activity_J": 0.001
},
"h4": {
"include_methods": [
"proposed_energy",
"direct_energy",
"matched_wrench_energy"
],
"epsilon_torque_impulse_Nms": 1e-12,
"audit_tolerance_J": 1e-10
},
"bilateral": {
"include_methods": [
"proposed_energy",
"direct_energy",
"matched_wrench_energy"
],
"projection_tolerance": 1e-12,
"contact_force_threshold_N": 1e-6
},
"status": "H3 reports an absolute mismatch for every trial and gates normalized epsilon below 1 mJ port activity; H4 reads effective energy bounds from each stored trial; bilateral diagnostics prevent selecting a trivially weak gain from Dproj alone"
}

39
code/config/experiments/metrics_h1_calibration_v2.json Normal file
View File

@ -0,0 +1,39 @@
{
"enabled": [
"h1"
],
"h1": {
"thresholds": {
"position_threshold_m": 0.005,
"orientation_threshold_rad": 0.05,
"joint_step_threshold_rad": 0.25,
"swivel_step_threshold_rad": 0.25,
"input_step_threshold_rad": 0.05
},
"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"
],
"validity_reason_labels": [
"none",
"reach_clipped_lower",
"reach_clipped_upper",
"joint_limit_active",
"geometry_degenerate",
"invalid_input",
"master_limit_violation",
"joint_limit_violation",
"task_tolerance_exceeded",
"solver_not_converged",
"numerical_failure",
"low_manipulability",
"unspecified_nonsmooth"
]
},
"status": "calibration-v2 only; validity reasons and seeded instances must be audited before threshold freeze"
}

543
code/experiments/executors.py
View File

@ -33,8 +33,9 @@ from core.retargeting_baselines import (
build_canonical_sew_target_baselines, build_canonical_sew_target_baselines,
) )
from core.wrench_solver import ScaledDLSSolver, UndampedSVDSolver from core.wrench_solver import ScaledDLSSolver, UndampedSVDSolver
from experiments.hashing import stable_hash
from experiments.io import TrialPayload from experiments.io import TrialPayload
from experiments.rng import generator_from_record from experiments.rng import generator_from_record, named_seed_record
from simulate_closed_loop import ( from simulate_closed_loop import (
SCENARIOS, SCENARIOS,
SimulationConfig, SimulationConfig,
@ -67,6 +68,30 @@ def _factor(trial: Mapping[str, Any], name: str, default: Any) -> Any:
return factors.get(name, default) return factors.get(name, default)
def _profiled_factor(
trial: Mapping[str, Any],
name: str,
default: Any,
*,
profile_name: str,
) -> Any:
"""Resolve a direct factor, then a coupled profile, then a default.
Direct factors deliberately take precedence. Profiles let calibration
studies express a small set of valid, directional network conditions
without expanding the Cartesian product of every delay/jitter/loss level.
"""
factors = trial.get("factors", {})
if not isinstance(factors, Mapping):
raise ValueError("trial factors must be a mapping")
if name in factors:
return factors[name]
profile = factors.get(profile_name, {})
if not isinstance(profile, Mapping):
raise ValueError(f"{profile_name} factor must be a mapping")
return profile.get(name, default)
def _enum_code(member: Enum) -> int: def _enum_code(member: Enum) -> int:
return list(type(member)).index(member) return list(type(member)).index(member)
@ -77,7 +102,12 @@ def _master_trajectory(
lower: np.ndarray, lower: np.ndarray,
upper: np.ndarray, upper: np.ndarray,
) -> np.ndarray: ) -> np.ndarray:
"""Generate a continuous, bounded master trajectory from a frozen spec.""" """Generate one seeded, paired, continuous master trajectory instance.
The trajectory random stream belongs to the pair, not the method. Thus
different replicates are genuine trajectory instances while all methods
inside one pair receive bit-identical master samples.
"""
specification = _trajectory_spec(trial) specification = _trajectory_spec(trial)
sample_count = int(specification.get("sample_count", 81)) sample_count = int(specification.get("sample_count", 81))
if sample_count < 3: if sample_count < 3:
@ -105,19 +135,85 @@ def _master_trajectory(
delta = np.zeros(7) delta = np.zeros(7)
delta[0] = -0.22 delta[0] = -0.22
elif family == "low_manipulability": elif family == "low_manipulability":
# Legacy calibration-v1 family retained only for reproducibility. It
# is reach-clipped and must not be treated as an isolated low-
# manipulability stratum; v2 uses ``low_manipulability_valid``.
center = center.copy() center = center.copy()
center[3] = 0.08 center[3] = 0.08
delta = np.array([0.04, 0.03, -0.04, 0.05, 0.02, -0.02, 0.02]) delta = np.array([0.04, 0.03, -0.04, 0.05, 0.02, -0.02, 0.02])
elif family == "low_manipulability_valid":
# Just inside the slave upper-reach boundary: low minimum singular
# value without the reach clipping that confounded calibration v1.
center = center.copy()
center[3] = 1.13
delta = np.array([0.025, -0.020, 0.015, 0.10, 0.015, 0.020, -0.015])
elif family == "reach_clip_upper":
# Deliberately outside the slave upper reach for the entire excursion.
center = center.copy()
center[3] = 0.85
delta = np.array([0.025, -0.020, 0.015, 0.10, 0.015, 0.020, -0.015])
elif family == "reach_boundary": elif family == "reach_boundary":
delta = 1.75 * delta delta = 1.75 * delta
elif family == "sew_degeneracy": elif family == "sew_degeneracy":
center = np.array([0.0, 0.0, 0.0, 0.12, 0.0, 0.0, 0.0]) center = np.array([0.0, 0.0, 0.0, 0.12, 0.0, 0.0, 0.0])
delta = np.array([0.0, 0.18, 0.0, 0.08, 0.0, -0.08, 0.0]) delta = np.array([0.0, 0.18, 0.0, 0.08, 0.0, -0.08, 0.0])
variation = specification.get("instance_variation", {})
if not isinstance(variation, Mapping):
raise ValueError("H1 instance_variation must be a mapping")
randomize = bool(variation.get("enabled", True))
center_std = float(variation.get("center_std_rad", 0.006))
delta_scale_range = np.asarray(
variation.get("delta_scale_range", [0.92, 1.08]), dtype=float
)
harmonic_range = np.asarray(
variation.get("harmonic_weight_range", [-0.08, 0.08]), dtype=float
)
jitter_mask = np.asarray(
variation.get("center_jitter_mask", [1, 1, 1, 1, 1, 1, 1]),
dtype=float,
)
if not np.isfinite(center_std) or center_std < 0.0:
raise ValueError("H1 center_std_rad must be finite and non-negative")
if (
delta_scale_range.shape != (2,)
or not np.all(np.isfinite(delta_scale_range))
or delta_scale_range[0] <= 0.0
or delta_scale_range[1] < delta_scale_range[0]
):
raise ValueError("H1 delta_scale_range must be two ordered positives")
if (
harmonic_range.shape != (2,)
or not np.all(np.isfinite(harmonic_range))
or harmonic_range[1] < harmonic_range[0]
or np.max(np.abs(harmonic_range)) >= 1.0
):
raise ValueError(
"H1 harmonic_weight_range must be ordered and inside (-1, 1)"
)
if jitter_mask.shape != (7,) or not np.all(np.isfinite(jitter_mask)):
raise ValueError("H1 center_jitter_mask must have seven finite entries")
if randomize:
seeds = trial.get("seeds")
if not isinstance(seeds, Mapping):
raise ValueError("H1 trial has no paired seed record")
trajectory_rng = generator_from_record(seeds, "trajectory")
center = center + center_std * jitter_mask * trajectory_rng.normal(size=7)
delta = delta * trajectory_rng.uniform(
delta_scale_range[0], delta_scale_range[1], size=7
)
harmonic_weight = float(
trajectory_rng.uniform(harmonic_range[0], harmonic_range[1])
)
else:
harmonic_weight = 0.0
phase = np.linspace(0.0, 1.0, sample_count) phase = np.linspace(0.0, 1.0, sample_count)
# One cosine excursion starts and ends at the same configuration with zero # One cosine excursion starts and ends at the same configuration with zero
# endpoint velocity, making discontinuities attributable to the mapper. # endpoint velocity, making discontinuities attributable to the mapper.
excursion = 0.5 - 0.5 * np.cos(2.0 * np.pi * phase) excursion = 0.5 - 0.5 * np.cos(2.0 * np.pi * phase)
excursion *= 1.0 + harmonic_weight * np.sin(2.0 * np.pi * phase)
trajectory = center[None, :] + excursion[:, None] * delta[None, :] trajectory = center[None, :] + excursion[:, None] * delta[None, :]
margin = 1e-4 margin = 1e-4
if np.any(trajectory < lower + margin) or np.any(trajectory > upper - margin): if np.any(trajectory < lower + margin) or np.any(trajectory > upper - margin):
@ -127,6 +223,74 @@ def _master_trajectory(
return trajectory return trajectory
class H1ValidityReason(str, Enum):
"""Primary reason a sample is unusable for smooth/differential mapping."""
NONE = "none"
REACH_CLIPPED_LOWER = "reach_clipped_lower"
REACH_CLIPPED_UPPER = "reach_clipped_upper"
JOINT_LIMIT_ACTIVE = "joint_limit_active"
GEOMETRY_DEGENERATE = "geometry_degenerate"
INVALID_INPUT = "invalid_input"
MASTER_LIMIT_VIOLATION = "master_limit_violation"
JOINT_LIMIT_VIOLATION = "joint_limit_violation"
TASK_TOLERANCE_EXCEEDED = "task_tolerance_exceeded"
SOLVER_NOT_CONVERGED = "solver_not_converged"
NUMERICAL_FAILURE = "numerical_failure"
LOW_MANIPULABILITY = "low_manipulability"
UNSPECIFIED_NONSMOOTH = "unspecified_nonsmooth"
_H1_FAILURE_TO_VALIDITY_REASON = {
RetargetingFailure.INVALID_INPUT: H1ValidityReason.INVALID_INPUT,
RetargetingFailure.MASTER_LIMIT_VIOLATION:
H1ValidityReason.MASTER_LIMIT_VIOLATION,
RetargetingFailure.DEGENERATE_GEOMETRY:
H1ValidityReason.GEOMETRY_DEGENERATE,
RetargetingFailure.JOINT_LIMIT_VIOLATION:
H1ValidityReason.JOINT_LIMIT_VIOLATION,
RetargetingFailure.TASK_TOLERANCE_EXCEEDED:
H1ValidityReason.TASK_TOLERANCE_EXCEEDED,
RetargetingFailure.SOLVER_NOT_CONVERGED:
H1ValidityReason.SOLVER_NOT_CONVERGED,
RetargetingFailure.NUMERICAL_FAILURE:
H1ValidityReason.NUMERICAL_FAILURE,
}
def _h1_validity_reason(
*,
smooth: bool,
failure: RetargetingFailure,
events: tuple[str, ...],
low_manipulability: bool,
) -> H1ValidityReason:
"""Classify branch validity without overwriting pose-solver failure."""
if smooth:
return H1ValidityReason.NONE
event_set = set(events)
if "reach_clipped_lower" in event_set:
return H1ValidityReason.REACH_CLIPPED_LOWER
if "reach_clipped_upper" in event_set:
return H1ValidityReason.REACH_CLIPPED_UPPER
if {
"master_shoulder_wrist_degenerate",
"master_arm_plane_degenerate",
"reference_axis_fallback",
"invalid_master_geometry",
} & event_set:
return H1ValidityReason.GEOMETRY_DEGENERATE
if "joint_limit_active" in event_set:
return H1ValidityReason.JOINT_LIMIT_ACTIVE
if failure is not RetargetingFailure.NONE:
return _H1_FAILURE_TO_VALIDITY_REASON.get(
failure, H1ValidityReason.UNSPECIFIED_NONSMOOTH
)
if low_manipulability:
return H1ValidityReason.LOW_MANIPULABILITY
return H1ValidityReason.UNSPECIFIED_NONSMOOTH
def _slave_swivel( def _slave_swivel(
model: pin.Model, model: pin.Model,
data: pin.Data, data: pin.Data,
@ -192,12 +356,31 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload:
solver_cost = np.empty(sample_count) solver_cost = np.empty(sample_count)
swivel = np.empty(sample_count) swivel = np.empty(sample_count)
degeneracy = np.zeros(sample_count, dtype=np.int8) degeneracy = np.zeros(sample_count, dtype=np.int8)
validity_reason = np.empty(sample_count, dtype=np.int16)
reach_clip_code = np.zeros(sample_count, dtype=np.int8)
joint_limit_active = np.zeros(sample_count, dtype=np.int8)
geometry_degenerate = np.zeros(sample_count, dtype=np.int8)
slave_min_singular_value = np.empty(sample_count)
slave_manipulability = np.empty(sample_count)
low_manipulability = np.zeros(sample_count, dtype=np.int8)
events: list[dict[str, Any]] = [] events: list[dict[str, Any]] = []
slave_data = models.slave.createData() slave_data = models.slave.createData()
shoulder_id = require_frame(models.slave, SLAVE_FRAMES["shoulder"]) shoulder_id = require_frame(models.slave, SLAVE_FRAMES["shoulder"])
elbow_id = require_frame(models.slave, SLAVE_FRAMES["elbow"]) elbow_id = require_frame(models.slave, SLAVE_FRAMES["elbow"])
wrist_id = require_frame(models.slave, SLAVE_FRAMES["wrist"]) wrist_id = require_frame(models.slave, SLAVE_FRAMES["wrist"])
low_manipulability_threshold = float(
_trajectory_spec(trial).get(
"low_manipulability_min_singular_threshold", 0.05
)
)
if (
not np.isfinite(low_manipulability_threshold)
or low_manipulability_threshold <= 0.0
):
raise ValueError(
"H1 low-manipulability singular-value threshold must be positive"
)
seed = None seed = None
previous_swivel = 0.0 previous_swivel = 0.0
for index, q_m in enumerate(q_master): for index, q_m in enumerate(q_master):
@ -223,6 +406,44 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload:
) )
swivel[index] = previous_swivel swivel[index] = previous_swivel
degeneracy[index] = int(is_degenerate) degeneracy[index] = int(is_degenerate)
jacobian = pin.computeFrameJacobian(
models.slave,
slave_data,
result.q_slave,
wrist_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED,
)
singular_values = np.linalg.svd(jacobian, compute_uv=False)
slave_min_singular_value[index] = float(singular_values[-1])
slave_manipulability[index] = float(np.prod(singular_values))
is_low_manipulability = bool(
singular_values[-1] <= low_manipulability_threshold
)
low_manipulability[index] = int(is_low_manipulability)
event_set = set(result.events)
if "reach_clipped_lower" in event_set:
reach_clip_code[index] = -1
elif "reach_clipped_upper" in event_set:
reach_clip_code[index] = 1
joint_limit_active[index] = int("joint_limit_active" in event_set)
geometry_degenerate[index] = int(
bool(
{
"master_shoulder_wrist_degenerate",
"master_arm_plane_degenerate",
"reference_axis_fallback",
"invalid_master_geometry",
}
& event_set
)
)
reason = _h1_validity_reason(
smooth=result.smooth,
failure=result.failure,
events=result.events,
low_manipulability=is_low_manipulability,
)
validity_reason[index] = _enum_code(reason)
if result.success: if result.success:
seed = result.q_slave.copy() seed = result.q_slave.copy()
for event in result.events: for event in result.events:
@ -231,6 +452,18 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload:
"sample_index": index, "sample_index": index,
"event": str(event), "event": str(event),
"failure_code": int(failure_code[index]), "failure_code": int(failure_code[index]),
"validity_reason": reason.value,
"validity_reason_code": int(validity_reason[index]),
}
)
if not result.smooth:
events.append(
{
"sample_index": index,
"event": "differential_invalid",
"failure_code": int(failure_code[index]),
"validity_reason": reason.value,
"validity_reason_code": int(validity_reason[index]),
} }
) )
@ -256,6 +489,13 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload:
"map_commanded_reset": np.zeros(sample_count, dtype=np.int8), "map_commanded_reset": np.zeros(sample_count, dtype=np.int8),
"map_degeneracy_transition": degeneracy, "map_degeneracy_transition": degeneracy,
"map_failure_code": failure_code, "map_failure_code": failure_code,
"map_validity_reason_code": validity_reason,
"map_reach_clip_code": reach_clip_code,
"map_joint_limit_active": joint_limit_active,
"map_geometry_degenerate": geometry_degenerate,
"map_slave_min_singular_value": slave_min_singular_value,
"map_slave_manipulability": slave_manipulability,
"map_low_manipulability": low_manipulability,
"map_solver_status": solver_status, "map_solver_status": solver_status,
"map_solver_iterations": iterations, "map_solver_iterations": iterations,
"map_runtime_s": runtime_s, "map_runtime_s": runtime_s,
@ -270,6 +510,21 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload:
"failure_enum": { "failure_enum": {
member.value: _enum_code(member) for member in RetargetingFailure member.value: _enum_code(member) for member in RetargetingFailure
}, },
"validity_reason_enum": {
member.value: _enum_code(member) for member in H1ValidityReason
},
"reach_clip_code": {"lower": -1, "none": 0, "upper": 1},
"low_manipulability_definition": {
"quantity": (
"minimum singular value of the LOCAL_WORLD_ALIGNED "
"slave-wrist geometric Jacobian"
),
"comparison": "<=",
"threshold": low_manipulability_threshold,
},
"trajectory_instance_hash": stable_hash(
q_master, prefix="h1-master-trajectory-instance"
),
"trajectory_family": _trajectory_spec(trial).get("family", "nominal"), "trajectory_family": _trajectory_spec(trial).get("family", "nominal"),
}, },
) )
@ -281,6 +536,54 @@ def _orthogonal(rng: np.random.Generator, size: int) -> np.ndarray:
return q * signs return q * signs
def _h2_data_seed_group(
trial: Mapping[str, Any],
) -> tuple[str, dict[str, Any], dict[str, list[int]]]:
"""Return H2 data streams independent of estimator tuning choices.
``pair_id`` intentionally includes every factor cell, which is the right
default for most studies but would give each characteristic-length or
damping candidate a different synthetic data realization. H2 calibration
instead defines a second, explicitly recorded grouping unit from only the
physical perturbation factors. Every estimator candidate in that group
therefore receives byte-identical truth, model-error, sensor-noise, and
trajectory streams.
"""
data_root_seed = int(_factor(trial, "h2_data_root_seed", 0))
if data_root_seed < 0:
raise ValueError("h2_data_root_seed must be non-negative")
physical_factors = {
"min_scaled_singular": float(
_factor(trial, "min_scaled_singular", 0.05)
),
"model_error_std": float(_factor(trial, "model_error_std", 0.01)),
"torque_noise_std_Nm": float(
_factor(trial, "torque_noise_std_Nm", 0.01)
),
"truth_characteristic_length_m": float(
_factor(trial, "truth_characteristic_length_m", 0.30)
),
}
basis = {
"study_id": str(trial.get("study_id", "")),
"split": str(trial.get("split", "")),
"data_root_seed": data_root_seed,
"trajectory": dict(_trajectory_spec(trial)),
"replicate": int(trial.get("replicate", 0)),
"physical_factors": physical_factors,
}
group_id = (
"h2-data-"
+ stable_hash(basis, prefix="h2-physical-data-group")[:16]
)
seed_record = named_seed_record(
data_root_seed,
basis,
("trajectory", "truth_model", "model_error", "sensor"),
)
return group_id, basis, seed_record
def execute_h2_synthetic(trial: Mapping[str, Any]) -> TrialPayload: def execute_h2_synthetic(trial: Mapping[str, Any]) -> TrialPayload:
"""Run a paired, truth/estimator-separated H2 sensitivity trial.""" """Run a paired, truth/estimator-separated H2 sensitivity trial."""
method_id = _method_id(trial) method_id = _method_id(trial)
@ -298,18 +601,35 @@ def execute_h2_synthetic(trial: Mapping[str, Any]) -> TrialPayload:
min_singular = float(_factor(trial, "min_scaled_singular", 0.05)) min_singular = float(_factor(trial, "min_scaled_singular", 0.05))
noise_std = float(_factor(trial, "torque_noise_std_Nm", 0.01)) noise_std = float(_factor(trial, "torque_noise_std_Nm", 0.01))
model_error_std = float(_factor(trial, "model_error_std", 0.01)) model_error_std = float(_factor(trial, "model_error_std", 0.01))
truth_characteristic_length = float(
_factor(trial, "truth_characteristic_length_m", 0.30)
)
operational_singular_threshold = float(
_factor(trial, "operational_min_scaled_singular", 0.05)
)
if min_singular < 0.0 or noise_std < 0.0 or model_error_std < 0.0: if min_singular < 0.0 or noise_std < 0.0 or model_error_std < 0.0:
raise ValueError("H2 perturbation factors must be non-negative") raise ValueError("H2 perturbation factors must be non-negative")
if truth_characteristic_length <= 0.0:
raise ValueError("truth_characteristic_length_m must be positive")
if operational_singular_threshold <= 0.0:
raise ValueError(
"operational_min_scaled_singular must be positive"
)
model_rng = generator_from_record(trial["seeds"], "model") data_group_id, data_group_basis, data_seeds = _h2_data_seed_group(trial)
sensor_rng = generator_from_record(trial["seeds"], "sensor") truth_model_rng = generator_from_record(data_seeds, "truth_model")
trajectory_rng = generator_from_record(trial["seeds"], "trajectory") model_error_rng = generator_from_record(data_seeds, "model_error")
u = _orthogonal(model_rng, 6) sensor_rng = generator_from_record(data_seeds, "sensor")
v = _orthogonal(model_rng, 7) trajectory_rng = generator_from_record(data_seeds, "trajectory")
u = _orthogonal(truth_model_rng, 6)
v = _orthogonal(truth_model_rng, 7)
singular = np.array([1.6, 1.25, 0.95, 0.65, 0.35, min_singular]) singular = np.array([1.6, 1.25, 0.95, 0.65, 0.35, min_singular])
base_scaled_truth = u @ np.diag(singular) @ v[:6, :] base_scaled_truth = u @ np.diag(singular) @ v[:6, :]
inverse_scaling = np.diag( # The physical truth is generated with a fixed reference length. The
[characteristic_length] * 3 + [1.0] * 3 # scanned characteristic length belongs only to the estimator scaling;
# otherwise changing ell would silently change the ground-truth Jacobian.
truth_inverse_scaling = np.diag(
[truth_characteristic_length] * 3 + [1.0] * 3
) )
phase = np.linspace(0.0, 2.0 * np.pi, count, endpoint=False) phase = np.linspace(0.0, 2.0 * np.pi, count, endpoint=False)
@ -334,7 +654,16 @@ def execute_h2_synthetic(trial: Mapping[str, Any]) -> TrialPayload:
friction=friction, friction=friction,
characteristic_length_m=characteristic_length, characteristic_length_m=characteristic_length,
damping=damping, damping=damping,
calibration_id="g0c-synthetic-frozen-v1", calibration_id=(
"g0c-synthetic-candidate-"
+ stable_hash(
{
"characteristic_length_m": characteristic_length,
"damping": damping,
},
prefix="h2-calibration-candidate",
)[:16]
),
) )
solver = ( solver = (
UndampedSVDSolver(characteristic_length) UndampedSVDSolver(characteristic_length)
@ -354,25 +683,31 @@ def execute_h2_synthetic(trial: Mapping[str, Any]) -> TrialPayload:
singular_log = np.empty((count, 6)) singular_log = np.empty((count, 6))
rank = np.empty(count, dtype=np.int16) rank = np.empty(count, dtype=np.int16)
status = np.empty(count, dtype=np.int16) status = np.empty(count, dtype=np.int16)
rank_threshold = np.empty(count)
condition_number = np.empty(count)
numerical_rank_deficient = np.empty(count, dtype=np.int8)
operationally_ill_conditioned = np.empty(count, dtype=np.int8)
truth_jacobian = np.empty((count, 42)) truth_jacobian = np.empty((count, 42))
estimator_jacobian = np.empty((count, 42)) estimator_jacobian = np.empty((count, 42))
residual_raw = np.empty((count, 7)) residual_raw = np.empty((count, 7))
residual_corrected = np.empty((count, 7)) residual_corrected = np.empty((count, 7))
sensor_noise = np.empty((count, 7))
for index in range(count): for index in range(count):
smooth_change = 0.015 * np.sin(phase[index]) smooth_change = 0.015 * np.sin(phase[index])
J_truth = inverse_scaling @ ( J_truth = truth_inverse_scaling @ (
base_scaled_truth base_scaled_truth
+ smooth_change * model_rng.normal(size=(6, 7)) + smooth_change * truth_model_rng.normal(size=(6, 7))
) )
J_estimator = J_truth + inverse_scaling @ ( J_estimator = J_truth + truth_inverse_scaling @ (
model_error_std * model_rng.normal(size=(6, 7)) model_error_std * model_error_rng.normal(size=(6, 7))
) )
interaction = J_truth.T @ wrench_reference[index] interaction = J_truth.T @ wrench_reference[index]
sensor_noise[index] = sensor_rng.normal(0.0, noise_std, 7)
measured = ( measured = (
interaction interaction
+ bias + bias
+ friction.torque(qd[index]) + friction.torque(qd[index])
+ sensor_rng.normal(0.0, noise_std, 7) + sensor_noise[index]
) )
estimate = estimator.estimate( estimate = estimator.estimate(
J_estimator, J_estimator,
@ -385,6 +720,13 @@ def execute_h2_synthetic(trial: Mapping[str, Any]) -> TrialPayload:
singular_log[index] = estimate.solve.singular_values singular_log[index] = estimate.solve.singular_values
rank[index] = estimate.solve.rank rank[index] = estimate.solve.rank
status[index] = _enum_code(estimate.solve.status) status[index] = _enum_code(estimate.solve.status)
rank_threshold[index] = estimate.solve.rank_threshold
condition_number[index] = estimate.solve.condition_number
numerical_rank_deficient[index] = int(estimate.solve.rank < 6)
operationally_ill_conditioned[index] = int(
estimate.solve.singular_values[-1]
<= operational_singular_threshold
)
truth_jacobian[index] = J_truth.reshape(-1) truth_jacobian[index] = J_truth.reshape(-1)
estimator_jacobian[index] = J_estimator.reshape(-1) estimator_jacobian[index] = J_estimator.reshape(-1)
residual_raw[index] = estimate.residual.raw_residual_nm residual_raw[index] = estimate.residual.raw_residual_nm
@ -402,8 +744,18 @@ def execute_h2_synthetic(trial: Mapping[str, Any]) -> TrialPayload:
"scaled_singular_values": singular_log, "scaled_singular_values": singular_log,
"solver_rank": rank, "solver_rank": rank,
"solver_status": status, "solver_status": status,
"solver_numerical_rank_threshold": rank_threshold,
"solver_condition_number": condition_number,
"solver_numerical_rank_deficient": numerical_rank_deficient,
"solver_operationally_ill_conditioned": (
operationally_ill_conditioned
),
"operational_min_scaled_singular_threshold": np.full(
count, operational_singular_threshold
),
"tau_residual_raw": residual_raw, "tau_residual_raw": residual_raw,
"tau_residual_corrected": residual_corrected, "tau_residual_corrected": residual_corrected,
"sensor_noise_Nm": sensor_noise,
}, },
metadata={ metadata={
"evidence_scope": ( "evidence_scope": (
@ -412,12 +764,30 @@ def execute_h2_synthetic(trial: Mapping[str, Any]) -> TrialPayload:
"method_id": method_id, "method_id": method_id,
"truth_estimator_models_separated": True, "truth_estimator_models_separated": True,
"calibration_id": calibration.calibration_id, "calibration_id": calibration.calibration_id,
"h2_data_group_id": data_group_id,
"h2_data_group_basis": data_group_basis,
"h2_data_seed_record": data_seeds,
"h2_data_seed_record_hash": stable_hash(
data_seeds, prefix="h2-data-seed-record"
),
"h2_data_root_seed": int(
_factor(trial, "h2_data_root_seed", 0)
),
"truth_characteristic_length_m": truth_characteristic_length,
"operational_ill_conditioning_definition": {
"quantity": "minimum singular value of estimator-scaled Jacobian",
"comparison": "<=",
"threshold": operational_singular_threshold,
"numerical_rank_is_reported_separately": True,
},
}, },
) )
def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload: def _bilateral_scenario_and_config(
"""Run one paired H3/H4 rigid-body trial with a frozen network trace.""" trial: Mapping[str, Any],
) -> tuple[Any, SimulationConfig]:
"""Build and validate the effective bilateral scenario/configuration."""
method_id = _method_id(trial) method_id = _method_id(trial)
scenarios = {scenario.key: scenario for scenario in SCENARIOS} scenarios = {scenario.key: scenario for scenario in SCENARIOS}
if method_id not in scenarios: if method_id not in scenarios:
@ -436,15 +806,113 @@ def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload:
contact_probe_fraction=float( contact_probe_fraction=float(
trajectory.get("contact_probe_fraction", 0.0) trajectory.get("contact_probe_fraction", 0.0)
), ),
feedback_delay_s=float(_factor(trial, "return_delay_s", 0.04)), feedback_delay_s=float(
forward_delay_s=float(_factor(trial, "forward_delay_s", 0.0)), _profiled_factor(
return_jitter_s=float(_factor(trial, "return_jitter_s", 0.0)), trial,
forward_jitter_s=float(_factor(trial, "forward_jitter_s", 0.0)), "return_delay_s",
return_packet_loss=float(_factor(trial, "return_packet_loss", 0.0)), 0.04,
forward_packet_loss=float(_factor(trial, "forward_packet_loss", 0.0)), profile_name="network_profile",
)
),
forward_delay_s=float(
_profiled_factor(
trial,
"forward_delay_s",
0.0,
profile_name="network_profile",
)
),
return_jitter_s=float(
_profiled_factor(
trial,
"return_jitter_s",
0.0,
profile_name="network_profile",
)
),
forward_jitter_s=float(
_profiled_factor(
trial,
"forward_jitter_s",
0.0,
profile_name="network_profile",
)
),
return_packet_loss=float(
_profiled_factor(
trial,
"return_packet_loss",
0.0,
profile_name="network_profile",
)
),
forward_packet_loss=float(
_profiled_factor(
trial,
"forward_packet_loss",
0.0,
profile_name="network_profile",
)
),
forward_timeout_s=float(
_profiled_factor(
trial,
"forward_timeout_s",
0.20,
profile_name="network_profile",
)
),
return_timeout_s=float(
_profiled_factor(
trial,
"return_timeout_s",
0.20,
profile_name="network_profile",
)
),
feedback_strength=float(
_profiled_factor(
trial,
"feedback_strength",
0.50,
profile_name="haptic_profile",
)
),
energy_min=float(
_profiled_factor(
trial,
"energy_min",
0.05,
profile_name="haptic_profile",
)
),
energy_max=float(
_profiled_factor(
trial,
"energy_max",
0.055,
profile_name="haptic_profile",
)
),
energy_initial=float(
_profiled_factor(
trial,
"energy_initial",
0.05,
profile_name="haptic_profile",
)
),
wall_stiffness=float(_factor(trial, "wall_stiffness", 800.0)), wall_stiffness=float(_factor(trial, "wall_stiffness", 800.0)),
wall_damping=float(_factor(trial, "wall_damping", 45.0)), wall_damping=float(_factor(trial, "wall_damping", 45.0)),
) )
config.validate()
return scenario, config
def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload:
"""Run one paired H3/H4 rigid-body trial with a frozen network trace."""
scenario, config = _bilateral_scenario_and_config(trial)
trajectory = _trajectory_spec(trial)
models = load_models(add_simulated_tcp=True) models = load_models(add_simulated_tcp=True)
mapper = build_mapper(models) mapper = build_mapper(models)
wall, wall_metadata, q_slave_start = make_wall(config, models, mapper) wall, wall_metadata, q_slave_start = make_wall(config, models, mapper)
@ -473,6 +941,19 @@ def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload:
samples["energy_before_J"] = samples["energy_before"].copy() samples["energy_before_J"] = samples["energy_before"].copy()
samples["energy_after_J"] = samples["tank_energy"].copy() samples["energy_after_J"] = samples["tank_energy"].copy()
samples["energy_preclip_J"] = samples["energy_preclip"].copy() samples["energy_preclip_J"] = samples["energy_preclip"].copy()
sample_count = samples["time"].shape[0]
samples["configured_feedback_strength"] = np.full(
sample_count, config.feedback_strength
)
samples["configured_energy_min_J"] = np.full(
sample_count, config.energy_min
)
samples["configured_energy_max_J"] = np.full(
sample_count, config.energy_max
)
samples["configured_energy_initial_J"] = np.full(
sample_count, config.energy_initial
)
return TrialPayload( return TrialPayload(
samples=samples, samples=samples,
events=(), events=(),
@ -486,6 +967,22 @@ def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload:
"supervisor": scenario.supervisor, "supervisor": scenario.supervisor,
"map_policy": scenario.map_policy, "map_policy": scenario.map_policy,
}, },
"effective_haptic_config": {
"feedback_strength": config.feedback_strength,
"energy_min_J": config.energy_min,
"energy_max_J": config.energy_max,
"energy_initial_J": config.energy_initial,
},
"effective_network_config": {
"forward_delay_s": config.forward_delay_s,
"return_delay_s": config.feedback_delay_s,
"forward_jitter_s": config.forward_jitter_s,
"return_jitter_s": config.return_jitter_s,
"forward_packet_loss": config.forward_packet_loss,
"return_packet_loss": config.return_packet_loss,
"forward_timeout_s": config.forward_timeout_s,
"return_timeout_s": config.return_timeout_s,
},
"wall": wall_metadata, "wall": wall_metadata,
"online_metrics_are_diagnostic_only": result.metrics, "online_metrics_are_diagnostic_only": result.metrics,
}, },

98
code/test/test_bilateral_calibration_v2.py Normal file
View File

@ -0,0 +1,98 @@
#!/usr/bin/env python3
"""Configuration propagation and bounded-size contracts for bilateral v2."""
from pathlib import Path
import sys
import unittest
CODE_ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(CODE_ROOT))
from experiments.executors import _bilateral_scenario_and_config # noqa: E402
from experiments.plan import build_trial_plan, load_document # noqa: E402
from experiments.rng import named_seed_record # noqa: E402
CONFIG_ROOT = CODE_ROOT / "config" / "experiments"
def bilateral_trial():
return {
"method": {"method_id": "proposed_energy"},
"trajectory": {
"trajectory_id": "unit_contact",
"family": "contact_roundtrip",
"duration_s": 0.8,
"contact_probe_fraction": 0.01,
},
"factors": {
"map_policy": "source_stamped",
"network_profile": {
"forward_delay_s": 0.04,
"return_delay_s": 0.08,
"forward_jitter_s": 0.003,
"return_jitter_s": 0.004,
"forward_packet_loss": 0.01,
"return_packet_loss": 0.02,
},
"haptic_profile": {
"feedback_strength": 0.25,
"energy_min": 0.0,
"energy_max": 0.3,
"energy_initial": 0.1,
},
# A direct factor must override the coupled profile.
"forward_delay_s": 0.02,
"energy_initial": 0.12,
},
"seeds": named_seed_record(7, {"test": "bilateral-v2"}),
}
class BilateralCalibrationV2Test(unittest.TestCase):
def test_haptic_and_network_factors_reach_simulation_config(self):
scenario, config = _bilateral_scenario_and_config(bilateral_trial())
self.assertEqual(scenario.map_policy, "source_stamped")
self.assertEqual(config.feedback_strength, 0.25)
self.assertEqual(config.energy_min, 0.0)
self.assertEqual(config.energy_max, 0.3)
self.assertEqual(config.energy_initial, 0.12)
self.assertEqual(config.forward_delay_s, 0.02)
self.assertEqual(config.feedback_delay_s, 0.08)
self.assertEqual(config.forward_jitter_s, 0.003)
self.assertEqual(config.return_jitter_s, 0.004)
self.assertEqual(config.forward_packet_loss, 0.01)
self.assertEqual(config.return_packet_loss, 0.02)
def test_v2_grids_are_directional_and_bounded(self):
energy = build_trial_plan(
load_document(CONFIG_ROOT / "bilateral_calibration_v2_energy.json")
)
network = build_trial_plan(
load_document(CONFIG_ROOT / "bilateral_calibration_v2_network.json")
)
self.assertEqual(energy["pair_count"], 36)
self.assertEqual(energy["trial_count"], 36)
self.assertEqual(network["pair_count"], 12)
self.assertEqual(network["trial_count"], 36)
profiles = {
trial["factors"]["network_profile"]["profile_id"]
for trial in network["trials"]
}
self.assertEqual(
profiles,
{
"nominal",
"return_delay_80ms",
"forward_delay_40ms",
"asymmetric_delay",
"asymmetric_delay_plus_jitter",
"asymmetric_delay_plus_loss",
},
)
if __name__ == "__main__":
unittest.main()

104
code/test/test_experiment_metrics.py
View File

@ -12,10 +12,13 @@ CODE_ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(CODE_ROOT)) sys.path.insert(0, str(CODE_ROOT))
from analysis.metrics import ( # noqa: E402 from analysis.metrics import ( # noqa: E402
MetricError,
audit_h4_energy, audit_h4_energy,
compute_h1_composite, compute_h1_composite,
compute_h2_wrench_metrics, compute_h2_wrench_metrics,
compute_h3_power_mismatch, compute_h3_power_mismatch,
compute_bilateral_diagnostics,
derive_trial_metrics,
) )
@ -47,9 +50,35 @@ class IndependentExperimentMetricsTest(unittest.TestCase):
def test_h2_separates_force_and_moment_rmse(self): def test_h2_separates_force_and_moment_rmse(self):
reference = np.zeros((3, 6)) reference = np.zeros((3, 6))
estimate = np.tile([3.0, 4.0, 0.0, 0.0, 0.0, 2.0], (3, 1)) estimate = np.tile([3.0, 4.0, 0.0, 0.0, 0.0, 2.0], (3, 1))
metrics = compute_h2_wrench_metrics(estimate, reference) 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_force_rmse_N"], 5.0)
self.assertAlmostEqual(metrics["h2_moment_rmse_Nm"], 2.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): 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]]) torque = np.array([[1.0, 2.0], [-2.0, 1.0], [0.5, -0.5]])
@ -62,6 +91,25 @@ class IndependentExperimentMetricsTest(unittest.TestCase):
dt=0.002, dt=0.002,
) )
self.assertAlmostEqual(metrics["h3_epsilon_P_act"], 0.0) 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): def test_h4_reconstructs_floor_and_projection_distortion(self):
metrics = audit_h4_energy( metrics = audit_h4_energy(
@ -101,6 +149,60 @@ class IndependentExperimentMetricsTest(unittest.TestCase):
) )
self.assertGreater(metrics["h4_software_preclip_max_error_J"], 0.0) 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__": if __name__ == "__main__":
unittest.main() unittest.main()

173
code/test/test_h1_calibration_v2.py Normal file
View File

@ -0,0 +1,173 @@
#!/usr/bin/env python3
"""H1 calibration-v2 seeded pairing and audit-semantic contracts."""
from __future__ import annotations
from copy import deepcopy
from pathlib import Path
import sys
import unittest
import numpy as np
CODE_ROOT = Path(__file__).resolve().parents[1]
if str(CODE_ROOT) not in sys.path:
sys.path.insert(0, str(CODE_ROOT))
from analysis.metrics import derive_trial_metrics # noqa: E402
from core.model_contract import ( # noqa: E402
MASTER_JOINT_NAMES,
finite_joint_limits,
load_models,
)
from experiments.executors import ( # noqa: E402
H1ValidityReason,
_master_trajectory,
execute_h1_retargeting,
)
from experiments.plan import build_trial_plan, load_document # noqa: E402
CONFIG_PATH = (
CODE_ROOT / "config" / "experiments" / "h1_calibration_v2.json"
)
METRIC_CONFIG_PATH = (
CODE_ROOT / "config" / "experiments" / "metrics_h1_calibration_v2.json"
)
class H1CalibrationV2Test(unittest.TestCase):
@classmethod
def setUpClass(cls) -> None:
cls.models = load_models(add_simulated_tcp=True)
cls.lower, cls.upper = finite_joint_limits(
cls.models.master, MASTER_JOINT_NAMES
)
cls.plan = build_trial_plan(load_document(CONFIG_PATH))
cls.metric_configuration = load_document(METRIC_CONFIG_PATH)
def _trajectory(self, trial):
return _master_trajectory(
trial, lower=self.lower, upper=self.upper
)
def _sew_trial(self, family: str, sample_count: int = 7):
trial = next(
item
for item in self.plan["trials"]
if item["method"]["method_id"] == "sew"
and item["trajectory"]["family"] == family
and item["replicate"] == 0
)
short_trial = deepcopy(trial)
short_trial["trajectory"]["sample_count"] = sample_count
return short_trial
def test_v2_plan_is_bounded_seeded_and_strictly_paired(self) -> None:
self.assertEqual(self.plan["pair_count"], 15)
self.assertEqual(self.plan["trial_count"], 60)
by_pair = {}
by_trajectory = {}
for trial in self.plan["trials"]:
trajectory = self._trajectory(trial)
self.assertTrue(np.all(trajectory > self.lower))
self.assertTrue(np.all(trajectory < self.upper))
by_pair.setdefault(trial["pair_id"], []).append(trajectory)
by_trajectory.setdefault(
trial["trajectory"]["trajectory_id"], {}
).setdefault(trial["replicate"], trajectory)
for trajectories in by_pair.values():
self.assertEqual(len(trajectories), 4)
for candidate in trajectories[1:]:
np.testing.assert_array_equal(candidate, trajectories[0])
for instances in by_trajectory.values():
self.assertEqual(set(instances), {0, 1, 2})
self.assertFalse(
np.array_equal(instances[0], instances[1]),
"replicates must not be deterministic pseudo-replicates",
)
self.assertFalse(np.array_equal(instances[1], instances[2]))
def test_valid_low_manipulability_is_not_reach_clipped(self) -> None:
payload = execute_h1_retargeting(
self._sew_trial("low_manipulability_valid")
)
samples = payload.samples
self.assertTrue(np.all(samples["map_pose_success"]))
self.assertTrue(np.all(samples["map_differential_valid"]))
self.assertTrue(np.any(samples["map_low_manipulability"]))
self.assertTrue(np.all(samples["map_reach_clip_code"] == 0))
self.assertTrue(
np.all(
samples["map_validity_reason_code"]
== list(H1ValidityReason).index(H1ValidityReason.NONE)
)
)
def test_reach_clip_has_reason_despite_pose_success(self) -> None:
payload = execute_h1_retargeting(
self._sew_trial("reach_clip_upper")
)
samples = payload.samples
upper_code = list(H1ValidityReason).index(
H1ValidityReason.REACH_CLIPPED_UPPER
)
self.assertTrue(np.all(samples["map_pose_success"]))
self.assertTrue(np.all(samples["map_differential_valid"] == 0))
self.assertTrue(np.all(samples["map_failure_code"] == 0))
self.assertTrue(np.all(samples["map_reach_clip_code"] == 1))
self.assertTrue(
np.all(samples["map_validity_reason_code"] == upper_code)
)
invalid_events = [
event
for event in payload.events
if event["event"] == "differential_invalid"
]
self.assertEqual(len(invalid_events), len(samples["sample_index"]))
self.assertTrue(
all(
event["validity_reason"]
== H1ValidityReason.REACH_CLIPPED_UPPER.value
for event in invalid_events
)
)
metrics = derive_trial_metrics(
samples, self.metric_configuration
)
self.assertEqual(metrics["h1_reach_clip_upper_fraction"], 1.0)
self.assertEqual(metrics["h1_reach_clip_fraction"], 1.0)
self.assertEqual(
metrics["h1_primary_invalid_reason"],
H1ValidityReason.REACH_CLIPPED_UPPER.value,
)
self.assertEqual(
metrics["h1_validity_reason_histogram"],
{H1ValidityReason.REACH_CLIPPED_UPPER.value: 7},
)
self.assertEqual(metrics["h1_unexplained_invalid_fraction"], 0.0)
self.assertGreater(
metrics["h1_min_slave_min_singular_value"], 0.0
)
def test_metric_reason_labels_match_executor_enum(self) -> None:
self.assertEqual(
self.metric_configuration["h1"]["validity_reason_labels"],
[member.value for member in H1ValidityReason],
)
def test_every_invalid_sample_has_a_nonzero_audit_reason(self) -> None:
payload = execute_h1_retargeting(self._sew_trial("joint_limit"))
samples = payload.samples
invalid = np.asarray(samples["map_differential_valid"]) == 0
self.assertTrue(np.any(invalid))
self.assertTrue(
np.all(np.asarray(samples["map_validity_reason_code"])[invalid] != 0)
)
if __name__ == "__main__":
unittest.main()

232
code/test/test_h2_synthetic_pairing.py Normal file
View File

@ -0,0 +1,232 @@
#!/usr/bin/env python3
"""H2 data pairing and conditioning-stratum regression tests."""
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 experiments.executors import execute_h2_synthetic # noqa: E402
from experiments.plan import build_trial_plan, load_document # noqa: E402
def h2_pairing_specification():
return {
"study_id": "h2_pairing_contract",
"split": "calibration",
"root_seed": 37,
"replicates": 2,
"methods": ["scaled_dls", "undamped_svd"],
"trajectories": [
{
"trajectory_id": "short_dynamic_wrench",
"family": "synthetic_dynamic",
"sample_count": 12,
}
],
"factors": {
"characteristic_length_m": [0.2, 0.4],
"damping": [0.02, 0.05],
"h2_data_root_seed": [1701],
"min_scaled_singular": [0.02],
"model_error_std": [0.02],
"operational_min_scaled_singular": [0.05],
"torque_noise_std_Nm": [0.002, 0.02],
"truth_characteristic_length_m": [0.3],
},
}
def select_trial(
plan,
*,
replicate,
method,
characteristic_length,
damping,
noise,
):
matches = [
trial
for trial in plan["trials"]
if trial["replicate"] == replicate
and trial["method"]["method_id"] == method
and trial["factors"]["characteristic_length_m"]
== characteristic_length
and trial["factors"]["damping"] == damping
and trial["factors"]["torque_noise_std_Nm"] == noise
]
if len(matches) != 1:
raise AssertionError(f"expected one H2 trial, found {len(matches)}")
return matches[0]
class H2SyntheticPairingTest(unittest.TestCase):
def test_algorithm_candidates_reuse_identical_physical_data(self):
plan = build_trial_plan(h2_pairing_specification())
first_trial = select_trial(
plan,
replicate=0,
method="scaled_dls",
characteristic_length=0.2,
damping=0.02,
noise=0.002,
)
second_trial = select_trial(
plan,
replicate=0,
method="undamped_svd",
characteristic_length=0.4,
damping=0.05,
noise=0.002,
)
# The general experiment pair changes with ell/damping. H2's explicit
# physical-data group must still bind both candidates to the same data.
self.assertNotEqual(first_trial["pair_id"], second_trial["pair_id"])
self.assertNotEqual(first_trial["seeds"], second_trial["seeds"])
first = execute_h2_synthetic(first_trial)
second = execute_h2_synthetic(second_trial)
self.assertEqual(
first.metadata["h2_data_group_id"],
second.metadata["h2_data_group_id"],
)
self.assertEqual(
first.metadata["h2_data_seed_record"],
second.metadata["h2_data_seed_record"],
)
self.assertNotEqual(
first.metadata["calibration_id"],
second.metadata["calibration_id"],
"scanned estimator candidates must not share a frozen-looking ID",
)
for field in (
"wrench_reference",
"qd_slave",
"jacobian_truth",
"jacobian_estimator",
"sensor_noise_Nm",
"tau_residual_raw",
):
np.testing.assert_array_equal(
first.samples[field], second.samples[field]
)
self.assertFalse(
np.array_equal(
first.samples["wrench_estimated"],
second.samples["wrench_estimated"],
)
)
def test_replicates_and_physical_factor_cells_get_distinct_data(self):
plan = build_trial_plan(h2_pairing_specification())
baseline = execute_h2_synthetic(
select_trial(
plan,
replicate=0,
method="scaled_dls",
characteristic_length=0.2,
damping=0.02,
noise=0.002,
)
)
next_replicate = execute_h2_synthetic(
select_trial(
plan,
replicate=1,
method="scaled_dls",
characteristic_length=0.2,
damping=0.02,
noise=0.002,
)
)
different_noise = execute_h2_synthetic(
select_trial(
plan,
replicate=0,
method="scaled_dls",
characteristic_length=0.2,
damping=0.02,
noise=0.02,
)
)
self.assertNotEqual(
baseline.metadata["h2_data_group_id"],
next_replicate.metadata["h2_data_group_id"],
)
self.assertNotEqual(
baseline.metadata["h2_data_group_id"],
different_noise.metadata["h2_data_group_id"],
)
self.assertFalse(
np.array_equal(
baseline.samples["jacobian_truth"],
next_replicate.samples["jacobian_truth"],
)
)
self.assertFalse(
np.array_equal(
baseline.samples["tau_residual_raw"],
different_noise.samples["tau_residual_raw"],
)
)
def test_operational_flag_is_separate_from_numerical_rank(self):
specification = h2_pairing_specification()
specification["replicates"] = 1
specification["factors"]["characteristic_length_m"] = [0.3]
specification["factors"]["damping"] = [0.03]
specification["factors"]["torque_noise_std_Nm"] = [0.01]
specification["factors"]["operational_min_scaled_singular"] = [10.0]
trial = build_trial_plan(specification)["trials"][0]
payload = execute_h2_synthetic(trial)
np.testing.assert_array_equal(
payload.samples["solver_numerical_rank_deficient"],
np.zeros(12, dtype=np.int8),
)
np.testing.assert_array_equal(
payload.samples["solver_operationally_ill_conditioned"],
np.ones(12, dtype=np.int8),
)
np.testing.assert_array_equal(
payload.samples["operational_min_scaled_singular_threshold"],
np.full(12, 10.0),
)
definition = payload.metadata[
"operational_ill_conditioning_definition"
]
self.assertTrue(definition["numerical_rank_is_reported_separately"])
def test_v2_scan_has_sixteen_physical_groups_and_576_trials(self):
specification = load_document(
CODE_ROOT / "config" / "experiments" / "h2_calibration_v2.json"
)
plan = build_trial_plan(specification)
self.assertEqual(plan["pair_count"], 144)
self.assertEqual(plan["trial_count"], 576)
group_ids = set()
for trial in plan["trials"]:
payload = execute_h2_synthetic(
{
**trial,
"trajectory": {
**trial["trajectory"],
"sample_count": 8,
},
}
)
group_ids.add(payload.metadata["h2_data_group_id"])
if len(group_ids) == 16:
break
self.assertEqual(len(group_ids), 16)
if __name__ == "__main__":
unittest.main()

12
code/test/test_paper_source_data.py
View File

@ -28,7 +28,11 @@ def wrench_executor(_trial):
samples={ samples={
"wrench_estimated": estimate, "wrench_estimated": estimate,
"wrench_reference": reference, "wrench_reference": reference,
} },
metadata={
"h2_data_group_id": "h2-data-test-group",
"h2_data_seed_record_hash": "abc123",
},
) )
@ -67,6 +71,12 @@ class PaperSourceDataTest(unittest.TestCase):
table = list(csv.DictReader(stream)) table = list(csv.DictReader(stream))
self.assertEqual(len(table), 2) self.assertEqual(len(table), 2)
self.assertIn("h2_force_rmse_N", table[0]) self.assertIn("h2_force_rmse_N", table[0])
self.assertEqual(
table[0]["h2_data_group_id"], "h2-data-test-group"
)
self.assertEqual(
table[0]["h2_data_seed_record_hash"], "abc123"
)
self.assertTrue((batch / "paper" / "artifact_manifest.json").is_file()) self.assertTrue((batch / "paper" / "artifact_manifest.json").is_file())
def test_metric_family_method_filter_prevents_mixed_supervisors(self): def test_metric_family_method_filter_prevents_mixed_supervisors(self):

67
docs/calibration/CALIBRATION_POLICY.md Normal file
View File

@ -0,0 +1,67 @@
# Calibration and Evidence-Locking Policy
This document defines when a numerical setting may move from development to a
locked experiment. Calibration output is engineering evidence, not a paper
result.
## Evidence states
Every setting or implementation contract receives one of four states:
1. **Verified contract**: a deterministic identity or accounting invariant has
passed an independent reconstruction from stored arrays.
2. **Provisional numerical setting**: calibration supports using the setting in
another calibration or pilot, but not in a locked experiment.
3. **Frozen setting**: the value, selection rule, analysis configuration, source
commit, and admissible operating envelope are fixed before locked data are
inspected.
4. **Not freeze-ready**: the calibration design is confounded, lacks the
required comparison, or has an unacceptable safety/transparency trade-off.
## Freeze gate
A setting can be frozen only if all of the following are true:
- trials are generated from a clean, immutable Git commit;
- paired methods receive identical trajectory, model, sensor, and network
inputs where the hypothesis requires pairing;
- calibration instances are genuinely distinct rather than timing-only
repetitions;
- failure, invalidity, timeout, clipping, intervention, and exclusion reasons
are machine-readable and retained;
- the primary endpoint is numerically well-defined for the selected stratum;
- the selected value is supported by a parameter scan or an external physical
calibration, not by a single untested level;
- tail behavior and failed trials are reviewed before the value is selected;
- the locked configuration and independent metric configuration receive new
hashes after the decision.
## H1--H4 decision boundaries
- **H1**: freeze only after reach clipping, joint-limit stress,
low-manipulability stress, and valid branch-continuity trajectories are
separately identifiable. The experimental unit is a complete trajectory.
- **H2**: numerical calibration may select a robust DLS region, but physical
wrench-accuracy claims require an independent six-axis F/T reference,
timestamp calibration, payload/friction calibration, and a causal
acceleration estimate.
- **H3**: the fixed-branch virtual-work identity is a deterministic contract.
Actual closed-loop power mismatch is a different performance endpoint and
must not be replaced by the identity check. Near-zero-power trials require a
preregistered normalizer gate or an absolute mismatch outcome.
- **H4**: passing the energy-accounting audit verifies implementation
correctness only. Energy capacity and feedback gain remain not freeze-ready
if the projection distortion is excessive or the tested network/contact
envelope is incomplete.
## Data handling
Raw batches remain under `output/experiments/` and are intentionally ignored by
Git because they can be regenerated from the plan, source commit, and recorded
seeds. The source-controlled audit records plan hashes, source-data hashes,
anomalies, and freeze decisions. No calibration value may be copied into the
manuscript Results section.
Locked experiments must use a new batch directory and must never overwrite a
calibration batch. A locked run is invalid if the worktree is dirty or if its
recorded source and model hashes do not match the approved lock record.