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

fixed unit tests

Browse Source
This commit is contained in:
Kevin Dee
2026-01-13 23:16:41 -08:00
parent bcb3bc3da3
commit 2604711c78
15 changed files with 160 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
@miSim/constrainMotion.m
View File

@@ -14,7 +14,7 @@ function [obj] = constrainMotion(obj)
agents = [obj.agents{:}];
v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
if all(isnan(v)) || all(v == zeros(1, 3))
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
% constrained
return;
@@ -39,7 +39,7 @@ function [obj] = constrainMotion(obj)
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - agents(jj).pos);
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
b(kk) = obj.barrierGain * h(ii, jj)^3;
b(kk) = obj.barrierGain * h(ii, jj)^obj.barrierExponent;
kk = kk + 1;
end
end
@@ -54,7 +54,7 @@ function [obj] = constrainMotion(obj)
hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2;
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos);
b(kk) = obj.barrierGain * hObs(ii, jj)^3;
b(kk) = obj.barrierGain * hObs(ii, jj)^obj.barrierExponent;
kk = kk + 1;
end
@@ -67,37 +67,37 @@ function [obj] = constrainMotion(obj)
% X minimum
h_xMin = (agents(ii).pos(1) - obj.domain.minCorner(1)) - agents(ii).collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
b(kk) = obj.barrierGain * h_xMin^3;
b(kk) = obj.barrierGain * h_xMin^obj.barrierExponent;
kk = kk + 1;
% X maximum
h_xMax = (obj.domain.maxCorner(1) - agents(ii).pos(1)) - agents(ii).collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
b(kk) = obj.barrierGain * h_xMax^3;
b(kk) = obj.barrierGain * h_xMax^obj.barrierExponent;
kk = kk + 1;
% Y minimum
h_yMin = (agents(ii).pos(2) - obj.domain.minCorner(2)) - agents(ii).collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
b(kk) = obj.barrierGain * h_yMin^3;
b(kk) = obj.barrierGain * h_yMin^obj.barrierExponent;
kk = kk + 1;
% Y maximum
h_yMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
b(kk) = obj.barrierGain * h_yMax^3;
b(kk) = obj.barrierGain * h_yMax^obj.barrierExponent;
kk = kk + 1;
% Z minimum
h_zMin = (agents(ii).pos(3) - obj.domain.minCorner(3)) - agents(ii).collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
b(kk) = obj.barrierGain * h_zMin^3;
b(kk) = obj.barrierGain * h_zMin^obj.barrierExponent;
kk = kk + 1;
% Z maximum
h_zMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
b(kk) = obj.barrierGain * h_zMax^3;
b(kk) = obj.barrierGain * h_zMax^obj.barrierExponent;
kk = kk + 1;
end
@@ -114,11 +114,7 @@ function [obj] = constrainMotion(obj)
A(kk, (3 * ii - 2):(3 * ii)) = 2 * (agents(ii).pos - agents(jj).pos);
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
b(kk) = obj.barrierGain * hComms(ii, jj);
% dVNominal = v(ii, 1:3) - v(jj, 1:3); % nominal velocities
% h_dot_nom = -2 * (agents(ii).pos - agents(jj).pos) * dVNominal';
% b(kk) = -h_dot_nom + obj.barrierGain * hComms(ii, jj)^3;
b(kk) = obj.barrierGain * hComms(ii, jj)^obj.barrierExponent;
kk = kk + 1;
end

19
@miSim/initialize.m
View File

