diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m
index 0ec6c8d..5705832 100644
--- a/@miSim/constrainMotion.m
+++ b/@miSim/constrainMotion.m
@@ -8,12 +8,6 @@ function [obj] = constrainMotion(obj)
nAgents = size(obj.agents, 1);
- if nAgents < 2
- nAAPairs = 0;
- else
- nAAPairs = nchoosek(nAgents, 2); % unique agent/agent pairs
- end
-
% Compute velocity matrix from unconstrained gradient-ascent step
v = zeros(nAgents, 3);
for ii = 1:nAgents
@@ -26,13 +20,9 @@ function [obj] = constrainMotion(obj)
end
% Initialize QP based on number of agents and obstacles
- nAOPairs = nAgents * size(obj.obstacles, 1); % unique agent/obstacle pairs
- nADPairs = nAgents * 6; % agents x (4 walls + 1 floor + 1 ceiling)
- nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - nAgents;
- total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
kk = 1;
- A = zeros(total, 3 * nAgents);
- b = zeros(total, 1);
+ A = zeros(obj.numBarriers, 3 * nAgents);
+ b = zeros(obj.numBarriers, 1);
% Set up collision avoidance constraints
h = NaN(nAgents, nAgents);
@@ -60,6 +50,10 @@ function [obj] = constrainMotion(obj)
end
end
+ idx = length(h(triu(true(size(h)), 1)));
+ obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
+ idx = idx + 1;
+
hObs = NaN(nAgents, size(obj.obstacles, 1));
% Set up obstacle avoidance constraints
for ii = 1:nAgents
@@ -80,6 +74,9 @@ function [obj] = constrainMotion(obj)
end
end
+ obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
+ idx = idx + numel(hObs);
+
% Set up domain constraints (walls and ceiling only)
% Floor constraint is implicit with an obstacle corresponding to the
% minimum allowed altitude, but I included it anyways
@@ -120,6 +117,9 @@ function [obj] = constrainMotion(obj)
A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
kk = kk + 1;
+
+ obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
+ idx = idx + 6;
end
if coder.target('MATLAB')
@@ -154,6 +154,7 @@ function [obj] = constrainMotion(obj)
end
end
end
+ obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
% Solve QP program generated earlier
vhat = reshape(v', 3 * nAgents, 1);
diff --git a/@miSim/initialize.m b/@miSim/initialize.m
index c7e0747..4cf8ca6 100644
--- a/@miSim/initialize.m
+++ b/@miSim/initialize.m
@@ -104,11 +104,25 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
% Create initial partitioning
obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
+ % Determine number of barrier functions that will be necessary
+ if size(obj.agents, 1) < 2
+ nAAPairs = 0;
+ else
+ nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
+ end
+ nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
+ nADPairs = size(obj.agents, 1) * 6; % agents x (4 walls + 1 floor + 1 ceiling)
+ nLNAPairs = sum(triu(obj.constraintAdjacencyMatrix, 1), "all");
+ obj.numBarriers = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
+
if coder.target('MATLAB')
% Initialize variable that will store agent positions for trail plots
obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
+ % Initialize variable that will store barrier function values per timestep for analysis purposes
+ obj.barriers = NaN(obj.numBarriers, size(obj.times, 1));
+
% Set up plots showing initialized state
obj = obj.plot();
diff --git a/@miSim/miSim.m b/@miSim/miSim.m
index 4b47798..4afd034 100644
--- a/@miSim/miSim.m
+++ b/@miSim/miSim.m
@@ -23,6 +23,8 @@ classdef miSim
fPerf; % performance plot figure
% Indicies for various plot types in the main tiled layout figure
spatialPlotIndices = [6, 4, 3, 2];
+ numBarriers = 0; % Number of barrier functions needed
+ barriers = []; % log barrier function values at each timestep for analysis
end
properties (Access = private)
diff --git a/@miSim/run.m b/@miSim/run.m
index cc4a280..e549935 100644
--- a/@miSim/run.m
+++ b/@miSim/run.m
@@ -63,10 +63,12 @@ function [obj] = run(obj)
end
end
+ % Close video
if coder.target('MATLAB')
if obj.makeVideo
% Close video file
v.close();
end
end
+
end
diff --git a/@miSim/teardown.m b/@miSim/teardown.m
index 4c18ab9..ba1d21d 100644
--- a/@miSim/teardown.m
+++ b/@miSim/teardown.m
@@ -11,6 +11,25 @@ function obj = teardown(obj)
close(obj.fPerf);
close(obj.f);
+ % Log results into matfile
+ histPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", strcat(obj.artifactName, "_miSimHist.mat"));
+ out = struct("agent", repmat(struct("pos", [], "perf", [], "sensor", struct("alphaDist", [], "betaDist", [], "alphaTilt", [], "betaTilt", []), "collisionRadius", [], "commsRadius", []), size(obj.agents)), "perf", [], "barriers", []);
+
+ out.perf = obj.performance(1:(end - 1));
+ out.barriers = [zeros(size(obj.barriers(1:end, 1), 1), 1), obj.barriers(1:end, 1:(end - 1))];
+ for ii = 1:size(obj.agents, 1)
+ out.agent(ii).pos = squeeze(obj.posHist(ii, 1:(end - 1), 1:3));
+ out.agent(ii).perf = obj.agents{ii}.performance(1:(end - 2));
+ out.agent(ii).sensor.alphaDist = obj.agents{ii}.sensorModel.alphaDist;
+ out.agent(ii).sensor.betaDist = obj.agents{ii}.sensorModel.betaDist;
+ out.agent(ii).sensor.alphaTilt = obj.agents{ii}.sensorModel.alphaTilt;
+ out.agent(ii).sensor.betaTilt = obj.agents{ii}.sensorModel.betaTilt;
+ out.agent(ii).collisionRadius = obj.agents{ii}.collisionGeometry.radius;
+ out.agent(ii).commsRadius = obj.agents{ii}.commsGeometry.radius;
+ end
+
+ save(histPath, "out");
+
% reset parameters
obj.timestep = NaN;
obj.timestepIndex = NaN;
diff --git a/resources/project/erFP4mR9aK32bO4kZAlU-FeDikA/mvGRiPTuORJmVPdRh4fiJMWoTJAd.xml b/resources/project/erFP4mR9aK32bO4kZAlU-FeDikA/mvGRiPTuORJmVPdRh4fiJMWoTJAd.xml
deleted file mode 100644
index 4356a6a..0000000
--- a/resources/project/erFP4mR9aK32bO4kZAlU-FeDikA/mvGRiPTuORJmVPdRh4fiJMWoTJAd.xml
+++ /dev/null
@@ -1,2 +0,0 @@
-
-
\ No newline at end of file
diff --git a/resources/project/erFP4mR9aK32bO4kZAlU-FeDikA/mvGRiPTuORJmVPdRh4fiJMWoTJAp.xml b/resources/project/erFP4mR9aK32bO4kZAlU-FeDikA/mvGRiPTuORJmVPdRh4fiJMWoTJAp.xml
deleted file mode 100644
index 01cb34e..0000000
--- a/resources/project/erFP4mR9aK32bO4kZAlU-FeDikA/mvGRiPTuORJmVPdRh4fiJMWoTJAp.xml
+++ /dev/null
@@ -1,2 +0,0 @@
-
-
\ No newline at end of file
diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/erFP4mR9aK32bO4kZAlU-FeDikAd.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/erFP4mR9aK32bO4kZAlU-FeDikAd.xml
deleted file mode 100644
index 4356a6a..0000000
--- a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/erFP4mR9aK32bO4kZAlU-FeDikAd.xml
+++ /dev/null
@@ -1,2 +0,0 @@
-
-
\ No newline at end of file
diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/erFP4mR9aK32bO4kZAlU-FeDikAp.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/erFP4mR9aK32bO4kZAlU-FeDikAp.xml
deleted file mode 100644
index ea15762..0000000
--- a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/erFP4mR9aK32bO4kZAlU-FeDikAp.xml
+++ /dev/null
@@ -1,2 +0,0 @@
-
-
\ No newline at end of file
diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m
index 9fc185c..cf95203 100644
--- a/test/parametricTestSuite.m
+++ b/test/parametricTestSuite.m
@@ -64,6 +64,9 @@ classdef parametricTestSuite < matlab.unittest.TestCase
% Run
tc.testClass = tc.testClass.run();
+
+ % Save results and clean up
+ tc.testClass = tc.testClass.teardown();
end
function csv_parametric_tests_random_agents(tc)
% Read in parameters to iterate over