/*
* Q P r o b l e m _ h o t s t a r t
*/
int_t QProblem_hotstart( int_t handle,
const real_t* const g,
const real_t* const lb, const real_t* const ub,
const real_t* const lbA, const real_t* const ubA,
int_t nWSRin, real_t maxCpuTimeIn,
Options* options,
int_t nOutputs, mxArray* plhs[]
)
{
int_t nWSRout = nWSRin;
real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
QProblem* globalSQP = getQPInstance(handle)->sqp;
if ( globalSQP == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
return -1;
}
int_t nV = globalSQP->getNV();
int_t nC = globalSQP->getNC();
/* 1) Solve QP with given options. */
globalSQP->setOptions( *options );
returnValue returnvalue = globalSQP->hotstart( g,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
/* 2) Assign lhs arguments. */
obtainOutputs( 0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
nOutputs,plhs,nV,nC );
return 0;
}
/*
* 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)
{
#ifndef __DSPACE__
using namespace qpOASES;
#endif
int i;
int nV, nC, status;
int nWSR = NWSR;
//int nU = NCONTROLINPUTS;
InputRealPtrsType in_H, in_g, in_A, in_lb, in_ub, in_lbA, in_ubA;
QProblem* problem;
double *H, *g, *A, *lb, *ub, *lbA, *ubA, *count;
double *xOpt;
real_T *out_objVal, *out_xOpt, *out_status, *out_nWSR;
/* get pointers to block inputs ... */
in_H = ssGetInputPortRealSignalPtrs(S, 0);
in_g = ssGetInputPortRealSignalPtrs(S, 1);
in_A = ssGetInputPortRealSignalPtrs(S, 2);
in_lb = ssGetInputPortRealSignalPtrs(S, 3);
in_ub = ssGetInputPortRealSignalPtrs(S, 4);
in_lbA = ssGetInputPortRealSignalPtrs(S, 5);
in_ubA = ssGetInputPortRealSignalPtrs(S, 6);
/* ... and to the QP data */
problem = (QProblem *) ssGetPWork(S)[0];
H = (double *) ssGetPWork(S)[1];
g = (double *) ssGetPWork(S)[2];
A = (double *) ssGetPWork(S)[3];
lb = (double *) ssGetPWork(S)[4];
ub = (double *) ssGetPWork(S)[5];
lbA = (double *) ssGetPWork(S)[6];
ubA = (double *) ssGetPWork(S)[7];
count = (double *) ssGetPWork(S)[8];
/* setup QP data */
nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
nC = (int) ( ((double) ssGetInputPortWidth(S, 2)) / ((double) nV) ); /* nC = size_A / size_g */
for ( i=0; i<nV*nV; ++i )
H[i] = (*in_H)[i];
for ( i=0; i<nC*nV; ++i )
A[i] = (*in_A)[i];
for ( i=0; i<nV; ++i )
{
g[i] = (*in_g)[i];
lb[i] = (*in_lb)[i];
ub[i] = (*in_ub)[i];
}
for ( i=0; i<nC; ++i )
{
lbA[i] = (*in_lbA)[i];
ubA[i] = (*in_ubA)[i];
}
xOpt = new double[nV];
if ( count[0] == 0 )
{
/* initialise and solve first QP */
status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
ssGetPWork(S)[0] = ( void* ) problem;
status = problem->getPrimalSolution( xOpt );
}
else
{
problem->reset( );
problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
/* solve neighbouring QP using hotstart technique */
//status = problem->hotstart( g,lb,ub,lbA,ubA, nWSR,0 );
//if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
//{
/* if an error occurs, reset problem data structures and initialise again */
// problem->reset( );
// problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
//}
//else
//{
/* otherwise obtain optimal solution */
status = problem->getPrimalSolution( xOpt );
//}
}
/* generate block output: status information ... */
out_objVal = ssGetOutputPortRealSignal(S, 0);
out_xOpt = ssGetOutputPortRealSignal(S, 1);
out_status = ssGetOutputPortRealSignal(S, 2);
out_nWSR = ssGetOutputPortRealSignal(S, 3);
out_objVal[0] = ((real_T) problem->getObjVal( ));
for ( i=0; i<nV; ++i )
out_xOpt[i] = ((real_T) xOpt[i]);
switch ( status )
{
case SUCCESSFUL_RETURN:
out_status[0] = 0.0;
break;
case RET_MAX_NWSR_REACHED:
out_status[0] = 1.0;
break;
default:
out_status[0] = -1.0;
break;
}
out_nWSR[0] = ((real_T) nWSR);
/* increase counter */
count[0] = count[0] + 1;
delete[] xOpt;
}
static void mdlStart(SimStruct *S)
{
#ifndef __DSPACE__
using namespace qpOASES;
#endif
//int nU = NCONTROLINPUTS;
int size_H, size_g, size_A, size_lb, size_ub, size_lbA, size_ubA;
int nV, nC;
QProblem* problem;
double* count;
/* get block inputs dimensions */
size_H = ssGetInputPortWidth(S, 0);
size_g = ssGetInputPortWidth(S, 1);
size_A = ssGetInputPortWidth(S, 2);
size_lb = ssGetInputPortWidth(S, 3);
size_ub = ssGetInputPortWidth(S, 4);
size_lbA = ssGetInputPortWidth(S, 5);
size_ubA = ssGetInputPortWidth(S, 6);
/* dimension checks */
nV = size_g;
nC = (int) ( ((double) size_A) / ((double) nV) );
if ( nV == 0 )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!" );
#endif
return;
}
if ( size_H != nV*nV )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!" );
#endif
return;
}
//if ( nU > nV )
//{
// #ifndef __DSPACE__
// printf( "ERROR (qpOASES): Dimension mismatch!" );
// #endif
// return;
// }
if ( size_lb != nV )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!" );
#endif
return;
}
if ( size_ub != nV )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!" );
#endif
return;
}
if ( size_lbA != nC )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!" );
#endif
return;
}
if ( size_ubA != nC )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!" );
#endif
return;
}
/* allocate QProblem object */
problem = new QProblem( nV,nC );
if ( problem == 0 )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Unable to create QProblem object!" );
#endif
return;
}
#ifndef __DEBUG__
problem->setPrintLevel( PL_LOW );
#endif
#ifdef __SUPPRESSANYOUTPUT__
problem->setPrintLevel( PL_NONE );
#endif
#ifdef __DSPACE__
problem->setPrintLevel( PL_NONE );
#endif
ssGetPWork(S)[0] = (void *) problem;
/* allocate memory for QP data ... */
ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(double) ); /* H */
ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(double) ); /* g */
ssGetPWork(S)[3] = (void *) calloc( size_A, sizeof(double) ); /* A */
ssGetPWork(S)[4] = (void *) calloc( size_lb, sizeof(double) ); /* lb */
ssGetPWork(S)[5] = (void *) calloc( size_ub, sizeof(double) ); /* ub */
ssGetPWork(S)[6] = (void *) calloc( size_lbA, sizeof(double) ); /* lbA */
ssGetPWork(S)[7] = (void *) calloc( size_ubA, sizeof(double) ); /* ubA */
ssGetPWork(S)[8] = (void *) calloc( 1, sizeof(double) ); /* count */
/* reset counter */
count = (double *) ssGetPWork(S)[8];
count[0] = 0.0;
}
