planning osqp P matrix is incorrectly formated

[col-in-coding]

OSQP needs only the elements in its upper triangle part for the P matrix

[nkuwenjian]

Apollo用的OSQP版本是0.5.0，所以输入的P矩阵（对称半正定矩阵）既可以是上三角矩阵，也可以是完整的对称矩阵。我测试过，0.6.2版本的OSQP要求输入的P矩阵必须是上三角矩阵，否则会报错

但是，Apollo里面piecewise_jerk_path_problem和piecewise_jerk_speed_problem输入的P矩阵既不是上三角矩阵，也不是对称矩阵，而是一个下三角矩阵 从矩阵乘法角度来看，非对角线元素放在一侧或者拆成一半放在对角线两侧结果是一样的，但是对于OSQP求解器来讲结果就不一样 我做了如下的测试 上图是我截取的一张图片，其中H对应P矩阵，f对应q矩阵，我把OSQP计算得到的结果再代入目标函数，得到的结果与OSQP打印的obj_val是一致的 但如果把H矩阵变成[1 0; -2 2]，则用0.5.0版本的OSQP也能计算出结果，但将结果代入到目标函数中得到的值与OSQP得到的obj_val是不一致的 总的一句话，就是Apollo piecewise jerk填充P矩阵的代码是有问题的，这个问题影响的是对jerk的优化，因为目标函数中只有jerk这一项会产生P矩阵的非对角线元素

@daohu527

[jilinzhouas]

https://github.com/ApolloAuto/apollo/issues/13455

[ycdhqzhiai]

我按照commit里面改了报错ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex

[zhaofuxie]

我按照commit里面改了报错ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex

我也出现了，还需要修改位置，这个函数对矩阵的构造有严格的限制

[chenjian88888]

我按照commit里面改了报错ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex

我也出现了，还需要修改位置，这个函数对矩阵的构造有严格的限制

列向量的填充需要是按顺序的，需要调整代码的位置