From 7127cf713593785071c9323da935c81afb0f8095 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Tue, 29 Sep 2020 19:22:18 +0900 Subject: [PATCH] remove ROS1 packages temporarily Signed-off-by: mitsudome-r --- common/osqp_interface/CMakeLists.txt | 50 --- .../include/osqp_interface/csc_matrix_conv.h | 44 --- .../include/osqp_interface/osqp_interface.h | 229 ------------- common/osqp_interface/package.xml | 16 - common/osqp_interface/src/csc_matrix_conv.cpp | 143 -------- common/osqp_interface/src/osqp_interface.cpp | 250 -------------- common/spline_interpolation/CMakeLists.txt | 43 --- .../spline_interpolation.h | 105 ------ common/spline_interpolation/package.xml | 15 - .../src/spline_interpolation.cpp | 316 ------------------ 10 files changed, 1211 deletions(-) delete mode 100644 common/osqp_interface/CMakeLists.txt delete mode 100644 common/osqp_interface/include/osqp_interface/csc_matrix_conv.h delete mode 100644 common/osqp_interface/include/osqp_interface/osqp_interface.h delete mode 100644 common/osqp_interface/package.xml delete mode 100644 common/osqp_interface/src/csc_matrix_conv.cpp delete mode 100644 common/osqp_interface/src/osqp_interface.cpp delete mode 100644 common/spline_interpolation/CMakeLists.txt delete mode 100644 common/spline_interpolation/include/spline_interpolation/spline_interpolation.h delete mode 100644 common/spline_interpolation/package.xml delete mode 100644 common/spline_interpolation/src/spline_interpolation.cpp diff --git a/common/osqp_interface/CMakeLists.txt b/common/osqp_interface/CMakeLists.txt deleted file mode 100644 index 86629bbb08170..0000000000000 --- a/common/osqp_interface/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(osqp_interface) - -add_compile_options(-std=c++14 -Ofast) - -find_package(catkin REQUIRED COMPONENTS - roscpp - rostest - rosunit -) - -find_package(Eigen3 REQUIRED) - -find_package(osqp REQUIRED) -get_target_property(osqp_INCLUDE_DIR osqp::osqpstatic INTERFACE_INCLUDE_DIRECTORIES) - -catkin_package( - INCLUDE_DIRS - include - ${osqp_INCLUDE_DIR} - LIBRARIES - osqp_interface -) - -include_directories( - include - ${EIGEN3_INCLUDE_DIR} - ${osqp_INCLUDE_DIR} - ${catkin_INCLUDE_DIRS} -) - -add_library(osqp_interface src/osqp_interface.cpp src/csc_matrix_conv.cpp) - -target_link_libraries(osqp_interface - osqp::osqpstatic - ${catkin_LIBRARIES} -) - -install( - TARGETS - osqp_interface - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Install project namespaced headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.h b/common/osqp_interface/include/osqp_interface/csc_matrix_conv.h deleted file mode 100644 index dedaf55011b08..0000000000000 --- a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef CSC_MATRIX_CONV_H -#define CSC_MATRIX_CONV_H - -#include -#include "types.h" // for 'c_int' type ('long' or 'long long') - -namespace osqp -{ -// Struct for containing a 'Compressed-Column-Sparse Matrix' object. -// -// Elements: -// vals: Vector of non-zero values. Ex: [4,1,1,2] -// row_idxs: Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') -// col_idxs: Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') -struct CSC_Matrix -{ - std::vector vals; - std::vector row_idxs; - std::vector col_idxs; -}; - -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); -CSC_Matrix calCSCMatrixTrapesoidal(const Eigen::MatrixXd & mat); - -void printCSCMatrix(CSC_Matrix & csc_mat); - -} // namespace osqp - -#endif // CSC_MATRIX_CONV_H \ No newline at end of file diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.h b/common/osqp_interface/include/osqp_interface/osqp_interface.h deleted file mode 100644 index 1497657771d06..0000000000000 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.h +++ /dev/null @@ -1,229 +0,0 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - * Author: Robin Karlsson - */ -#ifndef OSQP_INTERFACE_H -#define OSQP_INTERFACE_H - -#include -#include -#include "osqp.h" - -namespace osqp -{ -struct CSC_Matrix; -const c_float INF = OSQP_INFTY; -} // namespace osqp - -namespace osqp -{ -/** - * Implementation of a native C++ interface for the OSQP solver. - * - * The interface takes in the problem formalation as Eigen matrices and vectors, converts these objects into C-style - * CSC matrices and dynamic arrays, loads the data into the OSQP workspace dataholder, and runs the optimizer. - * - * The optimization results are return as a vector tuple by the optimization function. - * std::tuple, std::vector> result = osqp_interface.optimize(); - * std::vector param = std::get<0>(result); - * double x_0 = param[0]; - * double x_1 = param[1]; - * - * The interface can be used in several ways: - * - * 1. Initialize the interface WITHOUT data. Load the problem formulation at the optimization call. - * osqp_interface = OSQPInterface(); - * osqp_interface.optimize(P, A, q, l, u); - * - * 2. Initialize the interface WITH data. - * osqp_interface = OSQPInterface(P, A, q, l, u); - * osqp_interface.optimize(); - * - * 3. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. - * osqp_interface = OSQPInterface(P, A, q, l, u); - * osqp_interface.optimize(); - * while() - * { - * osqp_interface.updateP(P_new); - * osqp_interface.updateA(A_new); - * osqp_interface.updateQ(q_new); - * osqp_interface.updateL(l_new); - * osqp_interface.updateU(u_new); - * osqp_interface.optimize(); - * } - * - * Ref: https://osqp.org/docs/solver/index.html - */ -class OSQPInterface -{ -private: - /***************************** - * OSQP WORKSPACE STRUCTURES - *****************************/ - OSQPWorkspace * work; - OSQPSettings * settings; - OSQPData * data; - // Number of parameters to optimize - c_int param_n; - - // For destructor to know if matrices P, A are in - bool problem_in_memory = false; - - // Runs the solver on the stored problem. - std::tuple, std::vector, int, int> solve(); - - /***************************** - * DATA CONVERSION FUNCTIONS - *****************************/ - // Converts problem input matrices to CSC matrix structs. - CSC_Matrix transformP(const Eigen::MatrixXd & P, int * nonzeros); - CSC_Matrix transformA(const Eigen::MatrixXd & A); - // Converts problem input vectors to dynamic arrays. - double * transformQ(const std::vector & q); - double * transformL(const std::vector & l); - double * transformU(const std::vector & u); - // Converts an Eigen matrix into a CSC matrix struct. - CSC_Matrix convEigenMatrixToCSCMatrix(const Eigen::MatrixXd A); - // Converts an Eigen vector matrix into a dynamic array. - double * convEigenVecToDynFloatArray(const Eigen::MatrixXd x); - - // Exitflag - c_int exitflag; - - inline bool isEqual(double x, double y); - -public: - // Returns a flag for asserting interface condition (Healthy condition: 0). - c_int getExitFlag(void); - - /**************************** - * INITIALIZATION FUNCTIONS - ****************************/ - - // Initializes the OSQP interface without setting up the problem. - // - // Steps: - // 1. Initializes the OSQP object (incl. settings, data objects). - // 2. Solver settings (accuracy etc.). - OSQPInterface(const c_float eps_abs = 1.0e-4, const bool polish = true); - - // Initializes the OSQP solver interface and sets up the problem. - // - // Steps: - // 1. Runs the base constructor (without setting up the problem). - // 2. Sets up the problem. - // 2.1. Converts the Eigen matrices to CSC matrices. - // 2.2. Converts the vectors to dynamic arrays. - // 2.3. Loads the problem formulation into the OSQP data object and sets up the workspace. - // - // Args: - // P: (n,n) matrix defining relations between parameters. - // A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q: (n) vector defining the linear cost of the problem. - // l: (m) vector defining the lower bound problem constraint. - // u: (m) vector defining the upper bound problem constraint. - // eps_abs: Absolute convergence tolerance. - OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); - - // For freeing dynamic memory used by OSQP's data object. - ~OSQPInterface(); - - /**************** - * OPTIMIZATION - ****************/ - // Solves the stored convec quadratic program (QP) problem using the OSQP solver. - // - // The function returns a tuple containing the solution as two float vectors. - // The first element of the tuple contains the 'primal' solution. The second element contains the 'lagrange - // multiplier' solution. The third element contains an integer with solver polish status information. - // - // How to use: - // 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. - // 2. Initialize the interface and set up the problem. - // osqp_interface = OSQPInterface(P, A, q, l, u, 1e-6); - // 3. Call the optimization function. - // std::tuple, std::vector> result; - // result = osqp_interface.optimize(); - // 4. Access the optimized parameters. - // std::vector param = std::get<0>(result); - // double x_0 = param[0]; - // double x_1 = param[1]; - std::tuple, std::vector, int, int> optimize(); - - // Solves convex quadratic programs (QPs) using the OSQP solver. - // - // The function returns a tuple containing the solution as two float vectors. - // The first element of the tuple contains the 'primal' solution. The second element contains the 'lagrange - // multiplier' solution. The third element contains an integer with solver polish status information. - // - // How to use: - // 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. - // 2. Initialize the interface. - // osqp_interface = OSQPInterface(1e-6); - // 3. Call the optimization function with the problem formulation. - // std::tuple, std::vector> result; - // result = osqp_interface.optimize(P, A, q, l, u, 1e-6); - // 4. Access the optimized parameters. - // std::vector param = std::get<0>(result); - // double x_0 = param[0]; - // double x_1 = param[1]; - std::tuple, std::vector, int, int> optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - /************************** - * DATA-RELATED FUNCTIONS - **************************/ - - // Converts the input data and sets up the workspace object. - // - // Args: - // P: (n,n) matrix defining relations between parameters. - // A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q: (n) vector defining the linear cost of the problem. - // l: (m) vector defining the lower bound problem constraint. - // u: (m) vector defining the upper bound problem constraint. - c_int initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - // Updates problem parameters while keeping solution in memory. - // - // Args: - // P_new: (n,n) matrix defining relations between parameters. - // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q_new: (n) vector defining the linear cost of the problem. - // l_new: (m) vector defining the lower bound problem constraint. - // u_new: (m) vector defining the upper bound problem constraint. - void updateP(const Eigen::MatrixXd & P_new); - void updateA(const Eigen::MatrixXd & A_new); - void updateQ(const std::vector & q_new); - void updateL(const std::vector & l_new); - void updateU(const std::vector & u_new); - void updateBounds(const std::vector & l_new, const std::vector & u_new); - void updateEpsAbs(const double eps_abs); - void updateEpsRel(const double eps_rel); - void updateMaxIter(const int iter); - void updateVerbose(const bool verbose); - - int getTakenIter(); -}; - -} // namespace osqp - -#endif // OSQP_INTERFACE_H diff --git a/common/osqp_interface/package.xml b/common/osqp_interface/package.xml deleted file mode 100644 index 3032a839aa2c2..0000000000000 --- a/common/osqp_interface/package.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - osqp_interface - 0.1.0 - The osqp_interface package - Robin Karlsson - Apache 2 - - catkin - - - roscpp - rostest - rosunit - - diff --git a/common/osqp_interface/src/csc_matrix_conv.cpp b/common/osqp_interface/src/csc_matrix_conv.cpp deleted file mode 100644 index e8454cabc1296..0000000000000 --- a/common/osqp_interface/src/csc_matrix_conv.cpp +++ /dev/null @@ -1,143 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -#include -#include - -#include -#include - -#include "osqp_interface/csc_matrix_conv.h" - -namespace osqp -{ -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) -{ - int elem = mat.nonZeros(); - int rows = mat.rows(); - int cols = mat.cols(); - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - int elem_count = 0; - - col_idxs.push_back(0); - - for (int j = 0; j < cols; j++) // col iteration - { - for (int i = 0; i < rows; i++) // row iteration - { - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -CSC_Matrix calCSCMatrixTrapesoidal(const Eigen::MatrixXd & mat) -{ - int elem = mat.nonZeros(); - int rows = mat.rows(); - int cols = mat.cols(); - - if (rows != cols) { - throw std::invalid_argument("Matrix must be square (n, n)"); - } - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - int trap_last_idx = 0; - int elem_count = 0; - - col_idxs.push_back(0); - - for (int j = 0; j < cols; j++) // col iteration - { - for (int i = 0; i <= trap_last_idx; i++) // row iteration - { - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - trap_last_idx += 1; - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -void printCSCMatrix(CSC_Matrix & csc_mat) -{ - std::cout << "["; - for (std::vector::const_iterator it = csc_mat.vals.begin(); it != csc_mat.vals.end(); - it++) { - std::cout << *it << ", "; - } - std::cout << "]" << std::endl; - - std::cout << "["; - for (std::vector::const_iterator it = csc_mat.row_idxs.begin(); - it != csc_mat.row_idxs.end(); it++) { - std::cout << *it << ", "; - } - std::cout << "]" << std::endl; - - std::cout << "["; - for (std::vector::const_iterator it = csc_mat.col_idxs.begin(); - it != csc_mat.col_idxs.end(); it++) { - std::cout << *it << ", "; - } - std::cout << "]" << std::endl; -} - -} // namespace osqp \ No newline at end of file diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp deleted file mode 100644 index 25f9272e62c11..0000000000000 --- a/common/osqp_interface/src/osqp_interface.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - * Author: Robin Karlsson - */ -#include -#include -#include - -#include "osqp.h" -#include "osqp_interface/csc_matrix_conv.h" -#include "osqp_interface/osqp_interface.h" - -namespace osqp -{ -OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish) -{ - /************************ - * INITIALIZE WORKSPACE - ************************/ - settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings)); - data = (OSQPData *)c_malloc(sizeof(OSQPData)); - - /******************* - * SOLVER SETTINGS - *******************/ - if (settings) { - osqp_set_default_settings(settings); - settings->alpha = 1.6; // Change alpha parameter - settings->eps_rel = 1.0E-4; - 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 = false; - settings->polish = polish; - } - // Set flag for successful initialization - exitflag = 0; -} - -OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) -: OSQPInterface(eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -c_int OSQPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - /******************* - * SET UP MATRICES - *******************/ - CSC_Matrix P_csc = calCSCMatrixTrapesoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector 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 - **********************/ - // Number of constraints - c_int constr_m = A.rows(); - // Number of parameters - param_n = P.rows(); - - /***************** - * POLULATE DATA - *****************/ - data->m = constr_m; - data->n = param_n; - data->P = csc_matrix( - data->n, data->n, P_csc.vals.size(), P_csc.vals.data(), P_csc.row_idxs.data(), - P_csc.col_idxs.data()); - data->q = q_dyn; - data->A = csc_matrix( - data->m, data->n, 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; - - // For deconstructor - problem_in_memory = true; - - // Setup workspace - exitflag = osqp_setup(&work, data, settings); - - return exitflag; -} - -OSQPInterface::~OSQPInterface() -{ - // Cleanup dynamic OSQP memory - if (work) { - osqp_cleanup(work); - } - if (data) { - if (problem_in_memory) { - c_free(data->A); - c_free(data->P); - } - c_free(data); - } - if (settings) c_free(settings); -} - -void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) -{ - /* - // Transform 'P' into an 'upper trapesoidal matrix' - Eigen::MatrixXd P_trap = P_new.triangularView(); - // Transform 'P' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix P_sparse = P_trap.sparseView(); - double *P_val_ptr = P_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int P_elem_N = P_sparse.nonZeros(); - */ - CSC_Matrix P_csc = calCSCMatrixTrapesoidal(P_new); - osqp_update_P(work, P_csc.vals.data(), OSQP_NULL, P_csc.vals.size()); -} - -void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) -{ - /* - // Transform 'A' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix A_sparse = A_new.sparseView(); - double *A_val_ptr = A_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int A_elem_N = A_sparse.nonZeros(); - */ - CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A(work, A_csc.vals.data(), OSQP_NULL, 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(work, 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(work, 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(work, u_dyn); -} - -void OSQPInterface::updateBounds( - const std::vector & l_new, const std::vector & u_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - 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(work, l_dyn, u_dyn); -} - -void OSQPInterface::updateEpsAbs(const double eps_abs) { osqp_update_eps_abs(work, eps_abs); } - -void OSQPInterface::updateEpsRel(const double eps_rel) { osqp_update_eps_rel(work, eps_rel); } - -void OSQPInterface::updateMaxIter(const int max_iter) { osqp_update_max_iter(work, max_iter); } - -void OSQPInterface::updateVerbose(const bool is_verbose) { osqp_update_verbose(work, is_verbose); } - -int OSQPInterface::getTakenIter() { return work->info->iter; } - -std::tuple, std::vector, int, int> OSQPInterface::solve() -{ - // Solve Problem - osqp_solve(work); - - /******************** - * EXTRACT SOLUTION - ********************/ - double * sol_x = work->solution->x; - double * sol_y = work->solution->y; - std::vector sol_primal(sol_x, sol_x + static_cast(param_n)); - std::vector sol_lagrange_multiplier(sol_y, sol_y + static_cast(param_n)); - // Solver polish status - int status_polish = work->info->status_polish; - // Solver solution status - int status_solution = work->info->status_val; - // Result tuple - std::tuple, std::vector, int, int> result = - std::make_tuple(sol_primal, sol_lagrange_multiplier, status_polish, status_solution); - - return result; -} - -std::tuple, std::vector, int, int> OSQPInterface::optimize() -{ - // Run the solver on the stored problem representation. - std::tuple, std::vector, int, int> result = solve(); - return result; -} - -std::tuple, std::vector, int, int> OSQPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // Allocate memory for problem - initializeProblem(P, A, q, l, u); - - // Run the solver on the stored problem representation. - std::tuple, std::vector, int, int> result = solve(); - - // Free allocated memory for problem - osqp_cleanup(work); - return result; -} - -inline bool OSQPInterface::isEqual(double x, double y) -{ - const double epsilon = 1e-6; - return std::abs(x - y) <= epsilon * std::abs(x); -} - -c_int OSQPInterface::getExitFlag(void) { return exitflag; } - -} // namespace osqp diff --git a/common/spline_interpolation/CMakeLists.txt b/common/spline_interpolation/CMakeLists.txt deleted file mode 100644 index 264bd5da07b78..0000000000000 --- a/common/spline_interpolation/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(spline_interpolation) - -add_compile_options(-std=c++14) - -find_package(catkin REQUIRED COMPONENTS - roscpp -) - -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - spline_interpolation -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(spline_interpolation src/spline_interpolation.cpp) - -target_link_libraries(spline_interpolation - ${catkin_LIBRARIES} -) - -add_dependencies(spline_interpolation - ${catkin_EXPORTED_TARGETS} -) - -install( - TARGETS - spline_interpolation - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Install project namespaced headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/common/spline_interpolation/include/spline_interpolation/spline_interpolation.h b/common/spline_interpolation/include/spline_interpolation/spline_interpolation.h deleted file mode 100644 index 41b98392b480c..0000000000000 --- a/common/spline_interpolation/include/spline_interpolation/spline_interpolation.h +++ /dev/null @@ -1,105 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include - -namespace spline_interpolation -{ -enum class Method { Explicit, PCG, SOR }; - -class LinearSystemSolver -{ -public: - void setCoefficients( - const std::vector & h, const std::vector & a, const size_t max_iter); - virtual std::vector solve() const = 0; - -protected: - size_t max_iter_; - const double converge_range_ = 0.00001; - std::vector coef_prev_; - std::vector coef_diag_; - std::vector coef_next_; - std::vector rhs_; - - bool isConvergeL1(const std::vector & r1, const std::vector & r2) const; -}; - -class PreconditionedConjugateGradient : public LinearSystemSolver -{ -public: - PreconditionedConjugateGradient() = default; - std::vector solve() const; - -private: - std::vector calcMatrixVectorProduct(const std::vector & src) const; - std::vector calcDiagonalScaling(const std::vector & src) const; - double calcInnerProduct(const std::vector & src1, const std::vector & src2) const; -}; - -class SOR : public LinearSystemSolver -{ -public: - SOR() = default; - std::vector solve() const; - -private: - const double omega_ = 1.8; -}; - -class SplineInterpolator -{ -public: - SplineInterpolator() = default; - bool interpolate( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index, std::vector & return_value, - const Method method = Method::PCG); - -private: - double getValue(const double & query, const std::vector & base_index) const; - void generateSpline( - const std::vector & base_index, const std::vector & base_value); - bool isIncrease(const std::vector & x) const; - bool isNonDecrease(const std::vector & x) const; - bool isValidInput( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index, std::vector & return_value) const; - - std::vector solveLinearSystemExplicit(); - - bool initialized_; - Method method_; - - std::vector a_; - std::vector b_; - std::vector c_; - std::vector d_; - std::vector h_; - - PreconditionedConjugateGradient PCG_solver_; - SOR SOR_solver_; -}; - -} // namespace spline_interpolation diff --git a/common/spline_interpolation/package.xml b/common/spline_interpolation/package.xml deleted file mode 100644 index d1e0490876a81..0000000000000 --- a/common/spline_interpolation/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - spline_interpolation - 0.1.0 - The spline interpolation package - Fumiya Watanabe - Fumiya Watanabe - - Apache 2 - - catkin - - roscpp - - diff --git a/common/spline_interpolation/src/spline_interpolation.cpp b/common/spline_interpolation/src/spline_interpolation.cpp deleted file mode 100644 index 78b3e4e9ccd65..0000000000000 --- a/common/spline_interpolation/src/spline_interpolation.cpp +++ /dev/null @@ -1,316 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "spline_interpolation/spline_interpolation.h" - -namespace spline_interpolation -{ -void SplineInterpolator::generateSpline( - const std::vector & base_index, const std::vector & base_value) -{ - const size_t N = base_value.size(); - - a_.clear(); - b_.clear(); - c_.clear(); - d_.clear(); - h_.clear(); - - a_ = base_value; - - for (size_t i = 0; i < N - 1; ++i) { - h_.push_back(base_index[i + 1] - base_index[i]); - } - - if (method_ == Method::Explicit) { - c_ = solveLinearSystemExplicit(); - } else if (method_ == Method::PCG) { - PCG_solver_.setCoefficients(h_, a_, 1000); - c_ = PCG_solver_.solve(); - } else if (method_ == Method::SOR) { - SOR_solver_.setCoefficients(h_, a_, 1000); - c_ = SOR_solver_.solve(); - } else { - std::cerr << "Unsupported method. Supported solver: Explicit, PCG, SOR" << std::endl; - } - - for (size_t i = 0; i < N - 1; i++) { - d_.push_back((c_[i + 1] - c_[i]) / (3.0 * h_[i])); - b_.push_back((a_[i + 1] - a_[i]) / h_[i] - h_[i] * (2.0 * c_[i] + c_[i + 1]) / 3.0); - } - - d_.push_back(0.0); - b_.push_back(0.0); - - initialized_ = true; -}; - -double SplineInterpolator::getValue( - const double & query, const std::vector & base_index) const -{ - if (!initialized_) { - std::cerr << "[interpolate] spline is uninitialized" << std::endl; - return 0.0; - } - - size_t j = 0; - while (base_index[j] <= query) ++j; - --j; - const double ds = query - base_index[j]; - return a_[j] + (b_[j] + (c_[j] + d_[j] * ds) * ds) * ds; -} - -bool SplineInterpolator::interpolate( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index, std::vector & return_value, const Method method) -{ - method_ = method; - - if (!isValidInput(base_index, base_value, return_index, return_value)) { - std::cerr << "[interpolate] invalid input. interpolation failed." << std::endl; - return false; - } - - // calculate spline coefficients - generateSpline(base_index, base_value); - - // interpolate values at query points - for (size_t i = 0; i < return_index.size(); ++i) { - return_value.push_back(getValue(return_index[i], base_index)); - } - return true; -} - -bool SplineInterpolator::isIncrease(const std::vector & x) const -{ - for (int i = 0; i < static_cast(x.size()) - 1; ++i) { - if (x[i] >= x[i + 1]) return false; - } - return true; -}; - -bool SplineInterpolator::isNonDecrease(const std::vector & x) const -{ - for (int i = 0; i < static_cast(x.size()) - 1; ++i) { - if (x[i] > x[i + 1]) return false; - } - return true; -}; - -bool SplineInterpolator::isValidInput( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index, std::vector & return_value) const -{ - if (base_index.empty() || base_value.empty() || return_index.empty()) { - std::cout << "bad index : some vector is empty. base_index: " << base_index.size() - << ", base_value: " << base_value.size() << ", return_index: " << return_index.size() - << std::endl; - return false; - } - if (!isIncrease(base_index)) { - std::cout << "bad index : base_index is not monotonically increasing. base_index = [" - << base_index.front() << ", " << base_index.back() << "]" << std::endl; - return false; - } - if (!isNonDecrease(return_index)) { - std::cout << "bad index : base_index is not monotonically nondecreasing. return_index = [" - << return_index.front() << ", " << return_index.back() << "]" << std::endl; - return false; - } - if (return_index.front() < base_index.front()) { - std::cout << "bad index : return_index.front() < base_index.front()" << std::endl; - return false; - } - if (base_index.back() < return_index.back()) { - std::cout << "bad index : base_index.back() < return_index.back()" << std::endl; - return false; - } - if (base_index.size() != base_value.size()) { - std::cout << "bad index : base_index.size() != base_value.size()" << std::endl; - return false; - } - - return true; -} - -std::vector SplineInterpolator::solveLinearSystemExplicit() -{ - const size_t N = a_.size(); - std::vector ans; - ans.push_back(0.0); - for (size_t i = 1; i < N - 1; i++) ans.push_back(3.0 * (a_[i - 1] - 2.0 * a_[i] + a_[i + 1])); - ans.push_back(0.0); - - std::vector w; - w.push_back(0.0); - - for (size_t i = 1; i < N - 1; ++i) { - const double tmp = 1.0 / (4.0 - w[i - 1]); - ans[i] = (ans[i] - ans[i - 1]) * tmp; - w.push_back(tmp); - } - - for (size_t i = N - 2; i > 0; --i) ans[i] = ans[i] - ans[i + 1] * w[i]; - - return ans; -} - -void LinearSystemSolver::setCoefficients( - const std::vector & h, const std::vector & a, const size_t max_iter) -{ - max_iter_ = max_iter; - - rhs_.assign(a.size(), 0.0); - coef_prev_.assign(a.size(), 0.0); - coef_diag_.assign(a.size(), 1.0); - coef_next_.assign(a.size(), 0.0); - - for (size_t i = 1; i < a.size() - 1; ++i) { - rhs_[i] = 3.0 / h[i] * (a[i + 1] - a[i]) - 3.0 / h[i - 1] * (a[i] - a[i - 1]); - coef_prev_[i] = h[i - 1]; - coef_diag_[i] = 2.0 * (h[i] + h[i - 1]); - coef_next_[i] = h[i]; - } -} - -bool LinearSystemSolver::isConvergeL1( - const std::vector & r1, const std::vector & r2) const -{ - double d = 0.0; - for (size_t i = 0; i < r1.size(); ++i) { - d += std::fabs(r1.at(i) - r2.at(i)); - } - return d < converge_range_; -} - -std::vector PreconditionedConjugateGradient::solve() const -{ - // allocation - const size_t N = rhs_.size(); - std::vector x(N); // Solution - std::vector r(N - 2); // Residual - std::vector rn(N - 2); // Residual (next step) - std::vector z(N - 2); // Preconditioned residual - std::vector zn(N - 2); // Preconditioned residual (next step) - std::vector p(N - 2); // Basis - std::vector zeros(N - 2); // Zero vector (for check convergence) - - // initialization - x[0] = rhs_[0]; - x[N - 1] = rhs_[N - 1]; - - // extract rhs[1:N-2] (rhs[1] = rhs[1] - h[0]*rhs[0], rhs[N-2] = rhs[N-2] - h[N-2]*rhs[N-1]) - std::vector rhs_in = rhs_; - rhs_in[1] -= coef_prev_[1] * rhs_[0]; - rhs_in[N - 2] -= coef_next_[N - 2] * rhs_[N - 1]; - rhs_in.erase(rhs_in.begin()); - rhs_in.erase(std::prev(rhs_in.end())); - - // extract x[1:N-2] - std::vector x_in = x; - x_in.erase(x_in.begin()); - x_in.erase(std::prev(x_in.end())); - - // r = rhs - Ax - const std::vector Ax = calcMatrixVectorProduct(x_in); - std::transform(rhs_in.begin(), rhs_in.end(), Ax.begin(), r.begin(), std::minus()); - if (isConvergeL1(rhs_in, Ax)) return x; - // p0 = DiagonalScaling(r) - // z0 = p0 - p = calcDiagonalScaling(r); - z = p; - size_t num_iter = max_iter_; - for (size_t iter = 0; iter <= max_iter_; ++iter) { - // (1) y_k = A * p_k - const std::vector y = calcMatrixVectorProduct(p); - // (2) alpha = (r_k' * z_k) / (p_k' * y_k); - const double alpha = std::inner_product(r.begin(), r.end(), z.begin(), 0.0f) / - std::inner_product(p.begin(), p.end(), y.begin(), 0.0f); - // (3) x_k+1 = x_k + alpha * p_k - std::transform(x_in.begin(), x_in.end(), p.begin(), x_in.begin(), [alpha](double x, double p) { - return x + alpha * p; - }); - // (4) r_k+1 = r_k - alpha * y_k - std::transform(r.begin(), r.end(), y.begin(), rn.begin(), [alpha](double r, double y) { - return r - alpha * y; - }); - // (5) check convergence - if (isConvergeL1(rn, zeros)) { - num_iter = iter; - break; - } - // (6) zn = DiagonalScaling(rn) - zn = calcDiagonalScaling(rn); - // (7) beta = (r_k+1' * z_k+1) / (r_k * z_k) - const double beta = std::inner_product(rn.begin(), rn.end(), zn.begin(), 0.0f) / - std::inner_product(r.begin(), r.end(), z.begin(), 0.0f); - // (8) p_k+1 = z_k+1 + beta * p_k - std::transform(zn.begin(), zn.end(), p.begin(), p.begin(), [beta](double zn, double p) { - return zn + beta * p; - }); - r = rn; - z = zn; - } - - for (size_t i = 1; i < x.size(); ++i) x[i] = x_in[i - 1]; - - if (num_iter == max_iter_) ROS_WARN("[interpolate (PCG)] unconverged!"); - return x; -} - -std::vector PreconditionedConjugateGradient::calcMatrixVectorProduct( - const std::vector & src) const -{ - std::vector dst(src.size(), 0.0); - for (size_t i = 0; i < src.size(); ++i) { - if (i != 0) dst[i] += coef_prev_[i + 1] * src[i - 1]; - dst[i] += coef_diag_[i + 1] * src[i]; - if (i != (src.size() - 1)) dst[i] += coef_next_[i + 1] * src[i + 1]; - } - return dst; -} - -std::vector PreconditionedConjugateGradient::calcDiagonalScaling( - const std::vector & src) const -{ - std::vector dst(src.size(), 0.0); - for (size_t i = 0; i < src.size(); ++i) { - dst[i] = src[i] / coef_diag_[i + 1]; - } - return dst; -} - -std::vector SOR::solve() const -{ - // solve A * ans = rhs by SOR method - std::vector ans(rhs_.size(), 1.0); - std::vector ans_next(rhs_.size(), 0.0); - size_t num_iter = 0; - - while (!isConvergeL1(ans, ans_next) && num_iter <= max_iter_) { - ans = ans_next; - for (size_t i = 1; i < rhs_.size() - 1; ++i) { - ans_next[i] += omega_ / (coef_diag_[i]) * - (rhs_[i] - (coef_prev_[i] * ans_next[i - 1] + coef_diag_[i] * ans[i] + - coef_next_[i] * ans[i + 1])); - } - ++num_iter; - } - if (num_iter > max_iter_) ROS_WARN("[interpolate (SOR)] unconverged!"); - return ans_next; -} - -} // namespace spline_interpolation