diff --git a/inc/cvc/nav/detail/dyn_4w.h b/inc/cvc/nav/detail/dyn_4w.h index 63d0174db..e79eed61f 100644 --- a/inc/cvc/nav/detail/dyn_4w.h +++ b/inc/cvc/nav/detail/dyn_4w.h @@ -39,6 +39,11 @@ // 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. +// With grade_force (off by default) gravity along the ground acts on the body as well: each +// spring's horizontal reaction on the slope under its wheel, -F_i grad h there, F_i its +// linear spring force (step 5's) and grad h the ground's, by central differences over +// 1e-3 L (dyn4_grade). It acts on (vx, vy, r) only, from the step's start. Without it the +// ground only lifts the springs: the vehicle does not roll down a hill. // 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 @@ -54,6 +59,9 @@ // check the two agree). // // Left out: wheel spin, the unsprung masses, camber and the anti-roll bars. +// +// The plant in port-Hamiltonian form, and each step's energy books: dyn4_ph_* and +// dyn4_plant_books, below. #ifndef __CVC_NAV_DETAIL_DYN_4W_H__ #define __CVC_NAV_DETAIL_DYN_4W_H__ @@ -86,6 +94,7 @@ struct dyn4_params { 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; + int grade_force = 0; // 1: gravity along the ground's slope acts on the body (opt-in; see step 3) }; template struct dyn4_state { @@ -171,10 +180,41 @@ 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. +// Gravity along the ground, for grade_force: each spring's horizontal reaction on the slope under +// its wheel, -F_i grad h(P_i), as a force and yaw moment on the body, f = (F_x, F_y, M_z) on +// (vx, vy, r). F_i = Fz0_i + k_i (hg_i - c_i . q) is the spring's linear force, so f is minus the +// gradient of the springs' energy in the pose (dyn4_ph_hamiltonian): conservative. The slope is +// the ground's, by central differences over 1e-3 L, so any sampler serves. w is +// dyn4_wheel_state's at s. +template +CVC_HD inline void dyn4_grade(const dyn4_params &p, const G &ground, const dyn4_state &s, + const dyn4_wheels &w, S f[3]) { + const S lr = S(p.lr), e = S(1e-3) * (S(p.lf) + lr); + const S cps = s_cos(s.psi), sps = s_sin(s.psi); + const S cgx = s.x + lr * cps, cgy = s.y + lr * sps; + f[0] = f[1] = f[2] = S(0); + for (int i = 0; i < 4; ++i) { + const S wx = cgx + w.xi[i] * cps - w.yi[i] * sps, wy = cgy + w.xi[i] * sps + w.yi[i] * cps; + S mu, h_xp, h_xm, h_yp, h_ym; + ground.at(wx + e, wy, mu, h_xp); + ground.at(wx - e, wy, mu, h_xm); + ground.at(wx, wy + e, mu, h_yp); + ground.at(wx, wy - e, mu, h_ym); + const S gx = (h_xp - h_xm) / (S(2) * e), gy = (h_yp - h_ym) / (S(2) * e); + const S Fs = w.Fz0[i] + w.k[i] * (w.hg[i] - (s.z + w.yi[i] * s.phi - w.xi[i] * s.th)); + const S fx = -Fs * (cps * gx + sps * gy), fy = -Fs * (cps * gy - sps * gx); + f[0] = f[0] + fx; + f[1] = f[1] + fy; + f[2] = f[2] + w.xi[i] * fy - w.yi[i] * fx; + } +} + +// One step of the plant; see the header comment. `seen`, when given, gets the twist the step +// reached before its limiters (dyn_step_free, for dyn4_plant_books). 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 dyn4_state &s, S delta_cmd, S a_cmd, dyn4_state &o, + dyn_step_free *seen = nullptr) { 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; @@ -195,6 +235,10 @@ CVC_HD inline void dyn4_plant(const dyn4_params &p, const bike_veh &v, const G & 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; + // Gravity along the ground, when asked. + S Fg[3] = {zero, zero, zero}; + if (p.grade_force) + dyn4_grade(p, ground, s, W, Fg); // 3. Longitudinal: each wheel's drive or brake force within its grip, and what that leaves. const S F_drive = m * a1; @@ -218,7 +262,10 @@ CVC_HD inline void dyn4_plant(const dyn4_params &p, const bike_veh &v, const G & 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); + S FXpred = FXlong + F_res; + if (p.grade_force) + FXpred = FXpred + Fg[0]; + const S vx_p = dyn_clamp(s.vx + hdt * (FXpred / 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; @@ -252,6 +299,10 @@ CVC_HD inline void dyn4_plant(const dyn4_params &p, const bike_veh &v, const G & J21 = J21 + (xi[i] * dfy_y - yi[i] * dfx_y) * dwdvy[i]; J22 = J22 + (xi[i] * dfy_y - yi[i] * dfx_y) * dwdr[i]; } + if (p.grade_force) { + FY = FY + Fg[1]; + MZ = MZ + Fg[2]; + } 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); @@ -272,7 +323,16 @@ CVC_HD inline void dyn4_plant(const dyn4_params &p, const bike_veh &v, const G & 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); + S FXv = FXb; // with gravity along the ground, which does not pitch the body (step 5) + if (p.grade_force) + FXv = FXb + Fg[0]; + const S vx1_free = s.vx + hdt * (FXv / m + r1 * vy1); + const S vx1 = dyn_clamp(vx1_free, sp_min, vmax); + if (seen) { + seen->vx = vx1_free; + seen->vy = vy1; + seen->r = r1; + } // 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) @@ -372,6 +432,362 @@ CVC_HD inline void dyn4_plant_vjp(const dyn4_params &p, const bike_veh &v, const } } +// ── The plant in port-Hamiltonian form ──────────────────────────────────────────────────── +// dyn_bike.h's form (dyn_ph_*) on four wheels with suspension, at every slip. The body's state is +// its vertical displacement q = (z, phi, th) and the momenta p = M xi of its twist +// xi = (vx, vy, r, zd, phid, thd), M = diag(m, m, Iz, m, Ix, Iy); the pose (x, y, psi) enters only +// through the ground under the wheels. In x = (q, p), nine coordinates, dyn4_plant's vector field +// (its step as hdt -> 0, the actuators holding) is exactly +// +// H = 1/2 xi^T M xi + sum_i 1/2 k_i l_i^2 + m g z, l_i = Fz0_i / k_i + hg_i - c_i . q +// x' = (J(p) - R) dH/dx + G u + f_lever, dH/dx = (dH/dq, xi), u = (F_x1, ..., F_x4), +// y = G^T dH/dx +// +// H the kinetic energy, the corners' springs (l_i a spring's compression; the vertical step's +// springs are linear, the softplus floor is only on the tires' loads; a rigid corner's k_i +// is dyn4_rigid_k's, set by hdt) and gravity on heave. sum_i c_i Fz0_i = (m g, 0, 0): the +// preloads hold the body at q = 0 on flat ground. +// J [[0, 0, I], [0, J_h(p_h), 0], [-I, 0, 0]] over (q, p_h, p_v): canonical in the vertical, +// SE(2)'s coadjoint action in the horizontal (dyn_ph_J). Skew, so it moves no energy. +// R blockdiag(0, R_h, R_v). R_h = c_x e_x e_x^T + sum_i S_i n_i n_i^T: c_x = drag u + +// c_rr m g / u, drag and rolling resistance on vx (u = sqrt(vx^2 + eps^2)); n_i = +// (-sin d_i, cos d_i, x_i cos d_i + y_i sin d_i) carries wheel i's lateral force onto +// (vx, vy, r), so n_i^T v is w_i, the lateral velocity in the wheel's own frame that its +// slip alpha_i = -atan2(w_i, u_i) is measured in; and S_i = -F_yi / w_i (dyn4_ph_secant) +// is the tire's secant through its root w_i = 0, >= 0 at any slip because a Pacejka force +// (C < 2) always opposes its slip. R_v = sum_i c'_i c_i c_i^T, the dampers. So R is +// positive semi-definite sliding or not, at any steering angle. +// G columns T_i = (cos d_i, sin d_i, x_i sin d_i - y_i cos d_i, 0, h sin d_i, -h cos d_i) on +// the twist: wheel i's drive or brake force (within its grip) along the wheel, at its +// contact patch h below the centre of gravity. y_i is that patch's speed along the wheel, +// roll and pitch included, and y^T u the power the forces deliver. Steering is not an +// input: the wheels' angles d_i only turn n_i and T_i, so they do no work. +// f_lever (0, h sum_i F_yi cos d_i, h sum_i F_yi sin d_i) on (p_z, p_phi, p_th): the tires' +// lateral forces' moments on roll and pitch. The slips do not see the body's roll and +// pitch rates (a patch's lateral velocity has no h phid in it), so this coupling is +// one-way: neither skew nor dissipative. It is booked on its own line and vanishes with h. +// f_eps = 0. Each slip is measured in its wheel's frame and rooted at w_i = 0, so the secant +// carries the whole lateral force and nothing is left at the speed floor (dyn_bike's front +// slip, in the body's frame and rooted at the floored speed, leaves dyn_ph_floor). +// f_grade with grade_force on (dyn4_ph_grade): dyn4_grade's (F_x, F_y, M_z) on p_h, zero +// otherwise. It is minus the gradient of H's springs in the pose g, through the ground +// under the wheels: with g in the state, T*SE(2)'s canonical structure puts it in J and the +// terrain's energy is in H (where the single track can only book its grade as a power). +// Booked on its own line, its power cancels the ground's: +// xi^T f_grade = -sum_i k_i l_i hg_i'. +// +// So dH/dt = -dH/dx^T R dH/dx + y^T u + xi^T f_lever + xi^T f_grade + sum_i k_i l_i hg_i': passive +// with respect to the wheels' forces but for the lever, at every slip. The last term is the ground +// lifting the springs as the wheels roll over it (none on flat ground); without grade_force it is +// energy from outside, the vehicle climbing a hill for free, and with it the grade line cancels it. +// The inputs are the actuators' outputs (the state's delta and a). dyn4_plant's step departs from +// the vector field by its integrator (which holds the ground and the loads at the step's start) +// and by its limiters (the speed clamp, the low-speed blend); dyn4_plant_books charges the +// limiters and the ground's new heights their energy and leaves the integrator's error, second +// order per step, as the residual. These are evaluations: dyn4_plant's arithmetic does not depend +// on them. + +// A tire's secant stiffness through its root w = 0: -F / w, and D C B / u, its limit, at w = 0. +// dyn_secant_w (the implicit step's) is the same but where |B alpha| < 1e-4, where it takes the +// slope D C B u / (u^2 + w^2), within 1e-8 of this; the form takes the exact secant, so S w is +// the whole force. +template CVC_HD inline S dyn4_ph_secant(S F, S w, S B, S C, S D, S u) { + if (s_val(w) == 0.0) + return D * C * B / u; + return -F / w; +} + +// Everything the form is built from, at a state: the wheels (dyn4_wheel_state) and dyn4_plant's +// steps 3 and 4 at the state's own twist, delta and a. +template struct dyn4_ph_parts { + dyn4_wheels W; // offsets, static loads, springs, dampers, the ground under them, the loads + S cx; // drag and rolling resistance on vx + S Fx[4]; // each wheel's drive or brake force: the inputs + S Fy[4]; // each tire's lateral force + S w[4], ui[4]; // each wheel's lateral velocity in its own frame, and its floored speed + S Sw[4]; // the secants (dyn4_ph_secant) + S N[4][6]; // each tire's lateral force on the twist (vx, vy, r, zd, phid, thd) + S T[4][6]; // each wheel's drive or brake force on the twist +}; + +template +CVC_HD inline void dyn4_ph_parts_at(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, dyn4_ph_parts &q) { + const S m = S(p.m), g = S(p.g), h = S(p.h), Cp = S(p.C), L = S(p.lf) + S(p.lr); + const S zero = S(0), one = S(1); + dyn4_wheel_state(p, v, ground, s, q.W); + const S eps2 = S(p.eps) * S(p.eps); + const S u0 = s_sqrt(s.vx * s.vx + eps2); + q.cx = S(p.drag) * u0 + S(p.c_rr) * m * g / u0; + const S F_drive = m * s.a; + const S share = S(s_val(s.a) >= 0.0 ? p.front_drive : p.front_brake); + const S td = s_tan(s.delta); + const S Bw[4] = {S(p.B_f), S(p.B_f), S(p.B_r), S(p.B_r)}; + for (int i = 0; i < 4; ++i) { + const S xi = q.W.xi[i], yi = q.W.yi[i]; + const S cap = q.W.mu[i] * q.W.Fz[i]; + q.Fx[i] = dyn_sat(S(0.5) * F_drive * (i < 2 ? share : one - share), cap); + const S Dy = dyn_lateral_cap(q.Fx[i], cap); + const S dw = i < 2 ? dyn4_ackermann(L, yi, td) : zero; + const S cw = s_cos(dw), sw = s_sin(dw); + const S vxi = s.vx - s.r * yi, vyi = s.vy + s.r * xi; + q.w[i] = -vxi * sw + vyi * cw; + const S ul = vxi * cw + vyi * sw; + q.ui[i] = s_sqrt(ul * ul + eps2); + q.Fy[i] = dyn_lateral(-s_atan2(q.w[i], q.ui[i]), Bw[i], Cp, Dy); + q.Sw[i] = dyn4_ph_secant(q.Fy[i], q.w[i], Bw[i], Cp, Dy, q.ui[i]); + const S n[6] = {-sw, cw, xi * cw + yi * sw, zero, h * cw, h * sw}; + const S t[6] = {cw, sw, xi * sw - yi * cw, zero, h * sw, -h * cw}; + for (int k = 0; k < 6; ++k) { + q.N[i][k] = n[k]; + q.T[i][k] = t[k]; + } + } +} + +// The energy H; see above. +template +CVC_HD inline S dyn4_ph_hamiltonian(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s) { + dyn4_wheels W; + dyn4_wheel_state(p, v, ground, s, W); + const S m = S(p.m); + S H = S(0.5) * (m * (s.vx * s.vx + s.vy * s.vy + s.zd * s.zd) + S(p.Iz) * s.r * s.r + + S(p.Ix) * s.phid * s.phid + S(p.Iy) * s.thd * s.thd); + for (int i = 0; i < 4; ++i) { + const S l = W.Fz0[i] / W.k[i] + W.hg[i] - (s.z + W.yi[i] * s.phi - W.xi[i] * s.th); + H = H + S(0.5) * W.k[i] * l * l; + } + return H + m * S(p.g) * s.z; +} + +// x = (q, p): the vertical displacement and the momenta M xi. +template +CVC_HD inline void dyn4_ph_coords(const dyn4_params &p, const dyn4_state &s, S x[9]) { + const S m = S(p.m); + x[0] = s.z; + x[1] = s.phi; + x[2] = s.th; + x[3] = m * s.vx; + x[4] = m * s.vy; + x[5] = S(p.Iz) * s.r; + x[6] = m * s.zd; + x[7] = S(p.Ix) * s.phid; + x[8] = S(p.Iy) * s.thd; +} + +// dH/dx = (dH/dq, xi): the springs' and gravity's force on q, negated, and the twist. +template +CVC_HD inline void dyn4_ph_grad(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, S e[9]) { + dyn4_wheels W; + dyn4_wheel_state(p, v, ground, s, W); + e[0] = S(p.m) * S(p.g); + e[1] = S(0); + e[2] = S(0); + for (int i = 0; i < 4; ++i) { + const S kl = W.k[i] * (W.Fz0[i] / W.k[i] + W.hg[i] - (s.z + W.yi[i] * s.phi - W.xi[i] * s.th)); + e[0] = e[0] - kl; + e[1] = e[1] - kl * W.yi[i]; + e[2] = e[2] + kl * W.xi[i]; + } + e[3] = s.vx; + e[4] = s.vy; + e[5] = s.r; + e[6] = s.zd; + e[7] = s.phid; + e[8] = s.thd; +} + +// J(p): canonical in the vertical, the coadjoint structure in the horizontal. +template CVC_HD inline void dyn4_ph_J(const S x[9], S J[9][9]) { + for (int a = 0; a < 9; ++a) + for (int b = 0; b < 9; ++b) + J[a][b] = S(0); + S Jh[3][3]; + dyn_ph_J(x + 3, Jh); + for (int a = 0; a < 3; ++a) { + J[a][6 + a] = S(1); + J[6 + a][a] = S(-1); + for (int b = 0; b < 3; ++b) + J[3 + a][3 + b] = Jh[a][b]; + } +} + +// R, the dissipation: positive semi-definite at every slip. +template +CVC_HD inline void dyn4_ph_R(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, S R[9][9]) { + dyn4_ph_parts q; + dyn4_ph_parts_at(p, v, ground, s, q); + for (int a = 0; a < 9; ++a) + for (int b = 0; b < 9; ++b) + R[a][b] = S(0); + for (int a = 0; a < 3; ++a) + for (int b = a; b < 3; ++b) { + S hor = S(0), ver = S(0); + for (int i = 0; i < 4; ++i) { + const S ci[3] = {S(1), q.W.yi[i], -q.W.xi[i]}; + hor = hor + q.Sw[i] * (q.N[i][a] * q.N[i][b]); + ver = ver + q.W.c[i] * (ci[a] * ci[b]); + } + R[3 + a][3 + b] = hor; + R[3 + b][3 + a] = hor; + R[6 + a][6 + b] = ver; + R[6 + b][6 + a] = ver; + } + R[3][3] = R[3][3] + q.cx; +} + +// G, the input map for u = (F_x1, ..., F_x4): each wheel's force along it, at its contact patch. +template +CVC_HD inline void dyn4_ph_G(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, S Gm[9][4]) { + dyn4_ph_parts q; + dyn4_ph_parts_at(p, v, ground, s, q); + for (int i = 0; i < 4; ++i) + for (int a = 0; a < 9; ++a) + Gm[a][i] = a < 3 ? S(0) : q.T[i][a - 3]; +} + +// u: the wheels' drive or brake forces, from the actuator's a, within their grip. +template +CVC_HD inline void dyn4_ph_input(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, S u[4]) { + dyn4_ph_parts q; + dyn4_ph_parts_at(p, v, ground, s, q); + for (int i = 0; i < 4; ++i) + u[i] = q.Fx[i]; +} + +// f_lever: the tires' lateral forces' moments on roll and pitch, which their slips do not feel. +template +CVC_HD inline void dyn4_ph_lever(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, S f[9]) { + dyn4_ph_parts q; + dyn4_ph_parts_at(p, v, ground, s, q); + for (int a = 0; a < 9; ++a) + f[a] = S(0); + for (int i = 0; i < 4; ++i) + for (int k = 3; k < 6; ++k) + f[3 + k] = f[3 + k] + q.Fy[i] * q.N[i][k]; +} + +// f_grade: with grade_force, gravity along the ground (dyn4_grade) on p_h; zero without. +template +CVC_HD inline void dyn4_ph_grade(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, S f[9]) { + for (int a = 0; a < 9; ++a) + f[a] = S(0); + if (!p.grade_force) + return; + dyn4_wheels W; + dyn4_wheel_state(p, v, ground, s, W); + dyn4_grade(p, ground, s, W, f + 3); +} + +// x' = (J - R) dH/dx + G u + f_lever + f_grade at the state: dyn4_plant's vector field. +template +CVC_HD inline void dyn4_ph_rhs(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, S xdot[9]) { + S x[9], e[9], J[9][9], R[9][9], Gm[9][4], u[4], f[9], fg[9]; + dyn4_ph_coords(p, s, x); + dyn4_ph_grad(p, v, ground, s, e); + dyn4_ph_J(x, J); + dyn4_ph_R(p, v, ground, s, R); + dyn4_ph_G(p, v, ground, s, Gm); + dyn4_ph_input(p, v, ground, s, u); + dyn4_ph_lever(p, v, ground, s, f); + dyn4_ph_grade(p, v, ground, s, fg); + for (int a = 0; a < 9; ++a) { + S acc = f[a] + fg[a]; + for (int i = 0; i < 4; ++i) + acc = acc + Gm[a][i] * u[i]; + for (int b = 0; b < 9; ++b) + acc = acc + (J[a][b] - R[a][b]) * e[b]; + xdot[a] = acc; + } +} + +// The power balance at a state, on the ground under it: dH/dt = supplied - dissipated + lever + +// grade (+ the ground's, as the wheels move to new heights; dyn4_plant_books books it per step). +template struct dyn4_ph_rates { + S dissipated; // dH/dx^T R dH/dx >= 0: the tires' slip, drag, rolling resistance, the dampers + S supplied; // y^T u: the wheels' forces at their contact patches' speeds + S lever; // xi^T f_lever: the tires' lateral forces on roll and pitch + S grade; // xi^T f_grade: gravity along the ground (grade_force; 0 without it) +}; + +template +CVC_HD inline dyn4_ph_rates dyn4_ph_rates_at(const dyn4_params &p, const bike_veh &v, + const G &ground, const dyn4_state &s) { + S e[9], R[9][9], Gm[9][4], u[4], f[9], fg[9]; + dyn4_ph_grad(p, v, ground, s, e); + dyn4_ph_R(p, v, ground, s, R); + dyn4_ph_G(p, v, ground, s, Gm); + dyn4_ph_input(p, v, ground, s, u); + dyn4_ph_lever(p, v, ground, s, f); + dyn4_ph_grade(p, v, ground, s, fg); + dyn4_ph_rates out; + out.dissipated = S(0); + out.supplied = S(0); + out.lever = S(0); + out.grade = S(0); + for (int a = 0; a < 9; ++a) { + for (int b = 0; b < 9; ++b) + out.dissipated = out.dissipated + e[a] * R[a][b] * e[b]; + for (int i = 0; i < 4; ++i) + out.supplied = out.supplied + e[a] * Gm[a][i] * u[i]; + out.lever = out.lever + e[a] * f[a]; + out.grade = out.grade + e[a] * fg[a]; + } + return out; +} + +// One step's energy books, in J: dH = supplied - dissipated + lever + grade + ground + limiter + +// residual. The powers are the trapezoid between the step's start (with the inputs it applied, +// the actuators' outputs) and the state it reached before its limiters, both on the ground the +// step stood on (the start's pose: the pose enters H and the rates only through the ground). +// limiter is the kinetic energy the speed clamp and the low-speed blend then added (< 0: +// removed); ground is the springs' energy the new ground under the wheels added at the step's end +// (none on flat ground; with grade_force, grade + ground is second order in hdt per step); +// residual is the integrator's error, second order in hdt per step. +template struct dyn4_ph_books { + S dH, dissipated, supplied, lever, grade, ground, limiter, residual; +}; + +// dyn4_plant's step with its books. The step is dyn4_plant's, to the last bit. +template +CVC_HD inline void dyn4_plant_books(const dyn4_params &p, const bike_veh &v, const G &ground, + const dyn4_state &s, S delta_cmd, S a_cmd, dyn4_state &o, + dyn4_ph_books &b) { + dyn_step_free seen; + dyn4_plant(p, v, ground, s, delta_cmd, a_cmd, o, &seen); + dyn4_state s0 = s, f = o; + s0.delta = o.delta; + s0.a = o.a; + f.x = s.x; + f.y = s.y; + f.psi = s.psi; + dyn4_state e = f; + e.vx = seen.vx; + e.vy = seen.vy; + e.r = seen.r; + const dyn4_ph_rates r0 = dyn4_ph_rates_at(p, v, ground, s0), + r1 = dyn4_ph_rates_at(p, v, ground, e); + const S half = S(0.5) * S(v.hdt); + b.dissipated = half * (r0.dissipated + r1.dissipated); + b.supplied = half * (r0.supplied + r1.supplied); + b.lever = half * (r0.lever + r1.lever); + b.grade = half * (r0.grade + r1.grade); + const S h0 = dyn4_ph_hamiltonian(p, v, ground, s), he = dyn4_ph_hamiltonian(p, v, ground, e), + hf = dyn4_ph_hamiltonian(p, v, ground, f), h1 = dyn4_ph_hamiltonian(p, v, ground, o); + b.dH = h1 - h0; + b.limiter = hf - he; + b.ground = h1 - hf; + b.residual = b.dH - (b.supplied - b.dissipated + b.lever + b.grade + b.ground + b.limiter); +} + // One step under the trained controller, and its backward: bike_control's commands from the // rear axle's pose and the speed vx. `ext`, when given, is an external force joined to the // controller's (ext_in, diff_rollout.h), as dyn_bike_step takes it; the backward then returns diff --git a/inc/cvc/nav/dyn_4w.h b/inc/cvc/nav/dyn_4w.h index 999835864..ec18431c1 100644 --- a/inc/cvc/nav/dyn_4w.h +++ b/inc/cvc/nav/dyn_4w.h @@ -41,6 +41,7 @@ struct dyn4_si { 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 + bool grade_force = false; // gravity along the ground acts on the body (opt-in) }; inline diff::dyn4_params dyn4_params_of(const dyn4_si &si, const world_units::normalized_frame &f, @@ -77,6 +78,7 @@ inline diff::dyn4_params dyn4_params_of(const dyn4_si &si, const world_units::no 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); + p.grade_force = si.grade_force ? 1 : 0; return p; } diff --git a/src/cvc/tests/nav_dyn_4w_test.cpp b/src/cvc/tests/nav_dyn_4w_test.cpp index c40151808..24301da03 100644 --- a/src/cvc/tests/nav_dyn_4w_test.cpp +++ b/src/cvc/tests/nav_dyn_4w_test.cpp @@ -786,6 +786,504 @@ TEST(Dyn4, Golden) { << now[k]; } +// ── The plant in port-Hamiltonian form (dyn4_ph_*) ──────────────────────────────────────────── +// DynBikePH's gates (nav_dyn_bike_test) on four wheels with suspension. x = (q, p): the body's +// heave, roll and pitch and the momenta of its twist; H the kinetic energy, the corners' springs +// and gravity on heave; x' = (J - R) dH/dx + G u + f_lever, u the wheels' drive and brake forces, +// R the tires' secants, drag, rolling resistance and the dampers. The slips are in each wheel's +// frame, so no speed-floor term is left; f_lever is the tires' lateral forces' moments on roll and +// pitch, which the slips do not feel. These gates hold dyn4_plant to that form at every slip, +// sliding included, on flat and uneven ground, and book its steps. dyn4_plant's arithmetic is +// unchanged (Dyn4.Golden still pins it). + +namespace { + +constexpr double kTwoPi = 6.283185307179586; // M_PI is not portable (MSVC) + +// A straight ramp up along +x: height s wx. +struct ramp_ground { + double mu = 0.9, s = 0.05; + template void at(S wx, S, S &mu_out, S &height) const { + mu_out = S(mu); + height = S(s) * wx; + } +}; + +// Rolling hills everywhere: height A sin(wx / lx) cos(wy / ly), one friction. +struct rolling_ground { + double mu = 0.6, A = 0.05, lx = 1.3, ly = 1.7; + template void at(S wx, S wy, S &mu_out, S &height) const { + mu_out = S(mu); + height = S(A) * diff::s_sin(wx / S(lx)) * diff::s_cos(wy / S(ly)); + } +}; + +// A Humvee state anywhere in the envelope: forward or reversing, the wheel up to 0.5 rad, +// sideways and yaw rates well past the tires' peaks, the body heaving, rolling and pitching, and +// anywhere on golden_ground's split and bump. +dyn4_state sliding4(std::mt19937 &rng) { + std::uniform_real_distribution u(-1.0, 1.0); + dyn4_state s{}; + const double speed = 3.0 + 25.0 * 0.5 * (1.0 + u(rng)); + s.vx = u(rng) < -0.7 ? -std::min(speed, 8.0) : speed; + s.delta = 0.5 * u(rng); + s.vy = 4.0 * u(rng); + s.r = 1.2 * u(rng); + s.a = 5.0 * u(rng); + s.x = 4.0 + 2.0 * u(rng); + s.y = 2.0 * u(rng) + (u(rng) > 0.0 ? 3.0 : 0.0); + s.psi = 3.0 * u(rng); + s.z = 0.03 * u(rng); + s.zd = 0.2 * u(rng); + s.phi = 0.03 * u(rng); + s.phid = 0.2 * u(rng); + s.th = 0.02 * u(rng); + s.thd = 0.2 * u(rng); + return s; +} + +// How many of the tires are past their peak (the force falling as the slip grows). +int tires_past_peak(const dyn4_params &p, const diff::dyn4_ph_parts &q) { + int n = 0; + for (int i = 0; i < 4; ++i) { + const double B = i < 2 ? p.B_f : p.B_r, alpha = -std::atan2(q.w[i], q.ui[i]); + n += std::cos(p.C * std::atan(B * alpha)) < 0.0; + } + return n; +} + +// The least eigenvalue of a symmetric 9 x 9 matrix's trailing block from row lo, by cyclic +// Jacobi rotations. +double least_eigenvalue(const double M[9][9], int lo = 0) { + double A[9][9]; + for (int a = 0; a < 9; ++a) + for (int b = 0; b < 9; ++b) + A[a][b] = M[a][b]; + for (int sweep = 0; sweep < 50; ++sweep) { + double off = 0.0, diag = 0.0; + for (int a = lo; a < 9; ++a) { + diag += A[a][a] * A[a][a]; + for (int b = a + 1; b < 9; ++b) + off += A[a][b] * A[a][b]; + } + if (off <= 1e-32 * diag) + break; + for (int p = lo; p < 9; ++p) + for (int q = p + 1; q < 9; ++q) { + if (A[p][q] == 0.0) + continue; + const double theta = (A[q][q] - A[p][p]) / (2.0 * A[p][q]); + const double t = (theta >= 0.0 ? 1.0 : -1.0) / (std::fabs(theta) + std::hypot(theta, 1.0)); + const double c = 1.0 / std::sqrt(t * t + 1.0), s = t * c; + for (int k = lo; k < 9; ++k) { + const double kp = A[k][p], kq = A[k][q]; + A[k][p] = c * kp - s * kq; + A[k][q] = s * kp + c * kq; + } + for (int k = lo; k < 9; ++k) { + const double pk = A[p][k], qk = A[q][k]; + A[p][k] = c * pk - s * qk; + A[q][k] = s * pk + c * qk; + } + } + } + double lam = A[lo][lo]; + for (int a = lo + 1; a < 9; ++a) + lam = std::min(lam, A[a][a]); + return lam; +} + +// x's coordinates as the state's: q = (z, phi, th), then the twist (vx, vy, r, zd, phid, thd), +// and each twist coordinate's mass. +const int kCoord[9] = {6, 8, 10, 3, 4, 5, 7, 9, 11}; +double coord_mass(const dyn4_params &p, int a) { + const double M[9] = {1.0, 1.0, 1.0, p.m, p.m, p.Iz, p.m, p.Ix, p.Iy}; + return M[a]; +} + +// The structure at 500 states on one ground: J skew, R symmetric positive semi-definite (its +// secants >= 0), dH/dx = (dH/dq, xi) against H's central differences, and nothing supplied with +// the wheels' forces at zero, however they turn. +template void structure_gate(const dyn4_params &p, const G &ground, const char *label) { + const bike_veh v = limits(p.lf + p.lr); + std::mt19937 rng(3); + int past = 0; + double least_twist = 1e300, worst_grad = 0.0; + for (int k = 0; k < 500; ++k) { + dyn4_state s = sliding4(rng); + if (k % 5 == 0) + s.a = 0.0; + double x[9], e[9], J[9][9], R[9][9]; + diff::dyn4_ph_coords(p, s, x); + diff::dyn4_ph_grad(p, v, ground, s, e); + diff::dyn4_ph_J(x, J); + diff::dyn4_ph_R(p, v, ground, s, R); + diff::dyn4_ph_parts q; + diff::dyn4_ph_parts_at(p, v, ground, s, q); + double eJe = 0.0, eJe_abs = 0.0, norm = 0.0; + for (int a = 0; a < 9; ++a) + for (int b = 0; b < 9; ++b) { + EXPECT_EQ(J[a][b], -J[b][a]); + EXPECT_EQ(R[a][b], R[b][a]); + eJe += e[a] * J[a][b] * e[b]; + eJe_abs += std::fabs(e[a] * J[a][b] * e[b]); + norm += R[a][b] * R[a][b]; + } + norm = std::sqrt(norm); + EXPECT_LE(std::fabs(eJe), 1e-12 * eJe_abs); + // R's coefficients are nonnegative, and so is its spectrum. + EXPECT_GT(q.cx, 0.0); + for (int i = 0; i < 4; ++i) { + EXPECT_GE(q.Sw[i], 0.0) << label << ": tire " << i << " at w " << q.w[i]; + EXPECT_GT(q.W.c[i], 0.0); + } + EXPECT_GE(least_eigenvalue(R), -1e-9 * norm) << label << " at vx " << s.vx << ", vy " << s.vy; + // q's block is zero; on the twist the resistances, the four tires and the four dampers span. + least_twist = std::min(least_twist, least_eigenvalue(R, 3) / norm); + // dH/dx: in q directly, in p through the twist (p = M xi). + for (int a = 0; a < 9; ++a) { + const double dq = 1e-6; + dyn4_state sp = s, sm = s; + sp[kCoord[a]] += dq; + sm[kCoord[a]] -= dq; + const double fd = (diff::dyn4_ph_hamiltonian(p, v, ground, sp) - + diff::dyn4_ph_hamiltonian(p, v, ground, sm)) / + (2.0 * dq * coord_mass(p, a)); + const double scale = a < 3 ? p.m * p.g * (p.lf + p.lr) : 1.0 + std::fabs(e[a]); + worst_grad = std::max(worst_grad, std::fabs(fd - e[a]) / scale); + EXPECT_NEAR(fd, e[a], 1e-6 * scale) << label << ": coordinate " << a; + } + // Steering is not a port: with the wheels' forces at zero nothing is supplied. + if (s.a == 0.0) + EXPECT_EQ(diff::dyn4_ph_rates_at(p, v, ground, s).supplied, 0.0); + past += tires_past_peak(p, q) > 0; + } + std::printf(" %s: %d of 500 states have a tire past its peak; R's least eigenvalue on the " + "twist %.2e |R|; dH/dx within %.1e of H's differences\n", + label, past, least_twist, worst_grad); + EXPECT_GT(past, 100) << label << ": the states do not slide"; +} + +} // namespace + +// J is skew and R a sum of nonnegative multiples of outer products at every slip, on flat ground +// of two frictions and on golden_ground's split and bump, sprung and with rigid corners: R is +// positive semi-definite sliding or not, at any steering angle. +TEST(Dyn4PH, TheStructureIsPortHamiltonianAtEverySlip) { + const dyn4_params sprung = humvee4(); + dyn4_params rigid = sprung; + rigid.k_f = rigid.k_r = rigid.c_f = rigid.c_r = 0.0f; + diff::flat_ground grippy, slick; + grippy.mu = 0.9f; + slick.mu = 0.6f; + structure_gate(sprung, grippy, "flat, mu 0.9"); + structure_gate(sprung, slick, "flat, mu 0.6"); + structure_gate(sprung, golden_ground(), "split and bump"); + structure_gate(rigid, golden_ground(), "split and bump, rigid"); +} + +// One very short dyn4_plant step's change in x against the port-Hamiltonian vector field at every +// slip, forward and reversing, on flat ground and on golden_ground (there with and without the +// grade force), with the actuators holding (commands equal to their state) so u is the state's +// own. The low-speed blend is a per-step limiter, not part of the field, so it is off here +// (dyn4_plant_books charges it). +TEST(Dyn4PH, TheVectorFieldIsThePlantsAtEverySlip) { + dyn4_params p = humvee4(); + p.v_blend = 0.0f; + dyn4_params pg = p; + pg.grade_force = 1; + bike_veh lim = limits(p.lf + p.lr); + lim.allow_reverse = 1; + lim.hdt = 1e-5f; + const double mg = p.m * p.g, L = p.lf + p.lr, t = std::max(p.tf, p.tr); + // Relative to each row's scale: 1 m/s (rad/s) on q, the forces' on the momenta. + const double scale[9] = {1.0, 1.0, 1.0, mg, mg, mg * L, mg, mg * t, mg * L}; + const auto gate = [&](const dyn4_params &pp, const auto &ground, const char *label) { + std::mt19937 rng(5); + double worst = 0.0; + for (int k = 0; k < 400; ++k) { + const dyn4_state s = sliding4(rng); + dyn4_state o; + diff::dyn4_plant(pp, lim, ground, s, s.delta, s.a, o); + double x0[9], x1[9], f[9]; + diff::dyn4_ph_coords(pp, s, x0); + diff::dyn4_ph_coords(pp, o, x1); + diff::dyn4_ph_rhs(pp, lim, ground, s, f); + for (int a = 0; a < 9; ++a) { + const double fd = (x1[a] - x0[a]) / lim.hdt; + const double rel = std::fabs(fd - f[a]) / scale[a]; + worst = std::max(worst, rel); + EXPECT_LT(rel, 1e-3) << label << ", coordinate " << a << ": plant " << fd << ", pH " << f[a] + << " at vx " << s.vx << ", vy " << s.vy << ", r " << s.r << ", delta " + << s.delta; + } + } + std::printf(" vector field vs dyn4_plant at every slip, %s: worst |dx/dt - pH| = %.2e of " + "its scale\n", + label, worst); + }; + diff::flat_ground slick; + slick.mu = 0.6f; + gate(p, slick, "flat, mu 0.6"); + gate(p, golden_ground(), "split and bump"); + gate(pg, golden_ground(), "split and bump, grade force"); +} + +namespace { + +// One run's books: per-step mean |residual|, the sums, and the steps with a tire past its peak. +struct run_books4 { + double mean_abs_residual = 0.0, residual = 0.0, flow = 0.0, dH = 0.0, limiter = 0.0; + double limiter_abs = 0.0, dissipated = 0.0, supplied = 0.0, lever = 0.0, ground = 0.0; + double grade = 0.0, exchange_abs = 0.0; // the grade line, and sum |ground + grade| per step + int steps = 0, past = 0; +}; + +// dyn4_plant_books along a trajectory: `swerve` weaves at 0.2 Hz from nearly full lock to full +// lock (the steering slews at 0.35 rad/s), the tires sliding well past their peak, with the +// throttle working; otherwise the Humvee brakes hard to a stop in a turn, so the speed clamp and +// the low-speed blend act. +template +run_books4 book_run4(const dyn4_params &p, const G &ground, double hdt, bool swerve, + double seconds) { + bike_veh lim = limits(p.lf + p.lr); + lim.hdt = static_cast(hdt); + dyn4_state s = rolling(swerve ? 15.0 : 9.0); + const int n = static_cast(std::lround(seconds / hdt)); + run_books4 rb; + rb.steps = n; + const double h0 = diff::dyn4_ph_hamiltonian(p, lim, ground, s); + for (int t = 0; t < n; ++t) { + const double time = t * hdt; + const double dcmd = swerve ? 0.5 * std::tanh(3.0 * std::sin(kTwoPi * 0.2 * time)) : 0.35; + const double acmd = swerve ? 1.5 * std::sin(kTwoPi * time / 3.0) : -6.0; + dyn4_state o; + diff::dyn4_ph_books b; + diff::dyn4_plant_books(p, lim, ground, s, dcmd, acmd, o, b); + rb.mean_abs_residual += std::fabs(b.residual); + rb.residual += b.residual; + rb.limiter += b.limiter; + rb.limiter_abs += std::fabs(b.limiter); + rb.dissipated += b.dissipated; + rb.supplied += b.supplied; + rb.lever += b.lever; + rb.ground += b.ground; + rb.grade += b.grade; + rb.exchange_abs += std::fabs(b.ground + b.grade); + rb.flow += std::fabs(b.dissipated) + std::fabs(b.supplied) + std::fabs(b.lever) + + std::fabs(b.grade) + std::fabs(b.ground) + std::fabs(b.limiter); + diff::dyn4_ph_parts q; + diff::dyn4_ph_parts_at(p, lim, ground, o, q); + rb.past += tires_past_peak(p, q) > 0; + s = o; + } + rb.mean_abs_residual /= n; + rb.dH = diff::dyn4_ph_hamiltonian(p, lim, ground, s) - h0; + return rb; +} + +} // namespace + +// The gate: every step's books close but for the integrator's error, which is second order per +// step (so first order over a run), sliding or not, on flat ground and over hills, with the +// limiters and the ground's new heights charged when they act. The actuators lag as they do in +// training; u is their output. +TEST(Dyn4PH, StepBooksCloseToSecondOrder) { + const dyn4_params p = humvee4(); + diff::flat_ground slick; + slick.mu = 0.6f; + const rolling_ground hills; + const auto swerves = [&](const auto &ground, const char *label) { + // Per step, the residual falls by about 4 per halving of hdt. + double prev = 0.0; + for (const double hdt : {0.02, 0.01, 0.005, 0.0025}) { + const run_books4 rb = book_run4(p, ground, hdt, true, 8.0); + std::printf(" %s, swerving, hdt %.4f s: dH %.4g J, supplied %.4g, dissipated %.4g, lever " + "%.4g, ground %.4g; mean |residual| per step %.3e J, |sum| / flow %.2e; %d of " + "%d steps past the peak\n", + label, hdt, rb.dH, rb.supplied, rb.dissipated, rb.lever, rb.ground, + rb.mean_abs_residual, std::fabs(rb.residual) / rb.flow, rb.past, rb.steps); + EXPECT_GT(rb.past, rb.steps / 5) << label << ": the swerve does not slide"; + if (prev > 0.0) { + const double ratio = prev / rb.mean_abs_residual; + EXPECT_GT(ratio, 3.0) << label << ", hdt " << hdt; + EXPECT_LT(ratio, 5.0) << label << ", hdt " << hdt; + } + prev = rb.mean_abs_residual; + if (hdt <= 0.01) + EXPECT_LT(std::fabs(rb.residual) / rb.flow, 5e-3) << label << ", hdt " << hdt; + } + }; + swerves(slick, "flat, mu 0.6"); + swerves(hills, "hills, mu 0.6"); + // On flat ground the ground adds nothing; over hills it does. + EXPECT_EQ(book_run4(p, slick, 0.01, true, 8.0).ground, 0.0); + EXPECT_NE(book_run4(p, hills, 0.01, true, 8.0).ground, 0.0); + // Braking to a stop in a turn: the books charge the limiters what they change. The low-speed + // blend adds yaw on the way down (the kinematic bicycle turns harder than the sliding tires + // let it) and the clamp at zero speed takes what the brakes would push past it, so their sum's + // sign depends on how long the vehicle stands; the rest closes as before. + for (const double hdt : {0.01, 0.005}) { + const run_books4 rb = book_run4(p, slick, hdt, false, 3.0); + std::printf(" braking to a stop, hdt %.4f s: dH %.4g J, limiters %.4g J (|.| %.4g J), |sum " + "residual| / flow %.2e\n", + hdt, rb.dH, rb.limiter, rb.limiter_abs, std::fabs(rb.residual) / rb.flow); + EXPECT_GT(rb.limiter_abs, 100.0) << "the limiters did not act"; + EXPECT_LT(std::fabs(rb.residual) / rb.flow, 5e-3) << "hdt " << hdt; + } + // The lever is the centre of gravity's height: on the ground it is gone, and the books close + // without it. + dyn4_params low = p; + low.h = 0.0f; + const run_books4 rb = book_run4(low, slick, 0.005, true, 8.0); + std::printf(" centre of gravity on the ground, hdt 0.0050 s: lever %.4g J, |sum residual| / " + "flow %.2e\n", + rb.lever, std::fabs(rb.residual) / rb.flow); + EXPECT_EQ(rb.lever, 0.0); + EXPECT_LT(std::fabs(rb.residual) / rb.flow, 5e-3); +} + +// With grade_force, gravity along the ground is minus the gradient of H's springs in the pose, +// through the ground under the wheels, as a force and yaw moment at the centre of gravity: +// dyn4_ph_grade against H's central differences in (x, y, psi), on golden_ground's bump and on +// rolling hills. Without it f_grade is zero. +TEST(Dyn4PH, TheGradeForceIsMinusTheSpringsGradientInThePose) { + dyn4_params p = humvee4(); + p.grade_force = 1; + const bike_veh v = limits(p.lf + p.lr); + const double mg = p.m * p.g, L = p.lf + p.lr; + const auto gate = [&](const auto &ground, const char *label) { + std::mt19937 rng(9); + double worst = 0.0, biggest = 0.0; + for (int k = 0; k < 300; ++k) { + const dyn4_state s = sliding4(rng); + double f[9], dH[3]; + diff::dyn4_ph_grade(p, v, ground, s, f); + for (int a = 0; a < 3; ++a) { + const double d = 1e-6; + dyn4_state sp = s, sm = s; + sp[a] += d; + sm[a] -= d; + dH[a] = (diff::dyn4_ph_hamiltonian(p, v, ground, sp) - + diff::dyn4_ph_hamiltonian(p, v, ground, sm)) / + (2.0 * d); + } + // From the rear axle's (x, y) and psi to the force and moment at the centre of gravity. + const double c = std::cos(s.psi), sn = std::sin(s.psi); + const double want[3] = {-(c * dH[0] + sn * dH[1]), -(c * dH[1] - sn * dH[0]), + -dH[2] + p.lr * (c * dH[1] - sn * dH[0])}; + const double scale[3] = {mg, mg, mg * L}; + for (int a = 0; a < 3; ++a) { + worst = std::max(worst, std::fabs(f[3 + a] - want[a]) / scale[a]); + biggest = std::max(biggest, std::fabs(want[a]) / scale[a]); + EXPECT_NEAR(f[3 + a], want[a], 1e-5 * scale[a]) << label << ", component " << a; + } + for (const int a : {0, 1, 2, 6, 7, 8}) + EXPECT_EQ(f[a], 0.0); + } + std::printf(" %s: the grade force is within %.1e m g of -dH/dpose (largest %.3f m g)\n", label, + worst, biggest); + EXPECT_GT(biggest, 0.01) << label << ": the ground is too flat to test"; + }; + gate(golden_ground(), "split and bump"); + gate(rolling_ground(), "hills"); + dyn4_params off = p; + off.grade_force = 0; + double f[9]; + diff::dyn4_ph_grade(off, v, rolling_ground(), rolling(10.0), f); + for (const double x : f) + EXPECT_EQ(x, 0.0); +} + +// The difference the grade force makes. Without it the terrain only lifts the springs: the books' +// ground line is energy from outside, and a vehicle coasting up a ramp keeps its speed while H +// rises. With it gravity along the ground pulls the body and the grade line cancels the ground +// line but for the step's error (second order per step): the terrain's energy is in H, and the +// coast slows at g times the slope. The books close either way. +TEST(Dyn4PH, TheGradeForcePutsTheTerrainInH) { + const dyn4_params off = humvee4(); + dyn4_params on = off; + on.grade_force = 1; + const rolling_ground hills; + double prev = 0.0; + for (const double hdt : {0.02, 0.01, 0.005, 0.0025}) { + const run_books4 a = book_run4(off, hills, hdt, true, 8.0); + const run_books4 b = book_run4(on, hills, hdt, true, 8.0); + const double exchange = b.exchange_abs / b.steps; + std::printf(" hills, swerving, hdt %.4f s: without the grade force ground %.4g J, grade %.4g; " + "with it ground %.4g J, grade %.4g, mean |ground + grade| per step %.3e J; |sum " + "residual| / flow %.2e, %.2e\n", + hdt, a.ground, a.grade, b.ground, b.grade, exchange, std::fabs(a.residual) / a.flow, + std::fabs(b.residual) / b.flow); + EXPECT_EQ(a.grade, 0.0); + EXPECT_GT(b.past, b.steps / 5) << "hdt " << hdt << ": the swerve does not slide"; + if (prev > 0.0) { + const double ratio = prev / exchange; + EXPECT_GT(ratio, 3.0) << "hdt " << hdt; + EXPECT_LT(ratio, 5.0) << "hdt " << hdt; + } + prev = exchange; + if (hdt <= 0.01) { + EXPECT_LT(std::fabs(b.ground + b.grade), 0.05 * std::fabs(b.ground)) << "hdt " << hdt; + EXPECT_LT(std::fabs(b.residual) / b.flow, 5e-3) << "hdt " << hdt; + } + } + // Coasting straight up a 5% ramp from 10 m/s for 2 s, settled on it, without drag, rolling + // resistance or the dampers (nearly: 0 means critical). The plant's dampers act on the body's own + // vertical rate, not on its rate relative to the ground, so a steady climb would pay them too + // (c zd^2 a corner; over the hills above they act). The step holds the ground at its start, so + // the springs trail the ramp by about half a step's climb: an error first order in hdt. + const ramp_ground ramp; + const double slope = ramp.s, seconds = 2.0; + const auto coast = [&](dyn4_params p, double hdt, run_books4 &books, double &climb) { + p.drag = p.c_rr = 0.0f; + p.c_f = p.c_r = 1e-3f; + bike_veh lim = limits(p.lf + p.lr); + lim.hdt = static_cast(hdt); + dyn4_state s = rolling(10.0); + s.z = slope * p.lr; // the rear axle at x = 0: the body on the ramp, its springs at rest + s.zd = slope * s.vx; + s.th = -slope; + const double h0 = diff::dyn4_ph_hamiltonian(p, lim, ramp, s), x0 = s.x; + for (int t = 0; t < static_cast(std::lround(seconds / hdt)); ++t) { + dyn4_state o; + diff::dyn4_ph_books b; + diff::dyn4_plant_books(p, lim, ramp, s, 0.0, 0.0, o, b); + books.residual += b.residual; + books.ground += b.ground; + books.grade += b.grade; + s = o; + } + books.dH = diff::dyn4_ph_hamiltonian(p, lim, ramp, s) - h0; + climb = slope * (s.x - x0); + return s.vx; + }; + const double mg = off.m * off.g, want = off.g * slope * seconds; + double prev_err = 0.0; + for (const double hdt : {0.01, 0.0025}) { + run_books4 ra, rb; + double climb_a = 0.0, climb_b = 0.0; + const double v_off = coast(off, hdt, ra, climb_a), v_on = coast(on, hdt, rb, climb_b); + std::printf(" coasting up a 5%% ramp for 2 s from 10 m/s, hdt %.4f s: without the grade force " + "%.4f m/s, dH %.4g J (ground %.4g, m g climb %.4g); with it %.4f m/s (lost %.4f, " + "g s t %.4f), dH %.4g J (ground %.4g, grade %.4g)\n", + hdt, v_off, ra.dH, ra.ground, mg * climb_a, v_on, v_off - v_on, want, rb.dH, + rb.ground, rb.grade); + // Without it the climb is free: the speed holds and H gains m g times the climb, all from the + // ground line. With it the climb is paid from the speed, and H holds. + EXPECT_NEAR(v_off, 10.0, 1e-6); + EXPECT_LT(std::fabs(rb.ground + rb.grade), 0.01 * rb.ground) << "hdt " << hdt; + const double err = std::max( + {std::fabs(ra.dH / (mg * climb_a) - 1.0), std::fabs(ra.ground / (mg * climb_a) - 1.0), + std::fabs((v_off - v_on) / want - 1.0), std::fabs(rb.dH / (mg * climb_b))}); + if (prev_err > 0.0) { + EXPECT_LT(err, 0.01) << "hdt " << hdt; + EXPECT_LT(err, prev_err / 2.0) << "the step's error does not shrink with it"; + } + prev_err = err; + } +} + // ── The external-force port on the four-wheel plant (ext_in, dyn4_rollout_ext) ───────────────── namespace {