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

stopped randomly placing agents too close to objective, fixed row/col preference

Browse Source
This commit is contained in:
Kevin D
2025-10-25 23:41:12 -07:00
parent c5a2b644d8
commit 78538ab586
3 changed files with 70 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
geometries/rectangularPrismConstraint.m
View File

@@ -4,23 +4,23 @@ classdef rectangularPrismConstraint
tag = REGION_TYPE.INVALID; tag = REGION_TYPE.INVALID;
label = ""; label = "";
minCorner = NaN(3, 1); minCorner = NaN(1, 3);
maxCorner = NaN(3, 1); maxCorner = NaN(1, 3);
dimensions = NaN(3, 1); dimensions = NaN(1, 3);
center = NaN; center = NaN;
vertices = NaN(8, 3); vertices = NaN(8, 3);
footprint = NaN(2, 4); footprint = NaN(4, 2);
end end
methods (Access = public) methods (Access = public)
function obj = initialize(obj, bounds, tag, label) function obj = initialize(obj, bounds, tag, label)
arguments (Input) arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')}; obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
bounds (3, 2) double; bounds (2, 3) double;
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID; tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
label (1, 1) string = ""; label (1, 1) string = "";
end end
@@ -32,8 +32,8 @@ classdef rectangularPrismConstraint
obj.label = label; obj.label = label;
%% Define geometry bounds by LL corner and UR corner %% Define geometry bounds by LL corner and UR corner
obj.minCorner = bounds(:, 1); obj.minCorner = bounds(1, 1:3);
obj.maxCorner = bounds(:, 2); obj.maxCorner = bounds(2, 1:3);
% Compute L, W, H % Compute L, W, H
obj.dimensions = [obj.maxCorner(1) - obj.minCorner(1), obj.maxCorner(2) - obj.minCorner(2), obj.maxCorner(3) - obj.minCorner(3)]; obj.dimensions = [obj.maxCorner(1) - obj.minCorner(1), obj.maxCorner(2) - obj.minCorner(2), obj.maxCorner(3) - obj.minCorner(3)];
@@ -42,20 +42,20 @@ classdef rectangularPrismConstraint
obj.center = obj.minCorner + obj.dimensions ./ 2; obj.center = obj.minCorner + obj.dimensions ./ 2;
% Compute vertices % Compute vertices
obj.vertices = [obj.minCorner'; obj.vertices = [obj.minCorner;
obj.maxCorner(1), obj.minCorner(2:3)'; obj.maxCorner(1), obj.minCorner(2:3);
obj.maxCorner(1:2)', obj.minCorner(3); obj.maxCorner(1:2), obj.minCorner(3);
obj.minCorner(1), obj.maxCorner(2), obj.minCorner(3); obj.minCorner(1), obj.maxCorner(2), obj.minCorner(3);
obj.minCorner(1:2)', obj.maxCorner(3); obj.minCorner(1:2), obj.maxCorner(3);
obj.maxCorner(1), obj.minCorner(2), obj.maxCorner(3); obj.maxCorner(1), obj.minCorner(2), obj.maxCorner(3);
obj.minCorner(1), obj.maxCorner(2:3)' obj.minCorner(1), obj.maxCorner(2:3)
obj.maxCorner';]; obj.maxCorner;];
% Compute footprint % Compute footprint
obj.footprint = [obj.minCorner(1:2, 1), ... obj.footprint = [obj.minCorner(1:2); ...
[obj.minCorner(1, 1); obj.maxCorner(2, 1)], ... [obj.minCorner(1), obj.maxCorner(2)]; ...
[obj.maxCorner(1, 1); obj.minCorner(2, 1)], ... [obj.maxCorner(1), obj.minCorner(2)]; ...
obj.maxCorner(1:2, 1)]; obj.maxCorner(1:2)];
end end
function r = random(obj) function r = random(obj)
arguments (Input) arguments (Input)
@@ -72,9 +72,9 @@ classdef rectangularPrismConstraint
pos (:, 3) double; pos (:, 3) double;
end end
arguments (Output) arguments (Output)
c (1, 1) logical c (:, 1) logical
end end
c = all(pos >= repmat(obj.minCorner', size(pos, 1), 1), 2) & all(pos <= repmat(obj.maxCorner', size(pos, 1), 1), 2); c = all(pos >= repmat(obj.minCorner, size(pos, 2), 1), 2) & all(pos <= repmat(obj.maxCorner, size(pos, 2), 1), 2);
end end
function f = plotWireframe(obj, f) function f = plotWireframe(obj, f)
arguments (Input) arguments (Input)

10
sensingObjective.m
View File

@@ -16,7 +16,7 @@ classdef sensingObjective
arguments (Input) arguments (Input)
obj (1,1) {mustBeA(obj, 'sensingObjective')}; obj (1,1) {mustBeA(obj, 'sensingObjective')};
objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')}; objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
footprint (2, :) double; footprint (:, 2) double;
groundAlt (1, 1) double = 0; groundAlt (1, 1) double = 0;
discretizationStep (1, 1) double = 1; discretizationStep (1, 1) double = 1;
end end
@@ -27,10 +27,10 @@ classdef sensingObjective
obj.groundAlt = groundAlt; obj.groundAlt = groundAlt;
% Extract footprint limits % Extract footprint limits
xMin = min(footprint(1, :)); xMin = min(footprint(:, 1));
xMax = max(footprint(1, :)); xMax = max(footprint(:, 1));
yMin = min(footprint(2, :)); yMin = min(footprint(:, 2));
yMax = max(footprint(2, :)); yMax = max(footprint(:, 2));
xGrid = unique([xMin:discretizationStep:xMax, xMax]); xGrid = unique([xMin:discretizationStep:xMax, xMax]);
yGrid = unique([yMin:discretizationStep:yMax, yMax]); yGrid = unique([yMin:discretizationStep:yMax, yMax]);

49
test_miSim.m
View File

@@ -6,6 +6,7 @@ classdef test_miSim < matlab.unittest.TestCase
% Obstacles % Obstacles
constraintGeometries = cell(1, 0); constraintGeometries = cell(1, 0);
minObstacleDimension = 1;
% Objective % Objective
objective = sensingObjective; objective = sensingObjective;
@@ -32,14 +33,14 @@ classdef test_miSim < matlab.unittest.TestCase
function tc = setDomain(tc) function tc = setDomain(tc)
% random integer-sized domain within [-10, 10] in all dimensions % random integer-sized domain within [-10, 10] in all dimensions
L = ceil(5 + rand * 10 + rand * 10); L = ceil(5 + rand * 10 + rand * 10);
tc.domain = tc.domain.initialize(([0, L; 0, L; 0, L]), REGION_TYPE.DOMAIN, "Domain"); tc.domain = tc.domain.initialize([zeros(1, 3); L * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
end end
% Generate a random sensing objective within that domain % Generate a random sensing objective within that domain
function tc = setSensingObjective(tc) function tc = setSensingObjective(tc)
mu = tc.domain.random(); mu = tc.domain.random();
sig = [3, 1; 1, 4]; sig = [3, 1; 1, 4];
tc.objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu(1, 1:2), sig); tc.objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu(1, 1:2), sig);
tc.objective = tc.objective.initialize(tc.objectiveFunction, tc.domain.footprint, tc.domain.minCorner(3, 1), tc.objectiveDiscretizationStep); tc.objective = tc.objective.initialize(tc.objectiveFunction, tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep);
end end
% Instantiate agents, they will be initialized under different % Instantiate agents, they will be initialized under different
% parameters in individual test cases % parameters in individual test cases
@@ -64,38 +65,48 @@ classdef test_miSim < matlab.unittest.TestCase
% Randomly come up with constraint geometries until they % Randomly come up with constraint geometries until they
% fit within the domain % fit within the domain
candidateMinCorner = -Inf(3, 1); candidateMinCorner = [-Inf(1, 2), 0];
candidateMaxCorner = Inf(3, 1); candidateMaxCorner = Inf(1, 3);
% 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
obstructs = true; obstructs = true;
while obstructs while obstructs
% Make sure the obstacle is in the domain % Make sure the obstacle is in the domain
while any(candidateMinCorner(1:2, 1) < tc.domain.minCorner(1:2, 1)) while any(candidateMinCorner < tc.domain.minCorner)
candidateMinCorner = tc.domain.minCorner(1:3, 1) + [(tc.domain.maxCorner(1:2, 1) - tc.domain.minCorner(1:2, 1)) .* rand(2, 1); -Inf]; % random spots on the ground candidateMinCorner = tc.domain.minCorner(1:3) + [(tc.domain.maxCorner(1:2) - tc.domain.minCorner(1:2)) .* rand(1, 2), 0]; % random spots on the ground
end end
while any(candidateMaxCorner(1:3, 1) > tc.domain.maxCorner(1:3, 1)) while any(candidateMaxCorner > tc.domain.maxCorner)
candidateMaxCorner = [candidateMinCorner(1:2, 1); 0] + ((tc.domain.maxCorner(1:3, 1) - tc.domain.minCorner(1:3, 1)) .* rand(3, 1) ./ 2); % halved to keep from being excessively large candidateMaxCorner = [candidateMinCorner(1:2), 0] + ((tc.domain.maxCorner(1:3) - tc.domain.minCorner(1:3)) .* rand(1, 3) ./ 2); % halved to keep from being excessively large
end end
% once a domain-valid obstacle has been found, make % once a domain-valid obstacle has been found, make
% sure it doesn't obstruct the sensing target % sure it doesn't obstruct the sensing target
if all(candidateMinCorner(1:2, 1)' <= tc.objective.groundPos) && all(candidateMaxCorner(1:2, 1)' >= tc.objective.groundPos) if all(candidateMinCorner(1:2) <= tc.objective.groundPos) && all(candidateMaxCorner(1:2) >= tc.objective.groundPos)
% reset to try again % reset to try again
candidateMinCorner = -Inf(3, 1); candidateMinCorner = [-Inf(1, 2), 0];
candidateMaxCorner = Inf(3, 1); candidateMaxCorner = Inf(1, 3);
else else
obstructs = false; obstructs = false;
end end
end end
if min(candidateMaxCorner - candidateMinCorner) >= tc.minObstacleDimension
tooSmall = false;
else
candidateMinCorner = [-Inf(1, 2), 0];
candidateMaxCorner = Inf(1, 3);
end
end
% Reduce infinite dimensions to the domain % Reduce infinite dimensions to the domain
candidateMinCorner(isinf(candidateMinCorner)) = tc.domain.minCorner(isinf(candidateMinCorner)); candidateMinCorner(isinf(candidateMinCorner)) = tc.domain.minCorner(isinf(candidateMinCorner));
candidateMaxCorner(isinf(candidateMaxCorner)) = tc.domain.maxCorner(isinf(candidateMaxCorner)); candidateMaxCorner(isinf(candidateMaxCorner)) = tc.domain.maxCorner(isinf(candidateMaxCorner));
% Initialize constraint geometry % Initialize constraint geometry
tc.constraintGeometries{ii, 1} = tc.constraintGeometries{ii, 1}.initialize([candidateMinCorner, candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii)); tc.constraintGeometries{ii} = tc.constraintGeometries{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
end end
% Repeat this until a connected set of agent initial conditions % Repeat this until a connected set of agent initial conditions
@@ -106,10 +117,18 @@ classdef test_miSim < matlab.unittest.TestCase
for ii = 1:size(tc.agents, 1) for ii = 1:size(tc.agents, 1)
posInvalid = true; posInvalid = true;
while posInvalid while posInvalid
% Initialize the agent into a random spot in the domain % Initialize the agent into a random spot in the
% domain (that is not too close to the sensing
% objective)
boringInit = true;
while boringInit
candidatePos = tc.domain.random(); candidatePos = tc.domain.random();
if norm(candidatePos(1:2) - tc.objective.groundPos) >= norm(tc.domain.footprint(4, :) - tc.domain.footprint(1, :))/2
boringInit = false;
end
end
candidateGeometry = rectangularPrismConstraint; candidateGeometry = rectangularPrismConstraint;
tc.agents{ii, 1} = tc.agents{ii, 1}.initialize(candidatePos, zeros(1, 3), eye(3), candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii, 1) * ones(1, 3); candidatePos + tc.collisionRanges(ii, 1) * ones(1, 3)]', REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii)), ii, sprintf("Agent %d", ii)); 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 % Check obstacles to confirm that none are violated
for jj = 1:size(tc.constraintGeometries, 1) for jj = 1:size(tc.constraintGeometries, 1)
@@ -128,7 +147,7 @@ classdef test_miSim < matlab.unittest.TestCase
% Create a collision geometry for this agent % Create a collision geometry for this agent
candidateGeometry = rectangularPrismConstraint; candidateGeometry = rectangularPrismConstraint;
candidateGeometry = candidateGeometry.initialize([tc.agents{ii, 1}.pos - 0.1 * ones(1, 3); tc.agents{ii, 1}.pos + 0.1 * ones(1, 3)]', REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii)); 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 % Check previously placed agents for collisions
for jj = 1:(ii - 1) for jj = 1:(ii - 1)