Initial reproducible teleoperation paper and simulation

.gitignore

@@ -0,0 +1,47 @@
+# Local environments and Python build products
+.venv/
+**/__pycache__/
+*.py[cod]
+*.egg-info/
+.pytest_cache/
+.ruff_cache/
+.mypy_cache/
+
+# IDE and Codex-local state
+.idea/
+.agents/
+.codex/
+
+# Generated simulation, analysis, and scratch outputs
+output/
+tmp/
+code/logs/
+code/assets/
+code/test/MUJOCO_LOG.TXT
+
+# Patent drafts and third-party prior-art documents are not part of the
+# reproducible paper/code package and may contain unpublished material.
+patant/
+
+# LaTeX build products and vendor instructions
+paper/exoskeleton/Transactions-instructions-only.pdf
+# The canonical manuscript is IEEEtran/main2.tex. Keep the historical draft
+# and its unused illustrative/result figures local to avoid ambiguous evidence.
+paper/exoskeleton/IEEEtran/main.tex
+paper/exoskeleton/assets/energy_tank_dynamics.png
+paper/exoskeleton/assets/est_errors.png
+paper/exoskeleton/assets/fig_alpha.png
+paper/exoskeleton/assets/fig_energy.png
+paper/exoskeleton/assets/fig_metrics_barchart.png
+paper/exoskeleton/assets/fig_tau_fb_norm.png
+paper/exoskeleton/assets/sew_error_hist.png
+paper/exoskeleton/assets/sew_error_scatter.png
+paper/**/*.aux
+paper/**/*.bbl
+paper/**/*.blg
+paper/**/*.fdb_latexmk
+paper/**/*.fls
+paper/**/*.log
+paper/**/*.out
+paper/**/*.synctex.gz
+paper/**/*.toc

README.md

@@ -0,0 +1,82 @@
+# Heterogeneous 7-DoF Bilateral Teleoperation
+
+This repository contains the manuscript, canonical robot models, pre-prototype
+closed-loop simulator, and reproducible evidence pipeline for heterogeneous
+7-DoF master--slave teleoperation.
+
+The current implementation supports numerical and simulation evidence only.
+It must not be used to claim physical wrench accuracy, hardware stability, or
+human-subject performance before the corresponding locked hardware studies are
+completed.
+
+## Repository layout
+
+- `code/core/`: SEW retargeting, H1 baselines, wrench estimation, feedback
+  mapping, network emulation, command allocation, and passivity supervision.
+- `code/simulate_closed_loop.py`: rigid-body bilateral simulation using
+  `master_7dof.urdf` and `real_slave_7dof.urdf`.
+- `code/experiments/`: immutable plans, paired random streams, atomic trial
+  logging, manifests, validation, and executable H1--H4 adapters.
+- `code/analysis/`: independent endpoint reconstruction and paper source-data
+  generation.
+- `code/config/experiments/`: smoke, calibration, and locked-template study
+  specifications.
+- `paper/exoskeleton/IEEEtran/main2.tex`: canonical manuscript source.
+
+## Reproducible environment
+
+Python 3.10 is required. From the repository root:
+
+```bash
+uv sync --locked
+```
+
+The lock file pins the Pinocchio-compatible `urdfdom` and `tinyxml2` ABIs.
+
+## Verification
+
+```bash
+PYTHONDONTWRITEBYTECODE=1 \
+MPLCONFIGDIR=/tmp/exoskeleton-mpl-cache \
+XDG_CACHE_HOME=/tmp/exoskeleton-xdg-cache \
+.venv/bin/python -m unittest discover -s code/test -p 'test_*.py' -v
+```
+
+## Minimal H1 smoke study
+
+```bash
+.venv/bin/exo-experiment plan \
+  --spec code/config/experiments/h1_smoke.json \
+  --output /tmp/h1-plan.json
+
+.venv/bin/exo-experiment run \
+  --plan /tmp/h1-plan.json \
+  --batch-dir output/experiments/h1-smoke \
+  --executor experiments.executors:execute_h1_retargeting
+
+.venv/bin/exo-experiment validate \
+  --batch-dir output/experiments/h1-smoke
+
+.venv/bin/exo-paper-artifacts \
+  --batch-dir output/experiments/h1-smoke \
+  --metric-config code/config/experiments/metrics_h1_calibration.json
+```
+
+Equivalent executor/config pairs are documented in
+`code/experiments/README.md`.
+
+## Manuscript build
+
+Compile from `paper/exoskeleton` so the `assets/` paths resolve:
+
+```bash
+cd paper/exoskeleton
+latexmk -pdf -interaction=nonstopmode -halt-on-error IEEEtran/main2.tex
+```
+
+## Evidence locking
+
+Calibration and pilot studies may run without Git provenance. A `locked` study
+requires a clean Git worktree, an immutable commit, and matching hashes for the
+environment lock and canonical URDFs. Restoring a locked batch from a different
+commit or modified source files is rejected by design.

code/analysis/__init__.py

@@ -0,0 +1,17 @@
+"""Independent derivation of trial-level evidence and paper source data."""
+
+from .metrics import (
+    audit_h4_energy,
+    compute_h1_composite,
+    compute_h2_wrench_metrics,
+    compute_h3_power_mismatch,
+    derive_trial_metrics,
+)
+
+__all__ = [
+    "audit_h4_energy",
+    "compute_h1_composite",
+    "compute_h2_wrench_metrics",
+    "compute_h3_power_mismatch",
+    "derive_trial_metrics",
+]

code/analysis/make_paper_artifacts.py

@@ -0,0 +1,259 @@
+"""Generate versioned trial metrics and paper source-data tables."""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+import os
+import tempfile
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+
+from experiments.hashing import file_sha256, stable_hash, to_jsonable
+from experiments.io import atomic_write_json, atomic_write_jsonl, utc_now
+from experiments.plan import load_document
+from experiments.schema import SCHEMA_VERSION, STORAGE_FORMATS
+from experiments.validate import require_valid_batch
+
+from .metrics import derive_trial_metrics
+
+
+def _method_selected(
+    family: str,
+    family_configuration: Mapping[str, Any],
+    method_id: str,
+) -> bool:
+    """Apply explicit per-family method filters without silent typos."""
+    include = family_configuration.get("include_methods")
+    exclude = family_configuration.get("exclude_methods")
+    for name, value in (
+        ("include_methods", include),
+        ("exclude_methods", exclude),
+    ):
+        if value is not None and (
+            not isinstance(value, list)
+            or any(not isinstance(item, str) or not item for item in value)
+        ):
+            raise ValueError(f"{family}.{name} must be a list of method IDs")
+    if include is not None and method_id not in include:
+        return False
+    if exclude is not None and method_id in exclude:
+        return False
+    return True
+
+
+def _trial_metric_configuration(
+    configuration: Mapping[str, Any],
+    method_id: str,
+) -> dict[str, Any] | None:
+    enabled = configuration.get("enabled")
+    if not isinstance(enabled, list) or not enabled:
+        raise ValueError("metric configuration needs a non-empty enabled list")
+    selected = []
+    trial_configuration: dict[str, Any] = {}
+    for family in enabled:
+        if not isinstance(family, str) or not family:
+            raise ValueError("enabled metric family names must be non-empty strings")
+        family_configuration = configuration.get(family, {})
+        if not isinstance(family_configuration, Mapping):
+            raise ValueError(f"{family} metric configuration must be a mapping")
+        if _method_selected(family, family_configuration, method_id):
+            selected.append(family)
+            trial_configuration[family] = dict(family_configuration)
+    if not selected:
+        return None
+    trial_configuration["enabled"] = selected
+    return trial_configuration
+
+
+def _scalar_cell(value: Any) -> Any:
+    if value is None or isinstance(value, (str, int, float, bool)):
+        return value
+    return json.dumps(
+        to_jsonable(value),
+        sort_keys=True,
+        separators=(",", ":"),
+        ensure_ascii=False,
+    )
+
+
+def _identity_row(trial: Mapping[str, Any]) -> dict[str, Any]:
+    trajectory = trial["trajectory"]
+    method = trial["method"]
+    row = {
+        "trial_id": trial["trial_id"],
+        "pair_id": trial["pair_id"],
+        "study_id": trial["study_id"],
+        "split": trial["split"],
+        "method_id": method["method_id"],
+        "trajectory_id": trajectory["trajectory_id"],
+        "trajectory_family": trajectory.get("family", ""),
+        "replicate": trial["replicate"],
+    }
+    for name, value in sorted(trial.get("factors", {}).items()):
+        row[f"factor_{name}"] = _scalar_cell(value)
+    return row
+
+
+def _atomic_write_csv(path: Path, rows: Sequence[Mapping[str, Any]]) -> None:
+    path.parent.mkdir(parents=True, exist_ok=True)
+    identity_order = [
+        "trial_id",
+        "pair_id",
+        "study_id",
+        "split",
+        "method_id",
+        "trajectory_id",
+        "trajectory_family",
+        "replicate",
+    ]
+    all_fields = {key for row in rows for key in row}
+    fieldnames = [name for name in identity_order if name in all_fields]
+    fieldnames.extend(sorted(all_fields - set(fieldnames)))
+    fd, temporary_name = tempfile.mkstemp(
+        prefix=f".{path.name}.", suffix=".tmp", dir=path.parent
+    )
+    temporary = Path(temporary_name)
+    try:
+        with os.fdopen(fd, "w", encoding="utf-8", newline="") as stream:
+            writer = csv.DictWriter(stream, fieldnames=fieldnames)
+            writer.writeheader()
+            for row in rows:
+                writer.writerow(
+                    {name: _scalar_cell(row.get(name)) for name in fieldnames}
+                )
+            stream.flush()
+            os.fsync(stream.fileno())
+        os.replace(temporary, path)
+    except Exception:
+        temporary.unlink(missing_ok=True)
+        raise
+
+
+def generate_paper_source_data(
+    batch_dir: Path,
+    metric_configuration: Mapping[str, Any],
+    *,
+    output_root: Path | None = None,
+) -> dict[str, Any]:
+    """Recompute all configured metrics and emit traceable source-data tables."""
+    batch_dir = Path(batch_dir)
+    require_valid_batch(batch_dir, require_complete=True)
+    plan = load_document(batch_dir / "plan.json")
+    output_root = batch_dir if output_root is None else Path(output_root)
+    derived_dir = output_root / "derived"
+    source_dir = output_root / "paper" / "source_data"
+    derived_dir.mkdir(parents=True, exist_ok=True)
+    source_dir.mkdir(parents=True, exist_ok=True)
+
+    rows: list[dict[str, Any]] = []
+    input_files: dict[str, str] = {}
+    for trial in plan["trials"]:
+        trial_dir = batch_dir / "raw" / trial["trial_id"]
+        sample_path = trial_dir / "samples.npz"
+        input_files[str(sample_path.relative_to(batch_dir))] = file_sha256(sample_path)
+        with np.load(sample_path, allow_pickle=False) as archive:
+            samples = {name: archive[name] for name in archive.files}
+        method_id = trial["method"]["method_id"]
+        trial_configuration = _trial_metric_configuration(
+            metric_configuration,
+            method_id,
+        )
+        metrics = (
+            {}
+            if trial_configuration is None
+            else derive_trial_metrics(samples, trial_configuration)
+        )
+        rows.append({**_identity_row(trial), **metrics})
+
+    metric_path = derived_dir / "trial_metrics.jsonl"
+    atomic_write_jsonl(metric_path, rows)
+    table_files: dict[str, str] = {}
+    family_row_counts: dict[str, int] = {}
+    for family in metric_configuration["enabled"]:
+        family_rows: list[dict[str, Any]] = []
+        prefix = f"{family}_"
+        for row in rows:
+            if not any(key.startswith(prefix) for key in row):
+                continue
+            selected = {
+                key: value
+                for key, value in row.items()
+                if key in {
+                    "trial_id",
+                    "pair_id",
+                    "study_id",
+                    "split",
+                    "method_id",
+                    "trajectory_id",
+                    "trajectory_family",
+                    "replicate",
+                }
+                or key.startswith("factor_")
+                or key.startswith(prefix)
+            }
+            family_rows.append(selected)
+        if not family_rows:
+            raise ValueError(
+                f"metric family {family!r} selected no methods in this plan"
+            )
+        table_path = source_dir / f"{family}.csv"
+        _atomic_write_csv(table_path, family_rows)
+        table_files[str(table_path.relative_to(output_root))] = file_sha256(table_path)
+        family_row_counts[family] = len(family_rows)
+
+    manifest = {
+        "kind": "paper_artifact_manifest",
+        "schema_version": SCHEMA_VERSION,
+        "created_utc": utc_now(),
+        "source_batch": str(batch_dir),
+        "source_plan_hash": plan["plan_hash"],
+        "metric_configuration": to_jsonable(metric_configuration),
+        "metric_configuration_hash": stable_hash(
+            metric_configuration, prefix="metric-configuration"
+        ),
+        "row_count": len(rows),
+        "family_row_counts": family_row_counts,
+        "row_hash": stable_hash(rows, prefix="trial-metric-rows"),
+        "storage_formats": {
+            "trial_metrics": STORAGE_FORMATS["trial_metrics"],
+            "paper_source_data": STORAGE_FORMATS["paper_source_data"],
+        },
+        "inputs": input_files,
+        "outputs": {
+            str(metric_path.relative_to(output_root)): file_sha256(metric_path),
+            **table_files,
+        },
+    }
+    manifest_path = output_root / "paper" / "artifact_manifest.json"
+    atomic_write_json(manifest_path, manifest)
+    return manifest
+
+
+def build_parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(
+        description="Recompute trial metrics and paper source-data tables"
+    )
+    parser.add_argument("--batch-dir", type=Path, required=True)
+    parser.add_argument("--metric-config", type=Path, required=True)
+    parser.add_argument("--output-root", type=Path, default=None)
+    return parser
+
+
+def main(argv: list[str] | None = None) -> int:
+    args = build_parser().parse_args(argv)
+    configuration = load_document(args.metric_config)
+    manifest = generate_paper_source_data(
+        args.batch_dir,
+        configuration,
+        output_root=args.output_root,
+    )
+    print(json.dumps(manifest, indent=2, ensure_ascii=False, sort_keys=True))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

code/analysis/metrics.py

@@ -0,0 +1,636 @@
+"""Independent H1--H4 trial metrics.
+
+These functions consume stored arrays only.  They deliberately do not import
+the simulation or controller implementations, so paper endpoints can be
+reconstructed independently from raw evidence.
+"""
+
+from __future__ import annotations
+
+from typing import Any, Mapping
+
+import numpy as np
+
+
+METRIC_SCHEMA_VERSION = "1.0.0"
+
+
+class MetricError(ValueError):
+    pass
+
+
+def _vector(value: Any, name: str, *, dtype=float) -> np.ndarray:
+    array = np.asarray(value, dtype=dtype)
+    if array.ndim != 1 or array.size == 0:
+        raise MetricError(f"{name} must be a non-empty one-dimensional array")
+    return array
+
+
+def _matrix(value: Any, name: str, columns: int | None = None) -> np.ndarray:
+    array = np.asarray(value, dtype=float)
+    if array.ndim != 2 or array.shape[0] == 0:
+        raise MetricError(f"{name} must be a non-empty two-dimensional array")
+    if columns is not None and array.shape[1] != columns:
+        raise MetricError(f"{name} must have {columns} columns")
+    return array
+
+
+def _same_rows(named: Mapping[str, np.ndarray]) -> int:
+    counts = {name: value.shape[0] for name, value in named.items()}
+    if len(set(counts.values())) != 1:
+        raise MetricError(f"sample counts differ: {counts}")
+    return next(iter(counts.values()))
+
+
+def _finite(array: np.ndarray, name: str) -> None:
+    if not np.all(np.isfinite(array)):
+        raise MetricError(f"{name} contains non-finite values")
+
+
+def _wrapped_delta(array: np.ndarray) -> np.ndarray:
+    return (array + np.pi) % (2.0 * np.pi) - np.pi
+
+
+def compute_h1_composite(
+    *,
+    mapping_valid: Any,
+    position_error_m: Any,
+    orientation_error_rad: Any,
+    q_slave: Any,
+    swivel_angle_rad: Any,
+    master_step_norm: Any,
+    position_threshold_m: float,
+    orientation_threshold_rad: float,
+    joint_step_threshold_rad: float,
+    swivel_step_threshold_rad: float,
+    input_step_threshold_rad: float,
+    accepted: Any | None = None,
+    commanded_reset: Any | None = None,
+    degeneracy_transition: Any | None = None,
+) -> dict[str, Any]:
+    """Compute the trajectory-level H1 ``F_r``, ``D_r``, and ``C_r``."""
+    valid = _vector(mapping_valid, "mapping_valid", dtype=bool)
+    e_position = _vector(position_error_m, "position_error_m")
+    e_orientation = _vector(orientation_error_rad, "orientation_error_rad")
+    slave = _matrix(q_slave, "q_slave")
+    swivel = _vector(swivel_angle_rad, "swivel_angle_rad")
+    input_step = _vector(master_step_norm, "master_step_norm")
+    n = _same_rows(
+        {
+            "mapping_valid": valid,
+            "position_error_m": e_position,
+            "orientation_error_rad": e_orientation,
+            "q_slave": slave,
+            "swivel_angle_rad": swivel,
+            "master_step_norm": input_step,
+        }
+    )
+    accepted_array = (
+        np.ones(n, dtype=bool)
+        if accepted is None
+        else _vector(accepted, "accepted", dtype=bool)
+    )
+    reset = (
+        np.zeros(n, dtype=bool)
+        if commanded_reset is None
+        else _vector(commanded_reset, "commanded_reset", dtype=bool)
+    )
+    degeneracy = (
+        np.zeros(n, dtype=bool)
+        if degeneracy_transition is None
+        else _vector(
+            degeneracy_transition,
+            "degeneracy_transition",
+            dtype=bool,
+        )
+    )
+    _same_rows(
+        {
+            "accepted": accepted_array,
+            "commanded_reset": reset,
+            "degeneracy_transition": degeneracy,
+            "mapping_valid": valid,
+        }
+    )
+    for array, name in (
+        (e_position, "position_error_m"),
+        (e_orientation, "orientation_error_rad"),
+        (slave, "q_slave"),
+        (swivel, "swivel_angle_rad"),
+        (input_step, "master_step_norm"),
+    ):
+        _finite(array, name)
+    thresholds = {
+        "position_threshold_m": position_threshold_m,
+        "orientation_threshold_rad": orientation_threshold_rad,
+        "joint_step_threshold_rad": joint_step_threshold_rad,
+        "swivel_step_threshold_rad": swivel_step_threshold_rad,
+        "input_step_threshold_rad": input_step_threshold_rad,
+    }
+    if any(not np.isfinite(value) or value < 0.0 for value in thresholds.values()):
+        raise MetricError("H1 thresholds must be finite and non-negative")
+
+    failure_mask = accepted_array & (
+        ~valid
+        | (e_position > position_threshold_m)
+        | (e_orientation > orientation_threshold_rad)
+    )
+    if n > 1:
+        joint_step = np.max(
+            np.abs(_wrapped_delta(slave[1:] - slave[:-1])),
+            axis=1,
+        )
+        swivel_step = np.abs(_wrapped_delta(swivel[1:] - swivel[:-1]))
+        eligible = (
+            accepted_array[1:]
+            & accepted_array[:-1]
+            & valid[1:]
+            & valid[:-1]
+            & (input_step[1:] <= input_step_threshold_rad)
+            & ~reset[1:]
+            & ~degeneracy[1:]
+        )
+        discontinuity_mask = eligible & (
+            (joint_step > joint_step_threshold_rad)
+            | (swivel_step > swivel_step_threshold_rad)
+        )
+    else:
+        joint_step = np.empty(0, dtype=float)
+        swivel_step = np.empty(0, dtype=float)
+        eligible = np.empty(0, dtype=bool)
+        discontinuity_mask = np.empty(0, dtype=bool)
+
+    F_r = int(np.any(failure_mask))
+    D_r = int(np.any(discontinuity_mask))
+    return {
+        "h1_F_r": F_r,
+        "h1_D_r": D_r,
+        "h1_C_r": max(F_r, D_r),
+        "h1_failure_sample_count": int(np.sum(failure_mask)),
+        "h1_discontinuity_sample_count": int(np.sum(discontinuity_mask)),
+        "h1_eligible_increment_count": int(np.sum(eligible)),
+        "h1_mapping_valid_fraction": float(np.mean(valid[accepted_array]))
+        if np.any(accepted_array)
+        else 0.0,
+        "h1_max_position_error_m": float(np.max(e_position[accepted_array]))
+        if np.any(accepted_array)
+        else 0.0,
+        "h1_max_orientation_error_rad": float(np.max(e_orientation[accepted_array]))
+        if np.any(accepted_array)
+        else 0.0,
+        "h1_max_joint_step_rad": float(np.max(joint_step))
+        if joint_step.size
+        else 0.0,
+        "h1_max_swivel_step_rad": float(np.max(swivel_step))
+        if swivel_step.size
+        else 0.0,
+    }
+
+
+def compute_h2_wrench_metrics(
+    wrench_estimated: Any,
+    wrench_reference: Any,
+    *,
+    sample_mask: Any | None = None,
+) -> dict[str, Any]:
+    estimate = _matrix(wrench_estimated, "wrench_estimated", columns=6)
+    reference = _matrix(wrench_reference, "wrench_reference", columns=6)
+    n = _same_rows({"wrench_estimated": estimate, "wrench_reference": reference})
+    _finite(estimate, "wrench_estimated")
+    _finite(reference, "wrench_reference")
+    mask = (
+        np.ones(n, dtype=bool)
+        if sample_mask is None
+        else _vector(sample_mask, "sample_mask", dtype=bool)
+    )
+    if mask.shape[0] != n or not np.any(mask):
+        raise MetricError("H2 sample_mask must select at least one aligned sample")
+    error = estimate[mask] - reference[mask]
+    force_norm = np.linalg.norm(error[:, :3], axis=1)
+    moment_norm = np.linalg.norm(error[:, 3:], axis=1)
+    return {
+        "h2_sample_count": int(error.shape[0]),
+        "h2_force_rmse_N": float(np.sqrt(np.mean(force_norm**2))),
+        "h2_moment_rmse_Nm": float(np.sqrt(np.mean(moment_norm**2))),
+        "h2_force_mae_N": float(np.mean(force_norm)),
+        "h2_moment_mae_Nm": float(np.mean(moment_norm)),
+        "h2_force_bias_xyz_N": np.mean(error[:, :3], axis=0).tolist(),
+        "h2_moment_bias_xyz_Nm": np.mean(error[:, 3:], axis=0).tolist(),
+    }
+
+
+def _dt_array(dt: Any, count: int) -> np.ndarray:
+    array = np.asarray(dt, dtype=float)
+    if array.ndim == 0:
+        array = np.full(count, float(array), dtype=float)
+    if array.shape != (count,):
+        raise MetricError(f"dt must be scalar or have shape ({count},)")
+    if not np.all(np.isfinite(array)) or np.any(array <= 0.0):
+        raise MetricError("dt must contain finite positive intervals")
+    return array
+
+
+def compute_h3_power_mismatch(
+    *,
+    tau_master_raw: Any,
+    qd_master: Any,
+    tau_slave_source: Any,
+    qd_slave_source: Any,
+    dt: Any,
+    force_scale: float = 1.0,
+    epsilon_energy_J: float = 1e-12,
+    return_valid: Any | None = None,
+) -> dict[str, Any]:
+    tau_m = _matrix(tau_master_raw, "tau_master_raw")
+    qd_m = _matrix(qd_master, "qd_master")
+    tau_s = _matrix(tau_slave_source, "tau_slave_source")
+    qd_s = _matrix(qd_slave_source, "qd_slave_source")
+    n = _same_rows(
+        {
+            "tau_master_raw": tau_m,
+            "qd_master": qd_m,
+            "tau_slave_source": tau_s,
+            "qd_slave_source": qd_s,
+        }
+    )
+    if tau_m.shape != qd_m.shape or tau_s.shape != qd_s.shape:
+        raise MetricError("torque and velocity shapes must match at each port")
+    for array, name in (
+        (tau_m, "tau_master_raw"),
+        (qd_m, "qd_master"),
+        (tau_s, "tau_slave_source"),
+        (qd_s, "qd_slave_source"),
+    ):
+        _finite(array, name)
+    intervals = _dt_array(dt, n)
+    chi = (
+        np.ones(n, dtype=bool)
+        if return_valid is None
+        else _vector(return_valid, "return_valid", dtype=bool)
+    )
+    if chi.shape[0] != n:
+        raise MetricError("return_valid length mismatch")
+    force_scale = float(force_scale)
+    epsilon_energy_J = float(epsilon_energy_J)
+    if not np.isfinite(force_scale) or force_scale < 0.0:
+        raise MetricError("force_scale must be finite and non-negative")
+    if not np.isfinite(epsilon_energy_J) or epsilon_energy_J <= 0.0:
+        raise MetricError("epsilon_energy_J must be finite and positive")
+
+    power_master = np.einsum("ij,ij->i", tau_m, qd_m)
+    power_slave = chi.astype(float) * np.einsum("ij,ij->i", tau_s, qd_s)
+    scaled_slave = force_scale * power_slave
+    numerator = float(np.sum(np.abs(power_master - scaled_slave) * intervals))
+    denominator = float(
+        0.5
+        * np.sum((np.abs(power_master) + np.abs(scaled_slave)) * intervals)
+        + epsilon_energy_J
+    )
+    return {
+        "h3_epsilon_P_act": numerator / denominator,
+        "h3_power_mismatch_numerator_J": numerator,
+        "h3_power_normalizer_J": denominator,
+        "h3_return_valid_fraction": float(np.mean(chi)),
+        "h3_master_raw_work_J": float(np.sum(power_master * intervals)),
+        "h3_scaled_slave_work_J": float(np.sum(scaled_slave * intervals)),
+    }
+
+
+def audit_h4_energy(
+    *,
+    energy_before_J: Any,
+    energy_after_J: Any,
+    tau_candidate: Any,
+    tau_applied: Any,
+    tau_accepted: Any | None = None,
+    qd_master: Any,
+    dt: Any,
+    energy_min_J: float,
+    energy_max_J: float,
+    epsilon_torque_impulse_Nms: float = 1e-12,
+    audit_tolerance_J: float = 1e-10,
+    software_preclip_J: Any | None = None,
+) -> dict[str, Any]:
+    energy_before = _vector(energy_before_J, "energy_before_J")
+    energy_after = _vector(energy_after_J, "energy_after_J")
+    candidate = _matrix(tau_candidate, "tau_candidate")
+    applied = _matrix(tau_applied, "tau_applied")
+    accepted = (
+        applied
+        if tau_accepted is None
+        else _matrix(tau_accepted, "tau_accepted")
+    )
+    velocity = _matrix(qd_master, "qd_master")
+    n = _same_rows(
+        {
+            "energy_before_J": energy_before,
+            "energy_after_J": energy_after,
+            "tau_candidate": candidate,
+            "tau_applied": applied,
+            "tau_accepted": accepted,
+            "qd_master": velocity,
+        }
+    )
+    if not (
+        candidate.shape
+        == applied.shape
+        == accepted.shape
+        == velocity.shape
+    ):
+        raise MetricError(
+            "candidate, projected, accepted torque, and velocity must align"
+        )
+    for array, name in (
+        (energy_before, "energy_before_J"),
+        (energy_after, "energy_after_J"),
+        (candidate, "tau_candidate"),
+        (applied, "tau_applied"),
+        (accepted, "tau_accepted"),
+        (velocity, "qd_master"),
+    ):
+        _finite(array, name)
+    intervals = _dt_array(dt, n)
+    energy_min_J = float(energy_min_J)
+    energy_max_J = float(energy_max_J)
+    tolerance = float(audit_tolerance_J)
+    epsilon_tau = float(epsilon_torque_impulse_Nms)
+    if not (
+        np.isfinite(energy_min_J)
+        and np.isfinite(energy_max_J)
+        and 0.0 <= energy_min_J <= energy_max_J
+    ):
+        raise MetricError("invalid energy bounds")
+    if not np.isfinite(tolerance) or tolerance < 0.0:
+        raise MetricError("audit_tolerance_J must be finite and non-negative")
+    if not np.isfinite(epsilon_tau) or epsilon_tau <= 0.0:
+        raise MetricError(
+            "epsilon_torque_impulse_Nms must be finite and positive"
+        )
+
+    # The deterministic gate is reconstructed at the actuator-accepted port.
+    # If no drive readback exists, callers may omit tau_accepted and explicitly
+    # declare that projected == accepted for that backend.
+    applied_power = np.einsum("ij,ij->i", accepted, velocity)
+    candidate_power = np.einsum("ij,ij->i", candidate, velocity)
+    reconstructed_preclip = energy_before - applied_power * intervals
+    reconstructed_after = np.clip(
+        reconstructed_preclip,
+        energy_min_J,
+        energy_max_J,
+    )
+    deficit = np.maximum(0.0, energy_min_J - reconstructed_preclip)
+    accounting_error = np.abs(energy_after - reconstructed_after)
+
+    shadow_energy = float(energy_before[0])
+    shadow_min = shadow_energy
+    for power, interval in zip(candidate_power, intervals):
+        shadow_energy = min(energy_max_J, shadow_energy - float(power * interval))
+        shadow_min = min(shadow_min, shadow_energy)
+    shadow_deficit = max(0.0, energy_min_J - shadow_min)
+    projected_deficit = float(np.max(deficit))
+
+    numerator = float(
+        np.sum(np.linalg.norm(accepted - candidate, axis=1) * intervals)
+    )
+    denominator = float(
+        np.sum(np.linalg.norm(candidate, axis=1) * intervals) + epsilon_tau
+    )
+    preclip_discrepancy = 0.0
+    if software_preclip_J is not None:
+        software = _vector(software_preclip_J, "software_preclip_J")
+        if software.shape[0] != n:
+            raise MetricError("software_preclip_J length mismatch")
+        _finite(software, "software_preclip_J")
+        preclip_discrepancy = float(
+            np.max(np.abs(software - reconstructed_preclip))
+        )
+    audit_pass = (
+        projected_deficit <= tolerance
+        and float(np.max(accounting_error)) <= tolerance
+        and preclip_discrepancy <= tolerance
+    )
+    return {
+        "h4_energy_audit_pass": bool(audit_pass),
+        "h4_preclip_floor_deficit_max_J": projected_deficit,
+        "h4_energy_accounting_max_error_J": float(np.max(accounting_error)),
+        "h4_software_preclip_max_error_J": preclip_discrepancy,
+        "h4_shadow_floor_deficit_J": float(shadow_deficit),
+        "h4_projected_floor_deficit_J": projected_deficit,
+        "h4_delta_B_J": float(shadow_deficit - projected_deficit),
+        "h4_D_proj": numerator / denominator,
+        "h4_projection_distortion_numerator_Nms": numerator,
+        "h4_projection_distortion_normalizer_Nms": denominator,
+        "h4_shadow_energy_min_J": float(shadow_min),
+        "h4_downstream_modification_max_Nm": float(
+            np.max(np.linalg.norm(accepted - applied, axis=1))
+        ),
+    }
+
+
+def _field(
+    samples: Mapping[str, Any],
+    fields: Mapping[str, str],
+    logical_name: str,
+    default_name: str,
+    *,
+    optional: bool = False,
+) -> Any:
+    stored_name = fields.get(logical_name, default_name)
+    if stored_name not in samples:
+        if optional:
+            return None
+        raise MetricError(
+            f"missing sample field {stored_name!r} for {logical_name!r}"
+        )
+    return samples[stored_name]
+
+
+def derive_trial_metrics(
+    samples: Mapping[str, Any],
+    configuration: Mapping[str, Any],
+) -> dict[str, Any]:
+    """Derive configured endpoint families from one stored trial."""
+    enabled = configuration.get("enabled")
+    if not isinstance(enabled, list) or not enabled:
+        raise MetricError("metric configuration needs a non-empty enabled list")
+    result: dict[str, Any] = {"metric_schema_version": METRIC_SCHEMA_VERSION}
+    for family in enabled:
+        family_config = configuration.get(family, {})
+        fields = family_config.get("fields", {})
+        if family == "h1":
+            thresholds = family_config.get("thresholds", {})
+            required_thresholds = (
+                "position_threshold_m",
+                "orientation_threshold_rad",
+                "joint_step_threshold_rad",
+                "swivel_step_threshold_rad",
+                "input_step_threshold_rad",
+            )
+            missing = [name for name in required_thresholds if name not in thresholds]
+            if missing:
+                raise MetricError(f"missing H1 thresholds: {missing}")
+            pose_success = _field(
+                samples, fields, "pose_success", "map_pose_success"
+            )
+            differential_valid = _field(
+                samples,
+                fields,
+                "differential_valid",
+                "map_differential_valid",
+            )
+            mapping_valid = np.asarray(pose_success, dtype=bool) & np.asarray(
+                differential_valid, dtype=bool
+            )
+            result.update(
+                compute_h1_composite(
+                    mapping_valid=mapping_valid,
+                    position_error_m=_field(
+                        samples,
+                        fields,
+                        "position_error_m",
+                        "map_position_error_m",
+                    ),
+                    orientation_error_rad=_field(
+                        samples,
+                        fields,
+                        "orientation_error_rad",
+                        "map_orientation_error_rad",
+                    ),
+                    q_slave=_field(samples, fields, "q_slave", "map_q_slave"),
+                    swivel_angle_rad=_field(
+                        samples,
+                        fields,
+                        "swivel_angle_rad",
+                        "map_swivel_angle_rad",
+                    ),
+                    master_step_norm=_field(
+                        samples,
+                        fields,
+                        "master_step_norm",
+                        "map_master_step_norm",
+                    ),
+                    accepted=_field(
+                        samples,
+                        fields,
+                        "accepted",
+                        "map_accepted",
+                        optional=True,
+                    ),
+                    commanded_reset=_field(
+                        samples,
+                        fields,
+                        "commanded_reset",
+                        "map_commanded_reset",
+                        optional=True,
+                    ),
+                    degeneracy_transition=_field(
+                        samples,
+                        fields,
+                        "degeneracy_transition",
+                        "map_degeneracy_transition",
+                        optional=True,
+                    ),
+                    **thresholds,
+                )
+            )
+        elif family == "h2":
+            result.update(
+                compute_h2_wrench_metrics(
+                    _field(
+                        samples,
+                        fields,
+                        "wrench_estimated",
+                        "wrench_estimated",
+                    ),
+                    _field(
+                        samples,
+                        fields,
+                        "wrench_reference",
+                        "wrench_reference",
+                    ),
+                    sample_mask=_field(
+                        samples,
+                        fields,
+                        "sample_mask",
+                        "wrench_sample_mask",
+                        optional=True,
+                    ),
+                )
+            )
+        elif family == "h3":
+            result.update(
+                compute_h3_power_mismatch(
+                    tau_master_raw=_field(
+                        samples, fields, "tau_master_raw", "tau_master_raw"
+                    ),
+                    qd_master=_field(samples, fields, "qd_master", "qd_master"),
+                    tau_slave_source=_field(
+                        samples,
+                        fields,
+                        "tau_slave_source",
+                        "tau_slave_source",
+                    ),
+                    qd_slave_source=_field(
+                        samples,
+                        fields,
+                        "qd_slave_source",
+                        "qd_slave_source",
+                    ),
+                    dt=_field(samples, fields, "dt", "dt"),
+                    return_valid=_field(
+                        samples,
+                        fields,
+                        "return_valid",
+                        "return_valid",
+                        optional=True,
+                    ),
+                    force_scale=family_config.get("force_scale", 1.0),
+                    epsilon_energy_J=family_config.get(
+                        "epsilon_energy_J", 1e-12
+                    ),
+                )
+            )
+        elif family == "h4":
+            result.update(
+                audit_h4_energy(
+                    energy_before_J=_field(
+                        samples, fields, "energy_before_J", "energy_before_J"
+                    ),
+                    energy_after_J=_field(
+                        samples, fields, "energy_after_J", "energy_after_J"
+                    ),
+                    tau_candidate=_field(
+                        samples, fields, "tau_candidate", "tau_master_candidate"
+                    ),
+                    tau_applied=_field(
+                        samples, fields, "tau_applied", "tau_master_applied"
+                    ),
+                    tau_accepted=_field(
+                        samples,
+                        fields,
+                        "tau_accepted",
+                        "tau_master_accepted",
+                        optional=True,
+                    ),
+                    qd_master=_field(samples, fields, "qd_master", "qd_master"),
+                    dt=_field(samples, fields, "dt", "dt"),
+                    software_preclip_J=_field(
+                        samples,
+                        fields,
+                        "software_preclip_J",
+                        "energy_preclip_J",
+                        optional=True,
+                    ),
+                    energy_min_J=family_config["energy_min_J"],
+                    energy_max_J=family_config["energy_max_J"],
+                    epsilon_torque_impulse_Nms=family_config.get(
+                        "epsilon_torque_impulse_Nms", 1e-12
+                    ),
+                    audit_tolerance_J=family_config.get(
+                        "audit_tolerance_J", 1e-10
+                    ),
+                )
+            )
+        else:
+            raise MetricError(f"unknown metric family {family!r}")
+    return result

