external_packages/qpOASES-3.2.0/include/qpOASES/QProblemB.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-2015 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_QPROBLEMB_HPP
39 #define QPOASES_QPROBLEMB_HPP
40 
41 
42 #include <qpOASES/Flipper.hpp>
43 #include <qpOASES/Options.hpp>
44 #include <qpOASES/Matrices.hpp>
45 
46 
48 
49 
50 class SolutionAnalysis;
51 
63 class QProblemB
64 {
65  /* allow SolutionAnalysis class to access private members */
66  friend class SolutionAnalysis;
67 
68  /*
69  * PUBLIC MEMBER FUNCTIONS
70  */
71  public:
73  QProblemB( );
74 
80  QProblemB( int_t _nV,
81  HessianType _hessianType = HST_UNKNOWN
82  );
83 
85  QProblemB( const QProblemB& rhs
86  );
87 
89  virtual ~QProblemB( );
90 
92  virtual QProblemB& operator=( const QProblemB& rhs
93  );
94 
95 
99  virtual returnValue reset( );
100 
101 
123  const real_t* const _g,
124  const real_t* const _lb,
126  const real_t* const _ub,
128  int_t& nWSR,
130  real_t* const cputime = 0,
132  const real_t* const xOpt = 0,
134  const real_t* const yOpt = 0,
136  const Bounds* const guessedBounds = 0,
138  const real_t* const _R = 0
142  );
143 
164  returnValue init( const real_t* const _H,
166  const real_t* const _g,
167  const real_t* const _lb,
169  const real_t* const _ub,
171  int_t& nWSR,
173  real_t* const cputime = 0,
175  const real_t* const xOpt = 0,
177  const real_t* const yOpt = 0,
179  const Bounds* const guessedBounds = 0,
181  const real_t* const _R = 0
185  );
186 
207  returnValue init( const char* const H_file,
209  const char* const g_file,
210  const char* const lb_file,
212  const char* const ub_file,
214  int_t& nWSR,
216  real_t* const cputime = 0,
218  const real_t* const xOpt = 0,
220  const real_t* const yOpt = 0,
222  const Bounds* const guessedBounds = 0,
224  const char* const R_file = 0
227  );
228 
229 
248  returnValue hotstart( const real_t* const g_new,
249  const real_t* const lb_new,
251  const real_t* const ub_new,
253  int_t& nWSR,
255  real_t* const cputime = 0,
257  const Bounds* const guessedBounds = 0
259  );
260 
282  returnValue hotstart( const char* const g_file,
283  const char* const lb_file,
285  const char* const ub_file,
287  int_t& nWSR,
289  real_t* const cputime = 0,
291  const Bounds* const guessedBounds = 0
293  );
294 
295 
299  virtual returnValue getWorkingSet( real_t* workingSet
300  );
301 
305  virtual returnValue getWorkingSetBounds( real_t* workingSetB
306  );
307 
311  virtual returnValue getWorkingSetConstraints( real_t* workingSetC
312  );
313 
314 
318  inline returnValue getBounds( Bounds& _bounds
319  ) const;
320 
321 
324  inline int_t getNV( ) const;
325 
328  inline int_t getNFR( ) const;
329 
332  inline int_t getNFX( ) const;
333 
336  inline int_t getNFV( ) const;
337 
340  virtual int_t getNZ( ) const;
341 
342 
346  real_t getObjVal( ) const;
347 
350  real_t getObjVal( const real_t* const _x
351  ) const;
352 
357  ) const;
358 
362  virtual returnValue getDualSolution( real_t* const yOpt
363  ) const;
364 
365 
368  inline QProblemStatus getStatus( ) const;
369 
370 
374  inline BooleanType isInitialised( ) const;
375 
379  inline BooleanType isSolved( ) const;
380 
384  inline BooleanType isInfeasible( ) const;
385 
389  inline BooleanType isUnbounded( ) const;
390 
391 
394  inline HessianType getHessianType( ) const;
395 
398  inline returnValue setHessianType( HessianType _hessianType
399  );
400 
404  inline BooleanType usingRegularisation( ) const;
405 
408  inline Options getOptions( ) const;
409 
412  inline returnValue setOptions( const Options& _options
413  );
414 
417  inline PrintLevel getPrintLevel( ) const;
418 
421  returnValue setPrintLevel( PrintLevel _printlevel
422  );
423 
424 
427  inline uint_t getCount( ) const;
428 
431  inline returnValue resetCounter( );
432 
433 
436  virtual returnValue printProperties( );
437 
440  returnValue printOptions( ) const;
441 
442 
443  /*
444  * PROTECTED MEMBER FUNCTIONS
445  */
446  protected:
449  returnValue clear( );
450 
453  returnValue copy( const QProblemB& rhs
454  );
455 
462 
466  virtual returnValue setupSubjectToType( );
467 
471  virtual returnValue setupSubjectToType( const real_t* const lb_new,
472  const real_t* const ub_new
473  );
474 
483  virtual returnValue computeCholesky( );
484 
485 
492 
500  const real_t* const yOpt,
502  const Bounds* const guessedBounds,
503  Bounds* auxiliaryBounds
505  ) const;
506 
513  const real_t* const ub
514  ) const;
515 
519  virtual returnValue backsolveR( const real_t* const b,
520  BooleanType transposed,
521  real_t* const a
522  ) const;
523 
528  virtual returnValue backsolveR( const real_t* const b,
529  BooleanType transposed,
530  BooleanType removingBound,
531  real_t* const a
532  ) const;
533 
534 
538  const real_t* const lb_new,
539  const real_t* const ub_new,
540  real_t* const delta_g,
541  real_t* const delta_lb,
542  real_t* const delta_ub,
543  BooleanType& Delta_bB_isZero
544  );
545 
546 
551  const real_t* const _g,
552  const real_t* const _lb,
554  const real_t* const _ub
556  );
557 
564  returnValue setupQPdata( const real_t* const _H,
566  const real_t* const _g,
567  const real_t* const _lb,
569  const real_t* const _ub
571  );
572 
581  returnValue setupQPdataFromFile( const char* const H_file,
583  const char* const g_file,
584  const char* const lb_file,
586  const char* const ub_file
588  );
589 
595  returnValue loadQPvectorsFromFile( const char* const g_file,
596  const char* const lb_file,
598  const char* const ub_file,
600  real_t* const g_new,
601  real_t* const lb_new,
602  real_t* const ub_new
603  ) const;
604 
605 
612  BooleanType doThrowError = BT_FALSE
613  );
614 
615 
619  BooleanType isCPUtimeLimitExceeded( const real_t* const cputime,
620  real_t starttime,
621  int_t nWSR
622  ) const;
623 
624 
629 
630 
634  );
635 
641  inline returnValue setH( const real_t* const H_new
642  );
643 
647  inline returnValue setG( const real_t* const g_new
648  );
649 
653  inline returnValue setLB( const real_t* const lb_new
654  );
655 
660  inline returnValue setLB( int_t number,
661  real_t value
662  );
663 
667  inline returnValue setUB( const real_t* const ub_new
668  );
669 
674  inline returnValue setUB( int_t number,
675  real_t value
676  );
677 
678 
681  inline void computeGivens( real_t xold,
682  real_t yold,
683  real_t& xnew,
684  real_t& ynew,
685  real_t& c,
686  real_t& s
687  ) const;
688 
691  inline void applyGivens( real_t c,
692  real_t s,
693  real_t nu,
694  real_t xold,
696  real_t yold,
698  real_t& xnew,
700  real_t& ynew
702  ) const;
703 
704 
705 
710  const real_t* const lb_new,
711  const real_t* const ub_new
712  );
713 
717  virtual returnValue performRamping( );
718 
719 
721  returnValue updateFarBounds( real_t curFarBound,
722  int_t nRamp,
723  const real_t* const lb_new,
724  real_t* const lb_new_far,
725  const real_t* const ub_new,
726  real_t* const ub_new_far
727  ) const;
728 
729 
734  const int_t* const idxList,
735  const SubjectTo* const subjectTo,
736  const real_t* const num,
737  const real_t* const den,
738  real_t epsNum,
739  real_t epsDen,
740  real_t& t,
741  int_t& BC_idx
742  ) const;
743 
747  inline BooleanType isBlocking( real_t num,
748  real_t den,
749  real_t epsNum,
750  real_t epsDen,
751  real_t& t
753  ) const;
754 
755 
761  real_t diagVal = 1.0
762  );
763 
764 
765  /*
766  * PRIVATE MEMBER FUNCTIONS
767  */
768  private:
780  returnValue solveInitialQP( const real_t* const xOpt,
781  const real_t* const yOpt,
782  const Bounds* const guessedBounds,
783  const real_t* const _R,
784  int_t& nWSR,
786  real_t* const cputime
788  );
789 
802  returnValue solveQP( const real_t* const g_new,
803  const real_t* const lb_new,
805  const real_t* const ub_new,
807  int_t& nWSR,
809  real_t* const cputime,
811  int_t nWSRperformed = 0,
815  BooleanType isFirstCall = BT_TRUE
816  );
817 
818 
832  const real_t* const lb_new,
834  const real_t* const ub_new,
836  int_t& nWSR,
838  real_t* const cputime,
840  int_t nWSRperformed = 0,
843  BooleanType isFirstCall = BT_TRUE
844  );
845 
846 
854  returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds,
855  BooleanType setupAfresh
857  );
858 
863  const real_t* const yOpt
865  );
866 
872 
879  );
880 
881 
882  protected:
888  virtual returnValue setupAuxiliaryQP( const Bounds* const guessedBounds
889  );
890 
891  private:
895  returnValue determineStepDirection( const real_t* const delta_g,
896  const real_t* const delta_lb,
897  const real_t* const delta_ub,
898  BooleanType Delta_bB_isZero,
899  real_t* const delta_xFX,
900  real_t* const delta_xFR,
901  real_t* const delta_yFX
902  );
903 
909  returnValue performStep( const real_t* const delta_g,
910  const real_t* const delta_lb,
911  const real_t* const delta_ub,
912  const real_t* const delta_xFX,
913  const real_t* const delta_xFR,
914  const real_t* const delta_yFX,
915  int_t& BC_idx,
916  SubjectToStatus& BC_status
917  );
918 
924  SubjectToStatus BC_status
925  );
926 
930 
935  BooleanType shallRefactorise( const Bounds* const guessedBounds
936  ) const;
937 
938 
942  returnValue addBound( int_t number,
943  SubjectToStatus B_status,
944  BooleanType updateCholesky
945  );
946 
951  returnValue removeBound( int_t number,
952  BooleanType updateCholesky
953  );
954 
955 
959  int_t BC_idx,
960  SubjectToStatus BC_status,
961  real_t homotopyLength,
962  BooleanType isFirstCall = BT_TRUE
963  );
964 
965 
966  /*
967  * PROTECTED MEMBER VARIABLES
968  */
969  protected:
971  SymmetricMatrix* H;
973  real_t* g;
974  real_t* lb;
975  real_t* ub;
977  Bounds bounds;
979  real_t* R;
982  real_t* x;
983  real_t* y;
985  real_t tau;
999  real_t ramp0;
1000  real_t ramp1;
1003  Options options;
1005  Flipper flipper;
1008 };
1009 
1010 
1012 
1013 #include <qpOASES/QProblemB.ipp>
1014 
1015 #endif /* QPOASES_QPROBLEMB_HPP */
1016 
1017 
1018 /*
1019  * end of file
1020  */
real_t getRelativeHomotopyLength(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new)
HessianType getHessianType() const
Interfaces matrix-vector operations tailored to symmetric sparse matrices.
BooleanType isInfeasible() const
returnValue copy(const QProblemB &rhs)
Implements the online active set strategy for box-constrained QPs.
int getNFV() const
BooleanType isUnbounded() const
int getNFX()
void computeGivens(real_t xold, real_t yold, real_t &xnew, real_t &ynew, real_t &c, real_t &s) const
returnValue updateFarBounds(real_t curFarBound, int_t nRamp, const real_t *const lb_new, real_t *const lb_new_far, const real_t *const ub_new, real_t *const ub_new_far) const
returnValue obtainAuxiliaryWorkingSet(const real_t *const xOpt, const real_t *const yOpt, const Bounds *const guessedBounds, Bounds *auxiliaryBounds) const
BooleanType shallRefactorise(const Bounds *const guessedBounds) const
Allows to pass back messages to the calling function.
returnValue solveRegularisedQP(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, int &nWSR, real_t *const cputime, int nWSRperformed=0)
Base class for managing working sets of bounds and constraints.
returnValue determineStepDirection(const real_t *const delta_g, const real_t *const delta_lb, const real_t *const delta_ub, BooleanType Delta_bB_isZero, real_t *const delta_xFX, real_t *const delta_xFR, real_t *const delta_yFX)
virtual returnValue getWorkingSet(real_t *workingSet)
returnValue setupAuxiliaryWorkingSet(const Bounds *const auxiliaryBounds, BooleanType setupAfresh)
returnValue getBounds(Bounds *const _bounds) const
int getNV() const
returnValue setH(const real_t *const H_new)
returnValue setInfeasibilityFlag(returnValue returnvalue)
returnValue setupAuxiliaryQPsolution(const real_t *const xOpt, const real_t *const yOpt)
returnValue backsolveR(const real_t *const b, BooleanType transposed, real_t *const a)
returnValue init(const real_t *const _H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub, int &nWSR, const real_t *const yOpt=0, real_t *const cputime=0)
Auxiliary class for storing a copy of the current matrix factorisations.
SymSparseMat * createDiagSparseMat(int_t n, real_t diagVal=1.0)
Options getOptions() const
returnValue setOptions(const Options &_options)
returnValue changeActiveSet(int BC_idx, SubjectToStatus BC_status)
BooleanType usingRegularisation() const
BooleanType isCPUtimeLimitExceeded(const real_t *const cputime, real_t starttime, int nWSR) const
returnValue hotstart(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, int &nWSR, real_t *const cputime)
returnValue performRatioTest(int nIdx, const int *const idxList, const SubjectTo *const subjectTo, const real_t *const num, const real_t *const den, real_t epsNum, real_t epsDen, real_t &t, int &BC_idx) const
virtual returnValue getWorkingSetConstraints(real_t *workingSetC)
BooleanType isBlocking(real_t num, real_t den, real_t epsNum, real_t epsDen, real_t &t) const
returnValue determineDataShift(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, real_t *const delta_g, real_t *const delta_lb, real_t *const delta_ub, BooleanType &Delta_bB_isZero)
returnValue addBound(int number, SubjectToStatus B_status, BooleanType updateCholesky)
returnValue setupQPdata(const real_t *const _H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub)
const int nu
returnValue printIteration(int iteration, int BC_idx, SubjectToStatus BC_status)
Provides a generic way to set and pass user-specified options.
Definition: options.hpp:65
returnValue performStep(const real_t *const delta_g, const real_t *const delta_lb, const real_t *const delta_ub, const real_t *const delta_xFX, const real_t *const delta_xFR, const real_t *const delta_yFX, int &BC_idx, SubjectToStatus &BC_status)
BooleanType areBoundsConsistent(const real_t *const delta_lb, const real_t *const delta_ub) const
uint_t getCount() const
PrintLevel getPrintLevel() const
int getNFR()
returnValue setupQPdataFromFile(const char *const H_file, const char *const g_file, const char *const lb_file, const char *const ub_file)
returnValue setHessianType(HessianType _hessianType)
void rhs(const real_t *x, real_t *f)
PrintLevel
returnValue resetCounter()
virtual returnValue getWorkingSetBounds(real_t *workingSetB)
returnValue setupAuxiliaryQP(const Bounds *const guessedBounds)
BooleanType isInitialised() const
QProblemStatus getStatus() const
#define BT_TRUE
Definition: acado_types.hpp:47
Stores internal information for tabular (debugging) output.
returnValue setUB(const real_t *const ub_new)
returnValue setG(const real_t *const g_new)
#define HST_UNKNOWN
returnValue removeBound(int number, BooleanType updateCholesky)
returnValue solveQP(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, int &nWSR, real_t *const cputime, int nWSRperformed=0)
#define BT_FALSE
Definition: acado_types.hpp:49
Manages working sets of bounds (= box constraints).
double real_t
Definition: AD_test.c:10
void applyGivens(real_t c, real_t s, real_t xold, real_t yold, real_t &xnew, real_t &ynew) const
returnValue loadQPvectorsFromFile(const char *const g_file, const char *const lb_file, const char *const ub_file, real_t *const g_new, real_t *const lb_new, real_t *const ub_new) const
returnValue solveInitialQP(const real_t *const xOpt, const real_t *const yOpt, const Bounds *const guessedBounds, int &nWSR, real_t *const cputime)
returnValue setLB(const real_t *const lb_new)
Abstract base class for interfacing matrix-vector operations tailored to symmetric matrices...
BooleanType isSolved() const


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