Skip to content

Commit

Permalink
feat(osqp_interface): add interface to give csc matrix directly to en…
Browse files Browse the repository at this point in the history 
…able warm start for computation time improvement. (#378)

* add osqp initialization with csc matrix

Signed-off-by: Takayuki Murooka <[email protected]>

* fix test

Signed-off-by: Takayuki Murooka <[email protected]>

* remove unnecessary includes

Signed-off-by: Takayuki Murooka <[email protected]>

* add test for new functions

Signed-off-by: Takayuki Murooka <[email protected]>

* fix osqp dual variables output

Signed-off-by: Takamasa Horibe <[email protected]>

* fix osqp test

Signed-off-by: Takamasa Horibe <[email protected]>

* cosmetic change

Signed-off-by: Takamasa Horibe <[email protected]>

Co-authored-by: Takamasa Horibe <[email protected]>
  • Loading branch information
@takayuki5168 @TakaHoribe
takayuki5168 and TakaHoribe authored Feb 15, 2022
1 parent f45815b commit 942f81c
Show file tree
Hide file tree
Showing 3 changed files with 161 additions and 62 deletions.
14 changes: 11 additions & 3 deletions common/osqp_interface/include/osqp_interface/osqp_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
int64_t m_exitflag;

// Runs the solver on the stored problem.
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> solve();
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> solve();

static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept;

Expand All @@ -75,6 +75,9 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
OSQPInterface(
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u, const c_float eps_abs);
OSQPInterface(
const CSC_Matrix & P, const CSC_Matrix & A, const std::vector<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u, const c_float eps_abs);

/****************
* OPTIMIZATION
Expand All @@ -97,7 +100,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
/// \details std::vector<float> param = std::get<0>(result);
/// \details float64_t x_0 = param[0];
/// \details float64_t x_1 = param[1];
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> optimize();
std::tuple<std::vector<float64_t>, std::vector<float64_t>, 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.
Expand All @@ -115,7 +118,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
/// \details std::vector<float> param = std::get<0>(result);
/// \details float64_t x_0 = param[0];
/// \details float64_t x_1 = param[1];
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> optimize(
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> optimize(
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u);

Expand All @@ -128,6 +131,9 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
int64_t initializeProblem(
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u);
int64_t initializeProblem(
CSC_Matrix P, CSC_Matrix A, const std::vector<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u);

// Updates problem parameters while keeping solution in memory.
//
Expand All @@ -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<double> & q_new);
void updateL(const std::vector<double> & l_new);
void updateU(const std::vector<double> & u_new);
Expand Down
85 changes: 67 additions & 18 deletions common/osqp_interface/src/osqp_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u,
const c_float eps_abs)
: OSQPInterface(eps_abs)
{
initializeProblem(P, A, q, l, u);
}

void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept
{
if (ptr != nullptr) {
Expand All @@ -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<c_int>(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<c_int>(P_csc.m_vals.size()));
}

void OSQPInterface::updateA(const Eigen::MatrixXd & A_new)
{
/*
Expand All @@ -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<c_int>(A_csc.m_vals.size()));
}

void OSQPInterface::updateQ(const std::vector<double> & q_new)
{
std::vector<double> q_tmp(q_new.begin(), q_new.end());
Expand Down Expand Up @@ -184,11 +203,43 @@ int64_t OSQPInterface::initializeProblem(
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & 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<int>(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<int>(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<int>(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<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u)
{
// Dynamic float arrays
std::vector<float64_t> q_tmp(q.begin(), q.end());
std::vector<float64_t> l_tmp(l.begin(), l.end());
Expand All @@ -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<int>(q.size());
m_data->m = static_cast<int>(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<c_int>(P_csc.m_vals.size()), P_csc.m_vals.data(),
Expand All @@ -229,7 +277,7 @@ int64_t OSQPInterface::initializeProblem(
return m_exitflag;
}

std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> OSQPInterface::solve()
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> OSQPInterface::solve()
{
// Solve Problem
osqp_solve(m_work.get());
Expand All @@ -240,29 +288,30 @@ std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> OSQ
float64_t * sol_x = m_work->solution->x;
float64_t * sol_y = m_work->solution->y;
std::vector<float64_t> sol_primal(sol_x, sol_x + m_param_n);
std::vector<float64_t> sol_lagrange_multiplier(sol_y, sol_y + m_param_n);
// Solver polish status
std::vector<float64_t> 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<float64_t>, std::vector<float64_t>, int64_t, int64_t> result =
std::make_tuple(sol_primal, sol_lagrange_multiplier, status_polish, status_solution);
std::tuple<std::vector<float64_t>, std::vector<float64_t>, 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<float64_t>, std::vector<float64_t>, int64_t, int64_t>
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t>
OSQPInterface::optimize()
{
// Run the solver on the stored problem representation.
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> result = solve();
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> result = solve();
return result;
}

std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t>
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t>
OSQPInterface::optimize(
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u)
Expand All @@ -271,7 +320,7 @@ OSQPInterface::optimize(
initializeProblem(P, A, q, l, u);

// Run the solver on the stored problem representation.
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> result = solve();
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> result = solve();

m_work.reset();
m_work_initialized = false;
Expand Down
124 changes: 83 additions & 41 deletions common/osqp_interface/test/test_osqp_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<float64_t>, std::vector<float64_t>, 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<float64_t>, std::vector<float64_t>, 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<float64_t> q = {1.0, 1.0};
const std::vector<float64_t> l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF};
const std::vector<float64_t> 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<float64_t> q = {1.0, 1.0};
std::vector<float64_t> l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF};
std::vector<float64_t> u = {1.0, 0.7, 0.7, autoware::common::osqp::INF};
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int> result = osqp.optimize(
std::tuple<std::vector<float64_t>, std::vector<float64_t>, 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<float64_t> q = {1.0, 1.0};
std::vector<float64_t> l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF};
std::vector<float64_t> 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<float64_t>, std::vector<float64_t>, int, int> result = osqp.optimize();
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result = osqp.optimize();
check_result(result);
}

{
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int> result;
std::tuple<std::vector<float64_t>, std::vector<float64_t>, 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<float64_t> q(2, 0.0);
std::vector<float64_t> l(4, 0.0);
std::vector<float64_t> 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<float64_t> q_ini(2, 0.0);
std::vector<float64_t> l_ini(4, 0.0);
std::vector<float64_t> 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<float64_t>, std::vector<float64_t>, int, int, int> result = osqp.optimize();
check_result(result);
}

{
std::tuple<std::vector<float64_t>, std::vector<float64_t>, 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<float64_t> q_ini(2, 0.0);
std::vector<float64_t> l_ini(4, 0.0);
std::vector<float64_t> 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<float64_t> q_new = {1.0, 1.0};
std::vector<float64_t> l_new = {1.0, 0.0, 0.0, -autoware::common::osqp::INF};
std::vector<float64_t> 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);
}
Expand Down

0 comments on commit 942f81c

Please sign in to comment.