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

geometries move in plots as sim runs

Browse Source
This commit is contained in:
Kevin D
2025-10-27 22:38:39 -07:00
parent ade795b3ae
commit bbefb6111b
6 changed files with 139 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
agent.m
View File

@@ -9,15 +9,19 @@ classdef agent
sensingLength = 0.05; % length parameter used by sensing function
% State
pos = NaN(1, 3);
vel = NaN(1, 3);
cBfromC = NaN(3); % DCM body from sim cartesian (assume fixed for now)
lastPos = NaN(1, 3); % position from previous timestep
pos = NaN(1, 3); % current position
vel = NaN(1, 3); % current velocity
cBfromC = NaN(3); % current DCM body from sim cartesian (assume fixed for now)
% Collision
collisionGeometry;
% Communication
comRange = NaN;
% Plotting
scatterPoints;
end
methods (Access = public)
@@ -48,29 +52,66 @@ classdef agent
obj.index = index;
obj.label = label;
end
function obj = run(obj, objectiveFunction)
function obj = run(obj, objectiveFunction, domain)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'agent')};
objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
domain (1, 1) {mustBeGeometry};
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'agent')};
end
% Do sensing to determine target position
nextPos = obj.sensingFunction(objectiveFunction, obj.pos, obj.sensingLength);
nextPos = obj.sensingFunction(objectiveFunction, domain, obj.pos, obj.sensingLength);
% Move to next position
% (dynamics not modeled at this time)
obj.lastPos = obj.pos;
obj.pos = nextPos;
% Calculate movement
d = obj.pos - obj.collisionGeometry.center;
% Reinitialize collision geometry in the new position
obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
end
function f = plot(obj, f)
function updatePlots(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'agent')};
end
arguments (Output)
end
% Scatterplot point positions
for ii = 1:size(obj.scatterPoints, 1)
obj.scatterPoints(ii).XData = obj.pos(1);
obj.scatterPoints(ii).YData = obj.pos(2);
obj.scatterPoints(ii).ZData = obj.pos(3);
end
% Find change in agent position since last timestep
deltaPos = obj.pos - obj.lastPos;
% Collision geometry edges
for jj = 1:size(obj.collisionGeometry.lines, 2)
% Update plotting
for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
end
end
% Network connections
end
function [obj, f] = plot(obj, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'agent')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'agent')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
end
@@ -85,10 +126,15 @@ classdef agent
% Check if this is a tiled layout figure
if strcmp(f.Children(1).Type, 'tiledlayout')
% Add to other perspectives
copyobj(o, f.Children(1).Children(2));
copyobj(o, f.Children(1).Children(3));
copyobj(o, f.Children(1).Children(5));
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))];
end
obj.scatterPoints = o;
% Plot collision geometry
[obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(f);
end
end
end

10
firstPlotSetup.m
View File

@@ -9,7 +9,7 @@ function f = firstPlotSetup(f)
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("Top-down Perspective");
title(f.Children(1).Children(1), "Top-down Perspective");
% Communications graph
nexttile(3, [1, 1]);
@@ -17,33 +17,33 @@ function f = firstPlotSetup(f)
axis(f.Children(1).Children(1), "image");
grid(f.Children(1).Children(1), "off");
view(f.Children(1).Children(1), 0, 0);
title("Network Graph");
title(f.Children(1).Children(1), "Network Graph");
% 3D view
title("3D Perspective");
nexttile(4, [2, 2]);
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), 3);
xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y"); zlabel(f.Children(1).Children(1), "Z");
title(f.Children(1).Children(1), "3D Perspective");
% Side-on view
title("Side-on Perspective");
nexttile(6, [2, 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), 90, 0);
ylabel(f.Children(1).Children(1), "Y"); zlabel(f.Children(1).Children(1), "Z");
title(f.Children(1).Children(1), "Side-on Perspective");
% Front-on view
title("Front-on Perspective");
nexttile(10, [1, 2]);
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 Perspective");
end
end

21
geometries/rectangularPrism.m
View File

@@ -1,23 +1,25 @@
classdef rectangularPrism
% Rectangular prism geometry
properties (SetAccess = private, GetAccess = public)
% Meta
tag = REGION_TYPE.INVALID;
label = "";
% Spatial
minCorner = NaN(1, 3);
maxCorner = NaN(1, 3);
dimensions = NaN(1, 3);
center = NaN;
footprint = NaN(4, 2);
% Graph
vertices = NaN(8, 3);
edges = [1 2; 2 3; 3 4; 4 1; % bottom square
5 6; 6 8; 8 7; 7 5; % top square
1 5; 2 6; 3 8; 4 7]; % vertical edges
footprint = NaN(4, 2);
% Plotting
lines;
end
methods (Access = public)
@@ -161,12 +163,13 @@ classdef rectangularPrism
c = (tmax >= 0) && (tmin <= 1);
end
function f = plotWireframe(obj, f)
function [obj, f] = plotWireframe(obj, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
end
@@ -186,10 +189,12 @@ classdef rectangularPrism
% Check if this is a tiled layout figure
if strcmp(f.Children(1).Type, 'tiledlayout')
% Add to other perspectives
copyobj(o, f.Children(1).Children(2));
copyobj(o, f.Children(1).Children(3));
copyobj(o, f.Children(1).Children(5));
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))];
end
obj.lines = o;
end
end
end

63
miSim.m
View File

