Perf/solver speed

src/fast_lto/README.md

@@ -94,11 +94,12 @@ smoothness tricks, warm starts, and terminal regions:
 
 ### Solve (CasADi + IPOPT)
 
-CasADi builds the NLP and IPOPT solves it. A warm start — a previous solution,
-or a margin ladder when the centreline guess is infeasible — usually brings a
-full four-wheel lap down from several minutes cold to under one. The written
-artifact is a solution JSON with states, inputs, geometry, and the run config
-that produced them.
+CasADi builds the NLP and IPOPT solves it. A full four-wheel lap at
+`ds = 0.5` m takes on the order of ten seconds when the centreline guess is
+feasible. On tight corridors a warm start — a previous solution, or a margin
+ladder — is what makes it converge, and the solve can take a minute or two.
+The written artifact is a solution JSON with states, inputs, geometry, and the
+run config that produced them.
 
 ![IPOPT objective and lap-time convergence](../../docs/pipeline_ocp_convergence.png)
 

src/fast_lto/__init__.py

@@ -2,7 +2,13 @@
 Fast-LTO: minimum-time trajectory optimisation for Formula Student Driverless.
 """
 
-from fast_lto.pipeline import (
+import os
+
+# MUMPS calls OpenBLAS on small dense blocks; extra threads only add overhead.
+# Must be set before CasADi loads its BLAS. An explicit user value wins.
+os.environ.setdefault("OPENBLAS_NUM_THREADS", "1")
+
+from fast_lto.pipeline import (  # noqa: E402
     PipelineConfig,
     StepName,
     run_pipeline,

src/fast_lto/optimization/README.md

@@ -22,6 +22,20 @@ encoded by the grid) and the input `U` at every node.
 States and inputs are mapped to roughly `[-1, 1]` from their physical bounds;
 IPOPT's own scaling is off. Bounds must be finite.
 
+## Building the NLP
+
+One interval — integrator step, model and corridor constraints, and `dt` — is
+written once as a CasADi function and expanded to SX. `build_ocp` applies it
+to all nodes in a single `map` call, with a second map for brake-zone nodes.
+The NLP graph therefore holds one call per vehicle model instead of `N` copies
+of it, which keeps the build under a second and makes derivatives cheap to
+evaluate. Model code must stay expandable: no MX-only operations such as a
+symbolic `ca.solve`.
+
+Solver settings: MUMPS with QAMD ordering and its own scaling off (about 30%
+less time in the linear solves), and OpenBLAS limited to one thread, set in
+`fast_lto/__init__.py` unless `OPENBLAS_NUM_THREADS` is already defined.
+
 ## Smoothness
 
 Hard `max` operations that appear in the physics — flooring `ṡ` in a
@@ -36,7 +50,7 @@ would map both sides onto the same one).
 
 ## The terminal region
 
-- **Final-node constraints.** The node loop applies model constraints for
+- **Final-node constraints.** The stage function applies model constraints for
   `i < N-1` only. With `terminal_speed`, those constraints are reapplied at
   `X[N-1]` (needed when friction limits depend on persistent state, e.g.
   four-wheel tyre forces). Trackdrive omits the repeat: the closed-loop

src/fast_lto/optimization/global_ocp.py

@@ -332,6 +332,70 @@ def _constraint_eval_points(
     return points
 
 
+def _build_stage_function(
+    model: VehicleModel,
+    f_space: Callable,
+    eval_at_point: Callable,
+    integrator: SpaceIntegrator,
+    ds: float,
+    use_normalization: bool,
+) -> ca.Function:
+    """One interval as an SX function ``(x, u, kappa, kappa_half, kappa_next, w_left, w_right)``.
+
+    Returns ``(x_next, g, dt)`` with ``g <= 0`` holding the model and corridor
+    inequalities at node ``i``.
+    """
+    x = ca.MX.sym("x", model.nx_reduced)
+    u = ca.MX.sym("u", model.nu)
+    kappa, kappa_half, kappa_next, w_left, w_right = (
+        ca.MX.sym(name) for name in ("kappa", "kappa_half", "kappa_next", "w_left", "w_right")
+    )
+    s_dot_floor, s_dot_smooth_eps = _s_dot_guard_params(model)
+
+    x_next = integrator.step(f_space, x, u, kappa, ds, kappa_half=kappa_half, kappa_next=kappa_next)
+
+    g: list[ca.MX] = []
+    for x_c, kappa_c in _constraint_eval_points(
+        integrator, f_space, x, u, kappa, ds, kappa_half=kappa_half, kappa_next=kappa_next
+    ):
+        if use_normalization:
+            g += model.get_constraints_normalized(x_c, u, kappa_c)
+        else:
+            full_state, _ = eval_at_point(x_c, u, kappa_c)
+            g += model.get_constraints(full_state, u, kappa_c)
+
+    if use_normalization:
+        x_scale, x_shift = model.get_reduced_state_scaling()
+        if x_scale is None or x_shift is None:
+            raise RuntimeError("Normalization scales not defined.")
+        d_phys = x[0] * x_scale[0] + x_shift[0]
+        psi_phys = x[1] * x_scale[1] + x_shift[1]
+        lb_d = (-w_right - x_shift[0]) / x_scale[0]
+        ub_d = (w_left - x_shift[0]) / x_scale[0]
+    else:
+        d_phys, psi_phys = x[0], x[1]
+        lb_d, ub_d = -w_right, w_left
+    if model.get_corner_offsets():
+        g += model.get_corner_constraints(d_phys, psi_phys, kappa, w_left, w_right)
+    else:
+        g += [lb_d - x[0], x[0] - ub_d]
+
+    dt = integrator.time_step(
+        f_space,
+        eval_at_point,
+        x,
+        u,
+        kappa,
+        ds,
+        kappa_half=kappa_half,
+        kappa_next=kappa_next,
+        eps=s_dot_floor,
+        smooth_eps=s_dot_smooth_eps,
+    )
+    inputs = [x, u, kappa, kappa_half, kappa_next, w_left, w_right]
+    return ca.Function("stage", inputs, [x_next, ca.vertcat(*g), dt]).expand()
+
+
 def build_ocp(
     track: Dict,
     model: VehicleModel,
@@ -415,7 +479,6 @@ def build_ocp(
         f_space, eval_at_point = build_space_dynamics_normalized(model)
     else:
         f_space, eval_at_point = build_space_dynamics(model)
-    s_dot_floor, s_dot_smooth_eps = _s_dot_guard_params(model)
 
     brake_zone: _BrakeZone | None = None
     if mode == "autox" and D_safe_braking is not None:
@@ -426,12 +489,6 @@ def _node_model(i: int) -> Tuple[VehicleModel, Callable, Callable]:
             return brake_zone.model, brake_zone.f_space, brake_zone.eval_at_point
         return model, f_space, eval_at_point
 
-    total_time = 0
-    timed_time = 0
-    pure_timed_time = 0
-    # Per node, so a gate-to-gate time can be measured after the solve.
-    cumulative_time = [total_time]
-
     use_corner_constraints = len(model.get_corner_offsets()) > 0
     if use_normalization:
         x_scale, x_shift = model.get_reduced_state_scaling()
@@ -442,86 +499,48 @@ def _node_model(i: int) -> Tuple[VehicleModel, Callable, Callable]:
         psi_scale = x_scale[1]
         psi_shift = x_shift[1]
 
-    for i in range(N - 1):
-        x_i = X[i, :].T
-        u_i = U[i, :].T
-        kappa_i = kappa_param[i]
-        kappa_half_i = kappa_half_param[i]
-        kappa_next_i = kappa_param[i + 1]
-        model_i, f_space_i, eval_at_point_i = _node_model(i)
-
-        x_next = integrator.step(
-            f_space_i,
-            x_i,
-            u_i,
-            kappa_i,
-            ds,
-            kappa_half=kappa_half_i,
-            kappa_next=kappa_next_i,
-        )
-        opti.subject_to(X[i + 1, :].T == x_next)
-
-        for x_c, kappa_c in _constraint_eval_points(
+    # One mapped call per vehicle model; brake-zone nodes get their own.
+    stage = _build_stage_function(model, f_space, eval_at_point, integrator, ds, use_normalization)
+    stage_groups: list[tuple[np.ndarray, ca.Function]] = [(np.ones(N - 1, dtype=bool), stage)]
+    if brake_zone is not None:
+        brake_stage = _build_stage_function(
+            brake_zone.model,
+            brake_zone.f_space,
+            brake_zone.eval_at_point,
             integrator,
-            f_space_i,
-            x_i,
-            u_i,
-            kappa_i,
             ds,
-            kappa_half=kappa_half_i,
-            kappa_next=kappa_next_i,
-        ):
-            if use_normalization:
-                g_list = model_i.get_constraints_normalized(x_c, u_i, kappa_c)
-            else:
-                full_state, _ = eval_at_point_i(x_c, u_i, kappa_c)
-                g_list = model_i.get_constraints(full_state, u_i, kappa_c)
-            for g in g_list:
-                opti.subject_to(g <= 0)
-
-        if use_corner_constraints:
-            if use_normalization:
-                d_phys_i = x_i[0] * d_scale + d_shift
-                psi_phys_i = x_i[1] * psi_scale + psi_shift
-            else:
-                d_phys_i = x_i[0]
-                psi_phys_i = x_i[1]
-            for g in model.get_corner_constraints(
-                d_phys_i,
-                psi_phys_i,
-                kappa_i,
-                w_left_param[i],
-                w_right_param[i],
-            ):
-                opti.subject_to(g <= 0)
-        elif use_normalization:
-            ub_d_norm = (w_left_param[i] - d_shift) / d_scale
-            lb_d_norm = (-w_right_param[i] - d_shift) / d_scale
-            opti.subject_to(lb_d_norm <= x_i[0])
-            opti.subject_to(x_i[0] <= ub_d_norm)
-        else:
-            opti.subject_to(-w_right_param[i] <= x_i[0])
-            opti.subject_to(x_i[0] <= w_left_param[i])
-
-        dt_i = integrator.time_step(
-            f_space_i,
-            eval_at_point_i,
-            x_i,
-            u_i,
-            kappa_i,
-            ds,
-            kappa_half=kappa_half_i,
-            kappa_next=kappa_next_i,
-            eps=s_dot_floor,
-            smooth_eps=s_dot_smooth_eps,
+            use_normalization,
+        )
+        in_brake_zone = brake_zone.mask[: N - 1]
+        stage_groups = [(~in_brake_zone, stage), (in_brake_zone, brake_stage)]
+
+    dt = ca.MX(N - 1, 1)
+    for nodes, stage_fn in stage_groups:
+        idx = np.flatnonzero(nodes).tolist()
+        if not idx:
+            continue
+        nxt = [i + 1 for i in idx]
+        x_next, g, dt_nodes = stage_fn.map(len(idx))(
+            X[idx, :].T,
+            U[idx, :].T,
+            kappa_param[idx].T,
+            kappa_half_param[idx].T,
+            kappa_param[nxt].T,
+            w_left_param[idx].T,
+            w_right_param[idx].T,
         )
-        total_time += dt_i
-        cumulative_time.append(total_time)
-        if time_weights is not None:
-            w_i = float(time_weights[i])
-            timed_time += w_i * dt_i
-            if w_i >= 1.0 - 1e-9:
-                pure_timed_time += dt_i
+        opti.subject_to(X[nxt, :].T == x_next)
+        opti.subject_to(ca.vec(g) <= 0)
+        dt[idx] = dt_nodes.T
+
+    total_time = ca.sum1(dt)
+    # Per node, so a gate-to-gate time can be measured after the solve.
+    cumulative_time = ca.vertcat(0, ca.cumsum(dt))
+    timed_time = 0
+    pure_timed_time = 0
+    if time_weights is not None:
+        timed_time = ca.dot(ca.DM(time_weights), dt)
+        pure_timed_time = ca.dot(ca.DM((time_weights >= 1.0 - 1e-9).astype(float)), dt)
 
     if use_corner_constraints:
         x_last = X[N - 1, :].T
@@ -625,6 +644,8 @@ def _node_model(i: int) -> Tuple[VehicleModel, Callable, Callable]:
             "ipopt.sb": "yes",
             "ipopt.nlp_scaling_method": "none",  # IPOPT internal scaling deactivated
             "ipopt.max_cpu_time": 600.0,  # fail fast rather than hang
+            "ipopt.mumps_pivot_order": 6,  # QAMD
+            "ipopt.mumps_scaling": 0,
         },
         {},
     )
@@ -644,7 +665,7 @@ def _node_model(i: int) -> Tuple[VehicleModel, Callable, Callable]:
         total_time,
         objective_time,
         pure_timed_time,
-        ca.vertcat(*cumulative_time),
+        cumulative_time,
     )
 
 

src/fast_lto/vehicle_models/README.md

@@ -37,7 +37,9 @@ Optionally:
   optional friction ellipse.
 - **`four_wheel`** — per-wheel Pacejka, aero, load transfer, torque vectoring;
   wheel forces and steering are states; inputs are their rates (`dFxmax`,
-  `ddeltamax`). Originally developed by
+  `ddeltamax`). Quasi-static loads come from a 4x4 linear system that is
+  block lower-triangular, so it is solved in closed form (front pair, then
+  rear pair). Originally developed by
   [Tanmay Ganguli](https://github.com/TanmayGanguli09). This is the model AMZ
   used for most of the 2026 season; `point_mass` and `dynamic_bicycle` were
   mostly development aids.

src/fast_lto/vehicle_models/four_wheel.py

@@ -265,33 +265,30 @@ def _compute_vertical_loads(
         c_fl = h1 * sd * f_fl
         c_fr = h1 * sd * f_fr
 
-        A = ca.MX(4, 4)
-        # Row 1 (FL)
-        A[0, 0] = 1 + f_fl * (h1 * sd - h2 * cd)
-        A[0, 1] = f_fr * (h1 * sd - h2 * cd)
-        # Row 2 (FR)
-        A[1, 0] = f_fl * (h1 * sd + h2 * cd)
-        A[1, 1] = 1 + f_fr * (h1 * sd + h2 * cd)
-        # Row 3 (RR)
-        A[2, 0] = -c_fl
-        A[2, 1] = -c_fr
-        A[2, 2] = 1 + h2 * f_rr
-        A[2, 3] = h2 * f_rl
-        # Row 4 (RL)
-        A[3, 0] = -c_fl
-        A[3, 1] = -c_fr
-        A[3, 2] = -h2 * f_rr
-        A[3, 3] = 1 - h2 * f_rl
-
-        b_vec = ca.vertcat(b1, b2, b3, b4)
-        Fz_vec = ca.solve(A, b_vec)
+        # A is block lower-triangular: solve the front 2x2, then the rear.
+        a11 = 1 + f_fl * (h1 * sd - h2 * cd)
+        a12 = f_fr * (h1 * sd - h2 * cd)
+        a21 = f_fl * (h1 * sd + h2 * cd)
+        a22 = 1 + f_fr * (h1 * sd + h2 * cd)
+        det_front = a11 * a22 - a12 * a21
+        Fz_fl = (a22 * b1 - a12 * b2) / det_front
+        Fz_fr = (a11 * b2 - a21 * b1) / det_front
+
+        coupling = c_fl * Fz_fl + c_fr * Fz_fr
+        a33 = 1 + h2 * f_rr
+        a34 = h2 * f_rl
+        a43 = -h2 * f_rr
+        a44 = 1 - h2 * f_rl
+        det_rear = a33 * a44 - a34 * a43
+        Fz_rr = (a44 * (b3 + coupling) - a34 * (b4 + coupling)) / det_rear
+        Fz_rl = (a33 * (b4 + coupling) - a43 * (b3 + coupling)) / det_rear
 
         eps_fz = float(p.get("smoothmax_eps", 1e-3))
-        Fz_min = ca.MX(10.0)
-        Fz_fl_s = smoothmax(Fz_vec[0], Fz_min, eps_fz)
-        Fz_fr_s = smoothmax(Fz_vec[1], Fz_min, eps_fz)
-        Fz_rr_s = smoothmax(Fz_vec[2], Fz_min, eps_fz)
-        Fz_rl_s = smoothmax(Fz_vec[3], Fz_min, eps_fz)
+        Fz_min = 10.0
+        Fz_fl_s = smoothmax(Fz_fl, Fz_min, eps_fz)
+        Fz_fr_s = smoothmax(Fz_fr, Fz_min, eps_fz)
+        Fz_rr_s = smoothmax(Fz_rr, Fz_min, eps_fz)
+        Fz_rl_s = smoothmax(Fz_rl, Fz_min, eps_fz)
 
         return Fz_fl_s, Fz_fr_s, Fz_rr_s, Fz_rl_s