From 3d3517957964774d7e661847f1deeddb83fce575 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Wed, 12 Nov 2025 18:13:43 -0800 Subject: [PATCH] partitioning introduced to main loop --- agent.m | 7 +++--- guidanceModels/gradientAscent.m | 14 +++++++++++- miSim.m | 30 ++++++++++++++++++++------ sensingModels/fixedCardinalSensor.m | 19 +++++++++-------- sensingModels/sigmoidSensor.m | 33 +++++++++++++++++------------ test_miSim.m | 8 +++---- 6 files changed, 75 insertions(+), 36 deletions(-) diff --git a/agent.m b/agent.m index c922223..58d254f 100644 --- a/agent.m +++ b/agent.m @@ -58,18 +58,19 @@ classdef agent obj.index = index; obj.label = label; end - function obj = run(obj, objectiveFunction, domain) + function obj = run(obj, sensingObjective, domain, partitioning) arguments (Input) obj (1, 1) {mustBeA(obj, 'agent')}; - objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')}; + sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')}; domain (1, 1) {mustBeGeometry}; + partitioning (:, :) double; end arguments (Output) obj (1, 1) {mustBeA(obj, 'agent')}; end % Do sensing - [sensedValues, sensedPositions] = obj.sensorModel.sense(objectiveFunction, domain, obj.pos); + [sensedValues, sensedPositions] = obj.sensorModel.sense(obj, sensingObjective, domain, partitioning); % Determine next planned position nextPos = obj.guidanceModel(sensedValues, sensedPositions, obj.pos); diff --git a/guidanceModels/gradientAscent.m b/guidanceModels/gradientAscent.m index 16c3a46..2862c10 100644 --- a/guidanceModels/gradientAscent.m +++ b/guidanceModels/gradientAscent.m @@ -1,14 +1,26 @@ -function nextPos = gradientAscent(sensedValues, sensedPositions, pos) +function nextPos = gradientAscent(sensedValues, sensedPositions, pos, rate) arguments (Input) sensedValues (:, 1) double; sensedPositions (:, 3) double; pos (1, 3) double; + rate (1, 1) double = 0.1; end arguments (Output) nextPos(1, 3) double; end + + % As a default, maintain current position + if size(sensedValues, 1) == 0 && size(sensedPositions, 1) == 0 + nextPos = pos; + return; + end % Select next position by maximum sensed value nextPos = sensedPositions(sensedValues == max(sensedValues), :); nextPos = [nextPos(1, 1:2), pos(3)]; % just in case two get selected, simply pick one + + % rate-limit motion + v = nextPos - pos; + nextPos = pos + (v / norm(v, 2)) * rate; + end \ No newline at end of file diff --git a/miSim.m b/miSim.m index ac66900..b17604f 100644 --- a/miSim.m +++ b/miSim.m @@ -4,6 +4,7 @@ classdef miSim % Simulation parameters properties (SetAccess = private, GetAccess = public) timestep = NaN; % delta time interval for simulation iterations + partitioningFreq = NaN; % number of simulation timesteps at which the partitioning routine is re-run maxIter = NaN; % maximum number of simulation iterations domain = rectangularPrism; objective = sensingObjective; @@ -27,13 +28,14 @@ classdef miSim end methods (Access = public) - function [obj, f] = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles) + function [obj, f] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles) arguments (Input) obj (1, 1) {mustBeA(obj, 'miSim')}; domain (1, 1) {mustBeGeometry}; objective (1, 1) {mustBeA(objective, 'sensingObjective')}; agents (:, 1) cell; timestep (:, 1) double = 0.05; + partitoningFreq (:, 1) double = 0.25 maxIter (:, 1) double = 1000; obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1); end @@ -48,6 +50,7 @@ classdef miSim % Define domain obj.domain = domain; + obj.partitioningFreq = partitoningFreq; % Add geometries representing obstacles within the domain obj.obstacles = obstacles; @@ -106,6 +109,7 @@ classdef miSim % Set up times to iterate over times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)'; + partitioningTimes = times(obj.partitioningFreq:obj.partitioningFreq:size(times, 1)); % Start video writer v = setupVideoWriter(obj.timestep); @@ -116,16 +120,23 @@ classdef miSim t = times(ii); fprintf("Sim Time: %4.2f (%d/%d)\n", t, ii, obj.maxIter) + % Check if it's time for new partitions + updatePartitions = false; + if ismember(t, partitioningTimes) + updatePartitions = true; + obj = obj.partition(); + end + % Iterate over agents to simulate their motion for jj = 1:size(obj.agents, 1) - obj.agents{jj} = obj.agents{jj}.run(obj.objective.objectiveFunction, obj.domain); + obj.agents{jj} = obj.agents{jj}.run(obj.objective, obj.domain, obj.partitioning); end % Update adjacency matrix obj = obj.updateAdjacency; % Update plots - [obj, f] = obj.updatePlots(f); + [obj, f] = obj.updatePlots(f, updatePartitions); % Write frame in to video I = getframe(f); @@ -160,10 +171,11 @@ classdef miSim [i,j] = ndgrid(1:m, 1:n); obj.partitioning = agentInds(sub2ind(size(agentInds), i, j, idx)); end - function [obj, f] = updatePlots(obj, f) + function [obj, f] = updatePlots(obj, f, updatePartitions) arguments (Input) obj (1, 1) {mustBeA(obj, 'miSim')}; f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure; + updatePartitions (1, 1) logical = false; end arguments (Output) obj (1, 1) {mustBeA(obj, 'miSim')}; @@ -181,11 +193,17 @@ classdef miSim % Update agent connections plot delete(obj.connectionsPlot); - [obj, f] = obj.plotConnections(f); + [obj, f] = obj.plotConnections(obj.spatialPlotIndices, f); % Update network graph plot delete(obj.graphPlot); - [obj, f] = obj.plotGraph(f); + [obj, f] = obj.plotGraph(obj.networkGraphIndex, f); + + % Update partitioning plot + if updatePartitions + delete(obj.partitionPlot); + [obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f); + end drawnow; end diff --git a/sensingModels/fixedCardinalSensor.m b/sensingModels/fixedCardinalSensor.m index 544b979..954674e 100644 --- a/sensingModels/fixedCardinalSensor.m +++ b/sensingModels/fixedCardinalSensor.m @@ -15,12 +15,13 @@ classdef fixedCardinalSensor end obj.r = r; end - function [neighborValues, neighborPos] = sense(obj, objectiveFunction, domain, pos) + function [neighborValues, neighborPos] = sense(obj, agent, sensingObjective, domain, partitioning) arguments (Input) obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')}; - objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')}; + agent (1, 1) {mustBeA(agent, 'agent')}; + sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')}; domain (1, 1) {mustBeGeometry}; - pos (1, 3) double; + partitioning (:, :) double = NaN; end arguments (Output) neighborValues (4, 1) double; @@ -28,7 +29,7 @@ classdef fixedCardinalSensor end % Evaluate objective at position offsets +/-[r, 0, 0] and +/-[0, r, 0] - currentPos = pos(1:2); + currentPos = agent.pos(1:2); neighborPos = [currentPos(1) + obj.r, currentPos(2); ... % (+x) currentPos(1), currentPos(2) + obj.r; ... % (+y) currentPos(1) - obj.r, currentPos(2); ... % (-x) @@ -44,13 +45,13 @@ classdef fixedCardinalSensor end % Replace out of bounds positions with inoffensive in-bounds positions - neighborPos(outOfBounds, 1:3) = repmat(pos, sum(outOfBounds), 1); + neighborPos(outOfBounds, 1:3) = repmat(agent.pos, sum(outOfBounds), 1); % Sense values at selected positions - neighborValues = [objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x) - objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y) - objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x) - objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y) + neighborValues = [sensingObjective.objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x) + sensingObjective.objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y) + sensingObjective.objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x) + sensingObjective.objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y) ]; % Prevent out of bounds locations from ever possibly being selected diff --git a/sensingModels/sigmoidSensor.m b/sensingModels/sigmoidSensor.m index a52c1c3..175b9b6 100644 --- a/sensingModels/sigmoidSensor.m +++ b/sensingModels/sigmoidSensor.m @@ -1,12 +1,12 @@ classdef sigmoidSensor properties (SetAccess = private, GetAccess = public) % Sensor parameters - alphaDist; - betaDist; - alphaPan; - betaPan; - alphaTilt; - betaTilt; + alphaDist = NaN; + betaDist = NaN; + alphaPan = NaN; + betaPan = NaN; + alphaTilt = NaN; + betaTilt = NaN; end methods (Access = public) @@ -31,19 +31,26 @@ classdef sigmoidSensor obj.alphaTilt = alphaTilt; obj.betaTilt = betaTilt; end - function [neighborValues, neighborPos] = sense(obj, objectiveFunction, domain, pos) + function [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning) arguments (Input) obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}; - objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')}; + agent (1, 1) {mustBeA(agent, 'agent')}; + sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')}; domain (1, 1) {mustBeGeometry}; - pos (1, 3) double; + partitioning (:, :) double; end arguments (Output) - neighborValues (4, 1) double; - neighborPos (4, 3) double; + values (:, 1) double; + positions (:, 3) double; end - - + + % Find positions for this agent's assigned partition in the domain + idx = partitioning == agent.index; + positions = [sensingObjective.X(idx), sensingObjective.Y(idx), zeros(size(sensingObjective.X(idx)))]; + + % Evaluate objective function at every point in this agent's + % assigned partiton + values = sensingObjective.values(idx); end function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos) arguments (Input) diff --git a/test_miSim.m b/test_miSim.m index 35bf4e4..45423e4 100644 --- a/test_miSim.m +++ b/test_miSim.m @@ -6,6 +6,7 @@ classdef test_miSim < matlab.unittest.TestCase domain = rectangularPrism; % domain geometry maxIter = 250; timestep = 0.05 + partitoningFreq = 5; % Obstacles minNumObstacles = 1; % Minimum number of obstacles to be randomly generated @@ -242,7 +243,7 @@ classdef test_miSim < matlab.unittest.TestCase end % Initialize the simulation - [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles); + [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles); end function misim_run(tc) % randomly create obstacles @@ -410,7 +411,7 @@ classdef test_miSim < matlab.unittest.TestCase end % Initialize the simulation - [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles); + [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles); % Run simulation loop [tc.testClass, f] = tc.testClass.run(f); @@ -418,7 +419,6 @@ classdef test_miSim < matlab.unittest.TestCase function test_basic_partitioning(tc) % place agents a fixed distance +/- X from the domain's center d = 1; - c = 0.1; % make basic domain tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); @@ -442,7 +442,7 @@ classdef test_miSim < matlab.unittest.TestCase tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3*d, 2, sprintf("Agent %d", 2)); % Initialize the simulation - [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter); + [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter); end end end \ No newline at end of file