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

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