Beispiel #1
0
/** Example for qpOASES main function using the QProblem class. */
int main( )
{
	/* 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 };

	int_t nWSR;
	qpOASES_Options options;

	real_t xOpt[2];
	real_t yOpt[2];
	real_t obj;
	int_t status;

	qpOASES_Options_init( &options,0 );
	/*options.enableFlippingBounds = 0; */
	options.initialStatusBounds = ST_INACTIVE;
	options.numRefinementSteps = 1;
	options.enableCholeskyRefactorisation = 1;


	QProblemB_setup( 2,HST_UNKNOWN );

	/* Solve first QP. */
	nWSR = 10;
	QProblemB_init(	H,g,lb,ub,
					&nWSR,0,&options,
					xOpt,yOpt,&obj,&status
					);

	/* Print solution of first QP. */	
	printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e ];  objVal = %e\n\n", 
			xOpt[0],xOpt[1],yOpt[0],yOpt[1], obj );


	/* Solve second QP. */
	nWSR = 10;
	QProblemB_hotstart(	g_new,lb_new,ub_new,
						&nWSR,0,
						xOpt,yOpt,&obj,&status
						);

	/* Print solution of first QP. */	
	printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e ];  objVal = %e\n\n", 
			xOpt[0],xOpt[1],yOpt[0],yOpt[1], obj );

	
	QProblemB_cleanup();
	
	return 0;
}
Beispiel #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;
}
/*
 *	m e x F u n c t i o n
 */
