From 2604711c7864352c9856a5fa5c138a63d4366a63 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Tue, 13 Jan 2026 23:16:41 -0800 Subject: [PATCH] fixed unit tests --- @agent/agent.m | 10 +- @agent/initialize.m | 4 +- @miSim/constrainMotion.m | 24 ++--- @miSim/initialize.m | 19 +++- @miSim/miSim.m | 9 +- @miSim/plot.m | 3 + @miSim/setupVideoWriter.m | 4 +- @miSim/validate.m | 10 +- @miSim/writeParams.m | 25 +++++ @sensingObjective/sensingObjective.m | 12 +-- .../@rectangularPrism/initializeRandom.m | 2 +- .../BtZvZtLyHugR7nfGjg6ukY77EG0d.xml | 6 ++ .../BtZvZtLyHugR7nfGjg6ukY77EG0p.xml | 2 + test/parametricTestSuite.m | 25 +++-- test/test_miSim.m | 94 ++++++++++--------- 15 files changed, 160 insertions(+), 89 deletions(-) create mode 100644 @miSim/writeParams.m create mode 100644 resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/BtZvZtLyHugR7nfGjg6ukY77EG0d.xml create mode 100644 resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/BtZvZtLyHugR7nfGjg6ukY77EG0p.xml diff --git a/@agent/agent.m b/@agent/agent.m index 3f91e3d..88fe5f6 100644 --- a/@agent/agent.m +++ b/@agent/agent.m @@ -3,13 +3,13 @@ classdef agent % Identifiers label = ""; - % Sensor - sensorModel; - % State lastPos = NaN(1, 3); % position from previous timestep pos = NaN(1, 3); % current position + % Sensor + sensorModel; + % Collision collisionGeometry; @@ -17,10 +17,10 @@ classdef agent fovGeometry; % Communication - comRange = NaN; commsGeometry = spherical; lesserNeighbors = []; + % Performance performance = 0; % Plotting @@ -29,7 +29,7 @@ classdef agent end properties (SetAccess = private, GetAccess = public) - initialStepSize = 0.2; + initialStepSize = NaN; stepDecayRate = NaN; end diff --git a/@agent/initialize.m b/@agent/initialize.m index 2bcff5d..3bac576 100644 --- a/@agent/initialize.m +++ b/@agent/initialize.m @@ -1,4 +1,4 @@ -function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, label, plotCommsGeometry) +function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry) arguments (Input) obj (1, 1) {mustBeA(obj, 'agent')}; pos (1, 3) double; @@ -6,6 +6,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma sensorModel (1, 1) {mustBeSensor}; comRange (1, 1) double; maxIter (1, 1) double; + initialStepSize (1, 1) double = 0.2; label (1, 1) string = ""; plotCommsGeometry (1, 1) logical = false; end @@ -18,6 +19,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma obj.sensorModel = sensorModel; obj.label = label; obj.plotCommsGeometry = plotCommsGeometry; + obj.initialStepSize = initialStepSize; obj.stepDecayRate = obj.initialStepSize / maxIter; % Initialize performance vector diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index d20575e..8fd60b5 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -14,7 +14,7 @@ function [obj] = constrainMotion(obj) agents = [obj.agents{:}]; v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))'; - if all(isnan(v)) || all(v == zeros(1, 3)) + if all(isnan(v), 'all') || all(v == zeros(size(obj.agents, 1), 3), 'all') % Agents are not attempting to move, so there is no motion to be % constrained return; @@ -39,7 +39,7 @@ function [obj] = constrainMotion(obj) 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 * h(ii, jj)^3; + b(kk) = obj.barrierGain * h(ii, jj)^obj.barrierExponent; kk = kk + 1; end end @@ -54,7 +54,7 @@ function [obj] = constrainMotion(obj) hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2; A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos); - b(kk) = obj.barrierGain * hObs(ii, jj)^3; + b(kk) = obj.barrierGain * hObs(ii, jj)^obj.barrierExponent; kk = kk + 1; end @@ -67,37 +67,37 @@ function [obj] = constrainMotion(obj) % X minimum h_xMin = (agents(ii).pos(1) - obj.domain.minCorner(1)) - agents(ii).collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0]; - b(kk) = obj.barrierGain * h_xMin^3; + b(kk) = obj.barrierGain * h_xMin^obj.barrierExponent; kk = kk + 1; % X maximum h_xMax = (obj.domain.maxCorner(1) - agents(ii).pos(1)) - agents(ii).collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0]; - b(kk) = obj.barrierGain * h_xMax^3; + b(kk) = obj.barrierGain * h_xMax^obj.barrierExponent; kk = kk + 1; % Y minimum h_yMin = (agents(ii).pos(2) - obj.domain.minCorner(2)) - agents(ii).collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0]; - b(kk) = obj.barrierGain * h_yMin^3; + b(kk) = obj.barrierGain * h_yMin^obj.barrierExponent; kk = kk + 1; % Y maximum h_yMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0]; - b(kk) = obj.barrierGain * h_yMax^3; + b(kk) = obj.barrierGain * h_yMax^obj.barrierExponent; kk = kk + 1; % Z minimum h_zMin = (agents(ii).pos(3) - obj.domain.minCorner(3)) - agents(ii).collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1]; - b(kk) = obj.barrierGain * h_zMin^3; + b(kk) = obj.barrierGain * h_zMin^obj.barrierExponent; kk = kk + 1; % Z maximum h_zMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1]; - b(kk) = obj.barrierGain * h_zMax^3; + b(kk) = obj.barrierGain * h_zMax^obj.barrierExponent; kk = kk + 1; end @@ -114,11 +114,7 @@ function [obj] = constrainMotion(obj) 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); - - % 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; + b(kk) = obj.barrierGain * hComms(ii, jj)^obj.barrierExponent; kk = kk + 1; end diff --git a/@miSim/initialize.m b/@miSim/initialize.m index e12cfac..f26a6b3 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -1,8 +1,10 @@ -function obj = initialize(obj, domain, agents, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo) +function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo) arguments (Input) obj (1, 1) {mustBeA(obj, 'miSim')}; domain (1, 1) {mustBeGeometry}; agents (:, 1) cell; + barrierGain (1, 1) double = 100; + barrierExponent (1, 1) double = 3; minAlt (1, 1) double = 1; timestep (:, 1) double = 0.05; maxIter (:, 1) double = 1000; @@ -24,6 +26,9 @@ function obj = initialize(obj, domain, agents, minAlt, timestep, maxIter, obstac end obj.makeVideo = makeVideo; + % Generate artifact(s) name + obj.artifactName = strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss')); + % Define simulation time parameters obj.timestep = timestep; obj.timestepIndex = 0; @@ -37,10 +42,9 @@ function obj = initialize(obj, domain, agents, minAlt, timestep, maxIter, obstac % Add an additional obstacle spanning the domain's footprint to % represent the minimum allowable altitude - obj.minAlt = minAlt; - if obj.minAlt > 0 + if minAlt > 0 obj.obstacles{end + 1, 1} = rectangularPrism; - obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint"); + obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint"); end % Define agents @@ -61,6 +65,10 @@ function obj = initialize(obj, domain, agents, minAlt, timestep, maxIter, obstac end end + % Set CBF parameters + obj.barrierGain = barrierGain; + obj.barrierExponent = barrierExponent; + % Compute adjacency matrix and lesser neighbors obj = obj.updateAdjacency(); obj = obj.lesserNeighbor(); @@ -83,4 +91,7 @@ function obj = initialize(obj, domain, agents, minAlt, timestep, maxIter, obstac % Set up plots showing initialized state obj = obj.plot(); + + % Run validations + obj.validate(); end \ No newline at end of file diff --git a/@miSim/miSim.m b/@miSim/miSim.m index 3dce005..577a0b8 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -5,7 +5,6 @@ classdef miSim properties (SetAccess = private, GetAccess = public) timestep = NaN; % delta time interval for simulation iterations timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays) - partitioningFreq = NaN; % number of simulation timesteps at which the partitioning routine is re-run maxIter = NaN; % maximum number of simulation iterations domain = rectangularPrism; objective = sensingObjective; @@ -16,9 +15,9 @@ classdef miSim partitioning = NaN; perf; % sensor performance timeseries array performance = 0; % simulation performance timeseries vector - barrierGain = 100; % collision avoidance parameter - minAlt = 1; % minimum allowed altitude constraint - + barrierGain = 100; % CBF gain parameter + barrierExponent = 3; % CBF exponent parameter + artifactName = ""; fPerf; % performance plot figure end @@ -55,7 +54,7 @@ classdef miSim end methods (Access = public) - [obj] = initialize(obj, domain, agents, timestep, partitoningFreq, maxIter, obstacles); + [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo); [obj] = run(obj); [obj] = lesserNeighbor(obj); [obj] = constrainMotion(obj); diff --git a/@miSim/plot.m b/@miSim/plot.m index 52e1d3b..c494365 100644 --- a/@miSim/plot.m +++ b/@miSim/plot.m @@ -51,4 +51,7 @@ function obj = plot(obj) % Plot h functions obj = obj.plotH(); + + % Switch back to primary figure + figure(obj.f); end \ No newline at end of file diff --git a/@miSim/setupVideoWriter.m b/@miSim/setupVideoWriter.m index b7f23d7..af5d797 100644 --- a/@miSim/setupVideoWriter.m +++ b/@miSim/setupVideoWriter.m @@ -7,9 +7,9 @@ function v = setupVideoWriter(obj) end if ispc || ismac - v = VideoWriter(fullfile('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'MPEG-4'); + v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'MPEG-4'); elseif isunix - v = VideoWriter(fullfile('.', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'Motion JPEG AVI'); + v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'Motion JPEG AVI'); end v.FrameRate = 1 / obj.timestep; diff --git a/@miSim/validate.m b/@miSim/validate.m index 3b66ac6..c350c14 100644 --- a/@miSim/validate.m +++ b/@miSim/validate.m @@ -14,6 +14,14 @@ function validate(obj) warning("Eliminated network connections that were necessary"); end - %% + %% Obstacle Validators + AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obj.obstacles), obj.agents, 'UniformOutput', false); + AO_collisions = vertcat(AO_collisions{:}); + if any(AO_collisions) + [idx, idy] = find(AO_collisions); + for ii = 1:size(idx, 1) + error("Agent(s) %d colliding with obstacle(s) %d", idx(ii), idy(ii)); + end + end end diff --git a/@miSim/writeParams.m b/@miSim/writeParams.m new file mode 100644 index 0000000..8530bbf --- /dev/null +++ b/@miSim/writeParams.m @@ -0,0 +1,25 @@ +function writeParams(obj) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'miSim')}; + end + arguments (Output) + end + + % Collect agent parameters + collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents); + alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents); + betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents); + alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents); + betaTilt = cellfun(@(x) x.sensorModel.alphaDist, obj.agents); + comRange = cellfun(@(x) x.commsGeometry.radius, obj.agents); + + % Combine with simulation parameters + params = struct('timestep', obj.timestep, 'maxIter', obj.maxIter, 'minAlt', obj.obstacles{end}.maxCorner(3), 'discretizationStep', obj.domain.objective.discretizationStep, ... + 'collisionRadius', collisionRadii, 'alphaDist', alphaDist, 'betaDist', betaDist, ... + 'alphaTilt', alphaTilt, 'betaTilt', betaTilt, 'comRange', comRange); + + % Save all parameters to output file + paramsFile = strcat(obj.artifactName, "_miSimParams"); + paramsFile = fullfile(matlab.project.rootProject().RootFolder, 'sandbox', paramsFile); + save(paramsFile, "-struct", "params"); +end \ No newline at end of file diff --git a/@sensingObjective/sensingObjective.m b/@sensingObjective/sensingObjective.m index 40414c6..db10142 100644 --- a/@sensingObjective/sensingObjective.m +++ b/@sensingObjective/sensingObjective.m @@ -2,15 +2,15 @@ classdef sensingObjective % Sensing objective definition parent class properties (SetAccess = private, GetAccess = public) label = ""; - groundAlt = 0; - groundPos = [0, 0]; - discretizationStep = 1; - objectiveFunction = @(x, y) 0; % define objective functions over a grid in this manner + groundAlt = NaN; + groundPos = [NaN, NaN]; + discretizationStep = NaN; + objectiveFunction = @(x, y) NaN; % define objective functions over a grid in this manner X = []; Y = []; values = []; - protectedRange = 1; % keep obstacles from crowding objective - sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition + protectedRange = NaN; % keep obstacles from crowding objective + sensorPerformanceMinimum = NaN; % minimum sensor performance to allow assignment of a point in the domain to a partition end methods (Access = public) diff --git a/geometries/@rectangularPrism/initializeRandom.m b/geometries/@rectangularPrism/initializeRandom.m index a8218d4..0802e22 100644 --- a/geometries/@rectangularPrism/initializeRandom.m +++ b/geometries/@rectangularPrism/initializeRandom.m @@ -6,7 +6,7 @@ function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, d minDimension (1, 1) double = 10; maxDimension (1, 1) double = 20; domain (1, 1) {mustBeGeometry} = rectangularPrism; - minAlt (1, 1) double = 0; + minAlt (1, 1) double = 1; end arguments (Output) obj (1, 1) {mustBeA(obj, 'rectangularPrism')}; diff --git a/resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/BtZvZtLyHugR7nfGjg6ukY77EG0d.xml b/resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/BtZvZtLyHugR7nfGjg6ukY77EG0d.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/BtZvZtLyHugR7nfGjg6ukY77EG0d.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/BtZvZtLyHugR7nfGjg6ukY77EG0p.xml b/resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/BtZvZtLyHugR7nfGjg6ukY77EG0p.xml new file mode 100644 index 0000000..54e0a8a --- /dev/null +++ b/resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/BtZvZtLyHugR7nfGjg6ukY77EG0p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m index 14df5bb..118cf6c 100644 --- a/test/parametricTestSuite.m +++ b/test/parametricTestSuite.m @@ -7,7 +7,6 @@ classdef parametricTestSuite < matlab.unittest.TestCase %% Diagnostic Parameters % No effect on simulation dynamics - timestep = 1; makeVideo = true; % disable video writing for big performance increase makePlots = true; % disable plotting for big performance increase (also disables video) plotCommsGeometry = false; % disable plotting communications geometries @@ -15,16 +14,25 @@ classdef parametricTestSuite < matlab.unittest.TestCase end properties (TestParameter) %% Simulation Parameters + timestep = num2cell([1]); % duration of one simulation timestep maxIter = num2cell([25]); % number of timesteps to run % Domain parameters minAlt = num2cell([1]); % minimum allowed agent altitude, make sure test cases don't conflict with this + % Constraint parameters + barrierGain = num2cell([100]); + barrierExponent = num2cell([3]); + % Sensing Objective Parameters - discretizationStep = num2cell([0.01]); + sensorPerformanceMinimum = num2cell([1e-6]); % sensor performance threshhold for partition assignment + discretizationStep = num2cell([0.01]); % sensing objective discretization step size + % this value goes on to determine central differences used in + % gradient ascent and partitioning element sizes % Agent Parameters collisionRadius = num2cell([0.1]); + initialStepSize = num2cell([0.2]); % gradient ascent step size at the first iteration. Decreases linearly to 0 based on maxIter. % Sensor Model Parameters alphaDist = num2cell([2.5, 5]); @@ -38,23 +46,26 @@ classdef parametricTestSuite < matlab.unittest.TestCase methods (Test, ParameterCombination = "exhaustive") % Test cases - function single_agent_gradient_ascent(tc, maxIter, minAlt, discretizationStep, collisionRadius, alphaDist, betaDist, alphaTilt, betaTilt, comRange) + function single_agent_gradient_ascent(tc, timestep, maxIter, barrierGain, barrierExponent, minAlt, sensorPerformanceMinimum, discretizationStep, collisionRadius, initialStepSize, alphaDist, betaDist, alphaTilt, betaTilt, comRange) % Set up square domain l = 10; tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); - tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([.75 * l, 0.75 * l]), tc.domain, discretizationStep, tc.protectedRange); + tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([.75 * l, 0.75 * l]), tc.domain, discretizationStep, tc.protectedRange, sensorPerformanceMinimum); % Set up agent sensorModel = sigmoidSensor; sensorModel = sensorModel.initialize(alphaDist, betaDist, alphaTilt, betaTilt); - agentPos = [l/4, l/4, 3*l/4]; + agentPos = [l/4, l/4, l/4]; collisionGeometry = spherical; collisionGeometry = collisionGeometry.initialize(agentPos, collisionRadius, REGION_TYPE.COLLISION, "Agent 1 Collision Region"); agents = {agent}; - agents{1} = agents{1}.initialize(agentPos, collisionGeometry, sensorModel, comRange, maxIter, "Agent 1", tc.plotCommsGeometry); + agents{1} = agents{1}.initialize(agentPos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, "Agent 1", tc.plotCommsGeometry); % Set up simulation - tc.testClass = tc.testClass.initialize(tc.domain, agents, minAlt, tc.timestep, maxIter, tc.obstacles, tc.makePlots, tc.makeVideo); + tc.testClass = tc.testClass.initialize(tc.domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, tc.obstacles, tc.makePlots, tc.makeVideo); + + % Save simulation parameters to output file + tc.testClass.writeParams(); % Run tc.testClass = tc.testClass.run(); diff --git a/test/test_miSim.m b/test/test_miSim.m index 5174013..8c8a299 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -9,7 +9,7 @@ classdef test_miSim < matlab.unittest.TestCase plotCommsGeometry = false; % disable plotting communications geometries % Sim - maxIter = 250; + maxIter = 50; timestep = 0.05; % Domain @@ -30,6 +30,7 @@ classdef test_miSim < matlab.unittest.TestCase objective = sensingObjective; % Agents + initialStepSize = 0.2; % gradient ascent step size at the first iteration. Decreases linearly to 0 based on maxIter. minAgents = 3; % Minimum number of agents to be randomly generated maxAgents = 4; % Maximum number of agents to be randomly generated agents = cell(0, 1); @@ -51,6 +52,10 @@ classdef test_miSim < matlab.unittest.TestCase % Communications comRange = 8; % Maximum range between agents that forms a communications link + + % Constraints + barrierGain = 100; + barrierExponent = 3; end % Setup for each test @@ -160,7 +165,7 @@ classdef test_miSim < matlab.unittest.TestCase 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 - newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, sensor, tc.comRange, tc.maxIter); + newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, sensor, tc.comRange, tc.maxIter, tc.initialStepSize); % Make sure candidate agent doesn't collide with % domain @@ -208,7 +213,7 @@ classdef test_miSim < matlab.unittest.TestCase end % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, 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.makeVideo); end function misim_run(tc) % randomly create obstacles @@ -294,7 +299,7 @@ classdef test_miSim < matlab.unittest.TestCase 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 - newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, sensor, tc.comRange, tc.maxIter); + newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, sensor, tc.comRange, tc.maxIter, tc.initialStepSize); % Make sure candidate agent doesn't collide with % domain @@ -342,7 +347,10 @@ classdef test_miSim < matlab.unittest.TestCase end % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, 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.makeVideo); + + % Write out parameters + tc.testClass.writeParams(); % Run simulation loop tc.testClass = tc.testClass.run(); @@ -376,17 +384,17 @@ classdef test_miSim < matlab.unittest.TestCase % Initialize agents tc.agents = {agent; agent}; - tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + dh + [d, 0, 0], geometry1, sensor, 3*d, tc.maxIter); - tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + dh - [d, 0, 0], geometry2, sensor, 3*d, tc.maxIter); + tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + dh + [d, 0, 0], geometry1, sensor, 3*d, 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); % 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.agents{3} = tc.agents{3}.initialize(tc.domain.center + dh - [0, d, 0], geometry3, sensor, 3*d, tc.maxIter, tc.initialStepSize); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, cell(0, 1), false, false); + 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.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 @@ -418,10 +426,10 @@ classdef test_miSim < matlab.unittest.TestCase % Initialize agents tc.agents = {agent}; - tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], geometry1, sensor, 3, tc.maxIter); + tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], geometry1, sensor, 3, tc.maxIter, tc.initialStepSize); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, cell(0, 1), false, false); + tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, cell(0, 1), false, false); close(tc.testClass.fPerf); tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]); @@ -450,12 +458,12 @@ classdef test_miSim < matlab.unittest.TestCase % f = sensor.plotParameters(); % Initialize agents - nIter = 100; + nIter = 75; tc.agents = {agent}; - tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, sensor, 3, nIter); + 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 - tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.minAlt, tc.timestep, nIter, cell(0, 1)); + tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, nIter, cell(0, 1)); % Run the simulation tc.testClass = tc.testClass.run(); @@ -489,13 +497,13 @@ classdef test_miSim < matlab.unittest.TestCase sensor = sensor.initialize(alphaDist, 3, 15, 3); % Initialize agents - nIter = 50; + nIter = 25; tc.agents = {agent; agent}; - tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, sensor, 5, nIter); - tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, sensor, 5, nIter); + tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, sensor, 5, nIter, tc.initialStepSize); + tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, sensor, 5, nIter, tc.initialStepSize); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.minAlt, tc.timestep, nIter, cell(0, 1), tc.makeVideo, tc.makePlots); + 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); % Run the simulation tc.testClass.run(); @@ -517,7 +525,7 @@ classdef test_miSim < matlab.unittest.TestCase radius = 1.1; d = [3, 0, 0]; - yOffset = 0; + yOffset = 1; % choice of 0 leads to the agents getting stuck attempting to go around the obstacle on both sides % choice of 1 leads to one agent easily going around while the other gets stuck and the communications link is broken @@ -539,10 +547,10 @@ classdef test_miSim < matlab.unittest.TestCase % 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, radius * 1.1 - yOffset, 0], geometry1, sensor, commsRadius, tc.maxIter); - tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius *1.1 + yOffset, 0], geometry2, sensor, commsRadius, tc.maxIter); + 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, radius *1.1 + yOffset, 0], geometry2, sensor, commsRadius, tc.maxIter, tc.initialStepSize); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, 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.makeVideo); % Run the simulation tc.testClass.run(); @@ -576,14 +584,14 @@ classdef test_miSim < matlab.unittest.TestCase tc.obstacles = {}; % Initialize agents - nIter = 75; + nIter = 50; 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, geometry1, sensor, commsRadius, nIter); - tc.agents{2} = tc.agents{2}.initialize(dom.center - d, geometry2, sensor, commsRadius, nIter); + tc.agents{1} = tc.agents{1}.initialize(dom.center + d, geometry1, sensor, commsRadius, nIter, tc.initialStepSize); + tc.agents{2} = tc.agents{2}.initialize(dom.center - d, geometry2, sensor, commsRadius, nIter, tc.initialStepSize); % Initialize the simulation - tc.testClass = tc.testClass.initialize(dom, tc.agents, tc.minAlt, tc.timestep, nIter, tc.obstacles, true, false); + tc.testClass = tc.testClass.initialize(dom, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, nIter, tc.obstacles, true, false); % Run the simulation tc.testClass = tc.testClass.run(); @@ -616,8 +624,8 @@ classdef test_miSim < matlab.unittest.TestCase nIter = 125; commsRadius = 5; tc.agents = {agent; agent;}; - tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], geometry1, sensor, commsRadius, nIter); - tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], geometry2, sensor, commsRadius, nIter); + 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, sensor, commsRadius, nIter, tc.initialStepSize); % Initialize obstacles obstacleLength = 1.5; @@ -625,7 +633,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.agents, 0, tc.timestep, nIter, tc.obstacles, false, false); + tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, 0, tc.timestep, nIter, tc.obstacles, false, false); % No communications link should be established tc.assertEqual(tc.testClass.adjacency, logical(true(2))); @@ -662,14 +670,14 @@ classdef test_miSim < matlab.unittest.TestCase nIter = 125; commsRadius = d; tc.agents = {agent; agent; agent; agent; agent;}; - tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, sensor, commsRadius, nIter); - tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, 0, 0, geometry2, sensor, commsRadius, nIter); - tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [-d, d, 0], geometry3, sensor, commsRadius, nIter); - tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [-2*d, d, 0], geometry4, sensor, commsRadius, nIter); - tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], geometry5, sensor, commsRadius, nIter); + 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, sensor, commsRadius, nIter, 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, sensor, commsRadius, nIter, tc.initialStepSize); + tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], geometry5, sensor, commsRadius, nIter, tc.initialStepSize); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, 0, tc.timestep, nIter, tc.obstacles, false, false); + tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, 0, tc.timestep, nIter, tc.obstacles, false, false); % Constraint adjacency matrix defined by LNA should be as follows tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ... @@ -713,16 +721,16 @@ classdef test_miSim < matlab.unittest.TestCase nIter = 125; commsRadius = d; 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, sensor, commsRadius, nIter); - tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], geometry2, sensor, commsRadius, nIter); - tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], geometry3, sensor, commsRadius, nIter); - 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.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], geometry5, sensor, commsRadius, nIter); - tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, geometry6, sensor, commsRadius, nIter); - tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], geometry7, sensor, commsRadius, nIter); + 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, sensor, commsRadius, nIter, 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, sensor, commsRadius, nIter, 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, sensor, commsRadius, nIter, 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 - tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, 0, tc.timestep, nIter, tc.obstacles, false, false); + tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, 0, tc.timestep, nIter, tc.obstacles, false, false); % Constraint adjacency matrix defined by LNA should be as follows tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...