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 nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle 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) 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; total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
kk = 1; kk = 1;
A = zeros(total, 3 * size(obj.agents, 1)); A = zeros(total, 3 * size(obj.agents, 1));
@@ -98,14 +98,22 @@ function [obj] = constrainMotion(obj)
% Add communication network constraints % Add communication network constraints
hComms = NaN(size(obj.agents, 1)); 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 ii = 1:(size(obj.agents, 1) - 1)
for jj = (ii + 1):size(obj.agents, 1) for jj = (ii + 1):size(obj.agents, 1)
if obj.constraintAdjacencyMatrix(ii, jj) 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; % d = agents(ii).pos - agents(jj).pos;
hComms(jj, ii) = hComms(ii, jj); % 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)); A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
b(kk) = obj.barrierGain * hComms(ii, jj)^3; b(kk) = obj.barrierGain * hComms(ii, jj)^3;
kk = kk + 1; kk = kk + 1;
@@ -119,8 +127,12 @@ function [obj] = constrainMotion(obj)
f = -2 * vhat; f = -2 * vhat;
% Update solution based on constraints % 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'); 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))'; vNew = reshape(vNew, 3, size(obj.agents, 1))';
if exitflag <= 0 if exitflag <= 0

8
@miSim/lesserNeighbor.m
View File

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

12
@miSim/updateAdjacency.m
View File

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

6
test/test_miSim.m
View File

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