/* ----------------------------------------------
*
>>>>>> */
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;
}
return_t qpDUNES_setupSimpleBoundedInterval( qpData_t* const qpData,
interval_t* interval,
const real_t* const Q,
const real_t* const R,
const real_t* const S,
const real_t* const A,
const real_t* const B,
const real_t* const c,
const real_t* const xLow,
const real_t* const xUpp,
const real_t* const uLow,
const real_t* const uUpp
)
{
if ( R != 0 ) {
return qpDUNES_setupRegularInterval( qpData, interval, 0, Q, R, S, 0, 0, A, B, c, 0, 0, xLow, xUpp, uLow, uUpp, 0, 0, 0 );
}
else { /* final interval */
return qpDUNES_setupFinalInterval( qpData, interval, Q, 0, xLow, xUpp, 0, 0, 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);
}
