diff --git a/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md b/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md index 9a8260bee..396af67aa 100644 --- a/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md +++ b/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. diff --git a/inc/cvc/physics/nav_plant.h b/inc/cvc/physics/nav_plant.h index 10265005e..bed0b0ce9 100644 --- a/inc/cvc/physics/nav_plant.h +++ b/inc/cvc/physics/nav_plant.h @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -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 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; diff --git a/inc/cvc/physics/plant_log.h b/inc/cvc/physics/plant_log.h new file mode 100644 index 000000000..79c4c7235 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +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(_runs.size()); } + double dt() const { return _dt; } + const std::vector> &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 _vmax; + std::vector> _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 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> &runs, double dt, + const vehicle_desc &d, double mu = 0.9, int nsub = 1, + const std::vector &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> &runs, double dt, + const cvc::nav::dyn_bike_si &si, double wheelbase, + double max_steer, int nsub = 1, + const std::vector &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> &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> &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 data; // [flights * rows * kPlanarWidth], row-major + std::size_t flights = 0, rows = 0; + std::vector 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> &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 &data, + const std::vector &shape); + +} // namespace physics +} // namespace cvc + +#endif // __CVC_PHYSICS_PLANT_LOG_H__ diff --git a/inc/cvc/physics/plant_log_scene.h b/inc/cvc/physics/plant_log_scene.h new file mode 100644 index 000000000..22f2788a9 --- /dev/null +++ b/inc/cvc/physics/plant_log_scene.h @@ -0,0 +1,111 @@ +/* + 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 scenes: nav driving physics vehicles (nav_plant on Jolt) through the +// scenes `cvc plant-log` logs, and the directory it writes from them (cvc/physics/plant_log.h). +// +// plant_log_options o; +// o.vehicle = "roboracer"; +// o.scene = "city"; +// o.resolve(); // checks it, fills the scene's defaults +// const plant_log_drive d = drive_plant_log_scene(o, nav::coef_mlp::default_biased()); +// o.out = "runs/roboracer-city"; +// write_plant_log(o, d); // flights.npy, meta.json, residual.json +// +// Scenes: +// swerve open flat ground; each agent slaloms, its goal jumping from side to side, at a +// seed-varied speed with a corner cap of lateral_g (the swerves past 0.3 g). +// city the coef trainer's city (city_scene(96)) scaled to the vehicle, as the plant reach +// gate drives it. +// bundle a cvc::world city bundle (load_city_bundle): its buildings and terrain in the +// physics world, its rasterized occupancy for nav (50 m in from the terrain's edge), +// vehicles spawned on the terrain. +// In city each agent gets a new random goal when it arrives (the trained drive alone does well +// on so small a map). In bundle each drives vehicle routes (plan_vehicle_route with the plant's +// footprint and turning circle, route_follower) to random goals 5-15% of the map away, a new +// one at each arrival: the reactive drive alone stalls on long blocks. +// Logging starts after 0.5 s, once the vehicles have settled on their springs. Each seed is its +// own sim and physics world; the runs are deterministic given the options and the policy. +// +// The directory (write_plant_log): +// flights.npy float64 (F, T, 8): equal-length flights cut from the runs, rows +// [x, y, psi, vx, vy, r, drive, steer] (kPlanarWidth): the centre of mass on +// SE(2), its body twist, nav's acceleration command and the steering (steer +// actual or command). Row k's inputs acted from t_k-1 to t_k. +// meta.json the rate, the layout, the run's settings and each flight's seed and agent. +// residual.json the one-tick residuals of diff::dyn_plant and the kinematic bicycle against +// the plant, binned by lateral g (also reported). +// samples.csv with samples: every tick of every run. +// fit_dyn fits dyn_bike's lags, tire stiffness and yaw inertia to the runs of every seed but the +// last (fit_dyn_bike) and reports the last seed's residual with the spec-sheet and the fitted +// vehicle (residual.json's "fit"). tools/liesphgp_pack.py turns one or more of these directories +// into a LieSPHGP dataset pickle. + +#ifndef __CVC_PHYSICS_PLANT_LOG_SCENE_H__ +#define __CVC_PHYSICS_PLANT_LOG_SCENE_H__ + +#include +#include +#include +#include +#include + +namespace cvc { +namespace physics { + +struct plant_log_options { + // The drive. + std::string vehicle = "humvee"; // a plant_host preset + std::string scene = "swerve"; // swerve, city or bundle + std::string bundle; // the city bundle's directory (scene bundle) + std::vector seeds = {100, 101, 102, 103}; + int agents = -1; // < 0: 4 in swerve, 8 otherwise + double seconds = 60.0; // logged per seed + double dt = 0.02; // nav's tick, a whole number of 0.01 s physics steps + double mu = 0.9; // the ground's friction + double lateral_g = -1.0; // nav's corner cap; < 0: 0.9 in swerve, 0.5 otherwise + int grid = 600; // bundle: the occupancy grid's cells a side + int inflate = 0; // bundle: obstacle inflation in cells + double accel_margin = -1.0, brake_margin = -1.0; // the plant's; <= 0 keeps its defaults + // The directory. + std::string out; + double flight_seconds = 5.0, stride_seconds = 5.0; + steer_input steer = steer_input::actual; + int nsub = 0; // residual substeps; 0: the physics steps a tick + bool samples = false; + bool fit_dyn = false; + // Progress, per-seed summaries and the residual tables; nullptr: none. + std::FILE *report = nullptr; + bool verbose = false; // also every agent's state every 5 s + + // Checks the options and fills the scene's defaults (agents, lateral_g, nsub); throws + // std::invalid_argument. + void resolve(); +}; + +// What drive_plant_log_scene logged: one run per agent per seed, in seed order. +struct plant_log_drive { + std::vector> runs; + std::vector run_seed; + std::vector run_agent; + double accel_margin = 0.0, brake_margin = 0.0; // what the plant planned with +}; + +// Drives every seed of the scene with `policy` (resolved options). Throws std::runtime_error when +// this libcvc has no Jolt backend. +plant_log_drive drive_plant_log_scene(const plant_log_options &o, const nav::coef_mlp &policy); + +// Writes the directory o.out from a drive with the same options. +void write_plant_log(const plant_log_options &o, const plant_log_drive &d); + +} // namespace physics +} // namespace cvc + +#endif diff --git a/src/cvc-cli/cvc_cli_test.cpp b/src/cvc-cli/cvc_cli_test.cpp index 1ead2fc46..b8508544d 100644 --- a/src/cvc-cli/cvc_cli_test.cpp +++ b/src/cvc-cli/cvc_cli_test.cpp @@ -527,6 +527,40 @@ TEST_F(CvcCliTest, PsHelp) { EXPECT_EQ(0, r.exit_code); } +// =========================================================================== +// plant-log (nav driving Jolt vehicles; cvc/physics/plant_log_scene.h) +// =========================================================================== + +TEST_F(CvcCliTest, PlantLogHelp) { + auto r = cvc("plant-log --help"); + EXPECT_EQ(0, r.exit_code); + for (const char *opt : {"--out", "--vehicle", "--scene", "--bundle", "--seeds", "--fit-dyn"}) + EXPECT_NE(std::string::npos, r.output.find(opt)) << opt; +} + +TEST_F(CvcCliTest, PlantLogNeedsAnOutputDirectory) { + auto r = cvc("plant-log --scene swerve"); + EXPECT_NE(0, r.exit_code); +} + +TEST_F(CvcCliTest, PlantLogRejectsAnUnknownScene) { + auto r = cvc("plant-log --scene moon --out \"" + path("run") + "\""); + EXPECT_NE(0, r.exit_code); + EXPECT_NE(std::string::npos, r.output.find("swerve, city or bundle")) << r.output; +} + +TEST_F(CvcCliTest, PlantLogWritesARun) { + auto r = cvc("plant-log --seeds 3 --agents 1 --seconds 1 --flight-seconds 0.5 " + "--stride-seconds 0.5 --out \"" + + path("run") + "\""); + if (r.exit_code != 0 && r.output.find("without Jolt") != std::string::npos) + GTEST_SKIP() << "this libcvc was built without Jolt"; + ASSERT_EQ(0, r.exit_code) << r.output; + EXPECT_NE(std::string::npos, r.output.find("seed 3: 1 agents")) << r.output; + for (const char *f : {"flights.npy", "meta.json", "residual.json"}) + EXPECT_TRUE(fs::exists(fs::path(path("run")) / f)) << f; +} + // =========================================================================== // Bunny command (geometry only — fast) // =========================================================================== diff --git a/src/cvc-cli/main.cpp b/src/cvc-cli/main.cpp index 55279611c..be293ea3d 100644 --- a/src/cvc-cli/main.cpp +++ b/src/cvc-cli/main.cpp @@ -38,6 +38,8 @@ #include #include #include +#include +#include #include #include #include @@ -2062,6 +2064,86 @@ static int cmd_compare(int argc, char **argv) { return 0; } +// --------------------------------------------------------------------------- +// Vehicle physics +// --------------------------------------------------------------------------- + +static int cmd_plant_log(int argc, char **argv) { + po::options_description desc( + "cvc plant-log - nav driving physics vehicles on Jolt, logged for fitting a plant model\n\n" + "Logs every vehicle tick by tick as a planar rigid body, cuts the runs into flights\n" + "(flights.npy, meta.json; tools/liesphgp_pack.py packs them as a LieSPHGP dataset) and\n" + "reports the dynamic single-track model's one-tick residual against them\n" + "(residual.json). Scenes (cvc/physics/plant_log_scene.h):\n" + " swerve open flat ground, each agent slaloming\n" + " city the coef trainer's city, scaled to the vehicle\n" + " bundle a cvc::world city bundle (--bundle DIR), driven on vehicle routes\n\n" + " cvc plant-log --vehicle humvee --scene swerve --seeds 100,101,102 --out runs/swerve\n"); + cvc::physics::plant_log_options o; + std::string seeds = "100,101,102,103", steer = "actual", policy; + desc.add_options()("help,h", "show help")("out,o", po::value(&o.out)->required(), + "output directory")( + "vehicle", po::value(&o.vehicle)->default_value(o.vehicle), + "humvee or roboracer")("scene", po::value(&o.scene)->default_value(o.scene), + "swerve, city or bundle")("bundle", po::value(&o.bundle), + "city bundle directory (--scene bundle)")( + "seeds", po::value(&seeds)->default_value(seeds), + "comma-separated seeds, one run per agent each")( + "agents", po::value(&o.agents)->default_value(o.agents), + "vehicles per seed (< 0: 4 in swerve, 8 otherwise)")( + "seconds", po::value(&o.seconds)->default_value(o.seconds), + "seconds per seed")("dt", po::value(&o.dt)->default_value(o.dt), + "nav's tick, a whole number of 0.01 s physics steps")( + "mu", po::value(&o.mu)->default_value(o.mu), "the ground's friction")( + "lateral-g", po::value(&o.lateral_g)->default_value(o.lateral_g), + "nav's corner cap in g (< 0: 0.9 in swerve, 0.5 otherwise)")( + "flight-seconds", po::value(&o.flight_seconds)->default_value(o.flight_seconds), + "a flight's length")("stride-seconds", + po::value(&o.stride_seconds)->default_value(o.stride_seconds), + "from one flight's start to the next's")( + "steer", po::value(&steer)->default_value(steer), + "the steer input: actual (the road-wheel angle's mean over the tick) or command")( + "nsub", po::value(&o.nsub)->default_value(o.nsub), + "residual substeps a tick (0: the physics steps a tick)")( + "grid", po::value(&o.grid)->default_value(o.grid), + "bundle: grid cells a side")("inflate", po::value(&o.inflate)->default_value(o.inflate), + "bundle: obstacle inflation in cells")( + "accel-margin", po::value(&o.accel_margin)->default_value(o.accel_margin), + "the share of the vehicle's acceleration nav plans with (<= 0: the plant's)")( + "brake-margin", po::value(&o.brake_margin)->default_value(o.brake_margin), + "the share of its braking (<= 0: the plant's)")( + "policy", po::value(&policy), "a .cvcnav policy (default: the built-in one)")( + "verbose", po::bool_switch(&o.verbose), "every agent's state every 5 s")( + "samples", po::bool_switch(&o.samples), "also write samples.csv, every tick of every run")( + "fit-dyn", po::bool_switch(&o.fit_dyn), + "fit dyn_bike to every seed but the last and report the last"); + + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + if (vm.count("help")) { + std::cout << desc << "\n"; + return 0; + } + po::notify(vm); + + o.seeds.clear(); + std::stringstream ss(seeds); + for (std::string t; std::getline(ss, t, ',');) + o.seeds.push_back(static_cast(std::stoul(t))); + if (steer == "actual") + o.steer = cvc::physics::steer_input::actual; + else if (steer == "command") + o.steer = cvc::physics::steer_input::command; + else + throw std::invalid_argument("--steer is actual or command"); + o.resolve(); + o.report = stdout; + const cvc::nav::coef_mlp net = + policy.empty() ? cvc::nav::coef_mlp::default_biased() : cvc::nav::coef_mlp::load(policy); + cvc::physics::write_plant_log(o, cvc::physics::drive_plant_log_scene(o, net)); + return 0; +} + // =========================================================================== // Command dispatch table // =========================================================================== @@ -2142,6 +2224,9 @@ static const command_entry commands[] = { {"cluster-status", "State Server", "display cluster health report", cmd_cluster_status}, {"ps", "State Exec", "list state_exec processes", cmd_ps}, + // ── Vehicle physics ── + {"plant-log", "Vehicle Physics", "log nav driving Jolt vehicles for plant fitting", cmd_plant_log}, + // ── Network (legacy XMLRPC) ── #ifdef USING_XMLRPC {"server", "Network", "start XMLRPC server", cmd_server}, diff --git a/src/cvc/CMakeLists.txt b/src/cvc/CMakeLists.txt index a747f2af7..3540a81aa 100644 --- a/src/cvc/CMakeLists.txt +++ b/src/cvc/CMakeLists.txt @@ -70,6 +70,8 @@ set(INCLUDE_FILES ../../inc/cvc/physics/pose_tracker.h ../../inc/cvc/physics/nav_plant.h ../../inc/cvc/physics/nav_eval.h + ../../inc/cvc/physics/plant_log.h + ../../inc/cvc/physics/plant_log_scene.h ../../inc/cvc/state/state_hash_partition.h ../../inc/cvc/state/distributed_state_session.h ../../inc/cvc/state/state_transport.h @@ -260,6 +262,8 @@ set(SOURCE_FILES physics/binding_meta.cpp physics/nav_plant.cpp physics/nav_eval.cpp + physics/plant_log.cpp + physics/plant_log_scene.cpp state/state_change_journal.cpp state/state_subscription_router.cpp state/state_sync_adapter.cpp diff --git a/src/cvc/physics/nav_plant.cpp b/src/cvc/physics/nav_plant.cpp index d8be0a7f8..fb954db43 100644 --- a/src/cvc/physics/nav_plant.cpp +++ b/src/cvc/physics/nav_plant.cpp @@ -206,7 +206,8 @@ void nav_plant::attach(int n, const cvc::nav::plant_state *start, const double yaw = start[i].heading; const double c = det::cos(yaw), s = det::sin(yaw); const double h = _o.spawn_height >= 0.0 ? _o.spawn_height : resting_height(a.desc); - const vec3 origin{p.x - c * a.rear_x, p.y - s * a.rear_x, _o.ground_z + h}; + const double gz = _o.ground_height ? _o.ground_height(p.x, p.y) : _o.ground_z; + const vec3 origin{p.x - c * a.rear_x, p.y - s * a.rear_x, gz + h}; quat q; q.w = det::cos(0.5 * yaw); q.z = det::sin(0.5 * yaw); diff --git a/src/cvc/physics/plant_log.cpp b/src/cvc/physics/plant_log.cpp new file mode 100644 index 000000000..674cef43b --- /dev/null +++ b/src/cvc/physics/plant_log.cpp @@ -0,0 +1,474 @@ +/* + 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. +*/ + +// See plant_log.h. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cvc { +namespace physics { + +namespace { + +constexpr double kG = 9.80665; +constexpr double kPi = 3.14159265358979323846; + +vec3 cross(const vec3 &a, const vec3 &b) { + return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x}; +} + +vec3 rotate(const quat &q, const vec3 &v) { + const vec3 u{q.x, q.y, q.z}; + const vec3 t = cross(u, v) * 2.0; + return v + t * q.w + cross(u, t); +} + +// The rearmost unsteered axle's x, as nav_plant reads the rear axle; 0 without one. +double rear_axle_x(const vehicle_desc &d) { + double rear = 0.0; + bool have = false; + for (const axle_desc &a : d.axles) + if (!a.steered && (!have || a.x < rear)) { + rear = a.x; + have = true; + } + return rear; +} + +double wrap(double a) { + while (a > kPi) + a -= 2.0 * kPi; + while (a < -kPi) + a += 2.0 * kPi; + return a; +} + +// Accumulates one bin's squared residuals. +struct accum { + std::uint64_t n = 0; + double p2 = 0.0, pmax = 0.0, h2 = 0.0, hmax = 0.0, s2 = 0.0, smax = 0.0, vy2 = 0.0, r2 = 0.0; + void add(double p, double h, double s, double vy, double r) { + ++n; + p2 += p * p; + pmax = std::max(pmax, p); + h2 += h * h; + hmax = std::max(hmax, h); + s2 += s * s; + smax = std::max(smax, s); + vy2 += vy * vy; + r2 += r * r; + } + residual_bin bin(double lo, double hi) const { + residual_bin b; + b.lo_g = lo; + b.hi_g = hi; + b.ticks = n; + if (n) { + b.position_rms_m = std::sqrt(p2 / n); + b.position_max_m = pmax; + b.heading_rms_rad = std::sqrt(h2 / n); + b.heading_max_rad = hmax; + b.speed_rms_mps = std::sqrt(s2 / n); + b.speed_max_mps = smax; + b.lateral_rms_mps = std::sqrt(vy2 / n); + b.yaw_rate_rms = std::sqrt(r2 / n); + } + return b; + } +}; + +} // namespace + +plant_log::plant_log(const cvc::nav::sim_world &sim, nav_plant &plant) : _sim(sim), _plant(plant) { + const cvc::nav::sim_world::config &c = sim.current_config(); + // The tick is a whole number of physics steps (nav_plant::attach requires it); nav's dt is a + // float, so take it from the physics step. + const double fixed = plant.physics_world().config().fixed_dt; + _dt = std::max(1.0, std::round(c.veh.dt / fixed)) * fixed; + // The top speed sim_world plans each vehicle with: its own, capped at nav's. + for (int i = 0; i < plant.size(); ++i) { + const float lim = plant.limits(i).vmax; + const double vn = lim > 0.0f ? std::min(lim, c.veh.vmax) : c.veh.vmax; + _vmax.push_back(vn * plant.options().metres_per_world_unit / c.scale); + } + // nav's accelerations are normalized: (m / metres_per_world_unit) * scale per s^2. + _accel_si = plant.options().metres_per_world_unit / c.scale; +} + +planar_sample plant_log::sample(int i) const { + const vehicle_state vs = _plant.physics_world().vehicle_state(_plant.vehicle(i)); + const body_state &b = vs.body; + const vehicle_desc &d = _plant.desc(i); + const vec3 fwd = rotate(b.rotation, {1.0, 0.0, 0.0}); + const vec3 com = b.position + rotate(b.rotation, d.center_of_mass); + const vec3 rear = b.position + rotate(b.rotation, {rear_axle_x(d), 0.0, 0.0}); + planar_sample s; + s.psi = std::atan2(fwd.y, fwd.x); + const double c = std::cos(s.psi), sn = std::sin(s.psi); + s.x = com.x; + s.y = com.y; + s.vx = b.linear_velocity.x * c + b.linear_velocity.y * sn; + s.vy = -b.linear_velocity.x * sn + b.linear_velocity.y * c; + s.r = b.angular_velocity.z; + s.rear_x = rear.x; + s.rear_y = rear.y; + s.steer = vs.steer; + s.grade = std::max(-1.0, std::min(1.0, fwd.z)); + s.roll = std::max(-1.0, std::min(1.0, -rotate(b.rotation, {0.0, 1.0, 0.0}).z)); + s.wheels = static_cast(vs.wheels.size()); + for (const wheel_state &w : vs.wheels) + s.wheels_down += w.contact ? 1 : 0; + s.grounded = s.wheels_down + 1 >= s.wheels; + if (i < static_cast(_vmax.size())) + s.vmax = _vmax[i]; + return s; +} + +void plant_log::start() { + const int n = _plant.size(); + _runs.assign(static_cast(n), {}); + for (int i = 0; i < n; ++i) + _runs[i].push_back(sample(i)); +} + +void plant_log::record() { + if (_runs.empty()) + start(); + const float *sc = _sim.plant_steer_command(); + const float *ac = _sim.plant_accel_command(); + for (int i = 0; i < agents(); ++i) { + std::vector &run = _runs[i]; + const planar_sample &prev = run.back(); + planar_sample s = sample(i); + s.t = static_cast(run.size()) * _dt; + s.psi = prev.psi + wrap(s.psi - prev.psi); + s.steer_cmd = sc ? sc[i] : 0.0; + s.accel_cmd = ac ? ac[i] * _accel_si : 0.0; + const vehicle_desc &d = _plant.desc(i); + s.drive = std::min(std::max(s.accel_cmd, -d.max_decel), d.max_accel); + s.contacts = _plant.static_contacts_step(i); + // The centre of mass's acceleration over the tick, across the mean heading. + const double c0 = std::cos(prev.psi), s0 = std::sin(prev.psi); + const double c1 = std::cos(s.psi), s1 = std::sin(s.psi); + const double vwx0 = prev.vx * c0 - prev.vy * s0, vwy0 = prev.vx * s0 + prev.vy * c0; + const double vwx1 = s.vx * c1 - s.vy * s1, vwy1 = s.vx * s1 + s.vy * c1; + const double pm = 0.5 * (prev.psi + s.psi); + s.a_lat = (-(vwx1 - vwx0) * std::sin(pm) + (vwy1 - vwy0) * std::cos(pm)) / _dt; + s.a_lon = ((vwx1 - vwx0) * std::cos(pm) + (vwy1 - vwy0) * std::sin(pm)) / _dt; + run.push_back(s); + } +} + +namespace { + +// Why a tick is left out of the residual (0: it is not). +enum skip_why { kept = 0, contact, airborne, impact, rest }; + +skip_why why_skipped(const planar_sample &s0, const planar_sample &s1) { + if (s1.contacts > 0) + return contact; + if (!s0.grounded || !s1.grounded) + return airborne; + if (std::hypot(s1.a_lat, s1.a_lon) > kImpactG * kG) + return impact; + if (std::fabs(s0.vx) < 0.2 && std::fabs(s1.vx) < 0.2) + return rest; + return kept; +} + +// The dynamic model for one tick: from the plant's state at its start (and the model's own drive +// actuator, a_shadow, carried along the run), the held command for nsub substeps, as +// one_tick_residuals takes it. Updates a_shadow. +cvc::nav::diff::dyn_state dyn_tick(const cvc::nav::diff::dyn_bike_params &p, + cvc::nav::diff::bike_veh &lim, const planar_sample &s0, + const planar_sample &s1, int nsub, double &a_shadow) { + // Both models keep the speed within the plan's, as sim_world's reference does. + lim.vmax = static_cast(s0.vmax > 0.0 ? s0.vmax : 1e3); + cvc::nav::diff::dyn_state m{}; + m.x = s0.rear_x; + m.y = s0.rear_y; + m.psi = s0.psi; + m.vx = s0.vx; + m.vy = s0.vy; + m.r = s0.r; + m.delta = s0.steer; + m.a = a_shadow; + for (int j = 0; j < nsub; ++j) { + cvc::nav::diff::dyn_state o; + cvc::nav::diff::dyn_plant(p, lim, m, s1.steer_cmd, s1.accel_cmd, s0.grade, o); + m = o; + } + a_shadow = m.a; + return m; +} + +cvc::nav::diff::bike_veh dyn_limits(const cvc::nav::diff::dyn_bike_params &p, double max_steer, + double h) { + cvc::nav::diff::bike_veh lim{}; + lim.L = p.lf + p.lr; + lim.delta_max = static_cast(max_steer); + lim.hdt = static_cast(h); + lim.allow_reverse = 1; + return lim; +} + +cvc::nav::diff::dyn_bike_params si_params(const cvc::nav::dyn_bike_si &si) { + cvc::world_units::normalized_frame f; + f.scale = 1.0; + return cvc::nav::dyn_bike_params_of(si, f); +} + +void check_residual_args(double dt, int nsub, const std::vector &bins_g) { + if (!(dt > 0.0) || nsub < 1) + throw std::invalid_argument("cvc::physics::one_tick_residuals: needs dt > 0 and nsub >= 1"); + for (std::size_t k = 1; k < bins_g.size(); ++k) + if (!(bins_g[k] > bins_g[k - 1])) + throw std::invalid_argument("cvc::physics::one_tick_residuals: bins must ascend"); +} + +} // namespace + +plant_residuals one_tick_residuals(const std::vector> &runs, double dt, + const vehicle_desc &d, double mu, int nsub, + const std::vector &bins_g) { + return one_tick_residuals(runs, dt, nav_plant::dyn_bike_si_of(d, mu), d.wheelbase(), d.max_steer, + nsub, bins_g); +} + +plant_residuals one_tick_residuals(const std::vector> &runs, double dt, + const cvc::nav::dyn_bike_si &si, double wheelbase, + double max_steer, int nsub, const std::vector &bins_g) { + check_residual_args(dt, nsub, bins_g); + const cvc::nav::diff::dyn_bike_params p = si_params(si); + const double h = dt / nsub; + cvc::nav::diff::bike_veh lim = dyn_limits(p, max_steer, h); + const double L = wheelbase; + const std::size_t nb = bins_g.size() + 1; + std::vector dyn(nb), kin(nb); + plant_residuals out; + for (const std::vector &run : runs) { + double a_shadow = 0.0; // the model's drive actuator, run on the commands alone + for (std::size_t k = 0; k + 1 < run.size(); ++k) { + const planar_sample &s0 = run[k], &s1 = run[k + 1]; + const cvc::nav::diff::dyn_state m = dyn_tick(p, lim, s0, s1, nsub, a_shadow); + // The kinematic bicycle (D14's reference): the command at once, about the rear axle. + const double vmax = s0.vmax > 0.0 ? s0.vmax : 1e3; + double kx = s0.rear_x, ky = s0.rear_y, kth = s0.psi, ksp = s0.vx; + for (int j = 0; j < nsub; ++j) { + ksp = std::min(std::max(ksp + h * s1.accel_cmd, -0.25 * vmax), vmax); + kth = kth + h * (ksp / L) * std::tan(s1.steer_cmd); + kx = kx + h * ksp * std::cos(kth); + ky = ky + h * ksp * std::sin(kth); + } + switch (why_skipped(s0, s1)) { + case contact: + ++out.skipped_contact; + continue; + case airborne: + ++out.skipped_airborne; + continue; + case impact: + ++out.skipped_impact; + continue; + case rest: + ++out.skipped_rest; + continue; + case kept: + break; + } + const double g = std::fabs(s1.a_lat) / kG; + std::size_t b = 0; + while (b < bins_g.size() && g >= bins_g[b]) + ++b; + dyn[b].add(std::hypot(m.x - s1.rear_x, m.y - s1.rear_y), std::fabs(wrap(m.psi - s1.psi)), + std::fabs(m.vx - s1.vx), std::fabs(m.vy - s1.vy), std::fabs(m.r - s1.r)); + kin[b].add(std::hypot(kx - s1.rear_x, ky - s1.rear_y), std::fabs(wrap(kth - s1.psi)), + std::fabs(ksp - s1.vx), 0.0, 0.0); + } + } + for (std::size_t b = 0; b < nb; ++b) { + const double lo = b == 0 ? 0.0 : bins_g[b - 1]; + const double hi = b < bins_g.size() ? bins_g[b] : std::numeric_limits::infinity(); + out.dynamic.push_back(dyn[b].bin(lo, hi)); + out.kinematic.push_back(kin[b].bin(lo, hi)); + } + return out; +} + +double dyn_bike_cost(const std::vector> &runs, double dt, + const cvc::nav::dyn_bike_si &si, double max_steer, int nsub, + std::uint64_t *ticks) { + check_residual_args(dt, nsub, {}); + const cvc::nav::diff::dyn_bike_params p = si_params(si); + cvc::nav::diff::bike_veh lim = dyn_limits(p, max_steer, dt / nsub); + const double L = si.cg_to_front + si.cg_to_rear; + double sum = 0.0; + std::uint64_t n = 0; + for (const std::vector &run : runs) { + double a_shadow = 0.0; + for (std::size_t k = 0; k + 1 < run.size(); ++k) { + const planar_sample &s0 = run[k], &s1 = run[k + 1]; + const cvc::nav::diff::dyn_state m = dyn_tick(p, lim, s0, s1, nsub, a_shadow); + if (why_skipped(s0, s1) != kept) + continue; + const double ex = m.vx - s1.vx, ey = m.vy - s1.vy, er = L * (m.r - s1.r); + sum += ex * ex + ey * ey + er * er; + ++n; + } + } + if (ticks) + *ticks = n; + return n ? sum / static_cast(n) : 0.0; +} + +dyn_bike_fit fit_dyn_bike(const std::vector> &runs, double dt, + const vehicle_desc &d, double mu, int nsub, int sweeps) { + const cvc::nav::dyn_bike_si base = nav_plant::dyn_bike_si_of(d, mu); + dyn_bike_fit f; + f.si = base; + f.cost_default = dyn_bike_cost(runs, dt, base, d.max_steer, nsub, &f.ticks); + // The candidates, each a value or a factor on the spec-sheet vehicle's. + struct knob { + double *value; + std::vector grid; + }; + double drive_lag = base.drive_lag, steer_lag = base.steer_lag, bf = 1.0, br = 1.0, iz = 1.0; + double resist = 1.0; + const std::vector factors = {0.1, 0.15, 0.2, 0.25, 0.3, 0.4, 0.55, + 0.7, 0.85, 1.0, 1.2, 1.45, 1.75, 2.1}; + const std::vector knobs = {{&drive_lag, {0.0, 0.02, 0.04, 0.07, 0.1, 0.15, 0.2, 0.3}}, + {&steer_lag, {0.0, 0.02, 0.04, 0.07, 0.1}}, + {&resist, {0.0, 0.25, 0.5, 0.75, 1.0}}, + {&bf, factors}, + {&br, factors}, + {&iz, factors}}; + const auto make = [&]() { + cvc::nav::dyn_bike_si si = base; + si.drive_lag = drive_lag; + si.steer_lag = steer_lag; + si.tire_B_front = base.tire_B_front * bf; + si.tire_B_rear = base.tire_B_rear * br; + si.yaw_inertia = base.yaw_inertia * iz; + si.drag = base.drag * resist; + si.rolling_resistance = base.rolling_resistance * resist; + return si; + }; + double best = f.cost_default; + // Coordinate descent: each knob over its grid with the others held, `sweeps` times. + for (int sw = 0; sw < sweeps; ++sw) + for (const knob &k : knobs) { + double keep = *k.value; + for (const double v : k.grid) { + *k.value = v; + const double c = dyn_bike_cost(runs, dt, make(), d.max_steer, nsub, nullptr); + if (c < best) { + best = c; + keep = v; + } + } + *k.value = keep; + } + f.si = make(); + f.cost_fit = best; + f.drive_lag = drive_lag; + f.steer_lag = steer_lag; + f.tire_b_front_factor = bf; + f.tire_b_rear_factor = br; + f.yaw_inertia_factor = iz; + f.resistance_factor = resist; + return f; +} + +flight_set cut_flights(const std::vector> &runs, std::size_t rows, + std::size_t stride, double min_speed, steer_input steer) { + if (rows < 2 || stride < 1) + throw std::invalid_argument("cvc::physics::cut_flights: needs rows >= 2 and stride >= 1"); + flight_set f; + f.rows = rows; + for (std::size_t a = 0; a < runs.size(); ++a) { + const std::vector &run = runs[a]; + for (std::size_t s = 0; s + rows <= run.size(); s += stride) { + bool contact = false, moving = false; + double peak = 0.0; + for (std::size_t k = s; k < s + rows; ++k) { + contact = contact || !run[k].grounded || + (k > s && + (run[k].contacts > 0 || std::hypot(run[k].a_lat, run[k].a_lon) > kImpactG * kG)); + moving = moving || std::hypot(run[k].vx, run[k].vy) > min_speed; + if (k > s) + peak = std::max(peak, std::fabs(run[k].a_lat) / kG); + } + if (contact || !moving) { + ++f.dropped; + continue; + } + f.peak_lateral_g = std::max(f.peak_lateral_g, peak); + for (std::size_t k = s; k < s + rows; ++k) { + const planar_sample &q = run[k]; + const double u_steer = steer == steer_input::command + ? q.steer_cmd + : (k > 0 ? 0.5 * (run[k - 1].steer + q.steer) : q.steer); + const double row[kPlanarWidth] = {q.x, q.y, q.psi, q.vx, q.vy, q.r, q.drive, u_steer}; + f.data.insert(f.data.end(), row, row + kPlanarWidth); + } + f.agent.push_back(static_cast(a)); + f.first.push_back(static_cast(s)); + ++f.flights; + } + } + return f; +} + +void write_npy(const std::string &path, const std::vector &data, + const std::vector &shape) { + std::size_t n = 1; + std::string dims; + for (std::size_t k = 0; k < shape.size(); ++k) { + n *= shape[k]; + dims += std::to_string(shape[k]) + (shape.size() == 1 || k + 1 < shape.size() ? "," : ""); + if (k + 1 < shape.size()) + dims += " "; + } + if (n != data.size()) + throw std::runtime_error("cvc::physics::write_npy: the shape does not match the data"); + std::string header = "{'descr': '(magic), 8); + const std::uint16_t hl = static_cast(header.size()); + const unsigned char len[2] = {static_cast(hl & 0xff), + static_cast(hl >> 8)}; + out.write(reinterpret_cast(len), 2); + out.write(header.data(), static_cast(header.size())); + // Little-endian doubles: every libcvc target is little-endian. + out.write(reinterpret_cast(data.data()), + static_cast(data.size() * sizeof(double))); + if (!out) + throw std::runtime_error("cvc::physics::write_npy: short write to " + path); +} + +} // namespace physics +} // namespace cvc diff --git a/src/cvc/physics/plant_log_scene.cpp b/src/cvc/physics/plant_log_scene.cpp new file mode 100644 index 000000000..4334f962a --- /dev/null +++ b/src/cvc/physics/plant_log_scene.cpp @@ -0,0 +1,631 @@ +/* + 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. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cvc { +namespace physics { + +namespace { + +namespace nav = cvc::nav; + +void say(const plant_log_options &o, const char *fmt, ...) { + if (!o.report) + return; + va_list ap; + va_start(ap, fmt); + std::vfprintf(o.report, fmt, ap); + va_end(ap); +} + +int steps_per_tick(double dt) { return static_cast(std::lround(dt / 0.01)); } + +// A grid's free cells in its largest 8-connected free component (start and goal candidates). +std::vector largest_free_component(const std::vector &occ, int rows, int cols) { + std::vector label(occ.size(), -1), best; + int next = 0; + for (int s = 0; s < rows * cols; ++s) { + if (occ[s] || label[s] >= 0) + continue; + std::vector comp; + std::deque q{s}; + label[s] = next; + while (!q.empty()) { + const int c = q.front(); + q.pop_front(); + comp.push_back(c); + const int r = c / cols, k = c % cols; + for (int dr = -1; dr <= 1; ++dr) + for (int dk = -1; dk <= 1; ++dk) { + const int rr = r + dr, kk = k + dk; + if (rr < 0 || kk < 0 || rr >= rows || kk >= cols) + continue; + const int n = rr * cols + kk; + if (!occ[n] && label[n] < 0) { + label[n] = next; + q.push_back(n); + } + } + } + if (comp.size() > best.size()) + best.swap(comp); + ++next; + } + return best; +} + +// The sim and the plant for one seed. +struct run_setup { + nav::sim_world::config cfg; + std::vector occ; + std::vector free_cells; // goal candidates (city, bundle) + std::vector o, goal, col; // normalized starts and goals + plant_host_options host; // the physics world and the plant's options + const cvc::world::world_geometry *geometry = nullptr; + double vmax_mps = 0.0; +}; + +// The cell -> normalized position of a grid. +void cell_to_normalized(const nav::sim_world::config &c, int cell, float &x, float &y) { + const int r = cell / c.cols, k = cell % c.cols; + const double wx = c.min_x + k * (c.max_x - c.min_x) / (c.cols - 1); + const double wy = c.min_y + r * (c.max_y - c.min_y) / (c.rows - 1); + x = static_cast((wx - c.cx) * c.scale); + y = static_cast((wy - c.cy) * c.scale); +} + +void base_vehicle(nav::sim_world::config &c, double dt) { + c.veh.rr = 0.15f; + c.veh.d_hat = 0.35f; + c.veh.dt = static_cast(dt); + c.veh.nsub = 1; + c.freeze_sense = true; +} + +void scatter(run_setup &s, int n, std::mt19937 &rng) { + std::uniform_int_distribution pick(0, s.free_cells.size() - 1); + s.o.resize(2 * n); + s.goal.resize(2 * n); + s.col.assign(3 * n, 1.0f); + for (int i = 0; i < n; ++i) { + cell_to_normalized(s.cfg, s.free_cells[pick(rng)], s.o[2 * i], s.o[2 * i + 1]); + cell_to_normalized(s.cfg, s.free_cells[pick(rng)], s.goal[2 * i], s.goal[2 * i + 1]); + } +} + +} // namespace + +void plant_log_options::resolve() { + if (scene != "swerve" && scene != "city" && scene != "bundle") + throw std::invalid_argument("the scene is swerve, city or bundle"); + if (scene == "bundle" && bundle.empty()) + throw std::invalid_argument("the bundle scene needs a city bundle directory"); + if (seeds.empty()) + throw std::invalid_argument("no seeds"); + const int steps = steps_per_tick(dt); + if (steps < 1 || std::fabs(steps * 0.01 - dt) > 1e-9) + throw std::invalid_argument("dt must be a whole number of 0.01 s physics steps"); + if (agents < 0) + agents = scene == "swerve" ? 4 : 8; + if (lateral_g < 0.0) + lateral_g = scene == "swerve" ? 0.9 : 0.5; + if (nsub <= 0) + nsub = steps; +} + +plant_log_drive drive_plant_log_scene(const plant_log_options &o, const nav::coef_mlp &policy) { + if (!plant_host::available()) + throw std::runtime_error("this libcvc was built without Jolt"); + const vehicle_desc desc = plant_host::preset(o.vehicle); + const int ticks = static_cast(std::lround(o.seconds / o.dt)); + + // The city bundle is loaded once and shared by every seed. + std::unique_ptr city; + nav::world_planes planes; + if (o.scene == "bundle") { + city.reset(new cvc::world::city_bundle(cvc::world::load_city_bundle(o.bundle))); + if (!city->buildings_loaded) + say(o, "no buildings (%s); terrain only\n", city->buildings_error.c_str()); + planes = nav::rasterize(city->geometry, o.grid, o.grid, o.inflate); + } + + plant_log_drive d; + for (const unsigned seed : o.seeds) { + std::mt19937 rng(seed); + run_setup s; + s.host.plant.plan_with_vehicle = true; + s.host.ground_mu = o.mu; + const int n = o.agents; + if (o.scene == "swerve") { + // Open ground, 4 m cells over +-800 m; the trained metric (1 normalized unit = 20 m). + s.cfg.rows = s.cfg.cols = 401; + s.cfg.min_x = s.cfg.min_y = -800.0; + s.cfg.max_x = s.cfg.max_y = 800.0; + s.cfg.scale = 0.05; + base_vehicle(s.cfg, o.dt); + std::uniform_real_distribution frac(0.45, 0.85); + s.vmax_mps = frac(rng) * desc.max_speed; + s.cfg.veh.vmax = static_cast(s.vmax_mps * s.cfg.scale); + s.cfg.reach_tol = static_cast(4.0 * s.cfg.scale); + s.occ.assign(static_cast(s.cfg.rows) * s.cfg.cols, 0); + s.o.resize(2 * n); + s.goal.resize(2 * n); + s.col.assign(3 * n, 1.0f); + for (int i = 0; i < n; ++i) { + const double y0 = -300.0 + 600.0 * (i + 0.5) / n; + s.o[2 * i] = static_cast(-700.0 * s.cfg.scale); + s.o[2 * i + 1] = static_cast(y0 * s.cfg.scale); + s.goal[2 * i] = static_cast(-620.0 * s.cfg.scale); + s.goal[2 * i + 1] = static_cast((y0 + 30.0) * s.cfg.scale); + } + } else if (o.scene == "city") { + const nav::training_scene sc = nav::city_scene(96); + s.cfg.rows = sc.rows; + s.cfg.cols = sc.cols; + s.cfg.min_x = sc.min_x; + s.cfg.min_y = sc.min_y; + s.cfg.max_x = sc.max_x; + s.cfg.max_y = sc.max_y; + s.cfg.scale = sc.scale; + base_vehicle(s.cfg, o.dt); + s.cfg.veh.vmax = sc.vmax; + s.cfg.reach_tol = 0.8f * 0.15f; + // The city scaled to the vehicle, as the plant reach gate does: its wheelbase is + // coef_train's veh_L (0.035 normalized). + const double mpw = sc.scale / (0.035 / desc.wheelbase()); + s.host.plant.metres_per_world_unit = mpw; + // The vehicle plans with its own top speed when it is below nav's. + s.vmax_mps = std::min(sc.vmax / sc.scale * mpw, desc.max_speed); + s.occ = sc.occ; + s.free_cells = sc.free_cells; + scatter(s, n, rng); + } else { + s.cfg.rows = planes.rows; + s.cfg.cols = planes.cols; + s.cfg.min_x = planes.min_x; + s.cfg.min_y = planes.min_y; + s.cfg.max_x = planes.max_x; + s.cfg.max_y = planes.max_y; + s.cfg.scale = 0.05; + base_vehicle(s.cfg, o.dt); + std::uniform_real_distribution frac(0.45, 0.8); + s.vmax_mps = frac(rng) * desc.max_speed; + s.cfg.veh.vmax = static_cast(s.vmax_mps * s.cfg.scale); + s.cfg.reach_tol = static_cast(6.0 * s.cfg.scale); + s.occ = planes.occupancy; + // The physics terrain ends at the grid's edge: keep the agents 50 m inside it, where a + // vehicle can neither spawn on the rim nor drive off it. + const double cell = (planes.max_x - planes.min_x) / (planes.cols - 1); + const int band = std::max(1, static_cast(std::ceil(50.0 / cell))); + for (int r = 0; r < s.cfg.rows; ++r) + for (int k = 0; k < s.cfg.cols; ++k) + if (r < band || k < band || r >= s.cfg.rows - band || k >= s.cfg.cols - band) + s.occ[static_cast(r) * s.cfg.cols + k] = 1; + s.free_cells = largest_free_component(s.occ, s.cfg.rows, s.cfg.cols); + if (s.free_cells.empty()) + throw std::runtime_error("the bundle's grid has no free cells"); + scatter(s, n, rng); + s.geometry = &city->geometry; + // Spawn on the terrain: its height under each start, bilinear on the grid. + const nav::sim_world::config c = s.cfg; + const std::vector *hgt = &planes.height; + s.host.plant.ground_height = [c, hgt](double x, double y) { + const double fx = (x - c.min_x) / (c.max_x - c.min_x) * (c.cols - 1); + const double fy = (y - c.min_y) / (c.max_y - c.min_y) * (c.rows - 1); + const int k = std::max(0, std::min(c.cols - 2, static_cast(std::floor(fx)))); + const int r = std::max(0, std::min(c.rows - 2, static_cast(std::floor(fy)))); + const double ax = std::min(1.0, std::max(0.0, fx - k)); + const double ay = std::min(1.0, std::max(0.0, fy - r)); + const auto h = [&](int rr, int kk) { return (*hgt)[rr * c.cols + kk]; }; + return (1 - ay) * ((1 - ax) * h(r, k) + ax * h(r, k + 1)) + + ay * ((1 - ax) * h(r + 1, k) + ax * h(r + 1, k + 1)); + }; + } + + nav::sim_world sim(s.cfg, s.occ.data(), s.occ.data(), policy, s.o.data(), s.goal.data(), + s.col.data(), n); + plant_host host(s.host); + if (s.geometry) + host.set_geometry(s.geometry); + host.use(sim, o.vehicle); + nav_plant &plant = *host.plant(); + plant.set_lateral_g(o.lateral_g); + // The share of the vehicle's acceleration and braking nav plans with (nav_plant's knobs): + // below 1, the commands stay gentler, e.g. where hard braking pitches a low chassis onto + // the ground. + if (o.accel_margin > 0.0) + plant.set_accel_margin(o.accel_margin); + if (o.brake_margin > 0.0) + plant.set_brake_margin(o.brake_margin); + d.accel_margin = plant.options().accel_margin; + d.brake_margin = plant.options().brake_margin; + plant_log log(sim, plant); + + std::vector pos(2 * n), gw(2 * n), hd(n); + std::vector reached(n); + // Vehicle routes (bundle): the plant's footprint and turning circle, in world units. + const bool routed = o.scene == "bundle"; + nav::follower_options fo; + nav::route_vehicle rv; + std::vector route_fail(n, 0); + std::size_t planned = 0, unplanned = 0; + if (routed) { + const nav::plant_limits l = plant.limits(0); + nav::sim_world::plant_plan pp; + sim.plant_plan_of(0, pp); + // The planner's step is at most 0.6 x the turning radius; a radius below 2.5 cells would + // make a small vehicle's route cross the map in steps a fraction of a cell long. A wider + // turn than the vehicle's own is always drivable. + const double cell = (s.cfg.max_x - s.cfg.min_x) / (s.cfg.cols - 1); + rv.min_radius = std::max(l.wheelbase / s.cfg.scale / std::tan(l.max_steer), 2.5 * cell); + rv.disc_radius = pp.body_rr / s.cfg.scale; + for (const float off : pp.body_offsets) + rv.disc_offsets.push_back(off / s.cfg.scale); + fo.wheelbase = l.wheelbase / s.cfg.scale; + fo.max_steer = l.max_steer; + } + nav::route_follower follow(sim, fo); + const double span = s.cfg.max_x - s.cfg.min_x; + // A route from agent i's pose to a free cell 5-15% of the map away: the goal is drawn from + // the cells in that ring (a draw from the whole map lands in it ~6% of the time), and the + // planner gets a few of them. + std::size_t no_goal = 0, planner_failed = 0; + std::vector ring; + const auto plan_for = [&](int i) { + ring.clear(); + const double dx_cell = span / (s.cfg.cols - 1), + dy_cell = (s.cfg.max_y - s.cfg.min_y) / (s.cfg.rows - 1); + for (const int cell : s.free_cells) { + const double gx = s.cfg.min_x + (cell % s.cfg.cols) * dx_cell; + const double gy = s.cfg.min_y + (cell / s.cfg.cols) * dy_cell; + const double dist = std::hypot(gx - pos[2 * i], gy - pos[2 * i + 1]); + if (dist >= 0.05 * span && dist <= 0.15 * span) + ring.push_back(cell); + } + if (ring.empty()) { + ++no_goal; + ++unplanned; + ++route_fail[i]; + return; + } + std::uniform_int_distribution pk(0, ring.size() - 1); + for (int attempt = 0; attempt < 4; ++attempt) { + const int cell = ring[pk(rng)]; + const double gx = s.cfg.min_x + (cell % s.cfg.cols) * dx_cell; + const double gy = s.cfg.min_y + (cell / s.cfg.cols) * dy_cell; + nav::route_options ro; + ro.margin = 2.0 / s.host.plant.metres_per_world_unit; // 2 m off building meshes + const nav::vehicle_route r = nav::plan_vehicle_route( + s.occ.data(), s.cfg.rows, s.cfg.cols, s.cfg.min_x, s.cfg.min_y, s.cfg.max_x, + s.cfg.max_y, rv, {pos[2 * i], pos[2 * i + 1], hd[i], false}, gx, gy, ro); + if (r.reached && r.poses.size() > 2) { + follow.set(i, r); + ++planned; + return; + } + ++planner_failed; + } + ++unplanned; + ++route_fail[i]; + }; + std::uniform_real_distribution amp(15.0, 45.0), adv(50.0, 90.0); + std::vector side(n, 1), dir(n, 1); + // A routed agent stopped for 5 s (wedged against a building, or a leg it cannot drive) gets + // a new route from where it stands; the planner leads a wedged start out. + std::vector still(n, 0); + std::size_t replans = 0; + const int stuck_ticks = static_cast(std::lround(5.0 / o.dt)); + const int settle = static_cast(std::lround(0.5 / o.dt)); + std::uniform_int_distribution pick( + 0, s.free_cells.empty() ? 0 : s.free_cells.size() - 1); + for (int t = -settle; t < ticks; ++t) { + if (t == 0) + log.start(); + sim.snapshot(pos.data(), hd.data(), nullptr, nullptr, reached.data()); + sim.goals_world(gw.data()); + for (int i = 0; i < n; ++i) { + if (o.scene == "swerve") { + // The next gate once within 25 m of this one: ahead, on the other side. + const double dx = pos[2 * i] - gw[2 * i], dy = pos[2 * i + 1] - gw[2 * i + 1]; + if (std::hypot(dx, dy) < 25.0) { + if (gw[2 * i] > 650.0) + dir[i] = -1; + else if (gw[2 * i] < -650.0) + dir[i] = 1; + side[i] = -side[i]; + const double lane = -300.0 + 600.0 * (i + 0.5) / n; + const double nx = gw[2 * i] + dir[i] * adv(rng), ny = lane + side[i] * amp(rng); + sim.retarget(i, static_cast(nx * s.cfg.scale), + static_cast(ny * s.cfg.scale)); + } + } else if (routed) { + const bool stopped = t > 0 && std::fabs(log.runs()[i].back().vx) < 0.3; + still[i] = stopped ? still[i] + 1 : 0; + if (still[i] > stuck_ticks && follow.following(i) && !follow.done(i)) { + follow.clear(i); + still[i] = 0; + ++replans; + } + if ((!follow.following(i) || follow.done(i)) && route_fail[i] < 20) + plan_for(i); + } else if (reached[i]) { + float gx, gy; + cell_to_normalized(s.cfg, s.free_cells[pick(rng)], gx, gy); + sim.retarget(i, gx, gy); + } + } + if (routed) + follow.apply(); + sim.step(); + if (t >= 0) + log.record(); + // verbose: every agent's state every 5 s. + if (o.verbose && t > 0 && t % static_cast(std::lround(5.0 / o.dt)) == 0) + for (int i = 0; i < n; ++i) { + const planar_sample &q = log.runs()[i].back(); + const vehicle_state vs = plant.physics_world().vehicle_state(plant.vehicle(i)); + say(o, + " t %5.1f agent %d: (%7.1f, %7.1f, z %6.1f) vx %5.2f, wheels %d/%d, " + "contacts %d, grade %+.2f, cmd a %+6.2f steer %+.2f (has %+.2f), gear %d " + "throttle %.2f brake %.2f, route %s leg %d/%d%s\n", + t * o.dt, i, q.x, q.y, vs.body.position.z, q.vx, q.wheels_down, q.wheels, q.contacts, + q.grade, q.accel_cmd, q.steer_cmd, q.steer, vs.gear, vs.throttle, vs.brake, + routed && follow.following(i) ? "on" : "off", follow.leg(i), follow.legs(i), + routed && follow.done(i) ? " done" : ""); + } + } + for (int i = 0; i < n; ++i) { + d.runs.push_back(log.runs()[i]); + d.run_seed.push_back(seed); + d.run_agent.push_back(i); + } + std::size_t moving = 0; + for (int i = 0; i < n; ++i) + for (const planar_sample &q : log.runs()[i]) + moving += std::fabs(q.vx) > 0.5 ? 1 : 0; + say(o, + "seed %u: %d agents, %d ticks, vmax %.1f m/s, moving %.0f%% of the time, %.0f us " + "a tick\n", + seed, n, ticks, s.vmax_mps, 100.0 * moving / (static_cast(n) * (ticks + 1)), + plant.advance_us()); + if (routed) + say(o, + " routes: %zu planned, %zu requests unplanned (%zu with no goal in range), " + "%zu planner attempts failed, %zu replans of a stuck agent\n", + planned, unplanned, no_goal, planner_failed, replans); + host.use(sim, "kinematic"); // detach before the host and sim go + } + return d; +} + +void write_plant_log(const plant_log_options &o, const plant_log_drive &d) { + if (o.out.empty()) + throw std::invalid_argument("no output directory"); + const vehicle_desc desc = plant_host::preset(o.vehicle); + const std::vector> &runs = d.runs; + + // How often the wheels left the ground (by wheels down). + { + std::map down; + std::size_t total = 0; + for (const auto &run : runs) + for (const planar_sample &q : run) { + ++down[q.wheels - q.wheels_down]; + ++total; + } + say(o, "wheels off the ground:"); + for (const auto &kv : down) + say(o, " %d: %.1f%%", kv.first, 100.0 * kv.second / total); + say(o, "\n"); + } + + // The flights. + const std::size_t rows = static_cast(std::lround(o.flight_seconds / o.dt)) + 1; + const std::size_t stride = + std::max(1, static_cast(std::lround(o.stride_seconds / o.dt))); + const flight_set f = cut_flights(runs, rows, stride, 1.0, o.steer); + std::filesystem::create_directories(o.out); + write_npy((std::filesystem::path(o.out) / "flights.npy").string(), f.data, + {f.flights, f.rows, static_cast(kPlanarWidth)}); + + // samples: every sample of every run, for analysis (samples.csv). + if (o.samples) { + std::ofstream cs(std::filesystem::path(o.out) / "samples.csv"); + cs << "seed,agent,t,x,y,psi,vx,vy,r,rear_x,rear_y,steer,grade,roll,steer_cmd,accel_cmd," + "drive,vmax,a_lat,a_lon,contacts,wheels,wheels_down\n"; + char line[512]; + for (std::size_t k = 0; k < runs.size(); ++k) + for (const planar_sample &q : runs[k]) { + std::snprintf(line, sizeof line, + "%u,%d,%.3f,%.4f,%.4f,%.5f,%.4f,%.4f,%.5f,%.4f,%.4f,%.5f,%.5f,%.5f,%.5f," + "%.4f,%.4f,%.3f,%.4f,%.4f,%d,%d,%d\n", + d.run_seed[k], d.run_agent[k], q.t, q.x, q.y, q.psi, q.vx, q.vy, q.r, + q.rear_x, q.rear_y, q.steer, q.grade, q.roll, q.steer_cmd, q.accel_cmd, + q.drive, q.vmax, q.a_lat, q.a_lon, q.contacts, q.wheels, q.wheels_down); + cs << line; + } + } + + // The residuals. + const plant_residuals res = one_tick_residuals(runs, o.dt, desc, o.mu, o.nsub); + const auto print_bins = [&o](const char *name, const std::vector &bins, bool dyn) { + say(o, " %s\n %-9s %7s %12s %12s %12s %12s %12s %10s %10s\n", name, "g", "ticks", + "pos rms m", "pos max m", "head rms", "head max", "speed rms", "vy rms", "r rms"); + for (const residual_bin &b : bins) { + if (!b.ticks) + continue; + char lab[32]; + if (std::isfinite(b.hi_g)) + std::snprintf(lab, sizeof lab, "%.1f-%.1f", b.lo_g, b.hi_g); + else + std::snprintf(lab, sizeof lab, ">= %.1f", b.lo_g); + say(o, " %-9s %7llu %12.4f %12.4f %12.5f %12.5f %12.4f", lab, + static_cast(b.ticks), b.position_rms_m, b.position_max_m, + b.heading_rms_rad, b.heading_max_rad, b.speed_rms_mps); + if (dyn) + say(o, " %10.4f %10.4f", b.lateral_rms_mps, b.yaw_rate_rms); + say(o, "\n"); + } + }; + say(o, + "one-tick residual against %s on Jolt (dt %.3f s, %d substeps, mu %.2f); ticks " + "skipped: %llu static contact, %llu a wheel off the ground, %llu impact, %llu at " + "rest\n", + o.vehicle.c_str(), o.dt, o.nsub, o.mu, static_cast(res.skipped_contact), + static_cast(res.skipped_airborne), + static_cast(res.skipped_impact), + static_cast(res.skipped_rest)); + print_bins("diff::dyn_plant (dynamic single track)", res.dynamic, true); + print_bins("kinematic bicycle", res.kinematic, false); + + // fit_dyn: fit on every seed but the last, judge on the last. + dyn_bike_fit fit; + plant_residuals res_spec, res_fit; + double held_default = 0.0, held_fit = 0.0; + const unsigned held = o.seeds.back(); + if (o.fit_dyn) { + std::vector> fit_runs, test_runs; + for (std::size_t k = 0; k < runs.size(); ++k) + (o.seeds.size() > 1 && d.run_seed[k] == held ? test_runs : fit_runs).push_back(runs[k]); + if (test_runs.empty()) + test_runs = fit_runs; // one seed: in-sample + fit = fit_dyn_bike(fit_runs, o.dt, desc, o.mu, o.nsub); + const cvc::nav::dyn_bike_si spec = nav_plant::dyn_bike_si_of(desc, o.mu); + held_default = dyn_bike_cost(test_runs, o.dt, spec, desc.max_steer, o.nsub); + held_fit = dyn_bike_cost(test_runs, o.dt, fit.si, desc.max_steer, o.nsub); + say(o, + "dyn_bike fitted to %s on %zu runs (%llu ticks): drive lag %.3f s (spec %.3f), " + "steer lag %.3f s, tire B x%.2f front / x%.2f rear, yaw inertia x%.2f, drag and " + "rolling resistance x%.2f\n" + " mean squared twist residual (m/s)^2: fit set %.5f -> %.5f; held-out seed %u " + "%.5f -> %.5f\n", + o.vehicle.c_str(), fit_runs.size(), static_cast(fit.ticks), + fit.drive_lag, spec.drive_lag, fit.steer_lag, fit.tire_b_front_factor, + fit.tire_b_rear_factor, fit.yaw_inertia_factor, fit.resistance_factor, fit.cost_default, + fit.cost_fit, held, held_default, held_fit); + res_spec = one_tick_residuals(test_runs, o.dt, spec, desc.wheelbase(), desc.max_steer, o.nsub); + res_fit = one_tick_residuals(test_runs, o.dt, fit.si, desc.wheelbase(), desc.max_steer, o.nsub); + say(o, "held-out seed %u:\n", held); + print_bins("diff::dyn_plant, spec sheet", res_spec.dynamic, true); + print_bins("diff::dyn_plant, fitted", res_fit.dynamic, true); + print_bins("kinematic bicycle", res_spec.kinematic, false); + } + say(o, "flights: %zu x %zu rows (%.1f s at %.0f Hz), %zu windows dropped, peak %.2f g\n", + f.flights, f.rows, (f.rows - 1) * o.dt, 1.0 / o.dt, f.dropped, f.peak_lateral_g); + + // residual.json and meta.json. + const auto num = [](double x) { + if (!std::isfinite(x)) + return std::string("null"); + char b[40]; + std::snprintf(b, sizeof b, "%.9g", x); + return std::string(b); + }; + const auto bins_json = [&](const std::vector &bins) { + std::string s = "["; + for (std::size_t k = 0; k < bins.size(); ++k) { + const residual_bin &b = bins[k]; + s += std::string(k ? ",\n " : "") + "{\"lo_g\": " + num(b.lo_g) + + ", \"hi_g\": " + num(b.hi_g) + ", \"ticks\": " + std::to_string(b.ticks) + + ", \"position_rms_m\": " + num(b.position_rms_m) + + ", \"position_max_m\": " + num(b.position_max_m) + + ", \"heading_rms_rad\": " + num(b.heading_rms_rad) + + ", \"heading_max_rad\": " + num(b.heading_max_rad) + + ", \"speed_rms_mps\": " + num(b.speed_rms_mps) + + ", \"speed_max_mps\": " + num(b.speed_max_mps) + + ", \"lateral_rms_mps\": " + num(b.lateral_rms_mps) + + ", \"yaw_rate_rms\": " + num(b.yaw_rate_rms) + "}"; + } + return s + "]"; + }; + { + std::ofstream js(std::filesystem::path(o.out) / "residual.json"); + js << "{\n \"format\": \"cvc-plant-residual-1\",\n \"vehicle\": \"" << o.vehicle + << "\", \"scene\": \"" << o.scene << "\", \"dt\": " << num(o.dt) << ", \"nsub\": " << o.nsub + << ", \"mu\": " << num(o.mu) << ",\n \"skipped\": {\"contact\": " << res.skipped_contact + << ", \"airborne\": " << res.skipped_airborne << ", \"impact\": " << res.skipped_impact + << ", \"rest\": " << res.skipped_rest << "},\n \"dynamic\": " << bins_json(res.dynamic) + << ",\n \"kinematic\": " << bins_json(res.kinematic); + if (o.fit_dyn) + js << ",\n \"fit\": {\"held_out_seed\": " << held << ", \"drive_lag\": " << num(fit.drive_lag) + << ", \"steer_lag\": " << num(fit.steer_lag) + << ", \"tire_b_front_factor\": " << num(fit.tire_b_front_factor) + << ", \"tire_b_rear_factor\": " << num(fit.tire_b_rear_factor) + << ", \"yaw_inertia_factor\": " << num(fit.yaw_inertia_factor) + << ", \"resistance_factor\": " << num(fit.resistance_factor) << ", \"cost_fit_set\": [" + << num(fit.cost_default) << ", " << num(fit.cost_fit) << "], \"cost_held_out\": [" + << num(held_default) << ", " << num(held_fit) + << "],\n \"held_out_dynamic_spec\": " << bins_json(res_spec.dynamic) + << ",\n \"held_out_dynamic_fit\": " << bins_json(res_fit.dynamic) + << ",\n \"held_out_kinematic\": " << bins_json(res_spec.kinematic) << "}"; + js << "\n}\n"; + } + { + std::ofstream js(std::filesystem::path(o.out) / "meta.json"); + js << "{\n \"format\": \"cvc-plant-log-1\",\n \"vehicle\": \"" << o.vehicle + << "\", \"scene\": \"" << o.scene << "\", \"bundle\": \"" << o.bundle + << "\",\n \"dt\": " << num(o.dt) << ", \"rate_hz\": " << num(1.0 / o.dt) + << ", \"physics_dt\": 0.01, \"mu\": " << num(o.mu) << ", \"lateral_g\": " << num(o.lateral_g) + << ", \"seconds\": " << num(o.seconds) << ", \"accel_margin\": " << num(d.accel_margin) + << ", \"brake_margin\": " << num(d.brake_margin) + << ",\n \"layout\": [\"x\", \"y\", \"psi\", \"vx\", \"vy\", \"r\", \"drive\", \"steer\"]," + << "\n \"units\": [\"m\", \"m\", \"rad\", \"m/s\", \"m/s\", \"rad/s\", \"m/s^2\", " + "\"rad\"]," + << "\n \"frame\": \"physics world; (x, y) the centre of mass, (vx, vy) its velocity in " + "the body frame, psi continuous\"," + << "\n \"inputs\": \"row k's acted from t_k-1 to t_k; drive = nav's acceleration " + "command (zero-order hold over the tick) within the vehicle's -max_decel..max_accel, " + "steer = " + << (o.steer == steer_input::actual ? "the road-wheel angle's mean over the tick" + : "nav's steering command (held)") + << "\",\n \"shape\": [" << f.flights << ", " << f.rows << ", " << kPlanarWidth + << "], \"dropped\": " << f.dropped << ", \"peak_lateral_g\": " << num(f.peak_lateral_g) + << ",\n \"seeds\": ["; + for (std::size_t k = 0; k < o.seeds.size(); ++k) + js << (k ? ", " : "") << o.seeds[k]; + js << "],\n \"flight_seed\": ["; + for (std::size_t k = 0; k < f.flights; ++k) + js << (k ? ", " : "") << d.run_seed[f.agent[k]]; + js << "],\n \"flight_agent\": ["; + for (std::size_t k = 0; k < f.flights; ++k) + js << (k ? ", " : "") << d.run_agent[f.agent[k]]; + js << "],\n \"flight_first\": ["; + for (std::size_t k = 0; k < f.flights; ++k) + js << (k ? ", " : "") << f.first[k]; + js << "]\n}\n"; + } + say(o, "wrote %s/{flights.npy, meta.json, residual.json}\n", o.out.c_str()); +} + +} // namespace physics +} // namespace cvc diff --git a/src/cvc/tests/CMakeLists.txt b/src/cvc/tests/CMakeLists.txt index 7fb682e21..dffee9ffb 100644 --- a/src/cvc/tests/CMakeLists.txt +++ b/src/cvc/tests/CMakeLists.txt @@ -108,6 +108,7 @@ add_executable(physics_field_test physics_field_test.cpp) add_executable(physics_vehicle_test physics_vehicle_test.cpp) add_executable(physics_energy_test physics_energy_test.cpp) add_executable(nav_plant_test nav_plant_test.cpp) +add_executable(physics_plant_log_test physics_plant_log_test.cpp) add_executable(nav_dyn_bike_test nav_dyn_bike_test.cpp) add_executable(nav_terrain_test nav_terrain_test.cpp) add_executable(nav_binding_test nav_binding_test.cpp) @@ -288,6 +289,7 @@ set(TEST_TARGETS physics_vehicle_test physics_energy_test nav_plant_test + physics_plant_log_test nav_dyn_bike_test nav_terrain_test nav_binding_test @@ -1613,6 +1615,13 @@ target_link_libraries(nav_plant_test GTest::gtest_main ) +target_link_libraries(physics_plant_log_test + PRIVATE + cvc + GTest::gtest + GTest::gtest_main +) + target_link_libraries(nav_dyn_bike_test PRIVATE cvc @@ -1958,6 +1967,7 @@ target_compile_features(physics_field_test PRIVATE cxx_std_17) target_compile_features(physics_vehicle_test PRIVATE cxx_std_17) target_compile_features(physics_energy_test PRIVATE cxx_std_17) target_compile_features(nav_plant_test PRIVATE cxx_std_17) +target_compile_features(physics_plant_log_test PRIVATE cxx_std_17) target_compile_features(nav_dyn_bike_test PRIVATE cxx_std_17) target_compile_features(nav_terrain_test PRIVATE cxx_std_17) target_compile_features(nav_binding_test PRIVATE cxx_std_17) @@ -2405,6 +2415,7 @@ cvc_discover_tests(physics_field_test) cvc_discover_tests(physics_vehicle_test) cvc_discover_tests(physics_energy_test) cvc_discover_tests(nav_plant_test) +cvc_discover_tests(physics_plant_log_test) cvc_discover_tests(nav_dyn_bike_test) cvc_discover_tests(nav_terrain_test) cvc_discover_tests(nav_binding_test) diff --git a/src/cvc/tests/physics_plant_log_test.cpp b/src/cvc/tests/physics_plant_log_test.cpp new file mode 100644 index 000000000..49dda1259 --- /dev/null +++ b/src/cvc/tests/physics_plant_log_test.cpp @@ -0,0 +1,445 @@ +/* + 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. +*/ + +// physics_plant_log_test: cvc::physics::plant_log (cvc/physics/plant_log.h). +// - The residual's own arithmetic: a run made by a model itself leaves that model no residual +// (the kinematic bicycle's and diff::dyn_plant's), so the state, the command and the tick +// line up. +// - A Humvee slaloming on Jolt: the samples come at the tick, the command in row k + 1 is the +// one nav held over the tick, the centre of mass moves with its logged twist, the swerves +// pass 0.3 g, and the residuals are reported by lateral g. +// - cut_flights and write_npy: the windows, their layout and the file. +// - nav_plant_options::ground_height puts a start on the ground under it. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace cvc::physics; +using cvc::nav::sim_world; + +namespace { + +#define REQUIRE_JOLT() \ + if (!backend_available(backend::jolt)) \ + GTEST_SKIP() << "this libcvc was built without Jolt" + +constexpr double kG = 9.80665; + +// A run produced by the kinematic bicycle itself, about the rear axle, with weaving commands. +std::vector kinematic_run(const vehicle_desc &d, double dt, int nsub, int ticks) { + std::vector run(1); + run[0].vx = 6.0; + const double L = d.wheelbase(), h = dt / nsub; + for (int t = 1; t <= ticks; ++t) { + planar_sample s = run.back(); + s.t += dt; + s.steer_cmd = 0.15 * std::sin(0.3 * t); + s.accel_cmd = 0.8 * std::cos(0.2 * t); + s.drive = s.accel_cmd; + for (int j = 0; j < nsub; ++j) { + s.vx = s.vx + h * s.accel_cmd; + s.psi = s.psi + h * (s.vx / L) * std::tan(s.steer_cmd); + s.rear_x = s.rear_x + h * s.vx * std::cos(s.psi); + s.rear_y = s.rear_y + h * s.vx * std::sin(s.psi); + } + s.a_lat = s.vx * s.vx * std::tan(s.steer_cmd) / L; + run.push_back(s); + } + return run; +} + +// A run produced by diff::dyn_plant itself (the vehicle `si`, by default dyn_bike_si_of's), from +// rest in its actuator, as one_tick_residuals steps it. +std::vector dynamic_run(const vehicle_desc &d, double mu, double dt, int nsub, + int ticks, const cvc::nav::dyn_bike_si *vehicle = nullptr) { + cvc::world_units::normalized_frame si; + si.scale = 1.0; + const cvc::nav::diff::dyn_bike_params p = + cvc::nav::dyn_bike_params_of(vehicle ? *vehicle : nav_plant::dyn_bike_si_of(d, mu), si); + cvc::nav::diff::bike_veh lim{}; + lim.L = p.lf + p.lr; + lim.vmax = 1e3f; + lim.delta_max = static_cast(d.max_steer); + lim.hdt = static_cast(dt / nsub); + lim.allow_reverse = 1; + cvc::nav::diff::dyn_state m{}; + m.vx = 8.0; + std::vector run(1); + run[0].vx = m.vx; + for (int t = 1; t <= ticks; ++t) { + planar_sample s; + s.t = t * dt; + s.steer_cmd = 0.2 * std::sin(0.25 * t); + s.accel_cmd = std::cos(0.15 * t); + for (int j = 0; j < nsub; ++j) { + cvc::nav::diff::dyn_state o; + cvc::nav::diff::dyn_plant(p, lim, m, s.steer_cmd, s.accel_cmd, 0.0, o); + m = o; + } + s.rear_x = m.x; + s.rear_y = m.y; + s.psi = m.psi; + s.vx = m.vx; + s.vy = m.vy; + s.r = m.r; + s.steer = m.delta; + s.a_lat = m.vx * m.r; + run.push_back(s); + } + return run; +} + +double total_ticks(const std::vector &bins) { + double n = 0; + for (const residual_bin &b : bins) + n += static_cast(b.ticks); + return n; +} + +} // namespace + +// (The run is made in this file and replayed in plant_log.cpp, which a compiler may contract into +// FMAs differently, so "none" is to rounding: 1e-9.) +TEST(PlantLog, ARunAModelMadeLeavesThatModelNoResidual) { + const vehicle_desc d = vehicle_desc::humvee(); + for (const int nsub : {1, 2, 6}) { + const plant_residuals k = + one_tick_residuals({kinematic_run(d, 0.06, nsub, 300)}, 0.06, d, 0.9, nsub); + EXPECT_EQ(total_ticks(k.kinematic), 300.0); + for (const residual_bin &b : k.kinematic) { + EXPECT_LT(b.position_max_m, 1e-9) << "nsub " << nsub; + EXPECT_LT(b.heading_max_rad, 1e-9); + EXPECT_LT(b.speed_max_mps, 1e-9); + } + const plant_residuals y = + one_tick_residuals({dynamic_run(d, 0.9, 0.06, nsub, 300)}, 0.06, d, 0.9, nsub); + EXPECT_EQ(total_ticks(y.dynamic), 300.0); + for (const residual_bin &b : y.dynamic) { + EXPECT_LT(b.position_max_m, 1e-9) << "nsub " << nsub; + EXPECT_LT(b.heading_max_rad, 1e-9); + EXPECT_LT(b.speed_max_mps, 1e-9); + EXPECT_LT(b.lateral_rms_mps, 1e-9); + EXPECT_LT(b.yaw_rate_rms, 1e-9); + } + } + // Ticks at rest are skipped, and the bins must ascend. + std::vector rest(5); + EXPECT_EQ(one_tick_residuals({rest}, 0.06, d).skipped_rest, 4u); + // Off the ground or an impact: skipped too. + std::vector bumpy = kinematic_run(d, 0.06, 1, 4); + bumpy[2].grounded = false; + bumpy[4].a_lon = 2.0 * kG; + const plant_residuals b = one_tick_residuals({bumpy}, 0.06, d); + EXPECT_EQ(b.skipped_airborne, 2u); + EXPECT_EQ(b.skipped_impact, 1u); + EXPECT_EQ(b.skipped(), 3u); + // The kinematic reference keeps the speed within the plan's top speed. + std::vector fast = kinematic_run(d, 0.06, 1, 1); + fast[0].vmax = fast[1].vmax = 6.0; + fast[1].accel_cmd = 50.0; // a command past what the speed cap allows + const plant_residuals c = one_tick_residuals({fast}, 0.06, d); + EXPECT_NEAR(c.kinematic[0].speed_max_mps + c.kinematic[1].speed_max_mps + + c.kinematic.back().speed_max_mps, + std::fabs(fast[1].vx - 6.0), 1e-9); + EXPECT_THROW(one_tick_residuals({rest}, 0.06, d, 0.9, 1, {0.3, 0.1}), std::invalid_argument); +} + +// fit_dyn_bike finds a vehicle the spec sheet does not give from runs that vehicle made: every +// knob on its grid, the fitted cost about zero, and the spec sheet's cost well above it. +TEST(PlantLog, FitsTheDynamicModelToRunsOfAKnownVehicle) { + const vehicle_desc d = vehicle_desc::humvee(); + cvc::nav::dyn_bike_si truth = nav_plant::dyn_bike_si_of(d, 0.9); + truth.drive_lag = 0.04; + truth.tire_B_front *= 0.55; + truth.tire_B_rear *= 0.7; + truth.yaw_inertia *= 1.2; + truth.drag *= 0.5; + truth.rolling_resistance *= 0.5; + const std::vector> runs = {dynamic_run(d, 0.9, 0.06, 2, 400, &truth)}; + const dyn_bike_fit f = fit_dyn_bike(runs, 0.06, d, 0.9, 2, 3); + std::printf(" fit: drive lag %.3f, steer lag %.3f, B x%.2f / x%.2f, Iz x%.2f, resistance x%.2f; " + "cost %.3g -> %.3g\n", + f.drive_lag, f.steer_lag, f.tire_b_front_factor, f.tire_b_rear_factor, + f.yaw_inertia_factor, f.resistance_factor, f.cost_default, f.cost_fit); + EXPECT_NEAR(f.drive_lag, 0.04, 1e-12); + EXPECT_NEAR(f.steer_lag, 0.0, 1e-12); + EXPECT_NEAR(f.tire_b_front_factor, 0.55, 1e-12); + EXPECT_NEAR(f.tire_b_rear_factor, 0.7, 1e-12); + EXPECT_NEAR(f.yaw_inertia_factor, 1.2, 1e-12); + EXPECT_NEAR(f.resistance_factor, 0.5, 1e-12); + EXPECT_LT(f.cost_fit, 1e-12); + EXPECT_GT(f.cost_default, 1e-6); +} + +TEST(PlantLog, CutsFlightsAndWritesThemAsNpy) { + const vehicle_desc d = vehicle_desc::humvee(); + std::vector> runs = {kinematic_run(d, 0.02, 2, 120), + kinematic_run(d, 0.02, 2, 60)}; + runs[1][30].contacts = 1; // a window with a contact is dropped + runs[0][60].grounded = false; // and one with a wheel off the ground + for (planar_sample &s : runs[0]) + s.steer = s.steer_cmd; + const flight_set f = cut_flights(runs, 26, 25); + // Run 0: 121 samples -> windows at 0, 25, 50, 75 (the wheel off the ground drops 50); run 1: 61 + // -> 0, 25 (the contact drops 25). + EXPECT_EQ(f.flights, 4u); + EXPECT_EQ(f.dropped, 2u); + EXPECT_EQ(f.rows, 26u); + ASSERT_EQ(f.data.size(), f.flights * f.rows * kPlanarWidth); + const planar_sample &q = runs[0][25 + 3]; + const double *row = &f.data[(1 * f.rows + 3) * kPlanarWidth]; + EXPECT_EQ(row[0], q.x); + EXPECT_EQ(row[2], q.psi); + EXPECT_EQ(row[3], q.vx); + EXPECT_EQ(row[6], q.drive); + EXPECT_EQ(row[7], 0.5 * (runs[0][27].steer + q.steer)); // the steer's mean over the interval + EXPECT_EQ(cut_flights(runs, 26, 25, 1.0, steer_input::command).data[(1 * f.rows + 3) * 8 + 7], + q.steer_cmd); + + const std::string path = + (std::filesystem::temp_directory_path() / "cvc_plant_log_test.npy").string(); + write_npy(path, f.data, {f.flights, f.rows, static_cast(kPlanarWidth)}); + std::vector bytes; + { + // Closed before the remove below: Windows refuses to delete an open file. + std::ifstream in(path, std::ios::binary); + bytes.assign(std::istreambuf_iterator(in), std::istreambuf_iterator()); + } + std::error_code ec; + std::filesystem::remove(path, ec); + ASSERT_GT(bytes.size(), 10u); + EXPECT_EQ(std::memcmp(bytes.data(), "\x93NUMPY\x01\x00", 8), 0); + const std::size_t hl = static_cast(bytes[8]) | + (static_cast(static_cast(bytes[9])) << 8); + EXPECT_EQ((10 + hl) % 64, 0u); + const std::string header(bytes.data() + 10, hl); + EXPECT_NE(header.find("'descr': '(14.0 * c.scale); + c.freeze_sense = true; + c.reach_tol = static_cast(4.0 * c.scale); + std::vector occ(static_cast(c.rows) * c.cols, 0); + const float o[2] = {static_cast(-300 * c.scale), 0.0f}; + const float g[2] = {static_cast(-240 * c.scale), static_cast(25 * c.scale)}; + const float col[3] = {1, 1, 1}; + sim_world sim(c, occ.data(), occ.data(), cvc::nav::coef_mlp::default_biased(), o, g, col, 1); + plant_host_options ho; + ho.plant.plan_with_vehicle = true; + plant_host host(ho); + host.use(sim, "humvee"); + nav_plant &plant = *host.plant(); + plant.set_lateral_g(0.9); + plant_log log(sim, plant); + log.start(); + float pos[2], gw[2]; + int side = 1; + std::vector cmd_s, cmd_a; + for (int t = 0; t < 1500; ++t) { + sim.snapshot(pos, nullptr, nullptr, nullptr, nullptr); + sim.goals_world(gw); + if (std::hypot(pos[0] - gw[0], pos[1] - gw[1]) < 25.0) { + side = -side; + sim.retarget(0, static_cast((gw[0] + 70.0) * c.scale), + static_cast(side * 30.0 * c.scale)); + } + sim.step(); + log.record(); + cmd_s.push_back(sim.plant_steer_command()[0]); + cmd_a.push_back(sim.plant_accel_command()[0]); + } + ASSERT_EQ(log.agents(), 1); + const std::vector &run = log.runs()[0]; + ASSERT_EQ(run.size(), 1501u); + EXPECT_EQ(run[0].steer_cmd, 0.0); + double peak = 0.0, kin_err = 0.0, vmax = 0.0; + for (std::size_t k = 1; k < run.size(); ++k) { + EXPECT_NEAR(run[k].t, 0.02 * k, 1e-9); + // Row k's command is the one nav held over the tick that ended there, in SI. + EXPECT_EQ(run[k].steer_cmd, cmd_s[k - 1]); + EXPECT_NEAR(run[k].accel_cmd, cmd_a[k - 1] / c.scale, + 1e-9 * (1.0 + std::fabs(run[k].accel_cmd))); + // The centre of mass moves with its logged twist (the trapezoid over the tick). + const planar_sample &a = run[k - 1], &b = run[k]; + const double vxw = 0.5 * (a.vx * std::cos(a.psi) - a.vy * std::sin(a.psi) + + b.vx * std::cos(b.psi) - b.vy * std::sin(b.psi)); + const double vyw = 0.5 * (a.vx * std::sin(a.psi) + a.vy * std::cos(a.psi) + + b.vx * std::sin(b.psi) + b.vy * std::cos(b.psi)); + kin_err = std::max(kin_err, std::hypot((b.x - a.x) / 0.02 - vxw, (b.y - a.y) / 0.02 - vyw)); + peak = std::max(peak, std::fabs(b.a_lat) / kG); + vmax = std::max(vmax, b.vx); + } + std::printf(" swerve: peak %.2f g, top speed %.1f m/s, |CoM velocity - twist| max %.3f m/s\n", + peak, vmax, kin_err); + EXPECT_GT(peak, 0.3) << "the swerves pass 0.3 g"; + EXPECT_LT(kin_err, 0.1); + EXPECT_GT(vmax, 8.0); + + const plant_residuals res = one_tick_residuals(log.runs(), 0.02, d, 0.9, 2); + for (const char *which : {"dynamic", "kinematic"}) { + const std::vector &bins = + std::string(which) == "dynamic" ? res.dynamic : res.kinematic; + std::printf(" %s residual over one 0.02 s tick:\n", which); + for (const residual_bin &b : bins) + if (b.ticks) + std::printf(" %.1f-%.1f g: %4llu ticks, position rms %.4f m, heading rms %.5f rad, " + "speed rms %.4f m/s\n", + b.lo_g, std::isfinite(b.hi_g) ? b.hi_g : 9.9, + static_cast(b.ticks), b.position_rms_m, b.heading_rms_rad, + b.speed_rms_mps); + } + EXPECT_GT(total_ticks(res.dynamic), 1000.0); + EXPECT_GT(res.dynamic.back().ticks + res.dynamic[res.dynamic.size() - 2].ticks, 0u) + << "some ticks above 0.3 g"; + // One 0.02 s tick from the plant's own state: both models stay within centimetres. + for (const residual_bin &b : res.dynamic) + if (b.ticks) { + EXPECT_LT(b.position_rms_m, 0.05); + EXPECT_TRUE(std::isfinite(b.yaw_rate_rms)); + } + host.use(sim, "kinematic"); +} + +TEST(PlantLog, GroundHeightPutsAStartOnTheGroundUnderIt) { + REQUIRE_JOLT(); + const vehicle_desc d = vehicle_desc::roboracer(); + double z[2]; + for (int k = 0; k < 2; ++k) { + world_config wc; + wc.backend = backend::jolt; + world w(wc); + body_desc g; + g.shape = box{{50.0, 50.0, 0.5}}; + g.motion = motion_type::static_; + g.position = {0.0, 0.0, 3.0 - 0.5}; // the ground's top at z = 3 + w.add_body(g); + nav_plant_options po; + if (k == 0) + po.ground_z = 3.0; + else + po.ground_height = [](double, double) { return 3.0; }; + nav_plant plant(w, {d}, po); + cvc::nav::plant_state s0{}; + cvc::world_units::normalized_frame f; + f.scale = 0.05; + plant.attach(1, &s0, f, 0.02); + z[k] = w.vehicle_state(plant.vehicle(0)).body.position.z; + } + EXPECT_EQ(z[0], z[1]); + EXPECT_GT(z[1], 3.0); +} + +TEST(PlantLogScene, ResolveFillsTheSceneDefaultsAndRejectsBadOptions) { + plant_log_options o; + o.resolve(); + EXPECT_EQ(o.agents, 4); + EXPECT_EQ(o.lateral_g, 0.9); + EXPECT_EQ(o.nsub, 2); // 0.02 s is two physics steps + plant_log_options c; + c.scene = "city"; + c.dt = 0.06; + c.resolve(); + EXPECT_EQ(c.agents, 8); + EXPECT_EQ(c.lateral_g, 0.5); + EXPECT_EQ(c.nsub, 6); + for (const auto &bad : std::vector>{ + [](plant_log_options &x) { x.scene = "moon"; }, + [](plant_log_options &x) { x.scene = "bundle"; }, // no bundle directory + [](plant_log_options &x) { x.dt = 0.025; }, + [](plant_log_options &x) { x.seeds.clear(); }}) { + plant_log_options x; + bad(x); + EXPECT_THROW(x.resolve(), std::invalid_argument); + } +} + +// The scenes are deterministic: the same options drive the same runs, which write_plant_log +// turns into the directory liesphgp_pack.py reads. +TEST(PlantLogScene, DrivesTheSwerveTheSameTwiceAndWritesItsDirectory) { + REQUIRE_JOLT(); + plant_log_options o; + o.seeds = {7, 8}; + o.agents = 2; + o.seconds = 2.0; + o.flight_seconds = 1.0; + o.stride_seconds = 1.0; + o.resolve(); + const cvc::nav::coef_mlp net = cvc::nav::coef_mlp::default_biased(); + const plant_log_drive a = drive_plant_log_scene(o, net), b = drive_plant_log_scene(o, net); + ASSERT_EQ(a.runs.size(), 4u); + EXPECT_EQ(a.run_seed, (std::vector{7, 7, 8, 8})); + EXPECT_EQ(a.run_agent, (std::vector{0, 1, 0, 1})); + EXPECT_GT(a.accel_margin, 0.0); + ASSERT_EQ(b.runs.size(), a.runs.size()); + for (std::size_t k = 0; k < a.runs.size(); ++k) { + ASSERT_EQ(a.runs[k].size(), 101u); // the start and 100 ticks + ASSERT_EQ(b.runs[k].size(), a.runs[k].size()); + for (std::size_t t = 0; t < a.runs[k].size(); ++t) { + const planar_sample &p = a.runs[k][t], &q = b.runs[k][t]; + ASSERT_EQ(std::memcmp(&p.x, &q.x, sizeof p.x), 0) << k << " " << t; + ASSERT_EQ(std::memcmp(&p.psi, &q.psi, sizeof p.psi), 0) << k << " " << t; + ASSERT_EQ(std::memcmp(&p.vy, &q.vy, sizeof p.vy), 0) << k << " " << t; + ASSERT_EQ(std::memcmp(&p.steer, &q.steer, sizeof p.steer), 0) << k << " " << t; + } + } + EXPECT_GT(a.runs[0].back().vx, 1.0); // it drove + + o.out = (std::filesystem::temp_directory_path() / "cvc_plant_log_scene_test").string(); + std::error_code ec; + std::filesystem::remove_all(o.out, ec); + write_plant_log(o, a); + for (const char *f : {"flights.npy", "meta.json", "residual.json"}) + EXPECT_TRUE(std::filesystem::exists(std::filesystem::path(o.out) / f)) << f; + std::string meta; + { + std::ifstream in(std::filesystem::path(o.out) / "meta.json"); + meta.assign(std::istreambuf_iterator(in), std::istreambuf_iterator()); + } + std::filesystem::remove_all(o.out, ec); + EXPECT_NE(meta.find("\"format\": \"cvc-plant-log-1\""), std::string::npos) << meta; + EXPECT_NE(meta.find("\"seeds\": [7, 8]"), std::string::npos) << meta; + EXPECT_NE(meta.find("\"shape\": [8, 51, 8]"), std::string::npos) << meta; // 2 s / 1 s x 4 runs +} diff --git a/src/cvc/tools/liesphgp_pack.py b/src/cvc/tools/liesphgp_pack.py new file mode 100644 index 000000000..23cd93250 --- /dev/null +++ b/src/cvc/tools/liesphgp_pack.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python3 +"""Pack `cvc plant-log` runs into a LieSPHGP dataset pickle. + +`cvc plant-log` (cvc::physics::write_plant_log) writes, per run directory, ``flights.npy`` +(float64, (F, T, 8): x, y, psi, vx, vy, r, drive, steer at a uniform rate) and ``meta.json``. +This script turns one or more of those directories into the pickle CVC-Lab/LieSPHGP reads +(``src/models/SE3_Quadrotor/lie_ph/data.py`` and the ROV variant's loader at commit 65db994): + +- a dict, pickled with the highest protocol, with + - ``t``: ``arange(T) * dt`` (the loaders take dt as the median step of ``t``); + - ``train_trajectories``: (F_train, T, D), observation noise added when ``--noise`` > 0; + - ``test_trajectories``: the clean test flights; ``test_trajectories_noisy``: with noise; + - ``settings``: the rate, the layouts, the split, the noise and each source run's meta. +- Equal-length flights at a uniform rate, zero-order-hold inputs: row k's inputs acted from + t_k-1 to t_k, and row 0's are zero (unused), as LieSPHGP's generator writes them. +- ``--layout se3`` (the default): 20-wide rows that LieSPHGP's ROV loader reads with + ``model.control_dim: 2``: the planar pose lifted to SE(3), + ``[x, y, 0, vec(Rz(psi)) row-major, vx, vy, 0, 0, 0, r, drive, steer]``. + ``--layout se2``: the 8-wide rows as written, for an SE(2) port of the model. +- Noise (``--noise SIGMA``, absolute, as the paper's generator adds it): N(0, sigma) on x, y, + vx, vy and r, and the rotation perturbed by a yaw drawn from N(0, sigma^2); the inputs are + left exact. Only the planar channels get noise, so the lifted rows stay planar. + +The split is by seed (``--test-seeds``) or, without it, every ``--test-every``-th flight. +``--check`` re-reads the pickle and checks it against that contract. + +Standard library and numpy only. +""" + +from __future__ import annotations + +import argparse +import json +import math +import pickle +import sys +from pathlib import Path + +import numpy as np + +PLANAR = ("x", "y", "psi", "vx", "vy", "r", "drive", "steer") +SE3 = ( + ["x_w", "y_w", "z_w"] + + [f"R{i}{j}" for i in range(3) for j in range(3)] + + ["vx_b", "vy_b", "vz_b", "wx_b", "wy_b", "wz_b", "drive", "steer"] +) + + +def load_runs(dirs): + flights, seeds, metas, dt, rows = [], [], [], None, None + for d in dirs: + d = Path(d) + meta = json.loads((d / "meta.json").read_text(encoding="utf-8")) + if meta.get("format") != "cvc-plant-log-1": + raise SystemExit(f"{d}: not a cvc-plant-log run (format {meta.get('format')!r})") + f = np.load(d / "flights.npy", allow_pickle=False) + if f.ndim != 3 or f.shape[2] != len(PLANAR): + raise SystemExit(f"{d}: flights.npy has shape {f.shape}, expected (F, T, 8)") + if dt is None: + dt, rows = float(meta["dt"]), f.shape[1] + elif not math.isclose(float(meta["dt"]), dt) or f.shape[1] != rows: + raise SystemExit( + f"{d}: dt {meta['dt']} / {f.shape[1]} rows differ from the first run's" + ) + if len(meta["flight_seed"]) != f.shape[0]: + raise SystemExit(f"{d}: meta.json's flight_seed does not match flights.npy") + flights.append(f) + seeds.extend(int(s) for s in meta["flight_seed"]) + metas.append( + {"dir": str(d), **{k: v for k, v in meta.items() if not k.startswith("flight_")}} + ) + if not flights: + raise SystemExit("no runs") + return np.concatenate(flights, 0), np.asarray(seeds), metas, dt + + +def lift_se3(p): + """(F, T, 8) planar rows -> (F, T, 20) SE(3) rows (planar).""" + F, T, _ = p.shape + out = np.zeros((F, T, len(SE3)), np.float64) + c, s = np.cos(p[..., 2]), np.sin(p[..., 2]) + out[..., 0], out[..., 1] = p[..., 0], p[..., 1] + out[..., 3], out[..., 4], out[..., 6], out[..., 7] = c, -s, s, c + out[..., 11] = 1.0 + out[..., 12], out[..., 13], out[..., 17] = p[..., 3], p[..., 4], p[..., 5] + out[..., 18], out[..., 19] = p[..., 6], p[..., 7] + return out + + +def add_noise(p, sigma, rng): + """Absolute observation noise on the planar state (x, y, psi, vx, vy, r); inputs exact.""" + q = p.copy() + for k in (0, 1, 2, 3, 4, 5): + q[..., k] += rng.normal(0.0, sigma, size=q.shape[:2]) + return q + + +def build(planar, seeds, metas, dt, layout, sigma, noise_seed, test_seeds, test_every): + planar = planar.copy() + planar[:, 0, 6:8] = 0.0 # row 0's inputs: unused, zero as LieSPHGP writes them + if test_seeds: + test = np.isin(seeds, list(test_seeds)) + else: + test = (np.arange(planar.shape[0]) % test_every) == test_every - 1 + if test.all() or not test.any(): + raise SystemExit(f"the split leaves {int(test.sum())} of {test.size} flights for test") + rng = np.random.default_rng(noise_seed) + noisy = add_noise(planar, sigma, rng) if sigma > 0 else planar.copy() + to = lift_se3 if layout == "se3" else (lambda x: x) + T = planar.shape[1] + return { + "t": np.arange(T, dtype=np.float64) * dt, + "train_trajectories": to(noisy[~test]), + "test_trajectories": to(planar[test]), + "test_trajectories_noisy": to(noisy[test]), + "train_seed": seeds[~test], + "test_seed": seeds[test], + "settings": { + "source": "libcvc cvc-plant-log (cvc::physics::nav_plant on Jolt)", + "sample_dt": dt, + "physics_hz": 100.0, + "state_layout": list(SE3[:18] if layout == "se3" else PLANAR[:6]), + "control_layout": ["drive", "steer"], + "control_dim": 2, + "layout": layout, + "hold": "zero-order: row k's inputs acted from t_k-1 to t_k; row 0's are zero", + "observation_noise": {"type": "absolute", "sigma": sigma, "seed": noise_seed}, + "splits": { + "train": int((~test).sum()), + "test": int(test.sum()), + "test_seeds": sorted(int(s) for s in test_seeds) if test_seeds else None, + "test_every": None if test_seeds else test_every, + }, + "runs": metas, + }, + } + + +def check(path, layout): + with open(path, "rb") as fh: + d = pickle.load(fh) + width = len(SE3) if layout == "se3" else len(PLANAR) + for key in ("t", "train_trajectories", "test_trajectories", "test_trajectories_noisy"): + if key not in d: + raise SystemExit(f"check: {key} missing") + t = d["t"] + dt = float(np.median(np.diff(t))) + if not np.allclose(np.diff(t), dt): + raise SystemExit("check: t is not uniform") + for key in ("train_trajectories", "test_trajectories", "test_trajectories_noisy"): + a = d[key] + if a.ndim != 3 or a.shape[1] != t.size or a.shape[2] != width or a.dtype != np.float64: + raise SystemExit(f"check: {key} has shape {a.shape} {a.dtype}") + if not np.isfinite(a).all(): + raise SystemExit(f"check: {key} is not finite") + if np.any(a[:, 0, -2:] != 0.0): + raise SystemExit(f"check: {key} row 0 inputs are not zero") + if layout == "se3": + R = d["test_trajectories"][..., 3:12].reshape(-1, 3, 3) + err = np.abs(np.einsum("nij,nkj->nik", R, R) - np.eye(3)).max() + if err > 1e-9: + raise SystemExit(f"check: R is not orthonormal ({err:.2e})") + # The ROV loader's contract: 18 pose and twist columns, then control_dim inputs. + if d["test_trajectories"].shape[-1] - 18 != 2: + raise SystemExit("check: not 2 inputs after the 18 pose/twist columns") + return d + + +def main(argv=None): + ap = argparse.ArgumentParser(description=__doc__.split("\n")[0]) + ap.add_argument("runs", nargs="+", help="cvc plant-log --out directories") + ap.add_argument("--out", required=True, help="the pickle to write") + ap.add_argument("--layout", choices=("se3", "se2"), default="se3") + ap.add_argument("--noise", type=float, default=0.05, help="absolute observation noise sigma") + ap.add_argument("--noise-seed", type=int, default=0) + ap.add_argument("--test-seeds", default="", help="comma-separated seeds held out for test") + ap.add_argument("--test-every", type=int, default=5, help="without --test-seeds: every k-th") + ap.add_argument("--check", action="store_true", help="re-read and check the pickle") + a = ap.parse_args(argv) + planar, seeds, metas, dt = load_runs(a.runs) + test_seeds = {int(s) for s in a.test_seeds.split(",") if s.strip()} + d = build(planar, seeds, metas, dt, a.layout, a.noise, a.noise_seed, test_seeds, a.test_every) + with open(a.out, "wb") as fh: + pickle.dump(d, fh, protocol=pickle.HIGHEST_PROTOCOL) + s = d["settings"]["splits"] + print( + f"wrote {a.out}: {s['train']} train / {s['test']} test flights x {d['t'].size} rows " + f"at {1.0 / dt:.0f} Hz, layout {a.layout}, noise sigma {a.noise}" + ) + if a.check: + check(a.out, a.layout) + print("check: ok") + return 0 + + +if __name__ == "__main__": + sys.exit(main())