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

fixed init generation being really slow

Browse Source
This commit is contained in:
Kevin D
2025-10-26 19:14:50 -07:00
parent b82c87520a
commit fdbd90afdf
29 changed files with 290 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files

282
test_miSim.m
View File

@@ -1,61 +1,72 @@
classdef test_miSim < matlab.unittest.TestCase
properties (Access = private)
testClass = miSim;
% Domain
domain = rectangularPrism;
domain = rectangularPrism; % domain geometry
% Obstacles
minNumObstacles = 1;
maxNumObstacles = 3;
minNumObstacles = 1; % Minimum number of obstacles to be randomly generated
maxNumObstacles = 3; % Maximum number of obstacles to be randomly generated
minObstacleSize = 1; % Minimum size of a randomly generated obstacle
maxObstacleSize = 6; % Maximum size of a randomly generated obstacle
obstacles = cell(1, 0);
minObstacleDimension = 1;
% Objective
objectiveDiscretizationStep = 0.01; % Step at which the objective function is solved in X and Y space
protectedRange = 1; % Minimum distance between the sensing objective and the edge of the domain
objective = sensingObjective;
objectiveFunction = @(x, y) 0;
objectiveDiscretizationStep = 0.01;
protectedRange = 1;
% Agents
minAgents = 3;
maxAgents = 9;
agents = cell(1, 0);
minAgents = 3; % Minimum number of agents to be randomly generated
maxAgents = 9; % Maximum number of agents to be randomly generated
agents = cell(0, 1);
% Collision
minCollisionRange = 0.1;
maxCollisionRange = 0.5;
minCollisionRange = 0.1; % Minimum randomly generated collision geometry size
maxCollisionRange = 0.5; % Maximum randomly generated collision geometry size
collisionRanges = NaN;
% Communications
comRange = 5;
comRange = 5; % Maximum range between agents that forms a communications link
end
% Setup for each test
methods (TestMethodSetup)
% Generate a random domain
function tc = setDomain(tc)
% random integer-sized domain ranging from [0, 5] to [0, 25] in all dimensions
% random integer-sized cube domain ranging from [0, 5 -> 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)
% Using a bivariate normal distribution
% Set peak position (mean)
mu = tc.domain.minCorner;
while tc.domain.interiorDistance(mu) < tc.protectedRange
mu = tc.domain.random();
end
mu(3) = 0;
assert(tc.domain.contains(mu));
% Set standard deviations of bivariate distribution
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);
% Define objective
tc.objective = tc.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], mu(1:2), sig), tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep);
end
% Instantiate agents, they will be initialized under different
% parameters in individual test cases
% Instantiate agents
function tc = setAgents(tc)
% Agents will be initialized under different parameters in
% individual test cases
% Instantiate a random number of agents according to parameters
for ii = 1:randi([tc.minAgents, tc.maxAgents])
tc.agents{ii, 1} = agent;
end
% Define random collision ranges for each agent
tc.collisionRanges = tc.minCollisionRange + rand(size(tc.agents, 1), 1) * (tc.maxCollisionRange - tc.minCollisionRange);
end
end
@@ -66,140 +77,137 @@ classdef test_miSim < matlab.unittest.TestCase
% randomly create 2-3 obstacles
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
tc.obstacles = cell(nGeom, 1);
% Iterate over obstacles to initialize
for ii = 1:size(tc.obstacles, 1)
% Instantiate a rectangular prism obstacle
tc.obstacles{ii, 1} = rectangularPrism;
% Randomly come up with dimensions until they
% fit within the domain
candidateMinCorner = [-Inf(1, 2), 0];
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 or encroach on it too much
obstructs = true;
while obstructs
badCandidate = true;
while badCandidate
% Instantiate a rectangular prism obstacle
tc.obstacles{ii} = rectangularPrism;
% 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
% Randomly generate min corner for the obstacle
candidateMinCorner = tc.domain.random();
candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
% 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
% Randomly select a corresponding maximum corner that
% satisfies min/max obstacle size specifications
candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize);
% Initialize obstacle
tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
% Make sure that the obstacles are fully contained by
% the domain
if ~domainContainsObstacle(tc.domain, tc.obstacles{ii})
continue;
end
if min(candidateMaxCorner - candidateMinCorner) >= tc.minObstacleDimension
tooSmall = false;
else
candidateMinCorner = [-Inf(1, 2), 0];
candidateMaxCorner = Inf(1, 3);
% Make sure that the obstacles don't cover the sensing
% objective
if obstacleCoversObjective(tc.objective, tc.obstacles{ii})
continue;
end
% Make sure that the obstacles aren't too close to the
% sensing objective
if obstacleCrowdsObjective(tc.objective, tc.obstacles{ii}, tc.protectedRange)
continue;
end
badCandidate = false;
end
% Reduce infinite dimensions to the domain
candidateMinCorner(isinf(candidateMinCorner)) = tc.domain.minCorner(isinf(candidateMinCorner));
candidateMaxCorner(isinf(candidateMaxCorner)) = tc.domain.maxCorner(isinf(candidateMaxCorner));
% 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
for ii = 1:size(tc.agents, 1)
posInvalid = true;
while posInvalid
% Initialize the agent into a random spot in the
% domain (that is not too close to the sensing
% objective)
boringInit = true;
while boringInit
% Add agents individually, ensuring that each addition does not
% invalidate the initialization setup
for ii = 1:size(tc.agents, 1)
initInvalid = true;
while initInvalid
candidatePos = [tc.objective.groundPos, 0];
% Generate a random position for the agent based on
% existing agent positions
if ii == 1
while agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2)
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
else
candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
end
% Make sure that the candidate position is within the
% domain
if ~tc.domain.contains(candidatePos)
continue;
end
% Make sure that the candidate position does not crowd
% the sensing objective and create boring scenarios
if agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2)
continue;
end
% Make sure that there exist unobstructed lines of sight at
% appropriate ranges to form a connected communications
% graph between the agents
connections = false(1, ii - 1);
for jj = 1:(ii - 1)
if norm(tc.agents{jj}.pos - candidatePos) <= tc.comRange
% Check new agent position against all existing
% agent positions for communications range
connections(jj) = true;
for kk = 1:size(tc.obstacles, 1)
if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
connections(jj) = false;
end
end
end
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.obstacles, 1)
inside = false;
if tc.obstacles{jj, 1}.contains(tc.agents{ii, 1}.pos)
% Found a violation, stop checking
inside = true;
end
% New agent must be connected to an existing agent to
% be valid
if ii ~= 1 && ~any(connections)
continue;
end
% Initialize candidate agent
candidateGeometry = rectangularPrism;
newAgent = 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));
% Make sure candidate agent doesn't collide with
% domain, obstacles, or any existing agents
violation = false;
for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
% Check if collision geometry exits domain
if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
violation = true;
break;
end
% Check if collision geometry enters obstacle
for kk = 1:size(tc.obstacles, 1)
if tc.obstacles{kk}.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
violation = true;
break;
end
end
% Agent is inside obstacle, try again
if inside
continue;
end
% Create a collision geometry for this agent
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
for jj = 1:(ii - 1)
% Check if previously defined agents collide with
% this one
colliding = false;
if candidateGeometry.contains(tc.agents{jj, 1}.pos)
% Found a violation, stop checking
colliding = true;
% Check if collision geometry enters other
% collision geometry
for kk = 1:(ii - 1)
if tc.agents{kk}.collisionGeometry.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
violation = true;
break;
end
end
% Agent is colliding with another, try again
if ii ~= 1 && colliding
continue;
end
% Allow to proceed since no obstacle/collision
% violations were found
posInvalid = false;
end
end
% Collect all agent positions
posArray = arrayfun(@(x) x{1}.pos, tc.agents, 'UniformOutput', false);
posArray = reshape([posArray{:}], size(tc.agents, 1), 3);
% Communications checks
adjacency = false(size(tc.agents, 1), size(tc.agents, 1));
for ii = 1:size(tc.agents, 1)
% Compute distance from each to all agents
for jj = 1:(size(tc.agents, 1))
if norm(posArray(ii, 1:3) - posArray(jj, 1:3)) <= tc.comRange
adjacency(ii, jj) = true;
end
if violation
continue;
end
% Candidate agent is valid, store to pass in to sim
initInvalid = false;
tc.agents{ii} = newAgent;
end
% Check connectivity
G = graph(adjacency);
connected = all(conncomp(G) == 1);
nIter = nIter + 1;
end
% Initialize the simulation
@@ -215,7 +223,7 @@ classdef test_miSim < matlab.unittest.TestCase
% Plot obstacles
for ii = 1:size(tc.testClass.obstacles, 1)
tc.testClass.obstacles{ii, 1}.plotWireframe(f);
tc.testClass.obstacles{ii}.plotWireframe(f);
end
% Plot objective gradient
@@ -223,8 +231,8 @@ classdef test_miSim < matlab.unittest.TestCase
% Plot agents and their collision geometries
for ii = 1:size(tc.testClass.agents, 1)
f = tc.testClass.agents{ii, 1}.plot(f);
f = tc.testClass.agents{ii, 1}.collisionGeometry.plotWireframe(f);
f = tc.testClass.agents{ii}.plot(f);
f = tc.testClass.agents{ii}.collisionGeometry.plotWireframe(f);
end
end
end