diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 05333c0..df77edb 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -14,12 +14,16 @@ function [obj] = constrainMotion(obj) agents = [obj.agents{:}]; 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 - nCon = nchoosek(size(obj.agents, 1), 2); + nAAPairs = nchoosek(size(obj.agents, 1), 2); + nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); kk = 1; - A = zeros(nCon, 3 * size(obj.agents, 1)); - b = zeros(nCon, 1); + A = zeros(nAAPairs + nAOPairs, 3 * size(obj.agents, 1)); + b = zeros(nAAPairs + nAOPairs, 1); + + % Set up collision avoidance constraints 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; @@ -32,6 +36,23 @@ function [obj] = constrainMotion(obj) end end + hObs = NaN(size(obj.agents, 1), size(obj.obstacles, 1)); + % Set up obstacle avoidance constraints + for ii = 1:size(obj.agents, 1) + for jj = 1:size(obj.obstacles, 1) + % find closest position to agent on/in obstacle + cPos = obj.obstacles{jj}.closestToPoint(agents(ii).pos); + + hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2; + + A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos); + b(kk) = obj.barrierGain * hObs(ii, jj)^3; + + kk = kk + 1; + end + end + + % Solve QP program generated earlier vhat = reshape(v', 3 * size(obj.agents, 1), 1); H = 2 * eye(3 * size(obj.agents, 1)); diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 8378812..7b6807f 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -32,8 +32,8 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part % Add an additional obstacle spanning the domain's footprint to % represent the minimum allowable altitude obj.minAlt = minAlt; - obj.obstacles{end + 1} = rectangularPrism; - obj.obstacles{end} = obj.obstacles{end}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint"); + obj.obstacles{end + 1, 1} = rectangularPrism; + obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint"); % Define objective obj.objective = objective; diff --git a/geometries/@rectangularPrism/closestToPoint.m b/geometries/@rectangularPrism/closestToPoint.m new file mode 100644 index 0000000..1bbfd5a --- /dev/null +++ b/geometries/@rectangularPrism/closestToPoint.m @@ -0,0 +1,19 @@ +function cPos = closestToPoint(obj, pos) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'rectangularPrism')}; + pos (:, 3) double; + end + arguments (Output) + cPos (:, 3) double; + end + cPos = NaN(1, 3); + for ii = 1:3 + if pos(ii) < obj.minCorner(ii) + cPos(ii) = obj.minCorner(ii); + elseif pos(ii) > obj.maxCorner(ii) + cPos(ii) = obj.maxCorner(ii); + else + cPos(ii) = pos(ii); + end + end +end \ No newline at end of file diff --git a/geometries/@rectangularPrism/distance.m b/geometries/@rectangularPrism/distance.m index 80caad6..660a345 100644 --- a/geometries/@rectangularPrism/distance.m +++ b/geometries/@rectangularPrism/distance.m @@ -4,7 +4,7 @@ function d = distance(obj, pos) pos (:, 3) double; end arguments (Output) - d (:, 1) double + d (:, 1) double; end if obj.contains(pos) % Queried point is inside geometry diff --git a/geometries/@rectangularPrism/rectangularPrism.m b/geometries/@rectangularPrism/rectangularPrism.m index 1a28a7b..1caae2a 100644 --- a/geometries/@rectangularPrism/rectangularPrism.m +++ b/geometries/@rectangularPrism/rectangularPrism.m @@ -36,6 +36,7 @@ classdef rectangularPrism [obj ] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain); [r ] = random(obj); [c ] = contains(obj, pos); + [cPos ] = closestToPoint(obj, pos); [d ] = distance(obj, pos); [g ] = distanceGradient(obj, pos); [c ] = containsLine(obj, pos1, pos2); diff --git a/test/test_miSim.m b/test/test_miSim.m index f1d44f3..33c0f3a 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -503,7 +503,9 @@ classdef test_miSim < matlab.unittest.TestCase radius = 1.5; d = [3, 0, 0]; geometry1 = spherical; - geometry1 = geometry1.initialize(tc.domain.center - d, radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1)); + geometry2 = geometry1; + geometry1 = geometry1.initialize(tc.domain.center - d + [0, radius * 1.1, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1)); + geometry2 = geometry2.initialize(tc.domain.center - d - [0, radius * 1.1, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1)); % Initialize agent sensor model sensor = sigmoidSensor; @@ -511,8 +513,9 @@ classdef test_miSim < matlab.unittest.TestCase sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3); % Initialize agents - tc.agents = {agent}; - tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d, zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3, 1, sprintf("Agent %d", 1), false); + tc.agents = {agent; agent;}; + tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.1, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3, 1, sprintf("Agent %d", 1), false); + tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.1, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3, 2, sprintf("Agent %d", 2), false); % Initialize obstacles obstacleLength = 1;