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

fixed bug allowing obstructed coms connections

Browse Source
This commit is contained in:
Kevin D
2025-10-27 20:45:37 -07:00
parent f953cd3882
commit 8955d4d29b
3 changed files with 243 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
agent.m
View File

@@ -20,13 +20,14 @@ classdef agent
end
methods (Access = public)
function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, comRange, index, label)
function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, sensingFunction, comRange, index, label)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'agent')};
pos (1, 3) double;
vel (1, 3) double;
cBfromC (3, 3) double {mustBeDcm};
collisionGeometry (1, 1) {mustBeGeometry};
sensingFunction (1, 1) {mustBeA(sensingFunction, 'function_handle')}
comRange (1, 1) double = NaN;
index (1, 1) double = NaN;
label (1, 1) string = "";

54
miSim.m
View File

@@ -3,6 +3,8 @@ classdef miSim
% Simulation parameters
properties (SetAccess = private, GetAccess = public)
timestep = NaN; % delta time interval for simulation iterations
maxIter = NaN; % maximum number of simulation iterations
domain = rectangularPrism;
objective = sensingObjective;
obstacles = cell(0, 1); % geometries that define obstacles within the domain
@@ -11,33 +13,58 @@ classdef miSim
end
methods (Access = public)
function obj = initialize(obj, domain, objective, agents, obstacles)
function obj = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
domain (1, 1) {mustBeGeometry};
objective (1, 1) {mustBeA(objective, 'sensingObjective')};
agents (:, 1) cell {mustBeAgents};
timestep (:, 1) double = 0.05;
maxIter (:, 1) double = 1000;
obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
%% Define domain
% Define simulation time parameters
obj.timestep = timestep;
obj.maxIter = maxIter;
% Define domain
obj.domain = domain;
%% Add geometries representing obstacles within the domain
% Add geometries representing obstacles within the domain
obj.obstacles = obstacles;
%% Define objective
% Define objective
obj.objective = objective;
%% Define agents
% Define agents
obj.agents = agents;
%% Compute adjacency matrix
% Compute adjacency matrix
obj = obj.updateAdjacency();
end
function obj = run(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
keyboard
% Iterate over agents to simulate their motion
for ii = 1:size(obj.agents, 1)
obj.agents{ii}
end
% Update adjacency matrix
obj = obj.updateAdjacency;
% Update plots
end
function obj = updateAdjacency(obj)
arguments (Input)
@@ -54,7 +81,13 @@ classdef miSim
for ii = 2:size(A, 1)
for jj = 1:(ii - 1)
if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) <= min([obj.agents{ii}.comRange, obj.agents{jj}.comRange])
A(ii, jj) = true;
% Make sure that obstacles don't obstruct the line
% of sight, breaking the connection
for kk = 1:size(obj.obstacles, 1)
if ~obj.obstacles{kk}.containsLine(obj.agents{ii}.pos, obj.agents{jj}.pos)
A(ii, jj) = true;
end
end
end
end
end
@@ -86,7 +119,7 @@ classdef miSim
% Plot the connections
hold(f.CurrentAxes, "on");
o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 1, 'LineStyle', '--');
o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
hold(f.CurrentAxes, "off");
% Check if this is a tiled layout figure
@@ -111,11 +144,10 @@ classdef miSim
% Check if this is a tiled layout figure
if strcmp(f.Children(1).Type, 'tiledlayout')
o = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k');
o = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
else
o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k');
o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
end
end
end

201
test_miSim.m
View File

