/*
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);
}
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);
}
}
/* ----------------------------------------------
*
>>>>>> */
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_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;
}
/*
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);
}
/* ----------------------------------------------
* 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;
}
