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

Compare commits

merge into: kdee:main
Branches Tags
kdee:main kdee:parametric-testing kdee:gradient-ascent kdee:more-cleanup kdee:tessellation-fixes kdee:sensor-model-tessellation
...
pull from: kdee:parametric-testing
Branches Tags
kdee:parametric-testing kdee:gradient-ascent kdee:main kdee:more-cleanup kdee:tessellation-fixes kdee:sensor-model-tessellation

4 Commits
main ... parametric

Author SHA1 Message Date
Kevin D
177a1deeaa test case cleanup 2026-01-14 20:33:44 -08:00
Kevin D
9031c9206e copied agent trail lines to other perspective plots 2026-01-14 19:11:24 -08:00
Kevin D
105d91a492 improved objective function generation 2026-01-14 18:24:14 -08:00
Kevin D
4cce98a717 token commit 2026-01-14 17:54:19 -08:00
7 changed files with 185 additions and 209 deletions
Expand all files Collapse all files

2
@miSim/plotConnections.m
View File

@@ -32,11 +32,9 @@ function obj = plotConnections(obj)
end end
% Copy to other plots % Copy to other plots
if size(obj.spatialPlotIndices, 2) > 1
for ii = 2:size(obj.spatialPlotIndices, 2) for ii = 2:size(obj.spatialPlotIndices, 2)
o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))]; o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
end end
end
obj.connectionsPlot = o; obj.connectionsPlot = o;
end end

8
@miSim/plotTrails.m
View File

@@ -19,8 +19,12 @@ function obj = plotTrails(obj)
hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'off'); hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'off');
end end
% Copy trails to other figures? % Copy to other plots
obj.trailPlot = o; for ii = 2:size(obj.spatialPlotIndices, 2)
o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
end
% Add legend? % Add legend?
obj.trailPlot = o;
end end

8
@miSim/updatePlots.m
View File

@@ -40,10 +40,12 @@ function [obj] = updatePlots(obj)
end end
% Update agent trails % Update agent trails
for jj = 1:size(obj.spatialPlotIndices, 2)
for ii = 1:size(obj.agents, 1) for ii = 1:size(obj.agents, 1)
obj.trailPlot(ii).XData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 1); obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).XData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 1);
obj.trailPlot(ii).YData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 2); obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).YData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 2);
obj.trailPlot(ii).ZData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 3); obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).ZData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 3);
end
end end
drawnow; drawnow;

3
@sensingObjective/initializeRandomMvnpdf.m
View File

@@ -14,13 +14,12 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
while domain.distance(mu) < protectedRange while domain.distance(mu) < protectedRange
mu = domain.random(); mu = domain.random();
end end
mu = mu(1:2);
% Set random distribution parameters % Set random distribution parameters
sig = [2 + rand * 2, 1; 1, 2 + rand * 2]; sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
% Set up random bivariate normal distribution function % Set up random bivariate normal distribution function
objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu, sig); objectiveFunction = objectiveFunctionWrapper(mu(1:2), sig);
% Regular initialization % Regular initialization
obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange); obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange);

2
geometries/@spherical/plotWireframe.m
View File

@@ -28,7 +28,7 @@ function [obj, f] = plotWireframe(obj, ind, f)
o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2); o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
else else
hold(f.Children(1).Children(ind(1)), "on"); hold(f.Children(1).Children(ind(1)), "on");
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2); o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 1);
hold(f.Children(1).Children(ind(1)), "off"); hold(f.Children(1).Children(ind(1)), "off");
end end

357
test/test_miSim.m
View File

