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

debugging comms constraints

Browse Source
This commit is contained in:
Kevin Dee
2025-12-31 20:54:01 -08:00
parent d6a9c4ac06
commit 4735c2b77b
4 changed files with 30 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
@miSim/constrainMotion.m
View File

@@ -18,7 +18,7 @@ function [obj] = constrainMotion(obj)
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);
nLNAPairs = sum(obj.constraintAdjacencyMatrix, 'all') - size(obj.agents, 1);
total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
kk = 1;
A = zeros(total, 3 * size(obj.agents, 1));
@@ -98,14 +98,22 @@ function [obj] = constrainMotion(obj)
% Add communication network constraints
hComms = NaN(size(obj.agents, 1));
hComms(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
hComms(logical(eye(size(obj.agents, 1)))) = 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);
% 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;
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - agents(jj).pos);
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 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
b(kk) = obj.barrierGain * hComms(ii, jj)^3;
kk = kk + 1;
@@ -119,8 +127,12 @@ function [obj] = constrainMotion(obj)
f = -2 * vhat;
% Update solution based on constraints
assert(size(A,2) == size(H,1))
assert(size(A,1) == size(b,1))
assert(size(H,1) == length(f))
opt = optimoptions('quadprog', 'Display', 'off');
[vNew, ~, exitflag] = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opt);
[vNew, ~, exitflag, m] = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opt);
assert(exitflag == 1, sprintf('quadprog failure... %s%s', newline, m.message));
vNew = reshape(vNew, 3, size(obj.agents, 1))';
if exitflag <= 0

8
@miSim/lesserNeighbor.m
View File

@@ -21,12 +21,8 @@ function obj = lesserNeighbor(obj)
% indexed agent provides connectivity already
for jj = 1:(ii - 1)
for kk = 1:(jj - 1)
% Check if a connection between the two lesser agents
% already exists
if constraintAdjacencyMatrix(jj, kk)
constraintAdjacencyMatrix(jj, kk) = false;
constraintAdjacencyMatrix(kk, jj) = false;
end
constraintAdjacencyMatrix(ii, kk) = false;
constraintAdjacencyMatrix(kk, ii) = false;
end
end
end

12
@miSim/updateAdjacency.m
View File

@@ -18,12 +18,12 @@ function obj = updateAdjacency(obj)
continue;
end
% Check that agents do not have their line of sight obstructed
for kk = 1:size(obj.obstacles, 1)
if obj.obstacles{kk}.containsLine(obj.agents{jj}.pos, obj.agents{ii}.pos)
A(ii, jj) = false;
end
end
% % Check that agents do not have their line of sight obstructed
% for kk = 1:size(obj.obstacles, 1)
% if obj.obstacles{kk}.containsLine(obj.agents{jj}.pos, obj.agents{ii}.pos)
% A(ii, jj) = false;
% end
% end
end
end

6
test/test_miSim.m
View File

@@ -507,7 +507,7 @@ classdef test_miSim < matlab.unittest.TestCase
geometry1 = spherical;
geometry2 = geometry1;
geometry1 = geometry1.initialize(tc.domain.center - d + [0, radius * 1.5, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
geometry2 = geometry2.initialize(tc.domain.center - d - [0, radius * 1.5, 0] - [0, 1, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
geometry2 = geometry2.initialize(tc.domain.center - d - [0, radius * 1.5, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
% Initialize agent sensor model
sensor = sigmoidSensor;
@@ -516,8 +516,8 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agents
tc.agents = {agent; agent;};
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 4, 1, sprintf("Agent %d", 1), false, false);
tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.5, 0] - [0, 1, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 4, 2, sprintf("Agent %d", 2), false, false);
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 10, 1, sprintf("Agent %d", 1), false, false);
tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 10, 2, sprintf("Agent %d", 2), false, false);
% Initialize obstacles
obstacleLength = 1;