38 #include <qpOASES.hpp> 63 int_t nOutputs, mxArray* plhs[],
64 const double*
const guessedBounds,
65 const double*
const _R
68 int_t nWSRout = nWSRin;
69 real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn :
INFTY;
76 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid handle to QP instance!" );
88 if (guessedBounds != 0) {
89 for (
int_t i = 0; i < nV; i++) {
99 "ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
106 returnvalue = globalQPB->
init( H,g,lb,ub,
107 nWSRout,&maxCpuTimeOut,
109 (guessedBounds != 0) ? &bounds : 0,
114 obtainOutputs( 0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
115 nOutputs,plhs,nV,0,handle );
130 int_t nOutputs, mxArray* plhs[],
131 const double*
const guessedBounds,
const double*
const guessedConstraints,
132 const double*
const _R
135 int_t nWSRout = nWSRin;
136 real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn :
INFTY;
141 if ( globalSQP == 0 )
143 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid handle to QP instance!" );
157 if (guessedBounds != 0) {
158 for (
int_t i = 0; i < nV; i++) {
168 "ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
175 if (guessedConstraints != 0) {
176 for (
int_t i = 0; i < nC; i++) {
186 "ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
193 returnvalue = globalSQP->
init( H,g,A,lb,ub,lbA,ubA,
194 nWSRout,&maxCpuTimeOut,
196 (guessedBounds != 0) ? &bounds : 0, (guessedConstraints != 0) ? &constraints : 0,
201 obtainOutputs( 0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
202 nOutputs,plhs,nV,nC,handle );
217 int_t nOutputs, mxArray* plhs[]
220 int_t nWSRout = nWSRin;
221 real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn :
INFTY;
225 if ( globalQPB == 0 )
227 myMexErrMsgTxt(
"ERROR (qpOASES): QP needs to be initialised first!" );
238 obtainOutputs( 0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
239 nOutputs,plhs,nV,0 );
254 int_t nOutputs, mxArray* plhs[]
257 int_t nWSRout = nWSRin;
258 real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn :
INFTY;
262 if ( globalSQP == 0 )
264 myMexErrMsgTxt(
"ERROR (qpOASES): QP needs to be initialised first!" );
276 obtainOutputs( 0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
277 nOutputs,plhs,nV,nC );
291 int_t nOutputs, mxArray* plhs[]
294 int_t nWSRout = nWSRin;
295 real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn :
INFTY;
299 if ( globalSQP == 0 )
301 myMexErrMsgTxt(
"ERROR (qpOASES): QP needs to be initialised first!" );
310 returnValue returnvalue = globalSQP->
hotstart( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
325 obtainOutputs( 0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
326 nOutputs,plhs,nV,nC );
336 void mexFunction(
int nlhs, mxArray* plhs[],
int nrhs,
const mxArray* prhs[] )
343 double *x0=0, *
R=0, *R_for=0;
344 double *guessedBounds=0, *guessedConstraints=0;
346 int_t H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
347 int_t x0_idx=-1, auxInput_idx=-1;
356 #ifdef __SUPPRESSANYOUTPUT__ 361 uint_t nV=0, nC=0, handle=0;
363 real_t maxCpuTimeIn = -1.0;
368 if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
372 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
378 if ( mxIsChar( prhs[0] ) != 1 )
380 myMexErrMsgTxt(
"ERROR (qpOASES): First input argument must be a string!" );
384 mxGetString( prhs[0], typeString, 2 );
387 if ( ( strcmp( typeString,
"i" ) != 0 ) && ( strcmp( typeString,
"I" ) != 0 ) &&
388 ( strcmp( typeString,
"h" ) != 0 ) && ( strcmp( typeString,
"H" ) != 0 ) &&
389 ( strcmp( typeString,
"m" ) != 0 ) && ( strcmp( typeString,
"M" ) != 0 ) &&
390 ( strcmp( typeString,
"e" ) != 0 ) && ( strcmp( typeString,
"E" ) != 0 ) &&
391 ( strcmp( typeString,
"c" ) != 0 ) && ( strcmp( typeString,
"C" ) != 0 ) )
393 myMexErrMsgTxt(
"ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
400 if ( ( strcmp( typeString,
"i" ) == 0 ) || ( strcmp( typeString,
"I" ) == 0 ) )
403 if ( ( nlhs < 1 ) || ( nlhs > 7 ) )
405 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
409 if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
411 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
417 if ( mxIsEmpty(prhs[1]) == 1 )
420 nV = (
uint_t)mxGetM( prhs[ g_idx ] );
425 nV = (
uint_t)mxGetM( prhs[ H_idx ] );
430 if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ) ||
431 ( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
433 myMexErrMsgTxt(
"ERROR (qpOASES): All data has to be provided in double precision!" );
437 if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
439 myMexErrMsgTxt(
"ERROR (qpOASES): Hessian matrix dimension mismatch!" );
458 if ( isSimplyBoundedQp ==
BT_TRUE )
488 if ((!mxIsEmpty(prhs[5])) && (mxIsStruct(prhs[5])))
491 if ( ( nrhs >= 7 ) && ( !mxIsEmpty(prhs[6]) ) )
494 if ( mxIsStruct(prhs[6]) )
517 if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
519 myMexErrMsgTxt(
"ERROR (qpOASES): All data has to be provided in double precision!" );
524 nC = (
uint_t)mxGetM( prhs[ A_idx ] );
546 if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
548 myMexErrMsgTxt(
"ERROR (qpOASES): Constraint matrix dimension mismatch!" );
573 if ((!mxIsEmpty(prhs[8])) && (mxIsStruct(prhs[8])))
576 if ( ( nrhs >= 10 ) && ( !mxIsEmpty(prhs[9]) ) )
579 if ( mxIsStruct(prhs[9]) )
603 if ( auxInput_idx >= 0 )
604 setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &hessianType,&x0,&guessedBounds,&guessedConstraints,&R_for );
622 if ( ( nC > 0 ) && ( A_idx >= 0 ) )
629 if ( isSimplyBoundedQp ==
BT_TRUE )
643 globalQP->
H,g,globalQP->
A,
648 guessedBounds,guessedConstraints,
R 652 if (
R != 0)
delete R;
657 if ( ( strcmp( typeString,
"h" ) == 0 ) || ( strcmp( typeString,
"H" ) == 0 ) )
660 if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
662 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
666 if ( ( nrhs < 5 ) || ( nrhs > 8 ) )
668 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
679 if ( ( mxIsDouble( prhs[1] ) ==
false ) || (
mxIsScalar( prhs[1] ) ==
false ) )
681 myMexErrMsgTxt(
"ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
686 handle = (
uint_t)mxGetScalar( prhs[1] );
690 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid handle to QP instance!" );
694 nV = globalQP->
getNV();
711 if ( isSimplyBoundedQp ==
BT_TRUE )
729 if ( ( !mxIsEmpty( prhs[5] ) ) && ( mxIsStruct( prhs[5] ) ) )
734 nC = globalQP->
getNC( );
765 if ( ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
773 if ( isSimplyBoundedQp ==
BT_TRUE )
796 if ( ( strcmp( typeString,
"m" ) == 0 ) || ( strcmp( typeString,
"M" ) == 0 ) )
799 if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
801 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
805 if ( ( nrhs < 9 ) || ( nrhs > 10 ) )
807 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
811 if ( ( mxIsDouble( prhs[1] ) ==
false ) || (
mxIsScalar( prhs[1] ) ==
false ) )
813 myMexErrMsgTxt(
"ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
819 handle = (
uint_t)mxGetScalar( prhs[1] );
823 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid handle to QP instance!" );
830 if ( mxIsEmpty(prhs[2]) == 1 )
833 nV = (
uint_t)mxGetM( prhs[ g_idx ] );
838 nV = (
uint_t)mxGetM( prhs[ H_idx ] );
842 nC = (
uint_t)mxGetM( prhs[ A_idx ] );
851 if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[H_idx] ) == 0 ) ) ||
852 ( mxIsDouble( prhs[g_idx] ) == 0 ) ||
853 ( mxIsDouble( prhs[A_idx] ) == 0 ) )
855 myMexErrMsgTxt(
"ERROR (qpOASES): All data has to be provided in real_t precision!" );
884 myMexErrMsgTxt(
"ERROR (qpOASES): QP dimensions must be constant during a sequence! Try creating a new QP instance instead." );
888 if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
890 myMexErrMsgTxt(
"ERROR (qpOASES): Hessian matrix dimension mismatch!" );
894 if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
896 myMexErrMsgTxt(
"ERROR (qpOASES): Constraint matrix dimension mismatch!" );
920 if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
930 if ( ( nC > 0 ) && ( A_idx >= 0 ) )
948 if ( ( strcmp( typeString,
"e" ) == 0 ) || ( strcmp( typeString,
"E" ) == 0 ) )
951 if ( ( nlhs < 1 ) || ( nlhs > 4 ) )
953 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
957 if ( ( nrhs < 7 ) || ( nrhs > 8 ) )
959 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
963 if ( ( mxIsDouble( prhs[1] ) ==
false ) || (
mxIsScalar( prhs[1] ) ==
false ) )
965 myMexErrMsgTxt(
"ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
970 handle = (
uint_t)mxGetScalar( prhs[1] );
974 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid handle to QP instance!" );
980 nV = globalQP->
getNV( );
981 nC = globalQP->
getNC( );
1022 if ( ( nrhs == 8 ) && ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
1030 plhs[0] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
1031 x_out = mxGetPr(plhs[0]);
1034 plhs[1] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
1035 y_out = mxGetPr(plhs[1]);
1039 plhs[2] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
1040 real_t* workingSetB = mxGetPr(plhs[2]);
1045 plhs[3] = mxCreateDoubleMatrix( nC, nRHS, mxREAL );
1046 real_t* workingSetC = mxGetPr(plhs[3]);
1052 y_out =
new real_t[nV+nC];
1063 snprintf(msg,
MAX_STRING_LENGTH,
"ERROR (qpOASES): Couldn't solve current EQP (code %d)!", returnvalue);
1072 if ( ( strcmp( typeString,
"c" ) == 0 ) || ( strcmp( typeString,
"C" ) == 0 ) )
1077 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
1083 myMexErrMsgTxt(
"ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
1087 if ( ( mxIsDouble( prhs[1] ) ==
false ) || (
mxIsScalar( prhs[1] ) ==
false ) )
1089 myMexErrMsgTxt(
"ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
1094 handle = (
uint_t)mxGetScalar( prhs[1] );
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[])
#define USING_NAMESPACE_QPOASES
returnValue solveCurrentEQP(const int n_rhs, const real_t *g_in, const real_t *lb_in, const real_t *ub_in, const real_t *lbA_in, const real_t *ubA_in, real_t *x_out, real_t *y_out)
returnValue init(const real_t *const _H, const real_t *const _g, const real_t *const _A, const real_t *const _lb, const real_t *const _ub, const real_t *const _lbA, const real_t *const _ubA, int &nWSR, const real_t *const yOpt=0, real_t *const cputime=0)
BooleanType containsNaNorInf(const mxArray *prhs[], int_t rhs_index, bool mayContainInf)
Manages working sets of constraints.
returnValue setupHessianMatrix(const mxArray *prhsH, int_t nV, SymmetricMatrix **H, sparse_int_t **Hir, sparse_int_t **Hjc, real_t **Hv)
virtual returnValue getWorkingSetBounds(real_t *workingSetB)
Implements the online active set strategy for box-constrained QPs.
returnValue deleteQPMatrices()
Implements the online active set strategy for QPs with varying matrices.
bool mxIsScalar(const mxArray *pm)
returnValue setupOptions(Options *options, const mxArray *optionsPtr, int &nWSRin)
Allows to pass back messages to the calling function.
returnValue smartDimensionCheck(real_t **input, unsigned int m, unsigned int n, BooleanType emptyAllowed, const mxArray *prhs[], int idx)
returnValue init(const real_t *const _H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub, int &nWSR, const real_t *const yOpt=0, real_t *const cputime=0)
returnValue hotstart(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int &nWSR, real_t *const cputime)
returnValue convertFortranToC(const real_t *const A_for, int nV, int nC, real_t *const A)
virtual returnValue getWorkingSetConstraints(real_t *workingSetC)
static QProblemB * globalQPB
returnValue setupBound(int _number, SubjectToStatus _status)
returnValue setOptions(const Options &_options)
static std::vector< QPInstance * > g_instances
returnValue hotstart(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, int &nWSR, real_t *const cputime)
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
void allocateOutputs(int nlhs, mxArray *plhs[], int nV, int nC=0, int nP=1)
static SQProblem * globalSQP
returnValue setupAuxiliaryInputs(const mxArray *auxInput, uint_t nV, uint_t nC, HessianType *hessianType, double **x0, double **guessedBounds, double **guessedConstraints, double **R)
static QProblem * globalQP
returnValue setupConstraint(int _number, SubjectToStatus _status)
Provides a generic way to set and pass user-specified options.
void deleteQPInstance(int_t handle)
int_t allocateQPInstance(int_t nV, int_t nC, HessianType hessianType, BooleanType isSimplyBounded, const Options *options)
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)
Abstract base class for interfacing tailored matrix-vector operations.
int_t SQProblem_hotstart(int_t handle, SymmetricMatrix *H, real_t *g, Matrix *A, 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 SQProblem_init(int_t handle, SymmetricMatrix *H, real_t *g, Matrix *A, 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, const double *const x0, Options *options, int_t nOutputs, mxArray *plhs[], const double *const guessedBounds, const double *const guessedConstraints, const double *const _R)
void obtainOutputs(int k, QProblemB *qp, returnValue returnvalue, int nWSRin, int nlhs, mxArray *plhs[], int nV, int nC=0)
const uint_t MAX_STRING_LENGTH
returnValue setupConstraintMatrix(const mxArray *prhsA, int_t nV, int_t nC, Matrix **A, sparse_int_t **Air, sparse_int_t **Ajc, real_t **Av)
static int_t s_nexthandle
Manages working sets of bounds (= box constraints).
Implements the online active set strategy for QPs with general constraints.
QPInstance * getQPInstance(int_t handle)
BooleanType isEqual(real_t x, real_t y, real_t TOL=ZERO)
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[])
Abstract base class for interfacing matrix-vector operations tailored to symmetric matrices...
returnValue hotstart(const real_t *const H_new, const real_t *const g_new, const real_t *const A_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int &nWSR, real_t *const cputime)