Use numerical sensitivities for fmincon correctly #84

- modified termination criterion in `iterateAL()` - verified sensitivities against casadi
alexe15 · Apr 7, 2020 · 075888a · 075888a
1 parent 61b4357
commit 075888a
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 9 deletions.
diff --git a/src/core/createLocalSolvers.m b/src/core/createLocalSolvers.m
@@ -81,14 +81,16 @@
 
 function res = build_local_NLP(f, g, h, A, lambda, rho, z, Sigma, x0, lbx, ubx, dgdx)
     opts = optimoptions('fmincon');
-    opts.Algorithm = 'trust-region-reflective';
+    opts.Algorithm = 'interior-point';
     opts.CheckGradients = false;
     opts.SpecifyConstraintGradient = true;
     opts.SpecifyObjectiveGradient = true;
+    opts.MaxFunctionEvaluations = 5e3;
+%     opts.Display = 'iter';
 
     cost = @(x)build_cost_function(x, f(x), lambda, A, rho, z, Sigma);
     nonlcon = @(x)build_nonlcon(x, g, h, dgdx);
-    [xopt, fval, flag, out, multiplier] = fmincon(cost, x0, [], [], [], [], lbx, ubx, nonlcon);
+    [xopt, fval, flag, out, multiplier] = fmincon(cost, x0, [], [], [], [], lbx, ubx, nonlcon, opts);
     res.x = xopt;
     res.lam_g = [multiplier.eqnonlin; multiplier.ineqnonlin];
     res.lam_x = max(multiplier.lower, multiplier.upper);
@@ -103,14 +105,14 @@
 %     end
     fun = f + lambda'*A*x + 0.5*rho*(x - z)'*Sigma*(x - z);
     if nargout > 1
-        grad = A'*lambda + 0.5*rho*Sigma*(x - z);
+        grad = A'*lambda + rho*Sigma*(x - z);
     end
 end
 
 function [ineq, eq, jac_ineq, jac_eq] = build_nonlcon(x, g, h, dgdx)
     ineq = h(x);
     eq = g(x);
     jac_ineq = [];
-    iac_eq = dgdx(x);
+    jac_eq = dgdx(x)';
 end
 
diff --git a/src/core/iterateAL.m b/src/core/iterateAL.m
@@ -7,8 +7,7 @@
 iterTimer = tic;
 i         = 1;
 iter.i    = 1;
-while ((i <= opts.maxiter) && ( (strcmp(opts.term_eps,'false')) || ...
-                                      (iter.logg.consViol(i) >= opts.term_eps)))
+while (i <= opts.maxiter) && ( (strcmp(opts.term_eps,'false')) || (iter.logg.consViol(i) >= opts.term_eps))
 
     % solve local NLPs and evaluate sensitivities
    if (strcmp( opts.parfor, 'true' ))

diff --git a/src/core/loadDefOpts.m b/src/core/loadDefOpts.m
@@ -11,7 +11,7 @@
     optVariants.muUpdate     = {2};
     optVariants.muMax        = {2*1e6};
 
-    optVariants.eps          = {0};
+    optVariants.eps          = {1e-10};
     optVariants.maxiter      = {30};
     optVariants.actMargin    = {-1e-6};
     optVariants.solveQP      = {'MA57','ipopt','pinv','linsolve','sparseBs','MOSEK','quadprog'};
@@ -20,7 +20,7 @@
     optVariants.plot         = {'true', 'false'};
     optVariants.Sig          = {'const','dyn'};
     optVariants.lamInit      = {'false','true'};
-    optVariants.term_eps     = {0};
+    optVariants.term_eps     = {1e-10};
     optVariants.regParam     = {1e-4};
 
     % extensions

diff --git a/src/core/parallelStepCentral.m b/src/core/parallelStepCentral.m
@@ -30,8 +30,9 @@
     z = iter.yy{j};
     rho = iter.stepSizes.rho;
     lambda = iter.lam;
-    Sigma = opts.SSig{j};
+    Sigma = sparse(opts.SSig{j});
 
+    fprintf('Solving NLP in region %i\n', j);
     problem = sProb.nnlp{j};
     sol = problem.solve_nlp(x0, z, rho, lambda, Sigma, problem.pars);