qpOASES_matlab_utils.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 "mex.h"
00038 #include "matrix.h"
00039 #include "string.h"
00040 
00041 
00042 /*
00043  *      g e t S t a t u s
00044  */
00045 real_t getStatus( returnValue returnvalue )
00046 {
00047         /* determine status from returnvalue */
00048         switch ( returnvalue )
00049         {
00050                 case SUCCESSFUL_RETURN:
00051                         return 0.0;
00052 
00053                 case RET_MAX_NWSR_REACHED:
00054                         return 1.0;
00055 
00056                 case RET_INIT_FAILED_INFEASIBILITY:
00057                 case RET_HOTSTART_STOPPED_INFEASIBILITY:
00058                         return -2.0;
00059 
00060                 case RET_INIT_FAILED_UNBOUNDEDNESS:
00061                 case RET_HOTSTART_STOPPED_UNBOUNDEDNESS:
00062                         return -3.0;
00063 
00064                 default:
00065                         return -1.0;
00066         }
00067 }
00068 
00069 
00070 /*
00071  *      s m a r t D i m e n s i o n C h e c k
00072  */
00073 returnValue smartDimensionCheck(        real_t** input, unsigned int m, unsigned int n, BooleanType emptyAllowed,
00074                                                                         const mxArray* prhs[], int idx
00075                                                                         )
00076 {
00077         /* If index is negative, the input does not exist. */
00078         if ( idx < 0 )
00079         {
00080                 *input = 0;
00081                 return SUCCESSFUL_RETURN;
00082         }
00083 
00084         /* Otherwise the input has been passed by the user. */
00085         if ( mxIsEmpty( prhs[ idx ] ) )
00086         {
00087                 /* input is empty */
00088                 if ( emptyAllowed == BT_TRUE )
00089                 {
00090                         *input = 0;
00091                         return SUCCESSFUL_RETURN;
00092                 }
00093                 else
00094                 {
00095                         char msg[200];
00096                         snprintf(msg, 199, "ERROR (qpOASES): Empty argument %d not allowed!", idx+1);
00097                         mexErrMsgTxt( msg );
00098                         return RET_INVALID_ARGUMENTS;
00099                 }
00100         }
00101         else
00102         {
00103                 /* input is non-empty */
00104                 if ( ( mxGetM( prhs[ idx ] ) == m ) && ( mxGetN( prhs[ idx ] ) == n ) )
00105                 {
00106                         *input = (real_t*) mxGetPr( prhs[ idx ] );
00107                         return SUCCESSFUL_RETURN;
00108                 }
00109                 else
00110                 {
00111                         char msg[200];
00112                         snprintf(msg, 199, "ERROR (qpOASES): Input dimension mismatch for argument %d ([%ld,%ld] ~= [%d,%d]).",
00113                                         idx+1, (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
00114                         mexErrMsgTxt( msg );
00115                         return RET_INVALID_ARGUMENTS;
00116                 }
00117         }
00118 
00119         return SUCCESSFUL_RETURN;
00120 }
00121 
00122 
00123 /*
00124  *      c o n v e r t F o r t r a n T o C
00125  */
00126 returnValue convertFortranToC( const real_t* const A_for, int nV, int nC, real_t* const A )
00127 {
00128         int i,j;
00129 
00130         for ( i=0; i<nC; ++i )
00131                 for ( j=0; j<nV; ++j )
00132                         A[i*nV + j] = A_for[j*nC + i];
00133 
00134         return SUCCESSFUL_RETURN;
00135 }
00136 
00137 
00138 /*
00139  *      h a s O p t i o n s V a l u e
00140  */
00141 BooleanType hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, double** optionValue )
00142 {
00143         mxArray* optionName = mxGetField( optionsPtr,0,optionString );
00144 
00145     if( !mxIsEmpty(optionName) )
00146         {
00147                 if ( ( mxGetM( optionName ) != 1 ) || ( mxGetN( optionName ) != 1 ) )
00148                         mexErrMsgTxt( "ERROR (qpOASES_options): Option has to be a numerical constant." );
00149 
00150                 *optionValue = mxGetPr( optionName );
00151                 return BT_TRUE;
00152         }
00153 
00154         return BT_FALSE;
00155 }
00156 
00157 
00158 /*
00159  *      s e t u p O p t i o n s
00160  */
00161 returnValue setupOptions( Options* options, const mxArray* optionsPtr, int& nWSRin )
00162 {
00163         double* optionValue;
00164         int optionValueInt;
00165 
00166         if ( hasOptionsValue( optionsPtr,"maxIter",&optionValue ) == BT_TRUE )
00167                 if ( *optionValue > 0.0 )
00168                         nWSRin = (int)*optionValue;
00169 
00170         if ( hasOptionsValue( optionsPtr,"printLevel",&optionValue ) == BT_TRUE )
00171         {
00172                 optionValueInt = (int)*optionValue;
00173                 options->printLevel = (qpOASES::PrintLevel)optionValueInt;
00174         }
00175 
00176 
00177         if ( hasOptionsValue( optionsPtr,"enableRamping",&optionValue ) == BT_TRUE )
00178         {
00179                 optionValueInt = (int)*optionValue;
00180                 options->enableRamping = (qpOASES::BooleanType)optionValueInt;
00181         }
00182 
00183         if ( hasOptionsValue( optionsPtr,"enableFarBounds",&optionValue ) == BT_TRUE )
00184         {
00185                 optionValueInt = (int)*optionValue;
00186                 options->enableFarBounds = (qpOASES::BooleanType)optionValueInt;
00187         }
00188 
00189         if ( hasOptionsValue( optionsPtr,"enableFlippingBounds",&optionValue ) == BT_TRUE )
00190         {
00191                 optionValueInt = (int)*optionValue;
00192                 options->enableFlippingBounds = (qpOASES::BooleanType)optionValueInt;
00193         }
00194 
00195         if ( hasOptionsValue( optionsPtr,"enableRegularisation",&optionValue ) == BT_TRUE )
00196         {
00197                 optionValueInt = (int)*optionValue;
00198                 options->enableRegularisation = (qpOASES::BooleanType)optionValueInt;
00199         }
00200 
00201         if ( hasOptionsValue( optionsPtr,"enableFullLITests",&optionValue ) == BT_TRUE )
00202         {
00203                 optionValueInt = (int)*optionValue;
00204                 options->enableFullLITests = (qpOASES::BooleanType)optionValueInt;
00205         }
00206 
00207         if ( hasOptionsValue( optionsPtr,"enableNZCTests",&optionValue ) == BT_TRUE )
00208         {
00209                 optionValueInt = (int)*optionValue;
00210                 options->enableNZCTests = (qpOASES::BooleanType)optionValueInt;
00211         }
00212 
00213         if ( hasOptionsValue( optionsPtr,"enableDriftCorrection",&optionValue ) == BT_TRUE )
00214                 options->enableDriftCorrection = (int)*optionValue;
00215 
00216         if ( hasOptionsValue( optionsPtr,"enableCholeskyRefactorisation",&optionValue ) == BT_TRUE )
00217                 options->enableCholeskyRefactorisation = (int)*optionValue;
00218 
00219         if ( hasOptionsValue( optionsPtr,"enableEqualities",&optionValue ) == BT_TRUE )
00220         {
00221                 optionValueInt = (int)*optionValue;
00222                 options->enableEqualities = (qpOASES::BooleanType)optionValueInt;
00223         }
00224 
00225 
00226         if ( hasOptionsValue( optionsPtr,"terminationTolerance",&optionValue ) == BT_TRUE )
00227                 options->terminationTolerance = *optionValue;
00228 
00229         if ( hasOptionsValue( optionsPtr,"boundTolerance",&optionValue ) == BT_TRUE )
00230                 options->boundTolerance = *optionValue;
00231 
00232         if ( hasOptionsValue( optionsPtr,"boundRelaxation",&optionValue ) == BT_TRUE )
00233                 options->boundRelaxation = *optionValue;
00234 
00235         if ( hasOptionsValue( optionsPtr,"epsNum",&optionValue ) == BT_TRUE )
00236                 options->epsNum = *optionValue;
00237 
00238         if ( hasOptionsValue( optionsPtr,"epsDen",&optionValue ) == BT_TRUE )
00239                 options->epsDen = *optionValue;
00240 
00241         if ( hasOptionsValue( optionsPtr,"maxPrimalJump",&optionValue ) == BT_TRUE )
00242                 options->maxPrimalJump = *optionValue;
00243 
00244         if ( hasOptionsValue( optionsPtr,"maxDualJump",&optionValue ) == BT_TRUE )
00245                 options->maxDualJump = *optionValue;
00246 
00247 
00248         if ( hasOptionsValue( optionsPtr,"initialRamping",&optionValue ) == BT_TRUE )
00249                 options->initialRamping = *optionValue;
00250 
00251         if ( hasOptionsValue( optionsPtr,"finalRamping",&optionValue ) == BT_TRUE )
00252                 options->finalRamping = *optionValue;
00253 
00254         if ( hasOptionsValue( optionsPtr,"initialFarBounds",&optionValue ) == BT_TRUE )
00255                 options->initialFarBounds = *optionValue;
00256 
00257         if ( hasOptionsValue( optionsPtr,"growFarBounds",&optionValue ) == BT_TRUE )
00258                 options->growFarBounds = *optionValue;
00259 
00260         if ( hasOptionsValue( optionsPtr,"initialStatusBounds",&optionValue ) == BT_TRUE )
00261         {
00262                 optionValueInt = (int)*optionValue;
00263                 options->initialStatusBounds = (qpOASES::SubjectToStatus)optionValueInt;
00264         }
00265 
00266         if ( hasOptionsValue( optionsPtr,"epsFlipping",&optionValue ) == BT_TRUE )
00267                 options->epsFlipping = *optionValue;
00268 
00269         if ( hasOptionsValue( optionsPtr,"numRegularisationSteps",&optionValue ) == BT_TRUE )
00270                 options->numRegularisationSteps = (int)*optionValue;
00271 
00272         if ( hasOptionsValue( optionsPtr,"epsRegularisation",&optionValue ) == BT_TRUE )
00273                 options->epsRegularisation = *optionValue;
00274 
00275         if ( hasOptionsValue( optionsPtr,"numRefinementSteps",&optionValue ) == BT_TRUE )
00276                 options->numRefinementSteps = (int)*optionValue;
00277 
00278         if ( hasOptionsValue( optionsPtr,"epsIterRef",&optionValue ) == BT_TRUE )
00279                 options->epsIterRef = *optionValue;
00280 
00281         if ( hasOptionsValue( optionsPtr,"epsLITests",&optionValue ) == BT_TRUE )
00282                 options->epsLITests = *optionValue;
00283 
00284         if ( hasOptionsValue( optionsPtr,"epsNZCTests",&optionValue ) == BT_TRUE )
00285                 options->epsNZCTests = *optionValue;
00286 
00287         return SUCCESSFUL_RETURN;
00288 }
00289 
00290 
00291 
00292 /*
00293  *      a l l o c a t e O u t p u t s
00294  */
00295 void allocateOutputs(   int nlhs, mxArray* plhs[], int nV, int nC = 0, int nP = 1
00296                                                 )
00297 {
00298         /* Create output vectors and assign pointers to them. */
00299         plhs[0] = mxCreateDoubleMatrix( nV, nP, mxREAL );
00300 
00301         if ( nlhs >= 2 )
00302         {
00303                 plhs[1] = mxCreateDoubleMatrix( 1, nP, mxREAL );
00304 
00305                 if ( nlhs >= 3 )
00306                 {
00307                         plhs[2] = mxCreateDoubleMatrix( 1, nP, mxREAL );
00308 
00309                         if ( nlhs >= 4 )
00310                         {
00311                                 plhs[3] = mxCreateDoubleMatrix( 1, nP, mxREAL );
00312 
00313                                 if ( nlhs == 5 )
00314                                 {
00315                                         plhs[4] = mxCreateDoubleMatrix( nV+nC, nP, mxREAL );
00316                                 }
00317                         }
00318                 }
00319         }
00320 }
00321 
00322 
00323 /*
00324  *      o b t a i n O u t p u t s
00325  */
00326 void obtainOutputs( int k, QProblemB* qp, returnValue returnvalue, int nWSRin,
00327                                         int nlhs, mxArray* plhs[], int nV, int nC = 0
00328                                         )
00329 {
00330         /* Create output vectors and assign pointers to them. */
00331         double* x = mxGetPr( plhs[0] );
00332         qp->getPrimalSolution( &(x[k*nV]) );
00333 
00334         if ( nlhs >= 2 )
00335         {
00336                 double* obj = mxGetPr( plhs[1] );
00337                 obj[k] = qp->getObjVal( );
00338 
00339                 if ( nlhs >= 3 )
00340                 {
00341                         double* status = mxGetPr( plhs[2] );
00342                         status[k] = getStatus( returnvalue );
00343 
00344                         if ( nlhs >= 4 )
00345                         {
00346                                 double* nWSRout = mxGetPr( plhs[3] );
00347                                 nWSRout[k] = (real_t) nWSRin;
00348 
00349                                 if ( nlhs == 5 )
00350                                 {
00351                                         double* y = mxGetPr( plhs[4] );
00352                                         qp->getDualSolution( &(y[k*(nV+nC)]) );
00353                                 }
00354                         }
00355                 }
00356         }
00357 }
00358 
00359 
00360 /*
00361  *      end of file
00362  */


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