last plot updates
This commit is contained in:
@@ -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();
|
||||||
|
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|||||||
149
plot_1.m
149
plot_1.m
@@ -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;
|
||||||
pairDist{pp, ii} = vecnorm(positions{jj, ii} - positions{kk, ii}, 2, 2);
|
if A(jj, kk)
|
||||||
|
pairDist{pp, ii} = vecnorm(positions{jj, ii} - positions{kk, ii}, 2, 2);
|
||||||
|
end
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
% Cap pairwise distances at communications range
|
% Per-run statistics across all pairs and timesteps
|
||||||
for ii = 1:nRuns
|
|
||||||
nPairs = nchoosek(n(ii), 2);
|
|
||||||
for pp = 1:nPairs
|
|
||||||
pairDist{pp, ii} = min(pairDist{pp, ii}, commsRadius);
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
% Compute mean, min, max pairwise distance across all pairs and timesteps per run
|
|
||||||
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
154
plot_3.m
@@ -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");
|
||||||
110
test/results.m
110
test/results.m
@@ -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
|
||||||
|
|||||||
Reference in New Issue
Block a user