diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 432290d..08d11f9 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -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); - - A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - agents(jj).pos); + % 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) = 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 diff --git a/@miSim/lesserNeighbor.m b/@miSim/lesserNeighbor.m index fef0529..0bd723e 100644 --- a/@miSim/lesserNeighbor.m +++ b/@miSim/lesserNeighbor.m @@ -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 diff --git a/@miSim/updateAdjacency.m b/@miSim/updateAdjacency.m index f7a205c..1121dd8 100644 --- a/@miSim/updateAdjacency.m +++ b/@miSim/updateAdjacency.m @@ -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 diff --git a/test/test_miSim.m b/test/test_miSim.m index 97a0249..9ab0e96 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -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;