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

Redesign branch and bilateral contact calibration

Browse Source
This commit is contained in:
xtkuang 2026-07-27 17:05:55 +08:00
parent ea896b1012
commit 817ec23788
14 changed files with 2702 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
README.md
View File

@ -67,6 +67,20 @@ XDG_CACHE_HOME=/tmp/exoskeleton-xdg-cache \
Equivalent executor/config pairs are documented in Equivalent executor/config pairs are documented in
`code/experiments/README.md`. `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 The current calibration decision is recorded in
`docs/calibration/CALIBRATION_AUDIT_2026-07-27.md`. It deliberately leaves H1 `docs/calibration/CALIBRATION_AUDIT_2026-07-27.md`. It deliberately leaves H1
and the bilateral gain/energy settings unfrozen; calibration values are not and the bilateral gain/energy settings unfrozen; calibration values are not

1028
code/analysis/metrics.py
View File

File diff suppressed because it is too large Load Diff

81
code/config/experiments/bilateral_calibration_v3_energy_challenge.json Normal file
View File

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

99
code/config/experiments/bilateral_calibration_v3_stable_contact.json Normal file
View File

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

77
code/config/experiments/h1_calibration_v3.json Normal file
View File

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

58
code/config/experiments/metrics_bilateral_v3_energy_challenge.json Normal file
View File

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

57
code/config/experiments/metrics_bilateral_v3_stable_contact.json Normal file
View File

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

43
code/config/experiments/metrics_h1_calibration_v3.json Normal file
View File

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

11
code/core/sew_mapper2.py
View File

@ -317,7 +317,8 @@ class SEWMapper:
nm = arm_normal_raw / arm_normal_norm nm = arm_normal_raw / arm_normal_norm
nref_tilde = self.up - float(np.dot(self.up, xhat)) * xhat 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: if reference_fallback:
events.append("reference_axis_fallback") events.append("reference_axis_fallback")
candidates = ( candidates = (
@ -374,10 +375,14 @@ class SEWMapper:
"events": events, "events": events,
"hard_geometry_valid": hard_geometry_valid, "hard_geometry_valid": hard_geometry_valid,
"reference_fallback": reference_fallback, "reference_fallback": reference_fallback,
"reference_axis_norm": reference_axis_norm,
"phi_rad": phi,
"reach_clipped": clip_region != "none", "reach_clipped": clip_region != "none",
"clip_region": clip_region, "clip_region": clip_region,
"master_reach": d_m, "master_reach": d_m,
"slave_reach": d_s, "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, "master_arm_normal_norm": arm_normal_norm,
} }
@ -619,8 +624,12 @@ class SEWMapper:
"near_limit": bool(metrics["joint_limit_active_indices"]), "near_limit": bool(metrics["joint_limit_active_indices"]),
"position_error": position_error, "position_error": position_error,
"reference_fallback": target["reference_fallback"], "reference_fallback": target["reference_fallback"],
"reference_axis_norm": target["reference_axis_norm"],
"phi_rad": target["phi_rad"],
"master_reach": target["master_reach"], "master_reach": target["master_reach"],
"slave_reach": target["slave_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"], "master_arm_normal_norm": target["master_arm_normal_norm"],
"solver_success": bool(result.success), "solver_success": bool(result.success),
"solver_status": int(result.status), "solver_status": int(result.status),

20
code/experiments/README.md
View File

@ -36,6 +36,21 @@ execute_bilateral_simulation bilateral_calibration_v2_energy.json
execute_bilateral_simulation bilateral_calibration_v2_network.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 The bilateral network specification is a gated Stage B template. Its
`requires_stage_a_selection` flag means the haptic parameters are placeholders; `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 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 h2_calibration*.json metrics_h2.json
bilateral_calibration.json metrics_bilateral.json bilateral_calibration.json metrics_bilateral.json
bilateral_calibration_v2_*.json metrics_bilateral_v2.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, The bilateral configurations derive H3 for every mapping/supervisor condition,

495
code/experiments/executors.py
View File

