added function to initialize sim from saved-off inits file
This commit is contained in:
@@ -0,0 +1,87 @@
|
||||
function obj = initializeFromInits(obj, initsPath)
|
||||
% INITIALIZEFROMINITS Initialize miSim from a saved simInits matfile.
|
||||
%
|
||||
% Loads all simulation parameters and initial agent states written by
|
||||
% writeInits(), reconstructs domain, objective, agents, and obstacles, then
|
||||
% calls the standard obj.initialize() method. Plots and video are disabled.
|
||||
%
|
||||
% Usage:
|
||||
% sim = sim.initializeFromInits('sandbox/2025_01_01_12_00_00_miSimInits.mat');
|
||||
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
initsPath (1, 1) string;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
end
|
||||
|
||||
inits = load(initsPath);
|
||||
|
||||
% ---- Build domain ------------------------------------------------------------
|
||||
dom = rectangularPrism;
|
||||
dom = dom.initialize([inits.domainMin; inits.domainMax], REGION_TYPE.DOMAIN, "Domain");
|
||||
|
||||
% ---- Build sensing objective -------------------------------------------------
|
||||
dom.objective = sensingObjective;
|
||||
% reshape guards against MATLAB flattening the 1×2×2 singleton dimension on load
|
||||
objSigma = reshape(inits.objectiveSigma, [1 2 2]);
|
||||
objFcn = objectiveFunctionWrapper(inits.objectivePos, objSigma);
|
||||
dom.objective = dom.objective.initialize(objFcn, dom, ...
|
||||
inits.discretizationStep, inits.protectedRange, inits.sensorPerformanceMinimum, ...
|
||||
inits.objectivePos, objSigma);
|
||||
|
||||
% ---- Build agents ------------------------------------------------------------
|
||||
numAgents = inits.numAgents;
|
||||
agentList = cell(numAgents, 1);
|
||||
for ii = 1:numAgents
|
||||
pos = inits.pos(ii, :);
|
||||
|
||||
sensor = sigmoidSensor;
|
||||
sensor = sensor.initialize(inits.alphaDist(ii), inits.betaDist(ii), ...
|
||||
inits.alphaTilt(ii), inits.betaTilt(ii));
|
||||
|
||||
geom = spherical;
|
||||
geom = geom.initialize(pos, inits.collisionRadius(ii), REGION_TYPE.COLLISION, ...
|
||||
sprintf("UAV %d Collision", ii));
|
||||
ag = agent;
|
||||
ag = ag.initialize(pos, geom, sensor, inits.comRange(ii), inits.maxIter, ...
|
||||
inits.initialStepSize(ii), sprintf("UAV %d", ii));
|
||||
agentList{ii} = ag;
|
||||
end
|
||||
|
||||
% ---- Build obstacles ---------------------------------------------------------
|
||||
numObstacles = inits.numObstacles;
|
||||
obstacleList = cell(numObstacles, 1);
|
||||
if numObstacles > 0
|
||||
for ii = 1:numObstacles
|
||||
obs = rectangularPrism;
|
||||
obs = obs.initialize([inits.obsMinCorners(ii, :); inits.obsMaxCorners(ii, :)], ...
|
||||
REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii));
|
||||
obstacleList{ii} = obs;
|
||||
end
|
||||
end
|
||||
|
||||
% ---- Optional backward-compat fields -----------------------------------------
|
||||
if isfield(inits, 'useDoubleIntegrator')
|
||||
useDoubleIntegrator = logical(inits.useDoubleIntegrator);
|
||||
else
|
||||
useDoubleIntegrator = false;
|
||||
end
|
||||
if isfield(inits, 'dampingCoeff')
|
||||
dampingCoeff = inits.dampingCoeff;
|
||||
else
|
||||
dampingCoeff = 2.0;
|
||||
end
|
||||
if isfield(inits, 'useFixedTopology')
|
||||
useFixedTopology = logical(inits.useFixedTopology);
|
||||
else
|
||||
useFixedTopology = false;
|
||||
end
|
||||
|
||||
% ---- Initialize simulation (plots and video disabled) ------------------------
|
||||
obj = obj.initialize(dom, agentList, inits.barrierGain, inits.barrierExponent, ...
|
||||
inits.minAlt, inits.timestep, inits.maxIter, obstacleList, ...
|
||||
false, false, useDoubleIntegrator, dampingCoeff, useFixedTopology);
|
||||
|
||||
end
|
||||
@@ -71,6 +71,7 @@ classdef miSim
|
||||
end
|
||||
[obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
|
||||
[obj] = initializeFromCsv(obj, csvPath);
|
||||
[obj] = initializeFromInits(obj, initsPath);
|
||||
[obj] = run(obj);
|
||||
[obj] = lesserNeighbor(obj);
|
||||
[obj] = constrainMotion(obj);
|
||||
|
||||
+14
-7
@@ -5,28 +5,35 @@ function writeInits(obj)
|
||||
arguments (Output)
|
||||
end
|
||||
|
||||
% User-supplied obstacles only: initialize() appends a floor obstacle at
|
||||
% the end when minAlt > 0, so exclude it here to avoid double-counting on
|
||||
% reconstruction (initializeFromInits re-adds the floor via minAlt).
|
||||
numInputObs = size(obj.obstacles, 1) - (obj.minAlt > 0);
|
||||
userObstacles = obj.obstacles(1:numInputObs);
|
||||
|
||||
% 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);
|
||||
betaTilt = cellfun(@(x) x.sensorModel.betaTilt, obj.agents);
|
||||
comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
|
||||
initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
|
||||
pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
|
||||
obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, obj.obstacles, 'UniformOutput', false));
|
||||
obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, obj.obstacles, 'UniformOutput', false));
|
||||
obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, userObstacles, 'UniformOutput', false));
|
||||
obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, userObstacles, 'UniformOutput', false));
|
||||
|
||||
% Combine with simulation parameters
|
||||
inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
|
||||
inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter + 1, "minAlt", obj.minAlt, ...
|
||||
"discretizationStep", obj.domain.objective.discretizationStep, "protectedRange", obj.domain.objective.protectedRange, ...
|
||||
"sensorPerformanceMinimum", obj.domain.objective.sensorPerformanceMinimum, "initialStepSize", initialStepSize, ...
|
||||
"barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", size(obj.obstacles, 1), ...
|
||||
"barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", numInputObs, ...
|
||||
"numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ...
|
||||
"useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ...
|
||||
"useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, "useFixedTopology", obj.useFixedTopology, ...
|
||||
"alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
|
||||
... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
|
||||
"pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ...
|
||||
"domainMin", obj.domain.minCorner, "domainMax", obj.domain.maxCorner, ...
|
||||
"obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ...
|
||||
"objectiveIntegral", sum(obj.domain.objective.values(:)));
|
||||
|
||||
@@ -34,4 +41,4 @@ function writeInits(obj)
|
||||
initsFile = strcat(obj.artifactName, "_miSimInits");
|
||||
initsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", initsFile);
|
||||
save(initsFile, "-struct", "inits");
|
||||
end
|
||||
end
|
||||
|
||||
@@ -728,6 +728,128 @@ classdef test_miSim < matlab.unittest.TestCase
|
||||
0, 0, 0, 1, 1, 1, 1;
|
||||
0, 0, 0, 0, 0, 1, 1; ]));
|
||||
end
|
||||
function miSim_initializeFromInits(tc)
|
||||
% Build a minimal valid simulation, write it to a matfile, reload
|
||||
% via initializeFromInits, assert round-trip consistency, then
|
||||
% delete the file. No plotting or video at any stage.
|
||||
|
||||
% Obstacles
|
||||
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
|
||||
tc.obstacles = cell(nGeom, 1);
|
||||
for ii = 1:nGeom
|
||||
badCandidate = true;
|
||||
while badCandidate
|
||||
tc.obstacles{ii} = rectangularPrism;
|
||||
tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, ...
|
||||
sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, ...
|
||||
tc.domain, tc.minAlt);
|
||||
if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
|
||||
badCandidate = false;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
% Agents
|
||||
nAgents = tc.minAgents;
|
||||
tc.agents = cell(nAgents, 1);
|
||||
tc.collisionRanges = tc.minCollisionRange + rand(nAgents, 1) * (tc.maxCollisionRange - tc.minCollisionRange);
|
||||
tc.commsRanges = tc.minCommsRange + rand(nAgents, 1) * (tc.maxCommsRange - tc.minCommsRange);
|
||||
|
||||
for ii = 1:nAgents
|
||||
initInvalid = true;
|
||||
while initInvalid
|
||||
if ii == 1
|
||||
candidatePos = tc.domain.random();
|
||||
candidatePos(3) = tc.minAlt + rand * 3;
|
||||
while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
|
||||
candidatePos = tc.domain.random();
|
||||
candidatePos(3) = tc.minAlt + rand * 3;
|
||||
end
|
||||
else
|
||||
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;
|
||||
end
|
||||
|
||||
if ~tc.domain.contains(candidatePos), continue; end
|
||||
if agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2), continue; end
|
||||
|
||||
% Connectivity check
|
||||
connections = false(1, ii - 1);
|
||||
for jj = 1:(ii - 1)
|
||||
if norm(tc.agents{jj}.pos - candidatePos) <= min(tc.commsRanges([ii, jj]))
|
||||
connections(jj) = true;
|
||||
for kk = 1:size(tc.obstacles, 1)
|
||||
if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
|
||||
connections(jj) = false;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
if ii ~= 1 && ~any(connections), continue; end
|
||||
|
||||
geom = spherical;
|
||||
geom = geom.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);
|
||||
tc.sensor = sigmoidSensor;
|
||||
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));
|
||||
newAgent = agent;
|
||||
newAgent = newAgent.initialize(candidatePos, geom, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize);
|
||||
|
||||
% Domain / obstacle / agent collision checks
|
||||
violation = false;
|
||||
for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
|
||||
if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
|
||||
violation = true; break;
|
||||
end
|
||||
end
|
||||
if violation, continue; end
|
||||
for kk = 1:size(tc.obstacles, 1)
|
||||
if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
|
||||
violation = true; break;
|
||||
end
|
||||
end
|
||||
if violation, continue; end
|
||||
for kk = 1:(ii - 1)
|
||||
if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
|
||||
violation = true; break;
|
||||
end
|
||||
end
|
||||
if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
|
||||
violation = true;
|
||||
end
|
||||
if violation, continue; end
|
||||
|
||||
initInvalid = false;
|
||||
tc.agents{ii} = newAgent;
|
||||
end
|
||||
end
|
||||
|
||||
% Initialize first sim (no plots / video)
|
||||
sim1 = miSim;
|
||||
sim1 = sim1.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, ...
|
||||
tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, false, false, ...
|
||||
tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
|
||||
|
||||
% Write inits and build file path
|
||||
sim1.writeInits();
|
||||
initsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", ...
|
||||
strcat(sim1.artifactName, "_miSimInits.mat"));
|
||||
|
||||
% Load via initializeFromInits
|
||||
sim2 = miSim;
|
||||
sim2 = sim2.initializeFromInits(initsFile);
|
||||
|
||||
% Assertions
|
||||
tc.assertEqual(size(sim2.agents, 1), size(sim1.agents, 1));
|
||||
tc.assertEqual(sim2.maxIter, sim1.maxIter);
|
||||
tc.assertEqual(sim2.barrierGain, sim1.barrierGain);
|
||||
|
||||
% Cleanup
|
||||
delete(initsFile);
|
||||
end
|
||||
end
|
||||
|
||||
methods
|
||||
|
||||
Reference in New Issue
Block a user