From 2deebe1887416590f11d49a6d6b817e760ea6bc1 Mon Sep 17 00:00:00 2001 From: NorahXiong Date: Thu, 26 Dec 2024 11:12:17 +0800 Subject: [PATCH 1/3] feat(autoware_qp_interface): porting autoware_qp_interface package from autoware.universe 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 insertions(+) create mode 100644 common/autoware_qp_interface/CHANGELOG.rst create mode 100644 common/autoware_qp_interface/CMakeLists.txt create mode 100644 common/autoware_qp_interface/design/qp_interface-design.md create mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp create mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp create mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp create mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp create mode 100644 common/autoware_qp_interface/package.xml create mode 100644 common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp create mode 100644 common/autoware_qp_interface/src/osqp_interface.cpp create mode 100644 common/autoware_qp_interface/src/proxqp_interface.cpp create mode 100644 common/autoware_qp_interface/src/qp_interface.cpp create mode 100644 common/autoware_qp_interface/test/test_csc_matrix_conv.cpp create mode 100644 common/autoware_qp_interface/test/test_osqp_interface.cpp create 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 new file mode 100644 index 0000000000..f76dac861e --- /dev/null +++ b/common/autoware_qp_interface/CHANGELOG.rst @@ -0,0 +1,81 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 new file mode 100644 index 0000000000..1df75d2d71 --- /dev/null +++ b/common/autoware_qp_interface/CMakeLists.txt @@ -0,0 +1,64 @@ +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 new file mode 100644 index 0000000000..edc7fa9845 --- /dev/null +++ b/common/autoware_qp_interface/design/qp_interface-design.md @@ -0,0 +1,48 @@ +# 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 new file mode 100644 index 0000000000..9575d9d18c --- /dev/null +++ b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp @@ -0,0 +1,46 @@ +// 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 new file mode 100644 index 0000000000..a5777dd545 --- /dev/null +++ b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp @@ -0,0 +1,147 @@ +// 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 new file mode 100644 index 0000000000..324da4b18c --- /dev/null +++ b/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp @@ -0,0 +1,57 @@ +// 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 new file mode 100644 index 0000000000..be3c598512 --- /dev/null +++ b/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp @@ -0,0 +1,61 @@ +// 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 new file mode 100644 index 0000000000..b61d03a36d --- /dev/null +++ b/common/autoware_qp_interface/package.xml @@ -0,0 +1,30 @@ + + + + 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 new file mode 100644 index 0000000000..15314a9e4a --- /dev/null +++ b/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp @@ -0,0 +1,134 @@ +// 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 new file mode 100644 index 0000000000..fbb8e71f4c --- /dev/null +++ b/common/autoware_qp_interface/src/osqp_interface.cpp @@ -0,0 +1,394 @@ +// 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 new file mode 100644 index 0000000000..a0ebbf0db0 --- /dev/null +++ b/common/autoware_qp_interface/src/proxqp_interface.cpp @@ -0,0 +1,157 @@ +// 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 new file mode 100644 index 0000000000..f01e57772d --- /dev/null +++ b/common/autoware_qp_interface/src/qp_interface.cpp @@ -0,0 +1,70 @@ +// 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 new file mode 100644 index 0000000000..1f377a1a24 --- /dev/null +++ b/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp @@ -0,0 +1,181 @@ +// 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 new file mode 100644 index 0000000000..f97cc2888a --- /dev/null +++ b/common/autoware_qp_interface/test/test_osqp_interface.cpp @@ -0,0 +1,170 @@ +// 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 new file mode 100644 index 0000000000..e47af10c7a --- /dev/null +++ b/common/autoware_qp_interface/test/test_proxqp_interface.cpp @@ -0,0 +1,85 @@ +// 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 From b2eae2303a6cc4e66711d456b8de4c4f3fcf264d Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Thu, 9 Jan 2025 09:09:07 +0800 Subject: [PATCH 2/3] Delete CHANGELOG.rst since it's outdated --- common/autoware_qp_interface/CHANGELOG.rst | 81 ---------------------- 1 file changed, 81 deletions(-) delete mode 100644 common/autoware_qp_interface/CHANGELOG.rst diff --git a/common/autoware_qp_interface/CHANGELOG.rst b/common/autoware_qp_interface/CHANGELOG.rst deleted file mode 100644 index f76dac861e..0000000000 --- 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 From 4e3881eb8c7b4856675b2defe4189cb3e300151b Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 14 Jan 2025 10:06:28 +0900 Subject: [PATCH 3/3] Update common/autoware_qp_interface/package.xml --- common/autoware_qp_interface/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_qp_interface/package.xml b/common/autoware_qp_interface/package.xml index b61d03a36d..c1bf5b06d5 100644 --- a/common/autoware_qp_interface/package.xml +++ b/common/autoware_qp_interface/package.xml @@ -2,7 +2,7 @@ autoware_qp_interface - 0.40.0 + 0.0.0 Interface for the QP solvers Takayuki Murooka Fumiya Watanabe