/*
Uses all of the information calculated so far to set up the various qpDUNES
datastructures in preparation for the feedback step.
This is really inefficient right now – there's heaps of probably unnecessary
copying going on.
*/
void OptimalControlProblem::initialise_qp() {
uint32_t i;
real_t Q[NMPC_DELTA_DIM*NMPC_DELTA_DIM];
Eigen::Map<StateWeightMatrix> Q_map(Q);
real_t R[NMPC_CONTROL_DIM*NMPC_CONTROL_DIM];
Eigen::Map<ControlWeightMatrix> R_map(R);
real_t P[NMPC_DELTA_DIM*NMPC_DELTA_DIM];
Eigen::Map<StateWeightMatrix> P_map(P);
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;
/* Set up problem dimensions. */
/* TODO: Determine number of affine constraints (D), and add them. */
qpDUNES_setup(
&qp_data,
OCP_HORIZON_LENGTH,
NMPC_DELTA_DIM,
NMPC_CONTROL_DIM,
0,
&qp_options);
return_t status_flag;
/* Gradient vector fixed to zero. */
g_map = GradientVector::Zero();
/* Continuity constraint constant term fixed to zero. */
c_map = DeltaVector::Zero();
/* Zero Jacobians for now */
C_map = ContinuityConstraintMatrix::Zero();
Q_map = state_weights;
R_map = control_weights;
/* Copy the relevant data into the qpDUNES arrays. */
zLow_map.segment<NMPC_CONTROL_DIM>(NMPC_DELTA_DIM) = lower_control_bound;
zUpp_map.segment<NMPC_CONTROL_DIM>(NMPC_DELTA_DIM) = upper_control_bound;
for(i = 0; i < OCP_HORIZON_LENGTH; i++) {
status_flag = qpDUNES_setupRegularInterval(
&qp_data, qp_data.intervals[i],
0, Q, R, 0, g, C, 0, 0, c, zLow, zUpp, 0, 0, 0, 0, 0, 0, 0);
AssertOK(status_flag);
}
/* Set up final interval. */
P_map = terminal_weights;
status_flag = qpDUNES_setupFinalInterval(&qp_data, qp_data.intervals[i],
P, g, zLow, zUpp, 0, 0, 0);
AssertOK(status_flag);
qpDUNES_setupAllLocalQPs(&qp_data, QPDUNES_FALSE);
qpDUNES_indicateDataChange(&qp_data);
}
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;
}
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;
}
