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

fixed and verified communications constraint

Browse Source
This commit is contained in:
Kevin Dee
2026-01-06 12:24:42 -08:00
parent 4fe897455d
commit 1e7540226e
11 changed files with 89 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
@agent/agent.m
View File

@@ -29,7 +29,6 @@ classdef agent
comRange = NaN;
commsGeometry = spherical;
lesserNeighbors = [];
performance = 0;

3
@agent/initialize.m
View File

@@ -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;

22
@miSim/constrainMotion.m
View File

@@ -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

8
@miSim/lesserNeighbor.m
View File

@@ -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)

5
@miSim/miSim.m
View File

@@ -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);

3
@miSim/run.m
View File

@@ -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)

3
@miSim/updateAdjacency.m
View File

@@ -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

12
@miSim/validate.m Normal file
View File

@@ -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

1
@sensingObjective/initialize.m
View File

@@ -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

2
geometries/@rectangularPrism/rectangularPrism.m
View File

@@ -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

63
test/test_miSim.m
View File

@@ -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