void QPInstanceCON( QPInstance* _THIS,
int _nV,
int _nC,
HessianType _hessianType,
BooleanType _isSimplyBounded
)
{
if ( _nV > NVMAX )
{
myMexErrMsgTxt( "ERROR (qpOASES_e): Too many primal variables (try increasing QPOASES_NVMAX)!" );
return;
}
if ( _nC > NCMAX )
{
myMexErrMsgTxt( "ERROR (qpOASES_e): Too many constraints (try increasing QPOASES_NCMAX)!" );
return;
}
_THIS->handle = QPInstance_nexthandle;
if ( _nC > 0 )
_THIS->isSimplyBounded = BT_FALSE;
else
_THIS->isSimplyBounded = _isSimplyBounded;
if ( _THIS->isSimplyBounded == BT_FALSE )
QProblemCON( &(_THIS->sqp),_nV,_nC,_hessianType );
else
QProblemBCON( &(_THIS->qpb),_nV,_hessianType );
}
/** Example for qpOASES main function using the QProblemB class. */
int main( )
{
USING_NAMESPACE_QPOASES
/* Setup data of first QP. */
real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
real_t g[2] = { 1.5, 1.0 };
real_t lb[2] = { 0.5, -2.0 };
real_t ub[2] = { 5.0, 2.0 };
/* Setup data of second QP. */
real_t g_new[2] = { 1.0, 1.5 };
real_t lb_new[2] = { 0.0, -1.0 };
real_t ub_new[2] = { 5.0, -0.5 };
/* Setting up QProblemB object. */
static QProblemB example;
static Options options;
int nWSR = 10;
real_t xOpt[2];
QProblemBCON( &example,2,HST_UNKNOWN );
Options_setToDefault( &options );
/* options.enableFlippingBounds = BT_FALSE; */
options.initialStatusBounds = ST_INACTIVE;
options.numRefinementSteps = 1;
/* options.enableCholeskyRefactorisation = 1; */
QProblemB_setOptions( &example,options );
/* Solve first QP. */
nWSR = 10;
QProblemB_init( &example,H,g,lb,ub, &nWSR,0 );
/* Get and print solution of second QP. */
QProblemB_getPrimalSolution( &example,xOpt );
printf( "\nxOpt = [ %e, %e ]; objVal = %e\n\n", xOpt[0],xOpt[1],QProblemB_getObjVal(&example) );
/* Solve second QP. */
nWSR = 10;
QProblemB_hotstart( &example,g_new,lb_new,ub_new, &nWSR,0 );
/* Get and print solution of second QP. */
QProblemB_getPrimalSolution( &example,xOpt );
printf( "\nxOpt = [ %e, %e ]; objVal = %e\n\n", xOpt[0],xOpt[1],QProblemB_getObjVal(&example) );
return 0;
}
static void mdlStart(SimStruct *S)
{
USING_NAMESPACE_QPOASES
int nU = NCONTROLINPUTS;
int size_g, size_lb, size_ub;
int size_H, nRows_H, nCols_H;
int nV;
static QProblemB problem;
static Options problemOptions;
real_t* count;
/* get block inputs dimensions */
const mxArray* in_H = ssGetSFcnParam(S, 0);
if ( mxIsEmpty(in_H) == 1 )
{
if ( ( HESSIANTYPE != HST_ZERO ) && ( HESSIANTYPE != HST_IDENTITY ) )
{
#ifndef __SUPPRESSANYOUTPUT__
mexErrMsgTxt( "ERROR (qpOASES): Hessian can only be empty if type is set to HST_ZERO or HST_IDENTITY!" );
#endif
return;
}
nRows_H = 0;
nCols_H = 0;
size_H = 0;
}
else
{
nRows_H = (int)mxGetM(in_H);
nCols_H = (int)mxGetN(in_H);
size_H = nRows_H * nCols_H;
}
size_g = ssGetInputPortWidth(S, 0);
size_lb = ssGetInputPortWidth(S, 1);
size_ub = ssGetInputPortWidth(S, 2);
/* dimension checks */
nV = size_g;
if ( MAXITER < 0 )
{
#ifndef __SUPPRESSANYOUTPUT__
mexErrMsgTxt( "ERROR (qpOASES): Maximum number of iterations must not be negative!" );
#endif
return;
}
if ( nV <= 0 )
{
#ifndef __SUPPRESSANYOUTPUT__
mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
#endif
return;
}
if ( ( size_H != nV*nV ) && ( size_H != 0 ) )
{
#ifndef __SUPPRESSANYOUTPUT__
mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch in H!" );
#endif
return;
}
if ( nRows_H != nCols_H )
{
#ifndef __SUPPRESSANYOUTPUT__
mexErrMsgTxt( "ERROR (qpOASES): Hessian matrix must be square matrix!" );
#endif
return;
}
if ( ( nU < 1 ) || ( nU > nV ) )
{
#ifndef __SUPPRESSANYOUTPUT__
mexErrMsgTxt( "ERROR (qpOASES): Invalid number of control inputs!" );
#endif
return;
}
if ( ( size_lb != nV ) && ( size_lb != 0 ) )
{
#ifndef __SUPPRESSANYOUTPUT__
mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch in lb!" );
#endif
return;
}
if ( ( size_ub != nV ) && ( size_ub != 0 ) )
{
#ifndef __SUPPRESSANYOUTPUT__
mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch in ub!" );
#endif
return;
}
/* allocate QProblemB object */
QProblemBCON( &problem,nV,HESSIANTYPE );
Options_setToMPC( &problemOptions );
QProblemB_setOptions( &problem,problemOptions );
#ifndef __DEBUG__
QProblemB_setPrintLevel( &problem,PL_LOW );
#endif
#ifdef __SUPPRESSANYOUTPUT__
QProblemB_setPrintLevel( &problem,PL_NONE );
#endif
ssGetPWork(S)[0] = (void*)(&problem);
/* allocate memory for QP data ... */
if ( size_H > 0 )
ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) ); /* H */
else
ssGetPWork(S)[1] = 0;
ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) ); /* g */
if ( size_lb > 0 )
ssGetPWork(S)[3] = (void *) calloc( size_lb, sizeof(real_t) ); /* lb */
else
ssGetPWork(S)[3] = 0;
if ( size_ub > 0 )
ssGetPWork(S)[4] = (void *) calloc( size_ub, sizeof(real_t) ); /* ub */
else
ssGetPWork(S)[4] = 0;
}
/*
* s o l v e O Q P b e n c h m a r k
*/
returnValue solveOQPbenchmarkB( int nQP, int nV,
real_t* _H, const real_t* const g,
const real_t* const lb, const real_t* const ub,
BooleanType isSparse, BooleanType useHotstarts,
const Options* options, int maxAllowedNWSR,
real_t* maxNWSR, real_t* avgNWSR, real_t* maxCPUtime, real_t* avgCPUtime,
real_t* maxStationarity, real_t* maxFeasibility, real_t* maxComplementarity
)
{
int k;
myStatic QProblemB qp;
returnValue returnvalue;
/* I) SETUP AUXILIARY VARIABLES: */
/* 1) Keep nWSR and store current and maximum number of
* working set recalculations in temporary variables */
int nWSRcur;
real_t CPUtimeLimit = *maxCPUtime;
real_t CPUtimeCur = CPUtimeLimit;
real_t stat, feas, cmpl;
/* 2) Pointers to data of current QP ... */
const real_t* gCur;
const real_t* lbCur;
const real_t* ubCur;
/* 3) Vectors for solution obtained by qpOASES. */
myStatic real_t x[NVMAX];
myStatic real_t y[NVMAX];
/* 4) Prepare matrix objects */
DenseMatrix *H;
myStatic DenseMatrix HH;
DenseMatrixCON( &HH, nV, nV, nV, _H );
H = &HH;
*maxNWSR = 0;
*avgNWSR = 0;
*maxCPUtime = 0.0;
*avgCPUtime = 0.0;
*maxStationarity = 0.0;
*maxFeasibility = 0.0;
*maxComplementarity = 0.0;
/* II) SETUP QPROBLEM OBJECT */
QProblemBCON( &qp,nV,HST_UNKNOWN );
QProblemB_setOptions( &qp,*options );
/*QProblemB_setPrintLevel( &qp,PL_LOW );*/
/* III) RUN BENCHMARK SEQUENCE: */
for( k=0; k<nQP; ++k )
{
/* 1) Update pointers to current QP data. */
gCur = &( g[k*nV] );
lbCur = &( lb[k*nV] );
ubCur = &( ub[k*nV] );
/* 2) Set nWSR and maximum CPU time. */
nWSRcur = maxAllowedNWSR;
CPUtimeCur = CPUtimeLimit;
/* 3) Solve current QP. */
if ( ( k == 0 ) || ( useHotstarts == BT_FALSE ) )
{
/* initialise */
returnvalue = QProblemB_initM( &qp,H,gCur,lbCur,ubCur, &nWSRcur,&CPUtimeCur );
if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
return THROWERROR( returnvalue );
}
else
{
/* hotstart */
returnvalue = QProblemB_hotstart( &qp,gCur,lbCur,ubCur, &nWSRcur,&CPUtimeCur );
if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
return THROWERROR( returnvalue );
}
/* 4) Obtain solution vectors and objective function value ... */
QProblemB_getPrimalSolution( &qp,x );
QProblemB_getDualSolution( &qp,y );
/* 5) Compute KKT residuals */
qpOASES_getKktViolationSB( nV, _H,gCur,lbCur,ubCur, x,y, &stat,&feas,&cmpl );
/* 6) update maximum values. */
if ( nWSRcur > *maxNWSR )
*maxNWSR = nWSRcur;
if (stat > *maxStationarity) *maxStationarity = stat;
if (feas > *maxFeasibility) *maxFeasibility = feas;
if (cmpl > *maxComplementarity) *maxComplementarity = cmpl;
if ( CPUtimeCur > *maxCPUtime )
*maxCPUtime = CPUtimeCur;
*avgNWSR += nWSRcur;
*avgCPUtime += CPUtimeCur;
}
*avgNWSR /= nQP;
*avgCPUtime /= ((double)nQP);
return SUCCESSFUL_RETURN;
}
