/*
* m e x F u n c t i o n
*/
void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
{
/* inputs */
SymmetricMatrix *H=0;
Matrix *A=0;
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 H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
int options_idx=-1, x0_idx=-1, auxInput_idx=-1;
/* Setup default options */
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, nP=0;
BooleanType isSimplyBoundedQp = BT_FALSE;
/* sparse matrix indices and values */
sparse_int_t *Hir=0, *Hjc=0, *Air=0, *Ajc=0;
real_t *Hv=0, *Av=0;
/* I) CONSISTENCY CHECKS: */
/* 1a) Ensure that qpOASES is called with a feasible number of input arguments. */
if ( ( nrhs < 4 ) || ( nrhs > 9 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES' for further information." );
return;
}
/* 2) Check for proper number of output arguments. */
if ( nlhs > 6 )
{
myMexErrMsgTxt( "ERROR (qpOASES): At most six output arguments are allowed: \n [x,fval,exitflag,iter,lambda,auxOutput]!" );
return;
}
if ( nlhs < 1 )
{
myMexErrMsgTxt( "ERROR (qpOASES): At least one output argument is required: [x,...]!" );
return;
}
/* II) PREPARE RESPECTIVE QPOASES FUNCTION CALL: */
/* Choose between QProblem and QProblemB object and assign the corresponding
* indices of the input pointer array in to order to access QP data correctly. */
g_idx = 1;
if ( mxIsEmpty(prhs[0]) == 1 )
{
H_idx = -1;
nV = (int_t)mxGetM( prhs[ g_idx ] ); /* if Hessian is empty, row number of gradient vector */
}
else
{
H_idx = 0;
nV = (int_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
}
nP = (int_t)mxGetN( prhs[ g_idx ] ); /* number of columns of the gradient matrix (vectors series have to be stored columnwise!) */
if ( nrhs <= 6 )
isSimplyBoundedQp = BT_TRUE;
else
isSimplyBoundedQp = BT_FALSE;
/* 0) Check whether options are specified .*/
if ( isSimplyBoundedQp == BT_TRUE )
{
if ( ( nrhs >= 5 ) && ( !mxIsEmpty(prhs[4]) ) && ( mxIsStruct(prhs[4]) ) )
options_idx = 4;
}
else
{
/* Consistency check */
if ( ( !mxIsEmpty(prhs[4]) ) && ( mxIsStruct(prhs[4]) ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Fifth input argument must not be a struct when solving QP with general constraints!\nType 'help qpOASES' for further information." );
return;
}
if ( ( nrhs >= 8 ) && ( !mxIsEmpty(prhs[7]) ) && ( mxIsStruct(prhs[7]) ) )
options_idx = 7;
}
// Is the third argument constraint Matrix A?
int_t numberOfColumns = (int_t)mxGetN(prhs[2]);
/* 1) Simply bounded QP. */
if ( ( isSimplyBoundedQp == BT_TRUE ) ||
( ( numberOfColumns == 1 ) && ( nV != 1 ) ) )
{
lb_idx = 2;
ub_idx = 3;
if ( ( nrhs >= 6 ) && ( !mxIsEmpty(prhs[5]) ) )
{
/* auxInput specified */
if ( mxIsStruct(prhs[5]) )
{
auxInput_idx = 5;
x0_idx = -1;
}
else
{
auxInput_idx = -1;
x0_idx = 5;
}
}
else
{
auxInput_idx = -1;
x0_idx = -1;
}
}
else
{
A_idx = 2;
/* If constraint matrix is empty, use a QProblemB object! */
if ( mxIsEmpty( prhs[ A_idx ] ) )
{
lb_idx = 3;
ub_idx = 4;
nC = 0;
}
else
{
lb_idx = 3;
ub_idx = 4;
lbA_idx = 5;
ubA_idx = 6;
nC = (int_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
}
if ( ( nrhs >= 9 ) && ( !mxIsEmpty(prhs[8]) ) )
{
/* auxInput specified */
if ( mxIsStruct(prhs[8]) )
{
auxInput_idx = 8;
x0_idx = -1;
}
else
{
auxInput_idx = -1;
x0_idx = 8;
}
}
else
{
auxInput_idx = -1;
x0_idx = -1;
}
}
/* ensure that data is given in real_t 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;
}
/* 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,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
/* Check inputs dimensions and assign pointers to inputs. */
if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
{
char msg[MAX_STRING_LENGTH];
snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Hessian matrix dimension mismatch (%ld != %d)!",
(long int)mxGetN(prhs[H_idx]), (int)nV);
myMexErrMsgTxt(msg);
return;
}
if ( nC > 0 )
{
/* ensure that data is given in real_t precision */
if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
return;
}
/* Check inputs dimensions and assign pointers to inputs. */
if ( mxGetN( prhs[ A_idx ] ) != nV )
{
char msg[MAX_STRING_LENGTH];
snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Constraint matrix input dimension mismatch (%ld != %d)!",
(long int)mxGetN(prhs[A_idx]), (int)nV);
myMexErrMsgTxt(msg);
return;
}
if (containsNaNorInf(prhs,A_idx, 0 ) == BT_TRUE)
return;
if (containsNaNorInf(prhs,lbA_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf(prhs,ubA_idx, 1 ) == BT_TRUE)
return;
}
/* check dimensions and copy auxInputs */
if ( smartDimensionCheck( &g,nV,nP, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,nP, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,nP, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
return;
if ( nC > 0 )
{
if ( smartDimensionCheck( &lbA,nC,nP, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ubA,nC,nP, BT_TRUE,prhs,ubA_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 );
}
/* III) ACTUALLY PERFORM QPOASES FUNCTION CALL: */
int_t nWSRin = 5*(nV+nC);
real_t maxCpuTimeIn = -1.0;
if ( options_idx > 0 )
setupOptions( &options,prhs[options_idx],nWSRin,maxCpuTimeIn );
/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
if ( H_idx >= 0 )
setupHessianMatrix( prhs[H_idx],nV, &H,&Hir,&Hjc,&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, &A,&Air,&Ajc,&Av );
allocateOutputs( nlhs,plhs,nV,nC,nP );
if ( nC == 0 )
{
/* Call qpOASES (using QProblemB class). */
QProblemB_qpOASES( nV,hessianType, nP,
H,g,
lb,ub,
nWSRin,maxCpuTimeIn,
x0,&options,
nlhs,plhs,
guessedBounds,R
);
if (R != 0) delete R;
if (H != 0) delete H;
if (Hv != 0) delete[] Hv;
if (Hjc != 0) delete[] Hjc;
if (Hir != 0) delete[] Hir;
return;
}
else
{
if ( A == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Internal interface error related to constraint matrix!" );
return;
}
/* Call qpOASES (using QProblem class). */
QProblem_qpOASES( nV,nC,hessianType, nP,
H,g,A,
lb,ub,lbA,ubA,
nWSRin,maxCpuTimeIn,
x0,&options,
nlhs,plhs,
guessedBounds,guessedConstraints,R
);
if (R != 0) delete R;
if (A != 0) delete A;
if (H != 0) delete H;
if (Av != 0) delete[] Av;
if (Ajc != 0) delete[] Ajc;
if (Air != 0) delete[] Air;
if (Hv != 0) delete[] Hv;
if (Hjc != 0) delete[] Hjc;
if (Hir != 0) delete[] Hir;
return;
}
}
/*
* s e t u p A u x i l i a r y I n p u t s
*/
returnValue setupAuxiliaryInputs( const mxArray* auxInput, uint_t nV, uint_t nC,
HessianType* hessianType, double** x0, double** guessedBounds, double** guessedConstraints, double** R
)
{
mxArray* curField = 0;
/* hessianType */
curField = mxGetField( auxInput,0,"hessianType" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'hessianType'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
if ( mxIsEmpty(curField) == true )
{
*hessianType = HST_UNKNOWN;
}
else
{
if ( mxIsScalar(curField) == false )
return RET_INVALID_ARGUMENTS;
double* hessianTypeTmp = mxGetPr(curField);
int_t hessianTypeInt = (int_t)*hessianTypeTmp;
if ( hessianTypeInt < 0 )
hessianTypeInt = 6; /* == HST_UNKNOWN */
if ( hessianTypeInt > 5 )
hessianTypeInt = 6; /* == HST_UNKNOWN */
*hessianType = (REFER_NAMESPACE_QPOASES HessianType)hessianTypeInt;
}
}
/* x0 */
curField = mxGetField( auxInput,0,"x0" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'x0'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*x0 = mxGetPr(curField);
if ( smartDimensionCheck( x0,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
/* guessedWorkingSetB */
curField = mxGetField( auxInput,0,"guessedWorkingSetB" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetB'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*guessedBounds = mxGetPr(curField);
if ( smartDimensionCheck( guessedBounds,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
/* guessedWorkingSetC */
curField = mxGetField( auxInput,0,"guessedWorkingSetC" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetC'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*guessedConstraints = mxGetPr(curField);
if ( smartDimensionCheck( guessedConstraints,nC,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
/* R */
curField = mxGetField( auxInput,0,"R" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'R'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*R = mxGetPr(curField);
if ( smartDimensionCheck( R,nV,nV, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
return SUCCESSFUL_RETURN;
}
/*
* 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;
}
}
