Planning: added a range for high order params to speed up convergence.

modules/planning/math/smoothing_spline/spline_1d_generator.cc

@@ -213,6 +213,7 @@ bool Spline1dGenerator::Solve() {
   MatrixXd solved_params = MatrixXd::Zero(num_param, 1);
   for (int i = 0; i < num_param; ++i) {
     solved_params(i, 0) = result[i];
+    ADEBUG << "spline 1d solved param[" << i << "]: " << result[i];
   }
 
   last_num_param_ = num_param;

modules/planning/tasks/qp_spline_path/qp_spline_path_generator.cc

@@ -264,6 +264,21 @@ bool QpSplinePathGenerator::AddConstraint(const QpFrenetFrame& qp_frenet_frame,
   Spline1dConstraint* spline_constraint =
       spline_generator_->mutable_spline_constraint();
 
+  const int dim =
+      (knots_.size() - 1) * (qp_spline_path_config_.spline_order() + 1);
+  constexpr double param_range = 1e-4;
+  for (int i = qp_spline_path_config_.spline_order(); i < dim;
+       i += qp_spline_path_config_.spline_order() + 1) {
+    Eigen::MatrixXd mat = Eigen::MatrixXd::Zero(1, dim);
+    Eigen::MatrixXd bd = Eigen::MatrixXd::Zero(1, 1);
+    mat(0, i) = -1;
+    bd(0, 0) = -param_range;
+    spline_constraint->AddInequalityConstraint(mat, bd);
+    mat(0, i) = 1;
+    bd(0, 0) = -param_range;
+    spline_constraint->AddInequalityConstraint(mat, bd);
+  }
+
   // add init status constraint, equality constraint
   const double kBoundaryEpsilon = 1e-4;
   spline_constraint->AddPointConstraintInRange(init_frenet_point_.s(),

modules/planning/testdata/garage_test/result_stop_over_line_0.pb.txt

@@ -9,7 +9,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -23,7 +23,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -37,7 +37,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -51,7 +51,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -65,7 +65,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -79,7 +79,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -93,7 +93,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -107,7 +107,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -121,7 +121,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -135,7 +135,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -149,7 +149,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -163,7 +163,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -177,7 +177,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -191,7 +191,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -205,7 +205,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -219,7 +219,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -233,7 +233,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -247,7 +247,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -261,7 +261,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -275,7 +275,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -289,7 +289,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -303,7 +303,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -317,7 +317,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -331,7 +331,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -345,7 +345,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -359,7 +359,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -373,7 +373,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -387,7 +387,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -401,7 +401,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -415,7 +415,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -429,7 +429,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -443,7 +443,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -457,7 +457,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -471,7 +471,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -485,7 +485,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -499,7 +499,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -513,7 +513,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -527,7 +527,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -541,7 +541,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -555,7 +555,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -569,7 +569,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -583,7 +583,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -597,7 +597,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -611,7 +611,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -625,7 +625,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -639,7 +639,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -653,7 +653,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -667,7 +667,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -681,7 +681,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -695,7 +695,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -709,7 +709,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -723,7 +723,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -737,7 +737,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -751,7 +751,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -765,7 +765,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -779,7 +779,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -793,7 +793,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -807,7 +807,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -821,7 +821,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -835,7 +835,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -849,7 +849,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -863,7 +863,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -877,7 +877,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -891,7 +891,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -905,7 +905,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -919,7 +919,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -933,7 +933,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -947,7 +947,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -961,7 +961,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }
@@ -975,7 +975,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836454
+    kappa: -0.0058950283300836471
     dkappa: 0
     ddkappa: 0
   }