@ -9,6 +9,7 @@ from __future__ import annotations
from dataclasses import replace from dataclasses import replace
from enum import Enum from enum import Enum
from time import perf_counter
from typing import Any, Mapping from typing import Any, Mapping
import numpy as np import numpy as np
@ -112,6 +113,13 @@ def _master_trajectory(
sample_count = int(specification.get("sample_count", 81)) sample_count = int(specification.get("sample_count", 81))
if sample_count < 3: if sample_count < 3:
raise ValueError("H1 trajectory sample_count must be at least three") 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")) family = str(specification.get("family", "nominal"))
center = np.asarray( center = np.asarray(
specification.get( 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]) center = np.array([0.0, 0.0, 0.0, 0.12, 0.0, 0.0, 0.0])
delta = np.array([0.0, 0.18, 0.0, 0.08, 0.0, -0.08, 0.0]) delta = np.array([0.0, 0.18, 0.0, 0.08, 0.0, -0.08, 0.0])
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", {}) variation = specification.get("instance_variation", {})
if not isinstance(variation, Mapping): if not isinstance(variation, Mapping):
raise ValueError("H1 instance_variation must be a mapping") raise ValueError("H1 instance_variation must be a mapping")
@ -199,8 +221,10 @@ def _master_trajectory(
if not isinstance(seeds, Mapping): if not isinstance(seeds, Mapping):
raise ValueError("H1 trial has no paired seed record") raise ValueError("H1 trial has no paired seed record")
trajectory_rng = generator_from_record(seeds, "trajectory") trajectory_rng = generator_from_record(seeds, "trajectory")
center = center + center_std * jitter_mask * trajectory_rng.normal(size=7) start = start + (
delta = delta * trajectory_rng.uniform( center_std * jitter_mask * trajectory_rng.normal(size=7)
)
displacement = displacement * trajectory_rng.uniform(
delta_scale_range[0], delta_scale_range[1], size=7 delta_scale_range[0], delta_scale_range[1], size=7
) )
harmonic_weight = float( harmonic_weight = float(
@ -210,11 +234,20 @@ def _master_trajectory(
harmonic_weight = 0.0 harmonic_weight = 0.0
phase = np.linspace(0.0, 1.0, sample_count) phase = np.linspace(0.0, 1.0, sample_count)
# One cosine excursion starts and ends at the same configuration with zero if path_type == "linear":
# endpoint velocity, making discontinuities attributable to the mapper. path_coordinate = phase
excursion = 0.5 - 0.5 * np.cos(2.0 * np.pi * phase) else:
excursion *= 1.0 + harmonic_weight * np.sin(2.0 * np.pi * phase) # One cosine excursion starts and ends at the same configuration with
trajectory = center[None, :] + excursion[:, None] * delta[None, :] # 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 margin = 1e-4
if np.any(trajectory < lower + margin) or np.any(trajectory > upper - margin): if np.any(trajectory < lower + margin) or np.any(trajectory > upper - margin):
raise ValueError( raise ValueError(
@ -340,6 +373,36 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload:
if method_id not in methods: if method_id not in methods:
raise ValueError(f"unknown H1 method {method_id!r}") raise ValueError(f"unknown H1 method {method_id!r}")
method = methods[method_id] 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) lower, upper = finite_joint_limits(models.master, MASTER_JOINT_NAMES)
q_master = _master_trajectory(trial, lower=lower, upper=upper) 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) position_error = np.empty(sample_count)
orientation_error = np.empty(sample_count) orientation_error = np.empty(sample_count)
success = np.empty(sample_count, dtype=np.int8) 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) failure_code = np.empty(sample_count, dtype=np.int16)
solver_status = np.empty(sample_count, dtype=np.int16) solver_status = np.empty(sample_count, dtype=np.int16)
iterations = np.empty(sample_count, dtype=np.int32) 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_min_singular_value = np.empty(sample_count)
slave_manipulability = np.empty(sample_count) slave_manipulability = np.empty(sample_count)
low_manipulability = np.zeros(sample_count, dtype=np.int8) 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]] = [] events: list[dict[str, Any]] = []
slave_data = models.slave.createData() slave_data = models.slave.createData()
@ -384,12 +463,85 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload:
seed = None seed = None
previous_swivel = 0.0 previous_swivel = 0.0
for index, q_m in enumerate(q_master): for index, q_m in enumerate(q_master):
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) result = method.retarget(q_m, q_slave_seed=seed)
q_slave_log[index] = result.q_slave q_slave_log[index] = result.q_slave
position_error[index] = result.diagnostics.position_error_m position_error[index] = result.diagnostics.position_error_m
orientation_error[index] = result.diagnostics.orientation_error_rad orientation_error[index] = result.diagnostics.orientation_error_rad
success[index] = int(result.success) 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) failure_code[index] = _enum_code(result.failure)
solver_status[index] = _enum_code(result.diagnostics.status) solver_status[index] = _enum_code(result.diagnostics.status)
iterations[index] = result.diagnostics.iterations 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]), "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( events.append(
{ {
"sample_index": index, "sample_index": index,
@ -464,6 +622,9 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload:
"failure_code": int(failure_code[index]), "failure_code": int(failure_code[index]),
"validity_reason": reason.value, "validity_reason": reason.value,
"validity_reason_code": int(validity_reason[index]), "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, "q_master": q_master,
"map_q_slave": q_slave_log, "map_q_slave": q_slave_log,
"map_pose_success": success, "map_pose_success": success,
# H1 requires a valid/smooth branch. Differential A is evaluated in a "map_branch_smooth": branch_smooth,
# separate diagnostic study and is not silently imputed here. # For v2-compatible trials without an actual differential evaluation,
"map_differential_valid": smooth, # 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_position_error_m": position_error,
"map_orientation_error_rad": orientation_error, "map_orientation_error_rad": orientation_error,
"map_swivel_angle_rad": swivel, "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_master_step_norm": master_step,
"map_accepted": np.ones(sample_count, dtype=np.int8), "map_accepted": np.ones(sample_count, dtype=np.int8),
"map_commanded_reset": np.zeros(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_status": solver_status,
"map_solver_iterations": iterations, "map_solver_iterations": iterations,
"map_runtime_s": runtime_s, "map_runtime_s": runtime_s,
"map_warm_start": warm_start,
"map_solver_cost": solver_cost, "map_solver_cost": solver_cost,
} }
return TrialPayload( return TrialPayload(
@ -525,7 +705,34 @@ def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload:
"trajectory_instance_hash": stable_hash( "trajectory_instance_hash": stable_hash(
q_master, prefix="h1-master-trajectory-instance" 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( def _bilateral_scenario_and_config(
trial: Mapping[str, Any], trial: Mapping[str, Any],
) -> tuple[Any, SimulationConfig]: ) -> tuple[Any, SimulationConfig]:
@ -799,12 +1100,40 @@ def _bilateral_scenario_and_config(
trajectory = _trajectory_spec(trial) trajectory = _trajectory_spec(trial)
trajectory_rng = generator_from_record(trial["seeds"], "trajectory") trajectory_rng = generator_from_record(trial["seeds"], "trajectory")
seed = int(trajectory_rng.integers(0, np.iinfo(np.int32).max)) 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( config = replace(
SimulationConfig(), SimulationConfig(),
seed=seed, 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( 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( feedback_delay_s=float(
_profiled_factor( _profiled_factor(
@ -902,8 +1231,87 @@ def _bilateral_scenario_and_config(
profile_name="haptic_profile", profile_name="haptic_profile",
) )
), ),
wall_stiffness=float(_factor(trial, "wall_stiffness", 800.0)), energy_probe_mode=str(
wall_damping=float(_factor(trial, "wall_damping", 45.0)), _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() config.validate()
return scenario, config return scenario, config
@ -912,6 +1320,9 @@ def _bilateral_scenario_and_config(
def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload: def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload:
"""Run one paired H3/H4 rigid-body trial with a frozen network trace.""" """Run one paired H3/H4 rigid-body trial with a frozen network trace."""
scenario, config = _bilateral_scenario_and_config(trial) 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) trajectory = _trajectory_spec(trial)
models = load_models(add_simulated_tcp=True) models = load_models(add_simulated_tcp=True)
mapper = build_mapper(models) mapper = build_mapper(models)
@ -954,6 +1365,13 @@ def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload:
samples["configured_energy_initial_J"] = np.full( samples["configured_energy_initial_J"] = np.full(
sample_count, config.energy_initial 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( return TrialPayload(
samples=samples, samples=samples,
events=(), events=(),
@ -973,6 +1391,36 @@ def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload:
"energy_max_J": config.energy_max, "energy_max_J": config.energy_max,
"energy_initial_J": config.energy_initial, "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": { "effective_network_config": {
"forward_delay_s": config.forward_delay_s, "forward_delay_s": config.forward_delay_s,
"return_delay_s": config.feedback_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, "forward_timeout_s": config.forward_timeout_s,
"return_timeout_s": config.return_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, "wall": wall_metadata,
"online_metrics_are_diagnostic_only": result.metrics, "online_metrics_are_diagnostic_only": result.metrics,
}, },

195
code/simulate_closed_loop.py
View File

@ -184,6 +184,10 @@ class SimulationConfig:
energy_min: float = 0.05 energy_min: float = 0.05
energy_max: float = 0.055 energy_max: float = 0.055
energy_initial: float = 0.05 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 sensor_noise_std: float = 0.001
wrench_characteristic_length_m: float = 0.30 wrench_characteristic_length_m: float = 0.30
@ -242,6 +246,31 @@ class SimulationConfig:
raise ValueError( raise ValueError(
"energy values must satisfy 0 <= min <= initial <= max" "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: if len(self.haptic_torque_limits) != 7:
raise ValueError("haptic_torque_limits must contain seven entries") raise ValueError("haptic_torque_limits must contain seven entries")
if len(self.haptic_rate_limits) != 7: if len(self.haptic_rate_limits) != 7:
@ -359,6 +388,17 @@ def load_simulation_config(path: Path) -> SimulationConfig:
return config 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) @dataclass(frozen=True)
class Wall: class Wall:
point: np.ndarray point: np.ndarray
@ -368,34 +408,55 @@ class Wall:
force_limit: float force_limit: float
transition_depth: float = 0.0 transition_depth: float = 0.0
def wrench( def contact(
self, self,
position_world: np.ndarray, position_world: np.ndarray,
linear_velocity_world: np.ndarray, linear_velocity_world: np.ndarray,
) -> tuple[np.ndarray, float]: ) -> WallContactResult:
"""Return environment-on-robot wrench and unilateral penetration.""" """Return raw/applied wall force diagnostics and the applied wrench."""
penetration = max( penetration = max(
0.0, 0.0,
float(np.dot(self.normal, position_world - self.point)), float(np.dot(self.normal, position_world - self.point)),
) )
if penetration <= 0.0: 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)) normal_velocity = float(np.dot(self.normal, linear_velocity_world))
if self.transition_depth > 0.0: if self.transition_depth > 0.0:
damping_activation = min(1.0, penetration / self.transition_depth) damping_activation = min(1.0, penetration / self.transition_depth)
else: else:
damping_activation = 1.0 damping_activation = 1.0
force_magnitude = min( force_raw_N = max(
self.force_limit,
max(
0.0, 0.0,
self.stiffness * penetration self.stiffness * penetration
+ damping_activation * self.damping * normal_velocity, + damping_activation * self.damping * normal_velocity,
),
) )
force = -force_magnitude * self.normal force_applied_N = min(self.force_limit, force_raw_N)
return np.concatenate((force, np.zeros(3, dtype=float))), penetration 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 @dataclass
@ -878,6 +939,9 @@ def simulate_scenario(
"dt", "dt",
"missed_deadline", "missed_deadline",
"contact_force_norm", "contact_force_norm",
"wall_force_raw_N",
"wall_force_applied_N",
"wall_force_saturation_active",
"penetration", "penetration",
"force_estimation_error_norm", "force_estimation_error_norm",
"moment_estimation_error_norm", "moment_estimation_error_norm",
@ -906,6 +970,19 @@ def simulate_scenario(
"slave_tracking_error", "slave_tracking_error",
"feedback_torque_norm", "feedback_torque_norm",
"mapped_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 = ( vector_keys = (
"q_master", "q_master",
@ -923,6 +1000,7 @@ def simulate_scenario(
"tau_master_candidate", "tau_master_candidate",
"tau_master_applied", "tau_master_applied",
"tau_master_accepted", "tau_master_accepted",
"energy_probe_torque_Nm",
"wrench_external", "wrench_external",
"wrench_estimated", "wrench_estimated",
"wrench_feedback_source", "wrench_feedback_source",
@ -950,6 +1028,7 @@ def simulate_scenario(
haptic_torque_saturation_events = 0 haptic_torque_saturation_events = 0
energy_identity_errors: list[float] = [] energy_identity_errors: list[float] = []
shadow_energy = config.energy_initial shadow_energy = config.energy_initial
energy_probe_raw_work_J = 0.0
contact_steps = 0 contact_steps = 0
forward_packets_accepted = 0 forward_packets_accepted = 0
forward_packets_rejected = 0 forward_packets_rejected = 0
@ -1068,10 +1147,12 @@ def simulate_scenario(
qd_s, qd_s,
slave_tcp_id, slave_tcp_id,
) )
wrench_external, penetration = wall.wrench( wall_contact = wall.contact(
tcp_position, tcp_position,
tcp_linear_velocity, tcp_linear_velocity,
) )
wrench_external = wall_contact.wrench_applied
penetration = wall_contact.penetration
if penetration > 0.0: if penetration > 0.0:
contact_steps += 1 contact_steps += 1
tau_slave_external = J_slave_world.T @ wrench_external tau_slave_external = J_slave_world.T @ wrench_external
@ -1115,9 +1196,10 @@ def simulate_scenario(
-slave_acceleration_limits, -slave_acceleration_limits,
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) 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. # This is an explicitly simulated load-side equivalent measurement.
# It satisfies the estimator's declared residual convention: # It satisfies the estimator's declared residual convention:
@ -1205,6 +1287,44 @@ def simulate_scenario(
if not feedback.valid: if not feedback.valid:
tau_master_mapped = np.zeros(models.master.nv, dtype=float) 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 popc_damping_gain = 0.0
if scenario.supervisor == "tank": if scenario.supervisor == "tank":
tau_master_applied, rho = renderer.render_mapped_reaction( tau_master_applied, rho = renderer.render_mapped_reaction(
@ -1325,9 +1445,10 @@ def simulate_scenario(
-master_acceleration_limits, -master_acceleration_limits,
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) np.any(np.abs(qdd_m_raw) > master_acceleration_limits)
) )
master_acceleration_limit_events += int(master_acceleration_limited)
try: try:
differential_for_power = ( differential_for_power = (
@ -1360,6 +1481,11 @@ def simulate_scenario(
"dt": config.dt, "dt": config.dt,
"missed_deadline": 0, "missed_deadline": 0,
"contact_force_norm": float(np.linalg.norm(wrench_external[:3])), "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, "penetration": penetration,
"force_estimation_error_norm": float( "force_estimation_error_norm": float(
np.linalg.norm(wrench_estimated[:3] - wrench_external[:3]) 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)), "feedback_torque_norm": float(np.linalg.norm(tau_master_applied)),
"mapped_torque_norm": float(np.linalg.norm(tau_master_mapped)), "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 = { vector_values = {
"q_master": q_m.copy(), "q_master": q_m.copy(),
@ -1415,6 +1560,7 @@ def simulate_scenario(
"tau_master_candidate": tau_master_candidate.copy(), "tau_master_candidate": tau_master_candidate.copy(),
"tau_master_applied": tau_master_applied.copy(), "tau_master_applied": tau_master_applied.copy(),
"tau_master_accepted": tau_master_accepted.copy(), "tau_master_accepted": tau_master_accepted.copy(),
"energy_probe_torque_Nm": energy_probe_torque.copy(),
"wrench_external": wrench_external.copy(), "wrench_external": wrench_external.copy(),
"wrench_estimated": wrench_estimated.copy(), "wrench_estimated": wrench_estimated.copy(),
"wrench_feedback_source": delayed_wrench.copy(), "wrench_feedback_source": delayed_wrench.copy(),
@ -1450,6 +1596,16 @@ def simulate_scenario(
slave_limit_events += int(slave_clipped) slave_limit_events += int(slave_clipped)
master_velocity_limit_events += int(master_velocity_limited) master_velocity_limit_events += int(master_velocity_limited)
slave_velocity_limit_events += int(slave_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 ( if not (
np.all(np.isfinite(q_m)) 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_contact_duration_s": float(np.sum(contact_mask) * config.dt),
"wall_force_active_fraction": float(np.mean(force_active_mask)), "wall_force_active_fraction": float(np.mean(force_active_mask)),
"peak_contact_force_N": float(np.max(logs["contact_force_norm"])), "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"])), "max_penetration_mm": float(1e3 * np.max(logs["penetration"])),
"force_estimation_rmse_N": _rms( "force_estimation_rmse_N": _rms(
logs["force_estimation_error_norm"] logs["force_estimation_error_norm"]
@ -1530,6 +1696,9 @@ def simulate_scenario(
np.sum(positive_power) * config.dt np.sum(positive_power) * config.dt
), ),
"energy_absorbed_J": float(np.sum(absorbed_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)), "supervisor_intervention_fraction": float(np.mean(projection_mask)),
"energy_projection_fraction": ( "energy_projection_fraction": (
float(np.mean(projection_mask)) float(np.mean(projection_mask))

274
code/test/test_bilateral_calibration_v3.py Normal file
View File

@ -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()

316
code/test/test_h1_calibration_v3.py Normal file
View File

@ -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()