diff --git a/modules/planning/tasks/optimizers/piecewise_jerk_speed/BUILD b/modules/planning/tasks/optimizers/piecewise_jerk_speed/BUILD index 5beccc38ebd..96e5a96b23a 100644 --- a/modules/planning/tasks/optimizers/piecewise_jerk_speed/BUILD +++ b/modules/planning/tasks/optimizers/piecewise_jerk_speed/BUILD @@ -38,6 +38,7 @@ cc_library( "//modules/planning/common:st_graph_data", "//modules/planning/common/path:path_data", "//modules/planning/common/trajectory1d:piecewise_jerk_trajectory1d", + "//modules/planning/math/piecewise_jerk:piecewise_jerk_speed_problem", "//modules/planning/math/piecewise_jerk:piecewise_jerk_path_problem", "//modules/planning/proto:ipopt_return_status_proto", "//modules/planning/tasks/optimizers:speed_optimizer", diff --git a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.cc b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.cc index 4aee220c309..d20160454e3 100644 --- a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.cc +++ b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.cc @@ -34,6 +34,8 @@ #include "modules/planning/common/planning_gflags.h" #include "modules/planning/common/speed_profile_generator.h" #include "modules/planning/common/st_graph_data.h" +#include "modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.h" +#include "modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.h" #include "modules/planning/proto/ipopt_return_status.pb.h" #include "modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_ipopt_interface.h" @@ -120,6 +122,16 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::Process( s_bounds.emplace_back(s_lower_bound, s_upper_bound); } + // Check s bound feasibility + if (!CheckStBoundFeasibility(num_of_knots, delta_t, + {s_init, s_dot_init, s_ddot_init}, s_bounds)) { + std::string msg( + "Piecewise jerk speed nonlinear optimizer feasibility check failed!"); + AERROR << msg; + speed_data->clear(); + return Status(ErrorCode::PLANNING_ERROR, msg); + } + // Set s_dot bounary const double max_velocity = std::fmax(FLAGS_planning_upper_speed_limit, st_graph_data.init_point().v()); @@ -381,5 +393,40 @@ PiecewiseJerkSpeedNonlinearOptimizer::SmoothPathCurvature( return smooth_curvature; } + +bool PiecewiseJerkSpeedNonlinearOptimizer::CheckStBoundFeasibility( + const int num_of_knots, const double delta_t, + const std::array& init_s, + const std::vector>& s_bounds) { + const auto start_timestamp = std::chrono::system_clock::now(); + + PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots, delta_t, + init_s); + piecewise_jerk_problem.set_dx_bounds( + 0.0, std::fmax(FLAGS_planning_upper_speed_limit, init_s[1])); + // TODO(Hongyi): delete this when ready to use vehicle_params + piecewise_jerk_problem.set_ddx_bounds(-4.0, 2.0); + piecewise_jerk_problem.set_dddx_bound(FLAGS_longitudinal_jerk_lower_bound, + FLAGS_longitudinal_jerk_upper_bound); + piecewise_jerk_problem.set_x_bounds(s_bounds); + + // Set objective to be zeros to only utilize feasibility check feature + piecewise_jerk_problem.set_weight_x(0.0); + piecewise_jerk_problem.set_weight_dx(0.0); + piecewise_jerk_problem.set_weight_ddx(0.0); + piecewise_jerk_problem.set_weight_dddx(0.0); + + // Solve the problem + auto res = piecewise_jerk_problem.Optimize(); + + const auto end_timestamp = std::chrono::system_clock::now(); + + std::chrono::duration time_diff_ms = end_timestamp - start_timestamp; + + ADEBUG << " feasibility_check takes : " << time_diff_ms.count() * 1000.0 + << " ms."; + + return res; +} } // namespace planning } // namespace apollo diff --git a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.h b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.h index b3c24618955..30a001bde0a 100644 --- a/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.h +++ b/modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.h @@ -20,8 +20,10 @@ #pragma once +#include +#include + #include "modules/planning/common/trajectory1d/piecewise_jerk_trajectory1d.h" -#include "modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.h" #include "modules/planning/tasks/optimizers/speed_optimizer.h" namespace apollo { @@ -41,6 +43,11 @@ class PiecewiseJerkSpeedNonlinearOptimizer : public SpeedOptimizer { PiecewiseJerkTrajectory1d SmoothSpeedLimit(const SpeedLimit& speed_limit); PiecewiseJerkTrajectory1d SmoothPathCurvature(const PathData& path_data); + + bool CheckStBoundFeasibility( + const int num_of_knots, const double delta_t, + const std::array& init_s, + const std::vector>& s_bounds); }; } // namespace planning