"""Executable pre-prototype G0c studies. Each function accepts one immutable trial record produced by ``experiments.plan`` and returns an atomic ``TrialPayload``. The methods in a pair use the same recorded trajectory/model/sensor/network seeds. """ from __future__ import annotations from dataclasses import replace from enum import Enum from typing import Any, Mapping import numpy as np import pinocchio as pin from core.estimation_signals import ( JointFrictionCalibration, ResidualAblation, WrenchEstimatorCalibration, CalibratedResidualWrenchEstimator, ) from core.model_contract import ( MASTER_JOINT_NAMES, SLAVE_FRAMES, SLAVE_JOINT_NAMES, finite_joint_limits, load_models, require_frame, ) from core.retargeting_baselines import ( RetargetingFailure, build_canonical_sew_target_baselines, ) from core.wrench_solver import ScaledDLSSolver, UndampedSVDSolver from experiments.hashing import stable_hash from experiments.io import TrialPayload from experiments.rng import generator_from_record, named_seed_record from simulate_closed_loop import ( SCENARIOS, SimulationConfig, build_mapper, make_wall, simulate_scenario, ) def _method_id(trial: Mapping[str, Any]) -> str: method = trial.get("method") if not isinstance(method, Mapping) or not isinstance( method.get("method_id"), str ): raise ValueError("trial has no method.method_id") return method["method_id"] def _trajectory_spec(trial: Mapping[str, Any]) -> Mapping[str, Any]: trajectory = trial.get("trajectory") if not isinstance(trajectory, Mapping): raise ValueError("trial has no trajectory mapping") return trajectory def _factor(trial: Mapping[str, Any], name: str, default: Any) -> Any: factors = trial.get("factors", {}) if not isinstance(factors, Mapping): raise ValueError("trial factors must be a mapping") return factors.get(name, default) def _profiled_factor( trial: Mapping[str, Any], name: str, default: Any, *, profile_name: str, ) -> Any: """Resolve a direct factor, then a coupled profile, then a default. Direct factors deliberately take precedence. Profiles let calibration studies express a small set of valid, directional network conditions without expanding the Cartesian product of every delay/jitter/loss level. """ factors = trial.get("factors", {}) if not isinstance(factors, Mapping): raise ValueError("trial factors must be a mapping") if name in factors: return factors[name] profile = factors.get(profile_name, {}) if not isinstance(profile, Mapping): raise ValueError(f"{profile_name} factor must be a mapping") return profile.get(name, default) def _enum_code(member: Enum) -> int: return list(type(member)).index(member) def _master_trajectory( trial: Mapping[str, Any], *, lower: np.ndarray, upper: np.ndarray, ) -> np.ndarray: """Generate one seeded, paired, continuous master trajectory instance. The trajectory random stream belongs to the pair, not the method. Thus different replicates are genuine trajectory instances while all methods inside one pair receive bit-identical master samples. """ specification = _trajectory_spec(trial) sample_count = int(specification.get("sample_count", 81)) if sample_count < 3: raise ValueError("H1 trajectory sample_count must be at least three") family = str(specification.get("family", "nominal")) center = np.asarray( specification.get( "center", [0.534, 0.314, -0.10, 2.14, 0.38, 0.38, -0.72], ), dtype=float, ) delta = np.asarray( specification.get( "delta", [0.08, -0.06, 0.05, -0.12, 0.04, 0.05, -0.04], ), dtype=float, ) if center.shape != (7,) or delta.shape != (7,): raise ValueError("H1 center and delta must have seven entries") if family == "joint_limit": center = center.copy() center[0] = upper[0] - 0.03 delta = np.zeros(7) delta[0] = -0.22 elif family == "low_manipulability": # Legacy calibration-v1 family retained only for reproducibility. It # is reach-clipped and must not be treated as an isolated low- # manipulability stratum; v2 uses ``low_manipulability_valid``. center = center.copy() center[3] = 0.08 delta = np.array([0.04, 0.03, -0.04, 0.05, 0.02, -0.02, 0.02]) elif family == "low_manipulability_valid": # Just inside the slave upper-reach boundary: low minimum singular # value without the reach clipping that confounded calibration v1. center = center.copy() center[3] = 1.13 delta = np.array([0.025, -0.020, 0.015, 0.10, 0.015, 0.020, -0.015]) elif family == "reach_clip_upper": # Deliberately outside the slave upper reach for the entire excursion. center = center.copy() center[3] = 0.85 delta = np.array([0.025, -0.020, 0.015, 0.10, 0.015, 0.020, -0.015]) elif family == "reach_boundary": delta = 1.75 * delta elif family == "sew_degeneracy": 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]) variation = specification.get("instance_variation", {}) if not isinstance(variation, Mapping): raise ValueError("H1 instance_variation must be a mapping") randomize = bool(variation.get("enabled", True)) center_std = float(variation.get("center_std_rad", 0.006)) delta_scale_range = np.asarray( variation.get("delta_scale_range", [0.92, 1.08]), dtype=float ) harmonic_range = np.asarray( variation.get("harmonic_weight_range", [-0.08, 0.08]), dtype=float ) jitter_mask = np.asarray( variation.get("center_jitter_mask", [1, 1, 1, 1, 1, 1, 1]), dtype=float, ) if not np.isfinite(center_std) or center_std < 0.0: raise ValueError("H1 center_std_rad must be finite and non-negative") if ( delta_scale_range.shape != (2,) or not np.all(np.isfinite(delta_scale_range)) or delta_scale_range[0] <= 0.0 or delta_scale_range[1] < delta_scale_range[0] ): raise ValueError("H1 delta_scale_range must be two ordered positives") if ( harmonic_range.shape != (2,) or not np.all(np.isfinite(harmonic_range)) or harmonic_range[1] < harmonic_range[0] or np.max(np.abs(harmonic_range)) >= 1.0 ): raise ValueError( "H1 harmonic_weight_range must be ordered and inside (-1, 1)" ) if jitter_mask.shape != (7,) or not np.all(np.isfinite(jitter_mask)): raise ValueError("H1 center_jitter_mask must have seven finite entries") if randomize: seeds = trial.get("seeds") if not isinstance(seeds, Mapping): raise ValueError("H1 trial has no paired seed record") trajectory_rng = generator_from_record(seeds, "trajectory") center = center + center_std * jitter_mask * trajectory_rng.normal(size=7) delta = delta * trajectory_rng.uniform( delta_scale_range[0], delta_scale_range[1], size=7 ) harmonic_weight = float( trajectory_rng.uniform(harmonic_range[0], harmonic_range[1]) ) else: harmonic_weight = 0.0 phase = np.linspace(0.0, 1.0, sample_count) # 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, :] margin = 1e-4 if np.any(trajectory < lower + margin) or np.any(trajectory > upper - margin): raise ValueError( f"trajectory {specification.get('trajectory_id')} exceeds master limits" ) return trajectory class H1ValidityReason(str, Enum): """Primary reason a sample is unusable for smooth/differential mapping.""" NONE = "none" REACH_CLIPPED_LOWER = "reach_clipped_lower" REACH_CLIPPED_UPPER = "reach_clipped_upper" JOINT_LIMIT_ACTIVE = "joint_limit_active" GEOMETRY_DEGENERATE = "geometry_degenerate" INVALID_INPUT = "invalid_input" MASTER_LIMIT_VIOLATION = "master_limit_violation" JOINT_LIMIT_VIOLATION = "joint_limit_violation" TASK_TOLERANCE_EXCEEDED = "task_tolerance_exceeded" SOLVER_NOT_CONVERGED = "solver_not_converged" NUMERICAL_FAILURE = "numerical_failure" LOW_MANIPULABILITY = "low_manipulability" UNSPECIFIED_NONSMOOTH = "unspecified_nonsmooth" _H1_FAILURE_TO_VALIDITY_REASON = { RetargetingFailure.INVALID_INPUT: H1ValidityReason.INVALID_INPUT, RetargetingFailure.MASTER_LIMIT_VIOLATION: H1ValidityReason.MASTER_LIMIT_VIOLATION, RetargetingFailure.DEGENERATE_GEOMETRY: H1ValidityReason.GEOMETRY_DEGENERATE, RetargetingFailure.JOINT_LIMIT_VIOLATION: H1ValidityReason.JOINT_LIMIT_VIOLATION, RetargetingFailure.TASK_TOLERANCE_EXCEEDED: H1ValidityReason.TASK_TOLERANCE_EXCEEDED, RetargetingFailure.SOLVER_NOT_CONVERGED: H1ValidityReason.SOLVER_NOT_CONVERGED, RetargetingFailure.NUMERICAL_FAILURE: H1ValidityReason.NUMERICAL_FAILURE, } def _h1_validity_reason( *, smooth: bool, failure: RetargetingFailure, events: tuple[str, ...], low_manipulability: bool, ) -> H1ValidityReason: """Classify branch validity without overwriting pose-solver failure.""" if smooth: return H1ValidityReason.NONE event_set = set(events) if "reach_clipped_lower" in event_set: return H1ValidityReason.REACH_CLIPPED_LOWER if "reach_clipped_upper" in event_set: return H1ValidityReason.REACH_CLIPPED_UPPER if { "master_shoulder_wrist_degenerate", "master_arm_plane_degenerate", "reference_axis_fallback", "invalid_master_geometry", } & event_set: return H1ValidityReason.GEOMETRY_DEGENERATE if "joint_limit_active" in event_set: return H1ValidityReason.JOINT_LIMIT_ACTIVE if failure is not RetargetingFailure.NONE: return _H1_FAILURE_TO_VALIDITY_REASON.get( failure, H1ValidityReason.UNSPECIFIED_NONSMOOTH ) if low_manipulability: return H1ValidityReason.LOW_MANIPULABILITY return H1ValidityReason.UNSPECIFIED_NONSMOOTH def _slave_swivel( model: pin.Model, data: pin.Data, q_slave: np.ndarray, shoulder_id: int, elbow_id: int, wrist_id: int, previous: float, ) -> tuple[float, bool]: """Evaluate a continuous diagnostic arm-plane angle from slave geometry.""" pin.forwardKinematics(model, data, q_slave) pin.updateFramePlacements(model, data) shoulder = data.oMf[shoulder_id].translation elbow = data.oMf[elbow_id].translation wrist = data.oMf[wrist_id].translation axis = wrist - shoulder axis_norm = float(np.linalg.norm(axis)) if axis_norm <= 1e-9: return previous, True axis /= axis_norm radial = elbow - shoulder radial -= float(radial @ axis) * axis radial_norm = float(np.linalg.norm(radial)) if radial_norm <= 1e-9: return previous, True radial /= radial_norm reference = np.array([0.0, 0.0, 1.0]) reference -= float(reference @ axis) * axis if np.linalg.norm(reference) <= 1e-8: reference = np.array([1.0, 0.0, 0.0]) reference -= float(reference @ axis) * axis reference /= np.linalg.norm(reference) wrapped = float( np.arctan2(axis @ np.cross(reference, radial), reference @ radial) ) # Unwrap only against the previous accepted diagnostic value. delta = (wrapped - previous + np.pi) % (2.0 * np.pi) - np.pi return previous + float(delta), False def execute_h1_retargeting(trial: Mapping[str, Any]) -> TrialPayload: """Run one paired H1 trajectory through SEW or one formal baseline.""" models = load_models(add_simulated_tcp=True) baselines, sew = build_canonical_sew_target_baselines(models) methods = {**baselines, sew.name: sew} method_id = _method_id(trial) if method_id not in methods: raise ValueError(f"unknown H1 method {method_id!r}") method = methods[method_id] lower, upper = finite_joint_limits(models.master, MASTER_JOINT_NAMES) q_master = _master_trajectory(trial, lower=lower, upper=upper) sample_count = q_master.shape[0] q_slave_log = np.empty((sample_count, models.slave.nq)) 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) 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) runtime_s = np.empty(sample_count) solver_cost = np.empty(sample_count) swivel = np.empty(sample_count) degeneracy = np.zeros(sample_count, dtype=np.int8) validity_reason = np.empty(sample_count, dtype=np.int16) reach_clip_code = np.zeros(sample_count, dtype=np.int8) joint_limit_active = np.zeros(sample_count, dtype=np.int8) geometry_degenerate = np.zeros(sample_count, dtype=np.int8) slave_min_singular_value = np.empty(sample_count) slave_manipulability = np.empty(sample_count) low_manipulability = np.zeros(sample_count, dtype=np.int8) events: list[dict[str, Any]] = [] slave_data = models.slave.createData() shoulder_id = require_frame(models.slave, SLAVE_FRAMES["shoulder"]) elbow_id = require_frame(models.slave, SLAVE_FRAMES["elbow"]) wrist_id = require_frame(models.slave, SLAVE_FRAMES["wrist"]) low_manipulability_threshold = float( _trajectory_spec(trial).get( "low_manipulability_min_singular_threshold", 0.05 ) ) if ( not np.isfinite(low_manipulability_threshold) or low_manipulability_threshold <= 0.0 ): raise ValueError( "H1 low-manipulability singular-value threshold must be positive" ) seed = None previous_swivel = 0.0 for index, q_m in enumerate(q_master): 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) failure_code[index] = _enum_code(result.failure) solver_status[index] = _enum_code(result.diagnostics.status) iterations[index] = result.diagnostics.iterations runtime_s[index] = result.diagnostics.runtime_s solver_cost[index] = result.diagnostics.cost previous_swivel, is_degenerate = _slave_swivel( models.slave, slave_data, result.q_slave, shoulder_id, elbow_id, wrist_id, previous_swivel, ) swivel[index] = previous_swivel degeneracy[index] = int(is_degenerate) jacobian = pin.computeFrameJacobian( models.slave, slave_data, result.q_slave, wrist_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED, ) singular_values = np.linalg.svd(jacobian, compute_uv=False) slave_min_singular_value[index] = float(singular_values[-1]) slave_manipulability[index] = float(np.prod(singular_values)) is_low_manipulability = bool( singular_values[-1] <= low_manipulability_threshold ) low_manipulability[index] = int(is_low_manipulability) event_set = set(result.events) if "reach_clipped_lower" in event_set: reach_clip_code[index] = -1 elif "reach_clipped_upper" in event_set: reach_clip_code[index] = 1 joint_limit_active[index] = int("joint_limit_active" in event_set) geometry_degenerate[index] = int( bool( { "master_shoulder_wrist_degenerate", "master_arm_plane_degenerate", "reference_axis_fallback", "invalid_master_geometry", } & event_set ) ) reason = _h1_validity_reason( smooth=result.smooth, failure=result.failure, events=result.events, low_manipulability=is_low_manipulability, ) validity_reason[index] = _enum_code(reason) if result.success: seed = result.q_slave.copy() for event in result.events: events.append( { "sample_index": index, "event": str(event), "failure_code": int(failure_code[index]), "validity_reason": reason.value, "validity_reason_code": int(validity_reason[index]), } ) if not result.smooth: events.append( { "sample_index": index, "event": "differential_invalid", "failure_code": int(failure_code[index]), "validity_reason": reason.value, "validity_reason_code": int(validity_reason[index]), } ) master_step = np.zeros(sample_count) if sample_count > 1: master_step[1:] = np.linalg.norm( (q_master[1:] - q_master[:-1] + np.pi) % (2.0 * np.pi) - np.pi, axis=1, ) samples = { "sample_index": np.arange(sample_count, dtype=np.int64), "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_position_error_m": position_error, "map_orientation_error_rad": orientation_error, "map_swivel_angle_rad": swivel, "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), "map_degeneracy_transition": degeneracy, "map_failure_code": failure_code, "map_validity_reason_code": validity_reason, "map_reach_clip_code": reach_clip_code, "map_joint_limit_active": joint_limit_active, "map_geometry_degenerate": geometry_degenerate, "map_slave_min_singular_value": slave_min_singular_value, "map_slave_manipulability": slave_manipulability, "map_low_manipulability": low_manipulability, "map_solver_status": solver_status, "map_solver_iterations": iterations, "map_runtime_s": runtime_s, "map_solver_cost": solver_cost, } return TrialPayload( samples=samples, events=events, metadata={ "evidence_scope": "pre-prototype numerical retargeting only", "method_id": method_id, "failure_enum": { member.value: _enum_code(member) for member in RetargetingFailure }, "validity_reason_enum": { member.value: _enum_code(member) for member in H1ValidityReason }, "reach_clip_code": {"lower": -1, "none": 0, "upper": 1}, "low_manipulability_definition": { "quantity": ( "minimum singular value of the LOCAL_WORLD_ALIGNED " "slave-wrist geometric Jacobian" ), "comparison": "<=", "threshold": low_manipulability_threshold, }, "trajectory_instance_hash": stable_hash( q_master, prefix="h1-master-trajectory-instance" ), "trajectory_family": _trajectory_spec(trial).get("family", "nominal"), }, ) def _orthogonal(rng: np.random.Generator, size: int) -> np.ndarray: q, r = np.linalg.qr(rng.normal(size=(size, size))) signs = np.where(np.diag(r) >= 0.0, 1.0, -1.0) return q * signs def _h2_data_seed_group( trial: Mapping[str, Any], ) -> tuple[str, dict[str, Any], dict[str, list[int]]]: """Return H2 data streams independent of estimator tuning choices. ``pair_id`` intentionally includes every factor cell, which is the right default for most studies but would give each characteristic-length or damping candidate a different synthetic data realization. H2 calibration instead defines a second, explicitly recorded grouping unit from only the physical perturbation factors. Every estimator candidate in that group therefore receives byte-identical truth, model-error, sensor-noise, and trajectory streams. """ data_root_seed = int(_factor(trial, "h2_data_root_seed", 0)) if data_root_seed < 0: raise ValueError("h2_data_root_seed must be non-negative") physical_factors = { "min_scaled_singular": float( _factor(trial, "min_scaled_singular", 0.05) ), "model_error_std": float(_factor(trial, "model_error_std", 0.01)), "torque_noise_std_Nm": float( _factor(trial, "torque_noise_std_Nm", 0.01) ), "truth_characteristic_length_m": float( _factor(trial, "truth_characteristic_length_m", 0.30) ), } basis = { "study_id": str(trial.get("study_id", "")), "split": str(trial.get("split", "")), "data_root_seed": data_root_seed, "trajectory": dict(_trajectory_spec(trial)), "replicate": int(trial.get("replicate", 0)), "physical_factors": physical_factors, } group_id = ( "h2-data-" + stable_hash(basis, prefix="h2-physical-data-group")[:16] ) seed_record = named_seed_record( data_root_seed, basis, ("trajectory", "truth_model", "model_error", "sensor"), ) return group_id, basis, seed_record def execute_h2_synthetic(trial: Mapping[str, Any]) -> TrialPayload: """Run a paired, truth/estimator-separated H2 sensitivity trial.""" method_id = _method_id(trial) valid_methods = {"scaled_dls", "undamped_svd", "no_bias", "no_friction"} if method_id not in valid_methods: raise ValueError(f"unknown H2 method {method_id!r}") trajectory = _trajectory_spec(trial) count = int(trajectory.get("sample_count", 256)) if count < 8: raise ValueError("H2 synthetic trial needs at least eight samples") characteristic_length = float( _factor(trial, "characteristic_length_m", 0.30) ) damping = float(_factor(trial, "damping", 0.02)) min_singular = float(_factor(trial, "min_scaled_singular", 0.05)) noise_std = float(_factor(trial, "torque_noise_std_Nm", 0.01)) model_error_std = float(_factor(trial, "model_error_std", 0.01)) truth_characteristic_length = float( _factor(trial, "truth_characteristic_length_m", 0.30) ) operational_singular_threshold = float( _factor(trial, "operational_min_scaled_singular", 0.05) ) if min_singular < 0.0 or noise_std < 0.0 or model_error_std < 0.0: raise ValueError("H2 perturbation factors must be non-negative") if truth_characteristic_length <= 0.0: raise ValueError("truth_characteristic_length_m must be positive") if operational_singular_threshold <= 0.0: raise ValueError( "operational_min_scaled_singular must be positive" ) data_group_id, data_group_basis, data_seeds = _h2_data_seed_group(trial) truth_model_rng = generator_from_record(data_seeds, "truth_model") model_error_rng = generator_from_record(data_seeds, "model_error") sensor_rng = generator_from_record(data_seeds, "sensor") trajectory_rng = generator_from_record(data_seeds, "trajectory") u = _orthogonal(truth_model_rng, 6) v = _orthogonal(truth_model_rng, 7) singular = np.array([1.6, 1.25, 0.95, 0.65, 0.35, min_singular]) base_scaled_truth = u @ np.diag(singular) @ v[:6, :] # The physical truth is generated with a fixed reference length. The # scanned characteristic length belongs only to the estimator scaling; # otherwise changing ell would silently change the ground-truth Jacobian. truth_inverse_scaling = np.diag( [truth_characteristic_length] * 3 + [1.0] * 3 ) phase = np.linspace(0.0, 2.0 * np.pi, count, endpoint=False) amplitudes = np.array([18.0, 12.0, 9.0, 1.8, 1.2, 0.8]) offsets = trajectory_rng.uniform(-np.pi, np.pi, 6) wrench_reference = amplitudes[None, :] * np.sin( phase[:, None] * np.arange(1, 7)[None, :] + offsets[None, :] ) qd = 0.6 * np.sin( phase[:, None] * np.arange(1, 8)[None, :] + trajectory_rng.uniform(-np.pi, np.pi, (1, 7)) ) bias = np.array([0.08, -0.05, 0.035, -0.025, 0.015, -0.01, 0.02]) friction = JointFrictionCalibration( coulomb_nm=np.array([0.06, 0.05, 0.045, 0.04, 0.02, 0.02, 0.015]), viscous_nm_per_rad_s=np.array( [0.018, 0.017, 0.015, 0.014, 0.009, 0.008, 0.007] ), ) calibration = WrenchEstimatorCalibration( joint_bias_nm=bias, friction=friction, characteristic_length_m=characteristic_length, damping=damping, calibration_id=( "g0c-synthetic-candidate-" + stable_hash( { "characteristic_length_m": characteristic_length, "damping": damping, }, prefix="h2-calibration-candidate", )[:16] ), ) solver = ( UndampedSVDSolver(characteristic_length) if method_id == "undamped_svd" else ScaledDLSSolver(characteristic_length, damping) ) estimator = CalibratedResidualWrenchEstimator(calibration, solver) ablation = ( ResidualAblation.no_bias() if method_id == "no_bias" else ResidualAblation.no_friction() if method_id == "no_friction" else ResidualAblation() ) wrench_estimated = np.empty((count, 6)) singular_log = np.empty((count, 6)) rank = np.empty(count, dtype=np.int16) status = np.empty(count, dtype=np.int16) rank_threshold = np.empty(count) condition_number = np.empty(count) numerical_rank_deficient = np.empty(count, dtype=np.int8) operationally_ill_conditioned = np.empty(count, dtype=np.int8) truth_jacobian = np.empty((count, 42)) estimator_jacobian = np.empty((count, 42)) residual_raw = np.empty((count, 7)) residual_corrected = np.empty((count, 7)) sensor_noise = np.empty((count, 7)) for index in range(count): smooth_change = 0.015 * np.sin(phase[index]) J_truth = truth_inverse_scaling @ ( base_scaled_truth + smooth_change * truth_model_rng.normal(size=(6, 7)) ) J_estimator = J_truth + truth_inverse_scaling @ ( model_error_std * model_error_rng.normal(size=(6, 7)) ) interaction = J_truth.T @ wrench_reference[index] sensor_noise[index] = sensor_rng.normal(0.0, noise_std, 7) measured = ( interaction + bias + friction.torque(qd[index]) + sensor_noise[index] ) estimate = estimator.estimate( J_estimator, measured_torque_nm=measured, rigid_body_torque_nm=np.zeros(7), joint_velocity_rad_s=qd[index], ablation=ablation, ) wrench_estimated[index] = estimate.solve.wrench singular_log[index] = estimate.solve.singular_values rank[index] = estimate.solve.rank status[index] = _enum_code(estimate.solve.status) rank_threshold[index] = estimate.solve.rank_threshold condition_number[index] = estimate.solve.condition_number numerical_rank_deficient[index] = int(estimate.solve.rank < 6) operationally_ill_conditioned[index] = int( estimate.solve.singular_values[-1] <= operational_singular_threshold ) truth_jacobian[index] = J_truth.reshape(-1) estimator_jacobian[index] = J_estimator.reshape(-1) residual_raw[index] = estimate.residual.raw_residual_nm residual_corrected[index] = estimate.residual.residual_nm return TrialPayload( samples={ "sample_index": np.arange(count, dtype=np.int64), "wrench_reference": wrench_reference, "wrench_estimated": wrench_estimated, "wrench_sample_mask": np.ones(count, dtype=np.int8), "qd_slave": qd, "jacobian_truth": truth_jacobian, "jacobian_estimator": estimator_jacobian, "scaled_singular_values": singular_log, "solver_rank": rank, "solver_status": status, "solver_numerical_rank_threshold": rank_threshold, "solver_condition_number": condition_number, "solver_numerical_rank_deficient": numerical_rank_deficient, "solver_operationally_ill_conditioned": ( operationally_ill_conditioned ), "operational_min_scaled_singular_threshold": np.full( count, operational_singular_threshold ), "tau_residual_raw": residual_raw, "tau_residual_corrected": residual_corrected, "sensor_noise_Nm": sensor_noise, }, metadata={ "evidence_scope": ( "synthetic sensitivity only; not independent physical F/T evidence" ), "method_id": method_id, "truth_estimator_models_separated": True, "calibration_id": calibration.calibration_id, "h2_data_group_id": data_group_id, "h2_data_group_basis": data_group_basis, "h2_data_seed_record": data_seeds, "h2_data_seed_record_hash": stable_hash( data_seeds, prefix="h2-data-seed-record" ), "h2_data_root_seed": int( _factor(trial, "h2_data_root_seed", 0) ), "truth_characteristic_length_m": truth_characteristic_length, "operational_ill_conditioning_definition": { "quantity": "minimum singular value of estimator-scaled Jacobian", "comparison": "<=", "threshold": operational_singular_threshold, "numerical_rank_is_reported_separately": True, }, }, ) def _bilateral_scenario_and_config( trial: Mapping[str, Any], ) -> tuple[Any, SimulationConfig]: """Build and validate the effective bilateral scenario/configuration.""" method_id = _method_id(trial) scenarios = {scenario.key: scenario for scenario in SCENARIOS} if method_id not in scenarios: raise ValueError(f"unknown bilateral method {method_id!r}") scenario = scenarios[method_id] map_policy = str(_factor(trial, "map_policy", scenario.map_policy)) scenario = replace(scenario, map_policy=map_policy) trajectory = _trajectory_spec(trial) trajectory_rng = generator_from_record(trial["seeds"], "trajectory") seed = int(trajectory_rng.integers(0, np.iinfo(np.int32).max)) config = replace( SimulationConfig(), seed=seed, duration=float(trajectory.get("duration_s", 1.2)), contact_probe_fraction=float( trajectory.get("contact_probe_fraction", 0.0) ), feedback_delay_s=float( _profiled_factor( trial, "return_delay_s", 0.04, profile_name="network_profile", ) ), forward_delay_s=float( _profiled_factor( trial, "forward_delay_s", 0.0, profile_name="network_profile", ) ), return_jitter_s=float( _profiled_factor( trial, "return_jitter_s", 0.0, profile_name="network_profile", ) ), forward_jitter_s=float( _profiled_factor( trial, "forward_jitter_s", 0.0, profile_name="network_profile", ) ), return_packet_loss=float( _profiled_factor( trial, "return_packet_loss", 0.0, profile_name="network_profile", ) ), forward_packet_loss=float( _profiled_factor( trial, "forward_packet_loss", 0.0, profile_name="network_profile", ) ), forward_timeout_s=float( _profiled_factor( trial, "forward_timeout_s", 0.20, profile_name="network_profile", ) ), return_timeout_s=float( _profiled_factor( trial, "return_timeout_s", 0.20, profile_name="network_profile", ) ), feedback_strength=float( _profiled_factor( trial, "feedback_strength", 0.50, profile_name="haptic_profile", ) ), energy_min=float( _profiled_factor( trial, "energy_min", 0.05, profile_name="haptic_profile", ) ), energy_max=float( _profiled_factor( trial, "energy_max", 0.055, profile_name="haptic_profile", ) ), energy_initial=float( _profiled_factor( trial, "energy_initial", 0.05, profile_name="haptic_profile", ) ), wall_stiffness=float(_factor(trial, "wall_stiffness", 800.0)), wall_damping=float(_factor(trial, "wall_damping", 45.0)), ) config.validate() return scenario, config def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload: """Run one paired H3/H4 rigid-body trial with a frozen network trace.""" scenario, config = _bilateral_scenario_and_config(trial) trajectory = _trajectory_spec(trial) models = load_models(add_simulated_tcp=True) mapper = build_mapper(models) wall, wall_metadata, q_slave_start = make_wall(config, models, mapper) if trajectory.get("family") == "free_space": travel = float(wall_metadata["free_space_travel_m"]) wall = replace( wall, point=wall.point + 2.0 * travel * wall.normal, ) wall_metadata = { **wall_metadata, "condition": "free_space", "point_world_m": wall.point.tolist(), } result = simulate_scenario( scenario, config, models, wall, q_slave_start ) samples = {name: value.copy() for name, value in result.logs.items()} samples["tau_master_raw"] = samples["tau_master_mapped"].copy() samples["tau_slave_source"] = ( samples["tau_slave_residual_source"].copy() if scenario.mapping == "differential_residual" else samples["tau_slave_matched_wrench"].copy() ) samples["return_valid"] = samples["return_packet_active"].astype(np.int8) samples["energy_before_J"] = samples["energy_before"].copy() samples["energy_after_J"] = samples["tank_energy"].copy() samples["energy_preclip_J"] = samples["energy_preclip"].copy() sample_count = samples["time"].shape[0] samples["configured_feedback_strength"] = np.full( sample_count, config.feedback_strength ) samples["configured_energy_min_J"] = np.full( sample_count, config.energy_min ) samples["configured_energy_max_J"] = np.full( sample_count, config.energy_max ) samples["configured_energy_initial_J"] = np.full( sample_count, config.energy_initial ) return TrialPayload( samples=samples, events=(), metadata={ "evidence_scope": ( "pre-prototype rigid-body simulation; no physical or human claim" ), "scenario": { "key": scenario.key, "mapping": scenario.mapping, "supervisor": scenario.supervisor, "map_policy": scenario.map_policy, }, "effective_haptic_config": { "feedback_strength": config.feedback_strength, "energy_min_J": config.energy_min, "energy_max_J": config.energy_max, "energy_initial_J": config.energy_initial, }, "effective_network_config": { "forward_delay_s": config.forward_delay_s, "return_delay_s": config.feedback_delay_s, "forward_jitter_s": config.forward_jitter_s, "return_jitter_s": config.return_jitter_s, "forward_packet_loss": config.forward_packet_loss, "return_packet_loss": config.return_packet_loss, "forward_timeout_s": config.forward_timeout_s, "return_timeout_s": config.return_timeout_s, }, "wall": wall_metadata, "online_metrics_are_diagnostic_only": result.metrics, }, )