Ejemplo n.º 1
0
void QPInstanceCON(	QPInstance* _THIS,
					int _nV,
					int _nC,
					HessianType _hessianType,
					BooleanType _isSimplyBounded
					)
{
	if ( _nV > NVMAX )
	{
		myMexErrMsgTxt( "ERROR (qpOASES_e): Too many primal variables (try increasing QPOASES_NVMAX)!" );
		return;
	}

	if ( _nC > NCMAX )
	{
		myMexErrMsgTxt( "ERROR (qpOASES_e): Too many constraints (try increasing QPOASES_NCMAX)!" );
		return;
	}

	_THIS->handle = QPInstance_nexthandle;
	
	if ( _nC > 0 )
		_THIS->isSimplyBounded = BT_FALSE;
	else
		_THIS->isSimplyBounded = _isSimplyBounded;
	
	if ( _THIS->isSimplyBounded == BT_FALSE )
		QProblemCON( &(_THIS->sqp),_nV,_nC,_hessianType );
	else
		QProblemBCON( &(_THIS->qpb),_nV,_hessianType );
}	
Ejemplo n.º 2
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;
}
Ejemplo n.º 3
0
static void mdlStart(SimStruct *S)
{
	USING_NAMESPACE_QPOASES

	int nU = NCONTROLINPUTS;
	int size_g, size_lb, size_ub;
	int size_H, nRows_H, nCols_H;
	int nV;

	static QProblemB problem;
	static Options problemOptions;
	real_t* count;


	/* get block inputs dimensions */
	const mxArray* in_H = ssGetSFcnParam(S, 0);

	if ( mxIsEmpty(in_H) == 1 )
	{
		if ( ( HESSIANTYPE != HST_ZERO ) && ( HESSIANTYPE != HST_IDENTITY ) )
		{
			#ifndef __SUPPRESSANYOUTPUT__
			mexErrMsgTxt( "ERROR (qpOASES): Hessian can only be empty if type is set to HST_ZERO or HST_IDENTITY!" );
			#endif
			return;
		}
		
	    nRows_H = 0;
		nCols_H = 0;
		size_H  = 0;
	}
	else
	{
	    nRows_H = (int)mxGetM(in_H);
		nCols_H = (int)mxGetN(in_H);
		size_H  = nRows_H * nCols_H;
	}

	size_g   = ssGetInputPortWidth(S, 0);
	size_lb  = ssGetInputPortWidth(S, 1);
	size_ub  = ssGetInputPortWidth(S, 2);


	/* dimension checks */
	nV = size_g;

	if ( MAXITER < 0 )
	{
		#ifndef __SUPPRESSANYOUTPUT__
		mexErrMsgTxt( "ERROR (qpOASES): Maximum number of iterations must not be negative!" );
		#endif
		return;
	}

	if ( nV <= 0 )
	{
		#ifndef __SUPPRESSANYOUTPUT__
		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
		#endif
		return;
	}

	if ( ( size_H != nV*nV ) && ( size_H != 0 ) )
	{
		#ifndef __SUPPRESSANYOUTPUT__
		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch in H!" );
		#endif
		return;
	}

	if ( nRows_H != nCols_H )
	{
		#ifndef __SUPPRESSANYOUTPUT__
		mexErrMsgTxt( "ERROR (qpOASES): Hessian matrix must be square matrix!" );
		#endif
		return;
	}

	if ( ( nU < 1 ) || ( nU > nV ) )
	{
		#ifndef __SUPPRESSANYOUTPUT__
		mexErrMsgTxt( "ERROR (qpOASES): Invalid number of control inputs!" );
		#endif
		return;
	}

	if ( ( size_lb != nV ) && ( size_lb != 0 ) )
	{
		#ifndef __SUPPRESSANYOUTPUT__
		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch in lb!" );
		#endif
		return;
	}

	if ( ( size_ub != nV ) && ( size_ub != 0 ) )
	{
		#ifndef __SUPPRESSANYOUTPUT__
		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch in ub!" );
		#endif
		return;
	}


	/* allocate QProblemB object */
	QProblemBCON( &problem,nV,HESSIANTYPE );

	Options_setToMPC( &problemOptions );
	QProblemB_setOptions( &problem,problemOptions );
	#ifndef __DEBUG__
	QProblemB_setPrintLevel( &problem,PL_LOW );
	#endif
	#ifdef __SUPPRESSANYOUTPUT__
	QProblemB_setPrintLevel( &problem,PL_NONE );
	#endif

	ssGetPWork(S)[0] = (void*)(&problem);

	/* allocate memory for QP data ... */
	if ( size_H > 0 )
		ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) );	/* H */
	else
		ssGetPWork(S)[1] = 0;

	ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) );		/* g */

	if ( size_lb > 0 )
		ssGetPWork(S)[3] = (void *) calloc( size_lb, sizeof(real_t) );	/* lb */
	else
		ssGetPWork(S)[3] = 0;

	if ( size_ub > 0 )
		ssGetPWork(S)[4] = (void *) calloc( size_ub, sizeof(real_t) );	/* ub */
	else
		ssGetPWork(S)[4] = 0;
}
Ejemplo n.º 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;
}