Closed-loop MPC for multi-agent tracking sheaf problems

docs/make.jl

@@ -66,7 +66,9 @@ makedocs(
         "generated/control/controlled_planar_quadrotor.md",
         "generated/control/controlled_mass_spring_damper_chain.md",
         "control/single_integrator_target_tracking.md",
-        "generated/control/multi_quadrotor_target_tracking.md"
+        "generated/control/multi_quadrotor_target_tracking.md",
+        "generated/control/mpc_target_tracking.md"
+
         ],
       "Asynchronous Diffusion"=>Any[
         "generated/asynch/convergence_vs_delay.md",

src/ControlSheaves/QuadraticCosts.jl

@@ -24,73 +24,53 @@ module QuadraticCosts
 
 using SparseArrays
 using LinearAlgebra
+using CliqueTrees.Multifrontal
 using ..MultiAgentTracking: TrackingProblem, agent_vertex, extract_state_trajectories,
     extract_target_trajectories, build_time_expanded_tracking_sheaf
-using ....NetworkSheaves: sheaf_laplacian_matrix_direct
+using ....NetworkSheaves: sheaf_laplacian_matrix_direct, ldlt_pinv_solve
 
 export build_control_cost_matrix, solve_quadratic_on_basis
 
 """
-    build_control_cost_matrix(prob::TrackingProblem, λ::Number) -> SparseMatrixCSC{Float64,Int}
+    build_control_cost_matrix(prob::TrackingProblem, stalks, λ::Number) -> SparseMatrixCSC{Float64,Int}
 
 Construct a global quadratic cost matrix `Q` that penalises the control
 components of each agent with a uniform weight `λ`.  Internally this builds a
 block‑diagonal matrix where each control block is `λ * I(nu)`.
 """
-function build_control_cost_matrix(prob::TrackingProblem, λ::Number=1.0)
-    # Create a simple cost function that returns λ * I(nu) for each agent/time
-    cost_func = (agent_idx::Int, t_step::Int) -> begin
-        nu = size(prob.Bd[agent_idx], 2)
-        return λ * I(nu)
-    end
-    return build_control_cost_matrix(prob, cost_func)
+function build_control_cost_matrix(prob::TrackingProblem, stalks::AbstractVector{<:Integer}, λ::Number=1.0)
+    return build_control_cost_matrix(prob, stalks,
+        (agent_idx::Int, t_step::Int) -> λ * I(size(prob.Bd[agent_idx], 2)))
 end
 
 """
-    build_control_cost_matrix(prob::TrackingProblem, cost_func::Function) -> SparseMatrixCSC{Float64,Int}
+    build_control_cost_matrix(prob::TrackingProblem, stalks, cost_func::Function) -> SparseMatrixCSC{Float64,Int}
 
 Construct a global quadratic cost matrix `Q` using a user‑provided function
 `cost_func(agent_idx, t_step) -> Q_local`.
 
 * `prob` – the `TrackingProblem` describing the agents, dynamics, etc.
+* `stalks` – the sheaf's `vertex_stalks`, giving the DOF layout `Q` is built for.
 * `cost_func` – a function `(agent_idx, t_step) -> Q_local` where
   `Q_local` is a `nu × nu` positive semidefinite matrix for that agent at that timestep.
 
-The returned matrix has size `total_dim × total_dim` where `total_dim` is the
-length of the stacked vector of all stalk variables (states + controls) in the
-time‑expanded sheaf.
+The returned matrix has size `total_dim × total_dim` where `total_dim = sum(stalks)`.
+The control block is read from `stalks` (the trailing `nu` DOFs of each agent
+vertex), so an agent vertex that carries state only — e.g. the `t = 0` vertex of
+the MPC-window sheaf — has no control DOFs and is skipped automatically.
 """
-function build_control_cost_matrix(prob::TrackingProblem, cost_func::Function)
-    # Compute per‑vertex stalk sizes (shared with the scalar version)
-    stalk_sizes = Vector{Int}(undef, (prob.k + 1) * (prob.n_agents + prob.n_targets))
-    for t in 0:prob.k, i in 1:prob.n_agents
-        nx = size(prob.Ad[i], 1)
-        nu = size(prob.Bd[i], 2)
-        stalk_sizes[t * (prob.n_agents + prob.n_targets) + i] = nx + nu
-    end
-    for t in 0:prob.k, j in 1:prob.n_targets
-        nx = size(prob.target_Ad[j], 1)
-        nu = size(prob.target_Bd[j], 2)
-        stalk_sizes[t * (prob.n_agents + prob.n_targets) + prob.n_agents + j] = nx + nu
-    end
-    total_dim = sum(stalk_sizes)
-    Q = spzeros(Float64, total_dim, total_dim)
-
-    function control_range(agent_idx::Int, t_step::Int)
-        v = agent_vertex(prob, agent_idx, t_step)
-        nx = size(prob.Ad[agent_idx], 1)
-        nu = size(prob.Bd[agent_idx], 2)
-        offset = sum(stalk_sizes[1:(v-1)])
-        return (offset + nx + 1) : (offset + nx + nu)
-    end
-
-    for (i, Bd_i) in enumerate(prob.Bd)
-        nu = size(Bd_i, 2)
+function build_control_cost_matrix(prob::TrackingProblem, stalks::AbstractVector{<:Integer}, cost_func::Function)
+    offsets = [0; cumsum(stalks)]
+    Q = spzeros(Float64, last(offsets), last(offsets))
+    for i in 1:prob.n_agents
+        nx = size(prob.Ad[i], 1); nu = size(prob.Bd[i], 2)
         for t_step in 0:prob.k
-            idxs = control_range(i, t_step)
+            v = agent_vertex(prob, i, t_step)
+            stalks[v] == nx + nu || continue   # state-only vertex: no control to penalise
+            rng = (offsets[v] + nx + 1):(offsets[v] + nx + nu)
             Q_local = cost_func(i, t_step)
             @assert size(Q_local) == (nu, nu) "cost_func must return a $(nu)×$(nu) matrix for agent $i at time $t_step"
-            Q[idxs, idxs] = Q_local
+            Q[rng, rng] = Q_local
         end
     end
     return Q
@@ -121,11 +101,13 @@ function solve_quadratic_on_basis(
         return copy(point)
     end
     N = basis
-    QN = N' * Q * N
-    rhs = - N' * Q * point
-    # Solve in a least-squares / minimum-norm sense so rank-deficient
-    # reduced Hessians from semidefinite control costs do not throw.
-    α_opt = pinv(QN) * rhs
+    QN = Matrix(N' * Q * N)
+    rhs = -(N' * Q * point)
+    # The reduced Hessian N'QN is symmetric PSD and can be rank-deficient when the
+    # control cost is only semidefinite.  Solve via the LDLt pseudoinverse
+    # (min-norm, won't throw on null pivots) rather than a dense SVD.
+    Fq = ldlt!(DenseLDLtPivoted(QN), RowMaximum(); check=false)
+    α_opt = ldlt_pinv_solve(Fq, rhs)
     return point + N * α_opt
 end
 

src/ControlSheaves/TrackingProblems.jl

@@ -86,8 +86,22 @@ target_vertex(prob::TrackingProblem, j::Int, t::Int) =
 # Sheaf construction
 # ---------------------------------------------------------------------------
 
+# Restriction maps for the agent dynamics edge agent(i,t) ↔ agent(i,t+1).
+# Default:            source [A B], destination [I 0]  ⟹  x_{t+1} = A x_t + B u_t.
+# state_only_initial: controls reindexed so u_{t+1} produces x_{t+1} — source
+#   [A 0] (or just [A] at the state-only t=0 vertex), destination [I −B].
+function _agent_dynamics_maps(Ad, Bd, t::Int, state_only_initial::Bool)
+    nx, nu = size(Ad, 1), size(Bd, 2)
+    if state_only_initial
+        src_map = t == 0 ? Matrix(Ad) : hcat(Matrix(Ad), zeros(nx, nu))
+        return src_map, hcat(Matrix{Float64}(I, nx, nx), -Matrix(Bd))
+    else
+        return hcat(Matrix(Ad), Matrix(Bd)), hcat(Matrix{Float64}(I, nx, nx), zeros(nx, nu))
+    end
+end
+
 """
-    build_time_expanded_tracking_sheaf(prob::TrackingProblem)
+    build_time_expanded_tracking_sheaf(prob::TrackingProblem; state_only_initial=false)
 
 Construct the time-expanded `EuclideanSheaf` from a `TrackingProblem`.
 
@@ -106,8 +120,18 @@ Four edge families are added in order:
 Restriction maps for consensus and tracking edges are pre-scaled by
 `sqrt(consensus_weight)` and `sqrt(tracking_weight)` respectively so that the
 resulting Laplacian is `weight * R' * R`.
+
+When `state_only_initial = true` the initial agent vertices (`t = 0`) carry
+**state only** (no control), and the control is reindexed so that `u_t` produces
+`x_t`: the agent dynamics edge reads `A·x_t` from the source and `x_{t+1} − B·u_{t+1}`
+from the destination, encoding `x_{t+1} = A·x_t + B·u_{t+1}`. Consensus/tracking
+restriction maps at `t = 0` are then truncated to the agent's state columns
+(exact, since those columns multiplied the now-absent control block by zero).
+This makes the initial condition a clean full-vertex pin and is the form used by
+the MPC window solver; the resulting optimal trajectory is identical to the
+default form, which keeps `(x_0, u_0)` on the first vertex.
 """
-function build_time_expanded_tracking_sheaf(prob::TrackingProblem)
+function build_time_expanded_tracking_sheaf(prob::TrackingProblem; state_only_initial::Bool=false)
     @argcheck prob.consensus_weight >= 0 "consensus_weight must be non-negative, got $(prob.consensus_weight)"
     @argcheck prob.tracking_weight  >= 0 "tracking_weight must be non-negative, got $(prob.tracking_weight)"
     @argcheck all(t -> 0 <= t <= prob.k, prob.consensus_timesteps) "all consensus_timesteps must be in 0:$(prob.k), got $(prob.consensus_timesteps)"
@@ -128,11 +152,13 @@ function build_time_expanded_tracking_sheaf(prob::TrackingProblem)
         @argcheck size(prob.target_Bd[j], 1) == size(prob.target_Ad[j], 1) "target_Bd[$j] rows must match target_Ad[$j] rows, got $(size(prob.target_Bd[j],1)) vs $(size(prob.target_Ad[j],1))"
     end
 
-    # Allocate heterogeneous stalk sizes for every vertex (agents + targets)
+    # Allocate heterogeneous stalk sizes for every vertex (agents + targets).
+    # With state_only_initial the t=0 agent vertices hold state only (no control).
     n_per_t = prob.n_agents + prob.n_targets
     stalk_sizes = Vector{Int}(undef, (prob.k + 1) * n_per_t)
     for t in 0:prob.k, i in 1:prob.n_agents
-        stalk_sizes[t * n_per_t + i] = size(prob.Ad[i], 1) + size(prob.Bd[i], 2)
+        nx_i = size(prob.Ad[i], 1); nu_i = size(prob.Bd[i], 2)
+        stalk_sizes[t * n_per_t + i] = (state_only_initial && t == 0) ? nx_i : nx_i + nu_i
     end
     for t in 0:prob.k, j in 1:prob.n_targets
         stalk_sizes[t * n_per_t + prob.n_agents + j] = size(prob.target_Ad[j], 1) + size(prob.target_Bd[j], 2)
@@ -141,14 +167,11 @@ function build_time_expanded_tracking_sheaf(prob::TrackingProblem)
 
     # Agent dynamics edges (per-agent)
     for i in 1:prob.n_agents
-        Ad_i = prob.Ad[i]; Bd_i = prob.Bd[i]
-        nx_i = size(Ad_i, 1); nu_i = size(Bd_i, 2)
-        now_map  = hcat(Matrix(Ad_i), Matrix(Bd_i))
-        next_map = hcat(Matrix{Float64}(I, nx_i, nx_i), zeros(nx_i, nu_i))
         for t in 0:prob.k-1
+            src_map, dst_map = _agent_dynamics_maps(prob.Ad[i], prob.Bd[i], t, state_only_initial)
             add_sheaf_edge!(sheaf,
                 agent_vertex(prob, i, t), agent_vertex(prob, i, t + 1),
-                now_map, next_map)
+                src_map, dst_map)
         end
     end
 
@@ -167,18 +190,23 @@ function build_time_expanded_tracking_sheaf(prob::TrackingProblem)
         end
     end
 
+    # In window mode the t=0 agent stalk is state-only, so its restriction maps
+    # drop the (now absent) control columns; otherwise they are used as given.
+    agent_restr(R, i, t) = (state_only_initial && t == 0) ? R[:, 1:size(prob.Ad[i], 1)] : R
     cscale = sqrt(prob.consensus_weight)
     tscale = sqrt(prob.tracking_weight)
     for t in prob.consensus_timesteps, (i, j) in prob.agent_edges
         add_sheaf_edge!(sheaf,
             agent_vertex(prob, i, t), agent_vertex(prob, j, t),
-            cscale * prob.consensus_restriction, cscale * prob.consensus_restriction)
+            cscale * agent_restr(prob.consensus_restriction, i, t),
+            cscale * agent_restr(prob.consensus_restriction, j, t))
     end
     for t in prob.tracking_timesteps, te in prob.tracking_edges
         add_sheaf_edge!(sheaf,
             agent_vertex(prob, te.agent_index, t),
             target_vertex(prob, te.target_index, t),
-            tscale * te.agent_restriction, tscale * te.target_restriction)
+            tscale * agent_restr(te.agent_restriction, te.agent_index, t),
+            tscale * te.target_restriction)
     end
     return sheaf
 end

src/network_sheaves/EuclideanSheaves.jl

@@ -5,7 +5,7 @@ module EuclideanSheaves
 export EuclideanSheaf, UnorderedPair, sheaf_laplacian_matrix, sheaf_laplacian_matrix_direct,
     restricted_laplacian_blocks, sheaf_from_graph, energy_function,
     nearest_global_section, edge_stalk_dimensions, nullspace_ldlt, harmonic_extension,
-    ldlt_pseudoinverse_and_null, HarmonicExtensionLDLDiagnostics,
+    ldlt_pseudoinverse_and_null, ldiv_diag_pinv!, ldiv_diag_pinv, ldlt_pinv_solve, ldlt_pinv_solve!, HarmonicExtensionLDLDiagnostics,
     HarmonicExtensionSVDDiagnostics, harmonic_extension_ldl_diagnostics,
     harmonic_extension_svd_diagnostics, zero_sheaf, constant_sheaf, cycle_sheaf
 
@@ -544,6 +544,98 @@ end
 
 _default_ldlt_nullity_threshold(max_abs::Real) = sqrt(eps(Float64)) * max(1.0, Float64(max_abs))
 
+function ldiv_diag_pinv!(u, D, b, tol)
+    for i in eachindex(u)
+        d = D[i, i]
+        if abs(d) > tol
+            u[i] = b[i] / d
+        else
+            u[i] = zero(eltype(u))
+        end
+    end
+    return u
+end
+
+ldiv_diag_pinv(D, b, tol) = ldiv_diag_pinv!(similar(b), D, b, tol)
+
+"""    ldlt_pinv_solve(F, b; tol=nothing) -> x
+
+Apply the Moore–Penrose pseudoinverse of a symmetric positive-semidefinite
+matrix to the vector `b`, using a precomputed factorization
+``F = P^\\mathsf{T} L D L^\\mathsf{T} P`` (either `ChordalLDLt` or
+`DenseLDLtPivoted` from `CliqueTrees.Multifrontal`).
+
+Null pivots — diagonal entries of `D` with ``|D_{ii}| \\le`` `tol` — are zeroed
+in the diagonal solve, so the result is the minimum-norm solution lying in the
+range of the factorized matrix.  When `tol` is `nothing` it defaults to
+``\\sqrt{\\varepsilon} \\max(1, \\|D\\|_\\infty)``.
+
+Because `F` is reused, this is the per-application kernel for repeated solves
+against the same (possibly singular) matrix — e.g. building a harmonic-extension
+operator column by column.
+"""
+function ldlt_pinv_solve(F, b::AbstractVector; tol=nothing)
+    return ldlt_pinv_solve!(similar(b), F, b, similar(b); tol=tol)
+end
+
+"""    ldlt_pinv_solve!(out, F, b, w; tol=nothing) -> out
+
+In-place form of [`ldlt_pinv_solve`](@ref).  Writes the pseudoinverse solution
+into `out`, using `w` as scratch; both must have length `size(F.D, 1)` and be
+distinct from `b`.  Reusing `out`/`w` across repeated solves against the same
+factorization `F` allocates nothing on the caller's side, which is what makes a
+column-by-column operator build cheap.
+
+For ``F = P^\\mathsf{T} L D L^\\mathsf{T} P`` the result is the closed form
+
+```math
+x = P\\,\\bigl(L^\\mathsf{T} \\backslash\\, D^{+} (L \\backslash (P^\\mathsf{T} b))\\bigr),
+```
+
+read right-to-left: permute `b`, forward solve `L`, apply the diagonal
+pseudoinverse `D⁺` (zeroing null pivots), back solve `Lᵀ`, undo the permutation.
+The permutations are gathers through `F.P.perm` / `F.P.invp`; the triangular
+solves and `D⁺` run in place via `ldiv!` and [`ldiv_diag_pinv!`](@ref).
+"""
+function ldlt_pinv_solve!(out::AbstractVector, F, b::AbstractVector, w::AbstractVector; tol=nothing)
+    D = F.D
+    n = size(D, 1)
+    threshold = isnothing(tol) ?
+        _default_ldlt_nullity_threshold(maximum(i -> abs(D[i, i]), 1:n; init=0.0)) : tol
+    perm = F.P.perm; invp = F.P.invp
+    @inbounds for i in 1:n
+        out[i] = b[perm[i]]                # out = P' \ b  (permute rhs)
+    end
+    ldiv!(F.L, out)                        # forward solve, in place
+    ldiv_diag_pinv!(w, D, out, threshold)  # D⁺ on free directions only
+    ldiv!(F.L', w)                         # back solve, in place
+    @inbounds for i in 1:n
+        out[i] = w[invp[i]]                # out = P \ y  (undo permutation)
+    end
+    return out
+end
+
+"""    ldlt_pinv_solve(F, B::AbstractMatrix; tol=nothing) -> Matrix
+
+Apply the pseudoinverse of the factorized matrix to every column of `B`, i.e.
+return a dense matrix whose `j`-th column is `ldlt_pinv_solve(F, B[:, j])`.  This
+is the multiple-right-hand-side form used to build harmonic-extension operators
+(`X = M⁺ B`); the `out`/`w` scratch buffers are shared across columns.
+"""
+function ldlt_pinv_solve(F, B::AbstractMatrix; tol=nothing)
+    nI = size(B, 1)
+    X      = Matrix{Float64}(undef, nI, size(B, 2))
+    colbuf = Vector{Float64}(undef, nI)
+    wbuf   = Vector{Float64}(undef, nI)
+    outbuf = Vector{Float64}(undef, nI)
+    for j in axes(B, 2)
+        colbuf .= @view B[:, j]
+        ldlt_pinv_solve!(outbuf, F, colbuf, wbuf; tol=tol)
+        @inbounds X[:, j] = outbuf
+    end
+    return X
+end
+
 """    nullspace_ldlt(X::AbstractMatrix; tol=nothing) -> Matrix
 
 Compute a basis for the nullspace of the symmetric positive-semidefinite matrix `X`
@@ -557,7 +649,7 @@ directions; the corresponding columns of `P^{-1} (L^\\mathsf{T})^{-1}` form the
 returned basis matrix.
 """
 function nullspace_ldlt(X::AbstractMatrix; tol=nothing)
-    M = ldlt!(ChordalLDLt(X), RowMaximum())
+    M = ldlt!(ChordalLDLt(X), RowMaximum(); check=false)
     D = M.D; L = M.L; P = M.P
     max_abs = maximum(i -> abs(D[i, i]), 1:size(D, 1); init=0.0)
     threshold = isnothing(tol) ? _default_ldlt_nullity_threshold(max_abs) : tol
@@ -601,13 +693,11 @@ function ldlt_pseudoinverse_and_null(M, b; tol=nothing)
     null_vecs = P \ (Lfac' \ U)
 
     # Particular solution via pseudoinverse: suppress null directions in D
-    c = P' \ b          # permute rhs
-    z = Lfac \ c        # forward solve
+    c = P' \ b                          # permute rhs
+    z = Lfac \ c                        # forward solve
     w = zeros(n)
-    for i in setdiff(1:n, null_idx)
-        w[i] = z[i] / D[i, i]  # D⁺ on free directions only
-    end
-    x_p = P \ (Lfac' \ w)  # back solve + undo permutation
+    ldiv_diag_pinv!(w, D, z, threshold) # D⁺ on free directions only
+    x_p = P \ (Lfac' \ w)               # back solve + undo permutation
 
     return x_p, null_vecs
 end
@@ -733,7 +823,7 @@ function harmonic_extension_ldl_diagnostics(s::EuclideanSheaf,
             0.0, 0.0, 0.0, 0.0, Inf, Float64[], Float64[], Float64[])
     end
 
-    M = ldlt!(ChordalLDLt(L_II), RowMaximum())
+    M = ldlt!(ChordalLDLt(L_II), RowMaximum(); check=false)
     pivots = [M.D[i, i] for i in 1:size(M.D, 1)]
     abs_pivots = abs.(pivots)
     max_abs = maximum(abs_pivots; init=0.0)
@@ -861,7 +951,7 @@ function harmonic_extension(s::EuclideanSheaf, boundary::Dict{Int,<:AbstractVect
     end
 
     x_interior, null_interior = ldlt_pseudoinverse_and_null(
-        ldlt!(ChordalLDLt(L_II), RowMaximum()), b)
+        ldlt!(ChordalLDLt(L_II), RowMaximum(); check=false), b)
 
     x        = zeros(n_total)
     x[I_idx] = x_interior
@@ -876,4 +966,5 @@ function harmonic_extension(s::EuclideanSheaf, boundary::Dict{Int,<:AbstractVect
 end
 
 
-end # EuclideanSheaves
+
+end # EuclideanSheaves

src/network_sheaves/TrajectorySheaf.jl

@@ -1003,7 +1003,7 @@ function optimal_control_trajectory(ts::ControlledTrajectorySheaf{T},
 
     # Step 3: Solve using ChordalLDLt pseudoinverse.
     Rred_sparse = sparse(Rred)
-    M_ldlt      = ldlt!(ChordalLDLt(Rred_sparse), RowMaximum())
+    M_ldlt      = ldlt!(ChordalLDLt(Rred_sparse), RowMaximum(); check=false)
 
     # Cheap convexity check: for a symmetric matrix, a negative LDL pivot
     # certifies that the reduced Hessian is not positive semidefinite.