diff --git a/agent.m b/agent.m index 0b1ef5d..2302ed1 100644 --- a/agent.m +++ b/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; - sensingLength (1, 1) double = NaN; + sensorModel (1, 1) {mustBeSensor} + 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.sensingLength = sensingLength; + obj.sensorModel = sensorModel; + 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 - nextPos = obj.sensingFunction(objectiveFunction, domain, obj.pos, obj.sensingLength); + % Do sensing + [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) diff --git a/geometries/cone.m b/geometries/cone.m new file mode 100644 index 0000000..af1a9b1 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/guidanceModels/gradientAscent.m b/guidanceModels/gradientAscent.m new file mode 100644 index 0000000..16c3a46 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/miSim.m b/miSim.m index 8f7e173..7ee8ebf 100644 --- a/miSim.m +++ b/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.Quality = 90; + % obj.v.FrameRate = 1/obj.timestep; + % obj.v.Quality = 90; obj.v.open(); for ii = 1:size(times, 1) diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMd.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMd.xml new file mode 100644 index 0000000..39fd2ee --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMp.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMp.xml new file mode 100644 index 0000000..c8906b6 --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml deleted file mode 100644 index 34af544..0000000 --- a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml deleted file mode 100644 index 0f57430..0000000 --- a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4d.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4d.xml new file mode 100644 index 0000000..2c5c10e --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4d.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4p.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4p.xml new file mode 100644 index 0000000..22c085e --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUd.xml b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUp.xml b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUp.xml new file mode 100644 index 0000000..d2602e3 --- /dev/null +++ b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGId.xml b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGId.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGId.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGIp.xml b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGIp.xml new file mode 100644 index 0000000..6dbcfb8 --- /dev/null +++ b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGIp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8d.xml b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8d.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8d.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8p.xml b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8p.xml new file mode 100644 index 0000000..9ec3a36 --- /dev/null +++ b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Md.xml b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Md.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Md.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Mp.xml b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Mp.xml new file mode 100644 index 0000000..36091b7 --- /dev/null +++ b/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Mp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4d.xml b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4d.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4d.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4p.xml b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4p.xml new file mode 100644 index 0000000..80e2ce0 --- /dev/null +++ b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4d.xml b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4d.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4d.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4p.xml b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4p.xml new file mode 100644 index 0000000..e09fee5 --- /dev/null +++ b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUd.xml b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUd.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUp.xml b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUp.xml new file mode 100644 index 0000000..01cb34e --- /dev/null +++ b/resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8d.xml b/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8d.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8d.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8p.xml b/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8p.xml new file mode 100644 index 0000000..a34fa7a --- /dev/null +++ b/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8d.xml b/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8d.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8d.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8p.xml b/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8p.xml new file mode 100644 index 0000000..01cb34e --- /dev/null +++ b/resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/sensingFunctions/basicGradientAscent.m b/sensingFunctions/basicGradientAscent.m deleted file mode 100644 index 003f674..0000000 --- a/sensingFunctions/basicGradientAscent.m +++ /dev/null @@ -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 \ No newline at end of file diff --git a/sensingModels/fixedCardinalSensor.m b/sensingModels/fixedCardinalSensor.m new file mode 100644 index 0000000..d08a3cd --- /dev/null +++ b/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 \ No newline at end of file diff --git a/sensingModels/sigmoidSensor.m b/sensingModels/sigmoidSensor.m new file mode 100644 index 0000000..ff6a429 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/test_miSim.m b/test_miSim.m index b8a3a44..cdacb20 100644 --- a/test_miSim.m +++ b/test_miSim.m @@ -185,10 +185,17 @@ 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 violation = false; @@ -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 diff --git a/validators/arguments/mustBeAgents.m b/validators/arguments/mustBeAgents.m deleted file mode 100644 index a2b906f..0000000 --- a/validators/arguments/mustBeAgents.m +++ /dev/null @@ -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 \ No newline at end of file diff --git a/validators/arguments/mustBeGeometry.m b/validators/arguments/mustBeGeometry.m index a8242c1..870e109 100644 --- a/validators/arguments/mustBeGeometry.m +++ b/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 \ No newline at end of file diff --git a/validators/arguments/mustBeSensor.m b/validators/arguments/mustBeSensor.m new file mode 100644 index 0000000..6f252fc --- /dev/null +++ b/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 \ No newline at end of file