/* ----------------------------------------------
*
>>>>>> */
return_t qpDUNES_init( 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;
boolean_t isLTI = QPDUNES_FALSE; /* todo: auto-detect, or specify through interface! */
/** set up regular intervals */
for( kk=0; kk<_NI_; ++kk )
{
qpDUNES_setupRegularInterval( qpData, qpData->intervals[kk],
offsetArray(H_, kk*_NZ_*_NZ_), 0, 0, 0, offsetArray(g_, kk*_NZ_),
offsetArray(C_, kk*_NX_*_NZ_), 0, 0, offsetArray(c_, kk*_NX_),
offsetArray(zLow_, kk*_NZ_), offsetArray(zUpp_, kk*_NZ_), 0, 0, 0, 0,
offsetArray(D_, nDoffset*_NZ_), offsetArray(dLow_, nDoffset), offsetArray(dUpp_, nDoffset) );
nDoffset += qpData->intervals[kk]->nD;
}
/** set up final interval */
qpDUNES_setupFinalInterval( qpData, qpData->intervals[_NI_],
offsetArray(H_, _NI_*_NZ_*_NZ_), offsetArray(g_, _NI_*_NZ_),
offsetArray(zLow_, _NI_*_NZ_), offsetArray(zUpp_, _NI_*_NZ_),
offsetArray(D_, nDoffset*_NZ_), offsetArray(dLow_, nDoffset), offsetArray(dUpp_, nDoffset) );
/** determine local QP solvers and set up auxiliary data */
qpDUNES_setupAllLocalQPs( qpData, isLTI );
/* reset current active set to force Hessian refactorization (needed due to data change) */
qpDUNES_indicateDataChange( qpData );
return QPDUNES_OK;
}
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;
}
/*
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);
}
