Esempio n. 1
0
/*
 *	m e x F u n c t i o n
 */
void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
{
	unsigned int i,j;

	/* inputs */
	char* typeString;
	real_t *H_for=0, *H_mem=0, *g=0, *A_for=0, *A_mem=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;

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

	/* dimensions */
	unsigned int nV=0, nC=0;


	/* I) CONSISTENCY CHECKS: */
	/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
	if ( ( nrhs < 6 ) || ( nrhs > 10 ) )
		if ( nrhs != 1 )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );

	/* 2) Ensure that first input is a string ... */
	if ( mxIsChar( prhs[0] ) != 1 )
		mexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );

	typeString = (char*) mxGetPr( prhs[0] );

	/*    ... 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,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
	{
		mexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
	}


	/* 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 > 5 ) )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );

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

		/* ensure that data is given in real_t precision */
		if ( ( mxIsDouble( prhs[1] ) == 0 ) ||
			 ( mxIsDouble( prhs[2] ) == 0 ) ||
			 ( mxIsDouble( prhs[3] ) == 0 ) )
			mexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );


		/* ensure that matrices are stored in dense format */
// 		if ( ( mxIsSparse( prhs[1] ) != 0 ) || ( mxIsSparse( prhs[3] ) != 0 ) )
// 			mexErrMsgTxt( "ERROR (qpOASES): Matrices must not be stored in sparse format!" );

		/* Check inputs dimensions and assign pointers to inputs. */
		nV = mxGetM( prhs[1] ); /* row number of Hessian matrix */
		nC = mxGetM( prhs[3] ); /* row number of constraint matrix */

		if ( ( mxGetN( prhs[1] ) != nV ) || ( ( mxGetN( prhs[3] ) != 0 ) && ( mxGetN( prhs[3] ) != nV ) ) )
			mexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );


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

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

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

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

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

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


		/* Check whether x0 and options are specified .*/
		if ( nrhs > 8 )
		{
			if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,8 ) != SUCCESSFUL_RETURN )
				return;

			if ( nrhs > 9 )
				if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
					setupOptions( &options,prhs[9],nWSRin );
		}

		deleteGlobalQProblemInstance( );
		deleteGlobalQProblemMatrices( );

		/* check for sparsity */
		if ( mxIsSparse( prhs[1] ) != 0 )
		{
			long *ir = (long *)mxGetIr(prhs[1]);
			long *jc = (long *)mxGetJc(prhs[1]);
			real_t *v = (real_t*)mxGetPr(prhs[1]);
			/*
			for (long col = 0; col < nV; col++)
				for (long idx = jc[col]; idx < jc[col+1]; idx++)
					mexPrintf("   (%ld,%ld) %12.4f\n", ir[idx]+1, col+1, v[idx]);
					*/
			//mexPrintf( "%ld\n", ir[0] );
			SymSparseMat *sH;
			globalQP_H = sH = new SymSparseMat(nV, nV, ir, jc, v);
			globalQP_Hdiag = sH->createDiagInfo();
		}
		else
		{
			H_for = (real_t*) mxGetPr( prhs[1] );
			H_mem = new real_t[nV*nV];
			for( int i=0; i<nV*nV; ++i )
				H_mem[i] = H_for[i];
			globalQP_H = new SymDenseMat( nV, nV, nV, H_mem );
			globalQP_H->doFreeMemory();
		}

		/* Convert constraint matrix A from FORTRAN to C style
		 * (not necessary for H as it should be symmetric!). */
		if ( nC > 0 )
		{
			/* Check for sparsity. */
			if ( mxIsSparse( prhs[3] ) != 0 )
			{
				long *ir = (long *)mxGetIr(prhs[3]);
				long *jc = (long *)mxGetJc(prhs[3]);
				real_t *v = (real_t*)mxGetPr(prhs[3]);
				// mind pointer offsets due to 1-based indexing in Matlab
				globalQP_A = new SparseMatrix(nC, nV, ir, jc, v);
			}
			else
			{
				/* Convert constraint matrix A from FORTRAN to C style
				* (not necessary for H as it should be symmetric!). */
				A_for = (real_t*) mxGetPr( prhs[3] );
				A_mem = new real_t[nC*nV];
				convertFortranToC( A_for,nV,nC, A_mem );
				globalQP_A = new DenseMatrix(nC, nV, nV, A_mem );
				globalQP_A->doFreeMemory();
			}
		}

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

		/* Call qpOASES. */
		init(	nV,nC,
				globalQP_H,g,globalQP_A,
				lb,ub,lbA,ubA,
				nWSRin,x0,&options,
				nlhs,plhs
				);

		return;
	}

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

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

		/* has QP been initialised? */
		if ( globalQP == 0 )
			mexErrMsgTxt( "ERROR (qpOASES): QP sequence needs to be initialised first!" );


		/* Check inputs dimensions and assign pointers to inputs. */
		nV = globalQP->getNV( );
		nC = globalQP->getNC( );

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

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

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

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

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

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

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

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

		/* call qpOASES */
		hotstart(	g,
					lb,ub,lbA,ubA,
					nWSRin,&options,
					nlhs,plhs
					);

		return;
	}

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

		if ( nrhs != 1 )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );

		/* Cleanup global QProblem instance. */
		deleteGlobalQProblemInstance( );
		deleteGlobalQProblemMatrices( );

		return;
	}
}
Esempio n. 2
0
/*
 *	s o l v e O q p B e n c h m a r k
 */
