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

results plot1 WIP

Browse Source
This commit is contained in:
Kevin Dee
2026-03-15 13:42:35 -07:00
parent ca891a809f
commit 2a0e2e500f
19 changed files with 437 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
@agent/initialize.m
View File

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

9
@agent/run.m
View File

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

26
@miSim/constrainMotion.m
View File

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

2
@miSim/miSim.m
View File

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

1
@miSim/teardown.m
View File

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

9
@miSim/validate.m
View File

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

7
@miSim/writeInits.m
View File

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

15
@sensingObjective/initialize.m
View File

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

2
@sensingObjective/initializeRandomMvnpdf.m
View File

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

3
@sensingObjective/sensingObjective.m
View File

@@ -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 = [];

2
aerpaw/config/scenario.csv
View File

@@ -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
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
1 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
2 5 1 100 150 30.0 0.1 2.0 2.0 1 100 1 3 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

81
plot1.m Normal file
View File

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

2
resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info/>

2
resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="1" type="DIR_SIGNIFIER"/>

2
resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info/>

2
resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="plot1" type="File"/>

2
test/parametricTestSuite.m
View File

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

298
test/results.m Normal file
View File

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

6
util/objectiveFunctionWrapper.m
View File

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