From 472c99cae14cb2d469490f3020c3137b49697096 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 27 Oct 2026 19:32:40 -0500 Subject: [PATCH 1/2] Torch twin of libcvc's single-track port-Hamiltonian form and its step books grl_snam/dyn_bike.py gains the twins of libcvc's dyn_ph_* (cvc/nav/detail/ dyn_bike.h), beside dyn_plant: - dyn_ph_parts_at: everything the form is built from at a state (the axle forces, the tires' lateral forces, the secant stiffnesses, u, sg, Dx, and the vectors n_f, n_r, t_f, t_r); - dyn_ph_rates_at: the power balance dH/dt = supplied - dissipated + floor + grade, from R (the tires' secants, drag, rolling resistance), G, u and f_eps; - dyn_ph_hamiltonian: 1/2 v^T M v; - dyn_plant_books: dyn_plant's step, unchanged, and its books: the trapezoid of the rates between the step's start (with the actuators' outputs) and the twist it reached before its limiters; limiter, dH across the speed clamp and the low-speed blend; residual, the rest. dyn_plant takes with_free=True to also return that pre-limiter twist (libcvc's dyn_step_free); its step 2 moves into _axles, shared with the parts, with its arithmetic unchanged. tests/test_dyn_bike_ph_parity.py holds them to libcvc's dyn_bike_ph_golden.json (copied to tests/fixtures), double tier, 1e-9 relative: the parts, the rates, one step's next state, pre-limiter twist and books, and the autograd gradient of a weighting of the books against libcvc's forward-mode duals, on eight Humvee cases. A batch must match its rows, and a torch-only gate checks that the books close to second order on a swerve. Measured (torch 2.8 CPU): worst relative difference from libcvc 3.4e-16 on the parts, 5.3e-16 on the rates, 3.5e-14 on the books, 8.5e-14 on the gradients. On the swerve, the mean |residual| per step falls 3.94x and 3.97x per halving of the step (0.01 -> 0.005 -> 0.0025 s). --- grl_snam/dyn_bike.py | 247 +++++++++++++++++++++++-- tests/fixtures/dyn_bike_ph_golden.json | 80 ++++++++ tests/test_dyn_bike_ph_parity.py | 155 ++++++++++++++++ 3 files changed, 463 insertions(+), 19 deletions(-) create mode 100644 tests/fixtures/dyn_bike_ph_golden.json create mode 100644 tests/test_dyn_bike_ph_parity.py diff --git a/grl_snam/dyn_bike.py b/grl_snam/dyn_bike.py index d03126c..9b531d5 100644 --- a/grl_snam/dyn_bike.py +++ b/grl_snam/dyn_bike.py @@ -29,6 +29,20 @@ 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`` is what the slips' speed floor leaves. 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 +119,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 +147,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 +172,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 @@ -192,7 +220,9 @@ def dyn_plant( # 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 +238,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 slips' floored speed sqrt(vx^2 + eps^2) + sg: torch.Tensor # vx / u + 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' lateral velocities + wr: torch.Tensor + w0f: torch.Tensor # the front force's root + Sf: torch.Tensor # the secant stiffnesses through the roots + 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: the slips' speed floor + 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) + wf = vy + lf * r + wr = vy - lr * r + w0f = u * torch.tan(sg * delta) + af = sg * delta - torch.atan2(wf, u) + 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, + Fxf=Fxf, + Fxr=Fxr, + Fyf=Fyf, + Fyr=Fyr, + wf=wf, + wr=wr, + w0f=w0f, + Sf=_secant_w(Fyf, af, wf, w0f, p.B_f, p.C, Dyf, u), + 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 / cos delta) nf nf^T + Sr nr nr^T. + kf = q.Sf / q.cd + R = [[None] * 3 for _ in range(3)] + for i in range(3): + for j in range(i, 3): + R[i][j] = kf * (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. + 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 / q.cd * 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_ph_golden.json b/tests/fixtures/dyn_bike_ph_golden.json new file mode 100644 index 0000000..fcd01fb --- /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, "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}, + {"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}, + {"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}, + {"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}, + {"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}, + {"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}, + {"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}, + "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}, + {"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} + ] +} 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 From 73cde65a415ccb0afe66b98485573917f89454a4 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 27 Oct 2026 20:15:49 -0500 Subject: [PATCH 2/2] dyn_bike: the front slip in its wheel's frame, as libcvc's (f_eps = 0) Follows transfix/libcvc#852: dyn_plant's front slip becomes the wheel-frame slip dyn4's tires use, alpha_f = -atan2(w_f, u_f) with w_f = cos(delta) (vy + lf r) - sin(delta) vx (n_f.v) and u_f the floored forward speed in the wheel's frame, its secant rooted at w_f = 0. The implicit step's Jacobian takes the front's -S_f cos^2(delta) (1, lf) on the body's y. In the pH twin, R's front term is S_f n_f n_f^T, f_eps is round-off, and DynPHParts gains uf and loses w0f. The three fixtures are regenerated by libcvc#852: dyn_bike_golden.json, dyn_bike_ph_golden.json, and material_ext_golden.json (its dynamic case; the surrogate and bicycle cases are byte-identical). Measured (torch 2.8 CPU), worst relative difference from libcvc: plant 0 (next state) and 2.8e-16 (gradient); pH parts 3.4e-16, rates 3.4e-16, books 7.1e-14, books gradient 4.4e-14. The floor power is at most 6e-13 W on the golden's cases. The swerve's residual still falls 3.94x / 3.97x per halving of the step. --- grl_snam/dyn_bike.py | 62 ++++++------- tests/fixtures/dyn_bike_golden.json | 42 ++++----- tests/fixtures/dyn_bike_ph_golden.json | 110 ++++++++++++------------ tests/fixtures/material_ext_golden.json | 4 +- 4 files changed, 111 insertions(+), 107 deletions(-) diff --git a/grl_snam/dyn_bike.py b/grl_snam/dyn_bike.py index 9b531d5..2292b6f 100644 --- a/grl_snam/dyn_bike.py +++ b/grl_snam/dyn_bike.py @@ -21,8 +21,9 @@ 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. @@ -34,7 +35,8 @@ 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`` is what the slips' speed floor leaves. So +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, @@ -183,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) @@ -200,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) @@ -216,7 +220,7 @@ 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 @@ -252,16 +256,16 @@ 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 slips' floored speed sqrt(vx^2 + eps^2) + 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' lateral velocities + wf: torch.Tensor # the axles' sideways velocities in their wheels' frames: nf.v, nr.v wr: torch.Tensor - w0f: torch.Tensor # the front force's root - Sf: torch.Tensor # the secant stiffnesses through the roots + 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 @@ -278,7 +282,7 @@ class DynPHRates: 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: the slips' speed floor + floor: torch.Tensor # v^T f_eps: what the secants leave, round-off grade: torch.Tensor # -m g grade vx: gravity along the heading @@ -311,10 +315,11 @@ def dyn_ph_parts_at(p: DynBikeParams, state: torch.Tensor) -> DynPHParts: u = torch.sqrt(vx * vx + p.eps * p.eps) sg = vx / u cd, sd = torch.cos(delta), torch.sin(delta) - wf = vy + lf * r + vf = vy + lf * r + wf = -vx * sd + vf * cd wr = vy - lr * r - w0f = u * torch.tan(sg * delta) - af = sg * delta - torch.atan2(wf, u) + 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) @@ -322,14 +327,14 @@ def dyn_ph_parts_at(p: DynBikeParams, state: torch.Tensor) -> DynPHParts: return DynPHParts( u=u, sg=sg, + uf=uf, Fxf=Fxf, Fxr=Fxr, Fyf=Fyf, Fyr=Fyr, wf=wf, wr=wr, - w0f=w0f, - Sf=_secant_w(Fyf, af, wf, w0f, p.B_f, p.C, Dyf, u), + 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, @@ -350,21 +355,20 @@ def dyn_ph_rates_at( nr = q.nr.unbind(-1) tf = q.tf.unbind(-1) tr = q.tr.unbind(-1) - # R = Dx e_x e_x^T + (Sf / cos delta) nf nf^T + Sr nr nr^T. - kf = q.Sf / q.cd + # 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] = kf * (nf[i] * nf[j]) + q.Sr * (nr[i] * nr[j]) + 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. + # 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 / q.cd * nfv + 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) 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 index fcd01fb..e59bd61 100644 --- a/tests/fixtures/dyn_bike_ph_golden.json +++ b/tests/fixtures/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/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]}} ] }