Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(osqp_interface): add interface to give csc matrix directly to enable warm start for computation time improvement. #346

Closed
wants to merge 4 commits into from
Closed

feat(osqp_interface): add interface to give csc matrix directly to enable warm start for computation time improvement. #346

Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
87f5052
add osqp initialization with csc matrix
takayuki5168 Feb 7, 2022
f108d12
fix test
takayuki5168 Feb 7, 2022
8f85fc1
remove unnecessary includes
takayuki5168 Feb 7, 2022
a694806
add test for new functions
takayuki5168 Feb 7, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
add test for new functions 
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 committed Feb 7, 2022
commit a6948065d0b7ca983577e909c22a099eb2ef61b2
45 changes: 45 additions & 0 deletions common/osqp_interface/test/test_osqp_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,10 @@ using autoware::common::osqp::float64_t;
/// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py
// 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, int> & result) {
EXPECT_EQ(std::get<2>(result), 1);
Expand Down Expand Up @@ -87,5 +91,46 @@ TEST(TestOsqpInterface, BasicQp) {
result = osqp.optimize();
check_result(result);
}
{
// Define problem during initialization with csc matrix
Eigen::MatrixXd P(2, 2);
P << 4, 1, 1, 2;
CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
Eigen::MatrixXd A(4, 2);
A << 1, 1, 1, 0, 0, 1, 0, 1;
CSC_Matrix A_csc = calCSCMatrix(A);
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_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
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(2, 2);
CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(4, 2);
CSC_Matrix A_csc = calCSCMatrix(A);
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_csc, A_csc, q, l, u, 1e-6);
osqp.optimize();
// Redefine problem before optimization
Eigen::MatrixXd P_new(2, 2);
P_new << 4, 1, 1, 2;
CSC_Matrix P_new_csc = calCSCMatrixTrapezoidal(P_new);
Eigen::MatrixXd A_new(4, 2);
A_new << 1, 1, 1, 0, 0, 1, 0, 1;
CSC_Matrix A_new_csc = calCSCMatrix(A_new);
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_csc, A_new_csc, q_new, l_new, u_new);
result = osqp.optimize();
check_result(result);
}
}
} // namespace