fixed and verified communications constraint

@agent/agent.m

@@ -29,7 +29,6 @@ classdef agent
         comRange = NaN;
         commsGeometry = spherical;
         lesserNeighbors = [];
-        
 
         performance = 0;
 

@agent/initialize.m

@@ -22,14 +22,13 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
     obj.tilt = tilt;
     obj.collisionGeometry = collisionGeometry;
     obj.sensorModel = sensorModel;
-    obj.comRange = comRange;
     obj.guidanceModel = @gradientAscent;
     obj.label = label;
     obj.debug = debug;
     obj.plotCommsGeometry = plotCommsGeometry;
 
     % Add spherical geometry based on com range
-    obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, obj.comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));
+    obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));
 
     if obj.debug
         obj.debugFig = figure;

@miSim/constrainMotion.m

@@ -102,21 +102,17 @@ function [obj] = constrainMotion(obj)
     for ii = 1:(size(obj.agents, 1) - 1)
         for jj = (ii + 1):size(obj.agents, 1)
             if obj.constraintAdjacencyMatrix(ii, jj)
-                % d = agents(ii).pos - agents(jj).pos;
+                hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(agents(ii).pos - agents(jj).pos)^2;
-                % h = agents(ii).commsGeometry.radius^2 - dot(d,d);
-                % 
-                % A(kk, (3*ii-2):(3*ii)) =  2*d;
-                % A(kk, (3*jj-2):(3*jj)) = -2*d;
-                % b(kk) = obj.barrierGain * h^3;
-                % 
-                % kk = kk + 1;
 
-                hComms(ii, jj) = agents(ii).commsGeometry.radius^2 - norm(agents(ii).pos - agents(jj).pos)^2;
+                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (agents(ii).pos - agents(jj).pos);
-
-                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;
+                b(kk) = obj.barrierGain * hComms(ii, jj);
-                kk = kk + 1;
+
+                % dVNominal = v(ii, 1:3) - v(jj, 1:3); % nominal velocities
+                % h_dot_nom = -2 * (agents(ii).pos - agents(jj).pos) * dVNominal';
+                % b(kk) = -h_dot_nom + obj.barrierGain * hComms(ii, jj)^3;
+
+                kk = kk + 1; 
             end
         end
     end

@miSim/lesserNeighbor.m

@@ -6,12 +6,7 @@ function obj = lesserNeighbor(obj)
         obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
-    % Check possible connections from adjacency matrix
+    % initialize solution with self-connections only
-    % 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
-
     constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
 
     for ii = 1:size(obj.agents, 1)
@@ -34,6 +29,7 @@ function obj = lesserNeighbor(obj)
         subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors);
 
         % Find connected components in each agent's subgraph
+        % TODO: rewrite this using matlab "conncomp" function?
         visited = false(size(subgraphAdjacency, 1), 1);
         components = {};
         for jj = 1:size(subgraphAdjacency, 1)

@miSim/miSim.m

@@ -15,7 +15,8 @@ classdef miSim
         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
+        perf; % sensor performance timeseries array
+        performance = 0; % simulation performance timeseries vector
         barrierGain = 100; % collision avoidance parameter
         minAlt = 1; % minimum allowed altitude constraint
 
@@ -25,7 +26,6 @@ classdef miSim
     properties (Access = private)
         % Sim
         t = NaN; % current sim time
-        perf; % sensor performance timeseries array
         times;
         partitioningTimes;
 
@@ -61,6 +61,7 @@ classdef miSim
         [obj] = plotGraph(obj);
         [obj] = plotTrails(obj);
         [obj] = updatePlots(obj, updatePartitions);
+        validate(obj);
     end
     methods (Access = private)
         [v] = setupVideoWriter(obj);

@miSim/run.m

@@ -18,6 +18,9 @@ function [obj] = run(obj)
         obj.timestepIndex = ii;
         fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
 
+        % Validate current simulation configuration
+        obj.validate();
+
         % Check if it's time for new partitions
         updatePartitions = false;
         if ismember(obj.t, obj.partitioningTimes)

@miSim/updateAdjacency.m

@@ -13,7 +13,7 @@ function obj = updateAdjacency(obj)
     for ii = 2:size(A, 1)
         for jj = 1:(ii - 1)
             % Check that agents are not out of range
-            if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.comRange, obj.agents{jj}.comRange]);
+            if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])
                 A(ii, jj) = false; % comm range violation
                 continue;
             end
@@ -31,6 +31,5 @@ function obj = updateAdjacency(obj)
 
     if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
         warning("Eliminated network connections that were necessary");
-        keyboard
     end
 end

@miSim/validate.m

