exoskeleton/code/experiments/executors.py

"""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,
        },
    )