refactored agent sensing and guidance

agent.m

@@ -5,9 +5,12 @@ classdef agent
         label = "";
 
         % Sensor
-        sensingFunction = @(r) 0.5; % probability of detection as a function of range
+        sensorModel;
         sensingLength = 0.05; % length parameter used by sensing function
 
+        % Guidance
+        guidanceModel;
+
         % State
         lastPos = NaN(1, 3); % position from previous timestep
         pos = NaN(1, 3); % current position
@@ -25,15 +28,15 @@ classdef agent
     end
 
     methods (Access = public)
-        function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, sensingFunction, sensingLength, comRange, index, label)
+        function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, sensorModel, guidanceModel, 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')} = @(r) 0.5;
+                sensorModel (1, 1) {mustBeSensor}
-                sensingLength (1, 1) double = NaN;
+                guidanceModel (1, 1) {mustBeA(guidanceModel, 'function_handle')};
                 comRange (1, 1) double = NaN;
                 index (1, 1) double = NaN;
                 label (1, 1) string = "";
@@ -46,8 +49,8 @@ classdef agent
             obj.vel = vel;
             obj.cBfromC = cBfromC;
             obj.collisionGeometry = collisionGeometry;
-            obj.sensingFunction = sensingFunction;
+            obj.sensorModel = sensorModel;
-            obj.sensingLength = sensingLength;
+            obj.guidanceModel = guidanceModel;
             obj.comRange = comRange;
             obj.index = index;
             obj.label = label;
@@ -62,8 +65,11 @@ classdef agent
                 obj (1, 1) {mustBeA(obj, 'agent')};
             end
 
-            % Do sensing to determine target position
+            % Do sensing
-            nextPos = obj.sensingFunction(objectiveFunction, domain, obj.pos, obj.sensingLength);
+            [sensedValues, sensedPositions] = obj.sensorModel.sense(objectiveFunction, domain, obj.pos);
+
+            % Determine next planned position
+            nextPos = obj.guidanceModel(sensedValues, sensedPositions, obj.pos);
 
             % Move to next position
             % (dynamics not modeled at this time)

geometries/cone.m

@@ -0,0 +1,22 @@
+classdef cone
+    %CONE Summary of this class goes here
+    %   Detailed explanation goes here
+
+    properties
+        Property1
+    end
+
+    methods
+        function obj = cone(inputArg1,inputArg2)
+            %CONE Construct an instance of this class
+            %   Detailed explanation goes here
+            obj.Property1 = inputArg1 + inputArg2;
+        end
+
+        function outputArg = method1(obj,inputArg)
+            %METHOD1 Summary of this method goes here
+            %   Detailed explanation goes here
+            outputArg = obj.Property1 + inputArg;
+        end
+    end
+end

guidanceModels/gradientAscent.m

@@ -0,0 +1,14 @@
+function nextPos = gradientAscent(sensedValues, sensedPositions, pos)
+    arguments (Input)
+        sensedValues (:, 1) double;
+        sensedPositions (:, 3) double;
+        pos (1, 3) double;
+    end
+    arguments (Output)
+        nextPos(1, 3) double;
+    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
+end

miSim.m

@@ -24,7 +24,7 @@ classdef miSim
                 obj (1, 1) {mustBeA(obj, 'miSim')};
                 domain (1, 1) {mustBeGeometry};
                 objective (1, 1) {mustBeA(objective, 'sensingObjective')};
-                agents (:, 1) cell {mustBeAgents};
+                agents (:, 1) cell;
                 timestep (:, 1) double = 0.05;
                 maxIter (:, 1) double = 1000;
                 obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
@@ -94,8 +94,8 @@ classdef miSim
             times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
 
             % Start video writer
-            obj.v.FrameRate = 1/obj.timestep;
+            % obj.v.FrameRate = 1/obj.timestep;
-            obj.v.Quality = 90;
+            % obj.v.Quality = 90;
             obj.v.open();
 
             for ii = 1:size(times, 1)

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMd.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="420d04e4-3880-4a45-8609-11cb30d87302" type="Reference"/>

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

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

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

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4d.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1d8d2b42-2863-4985-9cf2-980917971eba" type="Reference"/>

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="mustBeSensor.m" type="File"/>

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGId.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGIp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="cone.m" type="File"/>

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8d.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="sensingModels" type="File"/>

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Md.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Mp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="guidanceModels" type="File"/>

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4d.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="sigmoidSensor.m" type="File"/>

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4d.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="fixedCardinalSensor.m" type="File"/>

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUd.xml

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

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8d.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="gradientAscent.m" type="File"/>

resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8d.xml

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

resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>

