From d6a9c4ac06076a72d56c358a110f7429431e353d Mon Sep 17 00:00:00 2001 From: krdee1 Date: Wed, 31 Dec 2025 19:19:36 -0800 Subject: [PATCH] Added lesser neighbor algorithm and constraints --- @miSim/constrainMotion.m | 27 +++++++++++--- @miSim/initialize.m | 5 +-- @miSim/lesserNeighbor.m | 35 +++++++++++++++++++ @miSim/miSim.m | 2 ++ @miSim/plotConnections.m | 4 +-- @miSim/plotGraph.m | 2 +- @miSim/run.m | 7 +++- @miSim/updateAdjacency.m | 5 +++ .../NSdjf1q7sJuLYHZ864kCIhygPfId.xml | 6 +--- test/test_miSim.m | 4 +-- 10 files changed, 80 insertions(+), 17 deletions(-) create mode 100644 @miSim/lesserNeighbor.m diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 155142e..432290d 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -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)); diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 9a27236..f134b04 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -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)'; diff --git a/@miSim/lesserNeighbor.m b/@miSim/lesserNeighbor.m new file mode 100644 index 0000000..fef0529 --- /dev/null +++ b/@miSim/lesserNeighbor.m @@ -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 \ No newline at end of file diff --git a/@miSim/miSim.m b/@miSim/miSim.m index 3ebc020..3bf1cee 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -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); diff --git a/@miSim/plotConnections.m b/@miSim/plotConnections.m index 8a33a89..1cc6eea 100644 --- a/@miSim/plotConnections.m +++ b/@miSim/plotConnections.m @@ -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)]; diff --git a/@miSim/plotGraph.m b/@miSim/plotGraph.m index 034a35a..0c6cb80 100644 --- a/@miSim/plotGraph.m +++ b/@miSim/plotGraph.m @@ -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) diff --git a/@miSim/run.m b/@miSim/run.m index bf027d9..8c3892c 100644 --- a/@miSim/run.m +++ b/@miSim/run.m @@ -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))]; diff --git a/@miSim/updateAdjacency.m b/@miSim/updateAdjacency.m index 782e0da..f7a205c 100644 --- a/@miSim/updateAdjacency.m +++ b/@miSim/updateAdjacency.m @@ -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 \ No newline at end of file diff --git a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml index 99772b4..4356a6a 100644 --- a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml +++ b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml @@ -1,6 +1,2 @@ - - - - \ No newline at end of file + \ No newline at end of file diff --git a/test/test_miSim.m b/test/test_miSim.m index 081cd10..97a0249 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -31,8 +31,8 @@ classdef test_miSim < matlab.unittest.TestCase objective = sensingObjective; % Agents - minAgents = 2; % Minimum number of agents to be randomly generated - maxAgents = 4; % Maximum number of agents to be randomly generated + minAgents = 4; % Minimum number of agents to be randomly generated + maxAgents = 6; % Maximum number of agents to be randomly generated sensingLength = 0.05; % length parameter used by sensing function agents = cell(0, 1);