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
|
||||
|
||||
Reference in New Issue
Block a user