@@ -13,7 +13,7 @@ classdef miSim
end
methods (Access = public)
function obj = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles)
function [obj, f] = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
domain (1, 1) {mustBeGeometry};
@@ -25,6 +25,7 @@ classdef miSim
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'miSim')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
end
% Define simulation time parameters
@@ -46,33 +47,87 @@ classdef miSim
% Compute adjacency matrix
obj = obj.updateAdjacency();
% Set up initial plot
% Set up axes arrangement
% Plot domain
[obj.domain, f] = obj.domain.plotWireframe();
% Set plotting limits to focus on the domain
xlim([obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
ylim([obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
zlim([obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
% Plot obstacles
for ii = 1:size(obj.obstacles, 1)
[obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(f);
end
function obj = run(obj)
% Plot objective gradient
f = obj.objective.plot(f);
% Plot agents and their collision geometries
for ii = 1:size(obj.agents, 1)
[obj.agents{ii}, f] = obj.agents{ii}.plot(f);
end
% Plot communication links
f = obj.plotNetwork(f);
% Plot abstract network graph
f = obj.plotGraph(f);
end
function [obj, f] = run(obj, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
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
% Create axes if they don't already exist
f = firstPlotSetup(f);
% Set up times to iterate over
times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
for ii = 1:size(times, 1)
% Get current sim time
% Display current sim time
t = times(ii);
fprintf("Sim Time: %4.2f (%d/%d)\n", t, ii, obj.maxIter)
% Iterate over agents to simulate their motion
for jj = 1:size(obj.agents, 1)
obj.agents{jj} = obj.agents{jj}.run(obj.objective.objectiveFunction);
obj.agents{jj} = obj.agents{jj}.run(obj.objective.objectiveFunction, obj.domain);
end
% Update adjacency matrix
obj = obj.updateAdjacency;
% Update plots
f = obj.updatePlots(f);
end
end
function f = updatePlots(obj, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
end
% Update agent positions, collision geometries, connections
for ii = 1:size(obj.agents, 1)
obj.agents{ii}.updatePlots();
end
% Update network graph plot
drawnow;
end
function obj = updateAdjacency(obj)
arguments (Input)

5
sensingFunctions/basicGradientAscent.m
View File

@@ -1,6 +1,7 @@
function nextPos = basicGradientAscent(objectiveFunction, pos, r)
function nextPos = basicGradientAscent(objectiveFunction, domain, pos, r)
arguments (Input)
objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
domain (1, 1) {mustBeGeometry};
pos (1, 3) double;
r (1, 1) double;
end
@@ -39,5 +40,5 @@ function nextPos = basicGradientAscent(objectiveFunction, pos, r)
% Select next position by maximum sensed value
nextPos = neighborPos(neighborValues == max(neighborValues), :);
nextPos = nextPos(1, 1:3); % just in case two get selected, simply pick one
nextPos = [nextPos(1, 1:2), pos(3)]; % just in case two get selected, simply pick one
end

63
test_miSim.m
View File

@@ -235,35 +235,7 @@ classdef test_miSim < matlab.unittest.TestCase
end
% Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
% Plot domain
f = tc.testClass.domain.plotWireframe;
% Set plotting limits to focus on the domain
xlim([tc.testClass.domain.minCorner(1), tc.testClass.domain.maxCorner(1)]);
ylim([tc.testClass.domain.minCorner(2), tc.testClass.domain.maxCorner(2)]);
zlim([tc.testClass.domain.minCorner(3), tc.testClass.domain.maxCorner(3)]);
% Plot obstacles
for ii = 1:size(tc.testClass.obstacles, 1)
tc.testClass.obstacles{ii}.plotWireframe(f);
end
% Plot objective gradient
f = tc.testClass.objective.plot(f);
% Plot agents and their collision geometries
for ii = 1:size(tc.testClass.agents, 1)
f = tc.testClass.agents{ii}.plot(f);
f = tc.testClass.agents{ii}.collisionGeometry.plotWireframe(f);
end
% Plot communication links
f = tc.testClass.plotNetwork(f);
% Plot abstract network graph
f = tc.testClass.plotGraph(f);
[tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
end
function misim_run(tc)
% randomly create obstacles
@@ -424,39 +396,10 @@ classdef test_miSim < matlab.unittest.TestCase
end
% Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
% Plot domain
f = tc.testClass.domain.plotWireframe;
% Set plotting limits to focus on the domain
xlim([tc.testClass.domain.minCorner(1), tc.testClass.domain.maxCorner(1)]);
ylim([tc.testClass.domain.minCorner(2), tc.testClass.domain.maxCorner(2)]);
zlim([tc.testClass.domain.minCorner(3), tc.testClass.domain.maxCorner(3)]);
% Plot obstacles
for ii = 1:size(tc.testClass.obstacles, 1)
tc.testClass.obstacles{ii}.plotWireframe(f);
end
% Plot objective gradient
f = tc.testClass.objective.plot(f);
% Plot agents and their collision geometries
for ii = 1:size(tc.testClass.agents, 1)
f = tc.testClass.agents{ii}.plot(f);
f = tc.testClass.agents{ii}.collisionGeometry.plotWireframe(f);
end
% Plot communication links
f = tc.testClass.plotNetwork(f);
% Plot abstract network graph
f = tc.testClass.plotGraph(f);
[tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
% Run simulation loop
tc.testClass.run();
[tc.testClass, f] = tc.testClass.run(f);
end
end
end