Failed to run MPC local planner

[bigpigdog]

I've installed all the dependencies, but this error occurs:

[FATAL] [1712800952.056223995, 1711543538.130066234]: Failed to create the mpc_planner/MPCPlanner planner, are you sure it is properly registered and that the containing library is built? Exception: Failed to load library /home/lhj/catkin_ws/devel/lib//libmpc_planner.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libOsqpEigen.so.0.8.1: cannot open shared object file: No such file or directory)

The libmpc_planner.so and libOsqpEigen.so.0.8.1 does exist. Why compiler show that libmpc_planner.so exist in ***/catkin_ws/devel/lib//libmpc_planner.so and not in ***/catkin_ws/devel/lib/libmpc_planner.so. In the latter, there are no two "/" that are the right path.

[bigpigdog]

Reinstalling osqp-eigen and osqp solved the issue. But I reinstalled it once before. It's very weird.

But another problem occurs.

ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex ERROR in osqp_setup: KKT matrix factorization. The problem seems to be non-convex. [OsqpEigen::Solver::initSolver] Unable to setup the workspace. [OsqpEigen::Solver::solveProblem] The solve has not been initialized yet. Please call initSolver() method.

Next I'm going to read the code and try to fix the problem...

[ai-winter]

@bigpigdog

We did not encounter this issue during testing. If possible, could you please try to reproduce the error and provide the relevant log files? We will attempt to identify and fix the issue.

[bigpigdog]

@ai-winter

用中文交流吧。用 mpc_planner 跑你们的 gazebo demo（main.sh）是没有任何错误的。（上一条发的一些log信息没有意义，我删除了）

Reinstalling osqp-eigen and osqp solved the issue. But I reinstalled it once before. It's very weird.

But another problem occurs.

ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex ERROR in osqp_setup: KKT matrix factorization. The problem seems to be non-convex. [OsqpEigen::Solver::initSolver] Unable to setup the workspace. [OsqpEigen::Solver::solveProblem] The solve has not been initialized yet. Please call initSolver() method.

Next I'm going to read the code and try to fix the problem...

我的想法是把你们的 mpc_planner 用于没有全局地图的路径规划中，我修改了 move_base 中 global_planner、local_costmap 以及 global_costmap 的运行方式（不使用全局规划器与全局地图，通过某种算法给定一条无碰撞路径，然后使用mpc跟随），然后遇到了如上的报错。**后来发现是 kappa 有时候太小会出现 nan，然后导致 u_r 和 u 也出现 nan，因此加了一个判断，kappa 为 nan 的时候让它等于0。**除了这个原因，其他时候也会出现，但频率已经非常非常小了，遂没有管他。

**我想知道如何显示 mpc k时刻的预测轨迹？**下面是我添加的代码，我不太清楚是否正确。solution 的结果是k时刻对未来k+n步的输出预测，加上 du_p 和 u_r 才是正确的 v 和 w 吗？X计算出了未来k+n步的相对坐标，加上 s_d 才是 odom 系下的真实坐标？

绿色线条就是我生成的 trajectory_path。但是感觉很奇怪，起始点不在 base_footprint 的原点上，而且线段距离很长，图中有3m多（dt = 0.05, p = 12, max_v = 0.5），这个参数的预测距离最长应该是 0.05 * 12 * 0.5 = 0.3m 吧？

  for (int i = 0; i < 2 * m_; ++i) 
  {
    if (i % 2 == 0) {
      solution[i] = solution[i] + du_p[0] + u_r[0];
    } else {
      solution[i] = regularizeAngle(solution[i] + du_p[1] + u_r[1]);
    }
  }
  Eigen::VectorXd X = S_x * x + S_u * solution; // (3p x 1)

  nav_msgs::Path trajectory_path;
  trajectory_path.header.stamp = ros::Time::now();
  trajectory_path.header.frame_id = "odom";
  geometry_msgs::PoseStamped pose;
  pose.header.frame_id = trajectory_path.header.frame_id;
  pose.header.stamp = ros::Time::now();
  pose.pose.orientation.w = 1.0;
  pose.pose.position.z = 1.0;
  for (int i = 0; i < p_; i++)
  {
    pose.pose.position.x = s_d[0] + X[i * 3];
    pose.pose.position.y = s_d[1] + X[i * 3 + 1];
    trajectory_path.poses.push_back(pose);
  }
  trajectory_pub_.publish(trajectory_path);

[ai-winter]

@bigpigdog

感谢你的分析，曲率的问题我们将在未来的工作中改进。关于预测轨迹，可以基于运动学模型向前推理若干时间步并可视化