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

implemented partitioning

Browse Source
This commit is contained in:
Kevin D
2025-11-11 12:50:43 -08:00
parent 74088a13f3
commit 9e948072e8
8 changed files with 295 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
agent.m
View File

@@ -114,9 +114,10 @@ classdef agent
% Network connections
end
function [obj, f] = plot(obj, f)
function [obj, f] = plot(obj, ind, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'agent')};
ind (1, :) double = NaN;
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
@@ -128,22 +129,22 @@ classdef agent
f = firstPlotSetup(f);
% Plot points representing the agent position
hold(f.CurrentAxes, "on");
o = scatter3(obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
hold(f.CurrentAxes, "off");
hold(f.Children(1).Children(end), "on");
o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
hold(f.Children(1).Children(end), "off");
% Check if this is a tiled layout figure
if strcmp(f.Children(1).Type, 'tiledlayout')
% Add to other perspectives
o = [o; copyobj(o(1), f.Children(1).Children(2))];
o = [o; copyobj(o(1), f.Children(1).Children(3))];
o = [o; copyobj(o(1), f.Children(1).Children(5))];
o = [o; copyobj(o(1), f.Children(1).Children(4))];
end
obj.scatterPoints = o;
% Plot collision geometry
[obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(f);
[obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(ind, f);
end
end
end

33
firstPlotSetup.m
View File

@@ -1,9 +1,15 @@
function f = firstPlotSetup(f)
arguments (Input)
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
end
if isempty(f.CurrentAxes)
tiledlayout(f, 5, 4, "TileSpacing", "tight", "Padding", "compact");
tiledlayout(f, 5, 5, "TileSpacing", "tight", "Padding", "compact");
% 3D view
nexttile(1, [4, 4]);
nexttile(1, [4, 5]);
axes(f.Children(1).Children(1));
axis(f.Children(1).Children(1), "image");
grid(f.Children(1).Children(1), "on");
@@ -12,7 +18,7 @@ function f = firstPlotSetup(f)
title(f.Children(1).Children(1), "3D View");
% Communications graph
nexttile(17, [1, 1]);
nexttile(21, [1, 1]);
axes(f.Children(1).Children(1));
axis(f.Children(1).Children(1), "image");
grid(f.Children(1).Children(1), "off");
@@ -28,7 +34,7 @@ function f = firstPlotSetup(f)
set(f.Children(1).Children(1), 'YColor', 'none');
% Top-down view
nexttile(18, [1, 1]);
nexttile(22, [1, 1]);
axes(f.Children(1).Children(1));
axis(f.Children(1).Children(1), "image");
grid(f.Children(1).Children(1), "on");
@@ -37,7 +43,7 @@ function f = firstPlotSetup(f)
title(f.Children(1).Children(1), "Top-down View");
% Side-on view
nexttile(19, [1, 1]);
nexttile(23, [1, 1]);
axes(f.Children(1).Children(1));
axis(f.Children(1).Children(1), "image");
grid(f.Children(1).Children(1), "on");
@@ -46,12 +52,27 @@ function f = firstPlotSetup(f)
title(f.Children(1).Children(1), "Side-on View");
% Front-on view
nexttile(20, [1, 1]);
nexttile(24, [1, 1]);
axes(f.Children(1).Children(1));
axis(f.Children(1).Children(1), "image");
grid(f.Children(1).Children(1), "on");
view(f.Children(1).Children(1), 0, 0);
xlabel(f.Children(1).Children(1), "X"); zlabel(f.Children(1).Children(1), "Z");
title(f.Children(1).Children(1), "Front-on View");
% Partitioning
nexttile(25, [1, 1]);
axes(f.Children(1).Children(1));
axis(f.Children(1).Children(1), "image");
grid(f.Children(1).Children(1), "on");
view(f.Children(1).Children(1), 0, 90);
xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y");
title(f.Children(1).Children(1), "Domain Partitioning");
set(f.Children(1).Children(1), 'XTickLabelMode', 'manual');
set(f.Children(1).Children(1), 'YTickLabelMode', 'manual');
set(f.Children(1).Children(1), 'XTickLabel', {});
set(f.Children(1).Children(1), 'YTickLabel', {});
set(f.Children(1).Children(1), 'XTick', []);
set(f.Children(1).Children(1), 'YTick', []);
end
end

26
geometries/rectangularPrism.m
View File

@@ -163,9 +163,10 @@ classdef rectangularPrism
c = (tmax >= 0) && (tmin <= 1);
end
function [obj, f] = plotWireframe(obj, f)
function [obj, f] = plotWireframe(obj, ind, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
ind (1, :) double = NaN;
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
@@ -181,17 +182,20 @@ classdef rectangularPrism
Y = [obj.vertices(obj.edges(:,1),2), obj.vertices(obj.edges(:,2),2)]';
Z = [obj.vertices(obj.edges(:,1),3), obj.vertices(obj.edges(:,2),3)]';
% Plot the boundaries of the geometry
hold(f.CurrentAxes, "on");
o = plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
hold(f.CurrentAxes, "off");
% Plot the boundaries of the geometry into 3D view
if isnan(ind)
o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
else
hold(f.Children(1).Children(ind(1)), "on");
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
hold(f.Children(1).Children(ind(1)), "off");
end
% Check if this is a tiled layout figure
if strcmp(f.Children(1).Type, 'tiledlayout')
% Add to other perspectives
o = [o, copyobj(o(:, 1), f.Children(1).Children(2))];
o = [o, copyobj(o(:, 1), f.Children(1).Children(3))];
o = [o, copyobj(o(:, 1), f.Children(1).Children(5))];
% Copy to other requested tiles
if numel(ind) > 1
for ii = 2:size(ind, 2)
o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
end
end
obj.lines = o;

131
miSim.m
View File

@@ -10,11 +10,20 @@ classdef miSim
obstacles = cell(0, 1); % geometries that define obstacles within the domain
agents = cell(0, 1); % agents that move within the domain
adjacency = NaN; % Adjacency matrix representing communications network graph
partitioning = NaN;
end
properties (Access = private)
% Plot objects
connectionsPlot; % objects for lines connecting agents in spatial plots
graphPlot; % objects for abstract network graph plot
partitionPlot; % objects for partition plot
% Indicies for various plot types in the main tiled layout figure
spatialPlotIndices = [6, 4, 3, 2];
objectivePlotIndices = [6, 4];
networkGraphIndex = 5;
partitionGraphIndex = 1;
end
methods (Access = public)
@@ -52,29 +61,35 @@ classdef miSim
% Compute adjacency matrix
obj = obj.updateAdjacency();
% Create initial partitioning
obj = obj.partition();
% Set up initial plot
% Set up axes arrangement
% Plot domain
[obj.domain, f] = obj.domain.plotWireframe();
[obj.domain, f] = obj.domain.plotWireframe(obj.spatialPlotIndices);
% Plot obstacles
for ii = 1:size(obj.obstacles, 1)
[obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(f);
[obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(obj.spatialPlotIndices, f);
end
% Plot objective gradient
f = obj.objective.plot(f);
f = obj.objective.plot(obj.objectivePlotIndices, f);
% Plot agents and their collision geometries
for ii = 1:size(obj.agents, 1)
[obj.agents{ii}, f] = obj.agents{ii}.plot(f);
[obj.agents{ii}, f] = obj.agents{ii}.plot(obj.spatialPlotIndices, f);
end
% Plot communication links
[obj, f] = obj.plotConnections(f);
[obj, f] = obj.plotConnections(obj.spatialPlotIndices, f);
% Plot abstract network graph
[obj, f] = obj.plotGraph(f);
[obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
% Plot domain partitioning
[obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
end
function [obj, f] = run(obj, f)
arguments (Input)
@@ -120,6 +135,31 @@ classdef miSim
% Close video file
v.close();
end
function obj = partition(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
% Assess sensing performance of each agent at each sample point
% in the domain
agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [obj.objective.X(:), obj.objective.Y(:), zeros(size(obj.objective.X(:)))]), size(obj.objective.X)), obj.agents, 'UniformOutput', false);
agentPerformances = cat(3, agentPerformances{:});
% Get highest performance value at each point
[~, idx] = max(agentPerformances, [], 3);
% Collect agent indices in the same way
agentInds = cellfun(@(x) x.index * ones(size(obj.objective.X)), obj.agents, 'UniformOutput', false);
agentInds = cat(3, agentInds{:});
% Get highest performing agent's index
[m,n,~] = size(agentInds);
[i,j] = ndgrid(1:m, 1:n);
obj.partitioning = agentInds(sub2ind(size(agentInds), i, j, idx));
end
function [obj, f] = updatePlots(obj, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
@@ -171,15 +211,20 @@ classdef miSim
A(ii, jj) = true;
end
end
% need extra handling for cases with no obstacles
if isempty(obj.obstacles)
A(ii, jj) = true;
end
end
end
end
obj.adjacency = A | A';
end
function [obj, f] = plotConnections(obj, f)
function [obj, f] = plotConnections(obj, ind, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
ind (1, :) double = NaN;
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
@@ -202,23 +247,57 @@ classdef miSim
X = X'; Y = Y'; Z = Z';
% Plot the connections
hold(f.CurrentAxes, "on");
o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
hold(f.CurrentAxes, "off");
if isnan(ind)
hold(f.CurrentAxes, "on");
o = plot3(f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
hold(f.CurrentAxes, "off");
else
hold(f.Children(1).Children(ind(1)), "on");
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
hold(f.Children(1).Children(ind(1)), "off");
end
% Check if this is a tiled layout figure
if strcmp(f.Children(1).Type, 'tiledlayout')
% Add to other plots
o = [o, copyobj(o(:, 1), f.Children(1).Children(2))];
o = [o, copyobj(o(:, 1), f.Children(1).Children(3))];
o = [o, copyobj(o(:, 1), f.Children(1).Children(5))];
% Copy to other plots
if size(ind, 2) > 1
for ii = 2:size(ind, 2)
o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
end
end
obj.connectionsPlot = o;
end
function [obj, f] = plotGraph(obj, f)
function [obj, f] = plotPartitions(obj, ind, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
ind (1, :) double = NaN;
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'miSim')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
end
if isnan(ind)
hold(f.CurrentAxes, 'on');
o = imagesc(f.CurrentAxes, obj.partitioning);
hold(f.CurrentAxes, 'off');
else
hold(f.Children(1).Children(ind(1)), 'on');
o = imagesc(f.Children(1).Children(ind(1)), obj.partitioning);
hold(f.Children(1).Children(ind(1)), 'on');
if size(ind, 2) > 1
for ii = 2:size(ind, 2)
o = [o, copyobj(o(1), f.Children(1).Children(ind(ii)))];
end
end
end
obj.partitionPlot = o;
end
function [obj, f] = plotGraph(obj, ind, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
ind (1, :) double = NaN;
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
@@ -230,9 +309,21 @@ classdef miSim
G = graph(obj.adjacency, 'omitselfloops');
% Plot graph object
hold(f.Children(1).Children(4), 'on');
obj.graphPlot = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
hold(f.Children(1).Children(4), 'off');
if isnan(ind)
hold(f.CurrentAxes, 'on');
o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
hold(f.CurrentAxes, 'off');
else
hold(f.Children(1).Children(ind(1)), 'on');
o = plot(f.Children(1).Children(ind(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
hold(f.Children(1).Children(ind(1)), 'off');
if size(ind, 2) > 1
for ii = 2:size(ind, 2)
o = [o; copyobj(o(1), f.Children(1).Children(ind(ii)))];
end
end
end
obj.graphPlot = o;
end
end

15
sensingModels/fixedCardinalSensor.m
View File

@@ -56,5 +56,20 @@ classdef fixedCardinalSensor
% Prevent out of bounds locations from ever possibly being selected
neighborValues(outOfBounds) = 0;
end
function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
agentPos (1, 3) double;
agentPan (1, 1) double;
agentTilt (1, 1) double;
targetPos (:, 3) double;
end
arguments (Output)
value (:, 1) double;
end
value = 0.5 * ones(size(targetPos, 1), 1);
end
end
end

82
sensingModels/sigmoidSensor.m
View File

@@ -1,16 +1,72 @@
function accuracy = sigmoid(sensorPos, targetPos)
arguments (Input)
sensorPos (1, 3) double;
targetPos (:, 3) double;
classdef sigmoidSensor
properties (SetAccess = private, GetAccess = public)
% Sensor parameters
alphaDist;
betaDist;
alphaPan;
betaPan;
alphaTilt;
betaTilt;
end
arguments (Output)
accuracy (:, 3) double;
methods (Access = public)
function obj = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
alphaDist (1, 1) double;
betaDist (1, 1) double;
alphaPan (1, 1) double;
betaPan (1, 1) double;
alphaTilt (1, 1) double;
betaTilt (1, 1) double;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
end
obj.alphaDist = alphaDist;
obj.betaDist = betaDist;
obj.alphaPan = alphaPan;
obj.betaPan = betaPan;
obj.alphaTilt = alphaTilt;
obj.betaTilt = betaTilt;
end
function [neighborValues, neighborPos] = sense(obj, objectiveFunction, domain, pos)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
domain (1, 1) {mustBeGeometry};
pos (1, 3) double;
end
arguments (Output)
neighborValues (4, 1) double;
neighborPos (4, 3) double;
end
end
function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
agentPos (1, 3) double;
agentPan (1, 1) double;
agentTilt (1, 1) double;
targetPos (:, 3) double;
end
arguments (Output)
value (:, 1) double;
end
d = vecnorm(agentPos - targetPos, 2, 2);
panAngle = atan2(targetPos(:, 2) - agentPos(2), targetPos(:, 1) - agentPos(1)) - agentPan;
tiltAngle = atan2(targetPos(:, 3) - agentPos(3), d) - agentTilt;
% Membership functions
mu_d = 1 - (1 ./ (1 + exp(-obj.betaDist .* (d - obj.alphaDist)))); % distance
mu_p = (1 ./ (1 + exp(-obj.betaPan .* (panAngle + obj.alphaPan)))) - (1 ./ (1 + exp(-obj.betaPan .* (panAngle - obj.alphaPan)))); % pan
mu_t = (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle + obj.alphaPan)))) - (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle - obj.alphaPan)))); % tilt
value = mu_d .* mu_p .* mu_t;
end
end
end
function distanceMembership()
end

34
sensingObjective.m
View File

@@ -46,9 +46,10 @@ classdef sensingObjective
idx = obj.values == max(obj.values, [], "all");
obj.groundPos = [obj.X(idx), obj.Y(idx)];
end
function f = plot(obj, f)
function f = plot(obj, ind, f)
arguments (Input)
obj (1,1) {mustBeA(obj, 'sensingObjective')};
ind (1, :) double = NaN;
f (1,1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
@@ -58,24 +59,27 @@ classdef sensingObjective
% Create axes if they don't already exist
f = firstPlotSetup(f);
% Check if this is a tiled layout figure
if strcmp(f.Children(1).Type, 'tiledlayout')
% Plot gradient on the "floor" of the domain
hold(f.Children(1).Children(3), "on");
o = surf(f.Children(1).Children(3), obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
o.HitTest = 'off';
o.PickableParts = 'none';
hold(f.Children(1).Children(3), "off");
% Add to other perspectives
copyobj(o, f.Children(1).Children(5));
else
% Plot gradient on the "floor" of the domain
% Plot gradient on the "floor" of the domain
if isnan(ind)
hold(f.CurrentAxes, "on");
o = surf(obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
o = surf(f.CurrentAxes, obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
o.HitTest = 'off';
o.PickableParts = 'none';
hold(f.CurrentAxes, "off");
else
hold(f.Children(1).Children(ind(1)), "on");
o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
o.HitTest = 'off';
o.PickableParts = 'none';
hold(f.Children(1).Children(ind(1)), "off");
end
% Add to other perspectives
if size(ind, 2) > 1
for ii = 2:size(ind, 2)
copyobj(o, f.Children(1).Children(ind(ii)));
end
end
end
end

35
test_miSim.m
View File

@@ -4,7 +4,7 @@ classdef test_miSim < matlab.unittest.TestCase
% Domain
domain = rectangularPrism; % domain geometry
maxIter = 1000;
maxIter = 250;
timestep = 0.05
% Obstacles
@@ -358,8 +358,8 @@ classdef test_miSim < matlab.unittest.TestCase
candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
% Initialize candidate agent sensor model
sensor = fixedCardinalSensor;
sensor.initialize(tc.sensingLength);
sensor = sigmoidSensor;
sensor = sensor.initialize(1, 1, 1, 1, 1, 1);
% Initialize candidate agent
newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii));
@@ -415,5 +415,34 @@ classdef test_miSim < matlab.unittest.TestCase
% Run simulation loop
[tc.testClass, f] = tc.testClass.run(f);
end
function test_basic_partitioning(tc)
% place agents a fixed distance +/- X from the domain's center
d = 1;
c = 0.1;
% make basic domain
tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective
tc.objective = tc.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2), eye(2)), tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep);
% Initialize agent collision geometry
geometry1 = rectangularPrism;
geometry2 = geometry1;
geometry1 = geometry1.initialize([tc.domain.center + [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
geometry2 = geometry2.initialize([tc.domain.center - [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center - [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 2));
% Initialize agent sensor model
sensor = sigmoidSensor;
sensor = sensor.initialize(1, 1, 1, 1, 1, 1);
% Initialize agents
tc.agents = {agent; agent};
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3*d, 1, sprintf("Agent %d", 1));
tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3*d, 2, sprintf("Agent %d", 2));
% Initialize the simulation
[tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter);
end
end
end