feat(autoware_qp_interface): porting autoware_qp_interface package from autoware.universe

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_

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 <proxsuite/helpers/optional.hpp>
+#include <proxsuite/proxqp/sparse/sparse.hpp>
+
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+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<double> settings_{};
+  std::shared_ptr<proxsuite::proxqp::sparse::QP<double, int>> qp_ptr_{nullptr};
+
+  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;
+
+  std::vector<double> optimizeImpl() override;
+};
+}  // namespace autoware::qp_interface
+
+#endif  // AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_