Skip to content

Commit

Permalink
remove autoware_auto_common dependency from simple_planning_simulator…
Browse files Browse the repository at this point in the history 
…, osqp_interface

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 committed Nov 10, 2022
1 parent e60e868 commit a86aa66
Show file tree
Hide file tree
Showing 22 changed files with 354 additions and 374 deletions.
49 changes: 23 additions & 26 deletions common/osqp_interface/include/osqp_interface/osqp_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#ifndef OSQP_INTERFACE__OSQP_INTERFACE_HPP_
#define OSQP_INTERFACE__OSQP_INTERFACE_HPP_

#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "osqp/osqp.h"
#include "osqp_interface/csc_matrix_conv.hpp"
Expand All @@ -36,8 +35,6 @@ namespace common
namespace osqp
{
constexpr c_float INF = 1e30;
using autoware::common::types::bool8_t;
using autoware::common::types::float64_t;

/**
* Implementation of a native C++ interface for the OSQP solver.
Expand All @@ -54,19 +51,19 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
// Number of parameters to optimize
int64_t m_param_n;
// Flag to check if the current work exists
bool8_t m_work_initialized = false;
bool m_work_initialized = false;
// Exitflag
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, int64_t> solve();
std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> solve();

static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept;

public:
/// \brief Constructor without problem formulation
explicit OSQPInterface(
const c_float eps_abs = std::numeric_limits<c_float>::epsilon(), const bool8_t polish = true);
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.
Expand All @@ -75,11 +72,11 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
/// \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<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u, const c_float eps_abs);
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
const std::vector<double> & l, const std::vector<double> & 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);
const CSC_Matrix & P, const CSC_Matrix & A, const std::vector<double> & q,
const std::vector<double> & l, const std::vector<double> & u, const c_float eps_abs);
~OSQPInterface();

/****************
Expand All @@ -97,13 +94,13 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
/// \details 2. Initialize the interface and set up the problem.
/// \details osqp_interface = OSQPInterface(P, A, q, l, u, 1e-6);
/// \details 3. Call the optimization function.
/// \details std::tuple<std::vector<float64_t>, std::vector<float64_t>> result;
/// \details std::tuple<std::vector<double>, std::vector<double>> result;
/// \details result = osqp_interface.optimize();
/// \details 4. Access the optimized parameters.
/// \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, int64_t> optimize();
/// \details double x_0 = param[0];
/// \details double x_1 = param[1];
std::tuple<std::vector<double>, std::vector<double>, 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,15 +112,15 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
/// \details 2. Initialize the interface.
/// \details osqp_interface = OSQPInterface(1e-6);
/// \details 3. Call the optimization function with the problem formulation.
/// \details std::tuple<std::vector<float64_t>, std::vector<float64_t>> result;
/// \details std::tuple<std::vector<double>, std::vector<double>> result;
/// \details result = osqp_interface.optimize(P, A, q, l, u, 1e-6);
/// \details 4. Access the optimized parameters.
/// \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, 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);
/// \details double x_0 = param[0];
/// \details double x_1 = param[1];
std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> optimize(
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
const std::vector<double> & l, const std::vector<double> & u);

/// \brief Converts the input data and sets up the workspace object.
/// \param P (n,n) matrix defining relations between parameters.
Expand All @@ -132,11 +129,11 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
/// \param l (m) vector defining the lower bound problem constraint.
/// \param u (m) vector defining the upper bound problem constraint.
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);
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
const std::vector<double> & l, const std::vector<double> & 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);
CSC_Matrix P, CSC_Matrix A, const std::vector<double> & q, const std::vector<double> & l,
const std::vector<double> & u);

// Setter functions for warm start
bool setWarmStart(
Expand Down Expand Up @@ -187,9 +184,9 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
return static_cast<int64_t>(m_latest_work_info.status_polish);
}
/// \brief Get the runtime of the latest problem solved
inline float64_t getRunTime() const { return m_latest_work_info.run_time; }
inline double getRunTime() const { return m_latest_work_info.run_time; }
/// \brief Get the objective value the latest problem solved
inline float64_t getObjVal() const { return m_latest_work_info.obj_val; }
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; }

Expand Down
1 change: 0 additions & 1 deletion common/osqp_interface/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@

<build_depend>autoware_cmake</build_depend>

<depend>autoware_auto_common</depend>
<depend>eigen</depend>
<depend>osqp_vendor</depend>
<depend>rclcpp</depend>
Expand Down
56 changes: 27 additions & 29 deletions common/osqp_interface/src/osqp_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ namespace common
{
namespace osqp
{
OSQPInterface::OSQPInterface(const c_float eps_abs, const bool8_t polish)
OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish)
: m_work{nullptr, OSQPWorkspaceDeleter}
{
m_settings = std::make_unique<OSQPSettings>();
Expand All @@ -53,16 +53,16 @@ OSQPInterface::OSQPInterface(const c_float eps_abs, const bool8_t polish)
}

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)
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
const std::vector<double> & l, const std::vector<double> & u, const c_float eps_abs)
: OSQPInterface(eps_abs)
{
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)
const CSC_Matrix & P, const CSC_Matrix & A, const std::vector<double> & q,
const std::vector<double> & l, const std::vector<double> & u, const c_float eps_abs)
: OSQPInterface(eps_abs)
{
initializeProblem(P, A, q, l, u);
Expand Down Expand Up @@ -281,8 +281,8 @@ bool OSQPInterface::setDualVariables(const std::vector<double> & dual_variables)
}

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)
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
const std::vector<double> & l, const std::vector<double> & u)
{
// check if arguments are valid
std::stringstream ss;
Expand Down Expand Up @@ -318,16 +318,16 @@ int64_t OSQPInterface::initializeProblem(
}

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)
CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector<double> & q, const std::vector<double> & l,
const std::vector<double> & u)
{
// Dynamic float arrays
std::vector<float64_t> q_tmp(q.begin(), q.end());
std::vector<float64_t> l_tmp(l.begin(), l.end());
std::vector<float64_t> u_tmp(u.begin(), u.end());
float64_t * q_dyn = q_tmp.data();
float64_t * l_dyn = l_tmp.data();
float64_t * u_dyn = u_tmp.data();
std::vector<double> q_tmp(q.begin(), q.end());
std::vector<double> l_tmp(l.begin(), l.end());
std::vector<double> u_tmp(u.begin(), u.end());
double * q_dyn = q_tmp.data();
double * l_dyn = l_tmp.data();
double * u_dyn = u_tmp.data();

/**********************
* OBJECTIVE FUNCTION
Expand Down Expand Up @@ -360,7 +360,7 @@ int64_t OSQPInterface::initializeProblem(
return m_exitflag;
}

std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t>
std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
OSQPInterface::solve()
{
// Solve Problem
Expand All @@ -369,17 +369,17 @@ OSQPInterface::solve()
/********************
* EXTRACT SOLUTION
********************/
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_data->m);
double * sol_x = m_work->solution->x;
double * sol_y = m_work->solution->y;
std::vector<double> sol_primal(sol_x, sol_x + m_param_n);
std::vector<double> sol_lagrange_multiplier(sol_y, sol_y + m_data->m);

