From 8e8814730fe3a193f2edd96ebda1bbdda8f997c3 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Mon, 19 Oct 2026 16:05:47 -0500 Subject: [PATCH] dyn4: the torch twin of libcvc's four-wheel plant, with float64 parity against its golden grl_snam.dyn4.dyn4_plant is libcvc's diff::dyn4_plant (P8d, transfix/libcvc#731): four contact patches with Ackermann front wheels, a ground callable (wx, wy) -> (mu, height) per wheel, spring-damper corners floored by softplus, four tires in the implicit (vy, r) solve, and heave, roll and pitch by an implicit 3x3 step. The corners' rates follow libcvc's float arithmetic. tests/test_dyn4_parity.py holds it to libcvc's dyn4_golden.json (Dyn4.Golden, libcvc@368c0e4a): the sprung and rigid Humvee on a mu split with a bump, eleven cases. Measured worst relative error: next state 4.2e-17, VJP 1.7e-15 (the gate is 1e-9). Also a batch, the Ackermann relation, and stopping from 20 m/s within 10% of v^2 / (2 mu g). --- grl_snam/dyn4.py | 345 ++++++++++++++++++++++++++++++++ tests/fixtures/dyn4_golden.json | 67 +++++++ tests/test_dyn4_parity.py | 138 +++++++++++++ 3 files changed, 550 insertions(+) create mode 100644 grl_snam/dyn4.py create mode 100644 tests/fixtures/dyn4_golden.json create mode 100644 tests/test_dyn4_parity.py 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)