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

double integrator dynamics

Browse Source
This commit is contained in:
Kevin Dee
2026-03-13 15:33:53 -07:00
parent 102f23316d
commit f003528a9c
12 changed files with 115 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
@miSim/constrainMotion.m
View File

@@ -8,14 +8,24 @@ function [obj] = constrainMotion(obj)
nAgents = size(obj.agents, 1);
% Compute velocity matrix from unconstrained gradient-ascent step
v = zeros(nAgents, 3);
% Compute current velocity and desired control input
v = zeros(nAgents, 3); % current velocity (for drift term in DI mode)
u_desired = zeros(nAgents, 3); % desired control: velocity (SI) or acceleration (DI)
for ii = 1:nAgents
v(ii, :) = (obj.agents{ii}.pos - obj.agents{ii}.lastPos) ./ obj.timestep;
if obj.useDoubleIntegrator
v(ii, :) = obj.agents{ii}.lastVel;
u_desired(ii, :) = (obj.agents{ii}.vel - obj.agents{ii}.lastVel) / obj.timestep;
else
v(ii, :) = (obj.agents{ii}.pos - obj.agents{ii}.lastPos) ./ obj.timestep;
u_desired(ii, :) = v(ii, :);
end
end
if all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all")
% Agents are not attempting to move, so there is no motion to be
% constrained
if ~obj.useDoubleIntegrator && (all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all"))
% Single-integrator: agents are not attempting to move
return;
end
if obj.useDoubleIntegrator && all(u_desired == 0, "all") && all(v == 0, "all")
% Double-integrator: no desired acceleration and no existing velocity
return;
end
@@ -156,10 +166,18 @@ function [obj] = constrainMotion(obj)
end
obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
% Solve QP program generated earlier
vhat = reshape(v', 3 * nAgents, 1);
% Double-integrator: transform QP from velocity to acceleration space.
% Single-integrator constraint: A * v <= b
% Double-integrator: A * a <= (b - A * v_current) / dt
if obj.useDoubleIntegrator
v_flat = reshape(v', 3 * nAgents, 1);
b = (b - A * v_flat) / obj.timestep;
end
% Solve QP: minimize ||u - u_desired||²
uhat = reshape(u_desired', 3 * nAgents, 1);
H = 2 * eye(3 * nAgents);
f = -2 * vhat;
f = -2 * uhat;
% Update solution based on constraints
if coder.target('MATLAB')
@@ -169,8 +187,8 @@ function [obj] = constrainMotion(obj)
end
opt = optimoptions("quadprog", "Display", "off", "Algorithm", "active-set", "UseCodegenSolver", true);
x0 = zeros(size(H, 1), 1);
[vNew, ~, exitflag] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
vNew = reshape(vNew, 3, nAgents)';
[uNew, ~, exitflag] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
uNew = reshape(uNew, 3, nAgents)';
if exitflag < 0
% Infeasible or other hard failure: hold all agents at current positions
@@ -179,9 +197,9 @@ function [obj] = constrainMotion(obj)
else
fprintf("[constrainMotion] QP infeasible (exitflag=%d), holding positions\n", int16(exitflag));
end
vNew = zeros(nAgents, 3);
uNew = zeros(nAgents, 3);
elseif exitflag == 0
% Max iterations exceeded: use suboptimal solution already in vNew
% Max iterations exceeded: use suboptimal solution already in uNew
if coder.target('MATLAB')
warning("QP max iterations exceeded, using suboptimal solution.");
else
@@ -189,10 +207,16 @@ function [obj] = constrainMotion(obj)
end
end
% Update the "next position" that was previously set by unconstrained
% GA using the constrained solution produced here
for ii = 1:size(vNew, 1)
obj.agents{ii}.pos = obj.agents{ii}.lastPos + vNew(ii, :) * obj.timestep;
% Update agent state using the constrained control input
for ii = 1:size(uNew, 1)
if obj.useDoubleIntegrator
% uNew is constrained acceleration
obj.agents{ii}.vel = obj.agents{ii}.lastVel + uNew(ii, :) * obj.timestep;
obj.agents{ii}.pos = obj.agents{ii}.lastPos + obj.agents{ii}.vel * obj.timestep;
else
% uNew is constrained velocity
obj.agents{ii}.pos = obj.agents{ii}.lastPos + uNew(ii, :) * obj.timestep;
end
end
% Here we run this at the simulation level, but in reality there is no

11
@miSim/initialize.m
View File

@@ -1,4 +1,4 @@
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, useDoubleIntegrator, dampingCoeff)
arguments (Input)
obj (1, 1) {mustBeA(obj, "miSim")};
domain (1, 1) {mustBeGeometry};
@@ -11,6 +11,8 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
makePlots(1, 1) logical = true;
makeVideo (1, 1) logical = true;
useDoubleIntegrator (1, 1) logical = false;
dampingCoeff (1, 1) double = 2.0;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, "miSim")};
@@ -86,6 +88,10 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
obj.barrierExponent = barrierExponent;
obj.minAlt = minAlt;
% Set dynamics model
obj.useDoubleIntegrator = useDoubleIntegrator;
obj.dampingCoeff = dampingCoeff;
% Compute adjacency matrix and lesser neighbors
obj = obj.updateAdjacency();
obj = obj.lesserNeighbor();
@@ -120,6 +126,9 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
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);
% Initialize velocity history (zeros at t=0, all agents start at rest)
obj.velHist = zeros(size(obj.agents, 1), obj.maxIter + 1, 3);
% Initialize variable that will store barrier function values per timestep for analysis purposes
obj.barriers = NaN(obj.numBarriers, size(obj.times, 1));

