From a7ffdf9a766ff6f559473cb63fec0112736850d0 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 15:20:14 -0500 Subject: [PATCH 01/12] physics: log nav_plant runs for fitting a plant model, and dyn_plant's residual against Jolt A port-Hamiltonian vehicle model fitted to the physics needs the physics' trajectories in a plant-learning format, and a baseline to beat. This adds both. - cvc::physics::plant_log (plant_log.h): samples a sim_world's agents on a nav_plant every tick as planar rigid bodies in SI: the centre of mass's pose on SE(2), its body twist, the steering, the command nav held over the tick (zero-order hold), the measured acceleration, contacts and wheels on the ground. - one_tick_residuals: the plan's per-tick drift (D14) with diff::dyn_plant beside the kinematic bicycle. Both step from Jolt's state with the held command, binned by the lateral g Jolt measured. Ticks with a static contact, a wheel off the ground, an impact (> 1.5 g) or at rest are left out, and counted. - cut_flights / write_npy: equal-length windows, [x, y, psi, vx, vy, r, drive, steer] float64. - cvc-plant-log: drives the Humvee or RoboRacer on Jolt through nav in a slalom (swerves past 0.3 g), the coef trainer's city, or a cvc::world city bundle (vehicle routes on its rasterized map, the vehicles spawned on its terrain), and writes flights.npy, meta.json and residual.json. src/cvc/tools/liesphgp_pack.py packs runs into the dataset pickle LieSPHGP's loaders read (the SE(3)-lifted 20-wide rows of its ROV loader with control_dim 2, or 8-wide SE(2)), with observation noise at a stated sigma. - nav_plant_options::ground_height: a start's ground for a terrain that is not flat (empty: ground_z, as before). - The roadmap's drift section publishes dyn_plant's residual at nav's 0.06 s tick: 4-18x closer than the kinematic bicycle above 0.5 g in position; worse in speed for the RoboRacer, whose driver model is faster than dyn_bike_si_of's 0.1 s drive lag. physics_plant_log_test (4): a run a model made leaves that model no residual (kinematic and dynamic, nsub 1/2/6); flights, their layout and the .npy; a Humvee slalom on Jolt (row k's command is the one held over the tick, the centre of mass moves with its logged twist to 0.023 m/s, peak 0.87 g); ground_height spawns as ground_z does. --- docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md | 31 + inc/cvc/physics/nav_plant.h | 4 + inc/cvc/physics/plant_log.h | 160 +++++ src/cvc/CMakeLists.txt | 12 + src/cvc/physics/nav_plant.cpp | 3 +- src/cvc/physics/plant_log.cpp | 341 +++++++++++ src/cvc/tests/CMakeLists.txt | 11 + src/cvc/tests/physics_plant_log_test.cpp | 334 +++++++++++ src/cvc/tools/cvc_plant_log_main.cpp | 630 ++++++++++++++++++++ src/cvc/tools/liesphgp_pack.py | 197 ++++++ 10 files changed, 1722 insertions(+), 1 deletion(-) create mode 100644 inc/cvc/physics/plant_log.h create mode 100644 src/cvc/physics/plant_log.cpp create mode 100644 src/cvc/tests/physics_plant_log_test.cpp create mode 100644 src/cvc/tools/cvc_plant_log_main.cpp create mode 100644 src/cvc/tools/liesphgp_pack.py diff --git a/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md b/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md index 877ac989f..10ed3a816 100644 --- a/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md +++ b/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md @@ -150,6 +150,37 @@ 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, dynamic / kinematic. + +| lateral g | Humvee slalom | Humvee city | RoboRacer slalom | RoboRacer city | +|---|---|---|---|---| +| 0.0-0.1 | 2.0 / 3.9 | 2.0 / 3.4 | 0.9 / 0.7 | 2.5 / 1.9 | +| 0.1-0.2 | 5.1 / 11.6 | 4.8 / 11.1 | 3.3 / 5.6 | 4.3 / 4.6 | +| 0.2-0.3 | 7.1 / 17.9 | 7.0 / 18.1 | 5.0 / 10.7 | 5.8 / 9.4 | +| 0.3-0.5 | 8.1 / 27.5 | 8.1 / 26.2 | 6.9 / 15.8 | 7.1 / 13.8 | +| >= 0.5 | 5.2 / 94.0 | 4.5 / 52.0 | 6.5 / 25.1 | 4.8 / 19.0 | + +- **Position and heading:** the dynamic model is closer than the kinematic bicycle wherever the tires + carry load. Above 0.5 g it is closer by 4-18x. +- **Speed:** for the RoboRacer the dynamic model is worse, with 0.02-0.12 m/s rms against + 0.007-0.06. Its 0.1 s drive lag (`dyn_bike_si_of`'s `drive_lag`) is slower than the vehicle's + driver model. +- **Yaw rate:** in the city it is off by 0.1-0.3 rad/s rms. + +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 cc6b84478..0db6fc754 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..1d3c96c52 --- /dev/null +++ b/inc/cvc/physics/plant_log.h @@ -0,0 +1,160 @@ +/* + 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 + +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 + 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}); + +// ── 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/src/cvc/CMakeLists.txt b/src/cvc/CMakeLists.txt index 2da302220..29b4a88a9 100644 --- a/src/cvc/CMakeLists.txt +++ b/src/cvc/CMakeLists.txt @@ -70,6 +70,7 @@ 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/state/state_hash_partition.h ../../inc/cvc/state/distributed_state_session.h ../../inc/cvc/state/state_transport.h @@ -256,6 +257,7 @@ set(SOURCE_FILES physics/binding_meta.cpp physics/nav_plant.cpp physics/nav_eval.cpp + physics/plant_log.cpp state/state_change_journal.cpp state/state_subscription_router.cpp state/state_sync_adapter.cpp @@ -2194,6 +2196,16 @@ if(CVC_BUILD_LSYS_TOOLS) endif() endif() +# cvc-plant-log: nav driving physics vehicles, logged as planar rigid bodies for fitting a plant +# model, with the dynamic single-track model's residual against them (cvc/physics/plant_log.h). +# Links only cvc::cvc; it needs the Jolt backend at run time. +option(CVC_BUILD_PLANT_LOG "Build the cvc-plant-log tool" ON) +if(CVC_BUILD_PLANT_LOG AND NOT EMSCRIPTEN) + add_executable(cvc-plant-log ${CMAKE_CURRENT_SOURCE_DIR}/tools/cvc_plant_log_main.cpp) + target_link_libraries(cvc-plant-log PRIVATE cvc) + target_compile_features(cvc-plant-log PRIVATE cxx_std_17) +endif() + # ariadne-server: the headless Ariadne conference RELAY (room-state relay + media SFU). Core cvc + # gRPC only (no cvcGL/VTK/GL), so it runs on a plain headless VM. Needs CVC_ENABLE_GRPC. if(CVC_ENABLE_GRPC) diff --git a/src/cvc/physics/nav_plant.cpp b/src/cvc/physics/nav_plant.cpp index 3495e9207..b1f49aa0c 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..b45141f76 --- /dev/null +++ b/src/cvc/physics/plant_log.cpp @@ -0,0 +1,341 @@ +/* + 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.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); + } +} + +plant_residuals one_tick_residuals(const std::vector> &runs, double dt, + const vehicle_desc &d, double mu, 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"); + cvc::world_units::normalized_frame si; + si.scale = 1.0; + const cvc::nav::diff::dyn_bike_params p = + cvc::nav::dyn_bike_params_of(nav_plant::dyn_bike_si_of(d, mu), si); + const double h = dt / nsub; + cvc::nav::diff::bike_veh lim{}; + lim.L = p.lf + p.lr; + lim.delta_max = static_cast(d.max_steer); + lim.hdt = static_cast(h); + lim.allow_reverse = 1; + const double L = d.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]; + // Both models keep the speed within the plan's, as sim_world's reference does. + const double vmax = s0.vmax > 0.0 ? s0.vmax : 1e3; + lim.vmax = static_cast(vmax); + // The dynamic model, from the plant's state at the tick's start. + 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; + // The kinematic bicycle (D14's reference): the command at once, about the rear axle. + 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); + } + if (s1.contacts > 0) { + ++out.skipped_contact; + continue; + } + if (!s0.grounded || !s1.grounded) { + ++out.skipped_airborne; + continue; + } + if (std::hypot(s1.a_lat, s1.a_lon) > kImpactG * kG) { + ++out.skipped_impact; + continue; + } + if (std::fabs(s0.vx) < 0.2 && std::fabs(s1.vx) < 0.2) { + ++out.skipped_rest; + continue; + } + 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; +} + +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/tests/CMakeLists.txt b/src/cvc/tests/CMakeLists.txt index 724c5689c..a0dc9f5ba 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) @@ -284,6 +285,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 @@ -1590,6 +1592,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 @@ -1923,6 +1932,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) @@ -2369,6 +2379,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..f11f4f5e9 --- /dev/null +++ b/src/cvc/tests/physics_plant_log_test.cpp @@ -0,0 +1,334 @@ +/* + 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 + +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 from dyn_bike_si_of), 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) { + cvc::world_units::normalized_frame si; + si.scale = 1.0; + const cvc::nav::diff::dyn_bike_params p = + cvc::nav::dyn_bike_params_of(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 + +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-12); + EXPECT_LT(b.speed_max_mps, 1e-12); + } + 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-12); + EXPECT_LT(b.speed_max_mps, 1e-12); + EXPECT_LT(b.lateral_rms_mps, 1e-12); + EXPECT_LT(b.yaw_rate_rms, 1e-12); + } + } + // 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); +} + +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::ifstream in(path, std::ios::binary); + std::vector bytes((std::istreambuf_iterator(in)), std::istreambuf_iterator()); + 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); +} diff --git a/src/cvc/tools/cvc_plant_log_main.cpp b/src/cvc/tools/cvc_plant_log_main.cpp new file mode 100644 index 000000000..07d76d8be --- /dev/null +++ b/src/cvc/tools/cvc_plant_log_main.cpp @@ -0,0 +1,630 @@ +/* + 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-plant-log: nav driving physics vehicles (cvc::physics::nav_plant on Jolt), logged as planar +// rigid bodies for fitting a plant model, and the dynamic single-track model's one-tick residual +// against them (cvc/physics/plant_log.h). +// +// cvc-plant-log --vehicle humvee --scene swerve --seeds 100,101,102 --out runs/humvee-swerve +// cvc-plant-log --vehicle roboracer --scene city --agents 8 --out runs/roboracer-city +// cvc-plant-log --vehicle humvee --scene bundle --bundle --out runs/humvee-city +// +// 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. +// +// Out (--out DIR): +// flights.npy float64 (F, T, 8): equal-length flights cut from the runs, rows +// [x, y, psi, vx, vy, r, drive, steer] (cvc::physics::kPlanarWidth): the centre +// of mass on SE(2), its body twist, nav's acceleration command and the steering +// (--steer actual|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 printed). +// tools/liesphgp_pack.py turns one or more of these directories into a LieSPHGP dataset pickle. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +namespace phys = cvc::physics; +namespace nav = cvc::nav; + +struct options { + std::string vehicle = "humvee"; + std::string scene = "swerve"; + std::string bundle; + std::string policy; + std::string out; + std::vector seeds = {100, 101, 102, 103}; + int agents = -1; // per scene default + double seconds = 60.0; + double dt = 0.02; + double mu = 0.9; + double lateral_g = -1.0; // per scene default + double flight_seconds = 5.0; + double stride_seconds = 5.0; + int nsub = 0; // residual substeps; 0 = the physics steps a tick + int grid = 600; + int inflate = 0; + phys::steer_input steer = phys::steer_input::actual; +}; + +void usage() { + std::printf( + "cvc-plant-log --out DIR [--vehicle humvee|roboracer] [--scene swerve|city|bundle]\n" + " [--bundle DIR] [--seeds 100,101,...] [--agents N] [--seconds S] [--dt S]\n" + " [--mu MU] [--lateral-g G] [--flight-seconds S] [--stride-seconds S]\n" + " [--steer actual|command] [--nsub N] [--grid N] [--inflate CELLS]\n" + " [--policy FILE.cvcnav]\n"); +} + +options parse(int argc, char **argv) { + options o; + for (int i = 1; i < argc; ++i) { + const std::string a = argv[i]; + const auto val = [&]() -> std::string { + if (i + 1 >= argc) + throw std::invalid_argument(a + " needs a value"); + return argv[++i]; + }; + if (a == "--vehicle") + o.vehicle = val(); + else if (a == "--scene") + o.scene = val(); + else if (a == "--bundle") + o.bundle = val(); + else if (a == "--policy") + o.policy = val(); + else if (a == "--out") + o.out = val(); + else if (a == "--seeds") { + o.seeds.clear(); + std::stringstream ss(val()); + std::string t; + while (std::getline(ss, t, ',')) + o.seeds.push_back(static_cast(std::stoul(t))); + } else if (a == "--agents") + o.agents = std::stoi(val()); + else if (a == "--seconds") + o.seconds = std::stod(val()); + else if (a == "--dt") + o.dt = std::stod(val()); + else if (a == "--mu") + o.mu = std::stod(val()); + else if (a == "--lateral-g") + o.lateral_g = std::stod(val()); + else if (a == "--flight-seconds") + o.flight_seconds = std::stod(val()); + else if (a == "--stride-seconds") + o.stride_seconds = std::stod(val()); + else if (a == "--nsub") + o.nsub = std::stoi(val()); + else if (a == "--grid") + o.grid = std::stoi(val()); + else if (a == "--inflate") + o.inflate = std::stoi(val()); + else if (a == "--steer") { + const std::string s = val(); + if (s == "actual") + o.steer = phys::steer_input::actual; + else if (s == "command") + o.steer = phys::steer_input::command; + else + throw std::invalid_argument("--steer is actual or command"); + } else if (a == "-h" || a == "--help") { + usage(); + std::exit(0); + } else + throw std::invalid_argument("unknown option " + a); + } + if (o.out.empty()) + throw std::invalid_argument("--out is required"); + if (o.scene != "swerve" && o.scene != "city" && o.scene != "bundle") + throw std::invalid_argument("--scene is swerve, city or bundle"); + if (o.scene == "bundle" && o.bundle.empty()) + throw std::invalid_argument("--scene bundle needs --bundle DIR"); + if (o.agents < 0) + o.agents = o.scene == "swerve" ? 4 : 8; + if (o.lateral_g < 0.0) + o.lateral_g = o.scene == "swerve" ? 0.9 : 0.5; + return o; +} + +// 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 + phys::plant_host_options host; // the physics world and the plant's options + const cvc::world::world_geometry *geometry = nullptr; + std::vector height; // the terrain's heights on the grid (bundle) + 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 + +int main(int argc, char **argv) { + options opt; + try { + opt = parse(argc, argv); + } catch (const std::exception &e) { + std::fprintf(stderr, "cvc-plant-log: %s\n", e.what()); + usage(); + return 2; + } + if (!phys::plant_host::available()) { + std::fprintf(stderr, "cvc-plant-log: this libcvc was built without Jolt\n"); + return 1; + } + try { + const phys::vehicle_desc desc = phys::plant_host::preset(opt.vehicle); + const nav::coef_mlp policy = + opt.policy.empty() ? nav::coef_mlp::default_biased() : nav::coef_mlp::load(opt.policy); + const int steps_per_tick = static_cast(std::lround(opt.dt / 0.01)); + if (steps_per_tick < 1 || std::fabs(steps_per_tick * 0.01 - opt.dt) > 1e-9) + throw std::invalid_argument("--dt must be a whole number of 0.01 s physics steps"); + const int nsub = opt.nsub > 0 ? opt.nsub : steps_per_tick; + const int ticks = static_cast(std::lround(opt.seconds / opt.dt)); + + // The city bundle is loaded once and shared by every seed. + std::unique_ptr city; + nav::world_planes planes; + if (opt.scene == "bundle") { + city.reset(new cvc::world::city_bundle(cvc::world::load_city_bundle(opt.bundle))); + if (!city->buildings_loaded) + std::fprintf(stderr, "cvc-plant-log: no buildings (%s); terrain only\n", + city->buildings_error.c_str()); + planes = nav::rasterize(city->geometry, opt.grid, opt.grid, opt.inflate); + } + + std::vector> runs; + std::vector run_seed; + std::vector run_agent; + for (const unsigned seed : opt.seeds) { + std::mt19937 rng(seed); + run_setup s; + s.host.plant.plan_with_vehicle = true; + s.host.ground_mu = opt.mu; + const int n = opt.agents; + if (opt.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, opt.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 (opt.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, opt.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, opt.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); + phys::plant_host host(s.host); + if (s.geometry) + host.set_geometry(s.geometry); + host.use(sim, opt.vehicle); + phys::nav_plant &plant = *host.plant(); + plant.set_lateral_g(opt.lateral_g); + phys::plant_log log(sim, plant); + + std::vector pos(2 * n), gw(2 * n), hd(n); + std::vector reached(n); + // Vehicle routes (city, bundle): the plant's footprint and turning circle, in world units. + const bool routed = opt.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; a few tries. + const auto plan_for = [&](int i) { + std::uniform_int_distribution pk(0, s.free_cells.size() - 1); + for (int attempt = 0; attempt < 8; ++attempt) { + const int cell = s.free_cells[pk(rng)]; + const int rr = cell / s.cfg.cols, kk = cell % s.cfg.cols; + const double gx = s.cfg.min_x + kk * span / (s.cfg.cols - 1); + const double gy = s.cfg.min_y + rr * (s.cfg.max_y - s.cfg.min_y) / (s.cfg.rows - 1); + const double dist = std::hypot(gx - pos[2 * i], gy - pos[2 * i + 1]); + if (dist < 0.05 * span || dist > 0.15 * span) + continue; + nav::route_options ro; + 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; + } + } + ++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); + const int settle = static_cast(std::lround(0.5 / opt.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 (opt.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) { + 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(); + } + for (int i = 0; i < n; ++i) { + runs.push_back(log.runs()[i]); + run_seed.push_back(seed); + run_agent.push_back(i); + } + std::size_t moving = 0; + for (int i = 0; i < n; ++i) + for (const phys::planar_sample &q : log.runs()[i]) + moving += std::fabs(q.vx) > 0.5 ? 1 : 0; + std::printf("seed %u: %d agents, %d ticks, vmax %.1f m/s, moving %.0f%% of the time, %zu " + "routes (%zu unplanned), %.0f us a tick\n", + seed, n, ticks, s.vmax_mps, + 100.0 * moving / (static_cast(n) * (ticks + 1)), planned, unplanned, + plant.advance_us()); + host.use(sim, "kinematic"); // detach before the host and sim go + } + + // 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 phys::planar_sample &q : run) { + ++down[q.wheels - q.wheels_down]; + ++total; + } + std::printf("wheels off the ground:"); + for (const auto &kv : down) + std::printf(" %d: %.1f%%", kv.first, 100.0 * kv.second / total); + std::printf("\n"); + } + + // The flights. + const std::size_t rows = static_cast(std::lround(opt.flight_seconds / opt.dt)) + 1; + const std::size_t stride = std::max( + 1, static_cast(std::lround(opt.stride_seconds / opt.dt))); + const phys::flight_set f = phys::cut_flights(runs, rows, stride, 1.0, opt.steer); + std::filesystem::create_directories(opt.out); + phys::write_npy((std::filesystem::path(opt.out) / "flights.npy").string(), f.data, + {f.flights, f.rows, static_cast(phys::kPlanarWidth)}); + + // The residuals. + const phys::plant_residuals res = phys::one_tick_residuals(runs, opt.dt, desc, opt.mu, nsub); + const auto print_bins = [](const char *name, const std::vector &bins, + bool dyn) { + std::printf(" %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 phys::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); + std::printf(" %-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) + std::printf(" %10.4f %10.4f", b.lateral_rms_mps, b.yaw_rate_rms); + std::printf("\n"); + } + }; + std::printf("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", + opt.vehicle.c_str(), opt.dt, nsub, opt.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); + std::printf("flights: %zu x %zu rows (%.1f s at %.0f Hz), %zu windows dropped, peak %.2f g\n", + f.flights, f.rows, (f.rows - 1) * opt.dt, 1.0 / opt.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 phys::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(opt.out) / "residual.json"); + js << "{\n \"format\": \"cvc-plant-residual-1\",\n \"vehicle\": \"" << opt.vehicle + << "\", \"scene\": \"" << opt.scene << "\", \"dt\": " << num(opt.dt) + << ", \"nsub\": " << nsub << ", \"mu\": " << num(opt.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) << "\n}\n"; + } + { + std::ofstream js(std::filesystem::path(opt.out) / "meta.json"); + js << "{\n \"format\": \"cvc-plant-log-1\",\n \"vehicle\": \"" << opt.vehicle + << "\", \"scene\": \"" << opt.scene << "\", \"bundle\": \"" << opt.bundle + << "\",\n \"dt\": " << num(opt.dt) << ", \"rate_hz\": " << num(1.0 / opt.dt) + << ", \"physics_dt\": 0.01, \"mu\": " << num(opt.mu) + << ", \"lateral_g\": " << num(opt.lateral_g) << ", \"seconds\": " << num(opt.seconds) + << ",\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 = " + << (opt.steer == phys::steer_input::actual ? "the road-wheel angle's mean over the tick" + : "nav's steering command (held)") + << "\",\n \"shape\": [" << f.flights << ", " << f.rows << ", " << phys::kPlanarWidth + << "], \"dropped\": " << f.dropped << ", \"peak_lateral_g\": " << num(f.peak_lateral_g) + << ",\n \"seeds\": ["; + for (std::size_t k = 0; k < opt.seeds.size(); ++k) + js << (k ? ", " : "") << opt.seeds[k]; + js << "],\n \"flight_seed\": ["; + for (std::size_t k = 0; k < f.flights; ++k) + js << (k ? ", " : "") << run_seed[f.agent[k]]; + js << "],\n \"flight_agent\": ["; + for (std::size_t k = 0; k < f.flights; ++k) + js << (k ? ", " : "") << 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"; + } + std::printf("wrote %s/{flights.npy, meta.json, residual.json}\n", opt.out.c_str()); + } catch (const std::exception &e) { + std::fprintf(stderr, "cvc-plant-log: %s\n", e.what()); + return 1; + } + return 0; +} diff --git a/src/cvc/tools/liesphgp_pack.py b/src/cvc/tools/liesphgp_pack.py new file mode 100644 index 000000000..71aabc06f --- /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 (src/cvc/tools/cvc_plant_log_main.cpp) 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()) From c80a36b884d6451441f3cacc13b2302e1bd78a4c Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 15:31:39 -0500 Subject: [PATCH 02/12] physics_plant_log_test: a model's own run leaves it no residual to rounding (1e-9) The run is made in the test and replayed in plant_log.cpp; a compiler may contract the two into FMAs differently (aarch64), so compare to rounding rather than to 1e-12. Include . --- src/cvc/tests/physics_plant_log_test.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/cvc/tests/physics_plant_log_test.cpp b/src/cvc/tests/physics_plant_log_test.cpp index f11f4f5e9..c23d64519 100644 --- a/src/cvc/tests/physics_plant_log_test.cpp +++ b/src/cvc/tests/physics_plant_log_test.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -121,6 +122,8 @@ double total_ticks(const std::vector &bins) { } // 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}) { @@ -129,18 +132,18 @@ TEST(PlantLog, ARunAModelMadeLeavesThatModelNoResidual) { 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-12); - EXPECT_LT(b.speed_max_mps, 1e-12); + 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-12); - EXPECT_LT(b.speed_max_mps, 1e-12); - EXPECT_LT(b.lateral_rms_mps, 1e-12); - EXPECT_LT(b.yaw_rate_rms, 1e-12); + 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. From 92af386c96e23eaec8ce9aeb9488e4fd04f4a805 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 16:59:15 -0500 Subject: [PATCH 03/12] plant_log: fit dyn_bike to the plant; city-bundle routes that get driven; gentler margins MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - fit_dyn_bike / dyn_bike_cost: dyn_bike_si_of's knobs a spec sheet does not give, fitted to logged runs by coordinate descent on the one-tick twist residual (drive and steering lags, factors on the tires' B, the yaw inertia, and drag + rolling resistance). dyn_bike_si_of is unchanged; this measures what the plant wants. one_tick_residuals takes a dyn_bike_si too. cvc-plant-log --fit-dyn fits on every seed but the last and reports the last. - Bundle routes: the goal is drawn from the free cells 5-15% of the map away. Drawing from the whole map landed in that ring ~6% of the time, which was the "planner fails on ~60% of goals". The planner keeps 2 m off building meshes, and an agent stopped for 5 s is replanned from where it stands. - --accel-margin / --brake-margin (nav_plant's), recorded in meta.json: at the full ±6 m/s^2 the RoboRacer's low chassis box pitches onto the ground under braking and acceleration. - --verbose (each agent every 5 s) and --samples (samples.csv); samples carry the roll. physics_plant_log_test: + FitsTheDynamicModelToRunsOfAKnownVehicle (a run made by a vehicle off the spec sheet: every knob recovered on its grid, cost 0). --- inc/cvc/physics/plant_log.h | 37 +++- src/cvc/physics/plant_log.cpp | 211 ++++++++++++++++++----- src/cvc/tests/physics_plant_log_test.cpp | 35 +++- src/cvc/tools/cvc_plant_log_main.cpp | 172 ++++++++++++++++-- 4 files changed, 396 insertions(+), 59 deletions(-) diff --git a/inc/cvc/physics/plant_log.h b/inc/cvc/physics/plant_log.h index 1d3c96c52..79c4c7235 100644 --- a/inc/cvc/physics/plant_log.h +++ b/inc/cvc/physics/plant_log.h @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -57,7 +58,8 @@ struct planar_sample { 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 + 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 @@ -122,6 +124,39 @@ plant_residuals one_tick_residuals(const std::vector> 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. diff --git a/src/cvc/physics/plant_log.cpp b/src/cvc/physics/plant_log.cpp index b45141f76..674cef43b 100644 --- a/src/cvc/physics/plant_log.cpp +++ b/src/cvc/physics/plant_log.cpp @@ -131,6 +131,7 @@ planar_sample plant_log::sample(int i) const { 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; @@ -175,25 +176,90 @@ void plant_log::record() { } } -plant_residuals one_tick_residuals(const std::vector> &runs, double dt, - const vehicle_desc &d, double mu, int nsub, - const std::vector &bins_g) { +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"); - cvc::world_units::normalized_frame si; - si.scale = 1.0; - const cvc::nav::diff::dyn_bike_params p = - cvc::nav::dyn_bike_params_of(nav_plant::dyn_bike_si_of(d, mu), si); +} + +} // 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{}; - lim.L = p.lf + p.lr; - lim.delta_max = static_cast(d.max_steer); - lim.hdt = static_cast(h); - lim.allow_reverse = 1; - const double L = d.wheelbase(); + 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; @@ -201,26 +267,9 @@ plant_residuals one_tick_residuals(const std::vector> 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]; - // Both models keep the speed within the plan's, as sim_world's reference does. - const double vmax = s0.vmax > 0.0 ? s0.vmax : 1e3; - lim.vmax = static_cast(vmax); - // The dynamic model, from the plant's state at the tick's start. - 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; + 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); @@ -228,21 +277,21 @@ plant_residuals one_tick_residuals(const std::vector> kx = kx + h * ksp * std::cos(kth); ky = ky + h * ksp * std::sin(kth); } - if (s1.contacts > 0) { + switch (why_skipped(s0, s1)) { + case contact: ++out.skipped_contact; continue; - } - if (!s0.grounded || !s1.grounded) { + case airborne: ++out.skipped_airborne; continue; - } - if (std::hypot(s1.a_lat, s1.a_lon) > kImpactG * kG) { + case impact: ++out.skipped_impact; continue; - } - if (std::fabs(s0.vx) < 0.2 && std::fabs(s1.vx) < 0.2) { + case rest: ++out.skipped_rest; continue; + case kept: + break; } const double g = std::fabs(s1.a_lat) / kG; std::size_t b = 0; @@ -263,6 +312,90 @@ plant_residuals one_tick_residuals(const std::vector> 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) diff --git a/src/cvc/tests/physics_plant_log_test.cpp b/src/cvc/tests/physics_plant_log_test.cpp index c23d64519..6480f3da0 100644 --- a/src/cvc/tests/physics_plant_log_test.cpp +++ b/src/cvc/tests/physics_plant_log_test.cpp @@ -72,14 +72,14 @@ std::vector kinematic_run(const vehicle_desc &d, double dt, int n return run; } -// A run produced by diff::dyn_plant itself (the vehicle from dyn_bike_si_of), from rest in its -// actuator, as one_tick_residuals steps it. +// 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) { + 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(nav_plant::dyn_bike_si_of(d, mu), si); + 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; @@ -168,6 +168,33 @@ TEST(PlantLog, ARunAModelMadeLeavesThatModelNoResidual) { 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), diff --git a/src/cvc/tools/cvc_plant_log_main.cpp b/src/cvc/tools/cvc_plant_log_main.cpp index 07d76d8be..f129ca4eb 100644 --- a/src/cvc/tools/cvc_plant_log_main.cpp +++ b/src/cvc/tools/cvc_plant_log_main.cpp @@ -38,6 +38,10 @@ // 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 printed). +// 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 (cvc::physics::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. #include @@ -88,6 +92,10 @@ struct options { int nsub = 0; // residual substeps; 0 = the physics steps a tick int grid = 600; int inflate = 0; + bool verbose = false; + bool samples = false; + bool fit_dyn = false; + double accel_margin = -1.0, brake_margin = -1.0; // the plant's; < 0 keeps its defaults phys::steer_input steer = phys::steer_input::actual; }; @@ -97,7 +105,8 @@ void usage() { " [--bundle DIR] [--seeds 100,101,...] [--agents N] [--seconds S] [--dt S]\n" " [--mu MU] [--lateral-g G] [--flight-seconds S] [--stride-seconds S]\n" " [--steer actual|command] [--nsub N] [--grid N] [--inflate CELLS]\n" - " [--policy FILE.cvcnav]\n"); + " [--accel-margin SHARE] [--brake-margin SHARE] [--policy FILE.cvcnav]\n" + " [--verbose] [--samples] [--fit-dyn]\n"); } options parse(int argc, char **argv) { @@ -145,6 +154,16 @@ options parse(int argc, char **argv) { o.grid = std::stoi(val()); else if (a == "--inflate") o.inflate = std::stoi(val()); + else if (a == "--verbose") + o.verbose = true; + else if (a == "--samples") + o.samples = true; + else if (a == "--fit-dyn") + o.fit_dyn = true; + else if (a == "--accel-margin") + o.accel_margin = std::stod(val()); + else if (a == "--brake-margin") + o.brake_margin = std::stod(val()); else if (a == "--steer") { const std::string s = val(); if (s == "actual") @@ -283,6 +302,7 @@ int main(int argc, char **argv) { } std::vector> runs; + double eff_accel_margin = 0.0, eff_brake_margin = 0.0; // what the plant planned with std::vector run_seed; std::vector run_agent; for (const unsigned seed : opt.seeds) { @@ -385,6 +405,15 @@ int main(int argc, char **argv) { host.use(sim, opt.vehicle); phys::nav_plant &plant = *host.plant(); plant.set_lateral_g(opt.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 (opt.accel_margin > 0.0) + plant.set_accel_margin(opt.accel_margin); + if (opt.brake_margin > 0.0) + plant.set_brake_margin(opt.brake_margin); + eff_accel_margin = plant.options().accel_margin; + eff_brake_margin = plant.options().brake_margin; phys::plant_log log(sim, plant); std::vector pos(2 * n), gw(2 * n), hd(n); @@ -412,18 +441,35 @@ int main(int argc, char **argv) { } 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; a few tries. + // 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) { - std::uniform_int_distribution pk(0, s.free_cells.size() - 1); - for (int attempt = 0; attempt < 8; ++attempt) { - const int cell = s.free_cells[pk(rng)]; - const int rr = cell / s.cfg.cols, kk = cell % s.cfg.cols; - const double gx = s.cfg.min_x + kk * span / (s.cfg.cols - 1); - const double gy = s.cfg.min_y + rr * (s.cfg.max_y - s.cfg.min_y) / (s.cfg.rows - 1); + 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) - continue; + 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); @@ -432,12 +478,18 @@ int main(int argc, char **argv) { ++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 / opt.dt)); const int settle = static_cast(std::lround(0.5 / opt.dt)); std::uniform_int_distribution pick( 0, s.free_cells.empty() ? 0 : s.free_cells.size() - 1); @@ -462,6 +514,13 @@ int main(int argc, char **argv) { 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]) { @@ -475,6 +534,19 @@ int main(int argc, char **argv) { sim.step(); if (t >= 0) log.record(); + // --verbose: every agent's state every 5 s. + if (opt.verbose && t > 0 && t % static_cast(std::lround(5.0 / opt.dt)) == 0) + for (int i = 0; i < n; ++i) { + const phys::planar_sample &q = log.runs()[i].back(); + const phys::vehicle_state vs = plant.physics_world().vehicle_state(plant.vehicle(i)); + std::printf(" 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 * opt.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) { runs.push_back(log.runs()[i]); @@ -485,11 +557,14 @@ int main(int argc, char **argv) { for (int i = 0; i < n; ++i) for (const phys::planar_sample &q : log.runs()[i]) moving += std::fabs(q.vx) > 0.5 ? 1 : 0; - std::printf("seed %u: %d agents, %d ticks, vmax %.1f m/s, moving %.0f%% of the time, %zu " - "routes (%zu unplanned), %.0f us a tick\n", + std::printf("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)), planned, unplanned, - plant.advance_us()); + 100.0 * moving / (static_cast(n) * (ticks + 1)), plant.advance_us()); + if (routed) + std::printf(" 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 } @@ -517,6 +592,24 @@ int main(int argc, char **argv) { phys::write_npy((std::filesystem::path(opt.out) / "flights.npy").string(), f.data, {f.flights, f.rows, static_cast(phys::kPlanarWidth)}); + // --samples: every sample of every run, for analysis (samples.csv). + if (opt.samples) { + std::ofstream cs(std::filesystem::path(opt.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 phys::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", + run_seed[k], 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 phys::plant_residuals res = phys::one_tick_residuals(runs, opt.dt, desc, opt.mu, nsub); const auto print_bins = [](const char *name, const std::vector &bins, @@ -549,6 +642,40 @@ int main(int argc, char **argv) { 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. + phys::dyn_bike_fit fit; + phys::plant_residuals res_spec, res_fit; + double held_default = 0.0, held_fit = 0.0; + const unsigned held = opt.seeds.back(); + if (opt.fit_dyn) { + std::vector> fit_runs, test_runs; + for (std::size_t k = 0; k < runs.size(); ++k) + (opt.seeds.size() > 1 && 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 = phys::fit_dyn_bike(fit_runs, opt.dt, desc, opt.mu, nsub); + const cvc::nav::dyn_bike_si spec = phys::nav_plant::dyn_bike_si_of(desc, opt.mu); + held_default = phys::dyn_bike_cost(test_runs, opt.dt, spec, desc.max_steer, nsub); + held_fit = phys::dyn_bike_cost(test_runs, opt.dt, fit.si, desc.max_steer, nsub); + std::printf("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", + opt.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 = + phys::one_tick_residuals(test_runs, opt.dt, spec, desc.wheelbase(), desc.max_steer, nsub); + res_fit = phys::one_tick_residuals(test_runs, opt.dt, fit.si, desc.wheelbase(), + desc.max_steer, nsub); + std::printf("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); + } std::printf("flights: %zu x %zu rows (%.1f s at %.0f Hz), %zu windows dropped, peak %.2f g\n", f.flights, f.rows, (f.rows - 1) * opt.dt, 1.0 / opt.dt, f.dropped, f.peak_lateral_g); @@ -586,7 +713,20 @@ int main(int argc, char **argv) { << ",\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) << "\n}\n"; + << ",\n \"kinematic\": " << bins_json(res.kinematic); + if (opt.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(opt.out) / "meta.json"); @@ -595,6 +735,8 @@ int main(int argc, char **argv) { << "\",\n \"dt\": " << num(opt.dt) << ", \"rate_hz\": " << num(1.0 / opt.dt) << ", \"physics_dt\": 0.01, \"mu\": " << num(opt.mu) << ", \"lateral_g\": " << num(opt.lateral_g) << ", \"seconds\": " << num(opt.seconds) + << ", \"accel_margin\": " << num(eff_accel_margin) + << ", \"brake_margin\": " << num(eff_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\"]," From 077f8304e734b003079097c16a6dc54aeaac07a3 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 17:08:12 -0500 Subject: [PATCH 04/12] roadmap: dyn_plant's residual against Jolt, refreshed, and dyn_bike fitted to Jolt The RoboRacer's city column is from runs planning with 0.4 of its acceleration and braking (its chassis box scraped the ground at the full 6 m/s^2). The fit (fit_dyn_bike, held-out seed) puts both vehicles' tires 2.5-4x softer than the spec sheet's B, and the RoboRacer with no drive lag and no uncompensated resistance. dyn_bike_si_of's defaults are unchanged. --- docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md | 52 +++++++++++++++------ 1 file changed, 39 insertions(+), 13 deletions(-) diff --git a/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md b/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md index 10ed3a816..17c6e7781 100644 --- a/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md +++ b/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md @@ -162,22 +162,48 @@ Ticks are left out when they have a static contact, a wheel off the ground, an i 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, dynamic / kinematic. +is the position rms in mm, spec-sheet dynamic / kinematic. 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 / 3.9 | 2.0 / 3.4 | 0.9 / 0.7 | 2.5 / 1.9 | -| 0.1-0.2 | 5.1 / 11.6 | 4.8 / 11.1 | 3.3 / 5.6 | 4.3 / 4.6 | -| 0.2-0.3 | 7.1 / 17.9 | 7.0 / 18.1 | 5.0 / 10.7 | 5.8 / 9.4 | -| 0.3-0.5 | 8.1 / 27.5 | 8.1 / 26.2 | 6.9 / 15.8 | 7.1 / 13.8 | -| >= 0.5 | 5.2 / 94.0 | 4.5 / 52.0 | 6.5 / 25.1 | 4.8 / 19.0 | - -- **Position and heading:** the dynamic model is closer than the kinematic bicycle wherever the tires - carry load. Above 0.5 g it is closer by 4-18x. -- **Speed:** for the RoboRacer the dynamic model is worse, with 0.02-0.12 m/s rms against - 0.007-0.06. Its 0.1 s drive lag (`dyn_bike_si_of`'s `drive_lag`) is slower than the vehicle's - driver model. -- **Yaw rate:** in the city it is off by 0.1-0.3 rad/s rms. +| 0.0-0.1 | 2.0 / 3.9 | 2.0 / 3.4 | 0.9 / 0.7 | 1.9 / 1.9 | +| 0.1-0.2 | 5.1 / 11.6 | 4.8 / 11.1 | 3.3 / 5.6 | 3.4 / 5.2 | +| 0.2-0.3 | 7.1 / 17.9 | 7.0 / 18.1 | 5.0 / 10.7 | 4.8 / 8.6 | +| 0.3-0.5 | 8.1 / 27.5 | 8.1 / 26.2 | 6.9 / 15.8 | 6.5 / 11.7 | +| >= 0.5 | 5.2 / 94.0 | 4.5 / 52.0 | 6.5 / 25.1 | 12.6 / 30.4 | + +- **Position and heading:** with `dyn_bike_si_of`'s spec-sheet values, the dynamic model is closer than + the kinematic bicycle wherever the tires carry load. +- **Speed:** for the RoboRacer it is worse, because two of its spec-sheet values are off: + - its 0.1 s drive lag; + - the drag and rolling resistance it subtracts. The RoboRacer's driver model tracks the commanded + acceleration with an integral trim, so it makes up for them. + +**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. Each fit uses seeds 100-102 +and is judged on seed 103. + +| run (50 Hz) | drive lag | tire B front / rear | yaw inertia | drag + rolling | held-out twist residual, (m/s)^2 | +|---|---|---|---|---|---| +| Humvee slalom | 0.15 s | x0.40 / x0.30 | x1.00 | x1 | 0.00287 -> 0.00045 | +| Humvee city | 0.02 s | x0.40 / x0.30 | x1.20 | x1 | 0.00280 -> 0.00060 | +| RoboRacer slalom | 0 | x0.30 / x0.25 | x0.85 | x0 | 0.00035 -> 0.00001 | +| RoboRacer city | 0 | x0.30 / x0.30 | x1.75 | x0 | 0.00229 -> 0.00018 | + +- **Tires:** both vehicles' Pacejka B from the spec sheet is 2.5-4x stiffer than Jolt's tires behave. +- **RoboRacer drive:** it has no drive lag and no uncompensated resistance. +- **Weakly identified:** the Humvee's drive lag 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.1-0.2, 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:** the fitted model's position residual against the spec sheet's + and the kinematic bicycle's, in mm: + - Humvee slalom, above 0.5 g: 3.0 / 6.4 / 55.2; + - RoboRacer city, 0.3-0.5 g: 1.6 / 6.1 / 11.2. + + The RoboRacer's speed residual comes down to the kinematic bicycle's. +- **The defaults are unchanged:** `dyn_bike_si_of` keeps the spec-sheet values, so P8b and its goldens + do not move. Whether to adopt fitted values is open. These are the targets a plant fitted to Jolt has to beat, bin by bin. From c4d549922f37bfc6b1279e9674c619c4e714dd99 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 18:52:51 -0500 Subject: [PATCH 05/12] plant_log test: close the .npy before removing it, as Windows refuses to delete an open file --- src/cvc/tests/physics_plant_log_test.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/cvc/tests/physics_plant_log_test.cpp b/src/cvc/tests/physics_plant_log_test.cpp index 6480f3da0..815e22df9 100644 --- a/src/cvc/tests/physics_plant_log_test.cpp +++ b/src/cvc/tests/physics_plant_log_test.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include using namespace cvc::physics; @@ -223,8 +224,14 @@ TEST(PlantLog, CutsFlightsAndWritesThemAsNpy) { 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::ifstream in(path, std::ios::binary); - std::vector bytes((std::istreambuf_iterator(in)), std::istreambuf_iterator()); + 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]) | @@ -235,7 +242,6 @@ TEST(PlantLog, CutsFlightsAndWritesThemAsNpy) { EXPECT_NE(header.find("'shape': (4, 26, 8)"), std::string::npos) << header; ASSERT_EQ(bytes.size(), 10 + hl + f.data.size() * sizeof(double)); EXPECT_EQ(std::memcmp(bytes.data() + 10 + hl, f.data.data(), f.data.size() * 8), 0); - std::filesystem::remove(path); } // A Humvee slaloming on flat ground through nav and Jolt, logged at the tick. From b59efdf387060ff3e94aed99c559f9ffe619b3f2 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 21:59:24 -0500 Subject: [PATCH 06/12] plant_log: the scenes in the library, `cvc plant-log` in cvc-cli The scene drivers and the directory writer move out of the standalone cvc-plant-log tool into cvc/physics/plant_log_scene.h (plant_log_options, drive_plant_log_scene, write_plant_log), so a trainer can drive the scenes in process. The command line becomes the `cvc plant-log` subcommand of cvc-cli, and the tool and its CVC_BUILD_PLANT_LOG option are gone. The three scenes run through `cvc plant-log` write files byte-identical to the old tool's (flights.npy, meta.json, residual.json, samples.csv), and the same stdout but for the timing. physics_plant_log_test adds the options' defaults and a swerve driven twice to identical runs; cvc_cli_test covers the subcommand. --- docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md | 4 +- inc/cvc/physics/plant_log_scene.h | 111 +++ src/cvc-cli/cvc_cli_test.cpp | 34 + src/cvc-cli/main.cpp | 85 +++ src/cvc/CMakeLists.txt | 12 +- src/cvc/physics/plant_log_scene.cpp | 631 ++++++++++++++++ src/cvc/tests/physics_plant_log_test.cpp | 75 ++ src/cvc/tools/cvc_plant_log_main.cpp | 772 -------------------- src/cvc/tools/liesphgp_pack.py | 6 +- 9 files changed, 943 insertions(+), 787 deletions(-) create mode 100644 inc/cvc/physics/plant_log_scene.h create mode 100644 src/cvc/physics/plant_log_scene.cpp delete mode 100644 src/cvc/tools/cvc_plant_log_main.cpp diff --git a/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md b/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md index 17c6e7781..7f133cf2b 100644 --- a/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md +++ b/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md @@ -159,7 +159,7 @@ are compared with where Jolt was at the tick's end. The bins are the lateral acc 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. +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, spec-sheet dynamic / kinematic. The RoboRacer's city runs plan with 0.4 @@ -180,7 +180,7 @@ of its acceleration and braking: at its full 6 m/s^2 its chassis box pitches ont - the drag and rolling resistance it subtracts. The RoboRacer's driver model tracks the commanded acceleration with an integral trim, so it makes up for them. -**Fitted to Jolt** (`cvc::physics::fit_dyn_bike`, `cvc-plant-log --fit-dyn`): a coordinate descent on +**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. Each fit uses seeds 100-102 and is judged on seed 103. 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 f2a6729bf..3540a81aa 100644 --- a/src/cvc/CMakeLists.txt +++ b/src/cvc/CMakeLists.txt @@ -71,6 +71,7 @@ set(INCLUDE_FILES ../../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 @@ -262,6 +263,7 @@ set(SOURCE_FILES 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 @@ -2237,16 +2239,6 @@ if(CVC_BUILD_LSYS_TOOLS) endif() endif() -# cvc-plant-log: nav driving physics vehicles, logged as planar rigid bodies for fitting a plant -# model, with the dynamic single-track model's residual against them (cvc/physics/plant_log.h). -# Links only cvc::cvc; it needs the Jolt backend at run time. -option(CVC_BUILD_PLANT_LOG "Build the cvc-plant-log tool" ON) -if(CVC_BUILD_PLANT_LOG AND NOT EMSCRIPTEN) - add_executable(cvc-plant-log ${CMAKE_CURRENT_SOURCE_DIR}/tools/cvc_plant_log_main.cpp) - target_link_libraries(cvc-plant-log PRIVATE cvc) - target_compile_features(cvc-plant-log PRIVATE cxx_std_17) -endif() - # The Ariadne conference RELAY moved to src/arisrv (the `arisrv` target, wired from # src/CMakeLists.txt when CVC_ENABLE_GRPC or CVC_ENABLE_WS is on) — it gained a WebSocket # control plane + a WS<->gRPC bridge, so it is no longer gRPC-only. 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/physics_plant_log_test.cpp b/src/cvc/tests/physics_plant_log_test.cpp index 815e22df9..49dda1259 100644 --- a/src/cvc/tests/physics_plant_log_test.cpp +++ b/src/cvc/tests/physics_plant_log_test.cpp @@ -29,12 +29,15 @@ #include #include #include +#include #include #include #include +#include #include #include #include +#include #include #include #include @@ -368,3 +371,75 @@ TEST(PlantLog, GroundHeightPutsAStartOnTheGroundUnderIt) { 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/cvc_plant_log_main.cpp b/src/cvc/tools/cvc_plant_log_main.cpp deleted file mode 100644 index f129ca4eb..000000000 --- a/src/cvc/tools/cvc_plant_log_main.cpp +++ /dev/null @@ -1,772 +0,0 @@ -/* - 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-plant-log: nav driving physics vehicles (cvc::physics::nav_plant on Jolt), logged as planar -// rigid bodies for fitting a plant model, and the dynamic single-track model's one-tick residual -// against them (cvc/physics/plant_log.h). -// -// cvc-plant-log --vehicle humvee --scene swerve --seeds 100,101,102 --out runs/humvee-swerve -// cvc-plant-log --vehicle roboracer --scene city --agents 8 --out runs/roboracer-city -// cvc-plant-log --vehicle humvee --scene bundle --bundle --out runs/humvee-city -// -// 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. -// -// Out (--out DIR): -// flights.npy float64 (F, T, 8): equal-length flights cut from the runs, rows -// [x, y, psi, vx, vy, r, drive, steer] (cvc::physics::kPlanarWidth): the centre -// of mass on SE(2), its body twist, nav's acceleration command and the steering -// (--steer actual|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 printed). -// 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 (cvc::physics::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. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace { - -namespace phys = cvc::physics; -namespace nav = cvc::nav; - -struct options { - std::string vehicle = "humvee"; - std::string scene = "swerve"; - std::string bundle; - std::string policy; - std::string out; - std::vector seeds = {100, 101, 102, 103}; - int agents = -1; // per scene default - double seconds = 60.0; - double dt = 0.02; - double mu = 0.9; - double lateral_g = -1.0; // per scene default - double flight_seconds = 5.0; - double stride_seconds = 5.0; - int nsub = 0; // residual substeps; 0 = the physics steps a tick - int grid = 600; - int inflate = 0; - bool verbose = false; - bool samples = false; - bool fit_dyn = false; - double accel_margin = -1.0, brake_margin = -1.0; // the plant's; < 0 keeps its defaults - phys::steer_input steer = phys::steer_input::actual; -}; - -void usage() { - std::printf( - "cvc-plant-log --out DIR [--vehicle humvee|roboracer] [--scene swerve|city|bundle]\n" - " [--bundle DIR] [--seeds 100,101,...] [--agents N] [--seconds S] [--dt S]\n" - " [--mu MU] [--lateral-g G] [--flight-seconds S] [--stride-seconds S]\n" - " [--steer actual|command] [--nsub N] [--grid N] [--inflate CELLS]\n" - " [--accel-margin SHARE] [--brake-margin SHARE] [--policy FILE.cvcnav]\n" - " [--verbose] [--samples] [--fit-dyn]\n"); -} - -options parse(int argc, char **argv) { - options o; - for (int i = 1; i < argc; ++i) { - const std::string a = argv[i]; - const auto val = [&]() -> std::string { - if (i + 1 >= argc) - throw std::invalid_argument(a + " needs a value"); - return argv[++i]; - }; - if (a == "--vehicle") - o.vehicle = val(); - else if (a == "--scene") - o.scene = val(); - else if (a == "--bundle") - o.bundle = val(); - else if (a == "--policy") - o.policy = val(); - else if (a == "--out") - o.out = val(); - else if (a == "--seeds") { - o.seeds.clear(); - std::stringstream ss(val()); - std::string t; - while (std::getline(ss, t, ',')) - o.seeds.push_back(static_cast(std::stoul(t))); - } else if (a == "--agents") - o.agents = std::stoi(val()); - else if (a == "--seconds") - o.seconds = std::stod(val()); - else if (a == "--dt") - o.dt = std::stod(val()); - else if (a == "--mu") - o.mu = std::stod(val()); - else if (a == "--lateral-g") - o.lateral_g = std::stod(val()); - else if (a == "--flight-seconds") - o.flight_seconds = std::stod(val()); - else if (a == "--stride-seconds") - o.stride_seconds = std::stod(val()); - else if (a == "--nsub") - o.nsub = std::stoi(val()); - else if (a == "--grid") - o.grid = std::stoi(val()); - else if (a == "--inflate") - o.inflate = std::stoi(val()); - else if (a == "--verbose") - o.verbose = true; - else if (a == "--samples") - o.samples = true; - else if (a == "--fit-dyn") - o.fit_dyn = true; - else if (a == "--accel-margin") - o.accel_margin = std::stod(val()); - else if (a == "--brake-margin") - o.brake_margin = std::stod(val()); - else if (a == "--steer") { - const std::string s = val(); - if (s == "actual") - o.steer = phys::steer_input::actual; - else if (s == "command") - o.steer = phys::steer_input::command; - else - throw std::invalid_argument("--steer is actual or command"); - } else if (a == "-h" || a == "--help") { - usage(); - std::exit(0); - } else - throw std::invalid_argument("unknown option " + a); - } - if (o.out.empty()) - throw std::invalid_argument("--out is required"); - if (o.scene != "swerve" && o.scene != "city" && o.scene != "bundle") - throw std::invalid_argument("--scene is swerve, city or bundle"); - if (o.scene == "bundle" && o.bundle.empty()) - throw std::invalid_argument("--scene bundle needs --bundle DIR"); - if (o.agents < 0) - o.agents = o.scene == "swerve" ? 4 : 8; - if (o.lateral_g < 0.0) - o.lateral_g = o.scene == "swerve" ? 0.9 : 0.5; - return o; -} - -// 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 - phys::plant_host_options host; // the physics world and the plant's options - const cvc::world::world_geometry *geometry = nullptr; - std::vector height; // the terrain's heights on the grid (bundle) - 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 - -int main(int argc, char **argv) { - options opt; - try { - opt = parse(argc, argv); - } catch (const std::exception &e) { - std::fprintf(stderr, "cvc-plant-log: %s\n", e.what()); - usage(); - return 2; - } - if (!phys::plant_host::available()) { - std::fprintf(stderr, "cvc-plant-log: this libcvc was built without Jolt\n"); - return 1; - } - try { - const phys::vehicle_desc desc = phys::plant_host::preset(opt.vehicle); - const nav::coef_mlp policy = - opt.policy.empty() ? nav::coef_mlp::default_biased() : nav::coef_mlp::load(opt.policy); - const int steps_per_tick = static_cast(std::lround(opt.dt / 0.01)); - if (steps_per_tick < 1 || std::fabs(steps_per_tick * 0.01 - opt.dt) > 1e-9) - throw std::invalid_argument("--dt must be a whole number of 0.01 s physics steps"); - const int nsub = opt.nsub > 0 ? opt.nsub : steps_per_tick; - const int ticks = static_cast(std::lround(opt.seconds / opt.dt)); - - // The city bundle is loaded once and shared by every seed. - std::unique_ptr city; - nav::world_planes planes; - if (opt.scene == "bundle") { - city.reset(new cvc::world::city_bundle(cvc::world::load_city_bundle(opt.bundle))); - if (!city->buildings_loaded) - std::fprintf(stderr, "cvc-plant-log: no buildings (%s); terrain only\n", - city->buildings_error.c_str()); - planes = nav::rasterize(city->geometry, opt.grid, opt.grid, opt.inflate); - } - - std::vector> runs; - double eff_accel_margin = 0.0, eff_brake_margin = 0.0; // what the plant planned with - std::vector run_seed; - std::vector run_agent; - for (const unsigned seed : opt.seeds) { - std::mt19937 rng(seed); - run_setup s; - s.host.plant.plan_with_vehicle = true; - s.host.ground_mu = opt.mu; - const int n = opt.agents; - if (opt.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, opt.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 (opt.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, opt.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, opt.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); - phys::plant_host host(s.host); - if (s.geometry) - host.set_geometry(s.geometry); - host.use(sim, opt.vehicle); - phys::nav_plant &plant = *host.plant(); - plant.set_lateral_g(opt.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 (opt.accel_margin > 0.0) - plant.set_accel_margin(opt.accel_margin); - if (opt.brake_margin > 0.0) - plant.set_brake_margin(opt.brake_margin); - eff_accel_margin = plant.options().accel_margin; - eff_brake_margin = plant.options().brake_margin; - phys::plant_log log(sim, plant); - - std::vector pos(2 * n), gw(2 * n), hd(n); - std::vector reached(n); - // Vehicle routes (city, bundle): the plant's footprint and turning circle, in world units. - const bool routed = opt.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 / opt.dt)); - const int settle = static_cast(std::lround(0.5 / opt.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 (opt.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 (opt.verbose && t > 0 && t % static_cast(std::lround(5.0 / opt.dt)) == 0) - for (int i = 0; i < n; ++i) { - const phys::planar_sample &q = log.runs()[i].back(); - const phys::vehicle_state vs = plant.physics_world().vehicle_state(plant.vehicle(i)); - std::printf(" 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 * opt.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) { - runs.push_back(log.runs()[i]); - run_seed.push_back(seed); - run_agent.push_back(i); - } - std::size_t moving = 0; - for (int i = 0; i < n; ++i) - for (const phys::planar_sample &q : log.runs()[i]) - moving += std::fabs(q.vx) > 0.5 ? 1 : 0; - std::printf("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) - std::printf(" 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 - } - - // 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 phys::planar_sample &q : run) { - ++down[q.wheels - q.wheels_down]; - ++total; - } - std::printf("wheels off the ground:"); - for (const auto &kv : down) - std::printf(" %d: %.1f%%", kv.first, 100.0 * kv.second / total); - std::printf("\n"); - } - - // The flights. - const std::size_t rows = static_cast(std::lround(opt.flight_seconds / opt.dt)) + 1; - const std::size_t stride = std::max( - 1, static_cast(std::lround(opt.stride_seconds / opt.dt))); - const phys::flight_set f = phys::cut_flights(runs, rows, stride, 1.0, opt.steer); - std::filesystem::create_directories(opt.out); - phys::write_npy((std::filesystem::path(opt.out) / "flights.npy").string(), f.data, - {f.flights, f.rows, static_cast(phys::kPlanarWidth)}); - - // --samples: every sample of every run, for analysis (samples.csv). - if (opt.samples) { - std::ofstream cs(std::filesystem::path(opt.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 phys::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", - run_seed[k], 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 phys::plant_residuals res = phys::one_tick_residuals(runs, opt.dt, desc, opt.mu, nsub); - const auto print_bins = [](const char *name, const std::vector &bins, - bool dyn) { - std::printf(" %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 phys::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); - std::printf(" %-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) - std::printf(" %10.4f %10.4f", b.lateral_rms_mps, b.yaw_rate_rms); - std::printf("\n"); - } - }; - std::printf("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", - opt.vehicle.c_str(), opt.dt, nsub, opt.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. - phys::dyn_bike_fit fit; - phys::plant_residuals res_spec, res_fit; - double held_default = 0.0, held_fit = 0.0; - const unsigned held = opt.seeds.back(); - if (opt.fit_dyn) { - std::vector> fit_runs, test_runs; - for (std::size_t k = 0; k < runs.size(); ++k) - (opt.seeds.size() > 1 && 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 = phys::fit_dyn_bike(fit_runs, opt.dt, desc, opt.mu, nsub); - const cvc::nav::dyn_bike_si spec = phys::nav_plant::dyn_bike_si_of(desc, opt.mu); - held_default = phys::dyn_bike_cost(test_runs, opt.dt, spec, desc.max_steer, nsub); - held_fit = phys::dyn_bike_cost(test_runs, opt.dt, fit.si, desc.max_steer, nsub); - std::printf("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", - opt.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 = - phys::one_tick_residuals(test_runs, opt.dt, spec, desc.wheelbase(), desc.max_steer, nsub); - res_fit = phys::one_tick_residuals(test_runs, opt.dt, fit.si, desc.wheelbase(), - desc.max_steer, nsub); - std::printf("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); - } - std::printf("flights: %zu x %zu rows (%.1f s at %.0f Hz), %zu windows dropped, peak %.2f g\n", - f.flights, f.rows, (f.rows - 1) * opt.dt, 1.0 / opt.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 phys::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(opt.out) / "residual.json"); - js << "{\n \"format\": \"cvc-plant-residual-1\",\n \"vehicle\": \"" << opt.vehicle - << "\", \"scene\": \"" << opt.scene << "\", \"dt\": " << num(opt.dt) - << ", \"nsub\": " << nsub << ", \"mu\": " << num(opt.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 (opt.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(opt.out) / "meta.json"); - js << "{\n \"format\": \"cvc-plant-log-1\",\n \"vehicle\": \"" << opt.vehicle - << "\", \"scene\": \"" << opt.scene << "\", \"bundle\": \"" << opt.bundle - << "\",\n \"dt\": " << num(opt.dt) << ", \"rate_hz\": " << num(1.0 / opt.dt) - << ", \"physics_dt\": 0.01, \"mu\": " << num(opt.mu) - << ", \"lateral_g\": " << num(opt.lateral_g) << ", \"seconds\": " << num(opt.seconds) - << ", \"accel_margin\": " << num(eff_accel_margin) - << ", \"brake_margin\": " << num(eff_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 = " - << (opt.steer == phys::steer_input::actual ? "the road-wheel angle's mean over the tick" - : "nav's steering command (held)") - << "\",\n \"shape\": [" << f.flights << ", " << f.rows << ", " << phys::kPlanarWidth - << "], \"dropped\": " << f.dropped << ", \"peak_lateral_g\": " << num(f.peak_lateral_g) - << ",\n \"seeds\": ["; - for (std::size_t k = 0; k < opt.seeds.size(); ++k) - js << (k ? ", " : "") << opt.seeds[k]; - js << "],\n \"flight_seed\": ["; - for (std::size_t k = 0; k < f.flights; ++k) - js << (k ? ", " : "") << run_seed[f.agent[k]]; - js << "],\n \"flight_agent\": ["; - for (std::size_t k = 0; k < f.flights; ++k) - js << (k ? ", " : "") << 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"; - } - std::printf("wrote %s/{flights.npy, meta.json, residual.json}\n", opt.out.c_str()); - } catch (const std::exception &e) { - std::fprintf(stderr, "cvc-plant-log: %s\n", e.what()); - return 1; - } - return 0; -} diff --git a/src/cvc/tools/liesphgp_pack.py b/src/cvc/tools/liesphgp_pack.py index 71aabc06f..23cd93250 100644 --- a/src/cvc/tools/liesphgp_pack.py +++ b/src/cvc/tools/liesphgp_pack.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -"""Pack cvc-plant-log runs into a LieSPHGP dataset pickle. +"""Pack `cvc plant-log` runs into a LieSPHGP dataset pickle. -cvc-plant-log (src/cvc/tools/cvc_plant_log_main.cpp) writes, per run directory, ``flights.npy`` +`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): @@ -168,7 +168,7 @@ def check(path, layout): 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("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") From 7d95ba99b29c76bef19088821c07517f84fbadc6 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 22:57:49 -0500 Subject: [PATCH 07/12] roadmap: dyn_plant's residual and the refits after #753's Jolt-fitted dyn_bike_si_of The 0.06 s residual table now shows the dynamic model before #753 / with its values now / the kinematic bicycle. Below 0.5 g the fitted values are 1.3-9x closer, and the RoboRacer's speed residual matches the kinematic bicycle's. Refitted against #753's values, the flat-ground fits find nothing left: tire factors back at x1, and the defaults do as well as a fit on the held-out seed. The datasets are byte-identical (Jolt drives; dyn_bike_si_of only enters the residuals). --- docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md | 80 ++++++++++++--------- 1 file changed, 48 insertions(+), 32 deletions(-) diff --git a/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md b/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md index 17d2c3f90..396af67aa 100644 --- a/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md +++ b/docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md @@ -197,48 +197,64 @@ Ticks are left out when they have a static contact, a wheel off the ground, an i 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, spec-sheet dynamic / kinematic. 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. +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 / 3.9 | 2.0 / 3.4 | 0.9 / 0.7 | 1.9 / 1.9 | -| 0.1-0.2 | 5.1 / 11.6 | 4.8 / 11.1 | 3.3 / 5.6 | 3.4 / 5.2 | -| 0.2-0.3 | 7.1 / 17.9 | 7.0 / 18.1 | 5.0 / 10.7 | 4.8 / 8.6 | -| 0.3-0.5 | 8.1 / 27.5 | 8.1 / 26.2 | 6.9 / 15.8 | 6.5 / 11.7 | -| >= 0.5 | 5.2 / 94.0 | 4.5 / 52.0 | 6.5 / 25.1 | 12.6 / 30.4 | - -- **Position and heading:** with `dyn_bike_si_of`'s spec-sheet values, the dynamic model is closer than - the kinematic bicycle wherever the tires carry load. -- **Speed:** for the RoboRacer it is worse, because two of its spec-sheet values are off: +| 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 subtracts. The RoboRacer's driver model tracks the commanded + - 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. Each fit uses seeds 100-102 -and is judged on seed 103. +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 | drag + rolling | held-out twist residual, (m/s)^2 | +| 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.15 s | x0.40 / x0.30 | x1.00 | x1 | 0.00287 -> 0.00045 | -| Humvee city | 0.02 s | x0.40 / x0.30 | x1.20 | x1 | 0.00280 -> 0.00060 | -| RoboRacer slalom | 0 | x0.30 / x0.25 | x0.85 | x0 | 0.00035 -> 0.00001 | -| RoboRacer city | 0 | x0.30 / x0.30 | x1.75 | x0 | 0.00229 -> 0.00018 | - -- **Tires:** both vehicles' Pacejka B from the spec sheet is 2.5-4x stiffer than Jolt's tires behave. -- **RoboRacer drive:** it has no drive lag and no uncompensated resistance. -- **Weakly identified:** the Humvee's drive lag 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.1-0.2, yaw +| 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:** the fitted model's position residual against the spec sheet's - and the kinematic bicycle's, in mm: - - Humvee slalom, above 0.5 g: 3.0 / 6.4 / 55.2; - - RoboRacer city, 0.3-0.5 g: 1.6 / 6.1 / 11.2. - - The RoboRacer's speed residual comes down to the kinematic bicycle's. -- **The defaults are unchanged:** `dyn_bike_si_of` keeps the spec-sheet values, so P8b and its goldens - do not move. Whether to adopt fitted values is open. +- **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. From 0fa251b7d6d9f27cff10d5e973a499c92daa379e Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 22:09:20 -0500 Subject: [PATCH 08/12] ph: the learned SE(2) port-Hamiltonian vehicle, natively, at parity with the JAX package cvc::ph's first piece: the SE(2) variant of LieSPHGP (stochastic port-Hamiltonian GPs on Lie groups) that Phase A fit to Jolt runs, as a torch- and JAX-free C++ forward. - mat3.h: 3x3 algebra, solve3, the six-level Cholesky form of an SPD matrix and its reverse, the SO(2) log. - rff.h: Matern random Fourier features and their GP, with the reverse. - rng.h: xoshiro256** with written-out normal and gamma draws, so a seed draws the same features on every compiler. - se2_vehicle.h: H, the drift (coadjoint, potential, D(v), G(v)), the Lie-IMEX step with the damping implicit, rollouts; float or double. se2_model holds the features, parameters and scales, and reads and writes them as JSON (cvc-ph-se2/1). ph_se2_test: against goldens generated once from the JAX package in float64 (the Humvee fit: 64 features, 1,691 parameters), one step at 16 states is within 1.9e-16 relative in double and 1.1e-7 in float; a 25-step rollout within 3.1e-15. The energy balance dH/dt = -v.Dv + v.Gu holds at every state; with the GPs off the drift is the hand-written linear-tire bicycle; the step is second order (x3.99 per halving). --- inc/cvc/ph/mat3.h | 158 +++++++++ inc/cvc/ph/rff.h | 117 +++++++ inc/cvc/ph/rng.h | 134 ++++++++ inc/cvc/ph/se2_vehicle.h | 443 ++++++++++++++++++++++++++ src/cvc/CMakeLists.txt | 5 + src/cvc/ph/se2_model.cpp | 330 +++++++++++++++++++ src/cvc/tests/CMakeLists.txt | 11 + src/cvc/tests/data/ph_se2_golden.json | 1 + src/cvc/tests/ph_se2_test.cpp | 408 ++++++++++++++++++++++++ 9 files changed, 1607 insertions(+) create mode 100644 inc/cvc/ph/mat3.h create mode 100644 inc/cvc/ph/rff.h create mode 100644 inc/cvc/ph/rng.h create mode 100644 inc/cvc/ph/se2_vehicle.h create mode 100644 src/cvc/ph/se2_model.cpp create mode 100644 src/cvc/tests/data/ph_se2_golden.json create mode 100644 src/cvc/tests/ph_se2_test.cpp diff --git a/inc/cvc/ph/mat3.h b/inc/cvc/ph/mat3.h new file mode 100644 index 000000000..8937254b4 --- /dev/null +++ b/inc/cvc/ph/mat3.h @@ -0,0 +1,158 @@ +/* + 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::ph's small linear algebra: 3x3 matrices (row-major std::array), the SO(2) log, and +// the positive-definite parameterization the port-Hamiltonian models use for M^-1 and D: +// +// A = L L^T, L = [[e^l0, 0, 0], [l3, e^l1, 0], [l4, l5, e^l2]] (six levels, any real values) +// +// with its reverse: for a scalar loss with dA = dLoss/dA (any 3x3, not necessarily symmetric), +// dL = (dA + dA^T) L, and the levels get dL's lower triangle, the diagonal times e^l. +// +// Templated on the scalar (float or double). solve3 is Gaussian elimination with partial pivoting. + +#ifndef __CVC_PH_MAT3_H__ +#define __CVC_PH_MAT3_H__ + +#include +#include +#include + +namespace cvc { +namespace ph { + +template using vec3 = std::array; +template using mat3 = std::array; + +template mat3 identity3() { + return {T(1), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(1)}; +} + +template mat3 mul(const mat3 &a, const mat3 &b) { + mat3 c{}; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + c[3 * i + j] = a[3 * i] * b[j] + a[3 * i + 1] * b[3 + j] + a[3 * i + 2] * b[6 + j]; + return c; +} + +template mat3 transpose(const mat3 &a) { + return {a[0], a[3], a[6], a[1], a[4], a[7], a[2], a[5], a[8]}; +} + +// A x +template vec3 matvec(const mat3 &a, const vec3 &x) { + return {a[0] * x[0] + a[1] * x[1] + a[2] * x[2], a[3] * x[0] + a[4] * x[1] + a[5] * x[2], + a[6] * x[0] + a[7] * x[1] + a[8] * x[2]}; +} + +// A^T x +template vec3 tmatvec(const mat3 &a, const vec3 &x) { + return {a[0] * x[0] + a[3] * x[1] + a[6] * x[2], a[1] * x[0] + a[4] * x[1] + a[7] * x[2], + a[2] * x[0] + a[5] * x[1] + a[8] * x[2]}; +} + +// acc += s x y^T +template void add_outer(mat3 &acc, const vec3 &x, const vec3 &y, T s = T(1)) { + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + acc[3 * i + j] += s * x[i] * y[j]; +} + +// x = A^-1 b by Gaussian elimination with partial pivoting. +template vec3 solve3(mat3 a, vec3 b) { + for (int col = 0; col < 3; ++col) { + int pivot = col; + for (int r = col + 1; r < 3; ++r) + if (std::abs(a[3 * r + col]) > std::abs(a[3 * pivot + col])) + pivot = r; + if (pivot != col) { + for (int j = 0; j < 3; ++j) + std::swap(a[3 * col + j], a[3 * pivot + j]); + std::swap(b[col], b[pivot]); + } + for (int r = col + 1; r < 3; ++r) { + const T f = a[3 * r + col] / a[3 * col + col]; + for (int j = col; j < 3; ++j) + a[3 * r + j] -= f * a[3 * col + j]; + b[r] -= f * b[col]; + } + } + vec3 x{}; + for (int r = 2; r >= 0; --r) { + T s = b[r]; + for (int j = r + 1; j < 3; ++j) + s -= a[3 * r + j] * x[j]; + x[r] = s / a[3 * r + r]; + } + return x; +} + +// x = A^-T b +template vec3 solve3_transposed(const mat3 &a, const vec3 &b) { + return solve3(transpose(a), b); +} + +// The lower-triangular factor of the six levels. +template mat3 spd_factor(const T *l) { + using std::exp; + return {exp(l[0]), T(0), T(0), l[3], exp(l[1]), T(0), l[4], l[5], exp(l[2])}; +} + +// A = L L^T. +template mat3 spd_from_levels(const T *l) { + const mat3 f = spd_factor(l); + return mul(f, transpose(f)); +} + +// dl += the levels' gradient for dA = dLoss/dA. +template void spd_from_levels_vjp(const T *l, const mat3 &dA, T *dl) { + const mat3 f = spd_factor(l); + mat3 sym{}; + for (int k = 0; k < 9; ++k) + sym[k] = dA[k]; + const mat3 t = transpose(dA); + for (int k = 0; k < 9; ++k) + sym[k] += t[k]; + const mat3 dL = mul(sym, f); + dl[0] += dL[0] * f[0]; + dl[1] += dL[4] * f[4]; + dl[2] += dL[8] * f[8]; + dl[3] += dL[3]; + dl[4] += dL[6]; + dl[5] += dL[7]; +} + +// The six levels of a symmetric positive-definite A (its Cholesky factor): the inverse of +// spd_from_levels. +template std::array spd_levels_of(const mat3 &a) { + using std::log; + using std::sqrt; + const T l00 = sqrt(a[0]); + const T l10 = a[3] / l00, l20 = a[6] / l00; + const T l11 = sqrt(a[4] - l10 * l10); + const T l21 = (a[7] - l20 * l10) / l11; + const T l22 = sqrt(a[8] - l20 * l20 - l21 * l21); + return {log(l00), log(l11), log(l22), l10, l20, l21}; +} + +// Log on SO(2): the angle in (-pi, pi]. Its derivative is 1 wherever it is used (away from the +// cut), so a reverse pass passes gradients straight through it. +template T wrap_angle(T a) { + using std::atan2; + using std::cos; + using std::sin; + return atan2(sin(a), cos(a)); +} + +} // namespace ph +} // namespace cvc + +#endif // __CVC_PH_MAT3_H__ diff --git a/inc/cvc/ph/rff.h b/inc/cvc/ph/rff.h new file mode 100644 index 000000000..855d32191 --- /dev/null +++ b/inc/cvc/ph/rff.h @@ -0,0 +1,117 @@ +/* + 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::ph::rff: random Fourier features of a Matern kernel, the basis of cvc::ph's Gaussian +// process subnetworks (LieSPHGP's features.py): +// +// phi_k(z) = sqrt(2 / n) cos(omega_k . z + b_k), omega_k = w_k / sqrt(s_k / (2 nu)) / ell, +// w_k ~ N(0, I), s_k ~ chi^2(2 nu), b_k ~ U(0, 2 pi) +// +// the Student-t spectral sample of the Matern-nu kernel k(|z - z'| / ell). A GP is then +// GP(z) = phi(z)^T W (W is n x outputs). Every derivative is bounded and closed form: +// d phi_k / dz = -sqrt(2 / n) sin(theta_k) omega_k, d^2 phi_k / dz^2 = -phi_k omega_k omega_k^T, +// which is what the reverse passes and the filter's Jacobians use. + +#ifndef __CVC_PH_RFF_H__ +#define __CVC_PH_RFF_H__ + +#include +#include +#include + +namespace cvc { +namespace ph { + +struct rff { + int input = 0; // the input's width + int count = 0; // n, the features + std::vector frequency; // [count x input], omega_k in rows (1 / ell folded in) + std::vector phase; // [count] + + // n features of a Matern-nu kernel with length scale ell on `input` dimensions. + static rff draw(rng &r, int input, int count, double nu, double ell) { + rff f; + f.input = input; + f.count = count; + f.frequency.resize(static_cast(count) * input); + f.phase.resize(count); + const double degrees = 2.0 * nu; + for (int k = 0; k < count; ++k) { + const double s = r.chi_square(degrees); + const double inv = 1.0 / (std::sqrt(s / degrees) * ell); + for (int d = 0; d < input; ++d) + f.frequency[static_cast(k) * input + d] = r.normal() * inv; + } + for (int k = 0; k < count; ++k) + f.phase[k] = 6.283185307179586476925286766559 * r.uniform(); + return f; + } + + double amplitude() const { return std::sqrt(2.0 / count); } + + // theta_k = omega_k . z + b_k, for k < count. + template void angles(const T *z, T *theta) const { + for (int k = 0; k < count; ++k) { + const double *w = &frequency[static_cast(k) * input]; + T t = static_cast(phase[k]); + for (int d = 0; d < input; ++d) + t += static_cast(w[d]) * z[d]; + theta[k] = t; + } + } + + // out[o] = sum_k phi_k(z) W[k][o] for o < outputs; W row-major [count x outputs]. + template void gp(const T *z, const T *W, int outputs, T *out, T *theta) const { + using std::cos; + angles(z, theta); + const T a = static_cast(amplitude()); + for (int o = 0; o < outputs; ++o) + out[o] = T(0); + for (int k = 0; k < count; ++k) { + const T phi = a * cos(theta[k]); + const T *row = W + static_cast(k) * outputs; + for (int o = 0; o < outputs; ++o) + out[o] += phi * row[o]; + } + } + + // The reverse of gp at z (theta from the forward): for dout = dLoss/dout, + // dW[k][o] += phi_k dout[o], dz += sum_k (dphi_k/dz) (W[k] . dout). + // dW or dz may be null. + template + void gp_vjp(const T *theta, const T *W, int outputs, const T *dout, T *dW, T *dz) const { + using std::cos; + using std::sin; + const T a = static_cast(amplitude()); + for (int k = 0; k < count; ++k) { + const T *row = W + static_cast(k) * outputs; + if (dW) { + const T phi = a * cos(theta[k]); + T *drow = dW + static_cast(k) * outputs; + for (int o = 0; o < outputs; ++o) + drow[o] += phi * dout[o]; + } + if (dz) { + T dot = T(0); + for (int o = 0; o < outputs; ++o) + dot += row[o] * dout[o]; + const T g = -a * sin(theta[k]) * dot; + const double *w = &frequency[static_cast(k) * input]; + for (int d = 0; d < input; ++d) + dz[d] += g * static_cast(w[d]); + } + } + } +}; + +} // namespace ph +} // namespace cvc + +#endif // __CVC_PH_RFF_H__ diff --git a/inc/cvc/ph/rng.h b/inc/cvc/ph/rng.h new file mode 100644 index 000000000..c761c1253 --- /dev/null +++ b/inc/cvc/ph/rng.h @@ -0,0 +1,134 @@ +/* + 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::ph::rng: the random numbers cvc::ph draws (random-feature frequencies, initial parameters, +// reparameterization draws, batches, observation noise). +// +// std::normal_distribution and std::gamma_distribution are not specified bit for bit, and +// libstdc++, libc++ and MSVC give different sequences for the same engine and seed. These are +// written out, so a seed means the same draws on every compiler: xoshiro256** seeded through +// splitmix64, Box-Muller normals, Marsaglia-Tsang gammas. (The transcendental functions can still +// differ in the last ulp between C libraries.) + +#ifndef __CVC_PH_RNG_H__ +#define __CVC_PH_RNG_H__ + +#include +#include + +namespace cvc { +namespace ph { + +class rng { +public: + explicit rng(std::uint64_t seed = 0) { reseed(seed); } + + void reseed(std::uint64_t seed) { + std::uint64_t x = seed; + for (auto &w : s_) + w = splitmix64(x); + has_spare_ = false; + } + + // A stream derived from this seed and a tag: independent draws for each purpose (features, + // initial values, batches, ...) that do not shift when another purpose draws more. + static rng derive(std::uint64_t seed, std::uint64_t tag) { + std::uint64_t x = seed ^ (0x9e3779b97f4a7c15ULL * (tag + 1)); + return rng(splitmix64(x)); + } + + std::uint64_t next() { + const std::uint64_t result = rotl(s_[1] * 5, 7) * 9; + const std::uint64_t t = s_[1] << 17; + s_[2] ^= s_[0]; + s_[3] ^= s_[1]; + s_[1] ^= s_[2]; + s_[0] ^= s_[3]; + s_[2] ^= t; + s_[3] = rotl(s_[3], 45); + return result; + } + + // Uniform on [0, 1), 53 bits. + double uniform() { return static_cast(next() >> 11) * 0x1.0p-53; } + + // Uniform integer in [0, n). + std::uint64_t below(std::uint64_t n) { + // Lemire's rejection keeps it unbiased. + const std::uint64_t threshold = (0 - n) % n; + for (;;) { + const std::uint64_t x = next(); + if (x >= threshold) + return x % n; + } + } + + // Standard normal (Box-Muller; the second value of each pair is kept for the next call). + double normal() { + if (has_spare_) { + has_spare_ = false; + return spare_; + } + double u1 = uniform(); + while (u1 <= 0.0) + u1 = uniform(); + const double u2 = uniform(); + const double radius = std::sqrt(-2.0 * std::log(u1)); + const double angle = 6.283185307179586476925286766559 * u2; + spare_ = radius * std::sin(angle); + has_spare_ = true; + return radius * std::cos(angle); + } + + // Gamma(shape, 1) by Marsaglia and Tsang; shape < 1 by the boost U^(1/shape). + double gamma(double shape) { + if (shape < 1.0) { + double u = uniform(); + while (u <= 0.0) + u = uniform(); + return gamma(shape + 1.0) * std::pow(u, 1.0 / shape); + } + const double d = shape - 1.0 / 3.0; + const double c = 1.0 / std::sqrt(9.0 * d); + for (;;) { + double x, v; + do { + x = normal(); + v = 1.0 + c * x; + } while (v <= 0.0); + v = v * v * v; + const double u = uniform(); + if (u < 1.0 - 0.0331 * x * x * x * x) + return d * v; + if (u > 0.0 && std::log(u) < 0.5 * x * x + d * (1.0 - v + std::log(v))) + return d * v; + } + } + + // Chi-square with k degrees of freedom. + double chi_square(double k) { return 2.0 * gamma(0.5 * k); } + +private: + static std::uint64_t rotl(std::uint64_t x, int k) { return (x << k) | (x >> (64 - k)); } + static std::uint64_t splitmix64(std::uint64_t &x) { + std::uint64_t z = (x += 0x9e3779b97f4a7c15ULL); + z = (z ^ (z >> 30)) * 0xbf58476d1ce4e5b9ULL; + z = (z ^ (z >> 27)) * 0x94d049bb133111ebULL; + return z ^ (z >> 31); + } + std::uint64_t s_[4]; + double spare_ = 0.0; + bool has_spare_ = false; +}; + +} // namespace ph +} // namespace cvc + +#endif // __CVC_PH_RNG_H__ diff --git a/inc/cvc/ph/se2_vehicle.h b/inc/cvc/ph/se2_vehicle.h new file mode 100644 index 000000000..d44bddb53 --- /dev/null +++ b/inc/cvc/ph/se2_vehicle.h @@ -0,0 +1,443 @@ +/* + 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::ph::se2: a learned port-Hamiltonian ground vehicle on SE(2), a native port of the SE(2) +// variant of LieSPHGP (stochastic port-Hamiltonian Gaussian processes on Lie groups, +// CVC-Lab/LieSPHGP; its SE(2) vehicle package was written for the Phase A fit to Jolt). No +// physical constant anywhere; everything is learned from logged runs. +// +// State s = (x, y, psi, v) with v = (v_x, v_y, r) the body twist, inputs u (control_dim of them, +// divided by their training RMS); momenta p = M v and H = 1/2 p . M^-1 p + V(x, y): +// +// d(x, y) = R(psi) (v_x, v_y) dt, dpsi = r dt +// dp = ( ad*_v p - [R(psi)^T grad V; 0] - D(v) v + G(v) u ) dt, ad*_v p = (p_y r, -p_x r, p_x +// v_y - p_y v_x) dv = M^-1 dp + Sigma dW (Sigma diagonal: 3 numbers) +// +// ad*_v p is SE(2)'s coadjoint action: the r v_y and -r v_x terms of a vehicle's body-frame +// equations. Each piece is a learned level plus a Gaussian-process residual on random features +// (rff.h), q(W) = N(mean, diag e^(2 log_std)): +// +// M^-1 = spd(level) a constant (a rigid body's inertia) +// D(v) = spd(level + GP_D(v / twist_scale)) positive definite: it can only dissipate +// G(v) = level + GP_G(v / twist_scale) 3 x control_dim, row-major +// V(x, y) = level . (x, y) + GP_V((x, y) / V_length_scale) +// +// spd() is mat3.h's six-level Cholesky form. twist_scale is the RMS of each twist channel over the +// training flights. Only M^-1 D, M^-1 G and M^-1 grad V are identifiable, plus M's shape through +// ad*_v p. +// +// One step of size h (Lie-IMEX: the damping implicit, everything else explicit; SO(2) is abelian, +// so the heading stays a continuous angle). With e = M^-1 (ad*_v p - body + G u) and K = M^-1 D(v) +// at each stage, and the twist increment eta = sqrt(h) Sigma z: +// +// (I + h K_1) v_p = v + h e_1 + eta, (x, y)_p = (x, y) + h R(psi) v, psi_p = +// psi + h r (I + h/2 K_2) v' = v + h/2 (e_1 + e_2 - K_1 v) + eta, (x, y)' = (x, y) + h/2 (R(psi) +// v + R(psi_p) v_p), psi' = psi + h/2 (r + r_p) +// +// se2_vehicle evaluates one weight draw (the posterior mean, or a sample) and is templated on the +// scalar, so the same code runs in float and double. Parity with the JAX package is a test +// (ph_se2_test, against goldens generated once from it). + +#ifndef __CVC_PH_SE2_VEHICLE_H__ +#define __CVC_PH_SE2_VEHICLE_H__ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cvc { +namespace ph { + +constexpr int kSe2MaxControls = 4; +constexpr int kSe2State = 6; // x, y, psi, v_x, v_y, r + +// A Gaussian-process subnetwork's trainable values. +struct se2_gp { + std::vector mean; // [features x outputs], row-major + std::vector log_std; // [features x outputs] + std::vector level; // D: 6 levels; G: 3 x control_dim, row-major; V: 2 +}; + +// Which optimizer a parameter leaf belongs to (LieSPHGP's train.py: Prodigy on each subnetwork's +// level, Adam on everything else). +enum class se2_leaf_group { adam, D_level, G_level, V_level, Minv_level }; + +struct se2_params { + std::array minv_level{}; + se2_gp D, G, V; + std::array process_log_sigma{}; // the diffusion Sigma, log + std::vector likelihood_log_sigma; // [position, heading, velocity, yaw rate]; empty + // when the model was not fit by the filter + + // fn(name, data, size, group) for every leaf in LieSPHGP's checkpoint order (sorted keys). + template void for_each_leaf(F &&fn) { + fn("D.level", D.level.data(), D.level.size(), se2_leaf_group::D_level); + fn("D.log_std", D.log_std.data(), D.log_std.size(), se2_leaf_group::adam); + fn("D.mean", D.mean.data(), D.mean.size(), se2_leaf_group::adam); + fn("G.level", G.level.data(), G.level.size(), se2_leaf_group::G_level); + fn("G.log_std", G.log_std.data(), G.log_std.size(), se2_leaf_group::adam); + fn("G.mean", G.mean.data(), G.mean.size(), se2_leaf_group::adam); + fn("Minv.level", minv_level.data(), minv_level.size(), se2_leaf_group::Minv_level); + fn("V.level", V.level.data(), V.level.size(), se2_leaf_group::V_level); + fn("V.log_std", V.log_std.data(), V.log_std.size(), se2_leaf_group::adam); + fn("V.mean", V.mean.data(), V.mean.size(), se2_leaf_group::adam); + if (!likelihood_log_sigma.empty()) + fn("likelihood.log_sigma", likelihood_log_sigma.data(), likelihood_log_sigma.size(), + se2_leaf_group::adam); + fn("process.log_sigma", process_log_sigma.data(), process_log_sigma.size(), + se2_leaf_group::adam); + } + template void for_each_leaf(F &&fn) const { + const_cast(this)->for_each_leaf( + [&](const char *name, double *data, std::size_t size, se2_leaf_group group) { + fn(name, static_cast(data), size, group); + }); + } + std::size_t size() const; + // The same shapes, every value zero (a gradient). + se2_params zeros_like() const; +}; + +// The random features and the options that are not trained. +struct se2_setup { + int control_dim = 2; + int features = 64; + double matern_nu = 2.5, matern_length = 1.0; + bool v_gp = true; + double v_length = 50.0; // V's GP reads (x, y) / v_length + rff D, G, V; // inputs 3, 3, 2 + + int g_outputs() const { return 3 * control_dim; } +}; + +// How a model is initialized (LieSPHGP's model.* keys; train.py's defaults for the Phase A fits). +struct se2_init { + int control_dim = 2; + int features = 64; + double matern_nu = 2.5, matern_length = 1.0; + bool v_gp = true; + double v_length = 50.0; + double initial_log_std = -5.0; + double initial_inverse_mass = 1.0; // M^-1's level starts at this times I + double initial_dissipation = 1.0; // and D's + double initial_control_gp_mean_scale = 0.0; // G's GP mean starts at this times N(0, 1) / n + double initial_diffusion = 0.1; + double initial_observation_sigma = 0.1; // only with_likelihood + double initial_randomness = 0.1; // every level and noise scale + this times N(0, 1) + bool with_likelihood = false; +}; + +// A model: its features, parameters, the data statistics it reads its inputs with, and where it +// came from. The file is JSON, format "cvc-ph-se2/1". +struct se2_model { + se2_setup setup; + se2_params params; + std::array twist_scale{1.0, 1.0, 1.0}; + std::vector control_scale; // the inputs are divided by these (their training RMS) + double interval = 0.02; // s between samples + int substeps = 1; // Lie-IMEX steps per interval + std::vector> provenance; + + // A fresh model: features drawn and parameters initialized from `seed` (LieSPHGP's + // initialize_parameters with this library's random numbers). + static se2_model initialize(const se2_init &init, std::uint64_t seed, + const std::array &twist_scale, + const std::vector &control_scale, double interval, + int substeps = 1); + + void save(const std::string &path) const; + std::string to_json() const; + static se2_model load(const std::string &path); // throws std::runtime_error + static se2_model from_json(const std::string &text); +}; + +// One weight draw: what the vector field reads. +template struct se2_weights { + int control_dim = 2; + std::vector wD, wG, wV; // the GP weights + std::array minv_level{}, d_level{}; + std::vector g_level; // 3 x control_dim + std::array v_level{}; + std::array diffusion{}; +}; + +// The posterior mean (W = mean), or W = mean + e^log_std z with z given ([features x outputs] for +// each of D, G, V; null for the mean). +template +se2_weights se2_draw(const se2_model &m, const double *zD = nullptr, const double *zG = nullptr, + const double *zV = nullptr) { + se2_weights w; + w.control_dim = m.setup.control_dim; + const auto draw = [](const se2_gp &gp, const double *z, std::vector &out) { + out.resize(gp.mean.size()); + for (std::size_t i = 0; i < gp.mean.size(); ++i) + out[i] = static_cast(z ? gp.mean[i] + std::exp(gp.log_std[i]) * z[i] : gp.mean[i]); + }; + draw(m.params.D, zD, w.wD); + draw(m.params.G, zG, w.wG); + draw(m.params.V, zV, w.wV); + for (int i = 0; i < 6; ++i) { + w.minv_level[i] = static_cast(m.params.minv_level[i]); + w.d_level[i] = static_cast(m.params.D.level[i]); + } + w.g_level.assign(m.params.G.level.begin(), m.params.G.level.end()); + for (int i = 0; i < 2; ++i) + w.v_level[i] = static_cast(m.params.V.level[i]); + for (int i = 0; i < 3; ++i) + w.diffusion[i] = static_cast(std::exp(m.params.process_log_sigma[i])); + return w; +} + +// Everything one evaluation of the drift computes, kept for a reverse pass. +template struct se2_stage { + T c = T(0), s = T(0); // cos, sin of the heading + vec3 v{}; // the twist + std::array u{}; + std::array dxy{}; // R(psi) (v_x, v_y) + vec3 p{}; // M v + std::array grad_v{}, body{}; // grad V (world), R^T grad V + vec3 coad{}; // ad*_v p + std::array d_levels{}; // D's level + GP + mat3 D{}; + std::array G{}; + vec3 q{}; // ad*_v p - body + G u + vec3 e{}; // M^-1 q + mat3 K{}; // M^-1 D + vec3 z{}; // v / twist_scale + std::vector theta_D, theta_G, theta_V; // the features' angles +}; + +template class se2_vehicle { +public: + se2_vehicle(const se2_setup &setup, se2_weights weights, const std::array &scale) + : setup_(&setup), w_(std::move(weights)) { + for (int i = 0; i < 3; ++i) + twist_scale_[i] = static_cast(scale[i]); + minv_ = spd_from_levels(w_.minv_level.data()); + } + + const se2_setup &setup() const { return *setup_; } + const se2_weights &weights() const { return w_; } + const std::array &twist_scale() const { return twist_scale_; } + int control_dim() const { return w_.control_dim; } + + const mat3 &inverse_mass() const { return minv_; } + + // D's six levels at the twist (level + GP); theta receives the features' angles. + std::array dissipation_levels(const vec3 &v, vec3 &z, T *theta) const { + for (int i = 0; i < 3; ++i) + z[i] = v[i] / twist_scale_[i]; + std::array gp{}; + setup_->D.gp(z.data(), w_.wD.data(), 6, gp.data(), theta); + for (int i = 0; i < 6; ++i) + gp[i] += w_.d_level[i]; + return gp; + } + + mat3 dissipation(const vec3 &v) const { + vec3 z; + std::vector theta(setup_->D.count); + const auto l = dissipation_levels(v, z, theta.data()); + return spd_from_levels(l.data()); + } + + // G(v), 3 x control_dim row-major, into out. + void control_matrix(const vec3 &z, T *out, T *theta) const { + const int n = setup_->g_outputs(); + setup_->G.gp(z.data(), w_.wG.data(), n, out, theta); + for (int i = 0; i < n; ++i) + out[i] += w_.g_level[i]; + } + void control_matrix_at(const vec3 &v, T *out) const { + vec3 z; + for (int i = 0; i < 3; ++i) + z[i] = v[i] / twist_scale_[i]; + std::vector theta(setup_->G.count); + control_matrix(z, out, theta.data()); + } + + T potential(T x, T y) const { + T value = w_.v_level[0] * x + w_.v_level[1] * y; + if (setup_->v_gp) { + const T inv = static_cast(1.0 / setup_->v_length); + const T zz[2] = {x * inv, y * inv}; + std::vector theta(setup_->V.count); + T out = T(0); + setup_->V.gp(zz, w_.wV.data(), 1, &out, theta.data()); + value += out; + } + return value; + } + + // grad V in the world frame; theta receives the V features' angles. + std::array grad_potential(T x, T y, T *theta) const { + std::array g{w_.v_level[0], w_.v_level[1]}; + if (setup_->v_gp) { + const T inv = static_cast(1.0 / setup_->v_length); + const T zz[2] = {x * inv, y * inv}; + setup_->V.angles(zz, theta); + const T a = static_cast(setup_->V.amplitude()); + for (int k = 0; k < setup_->V.count; ++k) { + using std::sin; + const T f = -a * sin(theta[k]) * w_.wV[k] * inv; + g[0] += f * static_cast(setup_->V.frequency[2 * k]); + g[1] += f * static_cast(setup_->V.frequency[2 * k + 1]); + } + } + return g; + } + std::array grad_potential(T x, T y) const { + std::vector theta(setup_->V.count); + return grad_potential(x, y, theta.data()); + } + + T hamiltonian(const T *state) const { + const vec3 v{state[3], state[4], state[5]}; + const vec3 p = solve3(minv_, v); + return T(0.5) * (p[0] * v[0] + p[1] * v[1] + p[2] * v[2]) + potential(state[0], state[1]); + } + + // Every piece of the drift at (state, u); st's feature buffers are sized here. + void stage(const T *state, const T *u, se2_stage &st) const { + using std::cos; + using std::sin; + st.theta_D.resize(setup_->D.count); + st.theta_G.resize(setup_->G.count); + st.theta_V.resize(setup_->V.count); + const int nu = w_.control_dim; + for (int j = 0; j < nu; ++j) + st.u[j] = u[j]; + st.c = cos(state[2]); + st.s = sin(state[2]); + st.v = {state[3], state[4], state[5]}; + st.dxy = {st.c * st.v[0] - st.s * st.v[1], st.s * st.v[0] + st.c * st.v[1]}; + st.p = solve3(minv_, st.v); + st.grad_v = grad_potential(state[0], state[1], st.theta_V.data()); + st.body = {st.c * st.grad_v[0] + st.s * st.grad_v[1], + -st.s * st.grad_v[0] + st.c * st.grad_v[1]}; + st.coad = {st.p[1] * st.v[2], -st.p[0] * st.v[2], st.p[0] * st.v[1] - st.p[1] * st.v[0]}; + st.d_levels = dissipation_levels(st.v, st.z, st.theta_D.data()); + st.D = spd_from_levels(st.d_levels.data()); + control_matrix(st.z, st.G.data(), st.theta_G.data()); + for (int i = 0; i < 3; ++i) { + T gu = T(0); + for (int j = 0; j < nu; ++j) + gu += st.G[i * nu + j] * st.u[j]; + st.q[i] = st.coad[i] + gu - (i < 2 ? st.body[i] : T(0)); + } + st.e = matvec(minv_, st.q); + st.K = mul(minv_, st.D); + } + + // The drift (6): (dx, dy, dpsi, dv), the damping included. + void vector_field(const T *state, const T *u, T *out) const { + se2_stage st; + stage(state, u, st); + const vec3 kv = matvec(st.K, st.v); + out[0] = st.dxy[0]; + out[1] = st.dxy[1]; + out[2] = st.v[2]; + for (int i = 0; i < 3; ++i) + out[3 + i] = st.e[i] - kv[i]; + } + + // M^-1 D(v), the matrix the step treats implicitly. + mat3 effective_damping(const T *state) const { + return mul(minv_, dissipation({state[3], state[4], state[5]})); + } + + // One Lie-IMEX step of size h from state (6) under u; increment (3, may be null) is the twist's + // noise increment eta. st1 and st2 receive the two stages; vp the predictor's twist. + void step(const T *state, const T *u, T h, const T *increment, T *out, se2_stage &st1, + se2_stage &st2, vec3 &vp) const { + stage(state, u, st1); + const vec3 &v = st1.v; + const vec3 eta = increment ? vec3{increment[0], increment[1], increment[2]} : vec3{}; + mat3 a1 = identity3(); + for (int k = 0; k < 9; ++k) + a1[k] += h * st1.K[k]; + vp = solve3(a1, vec3{v[0] + h * st1.e[0] + eta[0], v[1] + h * st1.e[1] + eta[1], + v[2] + h * st1.e[2] + eta[2]}); + const T pred[6] = {state[0] + h * st1.dxy[0], + state[1] + h * st1.dxy[1], + state[2] + h * v[2], + vp[0], + vp[1], + vp[2]}; + stage(pred, u, st2); + const vec3 k1v = matvec(st1.K, v); + const T hh = T(0.5) * h; + mat3 a2 = identity3(); + for (int k = 0; k < 9; ++k) + a2[k] += hh * st2.K[k]; + vec3 right; + for (int i = 0; i < 3; ++i) + right[i] = v[i] + hh * (st1.e[i] + st2.e[i] - k1v[i]) + eta[i]; + const vec3 vn = solve3(a2, right); + out[0] = state[0] + hh * (st1.dxy[0] + st2.dxy[0]); + out[1] = state[1] + hh * (st1.dxy[1] + st2.dxy[1]); + out[2] = state[2] + hh * (v[2] + vp[2]); + out[3] = vn[0]; + out[4] = vn[1]; + out[5] = vn[2]; + } + void step(const T *state, const T *u, T h, const T *increment, T *out) const { + se2_stage st1, st2; + vec3 vp; + step(state, u, h, increment, out, st1, st2, vp); + } + + // One observation interval of `substeps` steps under the held u. noise (substeps x 3 standard + // normals) may be null: the drift alone. + void interval_step(const T *state, const T *u, T interval, int substeps, const T *noise, + T *out) const { + const T h = interval / static_cast(substeps); + const T root = std::sqrt(h); + T cur[6]; + for (int i = 0; i < 6; ++i) + cur[i] = state[i]; + for (int k = 0; k < substeps; ++k) { + T inc[3]; + if (noise) + for (int i = 0; i < 3; ++i) + inc[i] = root * w_.diffusion[i] * noise[3 * k + i]; + step(cur, u, h, noise ? inc : nullptr, out); + for (int i = 0; i < 6; ++i) + cur[i] = out[i]; + } + } + + // path[(t + 1) * 6 + ...] for t < steps from state; controls [steps x control_dim]; path[0..5] is + // the start. + void rollout(const T *state, const T *controls, int steps, T interval, int substeps, + T *path) const { + for (int i = 0; i < 6; ++i) + path[i] = state[i]; + for (int t = 0; t < steps; ++t) + interval_step(path + 6 * t, controls + static_cast(t) * w_.control_dim, interval, + substeps, nullptr, path + 6 * (t + 1)); + } + +private: + const se2_setup *setup_; + se2_weights w_; + std::array twist_scale_{}; + mat3 minv_{}; +}; + +} // namespace ph +} // namespace cvc + +#endif // __CVC_PH_SE2_VEHICLE_H__ diff --git a/src/cvc/CMakeLists.txt b/src/cvc/CMakeLists.txt index 3540a81aa..ba6feeed4 100644 --- a/src/cvc/CMakeLists.txt +++ b/src/cvc/CMakeLists.txt @@ -129,6 +129,10 @@ set(INCLUDE_FILES ../../inc/cvc/nav/plant_types.h ../../inc/cvc/nav/terrain.h ../../inc/cvc/nav/vehicle_plant.h + ../../inc/cvc/ph/mat3.h + ../../inc/cvc/ph/rff.h + ../../inc/cvc/ph/rng.h + ../../inc/cvc/ph/se2_vehicle.h ../../inc/cvc/lod/select.h ../../inc/cvc/lod/pyramid.h ../../inc/cvc/lod/tiles.h @@ -325,6 +329,7 @@ set(SOURCE_FILES nav/sim_world_binding.cpp nav/sim_thread.cpp nav/nav_stats.cpp + ph/se2_model.cpp lod/select.cpp lod/pyramid.cpp lod/tiles.cpp diff --git a/src/cvc/ph/se2_model.cpp b/src/cvc/ph/se2_model.cpp new file mode 100644 index 000000000..c9718c8c0 --- /dev/null +++ b/src/cvc/ph/se2_model.cpp @@ -0,0 +1,330 @@ +/* + 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::ph::se2_model: initialization and the JSON model file (format "cvc-ph-se2/1"). +// +// The file's layout is LieSPHGP's SE(2) checkpoint written out: the features (setup), the +// parameters by subnetwork, the twist and control scales, the sample interval. Arrays are flat +// and row-major. Numbers are written with 17 significant digits, so a save and load round-trips +// every double exactly. + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cvc { +namespace ph { + +namespace { + +namespace pt = boost::property_tree; + +constexpr const char *kFormat = "cvc-ph-se2/1"; + +// Random streams, one per purpose (rng::derive tags). +enum stream : std::uint64_t { features_D = 1, features_G, features_V, levels, means }; + +std::array spd_level(double value) { + const double l = 0.5 * std::log(value); + return {l, l, l, 0.0, 0.0, 0.0}; +} + +void put_number(std::ostringstream &o, double x) { + if (!std::isfinite(x)) + throw std::runtime_error("se2_model: cannot write a non-finite number"); + char buf[32]; + std::snprintf(buf, sizeof(buf), "%.17g", x); + o << buf; +} + +template void put_array(std::ostringstream &o, const C &values) { + o << '['; + bool first = true; + for (double x : values) { + if (!first) + o << ','; + first = false; + put_number(o, x); + } + o << ']'; +} + +std::string quoted(const std::string &s) { + std::string out = "\""; + for (char ch : s) { + if (ch == '"' || ch == '\\') + out += '\\'; + if (static_cast(ch) < 0x20) { + char buf[8]; + std::snprintf(buf, sizeof(buf), "\\u%04x", static_cast(ch)); + out += buf; + continue; + } + out += ch; + } + return out + "\""; +} + +std::vector get_array(const pt::ptree &node, const std::string &path, + std::size_t expect = 0) { + const auto child = node.get_child_optional(path); + if (!child) + throw std::runtime_error(std::string("se2_model: ") + path + " is missing"); + std::vector out; + out.reserve(child->size()); + for (const auto &kv : *child) + out.push_back(std::stod(kv.second.data())); + if (expect && out.size() != expect) + throw std::runtime_error("se2_model: " + path + " has " + std::to_string(out.size()) + + " values, expected " + std::to_string(expect)); + return out; +} + +rff get_rff(const pt::ptree &node, const std::string &name, int input, int count) { + rff f; + f.input = input; + f.count = count; + f.frequency = + get_array(node, "setup." + name + ".frequencies", static_cast(input) * count); + f.phase = get_array(node, "setup." + name + ".phases", static_cast(count)); + return f; +} + +se2_gp get_gp(const pt::ptree &node, const std::string &name, std::size_t weights, + std::size_t levels) { + se2_gp g; + g.mean = get_array(node, "params." + name + ".mean", weights); + g.log_std = get_array(node, "params." + name + ".log_std", weights); + g.level = get_array(node, "params." + name + ".level", levels); + return g; +} + +} // namespace + +std::size_t se2_params::size() const { + std::size_t n = 0; + for_each_leaf([&](const char *, const double *, std::size_t size, se2_leaf_group) { n += size; }); + return n; +} + +se2_params se2_params::zeros_like() const { + se2_params z = *this; + z.for_each_leaf([](const char *, double *data, std::size_t size, se2_leaf_group) { + for (std::size_t i = 0; i < size; ++i) + data[i] = 0.0; + }); + return z; +} + +se2_model se2_model::initialize(const se2_init &init, std::uint64_t seed, + const std::array &twist_scale, + const std::vector &control_scale, double interval, + int substeps) { + if (init.control_dim < 1 || init.control_dim > kSe2MaxControls) + throw std::invalid_argument("se2_model: control_dim must be 1-4"); + if (static_cast(control_scale.size()) != init.control_dim) + throw std::invalid_argument("se2_model: one control scale per input"); + if (init.features < 1 || interval <= 0.0 || substeps < 1) + throw std::invalid_argument("se2_model: features, interval and substeps must be positive"); + se2_model m; + se2_setup &s = m.setup; + s.control_dim = init.control_dim; + s.features = init.features; + s.matern_nu = init.matern_nu; + s.matern_length = init.matern_length; + s.v_gp = init.v_gp; + s.v_length = init.v_length; + { + rng r = rng::derive(seed, features_D); + s.D = rff::draw(r, 3, init.features, init.matern_nu, init.matern_length); + } + { + rng r = rng::derive(seed, features_G); + s.G = rff::draw(r, 3, init.features, init.matern_nu, init.matern_length); + } + { + rng r = rng::derive(seed, features_V); + s.V = rff::draw(r, 2, init.features, init.matern_nu, init.matern_length); + } + + rng lv = rng::derive(seed, levels); + const auto noise = [&]() { return init.initial_randomness * lv.normal(); }; + se2_params &p = m.params; + const auto minv = spd_level(init.initial_inverse_mass); + for (int i = 0; i < 6; ++i) + p.minv_level[i] = minv[i] + noise(); + const auto d = spd_level(init.initial_dissipation); + p.D.level.resize(6); + for (int i = 0; i < 6; ++i) + p.D.level[i] = d[i] + noise(); + p.G.level.resize(3 * static_cast(init.control_dim)); + for (auto &x : p.G.level) + x = noise(); + p.V.level.resize(2); + for (auto &x : p.V.level) + x = noise(); + if (init.with_likelihood) { + p.likelihood_log_sigma.resize(4); + for (auto &x : p.likelihood_log_sigma) + x = std::log(init.initial_observation_sigma) + noise(); + } + for (auto &x : p.process_log_sigma) + x = std::log(init.initial_diffusion) + noise(); + + rng mv = rng::derive(seed, means); + const auto gp = [&](se2_gp &g, int outputs, double scale) { + const std::size_t n = static_cast(init.features) * outputs; + g.mean.resize(n); + g.log_std.assign(n, init.initial_log_std); + for (auto &x : g.mean) + x = scale * mv.normal() / init.features; + }; + gp(p.D, 6, 1.0); + gp(p.G, 3 * init.control_dim, init.initial_control_gp_mean_scale); + gp(p.V, 1, 1.0); + + m.twist_scale = twist_scale; + m.control_scale = control_scale; + m.interval = interval; + m.substeps = substeps; + return m; +} + +std::string se2_model::to_json() const { + std::ostringstream o; + o << "{\"format\":\"" << kFormat << "\",\"provenance\":{"; + for (std::size_t i = 0; i < provenance.size(); ++i) + o << (i ? "," : "") << quoted(provenance[i].first) << ':' << quoted(provenance[i].second); + o << "},\"control_dim\":" << setup.control_dim << ",\"feature_count\":" << setup.features + << ",\"G_input\":\"twist\",\"V_gp\":" << (setup.v_gp ? "true" : "false") + << ",\"V_length_scale\":"; + put_number(o, setup.v_length); + o << ",\"matern_smoothness\":"; + put_number(o, setup.matern_nu); + o << ",\"matern_length_scale\":"; + put_number(o, setup.matern_length); + o << ",\"interval\":"; + put_number(o, interval); + o << ",\"substeps\":" << substeps << ",\"twist_scale\":"; + put_array(o, twist_scale); + o << ",\"control_scale\":"; + put_array(o, control_scale); + o << ",\"setup\":{"; + const std::pair feats[] = { + {"D", &setup.D}, {"G", &setup.G}, {"V", &setup.V}}; + for (int i = 0; i < 3; ++i) { + o << (i ? "," : "") << '"' << feats[i].first << "\":{\"frequencies\":"; + put_array(o, feats[i].second->frequency); + o << ",\"phases\":"; + put_array(o, feats[i].second->phase); + o << '}'; + } + o << "},\"params\":{\"Minv\":{\"level\":"; + put_array(o, params.minv_level); + o << '}'; + const std::pair gps[] = { + {"D", ¶ms.D}, {"G", ¶ms.G}, {"V", ¶ms.V}}; + for (const auto &g : gps) { + o << ",\"" << g.first << "\":{\"mean\":"; + put_array(o, g.second->mean); + o << ",\"log_std\":"; + put_array(o, g.second->log_std); + o << ",\"level\":"; + put_array(o, g.second->level); + o << '}'; + } + o << ",\"process\":{\"log_sigma\":"; + put_array(o, params.process_log_sigma); + o << '}'; + if (!params.likelihood_log_sigma.empty()) { + o << ",\"likelihood\":{\"log_sigma\":"; + put_array(o, params.likelihood_log_sigma); + o << '}'; + } + o << "}}\n"; + return o.str(); +} + +void se2_model::save(const std::string &path) const { + std::ofstream out(path, std::ios::binary); + if (!out) + throw std::runtime_error("se2_model: cannot write " + path); + out << to_json(); + if (!out) + throw std::runtime_error("se2_model: writing " + path + " failed"); +} + +se2_model se2_model::from_json(const std::string &text) { + pt::ptree root; + std::istringstream in(text); + try { + pt::read_json(in, root); + } catch (const pt::json_parser_error &e) { + throw std::runtime_error(std::string("se2_model: ") + e.what()); + } + // A golden file wraps the model in "model". + const pt::ptree &node = root.get_child_optional("model") ? root.get_child("model") : root; + if (node.get("format", "") != kFormat) + throw std::runtime_error(std::string("se2_model: not a ") + kFormat + " model"); + if (node.get("G_input", "twist") != "twist") + throw std::runtime_error("se2_model: only G_input twist is supported"); + se2_model m; + se2_setup &s = m.setup; + s.control_dim = node.get("control_dim"); + s.features = node.get("feature_count"); + if (s.control_dim < 1 || s.control_dim > kSe2MaxControls || s.features < 1) + throw std::runtime_error("se2_model: control_dim 1-4 and a positive feature count"); + s.v_gp = node.get("V_gp"); + s.v_length = node.get("V_length_scale"); + s.matern_nu = node.get("matern_smoothness", 2.5); + s.matern_length = node.get("matern_length_scale", 1.0); + s.D = get_rff(node, "D", 3, s.features); + s.G = get_rff(node, "G", 3, s.features); + s.V = get_rff(node, "V", 2, s.features); + const std::size_t n = static_cast(s.features); + se2_params &p = m.params; + const auto minv = get_array(node, "params.Minv.level", 6); + std::copy(minv.begin(), minv.end(), p.minv_level.begin()); + p.D = get_gp(node, "D", n * 6, 6); + p.G = get_gp(node, "G", n * 3 * s.control_dim, 3 * static_cast(s.control_dim)); + p.V = get_gp(node, "V", n, 2); + if (node.get_child_optional("params.process")) { + const auto proc = get_array(node, "params.process.log_sigma", 3); + std::copy(proc.begin(), proc.end(), p.process_log_sigma.begin()); + } + if (node.get_child_optional("params.likelihood")) + p.likelihood_log_sigma = get_array(node, "params.likelihood.log_sigma", 4); + const auto ts = get_array(node, "twist_scale", 3); + std::copy(ts.begin(), ts.end(), m.twist_scale.begin()); + m.control_scale = get_array(node, "control_scale", static_cast(s.control_dim)); + m.interval = node.get("interval"); + m.substeps = node.get("substeps", 1); + if (const auto prov = node.get_child_optional("provenance")) + for (const auto &kv : *prov) + m.provenance.emplace_back(kv.first, kv.second.data()); + return m; +} + +se2_model se2_model::load(const std::string &path) { + std::ifstream in(path, std::ios::binary); + if (!in) + throw std::runtime_error("se2_model: cannot read " + path); + std::ostringstream text; + text << in.rdbuf(); + return from_json(text.str()); +} + +} // namespace ph +} // namespace cvc diff --git a/src/cvc/tests/CMakeLists.txt b/src/cvc/tests/CMakeLists.txt index dffee9ffb..2b449df19 100644 --- a/src/cvc/tests/CMakeLists.txt +++ b/src/cvc/tests/CMakeLists.txt @@ -110,6 +110,7 @@ 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(ph_se2_test ph_se2_test.cpp) add_executable(nav_terrain_test nav_terrain_test.cpp) add_executable(nav_binding_test nav_binding_test.cpp) add_executable(physics_parity_test physics_parity_test.cpp) @@ -291,6 +292,7 @@ set(TEST_TARGETS nav_plant_test physics_plant_log_test nav_dyn_bike_test + ph_se2_test nav_terrain_test nav_binding_test physics_parity_test @@ -1629,6 +1631,13 @@ target_link_libraries(nav_dyn_bike_test GTest::gtest_main ) +target_link_libraries(ph_se2_test + PRIVATE + cvc + GTest::gtest + GTest::gtest_main +) + target_link_libraries(nav_terrain_test PRIVATE cvc @@ -1969,6 +1978,7 @@ 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(ph_se2_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) target_compile_features(physics_parity_test PRIVATE cxx_std_17) @@ -2417,6 +2427,7 @@ 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(ph_se2_test) cvc_discover_tests(nav_terrain_test) cvc_discover_tests(nav_binding_test) cvc_discover_tests(physics_parity_test) diff --git a/src/cvc/tests/data/ph_se2_golden.json b/src/cvc/tests/data/ph_se2_golden.json new file mode 100644 index 000000000..237fb6fd4 --- /dev/null +++ b/src/cvc/tests/data/ph_se2_golden.json @@ -0,0 +1 @@ +{"format":"cvc-ph-se2-golden/1","model":{"format":"cvc-ph-se2/1","provenance":{"source":"LieSPHGP SE2_Vehicle/lie_ph (CVC-Lab/LieSPHGP @ 65db994 + the SE(2) package), float64","checkpoint":"checkpoint_final.pkl","checkpoint_sha256":"0d999a21957e7d59","run":"20-10-18-29_lie_ph_se2_humvee-vgp-v2","jax":"0.8.2","optax":"0.2.8"},"control_dim":2,"feature_count":64,"G_input":"twist","V_gp":true,"V_length_scale":50.0,"matern_smoothness":2.5,"matern_length_scale":1.0,"interval":0.020000000000000018,"substeps":1,"twist_scale":[12.290923118591309,0.35159429907798767,0.32184451818466187],"control_scale":[2.429356336593628,0.15309640765190125],"setup":{"D":{"frequencies":[-1.0342187881469727,-0.24865764379501343,-0.11409982293844223,-0.8096145391464233,0.32788965106010437,-0.3652215600013733,0.42222627997398376,-0.16267208755016327,-0.10076577216386795,-0.4221043586730957,-0.626578688621521,-0.4316460192203522,-0.1360386162996292,0.5128836631774902,0.6923832893371582,0.09006177634000778,0.26370641589164734,0.40533947944641113,1.0809624195098877,1.5597180128097534,2.5407602787017822,1.3084698915481567,1.6688073873519897,-0.1820651739835739,-1.1979610919952393,1.7170523405075073,1.3512659072875977,0.05505961552262306,-0.10207583010196686,0.8850380182266235,-1.7348355054855347,-1.714387059211731,0.7889661192893982,-0.6428692936897278,-1.0464195013046265,-0.2506575882434845,0.15924124419689178,0.03035890683531761,-0.08610636740922928,0.7131127119064331,-0.9707621335983276,-0.6099043488502502,1.154556393623352,0.37897759675979614,1.1914136409759521,-1.6391983032226562,1.8287122249603271,-1.2069027423858643,0.5853011012077332,-1.783240795135498,1.1583489179611206,-2.5358426570892334,2.653787136077881,-0.8812981843948364,0.6553343534469604,-0.8941996693611145,-0.5705599188804626,-1.0600043535232544,-1.7262423038482666,3.954671621322632,0.7241567373275757,1.3264907598495483,-2.783160924911499,1.4339169263839722,0.6321730017662048,-0.021980492398142815,-0.3844573199748993,-0.47726181149482727,0.2224900722503662,-0.24530081450939178,-0.8468049168586731,-0.32061031460762024,0.2834169268608093,0.2821618914604187,1.4066046476364136,0.068637415766716,0.44638535380363464,-0.3332032561302185,-1.8893622159957886,-2.7484984397888184,-0.7347099781036377,1.2651904821395874,-0.3207929730415344,-0.3163142204284668,-0.3656505346298218,1.3692946434020996,-0.2962559759616852,-1.2911274433135986,0.7748656272888184,-1.6598930358886719,-1.103315830230713,0.3080892860889435,0.7744307518005371,-0.11095956712961197,0.50914067029953,-0.40079349279403687,0.958842396736145,1.5961865186691284,-1.4842662811279297,0.7066019177436829,-0.3604499399662018,0.04417146369814873,0.3111487030982971,0.790237307548523,-0.6117162704467773,-0.7709850072860718,-0.13537909090518951,-0.16796265542507172,-0.8751378059387207,-1.4989228248596191,1.2261242866516113,-1.7568439245224,-0.7992311716079712,-2.9256489276885986,-0.5587509870529175,-1.5510073900222778,-0.2779901325702667,2.3920748233795166,-0.11681293696165085,2.3238182067871094,-2.897651195526123,1.1192474365234375,-1.4260562658309937,-3.5257575511932373,-0.3038647770881653,-1.1053675413131714,1.4798086881637573,2.081998348236084,2.927602529525757,0.04816140979528427,1.456353783607483,-0.3684508204460144,-1.0120859146118164,2.504969596862793,-0.09430015087127686,-0.8543103933334351,0.3404541611671448,1.4252746105194092,1.0150800943374634,-1.4676167964935303,0.4059446454048157,-1.1725106239318848,0.05049454793334007,0.12258540093898773,-1.6353394985198975,-2.1689131259918213,3.880004644393921,-0.6258113980293274,-0.07911837846040726,0.7145555019378662,-1.4959900379180908,0.8514469265937805,-0.5138803124427795,-0.02638201415538788,-1.401387095451355,-0.7070001363754272,-1.7983897924423218,1.5098671913146973,-1.0588747262954712,-0.3331710398197174,-0.007176711689680815,-0.16628868877887726,0.5915047526359558,1.4675604104995728,-0.4788692891597748,-1.246126413345337,-1.7748388051986694,1.1231592893600464,-0.16522331535816193,-0.32866451144218445,0.8189387321472168,0.4145035743713379,0.7250113487243652,-0.19108504056930542,-1.31224524974823,0.7393446564674377,-0.5947557687759399,0.3535582721233368,0.7421056628227234,-0.11192713677883148,0.14849001169204712,0.019994933158159256,0.9361298680305481,-0.542559802532196,-0.26961085200309753,0.7296697497367859,0.309438019990921,-0.9815306067466736,-0.056988392025232315,-0.7202655673027039,0.12964904308319092,-0.1401236653327942],"phases":[0.6622909903526306,2.0135011672973633,0.701740026473999,3.563077926635742,0.04859903082251549,1.1550711393356323,0.19370926916599274,5.396967887878418,4.9954514503479,5.8161940574646,4.558804512023926,3.358604669570923,5.332942962646484,4.53009557723999,0.16537556052207947,3.248685359954834,3.5226447582244873,5.936842918395996,3.7388648986816406,5.825463771820068,3.5850367546081543,1.951569676399231,2.5407979488372803,4.443055629730225,3.8041751384735107,1.9620416164398193,1.1115293502807617,4.545957565307617,0.6156266331672668,0.7314826250076294,1.0152039527893066,0.4278301000595093,5.870263576507568,2.545881509780884,3.3646109104156494,5.449133396148682,0.06876023858785629,2.341740131378174,0.18157300353050232,6.141096115112305,5.995807647705078,4.5871262550354,0.9183436632156372,3.595180034637451,4.872119426727295,3.366506576538086,0.19106075167655945,0.7877987623214722,6.020939350128174,6.239777088165283,5.6810150146484375,2.463674306869507,4.028932571411133,4.8583807945251465,5.991124629974365,5.5603837966918945,2.0829880237579346,4.149784564971924,3.885798215866089,5.647335529327393,2.3546202182769775,5.415251731872559,6.250678062438965,2.2023725509643555]},"G":{"frequencies":[-1.6322349309921265,0.753013551235199,-0.2732428312301636,-1.463238000869751,-0.7771066427230835,-0.17803440988063812,-2.2680978775024414,-0.47845199704170227,0.9496526122093201,-0.7576936483383179,-4.892375469207764,-2.6026859283447266,-2.7581255435943604,3.2523281574249268,-0.48980894684791565,0.6936832070350647,-0.6999497413635254,-0.9145890474319458,-0.7621150016784668,-1.360756516456604,0.3592242896556854,0.20316502451896667,1.885694980621338,-0.545159101486206,-0.8001859188079834,-0.8810426592826843,-1.155701994895935,1.4351754188537598,1.5620347261428833,-0.522300660610199,-1.6597391366958618,-0.37021028995513916,-0.5611467361450195,-0.14787061512470245,0.8005366325378418,-2.046806812286377,0.2632206678390503,0.756227433681488,-0.49021193385124207,-0.5192431211471558,0.6796483397483826,0.5346143841743469,-3.0942325592041016,0.13763876259326935,-0.3464253842830658,2.602701425552368,1.0730326175689697,-0.07997343689203262,-1.9467374086380005,-0.1755829006433487,-0.5476564764976501,-1.474759817123413,-0.2107570469379425,1.157562494277954,0.08131205290555954,-0.2979857623577118,-0.3556053042411804,-0.4813309609889984,-0.9071289896965027,0.7063530683517456,-1.7088828086853027,1.3925199508666992,-2.465446949005127,1.7863059043884277,0.43778470158576965,0.7074865102767944,0.4798715114593506,0.8551780581474304,0.6191495656967163,0.9983258843421936,-0.8724188804626465,1.0370228290557861,-0.8436298370361328,0.2791432738304138,-0.09370028972625732,0.6223551034927368,0.002868177369236946,0.6348597407341003,-1.1783212423324585,-0.3261532783508301,0.1479654163122177,0.006617859937250614,-1.130129098892212,1.6041202545166016,0.3359592854976654,-0.06426860392093658,1.4311059713363647,-0.7382051348686218,-1.2140487432479858,1.1965833902359009,-0.4430338144302368,0.12050163000822067,0.06727603822946548,0.17072618007659912,0.6656157374382019,0.4661948084831238,0.031977586448192596,-1.1679913997650146,-0.6206498742103577,-0.37962180376052856,0.06519113481044769,-0.35486429929733276,-0.9449909925460815,-1.138994812965393,-0.34326398372650146,-0.8021986484527588,0.07150379568338394,-0.06017499789595604,-0.32472988963127136,1.0061968564987183,-0.026383671909570694,0.6193994879722595,-0.04731758311390877,-2.7993669509887695,1.8542860746383667,1.2638965845108032,2.3294448852539062,2.8098793029785156,-4.504210472106934,0.6604323983192444,-0.7037103772163391,-1.3322292566299438,0.5407853126525879,1.1050957441329956,-0.8716010451316833,0.9072415828704834,1.4052174091339111,0.33513307571411133,0.5611130595207214,0.056113261729478836,0.12556275725364685,1.2636198997497559,3.264293670654297,2.6372416019439697,-1.2273132801055908,0.8623912930488586,1.1178377866744995,-1.8125032186508179,-0.4733419120311737,-0.1782117635011673,-1.1164436340332031,0.7037644982337952,1.0427823066711426,0.5131880640983582,-1.2208691835403442,1.3694322109222412,-0.4469498097896576,-0.18257279694080353,-0.22598128020763397,0.9922506213188171,-0.05264180526137352,-1.7696096897125244,0.31702879071235657,-0.2257080376148224,0.07091371715068817,-0.09529060870409012,-0.8070605397224426,-0.036380089819431305,-1.583725929260254,1.4753481149673462,-2.451756238937378,-0.9841103553771973,0.6416906714439392,0.8385003209114075,0.45871198177337646,0.7734227180480957,0.9365972876548767,0.7990297675132751,0.05818420648574829,0.16148166358470917,-0.3028990924358368,-1.015514612197876,-1.3765649795532227,0.07631266117095947,0.6911370754241943,0.2844720780849457,-0.7759150862693787,0.6324192881584167,0.05109211802482605,-1.863279938697815,0.5333992838859558,-1.3605586290359497,-0.330680787563324,-3.853147506713867,1.2370426654815674,1.6834181547164917,-2.5067098140716553,-4.432697772979736,-0.7960687875747681,-0.18112502992153168,0.4525585472583771,1.220003604888916],"phases":[3.320016860961914,0.23778125643730164,4.442488670349121,1.2989184856414795,5.726881504058838,2.9079108238220215,1.8395485877990723,0.08981053531169891,4.920551300048828,2.962012767791748,0.13006629049777985,4.129779815673828,5.845130443572998,2.717320442199707,1.8601981401443481,1.2942866086959839,0.7628446221351624,0.4466865360736847,5.747534275054932,3.8490095138549805,5.546431064605713,2.3997788429260254,0.8434280753135681,4.530018329620361,5.6593756675720215,5.3849310874938965,1.199222445487976,2.2736823558807373,1.668360710144043,3.8092362880706787,2.760378360748291,0.9226190447807312,3.029160499572754,4.564019680023193,0.07286933064460754,3.362637996673584,0.7163720726966858,2.4022088050842285,1.7454657554626465,0.1484014093875885,0.4268586337566376,3.5395379066467285,5.605421543121338,1.294582486152649,0.4464446008205414,1.5211164951324463,3.417339324951172,1.1113046407699585,4.624102592468262,4.404311180114746,0.21991726756095886,5.262454509735107,5.14784049987793,3.7471556663513184,1.8990764617919922,0.5622886419296265,1.5641915798187256,0.7208863496780396,5.119518756866455,2.78499698638916,1.0418418645858765,5.578840255737305,3.376727819442749,1.6593276262283325]},"V":{"frequencies":[-2.4732728004455566,-0.9350748658180237,1.4863007068634033,-1.0328333377838135,0.9091752767562866,-2.064310073852539,-1.1693878173828125,0.697191059589386,-0.940483570098877,-0.9851300120353699,-0.29906490445137024,0.025659283623099327,-0.40199515223503113,-1.6481746435165405,-1.143470287322998,1.9174878597259521,3.5581226348876953,2.1940948963165283,-0.06966820359230042,7.026697158813477,0.15388540923595428,-1.4417475461959839,-0.7796851992607117,-0.5400035977363586,0.7391716837882996,1.2950482368469238,0.48973098397254944,0.7585827112197876,-2.221118688583374,-4.03582239151001,1.9843674898147583,0.36960580945014954,2.3167197704315186,-1.953476071357727,2.083259105682373,-6.250360012054443,-0.45665353536605835,-0.32923784852027893,-1.476590633392334,-0.3773915767669678,-0.6123110055923462,1.6613759994506836,1.471958875656128,0.3514120280742645,-3.7334389686584473,-4.9189133644104,-0.8915342092514038,-0.3399175703525543,-0.7848415970802307,-0.5293780565261841,1.5196171998977661,0.3635600507259369,-0.6194587349891663,-0.7092453837394714,0.010581391863524914,0.29340222477912903,-0.6337217092514038,-0.75350022315979,-1.442506194114685,-0.44558992981910706,-0.2457050383090973,-0.23007988929748535,0.5241067409515381,-1.6170631647109985,0.42443934082984924,-0.2430412322282791,0.5116928219795227,-0.9950083494186401,0.4679160714149475,0.04651676118373871,-0.5993456244468689,0.767467737197876,0.1564277708530426,-0.3027668595314026,0.2551767826080322,0.6892732381820679,-0.05514207482337952,1.143007516860962,-0.15012215077877045,-0.4134480655193329,-0.49064192175865173,1.4564194679260254,1.3307846784591675,1.2280545234680176,-1.1704680919647217,-1.4081618785858154,-0.32234877347946167,0.6535465121269226,-0.24933630228042603,0.8514046669006348,-0.5334019660949707,0.3971366286277771,-0.2768160104751587,-2.104642152786255,1.720550775527954,1.17997407913208,0.7272934317588806,1.9386200904846191,0.6451432108879089,-0.18995119631290436,0.6990923285484314,0.7554796934127808,-0.5420261025428772,0.3031386137008667,0.73537278175354,1.30916166305542,-0.3074478507041931,2.4477744102478027,0.1300067901611328,0.4982741177082062,-1.864027738571167,-0.5166656970977783,-0.7006549835205078,1.3565374612808228,-1.4306681156158447,-0.8789982795715332,2.3733603954315186,2.6639914512634277,-0.3404211699962616,0.9790029525756836,-0.6793964505195618,1.0918086767196655,0.2998593747615814,-0.02037688158452511,-0.21440164744853973,0.8908513188362122,0.025154490023851395,0.7600708603858948],"phases":[3.0787017345428467,2.644871234893799,3.9541261196136475,5.951596260070801,2.449460983276367,2.7337591648101807,0.5867963433265686,1.6636223793029785,0.09803920239210129,3.5387814044952393,2.042447566986084,5.3860626220703125,1.7766104936599731,0.7921295762062073,5.259490013122559,3.851210117340088,3.9989163875579834,3.034080743789673,5.514401912689209,2.735818862915039,3.6110246181488037,0.6018994450569153,1.0810190439224243,3.4943056106567383,5.99398136138916,0.5453848838806152,4.339414596557617,1.8257278203964233,2.149547576904297,2.874218702316284,5.107766151428223,6.252526760101318,1.2168961763381958,1.9969052076339722,5.61834716796875,2.8579890727996826,0.26505061984062195,3.005270004272461,0.6239212155342102,4.925908088684082,3.5996711254119873,3.7527260780334473,5.073274612426758,3.154093027114868,1.788262128829956,4.058824062347412,2.5297861099243164,0.1259804219007492,3.2582051753997803,1.1732631921768188,5.072357654571533,2.045257806777954,0.9501595497131348,2.7862064838409424,4.98750114440918,2.7775869369506836,0.05528322979807854,4.3781938552856445,2.474517822265625,2.3860652446746826,2.3238027095794678,1.5335193872451782,1.7018595933914185,2.1954944133758545]}},"params":{"Minv":{"level":[0.05339641124010086,0.0337614081799984,0.3678686320781708,0.006756121758371592,0.03741947561502457,0.1038820669054985]},"D":{"mean":[-0.5077129602432251,0.3942815959453583,-0.07967714220285416,0.06046183034777641,0.1526423990726471,-0.6071742177009583,-0.1402173936367035,-0.2013237625360489,-0.29244720935821533,-0.27926790714263916,0.19126303493976593,-0.16270984709262848,-0.4844222664833069,0.6150729656219482,0.19157394766807556,-0.4501790404319763,-0.05461813881993294,-0.5866435766220093,0.3319311738014221,-0.38613736629486084,0.3028697073459625,0.15570785105228424,-0.187668576836586,-0.009707045741379261,-0.6561861038208008,0.26114383339881897,-0.19694609940052032,-0.05157717317342758,0.17214009165763855,0.07529645413160324,-0.4810406565666199,0.20374979078769684,0.19486208260059357,0.7437739968299866,0.4165053069591522,-0.29403597116470337,0.07363737374544144,0.0763452798128128,0.0007418137392960489,-0.056834626942873,-0.19072283804416656,0.27496108412742615,-0.05079587921500206,0.16737514734268188,0.4599605202674866,0.036251168698072433,-0.05215505138039589,-0.03200376406311989,0.10222088545560837,-0.12640886008739471,-0.032655343413352966,-0.3207458257675171,-0.055926818400621414,-0.43888452649116516,-0.3592710793018341,0.14143407344818115,0.265749454498291,-0.19718977808952332,0.04923625290393829,-0.03588088974356651,-0.09850680828094482,-0.22581011056900024,-0.17389506101608276,0.06351668387651443,0.08902717381715775,-0.06385131180286407,-0.07071155309677124,-0.15491388738155365,0.03622272238135338,0.05857796594500542,-0.04429919272661209,0.199705570936203,-0.6569218635559082,0.3268795609474182,0.007161120418459177,0.2846262753009796,0.3456653654575348,-0.5701664686203003,0.01403754111379385,0.006674033589661121,-0.35651686787605286,0.28981471061706543,-0.0076582785695791245,0.23918387293815613,-0.08798199892044067,-0.16638411581516266,-0.05211683362722397,0.2527776062488556,-0.20418009161949158,-0.10570115596055984,-0.1955687403678894,0.2717529237270355,0.09273175150156021,0.009221222251653671,-0.05866514891386032,0.24907343089580536,-0.10609011352062225,-0.007262028753757477,-0.014379591681063175,-0.013686460442841053,0.09618735313415527,0.23733821511268616,-0.15912367403507233,-0.02050282433629036,-0.07719327509403229,0.09923508018255234,0.13726907968521118,-0.12226828932762146,0.09730309247970581,-0.18730708956718445,0.11135601252317429,0.4027084708213806,0.01560478750616312,0.2033270299434662,0.33296823501586914,-0.046175118535757065,-0.15907926857471466,-0.0863719955086708,0.17788664996623993,-0.44411125779151917,0.2411523312330246,0.1070818305015564,-0.185390442609787,0.051575955003499985,-0.13763095438480377,0.30998140573501587,-0.06356630474328995,-0.09303271025419235,0.03762068599462509,0.08670571446418762,-0.08670220524072647,0.3451317548751831,0.03078432008624077,-0.19182643294334412,-0.3473505675792694,0.021873759105801582,0.010004986077547073,0.48555228114128113,0.15524643659591675,-0.11172665655612946,-0.10870981961488724,0.31938424706459045,-0.24643583595752716,-0.07109938561916351,0.1725511997938156,0.11468871682882309,-0.041088465601205826,-0.2025655210018158,0.17344234883785248,0.08229111135005951,0.22358013689517975,0.049160830676555634,-0.06143099442124367,-0.3953605890274048,-0.05425865948200226,0.10758184641599655,0.010058666579425335,-0.02162577398121357,0.02003653720021248,0.07638434320688248,-0.029974106699228287,-0.12874045968055725,-0.5656710863113403,0.151067852973938,-0.4856506288051605,0.54963618516922,0.3786013126373291,-0.12988561391830444,-0.17812152206897736,-0.13042432069778442,-0.050330400466918945,0.033435337245464325,-0.023096498101949692,-0.2463635951280594,-0.3884522318840027,0.16792942583560944,0.20211882889270782,0.10492094606161118,-0.12567494809627533,0.2602773606777191,-0.6009607315063477,0.4072817265987396,-0.10646001994609833,0.256209135055542,0.16191399097442627,-0.027493447065353394,-0.10426242649555206,-0.1676068902015686,0.2902580797672272,0.08755868673324585,0.07638983428478241,-0.2869662046432495,-0.010864494368433952,-0.20134073495864868,0.2244509905576706,-0.03653349354863167,0.10187671333551407,0.08964942395687103,0.37613537907600403,-0.39327338337898254,-0.08033178746700287,-0.16471172869205475,-0.15025974810123444,0.527245283126831,0.14157365262508392,0.23281307518482208,-0.3484758138656616,0.017199214547872543,-0.10789971798658371,0.00022283534053713083,0.1652977466583252,0.5332197546958923,0.875927746295929,0.5710338354110718,-0.21746663749217987,-0.5698456168174744,0.037142571061849594,-0.09992854297161102,0.2715757191181183,-0.021388111636042595,0.004526602569967508,0.008907348848879337,-0.18979626893997192,0.07406173646450043,0.009938365779817104,-0.0876835510134697,0.003245163708925247,0.02523166500031948,-0.0660945475101471,0.04631359875202179,0.20429320633411407,0.10473720729351044,-0.023075317963957787,-0.14263030886650085,0.004060947801917791,-0.24635279178619385,0.01353098638355732,0.06251098215579987,-0.12388765066862106,-0.14848119020462036,0.16726163029670715,0.44089260697364807,0.33416369557380676,-0.016442887485027313,-0.013386430218815804,0.25041618943214417,-0.09634046256542206,0.13666808605194092,-0.19278116524219513,-0.13426895439624786,0.0684635192155838,0.07114294916391373,-0.057346463203430176,-0.033681489527225494,0.005046794656664133,0.010338137857615948,0.02105015330016613,-0.1441369652748108,0.11755882948637009,0.11895223706960678,0.02769995667040348,0.04849385470151901,0.03880629688501358,0.3000563681125641,-0.05745679885149002,0.037344686686992645,-0.11067119240760803,0.19681425392627716,0.07494986802339554,-0.01987367309629917,0.22072619199752808,0.12358520179986954,0.07949165254831314,0.01218754518777132,-0.10476547479629517,-0.042723849415779114,0.05932924896478653,-0.007573401089757681,-0.06282304227352142,-0.12805651128292084,0.08273269981145859,-0.3361470103263855,-0.5483277440071106,0.4118667244911194,-0.053156908601522446,0.06391236931085587,0.002117670141160488,-0.6247889995574951,-0.2520023286342621,-0.1742253601551056,0.030123041942715645,-0.08532121032476425,-0.1177532747387886,0.044355910271406174,-0.2998614013195038,0.4019499719142914,0.3367939293384552,-0.2962035536766052,-0.03284742310643196,-0.3750978708267212,0.17605620622634888,0.2601161599159241,0.3098885416984558,0.01871441677212715,-0.09641101956367493,-0.31865784525871277,0.13227801024913788,-0.08333171159029007,0.1612975001335144,-0.058698270469903946,0.23556654155254364,0.08046960085630417,-0.07103881239891052,0.03727942705154419,-0.11353499442338943,0.014285475015640259,0.07141517847776413,0.21885204315185547,0.46691709756851196,-0.1455012559890747,0.4780203700065613,0.5550495386123657,-0.0241110697388649,0.218134343624115,-0.15234795212745667,-0.040314558893442154,0.1434108167886734,-0.08588448911905289,0.10218988358974457,-0.2243887186050415,0.02691834792494774,-0.12439211457967758,0.001769664348103106,0.004112930037081242,-0.34051916003227234,-0.09928452223539352,0.012350361794233322,0.2482733428478241,-0.11008499562740326,0.5587536692619324,-0.09223993867635727,-0.08152811974287033,-0.0716298520565033,0.04090172052383423,-0.167069673538208,0.016912970691919327,0.06779750436544418,0.11896070092916489,0.08086153119802475,0.08059965819120407,-0.31231489777565,-0.24626679718494415,0.11664924025535583,0.251564621925354,-0.06750006973743439,0.01801021583378315,0.09340086579322815,0.09564787894487381,0.19119969010353088,-0.24672827124595642,0.40898236632347107,-0.25895631313323975,-0.17414383590221405,0.333237886428833,-0.05906466767191887,0.07613237202167511,-0.02474997378885746,0.16055116057395935,0.24771928787231445,-0.6031357049942017,-0.07719586789608002,-0.1027187705039978,0.021628612652420998,0.07876516878604889,0.024364052340388298,0.008615111000835896,0.10871566832065582,-0.29356807470321655,-0.23553678393363953,-0.16414307057857513,-0.7079631090164185,-0.24436073005199432,0.1380506008863449,0.0631822943687439],"log_std":[-4.996837615966797,-5.047537326812744,-5.149330139160156,-5.084649562835693,-5.081878662109375,-4.785960674285889,-4.89841890335083,-4.8514180183410645,-4.580599308013916,-4.902632713317871,-5.113712787628174,-4.3310346603393555,-4.879497528076172,-4.529383182525635,-4.998393535614014,-4.784609794616699,-5.019876956939697,-4.4968037605285645,-4.9720964431762695,-4.735290050506592,-4.890305995941162,-4.977349281311035,-5.212104320526123,-4.5514349937438965,-4.963852882385254,-4.946896076202393,-5.1948113441467285,-5.005105972290039,-5.1277875900268555,-4.61916971206665,-4.870772361755371,-4.858957767486572,-4.983662128448486,-4.821446418762207,-4.810375690460205,-4.71835470199585,-4.917464256286621,-4.719168186187744,-5.17326021194458,-4.861770153045654,-5.186706066131592,-4.645802974700928,-4.983221054077148,-4.884627342224121,-5.095602989196777,-4.795456886291504,-5.111932277679443,-4.451730251312256,-5.012603759765625,-4.742806911468506,-5.244102954864502,-4.948520183563232,-5.1359639167785645,-4.6697468757629395,-4.934665203094482,-4.825124263763428,-4.925990104675293,-4.910012722015381,-4.983833312988281,-4.516378402709961,-5.0030436515808105,-4.874771595001221,-5.1306538581848145,-4.788467884063721,-5.178603649139404,-4.519491672515869,-4.938313961029053,-4.893383026123047,-4.960909366607666,-4.952142715454102,-5.041473865509033,-4.46645450592041,-4.954657077789307,-4.910126686096191,-4.8832478523254395,-4.959931373596191,-5.031613826751709,-4.736020565032959,-4.862293720245361,-4.951662063598633,-4.942718505859375,-5.029500961303711,-5.274571895599365,-4.609683990478516,-4.951093673706055,-4.751802921295166,-5.057819366455078,-4.986189842224121,-5.2100114822387695,-4.809177875518799,-4.9841508865356445,-5.011652946472168,-5.051380634307861,-4.960643291473389,-5.1269307136535645,-4.6612935066223145,-4.950726509094238,-4.936105728149414,-5.123089790344238,-4.883342742919922,-5.100924968719482,-4.788808345794678,-4.948946475982666,-4.887777328491211,-5.0171661376953125,-4.915505886077881,-5.13768196105957,-4.377565860748291,-4.885899066925049,-4.6551337242126465,-5.23330545425415,-4.886796951293945,-5.171762466430664,-4.415528297424316,-4.935211658477783,-4.847515106201172,-5.091218948364258,-4.9058403968811035,-5.185459136962891,-4.7094526290893555,-4.938620567321777,-4.83989143371582,-5.214753150939941,-4.946015357971191,-5.093501091003418,-4.65968656539917,-4.945995330810547,-4.768648147583008,-5.109734535217285,-4.951906681060791,-5.135313987731934,-4.647885799407959,-4.955707550048828,-4.709932804107666,-5.159799575805664,-4.901244163513184,-5.127753734588623,-4.722503185272217,-4.924498558044434,-4.836742877960205,-5.072536945343018,-5.001880645751953,-5.074484825134277,-4.612882137298584,-4.9538350105285645,-4.952520847320557,-5.21162748336792,-4.980715751647949,-5.062199115753174,-4.7325568199157715,-4.955205917358398,-4.866863250732422,-4.816762447357178,-4.917372703552246,-4.995599269866943,-4.580714225769043,-4.909700393676758,-4.834953784942627,-5.16994571685791,-4.934049606323242,-5.267879009246826,-4.637861728668213,-5.015071392059326,-5.01265811920166,-5.112275123596191,-4.99193811416626,-5.09466552734375,-4.7429118156433105,-4.99931526184082,-4.904232978820801,-5.02947473526001,-4.9439005851745605,-5.247103691101074,-4.443582534790039,-4.929415225982666,-4.7501726150512695,-4.9531073570251465,-4.997763156890869,-5.2028489112854,-4.609014987945557,-4.875265121459961,-4.743017673492432,-4.893904685974121,-4.952524662017822,-4.9446539878845215,-4.671578884124756,-4.984828472137451,-4.858651638031006,-5.331661701202393,-4.808559894561768,-5.049993991851807,-4.683816909790039,-4.872158527374268,-4.809431076049805,-5.084985733032227,-4.91129207611084,-5.15625524520874,-4.613544940948486,-4.927660942077637,-4.868190765380859,-5.0071001052856445,-5.028692722320557,-5.197424411773682,-4.754711151123047,-4.976065635681152,-4.835312843322754,-5.188666820526123,-4.997270584106445,-5.09881591796875,-4.595794200897217,-4.82856559753418,-4.5516276359558105,-4.797001361846924,-4.766721725463867,-4.785158634185791,-4.142914772033691,-4.942221164703369,-4.772607803344727,-5.116698265075684,-4.83260440826416,-5.219979763031006,-4.648238658905029,-4.99506139755249,-4.84705114364624,-5.269472599029541,-4.93174409866333,-5.1444196701049805,-4.544424057006836,-4.972264289855957,-4.934579372406006,-5.240677833557129,-4.986849308013916,-5.203749179840088,-4.697975158691406,-4.896591663360596,-4.732697010040283,-5.164917945861816,-4.9197001457214355,-5.17384147644043,-4.674450874328613,-4.952895641326904,-4.802824974060059,-5.042107105255127,-4.811776161193848,-5.230026721954346,-4.549141883850098,-4.965043544769287,-5.051380634307861,-5.076693058013916,-4.927885055541992,-5.225490570068359,-4.456277847290039,-4.974842071533203,-4.852508068084717,-5.173525333404541,-5.058469295501709,-5.180630683898926,-4.592512607574463,-4.92156982421875,-4.910032749176025,-4.875988960266113,-4.935262680053711,-5.121680736541748,-4.628632068634033,-4.980432510375977,-4.73817253112793,-5.029890060424805,-5.015472888946533,-5.182154178619385,-4.631508827209473,-4.969130516052246,-4.986622333526611,-5.05977201461792,-4.921027660369873,-5.229193210601807,-4.494158744812012,-4.899372100830078,-4.7513203620910645,-4.876585006713867,-4.83797550201416,-5.140792369842529,-4.628559112548828,-5.00227689743042,-4.983485221862793,-5.120603561401367,-4.961145401000977,-5.31196403503418,-4.917133808135986,-4.908939838409424,-4.8360819816589355,-5.229794502258301,-4.8631110191345215,-5.1326470375061035,-4.670746803283691,-4.91620397567749,-4.630568027496338,-5.007404327392578,-4.950438976287842,-5.056580543518066,-4.284328937530518,-4.938003063201904,-4.939512729644775,-5.135314464569092,-4.878323554992676,-5.032896995544434,-4.604923248291016,-4.935575485229492,-4.8545050621032715,-4.9291300773620605,-4.964080810546875,-5.0681538581848145,-4.5556793212890625,-4.9194440841674805,-4.663166522979736,-5.23483419418335,-4.811398029327393,-5.159553527832031,-4.446797847747803,-4.6556010246276855,-4.386896133422852,-4.502273082733154,-4.693727970123291,-4.848911762237549,-4.035583972930908,-4.9725446701049805,-4.956758975982666,-5.053884506225586,-4.8708672523498535,-5.2712907791137695,-4.62660026550293,-5.00145959854126,-5.118577003479004,-5.192661285400391,-4.957569122314453,-5.155188083648682,-4.696991443634033,-4.907733917236328,-4.965114116668701,-5.1311564445495605,-4.934189796447754,-5.180129528045654,-4.620665550231934,-4.911547660827637,-4.883821964263916,-5.031183242797852,-4.7950310707092285,-4.994411945343018,-4.558115005493164,-4.924315452575684,-4.842296123504639,-5.0547099113464355,-4.905458450317383,-5.2208638191223145,-4.490334987640381,-4.935340404510498,-4.683776378631592,-5.2996721267700195,-4.892334938049316,-5.170383930206299,-4.652858734130859,-5.02633810043335,-4.930722236633301,-5.043995380401611,-4.967798709869385,-4.993383407592773,-4.696078300476074,-4.967305660247803,-4.812968730926514,-5.0975823402404785,-4.926558017730713,-5.0693840980529785,-4.473304748535156,-4.970005989074707,-4.873239040374756,-4.978261947631836,-5.0610671043396,-5.103586673736572,-4.660257339477539,-4.749679088592529,-4.647808074951172,-4.719611167907715,-4.7409186363220215,-4.990137577056885,-4.322896957397461],"level":[-0.6018968820571899,0.4214533865451813,0.45419400930404663,-0.006864957045763731,-0.028100760653614998,0.004257347900420427]},"G":{"mean":[0.07683487236499786,-0.7216967940330505,0.5511465668678284,0.9792677164077759,0.1910201609134674,1.3742754459381104,0.050429318100214005,-0.15590082108974457,0.21553243696689606,-0.26847031712532043,0.15406151115894318,-0.4661594033241272,-0.31752586364746094,-0.008827519603073597,0.15955427289009094,-0.6099947690963745,0.012257156893610954,-0.4058452248573303,-0.06678454577922821,0.019472265616059303,-0.07808162271976471,-0.0959533303976059,0.015082167461514473,0.007150806020945311,0.10937588661909103,0.22149018943309784,-0.12350160628557205,-0.2711956202983856,-0.056295398622751236,-0.27720892429351807,-0.23592409491539001,-0.021567335352301598,0.2441619634628296,0.24842457473278046,0.12261882424354553,0.07432394474744797,0.14869940280914307,0.06992201507091522,-0.19340762495994568,0.5719677209854126,-0.011395750567317009,0.7878996133804321,-0.2238294929265976,-0.03305339068174362,0.22351452708244324,0.09206391125917435,-0.047603998333215714,0.2213180959224701,-0.16463406383991241,0.10049885511398315,0.10714113712310791,-0.17465808987617493,0.15349417924880981,-0.5007424354553223,0.2953735888004303,0.1719360053539276,-0.5796095132827759,0.20363730192184448,-0.1420842409133911,0.3268704414367676,0.005336761474609375,0.001263745711185038,0.15553955733776093,-1.0784213542938232,0.08768504858016968,-0.8886741399765015,-0.1037675216794014,-0.04827668145298958,-0.29829514026641846,0.7271600961685181,-0.13540422916412354,-0.05073335021734238,0.37705302238464355,0.02162519283592701,-0.07401502132415771,0.6154209971427917,-0.01260851975530386,0.4195367991924286,-0.16865339875221252,-0.001769362948834896,0.05953354388475418,0.44898322224617004,0.011274958029389381,0.3550838530063629,0.24865715205669403,0.05170203000307083,0.09459208697080612,2.0050578117370605,-0.019296588376164436,0.8900583386421204,0.26572185754776,0.3065869212150574,0.051553890109062195,-1.2888416051864624,0.0028435499407351017,-1.0188583135604858,0.35370275378227234,-0.1939064860343933,0.23615917563438416,-0.09891324490308762,0.07860036194324493,-0.17299073934555054,0.29842624068260193,0.35749778151512146,-0.4966365098953247,-0.9932572841644287,-0.24249237775802612,-0.47039029002189636,0.7716863751411438,0.04656063765287399,-0.14600101113319397,0.3719610273838043,-0.07336986064910889,0.4957761764526367,-0.3163711428642273,-0.2190166711807251,-0.0836753249168396,-0.8328496217727661,0.02809666469693184,-0.5337985754013062,0.10108951479196548,-0.020602989941835403,-0.20407266914844513,-1.4697271585464478,0.0692891925573349,-1.099905252456665,-0.14390933513641357,0.1594536304473877,-0.32423579692840576,0.47582876682281494,-0.02844109944999218,-0.03717454522848129,-0.0832332894206047,0.13352520763874054,-0.1267409771680832,-0.18656226992607117,-0.10619374364614487,0.027870014309883118,0.32470762729644775,-0.26261255145072937,-0.029085056856274605,1.6074212789535522,-0.2083180695772171,1.5723289251327515,0.1396501064300537,0.22170408070087433,-0.34188953042030334,-0.9474446177482605,-0.016782330349087715,-1.2971947193145752,0.6831005811691284,-0.21592235565185547,0.19659684598445892,0.46643078327178955,0.051501914858818054,0.8520745635032654,0.46639013290405273,-0.19330956041812897,-0.0769667699933052,1.5377980470657349,0.012797866947948933,1.3739492893218994,-0.32145413756370544,0.31919780373573303,-0.09966332465410233,0.3086896538734436,0.06301914155483246,0.22688578069210052,0.1203688308596611,-0.27980583906173706,-0.19414503872394562,0.2760597765445709,0.07980610430240631,-0.3112255334854126,-0.09749135375022888,-0.36198291182518005,-0.16346697509288788,-0.7986176609992981,0.03695698827505112,-0.23182512819766998,-0.6789618134498596,0.06159394979476929,0.05377286300063133,-0.29330286383628845,0.04398675635457039,0.11896951496601105,0.3937804400920868,0.17289161682128906,-0.23918484151363373,0.01740332692861557,-0.17477016150951385,0.3409019112586975,0.06694307178258896,0.11171387135982513,0.0342269241809845,-0.26433512568473816,0.060302894562482834,-0.1517593115568161,-0.7324609160423279,0.2641942799091339,-0.26459094882011414,-1.1027464866638184,-0.06404540687799454,-1.0766749382019043,-0.0462104007601738,-0.13365009427070618,0.03865836560726166,0.04211552441120148,-0.06391977518796921,-0.015589593909680843,-0.6845344305038452,0.08332718908786774,0.07395476847887039,-0.5273882746696472,0.06162272021174431,-0.06851556897163391,0.23996445536613464,-0.08198956400156021,-0.30065086483955383,0.547357976436615,-0.1620921641588211,0.7019781470298767,0.22401221096515656,-0.048667483031749725,0.18795405328273773,0.2110336422920227,-0.08035653084516525,0.0706816017627716,0.0980086401104927,-0.10995833575725555,-0.3600952625274658,0.25306594371795654,0.0584612675011158,-0.2739626169204712,0.13642698526382446,-0.20478428900241852,0.062764473259449,0.034169331192970276,-0.015094202943146229,0.06098935008049011,0.07063379138708115,0.08896411955356598,0.06374166160821915,0.5213262438774109,-0.03685768321156502,0.3980695605278015,-0.11907055974006653,-0.6548770666122437,0.09725320339202881,0.9893075227737427,0.008940005674958229,1.2898969650268555,0.5293169617652893,-0.19627057015895844,0.19122260808944702,0.13200128078460693,0.04624435678124428,0.4895949959754944,-0.09759552776813507,-0.20516365766525269,-0.18161502480506897,-0.27041739225387573,0.07029612362384796,-0.04616134986281395,0.12401426583528519,-0.26382753252983093,0.09969824552536011,-0.35413995385169983,0.06303063035011292,-0.27218276262283325,-0.09974010288715363,-0.48327696323394775,0.2244640588760376,-0.9774520993232727,-0.1564384400844574,-1.0459215641021729,-0.438829243183136,0.13677439093589783,-0.2576295733451843,0.791955828666687,-0.22090482711791992,0.1272886097431183,-0.14166247844696045,0.13409222662448883,-0.07790408283472061,-0.3013826608657837,-0.12531085312366486,-0.3922889828681946,-0.04330155625939369,-0.21628153324127197,0.3396955132484436,-1.1749299764633179,0.04692133888602257,-1.3252718448638916,-0.24233472347259521,-0.03026738204061985,0.061292607337236404,0.06191186234354973,-0.1668456792831421,-0.10607399046421051,-0.02426593378186226,-0.02342410571873188,-0.04433498531579971,0.15376083552837372,0.10310786217451096,0.17459268867969513,0.636020839214325,-0.059555478394031525,-0.16103224456310272,0.14032889902591705,-0.0014138788683339953,-0.24924740195274353,0.24391120672225952,-0.46767306327819824,-0.07508809119462967,-0.2868647277355194,0.16903837025165558,-0.6298049092292786,0.08100105822086334,-0.052869029343128204,0.16817902028560638,0.09556008130311966,-0.009584366343915462,0.22824883460998535,-0.1323794275522232,0.13690555095672607,-0.12006253749132156,-0.39605024456977844,-0.14852730929851532,-0.5508105158805847,-0.2670881748199463,0.1170763373374939,0.0987405776977539,-0.24177460372447968,0.010254363529384136,-0.22352907061576843,0.4446870684623718,-0.3163926899433136,0.32257336378097534,-0.24967774748802185,0.1421239823102951,-0.43886038661003113,0.022230058908462524,-0.13275863230228424,-0.11440478265285492,0.4176238179206848,-0.04490189999341965,0.45422860980033875,0.49895763397216797,0.0523146353662014,-0.11089584976434708,0.6261224746704102,0.14064350724220276,0.7036362290382385,0.5272777676582336,-0.08214205503463745,0.20723959803581238,0.4980108141899109,0.031203262507915497,0.2911876440048218,-0.44980382919311523,-0.08314356207847595,-0.11483781784772873,-0.779528796672821,0.11051762104034424,-0.7892173528671265,-0.008064509369432926,0.3611854016780853,0.03943628817796707,-0.03833266347646713,-0.040571484714746475,-0.3119910657405853,0.000682787096593529,-0.21617260575294495,-0.11287741363048553,0.021876201033592224,0.03181673586368561,0.0067147137597203255,-0.09381701052188873,-0.08384513109922409,-0.08454533666372299,0.22297948598861694,-0.03222363814711571,0.3445635139942169],"log_std":[-4.281789779663086,-4.352255344390869,-4.278665065765381,-4.552434921264648,-4.593936920166016,-4.713186740875244,-4.294738292694092,-4.461863994598389,-4.321507930755615,-4.3754096031188965,-4.374094009399414,-4.548329830169678,-4.412087917327881,-4.4792022705078125,-4.10463285446167,-4.649209499359131,-4.5049357414245605,-4.776200771331787,-4.2472429275512695,-4.2252726554870605,-4.309993743896484,-4.349365711212158,-4.692453384399414,-4.568768501281738,-4.350442409515381,-4.394404888153076,-4.281691074371338,-4.508152008056641,-4.653388023376465,-4.523532390594482,-4.406357765197754,-4.542749404907227,-4.347651958465576,-4.672932147979736,-4.538750648498535,-4.869749069213867,-4.237112045288086,-4.61288595199585,-4.198333263397217,-4.244279384613037,-4.530247688293457,-4.764841556549072,-4.562564373016357,-4.527966499328613,-4.274078845977783,-4.724695205688477,-4.6727070808410645,-4.934545993804932,-4.321147441864014,-4.447629928588867,-4.314279079437256,-4.521103858947754,-4.682596683502197,-4.630243301391602,-4.374367713928223,-4.414073467254639,-4.1135783195495605,-4.655952453613281,-4.600492000579834,-4.657410621643066,-4.2431745529174805,-4.511475563049316,-4.297574043273926,-4.533083915710449,-4.658661365509033,-4.873227596282959,-4.436520099639893,-4.534332752227783,-4.299946308135986,-4.420704364776611,-4.56466007232666,-4.624773979187012,-4.513160705566406,-4.328285217285156,-4.278951644897461,-4.642738342285156,-5.067972660064697,-4.7473859786987305,-4.308473110198975,-4.577667236328125,-4.282331943511963,-4.5358357429504395,-4.603659629821777,-4.803507328033447,-4.392784595489502,-4.54508113861084,-4.158050537109375,-4.508372783660889,-4.58640193939209,-4.849210739135742,-4.336431503295898,-4.573611736297607,-4.391414165496826,-4.593275547027588,-4.640023708343506,-4.76069450378418,-4.23014497756958,-4.281442165374756,-4.387508392333984,-4.117514133453369,-4.652942657470703,-4.575733661651611,-4.519308567047119,-4.381861209869385,-4.315721035003662,-4.5762248039245605,-4.635988235473633,-4.471594333648682,-4.586159706115723,-4.508670806884766,-4.3916425704956055,-4.4782328605651855,-4.754826545715332,-4.763216018676758,-4.652306079864502,-4.54194974899292,-4.337547779083252,-4.395195007324219,-4.689570903778076,-4.7875285148620605,-4.409513473510742,-4.530571937561035,-4.243918418884277,-4.561581611633301,-4.681066513061523,-4.828271865844727,-4.35300350189209,-4.328986167907715,-4.314785957336426,-4.598659515380859,-4.6415863037109375,-4.741457462310791,-4.230329990386963,-4.411123275756836,-4.230157375335693,-4.355348587036133,-4.48961067199707,-4.955726146697998,-4.340108871459961,-4.241026878356934,-4.089325428009033,-4.425859451293945,-4.522592544555664,-4.717109680175781,-4.13511323928833,-4.495460033416748,-3.9383528232574463,-4.327485084533691,-4.219367504119873,-4.604376792907715,-4.48759651184082,-4.450718879699707,-4.425006866455078,-4.622314453125,-4.655797004699707,-4.842947006225586,-4.670686721801758,-4.4908928871154785,-4.45579195022583,-4.624746322631836,-4.701210975646973,-4.793656826019287,-4.541235446929932,-4.421744346618652,-4.278253078460693,-4.651514053344727,-4.65534782409668,-4.80769157409668,-4.351590156555176,-4.445231914520264,-4.27618408203125,-4.554800510406494,-4.581430435180664,-4.78897762298584,-4.675981521606445,-4.420722484588623,-4.458662986755371,-4.468768119812012,-4.664756774902344,-4.756250858306885,-4.6606011390686035,-4.522146701812744,-4.351897716522217,-4.531942844390869,-4.738079071044922,-4.586228370666504,-4.333139419555664,-4.393027305603027,-4.320781707763672,-4.416572570800781,-4.358177185058594,-4.620943546295166,-4.303274631500244,-4.465325355529785,-4.333957672119141,-4.507344722747803,-4.6734466552734375,-4.749033451080322,-4.452231407165527,-4.2784647941589355,-4.168753147125244,-4.3141584396362305,-4.567962646484375,-4.637192726135254,-4.474176406860352,-4.6260786056518555,-4.307993412017822,-4.344953536987305,-4.4606428146362305,-4.76896333694458,-4.738208293914795,-4.637054443359375,-4.411850929260254,-4.677541732788086,-4.829576015472412,-4.862082481384277,-4.510946273803711,-4.514241695404053,-4.555917263031006,-4.5881218910217285,-4.713311195373535,-4.874416828155518,-4.439654350280762,-4.580296993255615,-4.359493732452393,-4.624353885650635,-4.788271427154541,-4.789612293243408,-4.214702606201172,-4.496664524078369,-4.176342964172363,-4.424498081207275,-4.502188682556152,-4.61671257019043,-4.403350830078125,-4.42713737487793,-4.160201549530029,-4.377034664154053,-4.699207305908203,-4.536319732666016,-4.597399711608887,-4.419417858123779,-4.317820072174072,-4.433877944946289,-4.72016716003418,-4.563908100128174,-4.301519393920898,-4.402935028076172,-4.1460041999816895,-4.492307662963867,-4.5355706214904785,-4.713468551635742,-4.364060401916504,-4.300692558288574,-4.290219306945801,-4.224859237670898,-4.525128364562988,-4.493911266326904,-4.231197834014893,-4.480058670043945,-4.34861421585083,-4.5236687660217285,-4.804821968078613,-4.6795735359191895,-4.474606037139893,-4.5937347412109375,-4.279457092285156,-4.355894088745117,-4.653635025024414,-4.714099407196045,-4.447097301483154,-4.364744186401367,-4.196397304534912,-4.498309135437012,-4.6291937828063965,-4.423984050750732,-4.432156085968018,-4.57427978515625,-4.309749126434326,-4.498096466064453,-4.705766201019287,-4.614935398101807,-4.205597877502441,-4.569710731506348,-4.1159234046936035,-4.558963298797607,-4.640238285064697,-4.546894073486328,-4.558395862579346,-4.445748805999756,-4.328676223754883,-4.480822563171387,-4.752261161804199,-4.9003496170043945,-4.402281284332275,-4.605950355529785,-4.327874660491943,-4.661898612976074,-4.656133651733398,-4.799848556518555,-4.488511562347412,-4.513323783874512,-4.402225971221924,-4.41648006439209,-4.755327224731445,-4.6309332847595215,-4.158997535705566,-4.271883010864258,-4.031497001647949,-4.302101135253906,-4.32374382019043,-4.556126117706299,-4.279576301574707,-4.489894390106201,-4.275816917419434,-4.474398612976074,-4.657111167907715,-4.795021057128906,-4.278961181640625,-4.395902156829834,-4.037969589233398,-4.359798908233643,-4.568714618682861,-4.6132965087890625,-4.561958312988281,-4.449991703033447,-4.224268913269043,-4.602438449859619,-4.688261032104492,-4.616482734680176,-4.317709922790527,-4.659637451171875,-4.233206748962402,-4.565982341766357,-4.529277801513672,-4.591994285583496,-3.7118887901306152,-3.6870343685150146,-3.755727767944336,-3.8189783096313477,-4.218265533447266,-4.038433074951172,-4.536793231964111,-4.604719161987305,-4.335142135620117,-4.423733711242676,-4.655956268310547,-4.82828426361084,-4.5599470138549805,-4.435677528381348,-4.3918046951293945,-4.476156711578369,-4.602895259857178,-4.856884002685547,-4.606173038482666,-4.494292736053467,-4.415182113647461,-4.724295139312744,-4.735100746154785,-4.819818019866943,-4.257631301879883,-4.44562292098999,-4.136447429656982,-4.393558979034424,-4.566776275634766,-4.511157989501953,-4.345821857452393,-4.701313018798828,-4.080080509185791,-4.573934078216553,-4.548791885375977,-4.8406081199646,-4.258687973022461,-4.477254390716553,-4.343955039978027,-4.454102039337158,-4.596735954284668,-4.647190570831299,-4.187984943389893,-4.505371570587158,-4.178543567657471,-4.573945045471191,-4.698629856109619,-4.654006481170654],"level":[0.9310189485549927,-0.13321763277053833,0.0639321431517601,1.0739073753356934,0.014718119986355305,0.8844640851020813]},"V":{"mean":[-0.3562813103199005,-0.11483975499868393,-0.2359822541475296,-0.38153308629989624,-0.21336780488491058,0.5831360816955566,-0.18875356018543243,-0.8292854428291321,-0.5033479928970337,0.24824845790863037,-0.23217439651489258,0.10881803929805756,0.7761468291282654,-0.0366569347679615,-0.11979545652866364,-0.49094006419181824,0.14854174852371216,0.4206203520298004,0.8453522324562073,-0.5346438884735107,0.9544311165809631,0.5475111603736877,-0.026165684685111046,-0.17074665427207947,0.00726903323084116,0.4867306649684906,-0.32786592841148376,0.03906463459134102,-0.26213616132736206,-0.4028298556804657,-0.6053956747055054,-0.5013040900230408,-0.09721771627664566,0.11257309466600418,-0.5459024906158447,-0.11788634955883026,-0.0762895941734314,0.8074920177459717,0.47240400314331055,-0.25585952401161194,0.7254817485809326,-0.23592568933963776,0.3695250153541565,0.20123636722564697,0.21774707734584808,0.39854156970977783,0.08467841893434525,0.2760111093521118,-0.17700855433940887,-0.6824360489845276,-0.07289312034845352,0.02646680548787117,0.017810653895139694,-0.19129711389541626,-0.1563120186328888,0.7768072485923767,-0.5659306645393372,0.8180843591690063,-0.10853558778762817,-0.26691633462905884,-0.5467629432678223,-0.9360339641571045,0.019329464063048363,-0.41659027338027954],"log_std":[-1.7351711988449097,-1.2090038061141968,-1.5651737451553345,-0.9690389633178711,-1.113929033279419,-0.08407220244407654,-1.2774312496185303,-1.5815569162368774,-2.230269193649292,-2.6566832065582275,-1.2991691827774048,-0.7482041716575623,-1.169205665588379,-0.723092257976532,-2.2478606700897217,-1.3998993635177612,-1.8075810670852661,-2.5979104042053223,-0.4078602194786072,-1.1816130876541138,-1.2216700315475464,-1.162850260734558,-2.449432849884033,-0.7187802195549011,-0.7892307043075562,-1.1085422039031982,-0.8150710463523865,-0.11610255390405655,-0.9488521814346313,-0.9947740435600281,-0.11800355464220047,-1.2781344652175903,-0.305707186460495,-0.8881601095199585,-0.22494639456272125,-0.795490562915802,-0.12348742038011551,-0.5740246176719666,-1.0232770442962646,-0.2583990693092346,-1.2118390798568726,-1.3615362644195557,-1.2604918479919434,-0.5745342373847961,-0.7035661339759827,-0.5717799067497253,-1.5440889596939087,-1.455535650253296,-1.4246463775634766,-0.44729700684547424,-1.0046885013580322,-0.3963156044483185,-1.2395222187042236,-1.6691628694534302,-0.46993136405944824,-1.2521226406097412,-1.23692786693573,-1.3037742376327515,-2.083632469177246,-0.7878087162971497,-1.0452739000320435,-0.13498051464557648,-0.6972000002861023,-0.61225426197052],"level":[-0.03701963648200035,-0.033443305641412735]},"process":{"log_sigma":[-1.4763426780700684,-1.9329760074615479,-2.3160672187805176]},"likelihood":{"log_sigma":[-2.993077039718628,-2.9938251972198486,-3.085759401321411,-3.0307302474975586]}}},"inverse_mass":[1.1127037034041087,0.0071266796040866725,0.03947184837673655,0.0071266796040866725,1.069900314689926,0.1077019583226187,0.03947184837673655,0.1077019583226187,2.099211811536642],"forward":[{"state":[-232.14573669433594,-96.596435546875,0.5489621758460999,13.956063270568848,-0.006610725075006485,0.07865437120199203,0.8232527375221252,0.041466232389211655],"increment":[0.019052390282354677,0.003240803998066429,-0.0073977325296114465],"vector_field":[11.908899821700354,7.2766648095478565,0.07865437120199203,1.4157964415732853,-0.2679448263410432,-0.691043664904579],"effective_damping":[0.02849449083966413,-0.07303380153358288,0.23644153912824523,-0.020645851171667232,7.330146907309338,-2.0707794569095914,0.1409632507692001,-4.252716021906085,9.197875112012555],"dissipation":[0.02336243180211044,-0.026303975424532753,0.0680608222967755,-0.026303975424532753,7.091930565847199,-2.389226536380251,0.0680608222967755,-2.389226536380251,4.502886726556506],"control_matrix":[1.9204387345617548,-0.19863935610468483,0.013507706680862638,3.2038559233915715,-0.07531927156716209,3.469828252987632],"hamiltonian":99.51449973127365,"potential":11.948732409930232,"grad_potential":[-0.03421590280696529,-0.040422938114008614],"step":[-231.9074050211117,-96.45061016597391,0.5504141892806921,13.984428988678157,-0.010883015196402118,0.0664626365253054],"step_increment":[-231.9072561306443,-96.45048811340006,0.5503531289683441,14.003491316167585,-0.007536628502001013,0.05997131385469893]},{"state":[-1392.1717529296875,69.1368637084961,-3.2716641426086426,-1.4063960313796997,0.26748377084732056,0.029235269874334335,-0.3316589295864105,-0.07701859623193741],"increment":[0.00954650954260215,0.0181827190821225,-0.04285045703375334],"vector_field":[1.3598216849574734,-0.4476408715328499,0.029235269874334335,-0.540004838667867,-2.6152048022028094,0.4982410848332036],"effective_damping":[0.09513046762328728,-0.1322294954042008,0.6118447714071324,-0.07062403535379493,9.35344975255511,-1.5953888858139014,0.05115397986717868,-5.3933470747870365,31.805523585494463],"dissipation":[0.08500453299985423,-0.06922580901238429,0.026321521593348773,-0.06922580901238429,9.048049192743736,-3.0321414857839035,0.026321521593348773,-3.0321414857839035,15.306245908932386],"control_matrix":[1.7574681560663727,0.05305603354544133,0.041932880203200504,0.11044158206682242,0.1259448467547154,0.08376813449470177],"hamiltonian":49.8484367253992,"potential":48.92327338134222,"grad_potential":[-0.04851470582903618,-0.05863210545817746],"step":[-1392.144388648463,69.12834092683188,-3.271047464484767,-1.4173577924145484,0.22138182907639264,0.035417347046724605],"step_increment":[-1392.1445054918495,69.12820839589618,-3.271299771493717,-1.4076007521744007,0.23604429452198125,0.003422125119360578]},{"state":[162.36610412597656,426.194091796875,3.004838466644287,10.484683990478516,-0.3562716245651245,-0.3698457181453705,0.8232653737068176,-0.8269832134246826],"increment":[-0.033964962598207055,-0.014432711713726728,0.0033062333342298274],"vector_field":[-10.338226026670721,1.7823048202074903,-0.3698457181453705,1.3773677967095022,0.19530194171667553,-0.06877191022092591],"effective_damping":[0.08760016965105305,0.1643356258984895,0.21851862776424424,0.1897724087923374,4.135901738151488,-1.029943870244397,0.07845067552224479,-2.6473135931977043,10.423836463502573],"dissipation":[0.07665430441055984,0.17414580961975595,0.02699543866891593,0.17414580961975595,4.0125311786472775,-1.4702398771766172,0.02699543866891593,-1.4702398771766172,5.040519760543375],"control_matrix":[2.049865489341194,-0.2422623083200378,0.04718429996628501,2.8587305806841306,0.0723663559825859,1.9552656728617908],"hamiltonian":29.115320935725588,"potential":-20.482678108532443,"grad_potential":[-0.056695005713461775,-0.03659699229371653],"step":[162.15919718162743,426.23050704549723,2.997431343607095,10.512120051306905,-0.35252575719522283,-0.3711852438641209],"step_increment":[162.15955130631446,426.23058871597823,2.997453372221972,10.478481223877502,-0.3663122248415193,-0.3681096374644191]},{"state":[-1395.794677734375,69.80753326416016,-3.3353099822998047,-0.33116576075553894,-0.03796292468905449,0.02158583328127861,-0.41163167357444763,-0.03747326135635376],"increment":[0.005297335250222114,-0.0068158259789849246,0.0008717605163218808],"vector_field":[0.33227960531744927,-0.026499219570051268,0.02158583328127861,-0.8834336086631732,0.19726785217461182,-0.8831572638110022],"effective_damping":[0.05963327000737991,-0.10652546926494554,0.5388524997003995,-0.01810574676108269,6.187589571135565,-2.013126230570676,0.00385771297759959,-6.292686825029543,29.453899042822382],"dissipation":[0.05364373983970024,-0.017453757809529707,0.0017245040679391684,-0.017453757809529707,6.116766397292258,-3.3111406740085028,0.0017245040679391684,-3.3111406740085028,14.20078104767816],"control_matrix":[1.845733348678424,0.02352092050476945,0.01654425685568075,-0.06351636952852102,-0.029701012529289195,-0.050505967028531384],"hamiltonian":49.10990193904101,"potential":49.05969355510896,"grad_potential":[-0.048732554212715724,-0.059998920425662475],"step":[-1395.7878660363822,69.80694047506974,-3.3349869428118897,-0.34873221440069585,-0.03439668765021795,0.008102994640168512],"step_increment":[-1395.7879061851534,69.80701009437841,-3.3349862597613154,-0.34344485225116206,-0.040816866178219315,0.008500732920257431]},{"state":[263.6922607421875,367.35858154296875,2.3604063987731934,12.73514461517334,0.010670812800526619,0.04798499122262001,0.823250949382782,0.025415463373064995],"increment":[0.03327409152482399,-0.02032540271695764,-0.010919399071160178],"vector_field":[-9.050469373314005,8.959521547400259,0.04798499122262001,1.423926745781861,-0.28899481569840546,-0.43655430982834287],"effective_damping":[0.028873806609493784,-0.0672153365394619,0.2443588347956695,-0.01031657945959609,7.4441640413386745,-2.203162247374209,0.12674533003490443,-4.591024305061963,10.097074470699615],"dissipation":[0.023896322977577402,-0.015916644085036295,0.060744871496369576,-0.015916644085036295,7.215309457649234,-2.5569115868605223,0.060744871496369576,-2.5569115868605223,4.939978465495553],"control_matrix":[1.948136000328766,-0.1726246637312377,0.0009626354300396878,3.160647959685117,-0.053254318630894734,3.349826219745992],"hamiltonian":51.441112767243105,"potential":-21.47723061399416,"grad_potential":[-0.026487298783619405,-0.025331028453053968],"step":[263.5110003377583,367.53792333280126,2.3612887902950637,12.763638778739395,0.0056645588712939425,0.04028450568501429],"step_increment":[263.51089103368463,367.538286024718,2.361183421099202,12.79695406514419,-0.012602125284113435,0.030529102720768364]},{"state":[-429.2090759277344,481.8510437011719,3.5203895568847656,7.184843063354492,0.6172317266464233,0.5380072593688965,0.8232653737068176,1.4728591442108154],"increment":[-0.010400768209034048,0.033282905473538925,-0.010303688660334685],"vector_field":[-6.447256994919488,-3.230452304379966,0.5380072593688965,0.6942337063785025,0.04692221522978972,-0.5591528419730817],"effective_damping":[0.18701716303482657,-0.2600233117801166,0.2718443265759251,-0.23286520428844637,4.204204921261811,-0.4671707149302172,0.08357072969343046,-1.6990377438512019,11.885806491606605],"dissipation":[0.1677994624390192,-0.2236138657649849,0.04812807619035072,-0.2236138657649849,4.032900155765747,-1.0120763060051365,0.04812807619035072,-1.0120763060051365,5.713053500223169],"control_matrix":[1.7901264904292824,0.0034944768071054344,0.12040599490361109,2.579385794065033,-0.07023039355948285,1.594033430878084],"hamiltonian":23.480623242450882,"potential":0.11928367557233688,"grad_potential":[-0.02284409099911568,-0.032767194911788255],"step":[-429.3377949806061,481.7856822275354,3.5310593948664373,7.199312294462598,0.6178242259594742,0.5277763385889178],"step_increment":[-429.33758477762814,481.785437812393,3.5309846837387022,7.189731408936821,0.6481218273073936,0.5180365503889108]},{"state":[-1274.5718994140625,-199.50262451171875,-3.6149673461914062,7.426351070404053,-0.6495876312255859,-0.6469913125038147,-0.823265016078949,-1.8019434213638306],"increment":[0.02558411298099741,-0.016343980505601215,0.0665359565828602],"vector_field":[-6.3135692426137,3.9637731683655852,-0.6469913125038147,-3.171704343202188,-0.8708929143478333,-0.16986568361228366],"effective_damping":[0.31133369511664666,0.42236398556600074,0.24486865986815615,0.42013696919213206,3.4126674420059264,-0.2870394651386801,0.18420908737427466,-1.1620532781202024,9.503506700336816],"dissipation":[0.275106906886067,0.3845286223471231,0.06285007681231222,0.3845286223471231,3.260436738804798,-0.728076484209328,0.06285007681231222,-0.728076484209328,4.5633513979612035],"control_matrix":[1.5814010259685751,0.10067254895885996,-0.0499472182234976,2.178960034860436,0.0652958995799782,1.1860445238178041],"hamiltonian":78.96584999011537,"potential":53.787205224621374,"grad_potential":[-0.019475138163170713,-0.02628909950299267],"step":[-1274.6970226314497,-199.42268946045866,-3.6279368632818825,7.3633984197225075,-0.6660742386546232,-0.6499257241443455],"step_increment":[-1274.6971751085632,-199.4224369158497,-3.62738152171601,7.392912818167043,-0.6799837419885725,-0.588303933729599]},{"state":[-696.7582397460938,76.21482849121094,0.38910219073295593,2.626915693283081,-0.007646531797945499,0.04768321290612221,0.823265016078949,-2.3654681058360438e-07],"increment":[0.019770291192671242,-0.01596317537391021,0.0011413952001166371],"vector_field":[2.4334545795554328,0.9894661885613671,0.04768321290612221,1.7096140365564416,0.20907761458855514,-1.3935278340671835],"effective_damping":[0.0469864049334186,-0.11202289836691862,0.5238552987470806,-0.017394677328657578,6.414908697967141,-2.266428261775631,0.03196166899669772,-6.6099002157462445,27.893691434730062],"dissipation":[0.04179792018021121,-0.018083608904198425,0.015367419211399056,-0.018083608904198425,6.345630621256,-3.4739816281999936,0.015367419211399056,-3.4739816281999936,13.465644259180818],"control_matrix":[1.97356634384755,-0.04011335249063451,0.11134466276734306,0.940759443651664,-0.041145426218495894,0.9421324062211888],"hamiltonian":27.040483748300847,"potential":23.93889579705545,"grad_potential":[-0.03742271163591621,-0.03198652564100057],"step":[-696.7092738237396,76.23479945336858,0.38987936500385506,2.6612021724304147,-0.004262124795908739,0.02617831187973809],"step_increment":[-696.7090375432055,76.2347434815611,0.3898745938698858,2.6809500877636054,-0.019224782743447588,0.02646119384935353]},{"state":[-366.6333312988281,62.33636474609375,0.6147100329399109,14.737223625183105,0.005324132274836302,-0.023110881447792053,0.8232527375221252,0.028318071737885475],"increment":[0.036272353448106316,0.013389922213613764,0.01696313235097588],"vector_field":[12.036367411889342,8.503625582717515,-0.023110881447792053,1.3949140637885649,0.21098541684486477,0.544035384874999],"effective_damping":[0.030627871452827773,-0.009917932923873607,0.19458899145941716,0.017270287793669698,7.825743986624714,-1.6321402461214978,0.12525386446414766,-3.1588711013646114,7.391751401572167],"dissipation":[0.025379787058718645,0.010066521155494768,0.05867339952962117,0.010066521155494768,7.504652595288969,-1.890011386559709,0.05867339952962117,-1.890011386559709,3.6170687206969245],"control_matrix":[1.913946596762398,-0.13527262900063844,-0.008949433238291712,3.12577594788454,-0.017041622003350215,3.3782239119320576],"hamiltonian":108.12125239182666,"potential":10.454763480665001,"grad_potential":[-0.02990896808952885,-0.034045244858455045],"step":[-366.39235965885166,62.506574065740224,0.6143441542468736,14.765083113462277,0.008597514764374299,-0.013425974839063312],"step_increment":[-366.39213305322954,62.50688073002468,0.614497753238172,14.801277899448959,0.019927705294091737,0.0016465288497678938]},{"state":[93.91934204101562,77.65283966064453,1.1294269561767578,9.851447105407715,-0.008580201305449009,0.10443056374788284,-0.7898380756378174,-0.0004737843119073659],"increment":[0.006337923707703447,-0.008158422561714323,0.011166159598649835],"vector_field":[4.21607946698633,8.903693481839811,0.10443056374788284,-2.030448394140323,-0.43670206958751756,-1.5949047012341098],"effective_damping":[0.029206567306195065,-0.10912106639464718,0.308309706815806,-0.030051698235525746,6.880255388602388,-2.5063355816999437,0.10381184727650214,-5.658586319445449,14.223371610090418],"dissipation":[0.024663393157002323,-0.03335637772430206,0.05070040297024675,-0.03335637772430206,6.737049769010845,-3.0405998574906166,0.05070040297024675,-3.0405998574906166,6.930624556627787],"control_matrix":[1.998391255297254,-0.24373747965216913,0.10591237017890129,2.9852696598232846,-0.08890742794278027,3.0634925446731254],"hamiltonian":37.667444144727114,"potential":-5.958955369482643,"grad_potential":[-0.03183293177975959,-0.0237763187017924],"step":[94.00338542565426,77.83059779417243,1.1312601089808643,9.81097536065427,-0.01608865772454334,0.07652621545602178],"step_increment":[94.00347374284307,77.83062556614941,1.1313409378484462,9.817296711241351,-0.02413048630061551,0.08615323419647747]},{"state":[12.0,-7.0,2.5,18.0,0.8,0.45,1.6,2.2],"increment":[-0.01131897972024271,-0.030131748696833528,-0.015156753102437815],"vector_field":[-14.899362795127972,10.131583701433671,0.45,0.9000573416537059,-4.160601951846226,3.6732998480640684],"effective_damping":[0.1356705687944223,-0.13560061115326752,0.13401167755044155,-0.11088116761406412,2.790020648565428,-0.6633748923662444,0.09465246260430112,-1.4775637222570603,4.491291844820748],"dissipation":[0.12091109822160911,-0.10931699615998541,0.04842461921848581,-0.10931699615998541,2.6930238284935277,-0.8399784665879504,0.04842461921848581,-0.8399784665879504,2.1816987386523503],"control_matrix":[1.2417575513384627,0.2756854087948891,0.06431858729081125,1.346591818483519,-0.06019965053447279,0.9201732229285383],"hamiltonian":145.1002372882363,"potential":-0.7260522911634115,"grad_potential":[-0.048012124954451924,-0.04743318172969893],"step":[11.701426181531412,-6.7979807681786815,2.5096526640023584,18.02129879362172,0.7168219465017807,0.5155176382232535],"step_increment":[11.701687497837547,-6.797815890430573,2.5095059907351294,18.01212335625456,0.6906860776060653,0.502555726410659]},{"state":[-40.0,30.0,-3.9,-2.5,0.1,-0.3,-1.2,0.4],"increment":[0.012084728810118328,-0.006490723114898886,-0.019335968042793444],"vector_field":[1.746054144581953,-1.7920086283799486,-0.3,-1.5880025591428633,-1.5919976584461482,5.2901562584195805],"effective_damping":[0.09766026571065403,0.06880762198485534,0.3192676311112557,0.09611257465465473,12.431285688393864,-2.2000380667005226,0.10425961990943015,-4.587859513022072,15.154143860351999],"dissipation":[0.08567465637433912,0.08486332781642178,0.043701133808187395,0.08486332781642178,11.9001688007125,-2.797659901378818,0.043701133808187395,-2.797659901378818,7.361683209381012],"control_matrix":[1.4995621203417553,0.2663119268707198,-0.4313156769766296,0.7341001954840691,0.031393627003554164,0.011273647291616395],"hamiltonian":3.6576245221878727,"potential":0.830949642528382,"grad_potential":[-0.04406726780361422,-0.04698189725620527],"step":[-39.96479779855158,29.96399614344779,-3.905203485024806,-2.532630223659911,0.07366090119143345,-0.20490484273787996],"step_increment":[-39.964845784193436,29.964121886271894,-3.905356118371769,-2.5204581002622874,0.06759522776508135,-0.22218462275955936]},{"state":[0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0],"increment":[-0.012512285677115527,0.043756256188526514,-0.03302702462241931],"vector_field":[0.05,0.0,0.0,0.05664846143834596,0.030737802977447177,0.004364490543509124],"effective_damping":[0.05958945451517296,-0.10145133459154199,0.5596345100804229,-0.010752878250522221,6.611188073452281,-2.102649415669871,0.015359574760377803,-6.50944972158379,30.28784128990834],"dissipation":[0.05338068460510916,-0.011098760904115742,0.006882534995432563,-0.011098760904115742,6.5251633199145935,-3.4354725241503115,0.006882534995432563,-3.4354725241503115,14.604327478370784],"control_matrix":[1.859729437690207,0.0608750679398154,-0.02861696649100716,0.03023266810424219,-0.004773768959003999,-0.0006075661951105982],"hamiltonian":-0.38687585194534707,"potential":-0.38800001629767705,"grad_potential":[-0.05340944443195485,-0.02787022869009675],"step":[0.0010113162786081857,5.468360631797902e-06,9.84795574643436e-07,0.05113233282957911,0.0005786114476102612,9.572334357118091e-05],"step_increment":[0.0008890670881582663,0.0003854138134430806,-0.00017386607310071928,0.03879023923820927,0.040913766146401076,-0.02315559867794242]},{"state":[250.0,-180.0,11.0,9.0,-1.5,0.9,0.3,-1.8],"increment":[0.030651276417322737,0.005950178523966296,0.004640862044949021],"vector_field":[-1.460154027933598,-9.006550405938407,0.9,-1.8468191722369076,-5.458423155458316,-6.732948357787513],"effective_damping":[0.14724419553212498,0.09977905024273465,0.12283446015560255,0.09432594053660706,3.7660008945478447,-0.27268532388023564,0.16555894224217726,-0.39067053009724706,2.3556050362846364],"dissipation":[0.1292517141091509,0.08002108382194893,0.07233129592318649,0.08002108382194893,3.5566768320932622,-0.37008660386359515,0.07233129592318649,-0.37008660386359515,1.1397654229911791],"control_matrix":[1.309286410526964,-0.329464891547058,0.19030476479990752,1.4235865362365134,0.003615566181467779,1.1090994417405637],"hamiltonian":34.95873912984775,"potential":-2.7031999641033324,"grad_potential":[-0.03764510743335252,-0.06039214688104303],"step":[249.971391355002,-180.18003913888978,11.01670752520132,8.967938618203444,-1.586000019683173,0.7792657460089008],"step_increment":[249.97145320952856,-180.18034322442804,11.016751290615575,8.99844378122375,-1.5807821147043564,0.7838233717929193]},{"state":[3.0,4.0,0.7,6.0,0.4,-0.6,-2.0,-2.5],"increment":[0.01856877330456709,-0.01781180868323249,0.011042314846935162],"vector_field":[4.331366048811854,4.171242998339942,-0.6,-2.720795815699683,-4.641365895595146,0.0021993854523288947],"effective_damping":[0.10520467297201984,0.26393457934834946,0.13706921510957248,0.22848477980707765,11.548198314730925,-0.9789280019275708,0.1562085460452259,-1.1452671177408964,3.918345347453152],"dissipation":[0.09102200763450845,0.20669979656085496,0.06209652859709251,0.20669979656085496,10.90396476545078,-1.1088944368058589,0.06209652859709251,-1.1088944368058589,1.9223045349362822],"control_matrix":[1.4892515025302855,-0.5262986737982891,-0.17727377672084715,0.6639529088578621,0.09892144859509387,0.2581810712517012],"hamiltonian":15.765051337096597,"potential":-0.6456110677784399,"grad_potential":[-0.05421253093303326,-0.02051297249862204],"step":[3.087186061882982,4.081978650196365,0.6879859895579342,5.9443511961811515,0.30622823391810056,-0.5997979696582545],"step_increment":[3.0874209065097973,4.081985521963204,0.6880847950417465,5.9622874751518635,0.28698314497056426,-0.5895419332939756]},{"state":[-5.0,60.0,1.3,14.0,-0.2,0.1,2.5,0.0],"increment":[-0.0025429890561916987,-0.01078037340941342,0.030836489092894263],"vector_field":[3.9376952378276613,13.436314830115785,0.1,5.054150240312578,1.3228819096568132,-1.5732173741272772],"effective_damping":[0.026308947586672897,-0.04211281054173227,0.20395412617124203,-0.0105985250300103,7.492333909726079,-1.6342077424030628,0.1177650959281534,-3.2614439673623625,8.02727417529307],"dissipation":[0.02174075216765737,-0.015738332623305047,0.05649834706886506,-0.015738332623305047,7.1964755225120385,-1.9225774316334363,0.05649834706886506,-1.9225774316334363,3.92152396927104],"control_matrix":[1.9470370414481204,-0.34547846103317803,0.29918567176324284,2.7697642135559994,-0.14365916835254394,3.1571590859313385],"hamiltonian":86.31148969015307,"potential":-1.8338491259459706,"grad_potential":[-0.05228514447333799,-0.02366822341568415],"step":[-4.921461133627314,60.26983919611536,1.3017393569617288,14.101283098159888,-0.17390398335508922,0.07188819425581475],"step_increment":[-4.92138494227912,60.269790666339205,1.3020002647366844,14.098480205454157,-0.18576195540214085,0.09936665747227302]}],"rollout":{"initial":[-249.15969848632812,-104.58634948730469,0.21728603541851044,11.796178817749023,-1.3709025382995605,0.8148626089096069,0.0,0.0],"controls":[0.8232527375221252,0.3808467388153076,0.8232527375221252,0.3603965938091278,0.8232527375221252,0.34045499563217163,0.8232527375221252,0.3210615813732147,0.8232527375221252,0.30225253105163574,0.8232527375221252,0.28406113386154175,0.8232527375221252,0.26651912927627563,0.8232527375221252,0.2496570497751236,0.8232527375221252,0.23350480198860168,0.8232527375221252,0.21809090673923492,0.8232527375221252,0.20344498753547668,0.8232527375221252,0.18959780037403107,0.8232527375221252,0.17658093571662903,0.8232527375221252,0.16442745923995972,0.8232527375221252,0.15317140519618988,0.8232527375221252,0.14284931123256683,0.8232527375221252,0.13349954783916473,0.8232527375221252,0.12516173720359802,0.8232527375221252,0.11787915974855423,0.8232527375221252,0.11167071759700775,0.8232527375221252,0.10644856840372086,0.8232527375221252,0.1020452156662941,0.8232527375221252,0.09831016510725021,0.8232527375221252,0.0951187014579773,0.8232527375221252,0.09236374497413635],"path":[-248.92388268716206,-104.56065421909076,0.2334946980180719,11.76375766884231,-1.397099827403017,0.8054462409210629,-248.68901501360682,-104.53180857146019,0.24950582132441243,11.731889397377747,-1.4211152249451404,0.7951830013962967,-248.45512978177504,-104.49983402998757,0.26530524360306873,11.700675677645775,-1.442435965167499,0.7843374999413497,-248.2222616003238,-104.46474521837055,0.2808836251467903,11.67019622041141,-1.4606565001339449,0.7731101592675953,-247.99044582941343,-104.42655112905673,0.2962348480633804,11.640506720239443,-1.4754632998980244,0.7616124617886045,-247.75971895972054,-104.38525632091805,0.3113540960974929,11.611642815686563,-1.4866061712558865,0.7498579524158502,-247.53011887329308,-104.34086195574945,0.3262358767189406,11.583627354680432,-1.4938694247598694,0.7377627181390395,-247.30168493742144,-104.29336673640279,0.3408721117085238,11.55647954368403,-1.4970525561269457,0.7251498459411889,-247.07445782733905,-104.24276794425586,0.3552503399554471,11.530226870186551,-1.4959679973341158,0.7117564892576953,-246.84847885987898,-104.18906287544813,0.3693521025035939,11.504923154702528,-1.4904635836412496,0.6972489889952128,-246.62378845561267,-104.13225103536708,0.38315177052393895,11.480677581557163,-1.4804759749455714,0.6812610524185295,-246.400423217143,-104.07233735995037,0.39661646922125665,11.457696587764117,-1.466110092267006,0.6634773557902376,-246.17841129850308,-104.00933624447143,0.40970820525196394,11.436325634076196,-1.4477086170012197,0.6437723927326098,-245.95776673982502,-103.94327508822643,0.4223891595676214,11.417049563308373,-1.425835962444533,0.6223534729296895,-245.73848530900494,-103.87419495508144,0.4346292479389443,11.400401423921025,-1.4011128666523147,0.5997718971926281,-245.5205453957669,-103.80214643822218,0.44641187068203425,11.386796168293467,-1.373948962343223,0.5766997444836032,-245.3039150972586,-103.72718156796145,0.4577331439759888,11.37639965086702,-1.344321621919053,0.5535797105328617,-245.08856243610074,-103.64934492490417,0.46859418975422884,11.369115550954207,-1.3116828118236261,0.5303741204888079,-244.87446481943627,-103.56866638252376,0.47899021273872194,11.364647628156982,-1.2749341070152167,0.506490713333221,-244.66161672481113,-103.48515576208041,0.488899493854641,11.362536637468725,-1.232388405351342,0.48078933984569217,-244.45003700204313,-103.39879932851345,0.49827266579702384,11.36209760146901,-1.1817757950768477,0.4515728677927141,-244.23977643622507,-103.30956166939875,0.5070215625237121,11.362233346188,-1.1206707772708489,0.4166166232195314,-244.03092006630243,-103.21740633015088,0.5150100806980707,11.361292470544889,-1.0480773794169378,0.37376222063350445,-243.82356557257032,-103.12235455249062,0.5220677599713988,11.357846267058367,-0.9666964595303907,0.32388021854498517,-243.6177591590196,-103.02454518616747,0.5280936424423525,11.353116101092345,-0.8819768092448469,0.27530464221379325],"steps":25},"gradient":{"windows":[-1394.722900390625,69.6255874633789,-3.180790424346924,-1.5190577507019043,0.05902763828635216,-0.047623708844184875,-0.10662980377674103,-0.03987441584467888,184.77682495117188,419.5346984863281,2.6280229091644287,15.842545509338379,0.08127302676439285,0.061585720628499985,0.823250949382782,0.1963811069726944,-1394.7899169921875,69.55240631103516,-3.4058258533477783,-1.4994330406188965,0.07441028207540512,0.0235703494399786,-0.039835285395383835,-0.03990570083260536,184.53936767578125,419.6918640136719,2.565589189529419,15.939602851867676,0.02250787615776062,0.04509340599179268,0.823250949382782,0.2421039342880249,-1394.7261962890625,69.51415252685547,-3.329740047454834,-1.563908338546753,0.12436845153570175,-0.007533746771514416,-0.12691056728363037,-0.039928168058395386,184.3394775390625,419.80926513671875,2.5742945671081543,15.792105674743652,0.10061556100845337,0.05686207860708237,0.823250949382782,0.2878267467021942,-1394.71142578125,69.55653381347656,-3.3510653972625732,-1.4287185668945312,0.0006256342167034745,0.009115037508308887,-0.1950594186782837,-0.03993416205048561,183.9862518310547,419.93255615234375,2.6513378620147705,15.952691078186035,0.1632990539073944,0.08305898308753967,0.823250949382782,0.3335495591163635,-1394.7371826171875,69.59013366699219,-3.3094120025634766,-1.482555627822876,0.06389924883842468,0.043315328657627106,-0.2386927455663681,-0.03992496058344841,183.7516632080078,420.1737976074219,2.4583823680877686,15.982477188110352,0.18844451010227203,0.10806316137313843,0.823250949382782,0.37927237153053284,-1394.6258544921875,69.60651397705078,-3.2819631099700928,-1.5351368188858032,0.08084245771169662,0.0004144003614783287,-0.25546252727508545,-0.039905913174152374,183.55316162109375,420.4201965332031,2.4951469898223877,15.914007186889648,0.07906687259674072,0.18120026588439941,-2.4697818756103516,0.4118618071079254,-1394.65283203125,69.55569458007812,-3.281120538711548,-1.4752309322357178,0.07400426268577576,-0.004127924330532551,-0.24328328669071198,-0.03987826034426689,183.27777099609375,420.42388916015625,2.5884649753570557,15.80299186706543,0.07359668612480164,0.16498833894729614,-2.4697818756103516,0.42306941747665405,-1394.4251708984375,69.54439544677734,-3.2561800479888916,-1.4153178930282593,-0.025988394394516945,0.016280893236398697,-0.20151953399181366,-0.039842210710048676,183.0423583984375,420.609619140625,2.5070269107818604,15.6608247756958,0.20338527858257294,0.1550268977880478,-2.4697818756103516,0.42741867899894714,-1394.4918212890625,69.46554565429688,-3.3389084339141846,-1.5381906032562256,0.10265708714723587,0.008308209478855133,-0.13062764704227448,-0.039799049496650696,182.68003845214844,420.9111022949219,2.5598819255828857,15.593113899230957,0.14046934247016907,0.14705374836921692,-2.4697818756103516,0.4334586560726166,-1394.48779296875,69.57160949707031,-3.310896396636963,-1.4712804555892944,0.08641651272773743,0.06303165853023529,-0.03306848928332329,-0.03974746912717819,182.37010192871094,421.0766296386719,2.562791585922241,15.359066009521484,0.10179661214351654,0.24566778540611267,-2.4697818756103516,0.44006940722465515,-1394.525390625,69.52714538574219,-3.355551242828369,-1.5582855939865112,0.08969669044017792,0.10058501362800598,0.0840819925069809,-0.03968731313943863,182.12481689453125,421.126953125,2.629096031188965,15.252915382385254,0.11845562607049942,0.22788497805595398,-2.4697818756103516,0.44718456268310547,-1394.4122314453125,69.54177856445312,-3.24517560005188,-1.4878531694412231,0.0666082575917244,-0.06430887430906296,0.12783554196357727,-0.039619866758584976,181.99148559570312,421.283935546875,2.5739433765411377,15.070195198059082,0.0322880744934082,0.25729697942733765,-2.4697818756103516,0.4547468423843384,-1394.4517822265625,69.54092407226562,-3.361682891845703,-1.4890645742416382,0.030171167105436325,0.011951877735555172,0.12729576230049133,-0.03954237699508667,181.7024688720703,421.4725036621094,2.5085244178771973,15.024026870727539,0.06072269007563591,0.32131171226501465,-2.4697818756103516,0.46264588832855225,-1394.36767578125,69.43888092041016,-3.3489186763763428,-1.4712774753570557,0.12896087765693665,0.014828617684543133,0.09230617433786392,-0.03944958373904228,181.38490295410156,421.6331787109375,2.6893270015716553,14.83786392211914,0.04654601961374283,0.26657354831695557,-2.4697818756103516,0.47069063782691956,-1394.3948974609375,69.5457534790039,-3.3428807258605957,-1.4881678819656372,0.01968570426106453,0.050474125891923904,0.05587146431207657,-0.039339806884527206,181.14710998535156,421.7527160644531,2.571382999420166,14.75310230255127,-0.0199520755559206,0.3512551486492157,-2.4697818756103516,0.4788394570350647,-1394.304931640625,69.5289535522461,-3.334228515625,-1.4361021518707275,0.13453128933906555,0.046163663268089294,0.03614028915762901,-0.03921256214380264,180.8506317138672,421.8706970214844,2.634462594985962,14.744157791137695,0.028539827093482018,0.2905287444591522,-2.4697818756103516,0.4871625006198883,-1394.3148193359375,69.4186019897461,-3.3403847217559814,-1.4348634481430054,0.16957828402519226,-0.08641237020492554,0.04227408021688461,-0.03906656429171562,180.62535095214844,422.03240966796875,2.6278042793273926,14.517077445983887,-0.04900774359703064,0.2636318504810333,-2.4697818756103516,0.4956791400909424,-1394.21142578125,69.3844985961914,-3.3329808712005615,-1.342877984046936,0.029983649030327797,0.08100300282239914,0.060412921011447906,-0.038902197033166885,180.3910369873047,422.26446533203125,2.6299383640289307,14.401880264282227,-0.10312215983867645,0.30885782837867737,-2.4697818756103516,0.504403293132782,-1394.2750244140625,69.46150970458984,-3.312965154647827,-1.416032314300537,0.10802111774682999,-0.005688616074621677,0.0737575963139534,-0.038720667362213135,180.11257934570312,422.3106384277344,2.6482255458831787,14.203326225280762,-0.10971397161483765,0.29632827639579773,-2.4697818756103516,0.5133423805236816,-1394.2889404296875,69.46485900878906,-3.35805082321167,-1.5230023860931396,0.15232625603675842,-0.02706471085548401,0.07938350737094879,-0.03852079063653946,179.78775024414062,422.53857421875,2.724290370941162,14.129619598388672,-0.08748092502355576,0.34447184205055237,-2.4697818756103516,0.5225014686584473,-1394.1514892578125,69.50695037841797,-3.3924062252044678,-1.3894860744476318,0.07477949559688568,-0.002877493156120181,0.08105190098285675,-0.03829941526055336,179.70721435546875,422.6384582519531,2.655595541000366,13.933926582336426,-0.053879592567682266,0.3878155052661896,-2.4697818756103516,0.5318810343742371],"window_points":21,"window_count":2,"z":{"D":[0.5046387313789391,-1.1408664395329051,-0.7003700291326315,-0.055108906362906565,0.038250425659932724,-1.0331361527439238,1.5119967309420723,-0.6787179535958675,1.3048066799587092,0.38684954551409834,0.5349933978066361,-0.374234995607529,0.3065904638896288,-1.5483076400622002,-0.3133449095705764,1.9338552283235824,-0.13482701028441227,0.4864370919249056,0.3611613627880569,-0.8771742100961776,-0.39798300930768143,-1.1032090852922294,0.657242617917387,-0.008953327931940891,0.6270508266127544,1.3476834641892663,-1.1988936559413101,1.2269267168469133,-0.5922399920887177,0.2577937173616632,-2.1477811820761983,-1.7046259788028506,-1.8312880209131193,1.6252778508507228,1.627741377229482,0.3566524118266746,-0.21255213597669323,0.7036620849101413,-0.39588284937108925,0.11250405023849505,-0.12373326907432394,0.3322432977611231,1.3884936748021892,0.9930038588165786,0.7117666210927656,-1.5623927327973481,-0.19798533157138778,-1.3159054919704511,-2.364137582386691,1.5133562630088928,1.2322351586953786,-0.06191791329995906,0.39262289948360213,-0.0665432628960681,-0.1725690277678801,0.22426709769832268,-1.5411142498865231,0.20702706734608448,-0.03779664847845476,-0.5341890904176739,1.1710155537505702,0.42719616506584746,0.24309869873911294,0.4543997310264046,-1.3065288279897447,0.35671832738840614,-0.4276426578670565,1.0202321357997826,0.4361432075460536,0.1438536481175607,0.0868247091549671,1.5584972449981163,-1.7440443888318446,0.1869493901499986,0.9202294283551662,-0.2514423073665184,-0.1639391472017893,1.4696728685079876,1.1385372804110483,-0.021914848989944558,1.58527496860646,0.9307826860933468,-1.2694488356736915,-0.45721208584662787,0.97360562859985,0.006469700950238946,-1.0852901221445188,-0.028020720092139354,-0.5156889717002796,-1.4641049036096065,0.9686536895222139,-1.2779444788223933,1.0077772167997798,1.3699205941222024,0.8576434042759318,2.466114607803635,0.38661639695868183,-0.5986945384058555,-0.4326704442671862,0.9706429624796252,-0.30353162356860014,-0.27613824091556993,0.3769213804207713,1.2304198647992384,-0.25051905811632214,-0.47124702449186523,-0.005310461464669475,0.3465631672004834,0.14778419706207577,3.1674337016638177,2.4580797522463054,0.37688580591781645,2.132671297519434,-1.6652288853278225,-2.09524554739758,0.5817058550067902,0.5340796748110744,0.7712473093787754,-0.0439166764505897,-1.6103112887876412,0.0855705832805853,-0.6679226976228424,-0.6523792502492914,0.9616402480168681,-0.5358478333587514,0.7592221252792744,-0.42603059968810136,0.7889715944159935,-0.12279971823642859,1.9551776662795144,-0.005180507845052535,0.7349268911793037,-0.1837917084291515,2.216726062996836,-1.933759734177229,0.01535481769943932,0.21152332534200477,-1.6702763932429827,-0.5779160058801002,-0.542966765849077,1.9032549329374429,-0.522386664451513,-0.9350495606939957,-0.09847368140993762,-0.9201962575119351,0.7308494422269057,-0.04885534356716054,-1.327511362161448,0.08155080143390236,3.0666293277370134,0.520370904745821,0.8016412441605493,1.7907497407103103,0.12814453355270225,-1.2775448911084233,-0.31561576006709097,-1.3192388537954463,-0.04692078267189798,0.524634342517998,-1.7669960333616688,-0.6911162361847752,1.2541613402570726,0.9075116685508564,0.5151774235108214,2.2841203570570467,0.7096544321402318,1.5789479498767274,-1.44419590929914,0.25906265722724003,0.6312281698072981,1.2230020965668356,-0.7360034537670256,-0.3442557180697429,0.8831517128889742,-0.03968750509673654,1.9962973123061625,-0.09246621832242666,-1.5231262714209017,0.22541584062619927,0.5454116168526183,-0.8662717184260001,-0.3953925530308937,0.30646806808352195,0.2811377059488569,2.2158208840659044,2.0264305833602356,-0.2601596563859374,-0.2207777005215506,-1.3627189310107484,0.20859844021653448,-1.4820306998353228,0.3029750675875489,-0.023931370141503795,0.24760272317814627,1.91728606212145,-0.04965100866082328,1.3422737579861557,-1.5300931376346765,-0.7557853313648073,-1.141277407674183,0.6099134053386495,1.8922987730204237,-0.14574638040561883,-0.37278011215443935,-1.7575942101335056,-0.4582831671186354,-1.542376830412627,0.1932901336998664,0.0973032534841174,0.14534341964430783,-0.6581719569233732,-0.772423621340496,-0.007734575812726181,0.7056849133406912,1.549532349007642,0.31128488193514986,1.1980804916073338,0.2886046723841372,-0.6048665580089505,0.22596088494222963,0.8824848736000072,0.7641136377663598,1.0387239209954453,-0.44040239902835704,-1.3867672064713865,-0.11688404080379398,-0.5841790542830879,-0.3433649160092582,0.24687861297359834,0.4484453236844938,-0.14895700524347288,0.5435516977571876,0.8961424013659359,0.055355477785682526,-1.1544514027188302,-0.45173379328542945,0.9908169588438756,0.6703334444420026,0.40667509044733424,-1.4470171586116642,-0.7713485676599484,-0.09798185489547433,0.23673460388383327,-1.7110573626190615,0.006585597466002179,-0.028971102882174325,-1.268768265116195,-0.3971990035879622,0.529228279992952,0.1657817742932416,0.8667520307699584,-0.8037349631238053,1.2999458992333612,0.22232199655727147,0.04706498328338832,-2.4425361440141033,-2.060469323042611,1.0692766769896433,-1.2649879887504976,-2.804828081598645,-0.6982450177404943,0.41708343391499814,0.6378016873747958,-0.3855246404028803,-0.5225729890229788,1.779602840341984,0.33095604764819914,-1.845863955577471,-1.3256166155645714,-0.3304133701520234,1.1386391361597032,-0.06649121520733277,-0.3692396617023142,0.6641925058278905,-0.8574086038758482,0.6490213206036034,0.8922225402298383,-1.3569005262264993,0.23190740500929494,0.269569838047331,0.8311647489655472,0.8360564520259374,-0.6951853657065167,-0.3931523203686932,-0.6007098736979589,0.20598491094798024,0.06330201217179944,1.2756770406813467,0.41928915927894633,-1.2978078604723537,-2.135646509159176,0.4966611783639018,0.27386900730049757,-0.05103762538846774,0.24315450960618862,0.28133465391525636,-0.03312890164511382,0.3139529546846754,1.0159179060987875,-0.8839678498373055,-0.5789328980028489,-2.176715828355841,-1.1962997140620693,1.0265981883373139,0.29066230117941033,0.051897727505485554,-0.7666939961216944,-0.7048552856241692,-2.0275355931791386,-1.6971074411774454,-3.2809185997933277,0.16084869032939783,0.7673136567409048,0.688494578040199,-0.8186825860549284,0.1857054613341001,-0.7655017986272845,0.36587022875725056,-1.4450571714640552,1.5546192295011163,-0.4517922011993737,1.1844815509203925,-0.017164587204469277,0.2563311036690292,-1.0955348767674657,1.3339806563555243,-0.5906956701265216,-1.8688075263151427,-0.7927359583333617,1.1722432858303418,0.38874916961843886,-2.1921317496043056,-0.7130205894858526,0.3660499568817054,2.286441309180352,0.5173533719108886,1.4272721958881756,0.1897399563756502,-0.5571837306877531,-1.3190748696759913,-1.813925898204053,0.48744657104961975,0.947894907361282,0.8113646971990712,1.1029390918566124,0.19024475521798395,0.5626553623660495,0.8796584337113653,0.2517114436236156,-0.4104520675267151,1.2270320347738402,-1.224696189999222,-0.6520189830892381,0.7641941867239865,-0.41792966737031423,-1.5588168774904565,-1.0405222091695279,1.16730616087545,-1.0042215357551276,1.046003161614128,-0.13534403761246266,-0.8607264361264286,-1.292678393368782,1.1488439675460227,-0.2719863695560882,-1.5886654558184146,0.4393775852017231,-1.2672685749764925,0.4491704731129671,-0.8013058191643863,-0.5694789369243852,0.8681656585820365,-0.1949850713867498,0.046447466256101896,0.9504467349611837,-0.04229962591786473,-0.3263471584539373,-1.860617338839368,-0.5765075900727091,-0.3975221523250519,1.397013785214356,-1.2671855996134715,0.8106915026882722,0.44999688566201823],"G":[-1.1123456307597488,-0.8859194424501039,0.5469876646147933,0.8316479229224492,-0.3769643773996963,1.0746519987382384,0.48628687850444596,-1.094429623119296,0.3073659193719141,1.6038788717129266,0.20179384554298063,0.10116377576296379,0.14880383413773068,-0.2431851735013841,1.623502275458109,0.8448576184320227,0.5717212669155004,0.4244603890722036,-0.9235600396899009,-0.1700753184181513,0.14582288419525108,0.07973751019070445,-0.45528112950876554,-1.664622812560708,0.5612766455598335,1.3329280465006192,-2.634520223127554,-0.5726111470134649,1.0755452721645162,-1.1132049304836948,0.1715415151258013,-0.30407964059799514,-2.0118776019474556,0.18972417648910037,-1.3382811058555528,0.05939033341264425,-0.6093404206337517,-0.18125720082412947,-1.8354324905548032,0.8105255993118555,-1.1190802361001382,-0.90429943804909,0.8035604076667383,0.3282153966858944,-0.9318195986736945,-0.29509740451786937,-0.6503997569872383,0.7701509096828812,-0.8012730618931242,-1.9491556608230456,1.2041029083822687,-1.704035957028762,-3.319649056035005,0.14577388867119825,-0.42913875206940605,0.10755958646247342,-0.025669761806366428,0.680837580053114,0.47375170218362606,-1.1003743116323785,-1.3219508292628748,0.200835696219664,0.423768900947549,0.5324619812614314,1.382901089411119,-1.0578601303171105,-0.4140007048792657,-0.6553437359281525,-0.8715687697964902,0.026414032365911777,-2.2684231150940866,1.137186593675598,0.7442288228212857,0.6061448022397139,1.9017680805193609,-1.1633957738836753,0.7350631385675979,1.5127991831544587,0.36772557894227037,-0.9139679282155252,-0.0333923361276491,1.6511931630072703,0.09310988680543877,-0.28625111281105353,0.10316317890839377,-0.9259140965219047,-0.16251262124357901,-1.1422081052264532,-0.730445057548525,-1.7881294482845527,0.7557497437633908,-1.6137655292372077,1.3038744921117245,0.9952804442804976,-0.10589418506307968,-0.9385099150611633,-0.4203964125439272,-0.7587663800918595,0.009814791159181381,-2.0517009906387162,0.97353917943456,0.7020122902173943,-0.05863416494530994,0.3842080464715513,-1.1039936479440575,0.030813534409950497,0.5704631585857257,0.3167195687139359,1.0119758499055949,0.7660843803454931,0.8308448945468868,-0.38814353770006665,-1.0487274674384934,0.13352503885333553,0.00846237313237834,1.0819139632016963,1.7664308045109722,0.5102145848284069,-0.9966385675139617,0.16764053653545924,-1.6316157518425822,1.9690339194355242,1.5021393482215384,0.6534785944336309,0.7172392690352514,-0.49244851856841626,-0.6769354046706959,-0.7204679329901271,-1.6135722731654158,0.4566497499113048,0.19393808007530833,-0.304228235035718,1.9785819194382446,0.2973539703846899,-0.42640450775834,-0.7604182867601231,-0.006180978474048781,0.4135880964821406,0.547469707540538,-2.165868444132111,-0.1669643166028697,0.5406676712962357,-0.763179305972261,-1.3300773998406057,-0.08301357405111756,2.389908917739439,1.001073669966646,0.3814606336766953,1.4576055223883746,-0.9133192148713173,0.21509305067134954,-1.1778959832869562,-0.936346074971768,-0.9166216778975796,-0.5584758135907036,0.44588194967342304,-0.5623963060931947,0.5061240824629726,0.8942145415652157,-0.37889080318888985,1.2671548905006544,-0.794897650078509,-0.314420780859989,0.4828378268593376,1.3913070332004682,-0.8414630839028034,0.22673465688286534,0.393314256498122,1.4250387107188391,0.38488602313111686,-1.4274493742178906,-0.29434167899543917,0.6252770894381191,1.0711293929770966,0.5490231816319227,0.0782805914255921,2.038000557287597,-1.0540887813771929,-0.5014667855027184,1.4490599395400796,0.957337934899667,-0.01583767004073009,-0.29360814027986315,0.325720034288121,-0.25732580562595664,-1.4641338029482112,-1.1386922990939008,-1.3571542587302747,0.5488854214151416,0.22147561781329572,-0.7901823467815727,0.19057044558052003,-0.026755195417904377,-1.0645762699248558,-1.3465451540840006,-1.5138946458064548,-0.6283028811040549,-1.4815177530252153,0.22191815336727483,0.3253420092874801,1.156888586609668,-0.13886273869271581,-0.8214892782899575,-1.433347781484899,0.4749170190587666,0.7910788154870083,1.2429855459801067,-0.1448530266367036,-0.11048255173027483,-0.03707347747740674,0.1578497196782759,-0.7547142319687676,0.23623966825144907,-1.493273047896795,-0.8947862306181816,0.9936083488022763,-0.019202999623948676,0.32479392732015533,0.2664480682238969,-0.3735169458326179,-0.6093616734669713,-1.0591222672801919,0.03801474872369287,-1.2615592065920518,2.3868713017351,0.8905734680249933,1.263186799543808,-1.2487537823135246,1.002716486246825,2.0588540542419596,0.4270942359460054,-0.24990674750762082,-1.4976222618603112,-1.6061747464953078,-2.1647526587679886,0.06019687066467419,-0.46158796828211407,1.4531464111019112,-2.951406210132269,-0.8061794316528008,0.326012942832395,0.7090123131543985,-0.3514918715112496,1.019467188238091,-3.0248911329590573,0.1855673546385574,0.6061191484152606,-0.1653331313430815,-0.3597461332171025,-1.796641852703045,-0.26334933234623714,-0.8059678709252962,-0.28806749242000557,0.8816918754743948,0.637792648344572,0.11603935670334437,0.49487418200431205,-0.5830593263215423,0.23748924522288228,-0.9349291502135441,-2.1176926526169644,1.297517180807258,-1.5139435113430573,-0.6300970201595092,0.04585665809358088,-0.5194044708898261,0.9615033114596727,0.9465215139783024,0.7026556566238774,1.4507072433025294,0.5321146588999972,-0.25208391033779287,2.8062526884359125,-1.293450663367607,2.401266550044019,-1.2813480962219745,-0.5593657352131941,-0.8496624697295635,-1.440327526041139,0.3144957372569292,-0.023276566365307116,0.6505563987345739,0.18045714494473386,0.21243461522286766,0.02066455625186967,-0.6277407658927149,2.17711533977075,-1.3539779420656413,-1.2528620804915005,-0.5333376157760598,-0.07855051023950275,-0.882041732012696,0.10467656680051386,0.7175377517406212,-1.1060409205489763,0.4458076906803939,0.874072018497355,0.4586824283184446,-0.646615674601538,0.16059840246277757,0.5409681995685881,-2.402532346097013,-1.3452034382924005,-0.6769533310119923,-2.519350890236135,-0.398337826735287,0.7293545997028444,-0.773480903269797,1.0526425104567052,-0.2392954741669407,-0.470917363331276,0.5126206710023191,0.8345401403380954,0.3713462157107559,-0.7740704261368275,0.33367061315763535,-0.7178924198817437,0.8987861184535154,1.23658237411517,0.6509350469000681,0.8699047434800827,-1.1909394741570634,0.4131983075015335,0.7485693997600525,0.22614291797424535,0.4382917891297917,-0.3434051272968364,1.60678774350558,-1.1497153638816957,-0.9719104706304442,-0.5901197621025228,0.4549321069556958,0.046386189971241526,0.4321272479267642,0.7465104710308552,0.22324357029968597,-0.696900014922744,-1.184464261510436,-1.6541630598083312,-1.3616982231463712,-2.323682670816685,-0.3868190207143051,-0.3573715894514897,-0.9390539518914147,0.4804741129905452,-0.36595367654247485,-0.5182505980415388,0.31326517035725715,-1.1095889026443697,-0.09218244122880966,-1.0306209101553576,0.0674422979212736,-0.2535989946467753,-0.4590518175879972,0.06182581653469247,0.9254328427465823,0.6818025364484009,0.2609125762517614,0.5864760436118474,0.4479346773431783,0.17834398650757122,-0.5230485921643199,-0.23343894908359525,0.620744168764639,-0.05999005569586074,0.8375154000932679,-0.8405822774620993,-1.612102803104779,-0.9258165967377303,-2.103082870079832,-0.3989052113926422,-0.2980262395751143,0.260205892829878,-0.14434179828239305,0.3684052841646384,0.4546836964708375,0.22373444747866875,-1.2409895507835058,-0.6976460476434885,-0.7130677842045395,-0.3579808265835771,-0.7707213581497485,-1.342065773189893,1.974918668194858],"V":[0.8941256253191501,0.3163354486014572,0.3382295318612035,0.33134315191332864,-1.3895392353540084,0.9228741578183604,0.31489067974187707,-0.9449977703281475,-1.0102100607949431,-0.28207340272861414,0.4291452817038805,-0.6628778396336192,-0.23712463343615356,-0.30485039704171824,0.21746313060063327,-0.22285546685244972,-0.17417436311736692,0.5826415829095132,-0.5332904893802323,-0.676520183859708,-0.08601534535789616,1.2211676294757239,-0.12160876157073694,-1.0940781345152275,0.755737292476554,-0.6379944059590555,0.4770903901828726,0.40860069668283416,-2.568886593991243,1.1698674353403218,0.5986670581466529,-0.1101005374400333,0.28227016721998593,1.182039277133619,-0.48399719620655535,0.432423916339463,-0.30390714865910956,-0.4329430636483743,-0.8379784237716424,0.2421357539065703,0.03286842185955734,0.23876119028820153,1.447080500634595,0.6028859830989881,1.0040314701518929,-0.46789821966183665,-0.45235943996858946,0.25937646607998577,0.8744763990150154,-0.9407267044145442,-0.6416264400689491,0.9829910158733661,-1.731308823165387,0.4706346377071593,-0.0953929069873214,0.40683944186294396,0.6249240315980132,1.9436494019543402,1.3380040281716954,-1.0727817649822646,-2.153654993960887,-1.6969202566498633,-0.5348513009507055,-1.3265449872387223]},"beta_scale":6.491228070175438e-07,"rollout_loss":0.07205084940719521,"kl":3317.1088950696007,"objective":0.07420406044434565,"grad":{"Minv":{"level":[-0.017919890533087957,-0.012241181420715482,0.026174272471572318,3.485579854183364,-0.1975533349010556,-0.0450979023508929]},"D":{"mean":[-0.0026450666115663594,0.0004805080296304545,-0.002107425952329121,-0.002440534619125125,-0.005752286448748646,-0.00015828002201492218,-0.0015594786933349131,2.047394903504725e-05,-0.0020088160704613126,-0.0014387471840692013,-0.005051514328294606,-0.0001496990924653046,-0.0017064845816468623,0.0004132812708293824,-0.0011782233333602235,-0.0016727290976688639,-0.0031998232698023875,-8.519987436121198e-05,0.0033281425473285996,-0.0005867401120801654,0.00255602259093,0.0029632391353014848,0.007141335303001931,0.0001879553057120585,-0.0035315662508385876,0.0006138806724086701,-0.002634363820402185,-0.003080643966467656,-0.0074694158870594555,-0.00018844330302614955,-0.00048737015492535097,0.0001304884731339877,-8.188335300998154e-06,-0.0002723334774853256,-0.00044185243153177116,-6.11450512376149e-07,0.0020369879467951337,0.00012686623817048244,0.0025002711197965443,0.0014012142097378406,0.006742964836826606,0.00015987653599938408,-0.001899750430058865,0.0004592677546930788,-0.0017650197077891331,-0.0021044705180516357,-0.004292840751742577,-0.0001471595140358026,0.0006477125236340651,0.00019491751135493004,0.00033419279422640527,-0.00017765271409240805,0.0019280803181576426,2.9940197437410613e-05,-0.0035396556876580287,0.0005801223583112979,-0.0027789640760619294,-0.0031273002121688116,-0.007728842456005839,-0.0001928274747316119,0.00204145625276922,-0.00031312807126464005,0.002331419759210035,0.002164137760718965,0.005600214262874626,0.00017335706769373122,0.0024685649242624057,-0.0005502765441370827,0.0017687492660690971,0.002346974184489458,0.004878452883387988,0.00013852369519298845,-0.002516666668713419,0.00040304789927346013,-0.002009568210248581,-0.0022135301103704125,-0.005588533469046439,-0.0001450059809076457,-0.0006237225297212878,-8.076387465289351e-05,-0.0005432715387282287,-0.0002125684809420436,-0.001891920964464892,-4.428371068318357e-05,0.0011558538662217103,0.00011031458226821004,0.0021711374312401543,0.0012608748112136476,0.004962435269106489,0.000156091620976233,-0.0010175007983800725,-4.568432048371861e-05,-0.0021143482383293283,-0.0013056657334552574,-0.004578865469402688,-0.0001576747386129396,0.0014488793651493783,-0.00024004553496072677,-7.740342248541192e-05,0.00041535242058561713,0.0014464762289740111,-3.3725858435104786e-05,0.002110735762346334,0.0001792831368488357,0.0023262784537501817,0.0011930730171142794,0.006754617799848987,0.00014674661753918792,0.0021563694954126627,-0.0005072330360130062,0.0017066867853406222,0.0022306159207752117,0.004361957245841363,0.00011916538273692135,-0.002404732704019594,0.00021732029760823947,-0.0012594544998216384,-0.0013535352407315418,-0.0042864387946677605,5.329374295708322e-05,0.002823218103398494,-0.00037994473708653596,0.001991024564101098,0.0022201109583970214,0.005741902040397388,5.608380759012433e-05,0.0023255943146544,-0.0003414318927863085,0.00221100620475629,0.002197194461849114,0.00577703182206571,0.00017155464825473782,0.0019420644622345894,-0.0004726727138135487,0.0015091214325779556,0.0020434980344538798,0.0038728875962678517,0.00011531247189259256,0.0025094930864866106,-0.0003361600364039315,0.0022759083944648437,0.0022335773220399286,0.006105503168238705,0.00016086518926330379,0.0001908347632510444,-0.00012648019975613703,-0.0010570640941589015,-0.0004939558070215321,-0.001434729502688927,-7.956040965434792e-05,0.001484083508315826,-0.0002139152549566942,0.0009276689891343513,0.0010499110725749102,0.0029984142770750565,5.939489852315836e-05,0.0014080479642226767,7.138071987477635e-05,0.0015388077658532152,0.0010358221406267015,0.004217797808354421,7.739499765640903e-05,-0.0026308451168719615,0.00014134407272179084,-0.002678657341535609,-0.0021271160517685,-0.007304915385251423,-0.00019525265463215876,-0.0033540587016936516,0.0005312180030076275,-0.0028042413392219663,-0.003087951413584756,-0.007594623680478465,-0.00020073704485609967,-0.0006087050137860776,2.1980109784434144e-05,0.0007200793150692997,0.0004038105687640398,0.00017464118792225833,6.613658926919624e-05,-0.003225468250227208,0.00041579591531481656,-0.0027962794141676317,-0.002772333242646793,-0.0076971849788368685,-0.00019850657444086062,-0.003513390570207614,0.0006018148661200777,-0.0028407813089618467,-0.0032229882135610287,-0.007738042237424001,-0.00020384524488191764,-0.0030596744461289117,0.0005546399292912296,-0.0025010571935847243,-0.0028470071521564176,-0.0066353736833252115,-0.00014918590129533657,0.0034065812654117205,-0.0005496843245947838,0.002779819314688565,0.0030653620099727135,0.007627960680794982,0.0002014836264725873,0.0032322158110300227,-0.0006233079273276775,0.0026473248630775412,0.003138439144689053,0.006994840057995858,0.0001895636719992435,0.000547927969672579,0.00016011733473613864,0.0010430086396950315,0.0003761913588709983,0.0026333171332005257,7.43310358538339e-05,-0.00260256255761842,0.0006090310467581188,-0.002308077232995498,-0.002875792708770435,-0.005587340595550844,-0.00015581002989498415,-0.0001102907809750484,-0.00046540453276522803,0.0006790709544059521,0.0014813583075597109,-0.00041808662781185146,4.404553185432274e-05,-0.002388891120458282,0.0005592768714844417,-0.0017166787703778487,-0.0023286274347466313,-0.004680852771185441,-0.00013884037202485885,0.00139023754339624,0.00031031207768415753,0.0009467226299502044,-8.768143717113424e-05,0.00415036777004131,5.341373092316957e-05,0.0002142139788613679,0.0004159410389709151,9.548396664176889e-05,-0.0008438629844869767,0.0014617877192446736,7.806790004847287e-07,-0.0028382482641260264,0.000345262180360113,-0.002354982316823557,-0.0023112598584790743,-0.00672564480120923,-0.0001782061114000256,0.00018161908967445192,0.00037162533066300554,-0.0006116839073451178,-0.0012767723040372834,0.0004465561211288394,-3.0004188302568365e-05,0.0029621032680315763,-0.0005653042510667571,0.0024826772528060287,0.00293301611139301,0.006500816319449792,0.00018259549397648588,0.0016451165956701697,-6.18849087424606e-05,0.0020665556089545095,0.0016544236134573547,0.005148508462147172,0.00016381298296837255,0.0034523728612429125,-0.0005914047536212637,0.0026881566936152387,0.0030649251779232595,0.0074282612931768355,0.00017779695991842554,-0.001196672192188886,0.00031135665642067584,-0.00013709084410177245,-0.0007538666074210869,-0.0011899488747033847,9.341341969870838e-06,-0.002774670939737368,0.00046391389309476187,-0.002369855270763226,-0.0025945103959549125,-0.006335612523619629,-0.00017594836781585377,-0.0013386107187802728,0.00032352259085475187,-0.0012605303791850659,-0.0016538394723872583,-0.0024338317227454265,4.006094054025609e-05,-0.003188021470492277,0.0006370594207337852,-0.002609544460863195,-0.003126089960404034,-0.006850839540711508,-0.00018729267113394075,0.0019146193578999148,6.129997063669574e-05,0.0026074058743391056,0.0016716307580795532,0.0066096874383987,0.00019386287547213922,0.0013223612665406522,-0.0003121351286837945,0.0005746318258030377,0.0010621369866984716,0.0020647703016328186,5.170552618914257e-05,0.00018142191323129717,5.7086666685395924e-05,-0.0007266425680813512,-0.0005293315649705897,-0.0006851767221796887,-7.030934836092933e-05,0.0009212369514512964,-4.698316623218409e-05,0.0010826690263303544,0.0008437975890890537,0.002758493771010183,7.943998052233287e-05,-0.003168506130008262,0.0006273542615016592,-0.0026055066036978184,-0.0030851569658183462,-0.006863937968478679,-0.00018897488303868545,0.0011343835897042844,0.0003013692776551415,0.0007281484096178135,-0.0002376171525222364,0.0033902180650128393,1.9289958736502878e-05,0.0019718183979687996,-0.00039836894760626176,0.0019703562015117265,0.002228984352203687,0.004693326783969867,0.00014424098432034045,0.00028314721584653035,-0.00023460808170966402,0.00014971798995081553,0.0006323476606469467,7.727811907315394e-05,2.073886524592614e-05,0.0018471385411933912,-0.000260077856949118,0.0010750681066638847,0.0013204672547355903,0.003589366388455144,6.79722486180549e-05,-0.0035177715126180403,0.0005761232003267312,-0.002837298022820658,-0.003125486011835542,-0.007828031789978932,-0.00020350645866290442,0.0033588921903403624,-0.0005212316275843397,0.002814803406743493,0.0030334893842024454,0.007645167472964489,0.00019688106446034045,-0.001244604379070288,0.00040465242656757757,-0.0012022751875542044,-0.0017105568604412061,-0.0025452068126835007,-9.263167412261461e-05,-0.003622846646066613,0.0006343829935245726,-0.0028291590391618395,-0.0032731148141198615,-0.007815426181579876,-0.0002023321488266237,-0.0005295134145502322,-0.00013580191649721691,-0.001000502320896242,-0.00040514827528330725,-0.0024935018938802927,-7.427538043197896e-05],"log_std":[-9.670288232410515e-06,-4.169451021801752e-06,7.916218279847861e-06,1.8358277883386415e-07,-2.0151031285114862e-06,7.12318725314518e-07,-1.823434086621766e-05,-7.584057874188474e-07,-2.7509691096264028e-05,-4.78227842666159e-06,-1.6901721518271754e-05,8.738749111715974e-08,-4.62504640504717e-06,-7.545044806388881e-06,1.8427537237413915e-06,-2.7678954004759337e-05,2.2005666705369754e-06,-1.1088571841111682e-06,7.678530979241234e-06,3.867777853304491e-06,-8.297350718869808e-06,-2.3179776642670666e-05,2.493230212105214e-05,-6.668083862561295e-07,-1.611743445728129e-05,5.227728092438322e-06,1.686377174852742e-05,-2.5986695509746213e-05,2.558227245926697e-05,-1.1282254158586373e-06,7.371777754923762e-06,-2.3731037216863106e-06,-5.448047296501768e-07,-4.220742433942739e-06,-6.510578697425362e-06,-6.504105471221708e-07,-3.81731402203622e-06,1.47146575789112e-07,-6.25745501449065e-06,5.705911972517924e-07,-5.313413865712826e-06,-1.3960394472488245e-07,-1.8722810816141795e-05,2.798756968757073e-06,-8.343385355527749e-06,2.653392552080098e-05,4.4711415795713614e-06,1.608268364942029e-06,-1.083651839132596e-05,1.922505975035783e-06,1.5247636695956272e-06,-5.711495182577137e-07,3.8032377461664972e-06,-6.679207517845896e-07,3.744276821058801e-06,3.948975849462067e-07,3.0426145278140032e-05,-5.422044255318906e-06,1.3513140332023977e-06,4.7652663720661523e-07,1.5410039431653756e-05,-1.6701645473498597e-06,2.702170315068378e-06,7.53763423893647e-06,-4.188544768237014e-05,2.4831034631241758e-08,-8.214824823653012e-06,-4.856656656941321e-06,4.755888537919417e-06,1.7372583077674524e-06,2.088969176786734e-06,1.8287668560402089e-06,3.0291557814561487e-05,-9.393657466256777e-08,-1.4652446367528558e-05,3.254723947518199e-06,5.33220842111942e-06,-2.514031031785878e-06,-6.140415973790396e-06,-6.365733697482793e-07,-6.7915328651570825e-06,-1.9446231220547063e-06,1.1647905774991371e-05,-4.467931842681335e-07,7.31387460617911e-06,-6.429047466422428e-07,-1.5634101506988053e-05,-8.904273421918246e-07,-1.4626138691323165e-05,-2.5134974075573034e-06,-7.395302770685977e-06,-2.587750546950827e-07,-1.4287598555381668e-05,-1.3184813828201797e-05,-2.3954861879137376e-05,-4.329052962063734e-06,3.3160625689297598e-06,3.8311796559279075e-07,-4.4960321656948335e-07,2.4035624763494786e-06,-3.323292862035346e-06,-5.712175800453974e-07,4.992554741148193e-06,1.0139065837421567e-06,-4.50906722270972e-06,-4.771155549988205e-06,-8.597014102468117e-07,-1.0120854582321243e-08,1.757600324762344e-06,-1.592866675543513e-05,2.173479377973399e-05,5.693622335679842e-06,5.213904530259205e-05,-3.045118330161949e-06,3.5575675884114786e-05,3.4315221094629115e-07,-4.785899239677769e-06,-8.377064327979985e-06,4.04608503099622e-07,-1.4264614150074267e-06,1.0816399521825547e-06,1.358105257728561e-06,-7.710075609574332e-06,1.4533769962688716e-05,-1.9530081215544084e-05,-2.473022538086929e-07,-7.695436163531953e-06,-2.936207076000368e-06,-2.2883786984552116e-06,2.9721822938251593e-05,-8.252341623019869e-07,5.574385454872865e-07,-3.1629943487030605e-06,-1.0082297679832828e-05,-1.741084133407358e-05,-4.1572359128765146e-07,4.208673705625339e-06,-2.35719341016454e-06,-1.1186907565584253e-05,7.985390651225009e-07,2.6495937469203072e-05,-8.495369735376866e-06,-3.635555053817378e-05,-8.062969102183433e-07,-1.8874871344017459e-06,-1.3026023619473314e-06,-3.675108163695338e-07,3.854026075760972e-06,-1.3899756363702173e-06,-2.798518160984776e-06,4.792304836971452e-06,-1.969269176446633e-06,1.279568584170166e-05,3.357638306608337e-07,-2.6573672011677565e-05,-8.409294630058743e-07,-1.434790500298166e-05,-6.757036361130033e-07,3.940325719121529e-06,-1.3821627105272826e-05,-1.5674575743688872e-05,2.913912802809119e-07,-1.6493247948096892e-05,-1.6496267059870048e-07,-3.749185911930564e-05,-1.0904189691904834e-05,-7.134797103681234e-05,1.806851223422477e-06,-6.507580316623706e-06,1.8377483082140068e-06,-2.3086075221612737e-05,1.5548284524927813e-05,1.3110259976179182e-05,-2.7310936161592538e-06,-4.7448136470465414e-07,-2.713958315615156e-07,-1.1191762060439842e-06,-4.801877517878784e-06,-4.324498406933018e-07,-2.9064391997784436e-07,2.06761408578405e-05,-2.0804902207818693e-06,-7.069444636064058e-06,-6.156361424590791e-06,-0.00012211000899399759,-4.41286692525558e-06,5.603707272646567e-06,-1.6804454737861899e-06,1.8073297487459764e-05,-6.1349608646762356e-06,7.285415597698875e-05,-1.2194346083732833e-06,-8.84368262581237e-08,4.707694116103277e-07,-3.0328462101452916e-05,3.9170923265405155e-07,-5.197985169697793e-05,1.6154701677346252e-06,-1.9296997201501786e-05,4.1712197424705774e-06,1.0694141718781693e-05,3.733074457242541e-05,-6.798025668840064e-06,-1.294742793984496e-06,-3.985294636403967e-05,1.6207420050977567e-06,-2.3432866402860495e-05,3.4495000157130784e-06,3.505431207435567e-06,-3.7094315103597667e-07,-3.532836514660363e-06,-1.9510395236308434e-06,-7.156329166073397e-07,1.6073994297321232e-06,3.343559318255162e-05,-2.797784384069758e-07,-2.2908405840076658e-05,8.377895615864147e-07,7.722130214163127e-06,-5.825332539405897e-06,-2.73118106774656e-05,-1.7894839153169222e-06,-1.4239585724896019e-06,9.60367541051187e-07,-5.495421519761961e-06,-1.8982050303944771e-06,7.752645253202233e-07,-8.096984853532953e-07,-4.73459040121521e-06,1.1549767830979961e-06,7.054173677060502e-07,-9.290657719970698e-06,-2.3702825858004007e-05,-7.190663029027541e-07,-1.2641348484959998e-05,-1.8836569822399321e-06,4.71030609920177e-06,-1.0784277147787274e-06,8.909002306491975e-06,-1.3715388123897034e-06,-1.8155321949510424e-06,-9.83303466141324e-07,-5.0340053727461e-07,1.1094572275003258e-05,-5.975682491895452e-07,-6.492395960958531e-07,2.447741501876232e-05,-1.5266198414957303e-06,-8.426379306189204e-06,-3.4237878786256685e-06,-3.199855542066984e-05,1.0136539315325838e-06,9.82566870300975e-07,-3.881062269886607e-09,-8.12179380903464e-07,1.9170345864809355e-05,-5.824093189022494e-06,-9.729575437563181e-07,-2.795550995712965e-05,1.1041805462444688e-05,-1.3871472096094828e-05,8.144686342390895e-06,2.408716925204961e-05,-1.335954239782072e-06,-6.556257550850486e-06,-1.6136030714843874e-06,3.823700191021358e-06,-2.0908218654220086e-05,-3.897693852995235e-05,-1.1762927923569405e-06,2.6667078969226517e-05,-3.8053000437746286e-07,-6.948849665660348e-06,1.4194454493768827e-05,-3.477383456805753e-05,6.405809975853433e-07,-8.605037738120806e-06,-4.29947730626282e-06,-8.913663894343553e-07,-2.2590300642410996e-06,-6.438283319090629e-06,-5.709853140767267e-07,1.231658175727895e-05,-1.8977625115440665e-06,7.853082404392354e-06,-4.3927793761427634e-06,-2.6272129666301585e-06,-2.288317250164427e-06,-4.79088983201925e-06,-3.9832161551169105e-06,1.3766024695047053e-05,-6.995327726150356e-06,-4.58224114715624e-06,-6.682004913233796e-07,-6.328413857768096e-06,1.0975455983872068e-06,-7.083789665578684e-08,-7.597589332732487e-06,-4.496463506266502e-05,1.629935185853532e-06,-8.594882857396674e-06,-1.6015807860999728e-06,-1.9005030524569443e-05,1.2409877745101338e-05,1.1876960795993048e-05,-5.483152122864246e-07,-7.93444535984309e-06,1.065199856742255e-06,-9.074382193642404e-06,-1.3239288725742326e-05,-4.3283853163681364e-05,-5.617521667590839e-07,3.678290863436554e-07,-2.7833000685641077e-07,2.519972205504416e-06,-1.4489075729688445e-06,2.3639918545954066e-06,-9.5102678767149e-07,-1.3302592342422935e-05,-1.5557724622520766e-06,-6.069022006766282e-06,8.494609990772314e-06,-1.020149837696365e-06,-2.896393848999693e-07,2.339002434452026e-05,5.239175430558269e-06,9.177369043701143e-06,4.3553047806513036e-05,2.7302940642349545e-05,-2.8156669319848185e-06,2.3178988475042087e-06,-4.603787561015162e-06,-3.5343108979669248e-06,-1.260564892795319e-06,4.407547854107495e-05,-5.577227545478817e-07,2.0146452308232084e-05,-1.1766832901539667e-06,-7.13732648968967e-06,-2.1804155496195794e-05,-4.855667926289643e-05,4.3481074756139054e-08,1.3269069507080988e-06,-2.0898444650444914e-06,4.3016186277882336e-07,3.45881396879054e-07,-3.5464673281604734e-07,-4.5854280624566723e-07,2.7299026820085797e-06,1.9322514656449914e-07,7.767590019019895e-06,-1.2627383166495714e-05,-1.3292871982137872e-05,-6.77910137129251e-08,9.918460832101504e-06,-8.950698141410746e-06,1.4092581850696523e-05,-2.802678227472355e-05,4.4021292535712814e-05,-2.6770192121674306e-06,-3.632347015208537e-06,2.5896083690502815e-06,-2.4111722903316313e-05,2.359941489720395e-05,-1.4752935836849136e-05,-3.50433408417946e-06,-4.456155348911241e-06,-4.8138707277728185e-06,-3.949914319837306e-06,9.28800623895839e-06,8.462335128493275e-06,-1.5659295511982564e-06,4.255544915296944e-06,-4.2373474505922285e-07,-1.9165463255698315e-05,2.2852020465510506e-07,1.4845377096569837e-05,2.9104328001703626e-06,1.9921007071025953e-06,-1.3216031494093355e-07,-1.3109075045025039e-05,3.831456325289889e-06,-1.4405078496905663e-05,-1.0924986568865793e-06],"level":[-0.02074210647962399,0.0036825227865057934,-0.01629926114716389,-0.018864953137700747,-0.04483258887059382,-0.0011798675623386325]},"G":{"mean":[-0.00046353116459633726,-0.0001183839808821036,-0.0007401491718195208,7.923113465705574e-05,-0.0009562940053138807,0.00034745585427768303,0.0003029621520613067,0.00015633928565639247,0.0004163828665455927,-1.5035575455440372e-05,0.0005676726621790591,-0.00027863298757626004,0.00034709720671065466,-6.580785062215738e-05,0.001155741097789035,-0.00023073689084276972,0.0017386142247783236,-0.00040144078358830837,0.00029371519697729694,4.1904864521974256e-05,0.0010980874678199661,-0.00015313415854762785,0.0020430307303620637,-0.0003076962844558643,0.000368409468028711,0.00019016478582124327,0.0009643925172986145,-0.00016400142298960742,0.0013712344501057997,-0.0008044216078591777,9.171943666522716e-05,-0.00016145384348642276,0.001449669639198806,-0.0004462521944795739,0.0022376476991687357,-0.001035806889500823,-0.00013222258024203043,-7.545984974208912e-05,-0.0007227649051176442,0.00017598780543476663,-0.001089985432382719,0.0006864279830930278,-6.359439933768896e-05,0.00017173450035437563,-0.0014359027046983708,0.00043632084217064574,-0.002246114800567887,0.0009539759570989891,0.00044141693789787994,2.1999547847469115e-05,0.0015387520159064088,-0.00035116944551775596,0.0022075170763298734,-0.0010210279282429422,-5.092159078755151e-05,-0.00021136415549609944,0.00035843492189538694,-0.00013707818359545815,0.0005756900018082271,4.933328455909981e-05,0.0005719044003285721,0.00012665974309563095,0.0012776383507820525,-0.00020667085619396124,0.0017869923571633929,-0.000721837713054229,-6.304046995111817e-05,-3.3352334586330825e-05,0.0009401148246789834,-0.0003020621367929169,0.0016699826669786177,-0.0008986502675878959,-2.1906826783557212e-05,0.00014170962225329404,-0.0013015752295729393,0.00041821383282540924,-0.002024063143238853,0.0010119957817716111,4.324216104186768e-05,-0.00017086228202713318,0.0012550420961695353,-0.0003871392605855667,0.001986624763312597,-0.0008193372393156666,2.034885126401021e-05,-0.00010414302356295505,0.0009104312849208762,-0.00028237031628924273,0.001462316918015011,-0.0006557359346401751,0.00033441639840290613,3.2154967157483926e-05,0.0003353849775125244,-4.465872462905307e-06,0.00030470427995610684,3.7473728337632086e-05,0.0003563029894475319,6.743494657359021e-05,0.0008734016319747533,-0.0001454936011011226,0.0012759548321004982,-0.0004660257631969656,-0.00013171995660059496,0.00017644280053936198,-0.0013567623485268843,0.0003668939178500866,-0.0021961596736892915,0.0006682676998351329,-2.4745794584707948e-05,0.00013798531492074573,-0.0010543454685194248,0.0003514618383310769,-0.0015895942427795315,0.0008251874262348194,-4.782290206804639e-06,-0.00010411970105583025,0.0011633889786692195,-0.0003768054823742635,0.0018354260381472332,-0.0009729187102122391,4.367330755597561e-05,0.00023993484848575274,-7.248759662564514e-05,1.6985563058736535e-05,-0.00016164788402971413,-0.0005575752326276771,-0.0003635859529438426,-7.191272397446425e-05,-0.0008570787608040751,0.00013112729047261106,-0.001275518744050196,0.0004150002287983009,0.0003494467379205429,8.85016332486092e-05,0.0007686605539465719,-0.00010252909688593513,0.0011390180589238865,-0.00036170691047343747,-0.00044813337636702026,-4.3704802610994756e-05,-0.0015287320545949615,0.0003377731670487532,-0.0022266328269560307,0.0010035426842447228,0.000332998804395036,0.00012821966031108462,0.0003150559847233405,1.2923650020585502e-05,0.00034263123114298516,-0.00012686567332608772,-0.00017760335680024847,9.709475088256481e-05,-0.0013865086806604235,0.00040162413850689167,-0.002058555126326301,0.0010151444539223202,-0.00031550989130462234,6.0626380870261604e-05,-0.0015229236378136086,0.00039309325007686206,-0.0022162026672707443,0.0010071241274476259,7.308713549560309e-05,-8.803008014297171e-05,0.001345201020666412,-0.0003785260078450475,0.0022458468818996784,-0.0009332005200887405,0.00034021793579461717,-2.940275444217311e-05,0.0015419080889804357,-0.00036526587992257147,0.002371333782745619,-0.0009492494548510506,-7.745489011111479e-05,-9.323325445051978e-05,0.0011063697541272115,-0.0003653354246567187,0.0018547661076214932,-0.0009674021437127768,-2.8594678811941862e-05,-0.00016176764018685527,0.0009384616209947975,-0.00032083216031936836,0.0014742844350761177,-0.0006710391076983518,0.0001627362340273544,8.481443660981698e-05,8.193422940855989e-05,5.4188343097007826e-05,0.00013822308145567958,7.635052282378004e-05,2.3996399399485373e-05,-0.0001638850468220876,0.0011064393126747126,-0.00037928796492159533,0.0016360956267975945,-0.0008687078802135477,0.00029313832034793077,-3.9599342569082826e-05,0.0012375527298139988,-0.00030169396785748113,0.001816131513649394,-0.0007598396614067988,0.00020065460376376783,0.00016796883947564904,-4.55284732576094e-05,0.00011122063512338826,-6.873034320210428e-05,7.800280704978605e-05,-1.1478332181622482e-05,-0.00016077194127739783,0.0009926443724012116,-0.0003370237873389014,0.0015328444057507466,-0.0007250350642978375,-0.00018331110673641758,0.0001302012031318821,-0.0013954610539486452,0.00040558450003968014,-0.002063093100533218,0.0009454393390606587,-0.0005707373312462115,4.816401341876694e-06,-0.001139268837548146,8.975503112780291e-05,-0.0018281718921257697,-0.00010599992870459569,-0.0003924688623970519,4.902676721046058e-05,-0.0015910318840902345,0.0003302145545388797,-0.002619086818525047,0.0007209870624060391,0.00033067895870891027,0.00017490166548790576,0.0012206071106942232,-0.00023017575215317134,0.001852766202539057,-0.0009531508318877993,-9.146598811819991e-05,0.00016414139449651065,-0.0014456369865507438,0.0004303895741401019,-0.0022467522258447226,0.000947318661983718,-0.000647359006219907,-0.00010143690505155331,-0.0013090154351923667,0.000184969296810927,-0.0018326760899712818,0.0005448240112889625,0.00018804519486963686,9.59579867434065e-05,5.345346756932393e-05,7.108756720342505e-05,9.385771854040946e-05,0.00011259215055865161,0.00041859772493146814,-1.1566306040912924e-05,0.0012439295284643794,-0.0002260944049590828,0.001913197519928042,-0.00047702271642007736,0.0006377885580643452,7.36140478365323e-05,0.0013055891206815932,-0.00014308285947656796,0.0018552415959849115,-0.00025195327374657615,-0.00029883638185961524,0.00011285879333133316,-0.0006385934707758623,7.104411816078119e-05,-0.0009809757688831336,-0.00024261074398655732,-0.0002237731469512244,-0.00011448824123286405,0.00035944937371120534,-0.0002231026225833691,0.0005626846280564182,-0.0005922794029043951,0.0004152437573451708,5.833247304284975e-05,0.0013012734876846493,-0.0002642809969248471,0.001894313626820619,-0.0008058498165479235,0.0004027848644114261,1.1817848510011521e-05,0.0015140096939101613,-0.00035936093775308084,0.002171089037321866,-0.0010304665627570327,-0.0002785889540316063,7.148425859884161e-06,-0.000493076321659471,4.0514318586073604e-05,-0.0007489161278600534,-4.5588242739931553e-05,-1.2670315708720044e-05,0.0001562178936567724,-0.0013857135183575602,0.0004408619951407785,-0.0021724089955680406,0.001038105886347129,9.729387073466714e-05,8.452498088389172e-05,-0.0002546669353028014,0.00012130639906052653,-0.0004285300778330861,0.0002377059538725805,0.00047891419893290085,6.640243496565443e-05,0.0015238328310268345,-0.00031542507864494845,0.002263067094831389,-0.0009694233535559419,-0.0002874905575954701,-0.00015555192471098923,0.00022242861080708803,-0.00011824793840687599,0.0006817384229160413,-4.521851967015597e-05,0.0002921987551044608,-6.453176818555962e-05,0.0015374979902293626,-0.0004046817000112408,0.002251233424144004,-0.001047740903290558,0.0004923152816616073,0.00011139185059978206,0.001104688291494151,-0.00016131474818680761,0.0016089742118583687,-0.0005471589579154178,-0.0001000113299972191,2.0476362414673282e-05,-0.000285385852894574,5.342905389895299e-05,-0.00042767380109944047,7.2435502241178e-05,-0.00014085229050716407,0.00014559726205339235,-0.001221744784206547,0.00035677072930904335,-0.0018040528099915825,0.0007577120933994752,-5.174927485924129e-05,3.0099517165423802e-05,-0.0008674559043764533,0.0002731884723858378,-0.0013025333507065864,0.0008130047465748475,-0.00026172559840550416,-8.40521416674713e-05,0.0005249992057534057,-0.00026911209158138246,0.0009626761136195927,-0.000809655501691027,0.0001521665483309408,9.200256817382471e-05,-0.0003553880442491547,0.00014330639859720795,-0.0007092516981971042,0.00027307881027516126,0.0006437910159682541,7.251090115148512e-05,0.0012212980657652847,-0.00011773644600438087,0.0018371572445118863,-0.0001959214809881973,-0.000678129567441336,-9.870019016748552e-05,-0.0011214619537159316,0.0001713087554438825,-0.0011703176250579621,0.0005423077310508516,0.0002932547766153451,-6.856501625293027e-05,0.0012672989098301388,-0.0002782430320580153,0.0020027367802560167,-0.0005659384344167015],"log_std":[6.476376443388881e-06,6.962400750581427e-07,-6.263432774200678e-06,3.9981743285001164e-08,2.9970269826087436e-06,2.693957614303883e-06,1.3603353224288474e-06,-2.624977837406908e-06,1.050000218378164e-06,-9.489447588186067e-07,7.940405780867586e-07,-9.470877196520644e-07,-2.2157106568523768e-08,-4.675361522111796e-07,3.0300660864643806e-05,-2.511273170704806e-06,1.0338911072615606e-05,-2.0842221647090803e-06,-4.529613242268641e-06,-7.531587012645082e-07,1.5021714261789646e-06,-8.06649494399034e-07,-9.173115703545784e-06,4.6628935568749145e-06,2.0181565910341373e-06,2.478081667944039e-06,-3.576270433447379e-05,3.8461062865247065e-07,1.3405301650840245e-05,9.065365965845035e-06,-4.5676205742673024e-07,-1.2652426534799937e-07,-3.83768471370941e-05,-1.4405303734758055e-06,-3.265046853965097e-05,-1.121266795716956e-06,5.160187128648842e-07,-5.132521145866203e-07,1.9273671485754685e-05,1.3930543706204765e-06,1.2497672175043118e-05,-5.9364388022954616e-06,-1.1810960728356638e-06,-4.007145737440969e-08,1.7984393168050853e-05,-1.7914312153393497e-06,1.3005422664956494e-05,4.635384711402052e-06,-5.348860197066482e-06,-1.1495169332107597e-06,2.4133487536826957e-05,5.8577139340581266e-06,-6.846796738053331e-05,-2.1001364234415728e-06,-3.727279476042328e-07,-9.243883897551101e-07,-7.995348898297052e-07,-1.5369885704546942e-06,2.091511787797822e-06,-1.1620684422099485e-06,-1.15069633549996e-05,-3.6968224463112007e-07,6.714594209977602e-06,-1.827741599632073e-06,2.277536685800919e-05,5.186773721893613e-06,-3.4042892905158967e-07,-4.146513393636154e-07,-1.1769624473031222e-05,-7.45128536711061e-07,-4.009952440379546e-05,-1.0669636807842732e-05,-8.297914951802871e-07,4.838659738451683e-07,-3.4948333932539455e-05,-5.330672137047809e-06,-1.0015102066522799e-05,1.2627302660680822e-05,-4.3452706016465404e-07,9.561068476299816e-07,-1.2277591117834425e-06,-7.505564108277434e-06,1.2034757989040663e-06,1.2748776157068465e-06,-6.232695872725263e-07,3.7527393249729793e-07,-2.9625549979416198e-06,2.920395663351598e-06,-1.1532934923150576e-05,8.545350078048797e-06,2.6558496332683853e-06,-1.1812818762053487e-06,4.765610895541967e-06,-6.856098220215001e-07,-9.60672234863958e-07,-9.554269625146263e-07,-2.8270456990903366e-06,-1.3575908006862225e-06,-5.424737898713682e-07,4.210113079588095e-06,1.11929086823091e-05,-4.017534965208341e-06,-5.647647138509573e-07,1.9738846262985855e-07,1.9352988369851088e-05,-5.324763568251511e-07,-1.2796264440717e-05,1.7710714665264538e-06,-9.094494865417111e-07,5.148768825688819e-07,-1.1493228266069754e-05,-2.196787166892023e-06,1.370386899239671e-05,2.9131337350538537e-07,-6.494332182147607e-07,-1.8474124975705646e-06,2.6208685989014756e-05,-3.017328800551909e-06,-1.7460917584309965e-05,-2.007715496507907e-06,-1.5143006636053431e-06,4.4415451518454995e-06,-2.208790099258014e-06,-5.265976047399472e-07,-1.724030118695053e-06,1.544814616526155e-06,2.5173230827891223e-06,3.4886226639857885e-08,1.7835793513658103e-05,-4.777301398813618e-08,-3.034281848954956e-06,-1.7508265143049002e-06,9.410903764995726e-06,-3.29819683711353e-07,-5.418441493128522e-06,3.507137931287479e-07,-7.280726548517891e-07,-1.70275435756687e-06,-3.848543119219526e-06,7.08104053166869e-07,3.626473614903882e-06,1.5291089330501166e-06,1.780585951418141e-05,-1.2571263773963357e-05,-1.09115452163773e-06,2.7667763379796728e-06,5.499418378537861e-06,-5.808351153193238e-07,6.696709966836131e-06,5.02853835133436e-07,-1.0797875106051909e-06,-1.985649958968719e-06,1.4897837640810936e-05,-4.265129400038814e-06,1.0280100684872454e-05,2.917447585175439e-06,1.0144176116425706e-06,-3.043370068289389e-07,-1.646077846119345e-05,-2.105872666382776e-06,-2.6160265287297684e-05,-7.273542286376689e-06,-8.947217003365965e-07,-1.1608455555231002e-06,2.530530380523434e-05,2.3934584812233224e-06,4.19375526374359e-06,-3.6470632599692755e-06,5.597136624933491e-06,-7.810077347065503e-07,-3.1235621544568677e-05,4.821743913110822e-07,1.453423080686854e-05,-9.10777157285112e-06,-1.0449173060552907e-06,-7.365743988105262e-07,2.5458958652081758e-05,3.7584485689620027e-06,-9.411959632206983e-06,-1.2699776405224744e-05,-9.040594536462674e-07,-6.212011898918081e-07,-4.198483294827086e-06,-1.772790141730815e-06,-3.9705757865797955e-06,9.364882700668859e-06,-3.077574207558853e-06,-2.0702276223501072e-06,-5.021394598178333e-08,-5.041349044498765e-07,-2.048384283850338e-06,-5.062505300072287e-07,-6.576712759255334e-07,1.3584007422725584e-06,-2.018907132615376e-05,5.6802208416364165e-06,-1.0250039225322148e-05,1.0495214405963494e-05,1.1022394625103342e-07,-8.283851468832174e-07,2.1504873324472455e-05,-8.988604053407571e-08,-1.6134300858072957e-05,9.88914399626246e-06,4.374413459066991e-07,6.528834890730708e-07,-1.4111731826732024e-06,-8.57951288314335e-07,-5.613466794682438e-07,-6.736285110799189e-07,-6.643205544652232e-07,5.266251233910781e-07,2.1959774683152693e-06,4.027507768527143e-06,-1.1607498270388602e-05,-6.220582362309863e-06,-6.103322509959842e-07,-1.8571526276458553e-07,-4.554399965270145e-06,-2.188680874844217e-06,1.0633466540420852e-05,-8.295151426407491e-06,-9.0511595607457e-07,-7.117552086896046e-07,-3.541845348089231e-05,1.338893225306865e-07,-1.9877885485731945e-05,4.5212699207135306e-07,-6.465078474024985e-06,4.776727553899658e-07,-1.108117399231807e-05,-1.6371799046296986e-06,4.283027994764294e-05,-1.2099299097323801e-05,-9.405868414944307e-06,-5.231328137588272e-07,-9.44047127262644e-06,-4.85133717069881e-06,-5.042416441914946e-05,7.583091829709013e-06,-9.497245915953508e-07,7.518169598873604e-07,6.123999695406272e-06,4.5543380496443685e-06,5.9929322480429406e-05,1.1824146522134918e-06,-5.964272147510212e-06,-4.4458752066777747e-07,6.804794572408278e-06,-4.356414676396225e-06,4.525185229577847e-06,-4.583417312647742e-06,-1.3371546282121327e-06,4.996979384657034e-07,-1.8295669154977124e-07,-5.284686386033053e-07,-1.460239182736612e-07,-1.3807069267349988e-06,7.96190067809598e-07,-5.279018102747004e-07,-3.469929585792025e-05,-3.829296268150211e-06,-2.4371087625218612e-05,2.1409462235024284e-06,-3.1582118376423636e-07,-1.0367082840793106e-06,1.6736661551014948e-05,-2.383934228036336e-06,1.1769901871643747e-05,-3.924647303779136e-06,-2.5110984846329026e-06,-1.0118465869177178e-06,-2.7625011910987354e-05,-1.6807308921093144e-06,-2.3643370638590962e-05,3.06673550635117e-06,8.372115472074586e-07,3.5500587302565865e-07,-7.608869323722783e-06,-1.431787788815237e-06,-7.675391641913353e-07,-4.465259675957541e-06,4.6867046608892255e-07,-5.208534293378413e-07,-2.1032981951114175e-07,1.0871176373922968e-06,3.9172847806235294e-05,1.0913069225945599e-05,-5.937405833216423e-06,-7.238341677661398e-07,-2.216822783480189e-06,2.9327560651286366e-06,1.3126599845899706e-06,-6.1485607734329564e-06,3.123249414246213e-06,-6.171270959902427e-07,-6.336547938674328e-06,-4.736554887717653e-07,3.952266543101757e-06,-7.09250198119982e-07,-7.259686035537836e-07,-4.763673740599573e-06,2.2185393052751735e-05,-4.252408734498883e-06,4.6452047900042814e-05,-4.678615489453344e-06,4.5498137112102773e-07,-1.5618014574961653e-06,-5.404660164586945e-06,-1.041751068112656e-06,2.0249024855387543e-06,6.316011144084789e-07,4.88405646714755e-06,-3.7106485726587144e-07,-1.704613341524352e-05,-1.847850517967786e-06,-1.6072391008061003e-05,-7.852444059198699e-06,-5.576160022682118e-06,-1.8969856019328801e-06,2.761289876668685e-06,1.1518202971945677e-06,2.2724929600421527e-06,-9.859115090349938e-07,4.111741320124632e-08,-9.798055009737092e-07,-8.376975804918972e-06,-7.165078440192996e-06,-2.446916704705643e-05,9.416060562792023e-06,-4.52317853951158e-06,-1.6949816180542026e-07,9.428090255516748e-08,-1.3732918642040796e-06,1.230909771477625e-05,-1.8864259577448218e-06,1.058991980516573e-06,-1.2622661660848084e-06,1.0398056299390904e-05,-2.2506257360784012e-06,1.3986724548517994e-05,-1.1447077106700416e-06,-1.1000444998950479e-07,-2.017721085395588e-06,-8.338470320632446e-06,-2.2131927210706976e-06,8.237337234799452e-06,1.2491542203774936e-06,-4.452404523599835e-08,-6.818659592359471e-07,1.041654618106481e-05,-4.397345707079471e-07,2.661909050014104e-06,-3.5490489651513054e-06,-8.109212167696906e-07,-1.5175452637458706e-06,3.678269414061616e-06,-1.273114340806876e-06,4.3087752729223505e-06,-3.575900201387758e-06,-2.6409583702283307e-07,-1.2138237249397968e-06,6.764483417396324e-07,4.5406539199930697e-07,-2.0687399332317688e-07,1.8681905234564315e-06,-7.662956575408762e-06,-1.7074336648694299e-06,-1.9764235468833518e-05,1.9050656151916644e-06,-8.402715450618288e-06,-1.8814905381176986e-07,-3.1441932569466233e-06,-4.873630482278265e-07,-6.01347712023811e-06,2.568248357854976e-07,-3.289684679205989e-06,-7.102056344250477e-06,-3.7546172122735753e-06,-1.0924649551356502e-07,-7.599841313085617e-06,1.5645937489625649e-06,-2.5129169168079755e-05,-1.1297373634618405e-05],"level":[-0.0005064332219386664,0.0009824577512947154,-0.008362479574717252,0.0026072345189184615,-0.012721793846838804,0.0059868153850152995]},"V":{"mean":[-0.00026137591314918994,-4.652483278910916e-05,1.3423615206193648e-05,-6.602073226681269e-05,-9.369659353617246e-05,2.4776218621767006e-05,-1.7913022156526762e-05,1.5242371329529897e-06,-0.0003535654896083465,0.00015496002637533114,-3.368307685170248e-05,-9.035214851068222e-05,9.294628422633855e-05,3.767776165104665e-05,0.00036852205595308866,-0.00013351868994219908,-4.58286334225773e-05,0.00012290330230493808,5.02286329169305e-05,8.010760815224968e-05,-1.726433260957451e-05,-0.00010721042599310014,0.00044354975497930645,-3.784235241859036e-05,-8.767247870955667e-05,5.716544521041293e-05,-6.12646367550696e-05,1.5200693248090125e-05,1.6973842061686812e-06,0.00013166679861442368,2.6086176650100608e-05,6.598576229058658e-07,2.4824302491370183e-05,7.925941477092153e-06,-3.893005747006645e-05,-2.4583436309687847e-05,-2.311062789968919e-06,5.119429177439321e-06,-1.0273644412245872e-05,-1.4527949115312627e-05,-1.3117390851470302e-05,-3.7880949939989845e-05,0.0001659161298139667,-8.160512994665717e-06,-1.687325275176998e-05,-1.4610089119712624e-05,-6.671985912082969e-05,-0.00011991847964666512,-0.00011577011390523465,4.034827190692108e-05,-7.734130808998541e-05,2.988909982950296e-05,6.989369972654018e-05,6.104717121977889e-05,2.689190122942779e-05,7.760953400895877e-05,-1.1718689297842915e-05,9.102833429671155e-05,3.447993446391066e-05,4.418085958347586e-06,4.799348496170809e-06,-1.4398748211840751e-05,-3.56624635278732e-06,7.419596003441897e-06],"log_std":[-4.181064189895801e-05,-4.977326641702001e-06,3.3922628785317693e-07,-8.82513496838472e-06,4.209635728217685e-05,2.059995513894769e-05,-2.1602724457935727e-06,-1.022510543992579e-06,3.771907035567793e-05,-3.710337304001829e-06,-4.525892193139094e-06,2.7860430456566477e-05,-7.395264594977278e-06,-6.073409661815768e-06,7.824684609188772e-06,6.711165704196049e-06,6.805812022612468e-07,4.672383340970234e-06,-1.79822931693541e-05,-1.728600872407809e-05,-1.3934223425098145e-07,-4.164675378892497e-05,-5.30173859006213e-06,1.962330360833676e-05,-3.061053423179903e-05,-1.2548845907849473e-05,-1.3413901197580376e-05,5.386452380020586e-06,-2.4093343628700035e-06,5.651514484470009e-05,1.3951272576760922e-05,-6.289722841543644e-07,4.877691246408631e-06,3.2796376654324515e-06,1.4674341174724194e-05,-5.3001087408085576e-06,4.6540322678726596e-07,-1.5637730834498542e-06,2.6213259352675586e-06,-2.947612481165919e-06,-7.245517892952879e-07,-2.9149278612844565e-06,6.737478447891252e-05,-3.2574488806873865e-06,-8.943251821998667e-06,3.485150992484832e-06,5.8296861486395315e-06,-7.88044446907825e-06,-2.494463734147765e-05,-2.4917980558202193e-05,1.7597131715518244e-05,1.9400521331498242e-05,-3.562333447590893e-05,4.7979689231400184e-06,-2.0049987028907932e-06,8.37238231968067e-06,-2.653554787411519e-06,4.715512719684279e-05,5.115337876141068e-06,-2.755155038150102e-06,-4.471767381822541e-06,2.0293938561926725e-05,4.650380681740942e-07,-5.9886670254531136e-06],"level":[-0.019823244060045642,-0.005519574630661041]},"process":{"log_sigma":[0.0,0.0,0.0]},"likelihood":{"log_sigma":[0.0,0.0,0.0,0.0]}}}} diff --git a/src/cvc/tests/ph_se2_test.cpp b/src/cvc/tests/ph_se2_test.cpp new file mode 100644 index 000000000..cbc7122f6 --- /dev/null +++ b/src/cvc/tests/ph_se2_test.cpp @@ -0,0 +1,408 @@ +/* + 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::ph's SE(2) port-Hamiltonian vehicle (inc/cvc/ph/se2_vehicle.h): parity with the JAX +// package it ports, and the structure it must have whatever its weights are. +// +// tests/data/ph_se2_golden.json was generated once, outside CI, from the JAX package in float64 +// (a fit of the Humvee to Jolt runs: 64 features, 1,691 parameters): the model's pieces and one +// Lie-IMEX step at 16 states, with and without a noise increment, and a 25-step rollout. Double +// must reproduce it to rounding; float to float precision. +// +// Analytic checks need no golden: the energy balance dH/dt = -v.D(v)v + v.G(v)u of the drift (the +// coadjoint and potential terms do no work), the hand-written linear-tire bicycle the model reduces +// to with its GPs off, and the step's second order. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +using namespace cvc::ph; +namespace fs = std::filesystem; +namespace pt = boost::property_tree; + +fs::path golden_path() { return fs::path(__FILE__).parent_path() / "data" / "ph_se2_golden.json"; } + +const pt::ptree &golden() { + static const pt::ptree tree = [] { + pt::ptree t; + pt::read_json(golden_path().string(), t); + return t; + }(); + return tree; +} + +const se2_model &golden_model() { + static const se2_model m = se2_model::load(golden_path().string()); + return m; +} + +std::vector numbers(const pt::ptree &node, const std::string &key) { + std::vector out; + for (const auto &kv : node.get_child(key)) + out.push_back(kv.second.get_value()); + return out; +} + +// |a - b| <= tol (1 + |b|) +void expect_close(const std::vector &got, const std::vector &want, double tol, + const std::string &what) { + ASSERT_EQ(got.size(), want.size()) << what; + for (std::size_t i = 0; i < got.size(); ++i) + EXPECT_LE(std::fabs(got[i] - want[i]), tol * (1.0 + std::fabs(want[i]))) + << what << "[" << i << "]: got " << got[i] << ", JAX " << want[i]; +} + +template std::vector as_doubles(const C &c) { + return std::vector(c.begin(), c.end()); +} + +// The worst |a - b| / (1 + |b|) over a case set, for the printout. +double worst_rel(const std::vector &got, const std::vector &want) { + double w = 0.0; + for (std::size_t i = 0; i < got.size(); ++i) + w = std::max(w, std::fabs(got[i] - want[i]) / (1.0 + std::fabs(want[i]))); + return w; +} + +TEST(PhSe2, LoadsTheGoldenModel) { + const se2_model &m = golden_model(); + EXPECT_EQ(m.setup.control_dim, 2); + EXPECT_EQ(m.setup.features, 64); + EXPECT_TRUE(m.setup.v_gp); + EXPECT_EQ(m.params.size(), 1691u); // the Phase A count, filter noise included + EXPECT_NEAR(m.interval, 0.02, 1e-15); // the median step of the dataset's float times +} + +TEST(PhSe2, SaveAndLoadRoundTripsExactly) { + const se2_model &m = golden_model(); + const se2_model back = se2_model::from_json(m.to_json()); + std::vector a, b; + m.params.for_each_leaf([&](const char *, const double *d, std::size_t n, se2_leaf_group) { + a.insert(a.end(), d, d + n); + }); + back.params.for_each_leaf([&](const char *, const double *d, std::size_t n, se2_leaf_group) { + b.insert(b.end(), d, d + n); + }); + ASSERT_EQ(a, b); + EXPECT_EQ(m.setup.D.frequency, back.setup.D.frequency); + EXPECT_EQ(m.setup.V.phase, back.setup.V.phase); + EXPECT_EQ(m.control_scale, back.control_scale); + EXPECT_EQ(m.provenance, back.provenance); +} + +// Double against JAX float64, piece by piece and one step, at 16 states. +TEST(PhSe2, MatchesJaxInDouble) { + const se2_model &m = golden_model(); + const se2_vehicle veh(m.setup, se2_draw(m), m.twist_scale); + expect_close(as_doubles(veh.inverse_mass()), numbers(golden(), "inverse_mass"), 1e-13, + "inverse_mass"); + double worst_step = 0.0; + int n = 0; + for (const auto &kv : golden().get_child("forward")) { + const pt::ptree &c = kv.second; + const auto s = numbers(c, "state"); + const auto inc = numbers(c, "increment"); + const std::string tag = "case " + std::to_string(n++); + double f[6]; + veh.vector_field(s.data(), s.data() + 6, f); + expect_close(std::vector(f, f + 6), numbers(c, "vector_field"), 1e-13, + tag + " vector_field"); + expect_close(as_doubles(veh.effective_damping(s.data())), numbers(c, "effective_damping"), + 1e-13, tag + " effective_damping"); + expect_close(as_doubles(veh.dissipation({s[3], s[4], s[5]})), numbers(c, "dissipation"), 1e-13, + tag + " dissipation"); + double G[6]; + veh.control_matrix_at({s[3], s[4], s[5]}, G); + expect_close(std::vector(G, G + 6), numbers(c, "control_matrix"), 1e-13, + tag + " control_matrix"); + EXPECT_NEAR(veh.hamiltonian(s.data()), c.get("hamiltonian"), + 1e-13 * (1.0 + std::fabs(c.get("hamiltonian")))) + << tag; + EXPECT_NEAR(veh.potential(s[0], s[1]), c.get("potential"), 1e-13) << tag; + expect_close(as_doubles(veh.grad_potential(s[0], s[1])), numbers(c, "grad_potential"), 1e-13, + tag + " grad_potential"); + double next[6], noisy[6]; + veh.step(s.data(), s.data() + 6, m.interval, nullptr, next); + veh.step(s.data(), s.data() + 6, m.interval, inc.data(), noisy); + const auto want = numbers(c, "step"), want_inc = numbers(c, "step_increment"); + expect_close(std::vector(next, next + 6), want, 1e-13, tag + " step"); + expect_close(std::vector(noisy, noisy + 6), want_inc, 1e-13, tag + " step_increment"); + worst_step = std::max({worst_step, worst_rel(std::vector(next, next + 6), want), + worst_rel(std::vector(noisy, noisy + 6), want_inc)}); + } + EXPECT_EQ(n, 16); + std::printf(" one step, double vs JAX float64: worst |d| / (1 + |x|) %.2e over %d states\n", + worst_step, n); +} + +// The same in float: float tier (the JAX fits trained in float32). +TEST(PhSe2, MatchesJaxInFloat) { + const se2_model &m = golden_model(); + const se2_vehicle veh(m.setup, se2_draw(m), m.twist_scale); + double worst = 0.0; + for (const auto &kv : golden().get_child("forward")) { + const pt::ptree &c = kv.second; + const auto s = numbers(c, "state"); + const auto inc = numbers(c, "increment"); + std::vector sf(s.begin(), s.end()), incf(inc.begin(), inc.end()); + float f[6], next[6], noisy[6]; + veh.vector_field(sf.data(), sf.data() + 6, f); + veh.step(sf.data(), sf.data() + 6, static_cast(m.interval), nullptr, next); + veh.step(sf.data(), sf.data() + 6, static_cast(m.interval), incf.data(), noisy); + // The drift's magnitude sets its scale; the step's state carries positions of 100s of metres. + const auto vf = numbers(c, "vector_field"); + double scale = 1.0; + for (double x : vf) + scale = std::max(scale, std::fabs(x)); + for (int i = 0; i < 6; ++i) + EXPECT_LE(std::fabs(f[i] - vf[i]), 2e-5 * scale) << "vector_field " << i; + expect_close(std::vector(next, next + 6), numbers(c, "step"), 2e-6, "float step"); + expect_close(std::vector(noisy, noisy + 6), numbers(c, "step_increment"), 2e-6, + "float step_increment"); + worst = std::max(worst, worst_rel(std::vector(next, next + 6), numbers(c, "step"))); + } + std::printf(" one step, float vs JAX float64: worst |d| / (1 + |x|) %.2e\n", worst); +} + +TEST(PhSe2, RolloutMatchesJax) { + const se2_model &m = golden_model(); + const pt::ptree &r = golden().get_child("rollout"); + const int steps = r.get("steps"); + const auto x0 = numbers(r, "initial"); + const auto u = numbers(r, "controls"); + const auto want = numbers(r, "path"); + const se2_vehicle veh(m.setup, se2_draw(m), m.twist_scale); + std::vector path(6 * (steps + 1)); + veh.rollout(x0.data(), u.data(), steps, m.interval, m.substeps, path.data()); + const std::vector got(path.begin() + 6, path.end()); + expect_close(got, want, 1e-12, "rollout"); + std::printf(" 25-step rollout, double vs JAX: worst %.2e\n", worst_rel(got, want)); +} + +// dH/dt along the drift is -v.D(v)v + v.G(v)u: ad*_v p does no work, and the potential's force +// returns exactly what moving through V costs. With the fitted GPs, at every golden state. +TEST(PhSe2, EnergyBalanceIsExact) { + const se2_model &m = golden_model(); + const se2_vehicle veh(m.setup, se2_draw(m), m.twist_scale); + for (const auto &kv : golden().get_child("forward")) { + const auto s = numbers(kv.second, "state"); + double f[6]; + veh.vector_field(s.data(), s.data() + 6, f); + se2_stage st; + veh.stage(s.data(), s.data() + 6, st); + // dH/dt = grad V . d(x, y) + p . dv (dH/dv = M v = p) + const double dH = st.grad_v[0] * f[0] + st.grad_v[1] * f[1] + st.p[0] * f[3] + st.p[1] * f[4] + + st.p[2] * f[5]; + const vec3 dv = matvec(st.D, st.v); + double supplied = 0.0, dissipated = 0.0; + for (int i = 0; i < 3; ++i) { + dissipated += st.v[i] * dv[i]; + supplied += st.v[i] * (st.G[2 * i] * s[6] + st.G[2 * i + 1] * s[7]); + } + EXPECT_GE(dissipated, -1e-12); // D is positive definite by construction + EXPECT_NEAR(dH, supplied - dissipated, 1e-11 * (1.0 + std::fabs(supplied) + dissipated)); + } +} + +// With its GP weights zero and its levels set from a linear-tire bicycle frozen at 10 m/s, the +// model's drift is that bicycle's body-frame equations, written out by hand. +se2_model bicycle_model() { + const double m = 2000.0, Iz = 3000.0, lf = 1.4, lr = 1.6, Cf = 8e4, Cr = 9e4, u0 = 10.0; + se2_init init; + init.features = 8; + init.v_gp = false; + init.initial_randomness = 0.0; + se2_model model = se2_model::initialize(init, 7, {1.0, 1.0, 1.0}, {1.0, 1.0}, 0.02); + for (se2_gp *g : {&model.params.D, &model.params.G, &model.params.V}) + std::fill(g->mean.begin(), g->mean.end(), 0.0); + const mat3 minv = {1 / m, 0, 0, 0, 1 / m, 0, 0, 0, 1 / Iz}; + const auto lm = spd_levels_of(minv); + std::copy(lm.begin(), lm.end(), model.params.minv_level.begin()); + mat3 D{}; + D[0] = 30.0; + D[4] = (Cf + Cr) / u0 + 1e-9; + D[5] = D[7] = (lf * Cf - lr * Cr) / u0; + D[8] = (lf * lf * Cf + lr * lr * Cr) / u0 + 1e-9; + const auto ld = spd_levels_of(D); + model.params.D.level.assign(ld.begin(), ld.end()); + model.params.G.level = {m, 0.0, 0.0, Cf, 0.0, lf * Cf}; + model.params.V.level = {0.0, 0.0}; + return model; +} + +TEST(PhSe2, ReducesToTheLinearTireBicycle) { + const se2_model model = bicycle_model(); + const se2_vehicle veh(model.setup, se2_draw(model), model.twist_scale); + const double m = 2000.0, Iz = 3000.0, lf = 1.4, lr = 1.6, Cf = 8e4, Cr = 9e4, u0 = 10.0; + const double s[8] = {3.0, -2.0, 0.4, 10.0, 0.3, 0.2, 0.5, 0.05}; + double f[6]; + veh.vector_field(s, s + 6, f); + const double vx = s[3], vy = s[4], r = s[5], drive = s[6], steer = s[7]; + const double ax = (-30.0 * vx + m * drive) / m + vy * r; + const double fy = Cf * (steer - (vy + lf * r) / u0) - Cr * (vy - lr * r) / u0; + const double ay = fy / m - vx * r; + const double ar = (lf * Cf * (steer - (vy + lf * r) / u0) + lr * Cr * (vy - lr * r) / u0) / Iz; + EXPECT_NEAR(f[0], std::cos(0.4) * vx - std::sin(0.4) * vy, 1e-12); + EXPECT_NEAR(f[1], std::sin(0.4) * vx + std::cos(0.4) * vy, 1e-12); + EXPECT_NEAR(f[2], r, 1e-15); + EXPECT_NEAR(f[3], ax, 1e-9); + EXPECT_NEAR(f[4], ay, 1e-9); + EXPECT_NEAR(f[5], ar, 1e-9); +} + +// Lie-IMEX is second order: against a fine RK4 of the same drift over one 0.02 s interval, the +// error falls about 4x per halving of the step. +TEST(PhSe2, StepIsSecondOrder) { + const se2_model model = bicycle_model(); + const se2_vehicle veh(model.setup, se2_draw(model), model.twist_scale); + const double s[6] = {3.0, -2.0, 0.4, 10.0, 0.3, 0.2}; + const double u[2] = {0.5, 0.05}; + const auto rk4 = [&](const double *x0, double h, int n, double *out) { + double x[6]; + std::copy(x0, x0 + 6, x); + for (int k = 0; k < n; ++k) { + double k1[6], k2[6], k3[6], k4[6], t[6]; + veh.vector_field(x, u, k1); + for (int i = 0; i < 6; ++i) + t[i] = x[i] + 0.5 * h * k1[i]; + veh.vector_field(t, u, k2); + for (int i = 0; i < 6; ++i) + t[i] = x[i] + 0.5 * h * k2[i]; + veh.vector_field(t, u, k3); + for (int i = 0; i < 6; ++i) + t[i] = x[i] + h * k3[i]; + veh.vector_field(t, u, k4); + for (int i = 0; i < 6; ++i) + x[i] += h / 6.0 * (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]); + } + std::copy(x, x + 6, out); + }; + // The bicycle's lateral modes are fast (~ -100 /s), so the comparison starts at 8 substeps, + // where every step is inside the asymptotic range. + double ref[6]; + rk4(s, 0.02 / 4000, 4000, ref); + std::vector err; + for (int sub : {8, 16, 32, 64}) { + double out[6]; + veh.interval_step(s, u, 0.02, sub, nullptr, out); + double e = 0.0; + for (int i = 0; i < 6; ++i) + e = std::max(e, std::fabs(out[i] - ref[i])); + err.push_back(e); + } + for (std::size_t i = 1; i < err.size(); ++i) { + const double ratio = err[i - 1] / err[i]; + std::printf(" substeps x2: error %.3e -> %.3e (x%.2f)\n", err[i - 1], err[i], ratio); + EXPECT_GT(ratio, 3.5); + EXPECT_LT(ratio, 4.5); + } +} + +// spd_from_levels' reverse against central differences. +TEST(PhMat3, SpdReverseMatchesFiniteDifferences) { + const double l[6] = {0.3, -0.7, 1.1, 0.4, -0.25, 0.9}; + const mat3 w = {0.3, -1.2, 0.5, 2.0, 0.1, -0.4, 0.7, 0.9, -1.5}; // loss = + double dl[6] = {}; + spd_from_levels_vjp(l, w, dl); + for (int i = 0; i < 6; ++i) { + double lp[6], lm[6]; + std::copy(l, l + 6, lp); + std::copy(l, l + 6, lm); + lp[i] += 1e-6; + lm[i] -= 1e-6; + const auto ap = spd_from_levels(lp), am = spd_from_levels(lm); + double fd = 0.0; + for (int k = 0; k < 9; ++k) + fd += w[k] * (ap[k] - am[k]) / 2e-6; + EXPECT_NEAR(dl[i], fd, 1e-7 * (1.0 + std::fabs(fd))) << i; + } + const auto a = spd_from_levels(l); + const auto back = spd_levels_of(a); + for (int i = 0; i < 6; ++i) + EXPECT_NEAR(back[i], l[i], 1e-12); +} + +TEST(PhMat3, Solve3PivotsAndSolves) { + const mat3 a = {0.0, 2.0, 1.0, 1.0, 0.0, 3.0, 4.0, 1.0, 0.0}; // a zero first pivot + const vec3 x = {1.5, -2.0, 0.25}; + const vec3 b = matvec(a, x); + const vec3 got = solve3(a, b); + for (int i = 0; i < 3; ++i) + EXPECT_NEAR(got[i], x[i], 1e-14); + const vec3 bt = tmatvec(a, x); + const vec3 gt = solve3_transposed(a, bt); + for (int i = 0; i < 3; ++i) + EXPECT_NEAR(gt[i], x[i], 1e-14); +} + +// The features are a Matern kernel's: their inner products approach k(r) = (1 + sqrt5 r + +// 5/3 r^2) exp(-sqrt5 r) as the count grows (nu 5/2, ell 1). +TEST(PhRff, FeaturesApproachTheMaternKernel) { + rng r(11); + const rff f = rff::draw(r, 3, 20000, 2.5, 1.0); + const double a[3] = {0.1, -0.2, 0.3}; + std::vector ta(f.count), tb(f.count); + for (double dist : {0.0, 0.3, 1.0, 2.0}) { + const double b[3] = {0.1 + dist, -0.2, 0.3}; + f.angles(a, ta.data()); + f.angles(b, tb.data()); + double k = 0.0; + for (int i = 0; i < f.count; ++i) + k += 2.0 / f.count * std::cos(ta[i]) * std::cos(tb[i]); + const double s5 = std::sqrt(5.0) * dist; + const double want = (1.0 + s5 + s5 * s5 / 3.0) * std::exp(-s5); + EXPECT_NEAR(k, want, 0.03) << "distance " << dist; + } +} + +TEST(PhRng, IsTheSameSequenceEverywhere) { + // The integers come from integer arithmetic alone (no libm), so every platform agrees on them + // (the pinned values are an independent Python transcription of xoshiro256** and splitmix64). + rng r(42); + const std::uint64_t first[3] = {r.next(), r.next(), r.next()}; + std::printf(" rng(42): %016llx %016llx %016llx\n", static_cast(first[0]), + static_cast(first[1]), static_cast(first[2])); + EXPECT_EQ(first[0], 0x15780b2e0c2ec716ULL); + EXPECT_EQ(first[1], 0x6104d9866d113a7eULL); + EXPECT_EQ(first[2], 0xae17533239e499a1ULL); + rng a(7), b(7), c = rng::derive(7, 1), d = rng::derive(7, 2); + for (int i = 0; i < 100; ++i) + ASSERT_EQ(a.next(), b.next()); + EXPECT_NE(c.next(), d.next()); + // Moments: uniform, normal, chi-square(5). + rng m(3); + const int n = 200000; + double su = 0, sn = 0, sn2 = 0, sc = 0; + for (int i = 0; i < n; ++i) { + su += m.uniform(); + const double z = m.normal(); + sn += z; + sn2 += z * z; + sc += m.chi_square(5.0); + } + EXPECT_NEAR(su / n, 0.5, 0.005); + EXPECT_NEAR(sn / n, 0.0, 0.01); + EXPECT_NEAR(sn2 / n, 1.0, 0.01); + EXPECT_NEAR(sc / n, 5.0, 0.05); +} + +} // namespace From 63651e83e6d933173569c174b53c8a29e1182f8f Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 22:34:04 -0500 Subject: [PATCH 09/12] ph: train the learned SE(2) vehicle natively (rollout loss), and cvc ph-train The rollout-loss trainer of LieSPHGP's SE(2) recipe, in C++ with a hand-written reverse pass: no Python, JAX or autodiff library at runtime or in CI. - se2_train.h: the objective (rollout loss + beta KL, one reparameterized draw per step), its reverse pass through every Lie-IMEX step of a window (the implicit 3x3 solves, the SPD levels, the random-feature GPs and grad V's own derivative), deterministic on any thread count; train.py's optimizer (global-norm clip, cosine-decayed Adam on the GP weights, Prodigy per level); the training loop with its held-out monitor; the data: cvc-plant-log run directories (flights.npy + meta.json), split by seed, observation noise, RMS scales, LieSPHGP's windows. - optim.h: cosine decay, clipping, Adam and Prodigy written to match optax 0.2.8. - cvc ph-train (cvc-cli): fits a model to plant-log runs and writes it as JSON; --dump-dataset writes the noisy flights it trained on. ph_se2_train_test: the gradient is within 8.1e-15 (relative to each leaf's largest entry) of JAX's autodiff of the same objective on the Humvee fit, and within 2.2e-6 of central differences on a small model; 300 optimizer steps match optax to 1.6e-16, and Prodigy alone matches it through d's growth from 1e-3 to 1.95; a 1,200-step fit to another model's flights cuts the held-out loss 22x. On the Humvee's Phase A runs a step takes about 11 ms on 8 threads (5,000 steps in about a minute). --- inc/cvc/ph/optim.h | 127 +++++ inc/cvc/ph/se2_train.h | 186 +++++++ src/cvc-cli/main.cpp | 160 ++++++ src/cvc/CMakeLists.txt | 4 + src/cvc/ph/se2_data.cpp | 195 ++++++++ src/cvc/ph/se2_train.cpp | 635 ++++++++++++++++++++++++ src/cvc/tests/CMakeLists.txt | 11 + src/cvc/tests/data/ph_optim_golden.json | 1 + src/cvc/tests/ph_se2_train_test.cpp | 380 ++++++++++++++ 9 files changed, 1699 insertions(+) create mode 100644 inc/cvc/ph/optim.h create mode 100644 inc/cvc/ph/se2_train.h create mode 100644 src/cvc/ph/se2_data.cpp create mode 100644 src/cvc/ph/se2_train.cpp create mode 100644 src/cvc/tests/data/ph_optim_golden.json create mode 100644 src/cvc/tests/ph_se2_train_test.cpp diff --git a/inc/cvc/ph/optim.h b/inc/cvc/ph/optim.h new file mode 100644 index 000000000..079feb44f --- /dev/null +++ b/inc/cvc/ph/optim.h @@ -0,0 +1,127 @@ +/* + 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::ph's optimizers, written to match optax 0.2.8 step for step (the JAX trainer the +// port-Hamiltonian models were first fit with): a cosine decay, global-norm clipping, Adam, and +// Prodigy (Mishchenko and Defazio, "Prodigy: an expeditiously adaptive parameter-free learner", +// 2023; optax.contrib.prodigy). ph_optim_test holds them to optax's own numbers. +// +// Each works on a flat span of doubles. `count` is the number of updates already applied, as +// optax's schedules read it (0 on the first update). + +#ifndef __CVC_PH_OPTIM_H__ +#define __CVC_PH_OPTIM_H__ + +#include +#include +#include +#include + +namespace cvc { +namespace ph { + +// optax.cosine_decay_schedule(init, decay_steps, alpha) at `count`. +inline double cosine_decay(double init, int decay_steps, double alpha, int count) { + const double t = static_cast(std::min(count, decay_steps)); + const double cosine = 0.5 * (1.0 + std::cos(3.14159265358979323846 * t / decay_steps)); + return init * ((1.0 - alpha) * cosine + alpha); +} + +// optax.clip_by_global_norm: scales g to norm `max_norm` when its norm is not below it. Returns +// the norm before clipping. +inline double clip_by_global_norm(double *g, std::size_t n, double max_norm) { + double sq = 0.0; + for (std::size_t i = 0; i < n; ++i) + sq += g[i] * g[i]; + const double norm = std::sqrt(sq); + if (!(norm < max_norm)) + for (std::size_t i = 0; i < n; ++i) + g[i] = g[i] / norm * max_norm; + return norm; +} + +// optax.adam (eps outside the root, eps_root 0). +struct adam { + double beta1 = 0.9, beta2 = 0.999, eps = 1e-8; + std::vector m, v; + int count = 0; + + void init(std::size_t n) { + m.assign(n, 0.0); + v.assign(n, 0.0); + count = 0; + } + // p -= lr * m_hat / (sqrt(v_hat) + eps); lr is the schedule at the count before this update. + void update(double *p, const double *g, std::size_t n, double lr) { + const int t = count + 1; + const double c1 = 1.0 - std::pow(beta1, t), c2 = 1.0 - std::pow(beta2, t); + for (std::size_t i = 0; i < n; ++i) { + m[i] = beta1 * m[i] + (1.0 - beta1) * g[i]; + v[i] = beta2 * v[i] + (1.0 - beta2) * g[i] * g[i]; + p[i] -= lr * (m[i] / c1) / (std::sqrt(v[i] / c2) + eps); + } + count = t; + } +}; + +// optax.contrib.prodigy, weight decay 0: Adam whose step size d (the distance-to-solution +// estimate) is learned. One instance per parameter group, as the JAX trainer has one per level. +// +// One deliberate difference: when a group's gradients have been exactly zero since the start, +// Prodigy's denominator is 0 and optax's d becomes NaN; here d stays where it is. +struct prodigy { + double beta1 = 0.9, beta2 = 0.999, eps = 1e-8; + double d0 = 1e-6; // estim_lr0, the initial (under-)estimate of d + double coef = 1.0; // estim_lr_coef + bool safeguard_warmup = false; + double d = 0.0; // estim_lr + double numerator_weighted = 0.0; + std::vector exp_avg, exp_avg_sq, grad_sum, p0; + int count = 0; + + void init(const double *p, std::size_t n) { + exp_avg.assign(n, 0.0); + exp_avg_sq.assign(n, 0.0); + grad_sum.assign(n, 0.0); + p0.assign(p, p + n); + d = d0; + numerator_weighted = 0.0; + count = 0; + } + // `sched` is the learning-rate schedule at the count before this update (1 is Prodigy's base). + void update(double *p, const double *g, std::size_t n, double sched) { + const int t = count + 1; + const double beta3 = std::sqrt(beta2); + const double bc = std::sqrt(1.0 - std::pow(beta2, t)) / (1.0 - std::pow(beta1, t)); + const double dlr = d * sched * bc; + double numerator_acum = 0.0; + for (std::size_t i = 0; i < n; ++i) + numerator_acum += g[i] * (p0[i] - p[i]); + double denominator = 0.0; + for (std::size_t i = 0; i < n; ++i) { + const double dg = d * g[i]; + exp_avg[i] = beta1 * exp_avg[i] + (1.0 - beta1) * dg; + exp_avg_sq[i] = beta2 * exp_avg_sq[i] + (1.0 - beta2) * dg * dg; + grad_sum[i] = beta3 * grad_sum[i] + (safeguard_warmup ? d : dlr) * dg / d0; + denominator += std::fabs(grad_sum[i]); + } + numerator_weighted = beta3 * numerator_weighted + (d / d0) * dlr * numerator_acum; + if (denominator > 0.0) + d = std::max(d, coef * numerator_weighted / denominator); + for (std::size_t i = 0; i < n; ++i) + p[i] -= dlr * exp_avg[i] / (std::sqrt(exp_avg_sq[i]) + d * eps); + count = t; + } +}; + +} // namespace ph +} // namespace cvc + +#endif // __CVC_PH_OPTIM_H__ diff --git a/inc/cvc/ph/se2_train.h b/inc/cvc/ph/se2_train.h new file mode 100644 index 000000000..5ac36d897 --- /dev/null +++ b/inc/cvc/ph/se2_train.h @@ -0,0 +1,186 @@ +/* + 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::ph::se2 training: the learned SE(2) vehicle (se2_vehicle.h) fit to logged flights in C++, +// with no Python and no autodiff library. The recipe is LieSPHGP's SE(2) trainer (train.py) with +// its rollout loss: +// +// L = rollout(windows | W) + beta KL(q || p) / (N_obs 6), W = mean + e^log_std z, z ~ N(0, I) +// +// rollout(): from each window's first sample, roll the model out under the recorded inputs and +// compare every later sample (position, wrapped heading, twist): the mean over the window of the +// squared error summed over the six channels, averaged over the windows that stay finite. One +// reparameterized weight draw per step; beta rises linearly to kl_beta_max over kl_anneal_steps; +// N_obs is the training windows times their intervals. +// +// The gradient is a hand-written reverse pass through every Lie-IMEX step of the window +// (backpropagation through time, as nav's coef_train does): the 3x3 implicit solves, the SPD +// parameterization, and the random-feature GPs, including grad V's own derivative in position. +// ph_se2_train_test holds it to central differences and to JAX's autodiff of the same objective. +// +// The optimizer is train.py's: clip the whole gradient to global norm 1, Adam (cosine-decayed) +// on the GP weights and the noise scales, Prodigy (one per subnetwork) on the levels. + +#ifndef __CVC_PH_SE2_TRAIN_H__ +#define __CVC_PH_SE2_TRAIN_H__ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cvc { +class thread_pool; +namespace ph { + +// ── data +// ────────────────────────────────────────────────────────────────────────────────────────── + +// Equal-length flights: rows [x, y, psi, v_x, v_y, r, u (control_dim)], row k's inputs acting +// from t_k-1 to t_k (cvc::physics::plant_log's layout; row 0's inputs unused). +struct se2_flights { + std::size_t count = 0, rows = 0; + int width = 8; // 6 + control_dim + std::vector data; // [count x rows x width] + std::vector seed; // each flight's seed, when known + double dt = 0.02; // s between rows + + int control_dim() const { return width - 6; } + const double *flight(std::size_t f) const { return data.data() + f * rows * width; } +}; + +// The RMS of each twist channel and of each input over every row of every flight: the model's +// twist_scale and control_scale (LieSPHGP's data statistics, rows 0 included). +std::array twist_rms(const se2_flights &f); +std::vector control_rms(const se2_flights &f); + +// The flights with their inputs divided by `scale`. +se2_flights scale_controls(const se2_flights &f, const std::vector &scale); + +// Absolute observation noise N(0, sigma) on x, y, psi, v_x, v_y and r of every row, the inputs +// left exact (the noise a plant-learning dataset is given; cvc's liesphgp_pack.py --noise). +se2_flights add_observation_noise(const se2_flights &f, double sigma, std::uint64_t seed); + +// cvc-plant-log run directories (cvc::physics::plant_log's flights.npy and meta.json, format +// cvc-plant-log-1), concatenated in order, each flight's seed from meta.json. Row 0's inputs are +// set to zero, as LieSPHGP's datasets have them (they act before the flight; no model reads them, +// but they count in control_rms). Throws std::runtime_error. +se2_flights load_plant_log_runs(const std::vector &dirs); + +// The flights whose seed is (not) in `seeds`. +se2_flights select_seeds(const se2_flights &f, const std::vector &seeds, bool keep); + +// A little-endian .npy array of float64 (or float32, widened), C order: its shape and values. +// Throws std::runtime_error. +std::vector read_npy(const std::string &path, std::vector &shape); +// Writes float64, C order. Throws std::runtime_error. +void write_npy(const std::string &path, const std::vector &data, + const std::vector &shape); + +// Training windows: `points` rows every `stride` rows of every flight, window-major. +struct se2_windows { + int points = 0, width = 8; + std::size_t count = 0; + std::vector data; // [count x points x width] + const double *window(std::size_t w) const { return data.data() + w * points * width; } +}; +se2_windows cut_windows(const se2_flights &f, int points, int stride); + +// ── the objective ──────────────────────────────────────────────────────────────────────────────── + +// One reparameterization draw: z for D, G and V ([features x outputs] each). +struct se2_draw_noise { + std::vector zD, zG, zV; + static se2_draw_noise standard_normal(const se2_model &m, rng &r); +}; + +struct se2_objective { + double value = 0.0; // rollout + beta_scale KL + double rollout = 0.0; // the rollout loss + double kl = 0.0; // KL(q || N(0, I)) of the three GPs + std::size_t healthy = 0; // windows whose rollout stayed finite +}; + +// The objective over windows `idx` (all when null) with the draw z (the posterior mean when null) +// and beta_scale = beta / (N_obs 6); its gradient into `grad` (shaped like m.params) when given. +// Pure. The windows run on `pool` when one is given; the result is the same bit for bit either +// way (each window's gradient is its own, summed in window order). +se2_objective se2_rollout_objective(const se2_model &m, const se2_windows &w, + const std::vector *idx, const se2_draw_noise *z, + double beta_scale, se2_params *grad = nullptr, + cvc::thread_pool *pool = nullptr); + +// ── the optimizer ──────────────────────────────────────────────────────────────────────────────── + +struct se2_optimizer_config { + int total_steps = 5000; // the cosine decay's length + double learning_rate = 3e-3; // Adam's + double final_learning_fraction = 0.1; + double level_learning_rate = 0.25; // Prodigy's (its base is 1) + double beta1 = 0.9, beta2 = 0.999, epsilon = 1e-8; + double gradient_clip_norm = 1.0; // <= 0: no clipping + double prodigy_d0 = 1e-3; // Prodigy's initial d +}; + +// train.py's make_optimizer: clip, then Adam on every leaf but the levels, Prodigy on each level. +class se2_optimizer { +public: + se2_optimizer(const se2_params &p0, const se2_optimizer_config &cfg); + // One update of p by grad (which is clipped in place). Returns the gradient's norm before. + double update(se2_params &p, se2_params &grad); + double prodigy_d(se2_leaf_group level) const; + int count() const { return count_; } + +private: + se2_optimizer_config cfg_; + adam adam_; + prodigy level_[4]; + int count_ = 0; +}; + +// ── training ───────────────────────────────────────────────────────────────────────────────────── + +struct se2_train_config { + std::uint64_t seed = 0; + int total_steps = 5000; + int batch_size = 16; + double kl_beta_max = 1.0; + int kl_anneal_steps = 1000; + int eval_every = 500; // the monitor: the mean model's loss on eval_windows test windows + int eval_windows = 64; + int threads = 0; // <= 0: hardware concurrency + se2_optimizer_config optimizer; +}; + +struct se2_train_event { + int step = 0; + se2_objective objective; // this step's batch, this step's draw + double beta = 0.0; + double gradient_norm = 0.0; + bool evaluated = false; // test_loss and train_loss are set + double test_loss = 0.0, train_loss = 0.0; + std::array prodigy_d{}; // D, G, V, Minv +}; + +// Fits `m` (an se2_model::initialize'd one, or a checkpoint to continue) to `train` in place. +// `test` (may be null) is only monitored, never trained on. on_step (may be empty) sees every +// step. Throws std::runtime_error on a non-finite loss or gradient, as train.py aborts. +void se2_train_rollout(se2_model &m, const se2_windows &train, const se2_windows *test, + const se2_train_config &cfg, + const std::function &on_step = {}); + +} // namespace ph +} // namespace cvc + +#endif // __CVC_PH_SE2_TRAIN_H__ diff --git a/src/cvc-cli/main.cpp b/src/cvc-cli/main.cpp index be293ea3d..7049a16fb 100644 --- a/src/cvc-cli/main.cpp +++ b/src/cvc-cli/main.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -2144,6 +2145,162 @@ static int cmd_plant_log(int argc, char **argv) { return 0; } +// ── ph-train: fit the learned port-Hamiltonian vehicle (cvc::ph) to plant logs ── + +static int cmd_ph_train(int argc, char **argv) { + po::options_description desc( + "cvc ph-train - fit cvc::ph's learned SE(2) port-Hamiltonian vehicle to cvc-plant-log runs\n" + "(LieSPHGP's SE(2) recipe with its rollout loss; no Python)"); + // clang-format off + desc.add_options() + ("help,h", "show help") + ("runs", po::value>()->multitoken()->required(), + "cvc-plant-log --out directories (flights.npy + meta.json)") + ("out,o", po::value()->required(), "the fitted model (JSON, cvc-ph-se2/1)") + ("test-seeds", po::value>()->multitoken()->default_value({101}, "101"), + "seeds held out: monitored, never trained on") + ("noise", po::value()->default_value(0.05, "0.05"), "observation noise sigma added to x, y, psi, v") + ("noise-seed", po::value()->default_value(0), "the noise's random seed") + ("seed", po::value()->default_value(0), "features, initial values, batches, draws") + ("steps", po::value()->default_value(5000), "optimizer steps") + ("batch", po::value()->default_value(16), "windows per step") + ("window-points", po::value()->default_value(101), "samples per training window") + ("window-stride", po::value()->default_value(75), "samples between window starts") + ("substeps", po::value()->default_value(1), "Lie-IMEX steps per sample interval") + ("features", po::value()->default_value(64), "random features per GP") + ("no-v-gp", "V(x, y) linear only (no GP)") + ("v-length", po::value()->default_value(50.0), "V's GP length scale (m)") + ("lr", po::value()->default_value(3e-3), "Adam's learning rate (cosine-decayed)") + ("level-lr", po::value()->default_value(0.25), "Prodigy's schedule on the levels") + ("final-lr-fraction", po::value()->default_value(0.1), "the decay's floor") + ("clip", po::value()->default_value(1.0), "global gradient-norm clip (0: none)") + ("kl-beta-max", po::value()->default_value(1.0), "the KL weight's final value") + ("kl-anneal", po::value()->default_value(1000), "steps for the KL weight to rise") + ("eval-every", po::value()->default_value(500), "steps between monitor evaluations") + ("eval-windows", po::value()->default_value(64), "held-out windows the monitor reads") + ("threads", po::value()->default_value(0), "worker threads (0: all cores)") + ("init", po::value(), "continue from this model instead of a fresh one") + ("dump-dataset", po::value(), + "also write the noisy train/test flights (npy) and their statistics to this directory") + ("log-every", po::value()->default_value(50), "steps between log lines"); + // clang-format on + po::variables_map vm; + po::store(po::command_line_parser(argc, argv).options(desc).run(), vm); + if (vm.count("help")) { + std::cout << desc << "\n"; + return 0; + } + po::notify(vm); + namespace ph = cvc::ph; + const auto runs = vm["runs"].as>(); + const auto test_seeds = vm["test-seeds"].as>(); + const double noise = vm["noise"].as(); + const std::uint64_t noise_seed = vm["noise-seed"].as(); + + const ph::se2_flights all = ph::load_plant_log_runs(runs); + const ph::se2_flights noisy = + noise > 0.0 ? ph::add_observation_noise(all, noise, noise_seed) : all; + const ph::se2_flights train_f = ph::select_seeds(noisy, test_seeds, false); + const ph::se2_flights test_f = ph::select_seeds(noisy, test_seeds, true); + if (train_f.count == 0) + throw std::runtime_error("every flight is held out"); + const std::array twist = ph::twist_rms(train_f); + const std::vector control = ph::control_rms(train_f); + if (vm.count("dump-dataset")) { + const fs::path dir(vm["dump-dataset"].as()); + fs::create_directories(dir); + const auto shape = [](const ph::se2_flights &f) { + return std::vector{f.count, f.rows, static_cast(f.width)}; + }; + ph::write_npy((dir / "train.npy").string(), train_f.data, shape(train_f)); + ph::write_npy((dir / "test_noisy.npy").string(), test_f.data, shape(test_f)); + const ph::se2_flights clean = ph::select_seeds(all, test_seeds, true); + ph::write_npy((dir / "test.npy").string(), clean.data, shape(clean)); + std::ofstream js((dir / "dataset.json").string()); + js << std::setprecision(17) << "{\"dt\": " << all.dt << ", \"noise\": " << noise + << ", \"noise_seed\": " << noise_seed << ", \"train_flights\": " << train_f.count + << ", \"test_flights\": " << test_f.count << "}\n"; + } + + const int points = vm["window-points"].as(); + const int stride = vm["window-stride"].as(); + const ph::se2_windows train = + ph::cut_windows(ph::scale_controls(train_f, control), points, stride); + const ph::se2_windows test = + test_f.count ? ph::cut_windows(ph::scale_controls(test_f, control), points, stride) + : ph::se2_windows{}; + ph::se2_model model; + if (vm.count("init")) { + model = ph::se2_model::load(vm["init"].as()); + } else { + ph::se2_init init; + init.control_dim = all.control_dim(); + init.features = vm["features"].as(); + init.v_gp = !vm.count("no-v-gp"); + init.v_length = vm["v-length"].as(); + model = ph::se2_model::initialize(init, vm["seed"].as(), twist, control, all.dt, + vm["substeps"].as()); + } + ph::se2_train_config cfg; + cfg.seed = vm["seed"].as(); + cfg.total_steps = vm["steps"].as(); + cfg.batch_size = vm["batch"].as(); + cfg.kl_beta_max = vm["kl-beta-max"].as(); + cfg.kl_anneal_steps = vm["kl-anneal"].as(); + cfg.eval_every = vm["eval-every"].as(); + cfg.eval_windows = vm["eval-windows"].as(); + cfg.threads = vm["threads"].as(); + cfg.optimizer.learning_rate = vm["lr"].as(); + cfg.optimizer.level_learning_rate = vm["level-lr"].as(); + cfg.optimizer.final_learning_fraction = vm["final-lr-fraction"].as(); + cfg.optimizer.gradient_clip_norm = vm["clip"].as(); + const int log_every = std::max(1, vm["log-every"].as()); + + std::cout << std::setprecision(6) << "ph-train: " << all.count << " flights x " << all.rows + << " rows at dt " << all.dt << " s; " << train_f.count << " train / " << test_f.count + << " held out; windows " << train.count << " / " << test.count << " of " << points + << " points\n twist scale " << twist[0] << " " << twist[1] << " " << twist[2] + << "; control scale"; + for (double c : control) + std::cout << " " << c; + std::cout << "; parameters " << model.params.size() << "\n"; + const auto started = std::chrono::steady_clock::now(); + ph::se2_train_rollout( + model, train, test.count ? &test : nullptr, cfg, [&](const ph::se2_train_event &ev) { + if (ev.step % log_every && ev.step != 1 && !ev.evaluated) + return; + const double s = + std::chrono::duration(std::chrono::steady_clock::now() - started).count(); + std::cout << "step " << std::setw(6) << ev.step << " loss " << ev.objective.value + << " rollout " << ev.objective.rollout << " kl " << ev.objective.kl << " beta " + << ev.beta << " |g| " << ev.gradient_norm << " (" << s << " s)"; + if (ev.evaluated) + std::cout << " MONITOR test " << ev.test_loss << " train subset " << ev.train_loss + << " prodigy d " << ev.prodigy_d[0] << " " << ev.prodigy_d[1] << " " + << ev.prodigy_d[2] << " " << ev.prodigy_d[3]; + std::cout << "\n" << std::flush; + }); + std::string run_list; + for (const auto &r : runs) + run_list += (run_list.empty() ? "" : ";") + r; + std::string seeds; + for (int s : test_seeds) + seeds += (seeds.empty() ? "" : ",") + std::to_string(s); + model.provenance = {{"source", "cvc ph-train (libcvc cvc::ph, rollout loss)"}, + {"runs", run_list}, + {"test_seeds", seeds}, + {"noise", std::to_string(noise)}, + {"noise_seed", std::to_string(noise_seed)}, + {"seed", std::to_string(cfg.seed)}, + {"steps", std::to_string(cfg.total_steps)}, + {"batch", std::to_string(cfg.batch_size)}, + {"window_points", std::to_string(points)}, + {"window_stride", std::to_string(stride)}}; + model.save(vm["out"].as()); + std::cout << "wrote " << vm["out"].as() << "\n"; + return 0; +} + // =========================================================================== // Command dispatch table // =========================================================================== @@ -2214,6 +2371,9 @@ static const command_entry commands[] = { {"img2vol", "Image I/O", "import image stack as volume", cmd_img2vol}, {"rgba-merge", "Image I/O", "merge 4 volumes into RGBA", cmd_rgba_merge}, + // ── Learned dynamics ── + {"ph-train", "Learned Dynamics","fit the port-Hamiltonian vehicle to plant logs", cmd_ph_train}, + // ── Test data ── {"bunny", "Test Data", "output Stanford bunny geometry or SDF", cmd_bunny}, diff --git a/src/cvc/CMakeLists.txt b/src/cvc/CMakeLists.txt index ba6feeed4..1bedf81d7 100644 --- a/src/cvc/CMakeLists.txt +++ b/src/cvc/CMakeLists.txt @@ -130,8 +130,10 @@ set(INCLUDE_FILES ../../inc/cvc/nav/terrain.h ../../inc/cvc/nav/vehicle_plant.h ../../inc/cvc/ph/mat3.h + ../../inc/cvc/ph/optim.h ../../inc/cvc/ph/rff.h ../../inc/cvc/ph/rng.h + ../../inc/cvc/ph/se2_train.h ../../inc/cvc/ph/se2_vehicle.h ../../inc/cvc/lod/select.h ../../inc/cvc/lod/pyramid.h @@ -329,7 +331,9 @@ set(SOURCE_FILES nav/sim_world_binding.cpp nav/sim_thread.cpp nav/nav_stats.cpp + ph/se2_data.cpp ph/se2_model.cpp + ph/se2_train.cpp lod/select.cpp lod/pyramid.cpp lod/tiles.cpp diff --git a/src/cvc/ph/se2_data.cpp b/src/cvc/ph/se2_data.cpp new file mode 100644 index 000000000..2dc9d57c0 --- /dev/null +++ b/src/cvc/ph/se2_data.cpp @@ -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::ph::se2 data: reading cvc-plant-log run directories and .npy arrays (se2_train.h). +// +// cvc::ph sits below cvc::physics and cvc::nav, so it reads the plant log's files rather than +// linking plant_log: flights.npy is (F, T, 6 + inputs) float64, meta.json carries "flight_seed" +// and "dt". + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cvc { +namespace ph { + +namespace { + +namespace pt = boost::property_tree; + +std::string header_value(const std::string &header, const std::string &key) { + const auto at = header.find("'" + key + "'"); + if (at == std::string::npos) + throw std::runtime_error("npy: header has no " + key); + const auto colon = header.find(':', at); + return header.substr(colon + 1); +} + +} // namespace + +std::vector read_npy(const std::string &path, std::vector &shape) { + std::ifstream in(path, std::ios::binary); + if (!in) + throw std::runtime_error("npy: cannot read " + path); + unsigned char magic[8]; + in.read(reinterpret_cast(magic), 8); + if (!in || std::memcmp(magic, "\x93NUMPY", 6) != 0) + throw std::runtime_error("npy: " + path + " is not a .npy file"); + std::size_t header_len = 0; + if (magic[6] == 1) { + unsigned char b[2]; + in.read(reinterpret_cast(b), 2); + header_len = b[0] | (b[1] << 8); + } else { + unsigned char b[4]; + in.read(reinterpret_cast(b), 4); + header_len = b[0] | (b[1] << 8) | (b[2] << 16) | (static_cast(b[3]) << 24); + } + std::string header(header_len, '\0'); + in.read(&header[0], static_cast(header_len)); + if (!in) + throw std::runtime_error("npy: " + path + " is truncated"); + const std::string descr = header_value(header, "descr"); + const bool f8 = descr.find("(std::stoull(tok))); + total *= shape.back(); + } + std::vector out(total); + if (f8) { + in.read(reinterpret_cast(out.data()), static_cast(total * 8)); + } else { + std::vector tmp(total); + in.read(reinterpret_cast(tmp.data()), static_cast(total * 4)); + for (std::size_t i = 0; i < total; ++i) + out[i] = tmp[i]; + } + if (!in) + throw std::runtime_error("npy: " + path + " is shorter than its shape"); + return out; +} + +void write_npy(const std::string &path, const std::vector &data, + const std::vector &shape) { + std::size_t total = 1; + std::string dims; + for (std::size_t d : shape) { + total *= d; + dims += std::to_string(d) + ","; + } + if (shape.size() > 1) + dims.pop_back(); + if (total != data.size()) + throw std::runtime_error("npy: data does not match the shape"); + std::string header = "{'descr': '(magic), 8); + const unsigned char hl[2] = {static_cast(header.size() & 0xff), + static_cast(header.size() >> 8)}; + out.write(reinterpret_cast(hl), 2); + out.write(header.data(), static_cast(header.size())); + out.write(reinterpret_cast(data.data()), + static_cast(data.size() * sizeof(double))); + if (!out) + throw std::runtime_error("npy: writing " + path + " failed"); +} + +se2_flights load_plant_log_runs(const std::vector &dirs) { + se2_flights all; + bool first = true; + for (const std::string &dir : dirs) { + const std::filesystem::path d(dir); + pt::ptree meta; + try { + pt::read_json((d / "meta.json").string(), meta); + } catch (const pt::json_parser_error &e) { + throw std::runtime_error(dir + ": " + e.what()); + } + if (meta.get("format", "") != "cvc-plant-log-1") + throw std::runtime_error(dir + ": not a cvc-plant-log run"); + std::vector shape; + std::vector data = read_npy((d / "flights.npy").string(), shape); + if (shape.size() != 3 || shape[2] < 7) + throw std::runtime_error(dir + ": flights.npy is not (flights, rows, 6 + inputs)"); + std::vector seeds; + for (const auto &kv : meta.get_child("flight_seed")) + seeds.push_back(kv.second.get_value()); + if (seeds.size() != shape[0]) + throw std::runtime_error(dir + ": meta.json's flight_seed does not match flights.npy"); + const double dt = meta.get("dt"); + if (first) { + all.rows = shape[1]; + all.width = static_cast(shape[2]); + all.dt = dt; + first = false; + } else if (shape[1] != all.rows || static_cast(shape[2]) != all.width || + std::fabs(dt - all.dt) > 1e-12) { + throw std::runtime_error(dir + ": rows, width or dt differ from the first run's"); + } + for (std::size_t f = 0; f < shape[0]; ++f) + for (int j = 6; j < all.width; ++j) + data[f * shape[1] * shape[2] + j] = 0.0; // row 0's inputs + all.data.insert(all.data.end(), data.begin(), data.end()); + all.seed.insert(all.seed.end(), seeds.begin(), seeds.end()); + all.count += shape[0]; + } + if (first) + throw std::runtime_error("load_plant_log_runs: no run directories"); + return all; +} + +se2_flights select_seeds(const se2_flights &f, const std::vector &seeds, bool keep) { + if (f.seed.size() != f.count) + throw std::invalid_argument("select_seeds: the flights carry no seeds"); + se2_flights out; + out.rows = f.rows; + out.width = f.width; + out.dt = f.dt; + const std::size_t n = f.rows * f.width; + for (std::size_t i = 0; i < f.count; ++i) { + const bool in = std::find(seeds.begin(), seeds.end(), f.seed[i]) != seeds.end(); + if (in != keep) + continue; + out.data.insert(out.data.end(), f.flight(i), f.flight(i) + n); + out.seed.push_back(f.seed[i]); + ++out.count; + } + return out; +} + +} // namespace ph +} // namespace cvc diff --git a/src/cvc/ph/se2_train.cpp b/src/cvc/ph/se2_train.cpp new file mode 100644 index 000000000..7459ee6d6 --- /dev/null +++ b/src/cvc/ph/se2_train.cpp @@ -0,0 +1,635 @@ +/* + 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::ph::se2 training (se2_train.h): the data, the rollout objective and its hand-written +// reverse pass, train.py's optimizer, and the training loop. +// +// The reverse pass, for one Lie-IMEX step (se2_vehicle.h's notation, a and b the two implicit +// systems' matrices and right-hand sides, hh = h / 2): +// +// v' = a_2^-1 b_2: bb_2 = a_2^-T dv', dK_2 = -hh bb_2 v'^T, +// dv += bb_2 - hh K_1^T bb_2, de_1 += hh bb_2, de_2 += hh bb_2, dK_1 -= hh +// bb_2 v^T +// psi', (x, y)': dpsi += dpsi', dr += hh dpsi', dr_p += hh dpsi'; d(x, y) += d(x, y)', +// ddxy_1 += hh d(x, y)', ddxy_2 += hh d(x, y)' +// stage 2 (at the predictor) gives d(x, y)_p, dpsi_p, dv_p; then (x, y)_p and psi_p feed back: +// d(x, y) += d(x, y)_p, ddxy_1 += h d(x, y)_p, dpsi += dpsi_p, dr += h +// dpsi_p +// v_p = a_1^-1 b_1: bb_1 = a_1^-T dv_p, dK_1 -= h bb_1 v_p^T, dv += bb_1, de_1 += h bb_1 +// stage 1 (at the step's start). +// +// and for a stage (K = M^-1 D, e = M^-1 q, q = ad*_v p - body + G u, p = M v): +// +// dM^-1 += dK D^T + de q^T - (M^-T dp) p^T; dD = M^-T dK; dq = M^-T de +// dG = dq u^T; dlevels_D = spd's reverse of dD; both GPs' reverse give dW and dz, dv += dz / +// scale ad*_v p's reverse gives dp and dv; dv += M^-T dp body = R^T grad V: d grad V = R +// dbody, dpsi += dbody . (body_y, -body_x) grad V = level + sum_k f_k omega_k / ell_V: dW_V,k +// and d(x, y) from d grad V (its Hessian) dxy = R v: dv += R^T ddxy, dpsi += ddxy . (-dxy_y, +// dxy_x) +// +// M^-1's gradient is collected as a 3x3 over the whole window and turned into its six levels +// once. The reparameterization W = mean + e^log_std z gives dmean = dW, dlog_std = dW e^log_std z. + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cvc { +namespace ph { + +namespace { + +using V3 = vec3; +using M3 = mat3; + +// ── data helpers ─────────────────────────────────────────────────────────────────────────────── + +void check_flights(const se2_flights &f) { + if (f.width < 7 || f.width > 6 + kSe2MaxControls) + throw std::invalid_argument("se2_flights: rows must be 6 + 1-4 inputs wide"); + if (f.data.size() != f.count * f.rows * static_cast(f.width)) + throw std::invalid_argument("se2_flights: data is not count x rows x width"); +} + +// ── the reverse pass ─────────────────────────────────────────────────────────────────────────── + +// The gradient with respect to one weight draw's values. +struct draw_grad { + std::vector wD, wG, wV; + std::array d_level{}; + std::vector g_level; + std::array v_level{}; + M3 minv{}; + + void reset(const se2_model &m) { + wD.assign(m.params.D.mean.size(), 0.0); + wG.assign(m.params.G.mean.size(), 0.0); + wV.assign(m.params.V.mean.size(), 0.0); + d_level.fill(0.0); + g_level.assign(m.params.G.level.size(), 0.0); + v_level.fill(0.0); + minv.fill(0.0); + } + void add(const draw_grad &o, double s) { + for (std::size_t i = 0; i < wD.size(); ++i) + wD[i] += s * o.wD[i]; + for (std::size_t i = 0; i < wG.size(); ++i) + wG[i] += s * o.wG[i]; + for (std::size_t i = 0; i < wV.size(); ++i) + wV[i] += s * o.wV[i]; + for (int i = 0; i < 6; ++i) + d_level[i] += s * o.d_level[i]; + for (std::size_t i = 0; i < g_level.size(); ++i) + g_level[i] += s * o.g_level[i]; + for (int i = 0; i < 2; ++i) + v_level[i] += s * o.v_level[i]; + for (int i = 0; i < 9; ++i) + minv[i] += s * o.minv[i]; + } +}; + +// One stage's reverse: (ddxy, de, dK) on its outputs -> (dpos, dpsi, dv) on its inputs (added), +// and the draw's gradient (added). +void stage_vjp(const se2_vehicle &veh, const se2_stage &st, + const std::array &ddxy, const V3 &de, const M3 &dK, double *dpos, + double &dpsi, V3 &dv, draw_grad &g) { + const se2_setup &s = veh.setup(); + const se2_weights &w = veh.weights(); + const M3 &minv = veh.inverse_mass(); + const int nu = veh.control_dim(); + + // K = M^-1 D + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + g.minv[3 * i + j] += dK[3 * i] * st.D[3 * j] + dK[3 * i + 1] * st.D[3 * j + 1] + + dK[3 * i + 2] * st.D[3 * j + 2]; + const M3 dD = mul(transpose(minv), dK); + // e = M^-1 q + add_outer(g.minv, de, st.q); + const V3 dq = tmatvec(minv, de); + // q = ad*_v p + G u - body + const V3 &dcoad = dq; + const double dbody[2] = {-dq[0], -dq[1]}; + std::array dG{}; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < nu; ++j) { + dG[i * nu + j] = dq[i] * st.u[j]; + g.g_level[i * nu + j] += dG[i * nu + j]; + } + V3 dz{}; + s.G.gp_vjp(st.theta_G.data(), w.wG.data(), 3 * nu, dG.data(), g.wG.data(), dz.data()); + // D = spd(level + GP_D(z)) + double dl[6] = {}; + spd_from_levels_vjp(st.d_levels.data(), dD, dl); + for (int i = 0; i < 6; ++i) + g.d_level[i] += dl[i]; + s.D.gp_vjp(st.theta_D.data(), w.wD.data(), 6, dl, g.wD.data(), dz.data()); + for (int i = 0; i < 3; ++i) + dv[i] += dz[i] / veh.twist_scale()[i]; + // ad*_v p = (p_y r, -p_x r, p_x v_y - p_y v_x) + const V3 &p = st.p, &v = st.v; + const V3 dp = {-v[2] * dcoad[1] + v[1] * dcoad[2], v[2] * dcoad[0] - v[0] * dcoad[2], 0.0}; + dv[0] -= p[1] * dcoad[2]; + dv[1] += p[0] * dcoad[2]; + dv[2] += p[1] * dcoad[0] - p[0] * dcoad[1]; + // p = M v = (M^-1)^-1 v + const V3 y = solve3_transposed(minv, dp); + for (int i = 0; i < 3; ++i) + dv[i] += y[i]; + add_outer(g.minv, y, p, -1.0); + // body = R^T grad V + const double dg0 = st.c * dbody[0] - st.s * dbody[1]; + const double dg1 = st.s * dbody[0] + st.c * dbody[1]; + dpsi += dbody[0] * st.body[1] - dbody[1] * st.body[0]; + // grad V = level + sum_k f_k omega_k / ell_V + g.v_level[0] += dg0; + g.v_level[1] += dg1; + if (s.v_gp) { + const double inv = 1.0 / s.v_length, a = s.V.amplitude(); + for (int k = 0; k < s.V.count; ++k) { + const double *om = &s.V.frequency[2 * static_cast(k)]; + const double sk = (om[0] * dg0 + om[1] * dg1) * inv; + g.wV[k] += -a * std::sin(st.theta_V[k]) * sk; + const double dtheta = -a * std::cos(st.theta_V[k]) * w.wV[k] * sk; + dpos[0] += dtheta * om[0] * inv; + dpos[1] += dtheta * om[1] * inv; + } + } + // dxy = R (v_x, v_y) + dv[0] += st.c * ddxy[0] + st.s * ddxy[1]; + dv[1] += -st.s * ddxy[0] + st.c * ddxy[1]; + dpsi += -ddxy[0] * st.dxy[1] + ddxy[1] * st.dxy[0]; +} + +struct workspace { + se2_stage st1, st2; + std::vector path; + draw_grad grad; +}; + +// One step's reverse: dnext (6) -> dstate (6, overwritten); the draw's gradient added. The +// forward is recomputed from `state`. +void step_vjp(const se2_vehicle &veh, const double *state, const double *u, double h, + const double *dnext, double *dstate, workspace &ws) { + double next[6]; + V3 vp; + veh.step(state, u, h, nullptr, next, ws.st1, ws.st2, vp); + const se2_stage &st1 = ws.st1, &st2 = ws.st2; + const double hh = 0.5 * h; + const V3 &v = st1.v; + const V3 vn = {next[3], next[4], next[5]}; + M3 a1 = identity3(), a2 = identity3(); + for (int k = 0; k < 9; ++k) { + a1[k] += h * st1.K[k]; + a2[k] += hh * st2.K[k]; + } + // v' = a_2^-1 b_2 + const V3 bb2 = solve3_transposed(a2, V3{dnext[3], dnext[4], dnext[5]}); + M3 dK2{}, dK1{}; + add_outer(dK2, bb2, vn, -hh); + V3 dv{}, de1{}, de2{}, dvp{}; + const V3 k1t = tmatvec(st1.K, bb2); + for (int i = 0; i < 3; ++i) { + dv[i] += bb2[i] - hh * k1t[i]; + de1[i] += hh * bb2[i]; + de2[i] += hh * bb2[i]; + } + add_outer(dK1, bb2, v, -hh); + // psi' and (x, y)' + double dpsi = dnext[2]; + dv[2] += hh * dnext[2]; + dvp[2] += hh * dnext[2]; + double dpos[2] = {dnext[0], dnext[1]}; + std::array ddxy1 = {hh * dnext[0], hh * dnext[1]}; + const std::array ddxy2 = ddxy1; + // stage 2, at the predictor + double dpos_p[2] = {0.0, 0.0}; + double dpsi_p = 0.0; + stage_vjp(veh, st2, ddxy2, de2, dK2, dpos_p, dpsi_p, dvp, ws.grad); + dpos[0] += dpos_p[0]; + dpos[1] += dpos_p[1]; + ddxy1[0] += h * dpos_p[0]; + ddxy1[1] += h * dpos_p[1]; + dpsi += dpsi_p; + dv[2] += h * dpsi_p; + // v_p = a_1^-1 b_1 + const V3 bb1 = solve3_transposed(a1, dvp); + add_outer(dK1, bb1, vp, -h); + for (int i = 0; i < 3; ++i) { + dv[i] += bb1[i]; + de1[i] += h * bb1[i]; + } + // stage 1 + stage_vjp(veh, st1, ddxy1, de1, dK1, dpos, dpsi, dv, ws.grad); + dstate[0] = dpos[0]; + dstate[1] = dpos[1]; + dstate[2] = dpsi; + dstate[3] = dv[0]; + dstate[4] = dv[1]; + dstate[5] = dv[2]; +} + +// The window's loss (the mean over its later samples of the squared error) and, when `grad`, +// its gradient into ws.grad (reset by the caller). Not finite when the rollout leaves the finite +// numbers; the gradient is then not computed. +double window_loss(const se2_vehicle &veh, const double *win, int points, int width, + double interval, int substeps, bool grad, workspace &ws) { + const int intervals = points - 1; + const int steps = intervals * substeps; + const double h = interval / substeps; + ws.path.resize(6 * static_cast(steps + 1)); + double *path = ws.path.data(); + for (int i = 0; i < 6; ++i) + path[i] = win[i]; + V3 vp; + for (int t = 0; t < intervals; ++t) { + const double *u = win + static_cast(t + 1) * width + 6; + for (int k = 0; k < substeps; ++k) { + const int j = t * substeps + k; + veh.step(path + 6 * j, u, h, nullptr, path + 6 * (j + 1), ws.st1, ws.st2, vp); + } + } + // error at the end of every interval + double loss = 0.0; + for (int t = 1; t <= intervals; ++t) { + const double *x = path + 6 * static_cast(t * substeps); + const double *y = win + static_cast(t) * width; + for (int i = 0; i < 6; ++i) { + const double e = i == 2 ? wrap_angle(x[i] - y[i]) : x[i] - y[i]; + loss += e * e; + } + } + loss /= intervals; + if (!grad || !std::isfinite(loss)) + return loss; + // backward through time + const double scale = 2.0 / intervals; + double g[6] = {}, prev[6]; + for (int t = intervals; t >= 1; --t) { + const double *x = path + 6 * static_cast(t * substeps); + const double *y = win + static_cast(t) * width; + for (int i = 0; i < 6; ++i) + g[i] += scale * (i == 2 ? wrap_angle(x[i] - y[i]) : x[i] - y[i]); + const double *u = win + static_cast(t) * width + 6; + for (int k = substeps - 1; k >= 0; --k) { + const int j = (t - 1) * substeps + k; + step_vjp(veh, path + 6 * j, u, h, g, prev, ws); + std::copy(prev, prev + 6, g); + } + } + return loss; +} + +double kl_of(const se2_gp &g) { + double kl = 0.0; + for (std::size_t i = 0; i < g.mean.size(); ++i) + kl += 0.5 * (std::exp(2.0 * g.log_std[i]) + g.mean[i] * g.mean[i] - 1.0 - 2.0 * g.log_std[i]); + return kl; +} + +// dmean += dW + beta_scale mean; dlog_std += dW e^log_std z + beta_scale (e^(2 log_std) - 1) +void gp_grad(const se2_gp &p, const std::vector &dW, const std::vector *z, + double beta_scale, se2_gp &out) { + for (std::size_t i = 0; i < p.mean.size(); ++i) { + out.mean[i] += dW[i] + beta_scale * p.mean[i]; + out.log_std[i] += (z ? dW[i] * std::exp(p.log_std[i]) * (*z)[i] : 0.0) + + beta_scale * (std::exp(2.0 * p.log_std[i]) - 1.0); + } +} + +} // namespace + +// ── data ───────────────────────────────────────────────────────────────────────────────────────── + +std::array twist_rms(const se2_flights &f) { + check_flights(f); + std::array sum{}; + const std::size_t n = f.count * f.rows; + for (std::size_t r = 0; r < n; ++r) + for (int i = 0; i < 3; ++i) { + const double x = f.data[r * f.width + 3 + i]; + sum[i] += x * x; + } + for (auto &x : sum) + x = std::sqrt(x / std::max(n, 1)); + return sum; +} + +std::vector control_rms(const se2_flights &f) { + check_flights(f); + std::vector sum(f.control_dim(), 0.0); + const std::size_t n = f.count * f.rows; + for (std::size_t r = 0; r < n; ++r) + for (int i = 0; i < f.control_dim(); ++i) { + const double x = f.data[r * f.width + 6 + i]; + sum[i] += x * x; + } + for (auto &x : sum) + x = std::sqrt(x / std::max(n, 1)); + return sum; +} + +se2_flights scale_controls(const se2_flights &f, const std::vector &scale) { + check_flights(f); + if (static_cast(scale.size()) != f.control_dim()) + throw std::invalid_argument("scale_controls: one scale per input"); + se2_flights out = f; + const std::size_t n = f.count * f.rows; + for (std::size_t r = 0; r < n; ++r) + for (int i = 0; i < f.control_dim(); ++i) + out.data[r * f.width + 6 + i] /= scale[i]; + return out; +} + +se2_flights add_observation_noise(const se2_flights &f, double sigma, std::uint64_t seed) { + check_flights(f); + se2_flights out = f; + rng r(seed); + const std::size_t n = f.count * f.rows; + for (int i = 0; i < 6; ++i) // channel by channel, as liesphgp_pack.py draws it + for (std::size_t k = 0; k < n; ++k) + out.data[k * f.width + i] += sigma * r.normal(); + return out; +} + +se2_windows cut_windows(const se2_flights &f, int points, int stride) { + check_flights(f); + if (points < 2 || stride < 1) + throw std::invalid_argument("cut_windows: at least 2 points and a positive stride"); + se2_windows w; + w.points = points; + w.width = f.width; + std::vector starts; + for (std::size_t s = 0; s + points <= f.rows; s += stride) + starts.push_back(s); + w.count = f.count * starts.size(); + w.data.resize(w.count * points * f.width); + std::size_t k = 0; + for (std::size_t fl = 0; fl < f.count; ++fl) // flight-major, as LieSPHGP's data.py stacks them + for (std::size_t s : starts) { + const double *src = f.flight(fl) + s * f.width; + std::copy(src, src + static_cast(points) * f.width, + w.data.begin() + k * points * f.width); + ++k; + } + return w; +} + +// ── the objective ──────────────────────────────────────────────────────────────────────────────── + +se2_draw_noise se2_draw_noise::standard_normal(const se2_model &m, rng &r) { + se2_draw_noise z; + for (auto [out, n] : + {std::pair *, std::size_t>{&z.zD, m.params.D.mean.size()}, + {&z.zG, m.params.G.mean.size()}, + {&z.zV, m.params.V.mean.size()}}) { + out->resize(n); + for (auto &x : *out) + x = r.normal(); + } + return z; +} + +se2_objective se2_rollout_objective(const se2_model &m, const se2_windows &w, + const std::vector *idx, const se2_draw_noise *z, + double beta_scale, se2_params *grad, cvc::thread_pool *pool) { + if (w.width != 6 + m.setup.control_dim) + throw std::invalid_argument("se2_rollout_objective: the windows' inputs are not the model's"); + const se2_vehicle veh(m.setup, + z ? se2_draw(m, z->zD.data(), z->zG.data(), z->zV.data()) + : se2_draw(m), + m.twist_scale); + const std::size_t n = idx ? idx->size() : w.count; + std::vector losses(n); + std::vector ws(grad ? n : 0); + const auto run = [&](int i) { + const std::size_t win = idx ? (*idx)[i] : static_cast(i); + if (grad) { + ws[i].grad.reset(m); + losses[i] = + window_loss(veh, w.window(win), w.points, w.width, m.interval, m.substeps, true, ws[i]); + } else { + thread_local workspace local; + losses[i] = + window_loss(veh, w.window(win), w.points, w.width, m.interval, m.substeps, false, local); + } + }; + if (pool) + pool->parallel_for(static_cast(n), run); + else + for (std::size_t i = 0; i < n; ++i) + run(static_cast(i)); + + se2_objective out; + double sum = 0.0; + for (std::size_t i = 0; i < n; ++i) + if (std::isfinite(losses[i])) { + sum += losses[i]; + ++out.healthy; + } + out.rollout = sum / std::max(out.healthy, 1); + out.kl = kl_of(m.params.D) + kl_of(m.params.G) + kl_of(m.params.V); + out.value = out.rollout + beta_scale * out.kl; + if (!grad) + return out; + + draw_grad total; + total.reset(m); + const double inv = 1.0 / std::max(out.healthy, 1); + for (std::size_t i = 0; i < n; ++i) + if (std::isfinite(losses[i])) + total.add(ws[i].grad, inv); + *grad = m.params.zeros_like(); + gp_grad(m.params.D, total.wD, z ? &z->zD : nullptr, beta_scale, grad->D); + gp_grad(m.params.G, total.wG, z ? &z->zG : nullptr, beta_scale, grad->G); + gp_grad(m.params.V, total.wV, z ? &z->zV : nullptr, beta_scale, grad->V); + for (int i = 0; i < 6; ++i) + grad->D.level[i] = total.d_level[i]; + for (std::size_t i = 0; i < total.g_level.size(); ++i) + grad->G.level[i] = total.g_level[i]; + for (int i = 0; i < 2; ++i) + grad->V.level[i] = total.v_level[i]; + spd_from_levels_vjp(m.params.minv_level.data(), total.minv, grad->minv_level.data()); + return out; +} + +// ── the optimizer ──────────────────────────────────────────────────────────────────────────────── + +namespace { +int level_index(se2_leaf_group g) { + switch (g) { + case se2_leaf_group::D_level: + return 0; + case se2_leaf_group::G_level: + return 1; + case se2_leaf_group::V_level: + return 2; + case se2_leaf_group::Minv_level: + return 3; + default: + return -1; + } +} +} // namespace + +se2_optimizer::se2_optimizer(const se2_params &p0, const se2_optimizer_config &cfg) : cfg_(cfg) { + adam_.beta1 = cfg.beta1; + adam_.beta2 = cfg.beta2; + adam_.eps = cfg.epsilon; + adam_.init(p0.size()); + p0.for_each_leaf([&](const char *, const double *data, std::size_t size, se2_leaf_group g) { + const int k = level_index(g); + if (k < 0) + return; + prodigy &pr = level_[k]; + pr.beta1 = cfg.beta1; + pr.beta2 = cfg.beta2; + pr.eps = cfg.epsilon; + pr.d0 = cfg.prodigy_d0; + pr.safeguard_warmup = true; + pr.init(data, size); + }); +} + +double se2_optimizer::update(se2_params &p, se2_params &grad) { + // the global norm over every leaf, then clip + double sq = 0.0; + grad.for_each_leaf([&](const char *, double *g, std::size_t n, se2_leaf_group) { + for (std::size_t i = 0; i < n; ++i) + sq += g[i] * g[i]; + }); + const double norm = std::sqrt(sq); + if (cfg_.gradient_clip_norm > 0.0 && !(norm < cfg_.gradient_clip_norm)) + grad.for_each_leaf([&](const char *, double *g, std::size_t n, se2_leaf_group) { + for (std::size_t i = 0; i < n; ++i) + g[i] = g[i] / norm * cfg_.gradient_clip_norm; + }); + const double lr = cosine_decay(cfg_.learning_rate, std::max(cfg_.total_steps, 1), + cfg_.final_learning_fraction, count_); + const double sched = cosine_decay(cfg_.level_learning_rate, std::max(cfg_.total_steps, 1), + cfg_.final_learning_fraction, count_); + // Adam's moments are kept over the flat leaf order; level leaves leave theirs at zero. + std::vector pdata; + std::vector psize; + p.for_each_leaf([&](const char *, double *d, std::size_t n, se2_leaf_group) { + pdata.push_back(d); + psize.push_back(n); + }); + std::size_t offset = 0, leaf = 0; + const int t = adam_.count + 1; + const double c1 = 1.0 - std::pow(adam_.beta1, t), c2 = 1.0 - std::pow(adam_.beta2, t); + grad.for_each_leaf([&](const char *, double *g, std::size_t n, se2_leaf_group group) { + double *x = pdata[leaf]; + const int k = level_index(group); + if (k >= 0) { + level_[k].update(x, g, n, sched); + } else { + for (std::size_t i = 0; i < n; ++i) { + double &m = adam_.m[offset + i], &v = adam_.v[offset + i]; + m = adam_.beta1 * m + (1.0 - adam_.beta1) * g[i]; + v = adam_.beta2 * v + (1.0 - adam_.beta2) * g[i] * g[i]; + x[i] -= lr * (m / c1) / (std::sqrt(v / c2) + adam_.eps); + } + } + offset += n; + ++leaf; + }); + adam_.count = t; + ++count_; + return norm; +} + +double se2_optimizer::prodigy_d(se2_leaf_group level) const { + const int k = level_index(level); + if (k < 0) + throw std::invalid_argument("se2_optimizer::prodigy_d: not a level group"); + return level_[k].d; +} + +// ── training ───────────────────────────────────────────────────────────────────────────────────── + +void se2_train_rollout(se2_model &m, const se2_windows &train, const se2_windows *test, + const se2_train_config &cfg, + const std::function &on_step) { + if (train.count == 0) + throw std::invalid_argument("se2_train_rollout: no training windows"); + if (train.width != 6 + m.setup.control_dim || (test && test->width != train.width)) + throw std::invalid_argument("se2_train_rollout: the windows' inputs are not the model's"); + // threads 1: inline, no pool (a pool of 0 workers means "hardware concurrency - 1") + std::unique_ptr owned; + if (cfg.threads != 1) + owned = std::make_unique( + cfg.threads > 1 ? static_cast(cfg.threads - 1) : 0u); + cvc::thread_pool *pool = owned.get(); + se2_optimizer_config ocfg = cfg.optimizer; + ocfg.total_steps = cfg.total_steps; + se2_optimizer opt(m.params, ocfg); + rng batches = rng::derive(cfg.seed, 101), draws = rng::derive(cfg.seed, 102); + const double observations = static_cast(train.count) * (train.points - 1); + const std::size_t batch = std::min(std::max(cfg.batch_size, 1), train.count); + + // the monitor's windows: eval_windows evenly spread test windows, the first training ones + std::vector test_idx, train_idx; + if (test && test->count) { + const std::size_t k = + cfg.eval_windows > 0 ? std::min(cfg.eval_windows, test->count) : test->count; + for (std::size_t i = 0; i < k; ++i) + test_idx.push_back( + k > 1 ? static_cast(static_cast(i) * (test->count - 1) / (k - 1)) + : 0); + for (std::size_t i = 0; i < std::min(k, train.count); ++i) + train_idx.push_back(i); + } + + std::vector pick(train.count); + se2_params grad; + for (int step = 1; step <= cfg.total_steps; ++step) { + // batch_size distinct windows (a partial Fisher-Yates shuffle), sorted + for (std::size_t i = 0; i < train.count; ++i) + pick[i] = i; + for (std::size_t i = 0; i < batch; ++i) + std::swap(pick[i], pick[i + batches.below(train.count - i)]); + std::vector idx(pick.begin(), pick.begin() + batch); + std::sort(idx.begin(), idx.end()); + const double beta = cfg.kl_beta_max * + std::min(1.0, static_cast(step) / std::max(1, cfg.kl_anneal_steps)); + const se2_draw_noise z = se2_draw_noise::standard_normal(m, draws); + se2_train_event ev; + ev.step = step; + ev.beta = beta; + ev.objective = + se2_rollout_objective(m, train, &idx, &z, beta / (observations * 6.0), &grad, pool); + ev.gradient_norm = opt.update(m.params, grad); + if (!std::isfinite(ev.objective.value) || !std::isfinite(ev.gradient_norm)) + throw std::runtime_error("se2_train_rollout: non-finite loss or gradient at step " + + std::to_string(step)); + ev.prodigy_d = {opt.prodigy_d(se2_leaf_group::D_level), opt.prodigy_d(se2_leaf_group::G_level), + opt.prodigy_d(se2_leaf_group::V_level), + opt.prodigy_d(se2_leaf_group::Minv_level)}; + if (!test_idx.empty() && cfg.eval_every > 0 && + (step % cfg.eval_every == 0 || step == cfg.total_steps)) { + ev.evaluated = true; + ev.test_loss = + se2_rollout_objective(m, *test, &test_idx, nullptr, 0.0, nullptr, pool).rollout; + ev.train_loss = + se2_rollout_objective(m, train, &train_idx, nullptr, 0.0, nullptr, pool).rollout; + } + if (on_step) + on_step(ev); + } +} + +} // namespace ph +} // namespace cvc diff --git a/src/cvc/tests/CMakeLists.txt b/src/cvc/tests/CMakeLists.txt index 2b449df19..68f68102a 100644 --- a/src/cvc/tests/CMakeLists.txt +++ b/src/cvc/tests/CMakeLists.txt @@ -111,6 +111,7 @@ 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(ph_se2_test ph_se2_test.cpp) +add_executable(ph_se2_train_test ph_se2_train_test.cpp) add_executable(nav_terrain_test nav_terrain_test.cpp) add_executable(nav_binding_test nav_binding_test.cpp) add_executable(physics_parity_test physics_parity_test.cpp) @@ -293,6 +294,7 @@ set(TEST_TARGETS physics_plant_log_test nav_dyn_bike_test ph_se2_test + ph_se2_train_test nav_terrain_test nav_binding_test physics_parity_test @@ -1638,6 +1640,13 @@ target_link_libraries(ph_se2_test GTest::gtest_main ) +target_link_libraries(ph_se2_train_test + PRIVATE + cvc + GTest::gtest + GTest::gtest_main +) + target_link_libraries(nav_terrain_test PRIVATE cvc @@ -1979,6 +1988,7 @@ 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(ph_se2_test PRIVATE cxx_std_17) +target_compile_features(ph_se2_train_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) target_compile_features(physics_parity_test PRIVATE cxx_std_17) @@ -2428,6 +2438,7 @@ cvc_discover_tests(nav_plant_test) cvc_discover_tests(physics_plant_log_test) cvc_discover_tests(nav_dyn_bike_test) cvc_discover_tests(ph_se2_test) +cvc_discover_tests(ph_se2_train_test) cvc_discover_tests(nav_terrain_test) cvc_discover_tests(nav_binding_test) cvc_discover_tests(physics_parity_test) diff --git a/src/cvc/tests/data/ph_optim_golden.json b/src/cvc/tests/data/ph_optim_golden.json new file mode 100644 index 000000000..876112214 --- /dev/null +++ b/src/cvc/tests/data/ph_optim_golden.json @@ -0,0 +1 @@ +{"format":"cvc-ph-optim-golden/1","config":{"optimizer":{"learning_rate":0.003,"final_learning_fraction":0.1,"level_learning_rate":0.25,"beta1":0.9,"beta2":0.999,"epsilon":1e-08,"gradient_clip_norm":1.0},"training":{"total_steps":250}},"leaves":["['D']['level']","['D']['log_std']","['D']['mean']","['G']['level']","['G']['log_std']","['G']['mean']","['Minv']['level']","['V']['level']","['V']['log_std']","['V']['mean']","['process']['log_sigma']"],"prodigy_alone":{"learning_rate":0.25,"betas":[0.9,0.999],"eps":1e-08,"estim_lr0":0.001,"safeguard_warmup":true,"params0":[0.029552020666133955,0.08414709848078966,0.09916648104524686,0.06754631805511513,0.004158066243329094,-0.06118578909427189],"steps":[{"step":1,"params":[0.02930202079888663,0.08389709856656068,0.09891648115286611,0.06729631822784779,0.003908066677383593,-0.060935789342191064],"d":0.001},{"step":2,"params":[0.02905476245071599,0.08365239311286014,0.09866673053629395,0.0670489934218202,0.0036647391602645358,-0.06068815683903979],"d":0.001},{"step":10,"params":[0.02745109009666046,0.08168863019000605,0.09667953762564015,0.0651264809535458,0.0028060708259257322,-0.05877648238601976],"d":0.001},{"step":100,"params":[0.009352397847095635,0.059731075981090065,0.07455546356299413,0.043715184055712245,-0.001597263245294015,-0.03747661833242722],"d":0.001},{"step":150,"params":[-0.002809983668155509,0.0451942583856724,0.05991450932632271,0.02946470527082527,-0.00453847517210705,-0.023354328539681467],"d":0.0015254138565904763},{"step":200,"params":[-0.06317897017880818,-0.02679457022594002,-0.012730204097483105,-0.04148333161273534,-0.01957645771418168,0.04685378073854591],"d":0.006541132878467644},{"step":210,"params":[-0.10385035148772834,-0.07647510849029354,-0.06295077702110692,-0.09012987500609658,-0.02659081134480416,0.09531804404561832],"d":0.01019682322663432},{"step":220,"params":[-0.1764316666913635,-0.16119847997000414,-0.1481670191504017,-0.1735588462524391,-0.04676737993428737,0.17755960379325253],"d":0.01618387530602711},{"step":250,"params":[-0.9078062562569605,-1.0412869754419174,-1.0395621334597274,-1.0462004672150733,-0.22335816948898846,1.041682087932129],"d":0.08336658651194233},{"step":300,"params":[-20.107501242757724,-24.175395249080232,-24.46974834070371,-23.948305183449023,-4.3608875022708755,23.75003469560369],"d":1.9513619817237449}]},"shapes":{"['D']['level']":[6],"['D']['log_std']":[4,6],"['D']['mean']":[4,6],"['G']['level']":[3,2],"['G']['log_std']":[4,6],"['G']['mean']":[4,6],"['Minv']['level']":[6],"['V']['level']":[2],"['V']['log_std']":[4,1],"['V']['mean']":[4,1],"['process']['log_sigma']":[3]},"params0":{"['D']['level']":[0.06389650702476501,0.10800185054540634,0.10406936705112457,-0.14543431997299194,0.08677218854427338,-0.1018524318933487],"['D']['log_std']":[-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0],"['D']['mean']":[0.1331792026758194,-0.3917304575443268,-0.10427036136388779,0.4634973704814911,0.08120843768119812,-0.14333952963352203,-0.433470219373703,0.08296264708042145,-0.01908436045050621,-0.05531034246087074,0.038433194160461426,-0.31975507736206055,-0.046034250408411026,0.3595625162124634,-0.024766938760876656,-0.014999395236372948,-0.07859843224287033,0.010053015314042568,-0.34586378931999207,0.019098935648798943,0.11116842925548553,0.11661052703857422,0.3499019742012024,0.1983456015586853],"['G']['level']":[0.09089954942464828,0.021007053554058075,0.1401442140340805,-0.08054587990045547,0.03419248014688492,-0.0880548283457756],"['G']['log_std']":[-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0,-5.0],"['G']['mean']":[-0.0,0.0,-0.0,-0.0,0.0,0.0,-0.0,-0.0,0.0,0.0,-0.0,-0.0,0.0,0.0,-0.0,0.0,-0.0,0.0,-0.0,0.0,0.0,0.0,0.0,-0.0],"['Minv']['level']":[0.0016217982629314065,0.061325740069150925,0.03283735737204552,0.09521422535181046,-0.1438298225402832,0.08397269994020462],"['V']['level']":[-0.04833054915070534,-0.009626680985093117],"['V']['log_std']":[-5.0,-5.0,-5.0,-5.0],"['V']['mean']":[-0.051354505121707916,-0.0036530299112200737,-0.0610194094479084,0.09522546827793121],"['process']['log_sigma']":[-2.386624813079834,-2.3100271224975586,-2.19207501411438]},"gradient_rule":{"scales":[0.02,0.3,4.0,0.05,2.0,0.7,0.01,0.2,1.5,0.03],"zero_process_at":3,"steps":300},"steps":[{"step":1,"params":{"['D']['level']":[0.06364651366222598,0.107751854833885,0.1038193724319737,-0.1456843113366513,0.08652221024515211,-0.10160244428870499],"['D']['log_std']":[-4.997000001560315,-4.997000001284498,-4.997000002766647,-5.0029999970345225,-5.002999998663901,-5.0029999984054845,-5.002999995887216,-4.997000017410369,-4.997000003884346,-4.99700000202104,-4.997000001520326,-4.997000002285003,-5.00299999462565,-5.002999998674336,-5.0029999987297264,-5.002999996886498,-4.997000005429498,-4.997000002412916,-4.997000002170666,-4.997000002063605,-4.997000002676449,-5.002999982636127,-5.002999998417973,-5.002999998831691],"['D']['mean']":[0.1301792047666566,-0.3887304631822442,-0.10127036309088587,0.4664973687087049,0.08420843503003146,-0.14033953446109612,-0.43647019928194064,0.0799626491996434,-0.022084359174177247,-0.05831034078769656,0.041433174042714696,-0.31675507892144783,-0.04303425177183611,0.3625625137570807,-0.02176695956106471,-0.01799938953891557,-0.08159842968172525,0.007053016928274319,-0.3488637876224166,0.016098945556001217,0.11416842748003188,0.11961052586295684,0.35290197238141596,0.20134465317591643],"['G']['level']":[0.09064955687812455,0.020757059423346057,0.1398942195481185,-0.08079587407493025,0.033942496565460015,-0.08780483483979476],"['G']['log_std']":[-4.997000001198915,-4.997000001429677,-4.997000010906113,-5.002999997852614,-5.002999998360211,-5.002999997810818,-5.002999996397203,-5.002999987357606,-4.997000003500586,-4.997000001435845,-4.997000001338527,-4.997000004023492,-5.002999997727965,-5.002999998689673,-5.002999998243054,-5.002999994491411,-4.997000010665749,-4.997000003410327,-4.9970000018672955,-4.997000001528122,-4.997000002829261,-5.002999996689838,-5.002999998756688,-5.0029999986373],"['G']['mean']":[-0.002999995565399956,0.0029999959035824043,0.002999997667506168,0.0029999978514282113,0.0029999979260218646,0.0029999969215593583,-0.0029999935898726055,-0.0029999985866220103,-0.002999998798962188,-0.0029999973114361966,0.0029999963484794055,0.0029999983393026604,0.002999998134640838,0.00299999711920321,0.0029999942924187565,-0.0029999910627331165,-0.0029999981624351676,-0.002999998739965414,-0.0029999980432798457,0.002999994113003221,0.0029999985646271096,0.0029999985676180665,0.002999997056179144,0.0029995668556000083],"['Minv']['level']":[0.001371810939506455,0.061075746162429834,0.03258736147680608,0.09496423024880869,-0.14358002114797977,0.08422269589218669],"['V']['level']":[-0.0480805522966866,-0.009376686710740436],"['V']['log_std']":[-5.002999987456409,-5.0029999973907575,-5.002999997797358,-5.002999997925546],"['V']['mean']":[-0.054354502781537535,-0.006653017054449737,-0.058019411438087226,0.09822546711028532],"['process']['log_sigma']":[-2.3836248146957875,-2.313027074479364,-2.19507501219616]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":2,"params":{"['D']['level']":[0.06343064403365807,0.10753302137227017,0.10360771455194785,-0.14588727689659817,0.08632195251435927,-0.10139919859383308],"['D']['log_std']":[-4.994522723493975,-4.994461357993797,-4.9942649291018935,-5.005484903023993,-5.005586456611206,-5.005604365070145,-5.005479139505976,-4.9990604777784595,-4.9944293886232884,-4.994533402822088,-4.994509307952319,-4.994401947549876,-5.00539897861704,-5.005532219247154,-5.005599270469207,-5.005622759969153,-4.9941109481838994,-4.9943308055111135,-4.994490117259353,-4.994540540936979,-4.994501696152939,-5.005324210068407,-5.00548528982833,-5.005556178409281],"['D']['mean']":[0.12751505595059334,-0.3862208625908833,-0.09864326978146419,0.4690847636981337,0.08667963910913593,-0.13792981922624897,-0.4389179371188896,0.07750411054377983,-0.024593784603438418,-0.06091401989197699,0.04374971665349623,-0.3141980895945437,-0.04042600949703474,0.3651173977372931,-0.01945753269010314,-0.02089533396804739,-0.08407818737230961,0.004579016221862579,-0.3513963711213044,0.01778190598641988,0.11666396174149711,0.12218427848481941,0.35552726508011856,0.20358541741375033],"['G']['level']":[0.09042073665585428,0.020543304707446444,0.13968855574866146,-0.08100176645625459,0.033714748176826785,-0.08760064941917368],"['G']['log_std']":[-4.99447363363035,-4.994393314709018,-4.996680685723726,-5.005606223615403,-5.0056192370520485,-5.005519071705551,-5.005425427915604,-5.005399843436374,-4.994556555954547,-4.994514231994676,-4.994446867314268,-4.994075520686283,-5.005512144914675,-5.005595233879913,-5.005596210643477,-5.005438522064953,-4.998764302228379,-4.994471747781807,-4.994533596072404,-4.994499964926236,-4.994342358641543,-5.005439288777315,-5.005544966285862,-5.005608381048169],"['G']['mean']":[-0.005606478970654222,0.005829847246699639,0.005637605927498495,0.005493692118531357,0.005460056737941773,0.005519233279808588,-0.005387610688703857,-0.005497470234937154,-0.0055691969187262845,-0.005705749592261146,0.0055538881227509855,0.005629295627663643,0.005569456564461339,0.0054550381610795565,0.005406385122601133,-0.005436538987042321,-0.00546572603014059,-0.00552036940694214,-0.005637472237174562,0.005418054702015267,0.005570284836602409,0.005609116346180349,0.005528532343019725,0.005236396466070797],"['Minv']['level']":[0.001147042065046207,0.06086976870977501,0.032380719192524124,0.09475153630510028,-0.14339203980412601,0.08443199232760866],"['V']['level']":[-0.0478651786860816,-0.009157686709013178],"['V']['log_std']":[-5.005512880902927,-5.005715361012085,-5.005540233260303,-5.00546268849137],"['V']['mean']":[-0.05683369207324393,-0.005386202770774193,-0.05555370768492125,0.10076361174990538],"['process']['log_sigma']":[-2.3809989298772525,-2.3153343476492165,-2.1976924316256703]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":3,"params":{"['D']['level']":[0.06321213267373049,0.10730124108092372,0.10338083219376817,-0.14610934022518532,0.08610143504843802,-0.10117969296685786],"['D']['log_std']":[-4.991814714541219,-4.9917331622561605,-4.991672211881509,-5.0081809509890265,-5.00832592184678,-5.008383504058213,-5.00813041064595,-5.001504122735767,-4.9917292526529184,-4.991844652836355,-4.991793621119421,-4.99175562021507,-5.008059427836603,-5.008258700311612,-5.008358360788613,-5.008386314056602,-4.99455050735359,-4.991516993104057,-4.991788893532874,-4.991848481230662,-4.991803558758209,-5.007869506244418,-5.008198443728113,-5.008290949041998],"['D']['mean']":[0.12472637639016564,-0.38376247254583745,-0.09590530030709234,0.4718494547613267,0.08935617904842082,-0.13526031131581698,-0.4413265454167601,0.07481469288395075,-0.027317097242040386,-0.06363422123566238,0.04637215194244563,-0.31147802418810905,-0.03765366400130114,0.3678426414179939,-0.016912786867984222,-0.02377367255867263,-0.08675888081586461,0.001873653053674792,-0.35411330061502755,0.020012813831457376,0.11937909982332645,0.1249255255117229,0.35831759260646256,0.20581418294507525],"['G']['level']":[0.09020074512905224,0.020315149009762987,0.13946463571601747,-0.08122718852192515,0.03369891988117953,-0.08737651856915032],"['G']['log_std']":[-4.9917456703711816,-4.991641935344056,-4.994873475322992,-5.008267110501188,-5.008404659949695,-5.008225290527748,-5.008089786517483,-5.007881930189685,-4.991899431965793,-4.991800746088162,-4.991718372499018,-4.99238868408537,-5.008216588958539,-5.00834427116061,-5.008368191140946,-5.008058224588042,-5.0010984400893985,-4.991785006037044,-4.99184023393773,-4.9917814042401165,-4.991890904011013,-5.008126916083356,-5.008275134207806,-5.0083769192260865],"['G']['mean']":[-0.00831242041476787,0.0066605620766754066,0.008433866891671695,0.008187174799182913,0.008155816051580674,0.008185166346278472,-0.008010405064735698,-0.008216185011405221,-0.008307145340732992,-0.008528457691726037,0.008100602268514369,0.00839000574304609,0.008317895241430742,0.00812475731594104,0.008076797964543338,-0.007999468585889997,-0.008163693984272032,-0.008246273689990584,-0.00834283580597164,0.008089131033282376,0.008297406360859943,0.00838755775535581,0.008225625756229968,0.00774891177961241],"['Minv']['level']":[0.0009173925854377853,0.06064617865232026,0.032154893550392125,0.0945259828523386,-0.14317969169867864,0.08465870023845119],"['V']['level']":[-0.047636224549057804,-0.008925098588443823],"['V']['log_std']":[-5.003761830187651,-5.008497090634536,-5.008260043695066,-5.008150795521731],"['V']['mean']":[-0.05954033905728072,-0.0033388427651210614,-0.05285226295465599,0.10349436482047736],"['process']['log_sigma']":[-2.378237220315982,-2.3173019604833773,-2.200387446806181]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":4,"params":{"['D']['level']":[0.06303050174549013,0.10707712060216748,0.10315345626787817,-0.1463284305827788,0.08591701553187792,-0.10095750219212246],"['D']['log_std']":[-4.989225171012833,-4.9892146321248285,-4.989308680174389,-5.010585252432992,-5.010865751135349,-5.011119441198398,-5.01082931490228,-5.003897739731386,-4.988962615052387,-4.989199850619105,-4.989237261701395,-4.989499715420552,-5.010597296227941,-5.01077599224578,-5.010971791129411,-5.0111915714325255,-4.995799705285797,-4.988726187819811,-4.989083278238119,-4.989235011651846,-4.989403104217433,-5.010467337950566,-5.010761073674848,-5.0108253322605645],"['D']['mean']":[0.12202519908041964,-0.38215281834758735,-0.09336343845385031,0.4745990394166939,0.09203014923268989,-0.13273486284761934,-0.44393896058701293,0.07219365802656202,-0.029864485330533527,-0.0661109800426253,0.04869332884449009,-0.3090089014792725,-0.034988028325770054,0.3705897745794776,-0.014328528062107921,-0.026689375065838614,-0.08944117335720696,-0.0007225461143204517,-0.35658001157197505,0.022356874799093193,0.1218986705571333,0.127477254729602,0.3610629199260378,0.208314381269706],"['G']['level']":[0.08999482825725517,0.020085917023831133,0.13924365290444676,-0.08143853098255385,0.03377517616105626,-0.08716073498176061],"['G']['log_std']":[-4.9892089257782395,-4.9890607814725945,-4.9926560678273875,-5.010572793996894,-5.011127312198788,-5.01093942079542,-5.010697318218305,-5.009410838341534,-4.989245490445853,-4.9892235450391444,-4.9892112967950135,-4.990785902367354,-5.010637438678872,-5.010918923127891,-5.011118905134193,-5.010729707544102,-5.003227465601018,-4.989050456528991,-4.989209308704264,-4.989244553517219,-4.989988393104928,-5.010660789189576,-5.010797056181833,-5.011024719207605],"['G']['mean']":[-0.01110059918417788,0.006618431441674905,0.011225689338739195,0.01087643356815194,0.010751536809119219,0.010441138385915238,-0.01062090686814193,-0.010769526446455875,-0.010847014593779295,-0.011323863804218896,0.010020082912960006,0.011002679048734391,0.01106258735835318,0.010781419271130504,0.010536488268757008,-0.010670142643232582,-0.010771807629090786,-0.010781881633187383,-0.010801232821307677,0.010460980487642957,0.010792164855886792,0.011086230185127076,0.01095735191918148,0.010281392866281389],"['Minv']['level']":[0.0006805305723642893,0.060423991193851015,0.03193980316550881,0.09432461275349581,-0.1429704041806691,0.08486841777792624],"['V']['level']":[-0.04742184550242443,-0.008693419509221551],"['V']['log_std']":[-5.001569577288924,-5.011200936569921,-5.01099181830628,-5.010787630871651],"['V']['mean']":[-0.062038395241108345,-0.0009929368724755773,-0.05027411427180845,0.1060279566069539],"['process']['log_sigma']":[-2.375975397827266,-2.3189134226055734,-2.2025946470090974]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":5,"params":{"['D']['level']":[0.06287053198257532,0.1068465902934333,0.10292194265381331,-0.14655777597623956,0.08583378624536869,-0.10072653446592139],"['D']['log_std']":[-4.986547467582926,-4.986635640949479,-4.986747981325034,-5.012689416381778,-5.013538986789459,-5.01389451870548,-5.013591137822487,-5.005924293849391,-4.986255334963462,-4.98643220898625,-4.986628814199654,-4.987335281130776,-5.013106748223262,-5.013342396773174,-5.013719858975471,-5.013893344970814,-4.997690407852396,-4.986011361281737,-4.986299134169889,-4.986511608311202,-4.987267931777523,-5.013204821828315,-5.013380858885691,-5.01345854624101],"['D']['mean']":[0.11928970050072361,-0.382257902075427,-0.09066510255033888,0.47737134623092636,0.0948134796689198,-0.13027383217892416,-0.4465078084355115,0.06945711546634277,-0.03246902826299037,-0.06872123456635211,0.049993475706455064,-0.30650087251950414,-0.032217169313485665,0.3733515867154051,-0.01160464947503938,-0.029071384012800232,-0.0922254941972724,-0.0034133849455055443,-0.3590247702368149,0.024748157590641508,0.12441571674706506,0.13015487933802627,0.36382559156148453,0.21083226611182407],"['G']['level']":[0.08977966229285647,0.019855537680409797,0.1390124890325816,-0.08164987911359031,0.03392379708205802,-0.08693800264797856],"['G']['log_std']":[-4.986608410563249,-4.98633564517556,-4.990603259671364,-5.012889607462677,-5.013897915313157,-5.013720879017288,-5.0134140970831504,-5.008842146287536,-4.986470486927287,-4.986569496778577,-4.986632450365159,-4.988731261737102,-5.012889190744292,-5.013635403971711,-5.013889850796244,-5.013491919894073,-5.003663332973126,-4.986301739810876,-4.986458361202411,-4.986675683741762,-4.9882368064156575,-5.013166036483677,-5.013392398720266,-5.013789343863447],"['G']['mean']":[-0.013763935760913344,0.006179461193240085,0.013958121597004472,0.013663181688873989,0.013439415003598437,0.012149946025147096,-0.013356670801338736,-0.013376407702623161,-0.013503687042461743,-0.013998385931028541,0.010894510933815424,0.013744890601631914,0.01383653602518465,0.01355854128690713,0.012701542625940742,-0.013379582638881733,-0.013485255075495407,-0.01337240123439068,-0.013430312344369463,0.012222565449906929,0.013376955436177054,0.013862716952857247,0.013718146846238195,0.012916992735502609],"['Minv']['level']":[0.00046553493081168286,0.060192127718724606,0.03171813270303162,0.09412708763161695,-0.14276173768192008,0.08507947531379227],"['V']['level']":[-0.04719603748796966,-0.008464721990781306],"['V']['log_std']":[-4.999168961701699,-5.0138698717583745,-5.013766147676803,-5.013546585002766],"['V']['mean']":[-0.06446532404179409,0.0015473856702086608,-0.0476258980916207,0.1086369870819148],"['process']['log_sigma']":[-2.3734598668531146,-2.3177543655897153,-2.204982760671163]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":10,"params":{"['D']['level']":[0.06194214942660424,0.10573849429431946,0.10182705481436483,-0.14762281691725695,0.08559662787002247,-0.09966088165696303],"['D']['log_std']":[-4.973732424604517,-4.97372898757391,-4.974255830005606,-5.022910221867548,-5.026829953996961,-5.027154258016549,-5.026420769861655,-5.0089378348072,-4.973595381377872,-4.973378808751304,-4.973980790289653,-4.9753796868377105,-5.0240606487925605,-5.026166977168675,-5.027127407969517,-5.026630815619876,-4.999575610247713,-4.973095121178852,-4.973167581825906,-4.9736577840731755,-4.976646874879757,-5.025116399746866,-5.026025806321683,-5.0266074293316025],"['D']['mean']":[0.10619575161879904,-0.38163302328959836,-0.07740521496016553,0.4905594969222297,0.10786315159352947,-0.11946025097077881,-0.45798750823588896,0.056558680936356534,-0.04526367389108168,-0.08185999566394096,0.052423239063777174,-0.2937220351397509,-0.018834096627097213,0.3863796885996394,4.8329032710323766e-05,-0.040472137197917195,-0.10529592161434272,-0.01625444238608453,-0.37141732525214716,0.029208870928920105,0.13672526697760348,0.1434517151292838,0.37701605776799424,0.2217390748123204],"['G']['level']":[0.08873934119370001,0.01876609983401731,0.13792318755681365,-0.08265885775306954,0.0342849570348341,-0.08589271010110354],"['G']['log_std']":[-4.973705190526285,-4.972973525726194,-4.980582893588173,-5.025299029232295,-5.027164746886022,-5.026843865301246,-5.0259423517828274,-5.005520915657412,-4.97357585705193,-4.973778199174562,-4.973653235955775,-4.977760284875005,-5.0242625787753825,-5.027004921112508,-5.027084830565649,-5.026189858901777,-4.999292198478931,-4.973441820521852,-4.97345074753775,-4.974102761411831,-4.977112432333001,-5.024692265320374,-5.026384875440702,-5.027179881571012],"['G']['mean']":[-0.026208949407668262,0.012906314827469593,0.02693273363135386,0.026799425407860008,0.026146117485969067,0.021246505540874448,-0.025665556127491482,-0.026079600231837325,-0.026744748625462035,-0.026736169879545895,0.01716573676454435,0.02705401029324971,0.027001140739046234,0.026538483811258055,0.021641647229069052,-0.025689787024162296,-0.026349751212100293,-0.02619903219964139,-0.02652540554215836,0.01914642523557018,0.02643111573674118,0.027205664525036866,0.02667535827773531,0.0230129783865883],"['Minv']['level']":[-0.0005442673498552707,0.0591018301987988,0.03065453372541466,0.0931025074255276,-0.1420185130852072,0.0861241510760162],"['V']['level']":[-0.046082119263788586,-0.007377843314627023],"['V']['log_std']":[-4.9899849532326535,-5.0266245419274425,-5.026869080243486,-5.026551951583085],"['V']['mean']":[-0.0761592169672641,0.011086233804900012,-0.03504522637154456,0.1216380270318284],"['process']['log_sigma']":[-2.3606666801070535,-2.3114103073425825,-2.2175947036150627]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":50,"params":{"['D']['level']":[0.055271633789413833,0.09742106237617859,0.09358309194330512,-0.1556892823584212,0.08397518860990383,-0.09171146876889379],"['D']['log_std']":[-4.8755207348947245,-4.876641614283482,-4.881715519539009,-5.102457666008185,-5.125471819374909,-5.1271315144140726,-5.12227716499129,-5.023048841792456,-4.878201416069216,-4.874529193075607,-4.876961211012577,-4.889389331169867,-5.109826063131272,-5.122909757312211,-5.12718900064139,-5.123154265233942,-5.002539931108104,-4.8752171264180895,-4.874422075418159,-4.875513365991814,-4.89475665541096,-5.1123386903513826,-5.123132298137514,-5.124938437183195],"['D']['mean']":[0.007766694092806532,-0.37658863586367225,0.020610624501229968,0.5900850107815466,0.20595536556822797,-0.0343753196885918,-0.5428537782368265,-0.041712235278672564,-0.14274935718633608,-0.17842133735233612,0.06593800227177533,-0.19864495902554127,0.08147001858710265,0.4845192037340984,0.0854519727382261,-0.12930914324745524,-0.2035058949677242,-0.11459705457411078,-0.4647448574879465,0.052062039537745027,0.2312579849535136,0.2425264693685814,0.47651856812636323,0.3010962247441942],"['G']['level']":[0.08095838020220135,0.010552234430958539,0.12968859579631573,-0.09047768244175372,0.03632771960702087,-0.07785363251844694],"['G']['log_std']":[-4.876081253267738,-4.873656131381876,-4.900654269426682,-5.114766283219526,-5.127059465640934,-5.125539656296076,-5.121495981303298,-4.975275629361171,-4.876693767545073,-4.875752834597126,-4.876667403132073,-4.892841778622455,-5.111024871024225,-5.126441913370855,-5.126675103587185,-5.1206126450061396,-4.954655151166669,-4.87689620741519,-4.874583431950291,-4.877865131774817,-4.899419005284153,-5.11488817477722,-5.123874809934842,-5.12737040374842],"['G']['mean']":[-0.12032317634357814,0.06335316948565986,0.12519106074771408,0.12561248867712635,0.12363113528464574,0.0904716020788399,-0.11826079097177437,-0.12330104422004175,-0.1254154072325014,-0.12347419146617686,0.06476169203115882,0.1261853396491522,0.1262870559791229,0.12427321521908286,0.09283998473731934,-0.11667571842737952,-0.12469684007765912,-0.1234961113754269,-0.12242697452502449,0.07388213669903032,0.12328360061100001,0.12749922675733477,0.12402585880852085,0.09604064839164093],"['Minv']['level']":[-0.008257226776752833,0.05088731469137495,0.022495772667224627,0.08548960489391104,-0.13654575530083543,0.09408387548173058],"['V']['level']":[-0.037784778784416086,0.0008442646383373807],"['V']['log_std']":[-4.927275175964254,-5.122924897288172,-5.125506260733689,-5.125217165130381],"['V']['mean']":[-0.16688982333579508,0.07485714893230472,0.06180608540413169,0.2195133179786707],"['process']['log_sigma']":[-2.2617368100147766,-2.253896855514738,-2.31177816959382]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":100,"params":{"['D']['level']":[0.04815488390022857,0.08878486009796356,0.08489282292139032,-0.16413040595591413,0.08226737594520529,-0.08334123323704037],"['D']['log_std']":[-4.771915247254339,-4.773670023546975,-4.788568700781529,-5.189390190287963,-5.228625228966586,-5.2316176693962255,-5.223673456430314,-5.036105768637212,-4.777070070732568,-4.770695362121645,-4.77405688394061,-4.798604480636341,-5.198882524073764,-5.225677588479735,-5.231403426750032,-5.224864574455448,-5.007041268736768,-4.773541765209185,-4.7703000320004705,-4.772442281491799,-4.805368678138964,-5.202659337791445,-5.226006237115974,-5.228634815384647],"['D']['mean']":[-0.09381129517240924,-0.3724591681143693,0.12197645956952267,0.6945223165227951,0.3092008540831332,0.05454786835992996,-0.6345217447335614,-0.14486903691397582,-0.24624312552648736,-0.27904168599274887,0.07911557950201235,-0.09784444477553943,0.18585252378491693,0.5880654111889777,0.17316408604172784,-0.22284435142939962,-0.3069571910945139,-0.21822751352106037,-0.5645283998374251,0.07364229676849342,0.33212584424969516,0.34643764985927294,0.5802217472169201,0.3873411976053176],"['G']['level']":[0.07310740691257396,0.0018974652587783907,0.12103642057563135,-0.09878523079032328,0.03870846940990828,-0.06941017586856564],"['G']['log_std']":[-4.772504938604763,-4.7703980343229535,-4.815101202537074,-5.2085077463463225,-5.231101011572072,-5.229635437683094,-5.221236745293003,-4.939286823958295,-4.774995981318163,-4.772109134084912,-4.774076374629863,-4.80911897016139,-5.205044810083031,-5.230113465857228,-5.2310393669511415,-5.220280729259511,-4.911452876872958,-4.774692142365055,-4.770728083397721,-4.77548658887071,-4.819481126832178,-5.210088541010516,-5.227081180647237,-5.231580870026858],"['G']['mean']":[-0.220217503846357,0.1116231938892307,0.22791179900830896,0.22971904154962905,0.22615195204854438,0.16731165928653285,-0.21433620868859157,-0.22647505913297764,-0.22908421547400473,-0.22337830767157543,0.11780610554033374,0.22876890937659183,0.23071646738632912,0.22693214377599832,0.1680744318235755,-0.2135149201747188,-0.22811033813433232,-0.2268326204061123,-0.22127477145236324,0.1330653095016265,0.2252864218496773,0.23199119029632423,0.22693758593572463,0.16858380776078566],"['Minv']['level']":[-0.01642622023113338,0.04224794826919954,0.013878088185085872,0.07735837555936034,-0.13118342579363274,0.10257019526422467],"['V']['level']":[-0.029111332183908205,0.009441685898209188],"['V']['log_std']":[-4.858689121812551,-5.221529967997791,-5.2295342238372235,-5.228813719430355],"['V']['mean']":[-0.2640890900933812,0.1425410475339751,0.16395550216441082,0.3231671113147227],"['process']['log_sigma']":[-2.158836107988376,-2.1883617055673197,-2.409883001853603]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":150,"params":{"['D']['level']":[0.04332409896209695,0.0829130073901635,0.07898766546554885,-0.16987686342719777,0.08113052601428401,-0.07765146693963508],"['D']['log_std']":[-4.70135675795804,-4.703618992493446,-4.725153461351429,-5.248550164887299,-5.298770761534538,-5.302638623533742,-5.292559534619358,-5.044702347570936,-4.708391006723442,-4.700040630057587,-4.703979132843028,-4.736935119348801,-5.259674562634201,-5.295597711141676,-5.302267514973048,-5.293958620433606,-5.010059660630875,-4.704420873489269,-4.699535713589785,-4.702254091355461,-4.744556816633879,-5.264108688290268,-5.2960705014997265,-5.299163540762483],"['D']['mean']":[-0.16288809851552533,-0.36991823811197794,0.1909100345897389,0.7654959126765409,0.37940144313037893,0.11524483035891407,-0.69670490942739,-0.21510482337096923,-0.3166916921452743,-0.3474585261979275,0.08775508745688342,-0.02929705078600131,0.2568241865412156,0.6584133222187252,0.23287559864843063,-0.28642223035367465,-0.377286397859058,-0.28880158246058885,-0.6324061125099062,0.08814627186856289,0.4008132322192196,0.41710294834493955,0.6507148590615709,0.4458229507171394],"['G']['level']":[0.06776247167106172,-0.0039828760778969925,0.11514998078339539,-0.10444632635117723,0.040324414608296916,-0.06365694159665437],"['G']['log_std']":[-4.7020255130441795,-4.700182152461304,-4.756938999418219,-5.272211355109701,-5.301829488595495,-5.300375063760393,-5.289185917756578,-4.914619582604181,-4.705821077157074,-4.701528934053257,-4.704300199823555,-4.751990924176815,-5.269005916864339,-5.30061070965587,-5.3019668153571375,-5.288007098872216,-4.88157871556981,-4.705270524691013,-4.700036369207951,-4.705779741819016,-4.765182736976364,-5.275021323928202,-5.297285459766067,-5.302438568556308],"['G']['mean']":[-0.28805074588415686,0.14453282007089546,0.29772213333239367,0.30048984739461376,0.2959882651717841,0.21944985632294273,-0.27976704641483174,-0.2967252650368261,-0.2995862643488617,-0.2913163261641193,0.15361384134568057,0.29852836145065087,0.3016789974642446,0.2967620716808593,0.21939458009185808,-0.27926269910945567,-0.2985332238922554,-0.2971575374687975,-0.2884976872723482,0.17328910020249133,0.2946497090040526,0.30302998201892184,0.2968495281301147,0.21803198896490697],"['Minv']['level']":[-0.021974556211406596,0.03637237457674148,0.008008948401618378,0.07183016085373409,-0.12752512883818512,0.10834660724977002],"['V']['level']":[-0.023213194333749458,0.015284650054189107],"['V']['log_std']":[-4.8122122895847355,-5.288611933704068,-5.300218389307852,-5.299321278211629],"['V']['mean']":[-0.33030846673595116,0.1884386745394633,0.23354917423683463,0.39368414977398525],"['process']['log_sigma']":[-2.088880770495878,-2.1438036668884766,-2.4765797732565353]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":200,"params":{"['D']['level']":[0.04074647126861629,0.07981273155707426,0.07587133011179086,-0.17290991352842777,0.08053983195372937,-0.07464602297684811],"['D']['log_std']":[-4.6641164144332325,-4.666546074289503,-4.6916048092481155,-5.279787443498755,-5.33590326256354,-5.340112759793426,-5.328934685573933,-5.049018191946791,-4.672169316248752,-4.662740235160419,-4.666976129772531,-4.704092293920183,-5.291611542828117,-5.332594708024817,-5.3397303476860545,-5.330377821564148,-5.011441049122414,-4.667980751327322,-4.662179735776056,-4.665215070135005,-4.712397943376575,-5.296578394471953,-5.333057717021231,-5.336493068894383],"['D']['mean']":[-0.19934696737925833,-0.36847079387504994,0.22739147523608436,0.8029382584807282,0.4164697163680274,0.14712590734547878,-0.7295462238399957,-0.25217249069477393,-0.3539216334201148,-0.383718786945529,0.09214392815899763,0.00703145417957645,0.2943115039768398,0.6955286848875988,0.2644064571301296,-0.319843418042629,-0.41442134574376416,-0.32604826496286066,-0.6683543777371499,0.09560754904516737,0.4370958252314291,0.454499969855165,0.6879047426498623,0.4767442880969575],"['G']['level']":[0.06493842586064431,-0.00708510971723026,0.11204219400490899,-0.10743232210603615,0.04117501378253729,-0.06062267669197737],"['G']['log_std']":[-4.664751212028562,-4.663041763112872,-4.726411682443666,-5.306093596266232,-5.339155881855934,-5.337714345644032,-5.325027623013051,-4.901710657618407,-4.669292530605321,-4.664268314845576,-4.66735166028527,-4.721858797123982,-5.302864418551961,-5.337905200340927,-5.33938400992183,-5.323785348322833,-4.865884967846296,-4.668634909257975,-4.662721384261627,-4.66895369519228,-4.736145330462692,-5.30920260626138,-5.334443467297539,-5.339876274256628],"['G']['mean']":[-0.3238085904672853,0.16229989743177684,0.33452739326439207,0.33785453089018147,0.33283279381526953,0.24714255546513506,-0.31429592587427546,-0.33383050859640423,-0.33690397901512487,-0.32712546690430694,0.17282260805238234,0.33540323606282263,0.3391186260748198,0.33363362710824224,0.2462461089580358,-0.3139999439826682,-0.3356972906335961,-0.3343428470535303,-0.32411893849264645,0.19430174782141924,0.3313981067838988,0.3405337673425629,0.33374497153011323,0.24404822550307984],"['Minv']['level']":[-0.02489619247444066,0.033270039041595874,0.0049111304026494265,0.06889737884287221,-0.1256106314970915,0.11140054078932264],"['V']['level']":[-0.020092875552068398,0.018365701427800703],"['V']['log_std']":[-4.78757214275517,-5.324000174384363,-5.3375193653693085,-5.336540697666719],"['V']['mean']":[-0.36525005613235595,0.21277265354187522,0.27026344947851544,0.4309934793420071],"['process']['log_sigma']":[-2.0519083243919782,-2.1204400893287594,-2.5119654395498556]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":250,"params":{"['D']['level']":[0.03959450060978357,0.07843568294192442,0.07448636174171604,-0.17425732126589802,0.08026768343519962,-0.07331123007517838],"['D']['log_std']":[-4.647539627112386,-4.6500215882436144,-4.676739451958928,-5.293797891613735,-5.35242276896012,-5.356761971902541,-5.345096399835291,-5.05093478744883,-4.656071909608402,-4.646161393273027,-4.650486498219986,-4.689442804694655,-5.305817189707029,-5.349089127817207,-5.356382240217195,-5.346558926333148,-5.012002006983035,-4.651802448577447,-4.6455781742756725,-4.648744562667521,-4.697982154783236,-5.31096616044443,-5.349536702402113,-5.353117721158125],"['D']['mean']":[-0.21553056956996594,-0.36772510930364555,0.24360024495585775,0.819575624237781,0.4329403113723856,0.1613229884295812,-0.7441443011582236,-0.26865246216245414,-0.37051517614895185,-0.39984850162078744,0.094164088853084,0.02322810603891913,0.3109678895587485,0.7120227992067002,0.2783690987604939,-0.33468250374298447,-0.43092238610549016,-0.34262386820238533,-0.6844013897501185,0.09887487015016033,0.4532838507763685,0.4711417702566684,0.7044236158356103,0.4904855067740416],"['G']['level']":[0.0636871235447246,-0.008463715860821949,0.11066099824320251,-0.10876378048324367,0.04154892955742399,-0.05927285663421878],"['G']['log_std']":[-4.648138227379425,-4.646533411945913,-4.712850786212132,-5.321187539869657,-5.3557365582480285,-5.354308599975738,-5.340951398717992,-4.895995708863835,-4.653066998095684,-4.647674838847715,-4.650887304688592,-4.708540678640264,-5.318017022700901,-5.3544877868740794,-5.356009081681234,-5.339675659139549,-4.8589902281348785,-4.652354060177595,-4.646131851336506,-4.65253342060606,-4.723221451108378,-5.324435630525842,-5.351001340275168,-5.356511482143479],"['G']['mean']":[-0.3396998370328438,0.1701727851864218,0.35087493145428417,0.3544595926753465,0.3492247273315982,0.2596070910378934,-0.32961611455372936,-0.3503671297290287,-0.353515186563209,-0.3430155834144915,0.1815146257091341,0.3517826626658788,0.3557564265213447,0.3500150219640396,0.25825122131656736,-0.3294311965610029,-0.3522280874944471,-0.35091877598691684,-0.33994471045637853,0.20376365372301086,0.34776515736603847,0.35719597005234244,0.3501413493626155,0.2555366231693534],"['Minv']['level']":[-0.026194308062808293,0.031891443126490886,0.0035318120430394154,0.06758806940973024,-0.12476480780643212,0.11276285388937496],"['V']['level']":[-0.01870503305269915,0.019734286043028446],"['V']['log_std']":[-4.776646222625833,-5.33969577357936,-5.35409651654939,-5.353084465041864],"['V']['mean']":[-0.3808554074378812,0.22353729545169998,0.2866082813505631,0.4476183035366218],"['process']['log_sigma']":[-2.0354856352556734,-2.110048685533968,-2.5277033351347353]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":251,"params":{"['D']['level']":[0.039580108345291166,0.07841621841840647,0.0744661380672328,-0.17427693010453982,0.08026607389158903,-0.07329139067743103],"['D']['log_std']":[-4.647306675488581,-4.649797170399011,-4.676535229164807,-5.293977202129697,-5.352649082141217,-5.357000652920686,-5.345336611246128,-5.050968111524862,-4.655833064607237,-4.645921616066971,-4.650258743910337,-4.68925466263089,-5.306016988821195,-5.3493129887779265,-5.356614212943449,-5.346796541968737,-5.012044730731329,-4.651569951250368,-4.645335279090417,-4.648509182584968,-4.697797138068775,-5.311184995288411,-5.3497651461539135,-5.353344760939505],"['D']['mean']":[-0.2157593593623682,-0.3677531630635035,0.24382320563791665,0.8198162934216172,0.433181663633382,0.1615201651729809,-0.7443700327539656,-0.26888890705391544,-0.3707429900177817,-0.40006692322656334,0.09417669057063585,0.023445154809832003,0.3112024655309101,0.7122645423115388,0.27858145155191244,-0.33490339393877194,-0.43116428512919275,-0.3428577121705394,-0.6846153589834831,0.09894009069941433,0.45350382795013716,0.471370257267949,0.7046600812807592,0.4907022129687988],"['G']['level']":[0.06366974882049448,-0.008483852875589119,0.11064096561529581,-0.10878206676858763,0.041557353435479664,-0.059253720795462275],"['G']['log_std']":[-4.647911248401666,-4.646304976013847,-4.712643585717047,-5.321384661508333,-5.355972815260687,-5.354551458461876,-5.3411816349297565,-4.8958850328421475,-4.65282853421393,-4.647443393302147,-4.65066427900555,-4.708354705229631,-5.31821441515357,-5.354716849957915,-5.3562490215630545,-5.339913162008134,-4.858888917593672,-4.652113119072454,-4.645893475320974,-4.652308733287785,-4.723060320225191,-5.324646163665925,-5.351226519243074,-5.356744859592337],"['G']['mean']":[-0.3399363135625495,0.17025924114255817,0.3511114911100556,0.3547022488084087,0.34945699014676856,0.2597564731419823,-0.3298423805799626,-0.3505950099528587,-0.35374251686789376,-0.3432432169686205,0.18160213087877536,0.3520108264253236,0.35599817652136856,0.35025469078475874,0.25840992540378843,-0.32966524390103275,-0.3524631963683453,-0.3511451962853444,-0.3401597167992633,0.20387748756267185,0.34798653421408454,0.3574323669408401,0.3503830529391012,0.25571364487038584],"['Minv']['level']":[-0.026213616680828358,0.031871334235475564,0.0035125127496200303,0.06757082982522743,-0.12475206938673841,0.11278133345284547],"['V']['level']":[-0.0186858761979727,0.019754063305253543],"['V']['log_std']":[-4.776462600750373,-5.339917755800311,-5.354339014714714,-5.353323213520146],"['V']['mean']":[-0.381065537729198,0.2237173550388504,0.28683756291985885,0.4478451868564443],"['process']['log_sigma']":[-2.0352567579779484,-2.109877236830443,-2.5279143727688127]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}},{"step":300,"params":{"['D']['level']":[0.038708712965084,0.07737539969027023,0.0734197706493038,-0.1752964700666617,0.08005499783748844,-0.07228291020123156],"['D']['log_std']":[-4.634751958083117,-4.637288381078209,-4.665301759852405,-5.304616835615291,-5.3651428139129544,-5.369582560583575,-5.357542313410027,-5.052460422855509,-4.6436796038345785,-4.633380507105929,-4.637766110973729,-4.678175258683524,-5.3168081726715535,-5.361801040285785,-5.369204455209087,-5.359016501529902,-5.012452551444681,-4.639346354885614,-4.632791005406245,-4.6360396995100475,-4.686857956373548,-5.322059667362602,-5.3622500372810915,-5.365923928013962],"['D']['mean']":[-0.22798994798404015,-0.36714198695947625,0.2560771118439269,0.832387230187131,0.44563099897282865,0.1723083955714557,-0.7553740269081638,-0.28136304399203826,-0.3833100823717138,-0.41226425935500327,0.09576596345855913,0.035702972763783064,0.3237936145662267,0.7247236019647935,0.28914147704564386,-0.3461057974909275,-0.4436350507101609,-0.3554103798442524,-0.6967681132659774,0.10143201411967978,0.46577258840941066,0.4839582013995307,0.7171430121292062,0.5010524248838919],"['G']['level']":[0.06272416388806025,-0.009525283675024317,0.10959634598348346,-0.10979168630272776,0.04183624802506059,-0.05823105647262285],"['G']['log_std']":[-4.635333323893734,-4.633823163910366,-4.702413252676822,-5.332798680008089,-5.368503769304055,-5.367089206931262,-5.353239673706339,-4.891609001211525,-4.6405613927254885,-4.6348749636503985,-4.638204364112322,-4.698292160536787,-5.32970234411546,-5.3672562158529,-5.368811044560692,-5.351914546162243,-4.853693643949376,-4.639818422075663,-4.633339739001011,-4.639868337816796,-4.713295092385327,-5.3362085831729,-5.363758593024788,-5.369320641975241],"['G']['mean']":[-0.3519323082323487,0.17620069884338777,0.36346176400240154,0.36725168434177247,0.3618726517049043,0.2692140532219254,-0.34143639533614056,-0.3631217352218286,-0.36630857544931505,-0.35524922238334555,0.18819696496701918,0.36439267830448646,0.3685686709860373,0.3626407132278248,0.2675587414372201,-0.3413096032226602,-0.3649795790859307,-0.36369698514361104,-0.35212349040472046,0.21110632682950298,0.36036912701898494,0.3700264021513529,0.36276696046669304,0.2644225394791227],"['Minv']['level']":[-0.027193533890013593,0.030829067715379015,0.0024677321074030216,0.06657965900129997,-0.12410877527882776,0.11381333918542172],"['V']['level']":[-0.01763630134289442,0.020788055674884624],"['V']['log_std']":[-4.768249404328384,-5.351778385382129,-5.366862483631647,-5.3658402938679695],"['V']['mean']":[-0.3929034171266095,0.2318190825793219,0.299221857021,0.46042900632242195],"['process']['log_sigma']":[-2.022842630997477,-2.1020590091675255,-2.53981360048067]},"prodigy_d":{"D_level":0.001,"G_level":0.001,"Minv_level":0.001,"V_level":0.001}}],"provenance":{"source":"LieSPHGP SE2_Vehicle/lie_ph/train.py make_optimizer","optax":"0.2.8"}} diff --git a/src/cvc/tests/ph_se2_train_test.cpp b/src/cvc/tests/ph_se2_train_test.cpp new file mode 100644 index 000000000..394c60aa3 --- /dev/null +++ b/src/cvc/tests/ph_se2_train_test.cpp @@ -0,0 +1,380 @@ +/* + 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::ph's SE(2) trainer (inc/cvc/ph/se2_train.h): the hand-written reverse pass and train.py's +// optimizer. +// +// - The gradient against central differences (no golden needed), and against JAX's autodiff of +// the same objective (rollout loss + beta KL, one fixed draw z, two 21-point held-out windows, +// the Humvee fit) from tests/data/ph_se2_golden.json. +// - The optimizer against optax: train.py's chain on a 4-feature parameter tree for 300 steps of +// gradients given by formula, and Prodigy alone where its d grows +// (tests/data/ph_optim_golden.json). +// - The objective is the same bit for bit on one thread or many, and a short fit to flights of a +// known model lowers the loss. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +using namespace cvc::ph; +namespace fs = std::filesystem; +namespace pt = boost::property_tree; + +fs::path data_dir() { return fs::path(__FILE__).parent_path() / "data"; } + +std::vector numbers(const pt::ptree &node) { + std::vector out; + for (const auto &kv : node) + out.push_back(kv.second.get_value()); + return out; +} + +std::vector flat(const se2_params &p) { + std::vector out; + p.for_each_leaf([&](const char *, const double *d, std::size_t n, se2_leaf_group) { + out.insert(out.end(), d, d + n); + }); + return out; +} + +// A small model with every piece switched on and its levels spread, for the gradient checks. +se2_model small_model(std::uint64_t seed) { + se2_init init; + init.features = 8; + init.initial_randomness = 0.3; + init.initial_control_gp_mean_scale = 1.0; + init.initial_log_std = -2.0; + se2_model m = se2_model::initialize(init, seed, {6.0, 0.5, 0.3}, {1.0, 1.0}, 0.02); + rng r(seed + 99); + for (se2_gp *g : {&m.params.D, &m.params.G, &m.params.V}) + for (auto &x : g->mean) + x = 0.3 * r.normal(); + m.setup.v_length = 5.0; // a potential that bends within a window + return m; +} + +// Windows rolled out from `truth` under smooth inputs, with noise. +se2_windows synthetic_windows(const se2_model &truth, int flights, int rows, double noise, + std::uint64_t seed) { + const se2_vehicle veh(truth.setup, se2_draw(truth), truth.twist_scale); + se2_flights f; + f.count = flights; + f.rows = rows; + f.width = 8; + f.data.resize(static_cast(flights) * rows * 8); + rng r(seed); + for (int fl = 0; fl < flights; ++fl) { + double s[6] = {r.normal(), r.normal(), r.normal(), + 4.0 + r.normal(), 0.1 * r.normal(), 0.1 * r.normal()}; + const double a = 0.5 * r.normal(), b = 0.5 * r.normal(), w = 1.0 + r.uniform(); + for (int k = 0; k < rows; ++k) { + double *row = f.data.data() + (static_cast(fl) * rows + k) * 8; + const double u[2] = {a * std::sin(w * 0.02 * k), b * std::cos(1.7 * w * 0.02 * k)}; + if (k > 0) { + double next[6]; + veh.interval_step(s, u, truth.interval, truth.substeps, nullptr, next); + std::copy(next, next + 6, s); + } + std::copy(s, s + 6, row); + row[6] = k ? u[0] : 0.0; + row[7] = k ? u[1] : 0.0; + } + } + return cut_windows(noise > 0 ? add_observation_noise(f, noise, seed + 1) : f, rows, rows); +} + +// The analytic gradient against central differences, on every level and on a sample of the +// weights and log-scales, with a draw and the KL term on. +TEST(PhSe2Train, GradientMatchesFiniteDifferences) { + const se2_model m0 = small_model(3); + const se2_windows w = synthetic_windows(small_model(4), 3, 16, 0.02, 5); + rng r(6); + const se2_draw_noise z = se2_draw_noise::standard_normal(m0, r); + const double beta_scale = 0.01; + se2_params grad; + const se2_objective base = se2_rollout_objective(m0, w, nullptr, &z, beta_scale, &grad); + ASSERT_EQ(base.healthy, w.count); + const std::vector g = flat(grad); + std::vector names; + std::vector offsets; + { + std::size_t off = 0; + m0.params.for_each_leaf([&](const char *name, const double *, std::size_t n, se2_leaf_group) { + names.emplace_back(name); + offsets.push_back(off); + off += n; + }); + } + int checked = 0; + double worst = 0.0; + std::size_t leaf = 0; + se2_model m = m0; + m.params.for_each_leaf([&](const char *name, double *d, std::size_t n, se2_leaf_group) { + const std::size_t stride = n > 12 ? n / 7 : 1; + for (std::size_t i = 0; i < n; i += stride) { + const double keep = d[i], h = 1e-6 * std::max(1.0, std::fabs(keep)); + d[i] = keep + h; + const double fp = se2_rollout_objective(m, w, nullptr, &z, beta_scale).value; + d[i] = keep - h; + const double fm = se2_rollout_objective(m, w, nullptr, &z, beta_scale).value; + d[i] = keep; + const double fd = (fp - fm) / (2.0 * h); + const double an = g[offsets[leaf] + i]; + const double err = std::fabs(an - fd) / (1e-6 + std::fabs(fd) + std::fabs(an)); + worst = std::max(worst, err); + EXPECT_LT(err, 2e-5) << name << "[" << i << "]: analytic " << an << ", central " << fd; + ++checked; + } + ++leaf; + }); + std::printf(" %d entries against central differences: worst relative %.2e\n", checked, worst); +} + +// JAX's autodiff of the same objective, on the Humvee fit and two held-out windows. +TEST(PhSe2Train, GradientMatchesJax) { + const fs::path path = data_dir() / "ph_se2_golden.json"; + pt::ptree doc; + pt::read_json(path.string(), doc); + const se2_model m = se2_model::load(path.string()); + const pt::ptree &gnode = doc.get_child("gradient"); + const int points = gnode.get("window_points"); + const int count = gnode.get("window_count"); + // the golden's windows are time-major (points x count x 8) + const std::vector tm = numbers(gnode.get_child("windows")); + se2_windows w; + w.points = points; + w.width = 8; + w.count = count; + w.data.resize(tm.size()); + for (int t = 0; t < points; ++t) + for (int b = 0; b < count; ++b) + for (int c = 0; c < 8; ++c) + w.data[(static_cast(b) * points + t) * 8 + c] = tm[(t * count + b) * 8 + c]; + se2_draw_noise z; + z.zD = numbers(gnode.get_child("z.D")); + z.zG = numbers(gnode.get_child("z.G")); + z.zV = numbers(gnode.get_child("z.V")); + const double beta_scale = gnode.get("beta_scale"); + se2_params grad; + const se2_objective obj = se2_rollout_objective(m, w, nullptr, &z, beta_scale, &grad); + EXPECT_NEAR(obj.rollout, gnode.get("rollout_loss"), 1e-13); + EXPECT_NEAR(obj.kl, gnode.get("kl"), 1e-9); + EXPECT_NEAR(obj.value, gnode.get("objective"), 1e-13); + const std::map key = {{"D.level", "D.level"}, + {"D.log_std", "D.log_std"}, + {"D.mean", "D.mean"}, + {"G.level", "G.level"}, + {"G.log_std", "G.log_std"}, + {"G.mean", "G.mean"}, + {"Minv.level", "Minv.level"}, + {"V.level", "V.level"}, + {"V.log_std", "V.log_std"}, + {"V.mean", "V.mean"}, + {"likelihood.log_sigma", "likelihood.log_sigma"}, + {"process.log_sigma", "process.log_sigma"}}; + double worst = 0.0; + grad.for_each_leaf([&](const char *name, const double *d, std::size_t n, se2_leaf_group) { + const std::vector want = numbers(gnode.get_child("grad." + key.at(name))); + ASSERT_EQ(want.size(), n) << name; + double scale = 0.0; + for (double x : want) + scale = std::max(scale, std::fabs(x)); + for (std::size_t i = 0; i < n; ++i) { + const double err = std::fabs(d[i] - want[i]) / (1e-12 + scale); + worst = std::max(worst, err); + EXPECT_LT(err, 1e-10) << name << "[" << i << "]: C++ " << d[i] << ", JAX " << want[i]; + } + }); + std::printf(" 1,691 entries against JAX: worst |d| / max|leaf| %.2e\n", worst); +} + +// Every leaf of a 4-feature parameter tree after 300 steps of train.py's optimizer. +TEST(PhSe2Train, OptimizerMatchesOptax) { + pt::ptree doc; + pt::read_json((data_dir() / "ph_optim_golden.json").string(), doc); + se2_init init; + init.features = 4; + se2_model m = se2_model::initialize(init, 1, {1.0, 1.0, 1.0}, {1.0, 1.0}, 0.02); + const auto golden_name = [](const std::string &name) { + const auto dot = name.find('.'); + return "['" + name.substr(0, dot) + "']['" + name.substr(dot + 1) + "']"; + }; + m.params.for_each_leaf([&](const char *name, double *d, std::size_t n, se2_leaf_group) { + const auto v = + numbers(doc.get_child("params0").get_child(pt::ptree::path_type(golden_name(name), '|'))); + ASSERT_EQ(v.size(), n) << name; + std::copy(v.begin(), v.end(), d); + }); + se2_optimizer_config cfg; + cfg.total_steps = doc.get("config.training.total_steps"); + cfg.learning_rate = doc.get("config.optimizer.learning_rate"); + cfg.final_learning_fraction = doc.get("config.optimizer.final_learning_fraction"); + cfg.level_learning_rate = doc.get("config.optimizer.level_learning_rate"); + cfg.gradient_clip_norm = doc.get("config.optimizer.gradient_clip_norm"); + se2_optimizer opt(m.params, cfg); + const std::vector scales = numbers(doc.get_child("gradient_rule.scales")); + const int steps = doc.get("gradient_rule.steps"); + auto record = doc.get_child("steps").begin(); + int compared = 0; + double worst = 0.0; + for (int t = 0; t < steps; ++t) { + se2_params g = m.params; + std::size_t index = 0; + g.for_each_leaf([&](const char *name, double *d, std::size_t n, se2_leaf_group) { + for (std::size_t i = 0; i < n; ++i, ++index) { + const double x = static_cast(index); + d[i] = scales[t % scales.size()] * + (std::sin(0.7 * x + 0.3) + 0.3 * std::cos(1.3 * x + 0.9 * t)); + if (t == 3 && std::string(name) == "process.log_sigma") + d[i] = 0.0; + } + }); + opt.update(m.params, g); + if (record != doc.get_child("steps").end() && record->second.get("step") == t + 1) { + const pt::ptree &rec = record->second; + m.params.for_each_leaf([&](const char *name, const double *d, std::size_t n, se2_leaf_group) { + const auto want = numbers( + rec.get_child("params").get_child(pt::ptree::path_type(golden_name(name), '|'))); + for (std::size_t i = 0; i < n; ++i) { + worst = std::max(worst, std::fabs(d[i] - want[i]) / (1.0 + std::fabs(want[i]))); + EXPECT_NEAR(d[i], want[i], 1e-12 * (1.0 + std::fabs(want[i]))) + << "step " << t + 1 << " " << name << "[" << i << "]"; + } + }); + for (auto [group, label] : {std::pair{se2_leaf_group::D_level, "D_level"}, + {se2_leaf_group::G_level, "G_level"}, + {se2_leaf_group::V_level, "V_level"}, + {se2_leaf_group::Minv_level, "Minv_level"}}) + EXPECT_NEAR(opt.prodigy_d(group), rec.get(std::string("prodigy_d.") + label), 1e-15) + << "step " << t + 1 << " " << label; + ++compared; + ++record; + } + } + EXPECT_EQ(compared, 13); + std::printf(" 300 optimizer steps against optax: worst %.2e at %d checkpoints\n", worst, + compared); +} + +// Prodigy alone, through the steps where its d grows from 1e-3 to about 2. +TEST(PhSe2Train, ProdigyMatchesOptax) { + pt::ptree doc; + pt::read_json((data_dir() / "ph_optim_golden.json").string(), doc); + const pt::ptree &g = doc.get_child("prodigy_alone"); + std::vector p = numbers(g.get_child("params0")); + prodigy opt; + opt.d0 = g.get("estim_lr0"); + opt.eps = g.get("eps"); + opt.safeguard_warmup = true; + opt.init(p.data(), p.size()); + auto record = g.get_child("steps").begin(); + for (int t = 0; t < 300; ++t) { + std::vector grad(p.size()); + for (std::size_t i = 0; i < p.size(); ++i) + grad[i] = std::sin(0.7 * i + 0.3) + 0.3 * std::cos(1.3 * i + 0.9 * t); + opt.update(p.data(), grad.data(), p.size(), g.get("learning_rate")); + if (record != g.get_child("steps").end() && record->second.get("step") == t + 1) { + const double want_d = record->second.get("d"); + EXPECT_NEAR(opt.d, want_d, 1e-11 * want_d) << "step " << t + 1; + const auto want = numbers(record->second.get_child("params")); + for (std::size_t i = 0; i < p.size(); ++i) + EXPECT_NEAR(p[i], want[i], 1e-10 * (1.0 + std::fabs(want[i]))) << "step " << t + 1; + ++record; + } + } + EXPECT_GT(opt.d, 1.0); // it grew +} + +TEST(PhSe2Train, ObjectiveIsTheSameOnAnyThreadCount) { + const se2_model m = small_model(8); + const se2_windows w = synthetic_windows(small_model(9), 12, 21, 0.02, 10); + rng r(11); + const se2_draw_noise z = se2_draw_noise::standard_normal(m, r); + se2_params g1, g8; + const auto o1 = se2_rollout_objective(m, w, nullptr, &z, 0.01, &g1); + cvc::thread_pool pool(7); + const auto o8 = se2_rollout_objective(m, w, nullptr, &z, 0.01, &g8, &pool); + EXPECT_EQ(o1.value, o8.value); + EXPECT_EQ(flat(g1), flat(g8)); +} + +// A fresh model fit for 1,200 steps to flights of another one: the rollout loss falls well below +// where it started, on windows it never trained on as well. +TEST(PhSe2Train, ShortFitLowersTheLoss) { + const se2_model truth = small_model(12); + const se2_windows train = synthetic_windows(truth, 40, 26, 0.01, 13); + const se2_windows test = synthetic_windows(truth, 10, 26, 0.01, 14); + se2_init init; + init.features = 8; + se2_model m = se2_model::initialize(init, 15, truth.twist_scale, {1.0, 1.0}, truth.interval); + m.setup.v_length = truth.setup.v_length; + const double before = se2_rollout_objective(m, test, nullptr, nullptr, 0.0).rollout; + se2_train_config cfg; + cfg.seed = 16; + cfg.total_steps = 1200; + cfg.batch_size = 8; + cfg.kl_anneal_steps = 100; + cfg.eval_every = 400; + cfg.eval_windows = 10; + cfg.threads = 4; + int events = 0, evaluated = 0; + double last_test = 0.0; + se2_train_rollout(m, train, &test, cfg, [&](const se2_train_event &ev) { + ++events; + if (ev.evaluated) { + ++evaluated; + last_test = ev.test_loss; + } + }); + EXPECT_EQ(events, 1200); + EXPECT_EQ(evaluated, 3); + const double after = se2_rollout_objective(m, test, nullptr, nullptr, 0.0).rollout; + EXPECT_DOUBLE_EQ(after, last_test); + std::printf(" held-out rollout loss %.4f -> %.4f\n", before, after); + EXPECT_LT(after, 0.1 * before); + // and the fitted model survives its file + const se2_model back = se2_model::from_json(m.to_json()); + EXPECT_EQ(se2_rollout_objective(back, test, nullptr, nullptr, 0.0).rollout, after); +} + +TEST(PhSe2Train, CutsWindowsAsLieSphgpDoes) { + se2_flights f; + f.count = 2; + f.rows = 250; + f.width = 8; + f.data.resize(2 * 250 * 8); + for (std::size_t i = 0; i < f.data.size(); ++i) + f.data[i] = static_cast(i); + const se2_windows w = cut_windows(f, 101, 75); // starts 0 and 75 + ASSERT_EQ(w.count, 4u); + EXPECT_EQ(w.window(1)[0], 75.0 * 8); + EXPECT_EQ(w.window(2)[0], 250.0 * 8); + EXPECT_EQ(w.window(3)[100 * 8 + 7], (250.0 + 75 + 100) * 8 + 7); + const auto t = twist_rms(f); + EXPECT_GT(t[0], 0.0); + const auto c = control_rms(f); + ASSERT_EQ(c.size(), 2u); + const se2_flights s = scale_controls(f, c); + EXPECT_NEAR(control_rms(s)[0], 1.0, 1e-12); +} + +} // namespace From f4acafdff2c481fc2b40c84adf94e9006bedc7f5 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 22:49:00 -0500 Subject: [PATCH 10/12] ph-train: report the training windows whose rollout diverged; CLI tests A window whose rollout leaves the finite numbers is left out of the loss and the gradient. JAX's rollout_loss masks it out of the loss but its gradient still carries the NaN, so train.py aborts there; here it is dropped, and ph-train now says so for each step and records the total in the model's provenance (diverged_windows). Tests: ADivergingWindowIsLeftOut (the loss and gradient equal the healthy windows' alone); cvc_cli_test's PhTrainFitsAModelToPlantLogs (a plant-log directory written by the test, a 20-step fit, the model loads with its provenance) and PhTrainRefusesADirectoryThatIsNotAPlantLog. --- src/cvc-cli/cvc_cli_test.cpp | 61 +++++++++++++++++++++++++++++ src/cvc-cli/main.cpp | 11 +++++- src/cvc/tests/ph_se2_train_test.cpp | 22 +++++++++++ 3 files changed, 93 insertions(+), 1 deletion(-) diff --git a/src/cvc-cli/cvc_cli_test.cpp b/src/cvc-cli/cvc_cli_test.cpp index b8508544d..35cd36760 100644 --- a/src/cvc-cli/cvc_cli_test.cpp +++ b/src/cvc-cli/cvc_cli_test.cpp @@ -23,11 +23,13 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -2286,3 +2288,62 @@ TEST_F(CvcCliTest, Regression_ClusterStatusDefaultIdsEndToEnd) { EXPECT_NE(std::string::npos, r.output.find("Session status")) << r.output; } #endif // !_WIN32 + +// =========================================================================== +// ph-train: the learned port-Hamiltonian vehicle (cvc::ph) from plant logs +// =========================================================================== + +// A cvc-plant-log run directory: six flights of a vehicle on a gentle curve, seeds 100 and 101. +static void write_plant_log_run(const fs::path &dir) { + fs::create_directories(dir); + const std::size_t flights = 6, rows = 61; + std::vector data(flights * rows * 8); + for (std::size_t f = 0; f < flights; ++f) { + double x = 0.0, y = 0.0, psi = 0.1 * f; + const double v = 3.0 + f, r = 0.05 * (f % 3); + for (std::size_t k = 0; k < rows; ++k) { + double *row = &data[(f * rows + k) * 8]; + row[0] = x; + row[1] = y; + row[2] = psi; + row[3] = v; + row[4] = 0.01; + row[5] = r; + row[6] = 0.2; + row[7] = 0.02 * (f % 3); + x += 0.02 * v * std::cos(psi); + y += 0.02 * v * std::sin(psi); + psi += 0.02 * r; + } + } + cvc::ph::write_npy((dir / "flights.npy").string(), data, {flights, rows, 8}); + std::ofstream(dir / "meta.json") << "{\"format\": \"cvc-plant-log-1\", \"dt\": 0.02, " + "\"flight_seed\": [100, 100, 100, 101, 101, 101]}\n"; +} + +TEST_F(CvcCliTest, PhTrainFitsAModelToPlantLogs) { + const fs::path run = fs::path(test_dir) / "run"; + write_plant_log_run(run); + const std::string out = path("model.json"); + auto r = cvc("ph-train --runs \"" + run.string() + "\" --out \"" + out + + "\" --steps 20 --window-points 21 --window-stride 20 --features 8 --eval-every 10" + " --threads 2"); + ASSERT_EQ(0, r.exit_code) << r.output; + EXPECT_NE(std::string::npos, r.output.find("3 train / 3 held out")) << r.output; + EXPECT_NE(std::string::npos, r.output.find("MONITOR test")) << r.output; + ASSERT_TRUE(fs::exists(out)); + const cvc::ph::se2_model m = cvc::ph::se2_model::load(out); + EXPECT_EQ(m.setup.features, 8); + EXPECT_EQ(m.setup.control_dim, 2); + bool has_runs = false; + for (const auto &kv : m.provenance) + has_runs |= kv.first == "runs" && kv.second == run.string(); + EXPECT_TRUE(has_runs); +} + +TEST_F(CvcCliTest, PhTrainRefusesADirectoryThatIsNotAPlantLog) { + fs::create_directories(path("empty")); + auto r = cvc("ph-train --runs \"" + path("empty") + "\" --out \"" + path("m.json") + "\""); + EXPECT_NE(0, r.exit_code); + EXPECT_FALSE(fs::exists(path("m.json"))); +} diff --git a/src/cvc-cli/main.cpp b/src/cvc-cli/main.cpp index 7049a16fb..3df18fbb1 100644 --- a/src/cvc-cli/main.cpp +++ b/src/cvc-cli/main.cpp @@ -2265,8 +2265,15 @@ static int cmd_ph_train(int argc, char **argv) { std::cout << " " << c; std::cout << "; parameters " << model.params.size() << "\n"; const auto started = std::chrono::steady_clock::now(); + const std::size_t batch = std::min(std::max(cfg.batch_size, 1), train.count); + std::size_t dropped = 0; // training windows whose rollout left the finite numbers ph::se2_train_rollout( model, train, test.count ? &test : nullptr, cfg, [&](const ph::se2_train_event &ev) { + if (ev.objective.healthy < batch) { + dropped += batch - ev.objective.healthy; + std::cout << "step " << ev.step << ": " << batch - ev.objective.healthy + << " window(s) diverged; left out of the loss and the gradient\n"; + } if (ev.step % log_every && ev.step != 1 && !ev.evaluated) return; const double s = @@ -2296,8 +2303,10 @@ static int cmd_ph_train(int argc, char **argv) { {"batch", std::to_string(cfg.batch_size)}, {"window_points", std::to_string(points)}, {"window_stride", std::to_string(stride)}}; + model.provenance.emplace_back("diverged_windows", std::to_string(dropped)); model.save(vm["out"].as()); - std::cout << "wrote " << vm["out"].as() << "\n"; + std::cout << "wrote " << vm["out"].as() << " (" << dropped + << " diverged training windows left out)\n"; return 0; } diff --git a/src/cvc/tests/ph_se2_train_test.cpp b/src/cvc/tests/ph_se2_train_test.cpp index 394c60aa3..321a9c73a 100644 --- a/src/cvc/tests/ph_se2_train_test.cpp +++ b/src/cvc/tests/ph_se2_train_test.cpp @@ -317,6 +317,28 @@ TEST(PhSe2Train, ObjectiveIsTheSameOnAnyThreadCount) { EXPECT_EQ(flat(g1), flat(g8)); } +// A window whose rollout leaves the finite numbers is left out of the loss and of the gradient +// (LieSPHGP's rollout_loss means to drop it too, but its gradient still carries the NaN, and +// train.py aborts). +TEST(PhSe2Train, ADivergingWindowIsLeftOut) { + const se2_model m = small_model(17); + const se2_windows healthy = synthetic_windows(small_model(18), 3, 16, 0.02, 19); + se2_windows with_bad = healthy; + with_bad.count += 1; + with_bad.data.insert(with_bad.data.end(), healthy.window(0), + healthy.window(0) + healthy.points * healthy.width); + with_bad.data[3 * healthy.points * healthy.width + 3] = 1e300; // v_x at its start + rng r(20); + const se2_draw_noise z = se2_draw_noise::standard_normal(m, r); + se2_params g_ok, g_bad; + const auto ok = se2_rollout_objective(m, healthy, nullptr, &z, 0.01, &g_ok); + const auto bad = se2_rollout_objective(m, with_bad, nullptr, &z, 0.01, &g_bad); + EXPECT_EQ(ok.healthy, 3u); + EXPECT_EQ(bad.healthy, 3u); + EXPECT_EQ(bad.value, ok.value); + EXPECT_EQ(flat(g_bad), flat(g_ok)); +} + // A fresh model fit for 1,200 steps to flights of another one: the rollout loss falls well below // where it started, on windows it never trained on as well. TEST(PhSe2Train, ShortFitLowersTheLoss) { From dec85a4434628d702b162afc0efe57ad92f2eebd Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 23:07:29 -0500 Subject: [PATCH 11/12] cvc-cli: ph-train beside plant-log in the Vehicle Physics group --- src/cvc-cli/main.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/cvc-cli/main.cpp b/src/cvc-cli/main.cpp index 3df18fbb1..035aa7c44 100644 --- a/src/cvc-cli/main.cpp +++ b/src/cvc-cli/main.cpp @@ -2380,9 +2380,6 @@ static const command_entry commands[] = { {"img2vol", "Image I/O", "import image stack as volume", cmd_img2vol}, {"rgba-merge", "Image I/O", "merge 4 volumes into RGBA", cmd_rgba_merge}, - // ── Learned dynamics ── - {"ph-train", "Learned Dynamics","fit the port-Hamiltonian vehicle to plant logs", cmd_ph_train}, - // ── Test data ── {"bunny", "Test Data", "output Stanford bunny geometry or SDF", cmd_bunny}, @@ -2395,6 +2392,7 @@ static const command_entry commands[] = { // ── Vehicle physics ── {"plant-log", "Vehicle Physics", "log nav driving Jolt vehicles for plant fitting", cmd_plant_log}, + {"ph-train", "Vehicle Physics", "fit the learned port-Hamiltonian vehicle to plant logs", cmd_ph_train}, // ── Network (legacy XMLRPC) ── #ifdef USING_XMLRPC From a4bbae47c56d2f2c593ff729ac72d3abf61f8204 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 23:36:06 -0500 Subject: [PATCH 12/12] ph-train --init: continue a model with its own input and twist scales A continued model reads its inputs divided by the RMS it was fit with, not the new runs'. PhTrainContinuesAModelWithItsOwnScales fails without this (the run logged and used the runs' scales). --- src/cvc-cli/cvc_cli_test.cpp | 26 ++++++++++++++++++++++++++ src/cvc-cli/main.cpp | 21 +++++++++++++-------- 2 files changed, 39 insertions(+), 8 deletions(-) diff --git a/src/cvc-cli/cvc_cli_test.cpp b/src/cvc-cli/cvc_cli_test.cpp index 35cd36760..9b47550f7 100644 --- a/src/cvc-cli/cvc_cli_test.cpp +++ b/src/cvc-cli/cvc_cli_test.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -2341,6 +2342,31 @@ TEST_F(CvcCliTest, PhTrainFitsAModelToPlantLogs) { EXPECT_TRUE(has_runs); } +// --init continues a model with its own scales, not the new runs' RMS. +TEST_F(CvcCliTest, PhTrainContinuesAModelWithItsOwnScales) { + const fs::path run = fs::path(test_dir) / "run"; + write_plant_log_run(run); + const std::string first = path("first.json"), second = path("second.json"); + const std::string common = + " --steps 5 --window-points 21 --window-stride 20 --features 8 --threads 1"; + ASSERT_EQ( + 0, + cvc("ph-train --runs \"" + run.string() + "\" --out \"" + first + "\"" + common).exit_code); + cvc::ph::se2_model m = cvc::ph::se2_model::load(first); + m.control_scale = {2.0 * m.control_scale[0], 3.0 * m.control_scale[1]}; + m.save(first); + auto r = cvc("ph-train --runs \"" + run.string() + "\" --out \"" + second + "\" --init \"" + + first + "\"" + common); + ASSERT_EQ(0, r.exit_code) << r.output; + std::ostringstream scales; // the inputs were read with the model's scales (the log says which) + scales << std::setprecision(6) << "control scale " << m.control_scale[0] << " " + << m.control_scale[1]; + EXPECT_NE(std::string::npos, r.output.find(scales.str())) << r.output; + const cvc::ph::se2_model back = cvc::ph::se2_model::load(second); + EXPECT_EQ(back.control_scale, m.control_scale); + EXPECT_EQ(back.twist_scale, m.twist_scale); +} + TEST_F(CvcCliTest, PhTrainRefusesADirectoryThatIsNotAPlantLog) { fs::create_directories(path("empty")); auto r = cvc("ph-train --runs \"" + path("empty") + "\" --out \"" + path("m.json") + "\""); diff --git a/src/cvc-cli/main.cpp b/src/cvc-cli/main.cpp index 035aa7c44..bfbdc9d0d 100644 --- a/src/cvc-cli/main.cpp +++ b/src/cvc-cli/main.cpp @@ -2224,14 +2224,13 @@ static int cmd_ph_train(int argc, char **argv) { const int points = vm["window-points"].as(); const int stride = vm["window-stride"].as(); - const ph::se2_windows train = - ph::cut_windows(ph::scale_controls(train_f, control), points, stride); - const ph::se2_windows test = - test_f.count ? ph::cut_windows(ph::scale_controls(test_f, control), points, stride) - : ph::se2_windows{}; ph::se2_model model; if (vm.count("init")) { + // A model continues with its own scales: the inputs are divided by the RMS it was fit with. model = ph::se2_model::load(vm["init"].as()); + if (model.setup.control_dim != all.control_dim()) + throw std::runtime_error("--init's model has " + std::to_string(model.setup.control_dim) + + " inputs; the runs have " + std::to_string(all.control_dim())); } else { ph::se2_init init; init.control_dim = all.control_dim(); @@ -2241,6 +2240,12 @@ static int cmd_ph_train(int argc, char **argv) { model = ph::se2_model::initialize(init, vm["seed"].as(), twist, control, all.dt, vm["substeps"].as()); } + const ph::se2_windows train = + ph::cut_windows(ph::scale_controls(train_f, model.control_scale), points, stride); + const ph::se2_windows test = + test_f.count + ? ph::cut_windows(ph::scale_controls(test_f, model.control_scale), points, stride) + : ph::se2_windows{}; ph::se2_train_config cfg; cfg.seed = vm["seed"].as(); cfg.total_steps = vm["steps"].as(); @@ -2259,9 +2264,9 @@ static int cmd_ph_train(int argc, char **argv) { std::cout << std::setprecision(6) << "ph-train: " << all.count << " flights x " << all.rows << " rows at dt " << all.dt << " s; " << train_f.count << " train / " << test_f.count << " held out; windows " << train.count << " / " << test.count << " of " << points - << " points\n twist scale " << twist[0] << " " << twist[1] << " " << twist[2] - << "; control scale"; - for (double c : control) + << " points\n twist scale " << model.twist_scale[0] << " " << model.twist_scale[1] + << " " << model.twist_scale[2] << "; control scale"; + for (double c : model.control_scale) std::cout << " " << c; std::cout << "; parameters " << model.params.size() << "\n"; const auto started = std::chrono::steady_clock::now();