Skip to content

ph: the learned SE(2) port-Hamiltonian vehicle, trained natively (cvc::ph) - #758

Merged
transfix merged 15 commits into
masterfrom
feat/ph-native
Oct 21, 2026
Merged

transfix merged 15 commits into
masterfrom
feat/ph-native

Conversation

@transfix

Copy link
Copy Markdown
Owner

Stacked on #749 (feat/plant-log-liesphgp): cvc ph-train reads the run directories cvc plant-log writes.

This ports to C++ the SE(2) port-Hamiltonian vehicle (CVC-Lab/LieSPHGP's method, the variant fitted to Jolt runs of the Humvee and the RoboRacer), together with its rollout-loss trainer. Nothing here needs Python or JAX, at runtime or in CI. The JAX package is now only the parity reference.

What it adds

  • inc/cvc/ph/ (new, cvc::ph):
    • mat3.h: 3x3 algebra, the six-level Cholesky form of an SPD matrix and its reverse, the SO(2) log.
    • rff.h: Matern random Fourier features and their GP.
    • rng.h: written-out normal and gamma draws. A seed draws the same features on every compiler; the std:: distributions differ between standard libraries.
    • se2_vehicle.h: H, the drift (coadjoint, potential, D(v), G(v)), the Lie-IMEX step with the damping implicit, and rollouts, in float or double. The model file is JSON (cvc-ph-se2/1).
    • optim.h: cosine decay, global-norm clipping, Adam and Prodigy, written to match optax 0.2.8.
    • se2_train.h: the rollout objective and its hand-written reverse pass, train.py's optimizer, the training loop, and the data.
      • The reverse pass is BPTT through every step: the implicit 3x3 solves, the SPD levels, the random-feature GPs, and grad V's own derivative.
      • The optimizer: clip, cosine-decayed Adam on the GP weights, Prodigy per level.
      • The data: plant-log directories, seed split, observation noise, RMS scales, windows.
      • A window whose rollout diverges is left out of the loss and the gradient, and reported.
  • cvc ph-train (cvc-cli, beside cvc plant-log in the Vehicle Physics group):
    • fits a model to plant-log runs and writes the JSON;
    • --init continues a model with its own scales;
    • --dump-dataset writes the noisy flights it trained on.

Tests

ph_se2_test (12), ph_se2_train_test (8), and 3 cvc_cli_test cases. The goldens (ph_se2_golden.json, ph_optim_golden.json) were generated once, outside CI, from the JAX package in float64.

  • Forward, against the Humvee fit (64 features, 1,691 parameters):
    • one step at 16 states is within 1.9e-16 relative in double and 1.1e-7 in float;
    • a 25-step rollout is within 3.1e-15;
    • dH/dt = -v.Dv + v.Gu holds exactly;
    • with the GPs off, the drift is the hand-written linear-tire bicycle;
    • the step is second order (x3.99 per halving).
  • Gradient: within 8.1e-15 of JAX's autodiff of the same objective on all 1,691 entries, and within 2.2e-6 of central differences.
  • Optimizer: 300 steps match optax to 1.6e-16, and Prodigy alone matches it while its d grows from 1e-3 to 1.95.
  • Determinism: the objective is bit-identical on any thread count, and a refit on 4 threads reproduces an 8-thread model bit for bit.

Locally on this branch: ph_se2_test 12/12, ph_se2_train_test 8/8, physics_plant_log_test 7/7, nav_dyn_bike_test 9/9, and the cvc_cli_test plant-log, ph-train, help and dispatch cases.

How it was checked against JAX (outside CI)

The C++ rollout fits were held to JAX's own rollout fits: the same data and recipe, 3 seeds a side, scored by one evaluator on the held-out seed (open-loop prediction binned by lateral g, at 0.02 / 0.06 / 0.5 / 1 s).

  • Within the seed spread: 98 of 100 cells per vehicle, and 199 of 200 with every seed (6 JAX and 6 C++ for the Humvee, 3 and 5 for the RoboRacer).
  • Lockstep: from the same initial model, batches and reparameterization draws, the C++ training loop follows JAX's train step (float64) to 1e-14 in the parameters for 50 steps, and to 1e-11 after 200.
  • The Humvee leaned higher at short horizons in the first 3 seeds (1.5x at one tick). It is the random initial draw:
    • with 6 seeds a side it is 1.24x, p = 0.11;
    • JAX in float64 lands beside JAX in float32;
    • started from JAX's initial models, the C++ fits reproduce each JAX seed to about 5%.
  • On the 2 s rollout loss both sides train, the C++ fits are as good or better: Humvee 0.87-1.10 against 0.96-1.56 on the held-out monitor.
  • Speed: a 5,000-step fit takes 71-87 s on 8 threads, against about 5 min for the JAX rollout fit and 35-47 min for the JAX EKF fit.

Known, for a later milestone

The rollout recipe can blow up on either side: the BPTT gradient explodes through a window whose learned dynamics have gone unstable, while the loss stays finite. It happened in 1 of 4 JAX RoboRacer seeds (float32 overflow at step 962) and 1 of 6 C++ ones (step 1,772). train.py aborts there, and so does this; a guard is open.

Next

  • The EKF-ELBO trainer, held to the JAX EKF fits' prediction and calibration numbers.
  • dyn_bike's linearization as the D/G prior.
  • The GRL-SNAM torch twin.
  • The hooks into the nav and GRL-SNAM training flows.

…s residual against Jolt

A port-Hamiltonian vehicle model fitted to the physics needs the physics' trajectories in a
plant-learning format, and a baseline to beat. This adds both.

- cvc::physics::plant_log (plant_log.h): samples a sim_world's agents on a nav_plant every tick
  as planar rigid bodies in SI: the centre of mass's pose on SE(2), its body twist, the
  steering, the command nav held over the tick (zero-order hold), the measured acceleration,
  contacts and wheels on the ground.
- one_tick_residuals: the plan's per-tick drift (D14) with diff::dyn_plant beside the kinematic
  bicycle. Both step from Jolt's state with the held command, binned by the lateral g Jolt
  measured. Ticks with a static contact, a wheel off the ground, an impact (> 1.5 g) or at
  rest are left out, and counted.
- cut_flights / write_npy: equal-length windows, [x, y, psi, vx, vy, r, drive, steer] float64.
- cvc-plant-log: drives the Humvee or RoboRacer on Jolt through nav in a slalom (swerves past
  0.3 g), the coef trainer's city, or a cvc::world city bundle (vehicle routes on its
  rasterized map, the vehicles spawned on its terrain), and writes flights.npy, meta.json and
  residual.json. src/cvc/tools/liesphgp_pack.py packs runs into the dataset pickle LieSPHGP's
  loaders read (the SE(3)-lifted 20-wide rows of its ROV loader with control_dim 2, or 8-wide
  SE(2)), with observation noise at a stated sigma.
