release v0.4.0

common/osqp_interface/CMakeLists.txt

@@ -0,0 +1,50 @@
+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}
+)

common/osqp_interface/include/osqp_interface/csc_matrix_conv.h

@@ -0,0 +1,44 @@
+/*
+ * 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 <eigen3/Eigen/Core>
+#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<c_float> vals;
+  std::vector<c_int> row_idxs;
+  std::vector<c_int> 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

common/osqp_interface/include/osqp_interface/osqp_interface.h

@@ -0,0 +1,227 @@
+/*
+ * 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 <eigen3/Eigen/Core>
+#include <vector>
+#include "osqp.h"
+
+namespace osqp
+{
+struct CSC_Matrix;
+}
+
+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<double>, std::vector<double>> result = osqp_interface.optimize();
+ *   std::vector<double> 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<double>, std::vector<double>, 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<double> & q);
+  double * transformL(const std::vector<double> & l);
+  double * transformU(const std::vector<double> & 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<double> & q,
+    const std::vector<double> & l, const std::vector<double> & 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<double>, std::vector<double>> result;
+  //        result = osqp_interface.optimize();
+  //   4. Access the optimized parameters.
+  //        std::vector<float> param = std::get<0>(result);
+  //        double x_0 = param[0];
+  //        double x_1 = param[1];
+  std::tuple<std::vector<double>, std::vector<double>, 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<double>, std::vector<double>> result;
+  //        result = osqp_interface.optimize(P, A, q, l, u, 1e-6);
+  //   4. Access the optimized parameters.
+  //        std::vector<float> param = std::get<0>(result);
+  //        double x_0 = param[0];
+  //        double x_1 = param[1];
+  std::tuple<std::vector<double>, std::vector<double>, int> optimize(
+    const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
+    const std::vector<double> & l, const std::vector<double> & 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<double> & q,
+    const std::vector<double> & l, const std::vector<double> & 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<double> & q_new);
+  void updateL(const std::vector<double> & l_new);
+  void updateU(const std::vector<double> & u_new);
+  void updateBounds(const std::vector<double> & l_new, const std::vector<double> & u_new);
+  void updateEpsAbs(const double eps_abs);
+  void updateEpsRel(const double eps_rel);
+  void updateMaxIter(const int iter);
+
+  int getTakenIter();
+};
+
+}  // namespace osqp
+
+#endif  // OSQP_INTERFACE_H

common/osqp_interface/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>osqp_interface</name>
+  <version>0.1.0</version>
+  <description>The osqp_interface package</description>
+  <maintainer email="robin.karlsson@tier4.jp">Robin Karlsson</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <depend>roscpp</depend>
+  <depend>rostest</depend>
+  <depend>rosunit</depend>
+
+</package>