SQProblemSchur.hpp
Go to the documentation of this file.
1 /*
2  * This file is part of qpOASES.
3  *
4  * qpOASES -- An Implementation of the Online Active Set Strategy.
5  * Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
6  * Christian Kirches et al. All rights reserved.
7  *
8  * qpOASES is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU Lesser General Public
10  * License as published by the Free Software Foundation; either
11  * version 2.1 of the License, or (at your option) any later version.
12  *
13  * qpOASES is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16  * See the GNU Lesser General Public License for more details.
17  *
18  * You should have received a copy of the GNU Lesser General Public
19  * License along with qpOASES; if not, write to the Free Software
20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21  *
22  */
23 
24 
38 #ifndef QPOASES_SQPROBLEMSCHUR_HPP
39 #define QPOASES_SQPROBLEMSCHUR_HPP
40 
41 
42 #include <qpOASES/SQProblem.hpp>
43 #include <qpOASES/SparseSolver.hpp>
44 
45 #ifdef __USE_SINGLE_PRECISION__
46 
48  //#define GEQRF sgeqrf_
50  //#define ORMQR sormqr_
52  #define TRTRS strtrs_
53 
54  #define TRCON strcon_
55 
56 #else
57 
59  //#define GEQRF dgeqrf_
61  //#define ORMQR dormqr_
63  #define TRTRS dtrtrs_
64 
65  #define TRCON dtrcon_
66 
67 #endif /* __USE_SINGLE_PRECISION__ */
68 
69 extern "C" {
71  //void dgeqrf_( const unsigned long *M, const unsigned long *N, double *A, const unsigned long *LDA,
72  //double *TAU, double *WORK, const unsigned long *LWORK, int *INFO );
74  //void sgeqrf_( const unsigned long *M, const unsigned long *N, float *A, const unsigned long *LDA,
75  //float *TAU, float *WORK, const unsigned long *LWORK, int *INFO );
76 
78  //void dormqr_( const char *SIDE, const char *TRANS, const unsigned long *M, const unsigned long *N, const unsigned long *K,
79  //double *A, const unsigned long *LDA, double *TAU, double *C, const unsigned long *LDC,
80  //double *WORK, const unsigned long *LWORK, int *INFO );
82  //void sormqr_( const char *SIDE, const char *TRANS, const unsigned long *M, const unsigned long *N, const unsigned long *K,
83  //float *A, const unsigned long *LDA, float *TAU, float *C, const unsigned long *LDC,
84  //float *WORK, const unsigned long *LWORK, int *INFO );
85 
87  void dtrtrs_( const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS,
88  double *A, const unsigned long *LDA, double *B, const unsigned long *LDB, long *INFO );
90  void strtrs_( const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS,
91  float *A, const unsigned long *LDA, float *B, const unsigned long *LDB, long *INFO );
92 
94  void dtrcon_( const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, double *A, const unsigned long *LDA,
95  double *RCOND, double *WORK, const unsigned long *IWORK, long *INFO );
97  void strcon_( const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, float *A, const unsigned long *LDA,
98  float *RCOND, float *WORK, const unsigned long *IWORK, long *INFO );
99 }
100 
102 
103 
115 class SQProblemSchur : public SQProblem
116 {
117  /* allow SolutionAnalysis class to access private members */
118  friend class SolutionAnalysis;
119 
120  /*
121  * PUBLIC MEMBER FUNCTIONS
122  */
123  public:
125  SQProblemSchur( );
126 
132  SQProblemSchur( int_t _nV,
133  int_t _nC,
134  HessianType _hessianType = HST_UNKNOWN,
135  int_t maxSchurUpdates = 75
136  );
137 
140  );
141 
143  virtual ~SQProblemSchur( );
144 
146  virtual SQProblemSchur& operator=( const SQProblemSchur& rhs
147  );
148 
152  virtual returnValue reset( );
153 
156  returnValue resetSchurComplement( BooleanType allowInertiaCorrection );
157 
159  inline int_t getNumFactorizations( ) const;
160 
161  /*
162  * PROTECTED MEMBER FUNCTIONS
163  */
164  protected:
167  returnValue clear( );
168 
172  );
173 
177 
182 
186 
198  Matrix *A_new,
200  const real_t *lb_new,
201  const real_t *ub_new,
202  const real_t *lbA_new,
203  const real_t *ubA_new
204  );
205 
212  virtual returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds,
213  const Constraints* const auxiliaryConstraints,
214  BooleanType setupAfresh
215  );
216 
217 
223  virtual returnValue addConstraint( int_t number,
224  SubjectToStatus C_status,
225  BooleanType updateCholesky,
226  BooleanType ensureLI = BT_TRUE
227  );
228 
234  virtual returnValue addConstraint_checkLI( int_t number
235  );
236 
245  virtual returnValue addConstraint_ensureLI( int_t number,
246  SubjectToStatus C_status
247  );
248 
254  virtual returnValue addBound( int_t number,
255  SubjectToStatus B_status,
256  BooleanType updateCholesky,
257  BooleanType ensureLI = BT_TRUE
258  );
259 
264  virtual returnValue addBound_checkLI( int_t number
265  );
266 
275  virtual returnValue addBound_ensureLI( int_t number,
276  SubjectToStatus B_status
277  );
278 
284  virtual returnValue removeConstraint( int_t number,
285  BooleanType updateCholesky,
286  BooleanType allowFlipping = BT_FALSE,
287  BooleanType ensureNZC = BT_FALSE
288  );
289 
295  virtual returnValue removeBound( int_t number,
296  BooleanType updateCholesky,
297  BooleanType allowFlipping = BT_FALSE,
298  BooleanType ensureNZC = BT_FALSE
299  );
300 
303  virtual returnValue backsolveT( const real_t* const b,
304  BooleanType transposed,
305  real_t* const a
306  ) const;
307 
310  virtual returnValue backsolveR( const real_t* const b,
311  BooleanType transposed,
312  real_t* const a
313  ) const;
314 
317  virtual returnValue backsolveR( const real_t* const b,
318  BooleanType transposed,
319  BooleanType removingBound,
320  real_t* const a
321  ) const;
322 
323 
328  virtual returnValue determineStepDirection( const real_t* const delta_g,
329  const real_t* const delta_lbA,
330  const real_t* const delta_ubA,
331  const real_t* const delta_lb,
332  const real_t* const delta_ub,
333  BooleanType Delta_bC_isZero,
334  BooleanType Delta_bB_isZero,
335  real_t* const delta_xFX,
336  real_t* const delta_xFR,
337  real_t* const delta_yAC,
338  real_t* const delta_yFX
339  );
340 
341  virtual returnValue determineStepDirection2( const real_t* const delta_g,
342  const real_t* const delta_lbA,
343  const real_t* const delta_ubA,
344  const real_t* const delta_lb,
345  const real_t* const delta_ub,
346  BooleanType Delta_bC_isZero,
347  BooleanType Delta_bB_isZero,
348  real_t* const delta_xFX,
349  real_t* const delta_xFR,
350  real_t* const delta_yAC,
351  real_t* const delta_yFX
352  );
353 
354  /*
355  * PRIVATE MEMBER FUNCTION
356  */
357  private:
364  real_t* const xiC,
365  real_t* const xiX
366  );
367 
374  real_t* const xiC,
375  real_t* const xiX
376  );
377 
380  returnValue computeMTimes( real_t alpha, const real_t* const x, real_t beta, real_t* const y );
381 
384  returnValue computeMTransTimes( real_t alpha, const real_t* const x, real_t beta, real_t* const y );
385 
387  returnValue addToSchurComplement( int_t number, SchurUpdateType update, int_t numNonzerosM, const sparse_int_t* M_pos, const real_t* const M_vals, int_t numNonzerosN, const sparse_int_t* Npos, const real_t* const Nvals, real_t N_diag );
388 
391 
394 
396  real_t calcDetSchur( int_t idxDel );
397 
399  returnValue updateSchurQR( int_t idxDel );
400 
402  returnValue backsolveSchurQR( int_t dimS, const real_t* const rhs, int_t dimRhs, real_t* const sol );
403 
406 
409 
410  returnValue stepCalcRhs( int_t nFR, int_t nFX, int_t nAC, int_t* FR_idx, int_t* FX_idx, int_t* AC_idx, real_t& rhs_max, const real_t* const delta_g,
411  const real_t* const delta_lbA, const real_t* const delta_ubA,
412  const real_t* const delta_lb, const real_t* const delta_ub,
413  BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
414  real_t* const delta_xFX, real_t* const delta_xFR,
415  real_t* const delta_yAC, real_t* const delta_yFX
416  );
417 
418  returnValue stepCalcReorder(int_t nFR, int_t nAC, int_t* FR_idx, int_t* AC_idx, int_t nFRStart, int_t nACStart,
419  int_t* FR_idxStart, int_t* AC_idxStart, int_t* FR_iSort, int_t* FR_iSortStart,
420  int_t* AC_iSort, int_t* AC_iSortStart, real_t* rhs);
421 
423  int_t dim, real_t* rhs, real_t* sol );
424 
425  returnValue stepCalcReorder2( int_t nFR, int_t nAC, int_t* FR_idx, int_t* AC_idx, int_t nFRStart, int_t nACStart,
426  int_t* FR_idxStart, int_t* AC_idxStart, int_t* FR_iSort, int_t* FR_iSortStart,
427  int_t* AC_iSort, int_t* AC_iSortStart, real_t* sol, real_t* const delta_xFR, real_t* const delta_yAC);
428 
430  BooleanType Delta_bC_isZero, real_t* const delta_xFX, real_t* const delta_xFR,
431  real_t* const delta_yAC, const real_t* const delta_g,
432  const real_t* const delta_lbA, const real_t* const delta_ubA, real_t& rnrm);
433 
434  returnValue stepCalcDeltayFx( int_t nFR, int_t nFX, int_t nAC, int_t* FX_idx, const real_t* const delta_g,
435  real_t* const delta_xFX, real_t* const delta_xFR, real_t* const delta_yAC, real_t* const delta_yFX);
436 
437  /*
438  * PROTECTED MEMBER VARIABLES
439  */
440  protected:
463 };
464 
465 
467 
468 #include <qpOASES/SQProblemSchur.ipp>
469 
470 #endif /* QPOASES_QPROBLEMSCHUR_HPP */
471 
472 
473 /*
474  * end of file
475  */
#define N
returnValue stepCalcBacksolveSchur(int_t nFR, int_t nFX, int_t nAC, int_t *FR_idx, int_t *FX_idx, int_t *AC_idx, int_t dim, real_t *rhs, real_t *sol)
virtual returnValue addBound(int_t number, SubjectToStatus B_status, BooleanType updateCholesky, BooleanType ensureLI=BT_TRUE)
virtual returnValue setupTQfactorisation()
returnValue deleteFromSchurComplement(int_t idx, BooleanType allowUndo=BT_FALSE)
virtual returnValue backsolveT(const real_t *const b, BooleanType transposed, real_t *const a) const
returnValue undoDeleteFromSchurComplement(int_t idx)
Implements the online active set strategy for QPs with varying matrices.
Allows to pass back messages to the calling function.
sparse_int_t * M_jc
virtual returnValue addConstraint_checkLI(int_t number)
virtual returnValue computeProjectedCholesky()
returnValue resetSchurComplement(BooleanType allowInertiaCorrection)
returnValue computeMTimes(real_t alpha, const real_t *const x, real_t beta, real_t *const y)
SparseSolver * sparseSolver
void dtrcon_(const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, double *A, const unsigned long *LDA, double *RCOND, double *WORK, const unsigned long *IWORK, long *INFO)
int_t * schurUpdateIndex
#define WORK(I)
void strtrs_(const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS, float *A, const unsigned long *LDA, float *B, const unsigned long *LDB, long *INFO)
virtual returnValue determineStepDirection2(const real_t *const delta_g, const real_t *const delta_lbA, const real_t *const delta_ubA, const real_t *const delta_lb, const real_t *const delta_ub, BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero, real_t *const delta_xFX, real_t *const delta_xFR, real_t *const delta_yAC, real_t *const delta_yFX)
real_t calcDetSchur(int_t idxDel)
sparse_int_t * M_ir
virtual returnValue reset()
returnValue addToSchurComplement(int_t number, SchurUpdateType update, int_t numNonzerosM, const sparse_int_t *M_pos, const real_t *const M_vals, int_t numNonzerosN, const sparse_int_t *Npos, const real_t *const Nvals, real_t N_diag)
returnValue correctInertia()
virtual returnValue addBound_ensureLI(int_t number, SubjectToStatus B_status)
virtual SQProblemSchur & operator=(const SQProblemSchur &rhs)
virtual returnValue addBound_checkLI(int_t number)
returnValue updateSchurQR(int_t idxDel)
Indexlist constraintsActiveStart
virtual ~SQProblemSchur()
returnValue stepCalcRhs(int_t nFR, int_t nFX, int_t nAC, int_t *FR_idx, int_t *FX_idx, int_t *AC_idx, real_t &rhs_max, const real_t *const delta_g, const real_t *const delta_lbA, const real_t *const delta_ubA, const real_t *const delta_lb, const real_t *const delta_ub, BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero, real_t *const delta_xFX, real_t *const delta_xFR, real_t *const delta_yAC, real_t *const delta_yFX)
returnValue repairSingularWorkingSet()
returnValue stepCalcResid(int_t nFR, int_t nFX, int_t nAC, int_t *FR_idx, int_t *FX_idx, int_t *AC_idx, BooleanType Delta_bC_isZero, real_t *const delta_xFX, real_t *const delta_xFR, real_t *const delta_yAC, const real_t *const delta_g, const real_t *const delta_lbA, const real_t *const delta_ubA, real_t &rnrm)
virtual returnValue addConstraint_ensureLI(int_t number, SubjectToStatus C_status)
real_t x[NVMAX]
Definition: QProblem.h:77
virtual returnValue addConstraint(int_t number, SubjectToStatus C_status, BooleanType updateCholesky, BooleanType ensureLI=BT_TRUE)
SchurUpdateType * schurUpdate
returnValue addConstraint_checkLISchur(int_t number, real_t *const xiC, real_t *const xiX)
Abstract base class for interfacing tailored matrix-vector operations.
virtual returnValue setupAuxiliaryWorkingSet(const Bounds *const auxiliaryBounds, const Constraints *const auxiliaryConstraints, BooleanType setupAfresh)
BEGIN_NAMESPACE_QPOASES const char *const TRANS
virtual returnValue determineStepDirection(const real_t *const delta_g, const real_t *const delta_lbA, const real_t *const delta_ubA, const real_t *const delta_lb, const real_t *const delta_ub, BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero, real_t *const delta_xFX, real_t *const delta_xFR, real_t *const delta_yAC, real_t *const delta_yFX)
void rhs(const real_t *x, real_t *f)
returnValue stepCalcReorder2(int_t nFR, int_t nAC, int_t *FR_idx, int_t *AC_idx, int_t nFRStart, int_t nACStart, int_t *FR_idxStart, int_t *AC_idxStart, int_t *FR_iSort, int_t *FR_iSortStart, int_t *AC_iSort, int_t *AC_iSortStart, real_t *sol, real_t *const delta_xFR, real_t *const delta_yAC)
real_t y[NVMAX+NCMAX]
Definition: QProblem.h:78
virtual returnValue removeConstraint(int_t number, BooleanType updateCholesky, BooleanType allowFlipping=BT_FALSE, BooleanType ensureNZC=BT_FALSE)
#define BT_TRUE
Definition: acado_types.hpp:47
returnValue computeMTransTimes(real_t alpha, const real_t *const x, real_t beta, real_t *const y)
returnValue copy(const SQProblemSchur &rhs)
Generic interface for sparse solvers to be coupled with ACADO Toolkit.
virtual returnValue setupAuxiliaryQP(SymmetricMatrix *H_new, Matrix *A_new, const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new)
#define HST_UNKNOWN
Indexlist boundsFreeStart
virtual returnValue backsolveR(const real_t *const b, BooleanType transposed, real_t *const a) const
virtual returnValue removeBound(int_t number, BooleanType updateCholesky, BooleanType allowFlipping=BT_FALSE, BooleanType ensureNZC=BT_FALSE)
virtual returnValue computeInitialCholesky()
returnValue addBound_checkLISchur(int_t number, real_t *const xiC, real_t *const xiX)
#define BT_FALSE
Definition: acado_types.hpp:49
Manages working sets of bounds (= box constraints).
Implements the online active set strategy for QPs with varying, sparse matrices.
returnValue clear()
void strcon_(const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, float *A, const unsigned long *LDA, float *RCOND, float *WORK, const unsigned long *IWORK, long *INFO)
double real_t
Definition: AD_test.c:10
returnValue stepCalcDeltayFx(int_t nFR, int_t nFX, int_t nAC, int_t *FX_idx, const real_t *const delta_g, real_t *const delta_xFX, real_t *const delta_xFR, real_t *const delta_yAC, real_t *const delta_yFX)
int_t getNumFactorizations() const
void dtrtrs_(const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS, double *A, const unsigned long *LDA, double *B, const unsigned long *LDB, long *INFO)
returnValue backsolveSchurQR(int_t dimS, const real_t *const rhs, int_t dimRhs, real_t *const sol)
returnValue stepCalcReorder(int_t nFR, int_t nAC, int_t *FR_idx, int_t *AC_idx, int_t nFRStart, int_t nACStart, int_t *FR_idxStart, int_t *AC_idxStart, int_t *FR_iSort, int_t *FR_iSortStart, int_t *AC_iSort, int_t *AC_iSortStart, real_t *rhs)
Abstract base class for interfacing matrix-vector operations tailored to symmetric matrices...


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:35:11