@@ -0,0 +1,12 @@
+function validate(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+    end
+
+    if max(conncomp(graph(obj.adjacency))) ~= 1
+        warning("Network is not connected");
+    end
+
+end

@sensingObjective/initialize.m

@@ -37,6 +37,7 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     % store ground position
     idx = obj.values == 1;
     obj.groundPos = [obj.X(idx), obj.Y(idx)];
+    obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
 
     assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
 end

geometries/@rectangularPrism/rectangularPrism.m

@@ -3,7 +3,6 @@ classdef rectangularPrism
     properties (SetAccess = private, GetAccess = public)
         % Meta
         tag = REGION_TYPE.INVALID;
-        label = "";
 
         % Spatial
         minCorner = NaN(1, 3);
@@ -27,6 +26,7 @@ classdef rectangularPrism
         dBarrierFunction;
     end
     properties (SetAccess = public, GetAccess = public)
+        label = "";
         % Sensing objective (for DOMAIN region type only)
         objective;
     end

test/test_miSim.m

@@ -491,6 +491,9 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.testClass.run();
         end
         function test_obstacle_avoidance(tc)
+            % Right now this seems to prove that the communications
+            % constraints are working, but the result is dissatisfying
+
             % Fixed single obstacle
             % Fixed two agents initial conditions
             % Exaggerated large collision geometries
@@ -506,30 +509,76 @@ classdef test_miSim < matlab.unittest.TestCase
             d = [3, 0, 0];
             geometry1 = spherical;
             geometry2 = geometry1;
-            geometry1 = geometry1.initialize(tc.domain.center - d + [0, radius * 1.5, 0], radius, REGION_TYPE.COLLISION);
+            geometry1 = geometry1.initialize(tc.domain.center - d + [0.1, radius * 1.1, 0], radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize(tc.domain.center - d - [0, radius * 1.5, 0], radius, REGION_TYPE.COLLISION);
+            geometry2 = geometry2.initialize(tc.domain.center - d - [0.1, radius * 1.1, 0], radius, REGION_TYPE.COLLISION);
             
             % Initialize agent sensor model
             sensor = sigmoidSensor;
             alphaDist = l/2; % half of domain length/width
             sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
-
-            % Initialize agents
-            tc.agents = {agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry1, sensor, 10);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry2, sensor, 10);
             
             % Initialize obstacles
             obstacleLength = 1;
             tc.obstacles{1} = rectangularPrism;
             tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, tc.minAlt; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1");
 
+            % Initialize agents
+            commsRadius = (2*radius + obstacleLength) * 0.9; % defined such that they cannot go around the obstacle on both sides
+            tc.agents = {agent; agent;};
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0.1, radius * 1.1, 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0.1, radius  *1.1, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
+
             % Initialize the simulation
             tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 100, tc.obstacles, tc.makeVideo);
             
             % Run the simulation
             tc.testClass.run();
         end
+        function test_communications_constraint(tc)
+            % No obstacles
+            % Fixed two agents initial conditions
+            % Negligible collision geometries
+            % Non-standard domain with two objectives that will try to pull the
+            % agents apart
+            l = 10; % domain size
+            dom = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            dom.objective = dom.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [2, 8]) + mvnpdf([x(:), y(:)], [8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
+        
+            % Initialize agent collision geometry
+            radius = 0.1;
+            d = [1, 0, 0];
+            geometry1 = spherical;
+            geometry2 = geometry1;
+            geometry1 = geometry1.initialize(dom.center + d, radius, REGION_TYPE.COLLISION);
+            geometry2 = geometry2.initialize(dom.center - d, radius, REGION_TYPE.COLLISION);
+            
+            % Initialize agent sensor model
+            sensor = sigmoidSensor;
+            alphaDist = l/2; % half of domain length/width
+            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
+            
+            % Initialize obstacles
+            tc.obstacles = {};
+            
+            % Initialize agents
+            commsRadius = 4; % defined such that they cannot reach their objective without breaking connectivity
+            tc.agents = {agent; agent;};
+            tc.agents{1} = tc.agents{1}.initialize(dom.center + d, zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
+            tc.agents{2} = tc.agents{2}.initialize(dom.center - d, zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
+
+            % Initialize the simulation
+            tc.testClass = tc.testClass.initialize(dom, dom.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 30, tc.obstacles, false, false);
+            
+            % Run the simulation
+            tc.testClass = tc.testClass.run();
+
+            % Assert that at some step, performance decreased
+            % Indicating that the communications constraint pulled agents
+            % together, away from their objectives
+            tc.verifyTrue(any(diff(tc.testClass.performance) < 0));
+        end
         function test_obstacle_blocks_comms_LOS(tc)
             % Fixed single obstacle
             % Fixed two agents initial conditions