feat(obstacle_avoidance_planner): faster optimization by sparser re-f…

…ormulation (autowarefoundation#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>
leo-drive · Aug 16, 2023 · 656e9f0 · 656e9f0
1 parent d76bc93
commit 656e9f0
Show file tree

Hide file tree

Showing 4 changed files with 116 additions and 142 deletions.
diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/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(

diff --git a/...bstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp b/...bstacle_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;
   };