code/config/config.yaml

@@ -0,0 +1,110 @@
+# Paths are resolved relative to the code/ directory by legacy entry points.
+master_urdf: "config/master_7dof.urdf"
+slave_urdf: "config/real_slave_7dof.urdf"
+
+m_base_frame: master_base
+m_shoulder_frame: master_shoulder
+m_elbow_frame: master_forearm
+m_wrist_frame: master_wrist
+m_ee_frame: master_ee
+s_base_frame: PELVIS_S
+s_shoulder_frame: R_SHOULDER_R_S
+s_elbow_frame: R_ELBOW_R_S
+s_wrist_frame: R_WRIST_R_S
+# The physical URDF has no calibrated TCP/FT frame yet.  R_WRIST_R_S is the
+# chain-end frame; the new simulator adds a documented R_EE_SIM frame at run
+# time.  Replace it with the measured TCP before hardware experiments.
+s_ee_frame: R_WRIST_R_S
+
+master_joint_names: [
+  "master_shoulder_pitch_joint",
+  "master_shoulder_yaw_joint",
+  "master_shoulder_roll_joint",
+  "master_elbow_flex_joint",
+  "master_wrist_roll_joint",
+  "master_wrist_yaw_joint",
+  "master_wrist_pitch_joint",
+]
+
+sew_mapper:
+  slave_joint_names: [
+    "R_SHOULDER_P",
+    "R_SHOULDER_R",
+    "R_SHOULDER_Y",
+    "R_ELBOW_R",
+    "R_WRIST_P",
+    "R_WRIST_Y",
+    "R_WRIST_R",
+  ]
+  shoulder_axis_order: "yxy"
+  shoulder_signs: [-1.0, 1.0, -1.0]
+  wrist_axis_order: "yzx"
+  wrist_signs: [-1.0, 1.0, 1.0]
+  elbow_axis_local: [1.0, 0.0, 0.0]
+  up_dir: [0, 0, 1.0]
+  eps_clip: 0.001
+  fd_eps: 0.0001
+
+interaction_est:
+  lambda_damp: 0.001
+
+haptic_render:
+  feedback_strength: 1.2
+  E_init: 1.0
+  E_max: 8.0
+  alpha_floor: 0.0
+  alpha_ceil: 1.0
+  E0: 1.0
+
+simulation:
+  dt: 0.002
+  duration: 4.0
+  mapping_hz: 50.0
+  seed: 7
+  # Independent forward/return channels.  G0c may override these with a
+  # frozen trace; the defaults reproduce the former fixed-return-delay case.
+  forward_delay_s: 0.0
+  feedback_delay_s: 0.080
+  forward_jitter_s: 0.0
+  return_jitter_s: 0.0
+  forward_packet_loss: 0.0
+  return_packet_loss: 0.0
+  forward_timeout_s: 0.20
+  return_timeout_s: 0.20
+  # A small smooth 3-cycle contact probe exposes delayed-channel activity
+  # without causing joint, torque, or position-limit events.
+  contact_probe_fraction: 0.03
+  contact_probe_cycles: 3.0
+  slave_tcp_frame: R_EE_SIM
+  slave_tcp_offset: [-0.01212, -0.17655, 0.07506]
+  wall_fraction: 0.55
+  wall_stiffness: 800.0
+  wall_damping: 45.0
+  wall_force_limit: 80.0
+  wall_transition_depth: 0.0001
+  master_kp: [196.0, 196.0, 144.0, 256.0, 100.0, 100.0, 81.0]
+  master_kd: [28.0, 28.0, 24.0, 32.0, 20.0, 20.0, 18.0]
+  slave_kp: [900.0, 900.0, 676.0, 1156.0, 400.0, 324.0, 324.0]
+  slave_kd: [54.0, 54.0, 46.8, 61.2, 36.0, 32.4, 32.4]
+  master_acceleration_limits: [100.0, 100.0, 120.0, 120.0, 160.0, 160.0, 160.0]
+  slave_acceleration_limits: [120.0, 120.0, 150.0, 150.0, 180.0, 180.0, 180.0]
+  master_tracking_effort_fraction: 0.65
+  slave_tracking_effort_fraction: 0.70
+  velocity_limit_fraction: 0.80
+  soft_limit_buffer: 0.12
+  feedback_strength: 0.50
+  haptic_filter_alpha: 0.222
+  haptic_torque_limits: [6.0, 6.0, 4.0, 3.0, 1.0, 1.0, 1.0]
+  haptic_rate_limits: [150.0, 150.0, 100.0, 80.0, 30.0, 30.0, 30.0]
+  energy_min: 0.050
+  energy_max: 0.055
+  energy_initial: 0.050
+  sensor_noise_std: 0.001
+  # G0c numerical scaling only.  Replace with a calibration-selected value
+  # before locked hardware experiments.
+  wrench_characteristic_length_m: 0.30
+  wrench_scaled_damping: 0.001
+  sensor_bias: [0.080, -0.050, 0.035, -0.025, 0.015, -0.010, 0.020]
+  bias_calibration_samples: 200
+  joint_limit_margin: 0.00001
+  differential_step: 0.0001

code/config/dual_arm.mjcf

@@ -0,0 +1,95 @@
+<mujoco model="dual_arm">
+  <compiler angle="radian"/>
+
+  <asset>
+    <mesh name="PELVIS_S" content_type="model/stl" file="meshes/PELVIS_S.STL"/>
+    <mesh name="L_SHOULDER_P_S" content_type="model/stl" file="meshes/L_SHOULDER_P_S.STL"/>
+    <mesh name="L_SHOULDER_R_S" content_type="model/stl" file="meshes/L_SHOULDER_R_S.STL"/>
+    <mesh name="L_SHOULDER_Y_S" content_type="model/stl" file="meshes/L_SHOULDER_Y_S.STL"/>
+    <mesh name="L_ELBOW_R_S" content_type="model/stl" file="meshes/L_ELBOW_R_S.STL"/>
+    <mesh name="L_WRIST_P_S" content_type="model/stl" file="meshes/L_WRIST_P_S.STL"/>
+    <mesh name="L_WRIST_Y_S" content_type="model/stl" file="meshes/L_WRIST_Y_S.STL"/>
+    <mesh name="L_WRIST_R_S" content_type="model/stl" file="meshes/L_WRIST_R_S.STL"/>
+    <mesh name="R_SHOULDER_P_S" content_type="model/stl" file="meshes/R_SHOULDER_P_S.STL"/>
+    <mesh name="R_SHOULDER_R_S" content_type="model/stl" file="meshes/R_SHOULDER_R_S.STL"/>
+    <mesh name="R_SHOULDER_Y_S" content_type="model/stl" file="meshes/R_SHOULDER_Y_S.STL"/>
+    <mesh name="R_ELBOW_R_S" content_type="model/stl" file="meshes/R_ELBOW_R_S.STL"/>
+    <mesh name="R_WRIST_P_S" content_type="model/stl" file="meshes/R_WRIST_P_S.STL"/>
+    <mesh name="R_WRIST_Y_S" content_type="model/stl" file="meshes/R_WRIST_Y_S.STL"/>
+    <mesh name="R_WRIST_R_S" content_type="model/stl" file="meshes/R_WRIST_R_S.STL"/>
+  </asset>
+
+  <worldbody>
+    <geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
+    <body name="L_SHOULDER_P_S" pos="0 0.0945 0.042">
+      <inertial pos="-0.00982259 0.0704593 1.15262e-06" quat="0.706163 0.705933 -0.0386962 -0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
+      <joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0" range="-1.57 1.57" actuatorfrcrange="-120 120"/>
+      <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
+      <body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
+        <inertial pos="-0.0346025 0.0917393 -1.67085e-08" quat="0.358778 0.609303 -0.358823 0.609323" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
+        <joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-2 2" actuatorfrcrange="-120 120"/>
+        <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
+        <body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
+          <inertial pos="-0.00440977 0.086362 9.50749e-09" quat="0.705001 0.704998 -0.054559 -0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
+          <joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0" range="-2.18 0" actuatorfrcrange="-80 80"/>
+          <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
+          <body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
+            <inertial pos="-0.0335624 0.06032 2.99656e-07" quat="0.674756 0.674714 0.211333 0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
+            <joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0" range="-2.05 0" actuatorfrcrange="-50 50"/>
+            <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
+            <body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
+              <inertial pos="-1.39659e-10 0.0675973 0.0192006" quat="0.467537 0.530481 -0.530481 0.467537" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
+              <joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0" range="0 3.14" actuatorfrcrange="-50 50"/>
+              <geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
+              <body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
+                <inertial pos="-0.00464136 -5.06427e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
+                <joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-0.78 0.78" actuatorfrcrange="-50 50"/>
+                <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
+                <body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
+                  <inertial pos="-0.0161477 0.0955047 -0.00499392" quat="0.595488 0.369507 -0.591676 0.39847" mass="0.504894" diaginertia="0.000280921 0.000188575 0.000178055"/>
+                  <joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0" range="-1.57 0.26" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+    <body name="R_SHOULDER_P_S" pos="0 -0.0945 0.042">
+      <inertial pos="-0.00982259 -0.0704593 -1.1507e-06" quat="0.706163 0.705933 0.0386962 0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
+      <joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0" range="-1.57 1.57" actuatorfrcrange="-120 120"/>
+      <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
+      <body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
+        <inertial pos="-0.0346025 -0.0917393 1.86281e-08" quat="0.609323 0.358823 -0.609303 0.358778" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
+        <joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-2 2" actuatorfrcrange="-120 120"/>
+        <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
+        <body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
+          <inertial pos="-0.00440977 -0.086362 -7.58792e-09" quat="0.705001 0.704998 0.054559 0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
+          <joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0" range="0 3.14" actuatorfrcrange="-80 80"/>
+          <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
+          <body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
+            <inertial pos="-0.0335624 -0.06032 -2.97736e-07" quat="0.674756 0.674714 -0.211333 -0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
+            <joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0" range="0 2.05" actuatorfrcrange="-80 80"/>
+            <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
+            <body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
+              <inertial pos="-1.39657e-10 -0.0675973 0.0192006" quat="0.530481 0.467537 -0.467537 0.530481" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
+              <joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0" range="-3.14 0" actuatorfrcrange="-50 50"/>
+              <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
+              <body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
+                <inertial pos="-0.00464136 -5.06426e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
+                <joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-0.78 0.78" actuatorfrcrange="-50 50"/>
+                <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
+                <body name="R_WRIST_R_S" pos="0.03 0 -0.039">
+                  <inertial pos="-0.0201642 -0.11075 -0.00598955" quat="0.483404 0.529786 -0.463203 0.520663" mass="0.504366" diaginertia="0.00027189 0.000186086 0.000130629"/>
+                  <joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0" range="-0.26 1.57" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+</mujoco>

code/config/dual_arm.urdf

@@ -0,0 +1,553 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="dual_arm">
+  <link name="PELVIS_S">
+    <inertial>
+      <origin xyz="3.78529087037144E-05 3.81781425684836E-07 0.0386396273530852" rpy="0 0 0" />
+      <mass value="2.10624590271277" />
+      <inertia
+        ixx="0.00165706865324979"
+        ixy="3.13663044821662E-09"
+        ixz="6.84209533216046E-07"
+        iyy="0.00136736758630241"
+        iyz="-1.01014607878816E-10"
+        izz="0.00168295663089359" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="L_SHOULDER_P_S">
+    <inertial>
+      <origin xyz="-0.00982258725282134 0.0704593083751867 1.15261874861217E-06" rpy="0 0 0" />
+      <mass value="0.880737519698403" />
+      <inertia
+        ixx="0.000583190308378148"
+        ixy="-1.53153074560473E-05"
+        ixz="6.58466471754072E-09"
+        iyy="0.000445532440067177"
+        iyz="6.37001404038516E-09"
+        izz="0.000465648071069952" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_SHOULDER_P" type="revolute">
+    <origin xyz="0 0.0945 0.042" rpy="0 0 0" />
+    <parent link="PELVIS_S" />
+    <child link="L_SHOULDER_P_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="L_SHOULDER_R_S">
+    <inertial>
+      <origin xyz="-0.0346025282975857 0.091739327988169 -1.67085072999562E-08" rpy="0 0 0" />
+      <mass value="0.594788424442483" />
+      <inertia
+        ixx="0.000380970396712656"
+        ixy="4.80751844573946E-05"
+        ixz="-1.34913746586865E-11"
+        iyy="0.000320962178597474"
+        iyz="-1.00039763442409E-09"
+        izz="0.000414771047901155" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_SHOULDER_R" type="revolute">
+    <origin xyz="0.035 0.0765 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_P_S" />
+    <child link="L_SHOULDER_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2" upper="2" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="L_SHOULDER_Y_S">
+    <inertial>
+      <origin xyz="-0.00440976862014801 0.0863620459174068 9.50748668682166E-09"
+        rpy="0 0 0" />
+      <mass value="0.563406026626801" />
+      <inertia
+        ixx="0.000327003834287552"
+        ixy="-1.8057438966771E-05"
+        ixz="7.28778136267901E-10"
+        iyy="0.000213830709361531"
+        iyz="1.49273668517817E-10"
+        izz="0.000297341029639189" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_SHOULDER_Y" type="revolute">
+    <origin xyz="-0.035 0.1475 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_R_S" />
+    <child link="L_SHOULDER_Y_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="-2.18" upper="0" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="L_ELBOW_R_S">
+    <inertial>
+      <origin xyz="-0.0335624237303443 0.0603199964106564 2.99655911639718E-07" rpy="0 0 0" />
+      <mass value="0.393571904406493" />
+      <inertia
+        ixx="0.00017277119386253"
+        ixy="2.34549801867355E-05"
+        ixz="-1.90560556659271E-09"
+        iyy="0.000155340245267897"
+        iyz="8.82493073600007E-09"
+        izz="0.00018104232734159" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_ELBOW_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_ELBOW_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_ELBOW_R" type="revolute">
+    <origin xyz="0.034 0.1025 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_Y_S" />
+    <child link="L_ELBOW_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2.05" upper="0" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="L_WRIST_P_S">
+    <inertial>
+      <origin xyz="-1.39659173115092E-10 0.0675972604744393 0.019200551565574" rpy="0 0 0" />
+      <mass value="0.442332465815496" />
+      <inertia
+        ixx="0.000476754055455895"
+        ixy="-4.61505824713347E-15"
+        ixz="6.03808112272229E-18"
+        iyy="0.000103466585850499"
+        iyz="-4.88426705501198E-05"
+        izz="0.000482956345754213" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_WRIST_P" type="revolute">
+    <origin xyz="-0.034 0.0965 0" rpy="0 0 0" />
+    <parent link="L_ELBOW_R_S" />
+    <child link="L_WRIST_P_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="0" upper="3.14" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="L_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887331864 -5.06426789392833E-10 -0.0341253783666609" rpy="0 0 0" />
+      <mass value="0.23573847772002" />
+      <inertia
+        ixx="5.10686940455785E-05"
+        ixy="7.45706654358429E-16"
+        ixz="6.2619568923368E-06"
+        iyy="6.00636019624519E-05"
+        iyz="1.27989654155383E-16"
+        izz="5.32182903609189E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_WRIST_Y" type="revolute">
+    <origin xyz="0 0.1525 0.039" rpy="0 0 0" />
+    <parent link="L_WRIST_P_S" />
+    <child link="L_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
+  </joint>
+
+  <link name="L_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0161477357620243 0.0955046558857351 -0.00499392489444117" rpy="0 0 0" />
+      <mass value="0.504894112043562" />
+      <inertia
+        ixx="0.000186833711781125"
+        ixy="-3.99488705622731E-06"
+        ixz="-1.51920720398336E-06"
+        iyy="0.000179960980467837"
+        iyz="3.6992669535716E-06"
+        izz="0.000280756101568308" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_WRIST_R" type="revolute">
+    <origin xyz="0.0258 0 -0.039" rpy="0 0 0" />
+    <parent link="L_WRIST_Y_S" />
+    <child link="L_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-1.57" upper="0.26" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_SHOULDER_P_S">
+    <inertial>
+      <origin xyz="-0.00982258725282141 -0.0704593093873431 -1.15069920925137E-06" rpy="0 0 0" />
+      <mass value="0.880737519698404" />
+      <inertia
+        ixx="0.000583190308378149"
+        ixy="1.53153074560471E-05"
+        ixz="-6.58466471736542E-09"
+        iyy="0.000445532440067178"
+        iyz="6.37001403998779E-09"
+        izz="0.000465648071069953" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_P" type="revolute">
+    <origin xyz="0 -0.0945 0.042" rpy="0 0 0" />
+    <parent link="PELVIS_S" />
+    <child link="R_SHOULDER_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="R_SHOULDER_R_S">
+    <inertial>
+      <origin xyz="-0.0346025282975784 -0.09173932900033 1.86280643132974E-08" rpy="0 0 0" />
+      <mass value="0.59478842444248" />
+      <inertia
+        ixx="0.000380970396712653"
+        ixy="-4.80751844573946E-05"
+        ixz="1.34913746041655E-11"
+        iyy="0.000320962178597472"
+        iyz="-1.00039763417375E-09"
+        izz="0.000414771047901152" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_R" type="revolute">
+    <origin xyz="0.035 -0.0765 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_P_S" />
+    <child link="R_SHOULDER_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2" upper="2" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="R_SHOULDER_Y_S">
+    <inertial>
+      <origin xyz="-0.00440976862014946 -0.0863620469295699 -7.58791862676134E-09" rpy="0 0 0" />
+      <mass value="0.563406026626801" />
+      <inertia
+        ixx="0.000327003834287551"
+        ixy="1.80574389667711E-05"
+        ixz="-7.28778136077729E-10"
+        iyy="0.000213830709361532"
+        iyz="1.49273668428957E-10"
+        izz="0.000297341029639189" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+      <material
+        name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_Y" type="revolute">
+    <origin xyz="-0.035 -0.1475 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_R_S" />
+    <child link="R_SHOULDER_Y_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="0" upper="3.14" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="R_ELBOW_R_S">
+    <inertial>
+      <origin xyz="-0.0335624237303424 -0.0603199974228191 -2.97736340082455E-07" rpy="0 0 0" />
+      <mass value="0.393571904406492" />
+      <inertia
+        ixx="0.000172771193862529"
+        ixy="-2.34549801867353E-05"
+        ixz="1.90560556668771E-09"
+        iyy="0.000155340245267897"
+        iyz="8.82493073602971E-09"
+        izz="0.00018104232734159" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_ELBOW_R" type="revolute">
+    <origin xyz="0.034 -0.1025 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_Y_S" />
+    <child link="R_ELBOW_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="0" upper="2.05" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="R_WRIST_P_S">
+    <inertial>
+      <origin xyz="-1.39656577968772E-10 -0.0675972614865965 0.0192005515655728" rpy="0 0 0" />
+      <mass value="0.442332465815497" />
+      <inertia
+        ixx="0.000476754055455896"
+        ixy="-4.61462558956188E-15"
+        ixz="-5.91762926004267E-18"
+        iyy="0.0001034665858505"
+        iyz="4.88426705501212E-05"
+        izz="0.000482956345754214" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_P" type="revolute">
+    <origin xyz="-0.034 -0.0965 0" rpy="0 0 0" />
+    <parent link="R_ELBOW_R_S" />
+    <child link="R_WRIST_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-3.14" upper="0" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887330083 -5.06426456325926E-10 -0.0341253783666614" rpy="0 0 0" />
+      <mass value="0.235738477720019" />
+      <inertia
+        ixx="5.1068694045578E-05"
+        ixy="7.45771487459288E-16"
+        ixz="6.26195689233664E-06"
+        iyy="6.00636019624515E-05"
+        iyz="1.27828046342987E-16"
+        izz="5.32182903609188E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_Y" type="revolute">
+    <origin xyz="0 -0.1525 0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_P_S" />
+    <child link="R_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
+  </joint>
+
+  <link name="R_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0201642233698132 -0.11074968386657 -0.00598955339232021" rpy="0 0 0" />
+      <mass value="0.504366058218534" />
+      <inertia
+        ixx="0.000185559065855467"
+        ixy="5.75889766751509E-06"
+        ixz="2.40683898454438E-06"
+        iyy="0.000131246007872084"
+        iyz="1.60533277040148E-06"
+        izz="0.000271800951396149" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+      <material
+        name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_R" type="revolute">
+    <origin xyz="0.03 0 -0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_Y_S" />
+    <child link="R_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.26" upper="1.57" effort="50" velocity="4.71" />
+  </joint>
+
+</robot>

code/config/experiments/bilateral_calibration.json

@@ -0,0 +1,50 @@
+{
+  "study_id": "g0c_bilateral_calibration_v1",
+  "split": "calibration",
+  "root_seed": 2026072703,
+  "replicates": 2,
+  "methods": [
+    "proposed_energy",
+    "proposed_no_energy",
+    "direct_energy",
+    "matched_wrench_energy",
+    "proposed_popc"
+  ],
+  "trajectories": [
+    {
+      "id": "free_space_roundtrip",
+      "family": "free_space",
+      "duration_s": 1.2,
+      "contact_probe_fraction": 0.0
+    },
+    {
+      "id": "contact_roundtrip",
+      "family": "contact_roundtrip",
+      "duration_s": 1.2,
+      "contact_probe_fraction": 0.02
+    }
+  ],
+  "factors": {
+    "map_policy": [
+      "current",
+      "source_stamped"
+    ],
+    "return_delay_s": [
+      0.0,
+      0.08
+    ],
+    "return_jitter_s": [
+      0.0
+    ],
+    "return_packet_loss": [
+      0.0
+    ],
+    "wall_damping": [
+      30.0
+    ],
+    "wall_stiffness": [
+      400.0,
+      800.0
+    ]
+  }
+}

code/config/experiments/bilateral_locked_template.json

@@ -0,0 +1,55 @@
+{
+  "study_id": "g0c_bilateral_locked_template_v1",
+  "split": "locked",
+  "root_seed": 2026072713,
+  "replicates": 20,
+  "methods": [
+    "proposed_energy",
+    "proposed_no_energy",
+    "direct_energy",
+    "matched_wrench_energy",
+    "proposed_popc"
+  ],
+  "trajectories": [
+    {
+      "id": "free_space_roundtrip_locked",
+      "family": "free_space",
+      "duration_s": 4.0,
+      "contact_probe_fraction": 0.0
+    },
+    {
+      "id": "contact_roundtrip_locked",
+      "family": "contact_roundtrip",
+      "duration_s": 4.0,
+      "contact_probe_fraction": 0.03
+    }
+  ],
+  "factors": {
+    "map_policy": [
+      "current",
+      "source_stamped"
+    ],
+    "return_delay_s": [
+      0.0,
+      0.04,
+      0.08,
+      0.12
+    ],
+    "return_jitter_s": [
+      0.0,
+      0.004
+    ],
+    "return_packet_loss": [
+      0.0,
+      0.02
+    ],
+    "wall_damping": [
+      30.0
+    ],
+    "wall_stiffness": [
+      300.0,
+      600.0,
+      900.0
+    ]
+  }
+}

code/config/experiments/g0c_calibration_example.json

@@ -0,0 +1,51 @@
+{
+  "study_id": "g0c_bilateral_calibration_example",
+  "split": "calibration",
+  "root_seed": 20260727,
+  "replicates": 2,
+  "methods": [
+    "proposed_energy",
+    "direct_energy",
+    "proposed_no_energy",
+    "matched_wrench_energy",
+    "popc"
+  ],
+  "trajectories": [
+    {
+      "trajectory_id": "free_space_001",
+      "family": "free_space_workspace_sweep"
+    },
+    {
+      "trajectory_id": "contact_001",
+      "family": "approach_contact_return"
+    },
+    {
+      "trajectory_id": "multi_axis_001",
+      "family": "multi_axis_contact_probe"
+    },
+    {
+      "trajectory_id": "boundary_001",
+      "family": "limit_singularity_degeneracy"
+    }
+  ],
+  "factors": {
+    "delay_ms": [0, 40, 80, 120],
+    "environment": [
+      {
+        "stiffness_N_per_m": 200,
+        "damping_Ns_per_m": 15
+      },
+      {
+        "stiffness_N_per_m": 500,
+        "damping_Ns_per_m": 30
+      },
+      {
+        "stiffness_N_per_m": 800,
+        "damping_Ns_per_m": 45
+      }
+    ]
+  },
+  "metadata": {
+    "status": "example only; do not treat as a locked protocol"
+  }
+}

code/config/experiments/h1_calibration.json

@@ -0,0 +1,34 @@
+{
+  "study_id": "g0c_sew_calibration_v1",
+  "split": "calibration",
+  "root_seed": 2026072701,
+  "replicates": 2,
+  "methods": [
+    "sew",
+    "scaled_joint_space",
+    "bounded_dls_ik",
+    "task_priority_ik"
+  ],
+  "trajectories": [
+    {
+      "id": "nominal_sweep",
+      "family": "nominal",
+      "sample_count": 81
+    },
+    {
+      "id": "reach_boundary",
+      "family": "reach_boundary",
+      "sample_count": 81
+    },
+    {
+      "id": "joint_limit_neighborhood",
+      "family": "joint_limit",
+      "sample_count": 81
+    },
+    {
+      "id": "low_manipulability",
+      "family": "low_manipulability",
+      "sample_count": 81
+    }
+  ]
+}

code/config/experiments/h1_locked_template.json

@@ -0,0 +1,34 @@
+{
+  "study_id": "g0c_sew_locked_template_v1",
+  "split": "locked",
+  "root_seed": 2026072711,
+  "replicates": 20,
+  "methods": [
+    "sew",
+    "scaled_joint_space",
+    "bounded_dls_ik",
+    "task_priority_ik"
+  ],
+  "trajectories": [
+    {
+      "id": "nominal_sweep_locked",
+      "family": "nominal",
+      "sample_count": 161
+    },
+    {
+      "id": "reach_boundary_locked",
+      "family": "reach_boundary",
+      "sample_count": 161
+    },
+    {
+      "id": "joint_limit_neighborhood_locked",
+      "family": "joint_limit",
+      "sample_count": 161
+    },
+    {
+      "id": "low_manipulability_locked",
+      "family": "low_manipulability",
+      "sample_count": 161
+    }
+  ]
+}

code/config/experiments/h1_smoke.json

@@ -0,0 +1,19 @@
+{
+  "study_id": "g0c_sew_smoke_v1",
+  "split": "pilot",
+  "root_seed": 2026072797,
+  "replicates": 1,
+  "methods": [
+    "sew",
+    "scaled_joint_space",
+    "bounded_dls_ik",
+    "task_priority_ik"
+  ],
+  "trajectories": [
+    {
+      "id": "short_nominal",
+      "family": "nominal",
+      "sample_count": 9
+    }
+  ]
+}

code/config/experiments/h2_calibration.json

@@ -0,0 +1,40 @@
+{
+  "study_id": "g0c_estimator_sensitivity_calibration_v1",
+  "split": "calibration",
+  "root_seed": 2026072702,
+  "replicates": 2,
+  "methods": [
+    "scaled_dls",
+    "undamped_svd",
+    "no_bias",
+    "no_friction"
+  ],
+  "trajectories": [
+    {
+      "id": "dynamic_wrench_sweep",
+      "family": "synthetic_dynamic",
+      "sample_count": 256
+    }
+  ],
+  "factors": {
+    "characteristic_length_m": [
+      0.3
+    ],
+    "damping": [
+      0.01,
+      0.03
+    ],
+    "min_scaled_singular": [
+      0.02,
+      0.1
+    ],
+    "model_error_std": [
+      0.0,
+      0.02
+    ],
+    "torque_noise_std_Nm": [
+      0.002,
+      0.02
+    ]
+  }
+}

code/config/experiments/h2_locked_template.json

@@ -0,0 +1,41 @@
+{
+  "study_id": "g0c_estimator_sensitivity_locked_template_v1",
+  "split": "locked",
+  "root_seed": 2026072712,
+  "replicates": 20,
+  "methods": [
+    "scaled_dls",
+    "undamped_svd",
+    "no_bias",
+    "no_friction"
+  ],
+  "trajectories": [
+    {
+      "id": "dynamic_wrench_sweep_locked",
+      "family": "synthetic_dynamic",
+      "sample_count": 512
+    }
+  ],
+  "factors": {
+    "characteristic_length_m": [
+      0.3
+    ],
+    "damping": [
+      0.01,
+      0.03
+    ],
+    "min_scaled_singular": [
+      0.01,
+      0.05,
+      0.2
+    ],
+    "model_error_std": [
+      0.0,
+      0.02
+    ],
+    "torque_noise_std_Nm": [
+      0.002,
+      0.02
+    ]
+  }
+}

code/config/experiments/h2_smoke.json

@@ -0,0 +1,36 @@
+{
+  "study_id": "g0c_estimator_smoke_v1",
+  "split": "pilot",
+  "root_seed": 2026072798,
+  "replicates": 1,
+  "methods": [
+    "scaled_dls",
+    "undamped_svd",
+    "no_bias",
+    "no_friction"
+  ],
+  "trajectories": [
+    {
+      "id": "short_wrench",
+      "family": "synthetic_dynamic",
+      "sample_count": 16
+    }
+  ],
+  "factors": {
+    "characteristic_length_m": [
+      0.3
+    ],
+    "damping": [
+      0.02
+    ],
+    "min_scaled_singular": [
+      0.05
+    ],
+    "model_error_std": [
+      0.01
+    ],
+    "torque_noise_std_Nm": [
+      0.01
+    ]
+  }
+}

code/config/experiments/metrics_bilateral.json

