1 /*
2  *    This file is part of CasADi.
3  *
4  *    CasADi -- A symbolic framework for dynamic optimization.
5  *    Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
6  *                            K.U. Leuven. All rights reserved.
7  *    Copyright (C) 2011-2014 Greg Horn
8  *
9  *    CasADi is free software; you can redistribute it and/or
10  *    modify it under the terms of the GNU Lesser General Public
11  *    License as published by the Free Software Foundation; either
12  *    version 3 of the License, or (at your option) any later version.
13  *
14  *    CasADi is distributed in the hope that it will be useful,
15  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
16  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17  *    Lesser General Public License for more details.
18  *
19  *    You should have received a copy of the GNU Lesser General Public
20  *    License along with CasADi; if not, write to the Free Software
21  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
22  *
23  */
24 
25 
26 #ifndef CASADI_BLOCKSQP_HPP
27 #define CASADI_BLOCKSQP_HPP
28 
29 #include <casadi/interfaces/blocksqp/casadi_nlpsol_blocksqp_export.h>
30 #include "casadi/core/linsol.hpp"
31 #include "casadi/core/nlpsol_impl.hpp"
32 #include "../qpoases/qpoases_interface.hpp"
33 
34 /** \defgroup plugin_Nlpsol_blocksqp
35   * This is a modified version of blockSQP by Janka et al.
36   *
37   * \author Dennis Janka, Joel Andersson
38   * \date 2012-2015, 2016
39 */
40 
41 /** \pluginsection{Nlpsol,blocksqp} */
42 
43 /// \cond INTERNAL
44 namespace casadi {
45   // Forward declaration
46   class Blocksqp;
47 
48   struct CASADI_NLPSOL_BLOCKSQP_EXPORT BlocksqpMemory : public NlpsolMemory {
49     /// Constructor
50     BlocksqpMemory();
51 
52     /// Destructor
53     ~BlocksqpMemory();
54 
55     qpOASES::SymSparseMat *H;
56     qpOASES::Matrix *A;
57     qpOASES::SQProblem* qp;
58 
59     // [Workaround] qpOASES memory block
60     QpoasesMemory* qpoases_mem;
61 
62     // Stats
63     casadi_int itCount;  // iteration number
64     casadi_int qpIterations;  // number of qp iterations in the current major iteration
65     casadi_int qpIterations2;  // number of qp iterations for solving convexified QPs
66     casadi_int qpItTotal;  // total number of qp iterations
67     casadi_int qpResolve;  // how often has QP to be convexified and resolved?
68     casadi_int nFunCalls;  // number of function calls
69     casadi_int nDerCalls;  // number of derivative calls
70     casadi_int nRestHeurCalls;  // number calls to feasibility restoration heuristic
71     casadi_int nRestPhaseCalls;  // number calls to feasibility restoration phase
72     casadi_int rejectedSR1;  // count how often the SR1 update is rejected
73     casadi_int hessSkipped;  // number of block updates skipped in the current iteration
74     casadi_int hessDamped;  // number of block updates damped in the current iteration
75     casadi_int nTotalUpdates;
76     casadi_int nTotalSkippedUpdates;
77     double averageSizingFactor;  // average value (over all blocks) of COL sizing factor
78 
79     // Variables that are updated during one SQP iteration
80     double obj;  // objective value
81     double qpObj;  // objective value of last QP subproblem
82     double cNorm;  // constraint violation
83     double cNormS;  // scaled constraint violation
84     double gradNorm;  // norm of Lagrangian gradient
85     double lambdaStepNorm;  // norm of step in dual variables
86     double tol;  // current optimality tolerance
87 
88     double *lam_xk, *lam_gk; // dual variables
89     double *gk;  // constraint vector
90 
91     double *jac_g;  // nonzero elements of Jacobian (length)
92 
93     double *deltaMat;  // last m primal steps
94     double *dxk;  // alias for current step
95     double *grad_fk;  // gradient of objective
96     double *grad_lagk;  // gradient of Lagrangian
97     double *gammaMat;  // Lagrangian gradient differences for last m steps
98     double *gamma;  // alias for current Lagrangian gradient
99 
100     double **hess;  // [blockwise] pointer to current Hessian of the Lagrangian
101     double **hess1;  // [blockwise] first Hessian approximation
102     double **hess2;  // [blockwise] second Hessian approximation (convexified)
103     double *hess_lag;  // nonzero elements of Hessian (length)
104     int *hessIndRow;  // row indices (length)
105     int *hessIndCol;  // indices to first entry of columns (nCols+1)
106     int *hessIndLo;  // Indices to first entry of lower triangle (including diagonal) (nCols)
107 
108     /*
109      * Variables for QP solver
110      */
111     double *lbx_qp, *ubx_qp, *lba_qp, *uba_qp;  // bounds for current step
112     double* lam_qp;  // dual variables of QP
113     double* jac_times_dxk;  // product of constraint Jacobian with dxk
114 
115     /*
116      * For modified BFGS updates
117      */
118     double *delta_norm;  // sTs
119     double* delta_norm_old;  // (from previous iteration)
120     double* delta_gamma;  // sTy
121     double* delta_gamma_old;  // (from previous iteration)
122     casadi_int *noUpdateCounter;  // count skipped updates for each block
123 
124     /*
125      * Variables for globalization strategy
126      */
127     casadi_int steptype;  // is current step a restoration step (1)?
128     double alpha;  // stepsize for line search
129     casadi_int nSOCS;  // number of second-order correction steps
130     casadi_int reducedStepCount;  // count number of consecutive reduced steps,
131     double* delta_h; // inertia correction (filter line search w indef Hessian)
132     double* trial_xk;  // new trial iterate (for line search)
133     std::set< std::pair<double, double> > filter; // Filter contains pairs (constrVio, objective)
134 
135     std::vector<int> colind, row;
136 
137     // Temporary memory
138     double* jac;
139     double* exact_hess_lag;
140 
141     casadi_int ret_;  // return value (only needed for first iteration of restoration phase)
142   };
143 
144   /** \brief \pluginbrief{Nlpsol,blocksqp}
145      @copydoc Nlpsol_doc
146      @copydoc plugin_Nlpsol_blocksqp
147   */
148   class CASADI_NLPSOL_BLOCKSQP_EXPORT Blocksqp : public Nlpsol {
149   public:
150     explicit Blocksqp(const std::string& name, const Function& nlp);
151     ~Blocksqp() override;
152 
153     // Get name of the plugin
plugin_name() const154     const char* plugin_name() const override { return "blocksqp";}
155 
156     // Get name of the class
class_name() const157     std::string class_name() const override { return "Blocksqp";}
158 
159     /** \brief  Create a new NLP Solver */
creator(const std::string & name,const Function & nlp)160     static Nlpsol* creator(const std::string& name, const Function& nlp) {
161       return new Blocksqp(name, nlp);
162     }
163 
164     ///@{
165     /** \brief Options */
166     static const Options options_;
get_options() const167     const Options& get_options() const override { return options_;}
168     ///@}
169 
170     // Initialize the solver
171     void init(const Dict& opts) override;
172 
173     /** \brief Create memory block */
alloc_mem() const174     void* alloc_mem() const override { return new BlocksqpMemory();}
175 
176     /** \brief Initalize memory block */
177     int init_mem(void* mem) const override;
178 
179     /** \brief Free memory block */
free_mem(void * mem) const180     void free_mem(void *mem) const override { delete static_cast<BlocksqpMemory*>(mem);}
181 
182     /** \brief Set the (persistent) work vectors */
183     void set_work(void* mem, const double**& arg, double**& res,
184                           casadi_int*& iw, double*& w) const override;
185 
186     // Solve the NLP
187     int solve(void* mem) const override;
188 
189     /// A documentation string
190     static const std::string meta_doc;
191 
192     // Block partitioning
193     casadi_int nblocks_;
194     std::vector<casadi_int> blocks_;
195     std::vector<casadi_int> dim_;
196     casadi_int nnz_H_;
197 
198     // Jacobian/Hessian sparsity
199     Sparsity Asp_, Hsp_;
200     Sparsity exact_hess_lag_sp_;
201 
202     /// Main Loop of SQP method
203     casadi_int run(BlocksqpMemory* m, casadi_int maxIt, casadi_int warmStart = 0) const;
204     /// Compute gradient of Lagrangian function (sparse version)
205     void calcLagrangeGradient(BlocksqpMemory* m,
206       const double* lam_x, const double* lam_g,
207       const double* grad_f, const double *jacNz,
208       double *grad_lag, casadi_int flag) const;
209 
210     /// Overloaded function for convenience, uses current variables of SQPiterate vars
211     void calcLagrangeGradient(BlocksqpMemory* m, double* grad_lag, casadi_int flag) const;
212     /// Print information about the SQP method
213     void printInfo(BlocksqpMemory* m) const;
214     /// Update optimization tolerance (similar to SNOPT) in current iterate
215     bool calcOptTol(BlocksqpMemory* m) const;
216 
217     /*
218      * Solve QP subproblem
219      */
220     // Update the bounds on the current step, i.e. the QP variables
221     void updateStepBounds(BlocksqpMemory* m, bool soc) const;
222     // Solve a QP with QPOPT or qpOASES to obtain a step deltaXi and estimates
223     // for the Lagrange multipliers
224     casadi_int solveQP(BlocksqpMemory* m, double* deltaXi, double* lambdaQP,
225       bool matricesChanged = true) const;
226     // Compute the next Hessian in the inner loop of increasingly convexified
227     // QPs and store it in vars->hess2
228     void computeNextHessian(BlocksqpMemory* m, casadi_int idx, casadi_int maxQP) const;
229 
230     /*
231      * Globalization Strategy
232      */
233     /// No globalization strategy
234     casadi_int fullstep(BlocksqpMemory* m) const;
235     /// Set new primal dual iterate
236     void acceptStep(BlocksqpMemory* m, const double* deltaXi,
237       const double* lambdaQP, double alpha, casadi_int nSOCS) const;
238     // Overloaded function for convenience, uses current variables of SQPiterate vars
239     void acceptStep(BlocksqpMemory* m, double alpha) const;
240     // Reduce stepsize if a step is rejected
241     void reduceStepsize(BlocksqpMemory* m, double *alpha) const;
242     // Determine steplength alpha by a filter based line search similar to IPOPT
243     casadi_int filterLineSearch(BlocksqpMemory* m) const;
244     // Remove all entries from filter
245     void initializeFilter(BlocksqpMemory* m) const;
246     // Is a pair (cNorm, obj) in the current filter?
247     bool pairInFilter(BlocksqpMemory* m, double cNorm, double obj) const;
248     // Augment current filter by pair (cNorm, obj)
249     void augmentFilter(BlocksqpMemory* m, double cNorm, double obj) const;
250     // Perform a second order correction step (solve QP)
251     bool secondOrderCorrection(BlocksqpMemory* m, double cNorm, double cNormTrial,
252       double dfTdeltaXi, bool swCond, casadi_int it) const;
253     // Reduce stepsize if a second order correction step is rejected
254     void reduceSOCStepsize(BlocksqpMemory* m, double *alphaSOC) const;
255     // Start feasibility restoration heuristic
256     casadi_int feasibilityRestorationHeuristic(BlocksqpMemory* m) const;
257     // Start feasibility restoration phase (solve NLP)
258     casadi_int feasibilityRestorationPhase(BlocksqpMemory* m) const;
259     // Check if full step reduces KKT error
260     casadi_int kktErrorReduction(BlocksqpMemory* m) const;
261 
262     /*
263      * Hessian Approximation
264      */
265     // Set initial Hessian: Identity matrix
266     void calcInitialHessian(BlocksqpMemory* m) const;
267     // [blockwise] Set initial Hessian: Identity matrix
268     void calcInitialHessian(BlocksqpMemory* m, casadi_int b) const;
269     // Reset Hessian to identity and remove past information on Lagrange gradient and steps
270     void resetHessian(BlocksqpMemory* m) const;
271     // [blockwise] Reset Hessian to identity and remove past information on
272     // Lagrange gradient and steps
273     void resetHessian(BlocksqpMemory* m, casadi_int b) const;
274     // Compute full memory Hessian approximations based on update formulas
275     void calcHessianUpdate(BlocksqpMemory* m, casadi_int updateType, casadi_int hessScaling) const;
276     // Compute limited memory Hessian approximations based on update formulas
277     void calcHessianUpdateLimitedMemory(BlocksqpMemory* m,
278         casadi_int updateType, casadi_int hessScaling) const;
279     // Compute exact Hessian update
280     void calcHessianUpdateExact(BlocksqpMemory* m) const;
281     // [blockwise] Compute new approximation for Hessian by SR1 update
282     void calcSR1(BlocksqpMemory* m, const double* gamma, const double* delta,
283       casadi_int b) const;
284     // [blockwise] Compute new approximation for Hessian by BFGS update with Powell modification
285     void calcBFGS(BlocksqpMemory* m, const double* gamma, const double* delta,
286       casadi_int b) const;
287     // Set pointer to correct step and Lagrange gradient difference in a limited memory context
288     void updateDeltaGamma(BlocksqpMemory* m) const;
289 
290     /*
291      * Scaling of Hessian Approximation
292      */
293     // [blockwise] Size Hessian using SP, OL, or mean sizing factor
294     void sizeInitialHessian(BlocksqpMemory* m, const double* gamma,
295       const double* delta, casadi_int b, casadi_int option) const;
296     // [blockwise] Size Hessian using the COL scaling factor
297     void sizeHessianCOL(BlocksqpMemory* m, const double* gamma,
298       const double* delta, casadi_int b) const;
299 
300     /*
301     * STATS
302     */
303     void initStats(BlocksqpMemory* m) const;
304     void updateStats(BlocksqpMemory* m) const;
305     /// Print one line of output to stdout about the current iteration
306     void printProgress(BlocksqpMemory* m) const;
307     /// Reset variables that any SQP code needs
308     void reset_sqp(BlocksqpMemory* m) const;
309     /// Convert *hess to column compressed sparse format
310     void convertHessian(BlocksqpMemory* m) const;
311     /// Set initial filter, objective function, tolerances etc.
312     void initIterate(BlocksqpMemory* m) const;
313 
314     /// Evaluate objective and constraints, including derivatives
315     casadi_int evaluate(BlocksqpMemory* m, double *f, double *g,
316                  double *grad_f, double *jac_g) const;
317 
318     /// Evaluate objective and constraints, no derivatives
319     casadi_int evaluate(BlocksqpMemory* m, const double *xk,
320                  double *f, double *g) const;
321 
322     /// Evaluate exact hessian of Lagrangian
323     casadi_int evaluate(BlocksqpMemory* m,
324                  double *exact_hess_lag) const;
325 
326     //  Declaration of general purpose routines for matrix and vector computations
327     double lInfConstraintNorm(BlocksqpMemory* m, const double* xk, const double* g) const;
328 
329     /// QP solver for the subproblems
330     //Function qpsol_;
331 
332     // Linear solver in qpOASES
333     std::string linsol_plugin_;
334 
335     // General options
336     bool print_header_;
337     bool print_iteration_;
338     double eps_;  // values smaller than this are regarded as numerically zero
339     double opttol_;  // optimality tolerance
340     double nlinfeastol_; // nonlinear feasibility tolerance
341 
342     // Algorithmic options
343     bool schur_;  // Use qpOASES schur compliment approach
344     bool globalization_; // Globalization strategy
345     bool restore_feas_;// Use feasibility restoration phase
346     casadi_int max_line_search_;  // Maximum number of steps in line search
347     casadi_int max_consec_reduced_steps_;// Maximum number of consecutive reduced steps
348     casadi_int max_consec_skipped_updates_; // Maximum number of consecutive skipped updates
349     casadi_int max_it_qp_;  // Maximum number of QP iterations per SQP iteration
350     casadi_int max_iter_; // Maximum number of SQP steps
351     bool warmstart_; // Use warmstarting
352     bool qp_init_;
353     bool block_hess_;  // Blockwise Hessian approximation?
354     casadi_int hess_scaling_;// Scaling strategy for Hessian approximation
355     casadi_int fallback_scaling_;  // If indefinite update is used, the type of fallback strategy
356     double max_time_qp_;  // Maximum number of time in seconds per QP solve per SQP iteration
357     double ini_hess_diag_;  // Initial Hessian guess: diagonal matrix diag(iniHessDiag)
358     double col_eps_;  // epsilon for COL scaling strategy
359     double col_tau1_; // tau1 for COL scaling strategy
360     double col_tau2_; // tau2 for COL scaling strategy
361     casadi_int hess_damp_;  // activate Powell damping for BFGS
362     double hess_damp_fac_;  // damping factor for BFGS Powell modification
363     casadi_int hess_update_; // Type of Hessian approximation
364     casadi_int fallback_update_;  // If indefinite update is used, the type of fallback strategy
365     casadi_int hess_lim_mem_; // Full or limited memory
366     casadi_int hess_memsize_;  // Memory size for L-BFGS updates
367     casadi_int which_second_derv_; // For which block should second derivatives be provided
368                                    // by the user
369     bool skip_first_globalization_;  // No globalization strategy in first iteration
370     casadi_int conv_strategy_;  // Convexification strategy
371     casadi_int max_conv_qp_;  // How many additional QPs may be solved for convexification
372                               // per iteration?
373 
374     // Filter line search parameters, cf. IPOPT paper
375     casadi_int max_soc_iter_; // Maximum number of SOC line search iterations
376     double gamma_theta_;
377     double gamma_f_;
378     double kappa_soc_;
379     double kappa_f_;
380     double theta_max_;
381     double theta_min_;
382     double delta_;
383     double s_theta_;
384     double s_f_;
385     double kappa_minus_;
386     double kappa_plus_;
387     double kappa_plus_max_;
388     double delta_h0_;
389     double eta_;
390     double obj_lo_;
391     double obj_up_;
392 
393     // feasibility restoration phase
394     double rho_;  // Regularization factor for first part of objective
395     double zeta_;  // Regularization factor for second part of objective
396     Function rp_solver_;  // restoration phase Solver
397     bool print_maxit_reached_;
398 
399     /** \brief Serialize an object without type information */
400     void serialize_body(SerializingStream &s) const override;
401 
402     /** \brief Deserialize into MX */
deserialize(DeserializingStream & s)403     static ProtoFunction* deserialize(DeserializingStream& s) { return new Blocksqp(s); }
404 
405   protected:
406     /** \brief Deserializing constructor */
407     explicit Blocksqp(DeserializingStream& s);
408   };
409 
410 } // namespace casadi
411 
412 /// \endcond
413 #endif // CASADI_BLOCKSQP_HPP
414