/** Example for qpOASES main function using the QProblem class. */
int main( )
{
/* 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 };
int_t nWSR;
qpOASES_Options options;
real_t xOpt[2];
real_t yOpt[2];
real_t obj;
int_t status;
qpOASES_Options_init( &options,0 );
/*options.enableFlippingBounds = 0; */
options.initialStatusBounds = ST_INACTIVE;
options.numRefinementSteps = 1;
options.enableCholeskyRefactorisation = 1;
QProblemB_setup( 2,HST_UNKNOWN );
/* Solve first QP. */
nWSR = 10;
QProblemB_init( H,g,lb,ub,
&nWSR,0,&options,
xOpt,yOpt,&obj,&status
);
/* Print solution of first QP. */
printf( "\nxOpt = [ %e, %e ]; yOpt = [ %e, %e ]; objVal = %e\n\n",
xOpt[0],xOpt[1],yOpt[0],yOpt[1], obj );
/* Solve second QP. */
nWSR = 10;
QProblemB_hotstart( g_new,lb_new,ub_new,
&nWSR,0,
xOpt,yOpt,&obj,&status
);
/* Print solution of first QP. */
printf( "\nxOpt = [ %e, %e ]; yOpt = [ %e, %e ]; objVal = %e\n\n",
xOpt[0],xOpt[1],yOpt[0],yOpt[1], obj );
QProblemB_cleanup();
return 0;
}
/** 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;
}
/*
* m e x F u n c t i o n
*/
void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
{
/* inputs */
char typeString[2];
real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0;
HessianType hessianType = HST_UNKNOWN;
double *x0=0, *R=0, *R_for=0;
double *guessedBounds=0, *guessedConstraints=0;
int_t H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
int_t x0_idx=-1, auxInput_idx=-1;
BooleanType isSimplyBoundedQp = BT_FALSE;
#ifdef SOLVER_MA57
BooleanType isSparse = BT_TRUE; /* This will be set to BT_FALSE later if a dense matrix is encountered. */
#else
BooleanType isSparse = BT_FALSE;
#endif
Options options;
options.printLevel = PL_LOW;
#ifdef __DEBUG__
options.printLevel = PL_HIGH;
#endif
#ifdef __SUPPRESSANYOUTPUT__
options.printLevel = PL_NONE;
#endif
/* dimensions */
uint_t nV=0, nC=0, handle=0;
int_t nWSRin;
real_t maxCpuTimeIn = -1.0;
QPInstance* globalQP = 0;
/* I) CONSISTENCY CHECKS: */
/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
{
if ( nrhs != 2 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
}
/* 2) Ensure that first input is a string ... */
if ( mxIsChar( prhs[0] ) != 1 )
{
myMexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
return;
}
mxGetString( prhs[0], typeString, 2 );
/* ... and if so, check if it is an allowed one. */
if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
( strcmp( typeString,"m" ) != 0 ) && ( strcmp( typeString,"M" ) != 0 ) &&
( strcmp( typeString,"e" ) != 0 ) && ( strcmp( typeString,"E" ) != 0 ) &&
( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
/* 1) Init (without or with initial guess for primal solution). */
if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
{
/* consistency checks */
if ( ( nlhs < 1 ) || ( nlhs > 7 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
g_idx = 2;
if ( mxIsEmpty(prhs[1]) == 1 )
{
H_idx = -1;
nV = (uint_t)mxGetM( prhs[ g_idx ] ); /* row number of Hessian matrix */
}
else
{
H_idx = 1;
nV = (uint_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
}
/* ensure that data is given in double precision */
if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ) ||
( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
return;
}
if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Hessian matrix dimension mismatch!" );
return;
}
/* Check for 'Inf' and 'Nan' in Hessian */
if (containsNaNorInf( prhs,H_idx, 0 ) == BT_TRUE)
return;
/* Check for 'Inf' and 'Nan' in gradient */
if (containsNaNorInf(prhs,g_idx, 0 ) == BT_TRUE)
return;
/* determine whether is it a simply bounded QP */
if ( nrhs <= 7 )
isSimplyBoundedQp = BT_TRUE;
else
isSimplyBoundedQp = BT_FALSE;
if ( isSimplyBoundedQp == BT_TRUE )
{
lb_idx = 3;
ub_idx = 4;
if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
/* Check inputs dimensions and assign pointers to inputs. */
nC = 0; /* row number of constraint matrix */
if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
return;
/* default value for nWSR */
nWSRin = 5*nV;
/* Check whether x0 and options are specified .*/
if ( nrhs >= 6 )
{
if ((!mxIsEmpty(prhs[5])) && (mxIsStruct(prhs[5])))
setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
if ( ( nrhs >= 7 ) && ( !mxIsEmpty(prhs[6]) ) )
{
/* auxInput specified */
if ( mxIsStruct(prhs[6]) )
{
auxInput_idx = 6;
x0_idx = -1;
}
else
{
auxInput_idx = -1;
x0_idx = 6;
}
}
else
{
auxInput_idx = -1;
x0_idx = -1;
}
}
}
else
{
A_idx = 3;
/* ensure that data is given in double precision */
if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
return;
}
/* Check inputs dimensions and assign pointers to inputs. */
nC = (uint_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
lb_idx = 4;
ub_idx = 5;
lbA_idx = 6;
ubA_idx = 7;
if (containsNaNorInf( prhs,A_idx, 0 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
return;
if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Constraint matrix dimension mismatch!" );
return;
}
if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
return;
/* default value for nWSR */
nWSRin = 5*(nV+nC);
/* Check whether x0 and options are specified .*/
if ( nrhs >= 9 )
{
if ((!mxIsEmpty(prhs[8])) && (mxIsStruct(prhs[8])))
setupOptions( &options,prhs[8],nWSRin,maxCpuTimeIn );
if ( ( nrhs >= 10 ) && ( !mxIsEmpty(prhs[9]) ) )
{
/* auxInput specified */
if ( mxIsStruct(prhs[9]) )
{
auxInput_idx = 9;
x0_idx = -1;
}
else
{
auxInput_idx = -1;
x0_idx = 9;
}
}
else
{
auxInput_idx = -1;
x0_idx = -1;
}
}
}
/* check dimensions and copy auxInputs */
if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
return;
if ( auxInput_idx >= 0 )
setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &hessianType,&x0,&guessedBounds,&guessedConstraints,&R_for );
/* convert Cholesky factor to C storage format */
if ( R_for != 0 )
{
R = new real_t[nV*nV];
convertFortranToC( R_for, nV,nV, R );
}
/* check if QP is sparse */
if ( H_idx >= 0 && !mxIsSparse( prhs[H_idx] ) )
isSparse = BT_FALSE;
if ( nC > 0 && A_idx >= 0 && !mxIsSparse( prhs[A_idx] ) )
isSparse = BT_FALSE;
/* allocate instance */
handle = allocateQPInstance( nV,nC,hessianType, isSimplyBoundedQp, isSparse, &options );
globalQP = getQPInstance( handle );
/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
if ( H_idx >= 0 )
setupHessianMatrix( prhs[H_idx],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
if ( ( nC > 0 ) && ( A_idx >= 0 ) )
setupConstraintMatrix( prhs[A_idx],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
/* Create output vectors and assign pointers to them. */
allocateOutputs( nlhs,plhs, nV,nC,1,handle );
/* Call qpOASES. */
if ( isSimplyBoundedQp == BT_TRUE )
{
QProblemB_init( handle,
globalQP->H,g,
lb,ub,
nWSRin,maxCpuTimeIn,
x0,&options,
nlhs,plhs,
guessedBounds,R
);
}
else
{
SQProblem_init( handle,
globalQP->H,g,globalQP->A,
lb,ub,lbA,ubA,
nWSRin,maxCpuTimeIn,
x0,&options,
nlhs,plhs,
guessedBounds,guessedConstraints,R
);
}
if (R != 0) delete R;
return;
}
/* 2) Hotstart. */
if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
{
/* consistency checks */
if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( nrhs < 5 ) || ( nrhs > 8 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* determine whether is it a simply bounded QP */
if ( nrhs < 7 )
isSimplyBoundedQp = BT_TRUE;
else
isSimplyBoundedQp = BT_FALSE;
if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* get QP instance */
handle = (uint_t)mxGetScalar( prhs[1] );
globalQP = getQPInstance( handle );
if ( globalQP == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
return;
}
nV = globalQP->getNV();
g_idx = 2;
lb_idx = 3;
ub_idx = 4;
if (containsNaNorInf( prhs,g_idx, 0 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
/* Check inputs dimensions and assign pointers to inputs. */
if ( isSimplyBoundedQp == BT_TRUE )
{
nC = 0;
if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
/* default value for nWSR */
nWSRin = 5*nV;
/* Check whether options are specified .*/
if ( nrhs == 6 )
if ( ( !mxIsEmpty( prhs[5] ) ) && ( mxIsStruct( prhs[5] ) ) )
setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
}
else
{
nC = globalQP->getNC( );
lbA_idx = 5;
ubA_idx = 6;
if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
return;
if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
return;
/* default value for nWSR */
nWSRin = 5*(nV+nC);
/* Check whether options are specified .*/
if ( nrhs == 8 )
if ( ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
}
/* Create output vectors and assign pointers to them. */
allocateOutputs( nlhs,plhs, nV,nC );
/* call qpOASES */
if ( isSimplyBoundedQp == BT_TRUE )
{
QProblemB_hotstart( handle, g,
lb,ub,
nWSRin,maxCpuTimeIn,
&options,
nlhs,plhs
);
}
else
{
QProblem_hotstart( handle, g,
lb,ub,lbA,ubA,
nWSRin,maxCpuTimeIn,
&options,
nlhs,plhs
);
}
return;
}
/* 3) Modify matrices. */
if ( ( strcmp( typeString,"m" ) == 0 ) || ( strcmp( typeString,"M" ) == 0 ) )
{
/* consistency checks */
if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( nrhs < 9 ) || ( nrhs > 10 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* get QP instance */
handle = (uint_t)mxGetScalar( prhs[1] );
globalQP = getQPInstance( handle );
if ( globalQP == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
return;
}
/* Check inputs dimensions and assign pointers to inputs. */
g_idx = 3;
if ( mxIsEmpty(prhs[2]) == 1 )
{
H_idx = -1;
nV = (uint_t)mxGetM( prhs[ g_idx ] ); /* if Hessian is empty, row number of gradient vector */
}
else
{
H_idx = 2;
nV = (uint_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
}
A_idx = 4;
nC = (uint_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
lb_idx = 5;
ub_idx = 6;
lbA_idx = 7;
ubA_idx = 8;
/* ensure that data is given in double precision */
if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[H_idx] ) == 0 ) ) ||
( mxIsDouble( prhs[g_idx] ) == 0 ) ||
( mxIsDouble( prhs[A_idx] ) == 0 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
return;
}
/* check if supplied data contains 'NaN' or 'Inf' */
if (containsNaNorInf(prhs,H_idx, 0) == BT_TRUE)
return;
if (containsNaNorInf( prhs,g_idx, 0 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,A_idx, 0 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
return;
/* Check that dimensions are consistent with existing QP instance */
if (nV != (uint_t) globalQP->getNV () || nC != (uint_t) globalQP->getNC ())
{
myMexErrMsgTxt( "ERROR (qpOASES): QP dimensions must be constant during a sequence! Try creating a new QP instance instead." );
return;
}
if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Hessian matrix dimension mismatch!" );
return;
}
if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Constraint matrix dimension mismatch!" );
return;
}
if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
return;
/* default value for nWSR */
nWSRin = 5*(nV+nC);
/* Check whether options are specified .*/
if ( nrhs > 9 )
if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
setupOptions( &options,prhs[9],nWSRin,maxCpuTimeIn );
globalQP->deleteQPMatrices( );
/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
if ( H_idx >= 0 )
setupHessianMatrix( prhs[H_idx],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
if ( ( nC > 0 ) && ( A_idx >= 0 ) )
setupConstraintMatrix( prhs[A_idx],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
/* Create output vectors and assign pointers to them. */
allocateOutputs( nlhs,plhs, nV,nC );
/* Call qpOASES */
SQProblem_hotstart( handle, globalQP->H,g,globalQP->A,
lb,ub,lbA,ubA,
nWSRin,maxCpuTimeIn,
&options,
nlhs,plhs
);
return;
}
/* 4) Solve current equality constrained QP. */
if ( ( strcmp( typeString,"e" ) == 0 ) || ( strcmp( typeString,"E" ) == 0 ) )
{
/* consistency checks */
if ( ( nlhs < 1 ) || ( nlhs > 4 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( nrhs < 7 ) || ( nrhs > 8 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* get QP instance */
handle = (uint_t)mxGetScalar( prhs[1] );
globalQP = getQPInstance( handle );
if ( globalQP == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
return;
}
/* Check inputs dimensions and assign pointers to inputs. */
int_t nRHS = (int_t)mxGetN(prhs[2]);
nV = globalQP->getNV( );
nC = globalQP->getNC( );
real_t *x_out, *y_out;
g_idx = 2;
lb_idx = 3;
ub_idx = 4;
lbA_idx = 5;
ubA_idx = 6;
/* check if supplied data contains 'NaN' or 'Inf' */
if (containsNaNorInf(prhs,g_idx, 0) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
return;
if ( smartDimensionCheck( &g,nV,nRHS, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,nRHS, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,nRHS, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lbA,nC,nRHS, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ubA,nC,nRHS, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
return;
/* Check whether options are specified .*/
if ( ( nrhs == 8 ) && ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
{
nWSRin = 5*(nV+nC);
setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
globalQP->sqp->setOptions( options );
}
/* Create output vectors and assign pointers to them. */
plhs[0] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
x_out = mxGetPr(plhs[0]);
if (nlhs >= 2)
{
plhs[1] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
y_out = mxGetPr(plhs[1]);
if (nlhs >= 3)
{
plhs[2] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
real_t* workingSetB = mxGetPr(plhs[2]);
globalQP->sqp->getWorkingSetBounds(workingSetB);
if ( nlhs >= 4 )
{
plhs[3] = mxCreateDoubleMatrix( nC, nRHS, mxREAL );
real_t* workingSetC = mxGetPr(plhs[3]);
globalQP->sqp->getWorkingSetConstraints(workingSetC);
}
}
}
else
y_out = new real_t[nV+nC];
/* Solve equality constrained QP */
returnValue returnvalue = globalQP->sqp->solveCurrentEQP( nRHS,g,lb,ub,lbA,ubA, x_out,y_out );
if (nlhs < 2)
delete[] y_out;
if (returnvalue != SUCCESSFUL_RETURN)
{
char msg[MAX_STRING_LENGTH];
snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Couldn't solve current EQP (code %d)!", returnvalue);
myMexErrMsgTxt(msg);
return;
}
return;
}
/* 5) Cleanup. */
if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
{
/* consistency checks */
if ( nlhs != 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( nrhs != 2 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* Cleanup SQProblem instance. */
handle = (uint_t)mxGetScalar( prhs[1] );
deleteQPInstance( handle );
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[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 );
}
/*
* 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;
}