@@ -0,0 +1,22 @@
+{
+  "enabled": [
+    "h3",
+    "h4"
+  ],
+  "h3": {
+    "force_scale": 1.0,
+    "epsilon_energy_J": 1e-12
+  },
+  "h4": {
+    "include_methods": [
+      "proposed_energy",
+      "direct_energy",
+      "matched_wrench_energy"
+    ],
+    "energy_min_J": 0.05,
+    "energy_max_J": 0.055,
+    "epsilon_torque_impulse_Nms": 1e-12,
+    "audit_tolerance_J": 1e-10
+  },
+  "status": "H3 includes all bilateral methods; H4 includes tank-supervised methods only"
+}

code/config/experiments/metrics_h1_calibration.json

@@ -0,0 +1,15 @@
+{
+  "enabled": [
+    "h1"
+  ],
+  "h1": {
+    "thresholds": {
+      "position_threshold_m": 0.005,
+      "orientation_threshold_rad": 0.05,
+      "joint_step_threshold_rad": 0.25,
+      "swivel_step_threshold_rad": 0.25,
+      "input_step_threshold_rad": 0.05
+    }
+  },
+  "status": "calibration-only thresholds; freeze a new hash before locked runs"
+}

code/config/experiments/metrics_h2.json

@@ -0,0 +1,6 @@
+{
+  "enabled": [
+    "h2"
+  ],
+  "h2": {}
+}

code/config/experiments/metrics_h3.json

@@ -0,0 +1,9 @@
+{
+  "enabled": [
+    "h3"
+  ],
+  "h3": {
+    "force_scale": 1.0,
+    "epsilon_energy_J": 1e-12
+  }
+}

code/config/experiments/metrics_h3_h4_example.json

@@ -0,0 +1,30 @@
+{
+  "enabled": ["h3", "h4"],
+  "h3": {
+    "force_scale": 1.0,
+    "epsilon_energy_J": 1e-12,
+    "fields": {
+      "tau_master_raw": "tau_master_raw",
+      "qd_master": "qd_master",
+      "tau_slave_source": "tau_slave_source",
+      "qd_slave_source": "qd_slave_source",
+      "dt": "dt",
+      "return_valid": "return_valid"
+    }
+  },
+  "h4": {
+    "energy_min_J": 0.05,
+    "energy_max_J": 0.055,
+    "audit_tolerance_J": 1e-10,
+    "epsilon_torque_impulse_Nms": 1e-12,
+    "fields": {
+      "energy_before_J": "energy_before_J",
+      "energy_after_J": "energy_after_J",
+      "software_preclip_J": "energy_preclip_J",
+      "tau_candidate": "tau_master_candidate",
+      "tau_applied": "tau_master_applied",
+      "qd_master": "qd_master",
+      "dt": "dt"
+    }
+  }
+}

code/config/experiments/metrics_h4_tank.json

@@ -0,0 +1,17 @@
+{
+  "enabled": [
+    "h4"
+  ],
+  "h4": {
+    "include_methods": [
+      "proposed_energy",
+      "direct_energy",
+      "matched_wrench_energy"
+    ],
+    "energy_min_J": 0.05,
+    "energy_max_J": 0.055,
+    "epsilon_torque_impulse_Nms": 1e-12,
+    "audit_tolerance_J": 1e-10
+  },
+  "status": "independent final-applied-port audit for tank-supervised trials"
+}

code/config/experiments/smoke.json

@@ -0,0 +1,34 @@
+{
+  "study_id": "g0c_bilateral_smoke_v1",
+  "split": "pilot",
+  "root_seed": 2026072799,
+  "replicates": 1,
+  "methods": [
+    "proposed_energy",
+    "direct_energy",
+    "matched_wrench_energy",
+    "proposed_popc"
+  ],
+  "trajectories": [
+    {
+      "id": "short_contact",
+      "family": "contact_roundtrip",
+      "duration_s": 0.6,
+      "contact_probe_fraction": 0.0
+    }
+  ],
+  "factors": {
+    "map_policy": [
+      "source_stamped"
+    ],
+    "return_delay_s": [
+      0.02
+    ],
+    "wall_damping": [
+      45.0
+    ],
+    "wall_stiffness": [
+      800.0
+    ]
+  }
+}

code/config/hardware.yaml

@@ -0,0 +1,17 @@
+bitrate: 1000000
+hz: 500
+
+canbus:
+  - dev_idx: 0
+    channel: 
+      - ch_idx: 0
+        mode: broadcast
+      - ch_idx: 1
+        mode: singular
+  - dev_idx: 1
+    channel: 
+      - ch_idx: 0
+        mode: broadcast
+      - ch_idx: 1
+        mode: broadcast
+

code/config/master_7dof.mjcf

@@ -0,0 +1,84 @@
+<mujoco model="master_7dof">
+  <compiler angle="radian" coordinate="local"/>
+  <option timestep="0.001" gravity="0 0 -9.81"/>
+
+  <asset>
+    <texture type="skybox" builtin="gradient"
+             rgb1="1 1 1" rgb2=".6 .8 1"
+             width="256" height="256"/>
+  </asset>
+
+  <default>
+    <joint type="hinge" limited="true" damping="0.2"/>
+    <geom contype="0" conaffinity="0" density="1000"/>
+  </default>
+
+  <worldbody>
+    <body name="master_base" pos="0 0 0">
+      <inertial pos="0 0 0" mass="1.0" diaginertia="1e-2 1e-2 1e-2"/>
+
+      <body name="master_shoulder" pos="0 0 0">
+        <inertial pos="0 0 0" mass="0.1" diaginertia="1e-4 1e-4 1e-4"/>
+
+        <body name="master_link_sp" pos="0 0 0">
+          <inertial pos="0 0 0" mass="0.1" diaginertia="1e-4 1e-4 1e-4"/>
+          <joint name="master_shoulder_pitch_joint" axis="0 1 0" range="-3.1416 3.1416"/>
+
+          <body name="master_link_sy" pos="0 0 0">
+            <inertial pos="0 0 0" mass="0.1" diaginertia="1e-4 1e-4 1e-4"/>
+            <joint name="master_shoulder_yaw_joint" axis="0 0 1" range="-3.1416 3.1416"/>
+
+            <body name="master_upper_arm" pos="0 0 0">
+              <inertial pos="0 0 0" mass="1.0" diaginertia="0.01 0.01 0.01"/>
+              <joint name="master_shoulder_roll_joint" axis="1 0 0" range="-3.1416 3.1416"/>
+
+              <geom name="geom_upper_arm" type="capsule" size="0.02"
+                    fromto="0 0 0  0.280 0 0"/>
+
+              <body name="master_forearm" pos="0.280 0 0">
+                <inertial pos="0 0 0" mass="0.8" diaginertia="0.008 0.008 0.008"/>
+                <joint name="master_elbow_flex_joint" axis="0 1 0" range="-3.1416 3.1416"/>
+
+                <geom name="geom_forearm" type="capsule" size="0.018"
+                      fromto="0 0 0  0.300 0 0"/>
+
+                <body name="master_wrist" pos="0.300 0 0">
+                  <inertial pos="0 0 0" mass="0.1" diaginertia="1e-4 1e-4 1e-4"/>
+
+                  <body name="master_link_wr" pos="0 0 0">
+                    <inertial pos="0 0 0" mass="0.1" diaginertia="1e-4 1e-4 1e-4"/>
+                    <joint name="master_wrist_roll_joint" axis="1 0 0" range="-3.1416 3.1416"/>
+
+                    <body name="master_link_wy" pos="0 0 0">
+                      <inertial pos="0 0 0" mass="0.1" diaginertia="1e-4 1e-4 1e-4"/>
+                      <joint name="master_wrist_yaw_joint" axis="0 1 0" range="-3.1416 3.1416"/>
+
+                      <body name="master_ee" pos="0 0 0">
+                        <inertial pos="0 0 0" mass="0.2" diaginertia="0.001 0.001 0.001"/>
+                        <joint name="master_wrist_pitch_joint" axis="0 0 1" range="-3.1416 3.1416"/>
+
+                        <geom name="geom_ee" type="box" pos="0.05 0 0"
+                              size="0.05 0.01 0.01"/>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <position name="act_sh_yaw"   joint="master_shoulder_pitch_joint" kp="200" ctrlrange="-3.14 3.14" forcerange="-50 50"/>
+    <position name="act_sh_pitch" joint="master_shoulder_yaw_joint"   kp="200" ctrlrange="-3.14 3.14" forcerange="-50 50"/>
+
+    <position name="act_sh_roll"  joint="master_shoulder_roll_joint"  kp="200" ctrlrange="-3.14 3.14" forcerange="-50 50"/>
+    <position name="act_elbow"    joint="master_elbow_flex_joint"     kp="200" ctrlrange="-3.14 3.14" forcerange="-50 50"/>
+    <position name="act_wr_roll"  joint="master_wrist_roll_joint"     kp="200" ctrlrange="-3.14 3.14" forcerange="-50 50"/>
+    <position name="act_wr_yaw"   joint="master_wrist_yaw_joint"      kp="200" ctrlrange="-3.14 3.14" forcerange="-50 50"/>
+    <position name="act_wr_pitch" joint="master_wrist_pitch_joint"    kp="200" ctrlrange="-3.14 3.14" forcerange="-50 50"/>
+  </actuator>
+</mujoco>

code/config/master_7dof.urdf

@@ -0,0 +1,208 @@
+<?xml version="1.0"?>
+<robot name="master_7dof">
+
+  <!-- Links -->
+  <link name="master_base">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1.0"/>
+      <inertia ixx="1e-2" iyy="1e-2" izz="1e-2" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="master_shoulder">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="master_link_sp">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="master_link_sy">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="master_upper_arm">
+    <visual>
+      <!-- cylinder aligned to X axis -->
+      <origin xyz="0 -0.140 0" rpy="1.57079632679 0 0"/>
+      <geometry>
+        <cylinder length="0.280" radius="0.02"/>
+      </geometry>
+      <material name="upper_arm_mat">
+        <color rgba="0.6 0.6 0.6 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.140 0" rpy="1.57079632679 0 0"/>
+      <geometry>
+        <cylinder length="0.280" radius="0.02"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 -0.140 0" rpy="1.57079632679 0 0"/>
+      <mass value="1.0"/>
+      <inertia ixx="0.01" iyy="0.01" izz="0.01" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="master_forearm">
+    <visual>
+      <origin xyz="0 -0.150 0" rpy="1.57079632679 0 0"/>
+      <geometry>
+        <cylinder length="0.300" radius="0.018"/>
+      </geometry>
+      <material name="forearm_mat">
+        <color rgba="0.7 0.7 0.9 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.150 0" rpy="1.57079632679 0 0"/>
+      <geometry>
+        <cylinder length="0.300" radius="0.018"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 -0.150 0" rpy="1.57079632679 0 0"/>
+      <mass value="0.8"/>
+      <inertia ixx="0.008" iyy="0.008" izz="0.008" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="master_wrist">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="master_link_wr">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="master_link_wy">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="master_ee">
+    <visual>
+      <origin xyz="0 -0.05 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.02 0.10 0.02"/>
+      </geometry>
+      <material name="ee_mat">
+        <color rgba="0.8 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.05 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.02 0.10 0.02"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 -0.05 0" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+
+  <!-- Joints -->
+
+  <!-- ✅ 关键：把整条手臂在 master_base 坐标系里转到 -Y 方向
+       +X -> -Y 等价于绕 Z 轴 -90° -->
+  <joint name="master_base_to_shoulder" type="fixed">
+    <parent link="master_base"/>
+    <child link="master_shoulder"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <!-- Shoulder order: pitch(Y) -> yaw(Z) -> roll(X) -->
+  <joint name="master_shoulder_pitch_joint" type="revolute">
+    <parent link="master_shoulder"/>
+    <child link="master_link_sp"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-3.1416" upper="3.1416" effort="50" velocity="4.0"/>
+  </joint>
+
+  <joint name="master_shoulder_yaw_joint" type="revolute">
+    <parent link="master_link_sp"/>
+    <child link="master_link_sy"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.1416" upper="3.1416" effort="50" velocity="4.0"/>
+  </joint>
+
+  <joint name="master_shoulder_roll_joint" type="revolute">
+    <parent link="master_link_sy"/>
+    <child link="master_upper_arm"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.1416" upper="3.1416" effort="50" velocity="4.0"/>
+  </joint>
+
+  <!-- Elbow at x = 0.280 -->
+  <joint name="master_elbow_flex_joint" type="revolute">
+    <parent link="master_upper_arm"/>
+    <child link="master_forearm"/>
+    <origin xyz="0 -0.280 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.1416" upper="3.1416" effort="30" velocity="4.0"/>
+  </joint>
+
+  <!-- Wrist fixed at x = 0.300 -->
+  <joint name="master_forearm_to_wrist" type="fixed">
+    <parent link="master_forearm"/>
+    <child link="master_wrist"/>
+    <origin xyz="0 -0.300 0" rpy="0 0 0"/>
+  </joint>
+
+  <!-- Wrist order: roll(X) -> yaw(Y) -> pitch(Z) -->
+  <joint name="master_wrist_roll_joint" type="revolute">
+    <parent link="master_wrist"/>
+    <child link="master_link_wr"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-3.1416" upper="3.1416" effort="20" velocity="4.0"/>
+  </joint>
+
+  <joint name="master_wrist_yaw_joint" type="revolute">
+    <parent link="master_link_wr"/>
+    <child link="master_link_wy"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-3.1416" upper="3.1416" effort="20" velocity="4.0"/>
+  </joint>
+
+  <joint name="master_wrist_pitch_joint" type="revolute">
+    <parent link="master_link_wy"/>
+    <child link="master_ee"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.1416" upper="3.1416" effort="20" velocity="4.0"/>
+  </joint>
+
+</robot>

code/config/meshes/right_arm.mjcf

@@ -0,0 +1,54 @@
+<mujoco model="right_arm">
+  <compiler angle="radian"/>
+
+  <asset>
+    <mesh name="PELVIS_S" content_type="model/stl" file="PELVIS_S.STL"/>
+    <mesh name="R_SHOULDER_P_S" content_type="model/stl" file="R_SHOULDER_P_S.STL"/>
+    <mesh name="R_SHOULDER_R_S" content_type="model/stl" file="R_SHOULDER_R_S.STL"/>
+    <mesh name="R_SHOULDER_Y_S" content_type="model/stl" file="R_SHOULDER_Y_S.STL"/>
+    <mesh name="R_ELBOW_R_S" content_type="model/stl" file="R_ELBOW_R_S.STL"/>
+    <mesh name="R_WRIST_P_S" content_type="model/stl" file="R_WRIST_P_S.STL"/>
+    <mesh name="R_WRIST_Y_S" content_type="model/stl" file="R_WRIST_Y_S.STL"/>
+    <mesh name="R_WRIST_R_S" content_type="model/stl" file="R_WRIST_R_S.STL"/>
+  </asset>
+
+  <worldbody>
+    <geom type="mesh" rgba="0.698 0.698 0.698 1" mesh="PELVIS_S"/>
+    <body name="R_SHOULDER_P_S" pos="0 -0.0945 0.042">
+      <inertial pos="-0.00982259 -0.0704593 -1.1507e-06" quat="0.706163 0.705933 0.0386962 0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
+      <joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0" range="-1.57 1.57" actuatorfrcrange="-120 120"/>
+      <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
+      <body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
+        <inertial pos="-0.0346025 -0.0917393 1.86281e-08" quat="0.609323 0.358823 -0.609303 0.358778" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
+        <joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-2 2" actuatorfrcrange="-120 120"/>
+        <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
+        <body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
+          <inertial pos="-0.00440977 -0.086362 -7.58792e-09" quat="0.705001 0.704998 0.054559 0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
+          <joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0" range="0 3.14" actuatorfrcrange="-80 80"/>
+          <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
+          <body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
+            <inertial pos="-0.0335624 -0.06032 -2.97736e-07" quat="0.674756 0.674714 -0.211333 -0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
+            <joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0" range="0 2.18" actuatorfrcrange="-80 80"/>
+            <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
+            <body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
+              <inertial pos="-1.39657e-10 -0.0675973 0.0192006" quat="0.530481 0.467537 -0.467537 0.530481" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
+              <joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0" range="-3.14 0" actuatorfrcrange="-50 50"/>
+              <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
+              <body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
+                <inertial pos="-0.00464136 -5.06426e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
+                <joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1" range="0 0.78" actuatorfrcrange="-50 50"/>
+                <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
+                <body name="R_WRIST_R_S" pos="0.03 0 -0.039">
+                  <inertial pos="-0.0201642 -0.11075 -0.00598955" quat="0.483404 0.529786 -0.463203 0.520663" mass="0.204366" diaginertia="0.00027189 0.000186086 0.000130629"/>
+                  <joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0" range="-0.26 1.57" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
+                  <geom size="0.005" pos="-0.01212 -0.17655 0.07506" quat="9.38184e-07 0.707105 -0.707108 -9.38187e-07" rgba="0 1 0 1"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+</mujoco>

code/config/meshes/right_arm.urdf

@@ -0,0 +1,288 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<robot name="right_arm">
+  <link name="PELVIS_S">
+    <inertial>
+      <origin xyz="3.78529e-05 3.81781e-07 0.0386396" rpy="0 0 0"/>
+      <mass value="2.10624590271277"/>
+      <inertia 
+        ixx="0.00165706865324979" ixy="3.13663e-09"  ixz="6.84210e-07"
+        iyy="0.00136736758630241" iyz="-1.01015e-10" izz="0.00168295663089359"/>
+    </inertial>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL"/>
+      </geometry>
+      <material name="light_gray">
+        <color rgba="0.698 0.698 0.698 1"/>
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="R_SHOULDER_P_S">
+    <inertial>
+      <origin xyz="-0.00982258725282141 -0.0704593093873431 -1.15069920925137E-06" rpy="0 0 0" />
+      <mass value="0.880737519698404" />
+      <inertia
+        ixx="0.000583190308378149"
+        ixy="1.53153074560471E-05"
+        ixz="-6.58466471736542E-09"
+        iyy="0.000445532440067178"
+        iyz="6.37001403998779E-09"
+        izz="0.000465648071069953" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_P" type="revolute">
+    <origin xyz="0 -0.0945 0.042" rpy="0 0 0" />
+    <parent link="PELVIS_S" />
+    <child link="R_SHOULDER_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="R_SHOULDER_R_S">
+    <inertial>
+      <origin xyz="-0.0346025282975784 -0.09173932900033 1.86280643132974E-08" rpy="0 0 0" />
+      <mass value="0.59478842444248" />
+      <inertia
+        ixx="0.000380970396712653" ixy="-4.80751844573946E-05"  ixz="1.34913746041655E-11"
+        iyy="0.000320962178597472" iyz="-1.00039763417375E-09" izz="0.000414771047901152" />
+    </inertial>
+    <visual>
+      <origin xyz="0.035 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.035 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint name="R_SHOULDER_R" type="revolute">
+    <origin xyz="0 -0.0765 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_P_S" />
+    <child link="R_SHOULDER_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2" upper="2" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="R_SHOULDER_Y_S">
+    <inertial>
+      <origin xyz="-0.00440976862014946 -0.0863620469295699 -7.58791862676134E-09" rpy="0 0 0" />
+      <mass value="0.563406026626801" />
+      <inertia
+        ixx="0.000327003834287551"
+        ixy="1.80574389667711E-05"
+        ixz="-7.28778136077729E-10"
+        iyy="0.000213830709361532"
+        iyz="1.49273668428957E-10"
+        izz="0.000297341029639189" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_Y" type="revolute">
+    <origin xyz="0 -0.1475 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_R_S" />
+    <child link="R_SHOULDER_Y_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="0" upper="3.14" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="R_ELBOW_R_S">
+    <inertial>
+      <origin xyz="-0.0335624237303424 -0.0603199974228191 -2.97736340082455E-07" rpy="0 0 0" />
+      <mass value="0.393571904406492" />
+      <inertia
+        ixx="0.000172771193862529"
+        ixy="-2.34549801867353E-05"
+        ixz="1.90560556668771E-09"
+        iyy="0.000155340245267897"
+        iyz="8.82493073602971E-09"
+        izz="0.00018104232734159" />
+    </inertial>
+    <visual>
+      <origin xyz="0.034 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.034 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_ELBOW_R" type="revolute">
+    <origin xyz="0 -0.1025 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_Y_S" />
+    <child link="R_ELBOW_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="0" upper="2.18" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="R_WRIST_P_S">
+    <inertial>
+      <origin xyz="-1.39656577968772E-10 -0.0675972614865965 0.0192005515655728" rpy="0 0 0" />
+      <mass value="0.442332465815497" />
+      <inertia
+        ixx="0.000476754055455896"
+        ixy="-4.61462558956188E-15"
+        ixz="-5.91762926004267E-18"
+        iyy="0.0001034665858505"
+        iyz="4.88426705501212E-05"
+        izz="0.000482956345754214" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_P" type="revolute">
+    <origin xyz="0 -0.0965 0" rpy="0 0 0" />
+    <parent link="R_ELBOW_R_S" />
+    <child link="R_WRIST_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-3.14" upper="0" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887330083 -5.06426456325926E-10 -0.0341253783666614" rpy="0 0 0" />
+      <mass value="0.235738477720019" />
+      <inertia
+        ixx="5.1068694045578E-05"
+        ixy="7.45771487459288E-16"
+        ixz="6.26195689233664E-06"
+        iyy="6.00636019624515E-05"
+        iyz="1.27828046342987E-16"
+        izz="5.32182903609188E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_Y" type="revolute">
+    <origin xyz="0 -0.1525 0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_P_S" />
+    <child link="R_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
+  </joint>
+
+  <link name="R_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0201642233698132 -0.11074968386657 -0.00598955339232021" rpy="0 0 0" />
+      <mass value="0.204366058218534" />
+      <inertia
+        ixx="0.000185559065855467"
+        ixy="5.75889766751509E-06"
+        ixz="2.40683898454438E-06"
+        iyy="0.000131246007872084"
+        iyz="1.60533277040148E-06"
+        izz="0.000271800951396149" />
+    </inertial>
+    <visual>
+      <origin xyz="0.03 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.03 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_R" type="revolute">
+    <origin xyz="0 0 -0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_Y_S" />
+    <child link="R_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.26" upper="1.57" effort="50" velocity="4.71" />
+  </joint>
+
+</robot>

code/config/real_slave_7dof.urdf

@@ -0,0 +1,288 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<robot name="right_arm">
+  <link name="PELVIS_S">
+    <inertial>
+      <origin xyz="3.78529e-05 3.81781e-07 0.0386396" rpy="0 0 0"/>
+      <mass value="2.10624590271277"/>
+      <inertia 
+        ixx="0.00165706865324979" ixy="3.13663e-09"  ixz="6.84210e-07"
+        iyy="0.00136736758630241" iyz="-1.01015e-10" izz="0.00168295663089359"/>
+    </inertial>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL"/>
+      </geometry>
+      <material name="light_gray">
+        <color rgba="0.698 0.698 0.698 1"/>
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="R_SHOULDER_P_S">
+    <inertial>
+      <origin xyz="-0.00982258725282141 -0.0704593093873431 -1.15069920925137E-06" rpy="0 0 0" />
+      <mass value="0.880737519698404" />
+      <inertia
+        ixx="0.000583190308378149"
+        ixy="1.53153074560471E-05"
+        ixz="-6.58466471736542E-09"
+        iyy="0.000445532440067178"
+        iyz="6.37001403998779E-09"
+        izz="0.000465648071069953" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_P" type="revolute">
+    <origin xyz="0 -0.0945 0.042" rpy="0 0 0" />
+    <parent link="PELVIS_S" />
+    <child link="R_SHOULDER_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="R_SHOULDER_R_S">
+    <inertial>
+      <origin xyz="-0.0346025282975784 -0.09173932900033 1.86280643132974E-08" rpy="0 0 0" />
+      <mass value="0.59478842444248" />
+      <inertia
+        ixx="0.000380970396712653" ixy="-4.80751844573946E-05"  ixz="1.34913746041655E-11"
+        iyy="0.000320962178597472" iyz="-1.00039763417375E-09" izz="0.000414771047901152" />
+    </inertial>
+    <visual>
+      <origin xyz="0.035 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.035 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_R" type="revolute">
+    <origin xyz="0 -0.0765 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_P_S" />
+    <child link="R_SHOULDER_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2" upper="2" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="R_SHOULDER_Y_S">
+    <inertial>
+      <origin xyz="-0.00440976862014946 -0.0863620469295699 -7.58791862676134E-09" rpy="0 0 0" />
+      <mass value="0.563406026626801" />
+      <inertia
+        ixx="0.000327003834287551"
+        ixy="1.80574389667711E-05"
+        ixz="-7.28778136077729E-10"
+        iyy="0.000213830709361532"
+        iyz="1.49273668428957E-10"
+        izz="0.000297341029639189" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_Y" type="revolute">
+    <origin xyz="0 -0.1475 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_R_S" />
+    <child link="R_SHOULDER_Y_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="0" upper="3.14" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="R_ELBOW_R_S">
+    <inertial>
+      <origin xyz="-0.0335624237303424 -0.0603199974228191 -2.97736340082455E-07" rpy="0 0 0" />
+      <mass value="0.393571904406492" />
+      <inertia
+        ixx="0.000172771193862529"
+        ixy="-2.34549801867353E-05"
+        ixz="1.90560556668771E-09"
+        iyy="0.000155340245267897"
+        iyz="8.82493073602971E-09"
+        izz="0.00018104232734159" />
+    </inertial>
+    <visual>
+      <origin xyz="0.034 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.034 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_ELBOW_R" type="revolute">
+    <origin xyz="0 -0.1025 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_Y_S" />
+    <child link="R_ELBOW_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="0" upper="2.18" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="R_WRIST_P_S">
+    <inertial>
+      <origin xyz="-1.39656577968772E-10 -0.0675972614865965 0.0192005515655728" rpy="0 0 0" />
+      <mass value="0.442332465815497" />
+      <inertia
+        ixx="0.000476754055455896"
+        ixy="-4.61462558956188E-15"
+        ixz="-5.91762926004267E-18"
+        iyy="0.0001034665858505"
+        iyz="4.88426705501212E-05"
+        izz="0.000482956345754214" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_P" type="revolute">
+    <origin xyz="0 -0.0965 0" rpy="0 0 0" />
+    <parent link="R_ELBOW_R_S" />
+    <child link="R_WRIST_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-3.14" upper="0" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887330083 -5.06426456325926E-10 -0.0341253783666614" rpy="0 0 0" />
+      <mass value="0.235738477720019" />
+      <inertia
+        ixx="5.1068694045578E-05"
+        ixy="7.45771487459288E-16"
+        ixz="6.26195689233664E-06"
+        iyy="6.00636019624515E-05"
+        iyz="1.27828046342987E-16"
+        izz="5.32182903609188E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_Y" type="revolute">
+    <origin xyz="0 -0.1525 0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_P_S" />
+    <child link="R_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
+  </joint>
+
+  <link name="R_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0201642233698132 -0.11074968386657 -0.00598955339232021" rpy="0 0 0" />
+      <mass value="0.204366058218534" />
+      <inertia
+        ixx="0.000185559065855467"
+        ixy="5.75889766751509E-06"
+        ixz="2.40683898454438E-06"
+        iyy="0.000131246007872084"
+        iyz="1.60533277040148E-06"
+        izz="0.000271800951396149" />
+    </inertial>
+    <visual>
+      <origin xyz="0.03 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.03 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_R" type="revolute">
+    <origin xyz="0 0 -0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_Y_S" />
+    <child link="R_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.26" upper="1.57" effort="50" velocity="4.71" />
+  </joint>
+
+</robot>

code/config/slave_7dof.urdf

@@ -0,0 +1,208 @@
+<?xml version="1.0"?>
+<robot name="slave_7dof">
+
+  <!-- Links -->
+  <link name="slave_base">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1.0"/>
+      <inertia ixx="1e-2" iyy="1e-2" izz="1e-2" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="slave_shoulder">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="slave_link_sp">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="slave_link_sy">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="slave_upper_arm">
+    <visual>
+      <!-- cylinder aligned to X axis -->
+      <origin xyz="0 -0.140 0" rpy="1.57079632679 0 0"/>
+      <geometry>
+        <cylinder length="0.280" radius="0.02"/>
+      </geometry>
+      <material name="upper_arm_mat">
+        <color rgba="0.6 0.6 0.6 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.140 0" rpy="1.57079632679 0 0"/>
+      <geometry>
+        <cylinder length="0.280" radius="0.02"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 -0.140 0" rpy="1.57079632679 0 0"/>
+      <mass value="1.0"/>
+      <inertia ixx="0.01" iyy="0.01" izz="0.01" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="slave_forearm">
+    <visual>
+      <origin xyz="0 -0.150 0" rpy="1.57079632679 0 0"/>
+      <geometry>
+        <cylinder length="0.300" radius="0.018"/>
+      </geometry>
+      <material name="forearm_mat">
+        <color rgba="0.7 0.7 0.9 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.150 0" rpy="1.57079632679 0 0"/>
+      <geometry>
+        <cylinder length="0.300" radius="0.018"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 -0.150 0" rpy="1.57079632679 0 0"/>
+      <mass value="0.8"/>
+      <inertia ixx="0.008" iyy="0.008" izz="0.008" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="slave_wrist">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="slave_link_wr">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="slave_link_wy">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-4" iyy="1e-4" izz="1e-4" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <link name="slave_ee">
+    <visual>
+      <origin xyz="0 -0.05 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.02 0.10 0.02"/>
+      </geometry>
+      <material name="ee_mat">
+        <color rgba="0.8 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.05 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.02 0.10 0.02"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 -0.05 0" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+
+  <!-- Joints -->
+
+  <!-- ✅ 关键：把整条手臂在 slave_base 坐标系里转到 -Y 方向
+       +X -> -Y 等价于绕 Z 轴 -90° -->
+  <joint name="slave_base_to_shoulder" type="fixed">
+    <parent link="slave_base"/>
+    <child link="slave_shoulder"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <!-- Shoulder order: pitch(Y) -> yaw(Z) -> roll(X) -->
+  <joint name="slave_shoulder_pitch_joint" type="revolute">
+    <parent link="slave_shoulder"/>
+    <child link="slave_link_sp"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 -1 0"/>
+    <limit lower="-3.1416" upper="3.1416" effort="50" velocity="4.0"/>
+  </joint>
+
+  <joint name="slave_shoulder_yaw_joint" type="revolute">
+    <parent link="slave_link_sp"/>
+    <child link="slave_link_sy"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-3.1416" upper="3.1416" effort="50" velocity="4.0"/>
+  </joint>
+
+  <joint name="slave_shoulder_roll_joint" type="revolute">
+    <parent link="slave_link_sy"/>
+    <child link="slave_upper_arm"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 -1 0"/>
+    <limit lower="-3.1416" upper="3.1416" effort="50" velocity="4.0"/>
+  </joint>
+
+  <!-- Elbow at x = 0.280 -->
+  <joint name="slave_elbow_flex_joint" type="revolute">
+    <parent link="slave_upper_arm"/>
+    <child link="slave_forearm"/>
+    <origin xyz="0 -0.280 0" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-3.1416" upper="3.1416" effort="30" velocity="4.0"/>
+  </joint>
+
+  <!-- Wrist fixed at x = 0.300 -->
+  <joint name="slave_forearm_to_wrist" type="fixed">
+    <parent link="slave_forearm"/>
+    <child link="slave_wrist"/>
+    <origin xyz="0 -0.300 0" rpy="0 0 0"/>
+  </joint>
+
+  <!-- Wrist order: roll(X) -> yaw(Y) -> pitch(Z) -->
+  <joint name="slave_wrist_roll_joint" type="revolute">
+    <parent link="slave_wrist"/>
+    <child link="slave_link_wr"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 -1 0"/>
+    <limit lower="-3.1416" upper="3.1416" effort="20" velocity="4.0"/>
+  </joint>
+
+  <joint name="slave_wrist_yaw_joint" type="revolute">
+    <parent link="slave_link_wr"/>
+    <child link="slave_link_wy"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.1416" upper="3.1416" effort="20" velocity="4.0"/>
+  </joint>
+
+  <joint name="slave_wrist_pitch_joint" type="revolute">
+    <parent link="slave_link_wy"/>
+    <child link="slave_ee"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-3.1416" upper="3.1416" effort="20" velocity="4.0"/>
+  </joint>
+
+</robot>

code/core/command_allocator.py

