124 lines
4.4 KiB
Python
124 lines
4.4 KiB
Python
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
from typing import Any
|
|
|
|
import numpy as np
|
|
|
|
|
|
@dataclass
|
|
class LoadedPolicy:
|
|
key: str
|
|
action_scale: float
|
|
backend: str
|
|
_session: Any | None = None
|
|
_input_name: str = "obs"
|
|
_output_name: str = "actions"
|
|
|
|
def infer(self, obs_61: np.ndarray) -> np.ndarray:
|
|
if obs_61.shape != (61,):
|
|
raise ValueError(f"Expected 61D observation, got {obs_61.shape}")
|
|
if self.backend == "dummy":
|
|
# Deterministic zero-action dummy policy for PC dry-run without onnxruntime.
|
|
return np.zeros(14, dtype=np.float32)
|
|
|
|
inp = np.asarray(obs_61, dtype=np.float32).reshape(1, 61)
|
|
out = self._session.run([self._output_name], {self._input_name: inp})[0]
|
|
act = np.asarray(out, dtype=np.float32).reshape(14)
|
|
if not np.all(np.isfinite(act)):
|
|
raise ValueError(f"Policy '{self.key}' returned non-finite action")
|
|
return act
|
|
|
|
|
|
class PolicySet:
|
|
def __init__(self, policies: dict[str, LoadedPolicy], backend: str) -> None:
|
|
self.policies = policies
|
|
self.backend = backend
|
|
|
|
@classmethod
|
|
def resolve_backend(cls, requested: str) -> str:
|
|
if requested in ("onnx", "dummy"):
|
|
return requested
|
|
if requested != "auto":
|
|
raise ValueError(f"Unknown policy backend: {requested}")
|
|
try:
|
|
import onnxruntime # noqa: F401
|
|
|
|
return "onnx"
|
|
except ImportError as exc:
|
|
raise RuntimeError(
|
|
"onnxruntime is not installed. Run 'bash scripts/install_deps.sh' on UNO Q, "
|
|
"or pass '--policy-backend dummy' for pre-hardware UI/protocol dry-run."
|
|
) from exc
|
|
|
|
@classmethod
|
|
def load_from_manifest(
|
|
cls,
|
|
base_dir: str | Path,
|
|
manifest: dict[str, Any],
|
|
backend: str = "auto",
|
|
) -> "PolicySet":
|
|
resolved_backend = cls.resolve_backend(backend)
|
|
ort = None
|
|
if resolved_backend == "onnx":
|
|
try:
|
|
import onnxruntime as ort_mod
|
|
|
|
ort = ort_mod
|
|
except ImportError as exc:
|
|
raise RuntimeError("onnxruntime is required for backend='onnx'") from exc
|
|
|
|
root = Path(base_dir)
|
|
default_scale = float(manifest.get("action_scale", 1.0))
|
|
skills = manifest.get("skills", {})
|
|
loaded: dict[str, LoadedPolicy] = {}
|
|
|
|
for key, spec in skills.items():
|
|
rel_path = spec.get("path")
|
|
if not rel_path:
|
|
continue
|
|
scale = float(spec.get("action_scale", default_scale))
|
|
if resolved_backend == "dummy":
|
|
loaded[key] = LoadedPolicy(key=key, action_scale=scale, backend="dummy")
|
|
continue
|
|
|
|
model_path = (root / rel_path).resolve()
|
|
if not model_path.is_file():
|
|
raise FileNotFoundError(f"Missing ONNX policy for '{key}': {model_path}")
|
|
|
|
sess_opts = ort.SessionOptions()
|
|
sess_opts.intra_op_num_threads = 2
|
|
sess = ort.InferenceSession(
|
|
str(model_path),
|
|
sess_options=sess_opts,
|
|
providers=["CPUExecutionProvider"],
|
|
)
|
|
inps = sess.get_inputs()
|
|
outs = sess.get_outputs()
|
|
if len(inps) != 1 or len(outs) != 1:
|
|
raise ValueError(f"Policy '{key}' must have 1 input / 1 output")
|
|
|
|
# Smoke validation mirroring custom-policy.js
|
|
smoke_obs = np.zeros((1, 61), dtype=np.float32)
|
|
smoke_out = sess.run([outs[0].name], {inps[0].name: smoke_obs})[0]
|
|
if tuple(np.asarray(smoke_out).shape) != (1, 14):
|
|
raise ValueError(
|
|
f"Policy '{key}' output shape {np.asarray(smoke_out).shape} != (1, 14)"
|
|
)
|
|
if not np.all(np.isfinite(smoke_out)):
|
|
raise ValueError(f"Policy '{key}' smoke output is non-finite")
|
|
|
|
loaded[key] = LoadedPolicy(
|
|
key=key,
|
|
action_scale=scale,
|
|
backend="onnx",
|
|
_session=sess,
|
|
_input_name=inps[0].name,
|
|
_output_name=outs[0].name,
|
|
)
|
|
|
|
if "walk" not in loaded or "stand" not in loaded:
|
|
raise ValueError("PolicySet must load at least 'walk' and 'stand'")
|
|
return cls(policies=loaded, backend=resolved_backend)
|