/*
* h o t s t a r t V M
*/
void hotstartVM( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
int* nWSR,
real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
)
{
/* has QP been initialised? */
if ( sqp == 0 )
{
*status = -1;
Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
return;
}
/* transform A into C style matrix */
transformA( A, sqp->getNV( ),sqp->getNC( ) );
/* solve QP */
returnValue returnvalue = sqp->hotstart( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
/* assign lhs arguments */
sqp->getPrimalSolution( x );
*obj = sqp->getObjVal( );
*status = getSimpleStatus( returnvalue );
*nWSRout = *nWSR;
sqp->getDualSolution( y );
return;
}
/*
* h o t s t a r t S B
*/
void hotstartSB( real_t* g, real_t* lb, real_t* ub,
int* nWSR,
real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
)
{
/* has QP been initialised? */
if ( qpb == 0 )
{
*status = -1;
Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
return;
}
/* solve QP */
returnValue returnvalue = qpb->hotstart( g,lb,ub, *nWSR,0 );
/* assign lhs arguments */
qpb->getPrimalSolution( x );
*obj = qpb->getObjVal( );
*status = getSimpleStatus( returnvalue );
*nWSRout = *nWSR;
qpb->getDualSolution( y );
return;
}
/*
* q p O A S E S _ o b t a i n O u t p u t s
*/
int_t qpOASES_obtainOutputs( const QProblemB* const globalQpObject,
returnValue returnvalue,
real_t* const x,
real_t* const y,
real_t* const obj,
int_t* const status
)
{
if ( globalQpObject == 0 )
return -1;
globalQpObject->getPrimalSolution( x );
globalQpObject->getDualSolution( y );
*obj = globalQpObject->getObjVal( );
*status = getSimpleStatus( returnvalue );
return 0;
}
/*
* i n i t S B
*/
void initSB( real_t* H, real_t* g, real_t* lb, real_t* ub,
int* nV, int* nWSR,
real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
)
{
cleanupSB( );
/* setup and solve initial QP */
qpb = new QProblemB( *nV );
qpb->setPrintLevel( PL_LOW );
returnValue returnvalue = qpb->init( H,g,lb,ub, *nWSR,0 );
/* assign lhs arguments */
qpb->getPrimalSolution( x );
*obj = qpb->getObjVal( );
*status = getSimpleStatus( returnvalue );
*nWSRout = *nWSR;
qpb->getDualSolution( y );
return;
}
/*
* q p o a s e s
*/
void qpoases( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
int *nV, int* nC, int* nWSR,
real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
)
{
/* transform A into C style matrix */
transformA( A, *nV,*nC );
/* setup and solve initial QP */
QProblem single_qp( *nV,*nC );
single_qp.setPrintLevel( PL_LOW );
returnValue returnvalue = single_qp.init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
/* assign lhs arguments */
single_qp.getPrimalSolution( x );
*obj = single_qp.getObjVal( );
*status = getSimpleStatus( returnvalue );
*nWSRout = *nWSR;
single_qp.getDualSolution( y );
return;
}
/*
* i n i t V M
*/
void initVM( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
int* nV, int* nC, int* nWSR,
real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
)
{
cleanupVM( );
/* transform A into C style matrix */
transformA( A, *nV,*nC );
/* setup and solve initial QP */
sqp = new SQProblem( *nV,*nC );
sqp->setPrintLevel( PL_LOW );
returnValue returnvalue = sqp->init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
/* assign lhs arguments */
sqp->getPrimalSolution( x );
*obj = sqp->getObjVal( );
*status = getSimpleStatus( returnvalue );
*nWSRout = *nWSR;
sqp->getDualSolution( y );
return;
}
/*
* o b t a i n O u t p u t s
*/
returnValue obtainOutputs( int_t k, QProblemB* qp, returnValue returnvalue, int_t _nWSRout, double _cpuTime,
int nlhs, mxArray* plhs[], int_t nV, int_t nC = 0, int_t handle = -1
)
{
/* Create output vectors and assign pointers to them. */
int_t curIdx = 0;
/* handle */
if ( handle >= 0 )
plhs[curIdx++] = mxCreateDoubleScalar( handle );
/* x */
double* x = mxGetPr( plhs[curIdx++] );
qp->getPrimalSolution( &(x[k*nV]) );
if ( nlhs > curIdx )
{
/* fval */
double* obj = mxGetPr( plhs[curIdx++] );
obj[k] = qp->getObjVal( );
if ( nlhs > curIdx )
{
/* exitflag */
double* status = mxGetPr( plhs[curIdx++] );
status[k] = (double)getSimpleStatus( returnvalue );
if ( nlhs > curIdx )
{
/* iter */
double* nWSRout = mxGetPr( plhs[curIdx++] );
nWSRout[k] = (double) _nWSRout;
if ( nlhs > curIdx )
{
/* lambda */
double* y = mxGetPr( plhs[curIdx++] );
qp->getDualSolution( &(y[k*(nV+nC)]) );
/* auxOutput */
if ( nlhs > curIdx )
{
QProblem* problemPointer;
problemPointer = dynamic_cast<QProblem*>(qp);
mxArray* auxOutput = plhs[curIdx];
mxArray* curField = 0;
/* working set bounds */
if ( nV > 0 )
{
curField = mxGetField( auxOutput,0,"workingSetB" );
double* workingSetB = mxGetPr(curField);
/* cast successful? */
if (problemPointer != NULL) {
problemPointer->getWorkingSetBounds( &(workingSetB[k*nV]) );
} else {
qp->getWorkingSetBounds( &(workingSetB[k*nV]) );
}
}
/* working set constraints */
if ( nC > 0 )
{
curField = mxGetField( auxOutput,0,"workingSetC" );
double* workingSetC = mxGetPr(curField);
/* cast successful? */
if (problemPointer != NULL) {
problemPointer->getWorkingSetConstraints( &(workingSetC[k*nC]) );
} else {
qp->getWorkingSetConstraints( &(workingSetC[k*nC]) );
}
}
/* cpu time */
curField = mxGetField( auxOutput,0,"cpuTime" );
double* cpuTime = mxGetPr(curField);
cpuTime[0] = (double) _cpuTime;
}
}
}
}
}
return SUCCESSFUL_RETURN;
}
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;
real_T *out_uOpt, *out_objVal, *out_status, *out_nWSR;
/* 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];
}
xOpt = new real_t[nV];
if ( problem->getCount() == 0 )
{
/* initialise and solve first QP */
status = problem->init( H,g,lb,ub, nWSR,0 );
problem->getPrimalSolution( xOpt );
}
else
{
/* solve neighbouring QP using hotstart technique */
status = problem->hotstart( g,lb,ub, nWSR,0 );
if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
{
/* if an error occurs, reset problem data structures ... */
problem->reset( );
/* ... and initialise/solve again with remaining number of iterations. */
int nWSR_retry = MAXITER - nWSR;
status = problem->init( H,g,lb,ub, nWSR_retry,0 );
nWSR += nWSR_retry;
}
/* obtain optimal solution */
problem->getPrimalSolution( 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)(problem->getObjVal());
out_status[0] = (real_t)(getSimpleStatus( status ));
out_nWSR[0] = (real_T)(nWSR);
removeNaNs( out_uOpt,nU );
removeInfs( out_uOpt,nU );
removeNaNs( out_objVal,1 );
removeInfs( out_objVal,1 );
delete[] xOpt;
}
/** Example for qpOASES main function using the QProblem class. */
int main( )
{
USING_NAMESPACE_QPOASES
/* Setup data of first QP. */
real_t H[5*5] = { 1.224642131370767e+01, 2.908638763113702e+00, 0.0, 0.0, 0.0,
2.908638763113702e+00, 2.497106275003180e+00, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 5.158460640334052e-02, 4.723556059962540e-02,
0.0, 0.0, 0.0, 4.723556059962540e-02, 4.325317843302175e-02 };
real_t A[2*5] = { -1.404358970692652e+00, -2.556613491156063e+00, 3.202524559238066e+00, -1.0, 0.0,
6.587910295430314e-01, -5.349454475937998e-01, 4.391976356955536e-01, 0.0, -1.0 };
real_t g[5] = { 2.474135331302147e+01,
5.857286430296258e+00,
2.359382646348721e-01,
1.721047069188781e-01,
1.575947337774199e-01 };
real_t lb[5] = { -10.0, -10.0, -10.0, -10.0, -10.0 };
real_t ub[5] = { 10.0, 10.0, 10.0, 10.0, 10.0 };
real_t lbA[2] = { 1.643135416077167e+00, 1.056813028189597e+00 };
real_t ubA[2] = { 1.643135416077167e+00, 1.056813028189597e+00 };
/* Setting up QProblem object. */
QProblem example( 5,2 );
Options options;
//options.enableFlippingBounds = BT_FALSE;
//options.enableEqualities = BT_TRUE;
//options.initialStatusBounds = ST_INACTIVE;
example.setOptions( options );
example.setPrintLevel( PL_NONE );
/* Solve first QP. */
returnValue retVal;
int_t simpleStatus = -1;
int_t nWSR = 10;
retVal = example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
simpleStatus = getSimpleStatus( retVal,BT_TRUE );
QPOASES_TEST_FOR_TRUE( simpleStatus == 0 );
/* Get and print solution of second QP. */
real_t xOpt[5];
real_t yOpt[5+2];
example.getPrimalSolution( xOpt );
example.getDualSolution( yOpt );
printf( "\nxOpt = [ %e, %e, ... ]; objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
/* Compute KKT tolerances */
real_t stat, feas, cmpl;
SolutionAnalysis analyzer;
analyzer.getKktViolation( &example, &stat,&feas,&cmpl );
printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
QPOASES_TEST_FOR_TOL( stat,1e-14 );
QPOASES_TEST_FOR_TOL( feas,1e-14 );
QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
return TEST_PASSED;
}
