diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp index 46bbc79f2f8cd..950f699b912dd 100644 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp +++ b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp @@ -57,7 +57,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface int64_t m_exitflag; // Runs the solver on the stored problem. - std::tuple, std::vector, int64_t, int64_t> solve(); + std::tuple, std::vector, int64_t, int64_t, int64_t> solve(); static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; @@ -75,6 +75,9 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface OSQPInterface( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u, const c_float eps_abs); + OSQPInterface( + const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, + const std::vector & l, const std::vector & u, const c_float eps_abs); /**************** * OPTIMIZATION @@ -97,7 +100,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface /// \details std::vector param = std::get<0>(result); /// \details float64_t x_0 = param[0]; /// \details float64_t x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t> optimize(); + std::tuple, std::vector, int64_t, int64_t, int64_t> optimize(); /// \brief Solves convex quadratic programs (QPs) using the OSQP solver. /// \return The function returns a tuple containing the solution as two float vectors. @@ -115,7 +118,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface /// \details std::vector param = std::get<0>(result); /// \details float64_t x_0 = param[0]; /// \details float64_t x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t> optimize( + std::tuple, std::vector, int64_t, int64_t, int64_t> optimize( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u); @@ -128,6 +131,9 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface int64_t initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u); + int64_t initializeProblem( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, + const std::vector & l, const std::vector & u); // Updates problem parameters while keeping solution in memory. // @@ -138,7 +144,9 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface // 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); diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp index 5b60dc55b4f96..3052844d6b6ea 100644 --- a/common/osqp_interface/src/osqp_interface.cpp +++ b/common/osqp_interface/src/osqp_interface.cpp @@ -60,6 +60,15 @@ OSQPInterface::OSQPInterface( 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 c_float eps_abs) +: OSQPInterface(eps_abs) +{ + initializeProblem(P, A, q, l, u); +} + void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept { if (ptr != nullptr) { @@ -82,6 +91,11 @@ void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) osqp_update_P(m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); } +void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) +{ + osqp_update_P(m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); +} + void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) { /* @@ -96,6 +110,11 @@ void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) return; } +void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) +{ + osqp_update_A(m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); +} + void OSQPInterface::updateQ(const std::vector & q_new) { std::vector q_tmp(q_new.begin(), q_new.end()); @@ -184,11 +203,43 @@ int64_t OSQPInterface::initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u) { - /******************* - * SET UP MATRICES - *******************/ + // 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()); + } + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); + return initializeProblem(P_csc, A_csc, q, l, u); +} + +int64_t OSQPInterface::initializeProblem( + 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()); @@ -200,15 +251,12 @@ int64_t OSQPInterface::initializeProblem( /********************** * OBJECTIVE FUNCTION **********************/ - // Number of constraints - c_int constr_m = A.rows(); - // Number of parameters - m_param_n = P.rows(); + m_param_n = static_cast(q.size()); + m_data->m = static_cast(l.size()); /***************** * POPULATE DATA *****************/ - m_data->m = constr_m; m_data->n = m_param_n; m_data->P = csc_matrix( m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), @@ -229,7 +277,7 @@ int64_t OSQPInterface::initializeProblem( return m_exitflag; } -std::tuple, std::vector, int64_t, int64_t> OSQPInterface::solve() +std::tuple, std::vector, int64_t, int64_t, int64_t> OSQPInterface::solve() { // Solve Problem osqp_solve(m_work.get()); @@ -240,29 +288,30 @@ std::tuple, std::vector, int64_t, int64_t> OSQ float64_t * sol_x = m_work->solution->x; float64_t * sol_y = m_work->solution->y; std::vector sol_primal(sol_x, sol_x + m_param_n); - std::vector sol_lagrange_multiplier(sol_y, sol_y + m_param_n); - // Solver polish status + std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); + int64_t status_polish = m_work->info->status_polish; - // Solver solution status int64_t status_solution = m_work->info->status_val; + int64_t status_iteration = m_work->info->iter; + // Result tuple - std::tuple, std::vector, int64_t, int64_t> result = - std::make_tuple(sol_primal, sol_lagrange_multiplier, status_polish, status_solution); + std::tuple, std::vector, int64_t, int64_t, int64_t> result = + std::make_tuple(sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration); m_latest_work_info = *(m_work->info); return result; } -std::tuple, std::vector, int64_t, int64_t> +std::tuple, std::vector, int64_t, int64_t, int64_t> OSQPInterface::optimize() { // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t> result = solve(); + std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); return result; } -std::tuple, std::vector, int64_t, int64_t> +std::tuple, std::vector, int64_t, int64_t, int64_t> OSQPInterface::optimize( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u) @@ -271,7 +320,7 @@ OSQPInterface::optimize( initializeProblem(P, A, q, l, u); // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t> result = solve(); + std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); m_work.reset(); m_work_initialized = false; diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/osqp_interface/test/test_osqp_interface.cpp index 5683023c7c3e3..e3cd601c9c6e4 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/osqp_interface/test/test_osqp_interface.cpp @@ -23,67 +23,109 @@ namespace { using autoware::common::osqp::float64_t; -/// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py +// 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 + // cppcheck-suppress syntaxError TEST(TestOsqpInterface, BasicQp) { + using autoware::common::osqp::CSC_Matrix; + using autoware::common::osqp::calCSCMatrix; + using autoware::common::osqp::calCSCMatrixTrapezoidal; + auto check_result = - [](const std::tuple, std::vector, int, int> & result) { - EXPECT_EQ(std::get<2>(result), 1); - EXPECT_EQ(std::get<3>(result), 1); - ASSERT_EQ(std::get<0>(result).size(), size_t(2)); - ASSERT_EQ(std::get<1>(result).size(), size_t(2)); - EXPECT_DOUBLE_EQ(std::get<0>(result)[0], 0.3); - EXPECT_DOUBLE_EQ(std::get<0>(result)[1], 0.7); - EXPECT_DOUBLE_EQ(std::get<1>(result)[0], -2.9); - EXPECT_NEAR(std::get<1>(result)[1], 0.2, 1e-6); + [](const std::tuple, std::vector, int, int, int> & result) { + EXPECT_EQ(std::get<2>(result), 1); // polish succeeded + EXPECT_EQ(std::get<3>(result), 1); // solution succeeded + + static const auto ep = 1.0e-8; + + const auto prime_val = std::get<0>(result); + ASSERT_EQ(prime_val.size(), size_t(2)); + EXPECT_NEAR(prime_val[0], 0.3, ep); + EXPECT_NEAR(prime_val[1], 0.7, ep); + + const auto dual_val = std::get<1>(result); + ASSERT_EQ(dual_val.size(), size_t(4)); + EXPECT_NEAR(dual_val[0], -2.9, ep); + EXPECT_NEAR(dual_val[1], 0.0, ep); + EXPECT_NEAR(dual_val[2], 0.2, ep); + EXPECT_NEAR(dual_val[3], 0.0, 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::common::osqp::INF}; + const std::vector u = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; + { // Define problem during optimization autoware::common::osqp::OSQPInterface osqp; - Eigen::MatrixXd P(2, 2); - P << 4, 1, 1, 2; - Eigen::MatrixXd A(2, 4); - A << 1, 1, 1, 0, 0, 1, 0, 1; - std::vector q = {1.0, 1.0}; - std::vector l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF}; - std::vector u = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; - std::tuple, std::vector, int, int> result = osqp.optimize( + std::tuple, std::vector, int, int, int> result = osqp.optimize( P, A, q, l, u); check_result(result); } + { // Define problem during initialization - Eigen::MatrixXd P(2, 2); - P << 4, 1, 1, 2; - Eigen::MatrixXd A(2, 4); - A << 1, 1, 1, 0, 0, 1, 0, 1; - std::vector q = {1.0, 1.0}; - std::vector l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF}; - std::vector u = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6); - std::tuple, std::vector, int, int> result = osqp.optimize(); + std::tuple, std::vector, int, int, int> result = osqp.optimize(); check_result(result); } + { - std::tuple, std::vector, int, int> result; + std::tuple, std::vector, int, int, int> result; // Dummy initial problem - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2, 4); - std::vector q(2, 0.0); - std::vector l(4, 0.0); - std::vector u(4, 0.0); - autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6); + 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::common::osqp::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); + + // Redefine problem before optimization + osqp.initializeProblem(P, A, q, l, u); + result = osqp.optimize(); + check_result(result); + } + + { + // Define problem during initialization with csc matrix + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); + std::tuple, std::vector, int, int, int> result = osqp.optimize(); + check_result(result); + } + + { + 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::common::osqp::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); + osqp.optimize(); + // Redefine problem before optimization - Eigen::MatrixXd P_new(2, 2); - P_new << 4, 1, 1, 2; - Eigen::MatrixXd A_new(2, 4); - A_new << 1, 1, 1, 0, 0, 1, 0, 1; - std::vector q_new = {1.0, 1.0}; - std::vector l_new = {1.0, 0.0, 0.0, -autoware::common::osqp::INF}; - std::vector u_new = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; - osqp.initializeProblem(P_new, A_new, q_new, l_new, u_new); + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + osqp.initializeProblem(P_csc, A_csc, q, l, u); result = osqp.optimize(); check_result(result); }