Flocking Example

For this example, agents implement the standard flocking goal of reaching consensus in velocities while staying a fixed distance away from all other agents. The constant sheaf $\underline{\R}^4$ on a fully connected communication topology along with potential functions summing the stan- dard consensus potential function on the velocity components and the fixed distance potential function with $r^2 = 5$ on the position components. Each agents’ objective is to minimize total control activation. Additionally, a designated leader agent tracks a constant rightward velocity vector. The results of this controller run for 65 iterations are shown below. Computing the distance between each agent confirms that they reached the desired pairwise distance of $\sqrt{5}$.

using Test
using AlgebraicOptimization
using LinearAlgebra
using BlockArrays
using Plots
include("../../../examples/paper-examples/PaperPlotting.jl")
using .PaperPlotting

Set up each agent's dynamics: $x(t+1) = Ax(t) + Bu(t)$

dt = 0.1 # Discretization step size
A = [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1]
B = [0 0; dt 0; 0 0; 0 dt]
C = [1 0 0 0; 0 0 1 0]
system = DiscreteLinearSystem(A, B, C)

DiscreteLinearSystem([1.0 0.1 0.0 0.0; 0.0 1.0 0.0 0.0; 0.0 0.0 1.0 0.1; 0.0 0.0 0.0 1.0], [0.0 0.0; 0.1 0.0; 0.0 0.0; 0.0 0.1], [1 0 0 0; 0 0 1 0])

Initialize the weight matrices such that the objective only concerns velocities

Q_leader = [0 0 0 0; 0 50 0 0; 0 0 0 0; 0 0 0 50]
Q_follower = zeros(4, 4)
R = I(2)

2×2 LinearAlgebra.Diagonal{Bool, Vector{Bool}}:
 1  ⋅
 ⋅  1

Define the parameters for the MPC

N = 10
control_bounds = [-2.0, 2.0]
params1 = MPCParams(Q_leader, R, system, control_bounds, N, [0.0, 1.0, 0.0, 0.0])
params2 = params3 = MPCParams(Q_follower, R, system, control_bounds, N)

MPCParams([0.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0], Bool[1 0; 0 1], DiscreteLinearSystem([1.0 0.1 0.0 0.0; 0.0 1.0 0.0 0.0; 0.0 0.0 1.0 0.1; 0.0 0.0 0.0 1.0], [0.0 0.0; 0.1 0.0; 0.0 0.0; 0.0 0.1], [1 0 0 0; 0 0 1 0]), [-2.0, 2.0], 10, [0.0, 0.0, 0.0, 0.0])

Define the potential functions

