diff --git a/README.md b/README.md index bd82838..37e5fed 100644 --- a/README.md +++ b/README.md @@ -67,6 +67,20 @@ XDG_CACHE_HOME=/tmp/exoskeleton-xdg-cache \ Equivalent executor/config pairs are documented in `code/experiments/README.md`. +The current v3 numerical redesign provides two deliberately separate +calibration paths: + +- `h1_calibration_v3.json` crosses the SEW angle representation cut once and + then twice on a round trip, while recording the actual SEW 7-by-7 + differential and descriptive 50 Hz timing. +- `bilateral_calibration_v3_stable_contact.json` calibrates slow, unsaturated + contact before any network study. +- `bilateral_calibration_v3_energy_challenge.json` adds a synthetic, + smooth upstream energy stress only for H4; it is not eligible for H3. + +All three remain pre-prototype calibration evidence and are not manuscript +Results or physical-system validation. + The current calibration decision is recorded in `docs/calibration/CALIBRATION_AUDIT_2026-07-27.md`. It deliberately leaves H1 and the bilateral gain/energy settings unfrozen; calibration values are not diff --git a/code/analysis/metrics.py b/code/analysis/metrics.py index 3eeec61..2c2d4ab 100644 --- a/code/analysis/metrics.py +++ b/code/analysis/metrics.py @@ -12,7 +12,7 @@ from typing import Any, Mapping import numpy as np -METRIC_SCHEMA_VERSION = "1.1.0" +METRIC_SCHEMA_VERSION = "1.2.0" class MetricError(ValueError): @@ -26,6 +26,25 @@ def _vector(value: Any, name: str, *, dtype=float) -> np.ndarray: return array +def _boolean_vector(value: Any, name: str) -> np.ndarray: + """Parse an evidence flag without allowing NaN/nonzero coercion to True.""" + raw = np.asarray(value) + if raw.ndim != 1 or raw.size == 0: + raise MetricError(f"{name} must be a non-empty one-dimensional array") + if np.issubdtype(raw.dtype, np.bool_): + return raw.astype(bool, copy=False) + try: + numeric = np.asarray(value, dtype=float) + except (TypeError, ValueError) as error: + raise MetricError(f"{name} must contain only 0/1 flags") from error + if ( + not np.all(np.isfinite(numeric)) + or not np.all((numeric == 0.0) | (numeric == 1.0)) + ): + raise MetricError(f"{name} must contain only finite 0/1 flags") + return numeric.astype(bool) + + 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: @@ -69,7 +88,7 @@ def compute_h1_composite( 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) + valid = _boolean_vector(mapping_valid, "mapping_valid") e_position = _vector(position_error_m, "position_error_m") e_orientation = _vector(orientation_error_rad, "orientation_error_rad") slave = _matrix(q_slave, "q_slave") @@ -88,20 +107,19 @@ def compute_h1_composite( accepted_array = ( np.ones(n, dtype=bool) if accepted is None - else _vector(accepted, "accepted", dtype=bool) + else _boolean_vector(accepted, "accepted") ) reset = ( np.zeros(n, dtype=bool) if commanded_reset is None - else _vector(commanded_reset, "commanded_reset", dtype=bool) + else _boolean_vector(commanded_reset, "commanded_reset") ) degeneracy = ( np.zeros(n, dtype=bool) if degeneracy_transition is None - else _vector( + else _boolean_vector( degeneracy_transition, "degeneracy_transition", - dtype=bool, ) ) _same_rows( @@ -160,15 +178,32 @@ def compute_h1_composite( 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)) + accepted_count = int(np.sum(accepted_array)) + eligible_count = int(np.sum(eligible)) + failure_metric_valid = accepted_count > 0 + discontinuity_metric_valid = eligible_count > 0 + composite_metric_valid = ( + failure_metric_valid and discontinuity_metric_valid + ) + F_r = int(np.any(failure_mask)) if failure_metric_valid else None + D_r = ( + int(np.any(discontinuity_mask)) + if discontinuity_metric_valid + else None + ) return { "h1_F_r": F_r, "h1_D_r": D_r, - "h1_C_r": max(F_r, D_r), + "h1_C_r": ( + max(F_r, D_r) if composite_metric_valid else None + ), + "h1_failure_metric_valid": failure_metric_valid, + "h1_discontinuity_metric_valid": discontinuity_metric_valid, + "h1_composite_metric_valid": composite_metric_valid, + "h1_accepted_sample_count": accepted_count, "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_eligible_increment_count": eligible_count, "h1_mapping_valid_fraction": float(np.mean(valid[accepted_array])) if np.any(accepted_array) else 0.0, @@ -187,6 +222,202 @@ def compute_h1_composite( } +def compute_h1_timing_branch_metrics( + *, + pose_runtime_s: Any, + warm_start: Any, + phi_rad: Any, + q_slave: Any, + branch_smooth: Any, + differential_applicable: Any, + differential_valid: Any, + differential_runtime_s: Any, + differential_max_one_sided_consistency: Any, + deadline_s: float = 0.020, + minimum_phi_wrap_crossings: int = 0, +) -> dict[str, Any]: + """Summarize H1 pose latency, actual differential, and phi-wrap stress.""" + pose_runtime = _vector(pose_runtime_s, "pose_runtime_s") + warm = _boolean_vector(warm_start, "warm_start") + phi = _vector(phi_rad, "phi_rad") + slave = _matrix(q_slave, "q_slave") + smooth = _boolean_vector(branch_smooth, "branch_smooth") + applicable = _boolean_vector( + differential_applicable, "differential_applicable" + ) + differential = _boolean_vector( + differential_valid, "differential_valid" + ) + differential_runtime = _vector( + differential_runtime_s, "differential_runtime_s" + ) + differential_consistency = _vector( + differential_max_one_sided_consistency, + "differential_max_one_sided_consistency", + ) + _same_rows( + { + "pose_runtime_s": pose_runtime, + "warm_start": warm, + "phi_rad": phi, + "q_slave": slave, + "branch_smooth": smooth, + "differential_applicable": applicable, + "differential_valid": differential, + "differential_runtime_s": differential_runtime, + "differential_max_one_sided_consistency": ( + differential_consistency + ), + } + ) + for array, name in ( + (pose_runtime, "pose_runtime_s"), + (phi, "phi_rad"), + (slave, "q_slave"), + (differential_runtime, "differential_runtime_s"), + ): + _finite(array, name) + valid_differential_mask = applicable & differential + if np.any(valid_differential_mask) and not np.all( + np.isfinite( + differential_consistency[valid_differential_mask] + ) + ): + raise MetricError( + "valid differential samples need finite consistency evidence" + ) + if np.any(pose_runtime < 0.0) or np.any(differential_runtime < 0.0): + raise MetricError("H1 runtime samples must be non-negative") + deadline_s = float(deadline_s) + if not np.isfinite(deadline_s) or deadline_s <= 0.0: + raise MetricError("H1 deadline_s must be finite and positive") + minimum_phi_wrap_crossings = int(minimum_phi_wrap_crossings) + if minimum_phi_wrap_crossings < 0: + raise MetricError( + "minimum_phi_wrap_crossings must be non-negative" + ) + + pose_ms = 1e3 * pose_runtime + warm_pose_ms = pose_ms[warm] + raw_phi_step = np.abs(np.diff(phi)) + wrap_crossing = raw_phi_step > np.pi + slave_step = ( + np.max(np.abs(_wrapped_delta(slave[1:] - slave[:-1])), axis=1) + if slave.shape[0] > 1 + else np.empty(0, dtype=float) + ) + crossing_indices = np.flatnonzero(wrap_crossing) + 1 + crossing_count = int(np.sum(wrap_crossing)) + wrap_metric_valid = crossing_count >= minimum_phi_wrap_crossings + result: dict[str, Any] = { + "h1_pose_runtime_p50_ms": float(np.percentile(pose_ms, 50)), + "h1_pose_runtime_p95_ms": float(np.percentile(pose_ms, 95)), + "h1_pose_runtime_p99_ms": float(np.percentile(pose_ms, 99)), + "h1_pose_runtime_max_ms": float(np.max(pose_ms)), + "h1_pose_runtime_warm_p95_ms": ( + float(np.percentile(warm_pose_ms, 95)) + if warm_pose_ms.size + else None + ), + "h1_pose_deadline_ms": 1e3 * deadline_s, + "h1_pose_deadline_miss_count": int( + np.sum(pose_runtime > deadline_s) + ), + "h1_pose_deadline_miss_fraction": float( + np.mean(pose_runtime > deadline_s) + ), + "h1_branch_smooth_fraction": float(np.mean(smooth)), + "h1_phi_raw_wrap_crossing_count": crossing_count, + "h1_phi_raw_wrap_crossing_indices": crossing_indices.tolist(), + "h1_phi_wrap_metric_valid": wrap_metric_valid, + "h1_phi_wrap_minimum_crossing_count": ( + minimum_phi_wrap_crossings + ), + "h1_phi_wrap_crossing_max_slave_joint_step_rad": ( + float(np.max(slave_step[wrap_crossing])) + if np.any(wrap_crossing) + else None + ), + "h1_differential_applicable_fraction": float( + np.mean(applicable) + ), + } + if np.any(applicable): + selected_runtime = differential_runtime[applicable] + selected_consistency = differential_consistency[applicable] + finite_consistency = selected_consistency[ + np.isfinite(selected_consistency) + ] + differential_ms = 1e3 * selected_runtime + feedback_ready_mask = applicable & differential + feedback_ready_ms = 1e3 * ( + pose_runtime[feedback_ready_mask] + + differential_runtime[feedback_ready_mask] + ) + result.update( + { + "h1_differential_valid_fraction": float( + np.mean(differential[applicable]) + ), + "h1_differential_runtime_p50_ms": float( + np.percentile(differential_ms, 50) + ), + "h1_differential_runtime_p95_ms": float( + np.percentile(differential_ms, 95) + ), + "h1_differential_runtime_p99_ms": float( + np.percentile(differential_ms, 99) + ), + "h1_differential_runtime_max_ms": float( + np.max(differential_ms) + ), + "h1_differential_max_one_sided_consistency": float( + np.max(finite_consistency) + ) + if finite_consistency.size + else None, + "h1_feedback_ready_valid_sample_count": int( + np.sum(feedback_ready_mask) + ), + "h1_feedback_ready_runtime_p95_ms": ( + float(np.percentile(feedback_ready_ms, 95)) + if feedback_ready_ms.size + else None + ), + "h1_feedback_ready_deadline_miss_count": ( + int(np.sum(feedback_ready_ms > 1e3 * deadline_s)) + if feedback_ready_ms.size + else None + ), + "h1_feedback_ready_deadline_miss_fraction": ( + float( + np.mean( + feedback_ready_ms > 1e3 * deadline_s + ) + ) + if feedback_ready_ms.size + else None + ), + } + ) + else: + result.update( + { + "h1_differential_valid_fraction": None, + "h1_differential_runtime_p50_ms": None, + "h1_differential_runtime_p95_ms": None, + "h1_differential_runtime_p99_ms": None, + "h1_differential_runtime_max_ms": None, + "h1_differential_max_one_sided_consistency": None, + "h1_feedback_ready_valid_sample_count": 0, + "h1_feedback_ready_runtime_p95_ms": None, + "h1_feedback_ready_deadline_miss_count": None, + "h1_feedback_ready_deadline_miss_fraction": None, + } + ) + return result + + def compute_h1_audit_metrics( *, differential_valid: Any, @@ -200,19 +431,19 @@ def compute_h1_audit_metrics( 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 + differential = _boolean_vector( + differential_valid, "differential_valid" ) 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 + joint_limit = _boolean_vector( + joint_limit_active, "joint_limit_active" ) - geometry = _vector( - geometry_degenerate, "geometry_degenerate", dtype=bool + geometry = _boolean_vector( + geometry_degenerate, "geometry_degenerate" ) - low_manip = _vector( - low_manipulability, "low_manipulability", dtype=bool + low_manip = _boolean_vector( + low_manipulability, "low_manipulability" ) minimum_singular = _vector( slave_min_singular_value, "slave_min_singular_value" @@ -322,7 +553,7 @@ def compute_h2_wrench_metrics( mask = ( np.ones(n, dtype=bool) if sample_mask is None - else _vector(sample_mask, "sample_mask", dtype=bool) + else _boolean_vector(sample_mask, "sample_mask") ) if mask.shape[0] != n or not np.any(mask): raise MetricError("H2 sample_mask must select at least one aligned sample") @@ -339,10 +570,9 @@ def compute_h2_wrench_metrics( "h2_moment_bias_xyz_Nm": np.mean(error[:, 3:], axis=0).tolist(), } if numerical_rank_deficient is not None: - numerical_flag = _vector( + numerical_flag = _boolean_vector( numerical_rank_deficient, "numerical_rank_deficient", - dtype=bool, ) if numerical_flag.shape[0] != n: raise MetricError("H2 numerical-rank flag length mismatch") @@ -350,10 +580,9 @@ def compute_h2_wrench_metrics( np.mean(numerical_flag[mask]) ) if operationally_ill_conditioned is not None: - operational_flag = _vector( + operational_flag = _boolean_vector( operationally_ill_conditioned, "operationally_ill_conditioned", - dtype=bool, ) if operational_flag.shape[0] != n: raise MetricError("H2 operational-condition flag length mismatch") @@ -486,7 +715,7 @@ def compute_h3_power_mismatch( chi = ( np.ones(n, dtype=bool) if return_valid is None - else _vector(return_valid, "return_valid", dtype=bool) + else _boolean_vector(return_valid, "return_valid") ) if chi.shape[0] != n: raise MetricError("return_valid length mismatch") @@ -673,8 +902,33 @@ def compute_bilateral_diagnostics( feedback_torque_Nm: Any, contact_force_N: Any, projection_factor: Any, + wall_force_raw_N: Any | None = None, + wall_force_applied_N: Any | None = None, + wall_force_saturation_active: Any | None = None, + wall_force_limit_N: Any | None = None, + master_joint_limit_active: Any | None = None, + slave_joint_limit_active: Any | None = None, + master_velocity_limit_active: Any | None = None, + slave_velocity_limit_active: Any | None = None, + master_acceleration_limit_active: Any | None = None, + slave_acceleration_limit_active: Any | None = None, + master_torque_saturation_active: Any | None = None, + slave_torque_saturation_active: Any | None = None, + haptic_rate_limit_active: Any | None = None, + haptic_torque_saturation_active: Any | None = None, + energy_probe_raw_work_J: Any | None = None, projection_tolerance: float = 1e-12, contact_force_threshold_N: float = 1e-6, + minimum_contact_fraction: float = 0.0, + minimum_contact_rms_N: float = 0.0, + maximum_force_limit_hit_fraction: float = 1.0, + minimum_force_headroom_N: float = 0.0, + maximum_master_tracking_rmse_rad: float | None = None, + maximum_slave_tracking_rmse_rad: float | None = None, + minimum_projection_intervention_fraction: float = 0.0, + maximum_projection_intervention_fraction: float = 1.0, + maximum_limit_active_fraction: float = 1.0, + minimum_energy_probe_raw_work_J: float = 0.0, ) -> dict[str, Any]: """Compute secondary task/transparency diagnostics from stored samples.""" master_error = _vector( @@ -686,6 +940,68 @@ def compute_bilateral_diagnostics( feedback = _matrix(feedback_torque_Nm, "feedback_torque_Nm") contact = _vector(contact_force_N, "contact_force_N") rho = _vector(projection_factor, "projection_factor") + applied = ( + contact.copy() + if wall_force_applied_N is None + else _vector(wall_force_applied_N, "wall_force_applied_N") + ) + raw = ( + applied.copy() + if wall_force_raw_N is None + else _vector(wall_force_raw_N, "wall_force_raw_N") + ) + sample_count = contact.shape[0] + if wall_force_limit_N is None: + force_limit = None + else: + force_limit_array = np.asarray(wall_force_limit_N, dtype=float) + if force_limit_array.ndim == 0: + force_limit = np.full(sample_count, float(force_limit_array)) + else: + force_limit = _vector( + force_limit_array, "wall_force_limit_N" + ) + if wall_force_saturation_active is None: + saturation = ( + np.zeros(sample_count, dtype=bool) + if force_limit is None + else raw > force_limit + ) + else: + saturation = _boolean_vector( + wall_force_saturation_active, + "wall_force_saturation_active", + ) + limit_inputs = { + "master_joint_limit_active": master_joint_limit_active, + "slave_joint_limit_active": slave_joint_limit_active, + "master_velocity_limit_active": master_velocity_limit_active, + "slave_velocity_limit_active": slave_velocity_limit_active, + "master_acceleration_limit_active": master_acceleration_limit_active, + "slave_acceleration_limit_active": slave_acceleration_limit_active, + "master_torque_saturation_active": master_torque_saturation_active, + "slave_torque_saturation_active": slave_torque_saturation_active, + "haptic_rate_limit_active": haptic_rate_limit_active, + "haptic_torque_saturation_active": ( + haptic_torque_saturation_active + ), + } + limit_flags = { + name: ( + np.zeros(sample_count, dtype=bool) + if value is None + else _boolean_vector(value, name) + ) + for name, value in limit_inputs.items() + } + probe_work = ( + np.zeros(sample_count, dtype=float) + if energy_probe_raw_work_J is None + else _vector( + energy_probe_raw_work_J, + "energy_probe_raw_work_J", + ) + ) _same_rows( { "master_tracking_error_rad": master_error, @@ -693,6 +1009,16 @@ def compute_bilateral_diagnostics( "feedback_torque_Nm": feedback, "contact_force_N": contact, "projection_factor": rho, + "wall_force_applied_N": applied, + "wall_force_raw_N": raw, + "wall_force_saturation_active": saturation, + **( + {} + if force_limit is None + else {"wall_force_limit_N": force_limit} + ), + **limit_flags, + "energy_probe_raw_work_J": probe_work, } ) for array, name in ( @@ -701,8 +1027,56 @@ def compute_bilateral_diagnostics( (feedback, "feedback_torque_Nm"), (contact, "contact_force_N"), (rho, "projection_factor"), + (applied, "wall_force_applied_N"), + (raw, "wall_force_raw_N"), + (probe_work, "energy_probe_raw_work_J"), ): _finite(array, name) + nonnegative_arrays = ( + (contact, "contact_force_N"), + (applied, "wall_force_applied_N"), + (raw, "wall_force_raw_N"), + ) + if any(np.any(array < 0.0) for array, _ in nonnegative_arrays): + names = ", ".join(name for _, name in nonnegative_arrays) + raise MetricError(f"{names} must be non-negative") + audit_tolerance_N = 1e-12 + if np.any(raw + audit_tolerance_N < applied): + raise MetricError( + "wall_force_raw_N cannot be smaller than applied force" + ) + if not np.allclose( + contact, + applied, + rtol=0.0, + atol=audit_tolerance_N, + ): + raise MetricError( + "contact_force_N must equal the applied wall-force magnitude" + ) + if np.any(np.diff(probe_work) < -1e-12): + raise MetricError( + "energy_probe_raw_work_J must be cumulative and non-decreasing" + ) + if force_limit is not None: + _finite(force_limit, "wall_force_limit_N") + if np.any(force_limit <= 0.0): + raise MetricError("wall_force_limit_N must be positive") + expected_applied = np.minimum(raw, force_limit) + expected_saturation = raw > force_limit + if not np.allclose( + applied, + expected_applied, + rtol=0.0, + atol=audit_tolerance_N, + ): + raise MetricError( + "applied wall force disagrees with raw force/force limit" + ) + if not np.array_equal(saturation, expected_saturation): + raise MetricError( + "wall-force saturation flags disagree with raw force/limit" + ) 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: @@ -714,27 +1088,184 @@ def compute_bilateral_diagnostics( raise MetricError( "contact_force_threshold_N must be finite and non-negative" ) + gate_values = { + "minimum_contact_fraction": minimum_contact_fraction, + "minimum_contact_rms_N": minimum_contact_rms_N, + "maximum_force_limit_hit_fraction": ( + maximum_force_limit_hit_fraction + ), + "minimum_force_headroom_N": minimum_force_headroom_N, + "maximum_projection_intervention_fraction": ( + maximum_projection_intervention_fraction + ), + "minimum_projection_intervention_fraction": ( + minimum_projection_intervention_fraction + ), + "maximum_limit_active_fraction": maximum_limit_active_fraction, + "minimum_energy_probe_raw_work_J": + minimum_energy_probe_raw_work_J, + } + if any( + not np.isfinite(float(value)) or float(value) < 0.0 + for value in gate_values.values() + ): + raise MetricError("bilateral gate thresholds must be finite/non-negative") + if ( + float(minimum_contact_fraction) > 1.0 + or float(maximum_force_limit_hit_fraction) > 1.0 + or float(maximum_projection_intervention_fraction) > 1.0 + or float(minimum_projection_intervention_fraction) > 1.0 + or float(maximum_limit_active_fraction) > 1.0 + ): + raise MetricError("bilateral fraction thresholds must lie in [0, 1]") + if ( + float(minimum_projection_intervention_fraction) + > float(maximum_projection_intervention_fraction) + ): + raise MetricError( + "minimum projection fraction cannot exceed its maximum" + ) + for value, name in ( + ( + maximum_master_tracking_rmse_rad, + "maximum_master_tracking_rmse_rad", + ), + ( + maximum_slave_tracking_rmse_rad, + "maximum_slave_tracking_rmse_rad", + ), + ): + if value is not None and ( + not np.isfinite(float(value)) or float(value) < 0.0 + ): + raise MetricError(f"{name} must be finite and non-negative") feedback_norm = np.linalg.norm(feedback, axis=1) + master_tracking_rmse = float( + np.sqrt(np.mean(np.square(master_error))) + ) + slave_tracking_rmse = float( + np.sqrt(np.mean(np.square(slave_error))) + ) + projection_intervention_fraction = float( + np.mean(rho < (1.0 - projection_tolerance)) + ) + any_limit_active = np.logical_or.reduce( + tuple(limit_flags.values()) + ) + limit_active_fraction = float(np.mean(any_limit_active)) + contact_mask = applied > contact_force_threshold_N + contact_fraction = float(np.mean(contact_mask)) + contact_rms_all = float(np.sqrt(np.mean(np.square(applied)))) + contact_rms_active = ( + float(np.sqrt(np.mean(np.square(applied[contact_mask])))) + if np.any(contact_mask) + else 0.0 + ) + force_limit_hit_fraction = float(np.mean(saturation)) + force_limit_hit_contact_fraction = ( + float(np.mean(saturation[contact_mask])) + if np.any(contact_mask) + else 0.0 + ) + force_headroom_N = ( + None + if force_limit is None + else float(np.min(force_limit - applied)) + ) + contact_fraction_gate = ( + contact_fraction >= float(minimum_contact_fraction) + ) + contact_rms_gate = ( + contact_rms_active >= float(minimum_contact_rms_N) + ) + force_limit_gate = ( + force_limit_hit_fraction + <= float(maximum_force_limit_hit_fraction) + ) + force_headroom_gate = ( + True + if force_headroom_N is None + else force_headroom_N >= float(minimum_force_headroom_N) + ) + master_tracking_gate = ( + True + if maximum_master_tracking_rmse_rad is None + else master_tracking_rmse + <= float(maximum_master_tracking_rmse_rad) + ) + slave_tracking_gate = ( + True + if maximum_slave_tracking_rmse_rad is None + else slave_tracking_rmse + <= float(maximum_slave_tracking_rmse_rad) + ) + projection_gate = ( + float(minimum_projection_intervention_fraction) + <= projection_intervention_fraction + <= float(maximum_projection_intervention_fraction) + ) + limit_active_gate = ( + limit_active_fraction <= float(maximum_limit_active_fraction) + ) + probe_work_final_J = float(probe_work[-1]) + probe_work_gate = ( + probe_work_final_J >= float(minimum_energy_probe_raw_work_J) + ) 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_master_tracking_rmse_rad": master_tracking_rmse, + "bilateral_slave_tracking_rmse_rad": slave_tracking_rmse, "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_rms_N": contact_rms_active, + "bilateral_contact_force_rms_all_samples_N": contact_rms_all, + "bilateral_contact_force_peak_N": float(np.max(applied)), + "bilateral_wall_force_raw_peak_N": float(np.max(raw)), + "bilateral_contact_fraction": contact_fraction, + "bilateral_force_limit_hit_fraction": force_limit_hit_fraction, + "bilateral_force_limit_hit_contact_fraction": ( + force_limit_hit_contact_fraction ), - "bilateral_contact_force_peak_N": float(np.max(contact)), - "bilateral_contact_fraction": float( - np.mean(contact > contact_force_threshold_N) + "bilateral_force_headroom_N": force_headroom_N, + "bilateral_contact_fraction_gate_pass": bool( + contact_fraction_gate + ), + "bilateral_contact_rms_gate_pass": bool(contact_rms_gate), + "bilateral_force_limit_gate_pass": bool(force_limit_gate), + "bilateral_force_headroom_gate_pass": bool( + force_headroom_gate + ), + "bilateral_master_tracking_gate_pass": bool( + master_tracking_gate + ), + "bilateral_slave_tracking_gate_pass": bool( + slave_tracking_gate + ), + "bilateral_projection_gate_pass": bool(projection_gate), + "bilateral_energy_probe_raw_work_J": probe_work_final_J, + "bilateral_energy_probe_raw_work_gate_pass": bool( + probe_work_gate + ), + "bilateral_limit_active_fraction": limit_active_fraction, + "bilateral_limit_active_gate_pass": bool(limit_active_gate), + **{ + f"bilateral_{name}_fraction": float(np.mean(flag)) + for name, flag in limit_flags.items() + }, + "bilateral_stable_contact_gate_pass": bool( + contact_fraction_gate + and contact_rms_gate + and force_limit_gate + and force_headroom_gate + and master_tracking_gate + and slave_tracking_gate + and projection_gate + and limit_active_gate + and probe_work_gate ), "bilateral_projection_intervention_fraction": float( - np.mean(rho < (1.0 - projection_tolerance)) + projection_intervention_fraction ), "bilateral_projection_factor_min": float(np.min(rho)), } @@ -842,8 +1373,40 @@ def derive_trial_metrics( "differential_valid", "map_differential_valid", ) - mapping_valid = np.asarray(pose_success, dtype=bool) & np.asarray( - differential_valid, dtype=bool + branch_smooth = _field( + samples, + fields, + "branch_smooth", + "map_branch_smooth", + optional=True, + ) + if branch_smooth is None: + # Pre-v3 evidence used map_differential_valid as the explicit + # branch-smooth proxy. + branch_smooth = differential_valid + differential_applicable = _field( + samples, + fields, + "differential_applicable", + "map_differential_applicable", + optional=True, + ) + if differential_applicable is None: + differential_applicable = np.zeros_like( + _boolean_vector( + differential_valid, "map_differential_valid" + ), + dtype=bool, + ) + branch_array = _boolean_vector( + branch_smooth, "map_branch_smooth" + ) + applicable_array = _boolean_vector( + differential_applicable, "map_differential_applicable" + ) + mapping_valid = ( + _boolean_vector(pose_success, "map_pose_success") + & branch_array ) result.update( compute_h1_composite( @@ -935,7 +1498,11 @@ def derive_trial_metrics( ) result.update( compute_h1_audit_metrics( - differential_valid=differential_valid, + # H1 C_r is the common pose/branch endpoint. Actual + # differential validity is reported separately because + # the baseline methods do not yet expose an equivalent + # 7x7 differential implementation. + differential_valid=branch_array, validity_reason_code=samples[ "map_validity_reason_code" ], @@ -960,6 +1527,118 @@ def derive_trial_metrics( ), ) ) + branch_timing = family_config.get("branch_timing") + if branch_timing is not None: + if not isinstance(branch_timing, Mapping): + raise MetricError( + "H1 branch_timing configuration must be a mapping" + ) + strict_fields = { + "branch_smooth": "map_branch_smooth", + "differential_applicable": + "map_differential_applicable", + "differential_A": "map_differential_A", + "differential_runtime_s": + "map_differential_runtime_s", + "differential_max_one_sided_consistency": ( + "map_differential_max_one_sided_consistency" + ), + "pose_runtime_s": "map_runtime_s", + "warm_start": "map_warm_start", + "phi_rad": "map_sew_phi_rad", + } + missing_strict = [ + fields.get(logical_name, default_name) + for logical_name, default_name in strict_fields.items() + if fields.get(logical_name, default_name) not in samples + ] + if missing_strict: + raise MetricError( + "H1 branch_timing requires v3 evidence fields: " + f"{sorted(missing_strict)}" + ) + differential_A = np.asarray( + _field( + samples, + fields, + "differential_A", + "map_differential_A", + ), + dtype=float, + ) + if differential_A.shape != ( + applicable_array.shape[0], + 7, + 7, + ): + raise MetricError( + "map_differential_A must have shape (samples, 7, 7)" + ) + differential_valid_array = _boolean_vector( + differential_valid, "map_differential_valid" + ) + valid_differential_mask = ( + applicable_array & differential_valid_array + ) + if np.any(valid_differential_mask) and not np.all( + np.isfinite( + differential_A[valid_differential_mask] + ) + ): + raise MetricError( + "valid H1 differential matrices must be finite" + ) + result.update( + compute_h1_timing_branch_metrics( + pose_runtime_s=_field( + samples, + fields, + "pose_runtime_s", + "map_runtime_s", + ), + warm_start=_field( + samples, + fields, + "warm_start", + "map_warm_start", + ), + phi_rad=_field( + samples, + fields, + "phi_rad", + "map_sew_phi_rad", + ), + q_slave=_field( + samples, fields, "q_slave", "map_q_slave" + ), + branch_smooth=branch_smooth, + differential_applicable=( + differential_applicable + ), + differential_valid=differential_valid, + differential_runtime_s=_field( + samples, + fields, + "differential_runtime_s", + "map_differential_runtime_s", + ), + differential_max_one_sided_consistency=_field( + samples, + fields, + "differential_max_one_sided_consistency", + ( + "map_differential_max_" + "one_sided_consistency" + ), + ), + deadline_s=branch_timing.get( + "deadline_s", 0.020 + ), + minimum_phi_wrap_crossings=branch_timing.get( + "minimum_phi_wrap_crossings", 0 + ), + ) + ) elif family == "h2": result.update( compute_h2_wrench_metrics( @@ -1027,6 +1706,39 @@ def derive_trial_metrics( ) ) elif family == "h3": + h3_eligible = _field( + samples, + fields, + "h3_eligible", + "h3_eligible", + optional=True, + ) + if h3_eligible is not None and not np.all( + _boolean_vector(h3_eligible, "h3_eligible") + ): + raise MetricError( + "H3 is ineligible for samples containing an unpaired " + "synthetic energy probe" + ) + probe_work = _field( + samples, + fields, + "energy_probe_raw_work_J", + "energy_probe_raw_work_J", + optional=True, + ) + if probe_work is not None: + probe_work_array = _vector( + probe_work, "energy_probe_raw_work_J" + ) + _finite( + probe_work_array, "energy_probe_raw_work_J" + ) + if float(probe_work_array[-1]) > 1e-15: + raise MetricError( + "H3 is ineligible when synthetic energy-probe work " + "is nonzero" + ) result.update( compute_h3_power_mismatch( tau_master_raw=_field( @@ -1118,6 +1830,39 @@ def derive_trial_metrics( ) ) elif family == "bilateral": + required_audit_fields = family_config.get( + "required_audit_fields", [] + ) + if ( + not isinstance(required_audit_fields, list) + or any( + not isinstance(name, str) or not name + for name in required_audit_fields + ) + ): + raise MetricError( + "bilateral required_audit_fields must be a list of names" + ) + missing_audit_fields = sorted( + name + for name in required_audit_fields + if name not in samples + ) + if missing_audit_fields: + raise MetricError( + "missing required bilateral audit fields: " + f"{missing_audit_fields}" + ) + stored_force_limit = _field( + samples, + fields, + "wall_force_limit_N", + "configured_wall_force_limit_N", + optional=True, + ) + gate_config = family_config.get("gates", {}) + if not isinstance(gate_config, Mapping): + raise MetricError("bilateral gates must be a mapping") result.update( compute_bilateral_diagnostics( master_tracking_error_rad=_field( @@ -1150,14 +1895,221 @@ def derive_trial_metrics( "projection_factor", "rho", ), + wall_force_raw_N=_field( + samples, + fields, + "wall_force_raw_N", + "wall_force_raw_N", + optional=True, + ), + wall_force_applied_N=_field( + samples, + fields, + "wall_force_applied_N", + "wall_force_applied_N", + optional=True, + ), + wall_force_saturation_active=_field( + samples, + fields, + "wall_force_saturation_active", + "wall_force_saturation_active", + optional=True, + ), + wall_force_limit_N=( + stored_force_limit + if stored_force_limit is not None + else family_config.get("wall_force_limit_N") + ), + master_joint_limit_active=_field( + samples, + fields, + "master_joint_limit_active", + "master_joint_limit_active", + optional=True, + ), + slave_joint_limit_active=_field( + samples, + fields, + "slave_joint_limit_active", + "slave_joint_limit_active", + optional=True, + ), + master_velocity_limit_active=_field( + samples, + fields, + "master_velocity_limit_active", + "master_velocity_limit_active", + optional=True, + ), + slave_velocity_limit_active=_field( + samples, + fields, + "slave_velocity_limit_active", + "slave_velocity_limit_active", + optional=True, + ), + master_acceleration_limit_active=_field( + samples, + fields, + "master_acceleration_limit_active", + "master_acceleration_limit_active", + optional=True, + ), + slave_acceleration_limit_active=_field( + samples, + fields, + "slave_acceleration_limit_active", + "slave_acceleration_limit_active", + optional=True, + ), + master_torque_saturation_active=_field( + samples, + fields, + "master_torque_saturation_active", + "master_torque_saturation_active", + optional=True, + ), + slave_torque_saturation_active=_field( + samples, + fields, + "slave_torque_saturation_active", + "slave_torque_saturation_active", + optional=True, + ), + haptic_rate_limit_active=_field( + samples, + fields, + "haptic_rate_limit_active", + "haptic_rate_limit_active", + optional=True, + ), + haptic_torque_saturation_active=_field( + samples, + fields, + "haptic_torque_saturation_active", + "haptic_torque_saturation_active", + optional=True, + ), + energy_probe_raw_work_J=_field( + samples, + fields, + "energy_probe_raw_work_J", + "energy_probe_raw_work_J", + optional=True, + ), projection_tolerance=family_config.get( "projection_tolerance", 1e-12 ), contact_force_threshold_N=family_config.get( "contact_force_threshold_N", 1e-6 ), + minimum_contact_fraction=gate_config.get( + "minimum_contact_fraction", 0.0 + ), + minimum_contact_rms_N=gate_config.get( + "minimum_contact_rms_N", 0.0 + ), + maximum_force_limit_hit_fraction=gate_config.get( + "maximum_force_limit_hit_fraction", 1.0 + ), + minimum_force_headroom_N=gate_config.get( + "minimum_force_headroom_N", 0.0 + ), + maximum_master_tracking_rmse_rad=gate_config.get( + "maximum_master_tracking_rmse_rad" + ), + maximum_slave_tracking_rmse_rad=gate_config.get( + "maximum_slave_tracking_rmse_rad" + ), + minimum_projection_intervention_fraction=( + gate_config.get( + "minimum_projection_intervention_fraction", + 0.0, + ) + ), + maximum_projection_intervention_fraction=( + gate_config.get( + "maximum_projection_intervention_fraction", + 1.0, + ) + ), + maximum_limit_active_fraction=gate_config.get( + "maximum_limit_active_fraction", 1.0 + ), + minimum_energy_probe_raw_work_J=gate_config.get( + "minimum_energy_probe_raw_work_J", 0.0 + ), ) ) + elif family == "energy_challenge": + required_metrics = ( + "h4_energy_audit_pass", + "h4_shadow_floor_deficit_J", + "h4_downstream_modification_max_Nm", + "h4_D_proj", + "bilateral_stable_contact_gate_pass", + ) + missing_metrics = [ + name for name in required_metrics if name not in result + ] + if missing_metrics: + raise MetricError( + "energy_challenge must follow H4 and bilateral metrics; " + f"missing={missing_metrics}" + ) + minimum_shadow_deficit_J = float( + family_config.get("minimum_shadow_deficit_J", 0.0) + ) + maximum_downstream_modification_Nm = float( + family_config.get( + "maximum_downstream_modification_Nm", 1e-10 + ) + ) + maximum_D_proj = float( + family_config.get("maximum_D_proj", 1.0) + ) + challenge_thresholds = ( + minimum_shadow_deficit_J, + maximum_downstream_modification_Nm, + maximum_D_proj, + ) + if any( + not np.isfinite(value) or value < 0.0 + for value in challenge_thresholds + ): + raise MetricError( + "energy challenge thresholds must be finite/non-negative" + ) + shadow_gate = bool( + result["h4_shadow_floor_deficit_J"] + >= minimum_shadow_deficit_J + ) + downstream_gate = bool( + result["h4_downstream_modification_max_Nm"] + <= maximum_downstream_modification_Nm + ) + distortion_gate = bool( + result["h4_D_proj"] <= maximum_D_proj + ) + result.update( + { + "energy_challenge_shadow_gate_pass": shadow_gate, + "energy_challenge_downstream_gate_pass": + downstream_gate, + "energy_challenge_distortion_gate_pass": + distortion_gate, + "energy_challenge_gate_pass": bool( + result["h4_energy_audit_pass"] + and result[ + "bilateral_stable_contact_gate_pass" + ] + and shadow_gate + and downstream_gate + and distortion_gate + ), + } + ) else: raise MetricError(f"unknown metric family {family!r}") return result diff --git a/code/config/experiments/bilateral_calibration_v3_energy_challenge.json b/code/config/experiments/bilateral_calibration_v3_energy_challenge.json new file mode 100644 index 0000000..087527d --- /dev/null +++ b/code/config/experiments/bilateral_calibration_v3_energy_challenge.json @@ -0,0 +1,81 @@ +{ + "study_id": "g0c_bilateral_calibration_v3_energy_challenge", + "split": "calibration", + "root_seed": 2026072732, + "replicates": 3, + "methods": [ + "proposed_energy" + ], + "trajectories": [ + { + "id": "slow_contact_supervisor_stress", + "family": "contact_roundtrip", + "duration_s": 6.0, + "contact_probe_fraction": 0.0 + } + ], + "factors": { + "bilateral_data_root_seed": [ + 2026072733 + ], + "stable_contact_selection": [ + { + "source_study_id": "g0c_bilateral_calibration_v3_stable_contact", + "environment_profile_id": "stable_candidate_k3200", + "haptic_profile_id": "gain_035_wide_tank", + "required_gate": "bilateral_stable_contact_gate_pass", + "selection_status": "provisional_smoke_only" + } + ], + "map_policy": [ + "source_stamped" + ], + "environment_profile": [ + { + "profile_id": "stable_mechanics_k3200", + "duration": 6.0, + "mapping_hz": 50.0, + "wall_fraction": 0.5, + "stiffness": 3200.0, + "damping": 25.0, + "force_limit": 20.0, + "transition_depth": 0.001, + "probe_fraction": 0.0, + "contact_probe_cycles": 0.0 + } + ], + "haptic_profile": [ + { + "profile_id": "velocity_aligned_generalized_stress", + "feedback_strength": 0.35, + "energy_min": 0.0, + "energy_max": 0.002, + "energy_initial": 0.002, + "energy_probe_mode": "velocity_aligned_generalized", + "energy_probe_torque_Nm": 0.0395, + "energy_probe_start_fraction": 0.2, + "energy_probe_end_fraction": 0.4 + } + ], + "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 + ] + }, + "h3_eligible": false, + "status": "Provisional three-seed synthetic supervisor stress: a smooth sin-squared generalized torque is injected upstream of shaping and the tank; it shares exogenous streams with the k3200 stable-contact control and remains excluded from H3 physical port-pair claims" +} diff --git a/code/config/experiments/bilateral_calibration_v3_stable_contact.json b/code/config/experiments/bilateral_calibration_v3_stable_contact.json new file mode 100644 index 0000000..b89a349 --- /dev/null +++ b/code/config/experiments/bilateral_calibration_v3_stable_contact.json @@ -0,0 +1,99 @@ +{ + "study_id": "g0c_bilateral_calibration_v3_stable_contact", + "split": "calibration", + "root_seed": 2026072731, + "replicates": 3, + "methods": [ + "proposed_energy" + ], + "trajectories": [ + { + "id": "slow_contact_approach", + "family": "contact_roundtrip", + "duration_s": 6.0, + "contact_probe_fraction": 0.0 + } + ], + "factors": { + "bilateral_data_root_seed": [ + 2026072733 + ], + "map_policy": [ + "source_stamped" + ], + "environment_profile": [ + { + "profile_id": "stable_candidate_k3200", + "duration": 6.0, + "mapping_hz": 50.0, + "wall_fraction": 0.5, + "stiffness": 3200.0, + "damping": 25.0, + "force_limit": 20.0, + "transition_depth": 0.001, + "probe_fraction": 0.0, + "contact_probe_cycles": 0.0 + }, + { + "profile_id": "stiff_candidate_k6400", + "duration": 6.0, + "mapping_hz": 50.0, + "wall_fraction": 0.5, + "stiffness": 6400.0, + "damping": 25.0, + "force_limit": 20.0, + "transition_depth": 0.001, + "probe_fraction": 0.0, + "contact_probe_cycles": 0.0 + } + ], + "haptic_profile": [ + { + "profile_id": "gain_020_wide_tank", + "feedback_strength": 0.2, + "energy_min": 0.0, + "energy_max": 2.0, + "energy_initial": 1.0, + "energy_probe_mode": "none", + "energy_probe_torque_Nm": 0.0 + }, + { + "profile_id": "gain_035_wide_tank", + "feedback_strength": 0.35, + "energy_min": 0.0, + "energy_max": 2.0, + "energy_initial": 1.0, + "energy_probe_mode": "none", + "energy_probe_torque_Nm": 0.0 + }, + { + "profile_id": "gain_050_wide_tank", + "feedback_strength": 0.5, + "energy_min": 0.0, + "energy_max": 2.0, + "energy_initial": 1.0, + "energy_probe_mode": "none", + "energy_probe_torque_Nm": 0.0 + } + ], + "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 + ] + }, + "status": "Three-seed calibration grid only, not frozen evidence: slow 6 s contact, no trajectory probe or network impairment; haptic gains share exogenous streams within each replicate and stiffness profile" +} diff --git a/code/config/experiments/h1_calibration_v3.json b/code/config/experiments/h1_calibration_v3.json new file mode 100644 index 0000000..912fc9a --- /dev/null +++ b/code/config/experiments/h1_calibration_v3.json @@ -0,0 +1,77 @@ +{ + "study_id": "g0c_sew_branch_calibration_v3", + "split": "calibration", + "root_seed": 2026072731, + "replicates": 1, + "methods": [ + "sew", + "scaled_joint_space", + "bounded_dls_ik", + "task_priority_ik" + ], + "trajectories": [ + { + "id": "sew_phi_wrap_linear", + "family": "sew_phi_wrap_valid", + "path_type": "linear", + "sample_count": 81, + "start": [ + -1.41892269, + -1.12163753, + -0.38803584, + 2.07, + -0.11990672, + 0.28954871, + -0.28440543 + ], + "end": [ + -1.41892269, + -1.12163753, + -0.38803584, + 2.16, + -0.11990672, + 0.28954871, + -0.28440543 + ], + "actual_differential": true, + "differential_fd_step_rad": 0.0001, + "differential_branch_jump_threshold_rad": 0.25, + "differential_consistency_tolerance": 0.05, + "instance_variation": { + "enabled": false + } + }, + { + "id": "sew_phi_wrap_roundtrip", + "family": "sew_phi_wrap_valid", + "path_type": "cosine_roundtrip", + "sample_count": 81, + "start": [ + -1.41892269, + -1.12163753, + -0.38803584, + 2.07, + -0.11990672, + 0.28954871, + -0.28440543 + ], + "end": [ + -1.41892269, + -1.12163753, + -0.38803584, + 2.16, + -0.11990672, + 0.28954871, + -0.28440543 + ], + "actual_differential": true, + "differential_fd_step_rad": 0.0001, + "differential_branch_jump_threshold_rad": 0.25, + "differential_consistency_tolerance": 0.05, + "instance_variation": { + "enabled": false + } + } + ], + "status": "Deterministic branch-crossing regression fixture; one fixed path instance is not statistical calibration or tail-latency evidence" +} diff --git a/code/config/experiments/metrics_bilateral_v3_energy_challenge.json b/code/config/experiments/metrics_bilateral_v3_energy_challenge.json new file mode 100644 index 0000000..d07ba23 --- /dev/null +++ b/code/config/experiments/metrics_bilateral_v3_energy_challenge.json @@ -0,0 +1,58 @@ +{ + "enabled": [ + "h4", + "bilateral", + "energy_challenge" + ], + "h4": { + "include_methods": [ + "proposed_energy" + ], + "epsilon_torque_impulse_Nms": 1e-12, + "audit_tolerance_J": 1e-10 + }, + "bilateral": { + "include_methods": [ + "proposed_energy" + ], + "projection_tolerance": 1e-12, + "contact_force_threshold_N": 0.000001, + "required_audit_fields": [ + "wall_force_raw_N", + "wall_force_applied_N", + "wall_force_saturation_active", + "configured_wall_force_limit_N", + "master_joint_limit_active", + "slave_joint_limit_active", + "master_velocity_limit_active", + "slave_velocity_limit_active", + "master_acceleration_limit_active", + "slave_acceleration_limit_active", + "master_torque_saturation_active", + "slave_torque_saturation_active", + "haptic_rate_limit_active", + "haptic_torque_saturation_active", + "energy_probe_raw_work_J", + "h3_eligible" + ], + "gates": { + "minimum_contact_fraction": 0.01, + "minimum_contact_rms_N": 0.1, + "maximum_force_limit_hit_fraction": 0.0, + "minimum_force_headroom_N": 1.0, + "maximum_master_tracking_rmse_rad": 0.08, + "maximum_slave_tracking_rmse_rad": 0.08, + "minimum_projection_intervention_fraction": 0.02, + "maximum_projection_intervention_fraction": 0.3, + "maximum_limit_active_fraction": 0.0, + "minimum_energy_probe_raw_work_J": 0.001 + } + }, + "energy_challenge": { + "minimum_shadow_deficit_J": 0.00001, + "maximum_downstream_modification_Nm": 1e-10, + "maximum_D_proj": 0.6 + }, + "h3_eligible": false, + "status": "Synthetic H4 supervisor stress only; H3 is intentionally disabled because the upstream velocity-aligned probe has no paired slave-port source" +} diff --git a/code/config/experiments/metrics_bilateral_v3_stable_contact.json b/code/config/experiments/metrics_bilateral_v3_stable_contact.json new file mode 100644 index 0000000..e9bcf35 --- /dev/null +++ b/code/config/experiments/metrics_bilateral_v3_stable_contact.json @@ -0,0 +1,57 @@ +{ + "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" + ], + "epsilon_torque_impulse_Nms": 1e-12, + "audit_tolerance_J": 1e-10 + }, + "bilateral": { + "include_methods": [ + "proposed_energy" + ], + "projection_tolerance": 1e-12, + "contact_force_threshold_N": 0.000001, + "required_audit_fields": [ + "wall_force_raw_N", + "wall_force_applied_N", + "wall_force_saturation_active", + "configured_wall_force_limit_N", + "master_joint_limit_active", + "slave_joint_limit_active", + "master_velocity_limit_active", + "slave_velocity_limit_active", + "master_acceleration_limit_active", + "slave_acceleration_limit_active", + "master_torque_saturation_active", + "slave_torque_saturation_active", + "haptic_rate_limit_active", + "haptic_torque_saturation_active", + "energy_probe_raw_work_J", + "h3_eligible" + ], + "gates": { + "minimum_contact_fraction": 0.01, + "minimum_contact_rms_N": 0.1, + "maximum_force_limit_hit_fraction": 0.0, + "minimum_force_headroom_N": 1.0, + "maximum_master_tracking_rmse_rad": 0.08, + "maximum_slave_tracking_rmse_rad": 0.08, + "minimum_projection_intervention_fraction": 0.0, + "maximum_projection_intervention_fraction": 0.0, + "maximum_limit_active_fraction": 0.0, + "minimum_energy_probe_raw_work_J": 0.0 + } + }, + "status": "Calibration gates use contact-active RMS, require observable contact, and reject any wall force-limit hit; thresholds are provisional and must be frozen only after the complete calibration grid" +} diff --git a/code/config/experiments/metrics_h1_calibration_v3.json b/code/config/experiments/metrics_h1_calibration_v3.json new file mode 100644 index 0000000..ed2605c --- /dev/null +++ b/code/config/experiments/metrics_h1_calibration_v3.json @@ -0,0 +1,43 @@ +{ + "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" + ], + "branch_timing": { + "deadline_s": 0.02, + "minimum_phi_wrap_crossings": 1 + } + }, + "status": "deterministic branch fixture with descriptive workstation timing; 20 ms is the 50 Hz accounting budget, not a statistical latency acceptance threshold" +} diff --git a/code/core/sew_mapper2.py b/code/core/sew_mapper2.py index bcb80ea..3a34a4b 100644 --- a/code/core/sew_mapper2.py +++ b/code/core/sew_mapper2.py @@ -317,7 +317,8 @@ class SEWMapper: nm = arm_normal_raw / arm_normal_norm nref_tilde = self.up - float(np.dot(self.up, xhat)) * xhat - reference_fallback = float(np.linalg.norm(nref_tilde)) < 1e-6 + reference_axis_norm = float(np.linalg.norm(nref_tilde)) + reference_fallback = reference_axis_norm < 1e-6 if reference_fallback: events.append("reference_axis_fallback") candidates = ( @@ -374,10 +375,14 @@ class SEWMapper: "events": events, "hard_geometry_valid": hard_geometry_valid, "reference_fallback": reference_fallback, + "reference_axis_norm": reference_axis_norm, + "phi_rad": phi, "reach_clipped": clip_region != "none", "clip_region": clip_region, "master_reach": d_m, "slave_reach": d_s, + "reach_lower_margin_m": d_m - d_min, + "reach_upper_margin_m": d_max - d_m, "master_arm_normal_norm": arm_normal_norm, } @@ -619,8 +624,12 @@ class SEWMapper: "near_limit": bool(metrics["joint_limit_active_indices"]), "position_error": position_error, "reference_fallback": target["reference_fallback"], + "reference_axis_norm": target["reference_axis_norm"], + "phi_rad": target["phi_rad"], "master_reach": target["master_reach"], "slave_reach": target["slave_reach"], + "reach_lower_margin_m": target["reach_lower_margin_m"], + "reach_upper_margin_m": target["reach_upper_margin_m"], "master_arm_normal_norm": target["master_arm_normal_norm"], "solver_success": bool(result.success), "solver_status": int(result.status), diff --git a/code/experiments/README.md b/code/experiments/README.md index 10c4857..d80ae1c 100644 --- a/code/experiments/README.md +++ b/code/experiments/README.md @@ -36,6 +36,21 @@ execute_bilateral_simulation bilateral_calibration_v2_energy.json execute_bilateral_simulation bilateral_calibration_v2_network.json ``` +The v3 redesign separates branch-crossing evidence from contact/energy stress: + +```text +execute_h1_retargeting h1_calibration_v3.json +execute_bilateral_simulation bilateral_calibration_v3_stable_contact.json +execute_bilateral_simulation bilateral_calibration_v3_energy_challenge.json +``` + +`h1_calibration_v3.json` is a deterministic branch-regression fixture, not +statistical tail-latency evidence. The bilateral v3 stable grid uses three +exogenous seed groups shared across haptic gains. The synthetic energy +challenge shares the corresponding `k=3200 N/m` groups, is explicitly +ineligible for H3, and must be analyzed only with its H4/challenge metric +configuration. + The bilateral network specification is a gated Stage B template. Its `requires_stage_a_selection` flag means the haptic parameters are placeholders; do not execute it as a locked study until the energy/gain Stage A acceptance @@ -78,6 +93,11 @@ h1_calibration_v2.json metrics_h1_calibration_v2.json h2_calibration*.json metrics_h2.json bilateral_calibration.json metrics_bilateral.json bilateral_calibration_v2_*.json metrics_bilateral_v2.json +h1_calibration_v3.json metrics_h1_calibration_v3.json +bilateral_calibration_v3_stable_contact.json + metrics_bilateral_v3_stable_contact.json +bilateral_calibration_v3_energy_challenge.json + metrics_bilateral_v3_energy_challenge.json ``` The bilateral configurations derive H3 for every mapping/supervisor condition, diff --git a/code/experiments/executors.py b/code/experiments/executors.py index cd2c17c..90126eb 100644 --- a/code/experiments/executors.py +++ b/code/experiments/executors.py @@ -9,6 +9,7 @@ from __future__ import annotations from dataclasses import replace from enum import Enum +from time import perf_counter from typing import Any, Mapping import numpy as np @@ -112,6 +113,13 @@ def _master_trajectory( sample_count = int(specification.get("sample_count", 81)) if sample_count < 3: raise ValueError("H1 trajectory sample_count must be at least three") + path_type = str( + specification.get("path_type", "cosine_roundtrip") + ) + if path_type not in {"linear", "cosine_roundtrip"}: + raise ValueError( + "H1 path_type must be 'linear' or 'cosine_roundtrip'" + ) family = str(specification.get("family", "nominal")) center = np.asarray( specification.get( @@ -158,6 +166,20 @@ def _master_trajectory( 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]) + explicit_start = specification.get("start") + explicit_end = specification.get("end") + if (explicit_start is None) != (explicit_end is None): + raise ValueError("H1 explicit paths require both start and end") + if explicit_start is not None: + start = np.asarray(explicit_start, dtype=float) + end = np.asarray(explicit_end, dtype=float) + if start.shape != (7,) or end.shape != (7,): + raise ValueError("H1 start and end must have seven entries") + displacement = end - start + else: + start = center.copy() + displacement = delta.copy() + variation = specification.get("instance_variation", {}) if not isinstance(variation, Mapping): raise ValueError("H1 instance_variation must be a mapping") @@ -199,8 +221,10 @@ def _master_trajectory( 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( + start = start + ( + center_std * jitter_mask * trajectory_rng.normal(size=7) + ) + displacement = displacement * trajectory_rng.uniform( delta_scale_range[0], delta_scale_range[1], size=7 ) harmonic_weight = float( @@ -210,11 +234,20 @@ def _master_trajectory( harmonic_weight = 0.0 phase = np.linspace(0.0, 1.0, sample_count) - # One cosine excursion starts and ends at the same configuration with zero - # endpoint velocity, making discontinuities attributable to the mapper. - 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, :] + if path_type == "linear": + path_coordinate = phase + else: + # One cosine excursion starts and ends at the same configuration with + # zero endpoint velocity, making discontinuities attributable to the + # mapper rather than an endpoint reset. + path_coordinate = 0.5 - 0.5 * np.cos(2.0 * np.pi * phase) + path_coordinate *= ( + 1.0 + harmonic_weight * np.sin(2.0 * np.pi * phase) + ) + trajectory = ( + start[None, :] + + path_coordinate[:, None] * displacement[None, :] + ) margin = 1e-4 if np.any(trajectory < lower + margin) or np.any(trajectory > upper - margin): raise ValueError( @@ -340,6 +373,36 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload: if method_id not in methods: raise ValueError(f"unknown H1 method {method_id!r}") method = methods[method_id] + specification = _trajectory_spec(trial) + actual_differential_requested = bool( + specification.get("actual_differential", False) + ) + differential_applicable_for_method = bool( + actual_differential_requested and method_id == "sew" + ) + differential_fd_step = float( + specification.get("differential_fd_step_rad", 1e-4) + ) + differential_branch_jump_threshold = float( + specification.get( + "differential_branch_jump_threshold_rad", 0.25 + ) + ) + differential_consistency_tolerance = float( + specification.get( + "differential_consistency_tolerance", 5e-2 + ) + ) + differential_settings = ( + differential_fd_step, + differential_branch_jump_threshold, + differential_consistency_tolerance, + ) + if any( + not np.isfinite(value) or value <= 0.0 + for value in differential_settings + ): + raise ValueError("H1 differential settings must be positive") lower, upper = finite_joint_limits(models.master, MASTER_JOINT_NAMES) q_master = _master_trajectory(trial, lower=lower, upper=upper) @@ -348,7 +411,17 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload: position_error = np.empty(sample_count) orientation_error = np.empty(sample_count) success = np.empty(sample_count, dtype=np.int8) - smooth = np.empty(sample_count, dtype=np.int8) + branch_smooth = np.empty(sample_count, dtype=np.int8) + differential_valid = np.empty(sample_count, dtype=np.int8) + differential_applicable = np.full( + sample_count, int(differential_applicable_for_method), dtype=np.int8 + ) + differential_A = np.full((sample_count, 7, 7), np.nan) + differential_runtime_s = np.zeros(sample_count) + differential_event_count = np.zeros(sample_count, dtype=np.int16) + differential_max_consistency = np.full(sample_count, np.nan) + differential_max_column_jump = np.full(sample_count, np.nan) + differential_branch_jump = np.zeros(sample_count, dtype=np.int8) failure_code = np.empty(sample_count, dtype=np.int16) solver_status = np.empty(sample_count, dtype=np.int16) iterations = np.empty(sample_count, dtype=np.int32) @@ -363,6 +436,12 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload: slave_min_singular_value = np.empty(sample_count) slave_manipulability = np.empty(sample_count) low_manipulability = np.zeros(sample_count, dtype=np.int8) + sew_phi_rad = np.empty(sample_count) + reference_axis_norm = np.empty(sample_count) + reach_lower_margin_m = np.empty(sample_count) + reach_upper_margin_m = np.empty(sample_count) + master_arm_normal_norm = np.empty(sample_count) + warm_start = np.empty(sample_count, dtype=np.int8) events: list[dict[str, Any]] = [] slave_data = models.slave.createData() @@ -384,12 +463,85 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload: seed = None previous_swivel = 0.0 for index, q_m in enumerate(q_master): + warm_start[index] = int(seed is not None) + target_debug = sew.mapper._target_from_master(q_m) + sew_phi_rad[index] = float(target_debug["phi_rad"]) + reference_axis_norm[index] = float( + target_debug["reference_axis_norm"] + ) + reach_lower_margin_m[index] = float( + target_debug["reach_lower_margin_m"] + ) + reach_upper_margin_m[index] = float( + target_debug["reach_upper_margin_m"] + ) + master_arm_normal_norm[index] = float( + target_debug["master_arm_normal_norm"] + ) result = method.retarget(q_m, q_slave_seed=seed) q_slave_log[index] = result.q_slave position_error[index] = result.diagnostics.position_error_m orientation_error[index] = result.diagnostics.orientation_error_rad success[index] = int(result.success) - smooth[index] = int(result.smooth) + branch_smooth[index] = int(result.smooth) + differential_valid[index] = int(result.smooth) + differential_info: Mapping[str, Any] | None = None + if differential_applicable_for_method: + differential_started = perf_counter() + differential_result, differential_info = ( + sew.mapper.compute_differential( + q_m, + q_s_init=result.q_slave, + fd_step=differential_fd_step, + branch_jump_threshold=( + differential_branch_jump_threshold + ), + consistency_tolerance=( + differential_consistency_tolerance + ), + ) + ) + differential_runtime_s[index] = ( + perf_counter() - differential_started + ) + differential_A[index] = differential_result + differential_valid[index] = int( + bool(differential_info["valid"]) + ) + differential_events = tuple( + str(event) + for event in differential_info.get("events", ()) + ) + differential_event_count[index] = len(differential_events) + columns = tuple(differential_info.get("columns", ())) + if columns: + differential_max_consistency[index] = max( + float(column["one_sided_consistency"]) + for column in columns + ) + differential_max_column_jump[index] = max( + max( + float(column["plus_jump"]), + float(column["minus_jump"]), + ) + for column in columns + ) + differential_branch_jump[index] = int( + any( + "branch_jump" in column.get("events", ()) + for column in columns + ) + ) + for differential_event in differential_events: + events.append( + { + "sample_index": index, + "event": f"differential:{differential_event}", + "differential_valid": bool( + differential_valid[index] + ), + } + ) failure_code[index] = _enum_code(result.failure) solver_status[index] = _enum_code(result.diagnostics.status) iterations[index] = result.diagnostics.iterations @@ -456,7 +608,13 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload: "validity_reason_code": int(validity_reason[index]), } ) - if not result.smooth: + if ( + not result.smooth + or ( + differential_applicable_for_method + and not differential_valid[index] + ) + ): events.append( { "sample_index": index, @@ -464,6 +622,9 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload: "failure_code": int(failure_code[index]), "validity_reason": reason.value, "validity_reason_code": int(validity_reason[index]), + "differential_applicable": ( + differential_applicable_for_method + ), } ) @@ -478,12 +639,30 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload: "q_master": q_master, "map_q_slave": q_slave_log, "map_pose_success": success, - # H1 requires a valid/smooth branch. Differential A is evaluated in a - # separate diagnostic study and is not silently imputed here. - "map_differential_valid": smooth, + "map_branch_smooth": branch_smooth, + # For v2-compatible trials without an actual differential evaluation, + # this retains the historical branch-smooth proxy. Applicability makes + # that distinction explicit for v3 analysis. + "map_differential_valid": differential_valid, + "map_differential_applicable": differential_applicable, + "map_differential_A": differential_A, + "map_differential_runtime_s": differential_runtime_s, + "map_differential_event_count": differential_event_count, + "map_differential_max_one_sided_consistency": ( + differential_max_consistency + ), + "map_differential_max_column_jump_rad": ( + differential_max_column_jump + ), + "map_differential_branch_jump": differential_branch_jump, "map_position_error_m": position_error, "map_orientation_error_rad": orientation_error, "map_swivel_angle_rad": swivel, + "map_sew_phi_rad": sew_phi_rad, + "map_reference_axis_norm": reference_axis_norm, + "map_reach_lower_margin_m": reach_lower_margin_m, + "map_reach_upper_margin_m": reach_upper_margin_m, + "map_master_arm_normal_norm": master_arm_normal_norm, "map_master_step_norm": master_step, "map_accepted": np.ones(sample_count, dtype=np.int8), "map_commanded_reset": np.zeros(sample_count, dtype=np.int8), @@ -499,6 +678,7 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload: "map_solver_status": solver_status, "map_solver_iterations": iterations, "map_runtime_s": runtime_s, + "map_warm_start": warm_start, "map_solver_cost": solver_cost, } return TrialPayload( @@ -525,7 +705,34 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload: "trajectory_instance_hash": stable_hash( q_master, prefix="h1-master-trajectory-instance" ), - "trajectory_family": _trajectory_spec(trial).get("family", "nominal"), + "trajectory_family": specification.get("family", "nominal"), + "path_type": specification.get( + "path_type", "cosine_roundtrip" + ), + "actual_differential_requested": ( + actual_differential_requested + ), + "actual_differential_applicable": ( + differential_applicable_for_method + ), + "differential_n_a_reason": ( + None + if differential_applicable_for_method + else ( + "actual differential was not requested" + if not actual_differential_requested + else "actual differential is currently implemented for SEW only" + ) + ), + "differential_settings": { + "fd_step_rad": differential_fd_step, + "branch_jump_threshold_rad": ( + differential_branch_jump_threshold + ), + "consistency_tolerance": ( + differential_consistency_tolerance + ), + }, }, ) @@ -784,6 +991,100 @@ def execute_h2_synthetic(trial: Mapping[str, Any]) -> TrialPayload: ) +def _bilateral_environment_factor( + trial: Mapping[str, Any], + name: str, + default: Any, + *, + aliases: tuple[str, ...] = (), +) -> Any: + """Resolve direct/environment-profile factors with explicit precedence.""" + factors = trial.get("factors", {}) + if not isinstance(factors, Mapping): + raise ValueError("trial factors must be a mapping") + for candidate in (name, *aliases): + if candidate in factors: + return factors[candidate] + profile = factors.get("environment_profile", {}) + if not isinstance(profile, Mapping): + raise ValueError("environment_profile factor must be a mapping") + for candidate in (name, *aliases): + if candidate in profile: + return profile[candidate] + return default + + +def _bilateral_data_seed_group( + trial: Mapping[str, Any], + config: SimulationConfig, +) -> tuple[ + str | None, + dict[str, Any] | None, + dict[str, list[int]] | None, +]: + """Create common exogenous streams across haptic tuning profiles. + + The default plan seed includes every factor, so changing only haptic gain + would otherwise also change sensor noise and network traces. A declared + ``bilateral_data_root_seed`` instead hashes only the effective mechanical, + trajectory, network, and replicate inputs. Stable-contact and energy + challenge batches can therefore share a trace without pretending that + their treatment settings belong to the same immutable plan pair. + """ + factors = trial.get("factors", {}) + if not isinstance(factors, Mapping): + raise ValueError("trial factors must be a mapping") + raw_root_seed = factors.get("bilateral_data_root_seed") + if raw_root_seed is None: + return None, None, None + data_root_seed = int(raw_root_seed) + if data_root_seed < 0: + raise ValueError("bilateral_data_root_seed must be non-negative") + basis = { + "data_root_seed": data_root_seed, + "trajectory_family": str( + _trajectory_spec(trial).get("family", "") + ), + "replicate": int(trial.get("replicate", 0)), + "mechanical_and_network_inputs": { + "dt_s": config.dt, + "duration_s": config.duration, + "mapping_hz": config.mapping_hz, + "contact_probe_fraction": config.contact_probe_fraction, + "contact_probe_cycles": config.contact_probe_cycles, + "wall_fraction": config.wall_fraction, + "wall_stiffness_N_per_m": config.wall_stiffness, + "wall_damping_Ns_per_m": config.wall_damping, + "wall_force_limit_N": config.wall_force_limit, + "wall_transition_depth_m": config.wall_transition_depth, + "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, + "sensor_noise_std_Nm": config.sensor_noise_std, + "bias_calibration_samples": config.bias_calibration_samples, + "master_kp": list(config.master_kp), + "master_kd": list(config.master_kd), + "slave_kp": list(config.slave_kp), + "slave_kd": list(config.slave_kd), + }, + } + group_id = ( + "bilateral-data-" + + stable_hash(basis, prefix="bilateral-data-group")[:16] + ) + seed_record = named_seed_record( + data_root_seed, + basis, + ("simulation",), + ) + return group_id, basis, seed_record + + def _bilateral_scenario_and_config( trial: Mapping[str, Any], ) -> tuple[Any, SimulationConfig]: @@ -799,12 +1100,40 @@ def _bilateral_scenario_and_config( trajectory = _trajectory_spec(trial) trajectory_rng = generator_from_record(trial["seeds"], "trajectory") seed = int(trajectory_rng.integers(0, np.iinfo(np.int32).max)) + duration_default = float(trajectory.get("duration_s", 1.2)) + probe_fraction_default = float( + trajectory.get("contact_probe_fraction", 0.0) + ) config = replace( SimulationConfig(), seed=seed, - duration=float(trajectory.get("duration_s", 1.2)), + duration=float( + _bilateral_environment_factor( + trial, + "duration", + duration_default, + aliases=("duration_s",), + ) + ), + mapping_hz=float( + _bilateral_environment_factor( + trial, "mapping_hz", SimulationConfig.mapping_hz + ) + ), contact_probe_fraction=float( - trajectory.get("contact_probe_fraction", 0.0) + _bilateral_environment_factor( + trial, + "contact_probe_fraction", + probe_fraction_default, + aliases=("probe_fraction",), + ) + ), + contact_probe_cycles=float( + _bilateral_environment_factor( + trial, + "contact_probe_cycles", + SimulationConfig.contact_probe_cycles, + ) ), feedback_delay_s=float( _profiled_factor( @@ -902,9 +1231,88 @@ def _bilateral_scenario_and_config( profile_name="haptic_profile", ) ), - wall_stiffness=float(_factor(trial, "wall_stiffness", 800.0)), - wall_damping=float(_factor(trial, "wall_damping", 45.0)), + energy_probe_mode=str( + _profiled_factor( + trial, + "energy_probe_mode", + "none", + profile_name="haptic_profile", + ) + ), + energy_probe_torque_Nm=float( + _profiled_factor( + trial, + "energy_probe_torque_Nm", + 0.0, + profile_name="haptic_profile", + ) + ), + energy_probe_start_fraction=float( + _profiled_factor( + trial, + "energy_probe_start_fraction", + 0.20, + profile_name="haptic_profile", + ) + ), + energy_probe_end_fraction=float( + _profiled_factor( + trial, + "energy_probe_end_fraction", + 0.80, + profile_name="haptic_profile", + ) + ), + wall_fraction=float( + _bilateral_environment_factor( + trial, "wall_fraction", SimulationConfig.wall_fraction + ) + ), + wall_stiffness=float( + _bilateral_environment_factor( + trial, + "wall_stiffness", + SimulationConfig.wall_stiffness, + aliases=("stiffness",), + ) + ), + wall_damping=float( + _bilateral_environment_factor( + trial, + "wall_damping", + SimulationConfig.wall_damping, + aliases=("damping",), + ) + ), + wall_force_limit=float( + _bilateral_environment_factor( + trial, + "wall_force_limit", + SimulationConfig.wall_force_limit, + aliases=("force_limit",), + ) + ), + wall_transition_depth=float( + _bilateral_environment_factor( + trial, + "wall_transition_depth", + SimulationConfig.wall_transition_depth, + aliases=("transition_depth",), + ) + ), ) + data_group_id, _, data_seeds = _bilateral_data_seed_group( + trial, config + ) + if data_group_id is not None: + assert data_seeds is not None + data_rng = generator_from_record(data_seeds, "simulation") + config = replace( + config, + seed=int( + data_rng.integers(0, np.iinfo(np.int32).max) + ), + ) config.validate() return scenario, config @@ -912,6 +1320,9 @@ def _bilateral_scenario_and_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) + data_group_id, data_group_basis, data_seed_record = ( + _bilateral_data_seed_group(trial, config) + ) trajectory = _trajectory_spec(trial) models = load_models(add_simulated_tcp=True) mapper = build_mapper(models) @@ -954,6 +1365,13 @@ def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload: samples["configured_energy_initial_J"] = np.full( sample_count, config.energy_initial ) + samples["configured_wall_force_limit_N"] = np.full( + sample_count, config.wall_force_limit + ) + h3_eligible = config.energy_probe_mode == "none" + samples["h3_eligible"] = np.full( + sample_count, int(h3_eligible), dtype=np.int8 + ) return TrialPayload( samples=samples, events=(), @@ -973,6 +1391,36 @@ def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload: "energy_max_J": config.energy_max, "energy_initial_J": config.energy_initial, }, + "energy_probe": { + "mode": config.energy_probe_mode, + "torque_Nm": config.energy_probe_torque_Nm, + "start_fraction": config.energy_probe_start_fraction, + "end_fraction": config.energy_probe_end_fraction, + "window": "hann_squared_sine", + "raw_work_J": result.metrics[ + "energy_probe_raw_work_J" + ], + "purpose": ( + "synthetic upstream supervisor stress; not a physical " + "environment input" + ), + }, + "h3_eligible": h3_eligible, + "h3_ineligibility_reason": ( + None + if h3_eligible + else ( + "synthetic velocity-aligned generalized torque is " + "injected upstream " + "of the haptic supervisor and has no slave-port pair" + ) + ), + "bilateral_data_group_id": data_group_id, + "bilateral_data_group_basis": data_group_basis, + "bilateral_data_seed_record": data_seed_record, + "stable_contact_selection": _factor( + trial, "stable_contact_selection", None + ), "effective_network_config": { "forward_delay_s": config.forward_delay_s, "return_delay_s": config.feedback_delay_s, @@ -983,6 +1431,17 @@ def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload: "forward_timeout_s": config.forward_timeout_s, "return_timeout_s": config.return_timeout_s, }, + "effective_environment_config": { + "duration_s": config.duration, + "mapping_hz": config.mapping_hz, + "contact_probe_fraction": config.contact_probe_fraction, + "contact_probe_cycles": config.contact_probe_cycles, + "wall_fraction": config.wall_fraction, + "wall_stiffness_N_per_m": config.wall_stiffness, + "wall_damping_Ns_per_m": config.wall_damping, + "wall_force_limit_N": config.wall_force_limit, + "wall_transition_depth_m": config.wall_transition_depth, + }, "wall": wall_metadata, "online_metrics_are_diagnostic_only": result.metrics, }, diff --git a/code/simulate_closed_loop.py b/code/simulate_closed_loop.py index 3689378..33a5f27 100644 --- a/code/simulate_closed_loop.py +++ b/code/simulate_closed_loop.py @@ -184,6 +184,10 @@ class SimulationConfig: energy_min: float = 0.05 energy_max: float = 0.055 energy_initial: float = 0.05 + energy_probe_mode: str = "none" + energy_probe_torque_Nm: float = 0.0 + energy_probe_start_fraction: float = 0.20 + energy_probe_end_fraction: float = 0.80 sensor_noise_std: float = 0.001 wrench_characteristic_length_m: float = 0.30 @@ -242,6 +246,31 @@ class SimulationConfig: raise ValueError( "energy values must satisfy 0 <= min <= initial <= max" ) + if self.energy_probe_mode not in { + "none", + "velocity_aligned_generalized", + }: + raise ValueError( + "energy_probe_mode must be 'none' or " + "'velocity_aligned_generalized'" + ) + if ( + not np.isfinite(self.energy_probe_torque_Nm) + or self.energy_probe_torque_Nm < 0.0 + ): + raise ValueError( + "energy_probe_torque_Nm must be finite and non-negative" + ) + if not ( + 0.0 + <= self.energy_probe_start_fraction + <= self.energy_probe_end_fraction + <= 1.0 + ): + raise ValueError( + "energy probe fractions must satisfy " + "0 <= start <= end <= 1" + ) if len(self.haptic_torque_limits) != 7: raise ValueError("haptic_torque_limits must contain seven entries") if len(self.haptic_rate_limits) != 7: @@ -359,6 +388,17 @@ def load_simulation_config(path: Path) -> SimulationConfig: return config +@dataclass(frozen=True) +class WallContactResult: + """One unilateral wall evaluation before and after force limiting.""" + + wrench_applied: np.ndarray + penetration: float + force_raw_N: float + force_applied_N: float + saturation_active: bool + + @dataclass(frozen=True) class Wall: point: np.ndarray @@ -368,34 +408,55 @@ class Wall: force_limit: float transition_depth: float = 0.0 - def wrench( + def contact( self, position_world: np.ndarray, linear_velocity_world: np.ndarray, - ) -> tuple[np.ndarray, float]: - """Return environment-on-robot wrench and unilateral penetration.""" + ) -> WallContactResult: + """Return raw/applied wall force diagnostics and the applied wrench.""" penetration = max( 0.0, float(np.dot(self.normal, position_world - self.point)), ) if penetration <= 0.0: - return np.zeros(6, dtype=float), 0.0 + return WallContactResult( + wrench_applied=np.zeros(6, dtype=float), + penetration=0.0, + force_raw_N=0.0, + force_applied_N=0.0, + saturation_active=False, + ) normal_velocity = float(np.dot(self.normal, linear_velocity_world)) if self.transition_depth > 0.0: damping_activation = min(1.0, penetration / self.transition_depth) else: damping_activation = 1.0 - force_magnitude = min( - self.force_limit, - max( - 0.0, - self.stiffness * penetration - + damping_activation * self.damping * normal_velocity, - ), + force_raw_N = max( + 0.0, + self.stiffness * penetration + + damping_activation * self.damping * normal_velocity, ) - force = -force_magnitude * self.normal - return np.concatenate((force, np.zeros(3, dtype=float))), penetration + force_applied_N = min(self.force_limit, force_raw_N) + force = -force_applied_N * self.normal + return WallContactResult( + wrench_applied=np.concatenate( + (force, np.zeros(3, dtype=float)) + ), + penetration=penetration, + force_raw_N=force_raw_N, + force_applied_N=force_applied_N, + saturation_active=force_raw_N > self.force_limit, + ) + + def wrench( + self, + position_world: np.ndarray, + linear_velocity_world: np.ndarray, + ) -> tuple[np.ndarray, float]: + """Return applied wrench/penetration with the legacy call signature.""" + result = self.contact(position_world, linear_velocity_world) + return result.wrench_applied, result.penetration @dataclass @@ -878,6 +939,9 @@ def simulate_scenario( "dt", "missed_deadline", "contact_force_norm", + "wall_force_raw_N", + "wall_force_applied_N", + "wall_force_saturation_active", "penetration", "force_estimation_error_norm", "moment_estimation_error_norm", @@ -906,6 +970,19 @@ def simulate_scenario( "slave_tracking_error", "feedback_torque_norm", "mapped_torque_norm", + "energy_probe_raw_power_W", + "energy_probe_raw_work_J", + "energy_probe_envelope", + "master_joint_limit_active", + "slave_joint_limit_active", + "master_velocity_limit_active", + "slave_velocity_limit_active", + "master_acceleration_limit_active", + "slave_acceleration_limit_active", + "master_torque_saturation_active", + "slave_torque_saturation_active", + "haptic_rate_limit_active", + "haptic_torque_saturation_active", ) vector_keys = ( "q_master", @@ -923,6 +1000,7 @@ def simulate_scenario( "tau_master_candidate", "tau_master_applied", "tau_master_accepted", + "energy_probe_torque_Nm", "wrench_external", "wrench_estimated", "wrench_feedback_source", @@ -950,6 +1028,7 @@ def simulate_scenario( haptic_torque_saturation_events = 0 energy_identity_errors: list[float] = [] shadow_energy = config.energy_initial + energy_probe_raw_work_J = 0.0 contact_steps = 0 forward_packets_accepted = 0 forward_packets_rejected = 0 @@ -1068,10 +1147,12 @@ def simulate_scenario( qd_s, slave_tcp_id, ) - wrench_external, penetration = wall.wrench( + wall_contact = wall.contact( tcp_position, tcp_linear_velocity, ) + wrench_external = wall_contact.wrench_applied + penetration = wall_contact.penetration if penetration > 0.0: contact_steps += 1 tau_slave_external = J_slave_world.T @ wrench_external @@ -1115,9 +1196,10 @@ def simulate_scenario( -slave_acceleration_limits, slave_acceleration_limits, ) - slave_acceleration_limit_events += int( + slave_acceleration_limited = bool( np.any(np.abs(qdd_s_raw) > slave_acceleration_limits) ) + slave_acceleration_limit_events += int(slave_acceleration_limited) # This is an explicitly simulated load-side equivalent measurement. # It satisfies the estimator's declared residual convention: @@ -1205,6 +1287,44 @@ def simulate_scenario( if not feedback.valid: tau_master_mapped = np.zeros(models.master.nv, dtype=float) + energy_probe_torque = np.zeros(models.master.nv, dtype=float) + energy_probe_envelope = 0.0 + normalized_time = t / config.duration + energy_probe_window = ( + config.energy_probe_end_fraction + - config.energy_probe_start_fraction + ) + if ( + config.energy_probe_mode == "velocity_aligned_generalized" + and energy_probe_window > 0.0 + and config.energy_probe_start_fraction + <= normalized_time + <= config.energy_probe_end_fraction + ): + probe_phase = ( + normalized_time - config.energy_probe_start_fraction + ) / energy_probe_window + energy_probe_envelope = float( + np.sin(np.pi * probe_phase) ** 2 + ) + velocity_norm = float(np.linalg.norm(qd_m)) + if velocity_norm > 1e-12: + energy_probe_torque = ( + config.energy_probe_torque_Nm + * energy_probe_envelope + * qd_m + / velocity_norm + ) + tau_master_mapped = ( + tau_master_mapped + energy_probe_torque + ) + energy_probe_raw_power_W = float( + np.dot(energy_probe_torque, qd_m) + ) + energy_probe_raw_work_J += ( + energy_probe_raw_power_W * config.dt + ) + popc_damping_gain = 0.0 if scenario.supervisor == "tank": tau_master_applied, rho = renderer.render_mapped_reaction( @@ -1325,9 +1445,10 @@ def simulate_scenario( -master_acceleration_limits, master_acceleration_limits, ) - master_acceleration_limit_events += int( + master_acceleration_limited = bool( np.any(np.abs(qdd_m_raw) > master_acceleration_limits) ) + master_acceleration_limit_events += int(master_acceleration_limited) try: differential_for_power = ( @@ -1360,6 +1481,11 @@ def simulate_scenario( "dt": config.dt, "missed_deadline": 0, "contact_force_norm": float(np.linalg.norm(wrench_external[:3])), + "wall_force_raw_N": wall_contact.force_raw_N, + "wall_force_applied_N": wall_contact.force_applied_N, + "wall_force_saturation_active": int( + wall_contact.saturation_active + ), "penetration": penetration, "force_estimation_error_norm": float( np.linalg.norm(wrench_estimated[:3] - wrench_external[:3]) @@ -1398,6 +1524,25 @@ def simulate_scenario( ), "feedback_torque_norm": float(np.linalg.norm(tau_master_applied)), "mapped_torque_norm": float(np.linalg.norm(tau_master_mapped)), + "energy_probe_raw_power_W": energy_probe_raw_power_W, + "energy_probe_raw_work_J": energy_probe_raw_work_J, + "energy_probe_envelope": energy_probe_envelope, + "master_acceleration_limit_active": int( + master_acceleration_limited + ), + "slave_acceleration_limit_active": int( + slave_acceleration_limited + ), + "master_torque_saturation_active": int( + master_torque_clipped + ), + "slave_torque_saturation_active": int(torque_clipped), + "haptic_rate_limit_active": int( + renderer.last_rate_limit_active + ), + "haptic_torque_saturation_active": int( + renderer.last_torque_saturation_active + ), } vector_values = { "q_master": q_m.copy(), @@ -1415,6 +1560,7 @@ def simulate_scenario( "tau_master_candidate": tau_master_candidate.copy(), "tau_master_applied": tau_master_applied.copy(), "tau_master_accepted": tau_master_accepted.copy(), + "energy_probe_torque_Nm": energy_probe_torque.copy(), "wrench_external": wrench_external.copy(), "wrench_estimated": wrench_estimated.copy(), "wrench_feedback_source": delayed_wrench.copy(), @@ -1450,6 +1596,16 @@ def simulate_scenario( slave_limit_events += int(slave_clipped) master_velocity_limit_events += int(master_velocity_limited) slave_velocity_limit_events += int(slave_velocity_limited) + log_lists["master_joint_limit_active"].append( + int(master_clipped) + ) + log_lists["slave_joint_limit_active"].append(int(slave_clipped)) + log_lists["master_velocity_limit_active"].append( + int(master_velocity_limited) + ) + log_lists["slave_velocity_limit_active"].append( + int(slave_velocity_limited) + ) if not ( np.all(np.isfinite(q_m)) @@ -1497,6 +1653,16 @@ def simulate_scenario( "wall_contact_duration_s": float(np.sum(contact_mask) * config.dt), "wall_force_active_fraction": float(np.mean(force_active_mask)), "peak_contact_force_N": float(np.max(logs["contact_force_norm"])), + "peak_wall_force_raw_N": float( + np.max(logs["wall_force_raw_N"]) + ), + "wall_force_limit_hit_fraction": float( + np.mean(logs["wall_force_saturation_active"] > 0.5) + ), + "wall_force_headroom_min_N": float( + config.wall_force_limit + - np.max(logs["wall_force_applied_N"]) + ), "max_penetration_mm": float(1e3 * np.max(logs["penetration"])), "force_estimation_rmse_N": _rms( logs["force_estimation_error_norm"] @@ -1530,6 +1696,9 @@ def simulate_scenario( np.sum(positive_power) * config.dt ), "energy_absorbed_J": float(np.sum(absorbed_power) * config.dt), + "energy_probe_raw_work_J": float( + logs["energy_probe_raw_work_J"][-1] + ), "supervisor_intervention_fraction": float(np.mean(projection_mask)), "energy_projection_fraction": ( float(np.mean(projection_mask)) diff --git a/code/test/test_bilateral_calibration_v3.py b/code/test/test_bilateral_calibration_v3.py new file mode 100644 index 0000000..9454be2 --- /dev/null +++ b/code/test/test_bilateral_calibration_v3.py @@ -0,0 +1,274 @@ +#!/usr/bin/env python3 +"""Bilateral v3 environment, force-audit, and stable-contact contracts.""" + +from pathlib import Path +import sys +import unittest + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(CODE_ROOT)) + +from analysis.metrics import MetricError, derive_trial_metrics # noqa: E402 +from experiments.executors import ( # noqa: E402 + _bilateral_scenario_and_config, + execute_bilateral_simulation, +) +from experiments.plan import build_trial_plan, load_document # noqa: E402 +from experiments.rng import named_seed_record # noqa: E402 +from simulate_closed_loop import Wall # noqa: E402 + + +CONFIG_ROOT = CODE_ROOT / "config" / "experiments" + + +def propagation_trial(): + return { + "method": {"method_id": "proposed_energy"}, + "trajectory": { + "trajectory_id": "unit_contact_v3", + "family": "contact_roundtrip", + "duration_s": 1.0, + "contact_probe_fraction": 0.03, + }, + "factors": { + "map_policy": "source_stamped", + "environment_profile": { + "duration": 6.0, + "mapping_hz": 40.0, + "wall_fraction": 0.5, + "stiffness": 3200.0, + "damping": 25.0, + "force_limit": 20.0, + "transition_depth": 0.001, + "probe_fraction": 0.0, + "contact_probe_cycles": 0.0, + }, + "haptic_profile": { + "feedback_strength": 0.35, + "energy_min": 0.0, + "energy_max": 2.0, + "energy_initial": 1.0, + }, + # Direct aliases must override the coupled environment profile. + "duration_s": 5.0, + "wall_stiffness": 6400.0, + "probe_fraction": 0.02, + }, + "seeds": named_seed_record(11, {"test": "bilateral-v3"}), + } + + +class BilateralCalibrationV3Test(unittest.TestCase): + def test_environment_profile_and_direct_factors_propagate(self): + _, config = _bilateral_scenario_and_config(propagation_trial()) + self.assertEqual(config.duration, 5.0) + self.assertEqual(config.mapping_hz, 40.0) + self.assertEqual(config.wall_fraction, 0.5) + self.assertEqual(config.wall_stiffness, 6400.0) + self.assertEqual(config.wall_damping, 25.0) + self.assertEqual(config.wall_force_limit, 20.0) + self.assertEqual(config.wall_transition_depth, 0.001) + self.assertEqual(config.contact_probe_fraction, 0.02) + self.assertEqual(config.contact_probe_cycles, 0.0) + + def test_wall_reports_raw_applied_and_saturation_compatibly(self): + wall = Wall( + point=np.zeros(3), + normal=np.array([1.0, 0.0, 0.0]), + stiffness=100.0, + damping=10.0, + force_limit=5.0, + ) + contact = wall.contact( + np.array([0.1, 0.0, 0.0]), + np.array([2.0, 0.0, 0.0]), + ) + self.assertAlmostEqual(contact.penetration, 0.1) + self.assertAlmostEqual(contact.force_raw_N, 30.0) + self.assertAlmostEqual(contact.force_applied_N, 5.0) + self.assertTrue(contact.saturation_active) + np.testing.assert_allclose( + contact.wrench_applied[:3], [-5.0, 0.0, 0.0] + ) + + legacy_wrench, legacy_penetration = wall.wrench( + np.array([0.1, 0.0, 0.0]), + np.array([2.0, 0.0, 0.0]), + ) + np.testing.assert_allclose(legacy_wrench, contact.wrench_applied) + self.assertEqual(legacy_penetration, contact.penetration) + + def test_v3_grids_are_separate_and_h3_challenge_is_disabled(self): + stable = build_trial_plan( + load_document( + CONFIG_ROOT + / "bilateral_calibration_v3_stable_contact.json" + ) + ) + challenge_spec = load_document( + CONFIG_ROOT + / "bilateral_calibration_v3_energy_challenge.json" + ) + challenge = build_trial_plan(challenge_spec) + challenge_metrics = load_document( + CONFIG_ROOT + / "metrics_bilateral_v3_energy_challenge.json" + ) + + self.assertEqual(stable["pair_count"], 18) + self.assertEqual(stable["trial_count"], 18) + self.assertEqual(challenge["pair_count"], 3) + self.assertEqual(challenge["trial_count"], 3) + self.assertFalse(challenge_spec["h3_eligible"]) + self.assertNotIn("h3", challenge_metrics["enabled"]) + self.assertIn("not frozen", stable["specification"]["status"]) + + def test_haptic_profiles_and_challenge_share_exogenous_seed_groups(self): + stable = build_trial_plan( + load_document( + CONFIG_ROOT + / "bilateral_calibration_v3_stable_contact.json" + ) + ) + challenge = build_trial_plan( + load_document( + CONFIG_ROOT + / "bilateral_calibration_v3_energy_challenge.json" + ) + ) + + stable_k3200_rep0 = [ + trial + for trial in stable["trials"] + if trial["replicate"] == 0 + and trial["factors"]["environment_profile"]["profile_id"] + == "stable_candidate_k3200" + ] + self.assertEqual(len(stable_k3200_rep0), 3) + stable_seeds = { + _bilateral_scenario_and_config(trial)[1].seed + for trial in stable_k3200_rep0 + } + self.assertEqual(len(stable_seeds), 1) + + challenge_rep0 = next( + trial + for trial in challenge["trials"] + if trial["replicate"] == 0 + ) + challenge_seed = _bilateral_scenario_and_config( + challenge_rep0 + )[1].seed + self.assertEqual(challenge_seed, next(iter(stable_seeds))) + + stable_k6400_rep0 = next( + trial + for trial in stable["trials"] + if trial["replicate"] == 0 + and trial["factors"]["environment_profile"]["profile_id"] + == "stiff_candidate_k6400" + ) + self.assertNotEqual( + _bilateral_scenario_and_config(stable_k6400_rep0)[1].seed, + challenge_seed, + ) + + def test_stable_six_second_smoke_has_contact_without_force_limiting(self): + plan = build_trial_plan( + load_document( + CONFIG_ROOT + / "bilateral_calibration_v3_stable_contact.json" + ) + ) + trial = next( + entry + for entry in plan["trials"] + if entry["factors"]["environment_profile"]["profile_id"] + == "stable_candidate_k3200" + and entry["factors"]["haptic_profile"]["profile_id"] + == "gain_035_wide_tank" + ) + payload = execute_bilateral_simulation(trial) + applied = payload.samples["wall_force_applied_N"] + + self.assertEqual(applied.shape[0], 3000) + self.assertTrue( + np.all(payload.samples["wall_force_saturation_active"] == 0) + ) + self.assertGreaterEqual(float(np.max(applied)), 0.5) + self.assertLessEqual(float(np.max(applied)), 5.0) + np.testing.assert_allclose( + payload.samples["wall_force_raw_N"], + payload.samples["wall_force_applied_N"], + ) + + metric_config = load_document( + CONFIG_ROOT + / "metrics_bilateral_v3_stable_contact.json" + ) + metrics = derive_trial_metrics( + payload.samples, + metric_config, + ) + self.assertTrue(metrics["bilateral_stable_contact_gate_pass"]) + self.assertEqual(metrics["bilateral_force_limit_hit_fraction"], 0.0) + self.assertEqual(metrics["bilateral_limit_active_fraction"], 0.0) + self.assertGreaterEqual( + metrics["bilateral_contact_force_rms_N"], 0.1 + ) + incomplete = dict(payload.samples) + incomplete.pop("haptic_rate_limit_active") + with self.assertRaisesRegex( + MetricError, "missing required bilateral audit fields" + ): + derive_trial_metrics(incomplete, metric_config) + + def test_energy_challenge_is_active_audited_and_unsaturated(self): + plan = build_trial_plan( + load_document( + CONFIG_ROOT + / "bilateral_calibration_v3_energy_challenge.json" + ) + ) + payload = execute_bilateral_simulation(plan["trials"][0]) + metrics = derive_trial_metrics( + payload.samples, + load_document( + CONFIG_ROOT + / "metrics_bilateral_v3_energy_challenge.json" + ), + ) + + self.assertTrue(metrics["h4_energy_audit_pass"]) + self.assertTrue(metrics["energy_challenge_gate_pass"]) + self.assertGreater(metrics["h4_shadow_floor_deficit_J"], 0.0) + self.assertGreater( + metrics["bilateral_energy_probe_raw_work_J"], 0.0 + ) + self.assertGreaterEqual( + metrics["bilateral_projection_intervention_fraction"], 0.02 + ) + self.assertLessEqual( + metrics["bilateral_projection_intervention_fraction"], 0.30 + ) + self.assertEqual(metrics["bilateral_force_limit_hit_fraction"], 0.0) + self.assertEqual(metrics["bilateral_limit_active_fraction"], 0.0) + self.assertTrue(metrics["bilateral_stable_contact_gate_pass"]) + self.assertTrue(np.all(payload.samples["h3_eligible"] == 0)) + self.assertFalse(payload.metadata["h3_eligible"]) + self.assertEqual( + payload.metadata["energy_probe"]["window"], + "hann_squared_sine", + ) + with self.assertRaisesRegex(MetricError, "H3 is ineligible"): + derive_trial_metrics( + payload.samples, + {"enabled": ["h3"], "h3": {}}, + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_h1_calibration_v3.py b/code/test/test_h1_calibration_v3.py new file mode 100644 index 0000000..118dbb5 --- /dev/null +++ b/code/test/test_h1_calibration_v3.py @@ -0,0 +1,316 @@ +#!/usr/bin/env python3 +"""H1 calibration-v3 branch-crossing and differential evidence 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 MetricError, derive_trial_metrics # noqa: E402 +from core.model_contract import ( # noqa: E402 + MASTER_JOINT_NAMES, + finite_joint_limits, + load_models, +) +from core.retargeting_baselines import ( # noqa: E402 + build_canonical_sew_target_baselines, +) +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_v3.json" +) +METRIC_CONFIG_PATH = ( + CODE_ROOT + / "config" + / "experiments" + / "metrics_h1_calibration_v3.json" +) + + +class H1CalibrationV3Test(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) + _, cls.sew_method = build_canonical_sew_target_baselines(cls.models) + + cls.sew_payloads = {} + for path_type in ("linear", "cosine_roundtrip"): + cls.sew_payloads[path_type] = execute_h1_retargeting( + cls._trial("sew", path_type) + ) + cls.na_payload = execute_h1_retargeting( + cls._trial("bounded_dls_ik", "linear") + ) + + @classmethod + def _trial(cls, method_id: str, path_type: str): + return next( + trial + for trial in cls.plan["trials"] + if trial["method"]["method_id"] == method_id + and trial["trajectory"]["path_type"] == path_type + ) + + @classmethod + def _trajectory(cls, trial): + return _master_trajectory( + trial, lower=cls.lower, upper=cls.upper + ) + + def test_explicit_linear_and_roundtrip_paths_are_strictly_paired(self) -> None: + self.assertEqual(self.plan["pair_count"], 2) + self.assertEqual(self.plan["trial_count"], 8) + by_pair = {} + for trial in self.plan["trials"]: + trajectory = self._trajectory(trial) + by_pair.setdefault(trial["pair_id"], []).append(trajectory) + self.assertLess( + np.max(np.linalg.norm(np.diff(trajectory, axis=0), axis=1)), + 0.05, + ) + + for trajectories in by_pair.values(): + self.assertEqual(len(trajectories), 4) + for candidate in trajectories[1:]: + np.testing.assert_array_equal(candidate, trajectories[0]) + + linear_trial = self._trial("sew", "linear") + linear = self._trajectory(linear_trial) + np.testing.assert_array_equal( + linear[0], np.asarray(linear_trial["trajectory"]["start"]) + ) + np.testing.assert_array_equal( + linear[-1], np.asarray(linear_trial["trajectory"]["end"]) + ) + + roundtrip_trial = self._trial("sew", "cosine_roundtrip") + roundtrip = self._trajectory(roundtrip_trial) + np.testing.assert_array_equal( + roundtrip[0], np.asarray(roundtrip_trial["trajectory"]["start"]) + ) + np.testing.assert_allclose( + roundtrip[len(roundtrip) // 2], + np.asarray(roundtrip_trial["trajectory"]["end"]), + atol=1e-15, + rtol=0.0, + ) + np.testing.assert_array_equal(roundtrip[-1], roundtrip[0]) + + def test_explicit_path_contract_rejects_partial_or_unknown_paths(self) -> None: + trial = deepcopy(self._trial("sew", "linear")) + del trial["trajectory"]["end"] + with self.assertRaisesRegex(ValueError, "both start and end"): + self._trajectory(trial) + + trial = deepcopy(self._trial("sew", "linear")) + trial["trajectory"]["path_type"] = "triangle" + with self.assertRaisesRegex(ValueError, "path_type"): + self._trajectory(trial) + + def test_target_debug_has_phi_reference_and_reach_margins(self) -> None: + linear = self.sew_payloads["linear"].samples + crossing = np.flatnonzero( + np.abs(np.diff(linear["map_sew_phi_rad"])) > np.pi + ) + self.assertEqual(crossing.size, 1) + debug = self.sew_method.mapper._target_from_master( + linear["q_master"][int(crossing[0]) + 1] + ) + for field in ( + "phi_rad", + "reference_axis_norm", + "reach_lower_margin_m", + "reach_upper_margin_m", + "master_arm_normal_norm", + ): + self.assertIn(field, debug) + self.assertTrue(np.isfinite(debug[field])) + self.assertGreater(debug["reference_axis_norm"], 0.0) + self.assertGreater(debug["reach_lower_margin_m"], 0.0) + self.assertGreater(debug["reach_upper_margin_m"], 0.0) + + for payload in self.sew_payloads.values(): + samples = payload.samples + self.assertTrue(np.all(samples["map_reach_clip_code"] == 0)) + self.assertTrue( + np.all(samples["map_reach_lower_margin_m"] > 0.0) + ) + self.assertTrue( + np.all(samples["map_reach_upper_margin_m"] > 0.0) + ) + self.assertTrue(np.all(samples["map_reference_axis_norm"] > 0.0)) + + def test_actual_sew_differential_and_branch_metrics_are_reconstructable( + self, + ) -> None: + expected_crossings = {"linear": 1, "cosine_roundtrip": 2} + for path_type, payload in self.sew_payloads.items(): + with self.subTest(path_type=path_type): + samples = payload.samples + self.assertEqual( + samples["map_differential_A"].shape, (81, 7, 7) + ) + self.assertTrue( + np.all(samples["map_differential_applicable"] == 1) + ) + self.assertTrue(np.all(samples["map_branch_smooth"] == 1)) + self.assertTrue( + np.all(samples["map_differential_valid"] == 1) + ) + self.assertTrue( + np.all(np.isfinite(samples["map_differential_A"])) + ) + self.assertTrue( + np.all(samples["map_differential_runtime_s"] >= 0.0) + ) + self.assertTrue( + np.all( + np.isfinite( + samples[ + "map_differential_max_" + "one_sided_consistency" + ] + ) + ) + ) + + metrics = derive_trial_metrics( + samples, self.metric_configuration + ) + self.assertEqual(metrics["h1_F_r"], 0) + self.assertEqual(metrics["h1_D_r"], 0) + self.assertEqual(metrics["h1_C_r"], 0) + self.assertEqual( + metrics["h1_phi_raw_wrap_crossing_count"], + expected_crossings[path_type], + ) + self.assertTrue(metrics["h1_phi_wrap_metric_valid"]) + self.assertLess( + metrics[ + "h1_phi_wrap_crossing_max_slave_joint_step_rad" + ], + 0.25, + ) + self.assertEqual( + metrics["h1_differential_valid_fraction"], 1.0 + ) + for name in ( + "h1_pose_runtime_p50_ms", + "h1_pose_runtime_p95_ms", + "h1_pose_runtime_p99_ms", + "h1_pose_runtime_max_ms", + "h1_pose_runtime_warm_p95_ms", + "h1_differential_runtime_p50_ms", + "h1_differential_runtime_p95_ms", + "h1_differential_runtime_p99_ms", + "h1_differential_runtime_max_ms", + "h1_feedback_ready_runtime_p95_ms", + ): + self.assertIn(name, metrics) + self.assertIsNotNone(metrics[name]) + self.assertGreaterEqual(metrics[name], 0.0) + + def test_non_sew_actual_differential_is_explicit_na_not_failure(self) -> None: + payload = self.na_payload + samples = payload.samples + self.assertTrue( + np.all(samples["map_differential_applicable"] == 0) + ) + self.assertTrue(np.all(np.isnan(samples["map_differential_A"]))) + np.testing.assert_array_equal( + samples["map_differential_valid"], + samples["map_branch_smooth"], + ) + self.assertIsNotNone(payload.metadata["differential_n_a_reason"]) + + metrics = derive_trial_metrics(samples, self.metric_configuration) + self.assertEqual(metrics["h1_F_r"], 0) + self.assertEqual(metrics["h1_D_r"], 0) + self.assertEqual(metrics["h1_differential_applicable_fraction"], 0.0) + self.assertIsNone(metrics["h1_differential_valid_fraction"]) + self.assertIsNone(metrics["h1_differential_runtime_p95_ms"]) + + def test_v3_metrics_reject_missing_or_nonbinary_evidence(self) -> None: + original = self.sew_payloads["linear"].samples + + missing = dict(original) + missing.pop("map_differential_applicable") + with self.assertRaisesRegex( + MetricError, "requires v3 evidence fields" + ): + derive_trial_metrics(missing, self.metric_configuration) + + nonbinary = { + name: value.copy() for name, value in original.items() + } + nonbinary["map_pose_success"] = np.asarray( + nonbinary["map_pose_success"], dtype=float + ) + nonbinary["map_pose_success"][0] = np.nan + with self.assertRaisesRegex(MetricError, "finite 0/1 flags"): + derive_trial_metrics(nonbinary, self.metric_configuration) + + def test_empty_or_missing_wrap_evidence_cannot_look_perfect(self) -> None: + original = self.sew_payloads["linear"].samples + no_crossing = { + name: value.copy() for name, value in original.items() + } + no_crossing["map_sew_phi_rad"] = np.zeros_like( + no_crossing["map_sew_phi_rad"] + ) + no_crossing_metrics = derive_trial_metrics( + no_crossing, self.metric_configuration + ) + self.assertFalse( + no_crossing_metrics["h1_phi_wrap_metric_valid"] + ) + self.assertIsNone( + no_crossing_metrics[ + "h1_phi_wrap_crossing_max_slave_joint_step_rad" + ] + ) + + no_accepted = { + name: value.copy() for name, value in original.items() + } + no_accepted["map_accepted"] = np.zeros_like( + no_accepted["map_accepted"] + ) + empty_metrics = derive_trial_metrics( + no_accepted, self.metric_configuration + ) + self.assertFalse(empty_metrics["h1_composite_metric_valid"]) + self.assertIsNone(empty_metrics["h1_F_r"]) + self.assertIsNone(empty_metrics["h1_D_r"]) + self.assertIsNone(empty_metrics["h1_C_r"]) + + 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], + ) + + +if __name__ == "__main__": + unittest.main()