From 2a0e2e500f3032074cdf5f659e16e1c159b31084 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Sun, 15 Mar 2026 13:42:35 -0700 Subject: [PATCH] results plot1 WIP --- @agent/initialize.m | 1 + @agent/run.m | 9 +- @miSim/constrainMotion.m | 26 +- @miSim/miSim.m | 2 - @miSim/teardown.m | 1 - @miSim/validate.m | 9 +- @miSim/writeInits.m | 7 +- @sensingObjective/initialize.m | 15 +- @sensingObjective/initializeRandomMvnpdf.m | 2 +- @sensingObjective/sensingObjective.m | 3 +- aerpaw/config/scenario.csv | 2 +- plot1.m | 81 +++++ .../C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml | 2 + .../C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml | 2 + .../o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml | 2 + .../o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml | 2 + test/parametricTestSuite.m | 2 +- test/results.m | 298 ++++++++++++++++++ util/objectiveFunctionWrapper.m | 6 +- 19 files changed, 437 insertions(+), 35 deletions(-) create mode 100644 plot1.m create mode 100644 resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml create mode 100644 resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml create mode 100644 resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml create mode 100644 resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml create mode 100644 test/results.m diff --git a/@agent/initialize.m b/@agent/initialize.m index 84b2f8d..a43bd94 100644 --- a/@agent/initialize.m +++ b/@agent/initialize.m @@ -15,6 +15,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma end obj.pos = pos; + obj.lastPos = pos; obj.vel = zeros(1, 3); obj.lastVel = zeros(1, 3); obj.collisionGeometry = collisionGeometry; diff --git a/@agent/run.m b/@agent/run.m index 9f818be..340b4cf 100644 --- a/@agent/run.m +++ b/@agent/run.m @@ -14,6 +14,13 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD obj (1, 1) {mustBeA(obj, "agent")}; end + % Always update lastPos/lastVel so constrainMotion evaluates barriers at + % the correct (most recent) position, even when this agent has no partition. + obj.lastPos = obj.pos; + if useDoubleIntegrator + obj.lastVel = obj.vel; + end + % Collect objective function values across partition partitionMask = partitioning == index; if ~any(partitionMask(:)) @@ -79,10 +86,8 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD gradNorm = norm(gradC); % Compute unconstrained next state - obj.lastPos = obj.pos; if useDoubleIntegrator % Double-integrator: gradient produces desired acceleration with damping - obj.lastVel = obj.vel; if gradNorm < 1e-100 a_gradient = zeros(1, 3); else diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 8992d12..c3e5b1c 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -39,10 +39,10 @@ function [obj] = constrainMotion(obj) h(logical(eye(nAgents))) = 0; % self value is 0 for ii = 1:(nAgents - 1) for jj = (ii + 1):nAgents - h(ii, jj) = norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2; + h(ii, jj) = norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2; h(jj, ii) = h(ii, jj); - A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos); + A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos); A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii)); % Slack derived from existing params: recovery velocity = max gradient approach velocity. % Correction splits between 2 agents, so |A| = 2*r_sum @@ -69,11 +69,11 @@ function [obj] = constrainMotion(obj) for ii = 1:nAgents for jj = 1:size(obj.obstacles, 1) % find closest position to agent on/in obstacle - cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.pos); + cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.lastPos); - hObs(ii, jj) = dot(obj.agents{ii}.pos - cPos, obj.agents{ii}.pos - cPos) - obj.agents{ii}.collisionGeometry.radius^2; + hObs(ii, jj) = dot(obj.agents{ii}.lastPos - cPos, obj.agents{ii}.lastPos - cPos) - obj.agents{ii}.collisionGeometry.radius^2; - A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - cPos); + A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.lastPos - cPos); % Floor for single-agent constraint: full correction on one agent, |A| = 2*r_i r_i = obj.agents{ii}.collisionGeometry.radius; v_max_i = obj.agents{ii}.initialStepSize / obj.timestep; @@ -93,37 +93,37 @@ function [obj] = constrainMotion(obj) h_xMin = 0.0; h_xMax = 0.0; h_yMin = 0.0; h_yMax = 0.0; h_zMin = 0.0; h_zMax = 0.0; for ii = 1:nAgents % X minimum - h_xMin = (obj.agents{ii}.pos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius; + h_xMin = (obj.agents{ii}.lastPos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0]; b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent; kk = kk + 1; % X maximum - h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.pos(1)) - obj.agents{ii}.collisionGeometry.radius; + h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.lastPos(1)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0]; b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent; kk = kk + 1; % Y minimum - h_yMin = (obj.agents{ii}.pos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius; + h_yMin = (obj.agents{ii}.lastPos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0]; b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent; kk = kk + 1; % Y maximum - h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.pos(2)) - obj.agents{ii}.collisionGeometry.radius; + h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.lastPos(2)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0]; b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent; kk = kk + 1; % Z minimum — enforce z >= minAlt + radius (not just z >= domain floor + radius) - h_zMin = (obj.agents{ii}.pos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius; + h_zMin = (obj.agents{ii}.lastPos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1]; b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent; kk = kk + 1; % Z maximum - h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.pos(3)) - obj.agents{ii}.collisionGeometry.radius; + h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.lastPos(3)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1]; b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent; kk = kk + 1; @@ -145,9 +145,9 @@ function [obj] = constrainMotion(obj) if obj.constraintAdjacencyMatrix(ii, jj) paddingFactor = 0.9; % Barrier at 90% of actual range; real comms still work beyond this r_comms = paddingFactor * min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius]); - hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2; + hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2; - A(kk, (3 * ii - 2):(3 * ii)) = 2 * (obj.agents{ii}.pos - obj.agents{jj}.pos); + A(kk, (3 * ii - 2):(3 * ii)) = 2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos); A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii)); % One-step forward invariance: b = h/dt ensures h cannot diff --git a/@miSim/miSim.m b/@miSim/miSim.m index 3432e90..f9f61e5 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -7,7 +7,6 @@ classdef miSim timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays) maxIter = NaN; % maximum number of simulation iterations domain; - objective; obstacles; % geometries that define obstacles within the domain agents; % agents that move within the domain adjacency = false(0, 0); % Adjacency matrix representing communications network graph @@ -67,7 +66,6 @@ classdef miSim obj (1, 1) miSim end obj.domain = rectangularPrism; - obj.objective = sensingObjective; obj.obstacles = {rectangularPrism}; obj.agents = {agent}; end diff --git a/@miSim/teardown.m b/@miSim/teardown.m index f4c26d0..d8b614b 100644 --- a/@miSim/teardown.m +++ b/@miSim/teardown.m @@ -39,7 +39,6 @@ function obj = teardown(obj) obj.timestepIndex = NaN; obj.maxIter = NaN; obj.domain = rectangularPrism; - obj.objective = sensingObjective; obj.obstacles = cell(0, 1); obj.agents = cell(0, 1); obj.adjacency = NaN; diff --git a/@miSim/validate.m b/@miSim/validate.m index 818a6c0..f57249e 100644 --- a/@miSim/validate.m +++ b/@miSim/validate.m @@ -7,11 +7,11 @@ function validate(obj) %% Communications Network Validators if max(conncomp(graph(obj.adjacency))) ~= 1 - warning("Network is not connected"); + error("Network is not connected"); end if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all") - warning("Eliminated network connections that were necessary"); + error("Eliminated network connections that were necessary"); end %% Obstacle Validators @@ -20,10 +20,9 @@ function validate(obj) for kk = 1:size(obj.agents, 1) P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner); d = obj.agents{kk}.pos - P; - if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2 - warning("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, dot(d, d) - obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail + if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2 - 1e-3 + error("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, - dot(d, d) + obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail end end end - end diff --git a/@miSim/writeInits.m b/@miSim/writeInits.m index 4f02b03..6c28f9c 100644 --- a/@miSim/writeInits.m +++ b/@miSim/writeInits.m @@ -14,6 +14,9 @@ function writeInits(obj) comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents); initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents); pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)); + obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, obj.obstacles, 'UniformOutput', false)); + obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, obj.obstacles, 'UniformOutput', false)); + % Combine with simulation parameters inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ... @@ -24,7 +27,9 @@ function writeInits(obj) "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ... "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ... ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv - "pos", pos); % still needs obstacle states and objective state + "pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ... + "obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ... + "objectiveIntegral", sum(obj.domain.objective.values(:))); % Save all parameters to output file initsFile = strcat(obj.artifactName, "_miSimInits"); diff --git a/@sensingObjective/initialize.m b/@sensingObjective/initialize.m index 8096ba2..7d10cae 100644 --- a/@sensingObjective/initialize.m +++ b/@sensingObjective/initialize.m @@ -1,4 +1,4 @@ -function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum) +function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum, objectiveMu, objectiveSigma) arguments (Input) obj (1,1) {mustBeA(obj, "sensingObjective")}; objectiveFunction (1, 1) {mustBeA(objectiveFunction, "function_handle")}; @@ -6,6 +6,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr discretizationStep (1, 1) double = 1; protectedRange (1, 1) double = 1; sensorPerformanceMinimum (1, 1) double = 1e-6; + objectiveMu (:, 2) double = NaN(1, 2); + objectiveSigma (:, 2, 2) double = NaN(1, 2, 2); end arguments (Output) obj (1,1) {mustBeA(obj, "sensingObjective")}; @@ -37,8 +39,13 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr % store ground position idx = obj.values == 1; - obj.groundPos = [obj.X(idx), obj.Y(idx)]; - obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow) + if any(isnan(objectiveMu)) + obj.groundPos = [obj.X(idx), obj.Y(idx)]; + obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow) + else + obj.groundPos = objectiveMu; + end + obj.objectiveSigma = objectiveSigma; - assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective") + assert(domain.distance([obj.groundPos, ones(size(obj.groundPos, 1), 1) .* domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective") end \ No newline at end of file diff --git a/@sensingObjective/initializeRandomMvnpdf.m b/@sensingObjective/initializeRandomMvnpdf.m index 6f9b8b7..06494ae 100644 --- a/@sensingObjective/initializeRandomMvnpdf.m +++ b/@sensingObjective/initializeRandomMvnpdf.m @@ -11,7 +11,7 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected % Set random objective position mu = domain.minCorner; - while domain.distance(mu) < protectedRange + while domain.distance(mu) < protectedRange * 1.01 mu = domain.random(); end diff --git a/@sensingObjective/sensingObjective.m b/@sensingObjective/sensingObjective.m index 46b41a8..0444a17 100644 --- a/@sensingObjective/sensingObjective.m +++ b/@sensingObjective/sensingObjective.m @@ -2,7 +2,8 @@ classdef sensingObjective % Sensing objective definition parent class properties (SetAccess = private, GetAccess = public) label = ""; - groundPos = [NaN, NaN]; + groundPos = NaN(1, 2); + objectiveSigma = NaN(1, 2, 2); discretizationStep = NaN; X = []; Y = []; diff --git a/aerpaw/config/scenario.csv b/aerpaw/config/scenario.csv index a79f914..05c1468 100644 --- a/aerpaw/config/scenario.csv +++ b/aerpaw/config/scenario.csv @@ -1,2 +1,2 @@ timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology -5, 100, 30.0, 0.1, 2.0, 2.0, 100, 3, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1 \ No newline at end of file +1, 150, 30.0, 0.1, 2.0, 1, 1, 1, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1 \ No newline at end of file diff --git a/plot1.m b/plot1.m new file mode 100644 index 0000000..76458cc --- /dev/null +++ b/plot1.m @@ -0,0 +1,81 @@ +clear; +% Load data +dataPath = fullfile('.', 'sandbox', 'plot1'); +simHists = dir(dataPath); simHists = simHists(3:end); +simInits = simHists(endsWith({simHists.name}, 'miSimInits.mat')); +simHists = simHists(endsWith({simHists.name}, 'miSimHist.mat')); +assert(length(simHists) == length(simInits), "input data availability mismatch"); + +% Initialize plotting data +Cfinal = NaN(12, 1); +n = NaN(12, 1); +doubleIntegrator = NaN(12, 1); +numObjective = NaN(12, 1); + +% Aggregate relevant data +for ii = 1:length(simHists) + initName = strrep(simInits(ii).name, "_miSimInits.mat", ""); + histName = strrep(simHists(ii).name, "_miSimHist.mat", ""); + assert(initName == histName); + + init = load(fullfile(simInits(ii).folder, simInits(ii).name)); + hist = load(fullfile(simHists(ii).folder, simHists(ii).name)); + + % Stash relevant data + Cfinal(ii) = hist.out.perf(end) / init.objectiveIntegral; + n(ii) = init.numAgents; + doubleIntegrator(ii) = init.useDoubleIntegrator; + numObjective(ii) = size(init.objectivePos, 1); + for jj = 1:length(hist.out.agent) + alphaDist(jj, ii) = hist.out.agent(jj).sensor.alphaDist; + end +end + +sensors = unique(alphaDist(1, :)); + +config = []; +for ii = 1:length(simHists) + % number of agents + s = num2str(n(ii)); + + % number of objectives + if numObjective(ii) == 1 + s = strcat(s, "_A"); + elseif numObjective(ii) == 2 + s = strcat(s, "_B"); + end + + % sensor pararmeter set + if alphaDist(1, ii) == sensors(1) + s = strcat(s, "_I"); + elseif alphaDist(1, ii) == sensors(2) + s = strcat(s, "_II"); + end + + % agent dynamics + if ~doubleIntegrator(ii) + s = strcat(s, '_alpha'); + elseif doubleIntegrator(ii) + s = strcat(s, '_beta'); + end + config = [config; s]; +end + +close all; +f = figure; +x = axes; grid(x, "on"); + +n_unique = sort(unique(n)); +C = []; +for ii = 1:length(n_unique) + nIdx = n == n_unique(ii); + C = [C; [Cfinal(nIdx)]']; +end +bar(C); +xlabel("Number of agents"); +ylabel("Final coverage (fraction of maximum)"); +title("Final performance of parameterizations"); +legend(["$AI\alpha$"; "$AI\beta$"; "$AII\alpha$"; "$BI\beta$"], "Interpreter", "latex"); +grid("on"); +keyboard + diff --git a/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml b/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml b/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml new file mode 100644 index 0000000..01cb34e --- /dev/null +++ b/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml new file mode 100644 index 0000000..cea1794 --- /dev/null +++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m index d714c0a..aa1078d 100644 --- a/test/parametricTestSuite.m +++ b/test/parametricTestSuite.m @@ -150,7 +150,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase end % randomly shuffle agents to make the network more interesting (probably) - agents = agents(randperm(numel(agents))); + agents = agents(randperm(numel(agents))); % Set up obstacles obstacles = cell(params.numObstacles(ii), 1); diff --git a/test/results.m b/test/results.m new file mode 100644 index 0000000..5e7a7dd --- /dev/null +++ b/test/results.m @@ -0,0 +1,298 @@ +classdef results < matlab.unittest.TestCase + properties (Constant, Access = private) + seed = 1; + end + + properties (Access = private) + % System under test + testClass = miSim; + + %% Diagnostic Parameters + % No effect on simulation dynamics + makeVideo = false; % disable video writing for big performance increase + makePlots = true; % disable plotting for big performance increase (also disables video) + plotCommsGeometry = false; % disable plotting communications geometries + + %% Fixed Test Parameters + useFixedTopology = true; % No lesser neighbor, fixed network instead + minDimension = 50; % minimum domain size + maxDimension = 100; % maximum domain size + discretizationStep = 0.1; + protectedRange = 5; + collisionRadius = 5; + sensorPerformanceMinimum = 0.005; + comRange = 20; + maxIter = 200; + initialStepSize = 1; + numObstacles = 3; + barrierGain = 1; + barrierExponent = 1; + timestep = 0.5; + dampingCoeff = 2; + end + + properties (TestParameter) + %% Test Iterations + % Specific parameter combos to run iterations on + n = struct('n3', 3, 'n5', 5, 'n6', 6); % number of agents + config = results.makeConfigs(); + end + + methods (TestClassSetup) + function setSeed(tc) + rng(tc.seed); + end + end + + methods (TestMethodSetup) + % % Generate a random domain for each test + % function tc = setDomain(tc) + % tc.testClass.domain = rectangularPrism; + % % random integer-dimensioned cubic domain + % tc.testClass.domain = tc.testClass.domain.initializeRandom(REGION_TYPE.DOMAIN, "Domain", tc.minDimension); + % % Random bivariate normal PDF objective + % tc.testClass.domain.objective = tc.testClass.domain.objective.initializeRandomMvnpdf(tc.testClass.domain, tc.discretizationStep, tc.protectedRange); + % end + end + + methods (Static, Access = private) + function c = makeConfigs() + rng(results.seed); + abMin = 6; % alpha*beta >= 6 ensures membership(0) = tanh(3) >= 0.995 + alphaDist = rand(1, 2) .* [100, 100]; + betaDist = abMin ./ alphaDist + rand(1, 2) .* (20 - abMin ./ alphaDist); + alphaTilt = 10 + rand(1, 2) .* [20, 20]; + betaTilt = abMin ./ alphaTilt + rand(1, 2) .* (50 - abMin ./ alphaTilt); + sensors = struct('alphaDist', num2cell(alphaDist), 'alphaTilt', num2cell(alphaTilt), 'betaDist', num2cell(betaDist), 'betaTilt', num2cell(betaTilt)); + sensor1 = sigmoidSensor; + sensor2 = sigmoidSensor; + sensor1 = sensor1.initialize(sensors(1).alphaDist, sensors(1).betaDist, sensors(1).alphaTilt, sensors(1).betaTilt); + sensor2 = sensor2.initialize(sensors(2).alphaDist, sensors(2).betaDist, sensors(2).alphaTilt, sensors(2).betaTilt); + sensor1.plotParameters; + sensor2.plotParameters; + c = struct('A_1_alpha', struct('numDist', 1, 'sensor', sensors(1), 'doubleIntegrator', false), ... + 'A_1_beta', struct('numDist', 1, 'sensor', sensors(1), 'doubleIntegrator', true), ... + 'A_2_alpha', struct('numDist', 1, 'sensor', sensors(2), 'doubleIntegrator', false), ... + 'B_1_beta', struct('numDist', 2, 'sensor', sensors(1), 'doubleIntegrator', true)); + end + end + + methods (Test) + function plot1_runs(tc, n, config) + % OVERRIDES + % function plot1_runs(tc) + % n = 3; + % config = struct('numDist', 1, 'sensor', struct('alphaDist', 100, 'alphaTilt', 2, 'betaDist', 10, 'betaTilt', 0.5), 'doubleIntegrator', false); + + % Set up random cube domain + minAlt = tc.minDimension(1) * rand() * 0.5; + tc.testClass.domain = tc.testClass.domain.initializeRandom(REGION_TYPE.DOMAIN, "Domain", tc.minDimension, tc.maxDimension, tc.testClass.domain, minAlt); + % Place sensing objective(s) + objectiveMu = []; + objectiveSigma = []; + for ii = 1:config.numDist + mu = tc.testClass.domain.minCorner; + while tc.testClass.domain.distance(mu) < tc.protectedRange * 1.01 + mu = tc.testClass.domain.random(); + end + notPosDef = true; + while notPosDef + sig = reshape(sort(rand(1, 4) * min(tc.testClass.domain.dimensions(1:2))), [1, 2, 2]); + sig(1, 2, 1) = sig(1, 1, 2); + [~, notPosDef] = chol(squeeze(sig)); + end + objectiveMu = [objectiveMu; mu(1:2)]; + objectiveSigma = cat(1, objectiveSigma, sig); + end + tc.testClass.domain.objective = tc.testClass.domain.objective.initialize(objectiveFunctionWrapper(objectiveMu, objectiveSigma), tc.testClass.domain, tc.discretizationStep, tc.protectedRange, tc.sensorPerformanceMinimum, objectiveMu, objectiveSigma); + + % Initialize agents + agents = cell(n, 1); + [agents{:}] = deal(agent); + + % Initialize sensor model + sensorModel = sigmoidSensor; + sensorModel = sensorModel.initialize(config.sensor.alphaDist, config.sensor.betaDist, config.sensor.alphaTilt, config.sensor.betaTilt); + + % Place agents in a quadrant that contains no objective peaks + midXY = (tc.testClass.domain.minCorner(1:2) + tc.testClass.domain.maxCorner(1:2)) / 2; + occupied = false(2, 2); + for ii = 1:size(objectiveMu, 1) + occupied(1 + (objectiveMu(ii, 1) >= midXY(1)), ... + 1 + (objectiveMu(ii, 2) >= midXY(2))) = true; + end + freeQ = find(~occupied); + if isempty(freeQ) + qi = 1; + else + qi = freeQ(randi(numel(freeQ))); + end + [xi, yi] = ind2sub([2, 2], qi); + xLim = [tc.testClass.domain.minCorner(1), midXY(1), tc.testClass.domain.maxCorner(1)]; + yLim = [tc.testClass.domain.minCorner(2), midXY(2), tc.testClass.domain.maxCorner(2)]; + agentBounds = [max(xLim(xi), tc.testClass.domain.minCorner(1) + tc.collisionRadius), ... + max(yLim(yi), tc.testClass.domain.minCorner(2) + tc.collisionRadius), ... + minAlt + tc.collisionRadius; ... + min(xLim(xi+1), tc.testClass.domain.maxCorner(1) - tc.collisionRadius), ... + min(yLim(yi+1), tc.testClass.domain.maxCorner(2) - tc.collisionRadius), ... + tc.testClass.domain.maxCorner(3) - tc.collisionRadius]; + collisionGeometry = spherical; + for jj = 1:n + retry = true; + while retry + retry = false; + + if jj == 1 + % First agent: uniform random within placement bounds + agentPos = agentBounds(1, :) + (agentBounds(2, :) - agentBounds(1, :)) .* rand(1, 3); + else + % Sample near centroid of existing agents to maximize + % probability of being within comRange of all others + positions = cell2mat(cellfun(@(x) x.pos, agents(1:(jj-1)), 'UniformOutput', false)); + centroid = mean(positions, 1); + maxSpread = max(vecnorm(positions - centroid, 2, 2)); + safeRadius = tc.comRange - maxSpread; + + if safeRadius > 2 * tc.collisionRadius + % Uniform random within guaranteed-connected sphere + dir = randn(1, 3); + dir = dir / norm(dir); + r = safeRadius * rand()^(1/3); + agentPos = centroid + r * dir; + else + % Safe sphere too small; sample within comms sphere + % of random existing agent (comRange check below) + baseIdx = randi(jj - 1); + agentPos = agents{baseIdx}.commsGeometry.random(); + end + end + + % Check within placement bounds + if any(agentPos <= agentBounds(1, :)) || any(agentPos >= agentBounds(2, :)) + retry = true; + continue; + end + + % Check sensor performance threshold + if sensorModel.sensorPerformance(agentPos, [agentPos(1:2), 0]) < tc.sensorPerformanceMinimum * 10 + retry = true; + continue; + end + + % Check within comRange of ALL existing agents (complete graph) + for kk = 1:(jj - 1) + if norm(agents{kk}.pos - agentPos) >= tc.comRange + retry = true; + break; + end + end + if retry, continue; end + + % Check collision with ALL existing agents + for kk = 1:(jj - 1) + if norm(agents{kk}.pos - agentPos) < agents{kk}.collisionGeometry.radius + tc.collisionRadius + retry = true; + break; + end + end + end + + % Initialize agent + collisionGeometry = collisionGeometry.initialize(agentPos, tc.collisionRadius, REGION_TYPE.COLLISION, sprintf("Agent %d Collision Region", jj)); + agents{jj} = agents{jj}.initialize(agentPos, collisionGeometry, sensorModel, tc.comRange, tc.maxIter, tc.initialStepSize, sprintf("Agent %d", jj), tc.plotCommsGeometry); + end + + % Randomly shuffle agents to vary index-based topology + agents = agents(randperm(numel(agents))); + + % Add random obstacles + obstacles = cell(tc.numObstacles, 1); + [obstacles{:}] = deal(rectangularPrism); + + % Define target region for obstacles (between agents and objective) + agentExtent = max(cell2mat(cellfun(@(x) x.pos(1:2), agents, "UniformOutput", false))) + max(cellfun(@(x) x.collisionGeometry.radius, agents)); + objExtent = tc.testClass.domain.objective.groundPos - tc.testClass.domain.objective.protectedRange; + % Per-axis: use gap if valid, else fall back to full domain + obsMin = zeros(1, 2); + obsMax = zeros(1, 2); + for dim = 1:2 + if agentExtent(dim) < objExtent(dim) + obsMin(dim) = agentExtent(dim); + obsMax(dim) = objExtent(dim); + else + obsMin(dim) = tc.testClass.domain.minCorner(dim); + obsMax(dim) = tc.testClass.domain.maxCorner(dim); + end + end + + for jj = 1:size(obstacles, 1) + retry = true; + while retry + retry = false; + + % Generate corners within target region + cornersXY = obsMin + sort(rand(2, 2), 1, "ascend") .* (obsMax - obsMin); + corners = [cornersXY, [minAlt; minAlt + rand * (tc.testClass.domain.maxCorner(3) - minAlt)]]; + + % Initialize obstacle using proposed coordinates + obstacles{jj} = obstacles{jj}.initialize(corners, REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", jj)); + + % Make sure the obstacle doesn't crowd the objective + for kk = 1:size(tc.testClass.domain.objective.groundPos, 1) + if ~retry && obstacles{jj}.distance([tc.testClass.domain.objective.groundPos(kk, 1:2), minAlt]) <= tc.testClass.domain.objective.protectedRange + retry = true; + continue; + end + end + + % Check if the obstacle collides with an existing obstacle + if ~retry && jj > 1 && tc.obstacleCollisionCheck(obstacles(1:(jj - 1)), obstacles{jj}) + retry = true; + continue; + end + + % Check if the obstacle collides with an agent + if ~retry + for kk = 1:size(agents, 1) + P = min(max(agents{kk}.pos, obstacles{jj}.minCorner), obstacles{jj}.maxCorner); + d = agents{kk}.pos - P; + if dot(d, d) <= agents{kk}.collisionGeometry.radius^2 + retry = true; + break; + end + end + end + + if retry + continue; + end + end + end + + % Set up simulation + tc.testClass = tc.testClass.initialize(tc.testClass.domain, agents, tc.barrierGain, tc.barrierExponent, minAlt, tc.timestep, tc.maxIter, obstacles, tc.makePlots, tc.makeVideo, config.doubleIntegrator, tc.dampingCoeff, tc.useFixedTopology); + + % Save simulation parameters to output file + tc.testClass.writeInits(); + + % Run + tc.testClass = tc.testClass.run(); + + % Cleanup + tc.testClass = tc.testClass.teardown(); + end + end + + methods + function c = obstacleCollisionCheck(~, obstacles, obstacle) + % Check if the obstacle intersects with any other obstacles + c = false; + for ii = 1:size(obstacles, 1) + if geometryIntersects(obstacles{ii}, obstacle) + c = true; + return; + end + end + end + end +end \ No newline at end of file diff --git a/util/objectiveFunctionWrapper.m b/util/objectiveFunctionWrapper.m index f0a5963..56328ac 100644 --- a/util/objectiveFunctionWrapper.m +++ b/util/objectiveFunctionWrapper.m @@ -4,12 +4,12 @@ function f = objectiveFunctionWrapper(center, sigma) % composite objectives in particular arguments (Input) center (:, 2) double; - sigma (2, 2) double = eye(2); + sigma (:, 2, 2) double = eye(2); end arguments (Output) f (1, 1) {mustBeA(f, "function_handle")}; end - - f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), sigma), 1:size(center,1), "UniformOutput", false)), 2); + assert(size(center, 1) == size(sigma, 1)); + f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), squeeze(sigma(i, :, :))), 1:size(center,1), "UniformOutput", false)), 2); end \ No newline at end of file