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); } }
double ProblemDescription::evaluateCollisionFunction( const double * xi, double * g) { //TODO throw error. assert( collision_function ); prepareData( xi ); double value; if ( g ){ MatMap g_map( g, N(), M() ); value = collision_function->evaluate( trajectory, g_map ); if ( doing_covariant ){ metric.multiplyLowerInverse( g_map ); } }else { value = collision_function->evaluate( trajectory ); } return value; }
/* 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); }