- nav_plant_options::ground_height: a start's ground for a terrain that is not flat (empty:
  ground_z, as before).
- The roadmap's drift section publishes dyn_plant's residual at nav's 0.06 s tick: 4-18x closer
  than the kinematic bicycle above 0.5 g in position; worse in speed for the RoboRacer, whose
  driver model is faster than dyn_bike_si_of's 0.1 s drive lag.

physics_plant_log_test (4): a run a model made leaves that model no residual (kinematic and
dynamic, nsub 1/2/6); flights, their layout and the .npy; a Humvee slalom on Jolt (row k's
command is the one held over the tick, the centre of mass moves with its logged twist to
0.023 m/s, peak 0.87 g); ground_height spawns as ground_z does.
…unding (1e-9)

The run is made in the test and replayed in plant_log.cpp; a compiler may contract the two into
FMAs differently (aarch64), so compare to rounding rather than to 1e-12. Include <iterator>.
…ven; gentler margins

- fit_dyn_bike / dyn_bike_cost: dyn_bike_si_of's knobs a spec sheet does not give, fitted to
  logged runs by coordinate descent on the one-tick twist residual (drive and steering lags,
  factors on the tires' B, the yaw inertia, and drag + rolling resistance). dyn_bike_si_of is
  unchanged; this measures what the plant wants. one_tick_residuals takes a dyn_bike_si too.
  cvc-plant-log --fit-dyn fits on every seed but the last and reports the last.
- Bundle routes: the goal is drawn from the free cells 5-15% of the map away. Drawing from the
  whole map landed in that ring ~6% of the time, which was the "planner fails on ~60% of
  goals". The planner keeps 2 m off building meshes, and an agent stopped for 5 s is replanned
  from where it stands.
- --accel-margin / --brake-margin (nav_plant's), recorded in meta.json: at the full ±6 m/s^2 the
  RoboRacer's low chassis box pitches onto the ground under braking and acceleration.
- --verbose (each agent every 5 s) and --samples (samples.csv); samples carry the roll.

physics_plant_log_test: + FitsTheDynamicModelToRunsOfAKnownVehicle (a run made by a vehicle off
the spec sheet: every knob recovered on its grid, cost 0).
…itted to Jolt

The RoboRacer's city column is from runs planning with 0.4 of its acceleration and braking (its
chassis box scraped the ground at the full 6 m/s^2). The fit (fit_dyn_bike, held-out seed) puts
both vehicles' tires 2.5-4x softer than the spec sheet's B, and the RoboRacer with no drive lag and
no uncompensated resistance. dyn_bike_si_of's defaults are unchanged.
The scene drivers and the directory writer move out of the standalone
cvc-plant-log tool into cvc/physics/plant_log_scene.h (plant_log_options,
drive_plant_log_scene, write_plant_log), so a trainer can drive the scenes in
process. The command line becomes the `cvc plant-log` subcommand of cvc-cli,
and the tool and its CVC_BUILD_PLANT_LOG option are gone.

The three scenes run through `cvc plant-log` write files byte-identical to the
old tool's (flights.npy, meta.json, residual.json, samples.csv), and the same
stdout but for the timing. physics_plant_log_test adds the options' defaults
and a swerve driven twice to identical runs; cvc_cli_test covers the
subcommand.
… dyn_bike_si_of

The 0.06 s residual table now shows the dynamic model before #753 / with its
values now / the kinematic bicycle. Below 0.5 g the fitted values are 1.3-9x
closer, and the RoboRacer's speed residual matches the kinematic bicycle's.
Refitted against #753's values, the flat-ground fits find nothing left: tire
factors back at x1, and the defaults do as well as a fit on the held-out seed.
The datasets are byte-identical (Jolt drives; dyn_bike_si_of only enters the
residuals).
…ith the JAX package

cvc::ph's first piece: the SE(2) variant of LieSPHGP (stochastic port-Hamiltonian
GPs on Lie groups) that Phase A fit to Jolt runs, as a torch- and JAX-free C++ forward.

