diff --git a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_ipopt_interface.cc b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_ipopt_interface.cc index f64c50e9109f08fe717f66d60b1cc13d5da11abe..48f60277b1f62a35c3e3d9e94ca5f07562cac533 100644 --- a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_ipopt_interface.cc +++ b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_ipopt_interface.cc @@ -47,6 +47,16 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_nlp_info( int &n, int &m, int &nnz_jac_g, int &nnz_h_lag, IndexStyleEnum &index_style) { num_of_variables_ = num_of_points_ * 3; + + if (use_soft_safety_bound_) { + // complementary slack variable for soft upper and lower s bound + num_of_variables_ += num_of_points_ * 2; + + lower_s_slack_offset_ = num_of_points_ * 3; + + upper_s_slack_offset_ = num_of_points_ * 4; + } + n = num_of_variables_; // s monotone constraints s_i+1 - s_i >= 0.0 @@ -71,6 +81,15 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_nlp_info( num_of_constraints_ += num_of_points_; } + if (use_soft_safety_bound_) { + // soft safety boundary constraints + // s_i - soft_lower_s_i + lower_slack_i >= 0.0 + num_of_constraints_ += num_of_points_; + + // s_i - soft_upper_s_i - upper_slack_i <= 0.0 + num_of_constraints_ += num_of_points_; + } + m = num_of_constraints_; nnz_jac_g = 0; @@ -93,6 +112,15 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_nlp_info( nnz_jac_g += num_of_points_ * 2; } + if (use_soft_safety_bound_) { + // soft safety boundary constraints + // s_i - soft_lower_s_i + lower_slack_i >= 0.0 + nnz_jac_g += num_of_points_ * 2; + + // s_i - soft_upper_s_i - upper_slack_i <= 0.0 + nnz_jac_g += num_of_points_ * 2; + } + nnz_h_lag = num_of_points_ * 5 - 1; index_style = IndexStyleEnum::C_STYLE; @@ -131,6 +159,20 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_bounds_info( x_u[a_offset_ + i] = s_ddot_max_; } + if (use_soft_safety_bound_) { + // lower_s_slack + for (int i = 0; i < num_of_points_; ++i) { + x_l[lower_s_slack_offset_ + i] = 0.0; + x_u[lower_s_slack_offset_ + i] = INF; + } + + // upper_s_slack + for (int i = 0; i < num_of_points_; ++i) { + x_l[upper_s_slack_offset_ + i] = 0.0; + x_u[upper_s_slack_offset_ + i] = INF; + } + } + // bounds for constraints // s monotone constraints s_i+1 - s_i for (int i = 0; i + 1 < num_of_points_; ++i) { @@ -149,7 +191,7 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_bounds_info( // position equality constraints, // s_i+1 - s_i - s_dot_i * delta_t - 1/3 * s_ddot_i * delta_t^2 - 1/6 * // s_ddot_i+1 * delta_t^2 - offset = offset + num_of_points_ - 1; + offset += num_of_points_ - 1; for (int i = 0; i + 1 < num_of_points_; ++i) { g_l[offset + i] = 0.0; g_u[offset + i] = 0.0; @@ -158,7 +200,7 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_bounds_info( // velocity equality constraints, // s_dot_i+1 - s_dot_i - 0.5 * delta_t * s_ddot_i - 0.5 * delta_t * // s_ddot_i+1 - offset = offset + num_of_points_ - 1; + offset += num_of_points_ - 1; for (int i = 0; i + 1 < num_of_points_; ++i) { g_l[offset + i] = 0.0; g_u[offset + i] = 0.0; @@ -167,7 +209,24 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_bounds_info( if (use_v_bound_) { // speed limit constraints // s_dot_i - v_bound_func_(s_i) <= 0.0 - offset = offset + num_of_points_ - 1; + offset += num_of_points_ - 1; + for (int i = 0; i < num_of_points_; ++i) { + g_l[offset + i] = -INF; + g_u[offset + i] = 0.0; + } + } + + if (use_soft_safety_bound_) { + // soft_s_bound constraints + // s_i - soft_lower_s_i + lower_slack_i >= 0.0 + offset += num_of_points_; + for (int i = 0; i < num_of_points_; ++i) { + g_l[offset + i] = 0.0; + g_u[offset + i] = INF; + } + + // s_i - soft_upper_s_i - upper_slack_i <= 0.0 + offset += num_of_points_; for (int i = 0; i < num_of_points_; ++i) { g_l[offset + i] = -INF; g_u[offset + i] = 0.0; @@ -186,9 +245,17 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_starting_point( x[v_offset_ + i] = x_warm_start_[i][1]; x[a_offset_ + i] = x_warm_start_[i][2]; } - return true; } + if (use_soft_safety_bound_) { + for (int i = 0; i < num_of_points_; ++i) { + x[lower_s_slack_offset_ + i] = 0.0; + x[upper_s_slack_offset_ + i] = 0.0; + } + } + + return true; + // TODO(Jinyun): Implement better default warm start based on safety_bounds for (int i = 0; i < num_of_points_; ++i) { x[i] = std::min(5.0 * delta_t_ * i + s_init_, s_max_); @@ -205,6 +272,13 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_starting_point( } x[a_offset_] = s_ddot_init_; + if (use_soft_safety_bound_) { + for (int i = 0; i < num_of_points_; ++i) { + x[lower_s_slack_offset_ + i] = 0.0; + x[upper_s_slack_offset_ + i] = 0.0; + } + } + return true; } @@ -256,6 +330,13 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_f(int n, const double *x, obj_value += a_diff * a_diff * w_target_a_; } + if (use_soft_safety_bound_) { + for (int i = 0; i < num_of_points_; ++i) { + obj_value += x[lower_s_slack_offset_ + i] * w_soft_s_bound_; + obj_value += x[upper_s_slack_offset_ + i] * w_soft_s_bound_; + } + } + return true; } @@ -320,24 +401,35 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_grad_f(int n, grad_f[a_offset_ + num_of_points_ - 1] += 2.0 * a_diff * w_target_a_; } + if (use_soft_safety_bound_) { + for (int i = 0; i < num_of_points_; ++i) { + grad_f[lower_s_slack_offset_ + i] += w_soft_s_bound_; + grad_f[upper_s_slack_offset_ + i] += w_soft_s_bound_; + } + } + return true; } bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_g(int n, const double *x, bool new_x, int m, double *g) { + int offset = 0; + // s monotone constraints s_i+1 - s_i for (int i = 0; i + 1 < num_of_points_; ++i) { - g[i] = x[i + 1] - x[i]; + g[i] = x[offset + i + 1] - x[i]; } + offset += num_of_points_ - 1; + // jerk bound constraint, |s_ddot_i+1 - s_ddot_i| <= s_dddot_max // position equality constraints, // s_i+1 - s_i - s_dot_i * delta_t - 1/3 * s_ddot_i * delta_t^2 - 1/6 * // s_ddot_i+1 * delta_t^2 // velocity equality constraints // s_dot_i+1 - s_dot_i - 0.5 * delta_t * (s_ddot_i + s_ddot_i+1) - int coffset_jerk = num_of_points_ - 1; + int coffset_jerk = offset; int coffset_position = coffset_jerk + num_of_points_ - 1; int coffset_velocity = coffset_position + num_of_points_ - 1; @@ -363,15 +455,43 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_g(int n, const double *x, g[coffset_velocity + i] = v1 - (v0 + a0 * t + 0.5 * j * t2); } + offset += 3 * (num_of_points_ - 1); + if (use_v_bound_) { // speed limit constraints + int coffset_speed_limit = offset; // s_dot_i - v_bound_func_(s_i) <= 0.0 - int coffset_speedlimit = coffset_velocity + num_of_points_ - 1; for (int i = 0; i < num_of_points_; ++i) { double s = x[i]; double s_dot = x[v_offset_ + i]; - g[coffset_speedlimit + i] = s_dot - v_bound_func_.Evaluate(0, s); + g[coffset_speed_limit + i] = s_dot - v_bound_func_.Evaluate(0, s); + } + + offset += num_of_points_; + } + + if (use_soft_safety_bound_) { + // soft safety boundary constraints + int coffset_soft_lower_s = offset; + // s_i - soft_lower_s_i + lower_slack_i >= 0.0 + for (int i = 0; i < num_of_points_; ++i) { + double s = x[i]; + double lower_s_slack = x[lower_s_slack_offset_ + i]; + g[coffset_soft_lower_s + i] = + s - soft_safety_bounds_[i].first + lower_s_slack; + } + offset += num_of_points_; + + int coffset_soft_upper_s = offset; + // s_i - soft_upper_s_i - upper_slack_i <= 0.0 + for (int i = 0; i < num_of_points_; ++i) { + double s = x[i]; + double upper_s_slack = x[upper_s_slack_offset_ + i]; + g[coffset_soft_upper_s + i] = + s - soft_safety_bounds_[i].second - upper_s_slack; } + + offset += num_of_points_; } return true; @@ -380,10 +500,6 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_g(int n, const double *x, bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_jac_g( int n, const double *x, bool new_x, int m, int nele_jac, int *iRow, int *jCol, double *values) { - int s_offset = 0; - int v_offset = num_of_points_; - int a_offset = 2 * num_of_points_; - if (values == nullptr) { int non_zero_index = 0; int constraint_index = 0; @@ -392,12 +508,12 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_jac_g( for (int i = 0; i + 1 < num_of_points_; ++i) { // s_i iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = s_offset + i; + jCol[non_zero_index] = i; non_zero_index++; // s_i+1 iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = s_offset + i + 1; + jCol[non_zero_index] = i + 1; non_zero_index++; constraint_index++; @@ -407,12 +523,12 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_jac_g( for (int i = 0; i + 1 < num_of_points_; ++i) { // a_i iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = a_offset + i; + jCol[non_zero_index] = a_offset_ + i; non_zero_index++; // a_i+1 iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = a_offset + i + 1; + jCol[non_zero_index] = a_offset_ + i + 1; non_zero_index++; constraint_index++; @@ -422,27 +538,27 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_jac_g( for (int i = 0; i + 1 < num_of_points_; ++i) { // s_i iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = s_offset + i; + jCol[non_zero_index] = i; non_zero_index++; // v_i iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = v_offset + i; + jCol[non_zero_index] = v_offset_ + i; non_zero_index++; // a_i iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = a_offset + i; + jCol[non_zero_index] = a_offset_ + i; non_zero_index++; // s_i+1 iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = s_offset + i + 1; + jCol[non_zero_index] = i + 1; non_zero_index++; // a_i+1 iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = a_offset + i + 1; + jCol[non_zero_index] = a_offset_ + i + 1; non_zero_index++; constraint_index++; @@ -452,22 +568,22 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_jac_g( for (int i = 0; i + 1 < num_of_points_; ++i) { // v_i iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = v_offset + i; + jCol[non_zero_index] = v_offset_ + i; non_zero_index++; // a_i iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = a_offset + i; + jCol[non_zero_index] = a_offset_ + i; non_zero_index++; // v_i+1 iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = v_offset + i + 1; + jCol[non_zero_index] = v_offset_ + i + 1; non_zero_index++; // a_i+1 iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = a_offset + i + 1; + jCol[non_zero_index] = a_offset_ + i + 1; non_zero_index++; constraint_index++; @@ -479,17 +595,51 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_jac_g( for (int i = 0; i < num_of_points_; ++i) { // s_i iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = s_offset + i; + jCol[non_zero_index] = i; non_zero_index++; // v_i iRow[non_zero_index] = constraint_index; - jCol[non_zero_index] = v_offset + i; + jCol[non_zero_index] = v_offset_ + i; non_zero_index++; constraint_index++; } } + + if (use_soft_safety_bound_) { + // soft_s_bound constraints + // s_i - soft_lower_s_i + lower_slack_i >= 0.0 + for (int i = 0; i < num_of_points_; ++i) { + // s_i + iRow[non_zero_index] = constraint_index; + jCol[non_zero_index] = i; + non_zero_index++; + + // lower_slack_i + iRow[non_zero_index] = constraint_index; + jCol[non_zero_index] = lower_s_slack_offset_ + i; + non_zero_index++; + + constraint_index++; + } + // s_i - soft_upper_s_i - upper_slack_i <= 0.0 + + for (int i = 0; i < num_of_points_; ++i) { + // s_i + iRow[non_zero_index] = constraint_index; + jCol[non_zero_index] = i; + non_zero_index++; + + // upper_slack_i + iRow[non_zero_index] = constraint_index; + jCol[non_zero_index] = upper_s_slack_offset_ + i; + non_zero_index++; + + constraint_index++; + } + } + } else { int non_zero_index = 0; // s monotone constraints s_i+1 - s_i @@ -561,7 +711,7 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_jac_g( // s_dot_i - v_bound_func_(s_i) <= 0.0 for (int i = 0; i < num_of_points_; ++i) { // s_i - double s = x[s_offset + i]; + double s = x[i]; values[non_zero_index] = -1.0 * v_bound_func_.Evaluate(1, s); non_zero_index++; @@ -570,6 +720,31 @@ bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_jac_g( non_zero_index++; } } + + if (use_soft_safety_bound_) { + // soft_s_bound constraints + // s_i - soft_lower_s_i + lower_slack_i >= 0.0 + for (int i = 0; i < num_of_points_; ++i) { + // s_i + values[non_zero_index] = 1.0; + non_zero_index++; + + // lower_slack_i + values[non_zero_index] = 1.0; + non_zero_index++; + } + + // s_i - soft_upper_s_i - upper_slack_i <= 0.0 + for (int i = 0; i < num_of_points_; ++i) { + // s_i + values[non_zero_index] = 1.0; + non_zero_index++; + + // upper_slack_i + values[non_zero_index] = -1.0; + non_zero_index++; + } + } } return true; } @@ -748,12 +923,10 @@ void PiecewiseJerkSpeedNonlinearIpoptInterface::finalize_solution( opt_v_.clear(); opt_a_.clear(); - int v_offset = num_of_points_; - int a_offset = 2 * num_of_points_; for (int i = 0; i < num_of_points_; ++i) { auto s = x[i]; - auto v = x[v_offset + i]; - auto a = x[a_offset + i]; + auto v = x[v_offset_ + i]; + auto a = x[a_offset_ + i]; opt_s_.push_back(s); opt_v_.push_back(v); @@ -787,6 +960,12 @@ void PiecewiseJerkSpeedNonlinearIpoptInterface::set_safety_bounds( safety_bounds_ = safety_bounds; } +void PiecewiseJerkSpeedNonlinearIpoptInterface::set_soft_safety_bounds( + const std::vector> &soft_safety_bounds) { + soft_safety_bounds_ = soft_safety_bounds; + use_soft_safety_bound_ = true; +} + void PiecewiseJerkSpeedNonlinearIpoptInterface::set_s_max(const double s_max) { s_max_ = s_max; } @@ -836,6 +1015,11 @@ void PiecewiseJerkSpeedNonlinearIpoptInterface:: w_ref_s_ = w_ref_s; } +void PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_soft_s_bound( + const double w_soft_s_bound) { + w_soft_s_bound_ = w_soft_s_bound; +} + void PiecewiseJerkSpeedNonlinearIpoptInterface::set_warm_start( const std::vector> &speed_profile) { x_warm_start_ = speed_profile; diff --git a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_ipopt_interface.h b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_ipopt_interface.h index b46e041cbc466bb924fc3af39b8b25a21b599890..5c56d23e5b0e7160c81513b46d8b9c02114c330c 100644 --- a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_ipopt_interface.h +++ b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_ipopt_interface.h @@ -65,6 +65,9 @@ class PiecewiseJerkSpeedNonlinearIpoptInterface : public Ipopt::TNLP { void set_safety_bounds( const std::vector> &safety_bounds); + void set_soft_safety_bounds( + const std::vector> &soft_safety_bounds); + void set_s_max(const double s_max); void set_w_target_state(const double w_target_s, const double w_target_v, @@ -80,6 +83,8 @@ class PiecewiseJerkSpeedNonlinearIpoptInterface : public Ipopt::TNLP { void set_w_reference_spatial_distance(const double w_ref_s); + void set_w_soft_s_bound(const double w_soft_s_bound); + /** Method to return some info about the nlp */ bool get_nlp_info(int &n, int &m, int &nnz_jac_g, int &nnz_h_lag, IndexStyleEnum &index_style) override; @@ -136,6 +141,8 @@ class PiecewiseJerkSpeedNonlinearIpoptInterface : public Ipopt::TNLP { bool use_v_bound_ = false; + bool use_soft_safety_bound_ = false; + PiecewiseJerkTrajectory1d v_bound_func_; double s_max_ = 150.0; @@ -160,6 +167,10 @@ class PiecewiseJerkSpeedNonlinearIpoptInterface : public Ipopt::TNLP { const int a_offset_; + int lower_s_slack_offset_ = 0; + + int upper_s_slack_offset_ = 0; + int num_of_variables_ = 0; int num_of_constraints_ = 0; @@ -180,6 +191,8 @@ class PiecewiseJerkSpeedNonlinearIpoptInterface : public Ipopt::TNLP { double w_overall_centripetal_acc_ = 500.0; + double w_soft_s_bound_ = 0.0; + double v_max_ = 0.0; double s_target_ = 0.0; @@ -192,6 +205,8 @@ class PiecewiseJerkSpeedNonlinearIpoptInterface : public Ipopt::TNLP { std::vector> safety_bounds_; + std::vector> soft_safety_bounds_; + bool has_end_state_target_ = false; std::vector opt_s_;