@@ -0,0 +1,182 @@
+"""Total-command allocation and accepted haptic-increment reconstruction."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+
+def _vector(value, size: int | None, name: str) -> np.ndarray:
+    array = np.asarray(value, dtype=float).reshape(-1)
+    if size is not None and array.shape != (size,):
+        raise ValueError(f"{name} must have shape ({size},), got {array.shape}")
+    if array.size == 0 or not np.all(np.isfinite(array)):
+        raise ValueError(f"{name} must be non-empty and finite")
+    return array
+
+
+@dataclass(frozen=True)
+class PreparedAllocation:
+    compensation: np.ndarray
+    haptic_raw: np.ndarray
+    haptic_candidate: np.ndarray
+    lower_haptic_bound: np.ndarray
+    upper_haptic_bound: np.ndarray
+    compensation_limited: bool
+    haptic_limited: bool
+
+
+@dataclass(frozen=True)
+class AcceptedAllocation:
+    compensation_requested: np.ndarray
+    haptic_projected: np.ndarray
+    total_requested: np.ndarray
+    total_quantized: np.ndarray
+    total_accepted: np.ndarray
+    compensation_accepted: np.ndarray
+    haptic_accepted: np.ndarray
+    downstream_modified: bool
+    quantization_active: bool
+    derating_active: bool
+    readback_used: bool
+
+
+class CommandAllocator:
+    """Reserve total actuator headroom before the final energy projection.
+
+    Call :meth:`prepare` before energy supervision.  Pass its
+    ``haptic_candidate`` through the selected energy supervisor, then call
+    :meth:`finalize`.  The latter never silently claims the projected torque was
+    accepted: quantization, derating, or drive readback are exposed and the
+    accepted haptic increment is reconstructed explicitly.
+    """
+
+    def __init__(
+        self,
+        total_torque_limit: float | np.ndarray,
+        *,
+        quantization_step: float | np.ndarray | None = None,
+    ):
+        limits = np.asarray(total_torque_limit, dtype=float)
+        if limits.ndim == 0:
+            limits = limits.reshape(1)
+        else:
+            limits = limits.reshape(-1)
+        if limits.size == 0 or np.any(~np.isfinite(limits)) or np.any(limits <= 0.0):
+            raise ValueError("total_torque_limit must contain finite positive values")
+        self.total_torque_limit = limits
+        if quantization_step is None:
+            self.quantization_step = np.zeros_like(limits)
+        else:
+            steps = np.asarray(quantization_step, dtype=float)
+            if steps.ndim == 0:
+                steps = np.full(limits.size, float(steps))
+            else:
+                steps = steps.reshape(-1)
+            if steps.shape != limits.shape or np.any(~np.isfinite(steps)) or np.any(steps < 0.0):
+                raise ValueError(
+                    "quantization_step must be scalar or match torque limits"
+                )
+            self.quantization_step = steps
+
+    def prepare(
+        self,
+        compensation: np.ndarray,
+        haptic_raw: np.ndarray,
+    ) -> PreparedAllocation:
+        size = self.total_torque_limit.size
+        comp_raw = _vector(compensation, size, "compensation")
+        haptic = _vector(haptic_raw, size, "haptic_raw")
+        comp = np.clip(
+            comp_raw, -self.total_torque_limit, self.total_torque_limit
+        )
+        lower = -self.total_torque_limit - comp
+        upper = self.total_torque_limit - comp
+        candidate = np.clip(haptic, lower, upper)
+        return PreparedAllocation(
+            compensation=comp.copy(),
+            haptic_raw=haptic.copy(),
+            haptic_candidate=candidate,
+            lower_haptic_bound=lower,
+            upper_haptic_bound=upper,
+            compensation_limited=bool(np.any(np.abs(comp - comp_raw) > 1e-12)),
+            haptic_limited=bool(np.any(np.abs(candidate - haptic) > 1e-12)),
+        )
+
+    def finalize(
+        self,
+        prepared: PreparedAllocation,
+        haptic_projected: np.ndarray,
+        *,
+        derating: float | np.ndarray = 1.0,
+        accepted_total: np.ndarray | None = None,
+        accepted_compensation: np.ndarray | None = None,
+    ) -> AcceptedAllocation:
+        size = self.total_torque_limit.size
+        projected = _vector(haptic_projected, size, "haptic_projected")
+        tolerance = 1e-12
+        if np.any(projected < prepared.lower_haptic_bound - tolerance) or np.any(
+            projected > prepared.upper_haptic_bound + tolerance
+        ):
+            raise ValueError(
+                "haptic_projected exceeds reserved headroom; projection must "
+                "not enlarge the prepared candidate"
+            )
+        total_requested = prepared.compensation + projected
+
+        quantized = total_requested.copy()
+        active_steps = self.quantization_step > 0.0
+        quantized[active_steps] = (
+            np.round(quantized[active_steps] / self.quantization_step[active_steps])
+            * self.quantization_step[active_steps]
+        )
+
+        derating_vector = np.asarray(derating, dtype=float)
+        if derating_vector.ndim == 0:
+            derating_vector = np.full(size, float(derating_vector))
+        else:
+            derating_vector = derating_vector.reshape(-1)
+        if (
+            derating_vector.shape != (size,)
+            or np.any(~np.isfinite(derating_vector))
+            or np.any(derating_vector < 0.0)
+            or np.any(derating_vector > 1.0)
+        ):
+            raise ValueError("derating must be scalar/vector in [0, 1]")
+        accepted_limits = derating_vector * self.total_torque_limit
+        expected_accepted = np.clip(quantized, -accepted_limits, accepted_limits)
+        readback_used = accepted_total is not None
+        accepted = (
+            expected_accepted
+            if accepted_total is None
+            else _vector(accepted_total, size, "accepted_total")
+        )
+        if np.any(np.abs(accepted) > accepted_limits + tolerance):
+            raise ValueError("accepted_total exceeds the declared derated limit")
+
+        comp_accepted = (
+            prepared.compensation
+            if accepted_compensation is None
+            else _vector(
+                accepted_compensation, size, "accepted_compensation"
+            )
+        )
+        haptic_accepted = accepted - comp_accepted
+        return AcceptedAllocation(
+            compensation_requested=prepared.compensation.copy(),
+            haptic_projected=projected.copy(),
+            total_requested=total_requested,
+            total_quantized=quantized,
+            total_accepted=accepted.copy(),
+            compensation_accepted=comp_accepted.copy(),
+            haptic_accepted=haptic_accepted,
+            downstream_modified=bool(
+                np.any(np.abs(haptic_accepted - projected) > tolerance)
+            ),
+            quantization_active=bool(
+                np.any(np.abs(quantized - total_requested) > tolerance)
+            ),
+            derating_active=bool(np.any(derating_vector < 1.0 - tolerance)),
+            readback_used=readback_used,
+        )

code/core/energy_audit.py

@@ -0,0 +1,152 @@
+"""Independent H3/H4 metrics reconstructed from immutable raw logs."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class EnergyAuditResult:
+    energy_before: np.ndarray
+    energy_preclip: np.ndarray
+    energy_after: np.ndarray
+    floor_deficit: np.ndarray
+    projected_shadow_energy: np.ndarray
+    candidate_shadow_energy: np.ndarray
+    candidate_power: np.ndarray
+    accepted_power: np.ndarray
+    max_floor_deficit: float
+    projected_floor_deficit: float
+    candidate_floor_deficit: float
+    delta_B: float
+    projection_distortion: float
+    released_energy: float
+    preclip_log_max_error: float | None
+    downstream_modification_max: float
+
+    @property
+    def passed_floor_gate(self) -> bool:
+        return self.max_floor_deficit <= 1e-12
+
+
+def _sample_matrix(value, name: str) -> np.ndarray:
+    array = np.asarray(value, dtype=float)
+    if array.ndim != 2 or array.shape[0] == 0 or array.shape[1] == 0:
+        raise ValueError(f"{name} must be a non-empty 2-D array")
+    if not np.all(np.isfinite(array)):
+        raise ValueError(f"{name} must contain only finite values")
+    return array
+
+
+def audit_haptic_energy(
+    *,
+    tau_candidate: np.ndarray,
+    tau_projected: np.ndarray,
+    tau_accepted: np.ndarray,
+    qd_master: np.ndarray,
+    dt: float | np.ndarray,
+    energy_initial: float,
+    energy_min: float,
+    energy_max: float,
+    epsilon_tau: float = 1e-12,
+    logged_preclip: np.ndarray | None = None,
+) -> EnergyAuditResult:
+    """Recompute the H4 budget and transparency endpoints.
+
+    The deterministic budget gate uses ``tau_accepted`` because this is the
+    actual actuator-port increment.  The same-run shadow uses the logged
+    pre-projection candidate and never feeds the counterfactual back into the
+    simulated trajectory.
+    """
+    candidate = _sample_matrix(tau_candidate, "tau_candidate")
+    projected = _sample_matrix(tau_projected, "tau_projected")
+    accepted = _sample_matrix(tau_accepted, "tau_accepted")
+    velocity = _sample_matrix(qd_master, "qd_master")
+    if not (
+        candidate.shape == projected.shape == accepted.shape == velocity.shape
+    ):
+        raise ValueError("all torque and velocity arrays must have equal shapes")
+    count = candidate.shape[0]
+    delta = np.broadcast_to(np.asarray(dt, dtype=float), (count,)).copy()
+    if np.any(delta <= 0.0) or not np.all(np.isfinite(delta)):
+        raise ValueError("dt must contain finite positive values")
+    if not (
+        np.isfinite(energy_initial)
+        and np.isfinite(energy_min)
+        and np.isfinite(energy_max)
+        and energy_min <= energy_initial <= energy_max
+    ):
+        raise ValueError("energy values must satisfy min <= initial <= max")
+    if epsilon_tau <= 0.0:
+        raise ValueError("epsilon_tau must be positive")
+
+    candidate_power = np.einsum("ij,ij->i", candidate, velocity)
+    accepted_power = np.einsum("ij,ij->i", accepted, velocity)
+    energy_before = np.empty(count, dtype=float)
+    energy_preclip = np.empty(count, dtype=float)
+    energy_after = np.empty(count, dtype=float)
+    floor_deficit = np.empty(count, dtype=float)
+    energy = float(energy_initial)
+    for index in range(count):
+        energy_before[index] = energy
+        preclip = energy - accepted_power[index] * delta[index]
+        energy_preclip[index] = preclip
+        floor_deficit[index] = max(0.0, energy_min - preclip)
+        energy = float(np.clip(preclip, energy_min, energy_max))
+        energy_after[index] = energy
+
+    candidate_shadow = np.empty(count + 1, dtype=float)
+    projected_shadow = np.empty(count + 1, dtype=float)
+    candidate_shadow[0] = energy_initial
+    projected_shadow[0] = energy_initial
+    for index in range(count):
+        candidate_shadow[index + 1] = min(
+            energy_max,
+            candidate_shadow[index] - candidate_power[index] * delta[index],
+        )
+        projected_shadow[index + 1] = min(
+            energy_max,
+            projected_shadow[index] - accepted_power[index] * delta[index],
+        )
+    candidate_B = float(
+        np.max(np.maximum(0.0, energy_min - candidate_shadow))
+    )
+    projected_B = float(
+        np.max(np.maximum(0.0, energy_min - projected_shadow))
+    )
+    numerator = float(
+        np.sum(np.linalg.norm(accepted - candidate, axis=1) * delta)
+    )
+    denominator = float(
+        np.sum(np.linalg.norm(candidate, axis=1) * delta) + epsilon_tau
+    )
+
+    preclip_error = None
+    if logged_preclip is not None:
+        logged = np.asarray(logged_preclip, dtype=float).reshape(-1)
+        if logged.shape != (count,) or not np.all(np.isfinite(logged)):
+            raise ValueError("logged_preclip must be finite with one value per sample")
+        preclip_error = float(np.max(np.abs(logged - energy_preclip)))
+
+    return EnergyAuditResult(
+        energy_before=energy_before,
+        energy_preclip=energy_preclip,
+        energy_after=energy_after,
+        floor_deficit=floor_deficit,
+        projected_shadow_energy=projected_shadow,
+        candidate_shadow_energy=candidate_shadow,
+        candidate_power=candidate_power,
+        accepted_power=accepted_power,
+        max_floor_deficit=float(np.max(floor_deficit)),
+        projected_floor_deficit=projected_B,
+        candidate_floor_deficit=candidate_B,
+        delta_B=candidate_B - projected_B,
+        projection_distortion=numerator / denominator,
+        released_energy=float(np.sum(accepted_power * delta)),
+        preclip_log_max_error=preclip_error,
+        downstream_modification_max=float(
+            np.max(np.linalg.norm(accepted - projected, axis=1))
+        ),
+    )

code/core/estimation_signals.py

