forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request autowarefoundation#498 from tier4/sync-upstream
chore: sync upstream
- Loading branch information
Showing
25 changed files
with
2,012 additions
and
44 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,59 @@ | ||
| cmake_minimum_required(VERSION 3.14) | ||
| project(qp_interface) | ||
|
|
||
| find_package(autoware_cmake REQUIRED) | ||
| autoware_package() | ||
|
|
||
| find_package(Eigen3 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 | ||
| ) | ||
|
|
||
| set(QP_INTERFACE_LIB_HEADERS | ||
| include/qp_interface/qp_interface.hpp | ||
| include/qp_interface/osqp_interface.hpp | ||
| include/qp_interface/osqp_csc_matrix_conv.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 -Wno-error=useless-cast) | ||
|
|
||
| target_include_directories(qp_interface | ||
| SYSTEM PUBLIC | ||
| "${OSQP_INCLUDE_DIR}" | ||
| "${EIGEN3_INCLUDE_DIR}" | ||
| ) | ||
|
|
||
| ament_target_dependencies(qp_interface | ||
| Eigen3 | ||
| osqp_vendor | ||
| ) | ||
|
|
||
| # 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 | ||
| ) | ||
| 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 | ||
| ) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,48 @@ | ||
| # Interface for QP solvers | ||
|
|
||
| This is the design document for the `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 |
46 changes: 46 additions & 0 deletions
46
common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ | ||
| #define QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ | ||
|
|
||
| #include "osqp/glob_opts.h" | ||
|
|
||
| #include <Eigen/Core> | ||
|
|
||
| #include <vector> | ||
|
|
||
| namespace qp | ||
| { | ||
| /// \brief Compressed-Column-Sparse Matrix | ||
| struct CSC_Matrix | ||
| { | ||
| /// Vector of non-zero values. Ex: [4,1,1,2] | ||
| std::vector<c_float> m_vals; | ||
| /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') | ||
| std::vector<c_int> m_row_idxs; | ||
| /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') | ||
| std::vector<c_int> m_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 qp | ||
|
|
||
| #endif // QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ |
148 changes: 148 additions & 0 deletions
148
common/qp_interface/include/qp_interface/osqp_interface.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,148 @@ | ||
| // 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 QP_INTERFACE__OSQP_INTERFACE_HPP_ | ||
| #define QP_INTERFACE__OSQP_INTERFACE_HPP_ | ||
|
|
||
| #include "osqp/osqp.h" | ||
| #include "qp_interface/osqp_csc_matrix_conv.hpp" | ||
| #include "qp_interface/qp_interface.hpp" | ||
|
|
||
| #include <Eigen/Core> | ||
|
|
||
| #include <limits> | ||
| #include <memory> | ||
| #include <string> | ||
| #include <tuple> | ||
| #include <vector> | ||
|
|
||
| namespace qp | ||
| { | ||
| constexpr c_float INF = 1e30; | ||
|
|
||
| class OSQPInterface : public QPInterface | ||
| { | ||
| public: | ||
| /// \brief Constructor without problem formulation | ||
| OSQPInterface( | ||
| const bool enable_warm_start = false, | ||
| const c_float eps_abs = std::numeric_limits<c_float>::epsilon(), const bool polish = true); | ||
| /// \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 getIteration() const override; | ||
| int 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>(m_latest_work_info.iter); } | ||
| /// \brief Get the status message for the latest problem solved | ||
| inline std::string getStatusMessage() const | ||
| { | ||
| return static_cast<std::string>(m_latest_work_info.status); | ||
| } | ||
| /// \brief Get the runtime of the latest problem solved | ||
| inline double getRunTime() const { return m_latest_work_info.run_time; } | ||
| /// \brief Get the objective value the latest problem solved | ||
| inline double getObjVal() const { return m_latest_work_info.obj_val; } | ||
| /// \brief Returns flag asserting interface condition (Healthy condition: 0). | ||
| inline int64_t getExitFlag() const { return m_exitflag; } | ||
|
|
||
| void logUnsolvedStatus(const std::string & prefix_message = "") const; | ||
|
|
||
| // 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 *)>> m_work; | ||
| std::unique_ptr<OSQPSettings> m_settings; | ||
| std::unique_ptr<OSQPData> m_data; | ||
| // store last work info since work is cleaned up at every execution to prevent memory leak. | ||
| OSQPInfo m_latest_work_info; | ||
| // Number of parameters to optimize | ||
| int64_t m_param_n; | ||
| // Flag to check if the current work exists | ||
| bool m_work_initialized = false; | ||
| // Exitflag | ||
| int64_t m_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 qp | ||
|
|
||
| #endif // QP_INTERFACE__OSQP_INTERFACE_HPP_ |
Oops, something went wrong.