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

Initializing domain, obstacles, objective, and agents

Browse Source
This commit is contained in:
Kevin D
2025-10-22 18:17:39 -07:00
parent 9743f1dac8
commit 5debb2b5f4
11 changed files with 558 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
REGION_TYPE.m Normal file
View File

@@ -0,0 +1,18 @@
classdef REGION_TYPE
properties
id
color
end
enumeration
INVALID (0, [255, 127, 255]); % default value
DOMAIN (1, [0, 0, 0]); % domain region
OBSTACLE (2, [255, 127, 127]); % obstacle region
COLLISION (3, [255, 255, 128]); % collision avoidance region
end
methods
function obj = REGION_TYPE(id, color)
obj.id = id;
obj.color = color./ 255;
end
end
end

59
agent.m Normal file
View File

@@ -0,0 +1,59 @@
classdef agent
properties (SetAccess = private, GetAccess = public)
% Identifiers
index = NaN;
label = "";
% Sensor
sensingFunction = @(r) 0.5; % probability of detection as a function of range
% State
pos = NaN(1, 3);
vel = NaN(1, 3);
cBfromC = NaN(3); % DCM body from sim cartesian (assume fixed for now)
% Collision
collisionGeometry;
end
methods (Access = public)
function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, index, label)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'agent')};
pos (1, 3) double;
vel (1, 3) double;
cBfromC (3, 3) double {mustBeDcm};
collisionGeometry (1, 1) {mustBeConstraintGeometries};
index (1, 1) double = NaN;
label (1, 1) string = "";
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'agent')};
end
obj.pos = pos;
obj.vel = vel;
obj.cBfromC = cBfromC;
obj.collisionGeometry = collisionGeometry;
obj.index = index;
obj.label = label;
end
function f = plot(obj, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'agent')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
end
% Create axes if they don't already exist
f = firstPlotSetup(f);
% Plot points representing the agent position
hold(f.CurrentAxes, "on");
scatter3(obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 50);
hold(f.CurrentAxes, "off");
end
end
end

8
firstPlotSetup.m Normal file
View File

@@ -0,0 +1,8 @@
function f = firstPlotSetup(f)
if isempty(f.CurrentAxes)
axes(f);
axis(f.CurrentAxes, "equal");
grid(f.CurrentAxes, "on");
view(f.CurrentAxes, 3);
end
end

64
miSim.m Normal file
View File

@@ -0,0 +1,64 @@
classdef miSim
% multiagent interconnection simulation
% Simulation parameters
properties (SetAccess = private, GetAccess = public)
domain = rectangularPrismConstraint;
objective = sensingObjective;
constraintGeometries = cell(0, 1); % geometries that define constraints 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)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
domain (1, 1) {mustBeConstraintGeometries};
objective (1, 1) {mustBeA(objective, 'sensingObjective')};
agents (:, 1) cell {mustBeAgents};
constraintGeometries (:, 1) cell {mustBeConstraintGeometries} = cell(0, 1);
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
%% Define domain
obj.domain = domain;
%% Add constraint geometries against the domain
obj.constraintGeometries = constraintGeometries;
%% Define objective
obj.objective = objective;
%% Define agents
obj.agents = agents;
end
end
methods (Access = private)
function validateInitialization(obj)
% Assert obstacles do not intersect with the domain
% Assert obstacles do not intersect with each other
% Assert the objective has only one maxima within the domain
% Assert the objective's sole maximum is not inaccessible due
% to the placement of an obstacle
end
function validateLoop(obj)
% Assert that agents are safely inside the domain
% Assert that agents are not in proximity to obstacles
% Assert that agents are not in proximity to each other
% Assert that agents form a connected graph
end
end
end

106
rectangularPrismConstraint.m Normal file
View File

