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 <[email protected]> * minor change Signed-off-by: Takayuki Murooka <[email protected]> * update Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
tier4 · Aug 18, 2023 · 0b5ba7b · 0b5ba7b
1 parent 32924ce
commit 0b5ba7b
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
@@ -236,6 +236,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};
 
   void updateVehicleCircles();
@@ -293,7 +294,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;
   };