Added lesser neighbor algorithm and constraints

@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));
+    A = zeros(total, 3 * size(obj.agents, 1));
-    b = zeros(nAAPairs + nAOPairs + nADPairs, 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));

@miSim/initialize.m

@@ -26,7 +26,6 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
     end
     obj.makeVideo = makeVideo;
 
-
     % Define simulation time parameters
     obj.timestep = timestep;
     obj.maxIter = maxIter - 1;
@@ -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)';

@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

@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);

@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)];

@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)

@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))];
 

@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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml

@@ -1,6 +1,2 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<Info>
+<Info/>
-    <Category UUID="FileClassCategory">
-        <Label UUID="design"/>
-    </Category>
-</Info>

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
+        minAgents = 4; % Minimum number of agents to be randomly generated
-        maxAgents = 4; % Maximum 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);