From 1e7540226efe92642b33cf1fdb74f78205919b0d Mon Sep 17 00:00:00 2001 From: Kevin D Date: Tue, 6 Jan 2026 12:24:42 -0800 Subject: [PATCH] fixed and verified communications constraint --- @agent/agent.m | 1 - @agent/initialize.m | 3 +- @miSim/constrainMotion.m | 22 +++---- @miSim/lesserNeighbor.m | 8 +-- @miSim/miSim.m | 5 +- @miSim/run.m | 3 + @miSim/updateAdjacency.m | 3 +- @miSim/validate.m | 12 ++++ @sensingObjective/initialize.m | 1 + .../@rectangularPrism/rectangularPrism.m | 2 +- test/test_miSim.m | 63 ++++++++++++++++--- 11 files changed, 89 insertions(+), 34 deletions(-) create mode 100644 @miSim/validate.m diff --git a/@agent/agent.m b/@agent/agent.m index a4c2c37..895f1eb 100644 --- a/@agent/agent.m +++ b/@agent/agent.m @@ -29,7 +29,6 @@ classdef agent comRange = NaN; commsGeometry = spherical; lesserNeighbors = []; - performance = 0; diff --git a/@agent/initialize.m b/@agent/initialize.m index 7ccfe12..2c40e0b 100644 --- a/@agent/initialize.m +++ b/@agent/initialize.m @@ -22,14 +22,13 @@ 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, obj.comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label)); + obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label)); if obj.debug obj.debugFig = figure; diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 08d11f9..71ebdad 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -102,21 +102,17 @@ function [obj] = constrainMotion(obj) for ii = 1:(size(obj.agents, 1) - 1) for jj = (ii + 1):size(obj.agents, 1) if obj.constraintAdjacencyMatrix(ii, jj) - % d = agents(ii).pos - agents(jj).pos; - % h = agents(ii).commsGeometry.radius^2 - dot(d,d); - % - % A(kk, (3*ii-2):(3*ii)) = 2*d; - % A(kk, (3*jj-2):(3*jj)) = -2*d; - % b(kk) = obj.barrierGain * h^3; - % - % kk = kk + 1; + hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(agents(ii).pos - agents(jj).pos)^2; - hComms(ii, jj) = agents(ii).commsGeometry.radius^2 - norm(agents(ii).pos - agents(jj).pos)^2; - - A(kk, (3 * ii - 2):(3 * ii)) = 2 * (agents(ii).pos - agents(jj).pos); + 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; + b(kk) = obj.barrierGain * hComms(ii, jj); + + % dVNominal = v(ii, 1:3) - v(jj, 1:3); % nominal velocities + % h_dot_nom = -2 * (agents(ii).pos - agents(jj).pos) * dVNominal'; + % b(kk) = -h_dot_nom + obj.barrierGain * hComms(ii, jj)^3; + + kk = kk + 1; end end end diff --git a/@miSim/lesserNeighbor.m b/@miSim/lesserNeighbor.m index 8c20200..c75fd36 100644 --- a/@miSim/lesserNeighbor.m +++ b/@miSim/lesserNeighbor.m @@ -6,12 +6,7 @@ function obj = lesserNeighbor(obj) 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 - + % initialize solution with self-connections only constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1))); for ii = 1:size(obj.agents, 1) @@ -34,6 +29,7 @@ function obj = lesserNeighbor(obj) subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors); % Find connected components in each agent's subgraph + % TODO: rewrite this using matlab "conncomp" function? visited = false(size(subgraphAdjacency, 1), 1); components = {}; for jj = 1:size(subgraphAdjacency, 1) diff --git a/@miSim/miSim.m b/@miSim/miSim.m index 42f0191..dc2f517 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -15,7 +15,8 @@ classdef miSim 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 + perf; % sensor performance timeseries array + performance = 0; % simulation performance timeseries vector barrierGain = 100; % collision avoidance parameter minAlt = 1; % minimum allowed altitude constraint @@ -25,7 +26,6 @@ classdef miSim properties (Access = private) % Sim t = NaN; % current sim time - perf; % sensor performance timeseries array times; partitioningTimes; @@ -61,6 +61,7 @@ classdef miSim [obj] = plotGraph(obj); [obj] = plotTrails(obj); [obj] = updatePlots(obj, updatePartitions); + validate(obj); end methods (Access = private) [v] = setupVideoWriter(obj); diff --git a/@miSim/run.m b/@miSim/run.m index 01a7dd4..d8be1a1 100644 --- a/@miSim/run.m +++ b/@miSim/run.m @@ -18,6 +18,9 @@ function [obj] = run(obj) obj.timestepIndex = ii; fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1); + % Validate current simulation configuration + obj.validate(); + % Check if it's time for new partitions updatePartitions = false; if ismember(obj.t, obj.partitioningTimes) diff --git a/@miSim/updateAdjacency.m b/@miSim/updateAdjacency.m index 1121dd8..fb595fa 100644 --- a/@miSim/updateAdjacency.m +++ b/@miSim/updateAdjacency.m @@ -13,7 +13,7 @@ function obj = updateAdjacency(obj) for ii = 2:size(A, 1) for jj = 1:(ii - 1) % Check that agents are not out of range - if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.comRange, obj.agents{jj}.comRange]); + if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius]) A(ii, jj) = false; % comm range violation continue; end @@ -31,6 +31,5 @@ function obj = updateAdjacency(obj) 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/@miSim/validate.m b/@miSim/validate.m new file mode 100644 index 0000000..b20d3d3 --- /dev/null +++ b/@miSim/validate.m @@ -0,0 +1,12 @@ +function validate(obj) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'miSim')}; + end + arguments (Output) + end + + if max(conncomp(graph(obj.adjacency))) ~= 1 + warning("Network is not connected"); + end + +end diff --git a/@sensingObjective/initialize.m b/@sensingObjective/initialize.m index 84752e0..5af4843 100644 --- a/@sensingObjective/initialize.m +++ b/@sensingObjective/initialize.m @@ -37,6 +37,7 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr % store ground position idx = obj.values == 1; obj.groundPos = [obj.X(idx), obj.Y(idx)]; + obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow) assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective") end \ No newline at end of file diff --git a/geometries/@rectangularPrism/rectangularPrism.m b/geometries/@rectangularPrism/rectangularPrism.m index 1caae2a..17a4b6f 100644 --- a/geometries/@rectangularPrism/rectangularPrism.m +++ b/geometries/@rectangularPrism/rectangularPrism.m @@ -3,7 +3,6 @@ classdef rectangularPrism properties (SetAccess = private, GetAccess = public) % Meta tag = REGION_TYPE.INVALID; - label = ""; % Spatial minCorner = NaN(1, 3); @@ -27,6 +26,7 @@ classdef rectangularPrism dBarrierFunction; end properties (SetAccess = public, GetAccess = public) + label = ""; % Sensing objective (for DOMAIN region type only) objective; end diff --git a/test/test_miSim.m b/test/test_miSim.m index 93ff347..0ed5ec1 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -491,6 +491,9 @@ classdef test_miSim < matlab.unittest.TestCase tc.testClass.run(); end function test_obstacle_avoidance(tc) + % Right now this seems to prove that the communications + % constraints are working, but the result is dissatisfying + % Fixed single obstacle % Fixed two agents initial conditions % Exaggerated large collision geometries @@ -506,30 +509,76 @@ classdef test_miSim < matlab.unittest.TestCase d = [3, 0, 0]; geometry1 = spherical; geometry2 = geometry1; - geometry1 = geometry1.initialize(tc.domain.center - d + [0, radius * 1.5, 0], radius, REGION_TYPE.COLLISION); - geometry2 = geometry2.initialize(tc.domain.center - d - [0, radius * 1.5, 0], radius, REGION_TYPE.COLLISION); + geometry1 = geometry1.initialize(tc.domain.center - d + [0.1, radius * 1.1, 0], radius, REGION_TYPE.COLLISION); + geometry2 = geometry2.initialize(tc.domain.center - d - [0.1, radius * 1.1, 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 - tc.agents = {agent; agent;}; - tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry1, sensor, 10); - tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry2, sensor, 10); % Initialize obstacles obstacleLength = 1; tc.obstacles{1} = rectangularPrism; tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, tc.minAlt; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1"); + % Initialize agents + commsRadius = (2*radius + obstacleLength) * 0.9; % defined such that they cannot go around the obstacle on both sides + tc.agents = {agent; agent;}; + tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0.1, radius * 1.1, 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius); + tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0.1, radius *1.1, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius); + % Initialize the simulation tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 100, tc.obstacles, tc.makeVideo); % Run the simulation tc.testClass.run(); end + function test_communications_constraint(tc) + % No obstacles + % Fixed two agents initial conditions + % Negligible collision geometries + % Non-standard domain with two objectives that will try to pull the + % agents apart + l = 10; % domain size + dom = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); + + % make basic sensing objective + dom.objective = dom.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [2, 8]) + mvnpdf([x(:), y(:)], [8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange); + + % Initialize agent collision geometry + radius = 0.1; + d = [1, 0, 0]; + geometry1 = spherical; + geometry2 = geometry1; + geometry1 = geometry1.initialize(dom.center + d, radius, REGION_TYPE.COLLISION); + geometry2 = geometry2.initialize(dom.center - d, 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 obstacles + tc.obstacles = {}; + + % Initialize agents + commsRadius = 4; % defined such that they cannot reach their objective without breaking connectivity + tc.agents = {agent; agent;}; + tc.agents{1} = tc.agents{1}.initialize(dom.center + d, zeros(1,3), 0, 0, geometry1, sensor, commsRadius); + tc.agents{2} = tc.agents{2}.initialize(dom.center - d, zeros(1,3), 0, 0, geometry2, sensor, commsRadius); + + % Initialize the simulation + tc.testClass = tc.testClass.initialize(dom, dom.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 30, tc.obstacles, false, false); + + % Run the simulation + tc.testClass = tc.testClass.run(); + + % Assert that at some step, performance decreased + % Indicating that the communications constraint pulled agents + % together, away from their objectives + tc.verifyTrue(any(diff(tc.testClass.performance) < 0)); + end function test_obstacle_blocks_comms_LOS(tc) % Fixed single obstacle % Fixed two agents initial conditions