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 c61d627f0d
commit 5a9ac0c2d5
12 changed files with 102 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
agent.m
View File

@@ -23,7 +23,7 @@ classdef agent
pos (1, 3) double;
vel (1, 3) double;
cBfromC (3, 3) double {mustBeDcm};
collisionGeometry (1, 1) {mustBeConstraintGeometries};
collisionGeometry (1, 1) {mustBeGeometry};
index (1, 1) double = NaN;
label (1, 1) string = "";
end

54
geometries/rectangularPrismConstraint.m → geometries/rectangularPrism.m
View File

@@ -1,5 +1,5 @@
classdef rectangularPrismConstraint
% Rectangular prism constraint geometry
classdef rectangularPrism
% Rectangular prism geometry
properties (SetAccess = private, GetAccess = public)
tag = REGION_TYPE.INVALID;
label = "";
@@ -19,13 +19,13 @@ classdef rectangularPrismConstraint
methods (Access = public)
function obj = initialize(obj, bounds, tag, label)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
bounds (2, 3) double;
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
label (1, 1) string = "";
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
end
obj.tag = tag;
@@ -59,26 +59,62 @@ classdef rectangularPrismConstraint
end
function r = random(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
end
arguments (Output)
r (1, 3) double
end
r = (obj.vertices(1, 1:3) + rand(1, 3) .* obj.vertices(8, 1:3) - obj.vertices(1, 1:3))';
end
function d = distance(obj, pos)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
pos (:, 3) double;
end
arguments (Output)
d (:, 1) 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
d = norm(cPos - pos);
end
function d = interiorDistance(obj, pos)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
pos (:, 3) double;
end
arguments (Output)
d (:, 1) double
end
% find minimum distance to any face
d = min([pos(1) - obj.minCorner(1), ...
pos(2) - obj.minCorner(2), ...
pos(3) - obj.minCorner(3), ...
obj.maxCorner(1) - pos(1), ...
obj.maxCorner(2) - pos(2), ...
obj.maxCorner(3) - pos(3)]);
end
function c = contains(obj, pos)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
pos (:, 3) double;
end
arguments (Output)
c (:, 1) logical
end
c = all(pos >= repmat(obj.minCorner, size(pos, 2), 1), 2) & all(pos <= repmat(obj.maxCorner, size(pos, 2), 1), 2);
c = all(pos >= repmat(obj.minCorner, size(pos, 1), 1), 2) & all(pos <= repmat(obj.maxCorner, size(pos, 1), 1), 2);
end
function f = plotWireframe(obj, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
@@ -97,7 +133,7 @@ classdef rectangularPrismConstraint
Y = [obj.vertices(edges(:,1),2), obj.vertices(edges(:,2),2)]';
Z = [obj.vertices(edges(:,1),3), obj.vertices(edges(:,2),3)]';
% Plot the boundaries of the constraint geometry
% Plot the boundaries of the geometry
hold(f.CurrentAxes, "on");
plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
hold(f.CurrentAxes, "off");

14
miSim.m
View File

@@ -3,20 +3,20 @@ classdef miSim
% Simulation parameters
properties (SetAccess = private, GetAccess = public)
domain = rectangularPrismConstraint;
domain = rectangularPrism;
objective = sensingObjective;
constraintGeometries = cell(0, 1); % geometries that define constraints within the domain
obstacles = cell(0, 1); % geometries that define obstacles within the domain
agents = cell(0, 1); % agents that move within the domain
end
methods (Access = public)
function obj = initialize(obj, domain, objective, agents, constraintGeometries)
function obj = initialize(obj, domain, objective, agents, obstacles)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
domain (1, 1) {mustBeConstraintGeometries};
domain (1, 1) {mustBeGeometry};
objective (1, 1) {mustBeA(objective, 'sensingObjective')};
agents (:, 1) cell {mustBeAgents};
constraintGeometries (:, 1) cell {mustBeConstraintGeometries} = cell(0, 1);
obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'miSim')};
@@ -25,8 +25,8 @@ classdef miSim
%% Define domain
obj.domain = domain;
%% Add constraint geometries against the domain
obj.constraintGeometries = constraintGeometries;
%% Add geometries representing obstacles within the domain
obj.obstacles = obstacles;
%% Define objective
obj.objective = objective;

2
resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/dcQXSYjif37BKJ8bynJny6jq5-sp.xml
View File

@@ -1,2 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="mustBeConstraintGeometries.m" type="File"/>

0
resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/dcQXSYjif37BKJ8bynJny6jq5-sd.xml → resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28d.xml
View File

2
resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28p.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="mustBeGeometry.m" type="File"/>

0
resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/ia5FvKngd_-0tJFYAO64kIgybSkd.xml → resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tId.xml
View File

2
resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tIp.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="rectangularPrism.m" type="File"/>

2
resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/ia5FvKngd_-0tJFYAO64kIgybSkp.xml
View File

@@ -1,2 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="rectangularPrismConstraint.m" type="File"/>

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

10
validators/mustBeConstraintGeometries.m
View File

@@ -1,10 +0,0 @@
function mustBeConstraintGeometries(constraintGeometry)
validGeometries = ["rectangularPrismConstraint";];
if isa(constraintGeometry, 'cell')
for ii = 1:size(constraintGeometry, 1)
assert(isa(constraintGeometry{ii}, validGeometries), "Constraint geometry in index %d is not a valid constraint geometry class", ii);
end
else
assert(isa(constraintGeometry, validGeometries), "Constraint geometry is not a valid constraint geometry class");
end
end

10
validators/mustBeGeometry.m Normal file
View File

@@ -0,0 +1,10 @@
function mustBeGeometry(geometry)
validGeometries = ["rectangularPrism";];
if isa(geometry, 'cell')
for ii = 1:size(geometry, 1)
assert(isa(geometry{ii}, validGeometries), "Geometry in index %d is not a valid geometry class", ii);
end
else
assert(isa(geometry, validGeometries), "Geometry is not a valid geometry class");
end
end