Closed-loop MPC for multi-agent tracking sheaf problems by Optimax14 · Pull Request #65 · AlgebraicJulia/CellularSheaves.jl

Optimax14 · 2026-06-05T05:00:30Z

This PR implements a receding-horizon MPC algorithm for multi-agent tracking sheaf problems.

Summary

Added window_problem to take the entire horizon and slice it up into sub problems of size k.
Added pre-computation of sheaf Laplacian factorization, which turns each MPC step from resolving the open loop problem on a smaller horizon to a simple solve of the harmonic extension.
Added run_mpc_scenario with :cached (reuses the factorization + orthogonal projection) & :naive (recomputes the open loop problem for each MPC step)
Added ldlt_pinv_solve helpers to EuclideanSheaves and switched solve_quadratic_on_basis to use svd\

Tests

40 new tests in test/network_sheaves/MultiAgentTrackingMPC.jl.

Docs

New literate page in docs/literature/control/mpc_target_tracking.jl showcasing the results of this PR and the application of the algorithm in different scenarios.

Closes #62

…ion, run_mpc_scenario

codecov · 2026-06-05T05:09:19Z

Codecov Report

❌ Patch coverage is 99.12281% with 2 lines in your changes missing coverage. Please review.
✅ Project coverage is 75.20%. Comparing base (c379f01) to head (d48429f).
⚠️ Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
src/network_sheaves/EuclideanSheaves.jl	95.55%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main      #65      +/-   ##
==========================================
+ Coverage   73.60%   75.20%   +1.59%     
==========================================
  Files          36       36              
  Lines        2550     2722     +172     
==========================================
+ Hits         1877     2047     +170     
- Misses        673      675       +2

☔ View full report in Codecov by Harness.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Copilot

Copilot encountered an error and was unable to review this pull request. You can try again by re-requesting a review.

Copilot

Pull request overview

Copilot reviewed 8 out of 8 changed files in this pull request and generated 5 comments.

…pc inputs

jpfairbanks

@copilot, there are some mechanical changes that you can help out with. Some of these comments need to be handled by @Optimax14 because they are conceptual clarification questions. If you want to try and answer these questions, I would be interested in your thoughts, even though they are not requests to modify the code directly.

jpfairbanks · 2026-06-09T20:32:39Z

+            (1, result1_ol,  "Open-loop"),
+            (2, result1_mpc, "MPC (W=k)"))
+        sp = p[panel]
+        plot!(sp; title="$title_str  —  t = $(round(times[t_idx]; digits=2))",


@copilot these titles need to be rounded to exactly 3 digits with @sprintf rather than numeric rounding. This should be done to all plot titles that use string interpolation of a number.

Fixed in the latest commit. Replaced round(times[t_idx]; digits=2) with @sprintf("%.3f", times[t_idx]) on line 290 and round(sr.residual; sigdigits=3) with @sprintf("%.3e", sr.residual) in the ScenarioResult recipe's plot_title on line 182. Printf was already imported.

jpfairbanks · 2026-06-09T20:39:13Z

+                              window=k, cost=1.0, y_col=IDX_Y, z_col=IDX_Z)
+
+show(stdout, MIME("text/plain"), result2_w2); println()
+show(stdout, MIME("text/plain"), result2_wk); println()


This shows a higher norm(dz) residual for the W=k case. Is that consistent with the text around this result?

ScenarioResult: label : MPC W=2 null dim : 2 ||dz|| : 2.471e-5 agents : 2 targets : 2 ScenarioResult: label : MPC W=k null dim : 8 ||dz|| : 0.003643 agents : 2 targets : 2

jpfairbanks · 2026-06-09T20:40:47Z

+    horizon(K)
+    times(Tall = 0:K)
+    track(tr1; agent=a1, target=t1, maps=(R_yz,R_yz), at=Tall)
+    track(tr2; agent=a2, target=t2, maps=(R_yz,R_yz), at=Tall)


There is no consensus term on this @tracking_problem, so the agents should just track independently their targets. I think that the cleanest comparison to make your point about window size is

consensus(c1; agents=(a1,a2), maps=(R_y,R_y), at=Tall) track(tr1; agent=a1, target=t1, maps=(R_yz,R_yz), at=Tall) track(tr2; agent=a2, target=t2, maps=(R_yz,R_yz), at=Tall)

But we should do an example like

consensus(c1; agents=(a1,a2), maps=(R_y,R_y), at=t[end]) track(tr1; agent=a1, target=t1, maps=(R_yz,R_yz), at=Tall) track(tr2; agent=a2, target=t2, maps=(R_yz,R_yz), at=Tall)

which will get the agents to try and get to tracking immediately and only arrive at consensus at the end of the horizon. Are also interesting.

jpfairbanks · 2026-06-09T20:43:31Z

+# minimum-energy compromise in which the dynamics edges carry small but nonzero
+# residual.  The open-loop extractor reads the sheaf directly while MPC enforces
+# the true dynamics at every step.  The trajectories are close but not
+# identical — both are valid answers to slightly different questions.


The nonzero residual in this model isn't caused by the infeasibility of the target trajectories. It is caused by the inherent contradiction of trying to track the targets and do consensus at the same time.

consensus(c1; agents=(a1,a2), maps=(R_y,R_y), at=Tall) track(tr1; agent=a1, target=t1, maps=(R_yz,R_yz), at=Tall) track(tr2; agent=a2, target=t2, maps=(R_yz,R_yz), at=Tall)

This says that you should try and be at consensus in y and tracking in yz every time step. Since the targets aren't at the same y coordinates as each other, this is an infeasible goal.

