qpOASES_QProblem.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 <stdlib.h>
00038 
00039 #include <qpOASES/QProblem.hpp>
00040 
00041 
00042 #ifdef __cplusplus
00043 extern "C" {
00044 #endif
00045 
00046 
00047 #define S_FUNCTION_NAME   qpOASES_QProblem              
00048 #define S_FUNCTION_LEVEL  2                                             
00050 #define MDL_START                                                               
00052 #include "simstruc.h"
00053 
00054 
00055 /* SETTINGS: */
00056 #define SAMPLINGTIME    0.1                                             
00057 #define NCONTROLINPUTS  2                                               
00058 #define NWSR            10                                              
00061 static void mdlInitializeSizes (SimStruct *S)   /* Init sizes array */
00062 {
00063         int nU = NCONTROLINPUTS;
00064 
00065         /* Specify the number of continuous and discrete states */
00066         ssSetNumContStates(S, 0);
00067         ssSetNumDiscStates(S, 0);
00068 
00069         /* Specify the number of intput ports */
00070         if ( !ssSetNumInputPorts(S, 7) )
00071                 return;
00072 
00073         /* Specify the number of output ports */
00074         if ( !ssSetNumOutputPorts(S, 4) )
00075                 return;
00076 
00077         /* Specify dimension information for the input ports */
00078         ssSetInputPortVectorDimension(S, 0, DYNAMICALLY_SIZED);
00079         ssSetInputPortVectorDimension(S, 1, DYNAMICALLY_SIZED);
00080         ssSetInputPortVectorDimension(S, 2, DYNAMICALLY_SIZED);
00081         ssSetInputPortVectorDimension(S, 3, DYNAMICALLY_SIZED);
00082         ssSetInputPortVectorDimension(S, 4, DYNAMICALLY_SIZED);
00083         ssSetInputPortVectorDimension(S, 5, DYNAMICALLY_SIZED);
00084         ssSetInputPortVectorDimension(S, 6, DYNAMICALLY_SIZED);
00085 
00086         /* Specify dimension information for the output ports */
00087         ssSetOutputPortVectorDimension(S, 0, 1 );
00088         ssSetOutputPortVectorDimension(S, 1, nU );
00089         ssSetOutputPortVectorDimension(S, 2, 1 );
00090         ssSetOutputPortVectorDimension(S, 3, 1 );
00091 
00092         /* Specify the direct feedthrough status */
00093         ssSetInputPortDirectFeedThrough(S, 0, 1);
00094         ssSetInputPortDirectFeedThrough(S, 1, 1);
00095         ssSetInputPortDirectFeedThrough(S, 2, 1);
00096         ssSetInputPortDirectFeedThrough(S, 3, 1);
00097         ssSetInputPortDirectFeedThrough(S, 4, 1);
00098         ssSetInputPortDirectFeedThrough(S, 5, 1);
00099         ssSetInputPortDirectFeedThrough(S, 6, 1);
00100 
00101         /* One sample time */
00102         ssSetNumSampleTimes(S, 1);
00103 
00104 
00105         /* global variables:
00106      * 0: problem
00107      * 1: H
00108      * 2: g
00109      * 3: A
00110      * 4: lb
00111      * 5: ub
00112      * 6: lbA
00113      * 7: ubA
00114      * 8: count
00115      */
00116 
00117         /* Specify the size of the block's pointer work vector */
00118     ssSetNumPWork(S, 9);
00119 }
00120 
00121 
00122 #if defined(MATLAB_MEX_FILE)
00123 
00124 #define MDL_SET_INPUT_PORT_DIMENSION_INFO
00125 #define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
00126 
00127 static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
00128 {
00129         if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
00130                 return;
00131 }
00132 
00133 static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
00134 {
00135         if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
00136                 return;
00137 }
00138 
00139 #endif
00140 
00141 
00142 static void mdlInitializeSampleTimes(SimStruct *S)
00143 {
00144         ssSetSampleTime(S, 0, SAMPLINGTIME);
00145         ssSetOffsetTime(S, 0, 0.0);
00146 }
00147 
00148 
00149 static void mdlStart(SimStruct *S)
00150 {
00151         #ifndef __DSPACE__
00152         using namespace qpOASES;
00153         #endif
00154 
00155         int nU = NCONTROLINPUTS;
00156         int size_H, size_g, size_A, size_lb, size_ub, size_lbA, size_ubA;
00157         int nV, nC;
00158 
00159         QProblem* problem;
00160         real_t* count;
00161 
00162 
00163         /* get block inputs dimensions */
00164         size_H   = ssGetInputPortWidth(S, 0);
00165         size_g   = ssGetInputPortWidth(S, 1);
00166         size_A   = ssGetInputPortWidth(S, 2);
00167         size_lb  = ssGetInputPortWidth(S, 3);
00168         size_ub  = ssGetInputPortWidth(S, 4);
00169         size_lbA = ssGetInputPortWidth(S, 5);
00170         size_ubA = ssGetInputPortWidth(S, 6);
00171 
00172 
00173         /* dimension checks */
00174         nV = size_g;
00175         nC = (int) ( ((real_t) size_A) / ((real_t) nV) );
00176 
00177         if ( nV == 0 )
00178         {
00179                 #ifndef __DSPACE__
00180                 mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
00181                 #endif
00182                 return;
00183         }
00184 
00185         if ( size_H != nV*nV )
00186         {
00187                 #ifndef __DSPACE__
00188                 mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
00189                 #endif
00190                 return;
00191         }
00192 
00193         if ( nU > nV )
00194         {
00195                 #ifndef __DSPACE__
00196                 mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
00197                 #endif
00198                 return;
00199         }
00200 
00201         if ( size_lb != nV )
00202         {
00203                 #ifndef __DSPACE__
00204                 mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
00205                 #endif
00206                 return;
00207         }
00208 
00209         if ( size_ub != nV )
00210         {
00211                 #ifndef __DSPACE__
00212                 mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
00213                 #endif
00214                 return;
00215         }
00216 
00217         if ( size_lbA != nC )
00218         {
00219                 #ifndef __DSPACE__
00220                 mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
00221                 #endif
00222                 return;
00223         }
00224 
00225         if ( size_ubA != nC )
00226         {
00227                 #ifndef __DSPACE__
00228                 mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
00229                 #endif
00230                 return;
00231         }
00232 
00233 
00234         /* allocate QProblem object */
00235         problem = new QProblem( nV,nC );
00236         if ( problem == 0 )
00237         {
00238                 #ifndef __DSPACE__
00239                 mexErrMsgTxt( "ERROR (qpOASES): Unable to create QProblem object!" );
00240                 #endif
00241                 return;
00242         }
00243 
00244         #ifndef __DEBUG__
00245         problem->setPrintLevel( PL_LOW );
00246         #endif
00247         #ifdef __SUPPRESSANYOUTPUT__
00248         problem->setPrintLevel( PL_NONE );
00249         #endif
00250         #ifdef __DSPACE__
00251         problem->setPrintLevel( PL_NONE );
00252         #endif
00253 
00254         ssGetPWork(S)[0] = (void *) problem;
00255 
00256         /* allocate memory for QP data ... */
00257         ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) );   /* H */
00258         ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) );   /* g */
00259         ssGetPWork(S)[3] = (void *) calloc( size_A, sizeof(real_t) );   /* A */
00260         ssGetPWork(S)[4] = (void *) calloc( size_lb, sizeof(real_t) );  /* lb */
00261         ssGetPWork(S)[5] = (void *) calloc( size_ub, sizeof(real_t) );  /* ub */
00262         ssGetPWork(S)[6] = (void *) calloc( size_lbA, sizeof(real_t) ); /* lbA */
00263         ssGetPWork(S)[7] = (void *) calloc( size_ubA, sizeof(real_t) ); /* ubA */
00264         ssGetPWork(S)[8] = (void *) calloc( 1, sizeof(real_t) ); /* count */
00265 
00266         /* reset counter */
00267         count = (real_t *) ssGetPWork(S)[8];
00268         count[0] = 0.0;
00269 }
00270 
00271 
00272 static void mdlOutputs(SimStruct *S, int_T tid)
00273 {
00274         #ifndef __DSPACE__
00275         using namespace qpOASES;
00276         #endif
00277 
00278         int i;
00279         int nV, nC, status;
00280 
00281         int nWSR = NWSR;
00282         int nU   = NCONTROLINPUTS;
00283 
00284         InputRealPtrsType in_H, in_g, in_A, in_lb, in_ub, in_lbA, in_ubA;
00285 
00286         QProblem* problem;
00287         real_t *H, *g, *A, *lb, *ub, *lbA, *ubA, *count;
00288 
00289         real_t *xOpt;
00290 
00291         real_T *out_objVal, *out_xOpt, *out_status, *out_nWSR;
00292 
00293 
00294         /* get pointers to block inputs ... */
00295         in_H   = ssGetInputPortRealSignalPtrs(S, 0);
00296         in_g   = ssGetInputPortRealSignalPtrs(S, 1);
00297         in_A   = ssGetInputPortRealSignalPtrs(S, 2);
00298         in_lb  = ssGetInputPortRealSignalPtrs(S, 3);
00299         in_ub  = ssGetInputPortRealSignalPtrs(S, 4);
00300         in_lbA = ssGetInputPortRealSignalPtrs(S, 5);
00301         in_ubA = ssGetInputPortRealSignalPtrs(S, 6);
00302 
00303 
00304         /* ... and to the QP data */
00305         problem = (QProblem *) ssGetPWork(S)[0];
00306 
00307         H = (real_t *) ssGetPWork(S)[1];
00308         g = (real_t *) ssGetPWork(S)[2];
00309         A = (real_t *) ssGetPWork(S)[3];
00310         lb = (real_t *) ssGetPWork(S)[4];
00311         ub = (real_t *) ssGetPWork(S)[5];
00312         lbA = (real_t *) ssGetPWork(S)[6];
00313         ubA = (real_t *) ssGetPWork(S)[7];
00314 
00315         count = (real_t *) ssGetPWork(S)[8];
00316 
00317 
00318         /* setup QP data */
00319         nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
00320         nC = (int) ( ((real_t) ssGetInputPortWidth(S, 2)) / ((real_t) nV) ); /* nC = size_A / size_g */
00321 
00322         for ( i=0; i<nV*nV; ++i )
00323                 H[i] = (*in_H)[i];
00324 
00325         for ( i=0; i<nC*nV; ++i )
00326                 A[i] = (*in_A)[i];
00327 
00328         for ( i=0; i<nV; ++i )
00329         {
00330                 g[i] = (*in_g)[i];
00331                 lb[i] = (*in_lb)[i];
00332                 ub[i] = (*in_ub)[i];
00333         }
00334 
00335         for ( i=0; i<nC; ++i )
00336         {
00337                 lbA[i] = (*in_lbA)[i];
00338                 ubA[i] = (*in_ubA)[i];
00339         }
00340 
00341         xOpt = new real_t[nV];
00342 
00343         if ( count[0] == 0 )
00344         {
00345                 /* initialise and solve first QP */
00346                 status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
00347                 ssGetPWork(S)[0] = ( void* ) problem;
00348                 problem->getPrimalSolution( xOpt );
00349         }
00350         else
00351         {
00352                 /* solve neighbouring QP using hotstart technique */
00353                 status = problem->hotstart( g,lb,ub,lbA,ubA, nWSR,0 );
00354                 if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
00355                 {
00356                         /* if an error occurs, reset problem data structures and initialise again */
00357                         problem->reset( );
00358                         problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
00359                 }
00360                 else
00361                 {
00362                 /* otherwise obtain optimal solution */
00363                 problem->getPrimalSolution( xOpt );
00364                 }
00365         }
00366 
00367         /* generate block output: status information ... */
00368         out_objVal = ssGetOutputPortRealSignal(S, 0);
00369         out_xOpt   = ssGetOutputPortRealSignal(S, 1);
00370         out_status = ssGetOutputPortRealSignal(S, 2);
00371         out_nWSR   = ssGetOutputPortRealSignal(S, 3);
00372 
00373         out_objVal[0] = ((real_T) problem->getObjVal( ));
00374 
00375         for ( i=0; i<nU; ++i )
00376                 out_xOpt[i] = ((real_T) xOpt[i]);
00377 
00378         switch ( status )
00379         {
00380                 case SUCCESSFUL_RETURN:
00381                         out_status[0] = 0.0;
00382                         break;
00383 
00384                 case RET_MAX_NWSR_REACHED:
00385                         out_status[0] = 1.0;
00386                         break;
00387 
00388                 case RET_INIT_FAILED_INFEASIBILITY:
00389                 case RET_HOTSTART_STOPPED_INFEASIBILITY:
00390                         out_status[0] = -2.0;
00391                         break;
00392                 
00393                 case RET_INIT_FAILED_UNBOUNDEDNESS:
00394                 case RET_HOTSTART_STOPPED_UNBOUNDEDNESS:
00395                         out_status[0] = -3.0;
00396                         break;
00397 
00398                 default:
00399                         out_status[0] = -1.0;
00400                         break;
00401         }
00402 
00403         out_nWSR[0] = ((real_T) nWSR);
00404 
00405         /* increase counter */
00406         count[0] = count[0] + 1;
00407 
00408         delete[] xOpt;
00409 }
00410 
00411 
00412 static void mdlTerminate(SimStruct *S)
00413 {
00414         #ifndef __DSPACE__
00415         using namespace qpOASES;
00416         #endif
00417 
00418         /* reset global message handler */
00419         getGlobalMessageHandler( )->reset( );
00420 
00421         int i;
00422         for ( i=0; i<9; ++i )
00423         {
00424                 if ( ssGetPWork(S)[i] != 0 )
00425                         free( ssGetPWork(S)[i] );
00426         }
00427 }
00428 
00429 
00430 #ifdef  MATLAB_MEX_FILE
00431 #include "simulink.c"
00432 #else
00433 #include "cg_sfun.h"
00434 #endif
00435 
00436 
00437 #ifdef __cplusplus
00438 }
00439 #endif
00440 
00441 
00442 /*
00443  *      end of file
00444  */


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