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
|
||||
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
|
||||
obj = obj.plot();
|
||||
|
||||
|
||||
@@ -27,6 +27,7 @@ classdef miSim
|
||||
spatialPlotIndices = [6, 4, 3, 2];
|
||||
numBarriers = 0; % Number of barrier functions needed
|
||||
barriers = []; % log barrier function values at each timestep for analysis
|
||||
constraintAdjacencyHist = []; % log constraint adjacency matrix at each timestep
|
||||
end
|
||||
|
||||
properties (Access = private)
|
||||
|
||||
@@ -34,6 +34,11 @@ function [obj] = run(obj)
|
||||
obj = obj.lesserNeighbor();
|
||||
end
|
||||
|
||||
% Log constraint adjacency for this timestep
|
||||
if coder.target('MATLAB')
|
||||
obj.constraintAdjacencyHist(:, :, ii) = obj.constraintAdjacencyMatrix;
|
||||
end
|
||||
|
||||
% Moving
|
||||
% Iterate over agents to simulate their unconstrained motion
|
||||
for jj = 1:size(obj.agents, 1)
|
||||
|
||||
@@ -20,6 +20,7 @@ function obj = teardown(obj)
|
||||
out.dampingCoeff = obj.dampingCoeff;
|
||||
out.useDoubleIntegrator = obj.useDoubleIntegrator;
|
||||
out.useFixedTopology = obj.useFixedTopology;
|
||||
out.constraintAdjacency = obj.constraintAdjacencyHist(:, :, 1:(end - 1));
|
||||
for ii = 1:size(obj.agents, 1)
|
||||
out.agent(ii).pos = squeeze(obj.posHist(ii, 1:(end - 1), 1:3));
|
||||
out.agent(ii).vel = squeeze(obj.velHist(ii, 1:(end - 1), 1:3));
|
||||
@@ -43,6 +44,7 @@ function obj = teardown(obj)
|
||||
obj.agents = cell(0, 1);
|
||||
obj.adjacency = NaN;
|
||||
obj.constraintAdjacencyMatrix = NaN;
|
||||
obj.constraintAdjacencyHist = [];
|
||||
obj.partitioning = NaN;
|
||||
obj.performance = 0;
|
||||
obj.barrierGain = NaN;
|
||||
|
||||
151
plot_1.m
151
plot_1.m
@@ -1,23 +1,27 @@
|
||||
clear;
|
||||
% Load data
|
||||
|
||||
%% 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'));
|
||||
dataFiles = dir(dataPath);
|
||||
dataFiles = dataFiles(~startsWith({dataFiles.name}, '.'));
|
||||
simInits = dataFiles(endsWith({dataFiles.name}, 'miSimInits.mat'));
|
||||
simHists = dataFiles(endsWith({dataFiles.name}, 'miSimHist.mat'));
|
||||
assert(length(simHists) == length(simInits), "input data availability mismatch");
|
||||
|
||||
% Initialize plotting data
|
||||
%% Aggregate run data
|
||||
nRuns = length(simHists);
|
||||
Cfinal = NaN(nRuns, 1);
|
||||
n = NaN(nRuns, 1);
|
||||
nAgents = NaN(nRuns, 1);
|
||||
doubleIntegrator = NaN(nRuns, 1);
|
||||
numObjective = NaN(nRuns, 1);
|
||||
positions = cell(6, nRuns);
|
||||
commsRadius = 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:length(simHists)
|
||||
for ii = 1:nRuns
|
||||
initName = strrep(simInits(ii).name, "_miSimInits.mat", "");
|
||||
histName = strrep(simHists(ii).name, "_miSimHist.mat", "");
|
||||
assert(initName == histName);
|
||||
@@ -25,38 +29,29 @@ for ii = 1:length(simHists)
|
||||
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;
|
||||
nAgents(ii) = init.numAgents;
|
||||
doubleIntegrator(ii) = init.useDoubleIntegrator;
|
||||
numObjective(ii) = size(init.objectivePos, 1);
|
||||
commsRadius(ii) = unique(init.comRange);
|
||||
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;
|
||||
positions{jj, ii} = hist.out.agent(jj).pos;
|
||||
assert(hist.out.agent(jj).commsRadius == commsRadius(ii));
|
||||
assert(hist.out.agent(jj).collisionRadius == collisionRadius(ii));
|
||||
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
|
||||
|
||||
commsRadius = unique(commsRadius); assert(isscalar(commsRadius));
|
||||
collisionRadius = unique(collisionRadius); assert(isscalar(collisionRadius));
|
||||
sensors = flip(unique(alphaDist(1, :)));
|
||||
n_unique = sort(unique(n));
|
||||
nGroups = length(n_unique);
|
||||
sensorTypes = flip(unique(alphaDist(1, :)));
|
||||
nValues = sort(unique(nAgents));
|
||||
nGroups = length(nValues);
|
||||
|
||||
% Build config label for each run
|
||||
config = strings(nRuns, 1);
|
||||
%% Build config labels
|
||||
baseConfig = strings(nRuns, 1);
|
||||
for ii = 1:nRuns
|
||||
s = "";
|
||||
@@ -65,9 +60,9 @@ for ii = 1:nRuns
|
||||
elseif numObjective(ii) == 2
|
||||
s = s + "B";
|
||||
end
|
||||
if alphaDist(1, ii) == sensors(1)
|
||||
if alphaDist(1, ii) == sensorTypes(1)
|
||||
s = s + "_I";
|
||||
elseif alphaDist(1, ii) == sensors(2)
|
||||
elseif alphaDist(1, ii) == sensorTypes(2)
|
||||
s = s + "_II";
|
||||
end
|
||||
if ~doubleIntegrator(ii)
|
||||
@@ -76,88 +71,77 @@ for ii = 1:nRuns
|
||||
s = s + "_beta";
|
||||
end
|
||||
baseConfig(ii) = s;
|
||||
config(ii) = n(ii) + "_" + s;
|
||||
end
|
||||
configOrder = unique(baseConfig(n == n_unique(1)), 'stable');
|
||||
nConfigsPerN = length(configOrder);
|
||||
configOrder = unique(baseConfig(nAgents == nValues(1)), 'stable');
|
||||
nConfigs = length(configOrder);
|
||||
configLabels = ["$AI\alpha$"; "$AI\beta$"; "$AII\alpha$"; "$BI\beta$"];
|
||||
|
||||
%%
|
||||
%% Plot 1: Final normalized coverage
|
||||
close all;
|
||||
f1 = figure;
|
||||
x1 = axes;
|
||||
|
||||
C_mean = NaN(nGroups, nConfigsPerN);
|
||||
C_var = NaN(nGroups, nConfigsPerN);
|
||||
C_mean = NaN(nGroups, nConfigs);
|
||||
C_var = NaN(nGroups, nConfigs);
|
||||
for ii = 1:nGroups
|
||||
for jj = 1:nConfigsPerN
|
||||
mask = (n == n_unique(ii)) & (baseConfig == configOrder(jj));
|
||||
for jj = 1:nConfigs
|
||||
mask = (nAgents == nValues(ii)) & (baseConfig == configOrder(jj));
|
||||
C_mean(ii, jj) = mean(Cfinal(mask));
|
||||
C_var(ii, jj) = var(Cfinal(mask));
|
||||
end
|
||||
end
|
||||
|
||||
hBar = bar(x1, C_mean);
|
||||
hold(x1, 'on');
|
||||
for jj = 1:nConfigsPerN
|
||||
xPos = hBar(jj).XEndPoints;
|
||||
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
|
||||
end
|
||||
hold(x1, 'off');
|
||||
set(x1, 'XTickLabel', string(n_unique));
|
||||
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]);
|
||||
bar(x1, C_mean);
|
||||
set(x1, 'XTickLabel', string(nValues));
|
||||
xlabel(x1, "Number of agents");
|
||||
ylabel(x1, "Final coverage (normalized)");
|
||||
title(x1, "Final performance of parameterizations");
|
||||
legend(x1, configLabels, "Interpreter", "latex", "Location", "northwest");
|
||||
grid(x1, "on");
|
||||
ylim(x1, [0, 1/2]);
|
||||
|
||||
savefig(f1, "plot1.fig");
|
||||
exportgraphics(f1, "plot1.png");
|
||||
|
||||
%%
|
||||
%% Plot 2: Pairwise agent distances
|
||||
f2 = figure;
|
||||
x2 = axes;
|
||||
|
||||
% Compute pairwise distances between agents
|
||||
maxPairs = nchoosek(6, 2);
|
||||
% Compute pairwise distances only for connected agents (static topology)
|
||||
maxPairs = nchoosek(maxAgents, 2);
|
||||
pairDist = cell(maxPairs, nRuns);
|
||||
for ii = 1:nRuns
|
||||
A = adjacency{ii};
|
||||
pp = 0;
|
||||
for jj = 1:n(ii)-1
|
||||
for kk = jj+1:n(ii)
|
||||
for jj = 1:nAgents(ii)-1
|
||||
for kk = jj+1:nAgents(ii)
|
||||
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
|
||||
|
||||
% 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
|
||||
|
||||
% 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);
|
||||
minPairDist = NaN(nRuns, 1);
|
||||
maxPairDist = NaN(nRuns, 1);
|
||||
for ii = 1:nRuns
|
||||
nPairs = nchoosek(n(ii), 2);
|
||||
nPairs = nchoosek(nAgents(ii), 2);
|
||||
D = vertcat(pairDist{1:nPairs, ii});
|
||||
meanPairDist(ii) = mean(D);
|
||||
meanPairDist(ii) = mean(D, "omitmissing");
|
||||
minPairDist(ii) = min(D);
|
||||
maxPairDist(ii) = max(D);
|
||||
end
|
||||
|
||||
% Group pairwise distance stats by (n, config), aggregating across reps
|
||||
meanD = NaN(nGroups, nConfigsPerN);
|
||||
minD = NaN(nGroups, nConfigsPerN);
|
||||
maxD = NaN(nGroups, nConfigsPerN);
|
||||
% Aggregate across trials per (n, config) group
|
||||
meanD = NaN(nGroups, nConfigs);
|
||||
minD = NaN(nGroups, nConfigs);
|
||||
maxD = NaN(nGroups, nConfigs);
|
||||
for ii = 1:nGroups
|
||||
for jj = 1:nConfigsPerN
|
||||
mask = (n == n_unique(ii)) & (baseConfig == configOrder(jj));
|
||||
for jj = 1:nConfigs
|
||||
mask = (nAgents == nValues(ii)) & (baseConfig == configOrder(jj));
|
||||
meanD(ii, jj) = mean(meanPairDist(mask));
|
||||
minD(ii, jj) = min(minPairDist(mask));
|
||||
maxD(ii, jj) = max(maxPairDist(mask));
|
||||
@@ -166,24 +150,25 @@ end
|
||||
|
||||
% Plot whiskers (min to max) with mean markers
|
||||
barWidth = 0.8;
|
||||
groupWidth = barWidth / nConfigsPerN;
|
||||
groupWidth = barWidth / nConfigs;
|
||||
hold(x2, 'on');
|
||||
for jj = 1:nConfigsPerN
|
||||
xPos = (1:nGroups) + (jj - (nConfigsPerN + 1) / 2) * groupWidth;
|
||||
for jj = 1:nConfigs
|
||||
xPos = (1:nGroups) + (jj - (nConfigs + 1) / 2) * groupWidth;
|
||||
errorbar(x2, xPos, meanD(:, jj), meanD(:, jj) - minD(:, jj), maxD(:, jj) - meanD(:, jj), ...
|
||||
'o', 'LineWidth', 1.5, 'MarkerSize', 6, 'CapSize', 10);
|
||||
end
|
||||
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");
|
||||
ylabel(x2, "Pairwise distance");
|
||||
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");
|
||||
|
||||
yline(collisionRadius, 'r--', "Label", "Collision Radius", "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
|
||||
yline(commsRadius, 'r--', "Label", "Communications Radius", "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
|
||||
ylim([0, commsRadius + 5]);
|
||||
yline(x2, collisionRadius, 'r--', "Label", "Collision Radius", ...
|
||||
"LabelHorizontalAlignment", "left", "HandleVisibility", "off");
|
||||
yline(x2, commsRadius, 'r--', "Label", "Communications Radius", ...
|
||||
"LabelHorizontalAlignment", "left", "HandleVisibility", "off");
|
||||
ylim(x2, [0, commsRadius + 5]);
|
||||
|
||||
savefig(f2, "plot2.fig");
|
||||
exportgraphics(f2, "plot2.png");
|
||||
exportgraphics(f2, "plot2.png");
|
||||
|
||||
156
plot_3.m
156
plot_3.m
@@ -1,142 +1,120 @@
|
||||
clear;
|
||||
% Load data
|
||||
|
||||
%% Load data
|
||||
dataPath = fullfile('.', 'sandbox', 'plot3');
|
||||
simHists = dir(dataPath); simHists = simHists(3:end);
|
||||
simInits = simHists(endsWith({simHists.name}, 'miSimInits.mat'));
|
||||
simHists = simHists(endsWith({simHists.name}, 'miSimHist.mat'));
|
||||
dataFiles = dir(dataPath);
|
||||
dataFiles = dataFiles(~startsWith({dataFiles.name}, '.'));
|
||||
simInits = dataFiles(endsWith({dataFiles.name}, 'miSimInits.mat'));
|
||||
simHists = dataFiles(endsWith({dataFiles.name}, 'miSimHist.mat'));
|
||||
assert(length(simHists) == length(simInits), "input data availability mismatch");
|
||||
assert(isscalar(simHists));
|
||||
|
||||
init = fullfile(simInits(1).folder, simInits(1).name);
|
||||
hist = fullfile(simHists(1).folder, simHists(1).name);
|
||||
|
||||
init = load(init);
|
||||
hist = load(hist);
|
||||
init = load(fullfile(simInits(1).folder, simInits(1).name));
|
||||
hist = load(fullfile(simHists(1).folder, simHists(1).name));
|
||||
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;
|
||||
x3 = axes;
|
||||
assert(size(init.objectivePos, 1) == 1)
|
||||
assert(hist.useDoubleIntegrator);
|
||||
|
||||
plot(hist.perf./init.objectiveIntegral, "LineWidth", 2);
|
||||
hold("on");
|
||||
for ii = 1:length(hist.agent)
|
||||
plot(hist.agent(ii).perf./init.objectiveIntegral, "LineWidth", 2);
|
||||
hold(x3, 'on');
|
||||
plot(x3, hist.perf ./ init.objectiveIntegral, "LineWidth", 2);
|
||||
for ii = 1:nAgents
|
||||
plot(x3, hist.agent(ii).perf ./ init.objectiveIntegral, "LineWidth", 2);
|
||||
end
|
||||
grid("on");
|
||||
ylabel("Performance (normalized)");
|
||||
xlabel("Timestep");
|
||||
legend(["Cumulative"; "Agent 1"; "Agent 2"; "Agent 3"; "Agent 4"], "Location", "northwest");
|
||||
title("$AII\beta$ Performance", "Interpreter", "latex");
|
||||
hold(x3, 'off');
|
||||
grid(x3, "on");
|
||||
ylabel(x3, "Performance (normalized)");
|
||||
xlabel(x3, "Timestep");
|
||||
legend(x3, ["Cumulative"; agentLabels], "Location", "northwest");
|
||||
title(x3, "$AII\beta$ Performance", "Interpreter", "latex");
|
||||
|
||||
savefig(f3, "plot3.fig");
|
||||
exportgraphics(f3, "plot3.png");
|
||||
|
||||
f4 = figure;
|
||||
x4 = axes;
|
||||
|
||||
% Compute pairwise distances between agents over time
|
||||
nAgents = length(hist.agent);
|
||||
%% Plot 4: Pairwise distances and barrier functions
|
||||
commsRadius = hist.agent(1).commsRadius;
|
||||
collisionRadius = hist.agent(1).collisionRadius;
|
||||
nPairs = nchoosek(nAgents, 2);
|
||||
T = size(hist.agent(1).pos, 1);
|
||||
|
||||
% Compute pairwise distances over time
|
||||
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);
|
||||
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);
|
||||
pairLabels(pp) = sprintf("Agents %d-%d Distance", jj, kk);
|
||||
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");
|
||||
exportgraphics(f4, "plot4_distanceOnly.png");
|
||||
|
||||
% Plot all barrier function values on right Y-axis
|
||||
% Right Y-axis: barrier function values
|
||||
nObs = init.numObstacles;
|
||||
nAA = nchoosek(nAgents, 2);
|
||||
nAO = nAgents * nObs;
|
||||
nAD = nAgents * 6;
|
||||
nComms = size(hist.barriers, 1) - nAA - nAO - nAD;
|
||||
|
||||
colStart = 1;
|
||||
comStart = colStart + nAA + nAO + nAD;
|
||||
|
||||
pairColors = lines(nAA);
|
||||
|
||||
yyaxis(x4, 'right');
|
||||
hold(x4, 'on');
|
||||
|
||||
% Color palettes: pairs share colors across collision/comms,
|
||||
% agents share colors across obstacle/domain
|
||||
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);
|
||||
hRight = gobjects(nAA + nComms, 1);
|
||||
rightLabels = strings(nAA + nComms, 1);
|
||||
idx = 0;
|
||||
for pp = 1:nAA
|
||||
hRight(end+1) = plot(x4, hist.barriers(colStart + pp - 1, :), '--', 'LineWidth', 1.5, 'Color', pairColors(pp, :));
|
||||
rightLabels(end+1) = sprintf('h_{col} %d', pp);
|
||||
idx = idx + 1;
|
||||
hRight(idx) = plot(x4, hist.barriers(colStart + pp - 1, :), '--', ...
|
||||
'LineWidth', 1.5, 'Color', pairColors(pp, :));
|
||||
rightLabels(idx) = sprintf('h_{col} %d', pp);
|
||||
end
|
||||
for pp = 1:nComms
|
||||
hRight(end+1) = plot(x4, hist.barriers(comStart + pp - 1, :), '-.', 'LineWidth', 1.5, 'Color', pairColors(pp, :));
|
||||
rightLabels(end+1) = sprintf('h_{com} %d', pp);
|
||||
idx = idx + 1;
|
||||
hRight(idx) = plot(x4, hist.barriers(comStart + pp - 1, :), '-.', ...
|
||||
'LineWidth', 1.5, 'Color', pairColors(pp, :));
|
||||
rightLabels(idx) = sprintf('h_{com} %d', pp);
|
||||
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');
|
||||
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, 'right'); ylim(x4, [0, 275]);
|
||||
x4.YAxis(2).Color = 'k';
|
||||
|
||||
% Combined legend
|
||||
l = legend([hLeft(:); hRight(:)], [pairLabels(:); rightLabels(:)], "Location", "eastoutside");
|
||||
legend([hLeft(:); hRight(:)], [pairLabels(:); rightLabels(:)], "Location", "eastoutside");
|
||||
|
||||
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
|
||||
% No effect on simulation dynamics
|
||||
makeVideo = true; % disable video writing for big performance increase
|
||||
makePlots = true; % disable plotting for big performance increase (also disables video)
|
||||
makeVideo = false; % disable video writing for big performance increase
|
||||
makePlots = false; % disable plotting for big performance increase (also disables video)
|
||||
plotCommsGeometry = false; % disable plotting communications geometries
|
||||
|
||||
%% Scenario Reinitialization
|
||||
@@ -72,8 +72,8 @@ classdef results < matlab.unittest.TestCase
|
||||
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;
|
||||
% sensor1.plotParameters;
|
||||
% sensor2.plotParameters;
|
||||
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_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();
|
||||
close all;
|
||||
end
|
||||
function AIIbeta_plots_3_4(tc)
|
||||
% test-specific parameters
|
||||
tc.makePlots = true;
|
||||
tc.makeVideo = true;
|
||||
maxIters = 400;
|
||||
|
||||
configs = results.makeConfigs();
|
||||
c = configs.A_2_alpha;
|
||||
c.doubleIntegrator = true; % make a2alpha into a2beta
|
||||
|
||||
% Set up fixed-size domain
|
||||
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");
|
||||
|
||||
% Set objective
|
||||
objectiveMu = [tc.domainSize(1) * 2 / 3, tc.domainSize(2) * 3 / 4];
|
||||
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);
|
||||
|
||||
% Set agent initial states (fully connected network of 4)
|
||||
centerPos = [tc.domainSize(1) / 4, tc.domainSize(2) / 4];
|
||||
d = tc.collisionRadius + (tc.comRange - tc.collisionRadius) / 4;
|
||||
agentsPos = centerPos + [1, 1; 1, -1; -1, -1; -1, 1] / sqrt(2) * d;
|
||||
agentAlt = minAlt * 1.5;
|
||||
agentsPos = [agentsPos, agentAlt * ones(4, 1) + rand * 5 - 2.5];
|
||||
|
||||
agents = {agent, agent, agent, agent};
|
||||
cg = spherical;
|
||||
sensorModel = sigmoidSensor;
|
||||
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{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{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{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");
|
||||
|
||||
% 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);
|
||||
|
||||
% Save simulation parameters to output file
|
||||
tc.testClass.writeInits();
|
||||
|
||||
% Run
|
||||
tc.testClass = tc.testClass.run();
|
||||
|
||||
% Cleanup
|
||||
tc.testClass = tc.testClass.teardown();
|
||||
end
|
||||
% function AIIbeta_plots_3_4(tc)
|
||||
% % test-specific parameters
|
||||
% tc.makePlots = true;
|
||||
% tc.makeVideo = true;
|
||||
% maxIters = 400;
|
||||
%
|
||||
% configs = results.makeConfigs();
|
||||
% c = configs.A_2_alpha;
|
||||
% c.doubleIntegrator = true; % make a2alpha into a2beta
|
||||
%
|
||||
% % Set up fixed-size domain
|
||||
% 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");
|
||||
%
|
||||
% % Set objective
|
||||
% objectiveMu = [tc.domainSize(1) * 2 / 3, tc.domainSize(2) * 3 / 4];
|
||||
% 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);
|
||||
%
|
||||
% % Set agent initial states (fully connected network of 4)
|
||||
% centerPos = [tc.domainSize(1) / 4, tc.domainSize(2) / 4];
|
||||
% d = tc.collisionRadius + (tc.comRange - tc.collisionRadius) / 4;
|
||||
% agentsPos = centerPos + [1, 1; 1, -1; -1, -1; -1, 1] / sqrt(2) * d;
|
||||
% agentAlt = minAlt * 1.5;
|
||||
% agentsPos = [agentsPos, agentAlt * ones(4, 1) + rand * 5 - 2.5];
|
||||
%
|
||||
% agents = {agent, agent, agent, agent};
|
||||
% cg = spherical;
|
||||
% sensorModel = sigmoidSensor;
|
||||
% 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{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{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{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");
|
||||
%
|
||||
% % 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);
|
||||
%
|
||||
% % Save simulation parameters to output file
|
||||
% tc.testClass.writeInits();
|
||||
%
|
||||
% % Run
|
||||
% tc.testClass = tc.testClass.run();
|
||||
%
|
||||
% % Cleanup
|
||||
% tc.testClass = tc.testClass.teardown();
|
||||
% end
|
||||
end
|
||||
|
||||
methods
|
||||
|
||||
Reference in New Issue
Block a user