last plot updates

This commit is contained in:
2026-03-18 16:05:23 -07:00
parent f23675a54c
commit 2ca0c286cd
7 changed files with 203 additions and 227 deletions

View File

@@ -138,6 +138,11 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
% Initialize variable that will store barrier function values per timestep for analysis purposes % Initialize variable that will store barrier function values per timestep for analysis purposes
obj.barriers = NaN(obj.numBarriers, size(obj.times, 1)); obj.barriers = NaN(obj.numBarriers, size(obj.times, 1));
% Initialize constraint adjacency history (nAgents x nAgents x nTimesteps)
nAgents = size(obj.agents, 1);
obj.constraintAdjacencyHist = false(nAgents, nAgents, size(obj.times, 1));
obj.constraintAdjacencyHist(:, :, 1) = obj.constraintAdjacencyMatrix;
% Set up plots showing initialized state % Set up plots showing initialized state
obj = obj.plot(); obj = obj.plot();

View File

@@ -27,6 +27,7 @@ classdef miSim
spatialPlotIndices = [6, 4, 3, 2]; spatialPlotIndices = [6, 4, 3, 2];
numBarriers = 0; % Number of barrier functions needed numBarriers = 0; % Number of barrier functions needed
barriers = []; % log barrier function values at each timestep for analysis barriers = []; % log barrier function values at each timestep for analysis
constraintAdjacencyHist = []; % log constraint adjacency matrix at each timestep
end end
properties (Access = private) properties (Access = private)

View File

@@ -34,6 +34,11 @@ function [obj] = run(obj)
obj = obj.lesserNeighbor(); obj = obj.lesserNeighbor();
end end
% Log constraint adjacency for this timestep
if coder.target('MATLAB')
obj.constraintAdjacencyHist(:, :, ii) = obj.constraintAdjacencyMatrix;
end
% Moving % Moving
% Iterate over agents to simulate their unconstrained motion % Iterate over agents to simulate their unconstrained motion
for jj = 1:size(obj.agents, 1) for jj = 1:size(obj.agents, 1)

View File

@@ -20,6 +20,7 @@ function obj = teardown(obj)
out.dampingCoeff = obj.dampingCoeff; out.dampingCoeff = obj.dampingCoeff;
out.useDoubleIntegrator = obj.useDoubleIntegrator; out.useDoubleIntegrator = obj.useDoubleIntegrator;
out.useFixedTopology = obj.useFixedTopology; out.useFixedTopology = obj.useFixedTopology;
out.constraintAdjacency = obj.constraintAdjacencyHist(:, :, 1:(end - 1));
for ii = 1:size(obj.agents, 1) for ii = 1:size(obj.agents, 1)
out.agent(ii).pos = squeeze(obj.posHist(ii, 1:(end - 1), 1:3)); out.agent(ii).pos = squeeze(obj.posHist(ii, 1:(end - 1), 1:3));
out.agent(ii).vel = squeeze(obj.velHist(ii, 1:(end - 1), 1:3)); out.agent(ii).vel = squeeze(obj.velHist(ii, 1:(end - 1), 1:3));
@@ -43,6 +44,7 @@ function obj = teardown(obj)
obj.agents = cell(0, 1); obj.agents = cell(0, 1);
obj.adjacency = NaN; obj.adjacency = NaN;
obj.constraintAdjacencyMatrix = NaN; obj.constraintAdjacencyMatrix = NaN;
obj.constraintAdjacencyHist = [];
obj.partitioning = NaN; obj.partitioning = NaN;
obj.performance = 0; obj.performance = 0;
obj.barrierGain = NaN; obj.barrierGain = NaN;

145
plot_1.m
View File

