From 8c5811ff6a003cbbc56898bd1d71d23fbc843817 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Tue, 3 Mar 2026 20:14:55 -0800 Subject: [PATCH] seems to line up well again, constrainMotion updates --- @miSim/constrainMotion.m | 29 +++-- aerpaw/controller.coderprj | 202 ++++++++++++++++------------------- aerpaw/results/plotGpsCsvs.m | 2 +- 3 files changed, 114 insertions(+), 119 deletions(-) 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)