returnValue solveOqpBenchmark(	int_t nQP, int_t nV,
								const real_t* const _H, const real_t* const g,
								const real_t* const lb, const real_t* const ub,
								BooleanType isSparse, BooleanType useHotstarts, 
								const Options& options, int_t maxAllowedNWSR,
								real_t& maxNWSR, real_t& avgNWSR, real_t& maxCPUtime, real_t& avgCPUtime,
								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
								)
{
	int_t k;

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

	real_t CPUtimeLimit = maxCPUtime;
	real_t CPUtimeCur = CPUtimeLimit;
	real_t stat, feas, cmpl;
	maxNWSR = 0;
	avgNWSR = 0;
	maxCPUtime = 0.0;
	avgCPUtime = 0.0;
	maxStationarity = 0.0;
	maxFeasibility = 0.0;
	maxComplementarity = 0.0;

	/* 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. */
	real_t* x = new real_t[nV];
	real_t* y = new real_t[nV];
	//real_t  obj;

	/* 4) Prepare matrix objects */
	SymmetricMatrix *H; 
	real_t* H_cpy = new real_t[nV*nV];
	memcpy( H_cpy,_H, ((uint_t)(nV*nV))*sizeof(real_t) );

	if ( isSparse == BT_TRUE )
	{
		SymSparseMat *Hs;
		H = Hs = new SymSparseMat(nV, nV, nV, H_cpy);
		Hs->createDiagInfo();
		delete[] H_cpy;
	}
	else
	{
		H = new SymDenseMat(nV, nV, nV, const_cast<real_t *>(H_cpy));
	}
	
	H->doFreeMemory( );

	/* II) SETUP QPROBLEM OBJECT */
	QProblemB qp( nV );
	qp.setOptions( options );
	//qp.setPrintLevel( PL_LOW );


	/* III) RUN BENCHMARK SEQUENCE: */
	returnValue returnvalue;

	for( k=0; k<nQP; ++k )
	{
		//if ( k%50 == 0 )
		//	printf( "%d\n",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 = qp.init( H,gCur,lbCur,ubCur, nWSRcur,&CPUtimeCur );
			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
			{
				delete H; delete[] y; delete[] x;
				return THROWERROR( returnvalue );
			}
		}
		else
		{
			/* hotstart */
			returnvalue = qp.hotstart( gCur,lbCur,ubCur, nWSRcur,&CPUtimeCur );
			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
			{
				delete H; delete[] y; delete[] x;
				return THROWERROR( returnvalue );
			}
		}

		/* 4) Obtain solution vectors and objective function value ... */
		qp.getPrimalSolution( x );
		qp.getDualSolution( y );
		//obj = qp.getObjVal( );

		/* 5) Compute KKT residuals */
		getKktViolation( 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);

	delete H; delete[] y; delete[] x;

	return SUCCESSFUL_RETURN;
}
Esempio n. 3
0
/*
 *	s o l v e O Q P b e n c h m a r k
 */
returnValue solveOQPbenchmark(	int nQP, int nV, int nC, int nEC,
								const real_t* const _H, const real_t* const g, const real_t* const _A,
								const real_t* const lb, const real_t* const ub,
								const real_t* const lbA, const real_t* const ubA,
								BooleanType isSparse, 
								const Options& options, int& nWSR, real_t& maxCPUtime,
								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
								)
{
	int k;

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

	real_t CPUtimeLimit = maxCPUtime;
	real_t CPUtimeCur = CPUtimeLimit;
	maxCPUtime = 0.0;
	maxStationarity = 0.0;
	maxFeasibility = 0.0;
	maxComplementarity = 0.0;
	real_t stat, feas, cmpl;

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

	/* 3) Vectors for solution obtained by qpOASES. */
	real_t* x = new real_t[nV];
	real_t* y = new real_t[nV+nC];

	/* 4) Prepare matrix objects */
	SymmetricMatrix *H; 
	Matrix *A;
	if ( isSparse == BT_TRUE)
	{
		SymSparseMat *Hs;
		H = Hs = new SymSparseMat(nV, nV, nV, _H);
		A = new SparseMatrix(nC, nV, nV, _A);
		Hs->createDiagInfo();
	}
	else
	{
		H = new SymDenseMat(nV, nV, nV, const_cast<real_t *>(_H));
		A = new DenseMatrix(nC, nV, nV, const_cast<real_t *>(_A));
	}


	/* II) SETUP QPROBLEM OBJECT */
	QProblem qp( nV,nC );
	qp.setOptions( options );
	qp.setPrintLevel( PL_LOW );

	/* III) RUN BENCHMARK SEQUENCE: */
	returnValue returnvalue;

	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] );
		lbACur = &( lbA[k*nC] );
		ubACur = &( ubA[k*nC] );

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

		/* 3) Solve current QP. */
		if ( k == 0 )
		{
			/* initialise */
			returnvalue = qp.init( H,gCur,A,lbCur,ubCur,lbACur,ubACur, nWSRcur,&CPUtimeCur );
			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
			{
				delete A; delete H; delete[] y; delete[] x;
				return THROWERROR( returnvalue );
			}
		}
		else
		{
			/* hotstart */
			returnvalue = qp.hotstart( gCur,lbCur,ubCur,lbACur,ubACur, nWSRcur,&CPUtimeCur );
			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
			{
				delete A; delete H; delete[] y; delete[] x;
				return THROWERROR( returnvalue );
			}
		}

		/* 4) Obtain solution vectors and objective function value */
		qp.getPrimalSolution( x );
		qp.getDualSolution( y );

		/* 5) Compute KKT residuals */
		getKKTResidual( nV, nC, _H,gCur,_A,lbCur,ubCur,lbACur,ubACur, 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;
	}
	nWSR = maxNWSR;

	delete A; delete H; delete[] y; delete[] x;

	return SUCCESSFUL_RETURN;
}
/*
 *	s e t u p H e s s i a n M a t r i x
 */
