'
This commit is contained in:
@@ -1,6 +1,6 @@
|
|||||||
function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
|
function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
obj (1, 1) {mustBeA(obj, "agent")};
|
||||||
pos (1, 3) double;
|
pos (1, 3) double;
|
||||||
collisionGeometry (1, 1) {mustBeGeometry};
|
collisionGeometry (1, 1) {mustBeGeometry};
|
||||||
sensorModel (1, 1) {mustBeSensor};
|
sensorModel (1, 1) {mustBeSensor};
|
||||||
@@ -11,7 +11,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
|
|||||||
plotCommsGeometry (1, 1) logical = false;
|
plotCommsGeometry (1, 1) logical = false;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
obj (1, 1) {mustBeA(obj, "agent")};
|
||||||
end
|
end
|
||||||
|
|
||||||
obj.pos = pos;
|
obj.pos = pos;
|
||||||
|
|||||||
@@ -1,8 +1,8 @@
|
|||||||
function [partitioning] = partition(obj, agents, objective)
|
function [partitioning] = partition(obj, agents, objective)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
obj (1, 1) {mustBeA(obj, "agent")};
|
||||||
agents (:, 1) {mustBeA(agents, 'cell')};
|
agents (:, 1) {mustBeA(agents, "cell")};
|
||||||
objective (1, 1) {mustBeA(objective, 'sensingObjective')};
|
objective (1, 1) {mustBeA(objective, "sensingObjective")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
partitioning (:, :) double;
|
partitioning (:, :) double;
|
||||||
@@ -10,7 +10,7 @@ function [partitioning] = partition(obj, agents, objective)
|
|||||||
|
|
||||||
% Assess sensing performance of each agent at each sample point
|
% Assess sensing performance of each agent at each sample point
|
||||||
% in the domain
|
% in the domain
|
||||||
agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, [objective.X(:), objective.Y(:), zeros(size(objective.X(:)))]), size(objective.X)), agents, 'UniformOutput', false);
|
agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, [objective.X(:), objective.Y(:), zeros(size(objective.X(:)))]), size(objective.X)), agents, "UniformOutput", false);
|
||||||
agentPerformances{end + 1} = objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
|
agentPerformances{end + 1} = objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
|
||||||
agentPerformances = cat(3, agentPerformances{:});
|
agentPerformances = cat(3, agentPerformances{:});
|
||||||
|
|
||||||
@@ -24,7 +24,7 @@ function [partitioning] = partition(obj, agents, objective)
|
|||||||
agentInds = reshape(agentInds, [size(agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
|
agentInds = reshape(agentInds, [size(agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
|
||||||
end
|
end
|
||||||
agentInds = num2cell(agentInds, 2:3);
|
agentInds = num2cell(agentInds, 2:3);
|
||||||
agentInds = cellfun(@(x) squeeze(x), agentInds, 'UniformOutput', false);
|
agentInds = cellfun(@(x) squeeze(x), agentInds, "UniformOutput", false);
|
||||||
agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
|
agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
|
||||||
agentInds = cat(3, agentInds{:});
|
agentInds = cat(3, agentInds{:});
|
||||||
|
|
||||||
|
|||||||
@@ -1,12 +1,12 @@
|
|||||||
function [obj, f] = plot(obj, ind, f)
|
function [obj, f] = plot(obj, ind, f)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
obj (1, 1) {mustBeA(obj, "agent")};
|
||||||
ind (1, :) double = NaN;
|
ind (1, :) double = NaN;
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
obj (1, 1) {mustBeA(obj, "agent")};
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Create axes if they don't already exist
|
% Create axes if they don't already exist
|
||||||
@@ -14,11 +14,11 @@ function [obj, f] = plot(obj, ind, f)
|
|||||||
|
|
||||||
% Plot points representing the agent position
|
% Plot points representing the agent position
|
||||||
hold(f.Children(1).Children(end), "on");
|
hold(f.Children(1).Children(end), "on");
|
||||||
o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
|
o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), "filled", "ko", "SizeData", 25);
|
||||||
hold(f.Children(1).Children(end), "off");
|
hold(f.Children(1).Children(end), "off");
|
||||||
|
|
||||||
% Check if this is a tiled layout figure
|
% Check if this is a tiled layout figure
|
||||||
if strcmp(f.Children(1).Type, 'tiledlayout')
|
if strcmp(f.Children(1).Type, "tiledlayout")
|
||||||
% Add to other perspectives
|
% Add to other perspectives
|
||||||
o = [o; copyobj(o(1), f.Children(1).Children(2))];
|
o = [o; copyobj(o(1), f.Children(1).Children(2))];
|
||||||
o = [o; copyobj(o(1), f.Children(1).Children(3))];
|
o = [o; copyobj(o(1), f.Children(1).Children(3))];
|
||||||
|
|||||||
10
@agent/run.m
10
@agent/run.m
@@ -1,14 +1,14 @@
|
|||||||
function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
|
function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
obj (1, 1) {mustBeA(obj, "agent")};
|
||||||
domain (1, 1) {mustBeGeometry};
|
domain (1, 1) {mustBeGeometry};
|
||||||
partitioning (:, :) double;
|
partitioning (:, :) double;
|
||||||
timestepIndex (1, 1) double;
|
timestepIndex (1, 1) double;
|
||||||
index (1, 1) double;
|
index (1, 1) double;
|
||||||
agents (:, 1) {mustBeA(agents, 'cell')};
|
agents (:, 1) {mustBeA(agents, "cell")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
obj (1, 1) {mustBeA(obj, "agent")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Collect objective function values across partition
|
% Collect objective function values across partition
|
||||||
@@ -82,9 +82,9 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
|
|||||||
|
|
||||||
% Reinitialize collision geometry in the new position
|
% Reinitialize collision geometry in the new position
|
||||||
d = obj.pos - obj.collisionGeometry.center;
|
d = obj.pos - obj.collisionGeometry.center;
|
||||||
if isa(obj.collisionGeometry, 'rectangularPrism')
|
if isa(obj.collisionGeometry, "rectangularPrism")
|
||||||
obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
|
obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
|
||||||
elseif isa(obj.collisionGeometry, 'spherical')
|
elseif isa(obj.collisionGeometry, "spherical")
|
||||||
obj.collisionGeometry = obj.collisionGeometry.initialize(obj.collisionGeometry.center + d, obj.collisionGeometry.radius, obj.collisionGeometry.tag, obj.collisionGeometry.label);
|
obj.collisionGeometry = obj.collisionGeometry.initialize(obj.collisionGeometry.center + d, obj.collisionGeometry.radius, obj.collisionGeometry.tag, obj.collisionGeometry.label);
|
||||||
else
|
else
|
||||||
error("?");
|
error("?");
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function updatePlots(obj)
|
function updatePlots(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
obj (1, 1) {mustBeA(obj, "agent")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
end
|
end
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function [obj] = constrainMotion(obj)
|
function [obj] = constrainMotion(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
if size(obj.agents, 1) < 2
|
if size(obj.agents, 1) < 2
|
||||||
@@ -14,7 +14,7 @@ function [obj] = constrainMotion(obj)
|
|||||||
|
|
||||||
agents = [obj.agents{:}];
|
agents = [obj.agents{:}];
|
||||||
v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
|
v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
|
||||||
if all(isnan(v), 'all') || all(v == zeros(size(obj.agents, 1), 3), 'all')
|
if all(isnan(v), "all") || all(v == zeros(size(obj.agents, 1), 3), "all")
|
||||||
% Agents are not attempting to move, so there is no motion to be
|
% Agents are not attempting to move, so there is no motion to be
|
||||||
% constrained
|
% constrained
|
||||||
return;
|
return;
|
||||||
@@ -23,7 +23,7 @@ function [obj] = constrainMotion(obj)
|
|||||||
% Initialize QP based on number of agents and obstacles
|
% Initialize QP based on number of agents and obstacles
|
||||||
nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
|
nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
|
||||||
nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling)
|
nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling)
|
||||||
nLNAPairs = sum(obj.constraintAdjacencyMatrix, 'all') - size(obj.agents, 1);
|
nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - size(obj.agents, 1);
|
||||||
total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
|
total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
|
||||||
kk = 1;
|
kk = 1;
|
||||||
A = zeros(total, 3 * size(obj.agents, 1));
|
A = zeros(total, 3 * size(obj.agents, 1));
|
||||||
@@ -130,9 +130,9 @@ function [obj] = constrainMotion(obj)
|
|||||||
assert(size(A,2) == size(H,1))
|
assert(size(A,2) == size(H,1))
|
||||||
assert(size(A,1) == size(b,1))
|
assert(size(A,1) == size(b,1))
|
||||||
assert(size(H,1) == length(f))
|
assert(size(H,1) == length(f))
|
||||||
opt = optimoptions('quadprog', 'Display', 'off');
|
opt = optimoptions("quadprog", "Display", "off");
|
||||||
[vNew, ~, exitflag, m] = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opt);
|
[vNew, ~, exitflag, m] = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opt);
|
||||||
assert(exitflag == 1, sprintf('quadprog failure... %s%s', newline, m.message));
|
assert(exitflag == 1, sprintf("quadprog failure... %s%s", newline, m.message));
|
||||||
vNew = reshape(vNew, 3, size(obj.agents, 1))';
|
vNew = reshape(vNew, 3, size(obj.agents, 1))';
|
||||||
|
|
||||||
if exitflag <= 0
|
if exitflag <= 0
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
|
function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
domain (1, 1) {mustBeGeometry};
|
domain (1, 1) {mustBeGeometry};
|
||||||
agents (:, 1) cell;
|
agents (:, 1) cell;
|
||||||
barrierGain (1, 1) double = 100;
|
barrierGain (1, 1) double = 100;
|
||||||
@@ -13,7 +13,7 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
|
|||||||
makeVideo (1, 1) logical = true;
|
makeVideo (1, 1) logical = true;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% enable/disable plotting and video writer
|
% enable/disable plotting and video writer
|
||||||
@@ -27,7 +27,7 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
|
|||||||
obj.makeVideo = makeVideo;
|
obj.makeVideo = makeVideo;
|
||||||
|
|
||||||
% Generate artifact(s) name
|
% Generate artifact(s) name
|
||||||
obj.artifactName = strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'));
|
obj.artifactName = strcat(string(datetime("now"), "yyyy_MM_dd_HH_mm_ss"));
|
||||||
|
|
||||||
% Define simulation time parameters
|
% Define simulation time parameters
|
||||||
obj.timestep = timestep;
|
obj.timestep = timestep;
|
||||||
@@ -87,7 +87,7 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
|
|||||||
|
|
||||||
% Initialize variable that will store agent positions for trail plots
|
% Initialize variable that will store agent positions for trail plots
|
||||||
obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
|
obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
|
||||||
obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3);
|
obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
|
||||||
|
|
||||||
% Set up plots showing initialized state
|
% Set up plots showing initialized state
|
||||||
obj = obj.plot();
|
obj = obj.plot();
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function obj = lesserNeighbor(obj)
|
function obj = lesserNeighbor(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% initialize solution with self-connections only
|
% initialize solution with self-connections only
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function obj = plot(obj)
|
function obj = plot(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% fast exit when plotting is disabled
|
% fast exit when plotting is disabled
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function obj = plotConnections(obj)
|
function obj = plotConnections(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Iterate over lower triangle off-diagonal region of the
|
% Iterate over lower triangle off-diagonal region of the
|
||||||
@@ -23,11 +23,11 @@ function obj = plotConnections(obj)
|
|||||||
% Plot the connections
|
% Plot the connections
|
||||||
if isnan(obj.spatialPlotIndices)
|
if isnan(obj.spatialPlotIndices)
|
||||||
hold(obj.f.CurrentAxes, "on");
|
hold(obj.f.CurrentAxes, "on");
|
||||||
o = plot3(obj.f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
|
o = plot3(obj.f.CurrentAxes, X, Y, Z, "Color", "g", "LineWidth", 2, "LineStyle", "--");
|
||||||
hold(obj.f.CurrentAxes, "off");
|
hold(obj.f.CurrentAxes, "off");
|
||||||
else
|
else
|
||||||
hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
|
hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
|
||||||
o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
|
o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, "Color", "g", "LineWidth", 2, "LineStyle", "--");
|
||||||
hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
|
hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|||||||
@@ -1,23 +1,23 @@
|
|||||||
function obj = plotGraph(obj)
|
function obj = plotGraph(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Form graph from adjacency matrix
|
% Form graph from adjacency matrix
|
||||||
G = graph(obj.constraintAdjacencyMatrix, 'omitselfloops');
|
G = graph(obj.constraintAdjacencyMatrix, "omitselfloops");
|
||||||
|
|
||||||
% Plot graph object
|
% Plot graph object
|
||||||
if isnan(obj.networkGraphIndex)
|
if isnan(obj.networkGraphIndex)
|
||||||
hold(obj.f.CurrentAxes, 'on');
|
hold(obj.f.CurrentAxes, "on");
|
||||||
o = plot(obj.f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
|
o = plot(obj.f.CurrentAxes, G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
|
||||||
hold(obj.f.CurrentAxes, 'off');
|
hold(obj.f.CurrentAxes, "off");
|
||||||
else
|
else
|
||||||
hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'on');
|
hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "on");
|
||||||
o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
|
o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
|
||||||
hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'off');
|
hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "off");
|
||||||
if size(obj.networkGraphIndex, 2) > 1
|
if size(obj.networkGraphIndex, 2) > 1
|
||||||
for ii = 2:size(ind, 2)
|
for ii = 2:size(ind, 2)
|
||||||
o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];
|
o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function obj = plotH(obj)
|
function obj = plotH(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
obj.hf = figure;
|
obj.hf = figure;
|
||||||
@@ -12,7 +12,7 @@ function obj = plotH(obj)
|
|||||||
nexttile(obj.hf.Children(1));
|
nexttile(obj.hf.Children(1));
|
||||||
axes(obj.hf.Children(1).Children(1));
|
axes(obj.hf.Children(1).Children(1));
|
||||||
grid(obj.hf.Children(1).Children(1), "on");
|
grid(obj.hf.Children(1).Children(1), "on");
|
||||||
xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
|
xlabel(obj.hf.Children(1).Children(1), "Time (s)");
|
||||||
title(obj.hf.Children(1).Children(1), "Collision Avoidance");
|
title(obj.hf.Children(1).Children(1), "Collision Avoidance");
|
||||||
hold(obj.hf.Children(1).Children(1), "on");
|
hold(obj.hf.Children(1).Children(1), "on");
|
||||||
obj.caPlot = plot(obj.h(1:(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2), :)');
|
obj.caPlot = plot(obj.h(1:(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2), :)');
|
||||||
@@ -22,13 +22,13 @@ function obj = plotH(obj)
|
|||||||
legendStrings = [legendStrings; sprintf("A%d A%d", jj, ii)];
|
legendStrings = [legendStrings; sprintf("A%d A%d", jj, ii)];
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
legend(obj.hf.Children(1).Children(1), legendStrings, 'Location', 'bestoutside');
|
legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
|
||||||
hold(obj.hf.Children(1).Children(2), "off");
|
hold(obj.hf.Children(1).Children(2), "off");
|
||||||
|
|
||||||
nexttile(obj.hf.Children(1));
|
nexttile(obj.hf.Children(1));
|
||||||
axes(obj.hf.Children(1).Children(1));
|
axes(obj.hf.Children(1).Children(1));
|
||||||
grid(obj.hf.Children(1).Children(1), "on");
|
grid(obj.hf.Children(1).Children(1), "on");
|
||||||
xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(2), "");
|
xlabel(obj.hf.Children(1).Children(1), "Time (s)");
|
||||||
title(obj.hf.Children(1).Children(1), "Obstacles");
|
title(obj.hf.Children(1).Children(1), "Obstacles");
|
||||||
hold(obj.hf.Children(1).Children(1), "on");
|
hold(obj.hf.Children(1).Children(1), "on");
|
||||||
obj.obsPlot = plot(obj.h((1 + (size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)):(((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1)), :)');
|
obj.obsPlot = plot(obj.h((1 + (size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)):(((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1)), :)');
|
||||||
@@ -38,23 +38,23 @@ function obj = plotH(obj)
|
|||||||
legendStrings = [legendStrings; sprintf("A%d O%d", jj, ii)];
|
legendStrings = [legendStrings; sprintf("A%d O%d", jj, ii)];
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
legend(obj.hf.Children(1).Children(1), legendStrings, 'Location', 'bestoutside');
|
legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
|
||||||
hold(obj.hf.Children(1).Children(2), "off");
|
hold(obj.hf.Children(1).Children(2), "off");
|
||||||
|
|
||||||
nexttile(obj.hf.Children(1));
|
nexttile(obj.hf.Children(1));
|
||||||
axes(obj.hf.Children(1).Children(1));
|
axes(obj.hf.Children(1).Children(1));
|
||||||
grid(obj.hf.Children(1).Children(1), "on");
|
grid(obj.hf.Children(1).Children(1), "on");
|
||||||
xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
|
xlabel(obj.hf.Children(1).Children(1), "Time (s)");
|
||||||
title(obj.hf.Children(1).Children(1), "Domain");
|
title(obj.hf.Children(1).Children(1), "Domain");
|
||||||
hold(obj.hf.Children(1).Children(1), "on");
|
hold(obj.hf.Children(1).Children(1), "on");
|
||||||
obj.domPlot = plot(obj.h((1 + (((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1))):size(obj.h, 1), 1:end)');
|
obj.domPlot = plot(obj.h((1 + (((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1))):size(obj.h, 1), 1:end)');
|
||||||
legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], 'Location', 'bestoutside');
|
legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], "Location", "bestoutside");
|
||||||
hold(obj.hf.Children(1).Children(2), "off");
|
hold(obj.hf.Children(1).Children(2), "off");
|
||||||
|
|
||||||
nexttile(obj.hf.Children(1));
|
nexttile(obj.hf.Children(1));
|
||||||
axes(obj.hf.Children(1).Children(1));
|
axes(obj.hf.Children(1).Children(1));
|
||||||
grid(obj.hf.Children(1).Children(1), "on");
|
grid(obj.hf.Children(1).Children(1), "on");
|
||||||
xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
|
xlabel(obj.hf.Children(1).Children(1), "Time (s)");
|
||||||
title(obj.hf.Children(1).Children(1), "Communications");
|
title(obj.hf.Children(1).Children(1), "Communications");
|
||||||
% skipped this for now because it is very complicated
|
% skipped this for now because it is very complicated
|
||||||
|
|
||||||
|
|||||||
@@ -1,19 +1,19 @@
|
|||||||
function obj = plotPartitions(obj)
|
function obj = plotPartitions(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
if isnan(obj.partitionGraphIndex)
|
if isnan(obj.partitionGraphIndex)
|
||||||
hold(obj.f.CurrentAxes, 'on');
|
hold(obj.f.CurrentAxes, "on");
|
||||||
o = imagesc(obj.f.CurrentAxes, obj.partitioning);
|
o = imagesc(obj.f.CurrentAxes, obj.partitioning);
|
||||||
hold(obj.f.CurrentAxes, 'off');
|
hold(obj.f.CurrentAxes, "off");
|
||||||
else
|
else
|
||||||
hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
|
hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), "on");
|
||||||
o = imagesc(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), obj.partitioning);
|
o = imagesc(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), obj.partitioning);
|
||||||
hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
|
hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), "off");
|
||||||
if size(obj.partitionGraphIndex, 2) > 1
|
if size(obj.partitionGraphIndex, 2) > 1
|
||||||
for ii = 2:size(ind, 2)
|
for ii = 2:size(ind, 2)
|
||||||
o = [o, copyobj(o(1), obj.f.Children(1).Children(obj.partitionGraphIndex(ii)))];
|
o = [o, copyobj(o(1), obj.f.Children(1).Children(obj.partitionGraphIndex(ii)))];
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function obj = plotPerformance(obj)
|
function obj = plotPerformance(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% fast exit when plotting is disabled
|
% fast exit when plotting is disabled
|
||||||
@@ -15,31 +15,31 @@ function obj = plotPerformance(obj)
|
|||||||
|
|
||||||
axes(obj.fPerf);
|
axes(obj.fPerf);
|
||||||
title(obj.fPerf.Children(1), "Sensor Performance");
|
title(obj.fPerf.Children(1), "Sensor Performance");
|
||||||
xlabel(obj.fPerf.Children(1), 'Time (s)');
|
xlabel(obj.fPerf.Children(1), "Time (s)");
|
||||||
ylabel(obj.fPerf.Children(1), 'Sensor Performance');
|
ylabel(obj.fPerf.Children(1), "Sensor Performance");
|
||||||
grid(obj.fPerf.Children(1), 'on');
|
grid(obj.fPerf.Children(1), "on");
|
||||||
|
|
||||||
% Plot current cumulative performance
|
% Plot current cumulative performance
|
||||||
hold(obj.fPerf.Children(1), 'on');
|
hold(obj.fPerf.Children(1), "on");
|
||||||
o = plot(obj.fPerf.Children(1), obj.perf(end, :));
|
o = plot(obj.fPerf.Children(1), obj.perf(end, :));
|
||||||
warning('off', 'MATLAB:gui:array:InvalidArrayShape'); % suppress this warning to avoid polluting output
|
warning("off", "MATLAB:gui:array:InvalidArrayShape"); % suppress this warning to avoid polluting output
|
||||||
o.XData = NaN(1, obj.maxIter); % correct time will be set at runtime
|
o.XData = NaN(1, obj.maxIter); % correct time will be set at runtime
|
||||||
o.YData = [0, NaN(1, obj.maxIter - 1)];
|
o.YData = [0, NaN(1, obj.maxIter - 1)];
|
||||||
hold(obj.fPerf.Children(1), 'off');
|
hold(obj.fPerf.Children(1), "off");
|
||||||
|
|
||||||
% Plot current agent performance
|
% Plot current agent performance
|
||||||
for ii = 1:(size(obj.perf, 1) - 1)
|
for ii = 1:(size(obj.perf, 1) - 1)
|
||||||
hold(obj.fPerf.Children(1), 'on');
|
hold(obj.fPerf.Children(1), "on");
|
||||||
o = [o; plot(obj.fPerf.Children(1), obj.perf(ii, :))];
|
o = [o; plot(obj.fPerf.Children(1), obj.perf(ii, :))];
|
||||||
o(end).XData = NaN(1, obj.maxIter); % correct time will be set at runtime
|
o(end).XData = NaN(1, obj.maxIter); % correct time will be set at runtime
|
||||||
o(end).YData = [0, NaN(1, obj.maxIter - 1)];
|
o(end).YData = [0, NaN(1, obj.maxIter - 1)];
|
||||||
hold(obj.fPerf.Children(1), 'off');
|
hold(obj.fPerf.Children(1), "off");
|
||||||
end
|
end
|
||||||
|
|
||||||
% Add legend
|
% Add legend
|
||||||
agentStrings = string(cellfun(@(x) x.label, obj.agents, 'UniformOutput', false));
|
agentStrings = string(cellfun(@(x) x.label, obj.agents, "UniformOutput", false));
|
||||||
agentStrings = ["Total"; agentStrings];
|
agentStrings = ["Total"; agentStrings];
|
||||||
legend(obj.fPerf.Children(1), agentStrings, 'Location', 'northwest');
|
legend(obj.fPerf.Children(1), agentStrings, "Location", "northwest");
|
||||||
|
|
||||||
obj.performancePlot = o;
|
obj.performancePlot = o;
|
||||||
end
|
end
|
||||||
@@ -1,9 +1,9 @@
|
|||||||
function obj = plotTrails(obj)
|
function obj = plotTrails(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')}
|
obj (1, 1) {mustBeA(obj, "miSim")}
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')}
|
obj (1, 1) {mustBeA(obj, "miSim")}
|
||||||
end
|
end
|
||||||
|
|
||||||
% fast exit when plotting is disabled
|
% fast exit when plotting is disabled
|
||||||
@@ -14,9 +14,9 @@ function obj = plotTrails(obj)
|
|||||||
% Plot full range of position history on each spatial plot axes
|
% Plot full range of position history on each spatial plot axes
|
||||||
o = [];
|
o = [];
|
||||||
for ii = 1:(size(obj.posHist, 1))
|
for ii = 1:(size(obj.posHist, 1))
|
||||||
hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'on');
|
hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
|
||||||
o = [o; plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), obj.posHist(ii, 1:obj.maxIter, 1), obj.posHist(ii, 1:obj.maxIter, 2), obj.posHist(ii, 1:obj.maxIter, 3), 'Color', 'k', 'LineWidth', 1)];
|
o = [o; plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), obj.posHist(ii, 1:obj.maxIter, 1), obj.posHist(ii, 1:obj.maxIter, 2), obj.posHist(ii, 1:obj.maxIter, 3), "Color", "k", "LineWidth", 1)];
|
||||||
hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'off');
|
hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
|
||||||
end
|
end
|
||||||
|
|
||||||
% Copy to other plots
|
% Copy to other plots
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function [obj] = run(obj)
|
function [obj] = run(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Start video writer
|
% Start video writer
|
||||||
@@ -41,7 +41,7 @@ function [obj] = run(obj)
|
|||||||
|
|
||||||
% After moving
|
% After moving
|
||||||
% Update agent position history array
|
% Update agent position history array
|
||||||
obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3);
|
obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
|
||||||
|
|
||||||
% Update total performance
|
% Update total performance
|
||||||
obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
|
obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
|
||||||
|
|||||||
@@ -1,15 +1,15 @@
|
|||||||
function v = setupVideoWriter(obj)
|
function v = setupVideoWriter(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
v (1, 1) {mustBeA(v, 'VideoWriter')};
|
v (1, 1) {mustBeA(v, "VideoWriter")};
|
||||||
end
|
end
|
||||||
|
|
||||||
if ispc || ismac
|
if ispc || ismac
|
||||||
v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'MPEG-4');
|
v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, "sandbox", strcat(obj.artifactName, "_miSimHist")), "MPEG-4");
|
||||||
elseif isunix
|
elseif isunix
|
||||||
v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'Motion JPEG AVI');
|
v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, "sandbox", strcat(obj.artifactName, "_miSimHist")), "Motion JPEG AVI");
|
||||||
end
|
end
|
||||||
|
|
||||||
v.FrameRate = 1 / obj.timestep;
|
v.FrameRate = 1 / obj.timestep;
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function obj = teardown(obj)
|
function obj = teardown(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Close plots
|
% Close plots
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function obj = updateAdjacency(obj)
|
function obj = updateAdjacency(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Initialize assuming only self-connections
|
% Initialize assuming only self-connections
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function [obj] = updatePlots(obj)
|
function [obj] = updatePlots(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Fast exit when plotting is disabled
|
% Fast exit when plotting is disabled
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function validate(obj)
|
function validate(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
end
|
end
|
||||||
@@ -10,12 +10,12 @@ function validate(obj)
|
|||||||
warning("Network is not connected");
|
warning("Network is not connected");
|
||||||
end
|
end
|
||||||
|
|
||||||
if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
|
if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
|
||||||
warning("Eliminated network connections that were necessary");
|
warning("Eliminated network connections that were necessary");
|
||||||
end
|
end
|
||||||
|
|
||||||
%% Obstacle Validators
|
%% Obstacle Validators
|
||||||
AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obj.obstacles), obj.agents, 'UniformOutput', false);
|
AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obj.obstacles), obj.agents, "UniformOutput", false);
|
||||||
AO_collisions = vertcat(AO_collisions{:});
|
AO_collisions = vertcat(AO_collisions{:});
|
||||||
if any(AO_collisions)
|
if any(AO_collisions)
|
||||||
[idx, idy] = find(AO_collisions);
|
[idx, idy] = find(AO_collisions);
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function writeParams(obj)
|
function writeParams(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
end
|
end
|
||||||
@@ -15,15 +15,15 @@ function writeParams(obj)
|
|||||||
initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
|
initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
|
||||||
|
|
||||||
% Combine with simulation parameters
|
% Combine with simulation parameters
|
||||||
params = struct('timestep', obj.timestep, 'maxIter', obj.maxIter, 'minAlt', obj.obstacles{end}.maxCorner(3), 'discretizationStep', obj.domain.objective.discretizationStep, ...
|
params = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), "discretizationStep", obj.domain.objective.discretizationStep, ...
|
||||||
'sensorPerformanceMinimum', obj.domain.objective.sensorPerformanceMinimum, 'collisionRadius', collisionRadii, 'alphaDist', alphaDist, 'betaDist', betaDist, ...
|
"sensorPerformanceMinimum", obj.domain.objective.sensorPerformanceMinimum, "collisionRadius", collisionRadii, "alphaDist", alphaDist, "betaDist", betaDist, ...
|
||||||
'alphaTilt', alphaTilt, 'betaTilt', betaTilt, 'comRange', comRange, 'initialStepSize', initialStepSize, 'barrierGain', obj.barrierGain, 'barrierExponent', obj.barrierExponent ...
|
"alphaTilt", alphaTilt, "betaTilt", betaTilt, "comRange", comRange, "initialStepSize", initialStepSize, "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent ...
|
||||||
);
|
);
|
||||||
|
|
||||||
% TODO add sensorPerformanceMinimum
|
% TODO add sensorPerformanceMinimum
|
||||||
|
|
||||||
% Save all parameters to output file
|
% Save all parameters to output file
|
||||||
paramsFile = strcat(obj.artifactName, "_miSimParams");
|
paramsFile = strcat(obj.artifactName, "_miSimParams");
|
||||||
paramsFile = fullfile(matlab.project.rootProject().RootFolder, 'sandbox', paramsFile);
|
paramsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", paramsFile);
|
||||||
save(paramsFile, "-struct", "params");
|
save(paramsFile, "-struct", "params");
|
||||||
end
|
end
|
||||||
@@ -1,14 +1,14 @@
|
|||||||
function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
|
function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1,1) {mustBeA(obj, 'sensingObjective')};
|
obj (1,1) {mustBeA(obj, "sensingObjective")};
|
||||||
objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
|
objectiveFunction (1, 1) {mustBeA(objectiveFunction, "function_handle")};
|
||||||
domain (1, 1) {mustBeGeometry};
|
domain (1, 1) {mustBeGeometry};
|
||||||
discretizationStep (1, 1) double = 1;
|
discretizationStep (1, 1) double = 1;
|
||||||
protectedRange (1, 1) double = 1;
|
protectedRange (1, 1) double = 1;
|
||||||
sensorPerformanceMinimum (1, 1) double = 1e-6;
|
sensorPerformanceMinimum (1, 1) double = 1e-6;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1,1) {mustBeA(obj, 'sensingObjective')};
|
obj (1,1) {mustBeA(obj, "sensingObjective")};
|
||||||
end
|
end
|
||||||
|
|
||||||
obj.discretizationStep = discretizationStep;
|
obj.discretizationStep = discretizationStep;
|
||||||
|
|||||||
@@ -1,12 +1,12 @@
|
|||||||
function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange)
|
function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'sensingObjective')};
|
obj (1, 1) {mustBeA(obj, "sensingObjective")};
|
||||||
domain (1, 1) {mustBeGeometry};
|
domain (1, 1) {mustBeGeometry};
|
||||||
discretizationStep (1, 1) double = 1;
|
discretizationStep (1, 1) double = 1;
|
||||||
protectedRange (1, 1) double = 1;
|
protectedRange (1, 1) double = 1;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'sensingObjective')};
|
obj (1, 1) {mustBeA(obj, "sensingObjective")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Set random objective position
|
% Set random objective position
|
||||||
|
|||||||
@@ -1,11 +1,11 @@
|
|||||||
function f = plot(obj, ind, f)
|
function f = plot(obj, ind, f)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1,1) {mustBeA(obj, 'sensingObjective')};
|
obj (1,1) {mustBeA(obj, "sensingObjective")};
|
||||||
ind (1, :) double = NaN;
|
ind (1, :) double = NaN;
|
||||||
f (1,1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
f (1,1) {mustBeA(f, "matlab.ui.Figure")} = figure;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
f (1,1) {mustBeA(f, 'matlab.ui.Figure')};
|
f (1,1) {mustBeA(f, "matlab.ui.Figure")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Create axes if they don't already exist
|
% Create axes if they don't already exist
|
||||||
@@ -14,15 +14,15 @@ function f = plot(obj, ind, f)
|
|||||||
% Plot gradient on the "floor" of the domain
|
% Plot gradient on the "floor" of the domain
|
||||||
if isnan(ind)
|
if isnan(ind)
|
||||||
hold(f.CurrentAxes, "on");
|
hold(f.CurrentAxes, "on");
|
||||||
o = surf(f.CurrentAxes, obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
|
o = surf(f.CurrentAxes, obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
|
||||||
o.HitTest = 'off';
|
o.HitTest = "off";
|
||||||
o.PickableParts = 'none';
|
o.PickableParts = "none";
|
||||||
hold(f.CurrentAxes, "off");
|
hold(f.CurrentAxes, "off");
|
||||||
else
|
else
|
||||||
hold(f.Children(1).Children(ind(1)), "on");
|
hold(f.Children(1).Children(ind(1)), "on");
|
||||||
o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
|
o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
|
||||||
o.HitTest = 'off';
|
o.HitTest = "off";
|
||||||
o.PickableParts = 'none';
|
o.PickableParts = "none";
|
||||||
hold(f.Children(1).Children(ind(1)), "off");
|
hold(f.Children(1).Children(ind(1)), "off");
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function x = distanceMembership(obj, d)
|
function x = distanceMembership(obj, d)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
|
obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
|
||||||
d (:, 1) double;
|
d (:, 1) double;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
|
|||||||
@@ -1,13 +1,13 @@
|
|||||||
function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt)
|
function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
|
obj (1, 1) {mustBeA(obj, "sigmoidSensor")}
|
||||||
alphaDist (1, 1) double;
|
alphaDist (1, 1) double;
|
||||||
betaDist (1, 1) double;
|
betaDist (1, 1) double;
|
||||||
alphaTilt (1, 1) double;
|
alphaTilt (1, 1) double;
|
||||||
betaTilt (1, 1) double;
|
betaTilt (1, 1) double;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
|
obj (1, 1) {mustBeA(obj, "sigmoidSensor")}
|
||||||
end
|
end
|
||||||
|
|
||||||
obj.alphaDist = alphaDist;
|
obj.alphaDist = alphaDist;
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function f = plotParameters(obj)
|
function f = plotParameters(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
|
obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Distance and tilt sample points
|
% Distance and tilt sample points
|
||||||
@@ -24,9 +24,9 @@ function f = plotParameters(obj)
|
|||||||
title("Distance Membership Sigmoid");
|
title("Distance Membership Sigmoid");
|
||||||
xlabel("Distance (m)");
|
xlabel("Distance (m)");
|
||||||
ylabel("Membership");
|
ylabel("Membership");
|
||||||
hold('on');
|
hold("on");
|
||||||
plot(d, d_x, 'LineWidth', 2);
|
plot(d, d_x, "LineWidth", 2);
|
||||||
hold('off');
|
hold("off");
|
||||||
ylim([0, 1]);
|
ylim([0, 1]);
|
||||||
|
|
||||||
% Tilt
|
% Tilt
|
||||||
@@ -35,8 +35,8 @@ function f = plotParameters(obj)
|
|||||||
title("Tilt Membership Sigmoid");
|
title("Tilt Membership Sigmoid");
|
||||||
xlabel("Tilt (deg)");
|
xlabel("Tilt (deg)");
|
||||||
ylabel("Membership");
|
ylabel("Membership");
|
||||||
hold('on');
|
hold("on");
|
||||||
plot(t, t_x, 'LineWidth', 2);
|
plot(t, t_x, "LineWidth", 2);
|
||||||
hold('off');
|
hold("off");
|
||||||
ylim([0, 1]);
|
ylim([0, 1]);
|
||||||
end
|
end
|
||||||
@@ -1,6 +1,6 @@
|
|||||||
function value = sensorPerformance(obj, agentPos, targetPos)
|
function value = sensorPerformance(obj, agentPos, targetPos)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
|
obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
|
||||||
agentPos (1, 3) double;
|
agentPos (1, 3) double;
|
||||||
targetPos (:, 3) double;
|
targetPos (:, 3) double;
|
||||||
end
|
end
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function x = tiltMembership(obj, t)
|
function x = tiltMembership(obj, t)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
|
obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
|
||||||
t (:, 1) double; % degrees
|
t (:, 1) double; % degrees
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function obj = initialize(obj, center, radius, height, tag, label)
|
function obj = initialize(obj, center, radius, height, tag, label)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'cone')};
|
obj (1, 1) {mustBeA(obj, "cone")};
|
||||||
center (1, 3) double;
|
center (1, 3) double;
|
||||||
radius (1, 1) double;
|
radius (1, 1) double;
|
||||||
height (1, 1) double;
|
height (1, 1) double;
|
||||||
@@ -8,7 +8,7 @@ function obj = initialize(obj, center, radius, height, tag, label)
|
|||||||
label (1, 1) string = "";
|
label (1, 1) string = "";
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'cone')};
|
obj (1, 1) {mustBeA(obj, "cone")};
|
||||||
end
|
end
|
||||||
|
|
||||||
obj.center = center;
|
obj.center = center;
|
||||||
|
|||||||
@@ -1,13 +1,13 @@
|
|||||||
function [obj, f] = plot(obj, ind, f, maxAlt)
|
function [obj, f] = plot(obj, ind, f, maxAlt)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'cone')};
|
obj (1, 1) {mustBeA(obj, "cone")};
|
||||||
ind (1, :) double = NaN;
|
ind (1, :) double = NaN;
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
|
||||||
maxAlt (1, 1) = 10;
|
maxAlt (1, 1) = 10;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'cone')};
|
obj (1, 1) {mustBeA(obj, "cone")};
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Create axes if they don't already exist
|
% Create axes if they don't already exist
|
||||||
@@ -31,7 +31,7 @@ function [obj, f] = plot(obj, ind, f, maxAlt)
|
|||||||
o = surf(f.CurrentAxes, X, Y, Z);
|
o = surf(f.CurrentAxes, X, Y, Z);
|
||||||
else
|
else
|
||||||
hold(f.Children(1).Children(ind(1)), "on");
|
hold(f.Children(1).Children(ind(1)), "on");
|
||||||
o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(obj.tag.color, 1, 1, 3), 'FaceAlpha', 0.25, 'EdgeColor', 'none');
|
o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(obj.tag.color, 1, 1, 3), "FaceAlpha", 0.25, "EdgeColor", "none");
|
||||||
hold(f.Children(1).Children(ind(1)), "off");
|
hold(f.Children(1).Children(ind(1)), "off");
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function cPos = closestToPoint(obj, pos)
|
function cPos = closestToPoint(obj, pos)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
pos (:, 3) double;
|
pos (:, 3) double;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function c = contains(obj, pos)
|
function c = contains(obj, pos)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
pos (:, 3) double;
|
pos (:, 3) double;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function c = containsLine(obj, pos1, pos2)
|
function c = containsLine(obj, pos1, pos2)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
pos1 (1, 3) double;
|
pos1 (1, 3) double;
|
||||||
pos2 (1, 3) double;
|
pos2 (1, 3) double;
|
||||||
end
|
end
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function d = distance(obj, pos)
|
function d = distance(obj, pos)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
pos (:, 3) double;
|
pos (:, 3) double;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function g = distanceGradient(obj, pos)
|
function g = distanceGradient(obj, pos)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
pos (:, 3) double;
|
pos (:, 3) double;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep)
|
function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
bounds (2, 3) double;
|
bounds (2, 3) double;
|
||||||
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
|
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
|
||||||
label (1, 1) string = "";
|
label (1, 1) string = "";
|
||||||
@@ -8,7 +8,7 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
|
|||||||
discretizationStep (1, 1) double = 1;
|
discretizationStep (1, 1) double = 1;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
end
|
end
|
||||||
|
|
||||||
obj.tag = tag;
|
obj.tag = tag;
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain, minAlt)
|
function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain, minAlt)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
|
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
|
||||||
label (1, 1) string = "";
|
label (1, 1) string = "";
|
||||||
minDimension (1, 1) double = 10;
|
minDimension (1, 1) double = 10;
|
||||||
@@ -9,7 +9,7 @@ function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, d
|
|||||||
minAlt (1, 1) double = 1;
|
minAlt (1, 1) double = 1;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Produce random bounds based on region type
|
% Produce random bounds based on region type
|
||||||
|
|||||||
@@ -1,12 +1,12 @@
|
|||||||
function [obj, f] = plotWireframe(obj, ind, f)
|
function [obj, f] = plotWireframe(obj, ind, f)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
ind (1, :) double = NaN;
|
ind (1, :) double = NaN;
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Create axes if they don't already exist
|
% Create axes if they don't already exist
|
||||||
@@ -19,10 +19,10 @@ function [obj, f] = plotWireframe(obj, ind, f)
|
|||||||
|
|
||||||
% Plot the boundaries of the geometry into 3D view
|
% Plot the boundaries of the geometry into 3D view
|
||||||
if isnan(ind)
|
if isnan(ind)
|
||||||
o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
|
o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 2);
|
||||||
else
|
else
|
||||||
hold(f.Children(1).Children(ind(1)), "on");
|
hold(f.Children(1).Children(ind(1)), "on");
|
||||||
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
|
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 2);
|
||||||
hold(f.Children(1).Children(ind(1)), "off");
|
hold(f.Children(1).Children(ind(1)), "off");
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function r = random(obj)
|
function r = random(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
obj (1, 1) {mustBeA(obj, "rectangularPrism")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
r (1, 3) double
|
r (1, 3) double
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function c = contains(obj, pos)
|
function c = contains(obj, pos)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'spherical')};
|
obj (1, 1) {mustBeA(obj, "spherical")};
|
||||||
pos (:, 3) double;
|
pos (:, 3) double;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function c = containsLine(obj, pos1, pos2)
|
function c = containsLine(obj, pos1, pos2)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'spherical')};
|
obj (1, 1) {mustBeA(obj, "spherical")};
|
||||||
pos1 (1, 3) double;
|
pos1 (1, 3) double;
|
||||||
pos2 (1, 3) double;
|
pos2 (1, 3) double;
|
||||||
end
|
end
|
||||||
|
|||||||
@@ -1,13 +1,13 @@
|
|||||||
function obj = initialize(obj, center, radius, tag, label)
|
function obj = initialize(obj, center, radius, tag, label)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'spherical')};
|
obj (1, 1) {mustBeA(obj, "spherical")};
|
||||||
center (1, 3) double;
|
center (1, 3) double;
|
||||||
radius (1, 1) double;
|
radius (1, 1) double;
|
||||||
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
|
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
|
||||||
label (1, 1) string = "";
|
label (1, 1) string = "";
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'spherical')};
|
obj (1, 1) {mustBeA(obj, "spherical")};
|
||||||
end
|
end
|
||||||
|
|
||||||
obj.tag = tag;
|
obj.tag = tag;
|
||||||
|
|||||||
@@ -1,12 +1,12 @@
|
|||||||
function [obj, f] = plotWireframe(obj, ind, f)
|
function [obj, f] = plotWireframe(obj, ind, f)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'spherical')};
|
obj (1, 1) {mustBeA(obj, "spherical")};
|
||||||
ind (1, :) double = NaN;
|
ind (1, :) double = NaN;
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
obj (1, 1) {mustBeA(obj, 'spherical')};
|
obj (1, 1) {mustBeA(obj, "spherical")};
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
|
||||||
end
|
end
|
||||||
|
|
||||||
% Create axes if they don't already exist
|
% Create axes if they don't already exist
|
||||||
@@ -25,10 +25,10 @@ function [obj, f] = plotWireframe(obj, ind, f)
|
|||||||
|
|
||||||
% Plot the boundaries of the geometry into 3D view
|
% Plot the boundaries of the geometry into 3D view
|
||||||
if isnan(ind)
|
if isnan(ind)
|
||||||
o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
|
o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 2);
|
||||||
else
|
else
|
||||||
hold(f.Children(1).Children(ind(1)), "on");
|
hold(f.Children(1).Children(ind(1)), "on");
|
||||||
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 1);
|
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 1);
|
||||||
hold(f.Children(1).Children(ind(1)), "off");
|
hold(f.Children(1).Children(ind(1)), "off");
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function r = random(obj)
|
function r = random(obj)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
obj (1, 1) {mustBeA(obj, 'spherical')};
|
obj (1, 1) {mustBeA(obj, "spherical")};
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
r (1, 3) double
|
r (1, 3) double
|
||||||
|
|||||||
@@ -16,7 +16,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
|
|||||||
protectedRange = 0;
|
protectedRange = 0;
|
||||||
|
|
||||||
%% Test iterations
|
%% Test iterations
|
||||||
csvPath = fullfile(matlab.project.rootProject().RootFolder, 'test', 'testIterations.csv');
|
csvPath = fullfile(matlab.project.rootProject().RootFolder, "test", "testIterations.csv");
|
||||||
end
|
end
|
||||||
|
|
||||||
methods (TestMethodSetup)
|
methods (TestMethodSetup)
|
||||||
@@ -37,20 +37,21 @@ classdef parametricTestSuite < matlab.unittest.TestCase
|
|||||||
|
|
||||||
% File input validation
|
% File input validation
|
||||||
assert(isfile(csvPath), "%s is not a valid filepath.");
|
assert(isfile(csvPath), "%s is not a valid filepath.");
|
||||||
assert(endsWith(csvPath, '.csv'), "%s is not a CSV file.");
|
assert(endsWith(csvPath, ".csv"), "%s is not a CSV file.");
|
||||||
|
|
||||||
% Read file
|
% Read file
|
||||||
csv = readtable(csvPath, 'TextType', 'string', 'NumHeaderLines', 0, "VariableNamingRule", "Preserve");
|
csv = readtable(csvPath, "TextType", "String", "NumHeaderLines", 0, "VariableNamingRule", "Preserve");
|
||||||
csv.Properties.VariableNames = ["timestep", "maxIter", "minAlt", "discretizationStep", "sensorPerformanceMinimum", "initialStepSize", "barrierGain", "barrierExponent", "numAgents", "collisionRadius", "comRange", "alphaDist", "betaDist", "alphaTilt", "betaTilt"];
|
csv.Properties.VariableNames = ["timestep", "maxIter", "minAlt", "discretizationStep", "sensorPerformanceMinimum", "initialStepSize", "barrierGain", "barrierExponent", "numObstacles", "numAgents", "collisionRadius", "comRange", "alphaDist", "betaDist", "alphaTilt", "betaTilt"];
|
||||||
|
|
||||||
for ii = 1:size(csv.Properties.VariableNames, 2)
|
for ii = 1:size(csv.Properties.VariableNames, 2)
|
||||||
csv.(csv.Properties.VariableNames{ii}) = cell2mat(cellfun(@(x) str2num(x), csv.(csv.Properties.VariableNames{ii}), 'UniformOutput', false));
|
csv.(csv.Properties.VariableNames{ii}) = cell2mat(cellfun(@(x) str2num(x), csv.(csv.Properties.VariableNames{ii}), "UniformOutput", false));
|
||||||
end
|
end
|
||||||
|
|
||||||
% Put params into standard structure
|
% Put params into standard structure
|
||||||
params = struct('timestep', csv.timestep, 'maxIter', csv.maxIter, 'minAlt', csv.minAlt, 'discretizationStep', csv.discretizationStep, ...
|
params = struct("timestep", csv.timestep, "maxIter", csv.maxIter, "minAlt", csv.minAlt, "discretizationStep", csv.discretizationStep, ...
|
||||||
'sensorPerformanceMinimum', csv.sensorPerformanceMinimum, 'initialStepSize', csv.initialStepSize, 'barrierGain', csv.barrierGain, 'barrierExponent', csv.barrierExponent, ...
|
"sensorPerformanceMinimum", csv.sensorPerformanceMinimum, "initialStepSize", csv.initialStepSize, ...
|
||||||
'numAgents', csv.numAgents, 'collisionRadius', csv.collisionRadius, 'comRange', csv.comRange, 'alphaDist', csv.alphaDist, 'betaDist', csv.betaDist, 'alphaTilt', csv.alphaTilt, 'betaTilt', csv.betaTilt);
|
"barrierGain", csv.barrierGain, "barrierExponent", csv.barrierExponent, "numObstacles", csv.numObstacles,...
|
||||||
|
"numAgents", csv.numAgents, "collisionRadius", csv.collisionRadius, "comRange", csv.comRange, "alphaDist", csv.alphaDist, "betaDist", csv.betaDist, "alphaTilt", csv.alphaTilt, "betaTilt", csv.betaTilt);
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|||||||
@@ -1,3 +1,3 @@
|
|||||||
timestep, maxIter, minAlt, discretizationStep, sensorPerformanceMinimum, initialStepSize, barrierGain, barrierExponent, numAgents, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt
|
timestep, maxIter, minAlt, discretizationStep, sensorPerformanceMinimum, initialStepSize, barrierGain, barrierExponent, numObstacles, numAgents, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt
|
||||||
1, 10, 1, 0.01, 1e-6, 0.2, 100, 3, 5, "0.1, 0.1, 0.1, 0.1, 0.1", "2.5, 2.5, 2.5, 2.5, 2.5", "3, 3, 3, 3, 3", "15, 15, 15, 15, 15", "3, 3, 3, 3, 3", "3, 3, 3, 3, 3"
|
1, 10, 1, 0.01, 1e-6, 0.2, 100, 3, 4, 5, "0.1, 0.1, 0.1, 0.1, 0.1", "2.5, 2.5, 2.5, 2.5, 2.5", "3, 3, 3, 3, 3", "15, 15, 15, 15, 15", "3, 3, 3, 3, 3", "3, 3, 3, 3, 3"
|
||||||
1, 10, 1, 0.01, 1e-6, 0.2, 100, 3, 5, "0.1, 0.1, 0.1, 0.1, 0.1", "3.5, 3.5, 3.5, 3.5, 3.5", "15, 15, 15, 15, 15", "30, 30, 30, 30, 30", "15, 15, 15, 15, 15", "3, 3, 3, 3, 3"
|
1, 10, 1, 0.01, 1e-6, 0.2, 100, 3, 4, 5, "0.1, 0.1, 0.1, 0.1, 0.1", "3.5, 3.5, 3.5, 3.5, 3.5", "15, 15, 15, 15, 15", "30, 30, 30, 30, 30", "15, 15, 15, 15, 15", "3, 3, 3, 3, 3"
|
||||||
|
|||||||
|
@@ -88,7 +88,7 @@ classdef test_miSim < matlab.unittest.TestCase
|
|||||||
|
|
||||||
methods (Test)
|
methods (Test)
|
||||||
% Test methods
|
% Test methods
|
||||||
function misim_initialization(tc)
|
function miSim_initialization(tc)
|
||||||
% randomly create obstacles
|
% randomly create obstacles
|
||||||
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
|
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
|
||||||
tc.obstacles = cell(nGeom, 1);
|
tc.obstacles = cell(nGeom, 1);
|
||||||
@@ -123,7 +123,7 @@ classdef test_miSim < matlab.unittest.TestCase
|
|||||||
candidatePos(3) = tc.minAlt + rand * 3; % place agents at decent altitudes for sensing
|
candidatePos(3) = tc.minAlt + rand * 3; % place agents at decent altitudes for sensing
|
||||||
end
|
end
|
||||||
else
|
else
|
||||||
candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
|
candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.commsRanges(ii)/sqrt(2));
|
||||||
candidatePos(3) = tc.minAlt + rand * 3; % place agents at decent altitudes for sensing
|
candidatePos(3) = tc.minAlt + rand * 3; % place agents at decent altitudes for sensing
|
||||||
end
|
end
|
||||||
|
|
||||||
@@ -144,7 +144,7 @@ classdef test_miSim < matlab.unittest.TestCase
|
|||||||
% graph between the agents
|
% graph between the agents
|
||||||
connections = false(1, ii - 1);
|
connections = false(1, ii - 1);
|
||||||
for jj = 1:(ii - 1)
|
for jj = 1:(ii - 1)
|
||||||
if norm(tc.agents{jj}.pos - candidatePos) <= tc.comRange
|
if norm(tc.agents{jj}.pos - candidatePos) <= min(tc.commsRanges([ii, jj]))
|
||||||
% Check new agent position against all existing
|
% Check new agent position against all existing
|
||||||
% agent positions for communications range
|
% agent positions for communications range
|
||||||
connections(jj) = true;
|
connections(jj) = true;
|
||||||
@@ -170,7 +170,7 @@ classdef test_miSim < matlab.unittest.TestCase
|
|||||||
tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
|
tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
|
||||||
|
|
||||||
% Initialize candidate agent
|
% Initialize candidate agent
|
||||||
newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.comRange, tc.maxIter, tc.initialStepSize);
|
newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize);
|
||||||
|
|
||||||
% Make sure candidate agent doesn't collide with
|
% Make sure candidate agent doesn't collide with
|
||||||
% domain
|
% domain
|
||||||
@@ -220,7 +220,7 @@ classdef test_miSim < matlab.unittest.TestCase
|
|||||||
% Initialize the simulation
|
% Initialize the simulation
|
||||||
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
|
tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
|
||||||
end
|
end
|
||||||
function misim_run(tc)
|
function miSim_run(tc)
|
||||||
% randomly create obstacles
|
% randomly create obstacles
|
||||||
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
|
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
|
||||||
tc.obstacles = cell(nGeom, 1);
|
tc.obstacles = cell(nGeom, 1);
|
||||||
@@ -254,7 +254,7 @@ classdef test_miSim < matlab.unittest.TestCase
|
|||||||
candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, tc.minAlt + rand * (tc.alphaDistMax * (1.1) - 0.5)]); % place agents at decent altitudes for sensing
|
candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, tc.minAlt + rand * (tc.alphaDistMax * (1.1) - 0.5)]); % place agents at decent altitudes for sensing
|
||||||
end
|
end
|
||||||
else
|
else
|
||||||
candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
|
candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.commsRanges(ii)/sqrt(2));
|
||||||
candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, tc.minAlt + rand * (tc.alphaDistMax * (1.1) - 0.5)]); % place agents at decent altitudes for sensing
|
candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, tc.minAlt + rand * (tc.alphaDistMax * (1.1) - 0.5)]); % place agents at decent altitudes for sensing
|
||||||
end
|
end
|
||||||
|
|
||||||
@@ -275,7 +275,7 @@ classdef test_miSim < matlab.unittest.TestCase
|
|||||||
% graph between the agents
|
% graph between the agents
|
||||||
connections = false(1, ii - 1);
|
connections = false(1, ii - 1);
|
||||||
for jj = 1:(ii - 1)
|
for jj = 1:(ii - 1)
|
||||||
if norm(tc.agents{jj}.pos - candidatePos) <= tc.comRange
|
if norm(tc.agents{jj}.pos - candidatePos) <= min(tc.commsRanges([ii, jj]))
|
||||||
% Check new agent position against all existing
|
% Check new agent position against all existing
|
||||||
% agent positions for communications range
|
% agent positions for communications range
|
||||||
connections(jj) = true;
|
connections(jj) = true;
|
||||||
@@ -303,7 +303,7 @@ classdef test_miSim < matlab.unittest.TestCase
|
|||||||
tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
|
tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
|
||||||
|
|
||||||
% Initialize candidate agent
|
% Initialize candidate agent
|
||||||
newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.comRange, tc.maxIter, tc.initialStepSize);
|
newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize);
|
||||||
|
|
||||||
% Make sure candidate agent doesn't collide with
|
% Make sure candidate agent doesn't collide with
|
||||||
% domain
|
% domain
|
||||||
@@ -389,9 +389,9 @@ classdef test_miSim < matlab.unittest.TestCase
|
|||||||
|
|
||||||
centerIdx = floor(size(tc.testClass.partitioning, 1) / 2);
|
centerIdx = floor(size(tc.testClass.partitioning, 1) / 2);
|
||||||
tc.verifyEqual(tc.testClass.partitioning(centerIdx, centerIdx:(centerIdx + 2)), [2, 3, 1]); % all three near center
|
tc.verifyEqual(tc.testClass.partitioning(centerIdx, centerIdx:(centerIdx + 2)), [2, 3, 1]); % all three near center
|
||||||
tc.verifyLessThan(sum(tc.testClass.partitioning == 1, 'all'), sum(tc.testClass.partitioning == 0, 'all')); % more non-assignments than partition 1 assignments
|
tc.verifyLessThan(sum(tc.testClass.partitioning == 1, "all"), sum(tc.testClass.partitioning == 0, "all")); % more non-assignments than partition 1 assignments
|
||||||
tc.verifyLessThan(sum(tc.testClass.partitioning == 2, 'all'), sum(tc.testClass.partitioning == 1, 'all')); % more partition 1 assignments than partition 2 assignments
|
tc.verifyLessThan(sum(tc.testClass.partitioning == 2, "all"), sum(tc.testClass.partitioning == 1, "all")); % more partition 1 assignments than partition 2 assignments
|
||||||
tc.verifyLessThan(sum(tc.testClass.partitioning == 3, 'all'), sum(tc.testClass.partitioning == 2, 'all')); % more partition 3 assignments than partition 2 assignments
|
tc.verifyLessThan(sum(tc.testClass.partitioning == 3, "all"), sum(tc.testClass.partitioning == 2, "all")); % more partition 3 assignments than partition 2 assignments
|
||||||
tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1; 2; 3;]);
|
tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1; 2; 3;]);
|
||||||
end
|
end
|
||||||
function test_single_partition(tc)
|
function test_single_partition(tc)
|
||||||
@@ -414,7 +414,7 @@ classdef test_miSim < matlab.unittest.TestCase
|
|||||||
close(tc.testClass.fPerf);
|
close(tc.testClass.fPerf);
|
||||||
|
|
||||||
tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]);
|
tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]);
|
||||||
tc.verifyLessThan(sum(tc.testClass.partitioning == 1, 'all'), sum(tc.testClass.partitioning == 0, 'all'));
|
tc.verifyLessThan(sum(tc.testClass.partitioning == 1, "all"), sum(tc.testClass.partitioning == 0, "all"));
|
||||||
end
|
end
|
||||||
function test_single_agent_gradient_ascent(tc)
|
function test_single_agent_gradient_ascent(tc)
|
||||||
% make basic domain
|
% make basic domain
|
||||||
|
|||||||
@@ -41,21 +41,21 @@ classdef test_sigmoidSensor < matlab.unittest.TestCase
|
|||||||
|
|
||||||
% Anticipate perfect performance for a point directly below and
|
% Anticipate perfect performance for a point directly below and
|
||||||
% extremely close
|
% extremely close
|
||||||
tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], 0, [0, 0, 0]), 1, 'RelTol', 1e-3);
|
tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], [0, 0, 0]), 1, "RelTol", 1e-3);
|
||||||
% It looks like mu_t can max out at really low values like 0.37
|
% It looks like mu_t can max out at really low values like 0.37
|
||||||
% when alphaTilt and betaTilt are small, which seems wrong
|
% when alphaTilt and betaTilt are small, which seems wrong
|
||||||
|
|
||||||
% Performance at nadir point, distance alphaDist should be 1/2 exactly
|
% Performance at nadir point, distance alphaDist should be 1/2 exactly
|
||||||
tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, alphaDist], 0, [0, 0, 0]), 1/2);
|
tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, alphaDist],[0, 0, 0]), 1/2);
|
||||||
|
|
||||||
% Performance at (almost) 0 distance, alphaTilt should be 1/2
|
% Performance at (almost) 0 distance, alphaTilt should be 1/2
|
||||||
tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], 0, [tand(alphaTilt)*h, 0, 0]), 1/2, 'RelTol', 1e-3);
|
tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], [tand(alphaTilt)*h, 0, 0]), 1/2, "RelTol", 1e-3);
|
||||||
|
|
||||||
% Performance at great distance should be 0
|
% Performance at great distance should be 0
|
||||||
tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, 10], 0, [0, 0, 0]), 0, 'AbsTol', 1e-9);
|
tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, 10], [0, 0, 0]), 0, "AbsTol", 1e-9);
|
||||||
|
|
||||||
% Performance at great tilt should be 0
|
% Performance at great tilt should be 0
|
||||||
tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], 0, [5, 5, 0]), 0, 'AbsTol', 1e-9);
|
tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], [5, 5, 0]), 0, "AbsTol", 1e-9);
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|||||||
@@ -1,9 +1,9 @@
|
|||||||
function f = firstPlotSetup(f)
|
function f = firstPlotSetup(f)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
|
||||||
end
|
end
|
||||||
if isempty(f.CurrentAxes)
|
if isempty(f.CurrentAxes)
|
||||||
tiledlayout(f, 5, 5, "TileSpacing", "tight", "Padding", "compact");
|
tiledlayout(f, 5, 5, "TileSpacing", "tight", "Padding", "compact");
|
||||||
@@ -24,14 +24,14 @@ function f = firstPlotSetup(f)
|
|||||||
grid(f.Children(1).Children(1), "off");
|
grid(f.Children(1).Children(1), "off");
|
||||||
view(f.Children(1).Children(1), 0, 90);
|
view(f.Children(1).Children(1), 0, 90);
|
||||||
title(f.Children(1).Children(1), "Network Graph");
|
title(f.Children(1).Children(1), "Network Graph");
|
||||||
set(f.Children(1).Children(1), 'XTickLabelMode', 'manual');
|
set(f.Children(1).Children(1), "XTickLabelMode", "manual");
|
||||||
set(f.Children(1).Children(1), 'YTickLabelMode', 'manual');
|
set(f.Children(1).Children(1), "YTickLabelMode", "manual");
|
||||||
set(f.Children(1).Children(1), 'XTickLabel', {});
|
set(f.Children(1).Children(1), "XTickLabel", {});
|
||||||
set(f.Children(1).Children(1), 'YTickLabel', {});
|
set(f.Children(1).Children(1), "YTickLabel", {});
|
||||||
set(f.Children(1).Children(1), 'XTick', []);
|
set(f.Children(1).Children(1), "XTick", []);
|
||||||
set(f.Children(1).Children(1), 'YTick', []);
|
set(f.Children(1).Children(1), "YTick", []);
|
||||||
set(f.Children(1).Children(1), 'XColor', 'none');
|
set(f.Children(1).Children(1), "XColor", "none");
|
||||||
set(f.Children(1).Children(1), 'YColor', 'none');
|
set(f.Children(1).Children(1), "YColor", "none");
|
||||||
|
|
||||||
% Top-down view
|
% Top-down view
|
||||||
nexttile(22, [1, 1]);
|
nexttile(22, [1, 1]);
|
||||||
@@ -68,11 +68,11 @@ function f = firstPlotSetup(f)
|
|||||||
view(f.Children(1).Children(1), 0, 90);
|
view(f.Children(1).Children(1), 0, 90);
|
||||||
xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y");
|
xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y");
|
||||||
title(f.Children(1).Children(1), "Domain Partitioning");
|
title(f.Children(1).Children(1), "Domain Partitioning");
|
||||||
set(f.Children(1).Children(1), 'XTickLabelMode', 'manual');
|
set(f.Children(1).Children(1), "XTickLabelMode", "manual");
|
||||||
set(f.Children(1).Children(1), 'YTickLabelMode', 'manual');
|
set(f.Children(1).Children(1), "YTickLabelMode", "manual");
|
||||||
set(f.Children(1).Children(1), 'XTickLabel', {});
|
set(f.Children(1).Children(1), "XTickLabel", {});
|
||||||
set(f.Children(1).Children(1), 'YTickLabel', {});
|
set(f.Children(1).Children(1), "YTickLabel", {});
|
||||||
set(f.Children(1).Children(1), 'XTick', []);
|
set(f.Children(1).Children(1), "XTick", []);
|
||||||
set(f.Children(1).Children(1), 'YTick', []);
|
set(f.Children(1).Children(1), "YTick", []);
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
@@ -7,9 +7,9 @@ function f = objectiveFunctionWrapper(center, sigma)
|
|||||||
sigma (2, 2) double = eye(2);
|
sigma (2, 2) double = eye(2);
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
f (1, 1) {mustBeA(f, 'function_handle')};
|
f (1, 1) {mustBeA(f, "function_handle")};
|
||||||
end
|
end
|
||||||
|
|
||||||
f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), sigma), 1:size(center,1), 'UniformOutput', false)), 2);
|
f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), sigma), 1:size(center,1), "UniformOutput", false)), 2);
|
||||||
|
|
||||||
end
|
end
|
||||||
@@ -1,6 +1,6 @@
|
|||||||
function c = agentsCrowdObjective(objective, positions, protectedRange)
|
function c = agentsCrowdObjective(objective, positions, protectedRange)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
objective (1, 1) {mustBeA(objective, 'sensingObjective')};
|
objective (1, 1) {mustBeA(objective, "sensingObjective")};
|
||||||
positions (:, 3) double; % this could be expanded to handle n obstacles in 1 call
|
positions (:, 3) double; % this could be expanded to handle n obstacles in 1 call
|
||||||
protectedRange (1, 1) double;
|
protectedRange (1, 1) double;
|
||||||
end
|
end
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function mustBeGeometry(geometry)
|
function mustBeGeometry(geometry)
|
||||||
validGeometries = ["rectangularPrism"; "spherical"];
|
validGeometries = ["rectangularPrism"; "spherical"];
|
||||||
if isa(geometry, 'cell')
|
if isa(geometry, "cell")
|
||||||
for ii = 1:size(geometry, 1)
|
for ii = 1:size(geometry, 1)
|
||||||
assert(any(arrayfun(@(x) isa(geometry{ii}, x), 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
|
end
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function mustBeSensor(sensorModel)
|
function mustBeSensor(sensorModel)
|
||||||
validSensorModels = ["fixedCardinalSensor"; "sigmoidSensor";];
|
validSensorModels = ["fixedCardinalSensor"; "sigmoidSensor";];
|
||||||
if isa(sensorModel, 'cell')
|
if isa(sensorModel, "cell")
|
||||||
for ii = 1:size(sensorModel, 1)
|
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);
|
assert(any(arrayfun(@(x) isa(sensorModel{ii}, x), validSensorModels)), "Sensor in index %d is not a valid sensor class", ii);
|
||||||
end
|
end
|
||||||
|
|||||||
@@ -8,9 +8,9 @@ function c = domainContainsObstacle(domain, obstacle)
|
|||||||
end
|
end
|
||||||
|
|
||||||
switch class(domain)
|
switch class(domain)
|
||||||
case 'rectangularPrism'
|
case "rectangularPrism"
|
||||||
switch class(obstacle)
|
switch class(obstacle)
|
||||||
case 'rectangularPrism'
|
case "rectangularPrism"
|
||||||
c = all(domain.minCorner <= obstacle.minCorner) && all(domain.maxCorner >= obstacle.maxCorner);
|
c = all(domain.minCorner <= obstacle.minCorner) && all(domain.maxCorner >= obstacle.maxCorner);
|
||||||
otherwise
|
otherwise
|
||||||
error("%s not implemented for obstacles of class %s", coder.mfunctionname, class(domain));
|
error("%s not implemented for obstacles of class %s", coder.mfunctionname, class(domain));
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function c = obstacleCoversObjective(objective, obstacle)
|
function c = obstacleCoversObjective(objective, obstacle)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
objective (1, 1) {mustBeA(objective, 'sensingObjective')};
|
objective (1, 1) {mustBeA(objective, "sensingObjective")};
|
||||||
obstacle (1, 1) {mustBeGeometry}; % this could be expanded to handle n obstacles in 1 call
|
obstacle (1, 1) {mustBeGeometry}; % this could be expanded to handle n obstacles in 1 call
|
||||||
end
|
end
|
||||||
arguments (Output)
|
arguments (Output)
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
function c = obstacleCrowdsObjective(objective, obstacle, protectedRange)
|
function c = obstacleCrowdsObjective(objective, obstacle, protectedRange)
|
||||||
arguments (Input)
|
arguments (Input)
|
||||||
objective (1, 1) {mustBeA(objective, 'sensingObjective')};
|
objective (1, 1) {mustBeA(objective, "sensingObjective")};
|
||||||
obstacle (1, 1) {mustBeGeometry}; % this could be expanded to handle n obstacles in 1 call
|
obstacle (1, 1) {mustBeGeometry}; % this could be expanded to handle n obstacles in 1 call
|
||||||
protectedRange (1, 1) double;
|
protectedRange (1, 1) double;
|
||||||
end
|
end
|
||||||
|
|||||||
Reference in New Issue
Block a user