From ddecf63d68b79d55eccb21549231c4cbf9641b5a Mon Sep 17 00:00:00 2001 From: Kevin D Date: Tue, 6 Jan 2026 21:57:30 -0800 Subject: [PATCH] added h plots --- @miSim/constrainMotion.m | 3 ++ @miSim/initialize.m | 3 ++ @miSim/miSim.m | 8 ++++++ @miSim/plot.m | 3 ++ @miSim/plotH.m | 61 ++++++++++++++++++++++++++++++++++++++++ @miSim/updatePlots.m | 11 ++++++++ 6 files changed, 89 insertions(+) create mode 100644 @miSim/plotH.m diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 71ebdad..d27a910 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -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; diff --git a/@miSim/initialize.m b/@miSim/initialize.m index d2457cb..60e9e85 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -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(); diff --git a/@miSim/miSim.m b/@miSim/miSim.m index dc2f517..cc3e5ad 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -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 diff --git a/@miSim/plot.m b/@miSim/plot.m index 4782a16..52e1d3b 100644 --- a/@miSim/plot.m +++ b/@miSim/plot.m @@ -48,4 +48,7 @@ function obj = plot(obj) % Plot performance obj = obj.plotPerformance(); + + % Plot h functions + obj = obj.plotH(); end \ No newline at end of file diff --git a/@miSim/plotH.m b/@miSim/plotH.m new file mode 100644 index 0000000..5b4a7c2 --- /dev/null +++ b/@miSim/plotH.m @@ -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 \ No newline at end of file diff --git a/@miSim/updatePlots.m b/@miSim/updatePlots.m index 017b3d5..c2ee5fb 100644 --- a/@miSim/updatePlots.m +++ b/@miSim/updatePlots.m @@ -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 \ No newline at end of file