diff --git a/grl_snam/dyn_bike.py b/grl_snam/dyn_bike.py index d03126c..2292b6f 100644 --- a/grl_snam/dyn_bike.py +++ b/grl_snam/dyn_bike.py @@ -21,14 +21,30 @@ 2. loads: quasi-static longitudinal load transfer, floored at ``fz_min * m * g``; 3. the speed, predicted from the drive or brake force (each axle saturated smoothly at ``mu Fz``), drag, rolling resistance and grade; -4. ``(vy, r)`` by implicit Euler, ``iters`` steps, each tire linearized by its secant through its - root in lateral velocity (Pacejka forces within the friction ellipse's remainder); +4. ``(vy, r)`` by implicit Euler, ``iters`` steps, each tire's slip in its wheel's frame (as + :mod:`grl_snam.dyn4`'s), linearized by its secant through its root, zero sideways velocity + there (Pacejka forces within the friction ellipse's remainder); 5. the speed corrected with the front lateral force the implicit step applied; 6. below ``v_blend``, ``(vy, r)`` blended toward the kinematic bicycle's; 7. the pose: the rear axle moves with ``(vx, vy - lr r)`` at the new heading. Every branch is a ``torch.where`` on values, as the C++ branches on ``s_val``; the division in the unselected branch is guarded so it never turns a gradient into NaN. + +The plant in port-Hamiltonian form (libcvc's ``dyn_ph_*``): with the twist ``v = (vx, vy, r)``, +the momenta ``p = M v``, ``M = diag(m, m, Iz)`` and ``H = 1/2 v^T M v``, the plant's vector field +is ``p' = (J(p) - R) v + G u + f_eps - m g grade e_x`` with the axle forces ``u = (F_xf, F_xr)`` +as the port and ``y = G^T v``. ``R`` is built from the tires' secants, so it is positive +semi-definite at every slip; ``f_eps``, what the secants leave of the tires' forces, is +round-off, since each slip's root is zero sideways velocity in its wheel's frame. So + + dH/dt = supplied - dissipated + floor + grade, + +each term returned by :func:`dyn_ph_rates_at`, and :func:`dyn_plant_books` books one step: +the trapezoid of those powers between the step's start (with the actuators' outputs) and the +twist it reached before its limiters, the kinetic energy the limiters then moved, and the +integrator's residual. The parity gate is ``tests/test_dyn_bike_ph_parity.py``, against +libcvc's ``dyn_bike_ph_golden.json``. """ from __future__ import annotations @@ -105,6 +121,27 @@ def _clamp(x, lo: float, hi: float): ) +def _axles(p: DynBikeParams, a: torch.Tensor): + """Each axle's drive or brake force within its grip, and the lateral grip it leaves, at the + actuator's ``a`` (``dyn_ph_axles``, the plant's step 2): Fxf, Fxr, Dyf, Dyr.""" + m, g, h, L = p.m, p.g, p.h, p.lf + p.lr + fz_floor = p.fz_min * m * g + Fzf = m * (g * p.lr - a * h) / L + Fzr = m * (g * p.lf + a * h) / L + Fzf = torch.where(Fzf < fz_floor, torch.full_like(Fzf, fz_floor), Fzf) + Fzr = torch.where(Fzr < fz_floor, torch.full_like(Fzr, fz_floor), Fzr) + cap_f, cap_r = p.mu * Fzf, p.mu * Fzr + F_drive = m * a + share = torch.where( + a >= 0.0, torch.full_like(a, p.front_drive), torch.full_like(a, p.front_brake) + ) + Fxf = _sat(F_drive * share, cap_f) + Fxr = _sat(F_drive * (1.0 - share), cap_r) + Dyf = torch.sqrt(cap_f * cap_f - 0.9975 * Fxf * Fxf) + Dyr = torch.sqrt(cap_r * cap_r - 0.9975 * Fxr * Fxr) + return Fxf, Fxr, Dyf, Dyr + + def dyn_plant( p: DynBikeParams, lim: PlantLimits, @@ -112,12 +149,18 @@ def dyn_plant( delta_cmd: torch.Tensor, a_cmd: torch.Tensor, grade: torch.Tensor | float = 0.0, -) -> torch.Tensor: - """One step of the plant: ``state`` ``(..., 8)`` to the next ``(..., 8)``.""" + *, + with_free: bool = False, +) -> torch.Tensor | tuple[torch.Tensor, torch.Tensor]: + """One step of the plant: ``state`` ``(..., 8)`` to the next ``(..., 8)``. + + With ``with_free``, also the twist ``(vx, vy, r)`` ``(..., 3)`` the step reached before its + limiters (the speed clamp, the low-speed blend): libcvc's ``dyn_step_free``. + """ x, y, psi, vx, vy, r, delta, a = state.unbind(-1) grade = torch.as_tensor(grade, dtype=state.dtype, device=state.device) hdt = lim.hdt - m, Iz, lf, lr, h, g, mu, Cp = p.m, p.Iz, p.lf, p.lr, p.h, p.g, p.mu, p.C + m, Iz, lf, lr, g, Cp = p.m, p.Iz, p.lf, p.lr, p.g, p.C L = lf + lr # 1. Actuators. @@ -131,20 +174,7 @@ def dyn_plant( a1 = a + (a_cmd - a) * k_drive # 2. Loads. - fz_floor = p.fz_min * m * g - Fzf = m * (g * lr - a1 * h) / L - Fzr = m * (g * lf + a1 * h) / L - Fzf = torch.where(Fzf < fz_floor, torch.full_like(Fzf, fz_floor), Fzf) - Fzr = torch.where(Fzr < fz_floor, torch.full_like(Fzr, fz_floor), Fzr) - cap_f, cap_r = mu * Fzf, mu * Fzr - F_drive = m * a1 - share = torch.where( - a1 >= 0.0, torch.full_like(a1, p.front_drive), torch.full_like(a1, p.front_brake) - ) - Fxf = _sat(F_drive * share, cap_f) - Fxr = _sat(F_drive * (1.0 - share), cap_r) - Dyf = torch.sqrt(cap_f * cap_f - 0.9975 * Fxf * Fxf) - Dyr = torch.sqrt(cap_r * cap_r - 0.9975 * Fxr * Fxr) + Fxf, Fxr, Dyf, Dyr = _axles(p, a1) cd, sd = torch.cos(delta1), torch.sin(delta1) eps2 = p.eps * p.eps sp_min = -0.25 * lim.vmax if lim.allow_reverse else 0.0 @@ -155,16 +185,16 @@ def dyn_plant( F_res = -p.drag * vx * u0 - p.c_rr * m * g * sg0 - m * g * grade vx_p = _clamp(vx + hdt * ((Fxf * cd + Fxr + F_res) / m), sp_min, lim.vmax) - # 4. Lateral and yaw. + # 4. Lateral and yaw, each slip in its wheel's frame. u = torch.sqrt(vx_p * vx_p + eps2) - sg = vx_p / u - w0f = u * torch.tan(sg * delta1) vy1, r1 = vy, r Fyf_step = torch.zeros_like(vy) for _ in range(max(1, p.iters)): - wf = vy1 + lf * r1 + vf = vy1 + lf * r1 + wf = -vx_p * sd + vf * cd wr = vy1 - lr * r1 - af = sg * delta1 - torch.atan2(wf, u) + uf = torch.sqrt((vx_p * cd + vf * sd) * (vx_p * cd + vf * sd) + eps2) + af = -torch.atan2(wf, uf) ar = -torch.atan2(wr, u) Fyf = _lateral(af, p.B_f, Cp, Dyf) Fyr = _lateral(ar, p.B_r, Cp, Dyr) @@ -172,13 +202,15 @@ def dyn_plant( MZ = lf * (Fxf * sd + Fyf * cd) - lr * Fyr f_vy = FY / m - r1 * vx_p f_r = MZ / Iz - Sf_t = _secant_w(Fyf, af, wf, w0f, p.B_f, Cp, Dyf, u) + Sf_t = _secant_w(Fyf, af, wf, torch.zeros_like(wf), p.B_f, Cp, Dyf, uf) Sr = _secant_w(Fyr, ar, wr, torch.zeros_like(wr), p.B_r, Cp, Dyr, u) + # dwf/d(vy, r) = cos(delta) (1, lf): Sf on the tire, Sfy on the body's y. Sf = Sf_t * cd - J11 = (-Sf - Sr) / m - J12 = (-Sf * lf + Sr * lr) / m - vx_p - J21 = (-Sf * lf + Sr * lr) / Iz - J22 = (-Sf * lf * lf - Sr * lr * lr) / Iz + Sfy = Sf * cd + J11 = (-Sfy - Sr) / m + J12 = (-Sfy * lf + Sr * lr) / m - vx_p + J21 = (-Sfy * lf + Sr * lr) / Iz + J22 = (-Sfy * lf * lf - Sr * lr * lr) / Iz A11, A12, A21, A22 = 1.0 - hdt * J11, -hdt * J12, -hdt * J21, 1.0 - hdt * J22 det = A11 * A22 - A12 * A21 det = torch.where(det < 0.05, torch.full_like(det, 0.05), det) @@ -188,11 +220,13 @@ def dyn_plant( dr = (A11 * b2 - A21 * b1) / det vy1 = vy1 + dvy r1 = r1 + dr - Fyf_step = Fyf - Sf_t * (dvy + lf * dr) + Fyf_step = Fyf - Sf * (dvy + lf * dr) # 5. The speed, corrected. FX = Fxf * cd - Fyf_step * sd + Fxr + F_res - vx1 = _clamp(vx + hdt * (FX / m + r1 * vy1), sp_min, lim.vmax) + vx1_free = vx + hdt * (FX / m + r1 * vy1) + vx1 = _clamp(vx1_free, sp_min, lim.vmax) + free = torch.stack((vx1_free, vy1, r1), dim=-1) if with_free else None # 6. Low speed. if p.v_blend > 0.0: @@ -208,4 +242,183 @@ def dyn_plant( c1, s1 = torch.cos(psi1), torch.sin(psi1) x1 = x + hdt * (vx1 * c1 - v_side * s1) y1 = y + hdt * (vx1 * s1 + v_side * c1) - return torch.stack((x1, y1, psi1, vx1, vy1, r1, delta1, a1), dim=-1) + nxt = torch.stack((x1, y1, psi1, vx1, vy1, r1, delta1, a1), dim=-1) + return (nxt, free) if with_free else nxt + + +# ── The plant in port-Hamiltonian form ────────────────────────────────────────────────────── +# Each function mirrors its libcvc namesake's arithmetic, sums in the same order, so the double +# tier holds to rounding. They are evaluations: dyn_plant does not depend on them. + + +@dataclass(frozen=True) +class DynPHParts: + """``dyn_ph_parts``: everything the form is built from, at a state (each ``(...)``, the + vectors ``(..., 3)`` on ``(vx, vy, r)``).""" + + u: torch.Tensor # the floored speed sqrt(vx^2 + eps^2) (the rear slip's) + sg: torch.Tensor # vx / u + uf: torch.Tensor # the front wheel's floored forward speed, in its frame + Fxf: torch.Tensor # the axles' drive or brake forces: the inputs + Fxr: torch.Tensor + Fyf: torch.Tensor # the tires' lateral forces + Fyr: torch.Tensor + wf: torch.Tensor # the axles' sideways velocities in their wheels' frames: nf.v, nr.v + wr: torch.Tensor + Sf: torch.Tensor # the secant stiffnesses through wf, wr = 0 + Sr: torch.Tensor + Dx: torch.Tensor # drag and rolling resistance on vx + cd: torch.Tensor # cos delta + nf: torch.Tensor # each tire's lateral force on (vx, vy, r) + nr: torch.Tensor + tf: torch.Tensor # each axle's drive force on (vx, vy, r) + tr: torch.Tensor + + +@dataclass(frozen=True) +class DynPHRates: + """``dyn_ph_rates``, the power balance at a state: dH/dt = supplied - dissipated + floor + + grade.""" + + dissipated: torch.Tensor # v^T R v >= 0: the tires' slip, drag and rolling resistance + supplied: torch.Tensor # y^T u: the axles' forces at their wheels' ground speed + floor: torch.Tensor # v^T f_eps: what the secants leave, round-off + grade: torch.Tensor # -m g grade vx: gravity along the heading + + +@dataclass(frozen=True) +class DynPHBooks: + """``dyn_ph_books``, one step's books in J: dH = supplied - dissipated + floor + grade + + limiter + residual.""" + + dH: torch.Tensor + dissipated: torch.Tensor + supplied: torch.Tensor + floor: torch.Tensor + grade: torch.Tensor + limiter: torch.Tensor # the kinetic energy the speed clamp and the low-speed blend added + residual: torch.Tensor # the integrator's error, second order in hdt per step + + +def dyn_ph_hamiltonian(p: DynBikeParams, state: torch.Tensor) -> torch.Tensor: + """The kinetic energy 1/2 v^T M v.""" + vx, vy, r = state[..., 3], state[..., 4], state[..., 5] + return 0.5 * (p.m * (vx * vx + vy * vy) + p.Iz * r * r) + + +def dyn_ph_parts_at(p: DynBikeParams, state: torch.Tensor) -> DynPHParts: + """The parts at a state ``(..., 8)``, at its own ``delta`` and ``a`` (the actuators' + outputs).""" + _, _, _, vx, vy, r, delta, a = state.unbind(-1) + lf, lr = p.lf, p.lr + Fxf, Fxr, Dyf, Dyr = _axles(p, a) + u = torch.sqrt(vx * vx + p.eps * p.eps) + sg = vx / u + cd, sd = torch.cos(delta), torch.sin(delta) + vf = vy + lf * r + wf = -vx * sd + vf * cd + wr = vy - lr * r + uf = torch.sqrt((vx * cd + vf * sd) * (vx * cd + vf * sd) + p.eps * p.eps) + af = -torch.atan2(wf, uf) + ar = -torch.atan2(wr, u) + Fyf = _lateral(af, p.B_f, p.C, Dyf) + Fyr = _lateral(ar, p.B_r, p.C, Dyr) + zero, one = torch.zeros_like(cd), torch.ones_like(cd) + return DynPHParts( + u=u, + sg=sg, + uf=uf, + Fxf=Fxf, + Fxr=Fxr, + Fyf=Fyf, + Fyr=Fyr, + wf=wf, + wr=wr, + Sf=_secant_w(Fyf, af, wf, zero, p.B_f, p.C, Dyf, uf), + Sr=_secant_w(Fyr, ar, wr, zero, p.B_r, p.C, Dyr, u), + Dx=p.drag * u + p.c_rr * p.m * p.g / u, + cd=cd, + nf=torch.stack((-sd, cd, lf * cd), dim=-1), + nr=torch.stack((zero, one, torch.full_like(cd, -lr)), dim=-1), + tf=torch.stack((cd, sd, lf * sd), dim=-1), + tr=torch.stack((one, zero, zero), dim=-1), + ) + + +def dyn_ph_rates_at( + p: DynBikeParams, state: torch.Tensor, grade: torch.Tensor | float = 0.0 +) -> DynPHRates: + """The power balance at a state ``(..., 8)``: R, G, u and f_eps evaluated there.""" + q = dyn_ph_parts_at(p, state) + v = (state[..., 3], state[..., 4], state[..., 5]) + nf = q.nf.unbind(-1) + nr = q.nr.unbind(-1) + tf = q.tf.unbind(-1) + tr = q.tr.unbind(-1) + # R = Dx e_x e_x^T + Sf nf nf^T + Sr nr nr^T. + R = [[None] * 3 for _ in range(3)] + for i in range(3): + for j in range(i, 3): + R[i][j] = q.Sf * (nf[i] * nf[j]) + q.Sr * (nr[i] * nr[j]) + R[j][i] = R[i][j] + R[0][0] = R[0][0] + q.Dx + # f_eps: what the secants leave of the tires' forces (round-off). + nfv = torch.zeros_like(q.u) + nrv = torch.zeros_like(q.u) + for i in range(3): + nfv = nfv + nf[i] * v[i] + nrv = nrv + nr[i] * v[i] + ef = q.Fyf + q.Sf * nfv + er = q.Fyr + q.Sr * nrv + f = [nf[i] * ef + nr[i] * er for i in range(3)] + dissipated = torch.zeros_like(q.u) + supplied = torch.zeros_like(q.u) + floor = torch.zeros_like(q.u) + for i in range(3): + for j in range(3): + dissipated = dissipated + v[i] * R[i][j] * v[j] + supplied = supplied + v[i] * (tf[i] * q.Fxf + tr[i] * q.Fxr) + floor = floor + v[i] * f[i] + return DynPHRates( + dissipated=dissipated, supplied=supplied, floor=floor, grade=-p.m * p.g * grade * v[0] + ) + + +def dyn_plant_books( + p: DynBikeParams, + lim: PlantLimits, + state: torch.Tensor, + delta_cmd: torch.Tensor, + a_cmd: torch.Tensor, + grade: torch.Tensor | float = 0.0, +) -> tuple[torch.Tensor, DynPHBooks]: + """:func:`dyn_plant`'s step, unchanged, and its books. + + The powers are the trapezoid between the step's start, with the inputs it applied (the + actuators' outputs ``delta1``, ``a1``), and the twist it reached before its limiters; + ``limiter`` is dH across the clamp and the blend; ``residual`` is the rest. + """ + nxt, free = dyn_plant(p, lim, state, delta_cmd, a_cmd, grade, with_free=True) + s0 = torch.cat((state[..., :6], nxt[..., 6:]), dim=-1) + e = torch.cat((nxt[..., :3], free, nxt[..., 6:]), dim=-1) + r0, r1 = dyn_ph_rates_at(p, s0, grade), dyn_ph_rates_at(p, e, grade) + half = 0.5 * lim.hdt + dissipated = half * (r0.dissipated + r1.dissipated) + supplied = half * (r0.supplied + r1.supplied) + floor = half * (r0.floor + r1.floor) + grade_w = half * (r0.grade + r1.grade) + h0 = dyn_ph_hamiltonian(p, state) + he = dyn_ph_hamiltonian(p, e) + h1 = dyn_ph_hamiltonian(p, nxt) + dH = h1 - h0 + limiter = h1 - he + residual = dH - (supplied - dissipated + floor + grade_w + limiter) + return nxt, DynPHBooks( + dH=dH, + dissipated=dissipated, + supplied=supplied, + floor=floor, + grade=grade_w, + limiter=limiter, + residual=residual, + ) diff --git a/tests/fixtures/dyn_bike_golden.json b/tests/fixtures/dyn_bike_golden.json index 5801024..b1d62bd 100644 --- a/tests/fixtures/dyn_bike_golden.json +++ b/tests/fixtures/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/tests/fixtures/dyn_bike_ph_golden.json b/tests/fixtures/dyn_bike_ph_golden.json new file mode 100644 index 0000000..e59bd61 --- /dev/null +++ b/tests/fixtures/dyn_bike_ph_golden.json @@ -0,0 +1,80 @@ +{ + "format": "cvc-dyn-bike-ph-golden-1", + "order": ["x", "y", "psi", "vx", "vy", "r", "delta", "a"], + "params": {"m": 2676, "Iz": 6283.69384765625, "lf": 1.5369999408721924, "lr": 1.7630000114440918, "h": 0.74000000953674316, "g": 9.8066501617431641, "mu": 0.89999997615814209, "B_f": 10.708646774291992, "B_r": 10.708646774291992, "C": 1.2021652460098267, "front_drive": 0.5, "front_brake": 0.70588237047195435, "drag": 1.5006250143051147, "c_rr": 0.014999999664723873, "tau_steer": 0.05000000074505806, "steer_rate": 0.34999999403953552, "tau_drive": 0.10000000149011612, "v_blend": 1, "v_width": 0.30000001192092896, "eps": 0.5, "fz_min": 0.05000000074505806, "iters": 3}, + "g_books": [0.90000000000000002, -0.40000000000000002, 0.69999999999999996, 1.3, -0.59999999999999998, 0.80000000000000004, 1.1000000000000001], + "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, "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, "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, "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, "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, "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, "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, "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.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, "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/tests/fixtures/material_ext_golden.json b/tests/fixtures/material_ext_golden.json index eb64a82..94d0d01 100644 --- a/tests/fixtures/material_ext_golden.json +++ b/tests/fixtures/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/tests/test_dyn_bike_ph_parity.py b/tests/test_dyn_bike_ph_parity.py new file mode 100644 index 0000000..11175d3 --- /dev/null +++ b/tests/test_dyn_bike_ph_parity.py @@ -0,0 +1,155 @@ +"""Parity for the dynamic single-track vehicle in port-Hamiltonian form, and its step books. + +libcvc's ``dyn_ph_parts_at``, ``dyn_ph_rates_at`` and ``dyn_plant_books``, differentiated with +forward-mode duals, against the torch twins in :mod:`grl_snam.dyn_bike`, differentiated by +autograd. The reference is libcvc's ``src/cvc/tests/data/dyn_bike_ph_golden.json`` (copied to +``tests/fixtures``; written by libcvc's ``DynBikePH.Golden`` with +``CVC_DYN_BIKE_PH_GOLDEN_WRITE=1``): the Humvee of ``dyn_bike_golden.json`` at its seven states +(cruising, a hard corner, trail braking, past the tire's peak, backing up, creeping, pulling +away from rest) and braking to a stop at the speed clamp. Each case records the parts and the +rates at its state; one step's next state, the twist it reached before its limiters, and its +books; and the gradient of a fixed weighting of the books on the state and the commands. + +Tier: DOUBLE contract, as ``test_dyn_bike_parity.py``: both sides run in double from the same +float parameters, so every value agrees to 1e-9 relative. +""" + +from __future__ import annotations + +import json +import math +from pathlib import Path + +import pytest + +torch = pytest.importorskip("torch") + +from grl_snam.dyn_bike import ( # noqa: E402 + ORDER, + DynBikeParams, + PlantLimits, + dyn_ph_hamiltonian, + dyn_ph_parts_at, + dyn_ph_rates_at, + dyn_plant, + dyn_plant_books, +) + +GOLDEN = Path(__file__).parent / "fixtures" / "dyn_bike_ph_golden.json" +RTOL = 1e-9 +BOOKS = ("dH", "dissipated", "supplied", "floor", "grade", "limiter", "residual") + + +def _golden(): + data = json.loads(GOLDEN.read_text(encoding="utf-8")) + assert data["format"] == "cvc-dyn-bike-ph-golden-1" + assert tuple(data["order"]) == ORDER + return data + + +def _close(a: float, b: float) -> bool: + return abs(a - b) <= RTOL * max(1.0, abs(a), abs(b)) + + +def _limits(c) -> PlantLimits: + lim = c["limits"] + return PlantLimits( + L=lim["L"], vmax=lim["vmax"], hdt=lim["hdt"], allow_reverse=bool(lim["allow_reverse"]) + ) + + +CASES = range(len(_golden()["cases"])) + + +@pytest.mark.parametrize("case", CASES) +def test_parts_and_rates_match_libcvc(case): + data = _golden() + p = DynBikeParams(**data["params"]) + c = data["cases"][case] + state = torch.tensor(c["state"], dtype=torch.float64) + q = dyn_ph_parts_at(p, state) + for name, want in c["parts"].items(): + got = getattr(q, name).item() + assert _close(got, want), f"{c['name']} parts.{name}: torch {got!r}, libcvc {want!r}" + rates = dyn_ph_rates_at(p, state, c["grade"]) + for name, want in c["rates"].items(): + got = float(getattr(rates, name)) + assert _close(got, want), f"{c['name']} rates.{name}: torch {got!r}, libcvc {want!r}" + + +@pytest.mark.parametrize("case", CASES) +def test_books_and_their_gradient_match_libcvc(case): + data = _golden() + p = DynBikeParams(**data["params"]) + c = data["cases"][case] + limits = _limits(c) + state = torch.tensor(c["state"], dtype=torch.float64, requires_grad=True) + dcmd = torch.tensor(c["delta_cmd"], dtype=torch.float64, requires_grad=True) + acmd = torch.tensor(c["a_cmd"], dtype=torch.float64, requires_grad=True) + nxt, books = dyn_plant_books(p, limits, state, dcmd, acmd, c["grade"]) + for k, name in enumerate(ORDER): + assert _close( + nxt[k].item(), c["next"][k] + ), f"{c['name']} next {name}: torch {nxt[k].item()!r}, libcvc {c['next'][k]!r}" + _, free = dyn_plant(p, limits, state, dcmd, acmd, c["grade"], with_free=True) + for k, name in enumerate(("vx", "vy", "r")): + got, want = free[k].item(), c["free"][name] + assert _close(got, want), f"{c['name']} free.{name}: torch {got!r}, libcvc {want!r}" + for name in BOOKS: + got, want = getattr(books, name).item(), c["books"][name] + assert _close(got, want), f"{c['name']} books.{name}: torch {got!r}, libcvc {want!r}" + weighted = sum(w * getattr(books, name) for w, name in zip(data["g_books"], BOOKS)) + weighted.backward() + for k, name in enumerate(ORDER): + got, want = state.grad[k].item(), c["g_state"][k] + assert _close(got, want), f"{c['name']} d/d{name}: torch {got!r}, libcvc {want!r}" + assert _close(dcmd.grad.item(), c["g_delta_cmd"]), (dcmd.grad.item(), c["g_delta_cmd"]) + assert _close(acmd.grad.item(), c["g_a_cmd"]), (acmd.grad.item(), c["g_a_cmd"]) + + +def test_a_batch_books_like_its_rows(): + data = _golden() + p = DynBikeParams(**data["params"]) + rows = [c for c in data["cases"] if not c["limits"]["allow_reverse"]] + limits = _limits(rows[0]) + state = torch.tensor([c["state"] for c in rows], dtype=torch.float64) + dcmd = torch.tensor([c["delta_cmd"] for c in rows], dtype=torch.float64) + acmd = torch.tensor([c["a_cmd"] for c in rows], dtype=torch.float64) + grade = torch.tensor([c["grade"] for c in rows], dtype=torch.float64) + _, books = dyn_plant_books(p, limits, state, dcmd, acmd, grade) + rates = dyn_ph_rates_at(p, state, grade) + for i, c in enumerate(rows): + for name in BOOKS: + assert _close(getattr(books, name)[i].item(), c["books"][name]), (c["name"], name) + for name, want in c["rates"].items(): + assert _close(getattr(rates, name)[i].item(), want), (c["name"], name) + + +def test_books_close_to_second_order(): + # The behaviour gate needs only torch, as libcvc's DynBikePH.StepBooksCloseToSecondOrder: on + # a swerve with the throttle working, every step's books close but for the integrator's + # error, which falls by about 4 per halving of the step, and the run's books close to a + # small fraction of the energy that flowed. + data = _golden() + p = DynBikeParams(**data["params"]) + seconds = 2.0 + means = [] + for hdt in (0.01, 0.005): + limits = PlantLimits(L=p.lf + p.lr, vmax=40.0, hdt=hdt) + s = torch.tensor([0.0, 0.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0], dtype=torch.float64) + h0 = dyn_ph_hamiltonian(p, s).item() + n = round(seconds / hdt) + abs_residual = residual = flow = 0.0 + for t in range(n): + time = t * hdt + dcmd = torch.tensor(0.3 * math.sin(2.0 * math.pi * 0.7 * time), dtype=torch.float64) + acmd = torch.tensor(1.5 * math.sin(2.0 * math.pi * time / 3.0), dtype=torch.float64) + s, b = dyn_plant_books(p, limits, s, dcmd, acmd) + abs_residual += abs(b.residual.item()) + residual += b.residual.item() + flow += sum(abs(getattr(b, name).item()) for name in BOOKS[1:6]) + dH = dyn_ph_hamiltonian(p, s).item() - h0 + assert abs(residual) / flow < 5e-3, (hdt, residual, flow, dH) + means.append(abs_residual / n) + ratio = means[0] / means[1] + assert 3.0 < ratio < 5.5, means