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

10
sensingObjective.m
View File

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

71
test_miSim.m
View File

@@ -6,6 +6,7 @@ classdef test_miSim < matlab.unittest.TestCase
% Obstacles
constraintGeometries = cell(1, 0);
minObstacleDimension = 1;
% Objective
objective = sensingObjective;
@@ -32,14 +33,14 @@ classdef test_miSim < matlab.unittest.TestCase
function tc = setDomain(tc)
% random integer-sized domain within [-10, 10] in all dimensions
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
% 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);
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
% Instantiate agents, they will be initialized under different
% parameters in individual test cases
@@ -64,29 +65,39 @@ classdef test_miSim < matlab.unittest.TestCase
% Randomly come up with constraint geometries until they
% fit within the domain
candidateMinCorner = -Inf(3, 1);
candidateMaxCorner = Inf(3, 1);
candidateMinCorner = [-Inf(1, 2), 0];
candidateMaxCorner = Inf(1, 3);
% make sure the obstacles don't contain the sensing objective
obstructs = true;
while obstructs
% make sure obstacles are not too small in any dimension
tooSmall = true;
while tooSmall
% make sure the obstacles don't contain the sensing objective
obstructs = true;
while obstructs
% Make sure the obstacle is in the domain
while any(candidateMinCorner(1:2, 1) < tc.domain.minCorner(1:2, 1))
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
% Make sure the obstacle is in the domain
while any(candidateMinCorner < tc.domain.minCorner)
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
while any(candidateMaxCorner > tc.domain.maxCorner)
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
% once a domain-valid obstacle has been found, make
% sure it doesn't obstruct the sensing target
if all(candidateMinCorner(1:2) <= tc.objective.groundPos) && all(candidateMaxCorner(1:2) >= tc.objective.groundPos)
% reset to try again
candidateMinCorner = [-Inf(1, 2), 0];
candidateMaxCorner = Inf(1, 3);
else
obstructs = false;
end
end
while any(candidateMaxCorner(1:3, 1) > tc.domain.maxCorner(1:3, 1))
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
end
% once a domain-valid obstacle has been found, make
% 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)
% reset to try again
candidateMinCorner = -Inf(3, 1);
candidateMaxCorner = Inf(3, 1);
if min(candidateMaxCorner - candidateMinCorner) >= tc.minObstacleDimension
tooSmall = false;
else
obstructs = false;
candidateMinCorner = [-Inf(1, 2), 0];
candidateMaxCorner = Inf(1, 3);
end
end
@@ -95,7 +106,7 @@ classdef test_miSim < matlab.unittest.TestCase
candidateMaxCorner(isinf(candidateMaxCorner)) = tc.domain.maxCorner(isinf(candidateMaxCorner));
% 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
% 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)
posInvalid = true;
while posInvalid
% Initialize the agent into a random spot in the domain
candidatePos = tc.domain.random();
% 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();
if norm(candidatePos(1:2) - tc.objective.groundPos) >= norm(tc.domain.footprint(4, :) - tc.domain.footprint(1, :))/2
boringInit = false;
end
end
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
for jj = 1:size(tc.constraintGeometries, 1)
@@ -128,7 +147,7 @@ classdef test_miSim < matlab.unittest.TestCase
% Create a collision geometry for this agent
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
for jj = 1:(ii - 1)