P is not upper triangular matrix

[lilintong22222]

In https://github.com/ApolloAuto/apollo/blob/v6.0.0/modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.cc#L84

I find P is not upper triangular matrix，and osqp 0.5.0 will trans P to upper triangular matrix by drop some elements：
https://github.com/oxfordcontrol/osqp/blob/v0.5.0/src/osqp.c#L114

https://github.com/oxfordcontrol/osqp/blob/v0.5.0/src/cs.c#L288

This makes the constraint（ -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)'' ) doesn't work

[jilinzhou]

Interestingly, all libs under /usr/local/lib are built with gcc 4.8.5 except libosqp.so which is built with gcc 5.4.0.

jilinzhou@in_dev_docker:/usr/local/lib$ readelf -p .comment ./libgflags.so

String dump of section '.comment':
  [     0]  GCC: (Ubuntu 4.8.5-4ubuntu8) 4.8.5

jilinzhou@in_dev_docker:/usr/local/lib$ readelf -p .comment ./libnlopt.so 

String dump of section '.comment':
  [     0]  GCC: (Ubuntu 4.8.5-4ubuntu8) 4.8.5

jilinzhou@in_dev_docker:/usr/local/lib$ readelf -p .comment ./libosqp.so 

String dump of section '.comment':
  [     0]  GCC: (Ubuntu 5.4.0-6ubuntu1~16.04.10) 5.4.0 20160609

And in osqp 0.5.0 release, it does not verify whether P is an upper triangle matrix in
c_int validate_data(const OSQPData *data).

Here is the link to the code:
https://github.com/oxfordcontrol/osqp/blob/release-0.5.0/src/auxil.c

[lilintong22222]

Interestingly, all libs under /usr/local/lib are built with gcc 4.8.5 except libosqp.so which is built with gcc 5.4.0.

jilinzhou@in_dev_docker:/usr/local/lib$ readelf -p .comment ./libgflags.so

String dump of section '.comment':
  [     0]  GCC: (Ubuntu 4.8.5-4ubuntu8) 4.8.5

jilinzhou@in_dev_docker:/usr/local/lib$ readelf -p .comment ./libnlopt.so 

String dump of section '.comment':
  [     0]  GCC: (Ubuntu 4.8.5-4ubuntu8) 4.8.5

jilinzhou@in_dev_docker:/usr/local/lib$ readelf -p .comment ./libosqp.so 

String dump of section '.comment':
  [     0]  GCC: (Ubuntu 5.4.0-6ubuntu1~16.04.10) 5.4.0 20160609

And in osqp 0.5.0 release, it does not verify whether P is an upper triangle matrix in
c_int validate_data(const OSQPData *data).

Here is the link to the code:
https://github.com/oxfordcontrol/osqp/blob/release-0.5.0/src/auxil.c

I can't understand.
I mean osqp 0.5.0 will force convert P matrix to a upper triangular matrix and all points in the lower triangle (2n+i+1, 2n+i) will be discarded

[jilinzhou]

You can convert it into an upper triangle matrix instead. Just remember to scale down the off-diagonal entries by 2.0 otherwise ospq lib will complain the problem is not convex. FemPosDeviationOsqpInterface::CalculateKernel() is a good example where the data matrix P is formed as an upper triangle matrix.

[lilintong22222]

FemPosDeviationOsqpInterface::CalculateKernel is just an example.
But where dose apollow convert P of modules/planning/math/piecewise_jerk to upper triangular matrix

[cshang412]

met same issue , when P is not upper triangular matrix , how to convert ?

[cshang412]

i currently avoid this by adding data->P = csc_to_triu(csc_matrix(kernel_dim, kernel_dim, P_data.size(), CopyData(P_data), CopyData(P_indices), CopyData(P_indptr))); in PiecewiseJerkProblem::FormulateProblem function

[jilinzhouas]

Found this note I did long time ago. It has been so long and I do not even understand it now. Hope it helps.

