/*
This step is the first part of the feedback step; the very latest sensor
measurement should be provided in order to to set up the initial state for
the SQP iteration. This allows the feedback delay to be significantly less
than one time step.
*/
void OptimalControlProblem::initial_constraint(StateVector measurement) {
real_t zLow[NMPC_GRADIENT_DIM];
Eigen::Map<GradientVector> zLow_map(zLow);
real_t zUpp[NMPC_GRADIENT_DIM];
Eigen::Map<GradientVector> zUpp_map(zUpp);
/* Control constraints are unchanged. */
zLow_map.segment<NMPC_CONTROL_DIM>(NMPC_DELTA_DIM) =
lower_control_bound - control_reference[0];
zUpp_map.segment<NMPC_CONTROL_DIM>(NMPC_DELTA_DIM) =
upper_control_bound - control_reference[0];
/*
Initial delta is constrained to be the difference between the measurement
and the initial state horizon point.
*/
DeltaVector initial_delta = state_to_delta(
state_reference[0],
measurement);
zLow_map.segment<NMPC_DELTA_DIM>(0) = initial_delta;
zUpp_map.segment<NMPC_DELTA_DIM>(0) = initial_delta;
return_t status_flag = qpDUNES_updateIntervalData(
&qp_data, qp_data.intervals[0],
0, 0, 0, 0, zLow, zUpp, 0, 0, 0, 0);
AssertOK(status_flag);
qpDUNES_indicateDataChange(&qp_data);
}
/* ----------------------------------------------
*
>>>>>> */
return_t qpDUNES_updateData( qpData_t* const qpData,
const real_t* const H_,
const real_t* const g_,
const real_t* const C_,
const real_t* const c_,
const real_t* const zLow_,
const real_t* const zUpp_,
const real_t* const D_,
const real_t* const dLow_,
const real_t* const dUpp_
)
{
int_t kk;
int_t nDoffset = 0;
/** setup regular intervals */
for( kk=0; kk<_NI_; ++kk )
{
qpDUNES_updateIntervalData( qpData, qpData->intervals[kk],
offsetArray(H_, kk*_NZ_*_NZ_), offsetArray(g_, kk*_NZ_),
offsetArray(C_, kk*_NX_*_NZ_), offsetArray(c_, kk*_NX_),
offsetArray(zLow_, kk*_NZ_), offsetArray(zUpp_, kk*_NZ_),
offsetArray(D_, nDoffset*_NZ_), offsetArray(dLow_, nDoffset), offsetArray(dUpp_, nDoffset),
0 );
nDoffset += qpData->intervals[kk]->nD;
}
/** set up final interval */
qpDUNES_updateIntervalData( qpData, qpData->intervals[_NI_],
offsetArray(H_, _NI_*_NZ_*_NZ_), offsetArray(g_, _NI_*_NZ_),
0, 0,
offsetArray(zLow_, _NI_*_NZ_), offsetArray(zUpp_, _NI_*_NZ_),
offsetArray(D_, nDoffset*_NZ_), offsetArray(dLow_, nDoffset), offsetArray(dUpp_, nDoffset),
0 );
/* reset current active set to force Hessian refactorization
* (needed if matrix data entering the Newton Hessian has changed) */
if ( (H_ !=0) || (C_ != 0) || (D_ != 0) ) {
qpDUNES_indicateDataChange(qpData);
}
return QPDUNES_OK;
}
void OptimalControlProblem::set_reference_point(const ReferenceVector &in,
uint32_t i) {
state_reference[i] = in.segment<NMPC_STATE_DIM>(0);
if(i > 0 && i <= OCP_HORIZON_LENGTH) {
control_reference[i-1] =
in.segment<NMPC_CONTROL_DIM>(NMPC_STATE_DIM);
solve_ivps(i-1);
real_t g[NMPC_GRADIENT_DIM];
Eigen::Map<GradientVector> g_map(g);
real_t C[(NMPC_STATE_DIM-1)*NMPC_GRADIENT_DIM];
Eigen::Map<ContinuityConstraintMatrix> C_map(C);
real_t c[NMPC_DELTA_DIM];
Eigen::Map<DeltaVector> c_map(c);
real_t zLow[NMPC_GRADIENT_DIM];
Eigen::Map<GradientVector> zLow_map(zLow);
real_t zUpp[NMPC_GRADIENT_DIM];
Eigen::Map<GradientVector> zUpp_map(zUpp);
zLow_map.segment<NMPC_DELTA_DIM>(0) = lower_state_bound;
zUpp_map.segment<NMPC_DELTA_DIM>(0) = upper_state_bound;
return_t status_flag;
/* Gradient vector fixed to zero. */
g_map = GradientVector::Zero();
/* Continuity constraint constant term fixed to zero. */
c_map = integration_residuals[i-1];
/* Copy the relevant data into the qpDUNES arrays. */
C_map = jacobians[i-1];
zLow_map.segment<NMPC_CONTROL_DIM>(NMPC_DELTA_DIM) =
lower_control_bound - control_reference[i-1];
zUpp_map.segment<NMPC_CONTROL_DIM>(NMPC_DELTA_DIM) =
upper_control_bound - control_reference[i-1];
status_flag = qpDUNES_updateIntervalData(
&qp_data, qp_data.intervals[i-1],
0, g, C, c, zLow, zUpp, 0, 0, 0, 0);
AssertOK(status_flag);
qpDUNES_indicateDataChange(&qp_data);
}
}
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( )
{
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;
}
