/* * 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 mdlOutputs(SimStruct *S, int_T tid) { #ifndef __DSPACE__ using namespace qpOASES; #endif int i; int nV, nC, status; int nWSR = NWSR; int nU = NCONTROLINPUTS; InputRealPtrsType in_H, in_g, in_A, in_lb, in_ub, in_lbA, in_ubA; SQProblem* problem; real_t *H, *g, *A, *lb, *ub, *lbA, *ubA, *count; real_t *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_A = ssGetInputPortRealSignalPtrs(S, 2); in_lb = ssGetInputPortRealSignalPtrs(S, 3); in_ub = ssGetInputPortRealSignalPtrs(S, 4); in_lbA = ssGetInputPortRealSignalPtrs(S, 5); in_ubA = ssGetInputPortRealSignalPtrs(S, 6); /* ... and to the QP data */ problem = (SQProblem *) ssGetPWork(S)[0]; H = (real_t *) ssGetPWork(S)[1]; g = (real_t *) ssGetPWork(S)[2]; A = (real_t *) ssGetPWork(S)[3]; lb = (real_t *) ssGetPWork(S)[4]; ub = (real_t *) ssGetPWork(S)[5]; lbA = (real_t *) ssGetPWork(S)[6]; ubA = (real_t *) ssGetPWork(S)[7]; count = (real_t *) ssGetPWork(S)[8]; /* setup QP data */ nV = ssGetInputPortWidth(S, 1); /* nV = size_g */ nC = (int) ( ((real_t) ssGetInputPortWidth(S, 2)) / ((real_t) nV) ); /* nC = size_A / size_g */ for ( i=0; i<nV*nV; ++i ) H[i] = (*in_H)[i]; for ( i=0; i<nC*nV; ++i ) A[i] = (*in_A)[i]; for ( i=0; i<nV; ++i ) { g[i] = (*in_g)[i]; lb[i] = (*in_lb)[i]; ub[i] = (*in_ub)[i]; } for ( i=0; i<nC; ++i ) { lbA[i] = (*in_lbA)[i]; ubA[i] = (*in_ubA)[i]; } xOpt = new real_t[nV]; if ( count[0] == 0 ) { /* initialise and solve first QP */ status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 ); ssGetPWork(S)[0] = ( void* ) problem; problem->getPrimalSolution( xOpt ); } else { /* solve neighbouring QP using hotstart technique */ status = problem->hotstart( H,g,A,lb,ub,lbA,ubA, 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,A,lb,ub,lbA,ubA, nWSR,0 ); } else { /* otherwise obtain optimal solution */ 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; case RET_INIT_FAILED_INFEASIBILITY: case RET_HOTSTART_STOPPED_INFEASIBILITY: out_status[0] = -2.0; break; case RET_INIT_FAILED_UNBOUNDEDNESS: case RET_HOTSTART_STOPPED_UNBOUNDEDNESS: out_status[0] = -3.0; break; default: out_status[0] = -1.0; break; } out_nWSR[0] = ((real_T) nWSR); /* increase counter */ count[0] = count[0] + 1; delete[] xOpt; }