void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
{
	/* inputs */
	char typeString[2];

	real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0;
	HessianType hessianType = HST_UNKNOWN;
	double *x0=0, *R=0, *R_for=0;
	double *guessedBounds=0, *guessedConstraints=0;

	int_t H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
	int_t x0_idx=-1, auxInput_idx=-1;

	BooleanType isSimplyBoundedQp = BT_FALSE;
	#ifdef SOLVER_MA57
	BooleanType isSparse = BT_TRUE; /* This will be set to BT_FALSE later if a dense matrix is encountered. */
	#else
	BooleanType isSparse = BT_FALSE;
	#endif

	Options options;
	options.printLevel = PL_LOW;
	#ifdef __DEBUG__
	options.printLevel = PL_HIGH;
	#endif
	#ifdef __SUPPRESSANYOUTPUT__
	options.printLevel = PL_NONE;
	#endif

	/* dimensions */
	uint_t nV=0, nC=0, handle=0;
	int_t nWSRin;
	real_t maxCpuTimeIn = -1.0;
	QPInstance* globalQP = 0;

	/* I) CONSISTENCY CHECKS: */
	/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
	if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
	{
		if ( nrhs != 2 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}
	}
	
	/* 2) Ensure that first input is a string ... */
	if ( mxIsChar( prhs[0] ) != 1 )
	{
		myMexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
		return;
	}

	mxGetString( prhs[0], typeString, 2 );

	/*    ... and if so, check if it is an allowed one. */
	if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
		 ( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
		 ( strcmp( typeString,"m" ) != 0 ) && ( strcmp( typeString,"M" ) != 0 ) &&
		 ( strcmp( typeString,"e" ) != 0 ) && ( strcmp( typeString,"E" ) != 0 ) &&
		 ( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
	{
		myMexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
		return;
	}


	/* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
	/* 1) Init (without or with initial guess for primal solution). */
	if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
	{
		/* consistency checks */
		if ( ( nlhs < 1 ) || ( nlhs > 7 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		g_idx = 2;

		if ( mxIsEmpty(prhs[1]) == 1 )
		{
			H_idx = -1;
			nV = (uint_t)mxGetM( prhs[ g_idx ] ); /* row number of Hessian matrix */
		}
		else
		{
			H_idx = 1;
			nV = (uint_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
		}


		/* ensure that data is given in double precision */
		if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ) ||
		     ( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
			return;
		}

		if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Hessian matrix dimension mismatch!" );
			return;
		}


		/* Check for 'Inf' and 'Nan' in Hessian */
		if (containsNaNorInf( prhs,H_idx, 0 ) == BT_TRUE)
			return;

		/* Check for 'Inf' and 'Nan' in gradient */
		if (containsNaNorInf(prhs,g_idx, 0 ) == BT_TRUE)
			return;

		/* determine whether is it a simply bounded QP */
		if ( nrhs <= 7 )
			isSimplyBoundedQp = BT_TRUE;
		else
			isSimplyBoundedQp = BT_FALSE;

		if ( isSimplyBoundedQp == BT_TRUE )
		{
			lb_idx = 3;
			ub_idx = 4;

			if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
				return;

			/* Check inputs dimensions and assign pointers to inputs. */
			nC = 0; /* row number of constraint matrix */


			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
				return;

			/* default value for nWSR */
			nWSRin = 5*nV;

			/* Check whether x0 and options are specified .*/
			if ( nrhs >= 6 )
			{
				if ((!mxIsEmpty(prhs[5])) && (mxIsStruct(prhs[5])))
					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );

				if ( ( nrhs >= 7 ) && ( !mxIsEmpty(prhs[6]) ) )
				{ 
					/* auxInput specified */
					if ( mxIsStruct(prhs[6]) )
					{
						auxInput_idx = 6;
						x0_idx = -1;
					}
					else
					{
						auxInput_idx = -1;
						x0_idx = 6;
					}
				}
				else
				{
					auxInput_idx = -1;
					x0_idx = -1;
				}
			}
		}
		else
		{
			A_idx = 3;

			/* ensure that data is given in double precision */
			if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
			{
				myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
				return;
			}
		
			/* Check inputs dimensions and assign pointers to inputs. */
			nC = (uint_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */

			lb_idx = 4;
			ub_idx = 5;
			lbA_idx = 6;
			ubA_idx = 7;

			if (containsNaNorInf( prhs,A_idx, 0 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
				return;

			if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
			{
				myMexErrMsgTxt( "ERROR (qpOASES): Constraint matrix dimension mismatch!" );
				return;
			}
		
			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
				return;
			
			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
				return;

			/* default value for nWSR */
			nWSRin = 5*(nV+nC);

			/* Check whether x0 and options are specified .*/
			if ( nrhs >= 9 )
			{
				if ((!mxIsEmpty(prhs[8])) && (mxIsStruct(prhs[8])))
					setupOptions( &options,prhs[8],nWSRin,maxCpuTimeIn );

				if ( ( nrhs >= 10 ) && ( !mxIsEmpty(prhs[9]) ) )
				{ 
					/* auxInput specified */
					if ( mxIsStruct(prhs[9]) )
					{
						auxInput_idx = 9;
						x0_idx = -1;
					}
					else
					{
						auxInput_idx = -1;
						x0_idx = 9;
					}
				}
				else
				{
					auxInput_idx = -1;
					x0_idx = -1;
				}
			}
		}


		/* check dimensions and copy auxInputs */
		if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( auxInput_idx >= 0 )
			setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &hessianType,&x0,&guessedBounds,&guessedConstraints,&R_for );

		/* convert Cholesky factor to C storage format */
		if ( R_for != 0 )
		{
			R = new real_t[nV*nV];
			convertFortranToC( R_for, nV,nV, R );
		}

		/* check if QP is sparse */
		if ( H_idx >= 0 && !mxIsSparse( prhs[H_idx] ) )
			isSparse = BT_FALSE;
		if ( nC > 0 && A_idx >= 0 && !mxIsSparse( prhs[A_idx] ) )
			isSparse = BT_FALSE;
		
		/* allocate instance */
		handle = allocateQPInstance( nV,nC,hessianType, isSimplyBoundedQp, isSparse, &options );	
		globalQP = getQPInstance( handle );

		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
		if ( H_idx >= 0 )
			setupHessianMatrix(	prhs[H_idx],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
		
		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
		if ( ( nC > 0 ) && ( A_idx >= 0 ) )
			setupConstraintMatrix( prhs[A_idx],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );

		/* Create output vectors and assign pointers to them. */
		allocateOutputs( nlhs,plhs, nV,nC,1,handle );

		/* Call qpOASES. */
		if ( isSimplyBoundedQp == BT_TRUE )
		{
			QProblemB_init(	handle,
							globalQP->H,g,
							lb,ub,
							nWSRin,maxCpuTimeIn,
							x0,&options,
							nlhs,plhs,
							guessedBounds,R
							);
		}
		else
		{
			SQProblem_init(	handle,
							globalQP->H,g,globalQP->A,
							lb,ub,lbA,ubA,
							nWSRin,maxCpuTimeIn,
							x0,&options,
							nlhs,plhs,
							guessedBounds,guessedConstraints,R
							);
		}

		if (R != 0) delete R;
		return;
	}

	/* 2) Hotstart. */
	if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
	{
		/* consistency checks */
		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( nrhs < 5 ) || ( nrhs > 8 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		/* determine whether is it a simply bounded QP */
		if ( nrhs < 7 )
			isSimplyBoundedQp = BT_TRUE;
		else
			isSimplyBoundedQp = BT_FALSE;


		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		/* get QP instance */
		handle = (uint_t)mxGetScalar( prhs[1] );
		globalQP = getQPInstance( handle );
		if ( globalQP == 0 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
			return;
		}

		nV = globalQP->getNV();

		g_idx = 2;
		lb_idx = 3;
		ub_idx = 4;

		if (containsNaNorInf( prhs,g_idx, 0 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
			return;


		/* Check inputs dimensions and assign pointers to inputs. */
		if ( isSimplyBoundedQp == BT_TRUE )
		{
			nC = 0;

			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
				return;

			/* default value for nWSR */
			nWSRin = 5*nV;

			/* Check whether options are specified .*/
			if ( nrhs == 6 )
				if ( ( !mxIsEmpty( prhs[5] ) ) && ( mxIsStruct( prhs[5] ) ) )
					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
		}
		else
		{
			nC = globalQP->getNC( );

			lbA_idx = 5;
			ubA_idx = 6;

			if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
				return;

			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
				return;

			/* default value for nWSR */
			nWSRin = 5*(nV+nC);

			/* Check whether options are specified .*/
			if ( nrhs == 8 )
				if ( ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
					setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
		}

		/* Create output vectors and assign pointers to them. */
		allocateOutputs( nlhs,plhs, nV,nC );

		/* call qpOASES */
		if ( isSimplyBoundedQp == BT_TRUE )
		{
			QProblemB_hotstart(	handle, g,
								lb,ub,
								nWSRin,maxCpuTimeIn,
								&options,
								nlhs,plhs
								);
		}
		else
		{
			QProblem_hotstart(	handle, g,
								lb,ub,lbA,ubA,
								nWSRin,maxCpuTimeIn,
								&options,
								nlhs,plhs
								);
		}

		return;
	}

	/* 3) Modify matrices. */
	if ( ( strcmp( typeString,"m" ) == 0 ) || ( strcmp( typeString,"M" ) == 0 ) )
	{
		/* consistency checks */
		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( nrhs < 9 ) || ( nrhs > 10 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
			return;
		}


		/* get QP instance */
		handle = (uint_t)mxGetScalar( prhs[1] );
		globalQP = getQPInstance( handle );
		if ( globalQP == 0 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
			return;
		}

		/* Check inputs dimensions and assign pointers to inputs. */
		g_idx = 3;
		
		if ( mxIsEmpty(prhs[2]) == 1 )
		{
			H_idx = -1;
			nV = (uint_t)mxGetM( prhs[ g_idx ] ); /* if Hessian is empty, row number of gradient vector */
		}
		else
		{
			H_idx = 2;
			nV = (uint_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
		}
		
		A_idx = 4;
		nC = (uint_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
				
		lb_idx = 5;
		ub_idx = 6;
		lbA_idx = 7;
		ubA_idx = 8;


		/* ensure that data is given in double precision */
		if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[H_idx] ) == 0 ) ) ||
			 ( mxIsDouble( prhs[g_idx] ) == 0 ) ||
			 ( mxIsDouble( prhs[A_idx] ) == 0 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
			return;
		}

		/* check if supplied data contains 'NaN' or 'Inf' */
		if (containsNaNorInf(prhs,H_idx, 0) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,g_idx, 0 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,A_idx, 0 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
			return;

		/* Check that dimensions are consistent with existing QP instance */
		if (nV != (uint_t) globalQP->getNV () || nC != (uint_t) globalQP->getNC ())
		{
			myMexErrMsgTxt( "ERROR (qpOASES): QP dimensions must be constant during a sequence! Try creating a new QP instance instead." );
			return;
		}

		if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Hessian matrix dimension mismatch!" );
			return;
		}

		if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Constraint matrix dimension mismatch!" );
			return;
		}

		if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
			return;

		/* default value for nWSR */
		nWSRin = 5*(nV+nC);

		/* Check whether options are specified .*/
		if ( nrhs > 9 )
			if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
				setupOptions( &options,prhs[9],nWSRin,maxCpuTimeIn );

		globalQP->deleteQPMatrices( );

		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
		if ( H_idx >= 0 )
			setupHessianMatrix(	prhs[H_idx],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );

		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
		if ( ( nC > 0 ) && ( A_idx >= 0 ) )
			setupConstraintMatrix( prhs[A_idx],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );

		/* Create output vectors and assign pointers to them. */
		allocateOutputs( nlhs,plhs, nV,nC );

		/* Call qpOASES */
		SQProblem_hotstart(	handle, globalQP->H,g,globalQP->A,
							lb,ub,lbA,ubA,
							nWSRin,maxCpuTimeIn,
							&options,
							nlhs,plhs
							);

		return;
	}

	/* 4) Solve current equality constrained QP. */
	if ( ( strcmp( typeString,"e" ) == 0 ) || ( strcmp( typeString,"E" ) == 0 ) )
	{
		/* consistency checks */
		if ( ( nlhs < 1 ) || ( nlhs > 4 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( nrhs < 7 ) || ( nrhs > 8 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		/* get QP instance */
		handle = (uint_t)mxGetScalar( prhs[1] );
		globalQP = getQPInstance( handle );
		if ( globalQP == 0 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
			return;
		}

		/* Check inputs dimensions and assign pointers to inputs. */
		int_t nRHS = (int_t)mxGetN(prhs[2]);
		nV = globalQP->getNV( );
		nC = globalQP->getNC( );
		real_t *x_out, *y_out;

		g_idx = 2;
		lb_idx = 3;
		ub_idx = 4;
		lbA_idx = 5;
		ubA_idx = 6;

		/* check if supplied data contains 'NaN' or 'Inf' */
		if (containsNaNorInf(prhs,g_idx, 0) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
			return;

		if ( smartDimensionCheck( &g,nV,nRHS, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &lb,nV,nRHS, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &ub,nV,nRHS, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &lbA,nC,nRHS, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &ubA,nC,nRHS, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
			return;

		/* Check whether options are specified .*/
		if ( ( nrhs == 8 ) && ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
		{
			nWSRin = 5*(nV+nC);
			setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
			globalQP->sqp->setOptions( options );
		}

		/* Create output vectors and assign pointers to them. */
		plhs[0] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
		x_out = mxGetPr(plhs[0]);
		if (nlhs >= 2)
		{
			plhs[1] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
			y_out = mxGetPr(plhs[1]);

			if (nlhs >= 3)
			{
				plhs[2] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
				real_t* workingSetB = mxGetPr(plhs[2]);
				globalQP->sqp->getWorkingSetBounds(workingSetB);

				if ( nlhs >= 4 )
				{
					plhs[3] = mxCreateDoubleMatrix( nC, nRHS, mxREAL );
					real_t* workingSetC = mxGetPr(plhs[3]);
					globalQP->sqp->getWorkingSetConstraints(workingSetC);
				}
			}
		}
		else
			y_out = new real_t[nV+nC];

		/* Solve equality constrained QP */
		returnValue returnvalue = globalQP->sqp->solveCurrentEQP( nRHS,g,lb,ub,lbA,ubA, x_out,y_out );

		if (nlhs < 2)
			delete[] y_out;

		if (returnvalue != SUCCESSFUL_RETURN)
		{
			char msg[MAX_STRING_LENGTH];
			snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Couldn't solve current EQP (code %d)!", returnvalue);
			myMexErrMsgTxt(msg);
			return;
		}

		return;
	}

	/* 5) Cleanup. */
	if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
	{		
		/* consistency checks */
		if ( nlhs != 0 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( nrhs != 2 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		/* Cleanup SQProblem instance. */
		handle = (uint_t)mxGetScalar( prhs[1] );
		deleteQPInstance( handle );
		
		return;
	}

}
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 );
}
Beispiel #5
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;
}