@@ -0,0 +1,425 @@
+"""Causal signal and calibration components for interaction estimation.
+
+These components are deliberately independent of the Pinocchio model.  A
+locked experiment can therefore replay the exact same measured signals through
+the nominal, no-bias, and no-friction conditions without changing its dynamics
+or sample selection.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from enum import Enum
+from typing import Optional
+
+import numpy as np
+
+from .wrench_solver import (
+    ScaledDLSSolver,
+    WrenchSolveResult,
+    WrenchSolver,
+)
+
+
+def _finite_vector(value, name: str, size: Optional[int] = None) -> np.ndarray:
+    vector = np.asarray(value, dtype=float).reshape(-1)
+    if size is not None and vector.shape != (size,):
+        raise ValueError(f"{name} must have shape ({size},), got {vector.shape}")
+    if vector.size == 0 or not np.all(np.isfinite(vector)):
+        raise ValueError(f"{name} must be a non-empty finite vector")
+    return vector
+
+
+def _immutable_vector(value, name: str, size: Optional[int] = None) -> np.ndarray:
+    vector = _finite_vector(value, name, size).copy()
+    vector.setflags(write=False)
+    return vector
+
+
+class AccelerationEstimateStatus(str, Enum):
+    INITIALIZING = "initializing"
+    VALID = "valid"
+    INVALID_DT = "invalid_dt"
+
+
+@dataclass(frozen=True)
+class AccelerationEstimatorConfig:
+    """Frozen one-pole differentiator configuration."""
+
+    cutoff_hz: float
+    minimum_dt_s: float
+    maximum_dt_s: float
+
+    def __post_init__(self) -> None:
+        values = (self.cutoff_hz, self.minimum_dt_s, self.maximum_dt_s)
+        if any(not np.isfinite(value) or value <= 0.0 for value in values):
+            raise ValueError("acceleration settings must be finite and positive")
+        if self.minimum_dt_s >= self.maximum_dt_s:
+            raise ValueError("minimum_dt_s must be smaller than maximum_dt_s")
+
+    @property
+    def nominal_filter_delay_s(self) -> float:
+        """Continuous one-pole time constant, logged as a delay descriptor."""
+        return 1.0 / (2.0 * np.pi * self.cutoff_hz)
+
+
+@dataclass(frozen=True)
+class AccelerationEstimate:
+    acceleration: np.ndarray
+    status: AccelerationEstimateStatus
+    dt_s: float
+    raw_acceleration: np.ndarray
+
+    def __post_init__(self) -> None:
+        acceleration = _immutable_vector(self.acceleration, "acceleration")
+        raw = _immutable_vector(
+            self.raw_acceleration, "raw_acceleration", acceleration.size
+        )
+        if not np.isfinite(self.dt_s):
+            raise ValueError("dt_s must be finite")
+        if (
+            self.status is not AccelerationEstimateStatus.INVALID_DT
+            and self.dt_s < 0.0
+        ):
+            raise ValueError("dt_s must be non-negative for a valid estimate")
+        object.__setattr__(self, "acceleration", acceleration)
+        object.__setattr__(self, "raw_acceleration", raw)
+
+
+class CausalAccelerationEstimator:
+    """Backward difference followed by a causal one-pole low-pass filter."""
+
+    def __init__(
+        self,
+        joint_count: int,
+        config: AccelerationEstimatorConfig,
+    ) -> None:
+        if int(joint_count) <= 0:
+            raise ValueError("joint_count must be positive")
+        self.joint_count = int(joint_count)
+        self.config = config
+        self.reset()
+
+    def reset(self) -> None:
+        self._previous_velocity: Optional[np.ndarray] = None
+        self._previous_time_s: Optional[float] = None
+        self._filtered = np.zeros(self.joint_count, dtype=float)
+
+    def update(
+        self, joint_velocity: np.ndarray, timestamp_s: float
+    ) -> AccelerationEstimate:
+        velocity = _finite_vector(
+            joint_velocity, "joint_velocity", self.joint_count
+        )
+        timestamp = float(timestamp_s)
+        if not np.isfinite(timestamp):
+            raise ValueError("timestamp_s must be finite")
+
+        if self._previous_velocity is None:
+            self._previous_velocity = velocity.copy()
+            self._previous_time_s = timestamp
+            zeros = np.zeros(self.joint_count, dtype=float)
+            return AccelerationEstimate(
+                acceleration=zeros,
+                raw_acceleration=zeros,
+                status=AccelerationEstimateStatus.INITIALIZING,
+                dt_s=0.0,
+            )
+
+        dt = timestamp - float(self._previous_time_s)
+        if dt < self.config.minimum_dt_s or dt > self.config.maximum_dt_s:
+            # Invalid timestamps are observable and do not contaminate state.
+            return AccelerationEstimate(
+                acceleration=self._filtered,
+                raw_acceleration=np.zeros(self.joint_count),
+                status=AccelerationEstimateStatus.INVALID_DT,
+                dt_s=dt,
+            )
+
+        raw = (velocity - self._previous_velocity) / dt
+        alpha = 1.0 - np.exp(-2.0 * np.pi * self.config.cutoff_hz * dt)
+        self._filtered = self._filtered + alpha * (raw - self._filtered)
+        self._previous_velocity = velocity.copy()
+        self._previous_time_s = timestamp
+        return AccelerationEstimate(
+            acceleration=self._filtered,
+            raw_acceleration=raw,
+            status=AccelerationEstimateStatus.VALID,
+            dt_s=dt,
+        )
+
+
+@dataclass(frozen=True)
+class JointFrictionCalibration:
+    """Per-joint smooth Coulomb plus viscous friction coefficients."""
+
+    coulomb_nm: np.ndarray
+    viscous_nm_per_rad_s: np.ndarray
+    smoothing_velocity_rad_s: float = 0.02
+
+    def __post_init__(self) -> None:
+        coulomb = _immutable_vector(self.coulomb_nm, "coulomb_nm")
+        viscous = _immutable_vector(
+            self.viscous_nm_per_rad_s,
+            "viscous_nm_per_rad_s",
+            coulomb.size,
+        )
+        if np.any(coulomb < 0.0) or np.any(viscous < 0.0):
+            raise ValueError("friction magnitudes must be non-negative")
+        smoothing = float(self.smoothing_velocity_rad_s)
+        if not np.isfinite(smoothing) or smoothing <= 0.0:
+            raise ValueError(
+                "smoothing_velocity_rad_s must be finite and positive"
+            )
+        object.__setattr__(self, "coulomb_nm", coulomb)
+        object.__setattr__(self, "viscous_nm_per_rad_s", viscous)
+        object.__setattr__(self, "smoothing_velocity_rad_s", smoothing)
+
+    @property
+    def joint_count(self) -> int:
+        return int(self.coulomb_nm.size)
+
+    def torque(self, joint_velocity: np.ndarray) -> np.ndarray:
+        velocity = _finite_vector(
+            joint_velocity, "joint_velocity", self.joint_count
+        )
+        return (
+            self.coulomb_nm
+            * np.tanh(velocity / self.smoothing_velocity_rad_s)
+            + self.viscous_nm_per_rad_s * velocity
+        )
+
+    @classmethod
+    def zeros(
+        cls,
+        joint_count: int,
+        *,
+        smoothing_velocity_rad_s: float = 0.02,
+    ) -> "JointFrictionCalibration":
+        if int(joint_count) <= 0:
+            raise ValueError("joint_count must be positive")
+        zeros = np.zeros(int(joint_count), dtype=float)
+        return cls(zeros, zeros, smoothing_velocity_rad_s)
+
+    @classmethod
+    def fit(
+        cls,
+        velocity_samples: np.ndarray,
+        friction_torque_samples: np.ndarray,
+        *,
+        smoothing_velocity_rad_s: float = 0.02,
+    ) -> "JointFrictionCalibration":
+        """Fit independent smooth-Coulomb/viscous models by least squares."""
+        velocity = np.asarray(velocity_samples, dtype=float)
+        torque = np.asarray(friction_torque_samples, dtype=float)
+        if velocity.ndim != 2 or torque.shape != velocity.shape:
+            raise ValueError(
+                "velocity_samples and friction_torque_samples must share "
+                "shape (n_samples, n_joints)"
+            )
+        if velocity.shape[0] < 2 or velocity.shape[1] < 1:
+            raise ValueError("at least two samples and one joint are required")
+        if not np.all(np.isfinite(velocity)) or not np.all(np.isfinite(torque)):
+            raise ValueError("friction calibration samples must be finite")
+        smoothing = float(smoothing_velocity_rad_s)
+        if not np.isfinite(smoothing) or smoothing <= 0.0:
+            raise ValueError(
+                "smoothing_velocity_rad_s must be finite and positive"
+            )
+
+        coulomb = np.zeros(velocity.shape[1], dtype=float)
+        viscous = np.zeros(velocity.shape[1], dtype=float)
+        for joint in range(velocity.shape[1]):
+            design = np.column_stack(
+                (
+                    np.tanh(velocity[:, joint] / smoothing),
+                    velocity[:, joint],
+                )
+            )
+            coefficients, _, _, _ = np.linalg.lstsq(
+                design, torque[:, joint], rcond=None
+            )
+            # Negative coefficients are non-physical and signal insufficient
+            # excitation; clipping makes that decision explicit and stable.
+            coulomb[joint], viscous[joint] = np.maximum(coefficients, 0.0)
+        return cls(coulomb, viscous, smoothing)
+
+
+@dataclass(frozen=True)
+class WrenchEstimatorCalibration:
+    """Immutable parameters selected only from the calibration split."""
+
+    joint_bias_nm: np.ndarray
+    friction: JointFrictionCalibration
+    characteristic_length_m: float
+    damping: float
+    relative_rank_tolerance: float = 1e-9
+    calibration_id: str = "unversioned"
+
+    def __post_init__(self) -> None:
+        bias = _immutable_vector(
+            self.joint_bias_nm, "joint_bias_nm", self.friction.joint_count
+        )
+        positive = (
+            self.characteristic_length_m,
+            self.damping,
+            self.relative_rank_tolerance,
+        )
+        if any(not np.isfinite(value) or value <= 0.0 for value in positive):
+            raise ValueError("wrench calibration scalars must be positive")
+        if self.relative_rank_tolerance >= 1.0:
+            raise ValueError("relative_rank_tolerance must be smaller than one")
+        if not isinstance(self.calibration_id, str) or not self.calibration_id:
+            raise ValueError("calibration_id must be a non-empty string")
+        object.__setattr__(self, "joint_bias_nm", bias)
+
+    def build_dls_solver(self) -> ScaledDLSSolver:
+        return ScaledDLSSolver(
+            self.characteristic_length_m,
+            self.damping,
+            relative_rank_tolerance=self.relative_rank_tolerance,
+        )
+
+
+@dataclass(frozen=True)
+class ResidualAblation:
+    """Explicit H2 switches; both default to the nominal estimator."""
+
+    use_bias_correction: bool = True
+    use_friction_compensation: bool = True
+
+    @classmethod
+    def no_bias(cls) -> "ResidualAblation":
+        return cls(use_bias_correction=False)
+
+    @classmethod
+    def no_friction(cls) -> "ResidualAblation":
+        return cls(use_friction_compensation=False)
+
+
+@dataclass(frozen=True)
+class ResidualTorqueBreakdown:
+    residual_nm: np.ndarray
+    raw_residual_nm: np.ndarray
+    applied_bias_nm: np.ndarray
+    applied_friction_nm: np.ndarray
+    ablation: ResidualAblation
+
+    def __post_init__(self) -> None:
+        residual = _immutable_vector(self.residual_nm, "residual_nm")
+        size = residual.size
+        object.__setattr__(
+            self,
+            "raw_residual_nm",
+            _immutable_vector(self.raw_residual_nm, "raw_residual_nm", size),
+        )
+        object.__setattr__(
+            self,
+            "applied_bias_nm",
+            _immutable_vector(self.applied_bias_nm, "applied_bias_nm", size),
+        )
+        object.__setattr__(
+            self,
+            "applied_friction_nm",
+            _immutable_vector(
+                self.applied_friction_nm, "applied_friction_nm", size
+            ),
+        )
+        object.__setattr__(self, "residual_nm", residual)
+
+
+class ResidualTorqueModel:
+    """Apply frozen bias/friction calibration with replayable ablations."""
+
+    def __init__(self, calibration: WrenchEstimatorCalibration) -> None:
+        self.calibration = calibration
+
+    def compute(
+        self,
+        measured_torque_nm: np.ndarray,
+        rigid_body_torque_nm: np.ndarray,
+        joint_velocity_rad_s: np.ndarray,
+        *,
+        ablation: ResidualAblation = ResidualAblation(),
+    ) -> ResidualTorqueBreakdown:
+        size = self.calibration.friction.joint_count
+        measured = _finite_vector(
+            measured_torque_nm, "measured_torque_nm", size
+        )
+        rigid = _finite_vector(
+            rigid_body_torque_nm, "rigid_body_torque_nm", size
+        )
+        velocity = _finite_vector(
+            joint_velocity_rad_s, "joint_velocity_rad_s", size
+        )
+        raw = measured - rigid
+        bias = (
+            self.calibration.joint_bias_nm
+            if ablation.use_bias_correction
+            else np.zeros(size)
+        )
+        friction = (
+            self.calibration.friction.torque(velocity)
+            if ablation.use_friction_compensation
+            else np.zeros(size)
+        )
+        return ResidualTorqueBreakdown(
+            residual_nm=raw - bias - friction,
+            raw_residual_nm=raw,
+            applied_bias_nm=bias,
+            applied_friction_nm=friction,
+            ablation=ablation,
+        )
+
+
+@dataclass(frozen=True)
+class CalibratedWrenchEstimate:
+    residual: ResidualTorqueBreakdown
+    solve: WrenchSolveResult
+
+
+class CalibratedResidualWrenchEstimator:
+    """Compose residual conditioning and a selectable H2 wrench solver."""
+
+    def __init__(
+        self,
+        calibration: WrenchEstimatorCalibration,
+        solver: Optional[WrenchSolver] = None,
+    ) -> None:
+        self.calibration = calibration
+        self.residual_model = ResidualTorqueModel(calibration)
+        self.solver: WrenchSolver = (
+            calibration.build_dls_solver() if solver is None else solver
+        )
+        if not np.isclose(
+            self.solver.characteristic_length_m,
+            calibration.characteristic_length_m,
+        ):
+            raise ValueError(
+                "solver and calibration must use the same characteristic length"
+            )
+        if not np.isclose(
+            self.solver.relative_rank_tolerance,
+            calibration.relative_rank_tolerance,
+        ):
+            raise ValueError(
+                "solver and calibration must use the same rank tolerance"
+            )
+
+    def estimate(
+        self,
+        jacobian: np.ndarray,
+        measured_torque_nm: np.ndarray,
+        rigid_body_torque_nm: np.ndarray,
+        joint_velocity_rad_s: np.ndarray,
+        *,
+        ablation: ResidualAblation = ResidualAblation(),
+    ) -> CalibratedWrenchEstimate:
+        residual = self.residual_model.compute(
+            measured_torque_nm,
+            rigid_body_torque_nm,
+            joint_velocity_rad_s,
+            ablation=ablation,
+        )
+        solve = self.solver.solve(jacobian, residual.residual_nm)
+        return CalibratedWrenchEstimate(residual=residual, solve=solve)

code/core/feedback_protocol.py

@@ -0,0 +1,364 @@
+"""Source-stamped bilateral-feedback contracts used by formal experiments.
+
+The current simulator historically delayed a residual and a wrench in separate
+queues.  That is sufficient for a visual demonstration but cannot guarantee a
+matched H3 comparison.  This module makes the comparison unit explicit: one
+return packet carries the reconstructed wrench and ``J_s.T @ wrench`` together,
+and both mapping conditions consume that immutable packet.
+
+All spatial vectors use the repository convention ``[linear; angular]`` for
+twists and ``[force; moment]`` for wrenches.
+"""
+
+from __future__ import annotations
+
+from collections import OrderedDict
+from dataclasses import dataclass
+from enum import Enum, IntEnum
+import hashlib
+from typing import Iterable
+
+import numpy as np
+
+
+def _readonly_vector(value, size: int, name: str) -> np.ndarray:
+    array = np.asarray(value, dtype=float).reshape(-1).copy()
+    if array.shape != (size,):
+        raise ValueError(f"{name} must have shape ({size},), got {array.shape}")
+    if not np.all(np.isfinite(array)):
+        raise ValueError(f"{name} must contain only finite values")
+    array.setflags(write=False)
+    return array
+
+
+def _readonly_matrix(value, shape: tuple[int, int], name: str) -> np.ndarray:
+    array = np.asarray(value, dtype=float).copy()
+    if array.shape != shape:
+        raise ValueError(f"{name} must have shape {shape}, got {array.shape}")
+    if not np.all(np.isfinite(array)):
+        raise ValueError(f"{name} must contain only finite values")
+    array.setflags(write=False)
+    return array
+
+
+def _payload_hash(parts: Iterable[np.ndarray | int | float]) -> str:
+    digest = hashlib.sha256()
+    for part in parts:
+        if isinstance(part, np.ndarray):
+            contiguous = np.ascontiguousarray(part, dtype=np.float64)
+            digest.update(str(contiguous.shape).encode("ascii"))
+            digest.update(contiguous.tobytes())
+        elif isinstance(part, (int, np.integer)):
+            digest.update(f"i:{int(part)}".encode("ascii"))
+        else:
+            digest.update(f"f:{float(part):.17g}".encode("ascii"))
+    return digest.hexdigest()
+
+
+class MappingKind(str, Enum):
+    """Feedback mappings required by the H3/H4 comparison."""
+
+    DIFFERENTIAL_RESIDUAL = "differential_residual"
+    MATCHED_DIFFERENTIAL_WRENCH = "matched_differential_wrench"
+    DIRECT_MASTER_JACOBIAN = "direct_master_jacobian"
+
+
+class MapPolicy(str, Enum):
+    """Which differential is used for a delayed return packet."""
+
+    CURRENT = "current"
+    SOURCE_STAMPED = "source_stamped"
+
+
+class PacketState(IntEnum):
+    EMPTY = 0
+    ACTIVE = 1
+    HELD = 2
+    TIMED_OUT = 3
+    RECOVERING = 4
+
+
+class PacketRejectReason(IntEnum):
+    ACCEPTED = 0
+    INVALID = 1
+    CORRUPT = 2
+    DUPLICATE_OR_STALE = 3
+
+
+@dataclass(frozen=True)
+class MapSnapshot:
+    """One accepted 50-Hz retargeting differential."""
+
+    map_id: int
+    source_index: int
+    source_time: float
+    differential: np.ndarray
+    valid: bool = True
+    reason_code: int = 0
+
+    def __post_init__(self) -> None:
+        if self.map_id < 0 or self.source_index < 0:
+            raise ValueError("map_id and source_index must be non-negative")
+        if not np.isfinite(self.source_time):
+            raise ValueError("source_time must be finite")
+        matrix = np.asarray(self.differential, dtype=float)
+        if matrix.ndim != 2 or matrix.shape[0] == 0 or matrix.shape[1] == 0:
+            raise ValueError("differential must be a non-empty matrix")
+        object.__setattr__(
+            self,
+            "differential",
+            _readonly_matrix(matrix, matrix.shape, "differential"),
+        )
+
+    @property
+    def digest(self) -> str:
+        return _payload_hash(
+            (
+                self.map_id,
+                self.source_index,
+                self.source_time,
+                self.differential,
+            )
+        )
+
+
+class MapRegistry:
+    """Bounded map-id registry for source-stamped delayed feedback."""
+
+    def __init__(self, capacity: int = 512):
+        if capacity <= 0:
+            raise ValueError("capacity must be positive")
+        self.capacity = int(capacity)
+        self._maps: OrderedDict[int, MapSnapshot] = OrderedDict()
+
+    def add(self, snapshot: MapSnapshot) -> None:
+        existing = self._maps.get(snapshot.map_id)
+        if existing is not None and existing.digest != snapshot.digest:
+            raise ValueError(
+                f"map_id {snapshot.map_id} cannot be reused for different data"
+            )
+        self._maps[snapshot.map_id] = snapshot
+        self._maps.move_to_end(snapshot.map_id)
+        while len(self._maps) > self.capacity:
+            self._maps.popitem(last=False)
+
+    def get(self, map_id: int) -> MapSnapshot:
+        try:
+            return self._maps[int(map_id)]
+        except KeyError as exc:
+            raise KeyError(f"map_id {map_id} is not available") from exc
+
+    @property
+    def latest(self) -> MapSnapshot:
+        if not self._maps:
+            raise KeyError("map registry is empty")
+        return next(reversed(self._maps.values()))
+
+    def __len__(self) -> int:
+        return len(self._maps)
+
+
+@dataclass(frozen=True)
+class ForwardPacket:
+    """Master-to-slave held reference packet."""
+
+    seq: int
+    source_index: int
+    source_time: float
+    map_id: int
+    q_slave_ref: np.ndarray
+    qd_slave_ctrl: np.ndarray
+    valid: bool = True
+
+    def __post_init__(self) -> None:
+        if self.seq < 0 or self.source_index < 0 or self.map_id < 0:
+            raise ValueError("packet identifiers must be non-negative")
+        if not np.isfinite(self.source_time):
+            raise ValueError("source_time must be finite")
+        q = np.asarray(self.q_slave_ref, dtype=float).reshape(-1)
+        qd = np.asarray(self.qd_slave_ctrl, dtype=float).reshape(-1)
+        if q.size == 0 or q.shape != qd.shape:
+            raise ValueError("q_slave_ref and qd_slave_ctrl must have equal shapes")
+        object.__setattr__(
+            self, "q_slave_ref", _readonly_vector(q, q.size, "q_slave_ref")
+        )
+        object.__setattr__(
+            self, "qd_slave_ctrl", _readonly_vector(qd, qd.size, "qd_slave_ctrl")
+        )
+
+
+@dataclass(frozen=True)
+class ReturnPacket:
+    """Slave-to-master packet supporting every declared feedback condition."""
+
+    seq: int
+    source_index: int
+    source_time: float
+    echoed_map_id: int
+    residual: np.ndarray
+    wrench: np.ndarray
+    js_t_wrench: np.ndarray
+    qd_slave_actual: np.ndarray
+    valid: bool = True
+
+    def __post_init__(self) -> None:
+        if self.seq < 0 or self.source_index < 0 or self.echoed_map_id < 0:
+            raise ValueError("packet identifiers must be non-negative")
+        if not np.isfinite(self.source_time):
+            raise ValueError("source_time must be finite")
+        residual = np.asarray(self.residual, dtype=float).reshape(-1)
+        js_t = np.asarray(self.js_t_wrench, dtype=float).reshape(-1)
+        qd = np.asarray(self.qd_slave_actual, dtype=float).reshape(-1)
+        if residual.size == 0 or residual.shape != js_t.shape or residual.shape != qd.shape:
+            raise ValueError(
+                "residual, js_t_wrench and qd_slave_actual must have equal shapes"
+            )
+        object.__setattr__(
+            self, "residual", _readonly_vector(residual, residual.size, "residual")
+        )
+        object.__setattr__(
+            self, "wrench", _readonly_vector(self.wrench, 6, "wrench")
+        )
+        object.__setattr__(
+            self,
+            "js_t_wrench",
+            _readonly_vector(js_t, js_t.size, "js_t_wrench"),
+        )
+        object.__setattr__(
+            self,
+            "qd_slave_actual",
+            _readonly_vector(qd, qd.size, "qd_slave_actual"),
+        )
+
+    @property
+    def matched_input_hash(self) -> str:
+        """Hash proving that H3 matched conditions consumed one packet."""
+        return _payload_hash(
+            (
+                self.seq,
+                self.source_index,
+                self.source_time,
+                self.echoed_map_id,
+                self.wrench,
+                self.js_t_wrench,
+            )
+        )
+
+
+@dataclass(frozen=True)
+class FeedbackResult:
+    tau_master_raw: np.ndarray
+    tau_slave_source: np.ndarray
+    selected_map_id: int | None
+    matched_input_hash: str
+    valid: bool
+    reason: str
+
+
+def map_return_feedback(
+    *,
+    kind: MappingKind,
+    packet: ReturnPacket,
+    master_jacobian: np.ndarray,
+    maps: MapRegistry,
+    map_policy: MapPolicy = MapPolicy.CURRENT,
+) -> FeedbackResult:
+    """Map one immutable return packet to a raw master generalized torque.
+
+    ``MATCHED_DIFFERENTIAL_WRENCH`` and ``DIRECT_MASTER_JACOBIAN`` both use the
+    packet's one reconstructed wrench.  The former consumes its stored
+    ``J_s.T @ wrench`` value; the latter consumes ``wrench`` directly.
+    """
+    Jm = np.asarray(master_jacobian, dtype=float)
+    if Jm.ndim != 2 or Jm.shape[0] != 6:
+        raise ValueError("master_jacobian must have shape (6, nv_master)")
+    if not np.all(np.isfinite(Jm)):
+        raise ValueError("master_jacobian must contain only finite values")
+
+    matched_hash = packet.matched_input_hash
+    if not packet.valid:
+        return FeedbackResult(
+            tau_master_raw=np.zeros(Jm.shape[1], dtype=float),
+            tau_slave_source=np.zeros_like(packet.residual),
+            selected_map_id=None,
+            matched_input_hash=matched_hash,
+            valid=False,
+            reason="invalid_return_packet",
+        )
+
+    if kind is MappingKind.DIRECT_MASTER_JACOBIAN:
+        return FeedbackResult(
+            tau_master_raw=np.asarray(Jm.T @ packet.wrench, dtype=float),
+            tau_slave_source=packet.js_t_wrench.copy(),
+            selected_map_id=None,
+            matched_input_hash=matched_hash,
+            valid=True,
+            reason="ok",
+        )
+
+    if map_policy is MapPolicy.SOURCE_STAMPED:
+        selected = maps.get(packet.echoed_map_id)
+    else:
+        selected = maps.latest
+    A = selected.differential
+    if A.shape[0] != packet.residual.size or A.shape[1] != Jm.shape[1]:
+        raise ValueError(
+            "differential shape is incompatible with slave/master dimensions"
+        )
+    if not selected.valid:
+        return FeedbackResult(
+            tau_master_raw=np.zeros(Jm.shape[1], dtype=float),
+            tau_slave_source=np.zeros(A.shape[0], dtype=float),
+            selected_map_id=selected.map_id,
+            matched_input_hash=matched_hash,
+            valid=False,
+            reason="invalid_differential",
+        )
+
+    source = (
+        packet.residual
+        if kind is MappingKind.DIFFERENTIAL_RESIDUAL
+        else packet.js_t_wrench
+    )
+    return FeedbackResult(
+        tau_master_raw=np.asarray(A.T @ source, dtype=float),
+        tau_slave_source=source.copy(),
+        selected_map_id=selected.map_id,
+        matched_input_hash=matched_hash,
+        valid=True,
+        reason="ok",
+    )
+
+
+def normalized_actual_power_mismatch(
+    tau_master_raw: np.ndarray,
+    qd_master: np.ndarray,
+    tau_slave_source: np.ndarray,
+    qd_slave_actual: np.ndarray,
+    dt: float | np.ndarray,
+    *,
+    sigma_feedback: float = 1.0,
+    epsilon_energy: float = 1e-12,
+) -> float:
+    """Compute the trajectory-level H3 endpoint from source-aligned samples."""
+    tm = np.asarray(tau_master_raw, dtype=float)
+    qm = np.asarray(qd_master, dtype=float)
+    ts = np.asarray(tau_slave_source, dtype=float)
+    qs = np.asarray(qd_slave_actual, dtype=float)
+    if tm.ndim != 2 or tm.shape != qm.shape or ts.ndim != 2 or ts.shape != qs.shape:
+        raise ValueError("torque and velocity arrays must be paired 2-D arrays")
+    if tm.shape[0] != ts.shape[0]:
+        raise ValueError("master and slave arrays must have the same sample count")
+    delta = np.broadcast_to(np.asarray(dt, dtype=float), (tm.shape[0],))
+    if np.any(delta <= 0.0) or not np.all(np.isfinite(delta)):
+        raise ValueError("dt must contain finite positive values")
+    if epsilon_energy <= 0.0:
+        raise ValueError("epsilon_energy must be positive")
+
+    pm = np.einsum("ij,ij->i", tm, qm)
+    ps = float(sigma_feedback) * np.einsum("ij,ij->i", ts, qs)
+    numerator = float(np.sum(np.abs(pm - ps) * delta))
+    denominator = float(
+        0.5 * np.sum((np.abs(pm) + np.abs(ps)) * delta) + epsilon_energy
+    )
+    return numerator / denominator

code/core/haptic_render.py

@@ -0,0 +1,658 @@
+# haptic_render.py
+# -*- coding: utf-8 -*-
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+try:
+    import pinocchio as pin
+except ModuleNotFoundError:  # Pure supervisor tests do not require Pinocchio.
+    pin = None
+
+
+# ===== Final applied-port energy supervision =====
+
+@dataclass
+class TankParams:
+    E_min: float = 0.5
+    E_max: float = 20.0
+    alpha_floor: float = 0.0
+    alpha_ceil: float = 1.0
+    power_epsilon: float = 1e-12
+
+    # Legacy fields are kept so existing configuration/ablation code can load.
+    # They are deliberately not used for energy accounting: filtering, a power
+    # dead-zone, or alpha smoothing at this stage would make the tank account a
+    # different signal from the torque actually applied at the master port.
+    force_alpha: float = 0.2
+    vel_alpha: float = 0.2
+    alpha_smooth: float = 0.1
+    power_deadzone: float = 0.0
+
+
+@dataclass(frozen=True)
+class AppliedPortDiagnostics:
+    """Snapshot of one final-port projection/accounting step."""
+
+    tau_candidate: np.ndarray
+    tau_applied: np.ndarray
+    rho: float
+    E_before: float
+    E_preclip: float
+    E_after: float
+    candidate_power: float
+    power: float
+    fail_safe_active: bool
+
+
+class AppliedPortEnergySupervisor:
+    """Passivity supervisor at the final master haptic-torque port.
+
+    Positive ``tau_app @ qd_m`` is power delivered by the device to the
+    operator and therefore discharges the tank.  Negative power charges the
+    tank.  The candidate torque passed to :meth:`apply` must
+    already include all nominal scaling, filtering, rate limiting, and actuator
+    saturation.  The returned torque is final: downstream code must not filter,
+    rescale, rate-limit, or saturate it.
+
+    The projection is radial, ``tau_app = alpha * tau_candidate``.  This keeps a
+    previously saturated candidate inside its actuator limits while imposing
+    the exact one-step energy budget.  ``alpha_floor`` is never allowed to
+    override the energy-safe upper bound.
+    """
+
+    def __init__(self, tank: TankParams | None = None, E0: float = 2.0):
+        self.tp = TankParams() if tank is None else tank
+        if not np.isfinite(self.tp.E_min) or not np.isfinite(self.tp.E_max):
+            raise ValueError("E_min and E_max must be finite")
+        if self.tp.E_max < self.tp.E_min:
+            raise ValueError("E_max must be greater than or equal to E_min")
+        if not 0.0 <= self.tp.alpha_ceil <= 1.0:
+            raise ValueError("alpha_ceil must lie in [0, 1]")
+        if not 0.0 <= self.tp.alpha_floor <= self.tp.alpha_ceil:
+            raise ValueError("alpha_floor must lie in [0, alpha_ceil]")
+        if self.tp.power_epsilon < 0.0:
+            raise ValueError("power_epsilon must be non-negative")
+
+        self.E = float(np.clip(E0, self.tp.E_min, self.tp.E_max))
+        self.alpha = 1.0
+        self.last_alpha = 1.0
+        self.last_power = 0.0
+        self.last_energy_before = self.E
+        self.last_energy_after = self.E
+        self.last_tau_candidate = np.empty(0, dtype=float)
+        self.last_tau_applied = np.empty(0, dtype=float)
+        self.fail_safe_active = False
+        self.last_diagnostics: AppliedPortDiagnostics | None = None
+
+    def reset(self, E0: float | None = None):
+        if E0 is None:
+            E0 = self.tp.E_min
+        self.E = float(np.clip(E0, self.tp.E_min, self.tp.E_max))
+        self.alpha = 1.0
+        self.last_alpha = 1.0
+        self.last_power = 0.0
+        self.last_energy_before = self.E
+        self.last_energy_after = self.E
+        self.last_tau_candidate = np.empty(0, dtype=float)
+        self.last_tau_applied = np.empty(0, dtype=float)
+        self.fail_safe_active = False
+        self.last_diagnostics = None
+
+    def _record(self,
+                tau_candidate: np.ndarray,
+                tau_applied: np.ndarray,
+                rho: float,
+                E_before: float,
+                E_preclip: float,
+                E_after: float,
+                candidate_power: float,
+                power: float,
+                fail_safe_active: bool) -> AppliedPortDiagnostics:
+        """Record one result while retaining legacy scalar/state attributes."""
+        diagnostics = AppliedPortDiagnostics(
+            tau_candidate=tau_candidate.copy(),
+            tau_applied=tau_applied.copy(),
+            rho=float(rho),
+            E_before=float(E_before),
+            E_preclip=float(E_preclip),
+            E_after=float(E_after),
+            candidate_power=float(candidate_power),
+            power=float(power),
+            fail_safe_active=bool(fail_safe_active),
+        )
+        self.alpha = diagnostics.rho
+        self.last_alpha = diagnostics.rho
+        self.last_power = diagnostics.power
+        self.last_energy_before = diagnostics.E_before
+        self.last_energy_after = diagnostics.E_after
+        self.last_tau_candidate = diagnostics.tau_candidate.copy()
+        self.last_tau_applied = diagnostics.tau_applied.copy()
+        self.fail_safe_active = diagnostics.fail_safe_active
+        self.last_diagnostics = diagnostics
+        return diagnostics
+
+    def _zero_fail_safe(
+        self,
+        tau_candidate: np.ndarray,
+    ) -> tuple[np.ndarray, AppliedPortDiagnostics]:
+        tau_zero = np.zeros_like(tau_candidate, dtype=float)
+        diagnostics = self._record(
+            tau_candidate=tau_candidate,
+            tau_applied=tau_zero,
+            rho=0.0,
+            E_before=self.E,
+            E_preclip=self.E,
+            E_after=self.E,
+            candidate_power=0.0,
+            power=0.0,
+            fail_safe_active=True,
+        )
+        return tau_zero, diagnostics
+
+    def apply(self,
+              tau_raw: np.ndarray,
+              qd: np.ndarray,
+              dt: float) -> tuple[np.ndarray, AppliedPortDiagnostics]:
+        """Return the final safe torque and diagnostics, accounting exactly once.
+
+        ``tau_raw`` is the already shaped and actuator-limited candidate at the
+        master joint port.  It may come directly from a mapping such as
+        ``A.T @ tau_s`` or ``J_m.T @ F``.  ``qd`` is the velocity at that same
+        port.  The returned ``tau_app`` must be sent unchanged to the haptic
+        output (apart from an emergency zero-output fail-safe).
+        """
+        tau_candidate = np.asarray(tau_raw, dtype=float).reshape(-1,)
+        qd_m = np.asarray(qd, dtype=float).reshape(-1,)
+        if tau_candidate.shape != qd_m.shape:
+            raise ValueError(
+                "tau_raw and qd must have identical one-dimensional shapes"
+            )
+        if (
+            tau_candidate.size == 0
+            or not np.isfinite(dt)
+            or dt <= 0.0
+            or not np.all(np.isfinite(tau_candidate))
+            or not np.all(np.isfinite(qd_m))
+        ):
+            return self._zero_fail_safe(tau_candidate)
+
+        energy_before = self.E
+        nominal_power = float(np.dot(tau_candidate, qd_m))
+        if not np.isfinite(nominal_power):
+            return self._zero_fail_safe(tau_candidate)
+        rho = self.tp.alpha_ceil
+
+        # Only device-to-human power consumes stored energy.  alpha_floor is a
+        # preference, not a safety constraint, and cannot force an unsafe gain.
+        if nominal_power > 0.0:
+            available = max(0.0, energy_before - self.tp.E_min)
+            rho_energy = available / (nominal_power * dt)
+            rho = min(rho, max(0.0, rho_energy))
+
+        tau_app = rho * tau_candidate
+        applied_power = float(np.dot(tau_app, qd_m))
+
+        # This is the sole tank update.  It uses the exact torque returned to
+        # the caller and the exact master velocity supplied for this sample.
+        energy_preclip = energy_before - applied_power * dt
+        self.E = float(np.clip(energy_preclip, self.tp.E_min, self.tp.E_max))
+        diagnostics = self._record(
+            tau_candidate=tau_candidate,
+            tau_applied=tau_app,
+            rho=rho,
+            E_before=energy_before,
+            E_preclip=energy_preclip,
+            E_after=self.E,
+            candidate_power=nominal_power,
+            power=applied_power,
+            fail_safe_active=False,
+        )
+        return tau_app, diagnostics
+
+    def project_and_account(self,
+                            tau_candidate: np.ndarray,
+                            qd_m: np.ndarray,
+                            dt: float) -> tuple[float, np.ndarray]:
+        """Compatibility wrapper around :meth:`apply`.
+
+        Returns ``(alpha, tau_app)``.  The wrapper invokes ``apply`` exactly
+        once, so it cannot introduce a second tank update.
+        """
+        tau_app, diagnostics = self.apply(tau_candidate, qd_m, dt)
+        return diagnostics.rho, tau_app
+
+    def observe_master(self, *_args, **_kwargs):
+        """Reject the former second accounting path."""
+        raise RuntimeError(
+            "observe_master() is disabled: use apply() exactly once "
+            "at the final applied master-torque port"
+        )
+
+    def enforce(self, *_args, **_kwargs):
+        """Reject wrench/twist-port enforcement from the former implementation."""
+        raise RuntimeError(
+            "enforce(CF, VC, dt) is disabled: passivity is enforced on final "
+            "master tau_app and qd_m via apply()"
+        )
+
+
+# Backward-compatible class name for existing imports.
+EnergyTankPOPC = AppliedPortEnergySupervisor
+
+
+# ===== 主端胸腔雅可比 =====
+
+class ChestJacobianMaster:
+    def __init__(self, model: pin.Model,
+                 chest_frame_name: str,
+                 ee_frame_name: str):
+        if pin is None:
+            raise ModuleNotFoundError(
+                "Pinocchio is required to construct ChestJacobianMaster"
+            )
+        self.model = model
+        self.data  = model.createData()
+        self.fid_C  = model.getFrameId(chest_frame_name)
+        self.fid_EE = model.getFrameId(ee_frame_name)
+
+    def chest_jacobian(self, q_m, qd_m):
+        pin.forwardKinematics(self.model, self.data, q_m, qd_m)
+        pin.updateFramePlacements(self.model, self.data)
+
+        # LOCAL_WORLD_ALIGNED expresses the twist at the EE point in axes
+        # parallel to the world frame.  The spatial-vector convention used by
+        # this project is [linear; angular].
+        Jw = pin.computeFrameJacobian(self.model, self.data,
+                                      q_m, self.fid_EE,
+                                      pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
+
+        # Rotate both vector blocks into chest axes, but do not translate the
+        # reference point: the wrench paired with this Jacobian still acts at
+        # the EE point.  A full spatial adjoint here would introduce a spurious
+        # moment-arm term between the chest origin and EE point.
+        oTC = self.data.oMf[self.fid_C]  # ^wT_C
+        R_cw = oTC.rotation.T
+        rotate_axes = np.zeros((6, 6))
+        rotate_axes[:3, :3] = R_cw
+        rotate_axes[3:, 3:] = R_cw
+        CJ_m = rotate_axes @ Jw
+        return CJ_m
+
+
+# ===== 主端力反馈渲染 =====
+
+class HapticRenderer:
+    """Render interaction wrench as final, passivity-supervised master torque.
+
+    The haptic path has one strict ordering:
+
+    ``J_m.T @ F`` (direct baseline)
+      -> optional nominal low-pass
+      -> nominal sign/strength scaling
+      -> torque-rate limiting
+      -> actuator torque saturation
+      -> final applied-port energy projection and single accounting update.
+
+    No filtering or limiting is performed after the energy projection.  The
+    second returned torque is therefore the exact ``tau_app`` used in
+    ``tau_app @ qd_m`` for the tank update.
+    """
+
+    def __init__(self,
+                 master_model: pin.Model,
+                 chest_frame_name: str,
+                 ee_frame_name: str,
+                 feedback_strength: float,
+                 E_init: float,
+                 E_max: float,
+                 alpha_floor: float,
+                 alpha_ceil: float,
+                 E0: float = 2.0,
+                 torque_limit: float | np.ndarray | None = None,
+                 torque_rate_limit: float | np.ndarray | None = None,
+                 tau_filter_alpha: float = 0.2):
+        # 约定 feedback_strength > 0，反作用时乘上一个负号
+        self.feedback_strength = float(feedback_strength)
+        if not np.isfinite(self.feedback_strength):
+            raise ValueError("feedback_strength must be finite")
+
+        self.CJ_master = ChestJacobianMaster(master_model,
+                                             chest_frame_name,
+                                             ee_frame_name)
+
+        tank_params = TankParams(
+            E_min=E_init,
+            E_max=E_max,
+            alpha_floor=alpha_floor,
+            alpha_ceil=alpha_ceil,
+        )
+        self.tank = AppliedPortEnergySupervisor(tank_params, E0)
+
+        # Nominal shaping state.  All of this is upstream of the energy
+        # projection.  Public tau_alpha is retained for existing scripts.
+        self.tau_alpha = float(tau_filter_alpha)
+        if not 0.0 <= self.tau_alpha <= 1.0:
+            raise ValueError("tau_filter_alpha must lie in [0, 1]")
+        self.torque_limit = torque_limit
+        self.torque_rate_limit = torque_rate_limit
+        self._tau_fb_state = np.zeros(master_model.nv)
+        self._tau_applied_prev = np.zeros(master_model.nv)
+        self.last_rate_limit_active = False
+        self.last_torque_saturation_active = False
+
+    def reset_tank(self, E0: float = None):
+        self.tank.reset(E0)
+        self._tau_fb_state[:] = 0.0
+        self._tau_applied_prev[:] = 0.0
+        self.last_rate_limit_active = False
+        self.last_torque_saturation_active = False
+
+    # ---------- 主端逆动力学：计算 tau_master_current ----------
+
+    def _inverse_dynamics(self,
+                          q_m: np.ndarray,
+                          qd_m: np.ndarray,
+                          qdd_m: np.ndarray,
+                          tau_ff_fric: np.ndarray | None = None) -> np.ndarray:
+        """
+        主端动力学模型：
+            τ = M(q) qdd + C(q,qd) qd + g(q) + τ_ff_fric
+        """
+        q_m  = np.asarray(q_m,  dtype=float).reshape(-1,)
+        qd_m = np.asarray(qd_m, dtype=float).reshape(-1,)
+        qdd_m = np.asarray(qdd_m, dtype=float).reshape(-1,)
+        if tau_ff_fric is not None:
+            tau_ff_fric = np.asarray(tau_ff_fric, dtype=float).reshape(-1,)
+
+        model = self.CJ_master.model
+        data  = self.CJ_master.data
+
+        # 惯量矩阵 M(q)
+        M = pin.crba(model, data, q_m)
+        M = (M + M.T) - np.diag(M.diagonal())   # 数值对称化
+
+        # 非线性项（科氏/离心 + 重力）
+        nle = pin.nonLinearEffects(model, data, q_m, qd_m)  # = C(q,qd)qd + g(q)
+        g   = pin.computeGeneralizedGravity(model, data, q_m)
+        Cqd = nle - g
+
+        tau = M @ qdd_m + Cqd + g
+        if tau_ff_fric is not None:
+            tau = tau + tau_ff_fric
+        return tau
+
+    def compute_tau_master_current(self,
+                                   q_m: np.ndarray,
+                                   qd_m: np.ndarray,
+                                   qdd_m: np.ndarray,
+                                   tau_ff_fric: np.ndarray | None = None) -> np.ndarray:
+        """
+        对外接口：给定主端 (q, qd, qdd)，计算
+            τ_master_current = M qdd + C qd + g + τ_ff_fric
+        """
+        return self._inverse_dynamics(q_m, qd_m, qdd_m, tau_ff_fric)
+
+    # ---------- 从端交互力 → 主端反馈扭矩 ----------
+
+    @staticmethod
+    def _symmetric_limit_vector(value: float | np.ndarray | None,
+                                size: int,
+                                name: str) -> np.ndarray:
+        if value is None:
+            return np.full(size, np.inf, dtype=float)
+        limit = np.asarray(value, dtype=float)
+        if limit.ndim == 0:
+            limit = np.full(size, float(limit), dtype=float)
+        else:
+            limit = limit.reshape(-1,)
+            if limit.size != size:
+                raise ValueError(f"{name} must be scalar or have {size} entries")
+        if np.any(np.isnan(limit)) or np.any(limit < 0.0):
+            raise ValueError(f"{name} must contain non-negative values")
+        return limit
+
+    def direct_from_CF(self,
+                       q_m: np.ndarray,
+                       qd_m: np.ndarray,
+                       CF_int_slave_C: np.ndarray) -> np.ndarray:
+        """Return the unmodified baseline mapping ``J_m.T @ F``.
+
+        This interface intentionally applies no sign convention, nominal gain,
+        filter, rate limit, saturation, or energy supervision.
+        """
+        CF = np.asarray(CF_int_slave_C, dtype=float).reshape(6,)
+        CJ_m = self.CJ_master.chest_jacobian(q_m, qd_m)
+        return np.asarray(CJ_m.T @ CF, dtype=float).reshape(-1,)
+
+    # Explicitly named alias for experiment/baseline code.
+    render_direct_from_CF = direct_from_CF
+
+    def _compute_feedback_tau(self,
+                              q_m: np.ndarray,
+                              qd_m: np.ndarray,
+                              CF_int_slave_C: np.ndarray,
+                              V_slave_C: np.ndarray | None,
+                              dt: float):
+        """Legacy extension hook.
+
+        The base implementation returns the direct mapping and ``None`` to
+        request final applied-port supervision.  Existing no-tank subclasses
+        that override this method and return a numeric alpha continue to bypass
+        only the energy projection; all upstream nominal actuator shaping still
+        applies.  ``V_slave_C`` is retained solely for call compatibility and is
+        never used for tank accounting.
+        """
+        del V_slave_C, dt
+        return self.direct_from_CF(q_m, qd_m, CF_int_slave_C), None
+
+    def _shape_and_supervise(self,
+                             tau_direct: np.ndarray,
+                             qd_m: np.ndarray,
+                             dt: float,
+                             bypass_energy_projection: bool = False
+                             ) -> tuple[np.ndarray, float]:
+        """Apply the strictly ordered haptic output pipeline."""
+        tau_saturated, valid = self._shape_candidate(tau_direct, qd_m, dt)
+        if not valid:
+            tau_zero, diagnostics = self.tank.apply(tau_direct, qd_m, dt)
+            self._tau_applied_prev = tau_zero.copy()
+            return tau_zero, diagnostics.rho
+
+        # Final energy projection and the only accounting update.
+        if bypass_energy_projection:
+            tau_app = tau_saturated.copy()
+            alpha = 1.0
+        else:
+            tau_app, diagnostics = self.tank.apply(tau_saturated, qd_m, dt)
+            alpha = diagnostics.rho
+
+        # Nothing may modify tau_app after projection.  Only state capture and
+        # addition of the independent inverse-dynamics command occur downstream.
+        self.commit_applied(tau_app)
+        return tau_app, float(alpha)
+
+    def _shape_candidate(
+        self,
+        tau_direct: np.ndarray,
+        qd_m: np.ndarray,
+        dt: float,
+    ) -> tuple[np.ndarray, bool]:
+        """Return the final upstream candidate without applying a supervisor.
+
+        This split is used by the formal PO/PC baseline.  The caller must pass
+        the candidate through exactly one supervisor and then invoke
+        :meth:`commit_applied`; no additional filtering or limiting is allowed.
+        """
+        tau_direct = np.asarray(tau_direct, dtype=float).reshape(-1,)
+        qd_m = np.asarray(qd_m, dtype=float).reshape(-1,)
+        if tau_direct.shape != qd_m.shape:
+            raise ValueError("direct haptic torque and qd_m must have equal shapes")
+
+        if self._tau_fb_state.shape != tau_direct.shape:
+            self._tau_fb_state = np.zeros_like(tau_direct)
+        if self._tau_applied_prev.shape != tau_direct.shape:
+            self._tau_applied_prev = np.zeros_like(tau_direct)
+
+        # Invalid samples must not contaminate nominal filter/rate-limit state.
+        if (
+            not np.isfinite(dt)
+            or dt <= 0.0
+            or not np.all(np.isfinite(tau_direct))
+            or not np.all(np.isfinite(qd_m))
+        ):
+            self.last_rate_limit_active = False
+            self.last_torque_saturation_active = False
+            return np.zeros_like(tau_direct), False
+
+        # 1) Optional nominal torque filtering.
+        a = float(self.tau_alpha)
+        if not 0.0 <= a <= 1.0:
+            raise ValueError("tau_alpha must lie in [0, 1]")
+        self._tau_fb_state = (1.0 - a) * self._tau_fb_state + a * tau_direct
+
+        # 2) Nominal sign and strength scaling.
+        tau_nominal = -self.feedback_strength * self._tau_fb_state
+
+        # 3) Per-joint rate limiting relative to the previously applied output.
+        rate_limit = self._symmetric_limit_vector(
+            self.torque_rate_limit, tau_nominal.size, "torque_rate_limit"
+        )
+        max_delta = rate_limit * dt
+        tau_rate_limited = np.clip(
+            tau_nominal,
+            self._tau_applied_prev - max_delta,
+            self._tau_applied_prev + max_delta,
+        )
+        self.last_rate_limit_active = bool(
+            np.any(np.abs(tau_rate_limited - tau_nominal) > 1e-12)
+        )
+
+        # 4) Per-joint symmetric actuator saturation.
+        torque_limit = self._symmetric_limit_vector(
+            self.torque_limit, tau_nominal.size, "torque_limit"
+        )
+        tau_saturated = np.clip(tau_rate_limited, -torque_limit, torque_limit)
+        self.last_torque_saturation_active = bool(
+            np.any(np.abs(tau_saturated - tau_rate_limited) > 1e-12)
+        )
+        return tau_saturated, True
+
+    def shape_mapped_reaction_candidate(
+        self,
+        tau_environment_on_master: np.ndarray,
+        qd_m: np.ndarray,
+        dt: float,
+    ) -> tuple[np.ndarray, bool]:
+        """Prepare the common upstream candidate for an external supervisor."""
+        reaction = np.asarray(
+            tau_environment_on_master,
+            dtype=float,
+        ).reshape(-1)
+        return self._shape_candidate(-reaction, qd_m, dt)
+
+    def commit_applied(self, tau_applied: np.ndarray) -> None:
+        """Record the exact supervisor output used at the master port."""
+        applied = np.asarray(tau_applied, dtype=float).reshape(-1)
+        if applied.shape != self._tau_applied_prev.shape:
+            raise ValueError("tau_applied has an unexpected shape")
+        if not np.all(np.isfinite(applied)):
+            raise ValueError("tau_applied must contain only finite values")
+        self._tau_applied_prev = applied.copy()
+
+    # ---------- 高层接口：算 tau_master_current + 反馈 ----------
+
+    def render_mapped_reaction(
+        self,
+        tau_environment_on_master: np.ndarray,
+        qd_m: np.ndarray,
+        dt: float,
+        *,
+        supervise_energy: bool = True,
+    ) -> tuple[np.ndarray, float]:
+        """Shape an arbitrary generalized reaction such as ``A.T @ tau_s``.
+
+        ``tau_environment_on_master`` already has the desired physical sign:
+        it is the environment-on-device reaction and should oppose penetration.
+        The historical renderer pipeline stores robot-on-environment action
+        internally and inserts its own leading minus sign, so the conversion is
+        performed exactly once here.  The returned torque is the final applied
+        haptic output; callers must not filter or limit it downstream.
+        """
+        reaction = np.asarray(
+            tau_environment_on_master,
+            dtype=float,
+        ).reshape(-1,)
+        return self._shape_and_supervise(
+            -reaction,
+            qd_m,
+            dt,
+            bypass_energy_projection=not supervise_energy,
+        )
+
+    def render_tau(self,
+                   q_m: np.ndarray,
+                   qd_m: np.ndarray,
+                   qdd_m: np.ndarray,
+                   CF_int_slave_C: np.ndarray,
+                   V_slave_C: np.ndarray | None,
+                   dt: float,
+                   tau_ff_fric: np.ndarray | None = None,
+                   ):
+        """
+        高层接口：给定 (q, qd, qdd) 和从端交互力，返回最终主端总扭矩。
+
+        输入：
+        - q_m, qd_m, qdd_m: 主端关节位置/速度/加速度（用于逆动力学）
+        - CF_int_slave_C: 从端 InteractionEstimator 估计出的 C F_int（6×1）
+        - V_slave_C: 仅为旧调用兼容而保留；能量只按主端
+          tau_app^T qd_m 计算
+        - tau_ff_fric: 主端摩擦前馈（可为 None）
+        - dt: 控制周期（秒）
+
+        输出：
+        - tau_cmd_m: 主端最终下发的总扭矩 = tau_master_current + tau_app
+        - tau_app: 最终反馈扭矩，也是能量记账使用的精确力矩
+        - alpha: 最终能量投影的径向缩放系数
+        """
+        # 1) 主端逆动力学扭矩
+        tau_master_current = self._inverse_dynamics(q_m, qd_m, qdd_m, tau_ff_fric)
+        tau_master_current = np.asarray(tau_master_current, dtype=float).reshape(-1,)
+
+        # 2) Render and account the haptic contribution exactly once.
+        tau_app, alpha = self.render_from_CF(
+            q_m, qd_m,
+            CF_int_slave_C=CF_int_slave_C,
+            V_slave_C=V_slave_C,
+            dt=dt,
+        )
+
+        # 3) tau_app is not modified after projection.
+        tau_cmd_m = tau_master_current + tau_app
+        return tau_cmd_m, tau_app, alpha
+
+    # 兼容旧接口：只关心反馈扭矩
+    def render_from_CF(self,
+                       q_m: np.ndarray,
+                       qd_m: np.ndarray,
+                       CF_int_slave_C: np.ndarray,
+                       V_slave_C: np.ndarray | None,
+                       dt: float):
+        """
+        兼容旧接口：返回最终施加的反馈扭矩与能量投影系数。
+        """
+        tau_direct, legacy_alpha = self._compute_feedback_tau(
+            q_m,
+            qd_m,
+            CF_int_slave_C,
+            V_slave_C,
+            dt,
+        )
+        return self._shape_and_supervise(
+            tau_direct,
+            qd_m,
+            dt,
+            bypass_energy_projection=legacy_alpha is not None,
+        )

code/core/interaction_estimater.py

@@ -0,0 +1,299 @@
+from __future__ import annotations
+
+from typing import Optional
+
+import numpy as np
+import pinocchio as pin
+
+from .wrench_solver import WrenchSolveResult, WrenchSolver
+
+
+def checked_frame_id(model: pin.Model, frame_name: str) -> int:
+    """Resolve a Pinocchio frame name and reject its not-found sentinel."""
+    if not isinstance(frame_name, str) or not frame_name:
+        raise ValueError("frame_name must be a non-empty string")
+
+    frame_id = int(model.getFrameId(frame_name))
+    if frame_id < 0 or frame_id >= model.nframes:
+        available = ", ".join(frame.name for frame in model.frames)
+        raise ValueError(
+            f"Frame not found: {frame_name!r}. "
+            f"Expected an id in [0, {model.nframes}), got {frame_id}. "
+            f"Available frames: {available}"
+        )
+    return frame_id
+
+
+def checked_joint_id(model: pin.Model, joint_name: str) -> int:
+    """Resolve a movable joint name and reject universe/not-found sentinels."""
+    if not isinstance(joint_name, str) or not joint_name:
+        raise ValueError("joint_name must be a non-empty string")
+
+    joint_id = int(model.getJointId(joint_name))
+    if joint_id <= 0 or joint_id >= model.njoints:
+        available = ", ".join(model.names[1:])
+        raise ValueError(
+            f"Movable joint not found: {joint_name!r}. "
+            f"Expected an id in [1, {model.njoints}), got {joint_id}. "
+            f"Available movable joints: {available}"
+        )
+    return joint_id
+
+
+def _as_vector(value, size: int, name: str) -> np.ndarray:
+    vector = np.asarray(value, dtype=float).reshape(-1)
+    if vector.shape != (size,):
+        raise ValueError(f"{name} must have shape ({size},), got {vector.shape}")
+    if not np.all(np.isfinite(vector)):
+        raise ValueError(f"{name} must contain only finite values")
+    return vector
+
+
+def _as_sample_matrix(value, width: int, name: str) -> np.ndarray:
+    samples = np.asarray(value, dtype=float)
+    if samples.ndim == 1:
+        samples = samples.reshape(1, -1)
+    if samples.ndim != 2 or samples.shape[1] != width:
+        raise ValueError(
+            f"{name} must have shape (n_samples, {width}), got {samples.shape}"
+        )
+    if samples.shape[0] == 0:
+        raise ValueError(f"{name} must contain at least one sample")
+    if not np.all(np.isfinite(samples)):
+        raise ValueError(f"{name} must contain only finite values")
+    return samples
+
+
+class InteractionEstimator:
+    """
+    Estimate an external wrench at the end-effector point.
+
+    Conventions are explicit throughout this class:
+
+    * twists are ``[linear_velocity; angular_velocity]``;
+    * wrenches are ``[force; moment]``;
+    * both are expressed along the chest-frame axes, but at the EE point;
+    * ``tau_int = tau_meas - tau_model - tau_bias``.
+
+    ``estimate`` keeps the original three-value return contract. Its first
+    result is the bias-corrected residual. The raw and corrected residuals are
+    also available through ``last_tau_residual_raw`` and
+    ``last_tau_residual_corrected``.
+    """
+
+    def __init__(
+        self,
+        model: pin.Model,
+        chest_frame_name: str,
+        ee_frame_name: str,
+        lambda_damp: float = 1e-3,
+        wrench_solver: Optional[WrenchSolver] = None,
+    ):
+        damping = float(lambda_damp)
+        if not np.isfinite(damping) or damping <= 0.0:
+            raise ValueError("lambda_damp must be a finite positive scalar")
+
+        self.model = model
+        self.data = model.createData()
+        self.lambda_damp = damping
+        self.wrench_solver = wrench_solver
+        self.fid_C = checked_frame_id(model, chest_frame_name)
+        self.fid_EE = checked_frame_id(model, ee_frame_name)
+
+        self._tau_bias = np.zeros(model.nv, dtype=float)
+        self._last_tau_residual_raw: Optional[np.ndarray] = None
+        self._last_tau_residual_corrected: Optional[np.ndarray] = None
+        self._last_wrench_solve: Optional[WrenchSolveResult] = None
+
+    @staticmethod
+    def _rotation6(rotation: np.ndarray) -> np.ndarray:
+        """Apply one 3-D rotation to both linear and angular blocks."""
+        rotation = np.asarray(rotation, dtype=float)
+        if rotation.shape != (3, 3):
+            raise ValueError(f"rotation must have shape (3, 3), got {rotation.shape}")
+        rotation6 = np.zeros((6, 6), dtype=float)
+        rotation6[:3, :3] = rotation
+        rotation6[3:, 3:] = rotation
+        return rotation6
+
+    @property
+    def tau_bias(self) -> np.ndarray:
+        """Current no-contact joint-torque bias (copy)."""
+        return self._tau_bias.copy()
+
+    @property
+    def last_tau_residual_raw(self) -> Optional[np.ndarray]:
+        """Latest ``tau_meas - tau_model`` sample, before bias removal."""
+        if self._last_tau_residual_raw is None:
+            return None
+        return self._last_tau_residual_raw.copy()
+
+    @property
+    def last_tau_residual_corrected(self) -> Optional[np.ndarray]:
+        """Latest raw residual minus the calibrated bias."""
+        if self._last_tau_residual_corrected is None:
+            return None
+        return self._last_tau_residual_corrected.copy()
+
+    @property
+    def last_wrench_solve(self) -> Optional[WrenchSolveResult]:
+        """Latest formal scaled-solver result, if one was configured."""
+        return self._last_wrench_solve
+
+    def calibrate_bias(self, tau_residual_raw_samples) -> np.ndarray:
+        """
+        Calibrate a constant joint-torque bias from offline no-contact samples.
+
+        Parameters
+        ----------
+        tau_residual_raw_samples:
+            One sample with shape ``(nv,)`` or a batch with shape
+            ``(n_samples, nv)``. Every row must already be the raw residual
+            ``tau_meas - tau_model`` collected under a no-contact condition.
+
+        Returns
+        -------
+        numpy.ndarray
+            A copy of the calibrated mean bias.
+        """
+        samples = _as_sample_matrix(
+            tau_residual_raw_samples, self.model.nv, "tau_residual_raw_samples"
+        )
+        self._tau_bias = np.mean(samples, axis=0)
+        return self.tau_bias
+
+    def calibrate_bias_from_measurements(
+        self,
+        q_samples,
+        qd_samples,
+        qdd_samples,
+        tau_meas_samples,
+        tau_ff_fric_samples=None,
+    ) -> np.ndarray:
+        """
+        Compute and calibrate bias from an offline no-contact measurement batch.
+
+        Each argument is a row-major sample matrix. ``tau_ff_fric_samples`` is
+        optional and, when supplied, must have the same ``(n_samples, nv)``
+        shape as the velocity/torque batches.
+        """
+        q_batch = _as_sample_matrix(q_samples, self.model.nq, "q_samples")
+        qd_batch = _as_sample_matrix(qd_samples, self.model.nv, "qd_samples")
+        qdd_batch = _as_sample_matrix(qdd_samples, self.model.nv, "qdd_samples")
+        tau_batch = _as_sample_matrix(
+            tau_meas_samples, self.model.nv, "tau_meas_samples"
+        )
+
+        sample_count = q_batch.shape[0]
+        batches = (qd_batch, qdd_batch, tau_batch)
+        if any(batch.shape[0] != sample_count for batch in batches):
+            raise ValueError("all calibration batches must have the same sample count")
+
+        friction_batch = None
+        if tau_ff_fric_samples is not None:
+            friction_batch = _as_sample_matrix(
+                tau_ff_fric_samples, self.model.nv, "tau_ff_fric_samples"
+            )
+            if friction_batch.shape[0] != sample_count:
+                raise ValueError(
+                    "tau_ff_fric_samples must match the calibration sample count"
+                )
+
+        residuals = np.empty((sample_count, self.model.nv), dtype=float)
+        for sample_index in range(sample_count):
+            friction = (
+                None if friction_batch is None else friction_batch[sample_index]
+            )
+            tau_model = self._tau_model(
+                q_batch[sample_index],
+                qd_batch[sample_index],
+                qdd_batch[sample_index],
+                friction,
+            )
+            residuals[sample_index] = tau_batch[sample_index] - tau_model
+        return self.calibrate_bias(residuals)
+
+    def clear_bias(self) -> None:
+        """Clear the calibrated joint-torque bias."""
+        self._tau_bias.fill(0.0)
+
+    def _chest_jacobian(self, q, qd):
+        """
+        Return the EE-point Jacobian expressed along chest-frame axes.
+
+        ``LOCAL_WORLD_ALIGNED`` is essential here: unlike ``WORLD``, its
+        translational rows are the linear velocity of the EE origin. Rotating
+        the two 3-D blocks changes only their coordinate axes; no translational
+        adjoint term is used, so the wrench/twist reference point stays at EE.
+        """
+        q = _as_vector(q, self.model.nq, "q")
+        qd = _as_vector(qd, self.model.nv, "qd")
+
+        pin.forwardKinematics(self.model, self.data, q, qd)
+        pin.updateFramePlacements(self.model, self.data)
+
+        jacobian_lwa = pin.computeFrameJacobian(
+            self.model,
+            self.data,
+            q,
+            self.fid_EE,
+            pin.ReferenceFrame.LOCAL_WORLD_ALIGNED,
+        )
+
+        rotation_world_from_chest = self.data.oMf[self.fid_C].rotation
+        rotation_chest_from_world = rotation_world_from_chest.T
+        return self._rotation6(rotation_chest_from_world) @ jacobian_lwa
+
+    def _tau_model(self, q, qd, qdd, tau_ff_fric=None):
+        """Return ``M qdd + C qd + g + tau_ff_fric``."""
+        q = _as_vector(q, self.model.nq, "q")
+        qd = _as_vector(qd, self.model.nv, "qd")
+        qdd = _as_vector(qdd, self.model.nv, "qdd")
+
+        mass_matrix = pin.crba(self.model, self.data, q)
+        mass_matrix = (
+            mass_matrix + mass_matrix.T - np.diag(mass_matrix.diagonal())
+        )
+        nonlinear = pin.nonLinearEffects(self.model, self.data, q, qd)
+        tau_model = mass_matrix @ qdd + nonlinear
+
+        if tau_ff_fric is not None:
+            tau_model = tau_model + _as_vector(
+                tau_ff_fric, self.model.nv, "tau_ff_fric"
+            )
+        return tau_model
+
+    def estimate(self, q, qd, qdd, tau_meas, tau_ff_fric=None):
+        """
+        Estimate the bias-corrected joint residual and chest-axis EE wrench.
+
+        A fixed damped least-squares solve is used:
+
+        ``F = (J J.T + lambda**2 I)^-1 J tau_int``.
+        """
+        tau_meas = _as_vector(tau_meas, self.model.nv, "tau_meas")
+        tau_model = self._tau_model(q, qd, qdd, tau_ff_fric)
+
+        tau_residual_raw = tau_meas - tau_model
+        tau_residual_corrected = tau_residual_raw - self._tau_bias
+        self._last_tau_residual_raw = tau_residual_raw.copy()
+        self._last_tau_residual_corrected = tau_residual_corrected.copy()
+
+        chest_jacobian = self._chest_jacobian(q, qd)
+        if self.wrench_solver is None:
+            normal_matrix = chest_jacobian @ chest_jacobian.T
+            normal_matrix = normal_matrix + (
+                self.lambda_damp**2
+            ) * np.eye(6, dtype=float)
+            wrench_chest = np.linalg.solve(
+                normal_matrix, chest_jacobian @ tau_residual_corrected
+            )
+            self._last_wrench_solve = None
+        else:
+            solve = self.wrench_solver.solve(
+                chest_jacobian, tau_residual_corrected
+            )
+            wrench_chest = solve.wrench.copy()
+            self._last_wrench_solve = solve
+
+        return tau_residual_corrected, wrench_chest, chest_jacobian

code/core/model_contract.py

@@ -0,0 +1,233 @@
+"""Canonical model contract for the heterogeneous teleoperation prototype.
+
+The repository contains several historical URDF/config combinations.  This
+module defines the only combination used by the new simulation:
+
+* master: ``config/master_7dof.urdf``
+* slave: ``config/real_slave_7dof.urdf``
+
+The physical slave URDF currently ends at the final wrist body and does not
+contain a calibrated tool/force-sensor frame.  For simulation only, an
+operational frame named ``R_EE_SIM`` is added at a documented fixed offset.
+Hardware experiments must replace that offset with the measured TCP transform.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Iterable, Sequence
+
+import numpy as np
+import pinocchio as pin
+
+
+CODE_ROOT = Path(__file__).resolve().parents[1]
+CONFIG_ROOT = CODE_ROOT / "config"
+MASTER_URDF = CONFIG_ROOT / "master_7dof.urdf"
+SLAVE_URDF = CONFIG_ROOT / "real_slave_7dof.urdf"
+
+MASTER_JOINT_NAMES = (
+    "master_shoulder_pitch_joint",
+    "master_shoulder_yaw_joint",
+    "master_shoulder_roll_joint",
+    "master_elbow_flex_joint",
+    "master_wrist_roll_joint",
+    "master_wrist_yaw_joint",
+    "master_wrist_pitch_joint",
+)
+
+SLAVE_JOINT_NAMES = (
+    "R_SHOULDER_P",
+    "R_SHOULDER_R",
+    "R_SHOULDER_Y",
+    "R_ELBOW_R",
+    "R_WRIST_P",
+    "R_WRIST_Y",
+    "R_WRIST_R",
+)
+
+MASTER_FRAMES = {
+    "base": "master_base",
+    "shoulder": "master_shoulder",
+    "elbow": "master_forearm",
+    "wrist": "master_wrist",
+    "ee": "master_ee",
+}
+
+SLAVE_FRAMES = {
+    "base": "PELVIS_S",
+    "shoulder": "R_SHOULDER_R_S",
+    "elbow": "R_ELBOW_R_S",
+    "wrist": "R_WRIST_R_S",
+    "ee": "R_EE_SIM",
+}
+
+# Approximate simulated TCP taken from the terminal marker in right_arm.mjcf.
+# This is deliberately not presented as a physical calibration.
+SLAVE_SIM_TCP_OFFSET = np.array([-0.01212, -0.17655, 0.07506], dtype=float)
+
+
+@dataclass(frozen=True)
+class TeleoperationModels:
+    master: pin.Model
+    slave: pin.Model
+
+
+def require_frame(model: pin.Model, name: str) -> int:
+    """Resolve a frame and reject Pinocchio's not-found sentinel."""
+    fid = int(model.getFrameId(name))
+    if fid < 0 or fid >= model.nframes:
+        raise ValueError(
+            f"Frame {name!r} not found in model {model.name!r}; "
+            f"available={[frame.name for frame in model.frames]}"
+        )
+    return fid
+
+
+def require_joint(model: pin.Model, name: str) -> int:
+    """Resolve an actuated joint and reject universe/not-found sentinels."""
+    jid = int(model.getJointId(name))
+    if jid <= 0 or jid >= model.njoints:
+        raise ValueError(
+            f"Joint {name!r} not found in model {model.name!r}; "
+            f"available={list(model.names)[1:]}"
+        )
+    return jid
+
+
+def joint_q_indices(model: pin.Model, names: Sequence[str]) -> np.ndarray:
+    indices = []
+    for name in names:
+        jid = require_joint(model, name)
+        joint = model.joints[jid]
+        if joint.nq != 1 or joint.nv != 1:
+            raise ValueError(f"Expected a scalar revolute joint, got {name!r}")
+        indices.append(int(joint.idx_q))
+    return np.asarray(indices, dtype=int)
+
+
+def joint_v_indices(model: pin.Model, names: Sequence[str]) -> np.ndarray:
+    indices = []
+    for name in names:
+        jid = require_joint(model, name)
+        joint = model.joints[jid]
+        if joint.nq != 1 or joint.nv != 1:
+            raise ValueError(f"Expected a scalar revolute joint, got {name!r}")
+        indices.append(int(joint.idx_v))
+    return np.asarray(indices, dtype=int)
+
+
+def validate_contract(
+    model: pin.Model,
+    joint_names: Sequence[str],
+    frame_names: Iterable[str],
+) -> None:
+    if model.nq != 7 or model.nv != 7:
+        raise ValueError(
+            f"Expected a fixed-base 7-DoF model, got nq={model.nq}, nv={model.nv}"
+        )
+    joint_q_indices(model, joint_names)
+    for frame_name in frame_names:
+        require_frame(model, frame_name)
+
+
+def add_fixed_operational_frame(
+    model: pin.Model,
+    *,
+    name: str,
+    parent_frame_name: str,
+    translation: np.ndarray,
+    rotation: np.ndarray | None = None,
+) -> int:
+    """Add an operational frame at a transform relative to an existing frame."""
+    existing = int(model.getFrameId(name))
+    if 0 <= existing < model.nframes:
+        return existing
+
+    parent_fid = require_frame(model, parent_frame_name)
+    parent = model.frames[parent_fid]
+    relative = pin.SE3(
+        np.eye(3) if rotation is None else np.asarray(rotation, dtype=float),
+        np.asarray(translation, dtype=float).reshape(3),
+    )
+    placement_in_parent_joint = parent.placement * relative
+    return int(
+        model.addFrame(
+            pin.Frame(
+                name,
+                parent.parentJoint,
+                parent_fid,
+                placement_in_parent_joint,
+                pin.FrameType.OP_FRAME,
+            )
+        )
+    )
+
+
+def load_models(*, add_simulated_tcp: bool = True) -> TeleoperationModels:
+    """Load and validate the canonical master/slave dynamics models."""
+    master = pin.buildModelFromUrdf(str(MASTER_URDF))
+    slave = pin.buildModelFromUrdf(str(SLAVE_URDF))
+
+    if add_simulated_tcp:
+        add_fixed_operational_frame(
+            slave,
+            name=SLAVE_FRAMES["ee"],
+            parent_frame_name=SLAVE_FRAMES["wrist"],
+            translation=SLAVE_SIM_TCP_OFFSET,
+        )
+
+    validate_contract(master, MASTER_JOINT_NAMES, MASTER_FRAMES.values())
+    validate_contract(slave, SLAVE_JOINT_NAMES, SLAVE_FRAMES.values())
+    return TeleoperationModels(master=master, slave=slave)
+
+
+def finite_joint_limits(
+    model: pin.Model,
+    joint_names: Sequence[str],
+) -> tuple[np.ndarray, np.ndarray]:
+    qidx = joint_q_indices(model, joint_names)
+    lower = np.asarray(model.lowerPositionLimit[qidx], dtype=float)
+    upper = np.asarray(model.upperPositionLimit[qidx], dtype=float)
+    if not np.all(np.isfinite(lower)) or not np.all(np.isfinite(upper)):
+        raise ValueError("All teleoperation joints must have finite position limits")
+    if np.any(lower >= upper):
+        raise ValueError("Invalid joint limits in teleoperation model")
+    return lower, upper
+
+
+def safe_configuration(
+    model: pin.Model,
+    joint_names: Sequence[str],
+    *,
+    fraction: float = 0.5,
+    margin: float = 1e-3,
+) -> np.ndarray:
+    """Return a configuration strictly inside all declared joint limits."""
+    if not 0.0 <= fraction <= 1.0:
+        raise ValueError("fraction must lie in [0, 1]")
+    lower, upper = finite_joint_limits(model, joint_names)
+    span = upper - lower
+    q7 = lower + fraction * span
+    q7 = np.minimum(np.maximum(q7, lower + margin), upper - margin)
+    q = pin.neutral(model)
+    q[joint_q_indices(model, joint_names)] = q7
+    return q
+
+
+def clip_configuration(
+    model: pin.Model,
+    q: np.ndarray,
+    joint_names: Sequence[str],
+    *,
+    margin: float = 1e-6,
+) -> tuple[np.ndarray, bool]:
+    """Clip scalar teleoperation joints and report whether clipping occurred."""
+    q_clipped = np.asarray(q, dtype=float).copy()
+    qidx = joint_q_indices(model, joint_names)
+    lower, upper = finite_joint_limits(model, joint_names)
+    clipped7 = np.minimum(np.maximum(q_clipped[qidx], lower + margin), upper - margin)
+    changed = bool(np.any(np.abs(clipped7 - q_clipped[qidx]) > 1e-12))
+    q_clipped[qidx] = clipped7
+    return q_clipped, changed

