qpOASES_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 <stdlib.h>
00038 
00039 #include <qpOASES/SQProblem.hpp>
00040 
00041 
00042 #ifdef __cplusplus
00043 extern "C" {
00044 #endif
00045 
00046 
00047 #define S_FUNCTION_NAME   qpOASES_SQProblem             
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         SQProblem* 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 SQProblem( 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 
00273 static void mdlOutputs(SimStruct *S, int_T tid)
00274 {
00275         #ifndef __DSPACE__
00276         using namespace qpOASES;
00277         #endif
00278 
00279         int i;
00280         int nV, nC, status;
00281 
00282         int nWSR = NWSR;
00283         int nU   = NCONTROLINPUTS;
00284 
00285         InputRealPtrsType in_H, in_g, in_A, in_lb, in_ub, in_lbA, in_ubA;
00286 
00287         SQProblem* problem;
00288         real_t *H, *g, *A, *lb, *ub, *lbA, *ubA, *count;
00289 
00290         real_t *xOpt;
00291 
00292         real_T *out_objVal, *out_xOpt, *out_status, *out_nWSR;
00293 
00294 
00295         /* get pointers to block inputs ... */
00296         in_H   = ssGetInputPortRealSignalPtrs(S, 0);
00297         in_g   = ssGetInputPortRealSignalPtrs(S, 1);
00298         in_A   = ssGetInputPortRealSignalPtrs(S, 2);
00299         in_lb  = ssGetInputPortRealSignalPtrs(S, 3);
00300         in_ub  = ssGetInputPortRealSignalPtrs(S, 4);
00301         in_lbA = ssGetInputPortRealSignalPtrs(S, 5);
00302         in_ubA = ssGetInputPortRealSignalPtrs(S, 6);
00303 
00304 
00305         /* ... and to the QP data */
00306         problem = (SQProblem *) ssGetPWork(S)[0];
00307 
00308         H = (real_t *) ssGetPWork(S)[1];
00309         g = (real_t *) ssGetPWork(S)[2];
00310         A = (real_t *) ssGetPWork(S)[3];
00311         lb = (real_t *) ssGetPWork(S)[4];
00312         ub = (real_t *) ssGetPWork(S)[5];
00313         lbA = (real_t *) ssGetPWork(S)[6];
00314         ubA = (real_t *) ssGetPWork(S)[7];
00315 
00316         count = (real_t *) ssGetPWork(S)[8];
00317 
00318 
00319         /* setup QP data */
00320         nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
00321         nC = (int) ( ((real_t) ssGetInputPortWidth(S, 2)) / ((real_t) nV) ); /* nC = size_A / size_g */
00322 
00323         for ( i=0; i<nV*nV; ++i )
00324                 H[i] = (*in_H)[i];
00325 
00326         for ( i=0; i<nC*nV; ++i )
00327                 A[i] = (*in_A)[i];
00328 
00329         for ( i=0; i<nV; ++i )
00330         {
00331                 g[i] = (*in_g)[i];
00332                 lb[i] = (*in_lb)[i];
00333                 ub[i] = (*in_ub)[i];
00334         }
00335 
00336         for ( i=0; i<nC; ++i )
00337         {
00338                 lbA[i] = (*in_lbA)[i];
00339                 ubA[i] = (*in_ubA)[i];
00340         }
00341 
00342         xOpt = new real_t[nV];
00343 
00344         if ( count[0] == 0 )
00345         {
00346                 /* initialise and solve first QP */
00347                 status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
00348                 ssGetPWork(S)[0] = ( void* ) problem;
00349                 problem->getPrimalSolution( xOpt );
00350         }
00351         else
00352         {
00353                 /* solve neighbouring QP using hotstart technique */
00354                 status = problem->hotstart( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
00355                 if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
00356                 {
00357                         /* if an error occurs, reset problem data structures and initialise again */
00358                         problem->reset( );
00359                         problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
00360                 }
00361                 else
00362                 {
00363                 /* otherwise obtain optimal solution */
00364                 problem->getPrimalSolution( xOpt );
00365                 }
00366         }
00367 
00368         /* generate block output: status information ... */
00369         out_objVal = ssGetOutputPortRealSignal(S, 0);
00370         out_xOpt   = ssGetOutputPortRealSignal(S, 1);
00371         out_status = ssGetOutputPortRealSignal(S, 2);
00372         out_nWSR   = ssGetOutputPortRealSignal(S, 3);
00373 
00374         out_objVal[0] = ((real_T) problem->getObjVal( ));
00375 
00376         for ( i=0; i<nU; ++i )
00377                 out_xOpt[i] = ((real_T) xOpt[i]);
00378 
00379         switch ( status )
00380         {
00381                 case SUCCESSFUL_RETURN:
00382                         out_status[0] = 0.0;
00383                         break;
00384 
00385                 case RET_MAX_NWSR_REACHED:
00386                         out_status[0] = 1.0;
00387                         break;
00388 
00389                 case RET_INIT_FAILED_INFEASIBILITY:
00390                 case RET_HOTSTART_STOPPED_INFEASIBILITY:
00391                         out_status[0] = -2.0;
00392                         break;
00393                 
00394                 case RET_INIT_FAILED_UNBOUNDEDNESS:
00395                 case RET_HOTSTART_STOPPED_UNBOUNDEDNESS:
00396                         out_status[0] = -3.0;
00397                         break;
00398 
00399                 default:
00400                         out_status[0] = -1.0;
00401                         break;
00402         }
00403 
00404         out_nWSR[0] = ((real_T) nWSR);
00405 
00406         /* increase counter */
00407         count[0] = count[0] + 1;
00408 
00409         delete[] xOpt;
00410 }
00411 
00412 
00413 static void mdlTerminate(SimStruct *S)
00414 {
00415         #ifndef __DSPACE__
00416         using namespace qpOASES;
00417         #endif
00418 
00419         /* reset global message handler */
00420         getGlobalMessageHandler( )->reset( );
00421 
00422         int i;
00423         for ( i=0; i<9; ++i )
00424         {
00425                 if ( ssGetPWork(S)[i] != 0 )
00426                         free( ssGetPWork(S)[i] );
00427         }
00428 }
00429 
00430 
00431 #ifdef  MATLAB_MEX_FILE
00432 #include "simulink.c"
00433 #else
00434 #include "cg_sfun.h"
00435 #endif
00436 
00437 
00438 #ifdef __cplusplus
00439 }
00440 #endif
00441 
00442 
00443 /*
00444  *      end of file
00445  */


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