From 14e372ae550f085b8d100985f39e120a97b57985 Mon Sep 17 00:00:00 2001 From: krdee1 Date: Tue, 23 Dec 2025 17:22:34 -0800 Subject: [PATCH] added domain constraints --- @miSim/constrainMotion.m | 51 +++++++++++++++++-- .../@rectangularPrism/initializeRandom.m | 7 +-- test/test_miSim.m | 20 ++++---- 3 files changed, 60 insertions(+), 18 deletions(-) diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index df77edb..155142e 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -17,11 +17,12 @@ function [obj] = constrainMotion(obj) % 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 - nAAPairs = nchoosek(size(obj.agents, 1), 2); - nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); + 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) kk = 1; - A = zeros(nAAPairs + nAOPairs, 3 * size(obj.agents, 1)); - b = zeros(nAAPairs + nAOPairs, 1); + A = zeros(nAAPairs + nAOPairs + nADPairs, 3 * size(obj.agents, 1)); + b = zeros(nAAPairs + nAOPairs + nADPairs, 1); % Set up collision avoidance constraints for ii = 1:(size(obj.agents, 1) - 1) @@ -51,7 +52,47 @@ function [obj] = constrainMotion(obj) kk = kk + 1; end end - + + % Set up domain constraints (walls and ceiling only) + % Floor constraint is implicit with an obstacle corresponding to the + % minimum allowed altitude, but I included it anyways + for ii = 1:size(obj.agents, 1) + % X minimum + h_xMin = (agents(ii).pos(1) - obj.domain.minCorner(1)) - agents(ii).collisionGeometry.radius; + A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0]; + b(kk) = obj.barrierGain * h_xMin^3; + kk = kk + 1; + + % X maximum + h_xMax = (obj.domain.maxCorner(1) - agents(ii).pos(1)) - agents(ii).collisionGeometry.radius; + A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0]; + b(kk) = obj.barrierGain * h_xMax^3; + kk = kk + 1; + + % Y minimum + h_yMin = (agents(ii).pos(2) - obj.domain.minCorner(2)) - agents(ii).collisionGeometry.radius; + A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0]; + b(kk) = obj.barrierGain * h_yMin^3; + kk = kk + 1; + + % Y maximum + h_yMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius; + A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0]; + b(kk) = obj.barrierGain * h_yMax^3; + kk = kk + 1; + + % Z minimum + h_zMin = (agents(ii).pos(3) - obj.domain.minCorner(3)) - agents(ii).collisionGeometry.radius; + A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1]; + b(kk) = obj.barrierGain * h_zMin^3; + kk = kk + 1; + + % Z maximum + h_zMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius; + A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1]; + b(kk) = obj.barrierGain * h_zMax^3; + kk = kk + 1; + end % Solve QP program generated earlier vhat = reshape(v', 3 * size(obj.agents, 1), 1); diff --git a/geometries/@rectangularPrism/initializeRandom.m b/geometries/@rectangularPrism/initializeRandom.m index aece5a7..a8218d4 100644 --- a/geometries/@rectangularPrism/initializeRandom.m +++ b/geometries/@rectangularPrism/initializeRandom.m @@ -1,11 +1,12 @@ -function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain) +function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain, minAlt) arguments (Input) obj (1, 1) {mustBeA(obj, 'rectangularPrism')}; tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID; label (1, 1) string = ""; minDimension (1, 1) double = 10; - maxDimension (1, 1) double= 20; + maxDimension (1, 1) double = 20; domain (1, 1) {mustBeGeometry} = rectangularPrism; + minAlt (1, 1) double = 0; end arguments (Output) obj (1, 1) {mustBeA(obj, 'rectangularPrism')}; @@ -27,7 +28,7 @@ function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, d while ~domain.contains(candidateMaxCorner) || all(domain.objective.groundPos + domain.objective.protectedRange >= candidateMinCorner(1:2), 2) && all(domain.objective.groundPos - domain.objective.protectedRange <= candidateMaxCorner(1:2), 2) if ii == 0 || ii > 10 candidateMinCorner = domain.random(); - candidateMinCorner(3) = 0; % bind to floor + candidateMinCorner(3) = minAlt; % bind to floor (plus minimum altitude constraint) ii = 1; end diff --git a/test/test_miSim.m b/test/test_miSim.m index 33c0f3a..a0d5b5b 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -4,7 +4,7 @@ classdef test_miSim < matlab.unittest.TestCase testClass = miSim; % Debug - makeVideo = false; % disable video writing for big performance increase + makeVideo = true; % disable video writing for big performance increase % Sim maxIter = 250; @@ -88,7 +88,7 @@ classdef test_miSim < matlab.unittest.TestCase while badCandidate % Instantiate a rectangular prism obstacle inside the domain tc.obstacles{ii} = rectangularPrism; - tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain); + tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain, tc.minAlt); % Check if the obstacle collides with an existing obstacle if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii}) @@ -221,7 +221,7 @@ classdef test_miSim < matlab.unittest.TestCase while badCandidate % Instantiate a rectangular prism obstacle inside the domain tc.obstacles{ii} = rectangularPrism; - tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain); + tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain, tc.minAlt); % Check if the obstacle collides with an existing obstacle if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii}) @@ -500,12 +500,12 @@ classdef test_miSim < matlab.unittest.TestCase tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange); % Initialize agent collision geometry - radius = 1.5; + radius = 1.1; d = [3, 0, 0]; geometry1 = spherical; 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)); + 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)); % Initialize agent sensor model sensor = sigmoidSensor; @@ -514,16 +514,16 @@ 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.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); + tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3*radius, 1, sprintf("Agent %d", 1), 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, 3*radius, 2, sprintf("Agent %d", 2), false); % Initialize obstacles obstacleLength = 1; tc.obstacles{1} = rectangularPrism; - tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, 0; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1"); + tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, tc.minAlt; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1"); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles, tc.makeVideo); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, tc.makeVideo); % Run the simulation tc.testClass.run();