From 5debb2b5f4a745ee33732d8b0055f15b044f96a3 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Wed, 22 Oct 2025 18:17:39 -0700 Subject: [PATCH] Initializing domain, obstacles, objective, and agents --- REGION_TYPE.m | 18 +++ agent.m | 59 +++++++ firstPlotSetup.m | 8 + miSim.m | 64 ++++++++ rectangularPrismConstraint.m | 106 +++++++++++++ sensingObjective.m | 69 ++++++++ startup.m | 1 + test_miSim.m | 201 ++++++++++++++++++++++++ validators/mustBeAgents.m | 10 ++ validators/mustBeConstraintGeometries.m | 10 ++ validators/mustBeDcm.m | 12 ++ 11 files changed, 558 insertions(+) create mode 100644 REGION_TYPE.m create mode 100644 agent.m create mode 100644 firstPlotSetup.m create mode 100644 miSim.m create mode 100644 rectangularPrismConstraint.m create mode 100644 sensingObjective.m create mode 100644 startup.m create mode 100644 test_miSim.m create mode 100644 validators/mustBeAgents.m create mode 100644 validators/mustBeConstraintGeometries.m create mode 100644 validators/mustBeDcm.m diff --git a/REGION_TYPE.m b/REGION_TYPE.m new file mode 100644 index 0000000..aa81a07 --- /dev/null +++ b/REGION_TYPE.m @@ -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 \ No newline at end of file diff --git a/agent.m b/agent.m new file mode 100644 index 0000000..1cb27b7 --- /dev/null +++ b/agent.m @@ -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 \ No newline at end of file diff --git a/firstPlotSetup.m b/firstPlotSetup.m new file mode 100644 index 0000000..d97b1f2 --- /dev/null +++ b/firstPlotSetup.m @@ -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 \ No newline at end of file diff --git a/miSim.m b/miSim.m new file mode 100644 index 0000000..9edb5c5 --- /dev/null +++ b/miSim.m @@ -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 \ No newline at end of file diff --git a/rectangularPrismConstraint.m b/rectangularPrismConstraint.m new file mode 100644 index 0000000..ca00c26 --- /dev/null +++ b/rectangularPrismConstraint.m @@ -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 \ No newline at end of file diff --git a/sensingObjective.m b/sensingObjective.m new file mode 100644 index 0000000..a00e7d7 --- /dev/null +++ b/sensingObjective.m @@ -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 \ No newline at end of file diff --git a/startup.m b/startup.m new file mode 100644 index 0000000..12d0f9e --- /dev/null +++ b/startup.m @@ -0,0 +1 @@ +addpath("validators"); \ No newline at end of file diff --git a/test_miSim.m b/test_miSim.m new file mode 100644 index 0000000..9edb27d --- /dev/null +++ b/test_miSim.m @@ -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 \ No newline at end of file diff --git a/validators/mustBeAgents.m b/validators/mustBeAgents.m new file mode 100644 index 0000000..a2b906f --- /dev/null +++ b/validators/mustBeAgents.m @@ -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 \ No newline at end of file diff --git a/validators/mustBeConstraintGeometries.m b/validators/mustBeConstraintGeometries.m new file mode 100644 index 0000000..d16301a --- /dev/null +++ b/validators/mustBeConstraintGeometries.m @@ -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 \ No newline at end of file diff --git a/validators/mustBeDcm.m b/validators/mustBeDcm.m new file mode 100644 index 0000000..747d0d6 --- /dev/null +++ b/validators/mustBeDcm.m @@ -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 \ No newline at end of file