kdee/miSim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactored agent sensing and guidance

Browse Source
This commit is contained in:
Kevin D
2025-11-09 22:17:21 -08:00
parent c7510812cb
commit 8b14bfc5ce
35 changed files with 224 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
agent.m
View File

@@ -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)

22
geometries/cone.m Normal file
View File

@@ -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

14
guidanceModels/gradientAscent.m Normal file
View File

@@ -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

6
miSim.m
View File

@@ -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)

2
resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMd.xml Normal file
View File

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

2
resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMp.xml Normal file
View File

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

2
resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml
View File

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

2
resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml
View File

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

2
resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4d.xml Normal file
View File

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

2
resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4p.xml Normal file
View File

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

6
resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUd.xml Normal file
View File

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

2
resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUp.xml Normal file
View File

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

6
resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGId.xml Normal file
View File

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

2
resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGIp.xml Normal file
View File

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

2
resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8d.xml Normal file
View File

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

2
resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8p.xml Normal file
View File

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

2
resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Md.xml Normal file
View File

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

2
resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Mp.xml Normal file
View File

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

6
resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4d.xml Normal file
View File

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

2
resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4p.xml Normal file
View File

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

6
resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4d.xml Normal file
View File

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

2
resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4p.xml Normal file
View File

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

2
resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUd.xml Normal file
View File

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

2
resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUp.xml Normal file
View File

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

6
resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8d.xml Normal file
View File

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

2
resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8p.xml Normal file
View File

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

2
resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8d.xml Normal file
View File

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

2
resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8p.xml Normal file
View File

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

44
sensingFunctions/basicGradientAscent.m
View File

@@ -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

60
sensingModels/fixedCardinalSensor.m Normal file
View File

@@ -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

16
sensingModels/sigmoidSensor.m Normal file
View File

@@ -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

24
test_miSim.m
View File

@@ -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

10
validators/arguments/mustBeAgents.m
View File

@@ -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

4
validators/arguments/mustBeGeometry.m
View File

@@ -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

10
validators/arguments/mustBeSensor.m Normal file
View File

@@ -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