构建Q:
参见公式11 。
void PiecewiseJerkPathProblem::CalculateOffset(std::vector<c_float>* q) {CHECK_NOTNULL(q);const int n = static_cast<int>(num_of_knots_);const int kNumParam = 3 * n;q->resize(kNumParam, 0.0);if (has_x_ref_) {for (int i = 0; i < n; ++i) {q->at(i) += -2.0 * weight_x_ref_vec_.at(i) * x_ref_[i] / scale_factor_[0];}}if (has_end_state_ref_) {q->at(n - 1) +=-2.0 * weight_end_state_[0] * end_state_ref_[0] / scale_factor_[0];q->at(2 * n - 1) +=-2.0 * weight_end_state_[1] * end_state_ref_[1] / scale_factor_[1];q->at(3 * n - 1) +=-2.0 * weight_end_state_[2] * end_state_ref_[2] / scale_factor_[2];}} 构建A:
void PiecewiseJerkProblem::CalculateAffineConstraint(std::vector<c_float>* A_data, std::vector<c_int>* A_indices,std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds,std::vector<c_float>* upper_bounds) {// 3N params bounds on x, x', x''// 3(N-1) constraints on x, x', x''// 3 constraints on x_init_const int n = static_cast<int>(num_of_knots_);const int num_of_variables = 3 * n;const int num_of_constraints = num_of_variables + 3 * (n - 1) + 3;lower_bounds->resize(num_of_constraints);upper_bounds->resize(num_of_constraints);std::vector<std::vector<std::pair<c_int, c_float>>> variables(num_of_variables);int constraint_index = 0;// set x, x', x'' bounds对应公式23的前3n行for (int i = 0; i < num_of_variables; ++i) {if (i < n) {variables[i].emplace_back(constraint_index, 1.0);lower_bounds->at(constraint_index) =x_bounds_[i].first * scale_factor_[0];upper_bounds->at(constraint_index) =x_bounds_[i].second * scale_factor_[0];} else if (i < 2 * n) {variables[i].emplace_back(constraint_index, 1.0);lower_bounds->at(constraint_index) =dx_bounds_[i - n].first * scale_factor_[1];upper_bounds->at(constraint_index) =dx_bounds_[i - n].second * scale_factor_[1];} else {variables[i].emplace_back(constraint_index, 1.0);lower_bounds->at(constraint_index) =ddx_bounds_[i - 2 * n].first * scale_factor_[2];upper_bounds->at(constraint_index) =ddx_bounds_[i - 2 * n].second * scale_factor_[2];}++constraint_index;}CHECK_EQ(constraint_index, num_of_variables);// x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s对应公式12for (int i = 0; i + 1 < n; ++i) {variables[2 * n + i].emplace_back(constraint_index, -1.0);variables[2 * n + i + 1].emplace_back(constraint_index, 1.0);lower_bounds->at(constraint_index) =dddx_bound_.first * delta_s_ * scale_factor_[2];upper_bounds->at(constraint_index) =dddx_bound_.second * delta_s_ * scale_factor_[2];++constraint_index;}// x(i+1)' - x(i)' - 0.5 * delta_s * x(i)'' - 0.5 * delta_s * x(i+1)'' = 0对应公式17for (int i = 0; i + 1 < n; ++i) {variables[n + i].emplace_back(constraint_index, -1.0 * scale_factor_[2]);variables[n + i + 1].emplace_back(constraint_index, 1.0 * scale_factor_[2]);variables[2 * n + i].emplace_back(constraint_index,-0.5 * delta_s_ * scale_factor_[1]);variables[2 * n + i + 1].emplace_back(constraint_index,-0.5 * delta_s_ * scale_factor_[1]);lower_bounds->at(constraint_index) = 0.0;upper_bounds->at(constraint_index) = 0.0;++constraint_index;}// x(i+1) - x(i) - delta_s * x(i)'// - 1/3 * delta_s^2 * x(i)'' - 1/6 * delta_s^2 * x(i+1)''对应公式16auto delta_s_sq_ = delta_s_ * delta_s_;for (int i = 0; i + 1 < n; ++i) {variables[i].emplace_back(constraint_index,-1.0 * scale_factor_[1] * scale_factor_[2]);variables[i + 1].emplace_back(constraint_index,1.0 * scale_factor_[1] * scale_factor_[2]);variables[n + i].emplace_back(constraint_index, -delta_s_ * scale_factor_[0] * scale_factor_[2]);variables[2 * n + i].emplace_back(constraint_index,-delta_s_sq_ / 3.0 * scale_factor_[0] * scale_factor_[1]);variables[2 * n + i + 1].emplace_back(constraint_index,-delta_s_sq_ / 6.0 * scale_factor_[0] * scale_factor_[1]);lower_bounds->at(constraint_index) = 0.0;upper_bounds->at(constraint_index) = 0.0;++constraint_index;}// constrain on x_initvariables[0].emplace_back(constraint_index, 1.0);lower_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];upper_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];++constraint_index;variables[n].emplace_back(constraint_index, 1.0);lower_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];upper_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];++constraint_index;variables[2 * n].emplace_back(constraint_index, 1.0);lower_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];upper_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];++constraint_index;CHECK_EQ(constraint_index, num_of_constraints);int ind_p = 0;for (int i = 0; i < num_of_variables; ++i) {A_indptr->push_back(ind_p);for (const auto& variable_nz : variables[i]) {// coefficientA_data->push_back(variable_nz.second);// constraint indexA_indices->push_back(variable_nz.first);++ind_p;}}// We indeed need this line because of// https://github.com/oxfordcontrol/osqp/blob/master/src/cs.c#L255A_indptr->push_back(ind_p);}