990 lines
37 KiB
Python
990 lines
37 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 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,
|
|
},
|
|
)
|