OQPinterface.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 
00038 #include <qpOASES/extras/OQPinterface.hpp>
00039 #include <qpOASES/QProblem.hpp>
00040 
00041 
00042 BEGIN_NAMESPACE_QPOASES
00043 
00044 
00045 /*
00046  *      r e a d O Q P d i m e n s i o n s
00047  */
00048 returnValue readOQPdimensions(  const char* path,
00049                                                                 int& nQP, int& nV, int& nC, int& nEC
00050                                                                 )
00051 {
00052         /* 1) Setup file name where dimensions are stored. */
00053         char filename[160];
00054         snprintf( filename,160,"%sdims.oqp",path );
00055 
00056         /* 2) Load dimensions from file. */
00057         int dims[4];
00058         if ( readFromFile( dims,4,filename ) != SUCCESSFUL_RETURN )
00059                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00060 
00061         nQP = dims[0];
00062         nV  = dims[1];
00063         nC  = dims[2];
00064         nEC = dims[3];
00065 
00066 
00067         /* consistency check */
00068         if ( ( nQP <= 0 ) || ( nV <= 0 ) || ( nC < 0 ) || ( nEC < 0 ) )
00069                 return THROWERROR( RET_FILEDATA_INCONSISTENT );
00070 
00071         return SUCCESSFUL_RETURN;
00072 }
00073 
00074 
00075 /*
00076  *      r e a d O Q P d a t a
00077  */
00078 returnValue readOQPdata(        const char* path,
00079                                                         int& nQP, int& nV, int& nC, int& nEC,
00080                                                         real_t** H, real_t** g, real_t** A, real_t** lb, real_t** ub, real_t** lbA, real_t** ubA,
00081                                                         real_t** xOpt, real_t** yOpt, real_t** objOpt
00082                                                         )
00083 {
00084         char filename[160];
00085 
00086         /* consistency check */
00087         if ( ( H == 0 ) || ( g == 0 ) || ( lb == 0 ) || ( ub == 0 ) )
00088                 return THROWERROR( RET_INVALID_ARGUMENTS );
00089 
00090 
00091         /* 1) Obtain OQP dimensions. */
00092         if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
00093                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00094 
00095 
00096         /* another consistency check */
00097         if ( ( nC > 0 ) && ( ( A == 0 ) || ( lbA == 0 ) || ( ubA == 0 ) ) )
00098                 return THROWERROR( RET_FILEDATA_INCONSISTENT );
00099 
00100 
00101         /* 2) Allocate memory and load OQP data: */
00102         /* Hessian matrix */
00103         *H  = new real_t[nV*nV];
00104         snprintf( filename,160,"%sH.oqp",path );
00105         if ( readFromFile( *H,nV,nV,filename ) != SUCCESSFUL_RETURN )
00106         {
00107                 delete[] *H;
00108                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00109         }
00110 
00111         /* gradient vector sequence */
00112         *g  = new real_t[nQP*nV];
00113         snprintf( filename,160,"%sg.oqp",path );
00114         if ( readFromFile( *g,nQP,nV,filename ) != SUCCESSFUL_RETURN )
00115         {
00116                 delete[] *g; delete[] *H;
00117                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00118         }
00119 
00120         /* lower bound vector sequence */
00121         *lb  = new real_t[nQP*nV];
00122         snprintf( filename,160,"%slb.oqp",path );
00123         if ( readFromFile( *lb,nQP,nV,filename ) != SUCCESSFUL_RETURN )
00124         {
00125                 delete[] *lb; delete[] *g; delete[] *H;
00126                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00127         }
00128 
00129         /* upper bound vector sequence */
00130         *ub  = new real_t[nQP*nV];
00131         snprintf( filename,160,"%sub.oqp",path );
00132         if ( readFromFile( *ub,nQP,nV,filename ) != SUCCESSFUL_RETURN )
00133         {
00134                 delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
00135                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00136         }
00137 
00138         if ( nC > 0 )
00139         {
00140                 /* Constraint matrix */
00141                 *A   = new real_t[nC*nV];
00142                 snprintf( filename,160,"%sA.oqp",path );
00143                 if ( readFromFile( *A,nC,nV,filename ) != SUCCESSFUL_RETURN )
00144                 {
00145                         delete[] *A;
00146                         delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
00147                         return THROWERROR( RET_UNABLE_TO_READ_FILE );
00148                 }
00149 
00150                 /* lower constraints' bound vector sequence */
00151                 *lbA = new real_t[nQP*nC];
00152                 snprintf( filename,160,"%slbA.oqp",path );
00153                 if ( readFromFile( *lbA,nQP,nC,filename ) != SUCCESSFUL_RETURN )
00154                 {
00155                         delete[] *lbA; delete[] *A;
00156                         delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
00157                         return THROWERROR( RET_UNABLE_TO_READ_FILE );
00158                 }
00159 
00160                 /* upper constraints' bound vector sequence */
00161                 *ubA = new real_t[nQP*nC];
00162                 snprintf( filename,160,"%subA.oqp",path );
00163                 if ( readFromFile( *ubA,nQP,nC,filename ) != SUCCESSFUL_RETURN )
00164                 {
00165                         delete[] *ubA; delete[] *lbA; delete[] *A;
00166                         delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
00167                         return THROWERROR( RET_UNABLE_TO_READ_FILE );
00168                 }
00169         }
00170         else
00171         {
00172                 *A = 0;
00173                 *lbA = 0;
00174                 *ubA = 0;
00175         }
00176 
00177         if ( xOpt != 0 )
00178         {
00179                 /* primal solution vector sequence */
00180                 *xOpt = new real_t[nQP*nV];
00181                 snprintf( filename,160,"%sx_opt.oqp",path );
00182                 if ( readFromFile( *xOpt,nQP,nV,filename ) != SUCCESSFUL_RETURN )
00183                 {
00184                         delete[] xOpt;
00185                         if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
00186                         delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
00187                         return THROWERROR( RET_UNABLE_TO_READ_FILE );
00188                 }
00189         }
00190 
00191         if ( yOpt != 0 )
00192         {
00193                 /* dual solution vector sequence */
00194                 *yOpt = new real_t[nQP*(nV+nC)];
00195                 snprintf( filename,160,"%sy_opt.oqp",path );
00196                 if ( readFromFile( *yOpt,nQP,nV+nC,filename ) != SUCCESSFUL_RETURN )
00197                 {
00198                         delete[] yOpt;
00199                         if ( xOpt != 0 ) { delete[] xOpt; };
00200                         if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
00201                         delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
00202                         return THROWERROR( RET_UNABLE_TO_READ_FILE );
00203                 }
00204         }
00205 
00206         if ( objOpt != 0 )
00207         {
00208                 /* dual solution vector sequence */
00209                 *objOpt = new real_t[nQP];
00210                 snprintf( filename,160,"%sobj_opt.oqp",path );
00211                 if ( readFromFile( *objOpt,nQP,1,filename ) != SUCCESSFUL_RETURN )
00212                 {
00213                         delete[] objOpt;
00214                         if ( yOpt != 0 ) { delete[] yOpt; };
00215                         if ( xOpt != 0 ) { delete[] xOpt; };
00216                         if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
00217                         delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
00218                         return THROWERROR( RET_UNABLE_TO_READ_FILE );
00219                 }
00220         }
00221 
00222         return SUCCESSFUL_RETURN;
00223 }
00224 
00225 
00226 /*
00227  *      s o l v e O Q P b e n c h m a r k
00228  */
00229 returnValue solveOQPbenchmark(  int nQP, int nV, int nC, int nEC,
00230                                                                 const real_t* const _H, const real_t* const g, const real_t* const _A,
00231                                                                 const real_t* const lb, const real_t* const ub,
00232                                                                 const real_t* const lbA, const real_t* const ubA,
00233                                                                 BooleanType isSparse, 
00234                                                                 const Options& options, int& nWSR, real_t& maxCPUtime,
00235                                                                 real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
00236                                                                 )
00237 {
00238         int k;
00239 
00240         /* I) SETUP AUXILIARY VARIABLES: */
00241         /* 1) Keep nWSR and store current and maximum number of
00242          *    working set recalculations in temporary variables */
00243         int nWSRcur;
00244         int maxNWSR = 0;
00245 
00246         real_t CPUtimeLimit = maxCPUtime;
00247         real_t CPUtimeCur = CPUtimeLimit;
00248         maxCPUtime = 0.0;
00249         maxStationarity = 0.0;
00250         maxFeasibility = 0.0;
00251         maxComplementarity = 0.0;
00252         real_t stat, feas, cmpl;
00253 
00254         /* 2) Pointers to data of current QP ... */
00255         const real_t* gCur;
00256         const real_t* lbCur;
00257         const real_t* ubCur;
00258         const real_t* lbACur;
00259         const real_t* ubACur;
00260 
00261         /* 3) Vectors for solution obtained by qpOASES. */
00262         real_t* x = new real_t[nV];
00263         real_t* y = new real_t[nV+nC];
00264 
00265         /* 4) Prepare matrix objects */
00266         SymmetricMatrix *H; 
00267         Matrix *A;
00268         if ( isSparse == BT_TRUE)
00269         {
00270                 SymSparseMat *Hs;
00271                 H = Hs = new SymSparseMat(nV, nV, nV, _H);
00272                 A = new SparseMatrix(nC, nV, nV, _A);
00273                 Hs->createDiagInfo();
00274         }
00275         else
00276         {
00277                 H = new SymDenseMat(nV, nV, nV, const_cast<real_t *>(_H));
00278                 A = new DenseMatrix(nC, nV, nV, const_cast<real_t *>(_A));
00279         }
00280 
00281 
00282         /* II) SETUP QPROBLEM OBJECT */
00283         QProblem qp( nV,nC );
00284         qp.setOptions( options );
00285         qp.setPrintLevel( PL_LOW );
00286 
00287         /* III) RUN BENCHMARK SEQUENCE: */
00288         returnValue returnvalue;
00289 
00290         for( k=0; k<nQP; ++k )
00291         {
00292                 /* 1) Update pointers to current QP data. */
00293                 gCur   = &( g[k*nV] );
00294                 lbCur  = &( lb[k*nV] );
00295                 ubCur  = &( ub[k*nV] );
00296                 lbACur = &( lbA[k*nC] );
00297                 ubACur = &( ubA[k*nC] );
00298 
00299                 /* 2) Set nWSR and maximum CPU time. */
00300                 nWSRcur = nWSR;
00301                 CPUtimeCur = CPUtimeLimit;
00302 
00303                 /* 3) Solve current QP. */
00304                 if ( k == 0 )
00305                 {
00306                         /* initialise */
00307                         returnvalue = qp.init( H,gCur,A,lbCur,ubCur,lbACur,ubACur, nWSRcur,&CPUtimeCur );
00308                         if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
00309                         {
00310                                 delete A; delete H; delete[] y; delete[] x;
00311                                 return THROWERROR( returnvalue );
00312                         }
00313                 }
00314                 else
00315                 {
00316                         /* hotstart */
00317                         returnvalue = qp.hotstart( gCur,lbCur,ubCur,lbACur,ubACur, nWSRcur,&CPUtimeCur );
00318                         if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
00319                         {
00320                                 delete A; delete H; delete[] y; delete[] x;
00321                                 return THROWERROR( returnvalue );
00322                         }
00323                 }
00324 
00325                 /* 4) Obtain solution vectors and objective function value */
00326                 qp.getPrimalSolution( x );
00327                 qp.getDualSolution( y );
00328 
00329                 /* 5) Compute KKT residuals */
00330                 getKKTResidual( nV, nC, _H,gCur,_A,lbCur,ubCur,lbACur,ubACur, x, y, stat, feas, cmpl );
00331                 
00332                 /* 6) Update maximum values. */
00333                 if ( nWSRcur > maxNWSR )
00334                         maxNWSR = nWSRcur;
00335                 if (stat > maxStationarity) maxStationarity = stat;
00336                 if (feas > maxFeasibility) maxFeasibility = feas;
00337                 if (cmpl > maxComplementarity) maxComplementarity = cmpl;
00338 
00339                 if ( CPUtimeCur > maxCPUtime )
00340                         maxCPUtime = CPUtimeCur;
00341         }
00342         nWSR = maxNWSR;
00343 
00344         delete A; delete H; delete[] y; delete[] x;
00345 
00346         return SUCCESSFUL_RETURN;
00347 }
00348 
00349 
00350 /*
00351  *      s o l v e O Q P b e n c h m a r k
00352  */
00353 returnValue solveOQPbenchmark(  int nQP, int nV,
00354                                                                 const real_t* const _H, const real_t* const g,
00355                                                                 const real_t* const lb, const real_t* const ub,
00356                                                                 BooleanType isSparse, 
00357                                                                 const Options& options, int& nWSR, real_t& maxCPUtime,
00358                                                                 real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
00359                                                                 )
00360 {
00361         int k;
00362 
00363         /* I) SETUP AUXILIARY VARIABLES: */
00364         /* 1) Keep nWSR and store current and maximum number of
00365          *    working set recalculations in temporary variables */
00366         int nWSRcur;
00367         int maxNWSR = 0;
00368 
00369         real_t CPUtimeLimit = maxCPUtime;
00370         real_t CPUtimeCur = CPUtimeLimit;
00371         real_t stat, feas, cmpl;
00372         maxCPUtime = 0.0;
00373         maxStationarity = 0.0;
00374         maxFeasibility = 0.0;
00375         maxComplementarity = 0.0;
00376 
00377         /* 2) Pointers to data of current QP ... */
00378         const real_t* gCur;
00379         const real_t* lbCur;
00380         const real_t* ubCur;
00381 
00382         /* 3) Vectors for solution obtained by qpOASES. */
00383         real_t* x = new real_t[nV];
00384         real_t* y = new real_t[nV];
00385 
00386         /* 4) Prepare matrix objects */
00387         SymmetricMatrix *H; 
00388         if ( isSparse == BT_TRUE )
00389         {
00390                 SymSparseMat *Hs;
00391                 H = Hs = new SymSparseMat(nV, nV, nV, _H);
00392                 Hs->createDiagInfo();
00393         }
00394         else
00395         {
00396                 H = new SymDenseMat(nV, nV, nV, const_cast<real_t *>(_H));
00397         }
00398 
00399         /* II) SETUP QPROBLEM OBJECT */
00400         QProblemB qp( nV );
00401         qp.setOptions( options );
00402         qp.setPrintLevel( PL_LOW );
00403 
00404 
00405         /* III) RUN BENCHMARK SEQUENCE: */
00406         returnValue returnvalue;
00407 
00408         for( k=0; k<nQP; ++k )
00409         {
00410                 /* 1) Update pointers to current QP data. */
00411                 gCur   = &( g[k*nV] );
00412                 lbCur  = &( lb[k*nV] );
00413                 ubCur  = &( ub[k*nV] );
00414 
00415                 /* 2) Set nWSR and maximum CPU time. */
00416                 nWSRcur = nWSR;
00417                 CPUtimeCur = CPUtimeLimit;
00418 
00419                 /* 3) Solve current QP. */
00420                 if ( k == 0 )
00421                 {
00422                         /* initialise */
00423                         returnvalue = qp.init( H,gCur,lbCur,ubCur, nWSRcur,&CPUtimeCur );
00424                         if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
00425                         {
00426                                 delete H; delete[] y; delete[] x;
00427                                 return THROWERROR( returnvalue );
00428                         }
00429                 }
00430                 else
00431                 {
00432                         /* hotstart */
00433                         returnvalue = qp.hotstart( gCur,lbCur,ubCur, nWSRcur,&CPUtimeCur );
00434                         if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
00435                         {
00436                                 delete H; delete[] y; delete[] x;
00437                                 return THROWERROR( returnvalue );
00438                         }
00439                 }
00440 
00441                 /* 4) Obtain solution vectors and objective function value ... */
00442                 qp.getPrimalSolution( x );
00443                 qp.getDualSolution( y );
00444 
00445                 /* 5) Compute KKT residuals */
00446                 getKKTResidual( nV,0, _H,gCur,0,lbCur,ubCur,0,0, x,y, stat,feas,cmpl );
00447 
00448                 /* 6) update maximum values. */
00449                 if ( nWSRcur > maxNWSR )
00450                         maxNWSR = nWSRcur;
00451                 if (stat > maxStationarity) maxStationarity = stat;
00452                 if (feas > maxFeasibility) maxFeasibility = feas;
00453                 if (cmpl > maxComplementarity) maxComplementarity = cmpl;
00454 
00455                 if ( CPUtimeCur > maxCPUtime )
00456                         maxCPUtime = CPUtimeCur;
00457         }
00458 
00459         delete H; delete[] y; delete[] x;
00460 
00461         return SUCCESSFUL_RETURN;
00462 }
00463 
00464 
00465 /*
00466  *      r u n O Q P b e n c h m a r k
00467  */
00468 returnValue runOQPbenchmark(    const char* path, BooleanType isSparse, const Options& options,
00469                                                                 int& nWSR, real_t& maxCPUtime,
00470                                                                 real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
00471                                                                 )
00472 {
00473         int nQP=0, nV=0, nC=0, nEC=0;
00474 
00475         real_t *H=0, *g=0, *A=0, *lb=0, *ub=0, *lbA=0, *ubA=0;
00476 
00477 
00478         returnValue returnvalue;
00479 
00480         /* I) SETUP BENCHMARK: */
00481         /* 1) Obtain QP sequence dimensions. */
00482         if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
00483                 return THROWERROR( RET_BENCHMARK_ABORTED );
00484 
00485         /* 2) Read OQP benchmark data. */
00486         if ( readOQPdata(       path,
00487                                                 nQP,nV,nC,nEC,
00488                                                 &H,&g,&A,&lb,&ub,&lbA,&ubA,
00489                                                 0,0,0
00490                                                 ) != SUCCESSFUL_RETURN )
00491         {
00492                 return THROWERROR( RET_UNABLE_TO_READ_BENCHMARK );
00493         }
00494 
00495 
00496         /* II) SOLVE BENCHMARK */
00497         if ( nC > 0 )
00498         {
00499                 returnvalue = solveOQPbenchmark(        nQP,nV,nC,nEC,
00500                                                                                         H,g,A,lb,ub,lbA,ubA,
00501                                                                                         isSparse,options,
00502                                                                                         nWSR,maxCPUtime,
00503                                                                                         maxStationarity,maxFeasibility,maxComplementarity
00504                                                                                         );
00505 
00506                 if ( returnvalue != SUCCESSFUL_RETURN )
00507                 {
00508                         delete[] H; delete[] A;
00509                         delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] g;
00510                         return THROWERROR( returnvalue );
00511                 }
00512         }
00513         else
00514         {
00515                 returnvalue = solveOQPbenchmark(        nQP,nV,
00516                                                                                         H,g,lb,ub,
00517                                                                                         isSparse,options,
00518                                                                                         nWSR,maxCPUtime,
00519                                                                                         maxStationarity,maxFeasibility,maxComplementarity
00520                                                                                         );
00521 
00522                 if ( returnvalue != SUCCESSFUL_RETURN )
00523                 {
00524                         delete[] H; delete[] A;
00525                         delete[] ub; delete[] lb; delete[] g;
00526                         return THROWERROR( returnvalue );
00527                 }
00528         }
00529 
00530         delete[] H; delete[] A;
00531         delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] g;
00532 
00533         return SUCCESSFUL_RETURN;
00534 }
00535 
00536 
00537 END_NAMESPACE_QPOASES
00538 
00539 
00540 /*
00541  *      end of file
00542  */


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