returnValue setupHessianMatrix(	const mxArray* prhsH, int_t nV,
								SymmetricMatrix** H, sparse_int_t** Hir, sparse_int_t** Hjc, real_t** Hv
								)
{
	if ( prhsH == 0 )
		return SUCCESSFUL_RETURN;

	if ( mxIsSparse( prhsH ) != 0 )
	{
		mwIndex *mat_ir = mxGetIr( prhsH );
		mwIndex *mat_jc = mxGetJc( prhsH );
		double *v = (double*)mxGetPr( prhsH );
		sparse_int_t nfill = 0;
		mwIndex i, j;
		BooleanType needInsertDiag;

		/* copy indices to avoid 64/32-bit integer confusion */
		/* also add explicit zeros on diagonal for regularization strategy */
		/* copy values, too */
		*Hir = new sparse_int_t[mat_jc[nV] + nV];
		*Hjc = new sparse_int_t[nV+1];
		*Hv = new real_t[mat_jc[nV] + nV];
        for (j = 0; j < nV; j++) 
		{
            needInsertDiag = BT_TRUE;
                
            (*Hjc)[j] = (sparse_int_t)(mat_jc[j]) + nfill;
            /* fill up to diagonal */
            for (i = mat_jc[j]; i < mat_jc[j+1]; i++) 
			{
                if ( mat_ir[i] == j )
                    needInsertDiag = BT_FALSE;
                    
                /* add zero diagonal element if not present */
                if ( ( mat_ir[i] > j ) && ( needInsertDiag == BT_TRUE ) )
                {
                    (*Hir)[i + nfill] = (sparse_int_t)j;
                    (*Hv)[i + nfill] = 0.0;
                    nfill++;
                    /* only add diag once */
                    needInsertDiag = BT_FALSE;
                }
                        
				(*Hir)[i + nfill] = (sparse_int_t)(mat_ir[i]);
				(*Hv)[i + nfill] = (real_t)(v[i]);
			}
		}
		(*Hjc)[nV] = (sparse_int_t)(mat_jc[nV]) + nfill;

		SymSparseMat *sH;
		*H = sH = new SymSparseMat(nV, nV, *Hir, *Hjc, *Hv);
		sH->createDiagInfo();
	}
	else
	{
		/* make a deep-copy in order to avoid modifying input data when regularising */
		real_t* H_for = (real_t*) mxGetPr( prhsH );
		real_t* H_mem = new real_t[nV*nV];
		memcpy( H_mem,H_for, nV*nV*sizeof(real_t) );

		*H = new SymDenseMat( nV,nV,nV, H_mem );
		(*H)->doFreeMemory( );
	}

	return SUCCESSFUL_RETURN;
}
Esempio n. 5
0
/** Example for calling qpOASES with sparse matrices. */
int main( )
{
	long i;
	int nWSR;
	real_t err, tic, toc;
	real_t *x1 = new real_t[180];
	real_t *y1 = new real_t[271];
	real_t *x2 = new real_t[180];
	real_t *y2 = new real_t[271];

	/* create sparse matrices */
	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
	SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);

	H->createDiagInfo();

	real_t* H_full = H->full();
	real_t* A_full = A->full();

	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
	DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);

	/* solve with dense matrices */
	nWSR = 1000;
	QProblem qrecipeD(180, 91);
	tic = getCPUtime();
	qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0);
	toc = getCPUtime();
	qrecipeD.getPrimalSolution(x1);
	qrecipeD.getDualSolution(y1);

	fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);

	/* solve with sparse matrices */
	nWSR = 1000;
	QProblem qrecipeS(180, 91);
	tic = getCPUtime();
	qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
	toc = getCPUtime();
	qrecipeS.getPrimalSolution(x2);
	qrecipeS.getDualSolution(y2);

	fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);

	/* check distance of solutions */
	err = 0.0;
	for (i = 0; i < 180; i++)
		if (getAbs(x1[i] - x2[i]) > err)
			err = getAbs(x1[i] - x2[i]);
	fprintf(stdFile, "Primal error: %9.2e\n", err);
	err = 0.0;
	for (i = 0; i < 271; i++)
		if (getAbs(y1[i] - y2[i]) > err)
			err = getAbs(y1[i] - y2[i]);
	fprintf(stdFile, "Dual error: %9.2e  (might not be unique)\n", err);

	delete H;
	delete A;
	delete[] H_full;
	delete[] A_full;
	delete Hd;
	delete Ad;

	delete[] y2;
	delete[] x2;
	delete[] y1;
	delete[] x1;

	return 0;
}
Esempio n. 6
0
int Symmetry()
{
	int i,j;
	real_t *Hv = new real_t[6*6];
	real_t *Z = new real_t[6*3];
	real_t *ZHZd = new real_t[3*3];
	real_t *ZHZs = new real_t[3*3];
	real_t ZHZv[] = {0.144, 0.426, 0.708, 0.426, 1.500, 2.574, 0.708, 2.574, 4.440};
	real_t err;
	SymDenseMat *Hd;
	SymSparseMat *Hs;
	Indexlist *cols = new Indexlist(6);

	for (i = 0; i < 36; i++) Hv[i] = 0.0;
	for (i = 0; i < 6; i++) Hv[i*7] = 1.0 - 0.1 * i;
	for (i = 0; i < 5; i++) Hv[i*7+1] = Hv[i*7+6] = -0.1 * (i+1);

	Hd = new SymDenseMat(6, 6, 6, Hv);	// deep-copy from Hv
	Hs = new SymSparseMat(6, 6, 6, Hv);	// deep-copy from Hv
	Hs->createDiagInfo();

	for (i = 0; i < 6; ++i)
	{
		for (j = 0; j < 6; ++j)
			fprintf (stderr, "%3.3f ", Hv[i*6+j]);
		fprintf (stderr, "\n");
	}
	fprintf (stderr, "\n");
	
	cols->addNumber(3);
	cols->addNumber(0);
	cols->addNumber(4);
	cols->addNumber(1);
	for (i = 0; i < 18; i++) Z[i] = 0.1 * (i+1);

	fprintf (stderr, "\n");
	for (i = 0; i < 6; ++i)
	{
		for (j = 0; j < 3; ++j)
			fprintf (stderr, "%3.3f ", Z[i+j*6]);
		fprintf (stderr, "\n");
	}
	fprintf (stderr, "\n");
	
	Hd->bilinear(cols, 3, Z, 6, ZHZd, 3);

	err = 0.0;
	for (i = 0; i < 9; i++)
		if (fabs(ZHZd[i] - ZHZv[i]) > err)
			err = fabs(ZHZd[i] - ZHZv[i]);
	fprintf(stdFile, "Error in indexed dense bilinear form: %9.2e\n", err);

	Hs->bilinear(cols, 3, Z, 6, ZHZs, 3);
	
	for (i = 0; i < 3; ++i)
	{
		for (j = 0; j < 3; ++j)
			fprintf (stderr, "%3.3f ", ZHZd[i*3+j]);
		fprintf (stderr, "\n");
	}
	fprintf (stderr, "\n");

	for (i = 0; i < 3; ++i)
	{
		for (j = 0; j < 3; ++j)
			fprintf (stderr, "%3.3f ", ZHZv[i*3+j]);
		fprintf (stderr, "\n");
	}
	fprintf (stderr, "\n");

	err = 0.0;
	for (i = 0; i < 9; i++)
		if (fabs(ZHZs[i] - ZHZv[i]) > err)
			err = fabs(ZHZs[i] - ZHZv[i]);
	fprintf(stdFile, "Error in indexed sparse bilinear form: %9.2e\n", err);

	delete cols;	
	delete Hs;
	delete Hd;
	delete[] ZHZs;
	delete[] ZHZd;
	delete[] Z;
	delete[] Hv;

	return 0;
}
Esempio n. 7
0
/*
 *	m e x F u n c t i o n
 */
