/* * S Q P r o b l e m _ h o t s t a r t */ 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 nWSRout = nWSRin; real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY; SQProblem* 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. */ globalSQP->setOptions( *options ); returnValue returnvalue = globalSQP->hotstart( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut ); switch (returnvalue) { case SUCCESSFUL_RETURN: case RET_QP_UNBOUNDED: case RET_QP_INFEASIBLE: break; default: myMexErrMsgTxt( "ERROR (qpOASES): Hotstart failed." ); return -1; } /* 2) Assign lhs arguments. */ obtainOutputs( 0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut, nOutputs,plhs,nV,nC ); return 0; }
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; SQProblem* problem; real_t* 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) ( ((real_t) size_A) / ((real_t) nV) ); if ( nV == 0 ) { #ifndef __DSPACE__ #ifndef __XPCTARGET__ mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" ); #endif #endif return; } if ( size_H != nV*nV ) { #ifndef __DSPACE__ #ifndef __XPCTARGET__ mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" ); #endif #endif return; } if ( nU > nV ) { #ifndef __DSPACE__ #ifndef __XPCTARGET__ mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" ); #endif #endif return; } if ( size_lb != nV ) { #ifndef __DSPACE__ #ifndef __XPCTARGET__ mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" ); #endif #endif return; } if ( size_ub != nV ) { #ifndef __DSPACE__ #ifndef __XPCTARGET__ mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" ); #endif #endif return; } if ( size_lbA != nC ) { #ifndef __DSPACE__ #ifndef __XPCTARGET__ mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" ); #endif #endif return; } if ( size_ubA != nC ) { #ifndef __DSPACE__ #ifndef __XPCTARGET__ mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" ); #endif #endif return; } /* allocate QProblem object */ problem = new SQProblem( nV,nC ); if ( problem == 0 ) { #ifndef __DSPACE__ #ifndef __XPCTARGET__ mexErrMsgTxt( "ERROR (qpOASES): Unable to create QProblem 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 ... */ ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) ); /* H */ ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) ); /* g */ ssGetPWork(S)[3] = (void *) calloc( size_A, sizeof(real_t) ); /* A */ ssGetPWork(S)[4] = (void *) calloc( size_lb, sizeof(real_t) ); /* lb */ ssGetPWork(S)[5] = (void *) calloc( size_ub, sizeof(real_t) ); /* ub */ ssGetPWork(S)[6] = (void *) calloc( size_lbA, sizeof(real_t) ); /* lbA */ ssGetPWork(S)[7] = (void *) calloc( size_ubA, sizeof(real_t) ); /* ubA */ ssGetPWork(S)[8] = (void *) calloc( 1, sizeof(real_t) ); /* count */ /* reset counter */ count = (real_t *) ssGetPWork(S)[8]; count[0] = 0.0; }
/* * S Q P r o b l e m _ i n i t */ 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 ) { int_t nWSRout = nWSRin; real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY; /* 1) setup initial QP. */ SQProblem* globalSQP = getQPInstance(handle)->sqp; if ( globalSQP == 0 ) { myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" ); return -1; } globalSQP->setOptions( *options ); /* 2) Solve initial QP. */ returnValue returnvalue; int_t nV = globalSQP->getNV(); int_t nC = globalSQP->getNC(); /* 3) Fill the working set. */ Bounds bounds(nV); Constraints constraints(nC); 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; } } } if (guessedConstraints != 0) { for (int_t i = 0; i < nC; i++) { if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) { constraints.setupConstraint(i, ST_LOWER); } else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) { constraints.setupConstraint(i, ST_UPPER); } else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) { constraints.setupConstraint(i, ST_INACTIVE); } else { char msg[MAX_STRING_LENGTH]; snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!"); myMexErrMsgTxt(msg); return -1; } } } returnvalue = globalSQP->init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut, x0,0, (guessedBounds != 0) ? &bounds : 0, (guessedConstraints != 0) ? &constraints : 0, _R ); /* 3) Assign lhs arguments. */ obtainOutputs( 0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut, nOutputs,plhs,nV,nC,handle ); return 0; }