\[q(x) = (x' * x - 5.0)^2\]

\[p(x) = [x[2], x[4]]' * [x[2], x[4]] + q([x[1], x[3]])\]

q(x) = (x' * x - 5.0)^2
p(x) = [x[2], x[4]]' * [x[2], x[4]] + q([x[1], x[3]])

p (generic function with 1 method)

Define the constant sheaf

c = PotentialSheaf([4, 4, 4], [2, 2, 2], [q, q, q])
set_edge_maps!(c, 1, 2, 1, C, C)
set_edge_maps!(c, 1, 3, 2, C, C)
set_edge_maps!(c, 2, 3, 3, C, C)

2×4 Matrix{Int64}:
 -1  0   0  0
  0  0  -1  0

Set up solver to perform MPC and solve the optimization problem with ADMM

x_init = BlockArray(rand(12), c.vertex_stalks)
prob = MultiAgentMPCProblem([params1, params1, params1], c, x_init)
alg = NonConvexADMM(1000.0, 10, 0.0001, 5000)
num_iters = 200

Run solver on MPC

trajectory, controls = do_mpc!(prob, alg, num_iters)

(Any[[0.780917100312421, 0.17640252358962838, 0.06902672832218315, 0.6343412574384096, 0.6660735519612098, 0.87316595264296, 0.4503549327803237, 0.42592124646838614, 0.09039397860275744, 0.4039331960979812, 0.4772240164581938, 0.36590905238639126], [0.7985573526713838, 0.3764025253417641, 0.13246085406602412, 0.43434125592864536, 0.7533901472255058, 0.9231186047385865, 0.49294705742716227, 0.22592125764038334, 0.13078729821255555, 0.6039331969589506, 0.513814921696833, 0.18192957011539357], [0.8361976052055602, 0.5764025254993291, 0.17589497965888867, 0.23434126433163466, 0.8457020076993644, 0.947869231861179, 0.5155391831912006, 0.11177327267974767, 0.19118061790845062, 0.7905018565492978, 0.5320078787083723, 0.08972528957028292], [0.8938378577554931, 0.7738269067101089, 0.19932910609205215, 0.11664335546029202, 0.9404889308854824, 0.9599706339952612, 0.5267165104591753, 0.05463853594768217, 0.2702308035633804, 0.8834232856678633, 0.5409804076654006, 0.04358145471101827], [0.971220548426504, 0.8723433072857316, 0.21099344163808134, 0.05779056753572734, 1.0364859942850084, 0.9657393621701309, 0.5321803640539435, 0.026059594213426438, 0.3585731321301667, 0.9295822253200114, 0.5453385531365024, 0.02050182943854571], [1.0584548791550772, 0.921542194789851, 0.21677249839165408, 0.028365395012086264, 1.1330599305020215, 0.9684242112149466, 0.5347863234752862, 0.011766351597686322, 0.45153135466216787, 0.9524008625418514, 0.547388736080357, 0.008959095062062992], [1.1506090986340622, 0.9460076934366264, 0.2196090378928627, 0.013653545155449913, 1.2299023516235161, 0.9695498824406196, 0.5359629586350548, 0.004618133521316226, 0.546771440916353, 0.9635783702465117, 0.5482846455865633, 0.0031865624295472784], [1.2452098679777248, 0.9581844065496032, 0.2209743924084077, 0.006298113945437794, 1.326857339867578, 0.9698873063389778, 0.5364247719871864, 0.0010435152723182535, 0.6431292779410042, 0.9689515416666042, 0.548603301829518, 0.0003000089521555736], [1.341028308632685, 0.9641095558656455, 0.22160420380295145, 0.0026207707201891047, 1.423846070501476, 0.9698262864672995, 0.5365291235144183, -0.000743759218724176, 0.7400244321076646, 0.9713010791165992, 0.5486333027247335, -0.0011431158285831308], [1.4374392642192497, 0.9668738562112932, 0.22186628087497037, 0.0007824107583505162, 1.5208286991482058, 0.9695691997262704, 0.5364547475925459, -0.001637089972300526, 0.8371545400193245, 0.9721293510454034, 0.5485189911418752, -0.0018643067291421206]  …  [18.896112118599834, 0.9579463682699406, 0.20381271916154925, -0.0009687695155349891, 18.97906610669232, 0.9579463691274361, 0.4924046305903257, -0.002329918680137505, 18.30490675630332, 0.957946369190713, 0.5034737705781521, -0.0023805240602957074], [18.99190675542683, 0.9579463682456142, 0.20371584220999575, -0.00096830903628609, 19.07486074360506, 0.9579463691206628, 0.49217163872231195, -0.0023288534977734254, 18.40070139322239, 0.9579463691860851, 0.5032357181721225, -0.00237943574241243], [19.08770139225139, 0.9579463682205571, 0.20361901130636714, -0.0009678487759141911, 19.17065538051713, 0.9579463691134829, 0.4919387533725346, -0.002327788802385502, 18.496496030141, 0.9579463691806421, 0.5029977745978813, -0.00237834792208587], [19.183496029073446, 0.9579463681950137, 0.2035222264287757, -0.0009673887343151884, 19.26645001742848, 0.9579463691059604, 0.4917059744922961, -0.002326724593750971, 18.592290667059064, 0.9579463691743885, 0.5027599398056727, -0.002377260599088834], [19.279290665892947, 0.9579463681695827, 0.2034254875553442, -0.0009669289113851411, 19.362244654339076, 0.9579463690978588, 0.491473302032921, -0.0023256608716474207, 18.688085303976504, 0.9579463691683278, 0.5025222137457638, -0.0023761737731943606], [19.375085302709905, 0.9579463681472075, 0.2033287946642057, -0.0009664693070200969, 19.458039291248863, 0.9579463690890602, 0.49124073594575624, -0.0023245976358523364, 18.783879940893335, 0.9579463691628924, 0.5022845963684444, -0.002375087444175515], [19.470879939524625, 0.9579463681261032, 0.20323214773350368, -0.0009660099211161531, 19.55383392815777, 0.9579463676291898, 0.491008276182171, -0.00232353489659366, 18.879674577809624, 0.9579463691573457, 0.5020470876240268, -0.002374001611805629], [19.566674576337235, 0.9579463681053326, 0.20313554674139206, -0.0009655507535694959, 19.649628564920686, 0.9579463668410222, 0.49077592269251163, -0.002322472638016754, 18.975469214725358, 0.95794636915138, 0.5018096874628463, -0.0023729162758581094], [19.66246921314777, 0.9579463680844204, 0.20303899166603512, -0.0009650918042763466, 19.745423201604787, 0.9579463664021285, 0.49054367542870997, -0.0023214108624896456, 19.071263851640495, 0.9579463691449349, 0.5015723958352605, -0.0023718314361065858], [19.75826384995621, 0.9579463680631013, 0.20294248248560748, -0.0009646330731329315, 19.841217838245, 0.9579463661446306, 0.490311534342461, -0.002320349571086021, 19.16705848855499, 0.9579463691380504, 0.5013352126916498, -0.002370747092324679]], Any[[2.0000000175213573, -2.0000000150976427, 0.49952652095626515, -1.9999998882800278, 2.0000000086096943, -1.8397948227099767], [2.00000000157565, -1.999999915970107, 0.24750627122592414, -1.1414798496063567, 1.8656865959034714, -0.9220428054511065], [1.9742438121077972, -1.1769790887134264, 0.12101402134082208, -0.5713473673206549, 0.9292142911856546, -0.46143834859264643], [0.9851640057562278, -0.5885278792456468, 0.057687281748697784, -0.2857894173425573, 0.4615893965214812, -0.2307962527247256], [0.4919888750411932, -0.29425172523641074, 0.02684849044815712, -0.14293242615740115, 0.2281863722184, -0.11542734376482716], [0.24465498646775355, -0.1471184985663635, 0.011256712256730179, -0.07148218076370096, 0.11177507704660351, -0.05772532632515714], [0.12176713112976895, -0.07355431210012119, 0.0033742389835818153, -0.035746182489979725, 0.05373171420092473, -0.028865534773917047], [0.0592514931604224, -0.036773432252486894, -0.0006101987167824818, -0.017872744910424294, 0.023495374499950095, -0.014431247807387044], [0.027643003456476405, -0.018383599618385885, -0.0025708674102911722, -0.0089333075357635, 0.00828271928804218, -0.007211909005589897], [0.011722059110767083, -0.00918898759438599, -0.0035484192495229424, -0.004462230067684741, 0.0006298638088630768, -0.0036011432360492325]  …  [-2.3777082146569914e-10, 4.606982299960581e-6, -6.277804325467118e-11, 1.065669562810378e-5, -3.830009846971731e-11, 1.0888156670424197e-5], [-2.432647315728695e-10, 4.604792488990943e-6, -6.773319370580272e-11, 1.0651823640798564e-5, -4.6278837787955024e-11, 1.0883178832771746e-5], [-2.505702686456796e-10, 4.602603718989732e-6, -7.179917049342639e-11, 1.0646953879231964e-5, -5.442946055847126e-11, 1.0878203265600515e-5], [-2.5543480873618956e-10, 4.600415990026038e-6, -7.522394160271398e-11, 1.0642086345311202e-5, -6.253696370734469e-11, 1.0873229970362383e-5], [-2.5431015253187637e-10, 4.598229300473005e-6, -8.101575116540162e-11, 1.0637221035505937e-5, -6.060730540042257e-11, 1.086825894473237e-5], [-2.2375246980236693e-10, 4.596043650441883e-6, -8.798584903717846e-11, 1.063235795084234e-5, -5.43534663845117e-11, 1.0863290188456739e-5], [-2.1104257595492307e-10, 4.593859039438571e-6, -1.4598702865280143e-8, 1.0627392586763225e-5, -5.5467424706696667e-11, 1.0858323698860739e-5], [-2.0770556660650334e-10, 4.591675466571694e-6, -7.881676777192096e-9, 1.0622585769059403e-5, -5.965685738198065e-11, 1.0853359475192212e-5], [-2.091232322777902e-10, 4.589492931492872e-6, -4.388936528830204e-9, 1.0617755271083314e-5, -6.445063991752066e-11, 1.0848397515237981e-5], [-2.1319102462929082e-10, 4.587311434151861e-6, -2.574979378308378e-9, 1.0612914036243606e-5, -6.884491956635257e-11, 1.0843437819065905e-5]])

Plot results

PaperPlotting.plot_trajectories(trajectory, C)