1649 lines
62 KiB
Python
1649 lines
62 KiB
Python
"""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 time import perf_counter
|
|
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 _boolean_factor(value: Any, name: str) -> bool:
|
|
"""Return a strict experiment boolean without accepting truthy strings."""
|
|
if not isinstance(value, (bool, np.bool_)):
|
|
raise ValueError(f"{name} must be boolean")
|
|
return bool(value)
|
|
|
|
|
|
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")
|
|
path_type = str(
|
|
specification.get("path_type", "cosine_roundtrip")
|
|
)
|
|
if path_type not in {"linear", "cosine_roundtrip"}:
|
|
raise ValueError(
|
|
"H1 path_type must be 'linear' or 'cosine_roundtrip'"
|
|
)
|
|
family = str(specification.get("family", "nominal"))
|
|
center = np.asarray(
|
|
specification.get(
|
|
"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])
|
|
|
|
explicit_start = specification.get("start")
|
|
explicit_end = specification.get("end")
|
|
if (explicit_start is None) != (explicit_end is None):
|
|
raise ValueError("H1 explicit paths require both start and end")
|
|
if explicit_start is not None:
|
|
start = np.asarray(explicit_start, dtype=float)
|
|
end = np.asarray(explicit_end, dtype=float)
|
|
if start.shape != (7,) or end.shape != (7,):
|
|
raise ValueError("H1 start and end must have seven entries")
|
|
displacement = end - start
|
|
else:
|
|
start = center.copy()
|
|
displacement = delta.copy()
|
|
|
|
variation = specification.get("instance_variation", {})
|
|
if not isinstance(variation, Mapping):
|
|
raise ValueError("H1 instance_variation must be a mapping")
|
|
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")
|
|
start = start + (
|
|
center_std * jitter_mask * trajectory_rng.normal(size=7)
|
|
)
|
|
displacement = displacement * trajectory_rng.uniform(
|
|
delta_scale_range[0], delta_scale_range[1], size=7
|
|
)
|
|
harmonic_weight = float(
|
|
trajectory_rng.uniform(harmonic_range[0], harmonic_range[1])
|
|
)
|
|
else:
|
|
harmonic_weight = 0.0
|
|
|
|
phase = np.linspace(0.0, 1.0, sample_count)
|
|
if path_type == "linear":
|
|
path_coordinate = phase
|
|
else:
|
|
# One cosine excursion starts and ends at the same configuration with
|
|
# zero endpoint velocity, making discontinuities attributable to the
|
|
# mapper rather than an endpoint reset.
|
|
path_coordinate = 0.5 - 0.5 * np.cos(2.0 * np.pi * phase)
|
|
path_coordinate *= (
|
|
1.0 + harmonic_weight * np.sin(2.0 * np.pi * phase)
|
|
)
|
|
trajectory = (
|
|
start[None, :]
|
|
+ path_coordinate[:, None] * displacement[None, :]
|
|
)
|
|
margin = 1e-4
|
|
if np.any(trajectory < lower + margin) or np.any(trajectory > upper - margin):
|
|
raise ValueError(
|
|
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]
|
|
specification = _trajectory_spec(trial)
|
|
actual_differential_requested = bool(
|
|
specification.get("actual_differential", False)
|
|
)
|
|
differential_applicable_for_method = bool(
|
|
actual_differential_requested and method_id == "sew"
|
|
)
|
|
differential_fd_step = float(
|
|
specification.get("differential_fd_step_rad", 1e-4)
|
|
)
|
|
differential_branch_jump_threshold = float(
|
|
specification.get(
|
|
"differential_branch_jump_threshold_rad", 0.25
|
|
)
|
|
)
|
|
differential_consistency_tolerance = float(
|
|
specification.get(
|
|
"differential_consistency_tolerance", 5e-2
|
|
)
|
|
)
|
|
differential_settings = (
|
|
differential_fd_step,
|
|
differential_branch_jump_threshold,
|
|
differential_consistency_tolerance,
|
|
)
|
|
if any(
|
|
not np.isfinite(value) or value <= 0.0
|
|
for value in differential_settings
|
|
):
|
|
raise ValueError("H1 differential settings must be positive")
|
|
|
|
lower, upper = finite_joint_limits(models.master, MASTER_JOINT_NAMES)
|
|
q_master = _master_trajectory(trial, lower=lower, upper=upper)
|
|
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)
|
|
branch_smooth = np.empty(sample_count, dtype=np.int8)
|
|
differential_valid = np.empty(sample_count, dtype=np.int8)
|
|
differential_applicable = np.full(
|
|
sample_count, int(differential_applicable_for_method), dtype=np.int8
|
|
)
|
|
differential_A = np.full((sample_count, 7, 7), np.nan)
|
|
differential_runtime_s = np.zeros(sample_count)
|
|
differential_event_count = np.zeros(sample_count, dtype=np.int16)
|
|
differential_max_consistency = np.full(sample_count, np.nan)
|
|
differential_max_column_jump = np.full(sample_count, np.nan)
|
|
differential_branch_jump = np.zeros(sample_count, dtype=np.int8)
|
|
failure_code = np.empty(sample_count, dtype=np.int16)
|
|
solver_status = np.empty(sample_count, dtype=np.int16)
|
|
iterations = np.empty(sample_count, dtype=np.int32)
|
|
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)
|
|
sew_phi_rad = np.empty(sample_count)
|
|
reference_axis_norm = np.empty(sample_count)
|
|
reach_lower_margin_m = np.empty(sample_count)
|
|
reach_upper_margin_m = np.empty(sample_count)
|
|
master_arm_normal_norm = np.empty(sample_count)
|
|
warm_start = np.empty(sample_count, dtype=np.int8)
|
|
events: list[dict[str, Any]] = []
|
|
|
|
slave_data = models.slave.createData()
|
|
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):
|
|
warm_start[index] = int(seed is not None)
|
|
target_debug = sew.mapper._target_from_master(q_m)
|
|
sew_phi_rad[index] = float(target_debug["phi_rad"])
|
|
reference_axis_norm[index] = float(
|
|
target_debug["reference_axis_norm"]
|
|
)
|
|
reach_lower_margin_m[index] = float(
|
|
target_debug["reach_lower_margin_m"]
|
|
)
|
|
reach_upper_margin_m[index] = float(
|
|
target_debug["reach_upper_margin_m"]
|
|
)
|
|
master_arm_normal_norm[index] = float(
|
|
target_debug["master_arm_normal_norm"]
|
|
)
|
|
result = method.retarget(q_m, q_slave_seed=seed)
|
|
q_slave_log[index] = result.q_slave
|
|
position_error[index] = result.diagnostics.position_error_m
|
|
orientation_error[index] = result.diagnostics.orientation_error_rad
|
|
success[index] = int(result.success)
|
|
branch_smooth[index] = int(result.smooth)
|
|
differential_valid[index] = int(result.smooth)
|
|
differential_info: Mapping[str, Any] | None = None
|
|
if differential_applicable_for_method:
|
|
differential_started = perf_counter()
|
|
differential_result, differential_info = (
|
|
sew.mapper.compute_differential(
|
|
q_m,
|
|
q_s_init=result.q_slave,
|
|
fd_step=differential_fd_step,
|
|
branch_jump_threshold=(
|
|
differential_branch_jump_threshold
|
|
),
|
|
consistency_tolerance=(
|
|
differential_consistency_tolerance
|
|
),
|
|
)
|
|
)
|
|
differential_runtime_s[index] = (
|
|
perf_counter() - differential_started
|
|
)
|
|
differential_A[index] = differential_result
|
|
differential_valid[index] = int(
|
|
bool(differential_info["valid"])
|
|
)
|
|
differential_events = tuple(
|
|
str(event)
|
|
for event in differential_info.get("events", ())
|
|
)
|
|
differential_event_count[index] = len(differential_events)
|
|
columns = tuple(differential_info.get("columns", ()))
|
|
if columns:
|
|
differential_max_consistency[index] = max(
|
|
float(column["one_sided_consistency"])
|
|
for column in columns
|
|
)
|
|
differential_max_column_jump[index] = max(
|
|
max(
|
|
float(column["plus_jump"]),
|
|
float(column["minus_jump"]),
|
|
)
|
|
for column in columns
|
|
)
|
|
differential_branch_jump[index] = int(
|
|
any(
|
|
"branch_jump" in column.get("events", ())
|
|
for column in columns
|
|
)
|
|
)
|
|
for differential_event in differential_events:
|
|
events.append(
|
|
{
|
|
"sample_index": index,
|
|
"event": f"differential:{differential_event}",
|
|
"differential_valid": bool(
|
|
differential_valid[index]
|
|
),
|
|
}
|
|
)
|
|
failure_code[index] = _enum_code(result.failure)
|
|
solver_status[index] = _enum_code(result.diagnostics.status)
|
|
iterations[index] = result.diagnostics.iterations
|
|
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
|
|
or (
|
|
differential_applicable_for_method
|
|
and not differential_valid[index]
|
|
)
|
|
):
|
|
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]),
|
|
"differential_applicable": (
|
|
differential_applicable_for_method
|
|
),
|
|
}
|
|
)
|
|
|
|
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,
|
|
"map_branch_smooth": branch_smooth,
|
|
# For v2-compatible trials without an actual differential evaluation,
|
|
# this retains the historical branch-smooth proxy. Applicability makes
|
|
# that distinction explicit for v3 analysis.
|
|
"map_differential_valid": differential_valid,
|
|
"map_differential_applicable": differential_applicable,
|
|
"map_differential_A": differential_A,
|
|
"map_differential_runtime_s": differential_runtime_s,
|
|
"map_differential_event_count": differential_event_count,
|
|
"map_differential_max_one_sided_consistency": (
|
|
differential_max_consistency
|
|
),
|
|
"map_differential_max_column_jump_rad": (
|
|
differential_max_column_jump
|
|
),
|
|
"map_differential_branch_jump": differential_branch_jump,
|
|
"map_position_error_m": position_error,
|
|
"map_orientation_error_rad": orientation_error,
|
|
"map_swivel_angle_rad": swivel,
|
|
"map_sew_phi_rad": sew_phi_rad,
|
|
"map_reference_axis_norm": reference_axis_norm,
|
|
"map_reach_lower_margin_m": reach_lower_margin_m,
|
|
"map_reach_upper_margin_m": reach_upper_margin_m,
|
|
"map_master_arm_normal_norm": master_arm_normal_norm,
|
|
"map_master_step_norm": master_step,
|
|
"map_accepted": np.ones(sample_count, dtype=np.int8),
|
|
"map_commanded_reset": np.zeros(sample_count, dtype=np.int8),
|
|
"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_warm_start": warm_start,
|
|
"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": 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
|
|
),
|
|
},
|
|
},
|
|
)
|
|
|
|
|
|
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_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")
|
|
pair_network_profiles = _boolean_factor(
|
|
factors.get("bilateral_pair_network_profiles", False),
|
|
"bilateral_pair_network_profiles",
|
|
)
|
|
if pair_network_profiles and not config.network_common_random_numbers:
|
|
raise ValueError(
|
|
"bilateral_pair_network_profiles requires "
|
|
"network_common_random_numbers=true"
|
|
)
|
|
raw_root_seed = factors.get("bilateral_data_root_seed")
|
|
if raw_root_seed is None:
|
|
if pair_network_profiles:
|
|
raise ValueError(
|
|
"bilateral_pair_network_profiles requires "
|
|
"bilateral_data_root_seed"
|
|
)
|
|
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")
|
|
if pair_network_profiles:
|
|
basis = {
|
|
"data_root_seed": data_root_seed,
|
|
"trajectory": dict(_trajectory_spec(trial)),
|
|
"replicate": int(trial.get("replicate", 0)),
|
|
"network_pairing_strategy": {
|
|
"enabled": True,
|
|
"name": "common_simulation_seed_across_network_profiles",
|
|
"excluded_network_inputs": [
|
|
"forward_delay_s",
|
|
"return_delay_s",
|
|
"forward_jitter_s",
|
|
"return_jitter_s",
|
|
"forward_packet_loss",
|
|
"return_packet_loss",
|
|
"forward_timeout_s",
|
|
"return_timeout_s",
|
|
],
|
|
"network_common_random_numbers": (
|
|
config.network_common_random_numbers
|
|
),
|
|
},
|
|
"mechanical_and_control_inputs": {
|
|
"dt_s": config.dt,
|
|
"duration_s": config.duration,
|
|
"mapping_hz": config.mapping_hz,
|
|
"slave_contact_frame": config.slave_contact_frame,
|
|
"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,
|
|
"master_kp": list(config.master_kp),
|
|
"master_kd": list(config.master_kd),
|
|
"slave_kp": list(config.slave_kp),
|
|
"slave_kd": list(config.slave_kd),
|
|
"master_acceleration_limits": list(
|
|
config.master_acceleration_limits
|
|
),
|
|
"slave_acceleration_limits": list(
|
|
config.slave_acceleration_limits
|
|
),
|
|
"master_tracking_effort_fraction": (
|
|
config.master_tracking_effort_fraction
|
|
),
|
|
"slave_tracking_effort_fraction": (
|
|
config.slave_tracking_effort_fraction
|
|
),
|
|
"velocity_limit_fraction": config.velocity_limit_fraction,
|
|
"soft_limit_buffer": config.soft_limit_buffer,
|
|
"feedback_strength": config.feedback_strength,
|
|
"haptic_filter_alpha": config.haptic_filter_alpha,
|
|
"haptic_torque_limits": list(
|
|
config.haptic_torque_limits
|
|
),
|
|
"haptic_rate_limits": list(config.haptic_rate_limits),
|
|
"energy_min_J": config.energy_min,
|
|
"energy_max_J": config.energy_max,
|
|
"energy_initial_J": config.energy_initial,
|
|
"energy_probe_mode": config.energy_probe_mode,
|
|
"energy_probe_torque_Nm": config.energy_probe_torque_Nm,
|
|
"energy_probe_start_fraction": (
|
|
config.energy_probe_start_fraction
|
|
),
|
|
"energy_probe_end_fraction": (
|
|
config.energy_probe_end_fraction
|
|
),
|
|
"sensor_noise_std_Nm": config.sensor_noise_std,
|
|
"wrench_characteristic_length_m": (
|
|
config.wrench_characteristic_length_m
|
|
),
|
|
"wrench_scaled_damping": config.wrench_scaled_damping,
|
|
"sensor_bias_Nm": list(config.sensor_bias),
|
|
"bias_calibration_samples": config.bias_calibration_samples,
|
|
"joint_limit_margin": config.joint_limit_margin,
|
|
"differential_step": config.differential_step,
|
|
},
|
|
}
|
|
else:
|
|
# Compatibility contract: do not add even constant keys here. Existing
|
|
# v3 plans depend on the exact historical seed-group hash.
|
|
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_network_pair_group_id(
|
|
trial: Mapping[str, Any],
|
|
data_group_basis: Mapping[str, Any] | None,
|
|
) -> str | None:
|
|
"""Identify the network-profile pairing block when explicitly enabled."""
|
|
enabled = _boolean_factor(
|
|
_factor(trial, "bilateral_pair_network_profiles", False),
|
|
"bilateral_pair_network_profiles",
|
|
)
|
|
if not enabled:
|
|
return None
|
|
if data_group_basis is None:
|
|
raise ValueError(
|
|
"network pairing requires a bilateral data-group basis"
|
|
)
|
|
return (
|
|
"network-pair-"
|
|
+ stable_hash(
|
|
data_group_basis,
|
|
prefix="bilateral-network-pair-group",
|
|
)[:16]
|
|
)
|
|
|
|
|
|
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))
|
|
duration_default = float(trajectory.get("duration_s", 1.2))
|
|
probe_fraction_default = float(
|
|
trajectory.get("contact_probe_fraction", 0.0)
|
|
)
|
|
config = replace(
|
|
SimulationConfig(),
|
|
seed=seed,
|
|
duration=float(
|
|
_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(
|
|
_bilateral_environment_factor(
|
|
trial,
|
|
"contact_probe_fraction",
|
|
probe_fraction_default,
|
|
aliases=("probe_fraction",),
|
|
)
|
|
),
|
|
contact_probe_cycles=float(
|
|
_bilateral_environment_factor(
|
|
trial,
|
|
"contact_probe_cycles",
|
|
SimulationConfig.contact_probe_cycles,
|
|
)
|
|
),
|
|
feedback_delay_s=float(
|
|
_profiled_factor(
|
|
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",
|
|
)
|
|
),
|
|
network_common_random_numbers=_boolean_factor(
|
|
_profiled_factor(
|
|
trial,
|
|
"network_common_random_numbers",
|
|
False,
|
|
profile_name="network_profile",
|
|
),
|
|
"network_common_random_numbers",
|
|
),
|
|
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",
|
|
)
|
|
),
|
|
energy_probe_mode=str(
|
|
_profiled_factor(
|
|
trial,
|
|
"energy_probe_mode",
|
|
"none",
|
|
profile_name="haptic_profile",
|
|
)
|
|
),
|
|
energy_probe_torque_Nm=float(
|
|
_profiled_factor(
|
|
trial,
|
|
"energy_probe_torque_Nm",
|
|
0.0,
|
|
profile_name="haptic_profile",
|
|
)
|
|
),
|
|
energy_probe_start_fraction=float(
|
|
_profiled_factor(
|
|
trial,
|
|
"energy_probe_start_fraction",
|
|
0.20,
|
|
profile_name="haptic_profile",
|
|
)
|
|
),
|
|
energy_probe_end_fraction=float(
|
|
_profiled_factor(
|
|
trial,
|
|
"energy_probe_end_fraction",
|
|
0.80,
|
|
profile_name="haptic_profile",
|
|
)
|
|
),
|
|
wall_fraction=float(
|
|
_bilateral_environment_factor(
|
|
trial, "wall_fraction", SimulationConfig.wall_fraction
|
|
)
|
|
),
|
|
wall_stiffness=float(
|
|
_bilateral_environment_factor(
|
|
trial,
|
|
"wall_stiffness",
|
|
SimulationConfig.wall_stiffness,
|
|
aliases=("stiffness",),
|
|
)
|
|
),
|
|
wall_damping=float(
|
|
_bilateral_environment_factor(
|
|
trial,
|
|
"wall_damping",
|
|
SimulationConfig.wall_damping,
|
|
aliases=("damping",),
|
|
)
|
|
),
|
|
wall_force_limit=float(
|
|
_bilateral_environment_factor(
|
|
trial,
|
|
"wall_force_limit",
|
|
SimulationConfig.wall_force_limit,
|
|
aliases=("force_limit",),
|
|
)
|
|
),
|
|
wall_transition_depth=float(
|
|
_bilateral_environment_factor(
|
|
trial,
|
|
"wall_transition_depth",
|
|
SimulationConfig.wall_transition_depth,
|
|
aliases=("transition_depth",),
|
|
)
|
|
),
|
|
)
|
|
data_group_id, _, data_seeds = _bilateral_data_seed_group(
|
|
trial, config
|
|
)
|
|
if data_group_id is not None:
|
|
assert data_seeds is not None
|
|
data_rng = generator_from_record(data_seeds, "simulation")
|
|
config = replace(
|
|
config,
|
|
seed=int(
|
|
data_rng.integers(0, np.iinfo(np.int32).max)
|
|
),
|
|
)
|
|
config.validate()
|
|
return scenario, config
|
|
|
|
|
|
def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload:
|
|
"""Run one paired H3/H4 rigid-body trial with a frozen network trace."""
|
|
scenario, config = _bilateral_scenario_and_config(trial)
|
|
data_group_id, data_group_basis, data_seed_record = (
|
|
_bilateral_data_seed_group(trial, config)
|
|
)
|
|
network_pair_group_id = _bilateral_network_pair_group_id(
|
|
trial, data_group_basis
|
|
)
|
|
network_pairing_enabled = network_pair_group_id is not None
|
|
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
|
|
)
|
|
samples["configured_wall_force_limit_N"] = np.full(
|
|
sample_count, config.wall_force_limit
|
|
)
|
|
samples["configured_forward_delay_s"] = np.full(
|
|
sample_count, config.forward_delay_s
|
|
)
|
|
samples["configured_return_delay_s"] = np.full(
|
|
sample_count, config.feedback_delay_s
|
|
)
|
|
samples["configured_forward_jitter_s"] = np.full(
|
|
sample_count, config.forward_jitter_s
|
|
)
|
|
samples["configured_return_jitter_s"] = np.full(
|
|
sample_count, config.return_jitter_s
|
|
)
|
|
samples["configured_forward_packet_loss"] = np.full(
|
|
sample_count, config.forward_packet_loss
|
|
)
|
|
samples["configured_return_packet_loss"] = np.full(
|
|
sample_count, config.return_packet_loss
|
|
)
|
|
samples["configured_forward_timeout_s"] = np.full(
|
|
sample_count, config.forward_timeout_s
|
|
)
|
|
samples["configured_return_timeout_s"] = np.full(
|
|
sample_count, config.return_timeout_s
|
|
)
|
|
samples["configured_mapping_hz"] = np.full(
|
|
sample_count, config.mapping_hz
|
|
)
|
|
raw_contact_expected = trajectory.get(
|
|
"contact_expected",
|
|
trajectory.get("family") != "free_space",
|
|
)
|
|
contact_expected = _boolean_factor(
|
|
raw_contact_expected, "trajectory.contact_expected"
|
|
)
|
|
samples["contact_expected"] = np.full(
|
|
sample_count, int(contact_expected), dtype=np.int8
|
|
)
|
|
samples["configured_network_common_random_numbers"] = np.full(
|
|
sample_count,
|
|
int(config.network_common_random_numbers),
|
|
dtype=np.int8,
|
|
)
|
|
h3_eligible = config.energy_probe_mode == "none"
|
|
samples["h3_eligible"] = np.full(
|
|
sample_count, int(h3_eligible), dtype=np.int8
|
|
)
|
|
return TrialPayload(
|
|
samples=samples,
|
|
events=(),
|
|
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,
|
|
},
|
|
"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,
|
|
"bilateral_data_seed_record_hash": (
|
|
stable_hash(
|
|
data_seed_record,
|
|
prefix="bilateral-data-seed-record",
|
|
)
|
|
if data_seed_record is not None
|
|
else None
|
|
),
|
|
"network_pairing_enabled": network_pairing_enabled,
|
|
**(
|
|
{"network_pair_group_id": network_pair_group_id}
|
|
if network_pairing_enabled
|
|
else {}
|
|
),
|
|
"stable_contact_selection": _factor(
|
|
trial, "stable_contact_selection", None
|
|
),
|
|
"effective_network_config": {
|
|
"forward_delay_s": config.forward_delay_s,
|
|
"return_delay_s": config.feedback_delay_s,
|
|
"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,
|
|
"common_random_numbers": (
|
|
config.network_common_random_numbers
|
|
),
|
|
"forward_timeout_s": config.forward_timeout_s,
|
|
"return_timeout_s": config.return_timeout_s,
|
|
},
|
|
"effective_environment_config": {
|
|
"duration_s": config.duration,
|
|
"mapping_hz": config.mapping_hz,
|
|
"contact_probe_fraction": config.contact_probe_fraction,
|
|
"contact_probe_cycles": config.contact_probe_cycles,
|
|
"wall_fraction": config.wall_fraction,
|
|
"wall_stiffness_N_per_m": config.wall_stiffness,
|
|
"wall_damping_Ns_per_m": config.wall_damping,
|
|
"wall_force_limit_N": config.wall_force_limit,
|
|
"wall_transition_depth_m": config.wall_transition_depth,
|
|
},
|
|
"wall": wall_metadata,
|
|
"online_metrics_are_diagnostic_only": result.metrics,
|
|
},
|
|
)
|