Planning: OpenSpace: use upper limit in ipopt

modules/planning/conf/planner_open_space_config.pb.txt

@@ -32,7 +32,6 @@ distance_approach_config : {
   weight_time : 0.5
   weight_time : 1
   min_safety_distance : 0.0
-  max_safety_distance : 1.0e2
   max_steer_angle : 0.6
   max_steer_rate : 0.6
   max_speed_forward : 2.0
@@ -41,7 +40,5 @@ distance_approach_config : {
   max_acceleration_reverse : 1.0
   min_time_sample_scaling : 0.1
   max_time_sample_scaling : 2.0
-  max_miu : 1.5
-  max_lambda : 2.0
   use_fix_time : false
 }

modules/planning/open_space/distance_approach_ipopt_interface.cc

@@ -83,7 +83,6 @@ DistanceApproachIPOPTInterface::DistanceApproachIPOPTInterface(
   weight_first_order_time_ = distance_approach_config_.weight_time(0);
   weight_second_order_time_ = distance_approach_config_.weight_time(1);
   min_safety_distance_ = distance_approach_config_.min_safety_distance();
-  max_safety_distance_ = distance_approach_config_.max_safety_distance();
   max_steer_angle_ = distance_approach_config_.max_steer_angle();
   max_speed_forward_ = distance_approach_config_.max_speed_forward();
   max_speed_reverse_ = distance_approach_config_.max_speed_reverse();
@@ -96,8 +95,6 @@ DistanceApproachIPOPTInterface::DistanceApproachIPOPTInterface(
   max_time_sample_scaling_ =
       distance_approach_config_.max_time_sample_scaling();
   max_steer_rate_ = distance_approach_config_.max_steer_rate();
-  max_lambda_ = distance_approach_config_.max_lambda();
-  max_miu_ = distance_approach_config_.max_miu();
   use_fix_time_ = distance_approach_config_.use_fix_time();
   wheelbase_ = vehicle_param_.wheel_base();
 }
@@ -188,8 +185,8 @@ bool DistanceApproachIPOPTInterface::get_bounds_info(int n, double* x_l,
     x_u[variable_index + 1] = XYbounds_[3];
 
     // phi
-    x_l[variable_index + 2] = -M_PI;
-    x_u[variable_index + 2] = M_PI;
+    x_l[variable_index + 2] = -2e19;
+    x_u[variable_index + 2] = 2e19;
 
     // v
     x_l[variable_index + 3] = -max_speed_reverse_;
@@ -241,7 +238,7 @@ bool DistanceApproachIPOPTInterface::get_bounds_info(int n, double* x_l,
   for (int i = 0; i < horizon_ + 1; ++i) {
     for (int j = 0; j < obstacles_edges_sum_; ++j) {
       x_l[variable_index] = 0.0;
-      x_u[variable_index] = max_lambda_;
+      x_u[variable_index] = 2e19;  // nlp_upper_bound_limit
       ++variable_index;
     }
   }
@@ -251,7 +248,7 @@ bool DistanceApproachIPOPTInterface::get_bounds_info(int n, double* x_l,
   for (int i = 0; i < horizon_ + 1; ++i) {
     for (int j = 0; j < 4 * obstacles_num_; ++j) {
       x_l[variable_index] = 0.0;
-      x_u[variable_index] = max_miu_;
+      x_u[variable_index] = 2e19;  // nlp_upper_bound_limit
 
       ++variable_index;
     }
@@ -309,7 +306,7 @@ bool DistanceApproachIPOPTInterface::get_bounds_info(int n, double* x_l,
 
       // c. -g'*mu + (A*t - b)*lambda > min_safety_distance_
       g_l[constraint_index + 3] = min_safety_distance_;
-      g_u[constraint_index + 3] = max_safety_distance_;
+      g_u[constraint_index + 3] = 2e19;  // nlp_upper_bound_limit
       constraint_index += 4;
     }
   }
@@ -1204,16 +1201,16 @@ bool DistanceApproachIPOPTInterface::eval_obj(int n, const T* x, T* obj_value) {
     T x3_diff = x[state_index + 2] - xWS_(2, i);
     T x4_abs = x[state_index + 3];
     *obj_value += weight_state_x_ * x1_diff * x1_diff +
-                 weight_state_y_ * x2_diff * x2_diff +
-                 weight_state_phi_ * x3_diff * x3_diff +
-                 weight_state_v_ * x4_abs * x4_abs;
+                  weight_state_y_ * x2_diff * x2_diff +
+                  weight_state_phi_ * x3_diff * x3_diff +
+                  weight_state_v_ * x4_abs * x4_abs;
     state_index += 4;
   }
 
   // 2. objective to minimize u square
   for (int i = 0; i < horizon_; ++i) {
     *obj_value += weight_input_steer_ * x[control_index] * x[control_index] +
-                 weight_input_a_ * x[control_index + 1] * x[control_index + 1];
+                  weight_input_a_ * x[control_index + 1] * x[control_index + 1];
     control_index += 2;
   }
 
@@ -1235,7 +1232,7 @@ bool DistanceApproachIPOPTInterface::eval_obj(int n, const T* x, T* obj_value) {
     T a_rate =
         (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
     *obj_value += weight_rate_steer_ * steering_rate * steering_rate +
-                 weight_rate_a_ * a_rate * a_rate;
+                  weight_rate_a_ * a_rate * a_rate;
     control_index += 2;
     time_index += 1;
   }

modules/planning/open_space/distance_approach_problem.cc

@@ -60,7 +60,7 @@ bool DistanceApproachProblem::Solve(
   // app->Options()->SetStringValue("derivative_test", "first-order");
   // app->Options()->SetNumericValue("derivative_test_tol", 1.0e-3);
   // TODO(QiL) : Change IPOPT settings to flag or configs
-  app->Options()->SetIntegerValue("print_level", 0);
+  app->Options()->SetIntegerValue("print_level", 5);
   app->Options()->SetIntegerValue("mumps_mem_percent", 6000);
   app->Options()->SetNumericValue("mumps_pivtol", 1e-6);
   app->Options()->SetIntegerValue("max_iter", 1000);

modules/planning/open_space/dual_variable_warm_start_ipopt_interface.cc

@@ -33,14 +33,12 @@ namespace apollo {
 namespace planning {
 
 DualVariableWarmStartIPOPTInterface::DualVariableWarmStartIPOPTInterface(
-    int num_of_variables, int num_of_constraints, size_t horizon, float ts,
-    const Eigen::MatrixXd& ego, const Eigen::MatrixXi& obstacles_edges_num,
-    const size_t obstacles_num, const Eigen::MatrixXd& obstacles_A,
-    const Eigen::MatrixXd& obstacles_b, const Eigen::MatrixXd& xWS,
+    size_t horizon, float ts, const Eigen::MatrixXd& ego,
+    const Eigen::MatrixXi& obstacles_edges_num, const size_t obstacles_num,
+    const Eigen::MatrixXd& obstacles_A, const Eigen::MatrixXd& obstacles_b,
+    const Eigen::MatrixXd& xWS,
     const PlannerOpenSpaceConfig& planner_open_space_config)
-    : num_of_variables_(num_of_variables),
-      num_of_constraints_(num_of_constraints),
-      ts_(ts),
+    : ts_(ts),
       ego_(ego),
       obstacles_edges_num_(obstacles_edges_num),
       obstacles_A_(obstacles_A),
@@ -69,6 +67,22 @@ DualVariableWarmStartIPOPTInterface::DualVariableWarmStartIPOPTInterface(
 bool DualVariableWarmStartIPOPTInterface::get_nlp_info(
     int& n, int& m, int& nnz_jac_g, int& nnz_h_lag,
     IndexStyleEnum& index_style) {
+  // n1 : lagrangian multiplier associated with obstacleShape
+  int n1 = obstacles_edges_sum_ * (horizon_ + 1);
+
+  // n2 : lagrangian multipier associated with car shape, obstacles_num*4 *
+  // (N+1)
+  int n2 = obstacles_num_ * 4 * (horizon_ + 1);
+
+  // n3 : dual variable, obstacles_num * (N+1)
+  int n3 = obstacles_num_ * (horizon_ + 1);
+
+  // m1 : obstacle constraints
+  int m1 = 4 * obstacles_num_ * (horizon_ + 1);
+
+  num_of_variables_ = n1 + n2 + n3;
+  num_of_constraints_ = m1;
+
   // number of variables
   n = num_of_variables_;
 
@@ -152,7 +166,7 @@ bool DualVariableWarmStartIPOPTInterface::get_bounds_info(int n, double* x_l,
   for (int i = 0; i < horizon_ + 1; ++i) {
     for (int j = 0; j < obstacles_edges_sum_; ++j) {
       x_l[variable_index] = 0.0;
-      x_u[variable_index] = 10.0;
+      x_u[variable_index] = 2e19;
       ++variable_index;
     }
   }
@@ -162,7 +176,7 @@ bool DualVariableWarmStartIPOPTInterface::get_bounds_info(int n, double* x_l,
   for (int i = 0; i < horizon_ + 1; ++i) {
     for (int j = 0; j < 4 * obstacles_num_; ++j) {
       x_l[variable_index] = 0.0;
-      x_u[variable_index] = 10.0;
+      x_u[variable_index] = 2e19;  // nlp_upper_bound_limit
       ++variable_index;
     }
   }
@@ -173,7 +187,7 @@ bool DualVariableWarmStartIPOPTInterface::get_bounds_info(int n, double* x_l,
     for (int j = 0; j < obstacles_num_; ++j) {
       // TODO(QiL): Load this from configuration
       x_l[variable_index] = 0.0;
-      x_u[variable_index] = 10.0;
+      x_u[variable_index] = 2e19;  // nlp_upper_bound_limit
       ++variable_index;
     }
   }

modules/planning/open_space/dual_variable_warm_start_ipopt_interface.h

@@ -44,8 +44,7 @@ namespace planning {
 class DualVariableWarmStartIPOPTInterface : public Ipopt::TNLP {
  public:
   explicit DualVariableWarmStartIPOPTInterface(
-      int num_of_variables, int num_of_constraints, std::size_t horizon,
-      float ts, const Eigen::MatrixXd& ego,
+      std::size_t horizon, float ts, const Eigen::MatrixXd& ego,
       const Eigen::MatrixXi& obstacles_edges_num,
       const std::size_t obstacles_num, const Eigen::MatrixXd& obstacles_A,
       const Eigen::MatrixXd& obstacles_b, const Eigen::MatrixXd& xWS,

modules/planning/open_space/dual_variable_warm_start_ipopt_interface_test.cc

@@ -67,15 +67,8 @@ class DualVariableWarmStartIPOPTInterfaceTest : public ::testing::Test {
 
 void DualVariableWarmStartIPOPTInterfaceTest::ProblemSetup() {
   obstacles_edges_num_ = 4 * Eigen::MatrixXi::Ones(obstacles_num_, 1);
-
-  num_of_variables_ = obstacles_edges_num_.sum() * (horizon_ + 1) +
-                      4 * obstacles_num_ * (horizon_ + 1) +
-                      obstacles_num_ * (horizon_ + 1);
-
-  num_of_constraints_ = 4 * obstacles_num_ * (horizon_ + 1);
   Eigen::MatrixXd xWS = Eigen::MatrixXd::Ones(4, horizon_ + 1);
-  ptop_.reset(new DualVariableWarmStartIPOPTInterface(
-      num_of_variables_, num_of_constraints_, horizon_, ts_, ego_,
+  ptop_.reset(new DualVariableWarmStartIPOPTInterface(horizon_, ts_, ego_,
       obstacles_edges_num_, obstacles_num_, obstacles_A_, obstacles_b_, xWS,
       planner_open_space_config_));
 }

modules/planning/open_space/dual_variable_warm_start_problem.cc

@@ -46,28 +46,11 @@ bool DualVariableWarmStartProblem::Solve(
     const Eigen::MatrixXd& obstacles_A, const Eigen::MatrixXd& obstacles_b,
     const Eigen::MatrixXd& xWS, Eigen::MatrixXd* l_warm_up,
     Eigen::MatrixXd* n_warm_up) {
-  // n1 : lagrangian multiplier associated with obstacleShape
-  int n1 = obstacles_edges_num.sum() * (horizon + 1);
-
-  // n2 : lagrangian multipier associated with car shape, obstacles_num*4 *
-  // (N+1)
-  int n2 = obstacles_num * 4 * (horizon + 1);
-
-  // n3 : dual variable, obstacles_num * (N+1)
-  int n3 = obstacles_num * (horizon + 1);
-
-  // m1 : obstacle constraints
-  int m1 = 4 * obstacles_num * (horizon + 1);
-
-  int num_of_variables = n1 + n2 + n3;
-  int num_of_constraints = m1;
-
   auto t_start = cyber::Time::Now().ToSecond();
   DualVariableWarmStartIPOPTInterface* ptop =
       new DualVariableWarmStartIPOPTInterface(
-          num_of_variables, num_of_constraints, horizon, ts, ego,
-          obstacles_edges_num, obstacles_num, obstacles_A, obstacles_b, xWS,
-          planner_open_space_config_);
+          horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
+          obstacles_b, xWS, planner_open_space_config_);
 
   Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;
 
@@ -78,7 +61,7 @@ bool DualVariableWarmStartProblem::Solve(
   // TODO(QiL) : Change IPOPT settings to flag or configs
   // app->Options()->SetStringValue("derivative_test", "first-order");
   // app->Options()->SetNumericValue("derivative_test_tol", 1.0e-3);
-  int print_level = 0;
+  int print_level = 5;
   app->Options()->SetIntegerValue("print_level", print_level);
   int num_iterations = 0;
   app->Options()->SetIntegerValue("max_iter", num_iterations);

modules/planning/proto/planner_open_space_config.proto

@@ -49,8 +49,5 @@ message DistanceApproachConfig {
   optional double max_acceleration_reverse = 12 [default = 2.0];
   optional double min_time_sample_scaling = 13 [default = 0.1];
   optional double max_time_sample_scaling = 14 [default = 10.0];
-  optional double max_miu = 15 [default = 1.0e10];
-  optional double max_lambda = 16 [default = 0.2];
-  optional double max_safety_distance = 17 [default = 1.0e10];
   optional bool use_fix_time = 18 [default = false];
 }