diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 60e9e85..fdff6b5 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -1,4 +1,4 @@ -function obj = initialize(obj, domain, objective, agents, minAlt, timestep, partitoningFreq, maxIter, obstacles, makePlots, makeVideo) +function obj = initialize(obj, domain, objective, agents, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo) arguments (Input) obj (1, 1) {mustBeA(obj, 'miSim')}; domain (1, 1) {mustBeGeometry}; @@ -6,7 +6,6 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part agents (:, 1) cell; minAlt (1, 1) double = 1; timestep (:, 1) double = 0.05; - partitoningFreq (:, 1) double = 0.25 maxIter (:, 1) double = 1000; obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1); makePlots(1, 1) logical = true; @@ -33,7 +32,6 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part % Define domain obj.domain = domain; - obj.partitioningFreq = partitoningFreq; % Add geometries representing obstacles within the domain obj.obstacles = obstacles; @@ -73,7 +71,6 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part % Set up times to iterate over obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)'; - obj.partitioningTimes = obj.times(obj.partitioningFreq:obj.partitioningFreq:size(obj.times, 1)); % Prepare performance data store (at t = 0, all have 0 performance) obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)]; @@ -82,7 +79,7 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1) - 1); % Create initial partitioning - obj = obj.partition(); + obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective); % Initialize variable that will store agent positions for trail plots obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3); diff --git a/@miSim/miSim.m b/@miSim/miSim.m index 3f5687d..80ead18 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -67,7 +67,7 @@ classdef miSim [obj] = plotGraph(obj); [obj] = plotTrails(obj); [obj] = plotH(obj); - [obj] = updatePlots(obj, updatePartitions); + [obj] = updatePlots(obj); validate(obj); end methods (Access = private) diff --git a/@miSim/partition.m b/@miSim/partition.m deleted file mode 100644 index 044a55d..0000000 --- a/@miSim/partition.m +++ /dev/null @@ -1,33 +0,0 @@ -function obj = partition(obj) - arguments (Input) - obj (1, 1) {mustBeA(obj, 'miSim')}; - end - arguments (Output) - obj (1, 1) {mustBeA(obj, 'miSim')}; - end - - % Assess sensing performance of each agent at each sample point - % in the domain - agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [obj.objective.X(:), obj.objective.Y(:), zeros(size(obj.objective.X(:)))]), size(obj.objective.X)), obj.agents, 'UniformOutput', false); - agentPerformances{end + 1} = obj.domain.objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton - agentPerformances = cat(3, agentPerformances{:}); - - % Get highest performance value at each point - [~, idx] = max(agentPerformances, [], 3); - - % Collect agent indices in the same way as performance - indices = 1:size(obj.agents, 1); - agentInds = squeeze(tensorprod(indices, ones(size(obj.objective.X)))); - if size(agentInds, 1) ~= size(obj.agents, 1) - agentInds = reshape(agentInds, [size(obj.agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive - end - agentInds = num2cell(agentInds, 2:3); - agentInds = cellfun(@(x) squeeze(x), agentInds, 'UniformOutput', false); - agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment - agentInds = cat(3, agentInds{:}); - - % Use highest performing agent's index to form partitions - [m, n, ~] = size(agentInds); - [jj, kk] = ndgrid(1:m, 1:n); - obj.partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx)); -end \ No newline at end of file diff --git a/@miSim/run.m b/@miSim/run.m index 7f54a59..78f808d 100644 --- a/@miSim/run.m +++ b/@miSim/run.m @@ -18,19 +18,18 @@ function [obj] = run(obj) obj.timestepIndex = ii; fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1); + % Before moving % Validate current simulation configuration obj.validate(); - % Check if it's time for new partitions - updatePartitions = false; - if ismember(obj.t, obj.partitioningTimes) - updatePartitions = true; - obj = obj.partition(); - end + % Update partitioning before moving (this one is strictly for + % plotting purposes, the real partitioning is done by the agents) + obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective); % Determine desired communications links obj = obj.lesserNeighbor(); + % Moving % 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.timestepIndex, jj, obj.agents); @@ -40,8 +39,7 @@ function [obj] = run(obj) % CBF constraints solved by QP obj = constrainMotion(obj); - % Finished simulation for this timestep, do accounting - + % After moving % Update agent position history array obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3); @@ -52,7 +50,7 @@ function [obj] = run(obj) obj = obj.updateAdjacency(); % Update plots - obj = obj.updatePlots(updatePartitions); + obj = obj.updatePlots(); % Write frame in to video if obj.makeVideo diff --git a/@miSim/updatePlots.m b/@miSim/updatePlots.m index 307d839..a07ffa9 100644 --- a/@miSim/updatePlots.m +++ b/@miSim/updatePlots.m @@ -1,7 +1,6 @@ -function [obj] = updatePlots(obj, updatePartitions) +function [obj] = updatePlots(obj) arguments (Input) obj (1, 1) {mustBeA(obj, 'miSim')}; - updatePartitions (1, 1) logical = false; end arguments (Output) obj (1, 1) {mustBeA(obj, 'miSim')}; @@ -30,10 +29,8 @@ function [obj] = updatePlots(obj, updatePartitions) obj = obj.plotGraph(); % Update partitioning plot - if updatePartitions - delete(obj.partitionPlot); - obj = obj.plotPartitions(); - end + delete(obj.partitionPlot); + obj = obj.plotPartitions(); % reset plot limits to fit domain for ii = 1:size(obj.spatialPlotIndices, 2) diff --git a/test/test_miSim.m b/test/test_miSim.m index e832def..6d79e89 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -210,7 +210,7 @@ classdef test_miSim < matlab.unittest.TestCase end % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles, tc.makeVideo); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makeVideo); end function misim_run(tc) % randomly create obstacles @@ -344,7 +344,7 @@ classdef test_miSim < matlab.unittest.TestCase end % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles, tc.makeVideo); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makeVideo); % Run simulation loop tc.testClass = tc.testClass.run(); @@ -385,10 +385,10 @@ classdef test_miSim < matlab.unittest.TestCase geometry3 = rectangularPrism; geometry3 = geometry3.initialize([tc.domain.center + dh - [0, d, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh - [0, d, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION); tc.agents{3} = agent; - tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + dh - [0, d, 0], zeros(1, 3), 0, 0, geometry3, sensor, 3*d); + tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + dh - [0, d, 0], zeros(1, 3), 0, 0, geometry3, sensor, 3*d, tc.maxIter); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1), false, false); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, cell(0, 1), false, false); tc.verifyEqual(tc.testClass.partitioning(500, 500:502), [2, 3, 1]); % all three near center tc.verifyLessThan(sum(tc.testClass.partitioning == 1, 'all'), sum(tc.testClass.partitioning == 0, 'all')); % more non-assignments than partition 1 assignments @@ -423,7 +423,7 @@ classdef test_miSim < matlab.unittest.TestCase tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], zeros(1,3), 0, 0, geometry1, sensor, 3, tc.maxIter); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1), false, false); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, cell(0, 1), false, false); close(tc.testClass.fPerf); tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]); @@ -435,7 +435,7 @@ classdef test_miSim < matlab.unittest.TestCase tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); % make basic sensing objective - tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [5, 7]), tc.domain, tc.discretizationStep, tc.protectedRange); + tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange); % Initialize agent collision geometry geometry1 = rectangularPrism; @@ -452,11 +452,12 @@ classdef test_miSim < matlab.unittest.TestCase % f = sensor.plotParameters(); % Initialize agents + nIter = 100; tc.agents = {agent}; - tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], zeros(1,3), 0, 0, geometry1, sensor, 3, tc.maxIter); + tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], zeros(1,3), 0, 0, geometry1, sensor, 3, nIter); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1)); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, nIter, cell(0, 1)); % Run the simulation tc.testClass = tc.testClass.run(); @@ -496,7 +497,7 @@ classdef test_miSim < matlab.unittest.TestCase tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, zeros(1,3), 0, 0, geometry2, sensor, 5, nIter); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, nIter, cell(0, 1), tc.makeVideo, tc.makePlots); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, nIter, cell(0, 1), tc.makeVideo, tc.makePlots); % Run the simulation tc.testClass.run(); @@ -545,7 +546,7 @@ classdef test_miSim < matlab.unittest.TestCase tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius *1.1 + yOffset, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius, tc.maxIter); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles, tc.makeVideo); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makeVideo); % Run the simulation tc.testClass.run(); @@ -586,7 +587,7 @@ classdef test_miSim < matlab.unittest.TestCase tc.agents{2} = tc.agents{2}.initialize(dom.center - d, zeros(1,3), 0, 0, geometry2, sensor, commsRadius, nIter); % Initialize the simulation - tc.testClass = tc.testClass.initialize(dom, dom.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, nIter, tc.obstacles, true, false); + tc.testClass = tc.testClass.initialize(dom, dom.objective, tc.agents, tc.minAlt, tc.timestep, nIter, tc.obstacles, true, false); % Run the simulation tc.testClass = tc.testClass.run(); @@ -628,7 +629,7 @@ classdef test_miSim < matlab.unittest.TestCase tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, 0; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1"); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, nIter, tc.obstacles, false, false); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, nIter, tc.obstacles, false, false); % No communications link should be established tc.assertEqual(tc.testClass.adjacency, logical(true(2))); @@ -672,7 +673,7 @@ classdef test_miSim < matlab.unittest.TestCase tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], zeros(1,3), 0, 0, geometry5, sensor, commsRadius, nIter); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, nIter, tc.obstacles, false, false); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, nIter, tc.obstacles, false, false); % Constraint adjacency matrix defined by LNA should be as follows tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ... @@ -725,7 +726,7 @@ classdef test_miSim < matlab.unittest.TestCase tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], zeros(1,3), 0, 0, geometry7, sensor, commsRadius, nIter); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, nIter, tc.obstacles, false, false); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, nIter, tc.obstacles, false, false); % Constraint adjacency matrix defined by LNA should be as follows tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...