├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── FindEigen.cmake ├── include └── ooqp_eigen_interface │ ├── OoqpEigenInterface.hpp │ ├── QuadraticProblemFormulation.hpp │ ├── ooqpei_assert_macros.hpp │ ├── ooqpei_gtest_eigen.hpp │ ├── ooqpei_numerical_comparisons.hpp │ └── ooqpei_source_file_pos.hpp ├── jenkins-pipeline ├── package.xml ├── src ├── OoqpEigenInterface.cpp └── QuadraticProblemFormulation.cpp └── test ├── OoqpEigenInterface_test.cpp ├── QuadraticProblemFormulation_test.cpp ├── matlab ├── OoqpEigenInterface_test.m └── QuadraticProblemFormulation_test.m └── test_main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Build folder 2 | /build/ 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | 22 | # Compiled Static libraries 23 | *.lai 24 | *.la 25 | *.a 26 | *.lib 27 | 28 | # Executables 29 | *.exe 30 | *.out 31 | *.app 32 | 33 | # Eclipse 34 | .cproject 35 | .project 36 | .settings 37 | *.pydevproject 38 | 39 | # gTest 40 | /gtest*/ 41 | gtest 42 | /Debug/ 43 | 44 | # Generated files 45 | FindOOQPEI.cmake 46 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------------------------- 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2014, Péter Fankhauser, Christian Gehring, Stelian Coros 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of Autonomous Systems Lab nor ETH Zurich 18 | # nor the names of its contributors may be used to endorse or 19 | # promote products derived from this software without specific 20 | # prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | #------------------------------------------------------------------------------- 35 | 36 | # Author(s): Peter Fankhauser, Christian Gehring 37 | # Date : Sep 9, 2013 38 | 39 | cmake_minimum_required(VERSION 2.8.3) 40 | project(ooqp_eigen_interface) 41 | 42 | add_definitions(-std=c++11) 43 | 44 | find_package(Eigen3 REQUIRED) 45 | find_package(catkin REQUIRED COMPONENTS 46 | ooqp_catkin 47 | ) 48 | 49 | catkin_package( 50 | INCLUDE_DIRS 51 | include 52 | ${EIGEN3_INCLUDE_DIR} 53 | LIBRARIES 54 | ooqpei 55 | CATKIN_DEPENDS 56 | ooqp_catkin 57 | DEPENDS 58 | ) 59 | 60 | include_directories(include) 61 | include_directories(${EIGEN3_INCLUDE_DIR}) 62 | include_directories(${catkin_INCLUDE_DIRS}) 63 | include_directories(${ooqp_catkin_INCLUDE_DIRS}/ooqp_catkin) 64 | 65 | add_library(ooqpei 66 | src/OoqpEigenInterface.cpp 67 | src/QuadraticProblemFormulation.cpp 68 | ) 69 | 70 | target_link_libraries(ooqpei 71 | ${catkin_LIBRARIES} 72 | ) 73 | 74 | install(TARGETS ooqpei 75 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 76 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 77 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 78 | ) 79 | 80 | install(DIRECTORY include/${PROJECT_NAME}/ 81 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 82 | ) 83 | 84 | catkin_add_gtest(test_ooqpei 85 | test/test_main.cpp 86 | test/OoqpEigenInterface_test.cpp 87 | test/QuadraticProblemFormulation_test.cpp 88 | ) 89 | target_link_libraries(test_ooqpei 90 | ooqpei 91 | ${catkin_LIBRARIES} 92 | ) 93 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2014, Péter Fankhauser, Christian Gehring, Stelian Coros 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of Autonomous Systems Lab nor ETH Zurich 17 | nor the names of its contributors may be used to endorse or 18 | promote products derived from this software without specific 19 | prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | OOQP-Eigen Interface 2 | ====================== 3 | 4 | Overview 5 | --------------- 6 | 7 | This C++ software provides a simple interface to use [OOQP] with the [Eigen] linear algebra library. 8 | 9 | *The Object Oriented Quadratic Programming solver package ([OOQP]) is written by Mike Gertz and Steve Wright from the University of Chicago ([copyright notice](http://pages.cs.wisc.edu/~swright/ooqp/COPYRIGHT.html)). This software is an independent interface to the OOQP library and is not affiliated with OOQP in any way.* 10 | 11 | **Authors: Péter Fankhauser, Christian Gehring, Stelian Coros**
12 | Contact: Péter Fankhauser, pfankhauser@ethz.ch
13 | Affiliation: Autonomous Systems Lab, ETH Zurich 14 | 15 | [![Build Status](https://ci.leggedrobotics.com/buildStatus/icon?job=github_ethz-asl/ooqp_eigen_interface/master)](https://ci.leggedrobotics.com/job/github_ethz-asl/job/ooqp_eigen_interface/job/master/) 16 | 17 | ### Links 18 | 19 | * [OOQP User Guide](http://pages.cs.wisc.edu/~swright/ooqp/ooqp-userguide.pdf) 20 | * [OOQP Paper](http://pages.cs.wisc.edu/~swright/ooqp/ooqp-paper.pdf) 21 | * The MA27 documentation is under `ma27-1.0.0.tar.gz -> /doc/ma27_Fortran.pdf` 22 | 23 | 24 | Installation 25 | ------------ 26 | 27 | ### Dependencies 28 | 29 | - [OOQP]: Object-oriented software for quadratic programming, 30 | - [Eigen]: Linear algebra library. 31 | 32 | ### Building 33 | 34 | This version uses [Catkin] as build system. There is a [CMake] version in the [cmake](https://github.com/ethz-asl/ooqp_eigen_interface/tree/cmake) branch. 35 | 36 | To build, clone the latest version from this repository into your catkin workspace and compile the package using 37 | 38 | cd catkin_ws/src 39 | git clone https://github.com/ethz-asl/ooqp_eigen_interface.git 40 | cd ../ 41 | catkin_make 42 | 43 | 44 | Usage 45 | ------------ 46 | 47 | TODO: Add an example project. 48 | 49 | 50 | Unit Tests 51 | ------------ 52 | 53 | Run the unit tests with 54 | 55 | catkin_make run_tests_ooqp_eigen_interface 56 | 57 | 58 | Bugs & Feature Requests 59 | ------------ 60 | 61 | Please report bugs and request features using the [Issue Tracker](https://github.com/ethz-asl/ooqp-eigen_interface/issues). 62 | 63 | 64 | [OOQP]: http://pages.cs.wisc.edu/~swright/ooqp/ 65 | [Eigen]: http://eigen.tuxfamily.org 66 | [Catkin]: http://wiki.ros.org/catkin 67 | [CMake]: http://www.cmake.org/ 68 | -------------------------------------------------------------------------------- /cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN_FOUND - system has eigen lib with correct version 10 | # EIGEN_INCLUDE_DIR - the eigen include directory 11 | # EIGEN_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen_FIND_VERSION) 19 | if(NOT Eigen_FIND_VERSION_MAJOR) 20 | set(Eigen_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen_FIND_VERSION_MAJOR) 22 | if(NOT Eigen_FIND_VERSION_MINOR) 23 | set(Eigen_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen_FIND_VERSION_MINOR) 25 | if(NOT Eigen_FIND_VERSION_PATCH) 26 | set(Eigen_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen_FIND_VERSION_PATCH) 28 | 29 | set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) 43 | if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 44 | set(EIGEN_VERSION_OK FALSE) 45 | else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 46 | set(EIGEN_VERSION_OK TRUE) 47 | endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 48 | 49 | if(NOT EIGEN_VERSION_OK) 50 | 51 | message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " 52 | "but at least version ${Eigen_FIND_VERSION} is required") 53 | endif(NOT EIGEN_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN_INCLUDE_DIRS) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN_FOUND ${EIGEN_VERSION_OK}) 61 | 62 | else () 63 | 64 | find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN_INCLUDE_DIR) 79 | SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") 80 | 81 | endif() 82 | 83 | 84 | -------------------------------------------------------------------------------- /include/ooqp_eigen_interface/OoqpEigenInterface.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, Péter Fankhauser, Christian Gehring, Stelian Coros 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Autonomous Systems Lab nor ETH Zurich 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | /*! 37 | * @file OoqpEigenInterface.hpp 38 | * @author Péter Fankhauser, Christian Gehring 39 | * @date Aug 13, 2013 40 | * @brief Uses the Object Oriented QP solver package (OOQP) to solve 41 | * convex quadratic optimization problems of the type: 42 | * Find x: min 1/2 x' Q x + c' x such that A x = b, d <= Cx <= f, and l <= x <= u 43 | * where Q is symmetric positive semidefinite (nxn), x is a vector (nx1), 44 | * A and C are (possibly null) matrices and b and d are vectors of appropriate dimensions. 45 | * We are using sparse matrices in the Harwell-Boeing row-major format. 46 | * Adapted from 'simulationandcontrol' by Stelian Coros. 47 | */ 48 | 49 | #pragma once 50 | 51 | #include 52 | #include 53 | 54 | namespace ooqpei { 55 | 56 | class OoqpEigenInterface 57 | { 58 | public: 59 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 60 | 61 | /*! 62 | * Solve min 1/2 x' Q x + c' x, such that A x = b, d <= Cx <= f, and l <= x <= u. 63 | * @param [in] Q a symmetric positive semidefinite matrix (nxn) 64 | * @param [in] c a vector (nx1) 65 | * @param [in] A a (possibly null) matrices (m_axn) 66 | * @param [in] b a vector (m_ax1) 67 | * @param [in] C a (possibly null) matrices (m_cxn) 68 | * @param [in] d a vector (m_cx1) 69 | * @param [in] f a vector (m_cx1) 70 | * @param [in] l a vector (nx1) 71 | * @param [in] u a vector (nx1) 72 | * @param [out] x a vector of variables (nx1) 73 | * @return true if successful 74 | */ 75 | static bool solve(const Eigen::SparseMatrix& Q, 76 | const Eigen::VectorXd& c, 77 | const Eigen::SparseMatrix& A, 78 | const Eigen::VectorXd& b, 79 | const Eigen::SparseMatrix& C, 80 | const Eigen::VectorXd& d, const Eigen::VectorXd& f, 81 | const Eigen::VectorXd& l, const Eigen::VectorXd& u, 82 | Eigen::VectorXd& x, 83 | const bool ignoreUnknownError = false); 84 | 85 | /*! 86 | * Solve min 1/2 x' Q x + c' x, such that A x = b, and d <= Cx <= f 87 | * @param [in] Q a symmetric positive semidefinite matrix (nxn) 88 | * @param [in] c a vector (nx1) 89 | * @param [in] A a (possibly null) matrices (m_axn) 90 | * @param [in] b a vector (m_ax1) 91 | * @param [in] C a (possibly null) matrices (m_cxn) 92 | * @param [in] d a vector (m_cx1) 93 | * @param [in] f a vector (m_cx1) 94 | * @param [out] x a vector of variables (nx1) 95 | * @return true if successful 96 | */ 97 | static bool solve(const Eigen::SparseMatrix& Q, 98 | const Eigen::VectorXd& c, 99 | const Eigen::SparseMatrix& A, 100 | const Eigen::VectorXd& b, 101 | const Eigen::SparseMatrix& C, 102 | const Eigen::VectorXd& d, const Eigen::VectorXd& f, 103 | Eigen::VectorXd& x, 104 | const bool ignoreUnknownError = false); 105 | 106 | /*! 107 | * Solve min 1/2 x' Q x + c' x, such that A x = b, and l <= x <= u. 108 | * @param [in] Q a symmetric positive semidefinite matrix (nxn) 109 | * @param [in] c a vector (nx1) 110 | * @param [in] A a (possibly null) matrices (m_axn) 111 | * @param [in] b a vector (m_ax1) 112 | * @param [in] l a vector (nx1) 113 | * @param [in] u a vector (nx1) 114 | * @param [out] x a vector of variables (nx1) 115 | * @return true if successful 116 | */ 117 | static bool solve(const Eigen::SparseMatrix& Q, 118 | const Eigen::VectorXd& c, 119 | const Eigen::SparseMatrix& A, 120 | const Eigen::VectorXd& b, 121 | const Eigen::VectorXd& l, const Eigen::VectorXd& u, 122 | Eigen::VectorXd& x, 123 | const bool ignoreUnknownError = false); 124 | 125 | /*! 126 | * Solve min 1/2 x' Q x + c' x, such that Cx <= f 127 | * @param [in] Q a symmetric positive semidefinite matrix (nxn) 128 | * @param [in] c a vector (nx1) 129 | * @param [in] C a (possibly null) matrices (m_cxn) 130 | * @param [in] f a vector (m_cx1) 131 | * @param [out] x a vector of variables (nx1) 132 | * @return true if successful 133 | */ 134 | static bool solve(const Eigen::SparseMatrix& Q, 135 | const Eigen::VectorXd& c, 136 | const Eigen::SparseMatrix& C, 137 | const Eigen::VectorXd& f, 138 | Eigen::VectorXd& x, 139 | const bool ignoreUnknownError = false); 140 | 141 | /*! 142 | * Solve min 1/2 x' Q x + c' x 143 | * @param [in] Q a symmetric positive semidefinite matrix (nxn) 144 | * @param [in] c a vector (nx1) 145 | * @param [out] x a vector of variables (nx1) 146 | * @return true if successful 147 | */ 148 | static bool solve(const Eigen::SparseMatrix& Q, 149 | const Eigen::VectorXd& c, 150 | Eigen::VectorXd& x, 151 | const bool ignoreUnknownError = false); 152 | 153 | /*! 154 | * Change to true to print debug information. 155 | * @return true if in debug mode 156 | */ 157 | static bool isInDebugMode() { return isInDebugMode_; }; 158 | static void setIsInDebugMode(bool isInDebugMode) { 159 | isInDebugMode_ = isInDebugMode; 160 | } 161 | 162 | private: 163 | /*! 164 | * Determine which limits are active and which are not. 165 | * @param [in] l 166 | * @param [in] u 167 | * @param [out] useLowerLimit 168 | * @param [out] useUpperLimit 169 | * @param [out] lowerLimit 170 | * @param [out] upperLimit 171 | */ 172 | static void generateLimits(const Eigen::VectorXd& l, const Eigen::VectorXd& u, 173 | Eigen::Matrix& useLowerLimit, 174 | Eigen::Matrix& useUpperLimit, 175 | Eigen::VectorXd& lowerLimit, Eigen::VectorXd& upperLimit); 176 | 177 | static void printProblemFormulation( 178 | const Eigen::SparseMatrix& Q, const Eigen::VectorXd& c, 179 | const Eigen::SparseMatrix& A, const Eigen::VectorXd& b, 180 | const Eigen::SparseMatrix& C, const Eigen::VectorXd& d, const Eigen::VectorXd& f, 181 | const Eigen::VectorXd& l, const Eigen::VectorXd& u); 182 | 183 | static void printLimits(const Eigen::Matrix& useLowerLimit, 184 | const Eigen::Matrix& useUpperLimit, 185 | const Eigen::VectorXd& lowerLimit, 186 | const Eigen::VectorXd& upperLimit); 187 | 188 | static void printSolution(const int status, const Eigen::VectorXd& x); 189 | 190 | private: 191 | static bool isInDebugMode_; 192 | }; 193 | 194 | } /* namespace ooqpei */ 195 | -------------------------------------------------------------------------------- /include/ooqp_eigen_interface/QuadraticProblemFormulation.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, Péter Fankhauser, Christian Gehring, Stelian Coros 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Autonomous Systems Lab nor ETH Zurich 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | /*! 37 | * @file QuadraticProblemFormulation.hpp 38 | * @author Péter Fankhauser 39 | * @date Aug 16, 2013 40 | */ 41 | 42 | #pragma once 43 | 44 | #include 45 | #include 46 | 47 | namespace ooqpei { 48 | 49 | class QuadraticProblemFormulation 50 | { 51 | public: 52 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | 54 | /*! 55 | * Finds x that minimizes f = (Ax-b)' S (Ax-b) + x' W x, such that Cx = c and d <= Dx <= f 56 | * @param [in] A a matrix (mxn) 57 | * @param [in] S a diagonal weighting matrix (mxm) 58 | * @param [in] b a vector (mx1) 59 | * @param [in] W a diagonal weighting matrix (nxn) 60 | * @param [in] C a (possibly null) matrix (m_cxn) 61 | * @param [in] c a vector (m_cx1) 62 | * @param [in] D a (possibly null) matrix (m_dxn) 63 | * @param [in] d a vector (m_dx1) 64 | * @param [in] f a vector (m_dx1) 65 | * @param [out] x a vector (nx1) 66 | * @return true if successful 67 | */ 68 | static bool solve(const Eigen::SparseMatrix& A, 69 | const Eigen::DiagonalMatrix& S, const Eigen::VectorXd& b, 70 | const Eigen::DiagonalMatrix& W, 71 | const Eigen::SparseMatrix& C, const Eigen::VectorXd& c, 72 | const Eigen::SparseMatrix& D, 73 | const Eigen::VectorXd& d, const Eigen::VectorXd& f, 74 | Eigen::VectorXd& x); 75 | 76 | /*! 77 | * Finds x that minimizes f = (Ax-b)' S (Ax-b) + x' W x, such that d <= Dx <= f 78 | * @param [in] A a matrix (mxn) 79 | * @param [in] S a diagonal weighting matrix (mxm) 80 | * @param [in] b a vector (mx1) 81 | * @param [in] W a diagonal weighting matrix (nxn) 82 | * @param [in] D a (possibly null) matrix (m_dxn) 83 | * @param [in] d a vector (m_dx1) 84 | * @param [in] f a vector (m_dx1) 85 | * @param [out] x a vector (nx1) 86 | * @return true if successful 87 | */ 88 | static bool solve(const Eigen::SparseMatrix& A, 89 | const Eigen::DiagonalMatrix& S, const Eigen::VectorXd& b, 90 | const Eigen::DiagonalMatrix& W, 91 | const Eigen::SparseMatrix& D, 92 | const Eigen::VectorXd& d, const Eigen::VectorXd& f, 93 | Eigen::VectorXd& x); 94 | }; 95 | 96 | } /* namespace ooqpei */ 97 | -------------------------------------------------------------------------------- /include/ooqp_eigen_interface/ooqpei_assert_macros.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ooqpei_assert_macros.hpp 3 | * @author Paul Furgale 4 | * @date Mon Dec 12 11:22:20 2011 5 | * 6 | * @brief Assert macros to facilitate rapid prototyping. Use them early and often. 7 | * 8 | * 9 | */ 10 | 11 | #ifndef OOQPEI_ASSERT_MACROS_HPP_ 12 | #define OOQPEI_ASSERT_MACROS_HPP_ 13 | 14 | 15 | #include 16 | #include 17 | #include 18 | #include "ooqpei_source_file_pos.hpp" 19 | 20 | //! Macro for defining an exception with a given parent 21 | // (std::runtime_error should be top parent) 22 | // adapted from ros/drivers/laser/hokuyo_driver/hokuyo.h 23 | #define OOQPEI_DEFINE_EXCEPTION(exceptionName, exceptionParent) \ 24 | class exceptionName : public exceptionParent { \ 25 | public: \ 26 | exceptionName(const char * message) : exceptionParent(message) {} \ 27 | exceptionName(std::string const & message) : exceptionParent(message) {} \ 28 | virtual ~exceptionName() throw() {} \ 29 | }; 30 | 31 | 32 | namespace ooqpei { 33 | namespace common { 34 | namespace internal { 35 | 36 | template 37 | inline void ooqpei_throw_exception(std::string const & exceptionType, ooqpei::source_file_pos sfp, std::string const & message) 38 | { 39 | std::stringstream ooqpei_assert_stringstream; 40 | #ifdef _WIN32 41 | // I have no idea what broke this on Windows but it doesn't work with the << operator. 42 | ooqpei_assert_stringstream << exceptionType << sfp.toString() << " " << message; 43 | #else 44 | ooqpei_assert_stringstream << exceptionType << sfp << " " << message; 45 | #endif 46 | throw(OOQPEI_EXCEPTION_T(ooqpei_assert_stringstream.str())); 47 | } 48 | 49 | template 50 | inline void ooqpei_throw_exception(std::string const & exceptionType, std::string const & function, std::string const & file, 51 | int line, std::string const & message) 52 | { 53 | ooqpei_throw_exception(exceptionType, ooqpei::source_file_pos(function,file,line),message); 54 | } 55 | 56 | 57 | } // namespace internal 58 | 59 | template 60 | inline void ooqpei_assert_throw(bool assert_condition, std::string message, ooqpei::source_file_pos sfp) { 61 | if(!assert_condition) 62 | { 63 | internal::ooqpei_throw_exception("", sfp,message); 64 | } 65 | } 66 | 67 | 68 | } // namespace common 69 | } // namespace rm 70 | 71 | 72 | 73 | 74 | #define OOQPEI_THROW(exceptionType, message) { \ 75 | std::stringstream ooqpei_assert_stringstream; \ 76 | ooqpei_assert_stringstream << message; \ 77 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, ooqpei_assert_stringstream.str()); \ 78 | } 79 | 80 | 81 | #define OOQPEI_THROW_SFP(exceptionType, SourceFilePos, message){ \ 82 | std::stringstream ooqpei_assert_stringstream; \ 83 | ooqpei_assert_stringstream << message; \ 84 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", SourceFilePos, ooqpei_assert_stringstream.str()); \ 85 | } 86 | 87 | #define OOQPEI_ASSERT_TRUE(exceptionType, condition, message) \ 88 | if(!(condition)) \ 89 | { \ 90 | std::stringstream ooqpei_assert_stringstream; \ 91 | ooqpei_assert_stringstream << "assert(" << #condition << ") failed: " << message; \ 92 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, ooqpei_assert_stringstream.str()); \ 93 | } 94 | 95 | #define OOQPEI_ASSERT_FALSE(exceptionType, condition, message) \ 96 | if((condition)) \ 97 | { \ 98 | std::stringstream ooqpei_assert_stringstream; \ 99 | ooqpei_assert_stringstream << "assert( not " << #condition << ") failed: " << message; \ 100 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, ooqpei_assert_stringstream.str()); \ 101 | } 102 | 103 | 104 | 105 | #define OOQPEI_ASSERT_GE_LT(exceptionType, value, lowerBound, upperBound, message) \ 106 | if((value) < (lowerBound) || (value) >= (upperBound)) \ 107 | { \ 108 | std::stringstream ooqpei_assert_stringstream; \ 109 | ooqpei_assert_stringstream << "assert(" << #lowerBound << " <= " << #value << " < " << #upperBound << ") failed [" << (lowerBound) << " <= " << (value) << " < " << (upperBound) << "]: " << message; \ 110 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 111 | } 112 | 113 | 114 | 115 | #define OOQPEI_ASSERT_LT(exceptionType, value, upperBound, message) \ 116 | if((value) >= (upperBound)) \ 117 | { \ 118 | std::stringstream ooqpei_assert_stringstream; \ 119 | ooqpei_assert_stringstream << "assert(" << #value << " < " << #upperBound << ") failed [" << (value) << " < " << (upperBound) << "]: " << message; \ 120 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 121 | } 122 | 123 | #define OOQPEI_ASSERT_GE(exceptionType, value, lowerBound, message) \ 124 | if((value) < (lowerBound)) \ 125 | { \ 126 | std::stringstream ooqpei_assert_stringstream; \ 127 | ooqpei_assert_stringstream << "assert(" << #value << " >= " << #lowerBound << ") failed [" << (value) << " >= " << (lowerBound) << "]: " << message; \ 128 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 129 | } 130 | 131 | 132 | 133 | #define OOQPEI_ASSERT_LE(exceptionType, value, upperBound, message) \ 134 | if((value) > (upperBound)) \ 135 | { \ 136 | std::stringstream ooqpei_assert_stringstream; \ 137 | ooqpei_assert_stringstream << "assert(" << #value << " <= " << #upperBound << ") failed [" << (value) << " <= " << (upperBound) << "]: " << message; \ 138 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 139 | } 140 | 141 | #define OOQPEI_ASSERT_GT(exceptionType, value, lowerBound, message) \ 142 | if((value) <= (lowerBound)) \ 143 | { \ 144 | std::stringstream ooqpei_assert_stringstream; \ 145 | ooqpei_assert_stringstream << "assert(" << #value << " > " << #lowerBound << ") failed [" << (value) << " > " << (lowerBound) << "]: " << message; \ 146 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 147 | } 148 | 149 | 150 | 151 | #define OOQPEI_ASSERT_EQ(exceptionType, value, testValue, message) \ 152 | if((value) != (testValue)) \ 153 | { \ 154 | std::stringstream ooqpei_assert_stringstream; \ 155 | ooqpei_assert_stringstream << "assert(" << #value << " == " << #testValue << ") failed [" << (value) << " == " << (testValue) << "]: " << message; \ 156 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 157 | } 158 | 159 | #define OOQPEI_ASSERT_NE(exceptionType, value, testValue, message) \ 160 | if((value) == (testValue)) \ 161 | { \ 162 | std::stringstream ooqpei_assert_stringstream; \ 163 | ooqpei_assert_stringstream << "assert(" << #value << " != " << #testValue << ") failed [" << (value) << " != " << (testValue) << "]: " << message; \ 164 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 165 | } 166 | 167 | #define OOQPEI_ASSERT_NEAR(exceptionType, value, testValue, abs_error, message) \ 168 | if(!(fabs((testValue) - (value)) <= fabs(abs_error))) \ 169 | { \ 170 | std::stringstream ooqpei_assert_stringstream; \ 171 | ooqpei_assert_stringstream << "assert(" << #value << " == " << #testValue << ") failed [" << (value) << " == " << (testValue) << " (" << fabs((testValue) - (value)) << " > " << fabs(abs_error) << ")]: " << message; \ 172 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 173 | } 174 | 175 | 176 | 177 | #ifndef NDEBUG 178 | 179 | #define OOQPEI_THROW_DBG(exceptionType, message){ \ 180 | std::stringstream ooqpei_assert_stringstream; \ 181 | ooqpei_assert_stringstream << message; \ 182 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, ooqpei_assert_stringstream.str()); \ 183 | } 184 | 185 | 186 | 187 | #define OOQPEI_ASSERT_TRUE_DBG(exceptionType, condition, message) \ 188 | if(!(condition)) \ 189 | { \ 190 | std::stringstream ooqpei_assert_stringstream; \ 191 | ooqpei_assert_stringstream << "debug assert(" << #condition << ") failed: " << message; \ 192 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, ooqpei_assert_stringstream.str()); \ 193 | } 194 | 195 | #define OOQPEI_ASSERT_FALSE_DBG(exceptionType, condition, message) \ 196 | if((condition)) \ 197 | { \ 198 | std::stringstream ooqpei_assert_stringstream; \ 199 | ooqpei_assert_stringstream << "debug assert( not " << #condition << ") failed: " << message; \ 200 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__, ooqpei_assert_stringstream.str()); \ 201 | } 202 | 203 | 204 | #define OOQPEI_ASSERT_DBG_RE( condition, message) OOQPEI_ASSERT_DBG(std::runtime_error, condition, message) 205 | 206 | #define OOQPEI_ASSERT_GE_LT_DBG(exceptionType, value, lowerBound, upperBound, message) \ 207 | if((value) < (lowerBound) || (value) >= (upperBound)) \ 208 | { \ 209 | std::stringstream ooqpei_assert_stringstream; \ 210 | ooqpei_assert_stringstream << "debug assert(" << #lowerBound << " <= " << #value << " < " << #upperBound << ") failed [" << (lowerBound) << " <= " << (value) << " < " << (upperBound) << "]: " << message; \ 211 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 212 | } 213 | 214 | 215 | 216 | #define OOQPEI_ASSERT_LT_DBG(exceptionType, value, upperBound, message) \ 217 | if((value) >= (upperBound)) \ 218 | { \ 219 | std::stringstream ooqpei_assert_stringstream; \ 220 | ooqpei_assert_stringstream << "debug assert(" << #value << " < " << #upperBound << ") failed [" << (value) << " < " << (upperBound) << "]: " << message; \ 221 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 222 | } 223 | 224 | 225 | 226 | #define OOQPEI_ASSERT_GE_DBG(exceptionType, value, lowerBound, message) \ 227 | if((value) < (lowerBound)) \ 228 | { \ 229 | std::stringstream ooqpei_assert_stringstream; \ 230 | ooqpei_assert_stringstream << "debug assert(" << #value << " >= " << #lowerBound << ") failed [" << (value) << " >= " << (lowerBound) << "]: " << message; \ 231 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 232 | } 233 | 234 | 235 | 236 | #define OOQPEI_ASSERT_LE_DBG(exceptionType, value, upperBound, message) \ 237 | if((value) > (upperBound)) \ 238 | { \ 239 | std::stringstream ooqpei_assert_stringstream; \ 240 | ooqpei_assert_stringstream << "debug assert(" << #value << " <= " << #upperBound << ") failed [" << (value) << " <= " << (upperBound) << "]: " << message; \ 241 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 242 | } 243 | 244 | #define OOQPEI_ASSERT_GT_DBG(exceptionType, value, lowerBound, message) \ 245 | if((value) <= (lowerBound)) \ 246 | { \ 247 | std::stringstream ooqpei_assert_stringstream; \ 248 | ooqpei_assert_stringstream << "debug assert(" << #value << " > " << #lowerBound << ") failed [" << (value) << " > " << (lowerBound) << "]: " << message; \ 249 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 250 | } 251 | 252 | 253 | 254 | #define OOQPEI_ASSERT_EQ_DBG(exceptionType, value, testValue, message) \ 255 | if((value) != (testValue)) \ 256 | { \ 257 | std::stringstream ooqpei_assert_stringstream; \ 258 | ooqpei_assert_stringstream << "debug assert(" << #value << " == " << #testValue << ") failed [" << (value) << " == " << (testValue) << "]: " << message; \ 259 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 260 | } 261 | 262 | 263 | #define OOQPEI_ASSERT_NE_DBG(exceptionType, value, testValue, message) \ 264 | if((value) == (testValue)) \ 265 | { \ 266 | std::stringstream ooqpei_assert_stringstream; \ 267 | ooqpei_assert_stringstream << "debug assert(" << #value << " != " << #testValue << ") failed [" << (value) << " != " << (testValue) << "]: " << message; \ 268 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 269 | } 270 | 271 | 272 | 273 | #define OOQPEI_ASSERT_NEAR_DBG(exceptionType, value, testValue, abs_error, message) \ 274 | if(!(fabs((testValue) - (value)) <= fabs(abs_error))) \ 275 | { \ 276 | std::stringstream ooqpei_assert_stringstream; \ 277 | ooqpei_assert_stringstream << "debug assert(" << #value << " == " << #testValue << ") failed [" << (value) << " == " << (testValue) << " (" << fabs((testValue) - (value)) << " > " << fabs(abs_error) << ")]: " << message; \ 278 | ooqpei::common::internal::ooqpei_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,ooqpei_assert_stringstream.str()); \ 279 | } 280 | 281 | 282 | #define OOQPEI_OUT(X) std::cout << #X << ": " << (X) << std::endl 283 | 284 | #else 285 | 286 | #define OOQPEI_OUT(X) 287 | #define OOQPEI_THROW_DBG(exceptionType, message) 288 | #define OOQPEI_ASSERT_TRUE_DBG(exceptionType, condition, message) 289 | #define OOQPEI_ASSERT_FALSE_DBG(exceptionType, condition, message) 290 | #define OOQPEI_ASSERT_GE_LT_DBG(exceptionType, value, lowerBound, upperBound, message) 291 | #define OOQPEI_ASSERT_LT_DBG(exceptionType, value, upperBound, message) 292 | #define OOQPEI_ASSERT_GT_DBG(exceptionType, value, lowerBound, message) 293 | #define OOQPEI_ASSERT_LE_DBG(exceptionType, value, upperBound, message) 294 | #define OOQPEI_ASSERT_GE_DBG(exceptionType, value, lowerBound, message) 295 | #define OOQPEI_ASSERT_NE_DBG(exceptionType, value, testValue, message) 296 | #define OOQPEI_ASSERT_EQ_DBG(exceptionType, value, testValue, message) 297 | #define OOQPEI_ASSERT_NEAR_DBG(exceptionType, value, testValue, abs_error, message) 298 | #endif 299 | 300 | #endif /* OOQPEI_ASSERT_MACROS_HPP_ */ 301 | -------------------------------------------------------------------------------- /include/ooqp_eigen_interface/ooqpei_gtest_eigen.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file gtest.hpp 3 | * @author Paul Furgale 4 | * @date Mon Dec 12 11:54:20 2011 5 | * 6 | * @brief Code to simplify Eigen integration into GTest. Pretty basic but the error messages are good. 7 | * 8 | * 9 | */ 10 | #ifndef OOQPEI_EIGEN_GTEST_HPP 11 | #define OOQPEI_EIGEN_GTEST_HPP 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace ooqpei { namespace eigen { 18 | 19 | 20 | 21 | template 22 | Eigen::Matrix randomCovariance() 23 | { 24 | Eigen::Matrix U; 25 | U.setRandom(); 26 | return U.transpose() * U + 5.0 * Eigen::Matrix::Identity(); 27 | } 28 | 29 | 30 | inline Eigen::MatrixXd randomCovarianceXd(int N) 31 | { 32 | Eigen::MatrixXd U(N,N); 33 | U.setRandom(); 34 | return U.transpose() * U + 5.0 * Eigen::MatrixXd::Identity(N,N); 35 | } 36 | 37 | 38 | 39 | template 40 | void assertEqual(const M1 & A, const M2 & B, ooqpei::source_file_pos const & sfp, std::string const & message = "") 41 | { 42 | ASSERT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString(); 43 | ASSERT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString(); 44 | 45 | for(int r = 0; r < A.rows(); r++) 46 | { 47 | for(int c = 0; c < A.cols(); c++) 48 | { 49 | ASSERT_EQ(A(r,c),B(r,c)) << message << "\nEquality comparison failed at (" << r << "," << c << ")\n" << sfp.toString() 50 | << "\nMatrix A:\n" << A << "\nand matrix B\n" << B; 51 | } 52 | } 53 | } 54 | 55 | 56 | template 57 | void assertNear(const M1 & A, const M2 & B, T tolerance, ooqpei::source_file_pos const & sfp, std::string const & message = "") 58 | { 59 | // Note: If these assertions fail, they only abort this subroutine. 60 | // see: http://code.google.com/p/googletest/wiki/AdvancedGuide#Using_Assertions_in_Sub-routines 61 | // \todo better handling of this 62 | ASSERT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString(); 63 | ASSERT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString(); 64 | 65 | for(int r = 0; r < A.rows(); r++) 66 | { 67 | for(int c = 0; c < A.cols(); c++) 68 | { 69 | ASSERT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n" << sfp.toString() 70 | << "\nMatrix A:\n" << A << "\nand matrix B\n" << B; 71 | } 72 | } 73 | } 74 | 75 | template 76 | void expectNear(const M1 & A, const M2 & B, T tolerance, ooqpei::source_file_pos const & sfp, std::string const & message = "") 77 | { 78 | EXPECT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString(); 79 | EXPECT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n" << sfp.toString(); 80 | 81 | for(int r = 0; r < A.rows(); r++) 82 | { 83 | for(int c = 0; c < A.cols(); c++) 84 | { 85 | EXPECT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n" << sfp.toString() 86 | << "\nMatrix A:\n" << A << "\nand matrix B\n" << B; 87 | } 88 | } 89 | } 90 | 91 | 92 | template 93 | void assertFinite(const M1 & A, ooqpei::source_file_pos const & sfp, std::string const & message = "") 94 | { 95 | for(int r = 0; r < A.rows(); r++) 96 | { 97 | for(int c = 0; c < A.cols(); c++) 98 | { 99 | ASSERT_TRUE(std::isfinite(A(r,c))) << sfp.toString() << std::endl << "Check for finite values failed at A(" << r << "," << c << "). Matrix A:" << std::endl << A << std::endl; 100 | } 101 | } 102 | } 103 | 104 | 105 | 106 | inline bool compareRelative(double a, double b, double percentTolerance, double * percentError = NULL) 107 | { 108 | // \todo: does anyone have a better idea? 109 | double fa = fabs(a); 110 | double fb = fabs(b); 111 | if( (fa < 1e-15 && fb < 1e-15) || // Both zero. 112 | (fa == 0.0 && fb < 1e-6) || // One exactly zero and the other small 113 | (fb == 0.0 && fa < 1e-6) ) // ditto 114 | return true; 115 | 116 | double diff = fabs(a - b)/std::max(fa,fb); 117 | if(diff > percentTolerance * 1e-2) 118 | { 119 | if(percentError) 120 | *percentError = diff * 100.0; 121 | return false; 122 | } 123 | return true; 124 | } 125 | 126 | 127 | #define ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \ 128 | ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \ 129 | ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \ 130 | for(int r = 0; r < (A).rows(); r++) \ 131 | { \ 132 | for(int c = 0; c < (A).cols(); c++) \ 133 | { \ 134 | double percentError = 0.0; \ 135 | ASSERT_TRUE(ooqpei::eigen::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) \ 136 | << MSG << "\nComparing:\n" \ 137 | << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \ 138 | << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \ 139 | << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \ 140 | << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \ 141 | } \ 142 | } 143 | 144 | 145 | #define ASSERT_DOUBLE_SPARSE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \ 146 | ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \ 147 | ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \ 148 | for(int r = 0; r < (A).rows(); r++) \ 149 | { \ 150 | for(int c = 0; c < (A).cols(); c++) \ 151 | { \ 152 | double percentError = 0.0; \ 153 | ASSERT_TRUE(ooqpei::eigen::compareRelative( (A).coeff(r,c), (B).coeff(r,c), PERCENT_TOLERANCE, &percentError)) \ 154 | << MSG << "\nComparing:\n" \ 155 | << #A << "(" << r << "," << c << ") = " << (A).coeff(r,c) << std::endl \ 156 | << #B << "(" << r << "," << c << ") = " << (B).coeff(r,c) << std::endl \ 157 | << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \ 158 | << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \ 159 | } \ 160 | } 161 | 162 | 163 | 164 | }} // namespace ooqpei::eigen 165 | 166 | #endif /* OOQPEI_EIGEN_GTEST_HPP */ 167 | -------------------------------------------------------------------------------- /include/ooqp_eigen_interface/ooqpei_numerical_comparisons.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * numerical_comparisons.hpp 3 | * 4 | * Created on: Aug 13, 2013 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #ifndef OOQPEI_NUMERICALCOMPARISON_HPP_ 10 | #define OOQPEI_NUMERICALCOMPARISON_HPP_ 11 | 12 | #include // std::max 13 | #include // std::numeric_limits 14 | #include // std::abs 15 | #include // std::invalid_argument 16 | #include "ooqpei_assert_macros.hpp" 17 | 18 | namespace ooqpei { 19 | 20 | /* 21 | * Functions to compare floating point numbers with a relative error (epsilon). 22 | * 23 | * As the precision of floating point numbers are limited and depending on the 24 | * magnitude, we compare two numbers a and b by comparing their difference |a - b| 25 | * to the magnitude of the the bigger number max(|a|, |b|) multiplied with the 26 | * relative error epsilon. Therefore, a and b are approximately equal if it holds 27 | * |a - b| <= max(|a|, |b|) * epsilon. This can be applied analogously to greater 28 | * and less than comparisons. 29 | * 30 | * More information on compare floating point numbers is given in 31 | * - http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ 32 | * - The art of computer programming, Volume 2 / Seminumerical Algorithms, Donald E. Knuth (page 218) 33 | */ 34 | 35 | namespace internal { 36 | 37 | /*! 38 | * Takes max(|a|, |b|) and multiplies it with epsilon. 39 | * @param a the first number. 40 | * @param b the second number. 41 | * @param epsilon the precision (epsilon > 0). 42 | * @return the result of max(|a|, |b|) * epsilon. 43 | */ 44 | template 45 | static inline ValueType_ maxTimesEpsilon(const ValueType_ a, const ValueType_ b, const ValueType_ epsilon) 46 | { 47 | OOQPEI_ASSERT_GT_DBG(std::invalid_argument, epsilon, 0.0, "This method is only valid for an epsilon greater than 0."); 48 | return std::max(std::abs(a), std::abs(b)) * epsilon; 49 | } 50 | 51 | } /* namespace internal */ 52 | 53 | /*! 54 | * Checks if two numbers a and b are equal within a relative error. 55 | * @param[in] a the first number to compare. 56 | * @param[in] b the second number to compare. 57 | * @param[in] epsilon the relative error (optional, if not declared the precision of the datatype). 58 | * @return true if a and b are approximately equal, false otherwise. 59 | */ 60 | template 61 | static bool approximatelyEqual(const ValueType_ a, const ValueType_ b, ValueType_ epsilon = std::numeric_limits::epsilon()) 62 | { 63 | return std::abs(a - b) <= internal::maxTimesEpsilon(a, b, epsilon); 64 | } 65 | 66 | /*! 67 | * Checks if a is greater than b (a > b) within a relative error. 68 | * @param a the first number to compare. 69 | * @param b the second number to compare. 70 | * @param epsilon the relative error (optional, if not declared the precision of the datatype). 71 | * @return true if a definitely greater than b, false otherwise. 72 | */ 73 | template 74 | static bool definitelyGreaterThan(const ValueType_ a, const ValueType_ b, ValueType_ epsilon = std::numeric_limits::epsilon()) 75 | { 76 | return (a - b) > internal::maxTimesEpsilon(a, b, epsilon); 77 | } 78 | 79 | /*! 80 | * Checks if a is less than b (a < b) within a relative error. 81 | * @param a the first number to compare. 82 | * @param b the second number to compare. 83 | * @param epsilon the relative error (optional, if not declared the precision of the datatype). 84 | * @return true if a definitely less than b, false otherwise. 85 | */ 86 | template 87 | static bool definitelyLessThan(const ValueType_ a, const ValueType_ b, ValueType_ epsilon = std::numeric_limits::epsilon()) 88 | { 89 | return (b - a) > internal::maxTimesEpsilon(a, b, epsilon); 90 | } 91 | 92 | } /* namespace ooqpei */ 93 | #endif /* OOQPEI_NUMERICALCOMPARISON_HPP_ */ 94 | -------------------------------------------------------------------------------- /include/ooqp_eigen_interface/ooqpei_source_file_pos.hpp: -------------------------------------------------------------------------------- 1 | // Copied from Paul Furgale's Schweizer-Messer 2 | 3 | #ifndef OOQPEI_SOURCE_FILE_POS_HPP 4 | #define OOQPEI_SOURCE_FILE_POS_HPP 5 | 6 | #include 7 | #include 8 | #include 9 | // A class and macro that gives you the current file position. 10 | 11 | namespace ooqpei { 12 | 13 | class source_file_pos 14 | { 15 | public: 16 | std::string function; 17 | std::string file; 18 | int line; 19 | 20 | source_file_pos(std::string function, std::string file, int line) : 21 | function(function), file(file), line(line) {} 22 | 23 | operator std::string() 24 | { 25 | return toString(); 26 | } 27 | 28 | std::string toString() const 29 | { 30 | std::stringstream s; 31 | s << file << ":" << line << ": " << function << "()"; 32 | return s.str(); 33 | } 34 | 35 | }; 36 | 37 | }// namespace ooqpei 38 | 39 | inline std::ostream & operator<<(std::ostream & out, const ooqpei::source_file_pos & sfp) 40 | { 41 | out << sfp.file << ":" << sfp.line << ": " << sfp.function << "()"; 42 | return out; 43 | } 44 | 45 | 46 | #define OOQPEI_SOURCE_FILE_POS ooqpei::source_file_pos(__FUNCTION__,__FILE__,__LINE__) 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /jenkins-pipeline: -------------------------------------------------------------------------------- 1 | library 'continuous_integration_pipeline' 2 | ciPipeline("") -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ooqp_eigen_interface 4 | 0.0.0 5 | ooqp_eigen_interface 6 | Péter Fankhauser 7 | BSD 8 | Péter Fankhauser 9 | Christian Gehring 10 | catkin 11 | 12 | eigen 13 | ooqp_catkin 14 | gtest 15 | 16 | -------------------------------------------------------------------------------- /src/OoqpEigenInterface.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, Péter Fankhauser, Christian Gehring, Stelian Coros 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Autonomous Systems Lab nor ETH Zurich 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | /*! 37 | * @file OoqpEigenInterface.cpp 38 | * @author Péter Fankhauser, Christian Gehring 39 | * @date Aug 13, 2013 40 | */ 41 | 42 | #include "ooqp_eigen_interface/OoqpEigenInterface.hpp" 43 | #include 44 | #include "ooqp_eigen_interface/ooqpei_assert_macros.hpp" 45 | #include "QpGenData.h" 46 | #include "QpGenVars.h" 47 | #include "QpGenResiduals.h" 48 | #include "GondzioSolver.h" 49 | #include "QpGenSparseMa27.h" 50 | #include "Status.h" 51 | #include "ooqp_eigen_interface/ooqpei_numerical_comparisons.hpp" 52 | 53 | using namespace Eigen; 54 | using namespace std; 55 | using namespace ooqpei; 56 | 57 | namespace ooqpei { 58 | 59 | bool OoqpEigenInterface::isInDebugMode_ = false; 60 | 61 | bool OoqpEigenInterface::solve(const Eigen::SparseMatrix& Q, 62 | const Eigen::VectorXd& c, 63 | const Eigen::SparseMatrix& A, 64 | const Eigen::VectorXd& b, 65 | const Eigen::SparseMatrix& C, 66 | const Eigen::VectorXd& d, const Eigen::VectorXd& f, 67 | const Eigen::VectorXd& l, const Eigen::VectorXd& u, 68 | Eigen::VectorXd& x, 69 | const bool ignoreUnknownError) 70 | { 71 | // Initialize. 72 | int nx = Q.rows(); // nx is the number of primal variables (x). 73 | OOQPEI_ASSERT_GT(range_error, nx, 0, "Matrix Q has size 0."); 74 | x.setZero(nx); 75 | 76 | // Make copies of variables that are changed. 77 | auto ccopy(c); 78 | auto Acopy(A); 79 | auto bcopy(b); 80 | auto Ccopy(C); 81 | 82 | // Make sure Q is in lower triangular form (Q is symmetric). 83 | // Refer to OOQP user guide section 2.2 (p. 11). 84 | // TODO Check if Q is really symmetric. 85 | SparseMatrix Q_triangular = Q.triangularView(); 86 | 87 | if (isInDebugMode()) printProblemFormulation(Q_triangular, ccopy, Acopy, bcopy, Ccopy, d, f, l, u); 88 | 89 | // Compress sparse Eigen matrices (refer to Eigen Sparse Matrix user manual). 90 | Q_triangular.makeCompressed(); 91 | Acopy.makeCompressed(); 92 | Ccopy.makeCompressed(); 93 | 94 | // Check matrices. 95 | OOQPEI_ASSERT_EQ(range_error, ccopy.size(), nx, "Vector c has wrong size."); 96 | OOQPEI_ASSERT_EQ(range_error, l.size(), nx, "Vector l has wrong size."); 97 | OOQPEI_ASSERT_EQ(range_error, u.size(), nx, "Vector u has wrong size."); 98 | OOQPEI_ASSERT_EQ(range_error, bcopy.size(), Acopy.rows(), "Vector b has wrong size."); 99 | if (Acopy.size() > 0) OOQPEI_ASSERT_EQ(range_error, Acopy.cols(), nx, "Matrix A has wrong size."); 100 | OOQPEI_ASSERT_EQ(range_error, d.size(), Ccopy.rows(), "Vector d has wrong size."); 101 | OOQPEI_ASSERT_EQ(range_error, f.size(), Ccopy.rows(), "Vector f has wrong size."); 102 | if (Ccopy.size() > 0) OOQPEI_ASSERT_EQ(range_error, Ccopy.cols(), nx, "Matrix C has wrong size."); 103 | 104 | // Determine which limits are active and which are not. 105 | // Refer to OOQP user guide section 2.2 (p. 10). 106 | Matrix useLowerLimitForX; 107 | Matrix useUpperLimitForX; 108 | VectorXd lowerLimitForX; 109 | VectorXd upperLimitForX; 110 | Matrix useLowerLimitForInequalityConstraints; 111 | Matrix useUpperLimitForInequalityConstraints; 112 | VectorXd lowerLimitForInequalityConstraints; 113 | VectorXd upperLimitForInequalityConstraints; 114 | 115 | generateLimits(l, u, useLowerLimitForX, useUpperLimitForX, 116 | lowerLimitForX, upperLimitForX); 117 | generateLimits(d, f, useLowerLimitForInequalityConstraints, 118 | useUpperLimitForInequalityConstraints, 119 | lowerLimitForInequalityConstraints, 120 | upperLimitForInequalityConstraints); 121 | 122 | if (isInDebugMode()) 123 | { 124 | cout << "-------------------------------" << endl; 125 | cout << "LIMITS FOR X" << endl; 126 | printLimits(useLowerLimitForX, useUpperLimitForX, lowerLimitForX, upperLimitForX); 127 | cout << "-------------------------------" << endl; 128 | cout << "LIMITS FOR INEQUALITY CONSTRAINTS" << endl; 129 | printLimits(useLowerLimitForInequalityConstraints, 130 | useUpperLimitForInequalityConstraints, 131 | lowerLimitForInequalityConstraints, 132 | upperLimitForInequalityConstraints); 133 | } 134 | 135 | // Setting up OOQP solver 136 | // Refer to OOQP user guide section 2.3 (p. 14). 137 | 138 | // Initialize new problem formulation. 139 | int my = bcopy.size(); 140 | int mz = lowerLimitForInequalityConstraints.size(); 141 | int nnzQ = Q_triangular.nonZeros(); 142 | int nnzA = Acopy.nonZeros(); 143 | int nnzC = Ccopy.nonZeros(); 144 | 145 | QpGenSparseMa27 * qp = new QpGenSparseMa27(nx, my, mz, nnzQ, nnzA, nnzC); 146 | 147 | // Fill in problem data. 148 | double* cp = &ccopy.coeffRef(0); 149 | int* krowQ = Q_triangular.outerIndexPtr(); 150 | int* jcolQ = Q_triangular.innerIndexPtr(); 151 | double* dQ = Q_triangular.valuePtr(); 152 | double* xlow = &lowerLimitForX.coeffRef(0); 153 | char* ixlow = &useLowerLimitForX.coeffRef(0); 154 | double* xupp = &upperLimitForX.coeffRef(0); 155 | char* ixupp = &useUpperLimitForX.coeffRef(0); 156 | int* krowA = Acopy.outerIndexPtr(); 157 | int* jcolA = Acopy.innerIndexPtr(); 158 | double* dA = Acopy.valuePtr(); 159 | double* bA = &bcopy.coeffRef(0); 160 | int* krowC = Ccopy.outerIndexPtr(); 161 | int* jcolC = Ccopy.innerIndexPtr(); 162 | double* dC = Ccopy.valuePtr(); 163 | double* clow = &lowerLimitForInequalityConstraints.coeffRef(0); 164 | char* iclow = &useLowerLimitForInequalityConstraints.coeffRef(0); 165 | double* cupp = &upperLimitForInequalityConstraints.coeffRef(0); 166 | char* icupp = &useUpperLimitForInequalityConstraints.coeffRef(0); 167 | 168 | QpGenData * prob = (QpGenData *) qp->makeData(cp, krowQ, jcolQ, dQ, 169 | xlow, ixlow, xupp, ixupp, 170 | krowA, jcolA, dA, bA, 171 | krowC, jcolC, dC, 172 | clow, iclow, cupp, icupp); 173 | 174 | // Create object to store problem variables. 175 | QpGenVars* vars = (QpGenVars*) qp->makeVariables(prob); 176 | // if (isInDebugMode()) prob->print(); // Matrices are printed as [index_x, index_y, value] 177 | 178 | // Create object to store problem residual data. 179 | QpGenResiduals* resid = (QpGenResiduals*) qp->makeResiduals(prob); 180 | 181 | // Create solver object. 182 | GondzioSolver* s = new GondzioSolver(qp, prob); 183 | if (isInDebugMode()) s->monitorSelf(); 184 | 185 | // Solve. 186 | int status = s->solve(prob, vars, resid); 187 | 188 | if ((status == SUCCESSFUL_TERMINATION) || (ignoreUnknownError && (status == UNKNOWN))) 189 | vars->x->copyIntoArray(&x.coeffRef(0)); 190 | 191 | if(isInDebugMode()) printSolution(status, x); 192 | //vars->x->writefToStream( cout, "x[%{index}] = %{value}" ); 193 | 194 | delete s; 195 | delete resid; 196 | delete vars; 197 | delete prob; 198 | delete qp; 199 | 200 | return ((status == SUCCESSFUL_TERMINATION) || (ignoreUnknownError && (status == UNKNOWN))); 201 | } 202 | 203 | bool OoqpEigenInterface::solve(const Eigen::SparseMatrix& Q, 204 | const Eigen::VectorXd& c, 205 | const Eigen::SparseMatrix& A, 206 | const Eigen::VectorXd& b, 207 | const Eigen::SparseMatrix& C, 208 | const Eigen::VectorXd& d, const Eigen::VectorXd& f, 209 | Eigen::VectorXd& x, 210 | const bool ignoreUnknownError) 211 | { 212 | int nx = Q.rows(); 213 | VectorXd u = std::numeric_limits::max() * VectorXd::Ones(nx); 214 | VectorXd l = (-u.array()).matrix(); 215 | return solve(Q, c, A, b, C, d, f, l, u, x, ignoreUnknownError); 216 | } 217 | 218 | bool OoqpEigenInterface::solve(const Eigen::SparseMatrix& Q, 219 | const Eigen::VectorXd& c, 220 | const Eigen::SparseMatrix& A, 221 | const Eigen::VectorXd& b, 222 | const Eigen::VectorXd& l, const Eigen::VectorXd& u, 223 | Eigen::VectorXd& x, 224 | const bool ignoreUnknownError) 225 | { 226 | SparseMatrix C; 227 | VectorXd d, f; 228 | return solve(Q, c, A, b, C, d, f, l, u, x, ignoreUnknownError); 229 | } 230 | 231 | bool OoqpEigenInterface::solve(const Eigen::SparseMatrix& Q, 232 | const Eigen::VectorXd& c, 233 | const Eigen::SparseMatrix& C, 234 | const Eigen::VectorXd& f, 235 | Eigen::VectorXd& x, 236 | const bool ignoreUnknownError) 237 | { 238 | SparseMatrix A; 239 | VectorXd b; 240 | VectorXd d = -std::numeric_limits::max() * VectorXd::Ones(C.rows()); 241 | return solve(Q, c, A, b, C, d, f, x, ignoreUnknownError); 242 | } 243 | 244 | bool OoqpEigenInterface::solve(const Eigen::SparseMatrix& Q, 245 | const Eigen::VectorXd& c, 246 | Eigen::VectorXd& x, 247 | const bool ignoreUnknownError) 248 | { 249 | SparseMatrix A, C; 250 | VectorXd b, d, f; 251 | return solve(Q, c, A, b, C, d, f, x, ignoreUnknownError); 252 | } 253 | 254 | void OoqpEigenInterface::generateLimits( 255 | const Eigen::VectorXd& l, const Eigen::VectorXd& u, 256 | Eigen::Matrix& useLowerLimit, 257 | Eigen::Matrix& useUpperLimit, 258 | Eigen::VectorXd& lowerLimit, Eigen::VectorXd& upperLimit) 259 | { 260 | int n = l.size(); 261 | useLowerLimit.setConstant(n, 1); 262 | useUpperLimit.setConstant(n, 1); 263 | lowerLimit = l; 264 | upperLimit = u; 265 | 266 | for (int i = 0; i < n; i++) 267 | { 268 | if (ooqpei::approximatelyEqual(l(i), -std::numeric_limits::max())) 269 | { 270 | useLowerLimit(i) = 0; 271 | lowerLimit(i) = 0.0; 272 | } 273 | if (ooqpei::approximatelyEqual(u(i), std::numeric_limits::max())) 274 | { 275 | useUpperLimit(i) = 0; 276 | upperLimit(i) = 0.0; 277 | } 278 | } 279 | } 280 | 281 | void OoqpEigenInterface::printProblemFormulation( 282 | const Eigen::SparseMatrix& Q, const Eigen::VectorXd& c, 283 | const Eigen::SparseMatrix& A, const Eigen::VectorXd& b, 284 | const Eigen::SparseMatrix& C, const Eigen::VectorXd& d, const Eigen::VectorXd& f, 285 | const Eigen::VectorXd& l, const Eigen::VectorXd& u) 286 | { 287 | cout << "-------------------------------" << endl; 288 | cout << "Find x: min 1/2 x' Q x + c' x such that A x = b, d <= Cx <= f, and l <= x <= u" << endl << endl; 289 | cout << "Q (triangular) << " << endl << MatrixXd(Q) << endl; 290 | cout << "c << " << c.transpose() << endl; 291 | cout << "A << " << endl << MatrixXd(A) << endl; 292 | cout << "b << " << b.transpose() << endl; 293 | cout << "C << " << endl << MatrixXd(C) << endl; 294 | cout << "d << " << d.transpose() << endl; 295 | cout << "f << " << f.transpose() << endl; 296 | cout << "l << " << l.transpose() << endl; 297 | cout << "u << " << u.transpose() << endl; 298 | } 299 | 300 | void OoqpEigenInterface::printLimits( 301 | const Eigen::Matrix& useLowerLimit, 302 | const Eigen::Matrix& useUpperLimit, 303 | const Eigen::VectorXd& lowerLimit, const Eigen::VectorXd& upperLimit) 304 | { 305 | cout << "useLowerLimit << " << std::boolalpha << useLowerLimit.cast().transpose() << endl; 306 | cout << "lowerLimit << " << lowerLimit.transpose() << endl; 307 | cout << "useUpperLimit << " << std::boolalpha << useUpperLimit.cast().transpose() << endl; 308 | cout << "upperLimit << " << upperLimit.transpose() << endl; 309 | } 310 | 311 | void OoqpEigenInterface::printSolution(const int status, const Eigen::VectorXd& x) 312 | { 313 | if (status == 0) 314 | { 315 | cout << "-------------------------------" << endl; 316 | cout << "SOLUTION" << endl; 317 | cout << "Ok, ended with status " << status << "."<< endl; 318 | cout << "x << " << x.transpose() << endl; 319 | } 320 | else 321 | { 322 | cout << "-------------------------------" << endl; 323 | cout << "SOLUTION" << endl; 324 | cout << "Error, ended with status " << status << "." << endl; 325 | } 326 | } 327 | 328 | } /* namespace ooqpei */ 329 | 330 | -------------------------------------------------------------------------------- /src/QuadraticProblemFormulation.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, Péter Fankhauser, Christian Gehring, Stelian Coros 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Autonomous Systems Lab nor ETH Zurich 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | /*! 37 | * @file QuadraticProblemFormulation.cpp 38 | * @author Péter Fankhauser, Christian Gehring 39 | * @date Aug 16, 2013 40 | */ 41 | 42 | #include "ooqp_eigen_interface/QuadraticProblemFormulation.hpp" 43 | #include "ooqp_eigen_interface/ooqpei_assert_macros.hpp" 44 | #include 45 | #include "ooqp_eigen_interface/OoqpEigenInterface.hpp" 46 | 47 | using namespace Eigen; 48 | using namespace std; 49 | 50 | namespace ooqpei { 51 | 52 | bool QuadraticProblemFormulation::solve( 53 | const Eigen::SparseMatrix& A, 54 | const Eigen::DiagonalMatrix& S, const Eigen::VectorXd& b, 55 | const Eigen::DiagonalMatrix& W, 56 | const Eigen::SparseMatrix& C, const Eigen::VectorXd& c, 57 | const Eigen::SparseMatrix& D, const Eigen::VectorXd& d, 58 | const Eigen::VectorXd& f, Eigen::VectorXd& x) 59 | { 60 | // f = (Ax-b)' S (Ax-b) + x' W x = x'A'SAx - 2x'A'Sb + b'Sb + x'Wx. 61 | // This means minimizing f is equivalent to minimizing: 1/2 x'Qx + c'x, 62 | // where Q = A'SA + W and c = -A'Sb. 63 | // Adapted from 'simulationandcontrol' by Stelian Coros. 64 | 65 | int m = A.rows(); 66 | int n = A.cols(); 67 | x.setZero(n); 68 | OOQPEI_ASSERT_EQ(range_error, b.size(), m, "Vector b has wrong size."); 69 | OOQPEI_ASSERT_EQ(range_error, S.rows(), m, "Matrix S has wrong size."); 70 | OOQPEI_ASSERT_EQ(range_error, W.rows(), n, "Matrix W has wrong size."); 71 | 72 | Eigen::SparseMatrix Q_temp; 73 | 74 | #if EIGEN_VERSION_AT_LEAST(3,2,92) 75 | Q_temp = A.transpose() * S * A + (Eigen::SparseMatrix)W.toDenseMatrix().sparseView(); 76 | #else 77 | Q_temp = A.transpose() * S * A + W.toDenseMatrix().sparseView(); 78 | #endif 79 | 80 | VectorXd c_temp = -A.transpose() * S * b; 81 | 82 | return OoqpEigenInterface::solve(Q_temp, c_temp, C, c, D, d, f, x); 83 | } 84 | 85 | bool QuadraticProblemFormulation::solve( 86 | const Eigen::SparseMatrix& A, 87 | const Eigen::DiagonalMatrix& S, const Eigen::VectorXd& b, 88 | const Eigen::DiagonalMatrix& W, 89 | const Eigen::SparseMatrix& D, const Eigen::VectorXd& d, 90 | const Eigen::VectorXd& f, Eigen::VectorXd& x) 91 | { 92 | Eigen::SparseMatrix C; 93 | Eigen::VectorXd c; 94 | return solve(A, S, b, W, C, c, D, d, f, x); 95 | } 96 | 97 | } /* namespace ooqpei */ 98 | -------------------------------------------------------------------------------- /test/OoqpEigenInterface_test.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, Péter Fankhauser, Christian Gehring, Stelian Coros 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Autonomous Systems Lab nor ETH Zurich 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | /*! 37 | * @file QuadraticProblemFormulation_test.cpp 38 | * @author Péter Fankhauser 39 | * @date Aug 16, 2013 40 | */ 41 | 42 | #include 43 | #include "ooqp_eigen_interface/OoqpEigenInterface.hpp" 44 | #include "ooqp_eigen_interface/ooqpei_gtest_eigen.hpp" 45 | #include 46 | #include 47 | 48 | using namespace Eigen; 49 | using namespace ooqpei; 50 | using namespace ooqpei::eigen; 51 | using namespace std; 52 | 53 | TEST(OOQPEITest, LeastSquares) 54 | { 55 | Vector2d solution(10.0, 8.0); 56 | 57 | SparseMatrix Q; 58 | Q.resize(2, 2); 59 | Q.insert(0, 0) = 1.0; 60 | Q.insert(0, 1) = -1.0; 61 | Q.insert(1, 0) = -1.0; 62 | Q.insert(1, 1) = 2.0; 63 | VectorXd c(2); 64 | c << -2.0, -6.0; 65 | VectorXd x; 66 | 67 | ooqpei::OoqpEigenInterface::solve(Q, c, x); 68 | 69 | expectNear(x, solution, 1e-8, OOQPEI_SOURCE_FILE_POS); 70 | } 71 | 72 | TEST(OOQPEITest, InequalityConstraints) 73 | { 74 | Vector2d solution(2.0 / 3.0, 1.0 + 1.0 / 3.0); 75 | 76 | SparseMatrix Q; 77 | Q.resize(2, 2); 78 | Q.insert(0, 0) = 1.0; 79 | Q.insert(0, 1) = -1.0; 80 | Q.insert(1, 0) = -1.0; 81 | Q.insert(1, 1) = 2.0; 82 | VectorXd c(2); 83 | c << -2.0, -6.0; 84 | SparseMatrix A; 85 | VectorXd b; 86 | SparseMatrix C; 87 | C.resize(3, 2); 88 | C.insert(0, 0) = 1.0; 89 | C.insert(0, 1) = 1.0; 90 | C.insert(1, 0) = -1.0; 91 | C.insert(1, 1) = 2.0; 92 | C.insert(2, 0) = 2.0; 93 | C.insert(2, 1) = 1.0; 94 | VectorXd d(3); 95 | d << -std::numeric_limits::max(), -std::numeric_limits::max(), -std::numeric_limits::max(); 96 | VectorXd f(3); 97 | f << 2.0, 2.0, 3.0; 98 | VectorXd l(2); 99 | l << 0.0, 0.0; 100 | VectorXd u(2); 101 | u << 1000.0, 1000.0; 102 | VectorXd x; 103 | 104 | OoqpEigenInterface::solve(Q, c, A, b, C, d, f, l, u, x); 105 | 106 | expectNear(x, solution, 1e-8, OOQPEI_SOURCE_FILE_POS); 107 | } 108 | 109 | TEST(OOQPEITest, LinearProgrammingNoConstraints) 110 | { 111 | Vector3d solution(0.0, 0.0, 0.0); 112 | 113 | SparseMatrix Q; 114 | Q.resize(3, 3); 115 | VectorXd c(3); 116 | c << 5.0, 4.0, 6.0; 117 | SparseMatrix A; 118 | VectorXd b; 119 | VectorXd l = VectorXd::Zero(3); 120 | VectorXd u = Vector3d::Constant(std::numeric_limits::max()); 121 | VectorXd x; 122 | 123 | OoqpEigenInterface::solve(Q, c, A, b, l, u, x); 124 | 125 | expectNear(x, solution, 1e-8, OOQPEI_SOURCE_FILE_POS); 126 | } 127 | 128 | TEST(OOQPEITest, LinearProgrammingWithInequalityConstraints) 129 | { 130 | Vector3d solution(0.0, 15.0, 3.0); 131 | 132 | SparseMatrix Q; 133 | Q.resize(3, 3); 134 | VectorXd c(3); 135 | c << -5.0, -4.0, -6.0; 136 | SparseMatrix A; 137 | VectorXd b; 138 | SparseMatrix C; 139 | C.resize(3, 3); 140 | C.insert(0, 0) = 1.0; 141 | C.insert(0, 1) = -1.0; 142 | C.insert(0, 2) = 1.0; 143 | C.insert(1, 0) = 3.0; 144 | C.insert(1, 1) = 2.0; 145 | C.insert(1, 2) = 4.0; 146 | C.insert(2, 0) = 3.0; 147 | C.insert(2, 1) = 2.0; 148 | C.insert(2, 2) = 0.0; 149 | VectorXd d(3); 150 | d = Vector3d::Constant(-std::numeric_limits::max()); 151 | VectorXd f(3); 152 | f << 20.0, 42.0, 30.0; 153 | VectorXd l = VectorXd::Zero(3); 154 | VectorXd u(3); 155 | u = Vector3d::Constant(std::numeric_limits::max()); 156 | VectorXd x; 157 | 158 | OoqpEigenInterface::solve(Q, c, A, b, C, d, f, l, u, x); 159 | 160 | expectNear(x, solution, 1e-8, OOQPEI_SOURCE_FILE_POS); 161 | } 162 | 163 | TEST(OOQPEITest, NaN) 164 | { 165 | SparseMatrix Q; 166 | Q.resize(2, 2); 167 | Q.insert(0, 0) = NAN; 168 | Q.insert(0, 1) = NAN; 169 | Q.insert(1, 0) = NAN; 170 | Q.insert(1, 1) = NAN; 171 | VectorXd c(2); 172 | c << NAN, NAN; 173 | VectorXd x; 174 | 175 | EXPECT_TRUE(ooqpei::OoqpEigenInterface::solve(Q, c, x)); 176 | } 177 | -------------------------------------------------------------------------------- /test/QuadraticProblemFormulation_test.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, Péter Fankhauser, Christian Gehring, Stelian Coros 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Autonomous Systems Lab nor ETH Zurich 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | /*! 37 | * @file QuadraticProblemFormulation_test.cpp 38 | * @author Péter Fankhauser 39 | * @date Aug 16, 2013 40 | */ 41 | 42 | #include 43 | #include "ooqp_eigen_interface/QuadraticProblemFormulation.hpp" 44 | #include "ooqp_eigen_interface/ooqpei_gtest_eigen.hpp" 45 | #include 46 | 47 | using namespace Eigen; 48 | using namespace ooqpei; 49 | using namespace ooqpei::eigen; 50 | using namespace std; 51 | 52 | TEST(OOQPEITest, QuadraticProblemFormulation) 53 | { 54 | Vector2d solution(1.030779872038734, -3.143697043057237); 55 | 56 | Eigen::SparseMatrix A, C, D; 57 | Eigen::DiagonalMatrix S, W; 58 | Eigen::VectorXd b, c, d, f, x; 59 | 60 | A.resize(3, 2); 61 | A.insert(0, 0) = 1.0; 62 | A.insert(0, 1) = -1.0; 63 | A.insert(1, 0) = -4.0; 64 | A.insert(1, 1) = -3.0; 65 | A.insert(2, 0) = 2.0; 66 | A.insert(2, 1) = 0.0; 67 | S = Vector3d(3.0, 2.0, 0.69).asDiagonal(); 68 | W = Vector2d(0.5, 0.2).asDiagonal(); 69 | b.resize(3); 70 | b << 3.0, 6.0, 9.0; 71 | 72 | QuadraticProblemFormulation::solve(A, S, b, W, C, c, D, d, f, x); 73 | 74 | expectNear(x, solution, 1e-10, OOQPEI_SOURCE_FILE_POS); 75 | } 76 | -------------------------------------------------------------------------------- /test/matlab/OoqpEigenInterface_test.m: -------------------------------------------------------------------------------- 1 | % OOQP Notation: min 1/2 x' Q x + c' x, such that A x = b, d <= Cx <= f, and l <= x <= u. 2 | 3 | clear all 4 | clc 5 | 6 | %% Quadratic Programming 7 | 8 | Q = [1 -1; -1 2]; 9 | c = [-2; -6]; 10 | C = [1 1; -1 2; 2 1]; 11 | f = [2; 2; 3]; 12 | A = zeros(2,2); 13 | b = zeros(2,1); 14 | l = zeros(2,1); 15 | u = [1000; 100]; 16 | 17 | opts = optimoptions('quadprog','Algorithm','interior-point-convex','Display','testing'); 18 | 19 | % No constraints 20 | [x,fval,exitflag,output,lambda] = quadprog(Q,c,[],[],[],[],[],[],[],opts); 21 | x,fval,exitflag 22 | 23 | % With constraints 24 | [x,fval,exitflag,output,lambda] = quadprog(Q,c,C,f,[],[],l,u,[],opts); 25 | x,fval,exitflag 26 | 27 | %% Linear Programming 28 | 29 | Q = zeros(3, 3); 30 | c = [-5; -4; -6]; 31 | C = [1 -1 1 32 | 3 2 4 33 | 3 2 0]; 34 | f = [20; 42; 30]; 35 | l = zeros(3,1); 36 | 37 | % No constraints (negative) 38 | [x,fval,exitflag,output,lambda] = linprog(-c,[],[],[],[],l); 39 | x,fval,lambda.ineqlin,lambda.lower 40 | 41 | % With constraints 42 | [x,fval,exitflag,output,lambda] = linprog(c,C,f,[],[],l); 43 | x,fval,lambda.ineqlin,lambda.lower -------------------------------------------------------------------------------- /test/matlab/QuadraticProblemFormulation_test.m: -------------------------------------------------------------------------------- 1 | clear all 2 | clc 3 | 4 | A = [1 -1; -4 -3; 2 0]; 5 | S = diag([3, 2, 0.69]); 6 | W = diag([0.5, 0.2]); 7 | b = [3; 6; 9]; 8 | 9 | Q = A'*S*A + W; 10 | c = -A'*S*b; 11 | %% 12 | opts = optimoptions('quadprog','Algorithm','interior-point-convex','Display','testing'); 13 | 14 | [x,fval,exitflag,output,lambda] = quadprog(Q,c,[],[],[],[],[],u,[],opts); 15 | x,fval,exitflag -------------------------------------------------------------------------------- /test/test_main.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, Péter Fankhauser, Christian Gehring, Stelian Coros 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Autonomous Systems Lab nor ETH Zurich 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | /*! 37 | * @file test_main.cpp 38 | * @author Péter Fankhauser 39 | * @date Aug 16, 2013 40 | */ 41 | 42 | #include 43 | 44 | /// Run all the tests that were declared with TEST() 45 | int main(int argc, char **argv){ 46 | testing::InitGoogleTest(&argc, argv); 47 | return RUN_ALL_TESTS(); 48 | } 49 | --------------------------------------------------------------------------------