commit 2effd7b88da64dffac0148c631fd09629a35f0d2 Author: xtkuang <87661715@qq.com> Date: Mon Jul 27 12:29:49 2026 +0800 Initial reproducible teleoperation paper and simulation diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..dbc8578 --- /dev/null +++ b/.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 diff --git a/README.md b/README.md new file mode 100644 index 0000000..8246870 --- /dev/null +++ b/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. diff --git a/code/analysis/__init__.py b/code/analysis/__init__.py new file mode 100644 index 0000000..aa0d4f8 --- /dev/null +++ b/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", +] diff --git a/code/analysis/make_paper_artifacts.py b/code/analysis/make_paper_artifacts.py new file mode 100644 index 0000000..1662028 --- /dev/null +++ b/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()) diff --git a/code/analysis/metrics.py b/code/analysis/metrics.py new file mode 100644 index 0000000..5b7bd9c --- /dev/null +++ b/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 diff --git a/code/config/config.yaml b/code/config/config.yaml new file mode 100644 index 0000000..c183327 --- /dev/null +++ b/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 diff --git a/code/config/dual_arm.mjcf b/code/config/dual_arm.mjcf new file mode 100644 index 0000000..13cb17d --- /dev/null +++ b/code/config/dual_arm.mjcf @@ -0,0 +1,95 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/config/dual_arm.urdf b/code/config/dual_arm.urdf new file mode 100644 index 0000000..3bb4689 --- /dev/null +++ b/code/config/dual_arm.urdf @@ -0,0 +1,553 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/config/experiments/bilateral_calibration.json b/code/config/experiments/bilateral_calibration.json new file mode 100644 index 0000000..ba21f9f --- /dev/null +++ b/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 + ] + } +} diff --git a/code/config/experiments/bilateral_locked_template.json b/code/config/experiments/bilateral_locked_template.json new file mode 100644 index 0000000..469b0bb --- /dev/null +++ b/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 + ] + } +} diff --git a/code/config/experiments/g0c_calibration_example.json b/code/config/experiments/g0c_calibration_example.json new file mode 100644 index 0000000..a8ee572 --- /dev/null +++ b/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" + } +} diff --git a/code/config/experiments/h1_calibration.json b/code/config/experiments/h1_calibration.json new file mode 100644 index 0000000..41f9261 --- /dev/null +++ b/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 + } + ] +} diff --git a/code/config/experiments/h1_locked_template.json b/code/config/experiments/h1_locked_template.json new file mode 100644 index 0000000..735e584 --- /dev/null +++ b/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 + } + ] +} diff --git a/code/config/experiments/h1_smoke.json b/code/config/experiments/h1_smoke.json new file mode 100644 index 0000000..f3be27b --- /dev/null +++ b/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 + } + ] +} diff --git a/code/config/experiments/h2_calibration.json b/code/config/experiments/h2_calibration.json new file mode 100644 index 0000000..23bc1c1 --- /dev/null +++ b/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 + ] + } +} diff --git a/code/config/experiments/h2_locked_template.json b/code/config/experiments/h2_locked_template.json new file mode 100644 index 0000000..e6b6ecd --- /dev/null +++ b/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 + ] + } +} diff --git a/code/config/experiments/h2_smoke.json b/code/config/experiments/h2_smoke.json new file mode 100644 index 0000000..7d5cc10 --- /dev/null +++ b/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 + ] + } +} diff --git a/code/config/experiments/metrics_bilateral.json b/code/config/experiments/metrics_bilateral.json new file mode 100644 index 0000000..dded1b1 --- /dev/null +++ b/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" +} diff --git a/code/config/experiments/metrics_h1_calibration.json b/code/config/experiments/metrics_h1_calibration.json new file mode 100644 index 0000000..8c2e769 --- /dev/null +++ b/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" +} diff --git a/code/config/experiments/metrics_h2.json b/code/config/experiments/metrics_h2.json new file mode 100644 index 0000000..e23f7f3 --- /dev/null +++ b/code/config/experiments/metrics_h2.json @@ -0,0 +1,6 @@ +{ + "enabled": [ + "h2" + ], + "h2": {} +} diff --git a/code/config/experiments/metrics_h3.json b/code/config/experiments/metrics_h3.json new file mode 100644 index 0000000..d0f7a44 --- /dev/null +++ b/code/config/experiments/metrics_h3.json @@ -0,0 +1,9 @@ +{ + "enabled": [ + "h3" + ], + "h3": { + "force_scale": 1.0, + "epsilon_energy_J": 1e-12 + } +} diff --git a/code/config/experiments/metrics_h3_h4_example.json b/code/config/experiments/metrics_h3_h4_example.json new file mode 100644 index 0000000..48f16ef --- /dev/null +++ b/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" + } + } +} diff --git a/code/config/experiments/metrics_h4_tank.json b/code/config/experiments/metrics_h4_tank.json new file mode 100644 index 0000000..49717d2 --- /dev/null +++ b/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" +} diff --git a/code/config/experiments/smoke.json b/code/config/experiments/smoke.json new file mode 100644 index 0000000..fcbe147 --- /dev/null +++ b/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 + ] + } +} diff --git a/code/config/hardware.yaml b/code/config/hardware.yaml new file mode 100644 index 0000000..152f46d --- /dev/null +++ b/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 + diff --git a/code/config/master_7dof.mjcf b/code/config/master_7dof.mjcf new file mode 100644 index 0000000..91356d5 --- /dev/null +++ b/code/config/master_7dof.mjcf @@ -0,0 +1,84 @@ + + + \ No newline at end of file diff --git a/code/config/master_7dof.urdf b/code/config/master_7dof.urdf new file mode 100644 index 0000000..4fcf650 --- /dev/null +++ b/code/config/master_7dof.urdf @@ -0,0 +1,208 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/code/config/meshes/L_ELBOW_R_S.STL b/code/config/meshes/L_ELBOW_R_S.STL new file mode 100644 index 0000000..17e4796 Binary files /dev/null and b/code/config/meshes/L_ELBOW_R_S.STL differ diff --git a/code/config/meshes/L_SHOULDER_P_S.STL b/code/config/meshes/L_SHOULDER_P_S.STL new file mode 100644 index 0000000..855e54e Binary files /dev/null and b/code/config/meshes/L_SHOULDER_P_S.STL differ diff --git a/code/config/meshes/L_SHOULDER_R_S.STL b/code/config/meshes/L_SHOULDER_R_S.STL new file mode 100644 index 0000000..1652ea7 Binary files /dev/null and b/code/config/meshes/L_SHOULDER_R_S.STL differ diff --git a/code/config/meshes/L_SHOULDER_Y_S.STL b/code/config/meshes/L_SHOULDER_Y_S.STL new file mode 100644 index 0000000..a966726 Binary files /dev/null and b/code/config/meshes/L_SHOULDER_Y_S.STL differ diff --git a/code/config/meshes/L_WRIST_P_S.STL b/code/config/meshes/L_WRIST_P_S.STL new file mode 100644 index 0000000..10e6156 Binary files /dev/null and b/code/config/meshes/L_WRIST_P_S.STL differ diff --git a/code/config/meshes/L_WRIST_R_S.STL b/code/config/meshes/L_WRIST_R_S.STL new file mode 100644 index 0000000..4255742 Binary files /dev/null and b/code/config/meshes/L_WRIST_R_S.STL differ diff --git a/code/config/meshes/L_WRIST_Y_S.STL b/code/config/meshes/L_WRIST_Y_S.STL new file mode 100644 index 0000000..ae6a7f9 Binary files /dev/null and b/code/config/meshes/L_WRIST_Y_S.STL differ diff --git a/code/config/meshes/NECK_P_S.STL b/code/config/meshes/NECK_P_S.STL new file mode 100644 index 0000000..b294c7f Binary files /dev/null and b/code/config/meshes/NECK_P_S.STL differ diff --git a/code/config/meshes/NECK_R_S.STL b/code/config/meshes/NECK_R_S.STL new file mode 100644 index 0000000..f9e097e Binary files /dev/null and b/code/config/meshes/NECK_R_S.STL differ diff --git a/code/config/meshes/NECK_Y_S.STL b/code/config/meshes/NECK_Y_S.STL new file mode 100644 index 0000000..f1e1b9b Binary files /dev/null and b/code/config/meshes/NECK_Y_S.STL differ diff --git a/code/config/meshes/PELVIS_S.STL b/code/config/meshes/PELVIS_S.STL new file mode 100644 index 0000000..133cf46 Binary files /dev/null and b/code/config/meshes/PELVIS_S.STL differ diff --git a/code/config/meshes/R_ELBOW_R_S.STL b/code/config/meshes/R_ELBOW_R_S.STL new file mode 100644 index 0000000..2fb7b53 Binary files /dev/null and b/code/config/meshes/R_ELBOW_R_S.STL differ diff --git a/code/config/meshes/R_SHOULDER_P_S.STL b/code/config/meshes/R_SHOULDER_P_S.STL new file mode 100644 index 0000000..8bfafb1 Binary files /dev/null and b/code/config/meshes/R_SHOULDER_P_S.STL differ diff --git a/code/config/meshes/R_SHOULDER_R_S.STL b/code/config/meshes/R_SHOULDER_R_S.STL new file mode 100644 index 0000000..b14ad9b Binary files /dev/null and b/code/config/meshes/R_SHOULDER_R_S.STL differ diff --git a/code/config/meshes/R_SHOULDER_Y_S.STL b/code/config/meshes/R_SHOULDER_Y_S.STL new file mode 100644 index 0000000..aea9b41 Binary files /dev/null and b/code/config/meshes/R_SHOULDER_Y_S.STL differ diff --git a/code/config/meshes/R_WRIST_P_S.STL b/code/config/meshes/R_WRIST_P_S.STL new file mode 100644 index 0000000..b48aa44 Binary files /dev/null and b/code/config/meshes/R_WRIST_P_S.STL differ diff --git a/code/config/meshes/R_WRIST_R_S.STL b/code/config/meshes/R_WRIST_R_S.STL new file mode 100644 index 0000000..70a997d Binary files /dev/null and b/code/config/meshes/R_WRIST_R_S.STL differ diff --git a/code/config/meshes/R_WRIST_Y_S.STL b/code/config/meshes/R_WRIST_Y_S.STL new file mode 100644 index 0000000..ef15308 Binary files /dev/null and b/code/config/meshes/R_WRIST_Y_S.STL differ diff --git a/code/config/meshes/base_link.STL b/code/config/meshes/base_link.STL new file mode 100644 index 0000000..f7dae5c Binary files /dev/null and b/code/config/meshes/base_link.STL differ diff --git a/code/config/meshes/index_force_sensor_1.STL b/code/config/meshes/index_force_sensor_1.STL new file mode 100644 index 0000000..b271a2a Binary files /dev/null and b/code/config/meshes/index_force_sensor_1.STL differ diff --git a/code/config/meshes/index_force_sensor_2.STL b/code/config/meshes/index_force_sensor_2.STL new file mode 100644 index 0000000..f056699 Binary files /dev/null and b/code/config/meshes/index_force_sensor_2.STL differ diff --git a/code/config/meshes/index_force_sensor_3.STL b/code/config/meshes/index_force_sensor_3.STL new file mode 100644 index 0000000..7bf3a71 Binary files /dev/null and b/code/config/meshes/index_force_sensor_3.STL differ diff --git a/code/config/meshes/little_force_sensor_1.STL b/code/config/meshes/little_force_sensor_1.STL new file mode 100644 index 0000000..c4bffaf Binary files /dev/null and b/code/config/meshes/little_force_sensor_1.STL differ diff --git a/code/config/meshes/little_force_sensor_2.STL b/code/config/meshes/little_force_sensor_2.STL new file mode 100644 index 0000000..c1d599d Binary files /dev/null and b/code/config/meshes/little_force_sensor_2.STL differ diff --git a/code/config/meshes/little_force_sensor_3.STL b/code/config/meshes/little_force_sensor_3.STL new file mode 100644 index 0000000..f5d06b4 Binary files /dev/null and b/code/config/meshes/little_force_sensor_3.STL differ diff --git a/code/config/meshes/middle_force_sensor_1.STL b/code/config/meshes/middle_force_sensor_1.STL new file mode 100644 index 0000000..d42d830 Binary files /dev/null and b/code/config/meshes/middle_force_sensor_1.STL differ diff --git a/code/config/meshes/middle_force_sensor_2.STL b/code/config/meshes/middle_force_sensor_2.STL new file mode 100644 index 0000000..2feead5 Binary files /dev/null and b/code/config/meshes/middle_force_sensor_2.STL differ diff --git a/code/config/meshes/middle_force_sensor_3.STL b/code/config/meshes/middle_force_sensor_3.STL new file mode 100644 index 0000000..bb35da1 Binary files /dev/null and b/code/config/meshes/middle_force_sensor_3.STL differ diff --git a/code/config/meshes/palm_force_sensor.STL b/code/config/meshes/palm_force_sensor.STL new file mode 100644 index 0000000..60824bf Binary files /dev/null and b/code/config/meshes/palm_force_sensor.STL differ diff --git a/code/config/meshes/right_arm.mjcf b/code/config/meshes/right_arm.mjcf new file mode 100644 index 0000000..211249b --- /dev/null +++ b/code/config/meshes/right_arm.mjcf @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/config/meshes/right_arm.urdf b/code/config/meshes/right_arm.urdf new file mode 100644 index 0000000..24b4f9b --- /dev/null +++ b/code/config/meshes/right_arm.urdf @@ -0,0 +1,288 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/config/meshes/right_index_1.STL b/code/config/meshes/right_index_1.STL new file mode 100644 index 0000000..c17ff30 Binary files /dev/null and b/code/config/meshes/right_index_1.STL differ diff --git a/code/config/meshes/right_index_2.STL b/code/config/meshes/right_index_2.STL new file mode 100644 index 0000000..3f5e462 Binary files /dev/null and b/code/config/meshes/right_index_2.STL differ diff --git a/code/config/meshes/right_little_1.STL b/code/config/meshes/right_little_1.STL new file mode 100644 index 0000000..cc79788 Binary files /dev/null and b/code/config/meshes/right_little_1.STL differ diff --git a/code/config/meshes/right_little_2.STL b/code/config/meshes/right_little_2.STL new file mode 100644 index 0000000..c42f6d7 Binary files /dev/null and b/code/config/meshes/right_little_2.STL differ diff --git a/code/config/meshes/right_middle_1.STL b/code/config/meshes/right_middle_1.STL new file mode 100644 index 0000000..a6942b7 Binary files /dev/null and b/code/config/meshes/right_middle_1.STL differ diff --git a/code/config/meshes/right_middle_2.STL b/code/config/meshes/right_middle_2.STL new file mode 100644 index 0000000..0f3865f Binary files /dev/null and b/code/config/meshes/right_middle_2.STL differ diff --git a/code/config/meshes/right_ring_1.STL b/code/config/meshes/right_ring_1.STL new file mode 100644 index 0000000..6fc79ca Binary files /dev/null and b/code/config/meshes/right_ring_1.STL differ diff --git a/code/config/meshes/right_ring_2.STL b/code/config/meshes/right_ring_2.STL new file mode 100644 index 0000000..a4cf458 Binary files /dev/null and b/code/config/meshes/right_ring_2.STL differ diff --git a/code/config/meshes/right_thumb_1.STL b/code/config/meshes/right_thumb_1.STL new file mode 100644 index 0000000..a4f441c Binary files /dev/null and b/code/config/meshes/right_thumb_1.STL differ diff --git a/code/config/meshes/right_thumb_2.STL b/code/config/meshes/right_thumb_2.STL new file mode 100644 index 0000000..0b29f9b Binary files /dev/null and b/code/config/meshes/right_thumb_2.STL differ diff --git a/code/config/meshes/right_thumb_3.STL b/code/config/meshes/right_thumb_3.STL new file mode 100644 index 0000000..cf5cab1 Binary files /dev/null and b/code/config/meshes/right_thumb_3.STL differ diff --git a/code/config/meshes/right_thumb_4.STL b/code/config/meshes/right_thumb_4.STL new file mode 100644 index 0000000..bf147e0 Binary files /dev/null and b/code/config/meshes/right_thumb_4.STL differ diff --git a/code/config/meshes/ring_force_sensor_1.STL b/code/config/meshes/ring_force_sensor_1.STL new file mode 100644 index 0000000..40d42a1 Binary files /dev/null and b/code/config/meshes/ring_force_sensor_1.STL differ diff --git a/code/config/meshes/ring_force_sensor_2.STL b/code/config/meshes/ring_force_sensor_2.STL new file mode 100644 index 0000000..0e9e95b Binary files /dev/null and b/code/config/meshes/ring_force_sensor_2.STL differ diff --git a/code/config/meshes/ring_force_sensor_3.STL b/code/config/meshes/ring_force_sensor_3.STL new file mode 100644 index 0000000..32c799b Binary files /dev/null and b/code/config/meshes/ring_force_sensor_3.STL differ diff --git a/code/config/meshes/thumb_force_sensor_1.STL b/code/config/meshes/thumb_force_sensor_1.STL new file mode 100644 index 0000000..9993ba8 Binary files /dev/null and b/code/config/meshes/thumb_force_sensor_1.STL differ diff --git a/code/config/meshes/thumb_force_sensor_2.STL b/code/config/meshes/thumb_force_sensor_2.STL new file mode 100644 index 0000000..ee1fe22 Binary files /dev/null and b/code/config/meshes/thumb_force_sensor_2.STL differ diff --git a/code/config/meshes/thumb_force_sensor_3.STL b/code/config/meshes/thumb_force_sensor_3.STL new file mode 100644 index 0000000..57f9055 Binary files /dev/null and b/code/config/meshes/thumb_force_sensor_3.STL differ diff --git a/code/config/meshes/thumb_force_sensor_4.STL b/code/config/meshes/thumb_force_sensor_4.STL new file mode 100644 index 0000000..1842e2e Binary files /dev/null and b/code/config/meshes/thumb_force_sensor_4.STL differ diff --git a/code/config/real_slave_7dof.urdf b/code/config/real_slave_7dof.urdf new file mode 100644 index 0000000..b71e5ec --- /dev/null +++ b/code/config/real_slave_7dof.urdf @@ -0,0 +1,288 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/config/slave_7dof.urdf b/code/config/slave_7dof.urdf new file mode 100644 index 0000000..0fd8bd3 --- /dev/null +++ b/code/config/slave_7dof.urdf @@ -0,0 +1,208 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/code/core/__init__.py b/code/core/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/code/core/command_allocator.py b/code/core/command_allocator.py new file mode 100644 index 0000000..806fc11 --- /dev/null +++ b/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, + ) diff --git a/code/core/energy_audit.py b/code/core/energy_audit.py new file mode 100644 index 0000000..15614a5 --- /dev/null +++ b/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)) + ), + ) diff --git a/code/core/estimation_signals.py b/code/core/estimation_signals.py new file mode 100644 index 0000000..c64986c --- /dev/null +++ b/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) diff --git a/code/core/feedback_protocol.py b/code/core/feedback_protocol.py new file mode 100644 index 0000000..178a124 --- /dev/null +++ b/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 diff --git a/code/core/haptic_render.py b/code/core/haptic_render.py new file mode 100644 index 0000000..e07c5d4 --- /dev/null +++ b/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, + ) diff --git a/code/core/interaction_estimater.py b/code/core/interaction_estimater.py new file mode 100644 index 0000000..d9d72ae --- /dev/null +++ b/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 diff --git a/code/core/model_contract.py b/code/core/model_contract.py new file mode 100644 index 0000000..3835759 --- /dev/null +++ b/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 diff --git a/code/core/network_emulator.py b/code/core/network_emulator.py new file mode 100644 index 0000000..0262ac4 --- /dev/null +++ b/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) diff --git a/code/core/retargeting_baselines.py b/code/core/retargeting_baselines.py new file mode 100644 index 0000000..36fd7ff --- /dev/null +++ b/code/core/retargeting_baselines.py @@ -0,0 +1,1123 @@ +"""Reproducible retargeting contracts and H1 baseline implementations. + +The historical retargeting scripts returned method-specific tuples and debug +dictionaries. Formal trajectory experiments need every method to expose the +same success/failure semantics and solver diagnostics. This module provides +that contract without changing :mod:`core.sew_mapper2`. + +The two Cartesian baselines intentionally share one :class:`PoseTargetBuilder` +so their comparison changes only the inverse-kinematics method. The builder +maps *changes* in the master shoulder-relative end-effector pose about frozen +reference configurations. This makes the reference pose exactly attainable +on the slave while keeping the mapping explicit and reproducible. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import Enum +from time import perf_counter +from typing import Optional, Protocol, Sequence, runtime_checkable + +import numpy as np +import pinocchio as pin + +from .model_contract import ( + MASTER_FRAMES, + MASTER_JOINT_NAMES, + SLAVE_FRAMES, + SLAVE_JOINT_NAMES, + TeleoperationModels, + finite_joint_limits, + joint_q_indices, + load_models, + require_frame, + safe_configuration, +) +from .sew_mapper2 import BallJointConfig, SEWMapper + + +def _readonly_array(value: np.ndarray, shape: tuple[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 _pose_error( + current_position: np.ndarray, + current_rotation: np.ndarray, + target: "PoseTarget", +) -> tuple[np.ndarray, np.ndarray]: + """Return position and world-axis orientation errors.""" + position_error = target.position - current_position + orientation_error = pin.log3(target.rotation @ current_rotation.T) + return position_error, orientation_error + + +class RetargetingFailure(str, Enum): + """Stable trajectory-level failure categories used by H1.""" + + NONE = "none" + INVALID_INPUT = "invalid_input" + MASTER_LIMIT_VIOLATION = "master_limit_violation" + DEGENERATE_GEOMETRY = "degenerate_geometry" + JOINT_LIMIT_VIOLATION = "joint_limit_violation" + TASK_TOLERANCE_EXCEEDED = "task_tolerance_exceeded" + SOLVER_NOT_CONVERGED = "solver_not_converged" + NUMERICAL_FAILURE = "numerical_failure" + + +class RetargetingSolverStatus(str, Enum): + """Method-independent solver termination status.""" + + CLOSED_FORM = "closed_form" + CONVERGED = "converged" + MAX_ITERATIONS = "max_iterations" + INVALID_INPUT = "invalid_input" + FAILED = "failed" + + +@dataclass(frozen=True) +class PoseTarget: + """Slave end-effector target, expressed in the slave model world frame.""" + + position: np.ndarray + rotation: np.ndarray + valid: bool = True + smooth: bool = True + events: tuple[str, ...] = () + + def __post_init__(self) -> None: + object.__setattr__( + self, "position", _readonly_array(self.position, (3,), "position") + ) + object.__setattr__( + self, "rotation", _readonly_array(self.rotation, (3, 3), "rotation") + ) + if not np.allclose(self.rotation.T @ self.rotation, np.eye(3), atol=1e-8): + raise ValueError("rotation must be orthonormal") + if np.linalg.det(self.rotation) <= 0.0: + raise ValueError("rotation must be a proper rotation") + object.__setattr__( + self, "events", tuple(str(event) for event in self.events) + ) + if self.smooth and not self.valid: + raise ValueError("an invalid target cannot be smooth") + + +@dataclass(frozen=True) +class RetargetingDiagnostics: + """Diagnostics recorded once per retargeting sample.""" + + status: RetargetingSolverStatus + iterations: int + runtime_s: float + cost: float + position_error_m: float + orientation_error_rad: float + active_limit_indices: tuple[int, ...] = () + clipped: bool = False + message: str = "" + + def __post_init__(self) -> None: + if self.iterations < 0: + raise ValueError("iterations must be non-negative") + numeric = ( + self.runtime_s, + self.cost, + self.position_error_m, + self.orientation_error_rad, + ) + if any(not np.isfinite(item) or item < 0.0 for item in numeric): + raise ValueError("diagnostic scalars must be finite and non-negative") + + +@dataclass(frozen=True) +class RetargetingResult: + """Uniform output returned by every retargeting method.""" + + method: str + q_slave: np.ndarray + success: bool + smooth: bool + failure: RetargetingFailure + diagnostics: RetargetingDiagnostics + target: Optional[PoseTarget] = None + events: tuple[str, ...] = () + + def __post_init__(self) -> None: + q_slave = np.asarray(self.q_slave, dtype=float).copy() + if q_slave.ndim != 1: + raise ValueError("q_slave must be a one-dimensional configuration") + if not np.all(np.isfinite(q_slave)): + raise ValueError("q_slave must contain only finite values") + q_slave.setflags(write=False) + object.__setattr__(self, "q_slave", q_slave) + if not self.method: + raise ValueError("method must be non-empty") + if self.success != (self.failure is RetargetingFailure.NONE): + raise ValueError("success must be equivalent to failure == NONE") + if self.smooth and not self.success: + raise ValueError("a failed result cannot be smooth") + + +@runtime_checkable +class Retargeter(Protocol): + """Structural interface consumed by a future trajectory experiment runner.""" + + name: str + + def retarget( + self, + q_master: np.ndarray, + q_slave_seed: Optional[np.ndarray] = None, + ) -> RetargetingResult: + ... + + +class PoseTargetProvider(Protocol): + """Target policy shared by Cartesian retargeting baselines.""" + + q_slave_reference: np.ndarray + + def build(self, q_master: np.ndarray) -> PoseTarget: + ... + + +class PoseTargetBuilder: + """Build one frozen, cross-embodiment Cartesian target convention.""" + + def __init__( + self, + master_model: pin.Model, + slave_model: pin.Model, + *, + master_joint_names: Sequence[str], + slave_joint_names: Sequence[str], + master_shoulder_frame: str, + master_ee_frame: str, + slave_shoulder_frame: str, + slave_ee_frame: str, + q_master_reference: Optional[np.ndarray] = None, + q_slave_reference: Optional[np.ndarray] = None, + translation_scale: Optional[float] = None, + ) -> None: + self.master_model = master_model + self.slave_model = slave_model + self.master_data = master_model.createData() + self.slave_data = slave_model.createData() + self.master_q_indices = joint_q_indices(master_model, master_joint_names) + self.slave_q_indices = joint_q_indices(slave_model, slave_joint_names) + self.master_shoulder_id = require_frame(master_model, master_shoulder_frame) + self.master_ee_id = require_frame(master_model, master_ee_frame) + self.slave_shoulder_id = require_frame(slave_model, slave_shoulder_frame) + self.slave_ee_id = require_frame(slave_model, slave_ee_frame) + + q_m_ref = ( + safe_configuration(master_model, master_joint_names) + if q_master_reference is None + else np.asarray(q_master_reference, dtype=float) + ) + q_s_ref = ( + safe_configuration(slave_model, slave_joint_names) + if q_slave_reference is None + else np.asarray(q_slave_reference, dtype=float) + ) + self.q_master_reference = _readonly_array( + q_m_ref, (master_model.nq,), "q_master_reference" + ) + self.q_slave_reference = _readonly_array( + q_s_ref, (slave_model.nq,), "q_slave_reference" + ) + + master_reference = self._master_pose(self.q_master_reference) + slave_reference = self._slave_pose(self.q_slave_reference) + self._master_relative_reference = ( + master_reference[0] - master_reference[2] + ) + self._slave_position_reference = slave_reference[0] + self._orientation_alignment = ( + slave_reference[1] @ master_reference[1].T + ) + self._position_axis_alignment = ( + slave_reference[3] @ master_reference[3].T + ) + + master_reach = float(np.linalg.norm(self._master_relative_reference)) + slave_reach = float( + np.linalg.norm(slave_reference[0] - slave_reference[2]) + ) + if master_reach <= 1e-9 or slave_reach <= 1e-9: + raise ValueError("reference shoulder-to-EE distances must be non-zero") + scale = slave_reach / master_reach if translation_scale is None else float( + translation_scale + ) + if not np.isfinite(scale) or scale <= 0.0: + raise ValueError("translation_scale must be finite and positive") + self.translation_scale = scale + + def _master_pose( + self, q: np.ndarray + ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + pin.forwardKinematics(self.master_model, self.master_data, q) + pin.updateFramePlacements(self.master_model, self.master_data) + ee = self.master_data.oMf[self.master_ee_id] + shoulder = self.master_data.oMf[self.master_shoulder_id] + return ( + ee.translation.copy(), + ee.rotation.copy(), + shoulder.translation.copy(), + shoulder.rotation.copy(), + ) + + def _slave_pose( + self, q: np.ndarray + ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + pin.forwardKinematics(self.slave_model, self.slave_data, q) + pin.updateFramePlacements(self.slave_model, self.slave_data) + ee = self.slave_data.oMf[self.slave_ee_id] + shoulder = self.slave_data.oMf[self.slave_shoulder_id] + return ( + ee.translation.copy(), + ee.rotation.copy(), + shoulder.translation.copy(), + shoulder.rotation.copy(), + ) + + def build(self, q_master: np.ndarray) -> PoseTarget: + q_master = _readonly_array( + q_master, (self.master_model.nq,), "q_master" + ) + position, rotation, shoulder_position, _ = self._master_pose(q_master) + relative = position - shoulder_position + relative_delta = relative - self._master_relative_reference + target_position = self._slave_position_reference + ( + self.translation_scale + * self._position_axis_alignment + @ relative_delta + ) + target_rotation = self._orientation_alignment @ rotation + return PoseTarget(target_position, target_rotation) + + +class SEWFeasiblePoseTargetBuilder: + """Expose the SEW mapper's feasible wrist target to comparator IK methods. + + This builder supports the attribution block in which bounded DLS and + task-priority IK receive exactly the same wrist position/orientation target + as the proposed SEW recovery. It deliberately does not expose the SEW + elbow target to either comparator. + """ + + def __init__( + self, + mapper: SEWMapper, + q_slave_reference: Optional[np.ndarray] = None, + ) -> None: + self.mapper = mapper + reference = ( + safe_configuration( + mapper.s_model, + tuple( + mapper.s_model.names[joint_id] + for joint_id in mapper.s_joint_ids + ), + ) + if q_slave_reference is None + else np.asarray(q_slave_reference, dtype=float) + ) + self.q_slave_reference = _readonly_array( + reference, (mapper.s_model.nq,), "q_slave_reference" + ) + + def build(self, q_master: np.ndarray) -> PoseTarget: + q_master = np.asarray(q_master, dtype=float) + if q_master.shape == (self.mapper.m_model.nq,): + q_master7 = q_master[self.mapper.m_qidx7] + elif q_master.shape == (7,): + q_master7 = q_master + else: + raise ValueError( + "q_master must be a full master configuration or a 7-vector" + ) + target = self.mapper._target_from_master(q_master7) + nonsmooth_events = { + "reach_clipped_lower", + "reach_clipped_upper", + "reference_axis_fallback", + "master_shoulder_wrist_degenerate", + "master_arm_plane_degenerate", + } + events = tuple(str(event) for event in target["events"]) + valid = bool(target["hard_geometry_valid"]) + smooth = bool( + valid and not any(event in nonsmooth_events for event in events) + ) + return PoseTarget( + target["pW_s_ref"], + target["RmEE"], + valid=valid, + smooth=smooth, + events=events, + ) + + +class _RetargetingBase: + """Shared validation and kinematic diagnostics.""" + + name = "retargeting_base" + + def __init__( + self, + models: TeleoperationModels, + target_builder: PoseTargetProvider, + *, + master_joint_names: Sequence[str] = MASTER_JOINT_NAMES, + slave_joint_names: Sequence[str] = SLAVE_JOINT_NAMES, + slave_ee_frame: str = SLAVE_FRAMES["ee"], + joint_limit_margin: float = 1e-4, + ) -> None: + self.master_model = models.master + self.slave_model = models.slave + self.target_builder = target_builder + self.master_q_indices = joint_q_indices( + self.master_model, master_joint_names + ) + self.slave_q_indices = joint_q_indices(self.slave_model, slave_joint_names) + self.master_lower, self.master_upper = finite_joint_limits( + self.master_model, master_joint_names + ) + self.slave_lower, self.slave_upper = finite_joint_limits( + self.slave_model, slave_joint_names + ) + self.slave_ee_id = require_frame(self.slave_model, slave_ee_frame) + self.slave_data = self.slave_model.createData() + self.joint_limit_margin = float(joint_limit_margin) + if not np.isfinite(self.joint_limit_margin) or self.joint_limit_margin < 0.0: + raise ValueError("joint_limit_margin must be finite and non-negative") + + def _master_configuration( + self, q_master: np.ndarray + ) -> tuple[Optional[np.ndarray], Optional[RetargetingFailure]]: + q_input = np.asarray(q_master, dtype=float) + if q_input.shape == (self.master_model.nq,): + q = q_input.copy() + elif q_input.shape == (len(self.master_q_indices),): + q = pin.neutral(self.master_model) + q[self.master_q_indices] = q_input + else: + return None, RetargetingFailure.INVALID_INPUT + if not np.all(np.isfinite(q)): + return None, RetargetingFailure.INVALID_INPUT + q7 = q[self.master_q_indices] + if np.any(q7 < self.master_lower) or np.any(q7 > self.master_upper): + return None, RetargetingFailure.MASTER_LIMIT_VIOLATION + return q, None + + def _slave_seed(self, seed: Optional[np.ndarray]) -> Optional[np.ndarray]: + if seed is None: + return np.asarray( + self.target_builder.q_slave_reference, dtype=float + ).copy() + seed_array = np.asarray(seed, dtype=float) + if seed_array.shape == (self.slave_model.nq,): + q = seed_array.copy() + elif seed_array.shape == (len(self.slave_q_indices),): + q = pin.neutral(self.slave_model) + q[self.slave_q_indices] = seed_array + else: + return None + if not np.all(np.isfinite(q)): + return None + q7 = q[self.slave_q_indices] + if np.any(q7 < self.slave_lower) or np.any(q7 > self.slave_upper): + return None + return q + + def _kinematics( + self, q_slave: np.ndarray, target: PoseTarget + ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + pin.forwardKinematics(self.slave_model, self.slave_data, q_slave) + pin.updateFramePlacements(self.slave_model, self.slave_data) + placement = self.slave_data.oMf[self.slave_ee_id] + position_error, orientation_error = _pose_error( + placement.translation, placement.rotation, target + ) + jacobian = pin.computeFrameJacobian( + self.slave_model, + self.slave_data, + q_slave, + self.slave_ee_id, + pin.ReferenceFrame.LOCAL_WORLD_ALIGNED, + ) + return position_error, orientation_error, jacobian[:, self.slave_q_indices] + + def _active_limits(self, q_slave: np.ndarray) -> tuple[int, ...]: + q7 = q_slave[self.slave_q_indices] + clearance = np.minimum(q7 - self.slave_lower, self.slave_upper - q7) + return tuple( + int(index) + for index in np.flatnonzero(clearance <= self.joint_limit_margin) + ) + + def _invalid_result( + self, + failure: RetargetingFailure, + started_at: float, + message: str, + ) -> RetargetingResult: + q = np.asarray(self.target_builder.q_slave_reference, dtype=float) + diagnostics = RetargetingDiagnostics( + status=RetargetingSolverStatus.INVALID_INPUT, + iterations=0, + runtime_s=max(0.0, perf_counter() - started_at), + cost=0.0, + position_error_m=0.0, + orientation_error_rad=0.0, + message=message, + ) + return RetargetingResult( + method=self.name, + q_slave=q, + success=False, + smooth=False, + failure=failure, + diagnostics=diagnostics, + events=(failure.value,), + ) + + def _invalid_target_result( + self, + target: PoseTarget, + started_at: float, + ) -> RetargetingResult: + diagnostics = RetargetingDiagnostics( + status=RetargetingSolverStatus.FAILED, + iterations=0, + runtime_s=max(0.0, perf_counter() - started_at), + cost=0.0, + position_error_m=0.0, + orientation_error_rad=0.0, + message="invalid Cartesian target", + ) + events = tuple( + dict.fromkeys( + (*target.events, RetargetingFailure.DEGENERATE_GEOMETRY.value) + ) + ) + return RetargetingResult( + method=self.name, + q_slave=self.target_builder.q_slave_reference, + success=False, + smooth=False, + failure=RetargetingFailure.DEGENERATE_GEOMETRY, + diagnostics=diagnostics, + target=target, + events=events, + ) + + +class ScaledJointSpaceRetargeter(_RetargetingBase): + """Anthropometrically scaled joint-range mapping baseline.""" + + name = "scaled_joint_space" + + def retarget( + self, + q_master: np.ndarray, + q_slave_seed: Optional[np.ndarray] = None, + ) -> RetargetingResult: + del q_slave_seed # Closed-form baseline has no branch state. + started_at = perf_counter() + q_m, failure = self._master_configuration(q_master) + if failure is not None: + return self._invalid_result(failure, started_at, failure.value) + + target = self.target_builder.build(q_m) + if not target.valid: + return self._invalid_target_result(target, started_at) + normalized = ( + q_m[self.master_q_indices] - self.master_lower + ) / (self.master_upper - self.master_lower) + q_slave = pin.neutral(self.slave_model) + q_slave[self.slave_q_indices] = self.slave_lower + normalized * ( + self.slave_upper - self.slave_lower + ) + position_error, orientation_error, _ = self._kinematics(q_slave, target) + active = self._active_limits(q_slave) + position_norm = float(np.linalg.norm(position_error)) + orientation_norm = float(np.linalg.norm(orientation_error)) + diagnostics = RetargetingDiagnostics( + status=RetargetingSolverStatus.CLOSED_FORM, + iterations=0, + runtime_s=perf_counter() - started_at, + cost=0.5 * (position_norm**2 + orientation_norm**2), + position_error_m=position_norm, + orientation_error_rad=orientation_norm, + active_limit_indices=active, + ) + return RetargetingResult( + method=self.name, + q_slave=q_slave, + success=True, + smooth=bool(target.smooth and not active), + failure=RetargetingFailure.NONE, + diagnostics=diagnostics, + target=target, + events=( + *target.events, + *(("joint_limit_active",) if active else ()), + ), + ) + + +class BoundedDLSIKRetargeter(_RetargetingBase): + """Bounded iterative damped-least-squares Cartesian IK baseline.""" + + name = "bounded_dls_ik" + + def __init__( + self, + models: TeleoperationModels, + target_builder: PoseTargetProvider, + *, + damping: float = 2e-3, + orientation_weight_m: float = 0.20, + max_iterations: int = 160, + step_limit_rad: float = 0.20, + position_tolerance_m: float = 2e-4, + orientation_tolerance_rad: float = 2e-3, + **kwargs, + ) -> None: + super().__init__(models, target_builder, **kwargs) + self.damping = float(damping) + self.orientation_weight_m = float(orientation_weight_m) + self.max_iterations = int(max_iterations) + self.step_limit_rad = float(step_limit_rad) + self.position_tolerance_m = float(position_tolerance_m) + self.orientation_tolerance_rad = float(orientation_tolerance_rad) + positive = ( + self.damping, + self.orientation_weight_m, + self.step_limit_rad, + self.position_tolerance_m, + self.orientation_tolerance_rad, + ) + if any(not np.isfinite(value) or value <= 0.0 for value in positive): + raise ValueError("DLS tuning parameters must be finite and positive") + if self.max_iterations <= 0: + raise ValueError("max_iterations must be positive") + + def _weighted_cost( + self, q_slave: np.ndarray, target: PoseTarget + ) -> tuple[float, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + ep, eo, jacobian = self._kinematics(q_slave, target) + error = np.concatenate((ep, self.orientation_weight_m * eo)) + weighted_jacobian = jacobian.copy() + weighted_jacobian[3:, :] *= self.orientation_weight_m + return 0.5 * float(error @ error), error, ep, eo, weighted_jacobian + + def retarget( + self, + q_master: np.ndarray, + q_slave_seed: Optional[np.ndarray] = None, + ) -> RetargetingResult: + started_at = perf_counter() + q_m, failure = self._master_configuration(q_master) + if failure is not None: + return self._invalid_result(failure, started_at, failure.value) + q_slave = self._slave_seed(q_slave_seed) + if q_slave is None: + return self._invalid_result( + RetargetingFailure.INVALID_INPUT, + started_at, + "invalid q_slave_seed", + ) + target = self.target_builder.build(q_m) + if not target.valid: + return self._invalid_target_result(target, started_at) + + status = RetargetingSolverStatus.MAX_ITERATIONS + iterations = 0 + numerical_failure = False + for iterations in range(1, self.max_iterations + 1): + cost, error, ep, eo, jacobian = self._weighted_cost(q_slave, target) + if ( + np.linalg.norm(ep) <= self.position_tolerance_m + and np.linalg.norm(eo) <= self.orientation_tolerance_rad + ): + status = RetargetingSolverStatus.CONVERGED + break + try: + normal = jacobian @ jacobian.T + ( + self.damping**2 + ) * np.eye(6) + step = jacobian.T @ np.linalg.solve(normal, error) + except np.linalg.LinAlgError: + numerical_failure = True + status = RetargetingSolverStatus.FAILED + break + step_norm = float(np.linalg.norm(step)) + if not np.isfinite(step_norm): + numerical_failure = True + status = RetargetingSolverStatus.FAILED + break + if step_norm > self.step_limit_rad: + step *= self.step_limit_rad / step_norm + + q7 = q_slave[self.slave_q_indices] + accepted = False + for scale in (1.0, 0.5, 0.25, 0.125, 0.0625): + candidate = q_slave.copy() + candidate[self.slave_q_indices] = np.clip( + q7 + scale * step, self.slave_lower, self.slave_upper + ) + candidate_cost = self._weighted_cost(candidate, target)[0] + if candidate_cost < cost: + q_slave = candidate + accepted = True + break + if not accepted: + # A deterministic small step lets the active set change while + # still bounding the solver near a stationary point. + candidate = q_slave.copy() + candidate[self.slave_q_indices] = np.clip( + q7 + 0.01 * step, self.slave_lower, self.slave_upper + ) + if self._weighted_cost(candidate, target)[0] < cost: + q_slave = candidate + else: + status = RetargetingSolverStatus.FAILED + break + + cost, _, ep, eo, _ = self._weighted_cost(q_slave, target) + position_norm = float(np.linalg.norm(ep)) + orientation_norm = float(np.linalg.norm(eo)) + converged = bool( + position_norm <= self.position_tolerance_m + and orientation_norm <= self.orientation_tolerance_rad + ) + if converged: + status = RetargetingSolverStatus.CONVERGED + failure = RetargetingFailure.NONE + elif numerical_failure: + failure = RetargetingFailure.NUMERICAL_FAILURE + elif iterations >= self.max_iterations: + failure = RetargetingFailure.SOLVER_NOT_CONVERGED + else: + failure = RetargetingFailure.TASK_TOLERANCE_EXCEEDED + active = self._active_limits(q_slave) + diagnostics = RetargetingDiagnostics( + status=status, + iterations=iterations, + runtime_s=perf_counter() - started_at, + cost=cost, + position_error_m=position_norm, + orientation_error_rad=orientation_norm, + active_limit_indices=active, + clipped=bool(active), + ) + events: list[str] = list(target.events) + if failure is not RetargetingFailure.NONE: + events.append(failure.value) + if active: + events.append("joint_limit_active") + return RetargetingResult( + method=self.name, + q_slave=q_slave, + success=converged, + smooth=bool(converged and target.smooth and not active), + failure=failure, + diagnostics=diagnostics, + target=target, + events=tuple(events), + ) + + +class TaskPriorityIKRetargeter(BoundedDLSIKRetargeter): + """Position-first IK with orientation, centering, and manipulability tasks.""" + + name = "task_priority_ik" + + def __init__( + self, + models: TeleoperationModels, + target_builder: PoseTargetProvider, + *, + joint_centering_gain: float = 0.05, + manipulability_gain: float = 0.005, + manipulability_fd_step_rad: float = 1e-4, + manipulability_regularization: float = 1e-8, + **kwargs, + ) -> None: + super().__init__(models, target_builder, **kwargs) + self.joint_centering_gain = float(joint_centering_gain) + self.manipulability_gain = float(manipulability_gain) + self.manipulability_fd_step_rad = float(manipulability_fd_step_rad) + self.manipulability_regularization = float( + manipulability_regularization + ) + if ( + not np.isfinite(self.joint_centering_gain) + or self.joint_centering_gain < 0.0 + ): + raise ValueError( + "joint_centering_gain must be finite and non-negative" + ) + nonnegative = ( + self.manipulability_gain, + self.manipulability_fd_step_rad, + self.manipulability_regularization, + ) + if any(not np.isfinite(value) or value < 0.0 for value in nonnegative): + raise ValueError( + "manipulability settings must be finite and non-negative" + ) + if self.manipulability_gain > 0.0 and ( + self.manipulability_fd_step_rad <= 0.0 + or self.manipulability_regularization <= 0.0 + ): + raise ValueError( + "enabled manipulability optimization needs positive FD and " + "regularization values" + ) + + def _damped_pseudoinverse(self, matrix: np.ndarray) -> np.ndarray: + rows = matrix.shape[0] + return matrix.T @ np.linalg.solve( + matrix @ matrix.T + (self.damping**2) * np.eye(rows), + np.eye(rows), + ) + + def _log_manipulability(self, q_slave: np.ndarray) -> float: + """Regularized log-volume of the six-dimensional velocity ellipsoid.""" + pin.forwardKinematics(self.slave_model, self.slave_data, q_slave) + pin.updateFramePlacements(self.slave_model, self.slave_data) + jacobian = pin.computeFrameJacobian( + self.slave_model, + self.slave_data, + q_slave, + self.slave_ee_id, + pin.ReferenceFrame.LOCAL_WORLD_ALIGNED, + )[:, self.slave_q_indices] + sign, log_determinant = np.linalg.slogdet( + jacobian @ jacobian.T + + self.manipulability_regularization * np.eye(6) + ) + return 0.5 * float(log_determinant) if sign > 0.0 else -np.inf + + def _manipulability_gradient(self, q_slave: np.ndarray) -> np.ndarray: + if self.manipulability_gain == 0.0: + return np.zeros(7, dtype=float) + gradient = np.zeros(7, dtype=float) + q7 = q_slave[self.slave_q_indices] + step = self.manipulability_fd_step_rad + for joint in range(7): + plus = q_slave.copy() + minus = q_slave.copy() + plus7 = q7.copy() + minus7 = q7.copy() + plus7[joint] = min(self.slave_upper[joint], plus7[joint] + step) + minus7[joint] = max(self.slave_lower[joint], minus7[joint] - step) + denominator = plus7[joint] - minus7[joint] + if denominator <= 0.0: + continue + plus[self.slave_q_indices] = plus7 + minus[self.slave_q_indices] = minus7 + gradient[joint] = ( + self._log_manipulability(plus) + - self._log_manipulability(minus) + ) / denominator + return gradient + + def retarget( + self, + q_master: np.ndarray, + q_slave_seed: Optional[np.ndarray] = None, + ) -> RetargetingResult: + started_at = perf_counter() + q_m, failure = self._master_configuration(q_master) + if failure is not None: + return self._invalid_result(failure, started_at, failure.value) + q_slave = self._slave_seed(q_slave_seed) + if q_slave is None: + return self._invalid_result( + RetargetingFailure.INVALID_INPUT, + started_at, + "invalid q_slave_seed", + ) + target = self.target_builder.build(q_m) + if not target.valid: + return self._invalid_target_result(target, started_at) + center = 0.5 * (self.slave_lower + self.slave_upper) + + status = RetargetingSolverStatus.MAX_ITERATIONS + numerical_failure = False + iterations = 0 + for iterations in range(1, self.max_iterations + 1): + cost, _, ep, eo, jacobian = self._weighted_cost(q_slave, target) + if ( + np.linalg.norm(ep) <= self.position_tolerance_m + and np.linalg.norm(eo) <= self.orientation_tolerance_rad + ): + status = RetargetingSolverStatus.CONVERGED + break + try: + jp = jacobian[:3, :] + jo = jacobian[3:, :] / self.orientation_weight_m + jp_inverse = self._damped_pseudoinverse(jp) + dq_position = jp_inverse @ ep + null_position = np.eye(7) - jp_inverse @ jp + + jo_null = jo @ null_position + jo_null_inverse = self._damped_pseudoinverse(jo_null) + dq_orientation = jo_null_inverse @ (eo - jo @ dq_position) + + full_jacobian = np.vstack((jp, jo)) + full_inverse = self._damped_pseudoinverse(full_jacobian) + null_full = np.eye(7) - full_inverse @ full_jacobian + q7 = q_slave[self.slave_q_indices] + dq_center = ( + self.joint_centering_gain * null_full @ (center - q7) + ) + dq_manipulability = ( + self.manipulability_gain + * null_full + @ self._manipulability_gradient(q_slave) + ) + step = ( + dq_position + + dq_orientation + + dq_center + + dq_manipulability + ) + except np.linalg.LinAlgError: + numerical_failure = True + status = RetargetingSolverStatus.FAILED + break + step_norm = float(np.linalg.norm(step)) + if not np.isfinite(step_norm): + numerical_failure = True + status = RetargetingSolverStatus.FAILED + break + if step_norm > self.step_limit_rad: + step *= self.step_limit_rad / step_norm + + q7 = q_slave[self.slave_q_indices] + accepted = False + for scale in (1.0, 0.5, 0.25, 0.125, 0.0625): + candidate = q_slave.copy() + candidate[self.slave_q_indices] = np.clip( + q7 + scale * step, self.slave_lower, self.slave_upper + ) + if self._weighted_cost(candidate, target)[0] < cost: + q_slave = candidate + accepted = True + break + if not accepted: + status = RetargetingSolverStatus.FAILED + break + + cost, _, ep, eo, _ = self._weighted_cost(q_slave, target) + position_norm = float(np.linalg.norm(ep)) + orientation_norm = float(np.linalg.norm(eo)) + converged = bool( + position_norm <= self.position_tolerance_m + and orientation_norm <= self.orientation_tolerance_rad + ) + if converged: + status = RetargetingSolverStatus.CONVERGED + failure = RetargetingFailure.NONE + elif numerical_failure: + failure = RetargetingFailure.NUMERICAL_FAILURE + elif iterations >= self.max_iterations: + failure = RetargetingFailure.SOLVER_NOT_CONVERGED + else: + failure = RetargetingFailure.TASK_TOLERANCE_EXCEEDED + active = self._active_limits(q_slave) + diagnostics = RetargetingDiagnostics( + status=status, + iterations=iterations, + runtime_s=perf_counter() - started_at, + cost=cost, + position_error_m=position_norm, + orientation_error_rad=orientation_norm, + active_limit_indices=active, + clipped=bool(active), + message="secondary_objective=log_manipulability", + ) + events: list[str] = list(target.events) + if failure is not RetargetingFailure.NONE: + events.append(failure.value) + if active: + events.append("joint_limit_active") + return RetargetingResult( + method=self.name, + q_slave=q_slave, + success=converged, + smooth=bool(converged and target.smooth and not active), + failure=failure, + diagnostics=diagnostics, + target=target, + events=tuple(events), + ) + + +class SEWRetargeterAdapter: + """Expose the existing bounded SEW mapper through :class:`Retargeter`.""" + + name = "sew" + + def __init__(self, mapper) -> None: + self.mapper = mapper + + def retarget( + self, + q_master: np.ndarray, + q_slave_seed: Optional[np.ndarray] = None, + ) -> RetargetingResult: + started_at = perf_counter() + try: + q_slave, debug = self.mapper.retarget( + q_master, q_s_init=q_slave_seed + ) + except (ValueError, FloatingPointError) as error: + diagnostics = RetargetingDiagnostics( + status=RetargetingSolverStatus.INVALID_INPUT, + iterations=0, + runtime_s=perf_counter() - started_at, + cost=0.0, + position_error_m=0.0, + orientation_error_rad=0.0, + message=str(error), + ) + return RetargetingResult( + method=self.name, + q_slave=pin.neutral(self.mapper.s_model), + success=False, + smooth=False, + failure=RetargetingFailure.INVALID_INPUT, + diagnostics=diagnostics, + events=(RetargetingFailure.INVALID_INPUT.value,), + ) + events = tuple(str(event) for event in debug.get("events", ())) + if debug.get("success", False): + failure = RetargetingFailure.NONE + elif "invalid_master_geometry" in events: + failure = RetargetingFailure.DEGENERATE_GEOMETRY + elif "joint_limit_violation" in events: + failure = RetargetingFailure.JOINT_LIMIT_VIOLATION + elif "task_tolerance_exceeded" in events: + failure = RetargetingFailure.TASK_TOLERANCE_EXCEEDED + elif "bounded_solver_not_converged" in events: + failure = RetargetingFailure.SOLVER_NOT_CONVERGED + else: + failure = RetargetingFailure.NUMERICAL_FAILURE + diagnostics = RetargetingDiagnostics( + status=( + RetargetingSolverStatus.CONVERGED + if failure is RetargetingFailure.NONE + else RetargetingSolverStatus.FAILED + ), + iterations=int(debug.get("solver_nfev", 0)), + runtime_s=perf_counter() - started_at, + cost=max(0.0, float(debug.get("solver_cost", 0.0))), + position_error_m=max(0.0, float(debug.get("position_error", 0.0))), + orientation_error_rad=max( + 0.0, float(debug.get("orientation_error", 0.0)) + ), + active_limit_indices=tuple( + int(index) + for index in debug.get("joint_limit_active_indices", ()) + ), + clipped=bool(debug.get("reach_clipped", False)), + ) + return RetargetingResult( + method=self.name, + q_slave=q_slave, + success=failure is RetargetingFailure.NONE, + smooth=bool(debug.get("smooth", False)), + failure=failure, + diagnostics=diagnostics, + events=events, + ) + + +def build_canonical_baselines( + models: Optional[TeleoperationModels] = None, +) -> dict[str, Retargeter]: + """Build the three frozen H1 baselines for the canonical URDF pair.""" + canonical_models = load_models() if models is None else models + target_builder = PoseTargetBuilder( + canonical_models.master, + canonical_models.slave, + master_joint_names=MASTER_JOINT_NAMES, + slave_joint_names=SLAVE_JOINT_NAMES, + master_shoulder_frame=MASTER_FRAMES["shoulder"], + master_ee_frame=MASTER_FRAMES["ee"], + slave_shoulder_frame=SLAVE_FRAMES["shoulder"], + slave_ee_frame=SLAVE_FRAMES["ee"], + ) + baselines: tuple[Retargeter, ...] = ( + ScaledJointSpaceRetargeter(canonical_models, target_builder), + BoundedDLSIKRetargeter(canonical_models, target_builder), + TaskPriorityIKRetargeter(canonical_models, target_builder), + ) + return {baseline.name: baseline for baseline in baselines} + + +def build_canonical_sew_target_baselines( + models: Optional[TeleoperationModels] = None, +) -> tuple[dict[str, Retargeter], SEWRetargeterAdapter]: + """Build the H1 common-target attribution baselines and proposed adapter.""" + canonical_models = load_models() if models is None else models + mapper = SEWMapper( + master_model=canonical_models.master, + slave_model=canonical_models.slave, + m_shoulder=MASTER_FRAMES["shoulder"], + m_elbow=MASTER_FRAMES["elbow"], + m_wrist=MASTER_FRAMES["wrist"], + m_ee=MASTER_FRAMES["ee"], + s_shoulder=SLAVE_FRAMES["shoulder"], + s_elbow=SLAVE_FRAMES["elbow"], + s_wrist=SLAVE_FRAMES["wrist"], + s_ee=SLAVE_FRAMES["wrist"], + master_joint_names=MASTER_JOINT_NAMES, + slave_joint_names=SLAVE_JOINT_NAMES, + slave_shoulder_cfg=BallJointConfig( + axis_order="yxy", + joint_names=SLAVE_JOINT_NAMES[:3], + signs=(-1.0, 1.0, -1.0), + ), + slave_wrist_cfg=BallJointConfig( + axis_order="yzx", + joint_names=SLAVE_JOINT_NAMES[4:], + signs=(-1.0, 1.0, 1.0), + ), + ) + target_builder = SEWFeasiblePoseTargetBuilder(mapper) + baselines: tuple[Retargeter, ...] = ( + ScaledJointSpaceRetargeter( + canonical_models, + target_builder, + slave_ee_frame=SLAVE_FRAMES["wrist"], + ), + BoundedDLSIKRetargeter( + canonical_models, + target_builder, + slave_ee_frame=SLAVE_FRAMES["wrist"], + ), + TaskPriorityIKRetargeter( + canonical_models, + target_builder, + slave_ee_frame=SLAVE_FRAMES["wrist"], + ), + ) + return ( + {baseline.name: baseline for baseline in baselines}, + SEWRetargeterAdapter(mapper), + ) diff --git a/code/core/sew_mapper.py b/code/core/sew_mapper.py new file mode 100644 index 0000000..1e9f004 --- /dev/null +++ b/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 diff --git a/code/core/sew_mapper2.py b/code/core/sew_mapper2.py new file mode 100644 index 0000000..bcb80ea --- /dev/null +++ b/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 diff --git a/code/core/time_domain_popc.py b/code/core/time_domain_popc.py new file mode 100644 index 0000000..b2cde2a --- /dev/null +++ b/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 diff --git a/code/core/wrench_solver.py b/code/core/wrench_solver.py new file mode 100644 index 0000000..6ffd5d7 --- /dev/null +++ b/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, + ) diff --git a/code/demo.py b/code/demo.py new file mode 100644 index 0000000..9d7dcd9 --- /dev/null +++ b/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() diff --git a/code/diagrams/closed-loop_teleoperation_architecture.png b/code/diagrams/closed-loop_teleoperation_architecture.png new file mode 100644 index 0000000..cac7044 Binary files /dev/null and b/code/diagrams/closed-loop_teleoperation_architecture.png differ diff --git a/code/diagrams/closed_loop_control.py b/code/diagrams/closed_loop_control.py new file mode 100644 index 0000000..47e4579 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/code/experiments/README.md b/code/experiments/README.md new file mode 100644 index 0000000..7d0a3f3 --- /dev/null +++ b/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. diff --git a/code/experiments/__init__.py b/code/experiments/__init__.py new file mode 100644 index 0000000..a605f12 --- /dev/null +++ b/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", +] diff --git a/code/experiments/cli.py b/code/experiments/cli.py new file mode 100644 index 0000000..ef2015a --- /dev/null +++ b/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()) diff --git a/code/experiments/executors.py b/code/experiments/executors.py new file mode 100644 index 0000000..8a90e1a --- /dev/null +++ b/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, + }, + ) diff --git a/code/experiments/hashing.py b/code/experiments/hashing.py new file mode 100644 index 0000000..16f019c --- /dev/null +++ b/code/experiments/hashing.py @@ -0,0 +1,62 @@ +"""Canonical serialization and stable SHA-256 helpers.""" + +from __future__ import annotations + +import dataclasses +import hashlib +import json +from pathlib import Path +from typing import Any, Mapping + +import numpy as np + + +def to_jsonable(value: Any) -> Any: + """Convert supported scientific-Python values to strict JSON values.""" + if dataclasses.is_dataclass(value): + return to_jsonable(dataclasses.asdict(value)) + if isinstance(value, Path): + return str(value) + if isinstance(value, np.ndarray): + return to_jsonable(value.tolist()) + if isinstance(value, np.generic): + return to_jsonable(value.item()) + if isinstance(value, Mapping): + return {str(key): to_jsonable(item) for key, item in value.items()} + if isinstance(value, (tuple, list)): + return [to_jsonable(item) for item in value] + if isinstance(value, (str, int, float, bool)) or value is None: + return value + raise TypeError(f"Unsupported value for canonical JSON: {type(value).__name__}") + + +def canonical_json_bytes(value: Any) -> bytes: + """Return deterministic UTF-8 JSON bytes, rejecting NaN and infinity.""" + return json.dumps( + to_jsonable(value), + sort_keys=True, + separators=(",", ":"), + ensure_ascii=False, + allow_nan=False, + ).encode("utf-8") + + +def stable_hash(value: Any, *, prefix: str = "") -> str: + """Hash a value using canonical JSON and an optional domain prefix.""" + digest = hashlib.sha256() + if prefix: + digest.update(prefix.encode("utf-8")) + digest.update(b"\0") + digest.update(canonical_json_bytes(value)) + return digest.hexdigest() + + +def file_sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + while True: + block = stream.read(1024 * 1024) + if not block: + break + digest.update(block) + return digest.hexdigest() diff --git a/code/experiments/io.py b/code/experiments/io.py new file mode 100644 index 0000000..c35d515 --- /dev/null +++ b/code/experiments/io.py @@ -0,0 +1,209 @@ +"""Atomic evidence writers using only the standard library and NumPy.""" + +from __future__ import annotations + +import json +import os +import shutil +import tempfile +import uuid +from dataclasses import dataclass, field +from datetime import datetime, timezone +from pathlib import Path +from typing import Any, Mapping, Sequence + +import numpy as np + +from .hashing import file_sha256, to_jsonable +from .schema import SCHEMA_VERSION, STORAGE_FORMATS, ContractError + + +def utc_now() -> str: + return datetime.now(timezone.utc).isoformat().replace("+00:00", "Z") + + +def _flush_and_sync(stream) -> None: + stream.flush() + os.fsync(stream.fileno()) + + +def atomic_write_json(path: Path, document: Mapping[str, Any]) -> None: + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + 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") as stream: + json.dump( + to_jsonable(document), + stream, + indent=2, + sort_keys=True, + ensure_ascii=False, + allow_nan=False, + ) + stream.write("\n") + _flush_and_sync(stream) + os.replace(temporary, path) + except Exception: + temporary.unlink(missing_ok=True) + raise + + +def atomic_write_jsonl(path: Path, rows: Sequence[Mapping[str, Any]]) -> None: + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + 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") as stream: + for row in rows: + stream.write( + json.dumps( + to_jsonable(row), + sort_keys=True, + separators=(",", ":"), + ensure_ascii=False, + allow_nan=False, + ) + ) + stream.write("\n") + _flush_and_sync(stream) + os.replace(temporary, path) + except Exception: + temporary.unlink(missing_ok=True) + raise + + +def atomic_write_npz(path: Path, arrays: Mapping[str, np.ndarray]) -> None: + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + temporary = path.parent / f".{path.stem}.{uuid.uuid4().hex}.tmp.npz" + try: + np.savez_compressed(temporary, **arrays) + with temporary.open("rb") as stream: + os.fsync(stream.fileno()) + os.replace(temporary, path) + except Exception: + temporary.unlink(missing_ok=True) + raise + + +def normalize_sample_arrays( + samples: Mapping[str, Any], +) -> tuple[dict[str, np.ndarray], int]: + if not isinstance(samples, Mapping) or not samples: + raise ContractError("trial samples must be a non-empty mapping") + arrays: dict[str, np.ndarray] = {} + sample_count: int | None = None + for name, value in samples.items(): + if not isinstance(name, str) or not name: + raise ContractError("sample field names must be non-empty strings") + array = np.asarray(value) + if array.dtype == object: + raise ContractError(f"sample field {name!r} cannot have object dtype") + if array.ndim == 0: + raise ContractError(f"sample field {name!r} must have a sample axis") + if sample_count is None: + sample_count = int(array.shape[0]) + elif array.shape[0] != sample_count: + raise ContractError( + f"sample field {name!r} has {array.shape[0]} rows, " + f"expected {sample_count}" + ) + arrays[name] = array + assert sample_count is not None + if sample_count <= 0: + raise ContractError("trial samples must contain at least one row") + return arrays, sample_count + + +@dataclass(frozen=True) +class TrialPayload: + samples: Mapping[str, Any] + events: Sequence[Mapping[str, Any]] = field(default_factory=tuple) + metadata: Mapping[str, Any] = field(default_factory=dict) + + +class AtomicTrialWriter: + """Commit one successful trial directory using a final atomic rename.""" + + def __init__(self, batch_dir: Path): + self.batch_dir = Path(batch_dir) + self.raw_dir = self.batch_dir / "raw" + self.raw_dir.mkdir(parents=True, exist_ok=True) + + def trial_dir(self, trial_id: str) -> Path: + return self.raw_dir / trial_id + + def write(self, trial: Mapping[str, Any], payload: TrialPayload) -> Path: + trial_id = str(trial["trial_id"]) + destination = self.trial_dir(trial_id) + if destination.exists(): + raise FileExistsError(f"trial artifact already exists: {destination}") + arrays, sample_count = normalize_sample_arrays(payload.samples) + temporary = self.raw_dir / f".{trial_id}.tmp.{uuid.uuid4().hex}" + temporary.mkdir(parents=False, exist_ok=False) + try: + sample_path = temporary / "samples.npz" + event_path = temporary / "events.jsonl" + atomic_write_npz(sample_path, arrays) + atomic_write_jsonl(event_path, list(payload.events)) + fields = { + name: {"dtype": str(array.dtype), "shape": list(array.shape)} + for name, array in sorted(arrays.items()) + } + manifest = { + "kind": "trial_manifest", + "schema_version": SCHEMA_VERSION, + "trial_id": trial_id, + "pair_id": trial["pair_id"], + "trial_spec_hash": trial["trial_spec_hash"], + "status": "completed", + "completed_utc": utc_now(), + "sample_count": sample_count, + "sample_fields": fields, + "storage_formats": { + "samples": STORAGE_FORMATS["samples"], + "events": STORAGE_FORMATS["events"], + }, + "files": { + "samples.npz": file_sha256(sample_path), + "events.jsonl": file_sha256(event_path), + }, + "metadata": to_jsonable(payload.metadata), + } + atomic_write_json(temporary / "trial_manifest.json", manifest) + os.replace(temporary, destination) + except Exception: + shutil.rmtree(temporary, ignore_errors=True) + raise + return destination + + +def record_trial_failure( + batch_dir: Path, + trial: Mapping[str, Any], + error: BaseException, +) -> Path: + failure_dir = Path(batch_dir) / "failures" / str(trial["trial_id"]) + failure_dir.mkdir(parents=True, exist_ok=True) + path = failure_dir / f"attempt-{utc_now().replace(':', '')}-{uuid.uuid4().hex}.json" + atomic_write_json( + path, + { + "kind": "trial_failure", + "schema_version": SCHEMA_VERSION, + "trial_id": trial["trial_id"], + "pair_id": trial["pair_id"], + "trial_spec_hash": trial["trial_spec_hash"], + "recorded_utc": utc_now(), + "error_type": type(error).__name__, + "error_message": str(error), + }, + ) + return path diff --git a/code/experiments/manifest.py b/code/experiments/manifest.py new file mode 100644 index 0000000..b044f7b --- /dev/null +++ b/code/experiments/manifest.py @@ -0,0 +1,227 @@ +"""Batch manifest creation and provenance capture.""" + +from __future__ import annotations + +import importlib +import importlib.metadata +import platform +import subprocess +import sys +from pathlib import Path +from typing import Any, Mapping + +import numpy as np + +from .hashing import file_sha256, stable_hash +from .io import atomic_write_json, utc_now +from .plan import validate_trial_plan +from .schema import SCHEMA_VERSION, STORAGE_FORMATS, ContractError, require_schema + + +_SOURCE_FILES = ( + Path("pyproject.toml"), + Path("uv.lock"), + Path("code/config/master_7dof.urdf"), + Path("code/config/real_slave_7dof.urdf"), +) + + +def _git_provenance(cwd: Path) -> dict[str, Any]: + try: + commit = subprocess.run( + ["git", "rev-parse", "HEAD"], + cwd=cwd, + check=True, + capture_output=True, + text=True, + timeout=5, + ).stdout.strip() + status = subprocess.run( + ["git", "status", "--porcelain"], + cwd=cwd, + check=True, + capture_output=True, + text=True, + timeout=5, + ).stdout + return {"available": True, "commit": commit, "dirty": bool(status.strip())} + except (OSError, subprocess.SubprocessError): + return {"available": False, "commit": None, "dirty": None} + + +def _source_file_hashes(source_root: Path) -> dict[str, dict[str, str]]: + """Hash reproducibility-critical files without requiring every file.""" + hashes: dict[str, dict[str, str]] = {} + for relative_path in _SOURCE_FILES: + path = source_root / relative_path + if path.is_file(): + hashes[relative_path.as_posix()] = {"sha256": file_sha256(path)} + return hashes + + +def _optional_module_version(module_name: str) -> str | None: + """Return an importable module's version, including distribution fallback.""" + try: + module = importlib.import_module(module_name) + except Exception: + return None + + version = getattr(module, "__version__", None) + if version is not None: + return str(version) + + try: + distributions = importlib.metadata.packages_distributions().get( + module_name, () + ) + for distribution in distributions: + try: + return importlib.metadata.version(distribution) + except importlib.metadata.PackageNotFoundError: + continue + except (ImportError, OSError): + pass + return "unknown" + + +def _runtime_provenance() -> dict[str, str]: + runtime = { + "python": sys.version.split()[0], + "numpy": np.__version__, + "platform": platform.platform(), + "processor": platform.processor(), + } + for module_name in ("scipy", "pinocchio"): + version = _optional_module_version(module_name) + if version is not None: + runtime[module_name] = version + return runtime + + +def _require_locked_git( + plan: Mapping[str, Any], + provenance: Mapping[str, Any], + source_root: Path, +) -> None: + """Reject evidence-locked runs that cannot identify immutable source.""" + if plan.get("split") != "locked": + return + if not provenance.get("available"): + raise ContractError( + "locked split requires available Git metadata; " + f"source_root={source_root} is not a usable Git worktree" + ) + if provenance.get("dirty"): + raise ContractError( + "locked split requires a clean Git worktree; " + "commit or stash all source changes before running" + ) + + +def _require_locked_resume_match( + stored_manifest: Mapping[str, Any], + current_provenance: Mapping[str, Any], + source_root: Path, +) -> None: + """Require a resumed locked batch to use its original immutable source.""" + stored_source = stored_manifest.get("source") + if not isinstance(stored_source, Mapping): + raise ContractError("locked batch manifest is missing source provenance") + + stored_commit = stored_source.get("commit") + current_commit = current_provenance.get("commit") + if not stored_commit or current_commit != stored_commit: + raise ContractError( + "locked batch Git commit mismatch; " + f"stored={stored_commit!r}, current={current_commit!r}" + ) + + stored_files = stored_source.get("files") + if not isinstance(stored_files, Mapping): + raise ContractError( + "locked batch manifest is missing reproducibility file hashes" + ) + current_files = _source_file_hashes(source_root) + if dict(stored_files) != current_files: + changed_paths = sorted( + relative_path + for relative_path in set(stored_files) | set(current_files) + if stored_files.get(relative_path) != current_files.get(relative_path) + ) + raise ContractError( + "locked batch source file hash mismatch for: " + + ", ".join(changed_paths) + ) + + +def build_batch_manifest( + plan: Mapping[str, Any], + *, + source_root: Path | None = None, +) -> dict[str, Any]: + validate_trial_plan(plan) + source_root = Path.cwd() if source_root is None else Path(source_root) + source = _git_provenance(source_root) + _require_locked_git(plan, source, source_root) + source["files"] = _source_file_hashes(source_root) + batch_id = f"batch-{str(plan['plan_hash'])[:16]}" + manifest = { + "kind": "batch_manifest", + "schema_version": SCHEMA_VERSION, + "batch_id": batch_id, + "study_id": plan["study_id"], + "split": plan["split"], + "plan_hash": plan["plan_hash"], + "created_utc": utc_now(), + "expected_pair_count": plan["pair_count"], + "expected_trial_count": plan["trial_count"], + "storage_formats": dict(STORAGE_FORMATS), + "runtime": _runtime_provenance(), + "source": source, + } + hash_basis = dict(manifest) + hash_basis.pop("created_utc") + manifest["manifest_hash"] = stable_hash(hash_basis, prefix="batch-manifest") + return manifest + + +def ensure_batch( + batch_dir: Path, + plan: Mapping[str, Any], + *, + source_root: Path | None = None, +) -> dict[str, Any]: + """Create or verify the immutable plan and batch manifest.""" + validate_trial_plan(plan) + batch_dir = Path(batch_dir) + source_root = Path.cwd() if source_root is None else Path(source_root) + manifest_path = batch_dir / "batch_manifest.json" + plan_path = batch_dir / "plan.json" + if manifest_path.exists() or plan_path.exists(): + if not manifest_path.exists() or not plan_path.exists(): + raise ContractError("batch has only one of plan.json/batch_manifest.json") + current_source = _git_provenance(source_root) + _require_locked_git(plan, current_source, source_root) + from .plan import load_document + + stored_plan = load_document(plan_path) + stored_manifest = load_document(manifest_path) + validate_trial_plan(stored_plan) + require_schema(stored_manifest, "batch_manifest") + if plan.get("split") == "locked": + _require_locked_resume_match( + stored_manifest, + current_source, + source_root, + ) + if stored_plan["plan_hash"] != plan["plan_hash"]: + raise ContractError("existing batch was created from a different plan") + if stored_manifest.get("plan_hash") != plan["plan_hash"]: + raise ContractError("batch manifest plan_hash mismatch") + return stored_manifest + + batch_dir.mkdir(parents=True, exist_ok=True) + manifest = build_batch_manifest(plan, source_root=source_root) + atomic_write_json(plan_path, plan) + atomic_write_json(manifest_path, manifest) + return manifest diff --git a/code/experiments/plan.py b/code/experiments/plan.py new file mode 100644 index 0000000..44a5eb3 --- /dev/null +++ b/code/experiments/plan.py @@ -0,0 +1,212 @@ +"""Deterministic paired-trial plan expansion.""" + +from __future__ import annotations + +import itertools +import json +from pathlib import Path +from typing import Any, Mapping + +from .hashing import stable_hash, to_jsonable +from .rng import named_seed_record +from .schema import ( + SCHEMA_VERSION, + STORAGE_FORMATS, + ContractError, + require_nonempty_string, + require_schema, + require_sequence, +) + + +def load_document(path: Path) -> dict[str, Any]: + """Load JSON, with optional YAML support when PyYAML is available.""" + path = Path(path) + suffix = path.suffix.lower() + with path.open("r", encoding="utf-8") as stream: + if suffix == ".json": + document = json.load(stream) + elif suffix in {".yaml", ".yml"}: + try: + import yaml + except ImportError as exc: + raise RuntimeError( + "YAML input requires PyYAML; use JSON in minimal environments" + ) from exc + document = yaml.safe_load(stream) + else: + raise ValueError("experiment documents must use .json, .yaml, or .yml") + if not isinstance(document, dict): + raise ContractError(f"{path} must contain a mapping") + return document + + +def _normalize_methods(methods: Any) -> list[dict[str, Any]]: + normalized: list[dict[str, Any]] = [] + for entry in require_sequence(methods, "methods"): + if isinstance(entry, str): + method = {"method_id": require_nonempty_string(entry, "method")} + elif isinstance(entry, Mapping): + method = dict(to_jsonable(entry)) + source_id = method.pop("id", method.get("method_id")) + method["method_id"] = require_nonempty_string( + source_id, "method.method_id" + ) + else: + raise ContractError("each method must be a string or mapping") + normalized.append(method) + identifiers = [method["method_id"] for method in normalized] + if len(set(identifiers)) != len(identifiers): + raise ContractError("method identifiers must be unique") + return normalized + + +def _normalize_trajectories(trajectories: Any) -> list[dict[str, Any]]: + normalized: list[dict[str, Any]] = [] + for entry in require_sequence(trajectories, "trajectories"): + if isinstance(entry, str): + trajectory = { + "trajectory_id": require_nonempty_string(entry, "trajectory") + } + elif isinstance(entry, Mapping): + trajectory = dict(to_jsonable(entry)) + source_id = trajectory.pop("id", trajectory.get("trajectory_id")) + trajectory["trajectory_id"] = require_nonempty_string( + source_id, "trajectory.trajectory_id" + ) + else: + raise ContractError("each trajectory must be a string or mapping") + normalized.append(trajectory) + identifiers = [trajectory["trajectory_id"] for trajectory in normalized] + if len(set(identifiers)) != len(identifiers): + raise ContractError("trajectory identifiers must be unique") + return normalized + + +def _factor_cells(factors: Any) -> list[dict[str, Any]]: + if factors is None: + return [{}] + if not isinstance(factors, Mapping): + raise ContractError("factors must be a mapping from name to levels") + names = sorted(factors) + levels = [list(require_sequence(factors[name], f"factors.{name}")) for name in names] + return [ + dict(zip(names, to_jsonable(combination))) + for combination in itertools.product(*levels) + ] + + +def build_trial_plan(specification: Mapping[str, Any]) -> dict[str, Any]: + """Expand a study specification into immutable paired trial records.""" + if not isinstance(specification, Mapping): + raise ContractError("study specification must be a mapping") + spec = dict(to_jsonable(specification)) + study_id = require_nonempty_string(spec.get("study_id"), "study_id") + split = require_nonempty_string(spec.get("split"), "split") + if split not in {"calibration", "pilot", "locked"}: + raise ContractError("split must be calibration, pilot, or locked") + root_seed = int(spec.get("root_seed", 0)) + if root_seed < 0: + raise ContractError("root_seed must be non-negative") + replicates = int(spec.get("replicates", 1)) + if replicates <= 0: + raise ContractError("replicates must be positive") + + methods = _normalize_methods(spec.get("methods")) + trajectories = _normalize_trajectories(spec.get("trajectories")) + factor_cells = _factor_cells(spec.get("factors", {})) + + trials: list[dict[str, Any]] = [] + pair_ids: set[str] = set() + for trajectory, factors, replicate in itertools.product( + trajectories, factor_cells, range(replicates) + ): + pair_basis = { + "study_id": study_id, + "split": split, + "trajectory": trajectory, + "factors": factors, + "replicate": replicate, + "root_seed": root_seed, + } + pair_id = f"pair-{stable_hash(pair_basis, prefix='pair')[:16]}" + seeds = named_seed_record(root_seed, pair_basis) + pair_ids.add(pair_id) + for method in methods: + trial_basis = { + "pair_id": pair_id, + "method": method, + "seeds": seeds, + } + trial_id = f"trial-{stable_hash(trial_basis, prefix='trial')[:16]}" + trial = { + "trial_index": len(trials), + "trial_id": trial_id, + "pair_id": pair_id, + "study_id": study_id, + "split": split, + "method": method, + "trajectory": trajectory, + "factors": factors, + "replicate": replicate, + "seeds": seeds, + } + trial["trial_spec_hash"] = stable_hash(trial, prefix="trial-spec") + trials.append(trial) + + spec_hash = stable_hash(spec, prefix="study-spec") + plan = { + "kind": "trial_plan", + "schema_version": SCHEMA_VERSION, + "study_id": study_id, + "split": split, + "specification": spec, + "spec_hash": spec_hash, + "storage_formats": dict(STORAGE_FORMATS), + "pair_count": len(pair_ids), + "trial_count": len(trials), + "trials": trials, + } + plan["plan_hash"] = stable_hash(plan, prefix="trial-plan") + validate_trial_plan(plan) + return plan + + +def validate_trial_plan(plan: Mapping[str, Any]) -> None: + require_schema(plan, "trial_plan") + trials = require_sequence(plan.get("trials"), "trials") + if int(plan.get("trial_count", -1)) != len(trials): + raise ContractError("trial_count does not match trials") + trial_ids: set[str] = set() + pairs: dict[str, dict[str, Any]] = {} + for trial in trials: + if not isinstance(trial, Mapping): + raise ContractError("each trial must be a mapping") + trial_id = require_nonempty_string(trial.get("trial_id"), "trial_id") + pair_id = require_nonempty_string(trial.get("pair_id"), "pair_id") + if trial_id in trial_ids: + raise ContractError(f"duplicate trial_id {trial_id}") + trial_ids.add(trial_id) + trial_without_hash = dict(trial) + recorded_trial_hash = trial_without_hash.pop("trial_spec_hash", None) + expected_trial_hash = stable_hash(trial_without_hash, prefix="trial-spec") + if recorded_trial_hash != expected_trial_hash: + raise ContractError(f"trial_spec_hash mismatch for {trial_id}") + pair_signature = { + "trajectory": trial.get("trajectory"), + "factors": trial.get("factors"), + "replicate": trial.get("replicate"), + "seeds": trial.get("seeds"), + "study_id": trial.get("study_id"), + "split": trial.get("split"), + } + previous = pairs.setdefault(pair_id, pair_signature) + if previous != pair_signature: + raise ContractError(f"paired inputs differ inside {pair_id}") + if int(plan.get("pair_count", -1)) != len(pairs): + raise ContractError("pair_count does not match unique pair identifiers") + plan_without_hash = dict(plan) + recorded_hash = plan_without_hash.pop("plan_hash", None) + expected_hash = stable_hash(plan_without_hash, prefix="trial-plan") + if recorded_hash != expected_hash: + raise ContractError("plan_hash mismatch") diff --git a/code/experiments/rng.py b/code/experiments/rng.py new file mode 100644 index 0000000..1305a2b --- /dev/null +++ b/code/experiments/rng.py @@ -0,0 +1,53 @@ +"""Order-independent named NumPy ``SeedSequence`` substreams.""" + +from __future__ import annotations + +import hashlib +from typing import Iterable, Mapping + +import numpy as np + +from .hashing import canonical_json_bytes + + +DEFAULT_STREAMS = ("trajectory", "sensor", "model", "network") + + +def _label_words(*labels: object) -> list[int]: + digest = hashlib.sha256(canonical_json_bytes(labels)).digest() + return [ + int.from_bytes(digest[offset : offset + 4], "little") + for offset in range(0, 16, 4) + ] + + +def named_seed_record( + root_seed: int, + namespace: object, + stream_names: Iterable[str] = DEFAULT_STREAMS, +) -> dict[str, list[int]]: + """Create stable named seed states independent of request order.""" + root_seed = int(root_seed) + if root_seed < 0: + raise ValueError("root_seed must be non-negative") + record: dict[str, list[int]] = {} + for name in sorted(set(stream_names)): + if not name: + raise ValueError("stream names must be non-empty") + entropy = [root_seed & 0xFFFFFFFF, (root_seed >> 32) & 0xFFFFFFFF] + entropy.extend(_label_words(namespace, name)) + state = np.random.SeedSequence(entropy).generate_state(4, dtype=np.uint32) + record[name] = [int(word) for word in state] + return record + + +def generator_from_record( + record: Mapping[str, list[int]], + stream_name: str, +) -> np.random.Generator: + if stream_name not in record: + raise KeyError(f"Unknown random stream {stream_name!r}") + words = [int(word) for word in record[stream_name]] + if len(words) != 4 or any(word < 0 or word > 0xFFFFFFFF for word in words): + raise ValueError(f"Invalid seed state for stream {stream_name!r}") + return np.random.default_rng(np.random.SeedSequence(words)) diff --git a/code/experiments/runner.py b/code/experiments/runner.py new file mode 100644 index 0000000..1d40f2a --- /dev/null +++ b/code/experiments/runner.py @@ -0,0 +1,113 @@ +"""Sequential, resumable execution of an immutable paired trial plan.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any, Callable, Mapping + +from .io import ( + AtomicTrialWriter, + TrialPayload, + atomic_write_json, + record_trial_failure, + utc_now, +) +from .manifest import ensure_batch +from .plan import validate_trial_plan +from .validate import validate_trial_directory + + +TrialExecutor = Callable[[Mapping[str, Any]], TrialPayload | Mapping[str, Any]] + + +def _normalize_payload(value: TrialPayload | Mapping[str, Any]) -> TrialPayload: + if isinstance(value, TrialPayload): + return value + if not isinstance(value, Mapping) or "samples" not in value: + raise TypeError("executor must return TrialPayload or a mapping with samples") + return TrialPayload( + samples=value["samples"], + events=value.get("events", ()), + metadata=value.get("metadata", {}), + ) + + +def run_trial_plan( + plan: Mapping[str, Any], + batch_dir: Path, + executor: TrialExecutor | None = None, + *, + resume: bool = True, + dry_run: bool = False, + continue_on_error: bool = True, + max_trials: int | None = None, + source_root: Path | None = None, +) -> dict[str, Any]: + """Run trials in plan order and atomically commit successful artifacts.""" + validate_trial_plan(plan) + selected = list(plan["trials"]) + if max_trials is not None: + if max_trials < 0: + raise ValueError("max_trials must be non-negative") + selected = selected[:max_trials] + if dry_run: + return { + "dry_run": True, + "study_id": plan["study_id"], + "split": plan["split"], + "pair_count": len({trial["pair_id"] for trial in selected}), + "trial_count": len(selected), + "plan_hash": plan["plan_hash"], + } + if executor is None: + raise ValueError("executor is required unless dry_run=True") + + batch_dir = Path(batch_dir) + ensure_batch(batch_dir, plan, source_root=source_root) + writer = AtomicTrialWriter(batch_dir) + completed = 0 + skipped = 0 + failed = 0 + failures: list[dict[str, str]] = [] + for trial in selected: + destination = writer.trial_dir(trial["trial_id"]) + if destination.exists(): + validation_errors = validate_trial_directory(destination, trial) + if resume and not validation_errors: + skipped += 1 + continue + if validation_errors: + raise RuntimeError( + f"existing trial {trial['trial_id']} is invalid: " + + "; ".join(validation_errors) + ) + raise FileExistsError(f"trial already completed: {trial['trial_id']}") + try: + payload = _normalize_payload(executor(trial)) + writer.write(trial, payload) + completed += 1 + except Exception as exc: + failed += 1 + record_trial_failure(batch_dir, trial, exc) + failures.append( + { + "trial_id": trial["trial_id"], + "error_type": type(exc).__name__, + "error_message": str(exc), + } + ) + if not continue_on_error: + raise + summary = { + "kind": "run_summary", + "schema_version": plan["schema_version"], + "plan_hash": plan["plan_hash"], + "recorded_utc": utc_now(), + "selected_trials": len(selected), + "completed_trials": completed, + "skipped_trials": skipped, + "failed_trials": failed, + "failures": failures, + } + atomic_write_json(batch_dir / "run_summary.json", summary) + return summary diff --git a/code/experiments/schema.py b/code/experiments/schema.py new file mode 100644 index 0000000..eb19066 --- /dev/null +++ b/code/experiments/schema.py @@ -0,0 +1,58 @@ +"""Versioned contracts shared by plans, trial artifacts, and analysis.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from typing import Any + + +SCHEMA_VERSION = "1.0.0" +SCHEMA_MAJOR = 1 + +STORAGE_FORMATS = { + "plan": "json", + "batch_manifest": "json", + "trial_manifest": "json", + "samples": "numpy-npz", + "events": "json-lines", + "trial_metrics": "json-lines", + "paper_source_data": "csv", +} + + +class ContractError(ValueError): + """Raised when an evidence artifact violates its declared contract.""" + + +def require_schema(document: Mapping[str, Any], expected_kind: str) -> None: + if not isinstance(document, Mapping): + raise ContractError("document must be a mapping") + if document.get("kind") != expected_kind: + raise ContractError( + f"expected kind {expected_kind!r}, got {document.get('kind')!r}" + ) + version = document.get("schema_version") + if not isinstance(version, str): + raise ContractError("schema_version must be a string") + try: + major = int(version.split(".", 1)[0]) + except (TypeError, ValueError) as exc: + raise ContractError(f"invalid schema_version {version!r}") from exc + if major != SCHEMA_MAJOR: + raise ContractError( + f"unsupported schema major {major}; expected {SCHEMA_MAJOR}" + ) + + +def require_nonempty_string(value: Any, name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ContractError(f"{name} must be a non-empty string") + return value + + +def require_sequence(value: Any, name: str) -> Sequence[Any]: + if isinstance(value, (str, bytes)) or not isinstance(value, Sequence): + raise ContractError(f"{name} must be a sequence") + if not value: + raise ContractError(f"{name} must not be empty") + return value diff --git a/code/experiments/validate.py b/code/experiments/validate.py new file mode 100644 index 0000000..c742595 --- /dev/null +++ b/code/experiments/validate.py @@ -0,0 +1,150 @@ +"""Independent validation of plans, manifests, and per-trial artifacts.""" + +from __future__ import annotations + +import json +from pathlib import Path +from typing import Any, Mapping + +import numpy as np + +from .hashing import file_sha256, stable_hash +from .plan import load_document, validate_trial_plan +from .schema import ContractError, require_schema + + +def validate_trial_directory( + trial_dir: Path, + trial: Mapping[str, Any], +) -> list[str]: + errors: list[str] = [] + trial_dir = Path(trial_dir) + manifest_path = trial_dir / "trial_manifest.json" + if not manifest_path.exists(): + return [f"{trial_dir}: missing trial_manifest.json"] + try: + manifest = load_document(manifest_path) + require_schema(manifest, "trial_manifest") + except Exception as exc: + return [f"{trial_dir}: invalid manifest: {exc}"] + if manifest.get("trial_id") != trial.get("trial_id"): + errors.append(f"{trial_dir}: trial_id mismatch") + if manifest.get("pair_id") != trial.get("pair_id"): + errors.append(f"{trial_dir}: pair_id mismatch") + if manifest.get("trial_spec_hash") != trial.get("trial_spec_hash"): + errors.append(f"{trial_dir}: trial_spec_hash mismatch") + if manifest.get("status") != "completed": + errors.append(f"{trial_dir}: status is not completed") + + declared_files = manifest.get("files", {}) + for relative, expected_hash in declared_files.items(): + path = trial_dir / relative + if not path.is_file(): + errors.append(f"{trial_dir}: missing {relative}") + elif file_sha256(path) != expected_hash: + errors.append(f"{trial_dir}: hash mismatch for {relative}") + + sample_path = trial_dir / "samples.npz" + if sample_path.exists(): + try: + with np.load(sample_path, allow_pickle=False) as archive: + expected_count = int(manifest.get("sample_count", -1)) + declared_fields = manifest.get("sample_fields", {}) + if set(archive.files) != set(declared_fields): + errors.append(f"{trial_dir}: NPZ fields differ from manifest") + for name in archive.files: + array = archive[name] + if array.ndim == 0 or array.shape[0] != expected_count: + errors.append( + f"{trial_dir}: invalid sample axis for field {name}" + ) + expected = declared_fields.get(name, {}) + if list(array.shape) != expected.get("shape"): + errors.append(f"{trial_dir}: shape mismatch for field {name}") + if str(array.dtype) != expected.get("dtype"): + errors.append(f"{trial_dir}: dtype mismatch for field {name}") + except Exception as exc: + errors.append(f"{trial_dir}: cannot read samples.npz: {exc}") + + event_path = trial_dir / "events.jsonl" + if event_path.exists(): + try: + with event_path.open("r", encoding="utf-8") as stream: + for line_number, line in enumerate(stream, start=1): + if line.strip(): + value = json.loads(line) + if not isinstance(value, dict): + raise ValueError(f"line {line_number} is not an object") + except Exception as exc: + errors.append(f"{trial_dir}: invalid events.jsonl: {exc}") + return errors + + +def validate_batch( + batch_dir: Path, + *, + require_complete: bool = True, +) -> dict[str, Any]: + batch_dir = Path(batch_dir) + errors: list[str] = [] + warnings: list[str] = [] + try: + plan = load_document(batch_dir / "plan.json") + validate_trial_plan(plan) + except Exception as exc: + return { + "valid": False, + "errors": [f"invalid plan: {exc}"], + "warnings": [], + "expected_trials": 0, + "completed_trials": 0, + } + try: + manifest = load_document(batch_dir / "batch_manifest.json") + require_schema(manifest, "batch_manifest") + if manifest.get("plan_hash") != plan.get("plan_hash"): + errors.append("batch manifest plan_hash mismatch") + hash_basis = dict(manifest) + recorded_manifest_hash = hash_basis.pop("manifest_hash", None) + hash_basis.pop("created_utc", None) + expected_manifest_hash = stable_hash( + hash_basis, + prefix="batch-manifest", + ) + if recorded_manifest_hash != expected_manifest_hash: + errors.append("batch manifest_hash mismatch") + except Exception as exc: + errors.append(f"invalid batch manifest: {exc}") + + completed = 0 + raw_dir = batch_dir / "raw" + for trial in plan["trials"]: + trial_dir = raw_dir / trial["trial_id"] + if not trial_dir.exists(): + if require_complete: + errors.append(f"missing trial {trial['trial_id']}") + continue + trial_errors = validate_trial_directory(trial_dir, trial) + errors.extend(trial_errors) + if not trial_errors: + completed += 1 + if raw_dir.exists(): + expected = {trial["trial_id"] for trial in plan["trials"]} + for entry in raw_dir.iterdir(): + if entry.name.startswith("."): + warnings.append(f"temporary trial artifact present: {entry.name}") + elif entry.is_dir() and entry.name not in expected: + warnings.append(f"unexpected trial directory: {entry.name}") + return { + "valid": not errors, + "errors": errors, + "warnings": warnings, + "expected_trials": plan["trial_count"], + "completed_trials": completed, + } + + +def require_valid_batch(batch_dir: Path, *, require_complete: bool = True) -> None: + report = validate_batch(batch_dir, require_complete=require_complete) + if not report["valid"]: + raise ContractError("; ".join(report["errors"])) diff --git a/code/hardware/can_msg_builder.py b/code/hardware/can_msg_builder.py new file mode 100644 index 0000000..c18de58 --- /dev/null +++ b/code/hardware/can_msg_builder.py @@ -0,0 +1,257 @@ +import can +import math +import struct +from typing import Dict, List, Tuple, Optional + +# 广播控制 +BROADCAST_TORQUE_CMD_ID = 0x280 +BROADCAST_VELOCITY_CMD_ID = 0x281 +BROADCAST_POSITION_CMD_ID = 0x282 +BROADCAST_MIX_CMD_ID = 0x288 +BROADCAST_CLEAR_ERR_CMD_ID = 0x9B +BROADCAST_TURN_ON_CMD_ID = 0x88 +BROADCAST_TURN_OFF_CMD_ID = 0x80 +BROADCAST_STOP_CMD_ID = 0x81 +BROADCAST_GET_STATE_CMD_ID = 0x9C +# 单电机控制 +SINGLE_MS_TORQUE_CMD_ID = 0xA0 +SINGLE_MG_TORQUE_CMD_ID = 0xA1 +SINGLE_VELOCITY_CMD_1_ID = 0xA2 +SINGLE_VELOCITY_CMD_2_ID = 0xAD +SINGLE_MULTITURN_POS_CMD_1_ID = 0xA3 +SINGLE_MULTITURN_POS_CMD_2_ID = 0xA4 +SINGLE_ONETURN_POS_CMD_1_ID = 0xA5 +SINGLE_ONETURN_POS_CMD_2_ID = 0xA6 +READ_MULTITURN_POS_CMD_ID = 0x92 +SET_MULTITURN_POS_CMD_ID = 0x95 +SINGLE_TURN_ON_CMD_ID = 0x88 +SINGLE_TURN_OFF_CMD_ID = 0x80 +SINGLE_STOP_CMD_ID = 0x81 +SINGLE_CLEAR_ERR_CMD_ID = 0x9B + + +def _clamp_int16(x: int) -> int: + return max(-32768, min(32767, int(x))) + +def _can_id_single_motor(motor_id: int) -> int: + """ + 单电机命令：标识符 = 0x140 + ID(1~32) + """ + assert 1 <= motor_id <= 32 + return 0x140 + motor_id + +def _pack_i16_le(x: int) -> Tuple[int, int]: + x = _clamp_int16(x) + u = x & 0xFFFF + return u & 0xFF, (u >> 8) & 0xFF + +def build_torque_broadcast(torques: List[int]) -> can.Message: + """ + 广播控制模式速度控制can帧构建 + torques: 电机电流控制量 + """ + # TODO： 不同电机型号的力矩与电流int16数值的换算 + t = [0, 0, 0, 0] + for i, torque in enumerate(torques): + t[i] = torque + + d0, d1 = _pack_i16_le(t[0]) + d2, d3 = _pack_i16_le(t[1]) + d4, d5 = _pack_i16_le(t[2]) + d6, d7 = _pack_i16_le(t[3]) + return can.Message( + arbitration_id=BROADCAST_TORQUE_CMD_ID, + is_extended_id=False, + data=[d0, d1, d2, d3, d4, d5, d6, d7], + ) + +def build_velocity_broadcast(vels: List[float]) -> can.Message: + """ + 广播控制模式速度控制can帧构建 + vels: 电机速度，单位：弧度/秒 + """ + v = [0, 0, 0, 0] + for i, vel in enumerate(vels): + v[i] = int(round(math.degrees(vel))) + d0, d1 = _pack_i16_le(v[0]) + d2, d3 = _pack_i16_le(v[1]) + d4, d5 = _pack_i16_le(v[2]) + d6, d7 = _pack_i16_le(v[3]) + return can.Message( + arbitration_id=BROADCAST_VELOCITY_CMD_ID, + is_extended_id=False, + data=[d0, d1, d2, d3, d4, d5, d6, d7], + ) + +def build_position_broadcast(rads: List[float]) -> can.Message: + """ + 广播控制模式位置控制can帧构建 + rad1: 电机1弧度 + rad2：电机2弧度 + rad2：电机3弧度 + rad2：电机4弧度 + """ + p = [0, 0, 0, 0] + for i, rad in enumerate(rads): + p[i] = int(round(math.degrees(rad) / 0.01)) + d0, d1 = _pack_i16_le(p[0]) + d2, d3 = _pack_i16_le(p[1]) + d4, d5 = _pack_i16_le(p[2]) + d6, d7 = _pack_i16_le(p[3]) + return can.Message( + arbitration_id=BROADCAST_POSITION_CMD_ID, + is_extended_id=False, + data=[d0, d1, d2, d3, d4, d5, d6, d7], + ) + +def build_turn_on_broadcast() -> can.Message: + return can.Message( + arbitration_id=BROADCAST_MIX_CMD_ID, + is_extended_id=False, + data=[ + BROADCAST_TURN_ON_CMD_ID, 0x00, + BROADCAST_TURN_ON_CMD_ID, 0x00, + BROADCAST_TURN_ON_CMD_ID, 0x00, + BROADCAST_TURN_ON_CMD_ID, 0x00] + ) + +def build_turn_off_broadcast() -> can.Message: + return can.Message( + arbitration_id=BROADCAST_MIX_CMD_ID, + is_extended_id=False, + data=[ + BROADCAST_TURN_OFF_CMD_ID, 0x00, + BROADCAST_TURN_OFF_CMD_ID, 0x00, + BROADCAST_TURN_OFF_CMD_ID, 0x00, + BROADCAST_TURN_OFF_CMD_ID, 0x00] + ) + +def build_stop_broadcast() -> can.Message: + return can.Message( + arbitration_id=BROADCAST_MIX_CMD_ID, + is_extended_id=False, + data=[ + BROADCAST_STOP_CMD_ID, 0x00, + BROADCAST_STOP_CMD_ID, 0x00, + BROADCAST_STOP_CMD_ID, 0x00, + BROADCAST_STOP_CMD_ID, 0x00 + ] + ) + +def build_clear_error_broadcast() -> can.Message: + return can.Message( + arbitration_id=BROADCAST_MIX_CMD_ID, + is_extended_id=False, + data=[ + BROADCAST_CLEAR_ERR_CMD_ID, 0x00, + BROADCAST_CLEAR_ERR_CMD_ID, 0x00, + BROADCAST_CLEAR_ERR_CMD_ID, 0x00, + BROADCAST_CLEAR_ERR_CMD_ID, 0x00 + ] + ) + +def build_get_state_broadcast() -> can.Message: + return can.Message( + arbitration_id=BROADCAST_MIX_CMD_ID, + is_extended_id=False, + data=[ + BROADCAST_GET_STATE_CMD_ID, 0x00, + BROADCAST_GET_STATE_CMD_ID, 0x00, + BROADCAST_GET_STATE_CMD_ID, 0x00, + BROADCAST_GET_STATE_CMD_ID, 0x00 + ] + ) + +def build_torque_single(motor_id: int, iq: int, is_MS: bool) -> can.Message: + """ + 单电机控制模式力矩控制can帧构建 + motor_id: 电机id + iq： 电流 + """ + # TODO： 不同电机型号的力矩与电流int16数值的换算 + d4, d5 = _pack_i16_le(iq) + if is_MS: + return can.Message( + arbitration_id=_can_id_single_motor(motor_id), + is_extended_id=False, + data=[SINGLE_MS_TORQUE_CMD_ID, 0x00, 0x00, 0x00, d4, d5, 0x00, 0x00], + ) + else: + return can.Message( + arbitration_id=_can_id_single_motor(motor_id), + is_extended_id=False, + data=[SINGLE_MG_TORQUE_CMD_ID, 0x00, 0x00, 0x00, d4, d5, 0x00, 0x00], + ) + +def build_velocity_single(motor_id: int, speed_rps: float) -> can.Message: + """ + 单电机控制模式速度控制can帧构建 + motor_id: 电机id + speed_rps： 电机转速，单位 弧度/秒 + """ + speed_control = int(round(math.degrees(speed_rps) / 0.01)) # 0.01 dps/LSB + data = bytearray(8) + data[0] = 0xA2 + data[1] = 0x00 + data[2] = 0x00 + data[3] = 0x00 + data[4:8] = struct.pack(" can.Message: + """ + 单电机控制模式多圈位置控制can帧构建 + motor_id: 电机id + rad： 电机多圈角度，单位 弧度 + """ + multiturn_pos = int(round(math.degrees(rad) / 0.01)) + data = bytearray(8) + data[0] = SET_MULTITURN_POS_CMD_ID + data[1] = 0x00 + data[2] = 0x00 + data[3] = 0x00 + data[4:8] = struct.pack(" can.Message: + return can.Message( + arbitration_id=_can_id_single_motor(motor_id), + is_extended_id=False, + data=[READ_MULTITURN_POS_CMD_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], + ) + +def build_turn_on_single(motor_id: int) -> can.Message: + return can.Message( + arbitration_id=_can_id_single_motor(motor_id), + is_extended_id=False, + data=[SINGLE_TURN_ON_CMD_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], + ) + +def build_turn_off_single(motor_id: int) -> can.Message: + return can.Message( + arbitration_id=_can_id_single_motor(motor_id), + is_extended_id=False, + data=[SINGLE_TURN_OFF_CMD_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], + ) + +def build_stop_single(motor_id: int) -> can.Message: + return can.Message( + arbitration_id=_can_id_single_motor(motor_id), + is_extended_id=False, + data=[SINGLE_STOP_CMD_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], + ) + +def build_clear_error_single(motor_id: int) -> can.Message: + return can.Message( + arbitration_id=_can_id_single_motor(motor_id), + is_extended_id=False, + data=[SINGLE_CLEAR_ERR_CMD_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], + ) \ No newline at end of file diff --git a/code/hardware/can_msg_parser.py b/code/hardware/can_msg_parser.py new file mode 100644 index 0000000..ac8da9e --- /dev/null +++ b/code/hardware/can_msg_parser.py @@ -0,0 +1,43 @@ +import math +import struct +from typing import Dict, List, Tuple, Optional +from can_msg_builder import BROADCAST_MIX_CMD_ID, BROADCAST_TORQUE_CMD_ID + + +def parse_single_turn_state(data8: bytes) -> Dict: + """ + 解析电机状态2回复can帧 + """ + assert len(data8)==8 + temperature = struct.unpack(" Dict: + """ + 解析“读取多圈角度(0x92)”回复帧 + - DATA[0] = 0x92 + - DATA[1..7] = int64 motorAngle 的低 7 字节（little-endian） + - 单位：0.01 deg / LSB + """ + assert len(data8) == 8 + # 取低 7 字节 + low7 = data8[1:8] # bytes length = 7 + # 符号扩展：看第7字节(低7字节中的最高字节)的最高位是否为1 + # 如果为1，说明负数，需要补 0xFF；否则补 0x00 + sign_ext = 0xFF if (low7[6] & 0x80) else 0x00 + raw8 = low7 + bytes([sign_ext]) # 补成 8 字节 little-endian 的 int64 + + motor_angle_raw = struct.unpack("= self.idle_timeout_s: + if now >= self._next_idle_ts: + try: + self._thread_idle() + except usb.core.USBTimeoutError: + time.sleep(0.005) + except can.CanError: + pass + # 维持均匀节拍 + self._next_idle_ts += self.idle_period + else: + # 睡到下一次 idle + time.sleep(min(0.001, self._next_idle_ts - now)) + else: + # 刚停止控制不久：短睡，等进入 idle + time.sleep(0.001) + + + +class BroadcastSenderThread(SenderThread): + def __init__( + self, + name: str, + bus: can.BusABC, + bus_lock: threading.Lock, + channel_index: int, # 0 or 1 + tick_event: threading.Event, + barrier: threading.Barrier, + stop_event: threading.Event, + ): + super().__init__(name, bus, bus_lock, channel_index, tick_event, barrier, stop_event) + + def turn_on(self): + self.queue.put(build_turn_on_broadcast()) + + def turn_off(self): + self.queue.put(build_turn_off_broadcast()) + + def stop(self): + self.queue.put(build_stop_broadcast()) + + def clear_error(self): + self.queue.put(build_clear_error_broadcast()) + + def set_zero(self): + pass + + def set_torq(self, torques): + assert len(torques) <= 4 + self.queue.put(build_torque_broadcast(torques)) + + def set_vel(self, vel: List[float]): + assert len(vel) <= 4 + self.queue.put(build_velocity_broadcast(vel)) + + def set_pos(self, pos): + assert len(pos) <= 4 + self.queue.put(build_position_broadcast(pos)) + + def _thread_idle(self): + msg = build_get_state_broadcast() + msg.channel = self.channel_index + try: + with self.bus_lock: + self.bus.send(msg) + except can.CanError: + pass + +class SingleSenderThread(SenderThread): + def __init__( + self, + name: str, + bus: can.BusABC, + bus_lock: threading.Lock, + channel_index: int, # 0 or 1 + tick_event: threading.Event, + barrier: threading.Barrier, + stop_event: threading.Event, + ms_index: List[int] + ): + super().__init__(name, bus, bus_lock, channel_index, tick_event, barrier, stop_event) + self.ms_index = ms_index + + def turn_on(self): + for i in range(1, 5): + self.queue.put(build_turn_on_single(i)) + + def turn_off(self): + for i in range(1, 5): + self.queue.put(build_turn_off_single(i)) + + def stop(self): + for i in range(1, 5): + self.queue.put(build_stop_single(i)) + + def clear_error(self): + for i in range(1, 5): + self.queue.put(build_clear_error_single(i)) + + def set_zero(self): + pass + + def set_torq(self, torques): + for i in range(1, 5): + if i in self.ms_index: + self.queue.put(build_torque_single(i, torques[i-1], True)) + else: + self.queue.put(build_torque_single(i, torques[i-1], False)) + + def set_vel(self, vel: List[float]): + for i in range(1, 5): + self.queue.put(build_velocity_single(i, vel[i - 1])) + + def set_pos(self, pos): + for i in range(1, 5): + self.queue.put(build_set_multiturn_position_single(i, pos[i - 1])) + + def _thread_idle(self): + for i in range(1, 5): + msg = build_get_multiturn_position_single(i) + msg.channel = self.channel_index + try: + with self.bus_lock: + self.bus.send(msg) + except can.CanError: + pass + except usb.core.USBTimeoutError: + time.sleep(0.001) + + + +@dataclass +class RxFrame: + t: float = 0.0 + arb: int = 0 + data: bytes = b"" + + +@dataclass +class MotorState: + stamp: float = 0.0 + temp: float = 0.0 + single_turn_rad: float = 0.0 + multi_turn_rad: float = 0.0 + vel: float = 0.0 + power_raw: float = 0.0 + + +class RxCache4Ch(can.Listener): + """ + 一个 Notifier 监听一个 “设备Bus(包含两个channel)”，按 msg.channel 分流到 ch0/ch1 的缓存。 + """ + def __init__(self, joint_map: Dict[int, List[str]]) -> None: + super().__init__() + self._lock = threading.Lock() + self.joint_state = {} + self.ch_mid_idx = {} + for ch, joint_names in joint_map.items(): + for i, joint_name in enumerate(joint_names): + self.ch_mid_idx[(ch, i+1)] = joint_name + self.joint_state[joint_name] = MotorState() + + def on_message_received(self, msg: can.Message): + # python-can Message.channel：对 canalystii 多通道会填 0/1 + ch = getattr(msg, "channel", None) + if ch is None: + # 没有 channel 信息就无法分流，直接忽略或当作 ch0 + ch = 0 + try: + ch = int(ch) + except Exception: + ch = 0 + + arb = msg.arbitration_id + if arb < 0x141 or arb > 0x160: + return + mid = arb - 0x140 + + joint_name = self.ch_mid_idx[(ch, mid)] + if joint_name is None: + return + + if msg.data[0] in [0xA0, 0xA1, 0x9C]: + res = parse_single_turn_state(msg.data) + # print(res) + with self._lock: + self.joint_state[joint_name].stamp = time.time() + self.joint_state[joint_name].temp = res["temperature_C"] + self.joint_state[joint_name].power_raw = res["iq_or_power_raw"] + self.joint_state[joint_name].vel = res["speed_dps"] + self.joint_state[joint_name].single_turn_rad = res["rad"] + elif msg.data[0] in [0x92]: + res = parse_multi_turn_state(msg.data) + with self._lock: + self.joint_state[joint_name].multi_turn_rad =res["rad"] + else: + print(msg.data[0]) + + def snapshot(self): + with self._lock: + return self.joint_state + + \ No newline at end of file diff --git a/code/hardware/exoskeleton.py b/code/hardware/exoskeleton.py new file mode 100644 index 0000000..a2335ce --- /dev/null +++ b/code/hardware/exoskeleton.py @@ -0,0 +1,172 @@ +import time + +import can +import threading +from typing import Dict, List, Tuple, Optional +from canbus import RxCache4Ch, BroadcastSenderThread, SingleSenderThread +from usb_can import resolve_canalyst_device_index +from can_msg_builder import * + + +class Exoskeleton: + def __init__(self): + self.bitrate = 1000000 + self.motor_map = { + ("dev0", 0): [1, 2, 3, 4], + ("dev0", 1): [1, 2, 3, 4], + ("dev1", 0): [1, 2, 3, 4], + ("dev1", 1): [1, 2, 3, 4], + } + self.joint_map = { + "sender-dev0-ch0": ["LJ1", "LJ2", "RJ1", "RJ2"], + "sender-dev0-ch1": ["LJ5", "LJ3", "RJ5", "RJ3"], + "sender-dev1-ch0": ["LJ7", "LJ6", "LJ4"], + "sender-dev1-ch1": ["RJ7", "RJ6", "RJ4"], + } + + idx0 = resolve_canalyst_device_index("/dev/canalystii_1") + idx1 = resolve_canalyst_device_index("/dev/canalystii_0") + self.bus0 = can.Bus(interface="canalystii", channel=[0, 1], bitrate=self.bitrate, device=idx0) + self.bus1 = can.Bus(interface="canalystii", channel=[0, 1], bitrate=self.bitrate, device=idx1) + + # ========= 接收缓存：每个设备一个 Notifier，一个 cache（按 channel 分流） ========= + self.cache0 = RxCache4Ch({0: ["LJ1", "LJ2", "RJ1", "RJ2"], 1: ["LJ5", "LJ3", "RJ5", "RJ3"]}) + self.cache1 = RxCache4Ch({0: ["LJ7", "LJ6", "LJ4"], 1: ["RJ7", "RJ6", "RJ4"]}) + self.notifier0 = can.Notifier(self.bus0, [self.cache0], timeout=0.01) + self.notifier1 = can.Notifier(self.bus1, [self.cache1], timeout=0.01) + + # ========= 发送线程：4个逻辑channel ========= + self.stop_event = threading.Event() + self.cmd_event = threading.Event() + self.barrier = threading.Barrier(4 + 1) # 4个sender + 主线程 + + self.bus0_lock = threading.Lock() + self.bus1_lock = threading.Lock() + + sender_dev0_ch0 = BroadcastSenderThread( + "sender-dev0-ch0", self.bus0, self.bus0_lock, 0, self.cmd_event, self.barrier, self.stop_event) + sender_dev0_ch1 = SingleSenderThread( + "sender-dev0-ch1", self.bus0, self.bus0_lock, 1, self.cmd_event, self.barrier, self.stop_event, [1, 3]) + sender_dev1_ch0 = BroadcastSenderThread( + "sender-dev1-ch0", self.bus1, self.bus1_lock, 0, self.cmd_event, self.barrier, self.stop_event) + sender_dev1_ch1 = BroadcastSenderThread( + "sender-dev1-ch1", self.bus1, self.bus1_lock, 1, self.cmd_event, self.barrier, self.stop_event) + self.senders = [sender_dev0_ch0, sender_dev0_ch1, sender_dev1_ch0, sender_dev1_ch1] + for sender in self.senders: + sender.start() + + def turn_on(self): + for sender in self.senders: + sender.turn_on() + self.cmd_event.set() + try: + self.barrier.wait(timeout=0.05) + except threading.BrokenBarrierError: + pass + self.cmd_event.clear() + + def turn_off(self): + for sender in self.senders: + sender.turn_off() + self.cmd_event.set() + try: + self.barrier.wait(timeout=0.05) + except threading.BrokenBarrierError: + pass + self.cmd_event.clear() + + def clear_error(self): + for sender in self.senders: + sender.clear_error() + self.cmd_event.set() + try: + self.barrier.wait(timeout=0.05) + except threading.BrokenBarrierError: + pass + self.cmd_event.clear() + + def stop_all(self): + for sender in self.senders: + sender.stop() + self.cmd_event.set() + try: + self.barrier.wait(timeout=0.05) + except threading.BrokenBarrierError: + pass + self.cmd_event.clear() + + def set_zero(self): + for sender in self.senders: + sender.set_zero() + self.cmd_event.set() + try: + self.barrier.wait(timeout=0.05) + except threading.BrokenBarrierError: + pass + self.cmd_event.clear() + + def set_torq(self, torq: Dict[str, float]): + torq_cmd = {} + for sender in self.senders: + torq_cmd[sender.name] = [torq[x] for x in self.joint_map[sender.name]] + for sender in self.senders: + sender.set_torq(torq_cmd[sender.name]) + self.cmd_event.set() + try: + self.barrier.wait(timeout=0.05) + except threading.BrokenBarrierError: + pass + self.cmd_event.clear() + + def set_pos(self, pos: Dict[str, float]): + pos_cmd = {} + for sender in self.senders: + pos_cmd[sender.name] = [pos[x] for x in self.joint_map[sender.name]] + for sender in self.senders: + sender.set_pos(pos_cmd[sender.name]) + self.cmd_event.set() + try: + self.barrier.wait(timeout=0.05) + except threading.BrokenBarrierError: + pass + self.cmd_event.clear() + + def get_states(self): + c0 = self.cache0.snapshot() + c1 = self.cache1.snapshot() + print({"c0": c0, "c1": c1}) + + @staticmethod + def _arb_id_single(motor_id: int) -> int: + return 0x140 + int(motor_id) + + +if __name__ == "__main__": + exoskeleton = Exoskeleton() + exoskeleton.turn_on() + count = 0 + try: + while True: + time.sleep(0.02) + exoskeleton.set_torq({ + "LJ1": 30, "LJ2": 30, "LJ3": 30, "LJ4": 30, "LJ5": 100, "LJ6": 100, "LJ7": 100, + "RJ1": 30, "RJ2": 30, "RJ3": 30, "RJ4": 30, "RJ5": 100, "RJ6": 100, "RJ7": 100, + }) + count += 1 + if count % 10 == 0: + exoskeleton.get_states() + except KeyboardInterrupt: + exoskeleton.set_torq({ + "LJ1": 0, "LJ2": 0, "LJ3": 0, "LJ4": 0, "LJ5": 0, "LJ6": 0, "LJ7": 0, + "RJ1": 0, "RJ2": 0, "RJ3": 0, "RJ4": 0, "RJ5": 0, "RJ6": 0, "RJ7": 0, + }) + exoskeleton.stop_all() + exoskeleton.turn_off() + exoskeleton.stop_event.set() + exoskeleton.cmd_event.set() + for th in exoskeleton.senders: + th.join(timeout=0.5) + exoskeleton.notifier0.stop() + exoskeleton.notifier1.stop() + exoskeleton.bus0.shutdown() + exoskeleton.bus1.shutdown() diff --git a/code/hardware/usb_can.py b/code/hardware/usb_can.py new file mode 100644 index 0000000..4693cb7 --- /dev/null +++ b/code/hardware/usb_can.py @@ -0,0 +1,54 @@ +import usb.core +import usb.util +import os +import re + +CANALYST_VID = 0x04d8 +CANALYST_PID = 0x0053 + + +def _bus_dev_from_symlink(symlink: str): + """ + /dev/canalyst_left -> /dev/bus/usb/003/004 + return (busnum, devnum) + """ + real = os.path.realpath(symlink) + m = re.search(r"/usb/(\d+)/(\d+)$", real) + if not m: + raise RuntimeError(f"Cannot parse bus/dev from {real}") + return int(m.group(1)), int(m.group(2)) + + +def resolve_canalyst_device_index(symlink: str) -> int: + """ + Resolve canalystii logical device index from udev symlink. + """ + target_bus, target_dev = _bus_dev_from_symlink(symlink) + + devices = list( + usb.core.find( + find_all=True, + idVendor=CANALYST_VID, + idProduct=CANALYST_PID, + ) + ) + + if not devices: + raise RuntimeError("No CANalyst-II device found") + + for idx, dev in enumerate(devices): + # pyusb 的 bus / address 就是 BUSNUM / DEVNUM + if dev.bus == target_bus and dev.address == target_dev: + return idx + + raise RuntimeError( + f"CANalyst-II {symlink} (bus={target_bus}, dev={target_dev}) not found in pyusb list" + ) + + +if __name__ == "__main__": + id0 = resolve_canalyst_device_index("/dev/canalystii_0") + id1 = resolve_canalyst_device_index("/dev/canalystii_1") + + print(id0) + print(id1) diff --git a/code/script/99-canalystii.rules b/code/script/99-canalystii.rules new file mode 100644 index 0000000..26447e8 --- /dev/null +++ b/code/script/99-canalystii.rules @@ -0,0 +1,2 @@ +SUBSYSTEM=="usb", ATTR{idVendor}=="04d8", ATTR{idProduct}=="0053", KERNELS=="3-7", SYMLINK+="canalystii_0", MODE="0666" +SUBSYSTEM=="usb", ATTR{idVendor}=="04d8", ATTR{idProduct}=="0053", KERNELS=="3-8", SYMLINK+="canalystii_1", MODE="0666" diff --git a/code/script/init_can.sh b/code/script/init_can.sh new file mode 100644 index 0000000..9bc7eda --- /dev/null +++ b/code/script/init_can.sh @@ -0,0 +1,4 @@ +sudo ip link set can0 down +sudo ip link set can0 type can bitrate 1000000 restart-ms 100 +sudo ip link set can0 up +ip -details link show can0 \ No newline at end of file diff --git a/code/script/probe_canalystii_ports.sh b/code/script/probe_canalystii_ports.sh new file mode 100755 index 0000000..edd4024 --- /dev/null +++ b/code/script/probe_canalystii_ports.sh @@ -0,0 +1,56 @@ +#!/usr/bin/env bash +set -euo pipefail + +echo "=== USB devices (candidate CAN adapters) ===" +# 先列出所有USB设备，方便你看到Vendor/Product +lsusb + +echo +echo "=== Finding candidate USB devices by vendor/product (you may need to adjust filters) ===" +echo "Tip: if you know VID:PID, set VIDPID like: VIDPID=1d50:606f ./probe_canalystii_ports.sh" +VIDPID="${VIDPID:-}" +if [[ -n "${VIDPID}" ]]; then + lsusb | grep -i "${VIDPID}" || true +else + echo "(No VIDPID specified. We'll show details for all USB devices; you can narrow it later.)" +fi + +echo +echo "=== Enumerate /dev/bus/usb/*/* and print stable attributes ===" + +# 遍历所有 USB device node，提取关键属性：idVendor/idProduct + DEVPATH + KERNELS(物理端口链) +for devnode in /dev/bus/usb/*/*; do + # 过滤：如果设置了VIDPID，则只看匹配的 + if [[ -n "${VIDPID}" ]]; then + info=$(udevadm info -a -n "$devnode" 2>/dev/null | head -n 80 || true) + echo "$info" | grep -qi "ATTR{idVendor}==\"${VIDPID%%:*}\"" || continue + echo "$info" | grep -qi "ATTR{idProduct}==\"${VIDPID##*:}\"" || continue + fi + + # udev属性输出 + path=$(udevadm info -q path -n "$devnode" 2>/dev/null || true) + [[ -z "$path" ]] && continue + + props=$(udevadm info -q property -n "$devnode" 2>/dev/null || true) + vid=$(echo "$props" | awk -F= '/^ID_VENDOR_ID=/{print $2}') + pid=$(echo "$props" | awk -F= '/^ID_MODEL_ID=/{print $2}') + busnum=$(echo "$props" | awk -F= '/^BUSNUM=/{print $2}') + devnum=$(echo "$props" | awk -F= '/^DEVNUM=/{print $2}') + + [[ -z "$vid" || -z "$pid" ]] && continue + + # 取 KERNELS 链：包含类似 2-3 / 1-1.2 这种物理端口信息 + kernels=$(udevadm info -a -n "$devnode" 2>/dev/null | awk -F'==' '/KERNELS==/{print $2}' | head -n 8 | tr -d '"' | tr '\n' ' ') + + echo "----" + echo "DEVNODE : $devnode" + echo "VID:PID : ${vid}:${pid}" + echo "BUS/DEV : ${busnum:-?}/${devnum:-?}" + echo "DEVPATH : $path" + echo "KERNELS : $kernels" +done + +echo +echo "=== Next steps ===" +echo "1) Identify the two CANalyst-II devices (same VID:PID) but different KERNELS like 2-3 and 2-4" +echo "2) Use those KERNELS values to write udev rules mapping them to /dev/canalyst_left and /dev/canalyst_right" diff --git a/code/simulate_closed_loop.py b/code/simulate_closed_loop.py new file mode 100644 index 0000000..3689378 --- /dev/null +++ b/code/simulate_closed_loop.py @@ -0,0 +1,1974 @@ +#!/usr/bin/env python3 +"""Closed-loop bilateral simulation for the heterogeneous 7-DoF arms. + +This program is deliberately a *simulation validation*, not a replacement for +the future prototype experiment. Both arms are integrated with their URDF +rigid-body dynamics. A computed-torque human proxy drives the master, a +computed-torque controller drives the slave, and a unilateral spring-damper +wall acts at a documented simulation-only TCP attached to the terminal wrist. + +Three otherwise identical cases are run: + +``proposed_energy`` + ``tau_m = A(q_m).T @ tau_s`` plus final applied-port energy supervision. +``direct_energy`` + The former direct baseline ``tau_m = J_m.T @ F_s`` with the same output + shaping and energy supervision. +``proposed_no_energy`` + The proposed differential mapping with the energy projection bypassed. + +The comparison separates the two implementation questions: virtual-work +consistency of the retargeting map, and the final-port energy safety layer. +""" + +from __future__ import annotations + +import argparse +import csv +import json +import math +import time +from collections import deque +from dataclasses import asdict, dataclass, fields, replace +from pathlib import Path +from typing import Any, Iterable + +import numpy as np +import pinocchio as pin +import yaml + +from core.haptic_render import HapticRenderer +from core.interaction_estimater import InteractionEstimator +from core.feedback_protocol import ( + ForwardPacket, + MapPolicy, + MapRegistry, + MapSnapshot, + MappingKind, + PacketState, + ReturnPacket, + map_return_feedback, +) +from core.network_emulator import ( + DeterministicChannel, + PacketReceiver, + generate_network_trace, +) +from core.model_contract import ( + MASTER_FRAMES, + MASTER_JOINT_NAMES, + MASTER_URDF, + SLAVE_FRAMES, + SLAVE_JOINT_NAMES, + SLAVE_URDF, + TeleoperationModels, + clip_configuration, + load_models, + require_frame, +) +from core.sew_mapper2 import BallJointConfig, SEWMapper +from core.time_domain_popc import TimeDomainPOPC +from core.wrench_solver import ScaledDLSSolver + + +DEFAULT_OUTPUT_DIR = Path(__file__).resolve().parents[1] / "output" / "simulation" +DEFAULT_CONFIG_PATH = Path(__file__).resolve().parent / "config" / "config.yaml" + + +@dataclass(frozen=True) +class SimulationConfig: + """Numerical and controller parameters for the reproducible comparison.""" + + dt: float = 0.002 + duration: float = 4.0 + mapping_hz: float = 50.0 + seed: int = 7 + slave_contact_frame: str = SLAVE_FRAMES["ee"] + feedback_delay_s: float = 0.080 + forward_delay_s: float = 0.0 + forward_jitter_s: float = 0.0 + return_jitter_s: float = 0.0 + forward_packet_loss: float = 0.0 + return_packet_loss: float = 0.0 + forward_timeout_s: float = 0.20 + return_timeout_s: float = 0.20 + contact_probe_fraction: float = 0.03 + contact_probe_cycles: float = 3.0 + + wall_fraction: float = 0.55 + wall_stiffness: float = 800.0 + wall_damping: float = 45.0 + wall_force_limit: float = 80.0 + wall_transition_depth: float = 0.0001 + + master_kp: tuple[float, ...] = ( + 196.0, + 196.0, + 144.0, + 256.0, + 100.0, + 100.0, + 81.0, + ) + master_kd: tuple[float, ...] = ( + 28.0, + 28.0, + 24.0, + 32.0, + 20.0, + 20.0, + 18.0, + ) + slave_kp: tuple[float, ...] = ( + 900.0, + 900.0, + 676.0, + 1156.0, + 400.0, + 324.0, + 324.0, + ) + slave_kd: tuple[float, ...] = ( + 54.0, + 54.0, + 46.8, + 61.2, + 36.0, + 32.4, + 32.4, + ) + master_acceleration_limits: tuple[float, ...] = ( + 100.0, + 100.0, + 120.0, + 120.0, + 160.0, + 160.0, + 160.0, + ) + slave_acceleration_limits: tuple[float, ...] = ( + 120.0, + 120.0, + 150.0, + 150.0, + 180.0, + 180.0, + 180.0, + ) + master_tracking_effort_fraction: float = 0.65 + slave_tracking_effort_fraction: float = 0.70 + velocity_limit_fraction: float = 0.80 + soft_limit_buffer: float = 0.12 + + feedback_strength: float = 0.50 + haptic_filter_alpha: float = 0.222 + haptic_torque_limits: tuple[float, ...] = ( + 6.0, + 6.0, + 4.0, + 3.0, + 1.0, + 1.0, + 1.0, + ) + haptic_rate_limits: tuple[float, ...] = ( + 150.0, + 150.0, + 100.0, + 80.0, + 30.0, + 30.0, + 30.0, + ) + + energy_min: float = 0.05 + energy_max: float = 0.055 + energy_initial: float = 0.05 + + sensor_noise_std: float = 0.001 + wrench_characteristic_length_m: float = 0.30 + wrench_scaled_damping: float = 1e-3 + sensor_bias: tuple[float, ...] = ( + 0.080, + -0.050, + 0.035, + -0.025, + 0.015, + -0.010, + 0.020, + ) + bias_calibration_samples: int = 200 + + joint_limit_margin: float = 1e-5 + differential_step: float = 1e-4 + + def validate(self) -> None: + if not np.isfinite(self.dt) or self.dt <= 0.0: + raise ValueError("dt must be finite and positive") + if not np.isfinite(self.duration) or self.duration <= 0.0: + raise ValueError("duration must be finite and positive") + if not np.isfinite(self.mapping_hz) or self.mapping_hz <= 0.0: + raise ValueError("mapping_hz must be finite and positive") + if self.mapping_hz > 1.0 / self.dt: + raise ValueError("mapping_hz cannot exceed the dynamics rate") + if not 0.0 < self.wall_fraction < 1.0: + raise ValueError("wall_fraction must lie strictly inside (0, 1)") + if ( + self.wall_stiffness <= 0.0 + or self.wall_damping < 0.0 + or self.wall_force_limit <= 0.0 + or self.wall_transition_depth < 0.0 + ): + raise ValueError("wall parameters must be non-negative") + network_times = ( + self.feedback_delay_s, + self.forward_delay_s, + self.forward_jitter_s, + self.return_jitter_s, + ) + if any(value < 0.0 or not np.isfinite(value) for value in network_times): + raise ValueError("network delays and jitter must be finite/non-negative") + if self.forward_timeout_s <= 0.0 or self.return_timeout_s <= 0.0: + raise ValueError("network timeouts must be positive") + if not 0.0 <= self.forward_packet_loss <= 1.0: + raise ValueError("forward_packet_loss must lie in [0, 1]") + if not 0.0 <= self.return_packet_loss <= 1.0: + raise ValueError("return_packet_loss must lie in [0, 1]") + if self.contact_probe_fraction < 0.0 or self.contact_probe_cycles < 0.0: + raise ValueError("contact probe parameters cannot be negative") + if not ( + 0.0 <= self.energy_min <= self.energy_initial <= self.energy_max + ): + raise ValueError( + "energy values must satisfy 0 <= min <= initial <= max" + ) + if len(self.haptic_torque_limits) != 7: + raise ValueError("haptic_torque_limits must contain seven entries") + if len(self.haptic_rate_limits) != 7: + raise ValueError("haptic_rate_limits must contain seven entries") + if len(self.sensor_bias) != 7: + raise ValueError("sensor_bias must contain seven entries") + if ( + self.wrench_characteristic_length_m <= 0.0 + or self.wrench_scaled_damping <= 0.0 + ): + raise ValueError("scaled wrench solver parameters must be positive") + vector_parameters = ( + self.master_kp, + self.master_kd, + self.slave_kp, + self.slave_kd, + self.master_acceleration_limits, + self.slave_acceleration_limits, + ) + if any(len(values) != 7 for values in vector_parameters): + raise ValueError("all gain and acceleration-limit vectors need 7 entries") + if not 0.0 < self.velocity_limit_fraction <= 1.0: + raise ValueError("velocity_limit_fraction must lie in (0, 1]") + if not 0.0 < self.master_tracking_effort_fraction <= 1.0: + raise ValueError("master_tracking_effort_fraction must lie in (0, 1]") + if not 0.0 < self.slave_tracking_effort_fraction <= 1.0: + raise ValueError("slave_tracking_effort_fraction must lie in (0, 1]") + + +@dataclass(frozen=True) +class Scenario: + key: str + mapping: str + supervise_energy: bool + description: str + supervisor: str = "tank" + map_policy: str = "current" + + +SCENARIOS = ( + Scenario( + key="proposed_energy", + mapping=MappingKind.DIFFERENTIAL_RESIDUAL.value, + supervise_energy=True, + description="A(q_m)^T tau_s with final applied-port supervision", + supervisor="tank", + map_policy=MapPolicy.CURRENT.value, + ), + Scenario( + key="direct_energy", + mapping=MappingKind.DIRECT_MASTER_JACOBIAN.value, + supervise_energy=True, + description="J_m^T F_s matched-wrench baseline with tank supervision", + supervisor="tank", + map_policy=MapPolicy.CURRENT.value, + ), + Scenario( + key="proposed_no_energy", + mapping=MappingKind.DIFFERENTIAL_RESIDUAL.value, + supervise_energy=False, + description="A(q_m)^T tau_s with final projection bypassed", + supervisor="bypass", + map_policy=MapPolicy.CURRENT.value, + ), + Scenario( + key="matched_wrench_energy", + mapping=MappingKind.MATCHED_DIFFERENTIAL_WRENCH.value, + supervise_energy=True, + description=( + "A(source)^T J_s^T F_s matched to direct J_m^T F_s input" + ), + supervisor="tank", + map_policy=MapPolicy.SOURCE_STAMPED.value, + ), + Scenario( + key="proposed_popc", + mapping=MappingKind.DIFFERENTIAL_RESIDUAL.value, + supervise_energy=False, + description="A(q_m)^T tau_s with time-domain PO/PC", + supervisor="popc", + map_policy=MapPolicy.CURRENT.value, + ), +) + + +def load_simulation_config(path: Path) -> SimulationConfig: + """Load the ``simulation`` block while rejecting silent key drift.""" + with path.open("r", encoding="utf-8") as stream: + document = yaml.safe_load(stream) + if not isinstance(document, dict) or not isinstance( + document.get("simulation"), dict + ): + raise ValueError(f"{path} has no mapping-valued 'simulation' block") + + raw = dict(document["simulation"]) + if "slave_tcp_frame" in raw: + raw["slave_contact_frame"] = raw.pop("slave_tcp_frame") + # The offset is consumed by model_contract.py; it is retained in YAML as + # provenance, not duplicated as a simulator constructor parameter. + raw.pop("slave_tcp_offset", None) + + field_map = {field.name: field for field in fields(SimulationConfig)} + unknown = sorted(set(raw) - set(field_map)) + if unknown: + raise ValueError(f"Unknown simulation config keys: {unknown}") + tuple_fields = { + field.name + for field in fields(SimulationConfig) + if isinstance(field.default, tuple) + } + for name in tuple_fields & raw.keys(): + raw[name] = tuple(raw[name]) + config = SimulationConfig(**raw) + config.validate() + return config + + +@dataclass(frozen=True) +class Wall: + point: np.ndarray + normal: np.ndarray + stiffness: float + damping: float + force_limit: float + transition_depth: float = 0.0 + + def wrench( + self, + position_world: np.ndarray, + linear_velocity_world: np.ndarray, + ) -> tuple[np.ndarray, float]: + """Return environment-on-robot wrench and unilateral penetration.""" + penetration = max( + 0.0, + float(np.dot(self.normal, position_world - self.point)), + ) + if penetration <= 0.0: + return np.zeros(6, dtype=float), 0.0 + + normal_velocity = float(np.dot(self.normal, linear_velocity_world)) + if self.transition_depth > 0.0: + damping_activation = min(1.0, penetration / self.transition_depth) + else: + damping_activation = 1.0 + force_magnitude = min( + self.force_limit, + max( + 0.0, + self.stiffness * penetration + + damping_activation * self.damping * normal_velocity, + ), + ) + force = -force_magnitude * self.normal + return np.concatenate((force, np.zeros(3, dtype=float))), penetration + + +@dataclass +class ScenarioResult: + scenario: Scenario + metrics: dict[str, Any] + logs: dict[str, np.ndarray] + + +def build_mapper(models: TeleoperationModels) -> SEWMapper: + """Construct the bounded mapper with the axes/signs in the real slave URDF.""" + return SEWMapper( + master_model=models.master, + slave_model=models.slave, + m_shoulder=MASTER_FRAMES["shoulder"], + m_elbow=MASTER_FRAMES["elbow"], + m_wrist=MASTER_FRAMES["wrist"], + m_ee=MASTER_FRAMES["ee"], + s_shoulder=SLAVE_FRAMES["shoulder"], + s_elbow=SLAVE_FRAMES["elbow"], + s_wrist=SLAVE_FRAMES["wrist"], + # Retarget the physical wrist orientation; the added TCP is only used + # for simulated contact and wrench estimation. + s_ee=SLAVE_FRAMES["wrist"], + master_joint_names=MASTER_JOINT_NAMES, + slave_joint_names=SLAVE_JOINT_NAMES, + slave_shoulder_cfg=BallJointConfig( + axis_order="yxy", + joint_names=SLAVE_JOINT_NAMES[:3], + signs=(-1.0, 1.0, -1.0), + ), + slave_wrist_cfg=BallJointConfig( + axis_order="yzx", + joint_names=SLAVE_JOINT_NAMES[4:], + signs=(-1.0, 1.0, 1.0), + ), + slave_elbow_axis_local=np.array([1.0, 0.0, 0.0]), + up_dir=np.array([0.0, 0.0, 1.0]), + ) + + +def master_endpoint_configurations() -> tuple[np.ndarray, np.ndarray]: + """Return an interior, smooth, exactly recoverable 4.6 cm wrist reach.""" + q_start = np.array( + [0.25, 0.25, -0.20, 1.90, -0.10, 0.10, -0.10], + dtype=float, + ) + q_end = q_start.copy() + q_end[3] = 1.80 + return q_start, q_end + + +def _smooth_transition( + t: float, + t0: float, + t1: float, +) -> tuple[float, float, float]: + if t <= t0: + return 0.0, 0.0, 0.0 + if t >= t1: + return 1.0, 0.0, 0.0 + duration = t1 - t0 + u = (t - t0) / duration + position = 10.0 * u**3 - 15.0 * u**4 + 6.0 * u**5 + velocity = (30.0 * u**2 - 60.0 * u**3 + 30.0 * u**4) / duration + acceleration = ( + 60.0 * u - 180.0 * u**2 + 120.0 * u**3 + ) / duration**2 + return position, velocity, acceleration + + +def master_reference( + t: float, + duration: float, + q_start: np.ndarray, + q_end: np.ndarray, + contact_probe_fraction: float = 0.0, + contact_probe_cycles: float = 0.0, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Approach, probe the delayed contact channel, return, then settle.""" + approach_start = 0.10 * duration + approach_end = 0.38 * duration + return_start = 0.53 * duration + return_end = 0.81 * duration + + delta = q_end - q_start + if t < return_start: + s, sd, sdd = _smooth_transition(t, approach_start, approach_end) + if ( + approach_end < t < return_start + and contact_probe_fraction > 0.0 + and contact_probe_cycles > 0.0 + ): + probe_duration = return_start - approach_end + u = (t - approach_end) / probe_duration + angular_frequency_u = 2.0 * np.pi * contact_probe_cycles + envelope = np.sin(np.pi * u) ** 2 + envelope_du = np.pi * np.sin(2.0 * np.pi * u) + envelope_du2 = 2.0 * np.pi**2 * np.cos(2.0 * np.pi * u) + carrier = np.sin(angular_frequency_u * u) + carrier_du = angular_frequency_u * np.cos( + angular_frequency_u * u + ) + carrier_du2 = -(angular_frequency_u**2) * carrier + probe = contact_probe_fraction * envelope * carrier + probe_du = contact_probe_fraction * ( + envelope_du * carrier + envelope * carrier_du + ) + probe_du2 = contact_probe_fraction * ( + envelope_du2 * carrier + + 2.0 * envelope_du * carrier_du + + envelope * carrier_du2 + ) + s += probe + sd += probe_du / probe_duration + sdd += probe_du2 / probe_duration**2 + else: + sr, srd, srdd = _smooth_transition(t, return_start, return_end) + s, sd, sdd = 1.0 - sr, -srd, -srdd + return q_start + s * delta, sd * delta, sdd * delta + + +def _mass_matrix(model: pin.Model, data: pin.Data, q: np.ndarray) -> np.ndarray: + upper = np.asarray(pin.crba(model, data, q), dtype=float) + return np.triu(upper) + np.triu(upper, 1).T + + +def computed_torque( + model: pin.Model, + data: pin.Data, + q: np.ndarray, + qd: np.ndarray, + q_ref: np.ndarray, + qd_ref: np.ndarray, + qdd_ref: np.ndarray, + kp: float | np.ndarray, + kd: float | np.ndarray, + soft_limit_acceleration: np.ndarray | None = None, +) -> np.ndarray: + """Model-based tracking command for a fixed-base revolute chain.""" + position_error = pin.difference(model, q, q_ref) + kp_vector = np.broadcast_to(np.asarray(kp, dtype=float), (model.nv,)) + kd_vector = np.broadcast_to(np.asarray(kd, dtype=float), (model.nv,)) + acceleration_command = ( + qdd_ref + + kp_vector * position_error + + kd_vector * (qd_ref - qd) + ) + if soft_limit_acceleration is not None: + acceleration_command = ( + acceleration_command + + np.asarray(soft_limit_acceleration, dtype=float) + ) + nonlinear = pin.nonLinearEffects(model, data, q, qd) + return nonlinear + _mass_matrix(model, data, q) @ acceleration_command + + +def _clip_actuator_torque( + model: pin.Model, + torque: np.ndarray, + effort_fraction: float = 1.0, +) -> tuple[np.ndarray, bool]: + limits = effort_fraction * np.asarray(model.effortLimit, dtype=float) + finite_limits = np.where(np.isfinite(limits), limits, np.inf) + clipped = np.clip(np.asarray(torque, dtype=float), -finite_limits, finite_limits) + return clipped, bool(np.any(np.abs(clipped - torque) > 1e-12)) + + +def soft_limit_acceleration( + model: pin.Model, + q: np.ndarray, + qd: np.ndarray, + buffer: float, +) -> np.ndarray: + """Continuous acceleration-domain guard before the last-resort projection.""" + if buffer <= 0.0: + return np.zeros(model.nv, dtype=float) + lower_zone = np.asarray(model.lowerPositionLimit, dtype=float) + buffer + upper_zone = np.asarray(model.upperPositionLimit, dtype=float) - buffer + acceleration = np.zeros(model.nv, dtype=float) + below = q < lower_zone + above = q > upper_zone + acceleration[below] += 400.0 * (lower_zone[below] - q[below]) + acceleration[below] += 36.0 * np.maximum(-qd[below], 0.0) + acceleration[above] -= 400.0 * (q[above] - upper_zone[above]) + acceleration[above] -= 36.0 * np.maximum(qd[above], 0.0) + return acceleration + + +def integrate_state( + model: pin.Model, + q: np.ndarray, + qd: np.ndarray, + qdd: np.ndarray, + dt: float, + joint_names: Iterable[str], + margin: float, + velocity_limit_fraction: float, +) -> tuple[np.ndarray, np.ndarray, bool, bool]: + """Semi-implicit integration with URDF speed and position guards.""" + velocity_limit = ( + velocity_limit_fraction * np.asarray(model.velocityLimit, dtype=float) + ) + qd_unlimited = qd + qdd * dt + qd_next = np.clip(qd_unlimited, -velocity_limit, velocity_limit) + velocity_limited = bool( + np.any(np.abs(qd_next - qd_unlimited) > 1e-12) + ) + q_next = pin.integrate(model, q, qd_next * dt) + q_next, clipped = clip_configuration( + model, + q_next, + tuple(joint_names), + margin=margin, + ) + + if clipped: + # All models in this experiment have nq == nv == 7 scalar revolute + # joints. At a clipped boundary, cancel only outward velocity. + lower = np.asarray(model.lowerPositionLimit, dtype=float) + margin + upper = np.asarray(model.upperPositionLimit, dtype=float) - margin + at_lower = q_next <= lower + 1e-12 + at_upper = q_next >= upper - 1e-12 + qd_next = qd_next.copy() + qd_next[at_lower & (qd_next < 0.0)] = 0.0 + qd_next[at_upper & (qd_next > 0.0)] = 0.0 + return q_next, qd_next, clipped, velocity_limited + + +def frame_kinematics( + model: pin.Model, + data: pin.Data, + q: np.ndarray, + qd: np.ndarray, + frame_id: int, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + pin.forwardKinematics(model, data, q, qd) + pin.updateFramePlacements(model, data) + jacobian = pin.computeFrameJacobian( + model, + data, + q, + frame_id, + pin.ReferenceFrame.LOCAL_WORLD_ALIGNED, + ) + position = np.asarray(data.oMf[frame_id].translation, dtype=float).copy() + linear_velocity = np.asarray(jacobian[:3] @ qd, dtype=float) + return position, linear_velocity, np.asarray(jacobian, dtype=float) + + +def make_wall( + config: SimulationConfig, + models: TeleoperationModels, + mapper: SEWMapper, +) -> tuple[Wall, dict[str, Any], np.ndarray]: + q_start, q_end = master_endpoint_configurations() + q_slave_start, start_debug = mapper.retarget(q_start) + q_slave_end, end_debug = mapper.retarget(q_end, q_s_init=q_slave_start) + if not start_debug["success"] or not end_debug["success"]: + raise RuntimeError( + "Cannot place the wall because endpoint retargeting failed: " + f"start={start_debug['events']}, end={end_debug['events']}" + ) + + frame_id = require_frame(models.slave, config.slave_contact_frame) + data = models.slave.createData() + p_start, _, _ = frame_kinematics( + models.slave, + data, + q_slave_start, + np.zeros(models.slave.nv), + frame_id, + ) + p_end, _, _ = frame_kinematics( + models.slave, + data, + q_slave_end, + np.zeros(models.slave.nv), + frame_id, + ) + travel = p_end - p_start + travel_norm = float(np.linalg.norm(travel)) + if travel_norm < 1e-6: + raise RuntimeError("Retargeted TCP motion is too small to define a wall") + normal = travel / travel_norm + point = p_start + config.wall_fraction * travel + wall = Wall( + point=point, + normal=normal, + stiffness=config.wall_stiffness, + damping=config.wall_damping, + force_limit=config.wall_force_limit, + transition_depth=config.wall_transition_depth, + ) + metadata = { + "start_tcp_world_m": p_start.tolist(), + "end_tcp_world_m": p_end.tolist(), + "free_space_travel_m": travel_norm, + "point_world_m": point.tolist(), + "normal_world": normal.tolist(), + "fraction_of_free_space_travel": config.wall_fraction, + "stiffness_N_per_m": config.wall_stiffness, + "damping_Ns_per_m": config.wall_damping, + "force_limit_N": config.wall_force_limit, + "transition_depth_m": config.wall_transition_depth, + } + return wall, metadata, q_slave_start + + +def make_renderer( + model: pin.Model, + config: SimulationConfig, +) -> HapticRenderer: + return HapticRenderer( + model, + chest_frame_name=MASTER_FRAMES["base"], + ee_frame_name=MASTER_FRAMES["ee"], + feedback_strength=config.feedback_strength, + E_init=config.energy_min, + E_max=config.energy_max, + alpha_floor=0.0, + alpha_ceil=1.0, + E0=config.energy_initial, + torque_limit=np.asarray(config.haptic_torque_limits, dtype=float), + torque_rate_limit=np.asarray(config.haptic_rate_limits, dtype=float), + tau_filter_alpha=config.haptic_filter_alpha, + ) + + +def _delay_line( + size: int, + sample_shape: tuple[int, ...], +) -> deque[np.ndarray]: + return deque( + [np.zeros(sample_shape, dtype=float) for _ in range(size)], + maxlen=size, + ) + + +def _push_delayed( + queue: deque[np.ndarray], + sample: np.ndarray, +) -> np.ndarray: + if queue.maxlen == 0: + return np.asarray(sample, dtype=float).copy() + delayed = queue.popleft() + queue.append(np.asarray(sample, dtype=float).copy()) + return delayed + + +def _rms(values: np.ndarray) -> float: + values = np.asarray(values, dtype=float) + if values.size == 0: + return 0.0 + return float(np.sqrt(np.mean(np.square(values)))) + + +def _finite_or_none(value: float) -> float | None: + return float(value) if np.isfinite(value) else None + + +def simulate_scenario( + scenario: Scenario, + config: SimulationConfig, + models: TeleoperationModels, + wall: Wall, + q_slave_start: np.ndarray, +) -> ScenarioResult: + """Run one closed-loop case with an independent but identically seeded state.""" + config.validate() + rng = np.random.default_rng(config.seed) + mapper = build_mapper(models) + renderer = make_renderer(models.master, config) + estimator = InteractionEstimator( + models.slave, + chest_frame_name=SLAVE_FRAMES["base"], + ee_frame_name=config.slave_contact_frame, + lambda_damp=1e-3, + wrench_solver=ScaledDLSSolver( + config.wrench_characteristic_length_m, + config.wrench_scaled_damping, + ), + ) + + sensor_bias = np.asarray(config.sensor_bias, dtype=float) + calibration = sensor_bias + rng.normal( + 0.0, + config.sensor_noise_std, + size=(config.bias_calibration_samples, models.slave.nv), + ) + estimator.calibrate_bias(calibration) + + q_start, q_end = master_endpoint_configurations() + q_m = q_start.copy() + qd_m = np.zeros(models.master.nv, dtype=float) + q_s = q_slave_start.copy() + qd_s = np.zeros(models.slave.nv, dtype=float) + + q_s_ref, A, map_debug = mapper.retarget_with_differential( + q_m, + q_s_init=q_s, + fd_step=config.differential_step, + ) + if not map_debug["success"] or not map_debug["differential_valid"]: + raise RuntimeError( + "Initial retargeting differential is invalid: " + f"pose={map_debug['events']}, " + f"A={map_debug['differential']['events']}" + ) + differential_feedback_valid = True + qd_s_ref_hold = np.zeros(models.slave.nv, dtype=float) + map_registry = MapRegistry(capacity=2048) + map_registry.add( + MapSnapshot( + map_id=0, + source_index=0, + source_time=0.0, + differential=A, + valid=True, + ) + ) + active_forward_map_id = 0 + next_map_id = 1 + forward_seq = 0 + + master_data_control = models.master.createData() + master_data_dynamics = models.master.createData() + slave_data_control = models.slave.createData() + slave_data_dynamics = models.slave.createData() + slave_data_contact = models.slave.createData() + slave_tcp_id = require_frame(models.slave, config.slave_contact_frame) + + step_count = int(round(config.duration / config.dt)) + mapping_stride = max( + 1, + int(round(1.0 / (config.mapping_hz * config.dt))), + ) + forward_trace = generate_network_trace( + step_count // mapping_stride + 2, + base_delay_s=config.forward_delay_s, + jitter_s=config.forward_jitter_s, + loss_probability=config.forward_packet_loss, + seed=config.seed + 1001, + ) + return_trace = generate_network_trace( + step_count, + base_delay_s=config.feedback_delay_s, + jitter_s=config.return_jitter_s, + loss_probability=config.return_packet_loss, + seed=config.seed + 1002, + ) + forward_channel: DeterministicChannel[ForwardPacket] = ( + DeterministicChannel(forward_trace) + ) + return_channel: DeterministicChannel[ReturnPacket] = ( + DeterministicChannel(return_trace) + ) + forward_receiver: PacketReceiver[ForwardPacket] = PacketReceiver( + config.forward_timeout_s + ) + return_receiver: PacketReceiver[ReturnPacket] = PacketReceiver( + config.return_timeout_s + ) + forward_channel.send( + ForwardPacket( + seq=forward_seq, + source_index=0, + source_time=0.0, + map_id=0, + q_slave_ref=q_s_ref, + qd_slave_ctrl=qd_s_ref_hold, + ), + now=0.0, + ) + forward_seq += 1 + + scalar_keys = ( + "time", + "sample_index", + "dt", + "missed_deadline", + "contact_force_norm", + "penetration", + "force_estimation_error_norm", + "moment_estimation_error_norm", + "raw_master_power", + "a_defined_slave_power", + "a_port_identity_error", + "source_slave_power", + "actual_power_mismatch_abs", + "actual_slave_environment_power", + "candidate_power", + "applied_power", + "rho", + "energy_before", + "tank_energy", + "energy_preclip", + "shadow_energy", + "popc_damping_gain", + "map_id", + "source_map_id", + "return_source_index", + "return_packet_age", + "forward_packet_state", + "return_packet_state", + "return_packet_active", + "master_tracking_error", + "slave_tracking_error", + "feedback_torque_norm", + "mapped_torque_norm", + ) + vector_keys = ( + "q_master", + "qd_master", + "q_master_ref", + "q_slave", + "qd_slave", + "q_slave_ref", + "tau_slave_external", + "tau_slave_estimated", + "tau_slave_residual_source", + "tau_slave_matched_wrench", + "qd_slave_source", + "tau_master_mapped", + "tau_master_candidate", + "tau_master_applied", + "tau_master_accepted", + "wrench_external", + "wrench_estimated", + "wrench_feedback_source", + "map_differential", + "tcp_position", + ) + log_lists: dict[str, list[np.ndarray | float]] = { + key: [] for key in scalar_keys + vector_keys + } + + map_update_count = 0 + map_pose_success_count = 0 + differential_valid_count = 0 + differential_fallback_count = 0 + mapping_runtimes_ms: list[float] = [] + master_limit_events = 0 + slave_limit_events = 0 + master_velocity_limit_events = 0 + slave_velocity_limit_events = 0 + master_acceleration_limit_events = 0 + slave_acceleration_limit_events = 0 + master_torque_saturation_events = 0 + slave_torque_saturation_events = 0 + haptic_rate_limit_events = 0 + haptic_torque_saturation_events = 0 + energy_identity_errors: list[float] = [] + shadow_energy = config.energy_initial + contact_steps = 0 + forward_packets_accepted = 0 + forward_packets_rejected = 0 + return_packets_accepted = 0 + return_packets_rejected = 0 + forward_timeout_steps = 0 + return_timeout_steps = 0 + popc = TimeDomainPOPC( + initial_energy=config.energy_initial, + minimum_energy=config.energy_min, + maximum_energy=config.energy_max, + ) + + run_start = time.perf_counter() + for step in range(step_count): + t = step * config.dt + q_m_ref, qd_m_ref, qdd_m_ref = master_reference( + t, + config.duration, + q_start, + q_end, + config.contact_probe_fraction, + config.contact_probe_cycles, + ) + + if step > 0 and step % mapping_stride == 0: + tic = time.perf_counter() + q_s_candidate, A_candidate, update_debug = ( + mapper.retarget_with_differential( + q_m, + q_s_init=q_s_ref, + fd_step=config.differential_step, + ) + ) + mapping_runtimes_ms.append( + 1e3 * (time.perf_counter() - tic) + ) + map_update_count += 1 + pose_valid = bool(update_debug["success"]) + differential_valid = bool(update_debug["differential_valid"]) + if pose_valid: + map_pose_success_count += 1 + if update_debug["differential_valid"]: + A = A_candidate + differential_valid_count += 1 + qd_s_candidate = np.clip( + A @ qd_m, + -config.velocity_limit_fraction + * np.asarray(models.slave.velocityLimit, dtype=float), + config.velocity_limit_fraction + * np.asarray(models.slave.velocityLimit, dtype=float), + ) + else: + differential_fallback_count += 1 + qd_s_candidate = np.zeros(models.slave.nv, dtype=float) + + snapshot_differential = ( + A_candidate + if np.all(np.isfinite(A_candidate)) + else np.zeros_like(A) + ) + map_registry.add( + MapSnapshot( + map_id=next_map_id, + source_index=step, + source_time=t, + differential=snapshot_differential, + valid=differential_valid, + reason_code=0 if differential_valid else 1, + ) + ) + forward_channel.send( + ForwardPacket( + seq=forward_seq, + source_index=step, + source_time=t, + map_id=next_map_id, + q_slave_ref=( + q_s_candidate if pose_valid else q_s_ref + ), + qd_slave_ctrl=qd_s_candidate, + valid=pose_valid, + ), + now=t, + ) + next_map_id += 1 + forward_seq += 1 + + for delivery in forward_channel.poll(t): + reception = forward_receiver.accept(delivery) + if reception.accepted: + forward_packets_accepted += 1 + else: + forward_packets_rejected += 1 + held_forward = forward_receiver.sample(t) + if held_forward.packet is not None: + q_s_ref = held_forward.packet.q_slave_ref.copy() + qd_s_ref_hold = held_forward.packet.qd_slave_ctrl.copy() + active_forward_map_id = held_forward.packet.map_id + elif held_forward.state is PacketState.TIMED_OUT: + # The position target is held while commanded velocity goes to zero. + qd_s_ref_hold.fill(0.0) + forward_timeout_steps += 1 + qd_s_ref = qd_s_ref_hold + try: + differential_feedback_valid = map_registry.get( + active_forward_map_id + ).valid + except KeyError: + differential_feedback_valid = False + + tcp_position, tcp_linear_velocity, J_slave_world = frame_kinematics( + models.slave, + slave_data_contact, + q_s, + qd_s, + slave_tcp_id, + ) + wrench_external, penetration = wall.wrench( + tcp_position, + tcp_linear_velocity, + ) + if penetration > 0.0: + contact_steps += 1 + tau_slave_external = J_slave_world.T @ wrench_external + + tau_slave_control = computed_torque( + models.slave, + slave_data_control, + q_s, + qd_s, + q_s_ref, + qd_s_ref, + np.zeros(models.slave.nv), + config.slave_kp, + config.slave_kd, + soft_limit_acceleration( + models.slave, + q_s, + qd_s, + config.soft_limit_buffer, + ), + ) + tau_slave_control, torque_clipped = _clip_actuator_torque( + models.slave, + tau_slave_control, + config.slave_tracking_effort_fraction, + ) + slave_torque_saturation_events += int(torque_clipped) + qdd_s_raw = pin.aba( + models.slave, + slave_data_dynamics, + q_s, + qd_s, + tau_slave_control + tau_slave_external, + ) + slave_acceleration_limits = np.asarray( + config.slave_acceleration_limits, + dtype=float, + ) + qdd_s = np.clip( + qdd_s_raw, + -slave_acceleration_limits, + slave_acceleration_limits, + ) + slave_acceleration_limit_events += int( + np.any(np.abs(qdd_s_raw) > slave_acceleration_limits) + ) + + # This is an explicitly simulated load-side equivalent measurement. + # It satisfies the estimator's declared residual convention: + # tau_int = tau_meas - (M qdd + h) - calibrated_bias. + tau_slave_model = estimator._tau_model(q_s, qd_s, qdd_s) + measurement_noise = rng.normal( + 0.0, + config.sensor_noise_std, + models.slave.nv, + ) + tau_slave_measured = ( + tau_slave_model + + tau_slave_external + + sensor_bias + + measurement_noise + ) + tau_slave_estimated, wrench_estimated, J_slave_chest = estimator.estimate( + q_s, + qd_s, + qdd_s, + tau_slave_measured, + ) + tau_slave_matched_wrench = J_slave_chest.T @ wrench_estimated + return_channel.send( + ReturnPacket( + seq=step, + source_index=step, + source_time=t, + echoed_map_id=active_forward_map_id, + residual=tau_slave_estimated, + wrench=wrench_estimated, + js_t_wrench=tau_slave_matched_wrench, + qd_slave_actual=qd_s, + ), + now=t, + ) + for delivery in return_channel.poll(t): + reception = return_receiver.accept(delivery) + if reception.accepted: + return_packets_accepted += 1 + else: + return_packets_rejected += 1 + held_return = return_receiver.sample(t) + delayed_packet = held_return.packet + return_packet_active = delayed_packet is not None + if held_return.state is PacketState.TIMED_OUT: + return_timeout_steps += 1 + + master_jacobian = renderer.CJ_master.chest_jacobian(q_m, qd_m) + if delayed_packet is None: + tau_master_mapped = np.zeros(models.master.nv, dtype=float) + delayed_tau_slave = np.zeros(models.slave.nv, dtype=float) + delayed_wrench = np.zeros(6, dtype=float) + delayed_tau_matched = np.zeros(models.slave.nv, dtype=float) + tau_slave_source_for_mapping = np.zeros( + models.slave.nv, dtype=float + ) + qd_slave_source = np.zeros(models.slave.nv, dtype=float) + selected_map_id = -1 + source_map_id = -1 + return_source_index = -1 + return_packet_age = math.nan + else: + feedback = map_return_feedback( + kind=MappingKind(scenario.mapping), + packet=delayed_packet, + master_jacobian=master_jacobian, + maps=map_registry, + map_policy=MapPolicy(scenario.map_policy), + ) + tau_master_mapped = feedback.tau_master_raw + delayed_tau_slave = delayed_packet.residual.copy() + delayed_wrench = delayed_packet.wrench.copy() + delayed_tau_matched = delayed_packet.js_t_wrench.copy() + tau_slave_source_for_mapping = feedback.tau_slave_source.copy() + qd_slave_source = delayed_packet.qd_slave_actual.copy() + selected_map_id = ( + -1 + if feedback.selected_map_id is None + else feedback.selected_map_id + ) + source_map_id = delayed_packet.echoed_map_id + return_source_index = delayed_packet.source_index + return_packet_age = max(0.0, t - delayed_packet.source_time) + if not feedback.valid: + tau_master_mapped = np.zeros(models.master.nv, dtype=float) + + popc_damping_gain = 0.0 + if scenario.supervisor == "tank": + tau_master_applied, rho = renderer.render_mapped_reaction( + tau_master_mapped, + qd_m, + config.dt, + supervise_energy=True, + ) + elif scenario.supervisor == "bypass": + tau_master_applied, rho = renderer.render_mapped_reaction( + tau_master_mapped, + qd_m, + config.dt, + supervise_energy=False, + ) + elif scenario.supervisor == "popc": + tau_master_candidate, candidate_valid = ( + renderer.shape_mapped_reaction_candidate( + tau_master_mapped, + qd_m, + config.dt, + ) + ) + if candidate_valid: + tau_master_applied, popc_diagnostics = popc.apply( + tau_master_candidate, + qd_m, + config.dt, + ) + else: + tau_master_applied = np.zeros_like(tau_master_candidate) + _, popc_diagnostics = popc.apply( + tau_master_applied, + np.zeros_like(qd_m), + config.dt, + ) + renderer.commit_applied(tau_master_applied) + rho = math.nan + popc_damping_gain = popc_diagnostics.damping_gain + else: + raise ValueError(f"Unknown supervisor: {scenario.supervisor}") + haptic_rate_limit_events += int(renderer.last_rate_limit_active) + haptic_torque_saturation_events += int( + renderer.last_torque_saturation_active + ) + if scenario.supervisor == "tank": + diagnostics = renderer.tank.last_diagnostics + assert diagnostics is not None + tau_master_candidate = diagnostics.tau_candidate.copy() + reconstructed_energy_preclip = float( + diagnostics.E_before - diagnostics.power * config.dt + ) + energy_identity_errors.append( + abs(diagnostics.E_preclip - reconstructed_energy_preclip) + ) + tank_energy = diagnostics.E_after + energy_before = diagnostics.E_before + energy_preclip = diagnostics.E_preclip + candidate_power = diagnostics.candidate_power + elif scenario.supervisor == "popc": + tank_energy = popc_diagnostics.observer_after + energy_before = popc_diagnostics.observer_before + energy_preclip = popc_diagnostics.observer_preclip + candidate_power = popc_diagnostics.candidate_power + else: + tau_master_candidate = tau_master_applied.copy() + tank_energy = math.nan + energy_before = math.nan + energy_preclip = math.nan + candidate_power = float(np.dot(tau_master_candidate, qd_m)) + + applied_power = float(np.dot(tau_master_applied, qd_m)) + tau_master_accepted = tau_master_applied.copy() + # Counterfactual storage obeys the same upper capacity but deliberately + # has no lower projection. Falling below E_min demonstrates the exact + # sample at which unsupervised output violates the configured budget. + shadow_energy = min( + config.energy_max, + shadow_energy - candidate_power * config.dt, + ) + + tau_human = computed_torque( + models.master, + master_data_control, + q_m, + qd_m, + q_m_ref, + qd_m_ref, + qdd_m_ref, + config.master_kp, + config.master_kd, + soft_limit_acceleration( + models.master, + q_m, + qd_m, + config.soft_limit_buffer, + ), + ) + tau_human, master_torque_clipped = _clip_actuator_torque( + models.master, + tau_human, + config.master_tracking_effort_fraction, + ) + master_torque_saturation_events += int(master_torque_clipped) + qdd_m_raw = pin.aba( + models.master, + master_data_dynamics, + q_m, + qd_m, + tau_human + tau_master_applied, + ) + master_acceleration_limits = np.asarray( + config.master_acceleration_limits, + dtype=float, + ) + qdd_m = np.clip( + qdd_m_raw, + -master_acceleration_limits, + master_acceleration_limits, + ) + master_acceleration_limit_events += int( + np.any(np.abs(qdd_m_raw) > master_acceleration_limits) + ) + + try: + differential_for_power = ( + map_registry.get(selected_map_id).differential + if selected_map_id >= 0 + else map_registry.latest.differential + ) + except KeyError: + differential_for_power = np.zeros( + (models.slave.nv, models.master.nv), dtype=float + ) + reference_slave_velocity = differential_for_power @ qd_m + raw_master_power = float(np.dot(tau_master_mapped, qd_m)) + a_defined_slave_power = float( + np.dot(tau_slave_source_for_mapping, reference_slave_velocity) + ) + a_port_identity_error = abs( + raw_master_power - a_defined_slave_power + ) + source_slave_power = float( + np.dot(tau_slave_source_for_mapping, qd_slave_source) + ) + actual_power_mismatch_abs = abs( + raw_master_power - source_slave_power + ) + + scalar_values = { + "time": t, + "sample_index": step, + "dt": config.dt, + "missed_deadline": 0, + "contact_force_norm": float(np.linalg.norm(wrench_external[:3])), + "penetration": penetration, + "force_estimation_error_norm": float( + np.linalg.norm(wrench_estimated[:3] - wrench_external[:3]) + ), + "moment_estimation_error_norm": float( + np.linalg.norm(wrench_estimated[3:] - wrench_external[3:]) + ), + "raw_master_power": raw_master_power, + "a_defined_slave_power": a_defined_slave_power, + "a_port_identity_error": a_port_identity_error, + "source_slave_power": source_slave_power, + "actual_power_mismatch_abs": actual_power_mismatch_abs, + "actual_slave_environment_power": float( + np.dot(tau_slave_external, qd_s) + ), + "candidate_power": candidate_power, + "applied_power": applied_power, + "rho": rho, + "energy_before": energy_before, + "tank_energy": tank_energy, + "energy_preclip": energy_preclip, + "shadow_energy": shadow_energy, + "popc_damping_gain": popc_damping_gain, + "map_id": selected_map_id, + "source_map_id": source_map_id, + "return_source_index": return_source_index, + "return_packet_age": return_packet_age, + "forward_packet_state": int(held_forward.state), + "return_packet_state": int(held_return.state), + "return_packet_active": int(return_packet_active), + "master_tracking_error": float( + np.linalg.norm(pin.difference(models.master, q_m, q_m_ref)) + ), + "slave_tracking_error": float( + np.linalg.norm(pin.difference(models.slave, q_s, q_s_ref)) + ), + "feedback_torque_norm": float(np.linalg.norm(tau_master_applied)), + "mapped_torque_norm": float(np.linalg.norm(tau_master_mapped)), + } + vector_values = { + "q_master": q_m.copy(), + "qd_master": qd_m.copy(), + "q_master_ref": q_m_ref.copy(), + "q_slave": q_s.copy(), + "qd_slave": qd_s.copy(), + "q_slave_ref": q_s_ref.copy(), + "tau_slave_external": tau_slave_external.copy(), + "tau_slave_estimated": tau_slave_estimated.copy(), + "tau_slave_residual_source": delayed_tau_slave.copy(), + "tau_slave_matched_wrench": delayed_tau_matched.copy(), + "qd_slave_source": qd_slave_source.copy(), + "tau_master_mapped": tau_master_mapped.copy(), + "tau_master_candidate": tau_master_candidate.copy(), + "tau_master_applied": tau_master_applied.copy(), + "tau_master_accepted": tau_master_accepted.copy(), + "wrench_external": wrench_external.copy(), + "wrench_estimated": wrench_estimated.copy(), + "wrench_feedback_source": delayed_wrench.copy(), + "map_differential": differential_for_power.reshape(-1).copy(), + "tcp_position": tcp_position.copy(), + } + for key, value in scalar_values.items(): + log_lists[key].append(float(value)) + for key, value in vector_values.items(): + log_lists[key].append(value) + + q_m, qd_m, master_clipped, master_velocity_limited = integrate_state( + models.master, + q_m, + qd_m, + qdd_m, + config.dt, + MASTER_JOINT_NAMES, + config.joint_limit_margin, + config.velocity_limit_fraction, + ) + q_s, qd_s, slave_clipped, slave_velocity_limited = integrate_state( + models.slave, + q_s, + qd_s, + qdd_s, + config.dt, + SLAVE_JOINT_NAMES, + config.joint_limit_margin, + config.velocity_limit_fraction, + ) + master_limit_events += int(master_clipped) + slave_limit_events += int(slave_clipped) + master_velocity_limit_events += int(master_velocity_limited) + slave_velocity_limit_events += int(slave_velocity_limited) + + if not ( + np.all(np.isfinite(q_m)) + and np.all(np.isfinite(qd_m)) + and np.all(np.isfinite(q_s)) + and np.all(np.isfinite(qd_s)) + ): + raise FloatingPointError( + f"{scenario.key}: non-finite state at t={t:.6f} s" + ) + + runtime = time.perf_counter() - run_start + logs = { + key: np.asarray(values, dtype=float) for key, values in log_lists.items() + } + contact_mask = logs["penetration"] > 0.0 + force_active_mask = logs["contact_force_norm"] > 1e-6 + power_scale = _rms(logs["a_defined_slave_power"][contact_mask]) + projection_mask = ( + logs["rho"] < (1.0 - 1e-12) + if scenario.supervisor == "tank" + else logs["popc_damping_gain"] > 0.0 + ) + positive_power = np.maximum(logs["applied_power"], 0.0) + absorbed_power = np.maximum(-logs["applied_power"], 0.0) + actual_mismatch_numerator = float( + np.sum(logs["actual_power_mismatch_abs"]) * config.dt + ) + actual_mismatch_denominator = float( + 0.5 + * np.sum( + np.abs(logs["raw_master_power"]) + + np.abs(logs["source_slave_power"]) + ) + * config.dt + + 1e-12 + ) + finite_rho = logs["rho"][np.isfinite(logs["rho"])] + + metrics = { + "completed": True, + "finite_state": True, + "simulated_duration_s": config.duration, + "wall_contact_fraction": float(np.mean(contact_mask)), + "wall_contact_duration_s": float(np.sum(contact_mask) * config.dt), + "wall_force_active_fraction": float(np.mean(force_active_mask)), + "peak_contact_force_N": float(np.max(logs["contact_force_norm"])), + "max_penetration_mm": float(1e3 * np.max(logs["penetration"])), + "force_estimation_rmse_N": _rms( + logs["force_estimation_error_norm"] + ), + "moment_estimation_rmse_Nm": _rms( + logs["moment_estimation_error_norm"] + ), + "master_tracking_rmse_rad": _rms(logs["master_tracking_error"]), + "slave_tracking_rmse_rad": _rms(logs["slave_tracking_error"]), + "feedback_torque_rms_Nm": _rms(logs["feedback_torque_norm"]), + "feedback_torque_peak_Nm": float( + np.max(logs["feedback_torque_norm"]) + ), + "a_port_identity_error_rms_W": _rms( + logs["a_port_identity_error"] + ), + "a_port_identity_error_max_W": float( + np.max(logs["a_port_identity_error"]) + ), + "a_port_identity_relative_rms": float( + _rms(logs["a_port_identity_error"][contact_mask]) + / max(power_scale, 1e-12) + ), + "actual_power_mismatch_normalized": ( + actual_mismatch_numerator / actual_mismatch_denominator + ), + "slave_environment_net_work_J": float( + np.sum(logs["actual_slave_environment_power"]) * config.dt + ), + "positive_energy_delivered_J": float( + np.sum(positive_power) * config.dt + ), + "energy_absorbed_J": float(np.sum(absorbed_power) * config.dt), + "supervisor_intervention_fraction": float(np.mean(projection_mask)), + "energy_projection_fraction": ( + float(np.mean(projection_mask)) + if scenario.supervisor == "tank" + else None + ), + "energy_projection_contact_fraction": float( + np.mean(projection_mask[contact_mask]) + if scenario.supervisor == "tank" and np.any(contact_mask) + else 0.0 + ), + "rho_min": ( + float(np.min(finite_rho)) if finite_rho.size else None + ), + "tank_energy_min_J": _finite_or_none( + float(np.nanmin(logs["tank_energy"])) + if scenario.supervisor in ("tank", "popc") + else math.nan + ), + "tank_energy_final_J": _finite_or_none( + float(logs["tank_energy"][-1]) + if scenario.supervisor in ("tank", "popc") + else math.nan + ), + "shadow_energy_min_J": float(np.min(logs["shadow_energy"])), + "shadow_energy_floor_violation_J": float( + max(0.0, config.energy_min - np.min(logs["shadow_energy"])) + ), + "energy_accounting_max_error_J": ( + float(max(energy_identity_errors, default=0.0)) + if scenario.supervisor == "tank" + else None + ), + "mapping_updates": map_update_count, + "mapping_pose_success_rate": float( + map_pose_success_count / max(map_update_count, 1) + ), + "differential_valid_rate": float( + differential_valid_count / max(map_update_count, 1) + ), + "differential_fallback_count": differential_fallback_count, + "mapping_runtime_median_ms": float( + np.median(mapping_runtimes_ms) if mapping_runtimes_ms else 0.0 + ), + "mapping_runtime_p95_ms": float( + np.percentile(mapping_runtimes_ms, 95.0) + if mapping_runtimes_ms + else 0.0 + ), + "mapping_runtime_max_ms": float( + max(mapping_runtimes_ms, default=0.0) + ), + "master_joint_limit_events": master_limit_events, + "slave_joint_limit_events": slave_limit_events, + "master_velocity_limit_events": master_velocity_limit_events, + "slave_velocity_limit_events": slave_velocity_limit_events, + "master_acceleration_limit_events": master_acceleration_limit_events, + "slave_acceleration_limit_events": slave_acceleration_limit_events, + "master_torque_saturation_events": master_torque_saturation_events, + "slave_torque_saturation_events": slave_torque_saturation_events, + "haptic_rate_limit_events": haptic_rate_limit_events, + "haptic_torque_saturation_events": haptic_torque_saturation_events, + "forward_packets_accepted": forward_packets_accepted, + "forward_packets_rejected": forward_packets_rejected, + "return_packets_accepted": return_packets_accepted, + "return_packets_rejected": return_packets_rejected, + "forward_timeout_steps": forward_timeout_steps, + "return_timeout_steps": return_timeout_steps, + "wall_contact_steps": contact_steps, + "wall_time_s": runtime, + } + return ScenarioResult(scenario=scenario, metrics=metrics, logs=logs) + + +def _write_npz(path: Path, logs: dict[str, np.ndarray]) -> None: + np.savez_compressed(path, **logs) + + +def _write_csv(path: Path, logs: dict[str, np.ndarray]) -> None: + scalar_keys = [ + key for key, value in logs.items() if value.ndim == 1 + ] + vector_keys = [ + key for key, value in logs.items() if value.ndim == 2 + ] + fieldnames = scalar_keys + [ + f"{key}_{index}" + for key in vector_keys + for index in range(logs[key].shape[1]) + ] + with path.open("w", encoding="utf-8", newline="") as stream: + writer = csv.DictWriter(stream, fieldnames=fieldnames) + writer.writeheader() + row_count = logs["time"].shape[0] + for row_index in range(row_count): + row: dict[str, float] = { + key: float(logs[key][row_index]) for key in scalar_keys + } + for key in vector_keys: + for column_index, value in enumerate(logs[key][row_index]): + row[f"{key}_{column_index}"] = float(value) + writer.writerow(row) + + +def make_comparison_plot( + results: list[ScenarioResult], + config: SimulationConfig, + output_path: Path, +) -> None: + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + colors = { + "proposed_energy": "#0072B2", + "direct_energy": "#D55E00", + "proposed_no_energy": "#009E73", + "matched_wrench_energy": "#CC79A7", + "proposed_popc": "#E69F00", + } + labels = { + "proposed_energy": r"$A^\mathsf{T}$ + final energy", + "direct_energy": r"$J_m^\mathsf{T}F$ + final energy", + "proposed_no_energy": r"$A^\mathsf{T}$, no energy projection", + "matched_wrench_energy": ( + r"$A_\mathrm{src}^\mathsf{T}J_s^\mathsf{T}F$ + final energy" + ), + "proposed_popc": r"$A^\mathsf{T}$ + time-domain PO/PC", + } + fig, axes = plt.subplots(3, 2, figsize=(13.0, 10.0), sharex=True) + + for result in results: + key = result.scenario.key + color = colors[key] + label = labels[key] + time_axis = result.logs["time"] + axes[0, 0].plot( + time_axis, + result.logs["contact_force_norm"], + color=color, + label=label, + ) + axes[0, 1].semilogy( + time_axis, + np.maximum(result.logs["a_port_identity_error"], 1e-14), + color=color, + label=label, + ) + axes[1, 0].plot( + time_axis, + result.logs["feedback_torque_norm"], + color=color, + label=label, + ) + supervisor_trace = ( + (result.logs["popc_damping_gain"] > 0.0).astype(float) + if result.scenario.supervisor == "popc" + else result.logs["rho"] + ) + axes[1, 1].plot( + time_axis, + supervisor_trace, + color=color, + label=label, + ) + if result.scenario.supervisor in ("tank", "popc"): + axes[2, 0].plot( + time_axis, + result.logs["tank_energy"], + color=color, + label=label, + ) + else: + axes[2, 0].plot( + time_axis, + result.logs["shadow_energy"], + color=color, + linestyle="--", + label="unsupervised counterfactual reserve", + ) + axes[2, 1].plot( + time_axis, + 1e3 * result.logs["penetration"], + color=color, + label=label, + ) + + axes[0, 0].set_ylabel("contact force [N]") + axes[0, 0].set_title("Closed-loop wall interaction") + axes[0, 1].set_ylabel("A-port identity error [W]") + axes[0, 1].set_title( + "Raw A-port power consistency (plot floor $10^{-14}$ W)" + ) + axes[1, 0].set_ylabel(r"$\|\tau_{m,app}\|$ [N m]") + axes[1, 0].set_title("Applied master haptic torque") + axes[1, 1].set_ylabel(r"tank $\rho$ / PO-PC active") + axes[1, 1].set_ylim(-0.04, 1.04) + axes[1, 1].set_title("Final-port supervisor activity") + axes[2, 0].axhline( + config.energy_min, + color="black", + linestyle="--", + linewidth=1.0, + label="configured E_min", + ) + axes[2, 0].set_ylabel("tank energy [J]") + axes[2, 0].set_title("Accounted / counterfactual energy reserve") + axes[2, 1].set_ylabel("wall penetration [mm]") + axes[2, 1].set_title("Slave TCP penetration") + for axis in axes[-1, :]: + axis.set_xlabel("time [s]") + for axis in axes.flat: + axis.grid(True, alpha=0.3) + axes[0, 0].legend(loc="best", fontsize=8) + axes[2, 0].legend(loc="best", fontsize=8) + fig.suptitle( + "Simulation only — master_7dof.urdf / real_slave_7dof.urdf", + fontsize=13, + ) + fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.97)) + fig.savefig(output_path, dpi=180) + plt.close(fig) + + +def _json_ready(value: Any) -> Any: + if isinstance(value, np.ndarray): + return value.tolist() + if isinstance(value, (np.floating, np.integer)): + return value.item() + if isinstance(value, dict): + return {key: _json_ready(item) for key, item in value.items()} + if isinstance(value, (tuple, list)): + return [_json_ready(item) for item in value] + return value + + +def run_comparison( + config: SimulationConfig, + output_dir: Path, + scenarios: tuple[Scenario, ...] = SCENARIOS, +) -> dict[str, Any]: + config.validate() + output_dir.mkdir(parents=True, exist_ok=True) + models = load_models(add_simulated_tcp=True) + wall_mapper = build_mapper(models) + wall, wall_metadata, q_slave_start = make_wall( + config, + models, + wall_mapper, + ) + + results: list[ScenarioResult] = [] + for scenario in scenarios: + print(f"[simulation] running {scenario.key}: {scenario.description}") + result = simulate_scenario( + scenario, + config, + models, + wall, + q_slave_start, + ) + results.append(result) + _write_npz(output_dir / f"{scenario.key}.npz", result.logs) + _write_csv(output_dir / f"{scenario.key}.csv", result.logs) + + make_comparison_plot( + results, + config, + output_dir / "closed_loop_comparison.png", + ) + summary = { + "evidence_scope": ( + "Rigid-body closed-loop simulation only; no prototype or human " + "subject result is claimed." + ), + "metric_definitions_and_limits": { + "a_port_identity": ( + "Compares tau_mapped^T qd_m with delayed_tau_s^T " + "(A qd_m). It is an algebraic implementation-consistency " + "metric, not equality of the actual master/slave port powers." + ), + "contact": ( + "wall_contact_fraction/duration/steps use penetration > 0; " + "wall_force_active_fraction separately uses force norm > 1e-6." + ), + "energy_stress_protocol": ( + "The tank starts at E_min with only " + f"{config.energy_max - config.energy_min:.6g} J capacity above " + "the floor. This deliberately tight budget exercises the " + "projection and is not a hardware tuning recommendation." + ), + "shadow_energy": ( + "For no-energy, this reconstructs that scenario's actual " + "unprojected candidate sequence. For supervised scenarios, " + "it is a stepwise witness on the supervised closed-loop " + "candidate sequence, not a second counterfactual simulation." + ), + "plotting_floor_W": 1e-14, + "estimator_scope": ( + "Torque bias/noise and load-side measurements are synthetic, " + "and plant/estimator share one URDF model; hardware robustness " + "is therefore not established." + ), + "claims_not_supported": ( + "No claim of prototype performance, human-in-the-loop " + "stability, global passivity, delay robustness, transparency, " + "or workspace-wide/statistical robustness." + ), + "ablation_scope": ( + "proposed_no_energy bypasses only the final energy projection. " + "It retains feedback gain/filtering, haptic rate and torque " + "limits, tracking effort limits, acceleration/velocity/position " + "guards, soft joint-limit guards, and the wall force cap." + ), + "feedback_chain_comparison": ( + "A.T receives the estimated joint residual, whereas the direct " + "baseline receives its DLS wrench projection. This is an " + "end-to-end feedback-chain ablation, not a same-input pure " + "matrix comparison." + ), + "slave_reference_sampling": ( + "q_s_ref and qd_s_ref are both sampled and held at the 50 Hz " + "mapping update rate; qdd_s_ref is zero." + ), + }, + "models": { + "master_urdf": str(MASTER_URDF), + "slave_urdf": str(SLAVE_URDF), + "slave_contact_frame": config.slave_contact_frame, + "contact_frame_status": ( + "R_EE_SIM is a simulation-only fixed frame from the repository " + "MJCF marker; a measured TCP/FT transform is required for hardware" + if config.slave_contact_frame == SLAVE_FRAMES["ee"] + else "terminal frame present in real_slave_7dof.urdf" + ), + }, + "config": asdict(config), + "wall": wall_metadata, + "scenarios": { + result.scenario.key: { + "mapping": result.scenario.mapping, + "energy_supervision": result.scenario.supervise_energy, + "supervisor": result.scenario.supervisor, + "map_policy": result.scenario.map_policy, + "description": result.scenario.description, + "metrics": result.metrics, + } + for result in results + }, + "artifacts": { + "plot": "closed_loop_comparison.png", + "machine_readable_logs": [ + f"{result.scenario.key}.npz" for result in results + ], + "tabular_logs": [ + f"{result.scenario.key}.csv" for result in results + ], + }, + } + with (output_dir / "summary.json").open("w", encoding="utf-8") as stream: + json.dump(_json_ready(summary), stream, indent=2, ensure_ascii=False) + + print("\nSimulation-only comparison") + print( + "scenario contact[N] A-port err[W] " + "rho_min E_min[J] map p95[ms]" + ) + for result in results: + metrics = result.metrics + tank_min = metrics["tank_energy_min_J"] + tank_text = " n/a" if tank_min is None else f"{tank_min:8.4f}" + rho_min = metrics["rho_min"] + rho_text = " n/a" if rho_min is None else f"{rho_min:9.3f}" + print( + f"{result.scenario.key:24s}" + f"{metrics['peak_contact_force_N']:10.3f}" + f"{metrics['a_port_identity_error_rms_W']:15.3e}" + f"{rho_text}" + f"{tank_text}" + f"{metrics['mapping_runtime_p95_ms']:13.3f}" + ) + print(f"\nArtifacts: {output_dir}") + return summary + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Run the closed-loop bilateral simulation comparison." + ) + parser.add_argument( + "--config", + type=Path, + default=DEFAULT_CONFIG_PATH, + help=f"YAML configuration (default: {DEFAULT_CONFIG_PATH})", + ) + parser.add_argument( + "--output-dir", + type=Path, + default=DEFAULT_OUTPUT_DIR, + help=f"artifact directory (default: {DEFAULT_OUTPUT_DIR})", + ) + parser.add_argument( + "--duration", + type=float, + default=None, + help="override simulated duration per scenario in seconds", + ) + parser.add_argument( + "--dt", + type=float, + default=None, + help="override dynamics integration period in seconds", + ) + parser.add_argument( + "--mapping-hz", + type=float, + default=None, + help="override retargeting/differential update rate", + ) + return parser.parse_args() + + +def main() -> None: + args = parse_args() + config = load_simulation_config(args.config.resolve()) + overrides = { + key: value + for key, value in { + "dt": args.dt, + "duration": args.duration, + "mapping_hz": args.mapping_hz, + }.items() + if value is not None + } + if overrides: + config = replace(config, **overrides) + run_comparison(config, args.output_dir.resolve()) + + +if __name__ == "__main__": + main() diff --git a/code/test/ablation_haptic.py b/code/test/ablation_haptic.py new file mode 100644 index 0000000..3a5a49a --- /dev/null +++ b/code/test/ablation_haptic.py @@ -0,0 +1,511 @@ +# -*- coding: utf-8 -*- +""" +ablation_haptic.py + +Quantitative ablation for the haptic rendering module. + +Modes: +- baseline : full method (energy tank + F/V filtering + alpha smoothing + torque LPF) +- no_tank : energy tank disabled (no passivity enforcement) +- no_filter : tank enabled but F/V filtering + alpha smoothing + torque LPF disabled + +Run: + python test/ablation_haptic.py +from the project root, with config/config.yaml following the same +structure as demo.py and test_sew.py. +""" + +import os +import sys +import time +from typing import Dict + +import numpy as np +import pinocchio as pin + +import matplotlib.pyplot as plt +from omegaconf import OmegaConf + +# Make sure project root is on sys.path +THIS_DIR = os.path.dirname(__file__) +PROJECT_ROOT = os.path.abspath(os.path.join(THIS_DIR, "..")) +if PROJECT_ROOT not in sys.path: + sys.path.append(PROJECT_ROOT) + +from core.sew_mapper import SEWMapper +from core.interaction_estimater import InteractionEstimator +from core.haptic_render import HapticRenderer + + +# -------------------- Utilities -------------------- + +def dummy_master_measure(step: int, model: pin.Model) -> np.ndarray: + """ + Synthetic master joint measurement used for offline ablation. + Start from neutral and add small sinusoidal motions on the first few joints. + """ + q = pin.neutral(model) + n_use = min(model.nq, 6) + for i in range(n_use): + q[i] = 0.5 * np.sin(0.01 * step + 0.7 * i) + return q + + +def dummy_slave_dynamics(q_cmd: np.ndarray, model: pin.Model) -> np.ndarray: + """ + Simple slave "execution" model: follow the commanded joint positions + with a small Gaussian disturbance, to emulate interaction / noise. + """ + q_cmd = np.asarray(q_cmd, dtype=float).reshape(-1,) + noise = 0.001 * np.random.randn(model.nq) + return q_cmd + noise + + +def load_config(): + """ + Load config/config.yaml in the project root (same style as demo.py / test_sew.py). + """ + cand = os.path.join(PROJECT_ROOT, "config", "config.yaml") + if not os.path.exists(cand): + raise FileNotFoundError(f"config.yaml not found at: {cand}") + conf = OmegaConf.load(cand) + print(f"[INFO] Loaded config from: {cand}") + return conf + + +def build_modules(conf): + # Build models from URDF + master_model = pin.buildModelFromUrdf(str(conf.master_urdf)) + slave_model = pin.buildModelFromUrdf(str(conf.slave_urdf)) + + # SEW mapper + sew = SEWMapper( + master_model=master_model, + slave_model=slave_model, + m_shoulder_frame=conf.m_shoulder_frame, + m_elbow_frame=conf.m_elbow_frame, + m_wrist_frame=conf.m_wrist_frame, + m_ee_frame=conf.m_ee_frame, + s_shoulder_frame=conf.s_shoulder_frame, + s_elbow_frame=conf.s_elbow_frame, + s_wrist_frame=conf.s_wrist_frame, + s_ee_frame=conf.s_ee_frame, + slave_joint_names=conf.sew_mapper.slave_joint_names, + up_dir=np.array(conf.sew_mapper.up_dir, dtype=float), + eps_clip=float(conf.sew_mapper.eps_clip), + ) + + # Interaction estimator on slave side + estimator = InteractionEstimator( + model=slave_model, + chest_frame_name=conf.s_base_frame, + ee_frame_name=conf.s_ee_frame, + lambda_damp=float(conf.interaction_est.lambda_damp), + ) + + return master_model, slave_model, sew, estimator + + + +# -------------------- Haptic renderer variants -------------------- + +class HapticRendererNoTank(HapticRenderer): + """ + Variant that completely disables the energy tank: + - directly maps interaction wrench to master joint torques through CJ_m^T + - no passivity enforcement + """ + + 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): + CF = np.asarray(CF_int_slave_C, dtype=float).reshape(6,) + # Chest Jacobian on master side + CJ_m = self.CJ_master.chest_jacobian(q_m, qd_m) + tau_fb_m_raw = CJ_m.T @ CF + alpha = 1.0 + return tau_fb_m_raw, alpha + + +def make_renderer(conf, master_model: pin.Model, mode: str) -> HapticRenderer: + """ + Construct different haptic renderers for ablation. + """ + common_kwargs = dict( + master_model=master_model, + chest_frame_name=conf.m_base_frame, + ee_frame_name=conf.m_ee_frame, + feedback_strength=float(conf.haptic_render.feedback_strength), + E_init=float(conf.haptic_render.E_init), + E_max=float(conf.haptic_render.E_max), + alpha_floor=float(conf.haptic_render.alpha_floor), + alpha_ceil=float(conf.haptic_render.alpha_ceil), + E0=float(conf.haptic_render.E0), + ) + + if mode == "baseline": + renderer = HapticRenderer(**common_kwargs) + + # baseline：能量罐 + 强滤波 + 平滑 + 扭矩低通（“管得很严”） + renderer.tank.tp.force_alpha = 0.05 + renderer.tank.tp.vel_alpha = 0.05 + renderer.tank.tp.alpha_smooth = 0.02 + renderer.tank.tp.power_deadzone = 0.0 + renderer.tau_alpha = 0.05 + + elif mode == "no_tank": + renderer = HapticRendererNoTank(**common_kwargs) + + # no_tank：完全不做能量控制，也不做扭矩低通，暴露最差情况 + renderer.tau_alpha = 1.0 + + elif mode == "no_filter": + renderer = HapticRenderer(**common_kwargs) + + # no_filter：保留能量罐，但不给它任何滤波/平滑能力 + renderer.tank.tp.force_alpha = 1.0 + renderer.tank.tp.vel_alpha = 1.0 + renderer.tank.tp.alpha_smooth = 1.0 + renderer.tank.tp.power_deadzone = 0.0 + renderer.tau_alpha = 1.0 + + else: + raise ValueError(f"Unknown ablation mode: {mode}") + + return renderer + + +# -------------------- Teleoperation simulation (offline) -------------------- + +def simulate_teleop(conf, + master_model: pin.Model, + slave_model: pin.Model, + sew: SEWMapper, + estimator: InteractionEstimator, + renderer: HapticRenderer, + Ts: float = 0.002, + steps: int = 800) -> Dict[str, np.ndarray]: + """ + Offline simulation similar to demo.py, but without real-time delays. + Returns logs for computing quantitative metrics. + """ + + # States + q_slave = pin.neutral(slave_model) + dq_slave = np.zeros(slave_model.nv) + q_slave_prev = q_slave.copy() + + q_m_init = dummy_master_measure(0, master_model) + pre_q_m = q_m_init.copy() + pre_qd_m = np.zeros_like(q_m_init) + + # Logs + log_tau_fb = [] + log_alpha = [] + log_E = [] + log_tau_int_norm = [] + log_qd_m = [] + ee_traj_world = [] + + # Harder virtual wall to highlight differences + d_wall = 0.35 + k_wall = 20000.0 + + for k in range(steps): + # --- master state --- + q_m = dummy_master_measure(k, master_model) + if k == 0: + qd_m = np.zeros_like(q_m) + qdd_m = np.zeros_like(q_m) + else: + qd_m = (q_m - pre_q_m) / Ts + qdd_m = (qd_m - pre_qd_m) / Ts + + pre_q_m = q_m + pre_qd_m = qd_m + log_qd_m.append(qd_m.copy()) + + # --- SEW retargetting: master -> desired slave --- + q_des_slave, _ = sew.retargetting(q_m, q_slave) + + # --- slave motion (with small noise) --- + q_slave = dummy_slave_dynamics(q_des_slave, slave_model) + dq_slave = (q_slave - q_slave_prev) / Ts + q_slave_prev = q_slave.copy() + qdd_slave = np.zeros(slave_model.nv) + + # --- virtual wall on slave side --- + CJ_env = estimator._chest_jacobian(q_slave, dq_slave) + + oTC = estimator.data.oMf[estimator.fid_C] # ^wT_C + oTEE = estimator.data.oMf[estimator.fid_EE] # ^wT_EE + + R_wc = oTC.rotation + p_wc = oTC.translation + R_cw = R_wc.T + p_cw = -R_cw @ p_wc + + p_we = oTEE.translation + ee_traj_world.append(p_we.copy()) + p_ce = R_cw @ p_we + p_cw + x_ce = float(p_ce[0]) + + if x_ce > d_wall: + delta = x_ce - d_wall + Fx = -k_wall * delta + CF_env = np.array([Fx, 0, 0, 0, 0, 0], dtype=float) + else: + CF_env = np.zeros(6, dtype=float) + + tau_env = CJ_env.T @ CF_env + + # --- interaction estimation on slave --- + tau_model = estimator._tau_model(q_slave, dq_slave, qdd_slave) + tau_meas = tau_model + tau_env + tau_int, CF_int, CJ = estimator.estimate(q_slave, dq_slave, qdd_slave, tau_meas) + + log_tau_int_norm.append(np.linalg.norm(tau_int)) + + V_slave_C = CJ @ dq_slave # 6 x 1 + + # --- haptic rendering on master side --- + tau_cmd_m, tau_fb_m, alpha = renderer.render_tau( + q_m=q_m, + qd_m=qd_m, + qdd_m=qdd_m, + CF_int_slave_C=CF_int, + V_slave_C=V_slave_C, + tau_ff_fric=None, + dt=Ts, + ) + + log_tau_fb.append(tau_fb_m.copy()) + log_alpha.append(alpha) + # not all variants really use the tank, but querying E is safe + log_E.append(getattr(renderer.tank, "E", 0.0)) + + # stack + log_tau_fb = np.vstack(log_tau_fb) + log_alpha = np.asarray(log_alpha) + log_E = np.asarray(log_E) + log_tau_int_norm = np.asarray(log_tau_int_norm) + log_qd_m = np.vstack(log_qd_m) + ee_traj_world = np.vstack(ee_traj_world) + + return dict( + tau_fb=log_tau_fb, + alpha=log_alpha, + E=log_E, + tau_int_norm=log_tau_int_norm, + qd_m=log_qd_m, + ee_traj_world=ee_traj_world, + ) + + +# -------------------- Metric computation -------------------- + +def compute_metrics(tau_fb: np.ndarray, + qd_m: np.ndarray, + E: np.ndarray, + Ts: float) -> Dict[str, float]: + """ + Compute quantitative metrics for ablation: + - rms / peak torque + - torque spike count / rate + - positive power injection ratio + - (optional) energy range if tank is used + """ + # torque norms + tau_norm = np.linalg.norm(tau_fb, axis=1) + rms_tau = float(np.sqrt(np.mean(tau_norm ** 2))) + peak_tau = float(np.max(tau_norm)) + + # torque spikes: ||Δτ||_∞ > threshold + dtau = np.diff(tau_fb, axis=0) + dtau_inf = np.max(np.abs(dtau), axis=1) + spike_th = 5.0 # Nm, can be adjusted to your system scale + n_spikes = int(np.sum(dtau_inf > spike_th)) + spike_rate = n_spikes / (len(dtau_inf) * Ts) + + # positive power injection ratio: tau_fb^T * qd_m > 0 + P = np.sum(tau_fb * qd_m, axis=1) + P_inject_ratio = float(np.mean(P > 0.0)) + + metrics = dict( + rms_tau=rms_tau, + peak_tau=peak_tau, + n_spikes=n_spikes, + spike_rate=spike_rate, + P_inject_ratio=P_inject_ratio, + ) + + if np.any(E != 0.0): + metrics["E_min"] = float(np.min(E)) + metrics["E_max"] = float(np.max(E)) + + return metrics + + +def plot_ablation_results(all_logs, all_metrics, Ts): + + modes = ["baseline", "no_tank", "no_filter"] + colors = { + "baseline": "tab:blue", + "no_tank": "tab:red", + "no_filter": "tab:green", + } + + # ------------------------------------------- + # 1) Torque feedback trajectories (norm vs time) + # ------------------------------------------- + plt.figure(figsize=(8,4)) + for mode in modes: + tau_norm = np.linalg.norm(all_logs[mode]["tau_fb"], axis=1) + t = np.arange(len(tau_norm)) * Ts + plt.plot(t, tau_norm, label=mode, color=colors[mode]) + plt.xlabel("Time [s]") + plt.ylabel(r"$\|\tau_{\mathrm{fb}}(t)\|\ \mathrm{[N\cdot m]}$") + plt.title("Feedback Torque Norm Over Time") + plt.legend() + plt.grid(True, alpha=0.3) + plt.tight_layout() + plt.savefig("logs_ablation/fig_tau_fb_norm.png", dpi=300) + + # ------------------------------------------- + # 2) Alpha(t) (energy tank scaling) + # ------------------------------------------- + plt.figure(figsize=(8,4)) + for mode in modes: + alpha = all_logs[mode]["alpha"] + t = np.arange(len(alpha)) * Ts + plt.plot(t, alpha, label=mode, color=colors[mode]) + plt.xlabel("Time [s]") + plt.ylabel(r"$\alpha(t)$") + plt.title("Energy Tank Scaling Factor") + plt.ylim([-0.1, 1.1]) + plt.legend() + plt.grid(True, alpha=0.3) + plt.tight_layout() + plt.savefig("logs_ablation/fig_alpha.png", dpi=300) + + # ------------------------------------------- + # 3) Tank Energy E(t) + # ------------------------------------------- + plt.figure(figsize=(8,4)) + for mode in modes: + E = all_logs[mode]["E"] + t = np.arange(len(E)) * Ts + plt.plot(t, E, label=mode, color=colors[mode]) + plt.xlabel("Time [s]") + plt.ylabel(r"$E(t)$") + plt.title("Energy Tank Level") + plt.legend() + plt.grid(True, alpha=0.3) + plt.tight_layout() + plt.savefig("logs_ablation/fig_energy.png", dpi=300) + + # ------------------------------------------- + # 4) Bar charts: RMS / Peak / Spikes / P>0 + # ------------------------------------------- + metrics_list = ["rms_tau", "peak_tau", "n_spikes", "P_inject_ratio"] + metric_names = [ + "RMS Torque", + "Peak Torque", + "# Torque Spikes", + "Positive Power Ratio", + ] + + plt.figure(figsize=(9,4)) + for i, key in enumerate(metrics_list): + plt.subplot(1,4,i+1) + vals = [all_metrics[m][key] for m in modes] + plt.bar(modes, vals, color=[colors[m] for m in modes]) + plt.title(metric_names[i]) + plt.xticks(rotation=45) + plt.grid(True, alpha=0.3) + plt.tight_layout() + plt.savefig("logs_ablation/fig_metrics_barchart.png", dpi=300) + + print("[INFO] All plots saved to logs_ablation/") + + + +# -------------------- Main -------------------- + +def main(): + conf = load_config() + master_model, slave_model, sew, estimator = build_modules(conf) + + Ts = 0.002 + steps = 800 + + modes = ["baseline", "no_tank", "no_filter"] + all_logs = {} + all_metrics = {} + + print(f"dt = {Ts:.4f} s, steps = {steps}") + + for mode in modes: + print(f"\n========== Running haptic ablation: {mode} ==========") + renderer = make_renderer(conf, master_model, mode) + renderer.reset_tank(float(conf.haptic_render.E0)) + + # fix random seed so that each mode sees the same slave noise + np.random.seed(0) + + t0 = time.time() + logs = simulate_teleop( + conf, + master_model, + slave_model, + sew, + estimator, + renderer, + Ts=Ts, + steps=steps, + ) + elapsed = time.time() - t0 + + all_logs[mode] = logs + + metrics = compute_metrics( + tau_fb=logs["tau_fb"], + qd_m=logs["qd_m"], + E=logs["E"], + Ts=Ts, + ) + all_metrics[mode] = metrics + + print( + f"[{mode}] done in {elapsed:.3f} s | " + f"rms_tau={metrics['rms_tau']:.3f}, " + f"peak_tau={metrics['peak_tau']:.3f}, " + f"spikes={metrics['n_spikes']}, " + f"spike_rate={metrics['spike_rate']:.3f} 1/s, " + f"P>0={metrics['P_inject_ratio']:.3f}, " + f"E_range=" + f"{'[{:.2f},{:.2f}]'.format(metrics['E_min'], metrics['E_max']) if 'E_min' in metrics else 'N/A'}" + ) + + # Save logs for further plotting if needed + save_dir = os.path.join(PROJECT_ROOT, "logs_ablation") + os.makedirs(save_dir, exist_ok=True) + save_path = os.path.join(save_dir, "haptics_ablation_results.npz") + + # pack as a single dict and rely on pickle when loading + np.savez( + save_path, + all_logs=all_logs, + all_metrics=all_metrics, + Ts=Ts, + ) + + print(f"\n[INFO] Ablation logs and metrics saved to:\n {save_path}") + plot_ablation_results(all_logs, all_metrics, Ts) + + +if __name__ == "__main__": + main() diff --git a/code/test/broadcast_control.py b/code/test/broadcast_control.py new file mode 100644 index 0000000..44a877e --- /dev/null +++ b/code/test/broadcast_control.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +瓴控电机 CAN 广播控制（V2.35）： +- 力矩/开环控制：帧ID 0x280，data[0..7] 为 4 个电机的 int16 torqueValue（低字节在前，小端） +- 混合命令：帧ID 0x288，data[0,2,4,6] 为 4 个电机 motorCmd（如 0x88 开启 / 0x81 停止 / 0x80 关闭） + +依赖： + pip install python-can + +运行前准备（Linux 推荐 SocketCAN）： + sudo ip link set can0 up type can bitrate 500000 + 或者 1000000（看你驱动与设备设置） +""" + +from __future__ import annotations +import time +from typing import Iterable, Tuple + +import can + + +TORQUE_CMD_ID = 0x280 +MIXED_CMD_ID = 0x288 + +# 混合命令 motorCmd（来自文档） +CMD_READ_STATUS1 = 0x9A +CMD_CLR_ERROR = 0x9B +CMD_READ_STATUS2 = 0x9C +CMD_MOTOR_OFF = 0x80 +CMD_MOTOR_ON = 0x88 +CMD_MOTOR_STOP = 0x81 + + +def _clamp_int16(x: int) -> int: + """限制到 int16 范围（-32768..32767），随后按补码打包。""" + return max(-32768, min(32767, int(x))) + + +def _pack_i16_le(x: int) -> Tuple[int, int]: + """int16 -> (low_byte, high_byte) 小端。""" + x = _clamp_int16(x) + u = x & 0xFFFF + return (u & 0xFF, (u >> 8) & 0xFF) + + +def build_torque_broadcast(t1: int, t2: int, t3: int, t4: int = 0) -> can.Message: + """ + 帧ID 0x280: + data[0..1]=#1 torqueValue (low,high) + data[2..3]=#2 + data[4..5]=#3 + data[6..7]=#4 + """ + d0, d1 = _pack_i16_le(t1) + d2, d3 = _pack_i16_le(t2) + d4, d5 = _pack_i16_le(t3) + d6, d7 = _pack_i16_le(t4) + data = [d0, d1, d2, d3, d4, d5, d6, d7] + return can.Message(arbitration_id=TORQUE_CMD_ID, is_extended_id=False, data=data) + + +def build_mixed_cmd(cmd1: int, cmd2: int, cmd3: int, cmd4: int = 0x00) -> can.Message: + """ + 帧ID 0x288: + data[0]=#1 motorCmd, data[1]=0 + data[2]=#2 motorCmd, data[3]=0 + data[4]=#3 motorCmd, data[5]=0 + data[6]=#4 motorCmd, data[7]=0 + """ + data = [cmd1 & 0xFF, 0x00, cmd2 & 0xFF, 0x00, cmd3 & 0xFF, 0x00, cmd4 & 0xFF, 0x00] + return can.Message(arbitration_id=MIXED_CMD_ID, is_extended_id=False, data=data) + + +def send_many(bus: can.BusABC, msgs: Iterable[can.Message], gap_s: float = 0.005) -> None: + """连续发几帧（给驱动一点处理间隔）。""" + for m in msgs: + bus.send(m) + print(m) + time.sleep(gap_s) + + +def main(): + # 1) 选择 CAN 接口 + # - Linux SocketCAN: channel="can0", bustype="socketcan" + # - 如果你用的是 USB-CAN 且已有 python-can 对应驱动，也可改 bustype/channel + bus = can.Bus(interface="canalystii", channel=0, bitrate=1000000) + + # 2) 可选：先广播“电机开启” + send_many(bus, [build_mixed_cmd(CMD_MOTOR_ON, CMD_MOTOR_ON, CMD_MOTOR_ON, CMD_MOTOR_ON)], gap_s=0.02) + + # 3) 力矩模式：控制 3 个电机一起转（示例：#1=+T, #2=-T, #3=+T，第4路=0） + # 注意 torqueValue 的具体量程与含义：文档写 MF/MG 为 -2000..2000（电流力矩），MS 为 -850..850（开环电压） + # 发送频率：文档提示 500kbps 下 4 电机广播最大约 600Hz；1Mbps 下约 1.2kHz。这里示例用 200Hz 更稳妥。 + freq_hz = 500.0 + dt = 1.0 / freq_hz + + T = 200 # 示例力矩指令（根据你的电机系列调整范围） + + count = 0 + + try: + while True: + # now = time.time() + # if now - t0 > duration_s: + # break + + msg = build_torque_broadcast( + t1=50, + t2=50, + t3=40, + t4=50 + ) + bus.send(msg) + + count += 1 + if count % 100 == 0: + print(msg) + + time.sleep(dt) + + except KeyboardInterrupt: + pass + finally: + # 4) 停止（建议退出前先 stop，再按需 off） + msg = build_torque_broadcast( + t1=0, + t2=0, + t3=0, + t4=0 + ) + bus.send(msg) + send_many(bus, [build_mixed_cmd(CMD_MOTOR_STOP, CMD_MOTOR_STOP, CMD_MOTOR_STOP, CMD_MOTOR_STOP)], gap_s=0.02) + send_many(bus, [build_mixed_cmd(CMD_MOTOR_OFF, CMD_MOTOR_OFF, CMD_MOTOR_OFF, CMD_MOTOR_OFF)], gap_s=0.02) + bus.shutdown() + + +if __name__ == "__main__": + main() diff --git a/code/test/broadcast_control2.py b/code/test/broadcast_control2.py new file mode 100644 index 0000000..52361a2 --- /dev/null +++ b/code/test/broadcast_control2.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import time +import struct +import threading +from dataclasses import dataclass, field +from typing import Dict, List, Tuple +import can + + +TORQUE_CMD_ID = 0x280 + + +def _clamp_int16(x: int) -> int: + return max(-32768, min(32767, int(x))) + + +def _pack_i16_le(x: int) -> Tuple[int, int]: + x = _clamp_int16(x) + u = x & 0xFFFF + return (u & 0xFF, (u >> 8) & 0xFF) + + +def build_torque_broadcast(t1: int, t2: int, t3: int, t4: int) -> can.Message: + d0, d1 = _pack_i16_le(t1) + d2, d3 = _pack_i16_le(t2) + d4, d5 = _pack_i16_le(t3) + d6, d7 = _pack_i16_le(t4) + return can.Message(arbitration_id=TORQUE_CMD_ID, is_extended_id=False, + data=[d0, d1, d2, d3, d4, d5, d6, d7]) + + +def _arb_id_single(motor_id: int) -> int: + return 0x140 + int(motor_id) + + +def dump_frame(mid: int, fr): + if fr.t <= 0: + print(f"M{mid}: (no reply yet)") + return + + d = fr.data + arb = fr.arb + cmd = d[0] + + if cmd == 0xA1: + temperature = struct.unpack(" 0x160: + return + mid = arb - 0x140 + if mid not in self.motor_ids: + return + with self._lock: + self.latest[mid] = RxFrame(time.time(), arb, bytes(msg.data)) + + def snapshot(self) -> Dict[int, RxFrame]: + with self._lock: + return {k: v for k, v in self.latest.items()} + + +def main(): + motor_ids = [1,2,3,4] + bus = can.Bus(interface="canalystii", channel=0, bitrate=1000000) + + # 只收 1~4 的回包（0x141~0x144） + try: + bus.set_filters([{"can_id": _arb_id_single(i), "can_mask": 0x7FF, "extended": False} for i in motor_ids]) + except Exception: + pass + + cache = RxCache(motor_ids) + notifier = can.Notifier(bus, [cache], timeout=0.01) + + hz = 500 + dt = 1.0 / hz + k = 0 + next_tick = time.perf_counter() + + try: + while True: + # 500Hz 广播力矩（示例：全 0，你可替换成控制输出） + bus.send(build_torque_broadcast(30, 30, 30, 30)) + + # 每 100ms 打印一次收到的自动回包 + if k % int(hz * 0.1) == 0: + snap = cache.snapshot() + print("---- RX snapshot ----") + for mid in motor_ids: + fr = snap[mid] + if fr.t <= 0: + print(f"M{mid}: (no reply yet)") + else: + dump_frame(mid, fr) + + k += 1 + + # 固定周期 + next_tick += dt + now = time.perf_counter() + if next_tick > now: + time.sleep(next_tick - now) + else: + next_tick = now + + except KeyboardInterrupt: + pass + finally: + notifier.stop() + bus.send(build_torque_broadcast(0, 0, 0, 0)) + bus.shutdown() + +if __name__ == "__main__": + main() diff --git a/code/test/broadcast_control3.py b/code/test/broadcast_control3.py new file mode 100644 index 0000000..81af19d --- /dev/null +++ b/code/test/broadcast_control3.py @@ -0,0 +1,330 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import time +import struct +import threading +from dataclasses import dataclass +from typing import Dict, List, Tuple, Optional + +import can + +TORQUE_CMD_ID = 0x280 +CMD_A1 = 0xA1 + + +def _clamp_int16(x: int) -> int: + return max(-32768, min(32767, int(x))) + + +def _pack_i16_le(x: int) -> Tuple[int, int]: + x = _clamp_int16(x) + u = x & 0xFFFF + return (u & 0xFF, (u >> 8) & 0xFF) + + +def build_torque_broadcast(t1: int, t2: int, t3: int, t4: int) -> can.Message: + d0, d1 = _pack_i16_le(t1) + d2, d3 = _pack_i16_le(t2) + d4, d5 = _pack_i16_le(t3) + d6, d7 = _pack_i16_le(t4) + return can.Message( + arbitration_id=TORQUE_CMD_ID, + is_extended_id=False, + data=[d0, d1, d2, d3, d4, d5, d6, d7], + ) + + +def _arb_id_single(motor_id: int) -> int: + return 0x140 + int(motor_id) + + +def parse_a1(data8: bytes) -> Dict: + # A1 回包（你实测） + # [0]=0xA1 [1]=temp(int8) [2..3]=iq/power(int16 LE) [4..5]=speed(int16 LE, dps) [6..7]=encoder(uint16 LE) + if len(data8) != 8 or data8[0] != CMD_A1: + raise ValueError("not A1") + temperature = struct.unpack(" str: + if fr.t <= 0: + return f"{tag} M{mid}: (no reply yet)" + d = fr.data + arb = fr.arb + cmd = d[0] + if cmd == CMD_A1: + s = parse_a1(d) + return (f"{tag} M{mid}: arb=0x{arb:X} " + f"T={s['temperature_C']}C iq/p={s['iq_or_power_raw']} " + f"spd={s['speed_dps']}dps enc={s['encoder']}") + return f"{tag} M{mid}: arb=0x{arb:X} cmd=0x{cmd:02X} raw={[hex(x) for x in d]}" + + +@dataclass +class RxFrame: + t: float = 0.0 + arb: int = 0 + data: bytes = b"" + + +class RxCache4Ch(can.Listener): + """ + 一个 Notifier 监听一个 “设备Bus(包含两个channel)”，按 msg.channel 分流到 ch0/ch1 的缓存。 + """ + def __init__(self, ch_to_motor_ids: Dict[int, List[int]]): + super().__init__() + self.ch_to_motor_ids = {int(ch): set(mids) for ch, mids in ch_to_motor_ids.items()} + self._lock = threading.Lock() + self.latest: Dict[Tuple[int, int], RxFrame] = {} + for ch, mids in self.ch_to_motor_ids.items(): + for mid in mids: + self.latest[(ch, mid)] = RxFrame() + + def on_message_received(self, msg: can.Message): + # python-can Message.channel：对 canalystii 多通道会填 0/1 + ch = getattr(msg, "channel", None) + if ch is None: + # 没有 channel 信息就无法分流，直接忽略或当作 ch0 + ch = 0 + try: + ch = int(ch) + except Exception: + ch = 0 + + if ch not in self.ch_to_motor_ids: + return + + arb = msg.arbitration_id + if arb < 0x141 or arb > 0x160: + return + mid = arb - 0x140 + if mid not in self.ch_to_motor_ids[ch]: + return + + with self._lock: + self.latest[(ch, mid)] = RxFrame(time.time(), arb, bytes(msg.data)) + + def snapshot(self) -> Dict[Tuple[int, int], RxFrame]: + with self._lock: + return dict(self.latest) + + +class SenderThread(threading.Thread): + """ + 每个“逻辑channel(共4个)”一个发送线程： + - 等主线程 tick_event + - barrier 同步放行 + - 加 bus_lock 后 send（同一个设备的两个channel共享一个bus对象，必须加锁） + """ + def __init__( + self, + name: str, + bus: can.BusABC, + bus_lock: threading.Lock, + channel_index: int, # 0 or 1 + tick_event: threading.Event, + barrier: threading.Barrier, + stop_event: threading.Event, + ): + super().__init__(name=name) + self.bus = bus + self.bus_lock = bus_lock + self.channel_index = int(channel_index) + self.tick_event = tick_event + self.barrier = barrier + self.stop_event = stop_event + self.torque = (0, 0, 0, 0) + + def run(self): + while not self.stop_event.is_set(): + if not self.tick_event.wait(timeout=0.5): + continue + if self.stop_event.is_set(): + break + + try: + self.barrier.wait(timeout=0.05) + except threading.BrokenBarrierError: + continue + + t1, t2, t3, t4 = self.torque + msg = build_torque_broadcast(t1, t2, t3, t4) + msg.channel = self.channel_index # ★ 指定发到该设备的哪个channel + + try: + with self.bus_lock: + self.bus.send(msg) + except can.CanError: + pass + + # 等主线程清 tick_event，避免同tick重复发 + while self.tick_event.is_set() and not self.stop_event.is_set(): + time.sleep(0) + + +def open_device_bus(device_idx: int, bitrate: int) -> can.BusABC: + """ + 每块 CANalyst-II 只打开一次 Bus，channel=[0,1] + """ + # canalystii 后端一般支持 device=0/1；如果你环境不支持会 TypeError，这里做兼容 + try: + return can.Bus(interface="canalystii", channel=[0, 1], bitrate=bitrate, device=device_idx) + except TypeError: + # 某些版本 canalystii 不暴露 device 参数（只能看到一块设备） + return can.Bus(interface="canalystii", channel=[0, 1], bitrate=bitrate) + + +def main(): + bitrate = 1000000 + hz = 500 + dt = 1.0 / hz + + # ========= 你需要按实际接线修改：每个“设备-channel”上有哪些电机ID ========= + # 逻辑4路：dev0-ch0, dev0-ch1, dev1-ch0, dev1-ch1 + motor_map = { + ("dev0", 0): [1, 2, 3, 4], + ("dev0", 1): [1, 2, 3, 4], + ("dev1", 0): [1, 2, 3, 4], + ("dev1", 1): [1, 2, 3, 4], + } + + # ========= 打开两块设备（每块一次性打开2路channel） ========= + bus0 = open_device_bus(device_idx=0, bitrate=bitrate) + bus1 = open_device_bus(device_idx=1, bitrate=bitrate) + + # 过滤：只收我们关心的 arb_id（两设备相同过滤即可） + def set_filters_for(bus: can.BusABC, all_motor_ids: List[int]): + try: + bus.set_filters([{"can_id": _arb_id_single(i), "can_mask": 0x7FF, "extended": False} for i in all_motor_ids]) + except Exception: + pass + + all_ids_dev0 = sorted(set(motor_map[("dev0", 0)] + motor_map[("dev0", 1)])) + all_ids_dev1 = sorted(set(motor_map[("dev1", 0)] + motor_map[("dev1", 1)])) + set_filters_for(bus0, all_ids_dev0) + set_filters_for(bus1, all_ids_dev1) + + # ========= 接收缓存：每个设备一个 Notifier，一个 cache（按 channel 分流） ========= + cache0 = RxCache4Ch({0: motor_map[("dev0", 0)], 1: motor_map[("dev0", 1)]}) + cache1 = RxCache4Ch({0: motor_map[("dev1", 0)], 1: motor_map[("dev1", 1)]}) + notifier0 = can.Notifier(bus0, [cache0], timeout=0.01) + notifier1 = can.Notifier(bus1, [cache1], timeout=0.01) + + # ========= 发送线程：4个逻辑channel ========= + stop_event = threading.Event() + tick_event = threading.Event() + barrier = threading.Barrier(4 + 1) # 4个sender + 主线程 + + bus0_lock = threading.Lock() + bus1_lock = threading.Lock() + + sender_dev0_ch0 = SenderThread("sender-dev0-ch0", bus0, bus0_lock, 0, tick_event, barrier, stop_event) + sender_dev0_ch1 = SenderThread("sender-dev0-ch1", bus0, bus0_lock, 1, tick_event, barrier, stop_event) + sender_dev1_ch0 = SenderThread("sender-dev1-ch0", bus1, bus1_lock, 0, tick_event, barrier, stop_event) + sender_dev1_ch1 = SenderThread("sender-dev1-ch1", bus1, bus1_lock, 1, tick_event, barrier, stop_event) + + senders = [sender_dev0_ch0, sender_dev0_ch1, sender_dev1_ch0, sender_dev1_ch1] + for th in senders: + th.start() + + # ========= 你的控制输出：这里演示固定 torque ========= + def torque_for(tag: str, ch: int) -> Tuple[int, int, int, int]: + # TODO：替换为你的控制算法输出 + return (30, 30, 30, 30) + + k = 0 + next_tick = time.perf_counter() + + try: + while True: + # 1) 写入4路 torque（主线程写共享变量） + sender_dev0_ch0.torque = torque_for("dev0", 0) + sender_dev0_ch1.torque = torque_for("dev0", 1) + sender_dev1_ch0.torque = torque_for("dev1", 0) + sender_dev1_ch1.torque = torque_for("dev1", 1) + + # 2) 同步tick发布 + barrier放行（尽量同时发送） + tick_event.set() + try: + barrier.wait(timeout=0.05) + except threading.BrokenBarrierError: + pass + tick_event.clear() + + # 3) 打印（每100ms一次） + if k % int(hz * 0.1) == 0: + print(f"\n==== tick {k} t={time.time():.3f} ====") + + snap0 = cache0.snapshot() + print("[dev0-ch0]") + for mid in motor_map[("dev0", 0)]: + print(" " + dump_frame("[dev0-ch0]", mid, snap0[(0, mid)])) + print("[dev0-ch1]") + for mid in motor_map[("dev0", 1)]: + print(" " + dump_frame("[dev0-ch1]", mid, snap0[(1, mid)])) + + snap1 = cache1.snapshot() + print("[dev1-ch0]") + for mid in motor_map[("dev1", 0)]: + print(" " + dump_frame("[dev1-ch0]", mid, snap1[(0, mid)])) + print("[dev1-ch1]") + for mid in motor_map[("dev1", 1)]: + print(" " + dump_frame("[dev1-ch1]", mid, snap1[(1, mid)])) + + k += 1 + + # 4) 500Hz 固定周期（绝对时间推进） + next_tick += dt + now = time.perf_counter() + if next_tick > now: + time.sleep(next_tick - now) + else: + next_tick = now + + except KeyboardInterrupt: + pass + finally: + # 停止发送线程 + stop_event.set() + tick_event.set() + for th in senders: + th.join(timeout=0.5) + + # 停止 Notifier（避免你之前那种 interpreter shutdown 崩溃） + try: + notifier0.stop() + notifier1.stop() + except Exception: + pass + + # 安全清零力矩（给每个设备两个channel都发一次） + try: + for ch in [0, 1]: + m = build_torque_broadcast(0, 0, 0, 0) + m.channel = ch + with bus0_lock: + bus0.send(m) + with bus1_lock: + bus1.send(m) + except Exception: + pass + + try: + bus0.shutdown() + bus1.shutdown() + except Exception: + pass + + +if __name__ == "__main__": + main() diff --git a/code/test/multi_turn_encode.py b/code/test/multi_turn_encode.py new file mode 100644 index 0000000..40c1271 --- /dev/null +++ b/code/test/multi_turn_encode.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import argparse +import math +import struct +import can + + +CMD_READ_MULTITURN_ANGLE = 0x92 + + +def arb_id_single(motor_id: int) -> int: + # 单电机命令/回包 arbitration id + return 0x140 + int(motor_id) + + +def build_read_multiturn_angle(motor_id: int) -> can.Message: + """ + 读取多圈角度命令： + data[0]=0x92，其余填0 + """ + data = [CMD_READ_MULTITURN_ANGLE, 0, 0, 0, 0, 0, 0, 0] + return can.Message(arbitration_id=arb_id_single(motor_id), is_extended_id=False, data=data) + + +def sign_extend(value: int, bits: int) -> int: + """ + 对 value 做符号扩展，使其成为 Python int（有符号） + """ + sign_bit = 1 << (bits - 1) + mask = (1 << bits) - 1 + value &= mask + return (value ^ sign_bit) - sign_bit + + +def parse_multiturn_angle_reply_int56(msg: can.Message) -> int: + """ + 兼容你贴的文档：DATA[1..7] 是角度的低->高字节（共7字节） + => 解析为有符号 int56 + 单位：0.01 deg / LSB + """ + d = bytes(msg.data) + if len(d) != 8 or d[0] != CMD_READ_MULTITURN_ANGLE: + raise ValueError("not 0x92 reply") + raw_u56 = int.from_bytes(d[1:8], byteorder="little", signed=False) # 7 bytes + raw_s56 = sign_extend(raw_u56, 56) + return raw_s56 + + +def parse_multiturn_angle_reply_int64(msg: can.Message) -> int: + """ + 如果你确认实际回的是 8 字节 motorAngle（int64）： + 常见做法是 DATA[0..7] 都是 motorAngle 的 8 字节（但会与“命令字节=0x92”冲突） + 或者命令字节在别处。 + 这个函数留作你确认后切换用。 + """ + d = bytes(msg.data) + # 这里假设 d[0] 仍是 0x92，则 motorAngle 应该在 d[1:9] 不存在 + # 所以只有当你的实际报文不是“d[0]=0x92”时才用它 + raise NotImplementedError("Need exact 8-byte layout confirmation.") + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--interface", default="canalystii", help="python-can interface: canalystii / socketcan / ...") + ap.add_argument("--channel", default="0", help="channel: canalystii 0/1, socketcan can0/can1 ...") + ap.add_argument("--bitrate", type=int, default=1000000) + ap.add_argument("--motor-id", type=int, default=2) + ap.add_argument("--timeout", type=float, default=0.2, help="seconds") + ap.add_argument("--repeat", type=int, default=3, help="query times") + args = ap.parse_args() + + motor_id = args.motor_id + rx_arb = arb_id_single(motor_id) + + bus = can.Bus(interface=args.interface, channel=args.channel, bitrate=args.bitrate) + + # 只收这个电机的回包（降低噪声） + try: + bus.set_filters([{"can_id": rx_arb, "can_mask": 0x7FF, "extended": False}]) + except Exception: + pass + + try: + for i in range(args.repeat): + # 发送 0x92 + bus.send(build_read_multiturn_angle(motor_id)) + + # 等回包 + msg = bus.recv(timeout=args.timeout) + if msg is None: + print(f"[M{motor_id}] timeout: no reply") + continue + if msg.arbitration_id != rx_arb or len(msg.data) != 8: + print(f"[M{motor_id}] unexpected frame: arb=0x{msg.arbitration_id:X} data={list(msg.data)}") + continue + + # 解析（按你贴的文档：int56 in DATA[1..7]） + motor_angle_raw = parse_multiturn_angle_reply_int56(msg) + + # 单位换算：0.01°/LSB + angle_deg = motor_angle_raw * 0.01 + angle_rad = angle_deg * math.pi / 180.0 + + print( + f"[M{motor_id}] motorAngle_raw={motor_angle_raw} " + f"angle_deg={angle_deg:+.4f}° angle_rad={angle_rad:+.6f} rad" + ) + + finally: + bus.shutdown() + + +if __name__ == "__main__": + main() diff --git a/code/test/plot_sew.py b/code/test/plot_sew.py new file mode 100644 index 0000000..7142a38 --- /dev/null +++ b/code/test/plot_sew.py @@ -0,0 +1,436 @@ +import os +import sys +import numpy as np +import pinocchio as pin +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +from omegaconf import OmegaConf + +sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) +from core.sew_mapper import SEWMapper, fk_update, pose_of_frame, rot_error_deg + + +def safe_normalize(v, eps=1e-8): + """Return v / ||v||, or None if the norm is too small.""" + n = np.linalg.norm(v) + if n < eps: + return None + return v / n + + +def check_once(mapper, q_m, names, visualize=True): + import numpy as np + import open3d as o3d + from utils.drwa import o3d_frame, o3d_lineset, o3d_sphere + + m_model, m_data = mapper.m_model, mapper.m_data + s_model, s_data = mapper.s_model, mapper.s_data + + q_s, dbg = mapper.retargetting(q_m) + + # 主端 FK + fk_update(m_model, m_data, q_m) + pS_m, RS_m = pose_of_frame(m_model, m_data, names["m_shoulder"]) + pE_m, REf_m = pose_of_frame(m_model, m_data, names["m_elbow"]) + pW_m, RW_m = pose_of_frame(m_model, m_data, names["m_wrist"]) + p_m, R_m = pose_of_frame(m_model, m_data, names["m_ee"]) + + # 从端 FK + fk_update(s_model, s_data, q_s) + pS_s, RS_s = pose_of_frame(s_model, s_data, names["s_shoulder"]) + pE_s, REf_s = pose_of_frame(s_model, s_data, names["s_elbow"]) + pW_s, RW_s = pose_of_frame(s_model, s_data, names["s_wrist"]) + p_s, R_s = pose_of_frame(s_model, s_data, names["s_ee"]) + + # 误差 + pos_err = np.linalg.norm(p_m - p_s) * 1000.0 # mm + rot_err = rot_error_deg(R_m, R_s) # degree + print(f"rot_err: {rot_err}, rot_err: {rot_err}") + + # 是否发生 reach 裁剪 + clipped = dbg["d_s"] < np.linalg.norm( + m_data.oMf[m_model.getFrameId(names["m_wrist"])].translation + - m_data.oMf[m_model.getFrameId(names["m_shoulder"])].translation) - 1e-9 \ + or dbg["d_s"] > mapper.L1 + mapper.L2 - mapper.eps_clip + 1e-9 + + if visualize: + geoms = [] + geoms.append(o3d_frame(np.eye(3), np.zeros(3), size=0.1)) + + pts_m = np.vstack([pS_m, pE_m, pW_m, p_m]) + geoms.append(o3d_lineset(pts_m, color=(1.0, 0.0, 0.0))) + geoms += [ + o3d_sphere(pS_m, 0.016, (1.0,0.4,0.4)), + o3d_sphere(pE_m, 0.014, (1.0,0.3,0.3)), + o3d_sphere(pW_m, 0.012, (1.0,0.2,0.2)), + o3d_sphere(p_m, 0.012, (1.0,0.0,0.0)), + o3d_frame(R_m, p_m, size=0.07) + ] + # 从臂（蓝） + pts_s = np.vstack([pS_s, pE_s, pW_s, p_s]) + geoms.append(o3d_lineset(pts_s, color=(0.0, 0.4, 1.0))) + geoms += [ + o3d_sphere(pS_s, 0.016, (0.4,0.6,1.0)), + o3d_sphere(pE_s, 0.014, (0.3,0.5,1.0)), + o3d_sphere(pW_s, 0.012, (0.2,0.4,1.0)), + o3d_sphere(p_s, 0.012, (0.0,0.2,1.0)), + o3d_frame(R_s, p_s, size=0.07) + ] + + geoms.append(o3d_sphere(dbg["pW_s_ref"], 0.008, (0.2, 1.0, 0.2))) + o3d.visualization.draw_geometries(geoms, window_name="SEW Retargeting (Master=Red, Slave=Blue)") + return dict(pos_mm=pos_err, rot_deg=rot_err, clipped=clipped, dbg=dbg, q_s=q_s) + + +def random_qm(mapper, N=50, margin=0.2): + lb = []; ub = [] + for j in mapper.m_model.joints[1:]: # skip universe + if j.nq == 1: # 1-DoF revolute + jid = j.id + iq = j.idx_q + lb.append(mapper.m_model.lowerPositionLimit[iq]) + ub.append(mapper.m_model.upperPositionLimit[iq]) + lb = np.array(lb); ub = np.array(ub) + rng = (ub - lb) + lb2 = lb + margin * rng + ub2 = ub - margin * rng + + qs = [] + for _ in range(N): + v = lb2 + np.random.rand(len(lb2)) * (ub2 - lb2) + q_full = pin.neutral(mapper.m_model) + k = 0 + for j in mapper.m_model.joints[1:]: + if j.nq == 1: + q_full[j.idx_q] = v[k]; k += 1 + qs.append(q_full) + return qs + + +def validation(mapper): + q_m0 = pin.neutral(mapper.m_model) + names = { + "m_shoulder": "master_shoulder", + "m_elbow": "master_forearm", + "m_wrist": "master_wrist", + "m_ee": "master_ee", + "s_shoulder": "slave_shoulder", + "s_elbow": "slave_forearm", + "s_wrist": "slave_wrist", + "s_ee": "slave_ee" + } + res0 = check_once(mapper, q_m0, names) + print("[Neutral] pos_err = %.3f mm, rot_err = %.3f deg, clipped=%s" % + (res0["pos_mm"], res0["rot_deg"], res0["clipped"])) + + # 2) 随机多组验证 + N = 100 + samples = random_qm(mapper, N=N, margin=0.1) + pos_errs = []; rot_errs = []; n_clip = 0 + for q_m in samples: + r = check_once(mapper, q_m, names) + pos_errs.append(r["pos_mm"]) + rot_errs.append(r["rot_deg"]) + n_clip += int(r["clipped"]) + + pos_errs = np.array(pos_errs) + rot_errs = np.array(rot_errs) + + +# ======================================================= +# 辅助函数：让 3D 轴等比例，图更“几何化” +# ======================================================= +def set_equal_aspect_3d(ax, pts): + """pts: (N,3) numpy array.""" + pts = np.asarray(pts) + + # 如果有 NaN 或 Inf，直接返回，不调坐标轴 + if not np.all(np.isfinite(pts)): + return + + x_min, y_min, z_min = pts.min(axis=0) + x_max, y_max, z_max = pts.max(axis=0) + max_range = max(x_max - x_min, y_max - y_min, z_max - z_min) + if max_range == 0: + max_range = 1.0 # 防止除零 + + x_mid = 0.5 * (x_max + x_min) + y_mid = 0.5 * (y_max + y_min) + z_mid = 0.5 * (z_max + z_min) + r = 0.6 * max_range + ax.set_xlim(x_mid - r, x_mid + r) + ax.set_ylim(y_mid - r, y_mid + r) + ax.set_zlim(z_mid - r, z_mid + r) + + + +# ======================================================= +# 绘图（论文插图专用） +# ======================================================= + +def plot_sew_geometry(pS_m, pE_m, pW_m, + pS_s, pE_s, pW_s_ref, + xhat_m, n_m, n_ref, phi_m): + """ + Fig. SEW Geometry： + - Master/Slave S-E-W + - Shoulder-Wrist 方向 x̂_m + - 上臂平面法向 n_m + - 参考法向 n_ref + """ + + fig = plt.figure(figsize=(6, 6)) + ax = fig.add_subplot(111, projection='3d') + + # Master skeleton (粗实体＋虚线前臂) + ax.plot([pS_m[0], pE_m[0]], [pS_m[1], pE_m[1]], [pS_m[2], pE_m[2]], + 'r-', lw=2.5, label='Master upper arm') + ax.plot([pE_m[0], pW_m[0]], [pE_m[1], pW_m[1]], [pE_m[2], pW_m[2]], + 'r--', lw=2.0, label='Master forearm') + + # Slave skeleton (蓝色) + ax.plot([pS_s[0], pE_s[0]], [pS_s[1], pE_s[1]], [pS_s[2], pE_s[2]], + 'b-', lw=2.5, label='Slave upper arm') + ax.plot([pE_s[0], pW_s_ref[0]], [pE_s[1], pW_s_ref[1]], [pE_s[2], pW_s_ref[2]], + 'b--', lw=2.0, label='Slave forearm') + + # --- Master 上臂平面 patch（S_m, E_m, W_m） --- + tri = np.vstack([pS_m, pE_m, pW_m]) + area = np.linalg.norm(np.cross(pE_m - pS_m, pW_m - pS_m)) + if area > 1e-8: + # 使用 Poly3DCollection 画简单三角面片，避免 qhull + verts = [tri] + poly = Poly3DCollection(verts, alpha=0.08, + facecolor='r', edgecolor='none') + ax.add_collection3d(poly) + ax.plot_trisurf(tri[:, 0], tri[:, 1], tri[:, 2], + color='r', alpha=0.08, edgecolor='none') + + # Vectors at S_m + arrow_len = 0.12 + ax.quiver(*pS_m, *(arrow_len * xhat_m), + color='g', lw=2.0, normalize=False, label=r'$\hat{x}_m$') + ax.quiver(*pS_m, *(arrow_len * n_m), + color='m', lw=2.0, normalize=False, label=r'$n_m$') + ax.quiver(*pS_m, *(arrow_len * n_ref), + color='c', lw=2.0, normalize=False, label=r'$n_{\mathrm{ref}}$') + + # Labels + ax.text(*pS_m, r'$S_m$', fontsize=10) + ax.text(*pE_m, r'$E_m$', fontsize=10) + ax.text(*pW_m, r'$W_m$', fontsize=10) + ax.text(*pS_s, r'$S_s$', fontsize=10) + ax.text(*pE_s, r'$E_s$', fontsize=10) + ax.text(*pW_s_ref, r'$W_s$', fontsize=10) + + # 视角和轴 + pts_all = np.vstack([pS_m, pE_m, pW_m, pS_s, pE_s, pW_s_ref]) + set_equal_aspect_3d(ax, pts_all) + ax.view_init(elev=22, azim=52) + ax.set_xlabel("X"); ax.set_ylabel("Y"); ax.set_zlabel("Z") + + title_deg = phi_m * 180.0 / np.pi + ax.set_title(r"SEW geometry (swivel $\phi_m = %.1f^\circ$)" % title_deg) + ax.legend(loc='upper right', fontsize=8) + plt.tight_layout() + plt.show() + + +def plot_swivel_angle(pS_m, n_ref, n_m, xhat_m): + """ + Fig. Swivel Angle: + 可视化 n_ref → n_m 绕 x̂_m 的旋转，并画出圆弧 φ_m。 + """ + + fig = plt.figure(figsize=(6, 6)) + ax = fig.add_subplot(111, projection='3d') + + # 以 S_m 为圆心，在垂直于 xhat_m 的平面内画单位圆 + # 构造平面正交基 b1, b2 + tmp = np.array([1.0, 0.0, 0.0]) + if abs(np.dot(tmp, xhat_m)) > 0.9: + tmp = np.array([0.0, 1.0, 0.0]) + b1 = tmp - np.dot(tmp, xhat_m) * xhat_m + b1 = b1 / np.linalg.norm(b1) + b2 = np.cross(xhat_m, b1) + + # 用 n_ref, n_m 在该平面上算出对应角度 + # 这里假设 n_ref 已经在平面内 + r = 0.12 + # Swivel angle + cos_phi = np.dot(n_ref, n_m) + sin_phi = np.dot(xhat_m, np.cross(n_ref, n_m)) + phi = np.arctan2(sin_phi, cos_phi) + + # 圆弧 + ts = np.linspace(0.0, phi, 80) + arc_pts = [] + for t in ts: + v = np.cos(t) * n_ref + np.sin(t) * np.cross(xhat_m, n_ref) + v = v / np.linalg.norm(v) + arc_pts.append(pS_m + r * v) + arc_pts = np.asarray(arc_pts) + ax.plot(arc_pts[:, 0], arc_pts[:, 1], arc_pts[:, 2], 'k-', lw=2.0, label=r'$\phi_m$') + + # 向量 + ax.quiver(*pS_m, *(r * n_ref), color='c', lw=2.0, normalize=False, label=r'$n_{\mathrm{ref}}$') + ax.quiver(*pS_m, *(r * n_m), color='m', lw=2.0, normalize=False, label=r'$n_m$') + ax.quiver(*pS_m, *(r * xhat_m), color='g', lw=2.0, normalize=False, label=r'$\hat{x}_m$') + + ax.text(*pS_m, r'$S_m$', fontsize=10) + + pts_all = np.vstack([arc_pts, pS_m]) + set_equal_aspect_3d(ax, pts_all) + ax.view_init(elev=20, azim=45) + ax.set_xlabel("X"); ax.set_ylabel("Y"); ax.set_zlabel("Z") + ax.set_title(r"Swivel angle definition") + ax.legend(loc='upper right', fontsize=8) + plt.tight_layout() + plt.show() + + +def plot_two_sphere_sew(pS_s, pW_s_ref, L1, L2, pE_s): + """ + Fig. Two-sphere intersection: + - 球心 S_s, W_s，半径 L1, L2 + - 交点 E_s + - 高亮三角形 S-E-W + """ + + fig = plt.figure(figsize=(6, 6)) + ax = fig.add_subplot(111, projection='3d') + + # 球面 + u, v = np.mgrid[0:2*np.pi:40j, 0:np.pi:20j] + sphere1 = pS_s.reshape(3,1,1) + L1 * np.array([ + np.cos(u)*np.sin(v), + np.sin(u)*np.sin(v), + np.cos(v) + ]) + sphere2 = pW_s_ref.reshape(3,1,1) + L2 * np.array([ + np.cos(u)*np.sin(v), + np.sin(u)*np.sin(v), + np.cos(v) + ]) + ax.plot_surface(sphere1[0], sphere1[1], sphere1[2], + alpha=0.15, color='r', edgecolor='none') + ax.plot_surface(sphere2[0], sphere2[1], sphere2[2], + alpha=0.15, color='b', edgecolor='none') + + # S-E-W 三角形 + tri = np.vstack([pS_s, pE_s, pW_s_ref]) + ax.plot_trisurf(tri[:, 0], tri[:, 1], tri[:, 2], + color='k', alpha=0.08, edgecolor='none') + ax.plot([pS_s[0], pE_s[0], pW_s_ref[0], pS_s[0]], + [pS_s[1], pE_s[1], pW_s_ref[1], pS_s[1]], + [pS_s[2], pE_s[2], pW_s_ref[2], pS_s[2]], + 'k-', lw=2.0) + + # 关键点 + ax.scatter(*pE_s, color='k', s=50, label=r'$E_s$') + ax.scatter(*pS_s, color='r', s=40, label=r'$S_s$') + ax.scatter(*pW_s_ref, color='b', s=40, label=r'$W_s$') + + ax.set_xlabel("X"); ax.set_ylabel("Y"); ax.set_zlabel("Z") + ax.set_title("Two-sphere intersection for SEW elbow") + + pts_all = np.vstack([pS_s, pE_s, pW_s_ref]) + set_equal_aspect_3d(ax, pts_all) + ax.view_init(elev=20, azim=40) + ax.legend(loc='upper right', fontsize=8) + plt.tight_layout() + plt.show() + + +# ======================================================= +# 主测试逻辑 +# ======================================================= + +def main(): + conf = OmegaConf.load("../config/config.yaml") + + master_model, _, _ = pin.buildModelsFromUrdf(str(conf.master_urdf)) + slave_model, _, _ = pin.buildModelsFromUrdf(str(conf.slave_urdf)) + + mapper = SEWMapper( + master_model=master_model, + slave_model=slave_model, + m_shoulder_frame=conf.m_shoulder_frame, + m_elbow_frame=conf.m_elbow_frame, + m_wrist_frame=conf.m_wrist_frame, + m_ee_frame=conf.m_ee_frame, + s_shoulder_frame=conf.s_shoulder_frame, + s_elbow_frame=conf.s_elbow_frame, + s_wrist_frame=conf.s_wrist_frame, + s_ee_frame=conf.s_ee_frame, + slave_joint_names=conf.sew_mapper.slave_joint_names, + up_dir=np.array(conf.sew_mapper.up_dir), + eps_clip=conf.sew_mapper.eps_clip, + ) + + # 选一组姿态示例（这里用 neutral） + # q_m = pin.neutral(mapper.m_model) + samples = random_qm(mapper, N=500, margin=0.1) + for q_m in samples: + q_s, dbg = mapper.retargetting(q_m) + + # 主端点 + fk_update(mapper.m_model, mapper.m_data, q_m) + pS_m, _ = pose_of_frame(mapper.m_model, mapper.m_data, conf.m_shoulder_frame) + pE_m, _ = pose_of_frame(mapper.m_model, mapper.m_data, conf.m_elbow_frame) + pW_m, _ = pose_of_frame(mapper.m_model, mapper.m_data, conf.m_wrist_frame) + + # 从端（SEW 参考） + pS_s = mapper.pS_s_fixed + pE_s = dbg["pE_s"] + pW_s_ref = dbg["pW_s_ref"] + + # 几何向量 + r_m = pW_m - pS_m + xhat_m = safe_normalize(r_m) + if xhat_m is None: + raise RuntimeError("Shoulder–wrist vector is zero; check FK / frames.") + + # master 上臂平面法向 + n_m_raw = np.cross(pE_m - pS_m, pW_m - pS_m) + n_m = safe_normalize(n_m_raw) + + # 参考法向（胸部 up 向量投影到垂直于 xhat_m 的平面） + nref_tilde = mapper.up - (mapper.up @ xhat_m) * xhat_m + n_ref = safe_normalize(nref_tilde) + + phi_m = dbg["phi_m"] + + degenerate_plane = (n_m is None or n_ref is None) + if degenerate_plane: + print("[Warn] S–E–W nearly collinear, swivel angle undefined for this posture.") + # 用一个合理的默认法向，方便画 SEW overall 图；swivel 角度取 0 + if n_ref is None: + # 如果连 n_ref 都 degenerate，就随便取一个与 xhat_m 垂直的向量 + tmp = np.array([1.0, 0.0, 0.0]) + if abs(np.dot(tmp, xhat_m)) > 0.9: + tmp = np.array([0.0, 1.0, 0.0]) + n_ref = safe_normalize(tmp - np.dot(tmp, xhat_m) * xhat_m) + if n_m is None: + n_m = n_ref.copy() + phi_m = 0.0 # 退化情形下设成 0 仅用于示意 + + # 绘制三类图 + print("Plotting SEW Geometry...") + plot_sew_geometry(pS_m, pE_m, pW_m, + pS_s, pE_s, pW_s_ref, + xhat_m, n_m, n_ref, phi_m) + + if not degenerate_plane: + print("Plotting Swivel Angle Geometry...") + plot_swivel_angle(pS_m, n_ref, n_m, xhat_m) + else: + print("Skip swivel-angle figure for this degenerate posture (S–E–W nearly collinear).") + + print("Plotting Two-Sphere Intersection...") + plot_two_sphere_sew(pS_s, pW_s_ref, mapper.L1, mapper.L2, pE_s) + + +if __name__ == '__main__': + main() diff --git a/code/test/speed_control.py b/code/test/speed_control.py new file mode 100644 index 0000000..0a88566 --- /dev/null +++ b/code/test/speed_control.py @@ -0,0 +1,169 @@ +import time +import struct +import can + + +def can_id_single_motor(motor_id: int) -> int: + """ + 单电机命令：标识符 = 0x140 + ID(1~32) + """ + assert 1 <= motor_id <= 32 + return 0x140 + motor_id + + +def send_run(bus: can.Bus, motor_id: int): + """ + 电机运行命令：DATA[0]=0x88，其余 0 + """ + arb_id = can_id_single_motor(motor_id) + data = bytes([0x88, 0, 0, 0, 0, 0, 0, 0]) + msg = can.Message(arbitration_id=arb_id, data=data, is_extended_id=False) + bus.send(msg) + + +def send_stop(bus: can.Bus, motor_id: int): + """ + 电机停止命令：DATA[0]=0x81，其余 0 + """ + arb_id = can_id_single_motor(motor_id) + data = bytes([0x81, 0, 0, 0, 0, 0, 0, 0]) + msg = can.Message(arbitration_id=arb_id, data=data, is_extended_id=False) + bus.send(msg) + + +def send_speed_cmd_a2(bus: can.Bus, motor_id: int, speed_dps: float): + """ + 速度闭环控制命令1（0xA2）： + DATA[0]=0xA2 + DATA[4..7]=speedControl(int32, little-endian), 0.01 dps/LSB + + speed_dps: 目标角速度 (deg/s) + """ + speed_control = int(round(speed_dps / 0.01)) # 0.01 dps/LSB + arb_id = can_id_single_motor(motor_id) + + data = bytearray(8) + data[0] = 0xA2 + data[1] = 0x00 + data[2] = 0x00 + data[3] = 0x00 + data[4:8] = struct.pack(" float: + # 1 rev = 360 deg; rpm -> deg/s + return rpm * 360.0 / 60.0 + + +def main(): + # 1) 配置你的 CAN 接口（按你的系统改 channel / bustype） + # 常见：socketcan: channel="can0" + # bus = can.Bus(interface="socketcan", channel="can0", bitrate=1000000) + bus = can.Bus(interface="canalystii", channel=0, bitrate=1000000) + + motor_id = 2 + # 2) 使能运行 + send_run(bus, motor_id) + time.sleep(0.05) + + # 3) 恒速：例如 30 rpm + target_rpm = 100.0 + target_dps = rpm_to_dps(target_rpm) + + # 推荐：循环周期性刷新速度命令（例如 50~200Hz），更稳 + + try: + last_print = 0.0 + while True: + # 方案A：纯速度闭环（0xA2） + send_speed_cmd_a2(bus, 1, target_dps) + send_speed_cmd_a2(bus, 2, target_dps) + send_speed_cmd_a2(bus, 3, target_dps) + # send_speed_cmd_a2(bus, 4, target_dps) + + # 方案B：带转矩电流限制（0xAD），例如限制 iq=300 + # send_speed_cmd_ad(bus, motor_id, target_dps, iq_limit=300) + + time.sleep(0.01) # 100 Hz + + if time.time() - last_print > 0.1: + st1 = read_status2(bus, 1) + st2 = read_status2(bus, 2) + st3 = read_status2(bus, 3) + # st4 = read_status2(bus, 4) + print("status:", [st1, st2, st3]) + last_print = time.time() + + except KeyboardInterrupt: + send_stop(bus, 1) + send_stop(bus, 2) + send_stop(bus, 3) + # send_stop(bus, 4) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/code/test/test_closed_loop_simulation.py b/code/test/test_closed_loop_simulation.py new file mode 100644 index 0000000..5c67356 --- /dev/null +++ b/code/test/test_closed_loop_simulation.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +"""Regression tests for the rigid-body closed-loop simulation.""" + +from dataclasses import replace +from pathlib import Path +import sys +import unittest + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(CODE_ROOT)) + +from simulate_closed_loop import ( # noqa: E402 + DEFAULT_CONFIG_PATH, + SCENARIOS, + SimulationConfig, + Wall, + build_mapper, + load_models, + load_simulation_config, + make_wall, + master_endpoint_configurations, + simulate_scenario, +) +from core.energy_audit import audit_haptic_energy # noqa: E402 + + +class ClosedLoopSimulationTest(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + cls.models = load_models(add_simulated_tcp=True) + + def test_yaml_configuration_is_complete(self) -> None: + config = load_simulation_config(DEFAULT_CONFIG_PATH) + self.assertEqual(config.slave_contact_frame, "R_EE_SIM") + self.assertEqual(config.dt, 0.002) + self.assertEqual(config.mapping_hz, 50.0) + self.assertLess(config.energy_min, config.energy_max) + + def test_wall_is_unilateral_dissipative_and_force_limited(self) -> None: + wall = Wall( + point=np.zeros(3), + normal=np.array([1.0, 0.0, 0.0]), + stiffness=100.0, + damping=10.0, + force_limit=5.0, + ) + free_wrench, free_penetration = wall.wrench( + np.array([-0.1, 0.0, 0.0]), + np.array([1.0, 0.0, 0.0]), + ) + np.testing.assert_allclose(free_wrench, 0.0) + self.assertEqual(free_penetration, 0.0) + + contact_wrench, penetration = wall.wrench( + np.array([0.1, 0.0, 0.0]), + np.array([2.0, 0.0, 0.0]), + ) + self.assertAlmostEqual(penetration, 0.1) + np.testing.assert_allclose(contact_wrench[:3], [-5.0, 0.0, 0.0]) + self.assertLessEqual(float(contact_wrench[:3] @ np.array([2.0, 0.0, 0.0])), 0.0) + + def test_generalized_mapping_preserves_virtual_work(self) -> None: + mapper = build_mapper(self.models) + q_master, _ = master_endpoint_configurations() + _, differential, debug = mapper.retarget_with_differential(q_master) + self.assertTrue(debug["differential_valid"]) + + qd_master = np.array([0.2, -0.1, 0.15, -0.3, 0.05, 0.08, -0.04]) + tau_slave = np.array([-1.0, 0.4, 0.8, -0.5, 0.2, -0.3, 0.1]) + tau_master = differential.T @ tau_slave + self.assertAlmostEqual( + float(tau_master @ qd_master), + float(tau_slave @ (differential @ qd_master)), + places=12, + ) + + def test_short_rigid_body_run_is_finite_and_respects_energy_floor(self) -> None: + config = replace( + SimulationConfig(), + duration=0.8, + feedback_delay_s=0.04, + contact_probe_fraction=0.0, + ) + mapper = build_mapper(self.models) + wall, _, q_slave_start = make_wall(config, self.models, mapper) + result = simulate_scenario( + SCENARIOS[0], + config, + self.models, + wall, + q_slave_start, + ) + + self.assertTrue(result.metrics["completed"]) + self.assertTrue(result.metrics["finite_state"]) + self.assertEqual(result.metrics["master_joint_limit_events"], 0) + self.assertEqual(result.metrics["slave_joint_limit_events"], 0) + self.assertEqual(result.metrics["differential_fallback_count"], 0) + self.assertGreater(result.metrics["wall_contact_duration_s"], 0.0) + self.assertGreaterEqual( + result.metrics["tank_energy_min_J"], + config.energy_min - 1e-12, + ) + + def test_matched_source_map_and_independent_energy_audit(self) -> None: + config = replace( + SimulationConfig(), + duration=0.6, + feedback_delay_s=0.04, + contact_probe_fraction=0.0, + ) + mapper = build_mapper(self.models) + wall, _, q_slave_start = make_wall(config, self.models, mapper) + matched = next( + scenario + for scenario in SCENARIOS + if scenario.key == "matched_wrench_energy" + ) + result = simulate_scenario( + matched, config, self.models, wall, q_slave_start + ) + active = result.logs["return_packet_active"] > 0.5 + self.assertTrue(np.any(active)) + self.assertTrue(np.all(result.logs["source_map_id"][active] >= 0)) + self.assertTrue(np.all(result.logs["map_id"][active] >= 0)) + + audit = audit_haptic_energy( + tau_candidate=result.logs["tau_master_candidate"], + tau_projected=result.logs["tau_master_applied"], + tau_accepted=result.logs["tau_master_accepted"], + qd_master=result.logs["qd_master"], + dt=config.dt, + energy_initial=config.energy_initial, + energy_min=config.energy_min, + energy_max=config.energy_max, + logged_preclip=result.logs["energy_preclip"], + ) + self.assertLessEqual(audit.max_floor_deficit, 1e-12) + self.assertLessEqual(audit.preclip_log_max_error, 1e-12) + + def test_time_domain_popc_is_a_distinct_executable_baseline(self) -> None: + config = replace( + SimulationConfig(), + duration=0.6, + feedback_delay_s=0.02, + contact_probe_fraction=0.0, + ) + mapper = build_mapper(self.models) + wall, _, q_slave_start = make_wall(config, self.models, mapper) + popc = next( + scenario for scenario in SCENARIOS if scenario.key == "proposed_popc" + ) + result = simulate_scenario( + popc, config, self.models, wall, q_slave_start + ) + self.assertTrue(result.metrics["completed"]) + self.assertIsNone(result.metrics["rho_min"]) + self.assertTrue(np.all(np.isfinite(result.logs["popc_damping_gain"]))) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_energy_audit_allocator.py b/code/test/test_energy_audit_allocator.py new file mode 100644 index 0000000..6c49f08 --- /dev/null +++ b/code/test/test_energy_audit_allocator.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 +"""Tests for final accepted-port allocation and independent H4 audit.""" + +from pathlib import Path +import sys +import unittest + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(CODE_ROOT)) + +from core.command_allocator import CommandAllocator # noqa: E402 +from core.energy_audit import audit_haptic_energy # noqa: E402 +from core.time_domain_popc import TimeDomainPOPC # noqa: E402 + + +class AllocatorAuditTest(unittest.TestCase): + def test_reserved_headroom_avoids_total_command_clipping(self): + allocator = CommandAllocator(np.array([5.0, 4.0])) + prepared = allocator.prepare( + compensation=np.array([4.0, -3.5]), + haptic_raw=np.array([3.0, -2.0]), + ) + np.testing.assert_allclose(prepared.haptic_candidate, [1.0, -0.5]) + final = allocator.finalize( + prepared, 0.5 * prepared.haptic_candidate + ) + self.assertFalse(final.downstream_modified) + np.testing.assert_allclose( + final.haptic_accepted, 0.5 * prepared.haptic_candidate + ) + + def test_quantization_is_reported_and_accepted_increment_reconstructed(self): + allocator = CommandAllocator( + np.array([5.0]), quantization_step=np.array([0.2]) + ) + prepared = allocator.prepare(np.array([1.0]), np.array([0.34])) + final = allocator.finalize(prepared, np.array([0.34])) + self.assertTrue(final.quantization_active) + self.assertTrue(final.downstream_modified) + np.testing.assert_allclose(final.total_accepted, [1.4]) + np.testing.assert_allclose(final.haptic_accepted, [0.4]) + + def test_independent_audit_catches_preclip_defect(self): + candidate = np.array([[2.0], [2.0]]) + projected = np.array([[0.5], [0.0]]) + qd = np.ones((2, 1)) + clean = audit_haptic_energy( + tau_candidate=candidate, + tau_projected=projected, + tau_accepted=projected, + qd_master=qd, + dt=0.5, + energy_initial=1.25, + energy_min=1.0, + energy_max=5.0, + ) + self.assertAlmostEqual(clean.max_floor_deficit, 0.0) + self.assertGreater(clean.delta_B, 0.0) + self.assertGreater(clean.projection_distortion, 0.0) + + defective = audit_haptic_energy( + tau_candidate=candidate, + tau_projected=projected, + tau_accepted=np.array([[0.7], [0.0]]), + qd_master=qd, + dt=0.5, + energy_initial=1.25, + energy_min=1.0, + energy_max=5.0, + logged_preclip=np.array([1.0, 1.0]), + ) + self.assertGreater(defective.max_floor_deficit, 0.0) + self.assertGreater(defective.downstream_modification_max, 0.0) + self.assertGreater(defective.preclip_log_max_error, 0.0) + + def test_projection_distortion_is_zero_without_intervention(self): + torque = np.array([[1.0, -2.0], [0.5, 0.25]]) + result = audit_haptic_energy( + tau_candidate=torque, + tau_projected=torque, + tau_accepted=torque, + qd_master=np.zeros_like(torque), + dt=0.01, + energy_initial=2.0, + energy_min=1.0, + energy_max=3.0, + ) + self.assertAlmostEqual(result.projection_distortion, 0.0) + self.assertAlmostEqual(result.delta_B, 0.0) + + +class TimeDomainPOPCTest(unittest.TestCase): + def test_no_intervention_for_passive_candidate(self): + controller = TimeDomainPOPC( + initial_energy=0.0, minimum_energy=0.0, maximum_energy=5.0 + ) + applied, diagnostics = controller.apply( + np.array([-2.0]), np.array([1.0]), 0.1 + ) + np.testing.assert_allclose(applied, [-2.0]) + self.assertFalse(diagnostics.intervention_active) + self.assertAlmostEqual(controller.energy, 0.2) + + def test_active_candidate_receives_damping_injection(self): + controller = TimeDomainPOPC( + initial_energy=0.1, minimum_energy=0.0, maximum_energy=5.0 + ) + applied, diagnostics = controller.apply( + np.array([2.0]), np.array([1.0]), 0.1 + ) + np.testing.assert_allclose(applied, [1.0]) + self.assertTrue(diagnostics.intervention_active) + self.assertAlmostEqual(diagnostics.damping_gain, 1.0) + self.assertAlmostEqual(controller.energy, 0.0) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_estimation_signals.py b/code/test/test_estimation_signals.py new file mode 100644 index 0000000..f9d384f --- /dev/null +++ b/code/test/test_estimation_signals.py @@ -0,0 +1,161 @@ +"""Tests for causal acceleration, friction, and replayable H2 ablations.""" + +from __future__ import annotations + +import sys +import unittest +from pathlib import Path + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +if str(CODE_ROOT) not in sys.path: + sys.path.insert(0, str(CODE_ROOT)) + +from core.estimation_signals import ( # noqa: E402 + AccelerationEstimatorConfig, + AccelerationEstimateStatus, + CalibratedResidualWrenchEstimator, + CausalAccelerationEstimator, + JointFrictionCalibration, + ResidualAblation, + ResidualTorqueModel, + WrenchEstimatorCalibration, +) +from core.wrench_solver import UndampedSVDSolver # noqa: E402 + + +class EstimationSignalsTest(unittest.TestCase): + def test_acceleration_estimator_is_causal_and_rejects_bad_dt(self) -> None: + config = AccelerationEstimatorConfig( + cutoff_hz=10.0, + minimum_dt_s=0.001, + maximum_dt_s=0.1, + ) + estimator = CausalAccelerationEstimator(2, config) + first = estimator.update(np.array([0.0, 0.0]), 1.0) + self.assertIs(first.status, AccelerationEstimateStatus.INITIALIZING) + + second = estimator.update(np.array([0.1, -0.2]), 1.01) + self.assertIs(second.status, AccelerationEstimateStatus.VALID) + np.testing.assert_allclose(second.raw_acceleration, [10.0, -20.0]) + expected_alpha = 1.0 - np.exp(-2.0 * np.pi * 10.0 * 0.01) + np.testing.assert_allclose( + second.acceleration, + expected_alpha * np.array([10.0, -20.0]), + ) + + invalid = estimator.update(np.array([99.0, 99.0]), 1.0101) + self.assertIs(invalid.status, AccelerationEstimateStatus.INVALID_DT) + # Invalid data do not advance the differentiator state. + recovered = estimator.update(np.array([0.2, -0.4]), 1.02) + self.assertIs(recovered.status, AccelerationEstimateStatus.VALID) + np.testing.assert_allclose(recovered.raw_acceleration, [10.0, -20.0]) + + def test_friction_fit_recovers_synthetic_coefficients(self) -> None: + velocity = np.linspace(-1.2, 1.2, 101) + velocity_samples = np.column_stack((velocity, 0.7 * velocity)) + true = JointFrictionCalibration( + coulomb_nm=np.array([0.4, 0.2]), + viscous_nm_per_rad_s=np.array([0.08, 0.12]), + smoothing_velocity_rad_s=0.03, + ) + torque_samples = np.vstack( + [true.torque(sample) for sample in velocity_samples] + ) + fitted = JointFrictionCalibration.fit( + velocity_samples, + torque_samples, + smoothing_velocity_rad_s=0.03, + ) + np.testing.assert_allclose(fitted.coulomb_nm, true.coulomb_nm, atol=1e-13) + np.testing.assert_allclose( + fitted.viscous_nm_per_rad_s, + true.viscous_nm_per_rad_s, + atol=1e-13, + ) + + def test_nominal_and_ablation_residuals_are_explicit(self) -> None: + friction = JointFrictionCalibration( + coulomb_nm=np.array([0.3, 0.2]), + viscous_nm_per_rad_s=np.array([0.1, 0.05]), + ) + calibration = WrenchEstimatorCalibration( + joint_bias_nm=np.array([0.12, -0.08]), + friction=friction, + characteristic_length_m=0.4, + damping=0.02, + calibration_id="fixture-v1", + ) + model = ResidualTorqueModel(calibration) + velocity = np.array([0.5, -0.4]) + contact = np.array([1.2, -0.7]) + rigid = np.array([3.0, 4.0]) + measured = ( + rigid + + contact + + calibration.joint_bias_nm + + friction.torque(velocity) + ) + + nominal = model.compute(measured, rigid, velocity) + no_bias = model.compute( + measured, rigid, velocity, ablation=ResidualAblation.no_bias() + ) + no_friction = model.compute( + measured, + rigid, + velocity, + ablation=ResidualAblation.no_friction(), + ) + np.testing.assert_allclose(nominal.residual_nm, contact, atol=1e-15) + np.testing.assert_allclose( + no_bias.residual_nm, + contact + calibration.joint_bias_nm, + atol=1e-15, + ) + np.testing.assert_allclose( + no_friction.residual_nm, + contact + friction.torque(velocity), + atol=1e-15, + ) + self.assertFalse(calibration.joint_bias_nm.flags.writeable) + + def test_calibrated_estimator_accepts_matched_undamped_solver(self) -> None: + joint_count = 7 + calibration = WrenchEstimatorCalibration( + joint_bias_nm=np.zeros(joint_count), + friction=JointFrictionCalibration.zeros(joint_count), + characteristic_length_m=0.5, + damping=0.03, + relative_rank_tolerance=1e-8, + calibration_id="synthetic-v1", + ) + solver = UndampedSVDSolver( + 0.5, relative_rank_tolerance=1e-8 + ) + estimator = CalibratedResidualWrenchEstimator(calibration, solver) + rng = np.random.default_rng(12) + jacobian = rng.normal(size=(6, joint_count)) + wrench = np.array([2.0, -1.0, 3.0, 0.2, -0.3, 0.4]) + residual = jacobian.T @ wrench + estimate = estimator.estimate( + jacobian, + measured_torque_nm=residual, + rigid_body_torque_nm=np.zeros(joint_count), + joint_velocity_rad_s=np.zeros(joint_count), + ) + np.testing.assert_allclose( + estimate.solve.wrench, wrench, rtol=1e-13, atol=1e-13 + ) + + mismatched = UndampedSVDSolver( + 0.6, relative_rank_tolerance=1e-8 + ) + with self.assertRaisesRegex(ValueError, "characteristic length"): + CalibratedResidualWrenchEstimator(calibration, mismatched) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_experiment_manifest.py b/code/test/test_experiment_manifest.py new file mode 100644 index 0000000..da74dd2 --- /dev/null +++ b/code/test/test_experiment_manifest.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python3 +"""Reproducibility provenance and locked-split safeguards.""" + +import hashlib +from pathlib import Path +import sys +import tempfile +import unittest +from unittest.mock import patch + + +CODE_ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(CODE_ROOT)) + +from experiments.manifest import build_batch_manifest, ensure_batch # noqa: E402 +from experiments.plan import build_trial_plan # noqa: E402 +from experiments.schema import ContractError # noqa: E402 + + +def _plan(split: str = "pilot"): + return build_trial_plan( + { + "study_id": "manifest_contract", + "split": split, + "root_seed": 7, + "replicates": 1, + "methods": ["method"], + "trajectories": ["trajectory"], + } + ) + + +class ExperimentManifestTest(unittest.TestCase): + def test_reproducibility_files_are_hashed_when_present(self): + contents = { + "pyproject.toml": b"[project]\nname='example'\n", + "uv.lock": b"version = 1\n", + "code/config/master_7dof.urdf": b"\n", + "code/config/real_slave_7dof.urdf": b"\n", + } + with tempfile.TemporaryDirectory() as temporary: + source_root = Path(temporary) + for relative_path, data in contents.items(): + path = source_root / relative_path + path.parent.mkdir(parents=True, exist_ok=True) + path.write_bytes(data) + + manifest = build_batch_manifest(_plan(), source_root=source_root) + + recorded = manifest["source"]["files"] + self.assertEqual(set(recorded), set(contents)) + for relative_path, data in contents.items(): + self.assertEqual( + recorded[relative_path]["sha256"], + hashlib.sha256(data).hexdigest(), + ) + + def test_runtime_records_optional_scientific_versions_when_importable(self): + manifest = build_batch_manifest(_plan()) + for module_name in ("scipy", "pinocchio"): + try: + module = __import__(module_name) + except Exception: + self.assertNotIn(module_name, manifest["runtime"]) + else: + self.assertIn(module_name, manifest["runtime"]) + expected = getattr(module, "__version__", None) + if expected is not None: + self.assertEqual( + manifest["runtime"][module_name], + str(expected), + ) + + def test_locked_split_rejects_missing_git_metadata(self): + with tempfile.TemporaryDirectory() as temporary: + with self.assertRaisesRegex( + ContractError, "locked split requires available Git metadata" + ): + build_batch_manifest( + _plan("locked"), + source_root=Path(temporary), + ) + + def test_locked_split_rejects_dirty_git_worktree(self): + dirty = {"available": True, "commit": "abc123", "dirty": True} + with patch("experiments.manifest._git_provenance", return_value=dirty): + with self.assertRaisesRegex( + ContractError, "locked split requires a clean Git worktree" + ): + build_batch_manifest(_plan("locked")) + + def test_pilot_split_allows_missing_git_metadata(self): + unavailable = {"available": False, "commit": None, "dirty": None} + with patch("experiments.manifest._git_provenance", return_value=unavailable): + manifest = build_batch_manifest(_plan("pilot")) + self.assertFalse(manifest["source"]["available"]) + + def test_existing_locked_batch_rechecks_current_git_state(self): + clean = {"available": True, "commit": "abc123", "dirty": False} + unavailable = {"available": False, "commit": None, "dirty": None} + with tempfile.TemporaryDirectory() as temporary: + source_root = Path(temporary) / "source" + batch_dir = Path(temporary) / "batch" + source_root.mkdir() + plan = _plan("locked") + with patch("experiments.manifest._git_provenance", return_value=clean): + ensure_batch(batch_dir, plan, source_root=source_root) + + with patch( + "experiments.manifest._git_provenance", + return_value=unavailable, + ): + with self.assertRaisesRegex( + ContractError, "locked split requires available Git metadata" + ): + ensure_batch(batch_dir, plan, source_root=source_root) + + def test_existing_locked_batch_rejects_commit_drift(self): + initial = {"available": True, "commit": "abc123", "dirty": False} + changed = {"available": True, "commit": "def456", "dirty": False} + with tempfile.TemporaryDirectory() as temporary: + source_root = Path(temporary) / "source" + batch_dir = Path(temporary) / "batch" + source_root.mkdir() + plan = _plan("locked") + with patch("experiments.manifest._git_provenance", return_value=initial): + ensure_batch(batch_dir, plan, source_root=source_root) + + with patch("experiments.manifest._git_provenance", return_value=changed): + with self.assertRaisesRegex( + ContractError, "locked batch Git commit mismatch" + ): + ensure_batch(batch_dir, plan, source_root=source_root) + + def test_existing_locked_batch_rejects_source_file_drift(self): + clean = {"available": True, "commit": "abc123", "dirty": False} + with tempfile.TemporaryDirectory() as temporary: + source_root = Path(temporary) / "source" + batch_dir = Path(temporary) / "batch" + source_root.mkdir() + pyproject = source_root / "pyproject.toml" + pyproject.write_text("initial\n", encoding="utf-8") + plan = _plan("locked") + with patch( + "experiments.manifest._git_provenance", + return_value=dict(clean), + ): + ensure_batch(batch_dir, plan, source_root=source_root) + + pyproject.write_text("changed\n", encoding="utf-8") + with patch( + "experiments.manifest._git_provenance", + return_value=dict(clean), + ): + with self.assertRaisesRegex( + ContractError, + "locked batch source file hash mismatch for: pyproject.toml", + ): + ensure_batch(batch_dir, plan, source_root=source_root) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_experiment_metrics.py b/code/test/test_experiment_metrics.py new file mode 100644 index 0000000..2861174 --- /dev/null +++ b/code/test/test_experiment_metrics.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +"""Independent numerical endpoint tests for H1--H4.""" + +from pathlib import Path +import sys +import unittest + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(CODE_ROOT)) + +from analysis.metrics import ( # noqa: E402 + audit_h4_energy, + compute_h1_composite, + compute_h2_wrench_metrics, + compute_h3_power_mismatch, +) + + +class IndependentExperimentMetricsTest(unittest.TestCase): + def test_h1_detects_only_eligible_discontinuity(self): + metrics = compute_h1_composite( + mapping_valid=[True, True, True], + position_error_m=[0.0, 0.0, 0.0], + orientation_error_rad=[0.0, 0.0, 0.0], + q_slave=np.array( + [ + [0.0, 0.0], + [0.05, 0.0], + [0.50, 0.0], + ] + ), + swivel_angle_rad=[0.0, 0.05, 0.10], + master_step_norm=[0.0, 0.05, 0.05], + position_threshold_m=0.01, + orientation_threshold_rad=0.1, + joint_step_threshold_rad=0.2, + swivel_step_threshold_rad=0.2, + input_step_threshold_rad=0.1, + ) + self.assertEqual(metrics["h1_F_r"], 0) + self.assertEqual(metrics["h1_D_r"], 1) + self.assertEqual(metrics["h1_C_r"], 1) + + def test_h2_separates_force_and_moment_rmse(self): + reference = np.zeros((3, 6)) + estimate = np.tile([3.0, 4.0, 0.0, 0.0, 0.0, 2.0], (3, 1)) + metrics = compute_h2_wrench_metrics(estimate, reference) + self.assertAlmostEqual(metrics["h2_force_rmse_N"], 5.0) + self.assertAlmostEqual(metrics["h2_moment_rmse_Nm"], 2.0) + + def test_h3_is_zero_for_identical_aligned_ports(self): + torque = np.array([[1.0, 2.0], [-2.0, 1.0], [0.5, -0.5]]) + velocity = np.array([[0.2, 0.1], [0.3, -0.1], [0.4, 0.2]]) + metrics = compute_h3_power_mismatch( + tau_master_raw=torque, + qd_master=velocity, + tau_slave_source=torque, + qd_slave_source=velocity, + dt=0.002, + ) + self.assertAlmostEqual(metrics["h3_epsilon_P_act"], 0.0) + + def test_h4_reconstructs_floor_and_projection_distortion(self): + metrics = audit_h4_energy( + energy_before_J=[1.2, 1.1], + energy_after_J=[1.1, 1.0], + tau_candidate=np.array([[0.4], [0.4]]), + tau_applied=np.array([[0.1], [0.1]]), + qd_master=np.ones((2, 1)), + dt=1.0, + energy_min_J=1.0, + energy_max_J=2.0, + audit_tolerance_J=1e-12, + ) + self.assertTrue(metrics["h4_energy_audit_pass"]) + self.assertAlmostEqual(metrics["h4_projected_floor_deficit_J"], 0.0) + self.assertAlmostEqual(metrics["h4_shadow_floor_deficit_J"], 0.6) + self.assertAlmostEqual(metrics["h4_delta_B_J"], 0.6) + self.assertAlmostEqual(metrics["h4_D_proj"], 0.75, places=10) + + def test_h4_detects_downstream_drive_modification(self): + metrics = audit_h4_energy( + energy_before_J=[1.1], + energy_after_J=[1.0], + tau_candidate=np.array([[0.2]]), + tau_applied=np.array([[0.1]]), + tau_accepted=np.array([[0.15]]), + qd_master=np.ones((1, 1)), + dt=1.0, + energy_min_J=1.0, + energy_max_J=2.0, + software_preclip_J=[1.0], + audit_tolerance_J=1e-12, + ) + self.assertFalse(metrics["h4_energy_audit_pass"]) + self.assertAlmostEqual( + metrics["h4_downstream_modification_max_Nm"], 0.05 + ) + self.assertGreater(metrics["h4_software_preclip_max_error_J"], 0.0) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_experiment_plan_contract.py b/code/test/test_experiment_plan_contract.py new file mode 100644 index 0000000..891f618 --- /dev/null +++ b/code/test/test_experiment_plan_contract.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 +"""Contracts for deterministic paired plans and random substreams.""" + +from pathlib import Path +import sys +import unittest + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(CODE_ROOT)) + +from experiments.plan import build_trial_plan, validate_trial_plan # noqa: E402 +from experiments.rng import generator_from_record, named_seed_record # noqa: E402 +from experiments.schema import ContractError # noqa: E402 + + +def example_specification(): + return { + "study_id": "g0c_bilateral_contract", + "split": "calibration", + "root_seed": 20260727, + "replicates": 2, + "methods": ["proposed", "direct"], + "trajectories": [ + { + "trajectory_id": "contact_probe_001", + "family": "approach_contact_return", + "parameters": {"amplitude": 0.1}, + } + ], + "factors": {"delay_ms": [0, 80], "stiffness_N_per_m": [500]}, + } + + +class ExperimentPlanContractTest(unittest.TestCase): + def test_plan_is_deterministic_and_paired(self): + first = build_trial_plan(example_specification()) + second = build_trial_plan(example_specification()) + self.assertEqual(first, second) + self.assertEqual(first["pair_count"], 4) + self.assertEqual(first["trial_count"], 8) + + by_pair = {} + for trial in first["trials"]: + by_pair.setdefault(trial["pair_id"], []).append(trial) + for pair in by_pair.values(): + self.assertEqual(len(pair), 2) + self.assertEqual(pair[0]["seeds"], pair[1]["seeds"]) + self.assertEqual(pair[0]["trajectory"], pair[1]["trajectory"]) + self.assertEqual(pair[0]["factors"], pair[1]["factors"]) + self.assertNotEqual( + pair[0]["method"]["method_id"], + pair[1]["method"]["method_id"], + ) + + def test_named_streams_are_order_independent_and_distinct(self): + forward = named_seed_record( + 9, + {"pair": 3}, + ["trajectory", "sensor", "network"], + ) + reverse = named_seed_record( + 9, + {"pair": 3}, + ["network", "sensor", "trajectory"], + ) + self.assertEqual(forward, reverse) + self.assertEqual(len({tuple(value) for value in forward.values()}), 3) + + generator_a = generator_from_record(forward, "trajectory") + generator_b = generator_from_record(reverse, "trajectory") + np.testing.assert_array_equal( + generator_a.integers(0, 2**31, size=16), + generator_b.integers(0, 2**31, size=16), + ) + + def test_plan_hash_detects_mutation(self): + plan = build_trial_plan(example_specification()) + plan["trials"][0]["factors"]["delay_ms"] = 999 + with self.assertRaises(ContractError): + validate_trial_plan(plan) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_experiment_runner_contract.py b/code/test/test_experiment_runner_contract.py new file mode 100644 index 0000000..c62ae73 --- /dev/null +++ b/code/test/test_experiment_runner_contract.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 +"""Atomic-write, resume, failure-retention, and validation tests.""" + +from pathlib import Path +import sys +import tempfile +import unittest + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(CODE_ROOT)) + +from experiments.io import TrialPayload # noqa: E402 +from experiments.plan import build_trial_plan # noqa: E402 +from experiments.runner import run_trial_plan # noqa: E402 +from experiments.validate import validate_batch # noqa: E402 + + +def runner_specification(): + return { + "study_id": "runner_contract", + "split": "pilot", + "root_seed": 17, + "replicates": 2, + "methods": ["a", "b"], + "trajectories": ["trajectory_001"], + "factors": {"delay_ms": [0]}, + } + + +def successful_executor(trial): + method_offset = 0.0 if trial["method"]["method_id"] == "a" else 1.0 + time = np.arange(5, dtype=float) * 0.002 + return TrialPayload( + samples={ + "time": time, + "signal": np.full((5, 2), method_offset, dtype=float), + }, + events=[{"sample_index": 2, "event": "marker"}], + metadata={"executor": "unit-test"}, + ) + + +class ExperimentRunnerContractTest(unittest.TestCase): + def test_dry_run_writes_nothing(self): + plan = build_trial_plan(runner_specification()) + with tempfile.TemporaryDirectory() as temporary: + batch = Path(temporary) / "batch" + report = run_trial_plan(plan, batch, dry_run=True) + self.assertEqual(report["trial_count"], 4) + self.assertFalse(batch.exists()) + + def test_atomic_run_validate_and_resume(self): + plan = build_trial_plan(runner_specification()) + with tempfile.TemporaryDirectory() as temporary: + batch = Path(temporary) / "batch" + first = run_trial_plan(plan, batch, successful_executor) + self.assertEqual(first["completed_trials"], 4) + self.assertEqual(first["failed_trials"], 0) + validation = validate_batch(batch) + self.assertTrue(validation["valid"], validation["errors"]) + self.assertEqual(validation["completed_trials"], 4) + self.assertFalse( + any(path.name.startswith(".") for path in (batch / "raw").iterdir()) + ) + + second = run_trial_plan(plan, batch, successful_executor, resume=True) + self.assertEqual(second["completed_trials"], 0) + self.assertEqual(second["skipped_trials"], 4) + + def test_failure_is_retained_without_committing_trial(self): + plan = build_trial_plan(runner_specification()) + + def failing_executor(_trial): + raise RuntimeError("intentional failure") + + with tempfile.TemporaryDirectory() as temporary: + batch = Path(temporary) / "batch" + report = run_trial_plan( + plan, + batch, + failing_executor, + max_trials=1, + ) + self.assertEqual(report["failed_trials"], 1) + self.assertFalse((batch / "raw" / plan["trials"][0]["trial_id"]).exists()) + failure_files = list((batch / "failures").rglob("*.json")) + self.assertEqual(len(failure_files), 1) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_feedback_protocol.py b/code/test/test_feedback_protocol.py new file mode 100644 index 0000000..ee6fded --- /dev/null +++ b/code/test/test_feedback_protocol.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +"""Pure numerical contracts for matched H3 feedback and packet semantics.""" + +from pathlib import Path +import sys +import unittest + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(CODE_ROOT)) + +from core.feedback_protocol import ( # noqa: E402 + ForwardPacket, + MapPolicy, + MapRegistry, + MapSnapshot, + MappingKind, + PacketRejectReason, + PacketState, + ReturnPacket, + map_return_feedback, + normalized_actual_power_mismatch, +) +from core.network_emulator import ( # noqa: E402 + DeterministicChannel, + NetworkTraceEntry, + PacketReceiver, +) + + +class FeedbackProtocolTest(unittest.TestCase): + def make_packet(self, seq=3, map_id=1): + wrench = np.array([1.0, -2.0, 0.5, 0.2, -0.3, 0.1]) + Js = np.arange(42, dtype=float).reshape(6, 7) / 50.0 + return ReturnPacket( + seq=seq, + source_index=10 + seq, + source_time=0.02 * seq, + echoed_map_id=map_id, + residual=np.linspace(-1.0, 1.0, 7), + wrench=wrench, + js_t_wrench=Js.T @ wrench, + qd_slave_actual=np.linspace(0.1, 0.7, 7), + ) + + def test_matched_conditions_share_one_input_hash(self): + registry = MapRegistry() + A = np.eye(7) + registry.add(MapSnapshot(1, 2, 0.04, A)) + packet = self.make_packet() + Jm = np.arange(42, dtype=float).reshape(6, 7) / 70.0 + + differential = map_return_feedback( + kind=MappingKind.MATCHED_DIFFERENTIAL_WRENCH, + packet=packet, + master_jacobian=Jm, + maps=registry, + ) + direct = map_return_feedback( + kind=MappingKind.DIRECT_MASTER_JACOBIAN, + packet=packet, + master_jacobian=Jm, + maps=registry, + ) + + self.assertEqual( + differential.matched_input_hash, direct.matched_input_hash + ) + np.testing.assert_allclose( + differential.tau_master_raw, A.T @ packet.js_t_wrench + ) + np.testing.assert_allclose( + direct.tau_master_raw, Jm.T @ packet.wrench + ) + + def test_source_stamped_policy_uses_echoed_map(self): + registry = MapRegistry() + registry.add(MapSnapshot(1, 0, 0.0, np.eye(7))) + registry.add(MapSnapshot(2, 1, 0.02, 2.0 * np.eye(7))) + packet = self.make_packet(map_id=1) + Jm = np.zeros((6, 7)) + source = map_return_feedback( + kind=MappingKind.MATCHED_DIFFERENTIAL_WRENCH, + packet=packet, + master_jacobian=Jm, + maps=registry, + map_policy=MapPolicy.SOURCE_STAMPED, + ) + current = map_return_feedback( + kind=MappingKind.MATCHED_DIFFERENTIAL_WRENCH, + packet=packet, + master_jacobian=Jm, + maps=registry, + map_policy=MapPolicy.CURRENT, + ) + self.assertEqual(source.selected_map_id, 1) + self.assertEqual(current.selected_map_id, 2) + np.testing.assert_allclose( + current.tau_master_raw, 2.0 * source.tau_master_raw + ) + + def test_power_endpoint_is_zero_for_exact_source_alignment(self): + tau_s = np.array([[1.0, 2.0], [-1.0, 0.5]]) + qd_s = np.array([[0.3, -0.1], [0.2, 0.4]]) + tau_m = tau_s.copy() + qd_m = qd_s.copy() + mismatch = normalized_actual_power_mismatch( + tau_m, qd_m, tau_s, qd_s, np.array([0.01, 0.02]) + ) + self.assertAlmostEqual(mismatch, 0.0) + + def test_duplicate_out_of_order_and_timeout_are_explicit(self): + packets = [ + ForwardPacket(i, i, i * 0.01, i, np.zeros(2), np.zeros(2)) + for i in range(3) + ] + # seq 0 arrives after seq 1, and seq 1 is duplicated. + channel = DeterministicChannel( + [ + NetworkTraceEntry(0, 0.04), + NetworkTraceEntry(1, 0.01, duplicate=True), + NetworkTraceEntry(2, 0.02), + ] + ) + for packet in packets: + channel.send(packet, packet.source_time) + receiver = PacketReceiver(timeout_s=0.05) + receptions = [] + for delivery in channel.poll(0.05): + receptions.append(receiver.accept(delivery)) + + self.assertTrue(receptions[0].accepted) + self.assertIn( + PacketRejectReason.DUPLICATE_OR_STALE, + [item.reason for item in receptions if not item.accepted], + ) + held = receiver.sample(0.051) + self.assertIn(held.state, (PacketState.ACTIVE, PacketState.RECOVERING)) + self.assertEqual(receiver.sample(0.2).state, PacketState.TIMED_OUT) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_haptic_applied_port.py b/code/test/test_haptic_applied_port.py new file mode 100644 index 0000000..e34cbec --- /dev/null +++ b/code/test/test_haptic_applied_port.py @@ -0,0 +1,328 @@ +#!/usr/bin/env python3 +"""Unit tests for final applied-port haptic energy supervision. + +These tests use no URDF, simulator, or Pinocchio model. They exercise the +energy supervisor and the renderer's nominal shaping order directly. +""" + +from __future__ import annotations + +import os +import sys +import unittest + +import numpy as np + +CODE_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "..")) +if CODE_DIR not in sys.path: + sys.path.insert(0, CODE_DIR) + +from core.haptic_render import ( # noqa: E402 + AppliedPortDiagnostics, + AppliedPortEnergySupervisor, + HapticRenderer, + TankParams, +) + + +def make_supervisor(E0: float = 3.0, + E_min: float = 1.0, + E_max: float = 5.0) -> AppliedPortEnergySupervisor: + return AppliedPortEnergySupervisor( + TankParams( + E_min=E_min, + E_max=E_max, + alpha_floor=0.0, + alpha_ceil=1.0, + ), + E0=E0, + ) + + +def make_renderer_state(size: int, + *, + E0: float = 3.0, + feedback_strength: float = 1.0, + torque_limit=None, + torque_rate_limit=None) -> HapticRenderer: + """Construct the pure torque pipeline without a Pinocchio dependency.""" + renderer = object.__new__(HapticRenderer) + renderer.feedback_strength = float(feedback_strength) + renderer.tank = make_supervisor(E0=E0) + renderer.tau_alpha = 1.0 + renderer.torque_limit = torque_limit + renderer.torque_rate_limit = torque_rate_limit + renderer._tau_fb_state = np.zeros(size, dtype=float) + renderer._tau_applied_prev = np.zeros(size, dtype=float) + return renderer + + +class FakeChestJacobian: + def __init__(self, jacobian: np.ndarray): + self.jacobian = np.asarray(jacobian, dtype=float) + + def chest_jacobian(self, _q_m, _qd_m): + return self.jacobian.copy() + + +class AppliedPortEnergySupervisorTest(unittest.TestCase): + def test_low_level_apply_returns_complete_diagnostics(self): + supervisor = make_supervisor(E0=3.0) + candidate = np.array([2.0, -1.0]) + tau_app, diagnostics = supervisor.apply( + candidate, + np.array([0.5, 1.0]), + dt=0.25, + ) + + self.assertIsInstance(diagnostics, AppliedPortDiagnostics) + np.testing.assert_allclose(diagnostics.tau_candidate, candidate) + np.testing.assert_allclose(diagnostics.tau_applied, tau_app) + self.assertAlmostEqual(diagnostics.rho, 1.0) + self.assertAlmostEqual(diagnostics.E_before, 3.0) + self.assertAlmostEqual(diagnostics.E_preclip, 3.0) + self.assertAlmostEqual(diagnostics.candidate_power, 0.0) + self.assertAlmostEqual(diagnostics.power, 0.0) + self.assertAlmostEqual(diagnostics.E_after, 3.0) + self.assertFalse(diagnostics.fail_safe_active) + self.assertIs(supervisor.last_diagnostics, diagnostics) + + def test_positive_power_discharges_once(self): + supervisor = make_supervisor(E0=3.0) + alpha, tau_app = supervisor.project_and_account( + np.array([2.0, 0.0]), + np.array([1.0, 0.0]), + dt=0.25, + ) + + self.assertAlmostEqual(alpha, 1.0) + np.testing.assert_allclose(tau_app, [2.0, 0.0]) + self.assertAlmostEqual(supervisor.last_power, 2.0) + self.assertAlmostEqual(supervisor.E, 2.5) + + def test_negative_power_charges_once(self): + supervisor = make_supervisor(E0=3.0) + alpha, tau_app = supervisor.project_and_account( + np.array([-2.0, 1.0]), + np.array([1.0, 0.0]), + dt=0.25, + ) + + self.assertAlmostEqual(alpha, 1.0) + np.testing.assert_allclose(tau_app, [-2.0, 1.0]) + self.assertAlmostEqual(supervisor.last_power, -2.0) + self.assertAlmostEqual(supervisor.E, 3.5) + + def test_zero_power_does_not_change_energy_or_torque(self): + supervisor = make_supervisor(E0=3.0) + candidate = np.array([2.0, -1.0]) + alpha, tau_app = supervisor.project_and_account( + candidate, + np.zeros(2), + dt=0.25, + ) + + self.assertAlmostEqual(alpha, 1.0) + np.testing.assert_allclose(tau_app, candidate) + self.assertAlmostEqual(supervisor.last_power, 0.0) + self.assertAlmostEqual(supervisor.E, 3.0) + + def test_energy_projection_hits_E_min_exactly(self): + supervisor = make_supervisor(E0=1.25) + alpha, tau_app = supervisor.project_and_account( + np.array([2.0]), + np.array([1.0]), + dt=0.5, + ) + + self.assertAlmostEqual(alpha, 0.25) + np.testing.assert_allclose(tau_app, [0.5]) + self.assertAlmostEqual(supervisor.last_power, 0.5) + self.assertAlmostEqual(supervisor.E, 1.0) + + alpha_at_min, tau_at_min = supervisor.project_and_account( + np.array([4.0]), + np.array([1.0]), + dt=0.1, + ) + self.assertAlmostEqual(alpha_at_min, 0.0) + np.testing.assert_allclose(tau_at_min, [0.0]) + self.assertAlmostEqual(supervisor.E, 1.0) + + def test_nonpositive_dt_uses_zero_fail_safe_without_accounting(self): + supervisor = make_supervisor(E0=3.0) + alpha, tau_app = supervisor.project_and_account( + np.array([2.0]), + np.array([1.0]), + dt=0.0, + ) + + self.assertAlmostEqual(alpha, 0.0) + np.testing.assert_allclose(tau_app, [0.0]) + self.assertTrue(supervisor.fail_safe_active) + self.assertAlmostEqual(supervisor.E, 3.0) + + +class HapticRendererPipelineTest(unittest.TestCase): + def test_external_supervisor_candidate_requires_explicit_commit(self): + renderer = make_renderer_state( + 1, + torque_limit=2.0, + torque_rate_limit=10.0, + ) + candidate, valid = renderer.shape_mapped_reaction_candidate( + np.array([-10.0]), + qd_m=np.zeros(1), + dt=0.1, + ) + self.assertTrue(valid) + np.testing.assert_allclose(candidate, [-1.0]) + np.testing.assert_allclose(renderer._tau_applied_prev, [0.0]) + renderer.commit_applied(np.array([0.25])) + np.testing.assert_allclose(renderer._tau_applied_prev, [0.25]) + + def test_generalized_reaction_keeps_environment_on_device_sign(self): + renderer = make_renderer_state(2) + reaction = np.array([-2.0, 1.0]) + + tau_app, rho = renderer.render_mapped_reaction( + reaction, + qd_m=np.zeros(2), + dt=0.01, + ) + + np.testing.assert_allclose(tau_app, reaction) + self.assertAlmostEqual(rho, 1.0) + + def test_direct_baseline_is_exact_J_transpose_F(self): + renderer = object.__new__(HapticRenderer) + jacobian = np.arange(12, dtype=float).reshape(6, 2) / 10.0 + renderer.CJ_master = FakeChestJacobian(jacobian) + wrench = np.array([1.0, -2.0, 0.5, 3.0, -1.0, 2.0]) + + tau_direct = renderer.direct_from_CF( + q_m=np.zeros(2), + qd_m=np.zeros(2), + CF_int_slave_C=wrench, + ) + + np.testing.assert_allclose(tau_direct, jacobian.T @ wrench) + + def test_rate_limit_precedes_energy_projection(self): + renderer = make_renderer_state( + 2, + torque_rate_limit=np.array([2.0, 1.0]), + ) + + tau_step_1, alpha_1 = renderer._shape_and_supervise( + tau_direct=np.array([-10.0, 10.0]), + qd_m=np.zeros(2), + dt=0.1, + ) + tau_step_2, alpha_2 = renderer._shape_and_supervise( + tau_direct=np.array([-10.0, 10.0]), + qd_m=np.zeros(2), + dt=0.1, + ) + + np.testing.assert_allclose(tau_step_1, [0.2, -0.1]) + np.testing.assert_allclose(tau_step_2, [0.4, -0.2]) + self.assertAlmostEqual(alpha_1, 1.0) + self.assertAlmostEqual(alpha_2, 1.0) + self.assertTrue(renderer.last_rate_limit_active) + self.assertFalse(renderer.last_torque_saturation_active) + np.testing.assert_allclose( + renderer.tank.last_tau_candidate, tau_step_2 + ) + + def test_saturation_precedes_energy_projection(self): + renderer = make_renderer_state( + 2, + torque_limit=np.array([3.0, 4.0]), + ) + + tau_app, alpha = renderer._shape_and_supervise( + tau_direct=np.array([-10.0, 10.0]), + qd_m=np.zeros(2), + dt=0.1, + ) + + self.assertAlmostEqual(alpha, 1.0) + np.testing.assert_allclose(tau_app, [3.0, -4.0]) + self.assertFalse(renderer.last_rate_limit_active) + self.assertTrue(renderer.last_torque_saturation_active) + np.testing.assert_allclose( + renderer.tank.last_tau_candidate, [3.0, -4.0] + ) + + def test_projection_is_last_and_uses_saturated_candidate(self): + renderer = make_renderer_state( + 1, + E0=1.1, + torque_limit=2.0, + ) + + tau_app, alpha = renderer._shape_and_supervise( + tau_direct=np.array([-10.0]), + qd_m=np.array([1.0]), + dt=1.0, + ) + + # Direct -> nominal +10 -> saturation +2 -> energy projection +0.1. + self.assertAlmostEqual(alpha, 0.05) + np.testing.assert_allclose(renderer.tank.last_tau_candidate, [2.0]) + np.testing.assert_allclose(renderer.tank.last_tau_applied, [0.1]) + np.testing.assert_allclose(tau_app, renderer.tank.last_tau_applied) + np.testing.assert_allclose(renderer._tau_applied_prev, tau_app) + self.assertAlmostEqual(renderer.tank.last_power, float(tau_app[0])) + self.assertAlmostEqual(renderer.tank.E, 1.0) + + def test_stepwise_offline_energy_reconstruction_matches_exactly(self): + renderer = make_renderer_state( + 2, + E0=2.5, + feedback_strength=1.5, + torque_limit=np.array([2.0, 1.5]), + torque_rate_limit=np.array([4.0, 3.0]), + ) + dt = 0.1 + direct_sequence = [ + np.array([-2.0, 1.0]), + np.array([-4.0, 2.0]), + np.array([1.0, -2.0]), + np.array([0.0, 0.0]), + np.array([-5.0, -5.0]), + np.array([2.0, 2.0]), + ] + velocity_sequence = [ + np.array([1.0, 0.5]), + np.array([0.7, -0.4]), + np.array([1.0, 1.0]), + np.zeros(2), + np.array([-0.6, -0.2]), + np.array([0.5, -0.8]), + ] + + energy_offline = 2.5 + for tau_direct, qd_m in zip(direct_sequence, velocity_sequence): + tau_app, _ = renderer._shape_and_supervise( + tau_direct=tau_direct, + qd_m=qd_m, + dt=dt, + ) + applied_power = float(np.dot(tau_app, qd_m)) + energy_offline = float(np.clip( + energy_offline - applied_power * dt, + renderer.tank.tp.E_min, + renderer.tank.tp.E_max, + )) + + np.testing.assert_allclose( + renderer.tank.last_tau_applied, tau_app + ) + self.assertAlmostEqual(renderer.tank.last_power, applied_power) + self.assertAlmostEqual(renderer.tank.E, energy_offline, places=14) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_haptic_render.py b/code/test/test_haptic_render.py new file mode 100644 index 0000000..e0f4fc9 --- /dev/null +++ b/code/test/test_haptic_render.py @@ -0,0 +1,309 @@ +# -*- coding: utf-8 -*- +""" +测试脚本：验证 InteractionEstimator + HapticRenderer 的正确性， +并绘制论文中使用的误差和能量罐曲线。 + +运行： + python test_haptic_render.py + +依赖： + - pinocchio + - numpy + - matplotlib + - omegaconf +""" + +import numpy as np +import pinocchio as pin +from omegaconf import OmegaConf +import matplotlib.pyplot as plt + +import os, sys +sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) +from core.interaction_estimater import InteractionEstimator +from core.haptic_render import HapticRenderer, TankParams + + +def moving_average(x, window: int = 7): + """简单滑动平均，用于平滑 alpha 等曲线。""" + if window <= 1: + return x + kernel = np.ones(window, dtype=float) / float(window) + # 使用 same 保持长度一致 + return np.convolve(x, kernel, mode="same") + + +def build_model_with_frames(): + """ + 备用：构造一个 6-DoF 示例机械臂，并添加： + - chest_link: 挂在关节1 + - ee_link: 挂在末端关节 + 目前脚本直接从 URDF 加载，不一定用得到。 + """ + model = pin.buildSampleModelManipulator() + chest_joint_id = 1 + chest_frame_name = "chest_link" + model.addFrame(pin.Frame( + chest_frame_name, + chest_joint_id, + chest_joint_id, + pin.SE3.Identity(), + pin.FrameType.OP_FRAME + )) + ee_joint_id = model.njoints - 1 + ee_frame_name = "ee_link" + model.addFrame(pin.Frame( + ee_frame_name, + ee_joint_id, + ee_joint_id, + pin.SE3.Identity(), + pin.FrameType.OP_FRAME + )) + return model, chest_frame_name, ee_frame_name + + +def main(): + conf = OmegaConf.load("./config/config.yaml") + np.random.seed(0) + + # 1) 构造主/从模型（这里用真实 URDF） + master_model, _, _ = pin.buildModelsFromUrdf(str(conf.master_urdf)) + slave_model, _, _ = pin.buildModelsFromUrdf(str(conf.slave_urdf)) + + # 从端交互估计器 + est_slave = InteractionEstimator( + slave_model, + chest_frame_name=conf.s_base_frame, + ee_frame_name=conf.s_ee_frame, + lambda_damp=conf.interaction_est.lambda_damp, + ) + + # 主端渲染器 + renderer = HapticRenderer( + master_model, + chest_frame_name=conf.m_base_frame, + ee_frame_name=conf.m_ee_frame, + feedback_strength=conf.haptic_render.feedback_strength, + E_init=conf.haptic_render.E_init, + E_max=conf.haptic_render.E_max, + alpha_floor=conf.haptic_render.alpha_floor, + alpha_ceil=conf.haptic_render.alpha_ceil, + E0=conf.haptic_render.E0, + ) + + nq_s, nv_s = slave_model.nq, slave_model.nv + nq_m, nv_m = master_model.nq, master_model.nv + + dt = 0.002 + n_steps = 500 # 足够画出平滑曲线 + + print("========== TEST START ==========") + print(f"slave nq={nq_s}, master nq={nq_m}, dt={dt}s, steps={n_steps}") + print("--------------------------------") + + # -------- 统计量 / 曲线数据 -------- + times = [] + err_tau_int_hist = [] + err_CF_hist = [] + err_tau_fb_hist = [] + E_hist = [] + alpha_hist = [] + + max_err_tau_int = 0.0 + max_err_CF = 0.0 + max_err_tau_fb = 0.0 + + for k in range(n_steps): + t = k * dt + times.append(t) + + # 2) 随机生成从端状态（可改成真实记录或特定轨迹） + q_s = 0.2 * (np.random.rand(nq_s) - 0.5) # [-0.1,0.1] + qd_s = 0.1 * (np.random.rand(nv_s) - 0.5) + qdd_s = np.zeros_like(qd_s) + + # 从端模型扭矩（M qdd + C qd + g） + tau_model_s = est_slave._tau_model(q_s, qd_s, qdd_s, tau_ff_fric=None) + + # 从端胸腔雅可比 C J_s + CJ_s = est_slave._chest_jacobian(q_s, qd_s) # 6 x nv_s + + # 3) 人为构造“真实”交互扳手 C F_true + # 也可以改成随时间变化的模式，例如正弦。 + CF_true = np.array([10.0, 0.0, 0.0, # 10 N 沿 Cx + 0.0, 0.0, 0.0]) + + # 真 · 交互关节力矩 + tau_int_true = CJ_s.T @ CF_true + + # 构造测量力矩：tau_meas = tau_model + tau_int_true + tau_meas_s = tau_model_s + tau_int_true + + # 4) 调用 InteractionEstimator 估计 + tau_int_est, CF_int_est, CJ_s_est = est_slave.estimate( + q_s, qd_s, qdd_s, + tau_meas=tau_meas_s, + tau_ff_fric=None + ) + + # 从端末端在 C 系的速度扭量（用于能量罐功率） + V_slave_C = CJ_s @ qd_s + + # 5) 主端状态（此处用从端状态代替，仅做算法验证） + q_m = q_s.copy() + qd_m = qd_s.copy() + + tau_fb_m_est, alpha = renderer.render_from_CF( + q_m, qd_m, + CF_int_slave_C=CF_int_est, + V_slave_C=V_slave_C, + dt=dt + ) + + # 主端胸腔雅可比，用于构造“真 · 反馈扭矩”（不经过能量罐） + CJ_m = renderer.CJ_master.chest_jacobian(q_m, qd_m) + tau_fb_m_true = CJ_m.T @ CF_true + + # -------- 误差计算 -------- + err_tau_int = np.linalg.norm(tau_int_est - tau_int_true) + err_CF = np.linalg.norm(CF_int_est - CF_true) + err_tau_fb = np.linalg.norm(tau_fb_m_est - tau_fb_m_true) + + err_tau_int_hist.append(err_tau_int) + err_CF_hist.append(err_CF) + err_tau_fb_hist.append(err_tau_fb) + + max_err_tau_int = max(max_err_tau_int, err_tau_int) + max_err_CF = max(max_err_CF, err_CF) + max_err_tau_fb = max(max_err_tau_fb, err_tau_fb) + + # 能量罐状态 + E_hist.append(renderer.tank.E) + alpha_hist.append(alpha) + + # ---- 每隔若干步打印一次 ---- + if k % 100 == 0: + print(f"\n--- Step {k} (t = {t:.3f} s) ---") + print(f"‣ ||tau_int_true|| = {np.linalg.norm(tau_int_true):.4e}") + print(f" ||tau_int_est - tau_int_true|| = {err_tau_int:.4e}") + print(f"‣ ||CF_true|| = {np.linalg.norm(CF_true):.4e}") + print(f" ||CF_int_est - CF_true|| = {err_CF:.4e}") + print(f"‣ ||tau_fb_m_true|| = {np.linalg.norm(tau_fb_m_true):.4e}") + print(f" ||tau_fb_m_est - tau_fb_m_true|| = {err_tau_fb:.4e}") + print(f" Energy tank: E = {renderer.tank.E:.4f}, alpha = {alpha:.4f}") + + # ---------- 数值结果摘要 ---------- + print("\n========== SUMMARY ==========") + print(f"max ||tau_int_est - tau_int_true|| = {max_err_tau_int:.4e}") + print(f"max ||CF_int_est - CF_true|| = {max_err_CF:.4e}") + print(f"max ||tau_fb_est - tau_fb_true|| = {max_err_tau_fb:.4e}") + + tol_tau_int = 1e-3 + tol_CF = 1e-3 + tol_tau_fb = 1e-3 + if max_err_tau_int < tol_tau_int and max_err_CF < tol_CF and max_err_tau_fb < tol_tau_fb: + print("[PASS] 所有误差均在容许范围内。") + else: + print("[WARN] 误差超出阈值，请检查算法或考虑调小阻尼 lambda_damp。") + + # ===================================================== + # 绘图部分（适合论文呈现） + # ===================================================== + + times = np.array(times) + err_tau_int_hist = np.array(err_tau_int_hist) + err_CF_hist = np.array(err_CF_hist) + err_tau_fb_hist = np.array(err_tau_fb_hist) + E_hist = np.array(E_hist) + alpha_hist = np.array(alpha_hist) + + # 对 alpha 做轻微平滑（论文图更清晰） + alpha_smooth = moving_average(alpha_hist, window=7) + + # ------------------ 误差子图（3 个独立子图） ------------------ + fig_err, (ax1, ax2, ax3) = plt.subplots( + 3, 1, sharex=True, figsize=(6, 7) + ) + + # (a) Interaction torque estimation error + ax1.plot( + times, + err_tau_int_hist, + color="C0", + label=r"$\|\tau_{\mathrm{int}}^{\mathrm{est}}-\tau_{\mathrm{int}}^{\mathrm{true}}\|$", + ) + ax1.set_title("(a) Interaction torque estimation error") + ax1.set_ylabel(r"Error norm $\|\cdot\|$") + ax1.set_yscale("log") + ax1.grid(True, linestyle="--", linewidth=0.5) + ax1.legend(loc="lower right") + + # (b) Interaction wrench estimation error + ax2.plot( + times, + err_CF_hist, + color="C1", + label=r"$\|{}^{C}F_{\mathrm{int}}^{\mathrm{est}}-{}^{C}F_{\mathrm{true}}\|$", + ) + ax2.set_title("(b) Interaction wrench estimation error") + ax2.set_ylabel(r"Error norm $\|\cdot\|$") + ax2.set_yscale("log") + ax2.grid(True, linestyle="--", linewidth=0.5) + ax2.legend(loc="lower right") + + # (c) Feedback torque rendering error + ax3.plot( + times, + err_tau_fb_hist, + color="C2", + label=r"$\|\tau_{\mathrm{fb}}^{\mathrm{est}}-\tau_{\mathrm{fb}}^{\mathrm{true}}\|$", + ) + ax3.set_title("(c) Haptic feedback torque error") + ax3.set_xlabel(r"Time $t$ [s]") + ax3.set_ylabel(r"Error norm $\|\cdot\|$") + ax3.set_yscale("log") + ax3.grid(True, linestyle="--", linewidth=0.5) + ax3.legend(loc="lower right") + + fig_err.tight_layout() + # fig_err.savefig("figs/haptic_errors_3subplots.png", dpi=300, bbox_inches="tight") + + # ------------------ 能量罐 + 缩放系数 ------------------ + fig_tank, ax1_t = plt.subplots(figsize=(6, 4)) + + ax1_t.set_title("Energy tank dynamics and scaling factor") + + l1 = ax1_t.plot( + times, + E_hist, + color="C0", + label=r"Tank energy $E[k]$", + ) + ax1_t.set_xlabel(r"Time $t$ [s]") + ax1_t.set_ylabel(r"Energy $E$ [J]") + ax1_t.grid(True, linestyle="--", linewidth=0.5) + + ax2_t = ax1_t.twinx() + l2 = ax2_t.plot( + times, + alpha_smooth, + color="C1", + linestyle="--", + label=r"Scaling $\alpha[k]$ (smoothed)", + ) + ax2_t.set_ylabel(r"Scaling factor $\alpha$") + + # Legend 合并并右下角防遮挡 + lines = l1 + l2 + labels = [l.get_label() for l in lines] + ax1_t.legend(lines, labels, loc="lower right") + + fig_tank.tight_layout() + # fig_tank.savefig("figs/haptic_tank_smooth.png", dpi=300, bbox_inches="tight") + + plt.show() + + + +if __name__ == "__main__": + main() diff --git a/code/test/test_int_estimater.py b/code/test/test_int_estimater.py new file mode 100644 index 0000000..ffbb752 --- /dev/null +++ b/code/test/test_int_estimater.py @@ -0,0 +1,374 @@ +# -*- coding: utf-8 -*- +import os, sys, numpy as np +import pinocchio as pin +from omegaconf import OmegaConf + + +sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) +from core.interaction_estimater import InteractionEstimator + + +# —— 误差函数（更鲁棒的相对误差 + 绝对误差）—— +def rel_err(a, b, eps=1e-9): + """对称式相对误差：‖a-b‖ / max(0.5(‖a‖+‖b‖), eps)""" + na, nb = np.linalg.norm(a), np.linalg.norm(b) + denom = max(0.5 * (na + nb), eps) + return np.linalg.norm(a - b) / denom + + +def abs_err(a, b): + """绝对误差：‖a-b‖""" + return np.linalg.norm(a - b) + + +# —— 固定范数的扳手采样（方向随机）—— +def sample_wrench_fixed_norm(F_lin_norm=10.0, F_ang_norm=2.0, rng=None): + """ + 在线性部分和角部分分别固定范数，方向随机的 6D 扳手采样 + """ + rng = np.random.default_rng() if rng is None else rng + v = rng.standard_normal(6) + f = v[:3] + m = v[3:] + f = f / (np.linalg.norm(f) + 1e-12) * F_lin_norm + m = m / (np.linalg.norm(m) + 1e-12) * F_ang_norm + return np.hstack([f, m]) + + +def load_model(urdf_path: str) -> pin.Model: + assert os.path.exists(urdf_path), f"URDF not found: {urdf_path}" + model = pin.buildModelFromUrdf(urdf_path) + print(f"[URDF] model loaded: nq={model.nq}, nv={model.nv}, njoints={model.njoints}") + return model + + +def oracle_test(model, + chest_frame: str = 'slave_shoulder', + ee_frame: str = 'slave_ee', + n_trials: int = 300, + noise_tau_std: float = 0.0, + lambda_damp: float = 1e-6, + seed: int = 11, + sv_min_skip: float = 1e-6): + """ + 使用 InteractionEstimator 的“理想/带噪声”仿真： + - 通过动力学模型生成 tau_model + - 叠加胸腔系扳手 CJ^T F_true 和测量噪声得到 tau_meas + - 调用 est.estimate 恢复 tau_int, F_est + - 统计相对/绝对误差的均值、方差、p90 + """ + rng = np.random.default_rng(seed) + est = InteractionEstimator(model, chest_frame, ee_frame, lambda_damp=lambda_damp) + data = model.createData() + + errs_rel, errs_abs = [], [] + kept = 0 + for k in range(n_trials): + # 随机状态 + q = pin.randomConfiguration(model) + qd = np.random.randn(model.nv) * 0.1 + qdd = np.random.randn(model.nv) * 0.2 + + # 动力学力矩 tau_model + M = pin.crba(model, data, q) + M = (M + M.T) - np.diag(M.diagonal()) + nle = pin.nonLinearEffects(model, data, q, qd) + g = pin.computeGeneralizedGravity(model, data, q) + Cqd = nle - g + tau_model = M @ qdd + Cqd + g + + # 胸腔系雅可比 CJ + CJ = est._chest_jacobian(q, qd) # 6 x nv + svals = np.linalg.svd(CJ, compute_uv=False) + if svals[-1] < sv_min_skip: + # 跳过奇异邻域样本 + continue + + # 固定范数扳手 + CF_true = sample_wrench_fixed_norm(10.0, 2.0, rng=rng) + + # 合成“测得力矩” tau_meas = tau_model + CJ^T F_true + noise + tau_meas = tau_model + CJ.T @ CF_true + np.random.randn(model.nv) * noise_tau_std + + # 利用 estimator 做估计 + tau_int, CF_est, CJ_check = est.estimate(q, qd, qdd, tau_meas) + + # 误差统计 + errs_rel.append(rel_err(CF_est, CF_true)) + errs_abs.append(abs_err(CF_est, CF_true)) + kept += 1 + + # 一致性：回投误差 & 功率一致性（抽样打印） + if k % 50 == 0: + back_err = rel_err(CJ_check.T @ CF_est, tau_int) + v_C = CJ @ qd + Pq = float(tau_int @ qd) + Pv = float(CF_true @ v_C) + print( + f"[{k:03d}] rel={errs_rel[-1]:.3e}, abs={errs_abs[-1]:.3e}, " + f"back={back_err:.3e}, power={abs(Pq - Pv):.3e}, sv_min={svals[-1]:.2e}" + ) + + errs_rel = np.array(errs_rel) + errs_abs = np.array(errs_abs) + if kept == 0: + print("[Oracle] all samples skipped by sv_min filter; try smaller sv_min_skip.") + # 返回空 summary + return dict( + errs_rel=errs_rel, + errs_abs=errs_abs, + kept=0, + mean_rel=np.nan, + std_rel=np.nan, + p90_rel=np.nan, + mean_abs=np.nan, + std_abs=np.nan, + p90_abs=np.nan, + ) + + mean_rel = float(errs_rel.mean()) + std_rel = float(errs_rel.std()) + p90_rel = float(np.percentile(errs_rel, 90)) + + mean_abs = float(errs_abs.mean()) + std_abs = float(errs_abs.std()) + p90_abs = float(np.percentile(errs_abs, 90)) + + print( + f"[Oracle REL] mean={mean_rel:.3e}, std={std_rel:.3e}, " + f"median={np.median(errs_rel):.3e}, p90={p90_rel:.3e}" + ) + print( + f"[Oracle ABS] mean={mean_abs:.3e}, std={std_abs:.3e}, " + f"median={np.median(errs_abs):.3e}, p90={p90_abs:.3e}" + ) + + summary = dict( + errs_rel=errs_rel, + errs_abs=errs_abs, + kept=kept, + mean_rel=mean_rel, + std_rel=std_rel, + p90_rel=p90_rel, + mean_abs=mean_abs, + std_abs=std_abs, + p90_abs=p90_abs, + ) + return summary + + +def oracle_test_baseline(model, + chest_frame: str = 'slave_shoulder', + ee_frame: str = 'slave_ee', + n_trials: int = 300, + noise_tau_std: float = 0.05, + seed: int = 17, + sv_min_skip: float = 1e-6): + """ + 基线：不调用 InteractionEstimator 的残差/阻尼公式， + 直接用未阻尼伪逆解胸腔扳手： + + tau_int_true = tau_meas - tau_model + F_est = (CJ CJ^T)^{-1} CJ tau_int_true + + 用来对比在同样噪声/配置下的数值稳定性。 + """ + rng = np.random.default_rng(seed) + est_tmp = InteractionEstimator(model, chest_frame, ee_frame, lambda_damp=0.0) + data = model.createData() + + errs_rel, errs_abs = [], [] + kept = 0 + for k in range(n_trials): + # 随机状态 + q = pin.randomConfiguration(model) + qd = np.random.randn(model.nv) * 0.1 + qdd = np.random.randn(model.nv) * 0.2 + + # 动力学力矩 tau_model + M = pin.crba(model, data, q) + M = (M + M.T) - np.diag(M.diagonal()) + nle = pin.nonLinearEffects(model, data, q, qd) + g = pin.computeGeneralizedGravity(model, data, q) + Cqd = nle - g + tau_model = M @ qdd + Cqd + g + + # 胸腔雅可比 CJ + CJ = est_tmp._chest_jacobian(q, qd) + svals = np.linalg.svd(CJ, compute_uv=False) + if svals[-1] < sv_min_skip: + continue + + # 固定范数扳手（与 oracle_test 相同范数） + CF_true = sample_wrench_fixed_norm(10.0, 2.0, rng=rng) + + # 测得力矩（含噪声） + tau_meas = tau_model + CJ.T @ CF_true + np.random.randn(model.nv) * noise_tau_std + + # “真实”关节残差力矩（在仿真中我们知道 tau_model） + tau_int_true = tau_meas - tau_model + + # 未阻尼伪逆 (CJ CJ^T)^{-1} CJ tau_int_true + JJt = CJ @ CJ.T + try: + F_est = np.linalg.solve(JJt, CJ @ tau_int_true) + except np.linalg.LinAlgError: + # 数值奇异，跳过该样本 + continue + + errs_rel.append(rel_err(F_est, CF_true)) + errs_abs.append(abs_err(F_est, CF_true)) + kept += 1 + + if k % 50 == 0: + back_err = rel_err(CJ.T @ F_est, tau_int_true) + print( + f"[BASE {k:03d}] rel={errs_rel[-1]:.3e}, abs={errs_abs[-1]:.3e}, " + f"back={back_err:.3e}, sv_min={svals[-1]:.2e}" + ) + + errs_rel = np.array(errs_rel) + errs_abs = np.array(errs_abs) + if kept == 0: + print("[Baseline] all samples skipped by sv_min filter; try smaller sv_min_skip.") + return dict( + errs_rel=errs_rel, + errs_abs=errs_abs, + kept=0, + mean_rel=np.nan, + std_rel=np.nan, + p90_rel=np.nan, + mean_abs=np.nan, + std_abs=np.nan, + p90_abs=np.nan, + ) + + mean_rel = float(errs_rel.mean()) + std_rel = float(errs_rel.std()) + p90_rel = float(np.percentile(errs_rel, 90)) + + mean_abs = float(errs_abs.mean()) + std_abs = float(errs_abs.std()) + p90_abs = float(np.percentile(errs_abs, 90)) + + print( + f"[BASELINE REL] mean={mean_rel:.3e}, std={std_rel:.3e}, " + f"median={np.median(errs_rel):.3e}, p90={p90_rel:.3e}" + ) + print( + f"[BASELINE ABS] mean={mean_abs:.3e}, std={std_abs:.3e}, " + f"median={np.median(errs_abs):.3e}, p90={p90_abs:.3e}" + ) + + summary = dict( + errs_rel=errs_rel, + errs_abs=errs_abs, + kept=kept, + mean_rel=mean_rel, + std_rel=std_rel, + p90_rel=p90_rel, + mean_abs=mean_abs, + std_abs=std_abs, + p90_abs=p90_abs, + ) + return summary + + +def consistency_sweep(model, + chest_frame: str = 'slave_shoulder', + ee_frame: str = 'slave_ee'): + """ + 对不同阻尼系数 lambda 进行一个小 sweep，查看： + - 估计扳手范数 + - 回投误差 + - cond(JJ^T) + """ + est_ref = InteractionEstimator(model, chest_frame, ee_frame, lambda_damp=1e-3) + q = pin.randomConfiguration(model) + qd = np.random.randn(model.nv) * 0.05 + qdd = np.zeros(model.nv) + + CJ = est_ref._chest_jacobian(q, qd) + CF_true = np.array([5.0, -3.0, 8.0, 0.5, 0.2, -0.1]) + + M = pin.crba(model, est_ref.data, q) + M = (M + M.T) - np.diag(M.diagonal()) + nle = pin.nonLinearEffects(model, est_ref.data, q, qd) + g = pin.computeGeneralizedGravity(model, est_ref.data, q) + Cqd = nle - g + tau_model = M @ qdd + Cqd + g + tau_meas = tau_model + CJ.T @ CF_true + + lambdas = [1e-6, 3e-6, 1e-5, 3e-5, 1e-4, 3e-4, 1e-3, 3e-3, 1e-2] + print("\n[Lambda sweep] λ, ‖F_est‖, back_err, cond(JJᵀ)") + for lam in lambdas: + est = InteractionEstimator(model, chest_frame, ee_frame, lambda_damp=lam) + tau_int, CF_est, CJ_use = est.estimate(q, qd, qdd, tau_meas) + back_err = rel_err(CJ_use.T @ CF_est, tau_int) + JJt = CJ_use @ CJ_use.T + cond = np.linalg.cond(JJt) if np.linalg.matrix_rank(JJt) == 6 else np.inf + print(f"{lam:8.1e} {np.linalg.norm(CF_est):8.3f} {back_err:8.2e} {cond:8.2e}") + + +def main(): + conf = OmegaConf.load("./config/config.yaml") + urdf_path = getattr(conf, "slave_urdf", None) + model = load_model(urdf_path) + + # 注意：chest_frame 在论文中记为 C 框架，这里保持一致 + chest_frame = "slave_base" # 或 "slave_shoulder"，视你的 URDF 定义而定 + ee_frame = "slave_ee" + + print("\n=== ORACLE (no noise, λ=1e-6) ===") + summary_ideal = oracle_test( + model, chest_frame, ee_frame, + n_trials=300, noise_tau_std=0.0, lambda_damp=1e-6, + seed=11, sv_min_skip=1e-6 + ) + + print("\n=== PROPOSED (tau noise 0.05 N·m, λ=1e-3) ===") + summary_proposed = oracle_test( + model, chest_frame, ee_frame, + n_trials=300, noise_tau_std=0.05, lambda_damp=1e-3, + seed=13, sv_min_skip=1e-6 + ) + + print("\n=== BASELINE (tau noise 0.05 N·m, undamped pseudoinverse) ===") + summary_baseline = oracle_test_baseline( + model, chest_frame, ee_frame, + n_trials=300, noise_tau_std=0.05, + seed=17, sv_min_skip=1e-6 + ) + + # 打印一个 markdown 风格的小表格，方便直接贴到论文 + def to_percent(x): + return 100.0 * x if x is not None and not np.isnan(x) else float("nan") + + print("\n=== SUMMARY (relative wrench error e_F) ===") + print("| Method | mean e_F [%] | std e_F [%] | p90 e_F [%] | Kept |") + print("|------------------------|-------------:|------------:|------------:|-----:|") + print( + f"| Ideal (no noise) | {to_percent(summary_ideal['mean_rel']):11.2f} | " + f"{to_percent(summary_ideal['std_rel']):11.2f} | " + f"{to_percent(summary_ideal['p90_rel']):11.2f} | " + f"{summary_ideal['kept']:4d} |" + ) + print( + f"| Proposed (damped) | {to_percent(summary_proposed['mean_rel']):11.2f} | " + f"{to_percent(summary_proposed['std_rel']):11.2f} | " + f"{to_percent(summary_proposed['p90_rel']):11.2f} | " + f"{summary_proposed['kept']:4d} |" + ) + print( + f"| Baseline (PI, undamp) | {to_percent(summary_baseline['mean_rel']):11.2f} | " + f"{to_percent(summary_baseline['std_rel']):11.2f} | " + f"{to_percent(summary_baseline['p90_rel']):11.2f} | " + f"{summary_baseline['kept']:4d} |" + ) + + # 可选：扫 lambda，看条件数/回投误差（和论文里 Fig. 6(a)(b) 对应） + consistency_sweep(model, chest_frame, ee_frame) + + +if __name__ == "__main__": + main() diff --git a/code/test/test_interaction_estimator_contract.py b/code/test/test_interaction_estimator_contract.py new file mode 100644 index 0000000..6ff2391 --- /dev/null +++ b/code/test/test_interaction_estimator_contract.py @@ -0,0 +1,197 @@ +from __future__ import annotations + +import sys +import unittest +from pathlib import Path + +import numpy as np +import pinocchio as pin + + +CODE_DIR = Path(__file__).resolve().parents[1] +if str(CODE_DIR) not in sys.path: + sys.path.insert(0, str(CODE_DIR)) + +from core.interaction_estimater import ( # noqa: E402 + InteractionEstimator, + checked_frame_id, + checked_joint_id, +) + + +MASTER_URDF = CODE_DIR / "config" / "master_7dof.urdf" +GENERAL_Q = np.array([0.3, -0.5, 0.4, 0.8, -0.2, 0.6, -0.4]) + + +class InteractionEstimatorContractTest(unittest.TestCase): + def setUp(self) -> None: + self.model = pin.buildModelFromUrdf(str(MASTER_URDF)) + + def test_pinocchio_not_found_sentinels_are_rejected(self) -> None: + model = self.model + self.assertLess(checked_frame_id(model, "master_ee"), model.nframes) + self.assertLess( + checked_joint_id(model, "master_elbow_flex_joint"), model.njoints + ) + + with self.assertRaisesRegex(ValueError, "Frame not found"): + InteractionEstimator(model, "missing_chest", "master_ee") + with self.assertRaisesRegex(ValueError, "Frame not found"): + InteractionEstimator(model, "master_base", "missing_ee") + with self.assertRaisesRegex(ValueError, "Movable joint not found"): + checked_joint_id(model, "missing_joint") + + def test_virtual_work_is_preserved_at_the_ee_point(self) -> None: + model = self.model + estimator = InteractionEstimator( + model, "master_forearm", "master_ee", lambda_damp=1e-4 + ) + joint_velocity = np.array([0.2, -0.1, 0.3, 0.4, -0.2, 0.1, 0.5]) + wrench_chest = np.array([8.0, -3.0, 5.0, 0.7, -0.4, 0.2]) + + jacobian_chest = estimator._chest_jacobian(GENERAL_Q, joint_velocity) + tau_external = jacobian_chest.T @ wrench_chest + twist_chest = jacobian_chest @ joint_velocity + + np.testing.assert_allclose( + tau_external @ joint_velocity, + wrench_chest @ twist_chest, + rtol=1e-13, + atol=1e-13, + ) + + def test_chest_axis_rotation_matches_lwa_jacobian(self) -> None: + model = self.model + estimator = InteractionEstimator( + model, "master_forearm", "master_ee", lambda_damp=1e-4 + ) + joint_velocity = np.array([0.1, 0.2, -0.3, 0.05, 0.4, -0.2, 0.1]) + + jacobian_chest = estimator._chest_jacobian(GENERAL_Q, joint_velocity) + jacobian_lwa = pin.computeFrameJacobian( + model, + estimator.data, + GENERAL_Q, + estimator.fid_EE, + pin.ReferenceFrame.LOCAL_WORLD_ALIGNED, + ) + rotation_world_from_chest = estimator.data.oMf[estimator.fid_C].rotation + rotation_chest_from_world = rotation_world_from_chest.T + rotation6 = estimator._rotation6(rotation_chest_from_world) + + np.testing.assert_allclose( + jacobian_chest, rotation6 @ jacobian_lwa, rtol=1e-13, atol=1e-13 + ) + + wrench_chest = np.array([4.0, -2.0, 6.0, 0.3, 0.5, -0.1]) + wrench_world = rotation6.T @ wrench_chest + np.testing.assert_allclose( + jacobian_chest.T @ wrench_chest, + jacobian_lwa.T @ wrench_world, + rtol=1e-13, + atol=1e-13, + ) + + def test_bias_corrected_ideal_wrench_is_recovered(self) -> None: + model = self.model + estimator = InteractionEstimator( + model, "master_base", "master_ee", lambda_damp=1e-7 + ) + qd = np.array([0.08, -0.04, 0.03, 0.02, -0.05, 0.06, -0.01]) + qdd = np.array([0.2, -0.1, 0.05, 0.08, -0.04, 0.03, -0.02]) + wrench_true = np.array([9.0, -4.0, 6.0, 0.8, -0.3, 0.5]) + bias_true = np.array([0.12, -0.08, 0.05, 0.03, -0.06, 0.04, -0.02]) + + jacobian = estimator._chest_jacobian(GENERAL_Q, qd) + self.assertEqual(np.linalg.matrix_rank(jacobian, tol=1e-9), 6) + tau_contact = jacobian.T @ wrench_true + tau_model = estimator._tau_model(GENERAL_Q, qd, qdd) + + calibration_delta = np.linspace(-0.01, 0.01, model.nv) + calibrated = estimator.calibrate_bias( + np.vstack( + [ + bias_true + calibration_delta, + bias_true - calibration_delta, + bias_true, + ] + ) + ) + np.testing.assert_allclose(calibrated, bias_true, atol=1e-15) + + tau_corrected, wrench_estimated, jacobian_estimated = estimator.estimate( + GENERAL_Q, qd, qdd, tau_model + bias_true + tau_contact + ) + + np.testing.assert_allclose(jacobian_estimated, jacobian, atol=1e-13) + np.testing.assert_allclose( + estimator.last_tau_residual_raw, bias_true + tau_contact, atol=1e-12 + ) + np.testing.assert_allclose( + estimator.last_tau_residual_corrected, tau_contact, atol=1e-12 + ) + np.testing.assert_allclose(tau_corrected, tau_contact, atol=1e-12) + np.testing.assert_allclose( + wrench_estimated, wrench_true, rtol=2e-11, atol=2e-11 + ) + + estimator.clear_bias() + np.testing.assert_array_equal(estimator.tau_bias, np.zeros(model.nv)) + + def test_offline_measurement_batch_calibrates_bias(self) -> None: + model = self.model + estimator = InteractionEstimator( + model, "master_base", "master_ee", lambda_damp=1e-4 + ) + bias_true = np.linspace(-0.09, 0.12, model.nv) + q_batch = np.vstack([GENERAL_Q, GENERAL_Q + 0.03, GENERAL_Q - 0.02]) + qd_batch = np.vstack( + [ + np.zeros(model.nv), + np.linspace(-0.02, 0.03, model.nv), + np.linspace(0.01, -0.04, model.nv), + ] + ) + qdd_batch = np.vstack( + [ + np.zeros(model.nv), + np.linspace(0.04, -0.02, model.nv), + np.linspace(-0.03, 0.05, model.nv), + ] + ) + tau_meas_batch = np.vstack( + [ + estimator._tau_model(q, qd, qdd) + bias_true + for q, qd, qdd in zip(q_batch, qd_batch, qdd_batch) + ] + ) + + calibrated = estimator.calibrate_bias_from_measurements( + q_batch, qd_batch, qdd_batch, tau_meas_batch + ) + np.testing.assert_allclose(calibrated, bias_true, atol=1e-12) + + def test_fixed_dls_remains_finite_at_a_singularity(self) -> None: + model = self.model + estimator = InteractionEstimator( + model, "master_base", "master_ee", lambda_damp=1e-3 + ) + q = np.zeros(model.nq) + qd = np.zeros(model.nv) + qdd = np.zeros(model.nv) + jacobian = estimator._chest_jacobian(q, qd) + self.assertLess(np.linalg.matrix_rank(jacobian, tol=1e-9), 6) + + tau_model = estimator._tau_model(q, qd, qdd) + residual = np.linspace(-0.5, 0.7, model.nv) + tau_corrected, wrench_estimated, jacobian_estimated = estimator.estimate( + q, qd, qdd, tau_model + residual + ) + + np.testing.assert_allclose(tau_corrected, residual, atol=1e-12) + np.testing.assert_allclose(jacobian_estimated, jacobian, atol=1e-13) + self.assertTrue(np.all(np.isfinite(wrench_estimated))) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_master_mujoco.py b/code/test/test_master_mujoco.py new file mode 100644 index 0000000..5d0f269 --- /dev/null +++ b/code/test/test_master_mujoco.py @@ -0,0 +1,43 @@ +"""Headless smoke test for the canonical master MuJoCo model. + +This file used to launch an interactive viewer at import time, which made test +discovery block or fail depending on the current working directory. +""" + +from __future__ import annotations + +import unittest +from pathlib import Path + +import mujoco + + +CONFIG_ROOT = Path(__file__).resolve().parents[1] / "config" +MASTER_MJCF = CONFIG_ROOT / "master_7dof.mjcf" +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", +) + + +class MasterMujocoModelTest(unittest.TestCase): + def test_canonical_model_loads_headlessly(self) -> None: + model = mujoco.MjModel.from_xml_path(str(MASTER_MJCF)) + self.assertEqual(model.nq, 7) + self.assertEqual(model.nv, 7) + for name in MASTER_JOINT_NAMES: + joint_id = mujoco.mj_name2id( + model, + mujoco.mjtObj.mjOBJ_JOINT, + name, + ) + self.assertGreaterEqual(joint_id, 0, name) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_model_contract.py b/code/test/test_model_contract.py new file mode 100644 index 0000000..586ad0f --- /dev/null +++ b/code/test/test_model_contract.py @@ -0,0 +1,54 @@ +import unittest + +import numpy as np +import pinocchio as pin + +from core.model_contract import ( + MASTER_FRAMES, + MASTER_JOINT_NAMES, + SLAVE_FRAMES, + SLAVE_JOINT_NAMES, + clip_configuration, + finite_joint_limits, + joint_q_indices, + load_models, + require_frame, + safe_configuration, +) + + +class ModelContractTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.models = load_models() + + def test_canonical_models_and_simulated_tcp_load(self): + self.assertEqual(self.models.master.nq, 7) + self.assertEqual(self.models.slave.nq, 7) + for name in MASTER_FRAMES.values(): + require_frame(self.models.master, name) + for name in SLAVE_FRAMES.values(): + require_frame(self.models.slave, name) + + def test_safe_slave_configuration_is_strictly_inside_limits(self): + q = safe_configuration(self.models.slave, SLAVE_JOINT_NAMES) + qidx = joint_q_indices(self.models.slave, SLAVE_JOINT_NAMES) + lower, upper = finite_joint_limits(self.models.slave, SLAVE_JOINT_NAMES) + self.assertTrue(np.all(q[qidx] > lower)) + self.assertTrue(np.all(q[qidx] < upper)) + + def test_clipping_reports_limit_event(self): + q = pin.neutral(self.models.slave) + qidx = joint_q_indices(self.models.slave, SLAVE_JOINT_NAMES) + lower, upper = finite_joint_limits(self.models.slave, SLAVE_JOINT_NAMES) + q[qidx] = upper + 0.1 + clipped, event = clip_configuration( + self.models.slave, q, SLAVE_JOINT_NAMES, margin=1e-4 + ) + self.assertTrue(event) + self.assertTrue(np.all(clipped[qidx] <= upper - 1e-4 + 1e-12)) + self.assertTrue(np.all(clipped[qidx] >= lower + 1e-4 - 1e-12)) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_paper_source_data.py b/code/test/test_paper_source_data.py new file mode 100644 index 0000000..659ebae --- /dev/null +++ b/code/test/test_paper_source_data.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +"""End-to-end source-data generation from completed raw trials.""" + +import csv +import json +from pathlib import Path +import sys +import tempfile +import unittest + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(CODE_ROOT)) + +from analysis.make_paper_artifacts import generate_paper_source_data # noqa: E402 +from experiments.io import TrialPayload # noqa: E402 +from experiments.plan import build_trial_plan # noqa: E402 +from experiments.runner import run_trial_plan # noqa: E402 + + +def wrench_executor(_trial): + reference = np.zeros((4, 6), dtype=float) + estimate = reference.copy() + estimate[:, 0] = 1.0 + return TrialPayload( + samples={ + "wrench_estimated": estimate, + "wrench_reference": reference, + } + ) + + +class PaperSourceDataTest(unittest.TestCase): + def test_source_tables_are_generated_from_raw_npz(self): + plan = build_trial_plan( + { + "study_id": "h2_source_data", + "split": "pilot", + "root_seed": 4, + "replicates": 1, + "methods": ["dls", "undamped"], + "trajectories": [ + {"trajectory_id": "load_001", "family": "static_load"} + ], + "factors": {"axis": ["x"]}, + } + ) + configuration = {"enabled": ["h2"], "h2": {}} + with tempfile.TemporaryDirectory() as temporary: + batch = Path(temporary) / "batch" + run_trial_plan(plan, batch, wrench_executor) + manifest = generate_paper_source_data(batch, configuration) + self.assertEqual(manifest["row_count"], 2) + metric_path = batch / "derived" / "trial_metrics.jsonl" + rows = [ + json.loads(line) + for line in metric_path.read_text(encoding="utf-8").splitlines() + ] + self.assertEqual(len(rows), 2) + self.assertTrue( + all(row["h2_force_rmse_N"] == 1.0 for row in rows) + ) + source_path = batch / "paper" / "source_data" / "h2.csv" + with source_path.open(newline="", encoding="utf-8") as stream: + table = list(csv.DictReader(stream)) + self.assertEqual(len(table), 2) + self.assertIn("h2_force_rmse_N", table[0]) + self.assertTrue((batch / "paper" / "artifact_manifest.json").is_file()) + + def test_metric_family_method_filter_prevents_mixed_supervisors(self): + plan = build_trial_plan( + { + "study_id": "filtered_source_data", + "split": "pilot", + "root_seed": 5, + "replicates": 1, + "methods": ["dls", "undamped"], + "trajectories": ["load_001"], + } + ) + configuration = { + "enabled": ["h2"], + "h2": {"include_methods": ["dls"]}, + } + with tempfile.TemporaryDirectory() as temporary: + batch = Path(temporary) / "batch" + run_trial_plan(plan, batch, wrench_executor) + manifest = generate_paper_source_data(batch, configuration) + self.assertEqual(manifest["row_count"], 2) + self.assertEqual(manifest["family_row_counts"], {"h2": 1}) + + metric_rows = [ + json.loads(line) + for line in (batch / "derived" / "trial_metrics.jsonl") + .read_text(encoding="utf-8") + .splitlines() + ] + dls_row = next(row for row in metric_rows if row["method_id"] == "dls") + undamped_row = next( + row for row in metric_rows if row["method_id"] == "undamped" + ) + self.assertIn("h2_force_rmse_N", dls_row) + self.assertNotIn("h2_force_rmse_N", undamped_row) + + with ( + batch / "paper" / "source_data" / "h2.csv" + ).open(newline="", encoding="utf-8") as stream: + table = list(csv.DictReader(stream)) + self.assertEqual([row["method_id"] for row in table], ["dls"]) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_pinocchio.py b/code/test/test_pinocchio.py new file mode 100644 index 0000000..481aa2d --- /dev/null +++ b/code/test/test_pinocchio.py @@ -0,0 +1,56 @@ +"""Headless dynamics smoke test for the canonical slave URDF.""" + +from __future__ import annotations + +import unittest +from pathlib import Path + +import numpy as np +import pinocchio as pin + + +CONFIG_ROOT = Path(__file__).resolve().parents[1] / "config" +SLAVE_URDF = CONFIG_ROOT / "real_slave_7dof.urdf" +SLAVE_JOINT_NAMES = ( + "R_SHOULDER_P", + "R_SHOULDER_R", + "R_SHOULDER_Y", + "R_ELBOW_R", + "R_WRIST_P", + "R_WRIST_Y", + "R_WRIST_R", +) + + +class SlavePinocchioModelTest(unittest.TestCase): + def test_canonical_model_dynamics_are_finite(self) -> None: + model = pin.buildModelFromUrdf(str(SLAVE_URDF)) + data = model.createData() + self.assertEqual(model.nq, 7) + self.assertEqual(model.nv, 7) + for name in SLAVE_JOINT_NAMES: + self.assertGreater(int(model.getJointId(name)), 0, name) + + q = pin.neutral(model) + qd = np.zeros(model.nv) + mass = pin.crba(model, data, q) + nonlinear = pin.nonLinearEffects(model, data, q, qd) + frame_id = int(model.getFrameId("R_WRIST_R_S")) + jacobian = pin.computeFrameJacobian( + model, + data, + q, + frame_id, + pin.ReferenceFrame.WORLD, + ) + + self.assertEqual(mass.shape, (7, 7)) + self.assertEqual(nonlinear.shape, (7,)) + self.assertEqual(jacobian.shape, (6, 7)) + self.assertTrue(np.all(np.isfinite(mass))) + self.assertTrue(np.all(np.isfinite(nonlinear))) + self.assertTrue(np.all(np.isfinite(jacobian))) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_retargeting_baselines.py b/code/test/test_retargeting_baselines.py new file mode 100644 index 0000000..2568c7b --- /dev/null +++ b/code/test/test_retargeting_baselines.py @@ -0,0 +1,199 @@ +"""Contract tests for the three formal H1 retargeting baselines.""" + +from __future__ import annotations + +import sys +import unittest +from pathlib import Path + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +if str(CODE_ROOT) not in sys.path: + sys.path.insert(0, str(CODE_ROOT)) + +from core.model_contract import ( # noqa: E402 + MASTER_FRAMES, + MASTER_JOINT_NAMES, + SLAVE_FRAMES, + SLAVE_JOINT_NAMES, + joint_q_indices, + load_models, +) +from core.retargeting_baselines import ( # noqa: E402 + Retargeter, + RetargetingFailure, + RetargetingSolverStatus, + SEWRetargeterAdapter, + build_canonical_baselines, + build_canonical_sew_target_baselines, +) +from core.sew_mapper2 import BallJointConfig, SEWMapper # noqa: E402 + + +class RetargetingBaselinesTest(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + cls.models = load_models() + cls.baselines = build_canonical_baselines(cls.models) + cls.slave_indices = joint_q_indices( + cls.models.slave, SLAVE_JOINT_NAMES + ) + + def test_factory_exposes_three_uniform_methods(self) -> None: + self.assertEqual( + set(self.baselines), + {"scaled_joint_space", "bounded_dls_ik", "task_priority_ik"}, + ) + for baseline in self.baselines.values(): + self.assertIsInstance(baseline, Retargeter) + + def test_frozen_reference_is_exact_for_all_methods(self) -> None: + q_master = np.zeros(7) + for name, baseline in self.baselines.items(): + with self.subTest(method=name): + result = baseline.retarget(q_master) + self.assertTrue(result.success, result.events) + self.assertTrue(result.smooth, result.events) + self.assertIs(result.failure, RetargetingFailure.NONE) + self.assertEqual(result.q_slave.shape, (self.models.slave.nq,)) + self.assertFalse(result.q_slave.flags.writeable) + self.assertIsNotNone(result.target) + self.assertGreaterEqual(result.diagnostics.runtime_s, 0.0) + self.assertGreaterEqual(result.diagnostics.cost, 0.0) + + def test_cartesian_baselines_share_target_and_converge(self) -> None: + q_master = np.array( + [0.05, -0.04, 0.03, 0.08, -0.02, 0.03, -0.01], + dtype=float, + ) + dls = self.baselines["bounded_dls_ik"].retarget(q_master) + priority = self.baselines["task_priority_ik"].retarget(q_master) + + self.assertTrue(dls.success, dls.events) + self.assertTrue(priority.success, priority.events) + self.assertIs( + dls.diagnostics.status, RetargetingSolverStatus.CONVERGED + ) + self.assertIs( + priority.diagnostics.status, RetargetingSolverStatus.CONVERGED + ) + np.testing.assert_allclose( + dls.target.position, priority.target.position, atol=0.0 + ) + np.testing.assert_allclose( + dls.target.rotation, priority.target.rotation, atol=0.0 + ) + + self.assertLess(dls.diagnostics.position_error_m, 2e-4) + self.assertLess(dls.diagnostics.orientation_error_rad, 2e-3) + self.assertLess(priority.diagnostics.position_error_m, 2e-4) + self.assertLess(priority.diagnostics.orientation_error_rad, 2e-3) + self.assertEqual( + priority.diagnostics.message, + "secondary_objective=log_manipulability", + ) + + def test_scaled_joint_mapping_uses_declared_urdf_ranges(self) -> None: + baseline = self.baselines["scaled_joint_space"] + result = baseline.retarget(np.zeros(7)) + expected_midpoint = 0.5 * ( + baseline.slave_lower + baseline.slave_upper + ) + np.testing.assert_allclose( + result.q_slave[self.slave_indices], expected_midpoint, atol=1e-15 + ) + self.assertIs( + result.diagnostics.status, RetargetingSolverStatus.CLOSED_FORM + ) + + def test_master_limit_failure_is_returned_not_thrown(self) -> None: + q_outside = np.zeros(7) + q_outside[0] = np.pi + 0.01 + for name, baseline in self.baselines.items(): + with self.subTest(method=name): + result = baseline.retarget(q_outside) + self.assertFalse(result.success) + self.assertFalse(result.smooth) + self.assertIs( + result.failure, + RetargetingFailure.MASTER_LIMIT_VIOLATION, + ) + self.assertIs( + result.diagnostics.status, + RetargetingSolverStatus.INVALID_INPUT, + ) + self.assertIn("master_limit_violation", result.events) + + def test_invalid_seed_has_stable_failure(self) -> None: + for name in ("bounded_dls_ik", "task_priority_ik"): + result = self.baselines[name].retarget( + np.zeros(7), q_slave_seed=np.zeros(3) + ) + self.assertIs(result.failure, RetargetingFailure.INVALID_INPUT) + self.assertEqual(result.events, ("invalid_input",)) + + def test_existing_sew_mapper_has_compatible_adapter(self) -> None: + mapper = SEWMapper( + master_model=self.models.master, + slave_model=self.models.slave, + m_shoulder=MASTER_FRAMES["shoulder"], + m_elbow=MASTER_FRAMES["elbow"], + m_wrist=MASTER_FRAMES["wrist"], + m_ee=MASTER_FRAMES["ee"], + s_shoulder=SLAVE_FRAMES["shoulder"], + s_elbow=SLAVE_FRAMES["elbow"], + s_wrist=SLAVE_FRAMES["wrist"], + s_ee=SLAVE_FRAMES["wrist"], + master_joint_names=MASTER_JOINT_NAMES, + slave_joint_names=SLAVE_JOINT_NAMES, + slave_shoulder_cfg=BallJointConfig( + "yxy", SLAVE_JOINT_NAMES[:3], (-1.0, 1.0, -1.0) + ), + slave_wrist_cfg=BallJointConfig( + "yzx", SLAVE_JOINT_NAMES[4:], (-1.0, 1.0, 1.0) + ), + ) + adapter = SEWRetargeterAdapter(mapper) + result = adapter.retarget( + np.array([0.534, 0.314, -0.1, 2.14, 0.38, 0.38, -0.72]) + ) + self.assertTrue(result.success, result.events) + self.assertEqual(result.method, "sew") + self.assertIs( + result.diagnostics.status, RetargetingSolverStatus.CONVERGED + ) + + invalid = adapter.retarget(np.zeros(3)) + self.assertIs(invalid.failure, RetargetingFailure.INVALID_INPUT) + + def test_common_sew_target_factory_shares_wrist_target(self) -> None: + baselines, proposed = build_canonical_sew_target_baselines(self.models) + q_master = np.array( + [0.534, 0.314, -0.1, 2.14, 0.38, 0.38, -0.72] + ) + dls = baselines["bounded_dls_ik"].retarget(q_master) + priority = baselines["task_priority_ik"].retarget(q_master) + sew = proposed.retarget(q_master) + self.assertTrue(dls.success, dls.events) + self.assertTrue(priority.success, priority.events) + self.assertTrue(sew.success, sew.events) + np.testing.assert_allclose( + dls.target.position, priority.target.position, atol=0.0 + ) + np.testing.assert_allclose( + dls.target.rotation, priority.target.rotation, atol=0.0 + ) + + degenerate = baselines["bounded_dls_ik"].retarget( + np.array([-np.pi / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + ) + self.assertIs( + degenerate.failure, RetargetingFailure.DEGENERATE_GEOMETRY + ) + self.assertIn("master_arm_plane_degenerate", degenerate.events) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_retargeting_differential.py b/code/test/test_retargeting_differential.py new file mode 100644 index 0000000..074afa6 --- /dev/null +++ b/code/test/test_retargeting_differential.py @@ -0,0 +1,179 @@ +"""Regression tests for the bounded SEW retargeting differential.""" + +from pathlib import Path +import sys +import unittest + +import numpy as np +import pinocchio as pin + + +CODE_ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(CODE_ROOT)) + +from core.sew_mapper2 import ( # noqa: E402 + BallJointConfig, + SEWMapper, + _wrap_angle_delta, +) + + +def build_real_mapper() -> SEWMapper: + config_dir = CODE_ROOT / "config" + master_model = pin.buildModelFromUrdf(str(config_dir / "master_7dof.urdf")) + slave_model = pin.buildModelFromUrdf(str(config_dir / "real_slave_7dof.urdf")) + + master_joints = ( + "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_joints = ( + "R_SHOULDER_P", + "R_SHOULDER_R", + "R_SHOULDER_Y", + "R_ELBOW_R", + "R_WRIST_P", + "R_WRIST_Y", + "R_WRIST_R", + ) + shoulder = BallJointConfig( + axis_order="yxy", + joint_names=("R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y"), + signs=(-1.0, 1.0, -1.0), + ) + wrist = BallJointConfig( + axis_order="yzx", + joint_names=("R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"), + signs=(-1.0, 1.0, 1.0), + ) + return SEWMapper( + master_model=master_model, + slave_model=slave_model, + m_shoulder="master_shoulder", + m_elbow="master_forearm", + m_wrist="master_wrist", + m_ee="master_ee", + s_shoulder="R_SHOULDER_R_S", + s_elbow="R_ELBOW_R_S", + s_wrist="R_WRIST_R_S", + s_ee="R_WRIST_R_S", + master_joint_names=master_joints, + slave_joint_names=slave_joints, + slave_shoulder_cfg=shoulder, + slave_wrist_cfg=wrist, + ) + + +class TestRetargetingDifferential(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + cls.mapper = build_real_mapper() + # An interior, exactly recoverable pose on the real slave arm. + cls.q_master = np.array( + [0.534, 0.314, -0.1, 2.14, 0.38, 0.38, -0.72], + dtype=float, + ) + + def test_virtual_work_identity(self) -> None: + q_slave, A, debug = self.mapper.retarget_with_differential(self.q_master) + + self.assertTrue(debug["success"], debug["events"]) + self.assertTrue( + debug["differential_valid"], + debug["differential"]["events"], + ) + self.assertFalse(debug["clipped"]) + self.assertFalse(debug["near_limit"]) + q_slave7 = self.mapper._slave_q7(q_slave) + self.assertTrue(np.all(q_slave7 >= self.mapper.s_lower7)) + self.assertTrue(np.all(q_slave7 <= self.mapper.s_upper7)) + + qdot_master = np.array( + [0.21, -0.34, 0.17, 0.09, -0.11, 0.28, -0.07] + ) + tau_slave = np.array( + [-1.2, 0.4, 2.3, -0.7, 0.5, -1.1, 0.8] + ) + qdot_slave = A @ qdot_master + tau_master = A.T @ tau_slave + self.assertAlmostEqual( + float(tau_master @ qdot_master), + float(tau_slave @ qdot_slave), + places=12, + ) + + def test_matches_independent_directional_finite_difference(self) -> None: + q_slave, A, debug = self.mapper.retarget_with_differential( + self.q_master, fd_step=1e-4 + ) + self.assertTrue(debug["differential_valid"]) + + direction = np.array([0.3, -0.2, 0.5, -0.1, 0.4, -0.25, 0.35]) + direction /= np.linalg.norm(direction) + independent_step = 2.5e-5 + q_plus, plus_debug = self.mapper.retarget( + self.q_master + independent_step * direction, + q_s_init=q_slave, + ) + q_minus, minus_debug = self.mapper.retarget( + self.q_master - independent_step * direction, + q_s_init=q_slave, + ) + self.assertTrue(plus_debug["success"], plus_debug["events"]) + self.assertTrue(minus_debug["success"], minus_debug["events"]) + + directional_fd = _wrap_angle_delta( + self.mapper._slave_q7(q_plus) - self.mapper._slave_q7(q_minus) + ) / (2.0 * independent_step) + np.testing.assert_allclose( + directional_fd, + A @ direction, + rtol=2e-4, + atol=2e-5, + ) + + def test_degenerate_geometry_invalidates_differential(self) -> None: + # Zero elbow flex makes the master shoulder-elbow-wrist plane undefined. + q_degenerate = np.array([-np.pi / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + A, debug = self.mapper.compute_differential(q_degenerate) + + self.assertFalse(debug["valid"]) + self.assertTrue(np.isnan(A).all()) + self.assertIn("base_invalid", debug["events"]) + self.assertIn( + "master_arm_plane_degenerate", + debug["base"]["events"], + ) + + def test_active_limit_invalidates_differential(self) -> None: + old_margin = self.mapper.joint_limit_margin + try: + # Enlarge only the diagnostic margin to exercise the active-set + # safety gate without changing the bounded pose solution. + self.mapper.joint_limit_margin = 10.0 + A, debug = self.mapper.compute_differential(self.q_master) + finally: + self.mapper.joint_limit_margin = old_margin + + self.assertTrue(debug["base"]["success"]) + self.assertTrue(debug["base"]["near_limit"]) + self.assertFalse(debug["valid"]) + self.assertTrue(np.isnan(A).all()) + self.assertIn("base_nonsmooth", debug["events"]) + + def test_angle_difference_wrap(self) -> None: + raw = np.array([2.0 * np.pi - 0.1, -2.0 * np.pi + 0.2, np.pi]) + np.testing.assert_allclose( + _wrap_angle_delta(raw), + np.array([-0.1, 0.2, -np.pi]), + atol=1e-14, + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_sew.py b/code/test/test_sew.py new file mode 100644 index 0000000..cacce94 --- /dev/null +++ b/code/test/test_sew.py @@ -0,0 +1,158 @@ +import os +import sys +import numpy as np +import pinocchio as pin +import matplotlib.pyplot as plt +from omegaconf import OmegaConf + +sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) +from core.sew_mapper import SEWMapper, fk_update, pose_of_frame, rot_error_deg + +# ...（这里保留你之前的 safe_normalize、random_qm、几何绘图函数等）... +def random_qm(mapper, N=50, margin=0.2): + lb = []; ub = [] + for j in mapper.m_model.joints[1:]: # skip universe + if j.nq == 1: # 1-DoF revolute + jid = j.id + iq = j.idx_q + lb.append(mapper.m_model.lowerPositionLimit[iq]) + ub.append(mapper.m_model.upperPositionLimit[iq]) + lb = np.array(lb); ub = np.array(ub) + rng = (ub - lb) + lb2 = lb + margin * rng + ub2 = ub - margin * rng + + qs = [] + for _ in range(N): + v = lb2 + np.random.rand(len(lb2)) * (ub2 - lb2) + q_full = pin.neutral(mapper.m_model) + k = 0 + for j in mapper.m_model.joints[1:]: + if j.nq == 1: + q_full[j.idx_q] = v[k]; k += 1 + qs.append(q_full) + return qs + + + +def evaluate_sew_statistics(mapper, conf, N=500, margin=0.1, save_dir="assets"): + """ + 统计验证 SEW retargeting 的精度，并输出论文用图： + (1) 腕点位置误差直方图 + (2) 腕点姿态误差直方图 + (3) （可选）位置误差 vs 肩-腕距离散点 + """ + os.makedirs(save_dir, exist_ok=True) + + # 随机采样 master 关节 + qs_m = random_qm(mapper, N=N, margin=margin) + + pos_errs = [] # mm + rot_errs = [] # deg + dists_sw = [] # shoulder–wrist distance (m) + clipped_flags = [] + + for q_m in qs_m: + # SEW 映射 + q_s, dbg = mapper.retargetting(q_m) + + # master FK + pS_m, _ = pose_of_frame(mapper.m_model, mapper.m_data, conf.m_shoulder_frame) + pE_m, RE_m = pose_of_frame(mapper.m_model, mapper.m_data, conf.m_ee_frame) + + # slave FK + fk_update(mapper.s_model, mapper.s_data, q_s) + pE_s, RE_s = pose_of_frame(mapper.s_model, mapper.s_data, conf.s_ee_frame) + + # 误差（改成 EE） + e_p = np.linalg.norm(pE_s - pE_m) * 1000.0 # 末端位置误差 [mm] + e_R = rot_error_deg(RE_m, RE_s) # 末端姿态误差 [deg] + + pos_errs.append(e_p) + rot_errs.append(e_R) + + d_m = np.linalg.norm(pE_m - pS_m) # m + dists_sw.append(d_m) + + # 是否发生 reach 裁剪（根据 d_s vs master 原始 d_m、以及 L1+L2 区间） + d_s = dbg["d_s"] + clipped = (d_s < d_m - 1e-9) or (d_s > mapper.L1 + mapper.L2 - mapper.eps_clip + 1e-9) + clipped_flags.append(clipped) + + pos_errs = np.asarray(pos_errs) + rot_errs = np.asarray(rot_errs) + dists_sw = np.asarray(dists_sw) + clipped_flags = np.asarray(clipped_flags, dtype=bool) + + # --------- 打印统计量 (你可以把结果手工抄进论文表格) ---------- + def summary_str(x): + return f"mean={x.mean():.3f}, std={x.std():.3f}, max={x.max():.3f}" + + print("=== SEW retargeting statistics over {} samples ===".format(N)) + print("Wrist position error [mm]:", summary_str(pos_errs)) + print("Wrist orientation error [deg]:", summary_str(rot_errs)) + print("Clipped configurations: {} / {} ({:.1f}%)" + .format(clipped_flags.sum(), N, 100.0 * clipped_flags.mean())) + + # --------- 图 1: 位置 & 姿态误差直方图 ---------- + fig, axes = plt.subplots(1, 2, figsize=(10, 4)) + + axes[0].hist(pos_errs, bins=30, color='C0', alpha=0.8) + axes[0].set_xlabel("Wrist position error $e_p$ [mm]") + axes[0].set_ylabel("Count") + axes[0].set_title("(a) Distribution of $e_p$") + + axes[1].hist(rot_errs, bins=30, color='C1', alpha=0.8) + axes[1].set_xlabel("Wrist orientation error $e_R$ [deg]") + axes[1].set_ylabel("Count") + axes[1].set_title("(b) Distribution of $e_R$") + + plt.tight_layout() + fig.savefig(os.path.join(save_dir, "sew_error_hist.png"), dpi=300) + # plt.show() + + # --------- 图 2: 位置误差 vs 肩-腕距离 (可视化剪裁区) ---------- + fig2, ax2 = plt.subplots(figsize=(5, 4)) + ax2.scatter(dists_sw[~clipped_flags], pos_errs[~clipped_flags], + s=15, c='C0', label="Unclipped") + ax2.scatter(dists_sw[clipped_flags], pos_errs[clipped_flags], + s=25, c='C3', marker='x', label="Clipped") + ax2.set_xlabel("Shoulder–wrist distance $d_m$ [m]") + ax2.set_ylabel("Position error $e_p$ [mm]") + ax2.set_title("SEW error vs. reach distance") + ax2.grid(True, linestyle='--', linewidth=0.5) + ax2.legend(loc="upper left") + + fig2.tight_layout() + fig2.savefig(os.path.join(save_dir, "sew_error_scatter.png"), dpi=300) + plt.show() + + +def main(): + conf = OmegaConf.load("./config/config.yaml") + + master_model, _, _ = pin.buildModelsFromUrdf(str(conf.master_urdf)) + slave_model, _, _ = pin.buildModelsFromUrdf(str(conf.slave_urdf)) + + mapper = SEWMapper( + master_model=master_model, + slave_model=slave_model, + m_shoulder_frame=conf.m_shoulder_frame, + m_elbow_frame=conf.m_elbow_frame, + m_wrist_frame=conf.m_wrist_frame, + m_ee_frame=conf.m_ee_frame, + s_shoulder_frame=conf.s_shoulder_frame, + s_elbow_frame=conf.s_elbow_frame, + s_wrist_frame=conf.s_wrist_frame, + s_ee_frame=conf.s_ee_frame, + slave_joint_names=conf.sew_mapper.slave_joint_names, + up_dir=np.array(conf.sew_mapper.up_dir), + eps_clip=conf.sew_mapper.eps_clip, + ) + + # 1) 如果要画几何示意图（方法部分），可以单独写一个函数；这里我们只跑统计 + evaluate_sew_statistics(mapper, conf, N=500, margin=0.1, save_dir="assets") + + +if __name__ == "__main__": + main() diff --git a/code/test/test_sew_mapper2.py b/code/test/test_sew_mapper2.py new file mode 100644 index 0000000..7588323 --- /dev/null +++ b/code/test/test_sew_mapper2.py @@ -0,0 +1,136 @@ +import numpy as np +import pinocchio as pin +from core.sew_mapper2 import SEWMapper, BallJointConfig + + +# -------------------------- quick test --------------------- ----- # + +def test_once(mapper: SEWMapper) -> None: + q_m7 = np.array([0.534, 0.314, -0.1, 2.14, 0.38, 0.38, -0.72], dtype=float) + q_s, dbg = mapper.retarget(q_m7) + + # evaluate + mapper._fk_master(_build_q7(mapper.m_model, mapper.m_qidx7, q_m7)) + mapper._fk_slave(q_s) + Rm = mapper._rot(mapper.m_data, mapper.fid_mEE) + Rs = mapper._rot(mapper.s_data, mapper.fid_sEE) + ang_err = float(np.linalg.norm(pin.log3(Rs.T @ Rm))) + pW = mapper._pos(mapper.s_data, mapper.fid_sW) + pos_err = float(np.linalg.norm(pW - dbg["pW_s_ref"])) + + print("q_s(7):", np.array([q_s[i] for i in mapper.s_qidx7])) + print("angle_err(rad):", ang_err) + print("wrist_pos_err(m):", pos_err) + +def _build_q7(model: pin.Model, qidx7, q7): + q = pin.neutral(model) + for i, idx in enumerate(qidx7): + q[idx] = float(q7[i]) + return q + + +def test_fake(): + master_urdf = "../config/master_7dof.urdf" + slave_urdf = "../config/slave_7dof.urdf" + + m_model = pin.buildModelFromUrdf(master_urdf) + s_model = pin.buildModelFromUrdf(slave_urdf) + + # frames + mS, mE, mW, mEE = "master_shoulder", "master_forearm", "master_wrist", "master_ee" + sS, sE, sW, sEE = "slave_shoulder", "slave_forearm", "slave_wrist", "slave_ee" + + # joint order: [S1,S2,S3, EL, W1,W2,W3] + 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 = ( + "slave_shoulder_pitch_joint", "slave_shoulder_yaw_joint", "slave_shoulder_roll_joint", + "slave_elbow_flex_joint", + "slave_wrist_roll_joint", "slave_wrist_yaw_joint", "slave_wrist_pitch_joint", + ) + + # configs (your confirmed settings) + s_sh_cfg = BallJointConfig( + axis_order="yxy", + joint_names=("slave_shoulder_pitch_joint", "slave_shoulder_yaw_joint", "slave_shoulder_roll_joint"), + signs=(-1.0, 1.0, -1.0), + ) + s_wr_cfg = BallJointConfig( + axis_order="yzx", + joint_names=("slave_wrist_roll_joint", "slave_wrist_yaw_joint", "slave_wrist_pitch_joint"), + signs=(-1.0, 1.0, 1.0), + ) + + mapper = SEWMapper( + master_model=m_model, + slave_model=s_model, + m_shoulder=mS, m_elbow=mE, m_wrist=mW, m_ee=mEE, + s_shoulder=sS, s_elbow=sE, s_wrist=sW, s_ee=sEE, + master_joint_names=master_joint_names, + slave_joint_names=slave_joint_names, + slave_shoulder_cfg=s_sh_cfg, + slave_wrist_cfg=s_wr_cfg, + slave_elbow_axis_local=np.array([1.0, 0.0, 0.0]), + debug=False, + ) + + test_once(mapper) + + +def test_real(): + master_urdf = "../config/master_7dof.urdf" + slave_urdf = "../config/real_slave_7dof.urdf" + + m_model = pin.buildModelFromUrdf(master_urdf) + s_model = pin.buildModelFromUrdf(slave_urdf) + + # frames + mS, mE, mW, mEE = "master_shoulder", "master_forearm", "master_wrist", "master_ee" + sS, sE, sW, sEE = "R_SHOULDER_R_S", "R_ELBOW_R_S", "R_WRIST_R_S", "R_WRIST_R_S" + + # joint order: [S1,S2,S3, EL, W1,W2,W3] + 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", + ) + + # configs (your confirmed settings) + s_sh_cfg = BallJointConfig( + axis_order="yxy", + joint_names=("R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y"), + signs=(-1.0, 1.0, -1.0), + ) + s_wr_cfg = BallJointConfig( + axis_order="yzx", + joint_names=("R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"), + signs=(-1.0, 1.0, 1.0), + ) + + mapper = SEWMapper( + master_model=m_model, + slave_model=s_model, + m_shoulder=mS, m_elbow=mE, m_wrist=mW, m_ee=mEE, + s_shoulder=sS, s_elbow=sE, s_wrist=sW, s_ee=sEE, + master_joint_names=master_joint_names, + slave_joint_names=slave_joint_names, + slave_shoulder_cfg=s_sh_cfg, + slave_wrist_cfg=s_wr_cfg, + slave_elbow_axis_local=np.array([1.0, 0.0, 0.0]), + debug=False, + ) + + test_once(mapper) + + +if __name__ == "__main__": + test_real() + diff --git a/code/test/test_slave_mujoco.py b/code/test/test_slave_mujoco.py new file mode 100644 index 0000000..7f75cce --- /dev/null +++ b/code/test/test_slave_mujoco.py @@ -0,0 +1,28 @@ +"""Document the unsupported historical direct-URDF MuJoCo path. + +The closed-loop simulator uses Pinocchio for the canonical slave dynamics. +MuJoCo's URDF importer strips the mesh subdirectory from this vendor URDF, so +the former interactive demo is not a valid automated contract test. +""" + +from __future__ import annotations + +import unittest +from pathlib import Path + + +CONFIG_ROOT = Path(__file__).resolve().parents[1] / "config" +SLAVE_URDF = CONFIG_ROOT / "real_slave_7dof.urdf" + + +class SlaveMujocoLegacyDemoTest(unittest.TestCase): + @unittest.skip( + "legacy direct-URDF MuJoCo demo is unsupported; " + "canonical slave dynamics are covered by Pinocchio model tests" + ) + def test_legacy_direct_urdf_import(self) -> None: + self.assertTrue(SLAVE_URDF.is_file()) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/test_wrench_solver.py b/code/test/test_wrench_solver.py new file mode 100644 index 0000000..ec9b330 --- /dev/null +++ b/code/test/test_wrench_solver.py @@ -0,0 +1,115 @@ +"""Numerical tests for the dimensionally scaled H2 solvers.""" + +from __future__ import annotations + +import sys +import unittest +from pathlib import Path + +import numpy as np + + +CODE_ROOT = Path(__file__).resolve().parents[1] +if str(CODE_ROOT) not in sys.path: + sys.path.insert(0, str(CODE_ROOT)) + +from core.wrench_solver import ( # noqa: E402 + ScaledDLSSolver, + UndampedSVDSolver, + WrenchSolveStatus, +) + + +class WrenchSolverTest(unittest.TestCase): + def setUp(self) -> None: + rng = np.random.default_rng(240319) + self.jacobian = rng.normal(size=(6, 7)) + self.wrench = np.array([8.0, -3.0, 5.0, 0.7, -0.4, 0.2]) + self.length = 0.37 + self.residual = self.jacobian.T @ self.wrench + + def test_scaling_preserves_virtual_work(self) -> None: + solver = UndampedSVDSolver(self.length) + scaled_jacobian = solver.scaled_jacobian(self.jacobian) + scaled_wrench = np.concatenate( + (self.length * self.wrench[:3], self.wrench[3:]) + ) + np.testing.assert_allclose( + scaled_jacobian.T @ scaled_wrench, + self.jacobian.T @ self.wrench, + rtol=1e-14, + atol=1e-14, + ) + + def test_undamped_full_rank_recovers_known_wrench(self) -> None: + result = UndampedSVDSolver( + self.length, relative_rank_tolerance=1e-10 + ).solve(self.jacobian, self.residual) + self.assertIs(result.status, WrenchSolveStatus.FULL_RANK) + self.assertEqual(result.rank, 6) + np.testing.assert_allclose( + result.wrench, self.wrench, rtol=2e-13, atol=2e-13 + ) + np.testing.assert_allclose( + result.reconstructed_residual, + self.residual, + rtol=2e-13, + atol=2e-13, + ) + + def test_dls_matches_scaled_svd_filter_formula(self) -> None: + damping = 0.08 + solver = ScaledDLSSolver(self.length, damping) + result = solver.solve(self.jacobian, self.residual) + + scaled_jacobian = solver.scaled_jacobian(self.jacobian) + u, singular_values, vt = np.linalg.svd( + scaled_jacobian, full_matrices=False + ) + expected_scaled = u @ ( + singular_values + / (singular_values**2 + damping**2) + * (vt @ self.residual) + ) + expected_wrench = np.diag( + [1.0 / self.length] * 3 + [1.0] * 3 + ) @ expected_scaled + np.testing.assert_allclose(result.wrench, expected_wrench, atol=1e-14) + np.testing.assert_allclose( + result.singular_values, singular_values, atol=0.0 + ) + self.assertAlmostEqual(result.damping, damping) + + def test_rank_deficiency_is_retained_and_reported(self) -> None: + jacobian = self.jacobian.copy() + jacobian[-1, :] = jacobian[-2, :] + residual = jacobian.T @ self.wrench + for solver in ( + UndampedSVDSolver(self.length), + ScaledDLSSolver(self.length, 0.05), + ): + with self.subTest(method=solver.name): + result = solver.solve(jacobian, residual) + self.assertIs( + result.status, WrenchSolveStatus.RANK_DEFICIENT + ) + self.assertEqual(result.rank, 5) + self.assertTrue(np.isinf(result.condition_number)) + self.assertTrue(np.all(np.isfinite(result.wrench))) + + def test_invalid_units_and_shapes_are_rejected(self) -> None: + with self.assertRaises(ValueError): + ScaledDLSSolver(0.0, 0.1) + with self.assertRaises(ValueError): + ScaledDLSSolver(self.length, 0.0) + with self.assertRaises(ValueError): + UndampedSVDSolver(self.length, relative_rank_tolerance=1.0) + solver = UndampedSVDSolver(self.length) + with self.assertRaisesRegex(ValueError, "jacobian"): + solver.solve(np.eye(7), np.ones(7)) + with self.assertRaisesRegex(ValueError, "joint_residual"): + solver.solve(self.jacobian, np.ones(6)) + + +if __name__ == "__main__": + unittest.main() diff --git a/code/test/torque_control.py b/code/test/torque_control.py new file mode 100644 index 0000000..2a0ce2d --- /dev/null +++ b/code/test/torque_control.py @@ -0,0 +1,80 @@ +import time +import struct +import can + + +def arb_id_single(motor_id: int) -> int: + # 单电机命令：0x140 + ID(1~32) + return 0x140 + motor_id + + +def send_cmd(bus, motor_id: int, data8: bytes): + msg = can.Message( + arbitration_id=arb_id_single(motor_id), + data=data8, + is_extended_id=False, + ) + bus.send(msg) + + +def motor_run(bus, motor_id: int): + # 电机运行命令：DATA[0]=0x88，其余 0 + send_cmd(bus, motor_id, bytes([0x88, 0, 0, 0, 0, 0, 0, 0])) + + +def motor_stop(bus, motor_id: int): + # 电机停止命令：DATA[0]=0x81，其余 0 + send_cmd(bus, motor_id, bytes([0x81, 0, 0, 0, 0, 0, 0, 0])) + + +def torque_iq_cmd_a1(bus, motor_id: int, iq_control: int): + """ + 转矩闭环控制命令 0xA1： + DATA[0] = 0xA1 + DATA[4..5] = iqControl (int16, little-endian), 范围 -2048~2048 + 其余字节 0 + 【协议：0xA1，iqControl:int16，DATA[4]=低字节，DATA[5]=高字节】 + """ + if not (-2048 <= iq_control <= 2048): + raise ValueError("iq_control must be in [-2048, 2048]") + + data = bytearray(8) + data[0] = 0xA1 + data[4:6] = struct.pack(" np.ndarray: + x, y, z = v + return np.array([[0, -z, y], + [z, 0, -x], + [-y, x, 0]], dtype=float) + + +def adjoint_SE3(T: np.ndarray) -> np.ndarray: + """Adjoint(T) for SE(3): 6x6, spatial 顺序([ω; v]).""" + R = T[:3, :3] + p = T[:3, 3] + Ad = np.block([[R, np.zeros((3, 3))], + [_skew(p) @ R, R]]) + return Ad + + +def spatial_to_geometric(J_spatial: np.ndarray) -> np.ndarray: + """把 Pinocchio 的 [ω; v] 排列变成 [v; ω]。""" + return np.vstack([J_spatial[3:, :], J_spatial[:3, :]]) + + +def wrench_geo_to_spa(F_geo: np.ndarray) -> np.ndarray: + """[f; τ] -> [τ; f]""" + return np.hstack([F_geo[3:], F_geo[:3]]) + + +def wrench_spa_to_geo(F_spa: np.ndarray) -> np.ndarray: + """[τ; f] -> [f; τ]""" + return np.hstack([F_spa[3:], F_spa[:3]]) \ No newline at end of file diff --git a/code/utils/math.py b/code/utils/math.py new file mode 100644 index 0000000..eb61813 --- /dev/null +++ b/code/utils/math.py @@ -0,0 +1,40 @@ +import numpy as np + +def hat(w): + wx, wy, wz = w + return np.array([[0, -wz, wy], + [wz, 0, -wx], + [-wy, wx, 0]], dtype=float) + + +def rotvec_to_R(w): + """Axis-angle (rotation vector) to rotation matrix.""" + th = np.linalg.norm(w) + if th < 1e-12: + return np.eye(3) + k = w / th + K = hat(k) + return np.eye(3) + np.sin(th) * K + (1-np.cos(th)) * (K @ K) + + +def R_to_rotvec(R): + """Rotation matrix to rotation vector (log map).""" + tr = np.trace(R) + cos_th = (tr - 1) / 2 + cos_th = np.clip(cos_th, -1.0, 1.0) + th = np.arccos(cos_th) + if th < 1e-12: + return np.zeros(3) + w_hat = (R - R.T) / (2*np.sin(th)) + return th * np.array([w_hat[2,1], w_hat[0,2], w_hat[1,0]]) + + +def normalize(v, eps=1e-12): + n = np.linalg.norm(v) + if n < eps: + return v*0.0 + return v / n + + +def clip(x, a, b): + return max(a, min(b, x)) diff --git a/paper/exoskeleton/IEEEtran/IEEEtran.cls b/paper/exoskeleton/IEEEtran/IEEEtran.cls new file mode 100644 index 0000000..8d2b1c6 --- /dev/null +++ b/paper/exoskeleton/IEEEtran/IEEEtran.cls @@ -0,0 +1,6347 @@ +%% +%% IEEEtran.cls 2015/08/26 version V1.8b +%% +%% This is the IEEEtran LaTeX class for authors of the Institute of +%% Electrical and Electronics Engineers (IEEE) Transactions journals and +%% conferences. +%% +%% Support sites: +%% http://www.michaelshell.org/tex/ieeetran/ +%% http://www.ctan.org/pkg/ieeetran +%% and +%% http://www.ieee.org/ +%% +%% Based on the original 1993 IEEEtran.cls, but with many bug fixes +%% and enhancements (from both JVH and MDS) over the 1996/7 version. +%% +%% +%% Contributors: +%% Gerry Murray (1993), Silvano Balemi (1993), +%% Jon Dixon (1996), Peter N"uchter (1996), +%% Juergen von Hagen (2000), and Michael Shell (2001-2014) +%% +%% +%% Copyright (c) 1993-2000 by Gerry Murray, Silvano Balemi, +%% Jon Dixon, Peter N"uchter, +%% Juergen von Hagen +%% and +%% Copyright (c) 2001-2015 by Michael Shell +%% +%% Current maintainer (V1.3 to V1.8b): Michael Shell +%% See: +%% http://www.michaelshell.org/ +%% for current contact information. +%% +%% Special thanks to Peter Wilson (CUA) and Donald Arseneau +%% for allowing the inclusion of the \@ifmtarg command +%% from their ifmtarg LaTeX package. +%% +%%************************************************************************* +%% Legal Notice: +%% This code is offered as-is without any warranty either expressed or +%% implied; without even the implied warranty of MERCHANTABILITY or +%% FITNESS FOR A PARTICULAR PURPOSE! +%% User assumes all risk. +%% In no event shall the IEEE or any contributor to this code be liable for +%% any damages or losses, including, but not limited to, incidental, +%% consequential, or any other damages, resulting from the use or misuse +%% of any information contained here. +%% +%% All comments are the opinions of their respective authors and are not +%% necessarily endorsed by the IEEE. +%% +%% This work is distributed under the LaTeX Project Public License (LPPL) +%% ( http://www.latex-project.org/ ) version 1.3, and may be freely used, +%% distributed and modified. A copy of the LPPL, version 1.3, is included +%% in the base LaTeX documentation of all distributions of LaTeX released +%% 2003/12/01 or later. +%% Retain all contribution notices and credits. +%% ** Modified files should be clearly indicated as such, including ** +%% ** renaming them and changing author support contact information. ** +%% +%% File list of work: IEEEtran.cls, IEEEtran_HOWTO.pdf, bare_adv.tex, +%% bare_conf.tex, bare_jrnl.tex, bare_conf_compsoc.tex, +%% bare_jrnl_compsoc.tex +%% +%% Major changes to the user interface should be indicated by an +%% increase in the version numbers. If a version is a beta, it will +%% be indicated with a BETA suffix, i.e., 1.4 BETA. +%% Small changes can be indicated by appending letters to the version +%% such as "IEEEtran_v14a.cls". +%% In all cases, \Providesclass, any \typeout messages to the user, +%% \IEEEtransversionmajor and \IEEEtransversionminor must reflect the +%% correct version information. +%% The changes should also be documented via source comments. +%%************************************************************************* +%% +% +% Available class options +% e.g., \documentclass[10pt,conference]{IEEEtran} +% +% *** choose only one from each category *** +% +% 9pt, 10pt, 11pt, 12pt +% Sets normal font size. The default is 10pt. +% +% conference, journal, technote, peerreview, peerreviewca +% determines format mode - conference papers, journal papers, +% correspondence papers (technotes), or peer review papers. The user +% should also select 9pt when using technote. peerreview is like +% journal mode, but provides for a single-column "cover" title page for +% anonymous peer review. The paper title (without the author names) is +% repeated at the top of the page after the cover page. For peer review +% papers, the \IEEEpeerreviewmaketitle command must be executed (will +% automatically be ignored for non-peerreview modes) at the place the +% cover page is to end, usually just after the abstract (keywords are +% not normally used with peer review papers). peerreviewca is like +% peerreview, but allows the author names to be entered and formatted +% as with conference mode so that author affiliation and contact +% information can be easily seen on the cover page. +% The default is journal. +% +% draft, draftcls, draftclsnofoot, final +% determines if paper is formatted as a widely spaced draft (for +% handwritten editor comments) or as a properly typeset final version. +% draftcls restricts draft mode to the class file while all other LaTeX +% packages (i.e., \usepackage{graphicx}) will behave as final - allows +% for a draft paper with visible figures, etc. draftclsnofoot is like +% draftcls, but does not display the date and the word "DRAFT" at the foot +% of the pages. If using one of the draft modes, the user will probably +% also want to select onecolumn. +% The default is final. +% +% letterpaper, a4paper, cspaper +% determines paper size: 8.5in X 11in, 210mm X 297mm or 7.875in X 10.75in. +% Changing the paper size in the standard journal and conference modes +% will not alter the typesetting of the document - only the margins will +% be affected. In particular, documents using the a4paper option will +% have reduced side margins (A4 is narrower than US letter) and a longer +% bottom margin (A4 is longer than US letter). For both cases, the top +% margins will be the same and the text will be horizontally centered. +% For the compsoc conference and draft modes, it is the margins that will +% remain constant, and thus the text area size will vary, with changes in +% the paper size. +% The cspaper option is the special ``trim'' paper size (7.875in x 10.75in) +% used in the actual publication of Computer Society journals. Under +% compsoc journal mode, this option does not alter the typesetting of the +% document. Authors should invoke the cspaper option only if requested to +% do so by the editors of the specific journal they are submitting to. +% For final submission to the IEEE, authors should generally use US letter +% (8.5 X 11in) paper unless otherwise instructed. Note that authors should +% ensure that all post-processing (ps, pdf, etc.) uses the same paper +% specificiation as the .tex document. Problems here are by far the number +% one reason for incorrect margins. IEEEtran will automatically set the +% default paper size under pdflatex (without requiring any change to +% pdftex.cfg), so this issue is more important to dvips users. Fix +% config.ps, config.pdf, or ~/.dvipsrc for dvips, or use the +% dvips -t papersize option instead as needed. For the cspaper option, +% the corresponding dvips paper name is "ieeecs". +% See the testflow documentation +% http://www.ctan.org/tex-archive/macros/latex/contrib/IEEEtran/testflow +% for more details on dvips paper size configuration. +% The default is letterpaper. +% +% oneside, twoside +% determines if layout follows single sided or two sided (duplex) +% printing. The only notable change is with the headings at the top of +% the pages. +% The default is oneside. +% +% onecolumn, twocolumn +% determines if text is organized into one or two columns per page. One +% column mode is usually used only with draft papers. +% The default is twocolumn. +% +% comsoc, compsoc, transmag +% Use the format of the IEEE Communications Society, IEEE Computer Society +% or IEEE Transactions on Magnetics, respectively. +% +% romanappendices +% Use the "Appendix I" convention when numbering appendices. IEEEtran.cls +% now defaults to Alpha "Appendix A" convention - the opposite of what +% v1.6b and earlier did. +% +% captionsoff +% disables the display of the figure/table captions. Some IEEE journals +% request that captions be removed and figures/tables be put on pages +% of their own at the end of an initial paper submission. The endfloat +% package can be used with this class option to achieve this format. +% +% nofonttune +% turns off tuning of the font interword spacing. Maybe useful to those +% not using the standard Times fonts or for those who have already "tuned" +% their fonts. +% The default is to enable IEEEtran to tune font parameters. +% +% +%---------- +% Available CLASSINPUTs provided (all are macros unless otherwise noted): +% \CLASSINPUTbaselinestretch +% \CLASSINPUTinnersidemargin +% \CLASSINPUToutersidemargin +% \CLASSINPUTtoptextmargin +% \CLASSINPUTbottomtextmargin +% +% Available CLASSINFOs provided: +% \ifCLASSINFOpdf (TeX if conditional) +% \CLASSINFOpaperwidth (macro) +% \CLASSINFOpaperheight (macro) +% \CLASSINFOnormalsizebaselineskip (length) +% \CLASSINFOnormalsizeunitybaselineskip (length) +% +% Available CLASSOPTIONs provided: +% all class option flags (TeX if conditionals) unless otherwise noted, +% e.g., \ifCLASSOPTIONcaptionsoff +% point size options provided as a single macro: +% \CLASSOPTIONpt +% which will be defined as 9, 10, 11, or 12 depending on the document's +% normalsize point size. +% also, class option peerreviewca implies the use of class option peerreview +% and classoption draft implies the use of class option draftcls + + + + + +\ProvidesClass{IEEEtran}[2015/08/26 V1.8b by Michael Shell] +\typeout{-- See the "IEEEtran_HOWTO" manual for usage information.} +\typeout{-- http://www.michaelshell.org/tex/ieeetran/} +\NeedsTeXFormat{LaTeX2e} + +% IEEEtran.cls version numbers, provided as of V1.3 +% These values serve as a way a .tex file can +% determine if the new features are provided. +% The version number of this IEEEtrans.cls can be obtained from +% these values. i.e., V1.4 +% KEEP THESE AS INTEGERS! i.e., NO {4a} or anything like that- +% (no need to enumerate "a" minor changes here) +\def\IEEEtransversionmajor{1} +\def\IEEEtransversionminor{8} + + +% hook to allow easy changeover to IEEEtran.cls/tools.sty error reporting +\def\@IEEEclspkgerror{\ClassError{IEEEtran}} + + +% These do nothing, but provide them like in article.cls +\newif\if@restonecol +\newif\if@titlepage + + +% class option conditionals +\newif\ifCLASSOPTIONonecolumn \CLASSOPTIONonecolumnfalse +\newif\ifCLASSOPTIONtwocolumn \CLASSOPTIONtwocolumntrue + +\newif\ifCLASSOPTIONoneside \CLASSOPTIONonesidetrue +\newif\ifCLASSOPTIONtwoside \CLASSOPTIONtwosidefalse + +\newif\ifCLASSOPTIONfinal \CLASSOPTIONfinaltrue +\newif\ifCLASSOPTIONdraft \CLASSOPTIONdraftfalse +\newif\ifCLASSOPTIONdraftcls \CLASSOPTIONdraftclsfalse +\newif\ifCLASSOPTIONdraftclsnofoot \CLASSOPTIONdraftclsnofootfalse + +\newif\ifCLASSOPTIONpeerreview \CLASSOPTIONpeerreviewfalse +\newif\ifCLASSOPTIONpeerreviewca \CLASSOPTIONpeerreviewcafalse + +\newif\ifCLASSOPTIONjournal \CLASSOPTIONjournaltrue +\newif\ifCLASSOPTIONconference \CLASSOPTIONconferencefalse +\newif\ifCLASSOPTIONtechnote \CLASSOPTIONtechnotefalse + +\newif\ifCLASSOPTIONnofonttune \CLASSOPTIONnofonttunefalse + +\newif\ifCLASSOPTIONcaptionsoff \CLASSOPTIONcaptionsofffalse + +\newif\ifCLASSOPTIONcomsoc \CLASSOPTIONcomsocfalse +\newif\ifCLASSOPTIONcompsoc \CLASSOPTIONcompsocfalse +\newif\ifCLASSOPTIONtransmag \CLASSOPTIONtransmagfalse + +\newif\ifCLASSOPTIONromanappendices \CLASSOPTIONromanappendicesfalse + + +% class info conditionals + +% indicates if pdf (via pdflatex) output +\newif\ifCLASSINFOpdf \CLASSINFOpdffalse + + +% V1.6b internal flag to show if using a4paper +\newif\if@IEEEusingAfourpaper \@IEEEusingAfourpaperfalse +% V1.6b internal flag to show if using cspaper +\newif\if@IEEEusingcspaper \@IEEEusingcspaperfalse + + +% IEEEtran class scratch pad registers +% dimen +\newdimen\@IEEEtrantmpdimenA +\newdimen\@IEEEtrantmpdimenB +\newdimen\@IEEEtrantmpdimenC +% count +\newcount\@IEEEtrantmpcountA +\newcount\@IEEEtrantmpcountB +\newcount\@IEEEtrantmpcountC +% token list +\newtoks\@IEEEtrantmptoksA + +% we use \CLASSOPTIONpt so that we can ID the point size (even for 9pt docs) +% as well as LaTeX's \@ptsize to retain some compatability with some +% external packages +\def\@ptsize{0} +% LaTeX does not support 9pt, so we set \@ptsize to 0 - same as that of 10pt +\DeclareOption{9pt}{\def\CLASSOPTIONpt{9}\def\@ptsize{0}} +\DeclareOption{10pt}{\def\CLASSOPTIONpt{10}\def\@ptsize{0}} +\DeclareOption{11pt}{\def\CLASSOPTIONpt{11}\def\@ptsize{1}} +\DeclareOption{12pt}{\def\CLASSOPTIONpt{12}\def\@ptsize{2}} + + + +\DeclareOption{letterpaper}{\setlength{\paperwidth}{8.5in}% + \setlength{\paperheight}{11in}% + \@IEEEusingAfourpaperfalse + \@IEEEusingcspaperfalse + \def\CLASSOPTIONpaper{letter}% + \def\CLASSINFOpaperwidth{8.5in}% + \def\CLASSINFOpaperheight{11in}} + + +\DeclareOption{a4paper}{\setlength{\paperwidth}{210mm}% + \setlength{\paperheight}{297mm}% + \@IEEEusingAfourpapertrue + \@IEEEusingcspaperfalse + \def\CLASSOPTIONpaper{a4}% + \def\CLASSINFOpaperwidth{210mm}% + \def\CLASSINFOpaperheight{297mm}} + +% special paper option for compsoc journals +\DeclareOption{cspaper}{\setlength{\paperwidth}{7.875in}% + \setlength{\paperheight}{10.75in}% + \@IEEEusingcspapertrue + \@IEEEusingAfourpaperfalse + \def\CLASSOPTIONpaper{ieeecs}% + \def\CLASSINFOpaperwidth{7.875in}% + \def\CLASSINFOpaperheight{10.75in}} + +\DeclareOption{oneside}{\@twosidefalse\@mparswitchfalse + \CLASSOPTIONonesidetrue\CLASSOPTIONtwosidefalse} +\DeclareOption{twoside}{\@twosidetrue\@mparswitchtrue + \CLASSOPTIONtwosidetrue\CLASSOPTIONonesidefalse} + +\DeclareOption{onecolumn}{\CLASSOPTIONonecolumntrue\CLASSOPTIONtwocolumnfalse} +\DeclareOption{twocolumn}{\CLASSOPTIONtwocolumntrue\CLASSOPTIONonecolumnfalse} + +% If the user selects draft, then this class AND any packages +% will go into draft mode. +\DeclareOption{draft}{\CLASSOPTIONdrafttrue\CLASSOPTIONdraftclstrue + \CLASSOPTIONdraftclsnofootfalse} +% draftcls is for a draft mode which will not affect any packages +% used by the document. +\DeclareOption{draftcls}{\CLASSOPTIONdraftfalse\CLASSOPTIONdraftclstrue + \CLASSOPTIONdraftclsnofootfalse} +% draftclsnofoot is like draftcls, but without the footer. +\DeclareOption{draftclsnofoot}{\CLASSOPTIONdraftfalse\CLASSOPTIONdraftclstrue + \CLASSOPTIONdraftclsnofoottrue} +\DeclareOption{final}{\CLASSOPTIONdraftfalse\CLASSOPTIONdraftclsfalse + \CLASSOPTIONdraftclsnofootfalse} + +\DeclareOption{journal}{\CLASSOPTIONpeerreviewfalse\CLASSOPTIONpeerreviewcafalse + \CLASSOPTIONjournaltrue\CLASSOPTIONconferencefalse\CLASSOPTIONtechnotefalse} + +\DeclareOption{conference}{\CLASSOPTIONpeerreviewfalse\CLASSOPTIONpeerreviewcafalse + \CLASSOPTIONjournalfalse\CLASSOPTIONconferencetrue\CLASSOPTIONtechnotefalse} + +\DeclareOption{technote}{\CLASSOPTIONpeerreviewfalse\CLASSOPTIONpeerreviewcafalse + \CLASSOPTIONjournalfalse\CLASSOPTIONconferencefalse\CLASSOPTIONtechnotetrue} + +\DeclareOption{peerreview}{\CLASSOPTIONpeerreviewtrue\CLASSOPTIONpeerreviewcafalse + \CLASSOPTIONjournalfalse\CLASSOPTIONconferencefalse\CLASSOPTIONtechnotefalse} + +\DeclareOption{peerreviewca}{\CLASSOPTIONpeerreviewtrue\CLASSOPTIONpeerreviewcatrue + \CLASSOPTIONjournalfalse\CLASSOPTIONconferencefalse\CLASSOPTIONtechnotefalse} + +\DeclareOption{nofonttune}{\CLASSOPTIONnofonttunetrue} + +\DeclareOption{captionsoff}{\CLASSOPTIONcaptionsofftrue} + +\DeclareOption{comsoc}{\CLASSOPTIONcomsoctrue\CLASSOPTIONcompsocfalse\CLASSOPTIONtransmagfalse} + +\DeclareOption{compsoc}{\CLASSOPTIONcomsocfalse\CLASSOPTIONcompsoctrue\CLASSOPTIONtransmagfalse} + +\DeclareOption{transmag}{\CLASSOPTIONtransmagtrue\CLASSOPTIONcomsocfalse\CLASSOPTIONcompsocfalse} + +\DeclareOption{romanappendices}{\CLASSOPTIONromanappendicestrue} + + +% default to US letter paper, 10pt, twocolumn, one sided, final, journal +\ExecuteOptions{letterpaper,10pt,twocolumn,oneside,final,journal} +% overrride these defaults per user requests +\ProcessOptions + + + +%% -- Command Argument Scanning Support Functions -- + +% Sets the category codes for punctuation to their normal values. +% For local use with argument scanning. +\def\IEEEnormalcatcodespunct{\catcode`\!=12 \catcode`\,=12 \catcode`\:=12 +\catcode`\;=12 \catcode`\`=12 \catcode`\'=12 \catcode`\"=12 \catcode`\.=12 +\catcode`\/=12 \catcode`\?=12 \catcode`\*=12 \catcode`\+=12 \catcode`\-=12 +\catcode`\<=12 \catcode`\>=12 \catcode`\(=12 \catcode`\)=12 \catcode`\[=12 +\catcode`\]=12 \catcode`\==12 \catcode`\|=12} +% Sets the category codes for numbers to their normal values. +% For local use with argument scanning. +\def\IEEEnormalcatcodesnum{\catcode`\0=12 \catcode`\1=12 \catcode`\2=12 +\catcode`\3=12 \catcode`\4=12 \catcode`\5=12 \catcode`\6=12 \catcode`\7=12 +\catcode`\8=12 \catcode`\9=12} +% combined action of \IEEEnormalcatcodespunct and \IEEEnormalcatcodesnum +\def\IEEEnormalcatcodes{\IEEEnormalcatcodespunct\IEEEnormalcatcodesnum} + + +% usage: \@IEEEextracttoken*{} +% \@IEEEextracttoken fully expands its argument (which it then stores in +% \@IEEEextracttokenarg) via \edef and then the meaning of the first +% nonbrace (but including the empty group) token found is assigned via \let +% to \@IEEEextractedtoken as well as stored in the macro +% \@IEEEextractedtokenmacro. Tokens that would otherwise be discarded during +% the acquisition of the first are stored in \@IEEEextractedtokensdiscarded, +% however their original relative brace nesting depths are not guaranteed to +% be preserved. +% If the argument is empty, or if a first nonbrace token does not exist (or +% is an empty group), \@IEEEextractedtoken will be \relax and +% \@IEEEextractedtokenmacro and \@IEEEextractedtokensdiscarded will be empty. +% +% For example: +% \@IEEEextracttoken{{{ab}{cd}}{{ef}g}} +% results in: +% +% \@IEEEextracttokenarg ==> a macro containing {{ab}{cd}}{{ef}g} +% \@IEEEextractedtoken ==> the letter a +% \@IEEEextractedtokenmacro ==> a macro containing a +% \@IEEEextractedtokensdiscarded ==> a macro containing bcd{ef}g +% +% the *-star form, \@IEEEextracttoken*, does not expand its argument +% contents during processing. +\def\@IEEEextracttoken{\@ifstar{\let\@IEEEextracttokendef=\def\@@IEEEextracttoken}{\let\@IEEEextracttokendef=\edef\@@IEEEextracttoken}} + +\def\@@IEEEextracttoken#1{\@IEEEextracttokendef\@IEEEextracttokenarg{#1}\relax +\def\@IEEEextractedtokensdiscarded{}\relax % initialize to empty +% if the macro is unchanged after being acquired as a single undelimited argument +% with anything after it being stripped off as a delimited argument +% we know we have one token without any enclosing braces. loop until this is true. +\let\@IEEEextracttokencurgroup\@IEEEextracttokenarg +\loop + % trap case of an empty argument as this would cause a problem with + % \@@@IEEEextracttoken's first (nondelimited) argument acquisition + \ifx\@IEEEextracttokencurgroup\@empty + \def\@IEEEextractedtokenmacro{}\relax + \else + \expandafter\@@@IEEEextracttoken\@IEEEextracttokencurgroup\@IEEEgeneralsequenceDELIMITER\relax + \fi + \ifx\@IEEEextractedtokenmacro\@IEEEextracttokencurgroup + \else + \let\@IEEEextracttokencurgroup=\@IEEEextractedtokenmacro +\repeat +% we can safely do a \let= here because there should be at most one token +% the relax is needed to handle the case of no token found +\expandafter\let\expandafter\@IEEEextractedtoken\@IEEEextractedtokenmacro\relax} + +\def\@@@IEEEextracttoken#1#2\@IEEEgeneralsequenceDELIMITER{\def\@IEEEextractedtokenmacro{#1}\relax +\def\@@IEEEextractedtokensdiscarded{#2}\expandafter\expandafter\expandafter\def\expandafter\expandafter\expandafter +\@IEEEextractedtokensdiscarded\expandafter\expandafter\expandafter +{\expandafter\@@IEEEextractedtokensdiscarded\@IEEEextractedtokensdiscarded}} +%% +%% -- End of Command Argument Scanning Support Functions -- + + + +% Computer Society conditional execution command +\long\def\@IEEEcompsoconly#1{\relax\ifCLASSOPTIONcompsoc\relax#1\relax\fi\relax} +% inverse +\long\def\@IEEEnotcompsoconly#1{\relax\ifCLASSOPTIONcompsoc\else\relax#1\relax\fi\relax} +% compsoc conference +\long\def\@IEEEcompsocconfonly#1{\relax\ifCLASSOPTIONcompsoc\ifCLASSOPTIONconference\relax#1\relax\fi\fi\relax} +% compsoc not conference +\long\def\@IEEEcompsocnotconfonly#1{\relax\ifCLASSOPTIONcompsoc\ifCLASSOPTIONconference\else\relax#1\relax\fi\fi\relax} + + +% comsoc verify that newtxmath, mtpro2, mt11p or mathtime has been loaded +\def\@IEEEcomsocverifymathfont{\typeout{-- Verifying Times compatible math font.}\relax + \@ifpackageloaded{newtxmath}{\typeout{-- newtxmath loaded, OK.}}{\@@IEEEcomsocverifymathfont}} +\def\@@IEEEcomsocverifymathfont{\@ifpackageloaded{mtpro2}{\typeout{-- mtpro2 loaded, OK.}}{\@@@IEEEcomsocverifymathfont}} +\def\@@@IEEEcomsocverifymathfont{\@ifpackageloaded{mt11p}{\typeout{-- mt11p2 loaded, OK.}}{\@@@@IEEEcomsocverifymathfont}} +\def\@@@@IEEEcomsocverifymathfont{\@ifpackageloaded{mathtime}{\typeout{-- mathtime loaded, OK.}}{\@IEEEcomsocenforcemathfont}} + +% comsoc, if a Times math font was not loaded by user, enforce it +\def\@IEEEcomsocenforcemathfont{\typeout{** Times compatible math font not found, forcing.}\relax +\IfFileExists{newtxmath.sty}{\typeout{-- Found newtxmath, loading.}\RequirePackage{newtxmath}}{\@@IEEEcomsocenforcemathfont}} +\def\@@IEEEcomsocenforcemathfont{\IfFileExists{mtpro2.sty}{\typeout{-- Found mtpro2, loading.}\RequirePackage{mtpro2}}{\@@@IEEEcomsocenforcemathfont}} +\def\@@@IEEEcomsocenforcemathfont{\IfFileExists{mt11p.sty}{\typeout{-- Found mt11p, loading.}\RequirePackage{mt11p}}{\@@@@IEEEcomsocenforcemathfont}} +\def\@@@@IEEEcomsocenforcemathfont{\IfFileExists{mathtime.sty}{\typeout{-- Found mathtime, loading.}\RequirePackage{mathtime}}{\@@@@@IEEEcomsocenforcemathfont}} +% if no acceptable Times math font package found, error with newtxmath requirement +\def\@@@@@IEEEcomsocenforcemathfont{\typeout{** No Times compatible math font package found. newtxmath is required.}\RequirePackage{newtxmath}} + + +\ifCLASSOPTIONcomsoc + % ensure that if newtxmath is used, the cmintegrals option is also invoked + \PassOptionsToPackage{cmintegrals}{newtxmath} + % comsoc requires a Times like math font + % ensure this requirement is satisfied at document start + \AtBeginDocument{\@IEEEcomsocverifymathfont} +\fi + + + +% The IEEE uses Times Roman font, so we'll default to Times. +% These three commands make up the entire times.sty package. +\renewcommand{\sfdefault}{phv} +\renewcommand{\rmdefault}{ptm} +\renewcommand{\ttdefault}{pcr} + +% V1.7 compsoc nonconference papers, use Palatino/Palladio as the main text font, +% not Times Roman. +\@IEEEcompsocnotconfonly{\renewcommand{\rmdefault}{ppl}} + +% enable the selected main text font +\normalfont\selectfont + + +\ifCLASSOPTIONcomsoc + \typeout{-- Using IEEE Communications Society mode.} +\fi + +\ifCLASSOPTIONcompsoc + \typeout{-- Using IEEE Computer Society mode.} +\fi + + +% V1.7 conference notice message hook +\def\@IEEEconsolenoticeconference{\typeout{}% +\typeout{** Conference Paper **}% +\typeout{Before submitting the final camera ready copy, remember to:}% +\typeout{}% +\typeout{ 1. Manually equalize the lengths of two columns on the last page}% +\typeout{ of your paper;}% +\typeout{}% +\typeout{ 2. Ensure that any PostScript and/or PDF output post-processing}% +\typeout{ uses only Type 1 fonts and that every step in the generation}% +\typeout{ process uses the appropriate paper size.}% +\typeout{}} + + +% we can send console reminder messages to the user here +\AtEndDocument{\ifCLASSOPTIONconference\@IEEEconsolenoticeconference\fi} + + +% warn about the use of single column other than for draft mode +\ifCLASSOPTIONtwocolumn\else% + \ifCLASSOPTIONdraftcls\else% + \typeout{** ATTENTION: Single column mode is not typically used with IEEE publications.}% + \fi% +\fi + + +% V1.7 improved paper size setting code. +% Set pdfpage and dvips paper sizes. Conditional tests are similar to that +% of ifpdf.sty. Retain within {} to ensure tested macros are never altered, +% even if only effect is to set them to \relax. +% if \pdfoutput is undefined or equal to relax, output a dvips special +{\@ifundefined{pdfoutput}{\AtBeginDvi{\special{papersize=\CLASSINFOpaperwidth,\CLASSINFOpaperheight}}}{% +% pdfoutput is defined and not equal to \relax +% check for pdfpageheight existence just in case someone sets pdfoutput +% under non-pdflatex. If exists, set them regardless of value of \pdfoutput. +\@ifundefined{pdfpageheight}{\relax}{\global\pdfpagewidth\paperwidth +\global\pdfpageheight\paperheight}% +% if using \pdfoutput=0 under pdflatex, send dvips papersize special +\ifcase\pdfoutput +\AtBeginDvi{\special{papersize=\CLASSINFOpaperwidth,\CLASSINFOpaperheight}}% +\else +% we are using pdf output, set CLASSINFOpdf flag +\global\CLASSINFOpdftrue +\fi}} + +% let the user know the selected papersize +\typeout{-- Using \CLASSINFOpaperwidth\space x \CLASSINFOpaperheight\space +(\CLASSOPTIONpaper)\space paper.} + +\ifCLASSINFOpdf +\typeout{-- Using PDF output.} +\else +\typeout{-- Using DVI output.} +\fi + + +% The idea hinted here is for LaTeX to generate markleft{} and markright{} +% automatically for you after you enter \author{}, \journal{}, +% \journaldate{}, journalvol{}, \journalnum{}, etc. +% However, there may be some backward compatibility issues here as +% well as some special applications for IEEEtran.cls and special issues +% that may require the flexible \markleft{}, \markright{} and/or \markboth{}. +% We'll leave this as an open future suggestion. +%\newcommand{\journal}[1]{\def\@journal{#1}} +%\def\@journal{} + + + +% pointsize values +% used with ifx to determine the document's normal size +\def\@IEEEptsizenine{9} +\def\@IEEEptsizeten{10} +\def\@IEEEptsizeeleven{11} +\def\@IEEEptsizetwelve{12} + + + +% FONT DEFINITIONS (No sizexx.clo file needed) +% V1.6 revised font sizes, displayskip values and +% revised normalsize baselineskip to reduce underfull vbox problems +% on the 58pc = 696pt = 9.5in text height we want +% normalsize #lines/column baselineskip (aka leading) +% 9pt 63 11.0476pt (truncated down) +% 10pt 58 12pt (exact) +% 11pt 52 13.3846pt (truncated down) +% 12pt 50 13.92pt (exact) +% + +% we need to store the nominal baselineskip for the given font size +% in case baselinestretch ever changes. +% this is a dimen, so it will not hold stretch or shrink +\newdimen\@IEEEnormalsizeunitybaselineskip +\@IEEEnormalsizeunitybaselineskip\baselineskip + + + +%% ******* WARNING! ******* +%% +%% Authors should not alter font sizes, baselineskip ("leading"), +%% margins or other spacing values in an attempt to squeeze more +%% material on each page. +%% +%% The IEEE's own typesetting software will restore the correct +%% values when re-typesetting/proofing the submitted document, +%% possibly resulting in unexpected article over length charges. +%% +%% ******* WARNING! ******* + + +% 9pt option defaults +\ifx\CLASSOPTIONpt\@IEEEptsizenine +\typeout{-- This is a 9 point document.} +\def\normalsize{\@setfontsize{\normalsize}{9}{11.0476pt}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{11.0476pt} +\normalsize +\abovedisplayskip 1.5ex plus 3pt minus 1pt +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 3pt +\belowdisplayshortskip 1.5ex plus 3pt minus 1pt +\def\small{\@setfontsize{\small}{8.5}{10pt}} +\def\footnotesize{\@setfontsize{\footnotesize}{8}{9pt}} +\def\scriptsize{\@setfontsize{\scriptsize}{7}{8pt}} +\def\tiny{\@setfontsize{\tiny}{5}{6pt}} +% sublargesize is the same as large - 10pt +\def\sublargesize{\@setfontsize{\sublargesize}{10}{12pt}} +\def\large{\@setfontsize{\large}{10}{12pt}} +\def\Large{\@setfontsize{\Large}{12}{14pt}} +\def\LARGE{\@setfontsize{\LARGE}{14}{17pt}} +\def\huge{\@setfontsize{\huge}{17}{20pt}} +\def\Huge{\@setfontsize{\Huge}{20}{24pt}} +\fi +% +% 10pt option defaults +\ifx\CLASSOPTIONpt\@IEEEptsizeten +\typeout{-- This is a 10 point document.} +\def\normalsize{\@setfontsize{\normalsize}{10}{12.00pt}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{12pt} +\normalsize +\abovedisplayskip 1.5ex plus 4pt minus 2pt +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 4pt +\belowdisplayshortskip 1.5ex plus 4pt minus 2pt +\def\small{\@setfontsize{\small}{9}{10pt}} +\def\footnotesize{\@setfontsize{\footnotesize}{8}{9pt}} +\def\scriptsize{\@setfontsize{\scriptsize}{7}{8pt}} +\def\tiny{\@setfontsize{\tiny}{5}{6pt}} +% sublargesize is a tad smaller than large - 11pt +\def\sublargesize{\@setfontsize{\sublargesize}{11}{13.4pt}} +\def\large{\@setfontsize{\large}{12}{14pt}} +\def\Large{\@setfontsize{\Large}{14}{17pt}} +\def\LARGE{\@setfontsize{\LARGE}{17}{20pt}} +\def\huge{\@setfontsize{\huge}{20}{24pt}} +\def\Huge{\@setfontsize{\Huge}{24}{28pt}} +\fi +% +% 11pt option defaults +\ifx\CLASSOPTIONpt\@IEEEptsizeeleven +\typeout{-- This is an 11 point document.} +\def\normalsize{\@setfontsize{\normalsize}{11}{13.3846pt}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{13.3846pt} +\normalsize +\abovedisplayskip 1.5ex plus 5pt minus 3pt +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 5pt +\belowdisplayshortskip 1.5ex plus 5pt minus 3pt +\def\small{\@setfontsize{\small}{10}{12pt}} +\def\footnotesize{\@setfontsize{\footnotesize}{9}{10.5pt}} +\def\scriptsize{\@setfontsize{\scriptsize}{8}{9pt}} +\def\tiny{\@setfontsize{\tiny}{6}{7pt}} +% sublargesize is the same as large - 12pt +\def\sublargesize{\@setfontsize{\sublargesize}{12}{14pt}} +\def\large{\@setfontsize{\large}{12}{14pt}} +\def\Large{\@setfontsize{\Large}{14}{17pt}} +\def\LARGE{\@setfontsize{\LARGE}{17}{20pt}} +\def\huge{\@setfontsize{\huge}{20}{24pt}} +\def\Huge{\@setfontsize{\Huge}{24}{28pt}} +\fi +% +% 12pt option defaults +\ifx\CLASSOPTIONpt\@IEEEptsizetwelve +\typeout{-- This is a 12 point document.} +\def\normalsize{\@setfontsize{\normalsize}{12}{13.92pt}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{13.92pt} +\normalsize +\abovedisplayskip 1.5ex plus 6pt minus 4pt +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 6pt +\belowdisplayshortskip 1.5ex plus 6pt minus 4pt +\def\small{\@setfontsize{\small}{10}{12pt}} +\def\footnotesize{\@setfontsize{\footnotesize}{9}{10.5pt}} +\def\scriptsize{\@setfontsize{\scriptsize}{8}{9pt}} +\def\tiny{\@setfontsize{\tiny}{6}{7pt}} +% sublargesize is the same as large - 14pt +\def\sublargesize{\@setfontsize{\sublargesize}{14}{17pt}} +\def\large{\@setfontsize{\large}{14}{17pt}} +\def\Large{\@setfontsize{\Large}{17}{20pt}} +\def\LARGE{\@setfontsize{\LARGE}{20}{24pt}} +\def\huge{\@setfontsize{\huge}{22}{26pt}} +\def\Huge{\@setfontsize{\Huge}{24}{28pt}} +\fi + + + +% V1.8a compsoc font sizes +% compsoc font sizes use bp "Postscript" point units (1/72in) +% rather than the traditional pt (1/72.27) +\ifCLASSOPTIONcompsoc +% -- compsoc defaults -- +% ** will override some of these values later ** +% 9pt +\ifx\CLASSOPTIONpt\@IEEEptsizenine +\def\normalsize{\@setfontsize{\normalsize}{9bp}{11bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{11bp} +\normalsize +\abovedisplayskip 1.5ex plus 3bp minus 1bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0bp plus 3bp +\belowdisplayshortskip 1.5ex plus 3bp minus 1bp +\def\small{\@setfontsize{\small}{8.5bp}{10bp}} +\def\footnotesize{\@setfontsize{\footnotesize}{8bp}{9bp}} +\def\scriptsize{\@setfontsize{\scriptsize}{7bp}{8bp}} +\def\tiny{\@setfontsize{\tiny}{5bp}{6bp}} +% sublargesize is the same as large - 10bp +\def\sublargesize{\@setfontsize{\sublargesize}{10bp}{12bp}} +\def\large{\@setfontsize{\large}{10bp}{12bp}} +\def\Large{\@setfontsize{\Large}{12bp}{14bp}} +\def\LARGE{\@setfontsize{\LARGE}{14bp}{17bp}} +\def\huge{\@setfontsize{\huge}{17bp}{20bp}} +\def\Huge{\@setfontsize{\Huge}{20bp}{24bp}} +\fi +% +% 10pt +\ifx\CLASSOPTIONpt\@IEEEptsizeten +\def\normalsize{\@setfontsize{\normalsize}{10bp}{12bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{12bp} +\normalsize +\abovedisplayskip 1.5ex plus 4bp minus 2bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 4bp +\belowdisplayshortskip 1.5ex plus 4bp minus 2bp +\def\small{\@setfontsize{\small}{9bp}{10bp}} +\def\footnotesize{\@setfontsize{\footnotesize}{8bp}{9bp}} +\def\scriptsize{\@setfontsize{\scriptsize}{7bp}{8bp}} +\def\tiny{\@setfontsize{\tiny}{5bp}{6bp}} +% sublargesize is a tad smaller than large - 11bp +\def\sublargesize{\@setfontsize{\sublargesize}{11bp}{13.5bp}} +\def\large{\@setfontsize{\large}{12bp}{14bp}} +\def\Large{\@setfontsize{\Large}{14bp}{17bp}} +\def\LARGE{\@setfontsize{\LARGE}{17bp}{20bp}} +\def\huge{\@setfontsize{\huge}{20bp}{24bp}} +\def\Huge{\@setfontsize{\Huge}{24bp}{28bp}} +\fi +% +% 11pt +\ifx\CLASSOPTIONpt\@IEEEptsizeeleven +\def\normalsize{\@setfontsize{\normalsize}{11bp}{13.5bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{13.5bp} +\normalsize +\abovedisplayskip 1.5ex plus 5bp minus 3bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 5bp +\belowdisplayshortskip 1.5ex plus 5bp minus 3bp +\def\small{\@setfontsize{\small}{10bp}{12bp}} +\def\footnotesize{\@setfontsize{\footnotesize}{9bp}{10.5bp}} +\def\scriptsize{\@setfontsize{\scriptsize}{8bp}{9bp}} +\def\tiny{\@setfontsize{\tiny}{6bp}{7bp}} +% sublargesize is the same as large - 12bp +\def\sublargesize{\@setfontsize{\sublargesize}{12bp}{14bp}} +\def\large{\@setfontsize{\large}{12bp}{14bp}} +\def\Large{\@setfontsize{\Large}{14bp}{17bp}} +\def\LARGE{\@setfontsize{\LARGE}{17bp}{20bp}} +\def\huge{\@setfontsize{\huge}{20bp}{24bp}} +\def\Huge{\@setfontsize{\Huge}{24bp}{28bp}} +\fi +% +% 12pt +\ifx\CLASSOPTIONpt\@IEEEptsizetwelve +\def\normalsize{\@setfontsize{\normalsize}{12bp}{14bp}}% +\setlength{\@IEEEnormalsizeunitybaselineskip}{14bp}% +\normalsize +\abovedisplayskip 1.5ex plus 6bp minus 4bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 6bp +\belowdisplayshortskip 1.5ex plus 6bp minus 4bp +\def\small{\@setfontsize{\small}{10bp}{12bp}} +\def\footnotesize{\@setfontsize{\footnotesize}{9bp}{10.5bp}} +\def\scriptsize{\@setfontsize{\scriptsize}{8bp}{9bp}} +\def\tiny{\@setfontsize{\tiny}{6bp}{7bp}} +% sublargesize is the same as large - 14bp +\def\sublargesize{\@setfontsize{\sublargesize}{14bp}{17bp}} +\def\large{\@setfontsize{\large}{14bp}{17bp}} +\def\Large{\@setfontsize{\Large}{17bp}{20bp}} +\def\LARGE{\@setfontsize{\LARGE}{20bp}{24bp}} +\def\huge{\@setfontsize{\huge}{22bp}{26bp}} +\def\Huge{\@setfontsize{\Huge}{24bp}{28bp}} +\fi +% +% -- override defaults: compsoc journals use special normalsizes -- +\ifCLASSOPTIONconference +% +% compsoc conferences +% 9pt +\ifx\CLASSOPTIONpt\@IEEEptsizenine +\def\normalsize{\@setfontsize{\normalsize}{9bp}{10.8bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{10.8bp} +\normalsize +\abovedisplayskip 1.5ex plus 3bp minus 1bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0bp plus 3bp +\belowdisplayshortskip 1.5ex plus 3bp minus 1bp +\fi +% 10pt +\ifx\CLASSOPTIONpt\@IEEEptsizeten +\def\normalsize{\@setfontsize{\normalsize}{10bp}{11.2bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{11.2bp} +\normalsize +\abovedisplayskip 1.5ex plus 4bp minus 2bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 4bp +\belowdisplayshortskip 1.5ex plus 4bp minus 2bp +\fi +% 11pt +\ifx\CLASSOPTIONpt\@IEEEptsizeeleven +\def\normalsize{\@setfontsize{\normalsize}{11bp}{13.2bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{13.2bp} +\normalsize +\abovedisplayskip 1.5ex plus 5bp minus 3bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 5bp +\belowdisplayshortskip 1.5ex plus 5bp minus 3bp +\fi +% 12pt +\ifx\CLASSOPTIONpt\@IEEEptsizetwelve +\def\normalsize{\@setfontsize{\normalsize}{12bp}{14.4bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{14.4bp} +\normalsize +\abovedisplayskip 1.5ex plus 6bp minus 4bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 6bp +\belowdisplayshortskip 1.5ex plus 6bp minus 4bp +\fi +% +% compsoc nonconferences +\else +% 9pt +\ifx\CLASSOPTIONpt\@IEEEptsizenine +\def\normalsize{\@setfontsize{\normalsize}{9bp}{10.8bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{10.8bp} +\normalsize +\abovedisplayskip 1.5ex plus 3bp minus 1bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0bp plus 3bp +\belowdisplayshortskip 1.5ex plus 3bp minus 1bp +\fi +% 10pt +\ifx\CLASSOPTIONpt\@IEEEptsizeten +% the official spec is 9.5bp with 11.4bp leading for 10pt, +% but measurements of proofs suggest upto 11.723bp leading +% here we'll use 11.54bp which gives 61 lines per column +% with the standard compsoc margins +\def\normalsize{\@setfontsize{\normalsize}{9.5bp}{11.54bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{11.54bp} +\normalsize +\abovedisplayskip 1.5ex plus 4bp minus 2bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 4bp +\belowdisplayshortskip 1.5ex plus 4bp minus 2bp +\fi +% 11pt +\ifx\CLASSOPTIONpt\@IEEEptsizeeleven +\def\normalsize{\@setfontsize{\normalsize}{11bp}{13.2bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{13.2bp} +\normalsize +\abovedisplayskip 1.5ex plus 5bp minus 3bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 5bp +\belowdisplayshortskip 1.5ex plus 5bp minus 3bp +\fi +% 12pt +\ifx\CLASSOPTIONpt\@IEEEptsizetwelve +\def\normalsize{\@setfontsize{\normalsize}{12bp}{14.4bp}} +\setlength{\@IEEEnormalsizeunitybaselineskip}{14.4bp} +\normalsize +\abovedisplayskip 1.5ex plus 6bp minus 4bp +\belowdisplayskip \abovedisplayskip +\abovedisplayshortskip 0pt plus 6bp +\belowdisplayshortskip 1.5ex plus 6bp minus 4bp +\fi +\fi\fi + + + + +% V1.6 The Computer Modern Fonts will issue a substitution warning for +% 24pt titles (24.88pt is used instead, but the default and correct +% Times font will scale exactly as needed) increase the substitution +% tolerance to turn off this warning. +% +% V1.8a, the compsoc bp font sizes can also cause bogus font substitution +% warnings with footnote or scriptsize math and the $\bullet$ itemized +% list of \IEEEcompsocitemizethanks. So, increase this to 1.5pt or more. +\def\fontsubfuzz{1.7bp} + + +% warn the user in case they forget to use the 9pt option with +% technote +\ifCLASSOPTIONtechnote% + \ifx\CLASSOPTIONpt\@IEEEptsizenine\else% + \typeout{** ATTENTION: Technotes are normally 9pt documents.}% + \fi% +\fi + + +% V1.7 +% Improved \textunderscore to provide a much better fake _ when used with +% OT1 encoding. Under OT1, detect use of pcr or cmtt \ttfamily and use +% available true _ glyph for those two typewriter fonts. +\def\@IEEEstringptm{ptm} % Times Roman family +\def\@IEEEstringppl{ppl} % Palatino Roman family +\def\@IEEEstringphv{phv} % Helvetica Sans Serif family +\def\@IEEEstringpcr{pcr} % Courier typewriter family +\def\@IEEEstringcmtt{cmtt} % Computer Modern typewriter family +\DeclareTextCommandDefault{\textunderscore}{\leavevmode +\ifx\f@family\@IEEEstringpcr\string_\else +\ifx\f@family\@IEEEstringcmtt\string_\else +\ifx\f@family\@IEEEstringptm\kern 0em\vbox{\hrule\@width 0.5em\@height 0.5pt\kern -0.3ex}\else +\ifx\f@family\@IEEEstringppl\kern 0em\vbox{\hrule\@width 0.5em\@height 0.5pt\kern -0.3ex}\else +\ifx\f@family\@IEEEstringphv\kern -0.03em\vbox{\hrule\@width 0.62em\@height 0.52pt\kern -0.33ex}\kern -0.03em\else +\kern 0.09em\vbox{\hrule\@width 0.6em\@height 0.44pt\kern -0.63pt\kern -0.42ex}\kern 0.09em\fi\fi\fi\fi\fi\relax} + + + + +% set the default \baselinestretch +\def\baselinestretch{1} +\ifCLASSOPTIONdraftcls + \def\baselinestretch{1.5}% default baselinestretch for draft modes +\fi + + +% process CLASSINPUT baselinestretch +\ifx\CLASSINPUTbaselinestretch\@IEEEundefined +\else + \edef\baselinestretch{\CLASSINPUTbaselinestretch} % user CLASSINPUT override + \typeout{** ATTENTION: Overriding \string\baselinestretch\space to + \baselinestretch\space via \string\CLASSINPUT.} +\fi + +\small\normalsize % make \baselinestretch take affect + + + + +% store the normalsize baselineskip +\newdimen\CLASSINFOnormalsizebaselineskip +\CLASSINFOnormalsizebaselineskip=\baselineskip\relax +% and the normalsize unity (baselinestretch=1) baselineskip +% we could save a register by giving the user access to +% \@IEEEnormalsizeunitybaselineskip. However, let's protect +% its read only internal status +\newdimen\CLASSINFOnormalsizeunitybaselineskip +\CLASSINFOnormalsizeunitybaselineskip=\@IEEEnormalsizeunitybaselineskip\relax +% store the nominal value of jot +\newdimen\IEEEnormaljot +\IEEEnormaljot=0.25\baselineskip\relax + +% set \jot +\jot=\IEEEnormaljot\relax + + + + +% V1.6, we are now going to fine tune the interword spacing +% The default interword glue for Times under TeX appears to use a +% nominal interword spacing of 25% (relative to the font size, i.e., 1em) +% a maximum of 40% and a minimum of 19%. +% For example, 10pt text uses an interword glue of: +% +% 2.5pt plus 1.49998pt minus 0.59998pt +% +% However, the IEEE allows for a more generous range which reduces the need +% for hyphenation, especially for two column text. Furthermore, the IEEE +% tends to use a little bit more nominal space between the words. +% The IEEE's interword spacing percentages appear to be: +% 35% nominal +% 23% minimum +% 50% maximum +% (They may even be using a tad more for the largest fonts such as 24pt.) +% +% for bold text, the IEEE increases the spacing a little more: +% 37.5% nominal +% 23% minimum +% 55% maximum + +% here are the interword spacing ratios we'll use +% for medium (normal weight) +\def\@IEEEinterspaceratioM{0.35} +\def\@IEEEinterspaceMINratioM{0.23} +\def\@IEEEinterspaceMAXratioM{0.50} + +% for bold +\def\@IEEEinterspaceratioB{0.375} +\def\@IEEEinterspaceMINratioB{0.23} +\def\@IEEEinterspaceMAXratioB{0.55} + + +% compsoc nonconference papers use Palatino, +% tweak settings to better match the proofs +\ifCLASSOPTIONcompsoc +\ifCLASSOPTIONconference\else +% for medium (normal weight) +\def\@IEEEinterspaceratioM{0.28} +\def\@IEEEinterspaceMINratioM{0.21} +\def\@IEEEinterspaceMAXratioM{0.47} +% for bold +\def\@IEEEinterspaceratioB{0.305} +\def\@IEEEinterspaceMINratioB{0.21} +\def\@IEEEinterspaceMAXratioB{0.52} +\fi\fi + + +% command to revise the interword spacing for the current font under TeX: +% \fontdimen2 = nominal interword space +% \fontdimen3 = interword stretch +% \fontdimen4 = interword shrink +% since all changes to the \fontdimen are global, we can enclose these commands +% in braces to confine any font attribute or length changes +\def\@@@IEEEsetfontdimens#1#2#3{{% +\setlength{\@IEEEtrantmpdimenB}{\f@size pt}% grab the font size in pt, could use 1em instead. +\setlength{\@IEEEtrantmpdimenA}{#1\@IEEEtrantmpdimenB}% +\fontdimen2\font=\@IEEEtrantmpdimenA\relax +\addtolength{\@IEEEtrantmpdimenA}{-#2\@IEEEtrantmpdimenB}% +\fontdimen3\font=-\@IEEEtrantmpdimenA\relax +\setlength{\@IEEEtrantmpdimenA}{#1\@IEEEtrantmpdimenB}% +\addtolength{\@IEEEtrantmpdimenA}{-#3\@IEEEtrantmpdimenB}% +\fontdimen4\font=\@IEEEtrantmpdimenA\relax}} + +% revise the interword spacing for each font weight +\def\@@IEEEsetfontdimens{{% +\mdseries +\@@@IEEEsetfontdimens{\@IEEEinterspaceratioM}{\@IEEEinterspaceMAXratioM}{\@IEEEinterspaceMINratioM}% +\bfseries +\@@@IEEEsetfontdimens{\@IEEEinterspaceratioB}{\@IEEEinterspaceMAXratioB}{\@IEEEinterspaceMINratioB}% +}} + +% revise the interword spacing for each font shape +% \slshape is not often used for IEEE work and is not altered here. The \scshape caps are +% already a tad too large in the free LaTeX fonts (as compared to what the IEEE uses) so we +% won't alter these either. +\def\@IEEEsetfontdimens{{% +\normalfont +\@@IEEEsetfontdimens +\normalfont\itshape +\@@IEEEsetfontdimens +}} + +% command to revise the interword spacing for each font size (and shape +% and weight). Only the \rmfamily is done here as \ttfamily uses a +% fixed spacing and \sffamily is not used as the main text of IEEE papers. +\def\@IEEEtunefonts{{\selectfont\rmfamily +\tiny\@IEEEsetfontdimens +\scriptsize\@IEEEsetfontdimens +\footnotesize\@IEEEsetfontdimens +\small\@IEEEsetfontdimens +\normalsize\@IEEEsetfontdimens +\sublargesize\@IEEEsetfontdimens +\large\@IEEEsetfontdimens +\LARGE\@IEEEsetfontdimens +\huge\@IEEEsetfontdimens +\Huge\@IEEEsetfontdimens}} + +% if the nofonttune class option is not given, revise the interword spacing +% now - in case IEEEtran makes any default length measurements, and make +% sure all the default fonts are loaded +\ifCLASSOPTIONnofonttune\else +\@IEEEtunefonts +\fi + +% and again at the start of the document in case the user loaded different fonts +\AtBeginDocument{\ifCLASSOPTIONnofonttune\else\@IEEEtunefonts\fi} + + + + + +% -- V1.8a page setup commands -- + +% The default sample text for calculating margins +% Note that IEEE publications use \scriptsize for headers and footers. +\def\IEEEdefaultsampletext{\normalfont\normalsize gT} +\def\IEEEdefaultheadersampletext{\normalfont\scriptsize T}% IEEE headers default to uppercase +\def\IEEEdefaultfootersampletext{\normalfont\scriptsize gT} + + + +% usage: \IEEEsettextwidth{inner margin}{outer margin} +% Sets \textwidth to allow the specified inner and outer margins +% for the current \paperwidth. +\def\IEEEsettextwidth#1#2{\@IEEEtrantmpdimenA\paperwidth +\@IEEEtrantmpdimenB#1\relax +\advance\@IEEEtrantmpdimenA by -\@IEEEtrantmpdimenB +\@IEEEtrantmpdimenB#2\relax +\advance\@IEEEtrantmpdimenA by -\@IEEEtrantmpdimenB +\textwidth\@IEEEtrantmpdimenA} + + + +% usage: \IEEEsetsidemargin{mode: i, o, c, a}{margin/offset} +% Sets \oddsidemargin and \evensidemargin to yield the specified margin +% of the given mode. +% The available modes are: +% i = inner margin +% o = outer margin +% c = centered, with the given offset +% a = adjust the margins using the given offset +% For the offsets, positive values increase the inner margin. +% \textwidth should be set properly for the given margins before calling this +% function. +\def\IEEEsetsidemargin#1#2{\@IEEEtrantmpdimenA #2\relax +\@IEEEextracttoken{#1}\relax +% check for mode errors +\ifx\@IEEEextractedtokenmacro\@empty + \@IEEEclspkgerror{Empty mode type in \string\IEEEsetsidemargin\space (line \the\inputlineno).\MessageBreak + Defaulting to `i'}{Valid modes for \string\IEEEsetsidemargin\space are: i, o, c and a.}\relax + \let\@IEEEextractedtoken=i\relax + \def\@IEEEextractedtokenmacro{i}\relax +\else + \ifx\@IEEEextractedtokensdiscarded\@empty\else + \typeout{** WARNING: \string\IEEEsetsidemargin\space mode specifiers after the first in `\@IEEEextracttokenarg' ignored (line \the\inputlineno).}\relax + \fi +\fi +% handle each mode +\if\@IEEEextractedtoken a\relax + \advance\oddsidemargin by \@IEEEtrantmpdimenA\relax +\else +\if\@IEEEextractedtoken c\relax + \oddsidemargin\paperwidth + \advance\oddsidemargin by -\textwidth + \divide\oddsidemargin by 2\relax + \advance\oddsidemargin by -1in\relax + \advance\oddsidemargin by \@IEEEtrantmpdimenA\relax +\else +\if\@IEEEextractedtoken o\relax + \oddsidemargin\paperwidth + \advance\oddsidemargin by -\textwidth + \advance\oddsidemargin by -\@IEEEtrantmpdimenA + \advance\oddsidemargin by -1in\relax +\else + \if\@IEEEextractedtoken i\relax + \else + \@IEEEclspkgerror{Unknown mode type `\@IEEEextractedtokenmacro' in \string\IEEEsetsidemargin\space (line \the\inputlineno).\MessageBreak + Defaulting to `i'}% + {Valid modes for \string\IEEEsetsidemargin\space are: i, o, c and a.}% + \fi + \oddsidemargin\@IEEEtrantmpdimenA + \advance\oddsidemargin by -1in\relax +\fi\fi\fi +% odd and even side margins both mean "inner" for single sided pages +\evensidemargin\oddsidemargin +% but are mirrors of each other when twosided is in effect +\if@twoside + \evensidemargin\paperwidth + \advance\evensidemargin by -\textwidth + \advance\evensidemargin by -\oddsidemargin + % have to compensate for both the builtin 1in LaTex offset + % and the fact we already subtracted this offset from \oddsidemargin + \advance\evensidemargin -2in\relax +\fi} + + + +% usage: \IEEEsettextheight[sample text]{top text margin}{bottom text margin} +% Sets \textheight based on the specified top margin and bottom margin. +% Takes into consideration \paperheight, \topskip, and (by default) the +% the actual height and depth of the \IEEEdefaultsampletext text. +\def\IEEEsettextheight{\@ifnextchar [{\@IEEEsettextheight}{\@IEEEsettextheight[\IEEEdefaultsampletext]}} +\def\@IEEEsettextheight[#1]#2#3{\textheight\paperheight\relax + \@IEEEtrantmpdimenA #2\relax + \advance \textheight by -\@IEEEtrantmpdimenA% subtract top margin + \@IEEEtrantmpdimenA #3\relax + \advance \textheight by -\@IEEEtrantmpdimenA% subtract bottom margin + \advance \textheight by \topskip% add \topskip + % subtract off everything above the top, and below the bottom, baselines + \settoheight{\@IEEEtrantmpdimenA}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance \textheight by -\@IEEEtrantmpdimenA + \settodepth{\@IEEEtrantmpdimenA}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance \textheight by -\@IEEEtrantmpdimenA} + + + +\newdimen\IEEEquantizedlength +\IEEEquantizedlength 0sp\relax +\newdimen\IEEEquantizedlengthdiff +\IEEEquantizedlengthdiff 0sp\relax +\def\IEEEquantizedlengthint{0} + +% usage: \IEEEquantizelength{mode: d, c, i}{base unit}{length} +% Sets the length \IEEEquantizedlength to be an integer multiple of the given +% (nonzero) base unit such that \IEEEquantizedlength approximates the given +% length. +% \IEEEquantizedlengthdiff is a length equal to the difference between the +% \IEEEquantizedlength and the given length. +% \IEEEquantizedlengthint is a macro containing the integer number of base units +% in \IEEEquantizedlength. +% i.e., \IEEEquantizedlength = \IEEEquantizedlengthint * base unit +% The mode determines how \IEEEquantizedlength is quantized: +% d = always decrease (always round down \IEEEquantizeint) +% c = use the closest match +% i = always increase (always round up \IEEEquantizeint) +% In anycase, if the given length is already quantized, +% \IEEEquantizedlengthdiff will be set to zero. +\def\IEEEquantizelength#1#2#3{\begingroup +% work in isolation so as not to externally disturb the \@IEEEtrantmp +% variables +% load the argument values indirectly via \IEEEquantizedlengthdiff +% in case the user refers to our \@IEEEtrantmpdimenX, \IEEEquantizedlength, +% etc. in the arguments. we also will work with these as counters, +% i.e., in sp units +% A has the base unit +\IEEEquantizedlengthdiff #2\relax\relax\relax\relax +\@IEEEtrantmpcountA\IEEEquantizedlengthdiff +% B has the input length +\IEEEquantizedlengthdiff #3\relax\relax\relax\relax +\@IEEEtrantmpcountB\IEEEquantizedlengthdiff +\@IEEEtrantmpdimenA\the\@IEEEtrantmpcountA sp\relax +\@IEEEtrantmpdimenB\the\@IEEEtrantmpcountB sp\relax +% \@IEEEtrantmpcountC will have the quantized int +% \IEEEquantizedlength will have the quantized length +% \@IEEEtrantmpdimenC will have the quantized diff +% initialize them to zero as this is what will be +% exported if an error occurs +\@IEEEtrantmpcountC 0\relax +\IEEEquantizedlength 0sp\relax +\@IEEEtrantmpdimenC 0sp\relax +% extract mode +\@IEEEextracttoken{#1}\relax +% check for mode errors +\ifx\@IEEEextractedtokenmacro\@empty + \@IEEEclspkgerror{Empty mode type in \string\IEEEquantizelength\space (line \the\inputlineno).\MessageBreak + Defaulting to `d'}{Valid modes for \string\IEEEquantizelength\space are: d, c and i.}\relax + \let\@IEEEextractedtoken=d\relax + \def\@IEEEextractedtokenmacro{d}\relax +\else + \ifx\@IEEEextractedtokensdiscarded\@empty\else + \typeout{** WARNING: \string\IEEEquantizelength\space mode specifiers after the first in `\@IEEEextracttokenarg' ignored (line \the\inputlineno).}\relax + \fi +\fi +% check for base unit is zero error +\ifnum\@IEEEtrantmpcountA=0\relax +\@IEEEclspkgerror{Base unit is zero in \string\IEEEquantizelength\space (line \the\inputlineno).\MessageBreak + \string\IEEEquantizedlength\space and \string\IEEEquantizedlengthdiff\space are set to zero}{Division by zero is not allowed.}\relax +\else% base unit is nonzero + % \@IEEEtrantmpcountC carries the number of integer units + % in the quantized length (integer length \ base) + \@IEEEtrantmpcountC\@IEEEtrantmpcountB\relax + \divide\@IEEEtrantmpcountC by \@IEEEtrantmpcountA\relax + % \IEEEquantizedlength has the (rounded down) quantized length + % = base * int + \IEEEquantizedlength\@IEEEtrantmpdimenA\relax + \multiply\IEEEquantizedlength by \@IEEEtrantmpcountC\relax + % \@IEEEtrantmpdimenC has the difference + % = quantized length - length + \@IEEEtrantmpdimenC\IEEEquantizedlength\relax + \advance\@IEEEtrantmpdimenC by -\@IEEEtrantmpdimenB\relax + % trap special case of length being already quantized + % to avoid a roundup under i option + \ifdim\@IEEEtrantmpdimenC=0sp\relax + \else % length not is already quantized + % set dimenA to carry the upper quantized (absolute value) difference: + % quantizedlength + base - length + \advance\@IEEEtrantmpdimenA by \IEEEquantizedlength\relax + \advance\@IEEEtrantmpdimenA by -\@IEEEtrantmpdimenB\relax + % set dimenB to carry the lower quantized (absolute value) difference: + % length - quantizedlength + \advance\@IEEEtrantmpdimenB by -\IEEEquantizedlength\relax + % handle each mode + \if\@IEEEextractedtoken c\relax + % compare upper and lower amounts, select upper if lower > upper + \ifdim\@IEEEtrantmpdimenB>\@IEEEtrantmpdimenA\relax + % use upper + \advance\IEEEquantizedlength by \the\@IEEEtrantmpcountA sp\relax + \advance\@IEEEtrantmpcountC by 1\relax + \@IEEEtrantmpdimenC\@IEEEtrantmpdimenA + \else% <=. uselower + % no need to do anything for lower, use output values already setup + \fi + \else% not mode c + \if\@IEEEextractedtoken i\relax + % always round up under i mode + \advance\IEEEquantizedlength by \the\@IEEEtrantmpcountA sp\relax + \advance\@IEEEtrantmpcountC by 1\relax + \@IEEEtrantmpdimenC\@IEEEtrantmpdimenA + \else + \if\@IEEEextractedtoken d\relax + \else + \@IEEEclspkgerror{Unknown mode type `\@IEEEextractedtokenmacro' in \string\IEEEquantizelength\space (line \the\inputlineno).\MessageBreak + Defaulting to `d'}% + {Valid modes for \string\IEEEquantizelength\space are: d, c, and i.}\relax + \fi % if d + % no need to do anything for d, use output values already setup + \fi\fi % if i, c + \fi % if length is already quantized +\fi% if base unit is zero +% globally assign the results to macros we use here to escape the enclosing +% group without needing to call \global on any of the \@IEEEtrantmp variables. +% \@IEEEtrantmpcountC has the quantized int +% \IEEEquantizedlength has the quantized length +% \@IEEEtrantmpdimenC has the quantized diff +\xdef\@IEEEquantizedlengthintmacro{\the\@IEEEtrantmpcountC}\relax +\@IEEEtrantmpcountC\IEEEquantizedlength\relax +\xdef\@IEEEquantizedlengthmacro{\the\@IEEEtrantmpcountC}\relax +\@IEEEtrantmpcountC\@IEEEtrantmpdimenC\relax +\xdef\@IEEEquantizedlengthdiffmacro{\the\@IEEEtrantmpcountC}\relax +\endgroup +% locally assign the outputs here from the macros +\expandafter\IEEEquantizedlength\@IEEEquantizedlengthmacro sp\relax +\expandafter\IEEEquantizedlengthdiff\@IEEEquantizedlengthdiffmacro sp\relax +\edef\IEEEquantizedlengthint{\@IEEEquantizedlengthintmacro}\relax} + + + +\newdimen\IEEEquantizedtextheightdiff +\IEEEquantizedtextheightdiff 0sp\relax + +% usage: \IEEEquantizetextheight[base unit]{mode: d, c, i} +% Sets \textheight to be an integer multiple of the current \baselineskip +% (or the optionally specified base unit) plus the first (\topskip) line. +% \IEEEquantizedtextheightdiff is a length equal to the difference between +% the new quantized and original \textheight. +% \IEEEquantizedtextheightlpc is a macro containing the integer number of +% lines per column under the quantized \textheight. i.e., +% \textheight = \IEEEquantizedtextheightlpc * \baselineskip + \topskip +% The mode determines how \textheight is quantized: +% d = always decrease (always round down the number of lines per column) +% c = use the closest match +% i = always increase (always round up the number of lines per column) +% In anycase, if \textheight is already quantized, it will remain unchanged, +% and \IEEEquantizedtextheightdiff will be set to zero. +% Depends on: \IEEEquantizelength +\def\IEEEquantizetextheight{\@ifnextchar [{\@IEEEquantizetextheight}{\@IEEEquantizetextheight[\baselineskip]}} +\def\@IEEEquantizetextheight[#1]#2{\begingroup +% use our \IEEEquantizedtextheightdiff as a scratch pad +% we need to subtract off \topskip before quantization +\IEEEquantizedtextheightdiff\textheight +\advance\IEEEquantizedtextheightdiff by -\topskip\relax +\IEEEquantizelength{#2}{#1}{\IEEEquantizedtextheightdiff} +% add back \topskip line +\advance\IEEEquantizedlength by \topskip +\@IEEEtrantmpcountC\IEEEquantizedlengthint\relax +\advance\@IEEEtrantmpcountC by 1\relax +% globally assign the results to macros we use here to escape the enclosing +% group without needing to call \global on any of the \@IEEEtrantmp variables. +\xdef\@IEEEquantizedtextheightlpcmacro{\the\@IEEEtrantmpcountC}\relax +\@IEEEtrantmpcountC\IEEEquantizedlength\relax +\xdef\@IEEEquantizedtextheightmacro{\the\@IEEEtrantmpcountC}\relax +\@IEEEtrantmpcountC\IEEEquantizedlengthdiff\relax +\xdef\@IEEEquantizedtextheightdiffmacro{\the\@IEEEtrantmpcountC}\relax +\endgroup +% locally assign the outputs here from the macros +\textheight\@IEEEquantizedtextheightmacro sp\relax +\IEEEquantizedtextheightdiff\@IEEEquantizedtextheightdiffmacro sp\relax +\edef\IEEEquantizedtextheightlpc{\@IEEEquantizedtextheightlpcmacro}} + + + +% usage: \IEEEsettopmargin[sample text]{mode: t, b, c, a, q}{margin/offset} +% Sets \topmargin based on the specified vertical margin. +% Takes into consideration the base 1in offset, \headheight, \headsep, +% \topskip, and (by default) the the actual height (or, for the bottom, depth) +% of the \IEEEdefaultsampletext text. +% The available modes are: +% t = top margin +% b = bottom margin +% c = vertically centered, with the given offset +% a = adjust the vertical margins using the given offset +% q = adjust the margins using \IEEEquantizedtextheightdiff and the given offset +% For the offsets, positive values increase the top margin. +% \headheight, \headsep, \topskip and \textheight should be set properly for the +% given margins before calling this function. +\def\IEEEsettopmargin{\@ifnextchar [{\@IEEEsettopmargin}{\@IEEEsettopmargin[\IEEEdefaultsampletext]}} +\def\@IEEEsettopmargin[#1]#2#3{\@IEEEtrantmpdimenA #3\relax +\@IEEEextracttoken{#2}\relax +% check for mode errors +\ifx\@IEEEextractedtokenmacro\@empty + \@IEEEclspkgerror{Empty mode type in \string\IEEEsettopmargin\space (line \the\inputlineno).\MessageBreak + Defaulting to `t'}{Valid modes for \string\IEEEsettopmargin\space are: t, b, c, a and q.}\relax + \let\@IEEEextractedtoken=t\relax + \def\@IEEEextractedtokenmacro{t}\relax +\else + \ifx\@IEEEextractedtokensdiscarded\@empty\else + \typeout{** WARNING: \string\IEEEsettopmargin\space mode specifiers after the first in `\@IEEEextracttokenarg' ignored (line \the\inputlineno).}\relax + \fi +\fi +% handle each mode +\if\@IEEEextractedtoken a\relax + \advance\topmargin by \@IEEEtrantmpdimenA\relax +\else +\if\@IEEEextractedtoken q\relax + % we need to adjust by half the \IEEEquantizedtextheightdiff value + \@IEEEtrantmpdimenB\IEEEquantizedtextheightdiff\relax + \divide\@IEEEtrantmpdimenB by 2\relax + % a positive \IEEEquantizedtextheightdiff means we need to reduce \topmargin + % because \textheight has been lenghtened + \advance\topmargin by -\@IEEEtrantmpdimenB\relax + \advance\topmargin by \@IEEEtrantmpdimenA\relax +\else +\if\@IEEEextractedtoken c\relax + \topmargin\paperheight + \advance\topmargin by -\textheight + % \textheight includes \topskip, but we should not count topskip whitespace here, backout + \advance \topmargin by \topskip + \settoheight{\@IEEEtrantmpdimenB}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\topmargin by -\@IEEEtrantmpdimenB\relax + \settodepth{\@IEEEtrantmpdimenB}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\topmargin by -\@IEEEtrantmpdimenB\relax + \divide\topmargin by 2\relax + \advance\topmargin by \@IEEEtrantmpdimenA\relax +\else +\if\@IEEEextractedtoken b\relax + \topmargin\paperheight + \advance\topmargin by -\textheight + % \textheight includes \topskip, but we should not count topskip whitespace here, backout + \advance \topmargin by \topskip + \settodepth{\@IEEEtrantmpdimenB}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\topmargin by -\@IEEEtrantmpdimenB\relax + \advance\topmargin by -\@IEEEtrantmpdimenA\relax +\else + \if\@IEEEextractedtoken t\relax + \else + \@IEEEclspkgerror{Unknown mode type `\@IEEEextractedtokenmacro' in \string\IEEEsettopmargin\space (line \the\inputlineno).\MessageBreak + Defaulting to `t'}% + {Valid modes for \string\IEEEsettopmargin\space are: t, b, c, a and q.}\relax + \fi + \topmargin\@IEEEtrantmpdimenA\relax + \settoheight{\@IEEEtrantmpdimenB}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\topmargin by \@IEEEtrantmpdimenB\relax +\fi\fi % if t, b, c +% convert desired top margin into actual \topmargin +% this is not done for the q or a modes because they are only adjustments +\advance \topmargin by -\topskip +\advance \topmargin by -1in +\advance \topmargin by -\headheight +\advance \topmargin by -\headsep +\fi\fi % if q, a +} + + + +% usage: \IEEEsetheadermargin[header sample][text sample]{mode: t, b, c, a}{margin/offset} +% Differentially adjusts \topmargin and \headsep (such that their sum is unchanged) +% based on the specified header margin. +% Takes into consideration the base 1in offset, \headheight, \topskip, and (by default) +% the actual height (or depth) of the \IEEEdefaultheadersampletext and +% \IEEEdefaultsampletext text. +% The available modes are: +% t = top margin (top of the header text to the top of the page) +% b = bottom margin (bottom of the header text to the top of the main text) +% c = vertically centered between the main text and the top of the page, +% with the given offset +% a = adjust the vertical position using the given offset +% For the offsets, positive values move the header downward. +% \headheight, \headsep, \topskip and \topmargin should be set properly before +% calling this function. +\def\IEEEsetheadermargin{\@ifnextchar [{\@IEEEsetheadermargin}{\@IEEEsetheadermargin[\IEEEdefaultheadersampletext]}} +\def\@IEEEsetheadermargin[#1]{\@ifnextchar [{\@@IEEEsetheadermargin[#1]}{\@@IEEEsetheadermargin[#1][\IEEEdefaultsampletext]}} +\def\@@IEEEsetheadermargin[#1][#2]#3#4{\@IEEEtrantmpdimenA #4\relax +\@IEEEextracttoken{#3}\relax +% check for mode errors +\ifx\@IEEEextractedtokenmacro\@empty + \@IEEEclspkgerror{Empty mode type in \string\IEEEsetheadermargin\space (line \the\inputlineno).\MessageBreak + Defaulting to `t'}{Valid modes for \string\IEEEsetheadermargin\space are: t, b, c, and a.}\relax + \let\@IEEEextractedtoken=t\relax + \def\@IEEEextractedtokenmacro{t}\relax +\else + \ifx\@IEEEextractedtokensdiscarded\@empty\else + \typeout{** WARNING: \string\IEEEsetheadermargin\space mode specifiers after the first in `\@IEEEextracttokenarg' ignored (line \the\inputlineno).}\relax + \fi +\fi +% handle each mode +\if\@IEEEextractedtoken a\relax + % No need to do anything here and can pass through the adjustment + % value as is. The end adjustment of \topmargin and \headsep will + % do all that is needed +\else +\if\@IEEEextractedtoken c\relax + % get the bottom margin + \@IEEEtrantmpdimenB\headsep\relax + \settodepth{\@IEEEtrantmpdimenC}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenC + \advance\@IEEEtrantmpdimenB by \topskip + \settoheight{\@IEEEtrantmpdimenC}{\begingroup #2\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenC + % at this point \@IEEEtrantmpdimenB has the actual header bottom margin + % subtract from it the top header margin + \advance\@IEEEtrantmpdimenB -1in\relax % take into consideration the system 1in offset of the top margin + \advance\@IEEEtrantmpdimenB by -\topmargin + \advance\@IEEEtrantmpdimenB by -\headheight + \settoheight{\@IEEEtrantmpdimenC}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenB by \@IEEEtrantmpdimenC + % at this point \@IEEEtrantmpdimenB has the difference between the bottom and top margins + % we need to adjust by half this amount to center the header + \divide\@IEEEtrantmpdimenB by 2\relax + % and add to offset + \advance\@IEEEtrantmpdimenA by \@IEEEtrantmpdimenB +\else +\if\@IEEEextractedtoken b\relax + \@IEEEtrantmpdimenB\headsep\relax + \settodepth{\@IEEEtrantmpdimenC}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenC + \advance\@IEEEtrantmpdimenB by \topskip + \settoheight{\@IEEEtrantmpdimenC}{\begingroup #2\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenC + % at this point \@IEEEtrantmpdimenB has the actual header bottom margin + % get the difference between the actual and the desired + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenA + \@IEEEtrantmpdimenA\@IEEEtrantmpdimenB +\else + \if\@IEEEextractedtoken t\relax + \else + \@IEEEclspkgerror{Unknown mode type `\@IEEEextractedtokenmacro' in \string\IEEEsetheadermargin\space (line \the\inputlineno).\MessageBreak + Defaulting to `t'}% + {Valid modes for \string\IEEEsetheadermargin\space are: t, b, c and a.}\relax + \fi + \@IEEEtrantmpdimenB 1in\relax % take into consideration the system 1in offset of the top margin + \advance\@IEEEtrantmpdimenB by \topmargin + \advance\@IEEEtrantmpdimenB by \headheight + \settoheight{\@IEEEtrantmpdimenC}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenC + % at this point \@IEEEtrantmpdimenB has the actual header top margin + % get the difference between the desired and the actual + \advance\@IEEEtrantmpdimenA by -\@IEEEtrantmpdimenB +\fi\fi % if t, b, c +\fi % if a +% advance \topmargin by the needed amount and reduce \headsep by the same +% so as not to disturb the location of the main text +\advance\topmargin by \@IEEEtrantmpdimenA\relax +\advance\headsep by -\@IEEEtrantmpdimenA\relax +} + + + +% usage: \IEEEsetfootermargin[footer sample][text sample]{mode: t, b, c, a}{margin/offset} +% Adjusts \footskip based on the specified footer margin. +% Takes into consideration the base 1in offset, \paperheight, \headheight, +% \headsep, \textheight and (by default) the actual height (or depth) of the +% \IEEEdefaultfootersampletext and \IEEEdefaultsampletext text. +% The available modes are: +% t = top margin (top of the footer text to the bottom of the main text) +% b = bottom margin (bottom of the footer text to the bottom of page) +% c = vertically centered between the main text and the bottom of the page, +% with the given offset +% a = adjust the vertical position using the given offset +% For the offsets, positive values move the footer downward. +% \headheight, \headsep, \topskip, \topmargin, and \textheight should be set +% properly before calling this function. +\def\IEEEsetfootermargin{\@ifnextchar [{\@IEEEsetfootermargin}{\@IEEEsetfootermargin[\IEEEdefaultfootersampletext]}} +\def\@IEEEsetfootermargin[#1]{\@ifnextchar [{\@@IEEEsetfootermargin[#1]}{\@@IEEEsetfootermargin[#1][\IEEEdefaultsampletext]}} +\def\@@IEEEsetfootermargin[#1][#2]#3#4{\@IEEEtrantmpdimenA #4\relax +\@IEEEextracttoken{#3}\relax +% check for mode errors +\ifx\@IEEEextractedtokenmacro\@empty + \@IEEEclspkgerror{Empty mode type in \string\IEEEsetfootermargin\space (line \the\inputlineno).\MessageBreak + Defaulting to `t'}{Valid modes for \string\IEEEsetfootermargin\space are: t, b, c, and a.}\relax + \let\@IEEEextractedtoken=t\relax + \def\@IEEEextractedtokenmacro{t}\relax +\else + \ifx\@IEEEextractedtokensdiscarded\@empty\else + \typeout{** WARNING: \string\IEEEsetfootermargin\space mode specifiers after the first in `\@IEEEextracttokenarg' ignored (line \the\inputlineno).}\relax + \fi +\fi +% handle each mode +\if\@IEEEextractedtoken a\relax + % No need to do anything here and can pass through the adjustment + % value as is. The end adjustment of \footskip will do all that + % is needed +\else +\if\@IEEEextractedtoken c\relax + % calculate the bottom margin + \@IEEEtrantmpdimenB 1in\relax % system 1in offset + \advance\@IEEEtrantmpdimenB\topmargin\relax + \advance\@IEEEtrantmpdimenB\headheight\relax + \advance\@IEEEtrantmpdimenB\headsep\relax + \advance\@IEEEtrantmpdimenB\textheight\relax + \advance\@IEEEtrantmpdimenB\footskip\relax + \settodepth{\@IEEEtrantmpdimenC}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenC by \@IEEEtrantmpdimenB + \@IEEEtrantmpdimenB\paperheight + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenC + % at this point \@IEEEtrantmpdimenB has the actual footer bottom margin + % now subtract off the footer top margin + \advance\@IEEEtrantmpdimenB -\footskip\relax + \settodepth{\@IEEEtrantmpdimenC}{\begingroup #2\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenB by \@IEEEtrantmpdimenC + \settoheight{\@IEEEtrantmpdimenC}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenB by \@IEEEtrantmpdimenC + % at this point \@IEEEtrantmpdimenB has the difference between the bottom + % and top footer margins + % our adjustment must be half this value to center the footer + \divide\@IEEEtrantmpdimenB by 2\relax + % add to the offset + \advance\@IEEEtrantmpdimenA by \@IEEEtrantmpdimenB +\else +\if\@IEEEextractedtoken b\relax + % calculate the bottom margin + \@IEEEtrantmpdimenB 1in\relax % system 1in offset + \advance\@IEEEtrantmpdimenB\topmargin\relax + \advance\@IEEEtrantmpdimenB\headheight\relax + \advance\@IEEEtrantmpdimenB\headsep\relax + \advance\@IEEEtrantmpdimenB\textheight\relax + \advance\@IEEEtrantmpdimenB\footskip\relax + \settodepth{\@IEEEtrantmpdimenC}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenC by \@IEEEtrantmpdimenB + \@IEEEtrantmpdimenB\paperheight + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenC + % at this point \@IEEEtrantmpdimenB has the actual footer bottom margin + % get the difference between the actual and the desired + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenA + \@IEEEtrantmpdimenA\@IEEEtrantmpdimenB +\else + \if\@IEEEextractedtoken t\relax + \else + \@IEEEclspkgerror{Unknown mode type `\@IEEEextractedtokenmacro' in \string\IEEEsetfootermargin\space (line \the\inputlineno).\MessageBreak + Defaulting to `t'}% + {Valid modes for \string\IEEEsetfootermargin\space are: t, b, c and a.}\relax + \fi + \@IEEEtrantmpdimenB\footskip\relax + \settodepth{\@IEEEtrantmpdimenC}{\begingroup #2\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenC + \settoheight{\@IEEEtrantmpdimenC}{\begingroup #1\relax\relax\relax\endgroup}\relax + \advance\@IEEEtrantmpdimenB by -\@IEEEtrantmpdimenC + % at this point \@IEEEtrantmpdimenB has the actual footer top margin + % get the difference between the desired and the actual + \advance\@IEEEtrantmpdimenA by -\@IEEEtrantmpdimenB +\fi\fi % if t, b, c +\fi % if a +% advance \footskip by the needed amount +\advance\footskip by \@IEEEtrantmpdimenA\relax +} + +% -- End V1.8a page setup commands -- + + + + + +% V1.6 +% LaTeX is a little to quick to use hyphenations +% So, we increase the penalty for their use and raise +% the badness level that triggers an underfull hbox +% warning. The author may still have to tweak things, +% but the appearance will be much better "right out +% of the box" than that under V1.5 and prior. +% TeX default is 50 +\hyphenpenalty=750 +\ifCLASSOPTIONcompsoc +\hyphenpenalty 500 +\fi +% If we didn't adjust the interword spacing, 2200 might be better. +% The TeX default is 1000 +\hbadness=1350 +% The IEEE does not use extra spacing after punctuation +\frenchspacing + +% V1.7 increase this a tad to discourage equation breaks +\binoppenalty=1000 % default 700 +\relpenalty=800 % default 500 + +% v1.8a increase these to discourage widows and orphans +\clubpenalty=1000 % default 150 +\widowpenalty=1000 % default 150 +\displaywidowpenalty=1000 % default 50 + + +% margin note stuff +\marginparsep 10pt +\marginparwidth 20pt +\marginparpush 25pt + + +% if things get too close, go ahead and let them touch +\lineskip 0pt +\normallineskip 0pt +\lineskiplimit 0pt +\normallineskiplimit 0pt + +% The distance from the lower edge of the text body to the +% footline +\footskip 0.4in + +% normally zero, should be relative to font height. +% put in a little rubber to help stop some bad breaks (underfull vboxes) +\parskip 0ex plus 0.2ex minus 0.1ex + +\parindent 1.0em +\ifCLASSOPTIONcompsoc + \parindent 1.5em +\fi + +\headheight 12pt +\headsep 18pt +% use the normal font baselineskip +% so that \topskip is unaffected by changes in \baselinestretch +\topskip=\@IEEEnormalsizeunitybaselineskip + + +% V1.8 \maxdepth defaults to 4pt, but should be font size dependent +\maxdepth=0.5\@IEEEnormalsizeunitybaselineskip +\textheight 58pc % 9.63in, 696pt + +% set the default top margin to 58pt +% which results in a \topmargin of -49.59pt for 10pt documents +\IEEEsettopmargin{t}{58pt} +% tweak textheight to a perfect integer number of lines/column. +% standard is: 9pt/63 lpc; 10pt/58 lpc; 11pt/52 lpc; 12pt/50 lpc +\IEEEquantizetextheight{c} +% tweak top margin so that the error is shared equally at the top and bottom +\IEEEsettopmargin{q}{0sp} + + +\columnsep 1pc +\textwidth 43pc % 2 x 21pc + 1pc = 43pc + +% set the default side margins to center the text +\IEEEsetsidemargin{c}{0pt} + + +% adjust margins for default conference mode +\ifCLASSOPTIONconference + \textheight 9.25in % The standard for conferences (668.4975pt) + \IEEEsettopmargin{t}{0.75in} + % tweak textheight to a perfect integer number of lines/page. + % standard is: 9pt/61 lpc; 10pt/56 lpc; 11pt/50 lpc; 12pt/48 lpc + \IEEEquantizetextheight{c} + % tweak top margin so that the error is shared equally at the top and bottom + \IEEEsettopmargin{q}{0sp} +\fi + + +% compsoc text sizes, margins and spacings +\ifCLASSOPTIONcompsoc + \columnsep 12bp + % CS specs for \textwdith are 6.875in + % \textwidth 6.875in + % however, measurements from proofs show they are using 3.5in columns + \textwidth 7in + \advance\textwidth by \columnsep + % set the side margins to center the text + \IEEEsetsidemargin{c}{0pt} + % top/bottom margins to center + % could just set \textheight to 9.75in for all the different paper sizes + % and then quantize, but we'll do it the long way here to allow for easy + % future per-paper size adjustments + \IEEEsettextheight{0.625in}{0.625in}% 11in - 2 * 0.625in = 9.75in is the standard text height for compsoc journals + \IEEEsettopmargin{t}{0.625in} + \if@IEEEusingcspaper + \IEEEsettextheight{0.5in}{0.5in}% 10.75in - 2 * 0.5in = 9.75in + \IEEEsettopmargin{t}{0.5in} + \fi + \if@IEEEusingAfourpaper + \IEEEsettextheight{24.675mm}{24.675mm}% 297mm - 2 * 24.675mm = 247.650mm (9.75in) + \IEEEsettopmargin{t}{24.675mm} + \fi + % tweak textheight to a perfect integer number of lines/page. + % standard is: 9pt/65 lpc; 10pt/61 lpc; 11pt/53 lpc; 12pt/49 lpc + \IEEEquantizetextheight{c} + % tweak top margin so that the error is shared equally at the top and bottom + \IEEEsettopmargin{q}{0sp} + +% compsoc conference + \ifCLASSOPTIONconference + % compsoc conference use a larger value for columnsep + \columnsep 0.25in + \IEEEsettextwidth{0.75in}{0.75in} + % set the side margins to center the text (0.75in for letterpaper) + \IEEEsetsidemargin{c}{0pt} + % compsoc conferences want 1in top and bottom margin + \IEEEsettextheight{1in}{1in} + \IEEEsettopmargin{t}{1in} + % tweak textheight to a perfect integer number of lines/page. + % standard is: 9pt/58 lpc; 10pt/53 lpc; 11pt/48 lpc; 12pt/46 lpc + \IEEEquantizetextheight{c} + % tweak top margin so that the error is shared equally at the top and bottom + \IEEEsettopmargin{q}{0sp} + \fi +\fi + + + +% draft mode settings override that of all other modes +% provides a nice 1in margin all around the paper and extra +% space between the lines for editor's comments +\ifCLASSOPTIONdraftcls + % we want 1in side margins regardless of paper type + \IEEEsettextwidth{1in}{1in} + \IEEEsetsidemargin{c}{0pt} + % want 1in top and bottom margins + \IEEEsettextheight{1in}{1in} + \IEEEsettopmargin{t}{1in} + % digitize textheight to be an integer number of lines. + % this may cause the top and bottom margins to be off a tad + \IEEEquantizetextheight{c} + % tweak top margin so that the error is shared equally at the top and bottom + \IEEEsettopmargin{q}{0sp} +\fi + + + +% process CLASSINPUT inner/outer margin +% if inner margin defined, but outer margin not, set outer to inner. +\ifx\CLASSINPUTinnersidemargin\@IEEEundefined +\else + \ifx\CLASSINPUToutersidemargin\@IEEEundefined + \edef\CLASSINPUToutersidemargin{\CLASSINPUTinnersidemargin} + \fi +\fi + +\ifx\CLASSINPUToutersidemargin\@IEEEundefined +\else + % if outer margin defined, but inner margin not, set inner to outer. + \ifx\CLASSINPUTinnersidemargin\@IEEEundefined + \edef\CLASSINPUTinnersidemargin{\CLASSINPUToutersidemargin} + \fi + \IEEEsettextwidth{\CLASSINPUTinnersidemargin}{\CLASSINPUToutersidemargin} + \IEEEsetsidemargin{i}{\CLASSINPUTinnersidemargin} + \typeout{** ATTENTION: Overriding inner side margin to \CLASSINPUTinnersidemargin\space and + outer side margin to \CLASSINPUToutersidemargin\space via \string\CLASSINPUT.} +\fi + + + +% process CLASSINPUT top/bottom text margin +% if toptext margin defined, but bottomtext margin not, set bottomtext to toptext margin +\ifx\CLASSINPUTtoptextmargin\@IEEEundefined +\else + \ifx\CLASSINPUTbottomtextmargin\@IEEEundefined + \edef\CLASSINPUTbottomtextmargin{\CLASSINPUTtoptextmargin} + \fi +\fi + +\ifx\CLASSINPUTbottomtextmargin\@IEEEundefined +\else + % if bottomtext margin defined, but toptext margin not, set toptext to bottomtext margin + \ifx\CLASSINPUTtoptextmargin\@IEEEundefined + \edef\CLASSINPUTtoptextmargin{\CLASSINPUTbottomtextmargin} + \fi + \IEEEsettextheight{\CLASSINPUTtoptextmargin}{\CLASSINPUTbottomtextmargin} + \IEEEsettopmargin{t}{\CLASSINPUTtoptextmargin} + \typeout{** ATTENTION: Overriding top text margin to \CLASSINPUTtoptextmargin\space and + bottom text margin to \CLASSINPUTbottomtextmargin\space via \string\CLASSINPUT.} +\fi + + + +% default to center header and footer text in the margins +\IEEEsetheadermargin{c}{0pt} +\IEEEsetfootermargin{c}{0pt} + +% adjust header and footer positions for compsoc journals +\ifCLASSOPTIONcompsoc + \ifCLASSOPTIONjournal + \IEEEsetheadermargin{b}{\@IEEEnormalsizeunitybaselineskip} + \IEEEsetfootermargin{t}{\@IEEEnormalsizeunitybaselineskip} + \fi +\fi + + +% V1.8a display lines per column info message on user's console +\def\IEEEdisplayinfolinespercolumn{\@IEEEtrantmpdimenA=\textheight +% topskip represents only one line even if > baselineskip +\advance\@IEEEtrantmpdimenA by -1\topskip +\@IEEEtrantmpcountA=\@IEEEtrantmpdimenA +\@IEEEtrantmpcountB=\@IEEEtrantmpdimenA +\divide\@IEEEtrantmpcountB by \baselineskip +% need to add one line to include topskip (first) line +\advance\@IEEEtrantmpcountB by 1 +% save lines per column value as text +\edef\@IEEEnumlinespercolumninfotxt{\the\@IEEEtrantmpcountB} +% backout topskip advance to allow direct \@IEEEtrantmpcountA comparison +\advance\@IEEEtrantmpcountB by -1 +% restore value as text height (without topskip) rather than just as number of lines +\multiply\@IEEEtrantmpcountB by \baselineskip +% is the column height an integer number of lines per column? +\ifnum\@IEEEtrantmpcountA=\@IEEEtrantmpcountB +\edef\@IEEEnumlinespercolumnexactinfotxt{exact} +\else +\@IEEEtrantmpdimenA\@IEEEtrantmpcountA sp\relax +\advance\@IEEEtrantmpdimenA by -\@IEEEtrantmpcountB sp\relax +\edef\@IEEEnumlinespercolumnexactinfotxt{approximate, difference = \the\@IEEEtrantmpdimenA} +\fi +\typeout{-- Lines per column: \@IEEEnumlinespercolumninfotxt\space (\@IEEEnumlinespercolumnexactinfotxt).}} +% delay execution till start of document to allow for user changes +\AtBeginDocument{\IEEEdisplayinfolinespercolumn} + + + +% LIST SPACING CONTROLS + +% Controls the amount of EXTRA spacing +% above and below \trivlist +% Both \list and IED lists override this. +% However, \trivlist will use this as will most +% things built from \trivlist like the \center +% environment. +\topsep 0.5\baselineskip + +% Controls the additional spacing around lists preceded +% or followed by blank lines. the IEEE does not increase +% spacing before or after paragraphs so it is set to zero. +% \z@ is the same as zero, but faster. +\partopsep \z@ + +% Controls the spacing between paragraphs in lists. +% The IEEE does not increase spacing before or after paragraphs +% so this is also zero. +% With IEEEtran.cls, global changes to +% this value DO affect lists (but not IED lists). +\parsep \z@ + +% Controls the extra spacing between list items. +% The IEEE does not put extra spacing between items. +% With IEEEtran.cls, global changes to this value DO affect +% lists (but not IED lists). +\itemsep \z@ + +% \itemindent is the amount to indent the FIRST line of a list +% item. It is auto set to zero within the \list environment. To alter +% it, you have to do so when you call the \list. +% However, the IEEE uses this for the theorem environment +% There is an alternative value for this near \leftmargini below +\itemindent -1em + +% \leftmargin, the spacing from the left margin of the main text to +% the left of the main body of a list item is set by \list. +% Hence this statement does nothing for lists. +% But, quote and verse do use it for indention. +\leftmargin 2em + +% we retain this stuff from the older IEEEtran.cls so that \list +% will work the same way as before. However, itemize, enumerate and +% description (IED) could care less about what these are as they +% all are overridden. +\leftmargini 2em +%\itemindent 2em % Alternative values: sometimes used. +%\leftmargini 0em +\leftmarginii 1em +\leftmarginiii 1.5em +\leftmarginiv 1.5em +\leftmarginv 1.0em +\leftmarginvi 1.0em +\labelsep 0.5em +\labelwidth \z@ + + +% The old IEEEtran.cls behavior of \list is retained. +% However, the new V1.3 IED list environments override all the +% @list stuff (\@listX is called within \list for the +% appropriate level just before the user's list_decl is called). +% \topsep is now 2pt as the IEEE puts a little extra space around +% lists - used by those non-IED macros that depend on \list. +% Note that \parsep and \itemsep are not redefined as in +% the sizexx.clo \@listX (which article.cls uses) so global changes +% of these values DO affect \list +% +\def\@listi{\leftmargin\leftmargini \topsep 2pt plus 1pt minus 1pt} +\let\@listI\@listi +\def\@listii{\leftmargin\leftmarginii\labelwidth\leftmarginii% + \advance\labelwidth-\labelsep \topsep 2pt} +\def\@listiii{\leftmargin\leftmarginiii\labelwidth\leftmarginiii% + \advance\labelwidth-\labelsep \topsep 2pt} +\def\@listiv{\leftmargin\leftmarginiv\labelwidth\leftmarginiv% + \advance\labelwidth-\labelsep \topsep 2pt} +\def\@listv{\leftmargin\leftmarginv\labelwidth\leftmarginv% + \advance\labelwidth-\labelsep \topsep 2pt} +\def\@listvi{\leftmargin\leftmarginvi\labelwidth\leftmarginvi% + \advance\labelwidth-\labelsep \topsep 2pt} + + +% The IEEE uses 5) not 5. +\def\labelenumi{\theenumi)} \def\theenumi{\arabic{enumi}} + +% The IEEE uses a) not (a) +\def\labelenumii{\theenumii)} \def\theenumii{\alph{enumii}} + +% The IEEE uses iii) not iii. +\def\labelenumiii{\theenumiii)} \def\theenumiii{\roman{enumiii}} + +% The IEEE uses A) not A. +\def\labelenumiv{\theenumiv)} \def\theenumiv{\Alph{enumiv}} + +% exactly the same as in article.cls +\def\p@enumii{\theenumi} +\def\p@enumiii{\theenumi(\theenumii)} +\def\p@enumiv{\p@enumiii\theenumiii} + +% itemized list label styles +\def\labelitemi{$\scriptstyle\bullet$} +\def\labelitemii{\textbf{--}} +\def\labelitemiii{$\ast$} +\def\labelitemiv{$\cdot$} + + + +% **** V1.3 ENHANCEMENTS **** +% Itemize, Enumerate and Description (IED) List Controls +% *************************** +% +% +% The IEEE seems to use at least two different values by +% which ITEMIZED list labels are indented to the right +% For The Journal of Lightwave Technology (JLT) and The Journal +% on Selected Areas in Communications (JSAC), they tend to use +% an indention equal to \parindent. For Transactions on Communications +% they tend to indent ITEMIZED lists a little more--- 1.3\parindent. +% We'll provide both values here for you so that you can choose +% which one you like in your document using a command such as: +% setlength{\IEEEilabelindent}{\IEEEilabelindentB} +\newdimen\IEEEilabelindentA +\IEEEilabelindentA \parindent + +\newdimen\IEEEilabelindentB +\IEEEilabelindentB 1.3\parindent +% However, we'll default to using \parindent +% which makes more sense to me +\newdimen\IEEEilabelindent +\IEEEilabelindent \IEEEilabelindentA + + +% This controls the default amount the enumerated list labels +% are indented to the right. +% Normally, this is the same as the paragraph indention +\newdimen\IEEEelabelindent +\IEEEelabelindent \parindent + +% This controls the default amount the description list labels +% are indented to the right. +% Normally, this is the same as the paragraph indention +\newdimen\IEEEdlabelindent +\IEEEdlabelindent \parindent + +% This is the value actually used within the IED lists. +% The IED environments automatically set its value to +% one of the three values above, so global changes do +% not have any effect +\newdimen\IEEElabelindent +\IEEElabelindent \parindent + +% The actual amount labels will be indented is +% \IEEElabelindent multiplied by the factor below +% corresponding to the level of nesting depth +% This provides a means by which the user can +% alter the effective \IEEElabelindent for deeper +% levels +% There may not be such a thing as correct "standard IEEE" +% values. What the IEEE actually does may depend on the specific +% circumstances. +% The first list level almost always has full indention. +% The second levels I've seen have only 75% of the normal indentation +% Three level or greater nestings are very rare. I am guessing +% that they don't use any indentation. +\def\IEEElabelindentfactori{1.0} % almost always one +\def\IEEElabelindentfactorii{0.75} % 0.0 or 1.0 may be used in some cases +\def\IEEElabelindentfactoriii{0.0} % 0.75? 0.5? 0.0? +\def\IEEElabelindentfactoriv{0.0} +\def\IEEElabelindentfactorv{0.0} +\def\IEEElabelindentfactorvi{0.0} + +% value actually used within IED lists, it is auto +% set to one of the 6 values above +% global changes here have no effect +\def\IEEElabelindentfactor{1.0} + +% This controls the default spacing between the end of the IED +% list labels and the list text, when normal text is used for +% the labels. +% compsoc uses a larger value here, but we'll set that later +% in the class so that this code block area can be extracted +% as-is for IEEEtrantools.sty +\newdimen\IEEEiednormlabelsep +\IEEEiednormlabelsep 0.6em + +% This controls the default spacing between the end of the IED +% list labels and the list text, when math symbols are used for +% the labels (nomenclature lists). The IEEE usually increases the +% spacing in these cases +\newdimen\IEEEiedmathlabelsep +\IEEEiedmathlabelsep 1.2em + +% This controls the extra vertical separation put above and +% below each IED list. the IEEE usually puts a little extra spacing +% around each list. However, this spacing is barely noticeable. +% compsoc uses a larger value here, but we'll set that later +% in the class so that this code block area can be extracted +% as-is for IEEEtrantools.sty +\newskip\IEEEiedtopsep +\IEEEiedtopsep 2pt plus 1pt minus 1pt + + +% This command is executed within each IED list environment +% at the beginning of the list. You can use this to set the +% parameters for some/all your IED list(s) without disturbing +% global parameters that affect things other than lists. +% i.e., renewcommand{\IEEEiedlistdecl}{\setlength{\labelsep}{5em}} +% will alter the \labelsep for the next list(s) until +% \IEEEiedlistdecl is redefined. +\def\IEEEiedlistdecl{\relax} + +% This command provides an easy way to set \leftmargin based +% on the \labelwidth, \labelsep and the argument \IEEElabelindent +% Usage: \IEEEcalcleftmargin{width-to-indent-the-label} +% output is in the \leftmargin variable, i.e., effectively: +% \leftmargin = argument + \labelwidth + \labelsep +% Note controlled spacing here, shield end of lines with % +\def\IEEEcalcleftmargin#1{\setlength{\leftmargin}{#1}% +\addtolength{\leftmargin}{\labelwidth}% +\addtolength{\leftmargin}{\labelsep}} + +% This command provides an easy way to set \labelwidth to the +% width of the given text. It is the same as +% \settowidth{\labelwidth}{label-text} +% and useful as a shorter alternative. +% Typically used to set \labelwidth to be the width +% of the longest label in the list +\def\IEEEsetlabelwidth#1{\settowidth{\labelwidth}{#1}} + +% When this command is executed, IED lists will use the +% IEEEiedmathlabelsep label separation rather than the normal +% spacing. To have an effect, this command must be executed via +% the \IEEEiedlistdecl or within the option of the IED list +% environments. +\def\IEEEusemathlabelsep{\setlength{\labelsep}{\IEEEiedmathlabelsep}} + +% A flag which controls whether the IED lists automatically +% calculate \leftmargin from \IEEElabelindent, \labelwidth and \labelsep +% Useful if you want to specify your own \leftmargin +% This flag must be set (\IEEEnocalcleftmargintrue or \IEEEnocalcleftmarginfalse) +% via the \IEEEiedlistdecl or within the option of the IED list +% environments to have an effect. +\newif\ifIEEEnocalcleftmargin +\IEEEnocalcleftmarginfalse + +% A flag which controls whether \IEEElabelindent is multiplied by +% the \IEEElabelindentfactor for each list level. +% This flag must be set via the \IEEEiedlistdecl or within the option +% of the IED list environments to have an effect. +\newif\ifIEEEnolabelindentfactor +\IEEEnolabelindentfactorfalse + + +% internal variable to indicate type of IED label +% justification +% 0 - left; 1 - center; 2 - right +\def\@IEEEiedjustify{0} + + +% commands to allow the user to control IED +% label justifications. Use these commands within +% the IED environment option or in the \IEEEiedlistdecl +% Note that changing the normal list justifications +% is nonstandard and the IEEE may not like it if you do so! +% I include these commands as they may be helpful to +% those who are using these enhanced list controls for +% other non-IEEE related LaTeX work. +% itemize and enumerate automatically default to right +% justification, description defaults to left. +\def\IEEEiedlabeljustifyl{\def\@IEEEiedjustify{0}}%left +\def\IEEEiedlabeljustifyc{\def\@IEEEiedjustify{1}}%center +\def\IEEEiedlabeljustifyr{\def\@IEEEiedjustify{2}}%right + + + + +% commands to save to and restore from the list parameter copies +% this allows us to set all the list parameters within +% the list_decl and prevent \list (and its \@list) +% from overriding any of our parameters +% V1.6 use \edefs instead of dimen's to conserve dimen registers +% Note controlled spacing here, shield end of lines with % +\def\@IEEEsavelistparams{\edef\@IEEEiedtopsep{\the\topsep}% +\edef\@IEEEiedlabelwidth{\the\labelwidth}% +\edef\@IEEEiedlabelsep{\the\labelsep}% +\edef\@IEEEiedleftmargin{\the\leftmargin}% +\edef\@IEEEiedpartopsep{\the\partopsep}% +\edef\@IEEEiedparsep{\the\parsep}% +\edef\@IEEEieditemsep{\the\itemsep}% +\edef\@IEEEiedrightmargin{\the\rightmargin}% +\edef\@IEEEiedlistparindent{\the\listparindent}% +\edef\@IEEEieditemindent{\the\itemindent}} + +% Note controlled spacing here +\def\@IEEErestorelistparams{\topsep\@IEEEiedtopsep\relax% +\labelwidth\@IEEEiedlabelwidth\relax% +\labelsep\@IEEEiedlabelsep\relax% +\leftmargin\@IEEEiedleftmargin\relax% +\partopsep\@IEEEiedpartopsep\relax% +\parsep\@IEEEiedparsep\relax% +\itemsep\@IEEEieditemsep\relax% +\rightmargin\@IEEEiedrightmargin\relax% +\listparindent\@IEEEiedlistparindent\relax% +\itemindent\@IEEEieditemindent\relax} + + +% v1.6b provide original LaTeX IED list environments +% note that latex.ltx defines \itemize and \enumerate, but not \description +% which must be created by the base classes +% save original LaTeX itemize and enumerate +\let\LaTeXitemize\itemize +\let\endLaTeXitemize\enditemize +\let\LaTeXenumerate\enumerate +\let\endLaTeXenumerate\endenumerate + +% provide original LaTeX description environment from article.cls +\newenvironment{LaTeXdescription} + {\list{}{\labelwidth\z@ \itemindent-\leftmargin + \let\makelabel\descriptionlabel}} + {\endlist} +\newcommand*\descriptionlabel[1]{\hspace\labelsep + \normalfont\bfseries #1} + + +% override LaTeX's default IED lists +\def\itemize{\@IEEEitemize} +\def\enditemize{\@endIEEEitemize} +\def\enumerate{\@IEEEenumerate} +\def\endenumerate{\@endIEEEenumerate} +\def\description{\@IEEEdescription} +\def\enddescription{\@endIEEEdescription} + +% provide the user with aliases - may help those using packages that +% override itemize, enumerate, or description +\def\IEEEitemize{\@IEEEitemize} +\def\endIEEEitemize{\@endIEEEitemize} +\def\IEEEenumerate{\@IEEEenumerate} +\def\endIEEEenumerate{\@endIEEEenumerate} +\def\IEEEdescription{\@IEEEdescription} +\def\endIEEEdescription{\@endIEEEdescription} + + +% V1.6 we want to keep the IEEEtran IED list definitions as our own internal +% commands so they are protected against redefinition +\def\@IEEEitemize{\@ifnextchar[{\@@IEEEitemize}{\@@IEEEitemize[\relax]}} +\def\@IEEEenumerate{\@ifnextchar[{\@@IEEEenumerate}{\@@IEEEenumerate[\relax]}} +\def\@IEEEdescription{\@ifnextchar[{\@@IEEEdescription}{\@@IEEEdescription[\relax]}} +\def\@endIEEEitemize{\endlist} +\def\@endIEEEenumerate{\endlist} +\def\@endIEEEdescription{\endlist} + + +% DO NOT ALLOW BLANK LINES TO BE IN THESE IED ENVIRONMENTS +% AS THIS WILL FORCE NEW PARAGRAPHS AFTER THE IED LISTS +% IEEEtran itemized list MDS 1/2001 +% Note controlled spacing here, shield end of lines with % +\def\@@IEEEitemize[#1]{% + \ifnum\@itemdepth>3\relax\@toodeep\else% + \ifnum\@listdepth>5\relax\@toodeep\else% + \advance\@itemdepth\@ne% + \edef\@itemitem{labelitem\romannumeral\the\@itemdepth}% + % get the IEEElabelindentfactor for this level + \advance\@listdepth\@ne% we need to know what the level WILL be + \edef\IEEElabelindentfactor{\csname IEEElabelindentfactor\romannumeral\the\@listdepth\endcsname}% + \advance\@listdepth-\@ne% undo our increment + \def\@IEEEiedjustify{2}% right justified labels are default + % set other defaults + \IEEEnocalcleftmarginfalse% + \IEEEnolabelindentfactorfalse% + \topsep\IEEEiedtopsep% + \IEEElabelindent\IEEEilabelindent% + \labelsep\IEEEiednormlabelsep% + \partopsep 0ex% + \parsep 0ex% + \itemsep 0ex% + \rightmargin 0em% + \listparindent 0em% + \itemindent 0em% + % calculate the label width + % the user can override this later if + % they specified a \labelwidth + \settowidth{\labelwidth}{\csname labelitem\romannumeral\the\@itemdepth\endcsname}% + \@IEEEsavelistparams% save our list parameters + \list{\csname\@itemitem\endcsname}{% + \@IEEErestorelistparams% override any list{} changes + % to our globals + \let\makelabel\@IEEEiedmakelabel% v1.6b setup \makelabel + \IEEEiedlistdecl% let user alter parameters + #1\relax% + % If the user has requested not to use the + % IEEElabelindent factor, don't revise \IEEElabelindent + \ifIEEEnolabelindentfactor\relax% + \else\IEEElabelindent=\IEEElabelindentfactor\IEEElabelindent% + \fi% + % Unless the user has requested otherwise, + % calculate our left margin based + % on \IEEElabelindent, \labelwidth and + % \labelsep + \ifIEEEnocalcleftmargin\relax% + \else\IEEEcalcleftmargin{\IEEElabelindent}% + \fi}\fi\fi}% + + +% DO NOT ALLOW BLANK LINES TO BE IN THESE IED ENVIRONMENTS +% AS THIS WILL FORCE NEW PARAGRAPHS AFTER THE IED LISTS +% IEEEtran enumerate list MDS 1/2001 +% Note controlled spacing here, shield end of lines with % +\def\@@IEEEenumerate[#1]{% + \ifnum\@enumdepth>3\relax\@toodeep\else% + \ifnum\@listdepth>5\relax\@toodeep\else% + \advance\@enumdepth\@ne% + \edef\@enumctr{enum\romannumeral\the\@enumdepth}% + % get the IEEElabelindentfactor for this level + \advance\@listdepth\@ne% we need to know what the level WILL be + \edef\IEEElabelindentfactor{\csname IEEElabelindentfactor\romannumeral\the\@listdepth\endcsname}% + \advance\@listdepth-\@ne% undo our increment + \def\@IEEEiedjustify{2}% right justified labels are default + % set other defaults + \IEEEnocalcleftmarginfalse% + \IEEEnolabelindentfactorfalse% + \topsep\IEEEiedtopsep% + \IEEElabelindent\IEEEelabelindent% + \labelsep\IEEEiednormlabelsep% + \partopsep 0ex% + \parsep 0ex% + \itemsep 0ex% + \rightmargin 0em% + \listparindent 0em% + \itemindent 0em% + % calculate the label width + % We'll set it to the width suitable for all labels using + % normalfont 1) to 9) + % The user can override this later + \settowidth{\labelwidth}{9)}% + \@IEEEsavelistparams% save our list parameters + \list{\csname label\@enumctr\endcsname}{\usecounter{\@enumctr}% + \@IEEErestorelistparams% override any list{} changes + % to our globals + \let\makelabel\@IEEEiedmakelabel% v1.6b setup \makelabel + \IEEEiedlistdecl% let user alter parameters + #1\relax% + % If the user has requested not to use the + % IEEElabelindent factor, don't revise \IEEElabelindent + \ifIEEEnolabelindentfactor\relax% + \else\IEEElabelindent=\IEEElabelindentfactor\IEEElabelindent% + \fi% + % Unless the user has requested otherwise, + % calculate our left margin based + % on \IEEElabelindent, \labelwidth and + % \labelsep + \ifIEEEnocalcleftmargin\relax% + \else\IEEEcalcleftmargin{\IEEElabelindent}% + \fi}\fi\fi}% + + +% DO NOT ALLOW BLANK LINES TO BE IN THESE IED ENVIRONMENTS +% AS THIS WILL FORCE NEW PARAGRAPHS AFTER THE IED LISTS +% IEEEtran description list MDS 1/2001 +% Note controlled spacing here, shield end of lines with % +\def\@@IEEEdescription[#1]{% + \ifnum\@listdepth>5\relax\@toodeep\else% + % get the IEEElabelindentfactor for this level + \advance\@listdepth\@ne% we need to know what the level WILL be + \edef\IEEElabelindentfactor{\csname IEEElabelindentfactor\romannumeral\the\@listdepth\endcsname}% + \advance\@listdepth-\@ne% undo our increment + \def\@IEEEiedjustify{0}% left justified labels are default + % set other defaults + \IEEEnocalcleftmarginfalse% + \IEEEnolabelindentfactorfalse% + \topsep\IEEEiedtopsep% + \IEEElabelindent\IEEEdlabelindent% + % assume normal labelsep + \labelsep\IEEEiednormlabelsep% + \partopsep 0ex% + \parsep 0ex% + \itemsep 0ex% + \rightmargin 0em% + \listparindent 0em% + \itemindent 0em% + % Bogus label width in case the user forgets + % to set it. + % TIP: If you want to see what a variable's width is you + % can use the TeX command \showthe\width-variable to + % display it on the screen during compilation + % (This might be helpful to know when you need to find out + % which label is the widest) + \settowidth{\labelwidth}{Hello}% + \@IEEEsavelistparams% save our list parameters + \list{}{\@IEEErestorelistparams% override any list{} changes + % to our globals + \let\makelabel\@IEEEiedmakelabel% v1.6b setup \makelabel + \IEEEiedlistdecl% let user alter parameters + #1\relax% + % If the user has requested not to use the + % labelindent factor, don't revise \IEEElabelindent + \ifIEEEnolabelindentfactor\relax% + \else\IEEElabelindent=\IEEElabelindentfactor\IEEElabelindent% + \fi% + % Unless the user has requested otherwise, + % calculate our left margin based + % on \IEEElabelindent, \labelwidth and + % \labelsep + \ifIEEEnocalcleftmargin\relax% + \else\IEEEcalcleftmargin{\IEEElabelindent}\relax% + \fi}\fi} + +% v1.6b we use one makelabel that does justification as needed. +\def\@IEEEiedmakelabel#1{\relax\if\@IEEEiedjustify 0\relax +\makebox[\labelwidth][l]{\normalfont #1}\else +\if\@IEEEiedjustify 1\relax +\makebox[\labelwidth][c]{\normalfont #1}\else +\makebox[\labelwidth][r]{\normalfont #1}\fi\fi} + + +% compsoc uses a larger value for the normal labelsep +% and also extra spacing above and below each list +\ifCLASSOPTIONcompsoc + \IEEEiednormlabelsep 1.2em + \IEEEiedtopsep 6pt plus 3pt minus 3pt +\fi + + +% VERSE and QUOTE +% V1.7 define environments with newenvironment +\newenvironment{verse}{\let\\=\@centercr + \list{}{\itemsep\z@ \itemindent -1.5em \listparindent \itemindent + \rightmargin\leftmargin\advance\leftmargin 1.5em}\item\relax} + {\endlist} +\newenvironment{quotation}{\list{}{\listparindent 1.5em \itemindent\listparindent + \rightmargin\leftmargin \parsep 0pt plus 1pt}\item\relax} + {\endlist} +\newenvironment{quote}{\list{}{\rightmargin\leftmargin}\item\relax} + {\endlist} + + +% \titlepage +% provided only for backward compatibility. \maketitle is the correct +% way to create the title page. +\def\titlepage{\@restonecolfalse\if@twocolumn\@restonecoltrue\onecolumn + \else \newpage \fi \thispagestyle{empty}\c@page\z@} +\def\endtitlepage{\if@restonecol\twocolumn \else \newpage \fi} + +% standard values from article.cls +\arraycolsep 5pt +\arrayrulewidth .4pt +\doublerulesep 2pt + +\tabcolsep 6pt +\tabbingsep 0.5em + + +%% FOOTNOTES +% +%\skip\footins 10pt plus 4pt minus 2pt +% V1.6 respond to changes in font size +% space added above the footnotes (if present) +\skip\footins 0.9\baselineskip plus 0.4\baselineskip minus 0.2\baselineskip + +% V1.6, we need to make \footnotesep responsive to changes +% in \baselineskip or strange spacings will result when in +% draft mode. Here is a little LaTeX secret - \footnotesep +% determines the height of an invisible strut that is placed +% *above* the baseline of footnotes after the first. Since +% LaTeX considers the space for characters to be 0.7\baselineskip +% above the baseline and 0.3\baselineskip below it, we need to +% use 0.7\baselineskip as a \footnotesep to maintain equal spacing +% between all the lines of the footnotes. The IEEE often uses a tad +% more, so use 0.8\baselineskip. This slightly larger value also helps +% the text to clear the footnote marks. Note that \thanks in IEEEtran +% uses its own value of \footnotesep which is set in \maketitle. +{\footnotesize +\global\footnotesep 0.8\baselineskip} + + +\skip\@mpfootins = \skip\footins +\fboxsep = 3pt +\fboxrule = .4pt +% V1.6 use 1em, then use LaTeX2e's \@makefnmark +% Note that the IEEE normally *left* aligns the footnote marks, so we don't need +% box resizing tricks here. +\long\def\@makefntext#1{\parindent 1em\indent\hbox{\@makefnmark}#1}% V1.6 use 1em +% V1.7 compsoc does not use superscipts for footnote marks +\ifCLASSOPTIONcompsoc +\def\@IEEEcompsocmakefnmark{\hbox{\normalfont\@thefnmark.\ }} +\long\def\@makefntext#1{\parindent 1em\indent\hbox{\@IEEEcompsocmakefnmark}#1} +\fi + +% The IEEE does not use footnote rules +\def\footnoterule{} + +% V1.7 for compsoc, the IEEE uses a footnote rule only for \thanks. We devise a "one-shot" +% system to implement this. +\newif\if@IEEEenableoneshotfootnoterule +\@IEEEenableoneshotfootnoterulefalse +\ifCLASSOPTIONcompsoc +\def\footnoterule{\relax\if@IEEEenableoneshotfootnoterule +\kern-5pt +\hbox to \columnwidth{\hfill\vrule width 0.5\columnwidth height 0.4pt\hfill} +\kern4.6pt +\global\@IEEEenableoneshotfootnoterulefalse +\else +\relax +\fi} +\fi + +% V1.6 do not allow LaTeX to break a footnote across multiple pages +\interfootnotelinepenalty=10000 + +% V1.6 discourage breaks within equations +% Note that amsmath normally sets this to 10000, +% but LaTeX2e normally uses 100. +\interdisplaylinepenalty=2500 + +% default allows section depth up to /paragraph +\setcounter{secnumdepth}{4} + +% technotes do not allow /paragraph +\ifCLASSOPTIONtechnote + \setcounter{secnumdepth}{3} +\fi +% neither do compsoc conferences +\@IEEEcompsocconfonly{\setcounter{secnumdepth}{3}} + + +\newcounter{section} +\newcounter{subsection}[section] +\newcounter{subsubsection}[subsection] +\newcounter{paragraph}[subsubsection] + +% used only by IEEEtran's IEEEeqnarray as other packages may +% have their own, different, implementations +\newcounter{IEEEsubequation}[equation] + +% as shown when called by user from \ref, \label and in table of contents +\def\theequation{\arabic{equation}} % 1 +\def\theIEEEsubequation{\theequation\alph{IEEEsubequation}} % 1a (used only by IEEEtran's IEEEeqnarray) +\ifCLASSOPTIONcompsoc +% compsoc is all arabic +\def\thesection{\arabic{section}} +\def\thesubsection{\thesection.\arabic{subsection}} +\def\thesubsubsection{\thesubsection.\arabic{subsubsection}} +\def\theparagraph{\thesubsubsection.\arabic{paragraph}} +\else +\def\thesection{\Roman{section}} % I +% V1.7, \mbox prevents breaks around - +\def\thesubsection{\mbox{\thesection-\Alph{subsection}}} % I-A +% V1.7 use I-A1 format used by the IEEE rather than I-A.1 +\def\thesubsubsection{\thesubsection\arabic{subsubsection}} % I-A1 +\def\theparagraph{\thesubsubsection\alph{paragraph}} % I-A1a +\fi + +% From Heiko Oberdiek. Because of the \mbox in \thesubsection, we need to +% tell hyperref to disable the \mbox command when making PDF bookmarks. +% This done already with hyperref.sty version 6.74o and later, but +% it will not hurt to do it here again for users of older versions. +\@ifundefined{pdfstringdefPreHook}{\let\pdfstringdefPreHook\@empty}{}% +\g@addto@macro\pdfstringdefPreHook{\let\mbox\relax} + + +% Main text forms (how shown in main text headings) +% V1.6, using \thesection in \thesectiondis allows changes +% in the former to automatically appear in the latter +\ifCLASSOPTIONcompsoc + \ifCLASSOPTIONconference% compsoc conference + \def\thesectiondis{\thesection.} + \def\thesubsectiondis{\thesectiondis\arabic{subsection}.} + \def\thesubsubsectiondis{\thesubsectiondis\arabic{subsubsection}.} + \def\theparagraphdis{\thesubsubsectiondis\arabic{paragraph}.} + \else% compsoc not conferencs + \def\thesectiondis{\thesection} + \def\thesubsectiondis{\thesectiondis.\arabic{subsection}} + \def\thesubsubsectiondis{\thesubsectiondis.\arabic{subsubsection}} + \def\theparagraphdis{\thesubsubsectiondis.\arabic{paragraph}} + \fi +\else% not compsoc + \def\thesectiondis{\thesection.} % I. + \def\thesubsectiondis{\Alph{subsection}.} % B. + \def\thesubsubsectiondis{\arabic{subsubsection})} % 3) + \def\theparagraphdis{\alph{paragraph})} % d) +\fi + +% just like LaTeX2e's \@eqnnum +\def\theequationdis{{\normalfont \normalcolor (\theequation)}}% (1) +% IEEEsubequation used only by IEEEtran's IEEEeqnarray +\def\theIEEEsubequationdis{{\normalfont \normalcolor (\theIEEEsubequation)}}% (1a) +% redirect LaTeX2e's equation number display and all that depend on +% it, through IEEEtran's \theequationdis +\def\@eqnnum{\theequationdis} + + + +% V1.7 provide string macros as article.cls does +\def\contentsname{Contents} +\def\listfigurename{List of Figures} +\def\listtablename{List of Tables} +\def\refname{References} +\def\indexname{Index} +\def\figurename{Fig.} +\def\tablename{TABLE} +\@IEEEcompsocconfonly{\def\figurename{Figure}} +\def\partname{Part} +\def\appendixname{Appendix} +\def\abstractname{Abstract} +% IEEE specific names +\def\IEEEkeywordsname{Index Terms} +\def\IEEEproofname{Proof} + + +% LIST OF FIGURES AND TABLES AND TABLE OF CONTENTS +% +\def\@pnumwidth{1.55em} +\def\@tocrmarg{2.55em} +\def\@dotsep{4.5} +\setcounter{tocdepth}{3} + +% adjusted some spacings here so that section numbers will not easily +% collide with the section titles. +% VIII; VIII-A; and VIII-A.1 are usually the worst offenders. +% MDS 1/2001 +\def\tableofcontents{\section*{\contentsname}\@starttoc{toc}} +\def\l@section#1#2{\addpenalty{\@secpenalty}\addvspace{1.0em plus 1pt}% + \@tempdima 2.75em \begingroup \parindent \z@ \rightskip \@pnumwidth% + \parfillskip-\@pnumwidth {\bfseries\leavevmode #1}\hfil\hbox to\@pnumwidth{\hss #2}\par% + \endgroup} +% argument format #1:level, #2:labelindent,#3:labelsep +\def\l@subsection{\@dottedtocline{2}{2.75em}{3.75em}} +\def\l@subsubsection{\@dottedtocline{3}{6.5em}{4.5em}} +% must provide \l@ defs for ALL sublevels EVEN if tocdepth +% is such as they will not appear in the table of contents +% these defs are how TOC knows what level these things are! +\def\l@paragraph{\@dottedtocline{4}{6.5em}{5.5em}} +\def\l@subparagraph{\@dottedtocline{5}{6.5em}{6.5em}} +\def\listoffigures{\section*{\listfigurename}\@starttoc{lof}} +\def\l@figure{\@dottedtocline{1}{0em}{2.75em}} +\def\listoftables{\section*{\listtablename}\@starttoc{lot}} +\let\l@table\l@figure + + +% Definitions for floats +% +% Normal Floats +% V1.8 floatsep et al. revised down by 0.15\baselineskip +% to account for the sideeffects of \topskip compensation +\floatsep 0.85\baselineskip plus 0.2\baselineskip minus 0.2\baselineskip +\textfloatsep 1.55\baselineskip plus 0.2\baselineskip minus 0.4\baselineskip +\@fptop 0pt plus 1fil +\@fpsep 0.75\baselineskip plus 2fil +\@fpbot 0pt plus 1fil +\def\topfraction{0.9} +\def\bottomfraction{0.4} +\def\floatpagefraction{0.8} +% V1.7, let top floats approach 90% of page +\def\textfraction{0.1} + +% Double Column Floats +\dblfloatsep 0.85\baselineskip plus 0.2\baselineskip minus 0.2\baselineskip + +\dbltextfloatsep 1.55\baselineskip plus 0.2\baselineskip minus 0.4\baselineskip +% Note that it would be nice if the rubber here actually worked in LaTeX2e. +% There is a long standing limitation in LaTeX, first discovered (to the best +% of my knowledge) by Alan Jeffrey in 1992. LaTeX ignores the stretchable +% portion of \dbltextfloatsep, and as a result, double column figures can and +% do result in an non-integer number of lines in the main text columns with +% underfull vbox errors as a consequence. A post to comp.text.tex +% by Donald Arseneau confirms that this had not yet been fixed in 1998. +% IEEEtran V1.6 will fix this problem for you in the titles, but it doesn't +% protect you from other double floats. Happy vspace'ing. + +\@dblfptop 0pt plus 1fil +\@dblfpsep 0.75\baselineskip plus 2fil +\@dblfpbot 0pt plus 1fil +\def\dbltopfraction{0.8} +\def\dblfloatpagefraction{0.8} +\setcounter{dbltopnumber}{4} + +\intextsep 0.85\baselineskip plus 0.2\baselineskip minus 0.2\baselineskip +\setcounter{topnumber}{2} +\setcounter{bottomnumber}{2} +\setcounter{totalnumber}{4} + + + +% article class provides these, we should too. +\newlength\abovecaptionskip +\newlength\belowcaptionskip +% but only \abovecaptionskip is used above figure captions and *below* table +% captions +\setlength\abovecaptionskip{0.5\baselineskip} +% compsoc journals are a little more generous +\ifCLASSOPTIONcompsoc\ifCLASSOPTIONjournal + \setlength\abovecaptionskip{0.75\baselineskip} +\fi\fi +\setlength\belowcaptionskip{0pt} +% V1.6 create hooks in case the caption spacing ever needs to be +% overridden by a user +\def\@IEEEfigurecaptionsepspace{\vskip\abovecaptionskip\relax}% +\def\@IEEEtablecaptionsepspace{\vskip\abovecaptionskip\relax}% + + +% 1.6b revise caption system so that \@makecaption uses two arguments +% as with LaTeX2e. Otherwise, there will be problems when using hyperref. +\def\@IEEEtablestring{table} + + +% V1.8 compensate for \topskip so top of top figures align with tops of the first lines of main text +% here we calculate a space equal to the amount \topskip exceeds the main text height +% we hook in at \@floatboxreset +\def\@IEEEfiguretopskipspace{\ifdim\prevdepth=-1000pt\relax +\setlength{\@IEEEtrantmpdimenA}{1\topskip}\relax +\addtolength{\@IEEEtrantmpdimenA}{-0.7\@IEEEnormalsizeunitybaselineskip}\relax +\vspace*{\@IEEEtrantmpdimenA}\fi} +% V1.8 compensate for \topskip at the top of top tables so caption text is on main text baseline +% use a strut set on the caption baseline within \@makecaption +\def\@IEEEtabletopskipstrut{\ifdim\prevdepth=-1000pt\rule{0pt}{\topskip}\fi} +% the \ifdim\prevdepth checks are always expected to be true for IEEE style float caption ordering +% because top of figure content and top of captions in tables is the first thing on the vertical +% list of these floats +% thanks to Donald Arseneau for his 2000/11/11 post "Re: caption hacking" with info on this topic. + + +\ifCLASSOPTIONcompsoc +% V1.7 compsoc \@makecaption +\ifCLASSOPTIONconference% compsoc conference +\long\def\@makecaption#1#2{% +% test if is a for a figure or table +\ifx\@captype\@IEEEtablestring% +% if a table, do table caption +\footnotesize\bgroup\par\centering\@IEEEtabletopskipstrut{\normalfont\footnotesize {#1.}\nobreakspace\scshape #2}\par\addvspace{0.5\baselineskip}\egroup% +\@IEEEtablecaptionsepspace +% if not a table, format it as a figure +\else +\@IEEEfigurecaptionsepspace +\setbox\@tempboxa\hbox{\normalfont\footnotesize {#1.}\nobreakspace #2}% +\ifdim \wd\@tempboxa >\hsize% +% if caption is longer than a line, let it wrap around +\setbox\@tempboxa\hbox{\normalfont\footnotesize {#1.}\nobreakspace}% +\parbox[t]{\hsize}{\normalfont\footnotesize \noindent\unhbox\@tempboxa#2}% +% if caption is shorter than a line, center +\else% +\hbox to\hsize{\normalfont\footnotesize\hfil\box\@tempboxa\hfil}% +\fi\fi} +% +\else% nonconference compsoc +\long\def\@makecaption#1#2{% +% test if is a for a figure or table +\ifx\@captype\@IEEEtablestring% +% if a table, do table caption +\footnotesize\bgroup\par\centering\@IEEEtabletopskipstrut{\normalfont\sffamily\footnotesize #1}\\{\normalfont\sffamily\footnotesize #2}\par\addvspace{0.5\baselineskip}\egroup% +\@IEEEtablecaptionsepspace +% if not a table, format it as a figure +\else +\@IEEEfigurecaptionsepspace +\setbox\@tempboxa\hbox{\normalfont\sffamily\footnotesize {#1.}\nobreakspace #2}% +\ifdim \wd\@tempboxa >\hsize% +% if caption is longer than a line, let it wrap around +\setbox\@tempboxa\hbox{\normalfont\sffamily\footnotesize {#1.}\nobreakspace}% +\parbox[t]{\hsize}{\normalfont\sffamily\footnotesize \noindent\unhbox\@tempboxa#2}% +% if caption is shorter than a line, left justify +\else% +\hbox to\hsize{\normalfont\sffamily\footnotesize\box\@tempboxa\hfil}% +\fi\fi} +\fi +% +\else% traditional noncompsoc \@makecaption +\long\def\@makecaption#1#2{% +% test if is a for a figure or table +\ifx\@captype\@IEEEtablestring% +% if a table, do table caption +\footnotesize\bgroup\par\centering\@IEEEtabletopskipstrut{\normalfont\footnotesize #1}\\{\normalfont\footnotesize\scshape #2}\par\addvspace{0.5\baselineskip}\egroup% +\@IEEEtablecaptionsepspace +% if not a table, format it as a figure +\else +\@IEEEfigurecaptionsepspace +% 3/2001 use footnotesize, not small; use two nonbreaking spaces, not one +\setbox\@tempboxa\hbox{\normalfont\footnotesize {#1.}\nobreakspace\nobreakspace #2}% +\ifdim \wd\@tempboxa >\hsize% +% if caption is longer than a line, let it wrap around +\setbox\@tempboxa\hbox{\normalfont\footnotesize {#1.}\nobreakspace\nobreakspace}% +\parbox[t]{\hsize}{\normalfont\footnotesize\noindent\unhbox\@tempboxa#2}% +% if caption is shorter than a line, center if conference, left justify otherwise +\else% +\ifCLASSOPTIONconference \hbox to\hsize{\normalfont\footnotesize\hfil\box\@tempboxa\hfil}% +\else \hbox to\hsize{\normalfont\footnotesize\box\@tempboxa\hfil}% +\fi\fi\fi} +\fi + + + +% V1.7 disable captions class option, do so in a way that retains operation of \label +% within \caption +\ifCLASSOPTIONcaptionsoff +\long\def\@makecaption#1#2{\vspace*{2em}\footnotesize\bgroup\par\addvspace{0.5\baselineskip}\centering{\footnotesize #1}\par\addvspace{0.5\baselineskip}\egroup% +\let\@IEEEtemporiglabeldefsave\label +\let\@IEEEtemplabelargsave\relax +\def\label##1{\gdef\@IEEEtemplabelargsave{##1}}% +\setbox\@tempboxa\hbox{#2}% +\let\label\@IEEEtemporiglabeldefsave +\ifx\@IEEEtemplabelargsave\relax\else\label{\@IEEEtemplabelargsave}\fi} +\fi + + +% V1.7 define end environments with \def not \let so as to work OK with +% preview-latex +\newcounter{figure} +\def\thefigure{\@arabic\c@figure} +\def\fps@figure{tbp} +\def\ftype@figure{1} +\def\ext@figure{lof} +\def\fnum@figure{\figurename\nobreakspace\thefigure} +% V1.8 within figures add \@IEEEfiguretopskipspace compensation to LaTeX2e's \@floatboxreset +\def\figure{\def\@floatboxreset{\reset@font\normalsize\@setminipage\@IEEEfiguretopskipspace}\@float{figure}} +\def\endfigure{\end@float} +% V1.8 also add \@IEEEfiguretopskipspace compensation to \figure* +\@namedef{figure*}{\def\@floatboxreset{\reset@font\normalsize\@setminipage\@IEEEfiguretopskipspace}\@dblfloat{figure}} +\@namedef{endfigure*}{\end@dblfloat} + +\newcounter{table} +\ifCLASSOPTIONcompsoc +\def\thetable{\arabic{table}} +\else +\def\thetable{\@Roman\c@table} +\fi +\def\fps@table{tbp} +\def\ftype@table{2} +\def\ext@table{lot} +\def\fnum@table{\tablename\nobreakspace\thetable} +% V1.6 The IEEE uses 8pt text for tables +% within tables alter LaTeX2e's \@floatboxreset to use \footnotesize +\def\table{\def\@floatboxreset{\reset@font\footnotesize\@setminipage}\@float{table}} +\def\endtable{\end@float} +% v1.6b double column tables need to default to footnotesize as well. +\@namedef{table*}{\def\@floatboxreset{\reset@font\footnotesize\@setminipage}\@dblfloat{table}} +\@namedef{endtable*}{\end@dblfloat} + + + + +%% -- Command Argument Scanning Support Functions -- +%% V1.8a + +% usage: \@IEEEstripouterbraces*{} +% \@IEEEstripouterbraces fully expands its argument (which it then stores +% in \@IEEEstripouterbracesarg) via \edef, then removes any outer enclosing +% braces, and finally stores the result in the macro +% \@IEEEstrippedouterbraces. +% +% For example: +% \@IEEEstripouterbraces{{{{ab}c}}} +% results in: +% +% \@IEEEstripouterbracesarg ==> a macro containing {{{ab}c}} +% \@IEEEstrippedouterbraces ==> a macro containing {ab}c +% +% the *-star form,\@IEEEstripouterbraces*, does not expand the argument +% contents during processing +\def\@IEEEstripouterbraces{\@ifstar{\let\@IEEEstripouterbracesdef=\def\@@IEEEstripouterbraces}{\let\@IEEEstripouterbracesdef=\edef\@@IEEEstripouterbraces}} + +\def\@@IEEEstripouterbraces#1{\@IEEEstripouterbracesdef\@IEEEstripouterbracesarg{#1}\relax +% If the macro is unchanged after being acquired as a single delimited +% argument, we know we have one sequence of tokens without any enclosing +% braces. Loop until this is true. +\loop + \expandafter\@@@IEEEstripouterbraces\@IEEEstripouterbracesarg\@IEEEgeneralsequenceDELIMITER +\ifx\@IEEEstrippedouterbraces\@IEEEstripouterbracesarg +\else + \let\@IEEEstripouterbracesarg\@IEEEstrippedouterbraces +\repeat} + +\def\@@@IEEEstripouterbraces#1\@IEEEgeneralsequenceDELIMITER{\def\@IEEEstrippedouterbraces{#1}} + + + +% usage: \@IEEEextractgroup*{} +% \@IEEEextractgroup fully expands its argument (which it then stores in +% \@IEEEextractgrouparg) via \edef and then assigns the first "brace group" +% of tokens to the macro \@IEEEextractedgroup. +% The remaining groups, if any, are stored in the macro +% \@IEEEextractedgroupremain. If the argument does not contain the requisite +% groups, the respective macros will be defined to be empty. +% There is an asymmetry in that \@IEEEextractedgroup is stripped of its first +% outer grouping while \@IEEEextractedgroupremain retains even the outer +% grouping (if present) that originally identified it as a group. +% +% For example: +% \@IEEEextractgroup{{{ab}}{c{de}}} +% results in: +% +% \@IEEEextractgrouparg ==> a macro containing {{ab}}{c{de}} +% \@IEEEextractedgroup ==> a macro containing {ab} +% \@IEEEextractedgroupremain ==> a macro containing {c{de}} +% +% The *-star form, \@IEEEextractgroup*, does not expand its argument +% contents during processing. +\def\@IEEEextractgroup{\@ifstar{\let\@IEEEextractgroupdef=\def\@@IEEEextractgroup}{\let\@IEEEextractgroupdef=\edef\@@IEEEextractgroup}} + +\def\@@IEEEextractgroup#1{\@IEEEextractgroupdef\@IEEEextractgrouparg{#1}\relax +% trap the case of an empty extracted group as this would cause problems with +% \@IEEEextractgroupremain's argument acquisition +\ifx\@IEEEextractgrouparg\@empty + \def\@IEEEextractedgroup{}\relax + \def\@IEEEextractedgroupremain{}\relax +\else + % We have to use some dirty tricks here. We want to insert {} around + % whatever remains after the first group so that TeX's argument scanner + % will preserve any originally enclosing braces as well as provide an + % empty argument to acquire even if there isn't a second group. + % In this first of two dirty tricks, we put a } at the end of the structure + % we are going to extract from. The \ifnum0=`{\fi keeps TeX happy to allow + % what would otherwise be an unbalanced macro definition for + % \@@IEEEextractgroup to be acceptable to it. + \ifnum0=`{\fi\expandafter\@IEEEextractgroupremain\@IEEEextractgrouparg}\relax +\fi} + +% In the second part of the dirty tricks, we insert a leading { right after +% the first group is acquired, but before the remainder is. Again, the +% \ifnum0=`}\fi keeps TeX happy during definition time, but will disappear +% during run time. +\def\@IEEEextractgroupremain#1{\def\@IEEEextractedgroup{#1}\expandafter\@@IEEEextractgroupremain\expandafter{\ifnum0=`}\fi} + +\def\@@IEEEextractgroupremain#1{\def\@IEEEextractedgroupremain{#1}} + + + +% \@IEEEextracttoken relocated at top because margin setting commands rely on it + + + +% usage: \@IEEEextracttokengroups*{} +% \@IEEEextracttokengroups fully expands its argument (which it then stores +% in \@IEEEextracttokengroupsarg) and then assigns the first "brace group" of +% tokens (with the outermost braces removed) to the macro +% \@IEEEextractedfirstgroup. +% The meaning of the first nonbrace (but including the empty group) token +% within this first group is assigned via \let to \@IEEEextractedfirsttoken +% as well as stored in the macro \@IEEEextractedfirsttokenmacro. If a first +% nonbrace token does not exist (or is an empty group), these will be \relax +% and empty, respectively. Tokens that would otherwise be discarded during +% the acquisition of the first token in the first group are stored in +% \@IEEEextractedfirsttokensdiscarded, however their original relative brace +% nesting depths are not guaranteed to be preserved. +% The first group within this first group is stored in the macro +% \@IEEEextractedfirstfirstgroup. +% Likewise for the next group after the first: \@IEEEextractednextgroup, +% \@IEEEextractednextfirstgroup, \@IEEEextractednextgroupfirsttoken, +% \@IEEEextractednextgroupfirsttokenmacro, and +% \@IEEEextractednextfirsttokensdiscarded. +% All tokens/groups after the first group, including any enclosing braces, +% are stored in the macro \@IEEEextractedafterfirstgroupremain which will +% be empty if none exist. +% +% For example: +% \@IEEEextracttokengroups{{{ab}{cd}}{{ef}g}} +% will result in: +% +% \@IEEEextracttokengroupsarg ==> a macro containing {{ab}{cd}}{{ef}g} +% \@IEEEextractedfirstgroup ==> a macro containing {ab}{cd} +% \@IEEEextractedafterfirstgroupremain ==> a macro containing {{ef}g} +% \@IEEEextractedfirsttoken ==> the letter a +% \@IEEEextractedfirsttokenmacro ==> a macro containing a +% \@IEEEextractedfirsttokensdiscarded ==> a macro containing bcd +% \@IEEEextractedfirstfirstgroup ==> a macro containing ab +% \@IEEEextractednextgroup ==> a macro containing {ef}g +% \@IEEEextractednextfirsttoken ==> the letter e +% \@IEEEextractednextfirsttokenmacro ==> a macro containing e +% \@IEEEextractednextfirsttokensdiscarded ==> a macro containing fg +% \@IEEEextractednextfirstgroup ==> a macro containing ef +% +% If given an empty argument, \@IEEEextractedfirsttoken and +% \@IEEEextractednextfirsttoken will be set to \relax +% and all the macros will be empty. +% the *-star form, \@IEEEextracttokengroups*, does not expand its argument +% contents during processing. +% +% Depends on: \@IEEEextractgroup, \@IEEEextracttoken +\def\@IEEEextracttokengroups{\@ifstar{\let\@IEEEextracttokengroupsdef=\def\@@IEEEextracttokengroups}{\let\@IEEEextracttokengroupsdef=\edef\@@IEEEextracttokengroups}} +\def\@@IEEEextracttokengroups#1{\@IEEEextracttokengroupsdef\@IEEEextracttokengroupsarg{#1}\relax +% begin extraction, these functions are safe with empty arguments +% first group +\expandafter\@IEEEextractgroup\expandafter*\expandafter{\@IEEEextracttokengroupsarg}\relax +\let\@IEEEextractedfirstgroup\@IEEEextractedgroup +\let\@IEEEextractedafterfirstgroupremain\@IEEEextractedgroupremain +\expandafter\@IEEEextracttoken\expandafter*\expandafter{\@IEEEextractedfirstgroup}\relax +\let\@IEEEextractedfirsttoken\@IEEEextractedtoken +\let\@IEEEextractedfirsttokenmacro\@IEEEextractedtokenmacro +\let\@IEEEextractedfirsttokensdiscarded\@IEEEextractedtokensdiscarded +% first first group +\expandafter\@IEEEextractgroup\expandafter*\expandafter{\@IEEEextractedfirstgroup}\relax +\let\@IEEEextractedfirstfirstgroup\@IEEEextractedgroup +% next group +\expandafter\@IEEEextractgroup\expandafter*\expandafter{\@IEEEextractedafterfirstgroupremain}\relax +\let\@IEEEextractednextgroup\@IEEEextractedgroup +\expandafter\@IEEEextracttoken\expandafter*\expandafter{\@IEEEextractednextgroup}\relax +\let\@IEEEextractednextfirsttoken\@IEEEextractedtoken +\let\@IEEEextractednextfirsttokenmacro\@IEEEextractedtokenmacro +\let\@IEEEextractednextfirsttokensdiscarded\@IEEEextractedtokensdiscarded +% next first group +\expandafter\@IEEEextractgroup\expandafter*\expandafter{\@IEEEextractednextgroup}\relax +\let\@IEEEextractednextfirstgroup\@IEEEextractedgroup} + + +%% -- End of Command Argument Scanning Support Functions -- + + + + +%% +%% START OF IEEEeqnarray DEFINITIONS +%% +%% Inspired by the concepts, examples, and previous works of LaTeX +%% coders and developers such as Donald Arseneau, Fred Bartlett, +%% David Carlisle, Tony Liu, Frank Mittelbach, Piet van Oostrum, +%% Roland Winkler and Mark Wooding. +%% I don't make the claim that my work here is even near their calibre. ;) + + +\newif\if@IEEEeqnarrayboxnojot% flag to indicate if the environment was called as the star form +\@IEEEeqnarrayboxnojotfalse + +\newif\if@advanceIEEEeqncolcnt% tracks if the environment should advance the col counter +% allows a way to make an \IEEEeqnarraybox that can be used within an \IEEEeqnarray +% used by IEEEeqnarraymulticol so that it can work properly in both +\@advanceIEEEeqncolcnttrue + +\newcount\@IEEEeqnnumcols % tracks how many IEEEeqnarray cols are defined +\newcount\@IEEEeqncolcnt % tracks how many IEEEeqnarray cols the user actually used + + +% The default math style used by the columns +\def\IEEEeqnarraymathstyle{\displaystyle} +% The default text style used by the columns +% default to using the current font +\def\IEEEeqnarraytextstyle{\relax} + +% like the iedlistdecl but for \IEEEeqnarray +\def\IEEEeqnarraydecl{\relax} +\def\IEEEeqnarrayboxdecl{\relax} + + + +% V1.8 flags to indicate that equation numbering is to persist +\newif\if@IEEEeqnumpersist% +\@IEEEeqnumpersistfalse +\newif\if@IEEEsubeqnumpersist% +\@IEEEsubeqnumpersistfalse +% +% V1.8 flags to indicate if (sub)equation number of last line was preadvanced +\newif\if@IEEEeqnumpreadv% +\@IEEEeqnumpreadvfalse +\newif\if@IEEEsubeqnumpreadv% +\@IEEEsubeqnumpreadvfalse + +\newcount\@IEEEsubeqnnumrollback% saves previous value of IEEEsubequation number in case we need to restore it + +% \yesnumber is the opposite of \nonumber +% a novel concept with the same def as the equationarray package +% However, we give IEEE versions too since some LaTeX packages such as +% the MDWtools mathenv.sty redefine \nonumber to something else. +% This command is intended for use in non-IEEEeqnarray math environments +\providecommand{\yesnumber}{\global\@eqnswtrue} + + +% IEEEyes/nonumber +% V1.8 add persistant * forms +% These commands can alter the type of equation an IEEEeqnarray line is. +\def\IEEEyesnumber{\@ifstar{\global\@IEEEeqnumpersisttrue\global\@IEEEsubeqnumpersistfalse\@IEEEyesnumber}{\@IEEEyesnumber}} + +\def\@IEEEyesnumber{\global\@eqnswtrue +\if@IEEEeqnarrayISinner% alter counters and label only inside an IEEEeqnarray +\ifnum\c@IEEEsubequation>0\relax + \stepcounter{equation}\setcounter{IEEEsubequation}{0}\gdef\@currentlabel{\p@equation\theequation}\relax + \gdef\@currentHref{\@IEEEtheHrefequation}% setup hyperref label +\fi +% even if we reached this eqn num via a preadv, it is legit now +\global\@IEEEeqnumpreadvfalse\global\@IEEEsubeqnumpreadvfalse +\fi} + +\def\IEEEnonumber{\@ifstar{\global\@IEEEeqnumpersistfalse\global\@IEEEsubeqnumpersistfalse\global\@eqnswfalse}{\global\@eqnswfalse}} + + +\def\IEEEyessubnumber{\@ifstar{\global\@IEEEsubeqnumpersisttrue\@IEEEyessubnumber}{\@IEEEyessubnumber}} +% +\def\@IEEEyessubnumber{\if@IEEEeqnarrayISinner% alter counters and label only inside an IEEEeqnarray + \ifnum\c@IEEEsubequation>0\relax% if it already is a subequation, we are good to go as-is + \else% if we are a regular equation we have to watch out for two cases + \if@IEEEeqnumpreadv% if this equation is the result of a preadvance, backout and bump the sub eqnnum + \global\advance\c@equation\m@ne\global\c@IEEEsubequation=\@IEEEsubeqnnumrollback\addtocounter{IEEEsubequation}{1}\relax + \else% non-preadvanced equations just need initialization of their sub eqnnum + \setcounter{IEEEsubequation}{1}\relax + \fi + \fi% fi already is subequation + \gdef\@currentlabel{\p@IEEEsubequation\theIEEEsubequation}\relax + \gdef\@currentHref{\@IEEEtheHrefsubequation}% setup hyperref label + \global\@IEEEeqnumpreadvfalse\global\@IEEEsubeqnumpreadvfalse% no longer a preadv anymore + \global\@eqnswtrue +\fi} + + +\def\IEEEnosubnumber{\@ifstar{\global\@IEEEsubeqnumpersistfalse\@IEEEnosubnumber}{\@IEEEnosubnumber}} +% +\def\@IEEEnosubnumber{\if@IEEEeqnarrayISinner% alter counters and label only inside an IEEEeqnarray + \if@eqnsw % we do nothing unless we know we will display because we play with the counters here + % if it currently is a subequation, bump up to the next equation number and turn off the subequation + \ifnum\c@IEEEsubequation>0\relax\addtocounter{equation}{1}\setcounter{IEEEsubequation}{0}\relax + \fi + \global\@IEEEeqnumpreadvfalse\global\@IEEEsubeqnumpreadvfalse% no longer a preadv anymore + \gdef\@currentlabel{\p@equation\theequation}\relax + \gdef\@currentHref{\@IEEEtheHrefequation}% setup hyperref label + \fi +\fi} + + + +% allows users to "push away" equations that get too close to the equation numbers +\def\IEEEeqnarraynumspace{\hphantom{\ifnum\c@IEEEsubequation>0\relax\theIEEEsubequationdis\else\theequationdis\fi}} + +% provides a way to span multiple columns within IEEEeqnarray environments +% will consider \if@advanceIEEEeqncolcnt before globally advancing the +% column counter - so as to work within \IEEEeqnarraybox +% usage: \IEEEeqnarraymulticol{number cols. to span}{col type}{cell text} +\long\def\IEEEeqnarraymulticol#1#2#3{\multispan{#1}\relax +% check if column is defined for the precolumn definition +% We have to be careful here because TeX scans for & even within an \iffalse +% where it does not expand macros. So, if we used only one \ifx and a #3 +% appeared in the false branch and the user inserted another alignment +% structure that uses & in the \IEEEeqnarraymulticol{}, TeX will not see that +% there is an inner alignment in the false branch yet still will see any & +% there and will think that they apply to the outer alignment resulting in an +% incomplete \ifx error. +% So, here we use separate checks for the pre and post parts in order to keep +% the #3 outside of all conditionals. +\relax\expandafter\ifx\csname @IEEEeqnarraycolDEF#2\endcsname\@IEEEeqnarraycolisdefined\relax +\csname @IEEEeqnarraycolPRE#2\endcsname +\else% if not, error and use default type +\@IEEEclspkgerror{Invalid column type "#2" in \string\IEEEeqnarraymulticol.\MessageBreak +Using a default centering column instead}% +{You must define IEEEeqnarray column types before use.}% +\csname @IEEEeqnarraycolPRE@IEEEdefault\endcsname +\fi +% The ten \relax are to help prevent misleading error messages in case a user +% accidently inserted a macro that tries to acquire additional arguments. +#3\relax\relax\relax\relax\relax\relax\relax\relax\relax\relax +% check if column is defined for the postcolumn definition +\expandafter\ifx\csname @IEEEeqnarraycolDEF#2\endcsname\@IEEEeqnarraycolisdefined\relax +\csname @IEEEeqnarraycolPOST#2\endcsname +\else% if not, use the default type +\csname @IEEEeqnarraycolPOST@IEEEdefault\endcsname +\fi +% advance column counter only if the IEEEeqnarray environment wants it +\if@advanceIEEEeqncolcnt\global\advance\@IEEEeqncolcnt by #1\relax\fi} + +% like \omit, but maintains track of the column counter for \IEEEeqnarray +\def\IEEEeqnarrayomit{\omit\if@advanceIEEEeqncolcnt\global\advance\@IEEEeqncolcnt by 1\relax\fi} + + +% provides a way to define a letter referenced column type +% usage: \IEEEeqnarraydefcol{col. type letter/name}{pre insertion text}{post insertion text} +\def\IEEEeqnarraydefcol#1#2#3{\expandafter\def\csname @IEEEeqnarraycolPRE#1\endcsname{#2}% +\expandafter\def\csname @IEEEeqnarraycolPOST#1\endcsname{#3}% +\expandafter\def\csname @IEEEeqnarraycolDEF#1\endcsname{1}} + + +% provides a way to define a numerically referenced inter-column glue types +% usage: \IEEEeqnarraydefcolsep{col. glue number}{glue definition} +\def\IEEEeqnarraydefcolsep#1#2{\expandafter\def\csname @IEEEeqnarraycolSEP\romannumeral #1\endcsname{#2}% +\expandafter\def\csname @IEEEeqnarraycolSEPDEF\romannumeral #1\endcsname{1}} + + +\def\@IEEEeqnarraycolisdefined{1}% just a macro for 1, used for checking undefined column types + + +% expands and appends the given argument to the \@IEEEtrantmptoksA token list +% used to build up the \halign preamble +\def\@IEEEappendtoksA#1{\edef\@@IEEEappendtoksA{\@IEEEtrantmptoksA={\the\@IEEEtrantmptoksA #1}}% +\@@IEEEappendtoksA} + +% also appends to \@IEEEtrantmptoksA, but does not expand the argument +% uses \toks8 as a scratchpad register +\def\@IEEEappendNOEXPANDtoksA#1{\toks8={#1}% +\edef\@@IEEEappendNOEXPANDtoksA{\@IEEEtrantmptoksA={\the\@IEEEtrantmptoksA\the\toks8}}% +\@@IEEEappendNOEXPANDtoksA} + +% define some common column types for the user +% math +\IEEEeqnarraydefcol{l}{$\IEEEeqnarraymathstyle}{$\hfil} +\IEEEeqnarraydefcol{c}{\hfil$\IEEEeqnarraymathstyle}{$\hfil} +\IEEEeqnarraydefcol{r}{\hfil$\IEEEeqnarraymathstyle}{$} +\IEEEeqnarraydefcol{L}{$\IEEEeqnarraymathstyle{}}{{}$\hfil} +\IEEEeqnarraydefcol{C}{\hfil$\IEEEeqnarraymathstyle{}}{{}$\hfil} +\IEEEeqnarraydefcol{R}{\hfil$\IEEEeqnarraymathstyle{}}{{}$} +% text +\IEEEeqnarraydefcol{s}{\IEEEeqnarraytextstyle}{\hfil} +\IEEEeqnarraydefcol{t}{\hfil\IEEEeqnarraytextstyle}{\hfil} +\IEEEeqnarraydefcol{u}{\hfil\IEEEeqnarraytextstyle}{} + +% vertical rules +\IEEEeqnarraydefcol{v}{}{\vrule width\arrayrulewidth} +\IEEEeqnarraydefcol{vv}{\vrule width\arrayrulewidth\hfil}{\hfil\vrule width\arrayrulewidth} +\IEEEeqnarraydefcol{V}{}{\vrule width\arrayrulewidth\hskip\doublerulesep\vrule width\arrayrulewidth} +\IEEEeqnarraydefcol{VV}{\vrule width\arrayrulewidth\hskip\doublerulesep\vrule width\arrayrulewidth\hfil}% +{\hfil\vrule width\arrayrulewidth\hskip\doublerulesep\vrule width\arrayrulewidth} + +% horizontal rules +\IEEEeqnarraydefcol{h}{}{\leaders\hrule height\arrayrulewidth\hfil} +\IEEEeqnarraydefcol{H}{}{\leaders\vbox{\hrule width\arrayrulewidth\vskip\doublerulesep\hrule width\arrayrulewidth}\hfil} + +% plain +\IEEEeqnarraydefcol{x}{}{} +\IEEEeqnarraydefcol{X}{$}{$} + +% the default column type to use in the event a column type is not defined +\IEEEeqnarraydefcol{@IEEEdefault}{\hfil$\IEEEeqnarraymathstyle}{$\hfil} + + +% a zero tabskip (used for "-" col types) +\def\@IEEEeqnarraycolSEPzero{0pt plus 0pt minus 0pt} +% a centering tabskip (used for "+" col types) +\def\@IEEEeqnarraycolSEPcenter{1000pt plus 0pt minus 1000pt} + +% top level default tabskip glues for the start, end, and inter-column +% may be reset within environments not always at the top level, e.g., \IEEEeqnarraybox +\edef\@IEEEeqnarraycolSEPdefaultstart{\@IEEEeqnarraycolSEPcenter}% default start glue +\edef\@IEEEeqnarraycolSEPdefaultend{\@IEEEeqnarraycolSEPcenter}% default end glue +\edef\@IEEEeqnarraycolSEPdefaultmid{\@IEEEeqnarraycolSEPzero}% default inter-column glue + + + +% creates a vertical rule that extends from the bottom to the top a a cell +% Provided in case other packages redefine \vline some other way. +% usage: \IEEEeqnarrayvrule[rule thickness] +% If no argument is provided, \arrayrulewidth will be used for the rule thickness. +\newcommand\IEEEeqnarrayvrule[1][\arrayrulewidth]{\vrule\@width#1\relax} + +% creates a blank separator row +% usage: \IEEEeqnarrayseprow[separation length][font size commands] +% default is \IEEEeqnarrayseprow[0.25\normalbaselineskip][\relax] +% blank arguments inherit the default values +% uses \skip5 as a scratch register - calls \@IEEEeqnarraystrutsize which uses more scratch registers +\def\IEEEeqnarrayseprow{\relax\@ifnextchar[{\@IEEEeqnarrayseprow}{\@IEEEeqnarrayseprow[0.25\normalbaselineskip]}} +\def\@IEEEeqnarrayseprow[#1]{\relax\@ifnextchar[{\@@IEEEeqnarrayseprow[#1]}{\@@IEEEeqnarrayseprow[#1][\relax]}} +\def\@@IEEEeqnarrayseprow[#1][#2]{\def\@IEEEeqnarrayseprowARGONE{#1}% +\ifx\@IEEEeqnarrayseprowARGONE\@empty% +% get the skip value, based on the font commands +% use skip5 because \IEEEeqnarraystrutsize uses \skip0, \skip2, \skip3 +% assign within a bogus box to confine the font changes +{\setbox0=\hbox{#2\relax\global\skip5=0.25\normalbaselineskip}}% +\else% +{\setbox0=\hbox{#2\relax\global\skip5=#1}}% +\fi% +\@IEEEeqnarrayhoptolastcolumn\IEEEeqnarraystrutsize{\skip5}{0pt}[\relax]\relax} + +% creates a blank separator row, but omits all the column templates +% usage: \IEEEeqnarrayseprowcut[separation length][font size commands] +% default is \IEEEeqnarrayseprowcut[0.25\normalbaselineskip][\relax] +% blank arguments inherit the default values +% uses \skip5 as a scratch register - calls \@IEEEeqnarraystrutsize which uses more scratch registers +\def\IEEEeqnarrayseprowcut{\multispan{\@IEEEeqnnumcols}\relax% span all the cols +% advance column counter only if the IEEEeqnarray environment wants it +\if@advanceIEEEeqncolcnt\global\advance\@IEEEeqncolcnt by \@IEEEeqnnumcols\relax\fi% +\@ifnextchar[{\@IEEEeqnarrayseprowcut}{\@IEEEeqnarrayseprowcut[0.25\normalbaselineskip]}} +\def\@IEEEeqnarrayseprowcut[#1]{\relax\@ifnextchar[{\@@IEEEeqnarrayseprowcut[#1]}{\@@IEEEeqnarrayseprowcut[#1][\relax]}} +\def\@@IEEEeqnarrayseprowcut[#1][#2]{\def\@IEEEeqnarrayseprowARGONE{#1}% +\ifx\@IEEEeqnarrayseprowARGONE\@empty% +% get the skip value, based on the font commands +% use skip5 because \IEEEeqnarraystrutsize uses \skip0, \skip2, \skip3 +% assign within a bogus box to confine the font changes +{\setbox0=\hbox{#2\relax\global\skip5=0.25\normalbaselineskip}}% +\else% +{\setbox0=\hbox{#2\relax\global\skip5=#1}}% +\fi% +\IEEEeqnarraystrutsize{\skip5}{0pt}[\relax]\relax} + + + +% draws a single rule across all the columns optional +% argument determines the rule width, \arrayrulewidth is the default +% updates column counter as needed and turns off struts +% usage: \IEEEeqnarrayrulerow[rule line thickness] +\def\IEEEeqnarrayrulerow{\multispan{\@IEEEeqnnumcols}\relax% span all the cols +% advance column counter only if the IEEEeqnarray environment wants it +\if@advanceIEEEeqncolcnt\global\advance\@IEEEeqncolcnt by \@IEEEeqnnumcols\relax\fi% +\@ifnextchar[{\@IEEEeqnarrayrulerow}{\@IEEEeqnarrayrulerow[\arrayrulewidth]}} +\def\@IEEEeqnarrayrulerow[#1]{\leaders\hrule height#1\hfil\relax% put in our rule +% turn off any struts +\IEEEeqnarraystrutsize{0pt}{0pt}[\relax]\relax} + + +% draws a double rule by using a single rule row, a separator row, and then +% another single rule row +% first optional argument determines the rule thicknesses, \arrayrulewidth is the default +% second optional argument determines the rule spacing, \doublerulesep is the default +% usage: \IEEEeqnarraydblrulerow[rule line thickness][rule spacing] +\def\IEEEeqnarraydblrulerow{\multispan{\@IEEEeqnnumcols}\relax% span all the cols +% advance column counter only if the IEEEeqnarray environment wants it +\if@advanceIEEEeqncolcnt\global\advance\@IEEEeqncolcnt by \@IEEEeqnnumcols\relax\fi% +\@ifnextchar[{\@IEEEeqnarraydblrulerow}{\@IEEEeqnarraydblrulerow[\arrayrulewidth]}} +\def\@IEEEeqnarraydblrulerow[#1]{\relax\@ifnextchar[{\@@IEEEeqnarraydblrulerow[#1]}% +{\@@IEEEeqnarraydblrulerow[#1][\doublerulesep]}} +\def\@@IEEEeqnarraydblrulerow[#1][#2]{\def\@IEEEeqnarraydblrulerowARG{#1}% +% we allow the user to say \IEEEeqnarraydblrulerow[][] +\ifx\@IEEEeqnarraydblrulerowARG\@empty% +\@IEEEeqnarrayrulerow[\arrayrulewidth]% +\else% +\@IEEEeqnarrayrulerow[#1]\relax% +\fi% +\def\@IEEEeqnarraydblrulerowARG{#2}% +\ifx\@IEEEeqnarraydblrulerowARG\@empty% +\\\IEEEeqnarrayseprow[\doublerulesep][\relax]% +\else% +\\\IEEEeqnarrayseprow[#2][\relax]% +\fi% +\\\multispan{\@IEEEeqnnumcols}% +% advance column counter only if the IEEEeqnarray environment wants it +\if@advanceIEEEeqncolcnt\global\advance\@IEEEeqncolcnt by \@IEEEeqnnumcols\relax\fi% +\def\@IEEEeqnarraydblrulerowARG{#1}% +\ifx\@IEEEeqnarraydblrulerowARG\@empty% +\@IEEEeqnarrayrulerow[\arrayrulewidth]% +\else% +\@IEEEeqnarrayrulerow[#1]% +\fi% +} + +% draws a double rule by using a single rule row, a separator (cutting) row, and then +% another single rule row +% first optional argument determines the rule thicknesses, \arrayrulewidth is the default +% second optional argument determines the rule spacing, \doublerulesep is the default +% usage: \IEEEeqnarraydblrulerow[rule line thickness][rule spacing] +\def\IEEEeqnarraydblrulerowcut{\multispan{\@IEEEeqnnumcols}\relax% span all the cols +% advance column counter only if the IEEEeqnarray environment wants it +\if@advanceIEEEeqncolcnt\global\advance\@IEEEeqncolcnt by \@IEEEeqnnumcols\relax\fi% +\@ifnextchar[{\@IEEEeqnarraydblrulerowcut}{\@IEEEeqnarraydblrulerowcut[\arrayrulewidth]}} +\def\@IEEEeqnarraydblrulerowcut[#1]{\relax\@ifnextchar[{\@@IEEEeqnarraydblrulerowcut[#1]}% +{\@@IEEEeqnarraydblrulerowcut[#1][\doublerulesep]}} +\def\@@IEEEeqnarraydblrulerowcut[#1][#2]{\def\@IEEEeqnarraydblrulerowARG{#1}% +% we allow the user to say \IEEEeqnarraydblrulerow[][] +\ifx\@IEEEeqnarraydblrulerowARG\@empty% +\@IEEEeqnarrayrulerow[\arrayrulewidth]% +\else% +\@IEEEeqnarrayrulerow[#1]% +\fi% +\def\@IEEEeqnarraydblrulerowARG{#2}% +\ifx\@IEEEeqnarraydblrulerowARG\@empty% +\\\IEEEeqnarrayseprowcut[\doublerulesep][\relax]% +\else% +\\\IEEEeqnarrayseprowcut[#2][\relax]% +\fi% +\\\multispan{\@IEEEeqnnumcols}% +% advance column counter only if the IEEEeqnarray environment wants it +\if@advanceIEEEeqncolcnt\global\advance\@IEEEeqncolcnt by \@IEEEeqnnumcols\relax\fi% +\def\@IEEEeqnarraydblrulerowARG{#1}% +\ifx\@IEEEeqnarraydblrulerowARG\@empty% +\@IEEEeqnarrayrulerow[\arrayrulewidth]% +\else% +\@IEEEeqnarrayrulerow[#1]% +\fi% +} + + + +% inserts a full row's worth of &'s +% relies on \@IEEEeqnnumcols to provide the correct number of columns +% uses \@IEEEtrantmptoksA, \count0 as scratch registers +\def\@IEEEeqnarrayhoptolastcolumn{\@IEEEtrantmptoksA={}\count0=1\relax% +\loop% add cols if the user did not use them all +\ifnum\count0<\@IEEEeqnnumcols\relax% +\@IEEEappendtoksA{&}% +\advance\count0 by 1\relax% update the col count +\repeat% +\the\@IEEEtrantmptoksA%execute the &'s +} + + + +\newif\if@IEEEeqnarrayISinner % flag to indicate if we are within the lines +\@IEEEeqnarrayISinnerfalse % of an IEEEeqnarray - after the IEEEeqnarraydecl + +\edef\@IEEEeqnarrayTHEstrutheight{0pt} % height and depth of IEEEeqnarray struts +\edef\@IEEEeqnarrayTHEstrutdepth{0pt} + +\edef\@IEEEeqnarrayTHEmasterstrutheight{0pt} % default height and depth of +\edef\@IEEEeqnarrayTHEmasterstrutdepth{0pt} % struts within an IEEEeqnarray + +\edef\@IEEEeqnarrayTHEmasterstrutHSAVE{0pt} % saved master strut height +\edef\@IEEEeqnarrayTHEmasterstrutDSAVE{0pt} % and depth + +\newif\if@IEEEeqnarrayusemasterstrut % flag to indicate that the master strut value +\@IEEEeqnarrayusemasterstruttrue % is to be used + + + +% saves the strut height and depth of the master strut +\def\@IEEEeqnarraymasterstrutsave{\relax% +\expandafter\skip0=\@IEEEeqnarrayTHEmasterstrutheight\relax% +\expandafter\skip2=\@IEEEeqnarrayTHEmasterstrutdepth\relax% +% remove stretchability +\dimen0\skip0\relax% +\dimen2\skip2\relax% +% save values +\edef\@IEEEeqnarrayTHEmasterstrutHSAVE{\the\dimen0}% +\edef\@IEEEeqnarrayTHEmasterstrutDSAVE{\the\dimen2}} + +% restores the strut height and depth of the master strut +\def\@IEEEeqnarraymasterstrutrestore{\relax% +\expandafter\skip0=\@IEEEeqnarrayTHEmasterstrutHSAVE\relax% +\expandafter\skip2=\@IEEEeqnarrayTHEmasterstrutDSAVE\relax% +% remove stretchability +\dimen0\skip0\relax% +\dimen2\skip2\relax% +% restore values +\edef\@IEEEeqnarrayTHEmasterstrutheight{\the\dimen0}% +\edef\@IEEEeqnarrayTHEmasterstrutdepth{\the\dimen2}} + + +% globally restores the strut height and depth to the +% master values and sets the master strut flag to true +\def\@IEEEeqnarraystrutreset{\relax% +\expandafter\skip0=\@IEEEeqnarrayTHEmasterstrutheight\relax% +\expandafter\skip2=\@IEEEeqnarrayTHEmasterstrutdepth\relax% +% remove stretchability +\dimen0\skip0\relax% +\dimen2\skip2\relax% +% restore values +\xdef\@IEEEeqnarrayTHEstrutheight{\the\dimen0}% +\xdef\@IEEEeqnarrayTHEstrutdepth{\the\dimen2}% +\global\@IEEEeqnarrayusemasterstruttrue} + + +% if the master strut is not to be used, make the current +% values of \@IEEEeqnarrayTHEstrutheight, \@IEEEeqnarrayTHEstrutdepth +% and the use master strut flag, global +% this allows user strut commands issued in the last column to be carried +% into the isolation/strut column +\def\@IEEEeqnarrayglobalizestrutstatus{\relax% +\if@IEEEeqnarrayusemasterstrut\else% +\xdef\@IEEEeqnarrayTHEstrutheight{\@IEEEeqnarrayTHEstrutheight}% +\xdef\@IEEEeqnarrayTHEstrutdepth{\@IEEEeqnarrayTHEstrutdepth}% +\global\@IEEEeqnarrayusemasterstrutfalse% +\fi} + + + +% usage: \IEEEeqnarraystrutsize{height}{depth}[font size commands] +% If called outside the lines of an IEEEeqnarray, sets the height +% and depth of both the master and local struts. If called inside +% an IEEEeqnarray line, sets the height and depth of the local strut +% only and sets the flag to indicate the use of the local strut +% values. If the height or depth is left blank, 0.7\normalbaselineskip +% and 0.3\normalbaselineskip will be used, respectively. +% The optional argument can be used to evaluate the lengths under +% a different font size and styles. If none is specified, the current +% font is used. +% uses scratch registers \skip0, \skip2, \skip3, \dimen0, \dimen2 +\def\IEEEeqnarraystrutsize#1#2{\relax\@ifnextchar[{\@IEEEeqnarraystrutsize{#1}{#2}}{\@IEEEeqnarraystrutsize{#1}{#2}[\relax]}} +\def\@IEEEeqnarraystrutsize#1#2[#3]{\def\@IEEEeqnarraystrutsizeARG{#1}% +\ifx\@IEEEeqnarraystrutsizeARG\@empty% +{\setbox0=\hbox{#3\relax\global\skip3=0.7\normalbaselineskip}}% +\skip0=\skip3\relax% +\else% arg one present +{\setbox0=\hbox{#3\relax\global\skip3=#1\relax}}% +\skip0=\skip3\relax% +\fi% if null arg +\def\@IEEEeqnarraystrutsizeARG{#2}% +\ifx\@IEEEeqnarraystrutsizeARG\@empty% +{\setbox0=\hbox{#3\relax\global\skip3=0.3\normalbaselineskip}}% +\skip2=\skip3\relax% +\else% arg two present +{\setbox0=\hbox{#3\relax\global\skip3=#2\relax}}% +\skip2=\skip3\relax% +\fi% if null arg +% remove stretchability, just to be safe +\dimen0\skip0\relax% +\dimen2\skip2\relax% +% dimen0 = height, dimen2 = depth +\if@IEEEeqnarrayISinner% inner does not touch master strut size +\edef\@IEEEeqnarrayTHEstrutheight{\the\dimen0}% +\edef\@IEEEeqnarrayTHEstrutdepth{\the\dimen2}% +\@IEEEeqnarrayusemasterstrutfalse% do not use master +\else% outer, have to set master strut too +\edef\@IEEEeqnarrayTHEmasterstrutheight{\the\dimen0}% +\edef\@IEEEeqnarrayTHEmasterstrutdepth{\the\dimen2}% +\edef\@IEEEeqnarrayTHEstrutheight{\the\dimen0}% +\edef\@IEEEeqnarrayTHEstrutdepth{\the\dimen2}% +\@IEEEeqnarrayusemasterstruttrue% use master strut +\fi} + + +% usage: \IEEEeqnarraystrutsizeadd{added height}{added depth}[font size commands] +% If called outside the lines of an IEEEeqnarray, adds the given height +% and depth to both the master and local struts. +% If called inside an IEEEeqnarray line, adds the given height and depth +% to the local strut only and sets the flag to indicate the use +% of the local strut values. +% In both cases, if a height or depth is left blank, 0pt is used instead. +% The optional argument can be used to evaluate the lengths under +% a different font size and styles. If none is specified, the current +% font is used. +% uses scratch registers \skip0, \skip2, \skip3, \dimen0, \dimen2 +\def\IEEEeqnarraystrutsizeadd#1#2{\relax\@ifnextchar[{\@IEEEeqnarraystrutsizeadd{#1}{#2}}{\@IEEEeqnarraystrutsizeadd{#1}{#2}[\relax]}} +\def\@IEEEeqnarraystrutsizeadd#1#2[#3]{\def\@IEEEeqnarraystrutsizearg{#1}% +\ifx\@IEEEeqnarraystrutsizearg\@empty% +\skip0=0pt\relax% +\else% arg one present +{\setbox0=\hbox{#3\relax\global\skip3=#1}}% +\skip0=\skip3\relax% +\fi% if null arg +\def\@IEEEeqnarraystrutsizearg{#2}% +\ifx\@IEEEeqnarraystrutsizearg\@empty% +\skip2=0pt\relax% +\else% arg two present +{\setbox0=\hbox{#3\relax\global\skip3=#2}}% +\skip2=\skip3\relax% +\fi% if null arg +% remove stretchability, just to be safe +\dimen0\skip0\relax% +\dimen2\skip2\relax% +% dimen0 = height, dimen2 = depth +\if@IEEEeqnarrayISinner% inner does not touch master strut size +% get local strut size +\expandafter\skip0=\@IEEEeqnarrayTHEstrutheight\relax% +\expandafter\skip2=\@IEEEeqnarrayTHEstrutdepth\relax% +% add it to the user supplied values +\advance\dimen0 by \skip0\relax% +\advance\dimen2 by \skip2\relax% +% update the local strut size +\edef\@IEEEeqnarrayTHEstrutheight{\the\dimen0}% +\edef\@IEEEeqnarrayTHEstrutdepth{\the\dimen2}% +\@IEEEeqnarrayusemasterstrutfalse% do not use master +\else% outer, have to set master strut too +% get master strut size +\expandafter\skip0=\@IEEEeqnarrayTHEmasterstrutheight\relax% +\expandafter\skip2=\@IEEEeqnarrayTHEmasterstrutdepth\relax% +% add it to the user supplied values +\advance\dimen0 by \skip0\relax% +\advance\dimen2 by \skip2\relax% +% update the local and master strut sizes +\edef\@IEEEeqnarrayTHEmasterstrutheight{\the\dimen0}% +\edef\@IEEEeqnarrayTHEmasterstrutdepth{\the\dimen2}% +\edef\@IEEEeqnarrayTHEstrutheight{\the\dimen0}% +\edef\@IEEEeqnarrayTHEstrutdepth{\the\dimen2}% +\@IEEEeqnarrayusemasterstruttrue% use master strut +\fi} + + +% allow user a way to see the struts +\newif\ifIEEEvisiblestruts +\IEEEvisiblestrutsfalse + +% inserts an invisible strut using the master or local strut values +% uses scratch registers \skip0, \skip2, \dimen0, \dimen2 +\def\@IEEEeqnarrayinsertstrut{\relax% +\if@IEEEeqnarrayusemasterstrut +% get master strut size +\expandafter\skip0=\@IEEEeqnarrayTHEmasterstrutheight\relax% +\expandafter\skip2=\@IEEEeqnarrayTHEmasterstrutdepth\relax% +\else% +% get local strut size +\expandafter\skip0=\@IEEEeqnarrayTHEstrutheight\relax% +\expandafter\skip2=\@IEEEeqnarrayTHEstrutdepth\relax% +\fi% +% remove stretchability, probably not needed +\dimen0\skip0\relax% +\dimen2\skip2\relax% +% dimen0 = height, dimen2 = depth +% allow user to see struts if desired +\ifIEEEvisiblestruts% +\vrule width0.2pt height\dimen0 depth\dimen2\relax% +\else% +\vrule width0pt height\dimen0 depth\dimen2\relax\fi} + + +% creates an invisible strut, useable even outside \IEEEeqnarray +% if \IEEEvisiblestrutstrue, the strut will be visible and 0.2pt wide. +% usage: \IEEEstrut[height][depth][font size commands] +% default is \IEEEstrut[0.7\normalbaselineskip][0.3\normalbaselineskip][\relax] +% blank arguments inherit the default values +% uses \dimen0, \dimen2, \skip0, \skip2 +\def\IEEEstrut{\relax\@ifnextchar[{\@IEEEstrut}{\@IEEEstrut[0.7\normalbaselineskip]}} +\def\@IEEEstrut[#1]{\relax\@ifnextchar[{\@@IEEEstrut[#1]}{\@@IEEEstrut[#1][0.3\normalbaselineskip]}} +\def\@@IEEEstrut[#1][#2]{\relax\@ifnextchar[{\@@@IEEEstrut[#1][#2]}{\@@@IEEEstrut[#1][#2][\relax]}} +\def\@@@IEEEstrut[#1][#2][#3]{\mbox{#3\relax% +\def\@IEEEstrutARG{#1}% +\ifx\@IEEEstrutARG\@empty% +\skip0=0.7\normalbaselineskip\relax% +\else% +\skip0=#1\relax% +\fi% +\def\@IEEEstrutARG{#2}% +\ifx\@IEEEstrutARG\@empty% +\skip2=0.3\normalbaselineskip\relax% +\else% +\skip2=#2\relax% +\fi% +% remove stretchability, probably not needed +\dimen0\skip0\relax% +\dimen2\skip2\relax% +\ifIEEEvisiblestruts% +\vrule width0.2pt height\dimen0 depth\dimen2\relax% +\else% +\vrule width0.0pt height\dimen0 depth\dimen2\relax\fi}} + + +% enables strut mode by setting a default strut size and then zeroing the +% \baselineskip, \lineskip, \lineskiplimit and \jot +\def\IEEEeqnarraystrutmode{\IEEEeqnarraystrutsize{0.7\normalbaselineskip}{0.3\normalbaselineskip}[\relax]% +\baselineskip=0pt\lineskip=0pt\lineskiplimit=0pt\jot=0pt} + + +% equation and subequation forms to use to setup hyperref's \@currentHref +\def\@IEEEtheHrefequation{equation.\theHequation} +\def\@IEEEtheHrefsubequation{equation.\theHequation\alph{IEEEsubequation}} + + +\def\IEEEeqnarray{\@IEEEeqnumpersisttrue\@IEEEsubeqnumpersistfalse\@IEEEeqnarray} +\def\endIEEEeqnarray{\end@IEEEeqnarray} + +\@namedef{IEEEeqnarray*}{\@IEEEeqnumpersistfalse\@IEEEsubeqnumpersistfalse\@IEEEeqnarray} +\@namedef{endIEEEeqnarray*}{\end@IEEEeqnarray} + + +% \IEEEeqnarray is an enhanced \eqnarray. +% The star form defaults to not putting equation numbers at the end of each row. +% usage: \IEEEeqnarray[decl]{cols} +\def\@IEEEeqnarray{\relax\@ifnextchar[{\@@IEEEeqnarray}{\@@IEEEeqnarray[\relax]}} +% We have to be careful here to normalize catcodes just before acquiring the +% cols as that specification may contain punctuation which could be subject +% to document catcode changes. +\def\@@IEEEeqnarray[#1]{\begingroup\IEEEnormalcatcodes\@@@IEEEeqnarray[#1]} +\def\@@@IEEEeqnarray[#1]#2{\endgroup + % default to showing the equation number or not based on whether or not + % the star form was involked + \if@IEEEeqnumpersist\global\@eqnswtrue + \else% not the star form + \global\@eqnswfalse + \fi% if star form + % provide a basic hyperref \theHequation if this has not already been setup (hyperref not loaded, or no section counter) + \@ifundefined{theHequation}{\def\theHequation{\arabic{equation}}}{}\relax + % provide dummy hyperref commands in case hyperref is not loaded + \providecommand{\Hy@raisedlink}[1]{}\relax + \providecommand{\hyper@anchorstart}[1]{}\relax + \providecommand{\hyper@anchorend}{}\relax + \providecommand{\@currentHref}{}\relax + \@IEEEeqnumpreadvfalse% reset eqnpreadv flag + \@IEEEsubeqnumpreadvfalse% reset subeqnpreadv flag + \@IEEEeqnarrayISinnerfalse% not yet within the lines of the halign + \@IEEEeqnarraystrutsize{0pt}{0pt}[\relax]% turn off struts by default + \@IEEEeqnarrayusemasterstruttrue% use master strut till user asks otherwise + \IEEEvisiblestrutsfalse% diagnostic mode defaults to off + % no extra space unless the user specifically requests it + \lineskip=0pt\relax + \lineskiplimit=0pt\relax + \baselineskip=\normalbaselineskip\relax% + \jot=\IEEEnormaljot\relax% + \mathsurround\z@\relax% no extra spacing around math + \@advanceIEEEeqncolcnttrue% advance the col counter for each col the user uses, + % used in \IEEEeqnarraymulticol and in the preamble build + %V1.8 Here we preadvance to the next equation number. + % If the user later wants a continued subequation, we can roll back. + \global\@IEEEsubeqnnumrollback=\c@IEEEsubequation% + \stepcounter{equation}\@IEEEeqnumpreadvtrue% advance equation counter before first line + \setcounter{IEEEsubequation}{0}% no subequation yet + \let\@IEEEcurrentlabelsave\@currentlabel% save current label as we later change it globally + \let\@IEEEcurrentHrefsave\@currentHref% save current href label as we later change it globally + \def\@currentlabel{\p@equation\theequation}% redefine the ref label + \def\@currentHref{\@IEEEtheHrefequation}% setup hyperref label + \IEEEeqnarraydecl\relax% allow a way for the user to make global overrides + #1\relax% allow user to override defaults + \let\\\@IEEEeqnarraycr% replace newline with one that can put in eqn. numbers + \global\@IEEEeqncolcnt\z@% col. count = 0 for first line + \@IEEEbuildpreamble{#2}\relax% build the preamble and put it into \@IEEEtrantmptoksA + % put in the column for the equation number + \ifnum\@IEEEeqnnumcols>0\relax\@IEEEappendtoksA{&}\fi% col separator for those after the first + \toks0={##}% + % advance the \@IEEEeqncolcnt for the isolation col, this helps with error checking + \@IEEEappendtoksA{\global\advance\@IEEEeqncolcnt by 1\relax}% + % add the isolation column + \@IEEEappendtoksA{\tabskip\z@skip\bgroup\the\toks0\egroup}% + % advance the \@IEEEeqncolcnt for the equation number col, this helps with error checking + \@IEEEappendtoksA{&\global\advance\@IEEEeqncolcnt by 1\relax}% + % add the equation number col to the preamble + \@IEEEappendtoksA{\tabskip\z@skip\hb@xt@\z@\bgroup\hss\the\toks0\egroup}% + % note \@IEEEeqnnumcols does not count the equation col or isolation col + % set the starting tabskip glue as determined by the preamble build + \tabskip=\@IEEEBPstartglue\relax + % begin the display alignment + \@IEEEeqnarrayISinnertrue% commands are now within the lines + $$\everycr{}\halign to\displaywidth\bgroup + % "exspand" the preamble + \span\the\@IEEEtrantmptoksA\cr} + +% enter isolation/strut column (or the next column if the user did not use +% every column), record the strut status, complete the columns, do the strut if needed, +% restore counters (to backout any equation setup for a next line that was never used) +% to their correct values and exit +\def\end@IEEEeqnarray{\@IEEEeqnarrayglobalizestrutstatus&\@@IEEEeqnarraycr\egroup +\if@IEEEsubeqnumpreadv\global\advance\c@IEEEsubequation\m@ne\fi +\if@IEEEeqnumpreadv\global\advance\c@equation\m@ne\global\c@IEEEsubequation=\@IEEEsubeqnnumrollback\fi +\global\let\@currentlabel\@IEEEcurrentlabelsave% restore current label +\global\let\@currentHref\@IEEEcurrentHrefsave% restore current href label +$$\@ignoretrue} + + +% IEEEeqnarray uses a modifed \\ instead of the plain \cr to +% end rows. This allows for things like \\*[vskip amount] +% These "cr" macros are modified versions of those for LaTeX2e's eqnarray +% the {\ifnum0=`} braces must be kept away from the last column to avoid +% altering spacing of its math, so we use & to advance to the next column +% as there is an isolation/strut column after the user's columns +\def\@IEEEeqnarraycr{\@IEEEeqnarrayglobalizestrutstatus&% save strut status and advance to next column + {\ifnum0=`}\fi + \@ifstar{% + \global\@eqpen\@M\@IEEEeqnarrayYCR + }{% + \global\@eqpen\interdisplaylinepenalty \@IEEEeqnarrayYCR + }% +} + +\def\@IEEEeqnarrayYCR{\@testopt\@IEEEeqnarrayXCR\z@skip} + +\def\@IEEEeqnarrayXCR[#1]{% + \ifnum0=`{\fi}% + \@@IEEEeqnarraycr + \noalign{\penalty\@eqpen\vskip\jot\vskip #1\relax}}% + +\def\@@IEEEeqnarraycr{\@IEEEtrantmptoksA={}% clear token register + \advance\@IEEEeqncolcnt by -1\relax% adjust col count because of the isolation column + \ifnum\@IEEEeqncolcnt>\@IEEEeqnnumcols\relax + \@IEEEclspkgerror{Too many columns within the IEEEeqnarray\MessageBreak + environment}% + {Use fewer \string &'s or put more columns in the IEEEeqnarray column\MessageBreak + specifications.}\relax% + \else + \loop% add cols if the user did not use them all + \ifnum\@IEEEeqncolcnt<\@IEEEeqnnumcols\relax + \@IEEEappendtoksA{&}% + \advance\@IEEEeqncolcnt by 1\relax% update the col count + \repeat + % this number of &'s will take us the the isolation column + \fi + % execute the &'s + \the\@IEEEtrantmptoksA% + % handle the strut/isolation column + \@IEEEeqnarrayinsertstrut% do the strut if needed + \@IEEEeqnarraystrutreset% reset the strut system for next line or IEEEeqnarray + &% and enter the equation number column + \if@eqnsw% only if we display something + \Hy@raisedlink{\hyper@anchorstart{\@currentHref}}% start a hyperref anchor + \global\@IEEEeqnumpreadvfalse\relax% displaying an equation number means + \global\@IEEEsubeqnumpreadvfalse\relax% the equation counters point to valid equations + % V1.8 Here we setup the counters, currentlabel and status for what would be the *next* + % equation line as would be the case under the current settings. However, there are two problems. + % One problem is that there might not ever be a next line. The second problem is that the user + % may later alter the meaning of a line with commands such as \IEEEyessubnumber. So, to handle + % these cases we have to record the current values of the (sub)equation counters and revert back + % to them if the next line is changed or never comes. The \if@IEEEeqnumpreadv, \if@IEEEsubeqnumpreadv + % and \@IEEEsubeqnnumrollback stuff tracks this. + % The logic to handle all this is surprisingly complex, but a nice feature of the approach here is + % that the equation counters and labels remain valid for what the line would be unless a + % \IEEEyessubnumber et al. later changes it. So, any hyperref links are always correct. + \ifnum\c@IEEEsubequation>0\relax% handle subequation + \theIEEEsubequationdis\relax + \if@IEEEsubeqnumpersist% setup for default type of next line + \stepcounter{IEEEsubequation}\global\@IEEEsubeqnumpreadvtrue\relax + \gdef\@currentlabel{\p@IEEEsubequation\theIEEEsubequation}\relax + \gdef\@currentHref{\@IEEEtheHrefsubequation}% setup hyperref label + \else + % if no subeqnum persist, go ahead and setup for a new equation number + \global\@IEEEsubeqnnumrollback=\c@IEEEsubequation + \stepcounter{equation}\global\@IEEEeqnumpreadvtrue\relax + \setcounter{IEEEsubequation}{0}\gdef\@currentlabel{\p@equation\theequation}\relax + \gdef\@currentHref{\@IEEEtheHrefequation}% setup hyperref label + \fi + \else% display a standard equation number + \theequationdis\relax + \setcounter{IEEEsubequation}{0}\relax% not really needed + \if@IEEEsubeqnumpersist% setup for default type of next line + % subequations that follow plain equations carry the same equation number e.g, 5, 5a rather than 5, 6a + \stepcounter{IEEEsubequation}\global\@IEEEsubeqnumpreadvtrue\relax + \gdef\@currentlabel{\p@IEEEsubequation\theIEEEsubequation}\relax + \gdef\@currentHref{\@IEEEtheHrefsubequation}% setup hyperref label + \else + % if no subeqnum persist, go ahead and setup for a new equation number + \global\@IEEEsubeqnnumrollback=\c@IEEEsubequation + \stepcounter{equation}\global\@IEEEeqnumpreadvtrue\relax + \setcounter{IEEEsubequation}{0}\gdef\@currentlabel{\p@equation\theequation}\relax + \gdef\@currentHref{\@IEEEtheHrefequation}% setup hyperref label + \fi + \fi% + \Hy@raisedlink{\hyper@anchorend}% end hyperref anchor + \fi% fi only if we display something + % reset the flags to indicate the default preferences of the display of equation numbers + \if@IEEEeqnumpersist\global\@eqnswtrue\else\global\@eqnswfalse\fi + \if@IEEEsubeqnumpersist\global\@eqnswtrue\fi% ditto for the subequation flag + % reset the number of columns the user actually used + \global\@IEEEeqncolcnt\z@\relax + % the real end of the line + \cr} + + + + + +% \IEEEeqnarraybox is like \IEEEeqnarray except the box form puts everything +% inside a vtop, vbox, or vcenter box depending on the letter in the second +% optional argument (t,b,c). Vbox is the default. Unlike \IEEEeqnarray, +% equation numbers are not displayed and \IEEEeqnarraybox can be nested. +% \IEEEeqnarrayboxm is for math mode (like \array) and does not put the vbox +% within an hbox. +% \IEEEeqnarrayboxt is for text mode (like \tabular) and puts the vbox within +% a \hbox{$ $} construct. +% \IEEEeqnarraybox will auto detect whether to use \IEEEeqnarrayboxm or +% \IEEEeqnarrayboxt depending on the math mode. +% The third optional argument specifies the width this box is to be set to - +% natural width is the default. +% The * forms do not add \jot line spacing +% usage: \IEEEeqnarraybox[decl][pos][width]{cols} +\def\IEEEeqnarrayboxm{\@IEEEeqnarrayboxnojotfalse\@IEEEeqnarrayboxHBOXSWfalse\@IEEEeqnarraybox} +\def\endIEEEeqnarrayboxm{\end@IEEEeqnarraybox} +\@namedef{IEEEeqnarrayboxm*}{\@IEEEeqnarrayboxnojottrue\@IEEEeqnarrayboxHBOXSWfalse\@IEEEeqnarraybox} +\@namedef{endIEEEeqnarrayboxm*}{\end@IEEEeqnarraybox} + +\def\IEEEeqnarrayboxt{\@IEEEeqnarrayboxnojotfalse\@IEEEeqnarrayboxHBOXSWtrue\@IEEEeqnarraybox} +\def\endIEEEeqnarrayboxt{\end@IEEEeqnarraybox} +\@namedef{IEEEeqnarrayboxt*}{\@IEEEeqnarrayboxnojottrue\@IEEEeqnarrayboxHBOXSWtrue\@IEEEeqnarraybox} +\@namedef{endIEEEeqnarrayboxt*}{\end@IEEEeqnarraybox} + +\def\IEEEeqnarraybox{\@IEEEeqnarrayboxnojotfalse\ifmmode\@IEEEeqnarrayboxHBOXSWfalse\else\@IEEEeqnarrayboxHBOXSWtrue\fi% +\@IEEEeqnarraybox} +\def\endIEEEeqnarraybox{\end@IEEEeqnarraybox} + +\@namedef{IEEEeqnarraybox*}{\@IEEEeqnarrayboxnojottrue\ifmmode\@IEEEeqnarrayboxHBOXSWfalse\else\@IEEEeqnarrayboxHBOXSWtrue\fi% +\@IEEEeqnarraybox} +\@namedef{endIEEEeqnarraybox*}{\end@IEEEeqnarraybox} + +% flag to indicate if the \IEEEeqnarraybox needs to put things into an hbox{$ $} +% for \vcenter in non-math mode +\newif\if@IEEEeqnarrayboxHBOXSW% +\@IEEEeqnarrayboxHBOXSWfalse + +\def\@IEEEeqnarraybox{\relax\@ifnextchar[{\@@IEEEeqnarraybox}{\@@IEEEeqnarraybox[\relax]}} +% We have to be careful here to normalize catcodes just before acquiring the +% cols as that specification may contain punctuation which could be subject +% to document catcode changes. +\def\@@IEEEeqnarraybox[#1]{\relax\begingroup\IEEEnormalcatcodes\@ifnextchar[{\@@@IEEEeqnarraybox[#1]}{\@@@IEEEeqnarraybox[#1][b]}} +\def\@@@IEEEeqnarraybox[#1][#2]{\relax\@ifnextchar[{\@@@@IEEEeqnarraybox[#1][#2]}{\@@@@IEEEeqnarraybox[#1][#2][\relax]}} + +% #1 = decl; #2 = t,b,c; #3 = width, #4 = col specs +\def\@@@@IEEEeqnarraybox[#1][#2][#3]#4{\endgroup\@IEEEeqnarrayISinnerfalse % not yet within the lines of the halign + \@IEEEeqnarraymasterstrutsave% save current master strut values + \@IEEEeqnarraystrutsize{0pt}{0pt}[\relax]% turn off struts by default + \@IEEEeqnarrayusemasterstruttrue% use master strut till user asks otherwise + \IEEEvisiblestrutsfalse% diagnostic mode defaults to off + % no extra space unless the user specifically requests it + \lineskip=0pt\relax% + \lineskiplimit=0pt\relax% + \baselineskip=\normalbaselineskip\relax% + \jot=\IEEEnormaljot\relax% + \mathsurround\z@\relax% no extra spacing around math + % the default end glues are zero for an \IEEEeqnarraybox + \edef\@IEEEeqnarraycolSEPdefaultstart{\@IEEEeqnarraycolSEPzero}% default start glue + \edef\@IEEEeqnarraycolSEPdefaultend{\@IEEEeqnarraycolSEPzero}% default end glue + \edef\@IEEEeqnarraycolSEPdefaultmid{\@IEEEeqnarraycolSEPzero}% default inter-column glue + \@advanceIEEEeqncolcntfalse% do not advance the col counter for each col the user uses, + % used in \IEEEeqnarraymulticol and in the preamble build + \IEEEeqnarrayboxdecl\relax% allow a way for the user to make global overrides + #1\relax% allow user to override defaults + \let\\\@IEEEeqnarrayboxcr% replace newline with one that allows optional spacing + \@IEEEbuildpreamble{#4}\relax% build the preamble and put it into \@IEEEtrantmptoksA + % add an isolation column to the preamble to stop \\'s {} from getting into the last col + \ifnum\@IEEEeqnnumcols>0\relax\@IEEEappendtoksA{&}\fi% col separator for those after the first + \toks0={##}% + % add the isolation column to the preamble + \@IEEEappendtoksA{\tabskip\z@skip\bgroup\the\toks0\egroup}% + % set the starting tabskip glue as determined by the preamble build + \tabskip=\@IEEEBPstartglue\relax + % begin the alignment + \everycr{}% + % use only the very first token to determine the positioning + \@IEEEextracttoken{#2}\relax + \ifx\@IEEEextractedtokensdiscarded\@empty\else + \typeout{** WARNING: IEEEeqnarraybox position specifiers after the first in `\@IEEEextracttokenarg' ignored (line \the\inputlineno).}\relax + \fi + % \@IEEEextractedtoken has the first token, the rest are ignored + % if we need to put things into and hbox and go into math mode, do so now + \if@IEEEeqnarrayboxHBOXSW \leavevmode \hbox \bgroup $\fi% + % use the appropriate vbox type + \if\@IEEEextractedtoken t\relax\vtop\else\if\@IEEEextractedtoken c\relax% + \vcenter\else\vbox\fi\fi\bgroup% + \@IEEEeqnarrayISinnertrue% commands are now within the lines + \ifx#3\relax\halign\else\halign to #3\relax\fi% + \bgroup + % "exspand" the preamble + \span\the\@IEEEtrantmptoksA\cr} + +% carry strut status and enter the isolation/strut column, +% exit from math mode if needed, and exit +\def\end@IEEEeqnarraybox{\@IEEEeqnarrayglobalizestrutstatus% carry strut status +&% enter isolation/strut column +\@IEEEeqnarrayinsertstrut% do strut if needed +\@IEEEeqnarraymasterstrutrestore% restore the previous master strut values +% reset the strut system for next IEEEeqnarray +% (sets local strut values back to previous master strut values) +\@IEEEeqnarraystrutreset% +% ensure last line, exit from halign, close vbox +\crcr\egroup\egroup% +% exit from math mode and close hbox if needed +\if@IEEEeqnarrayboxHBOXSW $\egroup\fi} + + + +% IEEEeqnarraybox uses a modifed \\ instead of the plain \cr to +% end rows. This allows for things like \\[vskip amount] +% This "cr" macros are modified versions those for LaTeX2e's eqnarray +% For IEEEeqnarraybox, \\* is the same as \\ +% the {\ifnum0=`} braces must be kept away from the last column to avoid +% altering spacing of its math, so we use & to advance to the isolation/strut column +% carry strut status into isolation/strut column +\def\@IEEEeqnarrayboxcr{\@IEEEeqnarrayglobalizestrutstatus% carry strut status +&% enter isolation/strut column +\@IEEEeqnarrayinsertstrut% do strut if needed +% reset the strut system for next line or IEEEeqnarray +\@IEEEeqnarraystrutreset% +{\ifnum0=`}\fi% +\@ifstar{\@IEEEeqnarrayboxYCR}{\@IEEEeqnarrayboxYCR}} + +% test and setup the optional argument to \\[] +\def\@IEEEeqnarrayboxYCR{\@testopt\@IEEEeqnarrayboxXCR\z@skip} + +% IEEEeqnarraybox does not automatically increase line spacing by \jot +\def\@IEEEeqnarrayboxXCR[#1]{\ifnum0=`{\fi}% +\cr\noalign{\if@IEEEeqnarrayboxnojot\else\vskip\jot\fi\vskip#1\relax}} + + + +% usage: \@IEEEbuildpreamble{column specifiers} +% starts the halign preamble build +% the assembled preamble is put in \@IEEEtrantmptoksA +\def\@IEEEbuildpreamble#1{\@IEEEtrantmptoksA={}% clear token register +\let\@IEEEBPcurtype=u%current column type is not yet known +\let\@IEEEBPprevtype=s%the previous column type was the start +\let\@IEEEBPnexttype=u%next column type is not yet known +% ensure these are valid +\def\@IEEEBPcurglue={0pt plus 0pt minus 0pt}% +\def\@IEEEBPcurcolname{@IEEEdefault}% name of current column definition +% currently acquired numerically referenced glue +% use a name that is easier to remember +\let\@IEEEBPcurnum=\@IEEEtrantmpcountA% +\@IEEEBPcurnum=0% +% tracks number of columns in the preamble +\@IEEEeqnnumcols=0% +% record the default end glues +\edef\@IEEEBPstartglue{\@IEEEeqnarraycolSEPdefaultstart}% +\edef\@IEEEBPendglue{\@IEEEeqnarraycolSEPdefaultend}% +\edef\@IEEEedefMACRO{#1}\relax% fully expand the preamble to support macro containers +% now parse the user's column specifications +% \ignorespaces is used as a delimiter, need at least one trailing \relax because +% \@@IEEEbuildpreamble looks into the future +\expandafter\@@IEEEbuildpreamble\@IEEEedefMACRO\ignorespaces\relax\relax} + + +% usage: \@@IEEEbuildpreamble{current column}{next column} +% parses and builds the halign preamble +\def\@@IEEEbuildpreamble#1#2{\let\@@nextIEEEbuildpreamble=\@@IEEEbuildpreamble% +% use only the very first token to check the end +\@IEEEextracttokengroups{#1}\relax +\ifx\@IEEEextractedfirsttoken\ignorespaces\let\@@nextIEEEbuildpreamble=\@@IEEEfinishpreamble\else% +% identify current and next token type +\@IEEEgetcoltype{#1}{\@IEEEBPcurtype}{1}% current, error on invalid +\@IEEEgetcoltype{#2}{\@IEEEBPnexttype}{0}% next, no error on invalid next +% if curtype is a glue, get the glue def +\if\@IEEEBPcurtype g\@IEEEgetcurglue{#1}{\@IEEEBPcurglue}\fi% +% if curtype is a column, get the column def and set the current column name +\if\@IEEEBPcurtype c\@IEEEgetcurcol{#1}\fi% +% if curtype is a numeral, acquire the user defined glue +\if\@IEEEBPcurtype n\@IEEEprocessNcol{#1}\fi% +% process the acquired glue +\if\@IEEEBPcurtype g\@IEEEprocessGcol\fi% +% process the acquired col +\if\@IEEEBPcurtype c\@IEEEprocessCcol\fi% +% ready prevtype for next col spec. +\let\@IEEEBPprevtype=\@IEEEBPcurtype% +% be sure and put back the future token(s) as a group +\fi\@@nextIEEEbuildpreamble{#2}} + + +% usage: \@@IEEEfinishpreamble{discarded} +% executed just after preamble build is completed +% warn about zero cols, and if prevtype type = u, put in end tabskip glue +% argument is not used +\def\@@IEEEfinishpreamble#1{\ifnum\@IEEEeqnnumcols<1\relax +\@IEEEclspkgerror{No column specifiers declared for IEEEeqnarray}% +{At least one column type must be declared for each IEEEeqnarray.}% +\fi%num cols less than 1 +%if last type undefined, set default end tabskip glue +\if\@IEEEBPprevtype u\@IEEEappendtoksA{\tabskip=\@IEEEBPendglue}\fi} + + +% usage: \@IEEEgetcoltype{col specifier}{\output}{error more} +% Identify and return the column specifier's type code in the given +% \output macro: +% n = number +% g = glue (any other char in catagory 12) +% c = letter +% e = \ignorespaces (end of sequence) +% u = undefined +% error mode: 0 = no error message, 1 = error on invalid char +\def\@IEEEgetcoltype#1#2#3{% +% use only the very first token to determine the type +\@IEEEextracttoken{#1}\relax +% \@IEEEextractedtoken has the first token, the rest are discarded +\let#2=u\relax% assume invalid until know otherwise +\ifx\@IEEEextractedtoken\ignorespaces\let#2=e\else +\ifcat\@IEEEextractedtoken\relax\else% screen out control sequences +\if0\@IEEEextractedtoken\let#2=n\else +\if1\@IEEEextractedtoken\let#2=n\else +\if2\@IEEEextractedtoken\let#2=n\else +\if3\@IEEEextractedtoken\let#2=n\else +\if4\@IEEEextractedtoken\let#2=n\else +\if5\@IEEEextractedtoken\let#2=n\else +\if6\@IEEEextractedtoken\let#2=n\else +\if7\@IEEEextractedtoken\let#2=n\else +\if8\@IEEEextractedtoken\let#2=n\else +\if9\@IEEEextractedtoken\let#2=n\else +\ifcat,\@IEEEextractedtoken\let#2=g\relax +\else\ifcat a\@IEEEextractedtoken\let#2=c\relax\fi\fi\fi\fi\fi\fi\fi\fi\fi\fi\fi\fi\fi\fi +\if#2u\relax +\if0\noexpand#3\relax\else\@IEEEclspkgerror{Invalid character in column specifications}% +{Only letters, numerals and certain other symbols are allowed \MessageBreak +as IEEEeqnarray column specifiers.}\fi\fi} + + +% usage: \@IEEEgetcurcol{col specifier} +% verify the letter referenced column exists +% and return its name in \@IEEEBPcurcolname +% if column specifier is invalid, use the default column @IEEEdefault +\def\@IEEEgetcurcol#1{\expandafter\ifx\csname @IEEEeqnarraycolDEF#1\endcsname\@IEEEeqnarraycolisdefined% +\def\@IEEEBPcurcolname{#1}\else% invalid column name +\@IEEEclspkgerror{Invalid column type "#1" in column specifications.\MessageBreak +Using a default centering column instead}% +{You must define IEEEeqnarray column types before use.}% +\def\@IEEEBPcurcolname{@IEEEdefault}\fi} + + +% usage: \@IEEEgetcurglue{glue specifier}{\output} +% identify the predefined (punctuation) glue value +% and return it in the given output macro +\def\@IEEEgetcurglue#1#2{% +% ! = \! (neg small) -0.16667em (-3/18 em) +% , = \, (small) 0.16667em ( 3/18 em) +% : = \: (med) 0.22222em ( 4/18 em) +% ; = \; (large) 0.27778em ( 5/18 em) +% ' = \quad 1em +% " = \qquad 2em +% . = 0.5\arraycolsep +% / = \arraycolsep +% ? = 2\arraycolsep +% * = 1fil +% + = \@IEEEeqnarraycolSEPcenter +% - = \@IEEEeqnarraycolSEPzero +% Note that all em values are referenced to the math font (textfont2) fontdimen6 +% value for 1em. +% +% use only the very first token to determine the type +\@IEEEextracttoken{#1}\relax +\ifx\@IEEEextractedtokensdiscarded\@empty\else + \typeout{** WARNING: IEEEeqnarray predefined inter-column glue type specifiers after the first in `\@IEEEextracttokenarg' ignored (line \the\inputlineno).}\relax +\fi +% get the math font 1em value +% LaTeX2e's NFSS2 does not preload the fonts, but \IEEEeqnarray needs +% to gain access to the math (\textfont2) font's spacing parameters. +% So we create a bogus box here that uses the math font to ensure +% that \textfont2 is loaded and ready. If this is not done, +% the \textfont2 stuff here may not work. +% Thanks to Bernd Raichle for his 1997 post on this topic. +{\setbox0=\hbox{$\displaystyle\relax$}}% +% fontdimen6 has the width of 1em (a quad). +\@IEEEtrantmpdimenA=\fontdimen6\textfont2\relax% +% identify the glue value based on the first token +% we discard anything after the first +\if!\@IEEEextractedtoken\@IEEEtrantmpdimenA=-0.16667\@IEEEtrantmpdimenA\edef#2{\the\@IEEEtrantmpdimenA}\else +\if,\@IEEEextractedtoken\@IEEEtrantmpdimenA=0.16667\@IEEEtrantmpdimenA\edef#2{\the\@IEEEtrantmpdimenA}\else +\if:\@IEEEextractedtoken\@IEEEtrantmpdimenA=0.22222\@IEEEtrantmpdimenA\edef#2{\the\@IEEEtrantmpdimenA}\else +\if;\@IEEEextractedtoken\@IEEEtrantmpdimenA=0.27778\@IEEEtrantmpdimenA\edef#2{\the\@IEEEtrantmpdimenA}\else +\if'\@IEEEextractedtoken\@IEEEtrantmpdimenA=1\@IEEEtrantmpdimenA\edef#2{\the\@IEEEtrantmpdimenA}\else +\if"\@IEEEextractedtoken\@IEEEtrantmpdimenA=2\@IEEEtrantmpdimenA\edef#2{\the\@IEEEtrantmpdimenA}\else +\if.\@IEEEextractedtoken\@IEEEtrantmpdimenA=0.5\arraycolsep\edef#2{\the\@IEEEtrantmpdimenA}\else +\if/\@IEEEextractedtoken\edef#2{\the\arraycolsep}\else +\if?\@IEEEextractedtoken\@IEEEtrantmpdimenA=2\arraycolsep\edef#2{\the\@IEEEtrantmpdimenA}\else +\if *\@IEEEextractedtoken\edef#2{0pt plus 1fil minus 0pt}\else +\if+\@IEEEextractedtoken\edef#2{\@IEEEeqnarraycolSEPcenter}\else +\if-\@IEEEextractedtoken\edef#2{\@IEEEeqnarraycolSEPzero}\else +\edef#2{\@IEEEeqnarraycolSEPzero}% +\@IEEEclspkgerror{Invalid predefined inter-column glue type "#1" in\MessageBreak +column specifications. Using a default value of\MessageBreak +0pt instead}% +{Only !,:;'"./?*+ and - are valid predefined glue types in the\MessageBreak +IEEEeqnarray column specifications.}\fi\fi\fi\fi\fi\fi\fi\fi\fi\fi\fi\fi} + + +% usage: \@IEEEprocessNcol{digit} +% process a numerical digit from the column specification +% and look up the corresponding user defined glue value +% can transform current type from n to g or a as the user defined glue is acquired +\def\@IEEEprocessNcol#1{\if\@IEEEBPprevtype g% +\@IEEEclspkgerror{Back-to-back inter-column glue specifiers in column\MessageBreak +specifications. Ignoring consecutive glue specifiers\MessageBreak +after the first}% +{You cannot have two or more glue types next to each other\MessageBreak +in the IEEEeqnarray column specifications.}% +\let\@IEEEBPcurtype=a% abort this glue, future digits will be discarded +\@IEEEBPcurnum=0\relax% +\else% if we previously aborted a glue +\if\@IEEEBPprevtype a\@IEEEBPcurnum=0\let\@IEEEBPcurtype=a%maintain digit abortion +\else%acquire this number +% save the previous type before the numerical digits started +\if\@IEEEBPprevtype n\else\let\@IEEEBPprevsavedtype=\@IEEEBPprevtype\fi% +\multiply\@IEEEBPcurnum by 10\relax% +\advance\@IEEEBPcurnum by #1\relax% add in number, \relax is needed to stop TeX's number scan +\if\@IEEEBPnexttype n\else%close acquisition +\expandafter\ifx\csname @IEEEeqnarraycolSEPDEF\expandafter\romannumeral\number\@IEEEBPcurnum\endcsname\@IEEEeqnarraycolisdefined% +\edef\@IEEEBPcurglue{\csname @IEEEeqnarraycolSEP\expandafter\romannumeral\number\@IEEEBPcurnum\endcsname}% +\else%user glue not defined +\@IEEEclspkgerror{Invalid user defined inter-column glue type "\number\@IEEEBPcurnum" in\MessageBreak +column specifications. Using a default value of\MessageBreak +0pt instead}% +{You must define all IEEEeqnarray numerical inter-column glue types via\MessageBreak +\string\IEEEeqnarraydefcolsep \space before they are used in column specifications.}% +\edef\@IEEEBPcurglue{\@IEEEeqnarraycolSEPzero}% +\fi% glue defined or not +\let\@IEEEBPcurtype=g% change the type to reflect the acquired glue +\let\@IEEEBPprevtype=\@IEEEBPprevsavedtype% restore the prev type before this number glue +\@IEEEBPcurnum=0\relax%ready for next acquisition +\fi%close acquisition, get glue +\fi%discard or acquire number +\fi%prevtype glue or not +} + + +% process an acquired glue +% add any acquired column/glue pair to the preamble +\def\@IEEEprocessGcol{\if\@IEEEBPprevtype a\let\@IEEEBPcurtype=a%maintain previous glue abortions +\else +% if this is the start glue, save it, but do nothing else +% as this is not used in the preamble, but before +\if\@IEEEBPprevtype s\edef\@IEEEBPstartglue{\@IEEEBPcurglue}% +\else%not the start glue +\if\@IEEEBPprevtype g%ignore if back to back glues +\@IEEEclspkgerror{Back-to-back inter-column glue specifiers in column\MessageBreak +specifications. Ignoring consecutive glue specifiers\MessageBreak +after the first}% +{You cannot have two or more glue types next to each other\MessageBreak +in the IEEEeqnarray column specifications.}% +\let\@IEEEBPcurtype=a% abort this glue +\else% not a back to back glue +\if\@IEEEBPprevtype c\relax% if the previoustype was a col, add column/glue pair to preamble +\ifnum\@IEEEeqnnumcols>0\relax\@IEEEappendtoksA{&}\fi +\toks0={##}% +% make preamble advance col counter if this environment needs this +\if@advanceIEEEeqncolcnt\@IEEEappendtoksA{\global\advance\@IEEEeqncolcnt by 1\relax}\fi +% insert the column defintion into the preamble, being careful not to expand +% the column definition +\@IEEEappendtoksA{\tabskip=\@IEEEBPcurglue}% +\@IEEEappendNOEXPANDtoksA{\begingroup\csname @IEEEeqnarraycolPRE}% +\@IEEEappendtoksA{\@IEEEBPcurcolname}% +\@IEEEappendNOEXPANDtoksA{\endcsname}% +\@IEEEappendtoksA{\the\toks0}% +\@IEEEappendNOEXPANDtoksA{\relax\relax\relax\relax\relax% +\relax\relax\relax\relax\relax\csname @IEEEeqnarraycolPOST}% +\@IEEEappendtoksA{\@IEEEBPcurcolname}% +\@IEEEappendNOEXPANDtoksA{\endcsname\relax\relax\relax\relax\relax% +\relax\relax\relax\relax\relax\endgroup}% +\advance\@IEEEeqnnumcols by 1\relax%one more column in the preamble +\else% error: non-start glue with no pending column +\@IEEEclspkgerror{Inter-column glue specifier without a prior column\MessageBreak +type in the column specifications. Ignoring this glue\MessageBreak +specifier}% +{Except for the first and last positions, glue can be placed only\MessageBreak +between column types.}% +\let\@IEEEBPcurtype=a% abort this glue +\fi% previous was a column +\fi% back-to-back glues +\fi% is start column glue +\fi% prev type not a +} + + +% process an acquired letter referenced column and, if necessary, add it to the preamble +\def\@IEEEprocessCcol{\if\@IEEEBPnexttype g\else +\if\@IEEEBPnexttype n\else +% we have a column followed by something other than a glue (or numeral glue) +% so we must add this column to the preamble now +\ifnum\@IEEEeqnnumcols>0\relax\@IEEEappendtoksA{&}\fi%col separator for those after the first +\if\@IEEEBPnexttype e\@IEEEappendtoksA{\tabskip=\@IEEEBPendglue\relax}\else%put in end glue +\@IEEEappendtoksA{\tabskip=\@IEEEeqnarraycolSEPdefaultmid\relax}\fi% or default mid glue +\toks0={##}% +% make preamble advance col counter if this environment needs this +\if@advanceIEEEeqncolcnt\@IEEEappendtoksA{\global\advance\@IEEEeqncolcnt by 1\relax}\fi +% insert the column definition into the preamble, being careful not to expand +% the column definition +\@IEEEappendNOEXPANDtoksA{\begingroup\csname @IEEEeqnarraycolPRE}% +\@IEEEappendtoksA{\@IEEEBPcurcolname}% +\@IEEEappendNOEXPANDtoksA{\endcsname}% +\@IEEEappendtoksA{\the\toks0}% +\@IEEEappendNOEXPANDtoksA{\relax\relax\relax\relax\relax% +\relax\relax\relax\relax\relax\csname @IEEEeqnarraycolPOST}% +\@IEEEappendtoksA{\@IEEEBPcurcolname}% +\@IEEEappendNOEXPANDtoksA{\endcsname\relax\relax\relax\relax\relax% +\relax\relax\relax\relax\relax\endgroup}% +\advance\@IEEEeqnnumcols by 1\relax%one more column in the preamble +\fi%next type not numeral +\fi%next type not glue +} + + +%% +%% END OF IEEEeqnarray DEFINITIONS +%% + + + + + +% set up the running headers and footers +% +% header and footer font and size specifications +\def\@IEEEheaderstyle{\normalfont\scriptsize} +\def\@IEEEfooterstyle{\normalfont\scriptsize} +% +% compsoc uses sans-serif headers and footers +\ifCLASSOPTIONcompsoc + \def\@IEEEheaderstyle{\normalfont\sffamily\scriptsize} + \def\@IEEEfooterstyle{\normalfont\sffamily\scriptsize} +\fi + + +% standard page style, ps@headings +\def\ps@headings{% default to standard twoside headers, no footers +% will change later if the mode requires otherwise +\def\@oddhead{\hbox{}\@IEEEheaderstyle\rightmark\hfil\thepage}\relax +\def\@evenhead{\@IEEEheaderstyle\thepage\hfil\leftmark\hbox{}}\relax +\let\@oddfoot\@empty +\let\@evenfoot\@empty +\ifCLASSOPTIONtechnote + % technote twoside + \def\@oddhead{\hbox{}\@IEEEheaderstyle\leftmark\hfil\thepage}\relax + \def\@evenhead{\@IEEEheaderstyle\thepage\hfil\leftmark\hbox{}}\relax +\fi +\ifCLASSOPTIONdraftcls + % draft footers + \def\@oddfoot{\@IEEEfooterstyle\@date\hfil DRAFT}\relax + \def\@evenfoot{\@IEEEfooterstyle DRAFT\hfil\@date}\relax +\fi +% oneside +\if@twoside\else + % standard one side headers + \def\@oddhead{\hbox{}\@IEEEheaderstyle\leftmark\hfil\thepage}\relax + \let\@evenhead\@empty + \ifCLASSOPTIONdraftcls + % oneside draft footers + \def\@oddfoot{\@IEEEfooterstyle\@date\hfil DRAFT}\relax + \let\@evenfoot\@empty + \fi +\fi +% turn off headers for conferences +\ifCLASSOPTIONconference + \let\@oddhead\@empty + \let\@evenhead\@empty +\fi +% turn off footers for draftclsnofoot +\ifCLASSOPTIONdraftclsnofoot + \let\@oddfoot\@empty + \let\@evenfoot\@empty +\fi} + + +% title page style, ps@IEEEtitlepagestyle +\def\ps@IEEEtitlepagestyle{% default title page headers, no footers +\def\@oddhead{\hbox{}\@IEEEheaderstyle\leftmark\hfil\thepage}\relax +\def\@evenhead{\@IEEEheaderstyle\thepage\hfil\leftmark\hbox{}}\relax +\let\@oddfoot\@empty +\let\@evenfoot\@empty +% will change later if the mode requires otherwise +\ifCLASSOPTIONdraftcls + % draft footers + \ifCLASSOPTIONdraftclsnofoot\else + % but only if not draftclsnofoot + \def\@oddfoot{\@IEEEfooterstyle\@date\hfil DRAFT}\relax + \def\@evenfoot{\@IEEEfooterstyle DRAFT\hfil\@date}\relax + \fi +\else + % all nondraft mode footers + \if@IEEEusingpubid + % for title pages that are using a pubid + % do not repeat pubid on the title page if using a peer review cover page + \ifCLASSOPTIONpeerreview\else + % for noncompsoc papers, the pubid uses footnotesize and + % is at the same vertical position as where the last baseline would normally be + \def\@oddfoot{\hbox{}\hss\@IEEEfooterstyle\footnotesize\raisebox{\footskip}[0pt][0pt]{\@IEEEpubid}\hss\hbox{}}\relax + \def\@evenfoot{\hbox{}\hss\@IEEEfooterstyle\footnotesize\raisebox{\footskip}[0pt][0pt]{\@IEEEpubid}\hss\hbox{}}\relax + \ifCLASSOPTIONcompsoc + % for compsoc papers, the pubid is at the same vertical position as the normal footer + \def\@oddfoot{\hbox{}\hss\@IEEEfooterstyle\raisebox{0pt}[0pt][0pt]{\@IEEEpubid}\hss\hbox{}}\relax + \def\@evenfoot{\hbox{}\hss\@IEEEfooterstyle\raisebox{0pt}[0pt][0pt]{\@IEEEpubid}\hss\hbox{}}\relax + \fi + \fi + \fi +\fi +% turn off headers for conferences +\ifCLASSOPTIONconference + \let\@oddhead\@empty + \let\@evenhead\@empty +\fi} + + +% peer review cover page style, ps@IEEEpeerreviewcoverpagestyle +\def\ps@IEEEpeerreviewcoverpagestyle{% default peer review cover no headers, no footers +\let\@oddhead\@empty +\let\@evenhead\@empty +\let\@oddfoot\@empty +\let\@evenfoot\@empty +% will change later if the mode requires otherwise +\ifCLASSOPTIONdraftcls + % draft footers + \ifCLASSOPTIONdraftclsnofoot\else + % but only if not draftclsnofoot + \def\@oddfoot{\@IEEEfooterstyle\@date\hfil DRAFT}\relax + \def\@evenfoot{\@IEEEfooterstyle DRAFT\hfil\@date}\relax + \fi +\else + % all nondraft mode footers + \if@IEEEusingpubid + % for peer review cover pages that are using a pubid + % for noncompsoc papers, the pubid uses footnotesize and + % is at the same vertical position as where the last baseline would normally be + \def\@oddfoot{\hbox{}\hss\@IEEEfooterstyle\footnotesize\raisebox{\footskip}[0pt][0pt]{\@IEEEpubid}\hss\hbox{}}\relax + \def\@evenfoot{\hbox{}\hss\@IEEEfooterstyle\footnotesize\raisebox{\footskip}[0pt][0pt]{\@IEEEpubid}\hss\hbox{}}\relax + \ifCLASSOPTIONcompsoc + % for compsoc papers, the pubid is at the same vertical position as the normal footer + \def\@oddfoot{\hbox{}\hss\@IEEEfooterstyle\raisebox{0pt}[0pt][0pt]{\@IEEEpubid}\hss\hbox{}}\relax + \def\@evenfoot{\hbox{}\hss\@IEEEfooterstyle\raisebox{0pt}[0pt][0pt]{\@IEEEpubid}\hss\hbox{}}\relax + \fi + \fi +\fi} + + + +%% Defines the command for putting the header. +%% Note that all the text is forced into uppercase, if you have some text +%% that needs to be in lower case, for instance et. al., then either manually +%% set \leftmark and \rightmark or use \MakeLowercase{et. al.} within the +%% arguments to \markboth. +%% V1.7b add \protect to work with Babel +\def\markboth#1#2{\def\leftmark{\MakeUppercase{\protect#1}}% +\def\rightmark{\MakeUppercase{\protect#2}}} + +\def\today{\ifcase\month\or + January\or February\or March\or April\or May\or June\or + July\or August\or September\or October\or November\or December\fi + \space\number\day, \number\year} + + + + +%% CITATION AND BIBLIOGRAPHY COMMANDS +%% +%% V1.6 no longer supports the older, nonstandard \shortcite and \citename setup stuff +% +% +% Modify Latex2e \@citex to separate citations with "], [" +\def\@citex[#1]#2{% + \let\@citea\@empty + \@cite{\@for\@citeb:=#2\do + {\@citea\def\@citea{], [}% + \edef\@citeb{\expandafter\@firstofone\@citeb\@empty}% + \if@filesw\immediate\write\@auxout{\string\citation{\@citeb}}\fi + \@ifundefined{b@\@citeb}{\mbox{\reset@font\bfseries ?}% + \G@refundefinedtrue + \@latex@warning + {Citation `\@citeb' on page \thepage \space undefined}}% + {\hbox{\csname b@\@citeb\endcsname}}}}{#1}} + +% V1.6 we create hooks for the optional use of Donald Arseneau's +% cite.sty package. cite.sty is "smart" and will notice that the +% following format controls are already defined and will not +% redefine them. The result will be the proper sorting of the +% citation numbers and auto detection of 3 or more entry "ranges" - +% all in IEEE style: [1], [2], [5]--[7], [12] +% This also allows for an optional note, i.e., \cite[mynote]{..}. +% If the \cite with note has more than one reference, the note will +% be applied to the last of the listed references. It is generally +% desired that if a note is given, only one reference is listed in +% that \cite. +% Thanks to Mr. Arseneau for providing the required format arguments +% to produce the IEEE style. +\def\citepunct{], [} +\def\citedash{]--[} + +% V1.7 default to using same font for urls made by url.sty +\AtBeginDocument{\csname url@samestyle\endcsname} + +% V1.6 class files should always provide these +\def\newblock{\hskip .11em\@plus.33em\@minus.07em} +\let\@openbib@code\@empty +% V1.8b article.cls is now providing these too +% we do not use \@mkboth, nor alter the page style +\newenvironment{theindex} + {\if@twocolumn + \@restonecolfalse + \else + \@restonecoltrue + \fi + \twocolumn[\section*{\indexname}]% + \parindent\z@ + \parskip\z@ \@plus .3\p@\relax + \columnseprule \z@ + \columnsep 35\p@ + \let\item\@idxitem} + {\if@restonecol\onecolumn\else\clearpage\fi} +\newcommand\@idxitem{\par\hangindent 40\p@} +\newcommand\subitem{\@idxitem \hspace*{20\p@}} +\newcommand\subsubitem{\@idxitem \hspace*{30\p@}} +\newcommand\indexspace{\par \vskip 10\p@ \@plus5\p@ \@minus3\p@\relax} + + + +% Provide support for the control entries of IEEEtran.bst V1.00 and later. +% V1.7 optional argument allows for a different aux file to be specified in +% order to handle multiple bibliographies. For example, with multibib.sty: +% \newcites{sec}{Secondary Literature} +% \bstctlcite[@auxoutsec]{BSTcontrolhak} +\def\bstctlcite{\@ifnextchar[{\@bstctlcite}{\@bstctlcite[@auxout]}} +\def\@bstctlcite[#1]#2{\@bsphack + \@for\@citeb:=#2\do{% + \edef\@citeb{\expandafter\@firstofone\@citeb}% + \if@filesw\immediate\write\csname #1\endcsname{\string\citation{\@citeb}}\fi}% + \@esphack} + +% \IEEEnoauxwrite{} allows for citations that do not add to or affect +% the order of the existing citation list. Can be useful for \cite +% within \thanks{}. +\DeclareRobustCommand{\IEEEnoauxwrite}[1]{\relax +\if@filesw +\@fileswfalse +#1\relax\relax\relax\relax\relax +\@fileswtrue +\else +#1\relax\relax\relax\relax\relax +\fi} + +% V1.6 provide a way for a user to execute a command just before +% a given reference number - used to insert a \newpage to balance +% the columns on the last page +\edef\@IEEEtriggerrefnum{0} % the default of zero means that + % the command is not executed +\def\@IEEEtriggercmd{\newpage} + +% allow the user to alter the triggered command +\long\def\IEEEtriggercmd#1{\long\def\@IEEEtriggercmd{#1}} + +% allow user a way to specify the reference number just before the +% command is executed +\def\IEEEtriggeratref#1{\@IEEEtrantmpcountA=#1% +\edef\@IEEEtriggerrefnum{\the\@IEEEtrantmpcountA}}% + +% trigger command at the given reference +\def\@IEEEbibitemprefix{\@IEEEtrantmpcountA=\@IEEEtriggerrefnum\relax% +\advance\@IEEEtrantmpcountA by -1\relax% +\ifnum\c@enumiv=\@IEEEtrantmpcountA\relax\@IEEEtriggercmd\relax\fi} + + +\def\@biblabel#1{[#1]} + +% compsoc journals and conferences left align the reference numbers +\@IEEEcompsoconly{\def\@biblabel#1{[#1]\hfill}} + +% controls bib item spacing +\def\IEEEbibitemsep{0pt plus .5pt} + +\@IEEEcompsocconfonly{\def\IEEEbibitemsep{0.5\baselineskip plus 0.25\baselineskip minus 0.25\baselineskip}} + + +\def\thebibliography#1{\section*{\refname}% + \addcontentsline{toc}{section}{\refname}% + % V1.6 add some rubber space here and provide a command trigger + \footnotesize\vskip 0.3\baselineskip plus 0.1\baselineskip minus 0.1\baselineskip% + \list{\@biblabel{\@arabic\c@enumiv}}% + {\settowidth\labelwidth{\@biblabel{#1}}% + \leftmargin\labelwidth + \advance\leftmargin\labelsep\relax + \itemsep \IEEEbibitemsep\relax + \usecounter{enumiv}% + \let\p@enumiv\@empty + \renewcommand\theenumiv{\@arabic\c@enumiv}}% + \let\@IEEElatexbibitem\bibitem% + \def\bibitem{\@IEEEbibitemprefix\@IEEElatexbibitem}% +\def\newblock{\hskip .11em plus .33em minus .07em}% +% originally: +% \sloppy\clubpenalty4000\widowpenalty4000% +% by adding the \interlinepenalty here, we make it more +% difficult, but not impossible, for LaTeX to break within a reference. +% The IEEE almost never breaks a reference (but they do it more often with +% technotes). You may get an underfull vbox warning around the bibliography, +% but the final result will be much more like what the IEEE will publish. +% MDS 11/2000 +\ifCLASSOPTIONtechnote\sloppy\clubpenalty4000\widowpenalty4000\interlinepenalty100% +\else\sloppy\clubpenalty4000\widowpenalty4000\interlinepenalty500\fi% + \sfcode`\.=1000\relax} +\let\endthebibliography=\endlist + + + + +% TITLE PAGE COMMANDS +% +% +% \IEEEmembership is used to produce the sublargesize italic font used to indicate author +% IEEE membership. compsoc uses a large size sans slant font +\def\IEEEmembership#1{{\@IEEEnotcompsoconly{\sublargesize}\normalfont\@IEEEcompsoconly{\sffamily}\textit{#1}}} + + +% \IEEEauthorrefmark{} produces a footnote type symbol to indicate author affiliation. +% When given an argument of 1 to 9, \IEEEauthorrefmark{} follows the standard LaTeX footnote +% symbol sequence convention. However, for arguments 10 and above, \IEEEauthorrefmark{} +% reverts to using lower case roman numerals, so it cannot overflow. Do note that you +% cannot use \footnotemark[] in place of \IEEEauthorrefmark{} within \author as the footnote +% symbols will have been turned off to prevent \thanks from creating footnote marks. +% \IEEEauthorrefmark{} produces a symbol that appears to LaTeX as having zero vertical +% height - this allows for a more compact line packing, but the user must ensure that +% the interline spacing is large enough to prevent \IEEEauthorrefmark{} from colliding +% with the text above. +% V1.7 make this a robust command +% V1.8 transmag uses an arabic author affiliation symbol +\ifCLASSOPTIONtransmag +\DeclareRobustCommand*{\IEEEauthorrefmark}[1]{\raisebox{0pt}[0pt][0pt]{\textsuperscript{\footnotesize #1}}} +\else +\DeclareRobustCommand*{\IEEEauthorrefmark}[1]{\raisebox{0pt}[0pt][0pt]{\textsuperscript{\footnotesize\ensuremath{\ifcase#1\or *\or \dagger\or \ddagger\or% + \mathsection\or \mathparagraph\or \|\or **\or \dagger\dagger% + \or \ddagger\ddagger \else\textsuperscript{\expandafter\romannumeral#1}\fi}}}} +\fi + + +% FONT CONTROLS AND SPACINGS FOR CONFERENCE MODE AUTHOR NAME AND AFFILIATION BLOCKS +% +% The default font styles for the author name and affiliation blocks (confmode) +\def\@IEEEauthorblockNstyle{\normalfont\@IEEEcompsocnotconfonly{\sffamily}\sublargesize} +\def\@IEEEauthorblockAstyle{\normalfont\@IEEEcompsocnotconfonly{\sffamily}\@IEEEcompsocconfonly{\itshape}\normalsize} +% The default if the user does not use an author block +\def\@IEEEauthordefaulttextstyle{\normalfont\@IEEEcompsocnotconfonly{\sffamily}\sublargesize} + +% adjustment spacing from title (or special paper notice) to author name blocks (confmode) +% can be negative +\def\@IEEEauthorblockconfadjspace{-0.25em} +% compsoc conferences need more space here +\@IEEEcompsocconfonly{\def\@IEEEauthorblockconfadjspace{0.75\@IEEEnormalsizeunitybaselineskip}} + +% spacing between name and affiliation blocks (confmode) +% This can be negative. +% The IEEE doesn't want any added spacing here, but I will leave these +% controls in place in case they ever change their mind. +% Personally, I like 0.75ex. +%\def\@IEEEauthorblockNtopspace{0.75ex} +%\def\@IEEEauthorblockAtopspace{0.75ex} +\def\@IEEEauthorblockNtopspace{0.0ex} +\def\@IEEEauthorblockAtopspace{0.0ex} +\ifCLASSOPTIONtransmag +% transmag uses one line of space above first affiliation block +\def\@IEEEauthorblockAtopspace{1\@IEEEnormalsizeunitybaselineskip} +\fi + +% baseline spacing within name and affiliation blocks (confmode) +% must be positive, spacings below certain values will make +% the position of line of text sensitive to the contents of the +% line above it i.e., whether or not the prior line has descenders, +% subscripts, etc. For this reason it is a good idea to keep +% these above 2.6ex +\def\@IEEEauthorblockNinterlinespace{2.6ex} +\def\@IEEEauthorblockAinterlinespace{2.75ex} + +% This tracks the required strut size. +% See the \@IEEEauthorhalign command for the actual default value used. +\def\@IEEEauthorblockXinterlinespace{2.7ex} + +% variables to retain font size and style across groups +% values given here have no effect as they will be overwritten later +\gdef\@IEEESAVESTATEfontsize{10} +\gdef\@IEEESAVESTATEfontbaselineskip{12} +\gdef\@IEEESAVESTATEfontencoding{OT1} +\gdef\@IEEESAVESTATEfontfamily{ptm} +\gdef\@IEEESAVESTATEfontseries{m} +\gdef\@IEEESAVESTATEfontshape{n} + +% saves the current font attributes +\def\@IEEEcurfontSAVE{\global\let\@IEEESAVESTATEfontsize\f@size% +\global\let\@IEEESAVESTATEfontbaselineskip\f@baselineskip% +\global\let\@IEEESAVESTATEfontencoding\f@encoding% +\global\let\@IEEESAVESTATEfontfamily\f@family% +\global\let\@IEEESAVESTATEfontseries\f@series% +\global\let\@IEEESAVESTATEfontshape\f@shape} + +% restores the saved font attributes +\def\@IEEEcurfontRESTORE{\fontsize{\@IEEESAVESTATEfontsize}{\@IEEESAVESTATEfontbaselineskip}% +\fontencoding{\@IEEESAVESTATEfontencoding}% +\fontfamily{\@IEEESAVESTATEfontfamily}% +\fontseries{\@IEEESAVESTATEfontseries}% +\fontshape{\@IEEESAVESTATEfontshape}% +\selectfont} + + +% variable to indicate if the current block is the first block in the column +\newif\if@IEEEprevauthorblockincol \@IEEEprevauthorblockincolfalse + + +% the command places a strut with height and depth = \@IEEEauthorblockXinterlinespace +% we use this technique to have complete manual control over the spacing of the lines +% within the halign environment. +% We set the below baseline portion at 30%, the above +% baseline portion at 70% of the total length. +% Responds to changes in the document's \baselinestretch +\def\@IEEEauthorstrutrule{\@IEEEtrantmpdimenA\@IEEEauthorblockXinterlinespace% +\@IEEEtrantmpdimenA=\baselinestretch\@IEEEtrantmpdimenA% +\rule[-0.3\@IEEEtrantmpdimenA]{0pt}{\@IEEEtrantmpdimenA}} + + +% blocks to hold the authors' names and affilations. +% Makes formatting easy for conferences +% +% use real definitions in conference mode +% name block +\def\IEEEauthorblockN#1{\relax\@IEEEauthorblockNstyle% set the default text style +\gdef\@IEEEauthorblockXinterlinespace{0pt}% disable strut for spacer row +% the \expandafter hides the \cr in conditional tex, see the array.sty docs +% for details, probably not needed here as the \cr is in a macro +% do a spacer row if needed +\if@IEEEprevauthorblockincol\expandafter\@IEEEauthorblockNtopspaceline\fi +\global\@IEEEprevauthorblockincoltrue% we now have a block in this column +%restore the correct strut value +\gdef\@IEEEauthorblockXinterlinespace{\@IEEEauthorblockNinterlinespace}% +% input the author names +#1% +% end the row if the user did not already +\crcr} +% spacer row for names +\def\@IEEEauthorblockNtopspaceline{\cr\noalign{\vskip\@IEEEauthorblockNtopspace}} +% +% affiliation block +\def\IEEEauthorblockA#1{\relax\@IEEEauthorblockAstyle% set the default text style +\gdef\@IEEEauthorblockXinterlinespace{0pt}%disable strut for spacer row +% the \expandafter hides the \cr in conditional tex, see the array.sty docs +% for details, probably not needed here as the \cr is in a macro +% do a spacer row if needed +\if@IEEEprevauthorblockincol\expandafter\@IEEEauthorblockAtopspaceline\fi +\global\@IEEEprevauthorblockincoltrue% we now have a block in this column +%restore the correct strut value +\gdef\@IEEEauthorblockXinterlinespace{\@IEEEauthorblockAinterlinespace}% +% input the author affiliations +#1% +% end the row if the user did not already +\crcr +% V1.8 transmag does not use any additional affiliation spacing after the first author +\ifCLASSOPTIONtransmag\gdef\@IEEEauthorblockAtopspace{0pt}\fi} + +% spacer row for affiliations +\def\@IEEEauthorblockAtopspaceline{\cr\noalign{\vskip\@IEEEauthorblockAtopspace}} + + +% allow papers to compile even if author blocks are used in modes other +% than conference or peerreviewca. For such cases, we provide dummy blocks. +\ifCLASSOPTIONconference +\else + \ifCLASSOPTIONpeerreviewca\else + % not conference, peerreviewca or transmag mode + \ifCLASSOPTIONtransmag\else + \def\IEEEauthorblockN#1{#1}% + \def\IEEEauthorblockA#1{#1}% + \fi + \fi +\fi + + + +% we provide our own halign so as not to have to depend on tabular +\def\@IEEEauthorhalign{\@IEEEauthordefaulttextstyle% default text style + \lineskip=0pt\relax% disable line spacing + \lineskiplimit=0pt\relax% + \baselineskip=0pt\relax% + \@IEEEcurfontSAVE% save the current font + \mathsurround\z@\relax% no extra spacing around math + \let\\\@IEEEauthorhaligncr% replace newline with halign friendly one + \tabskip=0pt\relax% no column spacing + \everycr{}% ensure no problems here + \@IEEEprevauthorblockincolfalse% no author blocks yet + \def\@IEEEauthorblockXinterlinespace{2.7ex}% default interline space + \vtop\bgroup%vtop box + \halign\bgroup&\relax\hfil\@IEEEcurfontRESTORE\relax ##\relax + \hfil\@IEEEcurfontSAVE\@IEEEauthorstrutrule\cr} + +% ensure last line, exit from halign, close vbox +\def\end@IEEEauthorhalign{\crcr\egroup\egroup} + +% handle bogus star form +\def\@IEEEauthorhaligncr{{\ifnum0=`}\fi\@ifstar{\@@IEEEauthorhaligncr}{\@@IEEEauthorhaligncr}} + +% test and setup the optional argument to \\[] +\def\@@IEEEauthorhaligncr{\@testopt\@@@IEEEauthorhaligncr\z@skip} + +% end the line and do the optional spacer +\def\@@@IEEEauthorhaligncr[#1]{\ifnum0=`{\fi}\cr\noalign{\vskip#1\relax}} + + + +% flag to prevent multiple \and warning messages +\newif\if@IEEEWARNand +\@IEEEWARNandtrue + +% if in conference or peerreviewca modes, we support the use of \and as \author is a +% tabular environment, otherwise we warn the user that \and is invalid +% outside of conference or peerreviewca modes. +\def\and{\relax} % provide a bogus \and that we will then override + +\renewcommand{\and}[1][\relax]{\if@IEEEWARNand\typeout{** WARNING: \noexpand\and is valid only + when in conference or peerreviewca}\typeout{modes (line \the\inputlineno).}\fi\global\@IEEEWARNandfalse} + +\ifCLASSOPTIONconference% +\renewcommand{\and}[1][\hfill]{\end{@IEEEauthorhalign}#1\begin{@IEEEauthorhalign}}% +\fi +\ifCLASSOPTIONpeerreviewca +\renewcommand{\and}[1][\hfill]{\end{@IEEEauthorhalign}#1\begin{@IEEEauthorhalign}}% +\fi +% V1.8 transmag uses conference author format +\ifCLASSOPTIONtransmag +\renewcommand{\and}[1][\hfill]{\end{@IEEEauthorhalign}#1\begin{@IEEEauthorhalign}}% +\fi + +% page clearing command +% based on LaTeX2e's \cleardoublepage, but allows different page styles +% for the inserted blank pages +\def\@IEEEcleardoublepage#1{\clearpage\if@twoside\ifodd\c@page\else +\hbox{}\thispagestyle{#1}\newpage\if@twocolumn\hbox{}\thispagestyle{#1}\newpage\fi\fi\fi} + +% V1.8b hooks to allow adjustment of space above title +\def\IEEEtitletopspace{0.5\baselineskip} +% an added extra amount to allow for adjustment/offset +\def\IEEEtitletopspaceextra{0pt} + +% user command to invoke the title page +\def\maketitle{\par% + \begingroup% + \normalfont% + \def\thefootnote{}% the \thanks{} mark type is empty + \def\footnotemark{}% and kill space from \thanks within author + \let\@makefnmark\relax% V1.7, must *really* kill footnotemark to remove all \textsuperscript spacing as well. + \footnotesize% equal spacing between thanks lines + \footnotesep 0.7\baselineskip%see global setting of \footnotesep for more info + % V1.7 disable \thanks note indention for compsoc + \@IEEEcompsoconly{\long\def\@makefntext##1{\parindent 1em\noindent\hbox{\@makefnmark}##1}}% + \normalsize% + \ifCLASSOPTIONpeerreview + \newpage\global\@topnum\z@ \@maketitle\@IEEEstatictitlevskip\@IEEEaftertitletext% + \thispagestyle{IEEEpeerreviewcoverpagestyle}\@thanks% + \else + \if@twocolumn% + \ifCLASSOPTIONtechnote% + \newpage\global\@topnum\z@ \@maketitle\@IEEEstatictitlevskip\@IEEEaftertitletext% + \else + \twocolumn[{\IEEEquantizevspace{\@maketitle}[\IEEEquantizedisabletitlecmds]{0pt}[-\topskip]{\baselineskip}{\@IEEENORMtitlevspace}{\@IEEEMINtitlevspace}\@IEEEaftertitletext}]% + \fi + \else + \newpage\global\@topnum\z@ \@maketitle\@IEEEstatictitlevskip\@IEEEaftertitletext% + \fi + \thispagestyle{IEEEtitlepagestyle}\@thanks% + \fi + % pullup page for pubid if used. + \if@IEEEusingpubid + \enlargethispage{-\@IEEEpubidpullup}% + \fi + \endgroup + \setcounter{footnote}{0}\let\maketitle\relax\let\@maketitle\relax + \gdef\@thanks{}% + % v1.6b do not clear these as we will need the title again for peer review papers + % \gdef\@author{}\gdef\@title{}% + \let\thanks\relax} + + +% V1.8 parbox to format \@IEEEtitleabstractindextext +\long\def\@IEEEtitleabstractindextextbox#1{\parbox{1\textwidth}{#1}} +% V1.8 compsoc is partial width +\ifCLASSOPTIONcompsoc +% comparison with proofs suggests it's in the range of 92.1-92.3% +\long\def\@IEEEtitleabstractindextextbox#1{\parbox{0.922\textwidth}{\@IEEEcompsocnotconfonly{\rightskip\@flushglue\leftskip\z@skip}#1}} +\fi + +% formats the Title, authors names, affiliations and special paper notice +% THIS IS A CONTROLLED SPACING COMMAND! Do not allow blank lines or unintentional +% spaces to enter the definition - use % at the end of each line +\def\@maketitle{\newpage +\bgroup\par\vskip\IEEEtitletopspace\vskip\IEEEtitletopspaceextra\centering% +\ifCLASSOPTIONtechnote% technotes, V1.8a abstract and index terms are not treated differently for compsoc technotes + {\bfseries\large\@IEEEcompsoconly{\Large\sffamily}\@title\par}\vskip 1.3em{\lineskip .5em\@IEEEcompsoconly{\large\sffamily}\@author + \@IEEEspecialpapernotice\par}\relax +\else% not a technote + \vskip0.2em{\Huge\ifCLASSOPTIONtransmag\bfseries\LARGE\fi\@IEEEcompsoconly{\sffamily}\@IEEEcompsocconfonly{\normalfont\normalsize\vskip 2\@IEEEnormalsizeunitybaselineskip + \bfseries\Large}\@IEEEcompsocnotconfonly{\vskip 0.75\@IEEEnormalsizeunitybaselineskip}\@title\par}\relax + \@IEEEcompsocnotconfonly{\vskip 0.5\@IEEEnormalsizeunitybaselineskip}\vskip1.0em\par% + % V1.6 handle \author differently if in conference mode + \ifCLASSOPTIONconference% + {\@IEEEspecialpapernotice\mbox{}\vskip\@IEEEauthorblockconfadjspace% + \mbox{}\hfill\begin{@IEEEauthorhalign}\@author\end{@IEEEauthorhalign}\hfill\mbox{}\par}\relax + \else% peerreviewca, peerreview or journal + \ifCLASSOPTIONpeerreviewca + % peerreviewca handles author names just like conference mode + {\@IEEEcompsoconly{\sffamily}\@IEEEspecialpapernotice\mbox{}\vskip\@IEEEauthorblockconfadjspace% + \mbox{}\hfill\begin{@IEEEauthorhalign}\@author\end{@IEEEauthorhalign}\hfill\mbox{}\par + {\@IEEEcompsoconly{\vskip 1.5em\relax + \@IEEEtitleabstractindextextbox{\@IEEEtitleabstractindextext}\par\noindent\hfill + \IEEEcompsocdiamondline\hfill\hbox{}\par}}}\relax + \else% journal, peerreview or transmag + \ifCLASSOPTIONtransmag + % transmag also handles author names just like conference mode + % it also uses \@IEEEtitleabstractindextex, but with one line less + % space above, and one more below + {\@IEEEspecialpapernotice\mbox{}\vskip\@IEEEauthorblockconfadjspace% + \mbox{}\hfill\begin{@IEEEauthorhalign}\@author\end{@IEEEauthorhalign}\hfill\mbox{}\par + {\vspace{0.5\baselineskip}\relax\@IEEEtitleabstractindextextbox{\@IEEEtitleabstractindextext}\vspace{-1\baselineskip}\par}}\relax + \else% journal or peerreview + {\lineskip.5em\@IEEEcompsoconly{\sffamily}\sublargesize\@author\@IEEEspecialpapernotice\par + {\@IEEEcompsoconly{\vskip 1.5em\relax + \@IEEEtitleabstractindextextbox{\@IEEEtitleabstractindextext}\par\noindent\hfill + \IEEEcompsocdiamondline\hfill\hbox{}\par}}}\relax + \fi + \fi + \fi +\fi\par\addvspace{0.5\baselineskip}\egroup} + + +% V1.7 Computer Society "diamond line" which follows index terms for nonconference papers +% V1.8a full width diamond line for single column use +\def\@IEEEcompsocdiamondlinei{\vrule depth 0pt height 0.5pt width 4cm\nobreak\hspace{7.5pt}\nobreak +\raisebox{-3.5pt}{\fontfamily{pzd}\fontencoding{U}\fontseries{m}\fontshape{n}\fontsize{11}{12}\selectfont\char70}\nobreak +\hspace{7.5pt}\nobreak\vrule depth 0pt height 0.5pt width 4cm\relax} +% V1.8a narrower width diamond line for double column use +\def\@IEEEcompsocdiamondlineii{\vrule depth 0pt height 0.5pt width 2.5cm\nobreak\hspace{7.5pt}\nobreak +\raisebox{-3.5pt}{\fontfamily{pzd}\fontencoding{U}\fontseries{m}\fontshape{n}\fontsize{11}{12}\selectfont\char70}\nobreak +\hspace{7.5pt}\nobreak\vrule depth 0pt height 0.5pt width 2.5cm\relax} +% V1.8a bare core without rules to base a last resort on for very narrow linewidths +\def\@IEEEcompsocdiamondlineiii{\mbox{}\nobreak\hspace{7.5pt}\nobreak +\raisebox{-3.5pt}{\fontfamily{pzd}\fontencoding{U}\fontseries{m}\fontshape{n}\fontsize{11}{12}\selectfont\char70}\nobreak +\hspace{7.5pt}\nobreak\mbox{}\relax} + +% V1.8a allow \IEEEcompsocdiamondline to adjust for different linewidths. +% Use \@IEEEcompsocdiamondlinei if its width is less than 0.66\linewidth (0.487 nominal for single column) +% if not, fall back to \@IEEEcompsocdiamondlineii if its width is less than 0.75\linewidth (0.659 nominal for double column) +% if all else fails, try to make a custom diamondline based on the abnormally narrow linewidth +\def\IEEEcompsocdiamondline{\settowidth{\@IEEEtrantmpdimenA}{\@IEEEcompsocdiamondlinei}\relax +\ifdim\@IEEEtrantmpdimenA<0.66\linewidth\relax\@IEEEcompsocdiamondlinei\relax +\else +\settowidth{\@IEEEtrantmpdimenA}{\@IEEEcompsocdiamondlineii}\relax +\ifdim\@IEEEtrantmpdimenA<0.75\linewidth\relax\@IEEEcompsocdiamondlineii\relax +\else +\settowidth{\@IEEEtrantmpdimenA}{\@IEEEcompsocdiamondlineiii}\relax +\@IEEEtrantmpdimenB=\linewidth\relax +\addtolength{\@IEEEtrantmpdimenB}{-1\@IEEEtrantmpdimenA}\relax +\vrule depth 0pt height 0.5pt width 0.33\@IEEEtrantmpdimenB\@IEEEcompsocdiamondlineiii\vrule depth 0pt height 0.5pt width 0.33\@IEEEtrantmpdimenB\relax +\fi\fi} + + +% V1.7 standard LateX2e \thanks, but with \itshape under compsoc. Also make it a \long\def +% We also need to trigger the one-shot footnote rule +\def\@IEEEtriggeroneshotfootnoterule{\global\@IEEEenableoneshotfootnoteruletrue} + + +\long\def\thanks#1{\footnotemark + \protected@xdef\@thanks{\@thanks + \protect\footnotetext[\the\c@footnote]{\@IEEEcompsoconly{\itshape + \protect\@IEEEtriggeroneshotfootnoterule\relax}\ignorespaces#1}}} +\let\@thanks\@empty + + +% V1.7 allow \author to contain \par's. This is needed to allow \thanks to contain \par. +\long\def\author#1{\gdef\@author{#1}} + + +% in addition to setting up IEEEitemize, we need to remove a baselineskip space above and +% below it because \list's \pars introduce blank lines because of the footnote struts. +\def\@IEEEsetupcompsocitemizelist{\def\labelitemi{$\bullet$}% +\setlength{\IEEElabelindent}{0pt}\setlength{\labelsep}{1.2em}\setlength{\parskip}{0pt}% +\setlength{\partopsep}{0pt}\setlength{\topsep}{0.5\baselineskip}\vspace{-1\baselineskip}\relax} + + +% flag for fake non-compsoc \IEEEcompsocthanksitem - prevents line break on very first item +\newif\if@IEEEbreakcompsocthanksitem \@IEEEbreakcompsocthanksitemfalse + +\ifCLASSOPTIONcompsoc +% V1.7 compsoc bullet item \thanks +% also, we need to redefine this to destroy the argument in \IEEEquantizevspace +\long\def\IEEEcompsocitemizethanks#1{\relax\@IEEEbreakcompsocthanksitemfalse\footnotemark + \protected@xdef\@thanks{\@thanks + \protect\footnotetext[\the\c@footnote]{\itshape\protect\@IEEEtriggeroneshotfootnoterule + {\let\IEEEiedlistdecl\relax\protect\begin{IEEEitemize}[\protect\@IEEEsetupcompsocitemizelist]\ignorespaces#1\relax + \protect\end{IEEEitemize}}\protect\vspace{-1\baselineskip}}}} +\DeclareRobustCommand*{\IEEEcompsocthanksitem}{\item} +\else +% non-compsoc, allow for dual compilation via rerouting to normal \thanks +\long\def\IEEEcompsocitemizethanks#1{\thanks{#1}} +% redirect to "pseudo-par" \hfil\break\indent after swallowing [] from \IEEEcompsocthanksitem[] +\DeclareRobustCommand{\IEEEcompsocthanksitem}{\@ifnextchar [{\@IEEEthanksswallowoptionalarg}% +{\@IEEEthanksswallowoptionalarg[\relax]}} +% be sure and break only after first item, be sure and ignore spaces after optional argument +\def\@IEEEthanksswallowoptionalarg[#1]{\relax\if@IEEEbreakcompsocthanksitem\hfil\break +\indent\fi\@IEEEbreakcompsocthanksitemtrue\ignorespaces} +\fi + + +% V1.6b define the \IEEEpeerreviewmaketitle as needed +\ifCLASSOPTIONpeerreview +\def\IEEEpeerreviewmaketitle{\@IEEEcleardoublepage{empty}% +\ifCLASSOPTIONtwocolumn +\twocolumn[{\IEEEquantizevspace{\@IEEEpeerreviewmaketitle}[\IEEEquantizedisabletitlecmds]{0pt}[-\topskip]{\baselineskip}{\@IEEENORMtitlevspace}{\@IEEEMINtitlevspace}}] +\else +\newpage\@IEEEpeerreviewmaketitle\@IEEEstatictitlevskip +\fi +\thispagestyle{IEEEtitlepagestyle}} +\else +% \IEEEpeerreviewmaketitle does nothing if peer review option has not been selected +\def\IEEEpeerreviewmaketitle{\relax} +\fi + +% peerreview formats the repeated title like the title in journal papers. +\def\@IEEEpeerreviewmaketitle{\bgroup\par\addvspace{0.5\baselineskip}\centering\@IEEEcompsoconly{\sffamily}% +\normalfont\normalsize\vskip0.2em{\Huge\@title\par}\vskip1.0em\par +\par\addvspace{0.5\baselineskip}\egroup} + + + +% V1.6 +% this is a static rubber spacer between the title/authors and the main text +% used for single column text, or when the title appears in the first column +% of two column text (technotes). +\def\@IEEEstatictitlevskip{{\normalfont\normalsize +% adjust spacing to next text +% v1.6b handle peer review papers +\ifCLASSOPTIONpeerreview +% for peer review papers, the same value is used for both title pages +% regardless of the other paper modes + \vskip 1\baselineskip plus 0.375\baselineskip minus 0.1875\baselineskip +\else + \ifCLASSOPTIONconference% conference + \vskip 1\baselineskip plus 0.375\baselineskip minus 0.1875\baselineskip% + \else% + \ifCLASSOPTIONtechnote% technote + \vskip 1\baselineskip plus 0.375\baselineskip minus 0.1875\baselineskip% + \else% journal uses more space + \vskip 2.5\baselineskip plus 0.75\baselineskip minus 0.375\baselineskip% + \fi + \fi +\fi}} + + +% set the nominal and minimum values for the quantized title spacer +% the quantization algorithm will not allow the spacer size to +% become less than \@IEEEMINtitlevspace - instead it will be lengthened +% default to journal values +\def\@IEEENORMtitlevspace{2.5\baselineskip} +\def\@IEEEMINtitlevspace{2\baselineskip} +% conferences and technotes need tighter spacing +\ifCLASSOPTIONconference% conference + \def\@IEEENORMtitlevspace{1\baselineskip} + \def\@IEEEMINtitlevspace{0.75\baselineskip} +\fi +\ifCLASSOPTIONtechnote% technote + \def\@IEEENORMtitlevspace{1\baselineskip} + \def\@IEEEMINtitlevspace{0.75\baselineskip} +\fi + + +% V1.8a +\def\IEEEquantizevspace{\begingroup\@ifstar{\@IEEEquantizevspacestarformtrue\@IEEEquantizevspace}{\@IEEEquantizevspacestarformfalse\@IEEEquantizevspace}} +% \IEEEquantizevspace[output dimen register]{object}[object decl] +% {top baselineskip} +% [offset][prevdepth][lineskip limit][lineskip] +% {unit height}{nominal vspace}{minimum vspace} +% +% Calculates and creates the vspace needed to make the combined height with +% the given object an integer multiple of the given unit height. This command +% is more general than the older \@IEEEdynamictitlevspace it replaces. +% +% The star form has no effect at present, but is reserved for future use. +% +% If the optional argument [output dimen register] is given, the calculated +% vspace height is stored in the given output dimen (or skip) register +% and no other action is taken, otherwise the object followed by a vspace* +% of the appropriate height is evaluated/output. +% +% The optional object decl (declarations) is code that is evaluated just +% before the object's height is evaluated. Its intented purpose is to allow +% for the alteration or disabling of code within the object during internal +% height evaluation (e.g., \long\def\thanks#1{\relax} ). +% This special code is not invoked if/when the object is rendered at the end. +% +% The nominal vspace is the target value of the added vspace and the minimum +% vspace is the lower allowed limit. The vspacer will be the value that achieves +% integral overall height, in terms of the given unit height, that is closest +% to the nominal vspace and that is not less than the specified minimum vspace. +% +% The line spacing algorithm of TeX is somewhat involved and requires special +% care with regard to the first line of a vertical list (which is indicated +% when \prevdepth is -1000pt or less). top baselineskip specifies the +% baselineskip or topskip used prior to the object. If the height of the +% first line of the object is greater than the given top baselineskip, then +% the top baselineskip is subtracted from the height of the first line and +% that difference is considered along with the rest of the object height +% (because the object will be shifted down by an amount = +% top line height - top baselineskip). Otherwise, the height of the first line +% of the object is ignored as far as the calculations are concerned. +% This algorithm is adequate for objects that appear at the top of a page +% (e.g., titles) where \topskip spacing is used. +% +% However, as explained on page 78 of the TeXbook, interline spacing is more +% complex when \baselineskip is being used (indicated by \prevdepth > +% -1000pt). The four optional parameters offset, prevdepth, lineskip limit and +% lineskip are assumed to be equal to be 0pt, \prevdepth, \lineskiplimit and +% \lineskip, respectively, if they are omitted. +% +% The prevdepth is the depth of the line before the object, the lineskip limit +% specifies how close the top of the object can come to the bottom of the +% previous line before \baselineskip is ignored and \lineskip is inserted +% between the object and the line above it. Lineskip does not come into +% play unless the first line of the object is high enough to "get too close" +% (as specified by lineskiplimit) to the line before it. The the prevdepth, +% lineskip limit, and lineskip optional parameters are not needed for the +% first object/line on a page (i.e., prevdepth <= -1000pt) where the simplier +% \topskip spacing rules are in effect. +% +% Offset is a manual adjustment that is added to the height calculations of +% object irrespective of the value of \prevdepth. It is useful when the top +% baselineskip will result in a noninteger unit height object placement even +% if the object itself has integral height. e.g., a footnotesize baselineskip +% is used before the object, thus an offset of, say -3pt, can be given as a +% correction. + +% Common combinations of these parameters include: +% +% top baselineskip: (and default values for offset, prevdepth, etc.) +% \topskip % for objects that appear at the top of a page +% \maxdimen % always ignore the height of the top line +% 0pt % always consider any positive height of the top line +% +% for objects to appear inline in normal text: +% top baselineskip = \baselineskip +% +% set prevdepth = -1000pt and top baselineskip = 0pt to consider the +% overall height of the object without any other external skip +% consideration + +\newif\if@IEEEquantizevspacestarform % flag to indicate star form +\newif\if@IEEEquantizevspaceuseoutdimenreg % flag to indicate output dimen register is to be used +% Use our own private registers because the object could contain a +% structure that uses the existing tmp scratch pad registers +\newdimen\@IEEEquantizeheightA +\newdimen\@IEEEquantizeheightB +\newdimen\@IEEEquantizeheightC +\newdimen\@IEEEquantizeprevdepth % need to save this early as can change +\newcount\@IEEEquantizemultiple +\newbox\@IEEEquantizeboxA + + +\def\@IEEEquantizevspace{\@ifnextchar [{\@IEEEquantizevspaceuseoutdimenregtrue\@@IEEEquantizevspace}{\@IEEEquantizevspaceuseoutdimenregfalse\@@IEEEquantizevspace[]}} + + +\long\def\@@IEEEquantizevspace[#1]#2{\relax +% acquire and store +% #1 optional output dimen register +% #2 object +\edef\@IEEEquantizeoutdimenreg{#1}\relax +% allow for object specifications that contain parameters +\@IEEEtrantmptoksA={#2}\relax +\long\edef\@IEEEquantizeobject{\the\@IEEEtrantmptoksA}\relax +\@ifnextchar [{\@@@IEEEquantizevspace}{\@@@IEEEquantizevspace[\relax]}} + +\long\def\@@@IEEEquantizevspace[#1]#2{\relax +% acquire and store +% [#1] optional object decl, is \relax if not given by user +% #2 top baselineskip +% allow for object decl specifications that have parameters +\@IEEEtrantmptoksA={#1}\relax +\long\edef\@IEEEquantizeobjectdecl{\the\@IEEEtrantmptoksA}\relax +\edef\@IEEEquantizetopbaselineskip{#2}\ivIEEEquantizevspace} + +% acquire optional argument set and store +% [offset][prevdepth][lineskip limit][lineskip] +\def\ivIEEEquantizevspace{\@ifnextchar [{\@vIEEEquantizevspace}{\@vIEEEquantizevspace[0pt]}} +\def\@vIEEEquantizevspace[#1]{\edef\@IEEEquantizeoffset{#1}\@ifnextchar [{\@viIEEEquantizevspace}{\@viIEEEquantizevspace[\prevdepth]}} +\def\@viIEEEquantizevspace[#1]{\@IEEEquantizeprevdepth=#1\relax\@ifnextchar [{\@viiIEEEquantizevspace}{\@viiIEEEquantizevspace[\lineskiplimit]}} +\def\@viiIEEEquantizevspace[#1]{\edef\@IEEEquantizelineskiplimit{#1}\@ifnextchar [{\@viiiIEEEquantizevspace}{\@viiiIEEEquantizevspace[\lineskip]}} +\def\@viiiIEEEquantizevspace[#1]{\edef\@IEEEquantizelineskip{#1}\@ixIEEEquantizevspace} + +% main routine +\def\@ixIEEEquantizevspace#1#2#3{\relax +\edef\@IEEEquantizeunitheight{#1}\relax +\edef\@IEEEquantizenomvspace{#2}\relax +\edef\@IEEEquantizeminvspace{#3}\relax +% \@IEEEquantizeoutdimenreg +% \@IEEEquantizeobject +% \@IEEEquantizeobjectdecl +% \@IEEEquantizetopbaselineskip +% \@IEEEquantizeoffset +% \@IEEEquantizeprevdepth +% \@IEEEquantizelineskiplimit +% \@IEEEquantizelineskip +% \@IEEEquantizeunitheight +% \@IEEEquantizenomvspace +% \@IEEEquantizeminvspace +% get overall height of object +\setbox\@IEEEquantizeboxA\vbox{\begingroup\@IEEEquantizeobjectdecl\@IEEEquantizeobject\relax\endgroup}\relax +\@IEEEquantizeheightA\ht\@IEEEquantizeboxA\relax +% get height of first line of object +\setbox\@IEEEquantizeboxA\vtop{\begingroup\@IEEEquantizeobjectdecl\@IEEEquantizeobject\relax\endgroup}\relax +\@IEEEquantizeheightB\ht\@IEEEquantizeboxA\relax +\ifdim\@IEEEquantizeprevdepth>-1000pt\relax % prevdepth > -1000pf means full baselineskip\lineskip rules in effect +% lineskip spacing rule takes effect if height of top line > baselineskip - prevdepth - lineskiplimit, +% otherwise the baselineskip rule is in effect and the height of the first line does not matter at all. +\@IEEEquantizeheightC=\@IEEEquantizetopbaselineskip\relax +\advance\@IEEEquantizeheightC-\@IEEEquantizeprevdepth\relax +\advance\@IEEEquantizeheightC-\@IEEEquantizelineskiplimit\relax % this works even though \@IEEEquantizelineskiplimit is a macro because TeX allows --10pt notation +\ifdim\@IEEEquantizeheightB>\@IEEEquantizeheightC\relax +% lineskip spacing rule is in effect i.e., the object is going to be shifted down relative to the +% baselineskip set position by its top line height (already a part of the total height) + prevdepth + lineskip - baselineskip +\advance\@IEEEquantizeheightA\@IEEEquantizeprevdepth\relax +\advance\@IEEEquantizeheightA\@IEEEquantizelineskip\relax +\advance\@IEEEquantizeheightA-\@IEEEquantizetopbaselineskip\relax +\else +% height of first line <= \@IEEEquantizetopbaselineskip - \@IEEEquantizeprevdepth - \@IEEEquantizelineskiplimit +% standard baselineskip rules are in effect, so don't consider height of first line +\advance\@IEEEquantizeheightA-\@IEEEquantizeheightB\relax +\fi +% +\else % prevdepth <= -1000pt, simplier \topskip type rules in effect +\ifdim\@IEEEquantizeheightB>\@IEEEquantizetopbaselineskip +% height of top line (already included in the total height) in excess of +% baselineskip is the amount it will be downshifted +\advance\@IEEEquantizeheightA-\@IEEEquantizetopbaselineskip\relax +\else +% height of first line is irrelevant, remove it +\advance\@IEEEquantizeheightA-\@IEEEquantizeheightB\relax +\fi +\fi % prevdepth <= -1000pt +% +% adjust height for any manual offset +\advance\@IEEEquantizeheightA\@IEEEquantizeoffset\relax +% add in nominal spacer +\advance\@IEEEquantizeheightA\@IEEEquantizenomvspace\relax +% check for nonzero unitheight +\@IEEEquantizeheightB=\@IEEEquantizeunitheight\relax +\ifnum\@IEEEquantizeheightB=0\relax +\@IEEEclspkgerror{IEEEquantizevspace unit height cannot be zero. Assuming 10pt.}% +{Division by zero is not allowed.} +\@IEEEquantizeheightB=10pt\relax +\fi +% get integer number of lines +\@IEEEquantizemultiple=\@IEEEquantizeheightA\relax +\divide\@IEEEquantizemultiple\@IEEEquantizeheightB\relax +% set A to contain the excess height over the \@IEEEquantizemultiple of lines +% A = height - multiple*unitheight +\@IEEEquantizeheightC\@IEEEquantizeheightB\relax +\multiply\@IEEEquantizeheightC\@IEEEquantizemultiple\relax +\advance\@IEEEquantizeheightA-\@IEEEquantizeheightC\relax +% set B to contain the height short of \@IEEEquantizemultiple+1 of lines +% B = unitheight - A +\advance\@IEEEquantizeheightB-\@IEEEquantizeheightA\relax +% choose A or B based on which is closer +\@IEEEquantizeheightC\@IEEEquantizenomvspace\relax +\ifdim\@IEEEquantizeheightA<\@IEEEquantizeheightB\relax +% C = nomvspace - A, go with lower +\advance\@IEEEquantizeheightC-\@IEEEquantizeheightA\relax +\else +% C = nomvspace + B, go with upper +\advance\@IEEEquantizeheightC\@IEEEquantizeheightB\relax +\fi +% if violate lower bound, use next integer bound +\ifdim\@IEEEquantizeheightC<\@IEEEquantizeminvspace\relax +% A + B = unitheight +\advance\@IEEEquantizeheightC\@IEEEquantizeheightA\relax +\advance\@IEEEquantizeheightC\@IEEEquantizeheightB\relax +\fi +% export object and spacer outside of group +\global\let\@IEEEquantizeobjectout\@IEEEquantizeobject\relax +\global\@IEEEquantizeheightC\@IEEEquantizeheightC\relax +\endgroup +\if@IEEEquantizevspaceuseoutdimenreg +\@IEEEquantizeoutdimenreg=\@IEEEquantizeheightC\relax +\else +\@IEEEquantizeobjectout\relax +\vskip\@IEEEquantizeheightC\relax +\fi} + + +% user command to disable all global assignments, possible use within object decl +\def\IEEEquantizedisableglobal{\let\global\relax +\let\gdef\def +\let\xdef\edef} +% user command to allow for the disabling of \thanks and other commands, possible use within object decl +\def\IEEEquantizedisabletitlecmds{\long\def\thanks##1{\relax}\relax +\long\def\IEEEcompsocitemizethanks##1{\relax}\def\newpage{\relax}} + + + + + +% V1.6 +% we allow the user access to the last part of the title area +% useful in emergencies such as when a different spacing is needed +% This text is NOT compensated for in the dynamic sizer. +\let\@IEEEaftertitletext=\relax +\long\def\IEEEaftertitletext#1{\def\@IEEEaftertitletext{#1}} + + +% V1.7 provide a way for users to enter abstract and keywords +% into the onecolumn title are. This text is compensated for +% in the dynamic sizer. +\let\@IEEEtitleabstractindextext=\relax +\long\def\IEEEtitleabstractindextext#1{\def\@IEEEtitleabstractindextext{#1}} + +% V1.7 provide a way for users to get the \@IEEEtitleabstractindextext if +% not in compsoc or transmag journal mode - this way abstract and keywords +% can still be placed in their conventional position if not in those modes. +\def\IEEEdisplaynontitleabstractindextext{% +% display for all conference formats +\ifCLASSOPTIONconference\@IEEEtitleabstractindextext\relax +\else% non-conferences + % V1.8a display for all technotes + \ifCLASSOPTIONtechnote\@IEEEtitleabstractindextext\relax + % V1.8a add diamond line after abstract and index terms for compsoc technotes + \@IEEEcompsoconly{\noindent\hfill\IEEEcompsocdiamondline\hfill\hbox{}\par}\relax + \else % non-conferences and non-technotes + \ifCLASSOPTIONcompsoc% display if not compsoc and not transmag + \else + \ifCLASSOPTIONtransmag + \else% not compsoc journal nor transmag journal + \@IEEEtitleabstractindextext\relax + \fi + \fi + \fi +\fi} + + +% command to allow alteration of baselinestretch, but only if the current +% baselineskip is unity. Used to tweak the compsoc abstract and keywords line spacing. +\def\@IEEEtweakunitybaselinestretch#1{{\def\baselinestretch{1}\selectfont +\global\@tempskipa\baselineskip}\ifnum\@tempskipa=\baselineskip% +\def\baselinestretch{#1}\selectfont\fi\relax} + + +% abstract and keywords are in \small, except +% for 9pt docs in which they are in \footnotesize +% Because 9pt docs use an 8pt footnotesize, \small +% becomes a rather awkward 8.5pt +\def\@IEEEabskeysecsize{\small} +\ifx\CLASSOPTIONpt\@IEEEptsizenine + \def\@IEEEabskeysecsize{\footnotesize} +\fi + +% compsoc journals use \footnotesize, compsoc conferences use normalsize +\@IEEEcompsoconly{\def\@IEEEabskeysecsize{\footnotesize}} +\@IEEEcompsocconfonly{\def\@IEEEabskeysecsize{\small}} + + +% V1.6 have abstract and keywords strip leading spaces, pars and newlines +% so that spacing is more tightly controlled. +\def\abstract{\normalfont + \if@twocolumn + \@IEEEabskeysecsize\bfseries\textit{\abstractname}---\relax + \else + \bgroup\par\addvspace{0.5\baselineskip}\centering\vspace{-1.78ex}\@IEEEabskeysecsize\textbf{\abstractname}\par\addvspace{0.5\baselineskip}\egroup\quotation\@IEEEabskeysecsize + \fi\@IEEEgobbleleadPARNLSP} +% V1.6 The IEEE wants only 1 pica from end of abstract to introduction heading when in +% conference mode (the heading already has this much above it) +\def\endabstract{\relax\ifCLASSOPTIONconference\vspace{0ex}\else\vspace{1.34ex}\fi\par\if@twocolumn\else\endquotation\fi + \normalfont\normalsize} + +\def\IEEEkeywords{\normalfont + \if@twocolumn + \@IEEEabskeysecsize\bfseries\textit{\IEEEkeywordsname}---\relax + \else + \bgroup\par\addvspace{0.5\baselineskip}\centering\@IEEEabskeysecsize\textbf{\IEEEkeywordsname}\par\addvspace{0.5\baselineskip}\egroup\quotation\@IEEEabskeysecsize + \fi\@IEEEgobbleleadPARNLSP} +\def\endIEEEkeywords{\relax\ifCLASSOPTIONtechnote\vspace{1.34ex}\else\vspace{0.67ex}\fi + \par\if@twocolumn\else\endquotation\fi% + \normalfont\normalsize} + +% V1.7 compsoc keywords index terms +\ifCLASSOPTIONcompsoc + \ifCLASSOPTIONconference% compsoc conference +\def\abstract{\normalfont\@IEEEtweakunitybaselinestretch{1.15}\bfseries + \if@twocolumn + \@IEEEabskeysecsize\noindent\textit{\abstractname}---\relax + \else + \bgroup\par\addvspace{0.5\baselineskip}\centering\vspace{-1.78ex}\@IEEEabskeysecsize\textbf{\abstractname}\par\addvspace{0.5\baselineskip}\egroup\quotation\@IEEEabskeysecsize% + \fi\@IEEEgobbleleadPARNLSP} +\def\IEEEkeywords{\normalfont\@IEEEtweakunitybaselinestretch{1.15}\bfseries + \if@twocolumn + \@IEEEabskeysecsize\vskip 0.5\baselineskip plus 0.25\baselineskip minus 0.25\baselineskip\noindent + \textit{\IEEEkeywordsname}---\relax + \else + \bgroup\par\addvspace{0.5\baselineskip}\centering\@IEEEabskeysecsize\textbf{\IEEEkeywordsname}\par\addvspace{0.5\baselineskip}\egroup\quotation\@IEEEabskeysecsize% + \fi\@IEEEgobbleleadPARNLSP} + \else% compsoc not conference +\def\abstract{\normalfont\@IEEEtweakunitybaselinestretch{1.15}\sffamily + \if@twocolumn + \@IEEEabskeysecsize\noindent\textbf{\abstractname}---\relax + \else + \bgroup\par\addvspace{0.5\baselineskip}\centering\vspace{-1.78ex}\@IEEEabskeysecsize\textbf{\abstractname}\par\addvspace{0.5\baselineskip}\egroup\quotation\@IEEEabskeysecsize% + \fi\@IEEEgobbleleadPARNLSP} +\def\IEEEkeywords{\normalfont\@IEEEtweakunitybaselinestretch{1.15}\sffamily + \if@twocolumn + \@IEEEabskeysecsize\vskip 0.5\baselineskip plus 0.25\baselineskip minus 0.25\baselineskip\noindent + \textbf{\IEEEkeywordsname}---\relax + \else + \bgroup\par\addvspace{0.5\baselineskip}\centering\@IEEEabskeysecsize\textbf{\IEEEkeywordsname}\par\addvspace{0.5\baselineskip}\egroup\quotation\@IEEEabskeysecsize% + \fi\@IEEEgobbleleadPARNLSP} + \fi +\fi + +% V1.8 transmag keywords index terms +% no abstract name, use indentation +\ifCLASSOPTIONtransmag +\def\abstract{\normalfont\parindent 1em\relax + \if@twocolumn + \@IEEEabskeysecsize\bfseries\indent + \else + \bgroup\par\addvspace{0.5\baselineskip}\centering\vspace{-1.78ex}\@IEEEabskeysecsize + \textbf{\abstractname}\par\addvspace{0.5\baselineskip}\egroup\quotation\@IEEEabskeysecsize + \fi\@IEEEgobbleleadPARNLSP} + +\def\IEEEkeywords{\normalfont\parindent 1em\relax + \if@twocolumn + \@IEEEabskeysecsize\vspace{1\baselineskip}\bfseries\indent\textit{\IEEEkeywordsname}---\relax + \else + \bgroup\par\vspace{1\baselineskip}\centering\@IEEEabskeysecsize + \textbf{\IEEEkeywordsname}\par\addvspace{0.5\baselineskip}\egroup\quotation\@IEEEabskeysecsize + \fi\@IEEEgobbleleadPARNLSP} +\fi + + + +% gobbles all leading \, \\ and \par, upon finding first token that +% is not a \ , \\ or a \par, it ceases and returns that token +% +% used to strip leading \, \\ and \par from the input +% so that such things in the beginning of an environment will not +% affect the formatting of the text +\long\def\@IEEEgobbleleadPARNLSP#1{\let\@IEEEswallowthistoken=0% +\let\@IEEEgobbleleadPARNLSPtoken#1% +\let\@IEEEgobbleleadPARtoken=\par% +\let\@IEEEgobbleleadNLtoken=\\% +\let\@IEEEgobbleleadSPtoken=\ % +\def\@IEEEgobbleleadSPMACRO{\ }% +\ifx\@IEEEgobbleleadPARNLSPtoken\@IEEEgobbleleadPARtoken% +\let\@IEEEswallowthistoken=1% +\fi% +\ifx\@IEEEgobbleleadPARNLSPtoken\@IEEEgobbleleadNLtoken% +\let\@IEEEswallowthistoken=1% +\fi% +\ifx\@IEEEgobbleleadPARNLSPtoken\@IEEEgobbleleadSPtoken% +\let\@IEEEswallowthistoken=1% +\fi% +% a control space will come in as a macro +% when it is the last one on a line +\ifx\@IEEEgobbleleadPARNLSPtoken\@IEEEgobbleleadSPMACRO% +\let\@IEEEswallowthistoken=1% +\fi% +% if we have to swallow this token, do so and taste the next one +% else spit it out and stop gobbling +\ifx\@IEEEswallowthistoken 1\let\@IEEEnextgobbleleadPARNLSP=\@IEEEgobbleleadPARNLSP\else% +\let\@IEEEnextgobbleleadPARNLSP=#1\fi% +\@IEEEnextgobbleleadPARNLSP}% + + + + +% TITLING OF SECTIONS +\def\@IEEEsectpunct{:\ \,} % Punctuation after run-in section heading (headings which are + % part of the paragraphs), need little bit more than a single space + % spacing from section number to title +% compsoc conferences use regular period/space punctuation +\ifCLASSOPTIONcompsoc +\ifCLASSOPTIONconference +\def\@IEEEsectpunct{.\ } +\fi\fi + + +\def\@seccntformat#1{\csname the#1dis\endcsname\hskip 0.5em\relax} + +\ifCLASSOPTIONcompsoc +% compsoc journals need extra spacing +\ifCLASSOPTIONconference\else +\def\@seccntformat#1{\csname the#1dis\endcsname\hskip 1em\relax} +\fi\fi + +%v1.7 put {} after #6 to allow for some types of user font control +%and use \@@par rather than \par +\def\@sect#1#2#3#4#5#6[#7]#8{% + \ifnum #2>\c@secnumdepth + \let\@svsec\@empty + \else + \refstepcounter{#1}% + % load section label and spacer into \@svsec + \protected@edef\@svsec{\@seccntformat{#1}\relax}% + \fi% + \@tempskipa #5\relax + \ifdim \@tempskipa>\z@% tempskipa determines whether is treated as a high + \begingroup #6{\relax% or low level heading + \noindent % subsections are NOT indented + % print top level headings. \@svsec is label, #8 is heading title + % The IEEE does not block indent the section title text, it flows like normal + {\hskip #3\relax\@svsec}{\interlinepenalty \@M #8\@@par}}% + \endgroup + \addcontentsline{toc}{#1}{\ifnum #2>\c@secnumdepth\relax\else + \protect\numberline{\csname the#1\endcsname}\fi#7}% + \else % printout low level headings + % svsechd seems to swallow the trailing space, protect it with \mbox{} + % got rid of sectionmark stuff + \def\@svsechd{#6{\hskip #3\relax\@svsec #8\@IEEEsectpunct\mbox{}}% + \addcontentsline{toc}{#1}{\ifnum #2>\c@secnumdepth\relax\else + \protect\numberline{\csname the#1\endcsname}\fi#7}}% + \fi%skip down + \@xsect{#5}} + + +% section* handler +%v1.7 put {} after #4 to allow for some types of user font control +%and use \@@par rather than \par +\def\@ssect#1#2#3#4#5{\@tempskipa #3\relax + \ifdim \@tempskipa>\z@ + %\begingroup #4\@hangfrom{\hskip #1}{\interlinepenalty \@M #5\par}\endgroup + % The IEEE does not block indent the section title text, it flows like normal + \begingroup \noindent #4{\relax{\hskip #1}{\interlinepenalty \@M #5\@@par}}\endgroup + % svsechd swallows the trailing space, protect it with \mbox{} + \else \def\@svsechd{#4{\hskip #1\relax #5\@IEEEsectpunct\mbox{}}}\fi + \@xsect{#3}} + + +%% SECTION heading spacing and font +%% +% arguments are: #1 - sectiontype name +% (for \@sect) #2 - section level +% #3 - section heading indent +% #4 - top separation (absolute value used, neg indicates not to indent main text) +% If negative, make stretch parts negative too! +% #5 - (absolute value used) positive: bottom separation after heading, +% negative: amount to indent main text after heading +% Both #4 and #5 negative means to indent main text and use negative top separation +% #6 - font control +% You've got to have \normalfont\normalsize in the font specs below to prevent +% trouble when you do something like: +% \section{Note}{\ttfamily TT-TEXT} is known to ... +% The IEEE sometimes REALLY stretches the area before a section +% heading by up to about 0.5in. However, it may not be a good +% idea to let LaTeX have quite this much rubber. +\ifCLASSOPTIONconference% +% The IEEE wants section heading spacing to decrease for conference mode +\def\section{\@startsection{section}{1}{\z@}{1.5ex plus 1.5ex minus 0.5ex}% +{0.7ex plus 1ex minus 0ex}{\normalfont\normalsize\centering\scshape}}% +\def\subsection{\@startsection{subsection}{2}{\z@}{1.5ex plus 1.5ex minus 0.5ex}% +{0.7ex plus .5ex minus 0ex}{\normalfont\normalsize\itshape}}% +\else % for journals +\def\section{\@startsection{section}{1}{\z@}{3.0ex plus 1.5ex minus 1.5ex}% V1.6 3.0ex from 3.5ex +{0.7ex plus 1ex minus 0ex}{\normalfont\normalsize\centering\scshape}}% +\def\subsection{\@startsection{subsection}{2}{\z@}{3.5ex plus 1.5ex minus 1.5ex}% +{0.7ex plus .5ex minus 0ex}{\normalfont\normalsize\itshape}}% +\fi + +% for both journals and conferences +% decided to put in a little rubber above the section, might help somebody +\def\subsubsection{\@startsection{subsubsection}{3}{\parindent}{0ex plus 0.1ex minus 0.1ex}% +{0ex}{\normalfont\normalsize\itshape}}% +\def\paragraph{\@startsection{paragraph}{4}{2\parindent}{0ex plus 0.1ex minus 0.1ex}% +{0ex}{\normalfont\normalsize\itshape}}% + + +% compsoc +\ifCLASSOPTIONcompsoc +\ifCLASSOPTIONconference +% compsoc conference +\def\section{\@startsection{section}{1}{\z@}{1\baselineskip plus 0.25\baselineskip minus 0.25\baselineskip}% +{1\baselineskip plus 0.25\baselineskip minus 0.25\baselineskip}{\normalfont\large\bfseries}}% +\def\subsection{\@startsection{subsection}{2}{\z@}{1\baselineskip plus 0.25\baselineskip minus 0.25\baselineskip}% +{1\baselineskip plus 0.25\baselineskip minus 0.25\baselineskip}{\normalfont\sublargesize\bfseries}}% +\def\subsubsection{\@startsection{subsubsection}{3}{\z@}{1\baselineskip plus 0.25\baselineskip minus 0.25\baselineskip}% +{0ex}{\normalfont\normalsize\bfseries}}% +\def\paragraph{\@startsection{paragraph}{4}{2\parindent}{0ex plus 0.1ex minus 0.1ex}% +{0ex}{\normalfont\normalsize}}% +\else% compsoc journals +% use negative top separation as compsoc journals do not indent paragraphs after section titles +\def\section{\@startsection{section}{1}{\z@}{-3.5ex plus -2ex minus -1.5ex}% +{0.7ex plus 1ex minus 0ex}{\normalfont\sublargesize\sffamily\bfseries\scshape}}% +% Note that subsection and smaller may not be correct for the Computer Society, +% I have to look up an example. +\def\subsection{\@startsection{subsection}{2}{\z@}{-3.5ex plus -1.5ex minus -1.5ex}% +{0.7ex plus .5ex minus 0ex}{\normalfont\normalsize\sffamily\bfseries}}% +\def\subsubsection{\@startsection{subsubsection}{3}{\z@}{-2.5ex plus -1ex minus -1ex}% +{0.5ex plus 0.5ex minus 0ex}{\normalfont\normalsize\sffamily\itshape}}% +\def\paragraph{\@startsection{paragraph}{4}{2\parindent}{-0ex plus -0.1ex minus -0.1ex}% +{0ex}{\normalfont\normalsize}}% +\fi\fi + +% transmag +\ifCLASSOPTIONtransmag +\def\subsection{\@startsection{subsection}{2}{0.75\parindent}{3.5ex plus 1.5ex minus 1.5ex}% +{0.7ex plus .5ex minus 0ex}{\normalfont\normalsize\itshape}}% +\def\subsubsection{\@startsection{subsubsection}{3}{1.25\parindent}{0.1ex plus 0.1ex minus 0.1ex}% +{0.1ex}{\normalfont\normalsize\itshape}}% +\fi + + +% V1.8a provide for a raised line Introduction section for use with Computer +% Society papers. We have to remove any spacing glue after the section +% heading and then remove the blank line for the new paragraph after it. +% LaTeX's section handler alters \everypar and we need to propogate those +% changes outside of the \parbox lest there be spacing problems at the top +% of the next section. +\def\IEEEraisesectionheading#1{\noindent\raisebox{1.5\baselineskip}[0pt][0pt]{\parbox[b]{\columnwidth}{#1\unskip\global\everypar=\everypar}}\vspace{-1\baselineskip}\vspace{-\parskip}\par} + + + +%% ENVIRONMENTS +% "box" symbols at end of proofs +\def\IEEEQEDclosed{\mbox{\rule[0pt]{1.3ex}{1.3ex}}} % for a filled box +% V1.6 some journals use an open box instead that will just fit around a closed one +\def\IEEEQEDopen{{\setlength{\fboxsep}{0pt}\setlength{\fboxrule}{0.2pt}\fbox{\rule[0pt]{0pt}{1.3ex}\rule[0pt]{1.3ex}{0pt}}}} +\ifCLASSOPTIONcompsoc +\def\IEEEQED{\IEEEQEDopen} % default to open for compsoc +\else +\def\IEEEQED{\IEEEQEDclosed} % otherwise default to closed +\fi + +%V1.8 flag to indicate if QED symbol is to be shown +\newif\if@IEEEQEDshow \@IEEEQEDshowtrue +\def\IEEEproofindentspace{2\parindent}% V1.8 allow user to change indentation amount if desired +% v1.7 name change to avoid namespace collision with amsthm. Also add support +% for an optional argument. +\def\IEEEproof{\@ifnextchar[{\@IEEEproof}{\@IEEEproof[\IEEEproofname]}} +\def\@IEEEproof[#1]{\@IEEEQEDshowtrue\par\noindent\hspace{\IEEEproofindentspace}{\itshape #1: }} +\def\endIEEEproof{\if@IEEEQEDshow\hspace*{\fill}\nobreakspace\IEEEQED\fi\par} +% qedhere for equation environments, similar to AMS \qedhere +\def\IEEEQEDhereeqn{\global\@IEEEQEDshowfalse\eqno\let\eqno\relax\let\leqno\relax + \let\veqno\relax\hbox{\IEEEQED}} +% IEEE style qedhere for IEEEeqnarray and other environments +\def\IEEEQEDhere{\global\@IEEEQEDshowfalse\IEEEQED} +% command to disable QED at end of IEEEproof +\def\IEEEQEDoff{\global\@IEEEQEDshowfalse} + + +%\itemindent is set to \z@ by list, so define new temporary variable +\newdimen\@IEEEtmpitemindent + +\ifCLASSOPTIONcompsoc +% V1.8a compsoc uses bold theorem titles, a period instead of a colon, vertical spacing, and hanging indentation +% V1.8 allow long theorem names to break across lines. +% Thanks to Miquel Payaro for reporting this. +\def\@begintheorem#1#2{\@IEEEtmpitemindent\itemindent\relax + \topsep 0.2\@IEEEnormalsizeunitybaselineskip plus 0.26\@IEEEnormalsizeunitybaselineskip minus 0.05\@IEEEnormalsizeunitybaselineskip + \rmfamily\trivlist\hangindent\parindent% + \item[]\textit{\bfseries\noindent #1\ #2.} \itemindent\@IEEEtmpitemindent\relax} +\def\@opargbegintheorem#1#2#3{\@IEEEtmpitemindent\itemindent\relax +\topsep 0.2\@IEEEnormalsizeunitybaselineskip plus 0.26\@IEEEnormalsizeunitybaselineskip minus 0.05\@IEEEnormalsizeunitybaselineskip +\rmfamily\trivlist\hangindent\parindent% +% V1.6 The IEEE is back to using () around theorem names which are also in italics +% Thanks to Christian Peel for reporting this. + \item[]\textit{\bfseries\noindent #1\ #2\ (#3).} \itemindent\@IEEEtmpitemindent\relax} +% V1.7 remove bogus \unskip that caused equations in theorems to collide with +% lines below. +\def\@endtheorem{\endtrivlist\vskip 0.25\@IEEEnormalsizeunitybaselineskip plus 0.26\@IEEEnormalsizeunitybaselineskip minus 0.05\@IEEEnormalsizeunitybaselineskip} +\else +% +% noncompsoc +% +% V1.8 allow long theorem names to break across lines. +% Thanks to Miquel Payaro for reporting this. +\def\@begintheorem#1#2{\@IEEEtmpitemindent\itemindent\relax\topsep 0pt\rmfamily\trivlist% + \item[]\textit{\indent #1\ #2:} \itemindent\@IEEEtmpitemindent\relax} +\def\@opargbegintheorem#1#2#3{\@IEEEtmpitemindent\itemindent\relax\topsep 0pt\rmfamily \trivlist% +% V1.6 The IEEE is back to using () around theorem names which are also in italics +% Thanks to Christian Peel for reporting this. + \item[]\textit{\indent #1\ #2\ (#3):} \itemindent\@IEEEtmpitemindent\relax} +% V1.7 remove bogus \unskip that caused equations in theorems to collide with +% lines below. +\def\@endtheorem{\endtrivlist} +\fi + + + +% V1.6 +% display command for the section the theorem is in - so that \thesection +% is not used as this will be in Roman numerals when we want arabic. +% LaTeX2e uses \def\@thmcounter#1{\noexpand\arabic{#1}} for the theorem number +% (second part) display and \def\@thmcountersep{.} as a separator. +% V1.7 intercept calls to the section counter and reroute to \@IEEEthmcounterinsection +% to allow \appendix(ices} to override as needed. +% +% special handler for sections, allows appendix(ices) to override +\gdef\@IEEEthmcounterinsection#1{\arabic{#1}} +% string macro +\edef\@IEEEstringsection{section} + +% redefine the #1#2[#3] form of newtheorem to use a hook to \@IEEEthmcounterinsection +% if section in_counter is used +\def\@xnthm#1#2[#3]{% + \expandafter\@ifdefinable\csname #1\endcsname + {\@definecounter{#1}\@newctr{#1}[#3]% + \edef\@IEEEstringtmp{#3} + \ifx\@IEEEstringtmp\@IEEEstringsection + \expandafter\xdef\csname the#1\endcsname{% + \noexpand\@IEEEthmcounterinsection{#3}\@thmcountersep + \@thmcounter{#1}}% + \else + \expandafter\xdef\csname the#1\endcsname{% + \expandafter\noexpand\csname the#3\endcsname \@thmcountersep + \@thmcounter{#1}}% + \fi + \global\@namedef{#1}{\@thm{#1}{#2}}% + \global\@namedef{end#1}{\@endtheorem}}} + + + +%% SET UP THE DEFAULT PAGESTYLE +\pagestyle{headings} +\pagenumbering{arabic} + +% normally the page counter starts at 1 +\setcounter{page}{1} +% however, for peerreview the cover sheet is page 0 or page -1 +% (for duplex printing) +\ifCLASSOPTIONpeerreview + \if@twoside + \setcounter{page}{-1} + \else + \setcounter{page}{0} + \fi +\fi + +% standard book class behavior - let bottom line float up and down as +% needed when single sided +\ifCLASSOPTIONtwoside\else\raggedbottom\fi +% if two column - turn on twocolumn, allow word spacings to stretch more and +% enforce a rigid position for the last lines +\ifCLASSOPTIONtwocolumn +% the peer review option delays invoking twocolumn + \ifCLASSOPTIONpeerreview\else + \twocolumn + \fi +\sloppy +\flushbottom +\fi + + + + +% \APPENDIX and \APPENDICES definitions + +% This is the \@ifmtarg command from the LaTeX ifmtarg package +% by Peter Wilson (CUA) and Donald Arseneau +% \@ifmtarg is used to determine if an argument to a command +% is present or not. +% For instance: +% \@ifmtarg{#1}{\typeout{empty}}{\typeout{has something}} +% \@ifmtarg is used with our redefined \section command if +% \appendices is invoked. +% The command \section will behave slightly differently depending +% on whether the user specifies a title: +% \section{My appendix title} +% or not: +% \section{} +% This way, we can eliminate the blank lines where the title +% would be, and the unneeded : after Appendix in the table of +% contents +\begingroup +\catcode`\Q=3 +\long\gdef\@ifmtarg#1{\@xifmtarg#1QQ\@secondoftwo\@firstoftwo\@nil} +\long\gdef\@xifmtarg#1#2Q#3#4#5\@nil{#4} +\endgroup +% end of \@ifmtarg defs + + +% V1.7 +% command that allows the one time saving of the original definition +% of section to \@IEEEappendixsavesection for \appendix or \appendices +% we don't save \section here as it may be redefined later by other +% packages (hyperref.sty, etc.) +\def\@IEEEsaveoriginalsectiononce{\let\@IEEEappendixsavesection\section +\let\@IEEEsaveoriginalsectiononce\relax} + +% neat trick to grab and process the argument from \section{argument} +% we process differently if the user invoked \section{} with no +% argument (title) +% note we reroute the call to the old \section* +\def\@IEEEprocessthesectionargument#1{% +\@ifmtarg{#1}{% +\@IEEEappendixsavesection*{\appendixname\nobreakspace\thesectiondis}% +\addcontentsline{toc}{section}{\appendixname\nobreakspace\thesection}}{% +\@IEEEappendixsavesection*{\appendixname\nobreakspace\thesectiondis\\* #1}% +\addcontentsline{toc}{section}{\appendixname\nobreakspace\thesection: #1}}} + +% we use this if the user calls \section{} after +% \appendix-- which has no meaning. So, we ignore the +% command and its argument. Then, warn the user. +\def\@IEEEdestroythesectionargument#1{\typeout{** WARNING: Ignoring useless +\protect\section\space in Appendix (line \the\inputlineno).}} + + +% remember \thesection forms will be displayed in \ref calls +% and in the Table of Contents. +% The \sectiondis form is used in the actual heading itself + +% appendix command for one single appendix +% normally has no heading. However, if you want a +% heading, you can do so via the optional argument: +% \appendix[Optional Heading] +\def\appendix{\relax} +\renewcommand{\appendix}[1][]{\@IEEEsaveoriginalsectiononce\par + % v1.6 keep hyperref's identifiers unique + \gdef\theHsection{Appendix.A}% + % v1.6 adjust hyperref's string name for the section + \xdef\Hy@chapapp{appendix}% + \setcounter{section}{0}% + \setcounter{subsection}{0}% + \setcounter{subsubsection}{0}% + \setcounter{paragraph}{0}% + \gdef\thesection{A}% + \gdef\thesectiondis{}% + \gdef\thesubsection{\Alph{subsection}}% + \gdef\@IEEEthmcounterinsection##1{A} + \refstepcounter{section}% update the \ref counter + \@ifmtarg{#1}{\@IEEEappendixsavesection*{\appendixname}% + \addcontentsline{toc}{section}{\appendixname}}{% + \@IEEEappendixsavesection*{\appendixname\nobreakspace\\* #1}% + \addcontentsline{toc}{section}{\appendixname: #1}}% + % redefine \section command for appendix + % leave \section* as is + \def\section{\@ifstar{\@IEEEappendixsavesection*}{% + \@IEEEdestroythesectionargument}}% throw out the argument + % of the normal form +} + + + +% appendices command for multiple appendices +% user then calls \section with an argument (possibly empty) to +% declare the individual appendices +\def\appendices{\@IEEEsaveoriginalsectiononce\par + % v1.6 keep hyperref's identifiers unique + \gdef\theHsection{Appendix.\Alph{section}}% + % v1.6 adjust hyperref's string name for the section + \xdef\Hy@chapapp{appendix}% + \setcounter{section}{-1}% we want \refstepcounter to use section 0 + \setcounter{subsection}{0}% + \setcounter{subsubsection}{0}% + \setcounter{paragraph}{0}% + \ifCLASSOPTIONromanappendices% + \gdef\thesection{\Roman{section}}% + \gdef\thesectiondis{\Roman{section}}% + \@IEEEcompsocconfonly{\gdef\thesectiondis{\Roman{section}.}}% + \gdef\@IEEEthmcounterinsection##1{A\arabic{##1}} + \else% + \gdef\thesection{\Alph{section}}% + \gdef\thesectiondis{\Alph{section}}% + \@IEEEcompsocconfonly{\gdef\thesectiondis{\Alph{section}.}}% + \gdef\@IEEEthmcounterinsection##1{\Alph{##1}} + \fi% + \refstepcounter{section}% update the \ref counter + \setcounter{section}{0}% NEXT \section will be the FIRST appendix + % redefine \section command for appendices + % leave \section* as is + \def\section{\@ifstar{\@IEEEappendixsavesection*}{% process the *-form + \refstepcounter{section}% or is a new section so, + \@IEEEprocessthesectionargument}}% process the argument + % of the normal form +} + + + +% V1.7 compoc uses nonbold drop cap and small caps word style +\ifCLASSOPTIONcompsoc + \def\IEEEPARstartFONTSTYLE{\mdseries} + \def\IEEEPARstartWORDFONTSTYLE{\scshape} + \def\IEEEPARstartWORDCAPSTYLE{\relax} +\fi +% +% +% \IEEEPARstart +% Definition for the big two line drop cap letter at the beginning of the +% first paragraph of journal papers. The first argument is the first letter +% of the first word, the second argument is the remaining letters of the +% first word which will be rendered in upper case. +% In V1.6 this has been completely rewritten to: +% +% 1. no longer have problems when the user begins an environment +% within the paragraph that uses \IEEEPARstart. +% 2. auto-detect and use the current font family +% 3. revise handling of the space at the end of the first word so that +% interword glue will now work as normal. +% 4. produce correctly aligned edges for the (two) indented lines. +% +% We generalize things via control macros - playing with these is fun too. +% +% V1.7 added more control macros to make it easy for IEEEtrantools.sty users +% to change the font style. +% +% the number of lines that are indented to clear it +% may need to increase if using decenders +\providecommand{\IEEEPARstartDROPLINES}{2} +% minimum number of lines left on a page to allow a \@IEEEPARstart +% Does not take into consideration rubber shrink, so it tends to +% be overly cautious +\providecommand{\IEEEPARstartMINPAGELINES}{2} +% V1.7 the height of the drop cap is adjusted to match the height of this text +% in the current font (when \IEEEPARstart is called). +\providecommand{\IEEEPARstartHEIGHTTEXT}{T} +% the depth the letter is lowered below the baseline +% the height (and size) of the letter is determined by the sum +% of this value and the height of the \IEEEPARstartHEIGHTTEXT in the current +% font. It is a good idea to set this value in terms of the baselineskip +% so that it can respond to changes therein. +\providecommand{\IEEEPARstartDROPDEPTH}{1.1\baselineskip} +% V1.7 the font the drop cap will be rendered in, +% can take zero or one argument. +\providecommand{\IEEEPARstartFONTSTYLE}{\bfseries} +% V1.7 any additional, non-font related commands needed to modify +% the drop cap letter, can take zero or one argument. +\providecommand{\IEEEPARstartCAPSTYLE}{\MakeUppercase} +% V1.7 the font that will be used to render the rest of the word, +% can take zero or one argument. +\providecommand{\IEEEPARstartWORDFONTSTYLE}{\relax} +% V1.7 any additional, non-font related commands needed to modify +% the rest of the word, can take zero or one argument. +\providecommand{\IEEEPARstartWORDCAPSTYLE}{\MakeUppercase} +% This is the horizontal separation distance from the drop letter to the main text. +% Lengths that depend on the font (e.g., ex, em, etc.) will be referenced +% to the font that is active when \IEEEPARstart is called. +\providecommand{\IEEEPARstartSEP}{0.15em} +% V1.7 horizontal offset applied to the left of the drop cap. +\providecommand{\IEEEPARstartHOFFSET}{0em} +% V1.7 Italic correction command applied at the end of the drop cap. +\providecommand{\IEEEPARstartITLCORRECT}{\/} + +% width of the letter output, set globally. Can be used in \IEEEPARstartSEP +% or \IEEEPARstartHOFFSET, but not the height lengths. +\newdimen\IEEEPARstartletwidth +\IEEEPARstartletwidth 0pt\relax + +% definition of \IEEEPARstart +% THIS IS A CONTROLLED SPACING AREA, DO NOT ALLOW SPACES WITHIN THESE LINES +% +% The token \@IEEEPARstartfont will be globally defined after the first use +% of \IEEEPARstart and will be a font command which creates the big letter +% The first argument is the first letter of the first word and the second +% argument is the rest of the first word(s). +\def\IEEEPARstart#1#2{\par{% +% if this page does not have enough space, break it and lets start +% on a new one +\@IEEEtranneedspace{\IEEEPARstartMINPAGELINES\baselineskip}{\relax}% +% V1.7 move this up here in case user uses \textbf for \IEEEPARstartFONTSTYLE +% which uses command \leavevmode which causes an unwanted \indent to be issued +\noindent +% calculate the desired height of the big letter +% it extends from the top of \IEEEPARstartHEIGHTTEXT in the current font +% down to \IEEEPARstartDROPDEPTH below the current baseline +\settoheight{\@IEEEtrantmpdimenA}{\IEEEPARstartHEIGHTTEXT}% +\addtolength{\@IEEEtrantmpdimenA}{\IEEEPARstartDROPDEPTH}% +% extract the name of the current font in bold +% and place it in \@IEEEPARstartFONTNAME +\def\@IEEEPARstartGETFIRSTWORD##1 ##2\relax{##1}% +{\IEEEPARstartFONTSTYLE{\selectfont\edef\@IEEEPARstartFONTNAMESPACE{\fontname\font\space}% +\xdef\@IEEEPARstartFONTNAME{\expandafter\@IEEEPARstartGETFIRSTWORD\@IEEEPARstartFONTNAMESPACE\relax}}}% +% define a font based on this name with a point size equal to the desired +% height of the drop letter +\font\@IEEEPARstartsubfont\@IEEEPARstartFONTNAME\space at \@IEEEtrantmpdimenA\relax% +% save this value as a counter (integer) value (sp points) +\@IEEEtrantmpcountA=\@IEEEtrantmpdimenA% +% now get the height of the actual letter produced by this font size +\settoheight{\@IEEEtrantmpdimenB}{\@IEEEPARstartsubfont\IEEEPARstartCAPSTYLE{#1}}% +% If something bogus happens like the first argument is empty or the +% current font is strange, do not allow a zero height. +\ifdim\@IEEEtrantmpdimenB=0pt\relax% +\typeout{** WARNING: IEEEPARstart drop letter has zero height! (line \the\inputlineno)}% +\typeout{ Forcing the drop letter font size to 10pt.}% +\@IEEEtrantmpdimenB=10pt% +\fi% +% and store it as a counter +\@IEEEtrantmpcountB=\@IEEEtrantmpdimenB% +% Since a font size doesn't exactly correspond to the height of the capital +% letters in that font, the actual height of the letter, \@IEEEtrantmpcountB, +% will be less than that desired, \@IEEEtrantmpcountA +% we need to raise the font size, \@IEEEtrantmpdimenA +% by \@IEEEtrantmpcountA / \@IEEEtrantmpcountB +% But, TeX doesn't have floating point division, so we have to use integer +% division. Hence the use of the counters. +% We need to reduce the denominator so that the loss of the remainder will +% have minimal affect on the accuracy of the result +\divide\@IEEEtrantmpcountB by 200% +\divide\@IEEEtrantmpcountA by \@IEEEtrantmpcountB% +% Then reequalize things when we use TeX's ability to multiply by +% floating point values +\@IEEEtrantmpdimenB=0.005\@IEEEtrantmpdimenA% +\multiply\@IEEEtrantmpdimenB by \@IEEEtrantmpcountA% +% \@IEEEPARstartfont is globaly set to the calculated font of the big letter +% We need to carry this out of the local calculation area to to create the +% big letter. +\global\font\@IEEEPARstartfont\@IEEEPARstartFONTNAME\space at \@IEEEtrantmpdimenB% +% Now set \@IEEEtrantmpdimenA to the width of the big letter +% We need to carry this out of the local calculation area to set the +% hanging indent +\settowidth{\global\@IEEEtrantmpdimenA}{\@IEEEPARstartfont +\IEEEPARstartCAPSTYLE{#1\IEEEPARstartITLCORRECT}}}% +% end of the isolated calculation environment +\global\IEEEPARstartletwidth\@IEEEtrantmpdimenA\relax% +% add in the extra clearance we want +\advance\@IEEEtrantmpdimenA by \IEEEPARstartSEP\relax% +% add in the optional offset +\advance\@IEEEtrantmpdimenA by \IEEEPARstartHOFFSET\relax% +% V1.7 don't allow negative offsets to produce negative hanging indents +\@IEEEtrantmpdimenB\@IEEEtrantmpdimenA +\ifnum\@IEEEtrantmpdimenB < 0 \@IEEEtrantmpdimenB 0pt\fi +% \@IEEEtrantmpdimenA has the width of the big letter plus the +% separation space and \@IEEEPARstartfont is the font we need to use +% Now, we make the letter and issue the hanging indent command +% The letter is placed in a box of zero width and height so that other +% text won't be displaced by it. +\hangindent\@IEEEtrantmpdimenB\hangafter=-\IEEEPARstartDROPLINES% +\makebox[0pt][l]{\hspace{-\@IEEEtrantmpdimenA}% +\raisebox{-\IEEEPARstartDROPDEPTH}[0pt][0pt]{\hspace{\IEEEPARstartHOFFSET}% +\@IEEEPARstartfont\IEEEPARstartCAPSTYLE{#1\IEEEPARstartITLCORRECT}% +\hspace{\IEEEPARstartSEP}}}% +{\IEEEPARstartWORDFONTSTYLE{\IEEEPARstartWORDCAPSTYLE{\selectfont#2}}}} + + + + +% determines if the space remaining on a given page is equal to or greater +% than the specified space of argument one +% if not, execute argument two (only if the remaining space is greater than zero) +% and issue a \newpage +% +% example: \@IEEEtranneedspace{2in}{\vfill} +% +% Does not take into consideration rubber shrinkage, so it tends to +% be overly cautious +% Based on an example posted by Donald Arseneau +% Note this macro uses \@IEEEtrantmpdimenB internally for calculations, +% so DO NOT PASS \@IEEEtrantmpdimenB to this routine +% if you need a dimen register, import with \@IEEEtrantmpdimenA instead +\def\@IEEEtranneedspace#1#2{\penalty-100\begingroup%shield temp variable +\@IEEEtrantmpdimenB\pagegoal\advance\@IEEEtrantmpdimenB-\pagetotal% space left +\ifdim #1>\@IEEEtrantmpdimenB\relax% not enough space left +\ifdim\@IEEEtrantmpdimenB>\z@\relax #2\fi% +\newpage% +\fi\endgroup} + + + +% IEEEbiography ENVIRONMENT +% Allows user to enter biography leaving place for picture (adapts to font size) +% As of V1.5, a new optional argument allows you to have a real graphic! +% V1.5 and later also fixes the "colliding biographies" which could happen when a +% biography's text was shorter than the space for the photo. +% MDS 7/2001 +% V1.6 prevent multiple biographies from making multiple TOC entries +\newif\if@IEEEbiographyTOCentrynotmade +\global\@IEEEbiographyTOCentrynotmadetrue + +% biography counter so hyperref can jump directly to the biographies +% and not just the previous section +\newcounter{IEEEbiography} +\setcounter{IEEEbiography}{0} + +% photo area size +\def\@IEEEBIOphotowidth{1.0in} % width of the biography photo area +\def\@IEEEBIOphotodepth{1.25in} % depth (height) of the biography photo area +% area cleared for photo +\def\@IEEEBIOhangwidth{1.14in} % width cleared for the biography photo area +\def\@IEEEBIOhangdepth{1.25in} % depth cleared for the biography photo area + % actual depth will be a multiple of + % \baselineskip, rounded up +\def\@IEEEBIOskipN{4\baselineskip}% nominal value of the vskip above the biography + +\newenvironment{IEEEbiography}[2][]{\normalfont\@IEEEcompsoconly{\sffamily}\footnotesize% +\unitlength 1in\parskip=0pt\par\parindent 1em\interlinepenalty500% +% we need enough space to support the hanging indent +% the nominal value of the spacer +% and one extra line for good measure +\@IEEEtrantmpdimenA=\@IEEEBIOhangdepth% +\advance\@IEEEtrantmpdimenA by \@IEEEBIOskipN% +\advance\@IEEEtrantmpdimenA by 1\baselineskip% +% if this page does not have enough space, break it and lets start +% with a new one +\@IEEEtranneedspace{\@IEEEtrantmpdimenA}{\relax}% +% nominal spacer can strech, not shrink use 1fil so user can out stretch with \vfill +\vskip \@IEEEBIOskipN plus 1fil minus 0\baselineskip% +% the default box for where the photo goes +\def\@IEEEtempbiographybox{{\setlength{\fboxsep}{0pt}\framebox{% +\begin{minipage}[b][\@IEEEBIOphotodepth][c]{\@IEEEBIOphotowidth}\centering PLACE\\ PHOTO\\ HERE \end{minipage}}}}% +% +% detect if the optional argument was supplied, this requires the +% \@ifmtarg command as defined in the appendix section above +% and if so, override the default box with what they want +\@ifmtarg{#1}{\relax}{\def\@IEEEtempbiographybox{\mbox{\begin{minipage}[b][\@IEEEBIOphotodepth][c]{\@IEEEBIOphotowidth}% +\centering% +#1% +\end{minipage}}}}% end if optional argument supplied +% Make an entry into the table of contents only if we have not done so before +\if@IEEEbiographyTOCentrynotmade% +% link labels to the biography counter so hyperref will jump +% to the biography, not the previous section +\setcounter{IEEEbiography}{-1}% +\refstepcounter{IEEEbiography}% +\addcontentsline{toc}{section}{Biographies}% +\global\@IEEEbiographyTOCentrynotmadefalse% +\fi% +% one more biography +\refstepcounter{IEEEbiography}% +% Make an entry for this name into the table of contents +\addcontentsline{toc}{subsection}{#2}% +% V1.6 properly handle if a new paragraph should occur while the +% hanging indent is still active. Do this by redefining \par so +% that it will not start a new paragraph. (But it will appear to the +% user as if it did.) Also, strip any leading pars, newlines, or spaces. +\let\@IEEEBIOORGparCMD=\par% save the original \par command +\edef\par{\hfil\break\indent}% the new \par will not be a "real" \par +\settoheight{\@IEEEtrantmpdimenA}{\@IEEEtempbiographybox}% get height of biography box +\@IEEEtrantmpdimenB=\@IEEEBIOhangdepth% +\@IEEEtrantmpcountA=\@IEEEtrantmpdimenB% countA has the hang depth +\divide\@IEEEtrantmpcountA by \baselineskip% calculates lines needed to produce the hang depth +\advance\@IEEEtrantmpcountA by 1% ensure we overestimate +% set the hanging indent +\hangindent\@IEEEBIOhangwidth% +\hangafter-\@IEEEtrantmpcountA% +% reference the top of the photo area to the top of a capital T +\settoheight{\@IEEEtrantmpdimenB}{\mbox{T}}% +% set the photo box, give it zero width and height so as not to disturb anything +\noindent\makebox[0pt][l]{\hspace{-\@IEEEBIOhangwidth}\raisebox{\@IEEEtrantmpdimenB}[0pt][0pt]{% +\raisebox{-\@IEEEBIOphotodepth}[0pt][0pt]{\@IEEEtempbiographybox}}}% +% now place the author name and begin the bio text +\noindent\textbf{#2\ }\@IEEEgobbleleadPARNLSP}{\relax\let\par=\@IEEEBIOORGparCMD\par% +% 7/2001 V1.5 detect when the biography text is shorter than the photo area +% and pad the unused area - preventing a collision from the next biography entry +% MDS +\ifnum \prevgraf <\@IEEEtrantmpcountA\relax% detect when the biography text is shorter than the photo + \advance\@IEEEtrantmpcountA by -\prevgraf% calculate how many lines we need to pad + \advance\@IEEEtrantmpcountA by -1\relax% we compensate for the fact that we indented an extra line + \@IEEEtrantmpdimenA=\baselineskip% calculate the length of the padding + \multiply\@IEEEtrantmpdimenA by \@IEEEtrantmpcountA% + \noindent\rule{0pt}{\@IEEEtrantmpdimenA}% insert an invisible support strut +\fi% +\par\normalfont} + + + +% V1.6 +% added biography without a photo environment +\newenvironment{IEEEbiographynophoto}[1]{% +% Make an entry into the table of contents only if we have not done so before +\if@IEEEbiographyTOCentrynotmade% +% link labels to the biography counter so hyperref will jump +% to the biography, not the previous section +\setcounter{IEEEbiography}{-1}% +\refstepcounter{IEEEbiography}% +\addcontentsline{toc}{section}{Biographies}% +\global\@IEEEbiographyTOCentrynotmadefalse% +\fi% +% one more biography +\refstepcounter{IEEEbiography}% +% Make an entry for this name into the table of contents +\addcontentsline{toc}{subsection}{#1}% +\normalfont\@IEEEcompsoconly{\sffamily}\footnotesize\interlinepenalty500% +\vskip 4\baselineskip plus 1fil minus 0\baselineskip% +\parskip=0pt\par% +\noindent\textbf{#1\ }\@IEEEgobbleleadPARNLSP}{\relax\par\normalfont} + + +% provide the user with some old font commands +% got this from article.cls +\DeclareOldFontCommand{\rm}{\normalfont\rmfamily}{\mathrm} +\DeclareOldFontCommand{\sf}{\normalfont\sffamily}{\mathsf} +\DeclareOldFontCommand{\tt}{\normalfont\ttfamily}{\mathtt} +\DeclareOldFontCommand{\bf}{\normalfont\bfseries}{\mathbf} +\DeclareOldFontCommand{\it}{\normalfont\itshape}{\mathit} +\DeclareOldFontCommand{\sl}{\normalfont\slshape}{\@nomath\sl} +\DeclareOldFontCommand{\sc}{\normalfont\scshape}{\@nomath\sc} +\DeclareRobustCommand*\cal{\@fontswitch\relax\mathcal} +\DeclareRobustCommand*\mit{\@fontswitch\relax\mathnormal} + + +% SPECIAL PAPER NOTICE COMMANDS +% +% holds the special notice text +\def\@IEEEspecialpapernotice{\relax} + +% for special papers, like invited papers, the user can do: +% \IEEEspecialpapernotice{(Invited Paper)} before \maketitle +\def\IEEEspecialpapernotice#1{\ifCLASSOPTIONconference% +\def\@IEEEspecialpapernotice{{\sublargesize\textit{#1}\vspace*{1em}}}% +\else% +\def\@IEEEspecialpapernotice{{\\*[1.5ex]\sublargesize\textit{#1}}\vspace*{-2ex}}% +\fi} + + + + +% PUBLISHER ID COMMANDS +% to insert a publisher's ID footer +% V1.6 \IEEEpubid has been changed so that the change in page size and style +% occurs in \maketitle. \IEEEpubid must now be issued prior to \maketitle +% use \IEEEpubidadjcol as before - in the second column of the title page +% These changes allow \maketitle to take the reduced page height into +% consideration when dynamically setting the space between the author +% names and the maintext. +% +% the amount the main text is pulled up to make room for the +% publisher's ID footer +% The IEEE uses about 1.3\baselineskip for journals, +% dynamic title spacing will clean up the fraction +\def\@IEEEpubidpullup{1.3\baselineskip} +\ifCLASSOPTIONtechnote +% for technotes it must be an integer of baselineskip as there can be no +% dynamic title spacing for two column mode technotes (the title is in the +% in first column) and we should maintain an integer number of lines in the +% second column +% There are some examples (such as older issues of "Transactions on +% Information Theory") in which the IEEE really pulls the text off the ID for +% technotes - about 0.55in (or 4\baselineskip). We'll use 2\baselineskip +% and call it even. +\def\@IEEEpubidpullup{2\baselineskip} +\fi + +% V1.7 compsoc does not use a pullup +\ifCLASSOPTIONcompsoc +\def\@IEEEpubidpullup{0pt} +\fi + +% holds the ID text +\def\@IEEEpubid{\relax} + +% flag so \maketitle can tell if \IEEEpubid was called +\newif\if@IEEEusingpubid +\global\@IEEEusingpubidfalse +% issue this command in the page to have the ID at the bottom +% V1.6 use before \maketitle +\def\IEEEpubid#1{\def\@IEEEpubid{#1}\global\@IEEEusingpubidtrue} + + +% command which will pull up (shorten) the column it is executed in +% to make room for the publisher ID. Place in the second column of +% the title page when using \IEEEpubid +% Is smart enough not to do anything when in single column text or +% if the user hasn't called \IEEEpubid +% currently needed in for the second column of a page with the +% publisher ID. If not needed in future releases, please provide this +% command and define it as \relax for backward compatibility +% v1.6b do not allow command to operate if the peer review option has been +% selected because \IEEEpubidadjcol will not be on the cover page. +% V1.7 do nothing if compsoc +\def\IEEEpubidadjcol{\ifCLASSOPTIONcompsoc\else\ifCLASSOPTIONpeerreview\else +\if@twocolumn\if@IEEEusingpubid\enlargethispage{-\@IEEEpubidpullup}\fi\fi\fi\fi} + +% Special thanks to Peter Wilson, Daniel Luecking, and the other +% gurus at comp.text.tex, for helping me to understand how best to +% implement the IEEEpubid command in LaTeX. + + + +%% Lockout some commands under various conditions + +% general purpose bit bucket +\newsavebox{\@IEEEtranrubishbin} + +% flags to prevent multiple warning messages +\newif\if@IEEEWARNthanks +\newif\if@IEEEWARNIEEEPARstart +\newif\if@IEEEWARNIEEEbiography +\newif\if@IEEEWARNIEEEbiographynophoto +\newif\if@IEEEWARNIEEEpubid +\newif\if@IEEEWARNIEEEpubidadjcol +\newif\if@IEEEWARNIEEEmembership +\newif\if@IEEEWARNIEEEaftertitletext +\@IEEEWARNthankstrue +\@IEEEWARNIEEEPARstarttrue +\@IEEEWARNIEEEbiographytrue +\@IEEEWARNIEEEbiographynophototrue +\@IEEEWARNIEEEpubidtrue +\@IEEEWARNIEEEpubidadjcoltrue +\@IEEEWARNIEEEmembershiptrue +\@IEEEWARNIEEEaftertitletexttrue + + +%% Lockout some commands when in various modes, but allow them to be restored if needed +%% +% save commands which might be locked out +% so that the user can later restore them if needed +\let\@IEEESAVECMDthanks\thanks +\let\@IEEESAVECMDIEEEPARstart\IEEEPARstart +\let\@IEEESAVECMDIEEEbiography\IEEEbiography +\let\@IEEESAVECMDendIEEEbiography\endIEEEbiography +\let\@IEEESAVECMDIEEEbiographynophoto\IEEEbiographynophoto +\let\@IEEESAVECMDendIEEEbiographynophoto\endIEEEbiographynophoto +\let\@IEEESAVECMDIEEEpubid\IEEEpubid +\let\@IEEESAVECMDIEEEpubidadjcol\IEEEpubidadjcol +\let\@IEEESAVECMDIEEEmembership\IEEEmembership +\let\@IEEESAVECMDIEEEaftertitletext\IEEEaftertitletext + + +% disable \IEEEPARstart when in draft mode +% This may have originally been done because the pre-V1.6 drop letter +% algorithm had problems with a non-unity baselinestretch +% At any rate, it seems too formal to have a drop letter in a draft +% paper. +\ifCLASSOPTIONdraftcls +\def\IEEEPARstart#1#2{#1#2\if@IEEEWARNIEEEPARstart\typeout{** ATTENTION: \noexpand\IEEEPARstart + is disabled in draft mode (line \the\inputlineno).}\fi\global\@IEEEWARNIEEEPARstartfalse} +\fi +% and for technotes +\ifCLASSOPTIONtechnote +\def\IEEEPARstart#1#2{#1#2\if@IEEEWARNIEEEPARstart\typeout{** WARNING: \noexpand\IEEEPARstart + is locked out for technotes (line \the\inputlineno).}\fi\global\@IEEEWARNIEEEPARstartfalse} +\fi + + +% lockout unneeded commands when in conference mode +\ifCLASSOPTIONconference +% when locked out, \thanks, \IEEEbiography, \IEEEbiographynophoto, \IEEEpubid, +% \IEEEmembership and \IEEEaftertitletext will all swallow their given text. +% \IEEEPARstart will output a normal character instead +% warn the user about these commands only once to prevent the console screen +% from filling up with redundant messages +\def\thanks#1{\if@IEEEWARNthanks\typeout{** WARNING: \noexpand\thanks + is locked out when in conference mode (line \the\inputlineno).}\fi\global\@IEEEWARNthanksfalse} +\def\IEEEPARstart#1#2{#1#2\if@IEEEWARNIEEEPARstart\typeout{** WARNING: \noexpand\IEEEPARstart + is locked out when in conference mode (line \the\inputlineno).}\fi\global\@IEEEWARNIEEEPARstartfalse} + + +% LaTeX treats environments and commands with optional arguments differently. +% the actual ("internal") command is stored as \\commandname +% (accessed via \csname\string\commandname\endcsname ) +% the "external" command \commandname is a macro with code to determine +% whether or not the optional argument is presented and to provide the +% default if it is absent. So, in order to save and restore such a command +% we would have to save and restore \\commandname as well. But, if LaTeX +% ever changes the way it names the internal names, the trick would break. +% Instead let us just define a new environment so that the internal +% name can be left undisturbed. +\newenvironment{@IEEEbogusbiography}[2][]{\if@IEEEWARNIEEEbiography\typeout{** WARNING: \noexpand\IEEEbiography + is locked out when in conference mode (line \the\inputlineno).}\fi\global\@IEEEWARNIEEEbiographyfalse% +\setbox\@IEEEtranrubishbin\vbox\bgroup}{\egroup\relax} +% and make biography point to our bogus biography +\let\IEEEbiography=\@IEEEbogusbiography +\let\endIEEEbiography=\end@IEEEbogusbiography + +\renewenvironment{IEEEbiographynophoto}[1]{\if@IEEEWARNIEEEbiographynophoto\typeout{** WARNING: \noexpand\IEEEbiographynophoto + is locked out when in conference mode (line \the\inputlineno).}\fi\global\@IEEEWARNIEEEbiographynophotofalse% +\setbox\@IEEEtranrubishbin\vbox\bgroup}{\egroup\relax} + +\def\IEEEpubid#1{\if@IEEEWARNIEEEpubid\typeout{** WARNING: \noexpand\IEEEpubid + is locked out when in conference mode (line \the\inputlineno).}\fi\global\@IEEEWARNIEEEpubidfalse} +\def\IEEEpubidadjcol{\if@IEEEWARNIEEEpubidadjcol\typeout{** WARNING: \noexpand\IEEEpubidadjcol + is locked out when in conference mode (line \the\inputlineno).}\fi\global\@IEEEWARNIEEEpubidadjcolfalse} +\def\IEEEmembership#1{\if@IEEEWARNIEEEmembership\typeout{** WARNING: \noexpand\IEEEmembership + is locked out when in conference mode (line \the\inputlineno).}\fi\global\@IEEEWARNIEEEmembershipfalse} +\def\IEEEaftertitletext#1{\if@IEEEWARNIEEEaftertitletext\typeout{** WARNING: \noexpand\IEEEaftertitletext + is locked out when in conference mode (line \the\inputlineno).}\fi\global\@IEEEWARNIEEEaftertitletextfalse} +\fi + + +% provide a way to restore the commands that are locked out +\def\IEEEoverridecommandlockouts{% +\typeout{** ATTENTION: Overriding command lockouts (line \the\inputlineno).}% +\let\thanks\@IEEESAVECMDthanks% +\let\IEEEPARstart\@IEEESAVECMDIEEEPARstart% +\let\IEEEbiography\@IEEESAVECMDIEEEbiography% +\let\endIEEEbiography\@IEEESAVECMDendIEEEbiography% +\let\IEEEbiographynophoto\@IEEESAVECMDIEEEbiographynophoto% +\let\endIEEEbiographynophoto\@IEEESAVECMDendIEEEbiographynophoto% +\let\IEEEpubid\@IEEESAVECMDIEEEpubid% +\let\IEEEpubidadjcol\@IEEESAVECMDIEEEpubidadjcol% +\let\IEEEmembership\@IEEESAVECMDIEEEmembership% +\let\IEEEaftertitletext\@IEEESAVECMDIEEEaftertitletext} + + + +% need a backslash character for typeout output +{\catcode`\|=0 \catcode`\\=12 +|xdef|@IEEEbackslash{\}} + + +% hook to allow easy disabling of all legacy warnings +\def\@IEEElegacywarn#1#2{\typeout{** ATTENTION: \@IEEEbackslash #1 is deprecated (line \the\inputlineno). +Use \@IEEEbackslash #2 instead.}} + + +% provide some legacy IEEEtran commands +\def\IEEEcompsoctitleabstractindextext{\@IEEElegacywarn{IEEEcompsoctitleabstractindextext}{IEEEtitleabstractindextext}\IEEEtitleabstractindextext} +\def\IEEEdisplaynotcompsoctitleabstractindextext{\@IEEElegacywarn{IEEEdisplaynotcompsoctitleabstractindextext}{IEEEdisplaynontitleabstractindextext}\IEEEdisplaynontitleabstractindextext} +% provide some legacy IEEEtran environments + + +% V1.8a no more support for these legacy commands +%\def\authorblockA{\@IEEElegacywarn{authorblockA}{IEEEauthorblockA}\IEEEauthorblockA} +%\def\authorblockN{\@IEEElegacywarn{authorblockN}{IEEEauthorblockN}\IEEEauthorblockN} +%\def\authorrefmark{\@IEEElegacywarn{authorrefmark}{IEEEauthorrefmark}\IEEEauthorrefmark} +%\def\PARstart{\@IEEElegacywarn{PARstart}{IEEEPARstart}\IEEEPARstart} +%\def\pubid{\@IEEElegacywarn{pubid}{IEEEpubid}\IEEEpubid} +%\def\pubidadjcol{\@IEEElegacywarn{pubidadjcol}{IEEEpubidadjcol}\IEEEpubidadjcol} +%\def\specialpapernotice{\@IEEElegacywarn{specialpapernotice}{IEEEspecialpapernotice}\IEEEspecialpapernotice} +% and environments +%\def\keywords{\@IEEElegacywarn{keywords}{IEEEkeywords}\IEEEkeywords} +%\def\endkeywords{\endIEEEkeywords} +% V1.8 no more support for legacy IED list commands +%\let\labelindent\IEEElabelindent +%\def\calcleftmargin{\@IEEElegacywarn{calcleftmargin}{IEEEcalcleftmargin}\IEEEcalcleftmargin} +%\def\setlabelwidth{\@IEEElegacywarn{setlabelwidth}{IEEEsetlabelwidth}\IEEEsetlabelwidth} +%\def\usemathlabelsep{\@IEEElegacywarn{usemathlabelsep}{IEEEusemathlabelsep}\IEEEusemathlabelsep} +%\def\iedlabeljustifyc{\@IEEElegacywarn{iedlabeljustifyc}{IEEEiedlabeljustifyc}\IEEEiedlabeljustifyc} +%\def\iedlabeljustifyl{\@IEEElegacywarn{iedlabeljustifyl}{IEEEiedlabeljustifyl}\IEEEiedlabeljustifyl} +%\def\iedlabeljustifyr{\@IEEElegacywarn{iedlabeljustifyr}{IEEEiedlabeljustifyr}\IEEEiedlabeljustifyr} +% V1.8 no more support for QED and proof stuff +%\def\QED{\@IEEElegacywarn{QED}{IEEEQED}\IEEEQED} +%\def\QEDclosed{\@IEEElegacywarn{QEDclosed}{IEEEQEDclosed}\IEEEQEDclosed} +%\def\QEDopen{\@IEEElegacywarn{QEDopen}{IEEEQEDopen}\IEEEQEDopen} +%\AtBeginDocument{\def\proof{\@IEEElegacywarn{proof}{IEEEproof}\IEEEproof}\def\endproof{\endIEEEproof}} +% V1.8 no longer support biography or biographynophoto +%\def\biography{\@IEEElegacywarn{biography}{IEEEbiography}\IEEEbiography} +%\def\biographynophoto{\@IEEElegacywarn{biographynophoto}{IEEEbiographynophoto}\IEEEbiographynophoto} +%\def\endbiography{\endIEEEbiography} +%\def\endbiographynophoto{\endIEEEbiographynophoto} +% V1.7 and later no longer supports \overrideIEEEmargins +%\def\overrideIEEEmargins{% +%\typeout{** WARNING: \string\overrideIEEEmargins \space no longer supported (line \the\inputlineno).}% +%\typeout{** Use the \string\CLASSINPUTinnersidemargin, \string\CLASSINPUToutersidemargin \space controls instead.}} + +\endinput + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%% End of IEEEtran.cls %%%%%%%%%%%%%%%%%%%%%%%%%%%% +% That's all folks! + diff --git a/paper/exoskeleton/IEEEtran/main2.tex b/paper/exoskeleton/IEEEtran/main2.tex new file mode 100644 index 0000000..df25a6e --- /dev/null +++ b/paper/exoskeleton/IEEEtran/main2.tex @@ -0,0 +1,2944 @@ + +%% bare_conf.tex +%% V1.4b +%% 2015/08/26 +%% by Michael Shell +%% See: +%% http://www.michaelshell.org/ +%% for current contact information. +%% +%% This is a skeleton file demonstrating the use of IEEEtran.cls +%% (requires IEEEtran.cls version 1.8b or later) with an IEEE +%% conference paper. +%% +%% Support sites: +%% http://www.michaelshell.org/tex/ieeetran/ +%% http://www.ctan.org/pkg/ieeetran +%% and +%% http://www.ieee.org/ + +%%************************************************************************* +%% Legal Notice: +%% This code is offered as-is without any warranty either expressed or +%% implied; without even the implied warranty of MERCHANTABILITY or +%% FITNESS FOR A PARTICULAR PURPOSE! +%% User assumes all risk. +%% In no event shall the IEEE or any contributor to this code be liable for +%% any damages or losses, including, but not limited to, incidental, +%% consequential, or any other damages, resulting from the use or misuse +%% of any information contained here. +%% +%% All comments are the opinions of their respective authors and are not +%% necessarily endorsed by the IEEE. +%% +%% This work is distributed under the LaTeX Project Public License (LPPL) +%% ( http://www.latex-project.org/ ) version 1.3, and may be freely used, +%% distributed and modified. A copy of the LPPL, version 1.3, is included +%% in the base LaTeX documentation of all distributions of LaTeX released +%% 2003/12/01 or later. +%% Retain all contribution notices and credits. +%% ** Modified files should be clearly indicated as such, including ** +%% ** renaming them and changing author support contact information. ** +%%************************************************************************* + + +% *** Authors should verify (and, if needed, correct) their LaTeX system *** +% *** with the testflow diagnostic prior to trusting their LaTeX platform *** +% *** with production work. The IEEE's font choices and paper sizes can *** +% *** trigger bugs that do not appear when using other class files. *** *** +% The testflow support page is at: +% http://www.michaelshell.org/tex/testflow/ + + + +\documentclass[conference]{IEEEtran} +% Some Computer Society conferences also require the compsoc mode option, +% but others use the standard conference format. +% +% If IEEEtran.cls has not been installed into the LaTeX system files, +% manually specify the path to it like: +% \documentclass[conference]{../sty/IEEEtran} + + + + + +% Some very useful LaTeX packages include: +% (uncomment the ones you want to load) + +\usepackage{amsmath} +\usepackage{amssymb} +\usepackage{booktabs} +\usepackage{graphicx} +\usepackage{siunitx} +\sisetup{detect-all} +\usepackage[caption=false,font=footnotesize]{subfig} +\usepackage{float} +\usepackage{tikz} +\usepackage{algorithm} +\usepackage{algpseudocode} +\usepackage{tabularx} +\usepackage{url} +\newtheorem{proposition}{Proposition} +\usetikzlibrary{fit} +\usetikzlibrary{arrows.meta, positioning, calc} +% *** MISC UTILITY PACKAGES *** +% +%\usepackage{ifpdf} +% Heiko Oberdiek's ifpdf.sty is very useful if you need conditional +% compilation based on whether the output is pdf or dvi. +% usage: +% \ifpdf +% % pdf code +% \else +% % dvi code +% \fi +% The latest version of ifpdf.sty can be obtained from: +% http://www.ctan.org/pkg/ifpdf +% Also, note that IEEEtran.cls V1.7 and later provides a builtin +% \ifCLASSINFOpdf conditional that works the same way. +% When switching from latex to pdflatex and vice-versa, the compiler may +% have to be run twice to clear warning/error messages. + + + + + + +% *** CITATION PACKAGES *** +% +%\usepackage{cite} +% cite.sty was written by Donald Arseneau +% V1.6 and later of IEEEtran pre-defines the format of the cite.sty package +% \cite{} output to follow that of the IEEE. Loading the cite package will +% result in citation numbers being automatically sorted and properly +% "compressed/ranged". e.g., [1], [9], [2], [7], [5], [6] without using +% cite.sty will become [1], [2], [5]--[7], [9] using cite.sty. cite.sty's +% \cite will automatically add leading space, if needed. Use cite.sty's +% noadjust option (cite.sty V3.8 and later) if you want to turn this off +% such as if a citation ever needs to be enclosed in parenthesis. +% cite.sty is already installed on most LaTeX systems. Be sure and use +% version 5.0 (2009-03-20) and later if using hyperref.sty. +% The latest version can be obtained at: +% http://www.ctan.org/pkg/cite +% The documentation is contained in the cite.sty file itself. + + + + + + +% *** GRAPHICS RELATED PACKAGES *** +% +\ifCLASSINFOpdf + % \usepackage[pdftex]{graphicx} + % declare the path(s) where your graphic files are + % \graphicspath{{../pdf/}{../jpeg/}} + % and their extensions so you won't have to specify these with + % every instance of \includegraphics + % \DeclareGraphicsExtensions{.pdf,.jpeg,.png} +\else + % or other class option (dvipsone, dvipdf, if not using dvips). graphicx + % will default to the driver specified in the system graphics.cfg if no + % driver is specified. + % \usepackage[dvips]{graphicx} + % declare the path(s) where your graphic files are + % \graphicspath{{../eps/}} + % and their extensions so you won't have to specify these with + % every instance of \includegraphics + % \DeclareGraphicsExtensions{.eps} +\fi +% graphicx was written by David Carlisle and Sebastian Rahtz. It is +% required if you want graphics, photos, etc. graphicx.sty is already +% installed on most LaTeX systems. The latest version and documentation +% can be obtained at: +% http://www.ctan.org/pkg/graphicx +% Another good source of documentation is "Using Imported Graphics in +% LaTeX2e" by Keith Reckdahl which can be found at: +% http://www.ctan.org/pkg/epslatex +% +% latex, and pdflatex in dvi mode, support graphics in encapsulated +% postscript (.eps) format. pdflatex in pdf mode supports graphics +% in .pdf, .jpeg, .png and .mps (metapost) formats. Users should ensure +% that all non-photo figures use a vector format (.eps, .pdf, .mps) and +% not a bitmapped formats (.jpeg, .png). The IEEE frowns on bitmapped formats +% which can result in "jaggedy"/blurry rendering of lines and letters as +% well as large increases in file sizes. +% +% You can find documentation about the pdfTeX application at: +% http://www.tug.org/applications/pdftex + + + + + +% *** MATH PACKAGES *** +% +%\usepackage{amsmath} +% A popular package from the American Mathematical Society that provides +% many useful and powerful commands for dealing with mathematics. +% +% Note that the amsmath package sets \interdisplaylinepenalty to 10000 +% thus preventing page breaks from occurring within multiline equations. Use: +%\interdisplaylinepenalty=2500 +% after loading amsmath to restore such page breaks as IEEEtran.cls normally +% does. amsmath.sty is already installed on most LaTeX systems. The latest +% version and documentation can be obtained at: +% http://www.ctan.org/pkg/amsmath + + + + + +% *** SPECIALIZED LIST PACKAGES *** +% +%\usepackage{algorithmic} +% algorithmic.sty was written by Peter Williams and Rogerio Brito. +% This package provides an algorithmic environment fo describing algorithms. +% You can use the algorithmic environment in-text or within a figure +% environment to provide for a floating algorithm. Do NOT use the algorithm +% floating environment provided by algorithm.sty (by the same authors) or +% algorithm2e.sty (by Christophe Fiorio) as the IEEE does not use dedicated +% algorithm float types and packages that provide these will not provide +% correct IEEE style captions. The latest version and documentation of +% algorithmic.sty can be obtained at: +% http://www.ctan.org/pkg/algorithms +% Also of interest may be the (relatively newer and more customizable) +% algorithmicx.sty package by Szasz Janos: +% http://www.ctan.org/pkg/algorithmicx + + + + +% *** ALIGNMENT PACKAGES *** +% +%\usepackage{array} +% Frank Mittelbach's and David Carlisle's array.sty patches and improves +% the standard LaTeX2e array and tabular environments to provide better +% appearance and additional user controls. As the default LaTeX2e table +% generation code is lacking to the point of almost being broken with +% respect to the quality of the end results, all users are strongly +% advised to use an enhanced (at the very least that provided by array.sty) +% set of table tools. array.sty is already installed on most systems. The +% latest version and documentation can be obtained at: +% http://www.ctan.org/pkg/array + + +% IEEEtran contains the IEEEeqnarray family of commands that can be used to +% generate multiline equations as well as matrices, tables, etc., of high +% quality. + + + + +% *** SUBFIGURE PACKAGES *** +%\ifCLASSOPTIONcompsoc +% \usepackage[caption=false,font=normalsize,labelfont=sf,textfont=sf]{subfig} +%\else +% \usepackage[caption=false,font=footnotesize]{subfig} +%\fi +% subfig.sty, written by Steven Douglas Cochran, is the modern replacement +% for subfigure.sty, the latter of which is no longer maintained and is +% incompatible with some LaTeX packages including fixltx2e. However, +% subfig.sty requires and automatically loads Axel Sommerfeldt's caption.sty +% which will override IEEEtran.cls' handling of captions and this will result +% in non-IEEE style figure/table captions. To prevent this problem, be sure +% and invoke subfig.sty's "caption=false" package option (available since +% subfig.sty version 1.3, 2005/06/28) as this is will preserve IEEEtran.cls +% handling of captions. +% Note that the Computer Society format requires a larger sans serif font +% than the serif footnote size font used in traditional IEEE formatting +% and thus the need to invoke different subfig.sty package options depending +% on whether compsoc mode has been enabled. +% +% The latest version and documentation of subfig.sty can be obtained at: +% http://www.ctan.org/pkg/subfig + + + + +% *** FLOAT PACKAGES *** +% +%\usepackage{fixltx2e} +% fixltx2e, the successor to the earlier fix2col.sty, was written by +% Frank Mittelbach and David Carlisle. This package corrects a few problems +% in the LaTeX2e kernel, the most notable of which is that in current +% LaTeX2e releases, the ordering of single and double column floats is not +% guaranteed to be preserved. Thus, an unpatched LaTeX2e can allow a +% single column figure to be placed prior to an earlier double column +% figure. +% Be aware that LaTeX2e kernels dated 2015 and later have fixltx2e.sty's +% corrections already built into the system in which case a warning will +% be issued if an attempt is made to load fixltx2e.sty as it is no longer +% needed. +% The latest version and documentation can be found at: +% http://www.ctan.org/pkg/fixltx2e + + +%\usepackage{stfloats} +% stfloats.sty was written by Sigitas Tolusis. This package gives LaTeX2e +% the ability to do double column floats at the bottom of the page as well +% as the top. (e.g., "\begin{figure*}[!b]" is not normally possible in +% LaTeX2e). It also provides a command: +%\fnbelowfloat +% to enable the placement of footnotes below bottom floats (the standard +% LaTeX2e kernel puts them above bottom floats). This is an invasive package +% which rewrites many portions of the LaTeX2e float routines. It may not work +% with other packages that modify the LaTeX2e float routines. The latest +% version and documentation can be obtained at: +% http://www.ctan.org/pkg/stfloats +% Do not use the stfloats baselinefloat ability as the IEEE does not allow +% \baselineskip to stretch. Authors submitting work to the IEEE should note +% that the IEEE rarely uses double column equations and that authors should try +% to avoid such use. Do not be tempted to use the cuted.sty or midfloat.sty +% packages (also by Sigitas Tolusis) as the IEEE does not format its papers in +% such ways. +% Do not attempt to use stfloats with fixltx2e as they are incompatible. +% Instead, use Morten Hogholm'a dblfloatfix which combines the features +% of both fixltx2e and stfloats: +% +% \usepackage{dblfloatfix} +% The latest version can be found at: +% http://www.ctan.org/pkg/dblfloatfix + + + + +% *** PDF, URL AND HYPERLINK PACKAGES *** +% +%\usepackage{url} +% url.sty was written by Donald Arseneau. It provides better support for +% handling and breaking URLs. url.sty is already installed on most LaTeX +% systems. The latest version and documentation can be obtained at: +% http://www.ctan.org/pkg/url +% Basically, \url{my_url_here}. + + + + +% *** Do not adjust lengths that control margins, column widths, etc. *** +% *** Do not use packages that alter fonts (such as pslatex). *** +% There should be no need to do such things with IEEEtran.cls V1.6 and later. +% (Unless specifically asked to do so by the journal or conference you plan +% to submit to, of course. ) + + +% correct bad hyphenation here +\hyphenation{op-tical net-works semi-conduc-tor} + + +\begin{document} +% +% paper title +% Titles are generally capitalized except for words such as a, an, and, as, +% at, but, by, for, in, nor, of, on, or, the, to and up, which are usually +% not capitalized unless they are the first or last word of the title. +% Linebreaks \\ can be used within to get better formatting as desired. +% Do not put math or special symbols in the title. +\title{Power-Dual SEW Retargeting and Haptic-Energy Supervision for Heterogeneous 7-DoF Exoskeleton Teleoperation} + + +% author names and affiliations +% use a multiple column layout for up to three different +% affiliations +\author{\IEEEauthorblockN{Xiangtian Kuang} +\IEEEauthorblockA{College of Mechanical and Vehicle Engineering\\ +Chongqing University\\ +Chongqing, China 400714\\ +Email: xiangtiankuang@gmail.com}} +% \and +% \IEEEauthorblockN{Homer Simpson} +% \IEEEauthorblockA{Twentieth Century Fox\\ +% Springfield, USA\\ +% Email: homer@thesimpsons.com} +% \and +% \IEEEauthorblockN{James Kirk\\ and Montgomery Scott} +% \IEEEauthorblockA{Starfleet Academy\\ +% San Francisco, California 96678--2391\\ +% Telephone: (800) 555--1212\\ +% Fax: (888) 555--1212}} + +% conference papers do not typically use \thanks and this command +% is locked out in conference mode. If really needed, such as for +% the acknowledgment of grants, issue a \IEEEoverridecommandlockouts +% after \documentclass + +% for over three affiliations, or if they all won't fit within the width +% of the page, use this alternative format: +% +%\author{\IEEEauthorblockN{Michael Shell\IEEEauthorrefmark{1}, +%Homer Simpson\IEEEauthorrefmark{2}, +%James Kirk\IEEEauthorrefmark{3}, +%Montgomery Scott\IEEEauthorrefmark{3} and +%Eldon Tyrell\IEEEauthorrefmark{4}} +%\IEEEauthorblockA{\IEEEauthorrefmark{1}School of Electrical and Computer Engineering\\ +%Georgia Institute of Technology, +%Atlanta, Georgia 30332--0250\\ Email: see http://www.michaelshell.org/contact.html} +%\IEEEauthorblockA{\IEEEauthorrefmark{2}Twentieth Century Fox, Springfield, USA\\ +%Email: homer@thesimpsons.com} +%\IEEEauthorblockA{\IEEEauthorrefmark{3}Starfleet Academy, San Francisco, California 96678-2391\\ +%Telephone: (800) 555--1212, Fax: (888) 555--1212} +%\IEEEauthorblockA{\IEEEauthorrefmark{4}Tyrell Inc., 123 Replicant Street, Los Angeles, California 90210--4321}} + + + + +% use for special paper notices +%\IEEEspecialpapernotice{(Invited Paper)} + + + + +% make the title area +\maketitle + + +% As a general rule, do not put math, special symbols or citations +% in the abstract +\begin{abstract} +Motion retargeting and force reflection cannot be designed independently when +a wearable master and a remote robot have different link lengths, joint axes, +and reachable workspaces. This paper develops a coupled formulation for a +heterogeneous 7-DoF exoskeleton--robot pair. An analytic +Shoulder--Elbow--Wrist (SEW) construction first generates feasible Cartesian +elbow and wrist targets; a bounded, branch-seeded least-squares stage then +recovers the slave configuration. The local differential of this complete map +is evaluated by guarded central differences and its transpose defines the +virtual-work-consistent generalized-force map. Slave interaction torque is +obtained from a bias-corrected inverse-dynamics residual, while a damped +least-squares reconstruction provides an end-effector wrench for diagnostics +and Cartesian baselines. On the master side, the mapped feedback passes through +filtering, gain, rate, and torque limits before a final radial energy +projection. A one-step storage invariant is established for this haptic +feedback contribution; no claim is made for the independently added dynamics +compensation or for arbitrary delayed two-port stability. Because the custom +master exoskeleton is still under fabrication, this version reports no +physical performance result. Instead, it fixes a gated validation protocol +with independent pose, wrench, timing, energy, task, and human-subject +evidence, so that later claims can be evaluated without retrospective changes +to endpoints or exclusions. +\end{abstract} + +\begin{IEEEkeywords} +Bilateral teleoperation; Upper-limb exoskeleton; Heterogeneous retargeting; +Shoulder--elbow--wrist angle; Interaction wrench estimation; +Energy-aware haptics; Human--robot interaction. +\end{IEEEkeywords} + + +% no keywords + + + + +% For peer review papers, you can put extra information on the cover +% page as needed: +% \ifCLASSOPTIONpeerreview +% \begin{center} \bfseries EDICS Category: 3-BBND \end{center} +% \fi +% +% For peerreview papers, this IEEEtran command inserts a page break and +% creates the second title. It will be ignored for other modes. +\IEEEpeerreviewmaketitle + +\section{Introduction} + +Bilateral teleoperation enables a human operator to perform manipulation in +remote, hazardous, or inaccessible environments while retaining human +perception and decision-making in the control loop +~\cite{Rebelo2014Teleop,Jiang2019Telerehab}. Upper-limb exoskeletons +are attractive master interfaces because their joint sensing can capture +operator posture and their distributed actuators can return kinesthetic cues +~\cite{Gull2020ExoReview,Kim2017Harmony}. They are also relevant to emerging +robot-demonstration and manipulation-data pipelines, in which the fidelity and +continuity of the human motion representation directly affect the quality of +the commanded robot behavior~\cite{Wang2023Dexcap,Fang2023Airexo}. + +The bilateral case becomes substantially more difficult when the wearable +master and remote robot are kinematically heterogeneous. Direct joint +mirroring is generally unavailable, while a task-space inverse-kinematics +solution may change redundancy branches, approach joint limits, or become +sensitive to initialization near singular configurations +~\cite{Sinha2019IK,Brahmi2019IK}. Moreover, a motion mapping alone +does not determine how a slave-side contact wrench should be reflected to the +operator. If motion scaling, reach saturation, filtering, actuator limits, and +communication effects are ignored in the force path, the resulting feedback +may be geometrically plausible but energetically inconsistent. + +The force path introduces two additional difficulties. First, model-based +wrench reconstruction from joint torques is affected by friction, inertial +parameter error, acceleration estimation, torque bias, and Jacobian +conditioning~\cite{Mohammadi2013DO,Indri2020FrictionID}. +Second, active force reflection can deliver net energy to the operator, +particularly during stiff contact or in the presence of delay and +asynchronous measurements. Passivity-oriented and semi-active haptic devices +illustrate useful safety principles +~\cite{Porcini2020Passivity,Buongiorno2019Teleop}, but an energy claim must identify exactly +which torque contribution is supervised and which downstream commands remain +outside the accounting. + +\vspace{1mm} +\noindent\textbf{Research question.} +This work asks the following overarching question: +\emph{How should motion retargeting, interaction-wrench reconstruction, and +force reflection be formulated together so that a heterogeneous 7-DoF +exoskeleton--robot pair preserves meaningful arm geometry while exposing and +bounding the energy released by its haptic feedback channel?} +The question is decomposed into three testable subquestions: + +\begin{itemize} + \item \textbf{RQ1 -- geometric retargeting:} Which SEW quantities should + be transferred between dissimilar embodiments, and how should reach + limits, redundancy branches, and geometric degeneracies be handled? + + \item \textbf{RQ2 -- wrench reconstruction:} Under what modeling, + sensing, and conditioning assumptions can slave joint-torque residuals + provide a useful six-dimensional interaction-wrench estimate? + + \item \textbf{RQ3 -- energy-aware force reflection:} How should the + differential of the heterogeneous motion map, actuator limits, signal + processing, and communication conditions enter the slave-to-master force + map and its discrete-time energy budget? +\end{itemize} + +\vspace{1mm} +\noindent\textbf{Contributions.} +The contributions of this work are threefold: + +\begin{enumerate} + \item \textbf{A branch-qualified motion/force contract.} We factor + heterogeneous retargeting into an analytic SEW Cartesian target and a + bounded embodiment-specific joint recovery, compute the differential of + the complete map on a fixed local branch, and use its transpose to derive + the associated generalized-force map. The same contract declares when + clipping, fallback geometry, solver failure, or an active joint limit makes + the differential unusable. + + \item \textbf{An implementation-explicit interaction pathway.} A + bias-corrected inverse-dynamics residual supplies the generalized slave + interaction torque used by the proposed force map. A frame-consistent + damped least-squares solve separately reconstructs the end-effector wrench + for interpretation and for a Cartesian-Jacobian baseline, avoiding an + unreported wrench-to-torque round trip in the proposed pathway. + + \item \textbf{A final-stage haptic-energy projection with a bounded claim.} + The complete feedback pipeline is specified as filter, gain, rate limit, + torque saturation, and radial energy projection. We prove the resulting + one-step storage invariant for the haptic contribution and provide a gated + protocol that independently tests geometry, estimation, mapping, timing, + energy, task performance, and human factors. +\end{enumerate} + +The custom 7-DoF upper-limb exoskeleton described in this paper is the target +master platform for this framework. Because fabrication and physical +integration are ongoing, the present manuscript deliberately separates the +method formulation and experimental protocol from empirical outcome claims. + +The remainder of the paper is organized as follows. +Section~\ref{sec:related_work} reviews heterogeneous retargeting, wrench +estimation, and energy-aware teleoperation. +Section~\ref{sec:problem} defines the system variables, mappings, and research +hypotheses. Section~\ref{sec:hardware} describes the master-exoskeleton +architecture and its measurable design requirements. +Section~\ref{sec:framework} presents the geometry- and energy-aware +teleoperation framework. Section~\ref{sec:experiments} specifies the planned +evaluation protocol, and Section~\ref{sec:results} reserves the reporting +structure for subsequently acquired data. +Section~\ref{sec:discussion} discusses limitations and validity threats, and +Section~\ref{sec:conclusion} concludes the paper. + +\section{Related Work} +\label{sec:related_work} + +Research on bilateral teleoperation spans posture retargeting, exoskeleton design, interaction force estimation, and passivity-based haptic rendering. Modern teleoperation and tactile-robotics frameworks emphasize bidirectional exchange of motion and force between a human and a remote environment, with a strong focus on safety and haptic awareness. This section reviews the most relevant lines of work and highlights the remaining gaps that motivate our approach. + +\subsection{Isomorphic and Vision-Aided Teleoperation} +In isomorphic teleoperation, the master and slave share similar kinematic structures, so joint-space mirroring or simple task-space tracking is sufficient. Representative systems include rehabilitation and assistance-focused upper-limb exoskeletons~\cite{Rebelo2014Teleop} and proprioceptive master devices~\cite{Lee2014Teaching}. + +Recent low-cost teleoperation and data-collection platforms demonstrate the +feasibility of dexterous manipulation through several distinct interface +families. Vision- or tracking-driven systems include ACE, AnyTeleop, and +OpenTeach~\cite{Yang2024ACE,Qin2023AnyTeleop,Iyer2024OpenTeach}. They support +multiple embodiments and scalable demonstration collection, but pose +measurement and visual feedback do not by themselves define a +power-conjugate kinesthetic channel. Physical master platforms such as HOMIE, +Mobile ALOHA, and GELLO instead exploit an isomorphic cockpit or a +robot-matched low-cost controller +~\cite{HOMIE2025,Fu2024MobileALOHA,Wu2023Gello}. Their embodiment match +simplifies command generation, whereas it does not resolve force reflection +for a kinematically different wearable master. System-level analyses further +summarize challenges in exoskeleton-based teleoperation and rehabilitation +interfaces~\cite{Gull2020ExoReview}. + +\subsection{Heterogeneous 7-DoF Arm Retargeting} +Retargeting between kinematically heterogeneous master--slave arms typically relies on numerical inverse kinematics (IK) with redundancy resolution or null-space shaping. Classical IK approaches for redundant 7-DoF manipulators~\cite{Sinha2019IK,Brahmi2019IK} have been widely applied in human--robot retargeting and humanoid control. Their continuity, convergence, and computational cost depend on the solver, initialization, constraints, and treatment of redundancy, particularly near joint or workspace boundaries. + +Recent exoskeleton-based teleoperation systems explore posture-based +homo--hetero mapping, such as the upper-limb heteromorphic teleoperation +framework in~\cite{Cheng2024HomoHetero}. Geometric SEW constructions +provide an alternative representation of reach and arm-plane redundancy +~\cite{Elias2024SEW}. +However, an analytic SEW point construction does not by itself resolve +platform-specific joint recovery, joint limits, branch continuity, or the +dual force map induced by the retargeting. + +\subsection{Force Estimation in Teleoperation and Exoskeletons} +Accurate estimation of the environment interaction wrench remains central for high-quality haptic feedback. Model-based residual-dynamics estimators and nonlinear disturbance observers have been applied to torque-controlled manipulators~\cite{Mohammadi2013DO}. However, the approach is sensitive to friction, modeling error, and torque noise. + +Exoskeleton-based teleoperation architectures often rely on direct F/T sensing or joint-based observers, such as force-sensitive exoskeletons for elderly care robotics~\cite{Toedtheide2023ForceSensitive} and lightweight tactile interfaces~\cite{Forouhar2024TactileExo}. These systems demonstrate feasibility but remain affected by compliance, transmission losses, and bias handling. Vision-only pose teleoperation~\cite{Li2020VisionTeleop} can avoid robot inverse-dynamics estimation, but its pose signal does not itself provide a power-conjugate wrench for kinesthetic feedback. These observations motivate configuration-stratified validation of residual estimation rather than a general robustness claim for lightweight hardware. + +\subsection{Passivity-Based Haptic Rendering} +The transparency--stability trade-off in bilateral teleoperation has been well established, especially under communication delay and stiff contact. Passive and semi-active actuators, including MR clutches, offer hardware-level means to limit active energy injection~\cite{Pisetskiy2021MRclutch}. Passivity-oriented control has also been integrated into multi-DoF exoskeleton teleoperation~\cite{Buongiorno2019Teleop}. + +The passivity observer/controller construction provides a foundational +sampled-data energy-accounting mechanism~\cite{Hannaford2002TDPC}. +Time-domain passivity control has also been applied to exoskeleton-based +bilateral teleoperation with independently passivated slave devices +~\cite{Porcini2020Passivity}. Multi-sensory control strategies further improve +dynamic transparency in upper-limb exoskeletons +~\cite{Zimmermann2020Transparency}. These methods establish the relevant +stability--transparency trade-off, while the force coordinate induced by a +heterogeneous motion map and the exact torque included in discrete energy +accounting still have to be declared for each implementation. + +\subsection{Research Gap} +The literature reveals three coupled gaps that motivate the present work: + +\begin{itemize} + \item \textbf{Retargeting is often separated from force reflection.} + Geometry-preserving motion mappings are commonly evaluated through pose + error alone, without deriving the differential map required to relate + generalized power between heterogeneous embodiments. + + \item \textbf{Wrench-estimation assumptions are insufficiently isolated.} + Residual-dynamics estimators depend on model quality, torque sensing, + friction compensation, acceleration estimation, and Jacobian conditioning; + these effects require independent ground truth and paired evaluation. + + \item \textbf{Energy claims must be tied to a precisely defined port.} + Filtering, scaling, saturation, timing, and communication can alter the + torque and velocity used for power computation. An energy argument is + therefore meaningful only when it identifies the exact controlled torque, + every downstream operation, and the scope of the communication model. +\end{itemize} + +To address these gaps, the present framework connects an SEW target +construction, platform-specific joint recovery, model-based residual wrench +estimation, and an energy-aware master torque supervisor through a common +problem formulation. The framework is evaluated only after its geometric, +estimation, timing, and energy assumptions have been tested separately. +No novelty is claimed for the SEW coordinate~\cite{Elias2024SEW}, residual +estimation~\cite{Mohammadi2013DO}, DLS inversion, or time-domain energy +accounting~\cite{Hannaford2002TDPC} in isolation. The claimed methodological +advance is their branch-qualified coupling: the differential is taken through +the complete constrained retargeting map; its transpose acts on the same +generalized slave interaction quantity; tracking, transport, shaping, and +post-allocation errors are separated; and the energy statement is attached to +the actual applied haptic increment rather than to an upstream wrench. This +coupling, and not a list of standard modules, defines the unit tested by the +protocol. + +\begin{table*}[t] +\centering +\footnotesize +\renewcommand{\arraystretch}{1.18} +\begin{tabularx}{\textwidth}{p{2.8cm} X X X} +\toprule +\textbf{Open interface} & \textbf{Required mathematical contract} +& \textbf{Method element in this work} & \textbf{Evidence required before a claim}\\ +\midrule +Heterogeneous motion +& Feasible targets, branch state, limits, and nonsmooth events +& SEW Cartesian construction plus bounded joint recovery +& Pose ground truth, failure/discontinuity rates, and timing against matched baselines\\ +Motion-to-force coupling +& Differential of the complete fixed-branch map and its virtual-work dual +& Guarded finite-difference $A$ and generalized-force map $A^\top$ +& Differential convergence, validity rate, and actual/reference power mismatch\\ +Interaction reconstruction +& Torque residual, frame, reference point, spatial order, and observability +& Offline bias correction and DLS end-effector wrench reconstruction +& Independent six-axis wrench reference, singular-value stratification, and perturbations\\ +Haptic energy +& Exact candidate/applied torque, velocity sample, timing, and downstream operations +& Final-stage radial projection of the shaped haptic contribution +& Independent energy reconstruction, intervention cost, and finite contact/network envelope\\ +\bottomrule +\end{tabularx} +\caption{Claim-to-evidence structure used in place of categorical +``yes/no'' comparisons. Each row identifies an interface that must be defined +mathematically and tested independently before an end-to-end conclusion.} +\label{tab:claim_evidence} +\end{table*} + +\section{System Overview and Problem Formulation} +\label{sec:problem} + +\subsection{System Boundary and Notation} + +The system comprises a human operator, a 7-DoF wearable master exoskeleton, a +communication channel, a 7-DoF robot arm, and a remote environment. Let +$q_m,\dot q_m\in\mathbb{R}^{7}$ denote the measured master joint position and +velocity, and let $q_s,\dot q_s\in\mathbb{R}^{7}$ denote the slave state. The +master and slave SEW points are denoted by +$p_{S_i},p_{E_i},p_{W_i}\in\mathbb{R}^{3}$, where $i\in\{m,s\}$. +Twists use the ordering $[v^\top\ \omega^\top]^\top$ and wrenches use +$[f^\top\ m^\top]^\top$, both at the end-effector point. The mathematical +formulation expresses their axes in a common task frame $\{C\}$. The current +URDF simulation realizes this as an identity alignment between the fixed +master and slave base axes; physical experiments require measured base-to-$C$ +calibrations and a calibrated tool-center point. + +The commanded slave configuration is written as +\begin{equation} + q_s^{\mathrm{ref}}[k] + = \mathcal{R}\!\left(q_m[k];b_k,\theta_R\right), + \label{eq:retarget_map} +\end{equation} +where $b_k$ is the numerical branch seed and $\theta_R$ collects link lengths, +frame calibrations, tolerances, and joint limits. Thus, $\mathcal{R}$ is not +assumed to be a globally single-valued smooth map. To avoid conflating two +distinct operations, we factor it as +\begin{equation} + y_s^{\mathrm{ref}} = \mathcal{G}(y_m;\theta_G),\qquad + q_s^{\mathrm{ref}} + = \mathcal{K}_s(y_s^{\mathrm{ref}};b_k,\theta_K). + \label{eq:retarget_factorization} +\end{equation} +Here, $\mathcal{G}$ is the analytic Cartesian SEW target construction and +$\mathcal{K}_s$ is the embodiment-specific joint recovery. The SEW descriptor +$y_i$ contains reach direction, reach magnitude, swivel angle, and a terminal +orientation objective. This factorization permits the geometric construction +to be evaluated independently of the numerical or analytic solver used by a +particular slave arm. + +\subsection{Differential Mapping and Power Objective} + +At configurations where $\mathcal{R}$ is differentiable and $b_k$ identifies +one fixed branch, define the local retargeting differential +\begin{equation} + A(q_m;b_k) = + \frac{\partial\mathcal{R}(q_m;b_k,\theta_R)}{\partial q_m}, + \qquad + \dot q_s^{\mathrm{map}} = A(q_m;b_k)\dot q_m. + \label{eq:retarget_differential} +\end{equation} +Reach clipping, branch changes, and joint-limit projections can make this map +nonsmooth; those events are logged explicitly and are not covered by an +unqualified smoothness assumption. Let $\bar A[k]$ denote the most recent +valid differential, updated at 50\,Hz and held between updates. Two velocities +must be distinguished in the current multirate simulator. At every 500-Hz +haptic sample, the unmodified algebraic velocity +$\dot q_s^{\mathrm{map}}[k]=\bar A[k]\dot q_m[k]$ is recomputed for the +virtual-work contract. At a 50-Hz mapping instant $k_u$, the slave-controller +feedforward +\begin{equation} + \dot q_s^{\mathrm{ctrl}}[k_u] + =\mathrm{clip}_{0.8\dot q_{s,\max}}\! + \left(\bar A[k_u]\dot q_m[k_u]\right) + \label{eq:controller_reference_velocity} +\end{equation} +is computed once and held until the next update. Thus, +$\dot q_s^{\mathrm{map}}$, $\dot q_s^{\mathrm{ctrl}}$, and the actual +$\dot q_s$ are conceptually separate signals. The current run-level log stores +the actual velocity and the resulting algebraic power scalar but not both +reference vectors; explicit vector logging is a G0a prerequisite. + +The present simulator colocates retargeting and slave control, so it has no +forward network delay. For the G3 network tests, mapping update $i$ creates the +forward packet +$u_f[i]=(q_s^{\mathrm{ref}}[i],\dot q_s^{\mathrm{ctrl}}[i],i)$, while the +corresponding $\bar A[i]$ is retained in the synchronized master log. If +$\kappa_f(k)$ is the most recent forward-packet index accepted by slave sample +$k$, the physical controller uses +\begin{equation} + q_{s,\mathrm{net}}^{\mathrm{ref}}[k] + =q_s^{\mathrm{ref}}[\kappa_f(k)],\qquad + \dot q_{s,\mathrm{net}}^{\mathrm{ctrl}}[k] + =\dot q_s^{\mathrm{ctrl}}[\kappa_f(k)]. + \label{eq:forward_held_reference} +\end{equation} +The slave does not require $A$ for tracking. Instead, each return packet echoes +the active forward-map identifier +$\mu(j)=\kappa_f(j)$ at its slave source sample $j$. This identifier binds a +returned residual and velocity to the exact held reference and historical +$\bar A[\mu(j)]$ that generated them. + +Let $\hat\tau_{s,\mathrm{int}}$ be the estimated generalized slave interaction +torque under the environment-on-robot sign convention. Virtual work motivates +the corresponding environment-on-master reaction +\begin{equation} + \tau_{m,\mathrm{dual}} + = \sigma_f A(q_m;b_k)^\top\hat\tau_{s,\mathrm{int}}, + \label{eq:power_dual_map} +\end{equation} +where $\sigma_f>0$ is a declared force scale. No additional minus sign is +introduced after fixing this reaction convention. On a valid branch, +\begin{equation} + \tau_{m,\mathrm{dual}}^\top\dot q_m + = + \sigma_f\hat\tau_{s,\mathrm{int}}^\top + \dot q_s^{\mathrm{map}}. + \label{eq:power_dual_identity} +\end{equation} +\begin{proposition}[Fixed-branch virtual-work dual] +If $\delta q_s=A\,\delta q_m$ on a certified local branch, then +$\tau_{m,\mathrm{dual}}=\sigma_fA^\top\tau_s$ is the unique generalized +master force satisfying +$\tau_{m,\mathrm{dual}}^\top\delta q_m +=\sigma_f\tau_s^\top\delta q_s$ for every $\delta q_m$. +\end{proposition} +\begin{IEEEproof} +Substitution gives +$\sigma_f\tau_s^\top\delta q_s +=\sigma_f\tau_s^\top A\delta q_m +=(\sigma_fA^\top\tau_s)^\top\delta q_m$. Equality for every virtual +displacement fixes the generalized-force coefficient uniquely. +\end{IEEEproof} +This is an algebraic identity, not actual bilateral power equality. Before +delay or master-side shaping, slave tracking error contributes +\begin{equation} + \Delta P_{\mathrm{trk}} + = \sigma_f\hat\tau_{s,\mathrm{int}}^\top + \left(A\dot q_m-\dot q_s\right). + \label{eq:tracking_power_error} +\end{equation} +Delay, filtering, rate limits, saturation, and energy projection introduce +additional measurable differences and are reported separately. + +\subsection{Controlled Haptic Contribution and Storage} + +All nominal filtering, gain, rate limiting, and haptic-torque saturation +precede the energy projection. Let $\tau_{h,\mathrm{app}}[k]$ denote the final +haptic feedback contribution returned by that projection. Using the convention +that positive power is delivered by this contribution to the operator, +\begin{equation} + P_h[k] + = \tau_{h,\mathrm{app}}[k]^\top\dot q_m[k], + \label{eq:applied_port_power} +\end{equation} +and the unbounded discrete storage update is +\begin{equation} + E^+[k+1] = E[k] - P_h[k]\Delta t_k, + \label{eq:applied_port_energy} +\end{equation} +where $\Delta t_k=t_{k+1}-t_k>0$ is the measured interval of the applied +output sample. The fixed-rate simulation is the special case +$\Delta t_k=T_s$. +The complete motor command may additionally contain independently designed +master dynamics compensation, +$\tau_{m,\mathrm{cmd}}=\tau_{m,\mathrm{comp}}+\tau_{h,\mathrm{app}}$. +The present storage does not account for $\tau_{m,\mathrm{comp}}$ or the +mechanical energy of the exoskeleton. Consequently, the analytical claim is +limited to the haptic contribution. No later operation may modify +$\tau_{h,\mathrm{app}}$ except an emergency zero-output action. On physical +hardware this requirement must be enforced either by reserving verified +actuator headroom for the haptic component or by relocating the projection +and accounting to the actual post-allocation torque. A combined low-level +saturation that changes the haptic increment invalidates the stated invariant +unless that actual increment is reconstructed and used for accounting. +Communication-delay robustness and total human--device passivity are not +inferred from \eqref{eq:applied_port_energy}. + +\subsection{Testable Hypotheses and Scope} + +The evaluation is organized around four hypotheses: +\begin{itemize} + \item \textbf{H1:} the SEW target construction reduces the locked + trajectory-level composite rate of solver/tolerance failure or observable + joint/swivel discontinuity relative to the primary IK baseline at + comparable task-space accuracy; + \item \textbf{H2:} on the preregistered noisy and near-singular subset, + regularized residual-torque inversion yields lower force and moment RMSE + than an undamped solve, with a measurable bias--variance trade-off; + \item \textbf{H3:} for a matched wrench input, the differential-dual map + $A^\top J_s^\top F$ reduces cross-embodiment power mismatch relative to + the mapping-agnostic $J_m^\top F$ reflection; + \item \textbf{H4:} when reconstructed from the actual post-allocation + haptic increment, the final energy projection prevents lower-budget + violations beyond numerical tolerance relative to projection bypass, with + a measurable transparency cost over the tested envelope. +\end{itemize} +The virtual-work identity and the one-step storage inequality are deterministic +contracts verified at G0a; they are not treated as statistically discovered +effects. +These hypotheses deliberately avoid universal claims about arbitrary stiffness +or delay. Their acceptance criteria, statistical tests, and failure reporting +are defined before physical data are collected. + +\begin{figure*}[!ht] + \centering + \includegraphics[width=0.32\linewidth]{assets/overview.jpg}% + \includegraphics[width=0.32\linewidth]{assets/explain.jpg}% + \includegraphics[width=0.32\linewidth]{assets/joint_distribution.jpg} + \caption{Overall hardware design of the proposed 7-DoF upper-limb exoskeleton. + (Left) Intended wearable arrangement and adjustable human--exoskeleton interface. + (Middle) Joint arrangement and kinematic routing used in the current mechanical design. + (Right) Distribution of the seven rotational joints ($q_0$--$q_6$) across the shoulder, elbow, and wrist modules.} + \label{fig:exo_overview} +\end{figure*} + +\section{Target Master Platform and Design Requirements} +\label{sec:hardware} + +Prior upper-limb exoskeletons have explored anatomically aligned actuation and precise joint-center matching to improve motion capture accuracy and wearer comfort. Representative shoulder--arm systems emphasize accurate glenohumeral alignment and low-distortion rotational layouts~\cite{Yan2014ShoulderExo}. Wrist and distal-arm devices similarly prioritize compactness and minimal reflected inertia to preserve natural hand motion~\cite{Buongiorno2018WRES}. + +The target master device is a custom 7-DoF upper-limb exoskeleton currently +under fabrication for bilateral teleoperation. Unlike a load-assistance +exoskeleton, its design objectives are posture-measurement fidelity, +backdrivability, bounded kinesthetic feedback, and compatibility with the SEW +representation in Section~\ref{sec:sew}. These objectives are not treated as +measured properties at the present stage. They are converted into explicit +characterization requirements in Section~\ref{sec:experiments}. An overview of +the current mechanical design is provided in Fig.~\ref{fig:exo_overview}. + +\subsection{Three-DoF Orthogonal Shoulder Architecture} +The shoulder module design (Fig.~\ref{fig:shoulder_arch}) places three nominally +orthogonal rotational axes near the human glenohumeral joint, consistent with +prior shoulder-exoskeleton design principles~\cite{Yan2014ShoulderExo}. +Fabrication tolerance, soft-tissue motion, and subject-specific anatomy will +prevent perfect alignment; the resulting misalignment must therefore be +measured rather than assumed. The proposed actuator layout is: + +\begin{itemize} + \item \textbf{(1) Back-mounted flexion/extension axis.} + The first DoF actuator is placed behind the scapular region to keep bulk + away from the upper arm and to approximate a flexion--extension axis. + + \item \textbf{(2) Lateral shoulder abduction axis.} + The second actuator is positioned laterally to approximate an + abduction--adduction axis; alignment error and parasitic motion remain + quantities to be measured. + + \item \textbf{(3) Humeral-roll ring gear.} + A lightweight annular ring gear encircles the upper arm and is driven by a + compact motor to approximate a coaxial humeral-roll axis. Its reflected + inertia and friction will be established experimentally rather than inferred + from the layout. +\end{itemize} + +\begin{figure}[t] + \centering + \includegraphics[width=0.75\linewidth]{assets/shoulder.jpg} + \caption{Intended three-DoF shoulder architecture; physical joint-center + alignment remains a characterization outcome.} + \label{fig:shoulder_arch} +\end{figure} + +\subsection{Forearm and Wrist Module with Symmetric 3-DoF Design} +The forearm--wrist assembly (Fig.~\ref{fig:wrist_module}) follows the same +geometric design objective. Three rotational DoFs---forearm roll, wrist pitch, +and wrist yaw---are realized through compact transmission mechanisms informed +by wrist-exoskeleton designs~\cite{Buongiorno2018WRES}. The present design does +not assume magnetorheological actuation. Actuator type, reduction ratio, +continuous torque, peak torque, friction, and torque bandwidth will be reported +from the fabricated system. + +\begin{figure}[t] + \centering + \includegraphics[width=0.78\linewidth]{assets/wrist.jpg} + \caption{Current symmetric three-DoF wrist-module design with nominal + yaw--pitch--roll axes.} + \label{fig:wrist_module} +\end{figure} + +\subsection{Structure and Human-Fit Requirements} +A hybrid frame combining aluminum or composite links with polymer interface +components is intended to balance structural stiffness, distal mass, and +wearability. These choices create testable requirements rather than guaranteed +outcomes: structural deflection must be separated from encoder motion, +backdriving torque must be measured throughout the workspace, and fitting +adjustments must cover a declared anthropometric range. Comfort and alignment +will be assessed independently of task performance. + +\subsection{Interface to the Teleoperation Framework} +The mechanical design exposes the following interfaces to the teleoperation +architecture: +\begin{itemize} + \item joint positions and synchronized velocities for master-side SEW + reconstruction; + \item calibrated joint-torque commands and measured actuator states for + kinesthetic feedback; + \item declared torque, velocity, position, and thermal limits for nominal + haptic shaping and independent low-level safety; + \item hardware timestamps and safety-state logs for independent power and + timing reconstruction. +\end{itemize} +The experiment protocol treats sensing accuracy, actuator bandwidth, +backdrivability, structural stiffness, alignment, and safety thresholds as +prerequisites for system-level evaluation. + +% ---------------- Geometry- and Energy-Aware Teleoperation Framework ---------------- + +\section{Geometry- and Energy-Aware Teleoperation Framework} +\label{sec:framework} + +The framework contains four stages. First, master joint measurements are +converted to a master SEW descriptor. Second, an +analytic geometric construction produces a feasible slave SEW target, after +which a bounded platform-specific solver recovers the slave joint reference +and a guarded finite difference estimates the differential $A$. Third, a +bias-corrected inverse-dynamics residual estimates generalized slave +interaction torque; a DLS solve provides a Cartesian wrench for diagnostics +and the direct-Jacobian baseline. Fourth, $A^\top$ maps the generalized +interaction to the master, followed in strict order by a torque filter, gain, +rate limit, saturation, and final haptic-energy projection. The current +repository implementation binds these stages to \texttt{master\_7dof.urdf} and +\texttt{real\_slave\_7dof.urdf}. Physical frame calibration, timing, and +behavior remain validation requirements in Section~\ref{sec:experiments}. + +\begin{figure*}[t] +\centering +\begin{tikzpicture}[ + >=Latex, + block/.style={draw, rounded corners, align=center, minimum height=8mm, + text width=25mm, font=\scriptsize}, + wide/.style={block, text width=31mm}, + line/.style={->, thick}, + base/.style={->, dashed, semithick} +] +\node[block] (master) {Master state\\$q_m,\dot q_m$}; +\node[block, right=6mm of master] (sew) {Analytic SEW\\target $\mathcal G$}; +\node[wide, right=6mm of sew] (recover) {Bounded recovery\\$\mathcal K_s(\cdot;b_k)$}; +\node[block, right=6mm of recover] (slave) {Slave tracking\\and environment}; + +\node[wide, below=12mm of slave] (residual) {Bias-corrected residual\\ +$\hat\tau_{s,\mathrm{int}}$}; +\node[block, left=6mm of residual] (dual) {Power-dual map\\$A^\top$}; +\node[wide, left=6mm of dual] (shape) {Filter $\rightarrow$ gain $\rightarrow$ rate\\ +$\rightarrow$ saturation}; +\node[wide, left=6mm of shape] (energy) {Final haptic-energy\\projection}; +\node[block, left=6mm of energy] (output) {Applied haptic\\$\tau_{h,\mathrm{app}}$}; + +\node[block, below=9mm of residual] (dls) {DLS wrench\\${}^C\hat F_{\mathrm{int}}$}; +\node[wide, left=6mm of dls] (direct) {Cartesian baseline\\ +$({}^CJ_{E_m})^\top{}^C\hat F_{\mathrm{int}}$}; + +\draw[line] (master) -- (sew); +\draw[line] (sew) -- (recover); +\draw[line] (recover) -- node[above,font=\scriptsize] {$q_s^{\mathrm{ref}}$} (slave); +\draw[line] (slave) -- (residual); +\draw[line] (residual) -- (dual); +\draw[line] (dual) -- (shape); +\draw[line] (shape) -- (energy); +\draw[line] (energy) -- (output); +\draw[line] (recover.south) |- node[pos=0.35,left,font=\scriptsize] + {guarded FD $A$} (dual.north); +\draw[base] (residual) -- (dls); +\draw[base] (dls) -- (direct); +\draw[base] (direct.west) -| node[pos=0.25,below,font=\scriptsize] + {baseline only} (shape.south); +\end{tikzpicture} +\caption{Executable information flow. The proposed feedback path maps the +bias-corrected generalized residual directly through $A^\top$. DLS wrench +reconstruction feeds diagnostics and the dashed Cartesian baseline. All +nominal shaping precedes the final projection, which supervises only the +applied haptic contribution.} +\label{fig:method_architecture} +\end{figure*} + +\subsection{Heterogeneous SEW Target Construction} +\label{sec:sew} + +\begin{figure*}[t] + \centering + \subfloat[SEW master and slave geometry\label{fig:sew_geometry_all_a}]{ + \includegraphics[width=0.31\linewidth]{assets/sew_geometry.png} + }\hfill + \subfloat[Swivel angle definition\label{fig:sew_geometry_all_b}]{ + \includegraphics[width=0.31\linewidth]{assets/swivel_angle.png} + }\hfill + \subfloat[Two-sphere intersection for elbow\label{fig:sew_geometry_all_c}]{ + \includegraphics[width=0.31\linewidth]{assets/two_sphere_intersection.png} + } + \caption{Geometric interpretation of the proposed SEW retargeting: + (a) master and slave SEW triangles remain geometrically consistent + despite different link lengths; + (b) the swivel angle $\phi_m$ is defined as the rotation from the + reference normal $n_{\mathrm{ref}}$ to the upper-arm normal $n_m$ + around the reach direction $\hat{x}_m$; + (c) the slave elbow lies on the intersection circle of two spheres + centered at $S_s$ and $W_s^{\mathrm{ref}}$ with radii $L_1$ and $L_2$, + with $\phi_m$ selecting a unique point on this circle.} + \label{fig:sew_geometry_all} +\end{figure*} + +Bilateral teleoperation between heterogeneous 7-DoF arms requires a posture +representation that does not assume identical joint coordinates. Motivated by +the conventional SEW redundancy coordinate for 7-DoF arms +~\cite{Elias2024SEW}, we use a Shoulder--Elbow--Wrist descriptor to construct a +feasible Cartesian target. +The target construction is analytic; recovery of a complete slave joint vector +is treated separately because it depends on the slave joint axes, limits, and +available kinematic solver. Geometry-preserving retargeting is also used in +vision-based pipelines, where pose or keypoint measurements may feed a broader +optimization-based robot mapping~\cite{Wu2023Gello,Rong2020FrankMocap}. + +% ========================================================== +\subsubsection{SEW Descriptor and Validity Domain} +SEW makes the redundant arm-plane coordinate explicit, but it does not by +itself guarantee temporal continuity or remove algorithmic singularities +~\cite{Elias2024SEW}. Fig.~\ref{fig:sew_geometry_all} illustrates the +geometric structure used here. Let +$p_{S_m},p_{E_m},p_{W_m}\in\mathbb{R}^3$ be the master shoulder, elbow, and +wrist positions expressed in the common chest frame $\{C\}$. + +The shoulder--wrist direction and reach are +\begin{equation} + r_m = p_{W_m}-p_{S_m},\qquad + d_m=\|r_m\|,\qquad + \hat{x}_m = \frac{r_m}{\|r_m\|}. +\end{equation} + +The master upper-arm plane normal is +\begin{equation} + n_m=\frac{(p_{E_m}-p_{S_m})\times(p_{W_m}-p_{S_m})} + {\|(p_{E_m}-p_{S_m})\times(p_{W_m}-p_{S_m})\|}. +\end{equation} +These normalized quantities are valid only when +\begin{equation} + d_m>\epsilon_r,\qquad + \left\|(p_{E_m}-p_{S_m})\times(p_{W_m}-p_{S_m})\right\|>\epsilon_n. + \label{eq:sew_validity} +\end{equation} +A violation marks the pose invalid for differential feedback. The current +defaults are $\epsilon_r=10^{-9}$\,m and +$\epsilon_n=10^{-8}$\,m$^2$; these are numerical degeneracy guards, not +physical accuracy thresholds, and G0c includes threshold sensitivity. +A geometric placeholder may be returned for software continuity, but it is not +treated as a valid SEW measurement. + +A chest-frame reference normal is obtained by projecting +the chest vertical axis $e_z=[0\ 0\ 1]^\top$ onto the plane +orthogonal to $\hat{x}_m$: +\begin{equation} + n_{\mathrm{ref}} + =\frac{ e_z-(e_z^\top\hat{x}_m)\hat{x}_m } + { \| e_z-(e_z^\top\hat{x}_m)\hat{x}_m \| }. +\end{equation} +The implemented reference-axis fallback is activated when the norm of the +projected unit axis in the denominator is below $10^{-6}$. + +The swivel angle is the signed rotation from $n_{\mathrm{ref}}$ to $n_m$ +about~$\hat{x}_m$ (see Fig.~\ref{fig:sew_geometry_all}(b)): +\begin{equation} + \phi_m=\mathrm{atan2}\!\left( + \hat{x}_m^\top(n_{\mathrm{ref}}\!\times n_m),~~ + n_{\mathrm{ref}}^\top n_m + \right), +\end{equation} +where the current realization uses the principal value. Because the +Rodrigues construction below is $2\pi$-periodic, this choice does not create a +Cartesian discontinuity solely at the angle wrap; branch and fallback +continuity are addressed separately. + +% ========================================================== +\subsubsection{Closed-form Wrist and Elbow Reconstruction} + +Let $p_{S_s}$ denote the slave shoulder position, and $L_1,L_2$ its upper-arm and +forearm link lengths. A feasible slave wrist distance must satisfy the triangle +inequality with a small safety margin $\varepsilon>0$: +\begin{equation} + d_{\min}=|L_1-L_2|+\varepsilon,\qquad + d_{\max}=L_1+L_2-\varepsilon. +\end{equation} +The present software uses $\varepsilon=10^{-3}$\,m; the reach margin and +degeneracy thresholds are varied in the same G0c sensitivity study. +We clip the master reach into this feasible interval: +\begin{equation} + d_s=\mathrm{clip}(d_m;\,d_{\min},d_{\max}),\qquad + p_{W_s}^{\mathrm{ref}}=p_{S_s}+d_s \hat{x}_m. +\end{equation} +This is the absolute-reach policy implemented in the current software. +Normalized reach and a calibrated affine reach scale are retained as +preregistered ablations because absolute transfer may overuse clipping when +master and slave link-length sums differ substantially. + +The slave elbow $p_{E_s}$ lies on the circle defined by the intersection of two +spheres centered at $p_{S_s}$ and $p_{W_s}^{\mathrm{ref}}$: +\[ +\|p_{E_s}-p_{S_s}\|=L_1,\qquad +\|p_{E_s}-p_{W_s}^{\mathrm{ref}}\|=L_2, +\] +as shown in Fig.~\ref{fig:sew_geometry_all}(c). Let +\[ +d=\|p_{W_s}^{\mathrm{ref}}-p_{S_s}\|,\qquad +\hat{e}_1 = \frac{p_{W_s}^{\mathrm{ref}}-p_{S_s}}{d}. +\] + +Using Rodrigues' rotation operator, +\begin{equation} + \mathcal{R}(\hat{e}_1,\phi_m) + =I+\sin\phi_m[\hat{e}_1]_\times + +(1-\cos\phi_m)[\hat{e}_1]^2_\times, +\end{equation} +we form an orthonormal basis of the circle plane as +\begin{equation} + \hat{e}_3=\mathcal{R}(\hat{e}_1,\phi_m)n_{\mathrm{ref}},\qquad + \hat{e}_2=\frac{\hat{e}_1\times \hat{e}_3} + {\|\hat{e}_1\times\hat{e}_3\|}. +\end{equation} + +From the cosine law, +\begin{equation} + \cos\theta=\frac{L_1^2+d^2-L_2^2}{2L_1d},\qquad + \sin\theta=\sqrt{1-\cos^2\theta}, +\end{equation} +the elbow target is obtained as +\begin{equation} + p_{E_s}^{\mathrm{ref}} + =p_{S_s}+L_1(\cos\theta\,\hat{e}_1+\sin\theta\,\hat{e}_2). + \label{eq:elbow_target} +\end{equation} + +\begin{proposition}[Feasible SEW target] +For $L_1,L_2>0$, $0<\varepsilon<\min(L_1,L_2)$, and +$d_s\in[d_{\min},d_{\max}]$, the targets in +\eqref{eq:elbow_target} satisfy +$\|p_{E_s}^{\mathrm{ref}}-p_{S_s}\|=L_1$ and +$\|p_{E_s}^{\mathrm{ref}}-p_{W_s}^{\mathrm{ref}}\|=L_2$, provided the SEW +basis is valid. +\end{proposition} +\begin{IEEEproof} +The clipped interval strictly satisfies the two triangle inequalities. +The cosine-law expression therefore yields +$\cos\theta\in[-1,1]$. Orthonormality of +$(\hat e_1,\hat e_2)$ gives the first distance directly; substituting +$p_{W_s}^{\mathrm{ref}}=p_{S_s}+d_s\hat e_1$ and the cosine law gives the +second. +\end{IEEEproof} + +This construction makes explicit how $p_{E_s}$ is selected on the SEW circle +by the chosen swivel branch. It provides an analytic Cartesian elbow target; +continuity of the resulting joint trajectory +still depends on reference-normal handling, numerical branch state, and the +platform-specific joint recovery described next. + +% ========================================================== +\subsubsection{Temporal Continuity Requirements} + +The current implementation obtains temporal branch preference by warm-starting +the bounded recovery from the preceding valid slave solution. It does not +maintain a separate swivel-angle filter, an unwrapped swivel state, or a +last-valid reference normal. Reach clipping, reference-axis fallback, a +degenerate master arm plane, and an active slave joint limit therefore mark the +sample as nonsmooth and invalidate $A$. Pose continuity and elbow-flip rate are +measured from trajectories rather than inferred from the geometry. Stateful +normal blending or angle unwrapping may be evaluated later, but is not part of +the method claimed here. + +% ========================================================== +\subsubsection{Embodiment-Specific Joint Recovery} + +The terminal orientation is transferred as +\begin{equation} + R_{s,\mathrm{EE}}^{\mathrm{ref}} + = R_{m,\mathrm{EE}}R_\Delta, + \label{eq:orientation_transfer} +\end{equation} +where $R_\Delta$ is a fixed neutral-pose calibration. The present aligned-URDF +simulation uses $R_\Delta=I$ and the current code path hard-wires that identity; +it does not yet expose $R_\Delta$ as a mapper parameter. Ingestion of the +measured orientation offset and its regression test are G0a requirements before +hardware use. + +Given the Cartesian targets and \eqref{eq:orientation_transfer}, the current +slave recovery solves the bounded residual problem +\begin{equation} + q_s^{\mathrm{ref}} + =\arg\min_{\underline q_s\le q_s\le\overline q_s} + \left\|r_K(q_s)\right\|_2^2, +\label{eq:joint_recovery} +\end{equation} +\begin{equation} +r_K(q_s)= +\begin{bmatrix} +s_E\!\left(p_{E_s}(q_s)-p_{E_s}^{\mathrm{ref}}\right)\\ +s_W\!\left(p_{W_s}(q_s)-p_{W_s}^{\mathrm{ref}}\right)\\ +s_R\,\mathrm{Log}\!\left(R_{s,\mathrm{EE}}(q_s)^\top +R_{s,\mathrm{EE}}^{\mathrm{ref}}\right)\\ +s_q\,\mathrm{wrap}(q_s-q_{\mathrm{seed}}) +\end{bmatrix}. +\label{eq:joint_recovery_residual} +\end{equation} +The scale factors make position and angular residuals explicit. The current +software fixes $s_E=s_W=5$, $s_R=1$, and $s_q=10^{-5}$ in SI units and uses +the URDF lower and upper joint bounds; $\mathrm{wrap}$ acts componentwise on +$[-\pi,\pi)$. These values are implementation +parameters, not empirically optimal constants, and are included in the +sensitivity protocol. + +A trust-region reflective least-squares solver is warm-started from the +previous solution during sequential operation. At initialization, a staged +shoulder--elbow--wrist seed and the joint-range midpoint are both evaluated, +and the lower task-residual solution is retained. The solver uses at most +400 function evaluations and +$\mathrm{ftol}=\mathrm{xtol}=\mathrm{gtol}=10^{-12}$. A recovery is accepted +only when the solver succeeds, all values are finite, the master geometry is +valid, no URDF limit is violated, both elbow and wrist errors are at most +$2\times10^{-4}$\,m, and the orientation error is at most +$2\times10^{-3}$\,rad. Accordingly, \emph{analytic} refers only to the +Cartesian SEW target, not the complete map +$q_m\mapsto q_s^{\mathrm{ref}}$. + +% ========================================================== +\subsubsection{SEW Degenerate Cases and Fallback Construction} +\label{sec:sew_degenerate} + +When $\hat{x}_m$ becomes nearly collinear with $e_z$, the projected reference +normal becomes ill-conditioned. Separately, a nearly zero reach or nearly +collinear shoulder--elbow--wrist points violates +\eqref{eq:sew_validity}. The latter two cases are invalid master geometry and +are never certified for differential feedback. + +To keep the reference construction defined, we introduce a fallback +orthogonal direction. +Let $e_x = [1\ 0\ 0]^\top$ and $e_y = [0\ 1\ 0]^\top$. The deterministic +fallback selects the candidate with the larger projected norm, +\begin{equation} + e_f=\arg\max_{e\in\{e_y,e_x\}} + \left\|e-(e^\top\hat x_m)\hat x_m\right\|. +\end{equation} +It is then projected onto the plane orthogonal to $\hat{x}_m$: +\[ +\tilde{n}_{\mathrm{ref}} = e_f - (e_f^\top \hat{x}_m)\hat{x}_m,\qquad +n_{\mathrm{ref}} = \frac{\tilde{n}_{\mathrm{ref}}}{\|\tilde{n}_{\mathrm{ref}}\|}. +\] +This construction keeps the reference normal defined when $\hat{x}_m$ is +nearly vertical, but activation is explicitly marked as nonsmooth. The current +implementation neither blends the threshold nor reuses a preceding normal. +Deliberate passages through this region are therefore retained in the failure +and continuity evaluation. + +The SEW module therefore provides an analytic construction of feasible reach, +direction, and swivel targets. Full-map continuity, joint-limit feasibility, +and real-time performance remain properties to be established for the selected +slave-joint recovery implementation. + +\subsubsection{Guarded Local Retargeting Differential} +\label{sec:retarget_differential} + +The complete bounded map has no analytic Jacobian in the current +implementation. Let $q_s^0=\mathcal R(q_m;b_k)$ be a valid, smooth base +solution and $e_j$ the $j$th master-joint basis vector. Its local differential +is approximated by +\begin{equation} + A_{:,j}\approx + \frac{\mathrm{wrap}\!\left( + \mathcal R(q_m+h e_j;q_s^0) + -\mathcal R(q_m-h e_j;q_s^0)\right)}{2h}, + \label{eq:finite_difference_A} +\end{equation} +with $h=10^{-4}$\,rad. All positive and negative perturbations are initialized +from the same bounded base solution so that the calculation targets one local +branch. + +A recovered pose is marked smooth only if it meets the stated task tolerances +and has no reach clipping, reference-axis fallback, degenerate master geometry, +joint-limit violation, or joint clearance below the configured +$10^{-4}$-rad limit margin. For column $j$, define +$d_j^+=\mathrm{wrap}(q_s^+-q_s^0)/h$ and +$d_j^-=\mathrm{wrap}(q_s^0-q_s^-)/h$. A column is certified only when both +perturbation solves succeed, all three poses are marked smooth, the reach-clip +region is unchanged, the largest wrapped joint jump from the base is no greater +than $0.25$\,rad, and +\begin{equation} + \eta_j = + \frac{\|d_j^+-d_j^-\|_2} + {1+\max(\|d_j^+\|_2,\|d_j^-\|_2)} + \le 0.05 . + \label{eq:differential_consistency} +\end{equation} +The finite-difference step and all three guard thresholds are current software +defaults, not universal constants; G0c varies them in a locked convergence and +sensitivity study. The active-limit gate currently refers to recovered +\emph{slave} joints. The mapper does not yet reject a master perturbation +$q_m\pm he_j$ outside the master URDF limits; G0a must add that domain check and +mark the corresponding differential invalid rather than silently using an +out-of-domain central sample. A preregistered one-sided boundary rule may be +evaluated separately. The complete $A$ is valid only if all seven columns are +finite and certified. Invalid columns are represented as non-finite values for +diagnostics; they are never used for feedback. In the closed-loop software, a +failed pose update holds the preceding pose reference, whereas an invalid +differential sets the new mapped-feedback target and reference velocity to zero +so that the upstream rate limit returns the haptic output toward zero. One +differential evaluation requires one base recovery and fourteen perturbed +recoveries. The present convenience wrapper first performs a separate nominal +pose recovery and then calls that evaluator, for a total of two nominal and +fourteen perturbed solves per 50-Hz update; reusing the first nominal solution +is an implementation optimization still to be made. High-percentile runtime +and validity rate are therefore mandatory evaluation outcomes. + + +\subsection{Locked Slave Tracking Interface} +\label{sec:slave_tracking} + +Slave tracking is not claimed as a contribution, but it is part of the +experimental plant and cannot be left implicit. The current numerical fixture +uses the fixed computed-torque law +\begin{equation} + \begin{aligned} + \tau_{s,\mathrm{ctrl}} + &=\mathrm{sat}_{\alpha_s\tau_{s,\max}}\!\left[ + h_s+M_s a_s^{\mathrm{cmd}}\right],\\ + a_s^{\mathrm{cmd}} + &=K_p\,\mathrm{diff}(q_s,q_s^{\mathrm{ref}}) + +K_d(\dot q_s^{\mathrm{ctrl}}-\dot q_s)+a_{\mathrm{lim}} . + \end{aligned} + \label{eq:slave_tracking_controller} +\end{equation} +where $a_{\mathrm{lim}}$ is a continuous acceleration-domain soft joint-limit +guard and $\alpha_s=0.70$ in the present simulation. The gain vectors, the +0.12-rad soft-limit buffer, the 500-Hz update, and all effort and numerical +acceleration limits are archived with the fixture configuration. Physical +hardware may require a different certified low-level controller; whichever +controller is selected will be locked and shared by every paired mapping and +haptic condition so that it is not an unreported experimental factor. + +\subsection{Inverse-Dynamics Residual Wrench Estimation} +\label{sec:interaction} + +The slave-side contact wrench is reconstructed from joint-torque measurements +without assuming that the residual is an unbiased measurement of contact. +The formulation follows model-based residual methods used for contact +detection and interaction estimation +~\cite{Mohammadi2013DO,DeLuca2006Collision,Wang2022InteractionTorque}. +Unlike a generalized-momentum observer, the present estimator explicitly uses +the instantaneous inverse-dynamics residual. Its performance therefore depends +on acceleration estimation, friction compensation, model quality, torque +sensing, and the spatial directions observable through the tool Jacobian. + +% ========================================================== +\subsubsection{Joint-Space Inverse-Dynamics Residual} + +Let $\tau_s^{\mathrm{meas}}$ denote a \emph{calibrated load-side-equivalent} +joint-torque signal, not an unqualified raw motor-current estimate. Under the +environment-on-robot convention used here, its required measurement balance is +\begin{equation} + \tau_s^{\mathrm{meas}} + =\tau_s^{\mathrm{dyn}}+\tau_{s,\mathrm{int}}+b_\tau+\eta_\tau, + \label{eq:load_side_torque_balance} +\end{equation} +where $b_\tau$ and $\eta_\tau$ are bias and measurement noise. If the physical +drive reports actuator-on-link torque under the usual manipulator dynamics +$M\ddot q+h=\tau_{\mathrm{act}}+\tau_{\mathrm{ext}}$, its sign and +transmission mapping must first be converted to +\eqref{eq:load_side_torque_balance}; the conversion will be verified against +the independent wrist force/torque sensor. +The model-predicted torque from rigid-body dynamics is +\begin{equation} + \tau_s^{\mathrm{dyn}} + = M_s(q_s)\ddot q_s + h_s(q_s,\dot q_s) + + \tau_{\mathrm{fric}}^{\mathrm{ff}}, +\end{equation} +where $h_s$ contains Coriolis, centrifugal, and gravity terms and +$\tau_{\mathrm{fric}}^{\mathrm{ff}}$ is an optional identified friction +feedforward term. It is zero in the present rigid-body simulation and must not +be described as identified until physical calibration is complete. + +We define the \emph{residual torque} +\begin{equation} + r_s = \tau_s^{\mathrm{meas}} - \tau_s^{\mathrm{dyn}}, +\end{equation} +which contains external joint torque together with modeling error, friction +error, sensor bias, and measurement noise. The estimate used by the current +formulation is +\begin{equation} + \hat\tau_{s,\mathrm{int}} = r_s-\hat b_\tau, + \label{eq:residual_torque_estimate} +\end{equation} +where $\hat b_\tau$ is identified from a declared, independently confirmed +no-contact calibration sequence. Static contact cannot be distinguished from +bias using low velocity alone; consequently, online bias updates are disabled +unless an independent contact gate is available. A true momentum-observer +extension may be introduced later, but it must then be implemented and +evaluated as a separate estimator rather than attributed to +\eqref{eq:residual_torque_estimate}. +For physical data, the causal acceleration estimator, its cutoff and order, +initialization, group delay, and timestamp alignment with torque measurements +will be fixed from calibration data and reported; the current rigid-body +simulation supplies acceleration directly and cannot validate that pipeline. + +% ========================================================== +\subsubsection{Mapping to Chest-Frame Wrench} + +Let $J_{E_s}^{\mathrm{LWA}}(q_s)\in\mathbb{R}^{6\times n_s}$ be the +local-world-aligned geometric Jacobian at the slave end-effector point. The +axes are rotated into $\{C\}$ without changing that reference point: +\begin{equation} + {}^C\!J_{E_s} + =\operatorname{blkdiag}(R_{CW},R_{CW}) + J_{E_s}^{\mathrm{LWA}}. + \label{eq:chest_jacobian} +\end{equation} +No translational adjoint term appears because the twist and wrench remain +referenced at the same end-effector point. With the +$[\mathrm{linear};\mathrm{angular}]$ and $[\mathrm{force};\mathrm{moment}]$ +orders, virtual work gives +\begin{equation} + \tau_{s,\mathrm{int}} + = \left({}^{C}\!J_{E_s}\right)^{\!\top} {}^{C}\!F_{\mathrm{int}}, +\end{equation} +where ${}^{C}\!F_{\mathrm{int}}\in\mathbb{R}^6$ is the interaction wrench at +the slave end-effector. The implemented DLS estimate is the regularized +least-squares solution only after the translational and rotational coordinates +have been given a common physical scale. Let the characteristic length +$\ell_c>0$ be selected from calibration data and frozen before the locked +test set, and define +\begin{equation} + \begin{aligned} + S_{\ell}&=\operatorname{diag}(\ell_c^{-1}I_3,I_3),\\ + \widetilde J_s&=S_{\ell}\,{}^C\!J_{E_s},\\ + \widetilde F&=S_{\ell}^{-\top}{}^C\!F + =[\ell_c f^\top\;m^\top]^\top . + \end{aligned} + \label{eq:spatial_scaling} +\end{equation} +Then $\widetilde J_s^\top\widetilde F +=({}^C\!J_{E_s})^\top{}^C\!F$, so the virtual-work pairing is unchanged +while all entries of $\widetilde F$ have torque units. The dimensionally +scaled DLS estimate is +\begin{equation} + \widetilde F_{\mathrm{int}}^\star + =\arg\min_{\widetilde F} + \left\|\widetilde J_s^\top\widetilde F + -\hat\tau_{s,\mathrm{int}}\right\|_2^2 + +\lambda_{\ell}^{\,2}\|\widetilde F\|_2^2, + \label{eq:dls_objective} +\end{equation} +namely +\begin{equation} + {}^C\!\hat F_{\mathrm{int}} + =S_{\ell}^{\top} + \left(\widetilde J_s\widetilde J_s^\top + +\lambda_{\ell}^{\,2}I_6\right)^{-1} + \widetilde J_s\hat\tau_{s,\mathrm{int}}. + \label{eq:dls_wrench} +\end{equation} + +For a singular value $\widetilde\sigma_i$ of $\widetilde J_s$, the +scaled residual-to-wrench gain is +$\widetilde\sigma_i/(\widetilde\sigma_i^2+\lambda_{\ell}^{\,2})$; damping +limits near-singular amplification but introduces projection bias. The +current simulation code predates \eqref{eq:spatial_scaling}: it applies an +unweighted six-dimensional solve to force and moment coordinates and uses the +numerical default $\lambda=10^{-3}$. Its reconstructed wrench and unscaled +condition number are therefore diagnostic only, because they depend on the +chosen length and unit convention. Before any physical accuracy claim, G0a +requires implementing \eqref{eq:spatial_scaling}--\eqref{eq:dls_wrench}, +selecting $(\ell_c,\lambda_{\ell})$ only on the calibration set, and using the +same frozen scaling for singularity stratification and all DLS baselines. The +rotation in \eqref{eq:chest_jacobian} does not improve the scaled Jacobian +singular values. + +% ========================================================== +\subsubsection{Signal Conditioning and Identifiability} + +The current estimator stores the raw residual, bias-corrected residual, and +unfiltered DLS wrench. It does not apply a separate wrench filter. Nominal +low-pass filtering is instead applied once to the mapped master generalized +torque in Section~\ref{sec:haptics}. Acceleration-estimation filtering, +friction identification, and any future wrench filter must be calibrated and +logged separately. Force and moment errors are reported separately because +their units and sensor characteristics differ; a combined norm requires a +declared characteristic length. + +% ========================================================== +\subsubsection{Estimator Scope} + +The residual estimator is expected to degrade under inertial-parameter error, +unmodeled payload, drivetrain friction, acceleration noise, torque bias, and +poor Jacobian conditioning. Damping can trade variance for bias but cannot +recover wrench components that are not observable from joint torque. These +effects are therefore experimental factors, not assumed robustness +properties. An independent wrist force/torque sensor supplies ground truth, +and trials near singular configurations remain in the analysis. + +The proposed differential-dual pathway applies $A^\top$ directly to the +bias-corrected generalized residual in +\eqref{eq:residual_torque_estimate}. The DLS wrench in +\eqref{eq:dls_wrench} is used for physical interpretation, comparison with +independent six-axis ground truth, and the direct +$({}^C\!J_{E_m})^\top{}^C\!\hat F_{\mathrm{int}}$ baseline. In general, +$({}^C\!J_{E_s})^\top{}^C\!\hat F_{\mathrm{int}} +\ne\hat\tau_{s,\mathrm{int}}$ because DLS introduces a regularized +projection; the method therefore does not silently map through this round +trip. + + + +\subsection{Power-Dual Haptic Rendering and Feedback-Energy Supervision} +\label{sec:haptics} + +Time-domain passivity and energy-supervision methods have been studied for +multi-DoF exoskeleton teleoperation +~\cite{Buongiorno2019Teleop,Porcini2020Passivity}. The present formulation +distinguishes whether the generalized-force map is dual to the heterogeneous +motion map from whether the shaped haptic contribution respects a discrete +energy budget. The latter is not a passivity claim for the total actuator port +or an arbitrary delayed bilateral network. + +% ========================================================== +\subsubsection{Differential-Dual Generalized-Force Mapping} + +For a valid held differential, let $\kappa_r(k)\le k$ denote the source index +of the most recent return-channel sample accepted at master sample $k$, and +let $\chi_r[k]\in\{0,1\}$ indicate that the held return sample has not exceeded +its timeout. The proposed current-map pathway applies the declared force scale +to that delayed, bias-corrected joint residual: +\begin{equation} + \tau_{h,\mathrm{raw}}[k] + =\chi_r[k]\sigma_f \bar A[k]^\top + \hat\tau_{s,\mathrm{int}}[\kappa_r(k)]. + \label{eq:raw_master_torque} +\end{equation} +Equation~\eqref{eq:raw_master_torque} states the timestamp policy of the +current implementation: the most recently arrived slave residual is +re-expressed through the \emph{current} held map. It therefore satisfies a +receiver-sample algebraic duality with +$\dot q_s^{\mathrm{map}}[k]=\bar A[k]\dot q_m[k]$, but it is not a claim of +source-time power equality at configuration $\kappa_r(k)$. For the present +fixed-delay queue, $\kappa_r(k)=k-d$ with integer $d\ge0$. The protocol +separately reports the resulting transport/configuration error and compares +this policy with source-stamped +$\bar A[\mu(\kappa_r(k))]^\top +\hat\tau_{s,\mathrm{int}}[\kappa_r(k)]$. +Here $\mu(\kappa_r(k))$ is the active forward-map identifier echoed by that +return packet, as defined in \eqref{eq:forward_held_reference}; using +$\bar A[\kappa_r(k)]$ without this binding would be incorrect under asymmetric +forward delay. +The current implementation sets $\sigma_f=1$ and uses $\gamma$ below as the +single feedback-strength parameter; the two gains are not tuned +simultaneously. The present fixture uses $\gamma=0.50$ and $a_\tau=0.222$. +The constructor currently checks finiteness but does not reject every +out-of-contract lower-bound value; strict validation of +$\gamma\ge0$ and $00 . + \end{cases} + \label{eq:energy_projection} +\end{equation} +The final haptic contribution and storage update are +\begin{equation} + \tau_{h,\mathrm{app}}[k] + =\rho[k]\tau_{h,\mathrm{cand}}[k], + \label{eq:applied_torque} +\end{equation} +\begin{equation} + E[k+1]=\mathrm{clip}\!\left( + E[k]-\tau_{h,\mathrm{app}}[k]^\top + \dot q_m[k]\Delta t_k,\, + E_{\min},E_{\max}\right). + \label{eq:bounded_energy_update} +\end{equation} +Charging above $E_{\max}$ is discarded as virtual dissipation. The numerical +lower clip is a finite-precision guard; the projection itself supplies the +lower-bound invariant. The initial value $E[0]\in[E_{\min},E_{\max}]$ and +$E_{\min}$, $E_{\max}$, $\bar\rho$, $a_\tau$, and $\gamma$ will be selected +from fixture-safe calibration runs and locked before confirmatory comparisons; +the present simulation values are software-test parameters rather than +validated human-use settings. + +\begin{proposition}[One-step haptic-storage invariant] +Assume $E[k]\in[E_{\min},E_{\max}]$, $\Delta t_k>0$, finite +$\tau_{h,\mathrm{cand}}[k]$ and $\dot q_m[k]$, and no downstream modification +of $\tau_{h,\mathrm{app}}[k]$. Equations +\eqref{eq:energy_projection}--\eqref{eq:bounded_energy_update} imply first that +the \emph{unclipped} update in \eqref{eq:applied_port_energy} satisfies +$E^+[k+1]\ge E_{\min}$, and consequently +$E[k+1]\in[E_{\min},E_{\max}]$. +\end{proposition} +\begin{IEEEproof} +If $P_{\mathrm{cand}}\le0$, the applied haptic power is nonpositive and the +unclipped storage cannot decrease. If $P_{\mathrm{cand}}>0$, +\eqref{eq:energy_projection} gives +$\rho P_{\mathrm{cand}}\Delta t_k\le E[k]-E_{\min}$, so the unclipped update is at +least $E_{\min}$. The final clip enforces the upper capacity and preserves the +lower bound. +\end{IEEEproof} +Applying the proposition inductively gives the same storage bound over any +finite sequence of valid samples; this still does not establish closed-loop +stability. More specifically, define discarded charge +\begin{equation} + D[k]=\max\!\left(0,E[k]-P_h[k]\Delta t_k-E_{\max}\right). +\end{equation} +Then, over $N$ valid samples, +\begin{equation} + \sum_{k=0}^{N-1}P_h[k]\Delta t_k + =E[0]-E[N]-\sum_{k=0}^{N-1}D[k] + \le E[0]-E_{\min}. + \label{eq:net_haptic_energy_bound} +\end{equation} +This bound concerns signed net released energy, not the cumulative positive +work after repeated recharge events. + +% ========================================================== +\subsubsection{Scope of the Energy Claim} + +Equations~\eqref{eq:energy_projection}--\eqref{eq:applied_torque} constrain +energy released by the sampled haptic feedback contribution under the stated +timing and sign conventions. They do not constrain independently added +dynamics compensation and do not establish stability of the human, robot, +environment, or communication channel as arbitrary dynamical systems. A +stronger delayed bilateral claim requires an explicit two-port treatment and +an analysis including sampling, filters, saturation, and delay. + +% ========================================================== +\subsubsection{Executable Update and Logging Contract} + +\begin{figure}[t] +\begin{algorithmic}[1] +\Require synchronized master/slave states, $\tau_s^{\mathrm{meas}}$, $E[k]$ +\If{retargeting update is due} + \State construct SEW target and solve bounded recovery + \State compute guarded $A$ using \eqref{eq:finite_difference_A} +\EndIf +\State compute $\hat\tau_{s,\mathrm{int}}$ and diagnostic +${}^C\!\hat F_{\mathrm{int}}$ +\State accept the declared return-channel sample and retain its source index +$\kappa_r(k)$ and echoed forward-map identifier $\mu(\kappa_r(k))$ +\State set $\tau_{h,\mathrm{raw}}\gets +\chi_r[k]\sigma_f\bar A[k]^\top +\hat\tau_{s,\mathrm{int}}[\kappa_r(k)]$ if the held +differential and return channel are currently valid; otherwise $0$ +\State apply \eqref{eq:haptic_filter}--\eqref{eq:candidate_torque} +\State project and account exactly once using +\eqref{eq:energy_projection}--\eqref{eq:bounded_energy_update} +\State output $\tau_{h,\mathrm{app}}$ unchanged +\end{algorithmic} +\caption{One executable update of the proposed motion, estimation, and haptic +path. Retargeting may run at a slower declared rate than the dynamics and +haptic loop; its output and validity flag are then held between updates.} +\label{alg:framework_update} +\end{figure} + +The present simulation stores time series for: +\begin{itemize} + \item the bias-corrected slave residual and DLS wrench; + \item raw, candidate, and applied haptic torque, the post-update storage + $E[k+1]$, and $\rho[k]$. +\end{itemize} +Retargeting success, differential validity, and fallback are currently stored +only as aggregate run-level rates or counts, as are rate-limit and +haptic-torque-saturation events. They have no per-sample reason codes. The +numerical supervisor likewise exposes a fail-safe flag internally, but the +current run-level simulator does not persist it. G0a requires per-sample flags, +reason codes, and the reference-vector logs identified above before such +evidence is reported. Physical tests additionally require +source timestamps, packet age, measured period and jitter, packet loss, +emergency-stop state, actuator derating/limit state, and the actual accepted +low-level torque. A quantity not recorded cannot support a timing or +energy-accounting claim. + +% ========================================================== +The haptic module therefore defines a power-dual raw reaction and a +final-stage storage projection for the haptic contribution. Whether the +implementation improves transparency and remains usable over a finite contact +and network envelope is determined by the protocol below. + + +\section{Experimental Design and Prospective Validation Protocol} +\label{sec:experiments} + +\noindent\textit{Protocol status:} +The experiments described in this section constitute a prospective validation +protocol. Physical data collection under this protocol has not yet begun, and +no experimental outcome is reported or implied. Numerical safety limits, +factor levels, and sample sizes that depend on fabricated hardware will be +fixed after calibration but before the corresponding locked outcomes are +inspected. Calibration and pilot data used for tuning will remain separate +from the confirmatory test set. The present repository supports deterministic +software checks and a closed-loop numerical smoke test (G0a--G0b below). +The multi-run simulation study (G0c) and all physical stages (G1--G4) remain +prospective; this distinction is maintained in every reported table and +figure. + +\subsection{Evaluation Questions and Staged Design} + +Table~\ref{tab:rq_traceability} prevents the hardware prerequisite, +deterministic contracts, empirical hypotheses, and end-to-end external +validation from being conflated. + +\begin{table*}[t] +\centering +\footnotesize +\renewcommand{\arraystretch}{1.16} +\begin{tabularx}{\textwidth}{p{1.8cm} p{3.1cm} X X p{1.0cm}} +\toprule +\textbf{Role} & \textbf{Claim or hypothesis} & \textbf{Locked comparison} +& \textbf{Primary evidence} & \textbf{Gate}\\ +\midrule +Prerequisite +& Master and slave hardware satisfy their declared operating envelopes +& Measured performance versus preregistered requirements +& Calibration, backdrive, bandwidth, latency, headroom, and safety limits +& G1\\ +RQ1 / H1 +& Branch-qualified SEW retargeting reduces the locked composite event rate +& SEW pathway versus matched bounded and task-priority IK +& Paired trajectory-level failure-or-discontinuity rate +& G0c/G2\\ +RQ2 / H2 +& Regularization improves noisy/near-singular wrench reconstruction +& DLS versus undamped solve, with bias/friction/model ablations +& Force RMSE and moment RMSE against an independent six-axis sensor +& G0c/G2\\ +RQ3 / H3 +& Differential-dual mapping reduces cross-embodiment power mismatch +& $A^\top J_s^\top F$ versus $J_m^\top F$ with the same $F$ +& Paired $\epsilon_{P,c}^{\mathrm{act}}$ and transport-error decomposition +& G0c/G3\\ +RQ3 / H4 +& Actual-applied haptic supervision enforces its budget at a transparency cost +& Projection on/off plus a locked time-domain PO/PC baseline +& Paired floor deficit, pre-clip audit, and torque-level projection distortion +& G0a/G3\\ +External validity +& Integrated usefulness after all component gates pass +& Proposed, PO/PC, and no-kinesthetic conditions +& Task success; human surface-force RMSE; safety events +& G4\\ +\bottomrule +\end{tabularx} +\caption{Traceability from research questions to claims, comparisons, primary +evidence, and validation gates. Hardware characterization is a prerequisite, +not a separate algorithmic research claim.} +\label{tab:rq_traceability} +\end{table*} + +All algorithm parameters will be tuned on a separate calibration set. +Trajectories, random seeds, failure definitions, exclusion rules, primary +outcomes, and statistical models will be frozen before evaluating the locked +test set. Compared methods will receive identical inputs, initial conditions, +joint and torque limits, network traces, and computing resources. + +\begin{table*}[t] +\centering +\footnotesize +\renewcommand{\arraystretch}{1.18} +\begin{tabularx}{\textwidth}{p{1.0cm} p{3.0cm} X X} +\toprule +\textbf{Gate} & \textbf{Stage and purpose} & \textbf{Required evidence} +& \textbf{Exit criterion before advancing}\\ +\midrule +G0a & Deterministic software contracts +& URDF/frame binding, bounded recovery, finite-difference checks, +DLS algebra, invalid-sample fail-safe, output ordering, and independent +energy reconstruction +& Versioned tests pass; no unexplained non-finite value; tolerances and +failure semantics frozen\\ +G0b & Current closed-loop numerical smoke test +& Declared master/slave models, one scripted free-space/contact trajectory, +three implemented feedback paths, and invariant/event logging +& All runs terminate normally; algebraic and storage invariants satisfy +frozen numerical tolerances; no performance claim is made\\ +G0c & Locked multi-run simulation study +& Paired trajectories and seeds, stratified kinematic cases, contact and +delay factors, robustness perturbations, matched-input baselines, and +per-sample protocol-state logs +& Runner, dataset, primary numerical endpoints, and analysis script frozen +before the locked batch\\ +G1 & Hardware bench characterization +& Master sensing, torque/backdrive, bandwidth, limits, temperature, +deflection and emergency stop; slave tracking, TCP/F/T, residual, and +accepted-command calibration +& Documented safe operating envelope and automatic termination limits\\ +G2 & Component validation +& SEW/solver comparison on a locked workspace and wrench estimation against +an independent six-axis sensor +& Primary component endpoints and failure definitions pass their +preregistered feasibility criteria\\ +G3 & Non-human bilateral fixture +& Power mismatch, haptic storage, delay/contact factors, transparency, +oscillation, and all single-factor ablations +& No unresolved safety event; human-safe conditions selected without viewing +human outcomes\\ +G4 & Integrated and human evaluation +& Locked manipulation tasks followed by ethics-approved, counterbalanced +within-participant testing +& Complete reporting of successes, failures, withdrawals, and protocol +deviations\\ +\bottomrule +\end{tabularx} +\caption{Gated evidence plan. A later stage cannot be used to compensate for +an unresolved component or safety failure at an earlier gate.} +\label{tab:validation_gates} +\end{table*} + +\subsection{Current-Code Simulation and Numerical Verification} +\label{sec:current_code_validation} + +\subsubsection{G0a: deterministic software contracts} + +The present regression suite exercises the declared URDF, joint, and frame +contract; bounded SEW recovery and invalid-domain handling; an independent +directional finite-difference check of $A$; the fixed-branch virtual-work +identity; DLS recovery of a synthetic known wrench; frame-mismatch rejection; +the filter--rate-limit--saturation--projection order; exact storage-floor +projection; the unilateral virtual-wall sign convention; and finite execution +of a short closed-loop run. These tests are treated as executable +specifications, not as samples in a statistical performance comparison. + +Before the locked G0c batch, the suite and logger will additionally cover +calibrated $R_\Delta$ and TCP/F/T transforms, convergence over multiple +finite-difference steps, explicit master-domain checks, source-stamped held +differentials, independent packet age and timeout state, and per-sample +validity and failure-reason codes. The logger will separately persist +$\dot q_s^{\mathrm{map}}$, $\dot q_s^{\mathrm{ctrl}}$, actual $\dot q_s$, +$E[k]$, the unclipped $E^+[k+1]$, clipped $E[k+1]$, and the numerical +fail-safe state. Random seeds, solver tolerances, loop rates, delay queues, and +test configurations will be archived. + +\subsubsection{G0b: current closed-loop smoke test} + +The executable smoke test loads \texttt{master\_7dof.urdf} and +\texttt{real\_slave\_7dof.urdf}. It integrates both rigid-body models at +\SI{500}{\hertz}, updates and holds the retargeting differential at +\SI{50}{\hertz}, and applies the currently implemented fixed +\SI{80}{\milli\second} return delay. A computed-torque human proxy drives the +master and a computed-torque controller drives the slave through a scripted +approach--contact--return trajectory against a unilateral spring--damper wall. +The only three presently executable paired conditions are: +\begin{enumerate} + \item $A(q_m)^\top\hat\tau_{s,\mathrm{int}}$ with final applied-port + energy supervision; + \item $J_m^\top{}^C\!\hat F_{\mathrm{int}}$ with the same filter, rate + limit, saturation, and energy supervision; + \item $A(q_m)^\top\hat\tau_{s,\mathrm{int}}$ with only the energy + projection bypassed while all other guards remain active. +\end{enumerate} +The run stores joint/reference trajectories, synthetic residual and DLS-wrench +signals, raw/candidate/applied master torque, contact force and penetration, +power-identity errors, applied power, $\rho$, supervised and shadow storage, +tracking errors, mapping runtime, and aggregate mapping/limit events. +Consequently it can expose integration, sign, ordering, non-finite-state, and +storage-accounting defects. + +It cannot establish physical wrench accuracy, transparency, human +performance, or stability over an operating envelope. The simulated +measurement is generated from the same nominal dynamics used by the plant, +with prescribed bias and noise; the virtual TCP is not yet a calibrated +physical TCP; and the present output contains one trajectory, one seed, one +contact impedance, and one fixed return delay. Moreover, condition 1 uses a +generalized residual whereas condition 2 uses a DLS wrench. Their contrast is +therefore an end-to-end feedback-chain smoke test, not the matched-input +mapping comparison required by H3. + +\begin{table*}[t] +\centering +\footnotesize +\renewcommand{\arraystretch}{1.15} +\begin{tabularx}{\textwidth}{p{2.1cm} p{4.0cm} X X} +\toprule +\textbf{Subsystem} & \textbf{Available in the current repository} +& \textbf{Evidence it may support now} +& \textbf{Required before the next claim}\\ +\midrule +Model contract +& Both seven-DoF URDFs, declared joint/frame names, limits, and +master/common/slave frame checks +& Reproducible numerical model binding +& Measured $R_\Delta$, master alignment, and slave TCP/F/T calibration\\ +Retargeting +& Bounded SEW target/recovery, fallback logic, finite-difference $A$, and +aggregate success/validity counts +& Local algebra and deterministic regression checks +& Locked IK baselines, stratified trajectory generator, source stamps, and +per-sample failure reasons\\ +Interaction estimate +& Bias-corrected generalized residual and fixed-damping DLS wrench with +synthetic bias/noise +& Estimator algebra under the shared nominal model +& Dimensionally scaled Jacobian, frozen damping, model/friction +perturbations, and independent six-axis ground truth\\ +Haptic path +& $A^\top\hat\tau_s$, direct $J_m^\top\hat F$, output shaping, applied-port +energy projection, and shadow storage +& Virtual-work, execution-order, and storage-invariant checks +& Matched $A^\top J_s^\top\hat F$ baseline, locked PO/PC, source-stamped $A$, +and actuator-accepted torque audit\\ +Closed loop and network +& One numerical contact trajectory, one seed, fixed return-delay queues, and +CSV/NPZ/JSON output +& G0b smoke testing only +& Multi-trajectory runner, asymmetric time-varying channels, jitter/loss and +timeouts, complete per-sample state, and independent physical logs\\ +\bottomrule +\end{tabularx} +\caption{Code-to-evidence boundary. ``Available'' describes executable +capability, not a measured result. Items in the last column are completion +criteria rather than assumed properties.} +\label{tab:code_evidence_boundary} +\end{table*} + +\subsubsection{G0c: locked multi-run simulation design} + +G0c converts the single-run smoke test into a reproducible paired experiment +without elevating numerical output to hardware evidence. Four master-motion +families will be generated: free-space workspace sweeps, repeated +approach--contact--retreat motion, multi-axis contact probes, and trajectories +that deliberately approach joint limits, low manipulability, or SEW +reference-normal degeneracy. Every feedback method receives the same initial +state, trajectory identifier, random seed, and environment/network trace. + +The core software-only development grid uses fixed return delays of +\SIlist{0;40;80;120}{\milli\second} and three virtual impedance pairs +$(K,B)=\{(200,15),(500,30),(800,45)\}$ in +\si{\newton\per\metre} and \si{\newton\second\per\metre}, respectively. +The three implemented conditions above are run first. After their interfaces +are frozen, the matched-input +$A^\top J_s^\top{}^C\!\hat F_{\mathrm{int}}$ condition and the locked PO/PC +baseline are added. A preliminary minimum of 20 paired +trajectory--seed realizations per core cell is used to estimate variability; +the final locked count is set by confidence-interval precision on the +trajectory-level primary endpoint, not by treating time samples as independent +replicates. The extended generator must make these distinct master +trajectories as well as distinct perturbation realizations; changing only the +current synthetic sensor-noise seed does not create an independent task +trajectory. These numerical factor levels are a development grid and do not +define safe hardware settings. + +A separate one-factor-at-a-time robustness block varies measurement-noise and +bias magnitudes, finite-difference step, dynamics/inertial and friction-model +perturbations, and dynamics-to-mapping rate ratio. Jitter, packet loss, +asymmetric delay, out-of-order rejection, and timeout/recovery traces enter +only after the corresponding two-channel state machine and per-sample logger +pass G0a. This separation prevents an incomplete network mock-up from being +presented as delay robustness. + +The G0c experimental unit is one complete trajectory--seed realization. +Deterministic primary gates are maximum virtual-work-identity error, maximum +independently reconstructed pre-clip storage deficit, and absence of +unexplained non-finite states. Paired trajectory-level numerical outcomes are +the composite SEW event $C_r$, mapping-valid fraction, runtime tail +percentiles, normalized power mismatch +\eqref{eq:normalized_actual_power_mismatch}, projection distortion +\eqref{eq:projection_distortion}, contact-force and motion-tracking errors, +limit/saturation events, and shadow-budget deficit +\eqref{eq:shadow_floor_deficit}. The present single-run artifacts are retained +as development records but are excluded from the locked batch and from every +confirmatory confidence interval. + +\subsection{Instrumentation, Synchronization, and Safety} + +The final manuscript will report the manufacturer, model, measurement range, +resolution, sampling rate, calibration method, and calibration date of each +external instrument. The minimum independent instrumentation is: +\begin{itemize} + \item a calibrated six-axis force/torque sensor at the slave wrist; + \item an external pose-reference system or metrology procedure for + end-effector and exoskeleton alignment measurements; + \item calibrated load cells or torque transducers for actuator and + backdrivability characterization; + \item a common clock or measured clock-offset process for master, slave, + network, and reference sensors. +\end{itemize} + +Raw signals will be recorded before filtering. Logged variables will include +joint position, velocity, measured and commanded torque, reference wrench, +estimated wrench, SEW state, mapping differential, raw/candidate/applied haptic +torque, independently added compensation torque, total low-level command, +the command accepted by the drive, active current/thermal derating limits, +storage level, projection factor, packet delay, controller period, and safety +events. Before G3, componentwise command allocation must demonstrate that the +declared haptic increment reaches the actuator unchanged; otherwise the +post-allocation increment, rather than the pre-allocation software value, will +be used in \eqref{eq:applied_port_energy}. The nominal loop rate will not be +inferred from a configuration value; the measured period distribution, missed +deadlines, and 99th-percentile jitter will be reported. + +Joint position, velocity, torque, temperature, and workspace limits will be +derived from measured hardware ratings and a documented risk assessment. +High-stiffness, high-delay, and controller-ablation tests will first use a +rigidly mounted robotic input fixture or recorded-motion replay without a +person wearing the device. Automatic termination thresholds will be locked +before testing. Terminated trials will be retained and reported as safety +events rather than silently discarded. + +\subsection{Hardware Characterization} +\label{sec:hardware_characterization} + +Hardware characterization will precede teleoperation experiments. Each joint +of the wearable master will be evaluated in both motion directions at no fewer +than three representative arm configurations and with repeated trials per +condition. The slave bench will independently characterize its tracking, +residual-estimation, TCP/F/T, and command-acceptance chain rather than +inheriting the nominal URDF properties. The following properties will be +measured: +\begin{enumerate} + \item range of motion, encoder resolution, absolute accuracy, + repeatability, zero-offset drift, and human-joint alignment error; + \item commanded-to-measured torque gain, linearity, hysteresis, + continuous and peak torque, saturation, and thermal behavior; + \item static breakaway torque, velocity-dependent friction, passive + backdriving torque, and observable reflected inertia; + \item closed-loop torque bandwidth and phase lag using a safely bounded + chirp or multisine excitation; + \item structural stiffness and end-point deflection under calibrated + loads; + \item total mass, moving mass by segment, fitting range, donning time, and + emergency-stop latency; + \item sensor-to-actuator latency, control-period jitter, and packet-loss + behavior. +\end{enumerate} + +Motor current alone will not be used as torque ground truth. Backdrivability +will be reported as a function of joint angle and velocity. Aggregate torque +norms will be accompanied by per-joint values and percentages of measured +continuous and peak ratings. + +\subsection{SEW Retargeting Evaluation} +\label{sec:sew_protocol} + +The complete retargeting pipeline will be compared with: +\begin{enumerate} + \item anthropometrically scaled joint-space mapping; + \item damped least-squares IK with warm starting and joint-limit + regularization; + \item task-priority IK with an explicit elbow-swivel or manipulability + objective. +\end{enumerate} +These three baseline runners are not part of the present single-trajectory +simulator. They must share the same input and output contract, expose the same +validity/failure fields, and pass G0a before their parameters are frozen for +G0c and G2. +Baseline parameters will be selected on calibration trajectories under the same +joint limits, task weights, and compute-time budget as the proposed method. +The analytic SEW target construction and the platform-specific joint recovery +will also be timed and evaluated separately. + +The locked configuration set will stratify samples across the nominal +workspace, reach boundaries, joint-limit neighborhoods, low-manipulability +postures, and deliberate reference-normal degeneracies. The trajectory set +will include workspace sweeps, repeated reach--retract motion, overhead motion, +and passages through fallback regions. Dataset sizes +$N_q$ and $N_{\mathrm{traj}}$ will be set by a precision analysis so that the +bootstrap 95\% confidence-interval half-width for the paired +composite-event-rate difference is below a preregistered tolerance +$\Delta_{\mathrm{SEW}}$. The tolerance and resulting sizes will be frozen +before executing the locked set. + +Because master and slave link lengths differ, position error will be measured +relative to the feasible slave target rather than directly between physical +wrists: +\begin{equation} + e_p=\|p_{W_s}-p_{W_s}^{\mathrm{ref}}\|. +\end{equation} +Additional outcomes are +\begin{align} + e_{\mathrm{dir}} + &=\cos^{-1}\!\left( + \operatorname{clip}_{[-1,1]}(\hat{x}_s^\top\hat{x}_m)\right),\\ + e_\phi + &=\left|\operatorname{wrap}_{[-\pi,\pi)} + (\phi_s-\phi_m)\right|,\\ + e_R + &=\cos^{-1}\!\left( + \operatorname{clip}_{[-1,1]}\!\left[ + \frac{\operatorname{tr}((R_{s,\mathrm{EE}}^{\mathrm{ref}})^\top + R_{s,\mathrm{EE}})-1}{2}\right] + \right). +\end{align} +For locked trajectory $r$, let $v_k=1$ denote a valid bounded recovery, +$\Delta q_s^{\,w}[k]=\operatorname{wrap}(q_s[k]-q_s[k-1])$, and +$\Delta\phi_s^{\,w}[k]= +\operatorname{wrap}(\phi_s[k]-\phi_s[k-1])$. These increments are evaluated +between adjacent accepted 50-Hz retargeting updates. Define +\begin{align} + F_r&=\mathbf{1}\!\left\{\exists k: + v_k=0\ \vee\ e_p[k]>\bar e_p\right.\nonumber\\[-1mm] + &\hspace{37mm}\left.\vee\ e_R[k]>\bar e_R\right\},\\ + D_r&=\mathbf{1}\!\left\{\exists k: + |\Delta\phi_s^{\,w}[k]|>\bar\delta_\phi\right.\nonumber\\[-1mm] + &\hspace{31mm}\left.\vee\ + \|\Delta q_s^{\,w}[k]\|_\infty>\bar\delta_q\right\},\\ + C_r&=\max(F_r,D_r). + \label{eq:sew_composite_event} +\end{align} +The discontinuity thresholds apply only between adjacent, valid master +samples whose increment is below a frozen input-step threshold; commanded +resets and preregistered degeneracy transitions are labeled separately rather +than silently excluded. All tolerances in +\eqref{eq:sew_composite_event} are selected on calibration trajectories and +frozen before the locked set. Thus $D_r$, rather than an unobservable latent +branch label, is the operational definition of a branch-discontinuity event. +The single primary H1 endpoint is the paired +trajectory-level rate of $C_r=1$. Solver/tolerance failure $F_r$, +discontinuity $D_r$, elbow-position error, joint-limit violations, +reach-clipping rate, differential validity, trajectory jerk, and runtime are +secondary or diagnostic endpoints. Runtime on the deployed processor will be +reported using median, 95th-percentile, 99th-percentile, and maximum values. A +real-time claim will be made only if the locked timing criterion is satisfied. +Absolute, normalized, and calibrated reach-transfer policies are compared as +a separate ablation. + +\subsection{Interaction-Wrench Estimation} +\label{sec:wrench_protocol} + +The current shared-model simulation can verify residual/DLS algebra and +sensitivity to prescribed numerical perturbations, but it cannot supply +independent wrench ground truth or support H2. Physical H2 evidence begins +only after the following sensor and model-calibration protocol passes G1. + +The wrist force/torque sensor will provide independent ground truth after tool +gravity, payload, coordinate-transform, and timestamp calibration. The test +matrix will contain: +\begin{enumerate} + \item unloaded free-space motion for bias and false-contact assessment; + \item quasi-static forces and moments in positive and negative directions + along all six wrench axes; + \item bounded multiaxial dynamic loading and contact transients; + \item configurations stratified by the minimum singular value and + condition number of the frozen scaled Jacobian $\widetilde J_s$; + \item payload, inertial-parameter, friction, and torque-noise perturbations + within the characterized uncertainty envelope. +\end{enumerate} +Each load, pose, direction, and motion condition will be repeated in randomized +order. The repeat count will be chosen so that the 95\% interval half-width for +each co-primary RMSE is below preregistered force and moment tolerances. + +Comparisons will include an undamped residual pseudoinverse, fixed-damping +residual inversion, removal of identified friction compensation, and removal +of bias correction. Damping and all observer parameters will be selected only +from calibration data. If a wrench filter is added in an exploratory analysis, +it will be reported as a distinct, non-confirmatory method rather than folded +into the current estimator. + +Force and moment will be analyzed separately. Outcomes will include RMSE, MAE, +signed bias, 95th-percentile absolute error, direction error, correlation, +frequency-response gain and phase, contact-onset latency, free-space +false-positive rate, and peak error. Error will also be plotted against +Jacobian singular value, speed, acceleration, payload perturbation, and torque +noise. Near-singular trials will not be removed or hidden inside a +workspace-wide average. +Force RMSE and moment RMSE over the locked matrix are the co-primary estimator +endpoints; the remaining quantities diagnose when and why errors occur. + +\subsection{Mapping Consistency, Energy, Delay, and Transparency} +\label{sec:energy_protocol} + +The non-human fixture study will compare: +\begin{enumerate} + \item the differential-dual force map with haptic-energy projection; + \item the same force map without the energy projection; + \item a direct master-Jacobian-transpose wrench reflection with matched + scale, shaping, and saturation; + \item an $A^\top J_s^\top\hat F$ variant using the same DLS wrench as the + direct baseline; + \item a discrete-time passivity observer/controller (PO/PC) baseline + following Hannaford and Ryu~\cite{Hannaford2002TDPC}, implemented at the + same master haptic port. +\end{enumerate} +Only items 1--3 are executable in the current simulator, and even their +item~1--item~3 contrast is the end-to-end chain comparison identified in +Table~\ref{tab:code_evidence_boundary}. Item 4 is required before any pure +$A^\top$-versus-$J_m^\top$ mapping claim, and item 5 is required before the +fixture and human comparisons; both must pass G0a and the locked G0c runner +before G3. +The PO/PC baseline will receive the same raw mapping, upstream filter, nominal +gain, and actuator limits; only its passivity-intervention law differs, and its +parameters are selected from the calibration set. The primary H3 mapping +contrast is item 4 versus item 3 because both receive +the same reconstructed wrench; it will also be repeated offline with the +independent force/torque reference. The full proposed path in item 1 uses the +unprojected bias-corrected joint residual, whereas item 3 uses the DLS wrench. +Their comparison is therefore labeled an end-to-end chain comparison, not a +pure mapping ablation. +Signal filtering, torque limits, environment trajectories, and network traces +will be identical across paired comparisons. Single-factor ablations will +separately remove the generalized-torque filter, rate limiting, torque +saturation when fixture safety permits, and energy projection; multiple +elements will not be removed under one ablation label. ``Without energy +projection'' retains all other limits and is not described as an unprotected +controller. + +The environment will include free space and preregistered stiffness and damping +levels within the verified fixture and actuator safety envelope. A network +emulator will impose constant, asymmetric, and time-varying delays, bounded +jitter, and packet loss. The matrix will include a zero-delay reference, the +measured nominal network condition, at least two fixed-delay levels, and one +recorded jitter/loss trace within the G3 safety envelope. Exact levels and +repeat counts will be locked before confirmatory execution. +The present implementation contains only a fixed return-delay queue; it does +not yet implement independent forward/return channels, packet sequencing, +jitter/loss injection, or timeout transitions. The semantics below are +therefore requirements to be implemented and deterministically tested before +G0c/G3, not descriptions of the current executable. + +Network semantics are fixed independently of those traces. Let +$\kappa_f(k)$ and $\kappa_r(k)$ be the newest accepted source indices at slave +and master sample $k$ for the forward reference packet $u_f$ in +\eqref{eq:forward_held_reference} and the return residual packet, +respectively. The return packet contains the residual, reconstructed wrench, +$({}^{C}\!J_{E_s})^\top{}^C\!\hat F_{\mathrm{int}}$, actual slave velocity, +active forward-map identifier $\mu$, source timestamp, and sequence number; +this payload supports both direct and matched-wrench baselines. +Packets use synchronized clocks and monotonically increasing sequence numbers. +Duplicates, corrupted packets, and packets no newer than the last accepted +sequence number are discarded; no interpolation or retransmission is +introduced by the experiment. The last accepted sample is held until its +preregistered channel timeout. A forward timeout invokes the same bounded +zero-velocity safe hold in every condition. A return timeout sets the raw +haptic target to zero, after which the declared filter, rate limit, saturation, +and energy projection produce the controlled fade. This sets $\chi_r[k]=0$; +accepting a fresh in-time packet sets it to one. Every arrival, rejection, loss, +held-sample age, active $\mu$, and timeout transition is logged. Constant +return delay on the 500-Hz channel is the special case +$\kappa_r(k)=k-d$. Because forward packets exist only on the 50-Hz update set +$\mathcal I_f$, a constant forward delay $d_f$ is instead +\begin{equation} + \kappa_f(k)=\max\{i\in\mathcal I_f:t_i^{f}+d_f\le t_k^{s}\}, +\end{equation} +when that set is nonempty. Thus asymmetric and time-varying traces change the +held references and their echoed map identifiers without changing controller +semantics. + +The fixed-branch algebraic identity will first be checked using +\begin{equation} + e_P^{\mathrm{map}}[k]= + \tau_{h,\mathrm{raw}}[k]^\top\dot q_m[k] + -\chi_r[k]\sigma_f + \hat\tau_{s,\mathrm{int}}[\kappa_r(k)]^\top + \dot q_s^{\mathrm{map}}[k]. +\end{equation} +where the residual in the second term is the same delayed sample +$\hat\tau_{s,\mathrm{int}}[\kappa_r(k)]$ used in +\eqref{eq:raw_master_torque}. This is a numerical contract check rather than a +performance endpoint. Errors +$\|\dot q_s^{\mathrm{ctrl}}-\dot q_s^{\mathrm{map}}\|$ and +$\|\dot q_s-\dot q_s^{\mathrm{ctrl}}\|$ separately quantify limiting/hold and +tracking effects. The current-map and source-stamped delay policies are +compared using +\begin{equation} + e_{\tau,A}[k]= + \chi_r[k]\sigma_f\left\|\left(\bar A[k] + -\bar A[\mu(\kappa_r(k))]\right)^\top + \hat\tau_{s,\mathrm{int}}[\kappa_r(k)]\right\|_2 . +\end{equation} +The primary H3 endpoint uses actual slave velocity and aligned source/arrival +timestamps. For condition $c$, let $\tau_{s,c}$ be the source generalized +torque actually supplied to its mapping: the bias-corrected residual for the +full proposed chain and +$({}^{C}\!J_{E_s})^\top{}^C\!\hat F_{\mathrm{int}}$ for the two matched-wrench +mapping conditions. Then +\begin{align} + P_{m,c}^{\mathrm{raw}}[k] + &=\tau_{m,c}^{\mathrm{raw}}[k]^\top\dot q_m[k],\\ + P_{s,c}[\kappa_r(k)] + &=\chi_r[k]\tau_{s,c}[\kappa_r(k)]^\top + \dot q_s[\kappa_r(k)]. +\end{align} +\begin{equation} + \epsilon_{P,c}^{\mathrm{act}}= + \frac{\sum_k|P_{m,c}^{\mathrm{raw}}[k] + -\sigma_fP_{s,c}[\kappa_r(k)]|\Delta t_k} + {\frac{1}{2}\sum_k\left( + |P_{m,c}^{\mathrm{raw}}[k]| + +|\sigma_fP_{s,c}[\kappa_r(k)]|\right)\Delta t_k+\epsilon_P}. + \label{eq:normalized_actual_power_mismatch} +\end{equation} +Here $\epsilon_P>0$ has energy units and is a locked numerical regularizer +used only for near-zero-power trials. +To prevent the stored lower clip from hiding an accounting defect, every +sample will independently reconstruct +\begin{align} + E_{\mathrm{audit}}^+[k+1] + &=E[k]-\tau_{h,\mathrm{app}}[k]^\top + \dot q_m[k]\Delta t_k,\\ + \delta_E^-[k] + &=\max(0,E_{\min}-E_{\mathrm{audit}}^+[k+1]). +\end{align} +The budget component of H4 is a deterministic gate: all independently +reconstructed samples must satisfy +$\delta_E^-[k]\le\bar\delta_E$, where $\bar\delta_E$ is the frozen numerical +roundoff tolerance. To make the projection-bypass comparison explicit, each +paired trajectory $r$ also carries an independently reconstructed shadow +budget for condition $c$: +\begin{align} + E_{c,r}^{\mathrm{sh}}[0]&=E[0],\\ + E_{c,r}^{\mathrm{sh}}[k+1] + &=\min\!\left(E_{\max}, + E_{c,r}^{\mathrm{sh}}[k] + -P_{h,c}[k]\Delta t_k\right),\\ + B_{c,r}&=\max_k\max\!\left( + 0,E_{\min}-E_{c,r}^{\mathrm{sh}}[k]\right). + \label{eq:shadow_floor_deficit} +\end{align} +For the confirmatory enforcement contrast, $c=\mathrm{proj}$ uses the actual +projected-run output, while $c=\mathrm{shadow}$ is an offline counterfactual +that substitutes that same run's logged $\tau_h^{\mathrm{cand}}$ for the +applied torque without feeding it back into the filter, rate limiter, robot, or +trajectory; explicitly, +$P_{h,\mathrm{proj}}=\tau_{h,\mathrm{app}}^\top\dot q_m$ and +$P_{h,\mathrm{shadow}}=(\tau_h^{\mathrm{cand}})^\top\dot q_m$ on the same +logged samples. This avoids falsely assuming that two live closed-loop +conditions retain an identical candidate sequence after intervention. The primary +floor-deficit reduction is +$\Delta B_r=B_{\mathrm{shadow},r}-B_{\mathrm{proj},r}$ on a +calibration-selected, energy-challenging fixture subset. The live projection- +bypass run remains an end-to-end secondary comparison with its own +condition-specific candidate and trajectory. + +The single primary transparency-cost scalar is the trajectory-level, +torque-space projection distortion +\begin{equation} + D_{\mathrm{proj},r}= + \frac{\sum_k\|\tau_{h,\mathrm{app}}[k] + -\tau_h^{\mathrm{cand}}[k]\|_2\Delta t_k} + {\sum_k\|\tau_h^{\mathrm{cand}}[k]\|_2\Delta t_k+\epsilon_\tau}, + \label{eq:projection_distortion} +\end{equation} +where $\epsilon_\tau>0$ has units of \si{\newton\metre\second} and is frozen +for near-zero-torque trials. This metric quantifies transparency only at the +generalized-torque projection level. Lower is better, and an acceptable upper +limit $\bar D_{\mathrm{proj}}$ is set from calibration/fixture data before +confirmatory trials. H4 is accepted only if the deterministic audit passes, +the one-sided 95\% confidence interval for the paired mean +$\Delta B_r$ lies above a frozen nonnegative superiority margin, and the +one-sided 95\% upper confidence bound for mean $D_{\mathrm{proj},r}$ does not +exceed $\bar D_{\mathrm{proj}}$. Haptic-storage diagnostics will include +$\min_k(E_{\mathrm{audit}}^+[k]-E_{\min})$, +$\max_k\delta_E^-[k]$, discrepancy between the reconstructed pre-clip value +and the software pre-clip value, number and duration of violations beyond a +fixed numerical tolerance, and signed net released energy from +\eqref{eq:net_haptic_energy_bound}. +Secondary transparency outcomes will include motion-tracking error, +force-reflection gain and phase, contact-force error, time with +$\rho<\bar\rho$, and rendered-impedance error. Oscillation amplitude, settling +time, spectral energy, torque-limit events, safety stops, and divergent trials +will be reported as diagnostics. + +Any passivity or stability statement will be limited to its analytical +assumptions and experimentally tested envelope. The absence of instability in +a finite dataset will not be described as proof for arbitrary stiffness or +delay. + +\subsection{End-to-End Teleoperation Tasks} +\label{sec:end_to_end_protocol} + +Integrated testing will progress through: +\begin{enumerate} + \item three-dimensional free-space path tracking; + \item force-regulated following of an instrumented surface; + \item constrained insertion with randomized initial pose and clearance; + \item manipulation of an instrumented compliant or fragile object. +\end{enumerate} +Targets, paths, object parameters, timeouts, and termination rules will be +sampled from a locked set. The complete method will be compared with no +kinesthetic feedback and the locked PO/PC baseline. Unsafe +unregulated conditions will remain fixture-only. + +The primary integrated-task outcome is success probability under a +preregistered success definition. Completion time, Cartesian and force +tracking error, peak unintended contact force, contact impulse, unintended +contacts, insertion retries, and safety stops are secondary outcomes. A +timeout or safety stop counts as failure; completion time among successful +trials and failure probability are analyzed separately. + +\subsection{Human-Subject Protocol} +\label{sec:user_protocol} + +Participant recruitment will begin only after approval by the appropriate +institutional ethics committee. The approval identifier, informed-consent +procedure, inclusion and exclusion criteria, anthropometric fitting range, +compensation, and withdrawal procedure will be reported. The confirmatory +sample size will be determined by an a priori repeated-measures power analysis +for the primary contrast, with $\alpha=0.05$, power of at least 0.80, +anticipated attrition declared separately, and an effect-size assumption +supported by pilot data or relevant literature. The calculation and resulting +integer sample size will be preregistered before confirmatory recruitment. +Pilot participants used for tuning will not enter the confirmatory analysis. + +A randomized, counterbalanced, within-participant design will compare: +\begin{enumerate} + \item the proposed energy-supervised kinesthetic feedback; + \item the locked PO/PC kinesthetic baseline; + \item visual teleoperation without kinesthetic force feedback. +\end{enumerate} +Direct unregulated force reflection will not be applied to human participants. +Participants will receive standardized fitting, training, and +criterion-based familiarization. Condition and task order will be balanced, +and mandatory rest intervals will reduce fatigue. + +The primary human-performance outcome is surface-following force RMSE; any +change requires a dated protocol amendment before confirmatory recruitment. +Secondary outcomes include success, completion time, path error, peak +unintended force, contact impulse, corrective motions, and safety +interventions. Subjective outcomes include NASA-TLX workload +~\cite{Hart1988TLX}, perceived force +fidelity, comfort, and preference. A separate psychophysical stiffness- +discrimination block may be included only if it is fixed before recruitment; +it will not replace the preregistered robotic-task endpoint. + +\subsection{Statistical Analysis} +\label{sec:statistics_protocol} + +The configuration or trajectory is the experimental unit for component tests; +the participant is the experimental unit for the human study. Repeated trials +from one participant will not be treated as independent human samples. +Algorithm comparisons on identical inputs will use paired analyses. + +Component-level outcomes will be summarized using mean, standard deviation, +median, interquartile range, bootstrap 95\% confidence interval, and relevant +tail percentiles. Human continuous outcomes will be analyzed with +mixed-effects models containing feedback condition, task, delay when +applicable, and their preregistered interactions as fixed effects and +participant as a random effect. Binary success will use a logistic +mixed-effects model; ordinal responses will use an ordinal model when +appropriate. + +Assumptions, residuals, and heteroscedasticity will be checked. Robust, +transformed, or preregistered nonparametric alternatives will be used when +needed. The four confirmatory algorithmic outcome families are fixed as: +H1, the single composite event $C_r$ in +\eqref{eq:sew_composite_event}; H2, force RMSE and moment RMSE as two +co-primary estimator endpoints; H3, normalized actual power mismatch +$\epsilon_{P,c}^{\mathrm{act}}$; and H4, the deterministic storage gate plus +paired floor deficit $\Delta B_r$ and the single torque-level transparency-cost +endpoint $D_{\mathrm{proj},r}$. + +All confirmatory directions and margins are frozen from calibration and +engineering tolerances before the locked data are inspected. H1 uses the +matched task-priority IK as its primary comparator and passes when the +one-sided 95\% upper confidence bound for +$p(C_r=1)_{\mathrm{SEW}}-p(C_r=1)_{\mathrm{IK}}$ is below +$-\Delta_{\mathrm{H1}}$. H2 passes only when the corresponding upper bounds +for both +$\mathrm{RMSE}_{F,\mathrm{DLS}}-\mathrm{RMSE}_{F,\mathrm{undamped}}$ and +$\mathrm{RMSE}_{M,\mathrm{DLS}}-\mathrm{RMSE}_{M,\mathrm{undamped}}$ are below +their negative superiority margins. H3 passes when the upper bound for the +paired mean difference +$\epsilon_{P,A^\top J_s^\top F}^{\mathrm{act}} +-\epsilon_{P,J_m^\top F}^{\mathrm{act}}$ is below +$-\Delta_{\mathrm{H3}}$. H4 uses the three conjunctive rules stated after +\eqref{eq:projection_distortion}; failure of any one rejects H4. Every margin +is nonnegative and has zero as the fallback only when calibration cannot +justify a larger practically relevant value. + +These are one-sided superiority or acceptability decisions; two-sided 95\% +intervals and effect sizes will still be reported. Additional pairwise method +comparisons will use Holm correction within the corresponding H1--H4 family. +Integrated-task and human outcomes form separately declared external-validity +families; all remaining measures are labeled secondary or diagnostic. Exact +$p$ values will be reported. Missing data, aborted trials, sensor dropouts, +outlier rules, and protocol deviations will be disclosed. + +\subsection{Reproducibility and Data Availability} +\label{sec:reproducibility_protocol} + +The replication package will contain the exact URDF and calibration files, +controller parameters, test trajectories, network-emulator traces, random +seeds, anonymized trial-level data, and analysis and figure-generation scripts. +The operating system, processor, real-time settings, software versions, +execution thread, and timing method will be reported. Raw and filtered signals +will remain distinguishable, and coordinate transforms, spatial-vector order, +units, and sign conventions will be machine-readable. + +All parameter sweeps, failed trials, safety stops, and protocol deviations will +be retained. Each final table and figure will be generated from one versioned +analysis entry point, and the released code and data will be archived at an +immutable commit and DOI subject to participant privacy and hardware-vendor +restrictions. + +\section{Results} +\label{sec:results} + +No quantitative result is reported in the present working draft. After the +protocol has been executed, this section will report outcomes in the following +fixed order: (i) deterministic software contracts and the locked multi-run +simulation, explicitly labeled numerical, (ii) hardware characterization and +timing, (iii) component-level retargeting and wrench estimation, +(iv) non-human mapping-consistency, energy, and delay tests, +(v) end-to-end task performance, and (vi) human-subject outcomes. Failed and +terminated trials will be included according to the rules above. + + +\section{Discussion and Limitations} +\label{sec:discussion} + +\subsection{Methodological Trade-offs} + +\textbf{SEW degeneracy and branch continuity.} +The reference-normal construction becomes ill-conditioned when the +shoulder--wrist direction approaches the reference axis. A fallback direction +keeps the equations defined but does not automatically make the threshold +transition smooth. Similarly, an analytic Cartesian elbow target does not +guarantee a continuous slave joint trajectory. The current implementation uses +warm-started recovery and validity gates rather than swivel hysteresis or +normal blending; failure behavior must therefore be evaluated explicitly. + +\textbf{Reach feasibility and differential validity.} +Reach clipping makes the target feasible but changes the local motion scale and +may introduce a nonsmooth boundary. The differential map in +\eqref{eq:retarget_differential} is therefore valid only on a fixed smooth +branch. Clipping and joint-limit events must be excluded from a smooth-map +proof or handled by a hybrid formulation, while remaining included in the +empirical failure analysis. + +\textbf{Residual-wrench identifiability.} +The inverse-dynamics residual contains contact torque together with inertial, +friction, bias, and sensing errors. Damped inversion reduces amplification but +introduces bias and cannot recover unobservable wrench components. Independent +force/torque ground truth and configuration-stratified analysis are necessary +before describing the estimator as robust. + +\textbf{Energy constraint versus bilateral stability.} +The energy projection constrains only the sampled haptic feedback contribution +under a specific timing and sign convention. Dynamics compensation, device +mechanics, the communication channel, and the slave port lie outside this +storage. A stronger total-port or bilateral claim requires an explicit +two-port treatment and a proof whose assumptions include filters, saturation, +sampling, compensation, and delay. + +\textbf{Transparency versus intervention.} +Any reduction of candidate torque used to preserve an energy reserve alters the +rendered impedance. The relevant question is therefore not whether the method +is simultaneously ``fully transparent'' and constrained, but how much force +fidelity, phase, and task performance are lost for a measured reduction in +released energy or oscillatory behavior. + +\subsection{Current Limitations and Validity Threats} + +\begin{itemize} + \item \textbf{Prototype status.} The master exoskeleton is under + fabrication, so mass, torque capability, backdrivability, bandwidth, + alignment, comfort, timing, and safety performance are not yet empirical + contributions. + + \item \textbf{Implementation and calibration status.} The software core now + includes bounded recovery, guarded $A$, the declared spatial-vector order, + direct residual mapping, and final haptic-energy projection. The current + simulation nevertheless assumes aligned base axes, a simulated TCP, + synchronous clocks, fixed delay, and simplified rigid-body contact. + Calibration ingestion for the master/common/slave transforms and + $R_\Delta$ is not yet implemented; timestamps, torque calibration, and + total-port energy measurements also remain pending. + + \item \textbf{Computational burden.} The current wrapper executes two + nominal recoveries and fourteen perturbed recoveries per differential + update. The present multirate implementation does not establish real-time + feasibility on the target processor; validity rate and tail latency may + require solve reuse, an analytic or differentiated map, or a reduced-order + alternative. + + \item \textbf{Model dependence.} Wrench reconstruction depends on the + slave rigid-body model, payload identification, friction compensation, and + measured joint torque. Conclusions may not transfer to position-controlled + robots without comparable sensing. + + \item \textbf{Finite evaluation envelope.} Contact stiffness, delay, + jitter, packet loss, motion speed, subject anthropometry, and tasks will + cover finite preregistered ranges. Claims must remain within those ranges. + + \item \textbf{Human-study generalizability.} A laboratory within-subject + study cannot by itself establish long-term comfort, training effects, or + performance in field environments. Participant diversity and device-fit + exclusions must be disclosed. +\end{itemize} + +These limitations are incorporated into the prospective protocol rather than +being inferred after observing favorable or unfavorable results. + +\section{Conclusion} +\label{sec:conclusion} + +This paper formulates a geometry- and energy-aware architecture for +heterogeneous 7-DoF exoskeleton teleoperation. The central distinction is +between an analytic SEW Cartesian target, platform-specific joint recovery, and +the differential map that defines a virtual-work-consistent force-reflection +objective. The proposed feedback path maps a bias-corrected generalized +slave residual directly through $A^\top$; DLS wrench reconstruction is retained +for physical interpretation and Cartesian baselines. Final-stage radial +projection supplies a one-step energy invariant for the shaped haptic +contribution, not for the complete bilateral system. + +The present working draft intentionally makes no quantitative hardware, +real-time, accuracy, passivity, stability, transparency, or usability claim. +Instead, it fixes the evidence required to evaluate those claims: measured +exoskeleton characterization, paired retargeting baselines, independent +six-axis wrench ground truth, finite contact and network envelopes, +single-factor ablations, end-to-end tasks, and a powered human-subject study. +After the prototype and software implementation are complete, the protocol will +be executed without replacing failed trials or retrospectively changing the +primary outcomes. The abstract, results, discussion, and conclusion will then +be updated using measured effect sizes and uncertainty intervals. + +% conference papers do not normally have an appendix + +% Add an acknowledgment section only after funding and contributor information +% have been confirmed. +% \section*{Acknowledgment} + +% trigger a \newpage just before the given reference +% number - used to balance the columns on the last page +% adjust value as needed - may need to be readjusted if +% the document is modified later +% The "triggered" command can be changed if desired: +%\IEEEtriggercmd{\enlargethispage{-5in}} +\IEEEtriggeratref{24} + +% references section + +% can use a bibliography generated by BibTeX as a .bbl file +% BibTeX documentation can be easily obtained at: +% http://mirror.ctan.org/biblio/bibtex/contrib/doc/ +% The IEEEtran BibTeX style support page is at: +% http://www.michaelshell.org/tex/ieeetran/bibtex/ +%\bibliographystyle{IEEEtran} +% argument is your BibTeX string definitions and bibliography database(s) +%\bibliography{IEEEabrv,../bib/paper} +% +% manually copy in the resultant .bbl file +% set second argument of \begin to the number of references +% (used to reserve space for the reference number labels box) + +\begin{thebibliography}{99} + +\bibitem{Rebelo2014Teleop} +J. Rebelo, T. Sednaoui, E. B. den Exter, T. Krueger, and A. Schiele, +``Bilateral robot teleoperation: A wearable arm exoskeleton featuring an intuitive user interface,'' +\textit{IEEE Robotics and Automation Magazine}, vol.~21, no.~4, pp.~62--69, 2014. + +\bibitem{Jiang2019Telerehab} +J.~Chen, D.~Hu, W.~Sun, X.~Tu, and J.~He, +``A novel telerehabilitation system based on bilateral upper limb exoskeleton robot,'' +in \emph{Proc. IEEE Int. Conf. Real-time Computing and Robotics (RCAR)}, 2019, pp.~391--396. + +\bibitem{Gull2020ExoReview} +M.~A.~Gull, S.~Bai, and T.~Bak, +``A review on design of upper limb exoskeletons,'' +\emph{Robotics}, vol.~9, no.~1, p.~16, 2020. + +\bibitem{Kim2017Harmony} +B.~Kim and A.~D.~Deshpande, +``An upper-body rehabilitation exoskeleton Harmony with an anatomical shoulder mechanism: Design, modeling, control, and performance evaluation,'' +\emph{Int. J. Robotics Research}, vol.~36, no.~4, pp.~414--435, 2017. + +\bibitem{Pisetskiy2021MRclutch} +S.~Pisetskiy and M.~Kermani, +``High-performance magneto-rheological clutches for direct-drive actuation: Design and development,'' +\emph{J. Intelligent Material Systems and Structures}, vol.~32, no.~20, pp.~2582--2600, Oct.~2021. + +\bibitem{Wang2023Dexcap} +C.~Wang, H.~Shi, W.~Wang, R.~Zhang, L.~Fei-Fei, and K.~Liu, +``DexCap: Scalable and portable mocap data collection system for dexterous manipulation,'' +in \emph{Proc. Robotics: Science and Systems (RSS)}, 2024, +doi: 10.15607/RSS.2024.XX.043. + +\bibitem{Fang2023Airexo} +H.~Fang, H.-S.~Fang, Y.~Wang, J.~Ren, J.~Chen, R.~Zhang, W.~Wang, and C.~Lu, +``Airexo: Low-cost exoskeletons for learning whole-arm manipulation in the wild,'' +in \emph{Proc. IEEE Int. Conf. Robotics and Automation (ICRA)}, 2024, pp.~15031--15038. + +\bibitem{Zimmermann2020Transparency} +Y.~Zimmermann, E.~B.~Kucuktabak, F.~Farshidian, R.~Riener, and M.~Hutter, +``Towards dynamic transparency: Robust interaction force tracking using multi-sensory control on an arm exoskeleton,'' +in \emph{Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems (IROS)}, 2020, pp.~7417--7424. + +\bibitem{Indri2020FrictionID} +M.~Indri and S.~Trapani, +``Framework for static and dynamic friction identification for industrial manipulators,'' +\emph{IEEE/ASME Trans. Mechatronics}, vol.~25, no.~3, pp.~1589--1599, 2020. + +\bibitem{Buongiorno2018WRES} +D.~Buongiorno, E.~Sotgiu, D.~Leonardis, S.~Marcheschi, M.~Solazzi, and A.~Frisoli, +``WRES: A novel 3-DoF wrist exoskeleton with tendon-driven differential transmission for neuro-rehabilitation and teleoperation,'' +\emph{IEEE Robotics and Automation Letters}, vol.~3, no.~3, pp.~2152--2159, Jul.~2018. + +\bibitem{Yan2014ShoulderExo} +H.~Yan, C.~Yang, Y.~Zhang, and Y.~Wang, +``Design and validation of a compatible 3-degrees-of-freedom shoulder exoskeleton with an adaptive center of rotation,'' +\emph{J. Mechanical Design}, vol.~136, no.~7, p.~071006, 2014. + +\bibitem{Lee2014Teaching} +H. Lee, J. Kim, and T. Kim, +``A robot teaching framework for a redundant dual arm manipulator with teleoperation from exoskeleton motion data,'' +in \textit{2014 IEEE-RAS International Conference on Humanoid Robots (Humanoids)}, 2014, pp.~1057--1062. + +\bibitem{Yang2024ACE} +S.~Yang, M.~Liu, Y.~Qin, R.~Ding, J.~Li, X.~Cheng, R.~Yang, S.~Yi, and X.~Wang, +``ACE: A cross-platform and visual-exoskeletons system for low-cost dexterous teleoperation,'' +in \emph{Proc. 8th Conf. Robot Learning (CoRL)}, ser. PMLR, +vol.~270, pp.~4895--4911, 2025. + +\bibitem{HOMIE2025} +Qingwei Ben, Fang Jia, Jiang Zeng, Jiahui Dong, Dahua Lin, and Jiangmiao Pang, +``HOMIE: Humanoid loco-manipulation with isomorphic exoskeleton cockpit,'' +arXiv preprint arXiv:2502.13013, 2025. + +\bibitem{Qin2023AnyTeleop} +Y.~Qin, W.~Yang, B.~Huang, K.~Van Wyk, H.~Su, X.~Wang, Y.-W.~Chao, and D.~Fox, +``AnyTeleop: A general vision-based dexterous robot arm-hand teleoperation system,'' +in \emph{Proc. Robotics: Science and Systems (RSS)}, 2023, +doi: 10.15607/RSS.2023.XIX.015. + +\bibitem{Fu2024MobileALOHA} +Z.~Fu, T.~Z.~Zhao, and C.~Finn, +``Mobile ALOHA: Learning bimanual mobile manipulation using low-cost whole-body teleoperation,'' +in \emph{Proc. 8th Conf. Robot Learning (CoRL)}, ser. PMLR, +vol.~270, pp.~4066--4083, 2025. + +\bibitem{Iyer2024OpenTeach} +A.~Iyer, Z.~Peng, Y.~Dai, I.~Guzey, S.~Haldar, S.~Chintala, and L.~Pinto, +``OPEN TEACH: A versatile teleoperation system for robotic manipulation,'' +in \emph{Proc. 8th Conf. Robot Learning (CoRL)}, ser. PMLR, +vol.~270, pp.~2372--2395, 2025. + +\bibitem{Sinha2019IK} +A. Sinha and N. Chakraborty, +``Geometric search-based inverse kinematics of 7-DoF redundant manipulator with multiple joint offsets,'' +in \textit{2019 International Conference on Robotics and Automation (ICRA)}, 2019, pp.~5592--5598. + +\bibitem{Brahmi2019IK} +Bilel Brahmi, Maarouf Saad, M. H. Rahman, and C. Ochoa-Luna, +``Cartesian trajectory tracking of a 7-DOF exoskeleton robot based on human inverse kinematics,'' +\textit{IEEE Transactions on Systems, Man, and Cybernetics: Systems}, vol.~49, no.~3, pp.~600--611, 2019. + +\bibitem{Cheng2024HomoHetero} +Cheng Cheng, Wen Dai, Tianyu Wu, Xinyu Chen, Ming Wu, Jia Yu, Jiahui Jiang, and Hongqiang Lu, +``Efficient and precise homo-hetero teleoperation based on an optimized upper limb exoskeleton,'' +\textit{IEEE/ASME Transactions on Mechatronics}, vol.~30, no.~5, +pp.~3722--3734, 2025, doi: 10.1109/TMECH.2024.3479873. + +\bibitem{Mohammadi2013DO} +Amir Mohammadi, M. Tavakoli, Humberto J. Marquez, and F. Hashemzadeh, +``Nonlinear disturbance observer design for robotic manipulators,'' +\textit{Control Engineering Practice}, vol.~21, no.~3, pp.~253--267, 2013. + +\bibitem{Toedtheide2023ForceSensitive} +Alexander Toedtheide, Xuanchen Chen, Hamed Sadeghian, Amir Naceri, and Sami Haddadin, +``A Force-Sensitive Exoskeleton for Teleoperation: An Application in Elderly Care Robotics,'' +in \textit{2023 IEEE International Conference on Robotics and Automation (ICRA)}, 2023, pp.~12624--12630. + +\bibitem{Forouhar2024TactileExo} +Mohammad Forouhar, Hamed Sadeghian, Diego Pablo Suay, Amir Naceri, and Sami Haddadin, +``A tactile lightweight exoskeleton for teleoperation: Design and control performance,'' +in \textit{2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 2024, pp.~178--183. + +\bibitem{Li2020VisionTeleop} +Shihui Li, Xuguang Ma, Hongyi Liang, Matthias G\"{o}rner, Philipp Ruppel, Bowen Fang, Fuchun Sun, and Jianwei Zhang, +``Vision-based teleoperation of Shadow dexterous hand using end-to-end deep neural network,'' +in \textit{2019 International Conference on Robotics and Automation (ICRA)}, 2019, pp.~416--422. + +\bibitem{Hannaford2002TDPC} +B.~Hannaford and J.-H.~Ryu, +``Time-domain passivity control of haptic interfaces,'' +\emph{IEEE Trans. Robotics and Automation}, vol.~18, no.~1, pp.~1--10, +2002, doi: 10.1109/70.988969. + +\bibitem{Buongiorno2019Teleop} +Dario Buongiorno, Davide Chiaradia, Simone Marcheschi, Marco Solazzi, and Antonio Frisoli, +``Multi-DOFs exoskeleton-based bilateral teleoperation with the time-domain passivity approach,'' +\textit{Robotica}, vol.~37, no.~9, pp.~1641--1662, 2019. + +\bibitem{Porcini2020Passivity} +Francesco Porcini, Davide Chiaradia, Simone Marcheschi, Marco Solazzi, and Antonio Frisoli, +``Evaluation of an exoskeleton-based bimanual teleoperation architecture with independently passivated slave devices,'' +in \textit{2020 IEEE International Conference on Robotics and Automation (ICRA)}, 2020, pp.~10205--10211. + +\bibitem{Elias2024SEW} +A.~J.~Elias and J.~T.~Wen, +``Redundancy parameterization and inverse kinematics of 7-DoF revolute manipulators,'' +\textit{Mechanism and Machine Theory}, vol.~204, art.~no.~105824, 2024, +doi: 10.1016/j.mechmachtheory.2024.105824. + +\bibitem{Wu2023Gello} +P.~Wu, Y.~Shentu, Z.~Yi, X.~Lin, and P.~Abbeel, +``Gello: A general, low-cost, and intuitive teleoperation framework for robot manipulators,'' +in \emph{Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems (IROS)}, +2024, pp.~12156--12163. + +\bibitem{Rong2020FrankMocap} +Yifei Rong, Takeo Shiratori, and Hanbyul Joo, +``FrankMocap: Fast monocular 3D hand and body motion capture by regression and integration,'' +arXiv preprint arXiv:2008.08324, 2020. + +\bibitem{DeLuca2006Collision} +Alessandro De~Luca, Alin Albu-Schaffer, Sami Haddadin, and Gerd Hirzinger, +``Collision detection and safe reaction with the DLR-III lightweight manipulator arm,'' +in \textit{2006 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, +IEEE, 2006, pp.~1623--1630. + +\bibitem{Wang2022InteractionTorque} +Yuan Wang, Arvin Zahedi, Yichao Zhao, and Dapeng Zhang, +``Extracting human-exoskeleton interaction torque for cable-driven upper-limb exoskeleton equipped with torque sensors,'' +\textit{IEEE/ASME Transactions on Mechatronics}, vol.~27, no.~6, pp.~4269--4280, 2022. + +\bibitem{Hart1988TLX} +S.~G.~Hart and L.~E.~Staveland, +``Development of NASA-TLX (Task Load Index): Results of empirical and +theoretical research,'' +in \emph{Human Mental Workload}, P.~A.~Hancock and N.~Meshkati, Eds. +Amsterdam, The Netherlands: North-Holland, 1988, pp.~139--183. + +\end{thebibliography} + + +% that's all folks +\end{document} diff --git a/paper/exoskeleton/assets/explain.jpg b/paper/exoskeleton/assets/explain.jpg new file mode 100644 index 0000000..21d788c Binary files /dev/null and b/paper/exoskeleton/assets/explain.jpg differ diff --git a/paper/exoskeleton/assets/joint_distribution.jpg b/paper/exoskeleton/assets/joint_distribution.jpg new file mode 100644 index 0000000..56b23ac Binary files /dev/null and b/paper/exoskeleton/assets/joint_distribution.jpg differ diff --git a/paper/exoskeleton/assets/overview.jpg b/paper/exoskeleton/assets/overview.jpg new file mode 100644 index 0000000..b583965 Binary files /dev/null and b/paper/exoskeleton/assets/overview.jpg differ diff --git a/paper/exoskeleton/assets/sew_geometry.png b/paper/exoskeleton/assets/sew_geometry.png new file mode 100644 index 0000000..1fc0375 Binary files /dev/null and b/paper/exoskeleton/assets/sew_geometry.png differ diff --git a/paper/exoskeleton/assets/shoulder.jpg b/paper/exoskeleton/assets/shoulder.jpg new file mode 100644 index 0000000..9dc891f Binary files /dev/null and b/paper/exoskeleton/assets/shoulder.jpg differ diff --git a/paper/exoskeleton/assets/swivel_angle.png b/paper/exoskeleton/assets/swivel_angle.png new file mode 100644 index 0000000..279c330 Binary files /dev/null and b/paper/exoskeleton/assets/swivel_angle.png differ diff --git a/paper/exoskeleton/assets/two_sphere_intersection.png b/paper/exoskeleton/assets/two_sphere_intersection.png new file mode 100644 index 0000000..a66d139 Binary files /dev/null and b/paper/exoskeleton/assets/two_sphere_intersection.png differ diff --git a/paper/exoskeleton/assets/wrist.jpg b/paper/exoskeleton/assets/wrist.jpg new file mode 100644 index 0000000..7ad0629 Binary files /dev/null and b/paper/exoskeleton/assets/wrist.jpg differ diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..1e883e3 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,39 @@ +[build-system] +requires = ["setuptools>=75", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "exoskeleton-teleoperation-evidence" +version = "0.1.0" +description = "Pre-prototype evidence pipeline for heterogeneous 7-DoF bilateral teleoperation" +requires-python = ">=3.10,<3.11" +dependencies = [ + "cmeel-tinyxml2==10.0.0", + "cmeel-urdfdom==4.0.1", + "matplotlib==3.10.7", + "mujoco==3.5.0", + "numpy==2.2.6", + "omegaconf==2.3.0", + "pin==3.8.0", + "PyYAML==6.0.3", + "scipy==1.15.3", +] + +[project.scripts] +exo-experiment = "experiments.cli:main" +exo-paper-artifacts = "analysis.make_paper_artifacts:main" + +[tool.setuptools] +package-dir = {"" = "code"} + +[tool.setuptools.packages.find] +where = ["code"] +include = ["analysis*", "core*", "experiments*", "utils*"] + +[tool.ruff] +line-length = 88 +target-version = "py310" + +[tool.ruff.lint] +select = ["E", "F", "I", "W"] +ignore = ["E501"] diff --git a/uv.lock b/uv.lock new file mode 100644 index 0000000..88ac6a2 --- /dev/null +++ b/uv.lock @@ -0,0 +1,657 @@ +version = 1 +revision = 3 +requires-python = "==3.10.*" + +[[package]] +name = "absl-py" +version = "2.5.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/d0/4f/d79676ab82f2e42fc3611618139f13a9c4c31d0cff4b486982047679a802/absl_py-2.5.0.tar.gz", hash = "sha256:0c996f25c0490700fadabe6351630f6111534fa0ae252cc6d2014ea3b141135f", size = 118119, upload-time = "2026-07-03T10:57:48.157Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/58/0a/a10b45aab35b175aded078a462dc8d0c698f5b13946e7cb0869097b78bb6/absl_py-2.5.0-py3-none-any.whl", hash = "sha256:0f17b89f2a4eaaedc4f28c622998aa690564b3012a396a4ffad0821007fe03ba", size = 137410, upload-time = "2026-07-03T10:57:46.735Z" }, +] + +[[package]] +name = "antlr4-python3-runtime" +version = "4.9.3" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/3e/38/7859ff46355f76f8d19459005ca000b6e7012f2f1ca597746cbcd1fbfe5e/antlr4-python3-runtime-4.9.3.tar.gz", hash = "sha256:f224469b4168294902bb1efa80a8bf7855f24c99aef99cbefc1bcd3cce77881b", size = 117034, upload-time = "2021-11-06T17:52:23.524Z" } + +[[package]] +name = "cmeel" +version = "0.60.1" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "tomli" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/54/46/ddc7df697e49cae32b1a97b3e1a3b47b815238f9059312f987bc62a2e756/cmeel-0.60.1.tar.gz", hash = "sha256:3e0b92eb933a3693ad3f1da8aae31defcbee5f25969daaf20e59c57d6a9474cf", size = 14972, upload-time = "2026-05-11T17:13:57.216Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/da/39/f2db2ff475d42222c70fe25c737028aeaafdd9c0aeba04e6b2dc66e7f93f/cmeel-0.60.1-py3-none-any.whl", hash = "sha256:3f92b68353a58b4d6b5a664ea96bf58a3fef0891a8ed570d3c153361bbbb94b7", size = 20612, upload-time = "2026-05-11T17:13:55.812Z" }, +] + +[[package]] +name = "cmeel-assimp" +version = "6.0.5" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, + { name = "cmeel-zlib" }, +] +wheels = [ + { url = "https://files.pythonhosted.org/packages/e6/57/444bedd35567a8d6b61850b03aadf6b0d2d20737ee79c620c0e40f9a56dc/cmeel_assimp-6.0.5-0-py3-none-macosx_10_9_x86_64.whl", hash = "sha256:840ab4bd398abbec5a612a731bb6f7fdc4e21b2708ae0dc0f99d7ff6d9992624", size = 10057855, upload-time = "2026-05-20T16:36:19.775Z" }, + { url = "https://files.pythonhosted.org/packages/6f/96/d4d128e074f59b79e90c78b8379127bd8953c72d1ad8a65229c23a09814f/cmeel_assimp-6.0.5-0-py3-none-macosx_11_0_arm64.whl", hash = "sha256:6d81f913e9efc8526a335900325906b85dc57891eeb7ece463714a107f14f63f", size = 9216472, upload-time = "2026-05-20T16:36:22.369Z" }, + { url = "https://files.pythonhosted.org/packages/77/37/11de8f263a5cde3cde4796e344ff034674bdbb7a2381f6b5303d49b24def/cmeel_assimp-6.0.5-0-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:db6fe58f8bd633cb05ab9f390c576a4e774927ed8c343083fad4f939e834bf6e", size = 13709230, upload-time = "2026-05-20T16:36:25.497Z" }, + { url = "https://files.pythonhosted.org/packages/62/66/7f12b2f003671828a46ab35964aed0a34632fcba5638b4049bdd918cf342/cmeel_assimp-6.0.5-0-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:7b5ee36f3027de9b1bd73bd4294b60fed1bbc6c1524f94f64f69f38c5a49bf03", size = 14840275, upload-time = "2026-05-20T16:36:28.311Z" }, +] + +[[package]] +name = "cmeel-boost" +version = "1.89.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, + { name = "numpy" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/94/7c/4d9bbc00d4f9286a48d38ffdcf030fe50c99fe00d3601303270740f22424/cmeel_boost-1.89.0.tar.gz", hash = "sha256:e28d4aa61f4b8dbcb6cb83e732e1076fe4f5a3a0d338d73d1c0821944b37a332", size = 4158, upload-time = "2025-08-22T22:29:36.37Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/75/6b/36e51770bfa546bb182eacc0a9c88cfb9817aa2914305cfd8d31ff7d5ae5/cmeel_boost-1.89.0-0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:509729c9a3549753df5b219773fef76b1be90e052089e40a6193d1fea4861f80", size = 30666822, upload-time = "2025-08-22T22:28:19.58Z" }, + { url = "https://files.pythonhosted.org/packages/40/35/89b58a680189f511d7543a89a7e647943d0058e81639badde448b8deaa23/cmeel_boost-1.89.0-0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:ad20fdf62d41eb74c3cba4c0f8d32d49f40bc2bf0b1fb508d97f63f911f7cece", size = 30565392, upload-time = "2025-08-22T22:28:23.378Z" }, + { url = "https://files.pythonhosted.org/packages/45/10/f59c72182391176fc18f76cab503df57bf10234d5b13856dd1f44237679c/cmeel_boost-1.89.0-0-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:82aaaed1bb6703664f87bc1ec666703ef917ff67baa61491c8796e13853bbd91", size = 35676696, upload-time = "2025-08-22T22:28:27.258Z" }, + { url = "https://files.pythonhosted.org/packages/79/85/7f61c694bf55a239a3821c65767e5d104adaf0faa890c9c63d8ed4dc44b1/cmeel_boost-1.89.0-0-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:78fd13be11d7c570e67400f5e734cf00787fe268cee7fd6bdde466ffd13e12be", size = 36013432, upload-time = "2025-08-22T22:28:32.225Z" }, + { url = "https://files.pythonhosted.org/packages/82/39/4ff562a699082dfc93a521418655653fe6223a61480cc58aab4333093864/cmeel_boost-1.89.0-1-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:7b7f0c174c7a9216cf0221b36bb0bac81e62f9cc1c25a6082474b1dedee9c62e", size = 30658750, upload-time = "2025-10-15T23:59:22.997Z" }, + { url = "https://files.pythonhosted.org/packages/7e/e6/e468de28bcb1140d8a61d45f6eb88c8cd79b1fa3ccdd538f58204dbae681/cmeel_boost-1.89.0-1-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:e76274a984c3f69bd57bbb8923e8acd68b1e65fe7d73df69393e2a5bca601094", size = 30551561, upload-time = "2025-10-15T23:59:26.451Z" }, + { url = "https://files.pythonhosted.org/packages/a8/d8/129cd530587cbcf032120f80176d67f06592e9dc62dddd7dde5f8ce80edc/cmeel_boost-1.89.0-1-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:849308a26b07c268f4b2b68a2a8730ffa82e3dc8569bb016088c542c89117580", size = 35676698, upload-time = "2025-10-15T23:59:29.852Z" }, + { url = "https://files.pythonhosted.org/packages/74/96/3b6a784577ff6e69a32b65c970d27ad1a8705645c7cdef181fa475fafd0d/cmeel_boost-1.89.0-1-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:2d35dd5a8bc06f75d3c0c1d2db38e700693bf87c7f40a7cbd52af29a51df64c0", size = 36013428, upload-time = "2025-10-15T23:59:33.402Z" }, +] + +[[package]] +name = "cmeel-console-bridge" +version = "1.0.2.3" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/83/13/2e9e9d23db8548aef975564055bdb4fb6da8a397a1e7df8cb61f5afebefb/cmeel_console_bridge-1.0.2.3.tar.gz", hash = "sha256:3b2837da7ab408e9d1a775c83c0a7772356062b3a3672e4ce247f2da71a8ecd9", size = 262061, upload-time = "2025-03-19T18:22:06.845Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/d4/a7/527fa060e5881acb3b0a07bf1d803ccb831cb87739abb62b6bcd14f5aed3/cmeel_console_bridge-1.0.2.3-0-py3-none-macosx_10_9_x86_64.whl", hash = "sha256:7aa19b2d006073a1fad55d32968c7d0c7136749e06f98405f4f73a71038a5c41", size = 21341, upload-time = "2025-03-19T18:21:56.834Z" }, + { url = "https://files.pythonhosted.org/packages/bb/db/f8643a8766e8909e0dbfcda6191ca92454cf9a3fadd89be417db261601a1/cmeel_console_bridge-1.0.2.3-0-py3-none-macosx_11_0_arm64.whl", hash = "sha256:c47d8c97cb120feed1c01f30845d16c67e4e8205941e3977951018972b9b8721", size = 21286, upload-time = "2025-03-19T18:21:57.984Z" }, + { url = "https://files.pythonhosted.org/packages/a4/b4/9c79177152a220ab2e4ffa0140722165035f6a5c2abbed2912352bd7e7b9/cmeel_console_bridge-1.0.2.3-0-py3-none-manylinux_2_17_i686.whl", hash = "sha256:cad9723ac44ab563cd23bf361b604733623d11847c4edf2a2b4ebd1d984ade09", size = 23740, upload-time = "2025-03-19T18:21:59.683Z" }, + { url = "https://files.pythonhosted.org/packages/92/65/5741de6f550fe701d0780546d97b283306676315a3e1f379a6038e8c0ab0/cmeel_console_bridge-1.0.2.3-0-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:372942e9c44f681bfff377fba25b348801283aa6f3826a00e4195089bda9737a", size = 25762, upload-time = "2025-03-19T18:22:01.055Z" }, + { url = "https://files.pythonhosted.org/packages/50/a5/70e23c5570506bb39b56aa4d0f3a4a414e38082ddb33e86a48b546620121/cmeel_console_bridge-1.0.2.3-0-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:5bb1115ed38441b2396e732e10ec63d1e68445674f9f5d321f7985eb10e9aeef", size = 24477, upload-time = "2025-03-19T18:22:02.091Z" }, + { url = "https://files.pythonhosted.org/packages/47/36/bfd5a255348902e39243ccc6eba693bce714b891cd3be5603a9bd50c6de5/cmeel_console_bridge-1.0.2.3-0-py3-none-musllinux_1_2_aarch64.whl", hash = "sha256:2b8d084b797f592942208c2040b08e06b82f8832aa6c5e582ba6f1a4a653505b", size = 24970, upload-time = "2025-03-19T18:22:03.075Z" }, + { url = "https://files.pythonhosted.org/packages/b3/02/3ae074e9acb9e150a4d5d97f341c2064573cd5fe9e5af20ab58bf8c0020a/cmeel_console_bridge-1.0.2.3-0-py3-none-musllinux_1_2_i686.whl", hash = "sha256:fb6753a9864217d969c4965389d66a476ac978136c03eadf1063b1619c359220", size = 24689, upload-time = "2025-03-19T18:22:04.084Z" }, + { url = "https://files.pythonhosted.org/packages/69/d0/321f74b7d4167a6c59bb7714a6899ba402d9fad611f62573b9d646107320/cmeel_console_bridge-1.0.2.3-0-py3-none-musllinux_1_2_x86_64.whl", hash = "sha256:9d446c0fc541413d8d2ceea3c1cfb9cbfd57938d6659c113121eca6c245caafe", size = 24404, upload-time = "2025-03-19T18:22:05.232Z" }, +] + +[[package]] +name = "cmeel-octomap" +version = "1.10.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/89/ab/2fed2dbee13e4b39949591685419f1dbb691295e32a6bbbaf87edc005922/cmeel_octomap-1.10.0.tar.gz", hash = "sha256:bd79d1d17adede534de242e42e13ef0d9f04bdd27daf7d56c57f7c43670c9b05", size = 1694189, upload-time = "2025-01-06T17:57:05.477Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/89/22/ea67d35df31ec4bb2ed6e594b173c572c72dbd2a87e96906eac67b4af930/cmeel_octomap-1.10.0-4-py3-none-macosx_14_0_arm64.whl", hash = "sha256:c116eb151920d26ee2b2c1f656cd7526862006739817205f11f9366ab0ef6cb4", size = 639956, upload-time = "2025-01-06T17:56:56.826Z" }, + { url = "https://files.pythonhosted.org/packages/b2/da/07725a8c11224881f536ad252e97a3d9801b48e5e776017d5f00fb39b17f/cmeel_octomap-1.10.0-4-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:76cc42553f54bae97584aaf0c7bc33753ff287e2738aa2ecac4820121101dd46", size = 1044402, upload-time = "2025-01-06T17:56:58.553Z" }, + { url = "https://files.pythonhosted.org/packages/a2/15/9617b7039afd6d17d3148f6f970d953f5e265d7736f8fdbca09c86e976a0/cmeel_octomap-1.10.0-4-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:5fdb04546fff3accac5f8626c3fc15c3b99e94ab887793565e0b92cedaf96468", size = 1105037, upload-time = "2025-01-06T17:57:01.287Z" }, + { url = "https://files.pythonhosted.org/packages/51/69/88c1d1eca1abf2387ee8263ac7e12708c8b1b5b70b46a0bd9f43b485165b/cmeel_octomap-1.10.0-4-py3-none-musllinux_1_1_x86_64.whl", hash = "sha256:84a7376cfced954bb7e3e347afbd02bdc1c83066b995afbdd0fb1e2d9f57ebec", size = 1108359, upload-time = "2025-01-06T17:57:04.085Z" }, + { url = "https://files.pythonhosted.org/packages/f5/59/57b3b38cf7a382855902b9d24266c283c29d977706438e6b7af62df74e2b/cmeel_octomap-1.10.0-5-py3-none-macosx_10_9_x86_64.whl", hash = "sha256:042b4a21b5e5e19ee78a9a7db78e1b06fb8a287c832031788aec0d3fcabbfecd", size = 748832, upload-time = "2025-02-12T11:57:34.252Z" }, + { url = "https://files.pythonhosted.org/packages/55/8b/f5ec7676808a48c0185e216c0da700e34cb13ba233f13a4557a5ec56324a/cmeel_octomap-1.10.0-5-py3-none-macosx_11_0_arm64.whl", hash = "sha256:79c15a0ece5ca3746170088ef2a377dfb3df8326fafde9bdba688852219758b9", size = 706924, upload-time = "2025-02-12T11:57:35.846Z" }, + { url = "https://files.pythonhosted.org/packages/09/50/56de5a4d9f8ca58100146f16f42c4e2fbb49c0957bfe40d3fd2bc910afe4/cmeel_octomap-1.10.0-5-py3-none-manylinux_2_17_i686.whl", hash = "sha256:e2923bf593ebdafed86b6f3890a122c62fbd9cc9f325d60dbecb72b6b60d78fb", size = 1073973, upload-time = "2025-02-12T11:57:37.914Z" }, + { url = "https://files.pythonhosted.org/packages/d3/f5/8dddf5cdd31176288acd85cc8bf0262b7c3de81d5cb2cb33aa6646f44eb1/cmeel_octomap-1.10.0-5-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:d9e6f9c826905e8de632e9df8cc20e59ce2eb5d1e0b368d8d4abbbc5c0829c1a", size = 1044533, upload-time = "2025-02-12T11:57:39.672Z" }, + { url = "https://files.pythonhosted.org/packages/d6/14/b85bd33bb05c9bb7e87b9ac8401793c12a80a6d594b3ca4bcb5e971a24b7/cmeel_octomap-1.10.0-5-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:b0b54fac180dce4f483afe7029c29cc55f6f2b21be8413e8e2275845b0c204d7", size = 1105199, upload-time = "2025-02-12T11:57:41.286Z" }, + { url = "https://files.pythonhosted.org/packages/99/ee/fe3360441159974ebdbb4c013a92ad0425d5f8bf414868d5161060e40660/cmeel_octomap-1.10.0-5-py3-none-musllinux_1_2_aarch64.whl", hash = "sha256:c8691e665bab7c12b6f51e6c5fbbb83ee6f91dce9d15d9d0387553950e7fb5ee", size = 1092962, upload-time = "2025-02-12T11:57:43.297Z" }, + { url = "https://files.pythonhosted.org/packages/82/a6/074166544cc0ce3a5d7844f97dfd13d1b3ec7bff6a6e2cfb18d66a671a7f/cmeel_octomap-1.10.0-5-py3-none-musllinux_1_2_i686.whl", hash = "sha256:735c0ad84dacbbcc8c4237f127c57244c236b7d6c7500b5c45a4c225e19daac1", size = 1083321, upload-time = "2025-02-12T11:57:46.121Z" }, + { url = "https://files.pythonhosted.org/packages/c7/a3/b19ea0d30837369091141b248936b0757ee17f58b809007399bad0b398e4/cmeel_octomap-1.10.0-5-py3-none-musllinux_1_2_x86_64.whl", hash = "sha256:5f86a83f6bd60de290cd327f0374d525328369e76591e3ab2ad1bc0b183678c4", size = 1109207, upload-time = "2025-02-12T11:57:48.709Z" }, +] + +[[package]] +name = "cmeel-qhull" +version = "8.0.2.1" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/51/dd/8d0bcfb18771b2ea02bf85dfbbc587c97b274496fb5419b72134eb69430b/cmeel_qhull-8.0.2.1.tar.gz", hash = "sha256:68e8d41d95f61830f2d460af1e4d760f0dbe4d46413d7c736f0ed701153ebe52", size = 1308055, upload-time = "2023-11-17T14:21:06.003Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/77/b4/d72ebd5e9ee711b68ad466e7bd4c0edcb45b0c2c8a358fdcdb64b092666a/cmeel_qhull-8.0.2.1-0-py3-none-macosx_12_0_arm64.whl", hash = "sha256:39f5183a6e026754c3c043239bac005bf1825240d72e1d8fdf090a0f3ea27307", size = 2804225, upload-time = "2023-11-17T14:15:39.958Z" }, + { url = "https://files.pythonhosted.org/packages/29/dc/4bfb8d51a09401cf740e66d10bdb388eacd7c73bae12ef78149cbbc93e83/cmeel_qhull-8.0.2.1-0-py3-none-macosx_12_0_x86_64.whl", hash = "sha256:f135c5a4f4c8ed53f061bc86b794aaca2c0c34761c9269c06b71329c9da56f82", size = 2972481, upload-time = "2023-11-17T14:20:58.418Z" }, + { url = "https://files.pythonhosted.org/packages/0a/7c/74b5c781cbfc8e4a9bb73b71659cc595bc0163223fd700b18133dbcf2831/cmeel_qhull-8.0.2.1-0-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:17f519106df79aed9fc5ec92833d4958d132d23021f02a78a9564cdf83a36c7c", size = 3078962, upload-time = "2023-11-17T14:21:00.183Z" }, + { url = "https://files.pythonhosted.org/packages/b4/16/ef7b6201835ba2492753c9c91b266d047b6664507be42ec858e2b24673b5/cmeel_qhull-8.0.2.1-0-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:c513abafa40e2b8eb7cd3640e3f92d5391fbd6ec0f4182dbf9536934d8a8ea3e", size = 3194917, upload-time = "2023-11-17T14:21:01.879Z" }, + { url = "https://files.pythonhosted.org/packages/4b/ae/200bdf257507e2c95d0656bf02278cd666d49f0a9e2e6d281ea76d7d085c/cmeel_qhull-8.0.2.1-0-py3-none-musllinux_1_1_x86_64.whl", hash = "sha256:20a69cb34b6250aee1f018412989734c9ddcad6ff66717a7c5516fc19f55d5ff", size = 3290068, upload-time = "2023-11-17T14:21:03.828Z" }, + { url = "https://files.pythonhosted.org/packages/01/1b/de3fa6091ef58ab40f02653e777c8943acf7cec486184d6007885123571d/cmeel_qhull-8.0.2.1-1-py3-none-macosx_10_9_x86_64.whl", hash = "sha256:b5d47b113c1cb8f519bc813cf015d0d01f8ce5b08912733a24a6018f7caa6e96", size = 2902499, upload-time = "2025-02-12T11:51:16.999Z" }, + { url = "https://files.pythonhosted.org/packages/05/0c/5e5d9a033c683eb272508ccf560c03ac6bf5d397b038fe05f896a2283eaf/cmeel_qhull-8.0.2.1-1-py3-none-macosx_11_0_arm64.whl", hash = "sha256:33a0169f4ee37d093c450195b0ef73d4fe0d9d62abb7899ebe79f778b36e1f36", size = 2773563, upload-time = "2025-02-12T11:51:19.893Z" }, + { url = "https://files.pythonhosted.org/packages/52/9b/00c73069348e60fbbdf6a5a10de046083f7d1ad36844958bbf12163ac688/cmeel_qhull-8.0.2.1-1-py3-none-manylinux_2_17_i686.whl", hash = "sha256:a577e76ac94d128f2966b137ead9f088749513df63749728e2b588f4564b7fdf", size = 3228684, upload-time = "2025-02-12T11:51:21.888Z" }, + { url = "https://files.pythonhosted.org/packages/c0/4a/81b8c88b444935a64d8c83b41e662f696c36dd5937c3ca687113ac4778d0/cmeel_qhull-8.0.2.1-1-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:fd0b2d4ce749b102c3cdead4588249befd34f1a660628f6bfc090ce942925aac", size = 3156051, upload-time = "2025-02-12T11:51:24.594Z" }, + { url = "https://files.pythonhosted.org/packages/4f/c1/44874cd8bfc1e3f7cb15678c836c7a1d5537f34f5a727a0207e01f395598/cmeel_qhull-8.0.2.1-1-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:2371a7c80a14f3e874876359ae3e3094861f081fcdd7a03987c3e880d14e07b9", size = 3262508, upload-time = "2025-02-12T11:51:27.147Z" }, + { url = "https://files.pythonhosted.org/packages/54/0e/425d9ce1f2a831025d39fa5b6479b856bd4d73614c9caa690ac72bbfca04/cmeel_qhull-8.0.2.1-1-py3-none-musllinux_1_2_aarch64.whl", hash = "sha256:197c14c2006dbeba8f5a5771700a7afea72c1a441aab7cdeaaf10b4ed8c1137d", size = 3172646, upload-time = "2025-02-12T11:51:28.967Z" }, + { url = "https://files.pythonhosted.org/packages/00/c1/e973e287a7d793911b8e6497b17586e601a678f2379ba2c615f72bd76480/cmeel_qhull-8.0.2.1-1-py3-none-musllinux_1_2_i686.whl", hash = "sha256:886d1be24b31842286ae42755af5c312a43a4199632826e4110185ec36dc5c6a", size = 3530837, upload-time = "2025-02-12T11:51:31.651Z" }, + { url = "https://files.pythonhosted.org/packages/fd/65/c6cd54f04b5fcaa4ec52f5b57692c1dcef812ff9ee86545e5607369d365e/cmeel_qhull-8.0.2.1-1-py3-none-musllinux_1_2_x86_64.whl", hash = "sha256:1a49ce7f8492c9a8b49f930e34cce75b5e9b9843b015033dd0a25421441159fc", size = 3301908, upload-time = "2025-02-12T11:51:34.53Z" }, +] + +[[package]] +name = "cmeel-tinyxml2" +version = "10.0.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/28/9f/030eca702c485f7a641f975f167fa93164911b3329f005fb0730ff5e793f/cmeel_tinyxml2-10.0.0.tar.gz", hash = "sha256:00252aefc1c94a55b89f25ad08ee79fda2da8d1d94703e051598ddb52a9088fe", size = 645297, upload-time = "2025-02-06T10:29:00.106Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/fe/5d/bc3a932eb7996a0a789979426a9bb8a3948bf57f3f17bab87dddbef62433/cmeel_tinyxml2-10.0.0-0-py3-none-macosx_10_9_x86_64.whl", hash = "sha256:924499bb1b60b9a17bd001d12a9af88ddbee4ca888638ae684ba7f0f3ce49e87", size = 111913, upload-time = "2025-02-06T10:28:45.723Z" }, + { url = "https://files.pythonhosted.org/packages/92/bf/67d11e123313c034712896e94038291fe506bb099bdb75a136392002ffd0/cmeel_tinyxml2-10.0.0-0-py3-none-macosx_11_0_arm64.whl", hash = "sha256:26a1eb30c2a00bfc172e89ed015a18b8efb2b383546252ca8859574aed684686", size = 109487, upload-time = "2025-02-06T10:28:47.546Z" }, + { url = "https://files.pythonhosted.org/packages/ca/48/d8c81ce19b4b278ed0e8f81f93ae8670209bf3a9ac20141b9c386bb40cc7/cmeel_tinyxml2-10.0.0-0-py3-none-manylinux_2_17_i686.whl", hash = "sha256:53d86e02864c712f51f9a9adfcd8b6046b2ed51d44a0c34a8438d93b72b48325", size = 160118, upload-time = "2025-02-06T10:28:49.627Z" }, + { url = "https://files.pythonhosted.org/packages/87/4e/62193e27c9581f8ba7aeaeca7805632a64f2f4a824b1db37ad02ee953e8a/cmeel_tinyxml2-10.0.0-0-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:74112e2e9473afbf6ee2d25c9942553e9f6a40465e714533db72db48bc7658e1", size = 158477, upload-time = "2025-02-06T10:28:51.667Z" }, + { url = "https://files.pythonhosted.org/packages/14/f9/d0420c39e9ade99beeec61cd3abc68880fe6e14d85e9df292af8fabe65c8/cmeel_tinyxml2-10.0.0-0-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:ecd6e99caa2a06ac0d4b333b740c20fca526d0ca426f99eb5c0a0039117afdb6", size = 147025, upload-time = "2025-02-06T10:28:53.944Z" }, + { url = "https://files.pythonhosted.org/packages/66/9e/df63147fc162ab487217fa5596778ab7a81a82d9b3ce4236fd3a1e48cecb/cmeel_tinyxml2-10.0.0-0-py3-none-musllinux_1_2_aarch64.whl", hash = "sha256:30993fffb7032a45d5d3b1e5670cb879dad667a13144cd68c8f4e0371a8a3d2e", size = 150958, upload-time = "2025-02-06T10:28:55.301Z" }, + { url = "https://files.pythonhosted.org/packages/0e/a8/b03567275fd83f5af33ddb61de942689dec72c5b21bec01e6a5b11101aa5/cmeel_tinyxml2-10.0.0-0-py3-none-musllinux_1_2_i686.whl", hash = "sha256:8c09ede51784af54211a6225884dc7ddbb02ea1681656d173060c7ad2a5b9a3c", size = 160300, upload-time = "2025-02-06T10:28:57.189Z" }, + { url = "https://files.pythonhosted.org/packages/3a/ec/2781635b66c1059ca1243ae0f5a0410e171a5d8b8a71be3e34cb172f9f2d/cmeel_tinyxml2-10.0.0-0-py3-none-musllinux_1_2_x86_64.whl", hash = "sha256:3bd511d6d0758224efdebc23d3ead6e94f0755b04141ebf7d5493377829e8332", size = 149184, upload-time = "2025-02-06T10:28:58.734Z" }, +] + +[[package]] +name = "cmeel-urdfdom" +version = "4.0.1" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, + { name = "cmeel-console-bridge" }, + { name = "cmeel-tinyxml2" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/31/09/be81a5e7db56f34b6ccdbe7afe855c95a18c8439e173519e0146e9276a8c/cmeel_urdfdom-4.0.1.tar.gz", hash = "sha256:2e3f41e8483889e195b574acb326a4464cf11a3c0a8724031ac28bcda2223efc", size = 291511, upload-time = "2025-02-12T12:07:09.699Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/be/d0/20147dd6bb723afc44a58d89ea624df2bad1bed7b898a2df112aaca4a479/cmeel_urdfdom-4.0.1-0-py3-none-macosx_10_9_x86_64.whl", hash = "sha256:2fe56939c6b47f6ec57021aac154123da47ecdcd79a217f3a5e3c4b705a07dee", size = 300860, upload-time = "2025-02-12T12:06:58.536Z" }, + { url = "https://files.pythonhosted.org/packages/8e/98/f832bca347e2d987c6b0ebb6930caf7b2c402535324aeed466b6aa2c4513/cmeel_urdfdom-4.0.1-0-py3-none-macosx_11_0_arm64.whl", hash = "sha256:00a0aba78b68c428b27abeed1db58d73e65319ed966911a0e97b37367442e756", size = 300616, upload-time = "2025-02-12T12:07:00.556Z" }, + { url = "https://files.pythonhosted.org/packages/cf/10/bf5765b6f388037cff166a754a0958ac2fee34ca3c0975ef64d0324e4647/cmeel_urdfdom-4.0.1-0-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:a701a8f9671331f11b18ecf37a6537db546a21e6a0e5d0ff53341fea0693ed7f", size = 385951, upload-time = "2025-02-12T12:07:02.556Z" }, + { url = "https://files.pythonhosted.org/packages/c3/82/cb3f8f587d293a17bdbea15b50cdaa4a1e28e04583eb4cb4821685b89466/cmeel_urdfdom-4.0.1-0-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:12e39fc388c077d79fc9b3841d3d972a1da90b90de754d3363194c1540e18abf", size = 399619, upload-time = "2025-02-12T12:07:04.388Z" }, + { url = "https://files.pythonhosted.org/packages/24/77/322d7ac92c692d8dfaeda9de2d937087d15e2b564dc457d656e5fde3991d/cmeel_urdfdom-4.0.1-0-py3-none-musllinux_1_2_aarch64.whl", hash = "sha256:c4a83925df1d5923c4485c3eb2b80b3a61b14f119ab724fb5bd04cec494690ee", size = 373969, upload-time = "2025-02-12T12:07:06.222Z" }, + { url = "https://files.pythonhosted.org/packages/9f/63/bdc6b55cc8bd99bb9dce6be801b30feffaa1c3841ecb7f4fe4d137424518/cmeel_urdfdom-4.0.1-0-py3-none-musllinux_1_2_x86_64.whl", hash = "sha256:4c4f44270971b3d05c45a4e21b1fb2df7e05a750363ae918f59532bff0bfe0e1", size = 388237, upload-time = "2025-02-12T12:07:08.326Z" }, + { url = "https://files.pythonhosted.org/packages/1d/2d/8463fc23230612daf4da1e31d3229f47708381f3ae4d1500f0f007ac0f92/cmeel_urdfdom-4.0.1-1-py3-none-macosx_10_9_x86_64.whl", hash = "sha256:f7535158f45992eb2ba79e90d9db1bf9adc3846d9c7ed3e7a8c1c4d5343afa37", size = 301006, upload-time = "2025-02-13T11:42:08.8Z" }, + { url = "https://files.pythonhosted.org/packages/0f/d5/c8cdf500e49300d85624cbc3ef804107ddcdc9c541b1d3f726bfb58a9fc1/cmeel_urdfdom-4.0.1-1-py3-none-macosx_11_0_arm64.whl", hash = "sha256:fef2a01a00d61d41b3d35dd4958bba973e9025c26eea1d3c9880932f4dba89a5", size = 300758, upload-time = "2025-02-13T11:42:10.449Z" }, + { url = "https://files.pythonhosted.org/packages/cf/b3/2f7bac1544113a7f8e0f6d8b1fab5e75c6a3d27ffbb584b03267251b2165/cmeel_urdfdom-4.0.1-1-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:7a52eb36950ce982014d99a55717ca29985da056e3705f20746f15d3244c1f7a", size = 386043, upload-time = "2025-02-13T11:42:11.923Z" }, + { url = "https://files.pythonhosted.org/packages/86/03/8bdeb36ba6a3e8125d523ecfc010403049e463fe589f9896858d4bdcaf1e/cmeel_urdfdom-4.0.1-1-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:9f3b9c80b10d7246821ff61c2573f799e3da23d483e6f7367ddcad8a48baf58f", size = 399719, upload-time = "2025-02-13T11:42:14.325Z" }, + { url = "https://files.pythonhosted.org/packages/3f/ed/43f99e7512460294cd8acc5753ba25f8a20bdf28d62e143eaf3ec7a28bb6/cmeel_urdfdom-4.0.1-1-py3-none-musllinux_1_2_aarch64.whl", hash = "sha256:2de69f47e8312cc09157624802d5bdaad6406443f863fb4b9ec62a19b4de3c72", size = 374073, upload-time = "2025-02-13T11:42:17.907Z" }, + { url = "https://files.pythonhosted.org/packages/17/c6/2e9bde6d7c02c1cf203ea896f8ce1afd441412f09b44830f1ee4a96d77de/cmeel_urdfdom-4.0.1-1-py3-none-musllinux_1_2_x86_64.whl", hash = "sha256:7708c1402de450fbeab21f7ca264a9a4676ed4c1cdf8d84d840bc5d057aac920", size = 388337, upload-time = "2025-02-13T11:42:19.657Z" }, +] + +[[package]] +name = "cmeel-zlib" +version = "1.3.2" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/ef/85/9c6b77d5c49b363b17ba974271d58730bd26cbe00b1c576596339bb624ea/cmeel_zlib-1.3.2.tar.gz", hash = "sha256:6e31f2956c334de9a7e19a4e4f8eed030ed8c8016c841ea57226ce8bed443712", size = 3200, upload-time = "2026-05-20T16:24:37.87Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/46/e7/c60533fb6f638fe92d56b79edbcff70ed74a7f19b3ace981532efab5c3dd/cmeel_zlib-1.3.2-0-py3-none-macosx_10_9_x86_64.whl", hash = "sha256:88a89882340391aa263ed1f7448fe0177e6fb9b93740cad381228437879f16a9", size = 1035696, upload-time = "2026-05-20T16:24:28.538Z" }, + { url = "https://files.pythonhosted.org/packages/85/f6/dfcc8b9c2989e7342aaac1781d147b06b31197337d1968029216be26ee43/cmeel_zlib-1.3.2-0-py3-none-macosx_11_0_arm64.whl", hash = "sha256:94a2f1adcc811617eab849b5771e608fa20e2fc44fb8cbd2e56a3e4d496461c1", size = 957505, upload-time = "2026-05-20T16:24:30.151Z" }, + { url = "https://files.pythonhosted.org/packages/3c/e9/6a3d6732e23fcf7072973f9ca8251eaef3e6738e46d6846064dd92e13331/cmeel_zlib-1.3.2-0-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:3b07fb7e28aaa917e6accb59a29dd0f5c1d272d42988f32ba2c232f5f455be4c", size = 1055975, upload-time = "2026-05-20T16:24:31.561Z" }, + { url = "https://files.pythonhosted.org/packages/70/a4/d345605b6f8c9d665baa1dd2f839ad6aee0d771e174be12c81fdd53067ed/cmeel_zlib-1.3.2-0-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:4658000c5531273d14ca8f0250abcd2a2ad85b336f10a273a77ecb38611a5f5a", size = 1052274, upload-time = "2026-05-20T16:24:33.078Z" }, + { url = "https://files.pythonhosted.org/packages/9b/9d/83737c38cc106e1b4959393da7744d5180b9ab5eac0369fc6ba0ea6c6428/cmeel_zlib-1.3.2-0-py3-none-musllinux_1_2_aarch64.whl", hash = "sha256:06cbf60a361ca18e8a4c46a238149cebcaf955047a60830705e130aa397b5116", size = 1057248, upload-time = "2026-05-20T16:24:34.822Z" }, + { url = "https://files.pythonhosted.org/packages/cb/28/b378182b0f33ed9e9c091b3d1e8d86a6f2d69add107087b2e09adcb12137/cmeel_zlib-1.3.2-0-py3-none-musllinux_1_2_x86_64.whl", hash = "sha256:2a3425a65a0adf0ddbb48d41204a0d9c13197bd693f7821d1450e8a969d77352", size = 1055409, upload-time = "2026-05-20T16:24:36.219Z" }, +] + +[[package]] +name = "coal" +version = "3.0.2" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, + { name = "cmeel-assimp" }, + { name = "cmeel-boost" }, + { name = "cmeel-octomap" }, + { name = "cmeel-qhull" }, + { name = "eigenpy" }, + { name = "libcoal" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/72/4f/9b1f2cb921827aa877c09f6e727215fb633e4e3671682bd2a6559cd42d09/coal-3.0.2.tar.gz", hash = "sha256:7ca3f961fe72962b543894492efb33ee71bdc1091d93b87dc6988cdf0d4dedca", size = 1463955, upload-time = "2025-10-16T00:52:33.982Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/51/da/8b4758f8183d6808e542f97b5719b191ceda8f23e5958a1c3324535b9049/coal-3.0.2-1-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:eab5b68f1e25babd10a5d788bdce2ae61196c3e548c900ff8d060462e60e5194", size = 1612332, upload-time = "2025-10-16T00:51:56.269Z" }, + { url = "https://files.pythonhosted.org/packages/5e/14/21ba9435ce088452f903cc54312e04fd337d00f63f1a5cc90ceb37511dba/coal-3.0.2-1-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:bb21e74d9071f87629026c1177b3c346145630869dc136cc4704899f3dfbf9db", size = 1505686, upload-time = "2025-10-16T00:51:57.948Z" }, + { url = "https://files.pythonhosted.org/packages/51/6c/68c42fe06b1ee8c5962edb4c9cecd9e8a042ebc5f850510d76dcb5beea0b/coal-3.0.2-1-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:6d97c0137b22a41e03090d044824596f76ccc065407f6fc538af7aedb2995306", size = 2218478, upload-time = "2025-10-16T00:51:59.583Z" }, + { url = "https://files.pythonhosted.org/packages/b2/7e/6977e63ca97451b6888f69531d26513b64ce94235aa06ea49b24b0e2bb12/coal-3.0.2-1-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:89af2fcc4f74474487e8e42ced4a2222db81ec50d27f0f9482fec9ca6309cad4", size = 2216338, upload-time = "2025-10-16T00:52:01.211Z" }, +] + +[[package]] +name = "contourpy" +version = "1.3.2" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "numpy" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/66/54/eb9bfc647b19f2009dd5c7f5ec51c4e6ca831725f1aea7a993034f483147/contourpy-1.3.2.tar.gz", hash = "sha256:b6945942715a034c671b7fc54f9588126b0b8bf23db2696e3ca8328f3ff0ab54", size = 13466130, upload-time = "2025-04-15T17:47:53.79Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/12/a3/da4153ec8fe25d263aa48c1a4cbde7f49b59af86f0b6f7862788c60da737/contourpy-1.3.2-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:ba38e3f9f330af820c4b27ceb4b9c7feee5fe0493ea53a8720f4792667465934", size = 268551, upload-time = "2025-04-15T17:34:46.581Z" }, + { url = "https://files.pythonhosted.org/packages/2f/6c/330de89ae1087eb622bfca0177d32a7ece50c3ef07b28002de4757d9d875/contourpy-1.3.2-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:dc41ba0714aa2968d1f8674ec97504a8f7e334f48eeacebcaa6256213acb0989", size = 253399, upload-time = "2025-04-15T17:34:51.427Z" }, + { url = "https://files.pythonhosted.org/packages/c1/bd/20c6726b1b7f81a8bee5271bed5c165f0a8e1f572578a9d27e2ccb763cb2/contourpy-1.3.2-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:9be002b31c558d1ddf1b9b415b162c603405414bacd6932d031c5b5a8b757f0d", size = 312061, upload-time = "2025-04-15T17:34:55.961Z" }, + { url = "https://files.pythonhosted.org/packages/22/fc/a9665c88f8a2473f823cf1ec601de9e5375050f1958cbb356cdf06ef1ab6/contourpy-1.3.2-cp310-cp310-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:8d2e74acbcba3bfdb6d9d8384cdc4f9260cae86ed9beee8bd5f54fee49a430b9", size = 351956, upload-time = "2025-04-15T17:35:00.992Z" }, + { url = "https://files.pythonhosted.org/packages/25/eb/9f0a0238f305ad8fb7ef42481020d6e20cf15e46be99a1fcf939546a177e/contourpy-1.3.2-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:e259bced5549ac64410162adc973c5e2fb77f04df4a439d00b478e57a0e65512", size = 320872, upload-time = "2025-04-15T17:35:06.177Z" }, + { url = "https://files.pythonhosted.org/packages/32/5c/1ee32d1c7956923202f00cf8d2a14a62ed7517bdc0ee1e55301227fc273c/contourpy-1.3.2-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ad687a04bc802cbe8b9c399c07162a3c35e227e2daccf1668eb1f278cb698631", size = 325027, upload-time = "2025-04-15T17:35:11.244Z" }, + { url = "https://files.pythonhosted.org/packages/83/bf/9baed89785ba743ef329c2b07fd0611d12bfecbedbdd3eeecf929d8d3b52/contourpy-1.3.2-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:cdd22595308f53ef2f891040ab2b93d79192513ffccbd7fe19be7aa773a5e09f", size = 1306641, upload-time = "2025-04-15T17:35:26.701Z" }, + { url = "https://files.pythonhosted.org/packages/d4/cc/74e5e83d1e35de2d28bd97033426b450bc4fd96e092a1f7a63dc7369b55d/contourpy-1.3.2-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:b4f54d6a2defe9f257327b0f243612dd051cc43825587520b1bf74a31e2f6ef2", size = 1374075, upload-time = "2025-04-15T17:35:43.204Z" }, + { url = "https://files.pythonhosted.org/packages/0c/42/17f3b798fd5e033b46a16f8d9fcb39f1aba051307f5ebf441bad1ecf78f8/contourpy-1.3.2-cp310-cp310-win32.whl", hash = "sha256:f939a054192ddc596e031e50bb13b657ce318cf13d264f095ce9db7dc6ae81c0", size = 177534, upload-time = "2025-04-15T17:35:46.554Z" }, + { url = "https://files.pythonhosted.org/packages/54/ec/5162b8582f2c994721018d0c9ece9dc6ff769d298a8ac6b6a652c307e7df/contourpy-1.3.2-cp310-cp310-win_amd64.whl", hash = "sha256:c440093bbc8fc21c637c03bafcbef95ccd963bc6e0514ad887932c18ca2a759a", size = 221188, upload-time = "2025-04-15T17:35:50.064Z" }, + { url = "https://files.pythonhosted.org/packages/33/05/b26e3c6ecc05f349ee0013f0bb850a761016d89cec528a98193a48c34033/contourpy-1.3.2-pp310-pypy310_pp73-macosx_10_15_x86_64.whl", hash = "sha256:fd93cc7f3139b6dd7aab2f26a90dde0aa9fc264dbf70f6740d498a70b860b82c", size = 265681, upload-time = "2025-04-15T17:44:59.314Z" }, + { url = "https://files.pythonhosted.org/packages/2b/25/ac07d6ad12affa7d1ffed11b77417d0a6308170f44ff20fa1d5aa6333f03/contourpy-1.3.2-pp310-pypy310_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:107ba8a6a7eec58bb475329e6d3b95deba9440667c4d62b9b6063942b61d7f16", size = 315101, upload-time = "2025-04-15T17:45:04.165Z" }, + { url = "https://files.pythonhosted.org/packages/8f/4d/5bb3192bbe9d3f27e3061a6a8e7733c9120e203cb8515767d30973f71030/contourpy-1.3.2-pp310-pypy310_pp73-win_amd64.whl", hash = "sha256:ded1706ed0c1049224531b81128efbd5084598f18d8a2d9efae833edbd2b40ad", size = 220599, upload-time = "2025-04-15T17:45:08.456Z" }, +] + +[[package]] +name = "cycler" +version = "0.12.1" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/a9/95/a3dbbb5028f35eafb79008e7522a75244477d2838f38cbb722248dabc2a8/cycler-0.12.1.tar.gz", hash = "sha256:88bb128f02ba341da8ef447245a9e138fae777f6a23943da4540077d3601eb1c", size = 7615, upload-time = "2023-10-07T05:32:18.335Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/e7/05/c19819d5e3d95294a6f5947fb9b9629efb316b96de511b418c53d245aae6/cycler-0.12.1-py3-none-any.whl", hash = "sha256:85cef7cff222d8644161529808465972e51340599459b8ac3ccbac5a854e0d30", size = 8321, upload-time = "2023-10-07T05:32:16.783Z" }, +] + +[[package]] +name = "eigenpy" +version = "3.12.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, + { name = "cmeel-boost" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/1e/a5/7ec1dc873df269332c84e5b79b033fe53d55c5fd6517bd6d8bb5fb24e707/eigenpy-3.12.0.tar.gz", hash = "sha256:e9d07219df1e61e45db6e42001697c5637743b3ad3e0bfcf069fc94c5fab218d", size = 6556548, upload-time = "2025-08-23T17:54:34.041Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/c4/6d/25e69e262ec336c3b51eebfae9da536f57e93970b434dc7d8506ed3ee3f7/eigenpy-3.12.0-0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:27525792572d6d2cdc7dc407b253280b7f52b9b4dda900ad9cc4b27879e251f2", size = 5635279, upload-time = "2025-08-23T17:53:58.956Z" }, + { url = "https://files.pythonhosted.org/packages/05/cf/fc8729917859ce949d7ab07f0853fe4df45555322e56765f98152e5f2bf1/eigenpy-3.12.0-0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:a3e993c2adc4029673d0578dadec20c5a683c3de5b7768abf98129767e57c40a", size = 4865885, upload-time = "2025-08-23T17:54:01.012Z" }, + { url = "https://files.pythonhosted.org/packages/d0/b0/651af0c2db36f1ad62b2a40d439b65c13da8b38bd0e0a1bf67f3af6d0034/eigenpy-3.12.0-0-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:291c21c38a1faeb1823e6f821be1910b7751e0fecf12047217f98c3ab748183b", size = 6200013, upload-time = "2025-08-23T17:54:03.107Z" }, + { url = "https://files.pythonhosted.org/packages/c1/90/c0fc4227cf3f02b60cbdcf36946eb1239683273057d242b97e9af7a44ee3/eigenpy-3.12.0-0-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:5d94b52d087d9f317e1029a48cdf170bfca7f0e1c10581b4ce79b1d067fc5e1a", size = 6004382, upload-time = "2025-08-23T17:54:04.667Z" }, + { url = "https://files.pythonhosted.org/packages/f4/53/d6c7ef75acd8ac099b1ae9e2209936fb0ada9c675dfd6762c4392be19df5/eigenpy-3.12.0-1-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:6d660cd9ebdff808f4e9d49027e9ae5621d19de069deef67ef226e5d48cdcfb2", size = 5415254, upload-time = "2025-10-15T20:13:13.051Z" }, + { url = "https://files.pythonhosted.org/packages/5d/31/a7358942489a31edbded67e00771ff18261c67efddb9f37bdb353633e493/eigenpy-3.12.0-1-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:37c05f8431f9edbd5db2ab04a01d004b894929b70c6faea0f80004f97ddb2e1f", size = 4226332, upload-time = "2025-10-15T20:13:15.092Z" }, + { url = "https://files.pythonhosted.org/packages/2e/38/4d06a01b1fe3efb9c9bc10521d9718d5a263b316306429220bd2da8e134e/eigenpy-3.12.0-1-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:c5cbbee1f043baed9900a99918fb4326151be0145f793f8129fd1b0b938f2974", size = 6200413, upload-time = "2025-10-15T20:13:16.952Z" }, + { url = "https://files.pythonhosted.org/packages/fe/38/5aff8d72ebaf891a07f1368176508f75cc9e7bf8967d07f7fa1baedc6ac7/eigenpy-3.12.0-1-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:f579f3b23754f2156b43f27906481bc00fd46030c4e061e8e770cd02aacc3d88", size = 6037835, upload-time = "2025-10-15T20:13:18.826Z" }, +] + +[[package]] +name = "etils" +version = "1.13.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/9b/a0/522bbff0f3cdd37968f90dd7f26c7aa801ed87f5ba335f156de7f2b88a48/etils-1.13.0.tar.gz", hash = "sha256:a5b60c71f95bcd2d43d4e9fb3dc3879120c1f60472bb5ce19f7a860b1d44f607", size = 106368, upload-time = "2025-07-15T10:29:10.563Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/e7/98/87b5946356095738cb90a6df7b35ff69ac5750f6e783d5fbcc5cb3b6cbd7/etils-1.13.0-py3-none-any.whl", hash = "sha256:d9cd4f40fbe77ad6613b7348a18132cc511237b6c076dbb89105c0b520a4c6bb", size = 170603, upload-time = "2025-07-15T10:29:09.076Z" }, +] + +[package.optional-dependencies] +epath = [ + { name = "fsspec" }, + { name = "importlib-resources" }, + { name = "typing-extensions" }, + { name = "zipp" }, +] + +[[package]] +name = "exoskeleton-teleoperation-evidence" +version = "0.1.0" +source = { editable = "." } +dependencies = [ + { name = "cmeel-tinyxml2" }, + { name = "cmeel-urdfdom" }, + { name = "matplotlib" }, + { name = "mujoco" }, + { name = "numpy" }, + { name = "omegaconf" }, + { name = "pin" }, + { name = "pyyaml" }, + { name = "scipy" }, +] + +[package.metadata] +requires-dist = [ + { name = "cmeel-tinyxml2", specifier = "==10.0.0" }, + { name = "cmeel-urdfdom", specifier = "==4.0.1" }, + { name = "matplotlib", specifier = "==3.10.7" }, + { name = "mujoco", specifier = "==3.5.0" }, + { name = "numpy", specifier = "==2.2.6" }, + { name = "omegaconf", specifier = "==2.3.0" }, + { name = "pin", specifier = "==3.8.0" }, + { name = "pyyaml", specifier = "==6.0.3" }, + { name = "scipy", specifier = "==1.15.3" }, +] + +[[package]] +name = "fonttools" +version = "4.63.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/84/69/c97f2c18e0db87d2c7b15da1974dace76ae938f1cfa22e2727a648b7ed43/fonttools-4.63.0.tar.gz", hash = "sha256:caeb583deeb5168e694b65cda8b4ee62abedfa66cf88488734466f2366b9c4e0", size = 3597189, upload-time = "2026-05-14T12:04:30.958Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/f2/c9/4141c90a90db20f807c7e10bfd689fe53eb8f7f4caff58ee4d4dfe46919f/fonttools-4.63.0-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:e3297a6a4059b4acc3a1e9a8b04741f240a80044eef08ebd32e8b5bcdddce75b", size = 2884632, upload-time = "2026-05-14T12:02:38.56Z" }, + { url = "https://files.pythonhosted.org/packages/b8/46/ad12b5c10eae602d7ef814b02afa08aacbf89da917fed5b071282b7eadc2/fonttools-4.63.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:b1cd75a03ad8cb5bc40c90bfde68c0c47de423aa19e5c0f362b43520645eea94", size = 2429441, upload-time = "2026-05-14T12:02:41.162Z" }, + { url = "https://files.pythonhosted.org/packages/90/8f/bdca24a84c81d56fffed052229cdcff368f6e05882e526f4558891481f65/fonttools-4.63.0-cp310-cp310-manylinux2014_aarch64.manylinux_2_17_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:c0425b277a59cff3d80ca42162a8de360f318438a2ac83570842a678d826d579", size = 4946346, upload-time = "2026-05-14T12:02:43.41Z" }, + { url = "https://files.pythonhosted.org/packages/04/59/a639c0e136441ee91a65b56fdf89e5d075927e7a09c559d1b0f5276577db/fonttools-4.63.0-cp310-cp310-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:d7e5c9973aa04c95650c96e5f5ad865fbf42d62079163ecfab1e01cbc2504c22", size = 4903184, upload-time = "2026-05-14T12:02:45.742Z" }, + { url = "https://files.pythonhosted.org/packages/e6/53/91b7e0cb45b536f3da1b29ba8cbab89f27e8b986809e0b1982303a3f4eca/fonttools-4.63.0-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:cb014d58140a38135f16064c74c652ed57aa0b75cbf8bb59cac821f7edb5334e", size = 4922967, upload-time = "2026-05-14T12:02:48.386Z" }, + { url = "https://files.pythonhosted.org/packages/c7/b7/87439bf44e6b97c5538cd29d0b7e366a5b8ce2cc132a4134fb67fa3f2fa2/fonttools-4.63.0-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:032038247a96c1690f9f31e377c389383c902531b085aa4e4dabd6f57f870e69", size = 5042799, upload-time = "2026-05-14T12:02:50.424Z" }, + { url = "https://files.pythonhosted.org/packages/ad/7c/8b96c3263b89ef99cded544c0f0636686f85dbd3c211c4dceef0231fca23/fonttools-4.63.0-cp310-cp310-win32.whl", hash = "sha256:a8b33a82979e0a6a34ff435cc81317be1f95ec1ebb7a3a2d1c8a6a54f02ae44e", size = 1519704, upload-time = "2026-05-14T12:02:52.523Z" }, + { url = "https://files.pythonhosted.org/packages/e5/4d/2c2f0069970b6907de8fb5b05c5c0193cc22f717df151d1c7aef1c738f58/fonttools-4.63.0-cp310-cp310-win_amd64.whl", hash = "sha256:0c18358a155d75034911c5ee397a5b44cd19dd325dbb8b35fb60bf421d6a72ac", size = 1568666, upload-time = "2026-05-14T12:02:54.917Z" }, + { url = "https://files.pythonhosted.org/packages/2c/47/c99d5268f354002ce80f8d029cd9d7d872969da1de8b93d32de4dc56d6f4/fonttools-4.63.0-py3-none-any.whl", hash = "sha256:445af2eab030a16b9171ea8bdda7ebf7d96bda2df88ee182a464252f6e05e20d", size = 1164562, upload-time = "2026-05-14T12:04:29.092Z" }, +] + +[[package]] +name = "fsspec" +version = "2026.6.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/10/a1/ae4e3e5003468d6391d2c77b6fa1cd73bd5d13511d81c642d7b28ac90ed4/fsspec-2026.6.0.tar.gz", hash = "sha256:f5bac145310fe30e16e1471bd6840b2d990d609e872251d7e674241822abf01a", size = 313646, upload-time = "2026-06-16T01:57:28.105Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/e5/22/4222d7ddf3da30f363edaa98e329c2bce6c65497c9cb2810931c8b2c0fbc/fsspec-2026.6.0-py3-none-any.whl", hash = "sha256:02e0b71817df9b2169dc30a16832045764def1191b43dcff5bb85bdee212d2a1", size = 203949, upload-time = "2026-06-16T01:57:26.358Z" }, +] + +[[package]] +name = "glfw" +version = "2.10.2" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/0f/62/096058bcb4b4fb28f7ecd28fb048f07969d90b243c417af5f6d09d45a0c2/glfw-2.10.2.tar.gz", hash = "sha256:5d2cf97c66bc42a6b583be0e307eae5a3945438322e2ed0c5e4f14dc251d693d", size = 36307, upload-time = "2026-07-21T14:42:36.732Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/c4/80/5e575ec14f54dc0a644e7215985b9ee7c5044fcc3594d6b83dfdeb04ccd4/glfw-2.10.2-py2.py3-none-macosx_10_6_intel.whl", hash = "sha256:a04d25bb535a3a29e173ea1abd658e161681b5d3816e00bc51c122f74cd7fa3a", size = 110295, upload-time = "2026-07-21T14:42:25.625Z" }, + { url = "https://files.pythonhosted.org/packages/e5/2e/22216db429690aa51fa87d2e2b5e6b610c5a37b9df7768da2927f52f8704/glfw-2.10.2-py2.py3-none-macosx_11_0_arm64.whl", hash = "sha256:c000b8a5e5fb4374b2c18d12347255ccb92001b9bcf80ef74c56072acbb98fe3", size = 107146, upload-time = "2026-07-21T14:42:26.854Z" }, + { url = "https://files.pythonhosted.org/packages/75/a4/9ccee9d5c3b9c5d6bafce3e8d53ab8318eff30124e3c19b95580bebaa6d4/glfw-2.10.2-py2.py3-none-manylinux2014_aarch64.whl", hash = "sha256:0ffe2b48b51f37b05e8f027f57c0b8cd213affaf22fc83401bd9621692d4c6c8", size = 235005, upload-time = "2026-07-21T14:42:27.955Z" }, + { url = "https://files.pythonhosted.org/packages/77/44/37aeed50c581c76e1c5bb83b696a06f98f8ecc979ed8564dde3eef908e8b/glfw-2.10.2-py2.py3-none-manylinux2014_x86_64.whl", hash = "sha256:82daf1f87b2a48815637dc6f6720d4a55a57ba1d3683322dc20dc28065754f97", size = 246951, upload-time = "2026-07-21T14:42:29.107Z" }, + { url = "https://files.pythonhosted.org/packages/52/fc/29b4f1a8c8e33d8934781330a0845458b01da7f389d9fe6a3350e752b7f3/glfw-2.10.2-py2.py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:7604425ec95665cc6a25c3ed7d07e9660d394a34472ceffaec0843c844e7b649", size = 236018, upload-time = "2026-07-21T14:42:30.623Z" }, + { url = "https://files.pythonhosted.org/packages/69/84/1b98671314ea5f40397586b6cd14db913de3196333be558fdc177e61708f/glfw-2.10.2-py2.py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:b860de3ca0686182483f98f3ddd12e660acf25b3e0d521450ec9a3f999f72a65", size = 248490, upload-time = "2026-07-21T14:42:32.377Z" }, + { url = "https://files.pythonhosted.org/packages/12/d9/bd2e6c8dbadcf69fcd2289705e75881258b46221c196903a52e9f6e97322/glfw-2.10.2-py2.py3-none-win32.whl", hash = "sha256:a94aefe8c48886fd83cd6e11e936846166a4b5b94abf1f4bf599d984774b0b1b", size = 557654, upload-time = "2026-07-21T14:42:33.903Z" }, + { url = "https://files.pythonhosted.org/packages/e0/6c/4ca5f3ab85a8d7f612ce857208726e9b834e54e559751e3d4f98cc2f7509/glfw-2.10.2-py2.py3-none-win_amd64.whl", hash = "sha256:15fd0666cd8f1b0ecb535c3abb712b8ddda053bf8d16de2353b1c6ced0e65402", size = 564433, upload-time = "2026-07-21T14:42:35.465Z" }, +] + +[[package]] +name = "importlib-resources" +version = "7.1.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/e4/06/b56dfa750b44e86157093bc8fca0ab81dccbf5260510de4eaf1cb69b5b99/importlib_resources-7.1.0.tar.gz", hash = "sha256:0722d4c6212489c530f2a145a34c0a7a3b4721bc96a15fada5930e2a0b760708", size = 44985, upload-time = "2026-04-12T16:36:09.232Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/8a/db/55a262f3606bebcae07cc14095338471ad7c0bbcaa37707e6f0ee49725b7/importlib_resources-7.1.0-py3-none-any.whl", hash = "sha256:1bd7b48b4088eddb2cd16382150bb515af0bd2c70128194392725f82ad2c96a1", size = 37232, upload-time = "2026-04-12T16:36:08.219Z" }, +] + +[[package]] +name = "kiwisolver" +version = "1.5.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/d0/67/9c61eccb13f0bdca9307614e782fec49ffdde0f7a2314935d489fa93cd9c/kiwisolver-1.5.0.tar.gz", hash = "sha256:d4193f3d9dc3f6f79aaed0e5637f45d98850ebf01f7ca20e69457f3e8946b66a", size = 103482, upload-time = "2026-03-09T13:15:53.382Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/ac/f8/06549565caa026e540b7e7bab5c5a90eb7ca986015f4c48dace243cd24d9/kiwisolver-1.5.0-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:32cc0a5365239a6ea0c6ed461e8838d053b57e397443c0ca894dcc8e388d4374", size = 122802, upload-time = "2026-03-09T13:12:37.515Z" }, + { url = "https://files.pythonhosted.org/packages/84/eb/8476a0818850c563ff343ea7c9c05dcdcbd689a38e01aa31657df01f91fa/kiwisolver-1.5.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:cc0b66c1eec9021353a4b4483afb12dfd50e3669ffbb9152d6842eb34c7e29fd", size = 66216, upload-time = "2026-03-09T13:12:38.812Z" }, + { url = "https://files.pythonhosted.org/packages/f3/c4/f9c8a6b4c21aed4198566e45923512986d6cef530e7263b3a5f823546561/kiwisolver-1.5.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:86e0287879f75621ae85197b0877ed2f8b7aa57b511c7331dce2eb6f4de7d476", size = 63917, upload-time = "2026-03-09T13:12:40.053Z" }, + { url = "https://files.pythonhosted.org/packages/f1/0e/ba4ae25d03722f64de8b2c13e80d82ab537a06b30fc7065183c6439357e3/kiwisolver-1.5.0-cp310-cp310-manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:62f59da443c4f4849f73a51a193b1d9d258dcad0c41bc4d1b8fb2bcc04bfeb22", size = 1628776, upload-time = "2026-03-09T13:12:41.976Z" }, + { url = "https://files.pythonhosted.org/packages/8a/e4/3f43a011bc8a0860d1c96f84d32fa87439d3feedf66e672fef03bf5e8bac/kiwisolver-1.5.0-cp310-cp310-manylinux_2_24_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:9190426b7aa26c5229501fa297b8d0653cfd3f5a36f7990c264e157cbf886b3b", size = 1228164, upload-time = "2026-03-09T13:12:44.002Z" }, + { url = "https://files.pythonhosted.org/packages/4b/34/3a901559a1e0c218404f9a61a93be82d45cb8f44453ba43088644980f033/kiwisolver-1.5.0-cp310-cp310-manylinux_2_24_ppc64le.manylinux_2_28_ppc64le.whl", hash = "sha256:c8277104ded0a51e699c8c3aff63ce2c56d4ed5519a5f73e0fd7057f959a2b9e", size = 1246656, upload-time = "2026-03-09T13:12:45.557Z" }, + { url = "https://files.pythonhosted.org/packages/87/9e/f78c466ea20527822b95ad38f141f2de1dcd7f23fb8716b002b0d91bbe59/kiwisolver-1.5.0-cp310-cp310-manylinux_2_24_s390x.manylinux_2_28_s390x.whl", hash = "sha256:8f9baf6f0a6e7571c45c8863010b45e837c3ee1c2c77fcd6ef423be91b21fedb", size = 1295562, upload-time = "2026-03-09T13:12:47.562Z" }, + { url = "https://files.pythonhosted.org/packages/0a/66/fd0e4a612e3a286c24e6d6f3a5428d11258ed1909bc530ba3b59807fd980/kiwisolver-1.5.0-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:cff8e5383db4989311f99e814feeb90c4723eb4edca425b9d5d9c3fefcdd9537", size = 2178473, upload-time = "2026-03-09T13:12:50.254Z" }, + { url = "https://files.pythonhosted.org/packages/dc/8e/6cac929e0049539e5ee25c1ee937556f379ba5204840d03008363ced662d/kiwisolver-1.5.0-cp310-cp310-musllinux_1_2_ppc64le.whl", hash = "sha256:ebae99ed6764f2b5771c522477b311be313e8841d2e0376db2b10922daebbba4", size = 2274035, upload-time = "2026-03-09T13:12:51.785Z" }, + { url = "https://files.pythonhosted.org/packages/ca/d3/9d0c18f1b52ea8074b792452cf17f1f5a56bd0302a85191f405cfbf9da16/kiwisolver-1.5.0-cp310-cp310-musllinux_1_2_s390x.whl", hash = "sha256:d5cd5189fc2b6a538b75ae45433140c4823463918f7b1617c31e68b085c0022c", size = 2443217, upload-time = "2026-03-09T13:12:53.329Z" }, + { url = "https://files.pythonhosted.org/packages/45/2a/6e19368803a038b2a90857bf4ee9e3c7b667216d045866bf22d3439fd75e/kiwisolver-1.5.0-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:f42c23db5d1521218a3276bb08666dcb662896a0be7347cba864eca45ff64ede", size = 2249196, upload-time = "2026-03-09T13:12:55.057Z" }, + { url = "https://files.pythonhosted.org/packages/75/2b/3f641dfcbe72e222175d626bacf2f72c3b34312afec949dd1c50afa400f5/kiwisolver-1.5.0-cp310-cp310-win_amd64.whl", hash = "sha256:94eff26096eb5395136634622515b234ecb6c9979824c1f5004c6e3c3c85ccd2", size = 73389, upload-time = "2026-03-09T13:12:56.496Z" }, + { url = "https://files.pythonhosted.org/packages/da/88/299b137b9e0025d8982e03d2d52c123b0a2b159e84b0ef1501ef446339cf/kiwisolver-1.5.0-cp310-cp310-win_arm64.whl", hash = "sha256:dd952e03bfbb096cfe2dd35cd9e00f269969b67536cb4370994afc20ff2d0875", size = 64782, upload-time = "2026-03-09T13:12:57.609Z" }, + { url = "https://files.pythonhosted.org/packages/17/6f/6fd4f690a40c2582fa34b97d2678f718acf3706b91d270c65ecb455d0a06/kiwisolver-1.5.0-pp310-pypy310_pp73-macosx_10_15_x86_64.whl", hash = "sha256:295d9ffe712caa9f8a3081de8d32fc60191b4b51c76f02f951fd8407253528f4", size = 59606, upload-time = "2026-03-09T13:15:40.81Z" }, + { url = "https://files.pythonhosted.org/packages/82/a0/2355d5e3b338f13ce63f361abb181e3b6ea5fffdb73f739b3e80efa76159/kiwisolver-1.5.0-pp310-pypy310_pp73-macosx_11_0_arm64.whl", hash = "sha256:51e8c4084897de9f05898c2c2a39af6318044ae969d46ff7a34ed3f96274adca", size = 57537, upload-time = "2026-03-09T13:15:42.071Z" }, + { url = "https://files.pythonhosted.org/packages/c8/b9/1d50e610ecadebe205b71d6728fd224ce0e0ca6aba7b9cbe1da049203ac5/kiwisolver-1.5.0-pp310-pypy310_pp73-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:b83af57bdddef03c01a9138034c6ff03181a3028d9a1003b301eb1a55e161a3f", size = 79888, upload-time = "2026-03-09T13:15:43.317Z" }, + { url = "https://files.pythonhosted.org/packages/cd/ee/b85ffcd75afed0357d74f0e6fc02a4507da441165de1ca4760b9f496390d/kiwisolver-1.5.0-pp310-pypy310_pp73-manylinux_2_24_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:bf4679a3d71012a7c2bf360e5cd878fbd5e4fcac0896b56393dec239d81529ed", size = 77584, upload-time = "2026-03-09T13:15:44.605Z" }, + { url = "https://files.pythonhosted.org/packages/6b/dd/644d0dde6010a8583b4cd66dd41c5f83f5325464d15c4f490b3340ab73b4/kiwisolver-1.5.0-pp310-pypy310_pp73-win_amd64.whl", hash = "sha256:41024ed50e44ab1a60d3fe0a9d15a4ccc9f5f2b1d814ff283c8d01134d5b81bc", size = 73390, upload-time = "2026-03-09T13:15:45.832Z" }, +] + +[[package]] +name = "libcoal" +version = "3.0.2" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, + { name = "cmeel-assimp" }, + { name = "cmeel-boost" }, + { name = "cmeel-octomap" }, + { name = "cmeel-qhull" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/cc/51/cb68b16abd786e3ebb5e7e64036894a6f69ea8fe45c04a433e6d5462d60e/libcoal-3.0.2.tar.gz", hash = "sha256:1d48cfdce1157d4b89cf6a7215fc1b1e120d54c4a8d975cd9f45f2c8cedec275", size = 1464086, upload-time = "2025-10-15T22:53:34.811Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/e3/ea/6aa65497d00ec494bf1c5e121b59ad8cf6da308e0cf01271a9d7c614752c/libcoal-3.0.2-0-py3-none-macosx_10_9_x86_64.whl", hash = "sha256:16702fdd13942080c42c9565eb1b692618ce4456192a5bc497c585f9142138e5", size = 1683950, upload-time = "2025-10-15T22:53:28.361Z" }, + { url = "https://files.pythonhosted.org/packages/ba/b0/3480197ba40cf9c6de71ca7f7a81a7504a40ca77a2b8604cbcc068f8f7ca/libcoal-3.0.2-0-py3-none-macosx_11_0_arm64.whl", hash = "sha256:527c710c6215936f1a4b99ca1d01b4bb15c6b52980fa96cfa5f1fd1a7ef12393", size = 1484168, upload-time = "2025-10-15T22:53:29.933Z" }, + { url = "https://files.pythonhosted.org/packages/e2/e5/5b9605496e48a0437152196e5f200433d3904e59c899cab799a3c27bcd4f/libcoal-3.0.2-0-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:ed45722c07a3d23a346211f837856549dce11167928743eb6c73bcf17a369dd6", size = 2257523, upload-time = "2025-10-15T22:53:31.214Z" }, + { url = "https://files.pythonhosted.org/packages/3c/49/c3bec783144c226b5ef3728ed66d7fc2d08c553922a3892591958284801a/libcoal-3.0.2-0-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:c4ca3fec02386e5c8ccc81030c44e74a546b06059886ce04bb6c16fe4628e9ba", size = 2285635, upload-time = "2025-10-15T22:53:33.142Z" }, +] + +[[package]] +name = "libpinocchio" +version = "3.8.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, + { name = "cmeel-boost" }, + { name = "cmeel-urdfdom" }, + { name = "libcoal" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/0b/9f/b4431f1acdce04300d798a87b98b064c1bb56061848abd9476c7b7e9dac2/libpinocchio-3.8.0.tar.gz", hash = "sha256:687442a8316d03cbe1a5c66e20499bf3fadb59439d6207e36118eef34f73d8c8", size = 4001141, upload-time = "2025-10-16T06:34:02.405Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/32/71/b17ca7f4c0cb0f216441222e22c3fb8d905ba038ec5ac7c120790340da95/libpinocchio-3.8.0-0-py3-none-macosx_10_9_x86_64.whl", hash = "sha256:b8266d37482c35b5aa27240f3a0274447cd038aa219bdd6413c0bafcad822e2b", size = 4663536, upload-time = "2025-10-16T06:33:55.707Z" }, + { url = "https://files.pythonhosted.org/packages/7b/a9/a4842e056d3f7d07c3f96f90c8f7fe7ef7e543c725f1c9498e5f4d58c47c/libpinocchio-3.8.0-0-py3-none-macosx_11_0_arm64.whl", hash = "sha256:0320c471bd4e78226cc266ad7927432f884709104fa8a253e565adbed7da8aac", size = 3781718, upload-time = "2025-10-16T06:33:57.483Z" }, + { url = "https://files.pythonhosted.org/packages/fa/f5/950cd3be129766d6f847cb0702f73ad5f6ed2d2b5775e073f9f017d923b4/libpinocchio-3.8.0-0-py3-none-manylinux_2_28_aarch64.whl", hash = "sha256:b70bc23fb9f53d0a65929c92bac8c0df836bef064225a54d009214cdd778bdb7", size = 4582702, upload-time = "2025-10-16T06:33:58.887Z" }, + { url = "https://files.pythonhosted.org/packages/28/0d/5deebded1fa71a381c9efd3ea69103a38f64d804da704148e92f4886762d/libpinocchio-3.8.0-0-py3-none-manylinux_2_28_x86_64.whl", hash = "sha256:b52ca3520635f551ab2c8c9bf5e8e555b54e92c4bb948020eb4e4dc1b3f9eb0b", size = 4803646, upload-time = "2025-10-16T06:34:00.662Z" }, +] + +[[package]] +name = "matplotlib" +version = "3.10.7" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "contourpy" }, + { name = "cycler" }, + { name = "fonttools" }, + { name = "kiwisolver" }, + { name = "numpy" }, + { name = "packaging" }, + { name = "pillow" }, + { name = "pyparsing" }, + { name = "python-dateutil" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/ae/e2/d2d5295be2f44c678ebaf3544ba32d20c1f9ef08c49fe47f496180e1db15/matplotlib-3.10.7.tar.gz", hash = "sha256:a06ba7e2a2ef9131c79c49e63dad355d2d878413a0376c1727c8b9335ff731c7", size = 34804865, upload-time = "2025-10-09T00:28:00.669Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/6c/87/3932d5778ab4c025db22710b61f49ccaed3956c5cf46ffb2ffa7492b06d9/matplotlib-3.10.7-cp310-cp310-macosx_10_12_x86_64.whl", hash = "sha256:7ac81eee3b7c266dd92cee1cd658407b16c57eed08c7421fa354ed68234de380", size = 8247141, upload-time = "2025-10-09T00:26:06.023Z" }, + { url = "https://files.pythonhosted.org/packages/45/a8/bfed45339160102bce21a44e38a358a1134a5f84c26166de03fb4a53208f/matplotlib-3.10.7-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:667ecd5d8d37813a845053d8f5bf110b534c3c9f30e69ebd25d4701385935a6d", size = 8107995, upload-time = "2025-10-09T00:26:08.669Z" }, + { url = "https://files.pythonhosted.org/packages/e2/3c/5692a2d9a5ba848fda3f48d2b607037df96460b941a59ef236404b39776b/matplotlib-3.10.7-cp310-cp310-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:cc1c51b846aca49a5a8b44fbba6a92d583a35c64590ad9e1e950dc88940a4297", size = 8680503, upload-time = "2025-10-09T00:26:10.607Z" }, + { url = "https://files.pythonhosted.org/packages/ab/a0/86ace53c48b05d0e6e9c127b2ace097434901f3e7b93f050791c8243201a/matplotlib-3.10.7-cp310-cp310-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:4a11c2e9e72e7de09b7b72e62f3df23317c888299c875e2b778abf1eda8c0a42", size = 9514982, upload-time = "2025-10-09T00:26:12.594Z" }, + { url = "https://files.pythonhosted.org/packages/a6/81/ead71e2824da8f72640a64166d10e62300df4ae4db01a0bac56c5b39fa51/matplotlib-3.10.7-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:f19410b486fdd139885ace124e57f938c1e6a3210ea13dd29cab58f5d4bc12c7", size = 9566429, upload-time = "2025-10-09T00:26:14.758Z" }, + { url = "https://files.pythonhosted.org/packages/65/7d/954b3067120456f472cce8fdcacaf4a5fcd522478db0c37bb243c7cb59dd/matplotlib-3.10.7-cp310-cp310-win_amd64.whl", hash = "sha256:b498e9e4022f93de2d5a37615200ca01297ceebbb56fe4c833f46862a490f9e3", size = 8108174, upload-time = "2025-10-09T00:26:17.015Z" }, + { url = "https://files.pythonhosted.org/packages/1e/6c/a9bcf03e9afb2a873e0a5855f79bce476d1023f26f8212969f2b7504756c/matplotlib-3.10.7-pp310-pypy310_pp73-macosx_10_15_x86_64.whl", hash = "sha256:5c09cf8f2793f81368f49f118b6f9f937456362bee282eac575cca7f84cda537", size = 8241204, upload-time = "2025-10-09T00:27:48.806Z" }, + { url = "https://files.pythonhosted.org/packages/5b/fd/0e6f5aa762ed689d9fa8750b08f1932628ffa7ed30e76423c399d19407d2/matplotlib-3.10.7-pp310-pypy310_pp73-macosx_11_0_arm64.whl", hash = "sha256:de66744b2bb88d5cd27e80dfc2ec9f0517d0a46d204ff98fe9e5f2864eb67657", size = 8104607, upload-time = "2025-10-09T00:27:50.876Z" }, + { url = "https://files.pythonhosted.org/packages/b9/a9/21c9439d698fac5f0de8fc68b2405b738ed1f00e1279c76f2d9aa5521ead/matplotlib-3.10.7-pp310-pypy310_pp73-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:53cc80662dd197ece414dd5b66e07370201515a3eaf52e7c518c68c16814773b", size = 8682257, upload-time = "2025-10-09T00:27:52.597Z" }, +] + +[[package]] +name = "mujoco" +version = "3.5.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "absl-py" }, + { name = "etils", extra = ["epath"] }, + { name = "glfw" }, + { name = "numpy" }, + { name = "pyopengl" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/56/0d/005f0d49ad5878f0611a7c018550b8504d480a7a17ad7e6773ff47d8627a/mujoco-3.5.0.tar.gz", hash = "sha256:5c85a6fc7560ab5fa4534f35ff459e12dc3609681f307e457dbb49b6217f4d73", size = 912543, upload-time = "2026-02-13T01:02:51.554Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/e8/20/9e0595e653543df3e4233bc3ad7e50b371b81dbe48d45ffbc867ed7c379d/mujoco-3.5.0-cp310-cp310-macosx_10_16_x86_64.whl", hash = "sha256:c4324161cb4f334dd984fbb4a4f7d7db9f914f40d06174b02dcf05463d8275e4", size = 7088320, upload-time = "2026-02-13T01:02:06.745Z" }, + { url = "https://files.pythonhosted.org/packages/8d/6b/fdac8ed97086e12ac930fb44e419eda1626e339010df73678cb1f22527d7/mujoco-3.5.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:5f3803ff0dd7bc04d6c47d53a794343843bde06f0aeefeac28bb62b4cf2baab3", size = 7093261, upload-time = "2026-02-13T01:02:09.857Z" }, + { url = "https://files.pythonhosted.org/packages/19/ce/abcd9cc6ee7802f97c729ae0ccd517c68f04882f5db755b178e199511dc2/mujoco-3.5.0-cp310-cp310-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:e13560991c779a139b53151733a0a6f3420ef09459b32d90302c2661c1b20992", size = 6637850, upload-time = "2026-02-13T01:02:11.808Z" }, + { url = "https://files.pythonhosted.org/packages/ca/d6/a5a7b615b257867b7c97db6b3ce07dec9351d5d9d5a5aca881cbb583d7a3/mujoco-3.5.0-cp310-cp310-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:01b12896ae906f157e18d8b1b7c24a8b72d2576fffa09869047150f186e92b33", size = 7079429, upload-time = "2026-02-13T01:02:13.738Z" }, + { url = "https://files.pythonhosted.org/packages/7e/91/d82dd3c16892e1b0e27a2f537eec8aad54d91d939cb3cd37db2e8c09ecc2/mujoco-3.5.0-cp310-cp310-win_amd64.whl", hash = "sha256:2328358d2f0031175897092560dd6d04b14bab1cc22caa145ce99b843c17daa2", size = 5624454, upload-time = "2026-02-13T01:02:15.714Z" }, +] + +[[package]] +name = "numpy" +version = "2.2.6" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/76/21/7d2a95e4bba9dc13d043ee156a356c0a8f0c6309dff6b21b4d71a073b8a8/numpy-2.2.6.tar.gz", hash = "sha256:e29554e2bef54a90aa5cc07da6ce955accb83f21ab5de01a62c8478897b264fd", size = 20276440, upload-time = "2025-05-17T22:38:04.611Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/9a/3e/ed6db5be21ce87955c0cbd3009f2803f59fa08df21b5df06862e2d8e2bdd/numpy-2.2.6-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:b412caa66f72040e6d268491a59f2c43bf03eb6c96dd8f0307829feb7fa2b6fb", size = 21165245, upload-time = "2025-05-17T21:27:58.555Z" }, + { url = "https://files.pythonhosted.org/packages/22/c2/4b9221495b2a132cc9d2eb862e21d42a009f5a60e45fc44b00118c174bff/numpy-2.2.6-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:8e41fd67c52b86603a91c1a505ebaef50b3314de0213461c7a6e99c9a3beff90", size = 14360048, upload-time = "2025-05-17T21:28:21.406Z" }, + { url = "https://files.pythonhosted.org/packages/fd/77/dc2fcfc66943c6410e2bf598062f5959372735ffda175b39906d54f02349/numpy-2.2.6-cp310-cp310-macosx_14_0_arm64.whl", hash = "sha256:37e990a01ae6ec7fe7fa1c26c55ecb672dd98b19c3d0e1d1f326fa13cb38d163", size = 5340542, upload-time = "2025-05-17T21:28:30.931Z" }, + { url = "https://files.pythonhosted.org/packages/7a/4f/1cb5fdc353a5f5cc7feb692db9b8ec2c3d6405453f982435efc52561df58/numpy-2.2.6-cp310-cp310-macosx_14_0_x86_64.whl", hash = "sha256:5a6429d4be8ca66d889b7cf70f536a397dc45ba6faeb5f8c5427935d9592e9cf", size = 6878301, upload-time = "2025-05-17T21:28:41.613Z" }, + { url = "https://files.pythonhosted.org/packages/eb/17/96a3acd228cec142fcb8723bd3cc39c2a474f7dcf0a5d16731980bcafa95/numpy-2.2.6-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:efd28d4e9cd7d7a8d39074a4d44c63eda73401580c5c76acda2ce969e0a38e83", size = 14297320, upload-time = "2025-05-17T21:29:02.78Z" }, + { url = "https://files.pythonhosted.org/packages/b4/63/3de6a34ad7ad6646ac7d2f55ebc6ad439dbbf9c4370017c50cf403fb19b5/numpy-2.2.6-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:fc7b73d02efb0e18c000e9ad8b83480dfcd5dfd11065997ed4c6747470ae8915", size = 16801050, upload-time = "2025-05-17T21:29:27.675Z" }, + { url = "https://files.pythonhosted.org/packages/07/b6/89d837eddef52b3d0cec5c6ba0456c1bf1b9ef6a6672fc2b7873c3ec4e2e/numpy-2.2.6-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:74d4531beb257d2c3f4b261bfb0fc09e0f9ebb8842d82a7b4209415896adc680", size = 15807034, upload-time = "2025-05-17T21:29:51.102Z" }, + { url = "https://files.pythonhosted.org/packages/01/c8/dc6ae86e3c61cfec1f178e5c9f7858584049b6093f843bca541f94120920/numpy-2.2.6-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:8fc377d995680230e83241d8a96def29f204b5782f371c532579b4f20607a289", size = 18614185, upload-time = "2025-05-17T21:30:18.703Z" }, + { url = "https://files.pythonhosted.org/packages/5b/c5/0064b1b7e7c89137b471ccec1fd2282fceaae0ab3a9550f2568782d80357/numpy-2.2.6-cp310-cp310-win32.whl", hash = "sha256:b093dd74e50a8cba3e873868d9e93a85b78e0daf2e98c6797566ad8044e8363d", size = 6527149, upload-time = "2025-05-17T21:30:29.788Z" }, + { url = "https://files.pythonhosted.org/packages/a3/dd/4b822569d6b96c39d1215dbae0582fd99954dcbcf0c1a13c61783feaca3f/numpy-2.2.6-cp310-cp310-win_amd64.whl", hash = "sha256:f0fd6321b839904e15c46e0d257fdd101dd7f530fe03fd6359c1ea63738703f3", size = 12904620, upload-time = "2025-05-17T21:30:48.994Z" }, + { url = "https://files.pythonhosted.org/packages/9e/3b/d94a75f4dbf1ef5d321523ecac21ef23a3cd2ac8b78ae2aac40873590229/numpy-2.2.6-pp310-pypy310_pp73-macosx_10_15_x86_64.whl", hash = "sha256:0b605b275d7bd0c640cad4e5d30fa701a8d59302e127e5f79138ad62762c3e3d", size = 21040391, upload-time = "2025-05-17T21:44:35.948Z" }, + { url = "https://files.pythonhosted.org/packages/17/f4/09b2fa1b58f0fb4f7c7963a1649c64c4d315752240377ed74d9cd878f7b5/numpy-2.2.6-pp310-pypy310_pp73-macosx_14_0_x86_64.whl", hash = "sha256:7befc596a7dc9da8a337f79802ee8adb30a552a94f792b9c9d18c840055907db", size = 6786754, upload-time = "2025-05-17T21:44:47.446Z" }, + { url = "https://files.pythonhosted.org/packages/af/30/feba75f143bdc868a1cc3f44ccfa6c4b9ec522b36458e738cd00f67b573f/numpy-2.2.6-pp310-pypy310_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ce47521a4754c8f4593837384bd3424880629f718d87c5d44f8ed763edd63543", size = 16643476, upload-time = "2025-05-17T21:45:11.871Z" }, + { url = "https://files.pythonhosted.org/packages/37/48/ac2a9584402fb6c0cd5b5d1a91dcf176b15760130dd386bbafdbfe3640bf/numpy-2.2.6-pp310-pypy310_pp73-win_amd64.whl", hash = "sha256:d042d24c90c41b54fd506da306759e06e568864df8ec17ccc17e9e884634fd00", size = 12812666, upload-time = "2025-05-17T21:45:31.426Z" }, +] + +[[package]] +name = "omegaconf" +version = "2.3.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "antlr4-python3-runtime" }, + { name = "pyyaml" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/09/48/6388f1bb9da707110532cb70ec4d2822858ddfb44f1cdf1233c20a80ea4b/omegaconf-2.3.0.tar.gz", hash = "sha256:d5d4b6d29955cc50ad50c46dc269bcd92c6e00f5f90d23ab5fee7bfca4ba4cc7", size = 3298120, upload-time = "2022-12-08T20:59:22.753Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/e3/94/1843518e420fa3ed6919835845df698c7e27e183cb997394e4a670973a65/omegaconf-2.3.0-py3-none-any.whl", hash = "sha256:7b4df175cdb08ba400f45cae3bdcae7ba8365db4d165fc65fd04b050ab63b46b", size = 79500, upload-time = "2022-12-08T20:59:19.686Z" }, +] + +[[package]] +name = "packaging" +version = "26.2" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/d7/f1/e7a6dd94a8d4a5626c03e4e99c87f241ba9e350cd9e6d75123f992427270/packaging-26.2.tar.gz", hash = "sha256:ff452ff5a3e828ce110190feff1178bb1f2ea2281fa2075aadb987c2fb221661", size = 228134, upload-time = "2026-04-24T20:15:23.917Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/df/b2/87e62e8c3e2f4b32e5fe99e0b86d576da1312593b39f47d8ceef365e95ed/packaging-26.2-py3-none-any.whl", hash = "sha256:5fc45236b9446107ff2415ce77c807cee2862cb6fac22b8a73826d0693b0980e", size = 100195, upload-time = "2026-04-24T20:15:22.081Z" }, +] + +[[package]] +name = "pillow" +version = "12.3.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/1c/3d/bb7fca845737cf9d7dbde16ed1843984665ff2e0a518f5db43e77ec540b9/pillow-12.3.0.tar.gz", hash = "sha256:3b8182a766685eaa002637e28b4ec8d6b18819a0c71f579bf0dbaa5830297cce", size = 47025035, upload-time = "2026-07-01T11:56:38.965Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/25/c2/669d88644cddb1485bd9534e63e8cf476c8e51cb3c3a1297677023505c0e/pillow-12.3.0-cp310-cp310-macosx_10_10_x86_64.whl", hash = "sha256:6c0016e7b354317c4e9e525b937ac8596c38d2d232b419529b9cd7a1cd46e39a", size = 5392418, upload-time = "2026-07-01T11:53:27.808Z" }, + { url = "https://files.pythonhosted.org/packages/6b/ba/3762f376a2948e3036488d773a146e0ae6ecc2ca03ac20e2615bd0b2ba02/pillow-12.3.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:bcc33feacfaefce60c12fd500a277533bdc02b10a19f7f6d348763d8140bbba7", size = 4785287, upload-time = "2026-07-01T11:53:29.761Z" }, + { url = "https://files.pythonhosted.org/packages/07/50/b5d688cc9c52d4482f3d5bcab6ce20bc2a74a85d2343841c907444a3be2c/pillow-12.3.0-cp310-cp310-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:5594fc43d548a7ed94949d139aa1341b270f1863f11cfd37f5a6c8b778a6b67f", size = 6253754, upload-time = "2026-07-01T11:53:32.298Z" }, + { url = "https://files.pythonhosted.org/packages/4e/89/36f4cd76cf4baf05c50ababb976249153f18c959171c7f6ba09a6f217260/pillow-12.3.0-cp310-cp310-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:f0606c8bf2cdefea14a43530f7657cbbb7ecf1c4222512492ef4a4434a9501ec", size = 6925605, upload-time = "2026-07-01T11:53:34.487Z" }, + { url = "https://files.pythonhosted.org/packages/eb/c0/4de58cf6633b9e3a6061ef4be6fb91fc3c90b812ece886f531e3c523d777/pillow-12.3.0-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:85f998ea1848bc6757289e739cfbdda3a04adfd58b02fc018ce54d754a5ce468", size = 6327788, upload-time = "2026-07-01T11:53:36.433Z" }, + { url = "https://files.pythonhosted.org/packages/87/3c/14d53682a19550dbbaf3b598f807d5457646c510805a44c7d7891cd1cd1a/pillow-12.3.0-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:25b9b82bb22e6e2b3cd07b39c68b7b862001226cb3dff7130d1cb914121b39ed", size = 7036288, upload-time = "2026-07-01T11:53:38.712Z" }, + { url = "https://files.pythonhosted.org/packages/38/1d/36279e3c77efe034e4cc2b0393ee74ffdb5a62391dacbf9b916154f5f0b8/pillow-12.3.0-cp310-cp310-win32.whl", hash = "sha256:37dc8f7bbb66efe481bb60defacef820c950c24713fb44962ed6aa2a50966de1", size = 6472396, upload-time = "2026-07-01T11:53:40.781Z" }, + { url = "https://files.pythonhosted.org/packages/48/7c/8fa0039574c476d7c6fa57dd7c32a130436877c6ec1e5ce1cc8ec44878c1/pillow-12.3.0-cp310-cp310-win_amd64.whl", hash = "sha256:300557495eb45ebb8aec96c2da9c4be642fbf7cd937278b4013ba894ea8eb0eb", size = 7226887, upload-time = "2026-07-01T11:53:42.764Z" }, + { url = "https://files.pythonhosted.org/packages/fa/17/e324be141d173c1c919428066c3259f21c1b8982e564e01a4a81e96dbdcf/pillow-12.3.0-cp310-cp310-win_arm64.whl", hash = "sha256:514435a37670e3e5e08f3945b68718b6ed329bb84367777e16f9f4dfe1e61a0f", size = 2568039, upload-time = "2026-07-01T11:53:45.372Z" }, +] + +[[package]] +name = "pin" +version = "3.8.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "cmeel" }, + { name = "cmeel-boost" }, + { name = "cmeel-urdfdom" }, + { name = "coal" }, + { name = "libpinocchio" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/3b/99/4e7393e8035985405e89bc61dc0037f9bd1792c7a0295192aa3791bf4844/pin-3.8.0.tar.gz", hash = "sha256:f3889867d6fb968299696e94974138d6668600663b8650723a59fe062356fece", size = 4000900, upload-time = "2025-10-16T14:04:29.889Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/f8/6b/0d280cc9753acb1bca1ffad8138f1c3939a797a336b9b058a051267b4aea/pin-3.8.0-0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:92046a8b0599d2396e0f5303f81f76ad306315d7a45cc44bb1ad8afacc59760c", size = 5634231, upload-time = "2025-10-16T14:03:49.108Z" }, + { url = "https://files.pythonhosted.org/packages/c1/df/b7c9cbb484a0c096e7b4beb22fed4c5bf77c5bb042fe22702ce9c3757bb7/pin-3.8.0-0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:2565eebc9dd2f84181cddc66c356f2896a64162ed1eadc7d3a60e6a034d6a5ae", size = 5420549, upload-time = "2025-10-16T14:03:51.642Z" }, + { url = "https://files.pythonhosted.org/packages/c3/2e/1cb2fc19cd5ee830a9bc992956d9ef83a3dcee347edbb56d8c35d069b374/pin-3.8.0-0-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:4b2e0ae3f5b06538f78f84e385c9d5d2a8470828b108520a1cf0657f658521e8", size = 7242690, upload-time = "2025-10-16T14:03:53.369Z" }, + { url = "https://files.pythonhosted.org/packages/78/9a/8f93ca590dab6058283d0cc3ee776ba3a72f6d8662e3c7e3b6b9424faee0/pin-3.8.0-0-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:d9a48b99f8d3b085575f88944f1537a048fcd262da3efc52fed732b220e1422f", size = 7402696, upload-time = "2025-10-16T14:03:55.133Z" }, +] + +[[package]] +name = "pyopengl" +version = "3.1.10" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/6f/16/912b7225d56284859cd9a672827f18be43f8012f8b7b932bc4bd959a298e/pyopengl-3.1.10.tar.gz", hash = "sha256:c4a02d6866b54eb119c8e9b3fb04fa835a95ab802dd96607ab4cdb0012df8335", size = 1915580, upload-time = "2025-08-18T02:33:01.76Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/de/e4/1ba6f44e491c4eece978685230dde56b14d51a0365bc1b774ddaa94d14cd/pyopengl-3.1.10-py3-none-any.whl", hash = "sha256:794a943daced39300879e4e47bd94525280685f42dbb5a998d336cfff151d74f", size = 3194996, upload-time = "2025-08-18T02:32:59.902Z" }, +] + +[[package]] +name = "pyparsing" +version = "3.3.2" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/f3/91/9c6ee907786a473bf81c5f53cf703ba0957b23ab84c264080fb5a450416f/pyparsing-3.3.2.tar.gz", hash = "sha256:c777f4d763f140633dcb6d8a3eda953bf7a214dc4eff598413c070bcdc117cbc", size = 6851574, upload-time = "2026-01-21T03:57:59.36Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/10/bd/c038d7cc38edc1aa5bf91ab8068b63d4308c66c4c8bb3cbba7dfbc049f9c/pyparsing-3.3.2-py3-none-any.whl", hash = "sha256:850ba148bd908d7e2411587e247a1e4f0327839c40e2e5e6d05a007ecc69911d", size = 122781, upload-time = "2026-01-21T03:57:55.912Z" }, +] + +[[package]] +name = "python-dateutil" +version = "2.9.0.post0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "six" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/66/c0/0c8b6ad9f17a802ee498c46e004a0eb49bc148f2fd230864601a86dcf6db/python-dateutil-2.9.0.post0.tar.gz", hash = "sha256:37dd54208da7e1cd875388217d5e00ebd4179249f90fb72437e91a35459a0ad3", size = 342432, upload-time = "2024-03-01T18:36:20.211Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/ec/57/56b9bcc3c9c6a792fcbaf139543cee77261f3651ca9da0c93f5c1221264b/python_dateutil-2.9.0.post0-py2.py3-none-any.whl", hash = "sha256:a8b2bc7bffae282281c8140a97d3aa9c14da0b136dfe83f850eea9a5f7470427", size = 229892, upload-time = "2024-03-01T18:36:18.57Z" }, +] + +[[package]] +name = "pyyaml" +version = "6.0.3" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/05/8e/961c0007c59b8dd7729d542c61a4d537767a59645b82a0b521206e1e25c2/pyyaml-6.0.3.tar.gz", hash = "sha256:d76623373421df22fb4cf8817020cbb7ef15c725b9d5e45f17e189bfc384190f", size = 130960, upload-time = "2025-09-25T21:33:16.546Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/f4/a0/39350dd17dd6d6c6507025c0e53aef67a9293a6d37d3511f23ea510d5800/pyyaml-6.0.3-cp310-cp310-macosx_10_13_x86_64.whl", hash = "sha256:214ed4befebe12df36bcc8bc2b64b396ca31be9304b8f59e25c11cf94a4c033b", size = 184227, upload-time = "2025-09-25T21:31:46.04Z" }, + { url = "https://files.pythonhosted.org/packages/05/14/52d505b5c59ce73244f59c7a50ecf47093ce4765f116cdb98286a71eeca2/pyyaml-6.0.3-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:02ea2dfa234451bbb8772601d7b8e426c2bfa197136796224e50e35a78777956", size = 174019, upload-time = "2025-09-25T21:31:47.706Z" }, + { url = "https://files.pythonhosted.org/packages/43/f7/0e6a5ae5599c838c696adb4e6330a59f463265bfa1e116cfd1fbb0abaaae/pyyaml-6.0.3-cp310-cp310-manylinux2014_aarch64.manylinux_2_17_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:b30236e45cf30d2b8e7b3e85881719e98507abed1011bf463a8fa23e9c3e98a8", size = 740646, upload-time = "2025-09-25T21:31:49.21Z" }, + { url = "https://files.pythonhosted.org/packages/2f/3a/61b9db1d28f00f8fd0ae760459a5c4bf1b941baf714e207b6eb0657d2578/pyyaml-6.0.3-cp310-cp310-manylinux2014_s390x.manylinux_2_17_s390x.manylinux_2_28_s390x.whl", hash = "sha256:66291b10affd76d76f54fad28e22e51719ef9ba22b29e1d7d03d6777a9174198", size = 840793, upload-time = "2025-09-25T21:31:50.735Z" }, + { url = "https://files.pythonhosted.org/packages/7a/1e/7acc4f0e74c4b3d9531e24739e0ab832a5edf40e64fbae1a9c01941cabd7/pyyaml-6.0.3-cp310-cp310-manylinux2014_x86_64.manylinux_2_17_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:9c7708761fccb9397fe64bbc0395abcae8c4bf7b0eac081e12b809bf47700d0b", size = 770293, upload-time = "2025-09-25T21:31:51.828Z" }, + { url = "https://files.pythonhosted.org/packages/8b/ef/abd085f06853af0cd59fa5f913d61a8eab65d7639ff2a658d18a25d6a89d/pyyaml-6.0.3-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:418cf3f2111bc80e0933b2cd8cd04f286338bb88bdc7bc8e6dd775ebde60b5e0", size = 732872, upload-time = "2025-09-25T21:31:53.282Z" }, + { url = "https://files.pythonhosted.org/packages/1f/15/2bc9c8faf6450a8b3c9fc5448ed869c599c0a74ba2669772b1f3a0040180/pyyaml-6.0.3-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:5e0b74767e5f8c593e8c9b5912019159ed0533c70051e9cce3e8b6aa699fcd69", size = 758828, upload-time = "2025-09-25T21:31:54.807Z" }, + { url = "https://files.pythonhosted.org/packages/a3/00/531e92e88c00f4333ce359e50c19b8d1de9fe8d581b1534e35ccfbc5f393/pyyaml-6.0.3-cp310-cp310-win32.whl", hash = "sha256:28c8d926f98f432f88adc23edf2e6d4921ac26fb084b028c733d01868d19007e", size = 142415, upload-time = "2025-09-25T21:31:55.885Z" }, + { url = "https://files.pythonhosted.org/packages/2a/fa/926c003379b19fca39dd4634818b00dec6c62d87faf628d1394e137354d4/pyyaml-6.0.3-cp310-cp310-win_amd64.whl", hash = "sha256:bdb2c67c6c1390b63c6ff89f210c8fd09d9a1217a465701eac7316313c915e4c", size = 158561, upload-time = "2025-09-25T21:31:57.406Z" }, +] + +[[package]] +name = "scipy" +version = "1.15.3" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "numpy" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/0f/37/6964b830433e654ec7485e45a00fc9a27cf868d622838f6b6d9c5ec0d532/scipy-1.15.3.tar.gz", hash = "sha256:eae3cf522bc7df64b42cad3925c876e1b0b6c35c1337c93e12c0f366f55b0eaf", size = 59419214, upload-time = "2025-05-08T16:13:05.955Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/78/2f/4966032c5f8cc7e6a60f1b2e0ad686293b9474b65246b0c642e3ef3badd0/scipy-1.15.3-cp310-cp310-macosx_10_13_x86_64.whl", hash = "sha256:a345928c86d535060c9c2b25e71e87c39ab2f22fc96e9636bd74d1dbf9de448c", size = 38702770, upload-time = "2025-05-08T16:04:20.849Z" }, + { url = "https://files.pythonhosted.org/packages/a0/6e/0c3bf90fae0e910c274db43304ebe25a6b391327f3f10b5dcc638c090795/scipy-1.15.3-cp310-cp310-macosx_12_0_arm64.whl", hash = "sha256:ad3432cb0f9ed87477a8d97f03b763fd1d57709f1bbde3c9369b1dff5503b253", size = 30094511, upload-time = "2025-05-08T16:04:27.103Z" }, + { url = "https://files.pythonhosted.org/packages/ea/b1/4deb37252311c1acff7f101f6453f0440794f51b6eacb1aad4459a134081/scipy-1.15.3-cp310-cp310-macosx_14_0_arm64.whl", hash = "sha256:aef683a9ae6eb00728a542b796f52a5477b78252edede72b8327a886ab63293f", size = 22368151, upload-time = "2025-05-08T16:04:31.731Z" }, + { url = "https://files.pythonhosted.org/packages/38/7d/f457626e3cd3c29b3a49ca115a304cebb8cc6f31b04678f03b216899d3c6/scipy-1.15.3-cp310-cp310-macosx_14_0_x86_64.whl", hash = "sha256:1c832e1bd78dea67d5c16f786681b28dd695a8cb1fb90af2e27580d3d0967e92", size = 25121732, upload-time = "2025-05-08T16:04:36.596Z" }, + { url = "https://files.pythonhosted.org/packages/db/0a/92b1de4a7adc7a15dcf5bddc6e191f6f29ee663b30511ce20467ef9b82e4/scipy-1.15.3-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:263961f658ce2165bbd7b99fa5135195c3a12d9bef045345016b8b50c315cb82", size = 35547617, upload-time = "2025-05-08T16:04:43.546Z" }, + { url = "https://files.pythonhosted.org/packages/8e/6d/41991e503e51fc1134502694c5fa7a1671501a17ffa12716a4a9151af3df/scipy-1.15.3-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:9e2abc762b0811e09a0d3258abee2d98e0c703eee49464ce0069590846f31d40", size = 37662964, upload-time = "2025-05-08T16:04:49.431Z" }, + { url = "https://files.pythonhosted.org/packages/25/e1/3df8f83cb15f3500478c889be8fb18700813b95e9e087328230b98d547ff/scipy-1.15.3-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:ed7284b21a7a0c8f1b6e5977ac05396c0d008b89e05498c8b7e8f4a1423bba0e", size = 37238749, upload-time = "2025-05-08T16:04:55.215Z" }, + { url = "https://files.pythonhosted.org/packages/93/3e/b3257cf446f2a3533ed7809757039016b74cd6f38271de91682aa844cfc5/scipy-1.15.3-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:5380741e53df2c566f4d234b100a484b420af85deb39ea35a1cc1be84ff53a5c", size = 40022383, upload-time = "2025-05-08T16:05:01.914Z" }, + { url = "https://files.pythonhosted.org/packages/d1/84/55bc4881973d3f79b479a5a2e2df61c8c9a04fcb986a213ac9c02cfb659b/scipy-1.15.3-cp310-cp310-win_amd64.whl", hash = "sha256:9d61e97b186a57350f6d6fd72640f9e99d5a4a2b8fbf4b9ee9a841eab327dc13", size = 41259201, upload-time = "2025-05-08T16:05:08.166Z" }, +] + +[[package]] +name = "six" +version = "1.17.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/94/e7/b2c673351809dca68a0e064b6af791aa332cf192da575fd474ed7d6f16a2/six-1.17.0.tar.gz", hash = "sha256:ff70335d468e7eb6ec65b95b99d3a2836546063f63acc5171de367e834932a81", size = 34031, upload-time = "2024-12-04T17:35:28.174Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/b7/ce/149a00dd41f10bc29e5921b496af8b574d8413afcd5e30dfa0ed46c2cc5e/six-1.17.0-py2.py3-none-any.whl", hash = "sha256:4721f391ed90541fddacab5acf947aa0d3dc7d27b2e1e8eda2be8970586c3274", size = 11050, upload-time = "2024-12-04T17:35:26.475Z" }, +] + +[[package]] +name = "tomli" +version = "2.4.1" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/22/de/48c59722572767841493b26183a0d1cc411d54fd759c5607c4590b6563a6/tomli-2.4.1.tar.gz", hash = "sha256:7c7e1a961a0b2f2472c1ac5b69affa0ae1132c39adcb67aba98568702b9cc23f", size = 17543, upload-time = "2026-03-25T20:22:03.828Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/7b/61/cceae43728b7de99d9b847560c262873a1f6c98202171fd5ed62640b494b/tomli-2.4.1-py3-none-any.whl", hash = "sha256:0d85819802132122da43cb86656f8d1f8c6587d54ae7dcaf30e90533028b49fe", size = 14583, upload-time = "2026-03-25T20:22:03.012Z" }, +] + +[[package]] +name = "typing-extensions" +version = "4.16.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/f6/cc/6253133b5bb138fc3306cebfbda2c520f545d36b5be2c7255cc528bb45d6/typing_extensions-4.16.0.tar.gz", hash = "sha256:dc983d19a509c94dba722ee6abd33940f7c05a89e243c47e907eb4db6f1a43e5", size = 113555, upload-time = "2026-07-02T08:40:05.92Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/49/d3/b8441a820a491ddfc024b0b0cf0393375b75ea13866d9c66727e54c2fc80/typing_extensions-4.16.0-py3-none-any.whl", hash = "sha256:481caa481374e813c1b176ada14e97f1f67a4539ce9cfeb3f350d78d6370c2e8", size = 45571, upload-time = "2026-07-02T08:40:04.659Z" }, +] + +[[package]] +name = "zipp" +version = "4.1.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/b9/d8/eab98a517c14134c0b2eb4e2387bc5f457334293ec5d2dd3857ec2966802/zipp-4.1.0.tar.gz", hash = "sha256:4cb57381f544315db7688e976e922a2b18cdb513d21cc194eb42232ba2a3e602", size = 26214, upload-time = "2026-05-18T20:08:57.967Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/3a/13/547360d81e6d88d58492968ffda9f9542854f11310ee556fef14260cc886/zipp-4.1.0-py3-none-any.whl", hash = "sha256:25ad4e16390cd314347dd8f1de67a2ac538ae658ed4ab9db16029c07c188e97f", size = 10238, upload-time = "2026-05-18T20:08:57.045Z" }, +]