Planning: add st boundary feasibility check

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",

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<double, 3>& init_s,
+    const std::vector<std::pair<double, double>>& 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<double> time_diff_ms = end_timestamp - start_timestamp;
+
+  ADEBUG << " feasibility_check takes : " << time_diff_ms.count() * 1000.0
+         << " ms.";
+
+  return res;
+}
 }  // namespace planning
 }  // namespace apollo

modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.h

@@ -20,8 +20,10 @@
 
 #pragma once
 
+#include <utility>
+#include <vector>
+
 #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<double, 3>& init_s,
+      const std::vector<std::pair<double, double>>& s_bounds);
 };
 
 }  // namespace planning