diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m
index 497768a..4676272 100644
--- a/@miSim/constrainMotion.m
+++ b/@miSim/constrainMotion.m
@@ -48,13 +48,13 @@ function [obj] = constrainMotion(obj)
% Correction splits between 2 agents, so |A| = 2*r_sum
r_sum_ij = obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius;
v_max_ij = max(obj.agents{ii}.initialStepSize, obj.agents{jj}.initialStepSize) / obj.timestep;
- slack = -(4 * r_sum_ij * v_max_ij / obj.barrierGain)^(1 / obj.barrierExponent);
+ hMin = -(4 * r_sum_ij * v_max_ij / obj.barrierGain)^(1 / obj.barrierExponent);
if norm(A(kk, :)) < 1e-9
% Agents are coincident: A-row is zero, so b < 0 would make
% 0 ≤ b unsatisfiable. Fall back to b = 0 (no correction possible).
b(kk) = 0;
else
- b(kk) = obj.barrierGain * max(slack, h(ii, jj))^obj.barrierExponent;
+ b(kk) = obj.barrierGain * max(hMin, h(ii, jj))^obj.barrierExponent;
end
kk = kk + 1;
end
@@ -73,8 +73,8 @@ function [obj] = constrainMotion(obj)
% Floor for single-agent constraint: full correction on one agent, |A| = 2*r_i
r_i = obj.agents{ii}.collisionGeometry.radius;
v_max_i = obj.agents{ii}.initialStepSize / obj.timestep;
- h_floor_i = -(2 * r_i * v_max_i / obj.barrierGain)^(1 / obj.barrierExponent);
- b(kk) = obj.barrierGain * max(h_floor_i, hObs(ii, jj))^obj.barrierExponent;
+ hMin = -(2 * r_i * v_max_i / obj.barrierGain)^(1 / obj.barrierExponent);
+ b(kk) = obj.barrierGain * max(hMin, hObs(ii, jj))^obj.barrierExponent;
kk = kk + 1;
end
@@ -157,17 +157,24 @@ function [obj] = constrainMotion(obj)
end
opt = optimoptions("quadprog", "Display", "off", "Algorithm", "active-set", "UseCodegenSolver", true);
x0 = zeros(size(H, 1), 1);
- [vNew, ~, exitflag, m] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
- if coder.target('MATLAB')
- assert(exitflag == 1, sprintf("quadprog failure... %s%s", newline, m.message));
- end
+ [vNew, ~, exitflag] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
vNew = reshape(vNew, 3, nAgents)';
- if exitflag <= 0
+ if exitflag < 0
+ % Infeasible or other hard failure: hold all agents at current positions
if coder.target('MATLAB')
- warning("QP failed, continuing with unconstrained solution...")
+ warning("QP infeasible (exitflag=%d), holding positions.", int16(exitflag));
+ else
+ fprintf("[constrainMotion] QP infeasible (exitflag=%d), holding positions\n", int16(exitflag));
+ end
+ vNew = zeros(nAgents, 3);
+ elseif exitflag == 0
+ % Max iterations exceeded: use suboptimal solution already in vNew
+ if coder.target('MATLAB')
+ warning("QP max iterations exceeded, using suboptimal solution.");
+ else
+ fprintf("[constrainMotion] QP max iterations exceeded, using suboptimal solution\n");
end
- vNew = v;
end
% Update the "next position" that was previously set by unconstrained
diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj
index cf8f232..e6ef62d 100644
--- a/aerpaw/controller.coderprj
+++ b/aerpaw/controller.coderprj
@@ -524,581 +524,569 @@
- /home/kdee/Desktop/miSim/aerpaw/codegen/factoryConstruct.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/factoryConstruct.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/find.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/find.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/find.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/find.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/norm.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/norm.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/partition.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/partition.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/repmat.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/repmat.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/repmat.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/repmat.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/rt_defines.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/rt_defines.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/run.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/run.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/run.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/run.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sort.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sort.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/string1.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/string1.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/sum.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/sum.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/unique.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/unique.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h
+ /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h
GENERATED_SOURCE
- /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp
+ /home/kdee/matlab/R2025a/extern/include/tmwtypes.h
GENERATED_SOURCE
-
- /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h
-
- GENERATED_SOURCE
-
-
-
- /home/kdee/matlab/R2025a/extern/include/tmwtypes.h
-
- GENERATED_SOURCE
-
-
/home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp
GENERATED_SOURCE
-
+
/home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.h
@@ -1106,7 +1094,7 @@
true
- 2026-03-03T18:47:15
+ 2026-03-03T19:58:03
diff --git a/aerpaw/results/plotGpsCsvs.m b/aerpaw/results/plotGpsCsvs.m
index cf8d105..3ff7ce8 100644
--- a/aerpaw/results/plotGpsCsvs.m
+++ b/aerpaw/results/plotGpsCsvs.m
@@ -20,7 +20,7 @@ fclose(fID);
lla0 = [str2double(yaml((strfind(yaml, 'lat:') + 4):(strfind(yaml, 'lon:') - 1))), str2double(yaml((strfind(yaml, 'lon:') + 4):(strfind(yaml, 'alt:') - 1))), seaToGroundLevel];
% Paths to logs
-gpsCsvs = dir(fullfile("sandbox", "test13", "*.csv"));
+gpsCsvs = dir(fullfile("sandbox", "test14", "*.csv"));
G = cell(size(gpsCsvs));
for ii = 1:size(gpsCsvs, 1)