void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
{
    /* inputs */
    SymmetricMatrix *H=0;
    Matrix *A=0;
    real_t *g=0, *A_for=0, *A_mem=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
    int H_idx, g_idx, A_idx, lb_idx, ub_idx, lbA_idx, ubA_idx, x0_idx=-1, options_idx=-1;

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

    /* dimensions */
    unsigned int nV=0, nC=0, nP=0;

    /* sparse matrix indices and values */
    sparse_int_t *Hdiag = 0, *Hir = 0, *Hjc = 0, *Air = 0, *Ajc = 0;
    real_t *Hv = 0, *Av = 0;

    /* I) CONSISTENCY CHECKS: */
    /* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
    if ( ( nrhs < 4 ) || ( nrhs > 9 ) )
        mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\n    Type 'help qpOASES' for further information." );

    /* 2) Check for proper number of output arguments. */
    if ( nlhs > 5 )
        mexErrMsgTxt( "ERROR (qpOASES): At most five output arguments are allowed: \n    [obj,x,y,status,nWSRout]!" );
    if ( nlhs < 1 )
        mexErrMsgTxt( "ERROR (qpOASES): At least one output argument is required: [obj,...]!" );


    /* II) PREPARE RESPECTIVE QPOASES FUNCTION CALL: */
    /*     Choose between QProblem and QProblemB object and assign the corresponding
     *     indices of the input pointer array in to order to access QP data correctly. */
    H_idx = 0;
    g_idx = 1;
    nV = mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
    nP = mxGetN( prhs[ g_idx ] ); /* number of columns of the gradient matrix (vectors series have to be stored columnwise!) */

    /* 0) Check whether options are specified .*/
    if ( ( !mxIsEmpty( prhs[nrhs-1] ) ) && ( mxIsStruct( prhs[nrhs-1] ) ) )
        options_idx = nrhs-1;

    /* 1) Simply bounded QP. */
    if ( ( ( nrhs >= 4 ) && ( nrhs <= 5 ) ) ||
            ( ( options_idx > 0 ) && ( nrhs == 6 ) ) )
    {
        lb_idx   = 2;
        ub_idx   = 3;

        if ( nrhs >= 5 ) /* x0 specified */
            x0_idx = 4;
        else
            x0_idx = -1;
    }
    else
    {
        A_idx = 2;

        /* If constraint matrix is empty, use a QProblemB object! */
        if ( mxIsEmpty( prhs[ A_idx ] ) )
        {
            lb_idx   = 3;
            ub_idx   = 4;

            if ( nrhs >= 8 ) /* x0 specified */
                x0_idx = 7;
            else
                x0_idx = -1;
        }
        else
        {
            lb_idx   = 3;
            ub_idx   = 4;
            lbA_idx  = 5;
            ubA_idx  = 6;

            if ( nrhs >= 8 ) /* x0 specified */
                x0_idx = 7;
            else
                x0_idx = -1;

            nC = mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
        }
    }


    /* III) ACTUALLY PERFORM QPOASES FUNCTION CALL: */
    int nWSRin = 5*(nV+nC);
    if ( options_idx > 0 )
        setupOptions( &options,prhs[options_idx],nWSRin );

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

    /* Check inputs dimensions and assign pointers to inputs. */
    if ( mxGetN( prhs[ H_idx ] ) != nV )
    {
        char msg[200];
        snprintf(msg, 199, "ERROR (qpOASES): Hessian matrix input dimension mismatch (%ld != %d)!",
                 (long int)mxGetN(prhs[H_idx]), nV);
        fprintf(stderr, "%s\n", msg);
        mexErrMsgTxt(msg);
    }

    /* check for sparsity */
    if ( mxIsSparse( prhs[ H_idx ] ) != 0 )
    {
        mwIndex *oct_ir = mxGetIr(prhs[H_idx]);
        mwIndex *oct_jc = mxGetJc(prhs[H_idx]);
        double *v = (double*)mxGetPr(prhs[H_idx]);
        long nfill = 0;
        long i, j;

        /* copy indices to avoid 64/32-bit integer confusion */
        /* also add explicit zeros on diagonal for regularization strategy */
        /* copy values, too */
        Hir = new sparse_int_t[oct_jc[nV] + nV];
        Hjc = new sparse_int_t[nV+1];
        Hv = new real_t[oct_jc[nV] + nV];
        for (j = 0; j < nV; j++)
        {
            Hjc[j] = oct_jc[j] + nfill;
            /* fill up to diagonal */
            for (i = oct_jc[j]; i < oct_jc[j+1] && oct_ir[i] <= j; i++)
            {
                Hir[i + nfill] = oct_ir[i];
                Hv[i + nfill] = v[i];
            }
            /* possibly add zero diagonal element */
            if (i >= oct_jc[j+1] || oct_ir[i] > j)
            {
                Hir[i + nfill] = j;
                Hv[i + nfill] = 0.0;
                nfill++;
            }
            /* fill up to diagonal */
            for (; i < oct_jc[j+1]; i++)
            {
                Hir[i + nfill] = oct_ir[i];
                Hv[i + nfill] = v[i];
            }
        }
        Hjc[nV] = oct_jc[nV] + nfill;

        SymSparseMat *sH;
        H = sH = new SymSparseMat(nV, nV, Hir, Hjc, Hv);
        Hdiag = sH->createDiagInfo();
    }
    else
    {
        H = new SymDenseMat(nV, nV, nV, (real_t*) mxGetPr(prhs[H_idx]));
    }

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

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

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

    if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
        return;

    if ( nC > 0 )
    {
        /* ensure that data is given in real_t precision */
        if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
            mexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );

        /* Check inputs dimensions and assign pointers to inputs. */
        if ( mxGetN( prhs[ A_idx ] ) != nV )
        {
            char msg[200];
            snprintf(msg, 199, "ERROR (qpOASES): Constraint matrix input dimension mismatch (%ld != %d)!",
                     (long int)mxGetN(prhs[A_idx]), nV);
            fprintf(stderr, "%s\n", msg);
            mexErrMsgTxt(msg);
        }

        A_for = (real_t*) mxGetPr( prhs[ A_idx ] );

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

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

        /* Check for sparsity. */
        if ( mxIsSparse( prhs[ A_idx ] ) != 0 )
        {
            mwIndex *oct_ir = mxGetIr(prhs[A_idx]);
            mwIndex *oct_jc = mxGetJc(prhs[A_idx]);
            double *v = (double*)mxGetPr(prhs[A_idx]);

            /* copy indices to avoid 64/32-bit integer confusion */
            Air = new sparse_int_t[oct_jc[nV]];
            Ajc = new sparse_int_t[nV+1];
            for (long i = 0; i < oct_jc[nV]; i++) Air[i] = oct_ir[i];
            for (long i = 0; i < nV + 1; i++) Ajc[i] = oct_jc[i];

            /* copy values, too */
            Av = new real_t[Ajc[nV]];
            for (long i = 0; i < Ajc[nV]; i++) Av[i] = v[i];

            A = new SparseMatrix(nC, nV, Air, Ajc, Av);
        }
        else
        {
            /* Convert constraint matrix A from FORTRAN to C style
             * (not necessary for H as it should be symmetric!). */
            A_mem = new real_t[nC*nV];
            convertFortranToC( A_for,nV,nC, A_mem );
            A = new DenseMatrix(nC, nV, nV, A_mem );
            A->doFreeMemory();
        }
    }

    allocateOutputs( nlhs,plhs,nV,nC,nP );

    if ( nC == 0 )
    {
        /* call qpOASES */
        qpOASESmex_bounds(	nV,nP,
                            H,g,
                            lb,ub,
                            nWSRin,x0,&options,
                            nlhs,plhs
                         );

        delete H;
        if (Hdiag) delete[] Hdiag;
        if (Hv) delete[] Hv;
        if (Hjc) delete[] Hjc;
        if (Hir) delete[] Hir;
        return;
        /* 2) Call usual version including constraints (using QProblem class) */
    }
    else
    {
        /* Call qpOASES. */
        qpOASESmex_constraints(	nV,nC,nP,
                                H,g,A,
                                lb,ub,lbA,ubA,
                                nWSRin,x0,&options,
                                nlhs,plhs
                              );

        delete A;
        delete H;
        if (Av) delete[] Av;
        if (Ajc) delete[] Ajc;
        if (Air) delete[] Air;
        if (Hdiag) delete[] Hdiag;
        if (Hv) delete[] Hv;
        if (Hjc) delete[] Hjc;
        if (Hir) delete[] Hir;
        return;
    }
}
Esempio n. 8
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;
	real_t *H_for=0, *H_mem=0, *g=0, *lb=0, *ub=0, *x0=0;

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

	/* dimensions */
	unsigned int nV=0;


	/* I) CONSISTENCY CHECKS: */
	/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
	if ( ( nrhs < 4 ) || ( nrhs > 7 ) )
		if ( nrhs != 1 )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequenceSB' for further information." );

	/* 2) Ensure that first input is a string (and if so, read it). */
	if ( mxIsChar( prhs[0] ) != 1 )
		mexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );

	typeString = (char*) mxGetPr( prhs[0] );


	/* 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 > 5 ) )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequenceSB' for further information." );

		if ( ( nrhs < 5 ) || ( nrhs > 7 ) )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequenceSB' for further information." );

		/* ensure that data is given in real_t precision */
		if ( ( mxIsDouble( prhs[1] ) == 0 ) ||
			 ( mxIsDouble( prhs[2] ) == 0 ) )
			mexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );

		/* ensure that Hessian matrix is stored in dense format */