code/core/network_emulator.py

@@ -0,0 +1,233 @@
+"""Deterministic packet channels and timeout semantics for G0c experiments."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+import heapq
+from typing import Generic, Iterable, TypeVar
+
+import numpy as np
+
+from core.feedback_protocol import (
+    PacketRejectReason,
+    PacketState,
+)
+
+
+PacketT = TypeVar("PacketT")
+
+
+@dataclass(frozen=True)
+class NetworkTraceEntry:
+    """Transport decision for one sequence number."""
+
+    seq: int
+    delay_s: float
+    lost: bool = False
+    duplicate: bool = False
+    corrupt: bool = False
+
+    def __post_init__(self) -> None:
+        if self.seq < 0:
+            raise ValueError("seq must be non-negative")
+        if not np.isfinite(self.delay_s) or self.delay_s < 0.0:
+            raise ValueError("delay_s must be finite and non-negative")
+
+
+def generate_network_trace(
+    count: int,
+    *,
+    base_delay_s: float,
+    jitter_s: float = 0.0,
+    loss_probability: float = 0.0,
+    duplicate_probability: float = 0.0,
+    corrupt_probability: float = 0.0,
+    seed: int = 0,
+) -> tuple[NetworkTraceEntry, ...]:
+    """Generate a reusable trace without coupling other random streams."""
+    if count < 0:
+        raise ValueError("count must be non-negative")
+    probabilities = (
+        loss_probability,
+        duplicate_probability,
+        corrupt_probability,
+    )
+    if any(not 0.0 <= value <= 1.0 for value in probabilities):
+        raise ValueError("network probabilities must lie in [0, 1]")
+    if base_delay_s < 0.0 or jitter_s < 0.0:
+        raise ValueError("delay and jitter must be non-negative")
+    rng = np.random.default_rng(seed)
+    entries = []
+    for seq in range(count):
+        jitter = rng.uniform(-jitter_s, jitter_s) if jitter_s else 0.0
+        entries.append(
+            NetworkTraceEntry(
+                seq=seq,
+                delay_s=max(0.0, base_delay_s + jitter),
+                lost=bool(rng.random() < loss_probability),
+                duplicate=bool(rng.random() < duplicate_probability),
+                corrupt=bool(rng.random() < corrupt_probability),
+            )
+        )
+    return tuple(entries)
+
+
+@dataclass(frozen=True)
+class Delivery(Generic[PacketT]):
+    packet: PacketT
+    source_time: float
+    arrival_time: float
+    corrupt: bool
+    duplicate_copy: bool
+
+
+class DeterministicChannel(Generic[PacketT]):
+    """One scheduled channel driven entirely by a frozen trace."""
+
+    def __init__(self, trace: Iterable[NetworkTraceEntry]):
+        self._trace = {entry.seq: entry for entry in trace}
+        self._pending: list[tuple[float, int, Delivery[PacketT]]] = []
+        self._insertion_order = 0
+
+    def send(self, packet: PacketT, now: float) -> None:
+        seq = int(getattr(packet, "seq"))
+        entry = self._trace.get(seq, NetworkTraceEntry(seq=seq, delay_s=0.0))
+        if entry.lost:
+            return
+        arrival = float(now) + entry.delay_s
+        delivery = Delivery(
+            packet=packet,
+            source_time=float(getattr(packet, "source_time")),
+            arrival_time=arrival,
+            corrupt=entry.corrupt,
+            duplicate_copy=False,
+        )
+        heapq.heappush(
+            self._pending, (arrival, self._insertion_order, delivery)
+        )
+        self._insertion_order += 1
+        if entry.duplicate:
+            duplicate = Delivery(
+                packet=packet,
+                source_time=delivery.source_time,
+                arrival_time=arrival + 1e-12,
+                corrupt=entry.corrupt,
+                duplicate_copy=True,
+            )
+            heapq.heappush(
+                self._pending,
+                (duplicate.arrival_time, self._insertion_order, duplicate),
+            )
+            self._insertion_order += 1
+
+    def poll(self, now: float) -> list[Delivery[PacketT]]:
+        ready: list[Delivery[PacketT]] = []
+        while self._pending and self._pending[0][0] <= float(now) + 1e-15:
+            _, _, delivery = heapq.heappop(self._pending)
+            ready.append(delivery)
+        return ready
+
+    @property
+    def pending_count(self) -> int:
+        return len(self._pending)
+
+
+@dataclass(frozen=True)
+class Reception:
+    seq: int
+    arrival_time: float
+    age_s: float
+    accepted: bool
+    reason: PacketRejectReason
+    state: PacketState
+
+
+@dataclass(frozen=True)
+class HeldPacket(Generic[PacketT]):
+    packet: PacketT | None
+    state: PacketState
+    age_s: float | None
+    fresh: bool
+
+
+class PacketReceiver(Generic[PacketT]):
+    """Newest-sequence hold, timeout, and recovery state machine."""
+
+    def __init__(self, timeout_s: float):
+        if not np.isfinite(timeout_s) or timeout_s <= 0.0:
+            raise ValueError("timeout_s must be finite and positive")
+        self.timeout_s = float(timeout_s)
+        self.last_seq = -1
+        self.last_packet: PacketT | None = None
+        self.last_arrival_time: float | None = None
+        self.state = PacketState.EMPTY
+        self._fresh = False
+        self._recovering_sample_pending = False
+
+    def accept(self, delivery: Delivery[PacketT]) -> Reception:
+        packet = delivery.packet
+        seq = int(getattr(packet, "seq"))
+        valid = bool(getattr(packet, "valid", True))
+        source_time = float(getattr(packet, "source_time"))
+        age = max(0.0, delivery.arrival_time - source_time)
+
+        if delivery.corrupt:
+            reason = PacketRejectReason.CORRUPT
+        elif not valid:
+            reason = PacketRejectReason.INVALID
+        elif seq <= self.last_seq:
+            reason = PacketRejectReason.DUPLICATE_OR_STALE
+        else:
+            previous = self.state
+            self.last_seq = seq
+            self.last_packet = packet
+            self.last_arrival_time = delivery.arrival_time
+            self._fresh = True
+            self._recovering_sample_pending = previous in (
+                PacketState.EMPTY,
+                PacketState.TIMED_OUT,
+            )
+            self.state = (
+                PacketState.RECOVERING
+                if self._recovering_sample_pending
+                else PacketState.ACTIVE
+            )
+            return Reception(
+                seq=seq,
+                arrival_time=delivery.arrival_time,
+                age_s=age,
+                accepted=True,
+                reason=PacketRejectReason.ACCEPTED,
+                state=self.state,
+            )
+
+        return Reception(
+            seq=seq,
+            arrival_time=delivery.arrival_time,
+            age_s=age,
+            accepted=False,
+            reason=reason,
+            state=self.state,
+        )
+
+    def sample(self, now: float) -> HeldPacket[PacketT]:
+        if self.last_packet is None or self.last_arrival_time is None:
+            self.state = PacketState.EMPTY
+            return HeldPacket(None, self.state, None, False)
+        age = max(0.0, float(now) - self.last_arrival_time)
+        if age > self.timeout_s:
+            self.state = PacketState.TIMED_OUT
+            self._fresh = False
+            return HeldPacket(None, self.state, age, False)
+
+        if self._recovering_sample_pending:
+            state = PacketState.RECOVERING
+            self._recovering_sample_pending = False
+        elif self._fresh:
+            state = PacketState.ACTIVE
+        else:
+            state = PacketState.HELD
+        fresh = self._fresh
+        self._fresh = False
+        self.state = state
+        return HeldPacket(self.last_packet, state, age, fresh)

code/core/sew_mapper.py

@@ -0,0 +1,295 @@
+# -*- coding: utf-8 -*-
+import numpy as np
+import pinocchio as pin
+from pathlib import Path
+
+
+def hat(v):
+    x, y, z = v
+    return np.array([[0, -z, y],
+                     [z,  0,-x],
+                     [-y, x, 0]], dtype=float)
+
+
+def normalize(v, eps=1e-12):
+    n = np.linalg.norm(v)
+    if n < eps:
+        return v * 0.0
+    return v / n
+
+
+def rodrigues(u, phi):
+    u = normalize(u)
+    K = hat(u)
+    return np.eye(3) + np.sin(phi)*K + (1-np.cos(phi))*(K@K)
+
+
+def minimal_rotation_align(a, b):
+    a = normalize(a); b = normalize(b)
+    v = np.cross(a, b)
+    s = np.linalg.norm(v)
+    c = float(np.dot(a, b))
+    if s < 1e-12:
+        # parallel or anti-parallel
+        if c > 0.0:
+            return np.eye(3)
+        # 180°: choose any axis orthogonal to a
+        axis = normalize(np.array([1.0,0,0]) if abs(a[0])<0.9 else np.array([0,1.0,0]))
+        axis = normalize(np.cross(a, axis))
+        K = hat(axis)
+        return np.eye(3) + 2*(K@K)  # R = I + 2[K]^2 for 180°
+    K = hat(v/s)
+    return np.eye(3) + K*s + (1-c)/(s*s) * (K@K)
+
+
+def euler_zyx_from_R(R):
+    """Return Z-Y-X Euler angles (about z,y,x of the shoulder frame)."""
+    sy = -R[2,0]
+    cy = np.sqrt(max(0.0, 1.0 - sy*sy))
+    if cy > 1e-9:
+        z = np.arctan2(R[1,0], R[0,0])
+        y = np.arctan2(sy, cy)
+        x = np.arctan2(R[2,1], R[2,2])
+    else:
+        # Gimbal case: cy ~ 0
+        z = np.arctan2(-R[0,1], R[1,1])
+        y = np.arctan2(sy, cy)
+        x = 0.0
+    return np.array([z, y, x], dtype=float)
+
+def euler_xzy_from_R(R):
+    """
+    Decompose R ≈ Rx(a) * Rz(b) * Ry(c)
+    Return (a, b, c)
+    """
+    # from derivation:
+    # sb = -R[0,1]; cb = sqrt(R[0,0]^2 + R[0,2]^2)
+    sb = -R[0,1]
+    cb = np.sqrt(max(0.0, R[0,0]**2 + R[0,2]**2))
+    b  = np.arctan2(sb, cb)
+    # c = atan2(R[0,2], R[0,0])
+    c  = np.arctan2(R[0,2], R[0,0])
+    # a from R[1,1] = ca*cb and R[2,1] = sa*cb
+    if cb < 1e-9:
+        # singular: cb≈0 => b≈±pi/2，退化时把 a=0，c 吸收残差（简化处理）
+        a = 0.0
+    else:
+        ca = np.clip(R[1,1] / cb, -1.0, 1.0)
+        sa = np.clip(R[2,1] / cb, -1.0, 1.0)
+        a  = np.arctan2(sa, ca)
+    return np.array([a, b, c], dtype=float)
+
+def euler_zy_from_R(R):
+    """Return Z-Y Euler angles for wrist (about z then y)."""
+    # R ≈ Rz(z) * Ry(y)
+    # y = asin(R[0,2])? We'll use standard decomposition.
+    # From R = Rz*Ry:
+    # R[2,0] = -sin(y)
+    y = np.arcsin(np.clip(R[0,2], -1.0, 1.0))
+    cy = np.cos(y)
+    if abs(cy) < 1e-9:
+        z = 0.0
+    else:
+        z = np.arctan2(-R[0,1]/cy, R[0,0]/cy)
+    return np.array([z, y], dtype=float)
+
+
+def rot_error_deg(RA, RB):
+    # 旋转误差：以李群对数映射的范数（弧度），再转度
+    R = RA.T @ RB
+    w = pin.log3(R)
+    return np.linalg.norm(w) * 180.0/np.pi
+
+def pose_of_frame(model, data, frame_name):
+    fid = model.getFrameId(frame_name)
+    oMf = data.oMf[fid]
+    return oMf.translation.copy(), oMf.rotation.copy()
+
+def fk_update(model, data, q):
+    pin.forwardKinematics(model, data, q)
+    pin.updateFramePlacements(model, data)
+
+
+
+class SEWMapper:
+    """
+    SEW Mapper (closed-form, no numeric IK):
+    - Loads master & slave URDFs
+    - Computes heteromorphic retargeting from master EE pose to slave q_s using SEW geometry
+    Assumptions:
+      * Slave arm is 7-DoF in the order: 3 shoulder + 1 elbow + 3 wrist (axes orthogonal at shoulder/wrist frames).
+      * Provide correct link/frame names and joint name order for your model.
+    """
+
+    def __init__(
+        self,
+        master_model: pin.Model,
+        slave_model: pin.Model,
+        # Frame/link names
+        m_shoulder_frame: str,
+        m_elbow_frame: str,
+        m_wrist_frame: str,
+        m_ee_frame: str,
+        s_shoulder_frame: str,
+        s_elbow_frame: str,
+        s_wrist_frame: str,
+        s_ee_frame: str,
+        # Joint name order for slave 7-DoF: [S1,S2,S3, EL, W1, W2, W3]
+        slave_joint_names: list,
+        # world/up direction and safety margins
+        up_dir=np.array([0,0,1.0]),
+        eps_clip=1e-3,
+    ):
+        # Load master
+        self.m_model = master_model
+        self.m_data  = self.m_model.createData()
+
+        # Load slave
+        self.s_model = slave_model
+        self.s_data  = self.s_model.createData()
+
+        # Frame IDs
+        self.fid_mS = self._get_frame_id(self.m_model, m_shoulder_frame)
+        self.fid_mE = self._get_frame_id(self.m_model, m_elbow_frame)
+        self.fid_mW = self._get_frame_id(self.m_model, m_wrist_frame)
+        self.fid_mEE= self._get_frame_id(self.m_model, m_ee_frame)
+
+        self.fid_sS = self._get_frame_id(self.s_model, s_shoulder_frame)
+        self.fid_sE = self._get_frame_id(self.s_model, s_elbow_frame)
+        self.fid_sW = self._get_frame_id(self.s_model, s_wrist_frame)
+        self.fid_sEE= self._get_frame_id(self.s_model, s_ee_frame)
+
+        # Up direction and epsilon
+        self.up = normalize(up_dir)
+        self.eps_clip = float(eps_clip)
+
+        # Query slave joint indices (order critical)
+        self.s_joint_ids = [self.s_model.getJointId(n) for n in slave_joint_names]
+        self.s_qidx      = [self.s_model.joints[jid].idx_q for jid in self.s_joint_ids]
+        assert len(self.s_qidx) == 7, "Provide 7 slave joints in order [S1,S2,S3, EL, W1, W2, W3]"
+
+        # Pre-compute slave segment lengths L1, L2 in reference (zero) config
+        self.qs0 = pin.neutral(self.s_model)
+        self._update_fk_slave(self.qs0)
+        pS = self._frame_pos(self.s_data, self.fid_sS)
+        pE = self._frame_pos(self.s_data, self.fid_sE)
+        pW = self._frame_pos(self.s_data, self.fid_sW)
+        self.L1 = float(np.linalg.norm(pE - pS))
+        self.L2 = float(np.linalg.norm(pW - pE))
+
+        # Fixed slave shoulder location in world
+        self.pS_s_fixed = pS.copy()
+
+    # ------------------ Utilities ------------------ #
+    def _get_frame_id(self, model, name):
+        try:
+            return model.getFrameId(name)
+        except:
+            # fallback: also try link->frame mapping via joint name
+            return model.getFrameId(name)
+
+    def _update_fk_master(self, q_m):
+        pin.forwardKinematics(self.m_model, self.m_data, q_m)
+        pin.updateFramePlacements(self.m_model, self.m_data)
+
+    def _update_fk_slave(self, q_s):
+        pin.forwardKinematics(self.s_model, self.s_data, q_s)
+        pin.updateFramePlacements(self.s_model, self.s_data)
+
+    def _frame_pos(self, data, fid):
+        return data.oMf[fid].translation.copy()
+
+    def _frame_rot(self, data, fid):
+        return data.oMf[fid].rotation.copy()
+
+    # ------------------ Core Retarget ------------------ #
+    def retargetting(self, q_m, q_s_init=None):
+        """
+        Input:
+          q_m     : master joint configuration (np.ndarray, size = master nq)
+          q_s_init: optional slave initial seed (ignored for geometry, used only to keep continuity of angle unwrap if desired)
+        Output:
+          q_s     : slave joint angles (np.ndarray, size = slave nq)
+          debug   : dict with intermediate targets (pE_s, pW_s_ref, phi_m, theta)
+        """
+        # 1) Master FK and SEW quantities
+        self._update_fk_master(q_m)
+        pS_m = self._frame_pos(self.m_data, self.fid_mS)
+        pE_m = self._frame_pos(self.m_data, self.fid_mE)
+        pW_m = self._frame_pos(self.m_data, self.fid_mW)
+        # RW_m = self._frame_rot(self.m_data, self.fid_mW)
+        RE_m = self._frame_rot(self.m_data, self.fid_mEE) 
+
+        r_m = pW_m - pS_m
+        d_m = float(np.linalg.norm(r_m))
+        xhat_m = normalize(r_m)
+
+        # Swivel (master)
+        nm_raw = np.cross(pE_m - pS_m, pW_m - pS_m)
+        nm = normalize(nm_raw)
+        nref_tilde = self.up - np.dot(self.up, xhat_m) * xhat_m
+        if np.linalg.norm(nref_tilde) < 1e-6:
+            ey = np.array([0,1.0,0])
+            nref_tilde = ey - np.dot(ey, xhat_m) * xhat_m
+        nref = normalize(nref_tilde)
+        num = np.dot(xhat_m, np.cross(nref, nm))
+        den = float(np.dot(nref, nm))
+        phi_m = np.arctan2(num, den)
+
+        # 2) Wrist pose and reach clipping on slave
+        d_min = abs(self.L1 - self.L2) + self.eps_clip
+        d_max = (self.L1 + self.L2) - self.eps_clip
+        d_s = np.clip(d_m, d_min, d_max)
+        xhat_s = xhat_m.copy()
+        pS_s = self.pS_s_fixed
+        pW_s_ref = pS_s + d_s * xhat_s
+        R_ee_ref = RE_m.copy()   # preserve orientation
+
+        # 3) Two-sphere elbow construction on slave
+        d = float(np.linalg.norm(pW_s_ref - pS_s))
+        e3 = rodrigues(xhat_s, phi_m) @ nref
+        e3 = normalize(e3)
+        e2 = normalize(np.cross(xhat_s, e3))
+        cos_th = (self.L1**2 + d**2 - self.L2**2) / (2*self.L1*d)
+        cos_th = float(np.clip(cos_th, -1.0, 1.0))
+        th = np.arccos(cos_th)
+        sin_th = np.sqrt(max(0.0, 1.0 - cos_th*cos_th))
+        pE_s = pS_s + self.L1*(cos_th * xhat_s + sin_th * e2)
+
+        # 4) Joint reconstruction (assumes 3-1-3 structure with Z-Y-X at shoulder and Z-Y at wrist)
+        q_s = pin.neutral(self.s_model) if q_s_init is None else q_s_init.copy()
+
+        # (a) Set shoulder (first 3 joints): align upper-arm vector
+        u = normalize(pE_s - pS_s)
+        y_axis = e3               # 让“肘轴=局部Y”严格对齐 SEW 的平面法向
+        x_axis = u
+        z_axis = normalize(np.cross(x_axis, y_axis))
+        RS_des = np.column_stack([x_axis, y_axis, z_axis])
+
+
+        dz, dy, dx = euler_zyx_from_R(RS_des)               # R ≈ Rz(dz)*Ry(dy)*Rx(dx)
+        q_s[self.s_qidx[0]] = dz
+        q_s[self.s_qidx[1]] = dy
+        q_s[self.s_qidx[2]] = dx
+        # (b) Elbow flex = pi - theta
+        fhat = normalize(pW_s_ref - pE_s)
+        u = normalize(pE_s - pS_s)# 上臂方向（从肩指向肘）
+        q_elbow = np.arctan2(np.dot(e3, np.cross(u, fhat)), np.dot(u, fhat))# 计算把 u 绕 e3 旋到 fhat 的有符号角：atan2( 轴·(u×f),  u·f )
+        q_s[self.s_qidx[3]] = q_elbow
+        self._update_fk_slave(q_s)
+        # (c) Wrist orientation match with X–Z–Y on wrist frame
+        RW_cur = self._frame_rot(self.s_data, self.fid_sW)
+        R_needed = RW_cur.T @ R_ee_ref         
+        a_b_c = euler_xzy_from_R(R_needed)      # (roll-X, yaw-Z, pitch-Y)
+        q_s[self.s_qidx[4]] = a_b_c[0]
+        q_s[self.s_qidx[5]] = a_b_c[1]
+        q_s[self.s_qidx[6]] = a_b_c[2]
+
+        debug = dict(
+            phi_m=phi_m,
+            d_s=d_s,
+            pW_s_ref=pW_s_ref,
+            pE_s=pE_s,
+            theta=th
+        )
+        return q_s, debug

code/core/sew_mapper2.py

@@ -0,0 +1,781 @@
+
+# -*- coding: utf-8 -*-
+"""
+SEW retargeting for the real 7-DoF slave arm.
+
+The geometric SEW construction provides elbow/wrist targets.  A bounded
+least-squares recovery then enforces the joint limits from the slave URDF.
+``compute_differential`` evaluates the local retargeting Jacobian with a
+central finite difference on one fixed local branch and reports every event
+that makes that differential invalid (failed recovery, branch jump, reach
+clipping, or an active joint limit).
+
+Assumptions matching ``real_slave_7dof.urdf``:
+- upper-arm / forearm links extend along local -Y;
+- elbow hinge axis is +X in the elbow joint local frame;
+- shoulder axes are y-x-y with signs (-1,+1,-1);
+- wrist axes are y-z-x with signs (-1,+1,+1).
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Dict, List, Optional, Sequence, Tuple
+
+import numpy as np
+import pinocchio as pin
+from scipy.optimize import least_squares
+
+
+# -------------------------- small SO(3) helpers -------------------------- #
+
+def _hat(v: np.ndarray) -> np.ndarray:
+    x, y, z = v
+    return np.array([[0.0, -z,  y],
+                     [z,  0.0, -x],
+                     [-y, x,  0.0]], dtype=float)
+
+def _normalize(v: np.ndarray, eps: float = 1e-12) -> np.ndarray:
+    n = float(np.linalg.norm(v))
+    return v * 0.0 if n < eps else (v / n)
+
+
+def _wrap_angle_delta(delta: np.ndarray) -> np.ndarray:
+    """Return revolute-joint differences on the principal interval [-pi, pi)."""
+    delta = np.asarray(delta, dtype=float)
+    return (delta + np.pi) % (2.0 * np.pi) - np.pi
+
+def _rodrigues(axis: np.ndarray, angle: float) -> np.ndarray:
+    a = _normalize(axis)
+    K = _hat(a)
+    return np.eye(3) + np.sin(angle) * K + (1.0 - np.cos(angle)) * (K @ K)
+
+def _ball_rot(axis_order: str, q3: np.ndarray) -> np.ndarray:
+    """Intrinsic chain: R = R(a1,q1) R(a2,q2) R(a3,q3) with a in {x,y,z} unit axes."""
+    axes = {
+        "x": np.array([1.0, 0.0, 0.0]),
+        "y": np.array([0.0, 1.0, 0.0]),
+        "z": np.array([0.0, 0.0, 1.0]),
+    }
+    ao = axis_order.lower()
+    a1, a2, a3 = axes[ao[0]], axes[ao[1]], axes[ao[2]]
+    return _rodrigues(a1, q3[0]) @ _rodrigues(a2, q3[1]) @ _rodrigues(a3, q3[2])
+
+def _solve_ball_gn(
+    R_des: np.ndarray,
+    axis_order: str,
+    q_seed: np.ndarray,
+    iters: int = 15,
+    damp: float = 1e-4,
+    fd_eps: float = 1e-6,
+) -> np.ndarray:
+    """Gauss-Newton solve q s.t. R(q) ~= R_des. Works for repeated axes (e.g. yxy)."""
+    q = q_seed.astype(float).copy()
+    ao = axis_order.lower()
+
+    for _ in range(iters):
+        R_cur = _ball_rot(ao, q)
+        err = pin.log3(R_cur.T @ R_des)
+        if float(np.linalg.norm(err)) < 1e-10:
+            break
+
+        J = np.zeros((3, 3), dtype=float)
+        for i in range(3):
+            dq = np.zeros(3, dtype=float)
+            dq[i] = fd_eps
+            R_p = _ball_rot(ao, q + dq)
+            err_p = pin.log3(R_p.T @ R_des)
+            J[:, i] = (err_p - err) / fd_eps
+
+        A = J @ J.T + damp * np.eye(3)
+        dq = - J.T @ np.linalg.solve(A, err)
+        q += dq
+
+    q = (q + np.pi) % (2 * np.pi) - np.pi
+    return q
+
+
+# -------------------------- joint config -------------------------- #
+
+@dataclass(frozen=True)
+class BallJointConfig:
+    axis_order: str
+    joint_names: Tuple[str, str, str]
+    signs: Tuple[float, float, float] = (1.0, 1.0, 1.0)
+
+    def resolve_qidx(self, model: pin.Model) -> Tuple[int, int, int]:
+        idxs = []
+        for name in self.joint_names:
+            jid = int(model.getJointId(name))
+            if jid <= 0 or jid >= model.njoints:
+                raise ValueError(
+                    f"Joint not found: {name!r}; "
+                    f"available={list(model.names)[1:]}"
+                )
+            if model.joints[jid].nq != 1 or model.joints[jid].nv != 1:
+                raise ValueError(f"Expected scalar revolute joint: {name!r}")
+            idxs.append(model.joints[jid].idx_q)
+        return (idxs[0], idxs[1], idxs[2])
+
+    def eff_from_q(self, q: np.ndarray, qidx: Tuple[int, int, int]) -> np.ndarray:
+        s0, s1, s2 = self.signs
+        return np.array([s0 * q[qidx[0]], s1 * q[qidx[1]], s2 * q[qidx[2]]], dtype=float)
+
+    def write_from_eff(self, q: np.ndarray, qidx: Tuple[int, int, int], q_eff: np.ndarray) -> None:
+        s0, s1, s2 = self.signs
+        q[qidx[0]] = s0 * float(q_eff[0])
+        q[qidx[1]] = s1 * float(q_eff[1])
+        q[qidx[2]] = s2 * float(q_eff[2])
+
+
+# -------------------------- mapper -------------------------- #
+
+class SEWMapper:
+    """
+    Minimal SEW mapper.
+    You must provide correct frame names and 7 joint names order:
+      [S1,S2,S3, EL, W1,W2,W3]
+    """
+    def __init__(
+        self,
+        master_model: pin.Model,
+        slave_model: pin.Model,
+        # frames
+        m_shoulder: str, m_elbow: str, m_wrist: str, m_ee: str,
+        s_shoulder: str, s_elbow: str, s_wrist: str, s_ee: str,
+        # joints order (7)
+        master_joint_names: Tuple[str, ...],
+        slave_joint_names: Tuple[str, ...],
+        # slave configs
+        slave_shoulder_cfg: BallJointConfig,
+        slave_wrist_cfg: BallJointConfig,
+        # elbow axis in elbow joint local frame
+        slave_elbow_axis_local: np.ndarray = np.array([1.0, 0.0, 0.0]),
+        up_dir: np.ndarray = np.array([0.0, 0.0, 1.0]),
+        eps_clip: float = 1e-3,
+        position_tolerance: float = 2e-4,
+        orientation_tolerance: float = 2e-3,
+        joint_limit_margin: float = 1e-4,
+        debug: bool = False,
+    ):
+        self.m_model = master_model
+        self.m_data = master_model.createData()
+        self.s_model = slave_model
+        self.s_data = slave_model.createData()
+
+        # frames
+        def checked_frame_id(model: pin.Model, name: str) -> int:
+            fid = int(model.getFrameId(name))
+            if fid < 0 or fid >= model.nframes:
+                raise ValueError(
+                    f"Frame not found: {name!r}; "
+                    f"available={[frame.name for frame in model.frames]}"
+                )
+            return fid
+
+        self.fid_mS = checked_frame_id(master_model, m_shoulder)
+        self.fid_mE = checked_frame_id(master_model, m_elbow)
+        self.fid_mW = checked_frame_id(master_model, m_wrist)
+        self.fid_mEE = checked_frame_id(master_model, m_ee)
+
+        self.fid_sS = checked_frame_id(slave_model, s_shoulder)
+        self.fid_sE = checked_frame_id(slave_model, s_elbow)
+        self.fid_sW = checked_frame_id(slave_model, s_wrist)
+        self.fid_sEE = checked_frame_id(slave_model, s_ee)
+
+        # joints mapping (7)
+        if len(master_joint_names) != 7 or len(slave_joint_names) != 7:
+            raise ValueError("master_joint_names and slave_joint_names must be length 7")
+        def checked_joint_id(model: pin.Model, name: str) -> int:
+            jid = int(model.getJointId(name))
+            if jid <= 0 or jid >= model.njoints:
+                raise ValueError(
+                    f"Joint not found: {name!r}; "
+                    f"available={list(model.names)[1:]}"
+                )
+            joint = model.joints[jid]
+            if joint.nq != 1 or joint.nv != 1:
+                raise ValueError(f"Expected scalar revolute joint: {name!r}")
+            return jid
+
+        self.m_joint_ids = [
+            checked_joint_id(master_model, name) for name in master_joint_names
+        ]
+        self.s_joint_ids = [
+            checked_joint_id(slave_model, name) for name in slave_joint_names
+        ]
+        self.m_qidx7 = [master_model.joints[j].idx_q for j in self.m_joint_ids]
+        self.s_qidx7 = [slave_model.joints[j].idx_q for j in self.s_joint_ids]
+        self.jid_s_elbow = self.s_joint_ids[3]  # joint id, not qidx
+
+        # configs
+        self.sh_cfg = slave_shoulder_cfg
+        self.wr_cfg = slave_wrist_cfg
+        self.sh_qidx = self.sh_cfg.resolve_qidx(slave_model)
+        self.wr_qidx = self.wr_cfg.resolve_qidx(slave_model)
+
+        self.elbow_axis_local = _normalize(slave_elbow_axis_local)
+        self.up = _normalize(up_dir)
+        self.eps_clip = float(eps_clip)
+        self.position_tolerance = float(position_tolerance)
+        self.orientation_tolerance = float(orientation_tolerance)
+        self.joint_limit_margin = float(joint_limit_margin)
+        self.debug = bool(debug)
+
+        self.s_lower7 = np.array(
+            [slave_model.lowerPositionLimit[idx] for idx in self.s_qidx7], dtype=float
+        )
+        self.s_upper7 = np.array(
+            [slave_model.upperPositionLimit[idx] for idx in self.s_qidx7], dtype=float
+        )
+        if (
+            not np.all(np.isfinite(self.s_lower7))
+            or not np.all(np.isfinite(self.s_upper7))
+            or np.any(self.s_lower7 >= self.s_upper7)
+        ):
+            raise ValueError("All seven slave joints must have finite, ordered URDF limits")
+
+        # segment lengths from slave neutral
+        qs0 = pin.neutral(slave_model)
+        self._fk_slave(qs0)
+        pS = self._pos(self.s_data, self.fid_sS)
+        pE = self._pos(self.s_data, self.fid_sE)
+        pW = self._pos(self.s_data, self.fid_sW)
+        self.L1 = float(np.linalg.norm(pE - pS))
+        self.L2 = float(np.linalg.norm(pW - pE))
+        self.pS_s_fixed = pS.copy()
+
+    # ---- FK helpers ----
+    def _fk_master(self, q_m: np.ndarray) -> None:
+        pin.forwardKinematics(self.m_model, self.m_data, q_m)
+        pin.updateFramePlacements(self.m_model, self.m_data)
+
+    def _fk_slave(self, q_s: np.ndarray) -> None:
+        pin.forwardKinematics(self.s_model, self.s_data, q_s)
+        pin.updateFramePlacements(self.s_model, self.s_data)
+
+    @staticmethod
+    def _pos(data: pin.Data, fid: int) -> np.ndarray:
+        return data.oMf[fid].translation.copy()
+
+    @staticmethod
+    def _rot(data: pin.Data, fid: int) -> np.ndarray:
+        return data.oMf[fid].rotation.copy()
+
+    # ---- vector helpers ----
+    def _build_q(self, model: pin.Model, qidx7: Sequence[int], q7: np.ndarray) -> np.ndarray:
+        q = pin.neutral(model)
+        for idx, value in zip(qidx7, q7):
+            q[idx] = float(value)
+        return q
+
+    def _slave_q7(self, q_s: np.ndarray) -> np.ndarray:
+        return np.array([q_s[idx] for idx in self.s_qidx7], dtype=float)
+
+    def _write_slave_q7(self, q_s: np.ndarray, q_s7: np.ndarray) -> None:
+        for idx, value in zip(self.s_qidx7, q_s7):
+            q_s[idx] = float(value)
+
+    def _interior_clip(self, q_s7: np.ndarray) -> np.ndarray:
+        # scipy requires x0 to be feasible.  Keeping it strictly inside also
+        # avoids declaring the seed itself to be the active-set solution.
+        pad = np.minimum(1e-9, 0.25 * (self.s_upper7 - self.s_lower7))
+        return np.minimum(np.maximum(q_s7, self.s_lower7 + pad), self.s_upper7 - pad)
+
+    # ---- target construction and bounded recovery ----
+    def _target_from_master(self, q_m7: np.ndarray) -> Dict:
+        q_m7 = np.asarray(q_m7, dtype=float)
+        if q_m7.shape != (7,) or not np.all(np.isfinite(q_m7)):
+            raise ValueError("q_m7 must be a finite vector with shape (7,)")
+
+        q_m = self._build_q(self.m_model, self.m_qidx7, q_m7)
+        self._fk_master(q_m)
+        pS_m = self._pos(self.m_data, self.fid_mS)
+        pE_m = self._pos(self.m_data, self.fid_mE)
+        pW_m = self._pos(self.m_data, self.fid_mW)
+        RmEE = self._rot(self.m_data, self.fid_mEE)
+
+        events: List[str] = []
+        r_m = pW_m - pS_m
+        d_m = float(np.linalg.norm(r_m))
+        hard_geometry_valid = d_m > 1e-9
+        if not hard_geometry_valid:
+            events.append("master_shoulder_wrist_degenerate")
+            xhat = np.array([1.0, 0.0, 0.0])
+        else:
+            xhat = r_m / d_m
+
+        arm_normal_raw = np.cross(pE_m - pS_m, pW_m - pS_m)
+        arm_normal_norm = float(np.linalg.norm(arm_normal_raw))
+        if arm_normal_norm < 1e-8:
+            events.append("master_arm_plane_degenerate")
+            hard_geometry_valid = False
+            nm = self.up - float(np.dot(self.up, xhat)) * xhat
+            if float(np.linalg.norm(nm)) < 1e-8:
+                nm = np.array([0.0, 1.0, 0.0])
+            nm = _normalize(nm)
+        else:
+            nm = arm_normal_raw / arm_normal_norm
+
+        nref_tilde = self.up - float(np.dot(self.up, xhat)) * xhat
+        reference_fallback = float(np.linalg.norm(nref_tilde)) < 1e-6
+        if reference_fallback:
+            events.append("reference_axis_fallback")
+            candidates = (
+                np.array([0.0, 1.0, 0.0]),
+                np.array([1.0, 0.0, 0.0]),
+            )
+            nref_tilde = max(
+                (axis - float(np.dot(axis, xhat)) * xhat for axis in candidates),
+                key=np.linalg.norm,
+            )
+        nref = _normalize(nref_tilde)
+        phi = float(
+            np.arctan2(
+                np.dot(xhat, np.cross(nref, nm)),
+                np.dot(nref, nm),
+            )
+        )
+
+        d_min = abs(self.L1 - self.L2) + self.eps_clip
+        d_max = (self.L1 + self.L2) - self.eps_clip
+        d_s = float(np.clip(d_m, d_min, d_max))
+        if d_m < d_min:
+            clip_region = "lower"
+            events.append("reach_clipped_lower")
+        elif d_m > d_max:
+            clip_region = "upper"
+            events.append("reach_clipped_upper")
+        else:
+            clip_region = "none"
+
+        pS_s = self.pS_s_fixed
+        pW_s_ref = pS_s + d_s * xhat
+        e3 = _normalize(_rodrigues(xhat, phi) @ nref)
+        e2 = _normalize(np.cross(xhat, e3))
+        cos_th_raw = (self.L1**2 + d_s**2 - self.L2**2) / (2.0 * self.L1 * d_s)
+        cos_th = float(np.clip(cos_th_raw, -1.0, 1.0))
+        sin_th = float(np.sqrt(max(0.0, 1.0 - cos_th * cos_th)))
+        pE_s_ref = pS_s + self.L1 * (cos_th * xhat + sin_th * e2)
+
+        # Desired shoulder-link orientation used only to create a strong seed.
+        u = _normalize(pE_s_ref - pS_s)
+        x_axis = e3
+        y_axis = -u
+        z_axis = _normalize(np.cross(x_axis, y_axis))
+        y_axis = _normalize(np.cross(z_axis, x_axis))
+        RS_des = np.column_stack([x_axis, y_axis, z_axis])
+
+        return {
+            "q_m": q_m,
+            "pE_s_ref": pE_s_ref,
+            "pW_s_ref": pW_s_ref,
+            "RmEE": RmEE,
+            "RS_des": RS_des,
+            "events": events,
+            "hard_geometry_valid": hard_geometry_valid,
+            "reference_fallback": reference_fallback,
+            "reach_clipped": clip_region != "none",
+            "clip_region": clip_region,
+            "master_reach": d_m,
+            "slave_reach": d_s,
+            "master_arm_normal_norm": arm_normal_norm,
+        }
+
+    def _staged_seed(self, target: Dict, q_seed: np.ndarray) -> np.ndarray:
+        """Analytic/sequential solve used as a seed; it is never returned unchecked."""
+        q_s = q_seed.copy()
+
+        q_sh_seed = self.sh_cfg.eff_from_q(q_s, self.sh_qidx)
+        q_sh_eff = _solve_ball_gn(
+            target["RS_des"], self.sh_cfg.axis_order, q_sh_seed
+        )
+        self.sh_cfg.write_from_eff(q_s, self.sh_qidx, q_sh_eff)
+
+        # The one-dimensional search respects the actual elbow limits.
+        theta_lo = float(self.s_lower7[3])
+        theta_hi = float(self.s_upper7[3])
+
+        def wrist_err(theta: float) -> float:
+            q_tmp = q_s.copy()
+            q_tmp[self.s_qidx7[3]] = theta
+            self._fk_slave(q_tmp)
+            return float(
+                np.linalg.norm(
+                    self._pos(self.s_data, self.fid_sW) - target["pW_s_ref"]
+                )
+            )
+
+        thetas = np.linspace(theta_lo, theta_hi, 181)
+        errs = np.array([wrist_err(theta) for theta in thetas])
+        k = int(np.argmin(errs))
+        lo = float(thetas[max(0, k - 1)])
+        hi = float(thetas[min(len(thetas) - 1, k + 1)])
+        gr = (np.sqrt(5.0) - 1.0) / 2.0
+        x1 = hi - gr * (hi - lo)
+        x2 = lo + gr * (hi - lo)
+        f1, f2 = wrist_err(x1), wrist_err(x2)
+        for _ in range(20):
+            if f1 > f2:
+                lo, x1, f1 = x1, x2, f2
+                x2 = lo + gr * (hi - lo)
+                f2 = wrist_err(x2)
+            else:
+                hi, x2, f2 = x2, x1, f1
+                x1 = hi - gr * (hi - lo)
+                f1 = wrist_err(x1)
+        q_s[self.s_qidx7[3]] = float(x1 if f1 < f2 else x2)
+        self._fk_slave(q_s)
+
+        RW = self._rot(self.s_data, self.fid_sW)
+        q_wr_seed = self.wr_cfg.eff_from_q(q_s, self.wr_qidx)
+        q_wr_eff = _solve_ball_gn(
+            RW.T @ target["RmEE"], self.wr_cfg.axis_order, q_wr_seed
+        )
+        self.wr_cfg.write_from_eff(q_s, self.wr_qidx, q_wr_eff)
+        return q_s
+
+    def _bounded_recovery(
+        self,
+        target: Dict,
+        q_template: np.ndarray,
+        q_seed7: np.ndarray,
+    ) -> Tuple[np.ndarray, object]:
+        q_seed7 = self._interior_clip(np.asarray(q_seed7, dtype=float))
+        q_regularization_ref = q_seed7.copy()
+
+        def residual(q_s7: np.ndarray) -> np.ndarray:
+            q_s = q_template.copy()
+            self._write_slave_q7(q_s, q_s7)
+            self._fk_slave(q_s)
+            pE = self._pos(self.s_data, self.fid_sE)
+            pW = self._pos(self.s_data, self.fid_sW)
+            RsEE = self._rot(self.s_data, self.fid_sEE)
+            return np.concatenate(
+                (
+                    5.0 * (pE - target["pE_s_ref"]),
+                    5.0 * (pW - target["pW_s_ref"]),
+                    pin.log3(RsEE.T @ target["RmEE"]),
+                    1e-5 * _wrap_angle_delta(q_s7 - q_regularization_ref),
+                )
+            )
+
+        result = least_squares(
+            residual,
+            q_seed7,
+            bounds=(self.s_lower7, self.s_upper7),
+            method="trf",
+            ftol=1e-12,
+            xtol=1e-12,
+            gtol=1e-12,
+            max_nfev=400,
+        )
+        q_s = q_template.copy()
+        self._write_slave_q7(q_s, result.x)
+        return q_s, result
+
+    def _solution_metrics(self, q_s: np.ndarray, target: Dict) -> Dict:
+        self._fk_slave(q_s)
+        q_s7 = self._slave_q7(q_s)
+        elbow_error = float(
+            np.linalg.norm(self._pos(self.s_data, self.fid_sE) - target["pE_s_ref"])
+        )
+        wrist_error = float(
+            np.linalg.norm(self._pos(self.s_data, self.fid_sW) - target["pW_s_ref"])
+        )
+        orientation_error = float(
+            np.linalg.norm(
+                pin.log3(
+                    self._rot(self.s_data, self.fid_sEE).T @ target["RmEE"]
+                )
+            )
+        )
+        lower_clearance = q_s7 - self.s_lower7
+        upper_clearance = self.s_upper7 - q_s7
+        violation = np.flatnonzero(
+            (lower_clearance < -1e-9) | (upper_clearance < -1e-9)
+        )
+        active = np.flatnonzero(
+            np.minimum(lower_clearance, upper_clearance) <= self.joint_limit_margin
+        )
+        return {
+            "q_s7": q_s7,
+            "elbow_position_error": elbow_error,
+            "wrist_position_error": wrist_error,
+            "orientation_error": orientation_error,
+            "joint_limit_violation_indices": violation.tolist(),
+            "joint_limit_active_indices": active.tolist(),
+            "minimum_joint_limit_clearance": float(
+                np.min(np.minimum(lower_clearance, upper_clearance))
+            ),
+        }
+
+    # ---- public API ----
+    def retarget(
+        self,
+        q_m7: np.ndarray,
+        q_s_init: Optional[np.ndarray] = None,
+    ) -> Tuple[np.ndarray, Dict]:
+        """
+        Retarget one master pose with bounded recovery.
+
+        ``success`` means the returned slave pose respects its URDF limits and
+        meets the configured elbow/wrist/orientation tolerances.  ``smooth`` is
+        stricter: it is false at reach clipping, fallback geometry, or an active
+        joint limit.  A differential may only be used when both are true.
+        """
+        q_m7 = np.asarray(q_m7, dtype=float)
+        target = self._target_from_master(q_m7)
+
+        if q_s_init is not None:
+            q_template = np.asarray(q_s_init, dtype=float).copy()
+            if q_template.shape != (self.s_model.nq,) or not np.all(np.isfinite(q_template)):
+                raise ValueError(
+                    f"q_s_init must be finite with shape ({self.s_model.nq},)"
+                )
+            # Supplying q_s_init explicitly requests this local branch.
+            seeds = [self._slave_q7(q_template)]
+        else:
+            q_template = pin.neutral(self.s_model)
+            staged = self._staged_seed(target, q_template)
+            midpoint = 0.5 * (self.s_lower7 + self.s_upper7)
+            seeds = [self._slave_q7(staged), midpoint]
+
+        candidates = [
+            self._bounded_recovery(target, q_template, seed) for seed in seeds
+        ]
+        candidate_metrics = [
+            self._solution_metrics(q_s, target) for q_s, _ in candidates
+        ]
+
+        def task_score(metrics: Dict) -> float:
+            return (
+                25.0 * metrics["elbow_position_error"] ** 2
+                + 25.0 * metrics["wrist_position_error"] ** 2
+                + metrics["orientation_error"] ** 2
+            )
+
+        selected_index = int(
+            np.argmin([task_score(metrics) for metrics in candidate_metrics])
+        )
+        q_s, result = candidates[selected_index]
+        metrics = candidate_metrics[selected_index]
+
+        events = list(target["events"])
+        if metrics["joint_limit_violation_indices"]:
+            events.append("joint_limit_violation")
+        if metrics["joint_limit_active_indices"]:
+            events.append("joint_limit_active")
+
+        finite = bool(
+            np.all(np.isfinite(metrics["q_s7"]))
+            and np.isfinite(metrics["elbow_position_error"])
+            and np.isfinite(metrics["wrist_position_error"])
+            and np.isfinite(metrics["orientation_error"])
+        )
+        within_tolerance = bool(
+            metrics["elbow_position_error"] <= self.position_tolerance
+            and metrics["wrist_position_error"] <= self.position_tolerance
+            and metrics["orientation_error"] <= self.orientation_tolerance
+        )
+        success = bool(
+            finite
+            and result.success
+            and target["hard_geometry_valid"]
+            and not metrics["joint_limit_violation_indices"]
+            and within_tolerance
+        )
+        if not result.success:
+            events.append("bounded_solver_not_converged")
+        if not within_tolerance:
+            events.append("task_tolerance_exceeded")
+        if not target["hard_geometry_valid"]:
+            events.append("invalid_master_geometry")
+
+        nonsmooth_events = {
+            "reach_clipped_lower",
+            "reach_clipped_upper",
+            "reference_axis_fallback",
+            "master_shoulder_wrist_degenerate",
+            "master_arm_plane_degenerate",
+            "joint_limit_active",
+            "joint_limit_violation",
+        }
+        smooth = bool(success and not any(event in nonsmooth_events for event in events))
+        position_error = max(
+            metrics["elbow_position_error"], metrics["wrist_position_error"]
+        )
+        dbg = {
+            **metrics,
+            "success": success,
+            "valid": success,
+            "invalid": not success,
+            "smooth": smooth,
+            "events": tuple(dict.fromkeys(events)),
+            "pE_s_ref": target["pE_s_ref"].copy(),
+            "pW_s_ref": target["pW_s_ref"].copy(),
+            "reach_clipped": target["reach_clipped"],
+            "clipped": target["reach_clipped"],
+            "clip_region": target["clip_region"],
+            "near_limit": bool(metrics["joint_limit_active_indices"]),
+            "position_error": position_error,
+            "reference_fallback": target["reference_fallback"],
+            "master_reach": target["master_reach"],
+            "slave_reach": target["slave_reach"],
+            "master_arm_normal_norm": target["master_arm_normal_norm"],
+            "solver_success": bool(result.success),
+            "solver_status": int(result.status),
+            "solver_cost": float(result.cost),
+            "solver_nfev": int(result.nfev),
+            "selected_seed_index": selected_index,
+        }
+        self._fk_slave(q_s)
+        return q_s, dbg
+
+    def compute_differential(
+        self,
+        q_m7: np.ndarray,
+        q_s_init: Optional[np.ndarray] = None,
+        fd_step: float = 1e-4,
+        branch_jump_threshold: float = 0.25,
+        consistency_tolerance: float = 5e-2,
+    ) -> Tuple[np.ndarray, Dict]:
+        """
+        Compute A = d(q_slave)/d(q_master) by a central finite difference.
+
+        Every +/- solve starts from the same bounded base solution.  Slave
+        angle differences are wrapped before division.  Invalid/nonsmooth
+        columns are filled with NaN; callers must also check ``info["valid"]``.
+        """
+        q_m7 = np.asarray(q_m7, dtype=float)
+        if q_m7.shape != (7,) or not np.all(np.isfinite(q_m7)):
+            raise ValueError("q_m7 must be a finite vector with shape (7,)")
+        if not np.isfinite(fd_step) or fd_step <= 0.0:
+            raise ValueError("fd_step must be finite and positive")
+        if branch_jump_threshold <= 0.0 or consistency_tolerance <= 0.0:
+            raise ValueError("differential thresholds must be positive")
+
+        base_q_s, base_dbg = self.retarget(q_m7, q_s_init=q_s_init)
+        base_q7 = self._slave_q7(base_q_s)
+        A = np.full((7, 7), np.nan, dtype=float)
+        events: List[str] = []
+        columns: List[Dict] = []
+
+        if not base_dbg["success"]:
+            events.append("base_invalid")
+        if not base_dbg["smooth"]:
+            events.append("base_nonsmooth")
+
+        if base_dbg["success"] and base_dbg["smooth"]:
+            for j in range(7):
+                q_plus = q_m7.copy()
+                q_minus = q_m7.copy()
+                q_plus[j] += fd_step
+                q_minus[j] -= fd_step
+
+                plus_q_s, plus_dbg = self.retarget(q_plus, q_s_init=base_q_s)
+                minus_q_s, minus_dbg = self.retarget(q_minus, q_s_init=base_q_s)
+                plus_q7 = self._slave_q7(plus_q_s)
+                minus_q7 = self._slave_q7(minus_q_s)
+
+                plus_jump = float(
+                    np.max(np.abs(_wrap_angle_delta(plus_q7 - base_q7)))
+                )
+                minus_jump = float(
+                    np.max(np.abs(_wrap_angle_delta(minus_q7 - base_q7)))
+                )
+                fwd = _wrap_angle_delta(plus_q7 - base_q7) / fd_step
+                bwd = _wrap_angle_delta(base_q7 - minus_q7) / fd_step
+                consistency = float(
+                    np.linalg.norm(fwd - bwd)
+                    / (1.0 + max(np.linalg.norm(fwd), np.linalg.norm(bwd)))
+                )
+
+                column_events: List[str] = []
+                if not plus_dbg["success"] or not minus_dbg["success"]:
+                    column_events.append("perturbation_invalid")
+                if not plus_dbg["smooth"] or not minus_dbg["smooth"]:
+                    column_events.append("perturbation_nonsmooth")
+                if (
+                    plus_dbg["clip_region"] != base_dbg["clip_region"]
+                    or minus_dbg["clip_region"] != base_dbg["clip_region"]
+                ):
+                    column_events.append("clip_region_changed")
+                if (
+                    plus_jump > branch_jump_threshold
+                    or minus_jump > branch_jump_threshold
+                ):
+                    column_events.append("branch_jump")
+                if consistency > consistency_tolerance:
+                    column_events.append("one_sided_derivative_mismatch")
+
+                column_valid = not column_events
+                if column_valid:
+                    A[:, j] = _wrap_angle_delta(plus_q7 - minus_q7) / (
+                        2.0 * fd_step
+                    )
+                else:
+                    events.extend(f"column_{j}:{event}" for event in column_events)
+
+                columns.append(
+                    {
+                        "index": j,
+                        "valid": column_valid,
+                        "events": tuple(column_events),
+                        "plus_success": plus_dbg["success"],
+                        "minus_success": minus_dbg["success"],
+                        "plus_jump": plus_jump,
+                        "minus_jump": minus_jump,
+                        "one_sided_consistency": consistency,
+                    }
+                )
+
+        valid = bool(
+            base_dbg["success"]
+            and base_dbg["smooth"]
+            and len(columns) == 7
+            and all(column["valid"] for column in columns)
+            and np.all(np.isfinite(A))
+        )
+        info = {
+            "valid": valid,
+            "invalid": not valid,
+            "smooth": valid,
+            "events": tuple(dict.fromkeys(events)),
+            "fd_step": float(fd_step),
+            "fixed_branch_seed": base_q7.copy(),
+            "base": base_dbg,
+            "columns": tuple(columns),
+        }
+        self._fk_slave(base_q_s)
+        return A, info
+
+    def retarget_with_differential(
+        self,
+        q_m7: np.ndarray,
+        q_s_init: Optional[np.ndarray] = None,
+        fd_step: float = 1e-4,
+        branch_jump_threshold: float = 0.25,
+        consistency_tolerance: float = 5e-2,
+    ) -> Tuple[np.ndarray, np.ndarray, Dict]:
+        """
+        Stable simulation-facing interface returning ``(q_s, A, debug)``.
+
+        ``debug["success"]`` describes the bounded pose recovery;
+        ``debug["differential_valid"]`` must be checked independently before
+        using A for velocity/force mapping.
+        """
+        q_s, pose_debug = self.retarget(q_m7, q_s_init=q_s_init)
+        A, differential_debug = self.compute_differential(
+            q_m7,
+            q_s_init=q_s,
+            fd_step=fd_step,
+            branch_jump_threshold=branch_jump_threshold,
+            consistency_tolerance=consistency_tolerance,
+        )
+        debug = {
+            **pose_debug,
+            "differential_valid": bool(differential_debug["valid"]),
+            "differential": differential_debug,
+        }
+        self._fk_slave(q_s)
+        return q_s, A, debug