- mat3.h: 3x3 algebra, solve3, the six-level Cholesky form of an SPD matrix and its
  reverse, the SO(2) log.
- rff.h: Matern random Fourier features and their GP, with the reverse.
- rng.h: xoshiro256** with written-out normal and gamma draws, so a seed draws the
  same features on every compiler.
- se2_vehicle.h: H, the drift (coadjoint, potential, D(v), G(v)), the Lie-IMEX step
  with the damping implicit, rollouts; float or double. se2_model holds the features,
  parameters and scales, and reads and writes them as JSON (cvc-ph-se2/1).

ph_se2_test: against goldens generated once from the JAX package in float64 (the
Humvee fit: 64 features, 1,691 parameters), one step at 16 states is within 1.9e-16
relative in double and 1.1e-7 in float; a 25-step rollout within 3.1e-15. The energy
balance dH/dt = -v.Dv + v.Gu holds at every state; with the GPs off the drift is the
hand-written linear-tire bicycle; the step is second order (x3.99 per halving).
…ph-train

The rollout-loss trainer of LieSPHGP's SE(2) recipe, in C++ with a hand-written reverse
pass: no Python, JAX or autodiff library at runtime or in CI.

- se2_train.h: the objective (rollout loss + beta KL, one reparameterized draw per step),
  its reverse pass through every Lie-IMEX step of a window (the implicit 3x3 solves, the
  SPD levels, the random-feature GPs and grad V's own derivative), deterministic on any
  thread count; train.py's optimizer (global-norm clip, cosine-decayed Adam on the GP
  weights, Prodigy per level); the training loop with its held-out monitor; the data:
  cvc-plant-log run directories (flights.npy + meta.json), split by seed, observation
  noise, RMS scales, LieSPHGP's windows.
