temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
kyoichi-sugahara · Mar 18, 2024 · e2eef20 · e2eef20
1 parent 5213ca8
commit e2eef20
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 11 deletions.
diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_cgmres.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_cgmres.hpp
@@ -54,7 +54,8 @@ class QPSolverCGMRES : public QPSolverInterface
     const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a,
     const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
     const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override;
-  bool solveCGMRES(const Eigen::VectorXd & x0, const double prediction_dt, Eigen::VectorXd & u);
+  bool solveCGMRES(
+    const Eigen::VectorXd & x0, const double prediction_dt, Eigen::VectorXd & u) override;
 
   int64_t getTakenIter() const override { return cgmressolver_.getTakenIter(); }
   double getRunTime() const override { return cgmressolver_.getRunTime(); }

diff --git a/...l/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/...l/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp
@@ -46,6 +46,24 @@ class QPSolverInterface
     const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
     const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) = 0;
 
+  /**
+   * @brief solve QP problem : minimize J = u' * h_mat * u + f_vec' * u without constraint
+   * @param [in] h_mat parameter matrix in object function
+   * @param [in] f_vec parameter matrix in object function
+   * @param [in] a parameter matrix for constraint lb_a < a*u < ub_a
+   * @param [in] lb parameter matrix for constraint lb < u < ub
+   * @param [in] ub parameter matrix for constraint lb < u < ub
+   * @param [in] lb_a parameter matrix for constraint lb_a < a*u < ub_a
+   * @param [in] ub_a parameter matrix for constraint lb_a < a*u < ub_a
+   * @param [out] u optimal variable vector
+   * @return true if the problem was solved
+   */
+  virtual bool solveCGMRES(
+    const Eigen::VectorXd & /* x0 */, const double /*prediction_dt */, Eigen::VectorXd & /*u*/)
+  {
+    return false;
+  }
+
   virtual int64_t getTakenIter() const { return 0; }
   virtual double getRunTime() const { return 0.0; }
   virtual double getObjVal() const { return 0.0; }

diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp
@@ -704,16 +704,7 @@ std::pair<bool, VectorXd> MPC::executeOptimization(
   // settings.opterr_tol = 1e-06;
 
   auto t_start = std::chrono::system_clock::now();
-  // m_qpsolver_ptr が QPSolverCGMRES 型のインスタンスを指しているかチェック
-  QPSolverCGMRES * cgmres_solver = dynamic_cast<QPSolverCGMRES *>(m_qpsolver_ptr.get());
-  if (cgmres_solver != nullptr) {
-    // m_qpsolver_ptr が QPSolverCGMRES のインスタンスを指している場合
-    // solveCGMRES メソッドを実行
-    cgmres_solver->solveCGMRES(x0, DT, Uex);
-  } else {
-    std::cout << "The solver is not QPSolverCGMRES." << std::endl;
-  }
-  // bool solve_result = m_qpsolver_ptr->solveCGMRES(x0, DT, Uex);
+  bool solve_result = m_qpsolver_ptr->solveCGMRES(x0, DT, Uex);
   auto t_end = std::chrono::system_clock::now();
   if (!solve_result) {
     warn_throttle("qp solver error");