jpfairbanks · 2026-06-09T20:50:07Z

+    annotate!(p_speed[2], i - 0.5, s + maximum(speedups) * 0.05,
+              text(@sprintf("%.1f x", s), 9, :center, :steelblue))
+end
+p_speed


This should include a tradoff of control performance. Once we run the simulation of the MPC controller, we have a trajectory of z_t = (x_t,u_t) pairs. We can score that trajectory by computing |dz| for that trajectory. So we can numerically compare the loss due to the changing window size.

Copilot · 2026-06-09T20:58:07Z

@copilot, there are some mechanical changes that you can help out with. Some of these comments need to be handled by @Optimax14 because they are conceptual clarification questions. If you want to try ...

The mechanical @sprintf formatting change (comment #3383669467) is addressed above. For the conceptual questions directed at @Optimax14 — the jump at t>1.8, the higher W=k residual, the no-consensus comparison, and the window-size tradeoff plot — those depend on domain decisions about the sheaf construction and scenario design that need @Optimax14's input.

Copilot

Pull request overview

Copilot reviewed 10 out of 10 changed files in this pull request and generated 4 comments.

+    if size(N, 2) > 0 && !(cost isa Number && iszero(cost))
+        Q_II   = Matrix(build_control_cost_matrix(window_prob, stalks, cost)[interior, interior])
+        NtQ_II = N' * Q_II
+        Fq     = ldlt!(DenseLDLtPivoted(NtQ_II * N), RowMaximum(); check=false)
+        coeff  = ldlt_pinv_solve(Fq, NtQ_II * H)
+        M = H - N * coeff


 """
-    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


+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


+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


… allocations

… scenario

Optimax14 added 4 commits June 5, 2026 00:04

add ldlt_pinv_solve helpers and use svd\ in solve_quadratic_on_basis

0d6d41a

add MPC support to MultiAgentTracking: window_problem, TrackingExtens…

8de7ee0

…ion, run_mpc_scenario

add MPC tests for MultiAgentTracking

f14ba58

add MPC literate doc and register in docs/make.jl

8990b4d

Optimax14 force-pushed the itay/mpc-multi-agent branch from 61d075f to 8990b4d Compare June 5, 2026 05:05

fix p_z plot not rendering in docs

a2ffcf4

jpfairbanks requested a review from Copilot June 5, 2026 18:15

Copilot started reviewing on behalf of jpfairbanks June 5, 2026 18:16 View session

Copilot AI reviewed Jun 5, 2026

jpfairbanks requested a review from Copilot June 5, 2026 18:40

Copilot started reviewing on behalf of jpfairbanks June 5, 2026 18:41 View session

Copilot AI reviewed Jun 5, 2026

View reviewed changes

Comment thread src/ControlSheaves/MultiAgentTracking.jl

Comment thread src/ControlSheaves/MultiAgentTracking.jl

Comment thread src/ControlSheaves/MultiAgentTracking.jl

Comment thread src/ControlSheaves/MultiAgentTracking.jl Outdated

Comment thread src/network_sheaves/EuclideanSheaves.jl Outdated

jpfairbanks requested changes Jun 5, 2026

View reviewed changes

Optimax14 self-assigned this Jun 5, 2026

Optimax14 added 4 commits June 9, 2026 12:43

address review cleanups regarding comments, sparsity, and cache rename

fe112f4

replace partial pinning with state-only initial vertex and validate m…

1697df9

…pc inputs

use ldlt pseudoinverse and in-place allocation-free pinv solve

5c630cd

dedup mpc operator and loop with shared helpers and mul!

13f60e4

jpfairbanks requested changes Jun 9, 2026

View reviewed changes

Copilot started work on behalf of jpfairbanks June 9, 2026 20:54 View session

Use @sprintf("%.3f"/%.3e") for all numeric plot title formatting

dfaa371

Copilot finished work on behalf of jpfairbanks June 9, 2026 20:58

Copilot AI requested a review from jpfairbanks June 9, 2026 20:58

jpfairbanks requested a review from Copilot June 12, 2026 01:31

Copilot started reviewing on behalf of jpfairbanks June 12, 2026 01:32 View session

Copilot AI reviewed Jun 12, 2026

View reviewed changes

Optimax14 added 2 commits June 12, 2026 02:00

unify ldiv_diag_pinv! dispatch and simplify ldlt field access

52b9fc1

preallocate scratch buffer in WindowSolverCache to eliminate hot-loop…

cdce5d7

… allocations

Optimax14 added 2 commits June 12, 2026 02:00

test cached and naive mpc produce identical trajectories on quadrotor…

619f955

… scenario

use sprintf for mpc doc titles and clarify source of nonzero residual

d48429f

Optimax14 force-pushed the itay/mpc-multi-agent branch from 1958934 to d48429f Compare June 12, 2026 06:10

Conversation

Optimax14 commented Jun 5, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Tests

Docs

Uh oh!

codecov Bot commented Jun 5, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

jpfairbanks left a comment

Choose a reason for hiding this comment

Uh oh!

jpfairbanks Jun 9, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Jun 9, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

jpfairbanks Jun 9, 2026

Choose a reason for hiding this comment

Uh oh!

jpfairbanks Jun 9, 2026

Choose a reason for hiding this comment

Uh oh!

jpfairbanks Jun 9, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

jpfairbanks Jun 9, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Copilot AI commented Jun 9, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

Optimax14 commented Jun 5, 2026 •

edited

Loading

codecov Bot commented Jun 5, 2026 •

edited

Loading