183 lines
8.7 KiB
Python
183 lines
8.7 KiB
Python
"""Re-run recorded native acceptance inputs against the application it ships in."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import json
|
|
from pathlib import Path
|
|
|
|
import mujoco
|
|
import numpy as np
|
|
|
|
from mjrl.controllers.model import finite
|
|
|
|
from .bundle import open_bundle
|
|
from .endpoint import compare_endpoint, measure_endpoints, parse_endpoints
|
|
from .play import RobotApplication
|
|
from .velocity import VelocityWindow
|
|
|
|
|
|
def _reference_root(app: RobotApplication, evidence: dict) -> int:
|
|
name = evidence["root_body"]
|
|
root = app.model.body(name).id if name else evidence["root_body_id"]
|
|
if (
|
|
type(root) is not int
|
|
or not 0 < root < app.model.nbody
|
|
or app.model.body_parentid[root] != 0
|
|
):
|
|
raise ValueError("invalid root body for physical response reference")
|
|
return root
|
|
|
|
|
|
def _velocity_window(app: RobotApplication, probe: dict, steps: int) -> VelocityWindow | None:
|
|
evidence = probe.get("velocity_evaluation")
|
|
if evidence is None:
|
|
return None
|
|
if evidence["frame"] != "body":
|
|
raise ValueError("invalid frame for velocity reference")
|
|
specs = {
|
|
field: {"target": result["expected"], "tolerance": result["tolerance"]}
|
|
for field, result in evidence["metrics"].items()
|
|
}
|
|
return VelocityWindow(
|
|
_reference_root(app, evidence),
|
|
evidence["start_s"],
|
|
steps * app.model.opt.timestep,
|
|
specs,
|
|
)
|
|
|
|
|
|
def _replay_endpoints(app: RobotApplication, root: int, initial: np.ndarray, evidence: dict):
|
|
metrics = evidence["metrics"]
|
|
expected = parse_endpoints({name: entry["expected"] for name, entry in metrics.items()})
|
|
recorded = parse_endpoints({name: entry["observed"] for name, entry in metrics.items()})
|
|
if not expected or expected.keys() != metrics.keys():
|
|
raise ValueError("endpoint reference has no supported physical response metrics")
|
|
observed = measure_endpoints(app.model, app.data, root, expected, initial)
|
|
results, parity_errors = {}, {}
|
|
for field, target in expected.items():
|
|
if isinstance(target, dict) and target.keys() != recorded[field].keys():
|
|
raise ValueError(f"endpoint reference has inconsistent object names for {field}")
|
|
error, target_value, observed_value = compare_endpoint(target, observed[field])
|
|
tolerance = finite(metrics[field]["tolerance"], f"{field}.tolerance")
|
|
if tolerance < 0:
|
|
raise ValueError(f"{field}.tolerance must be nonnegative")
|
|
parity_errors[field] = compare_endpoint(recorded[field], observed[field])[0]
|
|
results[field] = {
|
|
"expected": target_value,
|
|
"observed": observed_value,
|
|
"error": error,
|
|
"tolerance": tolerance,
|
|
"passed": error <= tolerance,
|
|
}
|
|
return results, parity_errors
|
|
|
|
|
|
def check_reference(path: Path, *, tolerance: float = 1e-6) -> dict:
|
|
if not np.isfinite(tolerance) or tolerance < 0:
|
|
raise ValueError("reference tolerance must be finite and nonnegative")
|
|
results = []
|
|
with open_bundle(path) as (root, manifest):
|
|
reference = json.loads((root / manifest["reference"]).read_text())
|
|
modes = reference.get("modes", {})
|
|
expected_modes = {
|
|
name
|
|
for name, entry in manifest["modes"].items()
|
|
if entry["control_mode"] != "model_viewer"
|
|
}
|
|
if set(modes) != expected_modes:
|
|
raise ValueError("reference must cover every controlled mode exactly")
|
|
for name, probes in modes.items():
|
|
app = RobotApplication(root, manifest, name)
|
|
if not probes:
|
|
raise ValueError(f"mode {name} has no recorded probes")
|
|
for probe in probes:
|
|
steps = probe["steps"]
|
|
if type(steps) is not int or not 1 <= steps <= 200000:
|
|
raise ValueError("invalid reference step count")
|
|
if probe["controller_substeps"] != app.substeps:
|
|
raise ValueError("reference control rate differs from the application")
|
|
samples = {sample["physics_step"]: sample for sample in probe.get("samples", [])}
|
|
if 0 not in samples or steps not in samples:
|
|
raise ValueError("reference must include initial and final measured samples")
|
|
app.reset()
|
|
inputs = probe["inputs"]
|
|
if set(inputs) - {"command", "pad", "keys"}:
|
|
raise ValueError("application reference expects a controller input")
|
|
errors = {key: 0.0 for key in ("qpos", "qvel", "ctrl", "actuator_force", "time")}
|
|
window = _velocity_window(app, probe, steps)
|
|
endpoint = probe.get("endpoint_response")
|
|
if endpoint is not None:
|
|
endpoint_root = _reference_root(app, endpoint)
|
|
endpoint_start = app.data.xpos[endpoint_root].copy()
|
|
for index in range(steps + 1):
|
|
if index in samples:
|
|
sample = samples[index]
|
|
for field in errors:
|
|
observed = np.asarray(getattr(app.data, field))
|
|
expected = np.asarray(sample[field])
|
|
if (
|
|
observed.shape != expected.shape
|
|
or not np.isfinite(expected).all()
|
|
or not np.isfinite(observed).all()
|
|
):
|
|
raise ValueError(
|
|
f"invalid reference {name}/{field} shape or values"
|
|
)
|
|
if observed.size:
|
|
errors[field] = max(
|
|
errors[field], float(np.max(np.abs(expected - observed)))
|
|
)
|
|
if index == steps:
|
|
break
|
|
if index % app.substeps == 0:
|
|
app.controller.step(
|
|
app.data,
|
|
inputs.get("command"),
|
|
**{key: inputs[key] for key in ("pad", "keys") if key in inputs},
|
|
)
|
|
if hasattr(app.controller, "apply"):
|
|
app.controller.apply(app.data)
|
|
mujoco.mj_step(app.model, app.data)
|
|
mujoco.mj_forward(app.model, app.data)
|
|
if any(warning.number for warning in app.data.warning):
|
|
raise ValueError("MuJoCo warning while replaying reference")
|
|
if window is not None:
|
|
window.sample(app.model, app.data, (index + 1) * app.model.opt.timestep)
|
|
measured = {"mode": name, "probe": probe["index"], "max_absolute_error": errors}
|
|
endpoint_passed = True
|
|
if endpoint is not None:
|
|
metrics, parity = _replay_endpoints(
|
|
app, endpoint_root, endpoint_start, endpoint
|
|
)
|
|
errors.update(parity)
|
|
endpoint_passed = all(result["passed"] for result in metrics.values())
|
|
measured["endpoint_response"] = {**endpoint, "metrics": metrics}
|
|
window_passed = True
|
|
if window is not None:
|
|
evidence = probe["velocity_evaluation"]
|
|
if evidence["physics_samples"] != window.count:
|
|
raise ValueError("velocity reference sample count differs from replay")
|
|
metrics = window.results()
|
|
for field, result in metrics.items():
|
|
expected = np.asarray(evidence["metrics"][field]["observed"], dtype=float)
|
|
if expected.shape != (3,) or not np.isfinite(expected).all():
|
|
raise ValueError(f"invalid reference {name}/{field} shape or values")
|
|
errors[field] = float(np.max(np.abs(expected - result["observed"])))
|
|
window_passed = all(result["passed"] for result in metrics.values())
|
|
measured["velocity_evaluation"] = {**evidence, "metrics": metrics}
|
|
results.append(
|
|
{
|
|
**measured,
|
|
"passed": endpoint_passed
|
|
and window_passed
|
|
and all(error <= tolerance for error in errors.values()),
|
|
}
|
|
)
|
|
return {
|
|
"schema": "mjrl-app-reference-check/1",
|
|
"passed": all(item["passed"] for item in results),
|
|
"tolerance": tolerance,
|
|
"probes": results,
|
|
"capability": "control_reference" if results else "model_viewer",
|
|
}
|