From f003528a9cf920eb3a0c1924c777e01ee9abb526 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Fri, 13 Mar 2026 15:33:53 -0700 Subject: [PATCH] double integrator dynamics --- @agent/agent.m | 2 ++ @agent/initialize.m | 2 ++ @agent/run.m | 36 ++++++++++++++--------- @miSim/constrainMotion.m | 58 +++++++++++++++++++++++++++----------- @miSim/initialize.m | 11 +++++++- @miSim/initializeFromCsv.m | 15 +++++++++- @miSim/miSim.m | 3 ++ @miSim/run.m | 5 ++-- @miSim/teardown.m | 15 ++++++---- @miSim/writeInits.m | 8 +++--- aerpaw/config/scenario.csv | 4 +-- test/parametricTestSuite.m | 2 +- 12 files changed, 115 insertions(+), 46 deletions(-) diff --git a/@agent/agent.m b/@agent/agent.m index 6f75803..1fc1ecd 100644 --- a/@agent/agent.m +++ b/@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; diff --git a/@agent/initialize.m b/@agent/initialize.m index 37ada2b..84b2f8d 100644 --- a/@agent/initialize.m +++ b/@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; diff --git a/@agent/run.m b/@agent/run.m index 4cb7e76..9f818be 100644 --- a/@agent/run.m +++ b/@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. - % 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; diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 5705832..8992d12 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -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 diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 4cf8ca6..6f4c460 100644 --- a/@miSim/initialize.m +++ b/@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)); diff --git a/@miSim/initializeFromCsv.m b/@miSim/initializeFromCsv.m index 38ef7c4..682ae64 100644 --- a/@miSim/initializeFromCsv.m +++ b/@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 diff --git a/@miSim/miSim.m b/@miSim/miSim.m index 4afd034..769cc2c 100644 --- a/@miSim/miSim.m +++ b/@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 diff --git a/@miSim/run.m b/@miSim/run.m index e549935..ae5b734 100644 --- a/@miSim/run.m +++ b/@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))]; diff --git a/@miSim/teardown.m b/@miSim/teardown.m index ba1d21d..358f62b 100644 --- a/@miSim/teardown.m +++ b/@miSim/teardown.m @@ -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 \ No newline at end of file diff --git a/@miSim/writeInits.m b/@miSim/writeInits.m index 6f150d7..4f02b03 100644 --- a/@miSim/writeInits.m +++ b/@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, ... - "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"); diff --git a/aerpaw/config/scenario.csv b/aerpaw/config/scenario.csv index 763c048..cd244c6 100644 --- a/aerpaw/config/scenario.csv +++ b/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 -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" \ No newline at end of file +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 \ No newline at end of file diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m index cf95203..cf8e8fb 100644 --- a/test/parametricTestSuite.m +++ b/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();