Planning: first completed version of osqp.

modules/planning/math/smoothing_spline/BUILD

@@ -177,12 +177,12 @@ cc_library(
 cc_library(
     name = "spline_2d_solver",
     srcs = [
-        "osqp_spline_2d_solver.cc",
         "active_set_spline_2d_solver.cc",
+        "osqp_spline_2d_solver.cc",
     ],
     hdrs = [
-        "osqp_spline_2d_solver.h",
         "active_set_spline_2d_solver.h",
+        "osqp_spline_2d_solver.h",
         "spline_2d_solver.h",
     ],
     deps = [

modules/planning/math/smoothing_spline/osqp_spline_2d_solver.cc

@@ -65,10 +65,17 @@ bool OsqpSpline2dSolver::Solve() {
   // change P to csc format
   const MatrixXd& P = kernel_.kernel_matrix();
   ADEBUG << "P: " << P.rows() << ", " << P.cols();
+  if (P.rows() == 0) {
+    return false;
+  }
+
   std::vector<double> P_data;
   std::vector<int> P_indices;
   std::vector<int> P_indptr;
   ToCSCMatrix(P, &P_data, &P_indices, &P_indptr);
+  if (P.rows() == 0) {
+    return false;
+  }
 
   c_int P_nnz = P_data.size();
   c_float P_x[P_nnz];  // NOLINT
@@ -93,8 +100,10 @@ bool OsqpSpline2dSolver::Solve() {
       inequality_constraint_matrix.rows() + equality_constraint_matrix.rows(),
       inequality_constraint_matrix.cols());
   A << inequality_constraint_matrix, equality_constraint_matrix;
-
   ADEBUG << "A: " << A.rows() << ", " << A.cols();
+  if (A.rows() == 0) {
+    return false;
+  }
 
   std::vector<double> A_data;
   std::vector<int> A_indices;
@@ -180,6 +189,14 @@ bool OsqpSpline2dSolver::Solve() {
   // Solve Problem
   osqp_solve(work);
 
+  MatrixXd solved_params = MatrixXd::Zero(P.rows(), 1);
+  for (int i = 0; i < P.rows(); ++i) {
+    solved_params(i, 0) = work->solution->x[i];
+  }
+
+  last_num_param_ = P.rows();
+  last_num_constraint_ = constraint_num;
+
   // Cleanup
   osqp_cleanup(work);
   c_free(data->A);
@@ -187,7 +204,7 @@ bool OsqpSpline2dSolver::Solve() {
   c_free(data);
   c_free(settings);
 
-  return true;
+  return spline_.set_splines(solved_params, spline_.spline_order());
 }
 
 // extract

modules/planning/math/smoothing_spline/osqp_spline_2d_solver_test.cc

@@ -19,6 +19,7 @@
  **/
 #include "modules/planning/math/smoothing_spline/osqp_spline_2d_solver.h"
 
+#include <chrono>
 #include "gtest/gtest.h"
 
 #include "modules/planning/math/curve_math.h"
@@ -133,7 +134,84 @@ TEST(OSQPSolverTest, solver_test_01) {
   kernel->AddRegularization(0.1);
   // constraint->add_point_angle_constraint(0, -1.21);
   // TODO(all): fix the test.
-  // EXPECT_TRUE(spline_solver.Solve());
+  auto start = std::chrono::system_clock::now();
+  EXPECT_TRUE(spline_solver.Solve());
+  auto end = std::chrono::system_clock::now();
+  std::chrono::duration<double> diff = end - start;
+  std::cout << "Time to solver is " << diff.count() << " s\n";
+
+  MatrixXd gold_res(51, 6);
+  // clang-format off
+  gold_res <<
+-1.211536842077514109 ,  434592.66747698455583,   4437011.3104558130726,   3.8080526249093837876,   -10.139728378837974176,  -4.9702012767283664302e-06, // NOLINT
+-1.2115424151423492827,  434593.04828098160215,   4437010.2964778374881,   3.8080266324837173109,   -10.139830860402472723,  -5.250894216084755994e-06, // NOLINT
+-1.211548079937599498 ,  434593.42908218270168,   4437009.282489891164 ,   3.8079967985589404655,   -10.139925941668183285,  -5.1522704816467339639e-06, // NOLINT
+-1.2115534670910343973,  434593.80988023628015,   4437008.2684932174161,   3.807963824342671888 ,   -10.140004109433711221,  -4.7509311852720327338e-06, // NOLINT
+-1.2115582901792760762,  434594.19067487574648,   4437007.2544898428023,   3.8079287063501028321,   -10.140059191754115631,  -4.1234548052899087269e-06, // NOLINT
+-1.2115623457107336236,  434594.57146594882943,   4437006.2404822465032,   3.8078927364039967252,   -10.140088357940907571,  -3.346398718521420416e-06, // NOLINT
+-1.2115655131115372622,  434594.95225344720529,   4437005.226473021321 ,   3.807857501634689612 ,   -10.140092118562053614,  -2.4963055868708661977e-06, // NOLINT
+-1.2115677547171821438,  434595.33303753606742,   4437004.2124645449221,   3.807824884480089267 ,   -10.140074325441972292,  -1.6497122000467622075e-06, // NOLINT
+-1.2115691157708639025,  434595.71381858363748,   4437003.1984586389735,   3.8077970626856774139,   -10.140042171661530546,  -8.8315837695925047957e-07, // NOLINT
+-1.2115697244277590094,  434596.09459719056031,   4437002.1844562413171,   3.807776509304506618 ,   -10.140006191558054383,  -2.7319352839184158338e-07, // NOLINT
+-1.2115697917627634705,  434596.47537421970628,   4437001.1704570697621,   3.8077659926972025062,   -10.139980260725319994,  1.0362151721092292462e-07, // NOLINT
+-1.2115695060626494595,  434596.85615080740536,   4437000.1564594125375,   3.8077678515443436069,   -10.139976408292163512,  4.6408745318981610703e-07, // NOLINT
+-1.2115687107848465143,  434597.23692818894051,   4436999.1424612496048,   3.8077816733989728881,   -10.139988712761946132,  1.0340515817845881855e-06, // NOLINT
+-1.2115672131285435409,  434597.61770749953575,   4436998.1284614419565,   3.8078062133014105584,   -10.140007918433125766,  1.7505174784501674035e-06, // NOLINT
+-1.2115648885226146803,  434597.99848974274937,   4436997.1144596887752,   3.8078400821457005776,   -10.140026487871562466,  2.550493252490749351e-06, // NOLINT
+-1.2115616806216027435,  434598.3792757759802 ,   4436996.1004563607275,   3.8078817466796182067,   -10.140038601910511318,  3.3709929708283110962e-06, // NOLINT
+-1.2115576013019537793,  434598.76006629614858,   4436995.0864523220807,   3.8079295295046677872,   -10.140040159650634877,  4.1490353176471159136e-06, // NOLINT
+-1.2115527306601105995,  434599.1408618252608 ,   4436994.0724487621337,   3.8079816090760862934,   -10.140028778459988956,  4.8216404409474350213e-06, // NOLINT
+-1.2115472170133783081,  434599.52166269585723,   4436993.058447021991 ,   3.8080360197028402247,   -10.140003793974035062,  5.3258259372985455144e-06, // NOLINT
+-1.2115412769037934293,  434599.90246903675143,   4436992.0444484241307,   3.8080906515476260488,   -10.139966260095631512,  5.598602925861600625e-06, // NOLINT
+-1.2115351951045365553,  434600.28328075853642,   4436991.030454098247 ,   3.808143250626879972 ,   -10.139918948995036985,  5.576973162632266028e-06, // NOLINT
+-1.2115294702539036731,  434600.66409751231549,   4436990.0164647698402,   3.8081903561870884545,   -10.139868008034298441,  4.8077597313767370503e-06, // NOLINT
+-1.2115251375632734021,  434601.04491840628907,   4436989.0024801855907,   3.808224824160084232 ,   -10.139826306862863348,  3.0486030083292842975e-06, // NOLINT
+-1.2115231321195760739,  434601.42574184050318,   4436987.9884988041595,   3.8082402598548479311,   -10.13980562520879225 ,  5.5275025457330651254e-07, // NOLINT
+-1.2115241146872521849,  434601.80656567483675,   4436986.9745180681348,   3.8082322584127257237,   -10.139814590034983866,  -2.4265100165787592887e-06, // NOLINT
+-1.2115284716799898934,  434602.18738742824644,   4436985.9605347122997,   3.8081984048074284388,   -10.139858675539189292,  -5.6358560536309203708e-06, // NOLINT
+-1.2115363151431126632,  434602.56820447766222,   4436984.9465450821444,   3.8081382738450333392,   -10.139940203154003129,  -8.8219528535762411261e-06, // NOLINT
+-1.2115474827532255464,  434602.94901425705757,   4436983.9325454477221,   3.8080534301639805683,   -10.140058341546867027,  -1.1731478591964790206e-05, // NOLINT
+-1.2115615378377246891,  434603.32981445622863,   4436982.9185323221609,   3.8079474282350775916,   -10.140209106620064361,  -1.4111151966075817204e-05, // NOLINT
+-1.2115777694127920494,  434603.71060322003905,   4436981.9045027736574,   3.8078258123614956432,   -10.140385361510730888,  -1.5707754920828727438e-05, // NOLINT
+-1.2115951922344931901,  434604.09137934719911,   4436980.8904547393322,   3.8076961166787692825,   -10.140576816590844089,  -1.6268145228448618606e-05, // NOLINT
+-1.2116125889487479039,  434604.47214246902149,   4436979.8763873716816,   3.8075670315926619658,   -10.140769106614820672,  -1.5673636657669147182e-05, // NOLINT
+-1.2116287906335816427,  434604.85289298970019,   4436978.8623015228659,   3.8074450081129960211,   -10.140943390519536749,  -1.4090310598988598536e-05, // NOLINT
+-1.2116428203719258327,  434605.23363187833456,   4436977.8481998452917,   3.8073351923255067675,   -10.141083264606631786,  -1.1692162504206557436e-05, // NOLINT
+-1.21165388963954479  ,  434605.61436058417894,   4436976.834086435847 ,   3.8072420352090459161,   -10.141176277484222013,  -8.6530843040166213536e-06, // NOLINT
+-1.2116613982288511053,  434605.99508096667705,   4436975.8199664391577,   3.8071692926355793496,   -10.141213930066893312,  -5.1468717174906078468e-06, // NOLINT
+-1.2116649341851788435,  434606.37579522602027,   4436974.8058456517756,   3.8071200253701888983,   -10.141191675575703002,  -1.3472491624807740227e-06, // NOLINT
+-1.2116642737662484119,  434606.75650583388051,   4436973.7917301254347,   3.8070965990710723403,   -10.141108919538183386,  2.5720935559557779256e-06, // NOLINT
+-1.2116593814313549871,  434607.13721546367742,   4436972.7776257768273,   3.8071006842895429578,   -10.140969019788338201,  6.4374585958675587059e-06, // NOLINT
+-1.2116504098626179609,  434607.51792692107847,   4436971.7635379871354,   3.8071332564700290924,   -10.140779286466644393,  1.0075097281741009924e-05, // NOLINT
+-1.2116377000164157973,  434607.89864307461539,   4436970.7494712099433,   3.8071945959500750334,   -10.140550982020052118,  1.3311172977170112426e-05, // NOLINT
+-1.2116217746240613984,  434608.27936676860554,   4436969.7354286238551,   3.8072835982384574116,   -10.140297281596426515,  1.5999703511677472223e-05, // NOLINT
+-1.2116032340269256018,  434608.66010057670064,   4436968.7214123532176,   3.8073960888063611563,   -10.140025573140695414,  1.8150697288770883026e-05, // NOLINT
+-1.2115826280259958114,  434609.04084661870729,   4436967.7074239440262,   3.8075275641774672941,   -10.139740809308877445,  1.9824220542444219705e-05, // NOLINT
+-1.2115604413276630513,  434609.42160658695502,   4436966.6934644868597,   3.8076739776390393644,   -10.139447151801833868,  2.1080345198949189623e-05, // NOLINT
+-1.2115370935378786399,  434609.80238179198932,   4436965.6795346960425,   3.8078317392419229748,   -10.139147971365266798,  2.1979154386414284563e-05, // NOLINT
+-1.211512939152885826 ,  434610.18317320814822,   4436964.6656349897385,   3.8079977158005493543,   -10.138845847789720978,  2.2580746160597712412e-05, // NOLINT
+-1.2114882675477143259,  434610.56398151925532,   4436963.6517655644566,   3.8081692308929300239,   -10.138542569910585556,  2.2945235784308586307e-05, // NOLINT
+-1.2114633029633761208,  434610.94480716442922,   4436962.6379264798015,   3.8083440648606621259,   -10.138239135608092312,  2.3132756898251399347e-05, // NOLINT
+-1.211438204493459958 ,  434611.3256503836019 ,   4436961.624117734842 ,   3.8085204548089248711,   -10.137935751807310325,  2.3203461921265504272e-05, // NOLINT
+-1.211413066070579303 ,  434611.70651126321172,   4436960.6103393463418,   3.8086970946064799826,   -10.13763183447815841 ,  2.3217522018132972405e-05; // NOLINT
+  // clang-format on
+
+  double t = 0;
+  for (int i = 0; i < 51; ++i) {
+    auto xy = spline_solver.spline()(t);
+    const double heading = std::atan2(spline_solver.spline().DerivativeY(t),
+                                      spline_solver.spline().DerivativeX(t));
+    const double kappa = CurveMath::ComputeCurvature(
+        spline_solver.spline().DerivativeX(t),
+        spline_solver.spline().SecondDerivativeX(t),
+        spline_solver.spline().DerivativeY(t),
+        spline_solver.spline().SecondDerivativeY(t));
+    EXPECT_NEAR(xy.first, gold_res(i, 1),
+                std::fmax(3e-3, gold_res(i, 1) * 1e-4));
+    EXPECT_NEAR(xy.second, gold_res(i, 2),
+                std::fmax(3e-3, gold_res(i, 2) * 1e-4));
+    t += 0.1;
+  }
 }
 
 }  // namespace planning

modules/planning/math/smoothing_spline/spline_2d_solver_test.cc

@@ -17,11 +17,13 @@
 /**
  * @file
  **/
-#include "modules/planning/math/smoothing_spline/active_set_spline_2d_solver.h"
+
+#include <chrono>
 
 #include "gtest/gtest.h"
 
 #include "modules/planning/math/curve_math.h"
+#include "modules/planning/math/smoothing_spline/active_set_spline_2d_solver.h"
 
 namespace apollo {
 namespace planning {
@@ -70,7 +72,11 @@ TEST(constraint_test, test_suit_one) {
 
   kernel->AddRegularization(0.1);
   // constraint->add_point_angle_constraint(0, -1.21);
+  auto start = std::chrono::system_clock::now();
   EXPECT_TRUE(spline_solver.Solve());
+  auto end = std::chrono::system_clock::now();
+  std::chrono::duration<double> diff = end - start;
+  std::cout << "Time to solver is " << diff.count() << " s\n";
 
   MatrixXd gold_res(51, 6);
   // clang-format off