code/core/time_domain_popc.py

@@ -0,0 +1,132 @@
+"""Discrete time-domain passivity observer/controller at the master port."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class POPCDiagnostics:
+    tau_candidate: np.ndarray
+    tau_applied: np.ndarray
+    observer_before: float
+    observer_preclip: float
+    observer_after: float
+    candidate_power: float
+    applied_power: float
+    damping_gain: float
+    intervention_active: bool
+    fail_safe_active: bool
+
+
+class TimeDomainPOPC:
+    """Causal PO/PC using dissipative velocity feedback.
+
+    Positive ``tau @ qd`` is energy delivered by the device.  When the
+    candidate would exhaust the observer balance, the controller injects
+    ``-beta * qd``.  This is intentionally a separate baseline from the radial
+    tank projection implemented in :mod:`core.haptic_render`.
+    """
+
+    def __init__(
+        self,
+        *,
+        initial_energy: float = 0.0,
+        minimum_energy: float = 0.0,
+        maximum_energy: float = np.inf,
+        velocity_epsilon: float = 1e-12,
+    ):
+        if not np.isfinite(initial_energy) or not np.isfinite(minimum_energy):
+            raise ValueError("initial and minimum energy must be finite")
+        if maximum_energy <= minimum_energy:
+            raise ValueError("maximum_energy must exceed minimum_energy")
+        if not minimum_energy <= initial_energy <= maximum_energy:
+            raise ValueError("initial_energy must lie within observer bounds")
+        if velocity_epsilon <= 0.0:
+            raise ValueError("velocity_epsilon must be positive")
+        self.minimum_energy = float(minimum_energy)
+        self.maximum_energy = float(maximum_energy)
+        self.velocity_epsilon = float(velocity_epsilon)
+        self.energy = float(initial_energy)
+        self.last_diagnostics: POPCDiagnostics | None = None
+
+    def reset(self, energy: float | None = None) -> None:
+        target = self.energy if energy is None else float(energy)
+        if not self.minimum_energy <= target <= self.maximum_energy:
+            raise ValueError("reset energy lies outside observer bounds")
+        self.energy = target
+        self.last_diagnostics = None
+
+    def apply(
+        self,
+        tau_candidate: np.ndarray,
+        qd_master: np.ndarray,
+        dt: float,
+    ) -> tuple[np.ndarray, POPCDiagnostics]:
+        candidate = np.asarray(tau_candidate, dtype=float).reshape(-1)
+        velocity = np.asarray(qd_master, dtype=float).reshape(-1)
+        if candidate.shape != velocity.shape or candidate.size == 0:
+            raise ValueError("candidate and velocity must have equal non-empty shapes")
+        before = self.energy
+        if (
+            not np.isfinite(dt)
+            or dt <= 0.0
+            or not np.all(np.isfinite(candidate))
+            or not np.all(np.isfinite(velocity))
+        ):
+            applied = np.zeros_like(candidate)
+            diagnostics = POPCDiagnostics(
+                tau_candidate=candidate.copy(),
+                tau_applied=applied,
+                observer_before=before,
+                observer_preclip=before,
+                observer_after=before,
+                candidate_power=0.0,
+                applied_power=0.0,
+                damping_gain=0.0,
+                intervention_active=True,
+                fail_safe_active=True,
+            )
+            self.last_diagnostics = diagnostics
+            return applied, diagnostics
+
+        candidate_power = float(candidate @ velocity)
+        available = max(0.0, before - self.minimum_energy)
+        allowed_power = available / dt
+        damping_gain = 0.0
+        applied = candidate.copy()
+        velocity_norm_sq = float(velocity @ velocity)
+        if candidate_power > allowed_power:
+            if velocity_norm_sq <= self.velocity_epsilon:
+                # With an almost-zero velocity, nonzero power is numerical
+                # contamination; zero output is the conservative response.
+                applied.fill(0.0)
+            else:
+                damping_gain = (
+                    candidate_power - allowed_power
+                ) / velocity_norm_sq
+                applied = candidate - damping_gain * velocity
+
+        applied_power = float(applied @ velocity)
+        preclip = before - applied_power * dt
+        self.energy = float(
+            np.clip(preclip, self.minimum_energy, self.maximum_energy)
+        )
+        diagnostics = POPCDiagnostics(
+            tau_candidate=candidate.copy(),
+            tau_applied=applied.copy(),
+            observer_before=before,
+            observer_preclip=preclip,
+            observer_after=self.energy,
+            candidate_power=candidate_power,
+            applied_power=applied_power,
+            damping_gain=damping_gain,
+            intervention_active=bool(
+                np.any(np.abs(applied - candidate) > 1e-12)
+            ),
+            fail_safe_active=False,
+        )
+        self.last_diagnostics = diagnostics
+        return applied, diagnostics

code/core/wrench_solver.py

@@ -0,0 +1,306 @@
+"""Dimensionally scaled residual-to-wrench solvers for H2.
+
+Twists and Jacobians use ``[linear; angular]`` order and wrenches use
+``[force; moment]`` order.  For characteristic length ``ell`` this module
+implements the manuscript convention
+
+``S = diag(1/ell I3, I3)``, ``J_tilde = S J`` and
+``F_tilde = S**(-T) F``.
+
+Consequently ``J_tilde.T @ F_tilde == J.T @ F`` and every element of
+``F_tilde`` has torque units.  Both the damped and undamped methods use the
+same scaling and frozen relative rank tolerance so their H2 comparison is
+well-defined.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from enum import Enum
+from typing import Protocol, runtime_checkable
+
+import numpy as np
+
+
+def _vector(value, size: int, name: str) -> np.ndarray:
+    array = np.asarray(value, dtype=float).reshape(-1)
+    if array.shape != (size,):
+        raise ValueError(f"{name} must have shape ({size},), got {array.shape}")
+    if not np.all(np.isfinite(array)):
+        raise ValueError(f"{name} must contain only finite values")
+    return array
+
+
+def _jacobian(value) -> np.ndarray:
+    array = np.asarray(value, dtype=float)
+    if array.ndim != 2 or array.shape[0] != 6 or array.shape[1] == 0:
+        raise ValueError(
+            f"jacobian must have shape (6, n_joints), got {array.shape}"
+        )
+    if not np.all(np.isfinite(array)):
+        raise ValueError("jacobian must contain only finite values")
+    return array
+
+
+def _readonly(value: np.ndarray) -> np.ndarray:
+    result = np.asarray(value, dtype=float).copy()
+    result.setflags(write=False)
+    return result
+
+
+class WrenchSolveStatus(str, Enum):
+    """Numerical rank status after the frozen scaled-Jacobian test."""
+
+    FULL_RANK = "full_rank"
+    RANK_DEFICIENT = "rank_deficient"
+
+
+@dataclass(frozen=True)
+class WrenchSolveResult:
+    """Solver result and all quantities needed for H2 stratification."""
+
+    method: str
+    wrench: np.ndarray
+    scaled_wrench: np.ndarray
+    reconstructed_residual: np.ndarray
+    residual_error_norm: float
+    singular_values: np.ndarray
+    rank: int
+    rank_threshold: float
+    condition_number: float
+    status: WrenchSolveStatus
+    characteristic_length_m: float
+    damping: float
+
+    def __post_init__(self) -> None:
+        wrench = _vector(self.wrench, 6, "wrench")
+        scaled_wrench = _vector(self.scaled_wrench, 6, "scaled_wrench")
+        reconstructed = np.asarray(
+            self.reconstructed_residual, dtype=float
+        ).reshape(-1)
+        singular_values = np.asarray(self.singular_values, dtype=float).reshape(-1)
+        if reconstructed.size == 0 or not np.all(np.isfinite(reconstructed)):
+            raise ValueError("reconstructed_residual must be a finite vector")
+        if singular_values.size == 0 or not np.all(np.isfinite(singular_values)):
+            raise ValueError("singular_values must be a non-empty finite vector")
+        if self.rank < 0 or self.rank > 6:
+            raise ValueError("rank must lie in [0, 6]")
+        finite_nonnegative = (
+            self.residual_error_norm,
+            self.rank_threshold,
+            self.characteristic_length_m,
+            self.damping,
+        )
+        if any(not np.isfinite(value) or value < 0.0 for value in finite_nonnegative):
+            raise ValueError(
+                "solver scalar diagnostics must be finite and non-negative"
+            )
+        if not (
+            np.isfinite(self.condition_number)
+            or np.isinf(self.condition_number)
+        ):
+            raise ValueError("condition_number must be finite or infinity")
+        object.__setattr__(self, "wrench", _readonly(wrench))
+        object.__setattr__(self, "scaled_wrench", _readonly(scaled_wrench))
+        object.__setattr__(
+            self, "reconstructed_residual", _readonly(reconstructed)
+        )
+        object.__setattr__(self, "singular_values", _readonly(singular_values))
+
+    @property
+    def force(self) -> np.ndarray:
+        return self.wrench[:3].copy()
+
+    @property
+    def moment(self) -> np.ndarray:
+        return self.wrench[3:].copy()
+
+
+@runtime_checkable
+class WrenchSolver(Protocol):
+    """Common H2 solver interface."""
+
+    name: str
+    characteristic_length_m: float
+    relative_rank_tolerance: float
+
+    def solve(
+        self, jacobian: np.ndarray, joint_residual: np.ndarray
+    ) -> WrenchSolveResult:
+        ...
+
+
+class _ScaledSolverBase:
+    def __init__(
+        self,
+        characteristic_length_m: float,
+        *,
+        relative_rank_tolerance: float = 1e-9,
+    ) -> None:
+        length = float(characteristic_length_m)
+        tolerance = float(relative_rank_tolerance)
+        if not np.isfinite(length) or length <= 0.0:
+            raise ValueError(
+                "characteristic_length_m must be finite and positive"
+            )
+        if not np.isfinite(tolerance) or tolerance <= 0.0 or tolerance >= 1.0:
+            raise ValueError(
+                "relative_rank_tolerance must be finite and lie in (0, 1)"
+            )
+        self.characteristic_length_m = length
+        self.relative_rank_tolerance = tolerance
+        self._scaling = np.diag(
+            np.array([1.0 / length] * 3 + [1.0] * 3, dtype=float)
+        )
+
+    def scaled_jacobian(self, jacobian: np.ndarray) -> np.ndarray:
+        """Return ``S_ell @ J`` using the frozen characteristic length."""
+        return self._scaling @ _jacobian(jacobian)
+
+    def _decompose(
+        self, jacobian: np.ndarray, joint_residual: np.ndarray
+    ) -> tuple[
+        np.ndarray,
+        np.ndarray,
+        np.ndarray,
+        np.ndarray,
+        np.ndarray,
+        float,
+        int,
+    ]:
+        jacobian = _jacobian(jacobian)
+        residual = _vector(
+            joint_residual, jacobian.shape[1], "joint_residual"
+        )
+        scaled_jacobian = self._scaling @ jacobian
+        u, singular_values, vt = np.linalg.svd(
+            scaled_jacobian, full_matrices=False
+        )
+        largest = float(singular_values[0]) if singular_values.size else 0.0
+        threshold = self.relative_rank_tolerance * largest
+        rank = int(np.count_nonzero(singular_values > threshold))
+        return (
+            jacobian,
+            residual,
+            u,
+            singular_values,
+            vt,
+            threshold,
+            rank,
+        )
+
+    def _result(
+        self,
+        *,
+        jacobian: np.ndarray,
+        residual: np.ndarray,
+        scaled_wrench: np.ndarray,
+        singular_values: np.ndarray,
+        threshold: float,
+        rank: int,
+        damping: float,
+    ) -> WrenchSolveResult:
+        wrench = self._scaling.T @ scaled_wrench
+        reconstructed = jacobian.T @ wrench
+        smallest = float(singular_values[-1])
+        condition = (
+            float(singular_values[0] / smallest)
+            if rank == 6 and smallest > 0.0
+            else float("inf")
+        )
+        return WrenchSolveResult(
+            method=self.name,
+            wrench=wrench,
+            scaled_wrench=scaled_wrench,
+            reconstructed_residual=reconstructed,
+            residual_error_norm=float(np.linalg.norm(reconstructed - residual)),
+            singular_values=singular_values,
+            rank=rank,
+            rank_threshold=threshold,
+            condition_number=condition,
+            status=(
+                WrenchSolveStatus.FULL_RANK
+                if rank == 6
+                else WrenchSolveStatus.RANK_DEFICIENT
+            ),
+            characteristic_length_m=self.characteristic_length_m,
+            damping=damping,
+        )
+
+
+class ScaledDLSSolver(_ScaledSolverBase):
+    """Dimensionally scaled damped least-squares wrench reconstruction."""
+
+    name = "scaled_dls"
+
+    def __init__(
+        self,
+        characteristic_length_m: float,
+        damping: float,
+        *,
+        relative_rank_tolerance: float = 1e-9,
+    ) -> None:
+        super().__init__(
+            characteristic_length_m,
+            relative_rank_tolerance=relative_rank_tolerance,
+        )
+        damping_value = float(damping)
+        if not np.isfinite(damping_value) or damping_value <= 0.0:
+            raise ValueError("damping must be finite and positive")
+        self.damping = damping_value
+
+    def solve(
+        self, jacobian: np.ndarray, joint_residual: np.ndarray
+    ) -> WrenchSolveResult:
+        (
+            jacobian,
+            residual,
+            u,
+            singular_values,
+            vt,
+            threshold,
+            rank,
+        ) = self._decompose(jacobian, joint_residual)
+        gains = singular_values / (singular_values**2 + self.damping**2)
+        scaled_wrench = u @ (gains * (vt @ residual))
+        return self._result(
+            jacobian=jacobian,
+            residual=residual,
+            scaled_wrench=scaled_wrench,
+            singular_values=singular_values,
+            threshold=threshold,
+            rank=rank,
+            damping=self.damping,
+        )
+
+
+class UndampedSVDSolver(_ScaledSolverBase):
+    """Frozen-tolerance, dimensionally scaled undamped pseudoinverse baseline."""
+
+    name = "undamped_svd"
+
+    def solve(
+        self, jacobian: np.ndarray, joint_residual: np.ndarray
+    ) -> WrenchSolveResult:
+        (
+            jacobian,
+            residual,
+            u,
+            singular_values,
+            vt,
+            threshold,
+            rank,
+        ) = self._decompose(jacobian, joint_residual)
+        inverse = np.zeros_like(singular_values)
+        retained = singular_values > threshold
+        inverse[retained] = 1.0 / singular_values[retained]
+        scaled_wrench = u @ (inverse * (vt @ residual))
+        return self._result(
+            jacobian=jacobian,
+            residual=residual,
+            scaled_wrench=scaled_wrench,
+            singular_values=singular_values,
+            threshold=threshold,
+            rank=rank,
+            damping=0.0,
+        )

