├── .editorconfig ├── .gitignore ├── .gitmodules ├── .travis.yml ├── CMakeLists.txt ├── Readme.md ├── cdd.patch ├── cmake └── FindNumPy.cmake ├── install_prereqs.sh ├── install_prereqs_python_3.sh ├── iris.sublime-project ├── lic.enc ├── license.txt ├── requirements.txt ├── src ├── .gitignore ├── CMakeLists.txt ├── cxx │ ├── CMakeLists.txt │ ├── IRISProblem.cpp │ ├── cvxgen │ │ ├── .gitignore │ │ ├── CMakeLists.txt │ │ ├── cvxgen_ldp.c │ │ ├── cvxgen_ldp.cpp │ │ ├── cvxgen_ldp_mex.c │ │ ├── cvxgen_ldp_mex.m │ │ ├── cvxsolve.m │ │ ├── description.cvxgen │ │ ├── iris │ │ │ └── cvxgen_ldp.h │ │ ├── ldl.c │ │ ├── make_csolve.m │ │ ├── matrix_support.c │ │ ├── solver.c │ │ ├── solver.h │ │ ├── testsolver.c │ │ └── util.c │ ├── geometry.cpp │ ├── iris-config.cmake.in │ ├── iris.cpp │ ├── iris.pc.in │ ├── iris │ │ ├── geometry.h │ │ ├── iris.h │ │ └── iris_mosek.h │ ├── iris_cdd.h │ ├── iris_demo.cpp │ ├── iris_mosek.cpp │ ├── iris_wrapper.cpp │ └── test │ │ ├── CMakeLists.txt │ │ ├── test_append_polyhedron.cpp │ │ ├── test_cdd.cpp │ │ ├── test_closest_point.cpp │ │ ├── test_for_thin_mosek_ellipsoid_bug.cpp │ │ ├── test_infeasible_ellipsoid.cpp │ │ ├── test_iris.cpp │ │ ├── test_mosek_ellipsoid.cpp │ │ ├── test_ppl.cpp │ │ ├── test_required_containment.cpp │ │ ├── test_separating_hyperplanes.cpp │ │ └── test_util.h ├── license.txt ├── matlab │ ├── +iris │ │ ├── +cspace │ │ │ ├── cspace3.m │ │ │ ├── minkowski_sum.m │ │ │ └── project_c_space_region.m │ │ ├── +drawing │ │ │ ├── animate_results.m │ │ │ ├── drawPolyFromVertices.m │ │ │ ├── draw_2d.m │ │ │ ├── draw_3d.m │ │ │ ├── draw_Nd.m │ │ │ └── intersect_obs_with_plane.m │ │ ├── +inner_ellipsoid │ │ │ ├── cvx_ellipsoid.m │ │ │ ├── mosek_nofusion.m │ │ │ └── spot_ellipsoid.m │ │ ├── +least_distance │ │ │ ├── cvxgen_ldp.m │ │ │ └── mosek_ldp.m │ │ ├── +terrain_grid │ │ │ ├── CollisionModel.m │ │ │ ├── Heightmap.m │ │ │ ├── Server.m │ │ │ ├── component_boundary.m │ │ │ ├── find_boundary.m │ │ │ ├── inflate_grid_region.m │ │ │ ├── obs_dist.m │ │ │ └── segment_grid.m │ │ ├── +test │ │ │ ├── compare_implementations.m │ │ │ ├── plot_timing_results.m │ │ │ ├── random_cubic_obstacles.m │ │ │ ├── random_obstacles.m │ │ │ ├── run_system_tests.m │ │ │ ├── test_boundary.m │ │ │ ├── test_c_space_3d.m │ │ │ ├── test_grid_2d.m │ │ │ ├── test_grid_segmentation.m │ │ │ ├── test_outer_ellipsoid.m │ │ │ ├── test_points_2d.m │ │ │ ├── test_poly_2d.m │ │ │ ├── test_poly_3d.m │ │ │ ├── test_poly_Nd.m │ │ │ ├── test_poly_selection_2d.m │ │ │ ├── test_polygon_recovery.m │ │ │ ├── test_simple_poly_2d.m │ │ │ ├── test_terrain.m │ │ │ ├── test_thin_ellipsoid_simple.m │ │ │ ├── test_timing.m │ │ │ └── test_uav_demo.m │ │ ├── +thirdParty │ │ │ ├── +mosek_lownerjohn │ │ │ │ ├── det_rootn.m │ │ │ │ ├── geometric_mean.m │ │ │ │ ├── lownerjohn_ellipsoid.m │ │ │ │ └── lownerjohn_inner.m │ │ │ └── +polytopes │ │ │ │ ├── lcon2vert.m │ │ │ │ ├── license.txt │ │ │ │ ├── qlcon2vert.m │ │ │ │ └── vert2lcon.m │ │ ├── +util │ │ │ ├── auto_seed_regions.m │ │ │ ├── equal_up_to_permutations.m │ │ │ ├── obstacle_distance_matrix.m │ │ │ ├── orthos.m │ │ │ └── rotmat.m │ │ ├── Ellipsoid.m │ │ ├── Polyhedron.m │ │ ├── TerrainRegion.m │ │ ├── callback_silent.m │ │ ├── inflate_region.m │ │ ├── inflate_region_fallback.m │ │ ├── inflate_regionmex.cpp │ │ ├── inflation_results.m │ │ ├── maximal_ellipse.m │ │ ├── pad_obstacle_points.m │ │ ├── sample_convex_polyhedron.m │ │ └── separating_hyperplanes.m │ ├── CMakeLists.txt │ └── data │ │ ├── example_feas_map.mat │ │ ├── example_heights.mat │ │ └── example_terrain_clicks.mat └── python │ ├── CMakeLists.txt │ ├── examples │ ├── iris_2d.ipynb │ └── polyhedrons.ipynb │ └── irispy │ ├── .gitignore │ ├── __init__.py │ ├── cspace.py │ ├── drawing.py │ ├── extensions │ ├── __init__.py │ ├── ellipsoid.py │ ├── irisdebugdata.py │ └── polyhedron.py │ ├── irispy.py │ ├── test │ ├── __init__.py │ ├── cpp_wrapper_test.py │ ├── python_interface_test.py │ ├── test_cpp_exception.py │ ├── test_ellipsoid.py │ ├── test_iris.py │ ├── test_iris_2d.py │ ├── test_iris_3d.py │ ├── test_polytope.py │ └── test_required_containment.py │ └── utils.py └── travis └── linux.sh /.editorconfig: -------------------------------------------------------------------------------- 1 | root = true 2 | 3 | [*] 4 | end_of_line = lf 5 | indent_size = 2 6 | indent_style = space 7 | insert_final_newline = true 8 | trim_trailing_whitespace = true 9 | 10 | [*.md] 11 | trim_trailing_whitespace = false 12 | 13 | [*.py] 14 | indent_size = 4 15 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | pod-build 2 | *.mex* 3 | *.so 4 | .DS_Store 5 | *.pyc 6 | build/ 7 | externals_config.cmake 8 | iris_wrapper.py 9 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rdeits/iris-distro/ff624610a82a858862d55732136dbc2cc9ab16fc/.gitmodules -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | 3 | notifications: 4 | email: false 5 | 6 | branches: 7 | only: 8 | - master 9 | 10 | matrix: 11 | include: 12 | - os: linux 13 | sudo: required 14 | dist: trusty 15 | python: "2.7" 16 | env: 17 | - PYBIND11_PYTHON_VERSION=2.7 18 | - os: linux 19 | sudo: required 20 | dist: trusty 21 | python: "3.5" 22 | env: 23 | - PYBIND11_PYTHON_VERSION=3.5 24 | - os: osx 25 | 26 | before_install: 27 | - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then source travis/linux.sh ; fi 28 | - mkdir -p $HOME/.config/matplotlib 29 | - echo backend\:\ agg >> $HOME/.config/matplotlib/matplotlibrc 30 | - export MATPLOTLIBRC=$HOME/.config/matplotlib 31 | - mkdir -p $HOME/mosek 32 | - openssl aes-256-cbc -K $encrypted_b15e1ad4334d_key -iv $encrypted_b15e1ad4334d_iv -in lic.enc -out $HOME/mosek/mosek.lic -d 33 | - pip install -r requirements.txt 34 | - pip install IPython==5.0 35 | - pip install jupyter 36 | 37 | install: 38 | - mkdir build 39 | - cd build 40 | - cmake .. -DCMAKE_INSTALL_PREFIX=install -DCMAKE_BUILD_TYPE=Release -DPYBIND11_PYTHON_VERSION=$PYBIND11_PYTHON_VERSION 41 | - make 42 | 43 | script: 44 | - cd iris-build 45 | - ctest --output-on-failure 46 | -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | Introduction 2 | ============ 3 | 4 | This package contains the IRIS algorithm for iterative convex regional inflation by semidefinite programming, implemented in C++ with bindings for Python. It is designed to take an environment containing many (convex) obstacles and a start point, and to compute a large convex obstacle-free region. This region can then be used to define linear constraints for some other objective function which the user might want to optimize over the obstacle-free space. The algorithm is described in: 5 | 6 | R. L. H. Deits and R. Tedrake, “Computing large convex regions of 7 | obstacle-free space through semidefinite programming,” Workshop on the Algorithmic Fundamentals of Robotics, Istanbul, Aug. 2014. 8 | [Online]. Available: 9 | http://groups.csail.mit.edu/robotics-center/public_papers/Deits14.pdf 10 | 11 | [![Build Status](https://travis-ci.org/rdeits/iris-distro.svg)](https://travis-ci.org/rdeits/iris-distro) 12 | 13 | MATLAB Support 14 | ============== 15 | 16 | A pure-MATLAB implementation of IRIS is also included in `src/matlab`. This will be slower and less flexible than the Python and C++ versions, but may be useful for legacy code. 17 | 18 | Requirements 19 | ============ 20 | 21 | Ubuntu (with apt-get): 22 | 23 | pkg-config 24 | cmake 25 | libgmp-dev 26 | 27 | Mac OSX (with homebrew): 28 | 29 | pkg-config 30 | cmake 31 | gmp 32 | 33 | You'll also need some python packages to build and use the python bindings. You can install them on ubuntu with these apt-get packages: 34 | 35 | python-numpy 36 | python-scipy 37 | python-matplotlib 38 | python-nose 39 | cython 40 | 41 | Or you can install the `liblapack-dev`, `libblas-dev`, and `gfortran` packages from apt-get, and then install the python modules with pip: 42 | 43 | pip install -r python_requirements.txt 44 | 45 | You'll also need a license for the Mosek optimization toolbox (this package includes a downloader for the Mosek code, but you have to get your own license). Mosek has free licenses available for academic use. 46 | 47 | Installation 48 | ============ 49 | 50 | This project is configured as a standard CMake project, so the general build process is: 51 | 52 | mkdir build 53 | cd build 54 | cmake .. 55 | make 56 | 57 | ------------------------------ 58 | Installation without externals 59 | ------------------------------ 60 | By default, IRIS will build its external dependencies as part of the build process. If you want to turn any or all of them off, you can set the `WITH_EIGEN`, `WITH_CDD`, and `WITH_MOSEK` options to `OFF` using cmake. The easiest way to do that is to run: 61 | 62 | cd build 63 | ccmake . 64 | 65 | which will launch a terminal-based GUI to let you change those options. 66 | 67 | If you're using IRIS as part of another project with cmake, you can just set the CMAKE_CACHE_ARGS to include `-DIRIS_WITH_EIGEN:BOOL=OFF` etc. For more information, see: . 68 | 69 | Example Usage 70 | ============= 71 | 72 | Python wrapper 73 | -------------- 74 | 75 | python -m irispy.test.test_iris_2d 76 | 77 | C++ library 78 | ----------- 79 | 80 | See `iris/src/iris_demo.cpp` for a basic usage example. 81 | 82 | Examples 83 | ======== 84 | 85 | Here are some animations of the algorithm running in various 86 | environments: 87 | 88 | 2-dimensional space, 30 obstacles: 89 | 90 | ![](https://rdeits.github.io/iris-distro/examples/poly_2d_N30/animation.gif) 91 | 92 | 2-dimensional space, 50 obstacles: 93 | 94 | ![](https://rdeits.github.io/iris-distro/examples/poly_2d_N50/animation.gif) 95 | 96 | 2-dimensional space, 50 obstacles: 97 | 98 | ![](https://rdeits.github.io/iris-distro/examples/poly_2d_N50_2/animation.gif) 99 | 100 | 2-dimensional space, 1000 obstacles: 101 | 102 | ![](https://rdeits.github.io/iris-distro/examples/poly_2d_N1000/animation.gif) 103 | 104 | 3-dimensional space: 105 | 106 | ![](https://rdeits.github.io/iris-distro/examples/poly_3d/animation.gif) 107 | 108 | 3-dimensional space: 109 | 110 | ![](https://rdeits.github.io/iris-distro/examples/poly_3d_2/animation.gif) 111 | 112 | 3-dimensional configuration space of a rod-shaped robot translating and yawing: 113 | 114 | ![](https://rdeits.github.io/iris-distro/examples/c_space_3d/animation.gif) 115 | 116 | 3-dimensional slice of a 4-dimensional region among 4D obstacles: 117 | 118 | ![](https://rdeits.github.io/iris-distro/examples/poly_4d/animation.gif) 119 | 120 | Example Application 121 | =================== 122 | This is a demonstration of path-planning for a simple UAV model around obstacles. Rather than constraining that the UAV be outside the obstacles, we seed several IRIS regions and require that the UAV be inside one of those regions at each time step. This turns a non-convex problem into a mixed-integer convex problem, which we can solve to its global optimum. You can try this out by running `iris.test.test_uav_demo();` or `iris.test.test_uav_demo('4d');` 123 | 124 | ![](http://rdeits.github.io/iris-distro/examples/uav/demo_uav.png) 125 | -------------------------------------------------------------------------------- /cdd.patch: -------------------------------------------------------------------------------- 1 | diff -p --exclude=.DS_Store -r cddlib-094h 2/src-gmp/Makefile.in cddlib-094h/src-gmp/Makefile.in 2 | *** cddlib-094h/src-gmp/Makefile.in 2012-03-23 04:51:54.000000000 -0400 3 | --- cddlib-094h/src-gmp/Makefile.in 2016-02-18 13:37:52.000000000 -0500 4 | *************** testlp1_gmp_SOURCES = testlp1.c 5 | *** 255,261 **** 6 | testlp2_gmp_SOURCES = testlp2.c 7 | testlp3_gmp_SOURCES = testlp3.c 8 | # cddmathlink_SOURCES = cddmathlink.c cddmlio.h cddmlio.c 9 | ! LDADD = ../lib-src-gmp/libcddgmp.la 10 | AM_LDFLAGS = -L$(gmplibdir) 11 | INCLUDES = -I../lib-src-gmp -I$(gmpincludedir) 12 | AM_CPPFLAGS = -DGMPRATIONAL 13 | --- 255,261 ---- 14 | testlp2_gmp_SOURCES = testlp2.c 15 | testlp3_gmp_SOURCES = testlp3.c 16 | # cddmathlink_SOURCES = cddmathlink.c cddmlio.h cddmlio.c 17 | ! LDADD = ../lib-src-gmp/libcddgmp.la -lgmp 18 | AM_LDFLAGS = -L$(gmplibdir) 19 | INCLUDES = -I../lib-src-gmp -I$(gmpincludedir) 20 | AM_CPPFLAGS = -DGMPRATIONAL 21 | -------------------------------------------------------------------------------- /cmake/FindNumPy.cmake: -------------------------------------------------------------------------------- 1 | # - Find the NumPy libraries 2 | # This module finds if NumPy is installed, and sets the following variables 3 | # indicating where it is. 4 | # 5 | # TODO: Update to provide the libraries and paths for linking npymath lib. 6 | # 7 | # NUMPY_FOUND - was NumPy found 8 | # NUMPY_VERSION - the version of NumPy found as a string 9 | # NUMPY_VERSION_MAJOR - the major version number of NumPy 10 | # NUMPY_VERSION_MINOR - the minor version number of NumPy 11 | # NUMPY_VERSION_PATCH - the patch version number of NumPy 12 | # NUMPY_VERSION_DECIMAL - e.g. version 1.6.1 is 10601 13 | # NUMPY_INCLUDE_DIRS - path to the NumPy include files 14 | 15 | #============================================================================ 16 | # Copyright 2012 Continuum Analytics, Inc. 17 | # 18 | # MIT License 19 | # 20 | # Permission is hereby granted, free of charge, to any person obtaining 21 | # a copy of this software and associated documentation files 22 | # (the "Software"), to deal in the Software without restriction, including 23 | # without limitation the rights to use, copy, modify, merge, publish, 24 | # distribute, sublicense, and/or sell copies of the Software, and to permit 25 | # persons to whom the Software is furnished to do so, subject to 26 | # the following conditions: 27 | # 28 | # The above copyright notice and this permission notice shall be included 29 | # in all copies or substantial portions of the Software. 30 | # 31 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 32 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 33 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 34 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR 35 | # OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 36 | # ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 37 | # OTHER DEALINGS IN THE SOFTWARE. 38 | # 39 | #============================================================================ 40 | 41 | # Finding NumPy involves calling the Python interpreter 42 | if(NumPy_FIND_REQUIRED) 43 | find_package(PythonInterp REQUIRED) 44 | else() 45 | find_package(PythonInterp) 46 | endif() 47 | 48 | if(NOT PYTHONINTERP_FOUND) 49 | set(NUMPY_FOUND FALSE) 50 | return() 51 | endif() 52 | 53 | execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" 54 | "import numpy as n; print(n.__version__); print(n.get_include());" 55 | RESULT_VARIABLE _NUMPY_SEARCH_SUCCESS 56 | OUTPUT_VARIABLE _NUMPY_VALUES_OUTPUT 57 | ERROR_VARIABLE _NUMPY_ERROR_VALUE 58 | OUTPUT_STRIP_TRAILING_WHITESPACE) 59 | 60 | if(NOT _NUMPY_SEARCH_SUCCESS MATCHES 0) 61 | if(NumPy_FIND_REQUIRED) 62 | message(FATAL_ERROR 63 | "NumPy import failure:\n${_NUMPY_ERROR_VALUE}") 64 | endif() 65 | set(NUMPY_FOUND FALSE) 66 | return() 67 | endif() 68 | 69 | # Convert the process output into a list 70 | string(REGEX REPLACE ";" "\\\\;" _NUMPY_VALUES ${_NUMPY_VALUES_OUTPUT}) 71 | string(REGEX REPLACE "\n" ";" _NUMPY_VALUES ${_NUMPY_VALUES}) 72 | # Just in case there is unexpected output from the Python command. 73 | list(GET _NUMPY_VALUES -2 NUMPY_VERSION) 74 | list(GET _NUMPY_VALUES -1 NUMPY_INCLUDE_DIRS) 75 | 76 | string(REGEX MATCH "^[0-9]+\\.[0-9]+\\.[0-9]+" _VER_CHECK "${NUMPY_VERSION}") 77 | if("${_VER_CHECK}" STREQUAL "") 78 | # The output from Python was unexpected. Raise an error always 79 | # here, because we found NumPy, but it appears to be corrupted somehow. 80 | message(FATAL_ERROR 81 | "Requested version and include path from NumPy, got instead:\n${_NUMPY_VALUES_OUTPUT}\n") 82 | return() 83 | endif() 84 | 85 | # Make sure all directory separators are '/' 86 | string(REGEX REPLACE "\\\\" "/" NUMPY_INCLUDE_DIRS ${NUMPY_INCLUDE_DIRS}) 87 | 88 | # Get the major and minor version numbers 89 | string(REGEX REPLACE "\\." ";" _NUMPY_VERSION_LIST ${NUMPY_VERSION}) 90 | list(GET _NUMPY_VERSION_LIST 0 NUMPY_VERSION_MAJOR) 91 | list(GET _NUMPY_VERSION_LIST 1 NUMPY_VERSION_MINOR) 92 | list(GET _NUMPY_VERSION_LIST 2 NUMPY_VERSION_PATCH) 93 | string(REGEX MATCH "[0-9]*" NUMPY_VERSION_PATCH ${NUMPY_VERSION_PATCH}) 94 | math(EXPR NUMPY_VERSION_DECIMAL 95 | "(${NUMPY_VERSION_MAJOR} * 10000) + (${NUMPY_VERSION_MINOR} * 100) + ${NUMPY_VERSION_PATCH}") 96 | 97 | find_package_message(NUMPY 98 | "Found NumPy: version \"${NUMPY_VERSION}\" ${NUMPY_INCLUDE_DIRS}" 99 | "${NUMPY_INCLUDE_DIRS}${NUMPY_VERSION}") 100 | 101 | set(NUMPY_FOUND TRUE) 102 | -------------------------------------------------------------------------------- /install_prereqs.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -euo pipefail 4 | 5 | case "${1:-}" in 6 | ("homebrew") 7 | brew install cmake gmp matplotlib numpy python scipy 8 | pip2 install ipython==5.4.1 jupyter nose 9 | ;; 10 | ("macports") 11 | port install cmake gmp py27-ipython py27-jupyter py27-matplotlib \ 12 | py27-nose py27-numpy py27-scipy python27 13 | ;; 14 | ("ubuntu") 15 | apt-get install --no-install-recommends cmake g++ gcc git libgmp-dev make \ 16 | python python-matplotlib python-nose python-numpy python-pip python-scipy 17 | pip install ipython==5.4.1 jupyter 18 | ;; 19 | (*) 20 | echo "Usage: $0 " 1>&2 21 | echo "where is one of the following:" 1>&2 22 | echo " homebrew" 1>&2 23 | echo " macports" 1>&2 24 | echo " ubuntu" 1>&2 25 | exit 1 26 | ;; 27 | esac 28 | -------------------------------------------------------------------------------- /install_prereqs_python_3.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -euo pipefail 4 | 5 | case "${1:-}" in 6 | ("homebrew") 7 | brew install cmake gmp numpy python3 scipy 8 | pip3 install matplotlib nose 9 | ;; 10 | ("macports") 11 | port install cmake gmp py36-matplotlib py36-nose py36-numpy py36-scipy \ 12 | python36 13 | ;; 14 | ("ubuntu") 15 | apt-get install --no-install-recommends cmake g++ gcc git libgmp-dev make \ 16 | python3 python3-matplotlib python3-nose python3-numpy python3-scipy 17 | ;; 18 | (*) 19 | echo "Usage: $0 " 1>&2 20 | echo "where is one of the following:" 1>&2 21 | echo " homebrew" 1>&2 22 | echo " macports" 1>&2 23 | echo " ubuntu" 1>&2 24 | exit 1 25 | ;; 26 | esac 27 | -------------------------------------------------------------------------------- /iris.sublime-project: -------------------------------------------------------------------------------- 1 | { 2 | "folders": 3 | [ 4 | { 5 | "path": "." 6 | } 7 | ] 8 | } 9 | -------------------------------------------------------------------------------- /lic.enc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rdeits/iris-distro/ff624610a82a858862d55732136dbc2cc9ab16fc/lic.enc -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | All software except src/matlab/+iris/+thirdParty is: 2 | Copyright (c) 2014, Robin Deits 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 18 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | 25 | Software in matlab+iris/+thirdParty/+mosek_lownerjohn is: 26 | Copyright (c) MOSEK ApS, Denmark 27 | and used with permission for non-commercial distribution. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 30 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 31 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 32 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 33 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 34 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 35 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 36 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 37 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 38 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 39 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | scipy 2 | numpy 3 | matplotlib 4 | nose 5 | -------------------------------------------------------------------------------- /src/.gitignore: -------------------------------------------------------------------------------- 1 | *.so 2 | .DS_Store 3 | *.pyc 4 | *.o 5 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(iris) 3 | 4 | if(APPLE) 5 | # Compiler ID for Apple Clang is now AppleClang. 6 | if(POLICY CMP0025) 7 | cmake_policy(SET CMP0025 NEW) 8 | endif() 9 | 10 | # MACOSX_RPATH is enabled by default. 11 | if(POLICY CMP0042) 12 | cmake_policy(SET CMP0042 NEW) 13 | endif() 14 | endif() 15 | 16 | include(CMakePackageConfigHelpers) 17 | include(CTest) 18 | include(GNUInstallDirs) 19 | 20 | list(INSERT CMAKE_MODULE_PATH 0 "${CMAKE_CURRENT_SOURCE_DIR}/../cmake") 21 | 22 | find_package(pybind11 CONFIG REQUIRED) 23 | find_package(Mosek CONFIG REQUIRED) 24 | find_package(NumPy MODULE REQUIRED) 25 | 26 | # Newer versions of Eigen 3 define imported targets in their package 27 | # configuration files, so try to locate such a file before using the legacy 28 | # find module. 29 | find_package(Eigen3 CONFIG) 30 | 31 | if(NOT Eigen3_FOUND) 32 | find_package(Eigen3 MODULE REQUIRED) 33 | endif() 34 | 35 | find_library(CDD_LIBRARY NAMES cdd) 36 | 37 | if(NOT CDD_LIBRARY) 38 | message(FATAL_ERROR "Could NOT find cdd") 39 | endif() 40 | 41 | find_path(CDD_INCLUDE_DIR NAMES cdd.h) 42 | 43 | if(NOT CDD_INCLUDE_DIR) 44 | message(FATAL_ERROR "Could NOT find cdd.h") 45 | endif() 46 | 47 | # Set RPATH for installed binaries. 48 | set(CMAKE_INSTALL_RPATH 49 | "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" 50 | "${CMAKE_INSTALL_PREFIX}/lib") 51 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ON) 52 | 53 | list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH) 54 | 55 | # Check that the compiler supports a flag for setting the C++ dialect to C++0x 56 | # or C++11. 57 | if(CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang") 58 | set(MIN_COMPILER_VERSION 4.0) 59 | set(CXX_FLAGS_CXX11 "-std=c++11") 60 | elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") 61 | set(MIN_COMPILER_VERSION 2.1) 62 | if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.1) 63 | set(CXX_FLAGS_CXX11 "-std=c++0x") 64 | else() 65 | set(CXX_FLAGS_CXX11 "-std=c++11") 66 | endif() 67 | elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") 68 | set(MIN_COMPILER_VERSION 4.4) 69 | if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.7) 70 | set(CXX_FLAGS_CXX11 "-std=c++0x") 71 | else() 72 | set(CXX_FLAGS_CXX11 "-std=c++11") 73 | endif() 74 | elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Intel") 75 | set(MIN_COMPILER_VERSION 12.1) 76 | if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS 13.0) 77 | if(WIN32) 78 | set(CXX_FLAGS_CXX11 "/Qstd=c++0x") 79 | else() 80 | set(CXX_FLAGS_CXX11 "-std=c++0x") 81 | endif() 82 | else() 83 | if(WIN32) 84 | set(CXX_FLAGS_CXX11 "/Qstd=c++11") 85 | else() 86 | set(CXX_FLAGS_CXX11 "-std=c++11") 87 | endif() 88 | endif() 89 | elseif(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") 90 | set(MIN_COMPILER_VERSION 1900) 91 | elseif(CMAKE_CXX_COMPILER_ID STREQUAL "PGI") 92 | set(MIN_COMPILER_VERSION 15) 93 | set(CXX_FLAGS_CXX11 "--c++11") 94 | elseif(CMAKE_CXX_COMPILER_ID STREQUAL "XL") 95 | set(MIN_COMPILER_VERSION 10.1) 96 | if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS 13.1.2) 97 | set(CXX_FLAGS_CXX11 "-qlanglvl=extended0x") 98 | else() 99 | set(CXX_FLAGS_CXX11 "-std=c++11") 100 | endif() 101 | else() 102 | set(CXX_FLAGS_CXX11) 103 | endif() 104 | 105 | if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS MIN_COMPILER_VERSION) 106 | message(FATAL_ERROR 107 | "Minimum ${CMAKE_CXX_COMPILER_ID} C++ compiler version is ${MIN_COMPILER_VERSION}") 108 | endif() 109 | 110 | # If possible, use the functionality in CMake 3.1 and above (CMake 3.9 and 111 | # above for the PGI and XL compilers) to set the compiler flag for setting the 112 | # C++ dialect to C++0x or C++11. 113 | if(CMAKE_VERSION VERSION_LESS 3.1 114 | OR (CMAKE_VERSION VERSION_LESS 3.9 AND CMAKE_CXX_COMPILER_ID MATCHES "^(PGI|XL)$")) 115 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CXX_FLAGS_CXX11}") 116 | else() 117 | set(CMAKE_CXX_STANDARD 11) 118 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 119 | endif() 120 | 121 | if(APPLE AND CMAKE_CXX_COMPILER_ID MATCHES "Clang$") 122 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++") 123 | endif() 124 | 125 | # Set warning-related compiler flags. 126 | if(CMAKE_CXX_COMPILER_ID MATCHES "(Clang|^GNU)$" 127 | OR (NOT WIN32 AND CMAKE_CXX_COMPILER_ID STREQUAL "Intel")) 128 | set(CXX_FLAGS_WARNING "-Wreturn-type -Wuninitialized -Wunused-variable") 129 | elseif(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") 130 | set(CXX_FLAGS_WARNING "/wd4996") # Disable sprintf security warning. 131 | else() 132 | set(CXX_FLAGS_WARNING) 133 | endif() 134 | 135 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CXX_FLAGS_WARNING}") 136 | 137 | set(IRIS_DIR ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}) 138 | set(IRIS_INCLUDE_DIR ${CMAKE_INSTALL_INCLUDEDIR}) 139 | set(IRIS_LIBRARY_DIR ${CMAKE_INSTALL_LIBDIR}) 140 | set(IRIS_PYTHON_DIR 141 | ${CMAKE_INSTALL_LIBDIR}/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/dist-packages) 142 | 143 | add_subdirectory(cxx) 144 | add_subdirectory(python) 145 | 146 | if(WITH_MATLAB) 147 | add_subdirectory(matlab) 148 | endif() 149 | 150 | install(FILES license.txt DESTINATION ${CMAKE_INSTALL_DOCDIR}) 151 | -------------------------------------------------------------------------------- /src/cxx/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(cvxgen) 2 | 3 | add_library(iris_geometry SHARED geometry.cpp 4 | iris/geometry.h iris_cdd.h) 5 | set_target_properties(iris_geometry PROPERTIES 6 | PUBLIC_HEADER iris/geometry.h 7 | SOVERSION 0) 8 | target_include_directories(iris_geometry PRIVATE "${CDD_INCLUDE_DIR}" 9 | PUBLIC $) 10 | if(TARGET Eigen3::Eigen) 11 | target_link_libraries(iris_geometry Eigen3::Eigen) 12 | else() 13 | target_include_directories(iris_geometry PUBLIC "${EIGEN3_INCLUDE_DIR}") 14 | endif() 15 | target_link_libraries(iris_geometry "${CDD_LIBRARY}") 16 | 17 | add_library(iris_mosek SHARED iris_mosek.cpp 18 | iris/iris_mosek.h) 19 | set_target_properties(iris_mosek PROPERTIES 20 | PUBLIC_HEADER iris/iris_mosek.h 21 | SOVERSION 0) 22 | target_include_directories(iris_mosek 23 | PUBLIC $) 24 | target_link_libraries(iris_mosek iris_geometry mosek) 25 | 26 | add_library(iris SHARED iris.cpp IRISProblem.cpp) 27 | set_target_properties(iris PROPERTIES 28 | PUBLIC_HEADER iris/iris.h 29 | SOVERSION 0) 30 | target_include_directories(iris 31 | PUBLIC $) 32 | if(TARGET Eigen3::Eigen) 33 | target_link_libraries(iris Eigen3::Eigen) 34 | else() 35 | target_include_directories(iris PUBLIC "${EIGEN3_INCLUDE_DIR}") 36 | endif() 37 | target_link_libraries(iris iris_cvxgen_ldp_cpp iris_geometry iris_mosek) 38 | 39 | export(TARGETS iris iris_geometry iris_mosek NAMESPACE iris:: 40 | APPEND FILE iris-targets.cmake) 41 | 42 | install(TARGETS iris iris_geometry iris_mosek 43 | EXPORT iris-targets 44 | INCLUDES DESTINATION ${IRIS_INCLUDE_DIR} 45 | LIBRARY DESTINATION ${IRIS_LIBRARY_DIR} 46 | PUBLIC_HEADER DESTINATION ${IRIS_INCLUDE_DIR}/iris 47 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 48 | 49 | configure_file(iris.pc.in iris.pc @ONLY) 50 | 51 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/iris.pc" 52 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig) 53 | 54 | # If we are using the Eigen3::Eigen imported target, the iris package 55 | # configuration file needs a find_dependency call so that the imported target 56 | # is defined in dependent projects. 57 | if(TARGET Eigen3::Eigen) 58 | set(FIND_DEPENDENCY_EIGEN3 "find_dependency(Eigen3)") 59 | else() 60 | set(FIND_DEPENDENCY_EIGEN3) 61 | endif() 62 | 63 | configure_package_config_file(iris-config.cmake.in iris-config.cmake 64 | INSTALL_DESTINATION ${IRIS_DIR} 65 | PATH_VARS IRIS_INCLUDE_DIR IRIS_LIBRARY_DIR 66 | NO_CHECK_REQUIRED_COMPONENTS_MACRO) 67 | 68 | install(EXPORT iris-targets NAMESPACE iris:: DESTINATION ${IRIS_DIR}) 69 | 70 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/iris-config.cmake" 71 | DESTINATION ${IRIS_DIR}) 72 | 73 | if(BUILD_TESTING) 74 | add_executable(irisDemo iris_demo.cpp) 75 | target_link_libraries(irisDemo iris) 76 | add_test(NAME irisDemo COMMAND irisDemo) 77 | 78 | add_subdirectory(test) 79 | endif() 80 | 81 | pybind11_add_module(iris_wrapper iris_wrapper.cpp) 82 | target_include_directories(iris_wrapper PRIVATE "${NUMPY_INCLUDE_DIRS}") 83 | target_link_libraries(iris_wrapper 84 | PRIVATE iris iris_geometry iris_cvxgen_ldp iris_cvxgen_ldp_cpp) 85 | 86 | install(TARGETS iris_wrapper 87 | LIBRARY DESTINATION ${IRIS_PYTHON_DIR}/irispy 88 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 89 | -------------------------------------------------------------------------------- /src/cxx/IRISProblem.cpp: -------------------------------------------------------------------------------- 1 | #include "iris/iris.h" 2 | #include 3 | 4 | namespace iris { 5 | 6 | void IRISProblem::setSeedPoint(Eigen::VectorXd point) { 7 | if (point.size() != this->getDimension()) { 8 | throw(std::runtime_error("The dimension of the seed point must match the dimension of the problem")); 9 | } 10 | this->seed = Ellipsoid::fromNSphere(point); 11 | } 12 | void IRISProblem::setSeedEllipsoid(Ellipsoid ellipsoid){ 13 | if (ellipsoid.getDimension() != this->getDimension()) { 14 | throw std::runtime_error("The dimension of the seed ellipsoid must match the dimension of the problem"); 15 | } 16 | this->seed = ellipsoid; 17 | } 18 | int IRISProblem::getDimension() const { 19 | return this->dim; 20 | } 21 | Ellipsoid IRISProblem::getSeed() const { 22 | return this->seed; 23 | } 24 | void IRISProblem::setBounds(Polyhedron new_bounds) { 25 | if (new_bounds.getDimension() != this->getDimension()) { 26 | throw std::runtime_error("The dimension of the bounds must match the dimension of the problem"); 27 | } 28 | this->bounds = new_bounds; 29 | } 30 | void IRISProblem::addObstacle(Eigen::MatrixXd new_obstacle_vertices) { 31 | if (new_obstacle_vertices.rows() != this->getDimension()) { 32 | throw std::runtime_error("The matrix of obstacle vertices must have the same number of row as the dimension of the problem"); 33 | } 34 | this->obstacle_pts.push_back(new_obstacle_vertices); 35 | } 36 | const std::vector& IRISProblem::getObstacles() const { 37 | return this->obstacle_pts; 38 | } 39 | Polyhedron IRISProblem::getBounds() const { 40 | return this->bounds; 41 | } 42 | 43 | } -------------------------------------------------------------------------------- /src/cxx/cvxgen/.gitignore: -------------------------------------------------------------------------------- 1 | *.so 2 | *.o 3 | *.mexa64 4 | *.mexmaci64 5 | 6 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(iris_cvxgen_ldp SHARED solver.c matrix_support.c ldl.c util.c cvxgen_ldp.c 2 | iris/cvxgen_ldp.h solver.h) 3 | target_include_directories(iris_cvxgen_ldp 4 | PUBLIC $) 5 | set_target_properties(iris_cvxgen_ldp PROPERTIES 6 | PUBLIC_HEADER iris/cvxgen_ldp.h 7 | SOVERSION 0) 8 | 9 | add_library(iris_cvxgen_ldp_cpp SHARED cvxgen_ldp.cpp) 10 | if(TARGET Eigen3::Eigen) 11 | target_link_libraries(iris_cvxgen_ldp_cpp Eigen3::Eigen) 12 | else() 13 | target_include_directories(iris_cvxgen_ldp_cpp PUBLIC "${EIGEN3_INCLUDE_DIR}") 14 | endif() 15 | target_link_libraries(iris_cvxgen_ldp_cpp iris_cvxgen_ldp) 16 | set_target_properties(iris_cvxgen_ldp_cpp PROPERTIES SOVERSION 0) 17 | 18 | export(TARGETS iris_cvxgen_ldp iris_cvxgen_ldp_cpp NAMESPACE iris:: 19 | APPEND FILE iris-targets.cmake) 20 | 21 | install(TARGETS iris_cvxgen_ldp iris_cvxgen_ldp_cpp 22 | EXPORT iris-targets 23 | LIBRARY DESTINATION ${IRIS_LIBRARY_DIR} 24 | PUBLIC_HEADER DESTINATION ${IRIS_INCLUDE_DIR}/iris 25 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 26 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/cvxgen_ldp.c: -------------------------------------------------------------------------------- 1 | #include "iris/cvxgen_ldp.h" 2 | #include "solver.h" 3 | 4 | Vars vars; 5 | Params params; 6 | Workspace work; 7 | Settings settings; 8 | 9 | void iris_cvxgen_closest_point_in_convex_hull(double *Y, double *v) { 10 | set_defaults(); 11 | setup_indexing(); 12 | settings.verbose = 0; 13 | settings.kkt_reg = 1e-8; 14 | double *src; 15 | double *dest; 16 | int i; 17 | src = Y; 18 | dest = params.Y; 19 | for (i = 0; i < 24; i++) { 20 | *dest++ = *src++; 21 | } 22 | solve(); 23 | dest = v; 24 | src = vars.v; 25 | for (i = 0; i < 3; i++) { 26 | *dest++ = *src++; 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/cvxgen_ldp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "iris/cvxgen_ldp.h" 3 | 4 | using namespace Eigen; 5 | 6 | namespace iris_cvxgen { 7 | void closest_point_in_convex_hull(MatrixXd Points, VectorXd &result) { 8 | int m = Points.rows(); 9 | int n = Points.cols(); 10 | if (m < IRIS_CVXGEN_LDP_MAX_ROWS || n < IRIS_CVXGEN_LDP_MAX_COLS) { 11 | Points.conservativeResize(IRIS_CVXGEN_LDP_MAX_ROWS, IRIS_CVXGEN_LDP_MAX_COLS); 12 | } else if (m > IRIS_CVXGEN_LDP_MAX_ROWS) { 13 | throw(std::runtime_error("Too many rows for CVXGEN solver")); 14 | } else if (n > IRIS_CVXGEN_LDP_MAX_COLS) { 15 | throw(std::runtime_error("Too many cols for CVXGEN solver")); 16 | } 17 | 18 | if (m < IRIS_CVXGEN_LDP_MAX_ROWS) { 19 | Points.bottomRows(IRIS_CVXGEN_LDP_MAX_ROWS - m).setZero(); 20 | } 21 | for (int i=n; i < IRIS_CVXGEN_LDP_MAX_COLS; i++) { 22 | Points.col(i) = Points.col(n - 1); 23 | } 24 | 25 | // std::cout << "resized points: " << std::endl << Points << std::endl; 26 | VectorXd resized_result(IRIS_CVXGEN_LDP_MAX_ROWS); 27 | iris_cvxgen_closest_point_in_convex_hull(Points.data(), resized_result.data()); 28 | result = resized_result.head(m); 29 | return; 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/cvxgen_ldp_mex.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2014-05-20 16:06:21 -0400. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: cvxgen_ldp_mex.c. */ 8 | /* Description: mex-able file for running cvxgen solver. */ 9 | #include "mex.h" 10 | #include "solver.h" 11 | Vars vars; 12 | Params params; 13 | Workspace work; 14 | Settings settings; 15 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 16 | int i, j; 17 | mxArray *xm, *cell, *xm_cell; 18 | double *src; 19 | double *dest; 20 | double *dest_cell; 21 | int valid_vars; 22 | int steps; 23 | int this_var_errors; 24 | int warned_diags; 25 | int prepare_for_c = 0; 26 | int extra_solves; 27 | const char *status_names[] = {"optval", "gap", "steps", "converged"}; 28 | mwSize dims1x1of1[1] = {1}; 29 | mwSize dims[1]; 30 | const char *var_names[] = {"v", "w"}; 31 | const int num_var_names = 2; 32 | /* Avoid compiler warnings of unused variables by using a dummy assignment. */ 33 | warned_diags = j = 0; 34 | extra_solves = 0; 35 | set_defaults(); 36 | /* Check we got the right number of arguments. */ 37 | if (nrhs == 0) 38 | mexErrMsgTxt("Not enough arguments: You need to specify at least the parameters.\n"); 39 | if (nrhs > 1) { 40 | /* Assume that the second argument is the settings. */ 41 | if (mxGetField(prhs[1], 0, "eps") != NULL) 42 | settings.eps = *mxGetPr(mxGetField(prhs[1], 0, "eps")); 43 | if (mxGetField(prhs[1], 0, "max_iters") != NULL) 44 | settings.max_iters = *mxGetPr(mxGetField(prhs[1], 0, "max_iters")); 45 | if (mxGetField(prhs[1], 0, "refine_steps") != NULL) 46 | settings.refine_steps = *mxGetPr(mxGetField(prhs[1], 0, "refine_steps")); 47 | if (mxGetField(prhs[1], 0, "verbose") != NULL) 48 | settings.verbose = *mxGetPr(mxGetField(prhs[1], 0, "verbose")); 49 | if (mxGetField(prhs[1], 0, "better_start") != NULL) 50 | settings.better_start = *mxGetPr(mxGetField(prhs[1], 0, "better_start")); 51 | if (mxGetField(prhs[1], 0, "verbose_refinement") != NULL) 52 | settings.verbose_refinement = *mxGetPr(mxGetField(prhs[1], 0, 53 | "verbose_refinement")); 54 | if (mxGetField(prhs[1], 0, "debug") != NULL) 55 | settings.debug = *mxGetPr(mxGetField(prhs[1], 0, "debug")); 56 | if (mxGetField(prhs[1], 0, "kkt_reg") != NULL) 57 | settings.kkt_reg = *mxGetPr(mxGetField(prhs[1], 0, "kkt_reg")); 58 | if (mxGetField(prhs[1], 0, "s_init") != NULL) 59 | settings.s_init = *mxGetPr(mxGetField(prhs[1], 0, "s_init")); 60 | if (mxGetField(prhs[1], 0, "z_init") != NULL) 61 | settings.z_init = *mxGetPr(mxGetField(prhs[1], 0, "z_init")); 62 | if (mxGetField(prhs[1], 0, "resid_tol") != NULL) 63 | settings.resid_tol = *mxGetPr(mxGetField(prhs[1], 0, "resid_tol")); 64 | if (mxGetField(prhs[1], 0, "extra_solves") != NULL) 65 | extra_solves = *mxGetPr(mxGetField(prhs[1], 0, "extra_solves")); 66 | else 67 | extra_solves = 0; 68 | if (mxGetField(prhs[1], 0, "prepare_for_c") != NULL) 69 | prepare_for_c = *mxGetPr(mxGetField(prhs[1], 0, "prepare_for_c")); 70 | } 71 | valid_vars = 0; 72 | this_var_errors = 0; 73 | xm = mxGetField(prhs[0], 0, "Y"); 74 | if (xm == NULL) { 75 | printf("could not find params.Y.\n"); 76 | } else { 77 | if (!((mxGetM(xm) == 3) && (mxGetN(xm) == 8))) { 78 | printf("Y must be size (3,8), not (%d,%d).\n", mxGetM(xm), mxGetN(xm)); 79 | this_var_errors++; 80 | } 81 | if (mxIsComplex(xm)) { 82 | printf("parameter Y must be real.\n"); 83 | this_var_errors++; 84 | } 85 | if (!mxIsClass(xm, "double")) { 86 | printf("parameter Y must be a full matrix of doubles.\n"); 87 | this_var_errors++; 88 | } 89 | if (mxIsSparse(xm)) { 90 | printf("parameter Y must be a full matrix.\n"); 91 | this_var_errors++; 92 | } 93 | if (this_var_errors == 0) { 94 | dest = params.Y; 95 | src = mxGetPr(xm); 96 | for (i = 0; i < 24; i++) 97 | *dest++ = *src++; 98 | valid_vars++; 99 | } 100 | } 101 | if (valid_vars != 1) { 102 | printf("Error: %d parameters are invalid.\n", 1 - valid_vars); 103 | mexErrMsgTxt("invalid parameters found."); 104 | } 105 | if (prepare_for_c) { 106 | printf("settings.prepare_for_c == 1. thus, outputting for C.\n"); 107 | for (i = 0; i < 24; i++) 108 | printf(" params.Y[%d] = %.6g;\n", i, params.Y[i]); 109 | } 110 | /* Perform the actual solve in here. */ 111 | steps = solve(); 112 | /* For profiling purposes, allow extra silent solves if desired. */ 113 | settings.verbose = 0; 114 | for (i = 0; i < extra_solves; i++) 115 | solve(); 116 | /* Update the status variables. */ 117 | plhs[1] = mxCreateStructArray(1, dims1x1of1, 4, status_names); 118 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 119 | mxSetField(plhs[1], 0, "optval", xm); 120 | *mxGetPr(xm) = work.optval; 121 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 122 | mxSetField(plhs[1], 0, "gap", xm); 123 | *mxGetPr(xm) = work.gap; 124 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 125 | mxSetField(plhs[1], 0, "steps", xm); 126 | *mxGetPr(xm) = steps; 127 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 128 | mxSetField(plhs[1], 0, "converged", xm); 129 | *mxGetPr(xm) = work.converged; 130 | /* Extract variable values. */ 131 | plhs[0] = mxCreateStructArray(1, dims1x1of1, num_var_names, var_names); 132 | xm = mxCreateDoubleMatrix(3, 1, mxREAL); 133 | mxSetField(plhs[0], 0, "v", xm); 134 | dest = mxGetPr(xm); 135 | src = vars.v; 136 | for (i = 0; i < 3; i++) { 137 | *dest++ = *src++; 138 | } 139 | xm = mxCreateDoubleMatrix(8, 1, mxREAL); 140 | mxSetField(plhs[0], 0, "w", xm); 141 | dest = mxGetPr(xm); 142 | src = vars.w; 143 | for (i = 0; i < 8; i++) { 144 | *dest++ = *src++; 145 | } 146 | } 147 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/cvxgen_ldp_mex.m: -------------------------------------------------------------------------------- 1 | % cvxgen_ldp_mex Solves a custom quadratic program very rapidly. 2 | % 3 | % [vars, status] = cvxgen_ldp_mex(params, settings) 4 | % 5 | % solves the convex optimization problem 6 | % 7 | % minimize(sum(square(v))) 8 | % subject to 9 | % Y*w == v 10 | % sum(w) == 1 11 | % 12 | % with variables 13 | % v 3 x 1 14 | % w 8 x 1 positive 15 | % 16 | % and parameters 17 | % Y 3 x 8 18 | % 19 | % Note: 20 | % - Check status.converged, which will be 1 if optimization succeeded. 21 | % - You don't have to specify settings if you don't want to. 22 | % - To hide output, use settings.verbose = 0. 23 | % - To change iterations, use settings.max_iters = 20. 24 | % - You may wish to compare with cvxsolve to check the solver is correct. 25 | % 26 | % Specify params.Y, ..., params.Y, then run 27 | % [vars, status] = cvxgen_ldp_mex(params, settings) 28 | % Produced by CVXGEN, 2014-05-20 16:06:21 -0400. 29 | % CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 30 | % The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 31 | % CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 32 | % applications without prior written permission from Jacob Mattingley. 33 | 34 | % Filename: cvxgen_ldp_mex.m. 35 | % Description: Help file for the Matlab solver interface. 36 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/cvxsolve.m: -------------------------------------------------------------------------------- 1 | % Produced by CVXGEN, 2014-05-20 16:06:21 -0400. 2 | % CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | % The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | % CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | % applications without prior written permission from Jacob Mattingley. 6 | 7 | % Filename: cvxsolve.m. 8 | % Description: Solution file, via cvx, for use with sample.m. 9 | function [vars, status] = cvxsolve(params, settings) 10 | Y = params.Y; 11 | cvx_begin 12 | % Caution: automatically generated by cvxgen. May be incorrect. 13 | variable v(3, 1); 14 | variable w(8, 1); 15 | w >= 0; 16 | 17 | minimize(sum(square(v))); 18 | subject to 19 | Y*w == v; 20 | sum(w) == 1; 21 | cvx_end 22 | vars.v = v; 23 | vars.w = w; 24 | status.cvx_status = cvx_status; 25 | % Provide a drop-in replacement for csolve. 26 | status.optval = cvx_optval; 27 | status.converged = strcmp(cvx_status, 'Solved'); 28 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/description.cvxgen: -------------------------------------------------------------------------------- 1 | # Produced by CVXGEN, 2014-05-20 16:06:21 -0400. 2 | # CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | # The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | # CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | # applications without prior written permission from Jacob Mattingley. 6 | 7 | # Filename: description.cvxgen. 8 | # Description: A description of the CVXGEN problem. 9 | 10 | dimensions 11 | dim = 3 12 | nw = 8 13 | end 14 | 15 | parameters 16 | Y (dim, nw) 17 | end 18 | 19 | variables 20 | w (nw) nonnegative 21 | v (dim) 22 | end 23 | 24 | minimize 25 | sum(square(v)) 26 | subject to 27 | Y * w == v 28 | sum(w) == 1 29 | end 30 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/iris/cvxgen_ldp.h: -------------------------------------------------------------------------------- 1 | #ifndef _CVXGEN_LDP_H 2 | #define _CVXGEN_LDP_H 3 | 4 | #define IRIS_CVXGEN_LDP_MAX_ROWS 3 5 | #define IRIS_CVXGEN_LDP_MAX_COLS 8 6 | 7 | #ifdef __cplusplus 8 | #include 9 | namespace iris_cvxgen { 10 | void closest_point_in_convex_hull(Eigen::MatrixXd Points, Eigen::VectorXd &result); 11 | } 12 | #endif 13 | 14 | #ifdef __cplusplus 15 | extern "C" { 16 | #endif 17 | void iris_cvxgen_closest_point_in_convex_hull(double *Y, double *v); 18 | #ifdef __cplusplus 19 | } 20 | #endif 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/make_csolve.m: -------------------------------------------------------------------------------- 1 | % Produced by CVXGEN, 2014-05-20 16:06:21 -0400. 2 | % CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | % The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | % CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | % applications without prior written permission from Jacob Mattingley. 6 | 7 | % Filename: make_csolve.m. 8 | % Description: Calls mex to generate the cvxgen_ldp_mex mex file. 9 | %mex -v cvxgen_ldp_mex.c ldl.c matrix_support.c solver.c util.c 10 | mex cvxgen_ldp_mex.c ldl.c matrix_support.c solver.c util.c 11 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/matrix_support.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2014-05-20 16:06:21 -0400. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: matrix_support.c. */ 8 | /* Description: Support functions for matrix multiplication and vector filling. */ 9 | #include "solver.h" 10 | void multbymA(double *lhs, double *rhs) { 11 | lhs[0] = -rhs[0]*(-1)-rhs[3]*(params.Y[0])-rhs[4]*(params.Y[3])-rhs[5]*(params.Y[6])-rhs[6]*(params.Y[9])-rhs[7]*(params.Y[12])-rhs[8]*(params.Y[15])-rhs[9]*(params.Y[18])-rhs[10]*(params.Y[21]); 12 | lhs[1] = -rhs[1]*(-1)-rhs[3]*(params.Y[1])-rhs[4]*(params.Y[4])-rhs[5]*(params.Y[7])-rhs[6]*(params.Y[10])-rhs[7]*(params.Y[13])-rhs[8]*(params.Y[16])-rhs[9]*(params.Y[19])-rhs[10]*(params.Y[22]); 13 | lhs[2] = -rhs[2]*(-1)-rhs[3]*(params.Y[2])-rhs[4]*(params.Y[5])-rhs[5]*(params.Y[8])-rhs[6]*(params.Y[11])-rhs[7]*(params.Y[14])-rhs[8]*(params.Y[17])-rhs[9]*(params.Y[20])-rhs[10]*(params.Y[23]); 14 | lhs[3] = -rhs[3]*(1)-rhs[4]*(1)-rhs[5]*(1)-rhs[6]*(1)-rhs[7]*(1)-rhs[8]*(1)-rhs[9]*(1)-rhs[10]*(1); 15 | } 16 | void multbymAT(double *lhs, double *rhs) { 17 | lhs[0] = -rhs[0]*(-1); 18 | lhs[1] = -rhs[1]*(-1); 19 | lhs[2] = -rhs[2]*(-1); 20 | lhs[3] = -rhs[0]*(params.Y[0])-rhs[1]*(params.Y[1])-rhs[2]*(params.Y[2])-rhs[3]*(1); 21 | lhs[4] = -rhs[0]*(params.Y[3])-rhs[1]*(params.Y[4])-rhs[2]*(params.Y[5])-rhs[3]*(1); 22 | lhs[5] = -rhs[0]*(params.Y[6])-rhs[1]*(params.Y[7])-rhs[2]*(params.Y[8])-rhs[3]*(1); 23 | lhs[6] = -rhs[0]*(params.Y[9])-rhs[1]*(params.Y[10])-rhs[2]*(params.Y[11])-rhs[3]*(1); 24 | lhs[7] = -rhs[0]*(params.Y[12])-rhs[1]*(params.Y[13])-rhs[2]*(params.Y[14])-rhs[3]*(1); 25 | lhs[8] = -rhs[0]*(params.Y[15])-rhs[1]*(params.Y[16])-rhs[2]*(params.Y[17])-rhs[3]*(1); 26 | lhs[9] = -rhs[0]*(params.Y[18])-rhs[1]*(params.Y[19])-rhs[2]*(params.Y[20])-rhs[3]*(1); 27 | lhs[10] = -rhs[0]*(params.Y[21])-rhs[1]*(params.Y[22])-rhs[2]*(params.Y[23])-rhs[3]*(1); 28 | } 29 | void multbymG(double *lhs, double *rhs) { 30 | lhs[0] = -rhs[3]*(-1); 31 | lhs[1] = -rhs[4]*(-1); 32 | lhs[2] = -rhs[5]*(-1); 33 | lhs[3] = -rhs[6]*(-1); 34 | lhs[4] = -rhs[7]*(-1); 35 | lhs[5] = -rhs[8]*(-1); 36 | lhs[6] = -rhs[9]*(-1); 37 | lhs[7] = -rhs[10]*(-1); 38 | } 39 | void multbymGT(double *lhs, double *rhs) { 40 | lhs[0] = 0; 41 | lhs[1] = 0; 42 | lhs[2] = 0; 43 | lhs[3] = -rhs[0]*(-1); 44 | lhs[4] = -rhs[1]*(-1); 45 | lhs[5] = -rhs[2]*(-1); 46 | lhs[6] = -rhs[3]*(-1); 47 | lhs[7] = -rhs[4]*(-1); 48 | lhs[8] = -rhs[5]*(-1); 49 | lhs[9] = -rhs[6]*(-1); 50 | lhs[10] = -rhs[7]*(-1); 51 | } 52 | void multbyP(double *lhs, double *rhs) { 53 | /* TODO use the fact that P is symmetric? */ 54 | /* TODO check doubling / half factor etc. */ 55 | lhs[0] = rhs[0]*(2); 56 | lhs[1] = rhs[1]*(2); 57 | lhs[2] = rhs[2]*(2); 58 | lhs[3] = 0; 59 | lhs[4] = 0; 60 | lhs[5] = 0; 61 | lhs[6] = 0; 62 | lhs[7] = 0; 63 | lhs[8] = 0; 64 | lhs[9] = 0; 65 | lhs[10] = 0; 66 | } 67 | void fillq(void) { 68 | work.q[0] = 0; 69 | work.q[1] = 0; 70 | work.q[2] = 0; 71 | work.q[3] = 0; 72 | work.q[4] = 0; 73 | work.q[5] = 0; 74 | work.q[6] = 0; 75 | work.q[7] = 0; 76 | work.q[8] = 0; 77 | work.q[9] = 0; 78 | work.q[10] = 0; 79 | } 80 | void fillh(void) { 81 | work.h[0] = 0; 82 | work.h[1] = 0; 83 | work.h[2] = 0; 84 | work.h[3] = 0; 85 | work.h[4] = 0; 86 | work.h[5] = 0; 87 | work.h[6] = 0; 88 | work.h[7] = 0; 89 | } 90 | void fillb(void) { 91 | work.b[0] = 0; 92 | work.b[1] = 0; 93 | work.b[2] = 0; 94 | work.b[3] = 1; 95 | } 96 | void pre_ops(void) { 97 | } 98 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/solver.h: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2014-05-20 16:06:22 -0400. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: solver.h. */ 8 | /* Description: Header file with relevant definitions. */ 9 | #ifndef SOLVER_H 10 | #define SOLVER_H 11 | /* Uncomment the next line to remove all library dependencies. */ 12 | /*#define ZERO_LIBRARY_MODE */ 13 | #ifdef MATLAB_MEX_FILE 14 | /* Matlab functions. MATLAB_MEX_FILE will be defined by the mex compiler. */ 15 | /* If you are not using the mex compiler, this functionality will not intrude, */ 16 | /* as it will be completely disabled at compile-time. */ 17 | #include "mex.h" 18 | #else 19 | #ifndef ZERO_LIBRARY_MODE 20 | #include 21 | #endif 22 | #endif 23 | /* Space must be allocated somewhere (testsolver.c, csolve.c or your own */ 24 | /* program) for the global variables vars, params, work and settings. */ 25 | /* At the bottom of this file, they are externed. */ 26 | #ifndef ZERO_LIBRARY_MODE 27 | #include 28 | #define pm(A, m, n) printmatrix(#A, A, m, n, 1) 29 | #endif 30 | typedef struct Params_t { 31 | double Y[24]; 32 | } Params; 33 | typedef struct Vars_t { 34 | double *v; /* 3 rows. */ 35 | double *w; /* 8 rows. */ 36 | } Vars; 37 | typedef struct Workspace_t { 38 | double h[8]; 39 | double s_inv[8]; 40 | double s_inv_z[8]; 41 | double b[4]; 42 | double q[11]; 43 | double rhs[31]; 44 | double x[31]; 45 | double *s; 46 | double *z; 47 | double *y; 48 | double lhs_aff[31]; 49 | double lhs_cc[31]; 50 | double buffer[31]; 51 | double buffer2[31]; 52 | double KKT[70]; 53 | double L[57]; 54 | double d[31]; 55 | double v[31]; 56 | double d_inv[31]; 57 | double gap; 58 | double optval; 59 | double ineq_resid_squared; 60 | double eq_resid_squared; 61 | double block_33[1]; 62 | /* Pre-op symbols. */ 63 | int converged; 64 | } Workspace; 65 | typedef struct Settings_t { 66 | double resid_tol; 67 | double eps; 68 | int max_iters; 69 | int refine_steps; 70 | int better_start; 71 | /* Better start obviates the need for s_init and z_init. */ 72 | double s_init; 73 | double z_init; 74 | int verbose; 75 | /* Show extra details of the iterative refinement steps. */ 76 | int verbose_refinement; 77 | int debug; 78 | /* For regularization. Minimum value of abs(D_ii) in the kkt D factor. */ 79 | double kkt_reg; 80 | } Settings; 81 | extern Vars vars; 82 | extern Params params; 83 | extern Workspace work; 84 | extern Settings settings; 85 | /* Function definitions in ldl.c: */ 86 | void ldl_solve(double *target, double *var); 87 | void ldl_factor(void); 88 | double check_factorization(void); 89 | void matrix_multiply(double *result, double *source); 90 | double check_residual(double *target, double *multiplicand); 91 | void fill_KKT(void); 92 | 93 | /* Function definitions in matrix_support.c: */ 94 | void multbymA(double *lhs, double *rhs); 95 | void multbymAT(double *lhs, double *rhs); 96 | void multbymG(double *lhs, double *rhs); 97 | void multbymGT(double *lhs, double *rhs); 98 | void multbyP(double *lhs, double *rhs); 99 | void fillq(void); 100 | void fillh(void); 101 | void fillb(void); 102 | void pre_ops(void); 103 | 104 | /* Function definitions in solver.c: */ 105 | double eval_gap(void); 106 | void set_defaults(void); 107 | void setup_pointers(void); 108 | void setup_indexing(void); 109 | void set_start(void); 110 | double eval_objv(void); 111 | void fillrhs_aff(void); 112 | void fillrhs_cc(void); 113 | void refine(double *target, double *var); 114 | double calc_ineq_resid_squared(void); 115 | double calc_eq_resid_squared(void); 116 | void better_start(void); 117 | void fillrhs_start(void); 118 | long solve(void); 119 | 120 | /* Function definitions in testsolver.c: */ 121 | int main(int argc, char **argv); 122 | void load_default_data(void); 123 | 124 | /* Function definitions in util.c: */ 125 | void tic(void); 126 | float toc(void); 127 | float tocq(void); 128 | void printmatrix(char *name, double *A, int m, int n, int sparse); 129 | double unif(double lower, double upper); 130 | float ran1(long*idum, int reset); 131 | float randn_internal(long *idum, int reset); 132 | double randn(void); 133 | void reset_rand(void); 134 | 135 | #endif 136 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/testsolver.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2014-05-20 16:06:22 -0400. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: testsolver.c. */ 8 | /* Description: Basic test harness for solver.c. */ 9 | #include "solver.h" 10 | Vars vars; 11 | Params params; 12 | Workspace work; 13 | Settings settings; 14 | #define NUMTESTS 1000 15 | int main(int argc, char **argv) { 16 | int num_iters; 17 | #if (NUMTESTS > 0) 18 | int i; 19 | double time; 20 | double time_per; 21 | #endif 22 | set_defaults(); 23 | setup_indexing(); 24 | load_default_data(); 25 | /* Solve problem instance for the record. */ 26 | settings.verbose = 1; 27 | num_iters = solve(); 28 | #ifndef ZERO_LIBRARY_MODE 29 | #if (NUMTESTS > 0) 30 | /* Now solve multiple problem instances for timing purposes. */ 31 | settings.verbose = 0; 32 | tic(); 33 | for (i = 0; i < NUMTESTS; i++) { 34 | solve(); 35 | } 36 | time = tocq(); 37 | printf("Timed %d solves over %.3f seconds.\n", NUMTESTS, time); 38 | time_per = time / NUMTESTS; 39 | if (time_per > 1) { 40 | printf("Actual time taken per solve: %.3g s.\n", time_per); 41 | } else if (time_per > 1e-3) { 42 | printf("Actual time taken per solve: %.3g ms.\n", 1e3*time_per); 43 | } else { 44 | printf("Actual time taken per solve: %.3g us.\n", 1e6*time_per); 45 | } 46 | #endif 47 | #endif 48 | return 0; 49 | } 50 | void load_default_data(void) { 51 | params.Y[0] = 0.20319161029830202; 52 | params.Y[1] = 0.8325912904724193; 53 | params.Y[2] = -0.8363810443482227; 54 | params.Y[3] = 0.04331042079065206; 55 | params.Y[4] = 1.5717878173906188; 56 | params.Y[5] = 1.5851723557337523; 57 | params.Y[6] = -1.497658758144655; 58 | params.Y[7] = -1.171028487447253; 59 | params.Y[8] = -1.7941311867966805; 60 | params.Y[9] = -0.23676062539745413; 61 | params.Y[10] = -1.8804951564857322; 62 | params.Y[11] = -0.17266710242115568; 63 | params.Y[12] = 0.596576190459043; 64 | params.Y[13] = -0.8860508694080989; 65 | params.Y[14] = 0.7050196079205251; 66 | params.Y[15] = 0.3634512696654033; 67 | params.Y[16] = -1.9040724704913385; 68 | params.Y[17] = 0.23541635196352795; 69 | params.Y[18] = -0.9629902123701384; 70 | params.Y[19] = -0.3395952119597214; 71 | params.Y[20] = -0.865899672914725; 72 | params.Y[21] = 0.7725516732519853; 73 | params.Y[22] = -0.23818512931704205; 74 | params.Y[23] = -1.372529046100147; 75 | } 76 | -------------------------------------------------------------------------------- /src/cxx/cvxgen/util.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2014-05-20 16:06:22 -0400. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: util.c. */ 8 | /* Description: Common utility file for all cvxgen code. */ 9 | #include "solver.h" 10 | #include 11 | #include 12 | #include 13 | long global_seed = 1; 14 | static clock_t tic_timestart; 15 | void tic(void) { 16 | tic_timestart = clock(); 17 | } 18 | float toc(void) { 19 | clock_t tic_timestop; 20 | tic_timestop = clock(); 21 | printf("time: %8.2f.\n", (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC); 22 | return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC; 23 | } 24 | float tocq(void) { 25 | clock_t tic_timestop; 26 | tic_timestop = clock(); 27 | return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC; 28 | } 29 | void printmatrix(char *name, double *A, int m, int n, int sparse) { 30 | int i, j; 31 | printf("%s = [...\n", name); 32 | for (i = 0; i < m; i++) { 33 | for (j = 0; j < n; j++) 34 | if ((sparse == 1) && (A[i+j*m] == 0)) 35 | printf(" 0"); 36 | else 37 | printf(" % 9.4f", A[i+j*m]); 38 | printf(",\n"); 39 | } 40 | printf("];\n"); 41 | } 42 | double unif(double lower, double upper) { 43 | return lower + ((upper - lower)*rand())/RAND_MAX; 44 | } 45 | /* Next function is from numerical recipes in C. */ 46 | #define IA 16807 47 | #define IM 2147483647 48 | #define AM (1.0/IM) 49 | #define IQ 127773 50 | #define IR 2836 51 | #define NTAB 32 52 | #define NDIV (1+(IM-1)/NTAB) 53 | #define EPS 1.2e-7 54 | #define RNMX (1.0-EPS) 55 | float ran1(long*idum, int reset) { 56 | int j; 57 | long k; 58 | static long iy=0; 59 | static long iv[NTAB]; 60 | float temp; 61 | if (reset) { 62 | iy = 0; 63 | } 64 | if (*idum<=0||!iy) { 65 | if (-(*idum)<1)*idum=1; 66 | else *idum=-(*idum); 67 | for (j=NTAB+7; j>=0; j--) { 68 | k = (*idum)/IQ; 69 | *idum=IA*(*idum-k*IQ)-IR*k; 70 | if (*idum<0)*idum+=IM; 71 | if (j RNMX) return RNMX; 82 | else return temp; 83 | } 84 | /* Next function is from numerical recipes in C. */ 85 | float randn_internal(long *idum, int reset) { 86 | static int iset=0; 87 | static float gset; 88 | float fac, rsq, v1, v2; 89 | if (reset) { 90 | iset = 0; 91 | } 92 | if (iset==0) { 93 | do { 94 | v1 = 2.0*ran1(idum, reset)-1.0; 95 | v2 = 2.0*ran1(idum, reset)-1.0; 96 | rsq = v1*v1+v2*v2; 97 | } while(rsq >= 1.0 || rsq == 0.0); 98 | fac = sqrt(-2.0*log(rsq)/rsq); 99 | gset = v1*fac; 100 | iset = 1; 101 | return v2*fac; 102 | } else { 103 | iset = 0; 104 | return gset; 105 | } 106 | } 107 | double randn(void) { 108 | return randn_internal(&global_seed, 0); 109 | } 110 | void reset_rand(void) { 111 | srand(15); 112 | global_seed = 1; 113 | randn_internal(&global_seed, 1); 114 | } 115 | -------------------------------------------------------------------------------- /src/cxx/geometry.cpp: -------------------------------------------------------------------------------- 1 | #include "iris/geometry.h" 2 | #include 3 | #include 4 | #include 5 | #include "iris_cdd.h" 6 | 7 | namespace iris { 8 | 9 | int factorial(int n) { 10 | return n == 0 ? 1 : factorial(n - 1) * n; 11 | } 12 | 13 | double nSphereVolume(int dim, double radius) { 14 | double v; 15 | int k = std::floor(dim / 2); 16 | if (dim % 2 == 0) { 17 | v = std::pow(M_PI, k) / static_cast(factorial(k)); 18 | } else { 19 | v = (2.0 * factorial(k) * std::pow(4 * M_PI, k)) / static_cast(factorial(2 * k + 1)); 20 | } 21 | return v * std::pow(radius, dim); 22 | } 23 | 24 | Ellipsoid::Ellipsoid(int dim) : 25 | C_(Eigen::MatrixXd(dim, dim)), 26 | d_(Eigen::VectorXd(dim)) {} 27 | Ellipsoid::Ellipsoid(Eigen::MatrixXd C, Eigen::VectorXd d): 28 | C_(C), 29 | d_(d) {} 30 | const Eigen::MatrixXd& Ellipsoid::getC() const { 31 | return C_; 32 | } 33 | const Eigen::VectorXd& Ellipsoid::getD() const { 34 | return d_; 35 | } 36 | void Ellipsoid::setC(const Eigen::MatrixXd &C) { 37 | C_ = C; 38 | } 39 | void Ellipsoid::setCEntry(Eigen::DenseIndex row, 40 | Eigen::DenseIndex col, double value) { 41 | C_(row, col) = value; 42 | } 43 | void Ellipsoid::setD(const Eigen::VectorXd &d) { 44 | d_ = d; 45 | } 46 | void Ellipsoid::setDEntry(Eigen::DenseIndex index, double value) { 47 | d_(index) = value; 48 | } 49 | int Ellipsoid::getDimension() const { 50 | return C_.cols(); 51 | } 52 | double Ellipsoid::getVolume() const { 53 | return C_.determinant() * nSphereVolume(this->getDimension(), 1.0); 54 | } 55 | Ellipsoid Ellipsoid::fromNSphere(Eigen::VectorXd ¢er, double radius) { 56 | const int dim = center.size(); 57 | Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim, dim); 58 | C.diagonal().setConstant(radius); 59 | Ellipsoid ellipsoid(C, center); 60 | return ellipsoid; 61 | } 62 | 63 | Polyhedron::Polyhedron(int dim): 64 | A_(0, dim), 65 | b_(0, 1), 66 | dd_representation_dirty_(true) {} 67 | Polyhedron::Polyhedron(Eigen::MatrixXd A, Eigen::VectorXd b): 68 | A_(A), 69 | b_(b), 70 | dd_representation_dirty_(true) {} 71 | void Polyhedron::setA(const Eigen::MatrixXd &A) { 72 | A_ = A; 73 | dd_representation_dirty_ = true; 74 | } 75 | const Eigen::MatrixXd& Polyhedron::getA() const { 76 | return A_; 77 | } 78 | void Polyhedron::setB(const Eigen::VectorXd &b) { 79 | b_ = b; 80 | dd_representation_dirty_ = true; 81 | } 82 | const Eigen::VectorXd& Polyhedron::getB() const { 83 | return b_; 84 | } 85 | int Polyhedron::getDimension() const { 86 | return A_.cols(); 87 | } 88 | int Polyhedron::getNumberOfConstraints() const { 89 | return A_.rows(); 90 | } 91 | void Polyhedron::appendConstraints(const Polyhedron &other) { 92 | A_.conservativeResize(A_.rows() + other.getA().rows(), A_.cols()); 93 | A_.bottomRows(other.getA().rows()) = other.getA(); 94 | b_.conservativeResize(b_.rows() + other.getB().rows()); 95 | b_.tail(other.getB().rows()) = other.getB(); 96 | dd_representation_dirty_ = true; 97 | } 98 | void Polyhedron::updateDDRepresentation() { 99 | generator_points_.clear(); 100 | generator_rays_.clear(); 101 | getGenerators(A_, b_, generator_points_, generator_rays_); 102 | dd_representation_dirty_ = false; 103 | } 104 | std::vector Polyhedron::generatorPoints() { 105 | if (dd_representation_dirty_) { 106 | updateDDRepresentation(); 107 | } 108 | return generator_points_; 109 | } 110 | std::vector Polyhedron::generatorRays() { 111 | if (dd_representation_dirty_) { 112 | updateDDRepresentation(); 113 | } 114 | return generator_rays_; 115 | } 116 | bool Polyhedron::contains(Eigen::VectorXd point, double tolerance) { 117 | return (A_ * point - b_).maxCoeff() <= tolerance; 118 | } 119 | 120 | } -------------------------------------------------------------------------------- /src/cxx/iris-config.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include(CMakeFindDependencyMacro) 4 | 5 | @FIND_DEPENDENCY_EIGEN3@ 6 | find_dependency(mosek) 7 | 8 | set(IRIS_FOUND TRUE) 9 | set_and_check(IRIS_INCLUDE_DIRS "@PACKAGE_IRIS_INCLUDE_DIR@") 10 | set_and_check(IRIS_LIBRARY_DIRS "@PACKAGE_IRIS_LIBRARY_DIR@") 11 | set(IRIS_LIBRARIES iris::iris) 12 | 13 | include("${CMAKE_CURRENT_LIST_DIR}/iris-targets.cmake") 14 | -------------------------------------------------------------------------------- /src/cxx/iris.pc.in: -------------------------------------------------------------------------------- 1 | # Generated by CMake @CMAKE_VERSION@ for @PROJECT_NAME@. Any changes to this 2 | # file will be overwritten by the next CMake run. The input file was 3 | # iris.pc.in. 4 | 5 | prefix=@CMAKE_INSTALL_PREFIX@ 6 | exec_prefix=${prefix} 7 | libdir=${prefix}/@IRIS_LIBRARY_DIR@ 8 | includedir=${prefix}/@IRIS_INCLUDE_DIR@ 9 | 10 | Name: @PROJECT_NAME@ 11 | Description: Iterative Regional Inflation by SDP 12 | Url: https://github.com/rdeits/iris-distro 13 | Requires: eigen3 mosek 14 | Libs: -L${libdir} -lcdd -liris -liris_cvxgen_ldp -liris_cvxgen_ldp_cpp -liris_geometry -liris_mosek 15 | Cflags: -I${includedir} 16 | -------------------------------------------------------------------------------- /src/cxx/iris/geometry.h: -------------------------------------------------------------------------------- 1 | #ifndef _IRIS_GEOMETRY_H 2 | #define _IRIS_GEOMETRY_H 3 | #include 4 | #include 5 | 6 | namespace iris { 7 | 8 | const double ELLIPSOID_C_EPSILON = 1e-4; 9 | 10 | 11 | class Polyhedron { 12 | public: 13 | Polyhedron(int dim=0); 14 | Polyhedron(Eigen::MatrixXd A, Eigen::VectorXd b); 15 | ~Polyhedron() { 16 | // std::cout << "deleting polyhedron: " << this << std::endl; 17 | } 18 | void setA(const Eigen::MatrixXd &A); 19 | const Eigen::MatrixXd& getA() const; 20 | void setB(const Eigen::VectorXd &b); 21 | const Eigen::VectorXd& getB() const; 22 | int getDimension() const; 23 | int getNumberOfConstraints() const; 24 | void appendConstraints(const Polyhedron &other); 25 | std::vector generatorPoints(); 26 | std::vector generatorRays(); 27 | bool contains(Eigen::VectorXd point, double tolerance); 28 | 29 | private: 30 | Eigen::MatrixXd A_; 31 | Eigen::VectorXd b_; 32 | bool dd_representation_dirty_; 33 | std::vector generator_points_; 34 | std::vector generator_rays_; 35 | void updateDDRepresentation(); 36 | }; 37 | 38 | 39 | class Ellipsoid { 40 | public: 41 | Ellipsoid(int dim=0); 42 | Ellipsoid(Eigen::MatrixXd C, Eigen::VectorXd d); 43 | ~Ellipsoid() { 44 | // std::cout << "deleting ellipsoid: " << this << std::endl; 45 | } 46 | const Eigen::MatrixXd& getC() const; 47 | const Eigen::VectorXd& getD() const; 48 | void setC(const Eigen::MatrixXd &C_); 49 | void setCEntry(Eigen::DenseIndex row, Eigen::DenseIndex col, double value); 50 | void setD(const Eigen::VectorXd &d_); 51 | void setDEntry(Eigen::DenseIndex idx, double value); 52 | int getDimension() const; 53 | static Ellipsoid fromNSphere(Eigen::VectorXd ¢er, double radius=ELLIPSOID_C_EPSILON); 54 | double getVolume() const; 55 | 56 | private: 57 | Eigen::MatrixXd C_; 58 | Eigen::VectorXd d_; 59 | }; 60 | 61 | } 62 | 63 | 64 | #endif //def _IRIS_GEOMETRY_H -------------------------------------------------------------------------------- /src/cxx/iris/iris.h: -------------------------------------------------------------------------------- 1 | #ifndef _IRIS_H 2 | #define _IRIS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "geometry.h" 9 | 10 | namespace iris { 11 | 12 | class IRISOptions { 13 | public: 14 | // If require_containment is true and required_containment_points is empty, 15 | // then the IRIS region is required to contain the center of the seed 16 | // ellipsoid. Otherwise, the IRIS region is required to contain all points 17 | // in required_containment_points. 18 | // If require_containment is false, then required_containment_points has no 19 | // effect. 20 | bool require_containment; 21 | bool error_on_infeasible_start; 22 | double termination_threshold; 23 | int iter_limit; 24 | std::vector required_containment_points; 25 | 26 | IRISOptions(): 27 | require_containment(false), 28 | required_containment_points({}), 29 | error_on_infeasible_start(false), 30 | termination_threshold(2e-2), 31 | iter_limit(100) {}; 32 | }; 33 | 34 | class IRISRegion { 35 | public: 36 | Polyhedron polyhedron; 37 | Ellipsoid ellipsoid; 38 | 39 | Polyhedron getPolyhedron() { 40 | return polyhedron; 41 | } 42 | 43 | Ellipsoid getEllipsoid() { 44 | return ellipsoid; 45 | } 46 | 47 | IRISRegion(int dim=0): 48 | polyhedron(dim), 49 | ellipsoid(dim) 50 | {} 51 | }; 52 | 53 | class IRISDebugData { 54 | public: 55 | IRISDebugData() {}; 56 | std::vector ellipsoid_history; 57 | std::vector polyhedron_history; 58 | std::vector obstacles; 59 | std::vector getObstacles() const { 60 | return obstacles; 61 | } 62 | Polyhedron bounds; 63 | // Eigen::VectorXd ellipsoid_times; 64 | // Eigen::VectorXd polyhedron_times; 65 | // double total_time; 66 | int iters; 67 | 68 | std::vector boundingPoints() { 69 | return bounds.generatorPoints(); 70 | } 71 | }; 72 | 73 | class IRISProblem { 74 | private: 75 | std::vector obstacle_pts; // each obstacle is a matrix of size (_dim, pts_per_obstacle) 76 | Polyhedron bounds; 77 | int dim; 78 | Ellipsoid seed; 79 | 80 | public: 81 | IRISProblem(int dim): 82 | bounds(dim), 83 | dim(dim), 84 | seed(dim) { 85 | } 86 | 87 | void setSeedPoint(Eigen::VectorXd point); 88 | void setSeedEllipsoid(Ellipsoid ellipsoid); 89 | int getDimension() const; 90 | Ellipsoid getSeed() const; 91 | void setBounds(Polyhedron new_bounds); 92 | void addObstacle(Eigen::MatrixXd new_obstacle_vertices); 93 | const std::vector& getObstacles() const; 94 | Polyhedron getBounds() const; 95 | }; 96 | 97 | IRISRegion inflate_region(const IRISProblem &problem, const IRISOptions &options, IRISDebugData *debug=NULL); 98 | 99 | void separating_hyperplanes(const std::vector obstacle_pts, const Ellipsoid &ellipsoid, Polyhedron &polyhedron, bool &infeasible_start); 100 | 101 | void getGenerators(const Eigen::MatrixXd &A, const Eigen::VectorXd &b, std::vector &points, std::vector &rays); 102 | 103 | class InitialPointInfeasibleError: public std::exception { 104 | const char * what () const throw () { 105 | return "Initial point is infeasible"; 106 | } 107 | }; 108 | 109 | } 110 | 111 | #endif -------------------------------------------------------------------------------- /src/cxx/iris/iris_mosek.h: -------------------------------------------------------------------------------- 1 | #ifndef _IRIS_MOSEK_H 2 | #define _IRIS_MOSEK_H 3 | 4 | #include 5 | #include 6 | #include "geometry.h" 7 | 8 | namespace iris_mosek { 9 | 10 | class IRISMosekError : public std::exception { 11 | private: 12 | std::string message; 13 | public: 14 | explicit IRISMosekError(MSKrescodee res) { 15 | /* In case of an error print error code and description. */ 16 | char symname[MSK_MAX_STR_LEN]; 17 | char desc[MSK_MAX_STR_LEN]; 18 | MSK_getcodedesc (res, 19 | symname, 20 | desc); 21 | message = std::string(symname) + ": " + std::string(desc); 22 | } 23 | 24 | const char * what () const throw () { 25 | return message.c_str(); 26 | } 27 | ~IRISMosekError() throw() {} 28 | }; 29 | 30 | class InnerEllipsoidInfeasibleError: public std::exception { 31 | const char * what () const throw () { 32 | return "Inner ellipsoid problem is infeasible (this likely means that the polyhedron has no interior)"; 33 | } 34 | }; 35 | 36 | double inner_ellipsoid(const iris::Polyhedron &polyhedron, iris::Ellipsoid *ellipsoid, MSKenv_t *existing_env=NULL); 37 | 38 | void closest_point_in_convex_hull(const Eigen::MatrixXd &Points, Eigen::VectorXd &result, MSKenv_t *existing_env=NULL); 39 | 40 | void check_res(MSKrescodee res); 41 | 42 | } 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /src/cxx/iris_cdd.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace Eigen; 6 | 7 | namespace { 8 | struct cdd_global_constants_initializer { 9 | cdd_global_constants_initializer() { 10 | dd_set_global_constants(); 11 | std::cout << "Loading cdd global constants" << std::endl; 12 | } 13 | ~cdd_global_constants_initializer() { 14 | std::cout << "Freeing cdd global constants" << std::endl; 15 | dd_free_global_constants(); 16 | } 17 | }; 18 | static cdd_global_constants_initializer cdd_init; 19 | } 20 | 21 | void dd_check(dd_ErrorType err) { 22 | if (err != dd_NoError) { 23 | throw std::runtime_error("dd error"); 24 | } 25 | } 26 | 27 | namespace iris { 28 | 29 | void getGenerators(const MatrixXd &A, const VectorXd &b, std::vector &points, std::vector &rays) { 30 | assert(A.rows() == b.rows()); 31 | int dim = A.cols(); 32 | dd_MatrixPtr hrep = dd_CreateMatrix(A.rows(), 1 + dim); 33 | for (int i=0; i < A.rows(); i++) { 34 | dd_set_d(hrep->matrix[i][0], b(i)); 35 | for (int j=0; j < dim; j++) { 36 | dd_set_d(hrep->matrix[i][j+1], -A(i,j)); 37 | } 38 | } 39 | hrep->representation = dd_Inequality; 40 | dd_ErrorType err; 41 | dd_PolyhedraPtr poly = dd_DDMatrix2Poly(hrep, &err); 42 | dd_check(err); 43 | 44 | dd_MatrixPtr generators = dd_CopyGenerators(poly); 45 | // dd_WriteMatrix(stdout, generators); 46 | 47 | // std::cout << "rowsize: " << generators->rowsize << " colsize: " << generators->colsize << std::endl; 48 | assert(dim + 1 == generators->colsize); 49 | for (int i=0; i < generators->rowsize; i++) { 50 | VectorXd point_or_ray(dim); 51 | for (int j=0; j < dim; j++) { 52 | point_or_ray(j) = dd_get_d(generators->matrix[i][j+1]); 53 | } 54 | if (dd_get_d(generators->matrix[i][0]) == 0) { 55 | rays.push_back(point_or_ray); 56 | } else { 57 | points.push_back(point_or_ray); 58 | } 59 | } 60 | 61 | dd_FreeMatrix(hrep); 62 | dd_FreeMatrix(generators); 63 | dd_FreePolyhedra(poly); 64 | } 65 | 66 | } -------------------------------------------------------------------------------- /src/cxx/iris_demo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "iris/iris.h" 4 | 5 | int main(int argc, char** argv) { 6 | iris::IRISProblem problem(2); 7 | problem.setSeedPoint(Eigen::Vector2d(0.1, 0.1)); 8 | 9 | Eigen::MatrixXd obs(2,2); 10 | // Inflate a region inside a 1x1 box 11 | obs << 0, 1, 12 | 0, 0; 13 | problem.addObstacle(obs); 14 | obs << 1, 1, 15 | 0, 1; 16 | problem.addObstacle(obs); 17 | obs << 1, 0, 18 | 1, 1; 19 | problem.addObstacle(obs); 20 | obs << 0, 0, 21 | 1, 0; 22 | problem.addObstacle(obs); 23 | 24 | iris::IRISOptions options; 25 | iris::IRISRegion region = inflate_region(problem, options); 26 | 27 | std::cout << "C: " << region.ellipsoid.getC() << std::endl; 28 | std::cout << "d: " << region.ellipsoid.getD() << std::endl; 29 | return 0; 30 | } -------------------------------------------------------------------------------- /src/cxx/iris_wrapper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "iris/geometry.h" 6 | #include "iris/iris.h" 7 | #include "iris/iris_mosek.h" 8 | 9 | namespace py = pybind11; 10 | 11 | PYBIND11_PLUGIN(iris_wrapper) { 12 | py::module m("iris_wrapper", "low-level bindings for IRIS"); 13 | 14 | py::class_(m, "Polyhedron") 15 | .def(py::init()) 16 | .def(py::init<>()) 17 | .def(py::init()) 18 | .def("setA", &iris::Polyhedron::setA) 19 | .def("getA", &iris::Polyhedron::getA) 20 | .def("setB", &iris::Polyhedron::setB) 21 | .def("getB", &iris::Polyhedron::getB) 22 | .def("getDimension", &iris::Polyhedron::getDimension) 23 | .def("getNumberOfConstraints", &iris::Polyhedron::getNumberOfConstraints) 24 | .def("appendConstraints", &iris::Polyhedron::appendConstraints) 25 | .def("generatorPoints", &iris::Polyhedron::generatorPoints) 26 | .def("generatorRays", &iris::Polyhedron::generatorRays) 27 | .def("contains", &iris::Polyhedron::contains) 28 | ; 29 | 30 | py::class_(m, "Ellipsoid") 31 | .def(py::init()) 32 | .def(py::init<>()) 33 | .def(py::init()) 34 | .def("getC", &iris::Ellipsoid::getC) 35 | .def("getD", &iris::Ellipsoid::getD) 36 | .def("setC", &iris::Ellipsoid::setC) 37 | .def("setCEntry", &iris::Ellipsoid::setCEntry) 38 | .def("setD", &iris::Ellipsoid::setD) 39 | .def("setDEntry", &iris::Ellipsoid::setDEntry) 40 | .def("getDimension", &iris::Ellipsoid::getDimension) 41 | .def_static("fromNSphere", &iris::Ellipsoid::fromNSphere, py::arg("center"), py::arg("radius")=iris::ELLIPSOID_C_EPSILON) 42 | .def("getVolume", &iris::Ellipsoid::getVolume) 43 | ; 44 | 45 | m.def("inflate_region", &iris::inflate_region, "Solve the given IRIS problem", py::arg("IRISProblem"), py::arg("IRISOptions"), py::arg("debug") = nullptr); 46 | 47 | m.def("inner_ellipsoid", 48 | [](const iris::Polyhedron &polyhedron) { 49 | // iris::Ellipsoid* ellipsoid = new iris::Ellipsoid(polyhedron.getDimension()); 50 | iris::Ellipsoid ellipsoid(polyhedron.getDimension()); 51 | iris_mosek::inner_ellipsoid(polyhedron, &ellipsoid); 52 | return ellipsoid; 53 | }); 54 | 55 | py::class_(m, "IRISOptions") 56 | .def(py::init<>()) 57 | .def_readwrite("require_containment", &iris::IRISOptions::require_containment) 58 | .def_readwrite("error_on_infeasible_start", &iris::IRISOptions::error_on_infeasible_start) 59 | .def_readwrite("termination_threshold", &iris::IRISOptions::termination_threshold) 60 | .def_readwrite("iter_limit", &iris::IRISOptions::iter_limit) 61 | .def_readwrite("required_containment_points", &iris::IRISOptions::required_containment_points) 62 | ; 63 | 64 | py::class_(m, "IRISRegion") 65 | .def(py::init()) 66 | .def("getPolyhedron", &iris::IRISRegion::getPolyhedron) 67 | .def("getEllipsoid", &iris::IRISRegion::getEllipsoid) 68 | .def_readonly("polyhedron", &iris::IRISRegion::polyhedron) 69 | .def_readonly("ellipsoid", &iris::IRISRegion::ellipsoid) 70 | ; 71 | 72 | py::class_(m, "IRISProblem") 73 | .def(py::init()) 74 | .def("setSeedPoint", &iris::IRISProblem::setSeedPoint) 75 | .def("setSeedEllipsoid", &iris::IRISProblem::setSeedEllipsoid) 76 | .def("getDimension", &iris::IRISProblem::getDimension) 77 | .def("getSeed", &iris::IRISProblem::getSeed) 78 | .def("setBounds", &iris::IRISProblem::setBounds) 79 | .def("addObstacle", &iris::IRISProblem::addObstacle) 80 | .def("getObstacles", &iris::IRISProblem::getObstacles) 81 | .def("getBounds", &iris::IRISProblem::getBounds) 82 | ; 83 | 84 | py::class_(m, "IRISDebugData") 85 | .def(py::init<>()) 86 | .def_readonly("ellipsoid_history", &iris::IRISDebugData::ellipsoid_history) 87 | .def_readonly("polyhedron_history", &iris::IRISDebugData::polyhedron_history) 88 | .def_readonly("obstacles", &iris::IRISDebugData::obstacles) 89 | .def_readonly("bounds", &iris::IRISDebugData::bounds) 90 | .def_readonly("iters", &iris::IRISDebugData::iters) 91 | .def("boundingPoints", &iris::IRISDebugData::boundingPoints) 92 | .def("getObstacles", &iris::IRISDebugData::getObstacles) 93 | ; 94 | 95 | return m.ptr(); 96 | } 97 | -------------------------------------------------------------------------------- /src/cxx/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(add_iris_test base_name) 2 | add_executable(${base_name} ${base_name}.cpp) 3 | target_link_libraries(${base_name} iris) 4 | add_test(NAME ${base_name} COMMAND ${base_name}) 5 | endfunction() 6 | 7 | add_iris_test(test_cdd) 8 | add_iris_test(test_append_polyhedron) 9 | add_iris_test(test_mosek_ellipsoid) 10 | add_iris_test(test_for_thin_mosek_ellipsoid_bug) 11 | add_iris_test(test_infeasible_ellipsoid) 12 | add_iris_test(test_closest_point) 13 | add_iris_test(test_separating_hyperplanes) 14 | add_iris_test(test_iris) 15 | add_iris_test(test_required_containment) 16 | -------------------------------------------------------------------------------- /src/cxx/test/test_append_polyhedron.cpp: -------------------------------------------------------------------------------- 1 | #include "test_util.h" 2 | #include 3 | #include "iris/iris.h" 4 | 5 | int main () { 6 | 7 | Eigen::MatrixXd A(3,2); 8 | A << -1, 0, 9 | 0, -1, 10 | 1, 1; 11 | Eigen::VectorXd b(3); 12 | b << 0, 0, 1; 13 | iris::Polyhedron p(A, b); 14 | 15 | Eigen::MatrixXd A2(2,2); 16 | A2 << 2, 3, 17 | 4, 5; 18 | Eigen::VectorXd b2(2); 19 | b2 << 6, 7; 20 | iris::Polyhedron other(A2, b2); 21 | 22 | p.appendConstraints(other); 23 | 24 | valuecheck(p.getNumberOfConstraints(), 5); 25 | Eigen::MatrixXd A_expected(5,2); 26 | A_expected << -1, 0, 27 | 0, -1, 28 | 1, 1, 29 | 2, 3, 30 | 4, 5; 31 | valuecheckMatrix(p.getA(), A_expected, 1e-12); 32 | 33 | return 0; 34 | } -------------------------------------------------------------------------------- /src/cxx/test/test_cdd.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "iris/iris.h" 3 | #include "test_util.h" 4 | 5 | using namespace Eigen; 6 | 7 | int main(int argc, char **argv) { 8 | MatrixXd A(4,2); 9 | A << 1, 0, 10 | 0, 1, 11 | -1, 0, 12 | 0, -1; 13 | VectorXd b(4); 14 | b << 1, 1, 0.5, 0.5; 15 | 16 | iris::Polyhedron poly(A, b); 17 | std::vector points = poly.generatorPoints(); 18 | 19 | for (auto pt = points.begin(); pt != points.end(); ++pt) { 20 | std::cout << pt->transpose() << std::endl; 21 | } 22 | 23 | 24 | MatrixXd pts_expected(2,4); 25 | pts_expected << -0.5, 1.0, 1.0, -0.5, 26 | -0.5, -0.5, 1.0, 1.0; 27 | 28 | for (auto pt = points.begin(); pt != points.end(); ++pt) { 29 | valuecheckMatrix(*pt, pts_expected.col(pt - points.begin()), 1e-6); 30 | } 31 | 32 | return 0; 33 | } -------------------------------------------------------------------------------- /src/cxx/test/test_closest_point.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "iris/iris.h" 3 | #include "iris/iris_mosek.h" 4 | #include "iris/cvxgen_ldp.h" 5 | #include "test_util.h" 6 | 7 | int main() { 8 | Eigen::MatrixXd points(2, 4); 9 | points << 0, 0, 1, 1, 10 | 0, 1, 0, 1; 11 | Eigen::VectorXd result(2); 12 | Eigen::VectorXd expected(2); 13 | expected << 0, 0; 14 | iris_mosek::closest_point_in_convex_hull(points, result); 15 | valuecheckMatrix(result, expected, 1e-6); 16 | 17 | iris_cvxgen::closest_point_in_convex_hull(points, result); 18 | valuecheckMatrix(result, expected, 1e-2); 19 | 20 | points << -2, -1, -1, 0, 21 | -1, -2, 0, -1; 22 | expected << -0.5, -0.5; 23 | iris_mosek::closest_point_in_convex_hull(points, result); 24 | valuecheckMatrix(result, expected, 1e-6); 25 | 26 | iris_cvxgen::closest_point_in_convex_hull(points, result); 27 | valuecheckMatrix(result, expected, 1e-2); 28 | 29 | Eigen::MatrixXd line(2,2); 30 | line << 9000, 9000, 31 | -1000, 9000; 32 | iris_cvxgen::closest_point_in_convex_hull(line, result); 33 | expected << 9000, 0; 34 | valuecheckMatrix(result, expected, 1); 35 | 36 | 37 | Eigen::MatrixXd points5(5, 32); 38 | points5 << -1872.48,-1829.59,-1947.79,-1783.39,-1712.13,-1757.48,-1797.66,-1753.24,-1767.25,-1926.22,-1926.78,-1738.69,-1813.46,-1952.15,-1888.93,-1821.83,-1894.04,-1832.17,-1885.76,-1760.49,-1885.13,-1921.02,-1864.35,-1845.11,-1856.77,-1824.37,-1828.71,-1863.75,-1754.67,-1822.5,-1717.14,-1746.51, 39 | -1766.57,-1722.45,-1600.82,-1622.11,-1635.21,-1774.41,-1708.03,-1673.71,-1744.08,-1825.43,-1634.01,-1622.61,-1750.8,-1695.51,-1692.97,-1616.53,-1637.12,-1696.21,-1833.12,-1692.46,-1769.92,-1655.63,-1665.76,-1813.13,-1764.97,-1608.74,-1776.96,-1731.31,-1760.92,-1652.26,-1639.92,-1742.6, 40 | -868.916,-1087.29,-1032.94,-1070.76,-1028.55,-966.867,-941.882,-1062.21,-1090.7,-1097.47,-955.821,-873.972,-969.474,-1039.29,-1067.84,-955.372,-894.304,-901.631,-1088.84,-895.907,-930.735,-1074.37,-889.848,-880.109,-938.567,-1046.24,-1084.81,-994.232,-1006.57,-1048.34,-892.176,-952.407, 41 | 2596.61,2527.3,2395.15,2595.03,2446.58,2585.61,2572.42,2362.46,2553.85,2379.93,2471.96,2514.34,2567.2,2476.63,2378.64,2413.77,2398.53,2540.33,2411.03,2514.93,2552.98,2538.95,2580.4,2413.49,2563.99,2543.24,2380.5,2599.27,2389.72,2430.37,2578.74,2520.03, 42 | -680.843,-573.134,-775.128,-723.158,-627.324,-665.019,-608.212,-784.947,-718.664,-747.286,-627.046,-723.587,-572.384,-610.751,-581.003,-623.084,-638.895,-618.737,-682.26,-760.279,-661.069,-661.551,-640.068,-763.236,-601.584,-578.084,-582.512,-711.361,-654.364,-586.518,-666.37,-742.146; 43 | result.resize(5); 44 | iris_mosek::closest_point_in_convex_hull(points5, result); 45 | 46 | expected.resize(5); 47 | expected << -1717.5, -1634.3, -1025.0, 2445.0, -627.1; 48 | valuecheckMatrix(result, expected, 1); 49 | 50 | return 0; 51 | } -------------------------------------------------------------------------------- /src/cxx/test/test_for_thin_mosek_ellipsoid_bug.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "iris/iris.h" 3 | #include "iris/iris_mosek.h" 4 | #include "test_util.h" 5 | 6 | int main() { 7 | Eigen::MatrixXd A(4,3); 8 | A << -1, 0, 0, 9 | 0, -1, 0, 10 | 0, 0, -1, 11 | 1, 1, 1; 12 | Eigen::VectorXd b(4); 13 | b << 0, 0, 0, 1; 14 | iris::Polyhedron polyhedron(A, b); 15 | 16 | iris::Ellipsoid ellipsoid(3); 17 | 18 | iris_mosek::inner_ellipsoid(polyhedron, &ellipsoid); 19 | 20 | Eigen::MatrixXd C_expected(3,3); 21 | C_expected << 0.2523, -0.0740, -0.0740, 22 | -0.0740, 0.2523, -0.0740, 23 | -0.0740, -0.0740, 0.2523; 24 | Eigen::VectorXd d_expected(3); 25 | d_expected << 0.2732, 0.2732, 0.2732; 26 | valuecheckMatrix(ellipsoid.getC(), C_expected, 1e-3); 27 | valuecheckMatrix(ellipsoid.getD(), d_expected, 1e-3); 28 | 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /src/cxx/test/test_infeasible_ellipsoid.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "iris/iris.h" 3 | #include "iris/iris_mosek.h" 4 | #include "test_util.h" 5 | 6 | int main() { 7 | Eigen::MatrixXd A(3,2); 8 | A << -1, 0, 9 | 0, -1, 10 | 1, 1; 11 | Eigen::VectorXd b(3); 12 | b << 0, 0, -1; 13 | iris::Polyhedron polyhedron(A, b); 14 | 15 | iris::Ellipsoid ellipsoid(2); 16 | 17 | 18 | try { 19 | iris_mosek::inner_ellipsoid(polyhedron, &ellipsoid); 20 | } catch (iris_mosek::InnerEllipsoidInfeasibleError &e) { 21 | return 0; 22 | } 23 | throw(std::runtime_error("expected an infeasible ellipsoid error")); 24 | return 1; 25 | } -------------------------------------------------------------------------------- /src/cxx/test/test_iris.cpp: -------------------------------------------------------------------------------- 1 | #include "test_util.h" 2 | #include "iris/iris.h" 3 | 4 | int main() { 5 | iris::IRISProblem problem(2); 6 | problem.setSeedPoint(Eigen::Vector2d(0.1, 0.1)); 7 | 8 | Eigen::MatrixXd obs(2,2); 9 | // Inflate a region inside a 1x1 box 10 | obs << 0, 1, 11 | 0, 0; 12 | problem.addObstacle(obs); 13 | obs << 1, 1, 14 | 0, 1; 15 | problem.addObstacle(obs); 16 | obs << 1, 0, 17 | 1, 1; 18 | problem.addObstacle(obs); 19 | obs << 0, 0, 20 | 1, 0; 21 | problem.addObstacle(obs); 22 | 23 | iris::IRISOptions options; 24 | auto region = inflate_region(problem, options); 25 | Eigen::MatrixXd C_expected(2,2); 26 | Eigen::VectorXd d_expected(2); 27 | C_expected << 0.5, 0, 28 | 0, 0.5; 29 | d_expected << 0.5, 0.5; 30 | valuecheckMatrix(region.ellipsoid.getC(), C_expected, 1e-3); 31 | valuecheckMatrix(region.ellipsoid.getD(), d_expected, 1e-3); 32 | 33 | return 0; 34 | } 35 | 36 | -------------------------------------------------------------------------------- /src/cxx/test/test_mosek_ellipsoid.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "iris/iris.h" 3 | #include "iris/iris_mosek.h" 4 | #include "test_util.h" 5 | 6 | int main() { 7 | Eigen::MatrixXd A(3,2); 8 | A << -1, 0, 9 | 0, -1, 10 | 1, 1; 11 | Eigen::VectorXd b(3); 12 | b << 0, 0, 1; 13 | iris::Polyhedron polyhedron(A, b); 14 | 15 | iris::Ellipsoid ellipsoid(2); 16 | 17 | iris_mosek::inner_ellipsoid(polyhedron, &ellipsoid); 18 | 19 | Eigen::MatrixXd C_expected(2,2); 20 | C_expected << 0.332799, -0.132021, 21 | -0.132021, 0.332799; 22 | Eigen::VectorXd d_expected(2); 23 | d_expected << 0.358029, 0.358029; 24 | valuecheckMatrix(ellipsoid.getC(), C_expected, 1e-5); 25 | valuecheckMatrix(ellipsoid.getD(), d_expected, 1e-5); 26 | 27 | return 0; 28 | } -------------------------------------------------------------------------------- /src/cxx/test/test_ppl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ppl.hh" 3 | #include "iris/iris.h" 4 | 5 | // Note: this test is no longer in use (and never really worked anyway). I'm just keeping it around in case I ever want to try again with PPL. 6 | 7 | using namespace Parma_Polyhedra_Library; 8 | using namespace Eigen; 9 | 10 | struct Floating_Real_Open_Interval_Info_Policy { 11 | const_bool_nodef(store_special, false); 12 | const_bool_nodef(store_open, true); 13 | const_bool_nodef(cache_empty, true); 14 | const_bool_nodef(cache_singleton, true); 15 | const_bool_nodef(cache_normalized, false); 16 | const_int_nodef(next_bit, 0); 17 | const_bool_nodef(may_be_empty, true); 18 | const_bool_nodef(may_contain_infinity, false); 19 | const_bool_nodef(check_empty_result, false); 20 | const_bool_nodef(check_inexact, false); 21 | }; 22 | 23 | typedef Interval_Info_Bitset Floating_Real_Open_Interval_Info; 25 | 26 | //! The type of an interval with floating point boundaries. 27 | typedef Interval FP_Interval; 29 | 30 | //! The type of an interval linear form. 31 | typedef Linear_Form FP_Linear_Form; 32 | 33 | //! The type of an interval abstract store. 34 | typedef Box FP_Interval_Abstract_Store; 35 | 36 | void getGenerators(const Polyhedron* self) { 37 | const int dim = self->getDimension(); 38 | NNC_Polyhedron ppl_polyhedron(dim); 39 | std::vector vars; 40 | for (int i=0; i < dim; i++) { 41 | Variable v(i); 42 | vars.push_back(v); 43 | } 44 | for (int i=0; i < self->getNumberOfConstraints(); i++) { 45 | FP_Linear_Form expr; 46 | for (int j=0; j < dim; j++) { 47 | expr += FP_Linear_Form(self->getA()(i,j) * vars[j]); 48 | } 49 | expr.print(); 50 | printf("\n"); 51 | FP_Linear_Form right(self->getB()(i) + 0.0 * vars[0]); 52 | right.print(); 53 | printf("\n"); 54 | ppl_polyhedron.refine_with_linear_form_inequality(expr, right); 55 | // ppl_polyhedron.add_constraint(expr <= Linear_Form(self->getB()(i))); 56 | } 57 | 58 | auto generators = ppl_polyhedron.generators(); 59 | generators.print(); 60 | std::cout << std::endl; 61 | for (auto gen = generators.begin(); gen != generators.end(); ++gen) { 62 | gen->print(); 63 | // for (int i=0; i < dim; i++) { 64 | // printf("%f", static_cast(gen->coefficient(vars[i]))); 65 | // } 66 | printf("\n"); 67 | // printf(" %d\n", static_cast gen->divisor()); 68 | } 69 | 70 | ppl_polyhedron.constraints().print(); 71 | } 72 | 73 | 74 | int main(int argc, char **argv) { 75 | 76 | MatrixXd A(4,2); 77 | A << 1.1, 0.9, 78 | 0, 1, 79 | -1, 0, 80 | 0, -1; 81 | VectorXd b(4); 82 | b << 0.5, 0.5, 0.5, 0.5; 83 | Polyhedron poly(A, b); 84 | getGenerators(&poly); 85 | 86 | return 0; 87 | } -------------------------------------------------------------------------------- /src/cxx/test/test_required_containment.cpp: -------------------------------------------------------------------------------- 1 | #include "test_util.h" 2 | #include "iris/iris.h" 3 | 4 | int main() { 5 | // Run IRIS with the required_containment_points including a point which is outside the feasible set, which should result in an empty polyhedron 6 | 7 | iris::IRISProblem problem(2); 8 | problem.setSeedPoint(Eigen::Vector2d(0.1, 0.1)); 9 | 10 | Eigen::MatrixXd obs(2,2); 11 | // Inflate a region inside a 1x1 box 12 | obs << 0, 1, 13 | 0, 0; 14 | problem.addObstacle(obs); 15 | obs << 1, 1, 16 | 0, 1; 17 | problem.addObstacle(obs); 18 | obs << 1, 0, 19 | 1, 1; 20 | problem.addObstacle(obs); 21 | obs << 0, 0, 22 | 1, 0; 23 | problem.addObstacle(obs); 24 | 25 | iris::IRISOptions options; 26 | options.require_containment = true; 27 | std::vector required_containment_points = {Eigen::Vector2d(1.5, 1.5)}; 28 | options.required_containment_points = required_containment_points; 29 | 30 | auto region = inflate_region(problem, options); 31 | if (region.polyhedron.getNumberOfConstraints() > 0) { 32 | throw std::runtime_error("polyhedron should be empty"); 33 | } 34 | 35 | return 0; 36 | } -------------------------------------------------------------------------------- /src/cxx/test/test_separating_hyperplanes.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "iris/iris.h" 3 | #include "test_util.h" 4 | 5 | int main() { 6 | // A simple example with a sphere centered at the origin 7 | Eigen::MatrixXd C(2,2); 8 | C << 1, 0, 9 | 0, 1; 10 | Eigen::VectorXd d(2); 11 | d << 0, 0; 12 | iris::Ellipsoid ellipsoid(C, d); 13 | 14 | Eigen::MatrixXd obs(2,4); 15 | obs << 2, 3, 3, 2, 16 | 2, 2, 3, 3; 17 | std::vector obstacles; 18 | obstacles.push_back(obs); 19 | 20 | iris::Polyhedron result(2); 21 | bool infeasible_start; 22 | iris::separating_hyperplanes(obstacles, ellipsoid, result, infeasible_start); 23 | valuecheck(infeasible_start, false); 24 | Eigen::MatrixXd A_expected(1, 2); 25 | A_expected << 1/std::sqrt(2), 1/std::sqrt(2); 26 | Eigen::VectorXd b_expected(1); 27 | b_expected << 2.0 * 2.0 * 1.0/std::sqrt(2); 28 | valuecheckMatrix(result.getA(), A_expected, 1e-6); 29 | valuecheckMatrix(result.getB(), b_expected, 1e-6); 30 | 31 | return 0; 32 | } -------------------------------------------------------------------------------- /src/cxx/test/test_util.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // valuecheck() and valuecheckMatrix are taken from the open-source Drake toolbox for planning and control: http://drake.mit.edu 6 | template 7 | void valuecheck(const T& a, const T& b) 8 | { 9 | if (a != b) { 10 | std::ostringstream stream; 11 | stream << "Expected:\n" << a << "\nbut got:" << b << "\n"; 12 | throw std::runtime_error(stream.str()); 13 | } 14 | } 15 | 16 | template 17 | std::string to_string(const Eigen::MatrixBase & a) 18 | { 19 | std::stringstream ss; 20 | ss << a; 21 | return ss.str(); 22 | } 23 | 24 | inline bool isNaN(double x) { 25 | #ifdef WIN32 26 | return _isnan(x) != 0; 27 | #else 28 | return std::isnan(x); 29 | #endif 30 | } 31 | 32 | template 33 | void valuecheckMatrix(const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, double tol, std::string error_msg = "") 34 | { 35 | // note: isApprox uses the L2 norm, so is bad for comparing against zero 36 | if (a.rows() != b.rows() || a.cols() != b.cols()) { 37 | throw std::runtime_error( 38 | "Drake:ValueCheck ERROR:" + error_msg + "size mismatch: (" + std::to_string(static_cast(a.rows())) + " by " + std::to_string(static_cast(a.cols())) + ") and (" + std::to_string(static_cast(b.rows())) + " by " 39 | + std::to_string(static_cast(b.cols())) + ")"); 40 | } 41 | if (!(a - b).isZero(tol)) { 42 | if (!a.allFinite() && !b.allFinite()) { 43 | // could be failing because inf-inf = nan 44 | bool ok = true; 45 | for (int i = 0; i < a.rows(); i++) 46 | for (int j = 0; j < a.cols(); j++) { 47 | bool both_positive_infinity = a(i, j) == std::numeric_limits::infinity() && b(i, j) == std::numeric_limits::infinity(); 48 | bool both_negative_infinity = a(i, j) == -std::numeric_limits::infinity() && b(i, j) == -std::numeric_limits::infinity(); 49 | bool both_nan = std::isnan(a(i, j)) && std::isnan(b(i, j)); 50 | ok = ok && (both_positive_infinity || both_negative_infinity || (both_nan) || (std::abs(a(i, j) - b(i, j)) < tol)); 51 | } 52 | if (ok) 53 | return; 54 | } 55 | error_msg += "A:\n" + to_string(a) + "\nB:\n" + to_string(b) + "\n"; 56 | throw std::runtime_error("Drake:ValueCheck ERROR:" + error_msg); 57 | } 58 | } -------------------------------------------------------------------------------- /src/license.txt: -------------------------------------------------------------------------------- 1 | All software except matlab/+iris/+thirdParty and python/irispy/mosek is: 2 | Copyright (c) 2014, Robin Deits 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 18 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | 25 | Software in matlab+iris/+thirdParty/+mosek_lownerjohn and python/irispy/mosek/lownerjohn_ellipsoid.py is: 26 | Copyright (c) MOSEK ApS, Denmark 27 | and used with permission for non-commercial distribution. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 30 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 31 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 32 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 33 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 34 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 35 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 36 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 37 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 38 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 39 | -------------------------------------------------------------------------------- /src/matlab/+iris/+cspace/cspace3.m: -------------------------------------------------------------------------------- 1 | function [c] = cspace3(obs, bot, theta_steps, post_rotation) 2 | if nargin < 4 3 | post_rotation = eye(2); 4 | end 5 | import iris.cspace.minkowski_sum; 6 | if iscell(obs) 7 | padded = iris.pad_obstacle_points(obs); 8 | obs = cell2mat(reshape(padded, size(padded, 1), [], length(obs))); 9 | end 10 | bot = -bot; 11 | 12 | if isscalar(theta_steps) 13 | th = linspace(-pi, pi, theta_steps); 14 | else 15 | th = theta_steps; 16 | end 17 | 18 | c = zeros(3, 2*size(bot, 2), size(obs,3) * (length(th)-1)); 19 | 20 | if size(obs, 2) == 1 21 | idx = 0; 22 | for j = 1:(length(th)-1) 23 | rbot0 = post_rotation * iris.util.rotmat(th(j)) * bot; 24 | c_obs0 = bsxfun(@plus, rbot0, obs); 25 | 26 | rbot1 = post_rotation * iris.util.rotmat(th(j+1)) * bot; 27 | c_obs1 = bsxfun(@plus, rbot1, obs); 28 | 29 | c(:,:,idx+(1:size(obs,3))) = [c_obs0, c_obs1; 30 | th(j)-1e-3+zeros([1,size(c_obs0,2),size(obs,3)]), th(j+1)+1e-3+zeros([1,size(c_obs1,2),size(obs,3)])]; 31 | idx = idx + size(obs,3); 32 | end 33 | else 34 | idx = 1; 35 | for j = 1:(length(th)-1) 36 | for k = 1:size(obs,3) 37 | rbot0 = post_rotation * iris.util.rotmat(th(j)) * bot; 38 | c_obs0 = minkowski_sum(rbot0, obs(:,:,k)); 39 | 40 | rbot1 = post_rotation * iris.util.rotmat(th(j+1)) * bot; 41 | c_obs1 = minkowski_sum(rbot1, obs(:,:,k)); 42 | c_pts = [c_obs0, c_obs1; 43 | th(j)*ones(1,size(c_obs0,2)) - 1e-3, th(j+1)*ones(1,size(c_obs1,2)) + 1e-3]; 44 | if size(c_pts, 2) > size(c, 2) 45 | c = [c, zeros([size(c, 1), size(c_pts,2)-size(c,2), size(c,3)])]; 46 | end 47 | c(:,:,idx) = c_pts; 48 | idx = idx + 1; 49 | end 50 | end 51 | end -------------------------------------------------------------------------------- /src/matlab/+iris/+cspace/minkowski_sum.m: -------------------------------------------------------------------------------- 1 | function p = minkowski_sum(a, b) 2 | 3 | if size(a, 2) == 1 || size(b, 2) == 1 4 | p = bsxfun(@plus, a, b); 5 | else 6 | p = zeros(2, size(a, 2) * size(b, 2)); 7 | idx = 0; 8 | for j = 1:size(a, 2) 9 | p(:,idx+(1:size(b,2))) = bsxfun(@plus, a(:,j), b); 10 | idx = idx + size(b,2); 11 | end 12 | k = convhull(p(1,:), p(2,:), 'simplify', true); 13 | p = p(:,k(1:end-1)); 14 | end 15 | -------------------------------------------------------------------------------- /src/matlab/+iris/+cspace/project_c_space_region.m: -------------------------------------------------------------------------------- 1 | function [inner_poly, outer_poly] = project_c_space_region(A, b ) 2 | % Given a polyhedron of free C-space Ax <= b, project it down into the plane. 3 | % inner_poly is a patch of xy poses where the polyhedron contains all orientations of the 4 | % bot, and outer_poly is a patch of xy poses where the polyhedron contains only some 5 | % orientations of the bot. inner_poly may be empty. 6 | 7 | import iris.thirdParty.polytopes.lcon2vert; 8 | % figure(h) 9 | V = lcon2vert(A, b)'; 10 | 11 | outer_poly = V(1:2,convhull(V(1,:), V(2,:), 'simplify', true)); 12 | % patch(projected_outer_poly(1,:), projected_outer_poly(2,:), 'y', 'FaceAlpha', 0.5); 13 | 14 | top_face = []; 15 | bottom_face = []; 16 | 17 | top_face_pts = V(:,abs(V(3,:) - pi) < 1e-4); 18 | if size(top_face_pts, 2) > 2 19 | try 20 | k = convhull(top_face_pts(1,:), top_face_pts(2,:), 'simplify', true); 21 | top_face = top_face_pts(:,k(end:-1:1)); 22 | end 23 | end 24 | 25 | bottom_face_pts = V(:,abs(V(3,:) + pi) < 1e-4); 26 | if size(bottom_face_pts, 2) > 2 27 | try 28 | k = convhull(bottom_face_pts(1,:), bottom_face_pts(2,:), 'simplify', true); 29 | bottom_face = bottom_face_pts(:,k(end:-1:1)); 30 | end 31 | end 32 | 33 | inner_poly = []; 34 | if ~isempty(top_face) && ~isempty(bottom_face) 35 | [inter_x, inter_y] = polybool('intersection',... 36 | top_face(1,:), top_face(2,:),... 37 | bottom_face(1,:), bottom_face(2,:)); 38 | if ~isempty(inter_x) 39 | inner_poly(1,:) = inter_x; 40 | inner_poly(2,:) = inter_y; 41 | end 42 | end 43 | 44 | end 45 | 46 | -------------------------------------------------------------------------------- /src/matlab/+iris/+drawing/animate_results.m: -------------------------------------------------------------------------------- 1 | function animate_results(results, record, folder_name) 2 | import iris.thirdParty.polytopes.*; 3 | 4 | if nargin < 2 5 | record = false; 6 | else 7 | if nargin < 3 8 | folder_name = ['videos/', datestr(now,'yyyy-mm-dd_HH.MM.SS')]; 9 | end 10 | end 11 | 12 | dim = length(results.start); 13 | C = results.e_history{1}.C; 14 | d = results.e_history{1}.d; 15 | A_bounds = results.p_history{1}.A(end-2*dim+1:end,:); 16 | b_bounds = results.p_history{1}.b(end-2*dim+1:end); 17 | bound_pts = lcon2vert(A_bounds, b_bounds); 18 | lb = min(bound_pts)'; 19 | ub = max(bound_pts)'; 20 | 21 | if record 22 | frame = 1; 23 | system(sprintf('mkdir -p %s', folder_name)); 24 | w = VideoWriter([folder_name, '/', 'animation']); 25 | w.FrameRate = 5; 26 | w.open(); 27 | save([folder_name, '/', 'results'], 'results'); 28 | 29 | delay = 1/w.FrameRate; 30 | delay1 = 1; 31 | delay2 = 2; 32 | gif_fname = [folder_name, '/', 'animation.gif']; 33 | loops = 65525; 34 | 35 | draw([], [], C, d, results.obstacles, lb, ub, results); 36 | h = gcf; 37 | movegui(h); 38 | w.writeVideo(getframe(h)); 39 | print(gcf, sprintf('%s/%d_a', folder_name, 0), '-dpdf'); 40 | img = getframe(); 41 | [M, c_map]= rgb2ind(img.cdata,256); 42 | imwrite(M,c_map,[gif_fname],'gif','LoopCount',loops,'DelayTime',delay1); 43 | end 44 | 45 | figure(2); 46 | clf; 47 | for j = 1:length(results.p_history) 48 | A = results.p_history{j}.A; 49 | b = results.p_history{j}.b; 50 | draw(A, b, C, d, results.obstacles, lb, ub, results); 51 | if record 52 | h = gcf; 53 | movegui(h); 54 | w.writeVideo(getframe(h)); 55 | print(gcf, sprintf('%s/%d_a', folder_name, j), '-dpdf'); 56 | img = getframe(); 57 | [M, c_map]= rgb2ind(img.cdata,256); 58 | imwrite(M,c_map,[gif_fname],'gif','WriteMode','append','DelayTime',delay) 59 | end 60 | % pause(0.1); 61 | C = results.e_history{j+1}.C; 62 | d = results.e_history{j+1}.d; 63 | draw(A, b, C, d, results.obstacles, lb, ub, results); 64 | if record 65 | h = gcf; 66 | movegui(h); 67 | w.writeVideo(getframe(h)); 68 | print(gcf, sprintf('%s/%d_b', folder_name, j), '-dpdf', '-painters'); 69 | img = getframe(); 70 | [M, c_map]= rgb2ind(img.cdata,256); 71 | if j==length(results.p_history) 72 | imwrite(M,c_map,[gif_fname],'gif','WriteMode','append','DelayTime',delay2); 73 | else 74 | imwrite(M,c_map,[gif_fname],'gif','WriteMode','append','DelayTime',delay); 75 | end 76 | end 77 | % pause(0.25); 78 | end 79 | 80 | if record 81 | w.close(); 82 | end 83 | end 84 | 85 | function draw(A, b, C, d, obstacles, lb, ub, results) 86 | import iris.drawing.*; 87 | dim = length(results.start); 88 | if dim == 2 89 | draw_2d(A,b,C,d,obstacles,lb,ub); 90 | plot(results.start(1), results.start(2), 'go', 'MarkerFaceColor', 'g', 'MarkerSize', 8); 91 | elseif dim == 3 92 | draw_3d(A,b,C,d,obstacles,lb,ub); 93 | plot3(results.start(1), results.start(2), results.start(3), 'go', ... 94 | 'MarkerFaceColor', 'g', 'MarkerSize', 15); 95 | else 96 | draw_Nd(A,b,C,d,obstacles,lb,ub); 97 | end 98 | end -------------------------------------------------------------------------------- /src/matlab/+iris/+drawing/drawPolyFromVertices.m: -------------------------------------------------------------------------------- 1 | function drawPolyFromVertices(c, color, varargin) 2 | 3 | if isempty(c) 4 | return 5 | end 6 | 7 | k = convhull(c(1,:), c(2,:), c(3,:)); 8 | X = reshape(c(1,k'), size(k')); 9 | Y = reshape(c(2,k'), size(k')); 10 | Z = reshape(c(3,k'), size(k')); 11 | plot3(X,Y,Z,color) 12 | fill3(X,Y,Z,color, 'FaceAlpha', 0.5, varargin{:}) -------------------------------------------------------------------------------- /src/matlab/+iris/+drawing/draw_2d.m: -------------------------------------------------------------------------------- 1 | function h = draw_2d(A,b,C,d,obstacles,lb,ub) 2 | import iris.thirdParty.polytopes.*; 3 | h = figure(2); 4 | cla 5 | hold on 6 | n_obs = numel(obstacles); 7 | if isempty(obstacles) 8 | n_obs = 0; 9 | end 10 | % Draw obstacle interiors 11 | for j = 1:n_obs 12 | obs = obstacles{j}; 13 | if size(obs, 2) > 1 14 | if size(obs, 2) > 2 15 | k = convhull(obs(1,:), obs(2,:)); 16 | else 17 | k = [1,2,1]; 18 | end 19 | patch(obs(1,k), obs(2,k), 'k', 'FaceColor', [.6,.6,.6], 'LineWidth', 0.1); 20 | else 21 | plot(obs(1,:), obs(2,:), 'ko'); 22 | end 23 | end 24 | 25 | % Draw obstacle boundaries on top 26 | for j = 1:n_obs 27 | obs = obstacles{j}; 28 | if size(obs, 2) > 1 29 | if size(obs, 2) > 2 30 | k = convhull(obs(1,:), obs(2,:)); 31 | else 32 | k = [1,2,1]; 33 | end 34 | plot(obs(1,k), obs(2,k), 'k', 'LineWidth', 2); 35 | end 36 | end 37 | for j = 1:size(A,1)-4 38 | % a'x = b 39 | % set x(1) = 0 40 | % x(2) = b / a(2) 41 | ai = A(j,:); 42 | bi = b(j); 43 | if ai(2) == 0 44 | x0 = [bi/ai(1); 0]; 45 | else 46 | x0 = [0; bi/ai(2)]; 47 | end 48 | u = [0,-1;1,0] * ai'; 49 | pts = [x0 - 1000*u, x0 + 1000*u]; 50 | plot(pts(1,:), pts(2,:), 'm--', 'LineWidth', 1.5) 51 | end 52 | if ~isempty(A) 53 | V = lcon2vert(A, b); 54 | k = convhull(V(:,1), V(:,2)); 55 | plot(V(k,1), V(k,2), 'ro-', 'LineWidth', 2); 56 | end 57 | th = linspace(0,2*pi,100); 58 | y = [cos(th);sin(th)]; 59 | x = bsxfun(@plus, C*y, d); 60 | plot(x(1,:), x(2,:), 'b-', 'LineWidth', 2); 61 | plot([lb(1),ub(1),ub(1),lb(1),lb(1)], [lb(2),lb(2),ub(2),ub(2),lb(2)], 'k-') 62 | 63 | pad = (ub - lb) * 0.05; 64 | xlim([lb(1)-pad(1),ub(1)+pad(1)]) 65 | ylim([lb(2)-pad(2),ub(2)+pad(2)]) 66 | axis off 67 | % drawnow() 68 | % pause() -------------------------------------------------------------------------------- /src/matlab/+iris/+drawing/draw_3d.m: -------------------------------------------------------------------------------- 1 | function h = draw_3d(A,b,C,d,obstacles,lb,ub) 2 | import iris.drawing.drawPolyFromVertices; 3 | import iris.thirdParty.polytopes.*; 4 | 5 | h = figure(2); 6 | cla 7 | hold on 8 | if ~isempty(obstacles) 9 | for j = 1:numel(obstacles) 10 | drawPolyFromVertices(obstacles{j},'k','FaceAlpha',0.5); 11 | end 12 | end 13 | 14 | if ~isempty(A) 15 | V = lcon2vert(A, b); 16 | drawPolyFromVertices(V', 'r'); 17 | end 18 | th = linspace(0,2*pi,20); 19 | y = [cos(th);sin(th);zeros(size(th))]; 20 | for phi = linspace(0,pi,10) 21 | T = makehgtform('xrotate', phi); 22 | R = T(1:3,1:3); 23 | y = [y, R * y]; 24 | end 25 | x = bsxfun(@plus, C*y, d); 26 | drawPolyFromVertices(x, 'b', 'FaceAlpha', 1) 27 | xlim([lb(1),ub(1)]) 28 | ylim([lb(2),ub(2)]) 29 | zlim([lb(3),ub(3)]) 30 | camtarget(0.5*(lb+ub)) 31 | campos([lb(1)-2,lb(2)-2,ub(3)]) 32 | % axis off; 33 | end -------------------------------------------------------------------------------- /src/matlab/+iris/+drawing/draw_Nd.m: -------------------------------------------------------------------------------- 1 | function [ h ] = draw_Nd( A,b,C,d,obstacles,lb,ub ) 2 | import iris.drawing.*; 3 | import iris.thirdParty.polytopes.*; 4 | 5 | h = figure(2); 6 | cla 7 | hold on 8 | for j = 1:size(obstacles, 3) 9 | obs = intersect_obs_with_plane(obstacles{j}, 3); 10 | drawPolyFromVertices(obs,'k','FaceAlpha',1); 11 | end 12 | if ~isempty(A) 13 | V = lcon2vert(A, b); 14 | drawPolyFromVertices(intersect_obs_with_plane(V', 3), 'r'); 15 | end 16 | xlim([lb(1),ub(1)]) 17 | ylim([lb(2),ub(2)]) 18 | zlim([lb(3),ub(3)]) 19 | camtarget(0.5*(lb(1:3)+ub(1:3))) 20 | campos([lb(1)-2,lb(2)-2,ub(3)]) 21 | 22 | end 23 | 24 | -------------------------------------------------------------------------------- /src/matlab/+iris/+drawing/intersect_obs_with_plane.m: -------------------------------------------------------------------------------- 1 | function [ obs ] = intersect_obs_with_plane(obs, dim) 2 | % Find the [dim]-dimensional intersection of the N-d convex body defined by 3 | % vertices obs with the x[dim+1] = x[dim+2] = .... = xN = 0 surface. 4 | 5 | 6 | while size(obs,1) > dim 7 | % Reduce one dimension at a time. Probably very inefficient. 8 | pts = []; 9 | N = size(obs, 1); 10 | for j = 1:size(obs, 2) 11 | for k = j+1:size(obs, 2) 12 | if sign(obs(end,j)) ~= sign(obs(end,k)) 13 | d1 = abs(obs(:,j)' * [zeros(N-1,1); 1]); 14 | d2 = abs(obs(:,k)' * [zeros(N-1,1); 1]); 15 | intersect = (d2 * obs(:,j) + d1 * obs(:,k)) / (d1 + d2); 16 | assert(intersect(end) == 0); 17 | pts(:,end+1) = intersect(1:end-1); 18 | elseif all(obs(end,j) == 0) 19 | pts(:,end+1) = obs(:,j); 20 | elseif all(obs(end,k) == 0) 21 | pts(:,end+1) = obs(:,k); 22 | end 23 | end 24 | end 25 | try 26 | k = convhull(pts(1,:), pts(2,:), 'simplify', true); 27 | catch 28 | k = 1:size(pts,2); 29 | end 30 | obs = pts(:,k); 31 | end 32 | 33 | -------------------------------------------------------------------------------- /src/matlab/+iris/+inner_ellipsoid/cvx_ellipsoid.m: -------------------------------------------------------------------------------- 1 | function [C, d] = cvx_ellipsoid(A, b) 2 | 3 | dim = size(A,2); 4 | 5 | cvx_begin sdp quiet 6 | cvx_solver SDPT3 7 | variable C(dim,dim) semidefinite 8 | variable d(dim) 9 | maximize(det_rootn(C)) 10 | subject to 11 | for i = 1:length(b) 12 | [(b(i) - A(i,:) * d) * eye(dim), C * (A(i,:)'); 13 | (C * (A(i,:)'))', (b(i) - A(i,:) * d)] >= 0; 14 | end 15 | cvx_end 16 | end 17 | 18 | -------------------------------------------------------------------------------- /src/matlab/+iris/+inner_ellipsoid/spot_ellipsoid.m: -------------------------------------------------------------------------------- 1 | function [C, d] = spot_ellipsoid(A, b) 2 | 3 | n = size(A, 2); 4 | 5 | % tic 6 | pr = spotsosprog; 7 | [pr, t] = pr.newFree(1); 8 | [pr, Z] = pr.newFree(n, n); 9 | [pr, C] = pr.newSym(n); 10 | [pr, d] = pr.newFree(n); 11 | z = diag(Z); 12 | pr = pr.withPSD([C, Z'; Z, diag(z)]); 13 | lor = [b - A * d, A * C']'; 14 | pr = pr.withLor(lor); 15 | 16 | l = ceil(log2(n)); 17 | m = 2^l - n; 18 | if m > 0 19 | x = [z; repmat(t, m, 1)]; 20 | else 21 | x = z; 22 | end 23 | [pr, g] = geo_mean_recursive(pr, x); 24 | pr = pr.withEqs(2^(l/2) * t - g); 25 | 26 | % fprintf(1, 'setup: %f\n', toc); 27 | 28 | % tic 29 | % disp('mosek') 30 | % solver = @spot_mosek; 31 | % sol = pr.minimize(-t, solver); 32 | % sol.eval(lor) 33 | % double(sol.eval(C)) 34 | % double(sol.eval(d)) 35 | % fprintf(1, 'mosek: %f\n', toc); 36 | 37 | % tic 38 | solver = @spot_sedumi; 39 | sol = pr.minimize(-t, solver); 40 | % sol.eval(lor) 41 | % fprintf(1, 'sedumi: %f\n', toc); 42 | 43 | % tic 44 | C = double(sol.eval(C)); 45 | d = double(sol.eval(d)); 46 | % fprintf(1, 'extract: %f\n', toc); 47 | 48 | end 49 | 50 | function [pr, g] = geo_mean_recursive(pr, x) 51 | n = length(x); 52 | if n > 1 53 | [pr, y] = pr.newFree(n/2); 54 | pr = pr.withRLor([reshape(x, 2, n/2); reshape(y, 1, n/2)]); 55 | [pr, g] = geo_mean_recursive(pr, y); 56 | else 57 | g = x; 58 | end 59 | end -------------------------------------------------------------------------------- /src/matlab/+iris/+least_distance/cvxgen_ldp.m: -------------------------------------------------------------------------------- 1 | function v = cvxgen_ldp(Y) 2 | [n, m] = size(Y); 3 | if n < 3 4 | Y = [Y; zeros(3-n, m)]; 5 | end 6 | if m < 8 7 | Y = [Y, repmat(Y(:,1), 1, (8-m))]; 8 | end 9 | [vars, ~] = cvxgen_ldp_mex(struct('Y', Y), struct('verbose', 0)); 10 | v = vars.v(1:n); 11 | end 12 | 13 | -------------------------------------------------------------------------------- /src/matlab/+iris/+least_distance/mosek_ldp.m: -------------------------------------------------------------------------------- 1 | function ystar = mosek_ldp(ys, res) 2 | % Use Mosek to find the closest point in the convex hull of the ys to the 3 | % origin. 4 | 5 | if nargin < 2 6 | [~, res] = mosekopt('symbcon echo(0)'); 7 | end 8 | 9 | dim = size(ys, 1); 10 | 11 | nw = size(ys,2); 12 | nvar= 1 + dim+nw; 13 | prob.c = [zeros(1, dim+nw) , 1]; 14 | prob.a = sparse([ [-eye(dim), ys, zeros(dim,1)];[ zeros(1,dim), ones(1,nw),0] ]); 15 | prob.blc = [zeros(dim,1);1]; 16 | prob.buc = [zeros(dim,1);1]; 17 | prob.blx = [-inf*ones(dim,1);zeros(nw+1,1)]; 18 | prob.bux = inf*ones(nvar,1); 19 | 20 | % Specify the cones. 21 | prob.cones.type = res.symbcon.MSK_CT_QUAD; 22 | prob.cones.sub = [nvar, 1:dim]; 23 | prob.cones.subptr = 1; 24 | 25 | % Optimize the problem. 26 | [~,solution]=mosekopt('minimize echo(0)',prob); 27 | %toc 28 | ystar = solution.sol.itr.xx(1:dim); 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /src/matlab/+iris/+terrain_grid/CollisionModel.m: -------------------------------------------------------------------------------- 1 | classdef CollisionModel 2 | properties 3 | foot_shape 4 | body_slices 5 | end 6 | 7 | methods 8 | function obj = CollisionModel(foot_shape, body_slices) 9 | obj.foot_shape = foot_shape; 10 | obj.body_slices = body_slices; 11 | end 12 | 13 | end 14 | end 15 | -------------------------------------------------------------------------------- /src/matlab/+iris/+terrain_grid/Heightmap.m: -------------------------------------------------------------------------------- 1 | classdef Heightmap 2 | % A simple container for height map data stored as a grid of heights with associated normal vectors. 3 | properties(SetAccess=private, GetAccess=public) 4 | X; 5 | Y; 6 | Z; 7 | normals; 8 | resolution; 9 | end 10 | 11 | methods 12 | function obj = Heightmap(X, Y, Z, normals) 13 | if length(X) == numel(X) && length(Y) == numel(Y) 14 | [X, Y] = meshgrid(X, Y); 15 | end 16 | obj.X = X; 17 | obj.Y = Y; 18 | obj.Z = Z; 19 | obj.normals = normals; 20 | if ~isempty(X) 21 | obj.resolution = norm(X(1,2) - X(1,1)); 22 | else 23 | obj.resolution = nan; 24 | end 25 | end 26 | 27 | function slope_angles = getSlopeAngles(obj) 28 | slope_angles = atan2(sqrt(sum(obj.normals(1:2,:).^2,1)), obj.normals(3,:)); 29 | slope_angles = reshape(slope_angles, size(obj.X)); 30 | end 31 | end 32 | end 33 | 34 | 35 | -------------------------------------------------------------------------------- /src/matlab/+iris/+terrain_grid/component_boundary.m: -------------------------------------------------------------------------------- 1 | function boundary = component_boundary(grid, ndx) 2 | 3 | if length(ndx) == 2 4 | ndx = sub2ind(size(grid), ndx(1), ndx(2)); 5 | end 6 | 7 | filled = imfill(~grid, ndx, 8); 8 | component = ~(xor(filled,grid)); 9 | boundary = iris.terrain_grid.find_boundary(component); 10 | 11 | % figure(21) 12 | % subplot(411) 13 | % imshow(grid) 14 | % subplot(412) 15 | % imshow(component) 16 | % subplot(413) 17 | % imshow(fast_boundary); 18 | % subplot(414) 19 | % imshow(boundary); 20 | % assert(all(all(fast_boundary == boundary))); 21 | -------------------------------------------------------------------------------- /src/matlab/+iris/+terrain_grid/find_boundary.m: -------------------------------------------------------------------------------- 1 | function boundary = find_boundary(component) 2 | % Find all the cells which are false but which are 8-way adjacent to a true cell. 3 | 4 | boundary = false(size(component)); 5 | [m, n] = size(component); 6 | for dx = -1:1 7 | ind_x = max(1-dx, 1):min(m,m-dx); 8 | for dy = -1:1 9 | if dx == 0 && dy == 0 10 | continue 11 | end 12 | ind_y = max(1-dy, 1):min(n, n-dy); 13 | boundary(ind_x,ind_y) = boundary(ind_x,ind_y) | component(ind_x,ind_y) < component(ind_x+dx,ind_y+dy); 14 | end 15 | end -------------------------------------------------------------------------------- /src/matlab/+iris/+terrain_grid/inflate_grid_region.m: -------------------------------------------------------------------------------- 1 | function [mask, A, b, obs] = inflate_grid_region(~, seed_grid) 2 | import iris.terrain_grid.*; 3 | import iris.inflate_region; 4 | 5 | dim = 2; 6 | 7 | grid = logical(seed_grid); 8 | [all_squares(1,:), all_squares(2,:)] = ind2sub(size(grid), 1:length(reshape(grid,[],1))); 9 | 10 | dists = obs_dist(seed_grid); 11 | [~, i0] = max(reshape(dists, [], 1)); 12 | [r, c] = ind2sub(size(grid), i0); 13 | x0 = [r; c]; 14 | 15 | [white_squares(1,:), white_squares(2,:)] = ind2sub(size(grid), find(grid)); 16 | [black_squares(1,:), black_squares(2,:)] = ind2sub(size(grid), find(~grid)); 17 | black_edges = []; 18 | [black_edges(1,:), black_edges(2,:)] = ind2sub(size(grid), find(component_boundary(grid, [r;c]))); 19 | 20 | obs_offsets = [0.5, 0.5, -0.5, -0.5; 21 | -0.5, 0.5, 0.5, -0.5]; 22 | obs_centers = reshape(black_edges, [], 1); 23 | n_obs = size(black_edges, 2); 24 | obs_pts = bsxfun(@plus, obs_centers, repmat(obs_offsets, n_obs, 1)); 25 | obstacles = mat2cell(obs_pts, dim*ones(n_obs,1), size(obs_offsets,2))'; 26 | obstacle_pts = reshape(cell2mat(obstacles), 2, 4, []); 27 | % obstacles = mat2cell(black_edges, 2, ones(1,size(black_edges,2))); 28 | 29 | lb = [0;0]; 30 | ub = [size(grid,1); size(grid,2)]; 31 | A_bounds = [-1,0;0,-1;1,0;0,1]; 32 | b_bounds = [-lb; ub]; 33 | 34 | [A,b,C,d] = inflate_region(obstacle_pts, A_bounds, b_bounds, x0); 35 | 36 | 37 | % for i = 1:size(A,2) 38 | % n = norm(A(i,:)); 39 | % A(i,:) = A(i,:) / n; 40 | % b(i) = b(i) / n; 41 | % end 42 | mask = false(size(grid)); 43 | inpoly = all_squares(:,all(bsxfun(@minus, A * all_squares, b) <= 0)); 44 | mask(sub2ind(size(grid), inpoly(1,:), inpoly(2,:))) = true; 45 | try 46 | k = convhull(inpoly(1,:), inpoly(2,:), 'simplify', true); 47 | catch 48 | k = 1:size(inpoly, 2); 49 | end 50 | obs = inpoly(:,k); 51 | 52 | % 53 | % mask = zeros(size(grid)); 54 | % mask(all(bsxfun(@minus, A * all_squares, b) < 0.5, 1)) = 1; 55 | % mask(~grid) = 0; 56 | -------------------------------------------------------------------------------- /src/matlab/+iris/+terrain_grid/obs_dist.m: -------------------------------------------------------------------------------- 1 | function dists = obs_dist(grid) 2 | 3 | dists = inf(size(grid)); 4 | while true 5 | old_dists = dists; 6 | dists(:,2:end) = min(dists(:,2:end), dists(:,1:end-1)+1); 7 | dists(:,1:end-1) = min(dists(:,1:end-1), dists(:,2:end)+1); 8 | dists(2:end,:) = min(dists(2:end,:), dists(1:end-1,:)+1); 9 | dists(1:end-1,:) = min(dists(1:end-1,:), dists(2:end,:)+1); 10 | dists(1:end,[1,end]) = 1; 11 | dists([1,end],1:end) = 1; 12 | dists(~grid) = 0; 13 | if all(all(dists == old_dists)) 14 | break 15 | end 16 | end 17 | 18 | -------------------------------------------------------------------------------- /src/matlab/+iris/+terrain_grid/segment_grid.m: -------------------------------------------------------------------------------- 1 | function [obstacles] = segment_grid(grid) 2 | import iris.terrain_grid.* 3 | obstacles = {}; 4 | while any(any(grid)) 5 | [mask, A, b, obs] = inflate_grid_region([],grid); 6 | grid(mask) = 0; 7 | % sum(sum(grid)) 8 | obstacles{end+1} = obs; 9 | end 10 | 11 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/compare_implementations.m: -------------------------------------------------------------------------------- 1 | function compare_implementations() 2 | 3 | n_trials = 50; 4 | n_obstacles = 5; 5 | matlab_times = nan(1, n_trials); 6 | cpp_times = nan(1, n_trials); 7 | 8 | iter = 1; 9 | while iter <= n_trials 10 | dim = 1 + ceil(iter / 20); 11 | A_bounds = [eye(dim); -eye(dim)]; 12 | b_bounds = [ones(dim, 1); zeros(dim, 1)]; 13 | 14 | obstacle_pts = iris.test.random_obstacles(dim, n_obstacles, zeros(dim,1), ones(dim,1), 0.05); 15 | obstacles = mat2cell(obstacle_pts, size(obstacle_pts, 1), size(obstacle_pts, 2), ones(1, size(obstacle_pts, 3))); 16 | start = 0.5 * ones(dim,1); 17 | options = struct('require_containment', false,... 18 | 'error_on_infeasible_start', false,... 19 | 'termination_threshold', 2e-2,... 20 | 'iter_limit', 100); 21 | 22 | try 23 | t0 = tic(); 24 | [A_c, b_c, C_c, d_c] = iris.inflate_regionmex(obstacles, A_bounds, b_bounds, start, options); 25 | cpp_times(iter) = toc(t0); 26 | 27 | t0 = tic(); 28 | [A_m, b_m, C_m, d_m] = iris.inflate_region_fallback(obstacle_pts, A_bounds, b_bounds, start, options); 29 | matlab_times(iter) = toc(t0); 30 | 31 | iter = iter + 1 32 | catch 33 | continue 34 | end 35 | 36 | % assert(iris.util.equal_up_to_permutations(A_c, A_m, 1e-2)); 37 | % assert(iris.util.equal_up_to_permutations(b_c, b_m, 1e-2)); 38 | assert(all(all(abs(C_c - C_m) < 1e-2))); 39 | assert(all(all(abs(d_c - d_m) < 1e-2))); 40 | 41 | end 42 | 43 | fprintf(1, 'c++ mean: %f std: %f\n', mean(cpp_times), std(cpp_times)); 44 | fprintf(1, 'matlab mean: %f std: %f\n', mean(matlab_times), std(matlab_times)); 45 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/plot_timing_results.m: -------------------------------------------------------------------------------- 1 | function plot_timing_results(all_results, fname) 2 | 3 | if nargin < 2 4 | fname = 'timing.dat' 5 | end 6 | 7 | n_trials = 10; 8 | n_obs = reshape([all_results.n_obs], n_trials, []); 9 | iters = reshape([all_results.iters], n_trials, []); 10 | total_time = reshape([all_results.total_time], n_trials, []); 11 | e_time = reshape([all_results.e_time], n_trials, []); 12 | p_time = reshape([all_results.p_time], n_trials, []); 13 | linear_ref = mean(n_obs) * (mean(total_time(:,end)) / mean(n_obs(:,end))); 14 | T = table(mean(n_obs)', std(n_obs)',... 15 | mean(iters)', std(iters)', ... 16 | mean(total_time)', std(total_time)',... 17 | mean(e_time)', std(e_time)',... 18 | mean(p_time)', std(p_time)',... 19 | linear_ref',... 20 | 'VariableNames', {'n_obs', 'n_obs_std',... 21 | 'iters', 'iters_std',... 22 | 'total_time', 'total_time_std',... 23 | 'e_time', 'e_time_std',... 24 | 'p_time', 'p_time_std',... 25 | 'linear_ref'}); 26 | writetable(T, fname, 'Delimiter', '\t'); 27 | figure(6) 28 | set(gcf, 'Position', [100, 100, 500, 700]) 29 | set(gcf, 'defaulttextinterpreter','latex'); 30 | clf 31 | subplot(3,1,[1,2]) 32 | cla 33 | hold on 34 | opts = {'LineWidth', 2}; 35 | errorbar(mean(n_obs), mean(total_time), std(total_time), 'k.-', opts{:}) 36 | errorbar(mean(n_obs), mean(e_time), std(e_time), '.-', 'Color', [.8,.1,.1], opts{:}) 37 | errorbar(mean(n_obs), mean(p_time), std(p_time), '.-', 'Color', [.1,.8,.1], opts{:}) 38 | set(gca, 'XScale', 'log') 39 | set(gca, 'YScale', 'log') 40 | % loglog([1, mean(n_obs(:,end))], [mean(total_time(:,end))/mean(n_obs(:,end)), mean(total_time(:,end))], 'k--') 41 | loglog(mean(n_obs), linear_ref, 'k--') 42 | legend({'Total Time', 'Time Computing Ellipse', 'Time Computing Planes', '$\frac{1}{2}$'}, 'Location', 'NorthWest', 'FontSize', 12, 'Interpreter', 'LaTeX'); 43 | xlim([min([all_results.n_obs])-1, max([all_results.n_obs])*1.5]); 44 | xlabel('Number of obstacles','FontSize', 12) 45 | ylabel('CPU Time (s)','FontSize', 12) 46 | set(gca, 'FontSize', 12); 47 | set(gca, 'defaulttextinterpreter','latex'); 48 | subplot(3,1,3) 49 | errorbar(mean(n_obs), mean(iters), std(iters), 'ko', opts{:}) 50 | set(gca, 'XScale', 'log'); 51 | ylim([0, max(mean(iters) + std(iters)) + 1]) 52 | xlim([min([all_results.n_obs])-1, max([all_results.n_obs])*1.5]); 53 | xlabel('Number of obstacles','FontSize', 12) 54 | ylabel('Number of major iterations','FontSize', 12) 55 | % h = bar([all_results.n_obs], [all_results.iters]); 56 | % set(h, 'BarWidth', 0.8); 57 | set(gca, 'FontSize', 12); 58 | set(gca, 'defaulttextinterpreter','latex'); 59 | set(gcf, 'PaperPositionMode', 'auto') 60 | print(gcf, 'fig/last_timing_results', '-dpdf') 61 | 62 | n_in_poly = 0; 63 | n_outside_poly = 0; 64 | for j = 1:size(all_results,1) 65 | for k = 1:size(all_results,2) 66 | hist = all_results(j,k).p_history{end}; 67 | if all(hist.A * all_results(j,k).start <= hist.b) 68 | n_in_poly = n_in_poly + 1; 69 | else 70 | n_outside_poly = n_outside_poly + 1; 71 | end 72 | end 73 | end 74 | n_in_poly 75 | n_outside_poly 76 | n_in_poly / (n_in_poly + n_outside_poly) 77 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/random_cubic_obstacles.m: -------------------------------------------------------------------------------- 1 | function obstacle_pts = random_cubic_obstacles(dim, n_obs, lb, ub, scale) 2 | % Generate a randomly distributed field of cubic obstacles 3 | if nargin < 5 4 | scale = 1; 5 | end 6 | if dim == 2 7 | offsets = scale * [1, 1, -1, -1; 8 | -1, 1, 1, -1]; 9 | elseif dim == 3 10 | offsets = scale * [1, 1, -1, -1, 1, 1, -1, -1; 11 | -1, 1, 1, -1, -1, 1, 1, -1; 12 | -1,-1,-1,-1, 1, 1, 1, 1]; 13 | end 14 | centers = bsxfun(@plus, bsxfun(@times, rand(dim, n_obs), ub - lb), lb); 15 | centers = reshape(centers, dim, 1, []); 16 | obstacle_pts = bsxfun(@plus, centers, repmat(offsets ./ (n_obs^(1/dim)), [1,1, n_obs])); 17 | 18 | end 19 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/random_obstacles.m: -------------------------------------------------------------------------------- 1 | function obstacle_pts = random_obstacles(dim, n_obs, lb, ub, scale) 2 | % Generate a randomly distributed field of random obstacles. Each 3 | % obstacle consists of 2^dim points uniformly randomly distributed 4 | % within a 2*scale-length cube around a randomly selected center point 5 | if nargin < 5 6 | scale = 1; 7 | end 8 | offsets = scale * (rand(dim, 2^dim * n_obs) * 2 - 1); 9 | centers = bsxfun(@plus, bsxfun(@times, rand(dim, n_obs), ub - lb), lb); 10 | centers = reshape(centers, dim, 1, []); 11 | obstacle_pts = bsxfun(@plus, centers, reshape(offsets ./ (n_obs^(1/dim)), [dim, 2^dim, n_obs])); 12 | 13 | end 14 | 15 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/run_system_tests.m: -------------------------------------------------------------------------------- 1 | function run_system_tests() 2 | iris.test.test_c_space_3d(); 3 | iris.test.test_grid_2d(); 4 | iris.test.test_grid_segmentation(); 5 | iris.test.test_points_2d; 6 | iris.test.test_poly_2d(); 7 | iris.test.test_poly_3d(); 8 | iris.test.test_poly_Nd(4); 9 | iris.test.test_polygon_recovery(); 10 | iris.test.test_simple_poly_2d(); 11 | iris.test.test_boundary(); 12 | if ~exist('gurobi') 13 | if exist('addpath_gurobi.m') 14 | addpath_gurobi() 15 | end 16 | end 17 | if exist('gurobi') 18 | disp('gurobi found, running additional tests') 19 | iris.test.test_uav_demo(); 20 | iris.test.test_uav_demo('4d'); 21 | else 22 | disp('gurobi not found, skipping additional tests') 23 | end 24 | iris.test.test_thin_ellipsoid_simple(); 25 | disp('Tests complete'); 26 | end 27 | 28 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_boundary.m: -------------------------------------------------------------------------------- 1 | function test_boundary() 2 | 3 | grid = logical(randi(2,100,100)-1); 4 | 5 | for j = 1:20 6 | ndx = randi(100^2); 7 | 8 | filled = imfill(~grid, ndx, 8); 9 | component = ~(xor(filled,grid)); 10 | t0 = tic(); 11 | boundary = imdilate(component, strel('square', 3)) - component; 12 | t_dil = toc(t0); 13 | fprintf('imdilate: %f, ', t_dil); 14 | t0 = tic(); 15 | fast_boundary = iris.terrain_grid.find_boundary(component); 16 | t_bound = toc(t0); 17 | fprintf('fast: %f, ratio: %f\n', t_bound, t_dil/t_bound); 18 | 19 | figure(23) 20 | subplot 211 21 | imshow(boundary, 'InitialMagnification', 'fit'); 22 | subplot 212 23 | imshow(fast_boundary, 'InitialMagnification', 'fit'); 24 | assert(all(all(boundary == fast_boundary))); 25 | end 26 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_c_space_3d.m: -------------------------------------------------------------------------------- 1 | function results = test_c_space_3d(record) 2 | import iris.cspace.cspace3; 3 | import iris.cspace.project_c_space_region; 4 | import iris.inflate_region; 5 | import iris.drawing.*; 6 | 7 | if nargin < 1 8 | record = false; 9 | end 10 | 11 | dim = 3; 12 | % obstacles = {}; 13 | n_obs = 10; 14 | lb = [0;0;-pi]; 15 | ub = [4;4;pi]; 16 | bot = [-0.005,-0.005,0.005,0.005;-0.3,0.3,0.3,-0.3]; 17 | A_bounds = [-1,0,0; 18 | 0,-1,0; 19 | 0,0,-1; 20 | 1,0,0; 21 | 0,1,0; 22 | 0,0,1]; 23 | base_obstacles = iris.test.random_obstacles(2, n_obs, lb(1:2), ub(1:2)); 24 | base_obstacles = mat2cell(reshape(base_obstacles(:,1,:), 2, []), 2, ones(1, n_obs)); 25 | obstacle_pts = cspace3(base_obstacles, bot, 10); 26 | b_bounds = [-lb;ub]; 27 | start = 0.5 * (lb + ub); 28 | 29 | % profile on 30 | [A,b,C,d,results] = inflate_region(obstacle_pts, A_bounds, b_bounds, start); 31 | % profile viewer 32 | % animate_results(results, record); 33 | figure(4) 34 | clf 35 | hold on 36 | for j = 1:length(base_obstacles) 37 | obs = base_obstacles{j}; 38 | if size(obs, 2) > 2 39 | patch(obs(1,:), obs(2,:), 'k'); 40 | else 41 | plot(obs(1,:), obs(2,:), 'ko-', 'MarkerFaceColor', 'k'); 42 | end 43 | end 44 | x = iris.sample_convex_polyhedron(A,b,50); 45 | for k = 1:size(x,2) 46 | R = iris.util.rotmat(x(3,k)); 47 | bot_x = bsxfun(@plus, R * bot, x(1:2,k)); 48 | patch(bot_x(1,:), bot_x(2,:), 'k'); 49 | end 50 | axis equal 51 | lb = lb - 0.3; 52 | ub = ub + 0.3; 53 | plot([lb(1),ub(1),ub(1),lb(1),lb(1)], [lb(2),lb(2),ub(2),ub(2),lb(2)], 'k--') 54 | [inner_poly, outer_poly] = project_c_space_region(A,b); 55 | patch(outer_poly(1,:), outer_poly(2,:), 'y', 'FaceAlpha', 0.5); 56 | if ~isempty(inner_poly) 57 | patch(inner_poly(1,:), inner_poly(2,:), 'g', 'FaceAlpha', 0.5); 58 | end 59 | axis off 60 | end -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_grid_2d.m: -------------------------------------------------------------------------------- 1 | import iris.inflate_region; 2 | import iris.terrain_grid.*; 3 | import iris.thirdParty.polytopes.*; 4 | 5 | load('data/example_feas_map.mat'); 6 | grid = Q(85:125,25:85); 7 | dists = obs_dist(grid); 8 | [~, i0] = max(reshape(dists, [], 1)); 9 | 10 | [r, c] = ind2sub(size(grid), i0); 11 | start = [r;c]; 12 | 13 | 14 | [white_squares(1,:), white_squares(2,:)] = ind2sub(size(grid), find(grid)); 15 | [black_squares(1,:), black_squares(2,:)] = ind2sub(size(grid), find(~grid)); 16 | black_edges = []; 17 | [black_edges(1,:), black_edges(2,:)] = ind2sub(size(grid), find(component_boundary(grid, [r;c]))); 18 | 19 | % offsets = [-0.5,-0.5,0.5,0.5;-0.5,0.5,0.5,-0.5]; 20 | offsets = [0;0]; 21 | obstacles = {}; 22 | for j = 1:size(black_edges,2) 23 | obstacles{j} = bsxfun(@plus, black_edges(:,j), offsets); 24 | end 25 | obstacle_pts = reshape(cell2mat(obstacles), 2, 1, []); 26 | 27 | lb = [0;0]; 28 | ub = [size(grid,1); size(grid,2)]; 29 | A_bounds = [-1,0;0,-1;1,0;0,1]; 30 | b_bounds = [-lb; ub]; 31 | 32 | % profile on 33 | [A,b,C,d] = inflate_region(obstacle_pts, A_bounds, b_bounds, start); 34 | % profile viewer 35 | 36 | figure(2) 37 | clf 38 | hold on 39 | for j = 1:length(obstacles) 40 | patch(obstacles{j}(1,:), obstacles{j}(2,:), 'k'); 41 | end 42 | V = lcon2vert(A, b); 43 | k = convhull(V(:,1), V(:,2)); 44 | plot(V(k,1), V(k,2), 'ro-'); 45 | th = linspace(0,2*pi,100); 46 | y = [cos(th);sin(th)]; 47 | x = bsxfun(@plus, C*y, d); 48 | plot(x(1,:), x(2,:), 'b-'); 49 | xlim([lb(1),ub(1)]) 50 | ylim([lb(1),ub(2)]) -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_grid_segmentation.m: -------------------------------------------------------------------------------- 1 | function test_grid_segmentation() 2 | load('data/example_feas_map') 3 | grid = ~Q(85:125,25:85); 4 | % grid = Q; 5 | 6 | figure(16) 7 | clf; 8 | imshow(grid, 'InitialMagnification', 'fit'); 9 | hold on; 10 | 11 | % profile on 12 | t0 = tic; 13 | obstacles = iris.terrain_grid.segment_grid(grid); 14 | toc(t0); 15 | % profile viewer 16 | 17 | figure(16) 18 | for j = 1:length(obstacles) 19 | obs = obstacles{j}; 20 | plot(obs(2,:), obs(1,:), 'r-'); 21 | end 22 | 23 | end 24 | 25 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_outer_ellipsoid.m: -------------------------------------------------------------------------------- 1 | function test_inner_ellipsoid() 2 | % Test the mosek inner ellipsoid function 3 | 4 | %% Try in 2D 5 | m = randi(10)+3; % number of points 6 | 7 | x = rand(m, 2); % random points of chosen size 8 | 9 | [P, c] = iris.thirdParty.mosek_lownerjohn.lownerjohn_ellipsoid.lownerjohn_outer(x); 10 | 11 | % P*x - c <= 1 12 | % y = P*x - c 13 | % Pi*y = x - Pi*c 14 | % x = Pi*y + Pi*c 15 | 16 | C = inv(P); 17 | d = inv(P) * c; 18 | 19 | th = linspace(0,2*pi,100); 20 | y = [cos(th);sin(th)]; 21 | z = bsxfun(@plus, C*y, d); 22 | figure(1); 23 | clf 24 | hold on 25 | plot(z(1,:), z(2,:), 'b-', 'LineWidth', 2); 26 | plot(x(:,1), x(:,2), 'ro') 27 | 28 | for j = 1:size(x, 1) 29 | assert(norm(P*(x(j,:)') - c) <= 1 + 1e-3); 30 | end 31 | 32 | 33 | %% Try again in 3d 34 | m = randi(10)+4; % number of points 35 | 36 | x = rand(m, 3); % random points of chosen size 37 | 38 | [P, c] = iris.thirdParty.mosek_lownerjohn.lownerjohn_ellipsoid.lownerjohn_outer(x); 39 | 40 | % P*x - c <= 1 41 | % y = P*x - c 42 | % Pi*y = x - Pi*c 43 | % x = Pi*y + Pi*c 44 | 45 | C = inv(P); 46 | d = inv(P) * c; 47 | 48 | figure(2); 49 | clf 50 | hold on 51 | th = linspace(0,2*pi,20); 52 | y = [cos(th);sin(th);zeros(size(th))]; 53 | for phi = linspace(0,pi,10) 54 | T = makehgtform('xrotate', phi); 55 | R = T(1:3,1:3); 56 | y = [y, R * y]; 57 | end 58 | z = bsxfun(@plus, C*y, d); 59 | iris.drawing.drawPolyFromVertices(z, 'b', 'FaceAlpha', 0.5) 60 | plot3(x(:,1), x(:,2), x(:,3), 'ro') 61 | 62 | for j = 1:size(x, 1) 63 | assert(norm(P*(x(j,:)') - c) <= 1 + 1e-3); 64 | end 65 | 66 | 67 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_points_2d.m: -------------------------------------------------------------------------------- 1 | function test_points_2d() 2 | import iris.drawing.* 3 | import iris.inflate_region; 4 | 5 | m = 10; 6 | n = 10; 7 | grid = rand(m, n) < 0.5; 8 | 9 | idx = find(~grid); 10 | [r,c] = ind2sub(size(grid), idx); 11 | obstacles = reshape([r'; c'], [2, 1, length(r)]); 12 | 13 | lb = [1;1]; 14 | ub = [m;n]; 15 | A = [-1,0;0,-1;1,0;0,1]; 16 | b = [-lb;ub]; 17 | start = [m/2 + 0.25; n/2 + 0.25]; 18 | 19 | [A,b,C,d, results] = inflate_region(obstacles, A, b, start); 20 | animate_results(results); 21 | 22 | end -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_poly_2d.m: -------------------------------------------------------------------------------- 1 | function results = test_poly_2d(record) 2 | import iris.inflate_region; 3 | import iris.drawing.*; 4 | 5 | if nargin < 1 6 | record = false; 7 | end 8 | 9 | lb = [0;0]; 10 | ub = [10;10]; 11 | dim = 2; 12 | 13 | n_obs = 20; 14 | obstacle_pts = iris.test.random_obstacles(dim, n_obs, lb, ub, 5); 15 | 16 | A_bounds = [-1,0;0,-1;1,0;0,1]; 17 | b_bounds = [-lb; ub]; 18 | 19 | start = 0.5 * (ub + lb); 20 | 21 | % profile on 22 | [A,b,C,d,results] = inflate_region(obstacle_pts, A_bounds, b_bounds, start); 23 | % profile viewer 24 | if n_obs < 1e4 25 | animate_results(results, record); 26 | end 27 | 28 | end 29 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_poly_3d.m: -------------------------------------------------------------------------------- 1 | function results = poly_segmentation_3d(record) 2 | if nargin < 1 3 | record = false; 4 | end 5 | import iris.drawing.*; 6 | 7 | lb = [0;0;0]; 8 | ub = [10;10;10]; 9 | dim = 3; 10 | 11 | n_obs = 20; 12 | % obstacles = iris.test.random_obstacles(dim, n_obs, lb, ub,3); 13 | obstacles = iris.test.random_cubic_obstacles(dim, n_obs, lb, ub); 14 | 15 | A_bounds = [-1,0,0; 16 | 0,-1,0; 17 | 0,0,-1; 18 | 1,0,0; 19 | 0,1,0; 20 | 0,0,1]; 21 | b_bounds = [-lb;ub]; 22 | 23 | start = 0.5 * (ub + lb); 24 | 25 | 26 | % profile on 27 | [A,b,C,d,results] = iris.inflate_region(obstacles, A_bounds, b_bounds, start); 28 | % profile viewer 29 | if n_obs < 50 30 | animate_results(results, record); 31 | end 32 | 33 | end 34 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_poly_Nd.m: -------------------------------------------------------------------------------- 1 | function [ results ] = test_poly_Nd(N, record) 2 | % Demonstrate that we can apply the algorithm in arbitrary dimension 3 | 4 | import iris.inflate_region; 5 | import iris.drawing.*; 6 | 7 | if nargin < 2 8 | record = false; 9 | end 10 | 11 | if nargin < 1 12 | N = 4; 13 | end 14 | 15 | lb = -ones(N,1); 16 | ub = ones(N,1); 17 | dim = N; 18 | n_obs = 50; 19 | 20 | obstacles = zeros(dim, 2^dim, n_obs); 21 | for j = 1:n_obs 22 | center = rand(dim, 1) .* (ub(1) - lb(1)) + lb(1); 23 | offsets = rand(dim, 2^dim) .* (0.3 - (-0.3)) + (-0.3); 24 | obstacles(:,:,j) = bsxfun(@plus, center, offsets); 25 | end 26 | 27 | A_bounds = []; 28 | for j = 1:dim 29 | row = zeros(1,dim); 30 | row(j) = -1; 31 | A_bounds(j,:) = row; 32 | b_bounds(j) = -lb(j); 33 | end 34 | for j = 1:dim 35 | row = zeros(1,dim); 36 | row(j) = 1; 37 | A_bounds(end+1,:) = row; 38 | b_bounds(end+1) = ub(j); 39 | end 40 | b_bounds = reshape(b_bounds,[],1); 41 | 42 | start = 0.5 * (ub + lb); 43 | 44 | % profile on 45 | [A,b,C,d,results] = inflate_region(obstacles, A_bounds, b_bounds, start); 46 | % profile viewer 47 | 48 | animate_results(results, record); 49 | 50 | 51 | end 52 | 53 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_poly_selection_2d.m: -------------------------------------------------------------------------------- 1 | function test_poly_selection_2d() 2 | % NOTEST 3 | 4 | import iris.terrain_grid.*; 5 | import iris.inflate_region; 6 | import iris.thirdParty.polytopes.*; 7 | lcmgl = drake.util.BotLCMGLClient(lcm.lcm.LCM.getSingleton(), 'segmentation'); 8 | % load('example_feas_map.mat','Q'); 9 | load('data/example_heights.mat','heights','px2world'); 10 | px2world(1,end) = px2world(1,end) - sum(px2world(1,1:3)); % stupid matlab 1-indexing... 11 | px2world(2,end) = px2world(2,end) - sum(px2world(2,1:3)); 12 | resample = 2; 13 | mag = 2^(resample-1); 14 | heights = interp2(heights, (resample-1)); 15 | px2world = px2world * [1/mag 0 0 (1-1/mag); 0 1/mag 0 (1-1/mag ); 0 0 1 0; 0 0 0 1]; 16 | 17 | world2px = inv(px2world); 18 | world2px_2x3 = world2px(1:2,[1,2,4]); 19 | px2world_2x3 = px2world(1:2, [1,2,4]); 20 | 21 | Q = imfilter(heights, [1, -1]) - (0.06/mag) > 0; 22 | Q = Q | imfilter(heights, [-1, 1]) - (0.06/mag) > 0; 23 | Q = Q | imfilter(heights, [1; -1]) - (0.06/mag) > 0; 24 | Q = Q | imfilter(heights, [-1; 1]) - (0.06/mag) > 0; 25 | Q(isnan(heights)) = 1; 26 | % grid = Q(85:125,25:85); 27 | grid = ~Q; 28 | [all_squares(1,:), all_squares(2,:)] = ind2sub(size(grid), 1:length(reshape(grid,[],1))); 29 | 30 | while true 31 | figure(2) 32 | subplot(211) 33 | imshow(grid, 'InitialMagnification', 'fit') 34 | [c,r] = ginput(1); 35 | if isempty(c) 36 | break 37 | end 38 | c = round(c); 39 | r = round(r); 40 | clear white_squares black_squares black_edges 41 | [white_squares(1,:), white_squares(2,:)] = ind2sub(size(grid), find(grid)); 42 | [black_squares(1,:), black_squares(2,:)] = ind2sub(size(grid), find(~grid)); 43 | black_edges = []; 44 | [black_edges(1,:), black_edges(2,:)] = ind2sub(size(grid), find(component_boundary(grid, [r;c]))); 45 | 46 | obstacles = mat2cell(black_edges, 2, ones(1,size(black_edges,2))); 47 | 48 | lb = [0;0]; 49 | ub = [size(grid,1); size(grid,2)]; 50 | A_bounds = [-1,0;0,-1;1,0;0,1]; 51 | b_bounds = [-lb; ub]; 52 | 53 | [A,b,C,d] = inflate_region(obstacles, A_bounds, b_bounds, [r;c]); 54 | 55 | figure(2) 56 | subplot(212) 57 | cla 58 | hold on 59 | for j = 1:length(obstacles) 60 | patch(obstacles{j}(1,:), obstacles{j}(2,:), 'k'); 61 | end 62 | V = lcon2vert(A, b); 63 | k = convhull(V(:,1), V(:,2)); 64 | plot(V(k,1), V(k,2), 'ro-'); 65 | th = linspace(0,2*pi,100); 66 | y = [cos(th);sin(th)]; 67 | x = bsxfun(@plus, C*y, d); 68 | plot(x(1,:), x(2,:), 'b-'); 69 | xlim([lb(1),ub(1)]) 70 | ylim([lb(1),ub(2)]) 71 | % grid(all(bsxfun(@le, A * all_squares, b), 1)) = 0; 72 | 73 | V = V(k,:); 74 | px_xy = [V(:,2)'; V(:,1)']; 75 | world_xy = px2world_2x3 * [px_xy; ones(1,size(px_xy,2))]; 76 | z = ones(size(px_xy,2)) * heights(r,c) + 0.03; 77 | world_xyz = [world_xy; z]; 78 | lcmgl.glColor3f(1,0,0); 79 | lcmgl.glLineWidth(10); 80 | lcmgl.glBegin(lcmgl.LCMGL_LINES); 81 | for j = 1:size(world_xy,2)-1 82 | lcmgl.glVertex3d(world_xyz(1,j),world_xyz(2,j),world_xyz(3,j)); 83 | lcmgl.glVertex3d(world_xyz(1,j+1),world_xyz(2,j+1),world_xyz(3,j+1)); 84 | end 85 | lcmgl.glVertex3d(world_xyz(1,end),world_xyz(2,end),world_xyz(3,end)); 86 | lcmgl.glVertex3d(world_xyz(1,1),world_xyz(2,1),world_xyz(3,1)); 87 | lcmgl.glEnd(); 88 | 89 | px_xy = [x(2,:); x(1,:)]; 90 | world_xy = px2world_2x3 * [px_xy; ones(1,size(px_xy,2))]; 91 | z = ones(size(px_xy,2)) * heights(r,c) + 0.03; 92 | world_xyz = [world_xy; z]; 93 | lcmgl.glColor3f(0,0,1); 94 | lcmgl.glLineWidth(5); 95 | lcmgl.glBegin(lcmgl.LCMGL_LINES); 96 | for j = 1:size(world_xy,2)-1 97 | lcmgl.glVertex3d(world_xyz(1,j),world_xyz(2,j),world_xyz(3,j)); 98 | lcmgl.glVertex3d(world_xyz(1,j+1),world_xyz(2,j+1),world_xyz(3,j+1)); 99 | end 100 | lcmgl.glVertex3d(world_xyz(1,end),world_xyz(2,end),world_xyz(3,end)); 101 | lcmgl.glVertex3d(world_xyz(1,1),world_xyz(2,1),world_xyz(3,1)); 102 | lcmgl.glEnd(); 103 | 104 | lcmgl.glColor3f(0,1,0); 105 | lcmgl.sphere([(px2world_2x3 * [c;r;1])', heights(r,c)], 0.05, 20, 20); 106 | 107 | 108 | % lcmgl.switchBuffers(); 109 | % plot_lcm_points([px_xy', px_z'],repmat([1,0,0],length(px_z),1),101,'region1',3,1); 110 | end 111 | lcmgl.switchBuffers(); 112 | end -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_polygon_recovery.m: -------------------------------------------------------------------------------- 1 | function test_polygon_recovery(record) 2 | import iris.inflate_region; 3 | 4 | if nargin < 1 5 | record = false; 6 | end 7 | 8 | r = 1; 9 | 10 | lb = [-r;-r]; 11 | ub = [r;r]; 12 | 13 | n_obs = 7; 14 | th = linspace(0,2*pi,n_obs+1); 15 | % th = [0,pi/4,pi/2,pi,5*pi/4,3*pi/2,2*pi]; 16 | 17 | % th = cumsum(rand(6, 1) * 3*pi./2); 18 | % th = th * (2*pi / th(end)); 19 | 20 | obstacles = zeros(2, 2, length(th)-1); 21 | for j = 1:length(th)-1 22 | obstacles(:,:,j) = [[r*cos(th(j));r*sin(th(j))],[r*cos(th(j+1));r*sin(th(j+1))]]; 23 | end 24 | 25 | A_bounds = [-1,0;0,-1;1,0;0,1]; 26 | b_bounds = [-lb; ub]; 27 | 28 | start = 0.5 .* (rand(2,1) .* (ub - lb) + lb); 29 | % start = [0;0]; 30 | % start = [0;0.98]; 31 | 32 | 33 | % profile on 34 | [A,b,C,d,results] = inflate_region(obstacles, A_bounds, b_bounds, start); 35 | % profile viewer 36 | iris.drawing.animate_results(results, record); 37 | 38 | end 39 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_simple_poly_2d.m: -------------------------------------------------------------------------------- 1 | function test_simple_poly_2d(record) 2 | import iris.inflate_region; 3 | import iris.drawing.*; 4 | 5 | if nargin < 1 6 | record = false; 7 | end 8 | 9 | lb = [0;0]; 10 | ub = [10;10]; 11 | 12 | obstacles = zeros(2, 4, 2); 13 | obs_offsets = [3, 3, -3, -3; 14 | -1.5, 1.5, 1.5, -1.5]; 15 | obstacles(:,:,1) = bsxfun(@plus, [3;1.5], obs_offsets); 16 | obstacles(:,:,2) = bsxfun(@plus, [3;8.5], obs_offsets); 17 | 18 | 19 | A_bounds = [-1,0;0,-1;1,0;0,1]; 20 | b_bounds = [-lb; ub]; 21 | 22 | start = [3;5]; 23 | 24 | % profile on 25 | [A,b,C,d,results] = inflate_region(obstacles, A_bounds, b_bounds, start); 26 | % profile viewer 27 | animate_results(results,record); 28 | 29 | end 30 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_terrain.m: -------------------------------------------------------------------------------- 1 | function selected_points = test_terrain() 2 | % NOTEST 3 | 4 | import iris.terrain_grid.*; 5 | import iris.inflate_region; 6 | import iris.thirdParty.polytopes.*; 7 | import iris.cspace.*; 8 | import iris.drawing.*; 9 | lcmgl = drake.util.BotLCMGLClient(lcm.lcm.LCM.getSingleton(), 'segmentation'); 10 | % load('example_feas_map.mat','Q'); 11 | load('data/example_heights.mat','heights','px2world'); 12 | load('data/example_state.mat', 'x0'); 13 | 14 | px2world(1,end) = px2world(1,end) - sum(px2world(1,1:3)); % stupid matlab 1-indexing... 15 | px2world(2,end) = px2world(2,end) - sum(px2world(2,1:3)); 16 | resample = 2; 17 | mag = 2^(resample-1); 18 | heights = interp2(heights, (resample-1)); 19 | px2world = px2world * [1/mag 0 0 (1-1/mag); 0 1/mag 0 (1-1/mag ); 0 0 1 0; 0 0 0 1]; 20 | 21 | world2px = inv(px2world); 22 | world2px_2x3 = world2px(1:2,[1,2,4]); 23 | px2world_2x3 = px2world(1:2, [1,2,4]); 24 | 25 | Q = imfilter(heights, [1, -1]) - (0.06/mag) > 0; 26 | Q = Q | imfilter(heights, [-1, 1]) - (0.06/mag) > 0; 27 | Q = Q | imfilter(heights, [1; -1]) - (0.06/mag) > 0; 28 | Q = Q | imfilter(heights, [-1; 1]) - (0.06/mag) > 0; 29 | Q(isnan(heights)) = 1; 30 | 31 | % grid = Q(85:125,25:85); 32 | grid = ~Q; 33 | lb = [180;70;-pi]; 34 | ub = [245; 160; pi]; 35 | A_bounds = [-1,0,0; 36 | 0,-1,0; 37 | 0,0,-1; 38 | 1,0,0; 39 | 0,1,0; 40 | 0,0,1]; 41 | b_bounds = [-lb;ub]; 42 | 43 | bot = world2px_2x3 * 0.9*bsxfun(@minus, [-0.0820,-0.0820, 0.1780, 0.1780; 44 | 0.0624,-0.0624, 0.0624,-0.0624; 45 | 0,0,0,0], [0.0480; 0; 0;]); 46 | % obstacles = segment_grid(~grid(lb(1):ub(1),lb(2):ub(2))); 47 | % for j = 1:length(obstacles) 48 | % obstacles{j} = bsxfun(@plus, obstacles{j}, lb(1:2)-1); 49 | % end 50 | % obstacles = cspace3(obstacles, bot, 4); 51 | selected_points = []; 52 | 53 | while true 54 | figure(6) 55 | imshow(grid, 'InitialMagnification', 'fit') 56 | [c,r] = ginput(1); 57 | if ~isempty(c) 58 | selected_points(:,end+1) = [r;c]; 59 | end 60 | if isempty(c) 61 | break 62 | end 63 | c = round(c); 64 | r = round(r); 65 | black_edges = []; 66 | [black_edges(1,:), black_edges(2,:)] = ind2sub(size(grid), find(component_boundary(grid, [r;c]))); 67 | obs_mask = all(bsxfun(@minus, A_bounds([1,2,4,5],1:2) * black_edges, b_bounds([1,2,4,5])) <= max(max(bot))); 68 | obstacles = mat2cell(black_edges(:,obs_mask) , 2, ones(1,sum(obs_mask))); 69 | obstacles = cspace3(obstacles, bot, 4); 70 | 71 | 72 | [A,b,C,d,results] = inflate_region(obstacles, A_bounds, b_bounds, [r;c;0]); 73 | % animate_results(results); 74 | 75 | [inner_poly, outer_poly] = project_c_space_region(A,b); 76 | 77 | % px_xy = [V(:,2)'; V(:,1)']; 78 | if ~isempty(inner_poly) 79 | px_xy = [inner_poly(2,:); inner_poly(1,:)]; 80 | world_xy = px2world_2x3 * [px_xy; ones(1,size(px_xy,2))]; 81 | z = ones(size(px_xy,2)) * heights(r,c) + 0.03; 82 | world_xyz = [world_xy; z]; 83 | lcmgl.glColor3f(0,1,0); 84 | lcmgl.glLineWidth(10); 85 | lcmgl.glBegin(lcmgl.LCMGL_LINES); 86 | for j = 1:size(world_xy,2)-1 87 | lcmgl.glVertex3d(world_xyz(1,j),world_xyz(2,j),world_xyz(3,j)); 88 | lcmgl.glVertex3d(world_xyz(1,j+1),world_xyz(2,j+1),world_xyz(3,j+1)); 89 | end 90 | lcmgl.glVertex3d(world_xyz(1,end),world_xyz(2,end),world_xyz(3,end)); 91 | lcmgl.glVertex3d(world_xyz(1,1),world_xyz(2,1),world_xyz(3,1)); 92 | lcmgl.glEnd(); 93 | end 94 | 95 | px_xy = [outer_poly(2,:); outer_poly(1,:)]; 96 | world_xy = px2world_2x3 * [px_xy; ones(1,size(px_xy,2))]; 97 | z = ones(size(px_xy,2)) * heights(r,c) + 0.03; 98 | world_xyz = [world_xy; z]; 99 | lcmgl.glColor3f(1,1,0); 100 | lcmgl.glLineWidth(10); 101 | lcmgl.glBegin(lcmgl.LCMGL_LINES); 102 | for j = 1:size(world_xy,2)-1 103 | lcmgl.glVertex3d(world_xyz(1,j),world_xyz(2,j),world_xyz(3,j)); 104 | lcmgl.glVertex3d(world_xyz(1,j+1),world_xyz(2,j+1),world_xyz(3,j+1)); 105 | end 106 | lcmgl.glVertex3d(world_xyz(1,end),world_xyz(2,end),world_xyz(3,end)); 107 | lcmgl.glVertex3d(world_xyz(1,1),world_xyz(2,1),world_xyz(3,1)); 108 | lcmgl.glEnd(); 109 | 110 | lcmgl.glColor3f(0,1,0); 111 | lcmgl.sphere([(px2world_2x3 * [c;r;1])', heights(r,c)], 0.05, 20, 20); 112 | 113 | 114 | % lcmgl.switchBuffers(); 115 | % plot_lcm_points([px_xy', px_z'],repmat([1,0,0],length(px_z),1),101,'region1',3,1); 116 | end 117 | lcmgl.switchBuffers(); 118 | end -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_thin_ellipsoid_simple.m: -------------------------------------------------------------------------------- 1 | A_bounds = [-eye(3); 2 | 1,1,1]; 3 | b_bounds = [0;0;0;1]; 4 | lb = [0;0;0]; 5 | ub = [1;1;1]; 6 | 7 | [C, d] = iris.inner_ellipsoid.mosek_nofusion(A_bounds, b_bounds); 8 | assert(det(C) > 0.01); 9 | 10 | iris.drawing.draw_3d(A_bounds, b_bounds, C, d, [], lb, ub); 11 | -------------------------------------------------------------------------------- /src/matlab/+iris/+test/test_timing.m: -------------------------------------------------------------------------------- 1 | function [all_results] = test_timing(dim) 2 | % NOTEST 3 | % Benchmark IRIS as a function of number of obstacles (for WAFR 2014 paper) 4 | 5 | 6 | lb = zeros(dim,1); 7 | ub = 10 * ones(dim,1); 8 | n_samples = 6; 9 | n_trials = 10; 10 | ns_obs = logspace(1, 6, n_samples); 11 | 12 | A_bounds = [-diag(ones(dim, 1)); diag(ones(dim,1))]; 13 | b_bounds = [-lb; ub]; 14 | all_results = iris.inflation_results.empty(); 15 | 16 | start = 0.5 * (ub + lb); 17 | for j = 1:n_samples 18 | for k = 1:n_trials 19 | ok = false; 20 | while ~ok 21 | n_obs = round(ns_obs(j)) 22 | obstacle_pts = iris.test.random_cubic_obstacles(dim, n_obs, lb, ub); 23 | try 24 | [A,b,C,d,results] = iris.inflate_region(obstacle_pts, A_bounds, b_bounds, start, []); 25 | ok = true; 26 | results.obstacles = []; 27 | catch e 28 | if strcmp(e.identifier, 'IRIS:InfeasibleStart') 29 | ok = false; 30 | else 31 | rethrow(e); 32 | end 33 | end 34 | end 35 | all_results(k,j) = results; 36 | end 37 | end 38 | 39 | -------------------------------------------------------------------------------- /src/matlab/+iris/+thirdParty/+mosek_lownerjohn/det_rootn.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Copyright: Copyright (c) MOSEK ApS, Denmark. All rights reserved. 3 | % 4 | % File: det_rootn.m 5 | % 6 | % Purpose: Models the hypograph of the n-th power of the 7 | % determinant of a positive definite matrix. 8 | % 9 | % The convex set (a hypograph) 10 | % 11 | % C = { (X, t) \in S^n_+ x R | t <= det(X)^{1/n} }, 12 | % 13 | % can be modeled as the intersection of a semidefinite cone 14 | % 15 | % [ X, Z; Z^T Diag(Z) ] >= 0 16 | % 17 | % and a number of rotated quadratic cones and affine hyperplanes, 18 | % 19 | % t <= (Z11*Z22*...*Znn)^{1/n} (see geometric_mean). 20 | % 21 | % References: 22 | % [1] "Lectures on Modern Optimization", Ben-Tal and Nemirovski, 2000. 23 | %% 24 | 25 | function [] = det_rootn(M, X, t) 26 | 27 | import mosek.fusion.*; 28 | import iris.thirdParty.mosek_lownerjohn.geometric_mean; 29 | 30 | n = sqrt(X.size()); 31 | 32 | % Setup variables 33 | Y = M.variable(Domain.inPSDCone(2*n)); 34 | 35 | % Setup Y = [X, Z; Z^T diag(Z)] 36 | Y11 = Y.slice([1, 1], [n+1, n+1]); 37 | Y21 = Y.slice([n+1, 1], [2*n+1, n+1]); 38 | Y22 = Y.slice([n+1, n+1], [2*n+1, 2*n+1]); 39 | 40 | S = Matrix.sparse(n, n, 1:n, 1:n, ones(1,n)); 41 | M.constraint( Expr.sub(Expr.mulElm(S,Y21), Y22), Domain.equalsTo(0.0) ); 42 | M.constraint( Expr.sub(X, Y11), Domain.equalsTo(0.0) ); 43 | 44 | % t^n <= (Z11*Z22*...*Znn) 45 | z = M.variable(n, Domain.unbounded()); 46 | for i=1:n, 47 | M.constraint( Expr.sub(z.index(i), Y22.index(i,i)), ... 48 | Domain.equalsTo(0.0) ); 49 | end 50 | 51 | geometric_mean(M, z, t); 52 | -------------------------------------------------------------------------------- /src/matlab/+iris/+thirdParty/+mosek_lownerjohn/geometric_mean.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Copyright: Copyright (c) MOSEK ApS, Denmark. All rights reserved. 3 | % 4 | % File: geometric_mean.py 5 | % 6 | % Purpose: Models the convex set 7 | % 8 | % S = { (x, t) \in R^n x R | x >= 0, t <= (x1 * x2 * ... *xn)^(1/n) }. 9 | % 10 | % as the intersection of rotated quadratic cones and affine hyperplanes, 11 | % see [1, p. 105]. This set can be interpreted as the hypograph of the 12 | % geometric mean of x. 13 | % 14 | % We illustrate the modeling procedure using the following example. 15 | % Suppose we have 16 | % 17 | % t <= (x1 * x2 * x3)^(1/3) 18 | % 19 | % for some t >= 0, x >= 0. We rewrite it as 20 | % 21 | % t^4 <= x1 * x2 * x3 * x4, x4 = t 22 | % 23 | % which is equivalent to (see [1]) 24 | % 25 | % x11^2 <= 2*x1*x2, x12^2 <= 2*x3*x4, 26 | % 27 | % x21^2 <= 2*x11*x12, 28 | % 29 | % sqrt(4)*x21 = t, x4 = t. 30 | % 31 | % References: 32 | % [1] "Lectures on Modern Optimization", Ben-Tal and Nemirovski, 2000. 33 | %% 34 | 35 | function [] = geometric_mean(M, x, t) 36 | % Models the convex set 37 | % 38 | % S = { (x, t) \in R^n x R | x >= 0, t <= (x1 * x2 * ... * xn)^(1/n) } 39 | % 40 | % as the intersection of rotated quadratic cones and affine hyperplanes. 41 | 42 | import mosek.fusion.*; 43 | 44 | n = x.size(); 45 | l = ceil(log2(n)); 46 | m = 2^l - n; 47 | 48 | if m>0 49 | x0 = Variable.vstack( x, M.variable(m, Domain.greaterThan(0.0) ) ); 50 | else 51 | x0=x; 52 | end 53 | 54 | z = x0; 55 | 56 | for i=1:l-1, 57 | xi = M.variable(2^(l-i), Domain.greaterThan(0.0)); 58 | 59 | for k=1:2^(l-i), 60 | M.constraint(Variable.vstack( z.index(2*k-1),z.index(2*k),xi.index(k)),... 61 | Domain.inRotatedQCone()); 62 | end 63 | z = xi; 64 | end 65 | 66 | t0 = M.variable(1, Domain.greaterThan(0.0)); 67 | M.constraint( Variable.vstack(z, t0), Domain.inRotatedQCone()); 68 | 69 | M.constraint(Expr.sub(Expr.mul(2^(0.5*l),t),t0), Domain.equalsTo(0.0)); 70 | for i=2^l-m+1:2^l 71 | M.constraint(Expr.sub(x0.index(i), t), Domain.equalsTo(0.0)); 72 | end 73 | -------------------------------------------------------------------------------- /src/matlab/+iris/+thirdParty/+mosek_lownerjohn/lownerjohn_ellipsoid.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Copyright: Copyright (c) MOSEK ApS, Denmark. All rights reserved. 3 | % 4 | % File: lownerjohn_ellipsoid.m 5 | % 6 | % Purpose: 7 | % Computes the Lowner-John inner and outer ellipsoidal 8 | % approximations of a polytope. 9 | % 10 | % 11 | % The inner ellipsoidal approximation to a polytope 12 | % 13 | % S = { x \in R^n | Ax < b }. 14 | % 15 | % maximizes the volume of the inscribed ellipsoid, 16 | % 17 | % { x | x = C*u + d, || u ||_2 <= 1 }. 18 | % 19 | % The volume is proportional to det(C)^(1/n), so the 20 | % problem can be solved as 21 | % 22 | % maximize t 23 | % subject to t <= det(C)^(1/n) 24 | % || C*ai ||_2 <= bi - ai^T * d, i=1,...,m 25 | % C is PSD 26 | % 27 | % which is equivalent to a mixed conic quadratic and semidefinite 28 | % programming problem. 29 | % 30 | % 31 | % The outer ellipsoidal approximation to a polytope given 32 | % as the convex hull of a set of points 33 | % 34 | % S = conv{ x1, x2, ... , xm } 35 | % 36 | % minimizes the volume of the enclosing ellipsoid, 37 | % 38 | % { x | || P*x-c ||_2 <= 1 } 39 | % 40 | % The volume is proportional to det(P)^{-1/n}, so the problem can 41 | % be solved as 42 | % 43 | % minimize t 44 | % subject to t >= det(P)^(-1/n) 45 | % || P*xi - c ||_2 <= 1, i=1,...,m 46 | % P is PSD. 47 | % 48 | % References: 49 | % [1] "Lectures on Modern Optimization", Ben-Tal and Nemirovski, 2000. 50 | % 51 | 52 | classdef lownerjohn_ellipsoid 53 | 54 | methods (Static) 55 | 56 | function [C, d] = lownerjohn_inner(A, b) 57 | 58 | import mosek.fusion.*; 59 | import iris.thirdParty.mosek_lownerjohn.det_rootn; 60 | 61 | M = Model('lownerjohn_inner'); 62 | 63 | [m, n] = size(A); 64 | 65 | A = DenseMatrix(A); 66 | 67 | % Setup variables 68 | t = M.variable('t', 1, Domain.greaterThan(0.0)); 69 | C = M.variable('C', NDSet(n,n), Domain.unbounded()); 70 | d = M.variable('d', n, Domain.unbounded()); 71 | 72 | % (bi - ai^T*d, C*ai) \in Q 73 | M.constraint('qc', Expr.hstack(Expr.sub(b, Expr.mul(A,d)), Expr.mul(A,C.transpose())), Domain.inQCone()); 74 | 75 | % t <= det(C)^{1/n} 76 | det_rootn(M, C, t); 77 | 78 | % Objective: Maximize t 79 | M.objective(ObjectiveSense.Maximize, t); 80 | 81 | M.solve(); 82 | 83 | C = reshape(C.level(), n, n); 84 | d = reshape(d.level(), n, 1); 85 | 86 | M.dispose(); 87 | end 88 | 89 | 90 | function [P, c] = lownerjohn_outer(x) 91 | 92 | import mosek.fusion.*; 93 | import iris.thirdParty.mosek_lownerjohn.det_rootn; 94 | 95 | M = Model('lownerjohn_outer'); 96 | 97 | [m, n] = size(x); 98 | 99 | % Setup variables 100 | t = M.variable('t', 1, Domain.greaterThan(0.0)); 101 | P = M.variable('P', NDSet(n,n), Domain.unbounded()); 102 | c = M.variable('c', n, Domain.unbounded()); 103 | 104 | % (1, P*xi-c) \in Q 105 | M.constraint('qc', ... 106 | Expr.hstack(Expr.constTerm(m,1.0), ... 107 | Expr.sub(Expr.mul(DenseMatrix(x),P.transpose()), ... 108 | Variable.reshape(Variable.repeat(c,m),NDSet(m,n)))), ... 109 | Domain.inQCone()); 110 | 111 | % t <= det(P)^{1/n} 112 | det_rootn(M, P, t); 113 | 114 | % Objective: Maximize t 115 | M.objective(ObjectiveSense.Maximize, t); 116 | M.solve(); 117 | 118 | P = reshape(P.level(), n, n); 119 | c = reshape(c.level(), n, 1); 120 | 121 | M.dispose(); 122 | end 123 | 124 | function plot_ellipse(C, d) 125 | 126 | N = 50; 127 | theta = 2*pi*[0:N-1]/(N-1); 128 | 129 | U = [cos(theta); sin(theta)]; 130 | X = C*U + d*ones(1,N); 131 | plot(X(1,:),X(2,:)); 132 | 133 | end 134 | 135 | function main() 136 | import mosek.fusion.*; 137 | 138 | p = [ 0., 0.; 1., 3.; 5.,4.; 7.,1.; 3.,-2. ]; 139 | m = length(p); 140 | 141 | A = zeros(m, 2); 142 | b = zeros(m, 1); 143 | 144 | A(1,:) = [ -p(1,2)+p(m,2), p(1,1)-p(m,1) ]; 145 | b(1) = A(1,:)*p(1,:)'; 146 | for i=2:size(p,1), 147 | A(i,:) = [ -p(i,2)+p(i-1,2), p(i,1)-p(i-1,1) ]; 148 | b(i) = A(i,:)*p(i,:)'; 149 | end; 150 | 151 | [Ci, di] = lownerjohn_ellipsoid.lownerjohn_inner(A, b); 152 | [Po, co] = lownerjohn_ellipsoid.lownerjohn_outer(p); 153 | 154 | plot([p(:,1); p(1,1)],[p(:,2); p(1,2)],'*-'); 155 | axis equal 156 | hold on 157 | lownerjohn_ellipsoid.plot_ellipse(Ci, di) 158 | lownerjohn_ellipsoid.plot_ellipse(inv(Po), Po\co) 159 | hold off 160 | 161 | end 162 | 163 | end 164 | 165 | end 166 | 167 | 168 | -------------------------------------------------------------------------------- /src/matlab/+iris/+thirdParty/+mosek_lownerjohn/lownerjohn_inner.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Copyright: Copyright (c) MOSEK ApS, Denmark. All rights reserved. 3 | % 4 | % File: lownerjohn_inner.m 5 | % 6 | % Purpose: 7 | % Computes the Lowner-John inner ellipsoidal 8 | % approximation of a polytope. 9 | % 10 | % 11 | % The inner ellipsoidal approximation to a polytope 12 | % 13 | % S = { x \in R^n | Ax < b }. 14 | % 15 | % maximizes the volume of the inscribed ellipsoid, 16 | % 17 | % { x | x = C*u + d, || u ||_2 <= 1 }. 18 | % 19 | % The volume is proportional to det(C)^(1/n), so the 20 | % problem can be solved as 21 | % 22 | % maximize t 23 | % subject to t <= det(C)^(1/n) 24 | % || C*ai ||_2 <= bi - ai^T * d, i=1,...,m 25 | % C is PSD 26 | % 27 | % which is equivalent to a mixed conic quadratic and semidefinite 28 | % programming problem. 29 | % 30 | % 31 | % References: 32 | % [1] "Lectures on Modern Optimization", Ben-Tal and Nemirovski, 2000. 33 | % 34 | function [C, d] = lownerjohn_inner(A, b) 35 | 36 | import mosek.fusion.*; 37 | import mosek_lownerjohn.det_rootn; 38 | 39 | M = Model('lownerjohn_inner'); 40 | 41 | [m, n] = size(A); 42 | 43 | % Setup variables 44 | t = M.variable('t', 1, Domain.greaterThan(0.0)); 45 | C = M.variable('C', NDSet(n,n), Domain.unbounded()); 46 | d = M.variable('d', n, Domain.unbounded()); 47 | 48 | % (bi - ai^T*d, C*ai) \in Q 49 | for i=1:m, 50 | M.constraint( sprintf('qc%d', i), ... 51 | Expr.vstack(Expr.sub(b(i) ,Expr.dot(A(i,:),d) ) , Expr.mul(C,A(i,:) )), ... 52 | Domain.inQCone() ); 53 | end 54 | 55 | % t <= det(C)^{1/n} 56 | det_rootn(M, C, t) 57 | 58 | % Objective: Maximize t 59 | M.objective(ObjectiveSense.Maximize, t) 60 | 61 | M.solve() 62 | 63 | C = reshape(C.level(), n, n); 64 | d = reshape(d.level(), n, 1); 65 | 66 | M.dispose(); -------------------------------------------------------------------------------- /src/matlab/+iris/+thirdParty/+polytopes/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Matt Jacobson 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the distribution 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /src/matlab/+iris/+thirdParty/+polytopes/qlcon2vert.m: -------------------------------------------------------------------------------- 1 | function [varargout]=qlcon2vert(x0,varargin) 2 | %A quicker version of lcon2vert that will skip some intensive steps when a 3 | %point x0 in the relative interior of the polyhedron is known a priori. 4 | % 5 | % [V,nr,nre]=qlcon2vert(x0, A,b,Aeq,beq,TOL) 6 | % 7 | %Here x0 is the known relative interior point. The rest of the input/output 8 | %arguments are as in LCON2VERT. 9 | % 10 | %In the interest of speed, QLCON2VERT will perform minimal error checking, 11 | %shifting the burden to the user of verifying that the input data has 12 | %appropriate properties. In particular, it will not try to verify that x0 13 | %is in fact in the relative interior. By default also, qlcon2vert will skip the 14 | %boundedness test, and presume the user knows the polyhedron to be bounded already. 15 | %However, you can re-activate the boundedness test by calling with the syntax, 16 | % 17 | % [V,nr,nre]=qlcon2vert(x0, A,b,Aeq,beq,TOL,1) 18 | 19 | 20 | if nargin<=3 21 | 22 | varargin(3:6)={[],[],[],0}; 23 | 24 | end 25 | 26 | x0=x0(:); 27 | 28 | [A,b,Aeq,beq]=deal(varargin{1:4}); 29 | 30 | b=b-A*x0; 31 | if ~isempty(beq) 32 | beq(:)=0; 33 | end 34 | 35 | varargin{2}=b; 36 | varargin{4}=beq; 37 | 38 | [varargout{1:max(nargout,1)}] = lcon2vert(varargin{:}); 39 | 40 | V=varargout{1}; 41 | 42 | V=bsxfun(@plus,x0.',V); %undo coordinate translation 43 | 44 | varargout{1}=V; 45 | -------------------------------------------------------------------------------- /src/matlab/+iris/+thirdParty/+polytopes/vert2lcon.m: -------------------------------------------------------------------------------- 1 | function [A,b,Aeq,beq]=vert2lcon(V,tol) 2 | %An extension of Michael Kleder's vert2con function, used for finding the 3 | %linear constraints defining a polyhedron in R^n given its vertices. This 4 | %wrapper extends the capabilities of vert2con to also handle cases where the 5 | %polyhedron is not solid in R^n, i.e., where the polyhedron is defined by 6 | %both equality and inequality constraints. 7 | % 8 | %SYNTAX: 9 | % 10 | % [A,b,Aeq,beq]=vert2lcon(V,TOL) 11 | % 12 | %The rows of the N x n matrix V are a series of N vertices of a polyhedron 13 | %in R^n. TOL is a rank-estimation tolerance (Default = 1e-10). 14 | % 15 | %Any point x inside the polyhedron will/must satisfy 16 | % 17 | % A*x <= b 18 | % Aeq*x = beq 19 | % 20 | %up to machine precision issues. 21 | % 22 | % 23 | %EXAMPLE: 24 | % 25 | %Consider V=eye(3) corresponding to the 3D region defined 26 | %by x+y+z=1, x>=0, y>=0, z>=0. 27 | % 28 | % 29 | % >>[A,b,Aeq,beq]=vert2lcon(eye(3)) 30 | % 31 | % 32 | % A = 33 | % 34 | % 0.4082 -0.8165 0.4082 35 | % 0.4082 0.4082 -0.8165 36 | % -0.8165 0.4082 0.4082 37 | % 38 | % 39 | % b = 40 | % 41 | % 0.4082 42 | % 0.4082 43 | % 0.4082 44 | % 45 | % 46 | % Aeq = 47 | % 48 | % 0.5774 0.5774 0.5774 49 | % 50 | % 51 | % beq = 52 | % 53 | % 0.5774 54 | 55 | 56 | %%initial stuff 57 | 58 | if nargin<2, tol=1e-10; end 59 | 60 | [M,N]=size(V); 61 | 62 | 63 | if M==1 64 | A=[];b=[]; 65 | Aeq=eye(N); beq=V(:); 66 | return 67 | end 68 | 69 | 70 | 71 | 72 | 73 | p=V(1,:).'; 74 | X=bsxfun(@minus,V.',p); 75 | 76 | 77 | %In the following, we need Q to be full column rank 78 | %and we prefer E compact. 79 | 80 | if M>N %X is wide 81 | 82 | [Q, R, E] = qr(X,0); %economy-QR ensures that E is compact. 83 | %Q automatically full column rank since X wide 84 | 85 | else%X is tall, hence non-solid polytope 86 | 87 | [Q, R, P]=qr(X); %non-economy-QR so that Q is full-column rank. 88 | 89 | [~,E]=max(P); %No way to get E compact. This is the alternative. 90 | clear P 91 | end 92 | 93 | 94 | diagr = abs(diag(R)); 95 | 96 | 97 | if nnz(diagr) 98 | 99 | %Rank estimation 100 | r = find(diagr >= tol*diagr(1), 1, 'last'); %rank estimation 101 | 102 | 103 | iE=1:length(E); 104 | iE(E)=iE; 105 | 106 | 107 | 108 | Rsub=R(1:r,iE).'; 109 | 110 | if r>1 111 | 112 | [A,b]=vert2con(Rsub,tol); 113 | 114 | elseif r==1 115 | 116 | A=[1;-1]; 117 | b=[max(Rsub);-min(Rsub)]; 118 | 119 | end 120 | 121 | A=A*Q(:,1:r).'; 122 | b=bsxfun(@plus,b,A*p); 123 | 124 | if r0; 219 | A(idx,:)=bsxfun(@rdivide,A(idx,:),normsA(idx)); 220 | b(idx)=b(idx)./normsA(idx); 221 | 222 | 223 | function [r,idx,Xsub]=lindep(X,tol) 224 | %Extract a linearly independent set of columns of a given matrix X 225 | % 226 | % [r,idx,Xsub]=lindep(X) 227 | % 228 | %in: 229 | % 230 | % X: The given input matrix 231 | % tol: A rank estimation tolerance. Default=1e-10 232 | % 233 | %out: 234 | % 235 | % r: rank estimate 236 | % idx: Indices (into X) of linearly independent columns 237 | % Xsub: Extracted linearly independent columns of X 238 | 239 | if ~nnz(X) %X has no non-zeros and hence no independent columns 240 | 241 | Xsub=[]; idx=[]; 242 | return 243 | end 244 | 245 | if nargin<2, tol=1e-10; end 246 | 247 | 248 | 249 | [Q, R, E] = qr(X,0); 250 | 251 | diagr = abs(diag(R)); 252 | 253 | 254 | %Rank estimation 255 | r = find(diagr >= tol*diagr(1), 1, 'last'); %rank estimation 256 | 257 | if nargout>1 258 | idx=sort(E(1:r)); 259 | idx=idx(:); 260 | end 261 | 262 | 263 | if nargout>2 264 | Xsub=X(:,idx); 265 | end -------------------------------------------------------------------------------- /src/matlab/+iris/+util/auto_seed_regions.m: -------------------------------------------------------------------------------- 1 | function safe_regions = auto_seed_regions(obstacle_pts, lb, ub, initial_seeds, num_regions, num_steps, callback) 2 | % Automatically try to seed a set of IRIS regions in an environment. Begins 3 | % by seeding at all of the initial_seeds values. Next, we grid the space by 4 | % units of (ub - lb) / num_steps, locate the grid cell which is farthest from the set of 5 | % obstacles and regions, and seed a new region there. We repeat until we have 6 | % num_regions regions. 7 | 8 | if isempty(callback) 9 | callback = @(r, s) false; 10 | end 11 | if iscell(obstacle_pts) 12 | padded = iris.pad_obstacle_points(obstacle_pts); 13 | obstacle_pts = cell2mat(reshape(padded, size(padded, 1), [], length(obstacle_pts))); 14 | end 15 | 16 | dim = length(lb); 17 | A_bounds = [eye(dim);-eye(dim)]; 18 | b_bounds = [ub; -lb]; 19 | 20 | step_size = (ub - lb) ./ num_steps; 21 | 22 | obs_regions = struct('A', cell(1, size(obstacle_pts, 3)), 'b', cell(1, size(obstacle_pts, 3)), 'C', cell(1, size(obstacle_pts, 3)), 'd', cell(1, size(obstacle_pts, 3)), 'point', cell(1, size(obstacle_pts, 3))); 23 | for j = 1:size(obstacle_pts, 3) 24 | [obs_regions(j).A, obs_regions(j).b] = iris.thirdParty.polytopes.vert2lcon(obstacle_pts(:,:,j)'); 25 | end 26 | safe_regions = struct('A', {}, 'b', {}, 'C', {}, 'd', {}, 'point', {}); 27 | 28 | for j = 1:size(initial_seeds, 2) 29 | seed = initial_seeds(:,j); 30 | [A, b, C, d] = iris.inflate_region(obstacle_pts, A_bounds, b_bounds, seed, 'require_containment', true, 'error_on_infeasible_start', false); 31 | safe_regions(end+1) = struct('A', A, 'b', b, 'C', C, 'd', d, 'point', seed); 32 | done = callback(safe_regions(end), seed); 33 | if done 34 | return 35 | end 36 | end 37 | 38 | while length(safe_regions) < num_regions 39 | dists = iris.util.obstacle_distance_matrix([obs_regions, safe_regions], lb, ub, num_steps); 40 | [~, idx] = max(dists(:)); 41 | subs = cell(1, dim); 42 | [subs{:}] = ind2sub(size(dists), idx); 43 | subs = cell2mat(subs)'; 44 | seed = (subs - 1) .* step_size + lb; 45 | [A, b, C, d] = iris.inflate_region(obstacle_pts, A_bounds, b_bounds, seed, 'require_containment', true, 'error_on_infeasible_start', false); 46 | safe_regions(end+1) = struct('A', A, 'b', b, 'C', C, 'd', d, 'point', seed); 47 | done = callback(safe_regions(end), seed); 48 | if done 49 | return 50 | end 51 | end 52 | 53 | -------------------------------------------------------------------------------- /src/matlab/+iris/+util/equal_up_to_permutations.m: -------------------------------------------------------------------------------- 1 | function isequal = equal_up_to_permutations(A, B, tolerance) 2 | % Returns true iff A and B are equal (to within tolerance) up to permutations of their rows. Greedily searches for matches row-by-row, which means that it may be possible for this function to return a false negative if several rows differ by amounts on the order of [tolerance]. 3 | 4 | if length(size(A)) ~= length(size(B)) || any(size(A) ~= size(B)) 5 | error('Sizes of A and B must match'); 6 | end 7 | 8 | B_rows_used = false(size(B, 1), 1); 9 | 10 | for j = 1:size(A, 1) 11 | available_rows = find(~B_rows_used); 12 | [max_diff, best_row] = min(max(abs(bsxfun(@minus, A(j,:), B(available_rows,:))), [], 2)); 13 | if max_diff > tolerance 14 | isequal = false; 15 | return; 16 | end 17 | B_rows_used(available_rows(best_row)) = true; 18 | end 19 | 20 | isequal = true; 21 | return; 22 | -------------------------------------------------------------------------------- /src/matlab/+iris/+util/obstacle_distance_matrix.m: -------------------------------------------------------------------------------- 1 | function dists = obstacle_distance_matrix(regions, lb, ub, num_cells) 2 | 3 | dim = length(lb); 4 | if size(num_cells) == 1 5 | num_cells = repmat(num_cells, size(lb)); 6 | end 7 | assert(all(size(lb) == size(ub))); 8 | 9 | for j = 1:length(regions) 10 | for k = 1:size(regions(j).A, 1) 11 | n = norm(regions(j).A(k,:)); 12 | regions(j).A(k,:) = regions(j).A(k,:) / n; 13 | regions(j).b(k) = regions(j).b(k) / n; 14 | % [regions(j).C, regions(j).d] = iris.maximal_ellipse(regions(j).A, regions(j).b); 15 | % regions(j).Cinv = inv(regions(j).C); 16 | end 17 | end 18 | 19 | 20 | dists = inf(reshape(num_cells, 1, [])); 21 | step_size = (ub - lb) ./ num_cells; 22 | 23 | s = cell(1, dim); 24 | for j = 1:numel(dists) 25 | [s{:}] = ind2sub(size(dists), j); 26 | 27 | x_vect = reshape(cell2mat(s), [], 1); 28 | x_vect = (x_vect - 1) .* step_size + lb; 29 | % for i = 1:size(obstacle_pts, 3) 30 | % Y = bsxfun(@minus, obstacle_pts(:,:,i), x_vect); 31 | % v = iris.least_distance.cvxgen_ldp(Y); 32 | % dists(j) = min(dists(j), norm(v)); 33 | % end 34 | dists(j) = min(dists(j), min(ub - x_vect)); 35 | dists(j) = min(dists(j), min(x_vect - lb)); 36 | for i = 1:length(regions) 37 | if all(regions(i).A * x_vect <= regions(i).b) 38 | dists(j) = 0; 39 | else 40 | dists(j) = min(dists(j), max(regions(i).A * x_vect - regions(i).b)); 41 | end 42 | % dists(j) = min(dists(j), norm(regions(i).Cinv * (x_vect - regions(i).d))-1); 43 | end 44 | 45 | end 46 | 47 | 48 | -------------------------------------------------------------------------------- /src/matlab/+iris/+util/orthos.m: -------------------------------------------------------------------------------- 1 | function [v1, v2] = orthos(y) 2 | 3 | if ~(y(2) == 0 && y(3) == 0) 4 | w = axis2rotmat([1,0,0,pi/2]) * y; 5 | elseif ~(y(1) == 0 && y(3) == 0) 6 | w = axis2rotmat([0,1,0,pi/2]) * y; 7 | else 8 | w = axis2rotmat([0,0,1,pi/2]) * y; 9 | end 10 | v1 = cross(y, w); 11 | v2 = cross(y, v1); 12 | 13 | end 14 | 15 | -------------------------------------------------------------------------------- /src/matlab/+iris/+util/rotmat.m: -------------------------------------------------------------------------------- 1 | function R = rotmat(theta) 2 | s = sin(theta); 3 | c = cos(theta); 4 | R = [c, -s; s, c]; 5 | end 6 | 7 | -------------------------------------------------------------------------------- /src/matlab/+iris/Ellipsoid.m: -------------------------------------------------------------------------------- 1 | classdef Ellipsoid 2 | properties 3 | C 4 | d 5 | end 6 | 7 | methods 8 | function obj = Ellipsoid(C, d) 9 | obj.C = C; 10 | obj.d = d; 11 | end 12 | end 13 | end 14 | -------------------------------------------------------------------------------- /src/matlab/+iris/Polyhedron.m: -------------------------------------------------------------------------------- 1 | classdef Polyhedron 2 | 3 | properties 4 | A; 5 | b; 6 | Aeq = []; 7 | beq = []; 8 | vertices; 9 | has_vertices = false; 10 | end 11 | 12 | methods 13 | function obj = Polyhedron(A, b, Aeq, beq) 14 | if nargin < 3 15 | Aeq = []; 16 | end 17 | if nargin < 4 18 | beq = []; 19 | end 20 | obj.A = A; 21 | obj.b = b; 22 | obj.Aeq = Aeq; 23 | obj.beq = beq; 24 | end 25 | 26 | function vertices = getVertices(obj) 27 | obj = obj.reduce(); 28 | if ~obj.has_vertices 29 | if exist('cddmex', 'file') 30 | H = struct('A', [obj.Aeq; obj.A], 'B', [obj.beq; obj.b], 'lin', (1:size(obj.Aeq, 1))'); 31 | V = cddmex('extreme', H); 32 | obj.vertices = V.V'; 33 | else 34 | obj.vertices = iris.thirdParty.polytopes.lcon2vert(obj.A, obj.b, obj.Aeq, obj.beq)'; 35 | end 36 | obj.has_vertices = true; 37 | end 38 | vertices = obj.vertices; 39 | end 40 | 41 | function reduced_poly = reduce(obj) 42 | % Find a minimal representation of the polyhedron 43 | if ~exist('cddmex', 'file') 44 | error('IRIS:MissingDependency', 'This function requires the cddmex tool. The easiest way to get it is using tbxmanager: http://www.tbxmanager.com/'); 45 | end 46 | H = struct('A', [obj.Aeq; obj.A], 'B', [obj.beq; obj.b], 'lin', (1:size(obj.Aeq, 1))'); 47 | Hred = cddmex('reduce_h', H); 48 | assert(isempty(Hred.lin), 'as far as I know, Hred.lin should always be empty. That is, the reduced polyhedron should not contain equality constraints. -rdeits'); 49 | reduced_poly = iris.Polyhedron(Hred.A, Hred.B); 50 | end 51 | 52 | function plotVertices(obj, varargin) 53 | vertices = obj.getVertices(); 54 | if ~isempty(vertices) 55 | if size(vertices, 2) > 2 56 | k = convhull(vertices(1,:), vertices(2,:)); 57 | else 58 | k = [1:size(vertices, 2), 1]; 59 | end 60 | plot(vertices(1,k), vertices(2,k), varargin{:}); 61 | end 62 | end 63 | 64 | function drawLCMGL(obj, lcmgl) 65 | lcmgl.glBegin(lcmgl.LCMGL_LINES); 66 | vertices = obj.getVertices(); 67 | if size(vertices, 2) > 2 68 | k = convhull(vertices(1,:), vertices(2,:)); 69 | else 70 | k = [1:size(vertices, 2), 1]; 71 | end 72 | for j = 1:length(k)-1 73 | lcmgl.glVertex3d(vertices(1,k(j)), vertices(2,k(j)), vertices(3,k(j))); 74 | lcmgl.glVertex3d(vertices(1,k(j+1)), vertices(2,k(j+1)), vertices(3,k(j+1))); 75 | end 76 | lcmgl.glEnd(); 77 | end 78 | 79 | function obj = normalize(obj) 80 | n = zeros(size(obj.A, 1), 1); 81 | for j = 1:size(obj.A, 1) 82 | n(j) = norm(obj.A(j,:)); 83 | obj.A(j,:) = obj.A(j,:) / n(j); 84 | obj.b(j) = obj.b(j) / n(j); 85 | end 86 | obj.A = obj.A(n > 0, :); 87 | obj.b = obj.b(n > 0); 88 | 89 | n = zeros(size(obj.Aeq, 1), 1); 90 | for j = 1:size(obj.Aeq, 1) 91 | n(j) = norm(obj.Aeq(j,:)); 92 | obj.Aeq(j,:) = obj.Aeq(j,:) / n(j); 93 | obj.beq(j) = obj.beq(j) / n(j); 94 | end 95 | obj.Aeq = obj.Aeq(n > 0, :); 96 | obj.beq = obj.beq(n > 0); 97 | end 98 | 99 | end 100 | 101 | methods(Static) 102 | function obj = fromVertices(vertices) 103 | [A, b] = iris.thirdParty.polytopes.vert2lcon(vertices'); 104 | obj = iris.Polyhedron(A, b); 105 | end 106 | 107 | function obj = from2DVertices(vertices) 108 | assert(size(vertices, 1) == 2); 109 | x = vertices(1,:); 110 | y = vertices(2,:); 111 | k = convhull(x,y, 'simplify', true); 112 | A = [(y(k(2:end)) - y(k(1:end-1)))', (x(k(1:end-1)) - x(k(2:end)))']; 113 | b = sum(A' .* [x(k(1:end-1)); y(k(1:end-1))], 1)'; 114 | obj = iris.Polyhedron(A, b); 115 | end 116 | 117 | function obj = from2DVerticesAndPlane(vertices, normal, v) 118 | assert(size(vertices, 1) == 2); 119 | % normal' * [x;y;z] = v; 120 | obj = iris.Polyhedron.from2DVertices(vertices); 121 | obj.Aeq = reshape(normal, 1, []); 122 | obj.beq = v; 123 | obj.A = [obj.A, zeros(size(obj.A, 1), 1)]; 124 | end 125 | 126 | function obj = fromBounds(lb, ub) 127 | % create a polyhedron representing a bounding box in n dimensions 128 | dim = length(lb); 129 | assert(length(lb) == length(ub)); 130 | A = [eye(dim); -eye(dim)]; 131 | b = [reshape(ub,[],1); reshape(-lb,[],1)]; 132 | obj = iris.Polyhedron(A, b); 133 | end 134 | end 135 | end 136 | -------------------------------------------------------------------------------- /src/matlab/+iris/TerrainRegion.m: -------------------------------------------------------------------------------- 1 | classdef TerrainRegion 2 | % A special class to describe a planar polyhedron of safe terrain. The inequality constraints 3 | % refer to the x, y, and yaw positions of a robot's foot, and the point and normal values 4 | % indicate the plane in x, y, z. 5 | properties 6 | A 7 | b 8 | C 9 | d 10 | point 11 | normal 12 | end 13 | 14 | methods 15 | function obj = TerrainRegion(A, b, C, d, point, normal) 16 | obj.A = A; 17 | obj.b = b; 18 | obj.C = C; 19 | obj.d = d; 20 | obj.point = point; 21 | obj.normal = normal; 22 | end 23 | 24 | function obj = reducePolyhedron(obj) 25 | reduced = iris.Polyhedron(obj.A, obj.b).reduce(); 26 | obj.A = reduced.A; 27 | obj.b = reduced.b; 28 | end 29 | 30 | function poly = getXYZPolyhedron(obj) 31 | A = [obj.A(:,1:2), zeros(size(obj.A, 1), 1)]; 32 | b = obj.b; 33 | n = reshape(obj.normal, 1, []); 34 | p = reshape(obj.point, [], 1); 35 | poly = iris.Polyhedron(A, b, n, n * p); 36 | end 37 | 38 | function ell = getXYEllipsoid(obj) 39 | C = obj.C(1:2,1:2); 40 | d = obj.d(1:2); 41 | ell = iris.Ellipsoid(C, d); 42 | end 43 | 44 | end 45 | end 46 | 47 | -------------------------------------------------------------------------------- /src/matlab/+iris/callback_silent.m: -------------------------------------------------------------------------------- 1 | function callback_silent(varargin) 2 | % A dummy callback which does nothing 3 | end 4 | -------------------------------------------------------------------------------- /src/matlab/+iris/inflate_region.m: -------------------------------------------------------------------------------- 1 | function [A, b, C, d, results] = inflate_region(obstacles, A_bounds, b_bounds, start, varargin) 2 | import iris.*; 3 | 4 | p = inputParser(); 5 | p.addOptional('require_containment', false, @isscalar); 6 | p.addOptional('error_on_infeasible_start', false, @isscalar); 7 | p.addOptional('termination_threshold', 2e-2, @(x) x > 0); 8 | p.addOptional('iter_limit', 100, @isnumeric); 9 | p.parse(varargin{:}); 10 | options = p.Results; 11 | 12 | if exist('+iris/inflate_regionmex', 'file') 13 | if ~iscell(obstacles) 14 | obs_cell = cell(1, size(obstacles, 3)); 15 | for j = 1:size(obstacles, 3) 16 | obs_cell{j} = obstacles(:,:,j); 17 | end 18 | obstacles = obs_cell; 19 | 20 | % Note: could replace this with the following, but mat2cell is 100 times slower than the for loop 21 | % obstacles = mat2cell(obstacles, size(obstacles, 1), size(obstacles, 2), ones(1, size(obstacles, 3))); 22 | end 23 | 24 | if nargout > 4 25 | [A, b, C, d, p_history, e_history] = inflate_regionmex(obstacles, A_bounds, b_bounds, start, options); 26 | results = inflation_results(); 27 | results.start = start; 28 | results.obstacles = obstacles; 29 | results.n_obs = numel(obstacles); 30 | results.e_history = e_history; 31 | results.p_history = p_history; 32 | else 33 | [A, b, C, d] = inflate_regionmex(obstacles, A_bounds, b_bounds, start, options); 34 | end 35 | else 36 | if iscell(obstacles) 37 | padded = pad_obstacle_points(obstacles); 38 | obstacle_pts = cell2mat(reshape(padded, size(padded, 1), [], length(obstacles))); 39 | else 40 | obstacle_pts = obstacles; 41 | end 42 | [A, b, C, d, results] = inflate_region_fallback(obstacle_pts, A_bounds, b_bounds, start, options); 43 | results.obstacles = obstacles; 44 | if ~iscell(results.obstacles) 45 | results.obstacles = mat2cell(results.obstacles, size(results.obstacles, 1), size(results.obstacles, 2), ones(1, size(results.obstacles, 3))); 46 | end 47 | end 48 | -------------------------------------------------------------------------------- /src/matlab/+iris/inflate_region_fallback.m: -------------------------------------------------------------------------------- 1 | function [A, b, C, d, results] = inflate_region_fallback(obstacle_pts, A_bounds, b_bounds, start, options) 2 | % MATLAB-only implementation of IRIS. This is slower and less flexible than the c++ interface, but does not require any compilation. 3 | import iris.*; 4 | 5 | DEBUG = false; 6 | 7 | if ~exist('mosekopt', 'file') 8 | addpath_mosek 9 | end 10 | 11 | results = inflation_results(); 12 | results.start = start; 13 | results.obstacles = obstacle_pts; 14 | results.n_obs = size(results.obstacles, 3); 15 | 16 | t_start = tic; 17 | 18 | dim = size(A_bounds, 2); 19 | d = start; 20 | C = 1e-4 * eye(dim); 21 | best_vol = -inf; 22 | iter = 1; 23 | results.e_history{1} = struct('C', C, 'd', d); 24 | 25 | while true 26 | t0 = tic(); 27 | [A, b, infeas_start] = separating_hyperplanes(obstacle_pts, C, d); 28 | if options.error_on_infeasible_start && infeas_start 29 | error('IRIS:InfeasibleStart', 'ellipse center is inside an obstacle'); 30 | end 31 | results.p_time = results.p_time + toc(t0); 32 | if iter > 1 && DEBUG 33 | for i = 1:length(b) 34 | assert(min(eig([(b(i) - A(i,:) * d) * eye(dim), C * (A(i,:)'); 35 | (C * (A(i,:)'))', (b(i) - A(i,:) * d)])) >= -1e-3); 36 | end 37 | end 38 | A = [A; A_bounds]; 39 | b = [b; b_bounds]; 40 | 41 | if options.require_containment 42 | if all(A * start <= b) || iter == 1 || infeas_start 43 | results.p_history{iter} = struct('A', A, 'b', b); 44 | else 45 | hist = results.p_history{iter-1}; 46 | A = hist.A; 47 | b = hist.b; 48 | disp('Breaking early because start point is no longer contained in polyhedron'); 49 | break 50 | end 51 | else 52 | results.p_history{iter} = struct('A', A, 'b', b); 53 | end 54 | 55 | t0 = tic(); 56 | [C, d, cvx_optval] = maximal_ellipse(A,b); 57 | results.e_time = results.e_time + toc(t0); 58 | results.e_history{iter+1} = struct('C', C, 'd', d); 59 | 60 | if abs(cvx_optval - best_vol)/best_vol < 2e-2 || iter >= options.iter_limit 61 | break 62 | end 63 | best_vol = cvx_optval; 64 | iter = iter + 1; 65 | end 66 | 67 | results.iters = iter; 68 | results.total_time = toc(t_start); 69 | -------------------------------------------------------------------------------- /src/matlab/+iris/inflate_regionmex.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "iris/iris.h" 5 | 6 | using namespace Eigen; 7 | 8 | // mxGetPrSafe and eigenToMatlab lovingly ripped off from the Drake toolbox: https://github.com/RobotLocomotion/drake 9 | double * mxGetPrSafe(const mxArray *pobj) { 10 | if (!mxIsDouble(pobj)) mexErrMsgIdAndTxt("Iris:mxGetPrSafe:WrongType", "mxGetPr can only be called on arguments which correspond to Matlab doubles"); 11 | return mxGetPr(pobj); 12 | } 13 | 14 | mxArray* eigenToMatlab(const MatrixXd &m) 15 | { 16 | // this avoids zero initialization that would occur using mxCreateDoubleMatrix with nonzero dimensions. 17 | // see https://classes.soe.ucsc.edu/ee264/Fall11/cmex.pdf, page 8 18 | mxArray* pm = mxCreateDoubleMatrix(0, 0, mxREAL); 19 | int rows = static_cast(m.rows()); 20 | int cols = static_cast(m.cols()); 21 | int numel = rows * cols; 22 | mxSetM(pm, rows); 23 | mxSetN(pm, cols); 24 | if (numel) 25 | mxSetData(pm, mxMalloc(sizeof(double) * numel)); 26 | memcpy(mxGetPr(pm), m.data(), sizeof(double)* numel); 27 | return pm; 28 | } 29 | 30 | DLL_EXPORT_SYM 31 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 32 | // auto begin = std::chrono::high_resolution_clock::now(); 33 | 34 | if (nrhs != 5 || nlhs > 6) { 35 | mexErrMsgTxt("usage: [A, b, C, d, p_history, e_history] = inflate_regionmex(obstacles, A_bounds, b_bounds, start, options)"); 36 | } 37 | 38 | int narg = 0; 39 | 40 | const mxArray* obstacles = prhs[narg]; 41 | if (!mxIsCell(obstacles)) { 42 | mexErrMsgTxt("obstacles should be a cell array of matrices"); 43 | } 44 | narg++; 45 | 46 | Map A_bounds(mxGetPrSafe(prhs[narg]), 47 | mxGetM(prhs[narg]), 48 | mxGetN(prhs[narg])); 49 | narg++; 50 | Map b_bounds(mxGetPrSafe(prhs[narg]), 51 | mxGetNumberOfElements(prhs[narg])); 52 | narg++; 53 | 54 | Map start(mxGetPrSafe(prhs[narg]), 55 | mxGetNumberOfElements(prhs[narg])); 56 | narg++; 57 | 58 | // mexPrintf("made maps\n"); 59 | 60 | const mxArray* options_ptr = prhs[narg]; 61 | 62 | const int dim = start.size(); 63 | if (A_bounds.cols() != dim) { 64 | mexPrintf("Problem dimension was inferred to be %d from length of [start], so [A_bounds] should have %d columns\n", start.size(), A_bounds.cols()); 65 | mexErrMsgTxt("Dimension of problem does not match size of A_bounds"); 66 | } 67 | if (A_bounds.rows() != b_bounds.size()) { 68 | mexErrMsgTxt("A_bounds and b_bounds should have the same number of rows"); 69 | } 70 | 71 | iris::IRISProblem problem(dim); 72 | problem.setSeedPoint(start); 73 | 74 | // mexPrintf("set seed\n"); 75 | 76 | const size_t n_obs = mxGetNumberOfElements(obstacles); 77 | for (size_t i=0; i < n_obs; i++) { 78 | const mxArray *obs_ptr = mxGetCell(obstacles, i); 79 | Map obs(mxGetPrSafe(obs_ptr), 80 | mxGetM(obs_ptr), 81 | mxGetN(obs_ptr)); 82 | if (obs.rows() != dim) { 83 | mexErrMsgTxt("Each obstacle should be of size M x N where M is the dimension of [start]."); 84 | } 85 | problem.addObstacle(obs); 86 | } 87 | 88 | // mexPrintf("added obstacles\n"); 89 | 90 | iris::IRISOptions options; 91 | const mxArray *opt; 92 | opt = mxGetField(options_ptr, 0, "require_containment"); 93 | if (opt) options.require_containment = static_cast(mxGetScalar(opt)); 94 | 95 | opt = mxGetField(options_ptr, 0, "error_on_infeasible_start"); 96 | if (opt) options.error_on_infeasible_start = static_cast(mxGetScalar(opt)); 97 | 98 | opt = mxGetField(options_ptr, 0, "termination_threshold"); 99 | if (opt) options.termination_threshold = mxGetScalar(opt); 100 | 101 | opt = mxGetField(options_ptr, 0, "iter_limit"); 102 | if (opt) options.iter_limit = static_cast(mxGetScalar(opt)); 103 | 104 | // mexPrintf("got options\n"); 105 | 106 | problem.setBounds(iris::Polyhedron(A_bounds, b_bounds)); 107 | 108 | // auto end = std::chrono::high_resolution_clock::now(); 109 | // auto elapsed = std::chrono::duration_cast>(end - begin); 110 | // std::cout << "pre-solve time: " << elapsed.count() << " s" << std::endl; 111 | 112 | iris::IRISRegion region; 113 | std::unique_ptr debug; 114 | // begin = std::chrono::high_resolution_clock::now(); 115 | try { 116 | if (nlhs > 4) { 117 | debug.reset(new iris::IRISDebugData()); 118 | region = iris::inflate_region(problem, options, debug.get()); 119 | } else { 120 | region = iris::inflate_region(problem, options); 121 | } 122 | } catch (iris::InitialPointInfeasibleError &exception) { 123 | mexErrMsgIdAndTxt("IRIS:InfeasibleStart", "Initial point is infeasible"); 124 | } 125 | // end = std::chrono::high_resolution_clock::now(); 126 | // elapsed = std::chrono::duration_cast>(end - begin); 127 | // std::cout << "solve time: " << elapsed.count() << " s" << std::endl; 128 | 129 | // begin = std::chrono::high_resolution_clock::now(); 130 | // mexPrintf("ran iris\n"); 131 | 132 | narg = 0; 133 | if (nlhs > narg) plhs[narg] = eigenToMatlab(region.polyhedron.getA()); 134 | narg++; 135 | 136 | if (nlhs > narg) plhs[narg] = eigenToMatlab(region.polyhedron.getB()); 137 | narg++; 138 | 139 | if (nlhs > narg) plhs[narg] = eigenToMatlab(region.ellipsoid.getC()); 140 | narg++; 141 | 142 | if (nlhs > narg) plhs[narg] = eigenToMatlab(region.ellipsoid.getD()); 143 | narg++; 144 | 145 | if (nlhs > narg) { 146 | const size_t n_polys[1] = {debug->polyhedron_history.size()}; 147 | plhs[narg] = mxCreateCellArray(1, n_polys); 148 | for (int i=0; i < debug->polyhedron_history.size(); i++) { 149 | const size_t dims[1] = {1}; 150 | const char* fields[2] = {"A", "b"}; 151 | mxArray* entry = mxCreateStructArray(1, dims, 2, fields); 152 | mxSetField(entry, 0, "A", eigenToMatlab(debug->polyhedron_history[i].getA())); 153 | mxSetField(entry, 0, "b", eigenToMatlab(debug->polyhedron_history[i].getB())); 154 | mxSetCell(plhs[narg], i, entry); 155 | } 156 | } 157 | narg++; 158 | 159 | if (nlhs > narg) { 160 | const size_t n_ellipsoids[1] = {debug->ellipsoid_history.size()}; 161 | plhs[narg] = mxCreateCellArray(1, n_ellipsoids); 162 | for (int i=0; i < debug->ellipsoid_history.size(); i++) { 163 | const size_t dims[1] = {1}; 164 | const char* fields[2] = {"C", "d"}; 165 | mxArray* entry = mxCreateStructArray(1, dims, 2, fields); 166 | mxSetField(entry, 0, "C", eigenToMatlab(debug->ellipsoid_history[i].getC())); 167 | mxSetField(entry, 0, "d", eigenToMatlab(debug->ellipsoid_history[i].getD())); 168 | mxSetCell(plhs[narg], i, entry); 169 | } 170 | } 171 | narg++; 172 | 173 | // end = std::chrono::high_resolution_clock::now(); 174 | // elapsed = std::chrono::duration_cast>(end - begin); 175 | // std::cout << "post-solve time: " << elapsed.count() << " s" << std::endl; 176 | } 177 | -------------------------------------------------------------------------------- /src/matlab/+iris/inflation_results.m: -------------------------------------------------------------------------------- 1 | classdef inflation_results 2 | 3 | properties 4 | e_history 5 | p_history 6 | start 7 | obstacles 8 | e_time 9 | p_time 10 | total_time 11 | iters 12 | n_obs 13 | end 14 | 15 | methods 16 | function obj = inflation_results() 17 | obj.e_history = {}; 18 | obj.p_history = {}; 19 | obj.start = []; 20 | obj.obstacles = {}; 21 | obj.n_obs = 0; 22 | obj.e_time = 0; 23 | obj.p_time = 0; 24 | obj.total_time = 0; 25 | obj.iters = 0; 26 | end 27 | end 28 | 29 | end 30 | 31 | -------------------------------------------------------------------------------- /src/matlab/+iris/maximal_ellipse.m: -------------------------------------------------------------------------------- 1 | function [C, d, volume] = maximal_ellipse(A,b) 2 | 3 | % poly = iris.Polyhedron(A, b).reduce(); 4 | [Ad, ia] = unique(A,'rows'); 5 | A = Ad; 6 | b = b(ia); 7 | 8 | [C, d] = iris.inner_ellipsoid.mosek_nofusion(A, b); 9 | % [C, d] = iris.inner_ellipsoid.mosek_ellipsoid(A, b); 10 | 11 | % If Mosek fails for you, you can use CVX with the free SDPT3 solver, 12 | % but it will be much (about 100X) slower. Just swap the above line for the 13 | % following: 14 | % [C, d] = iris.inner_ellipsoid.cvx_ellipsoid(A, b); 15 | 16 | volume = det(C); 17 | 18 | -------------------------------------------------------------------------------- /src/matlab/+iris/pad_obstacle_points.m: -------------------------------------------------------------------------------- 1 | function padded_obstacles = pad_obstacle_points(obstacles) 2 | 3 | padded_obstacles = obstacles; 4 | needs_pad = false; 5 | max_len = 0; 6 | for j = 1:length(obstacles) 7 | l = size(obstacles{j},2); 8 | if l > max_len 9 | max_len = l; 10 | end 11 | end 12 | for j = 1:length(obstacles) 13 | if size(obstacles{j},2) < max_len 14 | needs_pad = true; 15 | end 16 | end 17 | 18 | if needs_pad 19 | for j = 1:length(obstacles) 20 | if size(obstacles{j}, 2) < max_len 21 | obs = obstacles{j}; 22 | padded_obstacles{j} = [obs, repmat(obs(:,end), 1, max_len - size(obs,2))]; 23 | end 24 | end 25 | else 26 | padded_obstacles = obstacles; 27 | end -------------------------------------------------------------------------------- /src/matlab/+iris/sample_convex_polyhedron.m: -------------------------------------------------------------------------------- 1 | function x = sample_convex_polyhedron(A, b, n) 2 | % Generate n random samples from the convex polyhedron defined 3 | % as the intersection of half-spaces Ax <= b 4 | import iris.thirdParty.polytopes.*; 5 | if nargin < 3 6 | n = 1; 7 | end 8 | 9 | v = lcon2vert(A,b)'; 10 | 11 | lb = min(v, [], 2); 12 | ub = max(v, [], 2); 13 | 14 | dim = size(A,2); 15 | nsamples = 0; 16 | x = zeros(dim, n); 17 | while nsamples < n 18 | z = rand(dim, 1) .* (ub - lb) + lb; 19 | if all(A * z <= b) 20 | nsamples = nsamples + 1; 21 | x(:,nsamples) = z; 22 | end 23 | end 24 | 25 | -------------------------------------------------------------------------------- /src/matlab/+iris/separating_hyperplanes.m: -------------------------------------------------------------------------------- 1 | function [A, b, infeas_start] = separating_hyperplanes(obstacle_pts, C, d) 2 | 3 | persistent mosek_res 4 | 5 | dim = size(C,1); 6 | infeas_start = false; 7 | n_obs = size(obstacle_pts, 3); 8 | pts_per_obs = size(obstacle_pts, 2); 9 | Cinv = inv(C); 10 | Cinv2 = (Cinv * Cinv'); 11 | if n_obs == 0 || isempty(obstacle_pts) 12 | A = zeros(0, dim); 13 | b = zeros(0, 1); 14 | infeas_start = false; 15 | return; 16 | end 17 | 18 | uncovered_obstacles = true(n_obs,1); 19 | planes_to_use = false(n_obs, 1); 20 | 21 | 22 | image_pts = reshape(Cinv * bsxfun(@minus, reshape(obstacle_pts, dim, []), d), size(obstacle_pts)); 23 | image_dists = reshape(sum(image_pts.^2, 1), size(obstacle_pts, 2), size(obstacle_pts, 3)); 24 | obs_image_dists = min(image_dists, [], 1); 25 | [~, obs_sort_idx] = sort(obs_image_dists); 26 | 27 | flat_obs_pts = reshape(obstacle_pts, dim, []); 28 | 29 | A = zeros(n_obs,dim); 30 | b = zeros(n_obs,1); 31 | 32 | for i = obs_sort_idx; 33 | if uncovered_obstacles(i) 34 | obs = obstacle_pts(:,:,i); 35 | ys = image_pts(:,:,i); 36 | dists = image_dists(:,i); 37 | [~,idx] = min(dists); 38 | xi = obs(:,idx); 39 | nhat = 2 * Cinv2 * (xi - d); 40 | nhat = nhat / norm(nhat); 41 | b0 = nhat' * xi; 42 | if all(nhat' * obs - b0 >= 0) 43 | % nhat is feasible, so we can skip the optimization 44 | A(i,:) = nhat'; 45 | b(i) = b0; 46 | else 47 | if isempty(mosek_res) 48 | [~,mosek_res] = mosekopt('symbcon echo(0)'); 49 | end 50 | ystar = iris.least_distance.mosek_ldp(ys, mosek_res); 51 | 52 | if norm(ystar) < 1e-3 53 | % d is inside the obstacle. So we'll just reverse nhat to try to push the 54 | % ellipsoid out of the obstacle. 55 | % warning('IRIS:EllipseCenterInObstacle', 'ellipse center is inside an obstacle.'); 56 | infeas_start = true; 57 | A(i,:) = -nhat'; 58 | b(i) = -nhat' * xi; 59 | else 60 | xstar = C*ystar + d; 61 | nhat = 2 * Cinv2 * (xstar - d); 62 | nhat = nhat / norm(nhat); 63 | A(i,:) = nhat; 64 | b(i) = nhat' * xstar; 65 | end 66 | end 67 | 68 | check = A(i,:) * flat_obs_pts >= b(i); 69 | check = reshape(check', pts_per_obs, []); 70 | excluded = all(check, 1); 71 | uncovered_obstacles(excluded) = false; 72 | 73 | planes_to_use(i) = true; 74 | uncovered_obstacles(i) = false; 75 | 76 | if ~any(uncovered_obstacles) 77 | break 78 | end 79 | end 80 | end 81 | A = A(planes_to_use,:); 82 | b = b(planes_to_use); 83 | end -------------------------------------------------------------------------------- /src/matlab/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | install(DIRECTORY +iris DESTINATION matlab) 2 | -------------------------------------------------------------------------------- /src/matlab/data/example_feas_map.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rdeits/iris-distro/ff624610a82a858862d55732136dbc2cc9ab16fc/src/matlab/data/example_feas_map.mat -------------------------------------------------------------------------------- /src/matlab/data/example_heights.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rdeits/iris-distro/ff624610a82a858862d55732136dbc2cc9ab16fc/src/matlab/data/example_heights.mat -------------------------------------------------------------------------------- /src/matlab/data/example_terrain_clicks.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rdeits/iris-distro/ff624610a82a858862d55732136dbc2cc9ab16fc/src/matlab/data/example_terrain_clicks.mat -------------------------------------------------------------------------------- /src/python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | install(DIRECTORY irispy DESTINATION ${IRIS_PYTHON_DIR}) 2 | 3 | if(BUILD_TESTING) 4 | add_test(NAME python_tests 5 | COMMAND "${PYTHON_EXECUTABLE}" -m nose --nocapture irispy) 6 | set_property(TEST python_tests PROPERTY ENVIRONMENT "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${IRIS_PYTHON_DIR}") 7 | 8 | if(PYTHON_VERSION_MAJOR EQUAL 2) 9 | find_program(JUPYTER_EXECUTABLE NAMES jupyter) 10 | 11 | if(JUPYTER_EXECUTABLE) 12 | add_test(NAME example_iris_2d 13 | COMMAND "${JUPYTER_EXECUTABLE}" nbconvert --to notebook --execute iris_2d.ipynb --output iris_2d.ipynb 14 | WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/examples") 15 | set_tests_properties(example_iris_2d PROPERTIES ENVIRONMENT "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${IRIS_PYTHON_DIR}") 16 | 17 | add_test(NAME example_polyhedrons 18 | COMMAND "${JUPYTER_EXECUTABLE}" nbconvert --to notebook --execute polyhedrons.ipynb --output polyhedrons.ipynb 19 | WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/examples") 20 | set_tests_properties(example_polyhedrons PROPERTIES ENVIRONMENT "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${IRIS_PYTHON_DIR}") 21 | endif() 22 | endif() 23 | endif() 24 | -------------------------------------------------------------------------------- /src/python/irispy/.gitignore: -------------------------------------------------------------------------------- 1 | iris_wrapper.py 2 | -------------------------------------------------------------------------------- /src/python/irispy/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .irispy import (inflate_region, 4 | inner_ellipsoid, 5 | Polyhedron, 6 | Ellipsoid, 7 | IRISRegion, 8 | IRISOptions, 9 | IRISProblem) -------------------------------------------------------------------------------- /src/python/irispy/cspace.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | 3 | import numpy as np 4 | from scipy.spatial import ConvexHull 5 | 6 | 7 | def minkowski_sum(a, b): 8 | """ 9 | Compute the 2D Minkowski sum of the convex shapes defined by vertices in [a] and [b] 10 | """ 11 | assert a.shape[0] == 2 12 | assert b.shape[0] == 2 13 | if a.shape[1] == 1 or b.shape[1] == 1: 14 | return a + b 15 | else: 16 | p = np.zeros((2, a.shape[1] * b.shape[1])) 17 | idx = 0 18 | for j in range(a.shape[1]): 19 | p[:,idx:idx+b.shape[1]] = a[:,j].reshape((2,-1)) + b 20 | idx += b.shape[1] 21 | hull = ConvexHull(p.T) 22 | return p[:,hull.vertices] 23 | 24 | def cspace3(obs, bot, theta_steps): 25 | """ 26 | Compute the 3D (x, y, yaw) configuration space obstacle for a lit of convex 2D obstacles given by [obs] and a convex 2D robot given by vertices in [bot] at a variety of theta values. 27 | 28 | obs should be a 3D array of size (2, vertices_per_obstacle, num_obstacles) 29 | 30 | bot should be a 2d array of size (2, num_bot_vertices) 31 | 32 | theta_steps can either be a scalar, in which case it specifies the number of theta values, evenly spaced between -pi and +pi; or it can be a vector of theta values. 33 | """ 34 | bot = -np.array(bot) 35 | 36 | if np.isscalar(theta_steps): 37 | thetas = np.linspace(-np.pi, np.pi, num=theta_steps) 38 | else: 39 | thetas = theta_steps 40 | 41 | c_obs = [] 42 | for k in range(obs.shape[2]): 43 | for j in range(len(thetas)-1): 44 | th0 = thetas[j] 45 | th1 = thetas[j+1] 46 | 47 | bot_rot0 = rotmat(th0).dot(bot) 48 | c_obs0 = minkowski_sum(bot_rot0, obs[:,:,k]) 49 | 50 | bot_rot1 = rotmat(th1).dot(bot) 51 | c_obs1 = minkowski_sum(bot_rot1, obs[:,:,k]) 52 | 53 | c_pts = np.vstack((np.hstack((c_obs0, c_obs1)), 54 | np.hstack((th0 + np.zeros(c_obs0.shape[1]), 55 | th1 + np.zeros(c_obs1.shape[1]))))) 56 | c_obs.append(c_pts) 57 | if len(c_obs) == 0: 58 | return np.zeros((3, bot.shape[1] * 2, 0)) 59 | max_n_vert = max((x.shape[1] for x in c_obs)) 60 | return np.dstack((np.pad(c, pad_width=((0,0), (0,max_n_vert-c.shape[1])), mode='edge') for c in c_obs)) 61 | 62 | 63 | def rotmat(theta): 64 | c = np.cos(theta) 65 | s = np.sin(theta) 66 | return np.array([[c, -s], [s, c]]) -------------------------------------------------------------------------------- /src/python/irispy/drawing.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import matplotlib.pyplot as plt 4 | import scipy.spatial 5 | import numpy as np 6 | from matplotlib.colors import colorConverter 7 | import mpl_toolkits.mplot3d as a3 8 | 9 | 10 | def draw(self, ax=None, **kwargs): 11 | if self.getDimension() == 2: 12 | return self.draw2d(ax=ax, **kwargs) 13 | elif self.getDimension() == 3: 14 | return self.draw3d(ax=ax, **kwargs) 15 | else: 16 | raise NotImplementedError("drawing for objects of dimension <2 or >3 not implemented yet") 17 | 18 | def draw2d(self, ax=None, **kwargs): 19 | if ax is None: 20 | ax = plt.gca() 21 | points = self.getDrawingVertices() 22 | kwargs.setdefault("edgecolor", self.default_color) 23 | return draw_2d_convhull(points, ax, **kwargs) 24 | 25 | def draw3d(self, ax=None, **kwargs): 26 | if ax is None: 27 | ax = a3.Axes3D(plt.gcf()) 28 | points = self.getDrawingVertices() 29 | kwargs.setdefault("facecolor", self.default_color) 30 | return draw_3d_convhull(points, ax, **kwargs) 31 | 32 | def draw_convhull(points, ax, **kwargs): 33 | dim = points.shape[1] 34 | if dim == 2: 35 | return draw_2d_convhull(points, ax, **kwargs) 36 | elif dim == 3: 37 | return draw_3d_convhull(points, ax, **kwargs) 38 | else: 39 | raise NotImplementedError("not implemented for dimension < 2 or > 3") 40 | 41 | def draw_2d_convhull(points, ax, **kwargs): 42 | hull = scipy.spatial.ConvexHull(points) 43 | kwargs.setdefault("facecolor", "none") 44 | return [ax.add_patch(plt.Polygon(xy=points[hull.vertices],**kwargs))] 45 | 46 | def draw_3d_convhull(points, ax, **kwargs): 47 | kwargs.setdefault("edgecolor", "k") 48 | kwargs.setdefault("facecolor", "r") 49 | kwargs.setdefault("alpha", 0.5) 50 | kwargs["facecolor"] = colorConverter.to_rgba(kwargs["facecolor"], kwargs["alpha"]) 51 | hull = scipy.spatial.ConvexHull(points) 52 | artists = [] 53 | for simplex in hull.simplices: 54 | poly = a3.art3d.Poly3DCollection([points[simplex]], **kwargs) 55 | if "alpha" in kwargs: 56 | poly.set_alpha(kwargs["alpha"]) 57 | ax.add_collection3d(poly) 58 | artists.append(poly) 59 | return artists 60 | 61 | -------------------------------------------------------------------------------- /src/python/irispy/extensions/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rdeits/iris-distro/ff624610a82a858862d55732136dbc2cc9ab16fc/src/python/irispy/extensions/__init__.py -------------------------------------------------------------------------------- /src/python/irispy/extensions/ellipsoid.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def getDrawingVertices(self): 5 | if self.getDimension() == 2: 6 | theta = np.linspace(0, 2 * np.pi, 100) 7 | y = np.vstack((np.sin(theta), np.cos(theta))) 8 | return (self.getC().dot(y) + self.getD().reshape((-1, 1))).T 9 | elif self.getDimension() == 3: 10 | theta = np.linspace(0, 2 * np.pi, 20) 11 | y = np.vstack((np.sin(theta), np.cos(theta), np.zeros_like(theta))) 12 | for phi in np.linspace(0, np.pi, 10): 13 | R = np.array([[1.0, 0.0, 0.0], 14 | [0.0, np.cos(phi), -np.sin(phi)], 15 | [0.0, np.sin(phi), np.cos(phi)]]) 16 | y = np.hstack((y, R.dot(y))) 17 | x = self.getC().dot(y) + self.getD().reshape((-1, 1)) 18 | return x.T 19 | else: 20 | raise NotImplementedError("Ellipsoid vertices not implemented for dimension < 2 or > 3") 21 | -------------------------------------------------------------------------------- /src/python/irispy/extensions/irisdebugdata.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import mpl_toolkits.mplot3d as a3 4 | import matplotlib.animation as animation 5 | from .. import drawing 6 | 7 | 8 | def iterRegions(self): 9 | return zip(self.polyhedron_history, self.ellipsoid_history) 10 | 11 | def animate(self, fig=None, pause=0.5, show=True, repeat_delay=2.0): 12 | dim = self.bounds.getDimension() 13 | if dim < 2 or dim > 3: 14 | raise NotImplementedError("animation is not implemented for dimension < 2 or > 3") 15 | if fig is None: 16 | fig = plt.figure() 17 | if dim == 3: 18 | ax = a3.Axes3D(fig) 19 | else: 20 | ax = plt.gca() 21 | 22 | bounding_pts = np.vstack(self.boundingPoints()) 23 | if bounding_pts.size > 0: 24 | lb = bounding_pts.min(axis=0) 25 | ub = bounding_pts.max(axis=0) 26 | assert(lb.size == dim) 27 | assert(ub.size == dim) 28 | width = ub - lb 29 | ax.set_xlim(lb[0] - 0.1 * width[0], ub[0] + 0.1 * width[0]) 30 | ax.set_ylim(lb[1] - 0.1 * width[1], ub[1] + 0.1 * width[1]) 31 | if dim == 3: 32 | ax.set_zlim(lb[2] - 0.1 * width[2], ub[2] + 0.1 * width[2]) 33 | 34 | artist_sets = [] 35 | for poly, ellipsoid in self.iterRegions(): 36 | artists = [] 37 | d = self.ellipsoid_history[0].getD() 38 | if dim == 3: 39 | artists.extend(ax.plot([d[0]], [d[1]], 'go', zs=[d[2]], markersize=10)) 40 | else: 41 | artists.extend(ax.plot([d[0]], [d[1]], 'go', markersize=10)) 42 | artists.extend(poly.draw(ax)) 43 | artists.extend(ellipsoid.draw(ax)) 44 | for obs in self.getObstacles(): 45 | artists.extend(drawing.draw_convhull(obs.T, ax, edgecolor='k', facecolor='k', alpha=0.5)) 46 | artist_sets.append(tuple(artists)) 47 | 48 | ani = animation.ArtistAnimation(fig, artist_sets, interval=pause*1000, repeat_delay=repeat_delay*1000) 49 | if show: 50 | plt.show() 51 | -------------------------------------------------------------------------------- /src/python/irispy/extensions/polyhedron.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def fromBounds(cls, lb, ub): 5 | """ 6 | Return a new Polyhedron representing an n-dimensional box spanning 7 | from [lb] to [ub] 8 | """ 9 | lb = np.asarray(lb, dtype=np.float64) 10 | ub = np.asarray(ub, dtype=np.float64) 11 | p = cls() 12 | p.setA(np.vstack((np.eye(lb.size), -np.eye(lb.size)))) 13 | p.setB(np.hstack((ub, -lb))) 14 | return p 15 | 16 | # For backward compatibility 17 | from_bounds = fromBounds 18 | 19 | def getDrawingVertices(self): 20 | return np.vstack(self.generatorPoints()) -------------------------------------------------------------------------------- /src/python/irispy/irispy.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | import numpy as np 4 | 5 | from .iris_wrapper import (IRISOptions, IRISRegion, IRISProblem, 6 | IRISDebugData, Ellipsoid, Polyhedron, 7 | inner_ellipsoid) 8 | from .iris_wrapper import inflate_region as c_inflate_region 9 | from .extensions import ellipsoid 10 | from .extensions import polyhedron 11 | from .extensions import irisdebugdata 12 | from . import drawing 13 | 14 | 15 | Polyhedron.fromBounds = classmethod(polyhedron.fromBounds) 16 | Polyhedron.from_bounds = Polyhedron.fromBounds 17 | Polyhedron.getDrawingVertices = polyhedron.getDrawingVertices 18 | Polyhedron.default_color = "r" 19 | Polyhedron.draw = drawing.draw 20 | Polyhedron.draw2d = drawing.draw2d 21 | Polyhedron.draw3d = drawing.draw3d 22 | 23 | Ellipsoid.getDrawingVertices = ellipsoid.getDrawingVertices 24 | Ellipsoid.default_color = "r" 25 | Ellipsoid.draw = drawing.draw 26 | Ellipsoid.draw2d = drawing.draw2d 27 | Ellipsoid.draw3d = drawing.draw3d 28 | 29 | IRISDebugData.animate = irisdebugdata.animate 30 | IRISDebugData.iterRegions = irisdebugdata.iterRegions 31 | 32 | 33 | def inflate_region(obstacles, 34 | start_point_or_ellipsoid, 35 | bounds=None, 36 | require_containment=False, 37 | required_containment_points=[], 38 | error_on_infeasible_start=False, 39 | termination_threshold=2e-2, 40 | iter_limit=100, 41 | return_debug_data=False): 42 | if not isinstance(start_point_or_ellipsoid, Ellipsoid): 43 | # Assume it's a starting point instead 44 | 45 | seed_ellipsoid = Ellipsoid.fromNSphere(start_point_or_ellipsoid) 46 | else: 47 | seed_ellipsoid = start_point_or_ellipsoid 48 | 49 | dim = seed_ellipsoid.getDimension() 50 | problem = IRISProblem(dim) 51 | problem.setSeedEllipsoid(seed_ellipsoid) 52 | 53 | for obs in obstacles: 54 | problem.addObstacle(obs) 55 | 56 | if bounds is not None: 57 | problem.setBounds(bounds) 58 | 59 | options = IRISOptions() 60 | options.require_containment = require_containment 61 | options.required_containment_points = required_containment_points 62 | options.error_on_infeasible_start = error_on_infeasible_start 63 | options.termination_threshold = termination_threshold 64 | options.iter_limit = iter_limit 65 | 66 | if return_debug_data: 67 | debug = IRISDebugData() 68 | region = c_inflate_region(problem, options, debug) 69 | return region, debug 70 | else: 71 | region = c_inflate_region(problem, options) 72 | return region 73 | -------------------------------------------------------------------------------- /src/python/irispy/test/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rdeits/iris-distro/ff624610a82a858862d55732136dbc2cc9ab16fc/src/python/irispy/test/__init__.py -------------------------------------------------------------------------------- /src/python/irispy/test/cpp_wrapper_test.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import irispy.iris_wrapper as iris_wrapper 4 | import numpy as np 5 | 6 | 7 | def testCppWrapper(): 8 | p = iris_wrapper.Polyhedron() 9 | p.setA(np.eye(2)) 10 | p.setB(np.array([3.0, 4.0])) 11 | 12 | print(p.contains(np.array([2.5, 5.5]), 0.0)) 13 | try: 14 | print(p.contains(5, 0.0)) 15 | except TypeError as e: 16 | print("(successfully threw the expected error)") 17 | 18 | print(p.generatorPoints()) 19 | print(p.generatorPoints()[0]) 20 | for pt in p.generatorPoints(): 21 | print("generator:", pt) 22 | 23 | if __name__ == '__main__': 24 | testCppWrapper() 25 | -------------------------------------------------------------------------------- /src/python/irispy/test/python_interface_test.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import irispy 4 | import irispy.iris_wrapper 5 | import numpy as np 6 | 7 | 8 | def testInterface(show=False): 9 | p2 = irispy.Polyhedron() 10 | p2.setA(np.eye(2)) 11 | p2.setB(np.array([3.0, 4.0])) 12 | print(p2.contains(np.array([2.5, 5.5]), 0.0)) 13 | 14 | p3 = irispy.Polyhedron.fromBounds([-1, -1], [2, 2]) 15 | 16 | problem = irispy.IRISProblem(2) 17 | problem.setBounds(irispy.Polyhedron.fromBounds([-1, -1], [2, 2])) 18 | problem.setSeedPoint(np.array([0.0, 0.0])) 19 | problem.addObstacle(np.array([[1.5, 2], [1.5, 2]])) 20 | region = irispy.iris_wrapper.inflate_region(problem, irispy.IRISOptions()) 21 | print(region) 22 | print(region.getPolyhedron().generatorPoints()) 23 | print(region.getEllipsoid().getC()) 24 | print(region.getEllipsoid().getD()) 25 | 26 | import matplotlib.pyplot as plt 27 | region.polyhedron.draw2d() 28 | region.ellipsoid.draw2d() 29 | plt.gca().set_xlim([-1.5, 2.5]) 30 | plt.gca().set_ylim([-1.5, 2.5]) 31 | if show: 32 | plt.show() 33 | 34 | if __name__ == '__main__': 35 | testInterface(True) 36 | -------------------------------------------------------------------------------- /src/python/irispy/test/test_cpp_exception.py: -------------------------------------------------------------------------------- 1 | import irispy 2 | import numpy as np 3 | from unittest import TestCase 4 | 5 | 6 | class CPPExcpetionTest(TestCase): 7 | def test_dimension_error(self): 8 | obstacles = [np.array([[2., 0], [2, 2], [3, 2], [3, 0]]).T] 9 | bounds = irispy.Polyhedron.fromBounds([-1, -1], [3, 3]) 10 | seed_point = np.array([1.0, 2.0]) 11 | 12 | # this should pass 13 | region = irispy.inflate_region(obstacles, seed_point, bounds) 14 | 15 | 16 | # Now the obstacle is the wrong shape, so we should get a runtime error (but not a crash) 17 | obstacles = [np.array([[2., 0], [2, 2], [3, 2], [3, 0]])] 18 | try: 19 | region = irispy.inflate_region(obstacles, seed_point, bounds) 20 | self.assertTrue(False) # This should not have succeeded 21 | except RuntimeError as e: 22 | self.assertTrue(str(e) == "The matrix of obstacle vertices must have the same number of row as the dimension of the problem") 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/python/irispy/test/test_ellipsoid.py: -------------------------------------------------------------------------------- 1 | import irispy 2 | import numpy as np 3 | from unittest import TestCase 4 | 5 | 6 | class EllipsoidTest(TestCase): 7 | def test_volume(self): 8 | e = irispy.Ellipsoid.fromNSphere(np.array([0., 0.]), 1.0) 9 | self.assertAlmostEqual(e.getVolume(), np.pi) 10 | self.assertAlmostEqual(e.getC()[0,0], 1) 11 | self.assertAlmostEqual(e.getC()[0,1], 0) 12 | 13 | e = irispy.Ellipsoid.fromNSphere(np.array([5.0, 0.1,-1000]), 0.5) 14 | self.assertAlmostEqual(e.getVolume(), 4.0 / 3.0 * np.pi * 0.5 ** 3) 15 | self.assertAlmostEqual(e.getC()[1,1], 0.5) 16 | 17 | def test_inner_ellipsoid(self): 18 | # polyhedron from [-1.3, -1.4] to [1.1, 1.2] 19 | # center is at [-0.1, -0.1] 20 | A = np.vstack((np.eye(2), 21 | -np.eye(2))) 22 | b = np.array([1.1, 1.2, 1.3, 1.4]) 23 | p = irispy.Polyhedron(A, b) 24 | e = irispy.inner_ellipsoid(p) 25 | self.assertTrue(np.allclose(e.getD(), [-0.1, -0.1])) 26 | self.assertTrue(np.allclose(e.getC(), np.array([[1.2, 0], [0, 1.3]]))) 27 | -------------------------------------------------------------------------------- /src/python/irispy/test/test_iris.py: -------------------------------------------------------------------------------- 1 | import irispy 2 | import numpy as np 3 | from unittest import TestCase 4 | 5 | 6 | class IRISTest(TestCase): 7 | def test_box(self): 8 | obstacles = [np.array([[0.0, 1.0], 9 | [0.0, 0.0]]), 10 | np.array([[1.0, 1.0], 11 | [0.0, 1.0]]), 12 | np.array([[1.0, 0.0], 13 | [1.0, 1.0]]), 14 | np.array([[0.0, 0.0], 15 | [1.0, 0.0]]) 16 | ] 17 | start = irispy.Ellipsoid.fromNSphere(np.array([0.1, 0.1])) 18 | 19 | region = irispy.inflate_region(obstacles, start) 20 | 21 | C = region.getEllipsoid().getC() 22 | self.assertAlmostEqual(C[0,0], 0.5, 3) 23 | self.assertAlmostEqual(C[0,1], 0.0, 3) 24 | 25 | d = region.getEllipsoid().getD() 26 | self.assertAlmostEqual(d[0], 0.5, 3) 27 | self.assertAlmostEqual(d[1], 0.5, 3) 28 | 29 | def test_point_start(self): 30 | obstacles = [np.array([[0.0, 1.0], 31 | [0.0, 0.0]]), 32 | np.array([[1.0, 1.0], 33 | [0.0, 1.0]]), 34 | np.array([[1.0, 0.0], 35 | [1.0, 1.0]]), 36 | np.array([[0.0, 0.0], 37 | [1.0, 0.0]]) 38 | ] 39 | start = np.array([0.1, 0.1]) 40 | 41 | region = irispy.inflate_region(obstacles, start) 42 | 43 | C = region.getEllipsoid().getC() 44 | self.assertAlmostEqual(C[0,0], 0.5, 3) 45 | self.assertAlmostEqual(C[0,1], 0.0, 3) 46 | 47 | d = region.getEllipsoid().getD() 48 | self.assertAlmostEqual(d[0], 0.5, 3) 49 | self.assertAlmostEqual(d[1], 0.5, 3) 50 | 51 | def test_debug_data(): 52 | import matplotlib.pyplot as plt 53 | 54 | obstacles = [np.array([[0.3, 0.5, 1.0, 1.0], 55 | [0.1, 1.0, 1.0, 0.0]])] 56 | bounds = irispy.Polyhedron() 57 | bounds.setA(np.vstack((np.eye(2), -np.eye(2)))) 58 | bounds.setB(np.array([2.0, 2, 2, 2])) 59 | start = np.array([0.1, -0.05]) 60 | 61 | # print "running with debug" 62 | region, debug = irispy.inflate_region(obstacles, start, bounds=bounds, return_debug_data=True) 63 | # print "done" 64 | 65 | debug.animate(pause=0.5, show=False) 66 | 67 | 68 | -------------------------------------------------------------------------------- /src/python/irispy/test/test_iris_2d.py: -------------------------------------------------------------------------------- 1 | import irispy 2 | import numpy as np 3 | 4 | def test_random_obstacles_2d(show=False): 5 | bounds = irispy.Polyhedron.from_bounds([0, 0], [1, 1]) 6 | obstacles = [] 7 | for i in range(10): 8 | center = np.random.random((2,)) 9 | scale = np.random.random() * 0.3 10 | pts = np.random.random((2,4)) 11 | pts = pts - np.mean(pts, axis=1)[:,np.newaxis] 12 | pts = scale * pts + center[:,np.newaxis] 13 | obstacles.append(pts) 14 | start = np.array([0.5, 0.5]) 15 | 16 | region, debug = irispy.inflate_region(obstacles, start, bounds=bounds, return_debug_data=True) 17 | 18 | debug.animate(pause=0.5, show=show) 19 | 20 | if __name__ == '__main__': 21 | test_random_obstacles_2d(True) -------------------------------------------------------------------------------- /src/python/irispy/test/test_iris_3d.py: -------------------------------------------------------------------------------- 1 | import irispy 2 | import numpy as np 3 | 4 | def test_random_obstacles_3d(show=False): 5 | bounds = irispy.Polyhedron.from_bounds([0, 0, 0], [1, 1, 1]) 6 | obstacles = [] 7 | for i in range(5): 8 | center = np.random.random((3,)) 9 | scale = np.random.random() * 0.3 10 | pts = np.random.random((3,4)) 11 | pts = pts - np.mean(pts, axis=1)[:,np.newaxis] 12 | pts = scale * pts + center[:,np.newaxis] 13 | obstacles.append(pts) 14 | start = np.array([0.5, 0.5, 0.5]) 15 | 16 | region, debug = irispy.inflate_region(obstacles, start, bounds=bounds, return_debug_data=True) 17 | 18 | debug.animate(pause=0.5, show=show) 19 | 20 | if __name__ == '__main__': 21 | test_random_obstacles_3d(True) -------------------------------------------------------------------------------- /src/python/irispy/test/test_polytope.py: -------------------------------------------------------------------------------- 1 | import irispy 2 | import numpy as np 3 | import unittest 4 | import matplotlib.pyplot as plt 5 | import mpl_toolkits.mplot3d as a3 6 | 7 | class PolyhedronTest(unittest.TestCase): 8 | def test_constructor(self): 9 | p = irispy.Polyhedron() 10 | A = np.zeros((2,2)) 11 | 12 | # print A 13 | p.setA(A) 14 | A2 = p.getA() 15 | A2[0,0] = 1 16 | self.assertAlmostEqual(p.getA()[0,0], 0.0) 17 | 18 | def test_generators(self): 19 | p = irispy.Polyhedron() 20 | A = np.vstack((np.eye(2), 21 | -np.eye(2))) 22 | b = np.array([1.1, 1.2, 1.3, 1.4]) 23 | p.setA(A) 24 | p.setB(b) 25 | points = p.generatorPoints() 26 | 27 | expected = [np.array([1.1, 1.2]), 28 | np.array([-1.3, 1.2]), 29 | np.array([-1.3, -1.4]), 30 | np.array([1.1, -1.4])] 31 | found_expected = [False for i in expected] 32 | 33 | for point in points: 34 | for i, ex in enumerate(expected): 35 | if np.all(np.abs(point.T - ex) < 1e-3): 36 | found_expected[i] = True 37 | self.assertTrue(all(found_expected)) 38 | 39 | def test_plotting(self): 40 | fig = plt.figure() 41 | ax = fig.add_subplot(1,1,1) 42 | p = irispy.Polyhedron() 43 | A = np.vstack((np.eye(2), 44 | -np.eye(2))) 45 | b = np.array([1.1, 1.2, 1.3, 1.4]) 46 | p.setA(A) 47 | p.setB(b) 48 | p.draw(ax, alpha=0.5) 49 | ax.relim() 50 | ax.autoscale_view() 51 | # plt.show() 52 | 53 | def test_plotting_3d(self): 54 | fig = plt.figure() 55 | ax = a3.Axes3D(fig) 56 | p = irispy.Polyhedron() 57 | A = np.vstack((np.eye(3), 58 | -np.eye(3))) 59 | b = np.array([1.1, 1.2, 1.3, 1.4, 1.5, 1.6]) 60 | p.setA(A) 61 | p.setB(b) 62 | p.draw(ax) 63 | ax.relim() 64 | ax.autoscale_view() 65 | # plt.show() 66 | 67 | if __name__ == '__main__': 68 | unittest.main() 69 | 70 | 71 | -------------------------------------------------------------------------------- /src/python/irispy/test/test_required_containment.py: -------------------------------------------------------------------------------- 1 | import irispy 2 | import numpy as np 3 | import unittest 4 | 5 | class ContainmentTest(unittest.TestCase): 6 | def test_required_containment(self): 7 | obstacles = [np.array([[0., 1], 8 | [0, 0]]), 9 | np.array([[1., 1], 10 | [0, 1]]), 11 | np.array([[1., 0], 12 | [1, 1]]), 13 | np.array([[0., 0], 14 | [1, 0]])] 15 | required_containment_pts = [np.array([1.5, 1.5])] 16 | start = np.array([0.1, 0.1]) 17 | 18 | region = irispy.inflate_region(obstacles, start, 19 | require_containment=True, 20 | required_containment_points=required_containment_pts) 21 | self.assertTrue(region.polyhedron.getNumberOfConstraints() == 0, "polyhedron should be empty") 22 | 23 | if __name__ == '__main__': 24 | unittest.main() -------------------------------------------------------------------------------- /src/python/irispy/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import irispy 3 | 4 | def lcon_to_vert(A, b): 5 | poly = irispy.Polyhedron(A.shape[1]) 6 | poly.setA(A) 7 | poly.setB(b) 8 | V = np.vstack(poly.generatorPoints()).T 9 | 10 | def sample_convex_polytope(A, b, nsamples): 11 | poly = irispy.Polyhedron(A.shape[1]) 12 | poly.setA(A) 13 | poly.setB(b) 14 | generators = np.vstack(poly.generatorPoints()) 15 | lb = np.min(generators, axis=0) 16 | ub = np.max(generators, axis=0) 17 | 18 | n = 0 19 | samples = np.zeros((len(lb), nsamples)) 20 | while n < nsamples: 21 | z = np.random.uniform(lb, ub) 22 | if np.all(poly.A.dot(z) <= poly.b): 23 | samples[:,n] = z 24 | n += 1 25 | return samples -------------------------------------------------------------------------------- /travis/linux.sh: -------------------------------------------------------------------------------- 1 | set -e 2 | 3 | mkdir -p ~/tools 4 | pushd ~/tools 5 | 6 | wget https://cmake.org/files/v3.5/cmake-3.5.2-Linux-x86_64.tar.gz 7 | tar zxvf cmake-3.5.2-Linux-x86_64.tar.gz 8 | rm cmake-3.5.2-Linux-x86_64.tar.gz 9 | cd cmake-3.5.2-Linux-x86_64/bin 10 | export PATH=`pwd`:$PATH 11 | popd 12 | 13 | sudo apt-get install -y pkg-config libgmp-dev 14 | --------------------------------------------------------------------------------