kdee/miSim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

now doing partitioning on every timestep, looks super smooth

Browse Source
This commit is contained in:
Kevin Dee
2026-01-11 19:09:32 -08:00
parent 2a48b1d469
commit ff02e8a1c6
6 changed files with 28 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
@miSim/initialize.m
View File

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

2
@miSim/miSim.m
View File

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

33
@miSim/partition.m
View File

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

16
@miSim/run.m
View File

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

5
@miSim/updatePlots.m
View File

@@ -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
% reset plot limits to fit domain
for ii = 1:size(obj.spatialPlotIndices, 2)

29
test/test_miSim.m
View File

@@ -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( ...