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

added logging to matfile

Browse Source
This commit is contained in:
Kevin Dee
2026-03-13 10:55:46 -07:00
parent 24113f282f
commit 102f23316d
10 changed files with 53 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
@miSim/constrainMotion.m
View File

@@ -8,12 +8,6 @@ function [obj] = constrainMotion(obj)
nAgents = size(obj.agents, 1);
if nAgents < 2
nAAPairs = 0;
else
nAAPairs = nchoosek(nAgents, 2); % unique agent/agent pairs
end
% Compute velocity matrix from unconstrained gradient-ascent step
v = zeros(nAgents, 3);
for ii = 1:nAgents
@@ -26,13 +20,9 @@ function [obj] = constrainMotion(obj)
end
% Initialize QP based on number of agents and obstacles
nAOPairs = nAgents * size(obj.obstacles, 1); % unique agent/obstacle pairs
nADPairs = nAgents * 6; % agents x (4 walls + 1 floor + 1 ceiling)
nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - nAgents;
total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
kk = 1;
A = zeros(total, 3 * nAgents);
b = zeros(total, 1);
A = zeros(obj.numBarriers, 3 * nAgents);
b = zeros(obj.numBarriers, 1);
% Set up collision avoidance constraints
h = NaN(nAgents, nAgents);
@@ -60,6 +50,10 @@ function [obj] = constrainMotion(obj)
end
end
idx = length(h(triu(true(size(h)), 1)));
obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
idx = idx + 1;
hObs = NaN(nAgents, size(obj.obstacles, 1));
% Set up obstacle avoidance constraints
for ii = 1:nAgents
@@ -80,6 +74,9 @@ function [obj] = constrainMotion(obj)
end
end
obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
idx = idx + numel(hObs);
% Set up domain constraints (walls and ceiling only)
% Floor constraint is implicit with an obstacle corresponding to the
% minimum allowed altitude, but I included it anyways
@@ -120,6 +117,9 @@ function [obj] = constrainMotion(obj)
A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
kk = kk + 1;
obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
idx = idx + 6;
end
if coder.target('MATLAB')
@@ -154,6 +154,7 @@ function [obj] = constrainMotion(obj)
end
end
end
obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
% Solve QP program generated earlier
vhat = reshape(v', 3 * nAgents, 1);