Fix AERPAW compiled path obstacle avoidance: missing scenario params, barriers early exit, classdef mismatch, and DI infeasible coasting

@miSim/constrainMotion.m

@@ -31,6 +31,7 @@
 
     % Initialize QP based on number of agents and obstacles
     kk = 1;
+    idx = 0; % barriers log cursor (only used in the MATLAB path; unused in compiled path)
     A = zeros(obj.numBarriers, 3 * nAgents);
     b = zeros(obj.numBarriers, 1);
 
@@ -60,9 +61,11 @@
         end
     end
 
-    idx = length(h(triu(true(size(h)), 1)));
-    obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
-    idx = idx + 1;
+    if coder.target('MATLAB')
+        idx = length(h(triu(true(size(h)), 1)));
+        obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
+        idx = idx + 1;
+    end
 
     hObs = NaN(nAgents, size(obj.obstacles, 1));
     % Set up obstacle avoidance constraints
@@ -84,8 +87,10 @@
         end
     end
 
-    obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
-    idx = idx + numel(hObs);
+    if coder.target('MATLAB')
+        obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
+        idx = idx + numel(hObs);
+    end
 
     % Set up domain constraints (walls and ceiling only)
     % Floor constraint is implicit with an obstacle corresponding to the
@@ -128,8 +133,10 @@
         b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
         kk = kk + 1;
 
-        obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
-        idx = idx + 6;
+        if coder.target('MATLAB')
+            obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
+            idx = idx + 6;
+        end
     end
 
     % Add communication network constraints
@@ -159,7 +166,10 @@
             end
         end
     end
-    obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
+    if coder.target('MATLAB')
+        hComms_upper = hComms(triu(true(size(hComms)), 1));
+        obj.barriers(idx:(idx + length(hComms_upper) - 1), obj.timestepIndex) = hComms_upper;
+    end
 
     % Double-integrator: transform QP from velocity to acceleration space.
     % Single-integrator constraint: A * v <= b
@@ -186,13 +196,25 @@
     uNew = reshape(uNew, 3, nAgents)';
 
     if exitflag < 0
-        % Infeasible or other hard failure: hold all agents at current positions
+        % Infeasible or other hard failure: hold all agents at current positions.
+        % Zero uNew so that:
+        %   SI path: pos = lastPos + 0*dt  (position held)
+        %   DI path: vel = lastVel + 0*dt = lastVel, so we must also zero lastVel
+        %            to prevent coasting through obstacles when QP is infeasible.
         if coder.target('MATLAB')
             warning("QP infeasible (exitflag=%d), holding positions.", int16(exitflag));
         else
             fprintf("[constrainMotion] QP infeasible (exitflag=%d), holding positions\n", int16(exitflag));
         end
         uNew = zeros(nAgents, 3);
+        if obj.useDoubleIntegrator
+            % In DI mode zero acceleration alone still coasts at lastVel.
+            % Explicitly zero each agent's velocity so the position update
+            % below results in pos = lastPos (true hold).
+            for ii = 1:nAgents
+                obj.agents{ii}.lastVel = zeros(1, 3);
+            end
+        end
     elseif exitflag == 0
         % Max iterations exceeded: use suboptimal solution already in uNew
         if coder.target('MATLAB')

@miSim/miSim.m

@@ -69,7 +69,7 @@
             obj.obstacles = {rectangularPrism};
             obj.agents = {agent};
         end
-        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
+        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology);
         [obj] = initializeFromCsv(obj, csvPath);
         [obj] = initializeFromInits(obj, initsPath);
         [obj] = plotFromSimHist(obj, initsPath, histPath);

aerpaw/controller.m

@@ -57,7 +57,7 @@ function controller(numClients)
 end
 
 % Load guidance scenario from CSV (parameters for guidance_step)
-NUM_SCENARIO_PARAMS = 45;
+NUM_SCENARIO_PARAMS = 48;
 MAX_OBSTACLES_CTRL  = int32(8);
 scenarioParams = zeros(1, NUM_SCENARIO_PARAMS);
 obstacleMin    = zeros(MAX_OBSTACLES_CTRL, 3);

aerpaw/guidance_step.m

@@ -29,6 +29,9 @@
 %                                               39-40  objectivePos
 %                                               41-44  objectiveVar (2x2, col-major)
 %                                               45     sensorPerformanceMinimum
