physics: log nav_plant runs for fitting a plant model, and dyn_plant's residual against Jolt

docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md

@@ -185,6 +185,79 @@ P4 adds three things to measure and contain the drift:
 2. **A per-tick drift residual,** binned by lateral g and published next to the energy residual.
 3. **Lag-aware governors in the Jolt arm only.**
 
+### The dynamic model's residual (P8a against Jolt)
+
+`cvc::physics::one_tick_residuals` (`cvc/physics/plant_log.h`) measures the drift the same way for
+both models. Each tick, it steps P8a's `diff::dyn_plant` and the kinematic bicycle from Jolt's own
+state, with the command nav held over the tick. The models use the tick's substeps (`nsub = k`) and
+are compared with where Jolt was at the tick's end. The bins are the lateral acceleration Jolt
+measured over the tick.
+
+Ticks are left out when they have a static contact, a wheel off the ground, an impact (above
+1.5 g), or the vehicle at rest. `cvc plant-log` runs it and prints the bins.
+
+Below: nav's 0.06 s tick (6 substeps), 4 seeds, 60 s each, mu 0.9, measured 2026-10-20. Each cell
+is the position rms in mm, three ways:
+
+- the dynamic model with the spec-sheet values it had before the fit (#753);
+- the dynamic model with `dyn_bike_si_of`'s values now, fitted to Jolt;
+- the kinematic bicycle.
+
+The RoboRacer's city runs plan with 0.4 of its acceleration and braking: at its full 6 m/s^2 its
+chassis box pitches onto the ground.
+
+| lateral g | Humvee slalom | Humvee city | RoboRacer slalom | RoboRacer city |
+|---|---|---|---|---|
+| 0.0-0.1 | 2.0 / 1.2 / 3.9 | 2.0 / 1.6 / 3.4 | 0.9 / 0.3 / 0.7 | 1.9 / 1.3 / 1.9 |
+| 0.1-0.2 | 5.1 / 1.4 / 11.6 | 4.8 / 1.6 / 11.1 | 3.3 / 0.4 / 5.6 | 3.4 / 0.9 / 5.2 |
+| 0.2-0.3 | 7.1 / 1.3 / 17.9 | 7.0 / 2.0 / 18.1 | 5.0 / 0.6 / 10.7 | 4.8 / 1.0 / 8.6 |
+| 0.3-0.5 | 8.1 / 1.2 / 27.5 | 8.1 / 2.8 / 26.2 | 6.9 / 0.8 / 15.8 | 6.5 / 1.8 / 11.7 |
+| >= 0.5 | 5.2 / 4.2 / 94.0 | 4.5 / 3.8 / 52.0 | 6.5 / 0.7 / 25.1 | 12.6 / 11.5 / 30.4 |
+
+- **Position and heading:** the dynamic model is closer than the kinematic bicycle wherever the tires
+  carry load. Below 0.5 g the fitted values bring it 1.3-9x closer again.
+- **Speed:** below 0.5 g the RoboRacer's speed residual now matches the kinematic bicycle's,
+  0.001-0.035 m/s rms (it was 0.018-0.090). Two spec-sheet values had been off:
+  - its 0.1 s drive lag;
+  - the drag and rolling resistance it subtracted. The RoboRacer's driver model tracks the commanded
+    acceleration with an integral trim, so it makes up for them.
+
+  The Humvee's speed residual is unchanged, and below the kinematic bicycle's.
+- **What is left:** the RoboRacer in the city above 0.5 g (11.5 mm), and its yaw rate there,
+  0.04-0.34 rad/s rms.
+
+**Fitted to Jolt** (`cvc::physics::fit_dyn_bike`, `cvc plant-log --fit-dyn`): a coordinate descent on
+the one-tick twist residual over the knobs a spec sheet does not give. The knobs are the drive and
+steering lags, factors on the tires' B, the yaw inertia, and drag + rolling resistance. Each fit uses
+seeds 100-102 and is judged on seed 103.
+
+The first fits ran against the spec sheet: tire B x0.25-0.40, and for the RoboRacer no lag and no
+resistance. #753 adopted them as `dyn_bike_si_of`'s values. Refitted against those values:
+
+| run (50 Hz) | drive lag | tire B front / rear | yaw inertia | held-out twist residual, (m/s)^2: default -> fit | before #753 |
+|---|---|---|---|---|---|
+| Humvee slalom | 0.20 s | x1.45 / x1.20 | x1.00 | 0.00046 -> 0.00049 | 0.00287 -> 0.00045 |
+| Humvee city | 0.04 s | x1.00 / x1.00 | x1.00 | 0.00065 -> 0.00061 | 0.00280 -> 0.00060 |
+| RoboRacer slalom | 0 | x1.00 / x1.00 | x0.85 | 0.00001 -> 0.00001 | 0.00035 -> 0.00001 |
+| RoboRacer city | 0 | x1.00 / x1.20 | x1.75 | 0.00015 -> 0.00018 | 0.00229 -> 0.00018 |
+
+- **Nothing left to fit on flat ground:** on the held-out seed the defaults do as well as a fit. A
+  refit gains at most 6% (Humvee city) and loses up to 20% (RoboRacer city). The tire factors come back
+  at x1, except the Humvee slalom's, whose fit does worse held out. No steering lag is found, and drag
+  and rolling stay as they are.
+- **Weakly identified:** the Humvee's drive lag (0.04-0.20 s) and either vehicle's yaw inertia move
+  between scenes.
+- **The city-bundle fits (Austin) are not trustworthy:** they land on the grids' edges (B x0.3-0.7, yaw
+  inertia x1.75-2.1), absorbing what the planar model leaves out on rough terrain.
+- **Held out, at nav's 0.06 s tick:** position rms in mm, as fit / default now / spec sheet before #753
+  / kinematic:
+  - Humvee slalom, above 0.5 g: 2.7 / 3.4 / 6.4 / 55.2;
+  - RoboRacer city, 0.3-0.5 g: 1.1 / 1.1 / 6.1 / 11.2.
+- **The LieSPHGP datasets do not move:** Jolt drives the vehicles, and `dyn_bike_si_of` enters only
+  the residuals. The rerun's flights and pickles are byte-identical.
+
+These are the targets a plant fitted to Jolt has to beat, bin by bin.
+
 ## Status
 
 - **P0** is merged; see the clock roadmap.

