feat(obstacle_avoidance_planner): faster optimization by sparser re-formulation (#4600)

* feat(obstacle_avoidance_planner): re-formulate for faster QP

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* minor change

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp

@@ -237,6 +237,7 @@ class MPTOptimizer
   // previous data
   int prev_mat_n_ = 0;
   int prev_mat_m_ = 0;
+  int prev_solution_status_ = 0;
   std::shared_ptr<std::vector<ReferencePoint>> prev_ref_points_ptr_{nullptr};
   std::shared_ptr<std::vector<TrajectoryPoint>> prev_optimized_traj_points_ptr_{nullptr};
 
@@ -295,7 +296,7 @@ class MPTOptimizer
     const std::vector<ReferencePoint> & ref_points) const;
 
   std::optional<std::vector<TrajectoryPoint>> calcMPTPoints(
-    std::vector<ReferencePoint> & ref_points, const Eigen::VectorXd & U,
+    std::vector<ReferencePoint> & ref_points, const Eigen::VectorXd & optimized_variables,
     const StateEquationGenerator::Matrix & mpt_matrix) const;
 
   void publishDebugTrajectories(

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp

@@ -31,6 +31,7 @@ class StateEquationGenerator
 public:
   struct Matrix
   {
+    Eigen::MatrixXd A;
     Eigen::MatrixXd B;
     Eigen::VectorXd W;
   };