From 6939a3da23a41781a6d973a8267e2ea1cb3a600d Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Wed, 15 Jan 2025 09:57:09 +0800 Subject: [PATCH] feat(autoware_qp_interface): porting autoware_qp_interface package to autoware.core (#9824) Signed-off-by: NorahXiong --- common/autoware_qp_interface/CHANGELOG.rst | 81 ---- common/autoware_qp_interface/CMakeLists.txt | 64 --- .../design/qp_interface-design.md | 48 --- .../qp_interface/osqp_csc_matrix_conv.hpp | 46 -- .../autoware/qp_interface/osqp_interface.hpp | 147 ------- .../qp_interface/proxqp_interface.hpp | 57 --- .../autoware/qp_interface/qp_interface.hpp | 61 --- common/autoware_qp_interface/package.xml | 30 -- .../src/osqp_csc_matrix_conv.cpp | 134 ------ .../src/osqp_interface.cpp | 394 ------------------ .../src/proxqp_interface.cpp | 157 ------- .../src/qp_interface.cpp | 70 ---- .../test/test_csc_matrix_conv.cpp | 181 -------- .../test/test_osqp_interface.cpp | 170 -------- .../test/test_proxqp_interface.cpp | 85 ---- 15 files changed, 1725 deletions(-) delete mode 100644 common/autoware_qp_interface/CHANGELOG.rst delete mode 100644 common/autoware_qp_interface/CMakeLists.txt delete mode 100644 common/autoware_qp_interface/design/qp_interface-design.md delete mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp delete mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp delete mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp delete mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp delete mode 100644 common/autoware_qp_interface/package.xml delete mode 100644 common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp delete mode 100644 common/autoware_qp_interface/src/osqp_interface.cpp delete mode 100644 common/autoware_qp_interface/src/proxqp_interface.cpp delete mode 100644 common/autoware_qp_interface/src/qp_interface.cpp delete mode 100644 common/autoware_qp_interface/test/test_csc_matrix_conv.cpp delete mode 100644 common/autoware_qp_interface/test/test_osqp_interface.cpp delete mode 100644 common/autoware_qp_interface/test/test_proxqp_interface.cpp diff --git a/common/autoware_qp_interface/CHANGELOG.rst b/common/autoware_qp_interface/CHANGELOG.rst deleted file mode 100644 index f76dac861e1e6..0000000000000 --- a/common/autoware_qp_interface/CHANGELOG.rst +++ /dev/null @@ -1,81 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_qp_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* fix: fix package names in changelog files (`#9500 `_) -* fix(autoware_osqp_interface): fix clang-tidy errors (`#9440 `_) -* 0.39.0 -* update changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* fix(qp_interface): fix unreadVariable (`#8349 `_) - * fix:unreadVariable - * fix:unreadVariable - * fix:unreadVariable - --------- -* perf(velocity_smoother): use ProxQP for faster optimization (`#8028 `_) - * perf(velocity_smoother): use ProxQP for faster optimization - * consider max_iteration - * disable warm start - * fix test - --------- -* refactor(qp_interface): clean up the code (`#8029 `_) - * refactor qp_interface - * variable names: m_XXX -> XXX\_ - * update - * update - --------- -* fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions (`#7855 `_) - * fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Ryuta Kambe, Takayuki Murooka, Yutaka Kondo, kobayu858 - -0.26.0 (2024-04-03) -------------------- -* feat(qp_interface): support proxqp (`#3699 `_) - * feat(qp_interface): add proxqp interface - * update - --------- -* feat(qp_interface): add new package which will contain various qp solvers (`#3697 `_) -* Contributors: Takayuki Murooka diff --git a/common/autoware_qp_interface/CMakeLists.txt b/common/autoware_qp_interface/CMakeLists.txt deleted file mode 100644 index 1df75d2d719a0..0000000000000 --- a/common/autoware_qp_interface/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_qp_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) -find_package(proxsuite REQUIRED) - -# after find_package(osqp_vendor) in ament_auto_find_build_dependencies -find_package(osqp REQUIRED) -get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) -get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) - -set(QP_INTERFACE_LIB_SRC - src/qp_interface.cpp - src/osqp_interface.cpp - src/osqp_csc_matrix_conv.cpp - src/proxqp_interface.cpp -) - -set(QP_INTERFACE_LIB_HEADERS - include/autoware/qp_interface/qp_interface.hpp - include/autoware/qp_interface/osqp_interface.hpp - include/autoware/qp_interface/osqp_csc_matrix_conv.hpp - include/autoware/qp_interface/proxqp_interface.hpp -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - ${QP_INTERFACE_LIB_SRC} - ${QP_INTERFACE_LIB_HEADERS} -) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) - -target_include_directories(${PROJECT_NAME} - SYSTEM PUBLIC - "${OSQP_INCLUDE_DIR}" - "${EIGEN3_INCLUDE_DIR}" -) - -ament_target_dependencies(${PROJECT_NAME} - Eigen3 - osqp_vendor - proxsuite -) - -# crucial so downstream package builds because osqp_interface exposes osqp.hpp -ament_export_include_directories("${OSQP_INCLUDE_DIR}") -# crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a -ament_export_libraries(osqp::osqp) - -if(BUILD_TESTING) - set(TEST_SOURCES - test/test_osqp_interface.cpp - test/test_csc_matrix_conv.cpp - test/test_proxqp_interface.cpp - ) - set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) - ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/common/autoware_qp_interface/design/qp_interface-design.md b/common/autoware_qp_interface/design/qp_interface-design.md deleted file mode 100644 index edc7fa9845206..0000000000000 --- a/common/autoware_qp_interface/design/qp_interface-design.md +++ /dev/null @@ -1,48 +0,0 @@ -# Interface for QP solvers - -This is the design document for the `autoware_qp_interface` package. - -## Purpose / Use cases - -This packages provides a C++ interface for QP solvers. -Currently, supported QP solvers are - -- [OSQP library](https://osqp.org/docs/solver/index.html) - -## Design - -The class `QPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into -C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the QP workspace dataholder, and runs the optimizer. - -## Inputs / Outputs / API - -The interface can be used in several ways: - -1. Initialize the interface, and load the problem formulation at the optimization call. - - ```cpp - QPInterface qp_interface; - qp_interface.optimize(P, A, q, l, u); - ``` - -2. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. - - ```cpp - QPInterface qp_interface(true); - qp_interface.optimize(P, A, q, l, u); - qp_interface.optimize(P_new, A_new, q_new, l_new, u_new); - ``` - - The optimization results are returned as a vector by the optimization function. - - ```cpp - const auto solution = qp_interface.optimize(); - double x_0 = solution[0]; - double x_1 = solution[1]; - ``` - -## References / External links - -- OSQP library: - -## Related issues diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp deleted file mode 100644 index 9575d9d18c69f..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ -#define AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ - -#include "osqp/glob_opts.h" - -#include - -#include - -namespace autoware::qp_interface -{ -/// \brief Compressed-Column-Sparse Matrix -struct CSC_Matrix -{ - /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector vals_; - /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector row_idxs_; - /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector col_idxs_; -}; - -/// \brief Calculate CSC matrix from Eigen matrix -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); -/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix -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 autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp deleted file mode 100644 index a5777dd545e9f..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp +++ /dev/null @@ -1,147 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" -#include "autoware/qp_interface/qp_interface.hpp" -#include "osqp/osqp.h" - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -constexpr c_float OSQP_INF = 1e30; - -class OSQPInterface : public QPInterface -{ -public: - /// \brief Constructor without problem formulation - OSQPInterface( - const bool enable_warm_start = false, const int max_iteration = 20000, - 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. - /// \param q: (n) vector defining the linear cost of the problem. - /// \param l: (m) vector defining the lower bound problem constraint. - /// \param u: (m) vector defining the upper bound problem constraint. - /// \param eps_abs: Absolute convergence tolerance. - OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, - const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon()); - OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, - const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon()); - ~OSQPInterface(); - - static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; - - std::vector optimize( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - int getIterationNumber() const override; - bool isSolved() const override; - std::string getStatus() const override; - - int getPolishStatus() const; - std::vector getDualSolution() const; - - void updateEpsAbs(const double eps_abs) override; - void updateEpsRel(const double eps_rel) override; - void updateVerbose(const bool verbose) override; - - // Updates problem parameters while keeping solution in memory. - // - // Args: - // P_new: (n,n) matrix defining relations between parameters. - // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q_new: (n) vector defining the linear cost of the problem. - // l_new: (m) vector defining the lower bound problem constraint. - // u_new: (m) vector defining the upper bound problem constraint. - void updateP(const Eigen::MatrixXd & P_new); - void updateCscP(const CSC_Matrix & P_csc); - void updateA(const Eigen::MatrixXd & A_new); - void updateCscA(const CSC_Matrix & A_csc); - void updateQ(const std::vector & q_new); - void updateL(const std::vector & l_new); - void updateU(const std::vector & u_new); - void updateBounds(const std::vector & l_new, const std::vector & u_new); - - void updateMaxIter(const int iter); - void updateRhoInterval(const int rho_interval); - void updateRho(const double rho); - void updateAlpha(const double alpha); - void updateScaling(const int scaling); - void updatePolish(const bool polish); - void updatePolishRefinementIteration(const int polish_refine_iter); - 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(latest_work_info_.iter); } - /// \brief Get the status message for the latest problem solved - inline std::string getStatusMessage() const - { - return static_cast(latest_work_info_.status); - } - /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return latest_work_info_.run_time; } - /// \brief Get the objective value the latest problem solved - 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 exitflag_; } - - // Setter functions for warm start - bool setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables); - bool setPrimalVariables(const std::vector & primal_variables); - bool setDualVariables(const std::vector & dual_variables); - -private: - 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 latest_work_info_; - // Number of parameters to optimize - int64_t param_n_; - // Flag to check if the current work exists - bool work__initialized = false; - // Exitflag - int64_t exitflag_; - - void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) override; - - void initializeCSCProblemImpl( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - std::vector optimizeImpl() override; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp deleted file mode 100644 index 324da4b18c6fd..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ - -#include "autoware/qp_interface/qp_interface.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -class ProxQPInterface : public QPInterface -{ -public: - explicit ProxQPInterface( - const bool enable_warm_start, const int max_iteration, const double eps_abs, - const double eps_rel, const bool verbose = false); - - 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 settings_{}; - std::shared_ptr> qp_ptr_{nullptr}; - - void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) override; - - std::vector optimizeImpl() override; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp deleted file mode 100644 index be3c598512bf6..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ - -#include - -#include -#include -#include - -namespace autoware::qp_interface -{ -class QPInterface -{ -public: - 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 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 enable_warm_start_{false}; - - void initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - virtual void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) = 0; - - virtual std::vector optimizeImpl() = 0; - - std::optional variables_num_{std::nullopt}; - std::optional constraints_num_{std::nullopt}; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/package.xml b/common/autoware_qp_interface/package.xml deleted file mode 100644 index b61d03a36db72..0000000000000 --- a/common/autoware_qp_interface/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_qp_interface - 0.40.0 - Interface for the QP solvers - Takayuki Murooka - Fumiya Watanabe - Maxime CLEMENT - Satoshi Ota - Apache 2.0 - - ament_cmake_auto - autoware_cmake - - eigen - osqp_vendor - proxsuite - rclcpp - rclcpp_components - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp b/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp deleted file mode 100644 index 15314a9e4a5fa..0000000000000 --- a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp +++ /dev/null @@ -1,134 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" - -#include -#include - -#include -#include -#include - -namespace autoware::qp_interface -{ -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i < rows; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - if (rows != cols) { - throw std::invalid_argument("Matrix must be square (n, n)"); - } - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - Eigen::Index trap_last_idx = 0; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - trap_last_idx += 1; - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -void printCSCMatrix(const CSC_Matrix & csc_mat) -{ - std::cout << "["; - for (const c_float & val : csc_mat.vals_) { - std::cout << val << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & row : csc_mat.row_idxs_) { - std::cout << row << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & col : csc_mat.col_idxs_) { - std::cout << col << ", "; - } - std::cout << "]\n"; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/osqp_interface.cpp b/common/autoware_qp_interface/src/osqp_interface.cpp deleted file mode 100644 index fbb8e71f4c251..0000000000000 --- a/common/autoware_qp_interface/src/osqp_interface.cpp +++ /dev/null @@ -1,394 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_interface.hpp" - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -OSQPInterface::OSQPInterface( - const bool enable_warm_start, const int max_iteration, 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 = enable_warm_start; - settings_->max_iter = max_iteration; - settings_->verbose = verbose; - settings_->polish = polish; - } - exitflag_ = 0; -} - -OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const bool enable_warm_start, - const c_float eps_abs) -: OSQPInterface(enable_warm_start, eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const bool enable_warm_start, - const c_float eps_abs) -: OSQPInterface(enable_warm_start, eps_abs) -{ - initializeCSCProblemImpl(P, A, q, l, u); -} - -OSQPInterface::~OSQPInterface() -{ - if (data_->P) free(data_->P); - if (data_->A) free(data_->A); -} - -void OSQPInterface::initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - initializeCSCProblemImpl(P_csc, A_csc, q, l, u); -} - -void OSQPInterface::initializeCSCProblemImpl( - CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector u_tmp(u.begin(), u.end()); - double * q_dyn = q_tmp.data(); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - - /********************** - * OBJECTIVE FUNCTION - **********************/ - param_n_ = static_cast(q.size()); - data_->m = static_cast(l.size()); - - /***************** - * POPULATE DATA - *****************/ - 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; - exitflag_ = osqp_setup(&workspace, data_.get(), settings_.get()); - work_.reset(workspace); - work__initialized = true; -} - -void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept -{ - if (ptr != nullptr) { - osqp_cleanup(ptr); - } -} - -void OSQPInterface::updateEpsAbs(const double eps_abs) -{ - 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) -{ - 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) -{ - 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) -{ - 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) -{ - settings_->adaptive_rho_interval = rho_interval; // for default setting -} - -void OSQPInterface::updateRho(const double rho) -{ - settings_->rho = rho; - if (work__initialized) { - osqp_update_rho(work_.get(), rho); - } -} - -void OSQPInterface::updateAlpha(const double alpha) -{ - settings_->alpha = alpha; - if (work__initialized) { - osqp_update_alpha(work_.get(), alpha); - } -} - -void OSQPInterface::updateScaling(const int scaling) -{ - settings_->scaling = scaling; -} - -void OSQPInterface::updatePolish(const bool polish) -{ - settings_->polish = polish; - if (work__initialized) { - osqp_update_polish(work_.get(), polish); - } -} - -void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) -{ - if (polish_refine_iter < 0) { - std::cerr << "Polish refinement iterations must be positive" << std::endl; - return; - } - - settings_->polish_refine_iter = polish_refine_iter; - if (work__initialized) { - osqp_update_polish_refine_iter(work_.get(), polish_refine_iter); - } -} - -void OSQPInterface::updateCheckTermination(const int check_termination) -{ - if (check_termination < 0) { - std::cerr << "Check termination must be positive" << std::endl; - return; - } - - settings_->check_termination = check_termination; - if (work__initialized) { - osqp_update_check_termination(work_.get(), check_termination); - } -} - -bool OSQPInterface::setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables) -{ - return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); -} - -bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) -{ - if (!work__initialized) { - return false; - } - - 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; - } - - return true; -} - -bool OSQPInterface::setDualVariables(const std::vector & dual_variables) -{ - if (!work__initialized) { - return false; - } - - 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; - } - - return true; -} - -void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) -{ - /* - // Transform 'P' into an 'upper trapezoidal matrix' - Eigen::MatrixXd P_trap = P_new.triangularView(); - // Transform 'P' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix P_sparse = P_trap.sparseView(); - double *P_val_ptr = P_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int P_elem_N = P_sparse.nonZeros(); - */ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - 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(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); -} - -void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) -{ - /* - // Transform 'A' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix A_sparse = A_new.sparseView(); - double *A_val_ptr = A_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int A_elem_N = A_sparse.nonZeros(); - */ - CSC_Matrix A_csc = calCSCMatrix(A_new); - 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(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(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(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(work_.get(), u_dyn); -} - -void OSQPInterface::updateBounds( - const std::vector & l_new, const std::vector & u_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - 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(work_.get(), l_dyn, u_dyn); -} - -int OSQPInterface::getIterationNumber() const -{ - return work_->info->iter; -} - -std::string OSQPInterface::getStatus() const -{ - return "OSQP_SOLVED"; -} - -bool OSQPInterface::isSolved() const -{ - return latest_work_info_.status_val == 1; -} - -int OSQPInterface::getPolishStatus() const -{ - return static_cast(latest_work_info_.status_polish); -} - -std::vector OSQPInterface::getDualSolution() const -{ - 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(work_.get()); - - double * sol_x = work_->solution->x; - std::vector sol_primal(sol_x, sol_x + param_n_); - - latest_work_info_ = *(work_->info); - - if (!enable_warm_start_) { - work_.reset(); - work__initialized = false; - } - - return sol_primal; -} - -std::vector OSQPInterface::optimize( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - 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 autoware::qp_interface diff --git a/common/autoware_qp_interface/src/proxqp_interface.cpp b/common/autoware_qp_interface/src/proxqp_interface.cpp deleted file mode 100644 index a0ebbf0db0510..0000000000000 --- a/common/autoware_qp_interface/src/proxqp_interface.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/proxqp_interface.hpp" - -#include -#include -#include - -namespace autoware::qp_interface -{ -using proxsuite::proxqp::QPSolverOutput; - -ProxQPInterface::ProxQPInterface( - const bool enable_warm_start, const int max_iteration, const double eps_abs, const double eps_rel, - const bool verbose) -: QPInterface(enable_warm_start) -{ - settings_.max_iter = max_iteration; - settings_.eps_abs = eps_abs; - settings_.eps_rel = eps_rel; - settings_.verbose = verbose; -} - -void ProxQPInterface::initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - const size_t variables_num = q.size(); - const size_t constraints_num = l.size(); - - const bool enable_warm_start = [&]() { - if ( - !enable_warm_start_ // Warm start is designated - || !qp_ptr_ // QP pointer is initialized - // The number of variables is the same as the previous one. - || !variables_num_ || - *variables_num_ != variables_num - // The number of constraints is the same as the previous one - || !constraints_num_ || *constraints_num_ != constraints_num) { - return false; - } - return true; - }(); - - if (!enable_warm_start) { - qp_ptr_ = std::make_shared>( - variables_num, 0, constraints_num); - } - - settings_.initial_guess = - enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT - : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; - - qp_ptr_->settings = settings_; - - Eigen::SparseMatrix P_sparse(variables_num, constraints_num); - P_sparse = P.sparseView(); - - // NOTE: const std vector cannot be converted to eigen vector - std::vector non_const_q = q; - Eigen::VectorXd eigen_q = - Eigen::Map(non_const_q.data(), non_const_q.size()); - std::vector l_std_vec = l; - Eigen::VectorXd eigen_l = - Eigen::Map(l_std_vec.data(), l_std_vec.size()); - std::vector u_std_vec = u; - Eigen::VectorXd eigen_u = - Eigen::Map(u_std_vec.data(), u_std_vec.size()); - - if (enable_warm_start) { - qp_ptr_->update( - P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); - } else { - qp_ptr_->init( - P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); - } -} - -void ProxQPInterface::updateEpsAbs(const double eps_abs) -{ - settings_.eps_abs = eps_abs; -} - -void ProxQPInterface::updateEpsRel(const double eps_rel) -{ - settings_.eps_rel = eps_rel; -} - -void ProxQPInterface::updateVerbose(const bool is_verbose) -{ - settings_.verbose = is_verbose; -} - -bool ProxQPInterface::isSolved() const -{ - if (qp_ptr_) { - return qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED; - } - return false; -} - -int ProxQPInterface::getIterationNumber() const -{ - 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() -{ - qp_ptr_->solve(); - - std::vector result; - for (Eigen::Index i = 0; i < qp_ptr_->results.x.size(); ++i) { - result.push_back(qp_ptr_->results.x[i]); - } - return result; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/qp_interface.cpp b/common/autoware_qp_interface/src/qp_interface.cpp deleted file mode 100644 index f01e57772dc4f..0000000000000 --- a/common/autoware_qp_interface/src/qp_interface.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/qp_interface.hpp" - -#include -#include -#include - -namespace autoware::qp_interface -{ -void QPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // check if arguments are valid - std::stringstream ss; - if (P.rows() != P.cols()) { - ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() - << ", P.cols() = " << P.cols(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != static_cast(q.size())) { - ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() - << ", q.size() = " << q.size(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != A.cols()) { - ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() - << ", A.cols() = " << A.cols(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(l.size())) { - ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() - << ", l.size() = " << l.size(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(u.size())) { - ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() - << ", u.size() = " << u.size(); - throw std::invalid_argument(ss.str()); - } - - initializeProblemImpl(P, A, q, l, u); - - variables_num_ = q.size(); - constraints_num_ = l.size(); -} - -std::vector QPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - initializeProblem(P, A, q, l, u); - const auto result = optimizeImpl(); - - return result; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp deleted file mode 100644 index 1f377a1a24535..0000000000000 --- a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -TEST(TestCscMatrixConv, Nominal) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::CSC_Matrix; - - Eigen::MatrixXd rect1(1, 2); - rect1 << 0.0, 1.0; - - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - 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.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); - square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, - 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.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 autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd square2(3, 3); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; - rect1 << 0.0, 1.0; - - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - // Trapezoidal: skip the lower left triangle (2.0 in this example) - 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.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); - FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; - } catch (const std::invalid_argument & e) { - EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); - } -} -TEST(TestCscMatrixConv, Print) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - using autoware::qp_interface::printCSCMatrix; - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - rect1 << 0.0, 1.0; - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - printCSCMatrix(square_m1); - printCSCMatrix(rect_m1); -} diff --git a/common/autoware_qp_interface/test/test_osqp_interface.cpp b/common/autoware_qp_interface/test/test_osqp_interface.cpp deleted file mode 100644 index f97cc2888afa0..0000000000000 --- a/common/autoware_qp_interface/test/test_osqp_interface.cpp +++ /dev/null @@ -1,170 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include -#include - -namespace -{ -// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestOsqpInterface, BasicQp) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - - 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; - - 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, -autoware::qp_interface::OSQP_INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::qp_interface::OSQP_INF}; - - { - // Define problem during optimization - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - // Define problem during initialization - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem - Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); - } - - { - // Define problem during initialization with csc matrix - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - // add warm startup - { - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(true, 4000, 1e-6); // enable warm start - osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - { - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - - osqp.updateCheckTermination(1); - const auto primal_val = solution; - const auto dual_val = osqp.getDualSolution(); - for (size_t i = 0; i < primal_val.size(); ++i) { - std::cerr << primal_val.at(i) << std::endl; - } - osqp.setWarmStart(primal_val, dual_val); - } - - { - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - // NOTE: This should be true, but currently optimize function reset the workspace, which - // disables warm start. - // EXPECT_EQ(osqp.getTakenIter(), 1); - } -} -} // namespace diff --git a/common/autoware_qp_interface/test/test_proxqp_interface.cpp b/common/autoware_qp_interface/test/test_proxqp_interface.cpp deleted file mode 100644 index e47af10c7aabd..0000000000000 --- a/common/autoware_qp_interface/test/test_proxqp_interface.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/proxqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include -#include - -namespace -{ -// Problem taken from -// https://github.com/proxqp/proxqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestProxqpInterface, BasicQp) -{ - 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); - 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, -std::numeric_limits::max()}; - const std::vector u = {1.0, 0.7, 0.7, std::numeric_limits::max()}; - - { - // Define problem during optimization - autoware::qp_interface::ProxQPInterface proxqp(false, 4000, 1e-9, 1e-9, false); - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - } - - { - // Define problem during optimization with warm start - autoware::qp_interface::ProxQPInterface proxqp(true, 4000, 1e-9, 1e-9, false); - { - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - 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); - const auto status = proxqp.getStatus(); - check_result(solution, status); - EXPECT_EQ(proxqp.getIterationNumber(), 0); - } - } -} -} // namespace