// 		if ( mxIsSparse( prhs[1] ) != 0 )
// 			mexErrMsgTxt( "ERROR (qpOASES): Matrices must not be stored in sparse format!" );


		/* Check inputs dimensions and assign pointers to inputs. */
		nV = mxGetM( prhs[1] ); /* row number of Hessian matrix */

		if ( mxGetN( prhs[1] ) != nV )
			mexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );

		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 */
		int nWSRin = 5*nV;

		/* Check whether x0 and options are specified .*/
		if ( nrhs > 5 )
		{
			if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
				return;

			if ( nrhs > 6 )
				if ( ( !mxIsEmpty( prhs[6] ) ) && ( mxIsStruct( prhs[6] ) ) )
					setupOptions( &options,prhs[6],nWSRin );
		}

		deleteGlobalQProblemBInstance( );
		deleteGlobalQProblemBMatrices( );

		/* check for sparsity */
		if ( mxIsSparse( prhs[1] ) != 0 )
		{
			mwIndex *mat_ir = mxGetIr(prhs[1]);
			mwIndex *mat_jc = mxGetJc(prhs[1]);
			double *v = (double*)mxGetPr(prhs[1]);
			long nfill = 0;
			mwIndex i, j;

			/* copy indices to avoid 64/32-bit integer confusion */
			/* also add explicit zeros on diagonal for regularization strategy */
			/* copy values, too */
			globalQPB_Hir = new sparse_int_t[mat_jc[nV] + nV];
			globalQPB_Hjc = new sparse_int_t[nV+1];
			globalQPB_Hv = new real_t[mat_jc[nV] + nV];
			for (j = 0; j < nV; j++) 
			{
				globalQPB_Hjc[j] = mat_jc[j] + nfill;
				/* fill up to diagonal */
				for (i = mat_jc[j]; i < mat_jc[j+1] && mat_ir[i] <= j; i++) 
				{
					globalQPB_Hir[i + nfill] = mat_ir[i];
					globalQPB_Hv[i + nfill] = v[i];
				}
				/* possibly add zero diagonal element */
				if (i >= mat_jc[j+1] || mat_ir[i] > j)
				{
					globalQPB_Hir[i + nfill] = j;
					globalQPB_Hv[i + nfill] = 0.0;
					nfill++;
				}
				/* fill up to diagonal */
				for (; i < mat_jc[j+1]; i++) 
				{
					globalQPB_Hir[i + nfill] = mat_ir[i];
					globalQPB_Hv[i + nfill] = v[i];
				}
			}
			globalQPB_Hjc[nV] = mat_jc[nV] + nfill;

			SymSparseMat *sH;
			globalQPB_H = sH = new SymSparseMat(nV, nV, globalQPB_Hir, globalQPB_Hjc, globalQPB_Hv);
			globalQPB_Hdiag = sH->createDiagInfo();
		}
		else
		{
			H_for = (real_t*) mxGetPr( prhs[1] );
			H_mem = new real_t[nV*nV];
			for (unsigned long i = 0; i < nV*nV; ++i)
				H_mem[i] = H_for[i];
			globalQPB_H = new SymDenseMat( nV, nV, nV, H_mem );
			globalQPB_H->doFreeMemory();
		}

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

		/* call qpOASES */
		initSB(	nV,
				globalQPB_H,g,
				lb,ub,
				nWSRin,x0,&options,
				nlhs,plhs
				);

		return;
	}

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

		if ( ( nrhs < 4 ) || ( nrhs > 5 ) )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequenceSB' for further information." );

		/* has QP been initialised? */
		if ( globalQPB == 0 )
			mexErrMsgTxt( "ERROR (qpOASES): QP sequence needs to be initialised first!" );

		/* Check inputs dimensions and assign pointers to inputs. */
		nV = globalQPB->getNV( );

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

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

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

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

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

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

		/* call qpOASES */
		hotstartSB(	g,
					lb,ub,
					nWSRin,&options,
					nlhs,plhs
					);

		return;
	}

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

		if ( nrhs != 1 )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequenceSB' for further information." );

		/* Cleanup global QProblemB instance. */
		deleteGlobalQProblemBInstance( );
		deleteGlobalQProblemBMatrices( );
		return;
	}
}
int main( )
{
	USING_NAMESPACE_QPOASES

	long i;
	int nWSR;
	real_t tic, toc;
	real_t errP=0.0, errD=0.0;
	real_t *x1 = new real_t[180];
	real_t *y1 = new real_t[271];
	real_t *x2 = new real_t[180];
	real_t *y2 = new real_t[271];

	/* create sparse matrices */
	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
	SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);

	H->createDiagInfo();

	real_t* H_full = H->full();
	real_t* A_full = A->full();

	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
	DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);

	/* solve with dense matrices */
	nWSR = 1000;
	QProblem qrecipeD(180, 91);
	tic = getCPUtime();
	qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0);
	toc = getCPUtime();
	qrecipeD.getPrimalSolution(x1);
	qrecipeD.getDualSolution(y1);

	fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);

	/* Compute KKT tolerances */
	real_t stat, feas, cmpl;

	getKKTResidual(	180,91,
					H_full,g,A_full,lb,ub,lbA,ubA,
					x1,y1,
					stat,feas,cmpl
					);
	printf( "stat = %e\nfeas = %e\ncmpl = %e\n\n", stat,feas,cmpl );


	/* solve with sparse matrices */
	nWSR = 1000;
	QProblem qrecipeS(180, 91);
	tic = getCPUtime();
	qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
	toc = getCPUtime();
	qrecipeS.getPrimalSolution(x2);
	qrecipeS.getDualSolution(y2);

	fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
	
	/* Compute KKT tolerances */
	real_t stat2, feas2, cmpl2;
	getKKTResidual(	180,91,
					H_full,g,A_full,lb,ub,lbA,ubA,
					x2,y2,
					stat2,feas2,cmpl2
					);
	printf( "stat = %e\nfeas = %e\ncmpl = %e\n\n", stat2,feas2,cmpl2 );

	/* check distance of solutions */
	for (i = 0; i < 180; i++)
		if (getAbs(x1[i] - x2[i]) > errP)
			errP = getAbs(x1[i] - x2[i]);
	fprintf(stdFile, "Primal error: %9.2e\n", errP);

	for (i = 0; i < 271; i++)
		if (getAbs(y1[i] - y2[i]) > errD)
			errD = getAbs(y1[i] - y2[i]);
	fprintf(stdFile, "Dual error: %9.2e (might not be unique)\n", errD);

	delete H;
	delete A;
	delete[] H_full;
	delete[] A_full;
	delete Hd;
	delete Ad;

	delete[] y2;
	delete[] x2;
	delete[] y1;
	delete[] x1;

	QPOASES_TEST_FOR_TRUE( stat <= 1e-15 );
	QPOASES_TEST_FOR_TRUE( feas <= 1e-15 );
	QPOASES_TEST_FOR_TRUE( cmpl <= 1e-15 );
	
	QPOASES_TEST_FOR_TRUE( stat2 <= 1e-14 );
	QPOASES_TEST_FOR_TRUE( feas2 <= 1e-14 );
	QPOASES_TEST_FOR_TRUE( cmpl2 <= 1e-13 );

	QPOASES_TEST_FOR_TRUE( errP <= 1e-13 );

	return TEST_PASSED;
}
Esempio n. 10
0
/*
 *	m e x F u n c t i o n
 */
