From b82c87520aee6af82e2082b7f6dcba14a2b5e359 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Sun, 26 Oct 2025 13:30:09 -0700 Subject: [PATCH] protected objective from domain edges --- agent.m | 2 +- ...arPrismConstraint.m => rectangularPrism.m} | 54 ++++++++++++++--- miSim.m | 14 ++--- .../dcQXSYjif37BKJ8bynJny6jq5-sp.xml | 2 - ...d.xml => oA3NoVC5FBKXh-LtCKBuVxHDs28d.xml} | 0 .../oA3NoVC5FBKXh-LtCKBuVxHDs28p.xml | 2 + ...d.xml => NmnqsjHeF9rAhG9A30BKV_vI7tId.xml} | 0 .../NmnqsjHeF9rAhG9A30BKV_vI7tIp.xml | 2 + .../ia5FvKngd_-0tJFYAO64kIgybSkp.xml | 2 - test_miSim.m | 60 +++++++++++-------- validators/mustBeConstraintGeometries.m | 10 ---- validators/mustBeGeometry.m | 10 ++++ 12 files changed, 102 insertions(+), 56 deletions(-) rename geometries/{rectangularPrismConstraint.m => rectangularPrism.m} (64%) delete mode 100644 resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/dcQXSYjif37BKJ8bynJny6jq5-sp.xml rename resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/{dcQXSYjif37BKJ8bynJny6jq5-sd.xml => oA3NoVC5FBKXh-LtCKBuVxHDs28d.xml} (100%) create mode 100644 resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28p.xml rename resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/{ia5FvKngd_-0tJFYAO64kIgybSkd.xml => NmnqsjHeF9rAhG9A30BKV_vI7tId.xml} (100%) create mode 100644 resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tIp.xml delete mode 100644 resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/ia5FvKngd_-0tJFYAO64kIgybSkp.xml delete mode 100644 validators/mustBeConstraintGeometries.m create mode 100644 validators/mustBeGeometry.m diff --git a/agent.m b/agent.m index 1cb27b7..1f338af 100644 --- a/agent.m +++ b/agent.m @@ -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 diff --git a/geometries/rectangularPrismConstraint.m b/geometries/rectangularPrism.m similarity index 64% rename from geometries/rectangularPrismConstraint.m rename to geometries/rectangularPrism.m index d8f3bb4..c2b37a9 100644 --- a/geometries/rectangularPrismConstraint.m +++ b/geometries/rectangularPrism.m @@ -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"); diff --git a/miSim.m b/miSim.m index 9edb5c5..908f33f 100644 --- a/miSim.m +++ b/miSim.m @@ -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; diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/dcQXSYjif37BKJ8bynJny6jq5-sp.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/dcQXSYjif37BKJ8bynJny6jq5-sp.xml deleted file mode 100644 index 6e6097c..0000000 --- a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/dcQXSYjif37BKJ8bynJny6jq5-sp.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/dcQXSYjif37BKJ8bynJny6jq5-sd.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28d.xml similarity index 100% rename from resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/dcQXSYjif37BKJ8bynJny6jq5-sd.xml rename to resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28d.xml diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28p.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28p.xml new file mode 100644 index 0000000..60b28f2 --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/ia5FvKngd_-0tJFYAO64kIgybSkd.xml b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tId.xml similarity index 100% rename from resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/ia5FvKngd_-0tJFYAO64kIgybSkd.xml rename to resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tId.xml diff --git a/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tIp.xml b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tIp.xml new file mode 100644 index 0000000..75e44a0 --- /dev/null +++ b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tIp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/ia5FvKngd_-0tJFYAO64kIgybSkp.xml b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/ia5FvKngd_-0tJFYAO64kIgybSkp.xml deleted file mode 100644 index fd48245..0000000 --- a/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/ia5FvKngd_-0tJFYAO64kIgybSkp.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/test_miSim.m b/test_miSim.m index 1a5872e..7ca6362 100644 --- a/test_miSim.m +++ b/test_miSim.m @@ -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 diff --git a/validators/mustBeConstraintGeometries.m b/validators/mustBeConstraintGeometries.m deleted file mode 100644 index d16301a..0000000 --- a/validators/mustBeConstraintGeometries.m +++ /dev/null @@ -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 \ No newline at end of file diff --git a/validators/mustBeGeometry.m b/validators/mustBeGeometry.m new file mode 100644 index 0000000..a8242c1 --- /dev/null +++ b/validators/mustBeGeometry.m @@ -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 \ No newline at end of file