+%                                               46     useDoubleIntegrator
+%                                               47     dampingCoeff
+%                                               48     useFixedTopology
 %   obstacleMin       (MAX_OBSTACLES × 3) double  column-major obstacle corners (compiled path)
 %   obstacleMax       (MAX_OBSTACLES × 3) double
 %   numObstacles      (1,1) int32                 actual obstacle count
@@ -94,6 +97,9 @@
         OBJECTIVE_GROUND_POS        = scenarioParams(39:40);
         OBJECTIVE_VAR               = reshape(scenarioParams(41:44), 2, 2);
         SENSOR_PERFORMANCE_MINIMUM  = scenarioParams(45);
+        USE_DOUBLE_INTEGRATOR       = logical(scenarioParams(46));
+        DAMPING_COEFF               = scenarioParams(47);
+        USE_FIXED_TOPOLOGY          = logical(scenarioParams(48));
 
         % --- Build domain geometry ---
         dom = rectangularPrism;
@@ -146,7 +152,8 @@
         % --- Initialise simulation (plots and video disabled) ---
         sim = miSim;
         sim = sim.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, USE_FIXED_TOPOLOGY);
     end
 
     % On the init call return current positions unchanged
@@ -176,15 +183,18 @@
     sim.timestepIndex = sim.timestepIndex + 1;
 
     % 3. Update communications topology (Lesser Neighbour Assignment)
-    sim = sim.lesserNeighbor();
+    if ~sim.useFixedTopology
+        sim = sim.lesserNeighbor();
+    end
 
     % 4. Compute Voronoi partitioning
     sim.partitioning = sim.agents{1}.partition(sim.agents, sim.domain.objective);
 
     % 5. Unconstrained gradient-ascent step for each agent
     for ii = 1:size(sim.agents, 1)
         sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ...
-                                             sim.timestepIndex, ii, sim.agents);
+                                             sim.timestepIndex, ii, sim.agents, ...
+                                             sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep);
     end
 
     % 6. Apply CBF safety filter (collision / comms / domain constraints via QP)

aerpaw/impl/controller_impl.cpp

@@ -187,6 +187,9 @@ static int readScenarioDataRow(const char* filename, char* line, int lineSize) {
 //   38-39: objectivePos (east, north)
 //   40-43: objectiveVar (2x2 col-major: v11, v12, v21, v22)
 //   44   : sensorPerformanceMinimum
+//   45   : useDoubleIntegrator  (CSV column 23; 0=single-integrator, 1=double-integrator)
+//   46   : dampingCoeff         (CSV column 24)
+//   47   : useFixedTopology     (CSV column 25; 0=dynamic lesser-neighbor, 1=fixed)
 // Returns 1 on success, 0 on failure.
 int loadScenario(const char* filename, double* params) {
     char line[4096];
@@ -290,6 +293,25 @@ int loadScenario(const char* filename, double* params) {
         params[44] = atof(trimField(tmp));
     }
 
+    // useDoubleIntegrator (column 23), dampingCoeff (column 24), useFixedTopology (column 25).
+    // These columns are optional (introduced later); default to SI dynamics and dynamic topology
+    // if not present in the CSV.
+    if (nf >= 26) {
+        char tmp[64];
+        strncpy(tmp, fields[23], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[45] = atof(trimField(tmp));  // useDoubleIntegrator
+
+        strncpy(tmp, fields[24], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[46] = atof(trimField(tmp));  // dampingCoeff
+
+        strncpy(tmp, fields[25], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[47] = atof(trimField(tmp));  // useFixedTopology
+    } else {
+        params[45] = 0.0;  // useDoubleIntegrator = false (single-integrator)
+        params[46] = 2.0;  // dampingCoeff default
+        params[47] = 0.0;  // useFixedTopology = false (dynamic lesser-neighbor)
+    }
+
     printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g]\n",
            params[32], params[33], params[34], params[35], params[36], params[37]);
     return 1;

aerpaw/impl/controller_impl.h

@@ -30,7 +30,10 @@ int loadTargets(const char* filename, double* targets, int maxClients);
 //   38-39 objectivePos
 //   40-43 objectiveVar (2x2 col-major)
 //   44    sensorPerformanceMinimum
-#define NUM_SCENARIO_PARAMS 45
+//   45    useDoubleIntegrator
+//   46    dampingCoeff
+//   47    useFixedTopology
+#define NUM_SCENARIO_PARAMS 48
 #define MAX_CLIENTS_PER_PARAM 4
 // Maximum number of obstacles (upper bound for pre-allocated arrays).
 #define MAX_OBSTACLES 8