/*
* Q P r o b l e m B _ h o t s t a r t
*/
int_t QProblemB_hotstart( int_t handle,
const real_t* const g,
const real_t* const lb, const real_t* const ub,
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;
QProblemB* globalQPB = getQPInstance(handle)->qpb;
if ( globalQPB == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
return -1;
}
int_t nV = globalQPB->getNV();
/* 1) Solve QP with given options. */
globalQPB->setOptions( *options );
returnValue returnvalue = globalQPB->hotstart( g,lb,ub, nWSRout,&maxCpuTimeOut );
/* 2) Assign lhs arguments. */
obtainOutputs( 0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
nOutputs,plhs,nV,0 );
return 0;
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
#ifndef __DSPACE__
using namespace qpOASES;
#endif
int i;
int nV, status;
int nWSR = NWSR;
int nU = NCONTROLINPUTS;
InputRealPtrsType in_H, in_g, in_lb, in_ub;
QProblemB* problem;
double *H, *g, *lb, *ub, *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_lb = ssGetInputPortRealSignalPtrs(S, 2);
in_ub = ssGetInputPortRealSignalPtrs(S, 3);
/* ... and to the QP data */
problem = (QProblemB *) ssGetPWork(S)[0];
H = (double *) ssGetPWork(S)[1];
g = (double *) ssGetPWork(S)[2];
lb = (double *) ssGetPWork(S)[3];
ub = (double *) ssGetPWork(S)[4];
count = (double *) ssGetPWork(S)[5];
/* setup QP data */
nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
for ( i=0; i<nV*nV; ++i )
H[i] = (*in_H)[i];
for ( i=0; i<nV; ++i )
{
g[i] = (*in_g)[i];
lb[i] = (*in_lb)[i];
ub[i] = (*in_ub)[i];
}
xOpt = new double[nV];
if ( count[0] == 0 )
{
/* initialise and solve first QP */
status = problem->init( H,g,lb,ub, nWSR,0 );
ssGetPWork(S)[0] = ( void* ) problem;
problem->getPrimalSolution( xOpt );
}
else
{
#ifdef XXX
/* 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 and initialise again */
problem->reset( );
problem->init( H,g,lb,ub, nWSR,0 );
}
else
{
/* otherwise obtain optimal solution */
problem->getPrimalSolution( xOpt );
}
#endif
problem->reset( );
problem->init( H,g,lb,ub, nWSR,0 );
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<nU; ++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_lb, size_ub;
int nV;
QProblemB* problem;
double* count;
/* get block inputs dimensions */
size_H = ssGetInputPortWidth(S, 0);
size_g = ssGetInputPortWidth(S, 1);
size_lb = ssGetInputPortWidth(S, 2);
size_ub = ssGetInputPortWidth(S, 3);
/* dimension checks */
nV = size_g;
if ( nV == 0 )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!\n" );
#endif
return;
}
if ( size_H != nV*nV )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!\n" );
#endif
return;
}
if ( nU > nV )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!\n" );
#endif
return;
}
if ( size_lb != nV )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!\n" );
#endif
return;
}
if ( size_ub != nV )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Dimension mismatch!\n" );
#endif
return;
}
/* allocate QProblemB object */
problem = new QProblemB( nV );
if ( problem == 0 )
{
#ifndef __DSPACE__
printf( "ERROR (qpOASES): Unable to create QProblemB object!\n" );
#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_lb, sizeof(double) ); /* lb */
ssGetPWork(S)[4] = (void *) calloc( size_ub, sizeof(double) ); /* ub */
ssGetPWork(S)[5] = (void *) calloc( 1, sizeof(double) ); /* count */
/* reset counter */
count = (double *) ssGetPWork(S)[5];
count[0] = 0.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;
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;
}
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;
QProblemB* problem;
/* get block inputs dimensions */
const mxArray* in_H = ssGetSFcnParam(S, 0);
if ( mxIsEmpty(in_H) == 1 )
{
if ( ( HESSIANTYPE != HST_ZERO ) && ( HESSIANTYPE != HST_IDENTITY ) )
{
#ifndef __DSPACE__
#ifndef __XPCTARGET__
mexErrMsgTxt( "ERROR (qpOASES): Hessian can only be empty if type is set to HST_ZERO or HST_IDENTITY!" );
#endif
#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 __DSPACE__
#ifndef __XPCTARGET__
mexErrMsgTxt( "ERROR (qpOASES): Maximum number of iterations must not be negative!" );
#endif
#endif
return;
}
if ( nV <= 0 )
{
#ifndef __DSPACE__
#ifndef __XPCTARGET__
mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
#endif
#endif
return;
}
if ( ( size_H != nV*nV ) && ( size_H != 0 ) )
{
#ifndef __DSPACE__
#ifndef __XPCTARGET__
mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch in H!" );
#endif
#endif
return;
}
if ( nRows_H != nCols_H )
{
#ifndef __DSPACE__
#ifndef __XPCTARGET__
mexErrMsgTxt( "ERROR (qpOASES): Hessian matrix must be square matrix!" );
#endif
#endif
return;
}
if ( ( nU < 1 ) || ( nU > nV ) )
{
#ifndef __DSPACE__
#ifndef __XPCTARGET__
mexErrMsgTxt( "ERROR (qpOASES): Invalid number of control inputs!" );
#endif
#endif
return;
}
if ( ( size_lb != nV ) && ( size_lb != 0 ) )
{
#ifndef __DSPACE__
#ifndef __XPCTARGET__
mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch in lb!" );
#endif
#endif
return;
}
if ( ( size_ub != nV ) && ( size_ub != 0 ) )
{
#ifndef __DSPACE__
#ifndef __XPCTARGET__
mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch in ub!" );
#endif
#endif
return;
}
/* allocate QProblemB object */
problem = new QProblemB( nV,HESSIANTYPE );
if ( problem == 0 )
{
#ifndef __DSPACE__
#ifndef __XPCTARGET__
mexErrMsgTxt( "ERROR (qpOASES): Unable to create QProblemB object!" );
#endif
#endif
return;
}
Options problemOptions;
problemOptions.setToMPC();
problem->setOptions( problemOptions );
#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 ... */
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;
}
/*
* Q P r o b l e m B _ i n i t
*/
int_t QProblemB_init( int_t handle,
SymmetricMatrix* H, real_t* g,
const real_t* const lb, const real_t* const ub,
int_t nWSRin, real_t maxCpuTimeIn,
const double* const x0, Options* options,
int_t nOutputs, mxArray* plhs[],
const double* const guessedBounds,
const double* const _R
)
{
int_t nWSRout = nWSRin;
real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
/* 1) setup initial QP. */
QProblemB* globalQPB = getQPInstance(handle)->qpb;
if ( globalQPB == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
return -1;
}
globalQPB->setOptions( *options );
/* 2) Solve initial QP. */
returnValue returnvalue;
int_t nV = globalQPB->getNV();
/* 3) Fill the working set. */
Bounds bounds(nV);
if (guessedBounds != 0) {
for (int_t i = 0; i < nV; i++) {
if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
bounds.setupBound(i, ST_LOWER);
} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
bounds.setupBound(i, ST_UPPER);
} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
bounds.setupBound(i, ST_INACTIVE);
} else {
char msg[MAX_STRING_LENGTH];
snprintf(msg, MAX_STRING_LENGTH,
"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
myMexErrMsgTxt(msg);
return -1;
}
}
}
returnvalue = globalQPB->init( H,g,lb,ub,
nWSRout,&maxCpuTimeOut,
x0,0,
(guessedBounds != 0) ? &bounds : 0,
_R
);
/* 3) Assign lhs arguments. */
obtainOutputs( 0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
nOutputs,plhs,nV,0,handle );
return 0;
}
