From 82c7dc9c25bfd33870d5078c823fc8efd47635bf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tillmann=20M=C3=BChlpfordt?= <tillmann.muehlpfordt@kit.edu>
Date: Fri, 3 Apr 2020 10:25:45 +0200
Subject: [PATCH] use gradient information for fmincon #84

---
 .gitignore                    |  4 ++--
 src/core/createLocalSolvers.m | 32 ++++++++++++++++++++++++--------
 2 files changed, 26 insertions(+), 10 deletions(-)

diff --git a/.gitignore b/.gitignore
index 6b8cc12..ea2e53b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,7 +3,7 @@
 
 # OSX / *nix default autosave extension
 *.m~
-
+*.DS_Store
 # Compiled MEX binaries (all platforms)
 *.mex*
 
@@ -37,4 +37,4 @@ octave-workspace
 *.blg
 *.log
 *.synctex.gz
-*.toc
\ No newline at end of file
+*.toc
diff --git a/src/core/createLocalSolvers.m b/src/core/createLocalSolvers.m
index 9ef28a7..af4d42f 100644
--- a/src/core/createLocalSolvers.m
+++ b/src/core/createLocalSolvers.m
@@ -27,7 +27,8 @@
     if use_fmincon
         pars = [];
         funs = sProb.locFuns;
-        solve_nlp = @(x, z, rho, lambda, Sigma, pars)build_local_NLP(funs.ffi{i}, funs.ggi{i}, funs.hhi{i}, sProb.AA{i}, lambda, rho, z, Sigma, x, sProb.llbx{i}, sProb.uubx{i});   
+        jacs = sProb.locSens;
+        solve_nlp = @(x, z, rho, lambda, Sigma, pars)build_local_NLP(funs.ffi{i}, funs.ggi{i}, funs.hhi{i}, sProb.AA{i}, lambda, rho, z, Sigma, x, sProb.llbx{i}, sProb.uubx{i}, jacs.ggi{i});   
     else
         assert(isfield(sProb, 'locFunsCas'), 'locFunsCas field is missing')
         nlp_reference = build_nlp_reference(sProb.xxCas{i},...
@@ -78,9 +79,15 @@
     res.pars.lam_x = res.lam_x;
 end
 
-function res = build_local_NLP(f, g, h, A, lambda, rho, z, Sigma, x0, lbx, ubx)
-    cost = build_cost_function(f, lambda, A, rho, z, Sigma);
-    nonlcon = @(x)build_nonlcon(x, g, h);
+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.CheckGradients = false;
+    opts.SpecifyConstraintGradient = true;
+    opts.SpecifyObjectiveGradient = true;
+
+    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);
     res.x = xopt;
     res.lam_g = [multiplier.eqnonlin; multiplier.ineqnonlin];
@@ -88,13 +95,22 @@
     res.pars = [];
 end
 
-function fun = build_cost_function(f, lambda, A, rho, z, Sigma)
-    fun = @(x)f(x) + lambda'*A*x + 0.5*rho*(x - z)'*Sigma*(x - z);
+function [fun, grad] = build_cost_function(x, f, lambda, A, rho, z, Sigma)
+%     try chol(Sigma)
+%         disp('Matrix is symmetric positive definite.')
+%     catch ME
+%         disp('Matrix is not symmetric positive definite')
+%     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);
+    end
 end
 
-
-function [ineq, eq] = build_nonlcon(x, g, h)
+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);
 end