sensingFunctions/basicGradientAscent.m

@@ -1,44 +0,0 @@
-function nextPos = basicGradientAscent(objectiveFunction, domain, pos, r)
-    arguments (Input)
-        objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
-        domain (1, 1) {mustBeGeometry};
-        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:2), pos(3)]; % just in case two get selected, simply pick one
-end

sensingModels/fixedCardinalSensor.m

@@ -0,0 +1,60 @@
+classdef fixedCardinalSensor
+    % Senses in the +/-x, +/- y directions at some specified fixed length
+    properties
+        r = 0.1; % fixed sensing length
+    end
+
+    methods (Access = public)
+        function obj = initialize(obj, r)
+            arguments(Input)
+                obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
+                r (1, 1) double;
+            end
+            arguments(Output)
+                obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
+            end
+            obj.r = r;
+        end
+        function [neighborValues, neighborPos] = sense(obj, objectiveFunction, domain, pos)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
+                objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
+                domain (1, 1) {mustBeGeometry};
+                pos (1, 3) double;
+            end
+            arguments (Output)
+                neighborValues (4, 1) double;
+                neighborPos (4, 3) double;
+            end
+        
+            % Evaluate objective at position offsets +/-[r, 0, 0] and +/-[0, r, 0]
+            currentPos = 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)
+                           currentPos(1), currentPos(2) - obj.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;
+        end
+    end
+end

sensingModels/sigmoidSensor.m

@@ -0,0 +1,16 @@
+function accuracy = sigmoid(sensorPos, targetPos)
+    arguments (Input)
+        sensorPos (1, 3) double;
+        targetPos (:, 3) double;
+    end
+    arguments (Output)
+        accuracy (:, 3) double;
+    end
+    
+    
+
+end
+
+function distanceMembership()
+
+end

test_miSim.m

@@ -185,9 +185,16 @@ classdef test_miSim < matlab.unittest.TestCase
                         continue;
                     end
 
-                    % Initialize candidate agent
+                    % Initialize candidate agent collision geometry
                     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.sensingLength, tc.comRange, ii, sprintf("Agent %d", ii));
+                    candidateGeometry = 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));
+
+                    % Initialize candidate agent sensor model
+                    sensor = fixedCardinalSensor;
+                    sensor = sensor.initialize(tc.sensingLength);
+
+                    % Initialize candidate agent
+                    newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3), candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii)); 
 
                     % Make sure candidate agent doesn't collide with
                     % domain
@@ -346,9 +353,16 @@ classdef test_miSim < matlab.unittest.TestCase
                         continue;
                     end
 
-                    % Initialize candidate agent
+                    % Initialize candidate agent collision geometry
                     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)), @basicGradientAscent, tc.sensingLength, tc.comRange, ii, sprintf("Agent %d", ii));
+                    candidateGeometry = 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));
+                    
+                    % Initialize candidate agent sensor model
+                    sensor = fixedCardinalSensor;
+                    sensor.initialize(tc.sensingLength);
+
+                    % Initialize candidate agent
+                    newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3), candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii));
                     
                     % Make sure candidate agent doesn't collide with
                     % domain

validators/arguments/mustBeAgents.m

@@ -1,10 +0,0 @@
-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

validators/arguments/mustBeGeometry.m

@@ -2,9 +2,9 @@ function mustBeGeometry(geometry)
     validGeometries = ["rectangularPrism";];
     if isa(geometry, 'cell')
         for ii = 1:size(geometry, 1)
-            assert(isa(geometry{ii}, validGeometries), "Geometry in index %d is not a valid geometry class", ii);
+            assert(any(arrayfun(@(x) isa(geometry{ii}, x), validGeometries)), "Geometry in index %d is not a valid geometry class", ii);
         end
     else
-        assert(isa(geometry, validGeometries), "Geometry is not a valid geometry class");
+        assert(any(arrayfun(@(x) isa(geometry, x), validGeometries)), "Geometry is not a valid geometry class");
     end
 end

validators/arguments/mustBeSensor.m

@@ -0,0 +1,10 @@
+function mustBeSensor(sensorModel)
+    validSensorModels = ["fixedCardinalSensor"; "sigmoidSensor";];
+    if isa(sensorModel, 'cell')
+        for ii = 1:size(sensorModel, 1)
+            assert(any(arrayfun(@(x) isa(sensorModel{ii}, x), validSensorModels)), "Sensor in index %d is not a valid sensor class", ii);
+        end
+    else
+        assert(any(arrayfun(@(x) isa(sensorModel, x), validSensorModels)), "Sensor is not a valid sensor class");
+    end
+end