Beispiel #1
0
/** Example for qpOASES main function using the QProblemB class. */
int main( )
{
	USING_NAMESPACE_QPOASES

	/* Setup data of first QP. */
	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
	real_t g[2] = { 1.5, 1.0 };
	real_t lb[2] = { 0.5, -2.0 };
	real_t ub[2] = { 5.0, 2.0 };

	/* Setup data of second QP. */
	real_t g_new[2] = { 1.0, 1.5 };
	real_t lb_new[2] = { 0.0, -1.0 };
	real_t ub_new[2] = { 5.0, -0.5 };


	/* Setting up QProblemB object. */
	static QProblemB example;
	static Options options;

	int nWSR = 10;
	real_t xOpt[2];

	QProblemBCON( &example,2,HST_UNKNOWN );
	Options_setToDefault( &options );
	/* options.enableFlippingBounds = BT_FALSE; */
	options.initialStatusBounds = ST_INACTIVE;
	options.numRefinementSteps = 1;
	/* options.enableCholeskyRefactorisation = 1; */
	QProblemB_setOptions( &example,options );


	/* Solve first QP. */
	nWSR = 10;
	QProblemB_init( &example,H,g,lb,ub, &nWSR,0 );

	/* Get and print solution of second QP. */
	QProblemB_getPrimalSolution( &example,xOpt );
	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],QProblemB_getObjVal(&example) );
	
	/* Solve second QP. */
	nWSR = 10;
	QProblemB_hotstart( &example,g_new,lb_new,ub_new, &nWSR,0 );

	/* Get and print solution of second QP. */
	QProblemB_getPrimalSolution( &example,xOpt );
	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],QProblemB_getObjVal(&example) );

	return 0;
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
	USING_NAMESPACE_QPOASES

	int i;
	int nV;
	returnValue status;

	int nWSR = MAXITER;
	int nU   = NCONTROLINPUTS;

	InputRealPtrsType in_g, in_lb, in_ub;

	QProblemB* problem;
	real_t *H, *g, *lb, *ub;

	real_t xOpt[NVMAX];

	real_T *out_uOpt, *out_objVal, *out_status, *out_nWSR;

	int nWSR_retry;


	/* get pointers to block inputs ... */
	const mxArray* in_H = ssGetSFcnParam(S, 0);
	in_g  = ssGetInputPortRealSignalPtrs(S, 0);
	in_lb = ssGetInputPortRealSignalPtrs(S, 1);
	in_ub = ssGetInputPortRealSignalPtrs(S, 2);

	/* ... and to the QP data */
	problem = (QProblemB*)(ssGetPWork(S)[0]);

	H  = (real_t*)(ssGetPWork(S)[1]);
	g  = (real_t*)(ssGetPWork(S)[2]);
	lb = (real_t*)(ssGetPWork(S)[3]);
	ub = (real_t*)(ssGetPWork(S)[4]);


	/* setup QP data */
	nV = ssGetInputPortWidth(S, 1); /* nV = size_g */

	if ( H != 0 )
	{
		/* no conversion from FORTRAN to C as Hessian is symmetric! */
		for ( i=0; i<nV*nV; ++i )
			H[i] = (mxGetPr(in_H))[i];
	}

	for ( i=0; i<nV; ++i )
		g[i] = (*in_g)[i];

	if ( lb != 0 )
	{
		for ( i=0; i<nV; ++i )
			lb[i] = (*in_lb)[i];
	}

	if ( ub != 0 )
	{
		for ( i=0; i<nV; ++i )
			ub[i] = (*in_ub)[i];
	}

	if ( QProblemB_getCount( problem ) == 0 )
	{
		/* initialise and solve first QP */
		status = QProblemB_init( problem,H,g,lb,ub, &nWSR,0 );
		QProblemB_getPrimalSolution( problem,xOpt );
	}
	else
	{
		/* solve neighbouring QP using hotstart technique */
		status = QProblemB_hotstart( problem,g,lb,ub, &nWSR,0 );
		if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
		{
			/* if an error occurs, reset problem data structures ... */
			QProblemB_reset( problem );
            
            /* ... and initialise/solve again with remaining number of iterations. */
            nWSR_retry = MAXITER - nWSR;
			status = QProblemB_init( problem,H,g,lb,ub, &nWSR_retry,0 );
            nWSR += nWSR_retry;
		}

		/* obtain optimal solution */
		QProblemB_getPrimalSolution( problem,xOpt );
	}

	/* generate block output: status information ... */
	out_uOpt   = ssGetOutputPortRealSignal(S, 0);
	out_objVal = ssGetOutputPortRealSignal(S, 1);
	out_status = ssGetOutputPortRealSignal(S, 2);
	out_nWSR   = ssGetOutputPortRealSignal(S, 3);

	for ( i=0; i<nU; ++i )
		out_uOpt[i] = (real_T)(xOpt[i]);

	out_objVal[0] = (real_T)(QProblemB_getObjVal( problem ));
	out_status[0] = (real_t)(qpOASES_getSimpleStatus( status,BT_FALSE ));
	out_nWSR[0]   = (real_T)(nWSR);

	removeNaNs( out_uOpt,nU );
	removeInfs( out_uOpt,nU );
	removeNaNs( out_objVal,1 );
	removeInfs( out_objVal,1 );
}
/*
 *	o b t a i n O u t p u t s S B
 */
