feat(autoware_qp_interface): porting autoware_qp_interface package fr…

…om autoware.universe

Signed-off-by: NorahXiong <[email protected]>

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 <https://github.com/autowarefoundation/autoware.universe/issues/9587>`_)"
+  This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5.
+* fix: fix ticket links in CHANGELOG.rst (`#9588 <https://github.com/autowarefoundation/autoware.universe/issues/9588>`_)
+* chore(package.xml): bump version to 0.39.0 (`#9587 <https://github.com/autowarefoundation/autoware.universe/issues/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 <[email protected]>
+* fix: fix ticket links in CHANGELOG.rst (`#9588 <https://github.com/autowarefoundation/autoware.universe/issues/9588>`_)
+* fix(cpplint): include what you use - common (`#9564 <https://github.com/autowarefoundation/autoware.universe/issues/9564>`_)
+* fix: fix package names in changelog files (`#9500 <https://github.com/autowarefoundation/autoware.universe/issues/9500>`_)
+* fix(autoware_osqp_interface): fix clang-tidy errors (`#9440 <https://github.com/autowarefoundation/autoware.universe/issues/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 <https://github.com/autowarefoundation/autoware.universe/issues/9304>`_)
+* chore(package.xml): bump version to 0.38.0 (`#9266 <https://github.com/autowarefoundation/autoware.universe/issues/9266>`_) (`#9284 <https://github.com/autowarefoundation/autoware.universe/issues/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 <https://github.com/autowarefoundation/autoware.universe/issues/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 <https://github.com/autowarefoundation/autoware.universe/issues/9304>`_)
+* chore(package.xml): bump version to 0.38.0 (`#9266 <https://github.com/autowarefoundation/autoware.universe/issues/9266>`_) (`#9284 <https://github.com/autowarefoundation/autoware.universe/issues/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 <https://github.com/autowarefoundation/autoware.universe/issues/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 <https://github.com/autowarefoundation/autoware.universe/issues/8349>`_)
+  * fix:unreadVariable
+  * fix:unreadVariable
+  * fix:unreadVariable
+  ---------
+* perf(velocity_smoother): use ProxQP for faster optimization (`#8028 <https://github.com/autowarefoundation/autoware.universe/issues/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 <https://github.com/autowarefoundation/autoware.universe/issues/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 <https://github.com/autowarefoundation/autoware.universe/issues/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 <https://github.com/autowarefoundation/autoware.universe/issues/3699>`_)
+  * feat(qp_interface): add proxqp interface
+  * update
+  ---------
+* feat(qp_interface): add new package which will contain various qp solvers (`#3697 <https://github.com/autowarefoundation/autoware.universe/issues/3697>`_)
+* Contributors: Takayuki Murooka

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
+)

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: <https://osqp.org/>
+
+## Related issues

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 <Eigen/Core>
+
+#include <vector>
+
+namespace autoware::qp_interface
+{
+/// \brief Compressed-Column-Sparse Matrix
+struct CSC_Matrix
+{
+  /// Vector of non-zero values. Ex: [4,1,1,2]
+  std::vector<c_float> vals_;
+  /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner')
+  std::vector<c_int> row_idxs_;
+  /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer')
+  std::vector<c_int> 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_

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 <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+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<c_float>::epsilon(),
+    const c_float eps_rel = std::numeric_limits<c_float>::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<double> & q,
+    const std::vector<double> & l, const std::vector<double> & u,
+    const bool enable_warm_start = false,
+    const c_float eps_abs = std::numeric_limits<c_float>::epsilon());
+  OSQPInterface(
+    const CSC_Matrix & P, const CSC_Matrix & A, const std::vector<double> & q,
+    const std::vector<double> & l, const std::vector<double> & u,
+    const bool enable_warm_start = false,
+    const c_float eps_abs = std::numeric_limits<c_float>::epsilon());
+  ~OSQPInterface();
+
+  static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept;
+
+  std::vector<double> optimize(
+    CSC_Matrix P, CSC_Matrix A, const std::vector<double> & q, const std::vector<double> & l,
+    const std::vector<double> & u);
+
+  int getIterationNumber() const override;
+  bool isSolved() const override;
+  std::string getStatus() const override;
+
+  int getPolishStatus() const;
+  std::vector<double> 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<double> & q_new);
+  void updateL(const std::vector<double> & l_new);
+  void updateU(const std::vector<double> & u_new);
+  void updateBounds(const std::vector<double> & l_new, const std::vector<double> & 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<int64_t>(latest_work_info_.iter); }
+  /// \brief Get the status message for the latest problem solved
+  inline std::string getStatusMessage() const
+  {
+    return static_cast<std::string>(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<double> & primal_variables, const std::vector<double> & dual_variables);
+  bool setPrimalVariables(const std::vector<double> & primal_variables);
+  bool setDualVariables(const std::vector<double> & dual_variables);
+
+private:
+  std::unique_ptr<OSQPWorkspace, std::function<void(OSQPWorkspace *)>> work_;
+  std::unique_ptr<OSQPSettings> settings_;
+  std::unique_ptr<OSQPData> 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<double> & q,
+    const std::vector<double> & l, const std::vector<double> & u) override;
+
+  void initializeCSCProblemImpl(
+    CSC_Matrix P, CSC_Matrix A, const std::vector<double> & q, const std::vector<double> & l,
+    const std::vector<double> & u);
+
+  std::vector<double> optimizeImpl() override;
+};
+}  // namespace autoware::qp_interface
+
+#endif  // AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_