@@ -1,23 +1,27 @@
clear; clear;
% Load data
%% Load data
dataPath = fullfile('.', 'sandbox', 'plot1'); dataPath = fullfile('.', 'sandbox', 'plot1');
simHists = dir(dataPath); simHists = simHists(3:end); dataFiles = dir(dataPath);
simInits = simHists(endsWith({simHists.name}, 'miSimInits.mat')); dataFiles = dataFiles(~startsWith({dataFiles.name}, '.'));
simHists = simHists(endsWith({simHists.name}, 'miSimHist.mat')); simInits = dataFiles(endsWith({dataFiles.name}, 'miSimInits.mat'));
simHists = dataFiles(endsWith({dataFiles.name}, 'miSimHist.mat'));
assert(length(simHists) == length(simInits), "input data availability mismatch"); assert(length(simHists) == length(simInits), "input data availability mismatch");
% Initialize plotting data %% Aggregate run data
nRuns = length(simHists); nRuns = length(simHists);
Cfinal = NaN(nRuns, 1); Cfinal = NaN(nRuns, 1);
n = NaN(nRuns, 1); nAgents = NaN(nRuns, 1);
doubleIntegrator = NaN(nRuns, 1); doubleIntegrator = NaN(nRuns, 1);
numObjective = NaN(nRuns, 1); numObjective = NaN(nRuns, 1);
positions = cell(6, nRuns);
commsRadius = NaN(nRuns, 1); commsRadius = NaN(nRuns, 1);
collisionRadius = NaN(nRuns, 1); collisionRadius = NaN(nRuns, 1);
maxAgents = 6;
alphaDist = NaN(maxAgents, nRuns);
positions = cell(maxAgents, nRuns);
adjacency = cell(nRuns, 1);
% Aggregate relevant data for ii = 1:nRuns
for ii = 1:length(simHists)
initName = strrep(simInits(ii).name, "_miSimInits.mat", ""); initName = strrep(simInits(ii).name, "_miSimInits.mat", "");
histName = strrep(simHists(ii).name, "_miSimHist.mat", ""); histName = strrep(simHists(ii).name, "_miSimHist.mat", "");
assert(initName == histName); assert(initName == histName);
@@ -25,38 +29,29 @@ for ii = 1:length(simHists)
init = load(fullfile(simInits(ii).folder, simInits(ii).name)); init = load(fullfile(simInits(ii).folder, simInits(ii).name));
hist = load(fullfile(simHists(ii).folder, simHists(ii).name)); hist = load(fullfile(simHists(ii).folder, simHists(ii).name));
% Stash relevant data
Cfinal(ii) = hist.out.perf(end) / init.objectiveIntegral; Cfinal(ii) = hist.out.perf(end) / init.objectiveIntegral;
n(ii) = init.numAgents; nAgents(ii) = init.numAgents;
doubleIntegrator(ii) = init.useDoubleIntegrator; doubleIntegrator(ii) = init.useDoubleIntegrator;
numObjective(ii) = size(init.objectivePos, 1); numObjective(ii) = size(init.objectivePos, 1);
commsRadius(ii) = unique(init.comRange); commsRadius(ii) = unique(init.comRange);
collisionRadius(ii) = unique(init.collisionRadius); collisionRadius(ii) = unique(init.collisionRadius);
for jj = 1:length(hist.out.agent)
adjacency{ii} = hist.out.constraintAdjacency(:, :, 1);
for jj = 1:nAgents(ii)
alphaDist(jj, ii) = hist.out.agent(jj).sensor.alphaDist; alphaDist(jj, ii) = hist.out.agent(jj).sensor.alphaDist;
positions{jj, ii} = hist.out.agent(jj).pos; positions{jj, ii} = hist.out.agent(jj).pos;
assert(hist.out.agent(jj).commsRadius == commsRadius(ii)); assert(hist.out.agent(jj).commsRadius == commsRadius(ii));
assert(hist.out.agent(jj).collisionRadius == collisionRadius(ii)); assert(hist.out.agent(jj).collisionRadius == collisionRadius(ii));
end end
alphaDist2 = unique(alphaDist);
if length(alphaDist2) > 1
alphaDist2 = alphaDist2(1);
end
if doubleIntegrator(ii) && all(alphaDist(1:n(ii), ii) == alphaDist2) && numObjective(ii) == 1
a2betaIdx = ii;
a2beta = struct("init", init, "hist", hist.out);
end
end end
commsRadius = unique(commsRadius); assert(isscalar(commsRadius)); commsRadius = unique(commsRadius); assert(isscalar(commsRadius));
collisionRadius = unique(collisionRadius); assert(isscalar(collisionRadius)); collisionRadius = unique(collisionRadius); assert(isscalar(collisionRadius));
sensors = flip(unique(alphaDist(1, :))); sensorTypes = flip(unique(alphaDist(1, :)));
n_unique = sort(unique(n)); nValues = sort(unique(nAgents));
nGroups = length(n_unique); nGroups = length(nValues);
% Build config label for each run %% Build config labels
config = strings(nRuns, 1);
baseConfig = strings(nRuns, 1); baseConfig = strings(nRuns, 1);
for ii = 1:nRuns for ii = 1:nRuns
s = ""; s = "";
@@ -65,9 +60,9 @@ for ii = 1:nRuns
elseif numObjective(ii) == 2 elseif numObjective(ii) == 2
s = s + "B"; s = s + "B";
end end
if alphaDist(1, ii) == sensors(1) if alphaDist(1, ii) == sensorTypes(1)
s = s + "_I"; s = s + "_I";
elseif alphaDist(1, ii) == sensors(2) elseif alphaDist(1, ii) == sensorTypes(2)
s = s + "_II"; s = s + "_II";
end end
if ~doubleIntegrator(ii) if ~doubleIntegrator(ii)
@@ -76,88 +71,77 @@ for ii = 1:nRuns
s = s + "_beta"; s = s + "_beta";
end end
baseConfig(ii) = s; baseConfig(ii) = s;
config(ii) = n(ii) + "_" + s;
end end
configOrder = unique(baseConfig(n == n_unique(1)), 'stable'); configOrder = unique(baseConfig(nAgents == nValues(1)), 'stable');
nConfigsPerN = length(configOrder); nConfigs = length(configOrder);
configLabels = ["$AI\alpha$"; "$AI\beta$"; "$AII\alpha$"; "$BI\beta$"];
%% %% Plot 1: Final normalized coverage
close all; close all;
f1 = figure; f1 = figure;
x1 = axes; x1 = axes;
C_mean = NaN(nGroups, nConfigsPerN); C_mean = NaN(nGroups, nConfigs);
C_var = NaN(nGroups, nConfigsPerN); C_var = NaN(nGroups, nConfigs);
for ii = 1:nGroups for ii = 1:nGroups
for jj = 1:nConfigsPerN for jj = 1:nConfigs
mask = (n == n_unique(ii)) & (baseConfig == configOrder(jj)); mask = (nAgents == nValues(ii)) & (baseConfig == configOrder(jj));
C_mean(ii, jj) = mean(Cfinal(mask)); C_mean(ii, jj) = mean(Cfinal(mask));
C_var(ii, jj) = var(Cfinal(mask)); C_var(ii, jj) = var(Cfinal(mask));
end end
end end
hBar = bar(x1, C_mean); bar(x1, C_mean);
hold(x1, 'on'); set(x1, 'XTickLabel', string(nValues));
for jj = 1:nConfigsPerN xlabel(x1, "Number of agents");
xPos = hBar(jj).XEndPoints; ylabel(x1, "Final coverage (normalized)");
errorbar(x1, xPos, C_mean(:, jj), C_var(:, jj), 'k.', 'LineWidth', 1, 'HandleVisibility', 'off'); % disabled the error bars because they are small to the point of meaninglessness title(x1, "Final performance of parameterizations");
end legend(x1, configLabels, "Interpreter", "latex", "Location", "northwest");
hold(x1, 'off'); grid(x1, "on");
set(x1, 'XTickLabel', string(n_unique)); ylim(x1, [0, 1/2]);
xlabel("Number of agents");
ylabel("Final coverage (normalized)");
title("Final performance of parameterizations");
legend(["$AI\alpha$"; "$AI\beta$"; "$AII\alpha$"; "$BI\beta$"], "Interpreter", "latex", "Location", "northwest");
grid("on");
ylim([0, 1/2]);
savefig(f1, "plot1.fig"); savefig(f1, "plot1.fig");
exportgraphics(f1, "plot1.png"); exportgraphics(f1, "plot1.png");
%% %% Plot 2: Pairwise agent distances
f2 = figure; f2 = figure;
x2 = axes; x2 = axes;
% Compute pairwise distances between agents % Compute pairwise distances only for connected agents (static topology)
maxPairs = nchoosek(6, 2); maxPairs = nchoosek(maxAgents, 2);
pairDist = cell(maxPairs, nRuns); pairDist = cell(maxPairs, nRuns);
for ii = 1:nRuns for ii = 1:nRuns
A = adjacency{ii};
pp = 0; pp = 0;
for jj = 1:n(ii)-1 for jj = 1:nAgents(ii)-1
for kk = jj+1:n(ii) for kk = jj+1:nAgents(ii)
pp = pp + 1; pp = pp + 1;
if A(jj, kk)
pairDist{pp, ii} = vecnorm(positions{jj, ii} - positions{kk, ii}, 2, 2); pairDist{pp, ii} = vecnorm(positions{jj, ii} - positions{kk, ii}, 2, 2);
end end
end end
end end
% Cap pairwise distances at communications range
for ii = 1:nRuns
nPairs = nchoosek(n(ii), 2);
for pp = 1:nPairs
pairDist{pp, ii} = min(pairDist{pp, ii}, commsRadius);
end
end end
% Compute mean, min, max pairwise distance across all pairs and timesteps per run % Per-run statistics across all pairs and timesteps
meanPairDist = NaN(nRuns, 1); meanPairDist = NaN(nRuns, 1);
minPairDist = NaN(nRuns, 1); minPairDist = NaN(nRuns, 1);
maxPairDist = NaN(nRuns, 1); maxPairDist = NaN(nRuns, 1);
for ii = 1:nRuns for ii = 1:nRuns
nPairs = nchoosek(n(ii), 2); nPairs = nchoosek(nAgents(ii), 2);
D = vertcat(pairDist{1:nPairs, ii}); D = vertcat(pairDist{1:nPairs, ii});
meanPairDist(ii) = mean(D); meanPairDist(ii) = mean(D, "omitmissing");
minPairDist(ii) = min(D); minPairDist(ii) = min(D);
maxPairDist(ii) = max(D); maxPairDist(ii) = max(D);
end end
% Group pairwise distance stats by (n, config), aggregating across reps % Aggregate across trials per (n, config) group
meanD = NaN(nGroups, nConfigsPerN); meanD = NaN(nGroups, nConfigs);
minD = NaN(nGroups, nConfigsPerN); minD = NaN(nGroups, nConfigs);
maxD = NaN(nGroups, nConfigsPerN); maxD = NaN(nGroups, nConfigs);
for ii = 1:nGroups for ii = 1:nGroups
for jj = 1:nConfigsPerN for jj = 1:nConfigs
mask = (n == n_unique(ii)) & (baseConfig == configOrder(jj)); mask = (nAgents == nValues(ii)) & (baseConfig == configOrder(jj));
meanD(ii, jj) = mean(meanPairDist(mask)); meanD(ii, jj) = mean(meanPairDist(mask));
minD(ii, jj) = min(minPairDist(mask)); minD(ii, jj) = min(minPairDist(mask));
maxD(ii, jj) = max(maxPairDist(mask)); maxD(ii, jj) = max(maxPairDist(mask));
@@ -166,24 +150,25 @@ end
% Plot whiskers (min to max) with mean markers % Plot whiskers (min to max) with mean markers
barWidth = 0.8; barWidth = 0.8;
groupWidth = barWidth / nConfigsPerN; groupWidth = barWidth / nConfigs;
hold(x2, 'on'); hold(x2, 'on');
for jj = 1:nConfigsPerN for jj = 1:nConfigs
xPos = (1:nGroups) + (jj - (nConfigsPerN + 1) / 2) * groupWidth; xPos = (1:nGroups) + (jj - (nConfigs + 1) / 2) * groupWidth;
errorbar(x2, xPos, meanD(:, jj), meanD(:, jj) - minD(:, jj), maxD(:, jj) - meanD(:, jj), ... errorbar(x2, xPos, meanD(:, jj), meanD(:, jj) - minD(:, jj), maxD(:, jj) - meanD(:, jj), ...
'o', 'LineWidth', 1.5, 'MarkerSize', 6, 'CapSize', 10); 'o', 'LineWidth', 1.5, 'MarkerSize', 6, 'CapSize', 10);
end end
hold(x2, 'off'); hold(x2, 'off');
set(x2, 'XTick', 1:nGroups, 'XTickLabel', string(n_unique)); set(x2, 'XTick', 1:nGroups, 'XTickLabel', string(nValues));
xlabel(x2, "Number of agents"); xlabel(x2, "Number of agents");
ylabel(x2, "Pairwise distance"); ylabel(x2, "Pairwise distance");
title(x2, "Pairwise Agent Distances (min/mean/max)"); title(x2, "Pairwise Agent Distances (min/mean/max)");
legend(x2, ["$AI\alpha$"; "$AI\beta$"; "$AII\alpha$"; "$BI\beta$"], "Interpreter", "latex"); legend(x2, configLabels, "Interpreter", "latex");
grid(x2, "on"); grid(x2, "on");
yline(x2, collisionRadius, 'r--', "Label", "Collision Radius", ...
yline(collisionRadius, 'r--', "Label", "Collision Radius", "LabelHorizontalAlignment", "left", "HandleVisibility", "off"); "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
yline(commsRadius, 'r--', "Label", "Communications Radius", "LabelHorizontalAlignment", "left", "HandleVisibility", "off"); yline(x2, commsRadius, 'r--', "Label", "Communications Radius", ...
ylim([0, commsRadius + 5]); "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
ylim(x2, [0, commsRadius + 5]);
savefig(f2, "plot2.fig"); savefig(f2, "plot2.fig");
exportgraphics(f2, "plot2.png"); exportgraphics(f2, "plot2.png");

154
plot_3.m
View File

@@ -1,142 +1,120 @@
clear; clear;
% Load data
%% Load data
dataPath = fullfile('.', 'sandbox', 'plot3'); dataPath = fullfile('.', 'sandbox', 'plot3');
simHists = dir(dataPath); simHists = simHists(3:end); dataFiles = dir(dataPath);
simInits = simHists(endsWith({simHists.name}, 'miSimInits.mat')); dataFiles = dataFiles(~startsWith({dataFiles.name}, '.'));
simHists = simHists(endsWith({simHists.name}, 'miSimHist.mat')); simInits = dataFiles(endsWith({dataFiles.name}, 'miSimInits.mat'));
simHists = dataFiles(endsWith({dataFiles.name}, 'miSimHist.mat'));
assert(length(simHists) == length(simInits), "input data availability mismatch"); assert(length(simHists) == length(simInits), "input data availability mismatch");
assert(isscalar(simHists)); assert(isscalar(simHists));
init = fullfile(simInits(1).folder, simInits(1).name); init = load(fullfile(simInits(1).folder, simInits(1).name));
hist = fullfile(simHists(1).folder, simHists(1).name); hist = load(fullfile(simHists(1).folder, simHists(1).name));
init = load(init);
hist = load(hist);
hist = hist.out; hist = hist.out;
%% Plot 3: Per-agent and cumulative normalized performance
assert(size(init.objectivePos, 1) == 1);
assert(hist.useDoubleIntegrator);
nAgents = length(hist.agent);
agentLabels = "Agent " + string(1:nAgents)';
f3 = figure; f3 = figure;
x3 = axes; x3 = axes;
assert(size(init.objectivePos, 1) == 1) hold(x3, 'on');
assert(hist.useDoubleIntegrator); plot(x3, hist.perf ./ init.objectiveIntegral, "LineWidth", 2);
for ii = 1:nAgents
plot(hist.perf./init.objectiveIntegral, "LineWidth", 2); plot(x3, hist.agent(ii).perf ./ init.objectiveIntegral, "LineWidth", 2);
hold("on");
for ii = 1:length(hist.agent)
plot(hist.agent(ii).perf./init.objectiveIntegral, "LineWidth", 2);
end end
grid("on"); hold(x3, 'off');
ylabel("Performance (normalized)"); grid(x3, "on");
xlabel("Timestep"); ylabel(x3, "Performance (normalized)");
legend(["Cumulative"; "Agent 1"; "Agent 2"; "Agent 3"; "Agent 4"], "Location", "northwest"); xlabel(x3, "Timestep");
title("$AII\beta$ Performance", "Interpreter", "latex"); legend(x3, ["Cumulative"; agentLabels], "Location", "northwest");
title(x3, "$AII\beta$ Performance", "Interpreter", "latex");
savefig(f3, "plot3.fig"); savefig(f3, "plot3.fig");
exportgraphics(f3, "plot3.png"); exportgraphics(f3, "plot3.png");
f4 = figure; %% Plot 4: Pairwise distances and barrier functions
x4 = axes;
% Compute pairwise distances between agents over time
nAgents = length(hist.agent);
commsRadius = hist.agent(1).commsRadius; commsRadius = hist.agent(1).commsRadius;
collisionRadius = hist.agent(1).collisionRadius; collisionRadius = hist.agent(1).collisionRadius;
nPairs = nchoosek(nAgents, 2); nPairs = nchoosek(nAgents, 2);
T = size(hist.agent(1).pos, 1); T = size(hist.agent(1).pos, 1);
% Compute pairwise distances over time
pairDistMat = NaN(T, nPairs); pairDistMat = NaN(T, nPairs);
pp = 0;
for jj = 1:nAgents-1
for kk = jj+1:nAgents
pp = pp + 1;
pairDistMat(:, pp) = vecnorm(hist.agent(jj).pos - hist.agent(kk).pos, 2, 2);
end
end
% Cap at communications range
% pairDistMat = min(pairDistMat, commsRadius);
% Plot all pairwise distances over time
hold(x4, 'on');
hLeft = gobjects(nPairs, 1);
for pp = 1:nPairs
hLeft(pp) = plot(x4, pairDistMat(:, pp), 'LineWidth', 2);
end
yline(x4, collisionRadius, 'r--', "Label", "Collision Radius", "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
yline(x4, commsRadius, 'r--', "Label", "Communications Radius", "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
hold(x4, 'off');
xlabel(x4, "Timestep");
ylabel(x4, "Pairwise distance");
title(x4, "$AII\beta$ Pairwise Agent Distances and Barrier Function Values", "Interpreter", "latex");
grid(x4, "on");
ylim(x4, [0, commsRadius + 5]);
% Build legend labels
pairLabels = strings(nPairs, 1); pairLabels = strings(nPairs, 1);
pp = 0; pp = 0;
for jj = 1:nAgents-1 for jj = 1:nAgents-1
for kk = jj+1:nAgents for kk = jj+1:nAgents
pp = pp + 1; pp = pp + 1;
pairDistMat(:, pp) = vecnorm(hist.agent(jj).pos - hist.agent(kk).pos, 2, 2);
pairLabels(pp) = sprintf("Agents %d-%d Distance", jj, kk); pairLabels(pp) = sprintf("Agents %d-%d Distance", jj, kk);
end end
end end
l = legend(hLeft(:), pairLabels(:), "Location", "northeast"); f4 = figure;
x4 = axes;
% Left Y-axis: pairwise distances
hold(x4, 'on');
hLeft = gobjects(nPairs, 1);
for pp = 1:nPairs
hLeft(pp) = plot(x4, pairDistMat(:, pp), 'LineWidth', 2);
end
yline(x4, collisionRadius, 'r--', "Label", "Collision Radius", ...
"LabelHorizontalAlignment", "left", "HandleVisibility", "off");
yline(x4, commsRadius, 'r--', "Label", "Communications Radius", ...
"LabelHorizontalAlignment", "left", "HandleVisibility", "off");
hold(x4, 'off');
xlabel(x4, "Timestep");
ylabel(x4, "Pairwise distance");
title(x4, "$AII\beta$ Pairwise Agent Distances and Barrier Function Values", "Interpreter", "latex");
grid(x4, "on");
savefig(f4, "plot4_distanceOnly.fig"); savefig(f4, "plot4_distanceOnly.fig");
exportgraphics(f4, "plot4_distanceOnly.png"); exportgraphics(f4, "plot4_distanceOnly.png");
% Plot all barrier function values on right Y-axis % Right Y-axis: barrier function values
nObs = init.numObstacles; nObs = init.numObstacles;
nAA = nchoosek(nAgents, 2); nAA = nchoosek(nAgents, 2);
nAO = nAgents * nObs; nAO = nAgents * nObs;
nAD = nAgents * 6; nAD = nAgents * 6;
nComms = size(hist.barriers, 1) - nAA - nAO - nAD; nComms = size(hist.barriers, 1) - nAA - nAO - nAD;
colStart = 1;
comStart = colStart + nAA + nAO + nAD;
pairColors = lines(nAA);
yyaxis(x4, 'right'); yyaxis(x4, 'right');
hold(x4, 'on'); hold(x4, 'on');
hRight = gobjects(nAA + nComms, 1);
% Color palettes: pairs share colors across collision/comms, rightLabels = strings(nAA + nComms, 1);
% agents share colors across obstacle/domain idx = 0;
pairColors = lines(nAA);
agentColors = lines(nAgents);
% Row offsets in hist.barriers
colStart = 1;
obsStart = colStart + nAA;
domStart = obsStart + nAO;
comStart = domStart + nAD;
% Collision + Comms barriers grouped per pair (same color)
hRight = gobjects(0, 1);
rightLabels = strings(0, 1);
for pp = 1:nAA for pp = 1:nAA
hRight(end+1) = plot(x4, hist.barriers(colStart + pp - 1, :), '--', 'LineWidth', 1.5, 'Color', pairColors(pp, :)); idx = idx + 1;
rightLabels(end+1) = sprintf('h_{col} %d', pp); hRight(idx) = plot(x4, hist.barriers(colStart + pp - 1, :), '--', ...
'LineWidth', 1.5, 'Color', pairColors(pp, :));
rightLabels(idx) = sprintf('h_{col} %d', pp);
end end
for pp = 1:nComms for pp = 1:nComms
hRight(end+1) = plot(x4, hist.barriers(comStart + pp - 1, :), '-.', 'LineWidth', 1.5, 'Color', pairColors(pp, :)); idx = idx + 1;
rightLabels(end+1) = sprintf('h_{com} %d', pp); hRight(idx) = plot(x4, hist.barriers(comStart + pp - 1, :), '-.', ...
'LineWidth', 1.5, 'Color', pairColors(pp, :));
rightLabels(idx) = sprintf('h_{com} %d', pp);
end end
% Obstacle barriers colored by agent
% idx = obsStart;
% for aa = 1:nAgents
% for oo = 1:nObs
% hRight(end+1) = plot(x4, hist.barriers(idx, :), ':', 'LineWidth', 1, 'Color', agentColors(aa, :));
% rightLabels(end+1) = sprintf('h_{obs} a%d-o%d', aa, oo);
% idx = idx + 1;
% end
% end
hold(x4, 'off'); hold(x4, 'off');
ylabel(x4, "Barrier function $h$", "Interpreter", "latex"); ylabel(x4, "Barrier function $h$", "Interpreter", "latex");
% Clamp both Y-axes to start at 0 % Y-axis limits
yyaxis(x4, 'left'); ylim(x4, [0, 25]); yyaxis(x4, 'left'); ylim(x4, [0, 25]);
yyaxis(x4, 'right'); ylim(x4, [0, 275]); yyaxis(x4, 'right'); ylim(x4, [0, 275]);
x4.YAxis(2).Color = 'k'; x4.YAxis(2).Color = 'k';
% Combined legend legend([hLeft(:); hRight(:)], [pairLabels(:); rightLabels(:)], "Location", "eastoutside");
l = legend([hLeft(:); hRight(:)], [pairLabels(:); rightLabels(:)], "Location", "eastoutside");
savefig(f4, "plot4.fig"); savefig(f4, "plot4.fig");
exportgraphics(f4, "plot4.png"); exportgraphics(f4, "plot4.png");

View File

@@ -10,8 +10,8 @@ classdef results < matlab.unittest.TestCase
%% Diagnostic Parameters %% Diagnostic Parameters
% No effect on simulation dynamics % No effect on simulation dynamics
makeVideo = true; % disable video writing for big performance increase makeVideo = false; % disable video writing for big performance increase
makePlots = true; % disable plotting for big performance increase (also disables video) makePlots = false; % disable plotting for big performance increase (also disables video)
plotCommsGeometry = false; % disable plotting communications geometries plotCommsGeometry = false; % disable plotting communications geometries
%% Scenario Reinitialization %% Scenario Reinitialization
@@ -72,8 +72,8 @@ classdef results < matlab.unittest.TestCase
sensor2 = sigmoidSensor; sensor2 = sigmoidSensor;
sensor1 = sensor1.initialize(sensors(1).alphaDist, sensors(1).betaDist, sensors(1).alphaTilt, sensors(1).betaTilt); 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); sensor2 = sensor2.initialize(sensors(2).alphaDist, sensors(2).betaDist, sensors(2).alphaTilt, sensors(2).betaTilt);
sensor1.plotParameters; % sensor1.plotParameters;
sensor2.plotParameters; % sensor2.plotParameters;
c = struct('A_1_alpha', struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(1), 'doubleIntegrator', false), ... c = struct('A_1_alpha', struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(1), 'doubleIntegrator', false), ...
'A_1_beta', struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(1), 'doubleIntegrator', true), ... 'A_1_beta', struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(1), 'doubleIntegrator', true), ...
'A_2_alpha', struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(2), 'doubleIntegrator', false), ... 'A_2_alpha', struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(2), 'doubleIntegrator', false), ...
@@ -269,57 +269,57 @@ classdef results < matlab.unittest.TestCase
tc.testClass = tc.testClass.teardown(); tc.testClass = tc.testClass.teardown();
close all; close all;
end end
function AIIbeta_plots_3_4(tc) % function AIIbeta_plots_3_4(tc)
% test-specific parameters % % test-specific parameters
tc.makePlots = true; % tc.makePlots = true;
tc.makeVideo = true; % tc.makeVideo = true;
maxIters = 400; % maxIters = 400;
%
configs = results.makeConfigs(); % configs = results.makeConfigs();
c = configs.A_2_alpha; % c = configs.A_2_alpha;
c.doubleIntegrator = true; % make a2alpha into a2beta % c.doubleIntegrator = true; % make a2alpha into a2beta
%
% Set up fixed-size domain % % Set up fixed-size domain
minAlt = tc.domainSize(3)/10 + rand * 1/10 * tc.domainSize(3); % minAlt = tc.domainSize(3)/10 + rand * 1/10 * tc.domainSize(3);
tc.testClass.domain = tc.testClass.domain.initialize([zeros(1, 3); tc.domainSize], REGION_TYPE.DOMAIN, "Domain"); % tc.testClass.domain = tc.testClass.domain.initialize([zeros(1, 3); tc.domainSize], REGION_TYPE.DOMAIN, "Domain");
%
% Set objective % % Set objective
objectiveMu = [tc.domainSize(1) * 2 / 3, tc.domainSize(2) * 3 / 4]; % objectiveMu = [tc.domainSize(1) * 2 / 3, tc.domainSize(2) * 3 / 4];
objectiveSigma = reshape([215, 100; 100, 175], [1, 2, 2]); % objectiveSigma = reshape([215, 100; 100, 175], [1, 2, 2]);
tc.testClass.domain.objective = tc.testClass.domain.objective.initialize(objectiveFunctionWrapper(objectiveMu, objectiveSigma), tc.testClass.domain, tc.discretizationStep, tc.protectedRange, tc.sensorPerformanceMinimum, objectiveMu, objectiveSigma); % tc.testClass.domain.objective = tc.testClass.domain.objective.initialize(objectiveFunctionWrapper(objectiveMu, objectiveSigma), tc.testClass.domain, tc.discretizationStep, tc.protectedRange, tc.sensorPerformanceMinimum, objectiveMu, objectiveSigma);
%
% Set agent initial states (fully connected network of 4) % % Set agent initial states (fully connected network of 4)
centerPos = [tc.domainSize(1) / 4, tc.domainSize(2) / 4]; % centerPos = [tc.domainSize(1) / 4, tc.domainSize(2) / 4];
d = tc.collisionRadius + (tc.comRange - tc.collisionRadius) / 4; % d = tc.collisionRadius + (tc.comRange - tc.collisionRadius) / 4;
agentsPos = centerPos + [1, 1; 1, -1; -1, -1; -1, 1] / sqrt(2) * d; % agentsPos = centerPos + [1, 1; 1, -1; -1, -1; -1, 1] / sqrt(2) * d;
agentAlt = minAlt * 1.5; % agentAlt = minAlt * 1.5;
agentsPos = [agentsPos, agentAlt * ones(4, 1) + rand * 5 - 2.5]; % agentsPos = [agentsPos, agentAlt * ones(4, 1) + rand * 5 - 2.5];
%
agents = {agent, agent, agent, agent}; % agents = {agent, agent, agent, agent};
cg = spherical; % cg = spherical;
sensorModel = sigmoidSensor; % sensorModel = sigmoidSensor;
sensorModel = sensorModel.initialize(c.sensor.alphaDist, c.sensor.betaDist, c.sensor.alphaTilt, c.sensor.betaTilt); % sensorModel = sensorModel.initialize(c.sensor.alphaDist, c.sensor.betaDist, c.sensor.alphaTilt, c.sensor.betaTilt);
agents{1} = agents{1}.initialize(agentsPos(1, :), cg.initialize(agentsPos(1, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 1 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 1", false); % agents{1} = agents{1}.initialize(agentsPos(1, :), cg.initialize(agentsPos(1, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 1 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 1", false);
agents{2} = agents{2}.initialize(agentsPos(2, :), cg.initialize(agentsPos(2, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 2 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 2", false); % agents{2} = agents{2}.initialize(agentsPos(2, :), cg.initialize(agentsPos(2, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 2 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 2", false);
agents{3} = agents{3}.initialize(agentsPos(3, :), cg.initialize(agentsPos(3, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 3 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 3", false); % agents{3} = agents{3}.initialize(agentsPos(3, :), cg.initialize(agentsPos(3, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 3 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 3", false);
agents{4} = agents{4}.initialize(agentsPos(4, :), cg.initialize(agentsPos(4, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 4 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 4", false); % agents{4} = agents{4}.initialize(agentsPos(4, :), cg.initialize(agentsPos(4, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 4 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 4", false);
%
obstacles = cell(1, 1); % obstacles = cell(1, 1);
obstacles{1} = rectangularPrism; % obstacles{1} = rectangularPrism;
obstacles{1} = obstacles{1}.initialize([0, tc.domainSize(2)/2, 0; tc.domainSize(1) * 0.4, tc.domainSize(2), 40],REGION_TYPE.OBSTACLE, "Obstacle 1"); % obstacles{1} = obstacles{1}.initialize([0, tc.domainSize(2)/2, 0; tc.domainSize(1) * 0.4, tc.domainSize(2), 40],REGION_TYPE.OBSTACLE, "Obstacle 1");
%
% Set up simulation % % Set up simulation
tc.testClass = tc.testClass.initialize(tc.testClass.domain, agents, tc.barrierGain, tc.barrierExponent, minAlt, tc.timestep, maxIters, obstacles, tc.makePlots, tc.makeVideo, c.doubleIntegrator, tc.dampingCoeff, tc.useFixedTopology); % tc.testClass = tc.testClass.initialize(tc.testClass.domain, agents, tc.barrierGain, tc.barrierExponent, minAlt, tc.timestep, maxIters, obstacles, tc.makePlots, tc.makeVideo, c.doubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
%
% Save simulation parameters to output file % % Save simulation parameters to output file
tc.testClass.writeInits(); % tc.testClass.writeInits();
%
% Run % % Run
tc.testClass = tc.testClass.run(); % tc.testClass = tc.testClass.run();
%
% Cleanup % % Cleanup
tc.testClass = tc.testClass.teardown(); % tc.testClass = tc.testClass.teardown();
end % end
end end
methods methods