/** 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 mdlOutputs(SimStruct *S, int_T tid)
{
USING_NAMESPACE_QPOASES
int i;
int nV;
returnValue status;
int nWSR = MAXITER;
int nU = NCONTROLINPUTS;
InputRealPtrsType in_g, in_lb, in_ub;
QProblemB* problem;
real_t *H, *g, *lb, *ub;
real_t xOpt[NVMAX];
real_T *out_uOpt, *out_objVal, *out_status, *out_nWSR;
int nWSR_retry;
/* get pointers to block inputs ... */
const mxArray* in_H = ssGetSFcnParam(S, 0);
in_g = ssGetInputPortRealSignalPtrs(S, 0);
in_lb = ssGetInputPortRealSignalPtrs(S, 1);
in_ub = ssGetInputPortRealSignalPtrs(S, 2);
/* ... and to the QP data */
problem = (QProblemB*)(ssGetPWork(S)[0]);
H = (real_t*)(ssGetPWork(S)[1]);
g = (real_t*)(ssGetPWork(S)[2]);
lb = (real_t*)(ssGetPWork(S)[3]);
ub = (real_t*)(ssGetPWork(S)[4]);
/* setup QP data */
nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
if ( H != 0 )
{
/* no conversion from FORTRAN to C as Hessian is symmetric! */
for ( i=0; i<nV*nV; ++i )
H[i] = (mxGetPr(in_H))[i];
}
for ( i=0; i<nV; ++i )
g[i] = (*in_g)[i];
if ( lb != 0 )
{
for ( i=0; i<nV; ++i )
lb[i] = (*in_lb)[i];
}
if ( ub != 0 )
{
for ( i=0; i<nV; ++i )
ub[i] = (*in_ub)[i];
}
if ( QProblemB_getCount( problem ) == 0 )
{
/* initialise and solve first QP */
status = QProblemB_init( problem,H,g,lb,ub, &nWSR,0 );
QProblemB_getPrimalSolution( problem,xOpt );
}
else
{
/* solve neighbouring QP using hotstart technique */
status = QProblemB_hotstart( problem,g,lb,ub, &nWSR,0 );
if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
{
/* if an error occurs, reset problem data structures ... */
QProblemB_reset( problem );
/* ... and initialise/solve again with remaining number of iterations. */
nWSR_retry = MAXITER - nWSR;
status = QProblemB_init( problem,H,g,lb,ub, &nWSR_retry,0 );
nWSR += nWSR_retry;
}
/* obtain optimal solution */
QProblemB_getPrimalSolution( problem,xOpt );
}
/* generate block output: status information ... */
out_uOpt = ssGetOutputPortRealSignal(S, 0);
out_objVal = ssGetOutputPortRealSignal(S, 1);
out_status = ssGetOutputPortRealSignal(S, 2);
out_nWSR = ssGetOutputPortRealSignal(S, 3);
for ( i=0; i<nU; ++i )
out_uOpt[i] = (real_T)(xOpt[i]);
out_objVal[0] = (real_T)(QProblemB_getObjVal( problem ));
out_status[0] = (real_t)(qpOASES_getSimpleStatus( status,BT_FALSE ));
out_nWSR[0] = (real_T)(nWSR);
removeNaNs( out_uOpt,nU );
removeInfs( out_uOpt,nU );
removeNaNs( out_objVal,1 );
removeInfs( out_objVal,1 );
}
/*
* o b t a i n O u t p u t s S B
*/
returnValue obtainOutputsSB( int k, QProblemB* qp, returnValue returnvalue, int _nWSRout, double _cpuTime,
int nlhs, mxArray* plhs[], int nV, int handle
)
{
double *x=0, *obj=0, *status=0, *nWSRout=0, *y=0, *workingSetB=0, *workingSetC=0, *cpuTime;
mxArray *auxOutput=0, *curField=0;
/* Create output vectors and assign pointers to them. */
int curIdx = 0;
/* handle */
if ( handle >= 0 )
plhs[curIdx++] = mxCreateDoubleScalar( handle );
/* x */
x = mxGetPr( plhs[curIdx++] );
QProblemB_getPrimalSolution( qp,&(x[k*nV]) );
if ( nlhs > curIdx )
{
/* fval */
obj = mxGetPr( plhs[curIdx++] );
obj[k] = QProblemB_getObjVal( qp );
if ( nlhs > curIdx )
{
/* exitflag */
status = mxGetPr( plhs[curIdx++] );
status[k] = (double)qpOASES_getSimpleStatus( returnvalue,0 );
if ( nlhs > curIdx )
{
/* iter */
nWSRout = mxGetPr( plhs[curIdx++] );
nWSRout[k] = (double) _nWSRout;
if ( nlhs > curIdx )
{
/* lambda */
y = mxGetPr( plhs[curIdx++] );
QProblemB_getDualSolution( qp,&(y[k*nV]) );
/* auxOutput */
if ( nlhs > curIdx )
{
auxOutput = plhs[curIdx];
curField = 0;
/* working set bounds */
if ( nV > 0 )
{
curField = mxGetField( auxOutput,0,"workingSetB" );
workingSetB = mxGetPr(curField);
QProblemB_getWorkingSetBounds( qp,&(workingSetB[k*nV]) );
}
/* cpu time */
curField = mxGetField( auxOutput,0,"cpuTime" );
cpuTime = mxGetPr(curField);
cpuTime[0] = (double) _cpuTime;
}
}
}
}
}
return SUCCESSFUL_RETURN;
}
/*
* 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;
}
