diff --git a/@agent/initialize.m b/@agent/initialize.m
index 84b2f8d..a43bd94 100644
--- a/@agent/initialize.m
+++ b/@agent/initialize.m
@@ -15,6 +15,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
end
obj.pos = pos;
+ obj.lastPos = pos;
obj.vel = zeros(1, 3);
obj.lastVel = zeros(1, 3);
obj.collisionGeometry = collisionGeometry;
diff --git a/@agent/run.m b/@agent/run.m
index 9f818be..340b4cf 100644
--- a/@agent/run.m
+++ b/@agent/run.m
@@ -14,6 +14,13 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
obj (1, 1) {mustBeA(obj, "agent")};
end
+ % Always update lastPos/lastVel so constrainMotion evaluates barriers at
+ % the correct (most recent) position, even when this agent has no partition.
+ obj.lastPos = obj.pos;
+ if useDoubleIntegrator
+ obj.lastVel = obj.vel;
+ end
+
% Collect objective function values across partition
partitionMask = partitioning == index;
if ~any(partitionMask(:))
@@ -79,10 +86,8 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
gradNorm = norm(gradC);
% Compute unconstrained next state
- obj.lastPos = obj.pos;
if useDoubleIntegrator
% Double-integrator: gradient produces desired acceleration with damping
- obj.lastVel = obj.vel;
if gradNorm < 1e-100
a_gradient = zeros(1, 3);
else
diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m
index 8992d12..c3e5b1c 100644
--- a/@miSim/constrainMotion.m
+++ b/@miSim/constrainMotion.m
@@ -39,10 +39,10 @@ function [obj] = constrainMotion(obj)
h(logical(eye(nAgents))) = 0; % self value is 0
for ii = 1:(nAgents - 1)
for jj = (ii + 1):nAgents
- h(ii, jj) = norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
+ h(ii, jj) = norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
h(jj, ii) = h(ii, jj);
- A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
+ A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
% Slack derived from existing params: recovery velocity = max gradient approach velocity.
% Correction splits between 2 agents, so |A| = 2*r_sum
@@ -69,11 +69,11 @@ function [obj] = constrainMotion(obj)
for ii = 1:nAgents
for jj = 1:size(obj.obstacles, 1)
% find closest position to agent on/in obstacle
- cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.pos);
+ cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.lastPos);
- hObs(ii, jj) = dot(obj.agents{ii}.pos - cPos, obj.agents{ii}.pos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
+ hObs(ii, jj) = dot(obj.agents{ii}.lastPos - cPos, obj.agents{ii}.lastPos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
- A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - cPos);
+ A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.lastPos - cPos);
% Floor for single-agent constraint: full correction on one agent, |A| = 2*r_i
r_i = obj.agents{ii}.collisionGeometry.radius;
v_max_i = obj.agents{ii}.initialStepSize / obj.timestep;
@@ -93,37 +93,37 @@ function [obj] = constrainMotion(obj)
h_xMin = 0.0; h_xMax = 0.0; h_yMin = 0.0; h_yMax = 0.0; h_zMin = 0.0; h_zMax = 0.0;
for ii = 1:nAgents
% X minimum
- h_xMin = (obj.agents{ii}.pos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
+ h_xMin = (obj.agents{ii}.lastPos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent;
kk = kk + 1;
% X maximum
- h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.pos(1)) - obj.agents{ii}.collisionGeometry.radius;
+ h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.lastPos(1)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent;
kk = kk + 1;
% Y minimum
- h_yMin = (obj.agents{ii}.pos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
+ h_yMin = (obj.agents{ii}.lastPos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent;
kk = kk + 1;
% Y maximum
- h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.pos(2)) - obj.agents{ii}.collisionGeometry.radius;
+ h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.lastPos(2)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent;
kk = kk + 1;
% Z minimum — enforce z >= minAlt + radius (not just z >= domain floor + radius)
- h_zMin = (obj.agents{ii}.pos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
+ h_zMin = (obj.agents{ii}.lastPos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent;
kk = kk + 1;
% Z maximum
- h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.pos(3)) - obj.agents{ii}.collisionGeometry.radius;
+ h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.lastPos(3)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
kk = kk + 1;
@@ -145,9 +145,9 @@ function [obj] = constrainMotion(obj)
if obj.constraintAdjacencyMatrix(ii, jj)
paddingFactor = 0.9; % Barrier at 90% of actual range; real comms still work beyond this
r_comms = paddingFactor * min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius]);
- hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2;
+ hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2;
- A(kk, (3 * ii - 2):(3 * ii)) = 2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
+ A(kk, (3 * ii - 2):(3 * ii)) = 2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
% One-step forward invariance: b = h/dt ensures h cannot
diff --git a/@miSim/miSim.m b/@miSim/miSim.m
index 3432e90..f9f61e5 100644
--- a/@miSim/miSim.m
+++ b/@miSim/miSim.m
@@ -7,7 +7,6 @@ classdef miSim
timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
maxIter = NaN; % maximum number of simulation iterations
domain;
- objective;
obstacles; % geometries that define obstacles within the domain
agents; % agents that move within the domain
adjacency = false(0, 0); % Adjacency matrix representing communications network graph
@@ -67,7 +66,6 @@ classdef miSim
obj (1, 1) miSim
end
obj.domain = rectangularPrism;
- obj.objective = sensingObjective;
obj.obstacles = {rectangularPrism};
obj.agents = {agent};
end
diff --git a/@miSim/teardown.m b/@miSim/teardown.m
index f4c26d0..d8b614b 100644
--- a/@miSim/teardown.m
+++ b/@miSim/teardown.m
@@ -39,7 +39,6 @@ function obj = teardown(obj)
obj.timestepIndex = NaN;
obj.maxIter = NaN;
obj.domain = rectangularPrism;
- obj.objective = sensingObjective;
obj.obstacles = cell(0, 1);
obj.agents = cell(0, 1);
obj.adjacency = NaN;
diff --git a/@miSim/validate.m b/@miSim/validate.m
index 818a6c0..f57249e 100644
--- a/@miSim/validate.m
+++ b/@miSim/validate.m
@@ -7,11 +7,11 @@ function validate(obj)
%% Communications Network Validators
if max(conncomp(graph(obj.adjacency))) ~= 1
- warning("Network is not connected");
+ error("Network is not connected");
end
if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
- warning("Eliminated network connections that were necessary");
+ error("Eliminated network connections that were necessary");
end
%% Obstacle Validators
@@ -20,10 +20,9 @@ function validate(obj)
for kk = 1:size(obj.agents, 1)
P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner);
d = obj.agents{kk}.pos - P;
- if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2
- warning("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, dot(d, d) - obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
+ if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2 - 1e-3
+ error("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, - dot(d, d) + obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
end
end
end
-
end
diff --git a/@miSim/writeInits.m b/@miSim/writeInits.m
index 4f02b03..6c28f9c 100644
--- a/@miSim/writeInits.m
+++ b/@miSim/writeInits.m
@@ -14,6 +14,9 @@ function writeInits(obj)
comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
+ obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, obj.obstacles, 'UniformOutput', false));
+ obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, obj.obstacles, 'UniformOutput', false));
+
% Combine with simulation parameters
inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
@@ -24,7 +27,9 @@ function writeInits(obj)
"useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ...
"alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
- "pos", pos); % still needs obstacle states and objective state
+ "pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ...
+ "obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ...
+ "objectiveIntegral", sum(obj.domain.objective.values(:)));
% Save all parameters to output file
initsFile = strcat(obj.artifactName, "_miSimInits");
diff --git a/@sensingObjective/initialize.m b/@sensingObjective/initialize.m
index 8096ba2..7d10cae 100644
--- a/@sensingObjective/initialize.m
+++ b/@sensingObjective/initialize.m
@@ -1,4 +1,4 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum, objectiveMu, objectiveSigma)
arguments (Input)
obj (1,1) {mustBeA(obj, "sensingObjective")};
objectiveFunction (1, 1) {mustBeA(objectiveFunction, "function_handle")};
@@ -6,6 +6,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
discretizationStep (1, 1) double = 1;
protectedRange (1, 1) double = 1;
sensorPerformanceMinimum (1, 1) double = 1e-6;
+ objectiveMu (:, 2) double = NaN(1, 2);
+ objectiveSigma (:, 2, 2) double = NaN(1, 2, 2);
end
arguments (Output)
obj (1,1) {mustBeA(obj, "sensingObjective")};
@@ -37,8 +39,13 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
% store ground position
idx = obj.values == 1;
- obj.groundPos = [obj.X(idx), obj.Y(idx)];
- obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
+ if any(isnan(objectiveMu))
+ obj.groundPos = [obj.X(idx), obj.Y(idx)];
+ obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
+ else
+ obj.groundPos = objectiveMu;
+ end
+ obj.objectiveSigma = objectiveSigma;
- assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
+ assert(domain.distance([obj.groundPos, ones(size(obj.groundPos, 1), 1) .* domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
end
\ No newline at end of file
diff --git a/@sensingObjective/initializeRandomMvnpdf.m b/@sensingObjective/initializeRandomMvnpdf.m
index 6f9b8b7..06494ae 100644
--- a/@sensingObjective/initializeRandomMvnpdf.m
+++ b/@sensingObjective/initializeRandomMvnpdf.m
@@ -11,7 +11,7 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
% Set random objective position
mu = domain.minCorner;
- while domain.distance(mu) < protectedRange
+ while domain.distance(mu) < protectedRange * 1.01
mu = domain.random();
end
diff --git a/@sensingObjective/sensingObjective.m b/@sensingObjective/sensingObjective.m
index 46b41a8..0444a17 100644
--- a/@sensingObjective/sensingObjective.m
+++ b/@sensingObjective/sensingObjective.m
@@ -2,7 +2,8 @@ classdef sensingObjective
% Sensing objective definition parent class
properties (SetAccess = private, GetAccess = public)
label = "";
- groundPos = [NaN, NaN];
+ groundPos = NaN(1, 2);
+ objectiveSigma = NaN(1, 2, 2);
discretizationStep = NaN;
X = [];
Y = [];
diff --git a/aerpaw/config/scenario.csv b/aerpaw/config/scenario.csv
index a79f914..05c1468 100644
--- a/aerpaw/config/scenario.csv
+++ b/aerpaw/config/scenario.csv
@@ -1,2 +1,2 @@
timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
-5, 100, 30.0, 0.1, 2.0, 2.0, 100, 3, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1
\ No newline at end of file
+1, 150, 30.0, 0.1, 2.0, 1, 1, 1, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1
\ No newline at end of file
diff --git a/plot1.m b/plot1.m
new file mode 100644
index 0000000..76458cc
--- /dev/null
+++ b/plot1.m
@@ -0,0 +1,81 @@
+clear;
+% Load data
+dataPath = fullfile('.', 'sandbox', 'plot1');
+simHists = dir(dataPath); simHists = simHists(3:end);
+simInits = simHists(endsWith({simHists.name}, 'miSimInits.mat'));
+simHists = simHists(endsWith({simHists.name}, 'miSimHist.mat'));
+assert(length(simHists) == length(simInits), "input data availability mismatch");
+
+% Initialize plotting data
+Cfinal = NaN(12, 1);
+n = NaN(12, 1);
+doubleIntegrator = NaN(12, 1);
+numObjective = NaN(12, 1);
+
+% Aggregate relevant data
+for ii = 1:length(simHists)
+ initName = strrep(simInits(ii).name, "_miSimInits.mat", "");
+ histName = strrep(simHists(ii).name, "_miSimHist.mat", "");
+ assert(initName == histName);
+
+ init = load(fullfile(simInits(ii).folder, simInits(ii).name));
+ hist = load(fullfile(simHists(ii).folder, simHists(ii).name));
+
+ % Stash relevant data
+ Cfinal(ii) = hist.out.perf(end) / init.objectiveIntegral;
+ n(ii) = init.numAgents;
+ doubleIntegrator(ii) = init.useDoubleIntegrator;
+ numObjective(ii) = size(init.objectivePos, 1);
+ for jj = 1:length(hist.out.agent)
+ alphaDist(jj, ii) = hist.out.agent(jj).sensor.alphaDist;
+ end
+end
+
+sensors = unique(alphaDist(1, :));
+
+config = [];
+for ii = 1:length(simHists)
+ % number of agents
+ s = num2str(n(ii));
+
+ % number of objectives
+ if numObjective(ii) == 1
+ s = strcat(s, "_A");
+ elseif numObjective(ii) == 2
+ s = strcat(s, "_B");
+ end
+
+ % sensor pararmeter set
+ if alphaDist(1, ii) == sensors(1)
+ s = strcat(s, "_I");
+ elseif alphaDist(1, ii) == sensors(2)
+ s = strcat(s, "_II");
+ end
+
+ % agent dynamics
+ if ~doubleIntegrator(ii)
+ s = strcat(s, '_alpha');
+ elseif doubleIntegrator(ii)
+ s = strcat(s, '_beta');
+ end
+ config = [config; s];
+end
+
+close all;
+f = figure;
+x = axes; grid(x, "on");
+
+n_unique = sort(unique(n));
+C = [];
+for ii = 1:length(n_unique)
+ nIdx = n == n_unique(ii);
+ C = [C; [Cfinal(nIdx)]'];
+end
+bar(C);
+xlabel("Number of agents");
+ylabel("Final coverage (fraction of maximum)");
+title("Final performance of parameterizations");
+legend(["$AI\alpha$"; "$AI\beta$"; "$AII\alpha$"; "$BI\beta$"], "Interpreter", "latex");
+grid("on");
+keyboard
+
diff --git a/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml b/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml
new file mode 100644
index 0000000..4356a6a
--- /dev/null
+++ b/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml b/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml
new file mode 100644
index 0000000..01cb34e
--- /dev/null
+++ b/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml
new file mode 100644
index 0000000..4356a6a
--- /dev/null
+++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml
new file mode 100644
index 0000000..cea1794
--- /dev/null
+++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m
index d714c0a..aa1078d 100644
--- a/test/parametricTestSuite.m
+++ b/test/parametricTestSuite.m
@@ -150,7 +150,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
end
% randomly shuffle agents to make the network more interesting (probably)
- agents = agents(randperm(numel(agents)));
+ agents = agents(randperm(numel(agents)));
% Set up obstacles
obstacles = cell(params.numObstacles(ii), 1);
diff --git a/test/results.m b/test/results.m
new file mode 100644
index 0000000..5e7a7dd
--- /dev/null
+++ b/test/results.m
@@ -0,0 +1,298 @@
+classdef results < matlab.unittest.TestCase
+ properties (Constant, Access = private)
+ seed = 1;
+ end
+
+ properties (Access = private)
+ % System under test
+ testClass = miSim;
+
+ %% Diagnostic Parameters
+ % No effect on simulation dynamics
+ makeVideo = false; % disable video writing for big performance increase
+ makePlots = true; % disable plotting for big performance increase (also disables video)
+ plotCommsGeometry = false; % disable plotting communications geometries
+
+ %% Fixed Test Parameters
+ useFixedTopology = true; % No lesser neighbor, fixed network instead
+ minDimension = 50; % minimum domain size
+ maxDimension = 100; % maximum domain size
+ discretizationStep = 0.1;
+ protectedRange = 5;
+ collisionRadius = 5;
+ sensorPerformanceMinimum = 0.005;
+ comRange = 20;
+ maxIter = 200;
+ initialStepSize = 1;
+ numObstacles = 3;
+ barrierGain = 1;
+ barrierExponent = 1;
+ timestep = 0.5;
+ dampingCoeff = 2;
+ end
+
+ properties (TestParameter)
+ %% Test Iterations
+ % Specific parameter combos to run iterations on
+ n = struct('n3', 3, 'n5', 5, 'n6', 6); % number of agents
+ config = results.makeConfigs();
+ end
+
+ methods (TestClassSetup)
+ function setSeed(tc)
+ rng(tc.seed);
+ end
+ end
+
+ methods (TestMethodSetup)
+ % % Generate a random domain for each test
+ % function tc = setDomain(tc)
+ % tc.testClass.domain = rectangularPrism;
+ % % random integer-dimensioned cubic domain
+ % tc.testClass.domain = tc.testClass.domain.initializeRandom(REGION_TYPE.DOMAIN, "Domain", tc.minDimension);
+ % % Random bivariate normal PDF objective
+ % tc.testClass.domain.objective = tc.testClass.domain.objective.initializeRandomMvnpdf(tc.testClass.domain, tc.discretizationStep, tc.protectedRange);
+ % end
+ end
+
+ methods (Static, Access = private)
+ function c = makeConfigs()
+ rng(results.seed);
+ abMin = 6; % alpha*beta >= 6 ensures membership(0) = tanh(3) >= 0.995
+ alphaDist = rand(1, 2) .* [100, 100];
+ betaDist = abMin ./ alphaDist + rand(1, 2) .* (20 - abMin ./ alphaDist);
+ alphaTilt = 10 + rand(1, 2) .* [20, 20];
+ betaTilt = abMin ./ alphaTilt + rand(1, 2) .* (50 - abMin ./ alphaTilt);
+ sensors = struct('alphaDist', num2cell(alphaDist), 'alphaTilt', num2cell(alphaTilt), 'betaDist', num2cell(betaDist), 'betaTilt', num2cell(betaTilt));
+ sensor1 = sigmoidSensor;
+ sensor2 = sigmoidSensor;
+ sensor1 = sensor1.initialize(sensors(1).alphaDist, sensors(1).betaDist, sensors(1).alphaTilt, sensors(1).betaTilt);
+ sensor2 = sensor2.initialize(sensors(2).alphaDist, sensors(2).betaDist, sensors(2).alphaTilt, sensors(2).betaTilt);
+ sensor1.plotParameters;
+ sensor2.plotParameters;
+ c = struct('A_1_alpha', struct('numDist', 1, 'sensor', sensors(1), 'doubleIntegrator', false), ...
+ 'A_1_beta', struct('numDist', 1, 'sensor', sensors(1), 'doubleIntegrator', true), ...
+ 'A_2_alpha', struct('numDist', 1, 'sensor', sensors(2), 'doubleIntegrator', false), ...
+ 'B_1_beta', struct('numDist', 2, 'sensor', sensors(1), 'doubleIntegrator', true));
+ end
+ end
+
+ methods (Test)
+ function plot1_runs(tc, n, config)
+ % OVERRIDES
+ % function plot1_runs(tc)
+ % n = 3;
+ % config = struct('numDist', 1, 'sensor', struct('alphaDist', 100, 'alphaTilt', 2, 'betaDist', 10, 'betaTilt', 0.5), 'doubleIntegrator', false);
+
+ % Set up random cube domain
+ minAlt = tc.minDimension(1) * rand() * 0.5;
+ tc.testClass.domain = tc.testClass.domain.initializeRandom(REGION_TYPE.DOMAIN, "Domain", tc.minDimension, tc.maxDimension, tc.testClass.domain, minAlt);
+ % Place sensing objective(s)
+ objectiveMu = [];
+ objectiveSigma = [];
+ for ii = 1:config.numDist
+ mu = tc.testClass.domain.minCorner;
+ while tc.testClass.domain.distance(mu) < tc.protectedRange * 1.01
+ mu = tc.testClass.domain.random();
+ end
+ notPosDef = true;
+ while notPosDef
+ sig = reshape(sort(rand(1, 4) * min(tc.testClass.domain.dimensions(1:2))), [1, 2, 2]);
+ sig(1, 2, 1) = sig(1, 1, 2);
+ [~, notPosDef] = chol(squeeze(sig));
+ end
+ objectiveMu = [objectiveMu; mu(1:2)];
+ objectiveSigma = cat(1, objectiveSigma, sig);
+ end
+ tc.testClass.domain.objective = tc.testClass.domain.objective.initialize(objectiveFunctionWrapper(objectiveMu, objectiveSigma), tc.testClass.domain, tc.discretizationStep, tc.protectedRange, tc.sensorPerformanceMinimum, objectiveMu, objectiveSigma);
+
+ % Initialize agents
+ agents = cell(n, 1);
+ [agents{:}] = deal(agent);
+
+ % Initialize sensor model
+ sensorModel = sigmoidSensor;
+ sensorModel = sensorModel.initialize(config.sensor.alphaDist, config.sensor.betaDist, config.sensor.alphaTilt, config.sensor.betaTilt);
+
+ % Place agents in a quadrant that contains no objective peaks
+ midXY = (tc.testClass.domain.minCorner(1:2) + tc.testClass.domain.maxCorner(1:2)) / 2;
+ occupied = false(2, 2);
+ for ii = 1:size(objectiveMu, 1)
+ occupied(1 + (objectiveMu(ii, 1) >= midXY(1)), ...
+ 1 + (objectiveMu(ii, 2) >= midXY(2))) = true;
+ end
+ freeQ = find(~occupied);
+ if isempty(freeQ)
+ qi = 1;
+ else
+ qi = freeQ(randi(numel(freeQ)));
+ end
+ [xi, yi] = ind2sub([2, 2], qi);
+ xLim = [tc.testClass.domain.minCorner(1), midXY(1), tc.testClass.domain.maxCorner(1)];
+ yLim = [tc.testClass.domain.minCorner(2), midXY(2), tc.testClass.domain.maxCorner(2)];
+ agentBounds = [max(xLim(xi), tc.testClass.domain.minCorner(1) + tc.collisionRadius), ...
+ max(yLim(yi), tc.testClass.domain.minCorner(2) + tc.collisionRadius), ...
+ minAlt + tc.collisionRadius; ...
+ min(xLim(xi+1), tc.testClass.domain.maxCorner(1) - tc.collisionRadius), ...
+ min(yLim(yi+1), tc.testClass.domain.maxCorner(2) - tc.collisionRadius), ...
+ tc.testClass.domain.maxCorner(3) - tc.collisionRadius];
+ collisionGeometry = spherical;
+ for jj = 1:n
+ retry = true;
+ while retry
+ retry = false;
+
+ if jj == 1
+ % First agent: uniform random within placement bounds
+ agentPos = agentBounds(1, :) + (agentBounds(2, :) - agentBounds(1, :)) .* rand(1, 3);
+ else
+ % Sample near centroid of existing agents to maximize
+ % probability of being within comRange of all others
+ positions = cell2mat(cellfun(@(x) x.pos, agents(1:(jj-1)), 'UniformOutput', false));
+ centroid = mean(positions, 1);
+ maxSpread = max(vecnorm(positions - centroid, 2, 2));
+ safeRadius = tc.comRange - maxSpread;
+
+ if safeRadius > 2 * tc.collisionRadius
+ % Uniform random within guaranteed-connected sphere
+ dir = randn(1, 3);
+ dir = dir / norm(dir);
+ r = safeRadius * rand()^(1/3);
+ agentPos = centroid + r * dir;
+ else
+ % Safe sphere too small; sample within comms sphere
+ % of random existing agent (comRange check below)
+ baseIdx = randi(jj - 1);
+ agentPos = agents{baseIdx}.commsGeometry.random();
+ end
+ end
+
+ % Check within placement bounds
+ if any(agentPos <= agentBounds(1, :)) || any(agentPos >= agentBounds(2, :))
+ retry = true;
+ continue;
+ end
+
+ % Check sensor performance threshold
+ if sensorModel.sensorPerformance(agentPos, [agentPos(1:2), 0]) < tc.sensorPerformanceMinimum * 10
+ retry = true;
+ continue;
+ end
+
+ % Check within comRange of ALL existing agents (complete graph)
+ for kk = 1:(jj - 1)
+ if norm(agents{kk}.pos - agentPos) >= tc.comRange
+ retry = true;
+ break;
+ end
+ end
+ if retry, continue; end
+
+ % Check collision with ALL existing agents
+ for kk = 1:(jj - 1)
+ if norm(agents{kk}.pos - agentPos) < agents{kk}.collisionGeometry.radius + tc.collisionRadius
+ retry = true;
+ break;
+ end
+ end
+ end
+
+ % Initialize agent
+ collisionGeometry = collisionGeometry.initialize(agentPos, tc.collisionRadius, REGION_TYPE.COLLISION, sprintf("Agent %d Collision Region", jj));
+ agents{jj} = agents{jj}.initialize(agentPos, collisionGeometry, sensorModel, tc.comRange, tc.maxIter, tc.initialStepSize, sprintf("Agent %d", jj), tc.plotCommsGeometry);
+ end
+
+ % Randomly shuffle agents to vary index-based topology
+ agents = agents(randperm(numel(agents)));
+
+ % Add random obstacles
+ obstacles = cell(tc.numObstacles, 1);
+ [obstacles{:}] = deal(rectangularPrism);
+
+ % Define target region for obstacles (between agents and objective)
+ agentExtent = max(cell2mat(cellfun(@(x) x.pos(1:2), agents, "UniformOutput", false))) + max(cellfun(@(x) x.collisionGeometry.radius, agents));
+ objExtent = tc.testClass.domain.objective.groundPos - tc.testClass.domain.objective.protectedRange;
+ % Per-axis: use gap if valid, else fall back to full domain
+ obsMin = zeros(1, 2);
+ obsMax = zeros(1, 2);
+ for dim = 1:2
+ if agentExtent(dim) < objExtent(dim)
+ obsMin(dim) = agentExtent(dim);
+ obsMax(dim) = objExtent(dim);
+ else
+ obsMin(dim) = tc.testClass.domain.minCorner(dim);
+ obsMax(dim) = tc.testClass.domain.maxCorner(dim);
+ end
+ end
+
+ for jj = 1:size(obstacles, 1)
+ retry = true;
+ while retry
+ retry = false;
+
+ % Generate corners within target region
+ cornersXY = obsMin + sort(rand(2, 2), 1, "ascend") .* (obsMax - obsMin);
+ corners = [cornersXY, [minAlt; minAlt + rand * (tc.testClass.domain.maxCorner(3) - minAlt)]];
+
+ % Initialize obstacle using proposed coordinates
+ obstacles{jj} = obstacles{jj}.initialize(corners, REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", jj));
+
+ % Make sure the obstacle doesn't crowd the objective
+ for kk = 1:size(tc.testClass.domain.objective.groundPos, 1)
+ if ~retry && obstacles{jj}.distance([tc.testClass.domain.objective.groundPos(kk, 1:2), minAlt]) <= tc.testClass.domain.objective.protectedRange
+ retry = true;
+ continue;
+ end
+ end
+
+ % Check if the obstacle collides with an existing obstacle
+ if ~retry && jj > 1 && tc.obstacleCollisionCheck(obstacles(1:(jj - 1)), obstacles{jj})
+ retry = true;
+ continue;
+ end
+
+ % Check if the obstacle collides with an agent
+ if ~retry
+ for kk = 1:size(agents, 1)
+ P = min(max(agents{kk}.pos, obstacles{jj}.minCorner), obstacles{jj}.maxCorner);
+ d = agents{kk}.pos - P;
+ if dot(d, d) <= agents{kk}.collisionGeometry.radius^2
+ retry = true;
+ break;
+ end
+ end
+ end
+
+ if retry
+ continue;
+ end
+ end
+ end
+
+ % Set up simulation
+ tc.testClass = tc.testClass.initialize(tc.testClass.domain, agents, tc.barrierGain, tc.barrierExponent, minAlt, tc.timestep, tc.maxIter, obstacles, tc.makePlots, tc.makeVideo, config.doubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+
+ % Save simulation parameters to output file
+ tc.testClass.writeInits();
+
+ % Run
+ tc.testClass = tc.testClass.run();
+
+ % Cleanup
+ tc.testClass = tc.testClass.teardown();
+ end
+ end
+
+ methods
+ function c = obstacleCollisionCheck(~, obstacles, obstacle)
+ % Check if the obstacle intersects with any other obstacles
+ c = false;
+ for ii = 1:size(obstacles, 1)
+ if geometryIntersects(obstacles{ii}, obstacle)
+ c = true;
+ return;
+ end
+ end
+ end
+ end
+end
\ No newline at end of file
diff --git a/util/objectiveFunctionWrapper.m b/util/objectiveFunctionWrapper.m
index f0a5963..56328ac 100644
--- a/util/objectiveFunctionWrapper.m
+++ b/util/objectiveFunctionWrapper.m
@@ -4,12 +4,12 @@ function f = objectiveFunctionWrapper(center, sigma)
% composite objectives in particular
arguments (Input)
center (:, 2) double;
- sigma (2, 2) double = eye(2);
+ sigma (:, 2, 2) double = eye(2);
end
arguments (Output)
f (1, 1) {mustBeA(f, "function_handle")};
end
-
- f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), sigma), 1:size(center,1), "UniformOutput", false)), 2);
+ assert(size(center, 1) == size(sigma, 1));
+ f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), squeeze(sigma(i, :, :))), 1:size(center,1), "UniformOutput", false)), 2);
end
\ No newline at end of file