Merge pull request #40 from arocchi/add_free_solvers_upstream

Adds osqp and qpOASES solver interfaces

README.md

@@ -1,6 +1,21 @@
 # trajopt_ros
 Integration of TrajOpt into ROS
 
+## Solvers support
+`trajopt_ros` implements sequential convex optimization to solve the motion planning problem.
+It implements a penalty method to optimize for joint velocities while satisfying a set of constraints.
+Internally, it makes use of convex solvers that are able to solve linearly constrained quadratic problems.
+At the moment, the following solvers are supported:
+- `BPMPD` (interior point method, free for non-commercial use only)
+- `Gurobi` (simplex and interior point/parallel barrier, license required)
+- `OSQP` (ADMM, BSD2 license)
+- `qpOASES` (active set, LGPL 2.1 license)
+
+While the `BPMPD` library is bundled in the distribution, `Gurobi`, `OSQP` and `qpOASES` need to be installed in the system.
+To compile with `Gurobi` support, a `GUROBI_HOME` variable needs to be defined.
+Once `trajopt_ros` is compiled with support for a specific solver, you can select it by properly setting the `TRAJOPT_CONVEX_SOLVER` environment variable. Possible values are `GUROBI`, `BPMPD`, `OSQP`, `QPOASES`, `AUTO_SOLVER`.
+The selection to `AUTO_SOLVER` is the default and automatically picks the best between the available solvers.
+
 ## Build Branch Sphinx Documentation
 
 ```

trajopt/include/trajopt/problem_description.hpp

@@ -86,6 +86,7 @@ struct BasicInfo
   std::string manip;
   std::string robot;  // optional
   IntVec dofs_fixed;  // optional
+  sco::ModelType convex_solver; // which convex solver to use
 };
 
 /**

trajopt/src/problem_description.cpp

@@ -131,6 +131,7 @@ void ProblemConstructionInfo::readBasicInfo(const Json::Value& v)
   json_marshal::childFromJson(v, basic_info.manip, "manip");
   json_marshal::childFromJson(v, basic_info.robot, "robot", std::string(""));
   json_marshal::childFromJson(v, basic_info.dofs_fixed, "dofs_fixed", IntVec());
+  json_marshal::childFromJson(v, basic_info.convex_solver, "convex_solver", basic_info.convex_solver);
   // TODO: optimization parameters, etc?
 }
 
@@ -380,7 +381,8 @@ TrajOptProbPtr ConstructProblem(const Json::Value& root, tesseract::BasicEnvCons
   return ConstructProblem(pci);
 }
 
-TrajOptProb::TrajOptProb(int n_steps, const ProblemConstructionInfo& pci) : m_kin(pci.kin), m_env(pci.env)
+TrajOptProb::TrajOptProb(int n_steps, const ProblemConstructionInfo& pci)
+ : OptProb(pci.basic_info.convex_solver), m_kin(pci.kin), m_env(pci.env)
 {
   const Eigen::MatrixX2d& limits = m_kin->getLimits();
   int n_dof = m_kin->numJoints();

trajopt_sco/CMakeLists.txt

@@ -3,12 +3,19 @@ project(trajopt_sco)
 
 add_compile_options(-std=c++11 -Wall -Wextra)
 
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules/")
 find_package(catkin REQUIRED COMPONENTS trajopt_utils)
-
+find_package(GUROBI QUIET)
+find_package(osqp QUIET)
+find_package(qpOASES QUIET)
 find_package(Eigen3 REQUIRED)
 
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(JSONCPP jsoncpp)
+
 set(SCO_SOURCE_FILES
     src/solver_interface.cpp
+    src/solver_utils.cpp
     src/modeling.cpp
     src/expr_ops.cpp
     src/expr_vec_ops.cpp
@@ -23,18 +30,17 @@ endif()
 
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES ${PROJECT_NAME}
+  LIBRARIES ${PROJECT_NAME} ${JSONCPP_LIBRARIES}
   CATKIN_DEPENDS trajopt_utils
 #  DEPENDS system_lib
 )
 
-list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules/")
-find_package(GUROBI QUIET)
 
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   SYSTEM ${EIGEN3_INCLUDE_DIRS}
+  SYSTEM ${JSONCPP_INCLUDE_DIRS}
 )
 
 if (HAVE_BPMPD)
@@ -51,32 +57,49 @@ if (HAVE_BPMPD)
   list(APPEND SCO_SOURCE_FILES src/bpmpd_interface.cpp)
   set_property(SOURCE src/bpmpd_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS BPMPD_CALLER="\\\"${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}/bpmpd_caller\\\"")
 
-
   #TODO: Levi check if this is correct.
   set(BPMPD_WORKING_DIR "${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}/")
   set_property(SOURCE src/bpmpd_caller.cpp APPEND PROPERTY COMPILE_DEFINITIONS BPMPD_WORKING_DIR="${BPMPD_WORKING_DIR}")
   file(COPY src/bpmpd.par DESTINATION ${BPMPD_WORKING_DIR})
 
-  set_property(SOURCE src/solver_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS HAVE_BPMPD )
+  set_property(SOURCE src/solver_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS HAVE_BPMPD)
 endif()
 
 if (GUROBI_FOUND)
   include_directories(${GUROBI_INCLUDE_DIR})
-  set_property(SOURCE src/solver_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS HAVE_GUROBI )
+  set_property(SOURCE src/solver_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS HAVE_GUROBI)
   list(APPEND SCO_SOURCE_FILES src/gurobi_interface.cpp)
 endif(GUROBI_FOUND)
 
+if (osqp_FOUND)
+  set_property(SOURCE src/solver_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS HAVE_OSQP)
+  list(APPEND SCO_SOURCE_FILES src/osqp_interface.cpp)
+endif()
+
+if (qpOASES_FOUND)
+  include_directories(${qpOASES_INCLUDE_DIRS})
+  set_property(SOURCE src/solver_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS HAVE_QPOASES)
+  list(APPEND SCO_SOURCE_FILES src/qpoases_interface.cpp)
+endif()
+
 add_library(${PROJECT_NAME} ${SCO_SOURCE_FILES})
 
-set (SCO_LINK_LIBS ${catkin_LIBRARIES})
+set (SCO_LINK_LIBS ${catkin_LIBRARIES} ${CMAKE_DL_LIBS})
 if (GUROBI_FOUND)
   list(APPEND SCO_LINK_LIBS ${GUROBI_LIBRARIES})
 endif()
 if (HAVE_BPMPD)
   list(APPEND SCO_LINK_LIBS ${BPMPD_LIBRARY})
 endif()
+if (osqp_FOUND)
+  target_link_libraries(${PROJECT_NAME} PRIVATE osqp::osqpstatic)
+endif()
+if (qpOASES_FOUND)
+  target_link_libraries(${PROJECT_NAME} PRIVATE ${qpOASES_LIBRARIES})
+endif()
 
-target_link_libraries(${PROJECT_NAME} ${SCO_LINK_LIBS})
+target_link_libraries(${PROJECT_NAME} PRIVATE ${JSONCPP_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} PUBLIC ${SCO_LINK_LIBS})
 
 # Mark executables and/or libraries for installation
 install(
@@ -98,8 +121,12 @@ if (CATKIN_ENABLE_TESTING)
     test/unit.cpp
     test/small-problems-unit.cpp
     test/solver-interface-unit.cpp
+    test/solver-utils-unit.cpp
   )
 
   catkin_add_gtest(${PROJECT_NAME}-test ${SCO_TEST_SOURCE})
   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+  if (osqp_FOUND)
+    target_link_libraries(${PROJECT_NAME}-test osqp::osqpstatic)
+  endif()
 endif()

trajopt_sco/cmake/Modules/FindqpOASES.cmake

@@ -0,0 +1,51 @@
+#.rst:
+# FindqpOASES
+# -----------
+#
+# Try to find the qpOASES library.
+# Once done this will define the following variables::
+#
+#  qpOASES_FOUND         - System has qpOASES
+#  qpOASES_INCLUDE_DIRS  - qpOASES include directory
+#  qpOASES_LIBRARIES     - qpOASES libraries
+#
+# qpOASES does not have an "install" step, and the includes are in the source
+# tree, while the libraries are in the build tree.
+# Therefore the environment and cmake variables `qpOASES_SOURCE_DIR` and
+# `qpOASES_BINARY_DIR` will be used to locate the includes and libraries.
+
+#=============================================================================
+# Copyright 2014 iCub Facility, Istituto Italiano di Tecnologia
+#   Authors: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
+#
+# Distributed under the OSI-approved BSD License (the "License");
+# see accompanying file Copyright.txt for details.
+#
+# This software is distributed WITHOUT ANY WARRANTY; without even the
+# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+# See the License for more information.
+#=============================================================================
+# (To distribute this file outside of YCM, substitute the full
+#  License text for the above reference.)
+
+
+include(FindPackageHandleStandardArgs)
+
+find_path(qpOASES_INCLUDEDIR
+          NAMES qpOASES.hpp
+          HINTS "${qpOASES_SOURCE_DIR}"
+                ENV qpOASES_SOURCE_DIR
+          PATH_SUFFIXES include)
+find_library(qpOASES_LIB
+             NAMES qpOASES
+             HINTS "${qpOASES_BINARY_DIR}"
+                   ENV qpOASES_BINARY_DIR
+             PATH_SUFFIXES lib
+                           libs)
+
+set(qpOASES_INCLUDE_DIRS ${qpOASES_INCLUDEDIR})
+set(qpOASES_LIBRARIES ${qpOASES_LIB})
+
+find_package_handle_standard_args(qpOASES DEFAULT_MSG qpOASES_LIBRARIES
+                                                      qpOASES_INCLUDE_DIRS)
+set(qpOASES_FOUND ${QPOASES_FOUND})

trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp

@@ -1,4 +1,4 @@
-
+#pragma once
 #include <trajopt_sco/solver_interface.hpp>
 #include <trajopt_utils/macros.h>
 
@@ -22,19 +22,19 @@ class BPMPDModel : public Model
   virtual ~BPMPDModel();
 
   Var addVar(const std::string& name);
-  Cnt addEqCnt(const AffExpr&, const std::string& name);
-  Cnt addIneqCnt(const AffExpr&, const std::string& name);
-  Cnt addIneqCnt(const QuadExpr&, const std::string& name);
-  void removeVars(const VarVector& vars);
-  void removeCnts(const CntVector& cnts);
+  Cnt addEqCnt(const AffExpr&, const std::string& name) override;
+  Cnt addIneqCnt(const AffExpr&, const std::string& name) override;
+  Cnt addIneqCnt(const QuadExpr&, const std::string& name) override;
+  void removeVars(const VarVector& vars) override;
+  void removeCnts(const CntVector& cnts) override;
 
-  void update();
-  void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper);
-  DblVec getVarValues(const VarVector& vars) const;
-  virtual CvxOptStatus optimize();
-  virtual void setObjective(const AffExpr&);
-  virtual void setObjective(const QuadExpr&);
-  virtual void writeToFile(const std::string& fname);
-  virtual VarVector getVars() const;
+  void update() override;
+  void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override;
+  DblVec getVarValues(const VarVector& vars) const override;
+  virtual CvxOptStatus optimize() override;
+  virtual void setObjective(const AffExpr&) override;
+  virtual void setObjective(const QuadExpr&) override;
+  virtual void writeToFile(const std::string& fname) override;
+  virtual VarVector getVars() const override;
 };
 }

trajopt_sco/include/trajopt_sco/bpmpd_io.hpp

@@ -1,3 +1,4 @@
+#pragma once
 #include <string>
 #include <vector>
 #include <assert.h>

trajopt_sco/include/trajopt_sco/expr_ops.hpp

@@ -1,5 +1,4 @@
 #pragma once
-
 #include <trajopt_sco/sco_fwd.hpp>
 #include <trajopt_sco/solver_interface.hpp>
 
@@ -161,5 +160,4 @@ QuadExpr exprSquare(const Var&);
 QuadExpr exprSquare(const AffExpr&);
 
 AffExpr cleanupAff(const AffExpr&);
-QuadExpr cleanupQuad(const QuadExpr&);  // warning: might make it non-psd!
 }

trajopt_sco/include/trajopt_sco/expr_vec_ops.hpp

@@ -1,3 +1,4 @@
+#pragma once
 #include <Eigen/Core>
 #include <trajopt_sco/solver_interface.hpp>
 

trajopt_sco/include/trajopt_sco/gurobi_interface.hpp

@@ -1,3 +1,4 @@
+#pragma once
 #include <trajopt_sco/solver_interface.hpp>
 
 /**
@@ -22,30 +23,30 @@ class GurobiModel : public Model
 
   GurobiModel();
 
-  Var addVar(const std::string& name);
-  Var addVar(const std::string& name, double lower, double upper);
+  Var addVar(const std::string& name) override;
+  Var addVar(const std::string& name, double lower, double upper) override;
 
-  Cnt addEqCnt(const AffExpr&, const std::string& name);
-  Cnt addIneqCnt(const AffExpr&, const std::string& name);
-  Cnt addIneqCnt(const QuadExpr&, const std::string& name);
+  Cnt addEqCnt(const AffExpr&, const std::string& name) override;
+  Cnt addIneqCnt(const AffExpr&, const std::string& name) override;
+  Cnt addIneqCnt(const QuadExpr&, const std::string& name) override;
 
-  void removeVars(const VarVector&);
-  void removeCnts(const CntVector&);
+  void removeVars(const VarVector&) override;
+  void removeCnts(const CntVector&) override;
 
-  void update();
-  void setVarBounds(const VarVector&, const DblVec& lower, const DblVec& upper);
-  DblVec getVarValues(const VarVector&) const;
+  void update() override;
+  void setVarBounds(const VarVector&, const DblVec& lower, const DblVec& upper) override;
+  DblVec getVarValues(const VarVector&) const override;
 
-  CvxOptStatus optimize();
+  CvxOptStatus optimize() override;
   /** Don't use this function, because it adds constraints that aren't tracked
    */
   CvxOptStatus optimizeFeasRelax();
 
-  void setObjective(const AffExpr&);
-  void setObjective(const QuadExpr&);
-  void writeToFile(const std::string& fname);
+  void setObjective(const AffExpr&) override;
+  void setObjective(const QuadExpr&) override;
+  void writeToFile(const std::string& fname) override;
 
-  VarVector getVars() const;
+  VarVector getVars() const override;
 
   ~GurobiModel();
 };

trajopt_sco/include/trajopt_sco/modeling.hpp

@@ -163,7 +163,7 @@ Non-convex optimization problem
 class OptProb
 {
 public:
-  OptProb();
+  OptProb(ModelType convex_solver = ModelType::AUTO_SOLVER);
   /** create variables with bounds [-INFINITY, INFINITY]  */
   VarVector createVariables(const std::vector<std::string>& names);
   /** create variables with bounds [lb[i], ub[i] */