From ade795b3ae1bd9e5b47dbd646f86c76aa6440629 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Mon, 27 Oct 2025 21:29:54 -0700 Subject: [PATCH] implemented basic gradient ascent --- agent.m | 25 ++++++++++- miSim.m | 28 +++++++----- .../Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml | 2 + .../Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml | 2 + sensingFunctions/basicGradientAscent.m | 43 +++++++++++++++++++ test_miSim.m | 5 ++- 6 files changed, 91 insertions(+), 14 deletions(-) create mode 100644 resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml create mode 100644 resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml create mode 100644 sensingFunctions/basicGradientAscent.m diff --git a/agent.m b/agent.m index b75af2b..3787afe 100644 --- a/agent.m +++ b/agent.m @@ -6,6 +6,7 @@ classdef agent % Sensor sensingFunction = @(r) 0.5; % probability of detection as a function of range + sensingLength = 0.05; % length parameter used by sensing function % State pos = NaN(1, 3); @@ -20,14 +21,15 @@ classdef agent end methods (Access = public) - function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, sensingFunction, comRange, index, label) + function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, sensingFunction, sensingLength, comRange, 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) {mustBeGeometry}; - sensingFunction (1, 1) {mustBeA(sensingFunction, 'function_handle')} + sensingFunction (1, 1) {mustBeA(sensingFunction, 'function_handle')} = @(r) 0.5; + sensingLength (1, 1) double = NaN; comRange (1, 1) double = NaN; index (1, 1) double = NaN; label (1, 1) string = ""; @@ -40,10 +42,29 @@ classdef agent obj.vel = vel; obj.cBfromC = cBfromC; obj.collisionGeometry = collisionGeometry; + obj.sensingFunction = sensingFunction; + obj.sensingLength = sensingLength; obj.comRange = comRange; obj.index = index; obj.label = label; end + function obj = run(obj, objectiveFunction) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'agent')}; + objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')}; + end + arguments (Output) + obj (1, 1) {mustBeA(obj, 'agent')}; + end + + % Do sensing to determine target position + nextPos = obj.sensingFunction(objectiveFunction, obj.pos, obj.sensingLength); + + % Move to next position + % (dynamics not modeled at this time) + obj.pos = nextPos; + + end function f = plot(obj, f) arguments (Input) obj (1, 1) {mustBeA(obj, 'agent')}; diff --git a/miSim.m b/miSim.m index e60cdff..37cdcea 100644 --- a/miSim.m +++ b/miSim.m @@ -54,17 +54,25 @@ classdef miSim arguments (Output) obj (1, 1) {mustBeA(obj, 'miSim')}; end - keyboard - % Iterate over agents to simulate their motion - for ii = 1:size(obj.agents, 1) - obj.agents{ii} + + % Set up times to iterate over + times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)'; + + for ii = 1:size(times, 1) + % Get current sim time + t = times(ii); + + % Iterate over agents to simulate their motion + for jj = 1:size(obj.agents, 1) + obj.agents{jj} = obj.agents{jj}.run(obj.objective.objectiveFunction); + end + + % Update adjacency matrix + obj = obj.updateAdjacency; + + % Update plots + end - - % Update adjacency matrix - obj = obj.updateAdjacency; - - % Update plots - end function obj = updateAdjacency(obj) arguments (Input) diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml new file mode 100644 index 0000000..34af544 --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml new file mode 100644 index 0000000..0f57430 --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/sensingFunctions/basicGradientAscent.m b/sensingFunctions/basicGradientAscent.m new file mode 100644 index 0000000..9889f28 --- /dev/null +++ b/sensingFunctions/basicGradientAscent.m @@ -0,0 +1,43 @@ +function nextPos = basicGradientAscent(objectiveFunction, pos, r) + arguments (Input) + objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')}; + pos (1, 3) double; + r (1, 1) double; + end + arguments (Output) + nextPos(1, 3) double; + end + + % Evaluate objective at position offsets +/-[r, 0, 0] and +/-[0, r, 0] + currentPos = pos(1:2); + neighborPos = [currentPos(1) + r, currentPos(2); ... % (+x) + currentPos(1), currentPos(2) + r; ... % (+y) + currentPos(1) - r, currentPos(2); ... % (-x) + currentPos(1), currentPos(2) - r; ... % (-y) + ]; + + % Check for neighbor positions that fall outside of the domain + outOfBounds = false(size(neighborPos, 1), 1); + for ii = 1:size(neighborPos, 1) + if ~domain.contains([neighborPos(ii, :), 0]) + outOfBounds(ii) = true; + end + end + + % Replace out of bounds positions with inoffensive in-bounds positions + neighborPos(outOfBounds, 1:3) = repmat(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) + ]; + + % Prevent out of bounds locations from ever possibly being selected + neighborValues(outOfBounds) = 0; + + % Select next position by maximum sensed value + nextPos = neighborPos(neighborValues == max(neighborValues), :); + nextPos = nextPos(1, 1:3); % just in case two get selected, simply pick one +end \ No newline at end of file diff --git a/test_miSim.m b/test_miSim.m index f949ac8..3211893 100644 --- a/test_miSim.m +++ b/test_miSim.m @@ -22,6 +22,7 @@ classdef test_miSim < matlab.unittest.TestCase % Agents minAgents = 3; % Minimum number of agents to be randomly generated maxAgents = 9; % Maximum number of agents to be randomly generated + sensingLength = 0.05; % length parameter used by sensing function agents = cell(0, 1); % Collision @@ -186,7 +187,7 @@ classdef test_miSim < matlab.unittest.TestCase % 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)), @(r) 0.5, tc.comRange, ii, sprintf("Agent %d", ii)); + 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)), @(r) 0.5, tc.sensingLength, tc.comRange, ii, sprintf("Agent %d", ii)); % Make sure candidate agent doesn't collide with % domain @@ -375,7 +376,7 @@ classdef test_miSim < matlab.unittest.TestCase % 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)), @(r) 0.5, tc.comRange, ii, sprintf("Agent %d", ii)); + 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)), @basicGradientAscent, tc.sensingLength, tc.comRange, ii, sprintf("Agent %d", ii)); % Make sure candidate agent doesn't collide with % domain