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

included features from SPAWC 2026 branch

Browse Source
This commit is contained in:
Kevin Dee
2026-03-31 21:18:02 -07:00
parent ca891a809f
commit c3fa1de914
16 changed files with 65 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -48,6 +48,7 @@ sandbox/*
# Figures
*.fig
*.png
# Python Virtual Environment
aerpaw/venv/

1
@agent/initialize.m
View File

@@ -15,6 +15,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
end
obj.pos = pos;
obj.lastPos = pos;
obj.vel = zeros(1, 3);
obj.lastVel = zeros(1, 3);
obj.collisionGeometry = collisionGeometry;

9
@agent/run.m
View File

@@ -14,6 +14,13 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
obj (1, 1) {mustBeA(obj, "agent")};
end
% Always update lastPos/lastVel so constrainMotion evaluates barriers at
% the correct (most recent) position, even when this agent has no partition.
obj.lastPos = obj.pos;
if useDoubleIntegrator
obj.lastVel = obj.vel;
end
% Collect objective function values across partition
partitionMask = partitioning == index;
if ~any(partitionMask(:))
@@ -79,10 +86,8 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
gradNorm = norm(gradC);
% Compute unconstrained next state
obj.lastPos = obj.pos;
if useDoubleIntegrator
% Double-integrator: gradient produces desired acceleration with damping
obj.lastVel = obj.vel;
if gradNorm < 1e-100
a_gradient = zeros(1, 3);
else

26
@miSim/constrainMotion.m
View File

@@ -39,10 +39,10 @@ function [obj] = constrainMotion(obj)
h(logical(eye(nAgents))) = 0; % self value is 0
for ii = 1:(nAgents - 1)
for jj = (ii + 1):nAgents
h(ii, jj) = norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
h(ii, jj) = norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
h(jj, ii) = h(ii, jj);
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
% Slack derived from existing params: recovery velocity = max gradient approach velocity.
% Correction splits between 2 agents, so |A| = 2*r_sum
@@ -69,11 +69,11 @@ function [obj] = constrainMotion(obj)
for ii = 1:nAgents
for jj = 1:size(obj.obstacles, 1)
% find closest position to agent on/in obstacle
cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.pos);
cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.lastPos);
hObs(ii, jj) = dot(obj.agents{ii}.pos - cPos, obj.agents{ii}.pos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
hObs(ii, jj) = dot(obj.agents{ii}.lastPos - cPos, obj.agents{ii}.lastPos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - cPos);
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.lastPos - cPos);
% Floor for single-agent constraint: full correction on one agent, |A| = 2*r_i
r_i = obj.agents{ii}.collisionGeometry.radius;
v_max_i = obj.agents{ii}.initialStepSize / obj.timestep;
@@ -93,37 +93,37 @@ function [obj] = constrainMotion(obj)
h_xMin = 0.0; h_xMax = 0.0; h_yMin = 0.0; h_yMax = 0.0; h_zMin = 0.0; h_zMax = 0.0;
for ii = 1:nAgents
% X minimum
h_xMin = (obj.agents{ii}.pos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
h_xMin = (obj.agents{ii}.lastPos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent;
kk = kk + 1;
% X maximum
h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.pos(1)) - obj.agents{ii}.collisionGeometry.radius;
h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.lastPos(1)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent;
kk = kk + 1;
% Y minimum
h_yMin = (obj.agents{ii}.pos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
h_yMin = (obj.agents{ii}.lastPos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent;
kk = kk + 1;
% Y maximum
h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.pos(2)) - obj.agents{ii}.collisionGeometry.radius;
h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.lastPos(2)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent;
kk = kk + 1;
% Z minimum — enforce z >= minAlt + radius (not just z >= domain floor + radius)
h_zMin = (obj.agents{ii}.pos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
h_zMin = (obj.agents{ii}.lastPos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent;
kk = kk + 1;
% Z maximum
h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.pos(3)) - obj.agents{ii}.collisionGeometry.radius;
h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.lastPos(3)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
kk = kk + 1;
@@ -145,9 +145,9 @@ function [obj] = constrainMotion(obj)
if obj.constraintAdjacencyMatrix(ii, jj)
paddingFactor = 0.9; % Barrier at 90% of actual range; real comms still work beyond this
r_comms = paddingFactor * min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius]);
hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2;
hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2;
A(kk, (3 * ii - 2):(3 * ii)) = 2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
A(kk, (3 * ii - 2):(3 * ii)) = 2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
% One-step forward invariance: b = h/dt ensures h cannot

6
@miSim/initialize.m
View File

@@ -138,6 +138,12 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
% Initialize variable that will store barrier function values per timestep for analysis purposes
obj.barriers = NaN(obj.numBarriers, size(obj.times, 1));
% Initialize constraint adjacency history (nAgents x nAgents x nTimesteps)
nAgents = size(obj.agents, 1);
obj.constraintAdjacencyHist = false(nAgents, nAgents, size(obj.times, 1));
obj.constraintAdjacencyHist(:, :, 1) = obj.constraintAdjacencyMatrix;
% Set up plots showing initialized state
obj = obj.plot();

3
@miSim/miSim.m
View File

@@ -7,7 +7,6 @@ classdef miSim
timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
maxIter = NaN; % maximum number of simulation iterations
domain;
objective;
obstacles; % geometries that define obstacles within the domain
agents; % agents that move within the domain
adjacency = false(0, 0); % Adjacency matrix representing communications network graph
@@ -28,6 +27,7 @@ classdef miSim
spatialPlotIndices = [6, 4, 3, 2];
numBarriers = 0; % Number of barrier functions needed
barriers = []; % log barrier function values at each timestep for analysis
constraintAdjacencyHist = []; % log constraint adjacency matrix at each timestep
end
properties (Access = private)
@@ -67,7 +67,6 @@ classdef miSim
obj (1, 1) miSim
end
obj.domain = rectangularPrism;
obj.objective = sensingObjective;
obj.obstacles = {rectangularPrism};
obj.agents = {agent};
end

5
@miSim/run.m
View File

@@ -34,6 +34,11 @@ function [obj] = run(obj)
obj = obj.lesserNeighbor();
end
% Log constraint adjacency for this timestep
if coder.target('MATLAB')
obj.constraintAdjacencyHist(:, :, ii) = obj.constraintAdjacencyMatrix;
end
% Moving
% Iterate over agents to simulate their unconstrained motion
for jj = 1:size(obj.agents, 1)

3
@miSim/teardown.m
View File

@@ -20,6 +20,7 @@ function obj = teardown(obj)
out.dampingCoeff = obj.dampingCoeff;
out.useDoubleIntegrator = obj.useDoubleIntegrator;
out.useFixedTopology = obj.useFixedTopology;
out.constraintAdjacency = obj.constraintAdjacencyHist(:, :, 1:(end - 1));
for ii = 1:size(obj.agents, 1)
out.agent(ii).pos = squeeze(obj.posHist(ii, 1:(end - 1), 1:3));
out.agent(ii).vel = squeeze(obj.velHist(ii, 1:(end - 1), 1:3));
@@ -39,11 +40,11 @@ function obj = teardown(obj)
obj.timestepIndex = NaN;
obj.maxIter = NaN;
obj.domain = rectangularPrism;
obj.objective = sensingObjective;
obj.obstacles = cell(0, 1);
obj.agents = cell(0, 1);
obj.adjacency = NaN;
obj.constraintAdjacencyMatrix = NaN;
obj.constraintAdjacencyHist = [];
obj.partitioning = NaN;
obj.performance = 0;
obj.barrierGain = NaN;

7
@miSim/validate.m
View File

@@ -7,11 +7,11 @@ function validate(obj)
%% Communications Network Validators
if max(conncomp(graph(obj.adjacency))) ~= 1
warning("Network is not connected");
error("Network is not connected");
end
if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
warning("Eliminated network connections that were necessary");
error("Eliminated network connections that were necessary");
end
%% Obstacle Validators
@@ -21,9 +21,8 @@ function validate(obj)
P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner);
d = obj.agents{kk}.pos - P;
if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2
warning("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, dot(d, d) - obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
error("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, dot(d, d) - obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
end
end
end
end

6
@miSim/writeInits.m
View File

@@ -14,6 +14,8 @@ function writeInits(obj)
comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, obj.obstacles, 'UniformOutput', false));
obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, obj.obstacles, 'UniformOutput', false));
% Combine with simulation parameters
inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
@@ -24,7 +26,9 @@ function writeInits(obj)
"useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ...
"alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
"pos", pos); % still needs obstacle states and objective state
"pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ...
"obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ...
"objectiveIntegral", sum(obj.domain.objective.values(:)));
% Save all parameters to output file
initsFile = strcat(obj.artifactName, "_miSimInits");

15
@sensingObjective/initialize.m
View File

@@ -1,4 +1,4 @@
function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum, objectiveMu, objectiveSigma)
arguments (Input)
obj (1,1) {mustBeA(obj, "sensingObjective")};
objectiveFunction (1, 1) {mustBeA(objectiveFunction, "function_handle")};
@@ -6,6 +6,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
discretizationStep (1, 1) double = 1;
protectedRange (1, 1) double = 1;
sensorPerformanceMinimum (1, 1) double = 1e-6;
objectiveMu (:, 2) double = NaN(1, 2);
objectiveSigma (:, 2, 2) double = NaN(1, 2, 2);
end
arguments (Output)
obj (1,1) {mustBeA(obj, "sensingObjective")};
@@ -37,8 +39,13 @@ 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)
if any(isnan(objectiveMu))
obj.groundPos = [obj.X(idx), obj.Y(idx)];
obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
else
obj.groundPos = objectiveMu;
end
obj.objectiveSigma = objectiveSigma;
assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
assert(domain.distance([obj.groundPos, ones(size(obj.groundPos, 1), 1) .* domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective");
end

2
@sensingObjective/initializeRandomMvnpdf.m
View File

@@ -11,7 +11,7 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
% Set random objective position
mu = domain.minCorner;
while domain.distance(mu) < protectedRange
while domain.distance(mu) < protectedRange * 1.01
mu = domain.random();
end

3
@sensingObjective/sensingObjective.m
View File

@@ -2,7 +2,8 @@ classdef sensingObjective
% Sensing objective definition parent class
properties (SetAccess = private, GetAccess = public)
label = "";
groundPos = [NaN, NaN];
groundPos = NaN(1, 2);
objectiveSigma = NaN(1, 2, 2);
discretizationStep = NaN;
X = [];
Y = [];

2
aerpaw/config/scenario.csv
View File

@@ -1,2 +1,2 @@
timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
5, 100, 30.0, 0.1, 2.0, 2.0, 100, 3, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1
1, 150, 30.0, 0.1, 2.0, 1, 1, 1, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1
1 timestep maxIter minAlt discretizationStep protectedRange initialStepSize barrierGain barrierExponent collisionRadius comRange alphaDist betaDist alphaTilt betaTilt domainMin domainMax objectivePos objectiveVar sensorPerformanceMinimum initialPositions numObstacles obstacleMin obstacleMax useDoubleIntegrator dampingCoeff useFixedTopology
2 5 1 100 150 30.0 0.1 2.0 2.0 1 100 1 3 1 5.0, 5.0 25.0, 25.0 80.0, 80.0 0.25, 0.25 5.0, 5.0 0.1, 0.1 0.0, 0.0, 0.0 80.0, 80.0, 80.0 55.0, 55.0 40, 25, 25, 40 0.15 15.0, 10.0, 40.0, 5.0, 10.0, 45.0 1 1.0, 25.0, 0.0 30.0, 30.0, 50.0 1 2.0 1

6
util/objectiveFunctionWrapper.m
View File

@@ -4,12 +4,12 @@ function f = objectiveFunctionWrapper(center, sigma)
% composite objectives in particular
arguments (Input)
center (:, 2) double;
sigma (2, 2) double = eye(2);
sigma (:, 2, 2) double = eye(2);
end
arguments (Output)
f (1, 1) {mustBeA(f, "function_handle")};
end
f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), sigma), 1:size(center,1), "UniformOutput", false)), 2);
assert(size(center, 1) == size(sigma, 1));
f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), squeeze(sigma(i, :, :))), 1:size(center,1), "UniformOutput", false)), 2);
end