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

Added lesser neighbor algorithm and constraints

Browse Source
This commit is contained in:
Kevin Dee
2025-12-31 19:19:36 -08:00
parent fa8da50db1
commit d6a9c4ac06
10 changed files with 80 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
@miSim/constrainMotion.m
View File

@@ -15,16 +15,18 @@ function [obj] = constrainMotion(obj)
v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
% Initialize QP based on number of agents and obstacles
h = NaN(size(obj.agents, 1));
h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling)
nLNAPairs = sum(obj.constraintAdjacencyMatrix) - size(obj.agents, 1);
total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
kk = 1;
A = zeros(nAAPairs + nAOPairs + nADPairs, 3 * size(obj.agents, 1));
b = zeros(nAAPairs + nAOPairs + nADPairs, 1);
A = zeros(total, 3 * size(obj.agents, 1));
b = zeros(total, 1);
% Set up collision avoidance constraints
h = NaN(size(obj.agents, 1));
h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
for ii = 1:(size(obj.agents, 1) - 1)
for jj = (ii + 1):size(obj.agents, 1)
h(ii, jj) = norm(agents(ii).pos - agents(jj).pos)^2 - (agents(ii).collisionGeometry.radius + agents(jj).collisionGeometry.radius)^2;
@@ -94,6 +96,23 @@ function [obj] = constrainMotion(obj)
kk = kk + 1;
end
% Add communication network constraints
hComms = NaN(size(obj.agents, 1));
hComms(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
for ii = 1:(size(obj.agents, 1) - 1)
for jj = (ii + 1):size(obj.agents, 1)
if obj.constraintAdjacencyMatrix(ii, jj)
hComms(ii, jj) = (agents(ii).commsGeometry.radius + agents(jj).commsGeometry.radius)^2 - norm(agents(ii).pos - agents(jj).pos)^2;
hComms(jj, ii) = hComms(ii, jj);
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - agents(jj).pos);
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
b(kk) = obj.barrierGain * hComms(ii, jj)^3;
kk = kk + 1;
end
end
end
% Solve QP program generated earlier
vhat = reshape(v', 3 * size(obj.agents, 1), 1);
H = 2 * eye(3 * size(obj.agents, 1));

5
@miSim/initialize.m
View File

@@ -25,7 +25,6 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
end
end
obj.makeVideo = makeVideo;
% Define simulation time parameters
obj.timestep = timestep;
@@ -51,9 +50,11 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
% Define agents
obj.agents = agents;
obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1)));
% Compute adjacency matrix
% Compute adjacency matrix and lesser neighbors
obj = obj.updateAdjacency();
obj = obj.lesserNeighbor();
% Set up times to iterate over
obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';

35
@miSim/lesserNeighbor.m Normal file
View File

@@ -0,0 +1,35 @@
function obj = lesserNeighbor(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
% Check possible connections from adjacency matrix
% Choose connections which fully connect network by selecting maximum
% indices according to the previous columns (or rows) of the new
% constraint adjacency matrix
% Place that choice in the constraint adjacency matrix
% Begin with all possible connections and trim down
constraintAdjacencyMatrix = obj.adjacency;
% Iterate over each agent (by increasing index)
for ii = 1:size(obj.agents, 1)
% Iterate over each agent of lesser index and see if a higher
% indexed agent provides connectivity already
for jj = 1:(ii - 1)
for kk = 1:(jj - 1)
% Check if a connection between the two lesser agents
% already exists
if constraintAdjacencyMatrix(jj, kk)
constraintAdjacencyMatrix(jj, kk) = false;
constraintAdjacencyMatrix(kk, jj) = false;
end
end
end
end
obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix;
end

2
@miSim/miSim.m
View File

@@ -11,6 +11,7 @@ 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
constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
partitioning = NaN;
performance = 0; % cumulative sensor performance
@@ -47,6 +48,7 @@ classdef miSim
methods (Access = public)
[obj] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
[obj] = run(obj);
[obj] = lesserNeighbor(obj);
[obj] = constrainMotion(obj);
[obj] = partition(obj);
[obj] = updateAdjacency(obj);

4
@miSim/plotConnections.m
View File

@@ -9,9 +9,9 @@ function obj = plotConnections(obj)
% Iterate over lower triangle off-diagonal region of the
% adjacency matrix to plot communications links between agents
X = []; Y = []; Z = [];
for ii = 2:size(obj.adjacency, 1)
for ii = 2:size(obj.constraintAdjacencyMatrix, 1)
for jj = 1:(ii - 1)
if obj.adjacency(ii, jj)
if obj.constraintAdjacencyMatrix(ii, jj)
X = [X; obj.agents{ii}.pos(1), obj.agents{jj}.pos(1)];
Y = [Y; obj.agents{ii}.pos(2), obj.agents{jj}.pos(2)];
Z = [Z; obj.agents{ii}.pos(3), obj.agents{jj}.pos(3)];

2
@miSim/plotGraph.m
View File

@@ -7,7 +7,7 @@ function obj = plotGraph(obj)
end
% Form graph from adjacency matrix
G = graph(obj.adjacency, 'omitselfloops');
G = graph(obj.constraintAdjacencyMatrix, 'omitselfloops');
% Plot graph object
if isnan(obj.networkGraphIndex)

7
@miSim/run.m
View File

@@ -24,7 +24,10 @@ function [obj] = run(obj)
obj = obj.partition();
end
% Iterate over agents to simulate their motion
% Determine desired communications links
obj = obj.lesserNeighbor();
% Iterate over agents to simulate their unconstrained motion
for jj = 1:size(obj.agents, 1)
obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.t);
end
@@ -33,6 +36,8 @@ function [obj] = run(obj)
% CBF constraints solved by QP
obj = constrainMotion(obj);
% Finished simulation for this timestep, do accounting
% Update total performance
obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(end), obj.agents))];

5
@miSim/updateAdjacency.m
View File

@@ -28,4 +28,9 @@ function obj = updateAdjacency(obj)
end
obj.adjacency = A & A';
if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
warning("Eliminated network connections that were necessary");
keyboard
end
end