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; }
/* * 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; }