implemented basic gradient ascent

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')};

miSim.m

@@ -54,10 +54,17 @@ classdef miSim
             arguments (Output)
                 obj (1, 1) {mustBeA(obj, 'miSim')};
             end
-            keyboard
+
+            % 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 ii = 1:size(obj.agents, 1)
-                obj.agents{ii}
+                for jj = 1:size(obj.agents, 1)
+                    obj.agents{jj} = obj.agents{jj}.run(obj.objective.objectiveFunction);
                 end
     
                 % Update adjacency matrix
@@ -66,6 +73,7 @@ classdef miSim
                 % Update plots
 
             end
+        end
         function obj = updateAdjacency(obj)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'miSim')};

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Ref="sensingFunctions" Type="Relative"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="9c9ce3cb-5989-41e8-a20d-358a95c08b20" type="Reference"/>

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

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