OptimalControlProblem::OptimalControlProblem(DynamicsModel *d) {
#if defined(NMPC_INTEGRATOR_RK4)
integrator = IntegratorRK4();
#elif defined(NMPC_INTEGRATOR_HEUN)
integrator = IntegratorHeun();
#elif defined(NMPC_INTEGRATOR_EULER)
integrator = IntegratorEuler();
#endif
dynamics = d;
/* Initialise inequality constraints to +/-infinity. */
lower_state_bound = StateConstraintVector::Ones() * -NMPC_INFTY;
upper_state_bound = StateConstraintVector::Ones() * NMPC_INFTY;
lower_control_bound = ControlConstraintVector::Ones() * -NMPC_INFTY;
upper_control_bound = ControlConstraintVector::Ones() * NMPC_INFTY;
/* Initialise weight matrices. */
state_weights = StateWeightMatrix::Identity();
control_weights = ControlWeightMatrix::Identity();
terminal_weights = StateWeightMatrix::Identity();
qp_options = qpDUNES_setupDefaultOptions();
qp_options.maxIter = 5;
qp_options.printLevel = 0;
qp_options.stationarityTolerance = 1e-3;
}
int main( )
{
int iter;
int i, k;
return_t statusFlag;
/** number of MPC simulation steps */
const unsigned int nSteps = 20;
/** noise settings */
double maxNoise[] = { 1.e-2, 0.e-4 };
srand( (unsigned)time( 0 ) ); /* init pseudo-random seed */
/** timings */
double t, tMeas;
double tPrepTtl = 0.;
double tSolTtl = 0.;
double tPrepMax = 0.;
double tSolMax = 0.;
/** problem dimensions */
const unsigned int nI = 200; /* number of control intervals */
const int nX = 2; /* number of states */
const int nU = 1; /* number of controls */
const unsigned int nZ = nX+nU; /* number of stage variables */
unsigned int* nD = 0; /* number of constraints */
/** problem data */
double dt = 0.01; /* discretization sampling time 10ms, needed for constraints */
const double H[] =
{ 1.0e-4, 0.0, 0.0,
0.0, 1.0e-4, 0.0,
0.0, 0.0, 1.0
};
const double P[] =
{ 1.0e-4, 0.0,
0.0, 1.0e-4
};
const double C[] =
{
1.0, 1.0*dt, 0.0,
0.0, 1.0, 1.0*dt
};
const double c[] =
{ 0.0,
0.0
};
const double ziLow[3] =
{ -1.9, -3.0, -30.0 };
const double ziUpp[3] =
{ 1.9, 3.0, 30.0 };
double ziRef[3] =
{ -0.0, 0.0, 0.0 };
/** build up bounds and reference vectors */
double zLow[nZ*nI+nX];
double zUpp[nZ*nI+nX];
double zRef[nZ*nI+nX];
for ( k=0; k<nI; ++k ) {
for ( i=0; i<nZ; ++i ) {
zLow[k*nZ+i] = ziLow[i];
zUpp[k*nZ+i] = ziUpp[i];
zRef[k*nZ+i] = ziRef[i];
}
}
for ( i=0; i<nX; ++i ) {
zLow[nI*nZ+i] = ziLow[i];
zUpp[nI*nZ+i] = ziUpp[i];
zRef[nI*nZ+i] = ziRef[i];
}
/* arrival constraints */
int idxArrivalStart = 50; /* 44 is the minimum number for this constraint that is still feasible */
k = idxArrivalStart+0;
zLow[k*nZ+0] = ziRef[0];
zUpp[k*nZ+0] = ziRef[0];
k = idxArrivalStart+1;
zLow[k*nZ+0] = ziRef[0];
zUpp[k*nZ+0] = ziRef[0];
/** initial value */
double x0[] =
{ -1.0, 0.0 };
/** simulation variables */
double xLog[nX*(nSteps+1)];
double uLog[nX*nSteps];
/** set up a new mpcDUNES problem */
printf( "Solving double integrator MPC problem [nI = %d, nX = %d, nU = %d]\n", nI, nX, nU );
mpcProblem_t mpcProblem;
qpOptions_t qpOptions = qpDUNES_setupDefaultOptions();
/** set qpDUNES options */
qpOptions.maxIter = 30;
qpOptions.printLevel = 2;
qpOptions.stationarityTolerance = 1.e-6;
qpOptions.regParam = 1.e-6;
qpOptions.newtonHessDiagRegTolerance = 1.e-9;
qpOptions.lsType = QPDUNES_LS_ACCELERATED_GRADIENT_BISECTION_LS;
qpOptions.lineSearchReductionFactor = 0.1;
qpOptions.regType = QPDUNES_REG_SINGULAR_DIRECTIONS;
/** setup qpDUNES internal data */
statusFlag = mpcDUNES_setup( &mpcProblem, nI, nX, nU, nD, &(qpOptions) );
if (statusFlag != QPDUNES_OK)
{
printf("Setup of the QP solver failed\n");
return (int)statusFlag;
}
t = getTime();
statusFlag = mpcDUNES_initLtiSb( &mpcProblem, H, P, 0, C, c, zLow, zUpp, zRef ); /** initialize MPC problem; note that matrices are factorized here */
if (statusFlag != QPDUNES_OK)
{
printf("Initialization of the MPC problem failed\n");
return (int)statusFlag;
}
tMeas = getTime() - t;
tPrepTtl += tMeas;
if (tMeas > tPrepMax) tPrepMax = tMeas;
/** MAIN MPC SIMULATION LOOP */
for ( iter=0; iter<nSteps; ++iter ) {
/** solve QP for current initial value */
t = getTime();
statusFlag = mpcDUNES_solve( &mpcProblem, x0 );
if (statusFlag != QPDUNES_SUCC_OPTIMAL_SOLUTION_FOUND)
{
printf("QP solver failed. The error code is: %d\n", statusFlag);
return (int)statusFlag;
}
tMeas = getTime() - t;
tSolTtl += tMeas;
if (tMeas > tSolMax) tSolMax = tMeas;
/** save x0, u0 */
for (i=0; i<nX; ++i) {
xLog[iter*nX+i] = x0[i];
}
for (i=0; i<nU; ++i) {
uLog[iter*nU+i] = mpcProblem.uOpt[0*nU+i];
}
/** prepare QP for next solution: put new data in second but last interval */
t = getTime();
statusFlag = qpDUNES_updateIntervalData( &(mpcProblem.qpData), mpcProblem.qpData.intervals[nI-1], 0, 0, 0, 0, ziLow,ziUpp, 0, 0,0, 0 ); /* H, C, c do not need to be updated b/c LTI */
if (statusFlag != QPDUNES_OK)
{
printf("Update of interval data failed\n");
return (int)statusFlag;
}
tMeas = getTime() - t;
tPrepTtl += tMeas;
if (tMeas > tPrepMax) tPrepMax = tMeas;
/** simulate next initial value */
for (i=0; i<nX; ++i) {
x0[i] = mpcProblem.xOpt[1*nX+i] + maxNoise[i] * (double)rand()/RAND_MAX;
printf( "x0[%d]: % .5e -- with noise: % .5e\n", i, mpcProblem.xOpt[1*nX+i], x0[i] );
}
}
/* save last state of simulation */
for (i=0; i<nX; ++i) {
xLog[nSteps*nX+i] = x0[i];
}
/** print information */
printf( "xLog, uLog:\n[" );
for(k=0; k<nSteps; ++k) {
printf( "[% .12e % .12e % .12e]\n", xLog[k*nX+0], xLog[k*nX+1], uLog[k*nU+0] );
}
printf( "[% .12e % .12e % .12e]]\n\n", xLog[nSteps*nX+0], xLog[nSteps*nX+1], 0. );
#ifdef __MEASURE_TIMINGS__
printf( "Computation times Maximum Average Total \n" );
printf( "----------------- -------- -------- --------\n" );
printf( "Preparation %5.2lf ms %5.2lf ms %5.2lf ms\n", 1e3*tPrepMax, 1e3*tPrepTtl/(nSteps+1), 1e3*tPrepTtl );
printf( "Solution %5.2lf ms %5.2lf ms %5.2lf ms\n", 1e3*tSolMax, 1e3*tSolTtl/nSteps, 1e3*tSolTtl );
#endif /* __MEASURE_TIMINGS__ */
/** cleanup of allocated data */
mpcDUNES_cleanup( &mpcProblem );
return 0;
}
int main( )
{
int i;
int k;
boolean_t isLTI;
unsigned int nI = 30;
unsigned int nX = 12;
unsigned int nU = 3;
unsigned int* nD = 0;
// unsigned int nD[30+1] =
// { 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, /* 10 */
// 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, /* 10 */
// 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, /* 10 */
// 12
// };
double x0[12] =
{ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
double Q[12*12] =
{ 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0
};
double R[3*3] =
{ 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
};
double *S=0;
double* P = Q;
double A[12*12] = /** problematric that only 4 digits precision?? */
// { 0.7627, 0.1149, 0.0025, 0.0000, 0.0000, 0.0000, 0.4596, 0.0198, 0.0003, 0.0000, 0.0000, 0.0000,
// 0.1149, 0.7652, 0.1149, 0.0025, 0.0000, 0.0000, 0.0198, 0.4599, 0.0198, 0.0003, 0.0000, 0.0000,
// 0.0025, 0.1149, 0.7652, 0.1149, 0.0025, 0.0000, 0.0003, 0.0198, 0.4599, 0.0198, 0.0003, 0.0000,
// 0.0000, 0.0025, 0.1149, 0.7652, 0.1149, 0.0025, 0.0000, 0.0003, 0.0198, 0.4599, 0.0198, 0.0003,
// 0.0000, 0.0000, 0.0025, 0.1149, 0.7652, 0.1149, 0.0000, 0.0000, 0.0003, 0.0198, 0.4599, 0.0198,
// 0.0000, 0.0000, 0.0000, 0.0025, 0.1149, 0.7627, 0.0000, 0.0000, 0.0000, 0.0003, 0.0198, 0.4596,
// -0.8994, 0.4202, 0.0193, 0.0002, 0.0000, 0.0000, 0.7627, 0.1149, 0.0025, 0.0000, 0.0000, 0.0000,
// 0.4202, -0.8801, 0.4205, 0.0193, 0.0002, 0.0000, 0.1149, 0.7652, 0.1149, 0.0025, 0.0000, 0.0000,
// 0.0193, 0.4205, -0.8801, 0.4205, 0.0193, 0.0002, 0.0025, 0.1149, 0.7652, 0.1149, 0.0025, 0.0000,
// 0.0002, 0.0193, 0.4205, -0.8801, 0.4205, 0.0193, 0.0000, 0.0025, 0.1149, 0.7652, 0.1149, 0.0025,
// 0.0000, 0.0002, 0.0193, 0.4205, -0.8801, 0.4202, 0.0000, 0.0000, 0.0025, 0.1149, 0.7652, 0.1149,
// 0.0000, 0.0000, 0.0002, 0.0193, 0.4202, -0.8994, 0.0000, 0.0000, 0.0000, 0.0025, 0.1149, 0.7627
// };
{ 7.6272104759e-01,1.1488254659e-01,2.4765447407e-03,2.0938074941e-05,9.4222941754e-08,2.6306013529e-10,4.5961393973e-01,1.9813111713e-02,2.5126005603e-04,1.5075750676e-06,5.2612302474e-09,1.1999300634e-11,
1.1488254659e-01,7.6519759233e-01,1.1490348467e-01,2.4766389636e-03,2.0938338001e-05,9.4222941754e-08,1.9813111713e-02,4.5986519978e-01,1.9814619288e-02,2.5126531726e-04,1.5075870669e-06,5.2612302474e-09,
2.4765447407e-03,1.1490348467e-01,7.6519768656e-01,1.1490348493e-01,2.4766389636e-03,2.0938074941e-05,2.5126005603e-04,1.9814619288e-02,4.5986520504e-01,1.9814619300e-02,2.5126531726e-04,1.5075750676e-06,
2.0938074941e-05,2.4766389636e-03,1.1490348493e-01,7.6519768656e-01,1.1490348467e-01,2.4765447407e-03,1.5075750676e-06,2.5126531726e-04,1.9814619300e-02,4.5986520504e-01,1.9814619288e-02,2.5126005603e-04,
9.4222941754e-08,2.0938338001e-05,2.4766389636e-03,1.1490348467e-01,7.6519759233e-01,1.1488254659e-01,5.2612302474e-09,1.5075870669e-06,2.5126531726e-04,1.9814619288e-02,4.5986519978e-01,1.9813111713e-02,
2.6306013529e-10,9.4222941754e-08,2.0938074941e-05,2.4765447407e-03,1.1488254659e-01,7.6272104759e-01,1.1999300634e-11,5.2612302474e-09,1.5075750676e-06,2.5126005603e-04,1.9813111713e-02,4.5961393973e-01,
-8.9941476774e-01,4.2023897636e-01,1.9312099176e-02,2.4825016712e-04,1.4970646064e-06,5.2372316461e-09,7.6272104759e-01,1.1488254659e-01,2.4765447407e-03,2.0938074941e-05,9.4222941754e-08,2.6306013529e-10,
4.2023897636e-01,-8.8010266857e-01,4.2048722652e-01,1.9313596240e-02,2.4825540436e-04,1.4970646064e-06,1.1488254659e-01,7.6519759233e-01,1.1490348467e-01,2.4766389636e-03,2.0938338001e-05,9.4222941754e-08,
1.9312099176e-02,4.2048722652e-01,-8.8010117150e-01,4.2048723176e-01,1.9313596240e-02,2.4825016712e-04,2.4765447407e-03,1.1490348467e-01,7.6519768656e-01,1.1490348493e-01,2.4766389636e-03,2.0938074941e-05,
2.4825016712e-04,1.9313596240e-02,4.2048723176e-01,-8.8010117150e-01,4.2048722652e-01,1.9312099176e-02,2.0938074941e-05,2.4766389636e-03,1.1490348493e-01,7.6519768656e-01,1.1490348467e-01,2.4765447407e-03,
1.4970646064e-06,2.4825540436e-04,1.9313596240e-02,4.2048722652e-01,-8.8010266857e-01,4.2023897636e-01,9.4222941754e-08,2.0938338001e-05,2.4766389636e-03,1.1490348467e-01,7.6519759233e-01,1.1488254659e-01,
5.2372316461e-09,1.4970646064e-06,2.4825016712e-04,1.9312099176e-02,4.2023897636e-01,-8.9941476774e-01,2.6306013529e-10,9.4222941754e-08,2.0938074941e-05,2.4765447407e-03,1.1488254659e-01,7.6272104759e-01
};
double B[12*3] =
// { 0.1174, 0.0000, 0.0000,
// -0.1174, 0.0025, 0.0000,
// -0.0025, 0.1199, 0.0025,
// -0.0000, 0.0000, 0.1199,
// -0.0000, -0.1199, 0.0000,
// -0.0000, -0.0025, -0.1199,
// 0.4398, 0.0003, 0.0000,
// -0.4401, 0.0198, 0.0003,
// -0.0196, 0.4596, 0.0198,
// -0.0002, 0.0000, 0.4596,
// -0.0000, -0.4596, 0.0000,
// -0.0000, -0.0198, -0.4594,
// };
{
1.1738012390e-01,2.1127047947e-05,9.4750064792e-08,
-1.1740125121e-01,2.5187046136e-03,2.1127312010e-05,
-2.4976720527e-03,1.1989882851e-01,2.5187046146e-03,
-2.1032825507e-05,5.0104061525e-13,1.1989882877e-01,
-9.4487004629e-08,-1.1989882825e-01,9.4750566331e-08,
-2.6356150520e-10,-2.5186098636e-03,-1.1987770120e-01,
4.3980082801e-01,2.5125479480e-04,1.5075630683e-06,
-4.4005208807e-01,1.9813111701e-02,2.5126005603e-04,
-1.9563359232e-02,4.5961393973e-01,1.9813111725e-02,
-2.4975774219e-04,1.1999307797e-11,4.5961394499e-01,
-1.5023258367e-06,-4.5961393447e-01,1.5075750676e-06,
-5.2492309467e-09,-1.9811604138e-02,-4.5936267967e-01,
};
double c[12] =
{ 0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0 };
double xLow[12] =
{ -4, -4, -4, -4, -4, -4, -4, -4, -4, -4, -4, -4 };
double xUpp[12] =
{ 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, };
double uLow[3] =
{ -0.5, -0.5, -0.5 };
double uUpp[3] =
{ 0.5, 0.5, 0.5 };
/* double *xRef=0, *uRef=0; */
qpData_t qpData;
qpOptions_t qpOptions = qpDUNES_setupDefaultOptions();
qpOptions.maxIter = 20;
qpOptions.printLevel = 1;
qpOptions.stationarityTolerance = 1.e-6;
qpOptions.equalityTolerance = 2.221e-16;
qpOptions.QPDUNES_ZERO = 1.0e-50;
qpOptions.QPDUNES_INFTY = INFTY;
qpOptions.maxNumLineSearchIterations = 4;
qpDUNES_setup( &qpData, nI, nX, nU, nD, &(qpOptions) );
double t1 = getTime();
return_t status;
for ( k=0; k<1; ++k ) {
for( i=0; i<nI; ++i )
{
// qpDUNES_setupSimpleBoundedInterval( &qpData, qpData.intervals[i],Q,R,S, A,B,c, xLow,xUpp,uLow,uUpp );
}
// qpDUNES_setupSimpleBoundedInterval( &qpData, qpData.intervals[nI], P,0,0, 0,0,0, xLow,xUpp,0,0 );
status = qpDUNES_setupAllLocalQPs( &qpData, isLTI=QPDUNES_FALSE ); /* determine local QP solvers and set up auxiliary data */
if (status != QPDUNES_OK) return 1;
// double t1 = getTime();
// for ( i=0; i<100; ++i ) {
// qpDUNES_solve( &qpData, x0 );
/* TODO: adapt to use MPC interface */
status = qpDUNES_solve( &qpData );
if (status != QPDUNES_OK) return 2;
}
double t2 = getTime();
// printf( "Computation time: %lf ms\n", 1e3*(t2-t1) );
printf( "Average computation time of 100 runs: %lf ms\n", 1e3*(t2-t1)/1 );
qpDUNES_cleanup( &qpData );
return 0;
}
/* ----------------------------------------------
* memory allocation
*
#>>>>>> */
return_t qpDUNES_setup( qpData_t* const qpData,
uint_t nI,
uint_t nX,
uint_t nU,
uint_t* nD,
qpOptions_t* options
)
{
uint_t ii, kk;
int_t nZ = nX+nU;
int_t nDttl = 0; /* total number of constraints */
/* set up options */
if (options != 0) {
qpData->options = *options;
}
else {
qpData->options = qpDUNES_setupDefaultOptions();
}
/* set up dimensions */
qpData->nI = nI;
qpData->nX = nX;
qpData->nU = nU;
qpData->nZ = nZ;
if (nD != 0) {
for( ii=0; ii<nI+1; ++ii ) {
nDttl += nD[ii];
}
}
qpData->nDttl = nDttl;
if (nDttl != 0) {
qpDUNES_printError( qpData, __FILE__, __LINE__, "Sorry, affine constraints are not yet supported." );
return QPDUNES_ERR_INVALID_ARGUMENT;
}
qpData->intervals = (interval_t**)qpDUNES_calloc( nI+1,sizeof(interval_t*) );
/* normal intervals */
for( ii=0; ii<nI; ++ii )
{
qpData->intervals[ii] = qpDUNES_allocInterval( qpData, nX, nU, nZ, ( (nD != 0) ? nD[ii] : 0 ) );
qpData->intervals[ii]->id = ii; /* give interval its initial stage index */
qpData->intervals[ii]->xVecTmp.data = (real_t*)qpDUNES_calloc( nX,sizeof(real_t) );
qpData->intervals[ii]->uVecTmp.data = (real_t*)qpDUNES_calloc( nU,sizeof(real_t) );
qpData->intervals[ii]->zVecTmp.data = (real_t*)qpDUNES_calloc( nZ,sizeof(real_t) );
}
/* last interval */
qpData->intervals[nI] = qpDUNES_allocInterval( qpData, nX, nU, nX, ( (nD != 0) ? nD[nI] : 0 ) );
qpData->intervals[nI]->id = nI; /* give interval its initial stage index */
qpDUNES_setMatrixNull( &( qpData->intervals[nI]->C ) );
qpDUNES_free( &(qpData->intervals[nI]->c.data) );
qpData->intervals[nI]->xVecTmp.data = (real_t*)qpDUNES_calloc( nX,sizeof(real_t) );
qpData->intervals[nI]->uVecTmp.data = (real_t*)qpDUNES_calloc( nU,sizeof(real_t) );
qpData->intervals[nI]->zVecTmp.data = (real_t*)qpDUNES_calloc( nZ,sizeof(real_t) );
/* undefined not-defined lambda parts */
/* do not free, memory might be needed for interval rotation in MPC context */
qpData->intervals[0]->lambdaK.isDefined = QPDUNES_FALSE;
qpData->intervals[nI]->lambdaK1.isDefined = QPDUNES_FALSE;
/* remainder of qpData struct */
qpData->lambda.data = (real_t*)qpDUNES_calloc( nX*nI,sizeof(real_t) );
qpData->deltaLambda.data = (real_t*)qpDUNES_calloc( nX*nI,sizeof(real_t) );
qpData->hessian.data = (real_t*)qpDUNES_calloc( (nX*2)*(nX*nI),sizeof(real_t) );
qpData->cholHessian.data = (real_t*)qpDUNES_calloc( (nX*2)*(nX*nI),sizeof(real_t) );
qpData->gradient.data = (real_t*)qpDUNES_calloc( nX*nI,sizeof(real_t) );
qpData->xVecTmp.data = (real_t*)qpDUNES_calloc( nX,sizeof(real_t) );
qpData->uVecTmp.data = (real_t*)qpDUNES_calloc( nU,sizeof(real_t) );
qpData->zVecTmp.data = (real_t*)qpDUNES_calloc( nZ,sizeof(real_t) );
qpData->xnVecTmp.data = (real_t*)qpDUNES_calloc( nX*nI,sizeof(real_t) );
qpData->xnVecTmp2.data = (real_t*)qpDUNES_calloc( nX*nI,sizeof(real_t) );
qpData->xxMatTmp.data = (real_t*)qpDUNES_calloc( nX*nX,sizeof(real_t) );
qpData->xxMatTmp2.data = (real_t*)qpDUNES_calloc( nX*nX,sizeof(real_t) );
qpData->xzMatTmp.data = (real_t*)qpDUNES_calloc( nX*nZ,sizeof(real_t) );
qpData->uxMatTmp.data = (real_t*)qpDUNES_calloc( nU*nX,sizeof(real_t) );
qpData->zxMatTmp.data = (real_t*)qpDUNES_calloc( nZ*nX,sizeof(real_t) );
qpData->zzMatTmp.data = (real_t*)qpDUNES_calloc( nZ*nZ,sizeof(real_t) );
qpData->zzMatTmp2.data = (real_t*)qpDUNES_calloc( nZ*nZ,sizeof(real_t) );
/* set incumbent objective function value to minus infinity */
qpData->optObjVal = -qpData->options.QPDUNES_INFTY;
/* Set up log struct */
if ( qpData->options.logLevel >= QPDUNES_LOG_ITERATIONS )
{
qpDUNES_setupLog( qpData );
qpData->log.itLog = (itLog_t*)qpDUNES_calloc( qpData->options.maxIter+1, sizeof(itLog_t) );
for( ii=0; ii<qpData->options.maxIter+1; ++ii ) {
qpData->log.itLog[ii].ieqStatus = (int_t**)qpDUNES_calloc( nI+1,sizeof(int_t*) );
for( kk=0; kk<nI+1; ++kk ) {
qpData->log.itLog[ii].ieqStatus[kk] = (int_t*)qpDUNES_calloc( ((nD != 0) ? nD[kk] : 0) + _NV(kk),sizeof(int_t) );
}
qpData->log.itLog[ii].prevIeqStatus = 0;
if ( qpData->options.logLevel == QPDUNES_LOG_ALL_DATA )
{
qpData->log.itLog[ii].regDirections.data = (real_t*)qpDUNES_calloc( nX*nI,sizeof(real_t) );
qpData->log.itLog[ii].lambda.data = (real_t*)qpDUNES_calloc( nX*nI,sizeof(real_t) );
qpData->log.itLog[ii].deltaLambda.data = (real_t*)qpDUNES_calloc( nX*nI,sizeof(real_t) );
qpData->log.itLog[ii].gradient.data = (real_t*)qpDUNES_calloc( nX*nI,sizeof(real_t) );
qpData->log.itLog[ii].hessian.data = (real_t*)qpDUNES_calloc( (nX*2)*(nX*nI),sizeof(real_t) );
qpData->log.itLog[ii].cholHessian.data = (real_t*)qpDUNES_calloc( (nX*2)*(nX*nI),sizeof(real_t) );
#if defined(__ANALYZE_FACTORIZATION__)
qpData->log.itLog[ii].invHessian.data = (real_t*)qpDUNES_calloc( (nX*nI)*(nX*nI),sizeof(real_t) );
#endif
qpData->log.itLog[ii].dz.data = (real_t*)qpDUNES_calloc( nI*nZ+nX,sizeof(real_t) );
qpData->log.itLog[ii].zUnconstrained.data = (real_t*)qpDUNES_calloc( nI*nZ+nX,sizeof(real_t) );
qpData->log.itLog[ii].z.data = (real_t*)qpDUNES_calloc( nI*nZ+nX,sizeof(real_t) );
qpData->log.itLog[ii].y.data = (real_t*)qpDUNES_calloc( 2*nZ + 2*nDttl,sizeof(real_t) );
/* TODO: make multiplier definition clean! */
}
}
/* memory for itLog[0].prevIeqStatus needed in any case to enable AS comparison between subsequently solved QPs */
qpData->log.itLog[0].prevIeqStatus = (int_t**)qpDUNES_calloc( nI+1,sizeof(int_t*) );
for( kk=0; kk<nI+1; ++kk ) {
qpData->log.itLog[0].prevIeqStatus[kk] = (int_t*)qpDUNES_calloc( ((nD != 0) ? nD[kk] : 0) + _NV(kk),sizeof(int_t) );
}
}
else {
/* allocate only memory to check active set changes */
/* TODO: even remove this when no printing */
qpData->log.itLog = (itLog_t*)qpDUNES_calloc( 1, sizeof(itLog_t) );
qpData->log.itLog[0].ieqStatus = (int_t**)qpDUNES_calloc( nI+1,sizeof(int_t*) );
qpData->log.itLog[0].prevIeqStatus = (int_t**)qpDUNES_calloc( nI+1,sizeof(int_t*) );
for( kk=0; kk<nI+1; ++kk ) {
qpData->log.itLog[0].ieqStatus[kk] = (int_t*)qpDUNES_calloc( ((nD != 0) ? nD[kk] : 0) + nZ,sizeof(int_t) );
qpData->log.itLog[0].prevIeqStatus[kk] = (int_t*)qpDUNES_calloc( ((nD != 0) ? nD[kk] : 0) + nZ,sizeof(int_t) );
}
}
/* reset current active set to force initial Hessian factorization */
qpDUNES_indicateDataChange( qpData );
qpDUNES_printHeader( qpData );
return QPDUNES_OK;
}
int main( )
{
return_t statusFlag;
int iter, ii, kk;
/** define problem dimensions */
const unsigned int nI = 3; /* number of control intervals */
const int nX = 2; /* number of states */
const int nU = 1; /* number of controls */
const unsigned int nZ = nX+nU; /* number of stage variables */
unsigned int nD[nI+1]; /* number of constraints */
for ( kk=0; kk<nI+1; ++kk ) {
// nD[kk] = 0;
nD[kk] = 1;
}
// nD[nI] = 0;
/** define problem data */
const double H[] =
{ 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
1.0, 0.0,
0.0, 1.0
};
const double* g = 0;
const double C[] =
{
1.0, 0.01, 0.0,
0.0, 1.0, 0.01,
1.0, 0.01, 0.0,
0.0, 1.0, 0.01,
1.0, 0.01, 0.0,
0.0, 1.0, 0.01
};
const double c[] =
{ 0.0,
0.0,
0.0,
0.0,
0.0,
0.0
};
const double zLow[] =
{ -1.9, -3.0, -30.0,
-1.9, -3.0, -30.0,
-1.9, -3.0, -30.0,
-1.9, -3.0
};
const double zUpp[] =
{ 1.9, 3.0, 30.0,
1.9, 3.0, 30.0,
1.9, 3.0, 30.0,
1.9, 3.0
};
const double D[] =
{ 0.0, 5.0, 1.0, /* some random constraint limiting the acceleration for high velocities */
0.0, 5.0, 1.0,
0.0, 5.0, 1.0,
0.0, 5.0
};
const double dLow[] =
{ -INFTY,
-INFTY,
-INFTY,
-INFTY
};
const double dUpp[] =
{ 5.e0,
5.e0,
5.e0,
5.e0
};
/** define simulation environment */
const unsigned int nSteps = 100; /* number of simulation steps */
// const unsigned int nSteps = 2; /* number of simulation steps */
double x0[] = { -1.0, 0.0 }; /* initial value */
double z0Low[nX+nU]; /* auxiliary variables */
double z0Upp[nX+nU];
double zOpt[nI*nZ+nX]; /* primal solution */
double lambdaOpt[nI*nX]; /* dual solution */
double muOpt[2*nI*nZ+2*nX];
double zLog[nSteps*nZ]; /* log for trajectory */
/** (1) set up a new qpDUNES problem */
qpData_t qpData;
/** (2) set qpDUNES options */
qpOptions_t qpOptions = qpDUNES_setupDefaultOptions();
qpOptions.maxIter = 100;
qpOptions.printLevel = 3;
qpOptions.stationarityTolerance = 1.e-6;
qpOptions.regParam = 1.e-8;
qpOptions.newtonHessDiagRegTolerance = 1.e-8;
// qpOptions.lsType = QPDUNES_LS_BACKTRACKING_LS;
// qpOptions.lsType = QPDUNES_LS_ACCELERATED_GRADIENT_BISECTION_LS;
qpOptions.lsType = QPDUNES_LS_HOMOTOPY_GRID_SEARCH;
qpOptions.lineSearchReductionFactor = 0.1;
qpOptions.lineSearchMaxStepSize = 1.;
qpOptions.maxNumLineSearchIterations = 25;
qpOptions.maxNumLineSearchRefinementIterations = 150;
qpOptions.regType = QPDUNES_REG_SINGULAR_DIRECTIONS;
/** (3) allocate data for qpDUNES and set options */
qpDUNES_setup( &qpData, nI, nX, nU, nD, &(qpOptions) );
/** (4) set sparsity of primal Hessian and local constraint matrix */
for ( kk=0; kk<nI+1; ++kk ) {
qpData.intervals[kk]->H.sparsityType = QPDUNES_DIAGONAL;
// qpData.intervals[kk]->D.sparsityType = QPDUNES_IDENTITY;
qpData.intervals[kk]->D.sparsityType = QPDUNES_DENSE;
}
/** (5) initial MPC data setup: components not given here are set to zero (if applicable)
* instead of passing g, D, zLow, zUpp, one can also just pass NULL pointers (0) */
statusFlag = qpDUNES_init( &qpData, H, g, C, c, zLow,zUpp, D,dLow,dUpp );
if (statusFlag != QPDUNES_OK) {
printf( "Data init failed.\n" );
return (int)statusFlag;
}
/** MAIN MPC SIMULATION LOOP */
for ( iter=0; iter<nSteps; ++iter ) {
/** (1) embed current initial value */
for ( ii=0; ii<nX; ++ii ) {
z0Low[ii] = x0[ii];
z0Upp[ii] = x0[ii];
}
for ( ii=nX; ii<nX+nU; ++ii ) {
z0Low[ii] = zLow[ii];
z0Upp[ii] = zUpp[ii];
}
statusFlag = qpDUNES_updateIntervalData( &qpData, qpData.intervals[0], 0, 0, 0, 0, z0Low,z0Upp, 0,0,0, 0 );
if (statusFlag != QPDUNES_OK) {
printf( "Initial value embedding failed.\n" );
return (int)statusFlag;
}
/** (2) solve QP */
statusFlag = qpDUNES_solve( &qpData );
if (statusFlag != QPDUNES_SUCC_OPTIMAL_SOLUTION_FOUND) {
printf( "QP solution %d failed.\n", iter );
return (int)statusFlag;
}
// up to here in fdb step
/** (3) obtain primal and dual optimal solution */
qpDUNES_getPrimalSol( &qpData, zOpt );
qpDUNES_getDualSol( &qpData, lambdaOpt, muOpt );
/// ...
/** (4) prepare QP for next solution */
/** new interface: combined shift and update functionality for safety */
/** mandatory: data update */
/// H = ...
/// g = ...
/// C = ...
/// c = ...
/// zLow = ...
/// zUpp = ...
/** (4a) EITHER: MPC style update for RTIs, including shift */
// statusFlag = qpDUNES_shiftAndUpdate( &qpData, H, g, C, c, zLow,zUpp, D,dLow,dUpp ); /* data update: components not given here keep their previous value */
/** (4b) OR: SQP style update, excluding shift */
statusFlag = qpDUNES_updateData( &qpData, H, g, C, c, zLow,zUpp, D,dLow,dUpp ); /* data update: components not given here keep their previous value */
if (statusFlag != QPDUNES_OK) {
printf( "Data update failed.\n" );
return (int)statusFlag;
}
// /** OLD INTERFACE */
// /** optional: data shift */
// qpDUNES_shiftLambda( &qpData ); /* shift multipliers */
// qpDUNES_shiftIntervals( &qpData ); /* shift intervals (particulary important when using qpOASES for underlying local QPs) */
// /* WARNING: currently a shift needs to be followed by a data update.
// * A pure LTV shift may lead to data inconsistencies, but is not
// * prohibited yet. In a further version shift and data update
// * functionality might be combined for increased user friendliness
// */
//
// statusFlag = qpDUNES_updateData( &qpData, H, g, C, c, zLow,zUpp, D,dLow,dUpp ); /* data update: components not given here keep their previous value */
// if (statusFlag != QPDUNES_OK) {
// printf( "Data update failed.\n" );
// return (int)statusFlag;
// }
/** (5) simulate next initial value */
for (ii=0; ii<nX; ++ii) {
/// x0 = ...
x0[ii] = zOpt[1*nZ+ii];
}
// optional: logging
for (ii=0; ii<nZ; ++ii) {
zLog[iter*nZ+ii] = zOpt[ii];
}
printf( "Done with QP %d.\n", iter );
}
// optional: printing
#ifdef __PRINTTRAJECTORY__
printf("\nPROCESS TRAJECTORY:\n");
for (iter=0; iter<nSteps; ++iter) {
printf("\nt%02d:\t", iter);
for (ii=0; ii<nZ; ++ii) {
printf("%+.3e\t", zLog[iter*nZ+ii]);
}
}
#endif
printf("\n\n");
/** mandatory: cleanup of allocated data */
qpDUNES_cleanup( &qpData );
return 0;
}
