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