From 4fe897455d571741d1297edd7d6eec1744b6536d Mon Sep 17 00:00:00 2001 From: Kevin D Date: Tue, 6 Jan 2026 10:57:56 -0800 Subject: [PATCH] fixed lesser neighbor algorithm --- @agent/agent.m | 2 ++ @agent/initialize.m | 3 +- @miSim/lesserNeighbor.m | 69 ++++++++++++++++++++++++++++++------ test/test_miSim.m | 77 ++++++++++++++++++++++++++++++++++------- 4 files changed, 127 insertions(+), 24 deletions(-) diff --git a/@agent/agent.m b/@agent/agent.m index 6e64bbb..a4c2c37 100644 --- a/@agent/agent.m +++ b/@agent/agent.m @@ -28,6 +28,8 @@ classdef agent % Communication comRange = NaN; commsGeometry = spherical; + lesserNeighbors = []; + performance = 0; diff --git a/@agent/initialize.m b/@agent/initialize.m index 2c40e0b..7ccfe12 100644 --- a/@agent/initialize.m +++ b/@agent/initialize.m @@ -22,13 +22,14 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod obj.tilt = tilt; obj.collisionGeometry = collisionGeometry; obj.sensorModel = sensorModel; + obj.comRange = comRange; obj.guidanceModel = @gradientAscent; obj.label = label; obj.debug = debug; obj.plotCommsGeometry = plotCommsGeometry; % Add spherical geometry based on com range - obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label)); + obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, obj.comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label)); if obj.debug obj.debugFig = figure; diff --git a/@miSim/lesserNeighbor.m b/@miSim/lesserNeighbor.m index 0bd723e..8c20200 100644 --- a/@miSim/lesserNeighbor.m +++ b/@miSim/lesserNeighbor.m @@ -12,20 +12,69 @@ function obj = lesserNeighbor(obj) % constraint adjacency matrix % Place that choice in the constraint adjacency matrix - % Begin with all possible connections and trim down - constraintAdjacencyMatrix = obj.adjacency; + constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1))); - % 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 + % Find lesser neighbors of each agent + % Lesser neighbors of ii are jj < ii in range of ii + lesserNeighbors = []; for jj = 1:(ii - 1) - for kk = 1:(jj - 1) - constraintAdjacencyMatrix(ii, kk) = false; - constraintAdjacencyMatrix(kk, ii) = false; + if obj.adjacency(ii, jj) + lesserNeighbors = [lesserNeighbors, jj]; + end + end + obj.agents{ii}.lesserNeighbors = lesserNeighbors; + + % Early exit for isolated agents + if isempty(obj.agents{ii}.lesserNeighbors) + continue + end + + % Focus on subgraph defined by lesser neighbors + subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors); + + % Find connected components in each agent's subgraph + visited = false(size(subgraphAdjacency, 1), 1); + components = {}; + for jj = 1:size(subgraphAdjacency, 1) + if ~visited(jj) + reachable = bfs(subgraphAdjacency, jj); + visited(reachable) = true; + components{end+1} = obj.agents{ii}.lesserNeighbors(reachable); + end + end + + % Connect to the greatest index in each connected component in the + % lesser neighborhood of this agent + for jj = 1:size(components, 2) + constraintAdjacencyMatrix(ii, max(components{jj})) = true; + constraintAdjacencyMatrix(max(components{jj}), ii) = true; + end + end + obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix | constraintAdjacencyMatrix'; +end + +function cComp = bfs(subgraphAdjacency, startIdx) + n = size(subgraphAdjacency, 1); + visited = false(1, n); + queue = startIdx; + cComp = startIdx; + visited(startIdx) = true; + + while ~isempty(queue) + current = queue(1); + queue(1) = []; + + % Find all neighbors of current node in the subgraph + neighbors = find(subgraphAdjacency(current, :)); + + for neighbor = neighbors + if ~visited(neighbor) + visited(neighbor) = true; + cComp = [cComp, neighbor]; + queue = [queue, neighbor]; end end end - - obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix; + cComp = sort(cComp); end \ No newline at end of file diff --git a/test/test_miSim.m b/test/test_miSim.m index 9c6c314..93ff347 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -571,7 +571,8 @@ classdef test_miSim < matlab.unittest.TestCase % No communications link should be established tc.assertEqual(tc.testClass.adjacency, logical(eye(2))); end - function test_LNA_example_case(tc) + function test_LNA_case_1(tc) + % based on example in meeting % No obstacles % Fixed 5 agents initial conditions % unitary communicaitons radius @@ -586,16 +587,12 @@ classdef test_miSim < matlab.unittest.TestCase % Initialize agent collision geometry radius = .01; d = 1; - geometry1 = spherical; - geometry2 = geometry1; - geometry3 = geometry2; - geometry4 = geometry3; - geometry5 = geometry4; - geometry1 = geometry1.initialize(tc.domain.center + [d, 0, 0], radius, REGION_TYPE.COLLISION); - geometry2 = geometry2.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION); - geometry3 = geometry2.initialize(tc.domain.center + [-d, d, 0], radius, REGION_TYPE.COLLISION); - geometry4 = geometry2.initialize(tc.domain.center + [-2*d, d, 0], radius, REGION_TYPE.COLLISION); - geometry5 = geometry2.initialize(tc.domain.center + [0, d, 0], radius, REGION_TYPE.COLLISION); + geometry5 = spherical; + geometry1 = geometry5.initialize(tc.domain.center + [d, 0, 0], radius, REGION_TYPE.COLLISION); + geometry2 = geometry5.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION); + geometry3 = geometry5.initialize(tc.domain.center + [-d, d, 0], radius, REGION_TYPE.COLLISION); + geometry4 = geometry5.initialize(tc.domain.center + [-2*d, d, 0], radius, REGION_TYPE.COLLISION); + geometry5 = geometry5.initialize(tc.domain.center + [0, d, 0], radius, REGION_TYPE.COLLISION); % Initialize agent sensor model sensor = sigmoidSensor; @@ -619,8 +616,62 @@ classdef test_miSim < matlab.unittest.TestCase [ 1, 1, 0, 0, 0; ... 1, 1, 0, 0, 1; ... 0, 0, 1, 1, 1; - 0, 0, 1, 0, 0; - 0, 1, 1, 0, 0; ])); + 0, 0, 1, 1, 0; + 0, 1, 1, 0, 1; ])); + end + function test_LNA_case_2(tc) + % based on example in paper Asynchronous Local Construction of Bounded-Degree Network Topologies Using Only Neighborhood Information + % No obstacles + % Fixed 7 agents initial conditions + % unitary communicaitons radius + % negligible collision radius + % make basic domain + l = 10; % domain size + tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); + + % make basic sensing objective + tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange); + + % Initialize agent collision geometry + radius = .01; + d = 1; + geometry7 = spherical; + geometry1 = geometry7.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], radius, REGION_TYPE.COLLISION); + geometry2 = geometry7.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], radius, REGION_TYPE.COLLISION); + geometry3 = geometry7.initialize(tc.domain.center + [0.9 * d, 0, 0], radius, REGION_TYPE.COLLISION); + geometry4 = geometry7.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], radius, REGION_TYPE.COLLISION); + geometry5 = geometry7.initialize(tc.domain.center + [0, 0.9 * d, 0], radius, REGION_TYPE.COLLISION); + geometry6 = geometry7.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION); + geometry7 = geometry7.initialize(tc.domain.center + [d/2, d/2, 0], radius, REGION_TYPE.COLLISION); + + % Initialize agent sensor model + sensor = sigmoidSensor; + alphaDist = l/2; % half of domain length/width + sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3); + + % Initialize agents + commsRadius = d; + tc.agents = {agent; agent; agent; agent; agent; agent; agent;}; + tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius); + tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius); + tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], zeros(1,3), 0, 0, geometry3, sensor, commsRadius); + tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], zeros(1,3), 0, 0, geometry4, sensor, commsRadius); + tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], zeros(1,3), 0, 0, geometry5, sensor, commsRadius); + tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, zeros(1,3), 0, 0, geometry6, sensor, commsRadius); + tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], zeros(1,3), 0, 0, geometry7, sensor, commsRadius); + + % Initialize the simulation + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, false, false); + + % Constraint adjacency matrix defined by LNA should be as follows + tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ... + [ 1, 1, 0, 0, 0, 0, 0; ... + 1, 1, 0, 0, 1, 0, 0; ... + 0, 0, 1, 1, 0, 0, 0; + 0, 0, 1, 1, 0, 1, 0; + 0, 1, 0, 0, 1, 1, 0; + 0, 0, 0, 1, 1, 1, 1; + 0, 0, 0, 0, 0, 1, 1; ])); end end