diff --git a/.gitignore b/.gitignore index a35ae6e..546de0c 100644 --- a/.gitignore +++ b/.gitignore @@ -70,4 +70,17 @@ experiments/ *.tar *.rar *.7z -*.mp4 \ No newline at end of file +*.mp4 + +# Preserved on exploratory/v2.2-safety-investigation; do not let local +# certificate-investigation outputs leak into the focused model consumer. +vault/data/articulated_fcert_residuals_v2_2.npz* +vault/data/ssb_eval_odd_grid_v2_2_local_residual.npz* +vault/data/ssb_eval_odd_grid_v2_2_mean_corrected.npz* +vault/diagnostics/ + +# PUBLIC REPO: the controller lock and all model-derived artifacts live in the +# PRIVATE vault-controller sibling. Only sha256 pins and hash sidecars are tracked. +vault/data/MODEL_INPUTS.lock.json +vault/data/grid_reachavoid_odd.npz +vault/docs/*.pdf diff --git a/CODEOWNERS b/CODEOWNERS new file mode 100644 index 0000000..63a3b83 --- /dev/null +++ b/CODEOWNERS @@ -0,0 +1,59 @@ +# Ownership boundary for the vault safety package. +# +# This repository holds two layers with different authorities, and the split is +# currently only visible through `git log`. Writing it down is the point of this +# file: the reach-avoid safety-filter mathematics is Jaime's domain, and the +# four-state model and its contracts are Robert's. Changes should respect that +# regardless of who is holding the keyboard. +# +# The rule that matters: +# +# * MODEL/CONTRACT changes are ANNOUNCED to the math layer's owner before +# landing. They move the ground the math stands on -- a corrected bound or a +# widened ODD changes results without changing a line of the math. +# * MATH-LAYER changes are the math owner's call. A defect found there is +# reported with analysis, not silently fixed. +# +# Everything below is descriptive of the actual authorship (see `git log --reverse` +# on each file), not an aspiration. + +# --------------------------------------------------------------------------- +# Reach-avoid safety-filter mathematics -- Jaime Fernandez Fisac +# Introduced in 2c4f73d (2026-07-24). +# --------------------------------------------------------------------------- +/vault/reach_avoid_sac.py @jfisac +/vault/reach_avoid_value.py @jfisac +/vault/reach_avoid_eval.py @jfisac +/vault/reach_avoid_slice.py @jfisac +/vault/train_reach_avoid.py @jfisac +/vault/safety_filter.py @jfisac +/vault/tests/test_drabe_operator.py @jfisac + +# --------------------------------------------------------------------------- +# Upstream avoid-only SAC -- Haimin Hu, vendored. +# The entropy-bonus defect documented in reach_avoid_sac.py originates here, not +# in the reach-avoid extension. +# --------------------------------------------------------------------------- +/safety_sb3/ @haiminhu + +# --------------------------------------------------------------------------- +# Four-state model, certified plant, and contracts -- Robert Shi. +# Introduced in 4340e3b (2026-06-25) as the "4-state balance-safety package". +# These define what the math layer is reasoning ABOUT. +# --------------------------------------------------------------------------- +/vault/f_cert.py @rshi159 +/vault/grid.py @rshi159 +/vault/filter.py @rshi159 +/vault/distill.py @rshi159 +/vault/config.py @rshi159 +/vault/dynamics.py @rshi159 +/vault/env.py @rshi159 +/vault/model_release.py @rshi159 +/vault/mujoco_*.py @rshi159 +/vault/tools/ @rshi159 +/vault/data/ @rshi159 + +# The ODD contract, the roll constraint and the generated model artifacts live in +# the vault-controller repository and are pinned here by MODEL_INPUTS.lock.json. +# They are the model layer's authority; this repo consumes them and must never +# restate their values as literals. diff --git a/vault/CLAUDE.md b/vault/CLAUDE.md index 5329ed0..ab606ed 100644 --- a/vault/CLAUDE.md +++ b/vault/CLAUDE.md @@ -5,12 +5,18 @@ Certifiable reach-avoid safety for the Vault robot's balance subsystem, over the value filter + the SafetySAC RL env + a MuJoCo plant for adversarial RL / ISAACS. ## Scope / boundaries -- This package shares ONLY the 4-state reduced dynamics + the safety method + the MuJoCo balance - plant. It must NOT grow a dependency on the Vault hybrid simulator, the Julia/symbolic framework, - or the full robot model. -- The reduced dynamics live in `dynamics.py` (the vendored opt6 kernel `csrc/reduced_opt6.c`, - geometry baked in). `f_cert.py` is the ONE source of the certified one-step model + margins — - the env, the grid solver, and the filter all call it. Don't fork the dynamics. +- This package shares ONLY the 4-state reduced dynamics + the safety method + + the MuJoCo balance plant. It consumes the pinned model release from the + sibling `vault-controller` checkout and must NOT grow a dependency on the + Vault hybrid simulator, Julia/symbolic framework, or full robot model. +- `model_release.py` locates `vault-controller` through + `VAULT_CONTROLLER_ROOT` (default `../vault-controller`), requires this + package's `data/MODEL_INPUTS.lock.json` to byte-match the release lock, and + hash-checks artifacts on first access. +- The reduced dynamics live in `dynamics.py`, which compiles the release's + single opt6 kernel. `f_cert.py` is the ONE source of the one-step model + + margins; the env, grid solver, and filter all call it. Don't fork the + dynamics or vendor another kernel. - The deployed controller is NOT here — it's the separate private `vault-controller` repo. `evaluate.py` imports it and fails loud if absent (no fallback controller, by design). @@ -19,13 +25,15 @@ Run as modules from the repo root: `python -m vault.`. - `python -m vault.grid` regenerate the value function -> `data/grid_reachavoid_odd.npz` - `python -m vault.distill` conservative `V_mlp` from the grid -> `models/` - `python -m vault.train` SafetySAC reach-avoid V + pi_safe -- `python -m vault.evaluate` filter on the deployed controller (needs `vault-controller`) -The reduced-dynamics kernel compiles on first import (needs `cc`/`gcc`); it is cached. +- `python -m vault.evaluate` filter using the controller source checkout +The reduced-dynamics kernel compiles on first artifact use (needs `cc`/`gcc`); +it is cached. ## Conventions - Python, Google style. State `x=[v,theta,theta_dot,psi_dot]`; control `u=[tau_L,tau_R]` (N·m); `mu` = friction. The value function is mu-aware. -- Constants live in `config.py` — import from there, don't hard-code params or ODD bounds. +- Import runtime constants through `config.py`. Robot and ODD values originate + in the hash-locked controller release; do not hard-code or duplicate them. - Keep modules importable without heavy deps unless used (torch only in distill/filter/train; mujoco only in mujoco_plant/evaluate). diff --git a/vault/KNOWN_LIMITATIONS.md b/vault/KNOWN_LIMITATIONS.md new file mode 100644 index 0000000..6310dab --- /dev/null +++ b/vault/KNOWN_LIMITATIONS.md @@ -0,0 +1,57 @@ +# Known limitations of the v2.2 consumer (read before trusting a demo run) + +## The joystick/teleop demo has no v2.2-valid monitor checkpoint + +`vault/teleop.py` is unmodified and its wiring works, but on the v2.2 release there +is no correct policy to give it. A partial run can easily look like a working demo, +so this is stated explicitly. + +- The default monitor, `contact_safety_sac_v3`, is quarantined by + `vault/data/checkpoints_manifest.json` as incompatible with this release. That + quarantine is **correct and pre-existing**: it is a v2.0/v2.1-era contact policy + that was never retrained against v2.2. `_dry_run` loads a policy + unconditionally, so with the default model the demo does not start at all. +- The only checkpoint authorized under this release, + `reach_avoid_safety_sac_v22`, is **not a substitute**: + 1. **Value semantics differ.** `ValueMonitor` (`vault/safety_filter.py`) reads the + critic as an avoid-only safety value. `ReachAvoidSafetySAC` + (`vault/reach_avoid_sac.py`) deliberately changes the backup to + `V(x) = min(g(x), max(l(x), V(x_next)))`, so reaching the target set "cashes + in" value. Those numbers do not mean the same thing. `teleop` loads via + `load_safety_sac` (the parent class); `load_reach_avoid_safety_sac` exists but + is unused here, and **nothing gates monitor class against checkpoint class**. + 2. It is **undertrained and evaluation-only** (300k steps; 61.7% reached / 17.3% + limbo / 21.0% failed at mu=0.8). It has never been claimed as a control policy. + +### Correction to commit 50d1c49's message + +That commit reports `teleop --dry-run 150` completing with "margins positive +throughout". That validated the **control-loop wiring only** and overstates what was +shown. Two corrections: + +- The run used `reach_avoid_safety_sac_v22` as the monitor, i.e. the semantic + mismatch above. The resulting "filter overrode 100% of steps" is an **artifact of + that mismatch** — it is not evidence the filter is working hard, nor that it is + broken. Do not cite that number in either direction. +- The margins stayed positive largely because the robot barely moves in the run's + 1.5 s window (150 steps x 0.01 s). With the filter disabled the same scripted + command only reaches about 0.05 m/s, so filtered-vs-unfiltered is a far smaller + difference than "override on every step" suggests. + +### What would fix it + +Operator/maintainer decision — replacement RL training is authorized-only and was +not performed here. Either retrain a contact/avoid-only monitor against v2.2, or +point `teleop` at a monitor whose value semantics match its checkpoint. Either way, +gating the monitor/checkpoint pairing would stop this mismatch recurring silently. + +Longer form, with the release-side context: `vault-controller` +`docs/ssb_handoff/CHANGES_TO_THE_MODEL_LAYER.md` section 15. + +## Not a limitation here: the missing opt6 MVE/batch4 entries + +The v2.2 released opt6 kernel lacks its Arm Helium (M55) vector entries. That is a +**firmware-side** gap and does not affect this repo: `vault/dynamics.py` binds only +the scalar `reduced_struct_fjac_f32` entry, whose numerics are unchanged, and +nothing here references `batch4`. Recorded so the question does not have to be +re-derived. diff --git a/vault/README.md b/vault/README.md index 627ece7..357c623 100644 --- a/vault/README.md +++ b/vault/README.md @@ -10,21 +10,22 @@ This is **one of two repos**: | repo | contents | |------|----------| | **this** (`safety-stable-baselines/vault`) | the safety method + RL + MuJoCo sim | -| **`vault-controller`** (private) | the deployed C iLQR balance controller — `evaluate.py` imports it | +| **`vault-controller`** (private) | authoritative v2.2 model release and C iLQR controller source | ## Module map | module | what it is | |--------|-----------| -| `config.py` | single source of robot params, the ODD, and disturbance bounds | -| `dynamics.py` (+ `csrc/`) | opt6 reduced 4-state dynamics — `f` + analytic Jacobians (vendored C kernel) | +| `model_release.py` | strict loader for the sibling controller release and model-input lock | +| `config.py` | lazy adapter for the release-backed robot and central ODD contract | +| `dynamics.py` | opt6 reduced dynamics compiled from the single kernel in `vault-controller` | | `f_cert.py` | the certified one-step model + ODD margins (one source for env/grid/filter) | | `grid.py` | 4D grid HJ reach-avoid value iteration — **regenerate the value function** | | `distill.py` | conservative deployable `V_mlp` distilled from the grid value | -| `filter.py` | least-restrictive CBF-QP value filter (`from_mlp` / `from_grid`) | +| `filter.py` | ODD grid filter (`from_grid`), release MLP, and explicit capability-grid loader | | `env.py` | `BalanceSafetyEnv` — fast f_cert reach-avoid RL env (SafetySAC) | | `train.py` | SafetySAC training (reach-avoid V + `pi_safe`) | | `mujoco_plant.py` (+ `mujoco_model.py`) | high-fidelity MuJoCo plant + adversary disturbance hook | -| `evaluate.py` | in-the-loop filter eval on the **deployed controller** (needs `vault-controller`) | +| `evaluate.py` | in-the-loop filter evaluation using the controller source checkout | ## Install Requires **Python 3.10+** and a **C compiler** (`cc`/`gcc`) on PATH — the reduced-dynamics kernel @@ -40,13 +41,29 @@ cd safety-stable-baselines pip install -e . # provides safety_sb3 + stable-baselines3 pip install -r vault/requirements.txt -# 3. verify the install (compiles the kernel, then a ~10s grid solve) +# 3. clone the exact controller model release beside this checkout +git clone ../vault-controller +git -C ../vault-controller checkout feature/v2.2-model-release +export VAULT_CONTROLLER_ROOT="$PWD/../vault-controller" + +# 4. verify the install (validates the lock, compiles the kernel, then solves a smoke grid) python -m vault.grid --smoke # should converge + print a safe-set % ``` -To run `evaluate.py` (the filter on the deployed controller), also clone + build **vault-controller**: +All model-dependent modules require the pinned `vault-controller` checkout. By +default it is located at `../vault-controller`; set `VAULT_CONTROLLER_ROOT` to +an alternate path. On first artifact access, `model_release.py` requires +`vault/data/MODEL_INPUTS.lock.json` to be byte-identical to +`vault-controller/models/MODEL_INPUTS.lock.json` and verifies each consumed +artifact hash. + +The ODD bounds, grid axes and resolutions, control bound, friction slices, and +disturbance assumptions are declared once in +`vault-controller/models/source/odd_contract.json`. This package reads them +through `config.py`; it does not maintain a second set of literals. + +To run `evaluate.py`, also build the controller library: ```bash -git clone ../vault-controller make -C ../vault-controller/balance_controller/c lib PYTHONPATH=$PWD:../vault-controller python -m vault.evaluate ``` @@ -58,7 +75,7 @@ Run everything as a module from the `safety-stable-baselines` repo root: `python python -m vault.grid # 4D HJ reach-avoid -> data/grid_reachavoid_odd.npz python -m vault.distill # conservative V_mlp -> models/v_mlp.{pt,json} python -m vault.train --steps 300000 # SafetySAC reach-avoid V + pi_safe -python -m vault.evaluate # filter on the deployed controller (needs vault-controller) +python -m vault.evaluate # filter using the pinned controller source checkout ``` `grid.py --smoke` runs a small foreground solve to sanity-check changes. The full grid solve writes the oracle the deployable net is verified conservative against (`{V_mlp≥0} ⊆ {V_grid≥0}`). @@ -73,23 +90,27 @@ the oracle the deployable net is verified conservative against (`{V_mlp≥0} ⊆ A typical ISAACS loop: ego controls `step(u)`, adversary sets `apply_disturbance(...)` each step, both trained against the reach-avoid margin (`f_cert.margin` / `f_cert.odd_margin`). -## Evaluating the filter on the real controller -`evaluate.py` requires the **vault-controller** repo (the deployed C iLQR; there is no fallback -controller — by design, so results reflect what ships). Clone it beside this repo and: +## Evaluating the filter with the controller +`evaluate.py` requires the **vault-controller** source and C library; there is +no fallback controller. This is a source/simulation evaluation and does not by +itself establish what firmware image is flashed on hardware. ```bash make -C ../vault-controller/balance_controller/c lib PYTHONPATH=../vault-controller python -m vault.evaluate ``` -## Vendored vs regenerable -- `data/` (committed): `composite_params.json`, `coupled_residual_fit.json`, and - `grid_reachavoid_odd.npz` (the exact value function — regenerate with `grid.py`). +## Release-backed vs regenerable +- `data/` (committed): the byte-matching release lock, checkpoint quarantine + manifest, and `grid_reachavoid_odd.npz` with its model-hash sidecar. +- Robot parameters, residuals, controller limits, the opt6 kernel, capability + grids, and release MLPs are loaded from the pinned controller release rather + than copied here. - `models/` (git-ignored): `v_mlp.*` and the SafetySAC checkpoint — regenerate with `distill.py` / `train.py`. ## The method (brief) -`f_cert` = opt6 lossless coupled dynamics + per-wheel friction-cone cap (slip) + the measured -coupled friction residual, one symplectic-Euler step. The grid solver computes the robust +`f_cert` = release opt6 coupled dynamics + per-wheel friction-cone cap (slip) + +the release coupled residual fit, one symplectic-Euler step. The grid solver computes the robust reach-avoid value `V(x;μ) = min(g_odd, max_u min_d V(f_cert(x,u)+d))`, where `g_odd` is the signed distance to the ODD (negative outside → leaving the envelope is failure by construction). The safe set is `{V ≥ 0}`. The filter admits the task control while the worst-case one-step value stays diff --git a/vault/calibration_check.py b/vault/calibration_check.py index 9c3e511..5366aae 100644 --- a/vault/calibration_check.py +++ b/vault/calibration_check.py @@ -19,21 +19,23 @@ import argparse import numpy as np -from safety_sb3 import SafetySAC from . import config as C from . import contact_margin as CM from . import f_cert as F +from .checkpoints import load_safety_sac from .mujoco_plant import MujocoPlant -def _fallback_action(model, x, mu, tau_max=C.TAU_MAX): +def _fallback_action(model, x, mu, tau_max=None): + tau_max = C.TAU_MAX if tau_max is None else tau_max obs5 = np.append(np.asarray(x, np.float32), np.float32(mu)) a, _ = model.predict(obs5, deterministic=True) return np.clip(a, -1.0, 1.0) * tau_max -def _q_at(model, x, mu, u, tau_max=C.TAU_MAX): +def _q_at(model, x, mu, u, tau_max=None): + tau_max = C.TAU_MAX if tau_max is None else tau_max import torch obs5 = np.append(np.asarray(x, np.float32), np.float32(mu))[None] u_norm = np.clip(np.asarray(u, float) / tau_max, -1.0, 1.0).astype(np.float32)[None] @@ -45,7 +47,7 @@ def _q_at(model, x, mu, u, tau_max=C.TAU_MAX): def run(model_path: str, n: int, horizon: int, mu: float, seed: int): - model = SafetySAC.load(model_path) + model = load_safety_sac(model_path) rng = np.random.default_rng(seed) plant = MujocoPlant(wheel="cylinder", mu=mu, dt=0.0005, substeps=20, contact_geometry=True) diff --git a/vault/checkpoints.py b/vault/checkpoints.py new file mode 100644 index 0000000..64b9e83 --- /dev/null +++ b/vault/checkpoints.py @@ -0,0 +1,27 @@ +"""Model-release gates for SafetySAC checkpoint loading.""" +from __future__ import annotations + +from pathlib import Path +from typing import Any, TypeVar + +from .model_release import get_model_release + +ModelT = TypeVar("ModelT") + + +def load_checkpoint(model_class: type[ModelT], path: str | Path, **kwargs: Any) -> ModelT: + """Validate checkpoint provenance before delegating to an SB3 loader.""" + verified = get_model_release().verify_checkpoint(path) + return model_class.load(str(verified), **kwargs) + + +def load_safety_sac(path: str | Path, **kwargs: Any): + from safety_sb3 import SafetySAC + + return load_checkpoint(SafetySAC, path, **kwargs) + + +def load_reach_avoid_safety_sac(path: str | Path, **kwargs: Any): + from .reach_avoid_sac import ReachAvoidSafetySAC + + return load_checkpoint(ReachAvoidSafetySAC, path, **kwargs) diff --git a/vault/config.py b/vault/config.py index 87f97f2..4b9f822 100644 --- a/vault/config.py +++ b/vault/config.py @@ -1,48 +1,122 @@ -"""Repo-relative paths and shared physical constants / ODD spec for the 4-state -balance-safety package. Single source: every module imports its constants from here. -""" +"""Shared physical constants and ODD spec for the 4-state safety package.""" from __future__ import annotations -import json +from functools import lru_cache from pathlib import Path +from typing import Any + +from .model_release import get_model_release PKG = Path(__file__).resolve().parent DATA = PKG / "data" MODELS = PKG / "models" GRID_NPZ = DATA / "grid_reachavoid_odd.npz" -# --- robot parameters (vendored composite_params.json) --- -_CP = json.loads((DATA / "composite_params.json").read_text()) -MASS = _CP["m_b"] + 2 * _CP["m_wheel"] # total mass (kg) -TRACK = _CP["wheel_sep"] # wheel separation / track width (m) -WHEEL_R = _CP["wheel_radius"] # wheel radius (m) -GRAV = 9.81 -COM_H = _CP["h_cm"] + WHEEL_R # CoM height above ground (m) -A_TIP = GRAV * TRACK / (2 * COM_H) # no-liftoff lateral-accel bound (m/s^2) +# Model access is deliberately lazy: importing the package remains possible +# while a controller checkout is changing, but first artifact access still +# performs the strict byte-lock and per-file hash gates. +_RELEASE_VALUE_NAMES = frozenset( + { + "MODEL_RELEASE", + "MASS", + "TRACK", + "WHEEL_R", + "GRAV", + "COM_H_WHOLE", + "A_TIP", + "N_STATIC_MIN", + "C_THETA", + "YAW_K0", + "YAW_KC", + "YAW_KV", + "YAW_EPS", + "CONTROLLER_TAU_MAX", + "ODD_CONTRACT", + "THETA_MAX", + "TAU_MAX", + "V_ODD", + "PSI_ODD", + "EBAR_PSI", + "TAU_ROLL_BAR", + "MU_RANGE", + "MU_SLICES", + "DOMAIN_V", + "DOMAIN_THETA", + "DOMAIN_THETA_DOT", + "DOMAIN_PSI_DOT", + } +) -# --- coupled friction residual fit (vendored) --- -_FR = json.loads((DATA / "coupled_residual_fit.json").read_text()) -C_THETA = _FR["pitch"]["c0"] # pitch damping coefficient -_Y = _FR["yaw"] -YAW_K0, YAW_KC, YAW_KV, YAW_EPS = _Y["k0"], _Y["kc"], _Y["kv"], _Y["eps"] -# --- integration + control --- -DT = 0.01 # control/integration step (s) -THETA_MAX = 1.2 # pitch failure bound (rad) -TAU_MAX = 8.0 # per-wheel torque limit (N*m) +@lru_cache(maxsize=1) +def _release_values() -> dict[str, Any]: + release = get_model_release() + composite = release.load_json("composite_params") + limits = release.load_json("controller_limits") + residual = release.load_json("coupled_residual") + odd_contract = release.load_json("odd_contract") + mass = composite["m_b"] + 2 * composite["m_wheel"] + track = composite["wheel_sep"] + wheel_radius = composite["wheel_radius"] + gravity = limits["gravity"] + yaw = residual["yaw"] + # Roll-constraint quantities come from the generated roll artifact, never from + # the reduced dynamics parameters. h_cm is a sprung-EQUIVALENT pendulum length + # (M_total * z_com / m_sprung), not a geometric height, so h_cm + wheel_radius + # overstates the whole-robot centre-of-mass height above ground by 8.2 mm. The + # rollover moment balance needs the true height, and the reduction does not + # emit it -- hence the separate artifact. + roll = release.load_json("roll_constraint_params") + com_height_whole = roll["whole_com_height_above_ground_m"] + a_tip = roll["no_liftoff_lateral_acceleration_m_s2"]["symmetric_worst_case"] + return { + "MODEL_RELEASE": release, + "MASS": mass, + "TRACK": track, + "WHEEL_R": wheel_radius, + "GRAV": gravity, + "COM_H_WHOLE": com_height_whole, + "A_TIP": a_tip, + "N_STATIC_MIN": ( + roll["static_wheel_normal_loads_n"]["conservative_lower_per_wheel"] + ), + "C_THETA": residual["pitch"]["c0"], + "YAW_K0": yaw["k0"], + "YAW_KC": yaw["kc"], + "YAW_KV": yaw["kv"], + "YAW_EPS": yaw["eps"], + "CONTROLLER_TAU_MAX": limits["motor_torque_limit"], + "ODD_CONTRACT": odd_contract, + "THETA_MAX": odd_contract["odd"]["theta_failure"]["bounds"][1], + "TAU_MAX": odd_contract["control"]["tau_max"]["value"], + "V_ODD": tuple(odd_contract["odd"]["velocity"]["bounds"]), + "PSI_ODD": odd_contract["odd"]["yaw_rate"]["bounds"][1], + "EBAR_PSI": odd_contract["disturbances"]["yaw_acceleration"]["value"], + "TAU_ROLL_BAR": odd_contract["disturbances"]["roll_wrench"]["value"], + "MU_RANGE": tuple(odd_contract["friction"]["range"]), + "MU_SLICES": tuple(odd_contract["friction"]["slices"]), + "DOMAIN_V": tuple(odd_contract["grid_axes"]["velocity"]["bounds"]), + "DOMAIN_THETA": tuple(odd_contract["grid_axes"]["theta"]["bounds"]), + "DOMAIN_THETA_DOT": tuple( + odd_contract["grid_axes"]["theta_dot"]["bounds"] + ), + "DOMAIN_PSI_DOT": tuple( + odd_contract["grid_axes"]["yaw_rate"]["bounds"] + ), + } + -# --- ODD: the operational envelope we certify --- -V_ODD = (-0.3, 1.5) # forward-speed bounds (m/s) -PSI_ODD = 2.5 # |yaw rate| bound (rad/s) +def __getattr__(name: str) -> Any: + if name in _RELEASE_VALUE_NAMES: + return _release_values()[name] + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") -# --- disturbance upper bounds (empirical + margin -> modeling assumptions) --- -EBAR_PSI = 3.4 # yaw-accel forcing bound (rad/s^2) -TAU_ROLL_BAR = 4.0 # roll-wrench bound (N*m) -# --- friction (mu) for the mu-aware value function --- -MU_RANGE = (0.3, 1.0) -MU_SLICES = (0.3, 0.6, 1.0) # certified slices (grid keys V_mu3 / V_mu6 / V_mu10) +def __dir__() -> list[str]: + return sorted(set(globals()) | _RELEASE_VALUE_NAMES) +# --- integration + control --- +DT = 0.01 # control/integration step (s) # --- contact-based failure modes (mujoco_env.py / contact_margin.py), not part of f_cert --- SLAM_VEL_MAX = 1.0 # contact normal speed (m/s) above which = a "slam" LEG_GROUND_CLEARANCE = 0.02 # m: (a) mujoco_model.py's leg-tip standoff from @@ -55,11 +129,6 @@ # --- full certified-domain bounds (matches grid.AXES_FULL) for contact-training reset coverage -- # ContactSafetyEnv resets across this whole range (not just a narrow near-upright band) so # training sees challenging / already-failed ("no-win") states too, not only typical operation. -DOMAIN_V = (-0.5, 1.7) -DOMAIN_THETA = (-1.35, 1.35) # deliberately exceeds THETA_MAX=1.2 on both ends -DOMAIN_THETA_DOT = (-6.0, 6.0) -DOMAIN_PSI_DOT = (-3.0, 3.0) - # --- target set (target_margin.py): "at a stop" neighborhood, for reach-avoid training --- # CORRECTED 2026-07-24 (Jaime): the target set must ALSO bound theta tightly, not just the # velocity-type components -- a state with v=theta_dot=psi_dot=0 but theta=1.0 rad is NOT a diff --git a/vault/csrc/reduced_opt6.c b/vault/csrc/reduced_opt6.c deleted file mode 100644 index c12d9e5..0000000 --- a/vault/csrc/reduced_opt6.c +++ /dev/null @@ -1,78 +0,0 @@ -// opt6 = float32 CLOSED-FORM reduced f + Jacobian (Lever A, M55-correct). -// The 27 reduced coeffs (M_red 6, dM_red 6, h_red 3, dh_dx 12) are closed forms in -// (theta,v,td,pd) from the G/Mc structure (NOT FK), generated by gen_closed_form.jl and -// assembled by assemble_closed_form.py. Constants are f-SUFFIXED so the whole kernel is -// genuinely float32 (no double-promotion / soft-float on the M55). NO 10-D lift at runtime. -// B_red is the compile-time constant (Lever B1). Wrapper: LDL^T + 7 back-subs. -// Same entry name as the ports so validate_f32.c / driver_fjac_f32.c link it. -#include -static const float R=0.12705f, Dh=0.140375f; -static void cf_pieces(float* q, const float* X){ - - q[0] = 20.372532410231674f; - q[1] = 0; - q[2] = 1.147163950102f * cosf(X[0]) + 0.02024408710200641f * sinf(X[0]); - q[3] = 0.02024408710200641f * cosf(X[0]) + -1.147163950102f * sinf(X[0]); - q[4] = -0.14356214558599997f; - q[5] = 0; - q[6] = 0.3171964577563598f; - q[7] = 0; - q[8] = 0.00016453771084912455f * cosf(X[0]) + -0.000840699879529228f * sinf(X[0]); - q[9] = -0.000840699879529228f * cosf(X[0]) + -0.00016453771084912455f * sinf(X[0]); - q[10] = 0.01787385146468828f + (0.5883197284732619f * cosf(X[0]) + 0.0013777588985465225f * sinf(X[0])) * cosf(X[0]) + -1 * (-0.0013777588985465225f * cosf(X[0]) + -0.8457395837620845f * sinf(X[0])) * sinf(X[0]); - q[11] = cosf(X[0]) * (0.002755517797093045f * cosf(X[0]) + 0.2574198552888226f * sinf(X[0])) + -1 * (-0.2574198552888226f * cosf(X[0]) + 0.002755517797093045f * sinf(X[0])) * sinf(X[0]); - q[12] = (0.14356214558599997f * X[3] * X[2] * sinf(X[0]) + (0.02024408710200641f * X[2] + -0.14356214558599997f * X[3] * sinf(X[0]) + 19.465467000000007f * X[1] * sinf(X[0])) * X[2] + -19.465467000000007f * X[2] * X[1] * sinf(X[0]) + -1 * X[3] * (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * cosf(X[0])) * cosf(X[0]) + (-1 * (1.147163950102f * X[2] + -0.14356214558599997f * X[3] * cosf(X[0]) + 19.465467000000007f * X[1] * cosf(X[0])) * X[2] + -0.14356214558599997f * X[3] * X[2] * cosf(X[0]) + 19.465467000000007f * X[2] * X[1] * cosf(X[0]) + -1 * X[3] * (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * sinf(X[0])) * sinf(X[0]); - q[13] = -11.253678350500621f * sinf(X[0]) + -0.000840699879529228f * X[3] * X[2] * cosf(X[0]) + -0.00016453771084912455f * X[3] * X[2] * sinf(X[0]) + 0.02024408710200641f * X[2] * X[1] * cosf(X[0]) + -1.147163950102f * X[2] * X[1] * sinf(X[0]) + X[3] * (0.000840699879529228f * X[2] + -0.0013777588985465225f * X[3] * cosf(X[0]) + -0.8457395837620845f * X[3] * sinf(X[0]) + 0.14356214558599997f * X[1] * sinf(X[0])) * cosf(X[0]) + X[3] * (0.00016453771084912455f * X[2] + 0.5883197284732619f * X[3] * cosf(X[0]) + 0.0013777588985465225f * X[3] * sinf(X[0]) + -0.14356214558599997f * X[1] * cosf(X[0])) * sinf(X[0]) + -1 * (0.02024408710200641f * X[2] + -0.14356214558599997f * X[3] * sinf(X[0]) + 19.465467000000007f * X[1] * sinf(X[0])) * X[1] * cosf(X[0]) + (1.147163950102f * X[2] + -0.14356214558599997f * X[3] * cosf(X[0]) + 19.465467000000007f * X[1] * cosf(X[0])) * X[1] * sinf(X[0]); - q[14] = (-1 * (0.000840699879529228f * X[2] + -0.0013777588985465225f * X[3] * cosf(X[0]) + -0.8457395837620845f * X[3] * sinf(X[0]) + 0.14356214558599997f * X[1] * sinf(X[0])) * X[2] + 0.0013777588985465225f * X[3] * X[2] * cosf(X[0]) + -0.5883197284732619f * X[3] * X[2] * sinf(X[0]) + 0.14356214558599997f * X[2] * X[1] * sinf(X[0]) + -1 * X[3] * (0.33183803775635984f * X[2] + 0.00732079f * (-2 * X[2] + 15.741833923652106f * X[1]) + 0.00016453771084912455f * X[3] * cosf(X[0]) + -0.000840699879529228f * X[3] * sinf(X[0]) + 1.147163950102f * X[1] * cosf(X[0]) + 0.02024408710200641f * X[1] * sinf(X[0])) * sinf(X[0]) + (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * X[1] * cosf(X[0])) * cosf(X[0]) + -1 * ((0.00016453771084912455f * X[2] + 0.5883197284732619f * X[3] * cosf(X[0]) + 0.0013777588985465225f * X[3] * sinf(X[0]) + -0.14356214558599997f * X[1] * cosf(X[0])) * X[2] + -0.8457395837620845f * X[3] * X[2] * cosf(X[0]) + 0.0013777588985465225f * X[3] * X[2] * sinf(X[0]) + 0.14356214558599997f * X[2] * X[1] * cosf(X[0]) + -1 * X[3] * (0.33183803775635984f * X[2] + 0.00732079f * (-2 * X[2] + 15.741833923652106f * X[1]) + 0.00016453771084912455f * X[3] * cosf(X[0]) + -0.000840699879529228f * X[3] * sinf(X[0]) + 1.147163950102f * X[1] * cosf(X[0]) + 0.02024408710200641f * X[1] * sinf(X[0])) * cosf(X[0]) + -1 * (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * X[1] * sinf(X[0])) * sinf(X[0]); - q[15] = 0; - q[16] = 0.02024408710200641f * X[2] * cosf(X[0]) + -1.147163950102f * X[2] * sinf(X[0]) + (1.147163950102f * X[2] + -0.14356214558599997f * X[3] * cosf(X[0]) + 19.465467000000007f * X[1] * cosf(X[0])) * sinf(X[0]) + -1 * (0.02024408710200641f * X[2] + -0.14356214558599997f * X[3] * sinf(X[0]) + 19.465467000000007f * X[1] * sinf(X[0])) * cosf(X[0]); - q[17] = ((-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * cosf(X[0]) + -1 * X[3] * (0.1152426603699331f + 1.147163950102f * cosf(X[0]) + 0.02024408710200641f * sinf(X[0])) * sinf(X[0])) * cosf(X[0]) + -1 * (-1 * X[3] * cosf(X[0]) * (0.1152426603699331f + 1.147163950102f * cosf(X[0]) + 0.02024408710200641f * sinf(X[0])) + -1 * (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * sinf(X[0])) * sinf(X[0]); - q[18] = (-1.147163950102f * (X[2]*X[2]) + -1 * X[3] * (1.147163950102f * X[3] * cosf(X[0]) + 0.02024408710200641f * X[3] * sinf(X[0])) * cosf(X[0])) * cosf(X[0]) + (-1 * (0.14356214558599997f * X[3] * sinf(X[0]) + -19.465467000000007f * X[1] * sinf(X[0])) * X[2] + 0.14356214558599997f * X[3] * X[2] * sinf(X[0]) + -19.465467000000007f * X[2] * X[1] * sinf(X[0]) + -1 * X[3] * (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * cosf(X[0]) + -1 * X[3] * (1.147163950102f * X[3] * cosf(X[0]) + 0.02024408710200641f * X[3] * sinf(X[0])) * sinf(X[0])) * sinf(X[0]) + -1 * (0.14356214558599997f * X[3] * X[2] * sinf(X[0]) + (0.02024408710200641f * X[2] + -0.14356214558599997f * X[3] * sinf(X[0]) + 19.465467000000007f * X[1] * sinf(X[0])) * X[2] + -19.465467000000007f * X[2] * X[1] * sinf(X[0]) + -1 * X[3] * (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * cosf(X[0])) * sinf(X[0]); - q[19] = -11.253678350500621f * cosf(X[0]) + -0.00016453771084912455f * X[3] * X[2] * cosf(X[0]) + 0.000840699879529228f * X[3] * X[2] * sinf(X[0]) + -1.147163950102f * X[2] * X[1] * cosf(X[0]) + X[3] * (0.00016453771084912455f * X[2] + -0.2574198552888226f * X[3] * cosf(X[0]) + 0.002755517797093045f * X[3] * sinf(X[0])) * cosf(X[0]) + -1 * X[3] * (0.000840699879529228f * X[2] + -0.0013777588985465225f * X[3] * cosf(X[0]) + -0.8457395837620845f * X[3] * sinf(X[0]) + 0.14356214558599997f * X[1] * sinf(X[0])) * sinf(X[0]) + X[3] * (0.0013777588985465225f * X[3] * cosf(X[0]) + -0.5883197284732619f * X[3] * sinf(X[0]) + 0.14356214558599997f * X[1] * sinf(X[0])) * sinf(X[0]) + (1.147163950102f * X[2] + -0.14356214558599997f * X[3] * cosf(X[0]) + 19.465467000000007f * X[1] * cosf(X[0])) * X[1] * cosf(X[0]) + -1 * (-0.14356214558599997f * X[3] * cosf(X[0]) + 19.465467000000007f * X[1] * cosf(X[0])) * X[1] * cosf(X[0]); - q[20] = -1 * ((0.0013777588985465225f * X[3] * cosf(X[0]) + -0.5883197284732619f * X[3] * sinf(X[0]) + 0.14356214558599997f * X[1] * sinf(X[0])) * X[2] + 0.0013777588985465225f * X[3] * X[2] * cosf(X[0]) + 0.8457395837620845f * X[3] * X[2] * sinf(X[0]) + -0.14356214558599997f * X[2] * X[1] * sinf(X[0]) + -1 * X[3] * (-0.000840699879529228f * X[3] * cosf(X[0]) + -0.00016453771084912455f * X[3] * sinf(X[0]) + 0.02024408710200641f * X[1] * cosf(X[0]) + -1.147163950102f * X[1] * sinf(X[0])) * cosf(X[0]) + X[3] * (0.33183803775635984f * X[2] + 0.00732079f * (-2 * X[2] + 15.741833923652106f * X[1]) + 0.00016453771084912455f * X[3] * cosf(X[0]) + -0.000840699879529228f * X[3] * sinf(X[0]) + 1.147163950102f * X[1] * cosf(X[0]) + 0.02024408710200641f * X[1] * sinf(X[0])) * sinf(X[0]) + -1 * (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * X[1] * cosf(X[0]) + -1 * (1.147163950102f * X[3] * cosf(X[0]) + 0.02024408710200641f * X[3] * sinf(X[0])) * X[1] * sinf(X[0])) * sinf(X[0]) + (-1 * (0.00016453771084912455f * X[2] + -0.2574198552888226f * X[3] * cosf(X[0]) + 0.002755517797093045f * X[3] * sinf(X[0])) * X[2] + 0.2574198552888226f * X[3] * X[2] * cosf(X[0]) + -0.002755517797093045f * X[3] * X[2] * sinf(X[0]) + -1 * X[3] * (-0.000840699879529228f * X[3] * cosf(X[0]) + -0.00016453771084912455f * X[3] * sinf(X[0]) + 0.02024408710200641f * X[1] * cosf(X[0]) + -1.147163950102f * X[1] * sinf(X[0])) * sinf(X[0]) + (1.147163950102f * X[3] * cosf(X[0]) + 0.02024408710200641f * X[3] * sinf(X[0])) * X[1] * cosf(X[0])) * cosf(X[0]) + -1 * (-1 * (0.000840699879529228f * X[2] + -0.0013777588985465225f * X[3] * cosf(X[0]) + -0.8457395837620845f * X[3] * sinf(X[0]) + 0.14356214558599997f * X[1] * sinf(X[0])) * X[2] + 0.0013777588985465225f * X[3] * X[2] * cosf(X[0]) + -0.5883197284732619f * X[3] * X[2] * sinf(X[0]) + 0.14356214558599997f * X[2] * X[1] * sinf(X[0]) + -1 * X[3] * (0.33183803775635984f * X[2] + 0.00732079f * (-2 * X[2] + 15.741833923652106f * X[1]) + 0.00016453771084912455f * X[3] * cosf(X[0]) + -0.000840699879529228f * X[3] * sinf(X[0]) + 1.147163950102f * X[1] * cosf(X[0]) + 0.02024408710200641f * X[1] * sinf(X[0])) * sinf(X[0]) + (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * X[1] * cosf(X[0])) * sinf(X[0]); - q[21] = 0.04048817420401282f * X[2] * cosf(X[0]) + -2.294327900204f * X[2] * sinf(X[0]); - q[22] = 0; - q[23] = (-0.001681399759058456f * X[2] + 0.002755517797093045f * X[3] * cosf(X[0]) + -0.05977660246753713f * X[3] * sinf(X[0])) * cosf(X[0]) + -1 * (0.0003290754216982491f * X[2] + -0.5746163130451825f * X[3] * cosf(X[0]) + 0.002755517797093045f * X[3] * sinf(X[0])) * sinf(X[0]); - q[24] = (-1 * X[3] * cosf(X[0]) * (-0.02024408710200641f * cosf(X[0]) + 1.147163950102f * sinf(X[0])) + -1 * (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * cosf(X[0])) * cosf(X[0]) + (-1 * (-0.02024408710200641f * X[3] * cosf(X[0]) + 1.147163950102f * X[3] * sinf(X[0])) * sinf(X[0]) + -1 * X[3] * (-0.02024408710200641f * cosf(X[0]) + 1.147163950102f * sinf(X[0])) * sinf(X[0])) * sinf(X[0]); - q[25] = -0.000840699879529228f * X[2] * cosf(X[0]) + -0.00016453771084912455f * X[2] * sinf(X[0]) + (0.000840699879529228f * X[2] + -0.0013777588985465225f * X[3] * cosf(X[0]) + -0.8457395837620845f * X[3] * sinf(X[0]) + 0.14356214558599997f * X[1] * sinf(X[0])) * cosf(X[0]) + X[3] * cosf(X[0]) * (-0.0013777588985465225f * cosf(X[0]) + -0.8457395837620845f * sinf(X[0])) + X[3] * (0.5883197284732619f * cosf(X[0]) + 0.0013777588985465225f * sinf(X[0])) * sinf(X[0]) + (0.00016453771084912455f * X[2] + 0.5883197284732619f * X[3] * cosf(X[0]) + 0.0013777588985465225f * X[3] * sinf(X[0]) + -0.14356214558599997f * X[1] * cosf(X[0])) * sinf(X[0]); - q[26] = -1 * (-0.8457395837620845f * X[2] * cosf(X[0]) + X[2] * (0.5883197284732619f * cosf(X[0]) + 0.0013777588985465225f * sinf(X[0])) + 0.0013777588985465225f * X[2] * sinf(X[0]) + -1 * X[3] * cosf(X[0]) * (0.00016453771084912455f * cosf(X[0]) + -0.000840699879529228f * sinf(X[0])) + -1 * (0.33183803775635984f * X[2] + 0.00732079f * (-2 * X[2] + 15.741833923652106f * X[1]) + 0.00016453771084912455f * X[3] * cosf(X[0]) + -0.000840699879529228f * X[3] * sinf(X[0]) + 1.147163950102f * X[1] * cosf(X[0]) + 0.02024408710200641f * X[1] * sinf(X[0])) * cosf(X[0]) + -1 * X[1] * (-0.02024408710200641f * cosf(X[0]) + 1.147163950102f * sinf(X[0])) * sinf(X[0])) * sinf(X[0]) + (0.0013777588985465225f * X[2] * cosf(X[0]) + -1 * X[2] * (-0.0013777588985465225f * cosf(X[0]) + -0.8457395837620845f * sinf(X[0])) + -0.5883197284732619f * X[2] * sinf(X[0]) + -1 * (0.33183803775635984f * X[2] + 0.00732079f * (-2 * X[2] + 15.741833923652106f * X[1]) + 0.00016453771084912455f * X[3] * cosf(X[0]) + -0.000840699879529228f * X[3] * sinf(X[0]) + 1.147163950102f * X[1] * cosf(X[0]) + 0.02024408710200641f * X[1] * sinf(X[0])) * sinf(X[0]) + -1 * X[3] * (0.00016453771084912455f * cosf(X[0]) + -0.000840699879529228f * sinf(X[0])) * sinf(X[0]) + X[1] * cosf(X[0]) * (-0.02024408710200641f * cosf(X[0]) + 1.147163950102f * sinf(X[0]))) * cosf(X[0]); -} - - -void reduced_struct_fjac_f32(const float*x,const float*uc,float*xd,float*fx,float*fu){ - /* cf_pieces expects [theta,v,td,pd]; state x is [v,theta,td,pd] -> reorder */ - float Xr[4] = { x[1], x[0], x[2], x[3] }; - float q[27]; cf_pieces(q, Xr); - float td=x[2]; - float Mr00=q[0],Mr01=q[2],Mr02=q[4],Mr11=q[6],Mr12=q[8],Mr22=q[10]; - float dMr00=q[1],dMr01=q[3],dMr02=q[5],dMr11=q[7],dMr12=q[9],dMr22=q[11]; - float hr0=q[12],hr1=q[13],hr2=q[14]; - #define DH(a,k) q[15 + (a) + 3*(k)] - /* B_red (Lever B1). theta_dot row is -tau/wheel, NOT -2tau: the relative wheel-joint - coord already carries the body-pitch reaction (G[wheel,theta_dot]=-1), so an explicit - extra -tau double-counts it (energy-inconsistent). See firmware reduced_model.control_map_B - / firmware/safety_cert/test_control_map_energy.py. */ - const float Br[3][2] = {{1.0f/R, 1.0f/R}, {-1.0f, -1.0f}, {Dh/R, -Dh/R}}; - float d0=Mr00, L10=Mr01/d0, L20=Mr02/d0; - float d1=Mr11-L10*L10*d0, L21=(Mr12-L20*L10*d0)/d1; - float d2=Mr22-L20*L20*d0-L21*L21*d1; - #define SOLVE(r0,r1,r2,xo) do{ float y0=(r0),y1=(r1)-L10*y0,y2=(r2)-L20*y0-L21*y1; \ - float z0=y0/d0,z1=y1/d1,z2=y2/d2; xo[2]=z2; xo[1]=z1-L21*xo[2]; xo[0]=z0-L10*xo[1]-L20*xo[2]; }while(0) - float acc[3]; - SOLVE(Br[0][0]*uc[0]+Br[0][1]*uc[1]-hr0, - Br[1][0]*uc[0]+Br[1][1]*uc[1]-hr1, - Br[2][0]*uc[0]+Br[2][1]*uc[1]-hr2, acc); - xd[0]=acc[0]; xd[1]=td; xd[2]=acc[1]; xd[3]=acc[2]; - for(int k=0;k<4;k++){ - float r0=-DH(0,k), r1=-DH(1,k), r2=-DH(2,k); - if(k==1){ r0-=dMr00*acc[0]+dMr01*acc[1]+dMr02*acc[2]; - r1-=dMr01*acc[0]+dMr11*acc[1]+dMr12*acc[2]; - r2-=dMr02*acc[0]+dMr12*acc[1]+dMr22*acc[2]; } - float da[3]; SOLVE(r0,r1,r2,da); - fx[0+4*k]=da[0]; fx[1+4*k]=(k==2)?1.0f:0.0f; fx[2+4*k]=da[1]; fx[3+4*k]=da[2]; - } - for(int j=0;j<2;j++){ float du[3]; SOLVE(Br[0][j],Br[1][j],Br[2][j],du); - fu[0+4*j]=du[0]; fu[1+4*j]=0; fu[2+4*j]=du[1]; fu[3+4*j]=du[2]; } - #undef SOLVE - #undef DH -} diff --git a/vault/data/checkpoints_manifest.json b/vault/data/checkpoints_manifest.json new file mode 100644 index 0000000..163d558 --- /dev/null +++ b/vault/data/checkpoints_manifest.json @@ -0,0 +1,45 @@ +{ + "checkpoints": [ + { + "incompatible_with": "b6fefc97e833a439826645b209ab925ca2dfd7dac21f1f3a8ee0f77f095d4f9f", + "pattern": "balance_safety_sac*", + "reason": "trained before the v2.2 camera-inclusive reduced-model release", + "status": "incompatible" + }, + { + "incompatible_with": "b6fefc97e833a439826645b209ab925ca2dfd7dac21f1f3a8ee0f77f095d4f9f", + "pattern": "contact_safety_sac_v3*", + "reason": "trained before the v2.2 camera-inclusive reduced-model release", + "status": "incompatible" + }, + { + "also_valid_under_lock_sha256": { + "05ce740da53925f3fabd51ee3fe2dbb93fe057e3e0337af17cb9d0aa4371f992": "Release 05ce740d differs from the training release f5603068 in provenance and three filter changes, none of which touch the dynamics: BC_ISFINITE double-cast removal (bit-identical), fallback grid 7x7 -> 3x3 (searches exactly the certificate control set), and removal of the iterative projection (measurably safer: hardest-mission survival 38% -> 88%). composite_params_sha256 and opt6_kernel_sha256 unchanged. Evaluation-only status unaffected.", + "335e3138b4a92f04352dfc3857448ccdfad7481def833f2d6fc17f8bc57d588c": "Release 335e3138 differs from the training release f5603068 only in provenance and filter changes that do not touch the dynamics; composite_params_sha256 and opt6_kernel_sha256 unchanged. Evaluation-only status unaffected.", + "512fc77dcedf5daa16f0d9411ef7d69b0e357e69b709d5e9799ed2f664409b48": "Release 512fc77d differs from the training release f5603068 only in provenance and a soft-float fix: the added knee_wheel_geometry.json, the source manifest covering it, re-pinned generator commits, and dropping a double cast in BC_ISFINITE on the f32 build (behaviourally identical, verified across zeros/denormals/inf/NaN). composite_params_sha256 and opt6_kernel_sha256 unchanged, so the dynamics this policy trained against did not move. Evaluation-only status unaffected.", + "5d7c918f7c61ebf1b04401f98ef415a2489ae13b1c70fc447a8afa95c02b9f16": "Release 5d7c918f differs from the training release f5603068 only in provenance and filter changes that do not touch the dynamics: BC_ISFINITE cast removal, fallback 7x7 -> 3x3, removal of the iterative projection, and fusing all value evaluations into one batched pass (bit-identical, verified over 5,625 points). composite_params_sha256 and opt6_kernel_sha256 unchanged. Evaluation-only status unaffected.", + "a9595e99fd57e163af3e587c2f5ce6c9015dec2d42979771d80bf7cd5f6aeea5": "Release a9595e99 differs from the training release f5603068 in exactly three provenance keys and no physics: the ADDED file models/source/geometry/knee_wheel_geometry.json, the source manifest hash that now covers it, and the re-pinned generator commit (a39b85a1 -> 7e32bf93, which reproduced every release artifact byte-identically). composite_params_sha256 and opt6_kernel_sha256 are unchanged, so the dynamics this policy was trained against did not move by one bit. Evaluation-only status is unaffected and still applies.", + "e9ac4b9d49cf3c34e23b06de82eb34fcdd1e6d97db046e8d3680afdd6aeff9b6": "Release e9ac4b9d differs from the training release f5603068 only in provenance and two behaviour-preserving-or-safety-improving filter changes: the BC_ISFINITE double-cast removal (bit-identical) and the CBF-QP fallback grid 7x7 -> 3x3, which searches exactly the control set the certificate maximises over. composite_params_sha256 and opt6_kernel_sha256 unchanged, so the dynamics this policy trained against did not move. Evaluation-only status unaffected." + }, + "artifact_sha256": "ba482bc2bf7161bdd4fa065e5ce2c232d9f084ce9c7b9c836437892c6e09fc7a", + "lock_sha256": "f56030685ad79b16261abc7329448a78019b7f4748490f810699be5b03ca220a", + "model_hashes": { + "composite_params_sha256": "ff1cb3ea82565cfff9d8454d277e8bc4d63d467fcf5c6b5c36ddf372b7c2f5a9", + "opt6_kernel_sha256": "5b2aa6e4d2b337c45de8f57282c83f01266e0e2adaa19ebe08b0dc2074bc40c9" + }, + "pattern": "reach_avoid_safety_sac_v22*", + "reason": "trained 2026-07-29 on the v2.2 release under this lock, 300k steps, --saturate-target. AUTHORIZED FOR EVALUATION ONLY: its critic carries the inherited max-entropy term inside the DRABE backup (see docs/CHANGES_TO_THE_MODEL_LAYER.md), so it is not a certified safety value.", + "status": "compatible", + "training_stage": "reach_avoid", + "training_steps": 300000 + }, + { + "incompatible_with": "b6fefc97e833a439826645b209ab925ca2dfd7dac21f1f3a8ee0f77f095d4f9f", + "pattern": "reach_avoid_safety_sac*", + "reason": "trained before the v2.2 camera-inclusive reduced-model release", + "status": "incompatible" + } + ], + "release_lock_sha256": "335e3138b4a92f04352dfc3857448ccdfad7481def833f2d6fc17f8bc57d588c", + "schema_version": 1 +} diff --git a/vault/data/composite_params.json b/vault/data/composite_params.json deleted file mode 100644 index 24e551b..0000000 --- a/vault/data/composite_params.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "m_b": 17.145307000000006, - "h_cm": 0.0669083353305951, - "I_pitch": 0.2404416275037652, - "I_yaw": 0.5883197284732619, - "wheel_radius": 0.12705, - "wheel_sep": 0.28075, - "m_wheel": 1.16008, - "I_wheel": 0.00732079, - "_knee_wheel_inertia": 0.00014, - "_total_mass_3d": 19.465467000000007 -} \ No newline at end of file diff --git a/vault/data/controller_lock.sha256 b/vault/data/controller_lock.sha256 new file mode 100644 index 0000000..80cac25 --- /dev/null +++ b/vault/data/controller_lock.sha256 @@ -0,0 +1 @@ +335e3138b4a92f04352dfc3857448ccdfad7481def833f2d6fc17f8bc57d588c models/MODEL_INPUTS.lock.json (sha256 of the PRIVATE vault-controller lock; the lock itself must never be committed here) diff --git a/vault/data/coupled_residual_fit.json b/vault/data/coupled_residual_fit.json deleted file mode 100644 index ae3d6c0..0000000 --- a/vault/data/coupled_residual_fit.json +++ /dev/null @@ -1,25 +0,0 @@ -{ - "pitch": { - "form": "-(c0)*theta_dot", - "c0": 1.4192148851419473, - "cmu": 0.027236108714925922, - "ebar_max": 0.030553619565067636, - "note": "mu-independent linear viscous damping; linear => slope correct => backward-pass-ready" - }, - "yaw": { - "k0": 0.711818332881714, - "kc": 5.828874643814778, - "kv": -0.2941425910953992, - "eps": 0.1, - "N0_mg": 190.95623127000007, - "h": 0.1939583353305951, - "w": 0.28075, - "ebar_rms": 0.28712435061040287, - "ebar_max": 4.426216393345783, - "form": "-[(k0+kc*mu)*tanh(psidot/eps)+kv*psidot]*(N/N0)" - }, - "long": { - "form": "0", - "note": "opt6 captures v_dot; residual ~0.01 rad/s^2 (negligible)" - } -} \ No newline at end of file diff --git a/vault/data/grid_reachavoid_odd.npz b/vault/data/grid_reachavoid_odd.npz deleted file mode 100644 index 96856db..0000000 Binary files a/vault/data/grid_reachavoid_odd.npz and /dev/null differ diff --git a/vault/data/grid_reachavoid_odd.npz.model.json b/vault/data/grid_reachavoid_odd.npz.model.json new file mode 100644 index 0000000..2a36f51 --- /dev/null +++ b/vault/data/grid_reachavoid_odd.npz.model.json @@ -0,0 +1,9 @@ +{ + "artifact_sha256": "2be83d700fad6b67c2c72b46f416763c8d8c5a34b2752002faf7cfdfeb8ca0f7", + "lock_sha256": "335e3138b4a92f04352dfc3857448ccdfad7481def833f2d6fc17f8bc57d588c", + "model_hashes": { + "composite_params_sha256": "ff1cb3ea82565cfff9d8454d277e8bc4d63d467fcf5c6b5c36ddf372b7c2f5a9", + "opt6_kernel_sha256": "5b2aa6e4d2b337c45de8f57282c83f01266e0e2adaa19ebe08b0dc2074bc40c9" + }, + "schema_version": 1 +} diff --git a/vault/data/release_artifacts.model.json b/vault/data/release_artifacts.model.json new file mode 100644 index 0000000..2d407db --- /dev/null +++ b/vault/data/release_artifacts.model.json @@ -0,0 +1,23 @@ +{ + "artifacts": { + "composite_params": "models/generated/reduced/composite_params.json", + "controller_limits": "models/generated/reduced/controller_limits.json", + "coupled_residual": "models/generated/safety/residuals/coupled_residual_fit.json", + "grid_laden": "models/generated/safety/grids/robust_odd_dh30_dm20_bump0.npz", + "grid_unladen": "models/generated/safety/grids/robust_odd_dh0_dm0_bump0.npz", + "model_geometry": "models/source/geometry/model_geometry.json", + "odd_contract": "models/source/odd_contract.json", + "opt6_kernel": "models/generated/reduced/reduced_struct_fjac_f32_opt6.c", + "roll_constraint_params": "models/generated/safety/roll_constraint_params.json", + "safety_params_header": "balance_controller/c/include/balance_controller/bc_safety_params.h", + "v_mlp_laden_json": "models/generated/safety/mlp/v_mlp_laden.json", + "v_mlp_laden_pt": "models/generated/safety/mlp/v_mlp_laden.pt", + "v_mlp_unladen_json": "models/generated/safety/mlp/v_mlp_unladen.json", + "v_mlp_unladen_pt": "models/generated/safety/mlp/v_mlp_unladen.pt" + }, + "lock_sha256": "335e3138b4a92f04352dfc3857448ccdfad7481def833f2d6fc17f8bc57d588c", + "model_hashes": { + "composite_params_sha256": "ff1cb3ea82565cfff9d8454d277e8bc4d63d467fcf5c6b5c36ddf372b7c2f5a9", + "opt6_kernel_sha256": "5b2aa6e4d2b337c45de8f57282c83f01266e0e2adaa19ebe08b0dc2074bc40c9" + } +} diff --git a/vault/distill.py b/vault/distill.py index 3ef61d6..7356225 100644 --- a/vault/distill.py +++ b/vault/distill.py @@ -21,7 +21,9 @@ from . import config as C -_MU_KEYS = [(m, f"V_mu{int(m * 10)}") for m in C.MU_SLICES] + +def _mu_keys(): + return [(m, f"V_mu{int(m * 10)}") for m in C.MU_SLICES] class VNet(nn.Module): @@ -37,10 +39,14 @@ def forward(self, x): def _load_grid(): + mu_keys = _mu_keys() d = np.load(C.GRID_NPZ, allow_pickle=True) axes = [np.asarray(a, float) for a in d["axes"]] dims = [len(a) for a in axes] - rgis = {m: RGI(axes, d[k].reshape(dims), bounds_error=False, fill_value=None) for m, k in _MU_KEYS} + rgis = { + m: RGI(axes, d[k].reshape(dims), bounds_error=False, fill_value=None) + for m, k in mu_keys + } lo = np.array([a[0] for a in axes]) hi = np.array([a[-1] for a in axes]) return rgis, lo, hi @@ -53,11 +59,12 @@ def _normalizer(lo, hi): def _sample(n, rng, rgis, lo, hi): + mu_keys = _mu_keys() s = np.column_stack([rng.uniform(lo[i], hi[i], n) for i in range(4)]) - mus = np.array([m for m, _ in _MU_KEYS]) - idx = rng.integers(0, len(_MU_KEYS), n) + mus = np.array([m for m, _ in mu_keys]) + idx = rng.integers(0, len(mu_keys), n) v = np.empty(n) - for j, (mu, _) in enumerate(_MU_KEYS): + for j, (mu, _) in enumerate(mu_keys): m = idx == j if m.any(): v[m] = rgis[mu](s[m]) diff --git a/vault/docs/CHANGES_TO_THE_MODEL_LAYER.md b/vault/docs/CHANGES_TO_THE_MODEL_LAYER.md new file mode 100644 index 0000000..39786de --- /dev/null +++ b/vault/docs/CHANGES_TO_THE_MODEL_LAYER.md @@ -0,0 +1,19 @@ +# Model-layer changes under the reach-avoid work + +**The full document lives in the private model-authority repository:** +`vault-controller/docs/ssb_handoff/CHANGES_TO_THE_MODEL_LAYER.md` +(assumed checked out as a sibling of this repo; `vault/model_release.py` resolves it +the same way). + +It is kept there deliberately: this repository is public, and the document contains +the robot's mass properties, geometry, and certified bounds. Nothing in THIS +repository carries model values — every quantity is read at runtime from the private +sibling via the hash-pinned release loader (`vault/data/controller_lock.sha256`), +and the figures it references are likewise in `vault-controller/docs/ssb_handoff/`. + +Contents of the private document, for orientation: + §§1–7 corrections to the model layer and what is NOT established + §§8–13 the governor fix, four characterized limitations, the entropy-term + analysis, preconditions, candidate terminal set, consumer gaps + §14 ownership tables (safety theory vs filter architecture), the open-item + registry, and pick-up mechanics for a coding agent diff --git a/vault/dynamics.py b/vault/dynamics.py index 6fb85c4..bc200e9 100644 --- a/vault/dynamics.py +++ b/vault/dynamics.py @@ -2,10 +2,11 @@ This is the ONLY robot-specific dynamics shared with this package: the closed-form reduced equations of motion over x = [v, theta, theta_dot, psi_dot], generated by -symbolic projection of the full model and CSE-compiled to a single C function -(`csrc/reduced_opt6.c`, geometry baked in at codegen). It does NOT pull in the -framework / hybrid simulator or the deployed controller — just `f` and the analytic -Jacobians, which is everything the HJ reach-avoid solve, the RL env, and the iLQR need. +symbolic projection of the full model and CSE-compiled to a single C function in +the provenance-pinned vault-controller model release. It does NOT pull in the +framework / hybrid simulator or the deployed controller -- just `f` and the +analytic Jacobians, which is everything the HJ reach-avoid solve, the RL env, +and the iLQR need. `CoupledOpt6().f(x, u)` -> xd = [v_dot, theta_dot, theta_ddot, psi_ddot] `CoupledOpt6().jacobians(x, u)` -> (fx 4x4, fu 4x2) continuous-time analytic Jacobians @@ -14,19 +15,43 @@ temp .so and cached. Pure NumPy/ctypes otherwise. """ import ctypes +import os import shutil import subprocess import tempfile +from functools import lru_cache from pathlib import Path +from typing import Any import numpy as np -_SRC = Path(__file__).resolve().parent / "csrc" / "reduced_opt6.c" +from .model_release import get_model_release + _lib = None # cached ctypes handle -# Geometry baked into the kernel at codegen (metadata; the kernel itself carries it). -OPT6_R = 0.12705 # v2 body-wheel radius (m) -OPT6_DH = 0.140375 # v2 half wheel-separation (m) + +@lru_cache(maxsize=1) +def _release_inputs() -> tuple[Any, Path, dict[str, Any]]: + release = get_model_release() + return ( + release, + release.artifact_path("opt6_kernel"), + release.load_json("composite_params"), + ) + + +def __getattr__(name: str) -> Any: + if name == "_SRC": + return _release_inputs()[1] + if name == "OPT6_R": + return _release_inputs()[2]["wheel_radius"] + if name == "OPT6_DH": + return _release_inputs()[2]["wheel_sep"] / 2.0 + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + + +def __dir__() -> list[str]: + return sorted(set(globals()) | {"_SRC", "OPT6_R", "OPT6_DH"}) def _load(): @@ -37,11 +62,30 @@ def _load(): cc = shutil.which("cc") or shutil.which("gcc") if cc is None: raise RuntimeError("need a C compiler (cc/gcc) on PATH to build the reduced-dynamics kernel") - so = Path(tempfile.gettempdir()) / "libvault_reduced_opt6.so" - if (not so.exists()) or so.stat().st_mtime < _SRC.stat().st_mtime: - subprocess.run([cc, "-O2", "-fPIC", "-shared", "-o", str(so), str(_SRC), "-lm"], - check=True, capture_output=True) - lib = ctypes.CDLL(str(so)) + release, source, _ = _release_inputs() + source_hash = release.model_hashes["opt6_kernel_sha256"][:16] + so_path = Path(tempfile.gettempdir()) / f"libvault_reduced_opt6_{source_hash}.so" + if not so_path.exists(): + temporary = so_path.with_name(f"{so_path.name}.{os.getpid()}.tmp") + try: + subprocess.run( + [ + cc, + "-O2", + "-fPIC", + "-shared", + "-o", + str(temporary), + str(source), + "-lm", + ], + check=True, + capture_output=True, + ) + os.replace(temporary, so_path) + finally: + temporary.unlink(missing_ok=True) + lib = ctypes.CDLL(str(so_path)) lib.reduced_struct_fjac_f32.argtypes = [ctypes.POINTER(ctypes.c_float)] * 5 lib.reduced_struct_fjac_f32.restype = None _lib = lib @@ -57,8 +101,9 @@ class CoupledOpt6: def __init__(self): self._lib = _load() - self.r = OPT6_R - self.d = OPT6_DH + _, _, params = _release_inputs() + self.r = params["wheel_radius"] + self.d = params["wheel_sep"] / 2.0 def _fjac(self, x, u_LR): xf = np.ascontiguousarray(x, dtype=np.float32) diff --git a/vault/env.py b/vault/env.py index d707af1..a1d798e 100644 --- a/vault/env.py +++ b/vault/env.py @@ -26,17 +26,28 @@ class BalanceSafetyEnv(gym.Env): metadata = {"render_modes": []} - def __init__(self, mu_range=C.MU_RANGE, tau_max=C.TAU_MAX, max_steps=200, disturb=True, - ebar_theta=0.1, ebar_psi=C.EBAR_PSI, tau_roll_bar=C.TAU_ROLL_BAR, seed=None): + def __init__(self, mu_range=None, tau_max=None, max_steps=200, disturb=True, + ebar_theta=0.1, ebar_psi=None, tau_roll_bar=None, seed=None): super().__init__() - self.mu_range = mu_range - self.tau_max = tau_max + self.mu_range = C.MU_RANGE if mu_range is None else mu_range + self.tau_max = C.TAU_MAX if tau_max is None else tau_max self.max_steps = max_steps self.disturb = disturb self.ebar_theta = ebar_theta - self.ebar_psi = ebar_psi - self.tau_roll_bar = tau_roll_bar - hi = np.array([2.0, 1.5, 8.0, 3.0, 1.0], np.float32) + self.ebar_psi = C.EBAR_PSI if ebar_psi is None else ebar_psi + self.tau_roll_bar = ( + C.TAU_ROLL_BAR if tau_roll_bar is None else tau_roll_bar + ) + hi = np.array( + [ + max(abs(x) for x in C.DOMAIN_V), + max(abs(x) for x in C.DOMAIN_THETA), + max(abs(x) for x in C.DOMAIN_THETA_DOT), + max(abs(x) for x in C.DOMAIN_PSI_DOT), + C.MU_RANGE[1], + ], + np.float32, + ) self.observation_space = spaces.Box(-hi, hi, dtype=np.float32) self.action_space = spaces.Box(-1.0, 1.0, shape=(2,), dtype=np.float32) self._rng = np.random.default_rng(seed) @@ -66,7 +77,7 @@ def step(self, action): self.t += 1 g = float(F.margin(self.x)) if tau_roll: # roll wrench as effective lateral accel - a_lat = abs(self.x[0] * self.x[3]) + abs(tau_roll) / (C.MASS * C.COM_H) + a_lat = abs(self.x[0] * self.x[3]) + abs(tau_roll) / (C.MASS * C.COM_H_WHOLE) g = float(min(g, 1.0 - a_lat / C.A_TIP)) terminated = bool(g < 0.0) truncated = bool(self.t >= self.max_steps) diff --git a/vault/f_cert.py b/vault/f_cert.py index e78e174..434e709 100644 --- a/vault/f_cert.py +++ b/vault/f_cert.py @@ -17,19 +17,33 @@ """ from __future__ import annotations +from functools import lru_cache + import numpy as np from . import config as C from .dynamics import CoupledOpt6 -_opt6 = CoupledOpt6() +@lru_cache(maxsize=1) +def _opt6() -> CoupledOpt6: + """Load the provenance-gated dynamics kernel on first dynamics use.""" + return CoupledOpt6() def wheel_loads(v, psid): - """Per-wheel normal forces with centrifugal load transfer (outer wheel loaded).""" + """Per-wheel normal forces with centrifugal load transfer (outer wheel loaded). + + The static base is the CONSERVATIVE LOWER per-wheel load, applied to both wheels. + The v2.2 centre of mass is 7.28 mm off-centre laterally, so the true static split + is 101.80/91.77 N rather than even; assigning those to left and right requires a + sign convention that is not released yet, and getting it backwards would credit + the lighter wheel with the heavier load. Using the lower value for both wheels + understates total available friction and is safe under either convention. + """ n_tot = C.MASS * C.GRAV - dn = C.MASS * abs(v * psid) * C.COM_H / C.TRACK - nl, nr = (n_tot / 2 - dn, n_tot / 2 + dn) if v * psid >= 0 else (n_tot / 2 + dn, n_tot / 2 - dn) + dn = C.MASS * abs(v * psid) * C.COM_H_WHOLE / C.TRACK + base = C.N_STATIC_MIN + nl, nr = (base - dn, base + dn) if v * psid >= 0 else (base + dn, base - dn) return n_tot, max(nl, 0.0), max(nr, 0.0) @@ -39,10 +53,15 @@ def f_cert_step(x, u, mu): n_tot, nl, nr = wheel_loads(v, psid) u_cap = np.array([np.clip(u[0], -mu * nl * C.WHEEL_R, mu * nl * C.WHEEL_R), np.clip(u[1], -mu * nr * C.WHEEL_R, mu * nr * C.WHEEL_R)]) - xd = np.asarray(_opt6.f(x, u_cap), float) + xd = np.asarray(_opt6().f(x, u_cap), float) xd[2] += -C.C_THETA * thd # measured pitch damping xd[3] += -((C.YAW_K0 + C.YAW_KC * mu) * np.tanh(psid / C.YAW_EPS) + C.YAW_KV * psid) * (n_tot / (C.MASS * C.GRAV)) # measured yaw scrub + # NOTE: n_tot = C.MASS * C.GRAV exactly (wheel_loads), so the ratio above is + # identically 1.0. It is a no-op that LOOKS load-dependent. Retained rather than + # deleted because a genuine normal-load dependence belongs here if the total ever + # stops being static -- but today it scales nothing, and reading it as a load term + # is a misreading. v2 = v + xd[0] * C.DT thd2 = thd + xd[2] * C.DT psid2 = psid + xd[3] * C.DT @@ -68,7 +87,7 @@ def odd_margin(X, tau_roll_bar=None): trb = C.TAU_ROLL_BAR if tau_roll_bar is None else tau_roll_bar v, th, psid = X[..., 0], X[..., 1], X[..., 3] g_pitch = (C.THETA_MAX - np.abs(th)) / C.THETA_MAX - a_lat = np.abs(v * psid) + trb / (C.MASS * C.COM_H) + a_lat = np.abs(v * psid) + trb / (C.MASS * C.COM_H_WHOLE) g_roll = 1.0 - a_lat / C.A_TIP g_v = np.minimum(v - C.V_ODD[0], C.V_ODD[1] - v) / ((C.V_ODD[1] - C.V_ODD[0]) / 2) g_psi = (C.PSI_ODD - np.abs(psid)) / C.PSI_ODD diff --git a/vault/filter.py b/vault/filter.py index ecf4e29..d076ee3 100644 --- a/vault/filter.py +++ b/vault/filter.py @@ -16,10 +16,13 @@ """ from __future__ import annotations +from pathlib import Path + import numpy as np from . import config as C from . import f_cert as F +from .model_release import get_model_release def control_grid(n=5): @@ -55,15 +58,27 @@ def filter(self, x, u_task, mu): # --- value backends ---------------------------------------------------- @classmethod - def from_mlp(cls, models_dir=None, eps=0.0, controls=None): - """The conservative deployable V_mlp (matches what the robot would carry).""" + def from_mlp(cls, models_dir=None, eps=0.0, controls=None, mode="unladen"): + """Load the conservative release MLP or a sidecar-pinned external MLP.""" import json import torch from .distill import VNet - md = models_dir or C.MODELS - cfg = json.loads((md / "v_mlp.json").read_text()) + + release = get_model_release() + if mode not in ("unladen", "laden"): + raise ValueError(f"mode must be 'unladen' or 'laden', got {mode!r}") + if models_dir is None: + cfg_path = release.artifact_path(f"v_mlp_{mode}_json") + weights_path = release.artifact_path(f"v_mlp_{mode}_pt") + else: + md = Path(models_dir) + cfg_path = release.verify_external_artifact(md / "v_mlp.json") + weights_path = release.verify_external_artifact(md / "v_mlp.pt") + cfg = json.loads(cfg_path.read_text()) net = VNet(cfg["h"]) - net.load_state_dict(torch.load(md / "v_mlp.pt")) + net.load_state_dict( + torch.load(weights_path, map_location="cpu", weights_only=True) + ) net.eval() nlo, nhi, delta = np.array(cfg["nlo"]), np.array(cfg["nhi"]), cfg["delta"] @@ -77,14 +92,70 @@ def value_fn(X4, mu): @classmethod def from_grid(cls, npz=None, eps=0.0, controls=None): - """The exact grid V (nearest certified mu slice). Oracle baseline.""" + """Load Jaime's ODD reach-avoid grid, regenerated for the v2.2 model.""" + release = get_model_release() + path = C.GRID_NPZ if npz is None else Path(npz) + path = release.verify_external_artifact(path) + return cls._from_grid_path(path, eps=eps, controls=controls) + + @classmethod + def from_capability_grid(cls, mode="unladen", eps=0.0, controls=None): + """Explicitly load a controller capability grid. + + Capability grids encode the controller release's tip/capability + contract, not Jaime's SSB ODD reach-avoid contract. Keeping this as a + separate constructor prevents a model-release update from silently + changing the meaning of ``from_grid()``. + """ + release = get_model_release() + if mode not in ("unladen", "laden"): + raise ValueError(f"mode must be 'unladen' or 'laden', got {mode!r}") + path = release.artifact_path(f"grid_{mode}") + metadata = (0.0, 0.0, 0.0) if mode == "unladen" else (0.3, 0.2, 0.0) + return cls._from_grid_path( + path, eps=eps, controls=controls, expected_metadata=metadata + ) + + @classmethod + def _from_grid_path( + cls, path, eps=0.0, controls=None, expected_metadata=None + ): + """Build a filter from one already provenance-checked grid path.""" from scipy.interpolate import RegularGridInterpolator as RGI - d = np.load(npz or C.GRID_NPZ, allow_pickle=True) - axes = [np.asarray(a, float) for a in d["axes"]] - dims = [len(a) for a in axes] - rgis = {m: RGI(axes, d[f"V_mu{int(m * 10)}"].reshape(dims), bounds_error=False, fill_value=None) - for m in C.MU_SLICES} - slices = np.array(C.MU_SLICES) + + with np.load(path, allow_pickle=True) as data: + axes = [np.asarray(axis, float) for axis in data["axes"]] + dims = [len(axis) for axis in axes] + slices = ( + np.asarray(data["mus"], float) + if "mus" in data.files + else np.asarray(C.MU_SLICES, float) + ) + values = {} + for mu in slices: + release_key = f"V_{float(mu):.1f}" + legacy_key = f"V_mu{int(round(float(mu) * 10))}" + key = release_key if release_key in data.files else legacy_key + if key not in data.files: + raise ValueError( + f"grid {path} has no value array for mu={float(mu):g}" + ) + values[float(mu)] = np.asarray(data[key], float).reshape(dims) + if expected_metadata is not None: + metadata = tuple(float(data[name]) for name in ("dh", "dm", "bump")) + if not np.allclose( + metadata, expected_metadata, atol=1e-12, rtol=0.0 + ): + raise ValueError( + f"release grid {path} metadata {metadata} does not match " + f"expectation {expected_metadata}" + ) + rgis = { + mu: RGI( + axes, value, bounds_error=False, fill_value=None + ) + for mu, value in values.items() + } def value_fn(X4, mu): nearest = float(slices[np.argmin(np.abs(slices - mu))]) diff --git a/vault/grid.py b/vault/grid.py index c4ac6c1..8bc286d 100644 --- a/vault/grid.py +++ b/vault/grid.py @@ -1,4 +1,12 @@ -"""4D grid Hamilton-Jacobi reach-avoid value iteration — the certifiable oracle. +"""4D grid Hamilton-Jacobi AVOID-ONLY safety value iteration — the certifiable oracle. + +NAMING (corrected 2026-07-29): this module, its output `grid_reachavoid_odd.npz`, and +the `robust_odd_*` grids are all called "reach-avoid" for historical reasons. They are +NOT. The operator below is min(g, max_u min_d V) with NO target margin l(x): it +certifies "never fails", not "reaches a stop". The distinction is not cosmetic -- on the +same plant an avoid-only fallback leaves ~42% of the (v, theta) plane safe-but-never- +arriving, which a true reach-avoid objective collapses to zero. The reach-avoid work +lives in reach_avoid_sac.py and is a separate, learned object. Computes the reach-avoid value V(x; mu) over x = [v, theta, theta_dot, psi_dot] by robust value iteration on f_cert: @@ -21,18 +29,61 @@ from __future__ import annotations import argparse +from collections.abc import Iterator, Sequence +from functools import cached_property import time import numpy as np from . import config as C from . import f_cert as F +from .model_release import get_model_release + +_AXIS_NAMES = ("velocity", "theta", "theta_dot", "yaw_rate") + + +def _axes_from_contract(smoke: bool) -> list[np.ndarray]: + axes = [] + for name in _AXIS_NAMES: + specification = C.ODD_CONTRACT["grid_axes"][name] + count_key = "smoke_nodes" if smoke else "nodes" + axes.append( + np.linspace( + *specification["bounds"], + int(specification[count_key]), + ) + ) + # The v/yaw grids include the declared ODD boundaries exactly. The theta + # grid strictly contains the pitch-failure set so exit-unsafe interpolation + # has an exterior band on the failure axis. + assert axes[0][0] <= C.V_ODD[0] <= C.V_ODD[1] <= axes[0][-1] + assert axes[3][0] <= -C.PSI_ODD < C.PSI_ODD <= axes[3][-1] + assert axes[1][0] < -C.THETA_MAX < C.THETA_MAX < axes[1][-1] + return axes + + +class _ContractAxes(Sequence[np.ndarray]): + """Lazily materialize grid axes after the release lock is first accessed.""" + + def __init__(self, smoke: bool): + self._smoke = smoke + + @cached_property + def _axes(self) -> list[np.ndarray]: + return _axes_from_contract(self._smoke) + + def __getitem__(self, index): + return self._axes[index] + + def __len__(self) -> int: + return len(self._axes) + + def __iter__(self) -> Iterator[np.ndarray]: + return iter(self._axes) + -# Grid resolution; theta (the failure axis) is finest. Box strictly contains the ODD. -AXES_FULL = [np.linspace(-0.5, 1.7, 13), np.linspace(-1.35, 1.35, 29), - np.linspace(-6.0, 6.0, 17), np.linspace(-3.0, 3.0, 19)] -AXES_SMOKE = [np.linspace(-0.5, 1.7, 9), np.linspace(-1.35, 1.35, 15), - np.linspace(-6.0, 6.0, 11), np.linspace(-3.0, 3.0, 11)] +AXES_FULL = _ContractAxes(smoke=False) +AXES_SMOKE = _ContractAxes(smoke=True) V_OOB = -1.0 # value assigned to transitions that exit the grid box (exit-unsafe BC) @@ -64,8 +115,8 @@ def _stencil(axes, strides, points): return idx, w.astype(np.float32) -def solve(mu, axes, controls, max_iters=400, tol=1e-3, verbose=True): - """Robust reach-avoid value iteration for a single mu. Returns (V[grid-shape], n_iters).""" +def solve(mu, axes, controls, max_iters=1000, tol=1e-3, verbose=True): + """Run one robust solve and return value, iterations, terminal dV, convergence.""" dims = [len(a) for a in axes] strides = np.array([dims[1] * dims[2] * dims[3], dims[2] * dims[3], dims[3], 1]) lo = np.array([a[0] for a in axes]) @@ -96,14 +147,19 @@ def solve(mu, axes, controls, max_iters=400, tol=1e-3, verbose=True): print(f" mu={mu} iter {k:3d}: dV={dv:.4f} safe={100 * np.mean(V >= 0):.1f}%", flush=True) if dv < tol: break - return V.reshape(dims), k + 1 + return V.reshape(dims), k + 1, float(dv), bool(dv < tol) def main(): ap = argparse.ArgumentParser(description="4D grid HJ reach-avoid value iteration") ap.add_argument("--smoke", action="store_true", help="small grid, single mu, foreground") ap.add_argument("--mu", type=float, default=None, help="solve a single mu (does not save)") - ap.add_argument("--iters", type=int, default=400) + ap.add_argument( + "--iters", + type=int, + default=1000, + help="iteration budget only; convergence tolerance remains 1e-3", + ) ap.add_argument("--controls", type=int, default=5, help="per-axis control samples (n x n)") args = ap.parse_args() @@ -117,14 +173,29 @@ def main(): values = {} for mu in mus: t0 = time.time() - V, iters = solve(mu, axes, controls, max_iters=args.iters) + V, iters, terminal_dv, converged = solve( + mu, axes, controls, max_iters=args.iters + ) + if not converged: + print( + f"GATE FAIL: mu={mu} did not converge after {iters} iterations " + f"(dV={terminal_dv:.6g} >= 0.001); no grid saved", + flush=True, + ) + return 2 values[mu] = V - print(f"mu={mu}: safe-set {100 * np.mean(V >= 0):4.1f}% of grid | {iters} iters {time.time() - t0:.0f}s") + print( + f"mu={mu}: CONVERGED dV={terminal_dv:.6g} | " + f"safe-set {100 * np.mean(V >= 0):4.1f}% of grid | " + f"{iters} iters {time.time() - t0:.0f}s" + ) if not args.smoke and args.mu is None: np.savez(C.GRID_NPZ, **{f"V_mu{int(m * 10)}": V for m, V in values.items()}, axes=np.array(axes, dtype=object)) + sidecar = get_model_release().write_external_artifact_sidecar(C.GRID_NPZ) print(f"\nsaved -> {C.GRID_NPZ}") + print(f"model provenance -> {sidecar}") return 0 diff --git a/vault/model_release.py b/vault/model_release.py new file mode 100644 index 0000000..b7fb4c4 --- /dev/null +++ b/vault/model_release.py @@ -0,0 +1,329 @@ +"""Provenance-checked access to the vault-controller v2.2 model release. + +This package does not carry robot dynamics or safety artifacts. It consumes the +single release in ``vault-controller/models`` and refuses to load it unless this +repository's lock file is byte-identical to the controller lock. +""" +from __future__ import annotations + +import fnmatch +import hashlib +import json +import os +from functools import lru_cache +from pathlib import Path +from typing import Any + + +class ModelReleaseError(RuntimeError): + """The requested artifact is absent, stale, or not provenance-compatible.""" + + +_PKG_ROOT = Path(__file__).resolve().parent +_REPO_ROOT = _PKG_ROOT.parent +_LOCAL_LOCK = _PKG_ROOT / "data" / "controller_lock.sha256" +_RELEASE_SIDECAR = _PKG_ROOT / "data" / "release_artifacts.model.json" +_CHECKPOINT_MANIFEST = _PKG_ROOT / "data" / "checkpoints_manifest.json" + +_ARTIFACTS = { + "odd_contract": "models/source/odd_contract.json", + "model_geometry": "models/source/geometry/model_geometry.json", + "composite_params": "models/generated/reduced/composite_params.json", + "controller_limits": "models/generated/reduced/controller_limits.json", + "roll_constraint_params": ( + "models/generated/safety/roll_constraint_params.json" + ), + "opt6_kernel": ( + "models/generated/reduced/reduced_struct_fjac_f32_opt6.c" + ), + "coupled_residual": ( + "models/generated/safety/residuals/coupled_residual_fit.json" + ), + "grid_unladen": ( + "models/generated/safety/grids/robust_odd_dh0_dm0_bump0.npz" + ), + "grid_laden": ( + "models/generated/safety/grids/robust_odd_dh30_dm20_bump0.npz" + ), + "v_mlp_unladen_json": ( + "models/generated/safety/mlp/v_mlp_unladen.json" + ), + "v_mlp_unladen_pt": ( + "models/generated/safety/mlp/v_mlp_unladen.pt" + ), + "v_mlp_laden_json": "models/generated/safety/mlp/v_mlp_laden.json", + "v_mlp_laden_pt": "models/generated/safety/mlp/v_mlp_laden.pt", + "safety_params_header": ( + "balance_controller/c/include/balance_controller/bc_safety_params.h" + ), +} + + +def _sha256(path: Path) -> str: + return hashlib.sha256(path.read_bytes()).hexdigest() + + +def _read_json(path: Path) -> dict[str, Any]: + try: + value = json.loads(path.read_text()) + except FileNotFoundError as exc: + raise ModelReleaseError(f"required model release file is missing: {path}") from exc + except json.JSONDecodeError as exc: + raise ModelReleaseError(f"invalid JSON in model release file {path}: {exc}") from exc + if not isinstance(value, dict): + raise ModelReleaseError(f"expected a JSON object in {path}") + return value + + +class ModelRelease: + """Validated paths and metadata for one vault-controller model release.""" + + def __init__( + self, + controller_root: str | Path | None = None, + local_lock: str | Path = _LOCAL_LOCK, + release_sidecar: str | Path = _RELEASE_SIDECAR, + checkpoint_manifest: str | Path = _CHECKPOINT_MANIFEST, + ) -> None: + default_root = _REPO_ROOT.parent / "vault-controller" + configured_root = controller_root or os.environ.get("VAULT_CONTROLLER_ROOT") + self.controller_root = Path(configured_root or default_root).expanduser().resolve() + self.local_lock_path = Path(local_lock).resolve() + self.controller_lock_path = ( + self.controller_root / "models" / "MODEL_INPUTS.lock.json" + ) + self.release_sidecar_path = Path(release_sidecar).resolve() + self.checkpoint_manifest_path = Path(checkpoint_manifest).resolve() + + # PUBLIC-REPO SANITISATION (2026-07-29): this repository is public, so it + # must not CONTAIN the controller lock -- the lock's semantic fields carry + # the robot's mass properties, geometry, and deployment gates. The pin is + # therefore a sha256 DIGEST of the lock, which reveals nothing, and the + # lock itself is read at runtime from the private vault-controller sibling. + # The guarantee is unchanged: a byte-identical lock is exactly what a + # matching sha256 asserts. + try: + pinned = self.local_lock_path.read_text().split()[0].strip().lower() + controller_bytes = self.controller_lock_path.read_bytes() + except FileNotFoundError as exc: + raise ModelReleaseError( + "vault-controller v2.2 model release is unavailable; set " + "VAULT_CONTROLLER_ROOT to the pinned controller checkout " + f"(missing {exc.filename})" + ) from exc + if not (len(pinned) == 64 and all(c in "0123456789abcdef" for c in pinned)): + raise ModelReleaseError( + f"pin file {self.local_lock_path} does not hold a sha256 digest" + ) + actual = hashlib.sha256(controller_bytes).hexdigest() + if actual != pinned: + raise ModelReleaseError( + "MODEL_INPUTS.lock.json mismatch: safety-stable-baselines and " + "vault-controller are not pinned to the same model release " + f"(pinned {pinned[:12]}…, controller {actual[:12]}…)" + ) + + self.lock_sha256 = actual + self.lock = json.loads(controller_bytes) + if self.lock.get("release") != "vault-robot-v2.2-camera-inclusive-reduced-balance": + raise ModelReleaseError( + f"unsupported model release {self.lock.get('release')!r}" + ) + self._release_files = self.lock.get("release_files") + if not isinstance(self._release_files, dict): + raise ModelReleaseError("model lock has no release_files hash table") + + self.model_hashes = { + "composite_params_sha256": self._locked_hash( + _ARTIFACTS["composite_params"] + ), + "opt6_kernel_sha256": self._locked_hash(_ARTIFACTS["opt6_kernel"]), + } + self._verify_release_sidecar() + + def _locked_hash(self, relative_path: str) -> str: + expected = self._release_files.get(relative_path) + if not isinstance(expected, str) or len(expected) != 64: + raise ModelReleaseError( + f"model lock has no SHA-256 for release artifact {relative_path}" + ) + return expected + + def _verify_model_hashes(self, value: Any, source: Path) -> None: + if value != self.model_hashes: + raise ModelReleaseError( + f"model identity mismatch in {source}: expected {self.model_hashes}, " + f"got {value}" + ) + + def _verify_release_sidecar(self) -> None: + sidecar = _read_json(self.release_sidecar_path) + if sidecar.get("lock_sha256") != self.lock_sha256: + raise ModelReleaseError( + f"lock hash mismatch in release sidecar {self.release_sidecar_path}" + ) + self._verify_model_hashes( + sidecar.get("model_hashes"), self.release_sidecar_path + ) + artifacts = sidecar.get("artifacts") + if not isinstance(artifacts, dict): + raise ModelReleaseError( + f"release sidecar has no artifact map: {self.release_sidecar_path}" + ) + if artifacts != _ARTIFACTS: + raise ModelReleaseError( + "release artifact map differs from the consumer's expected roles" + ) + + def artifact_path(self, role: str) -> Path: + """Return an artifact path after lock, role, and file-hash validation.""" + try: + relative_path = _ARTIFACTS[role] + except KeyError as exc: + raise ModelReleaseError(f"unknown model release artifact role: {role}") from exc + path = self.controller_root / relative_path + if not path.is_file(): + raise ModelReleaseError(f"model release artifact is missing: {path}") + expected = self._locked_hash(relative_path) + actual = _sha256(path) + if actual != expected: + raise ModelReleaseError( + f"model release artifact hash mismatch for {path}: " + f"expected {expected}, got {actual}" + ) + return path + + def load_json(self, role: str) -> dict[str, Any]: + return _read_json(self.artifact_path(role)) + + def safety_grid_mus(self, mode: str = "unladen") -> tuple[float, ...]: + try: + values = self.lock["phase5_safety_artifacts"]["gates"][ + "robust_grid_convergence" + ]["terminal_dv"][mode] + except (KeyError, TypeError) as exc: + raise ModelReleaseError( + f"model lock has no robust-grid metadata for mode {mode!r}" + ) from exc + return tuple(float(value) for value in values) + + def verify_external_artifact( + self, artifact: str | Path, sidecar: str | Path | None = None + ) -> Path: + """Verify an external artifact against an adjacent model-hash sidecar.""" + path = Path(artifact).expanduser().resolve() + sidecar_path = ( + Path(sidecar).expanduser().resolve() + if sidecar is not None + else Path(f"{path}.model.json") + ) + metadata = _read_json(sidecar_path) + self._verify_model_hashes(metadata.get("model_hashes"), sidecar_path) + if metadata.get("lock_sha256") != self.lock_sha256: + raise ModelReleaseError( + f"model lock hash mismatch in artifact sidecar {sidecar_path}" + ) + expected = metadata.get("artifact_sha256") + if not isinstance(expected, str) or len(expected) != 64: + raise ModelReleaseError( + f"artifact sidecar has no valid artifact_sha256: {sidecar_path}" + ) + if not path.is_file(): + raise ModelReleaseError(f"artifact referenced by sidecar is missing: {path}") + actual = _sha256(path) + if actual != expected: + raise ModelReleaseError( + f"artifact hash mismatch for {path}: expected {expected}, got {actual}" + ) + return path + + def write_external_artifact_sidecar(self, artifact: str | Path) -> Path: + """Write provenance for a newly generated, non-release artifact.""" + path = Path(artifact).expanduser().resolve() + if not path.is_file(): + raise ModelReleaseError(f"cannot sidecar missing artifact: {path}") + sidecar_path = Path(f"{path}.model.json") + payload = { + "schema_version": 1, + "lock_sha256": self.lock_sha256, + "model_hashes": self.model_hashes, + "artifact_sha256": _sha256(path), + } + sidecar_path.write_text(json.dumps(payload, indent=2, sort_keys=True) + "\n") + return sidecar_path + + def verify_checkpoint(self, checkpoint: str | Path) -> Path: + """Require a matching sidecar or an explicit compatible manifest entry.""" + requested = Path(checkpoint).expanduser() + path = requested if requested.suffix == ".zip" else requested.with_suffix(".zip") + path = path.resolve() + sidecar = Path(f"{path}.model.json") + if sidecar.is_file(): + return self.verify_external_artifact(path, sidecar) + + manifest = _read_json(self.checkpoint_manifest_path) + if manifest.get("release_lock_sha256") != self.lock_sha256: + raise ModelReleaseError( + f"checkpoint manifest lock mismatch: {self.checkpoint_manifest_path}" + ) + entries = manifest.get("checkpoints") + if not isinstance(entries, list): + raise ModelReleaseError( + f"checkpoint manifest has no checkpoints list: {self.checkpoint_manifest_path}" + ) + names = (path.name, path.stem) + for entry in entries: + if not isinstance(entry, dict): + continue + pattern = entry.get("pattern") + if not isinstance(pattern, str) or not any( + fnmatch.fnmatch(name, pattern) for name in names + ): + continue + status = entry.get("status") + if status == "compatible": + self._verify_model_hashes( + entry.get("model_hashes"), self.checkpoint_manifest_path + ) + if entry.get("lock_sha256") != self.lock_sha256: + # `lock_sha256` records the release a policy was TRAINED under and + # is never restamped -- restamping would assert a policy had been + # trained against a bound it never saw. But a release can change + # in ways that provably do not touch the physics a policy learned + # (added provenance, a new scalar, a re-pinned generator commit), + # and the model_hashes check above is what actually guards that. + # So a checkpoint may carry an explicit allowlist of later releases + # it remains valid under, each with a written justification. A bare + # digest with no reason is refused: the reason is the review unit. + reason = (entry.get("also_valid_under_lock_sha256") or {}).get( + self.lock_sha256 + ) + if not (isinstance(reason, str) and reason.strip()): + raise ModelReleaseError( + f"checkpoint manifest lock mismatch for {path.name}: " + f"trained under {str(entry.get('lock_sha256'))[:12]}, " + f"release is {self.lock_sha256[:12]} and carries no " + "justified also_valid_under_lock_sha256 entry" + ) + expected = entry.get("artifact_sha256") + if not path.is_file() or _sha256(path) != expected: + raise ModelReleaseError( + f"compatible checkpoint hash mismatch for {path}" + ) + return path + raise ModelReleaseError( + f"checkpoint {path.name} is quarantined as incompatible with " + f"model release {self.lock_sha256}; replacement RL training requires " + "explicit operator authorization" + ) + raise ModelReleaseError( + f"checkpoint {path.name} has no model-hash sidecar or compatible manifest " + "entry; refusing an unpinned policy. Replacement RL training requires " + "explicit operator authorization" + ) + + +@lru_cache(maxsize=1) +def get_model_release() -> ModelRelease: + """Return the process-wide validated release.""" + return ModelRelease() diff --git a/vault/mujoco_env.py b/vault/mujoco_env.py index cec465c..13ea492 100644 --- a/vault/mujoco_env.py +++ b/vault/mujoco_env.py @@ -54,15 +54,24 @@ class ContactSafetyEnv(gym.Env): metadata = {"render_modes": []} - def __init__(self, mu_range=C.MU_RANGE, tau_max=C.TAU_MAX, max_steps=200, + def __init__(self, mu_range=None, tau_max=None, max_steps=200, wheel="cylinder", seed=None, reach_avoid=False): super().__init__() - self.mu_range = mu_range - self.tau_max = tau_max + self.mu_range = C.MU_RANGE if mu_range is None else mu_range + self.tau_max = C.TAU_MAX if tau_max is None else tau_max self.max_steps = max_steps self.wheel = wheel self.reach_avoid = reach_avoid - hi = np.array([2.0, 1.5, 8.0, 3.0, 1.0], np.float32) + hi = np.array( + [ + max(abs(x) for x in C.DOMAIN_V), + max(abs(x) for x in C.DOMAIN_THETA), + max(abs(x) for x in C.DOMAIN_THETA_DOT), + max(abs(x) for x in C.DOMAIN_PSI_DOT), + C.MU_RANGE[1], + ], + np.float32, + ) self.observation_space = spaces.Box(-hi, hi, dtype=np.float32) self.action_space = spaces.Box(-1.0, 1.0, shape=(2,), dtype=np.float32) self._rng = np.random.default_rng(seed) diff --git a/vault/mujoco_model.py b/vault/mujoco_model.py index c56ff40..02e8f17 100644 --- a/vault/mujoco_model.py +++ b/vault/mujoco_model.py @@ -11,55 +11,302 @@ State/sign conventions match f_cert: v fwd along +x, theta = pitch about +y (theta>0 nose-down/forward), psi about +z; R = Rz(psi)*Ry(theta), roll ~= 0 nominal. -Masses + inertias come from data/composite_params.json (the single source). The chassis yaw -inertia subtracts the wheels' parallel-axis contribution so the REASSEMBLED whole-robot yaw -inertia equals the composite value (no double-counting). Roll inertia is not a 4-state param -(roll is a wheel constraint), so it is reconstructed to a physically-valid value; it does not -enter the certified planar dynamics. Run this module to print the assembly invariants. +Masses, inertias, and actuator limits come from the provenance-pinned +vault-controller release. The chassis yaw inertia subtracts the wheels' +parallel-axis contribution so the REASSEMBLED whole-robot yaw inertia equals +the composite value (no double-counting). Roll inertia is not a 4-state param +(roll is a wheel constraint), so it is reconstructed to a physically-valid +value; it does not enter the certified planar dynamics. Run this module to +print the assembly invariants. """ from __future__ import annotations -import json from pathlib import Path from . import config as C - -_CP = Path(__file__).resolve().parent / "data" / "composite_params.json" # vendored single source +from .model_release import get_model_release def load_params() -> dict: - return json.loads(_CP.read_text()) + """Load structural parameters plus locked controller limits.""" + release = get_model_release() + params = release.load_json("composite_params") + params.update(release.load_json("controller_limits")) + params.update(release.load_json("model_geometry")) + return params # Non-wheel contact-geometry approximation (contact_geometry=True) -------------------- -# Robert's reduced 4-state model has no full-robot geometry (by design, see CLAUDE.md); -# this repo has no chassis/leg CAD either. The dimensions below are read directly from -# the real robot's URDF (SafeRoboticsLab/VaultRobot_IsaacLab, delivery_robot_v2), which -# is the actual embodiment behind the reduced model (its half-wheel-separation of -# 0.140375 m matches OPT6_DH in dynamics.py exactly): -# right_hip_joint parent=base_link xyz="0 -0.26780 -0.05100" -# right_...->knee xyz="0 -0.01577500 0.20350000" (upper leg span ~0.2035 m) -# right_lower->foot xyz="0 -0.02215000 0.20000" (lower leg span ~0.2000 m) -# i.e. hip mount ~0.05 m below the body_wheel/axle plane, at ~1.91x the wheel's lateral -# offset (0.2678 / 0.140375), with a ~0.40 m fully-extended leg. The physical robot's -# nominal stance folds the hip/knee so the foot sits near the wheel-contact height -# (composite_params.json's own derivation notes describe the legs as "locked ... -# tucked" for the reduced model) -- there is no folded-leg kinematics in this planar -# plant, so we approximate the tucked leg as a single rigid capsule fixed to the -# chassis: outboard of the wheel (no self-overlap), spanning from axle height down to -# just short of the ground at nominal upright pose (contacts only once the chassis -# pitches/rolls). This is a coarse stand-in for upper_leg_link + lower_leg_link, not a -# faithful reproduction of the coupled hip-knee linkage. -_LEG_Y_RATIO = 0.26780 / 0.140375 # hip lateral offset / body-wheel lateral offset +# Robert's reduced 4-state model has no full-robot geometry (by design). The +# articulated stand-in below is a COARSE VISUAL/CONTACT approximation of the +# tucked legs: a single rigid capsule per side, outboard of the wheel, spanning +# from axle height to just short of the ground at upright. The real linkage +# geometry lives in the PRIVATE release URDF (vault-controller sibling); only a +# dimensionless placement ratio is used here, and it is an approximation, not a +# released dimension. +_LEG_Y_RATIO = 1.91 # approx hip/wheel lateral placement (dimensionless, visual) _LEG_RADIUS = 0.025 # m, slender capsule (real leg links are thin) # Ground clearance: C.LEG_GROUND_CLEARANCE (config.py) -- shared with contact_margin.py, which # normalizes by the SAME constant so a nominal upright state's margin is ~1.0, not ~0.02 m. -def build_mjcf(wheel: str = "torus", mu: float = 1.0, tube_radius: float = 0.02, +def _visual_mesh_overlay(wheel_half_sep: float | None = None) -> dict | None: + """Release-mesh visual overlay, resolved from the PRIVATE sibling at runtime. + + Returns None SILENTLY only when vault-controller is not checked out -- the + plant then renders its primitive geoms exactly as before. Once the checkout is + found, any malformed content (unparseable URDF, missing/corrupt/unsupported + mesh, malformed joint, kinematic cycle, lock mismatch, broken wheel-frame + invariant) degrades to primitives WITH a warning naming the reason, never a + wrong render. Everything here is read at runtime: mesh paths, link names, and + placements come from the private articulated URDF, so this PUBLIC file carries + no dimensions. + + The URDF is verified against the controller release lock when the lock is + readable, so a dirty/stale visual source cannot silently ride alongside + hash-validated dynamics params. The decimated meshes are NOT pinned by the + lock today; they get format and structural-integrity checks only. + + ALL mesh-bearing links are attached, not just chassis+wheels: leg linkages, + knee wheels and feet are posed by forward kinematics at the TUCKED display + configuration (knees at 180 deg, all else zero) and fixed to the chassis body, + since the reduced plant has no leg joints. The two body-wheel meshes attach to + the spinning wheel bodies, whose URDF joint frames are cross-checked against + the plant's generated wheel frames (identity rotation at (0, +-wheel_sep/2, 0)). + + The overlay is VISUAL ONLY: contype=0 conaffinity=0, display group 2; every + body keeps its explicit . Dynamics equality is pinned by + test_visual_meshes_do_not_change_dynamics. + """ + import os + import warnings + + root = Path( + os.environ.get("VAULT_CONTROLLER_ROOT") + or Path(__file__).resolve().parents[2] / "vault-controller" + ) + urdf = root / "models/source/urdf/articulated_v2_2/robot.urdf" + meshdir = root / "models/assets/meshes_decimated" # highres exceeds MuJoCo's decoder + if not (urdf.is_file() and meshdir.is_dir()): + return None + try: + return _build_mesh_overlay(root, urdf, meshdir, wheel_half_sep) + except Exception as exc: + warnings.warn( + f"release-mesh overlay disabled ({exc}); rendering primitive geoms", + stacklevel=2) + return None + + +def _check_mesh_asset(path: Path) -> None: + """Reject missing, unsupported, or structurally corrupt mesh files up front, + so a bad asset degrades to primitives here instead of raising later inside + MjModel.from_xml_string. MuJoCo accepts STL/OBJ/MSH; STL (all we ship) also + gets an integrity check: binary size must match its triangle count.""" + if not path.is_file(): + raise ValueError(f"mesh not found: {path.name}") + suffix = path.suffix.lower() + if suffix not in (".stl", ".obj", ".msh"): + raise ValueError(f"unsupported mesh format: {path.name}") + if suffix == ".stl": + size = path.stat().st_size + with open(path, "rb") as fh: + head = fh.read(84) + if len(head) == 84: + ntri = int.from_bytes(head[80:84], "little") + if size == 84 + 50 * ntri: + return + if head[:5].lower() == b"solid": + return # ASCII STL + raise ValueError(f"corrupt STL (size/triangle-count mismatch): {path.name}") + + +def _build_mesh_overlay(root: Path, urdf: Path, meshdir: Path, + wheel_half_sep: float | None) -> dict: + """The actual overlay builder. Raises on ANY malformed input; the caller + turns that into warn-and-degrade.""" + import hashlib + import json + import xml.etree.ElementTree as ET + + import numpy as np + + tree = ET.parse(urdf).getroot() + + # Dirty/stale guard: the visual source must be the SAME bytes the release + # lock pinned, matching the hash discipline the dynamics params get. + lock_path = root / "models/MODEL_INPUTS.lock.json" + if lock_path.is_file(): + pinned = json.loads(lock_path.read_text()).get("release_files", {}).get( + "models/source/urdf/articulated_v2_2/robot.urdf") + if pinned and hashlib.sha256(urdf.read_bytes()).hexdigest() != pinned: + raise ValueError("articulated URDF does not match the release lock") + + def rot(rpy): + r, p_, y = rpy + cr, sr, cp, sp, cy, sy = np.cos(r), np.sin(r), np.cos(p_), np.sin(p_), np.cos(y), np.sin(y) + Rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]]) + Ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]]) + Rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]]) + return Rz @ Ry @ Rx + + def vec(s, n=3): + return np.array([float(x) for x in (s or " ".join(["0"] * n)).split()]) + + # Tucked display configuration: knees at 180 deg (operator-provided joint + # constant, matching the retract lane's knee = 180deg hold). All other joints + # at zero. This is a VISUAL pose only. + TUCKED_Q = {"right_knee_joint": np.pi, "left_knee_joint": np.pi} + + def axis_rot(a, angle): + # a must already be unit-norm; callers validate before normalizing. + K = np.array([[0, -a[2], a[1]], [a[2], 0, -a[0]], [-a[1], a[0], 0]]) + return np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * (K @ K) + + # joint tree: child link -> (parent link, R, t) at the DISPLAY configuration + parent_of: dict[str, tuple[str, np.ndarray, np.ndarray]] = {} + for j in tree.iter("joint"): + jname, jtype = j.get("name"), j.get("type") + parent, child = j.find("parent"), j.find("child") + if parent is None or child is None: + raise ValueError(f"joint {jname}: missing or ") + child_link = child.get("link") + if child_link in parent_of: + raise ValueError(f"link {child_link}: more than one parent joint") + o = j.find("origin") + R_origin = rot(vec(o.get("rpy") if o is not None else None)) + angle = TUCKED_Q.get(jname, 0.0) + if angle: + if jtype not in ("revolute", "continuous"): + raise ValueError( + f"tucked joint {jname} is type {jtype!r}, expected revolute/continuous") + ax = j.find("axis") + # URDF: an ABSENT defaults to +X; a PRESENT without xyz + # is malformed (it must not silently become a no-op rotation). + if ax is not None and ax.get("xyz") is None: + raise ValueError(f"joint {jname}: element without xyz") + axis = vec(ax.get("xyz")) if ax is not None else np.array([1.0, 0.0, 0.0]) + norm = float(np.linalg.norm(axis)) + if norm < 1e-9: + raise ValueError(f"joint {jname}: zero axis on a rotated tucked joint") + R_origin = R_origin @ axis_rot(axis / norm, angle) + parent_of[child_link] = ( + parent.get("link"), + R_origin, + vec(o.get("xyz") if o is not None else None), + ) + + def fk(link): + """Pose of link frame in base_link frame at the display configuration.""" + R, t = np.eye(3), np.zeros(3) + chain = [] + while link in parent_of: + chain.append(parent_of[link]) + link = parent_of[link][0] + if len(chain) > len(parent_of): + raise ValueError("joint graph contains a cycle") + if link != "base_link": + raise ValueError(f"kinematic chain roots at {link!r}, not base_link") + for _, Rj, tj in reversed(chain): + t = R @ tj + t + R = R @ Rj + return R, t + + def quat(R): + """Rotation matrix -> MJCF quaternion, valid for ALL rotations. + + The naive w = sqrt(1+trace)/2 form divides by w, which is EXACTLY ZERO for + any rotation by pi -- i.e. for the tucked knee fold itself. An earlier + version short-circuited that case to the identity, which discarded the + fold's orientation while keeping its translation: every pi-rotated link + rendered unrotated at the correct position (feet looked right, lower legs + pointed the wrong way, and the kinematic chain appeared broken). Use the + standard largest-component branch instead, which is well-conditioned + everywhere. + """ + tr = R[0, 0] + R[1, 1] + R[2, 2] + if tr > 0: + s = 2 * np.sqrt(1 + tr) + w, x, y, z = s / 4, (R[2, 1] - R[1, 2]) / s, (R[0, 2] - R[2, 0]) / s, (R[1, 0] - R[0, 1]) / s + elif R[0, 0] >= R[1, 1] and R[0, 0] >= R[2, 2]: + s = 2 * np.sqrt(max(1e-12, 1 + R[0, 0] - R[1, 1] - R[2, 2])) + w, x, y, z = (R[2, 1] - R[1, 2]) / s, s / 4, (R[0, 1] + R[1, 0]) / s, (R[0, 2] + R[2, 0]) / s + elif R[1, 1] >= R[2, 2]: + s = 2 * np.sqrt(max(1e-12, 1 + R[1, 1] - R[0, 0] - R[2, 2])) + w, x, y, z = (R[0, 2] - R[2, 0]) / s, (R[0, 1] + R[1, 0]) / s, s / 4, (R[1, 2] + R[2, 1]) / s + else: + s = 2 * np.sqrt(max(1e-12, 1 + R[2, 2] - R[0, 0] - R[1, 1])) + w, x, y, z = (R[1, 0] - R[0, 1]) / s, (R[0, 2] + R[2, 0]) / s, (R[1, 2] + R[2, 1]) / s, s / 4 + return f"{w:.8f} {x:.8f} {y:.8f} {z:.8f}" + + WHEELS = {"right_body_wheel_link": "right_wheel", "left_body_wheel_link": "left_wheel"} + + # The wheel meshes bypass fk(): their geoms attach to the PLANT's spinning + # wheel bodies, which build_mjcf generates independently at identity rotation, + # (0, -+wheel_sep/2, 0) in the chassis frame. That is an invariant between two + # separately-authored frame definitions, so VERIFY it instead of trusting it: + # any drift in the URDF wheel joints must degrade, not render wrong wheels. + if wheel_half_sep is None: + wheel_half_sep = load_params()["wheel_sep"] / 2.0 + for wlink, sign in (("right_body_wheel_link", -1.0), ("left_body_wheel_link", 1.0)): + if wlink not in parent_of: + raise ValueError(f"URDF has no joint for {wlink}") + Rw, tw = fk(wlink) + expected = np.array([0.0, sign * wheel_half_sep, 0.0]) + if not (np.allclose(Rw, np.eye(3), atol=1e-9) + and np.allclose(tw, expected, atol=1e-6)): + raise ValueError( + f"{wlink} joint frame {tw} does not match the plant wheel frame " + f"{expected} -- wheel overlay would render at the wrong pose") + + assets, chassis_geoms, wheel_geoms = [], [], {"right_wheel": "", "left_wheel": ""} + for link in tree.iter("link"): + name = link.get("name") + # ALL elements, not only the first; each gets its own MJCF mesh + # asset because per-visual scale attributes may differ. + for vi, vis in enumerate(link.findall("visual")): + mesh = vis.find("geometry/mesh") + if mesh is None: + continue + stl = meshdir / Path(mesh.get("filename", "")).name + _check_mesh_asset(stl) + scale = mesh.get("scale") + if scale is not None and len(vec(scale)) != 3: + raise ValueError(f"link {name} visual {vi}: malformed mesh scale") + scale_attr = f' scale="{scale}"' if scale is not None else "" + o = vis.find("origin") + Rv = rot(vec(o.get("rpy") if o is not None else None)) + tv = vec(o.get("xyz") if o is not None else None) + mid = f"vis_{name}" if vi == 0 else f"vis_{name}_{vi}" + assets.append(f'') + if name in WHEELS: + # wheel link frame == plant wheel body frame (verified above) + wheel_geoms[WHEELS[name]] += ( + f'') + continue + Rl, tl = fk(name) + Rg, tg = Rl @ Rv, Rl @ tv + tl + rgba = "0.88 0.88 0.90 1" if name == "base_link" else "0.72 0.74 0.78 1" + chassis_geoms.append( + f'') + + if not chassis_geoms or not all(wheel_geoms.values()): + raise ValueError("URDF yielded no chassis meshes or is missing a wheel mesh") + return { + "asset": "" + "".join(assets) + "", + "chassis": "".join(chassis_geoms), + "right_wheel": wheel_geoms["right_wheel"], + "left_wheel": wheel_geoms["left_wheel"], + } + + +def build_mjcf(wheel: str = "torus", mu: float = 1.0, params: dict | None = None, solref: tuple = (0.02, 1.0), margin: float = 0.0, imu_pos: tuple | None = None, imu_quat: tuple | None = None, - contact_geometry: bool = False) -> str: + contact_geometry: bool = False, visual_meshes: str = "auto") -> str: """Return an MJCF string. wheel in {'torus','cylinder'}; mu = wheel-ground friction. solref = (timeconst, dampratio): contact compliance. Default (0.02, 1.0) is MuJoCo's stiff @@ -76,6 +323,7 @@ def build_mjcf(wheel: str = "torus", mu: float = 1.0, tube_radius: float = 0.02, """ p = params or load_params() R = p["wheel_radius"] + wheel_contact_half_width = p["wheel_contact_half_width"] d = p["wheel_sep"] / 2.0 m_b, h_cm = p["m_b"], p["h_cm"] I_pitch, I_yaw = p["I_pitch"], p["I_yaw"] @@ -90,21 +338,29 @@ def build_mjcf(wheel: str = "torus", mu: float = 1.0, tube_radius: float = 0.02, # planar dynamics we certify. I_roll = max(I_yaw_chassis, abs(I_yaw_chassis - I_pitch) + 1e-3) + overlay = _visual_mesh_overlay(wheel_half_sep=d) if visual_meshes == "auto" else None + # When meshes are active, primitive geoms move to display group 3 (hidden by + # default in viewer and Renderer) -- PURELY visual; contact and inertial + # behaviour is untouched, which test_visual_meshes_do_not_change_dynamics pins. + hide = ' group="3"' if overlay else "" + if wheel == "torus": ext = ('' f'' - f'') + f'' + '') asset = '' def wheel_geom(name): - return (f'') elif wheel == "cylinder": ext = asset = "" def wheel_geom(name): - return (f'') else: @@ -113,7 +369,7 @@ def wheel_geom(name): if contact_geometry: # Chassis: same box, now with real collision (permitted-contact set excludes it). base_vis = (f'') leg_y = d * _LEG_Y_RATIO # Capsule "fromto" endpoint is the segment end, not the rounded surface -- the actual @@ -124,13 +380,13 @@ def wheel_geom(name): leg_geoms = "".join( f'' for side, sign in (("right", -1), ("left", 1)) ) else: base_vis = (f'') + f'contype="0" conaffinity="0"{hide} rgba="0.4 0.5 0.8 0.4"/>') leg_geoms = "" imu_site = sensor_block = "" @@ -142,15 +398,19 @@ def wheel_geom(name): '') # Chassis frame origin AT the wheel axle; upright => axle at height R above ground. + overlay_asset = overlay["asset"] if overlay else "" + overlay_chassis = overlay["chassis"] if overlay else "" + overlay_right = overlay["right_wheel"] if overlay else "" + overlay_left = overlay["left_wheel"] if overlay else "" return f"""