void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
{
	mwIndex i, j;

	/* inputs */
	char* typeString;
	real_t *H_for=0, *H_mem=0, *g=0, *A_for=0, *A_mem=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;

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

	/* dimensions */
	unsigned int nV=0, nC=0;


	/* I) CONSISTENCY CHECKS: */
	/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
	if ( ( nrhs < 6 ) || ( nrhs > 10 ) )
		if ( nrhs != 1 )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );

	/* 2) Ensure that first input is a string ... */
	if ( mxIsChar( prhs[0] ) != 1 )
		mexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );

	typeString = (char*) mxGetPr( prhs[0] );

	/*    ... 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 ) )
	{
		mexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
	}


	/* 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 > 5 ) )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );

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

		/* ensure that data is given in real_t precision */
		if ( ( mxIsDouble( prhs[1] ) == 0 ) ||
			 ( mxIsDouble( prhs[2] ) == 0 ) ||
			 ( mxIsDouble( prhs[3] ) == 0 ) )
			mexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );


		/* Check inputs dimensions and assign pointers to inputs. */
		nV = mxGetM( prhs[1] ); /* row number of Hessian matrix */
		nC = mxGetM( prhs[3] ); /* row number of constraint matrix */

		if ( ( mxGetN( prhs[1] ) != nV ) || ( ( mxGetN( prhs[3] ) != 0 ) && ( mxGetN( prhs[3] ) != nV ) ) )
			mexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );


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

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

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

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

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

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

		/* Check whether x0 and options are specified .*/
		if ( nrhs > 8 )
		{
			if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,8 ) != SUCCESSFUL_RETURN )
				return;

			if ( nrhs > 9 )
				if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
					setupOptions( &options,prhs[9],nWSRin );
		}

		deleteGlobalQProblemInstance( );
		deleteGlobalQProblemMatrices( );

		/* check for sparsity */
		if ( mxIsSparse( prhs[1] ) != 0 )
		{
			mwIndex *mat_ir = mxGetIr(prhs[1]);
			mwIndex *mat_jc = mxGetJc(prhs[1]);
			double *v = (double*)mxGetPr(prhs[1]);
			long nfill = 0;

			/* copy indices to avoid 64/32-bit integer confusion */
			/* also add explicit zeros on diagonal for regularization strategy */
			/* copy values, too */
			globalQP_Hir = new sparse_int_t[mat_jc[nV] + nV];
			globalQP_Hjc = new sparse_int_t[nV+1];
			globalQP_Hv = new real_t[mat_jc[nV] + nV];
			for (j = 0; j < nV; j++) 
			{
				globalQP_Hjc[j] = mat_jc[j] + nfill;
				/* fill up to diagonal */
				for (i = mat_jc[j]; i < mat_jc[j+1] && mat_ir[i] <= j; i++) 
				{
					globalQP_Hir[i + nfill] = mat_ir[i];
					globalQP_Hv[i + nfill] = v[i];
				}
				/* possibly add zero diagonal element */
				if (i >= mat_jc[j+1] || mat_ir[i] > j)
				{
					globalQP_Hir[i + nfill] = j;
					globalQP_Hv[i + nfill] = 0.0;
					nfill++;
				}
				/* fill up to diagonal */
				for (; i < mat_jc[j+1]; i++) 
				{
					globalQP_Hir[i + nfill] = mat_ir[i];
					globalQP_Hv[i + nfill] = v[i];
				}
			}
			globalQP_Hjc[nV] = mat_jc[nV] + nfill;

			SymSparseMat *sH;
			globalQP_H = sH = new SymSparseMat(nV, nV, globalQP_Hir, globalQP_Hjc, globalQP_Hv);
			globalQP_Hdiag = sH->createDiagInfo();
		}
		else
		{
			H_for = (real_t*) mxGetPr( prhs[1] );
			H_mem = new real_t[nV*nV];
			for (i = 0; i < nV*nV; ++i)
				H_mem[i] = H_for[i];
			globalQP_H = new SymDenseMat( nV, nV, nV, H_mem );
			globalQP_H->doFreeMemory();
		}

		/* Convert constraint matrix A from FORTRAN to C style
		 * (not necessary for H as it should be symmetric!). */
		if ( nC > 0 )
		{
			/* Check for sparsity. */
			if ( mxIsSparse( prhs[3] ) != 0 )
			{
				mwIndex *mat_ir = mxGetIr(prhs[3]);
				mwIndex *mat_jc = mxGetJc(prhs[3]);
				double *v = (double*)mxGetPr(prhs[3]);

				/* copy indices to avoid 64/32-bit integer confusion */
				globalQP_Air = new sparse_int_t[mat_jc[nV]];
				globalQP_Ajc = new sparse_int_t[nV+1];
				for (i = 0; i < mat_jc[nV]; i++) globalQP_Air[i] = mat_ir[i];
				for (i = 0; i < nV + 1; i++) globalQP_Ajc[i] = mat_jc[i];

				/* copy values, too */
				globalQP_Av = new real_t[globalQP_Ajc[nV]];
				for (i = 0; i < (mwIndex)globalQP_Ajc[nV]; i++) globalQP_Av[i] = v[i];

				globalQP_A = new SparseMatrix(nC, nV, globalQP_Air, globalQP_Ajc, globalQP_Av);
			}
			else
			{
				/* Convert constraint matrix A from FORTRAN to C style
				* (not necessary for H as it should be symmetric!). */
				A_for = (real_t*) mxGetPr( prhs[3] );
				A_mem = new real_t[nC*nV];
				convertFortranToC( A_for,nV,nC, A_mem );
				globalQP_A = new DenseMatrix(nC, nV, nV, A_mem );
				globalQP_A->doFreeMemory();
			}
		}

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

		/* Call qpOASES. */
		init(	nV,nC,
				globalQP_H,g,globalQP_A,
				lb,ub,lbA,ubA,
				nWSRin,x0,&options,
				nlhs,plhs
				);

		return;
	}

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

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

		/* has QP been initialised? */
		if ( globalQP == 0 )
			mexErrMsgTxt( "ERROR (qpOASES): QP sequence needs to be initialised first!" );


		/* Check inputs dimensions and assign pointers to inputs. */
		nV = globalQP->getNV( );
		nC = globalQP->getNC( );

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

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

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

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

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

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

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

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

		/* call qpOASES */
		hotstart(	g,
					lb,ub,lbA,ubA,
					nWSRin,&options,
					nlhs,plhs
					);

		return;
	}

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

		if ( nrhs != 1 )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );

		/* Cleanup global SQProblem instance. */
		deleteGlobalQProblemInstance( );
		deleteGlobalQProblemMatrices( );
		
		return;
	}

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

		if (nrhs != 6 && nrhs != 7)
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );

		/* has QP been initialised? */
		if ( globalQP == 0 )
			mexErrMsgTxt( "ERROR (qpOASES): QP sequence needs to be initialised first!" );


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

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

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

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

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

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

		/* Check whether options are specified .*/
		if ( (  nRHS == 7 ) && ( !mxIsEmpty( prhs[6] ) ) && ( mxIsStruct( prhs[6] ) ) )
		{
			int nWSRin = 5*(nV+nC);
			setupOptions( &options,prhs[6],nWSRin );
			globalQP->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]);
		}
		else
			y_out = new real_t[nV+nC];

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

		if (nlhs < 2)
			delete[] y_out;

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

		return;
	}

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

		if ( ( nrhs < 8 ) || ( nrhs > 10 ) )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequenceVM' for further information." );

		/* ensure that data is given in real_t precision */
		if ( ( mxIsDouble( prhs[1] ) == 0 ) ||
			 ( mxIsDouble( prhs[2] ) == 0 ) ||
			 ( mxIsDouble( prhs[3] ) == 0 ) )
			mexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );

		/* Check inputs dimensions and assign pointers to inputs. */
		nV = mxGetM( prhs[1] ); /* row number of Hessian matrix */
		nC = mxGetM( prhs[3] ); /* row number of constraint matrix */

		if ( ( mxGetN( prhs[1] ) != nV ) || ( ( mxGetN( prhs[3] ) != 0 ) && ( mxGetN( prhs[3] ) != nV ) ) )
			mexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );

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

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

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

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

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

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

		/* Check whether x0 and options are specified .*/
		if ( nrhs > 9 )
			mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequenceVM' for further information." );

		if ( nrhs > 8 )
			if ( ( !mxIsEmpty( prhs[8] ) ) && ( mxIsStruct( prhs[8] ) ) )
				setupOptions( &options,prhs[8],nWSRin );

		deleteGlobalQProblemMatrices( );

		/* check for sparsity */
		if ( mxIsSparse( prhs[1] ) != 0 )
		{
			mwIndex *mat_ir = mxGetIr(prhs[1]);
			mwIndex *mat_jc = mxGetJc(prhs[1]);
			double *v = (double*)mxGetPr(prhs[1]);
			long nfill = 0;

			/* copy indices to avoid 64/32-bit integer confusion */
			/* also add explicit zeros on diagonal for regularization strategy */
			/* copy values, too */
			globalQP_Hir = new sparse_int_t[mat_jc[nV] + nV];
			globalQP_Hjc = new sparse_int_t[nV+1];
			globalQP_Hv = new real_t[mat_jc[nV] + nV];
			for (j = 0; j < nV; j++) 
			{
				globalQP_Hjc[j] = mat_jc[j] + nfill;
				/* fill up to diagonal */
				for (i = mat_jc[j]; i < mat_jc[j+1] && mat_ir[i] <= j; i++) 
				{
					globalQP_Hir[i + nfill] = mat_ir[i];
					globalQP_Hv[i + nfill] = v[i];
				}
				/* possibly add zero diagonal element */
				if (i >= mat_jc[j+1] || mat_ir[i] > j)
				{
					globalQP_Hir[i + nfill] = j;
					globalQP_Hv[i + nfill] = 0.0;
					nfill++;
				}
				/* fill up to diagonal */
				for (; i < mat_jc[j+1]; i++) 
				{
					globalQP_Hir[i + nfill] = mat_ir[i];
					globalQP_Hv[i + nfill] = v[i];
				}
			}
			globalQP_Hjc[nV] = mat_jc[nV] + nfill;

			SymSparseMat *sH;
			globalQP_H = sH = new SymSparseMat(nV, nV, globalQP_Hir, globalQP_Hjc, globalQP_Hv);
			globalQP_Hdiag = sH->createDiagInfo();
		}
		else
		{
			H_for = (real_t*) mxGetPr( prhs[1] );
			H_mem = new real_t[nV*nV];
			for(i = 0; i < nV*nV; ++i)
				H_mem[i] = H_for[i];
			globalQP_H = new SymDenseMat( nV, nV, nV, H_mem );
			globalQP_H->doFreeMemory();
		}

		/* Convert constraint matrix A from FORTRAN to C style
		 * (not necessary for H as it should be symmetric!). */
		if ( nC > 0 )
		{
			/* Check for sparsity. */
			if ( mxIsSparse( prhs[3] ) != 0 )
			{
				mwIndex *mat_ir = mxGetIr(prhs[3]);
				mwIndex *mat_jc = mxGetJc(prhs[3]);
				double *v = (double*)mxGetPr(prhs[3]);

				/* copy indices to avoid 64/32-bit integer confusion */
				globalQP_Air = new sparse_int_t[mat_jc[nV]];
				globalQP_Ajc = new sparse_int_t[nV+1];
				for (i = 0; i < mat_jc[nV]; i++) globalQP_Air[i] = mat_ir[i];
				for (i = 0; i < nV + 1; i++) globalQP_Ajc[i] = mat_jc[i];

				/* copy values, too */
				globalQP_Av = new real_t[globalQP_Ajc[nV]];
				for (i = 0; i < (mwIndex)globalQP_Ajc[nV]; i++) globalQP_Av[i] = v[i];

				globalQP_A = new SparseMatrix(nC, nV, globalQP_Air, globalQP_Ajc, globalQP_Av);
			}
			else
			{
				/* Convert constraint matrix A from FORTRAN to C style
				* (not necessary for H as it should be symmetric!). */
				A_for = (real_t*) mxGetPr( prhs[3] );
				A_mem = new real_t[nC*nV];
				convertFortranToC( A_for,nV,nC, A_mem );
				globalQP_A = new DenseMatrix(nC, nV, nV, A_mem );
				globalQP_A->doFreeMemory();
			}
		}

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

		/* Call qpOASES */
		if ( globalQP == 0 )
			mexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );

		if ( ( (int)nV != globalQP->getNV( ) ) || ( (int)nC != globalQP->getNC( ) ) )
			mexErrMsgTxt( "ERROR (qpOASES): QP dimensions must be constant during a sequence!" );

		hotstartVM(	globalQP_H,g,globalQP_A,
					lb,ub,lbA,ubA,
					nWSRin,&options,
					nlhs,plhs
					);

		return;
	}
}
int main( )
{
	USING_NAMESPACE_QPOASES

	long i;
	int_t nWSR;
	real_t errP1, errP2, errP3, errD1, errD2, errD3, tic, toc;
	real_t *x1 = new real_t[180];
	real_t *y1 = new real_t[271];
	real_t *x2 = new real_t[180];
	real_t *y2 = new real_t[271];
	real_t *x3 = new real_t[180];
	real_t *y3 = new real_t[271];

	Options options;
	options.setToDefault();
	options.enableEqualities = BT_TRUE;

	/* create sparse matrices */
	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
	SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);

	H->createDiagInfo();

	real_t* H_full = H->full();
	real_t* A_full = A->full();

	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
	DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);

	/* solve with dense matrices */
	nWSR = 1000;
	QProblem qrecipeD(180, 91);
	qrecipeD.setOptions(options);
	tic = getCPUtime();
	qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0);
	toc = getCPUtime();
	qrecipeD.getPrimalSolution(x1);
	qrecipeD.getDualSolution(y1);

	fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", (int)nWSR, toc-tic);

	/* solve with sparse matrices (nullspace factorization) */
	nWSR = 1000;
	QProblem qrecipeS(180, 91);
	qrecipeS.setOptions(options);
	tic = getCPUtime();
	qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
	toc = getCPUtime();
	qrecipeS.getPrimalSolution(x2);
	qrecipeS.getDualSolution(y2);

	fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", (int)nWSR, toc-tic);

	/* solve with sparse matrices (Schur complement) */
	#ifndef SOLVER_NONE
	nWSR = 1000;
	SQProblemSchur qrecipeSchur(180, 91);
	qrecipeSchur.setOptions(options);
	tic = getCPUtime();
	qrecipeSchur.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
	toc = getCPUtime();
	qrecipeSchur.getPrimalSolution(x3);
	qrecipeSchur.getDualSolution(y3);

	fprintf(stdFile, "Solved sparse problem (Schur complement approach) in %d iterations, %.3f seconds.\n", (int)nWSR, toc-tic);
	#endif /* SOLVER_NONE */

	/* check distance of solutions */
	errP1 = 0.0;
	errP2 = 0.0;
	errP3 = 0.0;
	#ifndef SOLVER_NONE
	for (i = 0; i < 180; i++)
		if (getAbs(x1[i] - x2[i]) > errP1)
			errP1 = getAbs(x1[i] - x2[i]);
	for (i = 0; i < 180; i++)
		if (getAbs(x1[i] - x3[i]) > errP2)
			errP2 = getAbs(x1[i] - x3[i]);
	for (i = 0; i < 180; i++)
		if (getAbs(x2[i] - x3[i]) > errP3)
			errP3 = getAbs(x2[i] - x3[i]);
	#endif /* SOLVER_NONE */
	fprintf(stdFile, "Primal error (dense and sparse): %9.2e\n", errP1);
	fprintf(stdFile, "Primal error (dense and Schur):  %9.2e\n", errP2);
	fprintf(stdFile, "Primal error (sparse and Schur): %9.2e\n", errP3);

	errD1 = 0.0;
	errD2 = 0.0;
	errD3 = 0.0;
	for (i = 0; i < 271; i++)
		if (getAbs(y1[i] - y2[i]) > errD1)
			errD1 = getAbs(y1[i] - y2[i]);
	#ifndef SOLVER_NONE
	for (i = 0; i < 271; i++)
		if (getAbs(y1[i] - y3[i]) > errD2)
			errD2 = getAbs(y1[i] - y3[i]);
	for (i = 0; i < 271; i++)
		if (getAbs(y2[i] - y3[i]) > errD3)
			errD3 = getAbs(y2[i] - y3[i]);
	#endif /* SOLVER_NONE */
	fprintf(stdFile, "Dual error (dense and sparse): %9.2e  (might not be unique)\n", errD1);
	fprintf(stdFile, "Dual error (dense and Schur):  %9.2e  (might not be unique)\n", errD2);
	fprintf(stdFile, "Dual error (sparse and Schur): %9.2e  (might not be unique)\n", errD3);

	delete H;
	delete A;
	delete[] H_full;
	delete[] A_full;
	delete Hd;
	delete Ad;

	delete[] y3;
	delete[] x3;
	delete[] y2;
	delete[] x2;
	delete[] y1;
	delete[] x1;

	QPOASES_TEST_FOR_TOL( errP1,1e-13 );
	QPOASES_TEST_FOR_TOL( errP2,1e-13 );
	QPOASES_TEST_FOR_TOL( errP3,1e-13 );

	return TEST_PASSED;
}