@@ -0,0 +1,106 @@
classdef rectangularPrismConstraint
% Rectangular prism constraint geometry
properties (SetAccess = private, GetAccess = public)
tag = REGION_TYPE.INVALID;
label = "";
minCorner = NaN(3, 1);
maxCorner = NaN(3, 1);
dimensions = NaN(3, 1);
center = NaN;
vertices = NaN(8, 3);
footprint = NaN(2, 4);
end
methods (Access = public)
function obj = initialize(obj, bounds, tag, label)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
bounds (3, 2) double;
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
label (1, 1) string = "";
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
end
obj.tag = tag;
obj.label = label;
%% Define geometry bounds by LL corner and UR corner
obj.minCorner = bounds(:, 1);
obj.maxCorner = bounds(:, 2);
% Compute L, W, H
obj.dimensions = [obj.maxCorner(1) - obj.minCorner(1), obj.maxCorner(2) - obj.minCorner(2), obj.maxCorner(3) - obj.minCorner(3)];
% Compute center
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.minCorner(1), obj.maxCorner(2), obj.minCorner(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';];
% 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)];
end
function r = random(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
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 c = contains(obj, pos)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
pos (:, 3) double;
end
arguments (Output)
c (1, 1) logical
end
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')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
end
% Create axes if they don't already exist
f = firstPlotSetup(f);
edges = [1 2; 2 3; 3 4; 4 1; % bottom square
5 6; 6 8; 8 7; 7 5; % top square
1 5; 2 6; 3 8; 4 7]; % vertical edges
% Create plotting inputs from vertices and edges
X = [obj.vertices(edges(:,1),1), obj.vertices(edges(:,2),1)]';
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
hold(f.CurrentAxes, "on");
plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
hold(f.CurrentAxes, "off");
end
end
end

69
sensingObjective.m Normal file
View File

@@ -0,0 +1,69 @@
classdef sensingObjective
% Sensing objective definition parent class
properties (SetAccess = private, GetAccess = public)
label = "";
groundAlt = 0;
groundPos = [0, 0];
discretizationStep = 1;
objectiveFunction = @(x, y) 0; % define objective functions over a grid in this manner
X = [];
Y = [];
values = [];
end
methods (Access = public)
function obj = initialize(obj, objectiveFunction, footprint, groundAlt, discretizationStep)
arguments (Input)
obj (1,1) {mustBeA(obj, 'sensingObjective')};
objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
footprint (2, :) double;
groundAlt (1, 1) double = 0;
discretizationStep (1, 1) double = 1;
end
arguments (Output)
obj (1,1) {mustBeA(obj, 'sensingObjective')};
end
obj.groundAlt = groundAlt;
% Extract footprint limits
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]);
% Store grid points for plotting later
[obj.X, obj.Y] = meshgrid(xGrid, yGrid);
% Evaluate function over grid points
obj.objectiveFunction = objectiveFunction;
obj.values = reshape(obj.objectiveFunction(obj.X, obj.Y), size(obj.X));
% store ground position
idx = obj.values == max(obj.values, [], "all");
obj.groundPos = [obj.X(idx), obj.Y(idx)];
end
function f = plot(obj, f)
arguments (Input)
obj (1,1) {mustBeA(obj, 'sensingObjective')};
f (1,1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
f (1,1) {mustBeA(f, 'matlab.ui.Figure')};
end
% Create axes if they don't already exist
f = firstPlotSetup(f);
% Plot gradient on the "floor" of the domain
hold(f.CurrentAxes, "on");
s = surf(obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
s.HitTest = 'off';
s.PickableParts = 'none';
hold(f.CurrentAxes, "off");
end
end
end

1
startup.m Normal file
View File

@@ -0,0 +1 @@
addpath("validators");

201
test_miSim.m Normal file
View File

@@ -0,0 +1,201 @@
classdef test_miSim < matlab.unittest.TestCase
properties (Access = private)
testClass = miSim;
% Domain
domain = rectangularPrismConstraint;
% Obstacles
constraintGeometries = cell(1, 0);
% Objective
objective = sensingObjective;
objectiveFunction = @(x, y) 0;
objectiveDiscretizationStep = 0.01;
% Agents
minAgents = 3;
maxAgents = 9;
agents = cell(1, 0);
% Collision
minCollisionRange = 0.1;
maxCollisionRange = 0.5;
collisionRanges = NaN;
% Communications
comRange = 5;
end
% Setup for each test
methods (TestMethodSetup)
% Generate a random domain
function tc = setDomain(tc)
% random integer-sized domain within [-10, 10] in all dimensions
tc.domain = tc.domain.initialize(ceil([rand * -10, rand * 10; rand * -10, rand * 10; rand * -10, rand * 10]), 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);
end
% Instantiate agents, they will be initialized under different
% parameters in individual test cases
function tc = setAgents(tc)
for ii = 1:randi([tc.minAgents, tc.maxAgents])
tc.agents{ii, 1} = agent;
end
tc.collisionRanges = tc.minCollisionRange + rand(size(tc.agents, 1), 1) * (tc.maxCollisionRange - tc.minCollisionRange);
end
end
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 come up with constraint geometries until they
% fit within the domain
candidateMinCorner = -Inf(3, 1);
candidateMaxCorner = Inf(3, 1);
% 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
end
while any(candidateMaxCorner(1:2, 1) > tc.domain.maxCorner(1:2, 1))
candidateMaxCorner = [candidateMinCorner(1:2, 1); 0] + [(tc.domain.maxCorner(1:2, 1) - tc.domain.minCorner(1:2, 1)) .* rand(2, 1) ./ 2; Inf]; % 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);
else
obstructs = false;
end
end
% Reduce infinite dimensions to the domain
candidateMinCorner(isinf(candidateMinCorner)) = tc.domain.minCorner(isinf(candidateMinCorner));
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));
end
% Repeat this until a connected set of agent initial conditions
% is found by random chance
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
candidatePos = tc.domain.random();
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));
% Check obstacles to confirm that none are violated
for jj = 1:size(tc.constraintGeometries, 1)
inside = false;
if tc.constraintGeometries{jj, 1}.contains(tc.agents{ii, 1}.pos)
% Found a violation, stop checking
inside = true;
break;
end
end
% Agent is inside obstacle, try again
if inside
continue;
end
% 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));
% 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;
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
end
end
% Check connectivity
G = graph(adjacency);
connected = all(conncomp(G) == 1);
end
% Initialize the simulation
tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.constraintGeometries);
% Plot domain
f = tc.testClass.domain.plotWireframe;
% Set plotting limits to focus on the domain
xlim([tc.testClass.domain.minCorner(1) - 0.5, tc.testClass.domain.maxCorner(1) + 0.5]);
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);
end
% Plot objective gradient
f = tc.testClass.objective.plot(f);
% 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);
end
end
end
end

10
validators/mustBeAgents.m Normal file
View File

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

10
validators/mustBeConstraintGeometries.m Normal file
View File

@@ -0,0 +1,10 @@
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

12
validators/mustBeDcm.m Normal file
View File

@@ -0,0 +1,12 @@
function mustBeDcm(dcm)
% Assert 2D
assert(numel(size(dcm)) == 2, "DCM is not 2D");
% Assert square
assert(size(unique(size(dcm)), 1) == 1, "DCM is not a square matrix");
epsilon = 1e-9;
% Assert inverse equivalent to transpose
assert(all(abs(inv(dcm) - dcm') < epsilon, "all"), "DCM inverse is not equivalent to transpose");
% Assert determinant is 1
assert(det(dcm) > 1 - epsilon && det(dcm) < 1 + epsilon, "DCM has determinant not equal to 1");
end