returnValue obtainOutputsSB(	int k, QProblemB* qp, returnValue returnvalue, int _nWSRout, double _cpuTime,
								int nlhs, mxArray* plhs[], int nV, int handle
								)
{
	double *x=0, *obj=0, *status=0, *nWSRout=0, *y=0, *workingSetB=0, *workingSetC=0, *cpuTime;
	mxArray *auxOutput=0, *curField=0;

	/* Create output vectors and assign pointers to them. */
	int curIdx = 0;

	/* handle */
	if ( handle >= 0 )
		plhs[curIdx++] = mxCreateDoubleScalar( handle );

	/* x */
	x = mxGetPr( plhs[curIdx++] );
	QProblemB_getPrimalSolution( qp,&(x[k*nV]) );

	if ( nlhs > curIdx )
	{
		/* fval */
		obj = mxGetPr( plhs[curIdx++] );
		obj[k] = QProblemB_getObjVal( qp );

		if ( nlhs > curIdx )
		{
			/* exitflag */
			status = mxGetPr( plhs[curIdx++] );
			status[k] = (double)qpOASES_getSimpleStatus( returnvalue,0 );

			if ( nlhs > curIdx )
			{
				/* iter */
				nWSRout = mxGetPr( plhs[curIdx++] );
				nWSRout[k] = (double) _nWSRout;

				if ( nlhs > curIdx )
				{
					/* lambda */
					y = mxGetPr( plhs[curIdx++] );
					QProblemB_getDualSolution( qp,&(y[k*nV]) );

					/* auxOutput */
					if ( nlhs > curIdx )
					{
						auxOutput = plhs[curIdx];
						curField = 0;

						/* working set bounds */
						if ( nV > 0 )
						{
							curField = mxGetField( auxOutput,0,"workingSetB" );
							workingSetB = mxGetPr(curField);
							QProblemB_getWorkingSetBounds( qp,&(workingSetB[k*nV]) );
						}

						/* cpu time */
						curField = mxGetField( auxOutput,0,"cpuTime" );
						cpuTime = mxGetPr(curField);
						cpuTime[0] = (double) _cpuTime;
					}
				}
			}
		}
	}
	
	return SUCCESSFUL_RETURN;
}
Beispiel #4
0
/*
 *	s o l v e O Q P b e n c h m a r k
 */
