diff --git a/inc/cvc/nav/detail/dyn_bike.h b/inc/cvc/nav/detail/dyn_bike.h index 54ef8ac2a..06f297a7a 100644 --- a/inc/cvc/nav/detail/dyn_bike.h +++ b/inc/cvc/nav/detail/dyn_bike.h @@ -33,16 +33,18 @@ // resistance and grade, then corrected after step 4 with the lateral forces the implicit // step applied: vx1 = vx + hdt (FX / m + r1 vy1), clamped to [sp_min, vmax] like the // kinematic plant. -// 4. Lateral and yaw. Slip angles with the new speed, smooth through zero speed: -// alpha_f = sgn(vx1) delta - atan2(vy + lf r, u), alpha_r = -atan2(vy - lr r, u), -// u = sqrt(vx1^2 + eps^2), sgn(vx1) = vx1 / u (vx1 the predicted speed). Pacejka lateral -// forces D sin(C atan(B alpha)), D the grip left, infinitely smooth with a sign-correct -// slope past the peak. +// 4. Lateral and yaw. Slip angles in each wheel's frame with the new speed, smooth through zero +// speed, as dyn_4w's: alpha = -atan2(w, u_w), w the axle's sideways velocity in its wheel's +// frame and u_w its forward speed there, floored (sqrt(. ^2 + eps^2)). The front wheel is +// turned by delta: w_f = cos(delta) (vy + lf r) - sin(delta) vx1, u_wf from +// cos(delta) vx1 + sin(delta) (vy + lf r); the rear's is w_r = vy - lr r, u_wr from vx1 (vx1 +// the predicted speed). Pacejka lateral forces D sin(C atan(B alpha)), D the grip left, +// infinitely smooth with a sign-correct slope past the peak. // (vy, r) advance by implicit Euler, solved with `iters` steps from the current state, J -// the Jacobian of (vy', r') in (vy, r) with each tire as its secant through its root in -// lateral velocity (dyn_secant_w): stiff tires stay stable at nav's 0.06 s step and reach -// the kinematic bicycle, and a sliding tire pulls its slip toward zero rather than past it, -// even at a crawl with the wheel at full lock. +// the Jacobian of (vy', r') in (vy, r) with each tire as its secant through its root, zero +// sideways velocity in its wheel's frame (dyn_secant_w): stiff tires stay stable at nav's +// 0.06 s step and reach the kinematic bicycle, and a sliding tire pulls its slip toward zero +// rather than past it, even at a crawl with the wheel at full lock. // 5. Low speed. A sigmoid in |vx1| around v_blend blends (vy, r) toward the kinematic // bicycle's, r = vx1 tan(delta) / L and vy = lr r, where slip angles mean nothing. // 6. Pose. psi1 = psi + hdt r1; the rear axle moves with (vx1, vy1 - lr r1) at psi1, so in the @@ -288,27 +290,30 @@ CVC_HD inline void dyn_plant(const dyn_bike_params &p, const bike_veh &v, const // 4. Lateral and yaw: implicit Euler for (vy, r) with the predicted speed, solved by `iters` // Newton-like steps from the current state, each linearizing the tires by their secants through // their roots at its iterate: (I - hdt J) d = x0 + hdt f(x) - x. - const S u = s_sqrt(vx_p * vx_p + eps2), sg = vx_p / u; + const S u = s_sqrt(vx_p * vx_p + eps2); const S Bf = S(p.B_f), Br = S(p.B_r); - const S w0f = - u * s_tan(sg * delta1); // the front's lateral velocity at zero slip; the rear's is 0 S vy1 = s.vy, r1 = s.r, Fyf_step = S(0); const int iters = p.iters > 0 ? p.iters : 1; for (int it = 0; it < iters; ++it) { - const S wf = vy1 + lf * r1, wr = vy1 - lr * r1; - const S af = sg * delta1 - s_atan2(wf, u), ar = -s_atan2(wr, u); + // Each axle's velocity in its wheel's frame: the front's sideways wf and forward speed uf + // (floored), the rear's sideways wr. Each tire's root is wf or wr = 0. + const S vf = vy1 + lf * r1; + const S wf = -vx_p * sd + vf * cd, wr = vy1 - lr * r1; + const S uf = s_sqrt((vx_p * cd + vf * sd) * (vx_p * cd + vf * sd) + eps2); + const S af = -s_atan2(wf, uf), ar = -s_atan2(wr, u); const S Fyf = dyn_lateral(af, Bf, Cp, Dyf), Fyr = dyn_lateral(ar, Br, Cp, Dyr); const S FY = Fxf * sd + Fyf * cd + Fyr; const S MZ = lf * (Fxf * sd + Fyf * cd) - lr * Fyr; const S f_vy = FY / m - r1 * vx_p, f_r = MZ / Iz; - const S Sf_t = dyn_secant_w(Fyf, af, wf, w0f, Bf, Cp, Dyf, u); + const S Sf_t = dyn_secant_w(Fyf, af, wf, S(0), Bf, Cp, Dyf, uf); const S Sr = dyn_secant_w(Fyr, ar, wr, S(0), Br, Cp, Dyr, u); - const S Sf = Sf_t * cd; - // dFyf/d(vy, r) = -Sf_t (1, lf); dFyr/d(vy, r) = -Sr (1, -lr). - const S J11 = (-Sf - Sr) / m; - const S J12 = (-Sf * lf + Sr * lr) / m - vx_p; - const S J21 = (-Sf * lf + Sr * lr) / Iz; - const S J22 = (-Sf * lf * lf - Sr * lr * lr) / Iz; + // dwf/d(vy, r) = cos(delta) (1, lf), so dFyf/d(vy, r) = -Sf (1, lf), and on the body's y, + // d(Fyf cos delta)/d(vy, r) = -Sfy (1, lf); dFyr/d(vy, r) = -Sr (1, -lr). + const S Sf = Sf_t * cd, Sfy = Sf * cd; + const S J11 = (-Sfy - Sr) / m; + const S J12 = (-Sfy * lf + Sr * lr) / m - vx_p; + const S J21 = (-Sfy * lf + Sr * lr) / Iz; + const S J22 = (-Sfy * lf * lf - Sr * lr * lr) / Iz; const S A11 = one - hdt * J11, A12 = -hdt * J12, A21 = -hdt * J21, A22 = one - hdt * J22; S det = A11 * A22 - A12 * A21; if (s_val(det) < s_val(S(0.05))) @@ -318,7 +323,7 @@ CVC_HD inline void dyn_plant(const dyn_bike_params &p, const bike_veh &v, const vy1 = vy1 + dvy; r1 = r1 + dr; // The front force this linearization applies. - Fyf_step = Fyf - Sf_t * (dvy + lf * dr); + Fyf_step = Fyf - Sf * (dvy + lf * dr); } // 3, corrected: the speed with the front lateral force the implicit step applied. @@ -392,36 +397,35 @@ CVC_HD inline void dyn_plant_vjp(const dyn_bike_params &p, const bike_veh &v, co // It gives the + r vy in vx' and - r vx in vy' (and, with equal masses, nothing in r'). // Skew, so it moves no energy; a Lie-Poisson structure, so Jacobi holds (unlike the // kinematic bicycle, whose no-slip constraint is not integrable). -// R D_x e_x e_x^T + (S_f / cos delta) n_f n_f^T + S_r n_r n_r^T. D_x = drag u + c_rr m g / u -// is drag and rolling resistance, opposing vx (u = sqrt(vx^2 + eps^2), as the plant -// smooths them). n_f = (-sin delta, cos delta, lf cos delta) and n_r = (0, 1, -lr) carry -// each tire's lateral force onto (vx, vy, r) (so n^T v is its contact patch's sideways -// velocity in the wheel's frame), and S_f, S_r are the tires' secant stiffnesses through -// their roots (dyn_secant_w), >= 0 at any slip because a Pacejka force (C < 2) always -// opposes its slip. So R is positive semi-definite sliding or not, for |delta| < pi / 2. +// R D_x e_x e_x^T + S_f n_f n_f^T + S_r n_r n_r^T. D_x = drag u + c_rr m g / u is drag and +// rolling resistance, opposing vx (u = sqrt(vx^2 + eps^2), as the plant smooths them). +// n_f = (-sin delta, cos delta, lf cos delta) and n_r = (0, 1, -lr) carry each tire's +// lateral force onto (vx, vy, r), and n^T v is its contact patch's sideways velocity in +// its wheel's frame, the slip's w. S_f, S_r are the tires' secant stiffnesses through +// their roots at w = 0 (dyn_secant_w), >= 0 at any slip because a Pacejka force (C < 2) +// always opposes its slip. So R is positive semi-definite sliding or not, at any delta. // G [t_f, t_r], t_f = (cos delta, sin delta, lf sin delta), t_r = (1, 0, 0): each axle's // drive or brake force (within its grip) along its wheel. y^T u is the power those forces // deliver at the wheels' ground speed. Steering is not an input: delta only turns n_f and // t_f, so it does no work on the body. -// f_eps n_f (F_yf + (S_f / cos delta) n_f^T v) + n_r (F_yr + S_r n_r^T v): what the secants -// leave of the tires' forces. The front force's root is at the plant's floored speed, -// vy + lf r = u tan(sg delta) with sg = vx / u, not where n_f^T v = 0 (vy + lf r = -// vx tan delta); f_eps is the difference, S_f (u tan(sg delta) - vx tan delta) n_f, plus -// the secants' own rounding at zero slip. It vanishes as eps / |vx| -> 0 and is booked on -// its own line. +// f_eps n_f (F_yf + S_f n_f^T v) + n_r (F_yr + S_r n_r^T v): what the secants leave of the +// tires' forces. Each slip is its wheel's frame's, rooted where n^T v = 0, so this is only +// the secants' rounding (at zero slip, where dyn_secant_w takes the slope): round-off. The +// speed floor eps enters the slips' forward speeds alone, which the force's sign does not +// read. It is booked on its own line all the same. // grade gravity along the heading, booked as the power - m g grade vx (the plant has no side // slope, so H carries no potential). // -// So dH/dt = -v^T R v + y^T u + v^T f_eps - m g grade vx: passive with respect to the axle forces, -// at every slip. The inputs are the actuators' outputs (the state's delta and a): their lags are -// outside the body. dyn_plant's step departs from the vector field by its integrator (implicit -// Euler on (vy, r), a predicted then corrected vx) and by its limiters (the speed clamp, the -// low-speed blend toward the kinematic bicycle); dyn_plant_books charges each step's limiters -// their kinetic energy and leaves the integrator's error, second order per step, as the residual. -// Past a tire's peak its force falls as slip grows and the tangent (dyn_ph_tangent_D), the -// linearization's damping, loses definiteness: the motion can be locally unstable there, but the -// secant R, the energy's, stays positive semi-definite. These are evaluations: dyn_plant's -// arithmetic does not depend on them. +// So dH/dt = -v^T R v + y^T u + v^T f_eps - m g grade vx, with v^T f_eps at round-off: passive +// with respect to the axle forces, at every slip. The inputs are the actuators' outputs (the +// state's delta and a): their lags are outside the body. dyn_plant's step departs from the vector +// field by its integrator (implicit Euler on (vy, r), a predicted then corrected vx) and by its +// limiters (the speed clamp, the low-speed blend toward the kinematic bicycle); dyn_plant_books +// charges each step's limiters their kinetic energy and leaves the integrator's error, second order +// per step, as the residual. Past a tire's peak its force falls as slip grows and the tangent +// (dyn_ph_tangent_D), the linearization's damping, loses definiteness: the motion can be locally +// unstable there, but the secant R, the energy's, stays positive semi-definite. These are +// evaluations: dyn_plant's arithmetic does not depend on them. // The kinetic energy 1/2 v^T M v. template @@ -480,11 +484,12 @@ CVC_HD inline void dyn_ph_grip(const dyn_bike_params &p, const dyn_state &s, // Everything the form is built from, at a state. template struct dyn_ph_parts { - S u, sg; // the slips' floored speed sqrt(vx^2 + eps^2), and sg = vx / u + S u, sg; // the floored speed sqrt(vx^2 + eps^2) (the rear slip's), and sg = vx / u + S uf; // the front wheel's floored forward speed, in its frame S Fxf, Fxr; // the axles' drive or brake forces: the inputs S Fyf, Fyr; // the tires' lateral forces - S wf, wr, w0f; // the axles' lateral velocities, and the front force's root - S Sf, Sr; // the secant stiffnesses (dyn_secant_w) + S wf, wr; // the axles' sideways velocities in their wheels' frames: n_f^T v, n_r^T v + S Sf, Sr; // the secant stiffnesses through wf, wr = 0 (dyn_secant_w) S Dx; // drag and rolling resistance on vx S cd; // cos delta S nf[3], nr[3]; // each tire's lateral force on (vx, vy, r) @@ -501,13 +506,14 @@ CVC_HD inline void dyn_ph_parts_at(const dyn_bike_params &p, const dyn_state q.sg = s.vx / q.u; const S cd = s_cos(s.delta), sd = s_sin(s.delta); q.cd = cd; - q.wf = s.vy + lf * s.r; + const S vf = s.vy + lf * s.r; + q.wf = -s.vx * sd + vf * cd; q.wr = s.vy - lr * s.r; - q.w0f = q.u * s_tan(q.sg * s.delta); - const S af = q.sg * s.delta - s_atan2(q.wf, q.u), ar = -s_atan2(q.wr, q.u); + q.uf = s_sqrt((s.vx * cd + vf * sd) * (s.vx * cd + vf * sd) + S(p.eps) * S(p.eps)); + const S af = -s_atan2(q.wf, q.uf), ar = -s_atan2(q.wr, q.u); q.Fyf = dyn_lateral(af, Bf, Cp, Dyf); q.Fyr = dyn_lateral(ar, Br, Cp, Dyr); - q.Sf = dyn_secant_w(q.Fyf, af, q.wf, q.w0f, Bf, Cp, Dyf, q.u); + q.Sf = dyn_secant_w(q.Fyf, af, q.wf, z, Bf, Cp, Dyf, q.uf); q.Sr = dyn_secant_w(q.Fyr, ar, q.wr, z, Br, Cp, Dyr, q.u); q.Dx = S(p.drag) * q.u + S(p.c_rr) * S(p.m) * S(p.g) / q.u; q.nf[0] = -sd; @@ -529,10 +535,9 @@ template CVC_HD inline void dyn_ph_R(const dyn_bike_params &p, const dyn_state &s, S R[3][3]) { dyn_ph_parts q; dyn_ph_parts_at(p, s, q); - const S kf = q.Sf / q.cd; for (int i = 0; i < 3; ++i) for (int j = i; j < 3; ++j) { - R[i][j] = kf * (q.nf[i] * q.nf[j]) + q.Sr * (q.nr[i] * q.nr[j]); + R[i][j] = q.Sf * (q.nf[i] * q.nf[j]) + q.Sr * (q.nr[i] * q.nr[j]); R[j][i] = R[i][j]; } R[0][0] = R[0][0] + q.Dx; @@ -556,7 +561,8 @@ CVC_HD inline void dyn_ph_input(const dyn_bike_params &p, const dyn_state &s, dyn_ph_axles(p, s, u[0], u[1], Dyf, Dyr); } -// f_eps, what the secant dissipation leaves of the tires' lateral forces: the speed floor's. +// f_eps, what the secant dissipation leaves of the tires' lateral forces: round-off, with both +// slips in their wheels' frames. template CVC_HD inline void dyn_ph_floor(const dyn_bike_params &p, const dyn_state &s, S f[3]) { dyn_ph_parts q; @@ -567,7 +573,7 @@ CVC_HD inline void dyn_ph_floor(const dyn_bike_params &p, const dyn_state &s, nfv = nfv + q.nf[i] * v[i]; nrv = nrv + q.nr[i] * v[i]; } - const S ef = q.Fyf + q.Sf / q.cd * nfv, er = q.Fyr + q.Sr * nrv; + const S ef = q.Fyf + q.Sf * nfv, er = q.Fyr + q.Sr * nrv; for (int i = 0; i < 3; ++i) f[i] = q.nf[i] * ef + q.nr[i] * er; } @@ -596,7 +602,7 @@ CVC_HD inline void dyn_ph_rhs(const dyn_bike_params &p, const dyn_state &s, S template struct dyn_ph_rates { S dissipated; // v^T R v >= 0: the tires' slip, drag and rolling resistance S supplied; // y^T u, y = G^T v: the axles' forces at their wheels' ground speed - S floor; // v^T f_eps: the slips' speed floor + S floor; // v^T f_eps: what the secants leave, round-off S grade; // - m g grade vx: gravity along the heading }; @@ -660,25 +666,32 @@ CVC_HD inline void dyn_plant_books(const dyn_bike_params &p, const bike_veh &v, // The tangent dissipation of dyn_plant's Pacejka tires at the state: Dt = -d(Q_y, Q_r)/d(vy, r) // with Q the tires' lateral force and yaw moment, = k_f a a^T + k_r b b^T where k is each tire's -// dF/dalpha times its slip's sensitivity to its lateral velocity (and the front's cos delta). -// With a and b independent, Dt is positive semi-definite exactly when both k are >= 0, i.e. both -// tires are below their peak; kf and kr are returned with it. alpha_f and alpha_r are the slips. +// dF/dalpha times its slip's sensitivity to (vy, r) along a or b (the front's turned by delta, and +// its force with it). With a and b independent, Dt is positive semi-definite exactly when both k +// are >= 0, i.e. both tires are below their peak (the front's slip sensitivity stays positive +// while its sideways velocity is under about cot delta times its forward speed); kf and kr are +// returned with it. alpha_f and alpha_r are the slips (dyn_plant's, in the wheels' frames). template CVC_HD inline void dyn_ph_tangent_D(const dyn_bike_params &p, const dyn_state &s, S Dt[2][2], S &kf, S &kr, S &alpha_f, S &alpha_r) { S Dyf, Dyr; dyn_ph_grip(p, s, Dyf, Dyr); const S lf = S(p.lf), lr = S(p.lr), C = S(p.C), Bf = S(p.B_f), Br = S(p.B_r); - const S u = s_sqrt(s.vx * s.vx + S(p.eps) * S(p.eps)), sg = s.vx / u; - const S wf = s.vy + lf * s.r, wr = s.vy - lr * s.r; - alpha_f = sg * s.delta - s_atan2(wf, u); + const S eps2 = S(p.eps) * S(p.eps), u = s_sqrt(s.vx * s.vx + eps2); + const S cd = s_cos(s.delta), sd = s_sin(s.delta), vf = s.vy + lf * s.r; + const S wf = -s.vx * sd + vf * cd, wr = s.vy - lr * s.r; + const S uf = s_sqrt((s.vx * cd + vf * sd) * (s.vx * cd + vf * sd) + eps2); + alpha_f = -s_atan2(wf, uf); alpha_r = -s_atan2(wr, u); // dF/dalpha of D sin(C atan(B alpha)). const S bf = Bf * alpha_f, br = Br * alpha_r; const S dFf = Dyf * C * Bf * s_cos(C * s_atan(bf)) / (S(1) + bf * bf); const S dFr = Dyr * C * Br * s_cos(C * s_atan(br)) / (S(1) + br * br); - // dalpha/dw = -u / (u^2 + w^2); the front force reaches (vy, r) through cos delta. - kf = dFf * s_cos(s.delta) * u / (u * u + wf * wf); + // The rear's slip moves with its w alone: dalpha/dw = -u / (u^2 + w^2). The front's w and its + // forward speed lon (floored to uf) both move with (vy, r) along (1, lf), as cos delta and sin + // delta, and its force reaches them through cos delta. + const S lon = s.vx * cd + vf * sd; + kf = dFf * cd * (cd * uf - sd * wf * lon / uf) / (uf * uf + wf * wf); kr = dFr * u / (u * u + wr * wr); Dt[0][0] = kf + kr; Dt[0][1] = kf * lf - kr * lr; diff --git a/inc/cvc/physics/dyn_bike_plant.h b/inc/cvc/physics/dyn_bike_plant.h index 34c1e12a0..9a5bad77d 100644 --- a/inc/cvc/physics/dyn_bike_plant.h +++ b/inc/cvc/physics/dyn_bike_plant.h @@ -27,10 +27,10 @@ // kinetic now = kinetic at attach + drive - brake - dissipated + floor + grade + limiter // + residual // drive and brake are the axles' forces where they push and where they hold back, dissipated the -// tires' slip, drag and rolling resistance, floor the slips' speed floor, limiter the kinetic -// energy the speed clamp and the low-speed blend took, and residual the integrator's error (second -// order per step). Positions, speeds and accelerations cross into nav's normalized frame through -// world_units with the sim_world's frame, as nav_plant's do. +// tires' slip, drag and rolling resistance, floor what the tires' secants leave (round-off), +// limiter the kinetic energy the speed clamp and the low-speed blend took, and residual the +// integrator's error (second order per step). Positions, speeds and accelerations cross into +// nav's normalized frame through world_units with the sim_world's frame, as nav_plant's do. #ifndef __CVC_PHYSICS_DYN_BIKE_PLANT_H__ #define __CVC_PHYSICS_DYN_BIKE_PLANT_H__ @@ -61,7 +61,7 @@ struct dyn_bike_books { double drive = 0.0; // the axles' forces where they push (>= 0) double brake = 0.0; // where they hold back (>= 0) double dissipated = 0.0; // the tires' slip, drag and rolling resistance (>= 0) - double floor = 0.0; // the slips' speed floor + double floor = 0.0; // what the tires' secants leave: round-off double grade = 0.0; // gravity along the heading (0 on this flat plant) double limiter = 0.0; // the speed clamp's and the low-speed blend's (<= 0 as a rule) double residual = 0.0; // the integrator's diff --git a/src/cvc/tests/data/dyn_bike_golden.json b/src/cvc/tests/data/dyn_bike_golden.json index 5801024a0..b1d62bd79 100644 --- a/src/cvc/tests/data/dyn_bike_golden.json +++ b/src/cvc/tests/data/dyn_bike_golden.json @@ -6,38 +6,38 @@ "cases": [ {"state": [0, 0, 0, 12, 0.10000000000000001, 0.050000000000000003, 0.02, 0.5], "delta_cmd": 0.050000000000000003, "a_cmd": 1, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "next": [0.72170075978362558, 0.0014655898551522932, 0.0048612338851567365, 12.028322883477824, 0.10879309514548365, 0.08102056656356306, 0.035978411112368727, 0.72559417581953878], - "g_state": [0.69999999999999996, -0.29999999999999999, 0.88246385916630588, 0.44344941191751436, -0.58216522580427366, 0.44787693050917154, -0.31518187358286542, -0.067550279387271248], - "g_delta_cmd": -0.13140556288176811, "g_a_cmd": -0.055534351904751283}, + "next": [0.72170075975136427, 0.0014655899335806613, 0.004861234555351427, 12.028322882923678, 0.10879310808645795, 0.081020577733474819, 0.035978411112368727, 0.72559417581953878], + "g_state": [0.69999999999999996, -0.29999999999999999, 0.88246385912108438, 0.44344941524854337, -0.58216528949343793, 0.44787686564579765, -0.3151821665322459, -0.067550279030260257], + "g_delta_cmd": -0.13140568501816438, "g_a_cmd": -0.055534351611245841}, {"state": [1, -2, 0.29999999999999999, 15, -1.2, 0.59999999999999998, 0.25, 0], "delta_cmd": 0.29999999999999999, "a_cmd": 0, "grade": 0.02, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "next": [1.8867829073011033, -1.8363951317071947, 0.3362258629537182, 14.851775313286971, -1.2377173130869106, 0.60376439605715648, 0.26954533644854789, 0], - "g_state": [0.69999999999999996, -0.29999999999999999, 0.71944172000470541, 0.4699842462121625, -0.88320019268164862, 1.0507783479334352, 0.44490310874960692, -0.076361021377209612], - "g_delta_cmd": 0.045866872760495392, "g_a_cmd": -0.062777828181229955}, + "next": [1.8867829081549301, -1.8363951306411967, 0.33622585953649098, 14.851775340449274, -1.2377173506677357, 0.60376433910336746, 0.26954533644854789, 0], + "g_state": [0.69999999999999996, -0.29999999999999999, 0.71944171900235887, 0.4699842386486896, -0.8832001588377606, 1.0507783463866145, 0.44490271392520953, -0.076361022229101155], + "g_delta_cmd": 0.045866832056440862, "g_a_cmd": -0.062777828881585979}, {"state": [-3, 4, -1.1000000000000001, 14, 0.80000000000000004, 0.40000000000000002, 0.14999999999999999, -4], "delta_cmd": 0.14999999999999999, "a_cmd": -6, "grade": -0.029999999999999999, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "next": [-2.6188025127373895, 3.2714360882174414, -1.0723766257472249, 13.702551861063213, 0.58732269798076042, 0.4603895811700956, 0.14999999999999999, -4.9023767032781551], - "g_state": [0.69999999999999996, -0.29999999999999999, 1.495635492069008, 0.45176658307929768, -0.64916167547811943, 0.61889519194170228, -0.0099817306380819981, -0.073112649574912106], - "g_delta_cmd": -0.023158780690115677, "g_a_cmd": -0.060107280784200925}, + "next": [-2.6188025154037322, 3.2714360947192382, -1.0723765861183387, 13.702551735749431, 0.58732333216192778, 0.46039024165154702, 0.14999999999999999, -4.9023767032781551], + "g_state": [0.69999999999999996, -0.29999999999999999, 1.495635488317653, 0.45176678020997779, -0.64916254395814799, 0.61889452005688128, -0.0099815902073079887, -0.073112673392279448], + "g_delta_cmd": -0.023158454874321499, "g_a_cmd": -0.060107300364905421}, {"state": [2, 2, 2.5, 10, 2.5, 1.1000000000000001, 0.40000000000000002, 1], "delta_cmd": 0.45000000000000001, "a_cmd": 2, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "next": [1.4909879252982987, 2.3339382319443089, 2.5710190927434646, 10.145752585238649, 1.9848686819797339, 1.1836515721810814, 0.41954533644854791, 1.4511883516390776], - "g_state": [0.69999999999999996, -0.29999999999999999, 1.0189468600494942, 0.36877586997421102, -0.5469687755509326, 0.34522820509560315, -0.2583148462334196, -0.07068503726719233], - "g_delta_cmd": -0.026630729143777142, "g_a_cmd": -0.058111495164835755}, + "next": [1.4909888326903478, 2.3339373396570302, 2.5710213639083945, 10.145731593921647, 1.9849167211699097, 1.1836894249307619, 0.41954533644854791, 1.4511883516390776], + "g_state": [0.69999999999999996, -0.29999999999999999, 1.0189472124329746, 0.36880630963593192, -0.54701549293447493, 0.34517948655403108, -0.2580881259450748, -0.070683132111571817], + "g_delta_cmd": -0.026607355626233113, "g_a_cmd": -0.058109928900660059}, {"state": [0.5, 0.5, 0.20000000000000001, -2, 0.050000000000000003, -0.10000000000000001, 0.20000000000000001, -0.5], "delta_cmd": 0.25, "a_cmd": -0.80000000000000004, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 1}, - "next": [0.38163390582407375, 0.4801677888170704, 0.19227964555052293, -1.9995770449242993, -0.17430505462442358, -0.12867257703400767, 0.2195453364485479, -0.63535650549172329], - "g_state": [0.69999999999999996, -0.29999999999999999, 1.1493923760808287, 0.39264945863371031, -0.21968909271960391, 0.10685582785702351, 1.1849225895596516, -0.070144184766086387], - "g_delta_cmd": 0.12215849378781682, "g_a_cmd": -0.057666850177465923}, + "next": [0.38163530692629388, 0.48016866043246414, 0.19227568002672468, -1.9995515559036319, -0.17441970743633498, -0.12873866909878903, 0.2195453364485479, -0.63535650549172329], + "g_state": [0.69999999999999996, -0.29999999999999999, 1.1493913456193869, 0.39272222483394581, -0.21987471522277591, 0.1065944814842828, 1.1859449200172896, -0.0701446613306866], + "g_delta_cmd": 0.12226388999678342, "g_a_cmd": -0.057667241970163943}, {"state": [0, 0, 0, 0.59999999999999998, 0.02, 0.050000000000000003, 0.29999999999999999, 0.29999999999999999], "delta_cmd": 0.34999999999999998, "a_cmd": 0.5, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "next": [0.036071137283442252, -1.6430282470171074e-05, 0.0036051455458450143, 0.60118074078138972, 0.10349000305763768, 0.060085760440438306, 0.31954533644854788, 0.39023767032781553], - "g_state": [0.69999999999999996, -0.29999999999999999, 1.0891901600126965, 0.30814654633137256, 0.022983162019829553, 0.083479081063051172, 0.27490937611823435, -0.071961486030844291], - "g_delta_cmd": 0.028341526788878316, "g_a_cmd": -0.059160887639182654}, + "next": [0.03604862124528898, -1.636622014404493e-05, 0.0036172999629599093, 0.60080545007211938, 0.10384225850613707, 0.060288334063547763, 0.31954533644854788, 0.39023767032781553], + "g_state": [0.69999999999999996, -0.29999999999999999, 1.0891968699805141, 0.30795040189205575, 0.022457640666817331, 0.08274162873772585, 0.27113727747939381, -0.071970766724930635], + "g_delta_cmd": 0.027952645783317371, "g_a_cmd": -0.059168517471893736}, {"state": [0, 0, 0, 0, 0, 0, 0, 0], "delta_cmd": 0.10000000000000001, "a_cmd": 2, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "next": [0.0032481985805457771, 1.4550464025321322e-08, 2.063503148721843e-05, 0.054136644212622924, 0.00060545142240215992, 0.00034391719914078894, 0.020946006673345115, 0.90237670327815533], - "g_state": [0.69999999999999996, -0.29999999999999999, 1.0990255302405116, 0.42627209229010737, -0.0055533005664103159, 0.0088728813432996916, 0.57684392487435543, -0.068034179542302314], - "g_delta_cmd": 0.0020775987408388598, "g_a_cmd": -0.055932175300006146} + "next": [0.0032481984844283298, 1.4544839207109448e-08, 2.0635478199194948e-05, 0.054136642610663102, 0.00060546443039204611, 0.00034392464434056399, 0.020946006673345115, 0.90237670327815533], + "g_state": [0.69999999999999996, -0.29999999999999999, 1.0990255302732841, 0.42627192983680606, -0.0055542541340733737, 0.0088716001192301885, 0.57684245637054032, -0.068034185585478177], + "g_delta_cmd": 0.0020775934517796436, "g_a_cmd": -0.055932180268214393} ] } diff --git a/src/cvc/tests/data/dyn_bike_ph_golden.json b/src/cvc/tests/data/dyn_bike_ph_golden.json index fcd01fb80..e59bd61d3 100644 --- a/src/cvc/tests/data/dyn_bike_ph_golden.json +++ b/src/cvc/tests/data/dyn_bike_ph_golden.json @@ -6,75 +6,75 @@ "cases": [ {"name": "cruising", "state": [0, 0, 0, 12, 0.10000000000000001, 0.050000000000000003, 0.02, 0.5], "delta_cmd": 0.050000000000000003, "a_cmd": 1, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "parts": {"u": 12.010412149464313, "sg": 0.999133073092352, "Fxf": 668.99999999379133, "Fxr": 668.99999998711121, "Fyf": 833.2364968298391, "Fyr": -142.89201640763156, "wf": 0.17684999704360962, "wr": 0.01184999942779541, "w0f": 0.24003194964382116, "Sf": 13187.887720124836, "Sr": 12058.398591350429, "Dx": 50.797930946489572, "cd": 0.99980000666657776}, - "rates": {"dissipated": 7369.2303991363142, "supplied": 16056.760548503509, "floor": 4.6216263526531047e-05, "grade": -0}, - "next": [0.72170075978362558, 0.0014655898551522932, 0.0048612338851567365, 12.028322883477824, 0.10879309514548365, 0.08102056656356306, 0.035978411112368727, 0.72559417581953878], - "free": {"vx": 12.028322883477824, "vy": 0.10879309514548363, "r": 0.08102056656356306}, - "books": {"dH": 925.80375282801106, "dissipated": 480.2778572362302, "supplied": 1399.6434267494701, "floor": 5.6032430529408864e-05, "grade": -0, "limiter": 0, "residual": 6.438127282340588}, - "g_state": [0, 0, 0, 94.572062704034721, 266.26204849829253, 178.23833422073534, -2291.7756449200788, 1696.8050928496623], - "g_delta_cmd": -955.48663759140084, "g_a_cmd": 1394.9752983233941}, + "parts": {"u": 12.010412149464313, "sg": 0.999133073092352, "uf": 12.011548008141299, "Fxf": 668.99999999379133, "Fxr": 668.99999998711121, "Fyf": 833.23693799549858, "Fyr": -142.89201640763156, "wf": -0.063169372096811782, "wr": 0.01184999942779541, "Sf": 13190.521139239769, "Sr": 12058.398591350429, "Dx": 50.797930946489572, "cd": 0.99980000666657776}, + "rates": {"dissipated": 7369.2303807882108, "supplied": 16056.760548503509, "floor": -7.1815261545466192e-15, "grade": -0}, + "next": [0.72170075975136427, 0.0014655899335806613, 0.004861234555351427, 12.028322882923678, 0.10879310808645795, 0.081020577733474819, 0.035978411112368727, 0.72559417581953878], + "free": {"vx": 12.028322882923678, "vy": 0.10879310808645794, "r": 0.081020577733474819}, + "books": {"dH": 925.8037444454967, "dissipated": 480.27780755223807, "supplied": 1399.6434267487455, "floor": 0, "grade": -0, "limiter": 0, "residual": 6.4381252489893086}, + "g_state": [0, 0, 0, 94.572064520832427, 266.262240885626, 178.23863493457046, -2291.7811830755809, 1696.8050946214528], + "g_delta_cmd": -955.48894655807101, "g_a_cmd": 1394.9752997800158}, {"name": "hard corner", "state": [1, -2, 0.29999999999999999, 15, -1.2, 0.59999999999999998, 0.25, 0], "delta_cmd": 0.29999999999999999, "a_cmd": 0, "grade": 0.02, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "parts": {"u": 15.008331019803634, "sg": 0.99944490697915433, "Fxf": 0, "Fxr": 0, "Fyf": 12572.367058289443, "Fyr": 10317.129343040195, "wf": -0.27780003547668453, "wr": -2.257800006866455, "w0f": 3.8300376059888634, "Sf": 3060.5803236673237, "Sr": 4569.54969955868, "Dx": 48.749905116075631, "cd": 0.96891242171064473}, - "rates": {"dissipated": 84304.682707770335, "supplied": 0, "floor": 1.1111296915389401, "grade": -7872.7787498474127}, - "next": [1.8867829073011033, -1.8363951317071947, 0.3362258629537182, 14.851775313286971, -1.2377173130869106, 0.60376439605715648, 0.26954533644854789, 0], - "free": {"vx": 14.851775313286971, "vy": -1.2377173130869106, "r": 0.60376439605715648}, - "books": {"dH": -5783.0839400431141, "dissipated": 5285.4247061505548, "supplied": 0, "floor": 0.084403202039299102, "grade": -470.0328341572777, "limiter": 0, "residual": -27.710802937321205}, - "g_state": [0, 0, 0, -463.19990518332662, 1821.5216108242723, -2334.1217865019594, -12976.335618546113, 2130.7992682734298], - "g_delta_cmd": -1337.7832678807169, "g_a_cmd": 1751.7700515237402}, + "parts": {"u": 15.008331019803634, "sg": 0.99944490697915433, "uf": 14.473596491199604, "Fxf": 0, "Fxr": 0, "Fyf": 12572.361643527407, "Fyr": 10317.129343040195, "wf": -3.9802232939428617, "wr": -2.257800006866455, "Sf": 3158.7076189067425, "Sr": 4569.54969955868, "Dx": 48.749905116075631, "cd": 0.96891242171064473}, + "rates": {"dissipated": 84303.550026116834, "supplied": 0, "floor": 0, "grade": -7872.7787498474127}, + "next": [1.8867829081549301, -1.8363951306411967, 0.33622585953649098, 14.851775340449274, -1.2377173506677357, 0.60376433910336746, 0.26954533644854789, 0], + "free": {"vx": 14.851775340449274, "vy": -1.2377173506677357, "r": 0.60376433910336746}, + "books": {"dH": -5783.0829521248816, "dissipated": 5285.3392802645831, "supplied": 0, "floor": 1.2562780298827552e-13, "grade": -470.03283458496333, "limiter": 0, "residual": -27.710837275335507}, + "g_state": [0, 0, 0, -463.19491695551238, 1821.5232926523331, -2334.1207298619406, -12977.134125674449, 2130.801103547838], + "g_delta_cmd": -1337.8655892314412, "g_a_cmd": 1751.7715603372592}, {"name": "trail braking", "state": [-3, 4, -1.1000000000000001, 14, 0.80000000000000004, 0.40000000000000002, 0.14999999999999999, -4], "delta_cmd": 0.14999999999999999, "a_cmd": -6, "grade": -0.029999999999999999, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "parts": {"u": 14.0089257261219, "sg": 0.99936285434754957, "Fxf": -7551.3663373512209, "Fxr": -3148.1333008982315, "Fyf": 7001.6152821298219, "Fyr": -717.59662961739537, "wf": 1.414799976348877, "wr": 0.094799995422363281, "w0f": 2.1158726232176082, "Sf": 9987.003933760383, "Sr": 7569.5850661203158, "Dx": 49.121296036861125, "cd": 0.98877107793604224}, - "rates": {"dissipated": 14549.607263992975, "supplied": -150202.43120684219, "floor": 0.14143812354461546, "grade": 11021.890249786376}, - "next": [-2.6188025127373895, 3.2714360882174414, -1.0723766257472249, 13.702551861063213, 0.58732269798076042, 0.4603895811700956, 0.14999999999999999, -4.9023767032781551], - "free": {"vx": 13.702551861063213, "vy": 0.58732269798076042, "r": 0.4603895811700956}, - "books": {"dH": -11256.750776780944, "dissipated": 886.6551612188855, "supplied": -10910.422291267738, "floor": 0.0085982852894107929, "grade": 654.28817020049235, "limiter": 0, "residual": -113.97009278010228}, - "g_state": [0, 0, 0, -1495.197249568253, 595.46430683739527, -493.32365527179718, -3135.8971941691698, 1906.0968547428911], - "g_delta_cmd": -7275.6476827216156, "g_a_cmd": 1567.0379820187352}, + "parts": {"u": 14.0089257261219, "sg": 0.99936285434754957, "uf": 14.063111470527989, "Fxf": -7551.3663373512209, "Fxr": -3148.1333008982315, "Fyf": 7001.6926890943751, "Fyr": -717.59662961739537, "wf": -0.69322055695202289, "wr": 0.094799995422363281, "Sf": 10100.238111633142, "Sr": 7569.5850661203158, "Dx": 49.121296036861125, "cd": 0.98877107793604224}, + "rates": {"dissipated": 14549.519485968511, "supplied": -150202.43120684219, "floor": -6.3048042370794328e-13, "grade": 11021.890249786376}, + "next": [-2.6188025154037322, 3.2714360947192382, -1.0723765861183387, 13.702551735749431, 0.58732333216192778, 0.46039024165154702, 0.14999999999999999, -4.9023767032781551], + "free": {"vx": 13.702551735749431, "vy": 0.58732333216192778, "r": 0.46039024165154702}, + "books": {"dH": -11256.752464323392, "dissipated": 886.64892824579886, "supplied": -10910.422310739146, "floor": 0, "grade": 654.28816724078933, "limiter": 0, "residual": -113.96939257923623}, + "g_state": [0, 0, 0, -1495.1973149984308, 595.47077607144365, -493.31337498690755, -3135.9854249086593, 1906.0965965650007], + "g_delta_cmd": -7275.8523883403222, "g_a_cmd": 1567.037769765848}, {"name": "past the peak", "state": [2, 2, 2.5, 10, 2.5, 1.1000000000000001, 0.40000000000000002, 1], "delta_cmd": 0.45000000000000001, "a_cmd": 2, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "parts": {"u": 10.012492197250394, "sg": 0.99875233887784454, "Fxf": 1337.9999962059987, "Fxr": 1337.9999945396203, "Fyf": 480.06651262852165, "Fyr": -6926.5853953834903, "wf": 4.1906999349594116, "wr": 0.56069998741149885, "w0f": 4.2273249542110252, "Sf": 13107.611202344198, "Sr": 12353.460943276346, "Dx": 54.339776317364368, "cd": 0.9210609940028851}, - "rates": {"dissipated": 9334.4499331538391, "supplied": 27887.325598366322, "floor": 0.27295228804807259, "grade": -0}, - "next": [1.4909879252982987, 2.3339382319443089, 2.5710190927434646, 10.145752585238649, 1.9848686819797339, 1.1836515721810814, 0.41954533644854791, 1.4511883516390776], - "free": {"vx": 10.145752585238649, "vy": 1.9848686819797099, "r": 1.183651572181071}, - "books": {"dH": 1437.7772360406525, "dissipated": 624.33651714749806, "supplied": 2434.9378987666028, "floor": 0.20744834669852991, "grade": -0, "limiter": 2.3283064365386963e-10, "residual": -373.03159392538373}, - "g_state": [0, 0, 0, -196.40742046722073, 258.4249427185905, 162.64432813704843, -6670.0189402365349, 1471.2756168733499], - "g_delta_cmd": -687.63940737960604, "g_a_cmd": 1209.5632852663059}, + "parts": {"u": 10.012492197250394, "sg": 0.99875233887784454, "uf": 10.854067897474573, "Fxf": 1337.9999962059987, "Fxr": 1337.9999945396203, "Fyf": 487.91279032092478, "Fyr": -6926.5853953834903, "wf": -0.034293175424963795, "wr": 0.56069998741149885, "Sf": 14227.693535949067, "Sr": 12353.460943276346, "Dx": 54.339776317364368, "cd": 0.9210609940028851}, + "rates": {"dissipated": 9334.4460546432092, "supplied": 27887.325598366322, "floor": 0, "grade": -0}, + "next": [1.4909888326903478, 2.3339373396570302, 2.5710213639083945, 10.145731593921647, 1.9849167211699097, 1.1836894249307619, 0.41954533644854791, 1.4511883516390776], + "free": {"vx": 10.145731593921647, "vy": 1.9849167211698857, "r": 1.1836894249307515}, + "books": {"dH": 1437.7440273741377, "dissipated": 624.21828040870867, "supplied": 2434.9380796702599, "floor": -1.7483807432046598e-14, "grade": -0, "limiter": 2.0372681319713593e-10, "residual": -372.97577188761716}, + "g_state": [0, 0, 0, -196.55967045091668, 258.80841151206471, 163.14736952022884, -6675.3928259214636, 1471.2798431970089], + "g_delta_cmd": -688.19342313290485, "g_a_cmd": 1209.5667598062705}, {"name": "backing up", "state": [0.5, 0.5, 0.20000000000000001, -2, 0.050000000000000003, -0.10000000000000001, 0.20000000000000001, -0.5], "delta_cmd": 0.25, "a_cmd": -0.80000000000000004, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 1}, - "parts": {"u": 2.0615528128088303, "sg": -0.97014250014533188, "Fxf": -944.4706115932695, "Fxr": -393.5293883083641, "Fyf": -11960.444792630304, "Fyr": -9240.0034610141138, "wf": -0.10369999408721924, "wr": 0.22630000114440918, "w0f": -0.40509636691446466, "Sf": 39683.439719049973, "Sr": 40830.77072155106, "Dx": 194.03654493800514, "cd": 0.98006657784124163}, - "rates": {"dissipated": 6407.7299751548862, "supplied": 2657.8049284691688, "floor": 3.7985452324499418, "grade": 0}, - "next": [0.38163390582407375, 0.4801677888170704, 0.19227964555052293, -1.9995770449242993, -0.17430505462442358, -0.12867257703400767, 0.2195453364485479, -0.63535650549172329], - "free": {"vx": -1.9995770449242993, "vy": -0.17244089459456319, "r": -0.12848239017639973}, - "books": {"dH": 55.642971390501771, "dissipated": 250.1420964398819, "supplied": 204.26131283110482, "floor": 0.22447606907508749, "grade": 0, "limiter": 1.0185296015806671, "residual": 100.2807493286231}, - "g_state": [0, 0, 0, -14.765472460519435, -755.8199706286432, -373.902656142647, -935.21641114208364, -285.87310632729543], - "g_delta_cmd": -96.415267256589999, "g_a_cmd": -235.02164359479943}, + "parts": {"u": 2.0615528128088303, "sg": -0.97014250014533188, "uf": 2.042868520083168, "Fxf": -944.4706115932695, "Fxr": -393.5293883083641, "Fyf": -11960.136319019375, "Fyr": -9240.0034610141138, "wf": 0.29570576326290449, "wr": 0.22630000114440918, "Sf": 40446.071077708155, "Sr": 40830.77072155106, "Dx": 194.03654493800514, "cd": 0.98006657784124163}, + "rates": {"dissipated": 6403.8402124978693, "supplied": 2657.8049284691688, "floor": 0, "grade": 0}, + "next": [0.38163530692629388, 0.48016866043246414, 0.19227568002672468, -1.9995515559036319, -0.17441970743633498, -0.12873866909878903, 0.2195453364485479, -0.63535650549172329], + "free": {"vx": -1.9995515559036319, "vy": -0.17255881842201107, "r": -0.12855043991549875}, + "books": {"dH": 55.613531895101005, "dissipated": 249.89034374116878, "supplied": 204.26177956809806, "floor": -1.8245970483900868e-14, "grade": 0, "limiter": 1.0160887555757654, "residual": 100.22600731259598}, + "g_state": [0, 0, 0, -14.832047057434986, -756.19407761705133, -374.42277645231206, -939.40325009420303, -285.87289157067966], + "g_delta_cmd": -96.846905529528357, "g_a_cmd": -235.02146703935702}, {"name": "creeping", "state": [0, 0, 0, 0.59999999999999998, 0.02, 0.050000000000000003, 0.29999999999999999, 0.29999999999999999], "delta_cmd": 0.34999999999999998, "a_cmd": 0.5, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "parts": {"u": 0.78102496759066542, "sg": 0.76822127959737585, "Fxf": 401.39999999994171, "Fxr": 401.39999999985969, "Fyf": 10648.87473784531, "Fyr": 8751.4437535048728, "wf": 0.096849997043609623, "wr": -0.068150000572204592, "w0f": 0.18325608116457207, "Sf": 123242.18654484602, "Sr": 128414.43404292801, "Dx": 505.17502810405216, "cd": 0.95533648912560598}, - "rates": {"dissipated": 1705.6793444363263, "supplied": 482.41176208108084, "floor": 24.510905403308719, "grade": -0}, - "next": [0.036071137283442252, -1.6430282470171074e-05, 0.0036051455458450143, 0.60118074078138972, 0.10349000305763768, 0.060085760440438306, 0.31954533644854788, 0.39023767032781553], - "free": {"vx": 0.60118074078138972, "vy": 0.097734281068398154, "r": 0.05968543769212728}, - "books": {"dH": 19.181072855414982, "dissipated": 62.662709369358424, "supplied": 38.087341472711124, "floor": 1.0740101336170835, "grade": -0, "limiter": 1.7003021666398581, "residual": 40.982128451805337}, - "g_state": [0, 0, 0, 73.806285726693346, -138.28621683960841, 132.72750314154956, 36.853793319321852, 86.942492019706492], - "g_delta_cmd": 3.7994075916207333, "g_a_cmd": 71.47705370124973}, + "parts": {"u": 0.78102496759066542, "sg": 0.76822127959737585, "uf": 0.78242632430086545, "Fxf": 401.39999999994171, "Fxr": 401.39999999985969, "Fyf": 10679.154284884773, "Fyr": 8751.4437535048728, "wf": -0.084787787849336368, "wr": -0.068150000572204592, "Sf": 125951.56160768215, "Sr": 128414.43404292801, "Dx": 505.17502810405216, "cd": 0.95533648912560598}, + "rates": {"dissipated": 1683.7357748435741, "supplied": 482.41176208108084, "floor": 0, "grade": -0}, + "next": [0.03604862124528898, -1.636622014404493e-05, 0.0036172999629599093, 0.60080545007211938, 0.10384225850613707, 0.060288334063547763, 0.31954533644854788, 0.39023767032781553], + "free": {"vx": 0.60080545007211938, "vy": 0.098947758081639442, "r": 0.060384998727689883}, + "books": {"dH": 18.751840864307155, "dissipated": 61.57551538950834, "supplied": 38.087143756590955, "floor": -5.8147371602161802e-15, "grade": -0, "limiter": 1.2913905259552507, "residual": 40.948821971269297}, + "g_state": [0, 0, 0, 72.615683184303919, -133.28446614610823, 140.32456812711035, 17.021123399802804, 86.95547904339746], + "g_delta_cmd": 1.7547769072992061, "g_a_cmd": 71.487730577058286}, {"name": "pulling away", "state": [0, 0, 0, 0, 0, 0, 0, 0], "delta_cmd": 0.10000000000000001, "a_cmd": 2, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "parts": {"u": 0.5, "sg": 0, "Fxf": 0, "Fxr": 0, "Fyf": 0, "Fyr": -0, "wf": 0, "wr": 0, "w0f": 0, "Sf": 324875.10216936003, "Sr": 283229.16028579313, "Dx": 788.028169894862, "cd": 1}, + "parts": {"u": 0.5, "sg": 0, "uf": 0.5, "Fxf": 0, "Fxr": 0, "Fyf": -0, "Fyr": -0, "wf": 0, "wr": 0, "Sf": 324875.10216936003, "Sr": 283229.16028579313, "Dx": 788.028169894862, "cd": 1}, "rates": {"dissipated": 0, "supplied": 0, "floor": 0, "grade": -0}, - "next": [0.0032481985805457771, 1.4550464025321322e-08, 2.063503148721843e-05, 0.054136644212622924, 0.00060545142240215992, 0.00034391719914078894, 0.020946006673345115, 0.90237670327815533], - "free": {"vx": 0.054136644212622924, "vy": 0.00060314686984607434, "r": 0.00034521109633305774}, - "books": {"dH": 3.9222407051603652, "dissipated": 0.068884470032634745, "supplied": 3.9222400422277417, "floor": 5.7135242476698058e-10, "grade": -0, "limiter": 9.2523945038891497e-07, "residual": 0.068884207154455535}, - "g_state": [0, 0, 0, 228.92649419811758, 2.465602765855218, 3.2564308892131937, -0.0010988264864153741, 7.6917420463825845], - "g_delta_cmd": -3.9576052137056676e-06, "g_a_cmd": 6.323525430819652}, + "next": [0.0032481984844283298, 1.4544839207109448e-08, 2.0635478199194948e-05, 0.054136642610663102, 0.00060546443039204611, 0.00034392464434056399, 0.020946006673345115, 0.90237670327815533], + "free": {"vx": 0.054136642610663102, "vy": 0.00060322817490971381, "r": 0.00034525763128426669}, + "books": {"dH": 3.9222405102503846, "dissipated": 0.068884465200983236, "supplied": 3.9222400421321475, "floor": 5.9237282935762677e-21, "grade": -0, "limiter": 7.3021745494727952e-07, "residual": 0.068884203101765351}, + "g_state": [0, 0, 0, 228.92647957635094, 2.4657391465317136, 3.2566150350794718, -0.0011625576242231947, 7.6917416456190155], + "g_delta_cmd": -4.1871434405159457e-06, "g_a_cmd": 6.3235251013444014}, {"name": "stopping at the clamp", "state": [0, 0, 0, 0.29999999999999999, 0, 0.050000000000000003, 0.20000000000000001, -6], "delta_cmd": 0.20000000000000001, "a_cmd": -6, "grade": 0, "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, - "parts": {"u": 0.58309518948452999, "sg": 0.51449575542752657, "Fxf": -11240.746548567124, "Fxr": -4711.3658366127747, "Fyf": -3860.3908643551163, "Fyr": 5792.8950502960515, "wf": 0.076849997043609619, "wr": -0.088150000572204595, "w0f": 0.060212665452055336, "Sf": 232031.85217001935, "Sr": 65716.335935256517, "Dx": 675.96020050715163, "cd": 0.98006657784124163}, - "rates": {"dissipated": 629.96568739769941, "supplied": -4890.0345190848939, "floor": -2.1894094567378808, "grade": -0}, - "next": [1.7498980678721112e-09, -8.6535871941968698e-05, 2.0221649454310458e-05, 0, -0.00084808508129440973, 0.00033702749843832593, 0.20000000000000001, -6], - "free": {"vx": -0.059860071445697161, "vy": -0.0053382628087870069, "r": 0.0021214161174794573}, - "books": {"dH": -128.27329808069157, "dissipated": 20.195298564663304, "supplied": -118.31738102777641, "floor": -0.052474467419384233, "grade": 0, "limiter": -4.845308680962499, "residual": 15.137164660130026}, - "g_state": [0, 0, 0, -1327.1231135289845, 41.284887027157239, -65.788762611175002, -12.594168378664648, 0.89760497109159187], - "g_delta_cmd": -29.219941313896982, "g_a_cmd": 0.73793788550114403} + "parts": {"u": 0.58309518948452999, "sg": 0.51449575542752657, "uf": 0.58792762145910626, "Fxf": -11240.746548567124, "Fxr": -4711.3658366127747, "Fyf": -3683.5270922583381, "Fyr": 5792.8950502960515, "wf": 0.01571731437112165, "wr": -0.088150000572204595, "Sf": 234361.10045786831, "Sr": 65716.335935256517, "Dx": 675.96020050715163, "cd": 0.98006657784124163}, + "rates": {"dissipated": 629.37527334753008, "supplied": -4890.0345190848939, "floor": 5.7179256586538629e-14, "grade": -0}, + "next": [1.7399731866036951e-09, -8.6532572951146709e-05, 2.0107725068472442e-05, 0, -0.00085137757640991497, 0.00033512875863191886, 0.20000000000000001, -6], + "free": {"vx": -0.05985610192926677, "vy": -0.0053589873853782961, "r": 0.0021094645193253715}, + "books": {"dH": -128.27329460373102, "dissipated": 20.155082913704874, "supplied": -118.31663486590874, "floor": 1.5814496781514945e-15, "grade": 0, "limiter": -4.8448071251733467, "residual": 15.043230301055928}, + "g_state": [0, 0, 0, -1326.4071056378646, 40.968704718006236, -66.279837827341339, -12.643520545397898, 0.91563931235301865], + "g_delta_cmd": -29.334444103783618, "g_a_cmd": 0.75276425577032835} ] } diff --git a/src/cvc/tests/data/material_ext_golden.json b/src/cvc/tests/data/material_ext_golden.json index eb64a8284..94d0d01b9 100644 --- a/src/cvc/tests/data/material_ext_golden.json +++ b/src/cvc/tests/data/material_ext_golden.json @@ -81,7 +81,7 @@ "ext_patch": [0, 0.175529182, 0.284695387, 0.286225736, 0.179541647, 0.00497756759, -0.171468407, -0.283086658, -0.287677288, 0.0742211938, 0.230263054, 0.299248517, 0.255095959, 0.114498302, -0.0693881437, -0.227040753, -0.298855215, -0.257680357, 0.143827677, 0.270680279, 0.295195788, 0.208105519, 0.042336002, -0.139439642, -0.268496811, -0.296042383, -0.211662099, 0.204491645, 0.294267923, 0.27278924, 0.148176089, -0.0324585401, -0.200821444, -0.293259054, -0.27482307, -0.152483732, 0.252441287, 0.299559414, 0.233421966, 0.0790338069, -0.105234973, -0.249717161, -0.299787849, -0.23651661, -0.0838246495, 0.284695387, 0.286225736, 0.179541647, 0.00497756759, -0.171468407, -0.283086658, -0.287677288, -0.183504671, -0.0099537652, 0.299248517, 0.255095959, 0.114498302, -0.0693881437, -0.227040753, -0.298855215, -0.257680357, -0.119083315, 0.0645359978, 0.295195788, 0.208105519, 0.042336002, -0.139439642, -0.268496811, -0.296042383, -0.211662099, -0.0472579338, 0.135013223, 0.27278924, 0.148176089, -0.0324585401, -0.200821444, -0.293259054, -0.27482307, -0.152483732, 0.0275057256, 0.19709599, 0.300000012, 0.279152304, 0.219506666, 0.129352957, 0.0212211609, -0.0898600593, -0.18845211, -0.260852158, -0.296997756, 0.263274789, 0.297659308, 0.290673733, 0.243288934, 0.162090689, 0.0583643168, -0.053473819, -0.157879904, -0.240343079, 0.162090689, 0.243288934, 0.290673733, 0.297659308, 0.263274789, 0.192299068, 0.0945967138, -0.0162531417, -0.124844059, 0.0212211609, 0.129352957, 0.219506666, 0.279152304, 0.300000012, 0.279152304, 0.219506666, 0.129352957, 0.0212211609, -0.124844059, -0.0162531417, 0.0945967138, 0.192299068, 0.263274789, 0.297659308, 0.290673733, 0.243288934, 0.162090689, -0.240343079, -0.157879904, -0.053473819, 0.0583643168, 0.162090689, 0.243288934, 0.290673733, 0.297659308, 0.263274789, -0.296997756, -0.260852158, -0.18845211, -0.0898600593, 0.0212211609, 0.129352957, 0.219506666, 0.279152304, 0.300000012, -0.280937016, -0.299958736, -0.277290732, -0.216083542, -0.124844059, -0.0162531417, 0.0945967138, 0.192299068, 0.263274789, -0.196093097, -0.265624911, -0.298238903, -0.289402276, -0.240343079, -0.157879904, -0.053473819, 0.0583643168, 0.162090689, 0.193265304, 0.29098317, 0.278687924, 0.161028072, -0.0175122581, -0.189431667, -0.28973192, -0.280492187, -0.165205717, 0.244024664, 0.29999736, 0.242548913, 0.0933991, -0.0910624713, -0.241095677, -0.299976975, -0.245444879, -0.0981165096, 0.279611737, 0.290359169, 0.19132942, 0.0199630205, -0.158950865, -0.277769566, -0.291570932, -0.195137009, -0.0249268785, 0.297813892, 0.262667835, 0.128213957, -0.0547142625, -0.216956452, -0.297173083, -0.265036434, -0.13269645, 0.0498125739, 0.297499448, 0.218645111, 0.0571267791, -0.12598969, -0.261472702, -0.298099846, -0.222023234, -0.0620054789, 0.121454924, 0.278687924, 0.161028072, -0.0175122581, -0.189431667, -0.28973192, -0.280492187, -0.165205717, 0.0125406953, 0.185545802, 0.242548913, 0.0933991, -0.0910624713, -0.241095677, -0.299976975, -0.245444879, -0.0981165096, 0.0863071531, 0.238100335, 0.19132942, 0.0199630205, -0.158950865, -0.277769566, -0.291570932, -0.195137009, -0.0249268785, 0.154707447, 0.275850952, 0.128213957, -0.0547142625, -0.216956452, -0.297173083, -0.265036434, -0.13269645, 0.0498125739, 0.213488773, 0.296450466, 0.22945267, 0.142719671, 0.0361508206, -0.0754424408, -0.176550359, -0.253120452, -0.294510663, -0.294968396, -0.254430026, 0.294019997, 0.251757711, 0.174504936, 0.072998628, -0.0386533663, -0.144933119, -0.231069401, -0.285090536, -0.299488455, 0.286600977, 0.299156666, 0.270134151, 0.203567103, 0.108707316, -0.00126111251, -0.111054264, -0.205412567, -0.271221668, 0.209012017, 0.273311645, 0.299625099, 0.284295231, 0.22945267, 0.142719671, 0.0361508206, -0.0754424408, -0.176550359, 0.0802496672, 0.180550382, 0.255757362, 0.295417964, 0.294019997, 0.251757711, 0.174504936, 0.072998628, -0.0386533663, -0.0681606159, 0.0435841233, 0.149271324, 0.234212086, 0.286600977, 0.299156666, 0.270134151, 0.203567103, 0.108707316, -0.199882805, -0.104053065, 0.00623846287, 0.11566294, 0.209012017, 0.273311645, 0.299625099, 0.284295231, 0.22945267, -0.282666713, -0.226214439, -0.138321802, -0.0312045459, 0.0802496672, 0.180550382, 0.255757362, 0.295417964, 0.294019997, -0.296243966, -0.292990625, -0.249016061, -0.170432076, -0.0681606159, 0.0435841233, 0.149271324, 0.234212086, 0.286600977, 0.0886560678, 0.239586234, 0.299935132, 0.246886447, 0.100496463, -0.0838885903, -0.236557618, -0.299790353, -0.249680206, 0.156806186, 0.276806951, 0.292154312, 0.197045773, 0.027439408, -0.152541071, -0.274849802, -0.293244988, -0.200771913, 0.215206832, 0.296817154, 0.266208708, 0.134953752, -0.0473236963, -0.211709306, -0.296053171, -0.268467069, -0.139380619, 0.260226965, 0.298372746, 0.22371158, 0.0644709617, -0.119144447, -0.25771448, -0.298849404, -0.226997182, -0.069323279, 0.289067477, 0.281376928, 0.167305142, -0.010020325, -0.183557361, -0.287696213, -0.283064604, -0.17141369, 0.00504422747, 0.299935132, 0.246886447, 0.100496463, -0.0838885903, -0.236557618, -0.299790353, -0.249680206, -0.105172537, 0.0790981129, 0.292154312, 0.197045773, 0.027439408, -0.152541071, -0.274849802, -0.293244988, -0.200771913, -0.0323922634, 0.148234054, 0.266208708, 0.134953752, -0.0473236963, -0.211709306, -0.296053171, -0.268467069, -0.139380619, 0.0424020067, 0.208153531, 0.22371158, 0.0644709617, -0.119144447, -0.25771448, -0.298849404, -0.226997182, -0.069323279, 0.114559926, 0.255131036, 0.286600977, 0.234212086, 0.149271324, 0.0435841233, -0.0681606159, -0.170432076, -0.249016061, -0.292990625, -0.296243966, 0.294019997, 0.295417964, 0.255757362, 0.180550382, 0.0802496672, -0.0312045459, -0.138321802, -0.226214439, -0.282666713, 0.22945267, 0.284295231, 0.299625099, 0.273311645, 0.209012017, 0.11566294, 0.00623846287, -0.104053065, -0.199882805, 0.108707316, 0.203567103, 0.270134151, 0.299156666, 0.286600977, 0.234212086, 0.149271324, 0.0435841233, -0.0681606159, -0.0386533663, 0.072998628, 0.174504936, 0.251757711, 0.294019997, 0.295417964, 0.255757362, 0.180550382, 0.0802496672, -0.176550359, -0.0754424408, 0.0361508206, 0.142719671, 0.22945267, 0.284295231, 0.299625099, 0.273311645, 0.209012017, -0.271221668, -0.205412567, -0.111054264, -0.00126111251, 0.108707316, 0.203567103, 0.270134151, 0.299156666, 0.286600977, -0.299488455, -0.285090536, -0.231069401, -0.144933119, -0.0386533663, 0.072998628, 0.174504936, 0.251757711, 0.294019997, -0.254430026, -0.294968396, -0.294510663, -0.253120452, -0.176550359, -0.0754424408, 0.0361508206, 0.142719671, 0.22945267, 0.252441287, 0.299559414, 0.233421966, 0.0790338069, -0.105234973, -0.249717161, -0.299787849, -0.23651661, -0.0838246495, 0.284695387, 0.286225736, 0.179541647, 0.00497756759, -0.171468407, -0.283086658, -0.287677288, -0.183504671, -0.0099537652, 0.299248517, 0.255095959, 0.114498302, -0.0693881437, -0.227040753, -0.298855215, -0.257680357, -0.119083315, 0.0645359978, 0.295195788, 0.208105519, 0.042336002, -0.139439642, -0.268496811, -0.296042383, -0.211662099, -0.0472579338, 0.135013223, 0.27278924, 0.148176089, -0.0324585401, -0.200821444, -0.293259054, -0.27482307, -0.152483732, 0.0275057256, 0.19709599, 0.233421966, 0.0790338069, -0.105234973, -0.249717161, -0.299787849, -0.23651661, -0.0838246495, 0.100559212, 0.246924281, 0.179541647, 0.00497756759, -0.171468407, -0.283086658, -0.287677288, -0.183504671, -0.0099537652, 0.16736041, 0.281399995, 0.114498302, -0.0693881437, -0.227040753, -0.298855215, -0.257680357, -0.119083315, 0.0645359978, 0.223755941, 0.29837966, 0.042336002, -0.139439642, -0.268496811, -0.296042383, -0.211662099, -0.0472579338, 0.135013223, 0.266239434, 0.296807498, 0.162090689, 0.0583643168, -0.053473819, -0.157879904, -0.240343079, -0.289402276, -0.298238903, -0.265624911, -0.196093097, 0.263274789, 0.192299068, 0.0945967138, -0.0162531417, -0.124844059, -0.216083542, -0.277290732, -0.299958736, -0.280937016, 0.300000012, 0.279152304, 0.219506666, 0.129352957, 0.0212211609, -0.0898600593, -0.18845211, -0.260852158, -0.296997756, 0.263274789, 0.297659308, 0.290673733, 0.243288934, 0.162090689, 0.0583643168, -0.053473819, -0.157879904, -0.240343079, 0.162090689, 0.243288934, 0.290673733, 0.297659308, 0.263274789, 0.192299068, 0.0945967138, -0.0162531417, -0.124844059, 0.0212211609, 0.129352957, 0.219506666, 0.279152304, 0.300000012, 0.279152304, 0.219506666, 0.129352957, 0.0212211609, -0.124844059, -0.0162531417, 0.0945967138, 0.192299068, 0.263274789, 0.297659308, 0.290673733, 0.243288934, 0.162090689, -0.240343079, -0.157879904, -0.053473819, 0.0583643168, 0.162090689, 0.243288934, 0.290673733, 0.297659308, 0.263274789, -0.296997756, -0.260852158, -0.18845211, -0.0898600593, 0.0212211609, 0.129352957, 0.219506666, 0.279152304, 0.300000012, 0.16939275, 0.282241762, 0.28838259, 0.185493514, 0.0124742286, -0.165261239, -0.28051576, -0.289714664, -0.18938002, 0.225384131, 0.298624516, 0.25896284, 0.12139409, -0.0620705672, -0.222067982, -0.298107296, -0.261440098, -0.125929236, 0.267362237, 0.296440244, 0.213442028, 0.0497469716, -0.132756114, -0.265067607, -0.297163963, -0.216910452, -0.0546487831, 0.29271704, 0.275824785, 0.154650435, -0.0249931738, -0.195187524, -0.291586608, -0.277744442, -0.158894375, 0.0200294703, 0.2998721, 0.238059863, 0.0862434283, -0.0981793702, -0.24548313, -0.29997617, -0.24105607, -0.0909990147, 0.0934623778, 0.28838259, 0.185493514, 0.0124742286, -0.165261239, -0.28051576, -0.289714664, -0.18938002, -0.0174457729, 0.161084265, 0.25896284, 0.12139409, -0.0620705672, -0.222067982, -0.298107296, -0.261440098, -0.125929236, 0.057192158, 0.218690708, 0.213442028, 0.0497469716, -0.132756114, -0.265067607, -0.297163963, -0.216910452, -0.0546487831, 0.128274173, 0.262700021, 0.154650435, -0.0249931738, -0.195187524, -0.291586608, -0.277744442, -0.158894375, 0.0200294703, 0.191380695, 0.290375918, 0.24760069, 0.168350413, 0.0657019988, -0.046077989, -0.151453823, -0.235779896, -0.287336171, -0.298957109, -0.269027531, 0.298501253, 0.266787827, 0.197994962, 0.101683788, -0.00875986461, -0.117986031, -0.21081391, -0.274341911, -0.299740553, 0.276318312, 0.299906254, 0.281811833, 0.22454983, 0.136078835, 0.0286949575, -0.0826770812, -0.182558239, -0.257066637, 0.186482996, 0.259597212, 0.296631336, 0.292438239, 0.24760069, 0.168350413, 0.0657019988, -0.046077989, -0.151453823, 0.0509901494, 0.155729666, 0.238825157, 0.288727582, 0.298501253, 0.266787827, 0.197994962, 0.101683788, -0.00875986461, -0.0969868675, 0.0137341041, 0.122546248, 0.214326322, 0.276318312, 0.299906254, 0.281811833, 0.22454983, 0.136078835, -0.221218154, -0.131624088, -0.0237362608, 0.0874505118, 0.186482996, 0.259597212, 0.296631336, 0.292438239, 0.24760069, -0.291287452, -0.244756088, -0.164207324, -0.0608362257, 0.0509901494, 0.155729666, 0.238825157, 0.288727582, 0.298501253, -0.29003948, -0.297963262, -0.26447469, -0.194228172, -0.0969868675, 0.0137341041, 0.122546248, 0.214326322, 0.276318312, 0.289067477, 0.281376928, 0.167305142, -0.010020325, -0.183557361, -0.287696213, -0.283064604, -0.17141369, 0.00504422747, 0.299935132, 0.246886447, 0.100496463, -0.0838885903, -0.236557618, -0.299790353, -0.249680206, -0.105172537, 0.0790981129, 0.292154312, 0.197045773, 0.027439408, -0.152541071, -0.274849802, -0.293244988, -0.200771913, -0.0323922634, 0.148234054, 0.266208708, 0.134953752, -0.0473236963, -0.211709306, -0.296053171, -0.268467069, -0.139380619, 0.0424020067, 0.208153531, 0.22371158, 0.0644709617, -0.119144447, -0.25771448, -0.298849404, -0.226997182, -0.069323279, 0.114559926, 0.255131036, 0.167305142, -0.010020325, -0.183557361, -0.287696213, -0.283064604, -0.17141369, 0.00504422747, 0.179595053, 0.286245704, 0.100496463, -0.0838885903, -0.236557618, -0.299790353, -0.249680206, -0.105172537, 0.0790981129, 0.233463839, 0.29956302, 0.027439408, -0.152541071, -0.274849802, -0.293244988, -0.200771913, -0.0323922634, 0.148234054, 0.272816986, 0.294254959, -0.0473236963, -0.211709306, -0.296053171, -0.268467069, -0.139380619, 0.0424020067, 0.208153531, 0.295207679, 0.270651519, 0.0802496672, -0.0312045459, -0.138321802, -0.226214439, -0.282666713, -0.299832612, -0.275326371, -0.212553948, -0.120239705, 0.209012046, 0.11566294, 0.00623846287, -0.104053065, -0.199882805, -0.267931879, -0.298742503, -0.288032472, -0.237290338, 0.286600977, 0.234212115, 0.149271324, 0.0435841233, -0.0681606159, -0.170432076, -0.249016061, -0.292990625, -0.296243966, 0.294019997, 0.295417964, 0.255757362, 0.180550396, 0.0802496672, -0.0312045459, -0.138321802, -0.226214439, -0.282666713, 0.229452655, 0.284295231, 0.299625099, 0.273311645, 0.209012046, 0.11566294, 0.00623846287, -0.104053065, -0.199882805, 0.108707316, 0.203567088, 0.270134121, 0.299156666, 0.286600977, 0.234212115, 0.149271324, 0.0435841233, -0.0681606159, -0.0386533663, 0.072998628, 0.174504921, 0.251757681, 0.294019997, 0.295417964, 0.255757362, 0.180550396, 0.0802496672, -0.176550359, -0.0754424408, 0.0361508206, 0.142719671, 0.229452655, 0.284295231, 0.299625099, 0.273311645, 0.209012046, -0.271221668, -0.205412567, -0.111054264, -0.00126111251, 0.108707316, 0.203567088, 0.270134121, 0.299156666, 0.286600977], "patch_of_step": [0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0], "seeds": {"o": [-0.889639735, 0.662655652, -0.272526205, 0.958890021, -0.820357919, -0.206526786], "th": [-0.291723907, -0.0267240033, 0.981641889], "sp": [0.61656338, 0.528745234, 0.755806148], "x": [-0.970838487, -0.813273907, 0.653108478, 0.666985512, 0.784822047, 0.916027009, 0.122674242, -0.818902612, 0.991977811, -0.0451965556, 0.369831204, 0.686750531], "min_clear": [0.29891777, -0.778198481, 0.0473505743], "cum_risk": [0.63894558, -0.591690481, -0.015728008], "arc_length": [-0.514919162, -0.761809289, 0.463742197]}, - "out": {"o": [-0.2995193, 0.350879014, 0.410605401, -0.213698491, 0.296452999, 0.13719511], "th": [1.11090529, 1.36197555, 1.05473721], "sp": [0.923863947, 0.81962359, 0.819391787], "x": [0.240945876, 1.68225205, 0.507458448, 1.5486536, 0.265604079, 1.83672547, 0.599798739, 0.0372249484, 0.24611263, 1.7064333, 0.562793136, 2.04704523], "min_clear": [0.800081789, 0.643705428, 0.839208066], "cum_risk": [0.137823761, 0.178379834, 0.115861341], "hard_count": [0, 0, 0], "arc_length": [0.230409443, 0.298423886, 0.193637773]}, - "grad": {"alphas": [-12.0248299, -15.7960224, 0.23372899, -0.0957826301, -4.79887056, -2.15592718], "beta": [0, 0.0967349857, -1.7806325], "gamma": [0, -0.010979563, 0.284063578], "lam_soft": [0.0163165331, -0.00500075426, 0.103222154], "lam_hard": [0.0253563486, 0.00221359241, -0.0356018469], "fext": [0, 0, 0, 0, 2.84523273, -2.06630325, 0, 0, 0.0229568835, 0.0199241731, 0, 0, 0, 0, 0, 0, -0.329114884, -0.24894391, -0.998898745, 0.663600624, 1.69399476, -1.38874948, 0, 0]}} + "out": {"o": [-0.299535573, 0.350898534, 0.410558373, -0.213682398, 0.296428353, 0.137210146], "th": [1.1110456, 1.36226106, 1.05493629], "sp": [0.923988104, 0.819642961, 0.819449425], "x": [0.240992665, 1.68259752, 0.50738126, 1.54969406, 0.26573512, 1.83763278, 0.599798739, 0.0371892452, 0.246200457, 1.70704985, 0.562759876, 2.04762936], "min_clear": [0.800081789, 0.643705428, 0.839228213], "cum_risk": [0.137828439, 0.178378701, 0.115860209], "hard_count": [0, 0, 0], "arc_length": [0.230417252, 0.298421979, 0.193635851]}, + "grad": {"alphas": [-12.0301189, -15.8041697, 0.233429447, -0.0956598446, -4.79598475, -2.15026522], "beta": [0, 0.0966109931, -1.78128815], "gamma": [0, -0.0109654907, 0.284157515], "lam_soft": [0.0162640512, -0.00499434443, 0.10317982], "lam_hard": [0.0254018307, 0.00221075513, -0.0355935059], "fext": [0, 0, 0, 0, 2.84744382, -2.06744909, 0, 0, 0.0229274593, 0.0198986363, 0, 0, 0, 0, 0, 0, -0.329213798, -0.249018744, -0.999887526, 0.664377034, 1.69410789, -1.38845301, 0, 0]}} ] } diff --git a/src/cvc/tests/nav_dyn_bike_test.cpp b/src/cvc/tests/nav_dyn_bike_test.cpp index 7e6535087..dc3d3b24a 100644 --- a/src/cvc/tests/nav_dyn_bike_test.cpp +++ b/src/cvc/tests/nav_dyn_bike_test.cpp @@ -689,8 +689,9 @@ TEST(DynBike, Golden) { // H = 1/2 p^T M^-1 p, p' = (J - R) v + G u + f_eps - m g grade e_x, u = (F_xf, F_xr), with R the // tires' secants, drag and rolling resistance. These gates hold dyn_plant to that form at every // slip, sliding included, book its steps, and report where the tires' tangent stops being -// positive semi-definite past the Pacejka peak while the secant does not. dyn_plant's arithmetic -// is unchanged (DynBike.Golden still pins it). +// positive semi-definite past the Pacejka peak while the secant does not. Both slips are in their +// wheels' frames, rooted at zero sideways velocity there, so f_eps is round-off. DynBike.Golden +// pins dyn_plant's arithmetic. namespace { @@ -759,7 +760,7 @@ TEST(DynBikePH, TheStructureIsPortHamiltonianAtEverySlip) { EXPECT_NEAR(vJv, 0.0, 1e-9 * (1.0 + pm[0] * pm[0])); // The coefficients of R's three outer products are nonnegative: it is PSD. EXPECT_GT(q.Dx, 0.0); - EXPECT_GE(q.Sf / q.cd, 0.0) << "front secant at vx " << s.vx << ", vy " << s.vy; + EXPECT_GE(q.Sf, 0.0) << "front secant at vx " << s.vx << ", vy " << s.vy; EXPECT_GE(q.Sr, 0.0) << "rear secant at vx " << s.vx << ", vy " << s.vy; for (int t = 0; t < 4; ++t) { const double w[3] = {u(rng), u(rng), u(rng)}; @@ -894,10 +895,11 @@ TEST(DynBikePH, StepBooksCloseToSecondOrder) { } } -// f_eps is the speed floor's: it shrinks with the floor and is gone without it. -TEST(DynBikePH, TheFloorTermVanishesWithTheFloor) { +// f_eps is what the secants leave of the tires' forces. Each slip is in its wheel's frame with its +// secant through its root there, so that is round-off at any speed floor: the floor reaches only +// the slips' forward speeds, which the forces' signs do not read. +TEST(DynBikePH, TheFloorTermIsRoundOff) { dyn_bike_params p = humvee(); - double prev = 1e300; for (const double eps : {0.5, 0.05, 0.005, 1e-9}) { p.eps = static_cast(eps); std::mt19937 rng(7); @@ -909,10 +911,41 @@ TEST(DynBikePH, TheFloorTermVanishesWithTheFloor) { } std::printf(" speed floor %.0e m/s: worst |floor power| / (dissipated + |supplied|) = %.2e\n", eps, worst); - EXPECT_LT(worst, prev); - prev = worst; + EXPECT_LT(worst, 1e-12) << "speed floor " << eps; } - EXPECT_LT(prev, 1e-7); +} + +// dyn_ph_tangent_D is the tires' derivative: -d(Q_y, Q_r)/d(vy, r) by central differences of the +// lateral force and yaw moment (dyn_ph_parts_at's), at every slip, forward and reversing. +TEST(DynBikePH, TheTangentIsTheTiresDerivative) { + const dyn_bike_params p = humvee(); + const auto Q = [&](const dyn_state &s, double &Qy, double &Qr) { + diff::dyn_ph_parts q; + diff::dyn_ph_parts_at(p, s, q); + Qy = q.Fyf * q.cd + q.Fyr; + Qr = p.lf * q.Fyf * q.cd - p.lr * q.Fyr; + }; + std::mt19937 rng(11); + double worst = 0.0; + for (int k = 0; k < 300; ++k) { + const dyn_state s = sliding_state(rng); + double Dt[2][2], kf, kr, af, ar; + diff::dyn_ph_tangent_D(p, s, Dt, kf, kr, af, ar); + const double h = 1e-6, scale = std::fabs(Dt[0][0]) + std::fabs(Dt[1][1]) + 1.0; + for (int j = 0; j < 2; ++j) { + dyn_state sp = s, sm = s; + (j ? sp.r : sp.vy) += h; + (j ? sm.r : sm.vy) -= h; + double yp, rp, ym, rm; + Q(sp, yp, rp); + Q(sm, ym, rm); + const double fd[2] = {-(yp - ym) / (2.0 * h), -(rp - rm) / (2.0 * h)}; + for (int i = 0; i < 2; ++i) + worst = std::max(worst, std::fabs(fd[i] - Dt[i][j]) / scale); + } + } + std::printf(" tangent vs central differences: worst %.2e of its scale\n", worst); + EXPECT_LT(worst, 1e-6); } // The report: the tires' tangent (dyn_ph_tangent_D), the linearization's damping, along steer @@ -949,7 +982,7 @@ TEST(DynBikePH, TheSecantStaysPSDWhereTheTangentDoesNot) { lam_min[b] = std::min(lam_min[b], lmin / (p.m / 1.0)); diff::dyn_ph_parts q; diff::dyn_ph_parts_at(p, s, q); - secant_negative += q.Sf / q.cd < 0.0 || q.Sr < 0.0; + secant_negative += q.Sf < 0.0 || q.Sr < 0.0; }; for (const double v : {8.0, 12.0, 16.0, 20.0}) { // A steady ramp of the steer to the lock, the speed held by the throttle: past the limit. @@ -1034,8 +1067,8 @@ TEST(DynBikePH, Golden) { {"pulling away", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 0.1, 2.0, 0.0, false}, {"stopping at the clamp", {0.0, 0.0, 0.0, 0.3, 0.0, 0.05, 0.2, -6.0}, 0.2, -6.0, 0.0, false}, }; - const char *const part_keys[] = {"u", "sg", "Fxf", "Fxr", "Fyf", "Fyr", "wf", - "wr", "w0f", "Sf", "Sr", "Dx", "cd"}; + const char *const part_keys[] = {"u", "sg", "uf", "Fxf", "Fxr", "Fyf", "Fyr", + "wf", "wr", "Sf", "Sr", "Dx", "cd"}; const char *const rate_keys[] = {"dissipated", "supplied", "floor", "grade"}; const char *const free_keys[] = {"vx", "vy", "r"}; const char *const book_keys[] = {"dH", "dissipated", "supplied", "floor", @@ -1113,8 +1146,8 @@ TEST(DynBikePH, Golden) { s[k] = gc.s[k]; diff::dyn_ph_parts q; diff::dyn_ph_parts_at(p, s, q); - const double parts[] = {q.u, q.sg, q.Fxf, q.Fxr, q.Fyf, q.Fyr, q.wf, - q.wr, q.w0f, q.Sf, q.Sr, q.Dx, q.cd}; + const double parts[] = {q.u, q.sg, q.uf, q.Fxf, q.Fxr, q.Fyf, q.Fyr, + q.wf, q.wr, q.Sf, q.Sr, q.Dx, q.cd}; const diff::dyn_ph_rates rt = diff::dyn_ph_rates_at(p, s, gc.grade); const double rates[] = {rt.dissipated, rt.supplied, rt.floor, rt.grade}; dyn_state o, o_books;