inc/cvc/physics/nav_plant.h

@@ -38,6 +38,7 @@
 #include <cvc/nav/terrain.h>
 #include <cvc/nav/vehicle_plant.h>
 #include <cvc/physics/world.h>
+#include <functional>
 #include <set>
 #include <string>
 #include <vector>
@@ -53,6 +54,9 @@ struct nav_plant_options {
   // origin is added: < 0 = just above the wheels' full reach (their suspension extended).
   double ground_z = 0.0;
   double spawn_height = -1.0;
+  // The ground's height under a start (world metres, at the rear axle's x, y in metres), for a
+  // terrain that is not flat; empty: ground_z everywhere.
+  std::function<double(double x, double y)> ground_height;
   // advance() steps the world dt / fixed_dt times (a whole number, or attach() throws). false:
   // the world steps itself on the clock nav follows (world::follow), and advance() only reads.
   bool step_world = true;

inc/cvc/physics/plant_log.h

@@ -0,0 +1,195 @@
+/*
+  Copyright 2026 The University of Texas at Austin
+
+  This file is part of libcvc.
+
+  libcvc is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License version 2.1 as published by the Free Software Foundation.
+*/
+
+// cvc::physics::plant_log: what a nav_plant's vehicles did, tick by tick, as planar rigid bodies
+// in SI, for fitting a plant model to the physics; and the dynamic single-track model's one-tick
+// residual against them, beside the kinematic bicycle's.
+//
+//   plant_log log(sim, plant);   // sim drives `plant`
+//   log.start();                 // the agents before the first step
+//   for (...) { sim.step(); log.record(); }
+//   const auto res = one_tick_residuals(log.runs(), dt, desc, mu, nsub);
+//   const flight_set f = cut_flights(log.runs(), 250, 250);
+//   write_npy("flights.npy", f.data, {f.flights, f.rows, kPlanarWidth});
+//
+// A sample is the vehicle's centre of mass and heading on SE(2), its body twist there, the
+// steering it has, and the command nav held over the tick that ended at the sample (the
+// zero-order hold a plant-learning dataset wants: row k + 1's input acted from t_k to t_k+1).
+// Everything is in the physics world's frame (metres, radians, seconds).
+//
+// The residual is the vehicle-physics plan's per-tick drift (D14) with diff::dyn_plant beside
+// the kinematic bicycle: each tick both models start from the plant's own state at the tick's
+// start, take the command it held for `nsub` substeps of dt / nsub, and are compared with where
+// the plant was at the tick's end, binned by the lateral acceleration the plant measured over
+// that tick. The dynamic model starts from the measured steering and its own drive actuator
+// (which runs on the commands alone).
+
+#ifndef __CVC_PHYSICS_PLANT_LOG_H__
+#define __CVC_PHYSICS_PLANT_LOG_H__
+
+#include <cstddef>
+#include <cstdint>
+#include <cvc/nav/dyn_bike.h>
+#include <cvc/physics/vehicle.h>
+#include <string>
+#include <vector>
+
+namespace cvc {
+namespace nav {
+class sim_world;
+}
+namespace physics {
+
+class nav_plant;
+
+// One vehicle at one tick.
+struct planar_sample {
+  double t = 0.0;                    // s since start()
+  double x = 0.0, y = 0.0;           // the centre of mass
+  double psi = 0.0;                  // heading, continuous along the run (unwrapped)
+  double vx = 0.0, vy = 0.0;         // the centre of mass's velocity in the body frame
+  double r = 0.0;                    // yaw rate
+  double rear_x = 0.0, rear_y = 0.0; // the rear axle (the point nav tracks)
+  double steer = 0.0;                // the road-wheel (bicycle) angle the vehicle has
+  double grade = 0.0;                // sin of the slope along the heading, + uphill (the pitch)
+  double roll = 0.0;                 // sin of the body's roll, + left side up
+  double steer_cmd = 0.0;            // nav's command over the tick that ended here; 0 at start()
+  double accel_cmd = 0.0;            // m/s^2 along the body's x, as nav sent it
+  double drive = 0.0; // accel_cmd within what the vehicle can do: -max_decel..max_accel
+  double vmax = 0.0;  // the top speed nav plans this vehicle with (m/s)
+  double a_lat = 0.0, a_lon = 0.0; // the centre of mass's acceleration over that tick, body axes
+  int contacts = 0;                // static contacts during that tick
+  int wheels = 0, wheels_down = 0; // the vehicle's wheels, and how many touch the ground
+  bool grounded = true;            // at most one wheel off the ground (a planar body)
+};
+
+// Records every agent of `sim`, which drives `plant`. Both must outlive the log.
+class plant_log {
+public:
+  plant_log(const cvc::nav::sim_world &sim, nav_plant &plant);
+  void start();  // before the first step: the agents' state, no command yet
+  void record(); // after each sim.step(): the state reached and the command held
+  int agents() const { return static_cast<int>(_runs.size()); }
+  double dt() const { return _dt; }
+  const std::vector<std::vector<planar_sample>> &runs() const { return _runs; }
+
+private:
+  planar_sample sample(int i) const;
+  const cvc::nav::sim_world &_sim;
+  nav_plant &_plant;
+  double _dt = 0.0, _accel_si = 1.0;
+  std::vector<double> _vmax;
+  std::vector<std::vector<planar_sample>> _runs;
+};
+
+// ── the model's residual ────────────────────────────────────────────────────────────
+
+struct residual_bin {
+  double lo_g = 0.0, hi_g = 0.0; // |lateral acceleration| / g; the last bin is open (hi infinite)
+  std::uint64_t ticks = 0;
+  double position_rms_m = 0.0, position_max_m = 0.0; // the rear axle
+  double heading_rms_rad = 0.0, heading_max_rad = 0.0;
+  double speed_rms_mps = 0.0, speed_max_mps = 0.0; // vx
+  double lateral_rms_mps = 0.0;                    // vy at the centre of mass (dynamic only)
+  double yaw_rate_rms = 0.0;                       // r (dynamic only)
+};
+
+struct plant_residuals {
+  std::vector<residual_bin> dynamic, kinematic;
+  // Ticks left out, by why (the first that applies): a static contact; a wheel off the ground at
+  // either end; an impact (a horizontal acceleration past kImpactG, more than tires give: another
+  // vehicle); the vehicle at rest at both ends.
+  std::uint64_t skipped_contact = 0, skipped_airborne = 0, skipped_impact = 0, skipped_rest = 0;
+  std::uint64_t skipped() const {
+    return skipped_contact + skipped_airborne + skipped_impact + skipped_rest;
+  }
+};
+
+// The horizontal acceleration (g) past which a tick is an impact, not tire forces.
+constexpr double kImpactG = 1.5;
+
+// The one-tick residuals of diff::dyn_plant (the vehicle from dyn_bike_si_of(d, mu), in SI) and
+// of the kinematic bicycle (wheelbase d.wheelbase(), the commanded steer applied at once, its speed
+// within [-vmax / 4, vmax] as sim_world's reference has it) over every tick of `runs`, both given
+// the command as nav sent it. `bins_g` are the bins' upper edges, ascending. The ticks
+// plant_residuals lists are skipped (at rest: |vx| < 0.2 m/s at both ends).
+plant_residuals one_tick_residuals(const std::vector<std::vector<planar_sample>> &runs, double dt,
+                                   const vehicle_desc &d, double mu = 0.9, int nsub = 1,
+                                   const std::vector<double> &bins_g = {0.1, 0.2, 0.3, 0.5});
+
+// The same with the dynamic model built from `si` (SI) and the kinematic bicycle's wheelbase and
+// steering lock given: for a vehicle other than dyn_bike_si_of's, such as a fitted one.
+plant_residuals one_tick_residuals(const std::vector<std::vector<planar_sample>> &runs, double dt,
+                                   const cvc::nav::dyn_bike_si &si, double wheelbase,
+                                   double max_steer, int nsub = 1,
+                                   const std::vector<double> &bins_g = {0.1, 0.2, 0.3, 0.5});
+
+// ── fitting the dynamic model to the plant ───────────────────────────────────────────
+
+// The dynamic model's mean squared one-tick residual in the body twist over the ticks
+// one_tick_residuals keeps: (vx error)^2 + (vy error)^2 + (L x yaw-rate error)^2, in (m/s)^2,
+// L its wheelbase. `ticks`, when given, receives how many ticks it averaged.
+double dyn_bike_cost(const std::vector<std::vector<planar_sample>> &runs, double dt,
+                     const cvc::nav::dyn_bike_si &si, double max_steer, int nsub = 1,
+                     std::uint64_t *ticks = nullptr);
+
+// dyn_bike_si_of(d, mu) with the knobs a spec sheet does not give fitted to the plant by
+// coordinate descent on dyn_bike_cost: the drive and steering lags (s), and factors on the tires'
+// stiffness B (front, rear), on the yaw inertia, and on drag and rolling resistance together (a
+// driver model that tracks the commanded acceleration makes up for them: 0). dyn_bike_si_of's own
+// values are unchanged: this measures what the plant wants. Grids: lags 0-0.3 s, the resistance
+// factor 0-1, the others 0.1-2.1.
+struct dyn_bike_fit {
+  cvc::nav::dyn_bike_si si;                  // the fitted vehicle
+  double cost_default = 0.0, cost_fit = 0.0; // dyn_bike_cost before and after
+  std::uint64_t ticks = 0;
+  double drive_lag = 0.0, steer_lag = 0.0;
+  double tire_b_front_factor = 1.0, tire_b_rear_factor = 1.0, yaw_inertia_factor = 1.0;
+  double resistance_factor = 1.0;
+};
+dyn_bike_fit fit_dyn_bike(const std::vector<std::vector<planar_sample>> &runs, double dt,
+                          const vehicle_desc &d, double mu = 0.9, int nsub = 1, int sweeps = 2);
+
+// ── equal-length flights for a plant-learning dataset ──────────────────────────────
+
+// A row: the pose, the body twist and the two inputs.
+//   0 x, 1 y, 2 psi, 3 vx, 4 vy, 5 r, 6 the drive input (drive), 7 the steering input.
+// Row k's inputs acted from t_k-1 to t_k (row 0's from before the flight; unused by a model).
+constexpr int kPlanarWidth = 8;
+
+enum class steer_input {
+  actual, // the mean of the road-wheel angle at the interval's two ends
+  command // nav's command, held over the interval
+};
+
+struct flight_set {
+  std::vector<double> data; // [flights * rows * kPlanarWidth], row-major
+  std::size_t flights = 0, rows = 0;
+  std::vector<int> agent, first; // each flight's agent and first sample
+  std::size_t dropped = 0;       // windows left out: a contact, an impact, a wheel off the
+                                 // ground, or barely moving
+  double peak_lateral_g = 0.0;   // the largest |a_lat| / g in the kept flights
+};
+
+// Every window of `rows` samples, `stride` apart, of every run. A window is kept when it has no
+// static contact, no impact and no wheel off the ground, and the vehicle moves faster than
+// `min_speed` (m/s) somewhere in it. The drive input is planar_sample::drive.
+flight_set cut_flights(const std::vector<std::vector<planar_sample>> &runs, std::size_t rows,
+                       std::size_t stride, double min_speed = 1.0,
+                       steer_input steer = steer_input::actual);
+
+// A little-endian float64 .npy file (format 1.0, C order). Throws std::runtime_error.
+void write_npy(const std::string &path, const std::vector<double> &data,
+               const std::vector<std::size_t> &shape);
+
+} // namespace physics
+} // namespace cvc
+
+#endif // __CVC_PHYSICS_PLANT_LOG_H__