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

Added lesser neighbor algorithm and constraints

Browse Source
This commit is contained in:
Kevin Dee
2025-12-31 19:19:36 -08:00
parent fa8da50db1
commit d6a9c4ac06
10 changed files with 80 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
@miSim/constrainMotion.m
View File

@@ -15,16 +15,18 @@ function [obj] = constrainMotion(obj)
v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
% Initialize QP based on number of agents and obstacles
h = NaN(size(obj.agents, 1));
h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling)
nLNAPairs = sum(obj.constraintAdjacencyMatrix) - size(obj.agents, 1);
total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
kk = 1;
A = zeros(nAAPairs + nAOPairs + nADPairs, 3 * size(obj.agents, 1));
b = zeros(nAAPairs + nAOPairs + nADPairs, 1);
A = zeros(total, 3 * size(obj.agents, 1));
b = zeros(total, 1);
% Set up collision avoidance constraints
h = NaN(size(obj.agents, 1));
h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
for ii = 1:(size(obj.agents, 1) - 1)
for jj = (ii + 1):size(obj.agents, 1)
h(ii, jj) = norm(agents(ii).pos - agents(jj).pos)^2 - (agents(ii).collisionGeometry.radius + agents(jj).collisionGeometry.radius)^2;
@@ -94,6 +96,23 @@ function [obj] = constrainMotion(obj)
kk = kk + 1;
end
% Add communication network constraints
hComms = NaN(size(obj.agents, 1));
hComms(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
for ii = 1:(size(obj.agents, 1) - 1)
for jj = (ii + 1):size(obj.agents, 1)
if obj.constraintAdjacencyMatrix(ii, jj)
hComms(ii, jj) = (agents(ii).commsGeometry.radius + agents(jj).commsGeometry.radius)^2 - norm(agents(ii).pos - agents(jj).pos)^2;
hComms(jj, ii) = hComms(ii, jj);
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;
end
end
end
% Solve QP program generated earlier
vhat = reshape(v', 3 * size(obj.agents, 1), 1);
H = 2 * eye(3 * size(obj.agents, 1));