diff --git a/README.md b/README.md index 2b5934ba..6a76f6eb 100644 --- a/README.md +++ b/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 ``` diff --git a/trajopt/include/trajopt/problem_description.hpp b/trajopt/include/trajopt/problem_description.hpp index 86f970e3..c93277a4 100644 --- a/trajopt/include/trajopt/problem_description.hpp +++ b/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 }; /** diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp index f6fd479a..5b07e514 100644 --- a/trajopt/src/problem_description.cpp +++ b/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(); diff --git a/trajopt_sco/CMakeLists.txt b/trajopt_sco/CMakeLists.txt index 12a4e968..4df78d74 100644 --- a/trajopt_sco/CMakeLists.txt +++ b/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() diff --git a/trajopt_sco/cmake/Modules/FindqpOASES.cmake b/trajopt_sco/cmake/Modules/FindqpOASES.cmake new file mode 100644 index 00000000..457824fa --- /dev/null +++ b/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 +# +# 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}) diff --git a/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp b/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp index 3906bae5..1f98b9fa 100644 --- a/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp @@ -1,4 +1,4 @@ - +#pragma once #include #include @@ -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; }; } diff --git a/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp b/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp index 1f22d799..68b5e390 100644 --- a/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp +++ b/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp @@ -1,3 +1,4 @@ +#pragma once #include #include #include diff --git a/trajopt_sco/include/trajopt_sco/expr_ops.hpp b/trajopt_sco/include/trajopt_sco/expr_ops.hpp index 66cdc332..ac4facc9 100644 --- a/trajopt_sco/include/trajopt_sco/expr_ops.hpp +++ b/trajopt_sco/include/trajopt_sco/expr_ops.hpp @@ -1,5 +1,4 @@ #pragma once - #include #include @@ -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! } diff --git a/trajopt_sco/include/trajopt_sco/expr_vec_ops.hpp b/trajopt_sco/include/trajopt_sco/expr_vec_ops.hpp index 93ade82a..bbd48d27 100644 --- a/trajopt_sco/include/trajopt_sco/expr_vec_ops.hpp +++ b/trajopt_sco/include/trajopt_sco/expr_vec_ops.hpp @@ -1,3 +1,4 @@ +#pragma once #include #include diff --git a/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp b/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp index ce4b7912..39809169 100644 --- a/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp @@ -1,3 +1,4 @@ +#pragma once #include /** @@ -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(); }; diff --git a/trajopt_sco/include/trajopt_sco/modeling.hpp b/trajopt_sco/include/trajopt_sco/modeling.hpp index 7a00bfa2..2daebd5b 100644 --- a/trajopt_sco/include/trajopt_sco/modeling.hpp +++ b/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& names); /** create variables with bounds [lb[i], ub[i] */ diff --git a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp new file mode 100644 index 00000000..c9799f7c --- /dev/null +++ b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp @@ -0,0 +1,81 @@ +#pragma once +#include +#include + +#include +#include + +namespace sco +{ +/** + * OSQPModel uses the BSD solver OSQP to solve a linearly constrained QP. + * OSQP solves a problem in the form: + * ``` + * min 1/2*x'Px + q'x + * s.t. l <= Ax <= u + * ``` + * + * More informations about the solver are available at: + * https://osqp.org/docs/ + */ +class OSQPModel : public Model +{ + OSQPSettings osqp_settings_; /**< OSQP Settings */ + /** OSQPData. Some fields here (`osp_data_.A` and `osp_data_.P`) are + * automatically allocated by OSQP, but deallocated by us. */ + OSQPData osqp_data_; + /** OSQP Workspace. Memory here is managed by OSQP */ + OSQPWorkspace* osqp_workspace_; + + /** Updates OSQP quadratic cost matrix from QuadExpr expression. + * Transforms QuadExpr objective_ into the OSQP CSC matrix P_ */ + void updateObjective(); + + /** Updates qpOASES constraints from AffExpr expression. + * Transforms AffExpr cntr_exprs_ and box bounds lbs_ and ubs_ into the + * OSQP CSC matrix A_, and vectors lbA_ and ubA_ */ + void updateConstraints(); + + /** Creates or updates the solver and its workspace */ + void createOrUpdateSolver(); + + VarVector vars_; /**< model variables */ + CntVector cnts_; /**< model's constraints sizes */ + DblVec lbs_, ubs_; /**< variables bounds */ + AffExprVector cnt_exprs_; /**< constraints expressions */ + ConstraintTypeVector cnt_types_; /**< constraints types */ + DblVec solution_; /**< optimizizer's solution for current model */ + + std::vector P_row_indices_; /**< row indices for P, CSC format */ + std::vector P_column_pointers_; /**< column pointers for P, CSC format */ + DblVec P_csc_data_; /**< P values in CSC format */ + Eigen::VectorXd q_; /**< linear part of the objective */ + + std::vector A_row_indices_; /**< row indices for constraint matrix, CSC format */ + std::vector A_column_pointers_; /**< column pointers for constraint matrix, CSC format */ + DblVec A_csc_data_; /**< constraint matrix values in CSC format */ + DblVec l_, u_; /**< linear constraints upper and lower limits */ + + QuadExpr objective_; /**< objective QuadExpr expression */ + + public: + OSQPModel(); + virtual ~OSQPModel(); + + Var addVar(const std::string& name) override; + 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() 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; +}; +} diff --git a/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp b/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp new file mode 100644 index 00000000..a7e098a8 --- /dev/null +++ b/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp @@ -0,0 +1,92 @@ +#pragma once +#include +#include + +#include +#include + +namespace sco +{ +/** + * qpOASESModel uses the LGPL solver qpOASES to solve a linearly constrained QP. + * qpOASES solves a problem in the form: + * ``` + * min 1/2*x'Hx + x'g + * s.t. lb <= x <= ub + * lbA <= Ax <= ubA + * ``` + * + * More informations about the solver are available at: + * https://projects.coin-or.org/qpOASES + */ +class qpOASESModel : public Model +{ + /** pointer to a qpOASES Sequential Quadratic Problem*/ + std::shared_ptr qpoases_problem_; + qpOASES::Options qpoases_options_; /**< qpOASES solver options */ + + qpOASES::SymSparseMat H_; /**< Quadratic cost matrix */ + qpOASES::SparseMatrix A_; /**< Constraints matrix */ + + /** Updates qpOASES Hessian matrix from QuadExpr expression. + * Transforms QuadExpr objective_ into the qpOASES sparse matrix H_*/ + void updateObjective(); + + /** Updates qpOASES constraints from AffExpr expression. + * Transforms AffExpr cntr_exprs_ into the qpOASES sparse matrix A_, and + * vectors lbA_ and ubA_ */ + void updateConstraints(); + + /** + * Instantiates a new qpOASES problem if it has not been instantiated yet + * or if the size of the problem has changed. + * + * @returns true if a new qpOASES problem has been instantiated + */ + bool updateSolver(); + + /** + * Instantiates a new qpOASES problem + */ + void createSolver(); + + VarVector vars_; /**< model variables */ + CntVector cnts_; /**< model's constraints sizes */ + DblVec lb_, ub_; /**< variables bounds */ + AffExprVector cnt_exprs_; /**< constraints expressions */ + ConstraintTypeVector cnt_types_; /**< constraints types */ + DblVec solution_; /**< optimizizer's solution for current model */ + + IntVec H_row_indices_; /**< row indices for Hessian, CSC format */ + IntVec H_column_pointers_; /**< column pointers for Hessian, CSC format */ + DblVec H_csc_data_; /**< Hessian values in CSC format */ + Eigen::VectorXd g_; /**< gradient of the optimization problem */ + + IntVec A_row_indices_; /**< row indices for constraint matrix, CSC format */ + IntVec A_column_pointers_; /**< column pointers for constraint matrix, CSC format */ + DblVec A_csc_data_; /**< constraint matrix values in CSC format */ + DblVec lbA_, ubA_; /**< linear constraints upper and lower limits */ + + QuadExpr objective_; /**< objective QuadExpr expression */ + + public: + qpOASESModel(); + virtual ~qpOASESModel(); + + Var addVar(const std::string& name) override; + 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() 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; +}; +} diff --git a/trajopt_sco/include/trajopt_sco/solver_interface.hpp b/trajopt_sco/include/trajopt_sco/solver_interface.hpp index 10770db6..c2f305f8 100644 --- a/trajopt_sco/include/trajopt_sco/solver_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/solver_interface.hpp @@ -1,6 +1,7 @@ #pragma once #include #include +#include #include #include #include @@ -17,12 +18,12 @@ backends. namespace sco { - enum ConstraintType { EQ, INEQ }; + typedef std::vector ConstraintTypeVector; enum CvxOptStatus @@ -148,5 +149,53 @@ std::ostream& operator<<(std::ostream&, const Cnt&); std::ostream& operator<<(std::ostream&, const AffExpr&); std::ostream& operator<<(std::ostream&, const QuadExpr&); -ModelPtr createModel(); +class ModelType +{ +public: + enum Value { + GUROBI, + BPMPD, + OSQP, + QPOASES, + AUTO_SOLVER + }; + + static const std::vector MODEL_NAMES_; + + ModelType(); + ModelType(const ModelType::Value& v); + ModelType(const int& v); + ModelType(const std::string& s); + operator int() const; + bool operator==(const ModelType::Value& a) const; + bool operator==(const ModelType& a) const; + bool operator!=(const ModelType& a) const; + void fromJson(const Json::Value& v); + friend std::ostream& operator<<(std::ostream& os, const ModelType& cs); +private: + + Value value_; +}; + +std::vector availableSolvers(); + +std::ostream& operator<<(std::ostream& os, const ModelType& cs); + +ModelPtr createModel(ModelType model_type = ModelType::AUTO_SOLVER); + +IntVec vars2inds(const VarVector& vars); + +IntVec cnts2inds(const CntVector& cnts); + +/** + * @brief simplify2 gets as input a list of indices, corresponding to non-zero + * values in vals, checks that all indexed values are actually non-zero, + * and if they are not, removes them from vals and inds, so that + * inds_out.size() <= inds.size(). Also, it will compact vals so that + * vals_out.size() == inds_out.size() + * + * @param[in,out] inds indices of non-vero variables in vals + * @param[in,out] val values + */ +void simplify2(IntVec& inds, DblVec& vals); } diff --git a/trajopt_sco/include/trajopt_sco/solver_utils.hpp b/trajopt_sco/include/trajopt_sco/solver_utils.hpp new file mode 100644 index 00000000..8d61e24c --- /dev/null +++ b/trajopt_sco/include/trajopt_sco/solver_utils.hpp @@ -0,0 +1,150 @@ +#pragma once +#include +#include +#include +#include + +namespace sco +{ +/** + * @brief transform an `AffExpr` to an `Eigen::SparseVector` + * + * @param [in] expr an `AffExpr` expression + * @param [out] sparse_vector vector where to store results. It will be resized + * to the correct size. + * @param [in] n_vars the number of variables in expr. It is usually equal + * to `expr.size()`, but it might be larger. + */ +void exprToEigen(const AffExpr& expr, Eigen::SparseVector& sparse_vector, const int& n_vars); + +/** + * @brief transform a `QuadExpr` to an `Eigen::SparseMatrix` plus + * `Eigen::VectorXd` + * + * @param [in] expr a `QuadExpr` expression + * @param [out] sparse_matrix matrix where to store results. It will be resized + * to the correct size. + * @param [out] vector vector where to store the affine part of the + * `QuadExpr`. It will be resized to the correct size. + * @param [in] n_vars the number of variables in expr. It is usually equal + * to `expr.size()`, but it might be larger. + * @param [in] matrix_is_halved if `true`, sparse_matrix will be premultiplied + * by the coefficient `2`. + * @param [in] force_diagonal if true, we will forcibly add elements to the + * diagonal of the sparse matrix, adding `0.` if needed + */ +void exprToEigen(const QuadExpr& expr, + Eigen::SparseMatrix& sparse_matrix, + Eigen::VectorXd& vector, + const int& n_vars, + const bool& matrix_is_halved = false, + const bool& force_diagonal = false); + +/** + * @brief transform a vector of `AffExpr` to an `Eigen::SparseMatrix` plus an + * `Eigen::VectorXd`. + * Notice that the underlying transformation is so that the the affine + * expressions of the type `ax + b` will be stacked in matrix form into + * `Ax + b` and then transformed into the expression `Ax <= -b` so that + * `vector[i] = -expr_vec[i].constant` + * @param [in] expr_vec a an `AffExprVector` + * @param [out] sparse_matrix matrix where to store results. It will be resized + * to the correct size. + * @param [out] vector vector where to store all the constants of each `AffExpr`, + * ordered by their appearance order in `expr_vec`. + * It will be resized to the correct size. + * @param [in] n_vars the larger number of variables in expr. + * It is usually the same for each `expr` in `expr_vec`, + * and equal to `expr.size()`, but it might be larger. + */ +void exprToEigen(const AffExprVector& expr_vec, + Eigen::SparseMatrix& sparse_matrix, + Eigen::VectorXd& vector, + const int& n_vars = -1); +/** + * @brief Converts triplets to an `Eigen::SparseMatrix`. + * @param [in] rows_i a vector of row indices + * @param [in] cols_j a vector of columns indices + * @param [in] values_ij a vector of values, so that: + * `M[rows_i[k], cols_j[k]] = values_ij[k]` + * @param [in,out] sparse_matrix must be of the right size, as we should not + * be guessing the right size of sparse_matrix from + * a sparse triplet representation. + */ +void tripletsToEigen(const IntVec& rows_i, + const IntVec& cols_j, + const DblVec& values_ij, + Eigen::SparseMatrix& sparse_matrix); + +/** + * @brief Converts an `Eigen::SparseMatrix` into triplets format + * @param [in] sparse_matrix an `Eigen::SparseMatrix` + * @param [out] rows_i a vector of row indices + * @param [out] cols_j a vector of columns indices + * @param [out] values_ij a vector of values, so that: + * `M[rows_i[k], cols_j[k]] = values_ij[k]` + */ +void eigenToTriplets(const Eigen::SparseMatrix& sparse_matrix, + IntVec& rows_i, + IntVec& cols_j, + DblVec& values_ij); + +/** + * @brief converts a sparse matrix into compressed + * sparse column representation (CSC). + * + * @param [out] row_indices row indices for a CSC matrix + * @param [out] column_pointers column pointer for a CSC matrix + * @param [out] values pointer to non-zero elements in CSC representation + * @param [in,out] sparse_matrix input matrix: will be compressed + */ +template +void eigenToCSC(Eigen::SparseMatrix& sparse_matrix, + std::vector& row_indices, + std::vector& column_pointers, + DblVec& values) +{ + Eigen::SparseMatrix sm_t; + auto sm_ref = std::ref(sparse_matrix); + + if (eigenUpLoType > 0) + { + sm_t = sparse_matrix.triangularView(); + sm_ref = std::ref(sm_t); + } + sm_ref.get().makeCompressed(); + + if (sizeof(T) != sizeof(int)) + { + IntVec row_indices_int, column_pointers_int; + auto si_p = sm_ref.get().innerIndexPtr(); + row_indices_int.assign(si_p, si_p + sm_ref.get().nonZeros()); + row_indices.clear(); + row_indices.reserve(row_indices_int.size()); + for (const auto& v : row_indices_int) + row_indices.push_back(static_cast(v)); + + si_p = sm_ref.get().outerIndexPtr(); + column_pointers_int.assign(si_p, si_p + sm_ref.get().outerSize()); + column_pointers.clear(); + column_pointers.reserve(column_pointers_int.size()); + for (const auto& v : column_pointers_int) + column_pointers.push_back(static_cast(v)); + } + else + { + auto si_p = sm_ref.get().innerIndexPtr(); + row_indices.assign(si_p, si_p + sm_ref.get().nonZeros()); + + si_p = sm_ref.get().outerIndexPtr(); + column_pointers.assign(si_p, si_p + sm_ref.get().outerSize()); + } + + // while Eigen does not enforce this, CSC format requires that column + // pointers ends with the number of non-zero elements + column_pointers.push_back(sm_ref.get().nonZeros()); + + auto csc_v = sm_ref.get().valuePtr(); + values.assign(csc_v, csc_v + sm_ref.get().nonZeros()); +} +} diff --git a/trajopt_sco/src/bpmpd_interface.cpp b/trajopt_sco/src/bpmpd_interface.cpp index f85179bc..5ac9dd1a 100644 --- a/trajopt_sco/src/bpmpd_interface.cpp +++ b/trajopt_sco/src/bpmpd_interface.cpp @@ -137,8 +137,6 @@ A sample driver solving the above problem is included as well as the logfile it generates. **/ -double BIG = 1e+30; - // extern "C" { // extern void bpmpd(int *, int *, int *, int *, int *, int *, int *, // double *, int *, int *, double *, double *, double *, double *, @@ -148,9 +146,7 @@ double BIG = 1e+30; namespace sco { -extern void simplify2(IntVec& inds, DblVec& vals); -extern IntVec vars2inds(const VarVector& vars); -extern IntVec cnts2inds(const CntVector& cnts); +double BPMPD_BIG = 1e+30; ModelPtr createBPMPDModel() { @@ -234,8 +230,8 @@ BPMPDModel::~BPMPDModel() Var BPMPDModel::addVar(const std::string& name) { m_vars.push_back(new VarRep(m_vars.size(), name, this)); - m_lbs.push_back(-BIG); - m_ubs.push_back(BIG); + m_lbs.push_back(-BPMPD_BIG); + m_ubs.push_back(BPMPD_BIG); return m_vars.back(); } Cnt BPMPDModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) @@ -359,8 +355,8 @@ CvxOptStatus BPMPDModel::optimize() DBG(m_ubs); for (int iVar = 0; iVar < n; ++iVar) { - lbound[iVar] = fmax(m_lbs[iVar], -BIG); - ubound[iVar] = fmin(m_ubs[iVar], BIG); + lbound[iVar] = fmax(m_lbs[iVar], -BPMPD_BIG); + ubound[iVar] = fmin(m_ubs[iVar], BPMPD_BIG); } std::vector var2cntinds(n); @@ -377,7 +373,7 @@ CvxOptStatus BPMPDModel::optimize() var2cntvals[inds[i]].push_back(aff.coeffs[i]); // xxx maybe repeated/ } - lbound[n + iCnt] = (m_cntTypes[iCnt] == INEQ) ? -BIG : 0; + lbound[n + iCnt] = (m_cntTypes[iCnt] == INEQ) ? -BPMPD_BIG : 0; ubound[n + iCnt] = 0; rhs[iCnt] = -aff.constant; } diff --git a/trajopt_sco/src/expr_ops.cpp b/trajopt_sco/src/expr_ops.cpp index c07353c7..466130a5 100644 --- a/trajopt_sco/src/expr_ops.cpp +++ b/trajopt_sco/src/expr_ops.cpp @@ -95,21 +95,5 @@ AffExpr cleanupAff(const AffExpr& a) return out; } -QuadExpr cleanupQuad(const QuadExpr& q) -{ - QuadExpr out; - out.affexpr = cleanupAff(q.affexpr); - for (size_t i = 0; i < q.size(); ++i) - { - if (fabs(q.coeffs[i]) > 1e-8) - { - out.coeffs.push_back(q.coeffs[i]); - out.vars1.push_back(q.vars1[i]); - out.vars2.push_back(q.vars2[i]); - } - } - return out; -} - /////////////////////////////////////////////////////////////// } // namespace sco diff --git a/trajopt_sco/src/gurobi_interface.cpp b/trajopt_sco/src/gurobi_interface.cpp index 71b8f3c0..36feea59 100644 --- a/trajopt_sco/src/gurobi_interface.cpp +++ b/trajopt_sco/src/gurobi_interface.cpp @@ -13,10 +13,6 @@ extern "C" { namespace sco { -extern void simplify2(IntVec& inds, DblVec& vals); -extern IntVec vars2inds(const VarVector& vars); -extern IntVec cnts2inds(const CntVector& cnts); - GRBenv* gEnv; #if 0 diff --git a/trajopt_sco/src/modeling.cpp b/trajopt_sco/src/modeling.cpp index 4f566fcd..67057308 100644 --- a/trajopt_sco/src/modeling.cpp +++ b/trajopt_sco/src/modeling.cpp @@ -21,6 +21,7 @@ void ConvexObjective::addHinge(const AffExpr& affexpr, double coeff) AffExpr hinge_cost = exprMult(AffExpr(hinge), coeff); exprInc(quad_, hinge_cost); } + void ConvexObjective::addAbs(const AffExpr& affexpr, double coeff) { Var neg = model_->addVar("neg", 0, INFINITY); @@ -39,21 +40,25 @@ void ConvexObjective::addAbs(const AffExpr& affexpr, double coeff) affeq.coeffs.push_back(-1); eqs_.push_back(affeq); } + void ConvexObjective::addHinges(const AffExprVector& ev) { for (size_t i = 0; i < ev.size(); ++i) addHinge(ev[i], 1); } + void ConvexObjective::addL1Norm(const AffExprVector& ev) { for (size_t i = 0; i < ev.size(); ++i) addAbs(ev[i], 1); } + void ConvexObjective::addL2Norm(const AffExprVector& ev) { for (size_t i = 0; i < ev.size(); ++i) exprInc(quad_, exprSquare(ev[i])); } + void ConvexObjective::addMax(const AffExprVector& ev) { Var m = model_->addVar("max", -INFINITY, INFINITY); @@ -63,6 +68,7 @@ void ConvexObjective::addMax(const AffExprVector& ev) exprDec(ineqs_.back(), m); } } + void ConvexObjective::addConstraintsToModel() { cnts_.reserve(eqs_.size() + ineqs_.size()); @@ -75,13 +81,13 @@ void ConvexObjective::addConstraintsToModel() cnts_.push_back(model_->addIneqCnt(aff, "")); } } + void ConvexObjective::removeFromModel() { model_->removeCnts(cnts_); model_->removeVars(vars_); model_ = NULL; } -double ConvexObjective::value(const DblVec& x) { return quad_.value(x); } ConvexObjective::~ConvexObjective() { if (inModel()) @@ -102,6 +108,7 @@ void ConvexConstraints::addConstraintsToModel() cnts_.push_back(model_->addIneqCnt(aff, "")); } } + void ConvexConstraints::removeFromModel() { model_->removeCnts(cnts_); @@ -125,6 +132,7 @@ ConvexConstraints::~ConvexConstraints() removeFromModel(); } +double ConvexObjective::value(const DblVec& x) { return quad_.value(x); } DblVec Constraint::violations(const DblVec& x) { DblVec val = value(x); @@ -143,13 +151,16 @@ DblVec Constraint::violations(const DblVec& x) return out; } + double Constraint::violation(const DblVec& x) { return vecSum(violations(x)); } -OptProb::OptProb() : model_(createModel()) {} +OptProb::OptProb(ModelType convex_solver) : model_(createModel(convex_solver)) {} + VarVector OptProb::createVariables(const std::vector& var_names) { return createVariables(var_names, DblVec(var_names.size(), -INFINITY), DblVec(var_names.size(), INFINITY)); } + VarVector OptProb::createVariables(const std::vector& var_names, const DblVec& lb, const DblVec& ub) { size_t n_add = var_names.size(), n_cur = vars_.size(); @@ -167,16 +178,19 @@ VarVector OptProb::createVariables(const std::vector& var_names, co model_->update(); return VarVector(vars_.end() - n_add, vars_.end()); } + void OptProb::setLowerBounds(const DblVec& lb) { assert(lb.size() == vars_.size()); lower_bounds_ = lb; } + void OptProb::setUpperBounds(const DblVec& ub) { assert(ub.size() == vars_.size()); upper_bounds_ = ub; } + void OptProb::setLowerBounds(const DblVec& lb, const VarVector& vars) { setVec(lower_bounds_, vars, lb); } void OptProb::setUpperBounds(const DblVec& ub, const VarVector &vars) { setVec(upper_bounds_, vars, ub); } void OptProb::addCost(CostPtr cost) { costs_.push_back(cost); } @@ -187,16 +201,19 @@ void OptProb::addConstraint(ConstraintPtr cnt) else addIneqConstraint(cnt); } + void OptProb::addEqConstraint(ConstraintPtr cnt) { assert(cnt->type() == EQ); eqcnts_.push_back(cnt); } + void OptProb::addIneqConstraint(ConstraintPtr cnt) { assert(cnt->type() == INEQ); ineqcnts_.push_back(cnt); } + std::vector OptProb::getConstraints() const { std::vector out; @@ -205,6 +222,7 @@ std::vector OptProb::getConstraints() const out.insert(out.end(), ineqcnts_.begin(), ineqcnts_.end()); return out; } + void OptProb::addLinearConstraint(const AffExpr& expr, ConstraintType type) { if (type == EQ) @@ -221,6 +239,7 @@ DblVec OptProb::getCentralFeasiblePoint(const DblVec& x) center[i] = (lower_bounds_[i] + upper_bounds_[i]) / 2; return getClosestFeasiblePoint(center); } + DblVec OptProb::getClosestFeasiblePoint(const DblVec& x) { LOG_DEBUG("getClosestFeasiblePoint"); diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp new file mode 100644 index 00000000..7b708441 --- /dev/null +++ b/trajopt_sco/src/osqp_interface.cpp @@ -0,0 +1,266 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace sco +{ +double OSQP_INFINITY = std::numeric_limits::infinity(); + +ModelPtr createOSQPModel() +{ + ModelPtr out(new OSQPModel()); + return out; +} + +OSQPModel::OSQPModel() +{ + // Define Solver settings as default + // see https://osqp.org/docs/interfaces/solver_settings.html#solver-settings + osqp_set_default_settings(&osqp_settings_); + // tuning parameters to be less accurate, but add a polishing step + osqp_settings_.eps_abs = 1e-4; + osqp_settings_.eps_rel = 1e-6; + osqp_settings_.max_iter = 8192; + osqp_settings_.polish = 1; + + // Initialize data + osqp_data_.A = nullptr; + osqp_data_.P = nullptr; + osqp_workspace_ = nullptr; +} + +OSQPModel::~OSQPModel() +{ + // Cleanup + if (osqp_workspace_ != nullptr) + osqp_cleanup(osqp_workspace_); + if (osqp_data_.A != nullptr) + c_free(osqp_data_.A); + if (osqp_data_.P != nullptr) + c_free(osqp_data_.P); +} + +Var OSQPModel::addVar(const std::string& name) +{ + vars_.push_back(new VarRep(vars_.size(), name, this)); + lbs_.push_back(-OSQP_INFINITY); + ubs_.push_back(OSQP_INFINITY); + return vars_.back(); +} + +Cnt OSQPModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) +{ + cnts_.push_back(new CntRep(cnts_.size(), this)); + cnt_exprs_.push_back(expr); + cnt_types_.push_back(EQ); + return cnts_.back(); +} + +Cnt OSQPModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) +{ + cnts_.push_back(new CntRep(cnts_.size(), this)); + cnt_exprs_.push_back(expr); + cnt_types_.push_back(INEQ); + return cnts_.back(); +} + +Cnt OSQPModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/) +{ + throw std::runtime_error("NOT IMPLEMENTED"); + return Cnt(); +} + +void OSQPModel::removeVars(const VarVector& vars) +{ + IntVec inds = vars2inds(vars); + for (unsigned i = 0; i < vars.size(); ++i) + vars[i].var_rep->removed = true; +} + +void OSQPModel::removeCnts(const CntVector& cnts) +{ + IntVec inds = cnts2inds(cnts); + for (unsigned i = 0; i < cnts.size(); ++i) + cnts[i].cnt_rep->removed = true; +} + +void OSQPModel::updateObjective() +{ + const size_t n = vars_.size(); + osqp_data_.n = n; + + Eigen::SparseMatrix sm; + exprToEigen(objective_, sm, q_, n, true); + eigenToCSC(sm, P_row_indices_, P_column_pointers_, P_csc_data_); + + if (osqp_data_.P != nullptr) + c_free(osqp_data_.P); + osqp_data_.P = csc_matrix(osqp_data_.n, + osqp_data_.n, + P_csc_data_.size(), + P_csc_data_.data(), + P_row_indices_.data(), + P_column_pointers_.data()); + + osqp_data_.q = q_.data(); +} + +void OSQPModel::updateConstraints() +{ + const size_t n = vars_.size(); + const size_t m = cnts_.size(); + osqp_data_.m = m + n; + + Eigen::SparseMatrix sm; + Eigen::VectorXd v; + exprToEigen(cnt_exprs_, sm, v, n); + Eigen::SparseMatrix sm_e(m + n, n); + Eigen::SparseMatrix sm_e2 = sm; + sm.conservativeResize(m + n, Eigen::NoChange_t(n)); + + l_.clear(); + l_.resize(m + n, -OSQP_INFINITY); + u_.clear(); + u_.resize(m + n, OSQP_INFINITY); + + for (int i_cnt = 0; i_cnt < m; ++i_cnt) + { + l_[i_cnt] = (cnt_types_[i_cnt] == INEQ) ? -OSQP_INFINITY : v[i_cnt]; + u_[i_cnt] = v[i_cnt]; + } + + for (int i_bnd = 0; i_bnd < n; ++i_bnd) + { + l_[i_bnd + m] = fmax(lbs_[i_bnd], -OSQP_INFINITY); + u_[i_bnd + m] = fmin(ubs_[i_bnd], OSQP_INFINITY); + sm.insert(i_bnd + m, i_bnd) = 1.; + } + + eigenToCSC(sm, A_row_indices_, A_column_pointers_, A_csc_data_); + + if (osqp_data_.A != nullptr) + c_free(osqp_data_.A); + osqp_data_.A = csc_matrix(osqp_data_.m, + osqp_data_.n, + A_csc_data_.size(), + A_csc_data_.data(), + A_row_indices_.data(), + A_column_pointers_.data()); + + osqp_data_.l = l_.data(); + osqp_data_.u = u_.data(); +} + +void OSQPModel::createOrUpdateSolver() +{ + updateObjective(); + updateConstraints(); + + // TODO atm we are not updating the workspace, but recreating it each time. + // In the future, we will checking sparsity did not change and update instead + if (osqp_workspace_ != nullptr) + osqp_cleanup(osqp_workspace_); + // Setup workspace - this should be called only once + osqp_workspace_ = osqp_setup(&osqp_data_, &osqp_settings_); +} + +void OSQPModel::update() +{ + { + int inew = 0; + for (unsigned iold = 0; iold < vars_.size(); ++iold) + { + const Var& var = vars_[iold]; + if (!var.var_rep->removed) + { + vars_[inew] = var; + lbs_[inew] = lbs_[iold]; + ubs_[inew] = ubs_[iold]; + var.var_rep->index = inew; + ++inew; + } + else + delete var.var_rep; + } + vars_.resize(inew); + lbs_.resize(inew); + ubs_.resize(inew); + } + { + int inew = 0; + for (unsigned iold = 0; iold < cnts_.size(); ++iold) + { + const Cnt& cnt = cnts_[iold]; + if (!cnt.cnt_rep->removed) + { + cnts_[inew] = cnt; + cnt_exprs_[inew] = cnt_exprs_[iold]; + cnt_types_[inew] = cnt_types_[iold]; + cnt.cnt_rep->index = inew; + ++inew; + } + else + delete cnt.cnt_rep; + } + cnts_.resize(inew); + cnt_exprs_.resize(inew); + cnt_types_.resize(inew); + } +} + +void OSQPModel::setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) +{ + for (unsigned i = 0; i < vars.size(); ++i) + { + const int varind = vars[i].var_rep->index; + lbs_[varind] = lower[i]; + ubs_[varind] = upper[i]; + } +} +DblVec OSQPModel::getVarValues(const VarVector& vars) const +{ + DblVec out(vars.size()); + for (unsigned i = 0; i < vars.size(); ++i) + { + const int varind = vars[i].var_rep->index; + out[i] = solution_[varind]; + } + return out; +} + +CvxOptStatus OSQPModel::optimize() +{ + update(); + createOrUpdateSolver(); + + // Solve Problem + const int retcode = osqp_solve(osqp_workspace_); + + if (retcode == 0) + { + // opt += m_objective.affexpr.constant; + solution_ = DblVec(osqp_workspace_->solution->x, osqp_workspace_->solution->x + vars_.size()); + int status = osqp_workspace_->info->status_val; + if (status == OSQP_SOLVED || status == OSQP_SOLVED_INACCURATE) + return CVX_SOLVED; + else if (status == OSQP_PRIMAL_INFEASIBLE || status == OSQP_PRIMAL_INFEASIBLE_INACCURATE || + status == OSQP_DUAL_INFEASIBLE || status == OSQP_DUAL_INFEASIBLE_INACCURATE) + return CVX_INFEASIBLE; + } + return CVX_FAILED; +} +void OSQPModel::setObjective(const AffExpr& expr) { objective_.affexpr = expr; } +void OSQPModel::setObjective(const QuadExpr& expr) { objective_ = expr; } +void OSQPModel::writeToFile(const std::string& /*fname*/) +{ + return; // NOT IMPLEMENTED +} +VarVector OSQPModel::getVars() const { return vars_; } +} diff --git a/trajopt_sco/src/qpoases_interface.cpp b/trajopt_sco/src/qpoases_interface.cpp new file mode 100644 index 00000000..7435569b --- /dev/null +++ b/trajopt_sco/src/qpoases_interface.cpp @@ -0,0 +1,249 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace qpOASES; + +namespace sco +{ +double QPOASES_INFTY = qpOASES::INFTY; + +ModelPtr createqpOASESModel() +{ + ModelPtr out(new qpOASESModel()); + return out; +} + +qpOASESModel::qpOASESModel() +{ + // set to be fast. More details at: + // https://www.coin-or.org/qpOASES/doc/3.2/doxygen/classOptions.html + // https://projects.coin-or.org/qpOASES/browser/stable/3.2/src/Options.cpp#L191 + qpoases_options_.setToMPC(); + qpoases_options_.printLevel = qpOASES::PL_NONE; + // enable regularisation to deal with degenerate Hessians + qpoases_options_.enableRegularisation = qpOASES::BT_TRUE; + qpoases_options_.ensureConsistency(); +} + +qpOASESModel::~qpOASESModel() {} + +Var qpOASESModel::addVar(const std::string& name) +{ + vars_.push_back(new VarRep(vars_.size(), name, this)); + lb_.push_back(-QPOASES_INFTY); + ub_.push_back(QPOASES_INFTY); + return vars_.back(); +} + +Cnt qpOASESModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) +{ + cnts_.push_back(new CntRep(cnts_.size(), this)); + cnt_exprs_.push_back(expr); + cnt_types_.push_back(EQ); + return cnts_.back(); +} + +Cnt qpOASESModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) +{ + cnts_.push_back(new CntRep(cnts_.size(), this)); + cnt_exprs_.push_back(expr); + cnt_types_.push_back(INEQ); + return cnts_.back(); +} + +Cnt qpOASESModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/) +{ + assert(0 && "NOT IMPLEMENTED"); + return 0; +} + +void qpOASESModel::removeVars(const VarVector& vars) +{ + IntVec inds = vars2inds(vars); + for (unsigned i = 0; i < vars.size(); ++i) + vars[i].var_rep->removed = true; +} + +void qpOASESModel::removeCnts(const CntVector& cnts) +{ + IntVec inds = cnts2inds(cnts); + for (unsigned i = 0; i < cnts.size(); ++i) + cnts[i].cnt_rep->removed = true; +} + +void qpOASESModel::updateObjective() +{ + const size_t n = vars_.size(); + + Eigen::SparseMatrix sm; + exprToEigen(objective_, sm, g_, n, true, true); + eigenToCSC(sm, H_row_indices_, H_column_pointers_, H_csc_data_); + + H_ = SymSparseMat(vars_.size(), vars_.size(), H_row_indices_.data(), H_column_pointers_.data(), H_csc_data_.data()); + H_.createDiagInfo(); +} + +void qpOASESModel::updateConstraints() +{ + const size_t n = vars_.size(); + const size_t m = cnts_.size(); + + lbA_.clear(); + lbA_.resize(m, -QPOASES_INFTY); + ubA_.clear(); + ubA_.resize(m, QPOASES_INFTY); + + Eigen::SparseMatrix sm; + Eigen::VectorXd v; + exprToEigen(cnt_exprs_, sm, v, n); + + for (int i_cnt = 0; i_cnt < m; ++i_cnt) + { + lbA_[i_cnt] = (cnt_types_[i_cnt] == INEQ) ? -QPOASES_INFTY : v[i_cnt]; + ubA_[i_cnt] = v[i_cnt]; + } + + eigenToCSC(sm, A_row_indices_, A_column_pointers_, A_csc_data_); + A_ = SparseMatrix(cnts_.size(), vars_.size(), A_row_indices_.data(), A_column_pointers_.data(), A_csc_data_.data()); +} + +bool qpOASESModel::updateSolver() +{ + bool solver_updated = false; + if (!qpoases_problem_ || vars_.size() != qpoases_problem_->getNV() || cnts_.size() != qpoases_problem_->getNC()) + { + // Create Problem - this should be called only once + qpoases_problem_.reset(new SQProblem(vars_.size(), cnts_.size())); + qpoases_problem_->setOptions(qpoases_options_); + solver_updated = true; + } + return solver_updated; +} + +void qpOASESModel::createSolver() +{ + qpoases_problem_.reset(); + updateSolver(); +} + +void qpOASESModel::update() +{ + { + int inew = 0; + for (unsigned iold = 0; iold < vars_.size(); ++iold) + { + const Var& var = vars_[iold]; + if (!var.var_rep->removed) + { + vars_[inew] = var; + lb_[inew] = lb_[iold]; + ub_[inew] = ub_[iold]; + var.var_rep->index = inew; + ++inew; + } + else + delete var.var_rep; + } + vars_.resize(inew); + lb_.resize(inew, QPOASES_INFTY); + ub_.resize(inew, -QPOASES_INFTY); + } + { + int inew = 0; + for (unsigned iold = 0; iold < cnts_.size(); ++iold) + { + const Cnt& cnt = cnts_[iold]; + if (!cnt.cnt_rep->removed) + { + cnts_[inew] = cnt; + cnt_exprs_[inew] = cnt_exprs_[iold]; + cnt_types_[inew] = cnt_types_[iold]; + cnt.cnt_rep->index = inew; + ++inew; + } + else + delete cnt.cnt_rep; + } + cnts_.resize(inew); + cnt_exprs_.resize(inew); + cnt_types_.resize(inew); + } +} + +void qpOASESModel::setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) +{ + for (unsigned i = 0; i < vars.size(); ++i) + { + const int varind = vars[i].var_rep->index; + lb_[varind] = lower[i]; + ub_[varind] = upper[i]; + } +} +DblVec qpOASESModel::getVarValues(const VarVector& vars) const +{ + DblVec out(vars.size()); + for (unsigned i = 0; i < vars.size(); ++i) + { + const int varind = vars[i].var_rep->index; + out[i] = solution_[varind]; + } + return out; +} + +CvxOptStatus qpOASESModel::optimize() +{ + update(); + updateObjective(); + updateConstraints(); + updateSolver(); + qpOASES::returnValue val = qpOASES::RET_QP_SOLUTION_STARTED; + + // Solve Problem + int nWSR = 255; + if (qpoases_problem_->isInitialised()) + { + val = qpoases_problem_->hotstart( + &H_, g_.data(), &A_, lb_.data(), ub_.data(), lbA_.data(), ubA_.data(), nWSR, nullptr); + } + + if (val != qpOASES::SUCCESSFUL_RETURN) + { + // TODO ATM this means we are creating a new solver even if updateSolver + // returned true and the problem is not initialized. Still, it makes + // tests pass. + createSolver(); + + val = qpoases_problem_->init(&H_, g_.data(), &A_, lb_.data(), ub_.data(), lbA_.data(), ubA_.data(), nWSR, nullptr); + } + + if (val == qpOASES::SUCCESSFUL_RETURN) + { + // opt += m_objective.affexpr.constant; + solution_.resize(vars_.size(), 0.); + val = qpoases_problem_->getPrimalSolution(solution_.data()); + return CVX_SOLVED; + } + else if (val == qpOASES::RET_INIT_FAILED_INFEASIBILITY) + { + return CVX_INFEASIBLE; + } + else + { + return CVX_FAILED; + } +} +void qpOASESModel::setObjective(const AffExpr& expr) { objective_.affexpr = expr; } +void qpOASESModel::setObjective(const QuadExpr& expr) { objective_ = expr; } +void qpOASESModel::writeToFile(const std::string& /*fname*/) +{ + return; // NOT IMPLEMENTED +} +VarVector qpOASESModel::getVars() const { return vars_; } +} diff --git a/trajopt_sco/src/solver_interface.cpp b/trajopt_sco/src/solver_interface.cpp index f2ce1ba1..382722ee 100644 --- a/trajopt_sco/src/solver_interface.cpp +++ b/trajopt_sco/src/solver_interface.cpp @@ -7,6 +7,14 @@ namespace sco { +const std::vector ModelType::MODEL_NAMES_ = { + "GUROBI", + "BPMPD", + "OSQP", + "QPOASES", + "AUTO_SOLVER" +}; + IntVec vars2inds(const VarVector& vars) { IntVec inds(vars.size()); @@ -28,7 +36,7 @@ void simplify2(IntVec& inds, DblVec& vals) Int2Double ind2val; for (unsigned i = 0; i < inds.size(); ++i) { - if (vals[i] != 0) + if (vals[i] != 0.0) ind2val[inds[i]] += vals[i]; } inds.resize(ind2val.size()); @@ -51,7 +59,7 @@ double AffExpr::value(const double* x) const } return out; } -double AffExpr::value(const DblVec &x) const +double AffExpr::value(const DblVec& x) const { double out = constant; for (size_t i = 0; i < size(); ++i) @@ -117,11 +125,13 @@ std::ostream& operator<<(std::ostream& o, const Var& v) o << "nullvar"; return o; } + std::ostream& operator<<(std::ostream& o, const Cnt& c) { o << c.cnt_rep->expr << ((c.cnt_rep->type == EQ) ? " == 0" : " <= 0"); return o; } + std::ostream& operator<<(std::ostream& o, const AffExpr& e) { o << e.constant; @@ -131,6 +141,7 @@ std::ostream& operator<<(std::ostream& o, const AffExpr& e) } return o; } + std::ostream& operator<<(std::ostream& o, const QuadExpr& e) { o << e.affexpr; @@ -141,7 +152,110 @@ std::ostream& operator<<(std::ostream& o, const QuadExpr& e) return o; } -ModelPtr createModel() +std::ostream& operator<<(std::ostream& o, const ModelType& cs) +{ + int cs_ivalue_ = static_cast(cs.value_); + if (cs_ivalue_ > cs.MODEL_NAMES_.size()) + { + std::stringstream conversion_error; + conversion_error << "Error converting ModelType to string - " + << "enum value is " << cs_ivalue_ << std::endl; + throw std::runtime_error(conversion_error.str()); + } + o << ModelType::MODEL_NAMES_[cs_ivalue_]; + return o; +} + +ModelType::ModelType() +{ + value_ = ModelType::AUTO_SOLVER; +} + +ModelType::ModelType(const ModelType::Value& v) +{ + value_ = v; +} + +ModelType::ModelType(const int& v) +{ + value_ = static_cast(v); +} + +ModelType::ModelType(const std::string& s) +{ + for (unsigned int i = 0; i < ModelType::MODEL_NAMES_.size(); ++i) + { + if (s == ModelType::MODEL_NAMES_[i]) + { + value_ = static_cast(i); + return; + } + } + PRINT_AND_THROW(boost::format("invalid solver name:\"%s\"") % s); +} + +ModelType::operator int() const +{ + return static_cast(value_); +} + +bool ModelType::operator==(const ModelType::Value& a) const +{ + return value_ == a; +} + +bool ModelType::operator==(const ModelType& a) const +{ + return value_ == a.value_; +} + +bool ModelType::operator!=(const ModelType& a) const +{ + return value_ != a.value_; +} + +void ModelType::fromJson(const Json::Value& v) { + try + { + std::string ref = v.asString(); + ModelType cs(ref); + value_ = cs.value_; + } + catch (const std::runtime_error&) + { + PRINT_AND_THROW(boost::format("expected: %s, got %s") % ("string") % (v)); + } +} + +std::vector availableSolvers() +{ + std::vector has_solver(ModelType::AUTO_SOLVER, false); +#ifdef HAVE_GUROBI + has_solver[ModelType::GUROBI] = true; +#endif +#ifdef HAVE_BPMPD + has_solver[ModelType::BPMPD] = true; +#endif +#ifdef HAVE_OSQP + has_solver[ModelType::OSQP] = true; +#endif +#ifdef HAVE_QPOASES + has_solver[ModelType::QPOASES] = true; +#endif + int n_available_solvers = 0; + for (auto i = 0; i < ModelType::AUTO_SOLVER; ++i) + if (has_solver[i]) + ++n_available_solvers; + std::vector available_solvers(n_available_solvers, + ModelType::AUTO_SOLVER); + auto j = 0; + for (auto i = 0; i < ModelType::AUTO_SOLVER; ++i) + if (has_solver[i]) + available_solvers[j++] = static_cast(i); + return available_solvers; +} + +ModelPtr createModel(ModelType model_type) { #ifdef HAVE_GUROBI extern ModelPtr createGurobiModel(); @@ -149,53 +263,73 @@ ModelPtr createModel() #ifdef HAVE_BPMPD extern ModelPtr createBPMPDModel(); #endif - - enum ConvexSolver - { - GUROBI, - BPMPD, - INVALID - }; +#ifdef HAVE_OSQP + extern ModelPtr createOSQPModel(); +#endif +#ifdef HAVE_QPOASES + extern ModelPtr createqpOASESModel(); +#endif char* solver_env = getenv("TRAJOPT_CONVEX_SOLVER"); - ConvexSolver solver = INVALID; + ModelType solver = model_type; - if (solver_env) + if (solver == ModelType::AUTO_SOLVER) { - if (std::string(solver_env) == "GUROBI") - solver = GUROBI; - else if (std::string(solver_env) == "BPMPD") - solver = BPMPD; + if (solver_env and std::string(solver_env) != "AUTO_SOLVER") + { + try + { + solver = ModelType(std::string(solver_env)); + } + catch (std::runtime_error&) + { + PRINT_AND_THROW(boost::format("invalid solver \"%s\"specified by TRAJOPT_CONVEX_SOLVER") % solver_env); + } + } else - PRINT_AND_THROW(boost::format("invalid solver \"%s\"specified by TRAJOPT_CONVEX_SOLVER") % solver_env); + { + solver = availableSolvers()[0]; + } + } + #ifndef HAVE_GUROBI - if (solver == GUROBI) - PRINT_AND_THROW("you didn't build with GUROBI support"); + if (solver == ModelType::GUROBI) + PRINT_AND_THROW("you didn't build with GUROBI support"); #endif #ifndef HAVE_BPMPD - if (solver == BPMPD) - PRINT_AND_THROW("you don't have BPMPD support on this platform"); + if (solver == ModelType::BPMPD) + PRINT_AND_THROW("you don't have BPMPD support on this platform"); #endif - } - else - { -#ifdef HAVE_GUROBI - solver = GUROBI; -#else - solver = BPMPD; +#ifndef HAVE_OSQP + if (solver == ModelType::OSQP) + PRINT_AND_THROW("you don't have OSQP support on this platform"); +#endif +#ifndef HAVE_QPOASES + if (solver == ModelType::QPOASES) + PRINT_AND_THROW("you don't have qpOASES support on this platform"); #endif - } #ifdef HAVE_GUROBI - if (solver == GUROBI) + if (solver == ModelType::GUROBI) return createGurobiModel(); #endif #ifdef HAVE_BPMPD - if (solver == BPMPD) + if (solver == ModelType::BPMPD) return createBPMPDModel(); #endif - PRINT_AND_THROW("Failed to create solver"); +#ifdef HAVE_OSQP + if (solver == ModelType::OSQP) + return createOSQPModel(); +#endif +#ifdef HAVE_QPOASES + if (solver == ModelType::QPOASES) + return createqpOASESModel(); +#endif + std::stringstream solver_instatiation_error; + solver_instatiation_error << "Failed to create solver: unknown solver " + << solver << std::endl; + PRINT_AND_THROW(solver_instatiation_error.str()); return ModelPtr(); } } diff --git a/trajopt_sco/src/solver_utils.cpp b/trajopt_sco/src/solver_utils.cpp new file mode 100644 index 00000000..904e1fb7 --- /dev/null +++ b/trajopt_sco/src/solver_utils.cpp @@ -0,0 +1,131 @@ +#include +#include +#include +#include +#include + +namespace sco +{ +void exprToEigen(const AffExpr& expr, Eigen::SparseVector& sparse_vector, const int& n_vars) +{ + sparse_vector.resize(n_vars); + sparse_vector.reserve(expr.size()); + for (size_t i = 0; i < expr.size(); ++i) + { + int i_var_index = expr.vars[i].var_rep->index; + if (i_var_index >= n_vars) + { + std::stringstream msg; + msg << "Coefficient " << i << "has index " << i_var_index << " but n_vars is " << n_vars; + throw std::runtime_error(msg.str()); + } + if (expr.coeffs[i] != 0.) + sparse_vector.coeffRef(i_var_index) += expr.coeffs[i]; + } +} + +void exprToEigen(const QuadExpr& expr, + Eigen::SparseMatrix& sparse_matrix, + Eigen::VectorXd& vector, + const int& n_vars, + const bool& matrix_is_halved, + const bool& force_diagonal) +{ + IntVec ind1 = vars2inds(expr.vars1); + IntVec ind2 = vars2inds(expr.vars2); + sparse_matrix.resize(n_vars, n_vars); + sparse_matrix.reserve(2 * expr.size()); + + Eigen::SparseVector vector_sparse; + exprToEigen(expr.affexpr, vector_sparse, n_vars); + vector = vector_sparse; + + for (size_t i = 0; i < expr.coeffs.size(); ++i) + { + if (expr.coeffs[i] != 0.0) + { + if (ind1[i] == ind2[i]) + sparse_matrix.coeffRef(ind1[i], ind2[i]) += expr.coeffs[i]; + else + { + int c, r; + if (ind1[i] < ind2[i]) + { + r = ind1[i]; + c = ind2[i]; + } + else + { + r = ind2[i]; + c = ind1[i]; + } + sparse_matrix.coeffRef(r, c) += expr.coeffs[i]; + } + } + } + + auto sparse_matrix_T = Eigen::SparseMatrix(sparse_matrix.transpose()); + sparse_matrix = sparse_matrix + sparse_matrix_T; + + if (!matrix_is_halved) + sparse_matrix = 0.5 * sparse_matrix; + + if (force_diagonal) + for (unsigned int k = 0; k < n_vars; ++k) + sparse_matrix.coeffRef(k, k) += 0.0; +} + +void exprToEigen(const AffExprVector& expr_vec, + Eigen::SparseMatrix& sparse_matrix, + Eigen::VectorXd& vector, + const int& n_vars) +{ + vector.resize(expr_vec.size()); + vector.setZero(); + sparse_matrix.resize(expr_vec.size(), n_vars); + sparse_matrix.reserve(expr_vec.size() * n_vars); + + for (size_t i = 0; i < expr_vec.size(); ++i) + { + const AffExpr& expr = expr_vec[i]; + Eigen::SparseVector expr_vector; + double expr_constant; + exprToEigen(expr, expr_vector, n_vars); + for (Eigen::SparseVector::InnerIterator it(expr_vector); it; ++it) + sparse_matrix.coeffRef(i, it.index()) = it.value(); + vector[i] = -expr.constant; + } +} + +void tripletsToEigen(const IntVec& rows_i, + const IntVec& cols_j, + const DblVec& values_ij, + Eigen::SparseMatrix& sparse_matrix) +{ + typedef Eigen::Triplet T; + std::vector> triplets; + for (unsigned int i = 0; i < values_ij.size(); ++i) + triplets.push_back(T(rows_i[i], cols_j[i], values_ij[i])); + sparse_matrix.setFromTriplets(triplets.begin(), triplets.end()); +} + +void eigenToTriplets(const Eigen::SparseMatrix& sparse_matrix, + IntVec& rows_i, + IntVec& cols_j, + DblVec& values_ij) +{ + auto& sm = sparse_matrix; + rows_i.reserve(rows_i.size() + sm.nonZeros()); + cols_j.reserve(cols_j.size() + sm.nonZeros()); + values_ij.reserve(values_ij.size() + sm.nonZeros()); + for (int k = 0; k < sm.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(sm, k); it; ++it) + { + rows_i.push_back(it.row()); + cols_j.push_back(it.col()); + values_ij.push_back(it.value()); + } + } +} +} \ No newline at end of file diff --git a/trajopt_sco/test/small-problems-unit.cpp b/trajopt_sco/test/small-problems-unit.cpp index 000c97a7..eb272928 100644 --- a/trajopt_sco/test/small-problems-unit.cpp +++ b/trajopt_sco/test/small-problems-unit.cpp @@ -17,9 +17,14 @@ using namespace std; using namespace sco; using namespace Eigen; -void setupProblem(OptProbPtr& probptr, size_t nvars) +class SQP : public testing::TestWithParam { + protected: + SQP() {} +}; + +void setupProblem(OptProbPtr& probptr, size_t nvars, ModelType convex_solver) { - probptr.reset(new OptProb()); + probptr.reset(new OptProb(convex_solver)); vector var_names; for (size_t i = 0; i < nvars; ++i) { @@ -38,11 +43,11 @@ void expectAllNear(const DblVec& x, const DblVec& y, double abstol) } double f_QuadraticSeparable(const VectorXd& x) { return x(0) * x(0) + sq(x(1) - 1) + sq(x(2) - 2); } -TEST(SQP, QuadraticSeparable) +TEST_P(SQP, QuadraticSeparable) { // if the problem is exactly a QP, it should be solved in one iteration OptProbPtr prob; - setupProblem(prob, 3); + setupProblem(prob, 3, GetParam()); prob->addCost(CostPtr(new CostFromFunc(ScalarOfVector::construct(&f_QuadraticSeparable), prob->getVars(), "f"))); BasicTrustRegionSQP solver(prob); BasicTrustRegionSQPParameters ¶ms = solver.getParameters(); @@ -55,10 +60,10 @@ TEST(SQP, QuadraticSeparable) // todo: checks on number of iterations and function evaluates } double f_QuadraticNonseparable(const VectorXd& x) { return sq(x(0) - x(1) + 3 * x(2)) + sq(x(0) - 1) + sq(x(2) - 2); } -TEST(SQP, QuadraticNonseparable) +TEST_P(SQP, QuadraticNonseparable) { OptProbPtr prob; - setupProblem(prob, 3); + setupProblem(prob, 3, GetParam()); prob->addCost( CostPtr(new CostFromFunc(ScalarOfVector::construct(&f_QuadraticNonseparable), prob->getVars(), "f", true))); BasicTrustRegionSQP solver(prob); @@ -78,12 +83,14 @@ void testProblem(ScalarOfVectorPtr f, VectorOfVectorPtr g, ConstraintType cnt_type, const DblVec& init, - const DblVec& sol) + const DblVec& sol, + ModelType convex_solver + ) { OptProbPtr prob; size_t n = init.size(); assert(sol.size() == n); - setupProblem(prob, n); + setupProblem(prob, n, convex_solver); prob->addCost(CostPtr(new CostFromFunc(f, prob->getVars(), "f", true))); prob->addConstraint(ConstraintPtr(new ConstraintFromErrFunc(g, prob->getVars(), VectorXd(), cnt_type, "g"))); BasicTrustRegionSQP solver(prob); @@ -136,22 +143,30 @@ VectorXd g_TP7(const VectorXd& x) return out; } -TEST(SQP, TP1) +TEST_P(SQP, TP1) { testProblem( - ScalarOfVector::construct(&f_TP1), VectorOfVector::construct(&g_TP1), INEQ, {-2, 1}, {1, 1}); + ScalarOfVector::construct(&f_TP1), VectorOfVector::construct(&g_TP1), + INEQ, {-2, 1}, {1, 1}, GetParam()); } -TEST(SQP, TP3) +TEST_P(SQP, TP3) { testProblem( - ScalarOfVector::construct(&f_TP3), VectorOfVector::construct(&g_TP3), INEQ, {10, 1}, {0, 0}); + ScalarOfVector::construct(&f_TP3), VectorOfVector::construct(&g_TP3), + INEQ, {10, 1}, {0, 0}, GetParam()); } -TEST(SQP, TP6) +TEST_P(SQP, TP6) { - testProblem(ScalarOfVector::construct(&f_TP6), VectorOfVector::construct(&g_TP6), EQ, {10, 1}, {1, 1}); + testProblem( + ScalarOfVector::construct(&f_TP6), VectorOfVector::construct(&g_TP6), + EQ, {10, 1}, {1, 1}, GetParam()); } -TEST(SQP, TP7) +TEST_P(SQP, TP7) { testProblem( - ScalarOfVector::construct(&f_TP7), VectorOfVector::construct(&g_TP7), EQ, {2, 2}, {0., sqrtf(3.)}); + ScalarOfVector::construct(&f_TP7), VectorOfVector::construct(&g_TP7), + EQ, {2, 2}, {0., sqrtf(3.)}, GetParam()); } + +INSTANTIATE_TEST_CASE_P(AllSolvers, SQP, + testing::ValuesIn(availableSolvers())); \ No newline at end of file diff --git a/trajopt_sco/test/solver-interface-unit.cpp b/trajopt_sco/test/solver-interface-unit.cpp index 84ffa8c9..db75447a 100644 --- a/trajopt_sco/test/solver-interface-unit.cpp +++ b/trajopt_sco/test/solver-interface-unit.cpp @@ -6,11 +6,33 @@ #include #include +namespace sco +{ +extern void simplify2(IntVec& inds, DblVec& vals); +} + using namespace sco; -TEST(solver_interface, setup_problem) +class SolverInterface : public testing::TestWithParam { + protected: + SolverInterface() {} +}; + +TEST(SolverInterface, simplify2) { - ModelPtr solver = createModel(); + IntVec indices = {0, 1, 3}; + DblVec values = {1e-7, 1e3, 0., 0., 0.}; + simplify2(indices, values); + + EXPECT_EQ(indices.size(), 2); + EXPECT_EQ(values.size(), 2); + EXPECT_TRUE((indices == IntVec{0, 1})); + EXPECT_TRUE((values == DblVec{1e-7, 1e3})); +} + +TEST_P(SolverInterface, setup_problem) +{ + ModelPtr solver = createModel(GetParam()); VarVector vars; for (int i = 0; i < 3; ++i) { @@ -51,9 +73,9 @@ TEST(solver_interface, setup_problem) } // Tests multiplying larger terms -TEST(ExprMult_test1, setup_problem) +TEST_P(SolverInterface, DISABLED_ExprMult_test1) // QuadExpr not PSD { - ModelPtr solver = createModel(); + ModelPtr solver = createModel(GetParam()); VarVector vars; for (int i = 0; i < 3; ++i) { @@ -111,7 +133,7 @@ TEST(ExprMult_test1, setup_problem) } // Tests multiplication with 2 variables: v1=10, v2=20 => (2*v1)(v2) = 400 -TEST(ExprMult_test2, setup_problem) +TEST_P(SolverInterface, ExprMult_test2) { const double v1_val = 10; const double v2_val = 20; @@ -120,7 +142,7 @@ TEST(ExprMult_test2, setup_problem) const double aff1_const = 0; const double aff2_const = 0; - ModelPtr solver = createModel(); + ModelPtr solver = createModel(GetParam()); VarVector vars; vars.push_back(solver->addVar("v1")); vars.push_back(solver->addVar("v2")); @@ -163,7 +185,7 @@ TEST(ExprMult_test2, setup_problem) } // Tests multiplication with a constant: v1=10, v2=20 => (3*v1-3)(2*v2-5) = 945 -TEST(ExprMult_test3, setup_problem) +TEST_P(SolverInterface, ExprMult_test3) { const double v1_val = 10; const double v2_val = 20; @@ -172,7 +194,7 @@ TEST(ExprMult_test3, setup_problem) const double aff1_const = -3; const double aff2_const = -5; - ModelPtr solver = createModel(); + ModelPtr solver = createModel(GetParam()); VarVector vars; vars.push_back(solver->addVar("v1")); vars.push_back(solver->addVar("v2")); @@ -213,3 +235,6 @@ TEST(ExprMult_test3, setup_problem) double answer = (v1_coeff * v1_val + aff1_const) * (v2_coeff * v2_val + aff2_const); EXPECT_NEAR(aff12.value(soln), answer, 1e-6); } + +INSTANTIATE_TEST_CASE_P(AllSolvers, SolverInterface, + testing::ValuesIn(availableSolvers())); diff --git a/trajopt_sco/test/solver-utils-unit.cpp b/trajopt_sco/test/solver-utils-unit.cpp new file mode 100644 index 00000000..26e4be7a --- /dev/null +++ b/trajopt_sco/test/solver-utils-unit.cpp @@ -0,0 +1,258 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace sco; + +TEST(solver_utils, exprToEigen) +{ + int n_vars = 2; + std::vector x_info; + VarVector x; + for (unsigned int i = 0; i < n_vars; ++i) + { + std::stringstream var_name; + var_name << "x_" << i; + VarRepPtr x_el(new VarRep(i, var_name.str(), nullptr)); + x_info.push_back(x_el); + x.push_back(Var(x_el.get())); + } + + // x_affine = [3, 2]*x + 1 + AffExpr x_affine; + x_affine.vars = x; + x_affine.coeffs = DblVec{ 3, 2 }; + x_affine.constant = 1; + + std::cout << "x_affine= " << x_affine << std::endl; + std::cout << "expecting A = [3, 2];" << std::endl + << " u = [1]" << std::endl; + Eigen::MatrixXd m_A_expected(1, n_vars); + m_A_expected << 3, 2; + Eigen::VectorXd v_u_expected(1); + v_u_expected << -1; + + Eigen::SparseVector v_A; + exprToEigen(x_affine, v_A, n_vars); + ASSERT_EQ(v_A.size(), m_A_expected.cols()); + Eigen::VectorXd v_A_d = v_A; + Eigen::VectorXd m_A_r = m_A_expected.row(0); + EXPECT_TRUE(v_A_d.isApprox(m_A_r)) << "error converting x_affine" + << " to Eigen::SparseVector. " + << "v_A :" << std::endl + << v_A << std::endl; + + Eigen::SparseMatrix m_A; + Eigen::VectorXd v_u; + AffExprVector x_affine_vector(1, x_affine); + exprToEigen(x_affine_vector, m_A, v_u, n_vars); + ASSERT_EQ(v_u_expected.size(), v_u.size()); + EXPECT_TRUE(v_u_expected == v_u) << "v_u_expected != v_u" << std::endl << "v_u:" << std::endl << v_u << std::endl; + EXPECT_EQ(m_A.nonZeros(), 2) << "m_A.nonZeros() != 2" << std::endl; + ASSERT_EQ(m_A.rows(), m_A_expected.rows()); + ASSERT_EQ(m_A.cols(), m_A_expected.cols()); + EXPECT_TRUE(m_A.isApprox(m_A_expected)) << "error converting x_affine to " + << "Eigen::SparseMatrix. m_A :" << std::endl + << m_A << std::endl; + + QuadExpr x_squared = exprSquare(x_affine); + std::cout << "x_squared= " << x_squared << std::endl; + std::cout << "expecting Q = [9, 6;" << std::endl + << " 6, 4]" << std::endl + << " q = [6, 4]" << std::endl; + Eigen::MatrixXd m_Q_expected(2, 2); + m_Q_expected << 9, 6, 6, 4; + DblVec v_q_expected{ 6, 4 }; + + Eigen::SparseMatrix m_Q; + Eigen::VectorXd v_q; + exprToEigen(x_squared, m_Q, v_q, n_vars); + { + Eigen::Map v_q_e_eig(v_q_expected.data(), v_q_expected.size()); + ASSERT_EQ(v_q.size(), v_q_e_eig.size()); + EXPECT_TRUE(v_q_e_eig.isApprox(v_q)) << "v_q_expected != v_q" << std::endl + << "v_q:" << std::endl + << v_q << std::endl; + } + EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " + << "Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 4) << "m_Q.nonZeros() != 4" << std::endl; + + exprToEigen(x_squared, m_Q, v_q, n_vars, true); + { + Eigen::Map v_q_e_eig(v_q_expected.data(), v_q_expected.size()); + EXPECT_TRUE(v_q_e_eig.isApprox(v_q)) << "v_q_expected != v_q" << std::endl + << "v_q:" << std::endl + << v_q << std::endl; + } + EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to" + << " Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 4) << "m_Q.nonZeros() != 4" << std::endl; + + x_affine.coeffs = DblVec{ 0, 2 }; + std::cout << "x_affine= " << x_affine << std::endl; + std::cout << "expecting A = [0, 2];" << std::endl; + x_squared = exprSquare(x_affine); + std::cout << "x_squared= " << x_squared << std::endl; + std::cout << "expecting Q = [0, 0;" << std::endl << " 0, 4]" << std::endl; + m_Q_expected.setZero(); + m_Q_expected << 0, 0, 0, 4; + exprToEigen(x_squared, m_Q, v_q, n_vars, false, false); + EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " + << "Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 1) << "m_Q.nonZeros() != 1" << std::endl; + + exprToEigen(x_squared, m_Q, v_q, n_vars, true, false); + EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to" + << " Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 1) << "m_Q.nonZeros() != 1" << std::endl; + + exprToEigen(x_squared, m_Q, v_q, n_vars, false, true); + EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " + << "Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 2) << "m_Q.nonZeros() != 2" << std::endl; + + exprToEigen(x_squared, m_Q, v_q, n_vars, true, true); + EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to" + << "Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 2) << "m_Q.nonZeros() != 2" << std::endl; +} + +TEST(solver_utils, eigenToTriplets) +{ + Eigen::MatrixXd m_Q(2, 2); + m_Q << 9, 0, 6, 4; + Eigen::SparseMatrix m_Q_sparse_expected = m_Q.sparseView(); + IntVec m_Q_i, m_Q_j; + DblVec m_Q_ij; + eigenToTriplets(m_Q_sparse_expected, m_Q_i, m_Q_j, m_Q_ij); + Eigen::SparseMatrix m_Q_sparse(2, 2); + tripletsToEigen(m_Q_i, m_Q_j, m_Q_ij, m_Q_sparse); + EXPECT_TRUE(m_Q_sparse.isApprox(m_Q)) << "m_Q != m_Q_sparse when converting " + << "m_Q -> triplets -> m_Q_sparse. m_Q:" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q_sparse.nonZeros(), 3) << "m_Q.nonZeros() != 3" << std::endl; +} + +TEST(solver_utils, eigenToCSC) +{ + DblVec P; + IntVec rows_i; + IntVec cols_p; + { + /* + * M = [ 1, 2, 3, + * 1, 0, 9, + * 1, 8, 0] + */ + Eigen::MatrixXd M(3, 3); + M << 1, 2, 3, 1, 0, 9, 1, 8, 0; + Eigen::SparseMatrix Ms = M.sparseView(); + + eigenToCSC(Ms, rows_i, cols_p, P); + + EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; + EXPECT_TRUE((P == DblVec{ 1, 1, 1, 2, 8, 3, 9 })) << "bad P:\n" << CSTR(P); + EXPECT_TRUE((rows_i == IntVec{ 0, 1, 2, 0, 2, 0, 1 })) << "bad rows_i:\n" << CSTR(rows_i); + EXPECT_TRUE((cols_p == IntVec{ 0, 3, 5, 7 })) << "cols_p not in " + << "CRC form:\n" + << CSTR(cols_p); + } + { + /* + * M = [ 0, 2, 0, + * 7, 0, 0, + * 0, 0, 0] + */ + Eigen::SparseMatrix M(3, 3); + M.coeffRef(0, 1) = 2; + M.coeffRef(1, 0) = 7; + + eigenToCSC(M, rows_i, cols_p, P); + + EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; + EXPECT_TRUE((P == DblVec{ 7, 2 })) << "bad P:\n" << CSTR(P); + EXPECT_TRUE((rows_i == IntVec{ 1, 0 })) << "rows_i != data_j:\n" + << CSTR(rows_i) << " vs\n" + << CSTR((IntVec{ 1, 0 })); + EXPECT_TRUE((cols_p == IntVec{ 0, 1, 2, 2 })) << "cols_p not in " + << "CRC form:\n" + << CSTR(cols_p); + + std::vector rows_i_ll, cols_p_ll; + std::vector rows_i_ll_exp{ 1, 0 }; + std::vector cols_p_ll_exp{ 0, 1, 2, 2 }; + + eigenToCSC(M, rows_i_ll, cols_p_ll, P); + EXPECT_TRUE(rows_i_ll.size() == P.size()) << "rows_i_ll.size() != P.size()"; + EXPECT_TRUE((rows_i_ll == rows_i_ll_exp)); + EXPECT_TRUE((cols_p_ll == cols_p_ll_exp)); + + std::vector rows_i_ull, cols_p_ull; + std::vector rows_i_ull_exp{ 1, 0 }; + std::vector cols_p_ull_exp{ 0, 1, 2, 2 }; + eigenToCSC(M, rows_i_ull, cols_p_ull, P); + EXPECT_TRUE(rows_i_ull.size() == P.size()) << "rows_i_ll.size() != P.size()"; + EXPECT_TRUE((rows_i_ull == rows_i_ull_exp)); + EXPECT_TRUE((cols_p_ull == cols_p_ull_exp)); + } + { + /* + * M = [ 0, 0, 0, + * 0, 0, 0, + * 0, 6, 0] + */ + Eigen::SparseMatrix M(3, 3); + M.coeffRef(2, 1) = 6; + + eigenToCSC(M, rows_i, cols_p, P); + + EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; + EXPECT_TRUE((P == DblVec{ 6 })) << "bad P:\n" << CSTR(P); + EXPECT_TRUE((rows_i == IntVec{ 2 })) << "rows_i != data_j:\n" << CSTR(rows_i) << " vs\n" << CSTR((IntVec{ 1, 0 })); + EXPECT_TRUE((cols_p == IntVec{ 0, 0, 1, 1 })) << "cols_p not in " + << "CRC form:\n" + << CSTR(cols_p); + } +} + +TEST(solver_utils, eigenToCSC_upper_triangular) +{ + /* + * M = [ 1, 2, 0, + * 2, 4, 0, + * 0, 0, 9] + */ + Eigen::MatrixXd M(3, 3); + M << 1, 2, 0, 2, 4, 0, 0, 0, 9; + Eigen::SparseMatrix Ms = M.sparseView(); + + DblVec P; + IntVec rows_i; + IntVec cols_p; + eigenToCSC(Ms, rows_i, cols_p, P); + + EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; + EXPECT_TRUE((P == DblVec{ 1, 2, 4, 9 })) << "bad P:\n" << CSTR(P); + EXPECT_TRUE((rows_i == IntVec{ 0, 0, 1, 2 })) << "rows_i != expected" + << ":\n" + << CSTR(rows_i) << " vs\n" + << CSTR((IntVec{ 0, 0, 1, 2 })); + EXPECT_TRUE((cols_p == IntVec{ 0, 1, 3, 4 })) << "cols_p not in " + << "CRC form:\n" + << CSTR(cols_p); +} diff --git a/trajopt_test_support/config/numerical_ik1.json b/trajopt_test_support/config/numerical_ik1.json index 10cc362b..13d1489d 100644 --- a/trajopt_test_support/config/numerical_ik1.json +++ b/trajopt_test_support/config/numerical_ik1.json @@ -2,7 +2,8 @@ "basic_info" : { "n_steps" : 1, "manip" : "left_arm", - "start_fixed" : false + "start_fixed" : false, + "convex_solver" : "AUTO_SOLVER" }, "costs" : [ { @@ -17,6 +18,6 @@ } ], "init_info" : { - "type" : "stationary" + "type" : "stationary" } } diff --git a/trajopt_utils/src/logging.cpp b/trajopt_utils/src/logging.cpp index 0be6e0d8..13868ebd 100644 --- a/trajopt_utils/src/logging.cpp +++ b/trajopt_utils/src/logging.cpp @@ -13,12 +13,16 @@ int LoggingInit() char* lvlc = getenv("TRAJOPT_LOG_THRESH"); std::string lvlstr; - if (lvlc == NULL) + if (lvlc == nullptr) { std::printf("You can set logging level with TRAJOPT_LOG_THRESH. Valid values: " - "%s. Defaulting to INFO\n", + "%s. Defaulting to ERROR\n", VALID_THRESH_VALUES); - lvlstr = "INFO"; + #ifdef NDEBUG + lvlstr = "ERROR"; + #else + lvlstr = "DEBUG"; + #endif } else lvlstr = std::string(lvlc);