From fd6d09802b8e328460889af3cd9ad1c392724d23 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 23 Jul 2024 19:16:33 +0900 Subject: [PATCH 1/4] fix(velocity_smoother, obstacle_cruise_planner ): float type of processing time was wrong (#8161) fix(velocity_smoother): float type of processing time was wrong Signed-off-by: Takayuki Murooka --- .../include/autoware/obstacle_cruise_planner/node.hpp | 2 +- .../include/autoware/obstacle_cruise_planner/type_alias.hpp | 2 ++ planning/autoware_obstacle_cruise_planner/src/node.cpp | 4 ++-- .../include/autoware/velocity_smoother/node.hpp | 4 +++- planning/autoware_velocity_smoother/src/node.cpp | 4 ++-- 5 files changed, 10 insertions(+), 6 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index dd9a6c3880672..b5728fd3bd3d9 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -141,7 +141,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_stop_planning_info_pub_; rclcpp::Publisher::SharedPtr debug_cruise_planning_info_pub_; rclcpp::Publisher::SharedPtr debug_slow_down_planning_info_pub_; - rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; // subscriber rclcpp::Subscription::SharedPtr traj_sub_; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index 5721efd051f63..9eeb97d74ef04 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -28,6 +28,7 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" #include "tier4_planning_msgs/msg/stop_factor.hpp" #include "tier4_planning_msgs/msg/stop_reason_array.hpp" #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" @@ -51,6 +52,7 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32Stamped; +using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 9682ac63104c8..df46fd086c9c3 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -374,7 +374,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); // debug publisher - debug_calculation_time_pub_ = create_publisher("~/debug/processing_time_ms", 1); + debug_calculation_time_pub_ = create_publisher("~/debug/processing_time_ms", 1); debug_cruise_wall_marker_pub_ = create_publisher("~/debug/cruise/virtual_wall", 1); debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall", 1); debug_slow_down_wall_marker_pub_ = @@ -1645,7 +1645,7 @@ void ObstacleCruisePlannerNode::publishDebugInfo() const void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const { - Float32Stamped calculation_time_msg; + Float64Stamped calculation_time_msg; calculation_time_msg.stamp = now(); calculation_time_msg.data = calculation_time; debug_calculation_time_pub_->publish(calculation_time_msg); diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index bcc9338422c9f..655632f14d207 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -45,6 +45,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "std_srvs/srv/set_bool.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" // temporary +#include "tier4_debug_msgs/msg/float64_stamped.hpp" // temporary #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary #include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary #include "visualization_msgs/msg/marker_array.hpp" @@ -68,6 +69,7 @@ using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; using std_srvs::srv::SetBool; using tier4_debug_msgs::msg::Float32Stamped; // temporary +using tier4_debug_msgs::msg::Float64Stamped; // temporary using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary using tier4_planning_msgs::msg::VelocityLimit; // temporary using visualization_msgs::msg::MarkerArray; @@ -291,7 +293,7 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_closest_velocity_; rclcpp::Publisher::SharedPtr debug_closest_acc_; rclcpp::Publisher::SharedPtr debug_closest_jerk_; - rclcpp::Publisher::SharedPtr debug_calculation_time_; + rclcpp::Publisher::SharedPtr debug_calculation_time_; rclcpp::Publisher::SharedPtr debug_closest_max_velocity_; rclcpp::Publisher::SharedPtr debug_processing_time_detail_; diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index ca0fdc9ca7117..23dee0019bf94 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -84,7 +84,7 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti debug_closest_acc_ = create_publisher("~/closest_acceleration", 1); debug_closest_jerk_ = create_publisher("~/closest_jerk", 1); debug_closest_max_velocity_ = create_publisher("~/closest_max_velocity", 1); - debug_calculation_time_ = create_publisher("~/debug/processing_time_ms", 1); + debug_calculation_time_ = create_publisher("~/debug/processing_time_ms", 1); pub_trajectory_raw_ = create_publisher("~/debug/trajectory_raw", 1); pub_trajectory_vel_lim_ = create_publisher("~/debug/trajectory_external_velocity_limited", 1); @@ -1138,7 +1138,7 @@ void VelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const void VelocitySmootherNode::publishStopWatchTime() { - Float32Stamped calculation_time_data{}; + Float64Stamped calculation_time_data{}; calculation_time_data.stamp = this->now(); calculation_time_data.data = stop_watch_.toc(); debug_calculation_time_->publish(calculation_time_data); From a196826671b48149297d0633d62736d865d5da21 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 13 Jul 2024 14:56:46 +0900 Subject: [PATCH 2/4] refactor(qp_interface): clean up the code (#8029) * refactor qp_interface Signed-off-by: Takayuki Murooka * variable names: m_XXX -> XXX_ Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../qp_interface/osqp_csc_matrix_conv.hpp | 10 +- .../include/qp_interface/osqp_interface.hpp | 42 ++-- .../include/qp_interface/proxqp_interface.hpp | 18 +- .../include/qp_interface/qp_interface.hpp | 18 +- .../qp_interface/src/osqp_csc_matrix_conv.cpp | 10 +- common/qp_interface/src/osqp_interface.cpp | 236 +++++++++--------- common/qp_interface/src/proxqp_interface.cpp | 84 +++++-- common/qp_interface/src/qp_interface.cpp | 8 +- .../test/test_csc_matrix_conv.cpp | 210 ++++++++-------- .../qp_interface/test/test_osqp_interface.cpp | 64 ++--- .../test/test_proxqp_interface.cpp | 21 +- 11 files changed, 377 insertions(+), 344 deletions(-) diff --git a/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp index 74ec4c1282f46..8ec7280cfc100 100644 --- a/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp +++ b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp @@ -21,17 +21,17 @@ #include -namespace qp +namespace autoware::common { /// \brief Compressed-Column-Sparse Matrix struct CSC_Matrix { /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector m_vals; + std::vector vals_; /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector m_row_idxs; + std::vector row_idxs_; /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector m_col_idxs; + std::vector col_idxs_; }; /// \brief Calculate CSC matrix from Eigen matrix @@ -41,6 +41,6 @@ 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 +} // namespace autoware::common #endif // QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/qp_interface/include/qp_interface/osqp_interface.hpp b/common/qp_interface/include/qp_interface/osqp_interface.hpp index ef2c3bd17c89e..14c03a91d8fa1 100644 --- a/common/qp_interface/include/qp_interface/osqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/osqp_interface.hpp @@ -24,9 +24,9 @@ #include #include -namespace qp +namespace autoware::common { -constexpr c_float INF = 1e30; +constexpr c_float OSQP_INF = 1e30; class OSQPInterface : public QPInterface { @@ -34,7 +34,9 @@ class OSQPInterface : public QPInterface /// \brief Constructor without problem formulation OSQPInterface( const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); + const c_float eps_abs = std::numeric_limits::epsilon(), + const c_float eps_rel = std::numeric_limits::epsilon(), const bool polish = true, + const bool verbose = false); /// \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. @@ -60,8 +62,10 @@ class OSQPInterface : public QPInterface CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, const std::vector & u); - int getIteration() const override; - int getStatus() const override; + int getIterationNumber() const override; + bool isSolved() const override; + std::string getStatus() const override; + int getPolishStatus() const; std::vector getDualSolution() const; @@ -96,20 +100,18 @@ class OSQPInterface : public QPInterface 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(m_latest_work_info.iter); } + inline int64_t getTakenIter() const { return static_cast(latest_work_info_.iter); } /// \brief Get the status message for the latest problem solved inline std::string getStatusMessage() const { - return static_cast(m_latest_work_info.status); + return static_cast(latest_work_info_.status); } /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return m_latest_work_info.run_time; } + inline double getRunTime() const { return 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; } + inline double getObjVal() const { return 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; + inline int64_t getExitFlag() const { return exitflag_; } // Setter functions for warm start bool setWarmStart( @@ -118,17 +120,17 @@ class OSQPInterface : public QPInterface bool setDualVariables(const std::vector & dual_variables); private: - std::unique_ptr> m_work; - std::unique_ptr m_settings; - std::unique_ptr m_data; + std::unique_ptr> work_; + std::unique_ptr settings_; + std::unique_ptr data_; // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo m_latest_work_info; + OSQPInfo latest_work_info_; // Number of parameters to optimize - int64_t m_param_n; + int64_t param_n_; // Flag to check if the current work exists - bool m_work_initialized = false; + bool work__initialized = false; // Exitflag - int64_t m_exitflag; + int64_t exitflag_; void initializeProblemImpl( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -140,6 +142,6 @@ class OSQPInterface : public QPInterface std::vector optimizeImpl() override; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/proxqp_interface.hpp b/common/qp_interface/include/qp_interface/proxqp_interface.hpp index a940262d5f6da..f75fb3da5954c 100644 --- a/common/qp_interface/include/qp_interface/proxqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/proxqp_interface.hpp @@ -22,27 +22,29 @@ #include #include +#include #include -namespace qp +namespace autoware::common { class ProxQPInterface : public QPInterface { public: explicit ProxQPInterface( - const bool enable_warm_start = false, - const double eps_abs = std::numeric_limits::epsilon()); + const bool enable_warm_start, const double eps_abs, const double eps_rel, + const bool verbose = false); - int getIteration() const override; - int getStatus() const override; + int getIterationNumber() const override; + bool isSolved() const override; + std::string getStatus() const override; void updateEpsAbs(const double eps_abs) override; void updateEpsRel(const double eps_rel) override; void updateVerbose(const bool verbose) override; private: - proxsuite::proxqp::Settings m_settings; - std::shared_ptr> m_qp_ptr; + proxsuite::proxqp::Settings settings_{}; + std::shared_ptr> qp_ptr_{nullptr}; void initializeProblemImpl( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -50,6 +52,6 @@ class ProxQPInterface : public QPInterface std::vector optimizeImpl() override; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/qp_interface.hpp b/common/qp_interface/include/qp_interface/qp_interface.hpp index 026c0fe413b71..85e2d103cde7a 100644 --- a/common/qp_interface/include/qp_interface/qp_interface.hpp +++ b/common/qp_interface/include/qp_interface/qp_interface.hpp @@ -18,28 +18,30 @@ #include #include +#include #include -namespace qp +namespace autoware::common { class QPInterface { public: - explicit QPInterface(const bool enable_warm_start) : m_enable_warm_start(enable_warm_start) {} + explicit QPInterface(const bool enable_warm_start) : enable_warm_start_(enable_warm_start) {} std::vector optimize( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u); - virtual int getIteration() const = 0; - virtual int getStatus() const = 0; + virtual bool isSolved() const = 0; + virtual int getIterationNumber() const = 0; + virtual std::string getStatus() const = 0; virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; virtual void updateVerbose([[maybe_unused]] const bool verbose) {} protected: - bool m_enable_warm_start; + bool enable_warm_start_{false}; void initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -51,9 +53,9 @@ class QPInterface virtual std::vector optimizeImpl() = 0; - std::optional m_variables_num; - std::optional m_constraints_num; + std::optional variables_num_{std::nullopt}; + std::optional constraints_num_{std::nullopt}; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/qp_interface/src/osqp_csc_matrix_conv.cpp b/common/qp_interface/src/osqp_csc_matrix_conv.cpp index 0faf7586463fd..77a481f442000 100644 --- a/common/qp_interface/src/osqp_csc_matrix_conv.cpp +++ b/common/qp_interface/src/osqp_csc_matrix_conv.cpp @@ -21,7 +21,7 @@ #include #include -namespace qp +namespace autoware::common { CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) { @@ -114,21 +114,21 @@ CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) void printCSCMatrix(const CSC_Matrix & csc_mat) { std::cout << "["; - for (const c_float & val : csc_mat.m_vals) { + for (const c_float & val : csc_mat.vals_) { std::cout << val << ", "; } std::cout << "]\n"; std::cout << "["; - for (const c_int & row : csc_mat.m_row_idxs) { + for (const c_int & row : csc_mat.row_idxs_) { std::cout << row << ", "; } std::cout << "]\n"; std::cout << "["; - for (const c_int & col : csc_mat.m_col_idxs) { + for (const c_int & col : csc_mat.col_idxs_) { std::cout << col << ", "; } std::cout << "]\n"; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/osqp_interface.cpp b/common/qp_interface/src/osqp_interface.cpp index 81d2ffeeaaed5..0e8cb7e13988a 100644 --- a/common/qp_interface/src/osqp_interface.cpp +++ b/common/qp_interface/src/osqp_interface.cpp @@ -18,27 +18,29 @@ #include -namespace qp -{ -OSQPInterface::OSQPInterface(const bool enable_warm_start, const c_float eps_abs, const bool polish) -: QPInterface(enable_warm_start), m_work{nullptr, OSQPWorkspaceDeleter} -{ - m_settings = std::make_unique(); - m_data = std::make_unique(); - - if (m_settings) { - osqp_set_default_settings(m_settings.get()); - m_settings->alpha = 1.6; // Change alpha parameter - m_settings->eps_rel = 1.0E-4; - m_settings->eps_abs = eps_abs; - m_settings->eps_prim_inf = 1.0E-4; - m_settings->eps_dual_inf = 1.0E-4; - m_settings->warm_start = true; - m_settings->max_iter = 4000; - m_settings->verbose = false; - m_settings->polish = polish; +namespace autoware::common +{ +OSQPInterface::OSQPInterface( + const bool enable_warm_start, const c_float eps_abs, const c_float eps_rel, const bool polish, + const bool verbose) +: QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter} +{ + settings_ = std::make_unique(); + data_ = std::make_unique(); + + if (settings_) { + osqp_set_default_settings(settings_.get()); + settings_->alpha = 1.6; // Change alpha parameter + settings_->eps_rel = eps_rel; + settings_->eps_abs = eps_abs; + settings_->eps_prim_inf = 1.0E-4; + settings_->eps_dual_inf = 1.0E-4; + settings_->warm_start = true; + settings_->max_iter = 4000; + settings_->verbose = verbose; + settings_->polish = polish; } - m_exitflag = 0; + exitflag_ = 0; } OSQPInterface::OSQPInterface( @@ -61,8 +63,8 @@ OSQPInterface::OSQPInterface( OSQPInterface::~OSQPInterface() { - if (m_data->P) free(m_data->P); - if (m_data->A) free(m_data->A); + if (data_->P) free(data_->P); + if (data_->A) free(data_->A); } void OSQPInterface::initializeProblemImpl( @@ -89,30 +91,30 @@ void OSQPInterface::initializeCSCProblemImpl( /********************** * OBJECTIVE FUNCTION **********************/ - m_param_n = static_cast(q.size()); - m_data->m = static_cast(l.size()); + param_n_ = static_cast(q.size()); + data_->m = static_cast(l.size()); /***************** * POPULATE DATA *****************/ - m_data->n = m_param_n; - if (m_data->P) free(m_data->P); - m_data->P = csc_matrix( - m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), - P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); - m_data->q = q_dyn; - if (m_data->A) free(m_data->A); - m_data->A = csc_matrix( - m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), - A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); - m_data->l = l_dyn; - m_data->u = u_dyn; + data_->n = param_n_; + if (data_->P) free(data_->P); + data_->P = csc_matrix( + data_->n, data_->n, static_cast(P_csc.vals_.size()), P_csc.vals_.data(), + P_csc.row_idxs_.data(), P_csc.col_idxs_.data()); + data_->q = q_dyn; + if (data_->A) free(data_->A); + data_->A = csc_matrix( + data_->m, data_->n, static_cast(A_csc.vals_.size()), A_csc.vals_.data(), + A_csc.row_idxs_.data(), A_csc.col_idxs_.data()); + data_->l = l_dyn; + data_->u = u_dyn; // Setup workspace OSQPWorkspace * workspace; - m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); - m_work.reset(workspace); - m_work_initialized = true; + exitflag_ = osqp_setup(&workspace, data_.get(), settings_.get()); + work_.reset(workspace); + work__initialized = true; } void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept @@ -124,67 +126,67 @@ void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept void OSQPInterface::updateEpsAbs(const double eps_abs) { - m_settings->eps_abs = eps_abs; // for default setting - if (m_work_initialized) { - osqp_update_eps_abs(m_work.get(), eps_abs); // for current work + settings_->eps_abs = eps_abs; // for default setting + if (work__initialized) { + osqp_update_eps_abs(work_.get(), eps_abs); // for current work } } void OSQPInterface::updateEpsRel(const double eps_rel) { - m_settings->eps_rel = eps_rel; // for default setting - if (m_work_initialized) { - osqp_update_eps_rel(m_work.get(), eps_rel); // for current work + settings_->eps_rel = eps_rel; // for default setting + if (work__initialized) { + osqp_update_eps_rel(work_.get(), eps_rel); // for current work } } void OSQPInterface::updateMaxIter(const int max_iter) { - m_settings->max_iter = max_iter; // for default setting - if (m_work_initialized) { - osqp_update_max_iter(m_work.get(), max_iter); // for current work + settings_->max_iter = max_iter; // for default setting + if (work__initialized) { + osqp_update_max_iter(work_.get(), max_iter); // for current work } } void OSQPInterface::updateVerbose(const bool is_verbose) { - m_settings->verbose = is_verbose; // for default setting - if (m_work_initialized) { - osqp_update_verbose(m_work.get(), is_verbose); // for current work + settings_->verbose = is_verbose; // for default setting + if (work__initialized) { + osqp_update_verbose(work_.get(), is_verbose); // for current work } } void OSQPInterface::updateRhoInterval(const int rho_interval) { - m_settings->adaptive_rho_interval = rho_interval; // for default setting + settings_->adaptive_rho_interval = rho_interval; // for default setting } void OSQPInterface::updateRho(const double rho) { - m_settings->rho = rho; - if (m_work_initialized) { - osqp_update_rho(m_work.get(), rho); + settings_->rho = rho; + if (work__initialized) { + osqp_update_rho(work_.get(), rho); } } void OSQPInterface::updateAlpha(const double alpha) { - m_settings->alpha = alpha; - if (m_work_initialized) { - osqp_update_alpha(m_work.get(), alpha); + settings_->alpha = alpha; + if (work__initialized) { + osqp_update_alpha(work_.get(), alpha); } } void OSQPInterface::updateScaling(const int scaling) { - m_settings->scaling = scaling; + settings_->scaling = scaling; } void OSQPInterface::updatePolish(const bool polish) { - m_settings->polish = polish; - if (m_work_initialized) { - osqp_update_polish(m_work.get(), polish); + settings_->polish = polish; + if (work__initialized) { + osqp_update_polish(work_.get(), polish); } } @@ -195,9 +197,9 @@ void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter return; } - m_settings->polish_refine_iter = polish_refine_iter; - if (m_work_initialized) { - osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); + settings_->polish_refine_iter = polish_refine_iter; + if (work__initialized) { + osqp_update_polish_refine_iter(work_.get(), polish_refine_iter); } } @@ -208,9 +210,9 @@ void OSQPInterface::updateCheckTermination(const int check_termination) return; } - m_settings->check_termination = check_termination; - if (m_work_initialized) { - osqp_update_check_termination(m_work.get(), check_termination); + settings_->check_termination = check_termination; + if (work__initialized) { + osqp_update_check_termination(work_.get(), check_termination); } } @@ -222,11 +224,11 @@ bool OSQPInterface::setWarmStart( bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) { - if (!m_work_initialized) { + if (!work__initialized) { return false; } - const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); + const auto result = osqp_warm_start_x(work_.get(), primal_variables.data()); if (result != 0) { std::cerr << "Failed to set primal variables for warm start" << std::endl; return false; @@ -237,11 +239,11 @@ bool OSQPInterface::setPrimalVariables(const std::vector & primal_variab bool OSQPInterface::setDualVariables(const std::vector & dual_variables) { - if (!m_work_initialized) { + if (!work__initialized) { return false; } - const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); + const auto result = osqp_warm_start_y(work_.get(), dual_variables.data()); if (result != 0) { std::cerr << "Failed to set dual variables for warm start" << std::endl; return false; @@ -250,27 +252,6 @@ bool OSQPInterface::setDualVariables(const std::vector & dual_variables) return true; } -void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const -{ - const int status = getStatus(); - if (status == 1) { - // No need to log since optimization was solved. - return; - } - - // create message - std::string output_message = ""; - if (prefix_message != "") { - output_message = prefix_message + " "; - } - - const auto status_message = getStatusMessage(); - output_message += "Optimization failed due to " + status_message; - - // log with warning - RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); -} - void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) { /* @@ -283,14 +264,12 @@ void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) c_int P_elem_N = P_sparse.nonZeros(); */ CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); + osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.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())); + osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); } void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) @@ -303,36 +282,34 @@ void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) c_int A_elem_N = A_sparse.nonZeros(); */ CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); 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())); + osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); } void OSQPInterface::updateQ(const std::vector & q_new) { std::vector q_tmp(q_new.begin(), q_new.end()); double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(m_work.get(), q_dyn); + osqp_update_lin_cost(work_.get(), q_dyn); } void OSQPInterface::updateL(const std::vector & l_new) { std::vector l_tmp(l_new.begin(), l_new.end()); double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(m_work.get(), l_dyn); + osqp_update_lower_bound(work_.get(), l_dyn); } void OSQPInterface::updateU(const std::vector & u_new) { std::vector u_tmp(u_new.begin(), u_new.end()); double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(m_work.get(), u_dyn); + osqp_update_upper_bound(work_.get(), u_dyn); } void OSQPInterface::updateBounds( @@ -342,45 +319,50 @@ void OSQPInterface::updateBounds( std::vector u_tmp(u_new.begin(), u_new.end()); double * l_dyn = l_tmp.data(); double * u_dyn = u_tmp.data(); - osqp_update_bounds(m_work.get(), l_dyn, u_dyn); + osqp_update_bounds(work_.get(), l_dyn, u_dyn); +} + +int OSQPInterface::getIterationNumber() const +{ + return work_->info->iter; } -int OSQPInterface::getIteration() const +std::string OSQPInterface::getStatus() const { - return m_work->info->iter; + return "OSQP_SOLVED"; } -int OSQPInterface::getStatus() const +bool OSQPInterface::isSolved() const { - return static_cast(m_latest_work_info.status_val); + return latest_work_info_.status_val == 1; } int OSQPInterface::getPolishStatus() const { - return static_cast(m_latest_work_info.status_polish); + return static_cast(latest_work_info_.status_polish); } std::vector OSQPInterface::getDualSolution() const { - double * sol_y = m_work->solution->y; - std::vector dual_solution(sol_y, sol_y + m_data->m); + double * sol_y = work_->solution->y; + std::vector dual_solution(sol_y, sol_y + data_->m); return dual_solution; } std::vector OSQPInterface::optimizeImpl() { - osqp_solve(m_work.get()); + osqp_solve(work_.get()); - double * sol_x = m_work->solution->x; - double * 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_data->m); + double * sol_x = work_->solution->x; + double * sol_y = work_->solution->y; + std::vector sol_primal(sol_x, sol_x + param_n_); + std::vector sol_lagrange_multiplier(sol_y, sol_y + data_->m); - m_latest_work_info = *(m_work->info); + latest_work_info_ = *(work_->info); - if (!m_enable_warm_start) { - m_work.reset(); - m_work_initialized = false; + if (!enable_warm_start_) { + work_.reset(); + work__initialized = false; } return sol_primal; @@ -393,7 +375,17 @@ std::vector OSQPInterface::optimize( initializeCSCProblemImpl(P, A, q, l, u); const auto result = optimizeImpl(); + // show polish status if not successful + const int status_polish = static_cast(latest_work_info_.status_polish); + if (status_polish != 1) { + const auto msg = status_polish == 0 ? "unperformed" + : status_polish == -1 ? "unsuccessful" + : "unknown"; + std::cerr << "osqp polish process failed : " << msg << ". The result may be inaccurate" + << std::endl; + } + return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/proxqp_interface.cpp b/common/qp_interface/src/proxqp_interface.cpp index bf1f0229e1e21..507f3858cbf1b 100644 --- a/common/qp_interface/src/proxqp_interface.cpp +++ b/common/qp_interface/src/proxqp_interface.cpp @@ -14,12 +14,17 @@ #include "qp_interface/proxqp_interface.hpp" -namespace qp +namespace autoware::common { -ProxQPInterface::ProxQPInterface(const bool enable_warm_start, const double eps_abs) +using proxsuite::proxqp::QPSolverOutput; + +ProxQPInterface::ProxQPInterface( + const bool enable_warm_start, const double eps_abs, const double eps_rel, const bool verbose) : QPInterface(enable_warm_start) { - m_settings.eps_abs = eps_abs; + settings_.eps_abs = eps_abs; + settings_.eps_rel = eps_rel; + settings_.verbose = verbose; } void ProxQPInterface::initializeProblemImpl( @@ -31,28 +36,28 @@ void ProxQPInterface::initializeProblemImpl( const bool enable_warm_start = [&]() { if ( - !m_enable_warm_start // Warm start is designated - || !m_qp_ptr // QP pointer is initialized + !enable_warm_start_ // Warm start is designated + || !qp_ptr_ // QP pointer is initialized // The number of variables is the same as the previous one. - || !m_variables_num || - *m_variables_num != variables_num + || !variables_num_ || + *variables_num_ != variables_num // The number of constraints is the same as the previous one - || !m_constraints_num || *m_constraints_num != constraints_num) { + || !constraints_num_ || *constraints_num_ != constraints_num) { return false; } return true; }(); if (!enable_warm_start) { - m_qp_ptr = std::make_shared>( + qp_ptr_ = std::make_shared>( variables_num, 0, constraints_num); } - m_settings.initial_guess = + settings_.initial_guess = enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; - m_qp_ptr->settings = m_settings; + qp_ptr_->settings = settings_; Eigen::SparseMatrix P_sparse(variables_num, constraints_num); P_sparse = P.sparseView(); @@ -69,53 +74,78 @@ void ProxQPInterface::initializeProblemImpl( Eigen::Map(u_std_vec.data(), u_std_vec.size()); if (enable_warm_start) { - m_qp_ptr->update( + qp_ptr_->update( P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); } else { - m_qp_ptr->init( + qp_ptr_->init( P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); } } void ProxQPInterface::updateEpsAbs(const double eps_abs) { - m_settings.eps_abs = eps_abs; + settings_.eps_abs = eps_abs; } void ProxQPInterface::updateEpsRel(const double eps_rel) { - m_settings.eps_rel = eps_rel; + settings_.eps_rel = eps_rel; } void ProxQPInterface::updateVerbose(const bool is_verbose) { - m_settings.verbose = is_verbose; + settings_.verbose = is_verbose; } -int ProxQPInterface::getIteration() const +bool ProxQPInterface::isSolved() const { - if (m_qp_ptr) { - return m_qp_ptr->results.info.iter; + if (qp_ptr_) { + return qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED; } - return 0; + return false; } -int ProxQPInterface::getStatus() const +int ProxQPInterface::getIterationNumber() const { - if (m_qp_ptr) { - return static_cast(m_qp_ptr->results.info.status); + if (qp_ptr_) { + return qp_ptr_->results.info.iter; } return 0; } +std::string ProxQPInterface::getStatus() const +{ + if (qp_ptr_) { + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED) { + return "PROXQP_SOLVED"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_MAX_ITER_REACHED) { + return "PROXQP_MAX_ITER_REACHED"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { + return "PROXQP_PRIMAL_INFEASIBLE"; + } + // if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE) { + // return "PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE"; + // } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_DUAL_INFEASIBLE) { + return "PROXQP_DUAL_INFEASIBLE"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_NOT_RUN) { + return "PROXQP_NOT_RUN"; + } + } + return "None"; +} + std::vector ProxQPInterface::optimizeImpl() { - m_qp_ptr->solve(); + qp_ptr_->solve(); std::vector result; - for (Eigen::Index i = 0; i < m_qp_ptr->results.x.size(); ++i) { - result.push_back(m_qp_ptr->results.x[i]); + for (Eigen::Index i = 0; i < qp_ptr_->results.x.size(); ++i) { + result.push_back(qp_ptr_->results.x[i]); } return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/qp_interface.cpp b/common/qp_interface/src/qp_interface.cpp index e7a69bac0c67c..afd26132d7769 100644 --- a/common/qp_interface/src/qp_interface.cpp +++ b/common/qp_interface/src/qp_interface.cpp @@ -18,7 +18,7 @@ #include #include -namespace qp +namespace autoware::common { void QPInterface::initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -54,8 +54,8 @@ void QPInterface::initializeProblem( initializeProblemImpl(P, A, q, l, u); - m_variables_num = q.size(); - m_constraints_num = l.size(); + variables_num_ = q.size(); + constraints_num_ = l.size(); } std::vector QPInterface::optimize( @@ -67,4 +67,4 @@ std::vector QPInterface::optimize( return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/test/test_csc_matrix_conv.cpp b/common/qp_interface/test/test_csc_matrix_conv.cpp index fc604d762d2c4..a6bd5e3df0be1 100644 --- a/common/qp_interface/test/test_csc_matrix_conv.cpp +++ b/common/qp_interface/test/test_csc_matrix_conv.cpp @@ -23,42 +23,42 @@ TEST(TestCscMatrixConv, Nominal) { - using qp::calCSCMatrix; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrix; + using autoware::common::CSC_Matrix; Eigen::MatrixXd rect1(1, 2); rect1 << 0.0, 1.0; const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_vals[0], 1.0); - ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); - ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); + ASSERT_EQ(rect_m1.vals_.size(), size_t(1)); + EXPECT_EQ(rect_m1.vals_[0], 1.0); + ASSERT_EQ(rect_m1.row_idxs_.size(), size_t(1)); + EXPECT_EQ(rect_m1.row_idxs_[0], c_int(0)); + ASSERT_EQ(rect_m1.col_idxs_.size(), size_t(3)); // nb of columns + 1 + EXPECT_EQ(rect_m1.col_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m1.col_idxs_[1], c_int(0)); + EXPECT_EQ(rect_m1.col_idxs_[2], c_int(1)); Eigen::MatrixXd rect2(2, 4); rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_vals[0], 1.0); - EXPECT_EQ(rect_m2.m_vals[1], 6.0); - EXPECT_EQ(rect_m2.m_vals[2], 3.0); - EXPECT_EQ(rect_m2.m_vals[3], 7.0); - ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); - ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); - EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); + ASSERT_EQ(rect_m2.vals_.size(), size_t(4)); + EXPECT_EQ(rect_m2.vals_[0], 1.0); + EXPECT_EQ(rect_m2.vals_[1], 6.0); + EXPECT_EQ(rect_m2.vals_[2], 3.0); + EXPECT_EQ(rect_m2.vals_[3], 7.0); + ASSERT_EQ(rect_m2.row_idxs_.size(), size_t(4)); + EXPECT_EQ(rect_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(rect_m2.row_idxs_[2], c_int(0)); + EXPECT_EQ(rect_m2.row_idxs_[3], c_int(1)); + ASSERT_EQ(rect_m2.col_idxs_.size(), size_t(5)); // nb of columns + 1 + EXPECT_EQ(rect_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m2.col_idxs_[1], c_int(1)); + EXPECT_EQ(rect_m2.col_idxs_[2], c_int(2)); + EXPECT_EQ(rect_m2.col_idxs_[3], c_int(4)); + EXPECT_EQ(rect_m2.col_idxs_[4], c_int(4)); // Example from http://netlib.org/linalg/html_templates/node92.html Eigen::MatrixXd square2(6, 6); @@ -66,59 +66,59 @@ TEST(TestCscMatrixConv, Nominal) 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); - EXPECT_EQ(square_m2.m_vals[0], 10.0); - EXPECT_EQ(square_m2.m_vals[1], 3.0); - EXPECT_EQ(square_m2.m_vals[2], 3.0); - EXPECT_EQ(square_m2.m_vals[3], 9.0); - EXPECT_EQ(square_m2.m_vals[4], 7.0); - EXPECT_EQ(square_m2.m_vals[5], 8.0); - EXPECT_EQ(square_m2.m_vals[6], 4.0); - EXPECT_EQ(square_m2.m_vals[7], 8.0); - EXPECT_EQ(square_m2.m_vals[8], 8.0); - EXPECT_EQ(square_m2.m_vals[9], 7.0); - EXPECT_EQ(square_m2.m_vals[10], 7.0); - EXPECT_EQ(square_m2.m_vals[11], 9.0); - EXPECT_EQ(square_m2.m_vals[12], -2.0); - EXPECT_EQ(square_m2.m_vals[13], 5.0); - EXPECT_EQ(square_m2.m_vals[14], 9.0); - EXPECT_EQ(square_m2.m_vals[15], 2.0); - EXPECT_EQ(square_m2.m_vals[16], 3.0); - EXPECT_EQ(square_m2.m_vals[17], 13.0); - EXPECT_EQ(square_m2.m_vals[18], -1.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); - EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); - EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); - EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); + ASSERT_EQ(square_m2.vals_.size(), size_t(19)); + EXPECT_EQ(square_m2.vals_[0], 10.0); + EXPECT_EQ(square_m2.vals_[1], 3.0); + EXPECT_EQ(square_m2.vals_[2], 3.0); + EXPECT_EQ(square_m2.vals_[3], 9.0); + EXPECT_EQ(square_m2.vals_[4], 7.0); + EXPECT_EQ(square_m2.vals_[5], 8.0); + EXPECT_EQ(square_m2.vals_[6], 4.0); + EXPECT_EQ(square_m2.vals_[7], 8.0); + EXPECT_EQ(square_m2.vals_[8], 8.0); + EXPECT_EQ(square_m2.vals_[9], 7.0); + EXPECT_EQ(square_m2.vals_[10], 7.0); + EXPECT_EQ(square_m2.vals_[11], 9.0); + EXPECT_EQ(square_m2.vals_[12], -2.0); + EXPECT_EQ(square_m2.vals_[13], 5.0); + EXPECT_EQ(square_m2.vals_[14], 9.0); + EXPECT_EQ(square_m2.vals_[15], 2.0); + EXPECT_EQ(square_m2.vals_[16], 3.0); + EXPECT_EQ(square_m2.vals_[17], 13.0); + EXPECT_EQ(square_m2.vals_[18], -1.0); + ASSERT_EQ(square_m2.row_idxs_.size(), size_t(19)); + EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[2], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[3], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[4], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[5], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[6], c_int(5)); + EXPECT_EQ(square_m2.row_idxs_[7], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[8], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[9], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[10], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[11], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[12], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[13], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[14], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[15], c_int(5)); + EXPECT_EQ(square_m2.row_idxs_[16], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[17], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[18], c_int(5)); + ASSERT_EQ(square_m2.col_idxs_.size(), size_t(7)); // nb of columns + 1 + EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[1], c_int(3)); + EXPECT_EQ(square_m2.col_idxs_[2], c_int(7)); + EXPECT_EQ(square_m2.col_idxs_[3], c_int(9)); + EXPECT_EQ(square_m2.col_idxs_[4], c_int(12)); + EXPECT_EQ(square_m2.col_idxs_[5], c_int(16)); + EXPECT_EQ(square_m2.col_idxs_[6], c_int(19)); } TEST(TestCscMatrixConv, Trapezoidal) { - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd square2(3, 3); @@ -129,33 +129,33 @@ TEST(TestCscMatrixConv, Trapezoidal) const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m1.m_vals[0], 1.0); - EXPECT_EQ(square_m1.m_vals[1], 2.0); - EXPECT_EQ(square_m1.m_vals[2], 4.0); - ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); - EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); + ASSERT_EQ(square_m1.vals_.size(), size_t(3)); + EXPECT_EQ(square_m1.vals_[0], 1.0); + EXPECT_EQ(square_m1.vals_[1], 2.0); + EXPECT_EQ(square_m1.vals_[2], 4.0); + ASSERT_EQ(square_m1.row_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m1.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m1.row_idxs_[1], c_int(0)); + EXPECT_EQ(square_m1.row_idxs_[2], c_int(1)); + ASSERT_EQ(square_m1.col_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m1.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m1.col_idxs_[1], c_int(1)); + EXPECT_EQ(square_m1.col_idxs_[2], c_int(3)); const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m2.m_vals[0], 2.0); - EXPECT_EQ(square_m2.m_vals[1], 5.0); - EXPECT_EQ(square_m2.m_vals[2], 6.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); + ASSERT_EQ(square_m2.vals_.size(), size_t(3)); + EXPECT_EQ(square_m2.vals_[0], 2.0); + EXPECT_EQ(square_m2.vals_[1], 5.0); + EXPECT_EQ(square_m2.vals_[2], 6.0); + ASSERT_EQ(square_m2.row_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[2], c_int(1)); + ASSERT_EQ(square_m2.col_idxs_.size(), size_t(4)); + EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[1], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[2], c_int(2)); + EXPECT_EQ(square_m2.col_idxs_[3], c_int(3)); try { const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); @@ -166,10 +166,10 @@ TEST(TestCscMatrixConv, Trapezoidal) } TEST(TestCscMatrixConv, Print) { - using qp::calCSCMatrix; - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; - using qp::printCSCMatrix; + using autoware::common::calCSCMatrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; + using autoware::common::printCSCMatrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd rect1(1, 2); square1 << 1.0, 2.0, 2.0, 4.0; diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp index e5d7041469289..eddc8a452fe35 100644 --- a/common/qp_interface/test/test_osqp_interface.cpp +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -40,44 +40,44 @@ namespace // cppcheck-suppress syntaxError TEST(TestOsqpInterface, BasicQp) { - using qp::calCSCMatrix; - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; - auto check_result = []( - const auto & solution, const int solution_status, const int polish_status) { - EXPECT_EQ(solution_status, 1); - EXPECT_EQ(polish_status, 1); + auto check_result = + [](const auto & solution, const std::string & status, const int polish_status) { + EXPECT_EQ(status, "OSQP_SOLVED"); + EXPECT_EQ(polish_status, 1); - static const auto ep = 1.0e-8; + static const auto ep = 1.0e-8; - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; + ASSERT_EQ(solution.size(), size_t(2)); + EXPECT_NEAR(solution[0], 0.3, ep); + EXPECT_NEAR(solution[1], 0.7, 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, -qp::INF}; - const std::vector u = {1.0, 0.7, 0.7, qp::INF}; + 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 - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { // Define problem during initialization - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { @@ -88,7 +88,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); } @@ -96,12 +96,12 @@ TEST(TestOsqpInterface, BasicQp) // Define problem during initialization with csc matrix CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { @@ -112,7 +112,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization @@ -120,9 +120,9 @@ TEST(TestOsqpInterface, BasicQp) CSC_Matrix A_csc = calCSCMatrix(A); const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } // add warm startup @@ -133,7 +133,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(true, 1e-6); // enable warm start + autoware::common::OSQPInterface osqp(true, 1e-6); // enable warm start osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization @@ -141,9 +141,9 @@ TEST(TestOsqpInterface, BasicQp) CSC_Matrix A_csc = calCSCMatrix(A); { const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); osqp.updateCheckTermination(1); const auto primal_val = solution; @@ -156,9 +156,9 @@ TEST(TestOsqpInterface, BasicQp) { const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } // NOTE: This should be true, but currently optimize function reset the workspace, which diff --git a/common/qp_interface/test/test_proxqp_interface.cpp b/common/qp_interface/test/test_proxqp_interface.cpp index 96466665d5172..34d23f15e4a0b 100644 --- a/common/qp_interface/test/test_proxqp_interface.cpp +++ b/common/qp_interface/test/test_proxqp_interface.cpp @@ -41,7 +41,9 @@ namespace // cppcheck-suppress syntaxError TEST(TestProxqpInterface, BasicQp) { - auto check_result = [](const auto & solution) { + auto check_result = [](const auto & solution, const std::string & status) { + EXPECT_EQ(status, "PROXQP_SOLVED"); + static const auto ep = 1.0e-8; ASSERT_EQ(solution.size(), size_t(2)); EXPECT_NEAR(solution[0], 0.3, ep); @@ -56,23 +58,26 @@ TEST(TestProxqpInterface, BasicQp) { // Define problem during optimization - qp::ProxQPInterface proxqp(false, 1e-9); + autoware::common::ProxQPInterface proxqp(false, 1e-9, 1e-9, false); const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); + const auto status = proxqp.getStatus(); + check_result(solution, status); } { // Define problem during optimization with warm start - qp::ProxQPInterface proxqp(true, 1e-9); + autoware::common::ProxQPInterface proxqp(true, 1e-9, 1e-9, false); { const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); - EXPECT_NE(proxqp.getIteration(), 1); + const auto status = proxqp.getStatus(); + check_result(solution, status); + EXPECT_NE(proxqp.getIterationNumber(), 1); } { const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); - EXPECT_EQ(proxqp.getIteration(), 0); + const auto status = proxqp.getStatus(); + check_result(solution, status); + EXPECT_EQ(proxqp.getIterationNumber(), 0); } } } From 8311f5ad7c45b617b30750b35472d0b9b142345f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 30 Jul 2024 15:59:48 +0900 Subject: [PATCH 3/4] perf(velocity_smoother): use ProxQP for faster optimization (#8028) * perf(velocity_smoother): use ProxQP for faster optimization Signed-off-by: Takayuki Murooka * consider max_iteration Signed-off-by: Takayuki Murooka * disable warm start Signed-off-by: Takayuki Murooka * fix test Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../include/qp_interface/osqp_interface.hpp | 2 +- .../include/qp_interface/proxqp_interface.hpp | 4 +-- common/qp_interface/src/osqp_interface.cpp | 8 +++--- common/qp_interface/src/proxqp_interface.cpp | 4 ++- .../qp_interface/test/test_osqp_interface.cpp | 12 ++++----- .../test/test_proxqp_interface.cpp | 4 +-- .../smoother/jerk_filtered_smoother.hpp | 4 +-- .../autoware_velocity_smoother/package.xml | 1 + .../src/smoother/jerk_filtered_smoother.cpp | 27 +++++-------------- 9 files changed, 28 insertions(+), 38 deletions(-) diff --git a/common/qp_interface/include/qp_interface/osqp_interface.hpp b/common/qp_interface/include/qp_interface/osqp_interface.hpp index 14c03a91d8fa1..55792beaaff11 100644 --- a/common/qp_interface/include/qp_interface/osqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/osqp_interface.hpp @@ -33,7 +33,7 @@ class OSQPInterface : public QPInterface public: /// \brief Constructor without problem formulation OSQPInterface( - const bool enable_warm_start = false, + const bool enable_warm_start = false, const int max_iteration = 20000, const c_float eps_abs = std::numeric_limits::epsilon(), const c_float eps_rel = std::numeric_limits::epsilon(), const bool polish = true, const bool verbose = false); diff --git a/common/qp_interface/include/qp_interface/proxqp_interface.hpp b/common/qp_interface/include/qp_interface/proxqp_interface.hpp index f75fb3da5954c..775ba0fcc42b8 100644 --- a/common/qp_interface/include/qp_interface/proxqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/proxqp_interface.hpp @@ -31,8 +31,8 @@ class ProxQPInterface : public QPInterface { public: explicit ProxQPInterface( - const bool enable_warm_start, const double eps_abs, const double eps_rel, - const bool verbose = false); + const bool enable_warm_start, const int max_iteration, const double eps_abs, + const double eps_rel, const bool verbose = false); int getIterationNumber() const override; bool isSolved() const override; diff --git a/common/qp_interface/src/osqp_interface.cpp b/common/qp_interface/src/osqp_interface.cpp index 0e8cb7e13988a..3ada25e8fa023 100644 --- a/common/qp_interface/src/osqp_interface.cpp +++ b/common/qp_interface/src/osqp_interface.cpp @@ -21,8 +21,8 @@ namespace autoware::common { OSQPInterface::OSQPInterface( - const bool enable_warm_start, const c_float eps_abs, const c_float eps_rel, const bool polish, - const bool verbose) + const bool enable_warm_start, const int max_iteration, const c_float eps_abs, + const c_float eps_rel, const bool polish, const bool verbose) : QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter} { settings_ = std::make_unique(); @@ -35,8 +35,8 @@ OSQPInterface::OSQPInterface( settings_->eps_abs = eps_abs; settings_->eps_prim_inf = 1.0E-4; settings_->eps_dual_inf = 1.0E-4; - settings_->warm_start = true; - settings_->max_iter = 4000; + settings_->warm_start = enable_warm_start; + settings_->max_iter = max_iteration; settings_->verbose = verbose; settings_->polish = polish; } diff --git a/common/qp_interface/src/proxqp_interface.cpp b/common/qp_interface/src/proxqp_interface.cpp index 507f3858cbf1b..d26543f907e62 100644 --- a/common/qp_interface/src/proxqp_interface.cpp +++ b/common/qp_interface/src/proxqp_interface.cpp @@ -19,9 +19,11 @@ namespace autoware::common using proxsuite::proxqp::QPSolverOutput; ProxQPInterface::ProxQPInterface( - const bool enable_warm_start, const double eps_abs, const double eps_rel, const bool verbose) + const bool enable_warm_start, const int max_iteration, const double eps_abs, const double eps_rel, + const bool verbose) : QPInterface(enable_warm_start) { + settings_.max_iter = max_iteration; settings_.eps_abs = eps_abs; settings_.eps_rel = eps_rel; settings_.verbose = verbose; diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp index eddc8a452fe35..980a85b384747 100644 --- a/common/qp_interface/test/test_osqp_interface.cpp +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -64,7 +64,7 @@ TEST(TestOsqpInterface, BasicQp) { // Define problem during optimization - autoware::common::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 4000, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); @@ -73,7 +73,7 @@ TEST(TestOsqpInterface, BasicQp) { // Define problem during initialization - autoware::common::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 4000, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); @@ -88,7 +88,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 4000, 1e-6); osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); } @@ -96,7 +96,7 @@ TEST(TestOsqpInterface, BasicQp) // Define problem during initialization with csc matrix CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); - autoware::common::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 4000, 1e-6); const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); const auto status = osqp.getStatus(); @@ -112,7 +112,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 4000, 1e-6); osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization @@ -133,7 +133,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::OSQPInterface osqp(true, 1e-6); // enable warm start + autoware::common::OSQPInterface osqp(true, 4000, 1e-6); // enable warm start osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization diff --git a/common/qp_interface/test/test_proxqp_interface.cpp b/common/qp_interface/test/test_proxqp_interface.cpp index 34d23f15e4a0b..a19c673b52f4d 100644 --- a/common/qp_interface/test/test_proxqp_interface.cpp +++ b/common/qp_interface/test/test_proxqp_interface.cpp @@ -58,7 +58,7 @@ TEST(TestProxqpInterface, BasicQp) { // Define problem during optimization - autoware::common::ProxQPInterface proxqp(false, 1e-9, 1e-9, false); + autoware::common::ProxQPInterface proxqp(false, 4000, 1e-9, 1e-9, false); const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); const auto status = proxqp.getStatus(); check_result(solution, status); @@ -66,7 +66,7 @@ TEST(TestProxqpInterface, BasicQp) { // Define problem during optimization with warm start - autoware::common::ProxQPInterface proxqp(true, 1e-9, 1e-9, false); + autoware::common::ProxQPInterface proxqp(true, 4000, 1e-9, 1e-9, false); { const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); const auto status = proxqp.getStatus(); diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp index 06a6f2da7f30a..cc8b4f95ccbdc 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -19,7 +19,7 @@ #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "osqp_interface/osqp_interface.hpp" +#include "qp_interface/qp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -59,7 +59,7 @@ class JerkFilteredSmoother : public SmootherBase private: Param smoother_param_; - autoware::common::osqp::OSQPInterface qp_solver_; + std::shared_ptr qp_interface_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("jerk_filtered_smoother")}; TrajectoryPoints forwardJerkFilter( diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 37420b534938c..5ddbe3aa20ead 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -29,6 +29,7 @@ interpolation nav_msgs osqp_interface + qp_interface rclcpp std_srvs tf2 diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index d458c688d060c..ecb67338fb1c4 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -15,6 +15,7 @@ #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "qp_interface/proxqp_interface.hpp" #include @@ -40,11 +41,8 @@ JerkFilteredSmoother::JerkFilteredSmoother( p.over_j_weight = node.declare_parameter("over_j_weight"); p.jerk_filter_ds = node.declare_parameter("jerk_filter_ds"); - qp_solver_.updateMaxIter(20000); - qp_solver_.updateRhoInterval(0); // 0 means automatic - qp_solver_.updateEpsRel(1.0e-6); // def: 1.0e-4 - qp_solver_.updateEpsAbs(1.0e-8); // def: 1.0e-4 - qp_solver_.updateVerbose(false); + qp_interface_ = + std::make_shared(false, 20000, 1.0e-8, 1.0e-6, false); } void JerkFilteredSmoother::setParam(const Param & smoother_param) @@ -301,14 +299,13 @@ bool JerkFilteredSmoother::apply( // execute optimization time_keeper_->start_track("optimize"); - const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + const auto optval = qp_interface_->optimize(P, A, q, lower_bound, upper_bound); time_keeper_->end_track("optimize"); - const std::vector optval = std::get<0>(result); - const int status_val = std::get<3>(result); - if (status_val != 1) { - RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + if (!qp_interface_->isSolved()) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_interface_->getStatus().c_str()); return false; } + const auto has_nan = std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); if (has_nan) { @@ -332,16 +329,6 @@ bool JerkFilteredSmoother::apply( output.at(i).acceleration_mps2 = a_stop_decel; } - qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); - - const int status_polish = std::get<2>(result); - if (status_polish != 1) { - const auto msg = status_polish == 0 ? "unperformed" - : status_polish == -1 ? "unsuccessful" - : "unknown"; - RCLCPP_DEBUG(logger_, "osqp polish process failed : %s. The result may be inaccurate", msg); - } - if (VERBOSE_TRAJECTORY_VELOCITY) { const auto s_output = trajectory_utils::calcArclengthArray(output); From c74c9ca4ac5321219258dcb2dfb8351e504701e5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 30 Jul 2024 19:38:55 +0900 Subject: [PATCH 4/4] perf(velocity_smoother): not resample debug_trajectories is not used (#8030) * not resample debug_trajectories if not published Signed-off-by: Takayuki Murooka * update dependant packages Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../analytical_jerk_constrained_smoother.hpp | 3 ++- .../smoother/jerk_filtered_smoother.hpp | 3 ++- .../smoother/l2_pseudo_jerk_smoother.hpp | 3 ++- .../smoother/linf_pseudo_jerk_smoother.hpp | 3 ++- .../smoother/smoother_base.hpp | 3 ++- .../autoware_velocity_smoother/src/node.cpp | 19 ++++++++--------- .../analytical_jerk_constrained_smoother.cpp | 3 ++- .../src/smoother/jerk_filtered_smoother.cpp | 21 ++++++++++++------- .../src/smoother/l2_pseudo_jerk_smoother.cpp | 3 ++- .../smoother/linf_pseudo_jerk_smoother.cpp | 3 ++- .../src/utilization/trajectory_utils.cpp | 2 +- .../src/node.cpp | 2 +- 12 files changed, 40 insertions(+), 28 deletions(-) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index bbc3828bb1b15..bfcd7db6b9c43 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -73,7 +73,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp index cc8b4f95ccbdc..79dd5cda20d4b 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -47,7 +47,8 @@ class JerkFilteredSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index a2a07ec6909aa..8f6bbc2236eb6 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -45,7 +45,8 @@ class L2PseudoJerkSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 7c46a53431608..e27a4db10e748 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -45,7 +45,8 @@ class LinfPseudoJerkSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp index 5efb003db183e..893a66f7eb7fa 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -61,7 +61,8 @@ class SmootherBase virtual ~SmootherBase() = default; virtual bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) = 0; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) = 0; virtual TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 23dee0019bf94..a8a3a847adec3 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -676,7 +676,8 @@ bool VelocitySmootherNode::smoothVelocity( std::vector debug_trajectories; if (!smoother_->apply( - initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories)) { + initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories, + publish_debug_trajs_)) { RCLCPP_WARN(get_logger(), "Fail to solve optimization."); } @@ -716,15 +717,13 @@ bool VelocitySmootherNode::smoothVelocity( pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp)); } - if (!debug_trajectories.empty()) { - for (auto & debug_trajectory : debug_trajectories) { - debug_trajectory.insert( - debug_trajectory.begin(), traj_resampled.begin(), - traj_resampled.begin() + traj_resampled_closest); - for (size_t i = 0; i < traj_resampled_closest; ++i) { - debug_trajectory.at(i).longitudinal_velocity_mps = - debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; - } + for (auto & debug_trajectory : debug_trajectories) { + debug_trajectory.insert( + debug_trajectory.begin(), traj_resampled.begin(), + traj_resampled.begin() + traj_resampled_closest); + for (size_t i = 0; i < traj_resampled_closest; ++i) { + debug_trajectory.at(i).longitudinal_velocity_mps = + debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; } } publishDebugTrajectories(debug_trajectories); diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 3906222454d35..43b03748133e6 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -103,7 +103,8 @@ AnalyticalJerkConstrainedSmoother::Param AnalyticalJerkConstrainedSmoother::getP bool AnalyticalJerkConstrainedSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, [[maybe_unused]] std::vector & debug_trajectories) + TrajectoryPoints & output, [[maybe_unused]] std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) { RCLCPP_DEBUG(logger_, "-------------------- Start --------------------"); diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index ecb67338fb1c4..3e363b8bcbab9 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -57,7 +57,7 @@ JerkFilteredSmoother::Param JerkFilteredSmoother::getParam() const bool JerkFilteredSmoother::apply( const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, - std::vector & debug_trajectories) + std::vector & debug_trajectories, const bool publish_debug_trajs) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -114,10 +114,12 @@ bool JerkFilteredSmoother::apply( auto opt_resampled_trajectory = resample(filtered); // Set debug trajectories - debug_trajectories.resize(3); - debug_trajectories[0] = resample(forward_filtered); - debug_trajectories[1] = resample(backward_filtered); - debug_trajectories[2] = resample(filtered); + if (publish_debug_trajs) { + debug_trajectories.resize(3); + debug_trajectories[0] = resample(forward_filtered); + debug_trajectories[1] = resample(backward_filtered); + debug_trajectories[2] = resample(filtered); + } // Ensure terminal velocity is zero opt_resampled_trajectory.back().longitudinal_velocity_mps = 0.0; @@ -127,9 +129,12 @@ bool JerkFilteredSmoother::apply( // No need to do optimization output.front().longitudinal_velocity_mps = v0; output.front().acceleration_mps2 = a0; - debug_trajectories[0] = output; - debug_trajectories[1] = output; - debug_trajectories[2] = output; + if (publish_debug_trajs) { + debug_trajectories.resize(3); + debug_trajectories[0] = output; + debug_trajectories[1] = output; + debug_trajectories[2] = output; + } return true; } diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index f379b217187c9..678c8e01bf67e 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -53,7 +53,8 @@ L2PseudoJerkSmoother::Param L2PseudoJerkSmoother::getParam() const bool L2PseudoJerkSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) + TrajectoryPoints & output, std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) { debug_trajectories.clear(); diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index 2ab3d6dd80dfc..60345ff0fa6f4 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -53,7 +53,8 @@ LinfPseudoJerkSmoother::Param LinfPseudoJerkSmoother::getParam() const bool LinfPseudoJerkSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) + TrajectoryPoints & output, std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) { debug_trajectories.clear(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 5900bd3531266..6815ca3ff8cb4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -76,7 +76,7 @@ bool smoothPath( TrajectoryPoints traj_smoothed; clipped.insert( clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); - if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; return false; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 9be416f9e9bb0..76b64c0a4d5e2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -361,7 +361,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed; clipped.insert( clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); - if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { RCLCPP_ERROR(get_logger(), "failed to smooth"); } traj_smoothed.insert(