From 339f7449a0e415fac97eed4194f3d62d7932ec78 Mon Sep 17 00:00:00 2001
From: lihaoxiang1989 <hxl.colin@gmail.com>
Date: Thu, 18 Aug 2022 14:08:19 +0800
Subject: [PATCH] planning osqp P matrix is incorrectly formated

---
 .../piecewise_jerk_path_problem.cc             | 18 ++++++++++--------
 .../piecewise_jerk_speed_problem.cc            | 16 ++++++++--------
 2 files changed, 18 insertions(+), 16 deletions(-)

diff --git a/modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.cc b/modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.cc
index bdccf45f04..08f6a2b7b5 100644
--- a/modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.cc
+++ b/modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.cc
@@ -67,6 +67,16 @@ void PiecewiseJerkPathProblem::CalculateKernel(std::vector<c_float>* P_data,
                               (weight_ddx_ + weight_dddx_ / delta_s_square) /
                                   (scale_factor_[2] * scale_factor_[2]));
   ++value_index;
+
+  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
+  // OSQP needs only the elements in its upper triangle
+  for (int i = 1; i < n; ++i) {
+    columns[2 * n + i].emplace_back(2 * n + i - 1,
+                                    (-1.0 * weight_dddx_ / delta_s_square) /
+                                        (scale_factor_[2] * scale_factor_[2]));
+    ++value_index;
+  }
+
   for (int i = 1; i < n - 1; ++i) {
     columns[2 * n + i].emplace_back(
         2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /
@@ -79,14 +89,6 @@ void PiecewiseJerkPathProblem::CalculateKernel(std::vector<c_float>* P_data,
           (scale_factor_[2] * scale_factor_[2]));
   ++value_index;
 
-  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
-  for (int i = 0; i < n - 1; ++i) {
-    columns[2 * n + i].emplace_back(2 * n + i + 1,
-                                    (-2.0 * weight_dddx_ / delta_s_square) /
-                                        (scale_factor_[2] * scale_factor_[2]));
-    ++value_index;
-  }
-
   CHECK_EQ(value_index, num_of_nonzeros);
 
   int ind_p = 0;
diff --git a/modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.cc b/modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.cc
index fa017168fe..bd2cdb53ed 100644
--- a/modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.cc
+++ b/modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.cc
@@ -82,6 +82,14 @@ void PiecewiseJerkSpeedProblem::CalculateKernel(std::vector<c_float>* P_data,
                                   (scale_factor_[2] * scale_factor_[2]));
   ++value_index;
 
+  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
+  for (int i = 1; i < n; ++i) {
+    columns[2 * n + i].emplace_back(2 * n + i - 1,
+                                    -1.0 * weight_dddx_ / delta_s_square /
+                                        (scale_factor_[2] * scale_factor_[2]));
+    ++value_index;
+  }
+
   for (int i = 1; i < n - 1; ++i) {
     columns[2 * n + i].emplace_back(
         2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /
@@ -95,14 +103,6 @@ void PiecewiseJerkSpeedProblem::CalculateKernel(std::vector<c_float>* P_data,
           (scale_factor_[2] * scale_factor_[2]));
   ++value_index;
 
-  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
-  for (int i = 0; i < n - 1; ++i) {
-    columns[2 * n + i].emplace_back(2 * n + i + 1,
-                                    -2.0 * weight_dddx_ / delta_s_square /
-                                        (scale_factor_[2] * scale_factor_[2]));
-    ++value_index;
-  }
-
   CHECK_EQ(value_index, kNumValue);
 
   int ind_p = 0;