Moving Formation Example

This example combines a formation goal in positions with a consensus goal in velocities. As such, the coordination sheaf is the constant sheaf $\underline{\R}^4$ on the three vertex path graph. This encodes a leader-follower topology with the middle agent in the path acting as the leader. The leader’s objective is to track a constant rightward velocity vector and minimize its control actuation while the followers’ objectives are to simply minimize control actuation. The edge potential functions are of the form $U_e(y)=(1/2)\|y-b_e\|_2^2$ where the velocity coordinates of each be are 0 encoding consensus in velocity while the position coordinates are chosen to achieve a fixed displacement between the leader and its followers. The results of this controller run for 160 iterations are shown below.

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, 3.0, 0.0, 0.0]) # track this velocity
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 constant sheaf

c = CellularSheaf([4, 4, 4], [4, 4])
set_edge_maps!(c, 1, 2, 1, I(4), I(4))
set_edge_maps!(c, 1, 3, 2, I(4), I(4))

4×4 LinearAlgebra.Diagonal{Int64, Vector{Int64}}:
 -1   ⋅   ⋅   ⋅
  ⋅  -1   ⋅   ⋅
  ⋅   ⋅  -1   ⋅
  ⋅   ⋅   ⋅  -1

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

x_init = BlockArray(5 * rand(-1.0:0.1:1.0, 12), c.vertex_stalks)
b = [5, 0, 5, 0, -5, 0, 5, 0] # Desired pairwise distance
prob = MultiAgentMPCProblem([params1, params2, params3], c, x_init, b)
alg = ADMM(2.0, 10)
num_iters = 160

Run solver on MPC

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