@@ -4,6 +4,8 @@ classdef test_miSim < matlab.unittest.TestCase
% Domain
domain = rectangularPrism; % domain geometry
maxIter = 1000;
timestep = 0.05
% Obstacles
minNumObstacles = 1; % Minimum number of obstacles to be randomly generated
@@ -74,7 +76,7 @@ classdef test_miSim < matlab.unittest.TestCase
methods (Test)
% Test methods
function misim_initialization(tc)
% randomly create 2-3 obstacles
% randomly create obstacles
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
tc.obstacles = cell(nGeom, 1);
@@ -184,7 +186,7 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize candidate agent
candidateGeometry = rectangularPrism;
newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3),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)), tc.comRange, ii, sprintf("Agent %d", ii));
newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3),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)), @(r) 0.5, tc.comRange, ii, sprintf("Agent %d", ii));
% Make sure candidate agent doesn't collide with
% domain
@@ -232,7 +234,7 @@ classdef test_miSim < matlab.unittest.TestCase
end
% Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.obstacles);
tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
% Plot domain
f = tc.testClass.domain.plotWireframe;
@@ -262,5 +264,198 @@ classdef test_miSim < matlab.unittest.TestCase
% Plot abstract network graph
f = tc.testClass.plotGraph(f);
end
function misim_run(tc)
% randomly create obstacles
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
tc.obstacles = cell(nGeom, 1);
% Iterate over obstacles to initialize
for ii = 1:size(tc.obstacles, 1)
badCandidate = true;
while badCandidate
% Instantiate a rectangular prism obstacle
tc.obstacles{ii} = rectangularPrism;
% Randomly generate min corner for the obstacle
candidateMinCorner = tc.domain.random();
candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
% Randomly select a corresponding maximum corner that
% satisfies min/max obstacle size specifications
candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize);
% Initialize obstacle
tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
% Check if the obstacle intersects with any existing
% obstacles
violation = false;
for kk = 1:(ii - 1)
if geometryIntersects(tc.obstacles{kk}, tc.obstacles{ii})
violation = true;
break;
end
end
if violation
continue;
end
% Make sure that the obstacles are fully contained by
% the domain
if ~domainContainsObstacle(tc.domain, tc.obstacles{ii})
continue;
end
% Make sure that the obstacles don't cover the sensing
% objective
if obstacleCoversObjective(tc.objective, tc.obstacles{ii})
continue;
end
% Make sure that the obstacles aren't too close to the
% sensing objective
if obstacleCrowdsObjective(tc.objective, tc.obstacles{ii}, tc.protectedRange)
continue;
end
badCandidate = false;
end
end
% Add agents individually, ensuring that each addition does not
% invalidate the initialization setup
for ii = 1:size(tc.agents, 1)
initInvalid = true;
while initInvalid
candidatePos = [tc.objective.groundPos, 0];
% Generate a random position for the agent based on
% existing agent positions
if ii == 1
while agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2)
candidatePos = tc.domain.random();
end
else
candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
end
% Make sure that the candidate position is within the
% domain
if ~tc.domain.contains(candidatePos)
continue;
end
% Make sure that the candidate position does not crowd
% the sensing objective and create boring scenarios
if agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2)
continue;
end
% Make sure that there exist unobstructed lines of sight at
% appropriate ranges to form a connected communications
% graph between the agents
connections = false(1, ii - 1);
for jj = 1:(ii - 1)
if norm(tc.agents{jj}.pos - candidatePos) <= tc.comRange
% Check new agent position against all existing
% agent positions for communications range
connections(jj) = true;
for kk = 1:size(tc.obstacles, 1)
if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
connections(jj) = false;
end
end
end
end
% New agent must be connected to an existing agent to
% be valid
if ii ~= 1 && ~any(connections)
continue;
end
% Initialize candidate agent
candidateGeometry = rectangularPrism;
newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3),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)), @(r) 0.5, tc.comRange, ii, sprintf("Agent %d", ii));
% Make sure candidate agent doesn't collide with
% domain
violation = false;
for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
% Check if collision geometry exits domain
if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
violation = true;
break;
end
end
if violation
continue;
end
% Make sure candidate doesn't collide with obstacles
violation = false;
for kk = 1:size(tc.obstacles, 1)
if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
violation = true;
break;
end
end
if violation
continue;
end
% Make sure candidate doesn't collide with existing
% agents
violation = false;
for kk = 1:(ii - 1)
if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
violation = true;
break;
end
end
if violation
continue;
end
% Candidate agent is valid, store to pass in to sim
initInvalid = false;
tc.agents{ii} = newAgent;
end
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);
% Run simulation loop
tc.testClass.run();
end
end
end