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

added domain constraints

Browse Source
This commit is contained in:
Kevin Dee
2025-12-23 17:22:34 -08:00
parent 8315b6c511
commit 14e372ae55
3 changed files with 60 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
@miSim/constrainMotion.m
View File

@@ -17,11 +17,12 @@ function [obj] = constrainMotion(obj)
% Initialize QP based on number of agents and obstacles % Initialize QP based on number of agents and obstacles
h = NaN(size(obj.agents, 1)); h = NaN(size(obj.agents, 1));
h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0 h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
nAAPairs = nchoosek(size(obj.agents, 1), 2); nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); 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; kk = 1;
A = zeros(nAAPairs + nAOPairs, 3 * size(obj.agents, 1)); A = zeros(nAAPairs + nAOPairs + nADPairs, 3 * size(obj.agents, 1));
b = zeros(nAAPairs + nAOPairs, 1); b = zeros(nAAPairs + nAOPairs + nADPairs, 1);
% Set up collision avoidance constraints % Set up collision avoidance constraints
for ii = 1:(size(obj.agents, 1) - 1) for ii = 1:(size(obj.agents, 1) - 1)
@@ -51,7 +52,47 @@ function [obj] = constrainMotion(obj)
kk = kk + 1; kk = kk + 1;
end end
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 % Solve QP program generated earlier
vhat = reshape(v', 3 * size(obj.agents, 1), 1); vhat = reshape(v', 3 * size(obj.agents, 1), 1);

7
geometries/@rectangularPrism/initializeRandom.m
View File

@@ -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) arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrism')}; obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID; tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
label (1, 1) string = ""; label (1, 1) string = "";
minDimension (1, 1) double = 10; minDimension (1, 1) double = 10;
maxDimension (1, 1) double= 20; maxDimension (1, 1) double = 20;
domain (1, 1) {mustBeGeometry} = rectangularPrism; domain (1, 1) {mustBeGeometry} = rectangularPrism;
minAlt (1, 1) double = 0;
end end
arguments (Output) arguments (Output)
obj (1, 1) {mustBeA(obj, 'rectangularPrism')}; 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) 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 if ii == 0 || ii > 10
candidateMinCorner = domain.random(); candidateMinCorner = domain.random();
candidateMinCorner(3) = 0; % bind to floor candidateMinCorner(3) = minAlt; % bind to floor (plus minimum altitude constraint)
ii = 1; ii = 1;
end end

20
test/test_miSim.m
View File

@@ -4,7 +4,7 @@ classdef test_miSim < matlab.unittest.TestCase
testClass = miSim; testClass = miSim;
% Debug % Debug
makeVideo = false; % disable video writing for big performance increase makeVideo = true; % disable video writing for big performance increase
% Sim % Sim
maxIter = 250; maxIter = 250;
@@ -88,7 +88,7 @@ classdef test_miSim < matlab.unittest.TestCase
while badCandidate while badCandidate
% Instantiate a rectangular prism obstacle inside the domain % Instantiate a rectangular prism obstacle inside the domain
tc.obstacles{ii} = rectangularPrism; 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 % Check if the obstacle collides with an existing obstacle
if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii}) if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
@@ -221,7 +221,7 @@ classdef test_miSim < matlab.unittest.TestCase
while badCandidate while badCandidate
% Instantiate a rectangular prism obstacle inside the domain % Instantiate a rectangular prism obstacle inside the domain
tc.obstacles{ii} = rectangularPrism; 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 % Check if the obstacle collides with an existing obstacle
if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii}) 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); tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry % Initialize agent collision geometry
radius = 1.5; radius = 1.1;
d = [3, 0, 0]; d = [3, 0, 0];
geometry1 = spherical; geometry1 = spherical;
geometry2 = geometry1; geometry2 = geometry1;
geometry1 = geometry1.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.1, 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 % Initialize agent sensor model
sensor = sigmoidSensor; sensor = sigmoidSensor;
@@ -514,16 +514,16 @@ 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.1, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3, 1, sprintf("Agent %d", 1), 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.1, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3, 2, sprintf("Agent %d", 2), 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 % Initialize obstacles
obstacleLength = 1; obstacleLength = 1;
tc.obstacles{1} = rectangularPrism; 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 % 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 % Run the simulation
tc.testClass.run(); tc.testClass.run();