├── .gitignore ├── 2d_example.ipynb ├── CITATION.cff ├── README.md ├── alternative_formulations ├── cost_2norm_ch.npy ├── cost_2norm_edgewise.npy ├── cost_2norm_micp.npy ├── cost_2norm_proposed.npy ├── cost_2norm_squared_ch.npy ├── cost_2norm_squared_edgewise.npy ├── cost_2norm_squared_micp.npy ├── cost_2norm_squared_proposed.npy ├── plot.ipynb └── scales.npy ├── bilinear_programs.ipynb ├── cyclic_1d.ipynb ├── hpp_reduction_proof.ipynb ├── loose_cycles.ipynb ├── loose_symmetry.ipynb ├── sensory_coverage.ipynb ├── solution_d.npy ├── solution_nE.npy ├── solution_nV_nE.npy ├── solution_nom.npy ├── solution_vol.npy ├── spp ├── convex_functions.py ├── convex_sets.py ├── graph.py ├── pwa_systems.py ├── shortest_path.py ├── shortest_path_convex_hull.py ├── shortest_path_edgewise.py └── shortest_path_mccormick.py ├── statistical_analysis.ipynb ├── statistical_analysis.txt └── stepping_stones.ipynb /.gitignore: -------------------------------------------------------------------------------- 1 | ## Core latex/pdflatex auxiliary files: 2 | *.aux 3 | *.lof 4 | *.log 5 | *.lot 6 | *.fls 7 | *.out 8 | *.toc 9 | *.fmt 10 | *.fot 11 | *.cb 12 | *.cb2 13 | .*.lb 14 | 15 | ## Intermediate documents: 16 | *.dvi 17 | *.xdv 18 | *-converted-to.* 19 | # these rules might exclude image files for figures etc. 20 | # *.ps 21 | # *.eps 22 | # *.pdf 23 | 24 | ## Generated if empty string is given at "Please type another file name for output:" 25 | .pdf 26 | 27 | ## Bibliography auxiliary files (bibtex/biblatex/biber): 28 | *.bbl 29 | *.bcf 30 | *.blg 31 | *-blx.aux 32 | *-blx.bib 33 | *.run.xml 34 | 35 | ## Build tool auxiliary files: 36 | *.fdb_latexmk 37 | *.synctex 38 | *.synctex(busy) 39 | *.synctex.gz 40 | *.synctex.gz(busy) 41 | *.pdfsync 42 | 43 | ## Build tool directories for auxiliary files 44 | # latexrun 45 | latex.out/ 46 | 47 | ## Auxiliary and intermediate files from other packages: 48 | # algorithms 49 | *.alg 50 | *.loa 51 | 52 | # achemso 53 | acs-*.bib 54 | 55 | # amsthm 56 | *.thm 57 | 58 | # beamer 59 | *.nav 60 | *.pre 61 | *.snm 62 | *.vrb 63 | 64 | # changes 65 | *.soc 66 | 67 | # comment 68 | *.cut 69 | 70 | # cprotect 71 | *.cpt 72 | 73 | # elsarticle (documentclass of Elsevier journals) 74 | *.spl 75 | 76 | # endnotes 77 | *.ent 78 | 79 | # fixme 80 | *.lox 81 | 82 | # feynmf/feynmp 83 | *.mf 84 | *.mp 85 | *.t[1-9] 86 | *.t[1-9][0-9] 87 | *.tfm 88 | 89 | #(r)(e)ledmac/(r)(e)ledpar 90 | *.end 91 | *.?end 92 | *.[1-9] 93 | *.[1-9][0-9] 94 | *.[1-9][0-9][0-9] 95 | *.[1-9]R 96 | *.[1-9][0-9]R 97 | *.[1-9][0-9][0-9]R 98 | *.eledsec[1-9] 99 | *.eledsec[1-9]R 100 | *.eledsec[1-9][0-9] 101 | *.eledsec[1-9][0-9]R 102 | *.eledsec[1-9][0-9][0-9] 103 | *.eledsec[1-9][0-9][0-9]R 104 | 105 | # glossaries 106 | *.acn 107 | *.acr 108 | *.glg 109 | *.glo 110 | *.gls 111 | *.glsdefs 112 | *.lzo 113 | *.lzs 114 | 115 | # uncomment this for glossaries-extra (will ignore makeindex's style files!) 116 | # *.ist 117 | 118 | # gnuplottex 119 | *-gnuplottex-* 120 | 121 | # gregoriotex 122 | *.gaux 123 | *.gtex 124 | 125 | # htlatex 126 | *.4ct 127 | *.4tc 128 | *.idv 129 | *.lg 130 | *.trc 131 | *.xref 132 | 133 | # hyperref 134 | *.brf 135 | 136 | # knitr 137 | *-concordance.tex 138 | # TODO Comment the next line if you want to keep your tikz graphics files 139 | *.tikz 140 | *-tikzDictionary 141 | 142 | # listings 143 | *.lol 144 | 145 | # luatexja-ruby 146 | *.ltjruby 147 | 148 | # makeidx 149 | *.idx 150 | *.ilg 151 | *.ind 152 | 153 | # minitoc 154 | *.maf 155 | *.mlf 156 | *.mlt 157 | *.mtc[0-9]* 158 | *.slf[0-9]* 159 | *.slt[0-9]* 160 | *.stc[0-9]* 161 | 162 | # minted 163 | _minted* 164 | *.pyg 165 | 166 | # morewrites 167 | *.mw 168 | 169 | # nomencl 170 | *.nlg 171 | *.nlo 172 | *.nls 173 | 174 | # pax 175 | *.pax 176 | 177 | # pdfpcnotes 178 | *.pdfpc 179 | 180 | # sagetex 181 | *.sagetex.sage 182 | *.sagetex.py 183 | *.sagetex.scmd 184 | 185 | # scrwfile 186 | *.wrt 187 | 188 | # sympy 189 | *.sout 190 | *.sympy 191 | sympy-plots-for-*.tex/ 192 | 193 | # pdfcomment 194 | *.upa 195 | *.upb 196 | 197 | # pythontex 198 | *.pytxcode 199 | pythontex-files-*/ 200 | 201 | # tcolorbox 202 | *.listing 203 | 204 | # thmtools 205 | *.loe 206 | 207 | # TikZ & PGF 208 | *.dpth 209 | *.md5 210 | *.auxlock 211 | 212 | # todonotes 213 | *.tdo 214 | 215 | # vhistory 216 | *.hst 217 | *.ver 218 | 219 | # easy-todo 220 | *.lod 221 | 222 | # xcolor 223 | *.xcp 224 | 225 | # xmpincl 226 | *.xmpi 227 | 228 | # xindy 229 | *.xdy 230 | 231 | # xypic precompiled matrices and outlines 232 | *.xyc 233 | *.xyd 234 | 235 | # endfloat 236 | *.ttt 237 | *.fff 238 | 239 | # Latexian 240 | TSWLatexianTemp* 241 | 242 | ## Editors: 243 | # WinEdt 244 | *.bak 245 | *.sav 246 | 247 | # Texpad 248 | .texpadtmp 249 | 250 | # LyX 251 | *.lyx~ 252 | 253 | # Kile 254 | *.backup 255 | 256 | # gummi 257 | .*.swp 258 | 259 | # KBibTeX 260 | *~[0-9]* 261 | 262 | # TeXnicCenter 263 | *.tps 264 | 265 | # auto folder when using emacs and auctex 266 | ./auto/* 267 | *.el 268 | 269 | # expex forward references with \gathertags 270 | *-tags.tex 271 | 272 | # standalone packages 273 | *.sta 274 | 275 | # Makeindex log files 276 | *.lpz 277 | 278 | # Mac stuff 279 | *.DS_Store 280 | 281 | # python stuff 282 | .ipynb_checkpoints/ 283 | *.pyc 284 | 285 | # subfolders 286 | explorations/ 287 | 288 | # springer user guide 289 | usrguid3.pdf 290 | -------------------------------------------------------------------------------- /2d_example.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "%matplotlib notebook\n", 10 | "%load_ext autoreload\n", 11 | "%autoreload 2" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "import numpy as np\n", 21 | "import matplotlib.pyplot as plt\n", 22 | "from copy import deepcopy\n", 23 | "from spp.convex_sets import Singleton, Polyhedron, Ellipsoid\n", 24 | "from spp.convex_functions import TwoNorm, SquaredTwoNorm\n", 25 | "from spp.graph import GraphOfConvexSets\n", 26 | "from spp.shortest_path import ShortestPathProblem\n", 27 | "from spp.shortest_path_mccormick import ShortestPathProblem as ShortestPathProblemMC" 28 | ] 29 | }, 30 | { 31 | "cell_type": "code", 32 | "execution_count": null, 33 | "metadata": { 34 | "scrolled": false 35 | }, 36 | "outputs": [], 37 | "source": [ 38 | "# convex sets\n", 39 | "singletons = (\n", 40 | " Singleton((0, 0)),\n", 41 | " Singleton((9, 0)),\n", 42 | ")\n", 43 | "polyhedra = (\n", 44 | " Polyhedron.from_vertices(([1, 0], [1, 2], [3, 1], [3, 0])),\n", 45 | " Polyhedron.from_vertices(([4, 2], [3, 3], [2, 2], [2, 3])),\n", 46 | " Polyhedron.from_vertices(([2, -2], [1, -3], [2, -4], [4, -4], [4, -3])),\n", 47 | " Polyhedron.from_vertices(([5, -4], [7, -4], [6, -3])),\n", 48 | " Polyhedron.from_vertices(([7, -2], [8, -2], [9, -3], [8, -4])),\n", 49 | ")\n", 50 | "ellipsoids = (\n", 51 | " Ellipsoid((4, -1), ([1, 0], [0, 1])),\n", 52 | " Ellipsoid((7, 2), ([.25, 0], [0, 1])),\n", 53 | ")\n", 54 | "sets = singletons + polyhedra + ellipsoids\n", 55 | "\n", 56 | "# label for the vertices\n", 57 | "vertices = ['s', 't']\n", 58 | "vertices += [f'p{i}' for i in range(len(polyhedra))]\n", 59 | "vertices += [f'e{i}' for i in range(len(ellipsoids))]\n", 60 | "\n", 61 | "# add convex sets to the graph\n", 62 | "G = GraphOfConvexSets()\n", 63 | "G.add_sets(sets, vertices)\n", 64 | "G.set_source('s')\n", 65 | "G.set_target('t')\n", 66 | "\n", 67 | "# edges\n", 68 | "H = np.hstack((np.eye(2), -np.eye(2)))\n", 69 | "l = TwoNorm(H)\n", 70 | "edges = {\n", 71 | " 's': ('p0', 'p1', 'p2'),\n", 72 | " 'p0': ('e1',),\n", 73 | " 'p1': ('p2', 'e0', 'e1'),\n", 74 | " 'p2': ('p1', 'p3', 'e0'),\n", 75 | " 'p3': ('t', 'p2', 'p4', 'e1'),\n", 76 | " 'p4': ('t', 'e0'),\n", 77 | " 'e0': ('p3', 'p4', 'e1'),\n", 78 | " 'e1': ('t', 'p4', 'e0')\n", 79 | "}\n", 80 | "for u, vs in edges.items():\n", 81 | " for v in vs:\n", 82 | " G.add_edge(u, v, l)\n", 83 | " \n", 84 | "# draw convex sets and edges\n", 85 | "plt.figure()\n", 86 | "G.draw_sets()\n", 87 | "G.draw_edges()\n", 88 | "G.label_sets()" 89 | ] 90 | }, 91 | { 92 | "cell_type": "code", 93 | "execution_count": null, 94 | "metadata": {}, 95 | "outputs": [], 96 | "source": [ 97 | "# G.graphviz()" 98 | ] 99 | }, 100 | { 101 | "cell_type": "code", 102 | "execution_count": null, 103 | "metadata": {}, 104 | "outputs": [], 105 | "source": [ 106 | "spp = ShortestPathProblem(G, relaxation=0)\n", 107 | "sol = spp.solve()\n", 108 | "\n", 109 | "print('Cost:', sol.cost)\n", 110 | "print('\\nFlows:')\n", 111 | "for k, edge in enumerate(G.edges):\n", 112 | " flow = round(abs(sol.primal.phi[k]), 4)\n", 113 | " print(edge, flow)" 114 | ] 115 | }, 116 | { 117 | "cell_type": "code", 118 | "execution_count": null, 119 | "metadata": {}, 120 | "outputs": [], 121 | "source": [ 122 | "# edge lenghts\n", 123 | "l2 = SquaredTwoNorm(H)\n", 124 | "G2 = deepcopy(G)\n", 125 | "for e in G2.edges:\n", 126 | " G2.set_edge_length(e, l2)\n", 127 | "\n", 128 | "spp2 = ShortestPathProblem(G2, relaxation=0)\n", 129 | "sol2 = spp2.solve()\n", 130 | "\n", 131 | "print('Cost:', sol2.cost)\n", 132 | "print('\\nFlows:')\n", 133 | "for k, edge in enumerate(G2.edges):\n", 134 | " flow = round(abs(sol2.primal.phi[k]), 4)\n", 135 | " print(edge, flow)" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": null, 141 | "metadata": {}, 142 | "outputs": [], 143 | "source": [ 144 | "plt.figure(figsize=(5,5))\n", 145 | "G.draw_sets()\n", 146 | "G.draw_edges()\n", 147 | "\n", 148 | "offset = np.array([.25, 0])\n", 149 | "plt.text(*(G.source_set.center - offset), r'$s$', ha='center', va='bottom')\n", 150 | "plt.text(*(G.target_set.center + offset), r'$t$', ha='center', va='bottom')\n", 151 | "\n", 152 | "plt.plot([np.nan] * 2, c='orangered', linestyle='-', label='Euclidean distance', linewidth=2)\n", 153 | "plt.plot([np.nan] * 2, c='dodgerblue', linestyle='-', label='Euclidean distance squared', linewidth=2)\n", 154 | "G.draw_path(sol.primal.phi, sol.primal.x, color='orangered', linestyle='-', linewidth=2)\n", 155 | "G.draw_path(sol2.primal.phi, sol2.primal.x, color='dodgerblue', linestyle='-', linewidth=2)\n", 156 | "\n", 157 | "plt.xticks(range(10))\n", 158 | "plt.legend(loc='lower center', bbox_to_anchor=(0.5, 1.0))\n", 159 | "plt.grid()\n", 160 | "# plt.savefig('2d_setup.pdf', bbox_inches='tight')" 161 | ] 162 | }, 163 | { 164 | "cell_type": "code", 165 | "execution_count": null, 166 | "metadata": {}, 167 | "outputs": [], 168 | "source": [ 169 | "scales = np.logspace(-2, 1, 100)\n", 170 | "c_micp = []\n", 171 | "c_relaxation = []\n", 172 | "c_mc = []\n", 173 | "c_micp2 = []\n", 174 | "c_relaxation2 = []\n", 175 | "c_mc2 = []\n", 176 | "for s in scales:\n", 177 | " \n", 178 | " G_scaled = deepcopy(G)\n", 179 | " G_scaled.scale(s)\n", 180 | " spp = ShortestPathProblem(G_scaled, relaxation=0)\n", 181 | " c_micp.append(spp.solve().cost)\n", 182 | " spp = ShortestPathProblem(G_scaled, relaxation=1)\n", 183 | " c_relaxation.append(spp.solve().cost)\n", 184 | " spp = ShortestPathProblemMC(G_scaled, relaxation=1)\n", 185 | " c_mc.append(spp.solve().cost)\n", 186 | " \n", 187 | " G2_scaled = deepcopy(G2)\n", 188 | " G2_scaled.scale(s)\n", 189 | " spp = ShortestPathProblem(G2_scaled, relaxation=0)\n", 190 | " c_micp2.append(spp.solve().cost)\n", 191 | " spp = ShortestPathProblem(G2_scaled, relaxation=1)\n", 192 | " c_relaxation2.append(spp.solve().cost)\n", 193 | " spp = ShortestPathProblemMC(G2_scaled, relaxation=1)\n", 194 | " c_mc2.append(spp.solve().cost)" 195 | ] 196 | }, 197 | { 198 | "cell_type": "code", 199 | "execution_count": null, 200 | "metadata": {}, 201 | "outputs": [], 202 | "source": [ 203 | "plt.figure(figsize=(3.5, 4))\n", 204 | "plt.subplots_adjust(hspace=.25)\n", 205 | "\n", 206 | "plt.subplot(2, 1, 1)\n", 207 | "plt.plot(scales, c_micp, label='SPP in GCS', linestyle='-', c='orangered', linewidth=2)\n", 208 | "plt.plot(scales, c_relaxation, label='Our relaxation', linestyle='--', c='k', linewidth=2)\n", 209 | "plt.plot(scales, c_mc, label='McCormick', linestyle=':', c='k', linewidth=2)\n", 210 | "plt.ylabel('Cost')\n", 211 | "plt.xlim([scales[0], scales[-1]])\n", 212 | "plt.xscale('log')\n", 213 | "plt.grid(1)\n", 214 | "plt.gca().set_xticklabels([])\n", 215 | "# plt.legend(loc='lower center', bbox_to_anchor=(0.5, 1.0))\n", 216 | "plt.legend(loc=0)\n", 217 | "plt.gca().locator_params(nbins=8, axis='y')\n", 218 | "plt.title('Euclidean distance')\n", 219 | "\n", 220 | "plt.subplot(2, 1, 2)\n", 221 | "plt.plot(scales, c_micp2, label='SPP in GCS', linestyle='-', c='dodgerblue', linewidth=2)\n", 222 | "plt.plot(scales, c_relaxation2, label='Our relaxation', linestyle='--', c='k', linewidth=2)\n", 223 | "plt.plot(scales, c_mc2, label='McCormick', linestyle=':', c='k', linewidth=2)\n", 224 | "plt.ylabel('Cost')\n", 225 | "plt.xlim([scales[0], scales[-1]])\n", 226 | "plt.xscale('log')\n", 227 | "plt.grid(1)\n", 228 | "plt.legend(loc=0)\n", 229 | "plt.xlabel(r'Set size $\\sigma$')\n", 230 | "plt.gca().locator_params(nbins=8, axis='y')\n", 231 | "plt.title('Euclidean distance squared')\n", 232 | "\n", 233 | "plt.savefig('2d_results.pdf', bbox_inches='tight')" 234 | ] 235 | }, 236 | { 237 | "cell_type": "code", 238 | "execution_count": null, 239 | "metadata": {}, 240 | "outputs": [], 241 | "source": [] 242 | } 243 | ], 244 | "metadata": { 245 | "kernelspec": { 246 | "display_name": "Python 3 (ipykernel)", 247 | "language": "python", 248 | "name": "python3" 249 | }, 250 | "language_info": { 251 | "codemirror_mode": { 252 | "name": "ipython", 253 | "version": 3 254 | }, 255 | "file_extension": ".py", 256 | "mimetype": "text/x-python", 257 | "name": "python", 258 | "nbconvert_exporter": "python", 259 | "pygments_lexer": "ipython3", 260 | "version": "3.11.2" 261 | } 262 | }, 263 | "nbformat": 4, 264 | "nbformat_minor": 4 265 | } 266 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software, please cite it as below." 3 | authors: 4 | - family-names: "Marcucci" 5 | given-names: "Tobia" 6 | orcid: "https://orcid.org/0000-0001-8249-0434" 7 | title: "Shortest paths in graphs of convex sets: supporting software" 8 | version: 1.0 9 | date-released: 2021-28-12 10 | url: "https://github.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets" 11 | 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # shortest-paths-in-graphs-of-convex-sets 2 | 3 | This repository contains the python code necessary to reproduce the numerical results in the paper [**"Shortest Paths in Graphs of Convex Sets" by Tobia Marcucci, Jack Umenberger, Pablo A. Parrilo, and Russ Tedrake**](https://arxiv.org/abs/2101.11565). 4 | Optimization problems are solved using [**Drake**](https://drake.mit.edu) as an interface to the solver [**Mosek**](https://www.mosek.com) (free for academic use). 5 | A mature high-performance implementation of the shortest-path algorithm from the paper is under development in [**Drake's main project**](https://github.com/RobotLocomotion/drake). 6 | -------------------------------------------------------------------------------- /alternative_formulations/cost_2norm_ch.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/alternative_formulations/cost_2norm_ch.npy -------------------------------------------------------------------------------- /alternative_formulations/cost_2norm_edgewise.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/alternative_formulations/cost_2norm_edgewise.npy -------------------------------------------------------------------------------- /alternative_formulations/cost_2norm_micp.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/alternative_formulations/cost_2norm_micp.npy -------------------------------------------------------------------------------- /alternative_formulations/cost_2norm_proposed.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/alternative_formulations/cost_2norm_proposed.npy -------------------------------------------------------------------------------- /alternative_formulations/cost_2norm_squared_ch.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/alternative_formulations/cost_2norm_squared_ch.npy -------------------------------------------------------------------------------- /alternative_formulations/cost_2norm_squared_edgewise.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/alternative_formulations/cost_2norm_squared_edgewise.npy -------------------------------------------------------------------------------- /alternative_formulations/cost_2norm_squared_micp.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/alternative_formulations/cost_2norm_squared_micp.npy -------------------------------------------------------------------------------- /alternative_formulations/cost_2norm_squared_proposed.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/alternative_formulations/cost_2norm_squared_proposed.npy -------------------------------------------------------------------------------- /alternative_formulations/plot.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "10f13f6c", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "%matplotlib notebook\n", 11 | "%load_ext autoreload\n", 12 | "%autoreload 2" 13 | ] 14 | }, 15 | { 16 | "cell_type": "code", 17 | "execution_count": null, 18 | "id": "17b6f059", 19 | "metadata": {}, 20 | "outputs": [], 21 | "source": [ 22 | "import numpy as np\n", 23 | "import matplotlib.pyplot as plt" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "id": "5e85dc55", 30 | "metadata": {}, 31 | "outputs": [], 32 | "source": [ 33 | "scales = np.load('scales.npy')\n", 34 | "micp = np.load('cost_2norm_micp.npy')\n", 35 | "micp_squared = np.load('cost_2norm_squared_micp.npy')\n", 36 | "edgewise = np.load('cost_2norm_edgewise.npy')\n", 37 | "edgewise_squared = np.load('cost_2norm_squared_edgewise.npy')\n", 38 | "ch = np.load('cost_2norm_ch.npy')\n", 39 | "ch_squared = np.load('cost_2norm_squared_ch.npy')\n", 40 | "proposed = np.load('cost_2norm_proposed.npy')\n", 41 | "proposed_squared = np.load('cost_2norm_squared_proposed.npy')" 42 | ] 43 | }, 44 | { 45 | "cell_type": "code", 46 | "execution_count": null, 47 | "id": "d1dcb78e", 48 | "metadata": {}, 49 | "outputs": [], 50 | "source": [ 51 | "def micp_vs_relaxation(micp, proposed, edgewise, ch):\n", 52 | " plt.plot(scales, micp, label='MICP', linestyle='-', linewidth=2)\n", 53 | " plt.plot(scales, proposed, label='Proposed formulation', linestyle='--', linewidth=2)\n", 54 | " plt.plot(scales, edgewise, label='Edge-by-edge formulation', linestyle='-.', linewidth=2)\n", 55 | " plt.plot(scales, ch, label='Convex-hull formulation', linestyle=':', linewidth=2)\n", 56 | " plt.xlabel(r'Scale factor $r$')\n", 57 | " plt.ylabel('Cost')\n", 58 | " plt.xlim([scales[0], scales[-1]])\n", 59 | " plt.xscale('log')\n", 60 | " plt.grid(1)\n", 61 | " plt.legend(loc=3)" 62 | ] 63 | }, 64 | { 65 | "cell_type": "code", 66 | "execution_count": null, 67 | "id": "59f20256", 68 | "metadata": {}, 69 | "outputs": [], 70 | "source": [ 71 | "plt.figure(figsize=(5, 3))\n", 72 | "micp_vs_relaxation(micp, proposed, edgewise, ch)" 73 | ] 74 | }, 75 | { 76 | "cell_type": "code", 77 | "execution_count": null, 78 | "id": "ca4c23bf", 79 | "metadata": {}, 80 | "outputs": [], 81 | "source": [ 82 | "plt.figure(figsize=(5, 3))\n", 83 | "micp_vs_relaxation(micp_squared, proposed_squared, edgewise_squared, ch_squared)" 84 | ] 85 | } 86 | ], 87 | "metadata": { 88 | "kernelspec": { 89 | "display_name": "Python 3", 90 | "language": "python", 91 | "name": "python3" 92 | }, 93 | "language_info": { 94 | "codemirror_mode": { 95 | "name": "ipython", 96 | "version": 3 97 | }, 98 | "file_extension": ".py", 99 | "mimetype": "text/x-python", 100 | "name": "python", 101 | "nbconvert_exporter": "python", 102 | "pygments_lexer": "ipython3", 103 | "version": "3.9.4" 104 | } 105 | }, 106 | "nbformat": 4, 107 | "nbformat_minor": 5 108 | } 109 | -------------------------------------------------------------------------------- /alternative_formulations/scales.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/alternative_formulations/scales.npy -------------------------------------------------------------------------------- /bilinear_programs.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "90b8162a", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import numpy as np\n", 11 | "from spp.convex_sets import Polyhedron\n", 12 | "from pydrake.all import MathematicalProgram, MosekSolver" 13 | ] 14 | }, 15 | { 16 | "cell_type": "markdown", 17 | "id": "376954c9", 18 | "metadata": {}, 19 | "source": [ 20 | "# Examples from \"Global optimization: Deterministic approaches\"\n", 21 | "\n", 22 | "In this section we test the convex relaxation from the paper on two simple bilinear-optimization problems taken from Section IX.1 of Horst and Reiner \"Global optimization: Deterministic approaches.\"\n", 23 | "These are all the examples given in the book in which the convex sets are bounded.\n", 24 | "In both cases, our convex relaxation is tight." 25 | ] 26 | }, 27 | { 28 | "cell_type": "code", 29 | "execution_count": null, 30 | "id": "981ffae7", 31 | "metadata": {}, 32 | "outputs": [], 33 | "source": [ 34 | "def bilinear_program(X, Phi, p, q, r):\n", 35 | "\n", 36 | " prog = MathematicalProgram()\n", 37 | "\n", 38 | " phi = prog.NewContinuousVariables(Phi.dimension)\n", 39 | " x = prog.NewContinuousVariables(X.dimension)\n", 40 | " omega = prog.NewContinuousVariables(Phi.dimension * X.dimension)\n", 41 | "\n", 42 | " Phi.add_membership_constraint(prog, phi)\n", 43 | " X.add_membership_constraint(prog, x)\n", 44 | "\n", 45 | " for i in range(Phi.C.shape[0]):\n", 46 | " scale = Phi.d[i] - Phi.C[i].dot(phi)\n", 47 | " vector = Phi.d[i] * x - np.kron(Phi.C[i], np.eye(X.dimension)).dot(omega)\n", 48 | " X.add_perspective_constraint(prog, scale, vector)\n", 49 | "\n", 50 | " obj = p.dot(phi) + q.dot(x) + r.dot(omega)\n", 51 | " prog.AddLinearCost(- obj)\n", 52 | "\n", 53 | " solver = MosekSolver()\n", 54 | " result = solver.Solve(prog)\n", 55 | " \n", 56 | " obj = - result.get_optimal_cost()\n", 57 | " phi = result.GetSolution(phi)\n", 58 | " x = result.GetSolution(x)\n", 59 | " omega = result.GetSolution(omega)\n", 60 | " \n", 61 | " return obj, phi, x, omega" 62 | ] 63 | }, 64 | { 65 | "cell_type": "markdown", 66 | "id": "8fce68cd", 67 | "metadata": {}, 68 | "source": [ 69 | "- Example from Konno \"A cutting plane algorithm for solving bilinear programs\" (Figure 4.1). See also Example IX.1 from Horst and Reiner \"Global optimization: Deterministic approaches.\"\n", 70 | "- Our convex relaxation is tight: optimal value is 13." 71 | ] 72 | }, 73 | { 74 | "cell_type": "code", 75 | "execution_count": null, 76 | "id": "dd112ca2", 77 | "metadata": {}, 78 | "outputs": [], 79 | "source": [ 80 | "C1 = np.array([[1, 4], [4, 1], [3, 4], [-1, 0], [0, -1]])\n", 81 | "d1 = np.array([8, 12, 12, 0, 0])\n", 82 | "C2 = np.array([[2, 1], [1, 2], [1, 1], [-1, 0], [0, -1]])\n", 83 | "d2 = np.array([8, 8, 5, 0, 0])\n", 84 | "Phi = Polyhedron(ineq_matrices=(C1, d1))\n", 85 | "X = Polyhedron(ineq_matrices=(C2, d2))\n", 86 | "\n", 87 | "p = np.array([-1, 1])\n", 88 | "q = np.array([1, 0])\n", 89 | "r = np.array([1, -1, -1, 1])\n", 90 | "\n", 91 | "bilinear_program(X, Phi, p, q, r)" 92 | ] 93 | }, 94 | { 95 | "cell_type": "markdown", 96 | "id": "b6d9c5ab", 97 | "metadata": {}, 98 | "source": [ 99 | "- Example from Gallo and Ulkucu \"Bilinear programming: an exact algorithm\" (Appendix A). See also Example IX.2 from Horst and Reiner \"Global optimization: Deterministic approaches.\"\n", 100 | "- Our convex relaxation is tight: optimal value is 18." 101 | ] 102 | }, 103 | { 104 | "cell_type": "code", 105 | "execution_count": null, 106 | "id": "6ffff6a8", 107 | "metadata": {}, 108 | "outputs": [], 109 | "source": [ 110 | "C1 = np.array([\n", 111 | " [1, 1],\n", 112 | " [2, 1],\n", 113 | " [3, -1],\n", 114 | " [1, -2],\n", 115 | " [-1, 0],\n", 116 | " [0, -1]\n", 117 | "])\n", 118 | "d1 = np.array([5, 7, 6, 1, 0, 0])\n", 119 | "C2 = np.array([\n", 120 | " [1, 2],\n", 121 | " [3, 1],\n", 122 | " [2, 0],\n", 123 | " [0, 1],\n", 124 | " [-1, 0],\n", 125 | " [0, -1]\n", 126 | "])\n", 127 | "d2 = np.array([8, 14, 9, 3, 0, 0])\n", 128 | "Phi = Polyhedron(ineq_matrices=(C1, d1))\n", 129 | "X = Polyhedron(ineq_matrices=(C2, d2))\n", 130 | "\n", 131 | "p = np.array([2, 0])\n", 132 | "q = np.array([0, 1])\n", 133 | "r = np.array([1, -1, -1, 1])\n", 134 | "\n", 135 | "bilinear_program(X, Phi, p, q, r)" 136 | ] 137 | }, 138 | { 139 | "cell_type": "markdown", 140 | "id": "30be5547", 141 | "metadata": {}, 142 | "source": [ 143 | "# Adversarial instance\n", 144 | "\n", 145 | "The following bilinear pogram comes from CHSH inequalities in quantum.\n", 146 | "It is a kown hard bilinear program. Its optimal cost is 2, achieved for $\\varphi=x=(1,1)$.\n", 147 | "Our convex relaxation has an optimal cost of 4." 148 | ] 149 | }, 150 | { 151 | "cell_type": "code", 152 | "execution_count": null, 153 | "id": "ed470255", 154 | "metadata": {}, 155 | "outputs": [], 156 | "source": [ 157 | "Phi = Polyhedron.from_bounds([-1, -1], [1, 1])\n", 158 | "X = Polyhedron.from_bounds([-1, -1], [1, 1])\n", 159 | "\n", 160 | "p = np.array([0, 0])\n", 161 | "q = p\n", 162 | "r = np.array([1, 1, 1, -1])\n", 163 | "\n", 164 | "bilinear_program(X, Phi, p, q, r)" 165 | ] 166 | }, 167 | { 168 | "cell_type": "markdown", 169 | "id": "63ab094a", 170 | "metadata": {}, 171 | "source": [ 172 | "The SDP relaxation of the CHSH maximization has a cost of $2 \\sqrt 2$ and is tighter than the proposed relaxation." 173 | ] 174 | }, 175 | { 176 | "cell_type": "code", 177 | "execution_count": null, 178 | "id": "d69d3d96", 179 | "metadata": {}, 180 | "outputs": [], 181 | "source": [ 182 | "from pydrake.all import eq\n", 183 | "def sdp_relaxation(X, Phi, p, q, r):\n", 184 | " \n", 185 | " prog = MathematicalProgram()\n", 186 | "\n", 187 | " phi = prog.NewContinuousVariables(Phi.dimension, name='phi')\n", 188 | " x = prog.NewContinuousVariables(X.dimension, name='x')\n", 189 | " Omega = prog.NewContinuousVariables(Phi.dimension, X.dimension, name='w')\n", 190 | " \n", 191 | " Phi.add_membership_constraint(prog, phi)\n", 192 | " X.add_membership_constraint(prog, x)\n", 193 | " \n", 194 | " M2 = prog.NewContinuousVariables(Phi.dimension, Phi.dimension)\n", 195 | " M3 = prog.NewContinuousVariables(X.dimension, X.dimension)\n", 196 | " M = np.block([\n", 197 | " [np.ones(1), phi, x],\n", 198 | " [phi.reshape((Phi.dimension, 1)), M2, Omega],\n", 199 | " [x.reshape((X.dimension, 1)), Omega.T, M3]\n", 200 | " ])\n", 201 | " prog.AddPositiveSemidefiniteConstraint(M)\n", 202 | " \n", 203 | " prog.AddLinearConstraint(eq(np.diag(M2), 1))\n", 204 | " prog.AddLinearConstraint(eq(np.diag(M3), 1))\n", 205 | " \n", 206 | " obj = p.dot(phi) + q.dot(x) + r.dot(Omega.flatten())\n", 207 | " prog.AddLinearCost(- obj)\n", 208 | "\n", 209 | " solver = MosekSolver()\n", 210 | " result = solver.Solve(prog)\n", 211 | " \n", 212 | " obj = - result.get_optimal_cost()\n", 213 | " phi = result.GetSolution(phi)\n", 214 | " x = result.GetSolution(x)\n", 215 | " omega = result.GetSolution(Omega).flatten()\n", 216 | " \n", 217 | " return obj, phi, x, omega\n", 218 | "\n", 219 | "sdp_relaxation(X, Phi, p, q, r)" 220 | ] 221 | } 222 | ], 223 | "metadata": { 224 | "kernelspec": { 225 | "display_name": "Python 3", 226 | "language": "python", 227 | "name": "python3" 228 | }, 229 | "language_info": { 230 | "codemirror_mode": { 231 | "name": "ipython", 232 | "version": 3 233 | }, 234 | "file_extension": ".py", 235 | "mimetype": "text/x-python", 236 | "name": "python", 237 | "nbconvert_exporter": "python", 238 | "pygments_lexer": "ipython3", 239 | "version": "3.9.6" 240 | } 241 | }, 242 | "nbformat": 4, 243 | "nbformat_minor": 5 244 | } 245 | -------------------------------------------------------------------------------- /cyclic_1d.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 32, 6 | "metadata": {}, 7 | "outputs": [ 8 | { 9 | "name": "stdout", 10 | "output_type": "stream", 11 | "text": [ 12 | "The autoreload extension is already loaded. To reload it, use:\n", 13 | " %reload_ext autoreload\n" 14 | ] 15 | } 16 | ], 17 | "source": [ 18 | "%matplotlib notebook\n", 19 | "%load_ext autoreload\n", 20 | "%autoreload 2" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": 33, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "import numpy as np\n", 30 | "import matplotlib.pyplot as plt\n", 31 | "from copy import deepcopy\n", 32 | "from spp.convex_sets import Singleton, Polyhedron\n", 33 | "from spp.convex_functions import SquaredTwoNorm\n", 34 | "from spp.graph import GraphOfConvexSets\n", 35 | "from spp.shortest_path import ShortestPathProblem" 36 | ] 37 | }, 38 | { 39 | "cell_type": "code", 40 | "execution_count": 44, 41 | "metadata": { 42 | "scrolled": false 43 | }, 44 | "outputs": [], 45 | "source": [ 46 | "# convex sets\n", 47 | "sets = (\n", 48 | " Singleton((-1)),\n", 49 | " Singleton((1)),\n", 50 | " Polyhedron.from_bounds([-1], [1]),\n", 51 | " Singleton((0)),\n", 52 | ")\n", 53 | "\n", 54 | "# label for the vertices\n", 55 | "vertices = ['s', 't', '1', '2']\n", 56 | "\n", 57 | "# add convex sets to the graph\n", 58 | "G = GraphOfConvexSets()\n", 59 | "G.add_sets(sets, vertices)\n", 60 | "G.set_source('s')\n", 61 | "G.set_target('t')\n", 62 | "\n", 63 | "# edges\n", 64 | "H = np.array([[ 1., -1.]])\n", 65 | "l = SquaredTwoNorm(H)\n", 66 | "edges = {\n", 67 | " 's': ('1',),\n", 68 | " '1': ('2', 't'),\n", 69 | " '2': ('1')\n", 70 | "}\n", 71 | "for u, vs in edges.items():\n", 72 | " for v in vs:\n", 73 | " G.add_edge(u, v, l)" 74 | ] 75 | }, 76 | { 77 | "cell_type": "code", 78 | "execution_count": 47, 79 | "metadata": {}, 80 | "outputs": [ 81 | { 82 | "name": "stdout", 83 | "output_type": "stream", 84 | "text": [ 85 | "Cost: 1.9999999989168118\n", 86 | "\n", 87 | "Flows:\n", 88 | "('s', '1') 1.0\n", 89 | "('1', '2') 0.0\n", 90 | "('1', 't') 1.0\n", 91 | "('2', '1') 0.0\n" 92 | ] 93 | } 94 | ], 95 | "source": [ 96 | "spp = ShortestPathProblem(G, relaxation=0, cyclic=True)\n", 97 | "sol = spp.solve()\n", 98 | "\n", 99 | "print('Cost:', sol.cost)\n", 100 | "print('\\nFlows:')\n", 101 | "for k, edge in enumerate(G.edges):\n", 102 | " flow = round(abs(sol.primal.phi[k]), 4)\n", 103 | " print(edge, flow)" 104 | ] 105 | }, 106 | { 107 | "cell_type": "code", 108 | "execution_count": null, 109 | "metadata": {}, 110 | "outputs": [], 111 | "source": [] 112 | } 113 | ], 114 | "metadata": { 115 | "kernelspec": { 116 | "display_name": "Python 3 (ipykernel)", 117 | "language": "python", 118 | "name": "python3" 119 | }, 120 | "language_info": { 121 | "codemirror_mode": { 122 | "name": "ipython", 123 | "version": 3 124 | }, 125 | "file_extension": ".py", 126 | "mimetype": "text/x-python", 127 | "name": "python", 128 | "nbconvert_exporter": "python", 129 | "pygments_lexer": "ipython3", 130 | "version": "3.11.2" 131 | } 132 | }, 133 | "nbformat": 4, 134 | "nbformat_minor": 4 135 | } 136 | -------------------------------------------------------------------------------- /hpp_reduction_proof.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "%matplotlib notebook\n", 10 | "%load_ext autoreload\n", 11 | "%autoreload 2" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "import numpy as np\n", 21 | "import matplotlib.pyplot as plt\n", 22 | "from copy import deepcopy\n", 23 | "from spp.convex_sets import Singleton, Polyhedron\n", 24 | "from spp.convex_functions import SquaredTwoNorm\n", 25 | "from spp.graph import GraphOfConvexSets\n", 26 | "from spp.shortest_path import ShortestPathProblem" 27 | ] 28 | }, 29 | { 30 | "cell_type": "code", 31 | "execution_count": null, 32 | "metadata": { 33 | "scrolled": false 34 | }, 35 | "outputs": [], 36 | "source": [ 37 | "scale = 4\n", 38 | "\n", 39 | "v1 = np.array([[1, 1], [2, 1], [2, 2], [1, 2]]) - np.array([0, .01])\n", 40 | "v2 = np.array([[1, -1], [2, -1], [2, -2], [1, -2]])\n", 41 | "v3 = np.array([[3, 1], [4, 1], [4, 2], [3, 2]]) - np.array([0, .01])\n", 42 | "v4 = np.array([[3, -1], [4, -1], [4, -2], [3, -2]])\n", 43 | "\n", 44 | "# convex sets\n", 45 | "sets = (\n", 46 | " Singleton((0, 0)),\n", 47 | " Singleton((5, 0)),\n", 48 | " Polyhedron.from_vertices(v1),\n", 49 | " Polyhedron.from_vertices(v2),\n", 50 | " Polyhedron.from_vertices(v3),\n", 51 | " Polyhedron.from_vertices(v4),\n", 52 | ")\n", 53 | "\n", 54 | "# label for the vertices\n", 55 | "vertices = ['s', 't', '1', '2', '3', '4']\n", 56 | "\n", 57 | "# add convex sets to the graph\n", 58 | "G = GraphOfConvexSets()\n", 59 | "G.add_sets(sets, vertices)\n", 60 | "G.set_source('s')\n", 61 | "G.set_target('t')\n", 62 | "\n", 63 | "# edges\n", 64 | "H = np.hstack((np.eye(2), -np.eye(2)))\n", 65 | "l = SquaredTwoNorm(H)\n", 66 | "edges = {\n", 67 | " 's': ('1', '2'),\n", 68 | " '1': ('3',),\n", 69 | " '2': ('1', '4'),\n", 70 | " '3': ('4', 't'),\n", 71 | " '4': ('t',),\n", 72 | "}\n", 73 | "for u, vs in edges.items():\n", 74 | " for v in vs:\n", 75 | " G.add_edge(u, v, l)\n", 76 | "\n", 77 | "# scale sets\n", 78 | "G_scaled = deepcopy(G)\n", 79 | "G_scaled.scale(.5 * scale)\n", 80 | " \n", 81 | "# solve spp\n", 82 | "spp = ShortestPathProblem(G_scaled, relaxation=0)\n", 83 | "sol = spp.solve()\n", 84 | " \n", 85 | "# draw convex sets and edges\n", 86 | "plt.figure(figsize=(3,3))\n", 87 | "plt.axis('off')\n", 88 | "plt.xlim([-.2, 5.2])\n", 89 | "plt.ylim([-2.6, 2.6])\n", 90 | "\n", 91 | "G_scaled.draw_sets()\n", 92 | "G_scaled.draw_edges()\n", 93 | "G_scaled.draw_path(sol.primal.phi, sol.primal.x, color='r')\n", 94 | "# plt.savefig(f'reduction_{scale}.pdf', bbox_inches='tight')" 95 | ] 96 | } 97 | ], 98 | "metadata": { 99 | "kernelspec": { 100 | "display_name": "Python 3 (ipykernel)", 101 | "language": "python", 102 | "name": "python3" 103 | }, 104 | "language_info": { 105 | "codemirror_mode": { 106 | "name": "ipython", 107 | "version": 3 108 | }, 109 | "file_extension": ".py", 110 | "mimetype": "text/x-python", 111 | "name": "python", 112 | "nbconvert_exporter": "python", 113 | "pygments_lexer": "ipython3", 114 | "version": "3.11.2" 115 | } 116 | }, 117 | "nbformat": 4, 118 | "nbformat_minor": 4 119 | } 120 | -------------------------------------------------------------------------------- /loose_cycles.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "%matplotlib notebook\n", 10 | "%load_ext autoreload\n", 11 | "%autoreload 2" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "import numpy as np\n", 21 | "import matplotlib.pyplot as plt\n", 22 | "from spp.convex_sets import Singleton, Polyhedron\n", 23 | "from spp.convex_functions import SquaredTwoNorm\n", 24 | "from spp.graph import GraphOfConvexSets\n", 25 | "from spp.shortest_path import ShortestPathProblem" 26 | ] 27 | }, 28 | { 29 | "cell_type": "code", 30 | "execution_count": null, 31 | "metadata": {}, 32 | "outputs": [], 33 | "source": [ 34 | "# vertices\n", 35 | "cube = np.array([[-1, -1], [1, -1], [1, 1], [-1, 1]]) * .7\n", 36 | "sets = [\n", 37 | " Singleton([0, 0]),\n", 38 | " Polyhedron.from_vertices(cube + np.array([2, 2])),\n", 39 | " Polyhedron.from_vertices(cube + np.array([4, 2])),\n", 40 | " Polyhedron.from_vertices(cube + np.array([2, -2])),\n", 41 | " Polyhedron.from_vertices(cube + np.array([4, -2])),\n", 42 | " Singleton([6, 0])\n", 43 | "]\n", 44 | "vertices = ['s', '1', '2', '3', '4', 't']\n", 45 | "\n", 46 | "# add convex sets to the graph\n", 47 | "G = GraphOfConvexSets()\n", 48 | "G.add_sets(sets, vertices)\n", 49 | "G.set_source('s')\n", 50 | "G.set_target('t')\n", 51 | "\n", 52 | "# edges\n", 53 | "H = np.hstack((np.eye(2), -np.eye(2)))\n", 54 | "l = SquaredTwoNorm(H)\n", 55 | "edges = {\n", 56 | " 's': ('1', '3'),\n", 57 | " '1': ('2',),\n", 58 | " '2': ('1', 't'),\n", 59 | " '3': ('4',),\n", 60 | " '4': ('3', 't')\n", 61 | "}\n", 62 | "for u, vs in edges.items():\n", 63 | " for v in vs:\n", 64 | " G.add_edge(u, v, l)" 65 | ] 66 | }, 67 | { 68 | "cell_type": "markdown", 69 | "metadata": {}, 70 | "source": [ 71 | "# Plot solution of MICP" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "metadata": {}, 78 | "outputs": [], 79 | "source": [ 80 | "spp = ShortestPathProblem(G, relaxation=0)\n", 81 | "sol = spp.solve()\n", 82 | "phi = sol.primal.phi\n", 83 | "x = sol.primal.x\n", 84 | "\n", 85 | "print('Cost:', sol.cost)\n", 86 | "print('\\nFlows:')\n", 87 | "for k, edge in enumerate(G.edges):\n", 88 | " flow = round(abs(phi[k]), 4)\n", 89 | " print(edge, flow)" 90 | ] 91 | }, 92 | { 93 | "cell_type": "code", 94 | "execution_count": null, 95 | "metadata": {}, 96 | "outputs": [], 97 | "source": [ 98 | "plt.figure(figsize=(4,4))\n", 99 | "G.draw_sets()\n", 100 | "G.draw_edges()\n", 101 | "\n", 102 | "plt.text(0, -.2, r'$X_s$', ha='center', va='top', c='k')\n", 103 | "plt.text(2, 2.1, r'$X_1$', ha='center', va='bottom', c='k')\n", 104 | "plt.text(4, 2.1, r'$X_2$', ha='center', va='bottom', c='k')\n", 105 | "plt.text(2, -2.1, r'$X_3$', ha='center', va='top', c='k')\n", 106 | "plt.text(4, -2.1, r'$X_4$', ha='center', va='top', c='k')\n", 107 | "plt.text(6, -.2, r'$X_t$', ha='center', va='top', c='k')\n", 108 | "\n", 109 | "kwargs = {'marker': 'o', 'markeredgecolor': 'k', 'markerfacecolor': 'w'}\n", 110 | "G.draw_path(phi, x, color='b', linestyle='--')\n", 111 | "\n", 112 | "plt.grid()\n", 113 | "# plt.savefig('cycle_mip.pdf', bbox_inches='tight')" 114 | ] 115 | }, 116 | { 117 | "cell_type": "markdown", 118 | "metadata": {}, 119 | "source": [ 120 | "# Plot solution of convex relaxation" 121 | ] 122 | }, 123 | { 124 | "cell_type": "code", 125 | "execution_count": null, 126 | "metadata": {}, 127 | "outputs": [], 128 | "source": [ 129 | "spp = ShortestPathProblem(G, relaxation=1)\n", 130 | "sol = spp.solve()\n", 131 | "phi = sol.primal.phi\n", 132 | "x = sol.primal.x\n", 133 | "y = sol.primal.y\n", 134 | "z = sol.primal.z\n", 135 | "\n", 136 | "print('Cost:', sol.cost)\n", 137 | "print('\\nFlows:')\n", 138 | "for k, edge in enumerate(G.edges):\n", 139 | " flow = round(abs(phi[k]), 4)\n", 140 | " print(edge, flow)" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "plt.figure(figsize=(4,4))\n", 150 | "G.draw_sets()\n", 151 | "\n", 152 | "def phi(u, v):\n", 153 | " e = G.edges.index((u, v))\n", 154 | " return sol.primal.phi[e]\n", 155 | "\n", 156 | "def y(u, v):\n", 157 | " e = G.edges.index((u, v))\n", 158 | " return sol.primal.y[e] / phi(u, v)\n", 159 | "\n", 160 | "def z(u, v):\n", 161 | " e = G.edges.index((u, v))\n", 162 | " return sol.primal.z[e] / phi(u, v)\n", 163 | "\n", 164 | "def plot(start, stop):\n", 165 | " plt.plot([start[0], stop[0]], [start[1], stop[1]], 'b--', **kwargs)\n", 166 | " \n", 167 | "for e in G.edges:\n", 168 | " if e[0] not in [3, 4] and e[1] not in [3, 4]:\n", 169 | " plot(y(*e), z(*e))\n", 170 | "\n", 171 | "plt.text(*(y('s','1') + [.3, 0]), r'$\\bar y_{(s,1)}$', ha='left', va='center', c='k')\n", 172 | "plt.text(*(z('s','1') + [0, -.1]), r'$\\bar z_{(s,1)}$', ha='left', va='top', c='k')\n", 173 | "plt.text(*(y('1','2') + [-.09, 0]), r'$\\bar y_{(1,2)}$', ha='right', va='center', c='k')\n", 174 | "plt.text(*(z('1','2') + [.13, 0]), r'$\\bar z_{(1,2)}$', ha='left', va='center', c='k')\n", 175 | "plt.text(*(y('2','1') + [.1, 0]), r'$\\bar y_{(2,1)}$', ha='left', va='center', c='k')\n", 176 | "plt.text(*(z('2','1') + [-.1, 0]), r'$\\bar z_{(2,1)}$', ha='right', va='center', c='k')\n", 177 | "plt.text(*(y('2','t') + [0, -.1]), r'$\\bar y_{(2,t)}$', ha='right', va='top', c='k')\n", 178 | "plt.text(*(z('2','t') + [-.3, 0]), r'$\\bar z_{(2,t)}$', ha='right', va='center', c='k')\n", 179 | "\n", 180 | "def center(u, v):\n", 181 | " return (y(u,v) + z(u,v)) / 2\n", 182 | "plt.text(*(center('s','1') + [0, .05]), r'$0.5$', ha='right', va='bottom', c='r')\n", 183 | "plt.text(*(center('1','2') + [0, .05]), r'$1.0$', ha='center', va='bottom', c='r')\n", 184 | "plt.text(*(center('2','1') + [0, .05]), r'$0.5$', ha='center', va='bottom', c='r')\n", 185 | "plt.text(*(center('2','t') + [.05, 0]), r'$0.5$', ha='left', va='bottom', c='r')\n", 186 | "\n", 187 | "plt.grid()\n", 188 | "# plt.savefig('cycle_relaxation.pdf', bbox_inches='tight')" 189 | ] 190 | } 191 | ], 192 | "metadata": { 193 | "kernelspec": { 194 | "display_name": "Python 3", 195 | "language": "python", 196 | "name": "python3" 197 | }, 198 | "language_info": { 199 | "codemirror_mode": { 200 | "name": "ipython", 201 | "version": 3 202 | }, 203 | "file_extension": ".py", 204 | "mimetype": "text/x-python", 205 | "name": "python", 206 | "nbconvert_exporter": "python", 207 | "pygments_lexer": "ipython3", 208 | "version": "3.9.6" 209 | } 210 | }, 211 | "nbformat": 4, 212 | "nbformat_minor": 4 213 | } 214 | -------------------------------------------------------------------------------- /loose_symmetry.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "%matplotlib notebook\n", 10 | "%load_ext autoreload\n", 11 | "%autoreload 2" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "import numpy as np\n", 21 | "import matplotlib.pyplot as plt\n", 22 | "from spp.convex_sets import Singleton, Polyhedron\n", 23 | "from spp.convex_functions import TwoNorm\n", 24 | "from spp.graph import GraphOfConvexSets\n", 25 | "from spp.shortest_path import ShortestPathProblem" 26 | ] 27 | }, 28 | { 29 | "cell_type": "code", 30 | "execution_count": null, 31 | "metadata": {}, 32 | "outputs": [], 33 | "source": [ 34 | "# vertices\n", 35 | "sets = [\n", 36 | " Singleton([0, 0]),\n", 37 | " Singleton([0, 2]),\n", 38 | " Singleton([0, -2-1e-9]),\n", 39 | " Polyhedron.from_vertices([[2, -2.1], [4, -2.1], [4, 2.1], [2, 2.1]]),\n", 40 | " Singleton([5, 0])\n", 41 | "]\n", 42 | "sets[3]._center = np.array([3, 0])\n", 43 | "vertices = ['s', '1', '2', '3', 't']\n", 44 | "\n", 45 | "# add convex sets to the graph\n", 46 | "G = GraphOfConvexSets()\n", 47 | "G.add_sets(sets, vertices)\n", 48 | "G.set_source('s')\n", 49 | "G.set_target('t')\n", 50 | "\n", 51 | "# edges\n", 52 | "H = np.hstack((np.eye(2), -np.eye(2)))\n", 53 | "l = TwoNorm(H)\n", 54 | "edges = {\n", 55 | " 's': ('1', '2'),\n", 56 | " '1': ('3',),\n", 57 | " '2': ('3',),\n", 58 | " '3': ('t',),\n", 59 | "}\n", 60 | "for u, vs in edges.items():\n", 61 | " for v in vs:\n", 62 | " G.add_edge(u, v, l)" 63 | ] 64 | }, 65 | { 66 | "cell_type": "markdown", 67 | "metadata": {}, 68 | "source": [ 69 | "# Plot solution of MICP" 70 | ] 71 | }, 72 | { 73 | "cell_type": "code", 74 | "execution_count": null, 75 | "metadata": {}, 76 | "outputs": [], 77 | "source": [ 78 | "spp = ShortestPathProblem(G, relaxation=0, cyclic=False)\n", 79 | "sol = spp.solve()\n", 80 | "phi = sol.primal.phi\n", 81 | "x = sol.primal.x\n", 82 | "\n", 83 | "print('Cost:', sol.cost)\n", 84 | "print('\\nFlows:')\n", 85 | "for k, edge in enumerate(G.edges):\n", 86 | " flow = round(abs(phi[k]), 4)\n", 87 | " print(edge, flow)" 88 | ] 89 | }, 90 | { 91 | "cell_type": "code", 92 | "execution_count": null, 93 | "metadata": {}, 94 | "outputs": [], 95 | "source": [ 96 | "plt.figure(figsize=(3.5,3.5))\n", 97 | "G.draw_sets()\n", 98 | "G.draw_edges()\n", 99 | "\n", 100 | "plt.text(0.2, 0, r'$s$', ha='left', va='center')\n", 101 | "plt.text(0.3, 2, r'$1$', ha='left', va='center')\n", 102 | "plt.text(0.3, -2, r'$2$', ha='left', va='center')\n", 103 | "plt.text(3, 0.1, r'$3$', ha='center', va='bottom')\n", 104 | "plt.text(5, 0.1, r'$t$', ha='center', va='bottom')\n", 105 | "\n", 106 | "kwargs = {'marker': 'o', 'markeredgecolor': 'k', 'markerfacecolor': 'w'}\n", 107 | "G.draw_path(phi, x, color='orangered')\n", 108 | "\n", 109 | "plt.grid()\n", 110 | "plt.savefig('symmetry_mip.pdf', bbox_inches='tight')" 111 | ] 112 | }, 113 | { 114 | "cell_type": "markdown", 115 | "metadata": {}, 116 | "source": [ 117 | "# Plot solution of convex relaxation" 118 | ] 119 | }, 120 | { 121 | "cell_type": "code", 122 | "execution_count": null, 123 | "metadata": {}, 124 | "outputs": [], 125 | "source": [ 126 | "spp = ShortestPathProblem(G, relaxation=1)\n", 127 | "sol = spp.solve()\n", 128 | "phi = sol.primal.phi\n", 129 | "x = sol.primal.x\n", 130 | "y = sol.primal.y\n", 131 | "z = sol.primal.z\n", 132 | "\n", 133 | "print('Cost:', sol.cost)\n", 134 | "print('\\nFlows:')\n", 135 | "for k, edge in enumerate(G.edges):\n", 136 | " flow = round(abs(phi[k]), 4)\n", 137 | " print(edge, flow)" 138 | ] 139 | }, 140 | { 141 | "cell_type": "code", 142 | "execution_count": null, 143 | "metadata": {}, 144 | "outputs": [], 145 | "source": [ 146 | "import matplotlib\n", 147 | "# matplotlib.rc('text', usetex=True)\n", 148 | "# matplotlib.rcParams['text.latex.preamble'] = r'\\usepackage{bm}'\n", 149 | "\n", 150 | "plt.figure(figsize=(3.5,3.5))\n", 151 | "G.draw_sets()\n", 152 | "\n", 153 | "Z = r'\\bar{\\bf{z}}'\n", 154 | "plt.text(.2, 0,\n", 155 | " '$' + Z + '_{(s,1)} =' + Z + '_{(s,2)}$',\n", 156 | " ha='left', va='center')\n", 157 | "plt.text(.1, 1.9,\n", 158 | " '$' + Z + '_{(s,1)}^\\prime =' + Z + '_{(1,3)}$',\n", 159 | " ha='left', va='top')\n", 160 | "plt.text(.1, -1.9,\n", 161 | " r'$' + Z + '_{(s,2)}^\\prime =' + Z + '_{(2,3)}$',\n", 162 | " ha='left', va='bottom')\n", 163 | "plt.text(3, 1.9,\n", 164 | " r'$' + Z + '_{(1,3)}^\\prime$',\n", 165 | " ha='center', va='top')\n", 166 | "plt.text(3, -1.9,\n", 167 | " r'$' + Z + '_{(2,3)}^\\prime$',\n", 168 | " ha='center', va='bottom')\n", 169 | "plt.text(3, 0.1,\n", 170 | " r'$' + Z + '_{(3,t)}$',\n", 171 | " ha='center', va='bottom')\n", 172 | "plt.text(5, .1,\n", 173 | " r'$' + Z + '_{(3,t)}^\\prime$',\n", 174 | " ha='right', va='bottom')\n", 175 | "\n", 176 | "plt.text(.1, 1, r'$1/2$', ha='left', va='center', c='b')\n", 177 | "plt.text(.1, -1, r'$1/2$', ha='left', va='center', c='b')\n", 178 | "plt.text(1, 1.99, r'$1/2$', ha='center', va='bottom', c='b')\n", 179 | "plt.text(1, -2.05, r'$1/2$', ha='center', va='top', c='b')\n", 180 | "plt.text(3.8, 0, r'$1$', ha='right', va='bottom', c='b')\n", 181 | "\n", 182 | "y3t = y[4] / phi[4]\n", 183 | "z13 = z[2] / phi[2]\n", 184 | "z23 = z[3] / phi[3]\n", 185 | "\n", 186 | "plt.plot([0, 0], [0, 2], 'orangered', **kwargs)\n", 187 | "plt.plot([0, 0], [0, -2], 'orangered', **kwargs)\n", 188 | "plt.plot([0, z13[0]], [2, z13[1]], 'orangered', **kwargs)\n", 189 | "plt.plot([0, z23[0]], [-2, z23[1]], 'orangered', **kwargs)\n", 190 | "plt.plot([y3t[0], 5], [y3t[1], 0], 'orangered', **kwargs)\n", 191 | "\n", 192 | "plt.grid()\n", 193 | "plt.savefig('symmetry_relaxation.pdf', bbox_inches='tight')" 194 | ] 195 | }, 196 | { 197 | "cell_type": "code", 198 | "execution_count": null, 199 | "metadata": {}, 200 | "outputs": [], 201 | "source": [] 202 | } 203 | ], 204 | "metadata": { 205 | "kernelspec": { 206 | "display_name": "Python 3 (ipykernel)", 207 | "language": "python", 208 | "name": "python3" 209 | }, 210 | "language_info": { 211 | "codemirror_mode": { 212 | "name": "ipython", 213 | "version": 3 214 | }, 215 | "file_extension": ".py", 216 | "mimetype": "text/x-python", 217 | "name": "python", 218 | "nbconvert_exporter": "python", 219 | "pygments_lexer": "ipython3", 220 | "version": "3.11.2" 221 | } 222 | }, 223 | "nbformat": 4, 224 | "nbformat_minor": 4 225 | } 226 | -------------------------------------------------------------------------------- /sensory_coverage.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "Replicates results from Figure 3 of Burdick, Bouman, Rimon \"From Multi-Target Sensory Coverage to Complete Sensory Coverage: An Optimization-Based Robotic Sensory Coverage Approach\"" 8 | ] 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": null, 13 | "metadata": {}, 14 | "outputs": [], 15 | "source": [ 16 | "%matplotlib notebook\n", 17 | "%load_ext autoreload\n", 18 | "%autoreload 2" 19 | ] 20 | }, 21 | { 22 | "cell_type": "code", 23 | "execution_count": null, 24 | "metadata": {}, 25 | "outputs": [], 26 | "source": [ 27 | "import numpy as np\n", 28 | "import matplotlib.pyplot as plt\n", 29 | "from itertools import chain, combinations\n", 30 | "from spp.convex_sets import Singleton, Ellipsoid\n", 31 | "from spp.convex_functions import TwoNorm, SquaredTwoNorm\n", 32 | "from spp.graph import GraphOfConvexSets\n", 33 | "from spp.shortest_path import ShortestPathProblem" 34 | ] 35 | }, 36 | { 37 | "cell_type": "code", 38 | "execution_count": null, 39 | "metadata": { 40 | "scrolled": false 41 | }, 42 | "outputs": [], 43 | "source": [ 44 | "# convex sets\n", 45 | "A = ([2, 0], [0, 2])\n", 46 | "sets = [\n", 47 | " Singleton((0, 0)),\n", 48 | " Ellipsoid((1.5, 1), A),\n", 49 | " Ellipsoid((2, 2.5), A),\n", 50 | " Ellipsoid((3, 5), A),\n", 51 | " Ellipsoid((4, 7), A),\n", 52 | " Ellipsoid((6, 5), A),\n", 53 | " Ellipsoid((7, 3.5), A),\n", 54 | " Ellipsoid((8, 2), A),\n", 55 | "]\n", 56 | "\n", 57 | "# add convex sets to the graph\n", 58 | "G = GraphOfConvexSets()\n", 59 | "G.add_sets(sets)\n", 60 | "G.set_source(0)\n", 61 | "G.set_target(7)\n", 62 | "\n", 63 | "# edges\n", 64 | "H = np.hstack((np.eye(2), -np.eye(2)))\n", 65 | "l = TwoNorm(H)\n", 66 | "for u in range(len(sets)):\n", 67 | " for v in range(len(sets)):\n", 68 | " if u != v:\n", 69 | " G.add_edge(u, v, l)\n", 70 | " \n", 71 | "# draw convex sets and edges\n", 72 | "plt.figure()\n", 73 | "G.draw_sets()\n", 74 | "G.label_sets()\n", 75 | "plt.xlim([-.3, 9])\n", 76 | "plt.ylim([-.3, 8])\n", 77 | "plt.grid()" 78 | ] 79 | }, 80 | { 81 | "cell_type": "code", 82 | "execution_count": null, 83 | "metadata": {}, 84 | "outputs": [], 85 | "source": [ 86 | "# add sensory-coverage constraints\n", 87 | "spp = ShortestPathProblem(G, relaxation=1)\n", 88 | "for v, Xv in G.sets.items():\n", 89 | " if v not in [0, 7]:\n", 90 | " Ein = G.incoming_edges(v)[1]\n", 91 | " spp.prog.AddLinearConstraint(sum(spp.vars.phi[Ein]) == 1)\n", 92 | " \n", 93 | "# subtour elimination\n", 94 | "def powerset(iterable):\n", 95 | " s = list(iterable)\n", 96 | " return chain.from_iterable(combinations(s, r) for r in range(len(s)+1))\n", 97 | "for subtour in powerset(G.vertices[1:-1]):\n", 98 | " if len(subtour) >= 2:\n", 99 | " length = 0\n", 100 | " for u in subtour:\n", 101 | " for v in subtour:\n", 102 | " if u != v:\n", 103 | " length += spp.vars.phi[G.edges.index((u, v))]\n", 104 | " spp.prog.AddLinearConstraint(length <= len(subtour) - 1)\n", 105 | " \n", 106 | " # spatial subtour elimination\n", 107 | " if len(subtour) == 2:\n", 108 | " \n", 109 | " u, v = subtour\n", 110 | " uv = G.edges.index((u, v))\n", 111 | " vu = G.edges.index((v, u))\n", 112 | " nonnegative = 1 - spp.vars.phi[uv] - spp.vars.phi[vu]\n", 113 | " \n", 114 | " Euout = G.outgoing_edges(u)[1]\n", 115 | " xu = sum(spp.vars.y[e] for e in Euout)\n", 116 | " spatial = xu - spp.vars.y[uv] - spp.vars.z[vu]\n", 117 | " G.sets[u].add_perspective_constraint(spp.prog, nonnegative, spatial)\n", 118 | " \n", 119 | " Evin = G.incoming_edges(v)[1]\n", 120 | " xv = sum(spp.vars.z[e] for e in Evin)\n", 121 | " spatial = xv - spp.vars.y[vu] - spp.vars.z[uv]\n", 122 | " G.sets[v].add_perspective_constraint(spp.prog, nonnegative, spatial)\n", 123 | " \n", 124 | "sol = spp.solve()" 125 | ] 126 | }, 127 | { 128 | "cell_type": "code", 129 | "execution_count": null, 130 | "metadata": {}, 131 | "outputs": [], 132 | "source": [ 133 | "print('Cost:', sol.cost)\n", 134 | "print('\\nFlows:')\n", 135 | "for k, edge in enumerate(G.edges):\n", 136 | " flow = round(abs(sol.primal.phi[k]), 4)\n", 137 | " print(edge, flow)" 138 | ] 139 | }, 140 | { 141 | "cell_type": "code", 142 | "execution_count": null, 143 | "metadata": {}, 144 | "outputs": [], 145 | "source": [ 146 | "plt.figure()\n", 147 | "G.draw_sets()\n", 148 | "G.label_sets()\n", 149 | "G.draw_path(sol.primal.phi, sol.primal.x, color='r', linestyle='--')\n", 150 | "\n", 151 | "plt.xticks(range(9))\n", 152 | "plt.xlim([-.3, 9])\n", 153 | "plt.ylim([-.3, 8])\n", 154 | "plt.grid()\n", 155 | "plt.savefig('sensory_coverage.pdf', bbox_inches='tight')" 156 | ] 157 | } 158 | ], 159 | "metadata": { 160 | "kernelspec": { 161 | "display_name": "Python 3", 162 | "language": "python", 163 | "name": "python3" 164 | }, 165 | "language_info": { 166 | "codemirror_mode": { 167 | "name": "ipython", 168 | "version": 3 169 | }, 170 | "file_extension": ".py", 171 | "mimetype": "text/x-python", 172 | "name": "python", 173 | "nbconvert_exporter": "python", 174 | "pygments_lexer": "ipython3", 175 | "version": "3.9.7" 176 | } 177 | }, 178 | "nbformat": 4, 179 | "nbformat_minor": 4 180 | } 181 | -------------------------------------------------------------------------------- /solution_d.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/solution_d.npy -------------------------------------------------------------------------------- /solution_nE.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/solution_nE.npy -------------------------------------------------------------------------------- /solution_nV_nE.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/solution_nV_nE.npy -------------------------------------------------------------------------------- /solution_nom.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/solution_nom.npy -------------------------------------------------------------------------------- /solution_vol.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/shortest-paths-in-graphs-of-convex-sets/b6b41ca7a6135f5749c47e14fd11629a65613416/solution_vol.npy -------------------------------------------------------------------------------- /spp/convex_functions.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class ConvexFunction(): 4 | """Parent class for all the convex functions.""" 5 | 6 | def __call__(self, x): 7 | 8 | if self.D is not None and not self.D.contains(x): 9 | return np.inf 10 | else: 11 | return self._evaluate(x) 12 | 13 | def add_as_cost(self, prog, x): 14 | 15 | domain = self.enforce_domain(prog, 1, x) 16 | cost = self._add_as_cost(prog, x) 17 | 18 | return cost, domain 19 | 20 | def add_perspective_constraint(self, prog, slack, scale, x): 21 | 22 | cost = self._add_perspective_constraint(prog, slack, scale, x) 23 | domain = self.enforce_domain(prog, scale, x) 24 | 25 | return cost, domain 26 | 27 | def enforce_domain(self, prog, scale, x): 28 | if self.D is not None: 29 | return self.D.add_perspective_constraint(prog, scale, x) 30 | 31 | class Constant(ConvexFunction): 32 | """Function of the form c for x in D, where D is a ConvexSet.""" 33 | 34 | def __init__(self, c, D=None): 35 | 36 | self.c = c 37 | self.D = D 38 | 39 | def _evaluate(self, x): 40 | 41 | return self.c 42 | 43 | def _add_perspective_constraint(self, prog, slack, scale, x): 44 | 45 | return prog.AddLinearConstraint(slack >= self.c * scale) 46 | 47 | def _add_as_cost(self, prog, x): 48 | 49 | return prog.AddLinearCost(self.c) 50 | 51 | class TwoNorm(ConvexFunction): 52 | """Function of the form ||H x||_2 for x in D, where D is a ConvexSet.""" 53 | 54 | def __init__(self, H, D=None): 55 | 56 | self.H = H 57 | self.D = D 58 | 59 | def _evaluate(self, x): 60 | 61 | return np.linalg.norm(self.H.dot(x)) 62 | 63 | def _add_perspective_constraint(self, prog, slack, scale, x): 64 | 65 | Hx = self.H.dot(x) 66 | return prog.AddLorentzConeConstraint(slack, Hx.dot(Hx)) 67 | 68 | def _add_as_cost(self, prog, x): 69 | 70 | slack = prog.NewContinuousVariables(1) 71 | self._add_perspective_constraint(self, prog, slack, 1, x) 72 | return prog.AddLinearCost(slack) 73 | 74 | class SquaredTwoNorm(ConvexFunction): 75 | """Function of the form ||H x||_2^2 for x in D, where D is a ConvexSet.""" 76 | 77 | def __init__(self, H, D=None): 78 | 79 | self.H = H 80 | self.D = D 81 | 82 | def _evaluate(self, x): 83 | 84 | Hx = self.H.dot(x) 85 | return Hx.dot(Hx) 86 | 87 | def _add_perspective_constraint(self, prog, slack, scale, x): 88 | 89 | Hx = self.H.dot(x) 90 | return prog.AddRotatedLorentzConeConstraint(slack, scale, Hx.dot(Hx)) 91 | 92 | def _add_as_cost(self, prog, x): 93 | 94 | Hx = self.H.dot(x) 95 | return prog.AddQuadraticCost(Hx.dot(Hx)) 96 | -------------------------------------------------------------------------------- /spp/convex_sets.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy as sp 3 | import matplotlib.pyplot as plt 4 | import matplotlib.patches as patches 5 | from itertools import product 6 | from scipy.spatial import ConvexHull, HalfspaceIntersection 7 | from pydrake.all import Expression, MathematicalProgram, MosekSolver, eq, le 8 | 9 | class ConvexSet(): 10 | 11 | def __init__(self): 12 | raise NotImplementedError 13 | 14 | def contains(self, x): 15 | raise NotImplementedError 16 | 17 | def translate(self, x): 18 | raise NotImplementedError 19 | 20 | def bounding_box(self): 21 | x_min = np.zeros(self._dimension) 22 | x_max = np.zeros(self._dimension) 23 | for i in range(self._dimension): 24 | prog = MathematicalProgram() 25 | x = prog.NewContinuousVariables(self._dimension) 26 | self.add_membership_constraint(prog, x) 27 | prog.AddLinearCost(x[i]) 28 | result = MosekSolver().Solve(prog) 29 | x_opt = result.GetSolution(x) 30 | x_min[i] = x_opt[i] 31 | prog.AddLinearCost(-2*x[i]) 32 | result = MosekSolver().Solve(prog) 33 | x_opt = result.GetSolution(x) 34 | x_max[i] = x_opt[i] 35 | return Polyhedron.from_bounds(x_min, x_max) 36 | 37 | def scale(self, s): 38 | assert s > 0 39 | return self._scale(s) 40 | 41 | def add_perspective_constraint(self, prog, scale, x): 42 | raise NotImplementedError 43 | 44 | def _cheb_constraint(self, prog, x, r): 45 | raise NotImplementedError 46 | 47 | def add_membership_constraint(self, prog, x): 48 | return self.add_perspective_constraint(prog, 1, x) 49 | 50 | def plot(self, **kwargs): 51 | if self.dimension != 2: 52 | raise NotImplementedError 53 | options = {'facecolor':'mintcream', 'edgecolor':'k', 'zorder':1} 54 | options.update(kwargs) 55 | self._plot(**options) 56 | 57 | @property 58 | def dimension(self): 59 | return self._dimension 60 | 61 | @property 62 | def center(self): 63 | '''Chebyshev center (center of largest inscribed ball).''' 64 | if self._center is None: 65 | self._center = self._compute_center() 66 | return self._center 67 | 68 | class Singleton(ConvexSet): 69 | '''Singleton set {x}.''' 70 | 71 | def __init__(self, x): 72 | self._center = np.array(x).astype('float64') 73 | self._dimension = self.center.size 74 | 75 | def contains(self, x): 76 | return np.allclose(self.center, x) 77 | 78 | def translate(self, x): 79 | self._center + x 80 | 81 | def _scale(self, s): 82 | pass 83 | 84 | def add_perspective_constraint(self, prog, scale, x): 85 | return prog.AddLinearConstraint(eq(x, self.center * scale)) 86 | 87 | def _plot(self, **kwargs): 88 | plt.scatter(*self.center, c='k') 89 | 90 | def _cheb_constraint(self, prog, x, r): 91 | prog.AddLinearConstraint(eq(x, self._center)) 92 | prog.AddLinearConstraint(r == 0) 93 | 94 | class Polyhedron(ConvexSet): 95 | '''Polyhedron in halfspace representation {x : A x = b, C x <= d}.''' 96 | 97 | def __init__(self, eq_matrices=None, ineq_matrices=None): 98 | '''Arguments are eq_matrices = (A, b) and ineq_matrices = (C, d).''' 99 | 100 | if eq_matrices is not None: 101 | self.A, self.b = [M.astype('float64') for M in eq_matrices] 102 | self._dimension = self.A.shape[1] 103 | if ineq_matrices is not None: 104 | self.C, self.d = [M.astype('float64') for M in ineq_matrices] 105 | self._dimension = self.C.shape[1] 106 | 107 | if eq_matrices is None: 108 | self.A = np.zeros((0, self._dimension)) 109 | self.b = np.zeros(0) 110 | if ineq_matrices is None: 111 | self.C = np.zeros((0, self._dimension)) 112 | self.d = np.zeros(0) 113 | 114 | self._center = None 115 | self._vertices = None 116 | 117 | def contains(self, x): 118 | 119 | residual = self.A.dot(x) - self.b 120 | eq_matrices = np.allclose(residual, 0) 121 | 122 | residual = self.C.dot(x) - self.d 123 | ineq_matrices = np.isclose(max(max(residual, default=0), 0), 0) 124 | 125 | return eq_matrices and ineq_matrices 126 | 127 | def translate(self, x): 128 | 129 | self.b += self.A.dot(x) 130 | self.d += self.C.dot(x) 131 | 132 | if self._center is not None: 133 | self._center += x 134 | 135 | if self._vertices is not None: 136 | self._vertices += x 137 | 138 | def _scale(self, s): 139 | 140 | center = self.center.copy() 141 | self.translate(- center) 142 | self.b *= s 143 | self.d *= s 144 | if self._vertices is not None: 145 | self._vertices *= s 146 | self.translate(center) 147 | 148 | def add_perspective_constraint(self, prog, scale, x): 149 | 150 | if self.A.shape[0] == 0: 151 | eq_matrices = None 152 | else: 153 | residual = self.A.dot(x) - self.b * scale 154 | eq_matrices = prog.AddLinearConstraint(eq(residual, 0)) 155 | 156 | if self.C.shape[0] == 0: 157 | ineq_matrices = None 158 | else: 159 | residual = self.C.dot(x) - self.d * scale 160 | ineq_matrices = prog.AddLinearConstraint(le(residual, 0)) 161 | 162 | return eq_matrices, ineq_matrices 163 | 164 | def _plot(self, **kwargs): 165 | 166 | if self.vertices.shape[0] < 3: 167 | raise NotImplementedError 168 | 169 | hull = ConvexHull(self.vertices) # orders vertices counterclockwise 170 | vertices = self.vertices[hull.vertices] 171 | plt.fill(*vertices.T, **kwargs) 172 | 173 | def _compute_center(self): 174 | 175 | prog = MathematicalProgram() 176 | x = prog.NewContinuousVariables(self.dimension) 177 | r = prog.NewContinuousVariables(1)[0] 178 | prog.AddLinearConstraint(r >= 0) 179 | prog.AddLinearCost(-r) 180 | 181 | self._cheb_constraint(prog, x, r) 182 | result = MosekSolver().Solve(prog) 183 | 184 | return result.GetSolution(x) 185 | 186 | def _cheb_constraint(self, prog, x, r): 187 | 188 | if self.A.shape[0] > 0: 189 | prog.AddLinearConstraint(eq(self.A.dot(x), self.b)) 190 | 191 | if self.C.shape[0] > 0: 192 | C_row_norm = np.linalg.norm(self.C, axis=1) 193 | lhs = self.C.dot(x) + C_row_norm * r 194 | prog.AddLinearConstraint(le(lhs, self.d)) 195 | 196 | @property 197 | def vertices(self): 198 | 199 | if self._vertices is not None: 200 | return self._vertices 201 | 202 | elif self.A.shape[0] > 0: 203 | raise NotImplementedError 204 | 205 | else: 206 | halfspaces = np.column_stack((self.C, -self.d)) 207 | P = HalfspaceIntersection(halfspaces, self.center) 208 | self._vertices = P.intersections 209 | return self._vertices 210 | 211 | @staticmethod 212 | def from_bounds(x_min, x_max): 213 | 214 | I = np.eye(len(x_min)) 215 | C = np.vstack((I, -I)) 216 | d = np.concatenate((x_max, -np.array(x_min))) 217 | P = Polyhedron(ineq_matrices=(C, d)) 218 | P._vertices = np.array(list(product(*zip(x_min, x_max)))) 219 | 220 | return P 221 | 222 | @staticmethod 223 | def from_vertices(vertices): 224 | 225 | vertices = np.array(vertices).astype('float64') 226 | m, dimension = vertices.shape 227 | 228 | if m <= dimension: 229 | raise NotImplementedError 230 | else: 231 | ch = ConvexHull(vertices) 232 | ineq_matrices = (ch.equations[:, :-1], - ch.equations[:, -1]) 233 | 234 | P = Polyhedron(ineq_matrices=ineq_matrices) 235 | P._vertices = vertices 236 | 237 | return P 238 | 239 | class Ellipsoid(ConvexSet): 240 | '''Ellipsoid in the form {x : (x - center)' A (x - center) <= 1}. 241 | The matrix A is assumed to be PSD (and symmetric).''' 242 | 243 | def __init__(self, center, A): 244 | 245 | self._center = np.array(center) 246 | self.A = np.array(A).astype('float64') 247 | self._dimension = self._center.size 248 | 249 | def contains(self, x): 250 | 251 | d = np.array(x) - self.center 252 | ineq_matrices = d.dot(self.A).dot(d) - 1 253 | 254 | return np.isclose(max(ineq_matrices, 0), 0) 255 | 256 | def translate(self, x): 257 | self._center += x 258 | 259 | def _scale(self, s): 260 | self.A *= 1 / s ** 2 261 | 262 | def add_perspective_constraint(self, prog, scale, x): 263 | 264 | R = sp.linalg.sqrtm(self.A) 265 | v = np.concatenate(([scale], R.dot(x - self.center * scale))) 266 | cone_constraint = prog.AddLorentzConeConstraint(v) 267 | 268 | return cone_constraint 269 | 270 | def _cheb_constraint(self, prog, x, r): 271 | '''Section 8.5.1 of Boyd and Vandenberghe - Convex Optimization.''' 272 | 273 | l = prog.NewContinuousVariables(1)[0] 274 | I = np.eye(self.dimension) 275 | M11 = np.array([[1 - l]]) 276 | M12 = np.zeros((1, self.dimension)) 277 | M13 = np.array([x - self.center]) 278 | M22 = l * I 279 | M23 = r * I 280 | M33 = np.linalg.inv(self.A) 281 | M = np.block([[M11, M12, M13], [M12.T, M22, M23], [M13.T, M23.T, M33]]) 282 | prog.AddPositiveSemidefiniteConstraint(M * Expression(1)) 283 | 284 | def polyhedral_approximation(self, n=100): 285 | 286 | assert self.dimension == 2 287 | thetas = np.linspace(0, 2 * np.pi, n) 288 | vertices = np.zeros((n, 2)) 289 | for i, t in enumerate(thetas): 290 | d = np.array([np.cos(t), np.sin(t)]) 291 | scale = 1 / np.sqrt(d.dot(self.A).dot(d)) 292 | vertices[i] = self.center + scale * d 293 | 294 | return Polyhedron.from_vertices(vertices) 295 | 296 | def _plot(self, **kwargs): 297 | 298 | l, v = np.linalg.eig(self.A) 299 | angle = 180 * np.arctan2(*v[0]) / np.pi + 90 300 | ellipse = (self.center, 2 * l[0] ** -.5, 2 * l[1] ** -.5, angle) 301 | patch = patches.Ellipse(*ellipse, **kwargs) 302 | plt.gca().add_artist(patch) 303 | 304 | class Intersection(ConvexSet): 305 | 306 | def __init__(self, sets): 307 | 308 | self.sets = sets 309 | assert len(set(X.dimension for X in sets)) == 1 310 | self._dimension = sets[0].dimension 311 | self._center = None 312 | 313 | def contains(self, x): 314 | return all(X.contains(x) for X in self.sets) 315 | 316 | def translate(self, x): 317 | return Intersection([X.translate(x) for X in self.sets]) 318 | 319 | def _scale(self, s): 320 | return Intersection([X.scale(s) for X in self.sets]) 321 | 322 | def add_perspective_constraint(self, prog, scale, x): 323 | return [X.add_perspective_constraint(prog, scale, x) for X in self.sets] 324 | 325 | def _compute_center(self): 326 | 327 | prog = MathematicalProgram() 328 | x = prog.NewContinuousVariables(self.dimension) 329 | r = prog.NewContinuousVariables(1)[0] 330 | prog.AddLinearConstraint(r >= 0) 331 | prog.AddLinearCost(-r) 332 | 333 | for X in self.sets: 334 | X._cheb_constraint(prog, x, r) 335 | result = MosekSolver().Solve(prog) 336 | 337 | return result.GetSolution(x) 338 | 339 | def _plot(self, **kwargs): 340 | 341 | vertices = [] 342 | for X in self.sets: 343 | 344 | if isinstance(X, Singleton): 345 | raise NotImplementedError 346 | 347 | if not isinstance(X, Polyhedron): 348 | X = X.polyhedral_approximation() 349 | 350 | for v in X.vertices: 351 | if self.contains(v): 352 | vertices.append(v) 353 | 354 | P = Polyhedron.from_vertices(np.vstack(vertices)) 355 | P.plot() 356 | 357 | class CartesianProduct(ConvexSet): 358 | 359 | def __init__(self, sets): 360 | 361 | self.sets = sets 362 | self._split_at = np.cumsum([X.dimension for X in sets]) 363 | self._dimension = self._split_at[-1] 364 | self._center = None 365 | 366 | def split(self, x): 367 | return np.split(x, self._split_at[:-1]) 368 | 369 | def contains(self, x): 370 | return all(X.contains(p) for p, X in zip(self.split(x), self.sets)) 371 | 372 | def translate(self, x): 373 | return CartesianProduct([X.translate(p) for p, X in zip(self.split(x), self.sets)]) 374 | 375 | def _scale(self, s): 376 | return CartesianProduct([X.scale(s) for X in self.sets]) 377 | 378 | def add_perspective_constraint(self, prog, scale, x): 379 | return [X.add_perspective_constraint(prog, scale, p) for p, X in zip(self.split(x), self.sets)] 380 | 381 | def _compute_center(self): 382 | return np.concatenate([X.center for X in self.sets]) 383 | 384 | def _plot(self, **kwargs): 385 | raise NotImplementedError 386 | -------------------------------------------------------------------------------- /spp/graph.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import matplotlib.patches as patches 4 | from copy import deepcopy 5 | from spp.convex_sets import ConvexSet 6 | from spp.convex_functions import ConvexFunction, Constant 7 | # from graphviz import Digraph 8 | 9 | class GraphOfConvexSets(): 10 | 11 | def __init__(self): 12 | 13 | self.sets = {} 14 | self.lengths = {} 15 | 16 | self._source = None 17 | self._target = None 18 | 19 | def add_set(self, set, vertex=None): 20 | 21 | assert isinstance(set, ConvexSet) 22 | if vertex is None: 23 | vertex = len(self.sets) 24 | 25 | assert vertex not in self.sets 26 | self.sets[vertex] = set 27 | 28 | return vertex 29 | 30 | def add_sets(self, sets, vertices=None): 31 | 32 | if vertices is None: 33 | vertices = [None] * len(sets) 34 | else: 35 | assert len(sets) == len(vertices) 36 | 37 | for i, set in enumerate(sets): 38 | vertices[i] = self.add_set(set, vertices[i]) 39 | 40 | return vertices 41 | 42 | def remove_set(self, vertex): 43 | 44 | self.sets.pop(vertex) 45 | for edge in self.edges: 46 | if vertex in edge: 47 | self.remove_edge(edge) 48 | 49 | def add_edge(self, u, v, length=None): 50 | 51 | if length is None: 52 | length = Constant(0) 53 | 54 | assert u in self.sets and v in self.sets 55 | assert isinstance(length, ConvexFunction) 56 | self.lengths[(u, v)] = length 57 | 58 | def add_edges(self, us, vs, lengths=None): 59 | 60 | assert len(us) == len(vs) 61 | if lengths is None: 62 | lengths = [None] * len(us) 63 | else: 64 | assert len(us) == len(lengths) 65 | 66 | for u, v, length in zip(us, vs, lengths): 67 | self.add_edge(u, v, length) 68 | 69 | def remove_edge(self, edge): 70 | 71 | self.lengths.pop(edge) 72 | 73 | def set_source(self, vertex): 74 | 75 | assert vertex in self.sets 76 | self._source = vertex 77 | 78 | def set_target(self, vertex): 79 | 80 | assert vertex in self.sets 81 | self._target = vertex 82 | 83 | def self_transition(self, vertex, self_length, vertex_copy=None): 84 | 85 | vertex_copy = self.add_set(self.sets[vertex], vertex_copy) 86 | self.add_edge(vertex_copy, vertex, self_length) 87 | 88 | incomings = [edge for edge in self.edges if edge[1] == vertex] 89 | for edge in incomings: 90 | self.add_edge(edge[0], vertex_copy, self.lengths[edge]) 91 | 92 | return vertex_copy 93 | 94 | def double_visit(self, vertex, self_length, vertex_copy=None): 95 | vertex_copy = self.self_transition(vertex, self_length, vertex_copy) 96 | for edge, length in self.lengths.items(): 97 | if e[0] == vertex: 98 | self.add_edge(vertex_copy, e[1], length) 99 | return vertex_copy 100 | 101 | def set_edge_length(self, edge, length): 102 | self.lengths[edge] = length 103 | 104 | def edge_index(self, edge): 105 | return self.edges.index(edge) 106 | 107 | def edge_indices(self, edges): 108 | return [self.edges.index(edge) for edge in edges] 109 | 110 | def vertex_index(self, vertex): 111 | return self.vertices.index(vertex) 112 | 113 | def vertex_indices(self, vertices): 114 | return [self.vertices.index(vertex) for vertex in vertices] 115 | 116 | def incoming_edges(self, vertex): 117 | assert vertex in self.vertices 118 | edges = [edge for edge in self.edges if edge[1] == vertex] 119 | return edges, self.edge_indices(edges) 120 | 121 | def outgoing_edges(self, vertex): 122 | assert vertex in self.vertices 123 | edges = [edge for edge in self.edges if edge[0] == vertex] 124 | return edges, self.edge_indices(edges) 125 | 126 | def incident_edges(self, vertex): 127 | incomings = self.incoming_edges(vertex) 128 | outgoings = self.outgoing_edges(vertex) 129 | return [i + o for i, o in zip(incomings, outgoings)] 130 | 131 | def scale(self, s): 132 | for convex_set in set(self.sets.values()): 133 | convex_set.scale(s) 134 | 135 | def draw_sets(self, **kwargs): 136 | plt.rc('axes', axisbelow=True) 137 | plt.gca().set_aspect('equal') 138 | for set in self.sets.values(): 139 | set.plot(**kwargs) 140 | 141 | def draw_edges(self, **kwargs): 142 | options = {'color':'k', 'zorder':2, 143 | 'arrowstyle':'->, head_width=3, head_length=8'} 144 | options.update(kwargs) 145 | for edge in self.edges: 146 | tail = self.sets[edge[0]].center 147 | head = self.sets[edge[1]].center 148 | arrow = patches.FancyArrowPatch(tail, head, **options) 149 | plt.gca().add_patch(arrow) 150 | 151 | def label_sets(self, labels=None, **kwargs): 152 | 153 | options = {'c':'b'} 154 | options.update(kwargs) 155 | if labels is None: 156 | labels = self.vertices 157 | 158 | for set, label in zip(self.sets.values(), labels): 159 | plt.text(*set.center, label, **options) 160 | 161 | def label_edges(self, labels, **kwargs): 162 | options = {'c':'r', 'va':'top'} 163 | options.update(kwargs) 164 | for edge, label in zip(self.edges, labels): 165 | center = (self.sets[edge[0]].center + self.sets[edge[1]].center) / 2 166 | plt.text(*center, label, **options) 167 | 168 | def draw_vertices(self, x): 169 | options = {'marker':'o', 'facecolor':'w', 'edgecolor':'k', 'zorder':3} 170 | options.update(kwargs) 171 | plt.scatter(*x.T, **options) 172 | 173 | def draw_path(self, phis, x, **kwargs): 174 | options = {'color':'g', 'marker': 'o', 'markeredgecolor': 'k', 'markerfacecolor': 'w'} 175 | options.update(kwargs) 176 | for k, phi in enumerate(phis): 177 | if phi > 1 - 1e-3: 178 | edge = [self.vertices.index(vertex) for vertex in self.edges[k]] 179 | plt.plot(*x[edge].T, **options) 180 | 181 | def graphviz(self, vertex_labels=None, edge_labels=None): 182 | 183 | if vertex_labels is None: 184 | vertex_labels = self.vertices 185 | if edge_labels is None: 186 | edge_labels = [''] * len(self.edges) 187 | 188 | G = Digraph() 189 | for label in vertex_labels: 190 | G.node(str(label)) 191 | for k, edge in enumerate(self.edges): 192 | u = vertex_labels[self.vertices.index(edge[0])] 193 | v = vertex_labels[self.vertices.index(edge[1])] 194 | G.edge(str(u), str(v), str(edge_labels[k])) 195 | return G 196 | 197 | @property 198 | def vertices(self): 199 | return list(self.sets.keys()) 200 | 201 | @property 202 | def edges(self): 203 | return list(self.lengths.keys()) 204 | 205 | @property 206 | def source(self): 207 | return self.sets[self.vertices[0]] if self._source is None else self._source 208 | 209 | @property 210 | def target(self): 211 | return self.sets[self.vertices[-1]] if self._target is None else self._target 212 | 213 | @property 214 | def source_set(self): 215 | return self.sets[self.source] 216 | 217 | @property 218 | def target_set(self): 219 | return self.sets[self.target] 220 | 221 | @property 222 | def dimension(self): 223 | assert len(set(S.dimension for S in self.sets.values())) == 1 224 | return self.sets[self.vertices[0]].dimension 225 | 226 | @property 227 | def n_sets(self): 228 | return len(self.sets) 229 | 230 | @property 231 | def n_edges(self): 232 | return len(self.lengths) 233 | -------------------------------------------------------------------------------- /spp/pwa_systems.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy as sp 3 | from spp.convex_sets import Singleton, Polyhedron, CartesianProduct 4 | from spp.convex_functions import Constant, SquaredTwoNorm 5 | from spp.graph import GraphOfConvexSets 6 | from spp.shortest_path import ShortestPathProblem 7 | 8 | class PieceWiseAffineSystem(): 9 | '''Dynamical system of the form 10 | z(k+1) = Ai z(k) + Bi u(k) + ci if (z(k), u(k)) in Di 11 | where Di := {(z, u): Fi z + Gi u <= hi}.''' 12 | 13 | def __init__(self, dynamics, domains): 14 | '''Arguments must be: 15 | dynamics: list of triples (Ai, Bi, ci) 16 | domains: list of instances of Polyhedron (must be bounded) 17 | ''' 18 | self.dynamics = dynamics 19 | self.domains = domains 20 | self.nm = len(dynamics) 21 | self.nz, self.nu = dynamics[0][1].shape 22 | 23 | class RegulationSolution(): 24 | 25 | def __init__(self, z, u, ms, spp): 26 | 27 | self.z = z 28 | self.u = u 29 | self.ms = ms 30 | self.spp = spp 31 | 32 | class ShortestPathRegulator(): 33 | 34 | def __init__(self, pwa, K, z1, Z, cost_matrices, relaxation=False): 35 | 36 | self.pwa = pwa 37 | self.K = K 38 | self.z1 = z1 39 | self.Z = Z 40 | self.Q, self.R, self.S = cost_matrices 41 | graph = self._construct_graph() 42 | self.spp = ShortestPathProblem(graph, relaxation) 43 | 44 | def _construct_graph(self): 45 | 46 | # initialize graph 47 | graph = GraphOfConvexSets() 48 | 49 | # ficticious source set 50 | Zs = Singleton(self.z1) 51 | U = Singleton(np.zeros(self.pwa.nu)) 52 | graph.add_set(CartesianProduct((Zs, U)), 0) 53 | graph.set_source(0) 54 | 55 | # vertices for time steps k = 1, ..., K - 1 56 | for k in range(1, self.K): 57 | for i in range(self.pwa.nm): 58 | graph.add_set(self.pwa.domains[i], (k, i)) 59 | 60 | # target vertex 61 | graph.add_set(CartesianProduct((self.Z, U)), self.K) 62 | graph.set_target(self.K) 63 | 64 | # time step zero 65 | for i in range(self.pwa.nm): 66 | 67 | # force initial conditions 68 | I = np.eye(self.pwa.nz) 69 | zero = np.zeros((self.pwa.nz, self.pwa.nu)) 70 | A = np.hstack((I, zero, -I, zero)) 71 | b = np.zeros(self.pwa.nz) 72 | D = Polyhedron(eq_matrices=(A, b)) 73 | graph.add_edge(0, (1, i), Constant(0, D)) 74 | 75 | # domains for the edge lengths 76 | D = [] 77 | for i in range(self.pwa.nm): 78 | Ai, Bi, ci = self.pwa.dynamics[i] 79 | A = np.hstack((Ai, Bi, - np.eye(self.pwa.nz), np.zeros((self.pwa.nz, self.pwa.nu)))) 80 | b = - ci 81 | D.append(Polyhedron(eq_matrices=(A, b))) 82 | 83 | # edges for time steps k = 1, ..., K - 2 84 | H = sp.linalg.block_diag(self.Q, self.R, np.zeros((self.pwa.nz + self.pwa.nu,) * 2)) 85 | for k in range(1, self.K - 1): 86 | for i in range(self.pwa.nm): 87 | for j in range(self.pwa.nm): 88 | graph.add_edge((k, i), (k + 1, j), SquaredTwoNorm(H, D[i])) 89 | 90 | # edges for time step K - 1 91 | HT = sp.linalg.block_diag(self.Q, self.R, self.S, np.zeros((self.pwa.nu, self.pwa.nu))) 92 | for i in range(self.pwa.nm): 93 | graph.add_edge((self.K - 1, i), (self.K), SquaredTwoNorm(HT, D[i])) 94 | 95 | return graph 96 | 97 | def solve(self): 98 | '''if relaxation returns approximate value for states and controls.''' 99 | 100 | # solve shortest path problem 101 | sol = self.spp.solve() 102 | 103 | # initialize state, controls, and mode sequence 104 | z = np.full((self.K, self.pwa.nz), np.nan) 105 | u = np.full((self.K - 1, self.pwa.nu), np.nan) 106 | ms = np.full(self.K - 1, np.nan) 107 | 108 | # time step 1 109 | E_out = self.spp.graph.outgoing_edges(0)[1] 110 | zu = sum(sol.primal.z[E_out]) 111 | z[0], u[0] = np.split(zu, (self.pwa.nz,)) 112 | 113 | # all the remaining time steps 114 | for k in range(1, self.K): 115 | E_out = [e for i in range(self.pwa.nm) for e in self.spp.graph.outgoing_edges((k, i))[1]] 116 | zu = sum(sol.primal.z[E_out]) 117 | if k < self.K - 1: 118 | z[k], u[k] = np.split(zu, (self.pwa.nz,)) 119 | z[self.K - 1] = zu[:self.pwa.nz] 120 | 121 | # reconstruct mode sequence 122 | if not self.spp.relaxation: 123 | for j, edge in enumerate(self.spp.graph.edges): 124 | if edge[1] != self.spp.graph.target: 125 | if np.isclose(sol.primal.phi[j], 1): 126 | ms[edge[1][0] - 1] = edge[1][1] 127 | 128 | return RegulationSolution(z, u, ms, sol) 129 | -------------------------------------------------------------------------------- /spp/shortest_path.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from pydrake.all import MathematicalProgram, MosekSolver, eq 3 | class ShortestPathVariables(): 4 | 5 | def __init__(self, phi, y, z, l, x=None): 6 | 7 | self.phi = phi 8 | self.y = y 9 | self.z = z 10 | self.l = l 11 | self.x = x 12 | 13 | def reconstruct_x(self, graph): 14 | 15 | self.x = np.zeros((graph.n_sets, graph.dimension)) 16 | for i, vertex in enumerate(graph.sets): 17 | 18 | if vertex == graph.target: 19 | edges_in = graph.incoming_edges(vertex)[1] 20 | self.x[i] = sum(self.z[edges_in]) 21 | 22 | else: 23 | edges_out = graph.outgoing_edges(vertex)[1] 24 | self.x[i] = sum(self.y[edges_out]) 25 | 26 | if vertex != graph.source: 27 | center = graph.sets[vertex].center 28 | self.x[i] += (1 - sum(self.phi[edges_out])) * center 29 | 30 | @staticmethod 31 | def populate_program(prog, graph, relaxation=False): 32 | 33 | phi_type = prog.NewContinuousVariables if relaxation else prog.NewBinaryVariables 34 | phi = phi_type(graph.n_edges) 35 | y = prog.NewContinuousVariables(graph.n_edges, graph.dimension) 36 | z = prog.NewContinuousVariables(graph.n_edges, graph.dimension) 37 | l = prog.NewContinuousVariables(graph.n_edges) 38 | 39 | return ShortestPathVariables(phi, y, z, l) 40 | 41 | @staticmethod 42 | def from_result(result, vars): 43 | 44 | phi = result.GetSolution(vars.phi) 45 | y = result.GetSolution(vars.y) 46 | z = result.GetSolution(vars.z) 47 | l = result.GetSolution(vars.l) 48 | 49 | return ShortestPathVariables(phi, y, z, l) 50 | 51 | class ShortestPathConstraints(): 52 | 53 | def __init__(self, cons, deg, sp_cons, obj=None): 54 | 55 | # not all constraints of the spp are stored here 56 | # only the ones we care of (the linear ones) 57 | self.conservation = cons 58 | self.degree = deg 59 | self.spatial_conservation = sp_cons 60 | self.objective = obj 61 | 62 | @staticmethod 63 | def populate_program(prog, graph, vars): 64 | 65 | # containers for the constraints we want to keep track of 66 | cons = [] 67 | deg = [] 68 | sp_cons = [] 69 | 70 | for vertex, set in graph.sets.items(): 71 | 72 | edges_in, k_in = graph.incoming_edges(vertex) 73 | edges_out, k_out = graph.outgoing_edges(vertex) 74 | 75 | phi_in = sum(vars.phi[k_in]) 76 | phi_out = sum(vars.phi[k_out]) 77 | y_out = sum(vars.y[k_out]) 78 | z_in = sum(vars.z[k_in]) 79 | 80 | delta_sv = 1 if vertex == graph.source else 0 81 | delta_tv = 1 if vertex == graph.target else 0 82 | 83 | # conservation of flow 84 | if len(edges_in) > 0 or len(edges_out) > 0: 85 | residual = phi_out + delta_tv - phi_in - delta_sv 86 | cons.append(prog.AddLinearConstraint(residual == 0)) 87 | 88 | # spatial conservation of flow 89 | if vertex not in (graph.source, graph.target): 90 | residual = y_out - z_in 91 | sp_cons.append(prog.AddLinearConstraint(eq(residual, 0))) 92 | 93 | # degree constraints 94 | if len(edges_out) > 0: 95 | residual = phi_out + delta_tv - 1 96 | deg.append(prog.AddLinearConstraint(residual <= 0)) 97 | 98 | # # subtour elimination for two-cycles 99 | # if cyclic and vertex not in [graph.source, graph.target]: 100 | # for edge1, k1 in zip(edges_in, k_in): 101 | # edge2 = edge1[::-1] 102 | # if edge2 in edges_out: 103 | # k2 = k_out[edges_out.index(edge2)] 104 | # phi_v = phi_in - vars.phi[k1] - vars.phi[k2] 105 | # prog.AddLinearConstraint(phi_v >= 0) 106 | # graph.sets[vertex].add_perspective_constraint(prog, 107 | # phi_v, z_in - vars.z[k1] - vars.y[k2]) 108 | 109 | # spatial nonnegativity (not stored) 110 | for k, edge in enumerate(graph.edges): 111 | graph.sets[edge[0]].add_perspective_constraint(prog, vars.phi[k], vars.y[k]) 112 | graph.sets[edge[1]].add_perspective_constraint(prog, vars.phi[k], vars.z[k]) 113 | 114 | # slack constraints for the objetive (not stored) 115 | yz = np.concatenate((vars.y[k], vars.z[k])) 116 | graph.lengths[edge].add_perspective_constraint(prog, vars.l[k], vars.phi[k], yz) 117 | 118 | return ShortestPathConstraints(cons, deg, sp_cons) 119 | 120 | @staticmethod 121 | def from_result(result, constraints): 122 | 123 | def get_dual(result, constraints): 124 | dual = np.array([result.GetDualSolution(c) for c in constraints]) 125 | if dual.shape[1] == 1: 126 | return dual.flatten() 127 | return dual 128 | 129 | cons = get_dual(result, constraints.conservation) 130 | deg = get_dual(result, constraints.degree) 131 | sp_cons = get_dual(result, constraints.spatial_conservation) 132 | obj = cons[0] - cons[-1] + sum(deg[:-1]) 133 | 134 | return ShortestPathConstraints(cons, deg, sp_cons, obj) 135 | 136 | class ShortestPathSolution(): 137 | 138 | def __init__(self, cost, time, primal, dual): 139 | 140 | self.cost = cost 141 | self.time = time 142 | self.primal = primal 143 | self.dual = dual 144 | 145 | class ShortestPathProblem(): 146 | 147 | def __init__(self, graph, relaxation=False): 148 | 149 | self.graph = graph 150 | self.relaxation = relaxation 151 | 152 | self.prog = MathematicalProgram() 153 | self.vars = ShortestPathVariables.populate_program(self.prog, graph, relaxation) 154 | self.constraints = ShortestPathConstraints.populate_program(self.prog, graph, self.vars) 155 | self.prog.AddLinearCost(sum(self.vars.l)) 156 | 157 | def solve(self): 158 | 159 | result = MosekSolver().Solve(self.prog) 160 | cost = result.get_optimal_cost() 161 | time = result.get_solver_details().optimizer_time 162 | primal = ShortestPathVariables.from_result(result, self.vars) 163 | primal.reconstruct_x(self.graph) 164 | # dual = ShortestPathConstraints.from_result(result, self.constraints) if self.relaxation else None 165 | dual = None 166 | 167 | return ShortestPathSolution(cost, time, primal, dual) 168 | -------------------------------------------------------------------------------- /spp/shortest_path_convex_hull.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from pydrake.all import MathematicalProgram, MosekSolver, eq, ge 3 | 4 | class ShortestPathVariables(): 5 | 6 | def __init__(self, phi, y, z, l, x): 7 | 8 | self.phi = phi 9 | self.y = y 10 | self.z = z 11 | self.l = l 12 | self.x = x 13 | 14 | @staticmethod 15 | def populate_program(prog, graph): 16 | 17 | phi = prog.NewContinuousVariables(graph.n_edges) 18 | y = prog.NewContinuousVariables(graph.n_edges, graph.dimension) 19 | z = prog.NewContinuousVariables(graph.n_edges, graph.dimension) 20 | l = prog.NewContinuousVariables(graph.n_edges) 21 | x = prog.NewContinuousVariables(graph.n_sets, graph.dimension) 22 | 23 | return ShortestPathVariables(phi, y, z, l, x) 24 | 25 | @staticmethod 26 | def from_result(result, vars): 27 | 28 | phi = result.GetSolution(vars.phi) 29 | y = result.GetSolution(vars.y) 30 | z = result.GetSolution(vars.z) 31 | l = result.GetSolution(vars.l) 32 | x = result.GetSolution(vars.x) 33 | 34 | return ShortestPathVariables(phi, y, z, l, x) 35 | 36 | class ShortestPathConstraints(): 37 | 38 | @staticmethod 39 | def populate_program(prog, graph, vars, relaxation=False): 40 | 41 | # loop through the vertices, not source nor target 42 | for vertex, set in graph.sets.items(): 43 | if vertex != graph.source and vertex != graph.target: 44 | 45 | # indices of the edges incident with this vertex 46 | edges_in = graph.incoming_edges(vertex)[1] 47 | edges_out = graph.outgoing_edges(vertex)[1] 48 | 49 | # auxiliary flow variables 50 | if relaxation: 51 | alpha = prog.NewContinuousVariables(len(edges_in), len(edges_out)) 52 | prog.AddLinearConstraint(ge(alpha.flatten(), 0)) 53 | else: 54 | alpha = prog.NewBinaryVariables(len(edges_in), len(edges_out)) 55 | prog.AddLinearConstraint(alpha.sum() <= 1) 56 | 57 | # relate flows phi and alpha 58 | for j, k in enumerate(edges_in): 59 | prog.AddLinearConstraint(vars.phi[k] == alpha[j].sum()) 60 | for j, k in enumerate(edges_out): 61 | prog.AddLinearConstraint(vars.phi[k] == alpha[:, j].sum()) 62 | 63 | # auxiliary copies of the vertex positions 64 | x_aux = np.array([prog.NewContinuousVariables(len(edges_out), graph.dimension) for _ in edges_in]) 65 | for j, k in enumerate(edges_in): 66 | prog.AddLinearConstraint(eq(vars.z[k], x_aux[j].sum(axis=0))) 67 | for j, k in enumerate(edges_out): 68 | prog.AddLinearConstraint(eq(vars.y[k], x_aux[:, j].sum(axis=0))) 69 | 70 | # relate vertex positions and auxiliary variables 71 | i = graph.vertex_index(vertex) 72 | argument = vars.x[i] - x_aux.sum(axis=0).sum(axis=0) 73 | scaling = 1 - alpha.sum() 74 | set.add_perspective_constraint(prog, scaling, argument) 75 | 76 | # membership of the auxiliary variables 77 | for j in range(len(edges_in)): 78 | for k in range(len(edges_out)): 79 | set.add_perspective_constraint(prog, alpha[j, k], x_aux[j, k]) 80 | 81 | # source 82 | edges_in = graph.incoming_edges(graph.source)[1] 83 | edges_out = graph.outgoing_edges(graph.source)[1] 84 | 85 | for k in edges_in: 86 | prog.AddLinearConstraint(vars.phi[k] == 0) 87 | prog.AddLinearConstraint(eq(vars.z[k], 0)) 88 | 89 | s = graph.vertex_index(graph.source) 90 | prog.AddLinearConstraint(sum(vars.phi[edges_out]) == 1) 91 | prog.AddLinearConstraint(eq(vars.x[s], sum(vars.y[edges_out]))) 92 | for k in edges_out: 93 | graph.source_set.add_perspective_constraint(prog, vars.phi[k], vars.y[k]) 94 | 95 | # target 96 | edges_in = graph.incoming_edges(graph.target)[1] 97 | edges_out = graph.outgoing_edges(graph.target)[1] 98 | 99 | for k in edges_out: 100 | prog.AddLinearConstraint(vars.phi[k] == 0) 101 | prog.AddLinearConstraint(eq(vars.y[k], 0)) 102 | 103 | t = graph.vertex_index(graph.target) 104 | prog.AddLinearConstraint(sum(vars.phi[edges_in]) == 1) 105 | prog.AddLinearConstraint(eq(vars.x[t], sum(vars.z[edges_in]))) 106 | for k in edges_in: 107 | graph.target_set.add_perspective_constraint(prog, vars.phi[k], vars.z[k]) 108 | 109 | # cost function 110 | for k, edge in enumerate(graph.edges): 111 | yz = np.concatenate((vars.y[k], vars.z[k])) 112 | graph.lengths[edge].add_perspective_constraint(prog, vars.l[k], vars.phi[k], yz) 113 | 114 | class ShortestPathSolution(): 115 | 116 | def __init__(self, cost, time, primal): 117 | 118 | self.cost = cost 119 | self.time = time 120 | self.primal = primal 121 | self.dual = None 122 | 123 | class ShortestPathProblem(): 124 | 125 | def __init__(self, graph, relaxation=False): 126 | 127 | self.graph = graph 128 | self.relaxation = relaxation 129 | 130 | self.prog = MathematicalProgram() 131 | self.vars = ShortestPathVariables.populate_program(self.prog, graph) 132 | self.constraints = ShortestPathConstraints.populate_program(self.prog, graph, self.vars, relaxation) 133 | self.prog.AddLinearCost(sum(self.vars.l)) 134 | 135 | def solve(self): 136 | 137 | result = MosekSolver().Solve(self.prog) 138 | cost = result.get_optimal_cost() 139 | time = result.get_solver_details().optimizer_time 140 | primal = ShortestPathVariables.from_result(result, self.vars) 141 | 142 | return ShortestPathSolution(cost, time, primal) 143 | -------------------------------------------------------------------------------- /spp/shortest_path_edgewise.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from pydrake.all import MathematicalProgram, MosekSolver 3 | 4 | class ShortestPathVariables(): 5 | 6 | def __init__(self, phi, y, z, l, x): 7 | 8 | self.phi = phi 9 | self.y = y 10 | self.z = z 11 | self.l = l 12 | self.x = x 13 | 14 | @staticmethod 15 | def populate_program(prog, graph, relaxation=False): 16 | 17 | phi_type = prog.NewContinuousVariables if relaxation else prog.NewBinaryVariables 18 | phi = phi_type(graph.n_edges) 19 | y = prog.NewContinuousVariables(graph.n_edges, graph.dimension) 20 | z = prog.NewContinuousVariables(graph.n_edges, graph.dimension) 21 | l = prog.NewContinuousVariables(graph.n_edges) 22 | x = prog.NewContinuousVariables(graph.n_sets, graph.dimension) 23 | 24 | return ShortestPathVariables(phi, y, z, l, x) 25 | 26 | @staticmethod 27 | def from_result(result, vars): 28 | 29 | phi = result.GetSolution(vars.phi) 30 | y = result.GetSolution(vars.y) 31 | z = result.GetSolution(vars.z) 32 | l = result.GetSolution(vars.l) 33 | x = result.GetSolution(vars.x) 34 | 35 | return ShortestPathVariables(phi, y, z, l, x) 36 | 37 | class ShortestPathConstraints(): 38 | 39 | @staticmethod 40 | def populate_program(prog, graph, vars): 41 | 42 | # loop through the vertices 43 | for v, Xv in graph.sets.items(): 44 | 45 | # indices of the edges incident with this vertex 46 | edges_in = graph.incoming_edges(v)[1] 47 | edges_out = graph.outgoing_edges(v)[1] 48 | 49 | # incident flow variables 50 | phi_in = sum(vars.phi[edges_in]) 51 | phi_out = sum(vars.phi[edges_out]) 52 | 53 | # indicators for source and target 54 | delta_sv = 1 if v == graph.source else 0 55 | delta_tv = 1 if v == graph.target else 0 56 | 57 | # conservation of flow 58 | if len(edges_in) > 0 or len(edges_out) > 0: 59 | residual = phi_out + delta_tv - phi_in - delta_sv 60 | prog.AddLinearConstraint(residual == 0) 61 | 62 | # degree constraint 63 | if len(edges_out) > 0: 64 | residual = phi_out + delta_tv - 1 65 | prog.AddLinearConstraint(residual <= 0) 66 | 67 | # loop through the edges 68 | for k, e in enumerate(graph.edges): 69 | 70 | # spatial nonnegativity 71 | Xu, Xv = [graph.sets[v] for v in e] 72 | Xu.add_perspective_constraint(prog, vars.phi[k], vars.y[k]) 73 | Xv.add_perspective_constraint(prog, vars.phi[k], vars.z[k]) 74 | 75 | # spatial upper bound 76 | xu, xv = vars.x[graph.vertex_indices(e)] 77 | Xu.add_perspective_constraint(prog, 1 - vars.phi[k], xu - vars.y[k]) 78 | Xv.add_perspective_constraint(prog, 1 - vars.phi[k], xv - vars.z[k]) 79 | 80 | # slack constraints for the objetive 81 | yz = np.concatenate((vars.y[k], vars.z[k])) 82 | graph.lengths[e].add_perspective_constraint(prog, vars.l[k], vars.phi[k], yz) 83 | 84 | class ShortestPathSolution(): 85 | 86 | def __init__(self, cost, time, primal): 87 | 88 | self.cost = cost 89 | self.time = time 90 | self.primal = primal 91 | self.dual = None 92 | 93 | class ShortestPathProblem(): 94 | 95 | def __init__(self, graph, relaxation=False): 96 | 97 | self.graph = graph 98 | self.relaxation = relaxation 99 | 100 | self.prog = MathematicalProgram() 101 | self.vars = ShortestPathVariables.populate_program(self.prog, graph, relaxation) 102 | self.constraints = ShortestPathConstraints.populate_program(self.prog, graph, self.vars) 103 | self.prog.AddLinearCost(sum(self.vars.l)) 104 | 105 | def solve(self): 106 | 107 | # fixes Mosek's bug 108 | import mosek 109 | solver = MosekSolver() 110 | self.prog.SetSolverOption(solver.solver_id(), 'MSK_IPAR_INTPNT_SOLVE_FORM', mosek.solveform.primal) 111 | 112 | result = solver.Solve(self.prog) 113 | cost = result.get_optimal_cost() 114 | time = result.get_solver_details().optimizer_time 115 | primal = ShortestPathVariables.from_result(result, self.vars) 116 | 117 | return ShortestPathSolution(cost, time, primal) 118 | -------------------------------------------------------------------------------- /spp/shortest_path_mccormick.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from pydrake.all import MathematicalProgram, MosekSolver, eq 3 | 4 | class ShortestPathVariables(): 5 | 6 | def __init__(self, phi, x, y, z, l): 7 | 8 | self.phi = phi 9 | self.x = x 10 | self.y = y 11 | self.z = z 12 | self.l = l 13 | 14 | @staticmethod 15 | def populate_program(prog, graph, relaxation=False): 16 | 17 | phi_type = prog.NewContinuousVariables if relaxation else prog.NewBinaryVariables 18 | phi = phi_type(graph.n_edges) 19 | x = prog.NewContinuousVariables(graph.n_sets, graph.dimension) 20 | y = prog.NewContinuousVariables(graph.n_edges, graph.dimension) 21 | z = prog.NewContinuousVariables(graph.n_edges, graph.dimension) 22 | l = prog.NewContinuousVariables(graph.n_edges) 23 | 24 | return ShortestPathVariables(phi, x, y, z, l) 25 | 26 | @staticmethod 27 | def from_result(result, vars): 28 | 29 | phi = result.GetSolution(vars.phi) 30 | x = result.GetSolution(vars.x) 31 | y = result.GetSolution(vars.y) 32 | z = result.GetSolution(vars.z) 33 | l = result.GetSolution(vars.l) 34 | 35 | return ShortestPathVariables(phi, x, y, z, l) 36 | 37 | def populate_constraints(prog, graph, vars): 38 | 39 | bounding_boxes = {} 40 | 41 | for vertex, set in graph.sets.items(): 42 | 43 | v = graph.vertex_index(vertex) 44 | bounding_boxes[vertex] = set.bounding_box() 45 | set.add_membership_constraint(prog, vars.x[v]) 46 | 47 | edges_in, k_in = graph.incoming_edges(vertex) 48 | edges_out, k_out = graph.outgoing_edges(vertex) 49 | 50 | phi_in = sum(vars.phi[k_in]) 51 | phi_out = sum(vars.phi[k_out]) 52 | y_out = sum(vars.y[k_out]) 53 | z_in = sum(vars.z[k_in]) 54 | 55 | delta_sv = 1 if vertex == graph.source else 0 56 | delta_tv = 1 if vertex == graph.target else 0 57 | 58 | # conservation of flow 59 | if len(edges_in) > 0 or len(edges_out) > 0: 60 | residual = phi_out + delta_tv - phi_in - delta_sv 61 | prog.AddLinearConstraint(residual == 0) 62 | 63 | for k, edge in enumerate(graph.edges): 64 | 65 | Bu = bounding_boxes[edge[0]] 66 | Bv = bounding_boxes[edge[1]] 67 | # Bu = graph.sets[edge[0]] 68 | # Bv = graph.sets[edge[1]] 69 | u = graph.vertex_index(edge[0]) 70 | v = graph.vertex_index(edge[1]) 71 | 72 | Bu.add_perspective_constraint(prog, vars.phi[k], vars.y[k]) 73 | Bv.add_perspective_constraint(prog, vars.phi[k], vars.z[k]) 74 | Bu.add_perspective_constraint(prog, 1 - vars.phi[k], vars.x[u] - vars.y[k]) 75 | Bv.add_perspective_constraint(prog, 1 - vars.phi[k], vars.x[v] - vars.z[k]) 76 | 77 | # slack constraints for the objetive (not stored) 78 | yz = np.concatenate((vars.y[k], vars.z[k])) 79 | graph.lengths[edge].add_perspective_constraint(prog, vars.l[k], vars.phi[k], yz) 80 | 81 | class ShortestPathSolution(): 82 | 83 | def __init__(self, cost, time, primal): 84 | 85 | self.cost = cost 86 | self.time = time 87 | self.primal = primal 88 | 89 | class ShortestPathProblem(): 90 | 91 | def __init__(self, graph, relaxation=False): 92 | 93 | self.graph = graph 94 | self.relaxation = relaxation 95 | 96 | self.prog = MathematicalProgram() 97 | self.vars = ShortestPathVariables.populate_program(self.prog, graph, relaxation) 98 | populate_constraints(self.prog, graph, self.vars) 99 | self.prog.AddLinearCost(sum(self.vars.l)) 100 | 101 | def solve(self): 102 | 103 | from pydrake.solvers.mathematicalprogram import CommonSolverOption, SolverOptions 104 | solver_options = SolverOptions() 105 | solver_options.SetOption(CommonSolverOption.kPrintFileName, 'mosek_log.txt') 106 | 107 | result = MosekSolver().Solve(self.prog, solver_options=solver_options) 108 | cost = result.get_optimal_cost() 109 | time = result.get_solver_details().optimizer_time 110 | primal = ShortestPathVariables.from_result(result, self.vars) 111 | 112 | return ShortestPathSolution(cost, time, primal) 113 | -------------------------------------------------------------------------------- /statistical_analysis.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "%matplotlib notebook\n", 10 | "%load_ext autoreload\n", 11 | "%autoreload 2" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "import numpy as np\n", 21 | "import random as rd\n", 22 | "from copy import deepcopy\n", 23 | "from spp.convex_sets import Singleton, Polyhedron\n", 24 | "from spp.convex_functions import TwoNorm, SquaredTwoNorm\n", 25 | "from spp.graph import GraphOfConvexSets\n", 26 | "from spp.shortest_path import ShortestPathProblem\n", 27 | "from spp.shortest_path_mccormick import ShortestPathProblem as ShortestPathProblemMC" 28 | ] 29 | }, 30 | { 31 | "cell_type": "code", 32 | "execution_count": null, 33 | "metadata": {}, 34 | "outputs": [], 35 | "source": [ 36 | "def shuffle(l):\n", 37 | " '''\n", 38 | " Shuffles the given list randomly.\n", 39 | " '''\n", 40 | " return rd.sample(list(l), k=len(l))\n", 41 | "\n", 42 | "def split(l, n):\n", 43 | " '''\n", 44 | " Splits the given list in n sublists of random length.\n", 45 | " '''\n", 46 | " assert 1 <= n <= len(l)\n", 47 | " split_if = shuffle([True] * (n - 1) + [False] * (len(l) - n))\n", 48 | " split_at = np.where([True] + split_if + [True])[0]\n", 49 | " return [l[i:j] for i, j in zip(split_at[:-1], split_at[1:])]\n", 50 | "\n", 51 | "def partition(l):\n", 52 | " '''\n", 53 | " Partitions the given list in n sublists of random length.\n", 54 | " n is drawn uniformly at random from [1, len(l)].\n", 55 | " '''\n", 56 | " n = rd.randint(1, len(l))\n", 57 | " return split(l, n)\n", 58 | "\n", 59 | "def paths(V):\n", 60 | " '''\n", 61 | " Generates random paths by partitioning the set V - {s,t}.\n", 62 | " The number of paths (sets in the partition) is random.\n", 63 | " The number of vertices traversed by each path is random too.\n", 64 | " '''\n", 65 | " return [[V[0]] + p + [V[-1]] for p in partition(V[1:-1])]\n", 66 | "\n", 67 | "def edges_in_path(p):\n", 68 | " '''\n", 69 | " Returns the edges in the given path (list of vertices).\n", 70 | " '''\n", 71 | " return list(zip(p[:-1], p[1:]))\n", 72 | "\n", 73 | "def edges_in_paths(ps):\n", 74 | " '''\n", 75 | " Returns all the edges in the given list of paths.\n", 76 | " No edge is repeated.\n", 77 | " '''\n", 78 | " return list(set(e for p in ps for e in edges_in_path(p)))\n", 79 | "\n", 80 | "def extend_edges(E, V, nE):\n", 81 | " '''\n", 82 | " Extends the given edge set until it contains nE edges.\n", 83 | " Does not add self edges and does not generate repetitions.\n", 84 | " '''\n", 85 | " E = deepcopy(E)\n", 86 | " while len(E) < nE:\n", 87 | " i = rd.choice(V)\n", 88 | " j = rd.choice(V)\n", 89 | " e = (i, j)\n", 90 | " if i != j and not e in E:\n", 91 | " E.append(e)\n", 92 | " return E\n", 93 | " \n", 94 | "def box(d, vol):\n", 95 | " '''\n", 96 | " Returns an axis-aligned box in R^d of the given volume.\n", 97 | " The center of the box is drawn uniformly at random in [0,1]^d.\n", 98 | " '''\n", 99 | " center = np.array([rd.uniform(0, 1) for i in range(d)])\n", 100 | " side = .5 * vol ** (1 / d) * np.ones(d)\n", 101 | " x_min = center - side\n", 102 | " x_max = center + side\n", 103 | " return Polyhedron.from_bounds(x_min, x_max)\n", 104 | " \n", 105 | "def get_graph(params, l=None):\n", 106 | " '''\n", 107 | " Constructs a random instance of the SPP.\n", 108 | " '''\n", 109 | " assert params['nE'] >= params['nV'] - 1\n", 110 | " \n", 111 | " # graph\n", 112 | " G = GraphOfConvexSets()\n", 113 | " \n", 114 | " # convex sets\n", 115 | " Xs = Singleton(np.zeros(params['d']))\n", 116 | " Xt = Singleton(np.ones(params['d']))\n", 117 | " X = [Xs]\n", 118 | " for v in range(params['nV'] - 2):\n", 119 | " Xv = box(params['d'], params['vol'])\n", 120 | " X.append(Xv)\n", 121 | " X.append(Xt)\n", 122 | " V = list(range(params['nV']))\n", 123 | " G.add_sets(X, V)\n", 124 | " G.set_source(0)\n", 125 | " G.set_target(params['nV'] - 1)\n", 126 | " \n", 127 | " # edges\n", 128 | " E = edges_in_paths(paths(V))\n", 129 | " E = extend_edges(E, V, params['nE'])\n", 130 | " for e in E:\n", 131 | " G.add_edge(e[0], e[1], l)\n", 132 | " \n", 133 | " return G" 134 | ] 135 | }, 136 | { 137 | "cell_type": "code", 138 | "execution_count": null, 139 | "metadata": {}, 140 | "outputs": [], 141 | "source": [ 142 | "def solve(params, n_trials):\n", 143 | " \n", 144 | " I = np.eye(params['d'])\n", 145 | " H = np.hstack((I, -I))\n", 146 | " l = {'E': TwoNorm(H), 'E2': SquaredTwoNorm(H)}\n", 147 | " \n", 148 | " cost = {\n", 149 | " 0: {'E': np.zeros(n_trials), 'E2': np.zeros(n_trials)},\n", 150 | " 1: {'E': np.zeros(n_trials), 'E2': np.zeros(n_trials)}\n", 151 | " }\n", 152 | " time = deepcopy(cost)\n", 153 | " \n", 154 | " for i in range(n_trials):\n", 155 | " G = get_graph(params)\n", 156 | " print(f'Trial {i}', end ='\\r')\n", 157 | " for relaxation in [0, 1]:\n", 158 | " for norm in ['E', 'E2']:\n", 159 | " for e in G.edges:\n", 160 | " G.set_edge_length(e, l[norm]) \n", 161 | " spp = ShortestPathProblem(G, relaxation=relaxation)\n", 162 | " sol = spp.solve()\n", 163 | " cost[relaxation][norm][i] = sol.cost\n", 164 | " time[relaxation][norm][i] = sol.time\n", 165 | " \n", 166 | " return cost, time\n", 167 | "\n", 168 | "def set_stat(stat, values):\n", 169 | " stat['mean'] = np.mean(values)\n", 170 | " stat['median'] = np.median(values)\n", 171 | " stat['max'] = max(values)\n", 172 | "\n", 173 | "def rel_gap_stat(cost):\n", 174 | " stat = {'E': {}, 'E2': {}}\n", 175 | " for norm in ['E', 'E2']:\n", 176 | " gaps = 1 - cost[1][norm] / cost[0][norm]\n", 177 | " set_stat(stat[norm], gaps)\n", 178 | " return stat\n", 179 | "\n", 180 | "def time_stat(time):\n", 181 | " stat = {\n", 182 | " 0: {'E': {}, 'E2': {}},\n", 183 | " 1: {'E': {}, 'E2': {}}\n", 184 | " }\n", 185 | " for relaxation in [0, 1]:\n", 186 | " for norm in ['E', 'E2']:\n", 187 | " set_stat(stat[relaxation][norm], time[relaxation][norm])\n", 188 | " return stat\n", 189 | "\n", 190 | "tab = lambda n : ' ' * n\n", 191 | "\n", 192 | "def print_rel_gap(stat, label):\n", 193 | " print(label + ', relaxation gap:')\n", 194 | " for norm in ['E', 'E2']:\n", 195 | " print(tab(1), 'Norm:', norm)\n", 196 | " for key, value in stat[norm].items():\n", 197 | " print(tab(2), key, value)\n", 198 | "\n", 199 | "def print_time(stat, label):\n", 200 | " print(label + ', time:')\n", 201 | " for relaxation in [0, 1]:\n", 202 | " print(tab(1), 'Relaxation:', relaxation)\n", 203 | " for norm in ['E', 'E2']:\n", 204 | " print(tab(2), 'Norm:', norm)\n", 205 | " for key, value in stat[relaxation][norm].items():\n", 206 | " print(tab(3), key, value)\n", 207 | " \n", 208 | "def save(cost, time, params, name):\n", 209 | " import pickle\n", 210 | " solution = {}\n", 211 | " solution['cost'] = cost\n", 212 | " solution['time'] = time\n", 213 | " solution['parmas'] = params\n", 214 | "# pickle.dump(solution, open( \"save.p\", \"wb\" ) )\n", 215 | " np.save('solution_' + name, solution)" 216 | ] 217 | }, 218 | { 219 | "cell_type": "code", 220 | "execution_count": null, 221 | "metadata": {}, 222 | "outputs": [], 223 | "source": [ 224 | "n_trials = 100\n", 225 | "params_nom = {\n", 226 | " 'd': 4,\n", 227 | " 'nE': 100,\n", 228 | " 'nV': 50,\n", 229 | " 'vol': .01\n", 230 | "}\n", 231 | "rd.seed(0)\n", 232 | "cost_nom, time_nom = solve(params_nom, n_trials)\n", 233 | "print_rel_gap(rel_gap_stat(cost_nom), 'Nominal')\n", 234 | "print_time(time_stat(time_nom), 'Nominal')\n", 235 | "save(cost_nom, time_nom, params_nom, 'nom')" 236 | ] 237 | }, 238 | { 239 | "cell_type": "code", 240 | "execution_count": null, 241 | "metadata": { 242 | "scrolled": true 243 | }, 244 | "outputs": [], 245 | "source": [ 246 | "params_max = [\n", 247 | " {'d': 20},\n", 248 | " {'nE': 500},\n", 249 | " {'nV': 250, 'nE': 500},\n", 250 | " {'vol': .05}\n", 251 | "]\n", 252 | "results = {}\n", 253 | "for params_change in params_max:\n", 254 | " rd.seed(0)\n", 255 | " params = deepcopy(params_nom)\n", 256 | " for p, v in params_change.items():\n", 257 | " params[p] = v\n", 258 | " cost, time = solve(params, n_trials)\n", 259 | " label = ' '.join(params_change.keys())\n", 260 | " results[label] = [cost, time]\n", 261 | " print_rel_gap(rel_gap_stat(cost), label)\n", 262 | " print_time(time_stat(time), label)\n", 263 | " label = '_'.join(params_change.keys())\n", 264 | " save(cost, time, params, label)\n", 265 | " print('\\n')" 266 | ] 267 | }, 268 | { 269 | "cell_type": "markdown", 270 | "metadata": {}, 271 | "source": [ 272 | "# Plot results" 273 | ] 274 | }, 275 | { 276 | "cell_type": "code", 277 | "execution_count": null, 278 | "metadata": {}, 279 | "outputs": [], 280 | "source": [ 281 | "import numpy as np\n", 282 | "import matplotlib.pyplot as plt\n", 283 | "labels = {\n", 284 | " 'nom': 'Nominal',\n", 285 | " 'vol': r'$\\Lambda = 0.05$',\n", 286 | " 'd': r'$d = 20$',\n", 287 | " 'nE': r'$|\\mathcal{E}| = 500$',\n", 288 | " 'nV_nE': r'$|\\mathcal{V}| = 250$, $|\\mathcal{E}| = 500$',\n", 289 | "}\n", 290 | "labels = {\n", 291 | " 'nom': 'Nominal',\n", 292 | " 'vol': 'Large sets',\n", 293 | " 'd': 'High dimensions',\n", 294 | " 'nE': 'Dense graph',\n", 295 | " 'nV_nE': 'Large graph'\n", 296 | "}\n", 297 | "markers = {\n", 298 | " 'nom': 'o',\n", 299 | " 'vol': '^',\n", 300 | " 'd': 'v',\n", 301 | " 'nE': '<',\n", 302 | " 'nV_nE': '>',\n", 303 | "}\n", 304 | "titles = {\n", 305 | " 'E': 'Euclidean distance',\n", 306 | " 'E2': 'Euclidean distance squared',\n", 307 | "}\n", 308 | "fig = plt.figure(figsize=(5, 4))\n", 309 | "i = 0\n", 310 | "norm = 'E2'\n", 311 | "for label in labels.keys():\n", 312 | " solution = np.load('solution_' + label + '.npy', allow_pickle=True)[()]\n", 313 | " cost = solution['cost']\n", 314 | " time = solution['time']\n", 315 | " gaps = (1 - cost[1][norm] / cost[0][norm]) * 100\n", 316 | " plt.scatter(gaps, time[0][norm], marker=markers[label], label=labels[label], ec='k', linewidth=.5)\n", 317 | "plt.xlabel('Relaxation gap (%)')\n", 318 | "plt.title(titles[norm])\n", 319 | "plt.grid()\n", 320 | "plt.gca().set_yscale('log')\n", 321 | "plt.gca().set_axisbelow(True)\n", 322 | "plt.ylabel('MICP solve time (s)')\n", 323 | "plt.legend()\n", 324 | "\n", 325 | "# plt.savefig('statistical_analysis' + norm + '.pdf', bbox_inches='tight')" 326 | ] 327 | }, 328 | { 329 | "cell_type": "code", 330 | "execution_count": null, 331 | "metadata": {}, 332 | "outputs": [], 333 | "source": [ 334 | "import numpy as np\n", 335 | "import matplotlib.pyplot as plt\n", 336 | "labels = {\n", 337 | " 'nom': 'Nominal',\n", 338 | " 'vol': r'$\\Lambda = 0.05$',\n", 339 | " 'd': r'$d = 20$',\n", 340 | " 'nE': r'$|\\mathcal{E}| = 500$',\n", 341 | " 'nV_nE': r'$|\\mathcal{V}| = 250$, $|\\mathcal{E}| = 500$',\n", 342 | "}\n", 343 | "labels = {\n", 344 | " 'nom': 'Nominal',\n", 345 | " 'vol': 'Large sets',\n", 346 | " 'd': 'High dimensions',\n", 347 | " 'nE': 'Dense graph',\n", 348 | " 'nV_nE': 'Large graph'\n", 349 | "}\n", 350 | "markers = {\n", 351 | " 'nom': 'o',\n", 352 | " 'vol': '^',\n", 353 | " 'd': 'v',\n", 354 | " 'nE': '<',\n", 355 | " 'nV_nE': '>',\n", 356 | "}\n", 357 | "titles = {\n", 358 | " 'E': 'Euclidean distance',\n", 359 | " 'E2': 'Euclidean distance squared',\n", 360 | "}\n", 361 | "fig = plt.figure(figsize=(9.5, 3.4))\n", 362 | "for i, norm in enumerate(['E', 'E2']):\n", 363 | " plt.subplot(1, 2, i + 1)\n", 364 | " for k ,label in enumerate(labels.keys()):\n", 365 | " solution = np.load('solution_' + label + '.npy', allow_pickle=True)[()]\n", 366 | " cost = solution['cost']\n", 367 | " time = solution['time']\n", 368 | " gaps = (1 - cost[1][norm] / cost[0][norm]) * 100\n", 369 | " plt.scatter(gaps, time[0][norm], marker=markers[label],\n", 370 | " label=labels[label], ec='k', linewidth=.5, zorder=-k + 10)\n", 371 | " plt.xlabel('Relaxation gap (%)')\n", 372 | " plt.title(titles[norm])\n", 373 | " plt.grid()\n", 374 | " plt.gca().set_yscale('log')\n", 375 | " plt.gca().set_axisbelow(True)\n", 376 | " plt.yticks([.1, 1, 10, 100])\n", 377 | " plt.ylim([0.04, 300])\n", 378 | " if i == 0:\n", 379 | " plt.ylabel('MICP solution time (s)')\n", 380 | " else:\n", 381 | " plt.gca().set_yticklabels([''] * 4)\n", 382 | " \n", 383 | " plt.legend()\n", 384 | "plt.subplots_adjust(wspace=.05, hspace=0)\n", 385 | " \n", 386 | "# plt.savefig('statistical_analysis.pdf', bbox_inches='tight')" 387 | ] 388 | }, 389 | { 390 | "cell_type": "markdown", 391 | "metadata": {}, 392 | "source": [ 393 | "# Plot 2d projection of 4d instance" 394 | ] 395 | }, 396 | { 397 | "cell_type": "code", 398 | "execution_count": null, 399 | "metadata": { 400 | "scrolled": false 401 | }, 402 | "outputs": [], 403 | "source": [ 404 | "import matplotlib.pyplot as plt\n", 405 | "import matplotlib.patches as patches\n", 406 | "\n", 407 | "def plot_graph(G):\n", 408 | " plt.gca().set_aspect('equal')\n", 409 | "\n", 410 | " # sets\n", 411 | " for i, Xi in enumerate(list(G.sets.values())[1:-1]):\n", 412 | " plt.scatter(*Xi.center, s=15, facecolor='k', alpha=.2)\n", 413 | " Xi.plot(alpha=.5, facecolor='mintcream', edgecolor='k', zorder=0)\n", 414 | "\n", 415 | " # edges\n", 416 | " kw = dict(arrowstyle='->, head_width=3, head_length=8', color='k', linewidth=.2)\n", 417 | " for e in G.edges:\n", 418 | " c0 = G.sets[e[0]].center\n", 419 | " c1 = G.sets[e[1]].center\n", 420 | " plt.gca().add_patch(patches.FancyArrowPatch(c0, c1, **kw))\n", 421 | "\n", 422 | " # start and goal points\n", 423 | " plt.scatter(0, 0, c='k')\n", 424 | " plt.scatter(1, 1, c='k')\n", 425 | " plt.text(-.05, 0, r'$s$', ha='right', va='center', size=14, zorder=3)\n", 426 | " plt.text(1.05, 1, r'$t$', ha='left', va='center', size=14, zorder=3)\n", 427 | " \n", 428 | "rd.seed(0)\n", 429 | "params = {\n", 430 | " 'd': 2,\n", 431 | " 'nE': 100,\n", 432 | " 'nV': 50,\n", 433 | " 'vol': .01 ** (2/4)\n", 434 | "}\n", 435 | "G = get_graph(params)\n", 436 | " \n", 437 | "plt.figure(figsize=(3.5,3.5))\n", 438 | "plt.gca().axis('off')\n", 439 | "plot_graph(G)\n", 440 | "plt.savefig('random_instance.pdf', bbox_inches='tight')" 441 | ] 442 | } 443 | ], 444 | "metadata": { 445 | "kernelspec": { 446 | "display_name": "Python 3 (ipykernel)", 447 | "language": "python", 448 | "name": "python3" 449 | }, 450 | "language_info": { 451 | "codemirror_mode": { 452 | "name": "ipython", 453 | "version": 3 454 | }, 455 | "file_extension": ".py", 456 | "mimetype": "text/x-python", 457 | "name": "python", 458 | "nbconvert_exporter": "python", 459 | "pygments_lexer": "ipython3", 460 | "version": "3.11.2" 461 | } 462 | }, 463 | "nbformat": 4, 464 | "nbformat_minor": 4 465 | } 466 | -------------------------------------------------------------------------------- /statistical_analysis.txt: -------------------------------------------------------------------------------- 1 | Nominal, relaxation gap: 2 | Norm: E 3 | mean 0.0 4 | median -0.0 5 | max 0.0034 6 | Norm: E2 7 | mean 0.0014 8 | median 0.0 9 | max 0.02 10 | Nominal, time: 11 | Relaxation: 0 12 | Norm: E 13 | mean 0.107 14 | median 0.0868 15 | max 0.5026 16 | Norm: E2 17 | mean 0.1335 18 | median 0.0793 19 | max 0.669 20 | Relaxation: 1 21 | Norm: E 22 | mean 0.0224 23 | median 0.0212 24 | max 0.0729 25 | Norm: E2 26 | mean 0.0216 27 | median 0.021 28 | max 0.045 29 | 30 | d, relaxation gap: 31 | Norm: E 32 | mean 0.0 33 | median -0.0 34 | max 0.0021 35 | Norm: E2 36 | mean 0.0911 37 | median 0.0633 38 | max 0.2862 39 | d, time: 40 | Relaxation: 0 41 | Norm: E 42 | mean 0.7877 43 | median 0.5709 44 | max 5.8127 45 | Norm: E2 46 | mean 8.7384 47 | median 2.4696 48 | max 148.0636 49 | Relaxation: 1 50 | Norm: E 51 | mean 0.1263 52 | median 0.116 53 | max 0.4003 54 | Norm: E2 55 | mean 0.0892 56 | median 0.087 57 | max 0.1729 58 | 59 | nE, relaxation gap: 60 | Norm: E 61 | mean 0.0002 62 | median -0.0 63 | max 0.0124 64 | Norm: E2 65 | mean 0.1167 66 | median 0.1089 67 | max 0.249 68 | nE, time: 69 | Relaxation: 0 70 | Norm: E 71 | mean 3.5569 72 | median 2.4838 73 | max 13.6168 74 | Norm: E2 75 | mean 34.7257 76 | median 24.5724 77 | max 203.2021 78 | Relaxation: 1 79 | Norm: E 80 | mean 0.508 81 | median 0.4952 82 | max 1.0326 83 | Norm: E2 84 | mean 0.4276 85 | median 0.4135 86 | max 0.6953 87 | 88 | nV nE, relaxation gap: 89 | Norm: E 90 | mean 0.0 91 | median 0.0 92 | max 0.0025 93 | Norm: E2 94 | mean 0.0014 95 | median 0.0 96 | max 0.0528 97 | nV nE, time: 98 | Relaxation: 0 99 | Norm: E 100 | mean 1.3155 101 | median 1.1537 102 | max 4.7318 103 | Norm: E2 104 | mean 1.2705 105 | median 1.1098 106 | max 5.1592 107 | Relaxation: 1 108 | Norm: E 109 | mean 0.1578 110 | median 0.1437 111 | max 0.3262 112 | Norm: E2 113 | mean 0.1497 114 | median 0.1309 115 | max 0.3169 116 | 117 | vol, relaxation gap: 118 | Norm: E 119 | mean 0.0001 120 | median -0.0 121 | max 0.0056 122 | Norm: E2 123 | mean 0.0073 124 | median 0.0 125 | max 0.0767 126 | vol, time: 127 | Relaxation: 0 128 | Norm: E 129 | mean 0.1395 130 | median 0.1184 131 | max 0.7407 132 | Norm: E2 133 | mean 0.2451 134 | median 0.0851 135 | max 0.8685 136 | Relaxation: 1 137 | Norm: E 138 | mean 0.0242 139 | median 0.0224 140 | max 0.085 141 | Norm: E2 142 | mean 0.0224 143 | median 0.0206 144 | max 0.0533 -------------------------------------------------------------------------------- /stepping_stones.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "%matplotlib notebook\n", 10 | "%load_ext autoreload\n", 11 | "%autoreload 2" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "import numpy as np\n", 21 | "import matplotlib.pyplot as plt\n", 22 | "from spp.convex_sets import Singleton, Polyhedron, CartesianProduct\n", 23 | "from spp.convex_functions import SquaredTwoNorm\n", 24 | "from spp.pwa_systems import PieceWiseAffineSystem, ShortestPathRegulator" 25 | ] 26 | }, 27 | { 28 | "cell_type": "code", 29 | "execution_count": null, 30 | "metadata": {}, 31 | "outputs": [], 32 | "source": [ 33 | "# initial state\n", 34 | "z1 = np.array([.5, -3.5, 0, 0])\n", 35 | "q1 = z1[:2]\n", 36 | "\n", 37 | "# target set\n", 38 | "zK = np.array([6.5, 3.5, 0, 0])\n", 39 | "qK = zK[:2]\n", 40 | "Z = Singleton(zK)\n", 41 | "\n", 42 | "# time horizon\n", 43 | "K = 31\n", 44 | "\n", 45 | "# cost matrices\n", 46 | "q_dot_cost = .2 ** .5\n", 47 | "Q = np.diag([0, 0, q_dot_cost, q_dot_cost])\n", 48 | "R = np.eye(2)\n", 49 | "S = Q # ininfluential\n", 50 | "cost_matrices = (Q, R, S)" 51 | ] 52 | }, 53 | { 54 | "cell_type": "code", 55 | "execution_count": null, 56 | "metadata": {}, 57 | "outputs": [], 58 | "source": [ 59 | "# configuration bounds\n", 60 | "Dq = [\n", 61 | " Polyhedron.from_bounds([0, -4], [1, 3]),\n", 62 | " Polyhedron.from_bounds([1, -6], [3, -5]),\n", 63 | " Polyhedron.from_bounds([1, 4], [2, 5]),\n", 64 | " Polyhedron.from_bounds([3, -4], [4, 4]),\n", 65 | " Polyhedron.from_bounds([5, -5], [6, -4]),\n", 66 | " Polyhedron.from_bounds([4, 5], [6, 6]),\n", 67 | " Polyhedron.from_bounds([6, -3], [7, 4])\n", 68 | "]\n", 69 | "\n", 70 | "# velocity bounds\n", 71 | "qdot_max = np.ones(2) * 1\n", 72 | "qdot_min = - qdot_max\n", 73 | "Dqdot = Polyhedron.from_bounds(qdot_min, qdot_max)\n", 74 | "\n", 75 | "# control bounds\n", 76 | "u_max = np.ones(2) * 1\n", 77 | "u_min = - u_max\n", 78 | "Du = Polyhedron.from_bounds(u_min, u_max)\n", 79 | "\n", 80 | "# pwa domains\n", 81 | "domains = [CartesianProduct((Dqi, Dqdot, Du)) for Dqi in Dq]" 82 | ] 83 | }, 84 | { 85 | "cell_type": "code", 86 | "execution_count": null, 87 | "metadata": {}, 88 | "outputs": [], 89 | "source": [ 90 | "# dynamics\n", 91 | "A = np.array([\n", 92 | " [1, 0, 1, 0],\n", 93 | " [0, 1, 0, 1],\n", 94 | " [0, 0, 1, 0],\n", 95 | " [0, 0, 0, 1]\n", 96 | "])\n", 97 | "B = np.vstack((np.zeros((2, 2)), np.eye(2)))\n", 98 | "Bred = B / 10\n", 99 | "c = np.zeros(4)\n", 100 | "dynamics = [(A, Bred, c) if i in [1, 5] else (A, B, c) for i in range(len(domains))]\n", 101 | "\n", 102 | "# pieceiwse affine system\n", 103 | "pwa = PieceWiseAffineSystem(dynamics, domains)" 104 | ] 105 | }, 106 | { 107 | "cell_type": "code", 108 | "execution_count": null, 109 | "metadata": {}, 110 | "outputs": [], 111 | "source": [ 112 | "# solve optimal control problem\n", 113 | "relaxation = 0\n", 114 | "reg = ShortestPathRegulator(pwa, K, z1, Z, cost_matrices, relaxation=relaxation)\n", 115 | "sol = reg.solve()\n", 116 | "print('Cost:', sol.spp.cost)\n", 117 | "print('Solve time:', sol.spp.time)\n", 118 | "\n", 119 | "# unpack result\n", 120 | "q = sol.z[:, :2]\n", 121 | "u = sol.u" 122 | ] 123 | }, 124 | { 125 | "cell_type": "code", 126 | "execution_count": null, 127 | "metadata": {}, 128 | "outputs": [], 129 | "source": [ 130 | "def plot_terrain(q=None, u=None):\n", 131 | " plt.rc('axes', axisbelow=True)\n", 132 | " plt.gca().set_aspect('equal')\n", 133 | "\n", 134 | " for i, Dqi in enumerate(Dq):\n", 135 | " color = 'lightcoral' if i in [1, 5] else 'lightcyan'\n", 136 | " Dqi.plot(facecolor=color)\n", 137 | " \n", 138 | " plt.scatter(*q1, s=300, c='g', marker='+', zorder=2)\n", 139 | " plt.scatter(*qK, s=300, c='g', marker='x', zorder=2)\n", 140 | " \n", 141 | " if q is not None:\n", 142 | " plt.plot(*q.T, c='k', marker='o', markeredgecolor='k', markerfacecolor='w')\n", 143 | " \n", 144 | " if u is not None:\n", 145 | " for t, ut in enumerate(u):\n", 146 | " plt.arrow(*q[t], *ut, color='b', head_starts_at_zero=0, head_width=.15, head_length=.3)\n", 147 | " \n", 148 | " plt.xlabel(r'$q_1, a_1$')\n", 149 | " plt.ylabel(r'$q_2, a_2$')\n", 150 | " plt.grid(1)" 151 | ] 152 | }, 153 | { 154 | "cell_type": "code", 155 | "execution_count": null, 156 | "metadata": { 157 | "scrolled": false 158 | }, 159 | "outputs": [], 160 | "source": [ 161 | "# plot solution\n", 162 | "plt.figure(figsize=(3, 4))\n", 163 | "plot_terrain(q, u)\n", 164 | "plt.xticks(range(8))\n", 165 | "plt.yticks(range(-6, 7))\n", 166 | "\n", 167 | "# plot transparent triangles\n", 168 | "if relaxation:\n", 169 | " for v in reg.spp.graph.vertices:\n", 170 | " E_out = reg.spp.graph.outgoing_edges(v)[1]\n", 171 | " flow = sum(sol.spp.primal.phi[E_out])\n", 172 | " if not np.isclose(flow, 0):\n", 173 | " qv = sum(sol.spp.primal.y[E_out])[:2] / flow\n", 174 | " plt.scatter(*qv, alpha=flow,\n", 175 | " marker='^', edgecolor='k', facecolor='w', zorder=2)\n", 176 | "# plt.savefig('footstep_micp.pdf', bbox_inches='tight')" 177 | ] 178 | }, 179 | { 180 | "cell_type": "markdown", 181 | "metadata": {}, 182 | "source": [ 183 | "# Baseline formulation" 184 | ] 185 | }, 186 | { 187 | "cell_type": "code", 188 | "execution_count": null, 189 | "metadata": {}, 190 | "outputs": [], 191 | "source": [ 192 | "from pydrake.all import MathematicalProgram, MosekSolver, le, ge, eq\n", 193 | "\n", 194 | "def solve_baseline(relaxation=0):\n", 195 | " \n", 196 | " # initialize program\n", 197 | " prog = MathematicalProgram()\n", 198 | " \n", 199 | " # continuous decision variables\n", 200 | " z = prog.NewContinuousVariables(K, 4)\n", 201 | " u = prog.NewContinuousVariables(K - 1, 2)\n", 202 | " q = z[:, :2]\n", 203 | " qdot = z[:, 2:]\n", 204 | " \n", 205 | " # indicator variables\n", 206 | " if relaxation:\n", 207 | " b = prog.NewContinuousVariables(K - 1, len(Dq))\n", 208 | " prog.AddLinearConstraint(ge(b.flatten(), 0))\n", 209 | " else:\n", 210 | " b = prog.NewBinaryVariables(K - 1, len(Dq))\n", 211 | " \n", 212 | " # slack variables for the cost function\n", 213 | " s = prog.NewContinuousVariables(K - 1, len(Dq))\n", 214 | " prog.AddLinearConstraint(ge(s.flatten(), 0))\n", 215 | " prog.AddLinearCost(sum(sum(s)))\n", 216 | " \n", 217 | " # initial and terminal conditions\n", 218 | " prog.AddLinearConstraint(eq(z[0], z1))\n", 219 | " prog.AddLinearConstraint(eq(z[-1], zK))\n", 220 | " \n", 221 | " # containers for copies of state and the controls\n", 222 | " z_aux = []\n", 223 | " u_aux = []\n", 224 | " \n", 225 | " # loop over time\n", 226 | " for k in range(K - 1):\n", 227 | " \n", 228 | " # auxiliary copies of state and the controls\n", 229 | " Z = prog.NewContinuousVariables(len(Dq), 4)\n", 230 | " U = prog.NewContinuousVariables(len(Dq), 2)\n", 231 | " Znext = prog.NewContinuousVariables(len(Dq), 4)\n", 232 | " Q = Z[:, :2]\n", 233 | " Qdot = Z[:, 2:]\n", 234 | " z_aux.append(Z)\n", 235 | " u_aux.append(U)\n", 236 | " \n", 237 | " # loop over modes of the pwa system\n", 238 | " for i, Dqi in enumerate(Dq):\n", 239 | " \n", 240 | " # cone constraint for the perspective of the cost\n", 241 | " prog.AddRotatedLorentzConeConstraint(np.concatenate((\n", 242 | " [s[k, i]],\n", 243 | " [b[k, i]],\n", 244 | " U[i],\n", 245 | " q_dot_cost * Qdot[i]\n", 246 | " )))\n", 247 | " \n", 248 | " # state and input bounds\n", 249 | " prog.AddLinearConstraint(le(Dqi.C.dot(Q[i]), Dqi.d * b[k, i]))\n", 250 | " prog.AddLinearConstraint(le(U[i], u_max * b[k, i]))\n", 251 | " prog.AddLinearConstraint(ge(U[i], - u_max * b[k, i]))\n", 252 | " prog.AddLinearConstraint(le(Qdot[i], qdot_max * b[k, i]))\n", 253 | " prog.AddLinearConstraint(ge(Qdot[i], - qdot_max * b[k, i]))\n", 254 | " \n", 255 | " # pwa dynamics\n", 256 | " Ai, Bi, ci = pwa.dynamics[i]\n", 257 | " prog.AddLinearConstraint(eq(Ai.dot(Z[i]) + Bi.dot(U[i]) + ci, Znext[i]))\n", 258 | " \n", 259 | " # reconstruct auxiliary variables\n", 260 | " prog.AddLinearConstraint(eq(sum(Z), z[k]))\n", 261 | " prog.AddLinearConstraint(eq(sum(U), u[k]))\n", 262 | " prog.AddLinearConstraint(eq(sum(Znext), z[k + 1]))\n", 263 | " prog.AddLinearConstraint(sum(b[k]) == 1)\n", 264 | " \n", 265 | " # solve optimization\n", 266 | " solver = MosekSolver()\n", 267 | " result = solver.Solve(prog)\n", 268 | " print('Cost:', result.get_optimal_cost())\n", 269 | " print('Solve time:', result.get_solver_details().optimizer_time)\n", 270 | " \n", 271 | " # get optimal solution\n", 272 | " z_opt = result.GetSolution(z)\n", 273 | " u_opt = result.GetSolution(u)\n", 274 | " b_opt = result.GetSolution(b)\n", 275 | " z_aux_opt = [result.GetSolution(zk) for zk in z_aux]\n", 276 | " u_aux_opt = [result.GetSolution(uk) for uk in u_aux]\n", 277 | " \n", 278 | " return z_opt, u_opt, b_opt, z_aux_opt, u_aux_opt" 279 | ] 280 | }, 281 | { 282 | "cell_type": "code", 283 | "execution_count": null, 284 | "metadata": {}, 285 | "outputs": [], 286 | "source": [ 287 | "# solve the stepping stone problem using the perspective formulation\n", 288 | "relaxation = 0\n", 289 | "z, u, b, z_aux, u_aux = solve_baseline(relaxation)\n", 290 | "q = z[:, :2]\n", 291 | "qdot = z[:, 2:]" 292 | ] 293 | }, 294 | { 295 | "cell_type": "code", 296 | "execution_count": null, 297 | "metadata": {}, 298 | "outputs": [], 299 | "source": [ 300 | "# plot optimal solution\n", 301 | "plt.figure(figsize=(3, 4))\n", 302 | "plot_terrain(q, u)\n", 303 | "plt.xticks(range(8))\n", 304 | "plt.yticks(range(-6, 7))\n", 305 | "\n", 306 | "# plot transparent triangles\n", 307 | "if relaxation:\n", 308 | " for k in range(K - 1):\n", 309 | " for i, Dqi in enumerate(Dq):\n", 310 | " if not np.isclose(b[k][i], 0):\n", 311 | " plt.scatter(\n", 312 | " *z_aux[k][i][:2] / b[k][i],\n", 313 | " alpha=b[k][i],\n", 314 | " marker='^', edgecolor='k', facecolor='w', zorder=2\n", 315 | " )\n", 316 | "# plt.savefig('footstep_pf.pdf', bbox_inches='tight')" 317 | ] 318 | } 319 | ], 320 | "metadata": { 321 | "kernelspec": { 322 | "display_name": "Python 3 (ipykernel)", 323 | "language": "python", 324 | "name": "python3" 325 | }, 326 | "language_info": { 327 | "codemirror_mode": { 328 | "name": "ipython", 329 | "version": 3 330 | }, 331 | "file_extension": ".py", 332 | "mimetype": "text/x-python", 333 | "name": "python", 334 | "nbconvert_exporter": "python", 335 | "pygments_lexer": "ipython3", 336 | "version": "3.11.2" 337 | } 338 | }, 339 | "nbformat": 4, 340 | "nbformat_minor": 4 341 | } 342 | --------------------------------------------------------------------------------