code/demo.py

@@ -0,0 +1,14 @@
+#!/usr/bin/env python3
+"""Compatibility entry point for the closed-loop bilateral simulation.
+
+The former demo advanced the slave with ``q_des + random_noise`` and therefore
+did not simulate robot dynamics or a dynamically coupled virtual wall.  The
+real implementation now lives in :mod:`simulate_closed_loop`; this file stays
+as the short command users of the repository already know.
+"""
+
+from simulate_closed_loop import main
+
+
+if __name__ == "__main__":
+    main()

code/diagrams/closed_loop_control.py

@@ -0,0 +1,48 @@
+from diagrams import Diagram, Cluster
+from diagrams.generic.compute import Rack
+from diagrams.generic.blank import Blank
+from diagrams.generic.network import Switch
+
+# We will use Rack as generic processing block
+# Blank as hidden connectors
+
+with Diagram("Closed-loop Teleoperation Architecture",
+             show=False, direction="LR",
+             outformat="png"):
+
+    # ---- Human operator ----
+    human = Rack("Human\noperator")
+
+    # ---- Master exoskeleton ----
+    master = Rack("Master exoskeleton\nsensing & actuation")
+
+    # ---- SEW retargeting ----
+    sew = Rack("SEW-based posture\nretargeting (IK-free)")
+
+    # ---- Slave controller ----
+    ctrl = Rack("Slave joint\ncontroller")
+
+    # ---- Slave arm ----
+    slave = Rack("7-DoF slave arm\n+ environment")
+
+    # ---- Bottom blocks ----
+    est = Rack("Interaction\nforce estimator")
+    haptic = Rack("Haptic rendering\n+ energy tank")
+
+    # ---- Connections (Main forward loop) ----
+    human >> master
+    master >> sew
+    sew >> ctrl
+    ctrl >> slave
+
+    # ---- Feedback loop ----
+    slave >> est
+    est >> haptic
+    haptic >> master
+
+    # Optional: chest-frame Jacobian dashed arrows (use Blank as endpoints)
+    Jm = Blank(" ")
+    Js = Blank(" ")
+
+    master >> Jm >> haptic
+    slave >> Js >> est

code/experiments/README.md

@@ -0,0 +1,81 @@
+# Evidence pipeline
+
+This package is the experiment/evidence layer. The pre-prototype executors in
+`experiments.executors` adapt immutable trial records to H1 retargeting, H2
+synthetic sensitivity, and H3/H4 rigid-body simulation backends.
+
+## Minimal workflow
+
+From the repository root with `PYTHONPATH=code`:
+
+```bash
+python -m experiments.cli plan \
+  --spec code/config/experiments/h1_smoke.json \
+  --output /tmp/h1-plan.json
+
+python -m experiments.cli run \
+  --plan /tmp/h1-plan.json \
+  --batch-dir output/experiments/h1-smoke \
+  --executor experiments.executors:execute_h1_retargeting
+```
+
+Available executor/config pairs are:
+
+```text
+execute_h1_retargeting       h1_smoke.json / h1_calibration.json
+execute_h2_synthetic         h2_smoke.json / h2_calibration.json
+execute_bilateral_simulation smoke.json / bilateral_calibration.json
+```
+
+An executor callable receives one immutable trial mapping and returns:
+
+```python
+TrialPayload(
+    samples={"time": time_array, "...": sample_array},
+    events=[{"sample_index": 10, "event": "contact"}],
+    metadata={"backend": "simulation"},
+)
+```
+
+Every sample array must have the same first dimension. Object arrays are
+rejected. Successful trials are committed by one atomic directory rename;
+failures are retained separately and may be retried with `--resume`.
+
+Validate a completed batch:
+
+```bash
+python -m experiments.cli validate \
+  --batch-dir output/experiments/h1-smoke
+```
+
+Recompute independent endpoints and paper source-data:
+
+```bash
+python -m analysis.make_paper_artifacts \
+  --batch-dir output/experiments/h1-smoke \
+  --metric-config code/config/experiments/metrics_h1_calibration.json
+```
+
+Use `metrics_h2.json` for H2 batches and `metrics_bilateral.json` for bilateral
+batches.  The bilateral configuration derives H3 for every mapping/supervisor
+condition, but its H4 table contains only the three tank-supervised methods;
+PO/PC and bypass conditions cannot be silently mixed into a tank audit.
+
+The declared minimal storage contract is JSON for manifests/plans, NPZ for
+numeric sample arrays, JSON Lines for events/trial metrics, and CSV for paper
+source-data. No Parquet dependency is required.
+
+## Pairing and random numbers
+
+`pair_id` excludes the method and therefore identifies common inputs.
+`trial_id` includes the method. The trajectory, sensor, model, and network
+streams are derived independently with NumPy `SeedSequence`; their serialized
+states are identical across methods in the same pair.
+
+Calibration, pilot, and locked studies must use separate specifications. A
+locked plan is immutable: changing a factor, method, trajectory, or seed
+invalidates its hashes.
+
+Files named `*_locked_template.json` are deliberately not confirmatory plans.
+Copy and freeze them only after calibration thresholds, safety limits, repeat
+counts, the source commit, and the analysis configuration have been approved.

code/experiments/__init__.py

@@ -0,0 +1,18 @@
+"""Reproducible experiment orchestration for the teleoperation study.
+
+The package is intentionally independent from the current simulation and core
+controllers.  It provides evidence-layer contracts (plans, manifests, storage,
+validation, and random streams) that executors can adopt without changing the
+algorithms they exercise.
+"""
+
+from .plan import build_trial_plan, load_document, validate_trial_plan
+from .runner import TrialPayload, run_trial_plan
+
+__all__ = [
+    "TrialPayload",
+    "build_trial_plan",
+    "load_document",
+    "run_trial_plan",
+    "validate_trial_plan",
+]

code/experiments/cli.py

@@ -0,0 +1,90 @@
+"""Command-line interface for planning, running, and validating studies."""
+
+from __future__ import annotations
+
+import argparse
+import importlib
+import json
+from pathlib import Path
+from typing import Any
+
+from .io import atomic_write_json
+from .plan import build_trial_plan, load_document, validate_trial_plan
+from .runner import run_trial_plan
+from .validate import validate_batch
+
+
+def _load_executor(specification: str):
+    if ":" not in specification:
+        raise ValueError("executor must use module:function syntax")
+    module_name, attribute_name = specification.split(":", 1)
+    module = importlib.import_module(module_name)
+    executor = getattr(module, attribute_name)
+    if not callable(executor):
+        raise TypeError(f"{specification} is not callable")
+    return executor
+
+
+def build_parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(description="Reproducible experiment runner")
+    subparsers = parser.add_subparsers(dest="command", required=True)
+
+    plan_parser = subparsers.add_parser("plan", help="expand a study specification")
+    plan_parser.add_argument("--spec", type=Path, required=True)
+    plan_parser.add_argument("--output", type=Path, required=True)
+
+    run_parser = subparsers.add_parser("run", help="execute an immutable plan")
+    run_parser.add_argument("--plan", type=Path, required=True)
+    run_parser.add_argument("--batch-dir", type=Path, required=True)
+    run_parser.add_argument("--executor", default=None)
+    run_parser.add_argument("--dry-run", action="store_true")
+    run_parser.add_argument("--no-resume", action="store_true")
+    run_parser.add_argument("--stop-on-error", action="store_true")
+    run_parser.add_argument("--max-trials", type=int, default=None)
+
+    validate_parser = subparsers.add_parser("validate", help="validate a batch")
+    validate_parser.add_argument("--batch-dir", type=Path, required=True)
+    validate_parser.add_argument("--allow-incomplete", action="store_true")
+    return parser
+
+
+def main(argv: list[str] | None = None) -> int:
+    args = build_parser().parse_args(argv)
+    result: dict[str, Any]
+    if args.command == "plan":
+        plan = build_trial_plan(load_document(args.spec))
+        atomic_write_json(args.output, plan)
+        result = {
+            "kind": "plan_summary",
+            "schema_version": plan["schema_version"],
+            "study_id": plan["study_id"],
+            "split": plan["split"],
+            "pair_count": plan["pair_count"],
+            "trial_count": plan["trial_count"],
+            "plan_hash": plan["plan_hash"],
+            "output": str(args.output.resolve()),
+        }
+    elif args.command == "run":
+        plan = load_document(args.plan)
+        validate_trial_plan(plan)
+        executor = None if args.executor is None else _load_executor(args.executor)
+        result = run_trial_plan(
+            plan,
+            args.batch_dir,
+            executor,
+            resume=not args.no_resume,
+            dry_run=args.dry_run,
+            continue_on_error=not args.stop_on_error,
+            max_trials=args.max_trials,
+        )
+    else:
+        result = validate_batch(
+            args.batch_dir,
+            require_complete=not args.allow_incomplete,
+        )
+    print(json.dumps(result, indent=2, ensure_ascii=False, sort_keys=True))
+    return 0 if result.get("valid", True) else 1
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

code/experiments/executors.py

@@ -0,0 +1,492 @@
+"""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.io import TrialPayload
+from experiments.rng import generator_from_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 _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 a continuous, bounded master trajectory from a frozen spec."""
+    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":
+        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 == "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])
+
+    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)
+    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
+
+
+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)
+    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"])
+    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)
+        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]),
+                }
+            )
+
+    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_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
+            },
+            "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 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))
+    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")
+
+    model_rng = generator_from_record(trial["seeds"], "model")
+    sensor_rng = generator_from_record(trial["seeds"], "sensor")
+    trajectory_rng = generator_from_record(trial["seeds"], "trajectory")
+    u = _orthogonal(model_rng, 6)
+    v = _orthogonal(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, :]
+    inverse_scaling = np.diag(
+        [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-frozen-v1",
+    )
+    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)
+    truth_jacobian = np.empty((count, 42))
+    estimator_jacobian = np.empty((count, 42))
+    residual_raw = np.empty((count, 7))
+    residual_corrected = np.empty((count, 7))
+    for index in range(count):
+        smooth_change = 0.015 * np.sin(phase[index])
+        J_truth = inverse_scaling @ (
+            base_scaled_truth
+            + smooth_change * model_rng.normal(size=(6, 7))
+        )
+        J_estimator = J_truth + inverse_scaling @ (
+            model_error_std * model_rng.normal(size=(6, 7))
+        )
+        interaction = J_truth.T @ wrench_reference[index]
+        measured = (
+            interaction
+            + bias
+            + friction.torque(qd[index])
+            + sensor_rng.normal(0.0, noise_std, 7)
+        )
+        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)
+        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,
+            "tau_residual_raw": residual_raw,
+            "tau_residual_corrected": residual_corrected,
+        },
+        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,
+        },
+    )
+
+
+def execute_bilateral_simulation(trial: Mapping[str, Any]) -> TrialPayload:
+    """Run one paired H3/H4 rigid-body trial with a frozen network trace."""
+    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(_factor(trial, "return_delay_s", 0.04)),
+        forward_delay_s=float(_factor(trial, "forward_delay_s", 0.0)),
+        return_jitter_s=float(_factor(trial, "return_jitter_s", 0.0)),
+        forward_jitter_s=float(_factor(trial, "forward_jitter_s", 0.0)),
+        return_packet_loss=float(_factor(trial, "return_packet_loss", 0.0)),
+        forward_packet_loss=float(_factor(trial, "forward_packet_loss", 0.0)),
+        wall_stiffness=float(_factor(trial, "wall_stiffness", 800.0)),
+        wall_damping=float(_factor(trial, "wall_damping", 45.0)),
+    )
+    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()
+    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,
+            },
+            "wall": wall_metadata,
+            "online_metrics_are_diagnostic_only": result.metrics,
+        },
+    )