int64_t status_polish = m_work->info->status_polish;
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, int64_t> result =
std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result =
std::make_tuple(
sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration);

Expand All @@ -388,26 +388,24 @@ OSQPInterface::solve()
return result;
}

std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t>
std::tuple<std::vector<double>, std::vector<double>, 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, int64_t> result =
solve();
std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result = solve();
return result;
}

std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t>
std::tuple<std::vector<double>, std::vector<double>, 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)
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
const std::vector<double> & l, const std::vector<double> & u)
{
// Allocate memory for problem
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, int64_t> result =
solve();
std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result = solve();

m_work.reset();
m_work_initialized = false;
Expand Down
41 changes: 19 additions & 22 deletions common/osqp_interface/test/test_osqp_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@

namespace
{
using autoware::common::osqp::float64_t;
// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py
//
// min 1/2 * x' * P * x + q' * x
Expand All @@ -45,7 +44,7 @@ TEST(TestOsqpInterface, BasicQp)
using autoware::common::osqp::CSC_Matrix;

auto check_result =
[](const std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> & result) {
[](const std::tuple<std::vector<double>, std::vector<double>, int, int, int> & result) {
EXPECT_EQ(std::get<2>(result), 1); // polish succeeded
EXPECT_EQ(std::get<3>(result), 1); // solution succeeded

Expand All @@ -66,34 +65,33 @@ TEST(TestOsqpInterface, BasicQp)

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};
const std::vector<double> q = {1.0, 1.0};
const std::vector<double> l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF};
const std::vector<double> u = {1.0, 0.7, 0.7, autoware::common::osqp::INF};

{
// Define problem during optimization
autoware::common::osqp::OSQPInterface osqp;
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result =
std::tuple<std::vector<double>, std::vector<double>, int, int, int> result =
osqp.optimize(P, A, q, l, u);
check_result(result);
}

{
// Define problem during initialization
autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6);
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result =
osqp.optimize();
std::tuple<std::vector<double>, std::vector<double>, int, int, int> result = osqp.optimize();
check_result(result);
}

{
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result;
std::tuple<std::vector<double>, std::vector<double>, int, int, int> result;
// Dummy initial problem
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);
std::vector<double> q_ini(2, 0.0);
std::vector<double> l_ini(4, 0.0);
std::vector<double> u_ini(4, 0.0);
autoware::common::osqp::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6);
osqp.optimize();

Expand All @@ -108,19 +106,18 @@ TEST(TestOsqpInterface, BasicQp)
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();
std::tuple<std::vector<double>, std::vector<double>, int, int, int> result = osqp.optimize();
check_result(result);
}

{
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result;
std::tuple<std::vector<double>, std::vector<double>, 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);
std::vector<double> q_ini(2, 0.0);
std::vector<double> l_ini(4, 0.0);
std::vector<double> 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();

Expand All @@ -134,13 +131,13 @@ TEST(TestOsqpInterface, BasicQp)

// add warm startup
{
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result;
std::tuple<std::vector<double>, std::vector<double>, 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);
std::vector<double> q_ini(2, 0.0);
std::vector<double> l_ini(4, 0.0);
std::vector<double> 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();

Expand Down
Loading

0 comments on commit a86aa66

Please sign in to comment.