Here is an example to convert the lower triangle to upper triangle data matrix with piecewise_jerk_speed_optimizer.

  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
  // The docker osqp lib will fail with non convex problem if -2.0 is applied as
  // the coefficient. Basically if P is provided as a upper triangle matrix, the
  // coefficients of off-diagonal entries needs to be divided by 2.0.
  // However if it is provided as a lower triangle matrix, there is no need to
  // scale the coefficients of off-diagonal entries.
  for (int i = 1; i <= n - 1; ++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) /
                       (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }
  columns[3 * n - 1].emplace_back(
      3 * n - 1,
      (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
          (scale_factor_[2] * scale_factor_[2]));
  ++value_index;

  // ORIGINAL CODE for the off-diagonal terms // -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;
  // }

[breezygogo]

Found this note I did long time ago. It has been so long and I do not even understand it now. Hope it helps.

Here is an example to convert the lower triangle to upper triangle data matrix with piecewise_jerk_speed_optimizer.

  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
  // The docker osqp lib will fail with non convex problem if -2.0 is applied as
  // the coefficient. Basically if P is provided as a upper triangle matrix, the
  // coefficients of off-diagonal entries needs to be divided by 2.0.
  // However if it is provided as a lower triangle matrix, there is no need to
  // scale the coefficients of off-diagonal entries.
  for (int i = 1; i <= n - 1; ++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) /
                       (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }
  columns[3 * n - 1].emplace_back(
      3 * n - 1,
      (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
          (scale_factor_[2] * scale_factor_[2]));
  ++value_index;

  // ORIGINAL CODE for the off-diagonal terms // -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;
  // }

您好，您能告诉我为啥要这样做吗，我照着你这样改，效果真的很明显好很多了。

[lilintong22222]

Found this note I did long time ago. It has been so long and I do not even understand it now. Hope it helps.
Here is an example to convert the lower triangle to upper triangle data matrix with piecewise_jerk_speed_optimizer.

  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
  // The docker osqp lib will fail with non convex problem if -2.0 is applied as
  // the coefficient. Basically if P is provided as a upper triangle matrix, the
  // coefficients of off-diagonal entries needs to be divided by 2.0.
  // However if it is provided as a lower triangle matrix, there is no need to
  // scale the coefficients of off-diagonal entries.
  for (int i = 1; i <= n - 1; ++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) /
                       (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }
  columns[3 * n - 1].emplace_back(
      3 * n - 1,
      (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
          (scale_factor_[2] * scale_factor_[2]));
  ++value_index;

  // ORIGINAL CODE for the off-diagonal terms // -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;
  // }

您好，您能告诉我为啥要这样做吗，我照着你这样改，效果真的很明显好很多了。

因为 osqp 默认输入的P矩阵是对称的，并且只传入上三角部分，在代码中会把下三角部分 mask 掉。
举个例子，假设输入P矩阵为:
[1202.0 -1200.0
0 602.0]
对 osqp 来说实际的P矩阵为：
[1202.0 -1200.0
-1200.0 602.0]
因此正确的使用方式是把推导出来的P矩阵先转为对称矩阵：
[1202.0 -600.0
-600.0 602.0]
并保留上三角部分

[breezygogo]

Found this note I did long time ago. It has been so long and I do not even understand it now. Hope it helps.
Here is an example to convert the lower triangle to upper triangle data matrix with piecewise_jerk_speed_optimizer.

  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
  // The docker osqp lib will fail with non convex problem if -2.0 is applied as
  // the coefficient. Basically if P is provided as a upper triangle matrix, the
  // coefficients of off-diagonal entries needs to be divided by 2.0.
  // However if it is provided as a lower triangle matrix, there is no need to
  // scale the coefficients of off-diagonal entries.
  for (int i = 1; i <= n - 1; ++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) /
                       (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }
  columns[3 * n - 1].emplace_back(
      3 * n - 1,
      (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
          (scale_factor_[2] * scale_factor_[2]));
  ++value_index;

  // ORIGINAL CODE for the off-diagonal terms // -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;
  // }

您好，您能告诉我为啥要这样做吗，我照着你这样改，效果真的很明显好很多了。

因为 osqp 默认输入的P矩阵是对称的，并且只传入上三角部分，在代码中会把下三角部分 mask 掉。 举个例子，假设输入P矩阵为: [1202.0 -1200.0 0 602.0] 对 osqp 来说实际的P矩阵为： [1202.0 -1200.0 -1200.0 602.0] 因此正确的使用方式是把推导出来的P矩阵先转为对称矩阵： [1202.0 -600.0 -600.0 602.0] 并保留上三角部分

兄弟 我加你qq了，以后可以更加方便沟通交流哈哈。

[breezygogo]

Found this note I did long time ago. It has been so long and I do not even understand it now. Hope it helps.
Here is an example to convert the lower triangle to upper triangle data matrix with piecewise_jerk_speed_optimizer.

  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
  // The docker osqp lib will fail with non convex problem if -2.0 is applied as
  // the coefficient. Basically if P is provided as a upper triangle matrix, the
  // coefficients of off-diagonal entries needs to be divided by 2.0.
  // However if it is provided as a lower triangle matrix, there is no need to
  // scale the coefficients of off-diagonal entries.
  for (int i = 1; i <= n - 1; ++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) /
                       (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }
  columns[3 * n - 1].emplace_back(
      3 * n - 1,
      (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
          (scale_factor_[2] * scale_factor_[2]));
  ++value_index;

  // ORIGINAL CODE for the off-diagonal terms // -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;
  // }

您好，您能告诉我为啥要这样做吗，我照着你这样改，效果真的很明显好很多了。

因为 osqp 默认输入的P矩阵是对称的，并且只传入上三角部分，在代码中会把下三角部分 mask 掉。 举个例子，假设输入P矩阵为: [1202.0 -1200.0 0 602.0] 对 osqp 来说实际的P矩阵为： [1202.0 -1200.0 -1200.0 602.0] 因此正确的使用方式是把推导出来的P矩阵先转为对称矩阵： [1202.0 -600.0 -600.0 602.0] 并保留上三角部分
为啥有这么明显的错误，apollo官方不对其进行修改了@daohu527

[jilinzhou]

I do not remember how I figured it out: probably by studing the code of FemPosDeviationOsqpInterface::CalculateKernel()
Here is my guess of the issue: the osqp lib (apollo 5.5) in the docker image was not coming from a official release or something was modified.
jilinzhou@in_dev_docker:/usr/local/lib$ readelf -p .comment ./libosqp.so
String dump of section '.comment':
[ 0] GCC: (Ubuntu 5.4.0-6ubuntu1~16.04.10) 5.4.0 20160609

[breezygogo]

I do not remember how I figured it out: probably by studing the code of FemPosDeviationOsqpInterface::CalculateKernel() Here is my guess of the issue: the osqp lib (apollo 5.5) in the docker image was not coming from a official release or something was modified. jilinzhou@in_dev_docker:/usr/local/lib$ readelf -p .comment ./libosqp.so String dump of section '.comment': [ 0] GCC: (Ubuntu 5.4.0-6ubuntu1~16.04.10) 5.4.0 20160609

thanks Do you have wechat i think we can do more communication on wechat