qpOASES_sequence.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/QProblem.hpp>
00039 
00040 
00041 using namespace qpOASES;
00042 
00043 #include <qpOASES_matlab_utils.cpp>
00044 
00045 
00046 /* global pointer to QP object */
00047 static QProblem* globalQP = 0;
00048 static SymmetricMatrix* globalQP_H = 0;
00049 static Matrix* globalQP_A = 0;
00050 static long* globalQP_Hdiag = 0;
00051 
00052 
00053 /*
00054  *      a l l o c a t e G l o b a l Q P r o b l e m I n s t a n c e
00055  */
00056 void allocateGlobalQProblemInstance(    int nV, int nC, Options* options
00057                                                                                 )
00058 {
00059         globalQP = new QProblem( nV,nC );
00060         globalQP->setOptions( *options );
00061 
00062         return;
00063 }
00064 
00065 
00066 /*
00067  *      d e l e t e G l o b a l Q P r o b l e m I n s t a n c e
00068  */
00069 void deleteGlobalQProblemInstance( )
00070 {
00071         if ( globalQP != 0 )
00072         {
00073                 delete globalQP;
00074                 globalQP = 0;
00075         }
00076 
00077         return;
00078 }
00079 
00080 
00081 /*
00082  *      d e l e t e G l o b a l Q P r o b l e m M a t r i c e s
00083  */
00084 void deleteGlobalQProblemMatrices( )
00085 {
00086         if ( globalQP_H != 0 )
00087         {
00088                 delete globalQP_H;
00089                 globalQP_H = 0;
00090         }
00091 
00092         if ( globalQP_A != 0 )
00093         {
00094                 delete globalQP_A;
00095                 globalQP_A = 0;
00096         }
00097 
00098         if ( globalQP_Hdiag != 0 )
00099         {
00100                 delete[] globalQP_Hdiag;
00101                 globalQP_Hdiag = 0;
00102         }
00103 
00104         return;
00105 }
00106 
00107 
00108 /*
00109  *      i n i t
00110  */
00111 void init(      int nV, int nC,
00112                         SymmetricMatrix *H, real_t* g, Matrix *A,
00113                         const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
00114                         int nWSR, const real_t* const x0, Options* options,
00115                         int nOutputs, mxArray* plhs[]
00116                         )
00117 {
00118         /* 1) Setup initial QP. */
00119         allocateGlobalQProblemInstance( nV,nC,options );
00120 
00121         /* 2) Solve initial QP. */
00122         returnValue returnvalue;
00123 
00124         if ( x0 == 0 )
00125                 returnvalue = globalQP->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
00126         else
00127                 returnvalue = globalQP->init( H,g,A,lb,ub,lbA,ubA, nWSR,0, x0,0,0,0 );
00128 
00129         /* 3) Assign lhs arguments. */
00130         obtainOutputs(  0,globalQP,returnvalue,nWSR,
00131                                         nOutputs,plhs,0,0 );
00132 
00133         return;
00134 }
00135 
00136 
00137 /*
00138  *      h o t s t a r t
00139  */
00140 void hotstart(  const real_t* const g,
00141                                 const real_t* const lb, const real_t* const ub,
00142                                 const real_t* const lbA, const real_t* const ubA,
00143                                 int nWSR, Options* options,
00144                                 int nOutputs, mxArray* plhs[]
00145                                 )
00146 {
00147         /* 1) Solve QP with given options. */
00148         globalQP->setOptions( *options );
00149         returnValue returnvalue = globalQP->hotstart( g,lb,ub,lbA,ubA, nWSR,0 );
00150 
00151         /* 2) Assign lhs arguments. */
00152         obtainOutputs(  0,globalQP,returnvalue,nWSR,
00153                                         nOutputs,plhs,0,0 );
00154 
00155         return;
00156 }
00157 
00158 
00159 /*
00160  *      m e x F u n c t i o n
00161  */
00162 void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
00163 {
00164         unsigned int i,j;
00165 
00166         /* inputs */
00167         char* typeString;
00168         real_t *H_for=0, *H_mem=0, *g=0, *A_for=0, *A_mem=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
00169 
00170         Options options;
00171         options.printLevel = PL_LOW;
00172         #ifdef __DEBUG__
00173         options.printLevel = PL_HIGH;
00174         #endif
00175         #ifdef __SUPPRESSANYOUTPUT__
00176         options.printLevel = PL_NONE;
00177         #endif
00178 
00179         /* dimensions */
00180         unsigned int nV=0, nC=0;
00181 
00182 
00183         /* I) CONSISTENCY CHECKS: */
00184         /* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
00185         if ( ( nrhs < 6 ) || ( nrhs > 10 ) )
00186                 if ( nrhs != 1 )
00187                         mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
00188 
00189         /* 2) Ensure that first input is a string ... */
00190         if ( mxIsChar( prhs[0] ) != 1 )
00191                 mexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
00192 
00193         typeString = (char*) mxGetPr( prhs[0] );
00194 
00195         /*    ... and if so, check if it is an allowed one. */
00196         if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
00197                  ( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
00198                  ( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
00199         {
00200                 mexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
00201         }
00202 
00203 
00204         /* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
00205         /* 1) Init (without or with initial guess for primal solution). */
00206         if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
00207         {
00208                 /* consistency checks */
00209                 if ( ( nlhs < 1 ) || ( nlhs > 5 ) )
00210                         mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
00211 
00212                 if ( ( nrhs < 8 ) || ( nrhs > 10 ) )
00213                         mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
00214 
00215                 /* ensure that data is given in real_t precision */
00216                 if ( ( mxIsDouble( prhs[1] ) == 0 ) ||
00217                          ( mxIsDouble( prhs[2] ) == 0 ) ||
00218                          ( mxIsDouble( prhs[3] ) == 0 ) )
00219                         mexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
00220 
00221 
00222                 /* ensure that matrices are stored in dense format */
00223 //              if ( ( mxIsSparse( prhs[1] ) != 0 ) || ( mxIsSparse( prhs[3] ) != 0 ) )
00224 //                      mexErrMsgTxt( "ERROR (qpOASES): Matrices must not be stored in sparse format!" );
00225 
00226                 /* Check inputs dimensions and assign pointers to inputs. */
00227                 nV = mxGetM( prhs[1] ); /* row number of Hessian matrix */
00228                 nC = mxGetM( prhs[3] ); /* row number of constraint matrix */
00229 
00230                 if ( ( mxGetN( prhs[1] ) != nV ) || ( ( mxGetN( prhs[3] ) != 0 ) && ( mxGetN( prhs[3] ) != nV ) ) )
00231                         mexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
00232 
00233 
00234                 if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
00235                         return;
00236 
00237                 if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
00238                         return;
00239 
00240                 if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
00241                         return;
00242 
00243                 if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
00244                         return;
00245 
00246                 if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,7 ) != SUCCESSFUL_RETURN )
00247                         return;
00248 
00249                 /* default value for nWSR */
00250                 int nWSRin = 5*(nV+nC);
00251 
00252 
00253                 /* Check whether x0 and options are specified .*/
00254                 if ( nrhs > 8 )
00255                 {
00256                         if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,8 ) != SUCCESSFUL_RETURN )
00257                                 return;
00258 
00259                         if ( nrhs > 9 )
00260                                 if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
00261                                         setupOptions( &options,prhs[9],nWSRin );
00262                 }
00263 
00264                 deleteGlobalQProblemInstance( );
00265                 deleteGlobalQProblemMatrices( );
00266 
00267                 /* check for sparsity */
00268                 if ( mxIsSparse( prhs[1] ) != 0 )
00269                 {
00270                         long *ir = (long *)mxGetIr(prhs[1]);
00271                         long *jc = (long *)mxGetJc(prhs[1]);
00272                         real_t *v = (real_t*)mxGetPr(prhs[1]);
00273                         /*
00274                         for (long col = 0; col < nV; col++)
00275                                 for (long idx = jc[col]; idx < jc[col+1]; idx++)
00276                                         mexPrintf("   (%ld,%ld) %12.4f\n", ir[idx]+1, col+1, v[idx]);
00277                                         */
00278                         //mexPrintf( "%ld\n", ir[0] );
00279                         SymSparseMat *sH;
00280                         globalQP_H = sH = new SymSparseMat(nV, nV, ir, jc, v);
00281                         globalQP_Hdiag = sH->createDiagInfo();
00282                 }
00283                 else
00284                 {
00285                         H_for = (real_t*) mxGetPr( prhs[1] );
00286                         H_mem = new real_t[nV*nV];
00287                         for( int i=0; i<nV*nV; ++i )
00288                                 H_mem[i] = H_for[i];
00289                         globalQP_H = new SymDenseMat( nV, nV, nV, H_mem );
00290                         globalQP_H->doFreeMemory();
00291                 }
00292 
00293                 /* Convert constraint matrix A from FORTRAN to C style
00294                  * (not necessary for H as it should be symmetric!). */
00295                 if ( nC > 0 )
00296                 {
00297                         /* Check for sparsity. */
00298                         if ( mxIsSparse( prhs[3] ) != 0 )
00299                         {
00300                                 long *ir = (long *)mxGetIr(prhs[3]);
00301                                 long *jc = (long *)mxGetJc(prhs[3]);
00302                                 real_t *v = (real_t*)mxGetPr(prhs[3]);
00303                                 // mind pointer offsets due to 1-based indexing in Matlab
00304                                 globalQP_A = new SparseMatrix(nC, nV, ir, jc, v);
00305                         }
00306                         else
00307                         {
00308                                 /* Convert constraint matrix A from FORTRAN to C style
00309                                 * (not necessary for H as it should be symmetric!). */
00310                                 A_for = (real_t*) mxGetPr( prhs[3] );
00311                                 A_mem = new real_t[nC*nV];
00312                                 convertFortranToC( A_for,nV,nC, A_mem );
00313                                 globalQP_A = new DenseMatrix(nC, nV, nV, A_mem );
00314                                 globalQP_A->doFreeMemory();
00315                         }
00316                 }
00317 
00318                 /* Create output vectors and assign pointers to them. */
00319                 allocateOutputs( nlhs,plhs, nV,nC );
00320 
00321                 /* Call qpOASES. */
00322                 init(   nV,nC,
00323                                 globalQP_H,g,globalQP_A,
00324                                 lb,ub,lbA,ubA,
00325                                 nWSRin,x0,&options,
00326                                 nlhs,plhs
00327                                 );
00328 
00329                 return;
00330         }
00331 
00332         /* 2) Hotstart. */
00333         if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
00334         {
00335                 /* consistency checks */
00336                 if ( ( nlhs < 1 ) || ( nlhs > 5 ) )
00337                         mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
00338 
00339                 if ( ( nrhs < 6 ) || ( nrhs > 7 ) )
00340                         mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
00341 
00342                 /* has QP been initialised? */
00343                 if ( globalQP == 0 )
00344                         mexErrMsgTxt( "ERROR (qpOASES): QP sequence needs to be initialised first!" );
00345 
00346 
00347                 /* Check inputs dimensions and assign pointers to inputs. */
00348                 nV = globalQP->getNV( );
00349                 nC = globalQP->getNC( );
00350 
00351                 if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,1 ) != SUCCESSFUL_RETURN )
00352                         return;
00353 
00354                 if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,2 ) != SUCCESSFUL_RETURN )
00355                         return;
00356 
00357                 if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
00358                         return;
00359 
00360                 if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
00361                         return;
00362 
00363                 if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
00364                         return;
00365 
00366                 /* default value for nWSR */
00367                 int nWSRin = 5*(nV+nC);
00368 
00369                 /* Check whether options are specified .*/
00370                 if ( nrhs == 7 )
00371                         if ( ( !mxIsEmpty( prhs[6] ) ) && ( mxIsStruct( prhs[6] ) ) )
00372                                 setupOptions( &options,prhs[6],nWSRin );
00373 
00374                 /* Create output vectors and assign pointers to them. */
00375                 allocateOutputs( nlhs,plhs, nV,nC );
00376 
00377                 /* call qpOASES */
00378                 hotstart(       g,
00379                                         lb,ub,lbA,ubA,
00380                                         nWSRin,&options,
00381                                         nlhs,plhs
00382                                         );
00383 
00384                 return;
00385         }
00386 
00387         /* 3) Cleanup. */
00388         if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
00389         {
00390                 /* consistency checks */
00391                 if ( nlhs != 0 )
00392                         mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
00393 
00394                 if ( nrhs != 1 )
00395                         mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
00396 
00397                 /* Cleanup global QProblem instance. */
00398                 deleteGlobalQProblemInstance( );
00399                 deleteGlobalQProblemMatrices( );
00400 
00401                 return;
00402         }
00403 }
00404 
00405 /*
00406  *      end of file
00407  */


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