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

2
@agent/agent.m
View File

@@ -6,6 +6,8 @@ classdef agent
% State
lastPos = NaN(1, 3); % position from previous timestep
pos = NaN(1, 3); % current position
vel = zeros(1, 3); % velocity (double-integrator mode)
lastVel = zeros(1, 3); % pre-step velocity (double-integrator mode)
% Sensor
sensorModel;

2
@agent/initialize.m
View File

@@ -15,6 +15,8 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
end
obj.pos = pos;
obj.vel = zeros(1, 3);
obj.lastVel = zeros(1, 3);
obj.collisionGeometry = collisionGeometry;
obj.sensorModel = sensorModel;
obj.label = label;

36
@agent/run.m
View File

@@ -1,4 +1,4 @@
function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useDoubleIntegrator, dampingCoeff, dt)
arguments (Input)
obj (1, 1) {mustBeA(obj, "agent")};
domain (1, 1) {mustBeGeometry};
@@ -6,6 +6,9 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
timestepIndex (1, 1) double;
index (1, 1) double;
agents (:, 1) {mustBeA(agents, "cell")};
useDoubleIntegrator (1, 1) logical = false;
dampingCoeff (1, 1) double = 2.0;
dt (1, 1) double = 1.0;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, "agent")};
@@ -75,19 +78,26 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
gradNorm = norm(gradC);
% Compute unconstrained next position.
% Guard against near-zero gradient: when sensor performance is saturated
% or near-zero across the whole partition, rateFactor -> Inf and pNext
% explodes. Stay put instead.
if gradNorm < 1e-100
pNext = obj.pos;
else
pNext = obj.pos + (targetRate / gradNorm) * gradC;
end
% Move to next position
% Compute unconstrained next state
obj.lastPos = obj.pos;
obj.pos = pNext;
if useDoubleIntegrator
% Double-integrator: gradient produces desired acceleration with damping
obj.lastVel = obj.vel;
if gradNorm < 1e-100
a_gradient = zeros(1, 3);
else
% Scale so steady-state step ≈ targetRate (matching SI behavior)
a_gradient = (targetRate * dampingCoeff / (gradNorm * dt)) * gradC;
end
% Semi-implicit Euler: unconditionally stable for any dampingCoeff and dt
obj.vel = (obj.vel + a_gradient * dt) / (1 + dampingCoeff * dt);
obj.pos = obj.lastPos + obj.vel * dt;
else
% Single-integrator: gradient directly sets position step
if gradNorm >= 1e-100
obj.pos = obj.pos + (targetRate / gradNorm) * gradC;
end
end
% Reinitialize collision geometry in the new position
d = obj.pos - obj.collisionGeometry.center;

56
@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
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
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
end
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");

4
aerpaw/config/scenario.csv
View File

@@ -1,2 +1,2 @@
timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax
5, 100, 30.0, 0.1, 2.0, 2.0, 100, 3, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0"
timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff
5, 100, 30.0, 0.1, 2.0, 2.0, 100, 3, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0
1 timestep maxIter minAlt discretizationStep protectedRange initialStepSize barrierGain barrierExponent collisionRadius comRange alphaDist betaDist alphaTilt betaTilt domainMin domainMax objectivePos objectiveVar sensorPerformanceMinimum initialPositions numObstacles obstacleMin obstacleMax useDoubleIntegrator dampingCoeff
2 5 100 30.0 0.1 2.0 2.0 100 3 5.0, 5.0 25.0, 25.0 80.0, 80.0 0.25, 0.25 5.0, 5.0 0.1, 0.1 0.0, 0.0, 0.0 80.0, 80.0, 80.0 55.0, 55.0 40, 25, 25, 40 0.15 15.0, 10.0, 40.0, 5.0, 10.0, 45.0 1 1.0, 25.0, 0.0 30.0, 30.0, 50.0 1 2.0

2
test/parametricTestSuite.m
View File

@@ -57,7 +57,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
end
% Set up simulation
tc.testClass = tc.testClass.initialize(tc.domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, tc.makePlots, tc.makeVideo);
tc.testClass = tc.testClass.initialize(tc.domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, tc.makePlots, tc.makeVideo, logical(params.useDoubleIntegrator), params.dampingCoeff);
% Save simulation parameters to output file
tc.testClass.writeInits();