double integrator dynamics

@agent/agent.m

@@ -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;

@agent/initialize.m

@@ -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;

@agent/run.m

@@ -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.
+    % Compute unconstrained next state
-    % 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
     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;

@miSim/constrainMotion.m

@@ -8,14 +8,24 @@ function [obj] = constrainMotion(obj)
 
     nAgents = size(obj.agents, 1);
 
-    % Compute velocity matrix from unconstrained gradient-ascent step
+    % Compute current velocity and desired control input
-    v = zeros(nAgents, 3);
+    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")
+    end
-        % Agents are not attempting to move, so there is no motion to be
+    if ~obj.useDoubleIntegrator && (all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all"))
-        % constrained
+        % 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
+    % Double-integrator: transform QP from velocity to acceleration space.
-    vhat = reshape(v', 3 * nAgents, 1);
+    % 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);
+    [uNew, ~, exitflag] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
-    vNew = reshape(vNew, 3, nAgents)';
+    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
+    % Update agent state using the constrained control input
-    % GA using the constrained solution produced here
+    for ii = 1:size(uNew, 1)
-    for ii = 1:size(vNew, 1)
+        if obj.useDoubleIntegrator
-        obj.agents{ii}.pos = obj.agents{ii}.lastPos + vNew(ii, :) * obj.timestep;
+            % 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

@miSim/initialize.m

@@ -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));
 

@miSim/initializeFromCsv.m

@@ -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

@miSim/miSim.m

@@ -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

@miSim/run.m

@@ -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))];

@miSim/teardown.m

@@ -6,19 +6,22 @@ function obj = teardown(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    % Close plots
+    % % Close plots
-    close(obj.hf);
+    % close(obj.hf);
-    close(obj.fPerf);
+    % close(obj.fPerf);
-    close(obj.f);
+    % 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

@miSim/writeInits.m

@@ -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, ...
+                    "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ...
-                    "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
+                    "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");

aerpaw/config/scenario.csv

@@ -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
+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"
+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

test/parametricTestSuite.m

@@ -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();