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

protected objective from domain edges

Browse Source
This commit is contained in:
Kevin D
2025-10-26 13:30:09 -07:00
parent 78538ab586
commit b82c87520a
12 changed files with 102 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
test_miSim.m
View File

@@ -2,16 +2,19 @@ classdef test_miSim < matlab.unittest.TestCase
properties (Access = private)
testClass = miSim;
% Domain
domain = rectangularPrismConstraint;
domain = rectangularPrism;
% Obstacles
constraintGeometries = cell(1, 0);
minNumObstacles = 1;
maxNumObstacles = 3;
obstacles = cell(1, 0);
minObstacleDimension = 1;
% Objective
objective = sensingObjective;
objectiveFunction = @(x, y) 0;
objectiveDiscretizationStep = 0.01;
protectedRange = 1;
% Agents
minAgents = 3;
@@ -31,15 +34,20 @@ classdef test_miSim < matlab.unittest.TestCase
methods (TestMethodSetup)
% Generate a random domain
function tc = setDomain(tc)
% random integer-sized domain within [-10, 10] in all dimensions
% random integer-sized domain ranging from [0, 5] to [0, 25] in all dimensions
L = ceil(5 + rand * 10 + rand * 10);
tc.domain = tc.domain.initialize([zeros(1, 3); L * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
end
% Generate a random sensing objective within that domain
function tc = setSensingObjective(tc)
mu = tc.domain.random();
sig = [3, 1; 1, 4];
tc.objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu(1, 1:2), sig);
mu = tc.domain.minCorner;
while tc.domain.interiorDistance(mu) < tc.protectedRange
mu = tc.domain.random();
end
mu(3) = 0;
assert(tc.domain.contains(mu));
sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
tc.objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu(1:2), sig);
tc.objective = tc.objective.initialize(tc.objectiveFunction, tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep);
end
% Instantiate agents, they will be initialized under different
@@ -55,15 +63,14 @@ classdef test_miSim < matlab.unittest.TestCase
methods (Test)
% Test methods
function misim_initialization(tc)
% randomly create 2-3 constraint geometries
nGeom = 1 + randi(2);
tc.constraintGeometries = cell(nGeom, 1);
for ii = 1:size(tc.constraintGeometries, 1)
% Instantiate a rectangular prism constraint that spans the
% domain's height
tc.constraintGeometries{ii, 1} = rectangularPrismConstraint;
% randomly create 2-3 obstacles
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
tc.obstacles = cell(nGeom, 1);
for ii = 1:size(tc.obstacles, 1)
% Instantiate a rectangular prism obstacle
tc.obstacles{ii, 1} = rectangularPrism;
% Randomly come up with constraint geometries until they
% Randomly come up with dimensions until they
% fit within the domain
candidateMinCorner = [-Inf(1, 2), 0];
candidateMaxCorner = Inf(1, 3);
@@ -71,7 +78,8 @@ classdef test_miSim < matlab.unittest.TestCase
% make sure obstacles are not too small in any dimension
tooSmall = true;
while tooSmall
% make sure the obstacles don't contain the sensing objective
% make sure the obstacles don't contain the sensing
% objective or encroach on it too much
obstructs = true;
while obstructs
@@ -105,12 +113,13 @@ classdef test_miSim < matlab.unittest.TestCase
candidateMinCorner(isinf(candidateMinCorner)) = tc.domain.minCorner(isinf(candidateMinCorner));
candidateMaxCorner(isinf(candidateMaxCorner)) = tc.domain.maxCorner(isinf(candidateMaxCorner));
% Initialize constraint geometry
tc.constraintGeometries{ii} = tc.constraintGeometries{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
% Initialize obstacle geometry
tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
end
% Repeat this until a connected set of agent initial conditions
% is found by random chance
nIter = 0;
connected = false;
while ~connected
% Randomly place agents in the domain
@@ -127,13 +136,13 @@ classdef test_miSim < matlab.unittest.TestCase
boringInit = false;
end
end
candidateGeometry = rectangularPrismConstraint;
candidateGeometry = rectangularPrism;
tc.agents{ii} = tc.agents{ii}.initialize(candidatePos, zeros(1, 3), eye(3), candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii)), ii, sprintf("Agent %d", ii));
% Check obstacles to confirm that none are violated
for jj = 1:size(tc.constraintGeometries, 1)
for jj = 1:size(tc.obstacles, 1)
inside = false;
if tc.constraintGeometries{jj, 1}.contains(tc.agents{ii, 1}.pos)
if tc.obstacles{jj, 1}.contains(tc.agents{ii, 1}.pos)
% Found a violation, stop checking
inside = true;
break;
@@ -146,7 +155,7 @@ classdef test_miSim < matlab.unittest.TestCase
end
% Create a collision geometry for this agent
candidateGeometry = rectangularPrismConstraint;
candidateGeometry = rectangularPrism;
candidateGeometry = candidateGeometry.initialize([tc.agents{ii}.pos - 0.1 * ones(1, 3); tc.agents{ii}.pos + 0.1 * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
% Check previously placed agents for collisions
@@ -190,10 +199,11 @@ classdef test_miSim < matlab.unittest.TestCase
% Check connectivity
G = graph(adjacency);
connected = all(conncomp(G) == 1);
nIter = nIter + 1;
end
% Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.constraintGeometries);
tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.obstacles);
% Plot domain
f = tc.testClass.domain.plotWireframe;
@@ -203,9 +213,9 @@ classdef test_miSim < matlab.unittest.TestCase
ylim([tc.testClass.domain.minCorner(2) - 0.5, tc.testClass.domain.maxCorner(2) + 0.5]);
zlim([tc.testClass.domain.minCorner(3) - 0.5, tc.testClass.domain.maxCorner(3) + 0.5]);
% Plot constraint geometries
for ii = 1:size(tc.testClass.constraintGeometries, 1)
tc.testClass.constraintGeometries{ii, 1}.plotWireframe(f);
% Plot obstacles
for ii = 1:size(tc.testClass.obstacles, 1)
tc.testClass.obstacles{ii, 1}.plotWireframe(f);
end
% Plot objective gradient