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

added h plots

Browse Source
This commit is contained in:
Kevin Dee
2026-01-06 21:57:30 -08:00
parent 591430ad8a
commit ddecf63d68
6 changed files with 89 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
@miSim/constrainMotion.m
View File

@@ -96,6 +96,9 @@ function [obj] = constrainMotion(obj)
kk = kk + 1;
end
% Save off h function values (ignoring network constraints which may evolve in time)
obj.h(:, obj.timestepIndex) = [h(triu(true(size(obj.agents, 1)), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;];
% Add communication network constraints
hComms = NaN(size(obj.agents, 1));
hComms(logical(eye(size(obj.agents, 1)))) = 0;

3
@miSim/initialize.m
View File

@@ -78,6 +78,9 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
% Prepare performance data store (at t = 0, all have 0 performance)
obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
% Prepare h function data store
obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1) - 1);
% Create initial partitioning
obj = obj.partition();

8
@miSim/miSim.m
View File

@@ -46,6 +46,13 @@ classdef miSim
objectivePlotIndices = [6, 4];
networkGraphIndex = 5;
partitionGraphIndex = 1;
% CBF plotting
h; % h function values
hf; % h function plotting figure
caPlot; % objects for collision avoidance h function plot
obsPlot; % objects for obstacle h function plot
domPlot; % objects for domain h function plot
end
methods (Access = public)
@@ -60,6 +67,7 @@ classdef miSim
[obj] = plotPartitions(obj);
[obj] = plotGraph(obj);
[obj] = plotTrails(obj);
[obj] = plotH(obj);
[obj] = updatePlots(obj, updatePartitions);
validate(obj);
end

3
@miSim/plot.m
View File

@@ -48,4 +48,7 @@ function obj = plot(obj)
% Plot performance
obj = obj.plotPerformance();
% Plot h functions
obj = obj.plotH();
end

61
@miSim/plotH.m Normal file
View File

@@ -0,0 +1,61 @@
function obj = plotH(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
obj.hf = figure;
tiledlayout(obj.hf, 4, 1, "TileSpacing", "tight", "Padding", "compact");
nexttile(obj.hf.Children(1));
axes(obj.hf.Children(1).Children(1));
grid(obj.hf.Children(1).Children(1), "on");
xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
title(obj.hf.Children(1).Children(1), "Collision Avoidance");
hold(obj.hf.Children(1).Children(1), "on");
obj.caPlot = plot(obj.h(1:(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2), :)');
legendStrings = [];
for ii = 2:size(obj.agents, 1)
for jj = 1:(ii - 1)
legendStrings = [legendStrings; sprintf("A%d A%d", jj, ii)];
end
end
legend(obj.hf.Children(1).Children(1), legendStrings, 'Location', 'bestoutside');
hold(obj.hf.Children(1).Children(2), "off");
nexttile(obj.hf.Children(1));
axes(obj.hf.Children(1).Children(1));
grid(obj.hf.Children(1).Children(1), "on");
xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(2), "");
title(obj.hf.Children(1).Children(1), "Obstacles");
hold(obj.hf.Children(1).Children(1), "on");
obj.obsPlot = plot(obj.h((1 + (size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)):(((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1)), :)');
legendStrings = [];
for ii = 1:size(obj.obstacles, 1)
for jj = 1:size(obj.agents, 1)
legendStrings = [legendStrings; sprintf("A%d O%d", jj, ii)];
end
end
legend(obj.hf.Children(1).Children(1), legendStrings, 'Location', 'bestoutside');
hold(obj.hf.Children(1).Children(2), "off");
nexttile(obj.hf.Children(1));
axes(obj.hf.Children(1).Children(1));
grid(obj.hf.Children(1).Children(1), "on");
xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
title(obj.hf.Children(1).Children(1), "Domain");
hold(obj.hf.Children(1).Children(1), "on");
obj.domPlot = plot(obj.h((1 + (((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1))):size(obj.h, 1), 1:end)');
legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], 'Location', 'bestoutside');
hold(obj.hf.Children(1).Children(2), "off");
nexttile(obj.hf.Children(1));
axes(obj.hf.Children(1).Children(1));
grid(obj.hf.Children(1).Children(1), "on");
xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
title(obj.hf.Children(1).Children(1), "Communications");
% skipped this for now because it is very complicated
end

11
@miSim/updatePlots.m
View File

@@ -60,4 +60,15 @@ function [obj] = updatePlots(obj, updatePartitions)
obj.performancePlot(ii).YData(1:length(obj.performance)) = obj.agents{ii - 1}.performance * normalizingFactor;
obj.performancePlot(ii).XData(obj.timestepIndex) = obj.t;
end
% Update h function plots
for ii = 1:size(obj.caPlot, 1)
obj.caPlot(ii).YData(obj.timestepIndex) = obj.h(ii, obj.timestepIndex);
end
for ii = 1:size(obj.obsPlot, 1)
obj.obsPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1), obj.timestepIndex);
end
for ii = 1:size(obj.domPlot, 1)
obj.domPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1) + size(obj.obsPlot, 1), obj.timestepIndex);
end
end