returnValue solveOQPbenchmarkB(	int nQP, int nV,
								real_t* _H, const real_t* const g,
								const real_t* const lb, const real_t* const ub,
								BooleanType isSparse, BooleanType useHotstarts, 
								const Options* options, int maxAllowedNWSR,
								real_t* maxNWSR, real_t* avgNWSR, real_t* maxCPUtime, real_t* avgCPUtime,
								real_t* maxStationarity, real_t* maxFeasibility, real_t* maxComplementarity
								)
{
	int k;
	
	myStatic QProblemB qp;
	returnValue returnvalue;

	/* I) SETUP AUXILIARY VARIABLES: */
	/* 1) Keep nWSR and store current and maximum number of
	 *    working set recalculations in temporary variables */
	int nWSRcur;

	real_t CPUtimeLimit = *maxCPUtime;
	real_t CPUtimeCur = CPUtimeLimit;
	real_t stat, feas, cmpl;

	/* 2) Pointers to data of current QP ... */
	const real_t* gCur;
	const real_t* lbCur;
	const real_t* ubCur;

	/* 3) Vectors for solution obtained by qpOASES. */
	myStatic real_t x[NVMAX];
	myStatic real_t y[NVMAX];

	/* 4) Prepare matrix objects */
	DenseMatrix *H;
	myStatic DenseMatrix HH;
	
	DenseMatrixCON( &HH, nV, nV, nV, _H );
	H = &HH;
	
	*maxNWSR = 0;
	*avgNWSR = 0;
	*maxCPUtime = 0.0;
	*avgCPUtime = 0.0;
	*maxStationarity = 0.0;
	*maxFeasibility = 0.0;
	*maxComplementarity = 0.0;

	/* II) SETUP QPROBLEM OBJECT */
	QProblemBCON( &qp,nV,HST_UNKNOWN );
	QProblemB_setOptions( &qp,*options );
	/*QProblemB_setPrintLevel( &qp,PL_LOW );*/


	/* III) RUN BENCHMARK SEQUENCE: */
	for( k=0; k<nQP; ++k )
	{
		/* 1) Update pointers to current QP data. */
		gCur   = &( g[k*nV] );
		lbCur  = &( lb[k*nV] );
		ubCur  = &( ub[k*nV] );

		/* 2) Set nWSR and maximum CPU time. */
		nWSRcur = maxAllowedNWSR;
		CPUtimeCur = CPUtimeLimit;

		/* 3) Solve current QP. */
		if ( ( k == 0 ) || ( useHotstarts == BT_FALSE ) )
		{
			/* initialise */
			returnvalue = QProblemB_initM( &qp,H,gCur,lbCur,ubCur, &nWSRcur,&CPUtimeCur );
			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
				return THROWERROR( returnvalue );
		}
		else
		{
			/* hotstart */
			returnvalue = QProblemB_hotstart( &qp,gCur,lbCur,ubCur, &nWSRcur,&CPUtimeCur );
			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
				return THROWERROR( returnvalue );
		}

		/* 4) Obtain solution vectors and objective function value ... */
		QProblemB_getPrimalSolution( &qp,x );
		QProblemB_getDualSolution( &qp,y );

		/* 5) Compute KKT residuals */
		qpOASES_getKktViolationSB( nV, _H,gCur,lbCur,ubCur, x,y, &stat,&feas,&cmpl );

		/* 6) update maximum values. */
		if ( nWSRcur > *maxNWSR )
			*maxNWSR = nWSRcur;
		if (stat > *maxStationarity) *maxStationarity = stat;
		if (feas > *maxFeasibility) *maxFeasibility = feas;
		if (cmpl > *maxComplementarity) *maxComplementarity = cmpl;

		if ( CPUtimeCur > *maxCPUtime )
			*maxCPUtime = CPUtimeCur;
		
		*avgNWSR += nWSRcur;
		*avgCPUtime += CPUtimeCur;
	}
	*avgNWSR /= nQP;
	*avgCPUtime /= ((double)nQP);

	return SUCCESSFUL_RETURN;
}