SQProblem.cpp
Go to the documentation of this file.
00001 /*
00002  *      This file is part of qpOASES.
00003  *
00004  *      qpOASES -- An Implementation of the Online Active Set Strategy.
00005  *      Copyright (C) 2007-2011 by Hans Joachim Ferreau, Andreas Potschka,
00006  *      Christian Kirches et al. All rights reserved.
00007  *
00008  *      qpOASES is free software; you can redistribute it and/or
00009  *      modify it under the terms of the GNU Lesser General Public
00010  *      License as published by the Free Software Foundation; either
00011  *      version 2.1 of the License, or (at your option) any later version.
00012  *
00013  *      qpOASES is distributed in the hope that it will be useful,
00014  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016  *      See the GNU Lesser General Public License for more details.
00017  *
00018  *      You should have received a copy of the GNU Lesser General Public
00019  *      License along with qpOASES; if not, write to the Free Software
00020  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  */
00023 
00024 
00037 #include <qpOASES/SQProblem.hpp>
00038 
00039 
00040 BEGIN_NAMESPACE_QPOASES
00041 
00042 
00043 /*****************************************************************************
00044  *  P U B L I C                                                              *
00045  *****************************************************************************/
00046 
00047 
00048 /*
00049  *      S Q P r o b l e m
00050  */
00051 SQProblem::SQProblem( ) : QProblem( )
00052 {
00053 }
00054 
00055 
00056 /*
00057  *      S Q P r o b l e m
00058  */
00059 SQProblem::SQProblem( int _nV, int _nC, HessianType _hessianType ) : QProblem( _nV,_nC,_hessianType )
00060 {
00061 }
00062 
00063 
00064 /*
00065  *      S Q P r o b l e m
00066  */
00067 SQProblem::SQProblem( const SQProblem& rhs ) : QProblem( rhs )
00068 {
00069 }
00070 
00071 
00072 /*
00073  *      ~ S Q P r o b l e m
00074  */
00075 SQProblem::~SQProblem( )
00076 {
00077 }
00078 
00079 
00080 /*
00081  *      o p e r a t o r =
00082  */
00083 SQProblem& SQProblem::operator=( const SQProblem& rhs )
00084 {
00085         if ( this != &rhs )
00086         {
00087                 QProblem::operator=( rhs );
00088         }
00089 
00090         return *this;
00091 }
00092 
00093 
00094 /*
00095  *      h o t s t a r t
00096  */
00097 returnValue SQProblem::hotstart(        const real_t* const H_new, const real_t* const g_new, const real_t* const A_new,
00098                                                                         const real_t* const lb_new, const real_t* const ub_new,
00099                                                                         const real_t* const lbA_new, const real_t* const ubA_new,
00100                                                                         int& nWSR, real_t* const cputime )
00101 {
00102         if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
00103                  ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
00104                  ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
00105         {
00106                 return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
00107         }
00108 
00109         /* start runtime measurement */
00110         real_t starttime = 0.0;
00111     if ( cputime != 0 )
00112         starttime = getCPUtime( );
00113 
00114 
00115         /* I) UPDATE QP MATRICES AND VECTORS */
00116         if ( setupAuxiliaryQP( H_new,A_new ) != SUCCESSFUL_RETURN )
00117                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00118 
00119 
00120         /* II) PERFORM USUAL HOMOTOPY */
00121 
00122         /* Allow only remaining CPU time for usual hotstart. */
00123         if ( cputime != 0 )
00124                 *cputime -= getCPUtime( ) - starttime;
00125 
00126         returnValue returnvalue = QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
00127 
00128 
00129         /* stop runtime measurement */
00130         if ( cputime != 0 )
00131                 *cputime = getCPUtime( ) - starttime;
00132 
00133         return returnvalue;
00134 }
00135 
00136 
00137 /*
00138  *      h o t s t a r t
00139  */
00140 returnValue SQProblem::hotstart(        const char* const H_file, const char* const g_file, const char* const A_file,
00141                                                                         const char* const lb_file, const char* const ub_file,
00142                                                                         const char* const lbA_file, const char* const ubA_file,
00143                                                                         int& nWSR, real_t* const cputime
00144                                                                         )
00145 {
00146         int nV = getNV( );
00147         int nC = getNC( );
00148 
00149         returnValue returnvalue;
00150 
00151         /* consistency checks */
00152         if ( ( H_file == 0 ) || ( g_file == 0 ) )
00153                 return THROWERROR( RET_INVALID_ARGUMENTS );
00154 
00155         if ( ( nC > 0 ) && ( A_file == 0 ) )
00156                 return THROWERROR( RET_INVALID_ARGUMENTS );
00157 
00158 
00159         /* 1) Load new QP matrices from files. */
00160         real_t* H_new  = new real_t[nV*nV];
00161         real_t* A_new  = new real_t[nC*nV];
00162 
00163         if ( readFromFile( H_new, nV,nV, H_file ) != SUCCESSFUL_RETURN )
00164                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00165 
00166         if ( readFromFile( A_new, nC,nV, A_file ) != SUCCESSFUL_RETURN )
00167                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00168 
00169         /* 2) Load new QP vectors from files. */
00170         real_t* g_new  = new real_t[nV];
00171         real_t* lb_new = 0;
00172         real_t* ub_new = 0;
00173         real_t* lbA_new = 0;
00174         real_t* ubA_new = 0;
00175 
00176         if ( lb_file != 0 )
00177                 lb_new = new real_t[nV];
00178         if ( ub_file != 0 )
00179                 ub_new = new real_t[nV];
00180         if ( lbA_file != 0 )
00181                 lbA_new = new real_t[nC];
00182         if ( ubA_file != 0 )
00183                 ubA_new = new real_t[nC];
00184 
00185         returnvalue = loadQPvectorsFromFile(    g_file,lb_file,ub_file,lbA_file,ubA_file,
00186                                                                                         g_new,lb_new,ub_new,lbA_new,ubA_new
00187                                                                                         );
00188         if ( returnvalue != SUCCESSFUL_RETURN )
00189         {
00190                 if ( ubA_file != 0 )
00191                         delete[] ubA_new;
00192                 if ( lbA_file != 0 )
00193                         delete[] lbA_new;
00194                 if ( ub_file != 0 )
00195                         delete[] ub_new;
00196                 if ( lb_file != 0 )
00197                         delete[] lb_new;
00198                 delete[] g_new;
00199                 delete[] A_new;
00200                 delete[] H_new;
00201 
00202                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00203         }
00204 
00205         /* 3) Actually perform hotstart. */
00206         returnvalue = hotstart( H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
00207 
00208         if ( ubA_file != 0 )
00209                 delete[] ubA_new;
00210         if ( lbA_file != 0 )
00211                 delete[] lbA_new;
00212         if ( ub_file != 0 )
00213                 delete[] ub_new;
00214         if ( lb_file != 0 )
00215                 delete[] lb_new;
00216         delete[] g_new;
00217         delete[] A_new;
00218         delete[] H_new;
00219 
00220         return returnvalue;
00221 }
00222 
00223 /*
00224  * h o t s t a r t
00225  */
00226 returnValue SQProblem::hotstart(        SymmetricMatrix *H_new, 
00227                                                                         const real_t* const g_new,
00228                                                                         Matrix *A_new,  
00229                                                                         const real_t* const lb_new,
00230                                                                         const real_t* const ub_new,
00231                                                                         const real_t* const lbA_new,
00232                                                                         const real_t* const ubA_new,
00233                                                                         int& nWSR,                                      
00234                                                                         real_t* const cputime           
00235                                                                         )
00236 {
00237         if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
00238                  ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
00239                  ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
00240         {
00241                 return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
00242         }
00243 
00244         /* start runtime measurement */
00245         real_t starttime = 0.0;
00246     if ( cputime != 0 )
00247         starttime = getCPUtime( );
00248 
00249 
00250         /* I) UPDATE QP MATRICES AND VECTORS */
00251         if ( setupAuxiliaryQP( H_new,A_new ) != SUCCESSFUL_RETURN )
00252                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00253 
00254 
00255         /* II) PERFORM USUAL HOMOTOPY */
00256 
00257         /* Allow only remaining CPU time for usual hotstart. */
00258         if ( cputime != 0 )
00259                 *cputime -= getCPUtime( ) - starttime;
00260 
00261         returnValue returnvalue = QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
00262 
00263 
00264         /* stop runtime measurement */
00265         if ( cputime != 0 )
00266                 *cputime = getCPUtime( ) - starttime;
00267 
00268         return returnvalue;     
00269 }
00270 
00271 
00272 /*
00273  *      h o t s t a r t
00274  */
00275 returnValue SQProblem::hotstart(        const real_t* const g_new,
00276                                                                         const real_t* const lb_new, const real_t* const ub_new,
00277                                                                         const real_t* const lbA_new, const real_t* const ubA_new,
00278                                                                         int& nWSR, real_t* const cputime
00279                                                                         )
00280 {
00281         /* Call to hotstart function for fixed QP matrices. */
00282         return QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
00283 }
00284 
00285 
00286 /*
00287  *      h o t s t a r t
00288  */
00289 returnValue SQProblem::hotstart(        const char* const g_file,
00290                                                                         const char* const lb_file, const char* const ub_file,
00291                                                                         const char* const lbA_file, const char* const ubA_file,
00292                                                                         int& nWSR, real_t* const cputime
00293                                                                         )
00294 {
00295         /* Call to hotstart function for fixed QP matrices. */
00296         return QProblem::hotstart( g_file,lb_file,ub_file,lbA_file,ubA_file, nWSR,cputime );
00297 }
00298 
00299 
00300 /*
00301  *      h o t s t a r t
00302  */
00303 returnValue SQProblem::hotstart(        const real_t* const g_new,
00304                                                                         const real_t* const lb_new, const real_t* const ub_new,
00305                                                                         const real_t* const lbA_new, const real_t* const ubA_new,
00306                                                                         int& nWSR, real_t* const cputime,
00307                                                                         const Bounds* const guessedBounds, const Constraints* const guessedConstraints
00308                                                                         )
00309 {
00310         /* Call to hotstart function for fixed QP matrices. */
00311         return QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime, guessedBounds,guessedConstraints );
00312 }
00313 
00314 
00315 /*
00316  *      h o t s t a r t
00317  */
00318 returnValue SQProblem::hotstart(        const char* const g_file,
00319                                                                         const char* const lb_file, const char* const ub_file,
00320                                                                         const char* const lbA_file, const char* const ubA_file,
00321                                                                         int& nWSR, real_t* const cputime,
00322                                                                         const Bounds* const guessedBounds, const Constraints* const guessedConstraints
00323                                                                         )
00324 {
00325         /* Call to hotstart function for fixed QP matrices. */
00326         return QProblem::hotstart( g_file,lb_file,ub_file,lbA_file,ubA_file, nWSR,cputime, guessedBounds,guessedConstraints );
00327 }
00328 
00329 
00330 
00331 #ifdef __MATLAB__
00332 returnValue SQProblem::resetMatrixPointers( )
00333 {
00334         H = 0;
00335         A = 0;
00336         
00337         return SUCCESSFUL_RETURN;
00338 }
00339 #endif
00340 
00341 
00342 
00343 /*****************************************************************************
00344  *  P R O T E C T E D                                                        *
00345  *****************************************************************************/
00346 
00347 /*
00348  *      s e t u p A u x i l i a r y Q P
00349  */
00350 returnValue SQProblem::setupAuxiliaryQP( const real_t* const H_new, const real_t* const A_new )
00351 {
00352         int nV = getNV( );
00353         int nC = getNC( );
00354 
00355         DenseMatrix *dA = new DenseMatrix(nC, nV, nV, (real_t*) A_new);
00356         SymDenseMat *sH = new SymDenseMat(nV, nV, nV, (real_t*) H_new);
00357 
00358         returnValue returnvalue = setupAuxiliaryQP ( sH, dA );
00359 
00360         if ( H_new != 0 )
00361                 freeHessian = BT_TRUE;
00362         freeConstraintMatrix = BT_TRUE;
00363 
00364         return returnvalue;
00365 }
00366 
00367 
00368 /*
00369  *      s e t u p A u x i l i a r y Q P
00370  */
00371 returnValue SQProblem::setupAuxiliaryQP ( SymmetricMatrix *H_new, Matrix *A_new )
00372 {
00373 
00374         int i;
00375         int nV = getNV( );
00376         int nC = getNC( );
00377         returnValue returnvalue;
00378 
00379         if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
00380                  ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
00381                  ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
00382         {
00383                 return THROWERROR( RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED );
00384         }
00385 
00386         status = QPS_PREPARINGAUXILIARYQP;
00387 
00388 
00389         /* I) SETUP NEW QP MATRICES AND VECTORS: */
00390         /* 1) Shift constraints' bounds vectors by (A_new - A)'*x_opt to ensure
00391          *    that old optimal solution remains feasible for new QP data. */
00392         /*    Firstly, shift by -A'*x_opt and ... */
00393         if ( nC > 0 )
00394         {
00395                 if ( A_new == 0 )
00396                         return THROWERROR( RET_INVALID_ARGUMENTS );
00397 
00398                 for ( i=0; i<nC; ++i )
00399                 {
00400                         lbA[i] = -Ax_l[i];
00401                         ubA[i] =  Ax_u[i];
00402                 }
00403 
00404                 /* Set constraint matrix as well as ... */
00405                 setA( A_new );
00406 
00407                 /* ... secondly, shift by +A_new'*x_opt. */
00408                 for ( i=0; i<nC; ++i )
00409                 {
00410                         lbA[i] += Ax[i];
00411                         ubA[i] += Ax[i];
00412                 }
00413 
00414                 /* update constraint products. */
00415                 for ( i=0; i<nC; ++i )
00416                 {
00417                         Ax_u[i] = ubA[i] - Ax[i];
00418                         Ax_l[i] = Ax[i] - lbA[i];
00419                 }
00420         }
00421 
00422         /* 2) Set new Hessian matrix,determine Hessian type and
00423          *    regularise new Hessian matrix if necessary. */
00424         /* a) Setup new Hessian matrix and determine its type. */
00425         if ( H_new != 0 )
00426         {
00427                 setH( H_new );
00428 
00429                 hessianType = HST_UNKNOWN;
00430                 if ( determineHessianType( ) != SUCCESSFUL_RETURN )
00431                         return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00432 
00433                 /* b) Regularise new Hessian if necessary. */
00434                 if ( ( hessianType == HST_ZERO ) ||
00435                          ( hessianType == HST_SEMIDEF ) ||
00436                          ( usingRegularisation( ) == BT_TRUE ) )
00437                 {
00438                         isRegularised = BT_FALSE; /* reset previous regularisation */
00439 
00440                         if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
00441                                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00442                 }
00443         }
00444         else
00445         {
00446                 if ( H != 0 )
00447                         return THROWERROR( RET_NO_HESSIAN_SPECIFIED );
00448         }
00449 
00450 
00451 
00452         /* 3) Setup QP gradient. */
00453         if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
00454                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00455 
00456 
00457         /* II) SETUP WORKING SETS AND MATRIX FACTORISATIONS: */
00458         /* 1) Make a copy of current bounds/constraints ... */
00459         Bounds      oldBounds      = bounds;
00460         Constraints oldConstraints = constraints;
00461 
00462         /*    ... reset them ... */
00463         bounds.init( nV );
00464         constraints.init( nC );
00465 
00466         /*    ... and set them up afresh. */
00467         if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
00468                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00469 
00470         if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
00471                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00472 
00473         if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
00474                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00475 
00476         /* 2) Setup TQ factorisation. */
00477         if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
00478                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00479 
00480         /* 3) Setup old working sets afresh (updating TQ factorisation). */
00481         if ( setupAuxiliaryWorkingSet( &oldBounds,&oldConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
00482                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00483 
00484         /* 4) Calculate Cholesky decomposition. */
00485         if ( ( getNAC( ) + getNFX( ) ) == 0 )
00486         {
00487                 /* Factorise full Hessian if no bounds/constraints are active. */
00488                 returnvalue = setupCholeskyDecomposition( );
00489         }
00490         else
00491         {
00492                 /* Factorise projected Hessian if there active bounds/constraints. */
00493                 returnvalue = setupCholeskyDecompositionProjected( );
00494         }
00495         if ( returnvalue != SUCCESSFUL_RETURN )
00496         {
00497                 /* recede to trivial active set */
00498                 bounds.init(nV);
00499                 if (options.initialStatusBounds == ST_LOWER)
00500                         bounds.setupAllLower();
00501                 else
00502                         bounds.setupAllUpper();
00503                 constraints.init(nC);
00504                 constraints.setupAllInactive();
00505 
00506                 if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
00507                         return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00508         }
00509 
00510         status = QPS_AUXILIARYQPSOLVED;
00511 
00512         return SUCCESSFUL_RETURN;
00513 }
00514 
00515 END_NAMESPACE_QPOASES
00516 
00517 
00518 /*
00519  *      end of file
00520  */


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