/* * Q P r o b l e m _ h o t s t a r t */ 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[] ) { int_t nWSRout = nWSRin; real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY; QProblem* 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 with given options. */ globalSQP->setOptions( *options ); returnValue returnvalue = globalSQP->hotstart( g,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut ); /* 2) Assign lhs arguments. */ obtainOutputs( 0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut, nOutputs,plhs,nV,nC ); return 0; }
/* * o b t a i n O u t p u t s */ returnValue obtainOutputs( int_t k, QProblemB* qp, returnValue returnvalue, int_t _nWSRout, double _cpuTime, int nlhs, mxArray* plhs[], int_t nV, int_t nC = 0, int_t handle = -1 ) { /* Create output vectors and assign pointers to them. */ int_t curIdx = 0; /* handle */ if ( handle >= 0 ) plhs[curIdx++] = mxCreateDoubleScalar( handle ); /* x */ double* x = mxGetPr( plhs[curIdx++] ); qp->getPrimalSolution( &(x[k*nV]) ); if ( nlhs > curIdx ) { /* fval */ double* obj = mxGetPr( plhs[curIdx++] ); obj[k] = qp->getObjVal( ); if ( nlhs > curIdx ) { /* exitflag */ double* status = mxGetPr( plhs[curIdx++] ); status[k] = (double)getSimpleStatus( returnvalue ); if ( nlhs > curIdx ) { /* iter */ double* nWSRout = mxGetPr( plhs[curIdx++] ); nWSRout[k] = (double) _nWSRout; if ( nlhs > curIdx ) { /* lambda */ double* y = mxGetPr( plhs[curIdx++] ); qp->getDualSolution( &(y[k*(nV+nC)]) ); /* auxOutput */ if ( nlhs > curIdx ) { QProblem* problemPointer; problemPointer = dynamic_cast<QProblem*>(qp); mxArray* auxOutput = plhs[curIdx]; mxArray* curField = 0; /* working set bounds */ if ( nV > 0 ) { curField = mxGetField( auxOutput,0,"workingSetB" ); double* workingSetB = mxGetPr(curField); /* cast successful? */ if (problemPointer != NULL) { problemPointer->getWorkingSetBounds( &(workingSetB[k*nV]) ); } else { qp->getWorkingSetBounds( &(workingSetB[k*nV]) ); } } /* working set constraints */ if ( nC > 0 ) { curField = mxGetField( auxOutput,0,"workingSetC" ); double* workingSetC = mxGetPr(curField); /* cast successful? */ if (problemPointer != NULL) { problemPointer->getWorkingSetConstraints( &(workingSetC[k*nC]) ); } else { qp->getWorkingSetConstraints( &(workingSetC[k*nC]) ); } } /* cpu time */ curField = mxGetField( auxOutput,0,"cpuTime" ); double* cpuTime = mxGetPr(curField); cpuTime[0] = (double) _cpuTime; } } } } } return SUCCESSFUL_RETURN; }
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; QProblem* problem; double *H, *g, *A, *lb, *ub, *lbA, *ubA, *count; double *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 = (QProblem *) ssGetPWork(S)[0]; H = (double *) ssGetPWork(S)[1]; g = (double *) ssGetPWork(S)[2]; A = (double *) ssGetPWork(S)[3]; lb = (double *) ssGetPWork(S)[4]; ub = (double *) ssGetPWork(S)[5]; lbA = (double *) ssGetPWork(S)[6]; ubA = (double *) ssGetPWork(S)[7]; count = (double *) ssGetPWork(S)[8]; /* setup QP data */ nV = ssGetInputPortWidth(S, 1); /* nV = size_g */ nC = (int) ( ((double) ssGetInputPortWidth(S, 2)) / ((double) 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 double[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; status = problem->getPrimalSolution( xOpt ); } else { problem->reset( ); problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 ); /* solve neighbouring QP using hotstart technique */ //status = problem->hotstart( g,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 */ status = 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<nV; ++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; default: out_status[0] = -1.0; break; } out_nWSR[0] = ((real_T) nWSR); /* increase counter */ count[0] = count[0] + 1; delete[] xOpt; }
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; QProblem* problem; double* 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) ( ((double) size_A) / ((double) nV) ); if ( nV == 0 ) { #ifndef __DSPACE__ printf( "ERROR (qpOASES): Dimension mismatch!" ); #endif return; } if ( size_H != nV*nV ) { #ifndef __DSPACE__ printf( "ERROR (qpOASES): Dimension mismatch!" ); #endif return; } //if ( nU > nV ) //{ // #ifndef __DSPACE__ // printf( "ERROR (qpOASES): Dimension mismatch!" ); // #endif // return; // } if ( size_lb != nV ) { #ifndef __DSPACE__ printf( "ERROR (qpOASES): Dimension mismatch!" ); #endif return; } if ( size_ub != nV ) { #ifndef __DSPACE__ printf( "ERROR (qpOASES): Dimension mismatch!" ); #endif return; } if ( size_lbA != nC ) { #ifndef __DSPACE__ printf( "ERROR (qpOASES): Dimension mismatch!" ); #endif return; } if ( size_ubA != nC ) { #ifndef __DSPACE__ printf( "ERROR (qpOASES): Dimension mismatch!" ); #endif return; } /* allocate QProblem object */ problem = new QProblem( nV,nC ); if ( problem == 0 ) { #ifndef __DSPACE__ printf( "ERROR (qpOASES): Unable to create QProblem object!" ); #endif return; } #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(double) ); /* H */ ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(double) ); /* g */ ssGetPWork(S)[3] = (void *) calloc( size_A, sizeof(double) ); /* A */ ssGetPWork(S)[4] = (void *) calloc( size_lb, sizeof(double) ); /* lb */ ssGetPWork(S)[5] = (void *) calloc( size_ub, sizeof(double) ); /* ub */ ssGetPWork(S)[6] = (void *) calloc( size_lbA, sizeof(double) ); /* lbA */ ssGetPWork(S)[7] = (void *) calloc( size_ubA, sizeof(double) ); /* ubA */ ssGetPWork(S)[8] = (void *) calloc( 1, sizeof(double) ); /* count */ /* reset counter */ count = (double *) ssGetPWork(S)[8]; count[0] = 0.0; }