- optim.h: cosine decay, clipping, Adam and Prodigy written to match optax 0.2.8.
- cvc ph-train (cvc-cli): fits a model to plant-log runs and writes it as JSON;
  --dump-dataset writes the noisy flights it trained on.

ph_se2_train_test: the gradient is within 8.1e-15 (relative to each leaf's largest entry)
of JAX's autodiff of the same objective on the Humvee fit, and within 2.2e-6 of central
differences on a small model; 300 optimizer steps match optax to 1.6e-16, and Prodigy
alone matches it through d's growth from 1e-3 to 1.95; a 1,200-step fit to another
model's flights cuts the held-out loss 22x. On the Humvee's Phase A runs a step takes
about 11 ms on 8 threads (5,000 steps in about a minute).
A window whose rollout leaves the finite numbers is left out of the loss and the
gradient. JAX's rollout_loss masks it out of the loss but its gradient still carries the
NaN, so train.py aborts there; here it is dropped, and ph-train now says so for each step
and records the total in the model's provenance (diverged_windows).

Tests: ADivergingWindowIsLeftOut (the loss and gradient equal the healthy windows'
alone); cvc_cli_test's PhTrainFitsAModelToPlantLogs (a plant-log directory written by the
test, a 20-step fit, the model loads with its provenance) and
PhTrainRefusesADirectoryThatIsNotAPlantLog.
A continued model reads its inputs divided by the RMS it was fit with, not the new runs'.
PhTrainContinuesAModelWithItsOwnScales fails without this (the run logged and used the
runs' scales).
@transfix
transfix changed the base branch from feat/plant-log-liesphgp to master October 21, 2026 16:45
@transfix
transfix enabled auto-merge October 21, 2026 16:45
@transfix
transfix merged commit 17adc93 into master Oct 21, 2026
28 of 29 checks passed
transfix added a commit that referenced this pull request Oct 22, 2026
…e_qef) (#762)

LbieMesherTest.HexaVerticesStayInsideAVolumeTheSurfaceLeaves hit an access
violation on windows-2022 Release (#758, run 39637628700) in its
r = 1.5 sphere, after "finished collapse" and before "finished compute qef".

Octree::interpRect3Dpts_x/y/z take a second gradient sample at the grid point
after the one an edge's crossing falls in. When the edge ends on the volume's
far face exactly at the isovalue (ival == 1), that point is one plane off the
grid, and getVertGrad()/getValue() read past orig_vol: up to about 1 KB for a
z edge. The r = 1.5 sphere, sampled at 17^3 in [-1,1]^3, passes exactly through
24 grid points on the box's faces (8^2 + 4^2 + 8^2 = 12^2 in eighths); r = 1.2
passes through none. AddressSanitizer flags the read on every run:

  heap-buffer-overflow READ of size 4, 12 bytes after the 19652-byte orig_vol
    Octree::getValue           octree.cpp:3598
    Octree::getVertGrad        octree.cpp:3661
    Octree::interpRect3Dpts_z  octree.cpp:3501
    Octree::cell_comp          octree.cpp:606
    Octree::compute_qef        octree.cpp:760

getVertGrad() now clamps its point onto the grid. The off-grid sample's weight
is 0, so meshes are unchanged (344 vertices / 4374 quads and 64 / 162 for the
test's spheres, as before).

New LbieMesherTest.MeshesASurfaceThroughGridPointsOnTheFarFaces meshes that
sphere as TETRA, HEXA, DOUBLE and TETRA2; without the fix ASan flags each of
them (compute_qef or compute_qef_interval).
@transfix
transfix deleted the feat/ph-native branch October 29, 2026 20:52
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant