diff --git a/grl_snam/dyn4.py b/grl_snam/dyn4.py new file mode 100644 index 0000000..b124bb8 --- /dev/null +++ b/grl_snam/dyn4.py @@ -0,0 +1,345 @@ +"""The four-wheel vehicle with suspension, differentiable in torch. + +The torch twin of libcvc's ``cvc::nav::diff::dyn4_plant`` (``inc/cvc/nav/detail/dyn_4w.h``, +P8d of the vehicle-physics roadmap): P8a's single-track plant (:mod:`grl_snam.dyn_bike`) on four +contact patches, each with its own grip, its own terrain height and its own load from a +spring-damper corner. libcvc differentiates it with forward-mode duals; here autograd does, so +the two gradients are independent derivations of one model, the parity gate +(``tests/test_dyn4_parity.py``, against libcvc's ``dyn4_golden.json``). + +State per agent, in nav's normalized frame (lengths x S, time in s, mass in kg), the last axis +of a ``(..., 14)`` tensor in this order: + +- ``x, y``: the rear axle's midpoint; ``psi``: the heading; +- ``vx, vy``: the body-frame velocity at the centre of gravity; ``r``: the yaw rate; +- ``z, zd``: the body's heave from its rest height on flat ground, and its rate; +- ``phi, phid``: its roll (+ lifts the left side), and rate; ``th, thd``: its pitch (+ lowers + the nose), and rate; +- ``delta``: the virtual centre wheel's steering angle; ``a``: the drive or brake force per unit + mass, both behind their actuators. + +The wheels sit at body-frame ``(x_i, y_i)`` from the centre of gravity: front left +``(lf, tf)``, front right ``(lf, -tf)``, rear left ``(-lr, tr)``, rear right ``(-lr, -tr)``. + +The ground is a callable ``ground(wx, wy) -> (mu, height)`` on tensors of world positions, so +its gradients flow through autograd as libcvc's sampler's flow through its duals. +:class:`FlatGround` is the default. + +One step of ``hdt`` from the commands ``(delta_cmd, a_cmd)``: + +1. actuators, as P8a; +2. loads: each corner's spring-damper from the current vertical state, floored smoothly at zero + (softplus of width ``fz_min * m * g``); +3. the drive or brake force split between the axles, then evenly left and right, each wheel + saturated smoothly at ``mu_i Fz_i``; the speed predicted from these and the resistances; +4. ``(vy, r)`` by implicit Euler on four tires, the front pair at their Ackermann angles, each + linearized by its secant through its root in its own lateral velocity; +5. the speed corrected; ``(z, phi, th)`` by implicit Euler on the corners' springs and dampers + (a 3x3 solve), driven by the ground and the tire forces' moments about the centre of gravity; +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. + +The corners' rates follow libcvc's float arithmetic (``k = 0`` is a stiff spring of natural +frequency ``1 / hdt``, ``c = 0`` critical damping), so the two agree to the last bits. Every +branch is a ``torch.where`` on values, as the C++ branches on ``s_val``. +""" + +from __future__ import annotations + +import math +from collections.abc import Callable +from dataclasses import dataclass + +import numpy as np +import torch + +from grl_snam.dyn_bike import PlantLimits, _clamp, _lateral, _sat, _secant_w + +__all__ = ["ORDER", "Dyn4Params", "FlatGround", "PlantLimits", "ackermann", "dyn4_plant"] + +ORDER = ( + "x", + "y", + "psi", + "vx", + "vy", + "r", + "z", + "zd", + "phi", + "phid", + "th", + "thd", + "delta", + "a", +) + +Ground = Callable[[torch.Tensor, torch.Tensor], tuple[torch.Tensor, torch.Tensor]] + + +@dataclass(frozen=True) +class Dyn4Params: + """``cvc::nav::diff::dyn4_params``, in the normalized frame.""" + + m: float = 1.0 + Iz: float = 1.0 + Ix: float = 1.0 + Iy: float = 1.0 + lf: float = 0.5 + lr: float = 0.5 + tf: float = 0.0 + tr: float = 0.0 + h: float = 0.0 + g: float = 9.81 + k_f: float = 0.0 + k_r: float = 0.0 + c_f: float = 0.0 + c_r: float = 0.0 + B_f: float = 12.0 + B_r: float = 12.0 + C: float = 1.2 + front_drive: float = 0.0 + front_brake: float = 0.6 + drag: float = 0.0 + c_rr: float = 0.0 + tau_steer: float = 0.0 + steer_rate: float = 0.0 + tau_drive: float = 0.0 + v_blend: float = 0.0 + v_width: float = 1.0 + eps: float = 0.1 + fz_min: float = 0.02 + iters: int = 3 + + +@dataclass(frozen=True) +class FlatGround: + """Flat ground of one friction (libcvc's ``flat_ground``, whose ``mu`` is a float).""" + + mu: float = 0.9 + + def __call__(self, wx: torch.Tensor, wy: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + return torch.full_like(wx, self.mu), torch.zeros_like(wx) + + +def ackermann(L, y, tan_delta): + """A front wheel's angle at body-frame ``y``: tan delta_i = L tan delta / (L - y tan delta).""" + return torch.atan(L * tan_delta / (L - y * tan_delta)) + + +def _corner_rates(p: Dyn4Params, hdt: float) -> tuple[list[float], list[float]]: + """Each corner's spring and damper, in libcvc's float arithmetic.""" + f32 = np.float32 + m_corner = f32(0.25) * f32(p.m) + k, c = [], [] + for kc, cc in ((p.k_f, p.c_f), (p.k_f, p.c_f), (p.k_r, p.c_r), (p.k_r, p.c_r)): + if f32(kc) > 0.0: + kk = f32(kc) + else: + w = f32(1.0) / f32(hdt) + kk = m_corner * w * w + cd = f32(cc) if f32(cc) > 0.0 else f32(2.0) * np.sqrt(kk * m_corner) + k.append(float(kk)) + c.append(float(cd)) + return k, c + + +def _soft_pos(F: torch.Tensor, e: float) -> torch.Tensor: + """e log(1 + exp(F / e)), and F itself above 30 e.""" + q = F / e + big = q > 30.0 + q_safe = torch.where(big, torch.zeros_like(q), q) + return torch.where(big, F, e * torch.log(1.0 + torch.exp(q_safe))) + + +def _det3(A): + return ( + A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) + - A[0][1] * (A[1][0] * A[2][2] - A[1][2] * A[2][0]) + + A[0][2] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]) + ) + + +def dyn4_plant( + p: Dyn4Params, + lim: PlantLimits, + state: torch.Tensor, + delta_cmd: torch.Tensor, + a_cmd: torch.Tensor, + ground: Ground | None = None, +) -> torch.Tensor: + """One step of the plant: ``state`` ``(..., 14)`` to the next ``(..., 14)``.""" + if ground is None: + ground = FlatGround() + x, y, psi, vx, vy, r, z, zd, phi, phid, th, thd, delta, a = state.unbind(-1) + hdt = lim.hdt + m, Iz, Ix, Iy, lf, lr = p.m, p.Iz, p.Ix, p.Iy, p.lf, p.lr + h, g, Cp = p.h, p.g, p.C + L = lf + lr + + # 1. Actuators. + k_steer = 1.0 - math.exp(-hdt / p.tau_steer) if p.tau_steer > 0.0 else 1.0 + step = (delta_cmd - delta) * k_steer + if p.steer_rate > 0.0: + R = p.steer_rate * hdt + step = R * torch.tanh(step / R) + delta1 = delta + step + k_drive = 1.0 - math.exp(-hdt / p.tau_drive) if p.tau_drive > 0.0 else 1.0 + a1 = a + (a_cmd - a) * k_drive + + # 2. The wheels, the ground under them and their loads. + xi = [lf, lf, -lr, -lr] + yi = [p.tf, -p.tf, p.tr, -p.tr] + Fz0 = [0.5 * m * g * (lr if i < 2 else lf) / L for i in range(4)] + k, c = _corner_rates(p, hdt) + cps, sps = torch.cos(psi), torch.sin(psi) + cgx, cgy = x + lr * cps, y + lr * sps + mu_i, hg = [], [] + for i in range(4): + mu, height = ground(cgx + xi[i] * cps - yi[i] * sps, cgy + xi[i] * sps + yi[i] * cps) + mu_i.append(mu) + hg.append(height) + e = p.fz_min * m * g + Fz = [] + for i in range(4): + defl = hg[i] - (z + yi[i] * phi - xi[i] * th) + rate = zd + yi[i] * phid - xi[i] * thd + Fz.append(_soft_pos(Fz0[i] + k[i] * defl - c[i] * rate, e)) + + # 3. Longitudinal. + F_drive = m * a1 + share = torch.where( + a1 >= 0.0, torch.full_like(a1, p.front_drive), torch.full_like(a1, p.front_brake) + ) + td = torch.tan(delta1) + Fx, Dy, cdw, sdw = [], [], [], [] + for i in range(4): + axle_share = share if i < 2 else 1.0 - share + cap = mu_i[i] * Fz[i] + Fx.append(_sat(0.5 * F_drive * axle_share, cap)) + Dy.append(torch.sqrt(cap * cap - 0.9975 * Fx[i] * Fx[i])) + dw = ackermann(L, yi[i], td) if i < 2 else torch.zeros_like(td) + cdw.append(torch.cos(dw)) + sdw.append(torch.sin(dw)) + eps2 = p.eps * p.eps + sp_min = float(np.float32(-0.25) * np.float32(lim.vmax)) if lim.allow_reverse else 0.0 + u0 = torch.sqrt(vx * vx + eps2) + sg0 = vx / u0 + F_res = -p.drag * vx * u0 - p.c_rr * m * g * sg0 + FXlong = torch.zeros_like(vx) + for i in range(4): + FXlong = FXlong + Fx[i] * cdw[i] + vx_p = _clamp(vx + hdt * ((FXlong + F_res) / m), sp_min, lim.vmax) + + # 4. Lateral and yaw: implicit Euler for (vy, r), four tires. + Bw = [p.B_f, p.B_f, p.B_r, p.B_r] + vy1, r1 = vy, r + Fy_applied = [torch.zeros_like(vy)] * 4 + zero = torch.zeros_like(vy) + for _ in range(max(1, p.iters)): + FY, MZ = zero, zero + J11, J12, J21, J22 = zero, zero, zero, zero + Fy, St, dwdvy, dwdr = [], [], [], [] + for i in range(4): + vxi = vx_p - r1 * yi[i] + vyi = vy1 + r1 * xi[i] + wi = -vxi * sdw[i] + vyi * cdw[i] + fwd = vxi * cdw[i] + vyi * sdw[i] + ui = torch.sqrt(fwd * fwd + eps2) + alpha = -torch.atan2(wi, ui) + Fy.append(_lateral(alpha, Bw[i], Cp, Dy[i])) + St.append(_secant_w(Fy[i], alpha, wi, torch.zeros_like(wi), Bw[i], Cp, Dy[i], ui)) + dwdvy.append(cdw[i]) + dwdr.append(yi[i] * sdw[i] + xi[i] * cdw[i]) + fby = Fy[i] * cdw[i] + Fx[i] * sdw[i] + fbx = -Fy[i] * sdw[i] + Fx[i] * cdw[i] + FY = FY + fby + MZ = MZ + xi[i] * fby - yi[i] * fbx + dfy_y = -St[i] * cdw[i] + dfx_y = St[i] * sdw[i] + J11 = J11 + dfy_y * dwdvy[i] + J12 = J12 + dfy_y * dwdr[i] + J21 = J21 + (xi[i] * dfy_y - yi[i] * dfx_y) * dwdvy[i] + J22 = J22 + (xi[i] * dfy_y - yi[i] * dfx_y) * dwdr[i] + f_vy = FY / m - r1 * vx_p + f_r = MZ / Iz + A11 = 1.0 - hdt * (J11 / m) + A12 = -hdt * (J12 / m - vx_p) + A21 = -hdt * (J21 / Iz) + A22 = 1.0 - hdt * (J22 / Iz) + det = A11 * A22 - A12 * A21 + det = torch.where(det < 0.05, torch.full_like(det, 0.05), det) + b1 = vy + hdt * f_vy - vy1 + b2 = r + hdt * f_r - r1 + dvy = (A22 * b1 - A12 * b2) / det + dr = (A11 * b2 - A21 * b1) / det + vy1 = vy1 + dvy + r1 = r1 + dr + Fy_applied = [Fy[i] - St[i] * (dwdvy[i] * dvy + dwdr[i] * dr) for i in range(4)] + + # 3, corrected: the speed with the lateral forces the implicit step applied. + FXb, FYb = F_res, zero + for i in range(4): + FXb = FXb + Fx[i] * cdw[i] - Fy_applied[i] * sdw[i] + FYb = FYb + Fy_applied[i] * cdw[i] + Fx[i] * sdw[i] + vx1 = _clamp(vx + hdt * (FXb / m + r1 * vy1), sp_min, lim.vmax) + + # 5. Vertical: implicit Euler on (z, phi, th) with the corners' linear springs and dampers. + Kq = [[0.0] * 3 for _ in range(3)] + Dq = [[0.0] * 3 for _ in range(3)] + Gv = [-m * g + zero, h * FYb, -h * (FXb - F_res)] + for i in range(4): + ci = (1.0, yi[i], -xi[i]) + for aa in range(3): + Gv[aa] = Gv[aa] + ci[aa] * (Fz0[i] + k[i] * hg[i]) + for bb in range(3): + Kq[aa][bb] = Kq[aa][bb] + k[i] * ci[aa] * ci[bb] + Dq[aa][bb] = Dq[aa][bb] + c[i] * ci[aa] * ci[bb] + Mv = (m, Ix, Iy) + q0, qd0 = (z, phi, th), (zd, phid, thd) + A = [[0.0] * 3 for _ in range(3)] + rhs = [None] * 3 + for aa in range(3): + rhs[aa] = Mv[aa] * qd0[aa] + hdt * Gv[aa] + for bb in range(3): + A[aa][bb] = hdt * Dq[aa][bb] + hdt * hdt * Kq[aa][bb] + (Mv[aa] if aa == bb else 0.0) + rhs[aa] = rhs[aa] - hdt * Kq[aa][bb] * q0[bb] + det3 = _det3(A) + qd1 = [] + for col in range(3): + Ac = [[rhs[aa] if bb == col else A[aa][bb] for bb in range(3)] for aa in range(3)] + qd1.append(_det3(Ac) / det3) + + # 6. Low speed: toward the kinematic bicycle. + if p.v_blend > 0.0: + w = torch.sigmoid(-(torch.sqrt(vx1 * vx1 + eps2) - p.v_blend) / p.v_width) + r_k = vx1 * torch.tan(delta1) / L + vy_k = lr * r_k + vy1 = vy1 + w * (vy_k - vy1) + r1 = r1 + w * (r_k - r1) + + # 7. Pose. + psi1 = psi + hdt * r1 + v_side = vy1 - lr * r1 + 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, + z + hdt * qd1[0], + qd1[0], + phi + hdt * qd1[1], + qd1[1], + th + hdt * qd1[2], + qd1[2], + delta1, + a1, + ), + dim=-1, + ) diff --git a/tests/fixtures/dyn4_golden.json b/tests/fixtures/dyn4_golden.json new file mode 100644 index 0000000..f14b9d4 --- /dev/null +++ b/tests/fixtures/dyn4_golden.json @@ -0,0 +1,67 @@ +{ + "format": "cvc-dyn4-golden-1", + "order": ["x", "y", "psi", "vx", "vy", "r", "z", "zd", "phi", "phid", "th", "thd", "delta", "a"], + "params": [ + {"m": 2676, "Iz": 6283.69384765625, "Ix": 1240.4151611328125, "Iy": 5404.53857421875, "lf": 1.5369999408721924, "lr": 1.7630000114440918, "tf": 0.9100000262260437, "tr": 0.9100000262260437, "h": 0.74000000953674316, "g": 9.8066501617431641, "k_f": 47691.4921875, "k_r": 41577.89453125, "c_f": 5838.720703125, "c_r": 5090.251953125, "B_f": 36.121696472167969, "B_r": 42.496116638183594, "C": 1.2021652460098267, "front_drive": 0.5, "front_brake": 0.70588237047195435, "drag": 1.5006250143051147, "c_rr": 0.014999999664723873, "tau_steer": 0.05000000074505806, "steer_rate": 0.34999999403953552, "tau_drive": 0.10000000149011612, "v_blend": 1, "v_width": 0.30000001192092896, "eps": 0.5, "fz_min": 0.019999999552965164, "iters": 3}, + {"m": 2676, "Iz": 6283.69384765625, "Ix": 1240.4151611328125, "Iy": 5404.53857421875, "lf": 1.5369999408721924, "lr": 1.7630000114440918, "tf": 0.9100000262260437, "tr": 0.9100000262260437, "h": 0.74000000953674316, "g": 9.8066501617431641, "k_f": 0, "k_r": 0, "c_f": 0, "c_r": 0, "B_f": 36.121696472167969, "B_r": 42.496116638183594, "C": 1.2021652460098267, "front_drive": 0.5, "front_brake": 0.70588237047195435, "drag": 1.5006250143051147, "c_rr": 0.014999999664723873, "tau_steer": 0.05000000074505806, "steer_rate": 0.34999999403953552, "tau_drive": 0.10000000149011612, "v_blend": 1, "v_width": 0.30000001192092896, "eps": 0.5, "fz_min": 0.019999999552965164, "iters": 3} + ], + "ground": {"mu_left": 0.5, "mu_right": 0.90000000000000002, "split_width": 0.29999999999999999, "A": 0.059999999999999998, "cx": 4, "cy": 3, "r": 0.80000000000000004}, + "g_out": [0.69999999999999996, -0.29999999999999999, 1.1000000000000001, 0.40000000000000002, -0.90000000000000002, 0.25, 0.80000000000000004, -0.20000000000000001, 0.5, 0.29999999999999999, -0.59999999999999998, 0.14999999999999999, 0.59999999999999998, -0.14999999999999999], + "cases": [ + {"params": 0, "state": [0, 0, 0, 12, 0.10000000000000001, 0.050000000000000003, 0, 0, 0, 0, 0, 0, 0.02, 0.5], "delta_cmd": 0.050000000000000003, "a_cmd": 1, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [0.72163109892909338, 0.0027127276838284208, 0.0058889244396855478, 12.027242952884908, 0.14742080409674607, 0.098148742855221155, 1.1237378706555782e-06, 1.8728964929551291e-05, 0.0049169753148895108, 0.081949590413207929, -0.00044538681808697136, -0.0074231138007023788, 0.035978411112368727, 0.72559417581953878], + "g_state": [0.70000845938790757, -0.2997789477045501, 0.8820732682607586, 0.44312091145800947, -0.4925711709931741, 0.21922008644856103, 1.3030806186537625, -0.069309658165976826, -0.56302762318656074, 0.13841518326797711, -0.94129370266309165, 0.053660460669990528, -0.30373309173264523, -0.068287200865146447], + "g_delta_cmd": -0.12663233907217267, "g_a_cmd": -0.056140188876111045}, + {"params": 0, "state": [1, -2, 0.29999999999999999, 15, -0.59999999999999998, 0.5, 0.01, -0.02, 0.02, 0.050000000000000003, -0.01, 0.029999999999999999, 0.20000000000000001, 0], "delta_cmd": 0.25, "a_cmd": 0, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [1.8733965671167805, -1.7882058269684011, 0.32749468726116926, 14.918414006317281, -0.53227361235225956, 0.45824479792872391, 0.0079183275375169729, -0.034694541816867229, 0.037515284510706501, 0.29192141503672703, -0.0069017195809452231, 0.051638008138445719, 0.2195453364485479, 0], + "g_state": [0.70000000074478008, -0.30245335370142151, 0.68187759221869082, 0.45813632607380178, -0.91508696847319237, 1.1311165836919064, 3.6855836384438758, 0.2223727559172643, -0.67039457346825826, 0.12527058226039786, 3.288652008960633, 0.57151952168371545, 0.44759840068430301, -0.0679055872778591], + "g_delta_cmd": 0.046144741379054922, "g_a_cmd": -0.055826457187059836}, + {"params": 0, "state": [-3, 4, -1.1000000000000001, 14, 0.40000000000000002, 0.20000000000000001, -0.0050000000000000001, 0.01, -0.01, 0, 0.02, -0.050000000000000003, 0.10000000000000001, -4], "delta_cmd": 0.10000000000000001, "a_cmd": -6, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [-2.6217011934166194, 3.2687694820288757, -1.087524766248477, 13.721282525430517, 0.28637518945986973, 0.20792056717276949, -0.0039591637523130833, 0.017347271182523681, -0.0057452904957051975, 0.070911826656582883, 0.018192649196857116, -0.030122514059005404, 0.10000000000000001, -4.9023767032781551], + "g_state": [0.69999999999999996, -0.29613375116461865, 1.5010382520025809, 0.44437521459993867, -0.65428328308470851, 0.46626939827314468, 1.9903043729384213, 0.014825003237494805, -0.71051985569712739, 0.12035816909052562, -1.1287000539312668, 0.03071688480458868, 0.17862001245922571, -0.08162470594451543], + "g_delta_cmd": 0.41441928713513987, "g_a_cmd": -0.067105202008960599}, + {"params": 0, "state": [0.69999999999999996, 3.5, 0.050000000000000003, 8, 0.10000000000000001, 0.10000000000000001, 0, 0, 0, 0, 0, 0, 0.050000000000000003, 0], "delta_cmd": 0.10000000000000001, "a_cmd": 0.5, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [1.1791378256759428, 3.5247550029652142, 0.058517586554974022, 7.9960914861031691, 0.19511909995235585, 0.14195977908928203, 0.0020681050122793492, 0.03446841764175166, 0.0038744122711175271, 0.064573539295289861, -0.0014354355715143782, -0.023923926726647737, 0.069545336448547879, 0.22559417581953883], + "g_state": [0.70356339738855245, -0.32151265773690474, 0.86972007327801926, 0.44206479565118351, -0.50625612637799589, 0.18259168757573621, 1.4036257266363981, -0.057000234141410561, -0.5231539649742446, 0.14329679091094208, -0.86828165364232024, 0.06259909806229319, -0.19791671329915439, -0.068463577143273252], + "g_delta_cmd": -0.020404039728067598, "g_a_cmd": -0.056285191123118632}, + {"params": 0, "state": [-5, 0.10000000000000001, 0.02, 18, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, -3], "delta_cmd": 0, "a_cmd": -8, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [-3.9386802012423319, 0.12160607918483068, 0.019684132917521507, 17.692324773487073, 0.0025868156507929908, -0.0052644514923111995, -1.7708927936164909e-10, -2.951488055331717e-09, -0.00012475390530029479, -0.0020792318014793655, 0.011569695398484485, 0.026161590559498529, 0, -5.2559417581953882], + "g_state": [0.69999999999999996, -0.29988357587132003, 0.76701767325443215, 0.4407203793877052, -0.60647836155181134, 0.48424535898248711, 1.2556505011824981, -0.075116379468836883, -0.43688402983214147, 0.15385855009661809, -1.1725496351011697, 0.025348518028219677, 0.083628449572683131, -0.071412193302551691], + "g_delta_cmd": 0.19402776866360219, "g_a_cmd": -0.058709303782706848}, + {"params": 0, "state": [0, 0, 0, 0.59999999999999998, 0.02, 0.050000000000000003, 0, 0, 0, 0, 0, 0, 0.29999999999999999, 0.29999999999999999], "delta_cmd": 0.34999999999999998, "a_cmd": 0.5, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [0.035978224695669254, 7.8081977973431879e-05, 0.0036246756893130671, 0.59963786959080445, 0.10563292962679138, 0.060411262838848068, 1.123724685176085e-06, 1.8728745171554826e-05, 0.0036228270272335686, 0.06038045180350108, -6.7928721306615894e-05, -0.0011321453804156861, 0.31954533644854788, 0.39023767032781553], + "g_state": [0.70001053974965066, -0.2999640753925496, 1.0892551860334572, 0.33890845124895652, -0.17534856892921305, 0.078866476739045135, 1.1550504899660954, -0.087432525244901513, -0.52472959702914301, 0.14310389119306263, -0.91016909201952712, 0.057470949839098201, 0.33794784682064766, -0.071522513657545125], + "g_delta_cmd": 0.034840419374389746, "g_a_cmd": -0.058800000216119627}, + {"params": 0, "state": [0.5, 0.5, 0.20000000000000001, -2, 0.050000000000000003, -0.10000000000000001, 0, 0, 0.01, 0, 0, 0, 0.20000000000000001, -0.5], "delta_cmd": 0.25, "a_cmd": -0.80000000000000004, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 1}, + "next": [0.38195840865180714, 0.47840775241079153, 0.19289720870573102, -1.9998595522896305, -0.18475082146653074, -0.11837985755047914, 0.00039472712905614678, 0.0065787856313164307, -0.00037566480050625281, -0.17292775054034065, -0.00015308820464018688, -0.0025514701343662496, 0.2195453364485479, -0.63535650549172329], + "g_state": [0.70420950635366553, -0.30449033205848602, 1.1307740193593281, 0.41185168135905487, -0.45015816491923188, -0.078518139588756086, 0.81465603022538913, -0.12910595761612004, -0.50161102315083805, 0.1459342260937567, -0.67810873469641653, 0.085881376086457073, 0.8596739366222137, -0.070046096287712842], + "g_delta_cmd": 0.088627286011518758, "g_a_cmd": -0.057586209799288232}, + {"params": 0, "state": [2, 2, 2.5, 10, 2.5, 1.1000000000000001, 0.02, 0.10000000000000001, 0.040000000000000001, -0.10000000000000001, 0, 0.050000000000000003, 0.40000000000000002, 1], "delta_cmd": 0.45000000000000001, "a_cmd": 2, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [1.4902152892803535, 2.3340141722742125, 2.5696079841077641, 10.157398970499345, 1.9636287371126497, 1.1601330943937327, 0.020754548567750952, 0.012575809743607093, 0.036699065106174086, -0.055015582793459326, 0.0011981841856031474, 0.019969736873077525, 0.41954533644854791, 1.4511883516390776], + "g_state": [0.69630206391467186, -0.30067417911736455, 1.0214387256908726, 0.39993283960685022, -0.64523853589014846, 0.35062264195896164, 1.7812734195615509, -0.010766004648815291, -0.51843529593162418, 0.14387448280594123, -0.94804300061694113, 0.052834164704907302, 0.46402402843022839, -0.067408113478903797], + "g_delta_cmd": 0.047838126215027463, "g_a_cmd": -0.055417474644497866}, + {"params": 0, "state": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "delta_cmd": 0.10000000000000001, "a_cmd": 2, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [0.0032481979591471731, 4.8372584893719119e-08, 2.0625612237786974e-05, 0.054136633867614234, 0.0006057388656155033, 0.00034376021164675571, 1.1237433377405803e-06, 1.8729056047636695e-05, 2.6845568946861884e-05, 0.00044742615911511871, -0.00060189197217464137, -0.010031533093799594, 0.020946006673345115, 0.90237670327815533], + "g_state": [0.70001059340442395, -0.29996837172804014, 1.0991202417172341, 0.42866769277290073, -0.22191632627756169, 0.0075129776321974616, 1.1516621102278115, -0.08784735401093885, -0.52402769522888448, 0.14318982284185508, -0.90926746976154682, 0.057581332643911465, 0.58343049298677863, -0.068644632749062864], + "g_delta_cmd": 0.0021013213545767098, "g_a_cmd": -0.056434040333180419}, + {"params": 1, "state": [1, -2, 0.29999999999999999, 15, -0.59999999999999998, 0.5, 0, 0, 0, 0, 0, 0, 0.20000000000000001, 0], "delta_cmd": 0.25, "a_cmd": 0, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [1.8735265964795507, -1.7896031910519152, 0.32902486345977516, 14.910871487592926, -0.53287851918997664, 0.48374773514219088, -2.1132274580139618e-05, -0.0003522045842080462, 0.0075613580626087071, 0.12602263719363724, 0.00024935587984354761, 0.0041559314236180997, 0.2195453364485479, 0], + "g_state": [0.69999999948112257, -0.29184243155908923, 0.71319702455726042, 0.4594194668991195, -0.91704147061531727, 1.1358794954951639, 14.889206777005246, 1.5763531981775658, -1.6765094153238447, -0.023884123061825743, 18.244178225990495, 2.3456783201274147, 0.4609536472203607, -0.068001625204078597], + "g_delta_cmd": 0.047521588116035483, "g_a_cmd": -0.055905411767844013}, + {"params": 1, "state": [0.69999999999999996, 3.5, 0.050000000000000003, 8, 0.10000000000000001, 0.10000000000000001, 0, 0, 0, 0, 0, 0, 0.050000000000000003, -2], "delta_cmd": 0.10000000000000001, "a_cmd": -4, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "next": [1.1679073424592845, 3.5241181026862374, 0.058723013792275888, 7.8086049087102118, 0.19990106338159228, 0.14538356645417397, 0.0036065836355641647, 0.060109728602959878, -0.0015803389824868667, -0.02633898363016994, -0.001424414632490402, -0.023740244405475848, 0.069545336448547879, -2.9023767032781551], + "g_state": [0.70655864148715697, -0.33754149980399722, 0.82213749022949278, 0.43728049796338603, -0.40688686858379097, 0.20131319427365937, 2.8545555251391117, 0.13219512384477763, -0.90668420016993723, 0.068494897902084462, -0.82545382635621556, 0.057322594099918855, -0.69280663260984598, -0.07151098108506794], + "g_delta_cmd": -0.071424256294480681, "g_a_cmd": -0.058790519071938961} + ] +} diff --git a/tests/test_dyn4_parity.py b/tests/test_dyn4_parity.py new file mode 100644 index 0000000..e3b447f --- /dev/null +++ b/tests/test_dyn4_parity.py @@ -0,0 +1,138 @@ +"""Parity for the four-wheel vehicle with suspension (libcvc P8d). + +libcvc's ``cvc::nav::diff::dyn4_plant``, differentiated with forward-mode duals, against the torch +twin :func:`grl_snam.dyn4.dyn4_plant`, differentiated by autograd. The reference is libcvc's +``src/cvc/tests/data/dyn4_golden.json`` (copied to ``tests/fixtures``; written by libcvc's +``Dyn4.Golden`` with ``CVC_DYN4_GOLDEN_WRITE=1``): eleven cases of the Humvee, with its springs and +again with rigid corners, on a friction split with a round bump. The cases cover cruising, a hard +corner, braking in a turn, a wheel on the bump, braking across the split, creeping, backing up, +past the tires' peak, and pulling away from rest. Each case records the next state and the VJP of +a fixed output gradient, so the ground's gradients are held too. + +Tier: DOUBLE contract. Both sides run in double from the same float parameters, so the forward +and the gradient agree to 1e-9 relative. A libm or torch kernel differs only in the last bits. +""" + +from __future__ import annotations + +import json +from pathlib import Path + +import pytest + +torch = pytest.importorskip("torch") + +from grl_snam.dyn4 import ( # noqa: E402 + ORDER, + Dyn4Params, + FlatGround, + PlantLimits, + ackermann, + dyn4_plant, +) + +GOLDEN = Path(__file__).parent / "fixtures" / "dyn4_golden.json" +RTOL = 1e-9 + + +def _golden(): + data = json.loads(GOLDEN.read_text(encoding="utf-8")) + assert data["format"] == "cvc-dyn4-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 _ground(gd): + """libcvc's golden_ground: the friction split along y = 0 and a round bump.""" + + def at(wx, wy): + t = 0.5 + 0.5 * torch.tanh(wy / gd["split_width"]) + mu = gd["mu_right"] + t * (gd["mu_left"] - gd["mu_right"]) + dx, dy = wx - gd["cx"], wy - gd["cy"] + return mu, gd["A"] * torch.exp(-(dx * dx + dy * dy) / (gd["r"] * gd["r"])) + + return at + + +def _limits(c) -> PlantLimits: + lim = c["limits"] + return PlantLimits( + L=lim["L"], vmax=lim["vmax"], hdt=lim["hdt"], allow_reverse=bool(lim["allow_reverse"]) + ) + + +@pytest.mark.parametrize("case", range(len(_golden()["cases"]))) +def test_next_state_and_gradient_match_libcvc(case): + data = _golden() + c = data["cases"][case] + p = Dyn4Params(**data["params"][c["params"]]) + 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 = dyn4_plant(p, _limits(c), state, dcmd, acmd, _ground(data["ground"])) + for k, name in enumerate(ORDER): + assert _close( + nxt[k].item(), c["next"][k] + ), f"case {case} next {name}: torch {nxt[k].item()!r}, libcvc {c['next'][k]!r}" + g_out = torch.tensor(data["g_out"], dtype=torch.float64) + (nxt * g_out).sum().backward() + for k, name in enumerate(ORDER): + assert _close( + state.grad[k].item(), c["g_state"][k] + ), f"case {case} d/d{name}: torch {state.grad[k].item()!r}, libcvc {c['g_state'][k]!r}" + assert _close(dcmd.grad.item(), c["g_delta_cmd"]) + assert _close(acmd.grad.item(), c["g_a_cmd"]) + + +def test_a_batch_steps_like_its_rows(): + data = _golden() + rows = [c for c in data["cases"] if c["params"] == 0 and not c["limits"]["allow_reverse"]] + p = Dyn4Params(**data["params"][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) + nxt = dyn4_plant(p, _limits(rows[0]), state, dcmd, acmd, _ground(data["ground"])) + for i, c in enumerate(rows): + for k in range(len(ORDER)): + assert _close(nxt[i, k].item(), c["next"][k]) + + +def test_the_front_wheels_follow_ackermann(): + # libcvc's P2 gate, as Jolt's Humvee meets it: cot(right) - cot(left) = track / wheelbase. + p = Dyn4Params(**_golden()["params"][0]) + L = p.lf + p.lr + for delta in (0.05, 0.2, 0.45, -0.3): + t = torch.tan(torch.tensor(delta, dtype=torch.float64)) + left, right = ackermann(L, p.tf, t), ackermann(L, -p.tf, t) + cot = 1.0 / torch.tan(right) - 1.0 / torch.tan(left) + assert abs(cot.item() - 2.0 * p.tf / L) < 1e-12 + + +def test_braking_from_speed_stops_near_the_friction_limit(): + # The behaviour gate needs only torch: from 20 m/s, braking far past the tires' grip on flat + # ground, the sprung Humvee stops within 10% of v^2 / (2 mu g), as libcvc's Dyn4 gate requires. + p = Dyn4Params(**_golden()["params"][0]) + mu = 0.8 + limits = PlantLimits(L=p.lf + p.lr, vmax=40.0, hdt=0.06) + s = torch.zeros(14, dtype=torch.float64) + s[3] = 20.0 + dist = 0.0 + for _ in range(2000): + if s[3].item() <= 0.01: + break + n = dyn4_plant( + p, + limits, + s, + torch.tensor(0.0, dtype=torch.float64), + torch.tensor(-30.0, dtype=torch.float64), + FlatGround(mu), + ) + dist += torch.hypot(n[0] - s[0], n[1] - s[1]).item() + s = n + ideal = 20.0**2 / (2.0 * mu * p.g) + assert abs(dist / ideal - 1.0) < 0.10, (dist, ideal)