@@ -1,8 +1,10 @@
function obj = initialize(obj, domain, agents, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
domain (1, 1) {mustBeGeometry};
agents (:, 1) cell;
barrierGain (1, 1) double = 100;
barrierExponent (1, 1) double = 3;
minAlt (1, 1) double = 1;
timestep (:, 1) double = 0.05;
maxIter (:, 1) double = 1000;
@@ -24,6 +26,9 @@ function obj = initialize(obj, domain, agents, minAlt, timestep, maxIter, obstac
end
obj.makeVideo = makeVideo;
% Generate artifact(s) name
obj.artifactName = strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'));
% Define simulation time parameters
obj.timestep = timestep;
obj.timestepIndex = 0;
@@ -37,10 +42,9 @@ function obj = initialize(obj, domain, agents, minAlt, timestep, maxIter, obstac
% Add an additional obstacle spanning the domain's footprint to
% represent the minimum allowable altitude
obj.minAlt = minAlt;
if obj.minAlt > 0
if minAlt > 0
obj.obstacles{end + 1, 1} = rectangularPrism;
obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
end
% Define agents
@@ -61,6 +65,10 @@ function obj = initialize(obj, domain, agents, minAlt, timestep, maxIter, obstac
end
end
% Set CBF parameters
obj.barrierGain = barrierGain;
obj.barrierExponent = barrierExponent;
% Compute adjacency matrix and lesser neighbors
obj = obj.updateAdjacency();
obj = obj.lesserNeighbor();
@@ -83,4 +91,7 @@ function obj = initialize(obj, domain, agents, minAlt, timestep, maxIter, obstac
% Set up plots showing initialized state
obj = obj.plot();
% Run validations
obj.validate();
end

9
@miSim/miSim.m
View File

@@ -5,7 +5,6 @@ classdef miSim
properties (SetAccess = private, GetAccess = public)
timestep = NaN; % delta time interval for simulation iterations
timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
partitioningFreq = NaN; % number of simulation timesteps at which the partitioning routine is re-run
maxIter = NaN; % maximum number of simulation iterations
domain = rectangularPrism;
objective = sensingObjective;
@@ -16,9 +15,9 @@ classdef miSim
partitioning = NaN;
perf; % sensor performance timeseries array
performance = 0; % simulation performance timeseries vector
barrierGain = 100; % collision avoidance parameter
minAlt = 1; % minimum allowed altitude constraint
barrierGain = 100; % CBF gain parameter
barrierExponent = 3; % CBF exponent parameter
artifactName = "";
fPerf; % performance plot figure
end
@@ -55,7 +54,7 @@ classdef miSim
end
methods (Access = public)
[obj] = initialize(obj, domain, agents, timestep, partitoningFreq, maxIter, obstacles);
[obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
[obj] = run(obj);
[obj] = lesserNeighbor(obj);
[obj] = constrainMotion(obj);

3
@miSim/plot.m
View File

@@ -51,4 +51,7 @@ function obj = plot(obj)
% Plot h functions
obj = obj.plotH();
% Switch back to primary figure
figure(obj.f);
end

4
@miSim/setupVideoWriter.m
View File

@@ -7,9 +7,9 @@ function v = setupVideoWriter(obj)
end
if ispc || ismac
v = VideoWriter(fullfile('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'MPEG-4');
v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'MPEG-4');
elseif isunix
v = VideoWriter(fullfile('.', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'Motion JPEG AVI');
v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'Motion JPEG AVI');
end
v.FrameRate = 1 / obj.timestep;

10
@miSim/validate.m
View File

@@ -14,6 +14,14 @@ function validate(obj)
warning("Eliminated network connections that were necessary");
end
%%
%% Obstacle Validators
AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obj.obstacles), obj.agents, 'UniformOutput', false);
AO_collisions = vertcat(AO_collisions{:});
if any(AO_collisions)
[idx, idy] = find(AO_collisions);
for ii = 1:size(idx, 1)
error("Agent(s) %d colliding with obstacle(s) %d", idx(ii), idy(ii));
end
end
end

25
@miSim/writeParams.m Normal file
View File

@@ -0,0 +1,25 @@
function writeParams(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
arguments (Output)
end
% Collect agent parameters
collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents);
alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents);
alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents);
betaTilt = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
comRange = cellfun(@(x) x.commsGeometry.radius, obj.agents);
% Combine with simulation parameters
params = struct('timestep', obj.timestep, 'maxIter', obj.maxIter, 'minAlt', obj.obstacles{end}.maxCorner(3), 'discretizationStep', obj.domain.objective.discretizationStep, ...
'collisionRadius', collisionRadii, 'alphaDist', alphaDist, 'betaDist', betaDist, ...
'alphaTilt', alphaTilt, 'betaTilt', betaTilt, 'comRange', comRange);
% Save all parameters to output file
paramsFile = strcat(obj.artifactName, "_miSimParams");
paramsFile = fullfile(matlab.project.rootProject().RootFolder, 'sandbox', paramsFile);
save(paramsFile, "-struct", "params");
end