From f18027dee84153407a9214589e0ae902f6786a42 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Mon, 19 Oct 2026 15:42:55 -0500 Subject: [PATCH 1/5] nav: dyn4_plant, the differentiable two-track vehicle with suspension (P8d) diff::dyn4_plant (inc/cvc/nav/detail/dyn_4w.h) is P8a's vehicle on four contact patches: - the wheels at (lf, +-tf) and (-lr, +-tr), the front pair at the Ackermann angles of the virtual centre wheel; - each wheel's friction and terrain height from a ground sampler, templated on the scalar so the terrain's gradient flows through the duals; - each corner's spring-damper as its tire's load, softplus-floored; - Pacejka per tire within the friction ellipse, (vy, r) by P8a's implicit secant solve on four tires, and (z, roll, pitch) by an implicit 3x3 step on the corners' springs and dampers, driven by the ground and the tire forces' moments. The VJP is forward-mode duals (dyn4_plant_vjp); dyn4_bike_step / _bwd put bike_control in front. Parameters from the spec sheet: nav_plant::dyn4_si_of (P8a's numbers, the axles' half tracks, the chassis box's roll and pitch inertias, each corner's spring and damper from its axle's suspension frequency and damping ratio), dyn4_params_of and dyn4_params_at (inc/cvc/nav/dyn_4w.h). nav_dyn_4w_test (the Humvee): rests on flat ground; the single-track limit within 0.053 m of dyn_plant over 95 m; steady roll and braking load transfer exactly balanced; mu-split braking yaws toward the high-mu side; a bump loads its own wheel (1.37x against 1.11x); the VJP against central differences 3.7e-9; one step under the controller 6.2e-5; lateral acceleration <= 0.97 mu g and stopping within 8% of v^2/(2 mu g); within 4% and 7% of Jolt's Humvee skidpad and stopping. Documented in NAV_TRAINING.md. 372/372 nav/physics/plant ctest cases locally. --- docs/NAV_TRAINING.md | 73 ++++ inc/cvc/nav/detail/dyn_4w.h | 404 ++++++++++++++++++++++ inc/cvc/nav/dyn_4w.h | 86 +++++ inc/cvc/physics/nav_plant.h | 12 + src/cvc/physics/nav_plant.cpp | 46 +++ src/cvc/tests/CMakeLists.txt | 11 + src/cvc/tests/nav_dyn_4w_test.cpp | 549 ++++++++++++++++++++++++++++++ 7 files changed, 1181 insertions(+) create mode 100644 inc/cvc/nav/detail/dyn_4w.h create mode 100644 inc/cvc/nav/dyn_4w.h create mode 100644 src/cvc/tests/nav_dyn_4w_test.cpp diff --git a/docs/NAV_TRAINING.md b/docs/NAV_TRAINING.md index 2fe81b493..46a506390 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) @@ -349,6 +350,78 @@ 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, or cvcdbg's +comm-network trainer, 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): + +| 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.053 m over 95 m; velocities within 0.003 m/s | +| A steady turn: the roll moment the springs carry against h x the lateral force | -5082 against -5082 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.16 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 3.7e-9 | +| One step under the controller (a RoboRacer in the trainer's city, float) | directional relative error 6.2e-5 | +| Steady lateral acceleration, mu 0.4 / 0.9 | 0.97 / 0.95 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.56 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) | + +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); +- a GRL-SNAM torch twin and its goldens; +- 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 000000000..d0f820493 --- /dev/null +++ b/inc/cvc/nav/detail/dyn_4w.h @@ -0,0 +1,404 @@ +/* + 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, or cvcdbg's comm-network trainer, 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); + } +} + +// 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 Ackermann angles: tan delta_i = L tan delta / (L - y_i tan delta) at the front. + 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 ? s_atan(L * td / (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 000000000..999835864 --- /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 cc6b84478..db9762895 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 @@ -140,6 +141,17 @@ 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, 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 = 0.85, + double drive_lag = 0.1, 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 3495e9207..6432362fb 100644 --- a/src/cvc/physics/nav_plant.cpp +++ b/src/cvc/physics/nav_plant.cpp @@ -342,6 +342,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 a530fd407..b41cfe95b 100644 --- a/src/cvc/tests/CMakeLists.txt +++ b/src/cvc/tests/CMakeLists.txt @@ -107,6 +107,7 @@ 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(nav_dyn_bike_test nav_dyn_bike_test.cpp) +add_executable(nav_dyn_4w_test nav_dyn_4w_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) @@ -279,6 +280,7 @@ set(TEST_TARGETS physics_energy_test nav_plant_test nav_dyn_bike_test + nav_dyn_4w_test nav_terrain_test nav_binding_test physics_parity_test @@ -1561,6 +1563,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_terrain_test PRIVATE cvc @@ -1886,6 +1895,7 @@ 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(nav_dyn_bike_test PRIVATE cxx_std_17) +target_compile_features(nav_dyn_4w_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) @@ -2328,6 +2338,7 @@ cvc_discover_tests(physics_vehicle_test) cvc_discover_tests(physics_energy_test) cvc_discover_tests(nav_plant_test) cvc_discover_tests(nav_dyn_bike_test) +cvc_discover_tests(nav_dyn_4w_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/nav_dyn_4w_test.cpp b/src/cvc/tests/nav_dyn_4w_test.cpp new file mode 100644 index 000000000..140dcea82 --- /dev/null +++ b/src/cvc/tests/nav_dyn_4w_test.cpp @@ -0,0 +1,549 @@ +/* + 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 + +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); + 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); + } + 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); +} From 84b8e13324b38cc857d04d207ed947511c5d5f78 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Mon, 19 Oct 2026 15:46:01 -0500 Subject: [PATCH 2/5] nav: clang-format dyn_4w.h and nav_dyn_4w_test.cpp --- inc/cvc/nav/detail/dyn_4w.h | 7 +++---- src/cvc/tests/nav_dyn_4w_test.cpp | 4 ++-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/inc/cvc/nav/detail/dyn_4w.h b/inc/cvc/nav/detail/dyn_4w.h index d0f820493..ccec37da1 100644 --- a/inc/cvc/nav/detail/dyn_4w.h +++ b/inc/cvc/nav/detail/dyn_4w.h @@ -30,7 +30,8 @@ // 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), +// 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. @@ -115,9 +116,7 @@ CVC_HD inline float dyn4_rigid_k(float m_corner, float hdt) { 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); -} +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. diff --git a/src/cvc/tests/nav_dyn_4w_test.cpp b/src/cvc/tests/nav_dyn_4w_test.cpp index 140dcea82..ad517421e 100644 --- a/src/cvc/tests/nav_dyn_4w_test.cpp +++ b/src/cvc/tests/nav_dyn_4w_test.cpp @@ -84,8 +84,8 @@ struct bump_ground { 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)) { + 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; From 368c0e4a8be960c93fd0e5ef70f506bdd1f5c20b Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Mon, 19 Oct 2026 16:00:56 -0500 Subject: [PATCH 3/5] nav: dyn4's Ackermann gate and the golden for its torch twin (P8d) dyn4_ackermann(L, y, tan delta) is the front wheel's angle the plant uses. TheFrontWheelsFollowAckermann is Jolt's P2 gate (cot(right) - cot(left) = track / wheelbase, exact) plus the turn it gives: held at delta at 3 m/s with the low-speed blend off, the rear axle's circle is L / tan(delta) within +0.10 / +0.17 / +0.32% at 0.15 / 0.30 / 0.45 rad. Dyn4.Golden writes tests/data/dyn4_golden.json (CVC_DYN4_GOLDEN_WRITE=1): the sprung and the rigid Humvee on a mu split with a bump, eleven cases' next states and VJPs, which this build reproduces to 1e-9 and GRL-SNAM's torch twin is held to. --- docs/NAV_TRAINING.md | 5 +- inc/cvc/nav/detail/dyn_4w.h | 10 +- src/cvc/tests/data/dyn4_golden.json | 67 ++++++++ src/cvc/tests/nav_dyn_4w_test.cpp | 232 ++++++++++++++++++++++++++++ 4 files changed, 311 insertions(+), 3 deletions(-) create mode 100644 src/cvc/tests/data/dyn4_golden.json diff --git a/docs/NAV_TRAINING.md b/docs/NAV_TRAINING.md index 46a506390..c1e2c8c7a 100644 --- a/docs/NAV_TRAINING.md +++ b/docs/NAV_TRAINING.md @@ -411,6 +411,9 @@ force `a` behind their actuators. | Steady lateral acceleration, mu 0.4 / 0.9 | 0.97 / 0.95 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.56 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.10 / +0.17 / +0.32% 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 @@ -419,7 +422,7 @@ controller check above went from 5e-2 to 6.2e-5. Not yet: - the external-force port on this plant (with transfix/libcvc#728); -- a GRL-SNAM torch twin and its goldens; +- the GRL-SNAM torch twin, held to the golden above; - the anti-roll bars, the unsprung masses and wheel spin. ## Scene source diff --git a/inc/cvc/nav/detail/dyn_4w.h b/inc/cvc/nav/detail/dyn_4w.h index ccec37da1..2da504dcf 100644 --- a/inc/cvc/nav/detail/dyn_4w.h +++ b/inc/cvc/nav/detail/dyn_4w.h @@ -165,6 +165,12 @@ CVC_HD inline void dyn4_wheel_state(const dyn4_params &p, const bike_veh &v, con } } +// 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, @@ -194,14 +200,14 @@ CVC_HD inline void dyn4_plant(const dyn4_params &p, const bike_veh &v, const G & 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 Ackermann angles: tan delta_i = L tan delta / (L - y_i tan delta) at the front. + // 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 ? s_atan(L * td / (L - yi[i] * td)) : zero; + dw[i] = i < 2 ? dyn4_ackermann(L, yi[i], td) : zero; cdw[i] = s_cos(dw[i]); sdw[i] = s_sin(dw[i]); } diff --git a/src/cvc/tests/data/dyn4_golden.json b/src/cvc/tests/data/dyn4_golden.json new file mode 100644 index 000000000..f14b9d462 --- /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": 36.121696472167969, "B_r": 42.496116638183594, "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": 36.121696472167969, "B_r": 42.496116638183594, "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.72163109892909338, 0.0027127276838284208, 0.0058889244396855478, 12.027242952884908, 0.14742080409674607, 0.098148742855221155, 1.1237378706555782e-06, 1.8728964929551291e-05, 0.0049169753148895108, 0.081949590413207929, -0.00044538681808697136, -0.0074231138007023788, 0.035978411112368727, 0.72559417581953878], + "g_state": [0.70000845938790757, -0.2997789477045501, 0.8820732682607586, 0.44312091145800947, -0.4925711709931741, 0.21922008644856103, 1.3030806186537625, -0.069309658165976826, -0.56302762318656074, 0.13841518326797711, -0.94129370266309165, 0.053660460669990528, -0.30373309173264523, -0.068287200865146447], + "g_delta_cmd": -0.12663233907217267, "g_a_cmd": -0.056140188876111045}, + {"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.8733965671167805, -1.7882058269684011, 0.32749468726116926, 14.918414006317281, -0.53227361235225956, 0.45824479792872391, 0.0079183275375169729, -0.034694541816867229, 0.037515284510706501, 0.29192141503672703, -0.0069017195809452231, 0.051638008138445719, 0.2195453364485479, 0], + "g_state": [0.70000000074478008, -0.30245335370142151, 0.68187759221869082, 0.45813632607380178, -0.91508696847319237, 1.1311165836919064, 3.6855836384438758, 0.2223727559172643, -0.67039457346825826, 0.12527058226039786, 3.288652008960633, 0.57151952168371545, 0.44759840068430301, -0.0679055872778591], + "g_delta_cmd": 0.046144741379054922, "g_a_cmd": -0.055826457187059836}, + {"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.6217011934166194, 3.2687694820288757, -1.087524766248477, 13.721282525430517, 0.28637518945986973, 0.20792056717276949, -0.0039591637523130833, 0.017347271182523681, -0.0057452904957051975, 0.070911826656582883, 0.018192649196857116, -0.030122514059005404, 0.10000000000000001, -4.9023767032781551], + "g_state": [0.69999999999999996, -0.29613375116461865, 1.5010382520025809, 0.44437521459993867, -0.65428328308470851, 0.46626939827314468, 1.9903043729384213, 0.014825003237494805, -0.71051985569712739, 0.12035816909052562, -1.1287000539312668, 0.03071688480458868, 0.17862001245922571, -0.08162470594451543], + "g_delta_cmd": 0.41441928713513987, "g_a_cmd": -0.067105202008960599}, + {"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.1791378256759428, 3.5247550029652142, 0.058517586554974022, 7.9960914861031691, 0.19511909995235585, 0.14195977908928203, 0.0020681050122793492, 0.03446841764175166, 0.0038744122711175271, 0.064573539295289861, -0.0014354355715143782, -0.023923926726647737, 0.069545336448547879, 0.22559417581953883], + "g_state": [0.70356339738855245, -0.32151265773690474, 0.86972007327801926, 0.44206479565118351, -0.50625612637799589, 0.18259168757573621, 1.4036257266363981, -0.057000234141410561, -0.5231539649742446, 0.14329679091094208, -0.86828165364232024, 0.06259909806229319, -0.19791671329915439, -0.068463577143273252], + "g_delta_cmd": -0.020404039728067598, "g_a_cmd": -0.056285191123118632}, + {"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.9386802012423319, 0.12160607918483068, 0.019684132917521507, 17.692324773487073, 0.0025868156507929908, -0.0052644514923111995, -1.7708927936164909e-10, -2.951488055331717e-09, -0.00012475390530029479, -0.0020792318014793655, 0.011569695398484485, 0.026161590559498529, 0, -5.2559417581953882], + "g_state": [0.69999999999999996, -0.29988357587132003, 0.76701767325443215, 0.4407203793877052, -0.60647836155181134, 0.48424535898248711, 1.2556505011824981, -0.075116379468836883, -0.43688402983214147, 0.15385855009661809, -1.1725496351011697, 0.025348518028219677, 0.083628449572683131, -0.071412193302551691], + "g_delta_cmd": 0.19402776866360219, "g_a_cmd": -0.058709303782706848}, + {"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.035978224695669254, 7.8081977973431879e-05, 0.0036246756893130671, 0.59963786959080445, 0.10563292962679138, 0.060411262838848068, 1.123724685176085e-06, 1.8728745171554826e-05, 0.0036228270272335686, 0.06038045180350108, -6.7928721306615894e-05, -0.0011321453804156861, 0.31954533644854788, 0.39023767032781553], + "g_state": [0.70001053974965066, -0.2999640753925496, 1.0892551860334572, 0.33890845124895652, -0.17534856892921305, 0.078866476739045135, 1.1550504899660954, -0.087432525244901513, -0.52472959702914301, 0.14310389119306263, -0.91016909201952712, 0.057470949839098201, 0.33794784682064766, -0.071522513657545125], + "g_delta_cmd": 0.034840419374389746, "g_a_cmd": -0.058800000216119627}, + {"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.38195840865180714, 0.47840775241079153, 0.19289720870573102, -1.9998595522896305, -0.18475082146653074, -0.11837985755047914, 0.00039472712905614678, 0.0065787856313164307, -0.00037566480050625281, -0.17292775054034065, -0.00015308820464018688, -0.0025514701343662496, 0.2195453364485479, -0.63535650549172329], + "g_state": [0.70420950635366553, -0.30449033205848602, 1.1307740193593281, 0.41185168135905487, -0.45015816491923188, -0.078518139588756086, 0.81465603022538913, -0.12910595761612004, -0.50161102315083805, 0.1459342260937567, -0.67810873469641653, 0.085881376086457073, 0.8596739366222137, -0.070046096287712842], + "g_delta_cmd": 0.088627286011518758, "g_a_cmd": -0.057586209799288232}, + {"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.4902152892803535, 2.3340141722742125, 2.5696079841077641, 10.157398970499345, 1.9636287371126497, 1.1601330943937327, 0.020754548567750952, 0.012575809743607093, 0.036699065106174086, -0.055015582793459326, 0.0011981841856031474, 0.019969736873077525, 0.41954533644854791, 1.4511883516390776], + "g_state": [0.69630206391467186, -0.30067417911736455, 1.0214387256908726, 0.39993283960685022, -0.64523853589014846, 0.35062264195896164, 1.7812734195615509, -0.010766004648815291, -0.51843529593162418, 0.14387448280594123, -0.94804300061694113, 0.052834164704907302, 0.46402402843022839, -0.067408113478903797], + "g_delta_cmd": 0.047838126215027463, "g_a_cmd": -0.055417474644497866}, + {"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.0032481979591471731, 4.8372584893719119e-08, 2.0625612237786974e-05, 0.054136633867614234, 0.0006057388656155033, 0.00034376021164675571, 1.1237433377405803e-06, 1.8729056047636695e-05, 2.6845568946861884e-05, 0.00044742615911511871, -0.00060189197217464137, -0.010031533093799594, 0.020946006673345115, 0.90237670327815533], + "g_state": [0.70001059340442395, -0.29996837172804014, 1.0991202417172341, 0.42866769277290073, -0.22191632627756169, 0.0075129776321974616, 1.1516621102278115, -0.08784735401093885, -0.52402769522888448, 0.14318982284185508, -0.90926746976154682, 0.057581332643911465, 0.58343049298677863, -0.068644632749062864], + "g_delta_cmd": 0.0021013213545767098, "g_a_cmd": -0.056434040333180419}, + {"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.8735265964795507, -1.7896031910519152, 0.32902486345977516, 14.910871487592926, -0.53287851918997664, 0.48374773514219088, -2.1132274580139618e-05, -0.0003522045842080462, 0.0075613580626087071, 0.12602263719363724, 0.00024935587984354761, 0.0041559314236180997, 0.2195453364485479, 0], + "g_state": [0.69999999948112257, -0.29184243155908923, 0.71319702455726042, 0.4594194668991195, -0.91704147061531727, 1.1358794954951639, 14.889206777005246, 1.5763531981775658, -1.6765094153238447, -0.023884123061825743, 18.244178225990495, 2.3456783201274147, 0.4609536472203607, -0.068001625204078597], + "g_delta_cmd": 0.047521588116035483, "g_a_cmd": -0.055905411767844013}, + {"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.1679073424592845, 3.5241181026862374, 0.058723013792275888, 7.8086049087102118, 0.19990106338159228, 0.14538356645417397, 0.0036065836355641647, 0.060109728602959878, -0.0015803389824868667, -0.02633898363016994, -0.001424414632490402, -0.023740244405475848, 0.069545336448547879, -2.9023767032781551], + "g_state": [0.70655864148715697, -0.33754149980399722, 0.82213749022949278, 0.43728049796338603, -0.40688686858379097, 0.20131319427365937, 2.8545555251391117, 0.13219512384477763, -0.90668420016993723, 0.068494897902084462, -0.82545382635621556, 0.057322594099918855, -0.69280663260984598, -0.07151098108506794], + "g_delta_cmd": -0.071424256294480681, "g_a_cmd": -0.058790519071938961} + ] +} diff --git a/src/cvc/tests/nav_dyn_4w_test.cpp b/src/cvc/tests/nav_dyn_4w_test.cpp index ad517421e..fabc95d94 100644 --- a/src/cvc/tests/nav_dyn_4w_test.cpp +++ b/src/cvc/tests/nav_dyn_4w_test.cpp @@ -14,15 +14,23 @@ // 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; @@ -547,3 +555,227 @@ TEST(Dyn4, MatchesJoltsHumveeEnvelopes) { 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]; +} From a15b2280d8c5ee82547ea53bf711c094ad8a4405 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 03:54:08 -0500 Subject: [PATCH 4/5] nav: word the external-force comments by capability The comments and docs describe ext_force as a hook for a caller's own force, without naming a downstream user. --- docs/NAV_TRAINING.md | 4 ++-- inc/cvc/nav/detail/dyn_4w.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/NAV_TRAINING.md b/docs/NAV_TRAINING.md index c1e2c8c7a..55796397f 100644 --- a/docs/NAV_TRAINING.md +++ b/docs/NAV_TRAINING.md @@ -353,8 +353,8 @@ Not 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, or cvcdbg's -comm-network trainer, trains through per-wheel grip, the terrain under each wheel, and the load +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 diff --git a/inc/cvc/nav/detail/dyn_4w.h b/inc/cvc/nav/detail/dyn_4w.h index 2da504dcf..4496d6fba 100644 --- a/inc/cvc/nav/detail/dyn_4w.h +++ b/inc/cvc/nav/detail/dyn_4w.h @@ -10,7 +10,7 @@ // 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, or cvcdbg's comm-network trainer, trains through +// 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. // From 8ef864171109dbf1d0a89347cdb14d6c28a4a44f Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 20 Oct 2026 23:08:22 -0500 Subject: [PATCH 5/5] nav: dyn4_si_of follows dyn_bike_si_of's Jolt-fitted defaults (#753); golden regenerated #753 fitted dyn_bike_si_of's tires to Jolt and made its defaults front_stiffness 1 and drive_lag by drivetrain. dyn4_si_of still passed the old 0.85 and 0.1, so the four-wheel Humvee's front B was 0.85x the single track's and four dyn4 tests failed. Its defaults are now 1.0 and -1.0, and dyn4_golden.json is regenerated with the new tires. The envelope test measures the body's lateral acceleration (vy' + r vx), as DynBike.SteadyLateralNeverExceedsMuG does since #753: on the softer tires r vx alone read 1.03 mu g at mu 0.4. NAV_TRAINING.md's P8a parameters (stale since #753) and dyn4's gate numbers are updated. nav_dyn_4w_test 12/12. --- docs/NAV_TRAINING.md | 33 +++++++------- inc/cvc/physics/nav_plant.h | 7 +-- src/cvc/tests/data/dyn4_golden.json | 70 ++++++++++++++--------------- src/cvc/tests/nav_dyn_4w_test.cpp | 10 +++-- 4 files changed, 64 insertions(+), 56 deletions(-) diff --git a/docs/NAV_TRAINING.md b/docs/NAV_TRAINING.md index 55796397f..a6fab3834 100644 --- a/docs/NAV_TRAINING.md +++ b/docs/NAV_TRAINING.md @@ -167,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): @@ -396,23 +398,24 @@ force `a` behind their actuators. - `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): +**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.053 m over 95 m; velocities within 0.003 m/s | -| A steady turn: the roll moment the springs carry against h x the lateral force | -5082 against -5082 N m; the outside wheels loaded | +| 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.16 rad in 1.8 s) | +| 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 3.7e-9 | -| One step under the controller (a RoboRacer in the trainer's city, float) | directional relative error 6.2e-5 | -| Steady lateral acceleration, mu 0.4 / 0.9 | 0.97 / 0.95 mu g | +| 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.56 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) | +| 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.10 / +0.17 / +0.32% at 0.15 / 0.30 / 0.45 rad | +| 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 diff --git a/inc/cvc/physics/nav_plant.h b/inc/cvc/physics/nav_plant.h index 8b9684b89..78ea75330 100644 --- a/inc/cvc/physics/nav_plant.h +++ b/inc/cvc/physics/nav_plant.h @@ -152,14 +152,15 @@ 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, plus + // 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 = 0.85, - double drive_lag = 0.1, double gravity = 9.80665); + 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); diff --git a/src/cvc/tests/data/dyn4_golden.json b/src/cvc/tests/data/dyn4_golden.json index f14b9d462..02482a087 100644 --- a/src/cvc/tests/data/dyn4_golden.json +++ b/src/cvc/tests/data/dyn4_golden.json @@ -2,66 +2,66 @@ "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": 36.121696472167969, "B_r": 42.496116638183594, "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": 36.121696472167969, "B_r": 42.496116638183594, "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": 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.72163109892909338, 0.0027127276838284208, 0.0058889244396855478, 12.027242952884908, 0.14742080409674607, 0.098148742855221155, 1.1237378706555782e-06, 1.8728964929551291e-05, 0.0049169753148895108, 0.081949590413207929, -0.00044538681808697136, -0.0074231138007023788, 0.035978411112368727, 0.72559417581953878], - "g_state": [0.70000845938790757, -0.2997789477045501, 0.8820732682607586, 0.44312091145800947, -0.4925711709931741, 0.21922008644856103, 1.3030806186537625, -0.069309658165976826, -0.56302762318656074, 0.13841518326797711, -0.94129370266309165, 0.053660460669990528, -0.30373309173264523, -0.068287200865146447], - "g_delta_cmd": -0.12663233907217267, "g_a_cmd": -0.056140188876111045}, + "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.8733965671167805, -1.7882058269684011, 0.32749468726116926, 14.918414006317281, -0.53227361235225956, 0.45824479792872391, 0.0079183275375169729, -0.034694541816867229, 0.037515284510706501, 0.29192141503672703, -0.0069017195809452231, 0.051638008138445719, 0.2195453364485479, 0], - "g_state": [0.70000000074478008, -0.30245335370142151, 0.68187759221869082, 0.45813632607380178, -0.91508696847319237, 1.1311165836919064, 3.6855836384438758, 0.2223727559172643, -0.67039457346825826, 0.12527058226039786, 3.288652008960633, 0.57151952168371545, 0.44759840068430301, -0.0679055872778591], - "g_delta_cmd": 0.046144741379054922, "g_a_cmd": -0.055826457187059836}, + "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.6217011934166194, 3.2687694820288757, -1.087524766248477, 13.721282525430517, 0.28637518945986973, 0.20792056717276949, -0.0039591637523130833, 0.017347271182523681, -0.0057452904957051975, 0.070911826656582883, 0.018192649196857116, -0.030122514059005404, 0.10000000000000001, -4.9023767032781551], - "g_state": [0.69999999999999996, -0.29613375116461865, 1.5010382520025809, 0.44437521459993867, -0.65428328308470851, 0.46626939827314468, 1.9903043729384213, 0.014825003237494805, -0.71051985569712739, 0.12035816909052562, -1.1287000539312668, 0.03071688480458868, 0.17862001245922571, -0.08162470594451543], - "g_delta_cmd": 0.41441928713513987, "g_a_cmd": -0.067105202008960599}, + "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.1791378256759428, 3.5247550029652142, 0.058517586554974022, 7.9960914861031691, 0.19511909995235585, 0.14195977908928203, 0.0020681050122793492, 0.03446841764175166, 0.0038744122711175271, 0.064573539295289861, -0.0014354355715143782, -0.023923926726647737, 0.069545336448547879, 0.22559417581953883], - "g_state": [0.70356339738855245, -0.32151265773690474, 0.86972007327801926, 0.44206479565118351, -0.50625612637799589, 0.18259168757573621, 1.4036257266363981, -0.057000234141410561, -0.5231539649742446, 0.14329679091094208, -0.86828165364232024, 0.06259909806229319, -0.19791671329915439, -0.068463577143273252], - "g_delta_cmd": -0.020404039728067598, "g_a_cmd": -0.056285191123118632}, + "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.9386802012423319, 0.12160607918483068, 0.019684132917521507, 17.692324773487073, 0.0025868156507929908, -0.0052644514923111995, -1.7708927936164909e-10, -2.951488055331717e-09, -0.00012475390530029479, -0.0020792318014793655, 0.011569695398484485, 0.026161590559498529, 0, -5.2559417581953882], - "g_state": [0.69999999999999996, -0.29988357587132003, 0.76701767325443215, 0.4407203793877052, -0.60647836155181134, 0.48424535898248711, 1.2556505011824981, -0.075116379468836883, -0.43688402983214147, 0.15385855009661809, -1.1725496351011697, 0.025348518028219677, 0.083628449572683131, -0.071412193302551691], - "g_delta_cmd": 0.19402776866360219, "g_a_cmd": -0.058709303782706848}, + "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.035978224695669254, 7.8081977973431879e-05, 0.0036246756893130671, 0.59963786959080445, 0.10563292962679138, 0.060411262838848068, 1.123724685176085e-06, 1.8728745171554826e-05, 0.0036228270272335686, 0.06038045180350108, -6.7928721306615894e-05, -0.0011321453804156861, 0.31954533644854788, 0.39023767032781553], - "g_state": [0.70001053974965066, -0.2999640753925496, 1.0892551860334572, 0.33890845124895652, -0.17534856892921305, 0.078866476739045135, 1.1550504899660954, -0.087432525244901513, -0.52472959702914301, 0.14310389119306263, -0.91016909201952712, 0.057470949839098201, 0.33794784682064766, -0.071522513657545125], - "g_delta_cmd": 0.034840419374389746, "g_a_cmd": -0.058800000216119627}, + "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.38195840865180714, 0.47840775241079153, 0.19289720870573102, -1.9998595522896305, -0.18475082146653074, -0.11837985755047914, 0.00039472712905614678, 0.0065787856313164307, -0.00037566480050625281, -0.17292775054034065, -0.00015308820464018688, -0.0025514701343662496, 0.2195453364485479, -0.63535650549172329], - "g_state": [0.70420950635366553, -0.30449033205848602, 1.1307740193593281, 0.41185168135905487, -0.45015816491923188, -0.078518139588756086, 0.81465603022538913, -0.12910595761612004, -0.50161102315083805, 0.1459342260937567, -0.67810873469641653, 0.085881376086457073, 0.8596739366222137, -0.070046096287712842], - "g_delta_cmd": 0.088627286011518758, "g_a_cmd": -0.057586209799288232}, + "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.4902152892803535, 2.3340141722742125, 2.5696079841077641, 10.157398970499345, 1.9636287371126497, 1.1601330943937327, 0.020754548567750952, 0.012575809743607093, 0.036699065106174086, -0.055015582793459326, 0.0011981841856031474, 0.019969736873077525, 0.41954533644854791, 1.4511883516390776], - "g_state": [0.69630206391467186, -0.30067417911736455, 1.0214387256908726, 0.39993283960685022, -0.64523853589014846, 0.35062264195896164, 1.7812734195615509, -0.010766004648815291, -0.51843529593162418, 0.14387448280594123, -0.94804300061694113, 0.052834164704907302, 0.46402402843022839, -0.067408113478903797], - "g_delta_cmd": 0.047838126215027463, "g_a_cmd": -0.055417474644497866}, + "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.0032481979591471731, 4.8372584893719119e-08, 2.0625612237786974e-05, 0.054136633867614234, 0.0006057388656155033, 0.00034376021164675571, 1.1237433377405803e-06, 1.8729056047636695e-05, 2.6845568946861884e-05, 0.00044742615911511871, -0.00060189197217464137, -0.010031533093799594, 0.020946006673345115, 0.90237670327815533], - "g_state": [0.70001059340442395, -0.29996837172804014, 1.0991202417172341, 0.42866769277290073, -0.22191632627756169, 0.0075129776321974616, 1.1516621102278115, -0.08784735401093885, -0.52402769522888448, 0.14318982284185508, -0.90926746976154682, 0.057581332643911465, 0.58343049298677863, -0.068644632749062864], - "g_delta_cmd": 0.0021013213545767098, "g_a_cmd": -0.056434040333180419}, + "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.8735265964795507, -1.7896031910519152, 0.32902486345977516, 14.910871487592926, -0.53287851918997664, 0.48374773514219088, -2.1132274580139618e-05, -0.0003522045842080462, 0.0075613580626087071, 0.12602263719363724, 0.00024935587984354761, 0.0041559314236180997, 0.2195453364485479, 0], - "g_state": [0.69999999948112257, -0.29184243155908923, 0.71319702455726042, 0.4594194668991195, -0.91704147061531727, 1.1358794954951639, 14.889206777005246, 1.5763531981775658, -1.6765094153238447, -0.023884123061825743, 18.244178225990495, 2.3456783201274147, 0.4609536472203607, -0.068001625204078597], - "g_delta_cmd": 0.047521588116035483, "g_a_cmd": -0.055905411767844013}, + "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.1679073424592845, 3.5241181026862374, 0.058723013792275888, 7.8086049087102118, 0.19990106338159228, 0.14538356645417397, 0.0036065836355641647, 0.060109728602959878, -0.0015803389824868667, -0.02633898363016994, -0.001424414632490402, -0.023740244405475848, 0.069545336448547879, -2.9023767032781551], - "g_state": [0.70655864148715697, -0.33754149980399722, 0.82213749022949278, 0.43728049796338603, -0.40688686858379097, 0.20131319427365937, 2.8545555251391117, 0.13219512384477763, -0.90668420016993723, 0.068494897902084462, -0.82545382635621556, 0.057322594099918855, -0.69280663260984598, -0.07151098108506794], - "g_delta_cmd": -0.071424256294480681, "g_a_cmd": -0.058790519071938961} + "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 index fabc95d94..33c850c9e 100644 --- a/src/cvc/tests/nav_dyn_4w_test.cpp +++ b/src/cvc/tests/nav_dyn_4w_test.cpp @@ -398,17 +398,21 @@ TEST(Dyn4, EnvelopesOnFourWheels) { 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; + // 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 += std::fabs(s.vx * s.r) / tail; + ay += s.vx * s.r / tail; } - peak = std::max(peak, ay); + 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));