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