qp_solver_qpoases.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00034 #include <acado/bindings/acado_qpoases/qp_solver_qpoases.hpp>
00035 #include <qpOASES-3.0beta/include/qpOASES.hpp>
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 
00040 //
00041 // PUBLIC MEMBER FUNCTIONS:
00042 //
00043 
00044 QPsolver_qpOASES::QPsolver_qpOASES( ) : DenseQPsolver( )
00045 {
00046         qp = 0;
00047 }
00048 
00049 
00050 QPsolver_qpOASES::QPsolver_qpOASES( UserInteraction* _userInteraction ) : DenseQPsolver( _userInteraction )
00051 {
00052         qp = 0;
00053 }
00054 
00055 
00056 QPsolver_qpOASES::QPsolver_qpOASES( const QPsolver_qpOASES& rhs ) : DenseQPsolver( rhs )
00057 {
00058         if ( rhs.qp != 0 )
00059                 qp = new qpOASES::SQProblem( *(rhs.qp) );
00060         else
00061                 qp = 0;
00062 }
00063 
00064 
00065 QPsolver_qpOASES::~QPsolver_qpOASES( )
00066 {
00067         if ( qp != 0 )
00068                 delete qp;
00069 }
00070 
00071 
00072 QPsolver_qpOASES& QPsolver_qpOASES::operator=( const QPsolver_qpOASES& rhs )
00073 {
00074     if ( this != &rhs )
00075     {
00076                 DenseQPsolver::operator=( rhs );
00077 
00078                 if ( qp != 0 )
00079                         delete qp;
00080 
00081 
00082                 if ( rhs.qp != 0 )
00083                         qp = new qpOASES::SQProblem( *(rhs.qp) );
00084                 else
00085                         qp = 0;
00086 
00087     }
00088 
00089     return *this;
00090 }
00091 
00092 
00093 DenseCPsolver* QPsolver_qpOASES::clone( ) const
00094 {
00095         return new QPsolver_qpOASES(*this);
00096 }
00097 
00098 
00099 DenseQPsolver* QPsolver_qpOASES::cloneDenseQPsolver( ) const
00100 {
00101         return new QPsolver_qpOASES(*this);
00102 }
00103 
00104 
00105 returnValue QPsolver_qpOASES::solve( DenseCP *cp_  )
00106 {
00107         return DenseQPsolver::solve( cp_ );
00108 }
00109 
00110 
00111 returnValue QPsolver_qpOASES::solve(    double* H,
00112                                                                                 double* A,
00113                                                                                 double* g,
00114                                                                                 double* lb,
00115                                                                                 double* ub,
00116                                                                                 double* lbA,
00117                                                                                 double* ubA,
00118                                                                                 uint maxIter
00119                                                                                 )
00120 {
00121         if ( qp == 0 )
00122                 return ACADOERROR( RET_INITIALIZE_FIRST );
00123 
00124         /* call to qpOASES, using hotstart if possible and desired */
00125         numberOfSteps = maxIter;
00126         qpOASES::returnValue returnvalue;
00127         qpStatus = QPS_SOLVING;
00128 
00129         //printf( "nV: %d,  nC: %d \n",qp->getNV(),qp->getNC() );
00130 
00131         if ( (bool)qp->isInitialised( ) == false )
00132         {
00133                 returnvalue = qp->init( H,g,A,lb,ub,lbA,ubA,numberOfSteps,0 );
00134         }
00135         else
00136         {
00137                 int performHotstart = 0;
00138                 get( HOTSTART_QP,performHotstart );
00139 
00140                 if ( (bool)performHotstart == true )
00141                 {
00142                          returnvalue = qp->hotstart( H,g,A,lb,ub,lbA,ubA,numberOfSteps,0 );
00143                 }
00144                 else
00145                 {
00146                         /* if no hotstart is desired, reset QP and use cold start */
00147                         qp->reset( );
00148                         returnvalue = qp->init( H,g,A,lb,ub,lbA,ubA,numberOfSteps,0 );
00149                 }
00150         }
00151         setLast( LOG_NUM_QP_ITERATIONS, numberOfSteps );
00152 
00153         /* update QP status and determine return value */
00154         return updateQPstatus( returnvalue );
00155 }
00156 
00157 
00158 returnValue QPsolver_qpOASES::solve( DMatrix *H,
00159                                      DMatrix *A,
00160                                      DVector *g,
00161                                      DVector *lb,
00162                                      DVector *ub,
00163                                      DVector *lbA,
00164                                      DVector *ubA,
00165                                      uint maxIter       )
00166 {
00167         return solve(   H->data(),
00168                                         A->data(),
00169                                         g->data(),
00170                                         lb->data(),
00171                                         ub->data(),
00172                                         lbA->data(),
00173                                         ubA->data(),
00174                                         maxIter
00175                                         );
00176 }
00177 
00178 
00179 
00180 returnValue QPsolver_qpOASES::step(     double* H,
00181                                                                         double* A,
00182                                                                         double* g,
00183                                                                         double* lb,
00184                                                                         double* ub,
00185                                                                         double* lbA,
00186                                                                         double* ubA
00187                                                                         )
00188 {
00189         /* perform a single QP iteration */
00190         return solve( H,A,g,lb,ub,lbA,ubA,1 );
00191 }
00192 
00193 
00194 returnValue QPsolver_qpOASES::step(     DMatrix *H,
00195                                                                         DMatrix *A,
00196                                                                         DVector *g,
00197                                                                         DVector *lb,
00198                                                                         DVector *ub,
00199                                                                         DVector *lbA,
00200                                                                         DVector *ubA
00201                                                                         )
00202 {
00203         /* perform a single QP iteration */
00204         return solve( H,A,g,lb,ub,lbA,ubA,1 );
00205 }
00206 
00207 
00208 returnValue QPsolver_qpOASES::getPrimalSolution( DVector& xOpt ) const
00209 {
00210         if ( qp == 0 )
00211                 return ACADOERROR( RET_INITIALIZE_FIRST );
00212 
00213         uint dim = qp->getNV( );
00214         double* xOptTmp = new double[dim];
00215 
00216         if ( qp->getPrimalSolution( xOptTmp ) == qpOASES::SUCCESSFUL_RETURN )
00217         {
00218                 xOpt = DVector(dim, xOptTmp);
00219                 delete[] xOptTmp;
00220                 return SUCCESSFUL_RETURN;
00221         }
00222         else
00223         {
00224                 delete[] xOptTmp;
00225                 return ACADOERROR( RET_QP_NOT_SOLVED );
00226         }
00227 }
00228 
00229 
00230 returnValue QPsolver_qpOASES::getDualSolution( DVector& yOpt ) const
00231 {
00232         if ( qp == 0 )
00233                 return ACADOERROR( RET_INITIALIZE_FIRST );
00234 
00235         uint dim = qp->getNV( ) + qp->getNC( );
00236         double* yOptTmp = new double[dim];
00237 
00238         if ( qp->getDualSolution( yOptTmp ) == qpOASES::SUCCESSFUL_RETURN )
00239         {
00240                 yOpt = DVector(dim, yOptTmp);
00241                 delete[] yOptTmp;
00242                 return SUCCESSFUL_RETURN;
00243         }
00244         else
00245         {
00246                 delete[] yOptTmp;
00247                 return ACADOERROR( RET_QP_NOT_SOLVED );
00248         }
00249 }
00250 
00251 
00252 double QPsolver_qpOASES::getObjVal( ) const
00253 {
00254         if ( isUnbounded( ) == true )
00255                 return -INFTY;
00256 
00257         if ( ( isSolved( ) == false ) || ( qp == 0 ) )
00258                 return INFTY;
00259 
00260         return qp->getObjVal( );
00261 }
00262 
00263 
00264 returnValue QPsolver_qpOASES::getVarianceCovariance( DMatrix &var ){
00265 
00266     return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00267 }
00268 
00269 
00270 uint QPsolver_qpOASES::getNumberOfVariables( ) const
00271 {
00272         if ( qp != 0 )
00273                 return qp->getNV( );
00274         else
00275                 return 0;
00276 }
00277 
00278 uint QPsolver_qpOASES::getNumberOfConstraints( ) const
00279 {
00280         if ( qp != 0 )
00281                 return qp->getNC( );
00282         else
00283                 return 0;
00284 }
00285 
00286 
00287 returnValue QPsolver_qpOASES::getVarianceCovariance( DMatrix &H, DMatrix &var ){
00288 
00289     if ( qp == 0 )
00290         return ACADOERROR( RET_INITIALIZE_FIRST );
00291 
00292     if ( (bool)isSolved( ) == false ) return ACADOERROR( RET_QP_NOT_SOLVED );
00293 
00294     qpOASES::returnValue      returnvalue;
00295     qpOASES::SolutionAnalysis analyser   ;
00296 
00297     uint             NV, NC     ;
00298     uint             run1, run2 ;
00299 
00300     NV = qp->getNV();
00301     NC = qp->getNC();
00302 
00303     double *Var            = new double[(2*NV+NC)*(2*NV+NC)];
00304     double *PrimalDualVar  = new double[(2*NV+NC)*(2*NV+NC)];
00305 
00306     for( run1 = 0; run1 < (2*NV+NC)*(2*NV+NC); run1++ )
00307         Var[run1] = 0.0;
00308 
00309     for( run1 = 0; run1 < NV; run1++ )
00310         for( run2 = 0; run2 < NV; run2++ )
00311             Var[run1*(2*NV+NC)+run2] = H(run1,run2);
00312 
00313     returnvalue = analyser.getVarianceCovariance( qp, Var, PrimalDualVar );
00314 
00315     if( returnvalue != qpOASES::SUCCESSFUL_RETURN ){
00316         delete[] Var          ;
00317         delete[] PrimalDualVar;
00318         return ACADOERROR(RET_QP_NOT_SOLVED);
00319     }
00320 
00321     var.init( NV, NV );
00322 
00323     for( run1 = 0; run1 < NV; run1++ )
00324         for( run2 = 0; run2 < NV; run2++ )
00325             var( run1, run2 ) = PrimalDualVar[run1*(2*NV+NC)+run2];
00326 
00327     delete[] Var          ;
00328     delete[] PrimalDualVar;
00329     return SUCCESSFUL_RETURN;
00330 }
00331 
00332 
00333 
00334 //
00335 // PROTECTED MEMBER FUNCTIONS:
00336 //
00337 
00338 
00339 returnValue QPsolver_qpOASES::setupQPobject( uint nV, uint nC )
00340 {
00341         if ( qp != 0 )
00342                 delete qp;
00343 
00344         /* create new qpOASES QP object... */
00345         qp = new qpOASES::SQProblem( nV,nC );
00346         
00347         qpOASES::Options options;
00348         options.setToFast();
00349         
00350         qp->setOptions( options );
00351         
00352         /* ... and define its printLevel */
00353         int printLevel = 0;
00354         //get( PRINTLEVEL,printLevel );
00355 
00356         switch( (PrintLevel) printLevel )
00357         {
00358                 case HIGH:
00359                         qp->setPrintLevel( qpOASES::PL_MEDIUM );
00360                         break;
00361 
00362                 case DEBUG:
00363                         qp->setPrintLevel( qpOASES::PL_HIGH );
00364                         break;
00365 
00366                 // PL_NONE, PL_LOW, PL_MEDIUM
00367                 default:
00368                         qp->setPrintLevel( qpOASES::PL_NONE );
00369         }
00370 
00371         qpStatus = QPS_INITIALIZED;
00372 
00373         return SUCCESSFUL_RETURN;
00374 }
00375 
00376 
00377 returnValue QPsolver_qpOASES::updateQPstatus( int ret )
00378 {
00379         switch ( (qpOASES::returnValue)ret )
00380         {
00381                 case qpOASES::SUCCESSFUL_RETURN:
00382                         qpStatus = QPS_SOLVED;
00383                         return SUCCESSFUL_RETURN;
00384 
00385                 case qpOASES::RET_MAX_NWSR_REACHED:
00386                         qpStatus = QPS_NOTSOLVED;
00387                         return RET_QP_SOLUTION_REACHED_LIMIT;
00388 
00389                 default:
00390                         qpStatus = QPS_NOTSOLVED;
00391 
00392                         /* check for infeasibility */
00393                         if ( (bool)qp->isInfeasible( ) == true )
00394                         {
00395                                 qpStatus = QPS_INFEASIBLE;
00396                                 return RET_QP_INFEASIBLE;
00397                         }
00398 
00399                         /* check for unboundedness */
00400                         if ( (bool)qp->isUnbounded( ) == true )
00401                         {
00402                                 qpStatus = QPS_UNBOUNDED;
00403                                 return RET_QP_UNBOUNDED;
00404                         }
00405 
00406                         return RET_QP_SOLUTION_FAILED;
00407         }
00408 }
00409 
00410 
00411 CLOSE_NAMESPACE_ACADO
00412 
00413 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:43