From 59e39dd04f1a15f6adc8a75896ead51f5c729152 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 5 Sep 2026 00:49:58 +0800 Subject: [PATCH] feat(toolkits): add dynamics calibration workflow --- ...odichain.toolkits.dynamics_calibration.rst | 64 ++ .../embodichain/embodichain.toolkits.rst | 10 +- .../features/toolkits/dynamics_calibration.md | 166 +++++ docs/source/features/toolkits/index.rst | 8 + docs/source/guides/cli.md | 21 + embodichain/cli/main.py | 5 + embodichain/lab/sim/objects/articulation.py | 19 +- .../toolkits/dynamics_calibration/__init__.py | 74 ++ .../toolkits/dynamics_calibration/__main__.py | 26 + .../dynamics_calibration/asset_audit.py | 76 ++ .../toolkits/dynamics_calibration/cli.py | 198 ++++++ .../dynamics_calibration/evaluator.py | 239 +++++++ .../toolkits/dynamics_calibration/metrics.py | 657 ++++++++++++++++++ .../toolkits/dynamics_calibration/overlay.py | 118 ++++ .../toolkits/dynamics_calibration/report.py | 253 +++++++ .../toolkits/dynamics_calibration/schema.py | 580 ++++++++++++++++ .../tracking_evaluator.py | 245 +++++++ .../toolkits/dynamics_calibration/tuning.py | 232 +++++++ .../toolkits/dynamics_calibration/worker.py | 99 +++ tests/sim/objects/test_articulation.py | 25 + tests/test_main.py | 16 + .../assets/dynamics_calibration_pendulum.urdf | 36 + tests/toolkits/test_dynamics_calibration.py | 514 ++++++++++++++ 23 files changed, 3679 insertions(+), 2 deletions(-) create mode 100644 docs/source/api_reference/embodichain/embodichain.toolkits.dynamics_calibration.rst create mode 100644 docs/source/features/toolkits/dynamics_calibration.md create mode 100644 embodichain/toolkits/dynamics_calibration/__init__.py create mode 100644 embodichain/toolkits/dynamics_calibration/__main__.py create mode 100644 embodichain/toolkits/dynamics_calibration/asset_audit.py create mode 100644 embodichain/toolkits/dynamics_calibration/cli.py create mode 100644 embodichain/toolkits/dynamics_calibration/evaluator.py create mode 100644 embodichain/toolkits/dynamics_calibration/metrics.py create mode 100644 embodichain/toolkits/dynamics_calibration/overlay.py create mode 100644 embodichain/toolkits/dynamics_calibration/report.py create mode 100644 embodichain/toolkits/dynamics_calibration/schema.py create mode 100644 embodichain/toolkits/dynamics_calibration/tracking_evaluator.py create mode 100644 embodichain/toolkits/dynamics_calibration/tuning.py create mode 100644 embodichain/toolkits/dynamics_calibration/worker.py create mode 100644 tests/toolkits/assets/dynamics_calibration_pendulum.urdf create mode 100644 tests/toolkits/test_dynamics_calibration.py diff --git a/docs/source/api_reference/embodichain/embodichain.toolkits.dynamics_calibration.rst b/docs/source/api_reference/embodichain/embodichain.toolkits.dynamics_calibration.rst new file mode 100644 index 000000000..562625648 --- /dev/null +++ b/docs/source/api_reference/embodichain/embodichain.toolkits.dynamics_calibration.rst @@ -0,0 +1,64 @@ +embodichain.toolkits.dynamics_calibration +========================================= + +The dynamics-calibration package provides application-level effective drive +search, isolated evaluation, qualification gates, overlays, and reports. It +uses DexSim's SimReady API for asset facts and does not perform physical +parameter identification. + +.. automodule:: embodichain.toolkits.dynamics_calibration + :members: + :undoc-members: + :show-inheritance: + +Configuration +------------- + +.. automodule:: embodichain.toolkits.dynamics_calibration.schema + :members: + :undoc-members: + :show-inheritance: + +Metrics and qualification +------------------------- + +.. automodule:: embodichain.toolkits.dynamics_calibration.metrics + :members: + :undoc-members: + :show-inheritance: + +Candidate evaluation and tuning +------------------------------- + +.. automodule:: embodichain.toolkits.dynamics_calibration.evaluator + :members: + :undoc-members: + :show-inheritance: + +.. automodule:: embodichain.toolkits.dynamics_calibration.tuning + :members: + :undoc-members: + :show-inheritance: + +.. automodule:: embodichain.toolkits.dynamics_calibration.tracking_evaluator + :members: + :undoc-members: + +Artifacts and orchestration +--------------------------- + +.. automodule:: embodichain.toolkits.dynamics_calibration.overlay + :members: + :undoc-members: + +.. automodule:: embodichain.toolkits.dynamics_calibration.asset_audit + :members: + :undoc-members: + +.. automodule:: embodichain.toolkits.dynamics_calibration.report + :members: + :undoc-members: + +.. automodule:: embodichain.toolkits.dynamics_calibration.cli + :members: + :undoc-members: diff --git a/docs/source/api_reference/embodichain/embodichain.toolkits.rst b/docs/source/api_reference/embodichain/embodichain.toolkits.rst index e412f66a4..f45052d6c 100644 --- a/docs/source/api_reference/embodichain/embodichain.toolkits.rst +++ b/docs/source/api_reference/embodichain/embodichain.toolkits.rst @@ -2,7 +2,8 @@ embodichain.toolkits ==================== The :mod:`embodichain.toolkits` package contains asset-preparation and -manipulation utilities that can be used independently of the simulation loop. +manipulation utilities plus an isolated application-level dynamics-calibration +workflow. .. automodule:: embodichain.toolkits @@ -11,9 +12,16 @@ manipulation utilities that can be used independently of the simulation loop. .. autosummary:: acd + dynamics_calibration graspkit urdf_assembly +.. toctree:: + :maxdepth: 1 + :hidden: + + embodichain.toolkits.dynamics_calibration + GraspKit — Parallel-Gripper Grasp Sampling ------------------------------------------- diff --git a/docs/source/features/toolkits/dynamics_calibration.md b/docs/source/features/toolkits/dynamics_calibration.md new file mode 100644 index 000000000..d40c0c68a --- /dev/null +++ b/docs/source/features/toolkits/dynamics_calibration.md @@ -0,0 +1,166 @@ +# Dynamics Calibration + +The dynamics-calibration toolkit tunes effective robot drive properties for a +specific EmbodiChain application. It produces a reviewable YAML overlay and +qualification evidence; it never rewrites the source URDF. + +This V1 intentionally does **not** claim physical parameter identification. +Without torque/current or equivalent real-system evidence, mass, center of +mass, inertia, friction, stiffness, and damping are not uniquely identifiable +from position tracking alone. + +## Workflow and ownership + +The workflow has three commands: + +1. `audit` delegates generic URDF and inertia checks to `dexsim.simready`. +2. `tune-drive` runs each candidate in a fresh process, ranks candidates on + training trajectories, then checks the winner on a held-out trajectory. +3. `qualify` rechecks an existing overlay against the current asset hashes and + held-out gates. + +DexSim owns simulation-readiness facts. EmbodiChain owns application +trajectories, control-group selection, drive-parameter search, qualification +policy, and report assembly. An error-level DexSim finding blocks tuning; +warnings remain visible in the final report but permit application evaluation. + +## Configuration + +Save a YAML file such as `calibration.yaml`: + +```yaml +schema_version: 1 +assets: + - /absolute/path/to/robot.urdf +backend: physx +device: cpu +physics_dt: 0.004166666666666667 # 240 Hz +control_frequency_hz: 60 +seed: 7 +candidate_count: 9 + +evaluator: + target: embodichain.toolkits.dynamics_calibration.tracking_evaluator:evaluate + timeout_seconds: 120 + payload: + control_part: arm + robot_cfg: + control_parts: + arm: [joint1, joint2, joint3] + training_trajectory: + duration_seconds: 3 + warmup_seconds: 0.5 + amplitude: [0.10, 0.08, 0.06] + frequencies_hz: [0.25, 0.35, 0.45] + qualification_trajectory: + duration_seconds: 4 + warmup_seconds: 0.5 + amplitude: [0.07, 0.11, 0.09] + frequencies_hz: [0.30, 0.40, 0.55] + +parameters: + - name: arm_stiffness + field: stiffness + selector: arm + lower: 100 + upper: 20000 + initial: 5000 + scale: log + - name: arm_damping + field: damping + selector: arm + lower: 10 + upper: 2000 + initial: 500 + scale: log + +qualification: + aggregate_rmse_max: 0.05 + per_joint_rmse_max: 0.08 + per_control_group_rmse_max: 0.06 + cvar95_max: 0.12 + # Custom step-response evaluators can also gate overshoot_max and + # settling_time_seconds_max when they return those metrics. + saturation_fraction_max: 0.02 + velocity_saturation_fraction_max: 0.02 + joint_limit_violation_max: 0 + control_frequency_relative_error_max: 0 + expected_target_qvel_write_count: 0 + require_stable: true +``` + +Parameter `selector` values use the same exact-name, regular-expression, and +Robot control-part resolution as `RobotCfg.drive_pros`. The built-in evaluator +only writes qpos targets. Its qvel write count comes from public +`Articulation.set_qvel` instrumentation and counts successful batched target +write calls; it is not inferred from private engine state. + +The requested control frequency must map to an integral number of physics +updates. Set `allow_approximate_control_frequency: true` only when a changed +actual rate is acceptable and covered by the configured frequency-error gate. +The default qpos-only policy also requires `target_qvel_write_count` evidence; +an evaluator that omits it fails that gate rather than being assumed to have +written zero velocity targets. Set `expected_target_qvel_write_count: null` +only when that instrumentation is intentionally unavailable. + +## Commands + +```bash +# Inspect one or more URDFs without changing them. +embodichain calibrate-dynamics audit robot.urdf --output-dir audit_output + +# Audit, search, write the overlay, and run held-out qualification. +embodichain calibrate-dynamics tune-drive \ + --config calibration.yaml \ + --output-dir calibration_output + +# Re-qualify a previously generated overlay. +embodichain calibrate-dynamics qualify \ + --config calibration.yaml \ + --overlay calibration_output/drive_overlay.yaml \ + --output-dir qualification_output +``` + +An audit failure, worker exception, timeout, stale asset hash, or qualification +failure exits nonzero. Candidate results are content-addressed by the assets, +overlay, evaluator, runtime, backend, timestep, control schedule, seed, and +phase. + +## Artifacts + +`tune-drive` writes: + +- `drive_overlay.yaml`: the selected `drive_pros` values and exact asset hashes; +- `report.json`: all candidates, raw metrics, hard-gate decisions, versions, + timing, device, backend, and cache provenance; +- `report.md`: a compact human review; +- `cache/`: reusable isolated-candidate results. + +The report claim is always `effective_drive_tuning`. Confidence intervals and +domain-randomization ranges are marked as not estimated in V1; those require a +separate physical-identification workflow and suitable measurements. + +## Custom application evaluator + +Set `evaluator.target` to `module:function` or `/path/to/file.py:function`. +The callable receives `(overlay, context)` and returns a dictionary containing: + +```python +{ + "joint_names": ["joint1", "joint2"], + "target_qpos": [[0.0, 0.0], [0.1, -0.1]], + "actual_qpos": [[0.0, 0.0], [0.08, -0.12]], + "requested_control_hz": 60.0, + "actual_control_hz": 60.0, + "target_qvel_write_count": 0, + # Optional: control_groups, effort/effort_limits, qvel/qvel_limits, + # qpos_lower/qpos_upper, overshoot, settling_time_seconds, stable, + # and JSON-serializable metadata. +} +``` + +Tracking-error, saturation, limit metrics, and all hard gates remain centralized +in the toolkit. A custom step-response evaluator may additionally supply +application-defined `overshoot` and `settling_time_seconds` observations; the +toolkit validates them as finite non-negative values and applies the configured +gates without transforming them. diff --git a/docs/source/features/toolkits/index.rst b/docs/source/features/toolkits/index.rst index c8d50e845..9a9f1300c 100644 --- a/docs/source/features/toolkits/index.rst +++ b/docs/source/features/toolkits/index.rst @@ -28,6 +28,10 @@ Available Toolkits - Samples antipodal contacts, constructs grasp poses, and filters collisions for parallel-jaw grippers. - Annotate graspable regions and generate candidate grasps for manipulation. + * - :doc:`Dynamics Calibration ` + - Audits robot assets, tunes effective drive parameters in isolated runs, + and applies hard gates on held-out trajectories. + - Produce a reviewable drive overlay for one application and backend. Choosing a Toolkit ------------------ @@ -38,6 +42,9 @@ when the robot is distributed across multiple component files. Use grasp generation after the target object's mesh is ready and a manipulation workflow needs feasible end-effector poses. +Use dynamics calibration after a robot asset passes generic SimReady checks and +an application needs reproducible stiffness, damping, armature, or limit tuning. + The asset tools can be chained: preprocess component collision meshes, assemble the components into one URDF, and then load the resulting robot in a grasping task. @@ -48,3 +55,4 @@ task. URDF Convex Decomposition URDF Assembly Parallel-Gripper Grasp Generation + Dynamics Calibration diff --git a/docs/source/guides/cli.md b/docs/source/guides/cli.md index 891483cb2..83f57167b 100644 --- a/docs/source/guides/cli.md +++ b/docs/source/guides/cli.md @@ -59,6 +59,27 @@ The generated output contains the canonical source mesh under ``asset_source/``, --- +## Dynamics Calibration + +Audit a robot description and tune effective drive properties for an +application without changing its source asset: + +```bash +embodichain calibrate-dynamics audit robot.urdf +embodichain calibrate-dynamics tune-drive \ + --config calibration.yaml \ + --output-dir calibration_output +embodichain calibrate-dynamics qualify \ + --config calibration.yaml \ + --overlay calibration_output/drive_overlay.yaml +``` + +The tool uses DexSim for generic SimReady facts and EmbodiChain for trajectories, +search, and qualification. See {doc}`../features/toolkits/dynamics_calibration` +for the configuration schema, custom evaluator contract, artifacts, and scope. + +--- + (cli-preview-asset)= ## Preview Asset diff --git a/embodichain/cli/main.py b/embodichain/cli/main.py index f92e1461e..870de0b70 100644 --- a/embodichain/cli/main.py +++ b/embodichain/cli/main.py @@ -94,6 +94,11 @@ class Command: target="embodichain.toolkits.acd.cli:main", help="Generate convex collision meshes for a URDF.", ), + Command( + name="calibrate-dynamics", + target="embodichain.toolkits.dynamics_calibration.cli:main", + help="Audit, tune, and qualify effective robot drive parameters.", + ), Command( name="benchmark", target="scripts.benchmark.__main__:main", diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index d92825163..4124fec17 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -650,6 +650,7 @@ def __init__( self.cfg = cfg self._entities = entities self.device = device + self._target_qvel_write_count = 0 # Store all indices for batch operations self._all_indices = torch.arange(len(entities), dtype=torch.int32) @@ -1504,6 +1505,19 @@ def get_qvel(self, target: bool = False) -> torch.Tensor: """ return self.body_data.qvel if not target else self.body_data.target_qvel + @property + def target_qvel_write_count(self) -> int: + """Number of successful target-velocity API write calls on this handle. + + The counter is application-facing instrumentation. One batched + :meth:`set_qvel` invocation counts once, independent of the selected + environments or backend implementation. + + Returns: + Successful target-velocity calls since this handle was created. + """ + return getattr(self, "_target_qvel_write_count", 0) + def get_qvel_limits( self, joint_ids: Sequence[int] | torch.Tensor | None = None, @@ -1536,7 +1550,8 @@ def set_qvel( qvel (torch.Tensor): The velocities with shape (N, dof). joint_ids (Sequence[int] | None, optional): Joint indices to apply the velocities. If None, applies to all joints. env_ids (Sequence[int] | None, optional): Environment indices. Defaults to all indices. - If True, sets target positions for simulation. If False, updates current positions directly. + target (bool): If True, sets target velocities for simulation. If + False, updates current velocities directly. Raises: ValueError: If the length of `env_ids` does not match the length of `qvel`. @@ -1591,6 +1606,8 @@ def set_qvel( gpu_indices=indices, data_type=data_type, ) + if target: + self._target_qvel_write_count = self.target_qvel_write_count + 1 def set_qf( self, diff --git a/embodichain/toolkits/dynamics_calibration/__init__.py b/embodichain/toolkits/dynamics_calibration/__init__.py new file mode 100644 index 000000000..19670c19d --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/__init__.py @@ -0,0 +1,74 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Application-level effective-drive calibration and qualification.""" + +from __future__ import annotations + +from .asset_audit import audit_assets, audits_admit_calibration +from .evaluator import CandidateEvaluation, EvaluationError, run_candidate +from .metrics import ( + QualificationGate, + QualificationResult, + TrackingMetrics, + compute_tracking_metrics, + qualify, +) +from .overlay import build_drive_overlay, load_overlay, write_overlay +from .report import ( + build_calibration_report, + calibration_report_to_markdown, + write_calibration_reports, +) +from .schema import ( + CalibrationConfig, + ControlSchedule, + DriveParameterSpec, + EvaluatorConfig, + QualificationThresholds, + load_calibration_config, + resolve_control_schedule, +) +from .tuning import TuningResult, TuningTrial, tune_drive + +__all__ = [ + "CalibrationConfig", + "CandidateEvaluation", + "ControlSchedule", + "DriveParameterSpec", + "EvaluationError", + "EvaluatorConfig", + "QualificationGate", + "QualificationResult", + "QualificationThresholds", + "TrackingMetrics", + "TuningResult", + "TuningTrial", + "audit_assets", + "audits_admit_calibration", + "build_drive_overlay", + "build_calibration_report", + "calibration_report_to_markdown", + "compute_tracking_metrics", + "load_overlay", + "load_calibration_config", + "qualify", + "resolve_control_schedule", + "run_candidate", + "tune_drive", + "write_overlay", + "write_calibration_reports", +] diff --git a/embodichain/toolkits/dynamics_calibration/__main__.py b/embodichain/toolkits/dynamics_calibration/__main__.py new file mode 100644 index 000000000..07d388561 --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/__main__.py @@ -0,0 +1,26 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Execute ``embodichain calibrate-dynamics`` as a Python module.""" + +from __future__ import annotations + +from .cli import main + +main() + + +__all__: list[str] = [] diff --git a/embodichain/toolkits/dynamics_calibration/asset_audit.py b/embodichain/toolkits/dynamics_calibration/asset_audit.py new file mode 100644 index 000000000..2f833c4fe --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/asset_audit.py @@ -0,0 +1,76 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""EmbodiChain admission policy over DexSim's SimReady asset facts.""" + +from __future__ import annotations + +from collections.abc import Iterable +from pathlib import Path +from typing import Any + + +def audit_assets( + assets: Iterable[str | Path], *, reference_links: Iterable[str] = () +) -> tuple[Any, ...]: + """Audit URDF inputs through DexSim without duplicating engine semantics. + + Args: + assets: Robot asset paths to audit. + reference_links: Exact link names intentionally allowed to omit + inertial properties. + + Returns: + DexSim SimReady reports in the same order as ``assets``. + + Raises: + RuntimeError: If the installed DexSim does not expose SimReady auditing. + ValueError: If an asset format has no V1 audit implementation. + """ + try: + from dexsim.simready import audit_urdf + except ImportError as error: + raise RuntimeError( + "the installed DexSim does not provide dexsim.simready; install the " + "matching DexSim SimReady release" + ) from error + + reports = [] + for raw_asset in assets: + asset = Path(raw_asset).expanduser().resolve() + if asset.suffix.lower() != ".urdf": + raise ValueError( + f"V1 SimReady audit supports URDF assets only, received: {asset}" + ) + reports.append(audit_urdf(asset, reference_links=reference_links)) + return tuple(reports) + + +def audits_admit_calibration(reports: Iterable[Any]) -> bool: + """Return whether every DexSim report has no error-level diagnostics. + + Args: + reports: SimReady reports to evaluate under EmbodiChain's admission + policy. + + Returns: + ``True`` when at least one report is present and every report is ready. + """ + materialized = tuple(reports) + return bool(materialized) and all(bool(report.ready) for report in materialized) + + +__all__ = ["audit_assets", "audits_admit_calibration"] diff --git a/embodichain/toolkits/dynamics_calibration/cli.py b/embodichain/toolkits/dynamics_calibration/cli.py new file mode 100644 index 000000000..4b773d51f --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/cli.py @@ -0,0 +1,198 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Command-line workflow for effective dynamics calibration.""" + +from __future__ import annotations + +import argparse +import json +import sys +from collections.abc import Sequence +from pathlib import Path + +from .asset_audit import audit_assets, audits_admit_calibration +from .evaluator import EvaluationError, run_candidate +from .metrics import qualify +from .overlay import load_overlay, write_overlay +from .report import build_calibration_report, write_calibration_reports +from .schema import load_calibration_config +from .tuning import tune_drive + + +def build_parser() -> argparse.ArgumentParser: + """Build the dynamics-calibration command parser. + + Returns: + Parser for the ``audit``, ``tune-drive``, and ``qualify`` commands. + """ + parser = argparse.ArgumentParser( + prog="embodichain calibrate-dynamics", + description=( + "Audit robot assets, tune effective drive properties, and qualify " + "them on held-out application trajectories." + ), + ) + subparsers = parser.add_subparsers(dest="command", required=True) + + audit_parser = subparsers.add_parser( + "audit", help="Run DexSim SimReady checks without modifying assets." + ) + audit_parser.add_argument("assets", nargs="+", type=Path) + audit_parser.add_argument("--reference-link", action="append", default=[]) + audit_parser.add_argument("--output-dir", type=Path) + + tune_parser = subparsers.add_parser( + "tune-drive", help="Search effective drive parameters, then qualify the best." + ) + _add_config_arguments(tune_parser) + + qualify_parser = subparsers.add_parser( + "qualify", help="Evaluate one existing overlay on held-out conditions." + ) + _add_config_arguments(qualify_parser) + qualify_parser.add_argument("--overlay", required=True, type=Path) + return parser + + +def main(argv: Sequence[str] | None = None) -> None: + """Run a calibration command and preserve nonzero failure semantics. + + Args: + argv: Command arguments excluding the executable name. Uses process + arguments when omitted. + + Raises: + SystemExit: With status 2 when auditing, evaluation, or qualification + fails. + """ + args = build_parser().parse_args(argv) + try: + if args.command == "audit": + _run_audit(args) + elif args.command == "tune-drive": + _run_tune(args) + else: + _run_qualify(args) + except ( + EvaluationError, + FileNotFoundError, + RuntimeError, + TypeError, + ValueError, + ) as error: + print(f"calibrate-dynamics: error: {error}", file=sys.stderr) + raise SystemExit(2) from error + + +def _add_config_arguments(parser: argparse.ArgumentParser) -> None: + parser.add_argument("--config", required=True, type=Path) + parser.add_argument( + "--output-dir", type=Path, default=Path("dynamics_calibration_output") + ) + parser.add_argument("--cache-dir", type=Path) + parser.add_argument("--reference-link", action="append", default=[]) + + +def _run_audit(args: argparse.Namespace) -> None: + reports = audit_assets(args.assets, reference_links=args.reference_link) + payload = { + "schema_version": 1, + "kind": "embodichain.dynamics_calibration.asset_audits", + "status": ( + "pass" + if all(report.status == "pass" for report in reports) + else "review" if audits_admit_calibration(reports) else "fail" + ), + "reports": [report.to_dict() for report in reports], + } + if args.output_dir is None: + print(json.dumps(payload, indent=2, sort_keys=True)) + else: + args.output_dir.mkdir(parents=True, exist_ok=True) + (args.output_dir / "audit.json").write_text( + json.dumps(payload, indent=2, sort_keys=True) + "\n", encoding="utf-8" + ) + (args.output_dir / "audit.md").write_text( + "\n".join(report.to_markdown().rstrip() for report in reports) + "\n", + encoding="utf-8", + ) + if not audits_admit_calibration(reports): + raise SystemExit(2) + + +def _run_tune(args: argparse.Namespace) -> None: + config = load_calibration_config(args.config) + reports = audit_assets(config.assets, reference_links=args.reference_link) + if not audits_admit_calibration(reports): + report = build_calibration_report(config, audits=reports) + write_calibration_reports(args.output_dir, report) + raise SystemExit(2) + cache_dir = args.cache_dir or args.output_dir / "cache" + tuning = tune_drive(config, cache_dir=cache_dir) + args.output_dir.mkdir(parents=True, exist_ok=True) + write_overlay(args.output_dir / "drive_overlay.yaml", tuning.overlay) + held_out = run_candidate( + config.evaluator, + tuning.overlay, + config.evaluation_context("qualification"), + cache_dir=cache_dir, + ) + qualification = qualify(held_out.metrics, config.qualification) + report = build_calibration_report( + config, + audits=reports, + tuning=tuning, + qualification_evaluation=held_out, + qualification=qualification, + ) + json_path, markdown_path = write_calibration_reports(args.output_dir, report) + print(f"Wrote {json_path} and {markdown_path}") + if qualification.status != "pass": + raise SystemExit(2) + + +def _run_qualify(args: argparse.Namespace) -> None: + config = load_calibration_config(args.config) + reports = audit_assets(config.assets, reference_links=args.reference_link) + if not audits_admit_calibration(reports): + report = build_calibration_report(config, audits=reports) + write_calibration_reports(args.output_dir, report) + raise SystemExit(2) + overlay = load_overlay(args.overlay) + if overlay.get("assets") != config.asset_records(): + raise ValueError("overlay asset hashes do not match the current configuration") + cache_dir = args.cache_dir or args.output_dir / "cache" + held_out = run_candidate( + config.evaluator, + overlay, + config.evaluation_context("qualification"), + cache_dir=cache_dir, + ) + qualification = qualify(held_out.metrics, config.qualification) + report = build_calibration_report( + config, + audits=reports, + qualification_evaluation=held_out, + qualification=qualification, + ) + json_path, markdown_path = write_calibration_reports(args.output_dir, report) + print(f"Wrote {json_path} and {markdown_path}") + if qualification.status != "pass": + raise SystemExit(2) + + +__all__ = ["build_parser", "main"] diff --git a/embodichain/toolkits/dynamics_calibration/evaluator.py b/embodichain/toolkits/dynamics_calibration/evaluator.py new file mode 100644 index 000000000..f5730367d --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/evaluator.py @@ -0,0 +1,239 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Isolated candidate evaluation with content-addressed caching.""" + +from __future__ import annotations + +import hashlib +import importlib.metadata +import json +import os +import subprocess +import sys +import tempfile +from collections.abc import Mapping +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +from .metrics import TrackingMetrics, compute_tracking_metrics +from .schema import EvaluatorConfig + + +class EvaluationError(RuntimeError): + """Raised when an isolated evaluator does not produce valid evidence.""" + + +@dataclass(frozen=True) +class CandidateEvaluation: + """Metrics and cache provenance for one candidate. + + Attributes: + metrics: Centralized metrics computed from evaluator observations. + cache_hit: Whether the result came from an existing cache entry. + cache_key: Content-addressed identity for the evaluation inputs. + metadata: Strict-JSON metadata supplied by the application evaluator. + """ + + metrics: TrackingMetrics + cache_hit: bool + cache_key: str + metadata: dict[str, Any] + + +def run_candidate( + evaluator: EvaluatorConfig, + overlay: Mapping[str, Any], + context: Mapping[str, Any], + *, + cache_dir: str | Path, +) -> CandidateEvaluation: + """Run one evaluator in a fresh process, or restore its cached result. + + Args: + evaluator: Isolated evaluator target, timeout, and application payload. + overlay: Candidate drive overlay passed to the evaluator. + context: Reproducible backend, timing, asset, seed, and phase context. + cache_dir: Directory containing content-addressed result entries. + + Returns: + Metrics, evaluator metadata, and cache provenance for the candidate. + + Raises: + EvaluationError: If the worker times out, exits unsuccessfully, or + returns invalid observations or metadata. + TypeError: If inputs cannot be encoded as strict JSON. + ValueError: If inputs contain non-finite JSON numbers. + """ + canonical_input = { + "schema_version": 1, + "evaluator": { + "target": evaluator.target, + "fingerprint": _evaluator_fingerprint(evaluator.target), + "timeout_seconds": evaluator.timeout_seconds, + "payload": evaluator.payload, + }, + "runtime": _runtime_fingerprint(), + "overlay": dict(overlay), + "context": dict(context), + } + encoded = json.dumps( + canonical_input, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode("utf-8") + cache_key = hashlib.sha256(encoded).hexdigest() + resolved_cache_dir = Path(cache_dir) + resolved_cache_dir.mkdir(parents=True, exist_ok=True) + cache_path = resolved_cache_dir / f"{cache_key}.json" + if cache_path.is_file(): + try: + cached = json.loads(cache_path.read_text(encoding="utf-8")) + return CandidateEvaluation( + TrackingMetrics.from_dict(cached["metrics"]), + True, + cache_key, + dict(cached.get("metadata", {})), + ) + except (KeyError, TypeError, ValueError, json.JSONDecodeError): + # A partial or incompatible cache entry is recomputed and replaced. + pass + + with tempfile.TemporaryDirectory(prefix="embodichain-calibration-") as temp_dir: + temp_root = Path(temp_dir) + input_path = temp_root / "input.json" + output_path = temp_root / "output.json" + input_path.write_bytes(encoded) + command = [ + sys.executable, + "-m", + "embodichain.toolkits.dynamics_calibration.worker", + str(input_path), + str(output_path), + ] + try: + completed = subprocess.run( + command, + check=False, + capture_output=True, + text=True, + timeout=evaluator.timeout_seconds, + ) + except subprocess.TimeoutExpired as error: + raise EvaluationError( + f"evaluator timed out after {evaluator.timeout_seconds:g} seconds" + ) from error + + payload: dict[str, Any] = {} + if output_path.is_file(): + try: + payload = json.loads(output_path.read_text(encoding="utf-8")) + except json.JSONDecodeError: + payload = {} + if completed.returncode != 0 or payload.get("status") != "ok": + detail = payload.get("error") + if not detail: + detail = completed.stderr.strip() or completed.stdout.strip() + if not detail: + detail = f"worker exited with status {completed.returncode}" + raise EvaluationError(str(detail)) + raw_result = payload.get("result") + if not isinstance(raw_result, Mapping): + raise EvaluationError("evaluator result must be a mapping") + try: + metrics = compute_tracking_metrics(raw_result) + except (KeyError, TypeError, ValueError) as error: + raise EvaluationError(f"invalid evaluator result: {error}") from error + metadata = raw_result.get("metadata", {}) + if not isinstance(metadata, Mapping): + raise EvaluationError("evaluator metadata must be a mapping") + metadata = dict(metadata) + try: + json.dumps(metadata, allow_nan=False) + except (TypeError, ValueError) as error: + raise EvaluationError( + f"evaluator metadata is not strict JSON: {error}" + ) from error + + cache_payload = { + "schema_version": 1, + "metrics": metrics.to_dict(), + "metadata": metadata, + } + with tempfile.NamedTemporaryFile( + mode="w", + encoding="utf-8", + dir=resolved_cache_dir, + prefix=f".{cache_key}.", + suffix=".tmp", + delete=False, + ) as temporary: + json.dump(cache_payload, temporary, indent=2, sort_keys=True, allow_nan=False) + temporary.write("\n") + temporary_cache = Path(temporary.name) + os.replace(temporary_cache, cache_path) + return CandidateEvaluation(metrics, False, cache_key, metadata) + + +def _evaluator_fingerprint(target: str) -> str: + module_or_path, _ = target.rsplit(":", maxsplit=1) + source_path = Path(module_or_path) + if not source_path.is_file(): + relative = Path(*module_or_path.split(".")) + for entry in sys.path: + root = Path(entry or ".") + candidates = ( + root / relative.with_suffix(".py"), + root / relative / "__init__.py", + ) + source_path = next( + (candidate for candidate in candidates if candidate.is_file()), + source_path, + ) + if source_path.is_file(): + break + if source_path.is_file(): + return hashlib.sha256(source_path.read_bytes()).hexdigest() + return hashlib.sha256(target.encode("utf-8")).hexdigest() + + +def _runtime_fingerprint() -> dict[str, str]: + versions = { + "python": sys.version, + "calibration_implementation": _calibration_implementation_fingerprint(), + } + for package in ("embodichain", "dexsim_engine"): + try: + versions[package] = importlib.metadata.version(package) + except importlib.metadata.PackageNotFoundError: + versions[package] = "unavailable" + return versions + + +def _calibration_implementation_fingerprint() -> str: + """Hash toolkit sources that define worker and metric cache semantics.""" + digest = hashlib.sha256() + for source in sorted(Path(__file__).parent.glob("*.py")): + digest.update(source.name.encode("utf-8")) + digest.update(b"\0") + digest.update(source.read_bytes()) + digest.update(b"\0") + return digest.hexdigest() + + +__all__ = ["CandidateEvaluation", "EvaluationError", "run_candidate"] diff --git a/embodichain/toolkits/dynamics_calibration/metrics.py b/embodichain/toolkits/dynamics_calibration/metrics.py new file mode 100644 index 000000000..44da13dab --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/metrics.py @@ -0,0 +1,657 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tracking metrics and hard qualification gates.""" + +from __future__ import annotations + +import math +from collections.abc import Mapping +from dataclasses import dataclass +from typing import Any + +import numpy as np + +from .schema import QualificationThresholds + + +@dataclass(frozen=True) +class TrackingMetrics: + """Serializable tracking evidence produced from one evaluator run. + + Attributes: + joint_names: Ordered joint names matching observation columns. + sample_count: Number of time samples in the evaluation. + aggregate_rmse: Root-mean-square error over all joints and samples. + aggregate_p95: 95th percentile of absolute tracking error. + per_joint_rmse: RMSE indexed by joint name. + per_joint_p95: Absolute-error P95 indexed by joint name. + per_control_group_rmse: RMSE indexed by application control group. + per_control_group_p95: Absolute-error P95 by control group. + worst_joint_rmse: Largest per-joint RMSE. + worst_joint: Joint associated with ``worst_joint_rmse``. + cvar95: Mean absolute error over the worst five-percent tail. + overshoot: Optional application-defined step-response overshoot. + settling_time_seconds: Optional application-defined settling time. + saturation_fraction: Fraction of effort samples at their limits. + velocity_saturation_fraction: Fraction of velocity samples at limits. + joint_limit_violation: Maximum observed position-limit violation. + requested_control_hz: Application-requested control frequency. + actual_control_hz: Frequency represented by the physics schedule. + control_frequency_relative_error: Relative requested/actual mismatch. + target_qvel_write_count: Observed target-velocity API write count, or + ``None`` when the evaluator supplied no instrumentation evidence. + stable: Whether all required observations remained finite and stable. + """ + + joint_names: tuple[str, ...] + sample_count: int + aggregate_rmse: float + aggregate_p95: float + per_joint_rmse: dict[str, float] + per_joint_p95: dict[str, float] + per_control_group_rmse: dict[str, float] + per_control_group_p95: dict[str, float] + worst_joint_rmse: float + worst_joint: str + cvar95: float + overshoot: float | None + settling_time_seconds: float | None + saturation_fraction: float | None + velocity_saturation_fraction: float | None + joint_limit_violation: float | None + requested_control_hz: float + actual_control_hz: float + control_frequency_relative_error: float + target_qvel_write_count: int | None + stable: bool + + def to_dict(self) -> dict[str, Any]: + """Return a JSON-serializable representation. + + Returns: + Metrics payload with non-finite values represented as strings. + """ + return { + "joint_names": list(self.joint_names), + "sample_count": self.sample_count, + "aggregate_rmse": _json_number(self.aggregate_rmse), + "aggregate_p95": _json_number(self.aggregate_p95), + "per_joint_rmse": _json_number_mapping(self.per_joint_rmse), + "per_joint_p95": _json_number_mapping(self.per_joint_p95), + "per_control_group_rmse": _json_number_mapping(self.per_control_group_rmse), + "per_control_group_p95": _json_number_mapping(self.per_control_group_p95), + "worst_joint_rmse": _json_number(self.worst_joint_rmse), + "worst_joint": self.worst_joint, + "cvar95": _json_number(self.cvar95), + "overshoot": _json_number(self.overshoot), + "settling_time_seconds": _json_number(self.settling_time_seconds), + "saturation_fraction": _json_number(self.saturation_fraction), + "velocity_saturation_fraction": _json_number( + self.velocity_saturation_fraction + ), + "joint_limit_violation": _json_number(self.joint_limit_violation), + "requested_control_hz": self.requested_control_hz, + "actual_control_hz": self.actual_control_hz, + "control_frequency_relative_error": _json_number( + self.control_frequency_relative_error + ), + "target_qvel_write_count": self.target_qvel_write_count, + "stable": self.stable, + } + + @classmethod + def from_dict(cls, data: Mapping[str, Any]) -> TrackingMetrics: + """Restore metrics from a cache or report dictionary. + + Args: + data: Serialized metrics payload produced by :meth:`to_dict`. + + Returns: + Reconstructed immutable metrics. + + Raises: + KeyError: If a required metric is absent. + TypeError: If instrumentation fields have incompatible types. + ValueError: If numeric fields cannot be parsed. + """ + return cls( + joint_names=tuple(str(item) for item in data["joint_names"]), + sample_count=int(data["sample_count"]), + aggregate_rmse=float(data["aggregate_rmse"]), + aggregate_p95=float(data["aggregate_p95"]), + per_joint_rmse={ + str(key): float(value) + for key, value in dict(data["per_joint_rmse"]).items() + }, + per_joint_p95={ + str(key): float(value) + for key, value in dict(data["per_joint_p95"]).items() + }, + per_control_group_rmse={ + str(key): float(value) + for key, value in dict(data.get("per_control_group_rmse", {})).items() + }, + per_control_group_p95={ + str(key): float(value) + for key, value in dict(data.get("per_control_group_p95", {})).items() + }, + worst_joint_rmse=float(data["worst_joint_rmse"]), + worst_joint=str(data["worst_joint"]), + cvar95=float(data["cvar95"]), + overshoot=_optional_float(data.get("overshoot")), + settling_time_seconds=_optional_float(data.get("settling_time_seconds")), + saturation_fraction=_optional_float(data.get("saturation_fraction")), + velocity_saturation_fraction=_optional_float( + data.get("velocity_saturation_fraction") + ), + joint_limit_violation=_optional_float(data.get("joint_limit_violation")), + requested_control_hz=float(data["requested_control_hz"]), + actual_control_hz=float(data["actual_control_hz"]), + control_frequency_relative_error=float( + data["control_frequency_relative_error"] + ), + target_qvel_write_count=_optional_integer( + data.get("target_qvel_write_count") + ), + stable=bool(data["stable"]), + ) + + +@dataclass(frozen=True) +class QualificationGate: + """One auditable hard-gate decision. + + Attributes: + name: Stable metric or invariant name. + passed: Whether the observed value satisfies the gate. + observed: Value used for the decision, or ``None`` when unavailable. + expected: Configured upper bound or exact expected value. + entity: Optional joint or control-group associated with the value. + """ + + name: str + passed: bool + observed: float | int | bool | None + expected: float | int | bool + entity: str | None = None + + def to_dict(self) -> dict[str, Any]: + """Return a JSON-serializable representation. + + Returns: + Gate decision and associated observed and expected values. + """ + return { + "name": self.name, + "passed": self.passed, + "observed": _json_number(self.observed), + "expected": self.expected, + "entity": self.entity, + } + + +@dataclass(frozen=True) +class QualificationResult: + """Aggregate qualification status with every evaluated gate. + + Attributes: + status: ``pass`` only when every configured gate passes; otherwise + ``fail``. + gates: Complete ordered gate evidence. + """ + + status: str + gates: tuple[QualificationGate, ...] + + def to_dict(self) -> dict[str, Any]: + """Return a JSON-serializable representation. + + Returns: + Aggregate status and all individual gate decisions. + """ + return { + "status": self.status, + "gates": [gate.to_dict() for gate in self.gates], + } + + +def compute_tracking_metrics(raw: Mapping[str, Any]) -> TrackingMetrics: + """Compute aggregate and per-joint metrics from evaluator observations. + + The evaluator owns application execution but not metric definitions. This + central calculation keeps candidate comparisons and qualification stable. + + Args: + raw: Evaluator observations containing joint names, target and actual + position matrices, requested and actual control frequencies, and + optional group, limit, effort, velocity, and stability evidence. + + Returns: + Validated aggregate, per-joint, group, tail, and safety metrics. + + Raises: + KeyError: If a required observation is absent. + TypeError: If an observation has an incompatible type. + ValueError: If observation shapes, names, limits, or frequencies are + inconsistent. + """ + raw_joint_names = raw["joint_names"] + if not isinstance(raw_joint_names, list) or not all( + isinstance(name, str) and name for name in raw_joint_names + ): + raise TypeError("joint_names must be a list of non-empty strings") + joint_names = tuple(raw_joint_names) + target = np.asarray(raw["target_qpos"], dtype=np.float64) + actual = np.asarray(raw["actual_qpos"], dtype=np.float64) + if target.ndim != 2 or actual.ndim != 2: + raise ValueError("target_qpos and actual_qpos must be two-dimensional") + if target.shape != actual.shape: + raise ValueError("target_qpos and actual_qpos must have identical shapes") + if target.shape[0] == 0 or target.shape[1] == 0: + raise ValueError("tracking observations cannot be empty") + if target.shape[1] != len(joint_names): + raise ValueError("joint_names must match the observation joint dimension") + if len(joint_names) != len(set(joint_names)): + raise ValueError("joint_names must be unique") + + with np.errstate(over="ignore", invalid="ignore"): + error = actual - target + finite = bool(np.isfinite(error).all()) + safe_error = np.where(np.isfinite(error), error, np.inf) + absolute_error = np.abs(safe_error) + per_joint_rmse_values = np.asarray( + [_rmse(safe_error[:, index]) for index in range(safe_error.shape[1])] + ) + per_joint_p95_values = np.asarray( + [_p95(absolute_error[:, index]) for index in range(absolute_error.shape[1])] + ) + per_joint_rmse = { + name: float(value) for name, value in zip(joint_names, per_joint_rmse_values) + } + per_joint_p95 = { + name: float(value) for name, value in zip(joint_names, per_joint_p95_values) + } + group_rmse, group_p95 = _compute_control_group_metrics(raw, joint_names, safe_error) + worst_index = int(np.argmax(per_joint_rmse_values)) + flattened = np.sort(absolute_error.reshape(-1)) + tail_count = max(1, int(math.ceil(flattened.size * 0.05))) + + requested_hz = float(raw["requested_control_hz"]) + actual_hz = float(raw["actual_control_hz"]) + if not math.isfinite(requested_hz) or requested_hz <= 0.0: + raise ValueError("requested_control_hz must be finite and greater than zero") + if not math.isfinite(actual_hz) or actual_hz <= 0.0: + raise ValueError("actual_control_hz must be finite and greater than zero") + + saturation_fraction, effort_finite = _compute_saturation_fraction( + raw, + actual.shape, + value_name="effort", + limit_name="effort_limits", + ) + velocity_saturation_fraction, velocity_finite = _compute_saturation_fraction( + raw, + actual.shape, + value_name="qvel", + limit_name="qvel_limits", + ) + joint_limit_violation = _compute_joint_limit_violation(raw, actual) + overshoot, overshoot_finite = _optional_nonnegative_metric(raw, "overshoot") + settling_time, settling_finite = _optional_nonnegative_metric( + raw, "settling_time_seconds" + ) + raw_stable = raw.get("stable", True) + if not isinstance(raw_stable, bool): + raise TypeError("stable must be a boolean when provided") + raw_qvel_writes = raw.get("target_qvel_write_count") + if raw_qvel_writes is not None: + if isinstance(raw_qvel_writes, bool) or not isinstance( + raw_qvel_writes, (int, np.integer) + ): + raise TypeError("target_qvel_write_count must be an integer") + if raw_qvel_writes < 0: + raise ValueError("target_qvel_write_count cannot be negative") + stable = ( + raw_stable + and finite + and effort_finite + and velocity_finite + and overshoot_finite + and settling_finite + ) + return TrackingMetrics( + joint_names=joint_names, + sample_count=target.shape[0], + aggregate_rmse=_rmse(safe_error), + aggregate_p95=_p95(absolute_error), + per_joint_rmse=per_joint_rmse, + per_joint_p95=per_joint_p95, + per_control_group_rmse=group_rmse, + per_control_group_p95=group_p95, + worst_joint_rmse=float(per_joint_rmse_values[worst_index]), + worst_joint=joint_names[worst_index], + cvar95=_mean_absolute(flattened[-tail_count:]), + overshoot=overshoot, + settling_time_seconds=settling_time, + saturation_fraction=saturation_fraction, + velocity_saturation_fraction=velocity_saturation_fraction, + joint_limit_violation=joint_limit_violation, + requested_control_hz=requested_hz, + actual_control_hz=actual_hz, + control_frequency_relative_error=abs(actual_hz - requested_hz) / requested_hz, + target_qvel_write_count=( + None if raw_qvel_writes is None else int(raw_qvel_writes) + ), + stable=stable, + ) + + +def qualify( + metrics: TrackingMetrics, thresholds: QualificationThresholds +) -> QualificationResult: + """Apply configured aggregate, per-joint, timing, and stability gates. + + Args: + metrics: Held-out application tracking and safety evidence. + thresholds: Hard upper bounds and exact expected invariants. + + Returns: + Overall pass/fail result with every configured gate preserved. + """ + gates: list[QualificationGate] = [] + _append_upper_gate( + gates, "aggregate_rmse", metrics.aggregate_rmse, thresholds.aggregate_rmse_max + ) + _append_upper_gate( + gates, "aggregate_p95", metrics.aggregate_p95, thresholds.aggregate_p95_max + ) + _append_per_joint_gate( + gates, "per_joint_rmse", metrics.per_joint_rmse, thresholds.per_joint_rmse_max + ) + _append_per_joint_gate( + gates, "per_joint_p95", metrics.per_joint_p95, thresholds.per_joint_p95_max + ) + _append_group_gate( + gates, + "per_control_group_rmse", + metrics.per_control_group_rmse, + thresholds.per_control_group_rmse_max, + ) + _append_group_gate( + gates, + "per_control_group_p95", + metrics.per_control_group_p95, + thresholds.per_control_group_p95_max, + ) + _append_upper_gate( + gates, + "worst_joint_rmse", + metrics.worst_joint_rmse, + thresholds.worst_joint_rmse_max, + entity=metrics.worst_joint, + ) + _append_upper_gate(gates, "cvar95", metrics.cvar95, thresholds.cvar95_max) + _append_upper_gate(gates, "overshoot", metrics.overshoot, thresholds.overshoot_max) + _append_upper_gate( + gates, + "settling_time_seconds", + metrics.settling_time_seconds, + thresholds.settling_time_seconds_max, + ) + _append_upper_gate( + gates, + "saturation_fraction", + metrics.saturation_fraction, + thresholds.saturation_fraction_max, + ) + _append_upper_gate( + gates, + "velocity_saturation_fraction", + metrics.velocity_saturation_fraction, + thresholds.velocity_saturation_fraction_max, + ) + _append_upper_gate( + gates, + "joint_limit_violation", + metrics.joint_limit_violation, + thresholds.joint_limit_violation_max, + ) + _append_upper_gate( + gates, + "control_frequency_relative_error", + metrics.control_frequency_relative_error, + thresholds.control_frequency_relative_error_max, + ) + if thresholds.expected_target_qvel_write_count is not None: + expected = thresholds.expected_target_qvel_write_count + gates.append( + QualificationGate( + "target_qvel_write_count", + metrics.target_qvel_write_count is not None + and metrics.target_qvel_write_count == expected, + metrics.target_qvel_write_count, + expected, + ) + ) + if thresholds.require_stable: + gates.append(QualificationGate("stable", metrics.stable, metrics.stable, True)) + return QualificationResult( + status="pass" if all(gate.passed for gate in gates) else "fail", + gates=tuple(gates), + ) + + +def _compute_saturation_fraction( + raw: Mapping[str, Any], + observation_shape: tuple[int, ...], + *, + value_name: str, + limit_name: str, +) -> tuple[float | None, bool]: + has_values = value_name in raw + has_limits = limit_name in raw + if has_values != has_limits: + raise ValueError(f"{value_name} and {limit_name} must be provided together") + if not has_values: + return None, True + values = np.asarray(raw[value_name], dtype=np.float64) + limits = np.asarray(raw[limit_name], dtype=np.float64) + if values.shape != observation_shape: + raise ValueError(f"{value_name} must match the qpos observation shape") + try: + limits = np.broadcast_to(limits, values.shape) + except ValueError as error: + raise ValueError(f"{limit_name} cannot be broadcast to {value_name}") from error + if np.any(~np.isfinite(limits)) or np.any(limits <= 0.0): + raise ValueError(f"{limit_name} must be finite and greater than zero") + if not np.isfinite(values).all(): + return math.inf, False + return float(np.mean(np.abs(values) >= limits)), True + + +def _optional_nonnegative_metric( + raw: Mapping[str, Any], name: str +) -> tuple[float | None, bool]: + if name not in raw: + return None, True + value = float(raw[name]) + if math.isnan(value) or value < 0.0: + return math.inf, False + return value, math.isfinite(value) + + +def _compute_control_group_metrics( + raw: Mapping[str, Any], + joint_names: tuple[str, ...], + error: np.ndarray, +) -> tuple[dict[str, float], dict[str, float]]: + raw_groups = raw.get("control_groups", {}) + if raw_groups is None: + return {}, {} + if not isinstance(raw_groups, Mapping): + raise TypeError("control_groups must be a mapping of group names to joints") + indices_by_name = {name: index for index, name in enumerate(joint_names)} + rmse: dict[str, float] = {} + p95: dict[str, float] = {} + for raw_name, raw_members in raw_groups.items(): + if not isinstance(raw_name, str) or not raw_name: + raise TypeError("control group names must be non-empty strings") + name = raw_name + if isinstance(raw_members, (str, bytes)) or not isinstance(raw_members, list): + raise TypeError(f"control group {name!r} must contain a list of joints") + if not all(isinstance(member, str) and member for member in raw_members): + raise TypeError( + f"control group {name!r} must contain non-empty joint names" + ) + if len(raw_members) != len(set(raw_members)): + raise ValueError(f"control group {name!r} cannot repeat a joint") + unknown = [member for member in raw_members if member not in indices_by_name] + if unknown: + raise ValueError( + f"control group {name!r} contains unknown joints: {', '.join(unknown)}" + ) + if not raw_members: + raise ValueError(f"control group {name!r} cannot be empty") + group_error = error[:, [indices_by_name[member] for member in raw_members]] + rmse[name] = _rmse(group_error) + p95[name] = _p95(np.abs(group_error)) + return rmse, p95 + + +def _compute_joint_limit_violation( + raw: Mapping[str, Any], actual: np.ndarray +) -> float | None: + has_lower = "qpos_lower" in raw + has_upper = "qpos_upper" in raw + if has_lower != has_upper: + raise ValueError("qpos_lower and qpos_upper must be provided together") + if not has_lower: + return None + lower = np.asarray(raw["qpos_lower"], dtype=np.float64) + upper = np.asarray(raw["qpos_upper"], dtype=np.float64) + try: + lower = np.broadcast_to(lower, actual.shape) + upper = np.broadcast_to(upper, actual.shape) + except ValueError as error: + raise ValueError("qpos limits cannot be broadcast to actual_qpos") from error + if np.isnan(lower).any() or np.isnan(upper).any(): + raise ValueError("qpos limits cannot contain NaN") + if np.any(lower > upper): + raise ValueError("qpos_lower cannot exceed qpos_upper") + violation = np.maximum(np.maximum(lower - actual, actual - upper), 0.0) + return float(np.max(violation)) + + +def _append_upper_gate( + gates: list[QualificationGate], + name: str, + observed: float | None, + threshold: float | None, + *, + entity: str | None = None, +) -> None: + if threshold is None: + return + passed = observed is not None and math.isfinite(observed) and observed <= threshold + gates.append(QualificationGate(name, passed, observed, threshold, entity)) + + +def _append_per_joint_gate( + gates: list[QualificationGate], + name: str, + observed: Mapping[str, float], + threshold: float | None, +) -> None: + if threshold is None: + return + entity, worst = max(observed.items(), key=lambda item: item[1]) + passed = math.isfinite(worst) and worst <= threshold + gates.append(QualificationGate(name, passed, worst, threshold, entity)) + + +def _append_group_gate( + gates: list[QualificationGate], + name: str, + observed: Mapping[str, float], + threshold: float | None, +) -> None: + if threshold is None: + return + if not observed: + gates.append(QualificationGate(name, False, None, threshold)) + return + entity, worst = max(observed.items(), key=lambda item: item[1]) + passed = math.isfinite(worst) and worst <= threshold + gates.append(QualificationGate(name, passed, worst, threshold, entity)) + + +def _optional_float(value: Any) -> float | None: + return None if value is None else float(value) + + +def _optional_integer(value: Any) -> int | None: + if value is None: + return None + if isinstance(value, bool) or not isinstance(value, int): + raise TypeError("cached target_qvel_write_count must be an integer") + return value + + +def _p95(values: np.ndarray) -> float: + if not np.isfinite(values).all(): + return math.inf + return float(np.percentile(values, 95.0)) + + +def _rmse(values: np.ndarray) -> float: + maximum = float(np.max(np.abs(values))) + if not math.isfinite(maximum): + return math.inf + if maximum == 0.0: + return 0.0 + scaled = values / maximum + return maximum * float(np.sqrt(np.mean(np.square(scaled)))) + + +def _mean_absolute(values: np.ndarray) -> float: + maximum = float(np.max(np.abs(values))) + if not math.isfinite(maximum): + return math.inf + if maximum == 0.0: + return 0.0 + return maximum * float(np.mean(np.abs(values) / maximum)) + + +def _json_number(value: Any) -> Any: + if isinstance(value, (float, np.floating)) and not math.isfinite(float(value)): + if math.isnan(float(value)): + return "nan" + return "inf" if float(value) > 0.0 else "-inf" + return value + + +def _json_number_mapping(values: Mapping[str, float]) -> dict[str, float | str]: + return {name: _json_number(value) for name, value in values.items()} + + +__all__ = [ + "QualificationGate", + "QualificationResult", + "TrackingMetrics", + "compute_tracking_metrics", + "qualify", +] diff --git a/embodichain/toolkits/dynamics_calibration/overlay.py b/embodichain/toolkits/dynamics_calibration/overlay.py new file mode 100644 index 000000000..b1e17d39f --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/overlay.py @@ -0,0 +1,118 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Reviewable drive-overlay construction and serialization.""" + +from __future__ import annotations + +from collections.abc import Mapping +from pathlib import Path +from typing import Any + +import yaml + +from .schema import CalibrationConfig + +_OVERLAY_KIND = "embodichain.dynamics_calibration.drive_overlay" + + +def build_drive_overlay( + config: CalibrationConfig, candidate: Mapping[str, float] +) -> dict[str, Any]: + """Build a non-destructive RobotCfg-compatible drive-property overlay. + + Args: + config: Validated calibration configuration and asset identities. + candidate: Exact parameter-name to candidate-value mapping. + + Returns: + Serializable overlay containing ``drive_pros`` and provenance. + + Raises: + ValueError: If candidate names or bounds do not match the configuration. + """ + expected = {parameter.name for parameter in config.parameters} + if set(candidate) != expected: + missing = sorted(expected - set(candidate)) + extra = sorted(set(candidate) - expected) + raise ValueError(f"candidate keys mismatch; missing={missing}, extra={extra}") + drive_properties: dict[str, dict[str, float]] = {} + normalized_candidate: dict[str, float] = {} + for parameter in config.parameters: + value = float(candidate[parameter.name]) + if not parameter.lower <= value <= parameter.upper: + raise ValueError( + f"candidate {parameter.name!r}={value:g} is outside " + f"[{parameter.lower:g}, {parameter.upper:g}]" + ) + drive_properties.setdefault(parameter.field, {})[parameter.selector] = value + normalized_candidate[parameter.name] = value + return { + "schema_version": 1, + "kind": _OVERLAY_KIND, + "assets": config.asset_records(), + "backend": config.backend, + "device": config.device, + "physics_dt": config.physics_dt, + "control_frequency_hz": config.control_frequency_hz, + "drive_pros": drive_properties, + "calibration": { + "claim": "effective_drive_tuning", + "seed": config.seed, + "candidate_count": config.candidate_count, + "candidate": normalized_candidate, + }, + } + + +def write_overlay(path: str | Path, overlay: Mapping[str, Any]) -> None: + """Write an overlay as deterministic, human-reviewable YAML. + + Args: + path: Destination YAML path. + overlay: Overlay payload to serialize. + """ + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text( + yaml.safe_dump(dict(overlay), sort_keys=False, allow_unicode=True), + encoding="utf-8", + ) + + +def load_overlay(path: str | Path) -> dict[str, Any]: + """Load and minimally validate a dynamics-calibration overlay. + + Args: + path: Existing YAML overlay path. + + Returns: + Parsed V1 overlay payload. + + Raises: + ValueError: If the payload has an unsupported schema or shape. + """ + loaded = yaml.safe_load(Path(path).read_text(encoding="utf-8")) + if not isinstance(loaded, dict): + raise ValueError("drive overlay must contain a mapping") + if loaded.get("schema_version") != 1 or loaded.get("kind") != _OVERLAY_KIND: + raise ValueError("unsupported dynamics-calibration drive overlay") + if not isinstance(loaded.get("drive_pros"), dict): + raise ValueError("drive overlay must contain drive_pros") + return loaded + + +__all__ = ["build_drive_overlay", "load_overlay", "write_overlay"] diff --git a/embodichain/toolkits/dynamics_calibration/report.py b/embodichain/toolkits/dynamics_calibration/report.py new file mode 100644 index 000000000..3bcc7882b --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/report.py @@ -0,0 +1,253 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Machine-readable and reviewable calibration reports.""" + +from __future__ import annotations + +import importlib +import importlib.metadata +import json +import subprocess +from collections.abc import Iterable, Mapping +from datetime import datetime, timezone +from pathlib import Path +from typing import Any + +from .evaluator import CandidateEvaluation +from .metrics import QualificationResult +from .schema import CalibrationConfig, resolve_control_schedule +from .tuning import TuningResult + + +def build_calibration_report( + config: CalibrationConfig, + *, + audits: Iterable[Any] = (), + tuning: TuningResult | None = None, + qualification_evaluation: CandidateEvaluation | None = None, + qualification: QualificationResult | None = None, +) -> dict[str, Any]: + """Build the complete evidence envelope for a calibration run. + + Args: + config: Validated calibration inputs and runtime configuration. + audits: DexSim SimReady reports for the configured assets. + tuning: Optional candidate-search result. + qualification_evaluation: Optional held-out candidate evaluation. + qualification: Optional hard-gate result for the held-out evaluation. + + Returns: + Strict-JSON-compatible report payload with inputs and provenance. + """ + audit_payloads = [report.to_dict() for report in audits] + schedule = resolve_control_schedule( + config.physics_dt, + config.control_frequency_hz, + allow_approximate=config.allow_approximate_control_frequency, + ) + audit_failed = any(report["status"] == "fail" for report in audit_payloads) + audit_review = any(report["status"] == "review" for report in audit_payloads) + if audit_failed or (qualification is not None and qualification.status == "fail"): + status = "fail" + elif qualification is not None: + status = "review" if audit_review else qualification.status + elif tuning is not None: + status = "review" if audit_review else "candidate" + else: + status = "review" if audit_review else "audited" + payload: dict[str, Any] = { + "schema_version": 1, + "kind": "embodichain.dynamics_calibration.report", + "claim": "effective_drive_tuning", + "status": status, + "generated_at": datetime.now(timezone.utc).isoformat(), + "inputs": { + "assets": config.asset_records(), + "backend": config.backend, + "device": config.device, + "physics_dt": config.physics_dt, + "control_schedule": schedule.to_dict(), + "seed": config.seed, + "candidate_count": config.candidate_count, + "evaluator_target": config.evaluator.target, + }, + "versions": { + "embodichain": _package_version("embodichain"), + "dexsim_engine": _package_version("dexsim_engine"), + "dexsim_commit_id": _module_attribute("dexsim", "__commit_id__"), + "embodichain_git_sha": _git_sha(Path(__file__).resolve()), + }, + "asset_audits": audit_payloads, + "uncertainty": { + "status": "not_estimated", + "reason": ( + "V1 tunes effective drive behavior from tracking evidence and does " + "not identify physical parameters or confidence intervals." + ), + }, + } + if tuning is not None: + payload["tuning"] = tuning.to_dict() + if qualification_evaluation is not None: + payload["qualification_evaluation"] = { + "cache_hit": qualification_evaluation.cache_hit, + "cache_key": qualification_evaluation.cache_key, + "metrics": qualification_evaluation.metrics.to_dict(), + "evaluator_metadata": dict(qualification_evaluation.metadata), + } + if qualification is not None: + payload["qualification"] = qualification.to_dict() + return payload + + +def calibration_report_to_markdown(report: Mapping[str, Any]) -> str: + """Render a concise Markdown review of a calibration report. + + Args: + report: Report produced by :func:`build_calibration_report`. + + Returns: + Markdown summary suitable for reviewer inspection. + """ + inputs = report["inputs"] + schedule = inputs["control_schedule"] + lines = [ + "# EmbodiChain Dynamics Calibration", + "", + f"- Status: **{report['status']}**", + f"- Claim: `{report['claim']}`", + f"- Backend/device: `{inputs['backend']}` / `{inputs['device']}`", + f"- Physics timestep: `{inputs['physics_dt']:.9g}` s", + ( + "- Control frequency: " + f"requested `{schedule['requested_hz']:.9g}` Hz, " + f"actual `{schedule['actual_hz']:.9g}` Hz" + ), + f"- Seed: `{inputs['seed']}`", + "", + "## Asset audits", + "", + ] + audits = report.get("asset_audits", []) + if audits: + for audit in audits: + lines.append( + f"- **{audit['status']}** — `{audit['source']}` " + f"(`{audit['asset_sha256']}`)" + ) + else: + lines.append("No asset audit was attached.") + + tuning = report.get("tuning") + if tuning is not None: + lines.extend( + [ + "", + "## Drive tuning", + "", + f"- Baseline objective: `{_format_number(tuning['baseline_objective'])}`", + f"- Best objective: `{_format_number(tuning['best_objective'])}`", + f"- Best candidate: `{json.dumps(tuning['best_candidate'], sort_keys=True)}`", + f"- Evaluated candidates: `{len(tuning['trials'])}`", + ] + ) + + qualification = report.get("qualification") + if qualification is not None: + lines.extend(["", "## Qualification", ""]) + for gate in qualification["gates"]: + marker = "PASS" if gate["passed"] else "FAIL" + entity = f" ({gate['entity']})" if gate.get("entity") else "" + lines.append( + f"- **{marker}** `{gate['name']}`{entity}: " + f"observed `{gate['observed']}`, expected `{gate['expected']}`" + ) + lines.extend( + [ + "", + "## Scope", + "", + "This report supports effective drive tuning only. It does not claim " + "mass, center-of-mass, inertia, or friction identification.", + ] + ) + return "\n".join(lines) + "\n" + + +def write_calibration_reports( + output_dir: str | Path, report: Mapping[str, Any] +) -> tuple[Path, Path]: + """Write canonical JSON and Markdown reports into one output directory. + + Args: + output_dir: Directory that receives ``report.json`` and ``report.md``. + report: Strict-JSON-compatible calibration report. + + Returns: + Paths to the JSON and Markdown reports, respectively. + """ + destination = Path(output_dir) + destination.mkdir(parents=True, exist_ok=True) + json_path = destination / "report.json" + markdown_path = destination / "report.md" + json_path.write_text( + json.dumps(dict(report), indent=2, sort_keys=True, allow_nan=False) + "\n", + encoding="utf-8", + ) + markdown_path.write_text(calibration_report_to_markdown(report), encoding="utf-8") + return json_path, markdown_path + + +def _package_version(package: str) -> str: + try: + return importlib.metadata.version(package) + except importlib.metadata.PackageNotFoundError: + return "unavailable" + + +def _module_attribute(module_name: str, attribute: str) -> str: + try: + module = importlib.import_module(module_name) + except ImportError: + return "unavailable" + value = getattr(module, attribute, "unavailable") + return str(value) + + +def _format_number(value: Any) -> str: + return f"{value:.9g}" if isinstance(value, (int, float)) else str(value) + + +def _git_sha(source: Path) -> str: + try: + completed = subprocess.run( + ["git", "-C", str(source.parent), "rev-parse", "HEAD"], + check=True, + capture_output=True, + text=True, + timeout=5.0, + ) + except (OSError, subprocess.SubprocessError): + return "unavailable" + return completed.stdout.strip() or "unavailable" + + +__all__ = [ + "build_calibration_report", + "calibration_report_to_markdown", + "write_calibration_reports", +] diff --git a/embodichain/toolkits/dynamics_calibration/schema.py b/embodichain/toolkits/dynamics_calibration/schema.py new file mode 100644 index 000000000..78dd5abb3 --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/schema.py @@ -0,0 +1,580 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Configuration and result contracts for dynamics calibration.""" + +from __future__ import annotations + +import hashlib +import math +from collections.abc import Mapping +from dataclasses import MISSING, dataclass +from numbers import Integral +from pathlib import Path +from typing import Any + +import yaml + +from embodichain.utils import configclass + +_DRIVE_FIELDS = { + "armature", + "damping", + "friction", + "max_effort", + "max_velocity", + "stiffness", +} + + +@dataclass(frozen=True) +class ControlSchedule: + """An application control period resolved onto physics updates. + + Attributes: + physics_steps_per_control: Integral physics updates per control sample. + requested_hz: Application-requested control frequency. + actual_hz: Frequency produced by the integral physics schedule. + """ + + physics_steps_per_control: int + requested_hz: float + actual_hz: float + + @property + def relative_error(self) -> float: + """Return the relative difference between actual and requested rates. + + Returns: + Absolute frequency error divided by ``requested_hz``. + """ + return abs(self.actual_hz - self.requested_hz) / self.requested_hz + + def to_dict(self) -> dict[str, int | float]: + """Return a JSON-serializable representation. + + Returns: + Integral schedule and requested, actual, and relative frequencies. + """ + return { + "physics_steps_per_control": self.physics_steps_per_control, + "requested_hz": self.requested_hz, + "actual_hz": self.actual_hz, + "relative_error": self.relative_error, + } + + +def resolve_control_schedule( + physics_dt: float, + requested_hz: float, + *, + allow_approximate: bool = False, + tolerance: float = 1.0e-9, +) -> ControlSchedule: + """Resolve a control frequency without silently changing its timing. + + Args: + physics_dt: Duration of one physics update in seconds. + requested_hz: Requested application control frequency. + allow_approximate: Permit the nearest integral number of physics steps. + tolerance: Relative frequency error accepted as exact. + + Raises: + TypeError: If ``allow_approximate`` is not a boolean. + ValueError: If values are invalid or the requested frequency is not exact. + + Returns: + Exact or explicitly permitted approximate integral control schedule. + """ + if not isinstance(allow_approximate, bool): + raise TypeError("allow_approximate must be a boolean") + physics_dt = float(physics_dt) + requested_hz = float(requested_hz) + tolerance = float(tolerance) + if not math.isfinite(physics_dt) or physics_dt <= 0.0: + raise ValueError("physics_dt must be finite and greater than zero") + if not math.isfinite(requested_hz) or requested_hz <= 0.0: + raise ValueError("requested_hz must be finite and greater than zero") + if not math.isfinite(tolerance) or tolerance < 0.0: + raise ValueError("tolerance must be finite and non-negative") + + ideal_steps = 1.0 / (physics_dt * requested_hz) + lower_steps = max(1, math.floor(ideal_steps)) + upper_steps = max(1, math.ceil(ideal_steps)) + physics_steps = min( + {lower_steps, upper_steps}, + key=lambda steps: ( + abs((1.0 / (physics_dt * steps)) - requested_hz) / requested_hz, + steps, + ), + ) + actual_hz = 1.0 / (physics_dt * physics_steps) + relative_error = abs(actual_hz - requested_hz) / requested_hz + if relative_error > tolerance and not allow_approximate: + raise ValueError( + f"{requested_hz:g} Hz cannot be represented exactly by physics_dt " + f"{physics_dt:g}; nearest is {actual_hz:g} Hz " + f"({physics_steps} physics steps)" + ) + return ControlSchedule(physics_steps, requested_hz, actual_hz) + + +@configclass +class EvaluatorConfig: + """Configuration for one isolated application evaluator. + + Attributes: + target: ``module:function`` or ``/path/to/file.py:function`` callable. + timeout_seconds: Maximum wall time for each worker process. + payload: Application-owned strict-JSON evaluator configuration. + """ + + target: str = MISSING + timeout_seconds: float = 300.0 + payload: dict[str, Any] = {} + + @classmethod + def from_dict(cls, data: Mapping[str, Any], *, base_dir: Path) -> EvaluatorConfig: + """Parse and validate evaluator configuration. + + Args: + data: Evaluator mapping from the calibration configuration. + base_dir: Directory used to resolve evaluator file paths. + + Returns: + Validated evaluator configuration with absolute file targets. + + Raises: + FileNotFoundError: If an explicit evaluator file does not exist. + TypeError: If the payload is not a mapping. + ValueError: If the target or timeout is invalid. + """ + _reject_unknown(data, {"target", "timeout_seconds", "payload"}, "evaluator") + target = str(data.get("target", "")).strip() + if not target or ":" not in target: + raise ValueError("evaluator.target must use 'module:function' syntax") + module_or_path, attribute = target.rsplit(":", maxsplit=1) + if not module_or_path or not attribute: + raise ValueError("evaluator.target must use 'module:function' syntax") + possible_path = Path(module_or_path).expanduser() + if possible_path.suffix == ".py": + if not possible_path.is_absolute(): + possible_path = base_dir / possible_path + if not possible_path.is_file(): + raise FileNotFoundError( + f"evaluator module does not exist: {possible_path.resolve()}" + ) + target = f"{possible_path.resolve()}:{attribute}" + + timeout_seconds = float(data.get("timeout_seconds", 300.0)) + if not math.isfinite(timeout_seconds) or timeout_seconds <= 0.0: + raise ValueError("evaluator.timeout_seconds must be greater than zero") + payload = data.get("payload", {}) + if not isinstance(payload, Mapping): + raise TypeError("evaluator.payload must be a mapping") + return cls( + target=target, + timeout_seconds=timeout_seconds, + payload=dict(payload), + ) + + +@configclass +class DriveParameterSpec: + """One bounded effective drive parameter exposed to the search. + + Attributes: + name: Unique candidate-coordinate name. + field: RobotCfg ``drive_pros`` field to tune. + selector: Exact name, regular expression, or control-part selector. + lower: Inclusive lower search bound. + upper: Inclusive upper search bound. + initial: Baseline value included as the first candidate. + scale: ``linear`` or ``log`` sampling scale. + """ + + name: str = MISSING + field: str = MISSING + selector: str = MISSING + lower: float = MISSING + upper: float = MISSING + initial: float = MISSING + scale: str = "linear" + + @classmethod + def from_dict(cls, data: Mapping[str, Any]) -> DriveParameterSpec: + """Parse and validate one drive parameter. + + Args: + data: Parameter name, target, bounds, initial value, and scale. + + Returns: + Validated bounded parameter specification. + + Raises: + KeyError: If a required bound is absent. + TypeError: If a numeric field cannot be converted. + ValueError: If names, bounds, initial value, field, or scale are + invalid. + """ + _reject_unknown( + data, + {"name", "field", "selector", "lower", "upper", "initial", "scale"}, + "parameter", + ) + name = str(data.get("name", "")).strip() + field = str(data.get("field", "")).strip() + selector = str(data.get("selector", "")).strip() + if not name: + raise ValueError("parameter.name cannot be empty") + if field not in _DRIVE_FIELDS: + raise ValueError( + f"parameter {name!r} field must be one of {sorted(_DRIVE_FIELDS)}" + ) + if not selector: + raise ValueError(f"parameter {name!r} selector cannot be empty") + lower = float(data["lower"]) + upper = float(data["upper"]) + initial = float(data.get("initial", (lower + upper) / 2.0)) + if not all(math.isfinite(value) for value in (lower, upper, initial)): + raise ValueError(f"parameter {name!r} bounds must be finite") + if lower >= upper: + raise ValueError(f"parameter {name!r} lower must be less than upper") + if not lower <= initial <= upper: + raise ValueError(f"parameter {name!r} initial must lie within its bounds") + scale = str(data.get("scale", "linear")) + if scale not in {"linear", "log"}: + raise ValueError(f"parameter {name!r} scale must be 'linear' or 'log'") + if scale == "log" and lower <= 0.0: + raise ValueError(f"log-scaled parameter {name!r} must have positive bounds") + return cls(name, field, selector, lower, upper, initial, scale) + + +@configclass +class QualificationThresholds: + """Hard gates used to admit a candidate after held-out evaluation. + + Attributes: + aggregate_rmse_max: Maximum flattened RMSE. + aggregate_p95_max: Maximum flattened absolute-error P95. + per_joint_rmse_max: Maximum RMSE for every individual joint. + per_joint_p95_max: Maximum absolute-error P95 for every joint. + per_control_group_rmse_max: Maximum RMSE for every control group. + per_control_group_p95_max: Maximum absolute-error P95 per group. + worst_joint_rmse_max: Maximum worst-joint RMSE. + cvar95_max: Maximum mean absolute error in the worst five-percent tail. + overshoot_max: Maximum application-defined overshoot observation. + settling_time_seconds_max: Maximum application-defined settling time. + saturation_fraction_max: Maximum effort saturation fraction. + velocity_saturation_fraction_max: Maximum velocity saturation fraction. + joint_limit_violation_max: Maximum position-limit violation. + control_frequency_relative_error_max: Maximum requested/actual control + frequency mismatch. + expected_target_qvel_write_count: Exact expected target-velocity API + write count, or ``None`` to disable this instrumentation gate. + require_stable: Require finite, stable evaluator evidence. + """ + + aggregate_rmse_max: float | None = None + aggregate_p95_max: float | None = None + per_joint_rmse_max: float | None = None + per_joint_p95_max: float | None = None + per_control_group_rmse_max: float | None = None + per_control_group_p95_max: float | None = None + worst_joint_rmse_max: float | None = None + cvar95_max: float | None = None + overshoot_max: float | None = None + settling_time_seconds_max: float | None = None + saturation_fraction_max: float | None = None + velocity_saturation_fraction_max: float | None = None + joint_limit_violation_max: float | None = None + control_frequency_relative_error_max: float | None = 0.0 + expected_target_qvel_write_count: int | None = 0 + require_stable: bool = True + + @classmethod + def from_dict(cls, data: Mapping[str, Any]) -> QualificationThresholds: + """Parse and validate qualification thresholds. + + Args: + data: Hard-gate names and non-negative bounds. + + Returns: + Validated qualification policy. + + Raises: + TypeError: If boolean or count fields have incompatible types. + ValueError: If a threshold is unknown, negative, or non-finite. + """ + allowed = set(cls.__annotations__) + _reject_unknown(data, allowed, "qualification") + values = dict(data) + for key in allowed - {"require_stable", "expected_target_qvel_write_count"}: + if key in values and values[key] is not None: + values[key] = float(values[key]) + if not math.isfinite(values[key]) or values[key] < 0.0: + raise ValueError( + f"qualification.{key} must be finite and non-negative" + ) + if values.get("expected_target_qvel_write_count") is not None: + values["expected_target_qvel_write_count"] = _parse_integer( + values["expected_target_qvel_write_count"], + "qualification.expected_target_qvel_write_count", + ) + if values["expected_target_qvel_write_count"] < 0: + raise ValueError( + "qualification.expected_target_qvel_write_count cannot be negative" + ) + if "require_stable" in values: + values["require_stable"] = _parse_boolean( + values["require_stable"], "qualification.require_stable" + ) + return cls(**values) + + +@configclass +class CalibrationConfig: + """Complete effective-drive calibration configuration. + + Attributes: + schema_version: Configuration schema version; V1 is currently supported. + assets: Absolute robot asset paths. + backend: Application-selected physics backend identifier. + device: Evaluator device identifier. + physics_dt: Physics update duration in seconds. + control_frequency_hz: Requested application control frequency. + allow_approximate_control_frequency: Permit an explicitly recorded + approximate integral control schedule. + seed: Deterministic candidate-design seed. + candidate_count: Number of candidates to evaluate. + evaluator: Isolated application evaluator configuration. + parameters: Bounded effective drive parameters to search. + qualification: Hard gates for held-out admission. + """ + + assets: list[str] = MISSING + evaluator: EvaluatorConfig = MISSING + parameters: list[DriveParameterSpec] = MISSING + schema_version: int = 1 + backend: str = "default" + device: str = "cpu" + physics_dt: float = 1.0 / 240.0 + control_frequency_hz: float = 60.0 + allow_approximate_control_frequency: bool = False + seed: int = 0 + candidate_count: int = 9 + qualification: QualificationThresholds = QualificationThresholds() + + @classmethod + def from_dict( + cls, + data: Mapping[str, Any], + *, + base_dir: str | Path = ".", + ) -> CalibrationConfig: + """Parse a calibration configuration and resolve local paths. + + Args: + data: Complete V1 calibration mapping. + base_dir: Directory used to resolve relative asset and evaluator + paths. + + Returns: + Validated configuration with absolute asset and evaluator paths. + + Raises: + FileNotFoundError: If a configured asset or evaluator file is absent. + TypeError: If nested configuration values have incompatible types. + ValueError: If fields, parameters, bounds, or timing are invalid. + """ + allowed = set(cls.__annotations__) + _reject_unknown(data, allowed, "calibration") + schema_version = _parse_integer(data.get("schema_version", 1), "schema_version") + if schema_version != 1: + raise ValueError(f"unsupported calibration schema_version {schema_version}") + resolved_base = Path(base_dir).expanduser().resolve() + raw_assets = data.get("assets", []) + if not isinstance(raw_assets, list) or not raw_assets: + raise ValueError("assets must be a non-empty list") + assets: list[str] = [] + for raw_path in raw_assets: + asset = Path(str(raw_path)).expanduser() + if not asset.is_absolute(): + asset = resolved_base / asset + asset = asset.resolve() + if not asset.is_file(): + raise FileNotFoundError(f"calibration asset does not exist: {asset}") + assets.append(str(asset)) + + physics_dt = float(data.get("physics_dt", 1.0 / 240.0)) + control_frequency_hz = float(data.get("control_frequency_hz", 60.0)) + allow_approximate = _parse_boolean( + data.get("allow_approximate_control_frequency", False), + "allow_approximate_control_frequency", + ) + resolve_control_schedule( + physics_dt, + control_frequency_hz, + allow_approximate=allow_approximate, + ) + + candidate_count = _parse_integer( + data.get("candidate_count", 9), "candidate_count" + ) + if candidate_count < 1: + raise ValueError("candidate_count must be at least one") + raw_evaluator = data.get("evaluator") + if not isinstance(raw_evaluator, Mapping): + raise TypeError("evaluator must be a mapping") + raw_parameters = data.get("parameters") + if not isinstance(raw_parameters, list) or not raw_parameters: + raise ValueError("parameters must be a non-empty list") + parameters = [] + for index, item in enumerate(raw_parameters): + if not isinstance(item, Mapping): + raise TypeError(f"parameters[{index}] must be a mapping") + parameters.append(DriveParameterSpec.from_dict(item)) + names = [parameter.name for parameter in parameters] + if len(names) != len(set(names)): + raise ValueError("parameter names must be unique") + targets = [(parameter.field, parameter.selector) for parameter in parameters] + if len(targets) != len(set(targets)): + raise ValueError("each drive field/selector pair may be tuned only once") + raw_qualification = data.get("qualification", {}) + if not isinstance(raw_qualification, Mapping): + raise TypeError("qualification must be a mapping") + + backend = str(data.get("backend", "default")).strip() + if not backend: + raise ValueError("backend cannot be empty") + device = str(data.get("device", "cpu")).strip() + if not device: + raise ValueError("device cannot be empty") + return cls( + schema_version=schema_version, + assets=assets, + backend=backend, + device=device, + physics_dt=physics_dt, + control_frequency_hz=control_frequency_hz, + allow_approximate_control_frequency=allow_approximate, + seed=_parse_integer(data.get("seed", 0), "seed"), + candidate_count=candidate_count, + evaluator=EvaluatorConfig.from_dict(raw_evaluator, base_dir=resolved_base), + parameters=parameters, + qualification=QualificationThresholds.from_dict(raw_qualification), + ) + + def asset_records(self) -> list[dict[str, str]]: + """Return immutable asset identities used by overlays and cache keys. + + Returns: + Ordered absolute asset paths and their SHA-256 digests. + """ + return [ + { + "path": path, + "sha256": hashlib.sha256(Path(path).read_bytes()).hexdigest(), + } + for path in self.assets + ] + + def evaluation_context(self, phase: str) -> dict[str, Any]: + """Build the factual, serializable context passed to an evaluator. + + Args: + phase: ``training`` for search or ``qualification`` for held-out + admission. + + Returns: + Strict-JSON-compatible asset, runtime, timing, seed, and payload + context. + + Raises: + ValueError: If ``phase`` is unsupported or the configured control + schedule is not representable. + """ + if phase not in {"training", "qualification"}: + raise ValueError("evaluation phase must be 'training' or 'qualification'") + schedule = resolve_control_schedule( + self.physics_dt, + self.control_frequency_hz, + allow_approximate=self.allow_approximate_control_frequency, + ) + return { + "schema_version": self.schema_version, + "phase": phase, + "assets": self.asset_records(), + "backend": self.backend, + "device": self.device, + "physics_dt": self.physics_dt, + "requested_control_hz": schedule.requested_hz, + "actual_control_hz": schedule.actual_hz, + "physics_steps_per_control": schedule.physics_steps_per_control, + "seed": self.seed, + "payload": dict(self.evaluator.payload), + } + + +def _reject_unknown(data: Mapping[str, Any], allowed: set[str], location: str) -> None: + unknown = sorted(set(data) - allowed) + if unknown: + raise ValueError(f"unknown {location} fields: {', '.join(unknown)}") + + +def _parse_integer(value: Any, name: str) -> int: + if isinstance(value, bool) or not isinstance(value, Integral): + raise TypeError(f"{name} must be an integer") + return int(value) + + +def _parse_boolean(value: Any, name: str) -> bool: + if not isinstance(value, bool): + raise TypeError(f"{name} must be a boolean") + return value + + +def load_calibration_config(path: str | Path) -> CalibrationConfig: + """Load a YAML or JSON calibration configuration from disk. + + Args: + path: Configuration file path. + + Returns: + Validated configuration with paths resolved relative to the file. + + Raises: + FileNotFoundError: If the configuration or a referenced local file is + absent. + TypeError: If nested configuration values have incompatible types. + ValueError: If the document is not a valid V1 calibration mapping. + """ + source = Path(path).expanduser().resolve() + loaded = yaml.safe_load(source.read_text(encoding="utf-8")) + if not isinstance(loaded, Mapping): + raise ValueError("calibration configuration must contain a mapping") + return CalibrationConfig.from_dict(loaded, base_dir=source.parent) + + +__all__ = [ + "CalibrationConfig", + "ControlSchedule", + "DriveParameterSpec", + "EvaluatorConfig", + "QualificationThresholds", + "load_calibration_config", + "resolve_control_schedule", +] diff --git a/embodichain/toolkits/dynamics_calibration/tracking_evaluator.py b/embodichain/toolkits/dynamics_calibration/tracking_evaluator.py new file mode 100644 index 000000000..c76abf094 --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/tracking_evaluator.py @@ -0,0 +1,245 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Built-in qpos-only trajectory evaluator for robot drive calibration.""" + +from __future__ import annotations + +import copy +import math +from collections.abc import Mapping, Sequence +from typing import Any + + +def evaluate(overlay: dict[str, Any], context: dict[str, Any]) -> dict[str, Any]: + """Evaluate one drive overlay on a deterministic multisine trajectory. + + This callable is designed for the isolated calibration worker. Evaluator + payload options are passed under ``context["payload"]``: + + ``robot_cfg`` + Overrides for :class:`~embodichain.lab.sim.cfg.RobotCfg`. The first + configured asset is used as ``fpath`` unless explicitly repeated. + ``control_part`` + Optional Robot control-part name. All active joints are used otherwise. + ``training_trajectory`` / ``qualification_trajectory`` + Mappings with ``duration_seconds``, ``warmup_seconds``, ``amplitude`` + (scalar or per-joint radians), and ``frequencies_hz`` (scalar or list). + ``renderer`` + Headless renderer selection, defaulting to ``hybrid``. + + Args: + overlay: RobotCfg-compatible candidate drive overlay. + context: Asset, timing, backend, phase, seed, and evaluator payload + supplied by the calibration worker. + + Returns: + Raw observations consumed by :func:`compute_tracking_metrics`. + """ + import torch + + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import RenderCfg, RobotCfg + + payload = context.get("payload", {}) + if not isinstance(payload, Mapping): + raise TypeError("evaluator payload must be a mapping") + backend = str(context["backend"]) + if backend not in {"default", "physx"}: + raise ValueError( + f"built-in tracking evaluator supports 'default'/'physx', got {backend!r}" + ) + + robot_data = copy.deepcopy(dict(payload.get("robot_cfg", {}))) + assets = context.get("assets", []) + if not assets: + raise ValueError("evaluation context contains no robot asset") + robot_data.setdefault("fpath", str(assets[0]["path"])) + robot_data.setdefault("build_pk_chain", False) + robot_data.setdefault("solver_cfg", None) + _merge_drive_properties(robot_data, overlay) + + renderer = str(payload.get("renderer", "hybrid")) + sim_cfg = SimulationManagerCfg( + headless=True, + num_envs=1, + physics_dt=float(context["physics_dt"]), + sim_device=str(context.get("device", "cpu")), + render_cfg=RenderCfg(renderer=renderer), + ) + simulation = SimulationManager(sim_cfg) + robot = simulation.add_robot(RobotCfg.from_dict(robot_data)) + if robot is None: + raise RuntimeError("SimulationManager failed to create the calibration robot") + + control_part_value = payload.get("control_part") + control_part = None if control_part_value is None else str(control_part_value) + joint_ids = robot.get_joint_ids(name=control_part) + if not joint_ids: + raise ValueError(f"control part {control_part!r} resolved to no active joints") + joint_names = [robot.joint_names[index] for index in joint_ids] + initial = robot.get_qpos(name=control_part)[0].detach().clone() + limits = robot.get_qpos_limits(name=control_part)[0].detach().clone() + + phase = str(context["phase"]) + trajectory_key = f"{phase}_trajectory" + trajectory = payload.get(trajectory_key, payload.get("trajectory", {})) + if not isinstance(trajectory, Mapping): + raise TypeError(f"{trajectory_key} must be a mapping") + duration = _positive_float( + trajectory.get("duration_seconds", 4.0 if phase == "qualification" else 3.0), + f"{trajectory_key}.duration_seconds", + ) + warmup = _nonnegative_float( + trajectory.get("warmup_seconds", 0.5), + f"{trajectory_key}.warmup_seconds", + ) + requested_hz = float(context["requested_control_hz"]) + actual_hz = float(context["actual_control_hz"]) + physics_steps = int(context["physics_steps_per_control"]) + sample_count = max(1, int(round(duration * actual_hz))) + warmup_updates = int(math.ceil(warmup / float(context["physics_dt"]))) + amplitudes = _joint_values( + trajectory.get("amplitude", 0.1), len(joint_ids), "amplitude" + ).to(device=initial.device, dtype=initial.dtype) + default_frequency = 0.35 if phase == "qualification" else 0.25 + frequencies = _joint_values( + trajectory.get("frequencies_hz", default_frequency), + len(joint_ids), + "frequencies_hz", + ).to(device=initial.device, dtype=initial.dtype) + if torch.any(frequencies <= 0.0): + raise ValueError("trajectory frequencies must be greater than zero") + phase_offsets = torch.arange( + len(joint_ids), dtype=initial.dtype, device=initial.device + ) * (math.pi / max(1, len(joint_ids))) + if phase == "qualification": + phase_offsets = phase_offsets + math.pi / 5.0 + + robot.set_qpos(initial.unsqueeze(0), target=False, name=control_part) + robot.set_qpos(initial.unsqueeze(0), target=True, name=control_part) + if warmup_updates: + simulation.update(step=warmup_updates) + target_qvel_writes_before = robot.target_qvel_write_count + + target_rows: list[list[float]] = [] + actual_rows: list[list[float]] = [] + effort_rows: list[list[float]] = [] + velocity_rows: list[list[float]] = [] + for sample_index in range(sample_count): + time_seconds = sample_index / actual_hz + target = initial + amplitudes * torch.sin( + 2.0 * math.pi * frequencies * time_seconds + phase_offsets + ) + target = torch.minimum(torch.maximum(target, limits[:, 0]), limits[:, 1]) + robot.set_qpos(target.unsqueeze(0), target=True, name=control_part) + simulation.update(step=physics_steps) + target_rows.append(target.detach().cpu().tolist()) + actual_rows.append(robot.get_qpos(name=control_part)[0].detach().cpu().tolist()) + effort_rows.append(robot.get_qf(name=control_part)[0].detach().cpu().tolist()) + velocity_rows.append( + robot.get_qvel(name=control_part)[0].detach().cpu().tolist() + ) + + result: dict[str, Any] = { + "joint_names": joint_names, + "target_qpos": target_rows, + "actual_qpos": actual_rows, + "requested_control_hz": requested_hz, + "actual_control_hz": actual_hz, + "target_qvel_write_count": ( + robot.target_qvel_write_count - target_qvel_writes_before + ), + "control_groups": {control_part or "all": joint_names}, + "stable": _rows_are_finite(actual_rows), + "metadata": { + "evaluator": "embodichain.multisine_qpos_v1", + "phase": phase, + "backend": backend, + "device": str(context.get("device", "cpu")), + "renderer": renderer, + "physics_steps_per_control": physics_steps, + "target_qvel_instrumentation": "Articulation.set_qvel", + }, + } + effort_limits = robot.get_qf_limits(name=control_part)[0].detach().cpu() + if torch.isfinite(effort_limits).all() and torch.all(effort_limits > 0.0): + result["effort"] = effort_rows + result["effort_limits"] = effort_limits.tolist() + velocity_limits = robot.get_qvel_limits(name=control_part)[0].detach().cpu() + if torch.isfinite(velocity_limits).all() and torch.all(velocity_limits > 0.0): + result["qvel"] = velocity_rows + result["qvel_limits"] = velocity_limits.tolist() + finite_limits = torch.isfinite(limits).all() + if finite_limits: + result["qpos_lower"] = limits[:, 0].detach().cpu().tolist() + result["qpos_upper"] = limits[:, 1].detach().cpu().tolist() + return result + + +def _merge_drive_properties( + robot_data: dict[str, Any], overlay: Mapping[str, Any] +) -> None: + drive_overlay = overlay.get("drive_pros") + if not isinstance(drive_overlay, Mapping): + raise ValueError("candidate overlay has no drive_pros mapping") + configured = robot_data.setdefault("drive_pros", {}) + if not isinstance(configured, dict): + raise TypeError("robot_cfg.drive_pros must be a mapping") + for field, raw_values in drive_overlay.items(): + if not isinstance(raw_values, Mapping): + configured[str(field)] = raw_values + continue + existing = configured.setdefault(str(field), {}) + if not isinstance(existing, dict): + existing = {} + configured[str(field)] = existing + existing.update({str(key): float(value) for key, value in raw_values.items()}) + + +def _joint_values(raw: Any, count: int, name: str): + import torch + + if isinstance(raw, Sequence) and not isinstance(raw, (str, bytes)): + if len(raw) != count: + raise ValueError(f"trajectory {name} must contain {count} values") + values = [float(value) for value in raw] + else: + values = [float(raw)] * count + if not all(math.isfinite(value) and value >= 0.0 for value in values): + raise ValueError(f"trajectory {name} must contain finite non-negative values") + return torch.tensor(values, dtype=torch.float32) + + +def _positive_float(raw: Any, name: str) -> float: + value = float(raw) + if not math.isfinite(value) or value <= 0.0: + raise ValueError(f"{name} must be finite and greater than zero") + return value + + +def _nonnegative_float(raw: Any, name: str) -> float: + value = float(raw) + if not math.isfinite(value) or value < 0.0: + raise ValueError(f"{name} must be finite and non-negative") + return value + + +def _rows_are_finite(rows: list[list[float]]) -> bool: + return all(math.isfinite(value) for row in rows for value in row) + + +__all__ = ["evaluate"] diff --git a/embodichain/toolkits/dynamics_calibration/tuning.py b/embodichain/toolkits/dynamics_calibration/tuning.py new file mode 100644 index 000000000..3d775d456 --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/tuning.py @@ -0,0 +1,232 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Deterministic bounded search for effective drive parameters.""" + +from __future__ import annotations + +import math +import random +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +from .evaluator import run_candidate +from .metrics import TrackingMetrics +from .overlay import build_drive_overlay +from .schema import CalibrationConfig, DriveParameterSpec + + +@dataclass(frozen=True) +class TuningTrial: + """One evaluated candidate and its scalar ranking objective. + + Attributes: + candidate: Parameter-name to evaluated-value mapping. + objective: Scalar robust tracking objective used for ranking. + cache_hit: Whether this trial reused cached evaluator evidence. + metrics: Centralized metrics for the training trajectory. + evaluator_metadata: Application evidence attached by the evaluator. + """ + + candidate: dict[str, float] + objective: float + cache_hit: bool + metrics: TrackingMetrics + evaluator_metadata: dict[str, Any] + + def to_dict(self) -> dict[str, Any]: + """Return a JSON-serializable representation. + + Returns: + Candidate, objective, metrics, and evaluator provenance. + """ + return { + "candidate": dict(self.candidate), + "objective": _json_objective(self.objective), + "cache_hit": self.cache_hit, + "metrics": self.metrics.to_dict(), + "evaluator_metadata": dict(self.evaluator_metadata), + } + + +@dataclass(frozen=True) +class TuningResult: + """Best overlay plus complete candidate-search evidence. + + Attributes: + best_candidate: Parameter mapping with the lowest objective. + overlay: Non-destructive drive overlay for the best candidate. + best_objective: Lowest observed training objective. + baseline_objective: Objective of the configured initial candidate. + trials: Every evaluated candidate in deterministic order. + """ + + best_candidate: dict[str, float] + overlay: dict[str, Any] + best_objective: float + baseline_objective: float + trials: tuple[TuningTrial, ...] + + def to_dict(self) -> dict[str, Any]: + """Return a JSON-serializable representation. + + Returns: + Best candidate, overlay, objectives, and all trial evidence. + """ + return { + "best_candidate": dict(self.best_candidate), + "overlay": self.overlay, + "best_objective": _json_objective(self.best_objective), + "baseline_objective": _json_objective(self.baseline_objective), + "trials": [trial.to_dict() for trial in self.trials], + } + + +def tune_drive(config: CalibrationConfig, *, cache_dir: str | Path) -> TuningResult: + """Evaluate a reproducible candidate design and return the best overlay. + + Args: + config: Validated calibration search and evaluator configuration. + cache_dir: Directory for content-addressed candidate results. + + Returns: + Best candidate and complete deterministic search evidence. + + Raises: + EvaluationError: If any isolated candidate evaluation fails. + """ + candidates = _generate_candidates( + config.parameters, config.candidate_count, config.seed + ) + trials: list[TuningTrial] = [] + context = config.evaluation_context("training") + for candidate in candidates: + overlay = build_drive_overlay(config, candidate) + evaluation = run_candidate( + config.evaluator, + overlay, + context, + cache_dir=cache_dir, + ) + objective = _tracking_objective( + evaluation.metrics, candidate, config.parameters + ) + trials.append( + TuningTrial( + candidate, + objective, + evaluation.cache_hit, + evaluation.metrics, + evaluation.metadata, + ) + ) + best = min(trials, key=lambda trial: trial.objective) + return TuningResult( + best_candidate=dict(best.candidate), + overlay=build_drive_overlay(config, best.candidate), + best_objective=best.objective, + baseline_objective=trials[0].objective, + trials=tuple(trials), + ) + + +def _generate_candidates( + parameters: list[DriveParameterSpec], count: int, seed: int +) -> list[dict[str, float]]: + templates = [ + {parameter.name: parameter.initial for parameter in parameters}, + {parameter.name: parameter.lower for parameter in parameters}, + {parameter.name: parameter.upper for parameter in parameters}, + {parameter.name: _midpoint(parameter) for parameter in parameters}, + ] + candidates: list[dict[str, float]] = [] + seen: set[tuple[tuple[str, float], ...]] = set() + + def append(candidate: dict[str, float]) -> None: + key = tuple(sorted(candidate.items())) + if key not in seen and len(candidates) < count: + seen.add(key) + candidates.append(candidate) + + for template in templates: + append(template) + generator = random.Random(seed) + while len(candidates) < count: + candidate = { + parameter.name: _sample(parameter, generator) for parameter in parameters + } + append(candidate) + return candidates + + +def _midpoint(parameter: DriveParameterSpec) -> float: + if parameter.scale == "log": + return math.sqrt(parameter.lower * parameter.upper) + return (parameter.lower + parameter.upper) / 2.0 + + +def _sample(parameter: DriveParameterSpec, generator: random.Random) -> float: + unit = generator.random() + if parameter.scale == "log": + low = math.log(parameter.lower) + high = math.log(parameter.upper) + return math.exp(low + unit * (high - low)) + return parameter.lower + unit * (parameter.upper - parameter.lower) + + +def _tracking_objective( + metrics: TrackingMetrics, + candidate: dict[str, float], + parameters: list[DriveParameterSpec], +) -> float: + if not metrics.stable: + return math.inf + objective = ( + metrics.aggregate_rmse + + metrics.worst_joint_rmse + + 0.25 * metrics.aggregate_p95 + + 0.25 * metrics.cvar95 + ) + if metrics.saturation_fraction is not None: + objective += metrics.saturation_fraction + if metrics.velocity_saturation_fraction is not None: + objective += metrics.velocity_saturation_fraction + if metrics.overshoot is not None: + objective += metrics.overshoot + if metrics.joint_limit_violation is not None: + objective += metrics.joint_limit_violation + prior_deviation = sum( + ( + (candidate[parameter.name] - parameter.initial) + / (parameter.upper - parameter.lower) + ) + ** 2 + for parameter in parameters + ) / len(parameters) + objective += 0.01 * prior_deviation + return objective + + +def _json_objective(value: float) -> float | str: + if math.isfinite(value): + return value + if math.isnan(value): + return "nan" + return "inf" if value > 0.0 else "-inf" + + +__all__ = ["TuningResult", "TuningTrial", "tune_drive"] diff --git a/embodichain/toolkits/dynamics_calibration/worker.py b/embodichain/toolkits/dynamics_calibration/worker.py new file mode 100644 index 000000000..898ef8973 --- /dev/null +++ b/embodichain/toolkits/dynamics_calibration/worker.py @@ -0,0 +1,99 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Private child-process entry point for a calibration evaluator.""" + +from __future__ import annotations + +import importlib +import importlib.util +import json +import os +import sys +import traceback +from collections.abc import Callable +from pathlib import Path +from typing import Any + + +def _load_evaluator(target: str) -> Callable[[dict[str, Any], dict[str, Any]], Any]: + module_or_path, attribute = target.rsplit(":", maxsplit=1) + source_path = Path(module_or_path) + if source_path.is_file(): + module_name = f"_embodichain_calibration_{os.getpid()}" + spec = importlib.util.spec_from_file_location(module_name, source_path) + if spec is None or spec.loader is None: + raise ImportError(f"cannot load evaluator module from {source_path}") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + else: + module = importlib.import_module(module_or_path) + evaluator = getattr(module, attribute) + if not callable(evaluator): + raise TypeError(f"evaluator target {target!r} is not callable") + return evaluator + + +def _write_result(path: Path, payload: dict[str, Any]) -> None: + path.write_text( + json.dumps(payload, allow_nan=True, sort_keys=True), + encoding="utf-8", + ) + + +def main(argv: list[str] | None = None) -> None: + """Execute one evaluator and terminate without simulator teardown hooks. + + Args: + argv: Input and output JSON paths. Uses process arguments when omitted. + + Raises: + SystemExit: When called directly with invalid arguments or when a test + invocation supplies ``argv`` and the evaluator fails. + """ + arguments = list(sys.argv[1:] if argv is None else argv) + if len(arguments) != 2: + raise SystemExit("worker expects INPUT_JSON OUTPUT_JSON") + input_path, output_path = map(Path, arguments) + exit_code = 0 + try: + request = json.loads(input_path.read_text(encoding="utf-8")) + evaluator = _load_evaluator(request["evaluator"]["target"]) + result = evaluator(request["overlay"], request["context"]) + if not isinstance(result, dict): + raise TypeError("evaluate() must return a dictionary") + _write_result(output_path, {"status": "ok", "result": result}) + except BaseException as error: # noqa: BLE001 - child must serialize all failures. + exit_code = 1 + _write_result( + output_path, + { + "status": "error", + "error": f"{type(error).__name__}: {error}", + "traceback": traceback.format_exc(), + }, + ) + if argv is None: + os._exit(exit_code) + if exit_code: + raise SystemExit(exit_code) + + +if __name__ == "__main__": + main() + + +__all__: list[str] = [] diff --git a/tests/sim/objects/test_articulation.py b/tests/sim/objects/test_articulation.py index 6b3288191..ae88b6f30 100644 --- a/tests/sim/objects/test_articulation.py +++ b/tests/sim/objects/test_articulation.py @@ -94,6 +94,31 @@ def test_get_qf_returns_all_articulation_joint_efforts(): assert torch.equal(actual_qf, expected_qf) +@pytest.mark.no_sim +def test_target_qvel_write_count_tracks_successful_api_calls() -> None: + """Velocity-target instrumentation counts calls but not current-state writes.""" + + class Entity: + def set_target_qvel(self, _qvel, _joint_ids) -> None: + pass + + def set_current_qvel(self, _qvel, _joint_ids) -> None: + pass + + articulation = object.__new__(Articulation) + articulation._entities = [Entity()] + articulation._all_indices = torch.arange(1, dtype=torch.int32) + articulation._data = SimpleNamespace(dof=2) + articulation.device = torch.device("cpu") + articulation._target_qvel_write_count = 0 + values = torch.zeros((1, 2), dtype=torch.float32) + + articulation.set_qvel(values, target=True) + articulation.set_qvel(values, target=False) + + assert articulation.target_qvel_write_count == 1 + + @pytest.mark.no_sim def test_compute_fk_reorders_named_qpos_into_kinematic_joint_order(): articulation = object.__new__(Articulation) diff --git a/tests/test_main.py b/tests/test_main.py index 82e7f4a28..e2bc9c327 100644 --- a/tests/test_main.py +++ b/tests/test_main.py @@ -30,6 +30,7 @@ "analyze-workspace", "annotate-grasp", "benchmark", + "calibrate-dynamics", "data", "decompose-urdf", "list-task", @@ -220,6 +221,21 @@ def test_list_task_help_explains_environment_only_label( assert "Environment Only" in capsys.readouterr().out +def test_calibrate_dynamics_help_exposes_v1_workflow( + capsys: pytest.CaptureFixture[str], +) -> None: + """The unified command advertises the three supported V1 stages.""" + with pytest.raises(SystemExit) as exc_info: + cli.main(["calibrate-dynamics", "--help"]) + + assert exc_info.value.code == 0 + output = capsys.readouterr().out + assert "audit" in output + assert "tune-drive" in output + assert "qualify" in output + assert "fit-physical" not in output + + def test_config_environment_entries_use_task_paths_and_artifacts( tmp_path: Path, ) -> None: diff --git a/tests/toolkits/assets/dynamics_calibration_pendulum.urdf b/tests/toolkits/assets/dynamics_calibration_pendulum.urdf new file mode 100644 index 000000000..d2e076244 --- /dev/null +++ b/tests/toolkits/assets/dynamics_calibration_pendulum.urdf @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tests/toolkits/test_dynamics_calibration.py b/tests/toolkits/test_dynamics_calibration.py new file mode 100644 index 000000000..dca5dde58 --- /dev/null +++ b/tests/toolkits/test_dynamics_calibration.py @@ -0,0 +1,514 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import json +from pathlib import Path + +import numpy as np +import pytest +import yaml + +import embodichain.toolkits.dynamics_calibration.cli as calibration_cli +from embodichain.toolkits.dynamics_calibration import ( + CalibrationConfig, + EvaluationError, + QualificationThresholds, + build_drive_overlay, + compute_tracking_metrics, + load_overlay, + qualify, + resolve_control_schedule, + run_candidate, + tune_drive, + write_overlay, +) + + +def _write_asset(path: Path) -> Path: + path.write_text( + """\ + + + + + + + + +""", + encoding="utf-8", + ) + return path + + +def _config_dict(asset: Path, evaluator_target: str) -> dict[str, object]: + return { + "schema_version": 1, + "assets": [str(asset)], + "backend": "default", + "physics_dt": 1.0 / 240.0, + "control_frequency_hz": 60.0, + "seed": 7, + "candidate_count": 5, + "evaluator": { + "target": evaluator_target, + "timeout_seconds": 5.0, + "payload": {}, + }, + "parameters": [ + { + "name": "arm_stiffness", + "field": "stiffness", + "selector": "arm", + "lower": 0.0, + "upper": 10.0, + "initial": 0.0, + } + ], + "qualification": { + "aggregate_rmse_max": 0.25, + "per_joint_rmse_max": 0.25, + "control_frequency_relative_error_max": 0.0, + }, + } + + +def _write_evaluator(path: Path, body: str) -> str: + path.write_text(body, encoding="utf-8") + return f"{path}:evaluate" + + +def test_control_frequency_requires_an_exact_physics_schedule() -> None: + """Control timing cannot silently round to a different frequency.""" + schedule = resolve_control_schedule(physics_dt=1.0 / 240.0, requested_hz=60.0) + + assert schedule.physics_steps_per_control == 4 + assert schedule.actual_hz == pytest.approx(60.0) + + with pytest.raises(ValueError, match="cannot be represented"): + resolve_control_schedule(physics_dt=1.0 / 240.0, requested_hz=100.0) + + approximate = resolve_control_schedule( + physics_dt=1.0 / 240.0, + requested_hz=100.0, + allow_approximate=True, + ) + assert approximate.physics_steps_per_control == 2 + assert approximate.actual_hz == pytest.approx(120.0) + + closer_ceiling = resolve_control_schedule( + physics_dt=1.0 / 240.0, + requested_hz=96.0, + allow_approximate=True, + ) + assert closer_ceiling.physics_steps_per_control == 3 + assert closer_ceiling.actual_hz == pytest.approx(80.0) + + +def test_configuration_rejects_values_that_yaml_would_otherwise_coerce( + tmp_path: Path, +) -> None: + """Boolean strings, fractional counts, and malformed parameters fail early.""" + asset = _write_asset(tmp_path / "robot.urdf") + data = _config_dict(asset, "example.module:evaluate") + data["allow_approximate_control_frequency"] = "false" + with pytest.raises(TypeError, match="must be a boolean"): + CalibrationConfig.from_dict(data, base_dir=tmp_path) + + data = _config_dict(asset, "example.module:evaluate") + data["candidate_count"] = 1.5 + with pytest.raises(TypeError, match="must be an integer"): + CalibrationConfig.from_dict(data, base_dir=tmp_path) + + data = _config_dict(asset, "example.module:evaluate") + data["parameters"] = ["not-a-mapping"] + with pytest.raises(TypeError, match=r"parameters\[0\] must be a mapping"): + CalibrationConfig.from_dict(data, base_dir=tmp_path) + + +def test_one_bad_joint_fails_even_when_aggregate_gate_passes() -> None: + """Per-joint hard gates prevent aggregate metrics from hiding failure.""" + target = np.zeros((100, 10), dtype=np.float64) + actual = target.copy() + actual[:, -1] = 1.0 + metrics = compute_tracking_metrics( + { + "joint_names": [f"j{index}" for index in range(10)], + "target_qpos": target.tolist(), + "actual_qpos": actual.tolist(), + "requested_control_hz": 60.0, + "actual_control_hz": 60.0, + "target_qvel_write_count": 0, + } + ) + + result = qualify( + metrics, + QualificationThresholds( + aggregate_rmse_max=0.5, + per_joint_rmse_max=0.5, + control_frequency_relative_error_max=0.0, + ), + ) + + assert metrics.aggregate_rmse < 0.5 + assert result.status == "fail" + gate = next(item for item in result.gates if item.name == "per_joint_rmse") + assert not gate.passed + assert gate.entity == "j9" + + +def test_control_group_metrics_and_nonfinite_results_remain_hard_failures() -> None: + """Group regressions and divergence remain explicit and JSON serializable.""" + metrics = compute_tracking_metrics( + { + "joint_names": ["left", "right"], + "target_qpos": [[0.0, 0.0], [0.0, 0.0]], + "actual_qpos": [[0.0, float("nan")], [0.0, 1.0]], + "control_groups": {"arm": ["left", "right"]}, + "requested_control_hz": 60.0, + "actual_control_hz": 60.0, + "target_qvel_write_count": 0, + } + ) + result = qualify( + metrics, + QualificationThresholds(per_control_group_rmse_max=0.5), + ) + + assert not metrics.stable + assert result.status == "fail" + assert ( + next( + gate for gate in result.gates if gate.name == "per_control_group_rmse" + ).entity + == "arm" + ) + json.dumps(metrics.to_dict(), allow_nan=False) + + +def test_optional_application_metrics_and_velocity_saturation_are_gated() -> None: + """Optional observations use the same strict, centralized hard-gate path.""" + metrics = compute_tracking_metrics( + { + "joint_names": ["joint"], + "target_qpos": [[0.0], [0.1]], + "actual_qpos": [[0.0], [0.1]], + "qvel": [[0.9], [1.0]], + "qvel_limits": [1.0], + "overshoot": 0.2, + "settling_time_seconds": 0.4, + "requested_control_hz": 60.0, + "actual_control_hz": 60.0, + "target_qvel_write_count": 0, + } + ) + + result = qualify( + metrics, + QualificationThresholds( + velocity_saturation_fraction_max=0.25, + overshoot_max=0.1, + settling_time_seconds_max=0.5, + ), + ) + + assert metrics.velocity_saturation_fraction == pytest.approx(0.5) + assert result.status == "fail" + assert {gate.name: gate.passed for gate in result.gates}["settling_time_seconds"] + + +def test_missing_qvel_instrumentation_is_not_assumed_to_be_zero() -> None: + """The default qpos-only gate needs evidence, not an omitted field.""" + metrics = compute_tracking_metrics( + { + "joint_names": ["joint"], + "target_qpos": [[0.0]], + "actual_qpos": [[0.0]], + "requested_control_hz": 60.0, + "actual_control_hz": 60.0, + } + ) + + result = qualify(metrics, QualificationThresholds()) + + gate = next(item for item in result.gates if item.name == "target_qvel_write_count") + assert metrics.target_qvel_write_count is None + assert gate.observed is None + assert not gate.passed + assert result.status == "fail" + + +def test_overlay_round_trip_does_not_modify_source_asset(tmp_path: Path) -> None: + """A tuned candidate is emitted as a reviewable overlay only.""" + asset = _write_asset(tmp_path / "robot.urdf") + source_before = asset.read_bytes() + config = CalibrationConfig.from_dict( + _config_dict(asset, "example.module:evaluate"), + base_dir=tmp_path, + ) + overlay = build_drive_overlay(config, {"arm_stiffness": 5.0}) + output = tmp_path / "drive_overlay.yaml" + + write_overlay(output, overlay) + + assert load_overlay(output) == overlay + assert yaml.safe_load(output.read_text(encoding="utf-8"))["drive_pros"] == { + "stiffness": {"arm": 5.0} + } + assert asset.read_bytes() == source_before + + +def test_worker_exception_and_timeout_are_not_reported_as_success( + tmp_path: Path, +) -> None: + """Candidate infrastructure failures propagate instead of becoming metrics.""" + asset = _write_asset(tmp_path / "robot.urdf") + raising_target = _write_evaluator( + tmp_path / "raising.py", + "def evaluate(overlay, context):\n raise RuntimeError('boom')\n", + ) + config = CalibrationConfig.from_dict( + _config_dict(asset, raising_target), base_dir=tmp_path + ) + + with pytest.raises(EvaluationError, match="RuntimeError: boom"): + run_candidate( + config.evaluator, + build_drive_overlay(config, {"arm_stiffness": 1.0}), + config.evaluation_context("training"), + cache_dir=tmp_path / "cache", + ) + + sleeping_target = _write_evaluator( + tmp_path / "sleeping.py", + "import time\ndef evaluate(overlay, context):\n time.sleep(1)\n return {}\n", + ) + timeout_data = _config_dict(asset, sleeping_target) + timeout_data["evaluator"]["timeout_seconds"] = 0.05 # type: ignore[index] + timeout_config = CalibrationConfig.from_dict(timeout_data, base_dir=tmp_path) + with pytest.raises(EvaluationError, match="timed out"): + run_candidate( + timeout_config.evaluator, + build_drive_overlay(timeout_config, {"arm_stiffness": 1.0}), + timeout_config.evaluation_context("training"), + cache_dir=tmp_path / "timeout-cache", + ) + + +def test_tuning_improves_a_known_synthetic_perturbation(tmp_path: Path) -> None: + """The deterministic search recovers a held-out synthetic drive optimum.""" + asset = _write_asset(tmp_path / "robot.urdf") + evaluator_target = _write_evaluator( + tmp_path / "synthetic.py", + """\ +def evaluate(overlay, context): + value = overlay["drive_pros"]["stiffness"]["arm"] + error = abs(value - 5.0) / 5.0 + target = [[0.0], [0.0], [0.0], [0.0]] + return { + "joint_names": ["joint"], + "target_qpos": target, + "actual_qpos": [[error] for _ in target], + "requested_control_hz": context["requested_control_hz"], + "actual_control_hz": context["actual_control_hz"], + "target_qvel_write_count": 0, + } +""", + ) + config = CalibrationConfig.from_dict( + _config_dict(asset, evaluator_target), base_dir=tmp_path + ) + + result = tune_drive(config, cache_dir=tmp_path / "cache") + + assert result.best_candidate == {"arm_stiffness": 5.0} + assert result.best_objective < result.baseline_objective * 0.1 + repeated = tune_drive(config, cache_dir=tmp_path / "cache") + assert [trial.candidate for trial in repeated.trials] == [ + trial.candidate for trial in result.trials + ] + assert repeated.best_candidate == result.best_candidate + assert all(trial.cache_hit for trial in repeated.trials) + held_out = run_candidate( + config.evaluator, + result.overlay, + config.evaluation_context("qualification"), + cache_dir=tmp_path / "cache", + ) + assert qualify(held_out.metrics, config.qualification).status == "pass" + + +def test_candidate_cache_is_keyed_by_inputs(tmp_path: Path) -> None: + """An identical asset/config/candidate reuses its isolated result.""" + asset = _write_asset(tmp_path / "robot.urdf") + evaluator_target = _write_evaluator( + tmp_path / "constant.py", + """\ +def evaluate(overlay, context): + return { + "joint_names": ["joint"], + "target_qpos": [[0.0]], + "actual_qpos": [[0.0]], + "requested_control_hz": context["requested_control_hz"], + "actual_control_hz": context["actual_control_hz"], + "target_qvel_write_count": 0, + } +""", + ) + config = CalibrationConfig.from_dict( + _config_dict(asset, evaluator_target), base_dir=tmp_path + ) + overlay = build_drive_overlay(config, {"arm_stiffness": 5.0}) + cache_dir = tmp_path / "cache" + + first = run_candidate( + config.evaluator, + overlay, + config.evaluation_context("training"), + cache_dir=cache_dir, + ) + second = run_candidate( + config.evaluator, + overlay, + config.evaluation_context("training"), + cache_dir=cache_dir, + ) + + assert not first.cache_hit + assert second.cache_hit + assert first.metrics.to_dict() == second.metrics.to_dict() + assert len(list(cache_dir.glob("*.json"))) == 1 + + +def test_cli_writes_overlay_and_complete_qualification_report( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + """The public workflow joins audit, search, held-out gates, and artifacts.""" + asset = _write_asset(tmp_path / "robot.urdf") + evaluator_target = _write_evaluator( + tmp_path / "synthetic.py", + """\ +def evaluate(overlay, context): + value = overlay["drive_pros"]["stiffness"]["arm"] + error = abs(value - 5.0) / 5.0 + return { + "joint_names": ["joint"], + "target_qpos": [[0.0], [0.0]], + "actual_qpos": [[error], [error]], + "requested_control_hz": context["requested_control_hz"], + "actual_control_hz": context["actual_control_hz"], + "target_qvel_write_count": 0, + "metadata": {"phase": context["phase"]}, + } +""", + ) + config_path = tmp_path / "calibration.yaml" + config_path.write_text( + yaml.safe_dump(_config_dict(asset, evaluator_target)), encoding="utf-8" + ) + + class ReviewAudit: + ready = True + status = "review" + + def to_dict(self) -> dict[str, object]: + return { + "status": "review", + "source": str(asset), + "asset_sha256": "test-digest", + } + + def to_markdown(self) -> str: + return "# review audit\n" + + monkeypatch.setattr( + calibration_cli, + "audit_assets", + lambda *_args, **_kwargs: (ReviewAudit(),), + ) + output_dir = tmp_path / "output" + + calibration_cli.main( + ["tune-drive", "--config", str(config_path), "--output-dir", str(output_dir)] + ) + + overlay = load_overlay(output_dir / "drive_overlay.yaml") + report = json.loads((output_dir / "report.json").read_text(encoding="utf-8")) + assert overlay["drive_pros"] == {"stiffness": {"arm": 5.0}} + assert report["status"] == "review" + assert report["claim"] == "effective_drive_tuning" + assert report["qualification_evaluation"]["evaluator_metadata"] == { + "phase": "qualification" + } + assert "does not claim" in (output_dir / "report.md").read_text(encoding="utf-8") + + +@pytest.mark.slow +@pytest.mark.requires_sim +@pytest.mark.subprocess_sim +def test_builtin_tracking_evaluator_runs_in_a_real_isolated_simulator( + tmp_path: Path, +) -> None: + """The shipped evaluator drives qpos only and returns simulator evidence.""" + asset = Path(__file__).parent / "assets" / "dynamics_calibration_pendulum.urdf" + data = _config_dict( + asset, + "embodichain.toolkits.dynamics_calibration.tracking_evaluator:evaluate", + ) + data["candidate_count"] = 1 + data["evaluator"] = { + "target": "embodichain.toolkits.dynamics_calibration.tracking_evaluator:evaluate", + "timeout_seconds": 30.0, + "payload": { + "control_part": "arm", + "robot_cfg": {"control_parts": {"arm": ["joint1"]}}, + "trajectory": { + "duration_seconds": 0.05, + "warmup_seconds": 0.0, + "amplitude": 0.05, + "frequencies_hz": 1.0, + }, + }, + } + data["parameters"] = [ + { + "name": "arm_stiffness", + "field": "stiffness", + "selector": "arm", + "lower": 10.0, + "upper": 100.0, + "initial": 50.0, + } + ] + config = CalibrationConfig.from_dict(data, base_dir=tmp_path) + + evaluation = run_candidate( + config.evaluator, + build_drive_overlay(config, {"arm_stiffness": 50.0}), + config.evaluation_context("training"), + cache_dir=tmp_path / "cache", + ) + + assert evaluation.metrics.sample_count == 3 + assert evaluation.metrics.stable + assert evaluation.metrics.velocity_saturation_fraction is not None + assert evaluation.metrics.target_qvel_write_count == 0 + assert evaluation.metadata["target_qvel_instrumentation"] == ( + "Articulation.set_qvel" + )