dense_qp_solver.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/conic_solver/dense_qp_solver.hpp>
00035 
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 
00040 //
00041 // PUBLIC MEMBER FUNCTIONS:
00042 //
00043 
00044 DenseQPsolver::DenseQPsolver( ) : DenseCPsolver( )
00045 {
00046         setupLogging( );
00047         
00048     qpStatus = QPS_NOT_INITIALIZED;
00049     numberOfSteps = 0;
00050 }
00051 
00052 
00053 DenseQPsolver::DenseQPsolver( UserInteraction* _userInteraction ) : DenseCPsolver( _userInteraction )
00054 {
00055         setupLogging( );
00056         
00057     qpStatus = QPS_NOT_INITIALIZED;
00058     numberOfSteps = 0;
00059 }
00060 
00061 
00062 DenseQPsolver::DenseQPsolver( const DenseQPsolver& rhs ) : DenseCPsolver( rhs )
00063 {
00064     qpStatus = rhs.qpStatus;
00065     numberOfSteps = rhs.numberOfSteps;
00066 }
00067 
00068 
00069 DenseQPsolver::~DenseQPsolver( ){
00070 
00071 }
00072 
00073 
00074 DenseQPsolver& DenseQPsolver::operator=( const DenseQPsolver& rhs ){
00075 
00076     if ( this != &rhs ){
00077 
00078         DenseCPsolver::operator=( rhs );
00079 
00080         qpStatus = rhs.qpStatus;
00081     }
00082     return *this;
00083 }
00084 
00085 
00086 returnValue DenseQPsolver::init( uint nV, uint nC )
00087 {
00088         //printf( "nV: %d,   nC: %d!!\n",nV,nC );
00089         return setupQPobject( nV,nC );
00090 }
00091 
00092 
00093 returnValue DenseQPsolver::init( const DenseCP *cp )
00094 {
00095     ASSERT( cp != 0 );
00096 
00097     if( cp->isQP() == BT_FALSE )
00098         return ACADOERROR( RET_QP_SOLVER_CAN_ONLY_SOLVE_QP );
00099 
00100     return init( cp->getNV(),cp->getNC() );
00101 }
00102 
00103 
00104 returnValue DenseQPsolver::solve( DenseCP *cp )
00105 {
00106     ASSERT( cp != 0 );
00107 
00108     if( cp->isQP() == BT_FALSE )
00109         return ACADOERROR( RET_QP_SOLVER_CAN_ONLY_SOLVE_QP );
00110 
00111         if ( makeBoundsConsistent( cp ) != SUCCESSFUL_RETURN )
00112                 return ACADOERROR( RET_QP_HAS_INCONSISTENT_BOUNDS );
00113 
00114         int maxNumQPiterations;
00115         get( MAX_NUM_QP_ITERATIONS, maxNumQPiterations );
00116 
00117     returnValue returnvalue;
00118     returnvalue = solve( &cp->H, &cp->A, &cp->g, &cp->lb, &cp->ub, &cp->lbA, &cp->ubA, maxNumQPiterations );
00119 
00120         if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_QP_SOLUTION_REACHED_LIMIT ) )
00121                 return returnvalue;
00122 
00123 
00124     // GET THE PRIMAL AND DUAL SOLUTION FROM THE QP SOLVER AND
00125     // STORE THEM IN THE RIGHT FORMAT:
00126     // -------------------------------------------------------
00127     DVector xOpt, yOpt;
00128 
00129     getPrimalSolution( xOpt );
00130     getDualSolution  ( yOpt );
00131 
00132 //      xOpt.print("xOpt");
00133 //      yOpt.print("yOpt");
00134 
00135     cp->setQPsolution( xOpt,yOpt );
00136         
00137     return returnvalue;
00138 }
00139 
00140 
00141 uint DenseQPsolver::getNumberOfIterations( ) const
00142 {
00143     return numberOfSteps;
00144 }
00145 
00146 
00147 
00148 
00149 //
00150 // PROTECTED MEMBER FUNCTIONS:
00151 //
00152 
00153 
00154 returnValue DenseQPsolver::setupLogging( )
00155 {
00156         LogRecord tmp(LOG_AT_EACH_ITERATION, PS_DEFAULT);
00157 
00158         tmp.addItem( LOG_NUM_QP_ITERATIONS );
00159         tmp.addItem( LOG_IS_QP_RELAXED );
00160 
00161         addLogRecord( tmp );
00162   
00163         return SUCCESSFUL_RETURN;
00164 }
00165 
00166 
00167 
00168 returnValue DenseQPsolver::makeBoundsConsistent(        DenseCP *cp
00169                                                                                                         ) const
00170 {
00171         uint i;
00172         double mean;
00173 
00174         if ( cp->lb.getDim( ) != cp->ub.getDim( ) )
00175                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00176 
00177         for( i=0; i<cp->lb.getDim( ); ++i )
00178         {
00179                 if ( cp->lb(i) > cp->ub(i) )
00180                 {
00181                         if ( cp->lb(i) > cp->ub(i)+SQRT_EPS )
00182                                 return RET_QP_HAS_INCONSISTENT_BOUNDS;
00183                         else
00184                         {
00185                                 mean = ( cp->lb(i) + cp->ub(i) ) / 2.0;
00186                                 cp->lb(i) = mean;
00187                                 cp->ub(i) = mean;
00188                         }
00189                 }
00190         }
00191 
00192         if ( cp->lbA.getDim( ) != cp->ubA.getDim( ) )
00193                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00194 
00195         for( i=0; i<cp->lbA.getDim( ); ++i )
00196         {
00197                 if ( cp->lbA(i) > cp->ubA(i) )
00198                 {
00199                         if ( cp->lbA(i) > cp->ubA(i)+SQRT_EPS )
00200                                 return RET_QP_HAS_INCONSISTENT_BOUNDS;
00201                         else
00202                         {
00203                                 mean = ( cp->lbA(i) + cp->ubA(i) ) / 2.0;
00204                                 cp->lbA(i) = mean;
00205                                 cp->ubA(i) = mean;
00206                         }
00207                 }
00208         }
00209         
00210         return SUCCESSFUL_RETURN;
00211 }
00212 
00213 
00214 
00215 CLOSE_NAMESPACE_ACADO
00216 
00217 // end of file.


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