diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m
index 155142e..432290d 100644
--- a/@miSim/constrainMotion.m
+++ b/@miSim/constrainMotion.m
@@ -15,16 +15,18 @@ function [obj] = constrainMotion(obj)
v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
% Initialize QP based on number of agents and obstacles
- h = NaN(size(obj.agents, 1));
- h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling)
+ nLNAPairs = sum(obj.constraintAdjacencyMatrix) - size(obj.agents, 1);
+ total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
kk = 1;
- A = zeros(nAAPairs + nAOPairs + nADPairs, 3 * size(obj.agents, 1));
- b = zeros(nAAPairs + nAOPairs + nADPairs, 1);
+ A = zeros(total, 3 * size(obj.agents, 1));
+ b = zeros(total, 1);
% Set up collision avoidance constraints
+ h = NaN(size(obj.agents, 1));
+ h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
for ii = 1:(size(obj.agents, 1) - 1)
for jj = (ii + 1):size(obj.agents, 1)
h(ii, jj) = norm(agents(ii).pos - agents(jj).pos)^2 - (agents(ii).collisionGeometry.radius + agents(jj).collisionGeometry.radius)^2;
@@ -94,6 +96,23 @@ function [obj] = constrainMotion(obj)
kk = kk + 1;
end
+ % Add communication network constraints
+ hComms = NaN(size(obj.agents, 1));
+ hComms(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
+ for ii = 1:(size(obj.agents, 1) - 1)
+ for jj = (ii + 1):size(obj.agents, 1)
+ if obj.constraintAdjacencyMatrix(ii, jj)
+ hComms(ii, jj) = (agents(ii).commsGeometry.radius + agents(jj).commsGeometry.radius)^2 - norm(agents(ii).pos - agents(jj).pos)^2;
+ hComms(jj, ii) = hComms(ii, jj);
+
+ A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - agents(jj).pos);
+ A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
+ b(kk) = obj.barrierGain * hComms(ii, jj)^3;
+ kk = kk + 1;
+ end
+ end
+ end
+
% Solve QP program generated earlier
vhat = reshape(v', 3 * size(obj.agents, 1), 1);
H = 2 * eye(3 * size(obj.agents, 1));
diff --git a/@miSim/initialize.m b/@miSim/initialize.m
index 9a27236..f134b04 100644
--- a/@miSim/initialize.m
+++ b/@miSim/initialize.m
@@ -25,7 +25,6 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
end
end
obj.makeVideo = makeVideo;
-
% Define simulation time parameters
obj.timestep = timestep;
@@ -51,9 +50,11 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
% Define agents
obj.agents = agents;
+ obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1)));
- % Compute adjacency matrix
+ % Compute adjacency matrix and lesser neighbors
obj = obj.updateAdjacency();
+ obj = obj.lesserNeighbor();
% Set up times to iterate over
obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
diff --git a/@miSim/lesserNeighbor.m b/@miSim/lesserNeighbor.m
new file mode 100644
index 0000000..fef0529
--- /dev/null
+++ b/@miSim/lesserNeighbor.m
@@ -0,0 +1,35 @@
+function obj = lesserNeighbor(obj)
+ arguments (Input)
+ obj (1, 1) {mustBeA(obj, 'miSim')};
+ end
+ arguments (Output)
+ obj (1, 1) {mustBeA(obj, 'miSim')};
+ end
+
+ % Check possible connections from adjacency matrix
+ % Choose connections which fully connect network by selecting maximum
+ % indices according to the previous columns (or rows) of the new
+ % constraint adjacency matrix
+ % Place that choice in the constraint adjacency matrix
+
+ % Begin with all possible connections and trim down
+ constraintAdjacencyMatrix = obj.adjacency;
+
+ % Iterate over each agent (by increasing index)
+ for ii = 1:size(obj.agents, 1)
+ % Iterate over each agent of lesser index and see if a higher
+ % indexed agent provides connectivity already
+ for jj = 1:(ii - 1)
+ for kk = 1:(jj - 1)
+ % Check if a connection between the two lesser agents
+ % already exists
+ if constraintAdjacencyMatrix(jj, kk)
+ constraintAdjacencyMatrix(jj, kk) = false;
+ constraintAdjacencyMatrix(kk, jj) = false;
+ end
+ end
+ end
+ end
+
+ obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix;
+end
\ No newline at end of file
diff --git a/@miSim/miSim.m b/@miSim/miSim.m
index 3ebc020..3bf1cee 100644
--- a/@miSim/miSim.m
+++ b/@miSim/miSim.m
@@ -11,6 +11,7 @@ classdef miSim
obstacles = cell(0, 1); % geometries that define obstacles within the domain
agents = cell(0, 1); % agents that move within the domain
adjacency = NaN; % Adjacency matrix representing communications network graph
+ constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
partitioning = NaN;
performance = 0; % cumulative sensor performance
@@ -47,6 +48,7 @@ classdef miSim
methods (Access = public)
[obj] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
[obj] = run(obj);
+ [obj] = lesserNeighbor(obj);
[obj] = constrainMotion(obj);
[obj] = partition(obj);
[obj] = updateAdjacency(obj);
diff --git a/@miSim/plotConnections.m b/@miSim/plotConnections.m
index 8a33a89..1cc6eea 100644
--- a/@miSim/plotConnections.m
+++ b/@miSim/plotConnections.m
@@ -9,9 +9,9 @@ function obj = plotConnections(obj)
% Iterate over lower triangle off-diagonal region of the
% adjacency matrix to plot communications links between agents
X = []; Y = []; Z = [];
- for ii = 2:size(obj.adjacency, 1)
+ for ii = 2:size(obj.constraintAdjacencyMatrix, 1)
for jj = 1:(ii - 1)
- if obj.adjacency(ii, jj)
+ if obj.constraintAdjacencyMatrix(ii, jj)
X = [X; obj.agents{ii}.pos(1), obj.agents{jj}.pos(1)];
Y = [Y; obj.agents{ii}.pos(2), obj.agents{jj}.pos(2)];
Z = [Z; obj.agents{ii}.pos(3), obj.agents{jj}.pos(3)];
diff --git a/@miSim/plotGraph.m b/@miSim/plotGraph.m
index 034a35a..0c6cb80 100644
--- a/@miSim/plotGraph.m
+++ b/@miSim/plotGraph.m
@@ -7,7 +7,7 @@ function obj = plotGraph(obj)
end
% Form graph from adjacency matrix
- G = graph(obj.adjacency, 'omitselfloops');
+ G = graph(obj.constraintAdjacencyMatrix, 'omitselfloops');
% Plot graph object
if isnan(obj.networkGraphIndex)
diff --git a/@miSim/run.m b/@miSim/run.m
index bf027d9..8c3892c 100644
--- a/@miSim/run.m
+++ b/@miSim/run.m
@@ -24,7 +24,10 @@ function [obj] = run(obj)
obj = obj.partition();
end
- % Iterate over agents to simulate their motion
+ % Determine desired communications links
+ obj = obj.lesserNeighbor();
+
+ % Iterate over agents to simulate their unconstrained motion
for jj = 1:size(obj.agents, 1)
obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.t);
end
@@ -33,6 +36,8 @@ function [obj] = run(obj)
% CBF constraints solved by QP
obj = constrainMotion(obj);
+ % Finished simulation for this timestep, do accounting
+
% Update total performance
obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(end), obj.agents))];
diff --git a/@miSim/updateAdjacency.m b/@miSim/updateAdjacency.m
index 782e0da..f7a205c 100644
--- a/@miSim/updateAdjacency.m
+++ b/@miSim/updateAdjacency.m
@@ -28,4 +28,9 @@ function obj = updateAdjacency(obj)
end
obj.adjacency = A & A';
+
+ if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
+ warning("Eliminated network connections that were necessary");
+ keyboard
+ end
end
\ No newline at end of file
diff --git a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml
index 99772b4..4356a6a 100644
--- a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml
+++ b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml
@@ -1,6 +1,2 @@
-
-
-
-
-
\ No newline at end of file
+
\ No newline at end of file
diff --git a/test/test_miSim.m b/test/test_miSim.m
index 081cd10..97a0249 100644
--- a/test/test_miSim.m
+++ b/test/test_miSim.m
@@ -31,8 +31,8 @@ classdef test_miSim < matlab.unittest.TestCase
objective = sensingObjective;
% Agents
- minAgents = 2; % Minimum number of agents to be randomly generated
- maxAgents = 4; % Maximum number of agents to be randomly generated
+ minAgents = 4; % Minimum number of agents to be randomly generated
+ maxAgents = 6; % Maximum number of agents to be randomly generated
sensingLength = 0.05; % length parameter used by sensing function
agents = cell(0, 1);