/*
* r e a d O Q P d a t a
*/
returnValue readOQPdata( const char* path,
int& nQP, int& nV, int& nC, int& nEC,
real_t** H, real_t** g, real_t** A, real_t** lb, real_t** ub, real_t** lbA, real_t** ubA,
real_t** xOpt, real_t** yOpt, real_t** objOpt
)
{
char filename[160];
/* consistency check */
if ( ( H == 0 ) || ( g == 0 ) || ( lb == 0 ) || ( ub == 0 ) )
return THROWERROR( RET_INVALID_ARGUMENTS );
/* 1) Obtain OQP dimensions. */
if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
/* another consistency check */
if ( ( nC > 0 ) && ( ( A == 0 ) || ( lbA == 0 ) || ( ubA == 0 ) ) )
return THROWERROR( RET_FILEDATA_INCONSISTENT );
/* 2) Allocate memory and load OQP data: */
/* Hessian matrix */
*H = new real_t[nV*nV];
snprintf( filename,160,"%sH.oqp",path );
if ( readFromFile( *H,nV,nV,filename ) != SUCCESSFUL_RETURN )
{
delete[] *H;
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
/* gradient vector sequence */
*g = new real_t[nQP*nV];
snprintf( filename,160,"%sg.oqp",path );
if ( readFromFile( *g,nQP,nV,filename ) != SUCCESSFUL_RETURN )
{
delete[] *g; delete[] *H;
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
/* lower bound vector sequence */
*lb = new real_t[nQP*nV];
snprintf( filename,160,"%slb.oqp",path );
if ( readFromFile( *lb,nQP,nV,filename ) != SUCCESSFUL_RETURN )
{
delete[] *lb; delete[] *g; delete[] *H;
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
/* upper bound vector sequence */
*ub = new real_t[nQP*nV];
snprintf( filename,160,"%sub.oqp",path );
if ( readFromFile( *ub,nQP,nV,filename ) != SUCCESSFUL_RETURN )
{
delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
if ( nC > 0 )
{
/* Constraint matrix */
*A = new real_t[nC*nV];
snprintf( filename,160,"%sA.oqp",path );
if ( readFromFile( *A,nC,nV,filename ) != SUCCESSFUL_RETURN )
{
delete[] *A;
delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
/* lower constraints' bound vector sequence */
*lbA = new real_t[nQP*nC];
snprintf( filename,160,"%slbA.oqp",path );
if ( readFromFile( *lbA,nQP,nC,filename ) != SUCCESSFUL_RETURN )
{
delete[] *lbA; delete[] *A;
delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
/* upper constraints' bound vector sequence */
*ubA = new real_t[nQP*nC];
snprintf( filename,160,"%subA.oqp",path );
if ( readFromFile( *ubA,nQP,nC,filename ) != SUCCESSFUL_RETURN )
{
delete[] *ubA; delete[] *lbA; delete[] *A;
delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
}
else
{
*A = 0;
*lbA = 0;
*ubA = 0;
}
if ( xOpt != 0 )
{
/* primal solution vector sequence */
*xOpt = new real_t[nQP*nV];
snprintf( filename,160,"%sx_opt.oqp",path );
if ( readFromFile( *xOpt,nQP,nV,filename ) != SUCCESSFUL_RETURN )
{
delete[] xOpt;
if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
}
if ( yOpt != 0 )
{
/* dual solution vector sequence */
*yOpt = new real_t[nQP*(nV+nC)];
snprintf( filename,160,"%sy_opt.oqp",path );
if ( readFromFile( *yOpt,nQP,nV+nC,filename ) != SUCCESSFUL_RETURN )
{
delete[] yOpt;
if ( xOpt != 0 ) { delete[] xOpt; };
if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
}
if ( objOpt != 0 )
{
/* dual solution vector sequence */
*objOpt = new real_t[nQP];
snprintf( filename,160,"%sobj_opt.oqp",path );
if ( readFromFile( *objOpt,nQP,1,filename ) != SUCCESSFUL_RETURN )
{
delete[] objOpt;
if ( yOpt != 0 ) { delete[] yOpt; };
if ( xOpt != 0 ) { delete[] xOpt; };
if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
}
return SUCCESSFUL_RETURN;
}
/*
* r u n O Q P b e n c h m a r k
*/
returnValue runOQPbenchmark( const char* path, BooleanType isSparse, const Options& options,
int& nWSR, real_t& maxCPUtime,
real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
)
{
int nQP=0, nV=0, nC=0, nEC=0;
real_t *H=0, *g=0, *A=0, *lb=0, *ub=0, *lbA=0, *ubA=0;
returnValue returnvalue;
/* I) SETUP BENCHMARK: */
/* 1) Obtain QP sequence dimensions. */
if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_BENCHMARK_ABORTED );
/* 2) Read OQP benchmark data. */
if ( readOQPdata( path,
nQP,nV,nC,nEC,
&H,&g,&A,&lb,&ub,&lbA,&ubA,
0,0,0
) != SUCCESSFUL_RETURN )
{
return THROWERROR( RET_UNABLE_TO_READ_BENCHMARK );
}
/* II) SOLVE BENCHMARK */
if ( nC > 0 )
{
returnvalue = solveOQPbenchmark( nQP,nV,nC,nEC,
H,g,A,lb,ub,lbA,ubA,
isSparse,options,
nWSR,maxCPUtime,
maxStationarity,maxFeasibility,maxComplementarity
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
delete[] H; delete[] A;
delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] g;
return THROWERROR( returnvalue );
}
}
else
{
returnvalue = solveOQPbenchmark( nQP,nV,
H,g,lb,ub,
isSparse,options,
nWSR,maxCPUtime,
maxStationarity,maxFeasibility,maxComplementarity
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
delete[] H; delete[] A;
delete[] ub; delete[] lb; delete[] g;
return THROWERROR( returnvalue );
}
}
delete[] H; delete[] A;
delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] g;
return SUCCESSFUL_RETURN;
}
/*
* r u n O Q P b e n c h m a r k
*/
returnValue runOQPbenchmark( const char* path, BooleanType isSparse, BooleanType useHotstarts,
const Options& options, int maxAllowedNWSR,
real_t& maxNWSR, real_t& avgNWSR, real_t& maxCPUtime, real_t& avgCPUtime,
real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
)
{
int nQP=0, nV=0, nC=0, nEC=0;
real_t *H, *g, *A, *lb, *ub, *lbA, *ubA;
returnValue returnvalue;
/* I) SETUP BENCHMARK: */
/* 1) Obtain QP sequence dimensions. */
if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_BENCHMARK_ABORTED );
/* 2) Read OQP benchmark data. */
if ( readOQPdata( path,
nQP,nV,nC,nEC,
&H,&g,&A,&lb,&ub,&lbA,&ubA,
0,0,0
) != SUCCESSFUL_RETURN )
{
return THROWERROR( RET_UNABLE_TO_READ_BENCHMARK );
}
// normaliseConstraints( nV,nC,A,lbA,ubA ); //only works when nP==1
/* II) SOLVE BENCHMARK */
if ( nC > 0 )
{
returnvalue = solveOQPbenchmark( nQP,nV,nC,nEC,
H,g,A,lb,ub,lbA,ubA,
isSparse,useHotstarts,
options,maxAllowedNWSR,
maxNWSR,avgNWSR,maxCPUtime,avgCPUtime,
maxStationarity,maxFeasibility,maxComplementarity
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
delete[] H; delete[] A;
delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] g;
return THROWERROR( returnvalue );
}
}
else
{
returnvalue = solveOQPbenchmark( nQP,nV,
H,g,lb,ub,
isSparse,useHotstarts,
options,maxAllowedNWSR,
maxNWSR,avgNWSR,maxCPUtime,avgCPUtime,
maxStationarity,maxFeasibility,maxComplementarity
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
delete[] H; delete[] A;
delete[] ub; delete[] lb; delete[] g;
return THROWERROR( returnvalue );
}
}
delete[] H; delete[] A;
delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] g;
return SUCCESSFUL_RETURN;
}
/*
* r u n O Q P b e n c h m a r k
*/
returnValue runOQPbenchmark( const char* path,
int& nWSR, double& maxCPUtime,
double& maxPrimalDeviation, double& maxDualDeviation, double& maxObjDeviation
)
{
int nQP, nV, nC, nEC;
double *H, *g, *A, *lb, *ub, *lbA, *ubA;
double *xOpt, *yOpt, *objOpt;
/* I) SETUP BENCHMARK: */
/* 1) Obtain QP sequence dimensions. */
if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_BENCHMARK_ABORTED );
/* 2) Read OQP benchmark data. */
if ( readOQPdata( path,
nQP,nV,nC,nEC,
&H,&g,&A,&lb,&ub,&lbA,&ubA,
&xOpt,&yOpt,&objOpt
) != SUCCESSFUL_RETURN )
{
return THROWERROR( RET_UNABLE_TO_READ_BENCHMARK );
}
/* II) SOLVE BENCHMARK */
if ( nC > 0 )
{
if ( solveOQPbenchmark( nQP,nV,nC,nEC,
H,g,A,lb,ub,lbA,ubA,
xOpt,yOpt,objOpt,
nWSR,maxCPUtime,
maxPrimalDeviation,maxDualDeviation,maxObjDeviation
) != SUCCESSFUL_RETURN )
{
delete[] objOpt; delete[] yOpt; delete[] xOpt;
delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] A; delete[] g; delete[] H;
return THROWERROR( RET_BENCHMARK_ABORTED );
}
}
else
{
if ( solveOQPbenchmark( nQP,nV,
H,g,lb,ub,
xOpt,yOpt,objOpt,
nWSR,maxCPUtime,
maxPrimalDeviation,maxDualDeviation,maxObjDeviation
) != SUCCESSFUL_RETURN )
{
delete[] objOpt; delete[] yOpt; delete[] xOpt;
delete[] ub; delete[] lb; delete[] g; delete[] H;
return THROWERROR( RET_BENCHMARK_ABORTED );
}
}
delete[] objOpt; delete[] yOpt; delete[] xOpt;
delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] A; delete[] g; delete[] H;
return SUCCESSFUL_RETURN;
}
/** Example for qpOASES main function using the possibility to specify
* user-defined constraint product function. */
int main( )
{
int nQP, nV, nC, nEC;
real_t *H, *g, *A, *lb, *ub, *lbA, *ubA;
real_t cputime;
real_t xOpt[1000];
real_t yOpt[1000];
real_t xOptCP[1000+1000];
real_t yOptCP[1000+1000];
const char* path = "./cpp/data/oqp/chain80w/";
int k = 10; //th problem
if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
return TEST_DATA_NOT_FOUND;
readOQPdata( path, nQP,nV,nC,nEC,
&H,&g,&A,&lb,&ub,&lbA,&ubA,
0,0,0
);
Options myOptions;
//myOptions.setToMPC();
myOptions.printLevel = PL_LOW;
int nWSR = 500;
cputime = 20.0;
QProblem qp( nV,nC );
qp.setOptions( myOptions );
qp.init( H,&(g[k*nV]),A,&(lb[k*nV]),&(ub[k*nV]),&(lbA[k*nC]),&(ubA[k*nC]),nWSR,&cputime );
qp.getPrimalSolution( xOpt );
qp.getDualSolution( yOpt );
printf( "cputime without constraintProduct: %.3ems\n", cputime*1000.0 );
nWSR = 500;
cputime = 20.0;
MpcConstraintProduct myCP( nV,nC,2,A );
QProblem qpCP( nV,nC );
qpCP.setOptions( myOptions );
qpCP.setConstraintProduct( &myCP );
qpCP.init( H,&(g[k*nV]),A,&(lb[k*nV]),&(ub[k*nV]),&(lbA[k*nC]),&(ubA[k*nC]),nWSR,&cputime );
qpCP.getPrimalSolution( xOptCP );
qpCP.getDualSolution( yOptCP );
printf( "cputime with constraintProduct: %.3ems\n", cputime*1000.0 );
delete[] ubA;
delete[] lbA;
delete[] ub;
delete[] lb;
delete[] A;
delete[] g;
delete[] H;
for( int ii=0; ii<nV; ++ii )
QPOASES_TEST_FOR_NEAR( xOptCP[ii],xOpt[ii] );
for( int ii=0; ii<nV+nC; ++ii )
QPOASES_TEST_FOR_NEAR( yOptCP[ii],yOpt[ii] );
return 0;
}
/*
* r e a d O Q P d a t a
*/
returnValue readOQPdata( const char* path,
int* nQP, int* nV, int* nC, int* nEC,
real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
real_t* xOpt, real_t* yOpt, real_t* objOpt
)
{
char filename[QPOASES_MAX_STRING_LENGTH];
/* consistency check */
if ( ( H == 0 ) || ( g == 0 ) || ( lb == 0 ) || ( ub == 0 ) )
return THROWERROR( RET_INVALID_ARGUMENTS );
/* 1) Obtain OQP dimensions. */
if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
/* another consistency check */
if ( ( *nC > 0 ) && ( ( A == 0 ) || ( lbA == 0 ) || ( ubA == 0 ) ) )
return THROWERROR( RET_FILEDATA_INCONSISTENT );
/* 2) Allocate memory and load OQP data: */
/* Hessian matrix */
snprintf( filename,QPOASES_MAX_STRING_LENGTH,"%sH.oqp",path );
if ( qpOASES_readFromFileM( H,(*nV),(*nV),filename ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
/* gradient vector sequence */
snprintf( filename,QPOASES_MAX_STRING_LENGTH,"%sg.oqp",path );
if ( qpOASES_readFromFileM( g,(*nQP),(*nV),filename ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
/* lower bound vector sequence */
snprintf( filename,QPOASES_MAX_STRING_LENGTH,"%slb.oqp",path );
if ( qpOASES_readFromFileM( lb,(*nQP),(*nV),filename ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
/* upper bound vector sequence */
snprintf( filename,QPOASES_MAX_STRING_LENGTH,"%sub.oqp",path );
if ( qpOASES_readFromFileM( ub,(*nQP),(*nV),filename ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
if ( (*nC) > 0 )
{
/* Constraint matrix */
snprintf( filename,QPOASES_MAX_STRING_LENGTH,"%sA.oqp",path );
if ( qpOASES_readFromFileM( A,(*nC),(*nV),filename ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
/* lower constraints' bound vector sequence */
snprintf( filename,QPOASES_MAX_STRING_LENGTH,"%slbA.oqp",path );
if ( qpOASES_readFromFileM( lbA,(*nQP),(*nC),filename ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
/* upper constraints' bound vector sequence */
snprintf( filename,QPOASES_MAX_STRING_LENGTH,"%subA.oqp",path );
if ( qpOASES_readFromFileM( ubA,(*nQP),(*nC),filename ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
if ( xOpt != 0 )
{
/* primal solution vector sequence */
snprintf( filename,QPOASES_MAX_STRING_LENGTH,"%sx_opt.oqp",path );
if ( qpOASES_readFromFileM( xOpt,(*nQP),(*nV),filename ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
if ( yOpt != 0 )
{
/* dual solution vector sequence */
snprintf( filename,QPOASES_MAX_STRING_LENGTH,"%sy_opt.oqp",path );
if ( qpOASES_readFromFileM( yOpt,(*nQP),(*nV)+(*nC),filename ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
if ( objOpt != 0 )
{
/* dual solution vector sequence */
snprintf( filename,QPOASES_MAX_STRING_LENGTH,"%sobj_opt.oqp",path );
if ( qpOASES_readFromFileM( objOpt,(*nQP),1,filename ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNABLE_TO_READ_FILE );
}
return SUCCESSFUL_RETURN;
}