(Any[[4.0, -1.0, 0.5, -3.0, -4.5, -2.0, 0.0, 0.5, 0.5, 4.5, -0.5, -2.0], [3.9, -0.7999999980031839, 0.19999999999999996, -2.7999999980042687, -4.7, -1.7999999981152213, 0.05, 0.29999999811821965, 0.95, 4.388701237665706, -0.7, -1.996447794866939], [3.8200000001996814, -0.5999999960066017, -0.07999999980042694, -2.599999996008985, -4.879999999811522, -1.5999999961553986, 0.07999999981182197, 0.09999999616731242, 1.3888701237665706, 4.277159393178164, -0.8996447794866939, -1.9884809654374893], [3.760000000599021, -0.39999999401029107, -0.33999999940132547, -2.399999994014255, -5.039999999427062, -1.3999999942625514, 0.08999999942855322, -0.10000000567151882, 1.816586063084387, 4.166518084172223, -1.0984928760304429, -1.976326582829304], [3.720000001197992, -0.19999999201429938, -0.579999998802751, -2.1999999920202287, -5.179999998853317, -1.1999999923003177, 0.07999999886140133, -0.3000000075979688, 2.2332378715016095, 4.057846121513827, -1.2961255343133733, -1.9601151936739025], [3.700000001996562, 9.981312304452317e-9, -0.7999999980047738, -1.9999999900271268, -5.299999998083349, -0.9999999904021475, 0.04999999810160445, -0.5000000093152703, 2.639022483652992, 3.9523028456093794, -1.4921370536807634, -1.9400432144503401], [3.7000000029946936, 0.2000000119764637, -0.9999999970074865, -1.7999999880352546, -5.3999999971235635, -0.7999999884383684, -2.829922587932465e-9, -0.7000000111434624, 3.03425276821393, 3.850945169335493, -1.6861413751257974, -1.916712484067121], [3.72000000419234, 0.400000013971046, -1.179999995811012, -1.5999999860450733, -5.4799999959674, -0.5999999865368029, -0.07000000394426883, -0.9000000115916813, 3.4193472851474795, 3.7547307893768034, -1.8778126235325097, -1.8918359227689645], [3.7600000055894447, 0.600000015964907, -1.3399999944155192, -1.3999999840574189, -5.5399999946210805, -0.39999998463412, -0.16000000510343698, -1.090365457503293, 3.79482036408516, 3.6645409071456694, -2.066996215809406, -1.865839715298298], [3.8200000071859352, 0.8000000179578239, -1.479999992821261, -1.1999999820735305, -5.579999993084493, -0.19999998273064368, -0.2690365508537663, -1.263275738407156, 4.1612744547997265, 3.5811645406416908, -2.2535801873392356, -1.8400236925123283]  …  [44.92499666459401, 2.997866494998169, -1.9973891551028202, 0.004538051192488444, 39.951048405980146, 2.915902876971056, -6.794686742697907, -0.05927512465266219, 49.9625950605396, 2.9120254500180374, -6.795591526432395, -0.05805062919624964], [45.22478331409383, 2.9977644627064266, -1.9969353499835714, 0.004319172704203503, 40.24263869367725, 2.9221599676925174, -6.800614255163174, -0.06203936349073101, 50.2537976055414, 2.918355979150274, -6.80139658935202, -0.06089410271142192], [45.52455976036447, 2.9976777518031223, -1.996503432713151, 0.004098561158874783, 40.5348546904465, 2.9283311441336655, -6.806818191512247, -0.06453409770588352, 50.545633203456426, 2.924609583528614, -6.807485999623163, -0.06346677213482936], [45.82432753554478, 2.9976058946691007, -1.9960935765972636, 0.0038768209244036157, 40.82768780485987, 2.9344058436231077, -6.813271601282835, -0.06676540891562682, 50.83809416180929, 2.9307742690458594, -6.813832676836646, -0.06577438608297784], [46.124088125011696, 2.9975484088540756, -1.9957058945048232, 0.003654532280309952, 41.12112838922218, 2.940374176277499, -6.819948142174398, -0.0687397264155015, 51.131171588713876, 2.9368388178534293, -6.820410115444944, -0.06782308492004424], [46.4238429658971, 2.997504798894827, -1.995340441276792, 0.0034322510977515194, 41.41516580684993, 2.946226919444678, -6.826822114815948, -0.07046379716676401, 51.42485547049922, 2.942792780066431, -6.827192423936949, -0.06961936793803868], [46.723593445786584, 2.9974745580833986, -1.9949972161670169, 0.0032105085920945313, 41.709788498794396, 2.9519555108980664, -6.8338684945326245, -0.07194465634656196, 51.71913474850586, 2.948626463892249, -6.834154360730753, -0.07117006114141762], [47.02334090159492, 2.997457170183533, -1.9946761653078073, 0.0029898111454119644, 42.0049840498842, 2.9575520408373133, -6.841062960167281, -0.07318959850234986, 52.013997394895085, 2.954330924292277, -6.841271366844895, -0.07248228569014066], [47.323086618613274, 2.997452111092575, -1.994377184193266, 0.002770640196264016, 42.30073925396793, 2.963009242768958, -6.848381920017515, -0.07420614934986586, 52.30943048732431, 2.95989795027979, -6.848519595413909, -0.07356342705060696], [47.62283182972253, 2.9974588504530715, -1.9941001201736397, 0.0025534521938494997, 42.59704017824483, 2.9683204832386814, -6.855802534952502, -0.07500203824824599, 52.60542028235229, 2.96532005098072, -6.855875938118969, -0.07442110490053509]], Any[[2.0000000199681613, 2.0000000199573136, 2.000000018847787, -2.0000000188178033, -1.1129876233429423, 0.035522051330609115], [2.000000019965822, 2.000000019952839, 2.000000019598227, -2.000000019509072, -1.1154184448754163, 0.07966829429449658], [2.0000000199631063, 2.000000019947299, 2.000000018928471, -2.000000018388312, -1.1064130900594078, 0.12154382608185262], [2.000000019959917, 2.0000000199402623, 2.000000019622336, -2.0000000192644998, -1.086719626583961, 0.1621138915540151], [2.0000000199561168, 2.0000000199310173, 2.0000000189817024, -2.0000000171730155, -1.055432759044476, 0.20071979223562397], [2.0000000199515138, 2.000000019918721, 2.0000000196377914, -2.000000018281921, -1.0135767627388632, 0.23330730383219192], [2.000000019945823, 2.0000000199018126, 2.0000000190156553, -2.000000004482188, -0.9621437995868964, 0.24876561298156433], [2.0000000199386103, 2.0000000198765453, 2.000000019026828, -1.9036544591161173, -0.9018988223113422, 0.259962074706666], [2.0000000199291694, 2.0000000198388843, 2.000000019034763, -1.7291028090386298, -0.8337636650397852, 0.2581602278596964], [2.0000000199696513, 2.0000000199197414, 2.0000000190414404, -1.5666658217531142, -0.7586297187501015, 0.2505007635929856]  …  [-0.0011780460504193947, -0.0021651737005509686, 0.0633176531779551, -0.030394468702340433, 0.06394676903479833, -0.031196011099991344], [-0.0010203229174207621, -0.0021887848828494095, 0.06257090721461002, -0.027642388380688224, 0.06330529132236225, -0.028434735151722797], [-0.0008671090330416698, -0.0022061154532872075, 0.06171176441147859, -0.024947342151525068, 0.0625360437834046, -0.02572669423407434], [-0.0007185713402151983, -0.0022174023447116727, 0.0607469948944231, -0.0223131120974331, 0.061646855172452546, -0.02307613948148472], [-0.000574858150249289, -0.0022228864409366347, 0.05968332654391464, -0.019743174998746667, 0.060645488075699264, -0.020486988370664004], [-0.00043609959248875273, -0.002222811825584328, 0.058527431671788595, -0.01724070751262517, 0.05953962213001728, -0.01796283017994436], [-0.0003024081142828102, -0.0022174250565698828, 0.057285914533883556, -0.014808591797979435, 0.05833683825817998, -0.015506932033789457], [-0.00017387899865650526, -0.00220697446682567, 0.05596529939246696, -0.012449421557879118, 0.05704460400027929, -0.013122245487230342], [-5.059090957927708e-5, -0.0021917094914794826, 0.05457201931644537, -0.010165508475159893, 0.05567025987512933, -0.0108114136046631], [6.739360496275397e-5, -0.0021718800241451626, 0.05311240469723583, -0.00795888898380128, 0.05422100700929534, -0.008576778499281236]])

Plot results with triangles

PaperPlotting.plot_trajectories(trajectory, C, false, true)