15
@miSim/initializeFromCsv.m
View File

@@ -79,6 +79,18 @@ assert(numel(BETA_TILT_VEC) == numAgents, ...
numObstacles = scenario.numObstacles;
% Dynamics model (optional columns — backward compatible with older CSVs)
if isfield(scenario, 'useDoubleIntegrator')
USE_DOUBLE_INTEGRATOR = logical(scenario.useDoubleIntegrator);
else
USE_DOUBLE_INTEGRATOR = false;
end
if isfield(scenario, 'dampingCoeff')
DAMPING_COEFF = scenario.dampingCoeff;
else
DAMPING_COEFF = 2.0;
end
% ---- Build domain --------------------------------------------------------
dom = rectangularPrism;
dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
@@ -124,6 +136,7 @@ end
% ---- Initialise simulation (plots and video disabled) --------------------
obj = obj.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false);
MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false, ...
USE_DOUBLE_INTEGRATOR, DAMPING_COEFF);
end

3
@miSim/miSim.m
View File

@@ -18,6 +18,8 @@ classdef miSim
barrierGain = NaN; % CBF gain parameter
barrierExponent = NaN; % CBF exponent parameter
minAlt = 0; % minimum allowable altitude (m)
useDoubleIntegrator = false; % false = single-integrator, true = double-integrator dynamics
dampingCoeff = 2.0; % velocity-proportional damping for double-integrator mode
artifactName = "";
f; % main plotting tiled layout figure
fPerf; % performance plot figure
@@ -42,6 +44,7 @@ classdef miSim
performancePlot; % objects for sensor performance plot
posHist; % data for trail plot
velHist; % velocity history (double-integrator mode)
trailPlot; % objects for agent trail plot
% Indicies for various plot types in the main tiled layout figure

5
@miSim/run.m
View File

@@ -35,7 +35,7 @@ function [obj] = run(obj)
% Moving
% Iterate over agents to simulate their unconstrained motion
for jj = 1:size(obj.agents, 1)
obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents);
obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents, obj.useDoubleIntegrator, obj.dampingCoeff, obj.timestep);
end
% Adjust motion determined by unconstrained gradient ascent using
@@ -43,8 +43,9 @@ function [obj] = run(obj)
obj = constrainMotion(obj);
if coder.target('MATLAB')
% Update agent position history array
% Update agent position and velocity history arrays
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.velHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.vel, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
% Update total performance
obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];

15
@miSim/teardown.m
View File

@@ -6,19 +6,22 @@ function obj = teardown(obj)
obj (1, 1) {mustBeA(obj, "miSim")};
end
% Close plots
close(obj.hf);
close(obj.fPerf);
close(obj.f);
% % Close plots
% close(obj.hf);
% close(obj.fPerf);
% close(obj.f);
% Log results into matfile
histPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", strcat(obj.artifactName, "_miSimHist.mat"));
out = struct("agent", repmat(struct("pos", [], "perf", [], "sensor", struct("alphaDist", [], "betaDist", [], "alphaTilt", [], "betaTilt", []), "collisionRadius", [], "commsRadius", []), size(obj.agents)), "perf", [], "barriers", []);
out = struct("agent", repmat(struct("pos", [], "vel", [], "perf", [], "sensor", struct("alphaDist", [], "betaDist", [], "alphaTilt", [], "betaTilt", []), "collisionRadius", [], "commsRadius", []), size(obj.agents)), "perf", [], "barriers", [], "useDoubleIntegrator", [], "dampingCoeff", []);
out.perf = obj.performance(1:(end - 1));
out.barriers = [zeros(size(obj.barriers(1:end, 1), 1), 1), obj.barriers(1:end, 1:(end - 1))];
out.dampingCoeff = obj.dampingCoeff;
out.useDoubleIntegrator = obj.useDoubleIntegrator;
for ii = 1:size(obj.agents, 1)
out.agent(ii).pos = squeeze(obj.posHist(ii, 1:(end - 1), 1:3));
out.agent(ii).vel = squeeze(obj.velHist(ii, 1:(end - 1), 1:3));
out.agent(ii).perf = obj.agents{ii}.performance(1:(end - 2));
out.agent(ii).sensor.alphaDist = obj.agents{ii}.sensorModel.alphaDist;
out.agent(ii).sensor.betaDist = obj.agents{ii}.sensorModel.betaDist;
@@ -44,6 +47,8 @@ function obj = teardown(obj)
obj.performance = 0;
obj.barrierGain = NaN;
obj.barrierExponent = NaN;
obj.useDoubleIntegrator = false;
obj.dampingCoeff = 2.0;
obj.artifactName = "";
end

8
@miSim/writeInits.m
View File

@@ -15,16 +15,16 @@ function writeInits(obj)
initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
% Combine with simulation parameters
inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
"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), ...
"numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, "alphaDist", alphaDist, ...
"betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
"numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ...
"useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ...
"alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
"pos", pos);
"pos", pos); % still needs obstacle states and objective state
% Save all parameters to output file
initsFile = strcat(obj.artifactName, "_miSimInits");