From fa6922dc719efa780db77f6c28ea01123896fad2 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Sat, 17 Oct 2026 00:05:56 -0500 Subject: [PATCH] material: torch twin of the bicycle material rollout; parity with libcvc; trainer on the bicycle libcvc P8b moves the material trainer off the point mass: integrate_bicycle_material keeps the surrogate's force law and drives the deployed kinematic bicycle (transfix/libcvc#642). - grl_snam/material_bike.py: integrate_bicycle_material in torch, the same controller with every branch a torch.where. Autograd follows the branch each step took, as the C++ adjoint does. Guarded sqrt/norm keep an unselected zero from producing NaN gradients. - nav_native: integrate_bicycle_material / _vjp wrappers over the new pycvc entry points, gated by HAS_BICYCLE_MATERIAL_ROLLOUT. - MaterialTrainer(bicycle=BicycleParams(...)) takes the loss through the bicycle, gated by HAS_MATERIAL_TRAINER_BICYCLE; .integrator reports which. The module still imports without torch. - tests/test_material_bike_parity.py: - forward (rtol 1e-4) and gradient (rtol 1e-3) parity against libcvc; - a pure-torch check that the twin drives to its goal. Measured against a local libcvc build: forward within 5e-7 relative, gradient within 3e-6, so two independent derivations agree. - tests/test_material_train_native.py: the trainer descends on the bicycle, and a zero wheelbase is refused. The native tests skip until a pycvc with the bindings is installed. --- grl_snam/material_bike.py | 240 ++++++++++++++++++++++++++++ grl_snam/material_train_native.py | 38 +++++ grl_snam/nav_native.py | 192 ++++++++++++++++++++++ tests/test_material_bike_parity.py | 170 ++++++++++++++++++++ tests/test_material_train_native.py | 26 +++ 5 files changed, 666 insertions(+) create mode 100644 grl_snam/material_bike.py create mode 100644 tests/test_material_bike_parity.py diff --git a/grl_snam/material_bike.py b/grl_snam/material_bike.py new file mode 100644 index 0000000..206b76a --- /dev/null +++ b/grl_snam/material_bike.py @@ -0,0 +1,240 @@ +"""The material rollout on the kinematic bicycle, differentiable in torch. + +The torch twin of libcvc's ``cvc::nav::integrate_bicycle_material`` (P8b of the +vehicle-physics roadmap). The force law is the material surrogate's +(:func:`grl_snam.material_nav.integrate_surrogate_material`): + +- per-obstacle IPC barriers on the discs ``(C, R)``; +- the goal spring and damping; +- the soft and hard material forces from the 6-channel patch. + +Instead of integrating a point mass, it drives the deployed kinematic bicycle +(``drive.cpp``'s ``bicycle_rollout_material``, one substep per step): + +- the longitudinal command ``(F . heading - gamma * sp) / mass``, clamped; +- pure pursuit toward the goal, plus the steering bias + ``k_steer * tanh((F_rep + F_mat) . left)``; +- the corner, stop, creep and reverse governors; +- the ``d_cap`` steering limit and the kinematic update. + +The governor reads the nearest disc's clearance and normal. + +Every branch is a ``torch.where``, so autograd follows the branch each step took, +which is what the C++ hand-written adjoint does. Square roots and norms are +guarded so an unselected zero never turns a gradient into NaN. The C++ forward +and gradient are checked against this one in +``tests/test_material_bike_parity.py``. + +State per agent: position ``o`` (B,2), heading ``th`` (B,) and signed speed ``sp`` +(B,). Lengths are in the batch's units; the vehicle limits use the same units. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import torch + +from grl_snam.material_nav import bilinear_sample_patch, sdf_barrier_grad +from grl_snam.train_coef_energy import ipc_piecewise + + +@dataclass(frozen=True) +class BicycleParams: + """Vehicle limits (``cvc::nav::bicycle_material_params``).""" + + L: float = 0.035 # wheelbase + delta_max: float = 0.6 # steering lock, rad + a_max: float = 1.5 # longitudinal acceleration limit + a_lat_max: float = 1.0 # lateral acceleration limit + k_steer: float = 0.8 # steering-bias gain + vmax: float = 0.9 # speed limit + allow_reverse: bool = True + + +def _safe_sqrt(x: torch.Tensor) -> torch.Tensor: + """sqrt(x) for x >= 0 whose gradient is 0, not NaN, at x == 0.""" + pos = x > 0 + return torch.where( + pos, torch.sqrt(torch.where(pos, x, torch.ones_like(x))), torch.zeros_like(x) + ) + + +def _clamp(x: torch.Tensor, lo, hi) -> torch.Tensor: + """clamp as the C++ writes it: below lo -> lo, above hi -> hi, else x.""" + lo_t = torch.as_tensor(lo, dtype=x.dtype, device=x.device) + hi_t = torch.as_tensor(hi, dtype=x.dtype, device=x.device) + return torch.where(x < lo_t, lo_t.expand_as(x), torch.where(x > hi_t, hi_t.expand_as(x), x)) + + +def integrate_bicycle_material( + o0: torch.Tensor, + th0: torch.Tensor, + sp0: torch.Tensor, + goal: torch.Tensor, + C: torch.Tensor, + R: torch.Tensor, + mask: torch.Tensor, + alphas: torch.Tensor, + beta: torch.Tensor, + gamma: torch.Tensor, + lam_soft: torch.Tensor, + lam_hard: torch.Tensor, + rollout_patch: torch.Tensor, + rr: torch.Tensor, + d_hat: torch.Tensor, + dt: torch.Tensor, + H: torch.Tensor, + vehicle: BicycleParams = BicycleParams(), + margin_factor: float = 0.5, + mass: float = 1.0, + d_hat_sdf: float = 3.0, + k_sharp: float = 5.0, +) -> tuple[torch.Tensor, ...]: + """Roll the bicycle out; returns (oT, thT, spT, min_clear, cum_risk, hard_count, arc_length). + + Shapes as :func:`grl_snam.material_nav.integrate_surrogate_material`, with the start + heading ``th0`` and signed speed ``sp0`` (B,) in place of ``v0``. ``mask`` (B,N) bool. + """ + B, N = C.shape[:2] + dtype, dev = o0.dtype, o0.device + v = vehicle + L = float(v.L) + dmax = float(v.delta_max) + tan_dmax = torch.tan(torch.tensor(dmax, dtype=dtype)) + a_max, a_lat, vmax = float(v.a_max), float(v.a_lat_max), float(v.vmax) + sp_min = -0.25 * vmax if v.allow_reverse else 0.0 + v_creep_cap = 0.5 * torch.sqrt(torch.tensor(a_lat * L, dtype=dtype) / tan_dmax) + kfloor = tan_dmax / (L * 400.0) + rr_eff = margin_factor * rr + R_eff = R + rr_eff[:, None] + mask = mask.to(torch.bool) + rows = torch.arange(B, device=dev) + hdt = dt + + o, th, sp = o0.clone(), th0.clone(), sp0.clone() + min_clear = torch.full((B,), float("inf"), dtype=dtype, device=dev) + cum_risk = torch.zeros(B, dtype=dtype, device=dev) + hard_count = torch.zeros(B, dtype=dtype, device=dev) + arc_length = torch.zeros(B, dtype=dtype, device=dev) + zero, one = torch.zeros(B, dtype=dtype, device=dev), torch.ones(B, dtype=dtype, device=dev) + + for s in range(int(H.max().item()) if B else 0): + active = s < H + ch, sh = torch.cos(th), torch.sin(th) + + sem = bilinear_sample_patch(rollout_patch, o, o0) + risk = sem[:, 0].clamp(0.0, 1.0) + sdf = sem[:, 1].clamp(0.0, 50.0) + grx, gry, gpx, gpy = sem[:, 2], sem[:, 3], sem[:, 4], sem[:, 5] + + # Geometry: the barrier summed over discs; the governor reads the nearest. + if N: + diff = o.unsqueeze(1) - C + r = torch.linalg.norm(diff, dim=-1).clamp_min(1e-9) + n = diff / r.unsqueeze(-1) + d = r - R_eff + d_m = torch.where(mask, d, torch.full_like(d, 1e6)) + _, dbdd = ipc_piecewise(d_m, d_hat.view(-1, 1)) + dbdd = torch.where(mask, dbdd, torch.zeros_like(dbdd)) + Fg = (-(alphas * dbdd).unsqueeze(-1) * n).sum(1) + brep = dbdd.clamp(max=0.0) + Fr = (-(alphas * brep).unsqueeze(-1) * n).sum(1) + d_inf = torch.where(mask, d, torch.full_like(d, float("inf"))) + dmin, jmin = d_inf.min(dim=1) + has = torch.isfinite(dmin) + d_gov = torch.where(has, dmin, torch.full_like(dmin, 1e30)) + n_near = n[rows, jmin] + ngx = torch.where(has, n_near[:, 0], zero) + ngy = torch.where(has, n_near[:, 1], zero) + else: + Fg = torch.zeros_like(o) + Fr = torch.zeros_like(o) + dmin = torch.full((B,), float("inf"), dtype=dtype, device=dev) + d_gov = torch.full((B,), 1e30, dtype=dtype, device=dev) + ngx = ngy = zero + min_clear = torch.minimum(min_clear, dmin) + + Fgoal = -beta.unsqueeze(-1) * (o - goal) + _, db = sdf_barrier_grad(sdf, d_hat_sdf=d_hat_sdf, k_sharp=k_sharp) + Fmx = -lam_soft * grx + (-lam_hard * db) * gpx + Fmy = -lam_soft * gry + (-lam_hard * db) * gpy + Fx = (Fg[:, 0] + Fgoal[:, 0]) + Fmx + Fy = (Fg[:, 1] + Fgoal[:, 1]) + Fmy + + a0c = _clamp(((Fx * ch + Fy * sh) - gamma * sp) / mass, -a_max, a_max) + + # Pure pursuit toward the goal. + tgx, tgy = goal[:, 0] - o[:, 0], goal[:, 1] - o[:, 1] + Ld_raw = _safe_sqrt(tgx * tgx + tgy * tgy) + Ld = torch.where(Ld_raw < 1e-6, torch.full_like(Ld_raw, 1e-6), Ld_raw) + ang = torch.atan2(tgy, tgx) - th + sa = torch.sin(ang) + behind = torch.cos(ang) < 0.0 + turn_sign = torch.where(sa >= 0.0, one, -one) + cap_raw = 1.2 * sp + 4.0 * L + cap = torch.where(cap_raw < 4.0 * L, torch.full_like(cap_raw, 4.0 * L), cap_raw) + Ldeff = torch.where(cap < Ld, cap, Ld) + delta0 = torch.where(behind, turn_sign * dmax, torch.atan2(2.0 * L * sa, Ldeff)) + S = (Fr[:, 0] + Fmx) * (-sh) + (Fr[:, 1] + Fmy) * ch + delta2 = _clamp(delta0 + v.k_steer * torch.tanh(S), -dmax, dmax) + + # Governors. + kappa_raw = torch.abs(torch.tan(delta2)) / L + kappa = torch.where(kappa_raw < kfloor, kfloor.expand_as(kappa_raw), kappa_raw) + v_corner = torch.sqrt(a_lat / kappa) + ds_raw = d_gov - 0.5 * rr_eff + ds = torch.where(ds_raw < 0.0, zero, ds_raw) + v_stop = _safe_sqrt(2.0 * a_max * ds) + msign = torch.where(sp >= 0.0, one, -one) + appr = _clamp(-(ngx * ch + ngy * sh) * msign, 0.0, 1.0) + v_stop_dir = torch.where( + appr > 0.05, v_stop / appr.clamp_min(0.05), torch.full_like(appr, vmax) + ) + m = torch.where(v_stop_dir < v_corner, v_stop_dir, v_corner) + vlim0 = torch.where(m < vmax, m, torch.full_like(m, vmax)) + hard_steer = torch.abs(delta2) >= 0.7 * dmax + can_move = d_gov > 0.25 * rr_eff + v_floor = torch.where(hard_steer & can_move, torch.full_like(m, 0.08), zero) + v_lim = torch.where(vlim0 < v_floor, v_floor, vlim0) + half_vc = 0.5 * v_corner + v_creep = torch.where(v_creep_cap < half_vc, v_creep_cap.expand_as(half_vc), half_vc) + + fbh_raw = Fg[:, 0] * ch + Fg[:, 1] * sh + fbh = torch.where(fbh_raw > 0.0, zero, fbh_raw) + a_bh = torch.where(behind, (v_creep - sp) / hdt + fbh, a0c) + stuck = hard_steer & can_move & (torch.abs(sp) < 0.06) + floor_a = (0.08 - sp) / hdt + a_st = torch.where(stuck & (a_bh < floor_a), floor_a, a_bh) + if v.allow_reverse: + head_on = _clamp(-(ngx * ch + ngy * sh), 0.0, 1.0) + rev = behind & (head_on > 0.6) & (d_gov < 0.5 * rr_eff + 0.02) + else: + rev = torch.zeros(B, dtype=torch.bool, device=dev) + a1 = torch.where(rev, (-0.10 - sp) / hdt, a_st) + delta3 = torch.where(rev, -delta2, delta2) + a1c = _clamp(a1, -a_max, a_max) + lim = (v_lim - sp) / hdt + a3 = _clamp(torch.where(lim < a1c, lim, a1c), -a_max, a_max) + + sp1 = _clamp(sp + hdt * a3, sp_min, vmax) + sp2 = sp1 * sp1 + sp2 = torch.where(sp2 < 1e-9, torch.full_like(sp2, 1e-9), sp2) + d_cap = torch.atan(a_lat * L / sp2) + delta4 = torch.where(delta3 < -d_cap, -d_cap, torch.where(delta3 > d_cap, d_cap, delta3)) + th1 = th + hdt * (sp1 / L) * torch.tan(delta4) + o1 = torch.stack( + [o[:, 0] + hdt * sp1 * torch.cos(th1), o[:, 1] + hdt * sp1 * torch.sin(th1)], -1 + ) + + sdx, sdy = o1[:, 0] - o[:, 0], o1[:, 1] - o[:, 1] + disp = torch.where(active, _safe_sqrt(sdx * sdx + sdy * sdy), zero) + arc_length = arc_length + disp + cum_risk = cum_risk + risk * disp + hard_count = hard_count + torch.where(active & (sdf < 1.0), one, zero) + + o = torch.where(active.unsqueeze(-1), o1, o) + th = torch.where(active, th1, th) + sp = torch.where(active, sp1, sp) + + return o, th, sp, min_clear, cum_risk, hard_count, arc_length diff --git a/grl_snam/material_train_native.py b/grl_snam/material_train_native.py index df98617..8fbbeed 100644 --- a/grl_snam/material_train_native.py +++ b/grl_snam/material_train_native.py @@ -46,6 +46,10 @@ HAS_MATERIAL_TRAINER_CUDA = HAS_MATERIAL_TRAINER and hasattr( _pycvc, "nav_material_trainer_cuda_active" ) +# Training through the deployed kinematic bicycle instead of the point mass (libcvc P8b). +HAS_MATERIAL_TRAINER_BICYCLE = HAS_MATERIAL_TRAINER and hasattr( + _pycvc, "nav_material_trainer_set_bicycle" +) def _takes_multi_start_args() -> bool: @@ -155,6 +159,12 @@ class MaterialTrainer: Construct from a ``.cvcnm`` checkpoint (initial / Stage-1 warm-start weights); drive with :meth:`step` over batches; :meth:`save` the trained weights. + ``bicycle`` (a :class:`grl_snam.material_bike.BicycleParams`, or any object with its + fields) takes the loss through the deployed kinematic bicycle instead of the point-mass + surrogate (libcvc P8b). The limits are in the batch's length units. The bicycle starts + each agent at speed ``|v0|`` heading along ``v0``, and runs on the CPU even when + ``use_cuda`` is set. Needs :data:`HAS_MATERIAL_TRAINER_BICYCLE`. + L_multi start sampling (GRL-SNAM #113, libcvc #596): each of ``ms_count`` starts moves every agent ``frac ~ U(frac_range)`` of its clearance toward its nearest obstacle. ``seed`` fixes the draw, and only its low 32 bits count, as with @@ -199,6 +209,7 @@ def __init__( frac_range: tuple[float, float] = (0.9, 0.9), seed: int = 0, semi_implicit: bool = False, + bicycle=None, ): if not HAS_MATERIAL_TRAINER: raise RuntimeError( @@ -256,8 +267,35 @@ def __init__( int(seed) & 0xFFFFFFFF, int(bool(semi_implicit)), ] + if bicycle is not None and not HAS_MATERIAL_TRAINER_BICYCLE: + raise RuntimeError( + "bicycle= needs a pycvc with nav_material_trainer_set_bicycle (libcvc P8b); " + "this one trains on the point mass only." + ) self._h = _pycvc.nav_material_trainer_create(*args) self._requested_cuda = bool(use_cuda) + if bicycle is not None: + # Any object with the BicycleParams fields (grl_snam.material_bike, which + # needs torch, so it is not imported here). + _pycvc.nav_material_trainer_set_bicycle( + self._h, + 1, + float(bicycle.L), + float(bicycle.delta_max), + float(bicycle.a_max), + float(bicycle.a_lat_max), + float(bicycle.k_steer), + float(bicycle.vmax), + int(bool(bicycle.allow_reverse)), + ) + + @property + def integrator(self) -> str: + """``"bicycle"`` when the loss is taken through the kinematic bicycle (``bicycle=``), + else ``"surrogate"``, the point mass.""" + if not HAS_MATERIAL_TRAINER_BICYCLE: + return "surrogate" + return str(_pycvc.nav_material_trainer_integrator(self._h)) @property def cuda_active(self) -> bool: diff --git a/grl_snam/nav_native.py b/grl_snam/nav_native.py index 24ffc01..6a6e2ac 100644 --- a/grl_snam/nav_native.py +++ b/grl_snam/nav_native.py @@ -1127,3 +1127,195 @@ def integrate_surrogate_material( float(k_sharp), int(num_threads), ) + + +# The material rollout on the kinematic bicycle (libcvc P8b): the C++ twin of +# grl_snam.material_bike.integrate_bicycle_material, and its VJP. +HAS_BICYCLE_MATERIAL_ROLLOUT = AVAILABLE and hasattr(_pycvc, "nav_integrate_bicycle_material") + + +def _bike_args( + o0, + th0, + sp0, + goal, + C, + R, + mask, + alphas, + beta, + gamma, + lam_soft, + lam_hard, + rollout_patch, + rr, + d_hat, + dt, + H, + vehicle, + margin_factor, + mass, + d_hat_sdf, + k_sharp, +): + B = np.asarray(o0).reshape(-1, 2).shape[0] + f32 = lambda x, *shape: np.ascontiguousarray(x, np.float32).reshape(*shape) # noqa: E731 + return ( + f32(o0, -1, 2), + f32(th0, -1), + f32(sp0, -1), + f32(goal, -1, 2), + f32(C, B, -1, 2), + f32(R, B, -1), + np.ascontiguousarray(mask, np.uint8).reshape(B, -1), + f32(alphas, B, -1), + f32(beta, -1), + f32(gamma, -1), + f32(lam_soft, -1), + f32(lam_hard, -1), + np.ascontiguousarray(rollout_patch, np.float32), + f32(rr, -1), + f32(d_hat, -1), + f32(dt, -1), + np.ascontiguousarray(H, np.int32).reshape(-1), + float(margin_factor), + float(mass), + float(d_hat_sdf), + float(k_sharp), + float(vehicle.L), + float(vehicle.delta_max), + float(vehicle.a_max), + float(vehicle.a_lat_max), + float(vehicle.k_steer), + float(vehicle.vmax), + int(bool(vehicle.allow_reverse)), + ) + + +def integrate_bicycle_material( + o0, + th0, + sp0, + goal, + C, + R, + mask, + alphas, + beta, + gamma, + lam_soft, + lam_hard, + rollout_patch, + rr, + d_hat, + dt, + H, + vehicle, + *, + margin_factor=0.5, + mass=1.0, + d_hat_sdf=3.0, + k_sharp=5.0, + num_threads=0, +): + """cvc::nav::integrate_bicycle_material. ``vehicle`` is a + :class:`grl_snam.material_bike.BicycleParams`. Returns (oT (B,2), thT, spT, + min_clear, cum_risk, hard_count, arc_length (B,)).""" + return _pycvc.nav_integrate_bicycle_material( + *_bike_args( + o0, + th0, + sp0, + goal, + C, + R, + mask, + alphas, + beta, + gamma, + lam_soft, + lam_hard, + rollout_patch, + rr, + d_hat, + dt, + H, + vehicle, + margin_factor, + mass, + d_hat_sdf, + k_sharp, + ), + int(num_threads), + ) + + +def integrate_bicycle_material_vjp( + o0, + th0, + sp0, + goal, + C, + R, + mask, + alphas, + beta, + gamma, + lam_soft, + lam_hard, + rollout_patch, + rr, + d_hat, + dt, + H, + vehicle, + g_oT, + g_thT, + g_spT, + g_min_clear, + g_cum_risk, + g_arc_length, + *, + margin_factor=0.5, + mass=1.0, + d_hat_sdf=3.0, + k_sharp=5.0, + num_threads=0, +): + """The VJP of :func:`integrate_bicycle_material`: upstream grads on (oT, thT, spT, + min_clear, cum_risk, arc_length) -> (g_alphas (B,N), g_beta, g_gamma, g_lam_soft, + g_lam_hard (B,)).""" + f32 = lambda x, *shape: np.ascontiguousarray(x, np.float32).reshape(*shape) # noqa: E731 + return _pycvc.nav_integrate_bicycle_material_vjp( + *_bike_args( + o0, + th0, + sp0, + goal, + C, + R, + mask, + alphas, + beta, + gamma, + lam_soft, + lam_hard, + rollout_patch, + rr, + d_hat, + dt, + H, + vehicle, + margin_factor, + mass, + d_hat_sdf, + k_sharp, + ), + f32(g_oT, -1, 2), + f32(g_thT, -1), + f32(g_spT, -1), + f32(g_min_clear, -1), + f32(g_cum_risk, -1), + f32(g_arc_length, -1), + int(num_threads), + ) diff --git a/tests/test_material_bike_parity.py b/tests/test_material_bike_parity.py new file mode 100644 index 0000000..3381b72 --- /dev/null +++ b/tests/test_material_bike_parity.py @@ -0,0 +1,170 @@ +"""Parity for the material rollout on the kinematic bicycle (libcvc P8b). + +``cvc::nav::integrate_bicycle_material`` and its hand-written adjoint against the torch +twin :func:`grl_snam.material_bike.integrate_bicycle_material`, whose gradient comes from +autograd. Two independent derivations of the same controller agreeing is the check. + +Tier: FLOAT contract, as for the surrogate's parity. The forward is compared at +rtol 1e-4, and the gradient at rtol 1e-3, because the grid_sample rounding differs +by torch build. The batches sit at interior points: agents head roughly at the goal +below the speed limit, and the forces stay inside their clamps. Then no branch +flips between the two float paths. The behaviour test needs only torch. +""" + +from __future__ import annotations + +import numpy as np +import pytest + +torch = pytest.importorskip("torch") + +from grl_snam.material_bike import BicycleParams, integrate_bicycle_material # noqa: E402 + +VEHICLE = BicycleParams(L=0.3, delta_max=0.6, a_max=4.0, a_lat_max=1.5, k_steer=0.8, vmax=1.5) + + +def _batch(seed, B=6, N=3, P=13, H=(2, 3)): + rng = np.random.default_rng(seed) + o0 = rng.uniform(-0.5, 0.5, (B, 2)).astype(np.float32) + goal = rng.uniform(1.5, 2.5, (B, 2)).astype(np.float32) + th0 = ( + np.arctan2(goal[:, 1] - o0[:, 1], goal[:, 0] - o0[:, 0]) + rng.uniform(-0.2, 0.2, B) + ).astype(np.float32) + ang = rng.uniform(0, 2 * np.pi, (B, N)) + dist = rng.uniform(1.5, 1.9, (B, N)) + C = (o0[:, None, :] + dist[..., None] * np.stack([np.cos(ang), np.sin(ang)], -1)).astype( + np.float32 + ) + fy, fx = np.meshgrid(np.linspace(0, 1, P), np.linspace(0, 1, P), indexing="ij") + one = np.stack( + [ + 0.35 + 0.25 * np.sin(2.0 * fx + 1.3 * fy), + 2.0 + 1.2 * np.cos(1.7 * fx - 0.9 * fy), + 0.20 * np.cos(2.1 * fx), + 0.20 * np.sin(1.5 * fy), + 0.15 * np.sin(1.1 * fx + fy), + 0.15 * np.cos(0.8 * fx - fy), + ] + ).astype(np.float32) + return { + "o0": o0, + "th0": th0, + "sp0": rng.uniform(0.3, 0.6, B).astype(np.float32), + "goal": goal, + "C": C, + "R": rng.uniform(0.5, 0.7, (B, N)).astype(np.float32), + "mask": np.ones((B, N), np.uint8), + "alphas": rng.uniform(0.03, 0.08, (B, N)).astype(np.float32), + "beta": rng.uniform(0.8, 1.2, B).astype(np.float32), + "gamma": rng.uniform(0.2, 0.5, B).astype(np.float32), + "lam_soft": rng.uniform(0.7, 1.3, B).astype(np.float32), + "lam_hard": rng.uniform(0.7, 1.3, B).astype(np.float32), + "rollout_patch": np.broadcast_to(one, (B, 6, P, P)).copy(), + "rr": np.full(B, 0.5, np.float32), + "d_hat": np.full(B, 3.0, np.float32), + "dt": np.full(B, 0.1, np.float32), + "H": np.array([H[i % len(H)] for i in range(B)], np.int32), + } + + +_ORDER = ( + "o0", + "th0", + "sp0", + "goal", + "C", + "R", + "mask", + "alphas", + "beta", + "gamma", + "lam_soft", + "lam_hard", + "rollout_patch", + "rr", + "d_hat", + "dt", + "H", +) +_COEFS = ("alphas", "beta", "gamma", "lam_soft", "lam_hard") + + +def _torch_rollout(b, requires_grad=False): + t = {k: torch.from_numpy(np.asarray(b[k])) for k in _ORDER} + t["mask"] = t["mask"].to(torch.bool) + t["H"] = t["H"].to(torch.int64) + if requires_grad: + for k in _COEFS: + t[k] = t[k].clone().requires_grad_(True) + out = integrate_bicycle_material(*(t[k] for k in _ORDER), vehicle=VEHICLE) + return t, out + + +def test_torch_twin_drives_to_the_goal(): + b = _batch(21, B=8, N=1, H=(80,)) + b["mask"][:] = 0 + b["lam_soft"][:] = 0.0 + b["lam_hard"][:] = 0.0 + b["th0"] = b["th0"] + np.where(np.arange(8) % 2, 1.2, -1.2).astype(np.float32) + _, (oT, _thT, spT, *_rest) = _torch_rollout(b) + d0 = np.linalg.norm(b["goal"] - b["o0"], axis=1) + d1 = np.linalg.norm(b["goal"] - oT.numpy(), axis=1) + assert np.all(d1 < 0.25 * d0), f"did not close on the goal: {d0} -> {d1}" + assert np.all(spT.numpy() <= VEHICLE.vmax + 1e-6) + + +nav_native = pytest.importorskip("grl_snam.nav_native") +needs_native = pytest.mark.skipif( + not getattr(nav_native, "HAS_BICYCLE_MATERIAL_ROLLOUT", False), + reason="pycvc lacks nav_integrate_bicycle_material", +) + + +@needs_native +@pytest.mark.parametrize("seed,H", [(7, (2, 3)), (11, (4, 5))]) +def test_forward_matches_the_cpp_rollout(seed, H): + b = _batch(seed, H=H) + _, ref = _torch_rollout(b) + got = nav_native.integrate_bicycle_material(*(b[k] for k in _ORDER), VEHICLE) + names = ("oT", "thT", "spT", "min_clear", "cum_risk", "hard_count", "arc_length") + for name, r, g in zip(names, ref, got): + np.testing.assert_allclose(g, r.detach().numpy(), rtol=1e-4, atol=1e-5, err_msg=name) + + +@needs_native +@pytest.mark.parametrize("seed,H", [(7, (2, 3)), (11, (4, 5))]) +def test_gradient_matches_the_cpp_adjoint(seed, H): + b = _batch(seed, H=H) + B = b["o0"].shape[0] + rng = np.random.default_rng(99) + seeds = { + "oT": rng.uniform(-1, 1, (B, 2)).astype(np.float32), + "thT": rng.uniform(-1, 1, B).astype(np.float32), + "spT": rng.uniform(-1, 1, B).astype(np.float32), + "min_clear": rng.uniform(-1, 1, B).astype(np.float32), + "cum_risk": rng.uniform(-1, 1, B).astype(np.float32), + "arc_length": rng.uniform(-1, 1, B).astype(np.float32), + } + t, (oT, thT, spT, mc, cr, _hc, arc) = _torch_rollout(b, requires_grad=True) + loss = ( + (torch.from_numpy(seeds["oT"]) * oT).sum() + + (torch.from_numpy(seeds["thT"]) * thT).sum() + + (torch.from_numpy(seeds["spT"]) * spT).sum() + + (torch.from_numpy(seeds["min_clear"]) * mc).sum() + + (torch.from_numpy(seeds["cum_risk"]) * cr).sum() + + (torch.from_numpy(seeds["arc_length"]) * arc).sum() + ) + loss.backward() + got = nav_native.integrate_bicycle_material_vjp( + *(b[k] for k in _ORDER), + VEHICLE, + seeds["oT"], + seeds["thT"], + seeds["spT"], + seeds["min_clear"], + seeds["cum_risk"], + seeds["arc_length"], + ) + for name, g in zip(_COEFS, got): + ref = t[name].grad.numpy() + np.testing.assert_allclose(g, ref, rtol=1e-3, atol=1e-4, err_msg=name) diff --git a/tests/test_material_train_native.py b/tests/test_material_train_native.py index 18f33bb..488bfad 100644 --- a/tests/test_material_train_native.py +++ b/tests/test_material_train_native.py @@ -342,3 +342,29 @@ def native(semi): assert ref > 1e-3, "fixture must actually score near-obstacle starts" assert abs(semi - ref) <= 1e-5 * ref, f"semi-implicit native {semi!r} vs torch {ref!r}" assert abs(explicit - ref) > 1e-3 * ref, "the explicit default should NOT match" + + +@pytest.mark.skipif( + not mtn.HAS_MATERIAL_TRAINER_BICYCLE, reason="pycvc lacks nav_material_trainer_set_bicycle" +) +def test_bicycle_integrator_trains(tmp_path): + """bicycle= takes the loss through the kinematic bicycle (libcvc P8b), and it descends.""" + from grl_snam.material_bike import BicycleParams + + ckpt = tmp_path / "init.cvcnm" + _write_random_cvcnm(ckpt, patch_size=16, seed=5) + batch = _synthetic_batch(seed=13) + point_mass = mtn.MaterialTrainer(ckpt) + assert point_mass.integrator == "surrogate" + # Limits sized for the synthetic batch (positions within a few units, dt = 0.1). + vehicle = BicycleParams(L=0.3, a_max=2.0, a_lat_max=1.5, vmax=1.5) + bike = mtn.MaterialTrainer(ckpt, bicycle=vehicle) + assert bike.integrator == "bicycle" + assert bike.loss(batch) != point_mass.loss(batch) + L0 = bike.step(batch, lr=1e-3) + L = L0 + for _ in range(20): + L = bike.step(batch, lr=1e-3) + assert np.isfinite(L) and L < L0, f"bicycle training did not descend ({L0} -> {L})" + with pytest.raises(Exception): + mtn.MaterialTrainer(ckpt, bicycle=BicycleParams(L=0.0)) # a zero wheelbase is refused