/** 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; }