From 0eae67c50eaa41066c0b5e518a540828350f7f0f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 13 Jul 2024 14:56:46 +0900 Subject: [PATCH] refactor(qp_interface): clean up the code (#8029) * refactor qp_interface Signed-off-by: Takayuki Murooka * variable names: m_XXX -> XXX_ Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../qp_interface/osqp_csc_matrix_conv.hpp | 10 +- .../include/qp_interface/osqp_interface.hpp | 42 ++-- .../include/qp_interface/proxqp_interface.hpp | 18 +- .../include/qp_interface/qp_interface.hpp | 18 +- .../qp_interface/src/osqp_csc_matrix_conv.cpp | 10 +- common/qp_interface/src/osqp_interface.cpp | 236 +++++++++--------- common/qp_interface/src/proxqp_interface.cpp | 84 +++++-- common/qp_interface/src/qp_interface.cpp | 8 +- .../test/test_csc_matrix_conv.cpp | 210 ++++++++-------- .../qp_interface/test/test_osqp_interface.cpp | 64 ++--- .../test/test_proxqp_interface.cpp | 21 +- 11 files changed, 377 insertions(+), 344 deletions(-) diff --git a/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp index 74ec4c1282f46..8ec7280cfc100 100644 --- a/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp +++ b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp @@ -21,17 +21,17 @@ #include -namespace qp +namespace autoware::common { /// \brief Compressed-Column-Sparse Matrix struct CSC_Matrix { /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector m_vals; + std::vector vals_; /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector m_row_idxs; + std::vector row_idxs_; /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector m_col_idxs; + std::vector col_idxs_; }; /// \brief Calculate CSC matrix from Eigen matrix @@ -41,6 +41,6 @@ CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); /// \brief Print the given CSC matrix to the standard output void printCSCMatrix(const CSC_Matrix & csc_mat); -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/qp_interface/include/qp_interface/osqp_interface.hpp b/common/qp_interface/include/qp_interface/osqp_interface.hpp index ef2c3bd17c89e..14c03a91d8fa1 100644 --- a/common/qp_interface/include/qp_interface/osqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/osqp_interface.hpp @@ -24,9 +24,9 @@ #include #include -namespace qp +namespace autoware::common { -constexpr c_float INF = 1e30; +constexpr c_float OSQP_INF = 1e30; class OSQPInterface : public QPInterface { @@ -34,7 +34,9 @@ class OSQPInterface : public QPInterface /// \brief Constructor without problem formulation OSQPInterface( const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); + const c_float eps_abs = std::numeric_limits::epsilon(), + const c_float eps_rel = std::numeric_limits::epsilon(), const bool polish = true, + const bool verbose = false); /// \brief Constructor with problem setup /// \param P: (n,n) matrix defining relations between parameters. /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. @@ -60,8 +62,10 @@ class OSQPInterface : public QPInterface CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, const std::vector & u); - int getIteration() const override; - int getStatus() const override; + int getIterationNumber() const override; + bool isSolved() const override; + std::string getStatus() const override; + int getPolishStatus() const; std::vector getDualSolution() const; @@ -96,20 +100,18 @@ class OSQPInterface : public QPInterface void updateCheckTermination(const int check_termination); /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } + inline int64_t getTakenIter() const { return static_cast(latest_work_info_.iter); } /// \brief Get the status message for the latest problem solved inline std::string getStatusMessage() const { - return static_cast(m_latest_work_info.status); + return static_cast(latest_work_info_.status); } /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return m_latest_work_info.run_time; } + inline double getRunTime() const { return latest_work_info_.run_time; } /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return m_latest_work_info.obj_val; } + inline double getObjVal() const { return latest_work_info_.obj_val; } /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return m_exitflag; } - - void logUnsolvedStatus(const std::string & prefix_message = "") const; + inline int64_t getExitFlag() const { return exitflag_; } // Setter functions for warm start bool setWarmStart( @@ -118,17 +120,17 @@ class OSQPInterface : public QPInterface bool setDualVariables(const std::vector & dual_variables); private: - std::unique_ptr> m_work; - std::unique_ptr m_settings; - std::unique_ptr m_data; + std::unique_ptr> work_; + std::unique_ptr settings_; + std::unique_ptr data_; // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo m_latest_work_info; + OSQPInfo latest_work_info_; // Number of parameters to optimize - int64_t m_param_n; + int64_t param_n_; // Flag to check if the current work exists - bool m_work_initialized = false; + bool work__initialized = false; // Exitflag - int64_t m_exitflag; + int64_t exitflag_; void initializeProblemImpl( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -140,6 +142,6 @@ class OSQPInterface : public QPInterface std::vector optimizeImpl() override; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/proxqp_interface.hpp b/common/qp_interface/include/qp_interface/proxqp_interface.hpp index a940262d5f6da..f75fb3da5954c 100644 --- a/common/qp_interface/include/qp_interface/proxqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/proxqp_interface.hpp @@ -22,27 +22,29 @@ #include #include +#include #include -namespace qp +namespace autoware::common { class ProxQPInterface : public QPInterface { public: explicit ProxQPInterface( - const bool enable_warm_start = false, - const double eps_abs = std::numeric_limits::epsilon()); + const bool enable_warm_start, const double eps_abs, const double eps_rel, + const bool verbose = false); - int getIteration() const override; - int getStatus() const override; + int getIterationNumber() const override; + bool isSolved() const override; + std::string getStatus() const override; void updateEpsAbs(const double eps_abs) override; void updateEpsRel(const double eps_rel) override; void updateVerbose(const bool verbose) override; private: - proxsuite::proxqp::Settings m_settings; - std::shared_ptr> m_qp_ptr; + proxsuite::proxqp::Settings settings_{}; + std::shared_ptr> qp_ptr_{nullptr}; void initializeProblemImpl( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -50,6 +52,6 @@ class ProxQPInterface : public QPInterface std::vector optimizeImpl() override; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/qp_interface.hpp b/common/qp_interface/include/qp_interface/qp_interface.hpp index 026c0fe413b71..85e2d103cde7a 100644 --- a/common/qp_interface/include/qp_interface/qp_interface.hpp +++ b/common/qp_interface/include/qp_interface/qp_interface.hpp @@ -18,28 +18,30 @@ #include #include +#include #include -namespace qp +namespace autoware::common { class QPInterface { public: - explicit QPInterface(const bool enable_warm_start) : m_enable_warm_start(enable_warm_start) {} + explicit QPInterface(const bool enable_warm_start) : enable_warm_start_(enable_warm_start) {} std::vector optimize( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u); - virtual int getIteration() const = 0; - virtual int getStatus() const = 0; + virtual bool isSolved() const = 0; + virtual int getIterationNumber() const = 0; + virtual std::string getStatus() const = 0; virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; virtual void updateVerbose([[maybe_unused]] const bool verbose) {} protected: - bool m_enable_warm_start; + bool enable_warm_start_{false}; void initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -51,9 +53,9 @@ class QPInterface virtual std::vector optimizeImpl() = 0; - std::optional m_variables_num; - std::optional m_constraints_num; + std::optional variables_num_{std::nullopt}; + std::optional constraints_num_{std::nullopt}; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/qp_interface/src/osqp_csc_matrix_conv.cpp b/common/qp_interface/src/osqp_csc_matrix_conv.cpp index 0faf7586463fd..77a481f442000 100644 --- a/common/qp_interface/src/osqp_csc_matrix_conv.cpp +++ b/common/qp_interface/src/osqp_csc_matrix_conv.cpp @@ -21,7 +21,7 @@ #include #include -namespace qp +namespace autoware::common { CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) { @@ -114,21 +114,21 @@ CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) void printCSCMatrix(const CSC_Matrix & csc_mat) { std::cout << "["; - for (const c_float & val : csc_mat.m_vals) { + for (const c_float & val : csc_mat.vals_) { std::cout << val << ", "; } std::cout << "]\n"; std::cout << "["; - for (const c_int & row : csc_mat.m_row_idxs) { + for (const c_int & row : csc_mat.row_idxs_) { std::cout << row << ", "; } std::cout << "]\n"; std::cout << "["; - for (const c_int & col : csc_mat.m_col_idxs) { + for (const c_int & col : csc_mat.col_idxs_) { std::cout << col << ", "; } std::cout << "]\n"; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/osqp_interface.cpp b/common/qp_interface/src/osqp_interface.cpp index 81d2ffeeaaed5..0e8cb7e13988a 100644 --- a/common/qp_interface/src/osqp_interface.cpp +++ b/common/qp_interface/src/osqp_interface.cpp @@ -18,27 +18,29 @@ #include -namespace qp -{ -OSQPInterface::OSQPInterface(const bool enable_warm_start, const c_float eps_abs, const bool polish) -: QPInterface(enable_warm_start), m_work{nullptr, OSQPWorkspaceDeleter} -{ - m_settings = std::make_unique(); - m_data = std::make_unique(); - - if (m_settings) { - osqp_set_default_settings(m_settings.get()); - m_settings->alpha = 1.6; // Change alpha parameter - m_settings->eps_rel = 1.0E-4; - m_settings->eps_abs = eps_abs; - m_settings->eps_prim_inf = 1.0E-4; - m_settings->eps_dual_inf = 1.0E-4; - m_settings->warm_start = true; - m_settings->max_iter = 4000; - m_settings->verbose = false; - m_settings->polish = polish; +namespace autoware::common +{ +OSQPInterface::OSQPInterface( + const bool enable_warm_start, const c_float eps_abs, const c_float eps_rel, const bool polish, + const bool verbose) +: QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter} +{ + settings_ = std::make_unique(); + data_ = std::make_unique(); + + if (settings_) { + osqp_set_default_settings(settings_.get()); + settings_->alpha = 1.6; // Change alpha parameter + settings_->eps_rel = eps_rel; + settings_->eps_abs = eps_abs; + settings_->eps_prim_inf = 1.0E-4; + settings_->eps_dual_inf = 1.0E-4; + settings_->warm_start = true; + settings_->max_iter = 4000; + settings_->verbose = verbose; + settings_->polish = polish; } - m_exitflag = 0; + exitflag_ = 0; } OSQPInterface::OSQPInterface( @@ -61,8 +63,8 @@ OSQPInterface::OSQPInterface( OSQPInterface::~OSQPInterface() { - if (m_data->P) free(m_data->P); - if (m_data->A) free(m_data->A); + if (data_->P) free(data_->P); + if (data_->A) free(data_->A); } void OSQPInterface::initializeProblemImpl( @@ -89,30 +91,30 @@ void OSQPInterface::initializeCSCProblemImpl( /********************** * OBJECTIVE FUNCTION **********************/ - m_param_n = static_cast(q.size()); - m_data->m = static_cast(l.size()); + param_n_ = static_cast(q.size()); + data_->m = static_cast(l.size()); /***************** * POPULATE DATA *****************/ - m_data->n = m_param_n; - if (m_data->P) free(m_data->P); - m_data->P = csc_matrix( - m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), - P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); - m_data->q = q_dyn; - if (m_data->A) free(m_data->A); - m_data->A = csc_matrix( - m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), - A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); - m_data->l = l_dyn; - m_data->u = u_dyn; + data_->n = param_n_; + if (data_->P) free(data_->P); + data_->P = csc_matrix( + data_->n, data_->n, static_cast(P_csc.vals_.size()), P_csc.vals_.data(), + P_csc.row_idxs_.data(), P_csc.col_idxs_.data()); + data_->q = q_dyn; + if (data_->A) free(data_->A); + data_->A = csc_matrix( + data_->m, data_->n, static_cast(A_csc.vals_.size()), A_csc.vals_.data(), + A_csc.row_idxs_.data(), A_csc.col_idxs_.data()); + data_->l = l_dyn; + data_->u = u_dyn; // Setup workspace OSQPWorkspace * workspace; - m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); - m_work.reset(workspace); - m_work_initialized = true; + exitflag_ = osqp_setup(&workspace, data_.get(), settings_.get()); + work_.reset(workspace); + work__initialized = true; } void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept @@ -124,67 +126,67 @@ void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept void OSQPInterface::updateEpsAbs(const double eps_abs) { - m_settings->eps_abs = eps_abs; // for default setting - if (m_work_initialized) { - osqp_update_eps_abs(m_work.get(), eps_abs); // for current work + settings_->eps_abs = eps_abs; // for default setting + if (work__initialized) { + osqp_update_eps_abs(work_.get(), eps_abs); // for current work } } void OSQPInterface::updateEpsRel(const double eps_rel) { - m_settings->eps_rel = eps_rel; // for default setting - if (m_work_initialized) { - osqp_update_eps_rel(m_work.get(), eps_rel); // for current work + settings_->eps_rel = eps_rel; // for default setting + if (work__initialized) { + osqp_update_eps_rel(work_.get(), eps_rel); // for current work } } void OSQPInterface::updateMaxIter(const int max_iter) { - m_settings->max_iter = max_iter; // for default setting - if (m_work_initialized) { - osqp_update_max_iter(m_work.get(), max_iter); // for current work + settings_->max_iter = max_iter; // for default setting + if (work__initialized) { + osqp_update_max_iter(work_.get(), max_iter); // for current work } } void OSQPInterface::updateVerbose(const bool is_verbose) { - m_settings->verbose = is_verbose; // for default setting - if (m_work_initialized) { - osqp_update_verbose(m_work.get(), is_verbose); // for current work + settings_->verbose = is_verbose; // for default setting + if (work__initialized) { + osqp_update_verbose(work_.get(), is_verbose); // for current work } } void OSQPInterface::updateRhoInterval(const int rho_interval) { - m_settings->adaptive_rho_interval = rho_interval; // for default setting + settings_->adaptive_rho_interval = rho_interval; // for default setting } void OSQPInterface::updateRho(const double rho) { - m_settings->rho = rho; - if (m_work_initialized) { - osqp_update_rho(m_work.get(), rho); + settings_->rho = rho; + if (work__initialized) { + osqp_update_rho(work_.get(), rho); } } void OSQPInterface::updateAlpha(const double alpha) { - m_settings->alpha = alpha; - if (m_work_initialized) { - osqp_update_alpha(m_work.get(), alpha); + settings_->alpha = alpha; + if (work__initialized) { + osqp_update_alpha(work_.get(), alpha); } } void OSQPInterface::updateScaling(const int scaling) { - m_settings->scaling = scaling; + settings_->scaling = scaling; } void OSQPInterface::updatePolish(const bool polish) { - m_settings->polish = polish; - if (m_work_initialized) { - osqp_update_polish(m_work.get(), polish); + settings_->polish = polish; + if (work__initialized) { + osqp_update_polish(work_.get(), polish); } } @@ -195,9 +197,9 @@ void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter return; } - m_settings->polish_refine_iter = polish_refine_iter; - if (m_work_initialized) { - osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); + settings_->polish_refine_iter = polish_refine_iter; + if (work__initialized) { + osqp_update_polish_refine_iter(work_.get(), polish_refine_iter); } } @@ -208,9 +210,9 @@ void OSQPInterface::updateCheckTermination(const int check_termination) return; } - m_settings->check_termination = check_termination; - if (m_work_initialized) { - osqp_update_check_termination(m_work.get(), check_termination); + settings_->check_termination = check_termination; + if (work__initialized) { + osqp_update_check_termination(work_.get(), check_termination); } } @@ -222,11 +224,11 @@ bool OSQPInterface::setWarmStart( bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) { - if (!m_work_initialized) { + if (!work__initialized) { return false; } - const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); + const auto result = osqp_warm_start_x(work_.get(), primal_variables.data()); if (result != 0) { std::cerr << "Failed to set primal variables for warm start" << std::endl; return false; @@ -237,11 +239,11 @@ bool OSQPInterface::setPrimalVariables(const std::vector & primal_variab bool OSQPInterface::setDualVariables(const std::vector & dual_variables) { - if (!m_work_initialized) { + if (!work__initialized) { return false; } - const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); + const auto result = osqp_warm_start_y(work_.get(), dual_variables.data()); if (result != 0) { std::cerr << "Failed to set dual variables for warm start" << std::endl; return false; @@ -250,27 +252,6 @@ bool OSQPInterface::setDualVariables(const std::vector & dual_variables) return true; } -void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const -{ - const int status = getStatus(); - if (status == 1) { - // No need to log since optimization was solved. - return; - } - - // create message - std::string output_message = ""; - if (prefix_message != "") { - output_message = prefix_message + " "; - } - - const auto status_message = getStatusMessage(); - output_message += "Optimization failed due to " + status_message; - - // log with warning - RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); -} - void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) { /* @@ -283,14 +264,12 @@ void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) c_int P_elem_N = P_sparse.nonZeros(); */ CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); + osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); } void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) { - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); + osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); } void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) @@ -303,36 +282,34 @@ void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) c_int A_elem_N = A_sparse.nonZeros(); */ CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); return; } void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) { - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); } void OSQPInterface::updateQ(const std::vector & q_new) { std::vector q_tmp(q_new.begin(), q_new.end()); double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(m_work.get(), q_dyn); + osqp_update_lin_cost(work_.get(), q_dyn); } void OSQPInterface::updateL(const std::vector & l_new) { std::vector l_tmp(l_new.begin(), l_new.end()); double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(m_work.get(), l_dyn); + osqp_update_lower_bound(work_.get(), l_dyn); } void OSQPInterface::updateU(const std::vector & u_new) { std::vector u_tmp(u_new.begin(), u_new.end()); double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(m_work.get(), u_dyn); + osqp_update_upper_bound(work_.get(), u_dyn); } void OSQPInterface::updateBounds( @@ -342,45 +319,50 @@ void OSQPInterface::updateBounds( std::vector u_tmp(u_new.begin(), u_new.end()); double * l_dyn = l_tmp.data(); double * u_dyn = u_tmp.data(); - osqp_update_bounds(m_work.get(), l_dyn, u_dyn); + osqp_update_bounds(work_.get(), l_dyn, u_dyn); +} + +int OSQPInterface::getIterationNumber() const +{ + return work_->info->iter; } -int OSQPInterface::getIteration() const +std::string OSQPInterface::getStatus() const { - return m_work->info->iter; + return "OSQP_SOLVED"; } -int OSQPInterface::getStatus() const +bool OSQPInterface::isSolved() const { - return static_cast(m_latest_work_info.status_val); + return latest_work_info_.status_val == 1; } int OSQPInterface::getPolishStatus() const { - return static_cast(m_latest_work_info.status_polish); + return static_cast(latest_work_info_.status_polish); } std::vector OSQPInterface::getDualSolution() const { - double * sol_y = m_work->solution->y; - std::vector dual_solution(sol_y, sol_y + m_data->m); + double * sol_y = work_->solution->y; + std::vector dual_solution(sol_y, sol_y + data_->m); return dual_solution; } std::vector OSQPInterface::optimizeImpl() { - osqp_solve(m_work.get()); + osqp_solve(work_.get()); - double * sol_x = m_work->solution->x; - double * sol_y = m_work->solution->y; - std::vector sol_primal(sol_x, sol_x + m_param_n); - std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); + double * sol_x = work_->solution->x; + double * sol_y = work_->solution->y; + std::vector sol_primal(sol_x, sol_x + param_n_); + std::vector sol_lagrange_multiplier(sol_y, sol_y + data_->m); - m_latest_work_info = *(m_work->info); + latest_work_info_ = *(work_->info); - if (!m_enable_warm_start) { - m_work.reset(); - m_work_initialized = false; + if (!enable_warm_start_) { + work_.reset(); + work__initialized = false; } return sol_primal; @@ -393,7 +375,17 @@ std::vector OSQPInterface::optimize( initializeCSCProblemImpl(P, A, q, l, u); const auto result = optimizeImpl(); + // show polish status if not successful + const int status_polish = static_cast(latest_work_info_.status_polish); + if (status_polish != 1) { + const auto msg = status_polish == 0 ? "unperformed" + : status_polish == -1 ? "unsuccessful" + : "unknown"; + std::cerr << "osqp polish process failed : " << msg << ". The result may be inaccurate" + << std::endl; + } + return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/proxqp_interface.cpp b/common/qp_interface/src/proxqp_interface.cpp index bf1f0229e1e21..507f3858cbf1b 100644 --- a/common/qp_interface/src/proxqp_interface.cpp +++ b/common/qp_interface/src/proxqp_interface.cpp @@ -14,12 +14,17 @@ #include "qp_interface/proxqp_interface.hpp" -namespace qp +namespace autoware::common { -ProxQPInterface::ProxQPInterface(const bool enable_warm_start, const double eps_abs) +using proxsuite::proxqp::QPSolverOutput; + +ProxQPInterface::ProxQPInterface( + const bool enable_warm_start, const double eps_abs, const double eps_rel, const bool verbose) : QPInterface(enable_warm_start) { - m_settings.eps_abs = eps_abs; + settings_.eps_abs = eps_abs; + settings_.eps_rel = eps_rel; + settings_.verbose = verbose; } void ProxQPInterface::initializeProblemImpl( @@ -31,28 +36,28 @@ void ProxQPInterface::initializeProblemImpl( const bool enable_warm_start = [&]() { if ( - !m_enable_warm_start // Warm start is designated - || !m_qp_ptr // QP pointer is initialized + !enable_warm_start_ // Warm start is designated + || !qp_ptr_ // QP pointer is initialized // The number of variables is the same as the previous one. - || !m_variables_num || - *m_variables_num != variables_num + || !variables_num_ || + *variables_num_ != variables_num // The number of constraints is the same as the previous one - || !m_constraints_num || *m_constraints_num != constraints_num) { + || !constraints_num_ || *constraints_num_ != constraints_num) { return false; } return true; }(); if (!enable_warm_start) { - m_qp_ptr = std::make_shared>( + qp_ptr_ = std::make_shared>( variables_num, 0, constraints_num); } - m_settings.initial_guess = + settings_.initial_guess = enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; - m_qp_ptr->settings = m_settings; + qp_ptr_->settings = settings_; Eigen::SparseMatrix P_sparse(variables_num, constraints_num); P_sparse = P.sparseView(); @@ -69,53 +74,78 @@ void ProxQPInterface::initializeProblemImpl( Eigen::Map(u_std_vec.data(), u_std_vec.size()); if (enable_warm_start) { - m_qp_ptr->update( + qp_ptr_->update( P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); } else { - m_qp_ptr->init( + qp_ptr_->init( P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); } } void ProxQPInterface::updateEpsAbs(const double eps_abs) { - m_settings.eps_abs = eps_abs; + settings_.eps_abs = eps_abs; } void ProxQPInterface::updateEpsRel(const double eps_rel) { - m_settings.eps_rel = eps_rel; + settings_.eps_rel = eps_rel; } void ProxQPInterface::updateVerbose(const bool is_verbose) { - m_settings.verbose = is_verbose; + settings_.verbose = is_verbose; } -int ProxQPInterface::getIteration() const +bool ProxQPInterface::isSolved() const { - if (m_qp_ptr) { - return m_qp_ptr->results.info.iter; + if (qp_ptr_) { + return qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED; } - return 0; + return false; } -int ProxQPInterface::getStatus() const +int ProxQPInterface::getIterationNumber() const { - if (m_qp_ptr) { - return static_cast(m_qp_ptr->results.info.status); + if (qp_ptr_) { + return qp_ptr_->results.info.iter; } return 0; } +std::string ProxQPInterface::getStatus() const +{ + if (qp_ptr_) { + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED) { + return "PROXQP_SOLVED"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_MAX_ITER_REACHED) { + return "PROXQP_MAX_ITER_REACHED"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { + return "PROXQP_PRIMAL_INFEASIBLE"; + } + // if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE) { + // return "PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE"; + // } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_DUAL_INFEASIBLE) { + return "PROXQP_DUAL_INFEASIBLE"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_NOT_RUN) { + return "PROXQP_NOT_RUN"; + } + } + return "None"; +} + std::vector ProxQPInterface::optimizeImpl() { - m_qp_ptr->solve(); + qp_ptr_->solve(); std::vector result; - for (Eigen::Index i = 0; i < m_qp_ptr->results.x.size(); ++i) { - result.push_back(m_qp_ptr->results.x[i]); + for (Eigen::Index i = 0; i < qp_ptr_->results.x.size(); ++i) { + result.push_back(qp_ptr_->results.x[i]); } return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/qp_interface.cpp b/common/qp_interface/src/qp_interface.cpp index e7a69bac0c67c..afd26132d7769 100644 --- a/common/qp_interface/src/qp_interface.cpp +++ b/common/qp_interface/src/qp_interface.cpp @@ -18,7 +18,7 @@ #include #include -namespace qp +namespace autoware::common { void QPInterface::initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -54,8 +54,8 @@ void QPInterface::initializeProblem( initializeProblemImpl(P, A, q, l, u); - m_variables_num = q.size(); - m_constraints_num = l.size(); + variables_num_ = q.size(); + constraints_num_ = l.size(); } std::vector QPInterface::optimize( @@ -67,4 +67,4 @@ std::vector QPInterface::optimize( return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/test/test_csc_matrix_conv.cpp b/common/qp_interface/test/test_csc_matrix_conv.cpp index fc604d762d2c4..a6bd5e3df0be1 100644 --- a/common/qp_interface/test/test_csc_matrix_conv.cpp +++ b/common/qp_interface/test/test_csc_matrix_conv.cpp @@ -23,42 +23,42 @@ TEST(TestCscMatrixConv, Nominal) { - using qp::calCSCMatrix; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrix; + using autoware::common::CSC_Matrix; Eigen::MatrixXd rect1(1, 2); rect1 << 0.0, 1.0; const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_vals[0], 1.0); - ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); - ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); + ASSERT_EQ(rect_m1.vals_.size(), size_t(1)); + EXPECT_EQ(rect_m1.vals_[0], 1.0); + ASSERT_EQ(rect_m1.row_idxs_.size(), size_t(1)); + EXPECT_EQ(rect_m1.row_idxs_[0], c_int(0)); + ASSERT_EQ(rect_m1.col_idxs_.size(), size_t(3)); // nb of columns + 1 + EXPECT_EQ(rect_m1.col_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m1.col_idxs_[1], c_int(0)); + EXPECT_EQ(rect_m1.col_idxs_[2], c_int(1)); Eigen::MatrixXd rect2(2, 4); rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_vals[0], 1.0); - EXPECT_EQ(rect_m2.m_vals[1], 6.0); - EXPECT_EQ(rect_m2.m_vals[2], 3.0); - EXPECT_EQ(rect_m2.m_vals[3], 7.0); - ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); - ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); - EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); + ASSERT_EQ(rect_m2.vals_.size(), size_t(4)); + EXPECT_EQ(rect_m2.vals_[0], 1.0); + EXPECT_EQ(rect_m2.vals_[1], 6.0); + EXPECT_EQ(rect_m2.vals_[2], 3.0); + EXPECT_EQ(rect_m2.vals_[3], 7.0); + ASSERT_EQ(rect_m2.row_idxs_.size(), size_t(4)); + EXPECT_EQ(rect_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(rect_m2.row_idxs_[2], c_int(0)); + EXPECT_EQ(rect_m2.row_idxs_[3], c_int(1)); + ASSERT_EQ(rect_m2.col_idxs_.size(), size_t(5)); // nb of columns + 1 + EXPECT_EQ(rect_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m2.col_idxs_[1], c_int(1)); + EXPECT_EQ(rect_m2.col_idxs_[2], c_int(2)); + EXPECT_EQ(rect_m2.col_idxs_[3], c_int(4)); + EXPECT_EQ(rect_m2.col_idxs_[4], c_int(4)); // Example from http://netlib.org/linalg/html_templates/node92.html Eigen::MatrixXd square2(6, 6); @@ -66,59 +66,59 @@ TEST(TestCscMatrixConv, Nominal) 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); - EXPECT_EQ(square_m2.m_vals[0], 10.0); - EXPECT_EQ(square_m2.m_vals[1], 3.0); - EXPECT_EQ(square_m2.m_vals[2], 3.0); - EXPECT_EQ(square_m2.m_vals[3], 9.0); - EXPECT_EQ(square_m2.m_vals[4], 7.0); - EXPECT_EQ(square_m2.m_vals[5], 8.0); - EXPECT_EQ(square_m2.m_vals[6], 4.0); - EXPECT_EQ(square_m2.m_vals[7], 8.0); - EXPECT_EQ(square_m2.m_vals[8], 8.0); - EXPECT_EQ(square_m2.m_vals[9], 7.0); - EXPECT_EQ(square_m2.m_vals[10], 7.0); - EXPECT_EQ(square_m2.m_vals[11], 9.0); - EXPECT_EQ(square_m2.m_vals[12], -2.0); - EXPECT_EQ(square_m2.m_vals[13], 5.0); - EXPECT_EQ(square_m2.m_vals[14], 9.0); - EXPECT_EQ(square_m2.m_vals[15], 2.0); - EXPECT_EQ(square_m2.m_vals[16], 3.0); - EXPECT_EQ(square_m2.m_vals[17], 13.0); - EXPECT_EQ(square_m2.m_vals[18], -1.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); - EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); - EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); - EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); + ASSERT_EQ(square_m2.vals_.size(), size_t(19)); + EXPECT_EQ(square_m2.vals_[0], 10.0); + EXPECT_EQ(square_m2.vals_[1], 3.0); + EXPECT_EQ(square_m2.vals_[2], 3.0); + EXPECT_EQ(square_m2.vals_[3], 9.0); + EXPECT_EQ(square_m2.vals_[4], 7.0); + EXPECT_EQ(square_m2.vals_[5], 8.0); + EXPECT_EQ(square_m2.vals_[6], 4.0); + EXPECT_EQ(square_m2.vals_[7], 8.0); + EXPECT_EQ(square_m2.vals_[8], 8.0); + EXPECT_EQ(square_m2.vals_[9], 7.0); + EXPECT_EQ(square_m2.vals_[10], 7.0); + EXPECT_EQ(square_m2.vals_[11], 9.0); + EXPECT_EQ(square_m2.vals_[12], -2.0); + EXPECT_EQ(square_m2.vals_[13], 5.0); + EXPECT_EQ(square_m2.vals_[14], 9.0); + EXPECT_EQ(square_m2.vals_[15], 2.0); + EXPECT_EQ(square_m2.vals_[16], 3.0); + EXPECT_EQ(square_m2.vals_[17], 13.0); + EXPECT_EQ(square_m2.vals_[18], -1.0); + ASSERT_EQ(square_m2.row_idxs_.size(), size_t(19)); + EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[2], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[3], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[4], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[5], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[6], c_int(5)); + EXPECT_EQ(square_m2.row_idxs_[7], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[8], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[9], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[10], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[11], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[12], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[13], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[14], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[15], c_int(5)); + EXPECT_EQ(square_m2.row_idxs_[16], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[17], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[18], c_int(5)); + ASSERT_EQ(square_m2.col_idxs_.size(), size_t(7)); // nb of columns + 1 + EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[1], c_int(3)); + EXPECT_EQ(square_m2.col_idxs_[2], c_int(7)); + EXPECT_EQ(square_m2.col_idxs_[3], c_int(9)); + EXPECT_EQ(square_m2.col_idxs_[4], c_int(12)); + EXPECT_EQ(square_m2.col_idxs_[5], c_int(16)); + EXPECT_EQ(square_m2.col_idxs_[6], c_int(19)); } TEST(TestCscMatrixConv, Trapezoidal) { - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd square2(3, 3); @@ -129,33 +129,33 @@ TEST(TestCscMatrixConv, Trapezoidal) const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m1.m_vals[0], 1.0); - EXPECT_EQ(square_m1.m_vals[1], 2.0); - EXPECT_EQ(square_m1.m_vals[2], 4.0); - ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); - EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); + ASSERT_EQ(square_m1.vals_.size(), size_t(3)); + EXPECT_EQ(square_m1.vals_[0], 1.0); + EXPECT_EQ(square_m1.vals_[1], 2.0); + EXPECT_EQ(square_m1.vals_[2], 4.0); + ASSERT_EQ(square_m1.row_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m1.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m1.row_idxs_[1], c_int(0)); + EXPECT_EQ(square_m1.row_idxs_[2], c_int(1)); + ASSERT_EQ(square_m1.col_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m1.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m1.col_idxs_[1], c_int(1)); + EXPECT_EQ(square_m1.col_idxs_[2], c_int(3)); const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m2.m_vals[0], 2.0); - EXPECT_EQ(square_m2.m_vals[1], 5.0); - EXPECT_EQ(square_m2.m_vals[2], 6.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); + ASSERT_EQ(square_m2.vals_.size(), size_t(3)); + EXPECT_EQ(square_m2.vals_[0], 2.0); + EXPECT_EQ(square_m2.vals_[1], 5.0); + EXPECT_EQ(square_m2.vals_[2], 6.0); + ASSERT_EQ(square_m2.row_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[2], c_int(1)); + ASSERT_EQ(square_m2.col_idxs_.size(), size_t(4)); + EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[1], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[2], c_int(2)); + EXPECT_EQ(square_m2.col_idxs_[3], c_int(3)); try { const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); @@ -166,10 +166,10 @@ TEST(TestCscMatrixConv, Trapezoidal) } TEST(TestCscMatrixConv, Print) { - using qp::calCSCMatrix; - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; - using qp::printCSCMatrix; + using autoware::common::calCSCMatrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; + using autoware::common::printCSCMatrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd rect1(1, 2); square1 << 1.0, 2.0, 2.0, 4.0; diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp index 5ce1f0eb064a1..9a02dd4b00934 100644 --- a/common/qp_interface/test/test_osqp_interface.cpp +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -39,44 +39,44 @@ namespace TEST(TestOsqpInterface, BasicQp) { - using qp::calCSCMatrix; - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; - auto check_result = []( - const auto & solution, const int solution_status, const int polish_status) { - EXPECT_EQ(solution_status, 1); - EXPECT_EQ(polish_status, 1); + auto check_result = + [](const auto & solution, const std::string & status, const int polish_status) { + EXPECT_EQ(status, "OSQP_SOLVED"); + EXPECT_EQ(polish_status, 1); - static const auto ep = 1.0e-8; + static const auto ep = 1.0e-8; - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; + ASSERT_EQ(solution.size(), size_t(2)); + EXPECT_NEAR(solution[0], 0.3, ep); + EXPECT_NEAR(solution[1], 0.7, ep); + }; const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -qp::INF}; - const std::vector u = {1.0, 0.7, 0.7, qp::INF}; + const std::vector l = {1.0, 0.0, 0.0, -autoware::common::OSQP_INF}; + const std::vector u = {1.0, 0.7, 0.7, autoware::common::OSQP_INF}; { // Define problem during optimization - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { // Define problem during initialization - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { @@ -87,7 +87,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); } @@ -95,12 +95,12 @@ TEST(TestOsqpInterface, BasicQp) // Define problem during initialization with csc matrix CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { @@ -111,7 +111,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization @@ -119,9 +119,9 @@ TEST(TestOsqpInterface, BasicQp) CSC_Matrix A_csc = calCSCMatrix(A); const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } // add warm startup @@ -132,7 +132,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(true, 1e-6); // enable warm start + autoware::common::OSQPInterface osqp(true, 1e-6); // enable warm start osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization @@ -140,9 +140,9 @@ TEST(TestOsqpInterface, BasicQp) CSC_Matrix A_csc = calCSCMatrix(A); { const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); osqp.updateCheckTermination(1); const auto primal_val = solution; @@ -155,9 +155,9 @@ TEST(TestOsqpInterface, BasicQp) { const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } // NOTE: This should be true, but currently optimize function reset the workspace, which diff --git a/common/qp_interface/test/test_proxqp_interface.cpp b/common/qp_interface/test/test_proxqp_interface.cpp index c28678e5c872a..a8b1bb29dafa0 100644 --- a/common/qp_interface/test/test_proxqp_interface.cpp +++ b/common/qp_interface/test/test_proxqp_interface.cpp @@ -40,7 +40,9 @@ namespace TEST(TestProxqpInterface, BasicQp) { - auto check_result = [](const auto & solution) { + auto check_result = [](const auto & solution, const std::string & status) { + EXPECT_EQ(status, "PROXQP_SOLVED"); + static const auto ep = 1.0e-8; ASSERT_EQ(solution.size(), size_t(2)); EXPECT_NEAR(solution[0], 0.3, ep); @@ -55,23 +57,26 @@ TEST(TestProxqpInterface, BasicQp) { // Define problem during optimization - qp::ProxQPInterface proxqp(false, 1e-9); + autoware::common::ProxQPInterface proxqp(false, 1e-9, 1e-9, false); const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); + const auto status = proxqp.getStatus(); + check_result(solution, status); } { // Define problem during optimization with warm start - qp::ProxQPInterface proxqp(true, 1e-9); + autoware::common::ProxQPInterface proxqp(true, 1e-9, 1e-9, false); { const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); - EXPECT_NE(proxqp.getIteration(), 1); + const auto status = proxqp.getStatus(); + check_result(solution, status); + EXPECT_NE(proxqp.getIterationNumber(), 1); } { const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); - EXPECT_EQ(proxqp.getIteration(), 0); + const auto status = proxqp.getStatus(); + check_result(solution, status); + EXPECT_EQ(proxqp.getIterationNumber(), 0); } } }