diff --git a/docs/NAV_TRAINING.md b/docs/NAV_TRAINING.md index 65d1d095..bbfd1dc7 100644 --- a/docs/NAV_TRAINING.md +++ b/docs/NAV_TRAINING.md @@ -19,6 +19,7 @@ deploys on and dropped straight into the swarm. - [What gets trained](#what-gets-trained) - [The rollout switch: surrogate vs bicycle](#the-rollout-switch-surrogate-vs-bicycle) - [The dynamic vehicle: `dyn_bike_step` (P8a)](#the-dynamic-vehicle-dyn_bike_step-p8a) +- [The four-wheel vehicle: `dyn4_plant` (P8d)](#the-four-wheel-vehicle-dyn4_plant-p8d) - [Scene source](#scene-source) - [CPU vs CUDA (device-resident)](#cpu-vs-cuda-device-resident) - [Exporting weights](#exporting-weights) @@ -166,13 +167,15 @@ derives them from a `vehicle_desc`, so both plants read one vehicle: - mass, and a yaw inertia from the chassis box; - the axles relative to the centre of gravity; -- Pacejka B and C from the tire's peak slip angle and sliding fraction; -- the drive and brake splits, drag and rolling resistance; -- the steering's rate. +- Pacejka C from the tire's sliding fraction, and B fitted to how Jolt's tires corner: B C = + `kJoltTireStiffness` (1.12) / the peak slip angle (#753); +- the drive and brake splits, drag and rolling resistance (none for a direct drive, whose driver + model's trim makes up for them); +- the steering's rate, and the drive's lag (0 for a direct drive, 0.1 s through a converter). -`front_stiffness` (0.85) scales the front's B. It stands in for the load sensitivity and steering -compliance that give a road vehicle its understeer, which a single track has no other way to -show. +`front_stiffness` (1) scales the front's B. A road vehicle's load sensitivity and steering +compliance would put it below 1. Jolt's tires have neither, and the fit to Jolt puts front and +rear alike. Gates (`nav_dyn_bike_test`; the Humvee from its `vehicle_desc`, in metres): @@ -349,6 +352,82 @@ Not yet: turns on only the bicycle); - a CUDA build of the plant (it is `CVC_HD`, but nothing compiles it for the device yet). +## The four-wheel vehicle: `dyn4_plant` (P8d) + +`diff::dyn4_plant` (`inc/cvc/nav/detail/dyn_4w.h`) is P8a's single-track vehicle on its four +contact patches. `bike_control` commands it as it commands `dyn_plant`, so a policy trains +through per-wheel grip, the terrain under each wheel, and the load +each corner carries. + +```cpp +#include +cvc::nav::diff::dyn4_params p = cvc::physics::nav_plant::dyn4_params_at(vehicle_desc::roboracer(), cfg.veh_L); +my_ground ground; // at(wx, wy, mu, height) at a world point, templated on the scalar +cvc::nav::diff::dyn4_bike_step(F, s, gx, gy, al, be, ga, v, p, ground, s1); +cvc::nav::diff::dyn4_bike_step_bwd(F, s, gx, gy, al, be, ga, v, p, ground, g_s1, g_s, gal, gbe, gga); +``` + +**State** (14 values): P8a's `(x, y, psi, vx, vy, r)`; the body's heave `z`, roll `phi` and pitch +`th` from its rest height on flat ground, with their rates; and the steering `delta` and drive +force `a` behind their actuators. + +**One step**: + +- **Wheels.** Front left, front right, rear left and rear right sit at (lf, ±tf) and (-lr, ±tr) + from the centre of gravity. The front pair takes Ackermann angles for the virtual centre wheel: + tan delta_i = L tan delta / (L - y_i tan delta). +- **Ground.** Each wheel reads its friction and terrain height from the ground sampler at its + world position. The plant is templated on the sampler, so the terrain's gradient flows through + the dual numbers too. +- **Loads.** Each corner's spring-damper, F = Fz0 + k (height - corner height) - c (corner rate), + is that tire's load, floored by a softplus so a tire pushes but never pulls. +- **Tires.** Each wheel's drive or brake share saturates at mu_i Fz_i, and Pacejka gives the + lateral force within the friction ellipse's remainder. (vy, r) advance by implicit Euler, each + tire linearized by its secant through its root (P8a's solve, four tires). +- **Body.** (z, phi, th) advance by implicit Euler on the corners' springs and dampers, driven by + the ground and the tire forces' moments about the centre of gravity. +- **The rest is P8a's:** the speed prediction and correction, the low-speed blend and the pose. +- **Adjoint.** As P8a: the plant is written over a scalar type, and `dyn4_plant_vjp` runs it on + forward-mode duals. + +**Parameters** from the spec sheet: `nav_plant::dyn4_si_of(vehicle_desc)`. +- P8a's numbers, plus each axle's half track. +- The chassis box's roll and pitch inertias. +- Each corner's spring and damper, from its axle's suspension frequency and damping ratio at the + corner's static load. +- `dyn4_params_of` converts them by length exponent. +- `dyn4_params_at(desc, wheelbase)` gives the trainer's normalized vehicle, as P8a does. + +**Gates** (`nav_dyn_4w_test`, the Humvee from its vehicle_desc unless noted; the tires fitted to +Jolt's since #753): + +| Gate | Result | +|---|---| +| At rest on flat ground, 200 steps | stays put; the loads sum to m g | +| Single-track limit (zero track, rigid corners) against P8a's `dyn_plant`: steer, throttle, then brake from 15 m/s | positions within 0.130 m over 93 m; velocities within 0.016 m/s | +| A steady turn: the roll moment the springs carry against h x the lateral force | -5118 against -5116 N m; the outside wheels loaded | +| Steady braking at 3 m/s^2: the front axle against the quasi-static m (g lr - a h) / L | 15820 against 15820 N | +| Braking with the left wheels on mu 0.3 and the right on 0.9 | yaws toward the high-mu side (-0.89 rad in 1.8 s) | +| An 8 cm bump under the left wheels | front left peaks at 1.37 x its static load, front right 1.11 x | +| The plant's VJP against central differences (double; cruise, corner, braking in a turn, crawl, a bump) | worst relative 2.6e-9 | +| One step under the controller (a RoboRacer in the trainer's city, float) | directional relative error 4.0e-5 | +| Steady lateral acceleration (the body's, vy' + r vx), mu 0.4 / 0.9 | 0.94 / 0.89 mu g | +| Stopping from 20 m/s, mu 0.4 / 0.9 | 47.2 / 21.5 m against v^2 / (2 mu g) = 51.0 / 22.7 m | +| Against Jolt's Humvee (physics_vehicle_test's numbers) | skidpad 6.30 m/s^2 at mu 0.7 (Jolt 6.85); stopping 24.10 m from 20 m/s at mu 0.8 (Jolt 25.90) | +| Ackermann, as Jolt's gate: cot(right) - cot(left) against track / wheelbase | exact (1e-12) for delta from -0.3 to 0.45 | +| Held at delta at 3 m/s, the low-speed blend off: the rear axle's circle against L / tan(delta) | -0.03 / +0.18 / +0.62% at 0.15 / 0.30 / 0.45 rad | +| Golden for the torch twin (`tests/data/dyn4_golden.json`; the sprung and rigid Humvee on a mu split with a bump): next state and VJP | reproduced to 1e-9 | + +A gradient check's starting positions should not be the scene's sampled starts. Those sit on +occupancy-cell centres, which are the SDF field's grid nodes, where the bilinear field has a +kink: a central difference there averages two cells' slopes. Jittered off the nodes, the +controller check above went from 5e-2 to 6.2e-5. + +Not yet: +- the external-force port on this plant (with transfix/libcvc#728); +- the GRL-SNAM torch twin, held to the golden above; +- the anti-roll bars, the unsprung masses and wheel spin. + ## Scene source A `training_scene` is a static occupancy world + the vehicle/integration meta + diff --git a/inc/cvc/nav/detail/dyn_4w.h b/inc/cvc/nav/detail/dyn_4w.h new file mode 100644 index 00000000..4496d6fb --- /dev/null +++ b/inc/cvc/nav/detail/dyn_4w.h @@ -0,0 +1,409 @@ +/* + 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. +*/ + +// dyn_4w.h: the differentiable two-track (four-wheel) vehicle with suspension, the +// vehicle-physics plan's P8d. It extends P8a's single-track plant (dyn_bike.h) to the four +// contact patches a real car has, so a policy trains through +// per-wheel grip, terrain under each wheel and the load each corner carries. bike_control +// commands it exactly as it commands dyn_plant. +// +// State, in nav's normalized frame (lengths x S, time in s, mass in kg): +// (x, y) the rear axle's midpoint, as bike_step's o; psi the heading; +// vx, vy the body-frame velocity at the centre of gravity; r the yaw rate; +// z, zd the body's heave from its rest height on flat ground, and its rate; +// phi, phid its roll (+ lifts the left side), and rate; th, thd its pitch (+ lowers the +// nose), and rate; +// delta the virtual centre wheel's steering angle; a the drive and brake force per unit +// mass, both behind their actuators (as P8a). +// +// The wheels sit at body-frame (x_i, y_i) from the centre of gravity: front left (lf, tf), front +// right (lf, -tf), rear left (-lr, tr), rear right (-lr, -tr). Corner i's height above its rest +// is c_i . (z, phi, th) with c_i = (1, y_i, -x_i). +// +// One step of hdt, given (delta_cmd, a_cmd) and a ground sampler G (G.at(wx, wy, mu, height) at a +// world point; the plant is templated on it, so its height's gradient flows through the duals): +// 1. Actuators, as P8a. +// 2. Loads. Each corner's spring-damper: F_i = Fz0_i + k_i (height_i - c_i . q) - c_i' (c_i . +// qd), +// Fz0_i the static share of m g, floored smoothly at zero (softplus of width fz_min m g): a +// tire pushes, never pulls. From the current vertical state, so load transfer lags the tire +// forces by one step. +// 3. Longitudinal. The drive or brake force splits between the axles (front_drive / +// front_brake), then evenly left and right; each wheel saturates smoothly at mu_i Fz_i and +// leaves the friction ellipse's remainder for cornering (P8a's dyn_sat, dyn_lateral_cap). +// The speed is predicted from these and the resistances, then corrected after step 4. +// 4. Lateral and yaw. Each front wheel's angle is the Ackermann angle for the virtual one: +// tan delta_i = L tan delta / (L - y_i tan delta). A wheel's lateral velocity in its own +// frame is w_i = -(vx - r y_i) sin delta_i + (vy + r x_i) cos delta_i, its slip +// alpha_i = sgn(u_i) ... as P8a, and its force Pacejka's. (vy, r) advance by implicit Euler, +// each tire linearized by its secant through its root in w_i (dyn_secant_w). +// 5. Vertical. (z, phi, th) advance by implicit Euler on the linear springs and dampers, driven +// by the ground under each wheel and the tire forces' moments about the centre of gravity +// (h above the ground): roll + h FY, pitch - h FX. A 3x3 solve. +// 6. Low speed, as P8a: (vy, r) blend toward the kinematic bicycle's. +// 7. Pose, as P8a: the rear axle moves with (vx1, vy1 - lr r1) at psi1. +// With zero tracks, rigid corners and flat ground it reduces to P8a's vehicle, with the +// longitudinal load transfer from the pitch springs rather than quasi-statically (the gates +// check the two agree). +// +// Left out: wheel spin, the unsprung masses, camber and the anti-roll bars. + +#ifndef __CVC_NAV_DETAIL_DYN_4W_H__ +#define __CVC_NAV_DETAIL_DYN_4W_H__ + +#include + +namespace cvc { +namespace nav { +namespace diff { + +struct dyn4_params { + float m = 1.0f; // kg + float Iz = 1.0f; // kg x length^2: yaw inertia about the centre of gravity + float Ix = 1.0f; // roll inertia + float Iy = 1.0f; // pitch inertia + float lf = 0.5f; // length: centre of gravity to the front axle + float lr = 0.5f; // length: to the rear axle + float tf = 0.0f; // length: half the front track + float tr = 0.0f; // length: half the rear track + float h = 0.0f; // length: the centre of gravity's height above the ground at rest + float g = 9.81f; // length / s^2 + float k_f = 0.0f; // kg / s^2: a front corner's spring rate; 0 = rigid (very stiff) + float k_r = 0.0f; // a rear corner's + float c_f = 0.0f; // kg / s: a front corner's damping; 0 = critical for its spring + float c_r = 0.0f; // a rear corner's + float B_f = 12.0f, B_r = 12.0f, C = 1.2f; // Pacejka, as dyn_bike_params + float front_drive = 0.0f, front_brake = 0.6f; + float drag = 0.0f, c_rr = 0.0f; + float tau_steer = 0.0f, steer_rate = 0.0f, tau_drive = 0.0f; + float v_blend = 0.0f, v_width = 1.0f, eps = 0.1f; + float fz_min = 0.02f; // the load floor's width, as a fraction of m g + int iters = 3; +}; + +template struct dyn4_state { + S x, y, psi, vx, vy, r, z, zd, phi, phid, th, thd, delta, a; + static const int size = 14; + CVC_HD S &operator[](int k) { + S *p[14] = {&x, &y, &psi, &vx, &vy, &r, &z, &zd, &phi, &phid, &th, &thd, &delta, &a}; + return *p[k]; + } + CVC_HD const S &operator[](int k) const { return (*const_cast(this))[k]; } +}; + +// Flat ground of one friction. +struct flat_ground { + float mu = 0.9f; + template CVC_HD void at(S, S, S &mu_out, S &height) const { + mu_out = S(mu); + height = S(0); + } +}; + +// The corners' rates: a spring rate of 0 means rigid, which here is a stiff spring whose +// undamped period is a few steps; damping of 0 means critical damping. +CVC_HD inline float dyn4_rigid_k(float m_corner, float hdt) { + const float w = 1.0f / hdt; // natural frequency 1 / hdt: stiff, still resolved + return m_corner * w * w; +} + +CVC_HD inline float s_log(float x) { return logf(x); } +CVC_HD inline double s_log(double x) { return log(x); } +template CVC_HD inline dual s_log(dual a) { return dual(s_log(a.v), a.d / a.v); } + +// A smooth floor at zero, softplus at width e: e log(1 + exp(F / e)). Above 30 e it is F (the +// rest is below 1e-13 e), so a tire under a normal load carries exactly its spring's force. +template CVC_HD inline S dyn4_soft_pos(S F, S e) { + const S q = F / e; + if (s_val(q) > 30.0) + return F; + return e * s_log(S(1) + s_exp(q)); +} + +// The four wheels at state s: their body-frame offsets from the centre of gravity, static loads, +// corner rates, the ground under them (friction and height) and their normal loads (step 2 of +// the header comment). The plant uses it, and so can a caller that wants the loads. +template struct dyn4_wheels { + S xi[4], yi[4], Fz0[4], k[4], c[4], mu[4], hg[4], Fz[4]; +}; +template +CVC_HD inline void dyn4_wheel_state(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, dyn4_wheels &w) { + const S m = S(p.m), lf = S(p.lf), lr = S(p.lr), tf = S(p.tf), tr = S(p.tr), g = S(p.g); + const S L = lf + lr; + const S xs[4] = {lf, lf, -lr, -lr}, ys[4] = {tf, -tf, tr, -tr}; + for (int i = 0; i < 4; ++i) { + const bool front = i < 2; + w.xi[i] = xs[i]; + w.yi[i] = ys[i]; + w.Fz0[i] = S(0.5) * m * g * (front ? lr : lf) / L; + const float kc = front ? p.k_f : p.k_r, cc = front ? p.c_f : p.c_r; + const float m_corner = 0.25f * p.m; + const float kk = kc > 0.0f ? kc : dyn4_rigid_k(m_corner, v.hdt); + w.k[i] = S(kk); + w.c[i] = S(cc > 0.0f ? cc : 2.0f * sqrtf(kk * m_corner)); + } + // The ground under each wheel, at the current pose. + const S cps = s_cos(s.psi), sps = s_sin(s.psi); + const S cgx = s.x + lr * cps, cgy = s.y + lr * sps; // the centre of gravity + for (int i = 0; i < 4; ++i) + ground.at(cgx + w.xi[i] * cps - w.yi[i] * sps, cgy + w.xi[i] * sps + w.yi[i] * cps, w.mu[i], + w.hg[i]); + // Loads, from the current vertical state. + const S e = S(p.fz_min) * m * g; + for (int i = 0; i < 4; ++i) { + const S defl = w.hg[i] - (s.z + w.yi[i] * s.phi - w.xi[i] * s.th); + const S rate = s.zd + w.yi[i] * s.phid - w.xi[i] * s.thd; + w.Fz[i] = dyn4_soft_pos(w.Fz0[i] + w.k[i] * defl - w.c[i] * rate, e); + } +} + +// A front wheel's Ackermann angle, at body-frame y from the centreline, for the virtual centre +// wheel's tan(delta): tan delta_i = L tan delta / (L - y tan delta). The rear wheels do not steer. +template CVC_HD inline S dyn4_ackermann(S L, S y, S tan_delta) { + return s_atan(L * tan_delta / (L - y * tan_delta)); +} + +// One step of the plant; see the header comment. +template +CVC_HD inline void dyn4_plant(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, S delta_cmd, S a_cmd, dyn4_state &o) { + const S hdt = S(v.hdt), one = S(1), zero = S(0); + const S m = S(p.m), Iz = S(p.Iz), Ix = S(p.Ix), Iy = S(p.Iy), lf = S(p.lf), lr = S(p.lr); + const S h = S(p.h), g = S(p.g), Cp = S(p.C), L = lf + lr; + + // 1. Actuators. + const S k_steer = p.tau_steer > 0.0f ? one - s_exp(-hdt / S(p.tau_steer)) : one; + S step = (delta_cmd - s.delta) * k_steer; + if (p.steer_rate > 0.0f) { + const S R = S(p.steer_rate) * hdt; + step = R * s_tanh(step / R); + } + const S delta1 = s.delta + step; + const S k_drive = p.tau_drive > 0.0f ? one - s_exp(-hdt / S(p.tau_drive)) : one; + const S a1 = s.a + (a_cmd - s.a) * k_drive; + + // 2. The wheels, the ground under them and their loads. + dyn4_wheels W; + dyn4_wheel_state(p, v, ground, s, W); + const S *xi = W.xi, *yi = W.yi, *Fz0 = W.Fz0, *k = W.k, *c = W.c, *mu_i = W.mu, *hg = W.hg; + const S *Fz = W.Fz; + + // 3. Longitudinal: each wheel's drive or brake force within its grip, and what that leaves. + const S F_drive = m * a1; + const S share = S(s_val(a1) >= s_val(zero) ? p.front_drive : p.front_brake); + S Fx[4], Dy[4], dw[4], cdw[4], sdw[4]; + // The front wheels at their Ackermann angles. + const S td = s_tan(delta1); + for (int i = 0; i < 4; ++i) { + const S axle_share = i < 2 ? share : one - share; + const S cap = mu_i[i] * Fz[i]; + Fx[i] = dyn_sat(S(0.5) * F_drive * axle_share, cap); + Dy[i] = dyn_lateral_cap(Fx[i], cap); + dw[i] = i < 2 ? dyn4_ackermann(L, yi[i], td) : zero; + cdw[i] = s_cos(dw[i]); + sdw[i] = s_sin(dw[i]); + } + const S eps2 = S(p.eps) * S(p.eps); + const S sp_min = S(v.allow_reverse ? -0.25f * v.vmax : 0.0f), vmax = S(v.vmax); + const S u0 = s_sqrt(s.vx * s.vx + eps2), sg0 = s.vx / u0; + const S F_res = -S(p.drag) * s.vx * u0 - S(p.c_rr) * m * g * sg0; + S FXlong = zero; + for (int i = 0; i < 4; ++i) + FXlong = FXlong + Fx[i] * cdw[i]; + const S vx_p = dyn_clamp(s.vx + hdt * ((FXlong + F_res) / m), sp_min, vmax); + + // 4. Lateral and yaw: implicit Euler for (vy, r), four tires. + const S u = s_sqrt(vx_p * vx_p + eps2), sg = vx_p / u; + const S Bw[4] = {S(p.B_f), S(p.B_f), S(p.B_r), S(p.B_r)}; + S vy1 = s.vy, r1 = s.r; + S Fy_applied[4] = {zero, zero, zero, zero}; + const int iters = p.iters > 0 ? p.iters : 1; + for (int it = 0; it < iters; ++it) { + S FY = zero, MZ = zero, J11 = zero, J12 = zero, J21 = zero, J22 = zero; + S Fy[4], St[4], dwdvy[4], dwdr[4]; + for (int i = 0; i < 4; ++i) { + // The wheel's velocity in its own frame, the forward speed from the prediction. + const S vxi = vx_p - r1 * yi[i], vyi = vy1 + r1 * xi[i]; + const S wi = -vxi * sdw[i] + vyi * cdw[i]; + const S ui = s_sqrt((vxi * cdw[i] + vyi * sdw[i]) * (vxi * cdw[i] + vyi * sdw[i]) + eps2); + const S alpha = -s_atan2(wi, ui); + Fy[i] = dyn_lateral(alpha, Bw[i], Cp, Dy[i]); + // The root: zero lateral velocity in the wheel's frame. + St[i] = dyn_secant_w(Fy[i], alpha, wi, zero, Bw[i], Cp, Dy[i], ui); + dwdvy[i] = cdw[i]; + dwdr[i] = yi[i] * sdw[i] + xi[i] * cdw[i]; + // On the body: the lateral force's components, and the longitudinal one's. + const S fby = Fy[i] * cdw[i] + Fx[i] * sdw[i]; + const S fbx = -Fy[i] * sdw[i] + Fx[i] * cdw[i]; + FY = FY + fby; + MZ = MZ + xi[i] * fby - yi[i] * fbx; + // d(body force, moment)/d(vy, r) through the secant: dFy = -St dw. + const S dfy_y = -St[i] * cdw[i], dfx_y = St[i] * sdw[i]; // per unit dw + J11 = J11 + dfy_y * dwdvy[i]; + J12 = J12 + dfy_y * dwdr[i]; + J21 = J21 + (xi[i] * dfy_y - yi[i] * dfx_y) * dwdvy[i]; + J22 = J22 + (xi[i] * dfy_y - yi[i] * dfx_y) * dwdr[i]; + } + const S f_vy = FY / m - r1 * vx_p, f_r = MZ / Iz; + const S A11 = one - hdt * (J11 / m), A12 = -hdt * (J12 / m - vx_p); + const S A21 = -hdt * (J21 / Iz), A22 = one - hdt * (J22 / Iz); + S det = A11 * A22 - A12 * A21; + if (s_val(det) < s_val(S(0.05))) + det = S(0.05); + const S b1 = s.vy + hdt * f_vy - vy1, b2 = s.r + hdt * f_r - r1; + const S dvy = (A22 * b1 - A12 * b2) / det, dr = (A11 * b2 - A21 * b1) / det; + vy1 = vy1 + dvy; + r1 = r1 + dr; + for (int i = 0; i < 4; ++i) + Fy_applied[i] = Fy[i] - St[i] * (dwdvy[i] * dvy + dwdr[i] * dr); + } + + // 3, corrected: the speed with the lateral forces the implicit step applied. + S FXb = F_res, FYb = zero; + for (int i = 0; i < 4; ++i) { + FXb = FXb + Fx[i] * cdw[i] - Fy_applied[i] * sdw[i]; + FYb = FYb + Fy_applied[i] * cdw[i] + Fx[i] * sdw[i]; + } + const S vx1 = dyn_clamp(s.vx + hdt * (FXb / m + r1 * vy1), sp_min, vmax); + + // 5. Vertical: implicit Euler on (z, phi, th) with the corners' linear springs and dampers. + // M qdd = sum_i c_i (Fz0_i + k_i (hg_i - c_i . q) - c_i' (c_i . qd)) - (m g, 0, 0) + // + (0, h FY, -h FX); (M + hdt D + hdt^2 K) qd1 = M qd0 + hdt (G - K q0). + S Kq[3][3] = {{zero, zero, zero}, {zero, zero, zero}, {zero, zero, zero}}; + S Dq[3][3] = {{zero, zero, zero}, {zero, zero, zero}, {zero, zero, zero}}; + S Gv[3] = {-m * g, h * FYb, -h * (FXb - F_res)}; + for (int i = 0; i < 4; ++i) { + const S ci[3] = {one, yi[i], -xi[i]}; + for (int a = 0; a < 3; ++a) { + Gv[a] = Gv[a] + ci[a] * (Fz0[i] + k[i] * hg[i]); + for (int b = 0; b < 3; ++b) { + Kq[a][b] = Kq[a][b] + k[i] * ci[a] * ci[b]; + Dq[a][b] = Dq[a][b] + c[i] * ci[a] * ci[b]; + } + } + } + const S Mv[3] = {m, Ix, Iy}; + const S q0[3] = {s.z, s.phi, s.th}, qd0[3] = {s.zd, s.phid, s.thd}; + S A[3][3], rhs[3]; + for (int a = 0; a < 3; ++a) { + rhs[a] = Mv[a] * qd0[a] + hdt * Gv[a]; + for (int b = 0; b < 3; ++b) { + A[a][b] = hdt * Dq[a][b] + hdt * hdt * Kq[a][b] + (a == b ? Mv[a] : zero); + rhs[a] = rhs[a] - hdt * Kq[a][b] * q0[b]; + } + } + // Cramer's rule on the symmetric positive definite 3x3. + const S det3 = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) - + A[0][1] * (A[1][0] * A[2][2] - A[1][2] * A[2][0]) + + A[0][2] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); + S qd1[3]; + for (int col = 0; col < 3; ++col) { + S Ac[3][3]; + for (int a = 0; a < 3; ++a) + for (int b = 0; b < 3; ++b) + Ac[a][b] = b == col ? rhs[a] : A[a][b]; + qd1[col] = (Ac[0][0] * (Ac[1][1] * Ac[2][2] - Ac[1][2] * Ac[2][1]) - + Ac[0][1] * (Ac[1][0] * Ac[2][2] - Ac[1][2] * Ac[2][0]) + + Ac[0][2] * (Ac[1][0] * Ac[2][1] - Ac[1][1] * Ac[2][0])) / + det3; + } + + // 6. Low speed: toward the kinematic bicycle. + if (p.v_blend > 0.0f) { + const S w = one / (one + s_exp((s_sqrt(vx1 * vx1 + eps2) - S(p.v_blend)) / S(p.v_width))); + const S r_k = vx1 * s_tan(delta1) / L, vy_k = lr * r_k; + vy1 = vy1 + w * (vy_k - vy1); + r1 = r1 + w * (r_k - r1); + } + + // 7. Pose. + const S psi1 = s.psi + hdt * r1; + const S v_side = vy1 - lr * r1; + const S c1 = s_cos(psi1), s1 = s_sin(psi1); + o.x = s.x + hdt * (vx1 * c1 - v_side * s1); + o.y = s.y + hdt * (vx1 * s1 + v_side * c1); + o.psi = psi1; + o.vx = vx1; + o.vy = vy1; + o.r = r1; + o.zd = qd1[0]; + o.phid = qd1[1]; + o.thd = qd1[2]; + o.z = s.z + hdt * qd1[0]; + o.phi = s.phi + hdt * qd1[1]; + o.th = s.th + hdt * qd1[2]; + o.delta = delta1; + o.a = a1; +} + +// The plant's vector-Jacobian product on forward-mode duals, as dyn_plant_vjp: gradients on its +// outputs -> on its state and commands. The ground's height gradient flows when G samples in the +// dual type. +template +CVC_HD inline void dyn4_plant_vjp(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, T delta_cmd, T a_cmd, + const dyn4_state &g_o, dyn4_state &g_s, T &g_delta_cmd, + T &g_a_cmd) { + typedef dual D; + const int n = dyn4_state::size; + for (int j = 0; j < n + 2; ++j) { + dyn4_state ds, o; + for (int kk = 0; kk < n; ++kk) + ds[kk] = D(s[kk], kk == j ? T(1) : T(0)); + dyn4_plant(p, v, ground, ds, D(delta_cmd, j == n ? T(1) : T(0)), + D(a_cmd, j == n + 1 ? T(1) : T(0)), o); + T acc = T(0); + for (int kk = 0; kk < n; ++kk) + acc += g_o[kk] * o[kk].d; + if (j < n) + g_s[j] = acc; + else if (j == n) + g_delta_cmd = acc; + else + g_a_cmd = acc; + } +} + +// One step under the trained controller, and its backward: bike_control's commands from the +// rear axle's pose and the speed vx. +template +CVC_HD inline void dyn4_bike_step(const field &f, const dyn4_state &s, float gx, float gy, + float al, float be, float ga, const bike_veh &v, + const dyn4_params &p, const G &ground, dyn4_state &o) { + float delta_cmd, a_cmd; + bike_control(f, s.x, s.y, s.psi, s.vx, gx, gy, al, be, ga, v, delta_cmd, a_cmd); + dyn4_plant(p, v, ground, s, delta_cmd, a_cmd, o); +} + +template +CVC_HD inline void dyn4_bike_step_bwd(const field &f, const dyn4_state &s, float gx, + float gy, float al, float be, float ga, const bike_veh &v, + const dyn4_params &p, const G &ground, + const dyn4_state &g_o, dyn4_state &g_s, + float &gal, float &gbe, float &gga) { + float delta_cmd, a_cmd; + bike_control(f, s.x, s.y, s.psi, s.vx, gx, gy, al, be, ga, v, delta_cmd, a_cmd); + float g_dc, g_ac; + dyn4_plant_vjp(p, v, ground, s, delta_cmd, a_cmd, g_o, g_s, g_dc, g_ac); + float gox, goy, gth, gsp; + bike_control_bwd(f, s.x, s.y, s.psi, s.vx, gx, gy, al, be, ga, v, g_dc, g_ac, gox, goy, gth, gsp, + gal, gbe, gga); + g_s.x += gox; + g_s.y += goy; + g_s.psi += gth; + g_s.vx += gsp; +} + +} // namespace diff +} // namespace nav +} // namespace cvc + +#endif // __CVC_NAV_DETAIL_DYN_4W_H__ diff --git a/inc/cvc/nav/dyn_4w.h b/inc/cvc/nav/dyn_4w.h new file mode 100644 index 00000000..99983586 --- /dev/null +++ b/inc/cvc/nav/dyn_4w.h @@ -0,0 +1,86 @@ +/* + 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::nav::dyn4_si: the two-track vehicle's parameters in SI (the vehicle-physics plan's P8d), +// and their conversion into nav's normalized frame for diff::dyn4_plant (detail/dyn_4w.h). +// +// cvc::nav::dyn4_si si = cvc::physics::nav_plant::dyn4_si_of(vehicle_desc::humvee()); +// cvc::nav::diff::dyn4_params p = cvc::nav::dyn4_params_of(si, cvc::nav::frame_of(cfg)); +// +// It is P8a's dyn_bike_si (mass, yaw inertia, axles, tires, drive and brake splits, resistances, +// lags) plus what four wheels and a suspension add: the half tracks, the roll and pitch inertias, +// and each corner's spring and damper. The ground's friction is the ground sampler's, so +// `single.mu` is not used. Values convert by their length exponent, as dyn_bike_params_of does: +// tracks e = 1, the inertias e = 2; spring rates (N/m = kg/s^2) and damper rates (N s/m = kg/s) +// carry no length and pass through. + +#ifndef __CVC_NAV_DYN_4W_H__ +#define __CVC_NAV_DYN_4W_H__ + +#include +#include + +namespace cvc { +namespace nav { + +struct dyn4_si { + dyn_bike_si single; // the single-track numbers (its mu unused: the ground's) + double half_track_front = 0.8; // m + double half_track_rear = 0.8; // m + double roll_inertia = 800.0; // kg m^2, about the centre of gravity + double pitch_inertia = 2500.0; // kg m^2 + double spring_front = 0.0; // N/m per corner; 0 = rigid + double spring_rear = 0.0; + double damper_front = 0.0; // N s/m per corner; 0 = critical + double damper_rear = 0.0; + double min_wheel_load = 0.02; // the load floor's width, of m g +}; + +inline diff::dyn4_params dyn4_params_of(const dyn4_si &si, const world_units::normalized_frame &f, + double metres_per_world_unit = 1.0) { + const diff::dyn_bike_params b = dyn_bike_params_of(si.single, f, metres_per_world_unit); + const double k = f.scale / metres_per_world_unit; // normalized units per metre + diff::dyn4_params p; + p.m = b.m; + p.Iz = b.Iz; + p.lf = b.lf; + p.lr = b.lr; + p.h = b.h; + p.g = b.g; + p.B_f = b.B_f; + p.B_r = b.B_r; + p.C = b.C; + p.front_drive = b.front_drive; + p.front_brake = b.front_brake; + p.drag = b.drag; + p.c_rr = b.c_rr; + p.tau_steer = b.tau_steer; + p.steer_rate = b.steer_rate; + p.tau_drive = b.tau_drive; + p.v_blend = b.v_blend; + p.v_width = b.v_width; + p.eps = b.eps; + p.iters = b.iters; + p.tf = static_cast(si.half_track_front * k); + p.tr = static_cast(si.half_track_rear * k); + p.Ix = static_cast(si.roll_inertia * k * k); + p.Iy = static_cast(si.pitch_inertia * k * k); + p.k_f = static_cast(si.spring_front); + p.k_r = static_cast(si.spring_rear); + p.c_f = static_cast(si.damper_front); + p.c_r = static_cast(si.damper_rear); + p.fz_min = static_cast(si.min_wheel_load); + return p; +} + +} // namespace nav +} // namespace cvc + +#endif // __CVC_NAV_DYN_4W_H__ diff --git a/inc/cvc/physics/nav_plant.h b/inc/cvc/physics/nav_plant.h index bed0b0ce..e7b35539 100644 --- a/inc/cvc/physics/nav_plant.h +++ b/inc/cvc/physics/nav_plant.h @@ -34,6 +34,7 @@ #include #include +#include #include #include #include @@ -155,6 +156,18 @@ class nav_plant final : public cvc::nav::vehicle_plant { static cvc::nav::diff::dyn_bike_params dyn_bike_params_at(const vehicle_desc &d, double wheelbase, double mu = 0.9); + // The same vehicle on four wheels (nav/dyn_4w.h, P8d): dyn_bike_si_of's numbers, with its + // defaults (front_stiffness 1, drive_lag < 0 for the vehicle's own), plus + // - the half tracks of the front (steered) and rear axles; + // - the chassis box's roll and pitch inertias, m (hy^2 + hz^2) / 3 and m (hx^2 + hz^2) / 3; + // - each corner's spring from its axle's suspension frequency f and the corner's static + // load m_c (k = m_c (2 pi f)^2), and its damper from the damping ratio z (c = 2 z + // sqrt(k m_c)). The anti-roll bars are left out. + static cvc::nav::dyn4_si dyn4_si_of(const vehicle_desc &d, double front_stiffness = 1.0, + double drive_lag = -1.0, double gravity = 9.80665); + // dyn4_si_of in a normalized frame where its wheelbase is `wheelbase`, as dyn_bike_params_at. + static cvc::nav::diff::dyn4_params dyn4_params_at(const vehicle_desc &d, double wheelbase); + // ── the vehicles ── int size() const { return static_cast(_agents.size()); } vehicle_id vehicle(int i) const { return _agents.at(i).vehicle; } diff --git a/src/cvc/physics/nav_plant.cpp b/src/cvc/physics/nav_plant.cpp index fb954db4..4b5cd75e 100644 --- a/src/cvc/physics/nav_plant.cpp +++ b/src/cvc/physics/nav_plant.cpp @@ -348,6 +348,52 @@ cvc::nav::diff::dyn_bike_params nav_plant::dyn_bike_params_at(const vehicle_desc return p; } +cvc::nav::dyn4_si nav_plant::dyn4_si_of(const vehicle_desc &d, double front_stiffness, + double drive_lag, double gravity) { + cvc::nav::dyn4_si si; + si.single = dyn_bike_si_of(d, 0.9, front_stiffness, drive_lag, gravity); + const double hx = d.half_extents.x, hy = d.half_extents.y, hz = d.half_extents.z; + si.roll_inertia = d.mass * (hy * hy + hz * hz) / 3.0; + si.pitch_inertia = d.mass * (hx * hx + hz * hz) / 3.0; + // The corners' static loads: half of each axle's share of m g. + const double lf = si.single.cg_to_front, lr = si.single.cg_to_rear, L = lf + lr; + const double m_front = 0.5 * d.mass * lr / L, m_rear = 0.5 * d.mass * lf / L; + const double kPi = 3.14159265358979323846; + bool have_front = false, have_rear = false; + double front_x = 0.0, rear_x = 0.0; + for (const axle_desc &a : d.axles) { + const bool front = a.steered; + if (front ? (have_front && a.x <= front_x) : (have_rear && a.x >= rear_x)) + continue; // the frontmost steered axle and the rearmost unsteered one, as dyn_bike_si_of + const double mc = front ? m_front : m_rear; + const double w = 2.0 * kPi * a.suspension_frequency; + const double k = mc * w * w, c = 2.0 * a.suspension_damping * std::sqrt(k * mc); + if (front) { + have_front = true; + front_x = a.x; + si.half_track_front = a.half_track; + si.spring_front = k; + si.damper_front = c; + } else { + have_rear = true; + rear_x = a.x; + si.half_track_rear = a.half_track; + si.spring_rear = k; + si.damper_rear = c; + } + } + return si; +} + +cvc::nav::diff::dyn4_params nav_plant::dyn4_params_at(const vehicle_desc &d, double wheelbase) { + const cvc::nav::dyn4_si si = dyn4_si_of(d); + cvc::world_units::normalized_frame f; + f.scale = wheelbase / (si.single.cg_to_front + si.single.cg_to_rear); + cvc::nav::diff::dyn4_params p = cvc::nav::dyn4_params_of(si, f); + p.lr = static_cast(wheelbase) - p.lf; // exactly the controller's wheelbase + return p; +} + cvc::nav::plant_limits nav_plant::limits(int i) const { const agent &a = _agents.at(static_cast(i)); cvc::nav::plant_limits l; diff --git a/src/cvc/tests/CMakeLists.txt b/src/cvc/tests/CMakeLists.txt index eca12781..d64b0396 100644 --- a/src/cvc/tests/CMakeLists.txt +++ b/src/cvc/tests/CMakeLists.txt @@ -112,6 +112,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(nav_dyn_4w_test nav_dyn_4w_test.cpp) add_executable(nav_potential_test nav_potential_test.cpp) # nav_potential_test compares U logged by the material rollouts (built with -ffp-contract=off) # with the same inline evaluator called here, bit for bit: build it the same way, or clang @@ -306,6 +307,7 @@ set(TEST_TARGETS nav_plant_test physics_plant_log_test nav_dyn_bike_test + nav_dyn_4w_test nav_potential_test ph_se2_test ph_se2_train_test @@ -1664,6 +1666,13 @@ target_link_libraries(nav_dyn_bike_test GTest::gtest_main ) +target_link_libraries(nav_dyn_4w_test + PRIVATE + cvc + GTest::gtest + GTest::gtest_main +) + target_link_libraries(nav_potential_test PRIVATE cvc @@ -2025,6 +2034,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(nav_dyn_4w_test PRIVATE cxx_std_17) target_compile_features(nav_potential_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) @@ -2478,6 +2488,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(nav_dyn_4w_test) cvc_discover_tests(nav_potential_test) cvc_discover_tests(ph_se2_test) cvc_discover_tests(ph_se2_train_test) diff --git a/src/cvc/tests/data/dyn4_golden.json b/src/cvc/tests/data/dyn4_golden.json new file mode 100644 index 00000000..02482a08 --- /dev/null +++ b/src/cvc/tests/data/dyn4_golden.json @@ -0,0 +1,67 @@ +{ + "format": "cvc-dyn4-golden-1", + "order": ["x", "y", "psi", "vx", "vy", "r", "z", "zd", "phi", "phid", "th", "thd", "delta", "a"], + "params": [ + {"m": 2676, "Iz": 6283.69384765625, "Ix": 1240.4151611328125, "Iy": 5404.53857421875, "lf": 1.5369999408721924, "lr": 1.7630000114440918, "tf": 0.9100000262260437, "tr": 0.9100000262260437, "h": 0.74000000953674316, "g": 9.8066501617431641, "k_f": 47691.4921875, "k_r": 41577.89453125, "c_f": 5838.720703125, "c_r": 5090.251953125, "B_f": 10.708646774291992, "B_r": 10.708646774291992, "C": 1.2021652460098267, "front_drive": 0.5, "front_brake": 0.70588237047195435, "drag": 1.5006250143051147, "c_rr": 0.014999999664723873, "tau_steer": 0.05000000074505806, "steer_rate": 0.34999999403953552, "tau_drive": 0.10000000149011612, "v_blend": 1, "v_width": 0.30000001192092896, "eps": 0.5, "fz_min": 0.019999999552965164, "iters": 3}, + {"m": 2676, "Iz": 6283.69384765625, "Ix": 1240.4151611328125, "Iy": 5404.53857421875, "lf": 1.5369999408721924, "lr": 1.7630000114440918, "tf": 0.9100000262260437, "tr": 0.9100000262260437, "h": 0.74000000953674316, "g": 9.8066501617431641, "k_f": 0, "k_r": 0, "c_f": 0, "c_r": 0, "B_f": 10.708646774291992, "B_r": 10.708646774291992, "C": 1.2021652460098267, "front_drive": 0.5, "front_brake": 0.70588237047195435, "drag": 1.5006250143051147, "c_rr": 0.014999999664723873, "tau_steer": 0.05000000074505806, "steer_rate": 0.34999999403953552, "tau_drive": 0.10000000149011612, "v_blend": 1, "v_width": 0.30000001192092896, "eps": 0.5, "fz_min": 0.019999999552965164, "iters": 3} + ], + "ground": {"mu_left": 0.5, "mu_right": 0.90000000000000002, "split_width": 0.29999999999999999, "A": 0.059999999999999998, "cx": 4, "cy": 3, "r": 0.80000000000000004}, + "g_out": [0.69999999999999996, -0.29999999999999999, 1.1000000000000001, 0.40000000000000002, -0.90000000000000002, 0.25, 0.80000000000000004, -0.20000000000000001, 0.5, 0.29999999999999999, -0.59999999999999998, 0.14999999999999999, 0.59999999999999998, -0.14999999999999999], + "cases": [ + {"params": 0, "state": [0, 0, 0, 12, 0.10000000000000001, 0.050000000000000003, 0, 0, 0, 0, 0, 0, 0.02, 0.5], "delta_cmd": 0.050000000000000003, "a_cmd": 1, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [0.72171610548641807, 0.0012537184369223102, 0.0046174190455564069, 12.028570280986669, 0.10102935313846087, 0.076956985812729459, 1.1237385413207323e-06, 1.872897610730411e-05, 0.0023530980803366142, 0.039218302215540936, -0.0004645858159483847, -0.0077430971055447856, 0.035978411112368727, 0.72559417581953878], + "g_state": [0.70000966726104197, -0.29986137109283861, 0.88286874715465546, 0.44484474493209553, -0.70577777555178045, 0.56606900606432198, 1.2370041180011988, -0.077399198117433968, -0.54734303156615938, 0.14033539891607372, -0.89578839040903691, 0.059231534214903639, 0.17522203949634058, -0.068307903223559702], + "g_delta_cmd": 0.073053537208745703, "g_a_cmd": -0.056157208673331863}, + {"params": 0, "state": [1, -2, 0.29999999999999999, 15, -0.59999999999999998, 0.5, 0.01, -0.02, 0.02, 0.050000000000000003, -0.01, 0.029999999999999999, 0.20000000000000001, 0], "delta_cmd": 0.25, "a_cmd": 0, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [1.8752991109887653, -1.7938258655880679, 0.32936336754610479, 14.91559246582745, -0.60412949576872821, 0.48938947004045397, 0.0079183275376267619, -0.034694541815037401, 0.035692512323980644, 0.26154187791226063, -0.0069048625110613585, 0.051585625968672617, 0.2195453364485479, 0], + "g_state": [0.70000000058961986, -0.29966544384637211, 0.69347119743303609, 0.46552155512027971, -0.84036119636462525, 0.95017418162096901, 3.1211156043724522, 0.15326669505985246, -0.63217420238361066, 0.12994978311042152, 2.0841262098247166, 0.42405318469035502, 0.44125761182768347, -0.067802151890118872], + "g_delta_cmd": 0.045491043641349489, "g_a_cmd": -0.055741421014385582}, + {"params": 0, "state": [-3, 4, -1.1000000000000001, 14, 0.40000000000000002, 0.20000000000000001, -0.0050000000000000001, 0.01, -0.01, 0, 0.02, -0.050000000000000003, 0.10000000000000001, -4], "delta_cmd": 0.10000000000000001, "a_cmd": -6, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [-2.62333857011815, 3.2678122736648048, -1.0875913820145209, 13.72273551514942, 0.25375431896260653, 0.20681030438055811, -0.0039591637515850623, 0.017347271194657361, -0.0071390858306467474, 0.047681903888327812, 0.018171808290944193, -0.03046986249865128, 0.10000000000000001, -4.9023767032781551], + "g_state": [0.69999999999999996, -0.29622152692791454, 1.5021293843502168, 0.44530231186042091, -0.77840913733184514, 0.78893978630420381, 1.7344835819167908, -0.016494338490772014, -0.8122753368234894, 0.10790056277797726, -1.1744393357827434, 0.025117167402977127, 0.17330375801342485, -0.078776871804010107], + "g_delta_cmd": 0.40208495601890654, "g_a_cmd": -0.064763944137642471}, + {"params": 0, "state": [0.69999999999999996, 3.5, 0.050000000000000003, 8, 0.10000000000000001, 0.10000000000000001, 0, 0, 0, 0, 0, 0, 0.050000000000000003, 0], "delta_cmd": 0.10000000000000001, "a_cmd": 0.5, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [1.1794764482569011, 3.52178712629724, 0.057646612740292959, 7.9989209393242318, 0.12678378476562271, 0.12744354852013451, 0.0020681050136454908, 0.034468417664520683, 0.00074454484216007451, 0.012409080980032482, -0.0014745438382524911, -0.024575731186851916, 0.069545336448547879, 0.22559417581953883], + "g_state": [0.70340885522488106, -0.32072293134413715, 0.87422482667420776, 0.44601187772073675, -0.69427452597732198, 0.45336865275585025, 1.3439814773725467, -0.064302293613976019, -0.52498571797627036, 0.1430725351018946, -0.81833157872226925, 0.068714330034165105, 0.15041016268518975, -0.068387440013498974], + "g_delta_cmd": 0.015506396017676997, "g_a_cmd": -0.056222597360424301}, + {"params": 0, "state": [-5, 0.10000000000000001, 0.02, 18, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, -3], "delta_cmd": 0, "a_cmd": -8, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [-3.9386875232361644, 0.12198021173934362, 0.019519443224760458, 17.692322275617801, 0.006898163453229785, -0.0080092797663470407, -1.7708927936164909e-10, -2.951488055331717e-09, -6.6666174845082574e-05, -0.0011111029389197955, 0.011569695398484485, 0.026161590559498529, 0, -5.2559417581953882], + "g_state": [0.69999999999999996, -0.29991010886009178, 0.76664841006491435, 0.44057093432503358, -0.79614400097609528, 0.94276670331845158, 1.1819050794661019, -0.084144801540278716, -0.52559692469553265, 0.14299770696327227, -1.0504309615997165, 0.040299126496171123, 0.20189801359525461, -0.069611713819285409], + "g_delta_cmd": 0.46842696804338219, "g_a_cmd": -0.057229095823125326}, + {"params": 0, "state": [0, 0, 0, 0.59999999999999998, 0.02, 0.050000000000000003, 0, 0, 0, 0, 0, 0, 0.29999999999999999, 0.29999999999999999], "delta_cmd": 0.34999999999999998, "a_cmd": 0.5, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [0.036081417747239497, -5.5803140490900795e-05, 0.0036125673817050449, 0.60134969198248567, 0.10304679046342312, 0.060209457707537002, 1.1237253635220505e-06, 1.8728756477321171e-05, 0.0032657195450436602, 0.054428660300636367, -8.7347596227525145e-05, -0.0014557933029982683, 0.31954533644854788, 0.39023767032781553], + "g_state": [0.70001046436282943, -0.29995653665615302, 1.0893305598498919, 0.34581646138361588, -0.18709401888847829, 0.08329063979305415, 1.1609949517645226, -0.086704763323028941, -0.52608495150447887, 0.14293795937088377, -0.90990834906140472, 0.057502871780638593, 0.34244943819555163, -0.071342011261685573], + "g_delta_cmd": 0.035304506756005855, "g_a_cmd": -0.058651605810319377}, + {"params": 0, "state": [0.5, 0.5, 0.20000000000000001, -2, 0.050000000000000003, -0.10000000000000001, 0, 0, 0.01, 0, 0, 0, 0.20000000000000001, -0.5], "delta_cmd": 0.25, "a_cmd": -0.80000000000000004, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 1}, + "next": [0.38121154164971249, 0.48064958574292427, 0.19289446742913458, -2.0049138629171108, -0.14577968595069263, -0.11842554549477426, 0.00039472712720541367, 0.0065787856004708781, 0.001290283216430153, -0.14516194963745321, -0.00010010763530108295, -0.0016684606256443836, 0.2195453364485479, -0.63535650549172329], + "g_state": [0.70388591859683602, -0.30390010031905851, 1.1350277704447196, 0.42150296735997117, -0.4761403228457402, 0.04945665635196949, 0.81133536246216442, -0.12951249656089289, -0.4942461252827236, 0.14683588756849528, -0.8320337840043639, 0.067036812284282488, 0.89678740054640149, -0.06968854945557873], + "g_delta_cmd": 0.092453464102960264, "g_a_cmd": -0.057292263841132701}, + {"params": 0, "state": [2, 2, 2.5, 10, 2.5, 1.1000000000000001, 0.02, 0.10000000000000001, 0.040000000000000001, -0.10000000000000001, 0, 0.050000000000000003, 0.40000000000000002, 1], "delta_cmd": 0.45000000000000001, "a_cmd": 2, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [1.491050678899092, 2.3365674715318949, 2.5691559610478687, 10.168782543799257, 1.9116252313759368, 1.1525993765604183, 0.0207545485739059, 0.012575809846189548, 0.034348472280486021, -0.094192130763924492, 0.0010219877401690238, 0.017033129383537172, 0.41954533644854791, 1.4511883516390776], + "g_state": [0.69673321346927275, -0.30059948661126518, 1.0191332409408165, 0.40265130168605723, -0.75863974937784917, 0.65636451408887131, 1.484730951799931, -0.04707077432913484, -0.5441569961497581, 0.14072545525561095, -0.86197983673274825, 0.06337060977720986, 0.35498633820029124, -0.068657345394447469], + "g_delta_cmd": 0.036596986817438878, "g_a_cmd": -0.056444491637436048}, + {"params": 0, "state": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "delta_cmd": 0.10000000000000001, "a_cmd": 2, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [0.0032481982782280035, -2.3352023251957501e-09, 2.0640528464324285e-05, 0.054136639168179616, 0.00060533121665481276, 0.0003440088154279343, 1.123743337742635e-06, 1.8729056047670943e-05, 2.6739988452910819e-05, 0.00044566648417660251, -0.00060189203099415573, -0.010031534074124855, 0.020946006673345115, 0.90237670327815533], + "g_state": [0.70001059340549421, -0.29996837265397086, 1.0991202768915258, 0.42860038278928375, -0.21723281131037328, 0.010040710205294252, 1.1516613897344032, -0.087847442218699751, -0.52402750401448217, 0.14318984625163805, -0.90926854881623875, 0.057581200538611023, 0.5834249897778333, -0.068644704422837113], + "g_delta_cmd": 0.0021013015338600788, "g_a_cmd": -0.0564340992575348}, + {"params": 1, "state": [1, -2, 0.29999999999999999, 15, -0.59999999999999998, 0.5, 0, 0, 0, 0, 0, 0, 0.20000000000000001, 0], "delta_cmd": 0.25, "a_cmd": 0, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [1.8753148130137616, -1.7948532853275359, 0.33075898790549713, 14.90821709067032, -0.60021706945507325, 0.5126498098835689, -2.1006454824287213e-05, -0.00035010758823030127, 0.0069384711084254577, 0.11564118772520626, 0.00024787128144596889, 0.004131188116438732, 0.2195453364485479, 0], + "g_state": [0.69999999886616981, -0.28789843892489059, 0.72794696919758439, 0.46693962419576468, -0.83692442286221225, 0.94457941844877535, 12.103103160556598, 1.2420207817730444, -1.4922901425899369, -0.0017778114954533938, 12.538152018335111, 1.6609552111912729, 0.43367789122235761, -0.067895604076282617], + "g_delta_cmd": 0.044709619385758931, "g_a_cmd": -0.055818249809762306}, + {"params": 1, "state": [0.69999999999999996, 3.5, 0.050000000000000003, 8, 0.10000000000000001, 0.10000000000000001, 0, 0, 0, 0, 0, 0, 0.050000000000000003, -2], "delta_cmd": 0.10000000000000001, "a_cmd": -4, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [1.1682518918628417, 3.521177609254952, 0.057754410109680042, 7.81155993255026, 0.12974574421025459, 0.12924017138341026, 0.0036079463418947342, 0.060132440375643685, -0.0027510393256259037, -0.045850656451940426, -0.001440493757910954, -0.024008229835141653, 0.069545336448547879, -2.9023767032781551], + "g_state": [0.70616318771613662, -0.33571847452684145, 0.82991072817888289, 0.44390673042890294, -0.63809879029723704, 0.46640171175243472, 2.5970214106671912, 0.10129103173217083, -0.94749222714198511, 0.063597934922776309, -0.74824210804452296, 0.066587999810420739, -0.12901016257434517, -0.070737476326357948], + "g_delta_cmd": -0.013300182883052437, "g_a_cmd": -0.058154606299114883} + ] +} diff --git a/src/cvc/tests/nav_dyn_4w_test.cpp b/src/cvc/tests/nav_dyn_4w_test.cpp new file mode 100644 index 00000000..33c850c9 --- /dev/null +++ b/src/cvc/tests/nav_dyn_4w_test.cpp @@ -0,0 +1,785 @@ +/* + 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. +*/ + +// P8d's gates for diff::dyn4_plant, the two-track vehicle with suspension (nav/detail/dyn_4w.h). +// The Humvee in metres (S = 1): P8a's single-track numbers from its vehicle_desc, plus its tracks, +// its corners' springs and dampers (suspension frequency and damping ratio) and the chassis box's +// roll and pitch inertia. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace diff = cvc::nav::diff; +using diff::bike_veh; +using diff::dyn4_params; +using diff::dyn4_state; +using diff::dyn_bike_params; +using diff::dyn_state; + +namespace { + +constexpr double kG = 9.80665; + +dyn_bike_params humvee1() { + const cvc::nav::dyn_bike_si si = + cvc::physics::nav_plant::dyn_bike_si_of(cvc::physics::vehicle_desc::humvee()); + cvc::world_units::normalized_frame f; + f.scale = 1.0; + return cvc::nav::dyn_bike_params_of(si, f); +} + +dyn4_params humvee4() { + cvc::world_units::normalized_frame f; + f.scale = 1.0; + return cvc::nav::dyn4_params_of( + cvc::physics::nav_plant::dyn4_si_of(cvc::physics::vehicle_desc::humvee()), f); +} + +bike_veh limits(float L) { + bike_veh v{}; + v.L = L; + v.vmax = 40.0f; + v.delta_max = 0.6f; + v.hdt = 0.06f; + v.allow_reverse = 0; + return v; +} + +// Flat ground whose friction is mu_left on the vehicle's left of y = 0 and mu_right on its right, +// blended over a few centimetres (so the wheels sit cleanly on one side or the other). +struct split_ground { + double mu_left = 0.3, mu_right = 0.9; + template void at(S, S wy, S &mu, S &height) const { + const S t = S(0.5) + S(0.5) * diff::s_tanh(wy / S(0.05)); + mu = S(mu_right) + t * S(mu_left - mu_right); + height = S(0); + } +}; + +// Flat ground with a round bump: height A exp(-|w - c|^2 / r^2). +struct bump_ground { + double mu = 0.9, cx = 0.0, cy = 0.0, A = 0.08, r = 0.4; + template void at(S wx, S wy, S &mu_out, S &height) const { + const S dx = wx - S(cx), dy = wy - S(cy); + mu_out = S(mu); + height = S(A) * diff::s_exp(-(dx * dx + dy * dy) / S(r * r)); + } +}; + +template +std::vector> drive4(const dyn4_params &p, const bike_veh &v, const G &ground, + dyn4_state s, int n, double (*dcmd)(int), + double (*acmd)(int)) { + std::vector> out{s}; + for (int t = 0; t < n; ++t) { + dyn4_state o; + diff::dyn4_plant(p, v, ground, s, dcmd(t), acmd(t), o); + s = o; + out.push_back(s); + } + return out; +} + +dyn4_state rolling(double vx) { + dyn4_state s{}; + s.vx = vx; + return s; +} + +} // namespace + +// The Humvee's four-wheel numbers come from its vehicle_desc: P8a's single-track ones, the axles' +// half tracks, the chassis box's inertias, and each corner's spring and damper from its axle's +// suspension frequency and damping ratio at the corner's static load. +TEST(Dyn4, HumveeParametersFromItsSpecSheet) { + const cvc::physics::vehicle_desc d = cvc::physics::vehicle_desc::humvee(); + const cvc::nav::dyn4_si si = cvc::physics::nav_plant::dyn4_si_of(d); + const dyn4_params p = humvee4(); + const dyn_bike_params b = humvee1(); + EXPECT_FLOAT_EQ(p.m, b.m); + EXPECT_FLOAT_EQ(p.lf, b.lf); + EXPECT_FLOAT_EQ(p.lr, b.lr); + EXPECT_FLOAT_EQ(p.h, b.h); + EXPECT_FLOAT_EQ(p.B_f, b.B_f); + double f_front = 0.0, z_front = 0.0; + for (const cvc::physics::axle_desc &a : d.axles) + if (a.steered) { + EXPECT_DOUBLE_EQ(si.half_track_front, a.half_track); + f_front = a.suspension_frequency; + z_front = a.suspension_damping; + } + // The front corner's natural frequency and damping ratio, back from its spring and damper. + const double mc = + 0.5 * d.mass * si.single.cg_to_rear / (si.single.cg_to_front + si.single.cg_to_rear); + EXPECT_NEAR(std::sqrt(si.spring_front / mc) / (2.0 * 3.14159265358979), f_front, 1e-9); + EXPECT_NEAR(si.damper_front / (2.0 * std::sqrt(si.spring_front * mc)), z_front, 1e-9); + EXPECT_GT(si.roll_inertia, 0.0); + EXPECT_GT(si.pitch_inertia, si.roll_inertia); // a long, narrow box + EXPECT_GT(p.tf, 0.5f); + EXPECT_GT(p.tr, 0.5f); +} + +// At rest on flat ground with no commands it stays where it is, its loads the static ones. +TEST(Dyn4, RestsOnFlatGround) { + const dyn4_params p = humvee4(); + const bike_veh v = limits(p.lf + p.lr); + const diff::flat_ground ground; + dyn4_state s{}; + for (int t = 0; t < 200; ++t) { + dyn4_state o; + diff::dyn4_plant(p, v, ground, s, 0.0, 0.0, o); + s = o; + } + EXPECT_NEAR(s.x, 0.0, 1e-9); + EXPECT_NEAR(s.y, 0.0, 1e-9); + EXPECT_NEAR(s.z, 0.0, 1e-6); + EXPECT_NEAR(s.phi, 0.0, 1e-9); + EXPECT_NEAR(s.th, 0.0, 1e-6); + diff::dyn4_wheels w; + diff::dyn4_wheel_state(p, v, ground, s, w); + const double total = w.Fz[0] + w.Fz[1] + w.Fz[2] + w.Fz[3]; + EXPECT_NEAR(total, p.m * p.g, 1e-3 * p.m * p.g); +} + +// With no track and rigid corners on flat ground it is P8a's single-track vehicle: the same +// manoeuvre (steer, throttle, then brake from 15 m/s) through both plants. +TEST(Dyn4, SingleTrackLimitIsDynPlant) { + dyn4_params p4 = humvee4(); + p4.tf = p4.tr = 0.0f; + p4.k_f = p4.k_r = p4.c_f = p4.c_r = 0.0f; // rigid + dyn_bike_params p1 = humvee1(); + p1.mu = 0.9f; + const bike_veh v = limits(p4.lf + p4.lr); + const diff::flat_ground ground; // mu 0.9 + dyn4_state s4 = rolling(15.0); + dyn_state s1{}; + s1.vx = 15.0; + double worst_pos = 0.0, worst_v = 0.0, path = 0.0; + for (int t = 0; t < 150; ++t) { + const double dc = 0.05, ac = t < 50 ? 0.5 : (t < 100 ? -3.0 : 0.0); + dyn4_state o4; + dyn_state o1; + diff::dyn4_plant(p4, v, ground, s4, dc, ac, o4); + diff::dyn_plant(p1, v, s1, dc, ac, 0.0, o1); + path += std::hypot(o1.x - s1.x, o1.y - s1.y); + s4 = o4; + s1 = o1; + worst_pos = std::max(worst_pos, std::hypot(s4.x - s1.x, s4.y - s1.y)); + worst_v = std::max({worst_v, std::fabs(s4.vx - s1.vx), std::fabs(s4.vy - s1.vy)}); + } + std::printf(" single-track limit, 150 steps (%.1f m): positions within %.3f m, velocities " + "within %.3f m/s\n", + path, worst_pos, worst_v); + EXPECT_LT(worst_pos, 0.01 * path); + EXPECT_LT(worst_v, 0.2); +} + +// Steady states balance their moments: in a steady turn the roll springs carry h x the lateral +// force (sum y_i Fz_i = -h FY, the outside wheels loaded), and in steady braking the front axle +// carries the quasi-static m (g lr - a h) / L. +TEST(Dyn4, SteadyLoadTransferBalances) { + const dyn4_params p = humvee4(); + const bike_veh v = limits(p.lf + p.lr); + const diff::flat_ground ground; + // A steady left turn at 12 m/s, the speed held by a proportional throttle. + dyn4_state s = rolling(12.0); + for (int t = 0; t < 400; ++t) { + dyn4_state o; + const double a_cmd = 2.0 * (12.0 - s.vx) + p.drag * s.vx * s.vx / p.m + p.c_rr * p.g; + diff::dyn4_plant(p, v, ground, s, 0.06, a_cmd, o); + s = o; + } + diff::dyn4_wheels w; + diff::dyn4_wheel_state(p, v, ground, s, w); + double roll = 0.0; + for (int i = 0; i < 4; ++i) + roll += w.yi[i] * w.Fz[i]; + const double FY = p.m * s.r * s.vx; // steady: m (vy' + r vx), vy' = 0 + std::printf(" steady turn: a_y %.2f m/s^2, roll %.4f rad, sum y Fz %.0f N m against -h FY " + "%.0f; outside (right) loads %.0f / %.0f N, inside %.0f / %.0f\n", + s.r * s.vx, s.phi, roll, -p.h * FY, w.Fz[1], w.Fz[3], w.Fz[0], w.Fz[2]); + EXPECT_GT(s.r, 0.0); + EXPECT_NEAR(roll, -p.h * FY, 0.03 * std::fabs(p.h * FY)); + EXPECT_GT(w.Fz[1], w.Fz[0]); + EXPECT_GT(w.Fz[3], w.Fz[2]); + + // Steady braking at 3 m/s^2 (held until the pitch settles; the speed stays well above 0). + const double a = -3.0; + s = rolling(25.0); + for (int t = 0; t < 60; ++t) { + dyn4_state o; + diff::dyn4_plant(p, v, ground, s, 0.0, a, o); + s = o; + } + diff::dyn4_wheel_state(p, v, ground, s, w); + const double L = p.lf + p.lr, front = w.Fz[0] + w.Fz[1]; + // The deceleration the tires actually deliver, with drag and rolling resistance. + const double a_eff = s.a; // the actuator's force per unit mass, settled + const double want = p.m * (p.g * p.lr - a_eff * p.h) / L; + std::printf(" steady braking %.1f m/s^2: front axle %.0f N against quasi-static %.0f N " + "(static %.0f), pitch %.4f rad\n", + a_eff, front, want, p.m * p.g * p.lr / L, s.th); + EXPECT_NEAR(front, want, 0.03 * want); + EXPECT_GT(s.th, 0.0); // the nose dips +} + +// Braking with the left wheels on ice (mu 0.3) and the right on dry ground (0.9): the right side +// brakes harder, so the vehicle yaws toward it (to the right, psi < 0). +TEST(Dyn4, MuSplitBrakingYawsTowardHighMu) { + const dyn4_params p = humvee4(); + const bike_veh v = limits(p.lf + p.lr); + const split_ground ground; + dyn4_state s = rolling(15.0); + for (int t = 0; t < 30; ++t) { + dyn4_state o; + diff::dyn4_plant(p, v, ground, s, 0.0, -7.0, o); + s = o; + } + std::printf(" mu-split braking, 1.8 s from 15 m/s: heading %.4f rad, yaw rate %.4f rad/s, " + "speed %.2f m/s\n", + s.psi, s.r, s.vx); + EXPECT_LT(s.psi, -0.005); +} + +// A bump under the left wheels' path: the front left's load rises above its static load as it +// passes, more than the front right's. +TEST(Dyn4, ABumpLoadsItsWheel) { + const dyn4_params p = humvee4(); + const bike_veh v = limits(p.lf + p.lr); + bump_ground ground; + ground.cx = 5.0; + ground.cy = p.tf; + dyn4_state s = rolling(5.0); + s.x = -10.0; + double fl_max = 0.0, fr_max = 0.0, fz0 = 0.0; + for (int t = 0; t < 120; ++t) { + diff::dyn4_wheels w; + diff::dyn4_wheel_state(p, v, ground, s, w); + fz0 = w.Fz0[0]; + fl_max = std::max(fl_max, w.Fz[0]); + fr_max = std::max(fr_max, w.Fz[1]); + dyn4_state o; + const double a_cmd = 1.0 * (5.0 - s.vx); + diff::dyn4_plant(p, v, ground, s, 0.0, a_cmd, o); + s = o; + } + std::printf(" bump (8 cm) under the left wheels: front left peaks at %.2f x its static load, " + "front right at %.2f x\n", + fl_max / fz0, fr_max / fz0); + EXPECT_GT(fl_max, 1.2 * fz0); + EXPECT_GT(fl_max - fz0, 2.0 * (fr_max - fz0)); +} + +// The plant's VJP (forward-mode duals) against central differences in double, in several regimes: +// cruising, cornering, braking in a turn, crawling, and rolling over a bump (the terrain's +// gradient flows through the ground sampler). +TEST(Dyn4, PlantGradientMatchesFiniteDifference) { + const dyn4_params p = humvee4(); + const bike_veh v = limits(p.lf + p.lr); + bump_ground bump; + bump.cx = 1.0; + bump.cy = 0.5; + bump.A = 0.06; + bump.r = 0.8; + struct gcase { + dyn4_state s; + double dc, ac; + bool on_bump; + }; + std::vector cases; + { + gcase c{rolling(12.0), 0.02, 0.5, false}; + cases.push_back(c); + c.s = rolling(14.0); + c.s.vy = 0.4; + c.s.r = 0.3; + c.s.phi = 0.01; + c.s.delta = 0.08; + c.dc = 0.1; + c.ac = 0.1; // not 0: a zero drive force is the kink where front_drive hands over to + // front_brake, and a central difference across it is not a gradient + cases.push_back(c); + c.s.a = -2.0; + c.ac = -4.0; + c.s.th = 0.004; + c.s.thd = 0.01; + cases.push_back(c); + c = gcase{rolling(0.6), 0.3, 0.4, false}; + c.s.delta = 0.2; + cases.push_back(c); + c = gcase{rolling(6.0), 0.05, 0.3, true}; + c.s.x = 0.0; + c.s.y = 0.2; + c.s.zd = 0.05; + cases.push_back(c); + } + std::mt19937 rng(1); + std::uniform_real_distribution u(-1.0, 1.0); + double worst = 0.0; + for (const gcase &gc : cases) { + dyn4_state g_o; + for (int k = 0; k < dyn4_state::size; ++k) + g_o[k] = u(rng); + const auto loss = [&](const dyn4_state &s, double dc, double ac) { + dyn4_state o; + if (gc.on_bump) + diff::dyn4_plant(p, v, bump, s, dc, ac, o); + else + diff::dyn4_plant(p, v, diff::flat_ground{}, s, dc, ac, o); + double acc = 0.0; + for (int k = 0; k < dyn4_state::size; ++k) + acc += g_o[k] * o[k]; + return acc; + }; + dyn4_state g_s; + double g_dc = 0.0, g_ac = 0.0; + if (gc.on_bump) + diff::dyn4_plant_vjp(p, v, bump, gc.s, gc.dc, gc.ac, g_o, g_s, g_dc, g_ac); + else + diff::dyn4_plant_vjp(p, v, diff::flat_ground{}, gc.s, gc.dc, gc.ac, g_o, g_s, g_dc, + g_ac); + const int n = dyn4_state::size; + for (int j = 0; j < n + 2; ++j) { + const double eps = 1e-6; + dyn4_state sp = gc.s, sm = gc.s; + double dcp = gc.dc, dcm = gc.dc, acp = gc.ac, acm = gc.ac; + if (j < n) { + sp[j] += eps; + sm[j] -= eps; + } else if (j == n) { + dcp += eps; + dcm -= eps; + } else { + acp += eps; + acm -= eps; + } + const double fd = (loss(sp, dcp, acp) - loss(sm, dcm, acm)) / (2.0 * eps); + const double an = j < n ? g_s[j] : (j == n ? g_dc : g_ac); + worst = std::max(worst, std::fabs(an - fd) / std::max(1.0, std::fabs(fd))); + } + } + std::printf(" dyn4_plant VJP vs central differences, %zu regimes: worst relative %.2e\n", + cases.size(), worst); + EXPECT_LT(worst, 1e-5); +} + +// The envelopes P8a meets, on four wheels: steady lateral acceleration never above mu g + 10%, +// and stopping within 10% of v^2 / (2 mu g). +TEST(Dyn4, EnvelopesOnFourWheels) { + const dyn4_params p = humvee4(); + const bike_veh v = limits(p.lf + p.lr); + for (const double mu : {0.4, 0.9}) { + diff::flat_ground ground; + ground.mu = static_cast(mu); + double peak = 0.0; + for (const double delta : {0.05, 0.1, 0.2, 0.35}) { + dyn4_state s = rolling(15.0); + // The body's lateral acceleration over the tail, vy' + r vx: once the tires slide the + // vehicle drifts and r vx alone overstates it (as DynBike.SteadyLateralNeverExceedsMuG). + double ay = 0.0, vy0 = 0.0; + const int n = 200, tail = 17; + for (int t = 0; t < n; ++t) { + if (t == n - tail) + vy0 = s.vy; + dyn4_state o; + const double a_cmd = 2.0 * (15.0 - s.vx) + p.drag * s.vx * s.vx / p.m + p.c_rr * p.g; + diff::dyn4_plant(p, v, ground, s, delta, a_cmd, o); + s = o; + if (t >= n - tail) + ay += s.vx * s.r / tail; + } + peak = std::max(peak, std::fabs(ay + (s.vy - vy0) / (tail * v.hdt))); + } + std::printf(" mu %.1f: the steadiest lateral acceleration %.2f m/s^2 = %.2f mu g\n", mu, peak, + peak / (mu * kG)); + EXPECT_LE(peak, 1.1 * mu * kG); + + dyn4_state s = rolling(20.0); + double dist = 0.0; + for (int t = 0; t < 2000 && s.vx > 0.01; ++t) { + dyn4_state o; + diff::dyn4_plant(p, v, ground, s, 0.0, -30.0, o); + dist += std::hypot(o.x - s.x, o.y - s.y); + s = o; + } + const double want = 20.0 * 20.0 / (2.0 * mu * kG); + std::printf(" mu %.1f: stopping from 20 m/s in %.1f m against v^2/(2 mu g) = %.1f m\n", mu, + dist, want); + EXPECT_NEAR(dist, want, 0.1 * want); + } +} + +// One step under the trained controller (dyn4_bike_step) and its backward, through bike_control, +// against central differences in float: a RoboRacer in the trainer's city (nav_plant:: +// dyn4_params_at, the city's veh_L), from scattered starts toward their goals. +TEST(Dyn4, StepGradcheckThroughTheController) { + const cvc::nav::training_scene sc = cvc::nav::city_scene(48); + const cvc::nav::field_stack fs = sc.field(); + diff::field F; + F.data = fs.data; + F.H = fs.H; + F.W = fs.W; + F.S = static_cast(fs.S); + F.cx = static_cast(fs.cx); + F.cy = static_cast(fs.cy); + F.mnx = static_cast(fs.mnx); + F.mny = static_cast(fs.mny); + F.mxx = static_cast(fs.mxx); + F.mxy = static_cast(fs.mxy); + const cvc::nav::train_config tc; + bike_veh v{}; + v.rr = sc.rr; + v.d_hat = sc.d_hat; + v.vmax = sc.vmax; + v.L = tc.veh_L; + v.delta_max = tc.veh_delta_max; + v.a_max = tc.veh_a_max; + v.a_lat_max = tc.veh_a_lat_max; + v.k_steer = tc.veh_k_steer; + v.hdt = sc.dt; + v.allow_reverse = tc.veh_allow_reverse ? 1 : 0; + const dyn4_params p = + cvc::physics::nav_plant::dyn4_params_at(cvc::physics::vehicle_desc::roboracer(), tc.veh_L); + const diff::flat_ground ground; + const int n = 16; + std::vector o(2 * n), g(2 * n); + sc.sample_starts_goals(n, 7, o.data(), g.data()); + const float al = 0.6f, be = 1.8f, ga = 0.9f, eps = 1e-3f; + std::mt19937 rng(5); + std::uniform_real_distribution u(-1.0f, 1.0f); + double num = 0.0, den = 0.0; + for (int i = 0; i < n; ++i) { + dyn4_state s{}; + // Off the SDF's grid nodes: the scene's starts sit exactly on them (cell centres), where the + // bilinear field has a kink that a central difference straddles. + s.x = o[2 * i] + 0.04f * u(rng); + s.y = o[2 * i + 1] + 0.04f * u(rng); + s.psi = std::atan2(g[2 * i + 1] - s.y, g[2 * i] - s.x) + 0.2f * u(rng); + s.vx = 0.3f + 0.4f * (u(rng) + 1.0f); + s.vy = 0.02f * u(rng); + s.r = 0.3f * u(rng); + dyn4_state w, dir; + for (int k = 0; k < dyn4_state::size; ++k) { + w[k] = u(rng); + dir[k] = 0.0f; + } + // A direction in the planar state (the controller reads the pose and the speed). + dir.x = u(rng); + dir.y = u(rng); + dir.psi = u(rng); + dir.vx = u(rng); + dir.vy = u(rng); + dir.r = u(rng); + const auto loss = [&](float t) { + dyn4_state st = s, out; + for (int k = 0; k < dyn4_state::size; ++k) + st[k] += t * dir[k]; + diff::dyn4_bike_step(F, st, g[2 * i], g[2 * i + 1], al, be, ga, v, p, ground, out); + double acc = 0.0; + for (int k = 0; k < dyn4_state::size; ++k) + acc += static_cast(w[k]) * out[k]; + return acc; + }; + dyn4_state gs{}; + float gal = 0, gbe = 0, gga = 0; + diff::dyn4_bike_step_bwd(F, s, g[2 * i], g[2 * i + 1], al, be, ga, v, p, ground, w, gs, gal, + gbe, gga); + double an = 0.0; + for (int k = 0; k < dyn4_state::size; ++k) + an += static_cast(gs[k]) * dir[k]; + const double fd = (loss(eps) - loss(-eps)) / (2.0 * eps); + num += std::fabs(an - fd); + den += std::fabs(fd); + } + std::printf(" dyn4_bike_step through the controller, %d agents: directional relative error " + "%.2e\n", + n, num / den); + EXPECT_LT(num / den, 5e-2); +} + +// The P2 envelopes Jolt's wheeled Humvee meets (physics_vehicle_test, measured 2026-10): its +// skidpad's peak lateral acceleration at mu 0.7 is 6.85 m/s^2, and it stops from 20 m/s at mu 0.8 +// in 25.90 m. The four-wheel plant, on the same vehicle_desc, lands within 10% of both. +TEST(Dyn4, MatchesJoltsHumveeEnvelopes) { + const dyn4_params p = humvee4(); + const bike_veh v = limits(p.lf + p.lr); + diff::flat_ground ground; + ground.mu = 0.7f; + double peak = 0.0; + for (const double delta : {0.05, 0.1, 0.2, 0.35}) { + dyn4_state s = rolling(15.0); + double ay = 0.0; + const int n = 200, tail = 17; + for (int t = 0; t < n; ++t) { + dyn4_state o; + const double a_cmd = 2.0 * (15.0 - s.vx) + p.drag * s.vx * s.vx / p.m + p.c_rr * p.g; + diff::dyn4_plant(p, v, ground, s, delta, a_cmd, o); + s = o; + if (t >= n - tail) + ay += std::fabs(s.vx * s.r) / tail; + } + peak = std::max(peak, ay); + } + ground.mu = 0.8f; + dyn4_state s = rolling(20.0); + double dist = 0.0; + for (int t = 0; t < 2000 && s.vx > 0.01; ++t) { + dyn4_state o; + diff::dyn4_plant(p, v, ground, s, 0.0, -30.0, o); + dist += std::hypot(o.x - s.x, o.y - s.y); + s = o; + } + std::printf(" against Jolt's Humvee: skidpad peak %.2f m/s^2 at mu 0.7 (Jolt 6.85); stopping " + "from 20 m/s %.2f m at mu 0.8 (Jolt 25.90)\n", + peak, dist); + EXPECT_NEAR(peak, 6.85, 0.1 * 6.85); + EXPECT_NEAR(dist, 25.90, 0.1 * 25.90); +} + +// The P2 Ackermann gate, as Jolt's Humvee meets it (physics_vehicle_test): each front wheel takes +// its Ackermann angle, cot(right) - cot(left) = track / wheelbase on a left turn. And the +// vehicle turns on that geometry: slowly, held at delta, the rear axle's circle is the kinematic +// L / tan(delta) to 1%, from the tires alone (the low-speed blend off). +TEST(Dyn4, TheFrontWheelsFollowAckermann) { + const dyn4_params p = humvee4(); + const double L = double(p.lf) + p.lr, tf = p.tf; + EXPECT_NEAR(L, 3.30, 1e-6); + EXPECT_NEAR(2.0 * tf, 1.82, 1e-6); + const auto cot = [](double a) { return std::cos(a) / std::sin(a); }; + for (const double delta : {0.05, 0.2, 0.45, -0.3}) { + const double t = std::tan(delta); + const double left = diff::dyn4_ackermann(L, tf, t), right = diff::dyn4_ackermann(L, -tf, t); + EXPECT_NEAR(left, std::atan(L * t / (L - tf * t)), 1e-12); + EXPECT_NEAR(cot(right) - cot(left), 2.0 * tf / L, 1e-12) << delta; + EXPECT_GT(std::fabs(delta > 0 ? left : right), std::fabs(delta)) + << "the inner wheel steers more"; + } + dyn4_params tires = p; + tires.v_blend = 0.0f; + const bike_veh v = limits(p.lf + p.lr); + const diff::flat_ground ground; + for (const double delta : {0.15, 0.3, 0.45}) { + dyn4_state s = rolling(3.0); + for (int t = 0; t < 400; ++t) { + dyn4_state o; + const double a_cmd = 2.0 * (3.0 - s.vx) + p.drag * s.vx * s.vx / p.m + p.c_rr * p.g; + diff::dyn4_plant(tires, v, ground, s, delta, a_cmd, o); + s = o; + } + const double rear_speed = std::hypot(s.vx, s.vy - p.lr * s.r); + const double R = rear_speed / s.r, want = L / std::tan(delta); + std::printf(" delta %.2f at %.1f m/s: the rear axle's circle %.3f m, L / tan(delta) %.3f m " + "(%+.2f%%)\n", + delta, s.vx, R, want, 100.0 * (R / want - 1.0)); + EXPECT_NEAR(R, want, 0.01 * want); + } +} + +namespace { + +// The golden's ground: the friction split along y = 0 (left mu_left, right mu_right, blended +// over split_width) and a round bump, so the twin is held to the ground's gradients too. +struct golden_ground { + double mu_left = 0.5, mu_right = 0.9, split_width = 0.3; + double A = 0.06, cx = 4.0, cy = 3.0, r = 0.8; + template void at(S wx, S wy, S &mu, S &height) const { + const S t = S(0.5) + S(0.5) * diff::s_tanh(wy / S(split_width)); + mu = S(mu_right) + t * S(mu_left - mu_right); + const S dx = wx - S(cx), dy = wy - S(cy); + height = S(A) * diff::s_exp(-(dx * dx + dy * dy) / S(r * r)); + } +}; + +} // namespace + +// The C++ side of the parity gate for GRL-SNAM's torch twin: the values it is held to. The plant, +// in double, on fixed cases on golden_ground: the Humvee with its springs (and lags and the +// low-speed blend), and again with rigid corners. Each case's next state and its VJP for a fixed +// output gradient. CVC_DYN4_GOLDEN_WRITE=1 rewrites tests/data/dyn4_golden.json; otherwise this +// build must reproduce it to 1e-9. +TEST(Dyn4, Golden) { + namespace fs = std::filesystem; + const fs::path path = fs::path(__FILE__).parent_path() / "data" / "dyn4_golden.json"; + dyn4_params sprung = humvee4(); + sprung.tau_steer = 0.05f; + sprung.v_blend = 1.0f; + sprung.v_width = 0.3f; + dyn4_params rigid = sprung; + rigid.k_f = rigid.k_r = rigid.c_f = rigid.c_r = 0.0f; + const dyn4_params psets[2] = {sprung, rigid}; + const golden_ground ground; + struct gcase { + int params; + double s[14], dcmd, acmd; + bool reverse; + }; + // x, y, psi, vx, vy, r, z, zd, phi, phid, th, thd, delta, a + const gcase cases[] = { + {0, {0, 0, 0, 12, 0.1, 0.05, 0, 0, 0, 0, 0, 0, 0.02, 0.5}, 0.05, 1.0, false}, + {0, + {1, -2, 0.3, 15, -0.6, 0.5, 0.01, -0.02, 0.02, 0.05, -0.01, 0.03, 0.2, 0}, + 0.25, + 0.0, + false}, + {0, + {-3, 4, -1.1, 14, 0.4, 0.2, -0.005, 0.01, -0.01, 0.0, 0.02, -0.05, 0.1, -4}, + 0.1, + -6.0, + false}, + {0, {0.7, 3.5, 0.05, 8, 0.1, 0.1, 0, 0, 0, 0, 0, 0, 0.05, 0}, 0.1, 0.5, false}, + {0, {-5, 0.1, 0.02, 18, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, -3}, 0.0, -8.0, false}, + {0, {0, 0, 0, 0.6, 0.02, 0.05, 0, 0, 0, 0, 0, 0, 0.3, 0.3}, 0.35, 0.5, false}, + {0, {0.5, 0.5, 0.2, -2, 0.05, -0.1, 0, 0, 0.01, 0, 0, 0, 0.2, -0.5}, 0.25, -0.8, true}, + {0, {2, 2, 2.5, 10, 2.5, 1.1, 0.02, 0.1, 0.04, -0.1, 0, 0.05, 0.4, 1}, 0.45, 2.0, false}, + {0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 0.1, 2.0, false}, + {1, {1, -2, 0.3, 15, -0.6, 0.5, 0, 0, 0, 0, 0, 0, 0.2, 0}, 0.25, 0.0, false}, + {1, {0.7, 3.5, 0.05, 8, 0.1, 0.1, 0, 0, 0, 0, 0, 0, 0.05, -2}, 0.1, -4.0, false}, + }; + const int n = dyn4_state::size; + const double g_out[14] = {0.7, -0.3, 1.1, 0.4, -0.9, 0.25, 0.8, + -0.2, 0.5, 0.3, -0.6, 0.15, 0.6, -0.15}; + // Every value this build computes, in the file's order: per case, next[14], g_state[14], + // g_delta_cmd, g_a_cmd. + std::vector now; + std::ostringstream js; + const auto num = [](double x) { + char b[40]; + std::snprintf(b, sizeof b, "%.17g", x); + return std::string(b); + }; + const auto arr = [&](const double *v, int k) { + std::string out = "["; + for (int i = 0; i < k; ++i) + out += (i ? ", " : "") + num(v[i]); + return out + "]"; + }; + js << "{\n \"format\": \"cvc-dyn4-golden-1\",\n"; + js << " \"order\": [\"x\", \"y\", \"psi\", \"vx\", \"vy\", \"r\", \"z\", \"zd\", \"phi\", " + "\"phid\", \"th\", \"thd\", \"delta\", \"a\"],\n"; + js << " \"params\": [\n"; + for (int k = 0; k < 2; ++k) { + const dyn4_params &p = psets[k]; + const std::pair params[] = {{"m", p.m}, + {"Iz", p.Iz}, + {"Ix", p.Ix}, + {"Iy", p.Iy}, + {"lf", p.lf}, + {"lr", p.lr}, + {"tf", p.tf}, + {"tr", p.tr}, + {"h", p.h}, + {"g", p.g}, + {"k_f", p.k_f}, + {"k_r", p.k_r}, + {"c_f", p.c_f}, + {"c_r", p.c_r}, + {"B_f", p.B_f}, + {"B_r", p.B_r}, + {"C", p.C}, + {"front_drive", p.front_drive}, + {"front_brake", p.front_brake}, + {"drag", p.drag}, + {"c_rr", p.c_rr}, + {"tau_steer", p.tau_steer}, + {"steer_rate", p.steer_rate}, + {"tau_drive", p.tau_drive}, + {"v_blend", p.v_blend}, + {"v_width", p.v_width}, + {"eps", p.eps}, + {"fz_min", p.fz_min}}; + js << " {"; + bool first = true; + for (const auto &kv : params) { + js << (first ? "" : ", ") << "\"" << kv.first << "\": " << num(kv.second); + first = false; + } + js << ", \"iters\": " << p.iters << "}" << (k ? "" : ",") << "\n"; + } + js << " ],\n \"ground\": {\"mu_left\": " << num(ground.mu_left) + << ", \"mu_right\": " << num(ground.mu_right) + << ", \"split_width\": " << num(ground.split_width) << ", \"A\": " << num(ground.A) + << ", \"cx\": " << num(ground.cx) << ", \"cy\": " << num(ground.cy) + << ", \"r\": " << num(ground.r) << "},\n"; + js << " \"g_out\": " << arr(g_out, n) << ",\n \"cases\": [\n"; + const std::size_t ncase = sizeof cases / sizeof cases[0]; + for (std::size_t c = 0; c < ncase; ++c) { + const gcase &gc = cases[c]; + const dyn4_params &p = psets[gc.params]; + bike_veh lim = limits(p.lf + p.lr); + lim.allow_reverse = gc.reverse ? 1 : 0; + dyn4_state s, o, go, gs; + for (int k = 0; k < n; ++k) { + s[k] = gc.s[k]; + go[k] = g_out[k]; + } + diff::dyn4_plant(p, lim, ground, s, gc.dcmd, gc.acmd, o); + double g_dc = 0.0, g_ac = 0.0; + diff::dyn4_plant_vjp(p, lim, ground, s, gc.dcmd, gc.acmd, go, gs, g_dc, g_ac); + double next[14], gstate[14]; + for (int k = 0; k < n; ++k) { + next[k] = o[k]; + gstate[k] = gs[k]; + now.push_back(o[k]); + } + for (int k = 0; k < n; ++k) + now.push_back(gs[k]); + now.push_back(g_dc); + now.push_back(g_ac); + js << " {\"params\": " << gc.params << ", \"state\": " << arr(gc.s, n) + << ", \"delta_cmd\": " << num(gc.dcmd) << ", \"a_cmd\": " << num(gc.acmd) + << ",\n \"limits\": {\"L\": " << num(lim.L) << ", \"vmax\": " << num(lim.vmax) + << ", \"hdt\": " << num(lim.hdt) << ", \"allow_reverse\": " << lim.allow_reverse << "}" + << ",\n \"next\": " << arr(next, n) << ",\n \"g_state\": " << arr(gstate, n) + << ",\n \"g_delta_cmd\": " << num(g_dc) << ", \"g_a_cmd\": " << num(g_ac) << "}" + << (c + 1 < ncase ? "," : "") << "\n"; + } + js << " ]\n}\n"; + for (const double x : now) + ASSERT_TRUE(std::isfinite(x)); + const char *write = std::getenv("CVC_DYN4_GOLDEN_WRITE"); + if (write && std::string(write) == "1") { + std::ofstream(path, std::ios::binary) << js.str(); + std::printf(" wrote %s\n", path.string().c_str()); + return; + } + boost::property_tree::ptree want; + ASSERT_NO_THROW(boost::property_tree::read_json(path.string(), want)) << path; + std::vector golden; + for (const auto &c : want.get_child("cases")) { + for (const char *key : {"next", "g_state"}) + for (const auto &v : c.second.get_child(key)) + golden.push_back(v.second.get_value()); + golden.push_back(c.second.get("g_delta_cmd")); + golden.push_back(c.second.get("g_a_cmd")); + } + ASSERT_EQ(golden.size(), now.size()); + for (std::size_t k = 0; k < now.size(); ++k) + EXPECT_LE(std::fabs(now[k] - golden[k]), + 1e-9 * std::max(1.0, std::max(std::fabs(now[k]), std::fabs(golden[k])))) + << "value " << k << " (case " << k / (2 * n + 2) << "): golden " << golden[k] << ", now " + << now[k]; +}