@@ -49,9 +49,12 @@ classdef test_miSim < matlab.unittest.TestCase
alphaDistMax = 3; alphaDistMax = 3;
alphaTiltMin = 15; % degrees alphaTiltMin = 15; % degrees
alphaTiltMax = 30; % degrees alphaTiltMax = 30; % degrees
sensor = sigmoidSensor;
% Communications % Communications
comRange = 8; % Maximum range between agents that forms a communications link minCommsRange = 3; % Minimum randomly generated collision geometry size
maxCommsRange = 5; % Maximum randomly generated collision geometry size
commsRanges = NaN;
% Constraints % Constraints
barrierGain = 100; barrierGain = 100;
@@ -75,8 +78,11 @@ classdef test_miSim < matlab.unittest.TestCase
tc.agents{ii, 1} = agent; tc.agents{ii, 1} = agent;
end end
% Define random collision ranges for each agent % Random collision ranges for each agent
tc.collisionRanges = tc.minCollisionRange + rand(size(tc.agents, 1), 1) * (tc.maxCollisionRange - tc.minCollisionRange); tc.collisionRanges = tc.minCollisionRange + rand(size(tc.agents, 1), 1) * (tc.maxCollisionRange - tc.minCollisionRange);
% Random commuunications ranges for each agent
tc.commsRanges = tc.minCommsRange + rand(size(tc.agents, 1), 1) * (tc.maxCommsRange - tc.minCommsRange);
end end
end end
@@ -161,11 +167,10 @@ classdef test_miSim < matlab.unittest.TestCase
candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION); candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION);
% Initialize candidate agent sensor model % Initialize candidate agent sensor model
sensor = sigmoidSensor; tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
% Initialize candidate agent % Initialize candidate agent
newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, sensor, tc.comRange, tc.maxIter, tc.initialStepSize); newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.comRange, tc.maxIter, tc.initialStepSize);
% Make sure candidate agent doesn't collide with % Make sure candidate agent doesn't collide with
% domain % domain
@@ -213,7 +218,7 @@ classdef test_miSim < matlab.unittest.TestCase
end end
% Initialize the simulation % Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makeVideo); tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
end end
function misim_run(tc) function misim_run(tc)
% randomly create obstacles % randomly create obstacles
@@ -295,11 +300,10 @@ classdef test_miSim < matlab.unittest.TestCase
candidateGeometry = candidateGeometry.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION); candidateGeometry = candidateGeometry.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);
% Initialize candidate agent sensor model % Initialize candidate agent sensor model
sensor = sigmoidSensor; tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
% Initialize candidate agent % Initialize candidate agent
newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, sensor, tc.comRange, tc.maxIter, tc.initialStepSize); newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.comRange, tc.maxIter, tc.initialStepSize);
% Make sure candidate agent doesn't collide with % Make sure candidate agent doesn't collide with
% domain % domain
@@ -347,7 +351,7 @@ classdef test_miSim < matlab.unittest.TestCase
end end
% Initialize the simulation % Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makeVideo); tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
% Write out parameters % Write out parameters
tc.testClass.writeParams(); tc.testClass.writeParams();
@@ -359,77 +363,54 @@ classdef test_miSim < matlab.unittest.TestCase
% place agents a fixed distance +/- X from the domain's center % place agents a fixed distance +/- X from the domain's center
d = 1; d = 1;
% make basic domain
tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective
tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2)), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry % Initialize agent collision geometry
dh = [0,0,-1]; % bias agent altitude from domain center tc.agents = {agent; agent; agent};
geometry1 = rectangularPrism; geometry1 = spherical;
geometry2 = geometry1; geometry2 = geometry1;
geometry1 = geometry1.initialize([tc.domain.center + dh + [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh + [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION); geometry3 = geometry1;
geometry2 = geometry2.initialize([tc.domain.center + dh - [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh - [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION); geometry1 = geometry1.initialize(tc.domain.center + [d, 0, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
geometry2 = geometry2.initialize(tc.domain.center - [d, 0, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
geometry3 = geometry3.initialize(tc.domain.center - [0, d, 0], tc.collisionRanges(3), REGION_TYPE.COLLISION);
% Initialize agent sensor model % Initialize agent sensor model with fixed parameters
sensor = sigmoidSensor; tc.sensor = tc.sensor.initialize(tc.domain.maxCorner(3) / 2, 9, 22.5, 9);
% Homogeneous sensor model parameters
sensor = sensor.initialize(2.75, 9, 22.5, 9);
% Heterogeneous sensor model parameters
% sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
% Plot sensor parameters (optional)
% f = sensor.plotParameters();
% Initialize agents % Initialize agents
tc.agents = {agent; agent}; tc.commsRanges = 3 * d * ones(size(tc.agents));
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + dh + [d, 0, 0], geometry1, sensor, 3*d, tc.maxIter, tc.initialStepSize); tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + dh - [d, 0, 0], geometry2, sensor, 3*d, tc.maxIter, tc.initialStepSize); tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
tc.agents{3} = tc.agents{3}.initialize(tc.domain.center - [0, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize);
% Optional third agent along the +Y axis
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], geometry3, sensor, 3*d, tc.maxIter, tc.initialStepSize);
% Initialize the simulation % Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, cell(0, 1), false, false); tc.obstacles = cell(0, 1);
tc.makePlots = false;
tc.makeVideo = false;
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
tc.verifyEqual(tc.testClass.partitioning(500, 500:502), [2, 3, 1]); % all three near center centerIdx = floor(size(tc.testClass.partitioning, 1) / 2);
tc.verifyEqual(tc.testClass.partitioning(centerIdx, centerIdx:(centerIdx + 2)), [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 tc.verifyLessThan(sum(tc.testClass.partitioning == 1, 'all'), sum(tc.testClass.partitioning == 0, 'all')); % more non-assignments than partition 1 assignments
tc.verifyLessThan(sum(tc.testClass.partitioning == 2, 'all'), sum(tc.testClass.partitioning == 1, 'all')); % more partition 1 assignments than partition 2 assignments tc.verifyLessThan(sum(tc.testClass.partitioning == 2, 'all'), sum(tc.testClass.partitioning == 1, 'all')); % more partition 1 assignments than partition 2 assignments
tc.verifyLessThan(sum(tc.testClass.partitioning == 3, 'all'), sum(tc.testClass.partitioning == 2, 'all')); % more partition 3 assignments than partition 2 assignments tc.verifyLessThan(sum(tc.testClass.partitioning == 3, 'all'), sum(tc.testClass.partitioning == 2, 'all')); % more partition 3 assignments than partition 2 assignments
tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1; 2; 3;]); tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1; 2; 3;]);
end end
function test_single_partition(tc) function test_single_partition(tc)
% make basic domain
l = 10; % domain size
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(:)], tc.domain.center(1:2) + rand(1, 2) * 6 - 3), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry % Initialize agent collision geometry
geometry1 = rectangularPrism; tc.agents = {agent};
geometry1 = geometry1.initialize([[tc.domain.center(1:2), 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2), 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION); geometry1 = spherical;
geometry1 = geometry1.initialize([tc.domain.center(1:2), 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
% Initialize agent sensor model % Initialize agent sensor model with fixed parameters
sensor = sigmoidSensor; tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);
% Homogeneous sensor model parameters
% sensor = sensor.initialize(2.5666, 5.0807, 20.8614, 13); % 13
alphaDist = l/2; % half of domain length/width
sensor = sensor.initialize(alphaDist, 3, 20, 3);
% Plot sensor parameters (optional)
% f = sensor.plotParameters();
% Initialize agents % Initialize agents
tc.agents = {agent}; tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], geometry1, sensor, 3, tc.maxIter, tc.initialStepSize);
% Initialize the simulation % Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, cell(0, 1), false, false); tc.obstacles = cell(0, 1);
tc.makePlots = false;
tc.makeVideo = false;
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
close(tc.testClass.fPerf); close(tc.testClass.fPerf);
tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]); tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]);
@@ -437,73 +418,62 @@ classdef test_miSim < matlab.unittest.TestCase
end end
function test_single_agent_gradient_ascent(tc) function test_single_agent_gradient_ascent(tc)
% make basic domain % make basic domain
l = 10; % domain size tc.minDimension = 10; % domain size
tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective % make basic sensing objective
tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange); tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry % Initialize agent collision geometry
tc.agents = {agent};
geometry1 = rectangularPrism; geometry1 = rectangularPrism;
geometry1 = geometry1.initialize([[tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION); geometry1 = geometry1.initialize([[tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION);
% Initialize agent sensor model % Initialize agent sensor model with fixed parameters
sensor = sigmoidSensor; tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);
% Homogeneous sensor model parameters
% sensor = sensor.initialize(2.5666, 5.0807, 20.8614, 13); % 13
alphaDist = l/2; % half of domain length/width
sensor = sensor.initialize(alphaDist, 3, 20, 3);
% Plot sensor parameters (optional)
% f = sensor.plotParameters();
% Initialize agents % Initialize agents
nIter = 75; tc.maxIter = 75;
tc.agents = {agent}; tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, sensor, 3, nIter, tc.initialStepSize);
% Initialize the simulation % Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, nIter, cell(0, 1)); tc.obstacles = cell(0, 1);
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
% Run the simulation % Run the simulation
tc.testClass = tc.testClass.run(); tc.testClass = tc.testClass.run();end
% tc.verifyGreaterThan(tc.testClass.performance(end)/max(tc.testClass.performance), 0.99); % ends up very near a relative maximum
end
function test_collision_avoidance(tc) function test_collision_avoidance(tc)
% No obstacles % No obstacles
% Fixed agent initial conditions % Fixed agent initial conditions
% Exaggerated large collision geometries to test CA % Exaggerated large collision geometries to test CA
% make basic domain % make basic domain
l = 10; % domain size tc.minDimension = 10; % domain size
tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective % make basic sensing objective
tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [3, 7]), tc.domain, tc.discretizationStep, tc.protectedRange); tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([3, 7]), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry % Initialize agent collision geometry
radius = 1.5; tc.agents = {agent; agent};
tc.collisionRanges = 1.5 * ones(size(tc.agents));
d = [2.5, 0, 0]; d = [2.5, 0, 0];
geometry1 = spherical; geometry1 = spherical;
geometry2 = spherical; geometry2 = spherical;
geometry1 = geometry1.initialize(tc.domain.center + d, radius, REGION_TYPE.COLLISION); geometry1 = geometry1.initialize(tc.domain.center + d, tc.collisionRanges(1), REGION_TYPE.COLLISION);
geometry2 = geometry2.initialize(tc.domain.center - d, radius, REGION_TYPE.COLLISION); geometry2 = geometry2.initialize(tc.domain.center - d, tc.collisionRanges(2), REGION_TYPE.COLLISION);
% Initialize agent sensor model % Initialize agent sensor model with fixed parameters
sensor = sigmoidSensor; tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
% Homogeneous sensor model parameters
% sensor = sensor.initialize(2.5666, 5.0807, 20.8614, 13); % 13
alphaDist = l/2; % half of domain length/width
sensor = sensor.initialize(alphaDist, 3, 15, 3);
% Initialize agents % Initialize agents
nIter = 25; tc.maxIter = 25;
tc.agents = {agent; agent}; tc.commsRanges = 5 * ones(size(tc.agents));
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, sensor, 5, nIter, tc.initialStepSize); tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, sensor, 5, nIter, tc.initialStepSize); tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
% Initialize the simulation % Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, nIter, cell(0, 1), tc.makeVideo, tc.makePlots); tc.obstacles = cell(0, 1);
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
% Run the simulation % Run the simulation
tc.testClass.run(); tc.testClass.run();
@@ -515,14 +485,15 @@ classdef test_miSim < matlab.unittest.TestCase
% Fixed two agents initial conditions % Fixed two agents initial conditions
% Exaggerated large collision geometries % Exaggerated large collision geometries
% make basic domain % make basic domain
l = 10; % domain size tc.minDimension = 10; % domain size
tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective % make basic sensing objective
tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [8, 5.2195]), tc.domain, tc.discretizationStep, tc.protectedRange); tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5.2195]), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry % Initialize agent collision geometry
radius = 1.1; tc.agents = {agent; agent;};
tc.collisionRanges = 1.1 * ones(size(tc.agents));
d = [3, 0, 0]; d = [3, 0, 0];
yOffset = 1; yOffset = 1;
@@ -531,13 +502,11 @@ classdef test_miSim < matlab.unittest.TestCase
geometry1 = spherical; geometry1 = spherical;
geometry2 = geometry1; geometry2 = geometry1;
geometry1 = geometry1.initialize(tc.domain.center - d + [0, radius * 1.1 - yOffset, 0], radius, REGION_TYPE.COLLISION); geometry1 = geometry1.initialize(tc.domain.center - d + [0, tc.collisionRanges(1) * 1.1 - yOffset, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
geometry2 = geometry2.initialize(tc.domain.center - d - [0, radius * 1.1 + yOffset, 0], radius, REGION_TYPE.COLLISION); geometry2 = geometry2.initialize(tc.domain.center - d - [0, tc.collisionRanges(2) * 1.1 + yOffset, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
% Initialize agent sensor model % Initialize agent sensor model with fixed parameters
sensor = sigmoidSensor; tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
alphaDist = l/2; % half of domain length/width
sensor = sensor.initialize(alphaDist, 3, 15, 3);
% Initialize obstacles % Initialize obstacles
obstacleLength = 1; obstacleLength = 1;
@@ -545,12 +514,12 @@ classdef test_miSim < matlab.unittest.TestCase
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"); 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 % Initialize agents
commsRadius = (2*radius + obstacleLength) * 0.9; % defined such that they cannot go around the obstacle on both sides tc.commsRanges = (2 * tc.collisionRanges(1) + obstacleLength) * 0.9 * ones(size(tc.agents)); % 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, tc.collisionRanges(1) * 1.1 - yOffset, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.1 - yOffset, 0], geometry1, sensor, commsRadius, tc.maxIter, tc.initialStepSize); tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, tc.collisionRanges(2) *1.1 + yOffset, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius *1.1 + yOffset, 0], geometry2, sensor, commsRadius, tc.maxIter, tc.initialStepSize);
% Initialize the simulation % Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makeVideo); tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
% Run the simulation % Run the simulation
tc.testClass.run(); tc.testClass.run();
@@ -561,37 +530,36 @@ classdef test_miSim < matlab.unittest.TestCase
% Negligible collision geometries % Negligible collision geometries
% Non-standard domain with two objectives that will try to pull the % Non-standard domain with two objectives that will try to pull the
% agents apart % agents apart
l = 10; % domain size tc.minDimension = 10; % domain size
dom = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective % 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); tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([2, 8; 8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry % Initialize agent collision geometry
radius = 0.1; tc.agents = {agent; agent;};
tc.collisionRanges = .25 * ones(size(tc.agents));
d = [1, 0, 0]; d = [1, 0, 0];
geometry1 = spherical; geometry1 = spherical;
geometry2 = geometry1; geometry2 = geometry1;
geometry1 = geometry1.initialize(dom.center + d, radius, REGION_TYPE.COLLISION); geometry1 = geometry1.initialize(tc.domain.center + d, tc.collisionRanges(1), REGION_TYPE.COLLISION);
geometry2 = geometry2.initialize(dom.center - d, radius, REGION_TYPE.COLLISION); geometry2 = geometry2.initialize(tc.domain.center - d, tc.collisionRanges(2), REGION_TYPE.COLLISION);
% Initialize agent sensor model % Initialize agent sensor model
sensor = sigmoidSensor; tc.sensor = sigmoidSensor;
alphaDist = l/2; % half of domain length/width tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
sensor = sensor.initialize(alphaDist, 3, 15, 3);
% Initialize obstacles % Initialize obstacles
tc.obstacles = {}; tc.obstacles = {};
% Initialize agents % Initialize agents
nIter = 50; tc.maxIter = 50;
commsRadius = 4; % defined such that they cannot reach their objective without breaking connectivity tc.commsRanges = 4 * ones(size(tc.agents)); % defined such that they cannot reach their objective without breaking connectivity
tc.agents = {agent; agent;}; tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
tc.agents{1} = tc.agents{1}.initialize(dom.center + d, geometry1, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
tc.agents{2} = tc.agents{2}.initialize(dom.center - d, geometry2, sensor, commsRadius, nIter, tc.initialStepSize);
% Initialize the simulation % Initialize the simulation
tc.testClass = tc.testClass.initialize(dom, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, nIter, tc.obstacles, true, false); tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
% Run the simulation % Run the simulation
tc.testClass = tc.testClass.run(); tc.testClass = tc.testClass.run();
@@ -601,31 +569,29 @@ classdef test_miSim < matlab.unittest.TestCase
% Fixed two agents initial conditions % Fixed two agents initial conditions
% Exaggerated large communications radius % Exaggerated large communications radius
% make basic domain % make basic domain
l = 10; % domain size tc.minDimension = 10; % domain size
tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); tc.domain = tc.domain.initialize([zeros(1, 3); tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective % make basic sensing objective
tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange); tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry % Initialize agent collision geometry
radius = .25; tc.agents = {agent; agent;};
tc.collisionRanges = .25 * ones(size(tc.agents));
d = 2; d = 2;
geometry1 = spherical; geometry1 = spherical;
geometry2 = geometry1; geometry2 = geometry1;
geometry1 = geometry1.initialize(tc.domain.center - [d, 0, 0], radius, REGION_TYPE.COLLISION); geometry1 = geometry1.initialize(tc.domain.center - [d, 0, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
geometry2 = geometry2.initialize(tc.domain.center - [0, d, 0], radius, REGION_TYPE.COLLISION); geometry2 = geometry2.initialize(tc.domain.center - [0, d, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
% Initialize agent sensor model % Initialize agent sensor model
sensor = sigmoidSensor; tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
alphaDist = l/2; % half of domain length/width
sensor = sensor.initialize(alphaDist, 3, 15, 3);
% Initialize agents % Initialize agents
nIter = 125; tc.maxIter = 125;
commsRadius = 5; tc.commsRanges = 5 * ones(size(tc.agents));
tc.agents = {agent; agent;}; tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], geometry1, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], geometry2, sensor, commsRadius, nIter, tc.initialStepSize);
% Initialize obstacles % Initialize obstacles
obstacleLength = 1.5; obstacleLength = 1.5;
@@ -633,51 +599,55 @@ 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"); 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 % Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, 0, tc.timestep, nIter, tc.obstacles, false, false); tc.minAlt = 0;
tc.makePlots = false;
tc.makeVideo = false;
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
% No communications link should be established % Communications link should be established
tc.assertEqual(tc.testClass.adjacency, logical(true(2))); tc.assertEqual(tc.testClass.adjacency, logical(true(2)));
end end
function test_LNA_case_1(tc) function test_LNA_case_1(tc)
% based on example in meeting % based on example in meeting
% No obstacles % no obstacles
% Fixed 5 agents initial conditions % fixed 5 agents initial conditions
% unitary communicaitons radius % unit communicaitons radius
% negligible collision radius % negligible collision radius
% make basic domain % make basic domain
l = 10; % domain size tc.minDimension = 10; % domain size
tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective % make basic sensing objective
tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange); tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry % Initialize agent collision geometry
radius = .01; tc.agents = {agent; agent; agent; agent; agent;};
tc.collisionRanges = .01 * ones(size(tc.agents));
d = 1; d = 1;
geometry5 = spherical; geometry5 = spherical;
geometry1 = geometry5.initialize(tc.domain.center + [d, 0, 0], radius, REGION_TYPE.COLLISION); geometry1 = geometry5.initialize(tc.domain.center + [d, 0, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
geometry2 = geometry5.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION); geometry2 = geometry5.initialize(tc.domain.center, tc.collisionRanges(2), REGION_TYPE.COLLISION);
geometry3 = geometry5.initialize(tc.domain.center + [-d, d, 0], radius, REGION_TYPE.COLLISION); geometry3 = geometry5.initialize(tc.domain.center + [-d, d, 0], tc.collisionRanges(3), REGION_TYPE.COLLISION);
geometry4 = geometry5.initialize(tc.domain.center + [-2*d, d, 0], radius, REGION_TYPE.COLLISION); geometry4 = geometry5.initialize(tc.domain.center + [-2*d, d, 0], tc.collisionRanges(4), REGION_TYPE.COLLISION);
geometry5 = geometry5.initialize(tc.domain.center + [0, d, 0], radius, REGION_TYPE.COLLISION); geometry5 = geometry5.initialize(tc.domain.center + [0, d, 0], tc.collisionRanges(5), REGION_TYPE.COLLISION);
% Initialize agent sensor model % Initialize agent sensor model
sensor = sigmoidSensor; tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
alphaDist = l/2; % half of domain length/width
sensor = sensor.initialize(alphaDist, 3, 15, 3);
% Initialize agents % Initialize agents
nIter = 125; tc.maxIter = 125;
commsRadius = d; tc.commsRanges = ones(size(tc.agents));
tc.agents = {agent; agent; agent; agent; agent;}; tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, geometry2, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [-d, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize);
tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [-d, d, 0], geometry3, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [-2*d, d, 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize);
tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [-2*d, d, 0], geometry4, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize);
tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], geometry5, sensor, commsRadius, nIter, tc.initialStepSize);
% Initialize the simulation % Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, 0, tc.timestep, nIter, tc.obstacles, false, false); tc.minAlt = 0;
tc.makePlots = false;
tc.makeVideo = false;
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
% Constraint adjacency matrix defined by LNA should be as follows % Constraint adjacency matrix defined by LNA should be as follows
tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ... tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
@@ -694,43 +664,44 @@ classdef test_miSim < matlab.unittest.TestCase
% unitary communicaitons radius % unitary communicaitons radius
% negligible collision radius % negligible collision radius
% make basic domain % make basic domain
l = 10; % domain size tc.minDimension = 10; % domain size
tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); tc.domain = tc.domain.initialize([zeros(1, 3); tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective % make basic sensing objective
tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange); tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry % Initialize agent collision geometry
radius = .01; tc.agents = {agent; agent; agent; agent; agent; agent; agent;};
tc.collisionRanges = .01 * ones(size(tc.agents));
d = 1; d = 1;
geometry7 = spherical; geometry7 = spherical;
geometry1 = geometry7.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], radius, REGION_TYPE.COLLISION); geometry1 = geometry7.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
geometry2 = geometry7.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], radius, REGION_TYPE.COLLISION); geometry2 = geometry7.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
geometry3 = geometry7.initialize(tc.domain.center + [0.9 * d, 0, 0], radius, REGION_TYPE.COLLISION); geometry3 = geometry7.initialize(tc.domain.center + [0.9 * d, 0, 0], tc.collisionRanges(3), REGION_TYPE.COLLISION);
geometry4 = geometry7.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], radius, REGION_TYPE.COLLISION); geometry4 = geometry7.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], tc.collisionRanges(4), REGION_TYPE.COLLISION);
geometry5 = geometry7.initialize(tc.domain.center + [0, 0.9 * d, 0], radius, REGION_TYPE.COLLISION); geometry5 = geometry7.initialize(tc.domain.center + [0, 0.9 * d, 0], tc.collisionRanges(5), REGION_TYPE.COLLISION);
geometry6 = geometry7.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION); geometry6 = geometry7.initialize(tc.domain.center, tc.collisionRanges(6), REGION_TYPE.COLLISION);
geometry7 = geometry7.initialize(tc.domain.center + [d/2, d/2, 0], radius, REGION_TYPE.COLLISION); geometry7 = geometry7.initialize(tc.domain.center + [d/2, d/2, 0], tc.collisionRanges(7), REGION_TYPE.COLLISION);
% Initialize agent sensor model % Initialize agent sensor model
sensor = sigmoidSensor; tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
alphaDist = l/2; % half of domain length/width
sensor = sensor.initialize(alphaDist, 3, 15, 3);
% Initialize agents % Initialize agents
nIter = 125; tc.maxIter = 125;
commsRadius = d; tc.commsRanges = d * ones(size(tc.agents));
tc.agents = {agent; agent; agent; agent; agent; agent; agent;}; tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], geometry1, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], geometry2, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize);
tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], geometry3, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize);
tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], geometry4, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize);
tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], geometry5, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, geometry6, tc.sensor, tc.commsRanges(6), tc.maxIter, tc.initialStepSize);
tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, geometry6, sensor, commsRadius, nIter, tc.initialStepSize); tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], geometry7, tc.sensor, tc.commsRanges(7), tc.maxIter, tc.initialStepSize);
tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], geometry7, sensor, commsRadius, nIter, tc.initialStepSize);
% Initialize the simulation % Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, 0, tc.timestep, nIter, tc.obstacles, false, false); tc.minAlt = 0;
tc.makePlots = false;
tc.makeVideo = false;
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
% Constraint adjacency matrix defined by LNA should be as follows % Constraint adjacency matrix defined by LNA should be as follows
tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ... tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...

8
util/objectiveFunctionWrapper.m
View File

@@ -1,13 +1,15 @@
function f = objectiveFunctionWrapper(center) function f = objectiveFunctionWrapper(center, sigma)
% Convenience function to generate MVNPDFs at a point % Convenience function to generate MVNPDFs at a point
% Makes it look a lot neater to instantiate and sum these to make % Makes it look a lot neater to instantiate and sum these to make
% composite objectives in particular % composite objectives in particular
arguments (Input) arguments (Input)
center (1, 2) double; center (:, 2) double;
sigma (2, 2) double = eye(2);
end end
arguments (Output) arguments (Output)
f (1, 1) {mustBeA(f, 'function_handle')}; f (1, 1) {mustBeA(f, 'function_handle')};
end end
f = @(x, y) mvnpdf([x(:), y(:)], center); f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), sigma), 1:size(center,1), 'UniformOutput', false)), 2);
end end