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

results plot1 WIP

Browse Source
This commit is contained in:
Kevin Dee
2026-03-15 13:42:35 -07:00
parent ca891a809f
commit 2a0e2e500f
19 changed files with 437 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

2
@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
@@ -67,7 +66,6 @@ classdef miSim
obj (1, 1) miSim
end
obj.domain = rectangularPrism;
obj.objective = sensingObjective;
obj.obstacles = {rectangularPrism};
obj.agents = {agent};
end

1
@miSim/teardown.m
View File

@@ -39,7 +39,6 @@ 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;

9
@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
@@ -20,10 +20,9 @@ function validate(obj)
for kk = 1:size(obj.agents, 1)
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
if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2 - 1e-3
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

7
@miSim/writeInits.m
View File

@@ -14,6 +14,9 @@ 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 +27,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");