├── .gitignore ├── LICENSE ├── PRMPlanner.py ├── README.md ├── RRTFamilyOfPlanners.py ├── Sample Based Path Planning - Simulator.ipynb ├── SamplingBasedPathPlanningMethods.py ├── bugtrap.yaml ├── drawer.py ├── easy_environment.yaml ├── environment.py ├── graph.py ├── hard_environment.yaml ├── search_classes.py ├── simple.yaml └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *,cover 47 | .hypothesis/ 48 | 49 | # Translations 50 | *.mo 51 | *.pot 52 | 53 | # Django stuff: 54 | *.log 55 | local_settings.py 56 | 57 | # Flask stuff: 58 | instance/ 59 | .webassets-cache 60 | 61 | # Scrapy stuff: 62 | .scrapy 63 | 64 | # Sphinx documentation 65 | docs/_build/ 66 | 67 | # PyBuilder 68 | target/ 69 | 70 | # Jupyter Notebook 71 | .ipynb_checkpoints 72 | 73 | # pyenv 74 | .python-version 75 | 76 | # celery beat schedule file 77 | celerybeat-schedule 78 | 79 | # SageMath parsed files 80 | *.sage.py 81 | 82 | # dotenv 83 | .env 84 | 85 | # virtualenv 86 | .venv 87 | venv/ 88 | ENV/ 89 | 90 | # Spyder project settings 91 | .spyderproject 92 | 93 | # Rope project settings 94 | .ropeproject 95 | 96 | # mkdocs documentation 97 | /site -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 yrouben 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /PRMPlanner.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import math 3 | import numpy as np 4 | import random 5 | from graph import Graph, Edge 6 | from search_classes import SearchNode, Path 7 | from utils import * 8 | from shapely.geometry import Point, LineString 9 | 10 | class PRMPathPlanner(): 11 | 12 | """Plans path using PRM algorithm on a 2D environment. 13 | 14 | """ 15 | def initialise(self, environment, bounds, start_pose, goal_region, radius, resolution, isLazy): 16 | """ 17 | Initialises the main parameters for the PRM based path planner. 18 | 19 | Args: 20 | environment (a yaml environment): Environment where the planner will run. 21 | bounds ( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world. 22 | radius (float): Radius of the object 23 | resolution (int): Number of segments used to approximate a circle around a point 24 | isLazy (bool): If true, graph is created and collisions are only detected after running A* to find the optimum path instead of during the creation of the graph. 25 | goal_region: (Polygon): A polygon representing the region we want our object to reach. 26 | start_pose: ( ( float float) ): Starting x and y coordinates of the object in question. 27 | 28 | Returns: 29 | None 30 | """ 31 | self.env = environment 32 | self.bounds = bounds 33 | self.radius = radius 34 | self.resolution = resolution 35 | self.isLazy = isLazy 36 | self.goal_region = goal_region 37 | self.start_pose = start_pose 38 | 39 | def path (self,environment, bounds, q_init_point, q_goal_region, radius, resolution, isLazy): 40 | """ 41 | Creates and returns a path along with the set of vertices and edges that make up the graph used in the PRM algorithm. 42 | 43 | Args: 44 | environment (a yaml environment): Environment where the planner will run. 45 | bounds ( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world. 46 | radius (float): Radius of the object 47 | resolution (int): Number of segments used to approximate a circle around a point 48 | isLazy (bool): If true, graph is created and collisions are only detected after running A* to find the optimum path instead of during the creation of the graph. 49 | goal_region: (Polygon): A polygon representing the region we want our object to reach. 50 | start_pose: ( ( float float) ): Starting x and y coordinates of the object in question. 51 | 52 | Returns: 53 | path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region 54 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree 55 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree 56 | """ 57 | self.initialise(environment, bounds, q_init_point, q_goal_region, radius, resolution, isLazy) 58 | 59 | x0, y0 = q_init_point 60 | x1, y1 = q_goal_region.centroid.coords[0] 61 | q_init = (x0, y0) 62 | q_goal = (x1, y1) 63 | V = set() 64 | E = set() 65 | 66 | if q_init == q_goal: 67 | path = [q_init, q_goal] 68 | V.union([q_init, q_goall]) 69 | E.union([(q_init, q_goal)]) 70 | elif self.isEdgeCollisionFree(q_init, q_goal, self.radius): 71 | path = [q_init, q_goal] 72 | V.union([q_init, q_goal]) 73 | E.union([(q_init, q_goal)]) 74 | else: 75 | path, V, E = self.PRMSearch(self.bounds, q_init, self.radius, q_goal, isLazy) 76 | 77 | return path, V, E 78 | 79 | def PRMSearch(self, bounds, q_init, radius, q_goal, isLazy): 80 | '''print "starting roadmap construction"''' 81 | # number of nodes to put in the roadmap 82 | n = min(max(len(self.env.obstacles) *2, 500), 2000) 83 | # number of closest neighbours to examine for each configuration 84 | k = min(int(n/50),15) 85 | 86 | # construct the probabilistic roadmap 87 | V,E = self.roadmapConstruction(bounds, radius, k, n, isLazy) 88 | 89 | # k closest neighbours of start configuration q_init, from V, sorted by distance 90 | Nqinit = self.find_k_closest_neighbours(V, q_init, k) 91 | # k closest neighbours of goal configuration q_goal, from V, sorted by distance 92 | Nqgoal = self.find_k_closest_neighbours(V, q_goal, k) 93 | # V is the union of itself and Nqinit and Nqgoal 94 | V = V.union(Nqinit, Nqgoal) 95 | 96 | # iterate through Nqinit and try to find the closest neighbour to q_init that has a collision free edge between them 97 | for qprime in Nqinit: 98 | if self.isEdgeCollisionFree(q_init, qprime, radius): 99 | E.add((q_init, qprime)) 100 | break 101 | 102 | # iterate through Nqgoal and try to find the closest neighbour to q_goal that has a collision free edge between them 103 | for qprime in Nqgoal: 104 | if self.isEdgeCollisionFree(q_goal, qprime, radius): 105 | E.add((q_goal, qprime)) 106 | break 107 | '''print "starting search"''' 108 | # find, using A* Search, the shortest path between our intial point and goal point, given the constructed graph. 109 | path = None 110 | if isLazy: 111 | ok_edges = set() 112 | attempt = 1 113 | 114 | while True: 115 | 116 | path_found = True 117 | path = self.findShortestPath(V, E, q_init, q_goal) 118 | 119 | if path == None: 120 | if attempt == 5: 121 | break 122 | '''print attempt''' 123 | n += 200 124 | V, E = self.roadmapReconstruction(bounds, radius, k, n, isLazy, V, E, q_init, q_goal) 125 | attempt += 1 126 | continue 127 | 128 | for i in xrange(len(path)-1): 129 | edge = (path[i], path[i+1]) 130 | reversed_edge = (path[i+1], path[i]) 131 | 132 | if edge not in ok_edges: 133 | if not self.isEdgeCollisionFree(path[i], path[i+1], radius): 134 | if edge in E: 135 | E.remove(edge) 136 | if reversed_edge in E: 137 | E.remove(reversed_edge) 138 | path_found = False 139 | else: 140 | ok_edges.add(edge) 141 | ok_edges.add(reversed_edge) 142 | if path_found: 143 | break 144 | 145 | 146 | else: 147 | for attempt in xrange(1,6): 148 | '''print attempt''' 149 | path = self.findShortestPath(V, E, q_init, q_goal) 150 | if path == None: 151 | if attempt == 5: 152 | break 153 | n += 200 154 | V, E = self.roadmapReconstruction(bounds, radius, k, n, isLazy, V, E, q_init, q_goal) 155 | else: 156 | break 157 | 158 | # returns path: a list of tuples representing points along the shortest path. 159 | return path, V, E 160 | 161 | 162 | 163 | def roadmapConstruction(self, bounds, radius, k, n, isLazy): 164 | 165 | # Vertices in our roadmap 166 | V = set() 167 | # Edges in our roadmap 168 | E = set() 169 | '''print "start finding n free points"''' 170 | while len(V) < n: 171 | # random collision free configuration in our configuration space 172 | q = self.find_random_collision_free_configuration(bounds, radius) 173 | # V is the union of itself and the n nodes we put into our roadmap 174 | V.add(q) 175 | '''print "starting finding k nearest neighbours"''' 176 | for q in V: 177 | # k closest neighbours of configuration q, from V, sorted by distance 178 | Nq = self.find_k_closest_neighbours(V, q, k) 179 | # set of collision free edges between q and its k closest neighbours 180 | q_edges = self.find_collision_free_edges(q, Nq, radius, E, isLazy) 181 | # E is the union of itself and the newly found edges 182 | E = E.union(q_edges) 183 | 184 | # retuns V,E: the set of n Vertices and corresponding connected edges that form the probabilistic roadmap 185 | return V,E 186 | 187 | 188 | def roadmapReconstruction(self, bounds, radius, k, n, isLazy, V, E, q_init, q_goal): 189 | '''print "start finding n free points"''' 190 | V_new_points = set() 191 | while len(V) < n: 192 | # random collision free configuration in our configuration space 193 | q = self.find_random_collision_free_configuration(bounds, radius) 194 | # V is the union of itself and the n nodes we put into our roadmap 195 | V.add(q) 196 | V_new_points.add(q) 197 | '''print "starting finding k nearest neighbours"''' 198 | for q in V_new_points: 199 | # k closest neighbours of configuration q, from V, sorted by distance 200 | Nq = self.find_k_closest_neighbours(V, q, k) 201 | # set of collision free edges between q and its k closest neighbours 202 | q_edges = self.find_collision_free_edges(q, Nq, radius, E, isLazy) 203 | # E is the union of itself and the newly found edges 204 | E = E.union(q_edges) 205 | 206 | # k closest neighbours of start configuration q_init, from V, sorted by distance 207 | Nqinit = self.find_k_closest_neighbours(V, q_init, k) 208 | # k closest neighbours of goal configuration q_goal, from V, sorted by distance 209 | Nqgoal = self.find_k_closest_neighbours(V, q_goal, k) 210 | # V is the union of itself and Nqinit and Nqgoal 211 | V = V.union(Nqinit, Nqgoal) 212 | 213 | for qprime in Nqinit: 214 | #if (q_init, qprime) not in E and (qprime, q_init) not in E: 215 | if self.isEdgeCollisionFree(q_init, qprime, radius): 216 | E.add((q_init, qprime)) 217 | break 218 | 219 | for qprime in Nqgoal: 220 | #if (q_goal, qprime) not in E and (qprime, q_goal) not in E: 221 | if self.isEdgeCollisionFree(q_goal, qprime, radius): 222 | E.add((q_goal, qprime)) 223 | break 224 | 225 | return V,E 226 | 227 | 228 | def find_random_collision_free_configuration(self, bounds, radius): 229 | # continue until a point is found 230 | while True: 231 | # get a random point 232 | q = self.get_random_point(bounds) 233 | # verify if it is collision free with all obstacles and whether it is within the bounds of the problem space 234 | if self.isPointCollisionFree(q, radius) and not self.isOutOfBounds(q, bounds, radius) : 235 | return q 236 | 237 | 238 | def find_k_closest_neighbours(self, V, q, k): 239 | # convert V to a list so that we can index elements inside 240 | list_V = list(V) 241 | 242 | # initialise list to store euclidian distance between every point v in V and q 243 | neighbour_distances = [] 244 | 245 | # calculate the distances and populate the list 246 | for v in list_V: 247 | neighbour_distances.append(self.euclidianDist(q, v)) 248 | 249 | # sort the neighbours_distances list and extract their position index in V 250 | sorting_indexes = np.argsort(neighbour_distances) 251 | 252 | # extract k closest neighbours to q, by extracting the elements from V, with indexes in the 1:k+1 slots of sorting_indexes. 253 | # Note: We ignore the first index in sorting_indexes, because this will simply index the point q in V 254 | k_closest_neighbours = [list_V[sorting_indexes[i]] for i in range(1,k+1)] 255 | 256 | # we return a set containing the k closest neighbours, so that the union can be taken with V 257 | return set(k_closest_neighbours) 258 | 259 | 260 | # finds the set of all edges between q and its neighbours in Nq, that are collision free 261 | def find_collision_free_edges(self, q, Nq, radius, E, isLazy): 262 | # initialise set that will store all collision free edges 263 | edges = set() 264 | # iterate through all neighbours of q in Nq and find collision free edges 265 | for neighbour in Nq: 266 | if (q, neighbour) not in E and (neighbour,q) not in E: 267 | # add the tuple edge to edges if it is collision free. edge = (q, neighbour) 268 | if isLazy: 269 | edges.add((q, neighbour)) 270 | else: 271 | if self.isEdgeCollisionFree(q, neighbour, radius): 272 | edges.add((q, neighbour)) 273 | # return the set of edges so that it's union may be taken with E 274 | return edges 275 | 276 | 277 | # checks if the edge between two points in our configuration space have a collision free edge 278 | def isEdgeCollisionFree(self, q_init, qprime, radius): 279 | # generate a line between the two points using the shapely library 280 | line = LineString([q_init, qprime]) 281 | # buffer the line so that it represents the physicality (shape) of the object 282 | expanded_line = line.buffer(radius) 283 | # iterate through all obstacles in the environment and verify if the buffered edge interesects any one of them 284 | for obstacle in self.env.obstacles: 285 | # terminate false as soon as intersection exists 286 | if expanded_line.intersects(obstacle): 287 | return False 288 | # terminate true if no intersection exists 289 | return True 290 | 291 | 292 | 293 | def findShortestPath(self, V, E, start_state, goal_state): 294 | graph = self.generateGraph(V,E) 295 | problem = GraphSearchProblem(graph, start_state, goal_state) 296 | return self.astar_search(problem, self.heuristic_to_goal) 297 | 298 | 299 | def generateGraph(self, V, E): 300 | graph = Graph() 301 | for edge in E: 302 | graph.add_edge(edge[0], edge[1], self.euclidianDist(edge[0], edge[1])) 303 | vertex_locations_dictionary = dict() 304 | for vertex in V: 305 | vertex_locations_dictionary[vertex] = vertex 306 | graph.set_node_positions(vertex_locations_dictionary) 307 | return graph 308 | 309 | 310 | def astar_search(self, problem, h): 311 | cost_func = lambda x: x.cost 312 | f = lambda x: cost_func(x) + h(x, problem.goal) 313 | return self.best_first_search(problem, f) 314 | 315 | def best_first_search(self, problem, f): 316 | queue = PriorityQueue(f=f) 317 | queue.append(SearchNode(problem.start)) 318 | expanded = set([problem.start]) 319 | max_queue = 1 320 | path = list() 321 | while queue: 322 | current_node = queue.pop() 323 | expanded.add(current_node.state) 324 | 325 | if(current_node.state == problem.goal): 326 | final_path = Path(current_node).path 327 | return final_path 328 | 329 | expanded_sn = problem.expand_node(current_node) 330 | 331 | for sn in expanded_sn: 332 | if(sn.state not in expanded): 333 | if sn in queue: 334 | if(sn.cost < queue[sn].cost): 335 | del queue[sn] 336 | queue.append(sn) 337 | else: 338 | queue.append(sn) 339 | if(len(queue) > max_queue): 340 | max_queue = len(queue) 341 | # If we get to here, no solution has been found. 342 | return None 343 | 344 | 345 | def heuristic_to_goal(self, search_node, goal_state): 346 | return self.euclidianDist(search_node.state, goal_state) 347 | 348 | # find a random point within the configuration space 349 | # Note: it finds an actual point and not a buffered point that represents the phsicality of our object 350 | def get_random_point(self, bounds): 351 | x = bounds[0] + random.random()*(bounds[2]-bounds[0]) 352 | y = bounds[1] + random.random()*(bounds[3]-bounds[1]) 353 | return (x, y) 354 | 355 | # check if configuration point q is collision free with all obstacles in the environment 356 | def isPointCollisionFree(self, q, radius): 357 | # buffer the point using the shapely class to check if the physical representation of q intersects any obstacles 358 | buffered_point = Point(q).buffer(radius) 359 | # verify collisions with every obstacle in the environment 360 | for obstacle in self.env.obstacles: 361 | # if q is in any obstacle, terminate false 362 | if obstacle.intersects(buffered_point): 363 | return False 364 | # if no obstacle contains q, then terminate true 365 | return True 366 | 367 | 368 | # check if configuration point q is out of bounds 369 | def isOutOfBounds(self, q, bounds, radius): 370 | # left boundary check 371 | if((q[0]-radius) < bounds[0]): 372 | return True 373 | # bottom boundary check 374 | if((q[1]-radius) < bounds[1]): 375 | return True 376 | # right boundary check 377 | if((q[0]+radius) > bounds[2]): 378 | return True 379 | # top boundary check 380 | if((q[1]+radius) > bounds[3]): 381 | return True 382 | # if all checks fail, then ball q wih given radius is within the boundary 383 | return False 384 | 385 | # calculate the euclidian distance between two configuration points q1, q2 386 | def euclidianDist(self, q1, q2): 387 | return math.sqrt((q2[0]-q1[0])**2 + (q2[1]-q1[1])**2) 388 | 389 | 390 | class GraphSearchProblem(object): 391 | def __init__(self, graph, start, goal): 392 | self.graph = graph 393 | self.start = start 394 | self.goal = goal 395 | def test_goal(self, state): 396 | return self.goal == state 397 | def expand_node(self, search_node): 398 | """Return a list of SearchNodes, having the correct state, parent and updated cost.""" 399 | current_node = search_node.state 400 | current_cost = search_node.cost 401 | outgoing_edges = self.graph.node_edges(current_node) 402 | expanded_sn = [SearchNode(edge.target, search_node, current_cost + edge.weight) for edge in outgoing_edges ] 403 | return expanded_sn 404 | 405 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Sampling-Based-Path-Planning-Library 2 | 3 | 4 | This library provides 2-D implementations of the PRM, RRT, RRT* and Informed-RRT* algorithms for sampling based path planning. 5 | A simulator of sorts is provided in the "Sample Based Path Planning - Simulator.ipynb" file. 6 | 7 | The PRM algorithm is implemented in the "PRMPlanner.py" file. 8 | The RRT family of algorithms are implemented in the "RRTFamilyOfPlanners.py" file. 9 | 10 | Running the simulator requires the ability to run IPython notebooks (i.e. jupyter). 11 | Additionally, this software requires the installation of several python libraries, namely: 12 | 13 | - shapely 14 | - numpy 15 | - yaml 16 | - matplotlib 17 | - descartes 18 | - networkx 19 | - bisect -------------------------------------------------------------------------------- /RRTFamilyOfPlanners.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | from shapely.geometry import Point, LineString 3 | import random 4 | import math 5 | import numpy as np 6 | 7 | 8 | class RRTFamilyPathPlanner(): 9 | 10 | """Plans path using an algorithm from the RRT family. 11 | 12 | Contains methods for simple RRT based search, RRTstar based search and informed RRTstar based search. 13 | 14 | """ 15 | 16 | def initialise(self, environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution, runForFullIterations): 17 | """Initialises the planner with information about the environment and parameters for the rrt path planers 18 | 19 | Args: 20 | environment (A yaml environment): Environment where the planner will be run. Includes obstacles. 21 | bounds( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world. 22 | start_pose( (float float) ): Starting x and y coordinates of the object in question. 23 | goal_region (Polygon): A polygon representing the region that we want our object to go to. 24 | object_radius (float): Radius of the object. 25 | steer_distance (float): Limits the length of the branches 26 | num_iterations (int): How many points are sampled for the creationg of the tree 27 | resolution (int): Number of segments used to approximate a quarter circle around a point. 28 | runForFullIterations (bool): If True RRT and RRTStar return the first path found without having to sample all num_iterations points. 29 | 30 | Returns: 31 | None 32 | """ 33 | self.env = environment 34 | self.obstacles = environment.obstacles 35 | self.bounds = bounds 36 | self.minx, self.miny, self.maxx, self.maxy = bounds 37 | self.start_pose = start_pose 38 | self.goal_region = goal_region 39 | self.obj_radius = object_radius 40 | self.N = num_iterations 41 | self.resolution = resolution 42 | self.steer_distance = steer_distance 43 | self.V = set() 44 | self.E = set() 45 | self.child_to_parent_dict = dict() #key = child, value = parent 46 | self.runForFullIterations = runForFullIterations 47 | self.goal_pose = (goal_region.centroid.coords[0]) 48 | 49 | 50 | def path(self, environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution, runForFullIterations, RRT_Flavour): 51 | """Returns a path from the start_pose to the goal region in the current environment using the specified RRT-variant algorithm. 52 | 53 | Args: 54 | environment (A yaml environment): Environment where the planner will be run. Includes obstacles. 55 | bounds( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world. 56 | start_pose( (float float) ): Starting x and y coordinates of the object in question. 57 | goal_region (Polygon): A polygon representing the region that we want our object to go to. 58 | object_radius (float): Radius of the object. 59 | steer_distance (float): Limits the length of the branches 60 | num_iterations (int): How many points are sampled for the creationg of the tree 61 | resolution (int): Number of segments used to approximate a quarter circle around a point. 62 | runForFullIterations (bool): If True RRT and RRTStar return the first path found without having to sample all num_iterations points. 63 | RRT_Flavour (str): A string representing what type of algorithm to use. 64 | Options are 'RRT', 'RRT*', and 'InformedRRT*'. Anything else returns None,None,None. 65 | 66 | Returns: 67 | path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region 68 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree 69 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree 70 | """ 71 | self.env = environment 72 | 73 | self.initialise(environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution, runForFullIterations) 74 | 75 | # Define start and goal in terms of coordinates. The goal is the centroid of the goal polygon. 76 | x0, y0 = start_pose 77 | x1, y1 = goal_region.centroid.coords[0] 78 | start = (x0, y0) 79 | goal = (x1, y1) 80 | 81 | # Handle edge case where where the start is already at the goal 82 | if start == goal: 83 | path = [start, goal] 84 | self.V.union([start, goal]) 85 | self.E.union([(start, goal)]) 86 | # There might also be a straight path to goal, consider this case before invoking algorithm 87 | elif self.isEdgeCollisionFree(start, goal): 88 | path = [start, goal] 89 | self.V.union([start, goal]) 90 | self.E.union([(start, goal)]) 91 | # Run the appropriate RRT algorithm according to RRT_Flavour 92 | else: 93 | if RRT_Flavour == "RRT": 94 | path, self.V, self.E = self.RRTSearch() 95 | elif RRT_Flavour == "RRT*": 96 | path, self.V, self.E = self.RRTStarSearch() 97 | elif RRT_Flavour == "InformedRRT*": 98 | path, self.V, self.E = self.InformedRRTStarSearch() 99 | else: 100 | # The RRT flavour has no defined algorithm, therefore return None for all values 101 | return None, None, None 102 | 103 | return path, self.V, self.E 104 | 105 | 106 | def RRTSearch(self): 107 | """Returns path using RRT algorithm. 108 | 109 | Builds a tree exploring from the start node until it reaches the goal region. It works by sampling random points in the map and connecting them with 110 | the tree we build off on each iteration of the algorithm. 111 | 112 | Returns: 113 | path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region 114 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree 115 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree 116 | """ 117 | 118 | # Initialize path and tree to be empty. 119 | path = [] 120 | path_length = float('inf') 121 | tree_size = 0 122 | path_size = 0 123 | self.V.add(self.start_pose) 124 | goal_centroid = self.get_centroid(self.goal_region) 125 | 126 | # Iteratively sample N random points in environment to build tree 127 | for i in xrange(self.N): 128 | if(random.random()>=1.95): # Change to a value under 1 to bias search towards goal, right now this line doesn't run 129 | random_point = goal_centroid 130 | else: 131 | random_point = self.get_collision_free_random_point() 132 | 133 | # The new point to be added to the tree is not the sampled point, but a colinear point with it and the nearest point in the tree. 134 | # This keeps the branches short 135 | nearest_point = self.find_nearest_point(random_point) 136 | new_point = self.steer(nearest_point, random_point) 137 | 138 | # If there is no obstacle between nearest point and sampled point, add the new point to the tree. 139 | if self.isEdgeCollisionFree(nearest_point, new_point): 140 | self.V.add(new_point) 141 | self.E.add((nearest_point, new_point)) 142 | self.setParent(nearest_point, new_point) 143 | # If new point of the tree is at the goal region, we can find a path in the tree from start node to goal. 144 | if self.isAtGoalRegion(new_point): 145 | if not self.runForFullIterations: # If not running for full iterations, terminate as soon as a path is found. 146 | path, tree_size, path_size, path_length = self.find_path(self.start_pose, new_point) 147 | break 148 | else: # If running for full iterations, we return the shortest path found. 149 | tmp_path, tmp_tree_size, tmp_path_size, tmp_path_length = self.find_path(self.start_pose, new_point) 150 | if tmp_path_length < path_length: 151 | path_length = tmp_path_length 152 | path = tmp_path 153 | tree_size = tmp_tree_size 154 | path_size = tmp_path_size 155 | 156 | # If no path is found, then path would be an empty list. 157 | return path, self.V, self.E 158 | 159 | 160 | def RRTStarSearch(self): 161 | """Returns path using RRTStar algorithm. 162 | 163 | Uses the same structure as RRTSearch, except there's an additional 'rewire' call when adding nodes to the tree. 164 | This can be seen as a way to optimise the branches of the subtree where the new node is being added. 165 | 166 | Returns: 167 | path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region 168 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree 169 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree 170 | """ 171 | # Code is very similar to RRTSearch, so for simplicity's sake only the main differences have been commented. 172 | path = [] 173 | path_length = float('inf') 174 | tree_size = 0 175 | path_size = 0 176 | self.V.add(self.start_pose) 177 | goal_centroid = self.get_centroid(self.goal_region) 178 | 179 | for i in xrange(self.N): 180 | if(random.random()>=1.95): 181 | random_point = goal_centroid 182 | else: 183 | random_point = self.get_collision_free_random_point() 184 | 185 | nearest_point = self.find_nearest_point(random_point) 186 | new_point = self.steer(nearest_point, random_point) 187 | 188 | if self.isEdgeCollisionFree(nearest_point, new_point): 189 | # Find the nearest set of points around the new point 190 | nearest_set = self.find_nearest_set(new_point) 191 | min_point = self.find_min_point(nearest_set, nearest_point, new_point) 192 | self.V.add(new_point) 193 | self.E.add((min_point, new_point)) 194 | self.setParent(min_point, new_point) 195 | # Main difference between RRT and RRT*, modify the points in the nearest set to optimise local path costs. 196 | self.rewire(nearest_set, min_point, new_point) 197 | if self.isAtGoalRegion(new_point): 198 | if not self.runForFullIterations: 199 | path, tree_size, path_size, path_length = self.find_path(self.start_pose, new_point) 200 | break 201 | else: 202 | tmp_path, tmp_tree_size, tmp_path_size, tmp_path_length = self.find_path(self.start_pose, new_point) 203 | if tmp_path_length < path_length: 204 | path_length = tmp_path_length 205 | path = tmp_path 206 | tree_size = tmp_tree_size 207 | path_size = tmp_path_size 208 | 209 | return path, self.V, self.E 210 | 211 | 212 | def InformedRRTStarSearch(self): 213 | """Returns path using informed RRTStar algorithm. 214 | 215 | Uses the same structure as RRTStarSearch, except that once a path is found, sampling is restricted to an ellipse 216 | containing the shortest path found. 217 | 218 | Returns: 219 | path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region 220 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree 221 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree 222 | """ 223 | # Code is very similar to RRTStarSearch, so for simplicity's sake only the main differences have been commented. 224 | path = [] 225 | path_length = float('inf') 226 | c_best = float('inf') # Max length we expect to find in our 'informed' sample space starts as infinite 227 | tree_size = 0 228 | path_size = 0 229 | self.V.add(self.start_pose) 230 | goal_centroid = self.get_centroid(self.goal_region) 231 | solution_set = set() 232 | 233 | start_obj = Point(self.start_pose).buffer(self.obj_radius, self.resolution) 234 | # The following equations define the space of the environment that is being sampled. 235 | c_min = start_obj.distance(self.goal_region) 236 | x_center = np.matrix([[(self.start_pose[0] + self.goal_pose[0]) / 2.0],[(self.start_pose[1] + self.goal_pose[1]) / 2.0], [0]]) 237 | a_1 = np.matrix([[(self.goal_pose[0] - self.start_pose[0]) / c_min],[(self.goal_pose[1] - self.start_pose[1]) / c_min], [0]]) 238 | id1_t = np.matrix([1.0,0,0]) 239 | M = np.dot(a_1, id1_t) 240 | U,S,Vh = np.linalg.svd(M, 1, 1) 241 | C = np.dot(np.dot(U, np.diag([1.0,1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) 242 | 243 | for i in xrange(self.N): 244 | # The main difference in this algorithm is that we limit our sample space. 245 | # Sample space is defined by c_best (our best path and the maximum path length inside the new space) 246 | # c_min (the distance between start and goal), x_center (midpoint between start and goal) and C 247 | # only c_best changes whenever a new path is found. 248 | random_point = self.sample(c_best, c_min, x_center, C) 249 | 250 | nearest_point = self.find_nearest_point(random_point) 251 | new_point = self.steer(nearest_point, random_point) 252 | 253 | if self.isEdgeCollisionFree(nearest_point, new_point): 254 | nearest_set = self.find_nearest_set(new_point) 255 | min_point = self.find_min_point(nearest_set, nearest_point, new_point) 256 | self.V.add(new_point) 257 | self.E.add((min_point, new_point)) 258 | self.setParent(min_point, new_point) 259 | self.rewire(nearest_set, min_point, new_point) 260 | if self.isAtGoalRegion(new_point): 261 | solution_set.add(new_point) 262 | tmp_path, tmp_tree_size, tmp_path_size, tmp_path_length = self.find_path(self.start_pose, new_point) 263 | if tmp_path_length < path_length: 264 | path_length = tmp_path_length 265 | path = tmp_path 266 | tree_size = tmp_tree_size 267 | path_size = tmp_path_size 268 | c_best = tmp_path_length # c_best is calculated everytime a path is found. Affecting the sample space. 269 | 270 | return path, self.V, self.E 271 | 272 | 273 | """ 274 | ****************************************************************************************************************************************** 275 | ***************************************************** Helper Functions ******************************************************************* 276 | ****************************************************************************************************************************************** 277 | """ 278 | 279 | def sample(self, c_max, c_min, x_center, C): 280 | if c_max < float('inf'): 281 | r= [c_max /2.0, math.sqrt(c_max**2 - c_min**2)/2.0, math.sqrt(c_max**2 - c_min**2)/2.0] 282 | L = np.diag(r) 283 | x_ball = self.sample_unit_ball() 284 | random_point = np.dot(np.dot(C,L), x_ball) + x_center 285 | random_point = (random_point[(0,0)], random_point[(1,0)]) 286 | else: 287 | random_point = self.get_collision_free_random_point() 288 | return random_point 289 | 290 | def sample_unit_ball(self): 291 | a = random.random() 292 | b = random.random() 293 | 294 | if b < a: 295 | tmp = b 296 | b = a 297 | a = tmp 298 | sample = (b*math.cos(2*math.pi*a/b), b*math.sin(2*math.pi*a/b)) 299 | return np.array([[sample[0]], [sample[1]], [0]]) 300 | 301 | def find_nearest_set(self, new_point): 302 | points = set() 303 | ball_radius = self.find_ball_radius() 304 | for vertex in self.V: 305 | euc_dist = self.euclidian_dist(new_point, vertex) 306 | if euc_dist < ball_radius: 307 | points.add(vertex) 308 | return points 309 | 310 | def find_ball_radius(self): 311 | unit_ball_volume = math.pi 312 | n = len(self.V) 313 | dimensions = 2.0 314 | gamma = (2**dimensions)*(1.0 + 1.0/dimensions) * (self.maxx - self.minx) * (self.maxy - self.miny) 315 | ball_radius = min(((gamma/unit_ball_volume) * math.log(n) / n)**(1.0/dimensions), self.steer_distance) 316 | return ball_radius 317 | 318 | 319 | def find_min_point(self, nearest_set, nearest_point, new_point): 320 | min_point = nearest_point 321 | min_cost = self.cost(nearest_point) + self.linecost(nearest_point, new_point) 322 | for vertex in nearest_set: 323 | if self.isEdgeCollisionFree(vertex, new_point): 324 | temp_cost = self.cost(vertex) + self.linecost(vertex, new_point) 325 | if temp_cost < min_cost: 326 | min_point = vertex 327 | min_cost = temp_cost 328 | return min_point 329 | 330 | def rewire(self, nearest_set, min_point, new_point): 331 | # Discards edges in the nearest_set that lead to a longer path than going through the new_point first 332 | # Then add an edge from new_point to the vertex in question and update its parent accordingly. 333 | for vertex in nearest_set - set([min_point]): 334 | if self.isEdgeCollisionFree(vertex, new_point): 335 | if self.cost(vertex) > self.cost(new_point) + self.linecost(vertex, new_point): 336 | parent_point = self.getParent(vertex) 337 | self.E.discard((parent_point, vertex)) 338 | self.E.discard((vertex, parent_point)) 339 | self.E.add((new_point, vertex)) 340 | self.setParent(new_point, vertex) 341 | 342 | 343 | def cost(self, vertex): 344 | path, tree_size, path_size, path_length = self.find_path(self.start_pose, vertex) 345 | return path_length 346 | 347 | 348 | def linecost(self, point1, point2): 349 | return self.euclidian_dist(point1, point2) 350 | 351 | def getParent(self, vertex): 352 | return self.child_to_parent_dict[vertex] 353 | 354 | def setParent(self, parent, child): 355 | self.child_to_parent_dict[child] = parent 356 | 357 | def get_random_point(self): 358 | x = self.minx + random.random() * (self.maxx - self.minx) 359 | y = self.miny + random.random() * (self.maxy - self.miny) 360 | return (x, y) 361 | 362 | def get_collision_free_random_point(self): 363 | # Run until a valid point is found 364 | while True: 365 | point = self.get_random_point() 366 | # Pick a point, if no obstacle overlaps with a circle centered at point with some obj_radius then return said point. 367 | buffered_point = Point(point).buffer(self.obj_radius, self.resolution) 368 | if self.isPointCollisionFree(buffered_point): 369 | return point 370 | 371 | def isPointCollisionFree(self, point): 372 | for obstacle in self.obstacles: 373 | if obstacle.contains(point): 374 | return False 375 | return True 376 | 377 | def find_nearest_point(self, random_point): 378 | closest_point = None 379 | min_dist = float('inf') 380 | for vertex in self.V: 381 | euc_dist = self.euclidian_dist(random_point, vertex) 382 | if euc_dist < min_dist: 383 | min_dist = euc_dist 384 | closest_point = vertex 385 | return closest_point 386 | 387 | def isOutOfBounds(self, point): 388 | if((point[0] - self.obj_radius) < self.minx): 389 | return True 390 | if((point[1] - self.obj_radius) < self.miny): 391 | return True 392 | if((point[0] + self.obj_radius) > self.maxx): 393 | return True 394 | if((point[1] + self.obj_radius) > self.maxy): 395 | return True 396 | return False 397 | 398 | 399 | def isEdgeCollisionFree(self, point1, point2): 400 | if self.isOutOfBounds(point2): 401 | return False 402 | line = LineString([point1, point2]) 403 | expanded_line = line.buffer(self.obj_radius, self.resolution) 404 | for obstacle in self.obstacles: 405 | if expanded_line.intersects(obstacle): 406 | return False 407 | return True 408 | 409 | def steer(self, from_point, to_point): 410 | fromPoint_buffered = Point(from_point).buffer(self.obj_radius, self.resolution) 411 | toPoint_buffered = Point(to_point).buffer(self.obj_radius, self.resolution) 412 | if fromPoint_buffered.distance(toPoint_buffered) < self.steer_distance: 413 | return to_point 414 | else: 415 | from_x, from_y = from_point 416 | to_x, to_y = to_point 417 | theta = math.atan2(to_y - from_y, to_x- from_x) 418 | new_point = (from_x + self.steer_distance * math.cos(theta), from_y + self.steer_distance * math.sin(theta)) 419 | return new_point 420 | 421 | def isAtGoalRegion(self, point): 422 | buffered_point = Point(point).buffer(self.obj_radius, self.resolution) 423 | intersection = buffered_point.intersection(self.goal_region) 424 | inGoal = intersection.area / buffered_point.area 425 | return inGoal >= 0.5 426 | 427 | def euclidian_dist(self, point1, point2): 428 | return math.sqrt((point2[0] - point1[0])**2 + (point2[1] - point1[1])**2) 429 | 430 | def find_path(self, start_point, end_point): 431 | # Returns a path by backtracking through the tree formed by one of the RRT algorithms starting at the end_point until reaching start_node. 432 | path = [end_point] 433 | tree_size, path_size, path_length = len(self.V), 1, 0 434 | current_node = end_point 435 | previous_node = None 436 | target_node = start_point 437 | while current_node != target_node: 438 | parent = self.getParent(current_node) 439 | path.append(parent) 440 | previous_node = current_node 441 | current_node = parent 442 | path_length += self.euclidian_dist(current_node, previous_node) 443 | path_size += 1 444 | path.reverse() 445 | return path, tree_size, path_size, path_length 446 | 447 | def get_centroid(self, region): 448 | centroid = region.centroid.wkt 449 | filtered_vals = centroid[centroid.find("(")+1:centroid.find(")")] 450 | filtered_x = filtered_vals[0:filtered_vals.find(" ")] 451 | filtered_y = filtered_vals[filtered_vals.find(" ") + 1: -1] 452 | (x,y) = (float(filtered_x), float(filtered_y)) 453 | return (x,y) 454 | -------------------------------------------------------------------------------- /SamplingBasedPathPlanningMethods.py: -------------------------------------------------------------------------------- 1 | from RRTFamilyOfPlanners import RRTFamilyPathPlanner 2 | from PRMPlanner import PRMPathPlanner 3 | from drawer import draw_results 4 | import time 5 | 6 | class SamplingBasedPathPlanner(): 7 | 8 | """Plans path using a sampling based algorithm on a 2D environment. 9 | 10 | Contains methods for simple RRT based search, RRTstar based search, informed RRTstar based search, and PRM based search, all in 2D space. 11 | Methods also have the option to draw the results. 12 | 13 | """ 14 | def __init__(self): 15 | """ 16 | The planner contains two objects. One for planning using RRT algorithms and another for using a PRM planner. 17 | """ 18 | self.RRTFamilySolver = RRTFamilyPathPlanner() 19 | self.PRMSolver = PRMPathPlanner() 20 | 21 | def RRT(self, environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution=3, runForFullIterations=False, drawResults=False): 22 | """Returns a path from the start_pose to the goal region in the current environment using RRT. 23 | 24 | Args: 25 | environment (A yaml environment): Environment where the planner will be run. Includes obstacles. 26 | bounds( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world. 27 | start_pose( (float float) ): Starting x and y coordinates of the object in question. 28 | goal_region (Polygon): A polygon representing the region that we want our object to go to. 29 | object_radius (float): Radius of the object. 30 | steer_distance (float): Limits the length of the branches 31 | num_iterations (int): How many points are sampled for the creationg of the tree 32 | resolution (int): Number of segments used to approximate a quarter circle around a point. 33 | runForFullIterations (bool): Optional, if True return the first path found without having to sample all num_iterations points. 34 | drawResults (bool): Optional, if set to True it plots the path and enviornment using a matplotlib plot. 35 | 36 | Returns: 37 | path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region 38 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree 39 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree 40 | """ 41 | start_time = time.time() 42 | path, V, E = self.RRTFamilySolver.path(environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution, runForFullIterations, RRT_Flavour="RRT") 43 | elapsed_time = time.time() - start_time 44 | if path and drawResults: 45 | draw_results("RRT", path, V, E, environment, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time) 46 | return path, V, E 47 | 48 | def RRTStar(self, environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution=3, runForFullIterations=False, drawResults=False): 49 | """Returns a path from the start_pose to the goal region in the current environment using RRT*. 50 | 51 | Args: 52 | environment (A yaml environment): Environment where the planner will be run. Includes obstacles. 53 | bounds( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world. 54 | start_pose( (float float) ): Starting x and y coordinates of the object in question. 55 | goal_region (Polygon): A polygon representing the region that we want our object to go to. 56 | object_radius (float): Radius of the object. 57 | steer_distance (float): Limits the length of the branches 58 | num_iterations (int): How many points are sampled for the creationg of the tree 59 | resolution (int): Number of segments used to approximate a quarter circle around a point. 60 | runForFullIterations (bool): Optional, if True return the first path found without having to sample all num_iterations points. 61 | drawResults (bool): Optional, if set to True it plots the path and enviornment using a matplotlib plot. 62 | 63 | Returns: 64 | path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region 65 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree 66 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree 67 | """ 68 | start_time = time.time() 69 | path, V, E = self.RRTFamilySolver.path(environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution, runForFullIterations, RRT_Flavour="RRT*") 70 | elapsed_time = time.time() - start_time 71 | if path and drawResults: 72 | draw_results("RRT*", path, V, E, environment, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time) 73 | return path, V, E 74 | 75 | def InformedRRTStar(self, environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution=3, drawResults=False): 76 | """Returns a path from the start_pose to the goal region in the current environment using informed RRT*. 77 | 78 | Args: 79 | environment (A yaml environment): Environment where the planner will be run. Includes obstacles. 80 | bounds( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world. 81 | start_pose( (float float) ): Starting x and y coordinates of the object in question. 82 | goal_region (Polygon): A polygon representing the region that we want our object to go to. 83 | object_radius (float): Radius of the object. 84 | steer_distance (float): Limits the length of the branches 85 | num_iterations (int): How many points are sampled for the creationg of the tree 86 | resolution (int): Number of segments used to approximate a quarter circle around a point. 87 | drawResults (bool): Optional, if set to True it plots the path and enviornment using a matplotlib plot. 88 | 89 | Returns: 90 | path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region 91 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree 92 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree 93 | """ 94 | start_time = time.time() 95 | path, V, E = self.RRTFamilySolver.path(environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution, runForFullIterations=True, RRT_Flavour="InformedRRT*") 96 | elapsed_time = time.time() - start_time 97 | if path and drawResults: 98 | draw_results("InformedRRT*", path, V, E, environment, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time) 99 | return path, V, E 100 | 101 | def PRM(self, environment, bounds, start_pose, goal_region, object_radius, resolution=3, isLAzy=True, drawResults=False): 102 | """Returns a path from the start_pose to the goal region in the current environment using PRM. 103 | 104 | Args: 105 | environment (A yaml environment): Environment where the planner will be run. Includes obstacles. 106 | bounds( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world. 107 | start_pose( (float float) ): Starting x and y coordinates of the object in question. 108 | goal_region (Polygon): A polygon representing the region that we want our object to go to. 109 | object_radius (float): Radius of the object. 110 | resolution (int): Number of segments used to approximate a quarter circle around a point. 111 | isLazy (bool): Optional, if True it adds edges to the graph regardless of whether it collides with an obstacle and only checks for collisions when building the path. 112 | drawResults (bool): Optional, if set to True it plots the path and enviornment using a matplotlib plot. 113 | 114 | Returns: 115 | path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region 116 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the graph. 117 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the graph. 118 | """ 119 | start_time = time.time() 120 | path, V, E = self.PRMSolver.path(environment, bounds, start_pose, goal_region, object_radius, resolution, isLAzy) 121 | elapsed_time = time.time() - start_time 122 | if path and drawResults: 123 | draw_results("PRM", path, V, E, environment, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time) 124 | return path, V, E 125 | 126 | 127 | 128 | 129 | 130 | -------------------------------------------------------------------------------- /bugtrap.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | obstacles: 3 | obstacle0000: 4 | corners: 5 | - [0.0, 0.0] 6 | - [0.0, 1.0] 7 | - [4.0, 1.0] 8 | - [4.0, 0.0] 9 | - [0.0, 0.0] 10 | shape: polygon 11 | obstacle0001: 12 | corners: 13 | - [0.0, 4.0] 14 | - [0.0, 5.0] 15 | - [4.0, 5.0] 16 | - [4.0, 4.0] 17 | - [0.0, 4.0] 18 | shape: polygon 19 | obstacle0002: 20 | corners: 21 | - [3.0, 1.0] 22 | - [3.0, 4.0] 23 | - [4.0, 4.0] 24 | - [4.0, 1.0] 25 | - [3.0, 1.0] 26 | shape: polygon 27 | obstacle0003: 28 | corners: 29 | - [7.0, 3.0] 30 | - [7.0, 8.0] 31 | - [8.0, 8.0] 32 | - [8.0, 3.0] 33 | - [7.0, 3.0] 34 | shape: polygon 35 | obstacle0004: 36 | corners: 37 | - [7.0, -3.0] 38 | - [7.0, 2.0] 39 | - [8.0, 2.0] 40 | - [8.0, -3.0] 41 | - [7.0, -3.0] 42 | shape: polygon 43 | -------------------------------------------------------------------------------- /drawer.py: -------------------------------------------------------------------------------- 1 | from shapely.geometry import Point, Polygon, LineString, box 2 | from environment import Environment, plot_environment, plot_line, plot_poly 3 | from math import sqrt 4 | 5 | def draw_results(algo_name, path, V, E, env, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time): 6 | """ 7 | Plots the path from start node to goal region as well as the graph (or tree) searched with the Sampling Based Algorithms. 8 | 9 | Args: 10 | algo_name (str): The name of the algorithm used (used as title of the plot) 11 | path (list<(float,float), (float,float)>): The sequence of coordinates traveled to reach goal from start node 12 | V (set<(float, float)>): All nodes in the explored graph/tree 13 | E (set<(float,float), (float, float)>): The set of all edges considered in the graph/tree 14 | env (yaml environment): 2D yaml environment for the path planning to take place 15 | bounds (int, int int int): min x, min y, max x, max y of the coordinates in the environment. 16 | object_radius (float): radius of our object. 17 | resolution (int): Number of segments used to approximate a quarter circle around a point. 18 | start_pose(float,float): Coordinates of initial point of the path. 19 | goal_region (Polygon): A polygon object representing the end goal. 20 | elapsed_time (float): Time it took for the algorithm to run 21 | 22 | Return: 23 | None 24 | 25 | Action: 26 | Plots a path using the environment module. 27 | """ 28 | 29 | graph_size = len(V) 30 | path_size = len(path) 31 | # Calculate path length 32 | path_length = 0.0 33 | for i in xrange(len(path)-1): 34 | path_length += euclidian_dist(path[i], path[i+1]) 35 | 36 | # Create title with descriptive information based on environment, path length, and elapsed_time 37 | title = algo_name + "\n" + str(graph_size) + " Nodes. " + str(len(env.obstacles)) + " Obstacles. Path Size: " + str(path_size) + "\n Path Length: " + str(path_length) + "\n Runtime(s)= " + str(elapsed_time) 38 | 39 | # Plot environment 40 | env_plot = plot_environment(env, bounds) 41 | # Add title 42 | env_plot.set_title(title) 43 | # Plot goal 44 | plot_poly(env_plot, goal_region, 'green') 45 | # Plot start 46 | buffered_start_vertex = Point(start_pose).buffer(object_radius, resolution) 47 | plot_poly(env_plot, buffered_start_vertex, 'red') 48 | 49 | # Plot Edges explored by ploting lines between each edge 50 | for edge in E: 51 | line = LineString([edge[0], edge[1]]) 52 | plot_line(env_plot, line) 53 | 54 | # Plot path 55 | plot_path(env_plot, path, object_radius) 56 | 57 | 58 | def euclidian_dist(point1, point2): 59 | return sqrt((point2[0] - point1[0])**2 + (point2[1] - point1[1])**2) 60 | 61 | 62 | def plot_path(env_plot, path, object_radius): 63 | # Plots path by taking an enviroment plot and ploting in red the edges that form part of the path 64 | line = LineString(path) 65 | x, y = line.xy 66 | env_plot.plot(x, y, color='red', linewidth=3, solid_capstyle='round', zorder=1) 67 | -------------------------------------------------------------------------------- /easy_environment.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | obstacles: 3 | obstacle0000: 4 | corners: 5 | - [2.9839104079712175, 6.186458318803618] 6 | - [2.9839104079712175, 6.271793210038175] 7 | - [3.848066308639373, 6.986919616083656] 8 | - [3.850762263885308, 6.186458318803618] 9 | - [2.9839104079712175, 6.186458318803618] 10 | shape: polygon 11 | obstacle0001: 12 | corners: 13 | - [11.835466877246333, 0.19677969405417084] 14 | - [11.835466877246333, 0.8341283395723832] 15 | - [11.996242713521891, 0.8469460929781056] 16 | - [12.428975786923017, 0.19677969405417084] 17 | - [11.835466877246333, 0.19677969405417084] 18 | shape: polygon 19 | obstacle0002: 20 | corners: 21 | - [-0.43272791175301295, -2.276887502335004] 22 | - [-0.43272791175301295, -2.0290654383817035] 23 | - [-0.012004810001607469, -1.9362773862651994] 24 | - [-0.38623676007092167, -2.276887502335004] 25 | - [-0.43272791175301295, -2.276887502335004] 26 | shape: polygon 27 | obstacle0003: 28 | corners: 29 | - [8.723215266444647, -1.8331369094366883] 30 | - [8.723215266444647, -1.1864386276939678] 31 | - [9.13568620909821, -1.3154191393792187] 32 | - [9.310266593164789, -1.8331369094366883] 33 | - [8.723215266444647, -1.8331369094366883] 34 | shape: polygon 35 | obstacle0004: 36 | corners: 37 | - [4.0721237188902855, 0.6479468806497906] 38 | - [4.0721237188902855, 1.606125682939737] 39 | - [4.805284377533802, 1.2996350657826468] 40 | - [5.06162468268455, 0.6479468806497906] 41 | - [4.0721237188902855, 0.6479468806497906] 42 | shape: polygon 43 | obstacle0005: 44 | corners: 45 | - [3.105971761081629, 1.4632028537459165] 46 | - [3.105971761081629, 1.78248759738799] 47 | - [3.9193529057073735, 1.920279229710249] 48 | - [3.8743779207400406, 1.4632028537459165] 49 | - [3.105971761081629, 1.4632028537459165] 50 | shape: polygon 51 | obstacle0006: 52 | corners: 53 | - [11.436337466761742, -2.6514905187535804] 54 | - [11.436337466761742, -1.8608952839600075] 55 | - [11.495792014919623, -2.5033947015297278] 56 | - [11.493848952585088, -2.6514905187535804] 57 | - [11.436337466761742, -2.6514905187535804] 58 | shape: polygon 59 | obstacle0007: 60 | corners: 61 | - [0.9587347750510444, 2.5690729756526007] 62 | - [0.9587347750510444, 3.1015859901088234] 63 | - [1.0829913185778948, 3.062306386373481] 64 | - [1.2760917987752256, 2.5690729756526007] 65 | - [0.9587347750510444, 2.5690729756526007] 66 | shape: polygon 67 | obstacle0008: 68 | corners: 69 | - [2.787362692367508, -0.4632549397993455] 70 | - [2.787362692367508, 0.264474804709257] 71 | - [3.019712115226066, 0.19461899288075557] 72 | - [3.2819049592324, -0.4632549397993455] 73 | - [2.787362692367508, -0.4632549397993455] 74 | shape: polygon 75 | obstacle0009: 76 | corners: 77 | - [1.0507499414697383, 7.354685925637526] 78 | - [1.0507499414697383, 8.002038407905196] 79 | - [1.1772130510697387, 7.6340219523335024] 80 | - [1.7906946574458904, 7.354685925637526] 81 | - [1.0507499414697383, 7.354685925637526] 82 | shape: polygon 83 | obstacle0010: 84 | corners: 85 | - [-0.1805311407962844, 7.958535920301916] 86 | - [-0.1805311407962844, 7.990919252765019] 87 | - [0.4413540087429786, 8.434941985422734] 88 | - [-0.0625636959970447, 7.958535920301916] 89 | - [-0.1805311407962844, 7.958535920301916] 90 | shape: polygon 91 | obstacle0011: 92 | corners: 93 | - [6.596156274371165, -1.3454797042856077] 94 | - [6.596156274371165, -1.080999120595753] 95 | - [7.056544340259772, -0.9037835835654712] 96 | - [7.483054782027152, -1.3454797042856077] 97 | - [6.596156274371165, -1.3454797042856077] 98 | shape: polygon 99 | obstacle0012: 100 | corners: 101 | - [2.9163006556118525, 2.348379690733056] 102 | - [2.9163006556118525, 2.494163177687918] 103 | - [3.658712777987102, 3.2203486296835613] 104 | - [3.5098403109354996, 2.348379690733056] 105 | - [2.9163006556118525, 2.348379690733056] 106 | shape: polygon 107 | obstacle0013: 108 | corners: 109 | - [2.082171329004842, -1.8958726432025539] 110 | - [2.082171329004842, -1.127813922728222] 111 | - [2.6458959425110895, -1.8274468114331914] 112 | - [2.879212035752058, -1.8958726432025539] 113 | - [2.082171329004842, -1.8958726432025539] 114 | shape: polygon 115 | obstacle0014: 116 | corners: 117 | - [6.684557153692193, -0.604539163888397] 118 | - [6.684557153692193, -0.02847487502507462] 119 | - [7.257054775050603, -0.08970930507098485] 120 | - [7.098510098807653, -0.604539163888397] 121 | - [6.684557153692193, -0.604539163888397] 122 | shape: polygon 123 | obstacle0015: 124 | corners: 125 | - [-0.09123543170804793, 5.347407759683774] 126 | - [-0.09123543170804793, 5.517254017241518] 127 | - [0.11144006267767359, 5.753685944539049] 128 | - [0.5650247471731895, 5.347407759683774] 129 | - [-0.09123543170804793, 5.347407759683774] 130 | shape: polygon 131 | obstacle0016: 132 | corners: 133 | - [-1.61727031611353, -0.25459610664124277] 134 | - [-1.61727031611353, 0.09210240845057105] 135 | - [-1.1571619564617612, 0.6344237293400438] 136 | - [-1.548781991277861, -0.25459610664124277] 137 | - [-1.61727031611353, -0.25459610664124277] 138 | shape: polygon 139 | obstacle0017: 140 | corners: 141 | - [1.636376338514248, -1.9618324866509083] 142 | - [1.636376338514248, -0.9729146144818873] 143 | - [1.7931804855803173, -1.4487146963904056] 144 | - [2.319159916614676, -1.9618324866509083] 145 | - [1.636376338514248, -1.9618324866509083] 146 | shape: polygon 147 | obstacle0018: 148 | corners: 149 | - [-1.920482235592353, -2.5955974432124918] 150 | - [-1.920482235592353, -2.566065149119025] 151 | - [-1.4924561258008815, -2.5231812434778367] 152 | - [-1.7386018571294346, -2.5955974432124918] 153 | - [-1.920482235592353, -2.5955974432124918] 154 | shape: polygon 155 | obstacle0019: 156 | corners: 157 | - [9.066510430027726, 2.9605336387493546] 158 | - [9.066510430027726, 3.328746607054771] 159 | - [9.5686539410284, 3.1452271012170385] 160 | - [9.577317392440776, 2.9605336387493546] 161 | - [9.066510430027726, 2.9605336387493546] 162 | shape: polygon 163 | obstacle0020: 164 | corners: 165 | - [3.0283078130718364, 3.992800237029212] 166 | - [3.0283078130718364, 4.686803434441387] 167 | - [3.884269327875805, 4.380736214323954] 168 | - [3.957245355814636, 3.992800237029212] 169 | - [3.0283078130718364, 3.992800237029212] 170 | shape: polygon 171 | obstacle0021: 172 | corners: 173 | - [3.9310094226397236, 6.111833829074321] 174 | - [3.9310094226397236, 6.531379070517231] 175 | - [4.574112725799303, 6.402860892735512] 176 | - [4.419811661309642, 6.111833829074321] 177 | - [3.9310094226397236, 6.111833829074321] 178 | shape: polygon 179 | obstacle0022: 180 | corners: 181 | - [-0.348466418556149, -1.8307802077666935] 182 | - [-0.348466418556149, -1.440463843200745] 183 | - [0.10937982500040433, -0.900337801061663] 184 | - [-0.0631951490385041, -1.8307802077666935] 185 | - [-0.348466418556149, -1.8307802077666935] 186 | shape: polygon 187 | obstacle0023: 188 | corners: 189 | - [6.805661552819894, 2.982522464814058] 190 | - [6.805661552819894, 3.3688510927035713] 191 | - [7.765781079091344, 3.0018744065269547] 192 | - [6.997926131082664, 2.982522464814058] 193 | - [6.805661552819894, 2.982522464814058] 194 | shape: polygon 195 | obstacle0024: 196 | corners: 197 | - [0.490673679939456, 1.6226644463327844] 198 | - [0.490673679939456, 2.5796188917712026] 199 | - [0.9491739199907697, 1.857190587916952] 200 | - [1.1878428686377012, 1.6226644463327844] 201 | - [0.490673679939456, 1.6226644463327844] 202 | shape: polygon 203 | obstacle0025: 204 | corners: 205 | - [6.609903087195489, 5.9963294341191755] 206 | - [6.609903087195489, 6.248968449589411] 207 | - [7.433625013756638, 6.136076790288626] 208 | - [6.648247694184747, 5.9963294341191755] 209 | - [6.609903087195489, 5.9963294341191755] 210 | shape: polygon 211 | obstacle0026: 212 | corners: 213 | - [5.363865915960956, 5.7041462488317425] 214 | - [5.363865915960956, 5.740751675604633] 215 | - [6.259282576552298, 5.954655040363959] 216 | - [5.667975638031967, 5.7041462488317425] 217 | - [5.363865915960956, 5.7041462488317425] 218 | shape: polygon 219 | obstacle0027: 220 | corners: 221 | - [-0.3109484100027209, 0.1368169641854986] 222 | - [-0.3109484100027209, 1.0979338009180655] 223 | - [-0.10458040718748629, 0.31090430103541933] 224 | - [0.4857256821571726, 0.1368169641854986] 225 | - [-0.3109484100027209, 0.1368169641854986] 226 | shape: polygon 227 | obstacle0028: 228 | corners: 229 | - [7.306122084011111, 4.598091718842464] 230 | - [7.306122084011111, 5.413296213185481] 231 | - [7.876936836866218, 5.510787778839934] 232 | - [7.605338470380387, 4.598091718842464] 233 | - [7.306122084011111, 4.598091718842464] 234 | shape: polygon 235 | obstacle0029: 236 | corners: 237 | - [-0.9391902261775713, -0.1484796564386044] 238 | - [-0.9391902261775713, 0.6373266497816434] 239 | - [-0.07602805418922476, 0.7164719366966849] 240 | - [-0.5433674672529825, -0.1484796564386044] 241 | - [-0.9391902261775713, -0.1484796564386044] 242 | shape: polygon 243 | obstacle0030: 244 | corners: 245 | - [6.907327088726943, 0.6201515742891135] 246 | - [6.907327088726943, 0.7670052692741286] 247 | - [7.482136754047054, 0.6585644786763131] 248 | - [7.717625219015401, 0.6201515742891135] 249 | - [6.907327088726943, 0.6201515742891135] 250 | shape: polygon 251 | obstacle0031: 252 | corners: 253 | - [10.730193572271046, 0.50980037322699] 254 | - [10.730193572271046, 1.1081452835723558] 255 | - [10.977059371544, 0.5146203539798301] 256 | - [10.87453047330504, 0.50980037322699] 257 | - [10.730193572271046, 0.50980037322699] 258 | shape: polygon 259 | obstacle0032: 260 | corners: 261 | - [-0.02072267884039447, 7.3794577801834595] 262 | - [-0.02072267884039447, 7.433144782710557] 263 | - [0.8942783825231081, 7.430087181119402] 264 | - [0.6526602799180704, 7.3794577801834595] 265 | - [-0.02072267884039447, 7.3794577801834595] 266 | shape: polygon 267 | obstacle0033: 268 | corners: 269 | - [10.769242388357569, 2.0108768602215825] 270 | - [10.769242388357569, 2.7620337273299644] 271 | - [11.290026696269294, 2.2588043557233553] 272 | - [11.31311975992044, 2.0108768602215825] 273 | - [10.769242388357569, 2.0108768602215825] 274 | shape: polygon 275 | obstacle0034: 276 | corners: 277 | - [7.841347665366101, 2.273955796319173] 278 | - [7.841347665366101, 2.5601308874148248] 279 | - [8.238522379724763, 3.1806985300573087] 280 | - [8.692921352000903, 2.273955796319173] 281 | - [7.841347665366101, 2.273955796319173] 282 | shape: polygon 283 | obstacle0035: 284 | corners: 285 | - [3.935400352807674, 1.9929946460360055] 286 | - [3.935400352807674, 2.65398341162597] 287 | - [4.648289373141967, 2.5485673509158504] 288 | - [4.234807271228027, 1.9929946460360055] 289 | - [3.935400352807674, 1.9929946460360055] 290 | shape: polygon 291 | obstacle0036: 292 | corners: 293 | - [4.3696590417506265, 3.779760218672047] 294 | - [4.3696590417506265, 3.9985748037929065] 295 | - [5.364319853046733, 3.9977742441265587] 296 | - [4.865339955507353, 3.779760218672047] 297 | - [4.3696590417506265, 3.779760218672047] 298 | shape: polygon 299 | obstacle0037: 300 | corners: 301 | - [4.972866023109896, 2.894240967684075] 302 | - [4.972866023109896, 3.456234847922146] 303 | - [5.270132752106374, 3.1304773229953806] 304 | - [5.647400042148806, 2.894240967684075] 305 | - [4.972866023109896, 2.894240967684075] 306 | shape: polygon 307 | obstacle0038: 308 | corners: 309 | - [-0.4155976392300724, -2.3586271817934468] 310 | - [-0.4155976392300724, -1.7152214152181804] 311 | - [0.32664863322838233, -2.2060759353329593] 312 | - [0.0011081459473338828, -2.3586271817934468] 313 | - [-0.4155976392300724, -2.3586271817934468] 314 | shape: polygon 315 | obstacle0039: 316 | corners: 317 | - [-1.8449159713737653, 5.899238708655524] 318 | - [-1.8449159713737653, 6.642132384757154] 319 | - [-1.5818563610515635, 6.124186707162927] 320 | - [-1.26067591636474, 5.899238708655524] 321 | - [-1.8449159713737653, 5.899238708655524] 322 | shape: polygon 323 | -------------------------------------------------------------------------------- /environment.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | import math 3 | 4 | import shapely.geometry as geom 5 | from shapely import affinity 6 | import itertools 7 | from matplotlib import pyplot as plt 8 | from descartes import PolygonPatch 9 | 10 | 11 | def plot_environment(env, bounds=None, figsize=None): 12 | if bounds is None and env.bounds: 13 | minx, miny, maxx, maxy = env.bounds 14 | elif bounds: 15 | minx, miny, maxx, maxy = bounds 16 | else: 17 | minx, miny, maxx, maxy = (-10,-5,10,5) 18 | 19 | max_width, max_height = 12, 5.5 20 | if figsize is None: 21 | width, height = max_width, (maxy-miny)*max_width/(maxx-minx) 22 | if height > 5: 23 | width, height = (maxx-minx)*max_height/(maxy-miny), max_height 24 | figsize = (width, height) 25 | print figsize 26 | f = plt.figure(figsize=figsize) 27 | f.hold('on') 28 | ax = f.add_subplot(111) 29 | for i, obs in enumerate(env.obstacles): 30 | patch = PolygonPatch(obs, fc='blue', ec='blue', alpha=0.5, zorder=20) 31 | ax.add_patch(patch) 32 | 33 | plt.xlim([minx, maxx]) 34 | plt.ylim([miny, maxy]) 35 | ax.set_aspect('equal', adjustable='box') 36 | return ax 37 | 38 | def plot_line(ax, line): 39 | x, y = line.xy 40 | ax.plot(x, y, color='gray', linewidth=1, solid_capstyle='round', zorder=1) 41 | 42 | 43 | def plot_poly(ax, poly, color, alpha=1.0, zorder=1): 44 | patch = PolygonPatch(poly, fc=color, ec="black", alpha=alpha, zorder=zorder) 45 | ax.add_patch(patch) 46 | 47 | class Environment: 48 | def __init__(self, yaml_file=None, bounds=None): 49 | self.yaml_file = yaml_file 50 | self.environment_loaded = False 51 | self.obstacles = [] 52 | self.obstacles_map = {} 53 | self.bounds = bounds 54 | if not yaml_file is None: 55 | if self.load_from_yaml_file(yaml_file): 56 | if bounds is None: 57 | self.calculate_scene_dimensions() 58 | self.environment_loaded = True 59 | 60 | @property 61 | def bounds(self): 62 | return self.bounds 63 | 64 | def add_obstacles(self, obstacles): 65 | self.obstacles = self.obstacles + obstacles 66 | self.calculate_scene_dimensions() 67 | 68 | def calculate_scene_dimensions(self): 69 | """Compute scene bounds from obstacles.""" 70 | points = [] 71 | for elem in self.obstacles: 72 | points = points + list(elem.boundary.coords) 73 | 74 | mp = geom.MultiPoint(points) 75 | self.bounds = mp.bounds 76 | 77 | def load_from_yaml_file(self, yaml_file): 78 | f = open(yaml_file) 79 | self.data = yaml.safe_load(f) 80 | f.close() 81 | return self.parse_yaml_data(self.data) 82 | 83 | def parse_yaml_data(self, data): 84 | if 'environment' in data: 85 | env = data['environment'] 86 | self.parse_yaml_obstacles(env['obstacles']) 87 | # self.parse_yaml_features(env['features']) 88 | return True 89 | else: 90 | return False 91 | 92 | def parse_yaml_obstacles(self, obstacles): 93 | self.obstacles = [] 94 | self.obstacles_map = {} 95 | for name, description in obstacles.iteritems(): 96 | # Double underscore not allowed in region names. 97 | if name.find("__") != -1: 98 | raise Exception("Names cannot contain double underscores.") 99 | if description['shape'] == 'rectangle': 100 | parsed = self.parse_rectangle(name, description) 101 | elif description['shape'] == 'polygon': 102 | parsed = self.parse_polygon(name, description) 103 | else: 104 | raise Exception("not a rectangle") 105 | if not parsed.is_valid: 106 | raise Exception("%s is not valid!"%name) 107 | self.obstacles.append(parsed) 108 | self.obstacles_map[name] = parsed 109 | self.expanded_obstacles = [obs.buffer(0.75/2, resolution=2) for obs in self.obstacles] 110 | 111 | 112 | def parse_rectangle(self, name, description): 113 | center = description['center'] 114 | center = geom.Point((center[0], center[1])) 115 | length = description['length'] 116 | width = description['width'] 117 | # convert rotation to radians 118 | rotation = description['rotation']# * math.pi/180 119 | # figure out the four corners. 120 | corners = [(center.x - length/2., center.y - width/2.), 121 | (center.x + length/2., center.y - width/2.), 122 | (center.x + length/2., center.y + width/2.), 123 | (center.x - length/2., center.y + width/2.)] 124 | # print corners 125 | polygon = geom.Polygon(corners) 126 | out = affinity.rotate(polygon, rotation, origin=center) 127 | out.name = name 128 | out.cc_length = length 129 | out.cc_width = width 130 | out.cc_rotation = rotation 131 | return out 132 | 133 | def parse_polygon(self, name, description): 134 | _points = description['corners'] 135 | for points in itertools.permutations(_points): 136 | polygon = geom.Polygon(points) 137 | polygon.name = name 138 | if polygon.is_valid: 139 | return polygon 140 | 141 | def save_to_yaml(self, yaml_file): 142 | yaml_dict = {} 143 | obstacles = {} 144 | for i, ob in enumerate(self.obstacles): 145 | ob_dict = {} 146 | ob_dict['shape'] = 'polygon' 147 | ob_dict['corners'] = [list(t) for t in list(ob.boundary.coords)] 148 | ob_name = "obstacle%.4d"%i 149 | obstacles[ob_name] = ob_dict 150 | yaml_dict['environment'] = {'obstacles' : obstacles} 151 | 152 | f = open(yaml_file, 'w') 153 | f.write(yaml.dump(yaml_dict, default_flow_style=None)) 154 | f.close() 155 | 156 | -------------------------------------------------------------------------------- /graph.py: -------------------------------------------------------------------------------- 1 | #import pydot_ng as pydot 2 | import networkx as nx 3 | import matplotlib.pyplot as plt 4 | 5 | class NodeNotInGraph(Exception): 6 | def __init__(self, node): 7 | self.node = node 8 | 9 | def __str__(self): 10 | return "Node %s not in graph." % str(self.node) 11 | 12 | 13 | class Edge(object): 14 | def __init__(self, source, target, weight=1.0): 15 | self.source = source 16 | self.target = target 17 | self.weight = weight 18 | 19 | def __hash__(self): 20 | return hash("%s_%s_%f" % (self.source, self.target, self.weight)) 21 | 22 | def __eq__(self, other): 23 | return self.source == other.source and self.target == other.target \ 24 | and self.weight == other.weight 25 | def __repr__(self): 26 | return "Edge(%r,%r,%r)" % (self.source, self.target, self.weight) 27 | 28 | 29 | class Graph(object): 30 | def __init__(self, node_label_fn=None): 31 | self._nodes = set() 32 | self._edges = dict() 33 | self.node_label_fn = node_label_fn if node_label_fn else lambda x: x 34 | self.node_positions = dict() 35 | 36 | def __contains__(self, node): 37 | return node in self._nodes 38 | 39 | def add_node(self, node): 40 | """Adds a node to the graph.""" 41 | self._nodes.add(node) 42 | 43 | def add_edge(self, node1, node2, weight=1.0, bidirectional=True): 44 | """Adds an edge between node1 and node2. Adds the nodes to the graph first 45 | if they don't exist.""" 46 | self.add_node(node1) 47 | self.add_node(node2) 48 | node1_edges = self._edges.get(node1, set()) 49 | node1_edges.add(Edge(node1, node2, weight)) 50 | self._edges[node1] = node1_edges 51 | if bidirectional: 52 | node2_edges = self._edges.get(node2, set()) 53 | node2_edges.add(Edge(node2, node1, weight)) 54 | self._edges[node2] = node2_edges 55 | 56 | def set_node_positions(self, positions): 57 | self.node_positions = positions 58 | 59 | def set_node_pos(self, node, pos): 60 | """Sets the (x,y) pos of the node, if it exists in the graph.""" 61 | if not node in self: 62 | raise NodeNotInGraph(node) 63 | self.node_positions[node] = pos 64 | 65 | def get_node_pos(self, node): 66 | if not node in self: 67 | raise NodeNotInGraph(node) 68 | return self.node_positions[node] 69 | 70 | def node_edges(self, node): 71 | if not node in self: 72 | raise NodeNotInGraph(node) 73 | return self._edges.get(node, set()) 74 | 75 | def draw(self, highlight_edges=None): 76 | nxg = nx.DiGraph() 77 | edges = [(e.source, e.target, {'weight':e.weight, 'inv_weight':1.0/e.weight}) for node_set in self._edges.values() for e in node_set] 78 | nxg.add_edges_from(edges) 79 | if len(self.node_positions) < len(self._nodes): 80 | # Calculate positions for nodes whose pos is not specified. 81 | pos = nx.spring_layout(nxg, weight='inv_weight', pos=self.node_positions, fixed=self.node_positions.keys() if self.node_positions else None) 82 | else: 83 | pos = self.node_positions 84 | 85 | f = plt.figure(figsize=(12,12)) 86 | plt.gca().set_aspect('equal', adjustable='box') 87 | nx.draw_networkx_nodes(nxg, pos, node_color='w') 88 | nx.draw_networkx_edges(nxg, pos, edges) 89 | nx.draw_networkx_labels(nxg, pos) 90 | edge_labels=dict([((u,v,),"%s" % d['weight']) 91 | for u,v,d in nxg.edges(data=True)]) 92 | nx.draw_networkx_edge_labels(nxg, pos, edge_labels=edge_labels) 93 | 94 | 95 | if highlight_edges: 96 | nx.draw_networkx_edges(nxg, pos, highlight_edges, edge_color='r') 97 | 98 | plt.axis('off') 99 | plt.show() 100 | 101 | def draw_edges(self, edges): 102 | # print edges 103 | nx.draw_networkx_edges(nxg, pos, edges, edge_color='r') 104 | reduced_labels = {(u,v): edge_labels[(u,v)] for u,v,_ in edges} 105 | nx.draw_networkx_edge_labels(nxg, pos, edge_labels=reduced_labels, font_color='r') 106 | 107 | reduced_nodes = set([u for u,_,_ in edges]) 108 | reduced_nodes.update([v for _,v,_ in edges]) 109 | # nx.draw_networkx_nodes(nxg, pos, nodelist=reduced_nodes, node_color='r') 110 | red_labels = {n:n for n in reduced_nodes} 111 | print red_labels 112 | nx.draw_networkx_labels(nxg, pos, labels=red_labels, font_color='r') 113 | 114 | def highlight_edges(self, edges): 115 | nx.draw_networkx_edges(nxg, pos, edges, edge_color='r') 116 | reduced_labels = {(u,v): edge_labels[(u,v)] for u,v,_ in edges} 117 | nx.draw_networkx_edge_labels(nxg, pos, edge_labels=reduced_labels, font_color='r') 118 | 119 | reduced_nodes = set([u for u,_,_ in edges]) 120 | reduced_nodes.update([v for _,v,_ in edges]) 121 | # nx.draw_networkx_nodes(nxg, pos, nodelist=reduced_nodes, node_color='r') 122 | red_labels = {n:n for n in reduced_nodes} 123 | print red_labels 124 | nx.draw_networkx_labels(nxg, pos, labels=red_labels, font_color='r') 125 | ''' 126 | def _create_dot_graph(self): 127 | dot_graph = pydot.Dot(graph_type='digraph', concentrate=True, rankdir="LR") 128 | dot_graph.set_node_defaults(shape='rect', fontsize=12) 129 | for n in self._nodes: 130 | node_name = self.node_label_fn(n) 131 | node = pydot.Node(shape="ellipse", name=node_name) 132 | if n in self.node_positions: 133 | node.set_pos("%d,%d!" % (self.node_positions[n][0], self.node_positions[n][1])) 134 | dot_graph.add_node(node) 135 | for src_node, edges in self._edges.items(): 136 | for e in edges: 137 | dot_graph.add_edge(pydot.Edge(self.node_label_fn(src_node), self.node_label_fn(e.target), label=e.weight if e.weight!=1.0 else "")) 138 | return dot_graph 139 | 140 | def _repr_svg_(self): 141 | return self._create_dot_graph().create_svg() 142 | ''' 143 | -------------------------------------------------------------------------------- /hard_environment.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | obstacles: 3 | obstacle0000: 4 | corners: 5 | - [10.541006280700623, 5.802977616023888] 6 | - [10.541006280700623, 6.200427396494111] 7 | - [10.814326712005043, 5.977697592799181] 8 | - [10.951265971294792, 5.802977616023888] 9 | - [10.541006280700623, 5.802977616023888] 10 | shape: polygon 11 | obstacle0001: 12 | corners: 13 | - [9.371487731643258, 0.4712549408585174] 14 | - [9.371487731643258, 0.5160548106886471] 15 | - [9.489391081637274, 0.7904886681674737] 16 | - [9.479680176962471, 0.4712549408585174] 17 | - [9.371487731643258, 0.4712549408585174] 18 | shape: polygon 19 | obstacle0002: 20 | corners: 21 | - [1.889413023995501, 7.305262770694151] 22 | - [1.889413023995501, 7.5021218221667985] 23 | - [2.331197058196516, 7.570132966242142] 24 | - [1.8903884784987852, 7.305262770694151] 25 | - [1.889413023995501, 7.305262770694151] 26 | shape: polygon 27 | obstacle0003: 28 | corners: 29 | - [9.23312425439498, -0.2626050026528315] 30 | - [9.23312425439498, -0.10438248568830066] 31 | - [9.494312620562605, 0.07239919136991718] 32 | - [9.346034258162813, -0.2626050026528315] 33 | - [9.23312425439498, -0.2626050026528315] 34 | shape: polygon 35 | obstacle0004: 36 | corners: 37 | - [6.305143261663616, 6.287491375779087] 38 | - [6.305143261663616, 6.6469504092293485] 39 | - [6.717716689542741, 6.651648729215965] 40 | - [6.635046252077236, 6.287491375779087] 41 | - [6.305143261663616, 6.287491375779087] 42 | shape: polygon 43 | obstacle0005: 44 | corners: 45 | - [8.070617254765333, -0.9900282602482817] 46 | - [8.070617254765333, -0.8781184219375617] 47 | - [8.319660609679566, -0.9221708313153942] 48 | - [8.246707817700031, -0.9900282602482817] 49 | - [8.070617254765333, -0.9900282602482817] 50 | shape: polygon 51 | obstacle0006: 52 | corners: 53 | - [-0.3056280935505904, 4.357491324315681] 54 | - [-0.3056280935505904, 4.783903999328234] 55 | - [-0.2092365684487198, 4.626096831287897] 56 | - [0.13913646799647128, 4.357491324315681] 57 | - [-0.3056280935505904, 4.357491324315681] 58 | shape: polygon 59 | obstacle0007: 60 | corners: 61 | - [4.598449673040057, -2.2662466323693424] 62 | - [4.598449673040057, -2.054053792689184] 63 | - [4.600048283602299, -2.172005428809114] 64 | - [4.81778351730776, -2.2662466323693424] 65 | - [4.598449673040057, -2.2662466323693424] 66 | shape: polygon 67 | obstacle0008: 68 | corners: 69 | - [-1.4451261363196952, 0.08261293367095002] 70 | - [-1.4451261363196952, 0.1346746169745796] 71 | - [-1.3119567388745024, 0.36605427924039924] 72 | - [-1.3780974203151544, 0.08261293367095002] 73 | - [-1.4451261363196952, 0.08261293367095002] 74 | shape: polygon 75 | obstacle0009: 76 | corners: 77 | - [2.1420154629304804, 4.71589618557112] 78 | - [2.1420154629304804, 4.73992260656143] 79 | - [2.3360848793780775, 4.767194639655451] 80 | - [2.143237889786108, 4.71589618557112] 81 | - [2.1420154629304804, 4.71589618557112] 82 | shape: polygon 83 | obstacle0010: 84 | corners: 85 | - [6.068990574564943, -2.352612299453624] 86 | - [6.068990574564943, -2.189089536525233] 87 | - [6.269502619303295, -1.9490452010360886] 88 | - [6.330739068211376, -2.352612299453624] 89 | - [6.068990574564943, -2.352612299453624] 90 | shape: polygon 91 | obstacle0011: 92 | corners: 93 | - [7.381440624363471, 7.686135076589494] 94 | - [7.381440624363471, 7.815464913248987] 95 | - [7.643547165031658, 8.130983648851084] 96 | - [7.665945867387487, 7.686135076589494] 97 | - [7.381440624363471, 7.686135076589494] 98 | shape: polygon 99 | obstacle0012: 100 | corners: 101 | - [6.292111494970804, -2.1438185501864044] 102 | - [6.292111494970804, -2.09731519556615] 103 | - [6.560708044261034, -1.68088464347867] 104 | - [6.368374201382196, -2.1438185501864044] 105 | - [6.292111494970804, -2.1438185501864044] 106 | shape: polygon 107 | obstacle0013: 108 | corners: 109 | - [5.071805402266033, 4.109941445614866] 110 | - [5.071805402266033, 4.5702194788876085] 111 | - [5.479987136342182, 4.350166150010335] 112 | - [5.459507839164229, 4.109941445614866] 113 | - [5.071805402266033, 4.109941445614866] 114 | shape: polygon 115 | obstacle0014: 116 | corners: 117 | - [1.4869187946538713, -0.6914498358325156] 118 | - [1.4869187946538713, -0.6412816038570794] 119 | - [1.5255525063463546, -0.4033439358581279] 120 | - [1.9605359431049578, -0.6914498358325156] 121 | - [1.4869187946538713, -0.6914498358325156] 122 | shape: polygon 123 | obstacle0015: 124 | corners: 125 | - [0.14116174786776803, 0.5522592203974601] 126 | - [0.14116174786776803, 0.6439885061305095] 127 | - [0.5960895582545254, 0.8854898715737779] 128 | - [0.5446736634908401, 0.5522592203974601] 129 | - [0.14116174786776803, 0.5522592203974601] 130 | shape: polygon 131 | obstacle0016: 132 | corners: 133 | - [-1.1368218080038883, -2.976911074233665] 134 | - [-1.1368218080038883, -2.8785505397404862] 135 | - [-0.723276933093757, -2.7361751016206766] 136 | - [-0.6515764713217478, -2.976911074233665] 137 | - [-1.1368218080038883, -2.976911074233665] 138 | shape: polygon 139 | obstacle0017: 140 | corners: 141 | - [7.810077591748387, 6.556715380590957] 142 | - [7.810077591748387, 6.826511871260958] 143 | - [7.966749742698549, 6.865716525634949] 144 | - [8.007119211663023, 6.556715380590957] 145 | - [7.810077591748387, 6.556715380590957] 146 | shape: polygon 147 | obstacle0018: 148 | corners: 149 | - [2.790566930296892, 1.489657905655057] 150 | - [2.790566930296892, 1.5664846626651423] 151 | - [3.0706223352746904, 1.7772707576191866] 152 | - [2.9489565022020505, 1.489657905655057] 153 | - [2.790566930296892, 1.489657905655057] 154 | shape: polygon 155 | obstacle0019: 156 | corners: 157 | - [9.235441911747937, 3.306642323122282] 158 | - [9.235441911747937, 3.5264951036605514] 159 | - [9.52411296240937, 3.7653061954445524] 160 | - [9.377739814207107, 3.306642323122282] 161 | - [9.235441911747937, 3.306642323122282] 162 | shape: polygon 163 | obstacle0020: 164 | corners: 165 | - [9.011210238200801, 4.241054558361154] 166 | - [9.011210238200801, 4.53751115156643] 167 | - [9.078123357060152, 4.265796731072795] 168 | - [9.081861566326976, 4.241054558361154] 169 | - [9.011210238200801, 4.241054558361154] 170 | shape: polygon 171 | obstacle0021: 172 | corners: 173 | - [11.938143579996131, 3.153422198016396] 174 | - [11.938143579996131, 3.6180211873258155] 175 | - [12.293658291849528, 3.2137910655105144] 176 | - [12.365267744233506, 3.153422198016396] 177 | - [11.938143579996131, 3.153422198016396] 178 | shape: polygon 179 | obstacle0022: 180 | corners: 181 | - [5.526047228481619, -2.0786005694265253] 182 | - [5.526047228481619, -1.7565108406479495] 183 | - [5.742234919857772, -2.0331970666098154] 184 | - [5.836793820250222, -2.0786005694265253] 185 | - [5.526047228481619, -2.0786005694265253] 186 | shape: polygon 187 | obstacle0023: 188 | corners: 189 | - [-0.001719799258930621, -0.4321753240360464] 190 | - [-0.001719799258930621, -0.2894649287995778] 191 | - [0.4532087655958255, -0.10343588299561429] 192 | - [0.15687827105757035, -0.4321753240360464] 193 | - [-0.001719799258930621, -0.4321753240360464] 194 | shape: polygon 195 | obstacle0024: 196 | corners: 197 | - [11.64368098357464, -0.7404023142353608] 198 | - [11.64368098357464, -0.58112159732462] 199 | - [11.778970303435832, -0.7133785668420687] 200 | - [11.815371322108424, -0.7404023142353608] 201 | - [11.64368098357464, -0.7404023142353608] 202 | shape: polygon 203 | obstacle0025: 204 | corners: 205 | - [1.39284471179261, 1.6907708522505178] 206 | - [1.39284471179261, 1.9985763482159864] 207 | - [1.7352991032272613, 2.030230283253751] 208 | - [1.886141977798935, 1.6907708522505178] 209 | - [1.39284471179261, 1.6907708522505178] 210 | shape: polygon 211 | obstacle0026: 212 | corners: 213 | - [9.44812034963401, 3.3602247567281873] 214 | - [9.44812034963401, 3.6282016400717865] 215 | - [9.753466670059481, 3.5830630236319467] 216 | - [9.892784787568543, 3.3602247567281873] 217 | - [9.44812034963401, 3.3602247567281873] 218 | shape: polygon 219 | obstacle0027: 220 | corners: 221 | - [2.218848059282264, -2.5904337212281563] 222 | - [2.218848059282264, -2.427030210067971] 223 | - [2.6342988929499715, -2.3327187733792742] 224 | - [2.430826941575915, -2.5904337212281563] 225 | - [2.218848059282264, -2.5904337212281563] 226 | shape: polygon 227 | obstacle0028: 228 | corners: 229 | - [10.736595181041958, 2.24994376338908] 230 | - [10.736595181041958, 2.7196802271610645] 231 | - [10.947600845056833, 2.7230181178787705] 232 | - [10.845788901851442, 2.24994376338908] 233 | - [10.736595181041958, 2.24994376338908] 234 | shape: polygon 235 | obstacle0029: 236 | corners: 237 | - [5.485888122194506, -2.189577803759496] 238 | - [5.485888122194506, -1.7785379411688647] 239 | - [5.916108928384502, -1.771148785756576] 240 | - [5.840182220410708, -2.189577803759496] 241 | - [5.485888122194506, -2.189577803759496] 242 | shape: polygon 243 | obstacle0030: 244 | corners: 245 | - [3.310983754646303, 6.091022258516851] 246 | - [3.310983754646303, 6.1157733308196205] 247 | - [3.6900890194952143, 6.578871740615594] 248 | - [3.4034402331645737, 6.091022258516851] 249 | - [3.310983754646303, 6.091022258516851] 250 | shape: polygon 251 | obstacle0031: 252 | corners: 253 | - [4.907584980876993, 0.05044404414541148] 254 | - [4.907584980876993, 0.3681470745331679] 255 | - [5.2040238155870435, 0.5080490349633691] 256 | - [5.033083477255353, 0.05044404414541148] 257 | - [4.907584980876993, 0.05044404414541148] 258 | shape: polygon 259 | obstacle0032: 260 | corners: 261 | - [9.758657107128098, 1.0394866821870812] 262 | - [9.758657107128098, 1.0776034419506069] 263 | - [10.004505331185811, 1.4925101115846964] 264 | - [9.906836127259487, 1.0394866821870812] 265 | - [9.758657107128098, 1.0394866821870812] 266 | shape: polygon 267 | obstacle0033: 268 | corners: 269 | - [7.797058844047578, -1.5210315715300171] 270 | - [7.797058844047578, -1.0271995588886957] 271 | - [7.960159444967557, -1.1769819408004072] 272 | - [8.252952241103127, -1.5210315715300171] 273 | - [7.797058844047578, -1.5210315715300171] 274 | shape: polygon 275 | obstacle0034: 276 | corners: 277 | - [4.829012869383646, 5.096994475774416] 278 | - [4.829012869383646, 5.494370398078239] 279 | - [4.999394574434983, 5.377975982532807] 280 | - [5.209638209393141, 5.096994475774416] 281 | - [4.829012869383646, 5.096994475774416] 282 | shape: polygon 283 | obstacle0035: 284 | corners: 285 | - [6.110282998815578, 1.6018894484973218] 286 | - [6.110282998815578, 1.941074868212656] 287 | - [6.415926785024004, 1.8240242447954893] 288 | - [6.514539053180819, 1.6018894484973218] 289 | - [6.110282998815578, 1.6018894484973218] 290 | shape: polygon 291 | obstacle0036: 292 | corners: 293 | - [0.5845332167987465, -1.0967739442835187] 294 | - [0.5845332167987465, -0.9086701263316992] 295 | - [0.6805198982956882, -0.688130109620831] 296 | - [0.653066555867891, -1.0967739442835187] 297 | - [0.5845332167987465, -1.0967739442835187] 298 | shape: polygon 299 | obstacle0037: 300 | corners: 301 | - [-1.2274877510447824, 5.50080788473462] 302 | - [-1.2274877510447824, 5.570033417310138] 303 | - [-1.1989470167978817, 5.809029974160012] 304 | - [-1.2022400929017196, 5.50080788473462] 305 | - [-1.2274877510447824, 5.50080788473462] 306 | shape: polygon 307 | obstacle0038: 308 | corners: 309 | - [-1.2789554767068276, 3.3836208385760225] 310 | - [-1.2789554767068276, 3.5584547632817425] 311 | - [-0.9331189393677288, 3.5961746399482353] 312 | - [-1.257158122983222, 3.3836208385760225] 313 | - [-1.2789554767068276, 3.3836208385760225] 314 | shape: polygon 315 | obstacle0039: 316 | corners: 317 | - [2.983452749639852, 6.0713888261697555] 318 | - [2.983452749639852, 6.303534238938726] 319 | - [3.4730495839510542, 6.301160450435157] 320 | - [3.4575760326839067, 6.0713888261697555] 321 | - [2.983452749639852, 6.0713888261697555] 322 | shape: polygon 323 | obstacle0040: 324 | corners: 325 | - [6.812284829516445, -0.5052590365514118] 326 | - [6.812284829516445, -0.1571393539530701] 327 | - [7.269093867944412, -0.32892040386549865] 328 | - [7.188142841021887, -0.5052590365514118] 329 | - [6.812284829516445, -0.5052590365514118] 330 | shape: polygon 331 | obstacle0041: 332 | corners: 333 | - [2.9269791038327853, 4.057768953331344] 334 | - [2.9269791038327853, 4.423171281178856] 335 | - [3.3636011543864073, 4.4611829834142025] 336 | - [3.0126420283005215, 4.057768953331344] 337 | - [2.9269791038327853, 4.057768953331344] 338 | shape: polygon 339 | obstacle0042: 340 | corners: 341 | - [11.265638713738985, 5.8794078741123545] 342 | - [11.265638713738985, 6.360777882142309] 343 | - [11.452893642399525, 6.293212810083998] 344 | - [11.760721225863366, 5.8794078741123545] 345 | - [11.265638713738985, 5.8794078741123545] 346 | shape: polygon 347 | obstacle0043: 348 | corners: 349 | - [7.597727967268556, 1.0331878413910154] 350 | - [7.597727967268556, 1.1695458925410602] 351 | - [7.694311374922203, 1.4472566908917666] 352 | - [7.830956366411029, 1.0331878413910154] 353 | - [7.597727967268556, 1.0331878413910154] 354 | shape: polygon 355 | obstacle0044: 356 | corners: 357 | - [5.205051444352332, -1.9363365220857247] 358 | - [5.205051444352332, -1.6081237472596817] 359 | - [5.433889529009372, -1.6391229936185354] 360 | - [5.528194827145246, -1.9363365220857247] 361 | - [5.205051444352332, -1.9363365220857247] 362 | shape: polygon 363 | obstacle0045: 364 | corners: 365 | - [4.596612988352727, -0.41884041452520515] 366 | - [4.596612988352727, -0.34526399738569236] 367 | - [4.9942873312814715, -0.2277698858441516] 368 | - [4.632391385789873, -0.41884041452520515] 369 | - [4.596612988352727, -0.41884041452520515] 370 | shape: polygon 371 | obstacle0046: 372 | corners: 373 | - [1.7140222223019808, 5.746464967181149] 374 | - [1.7140222223019808, 5.858605715169975] 375 | - [1.8045682576704583, 6.23045257065979] 376 | - [2.1021447611251256, 5.746464967181149] 377 | - [1.7140222223019808, 5.746464967181149] 378 | shape: polygon 379 | obstacle0047: 380 | corners: 381 | - [1.5984754413350517, 6.302990122143479] 382 | - [1.5984754413350517, 6.458940332493194] 383 | - [1.886626475257895, 6.524987387919462] 384 | - [1.9196873004395303, 6.302990122143479] 385 | - [1.5984754413350517, 6.302990122143479] 386 | shape: polygon 387 | obstacle0048: 388 | corners: 389 | - [10.343392816504807, 6.536750944344639] 390 | - [10.343392816504807, 6.631122118170444] 391 | - [10.611927160141178, 6.698168491821299] 392 | - [10.834505573478415, 6.536750944344639] 393 | - [10.343392816504807, 6.536750944344639] 394 | shape: polygon 395 | obstacle0049: 396 | corners: 397 | - [-0.29321963520175887, 5.428173034315609] 398 | - [-0.29321963520175887, 5.463052287257741] 399 | - [-0.2780615894402927, 5.823070230334386] 400 | - [-0.0402434884573295, 5.428173034315609] 401 | - [-0.29321963520175887, 5.428173034315609] 402 | shape: polygon 403 | obstacle0050: 404 | corners: 405 | - [4.294090315272281, 1.0022507263745535] 406 | - [4.294090315272281, 1.1227404938927252] 407 | - [4.480212814285704, 1.0399170502673816] 408 | - [4.741495115065223, 1.0022507263745535] 409 | - [4.294090315272281, 1.0022507263745535] 410 | shape: polygon 411 | obstacle0051: 412 | corners: 413 | - [3.7078104574931423, 2.407423430560298] 414 | - [3.7078104574931423, 2.6653616631723156] 415 | - [3.962632592642504, 2.6608649125304957] 416 | - [4.040499492212192, 2.407423430560298] 417 | - [3.7078104574931423, 2.407423430560298] 418 | shape: polygon 419 | obstacle0052: 420 | corners: 421 | - [9.8432583847123, 3.394728532557325] 422 | - [9.8432583847123, 3.5521083335102492] 423 | - [10.149910454987491, 3.7052121418815185] 424 | - [10.039541053159228, 3.394728532557325] 425 | - [9.8432583847123, 3.394728532557325] 426 | shape: polygon 427 | obstacle0053: 428 | corners: 429 | - [1.4855168555291192, 3.430144737324243] 430 | - [1.4855168555291192, 3.460349002993958] 431 | - [1.5698075478241298, 3.82329426520539] 432 | - [1.9392991128364856, 3.430144737324243] 433 | - [1.4855168555291192, 3.430144737324243] 434 | shape: polygon 435 | obstacle0054: 436 | corners: 437 | - [3.2948911518491677, 0.6526274472530851] 438 | - [3.2948911518491677, 0.9520540241935729] 439 | - [3.7056624710802017, 1.0737423905069892] 440 | - [3.7127503645941236, 0.6526274472530851] 441 | - [3.2948911518491677, 0.6526274472530851] 442 | shape: polygon 443 | obstacle0055: 444 | corners: 445 | - [8.067140689100352, 0.6874064399294095] 446 | - [8.067140689100352, 0.9361269354904996] 447 | - [8.444273118077508, 1.027402961543543] 448 | - [8.559175064322394, 0.6874064399294095] 449 | - [8.067140689100352, 0.6874064399294095] 450 | shape: polygon 451 | obstacle0056: 452 | corners: 453 | - [5.511092719296774, 4.8407363792223475] 454 | - [5.511092719296774, 4.84561820168008] 455 | - [5.888092412188405, 4.948502378161921] 456 | - [5.558608839251203, 4.8407363792223475] 457 | - [5.511092719296774, 4.8407363792223475] 458 | shape: polygon 459 | obstacle0057: 460 | corners: 461 | - [10.367985022432656, 1.3654113595272293] 462 | - [10.367985022432656, 1.428052909270855] 463 | - [10.431596937974314, 1.6454107131890765] 464 | - [10.667137633950562, 1.3654113595272293] 465 | - [10.367985022432656, 1.3654113595272293] 466 | shape: polygon 467 | obstacle0058: 468 | corners: 469 | - [5.160136039739995, 4.73779394158382] 470 | - [5.160136039739995, 4.81641435084191] 471 | - [5.4733845869793445, 4.77100913606888] 472 | - [5.246868898832078, 4.73779394158382] 473 | - [5.160136039739995, 4.73779394158382] 474 | shape: polygon 475 | obstacle0059: 476 | corners: 477 | - [10.576306860548552, -1.2986046651113292] 478 | - [10.576306860548552, -1.1533263955385642] 479 | - [10.708090268482707, -0.8746673454450555] 480 | - [10.73270690929141, -1.2986046651113292] 481 | - [10.576306860548552, -1.2986046651113292] 482 | shape: polygon 483 | obstacle0060: 484 | corners: 485 | - [3.456749402690292, 3.325358443944044] 486 | - [3.456749402690292, 3.6941771494548457] 487 | - [3.762424798041372, 3.8093174936395915] 488 | - [3.4660581295338964, 3.325358443944044] 489 | - [3.456749402690292, 3.325358443944044] 490 | shape: polygon 491 | obstacle0061: 492 | corners: 493 | - [5.397397865436528, 5.758016020489018] 494 | - [5.397397865436528, 5.827128819620937] 495 | - [5.76739581383978, 6.090899362823151] 496 | - [5.539997250363836, 5.758016020489018] 497 | - [5.397397865436528, 5.758016020489018] 498 | shape: polygon 499 | obstacle0062: 500 | corners: 501 | - [7.850502471382178, 6.722846762450244] 502 | - [7.850502471382178, 6.776695304542487] 503 | - [7.893684852874322, 6.807969166473502] 504 | - [8.323240747500572, 6.722846762450244] 505 | - [7.850502471382178, 6.722846762450244] 506 | shape: polygon 507 | obstacle0063: 508 | corners: 509 | - [0.5675026171687731, -0.37869392186540285] 510 | - [0.5675026171687731, 0.018449378323336707] 511 | - [0.5699709078187141, -0.18234150294237317] 512 | - [0.7423339420072634, -0.37869392186540285] 513 | - [0.5675026171687731, -0.37869392186540285] 514 | shape: polygon 515 | obstacle0064: 516 | corners: 517 | - [8.739720521813494, -0.410266380680258] 518 | - [8.739720521813494, -0.37691432507227923] 519 | - [9.200976805352363, -0.26776556253735967] 520 | - [8.780447366278553, -0.410266380680258] 521 | - [8.739720521813494, -0.410266380680258] 522 | shape: polygon 523 | obstacle0065: 524 | corners: 525 | - [-1.4070877207233337, 3.9380832542031516] 526 | - [-1.4070877207233337, 4.273073475958964] 527 | - [-1.335048033419371, 4.122436360337561] 528 | - [-1.1640816111049874, 3.9380832542031516] 529 | - [-1.4070877207233337, 3.9380832542031516] 530 | shape: polygon 531 | obstacle0066: 532 | corners: 533 | - [7.4452171733416055, 7.211448056864551] 534 | - [7.4452171733416055, 7.630399045058814] 535 | - [7.842847753458106, 7.61456493968851] 536 | - [7.92499792184622, 7.211448056864551] 537 | - [7.4452171733416055, 7.211448056864551] 538 | shape: polygon 539 | obstacle0067: 540 | corners: 541 | - [6.729543252652517, 0.7085411862497653] 542 | - [6.729543252652517, 1.0478062087424054] 543 | - [7.156160805976991, 1.0022414639606985] 544 | - [7.189679485970102, 0.7085411862497653] 545 | - [6.729543252652517, 0.7085411862497653] 546 | shape: polygon 547 | obstacle0068: 548 | corners: 549 | - [7.14987716132827, -2.5463834364338465] 550 | - [7.14987716132827, -2.1341694222097236] 551 | - [7.213045876351135, -2.1931421005262544] 552 | - [7.392123102691868, -2.5463834364338465] 553 | - [7.14987716132827, -2.5463834364338465] 554 | shape: polygon 555 | obstacle0069: 556 | corners: 557 | - [2.896251987261297, 0.7862244296391183] 558 | - [2.896251987261297, 1.241883575947361] 559 | - [3.1764102862791606, 1.2394478717872932] 560 | - [3.3617612709112725, 0.7862244296391183] 561 | - [2.896251987261297, 0.7862244296391183] 562 | shape: polygon 563 | obstacle0070: 564 | corners: 565 | - [5.2562335142982235, 2.1293195073540643] 566 | - [5.2562335142982235, 2.3531193712706777] 567 | - [5.306881207654761, 2.3773693809079566] 568 | - [5.720410474882681, 2.1293195073540643] 569 | - [5.2562335142982235, 2.1293195073540643] 570 | shape: polygon 571 | obstacle0071: 572 | corners: 573 | - [10.587315983078415, 0.03922433812426185] 574 | - [10.587315983078415, 0.27395112110295405] 575 | - [10.942656459458458, 0.2007376998675996] 576 | - [11.021637420060129, 0.03922433812426185] 577 | - [10.587315983078415, 0.03922433812426185] 578 | shape: polygon 579 | obstacle0072: 580 | corners: 581 | - [5.125977598737623, -0.9594512002323237] 582 | - [5.125977598737623, -0.7652376117531918] 583 | - [5.4079582933727774, -0.8706808400869166] 584 | - [5.562256484126843, -0.9594512002323237] 585 | - [5.125977598737623, -0.9594512002323237] 586 | shape: polygon 587 | obstacle0073: 588 | corners: 589 | - [4.545321098118791, 5.94712853202803] 590 | - [4.545321098118791, 6.416956888793299] 591 | - [5.003342013081746, 5.947133286297528] 592 | - [5.040416798443583, 5.94712853202803] 593 | - [4.545321098118791, 5.94712853202803] 594 | shape: polygon 595 | obstacle0074: 596 | corners: 597 | - [7.2919659615146095, 7.7945976620563115] 598 | - [7.2919659615146095, 7.799465005873928] 599 | - [7.716580622979663, 8.056111725461895] 600 | - [7.38573298589603, 7.7945976620563115] 601 | - [7.2919659615146095, 7.7945976620563115] 602 | shape: polygon 603 | obstacle0075: 604 | corners: 605 | - [-0.7737302874926395, 2.8639280269924487] 606 | - [-0.7737302874926395, 2.958933316539124] 607 | - [-0.5657259007470105, 3.032173602688297] 608 | - [-0.4900617800117737, 2.8639280269924487] 609 | - [-0.7737302874926395, 2.8639280269924487] 610 | shape: polygon 611 | obstacle0076: 612 | corners: 613 | - [-1.4356303133886497, 2.874984669460253] 614 | - [-1.4356303133886497, 2.997927522189925] 615 | - [-1.012789589687142, 2.99751792111796] 616 | - [-1.2465684955479515, 2.874984669460253] 617 | - [-1.4356303133886497, 2.874984669460253] 618 | shape: polygon 619 | obstacle0077: 620 | corners: 621 | - [9.045889984649827, 1.0891375326436226] 622 | - [9.045889984649827, 1.4197262414363232] 623 | - [9.147411626746537, 1.4278943521620404] 624 | - [9.484713655334325, 1.0891375326436226] 625 | - [9.045889984649827, 1.0891375326436226] 626 | shape: polygon 627 | obstacle0078: 628 | corners: 629 | - [0.6271216305192433, 0.23478174762474247] 630 | - [0.6271216305192433, 0.5814719094556084] 631 | - [0.6401316035477174, 0.636509331354252] 632 | - [0.9925761791645713, 0.23478174762474247] 633 | - [0.6271216305192433, 0.23478174762474247] 634 | shape: polygon 635 | obstacle0079: 636 | corners: 637 | - [2.307023540205992, 7.193168469188739] 638 | - [2.307023540205992, 7.2533919942601734] 639 | - [2.8061379524264276, 7.660321579398377] 640 | - [2.5876679320698748, 7.193168469188739] 641 | - [2.307023540205992, 7.193168469188739] 642 | shape: polygon 643 | obstacle0080: 644 | corners: 645 | - [10.167478233520484, 3.8941934269089957] 646 | - [10.167478233520484, 4.391648594831972] 647 | - [10.385989752738272, 4.1782416651913605] 648 | - [10.528966447162777, 3.8941934269089957] 649 | - [10.167478233520484, 3.8941934269089957] 650 | shape: polygon 651 | obstacle0081: 652 | corners: 653 | - [11.879895093923972, -1.6452220876396209] 654 | - [11.879895093923972, -1.6433321315329468] 655 | - [12.24679838716821, -1.3215916084511998] 656 | - [12.230517063936057, -1.6452220876396209] 657 | - [11.879895093923972, -1.6452220876396209] 658 | shape: polygon 659 | obstacle0082: 660 | corners: 661 | - [3.050805349867648, -0.5103990733092467] 662 | - [3.050805349867648, -0.09171599525221547] 663 | - [3.3323979197072866, -0.4603539773030857] 664 | - [3.5128909040904195, -0.5103990733092467] 665 | - [3.050805349867648, -0.5103990733092467] 666 | shape: polygon 667 | obstacle0083: 668 | corners: 669 | - [2.0356005251974816, 5.068268754863082] 670 | - [2.0356005251974816, 5.481322004907431] 671 | - [2.0640882157598504, 5.172967088473713] 672 | - [2.3910794003806455, 5.068268754863082] 673 | - [2.0356005251974816, 5.068268754863082] 674 | shape: polygon 675 | obstacle0084: 676 | corners: 677 | - [-1.045040477051853, 3.644399128941002] 678 | - [-1.045040477051853, 4.099112438209909] 679 | - [-0.8522046329532376, 4.104610437745816] 680 | - [-0.6168517789735997, 3.644399128941002] 681 | - [-1.045040477051853, 3.644399128941002] 682 | shape: polygon 683 | obstacle0085: 684 | corners: 685 | - [8.035929953225219, -1.1747521435685844] 686 | - [8.035929953225219, -0.8994946451846779] 687 | - [8.143759321187003, -0.7431726794735621] 688 | - [8.318223125374685, -1.1747521435685844] 689 | - [8.035929953225219, -1.1747521435685844] 690 | shape: polygon 691 | obstacle0086: 692 | corners: 693 | - [-1.8395725165064292, 3.784359449104403] 694 | - [-1.8395725165064292, 3.7898968917565434] 695 | - [-1.7072748880087079, 4.240465677990078] 696 | - [-1.8004544766874022, 3.784359449104403] 697 | - [-1.8395725165064292, 3.784359449104403] 698 | shape: polygon 699 | obstacle0087: 700 | corners: 701 | - [10.47842670293207, 4.263756813786489] 702 | - [10.47842670293207, 4.684733621335576] 703 | - [10.50278588791989, 4.637117454307518] 704 | - [10.915761217796105, 4.263756813786489] 705 | - [10.47842670293207, 4.263756813786489] 706 | shape: polygon 707 | obstacle0088: 708 | corners: 709 | - [-0.6213850864685506, 2.6244142922583853] 710 | - [-0.6213850864685506, 2.817777205433762] 711 | - [-0.4662821371875291, 3.062172020385678] 712 | - [-0.22085559416748973, 2.6244142922583853] 713 | - [-0.6213850864685506, 2.6244142922583853] 714 | shape: polygon 715 | obstacle0089: 716 | corners: 717 | - [5.288359053744408, -1.0995727706567675] 718 | - [5.288359053744408, -0.8877340383143311] 719 | - [5.670135814247118, -1.0484588938080335] 720 | - [5.778292061608171, -1.0995727706567675] 721 | - [5.288359053744408, -1.0995727706567675] 722 | shape: polygon 723 | obstacle0090: 724 | corners: 725 | - [4.380969931158351, 6.200434615630378] 726 | - [4.380969931158351, 6.314543369575248] 727 | - [4.466585546608846, 6.499556227291648] 728 | - [4.677645245312315, 6.200434615630378] 729 | - [4.380969931158351, 6.200434615630378] 730 | shape: polygon 731 | obstacle0091: 732 | corners: 733 | - [1.5224162401893975, 3.9817268231644327] 734 | - [1.5224162401893975, 4.243478277118156] 735 | - [1.9831531798667252, 4.155845663036112] 736 | - [1.7200938316667351, 3.9817268231644327] 737 | - [1.5224162401893975, 3.9817268231644327] 738 | shape: polygon 739 | obstacle0092: 740 | corners: 741 | - [10.61800069163808, 2.7140453729219667] 742 | - [10.61800069163808, 2.9292202531858624] 743 | - [10.671074566369937, 2.944767794276779] 744 | - [10.799827894627926, 2.7140453729219667] 745 | - [10.61800069163808, 2.7140453729219667] 746 | shape: polygon 747 | obstacle0093: 748 | corners: 749 | - [7.112105312975645, 1.501671448092539] 750 | - [7.112105312975645, 1.8939875444676406] 751 | - [7.237445804132998, 1.734768524314492] 752 | - [7.114016184142887, 1.501671448092539] 753 | - [7.112105312975645, 1.501671448092539] 754 | shape: polygon 755 | obstacle0094: 756 | corners: 757 | - [10.033900293357673, 4.21390949516371] 758 | - [10.033900293357673, 4.417033659439725] 759 | - [10.435455852111579, 4.66544851162468] 760 | - [10.227943100720356, 4.21390949516371] 761 | - [10.033900293357673, 4.21390949516371] 762 | shape: polygon 763 | obstacle0095: 764 | corners: 765 | - [9.107965633749028, -1.8339735796881298] 766 | - [9.107965633749028, -1.3410089903517495] 767 | - [9.505493820332106, -1.5905479532391036] 768 | - [9.484665751987329, -1.8339735796881298] 769 | - [9.107965633749028, -1.8339735796881298] 770 | shape: polygon 771 | obstacle0096: 772 | corners: 773 | - [6.349872912164821, 5.612070052425082] 774 | - [6.349872912164821, 6.090751034803253] 775 | - [6.631515254520479, 5.9954552307729525] 776 | - [6.643691664489456, 5.612070052425082] 777 | - [6.349872912164821, 5.612070052425082] 778 | shape: polygon 779 | obstacle0097: 780 | corners: 781 | - [7.111910297077856, -2.1813075167175997] 782 | - [7.111910297077856, -2.1646688318048315] 783 | - [7.278601061498141, -1.9268557604853789] 784 | - [7.435422446967474, -2.1813075167175997] 785 | - [7.111910297077856, -2.1813075167175997] 786 | shape: polygon 787 | obstacle0098: 788 | corners: 789 | - [5.757775875061493, 7.579629424497439] 790 | - [5.757775875061493, 7.964487416465216] 791 | - [5.787820950031786, 7.661760945857748] 792 | - [5.804342573567151, 7.579629424497439] 793 | - [5.757775875061493, 7.579629424497439] 794 | shape: polygon 795 | obstacle0099: 796 | corners: 797 | - [0.3397430586659329, -1.9117380095866117] 798 | - [0.3397430586659329, -1.7532610235562658] 799 | - [0.6670914169266007, -1.6502904937078755] 800 | - [0.7875692209022296, -1.9117380095866117] 801 | - [0.3397430586659329, -1.9117380095866117] 802 | shape: polygon 803 | obstacle0100: 804 | corners: 805 | - [2.5120641111428332, 1.859320900855825] 806 | - [2.5120641111428332, 2.250076654156392] 807 | - [2.6288435780498265, 2.040986022582239] 808 | - [2.6929482815064127, 1.859320900855825] 809 | - [2.5120641111428332, 1.859320900855825] 810 | shape: polygon 811 | obstacle0101: 812 | corners: 813 | - [8.0674234009605, 7.693480779734703] 814 | - [8.0674234009605, 8.188994699901189] 815 | - [8.227962587030355, 7.735403159834499] 816 | - [8.362796410318863, 7.693480779734703] 817 | - [8.0674234009605, 7.693480779734703] 818 | shape: polygon 819 | obstacle0102: 820 | corners: 821 | - [2.1637297949077094, 0.5506902299666101] 822 | - [2.1637297949077094, 0.8295437973817427] 823 | - [2.3899862491410975, 0.8130837303848231] 824 | - [2.596597586984155, 0.5506902299666101] 825 | - [2.1637297949077094, 0.5506902299666101] 826 | shape: polygon 827 | obstacle0103: 828 | corners: 829 | - [2.7695146733008453, 2.422496466378809] 830 | - [2.7695146733008453, 2.4834263266754437] 831 | - [2.8465268830169603, 2.4505579596325453] 832 | - [2.8673813707437055, 2.422496466378809] 833 | - [2.7695146733008453, 2.422496466378809] 834 | shape: polygon 835 | obstacle0104: 836 | corners: 837 | - [1.586715936104004, 5.948260905574232] 838 | - [1.586715936104004, 6.277492726198003] 839 | - [1.7115751608007628, 5.971120929911539] 840 | - [1.9406474546436376, 5.948260905574232] 841 | - [1.586715936104004, 5.948260905574232] 842 | shape: polygon 843 | obstacle0105: 844 | corners: 845 | - [0.8670611697777959, 4.584644621771678] 846 | - [0.8670611697777959, 4.9729268787855325] 847 | - [1.0803840882692735, 4.935115857721978] 848 | - [1.2954484697730495, 4.584644621771678] 849 | - [0.8670611697777959, 4.584644621771678] 850 | shape: polygon 851 | obstacle0106: 852 | corners: 853 | - [3.5456890019440177, -0.7250161628611416] 854 | - [3.5456890019440177, -0.39705202047686716] 855 | - [4.0013096127643895, -0.25980635327357277] 856 | - [3.850224110876296, -0.7250161628611416] 857 | - [3.5456890019440177, -0.7250161628611416] 858 | shape: polygon 859 | obstacle0107: 860 | corners: 861 | - [6.787861816197175, 2.687962856662031] 862 | - [6.787861816197175, 2.826183435632956] 863 | - [6.923679450922211, 3.1413409456711685] 864 | - [7.066899470243859, 2.687962856662031] 865 | - [6.787861816197175, 2.687962856662031] 866 | shape: polygon 867 | obstacle0108: 868 | corners: 869 | - [5.814433797671925, 3.891996003301206] 870 | - [5.814433797671925, 3.9411868497589992] 871 | - [5.852773454874552, 4.2986070829350576] 872 | - [6.172984124381726, 3.891996003301206] 873 | - [5.814433797671925, 3.891996003301206] 874 | shape: polygon 875 | obstacle0109: 876 | corners: 877 | - [2.3036365624955213, 7.3728791255704405] 878 | - [2.3036365624955213, 7.515259991735652] 879 | - [2.583830346139653, 7.698566900380251] 880 | - [2.5012527611769975, 7.3728791255704405] 881 | - [2.3036365624955213, 7.3728791255704405] 882 | shape: polygon 883 | obstacle0110: 884 | corners: 885 | - [6.456311710986437, 1.6941292636726342] 886 | - [6.456311710986437, 1.9970909558510752] 887 | - [6.7070694331329905, 1.831047313210815] 888 | - [6.918801720548818, 1.6941292636726342] 889 | - [6.456311710986437, 1.6941292636726342] 890 | shape: polygon 891 | obstacle0111: 892 | corners: 893 | - [11.969119758023052, 6.556834940075614] 894 | - [11.969119758023052, 6.567056079228323] 895 | - [12.408453820104597, 6.837920919720257] 896 | - [12.216479484870932, 6.556834940075614] 897 | - [11.969119758023052, 6.556834940075614] 898 | shape: polygon 899 | obstacle0112: 900 | corners: 901 | - [0.2691261291599085, 7.418141905650829] 902 | - [0.2691261291599085, 7.726546365394465] 903 | - [0.6893853997202004, 7.5152777630436045] 904 | - [0.3803322481690314, 7.418141905650829] 905 | - [0.2691261291599085, 7.418141905650829] 906 | shape: polygon 907 | obstacle0113: 908 | corners: 909 | - [0.552888156863931, -2.8586624405894643] 910 | - [0.552888156863931, -2.7869624341092987] 911 | - [0.9820861643651826, -2.4168805809562977] 912 | - [0.8042241632294028, -2.8586624405894643] 913 | - [0.552888156863931, -2.8586624405894643] 914 | shape: polygon 915 | obstacle0114: 916 | corners: 917 | - [0.10748807930563498, 5.022004773751188] 918 | - [0.10748807930563498, 5.1476365307539] 919 | - [0.5911311444349412, 5.183634449541428] 920 | - [0.40929195288415776, 5.022004773751188] 921 | - [0.10748807930563498, 5.022004773751188] 922 | shape: polygon 923 | obstacle0115: 924 | corners: 925 | - [1.5692754683738785, 0.7305530682046775] 926 | - [1.5692754683738785, 0.7368138192006319] 927 | - [1.5915223058762864, 0.7352199039697807] 928 | - [1.7474662435622412, 0.7305530682046775] 929 | - [1.5692754683738785, 0.7305530682046775] 930 | shape: polygon 931 | obstacle0116: 932 | corners: 933 | - [3.5489167319014543, -2.6455105679880226] 934 | - [3.5489167319014543, -2.526376395367312] 935 | - [3.815270482095829, -2.3371037496706317] 936 | - [3.937805282962316, -2.6455105679880226] 937 | - [3.5489167319014543, -2.6455105679880226] 938 | shape: polygon 939 | obstacle0117: 940 | corners: 941 | - [7.056585088660592, 0.39611040482622784] 942 | - [7.056585088660592, 0.8662238374675584] 943 | - [7.21626623236966, 0.41261674781885044] 944 | - [7.23873804232923, 0.39611040482622784] 945 | - [7.056585088660592, 0.39611040482622784] 946 | shape: polygon 947 | obstacle0118: 948 | corners: 949 | - [3.4778549274134596, 0.644147353394299] 950 | - [3.4778549274134596, 1.0600692320661251] 951 | - [3.917166590601416, 0.8140554540875943] 952 | - [3.634680470884014, 0.644147353394299] 953 | - [3.4778549274134596, 0.644147353394299] 954 | shape: polygon 955 | obstacle0119: 956 | corners: 957 | - [9.343651118160892, 1.5041516425067742] 958 | - [9.343651118160892, 1.6765950339649933] 959 | - [9.466881634693902, 1.6833266138538734] 960 | - [9.467094536027263, 1.5041516425067742] 961 | - [9.343651118160892, 1.5041516425067742] 962 | shape: polygon 963 | obstacle0120: 964 | corners: 965 | - [0.9885808216406429, -0.498770770666086] 966 | - [0.9885808216406429, -0.3506093792265287] 967 | - [1.2289023265938854, -0.11353044272488144] 968 | - [1.25326439954119, -0.498770770666086] 969 | - [0.9885808216406429, -0.498770770666086] 970 | shape: polygon 971 | obstacle0121: 972 | corners: 973 | - [8.30477008192944, -2.6433450140141677] 974 | - [8.30477008192944, -2.554573605072253] 975 | - [8.337128373397386, -2.3632645922107987] 976 | - [8.70253954518853, -2.6433450140141677] 977 | - [8.30477008192944, -2.6433450140141677] 978 | shape: polygon 979 | obstacle0122: 980 | corners: 981 | - [4.305268682845206, -0.35077043762020477] 982 | - [4.305268682845206, 0.0034651769735369875] 983 | - [4.542867772672027, -0.3349071804228283] 984 | - [4.744593265048073, -0.35077043762020477] 985 | - [4.305268682845206, -0.35077043762020477] 986 | shape: polygon 987 | obstacle0123: 988 | corners: 989 | - [2.4369598750207215, -1.2963561459695077] 990 | - [2.4369598750207215, -1.1791304956368316] 991 | - [2.6368514680980586, -0.9464782741791018] 992 | - [2.747968621556723, -1.2963561459695077] 993 | - [2.4369598750207215, -1.2963561459695077] 994 | shape: polygon 995 | obstacle0124: 996 | corners: 997 | - [11.6908492510472, 0.3897259177553689] 998 | - [11.6908492510472, 0.8080066545492017] 999 | - [11.905316976117058, 0.564399626121618] 1000 | - [11.919012984934959, 0.3897259177553689] 1001 | - [11.6908492510472, 0.3897259177553689] 1002 | shape: polygon 1003 | obstacle0125: 1004 | corners: 1005 | - [8.426142524068085, 6.92563705126034] 1006 | - [8.426142524068085, 7.188959422281538] 1007 | - [8.815770805320469, 7.241705981783072] 1008 | - [8.772157651364525, 6.92563705126034] 1009 | - [8.426142524068085, 6.92563705126034] 1010 | shape: polygon 1011 | obstacle0126: 1012 | corners: 1013 | - [-0.8687840322986449, 5.946180922600233] 1014 | - [-0.8687840322986449, 6.103960492323569] 1015 | - [-0.7902476441731274, 6.142801938291369] 1016 | - [-0.7083858664050344, 5.946180922600233] 1017 | - [-0.8687840322986449, 5.946180922600233] 1018 | shape: polygon 1019 | obstacle0127: 1020 | corners: 1021 | - [0.06952701356919633, 2.172145836297366] 1022 | - [0.06952701356919633, 2.365137552259673] 1023 | - [0.24862500302258134, 2.6150321978368023] 1024 | - [0.3616455575079615, 2.172145836297366] 1025 | - [0.06952701356919633, 2.172145836297366] 1026 | shape: polygon 1027 | obstacle0128: 1028 | corners: 1029 | - [8.40303702786344, -0.5875914559797373] 1030 | - [8.40303702786344, -0.3645693128165089] 1031 | - [8.621602923234567, -0.11635468764448165] 1032 | - [8.759395157510136, -0.5875914559797373] 1033 | - [8.40303702786344, -0.5875914559797373] 1034 | shape: polygon 1035 | obstacle0129: 1036 | corners: 1037 | - [11.047116016717816, -1.8681585708748245] 1038 | - [11.047116016717816, -1.7458611813900444] 1039 | - [11.269602412877834, -1.4274709212278704] 1040 | - [11.216702642860353, -1.8681585708748245] 1041 | - [11.047116016717816, -1.8681585708748245] 1042 | shape: polygon 1043 | obstacle0130: 1044 | corners: 1045 | - [-1.3620644205553643, 1.6354618184534644] 1046 | - [-1.3620644205553643, 2.1277164749465967] 1047 | - [-1.3095112012122723, 2.106814011464805] 1048 | - [-1.0455276442649553, 1.6354618184534644] 1049 | - [-1.3620644205553643, 1.6354618184534644] 1050 | shape: polygon 1051 | obstacle0131: 1052 | corners: 1053 | - [0.525543315695721, 2.0402939485230016] 1054 | - [0.525543315695721, 2.5084460136891655] 1055 | - [0.5611243537158964, 2.3081086329416465] 1056 | - [0.546934215257314, 2.0402939485230016] 1057 | - [0.525543315695721, 2.0402939485230016] 1058 | shape: polygon 1059 | obstacle0132: 1060 | corners: 1061 | - [2.5934447050095075, 4.80013352205342] 1062 | - [2.5934447050095075, 5.05764359346268] 1063 | - [2.6153931878968093, 4.987582656728011] 1064 | - [2.7985112231136986, 4.80013352205342] 1065 | - [2.5934447050095075, 4.80013352205342] 1066 | shape: polygon 1067 | obstacle0133: 1068 | corners: 1069 | - [8.674968288958512, 0.2230474958822133] 1070 | - [8.674968288958512, 0.5398824823822178] 1071 | - [8.887163520950313, 0.6608435325550259] 1072 | - [8.701216051850249, 0.2230474958822133] 1073 | - [8.674968288958512, 0.2230474958822133] 1074 | shape: polygon 1075 | obstacle0134: 1076 | corners: 1077 | - [5.752011386034137, 0.7099251673029583] 1078 | - [5.752011386034137, 1.0585328797599445] 1079 | - [6.120524758377667, 0.7775166447632116] 1080 | - [6.091314508074307, 0.7099251673029583] 1081 | - [5.752011386034137, 0.7099251673029583] 1082 | shape: polygon 1083 | obstacle0135: 1084 | corners: 1085 | - [2.1746643337685283, 3.9041812901776316] 1086 | - [2.1746643337685283, 4.1097843408463826] 1087 | - [2.4973806138294643, 4.258699934734111] 1088 | - [2.6419334760684237, 3.9041812901776316] 1089 | - [2.1746643337685283, 3.9041812901776316] 1090 | shape: polygon 1091 | obstacle0136: 1092 | corners: 1093 | - [3.281991590288367, 5.395614989502926] 1094 | - [3.281991590288367, 5.608922733591411] 1095 | - [3.3996275478237377, 5.761667594586075] 1096 | - [3.606466650835401, 5.395614989502926] 1097 | - [3.281991590288367, 5.395614989502926] 1098 | shape: polygon 1099 | obstacle0137: 1100 | corners: 1101 | - [-0.07255251941699337, 2.367921712700359] 1102 | - [-0.07255251941699337, 2.4401178469829468] 1103 | - [0.33549915069539615, 2.420434717802279] 1104 | - [0.28071755091800804, 2.367921712700359] 1105 | - [-0.07255251941699337, 2.367921712700359] 1106 | shape: polygon 1107 | obstacle0138: 1108 | corners: 1109 | - [5.403868094677703, -1.4561530423434021] 1110 | - [5.403868094677703, -1.0717045596858628] 1111 | - [5.602306816704407, -1.2867075445625569] 1112 | - [5.545663351029302, -1.4561530423434021] 1113 | - [5.403868094677703, -1.4561530423434021] 1114 | shape: polygon 1115 | obstacle0139: 1116 | corners: 1117 | - [1.053238853465312, 6.348299561211926] 1118 | - [1.053238853465312, 6.492120544360621] 1119 | - [1.476319624296936, 6.561771597886145] 1120 | - [1.1162552861006585, 6.348299561211926] 1121 | - [1.053238853465312, 6.348299561211926] 1122 | shape: polygon 1123 | obstacle0140: 1124 | corners: 1125 | - [9.110735793169114, 0.5458062487549333] 1126 | - [9.110735793169114, 0.8758141538844048] 1127 | - [9.3135966901242, 0.6065159945570302] 1128 | - [9.167412669350647, 0.5458062487549333] 1129 | - [9.110735793169114, 0.5458062487549333] 1130 | shape: polygon 1131 | obstacle0141: 1132 | corners: 1133 | - [2.562505193107051, -2.3087181601974827] 1134 | - [2.562505193107051, -2.0476601941764714] 1135 | - [3.021939588895501, -1.903191545598106] 1136 | - [2.852499399728076, -2.3087181601974827] 1137 | - [2.562505193107051, -2.3087181601974827] 1138 | shape: polygon 1139 | obstacle0142: 1140 | corners: 1141 | - [-1.0483785859988521, 5.737447926189224] 1142 | - [-1.0483785859988521, 5.974462963964644] 1143 | - [-1.019488034505298, 6.099108541972842] 1144 | - [-0.8092218063974729, 5.737447926189224] 1145 | - [-1.0483785859988521, 5.737447926189224] 1146 | shape: polygon 1147 | obstacle0143: 1148 | corners: 1149 | - [-1.0351321608096864, 7.695660086620762] 1150 | - [-1.0351321608096864, 7.7939738956433535] 1151 | - [-0.545904553396184, 8.057300000206457] 1152 | - [-0.7097775818562777, 7.695660086620762] 1153 | - [-1.0351321608096864, 7.695660086620762] 1154 | shape: polygon 1155 | obstacle0144: 1156 | corners: 1157 | - [3.030982568135731, 5.983329027173484] 1158 | - [3.030982568135731, 6.433846518701007] 1159 | - [3.1886729324905843, 6.034705187346794] 1160 | - [3.2296914482479604, 5.983329027173484] 1161 | - [3.030982568135731, 5.983329027173484] 1162 | shape: polygon 1163 | obstacle0145: 1164 | corners: 1165 | - [7.632313668213804, 1.209183604979919] 1166 | - [7.632313668213804, 1.5898669205322247] 1167 | - [7.810633799741259, 1.2193379515250489] 1168 | - [7.849583937193531, 1.209183604979919] 1169 | - [7.632313668213804, 1.209183604979919] 1170 | shape: polygon 1171 | obstacle0146: 1172 | corners: 1173 | - [6.28534642260578, 6.71814581792386] 1174 | - [6.28534642260578, 7.040426922439252] 1175 | - [6.688621646879843, 7.005817396283726] 1176 | - [6.578313314588069, 6.71814581792386] 1177 | - [6.28534642260578, 6.71814581792386] 1178 | shape: polygon 1179 | obstacle0147: 1180 | corners: 1181 | - [3.2503504827389103, -0.9686260707498677] 1182 | - [3.2503504827389103, -0.7342133395713436] 1183 | - [3.3086871827304565, -0.5437113100040023] 1184 | - [3.284516013689215, -0.9686260707498677] 1185 | - [3.2503504827389103, -0.9686260707498677] 1186 | shape: polygon 1187 | obstacle0148: 1188 | corners: 1189 | - [11.647991273105646, -0.8967016292929086] 1190 | - [11.647991273105646, -0.7508456438953175] 1191 | - [12.116085029726717, -0.7889737812219628] 1192 | - [11.785512722488773, -0.8967016292929086] 1193 | - [11.647991273105646, -0.8967016292929086] 1194 | shape: polygon 1195 | obstacle0149: 1196 | corners: 1197 | - [-0.6559350145112164, 7.555271628008837] 1198 | - [-0.6559350145112164, 7.896342298013392] 1199 | - [-0.6371602666603674, 7.826387807419523] 1200 | - [-0.6057401207345416, 7.555271628008837] 1201 | - [-0.6559350145112164, 7.555271628008837] 1202 | shape: polygon 1203 | obstacle0150: 1204 | corners: 1205 | - [2.9175882955239416, 7.757551884246533] 1206 | - [2.9175882955239416, 8.251743210141523] 1207 | - [3.3474906416744408, 7.952220840489373] 1208 | - [3.204440020885744, 7.757551884246533] 1209 | - [2.9175882955239416, 7.757551884246533] 1210 | shape: polygon 1211 | obstacle0151: 1212 | corners: 1213 | - [-1.2210898415240963, 5.664633286701699] 1214 | - [-1.2210898415240963, 5.809083131159776] 1215 | - [-0.969106105555986, 6.05777914077688] 1216 | - [-0.9832035868841362, 5.664633286701699] 1217 | - [-1.2210898415240963, 5.664633286701699] 1218 | shape: polygon 1219 | obstacle0152: 1220 | corners: 1221 | - [2.91468135851661, 4.648465862981446] 1222 | - [2.91468135851661, 4.804150444246866] 1223 | - [3.040019178733702, 5.027225903151377] 1224 | - [3.039167910515475, 4.648465862981446] 1225 | - [2.91468135851661, 4.648465862981446] 1226 | shape: polygon 1227 | obstacle0153: 1228 | corners: 1229 | - [6.7437345569762, 7.321239536689767] 1230 | - [6.7437345569762, 7.5644194794303905] 1231 | - [7.048798962439406, 7.390123481616965] 1232 | - [7.200009020707861, 7.321239536689767] 1233 | - [6.7437345569762, 7.321239536689767] 1234 | shape: polygon 1235 | obstacle0154: 1236 | corners: 1237 | - [9.746037448234947, 1.2965469083929406] 1238 | - [9.746037448234947, 1.67603526266391] 1239 | - [10.241247370815044, 1.6484652027987214] 1240 | - [9.985780239816568, 1.2965469083929406] 1241 | - [9.746037448234947, 1.2965469083929406] 1242 | shape: polygon 1243 | obstacle0155: 1244 | corners: 1245 | - [4.758978121854377, 5.728482287297092] 1246 | - [4.758978121854377, 5.755260821691009] 1247 | - [5.119309829655743, 6.078843616745766] 1248 | - [5.032842984025072, 5.728482287297092] 1249 | - [4.758978121854377, 5.728482287297092] 1250 | shape: polygon 1251 | obstacle0156: 1252 | corners: 1253 | - [3.0306693284259385, 2.9945144953284606] 1254 | - [3.0306693284259385, 3.186982418859557] 1255 | - [3.2623494727502615, 3.112778979300257] 1256 | - [3.386645915664221, 2.9945144953284606] 1257 | - [3.0306693284259385, 2.9945144953284606] 1258 | shape: polygon 1259 | obstacle0157: 1260 | corners: 1261 | - [1.30819096083227, -0.8902132524201538] 1262 | - [1.30819096083227, -0.6555897668091827] 1263 | - [1.620806187872, -0.42675904048452845] 1264 | - [1.5780509277468129, -0.8902132524201538] 1265 | - [1.30819096083227, -0.8902132524201538] 1266 | shape: polygon 1267 | obstacle0158: 1268 | corners: 1269 | - [1.6173684737981575, -2.6316130954429373] 1270 | - [1.6173684737981575, -2.6239202010639975] 1271 | - [1.9039588363032796, -2.2034667619338504] 1272 | - [1.6404445143315927, -2.6316130954429373] 1273 | - [1.6173684737981575, -2.6316130954429373] 1274 | shape: polygon 1275 | obstacle0159: 1276 | corners: 1277 | - [6.743853679949101, 7.419604572936366] 1278 | - [6.743853679949101, 7.759300058263593] 1279 | - [7.240878114149494, 7.458239104277435] 1280 | - [6.962489608815796, 7.419604572936366] 1281 | - [6.743853679949101, 7.419604572936366] 1282 | shape: polygon 1283 | obstacle0160: 1284 | corners: 1285 | - [4.950395459842605, -1.1230027780201646] 1286 | - [4.950395459842605, -1.0017887654288276] 1287 | - [5.358585789354743, -0.7292958353407393] 1288 | - [5.263057108215965, -1.1230027780201646] 1289 | - [4.950395459842605, -1.1230027780201646] 1290 | shape: polygon 1291 | obstacle0161: 1292 | corners: 1293 | - [4.899176819299305, 1.168971580828324] 1294 | - [4.899176819299305, 1.5210390756873415] 1295 | - [5.176512121051247, 1.5130759189346874] 1296 | - [5.31988965239358, 1.168971580828324] 1297 | - [4.899176819299305, 1.168971580828324] 1298 | shape: polygon 1299 | obstacle0162: 1300 | corners: 1301 | - [2.8192863828877446, 6.006295516377577] 1302 | - [2.8192863828877446, 6.250629951750305] 1303 | - [3.0726423591960184, 6.392369958704168] 1304 | - [3.1389675835248236, 6.006295516377577] 1305 | - [2.8192863828877446, 6.006295516377577] 1306 | shape: polygon 1307 | obstacle0163: 1308 | corners: 1309 | - [11.614734445823848, 1.2141496910883092] 1310 | - [11.614734445823848, 1.4623165624246195] 1311 | - [11.767161827433142, 1.4553540093943378] 1312 | - [11.738557972410888, 1.2141496910883092] 1313 | - [11.614734445823848, 1.2141496910883092] 1314 | shape: polygon 1315 | obstacle0164: 1316 | corners: 1317 | - [1.0968483446606667, 4.06070417956758] 1318 | - [1.0968483446606667, 4.3936988303678595] 1319 | - [1.3087932802274085, 4.092594490409715] 1320 | - [1.4764483320723418, 4.06070417956758] 1321 | - [1.0968483446606667, 4.06070417956758] 1322 | shape: polygon 1323 | obstacle0165: 1324 | corners: 1325 | - [5.274445362195401, 3.336229729047247] 1326 | - [5.274445362195401, 3.716833561447135] 1327 | - [5.298394821968279, 3.6550656311204217] 1328 | - [5.613649540897244, 3.336229729047247] 1329 | - [5.274445362195401, 3.336229729047247] 1330 | shape: polygon 1331 | obstacle0166: 1332 | corners: 1333 | - [11.428914356621279, -2.9170443932158516] 1334 | - [11.428914356621279, -2.521917035806707] 1335 | - [11.788378593387456, -2.5245337873718414] 1336 | - [11.864562839184039, -2.9170443932158516] 1337 | - [11.428914356621279, -2.9170443932158516] 1338 | shape: polygon 1339 | obstacle0167: 1340 | corners: 1341 | - [6.33493008192853, -1.197991381022456] 1342 | - [6.33493008192853, -0.8478710479316474] 1343 | - [6.471398544964578, -0.7842399319920916] 1344 | - [6.6348544739870166, -1.197991381022456] 1345 | - [6.33493008192853, -1.197991381022456] 1346 | shape: polygon 1347 | obstacle0168: 1348 | corners: 1349 | - [0.4326303679569632, 5.175642434116746] 1350 | - [0.4326303679569632, 5.526274936623666] 1351 | - [0.7085359505925309, 5.443489503299566] 1352 | - [0.7206455165870349, 5.175642434116746] 1353 | - [0.4326303679569632, 5.175642434116746] 1354 | shape: polygon 1355 | obstacle0169: 1356 | corners: 1357 | - [10.798400593716698, -0.05207758858998579] 1358 | - [10.798400593716698, -0.051279198909210566] 1359 | - [10.972355532557058, 0.16831817885738454] 1360 | - [10.903436775082572, -0.05207758858998579] 1361 | - [10.798400593716698, -0.05207758858998579] 1362 | shape: polygon 1363 | obstacle0170: 1364 | corners: 1365 | - [4.025474163363873, 4.553472186662641] 1366 | - [4.025474163363873, 4.860553358452599] 1367 | - [4.080835120564518, 4.696060089180795] 1368 | - [4.099871477700698, 4.553472186662641] 1369 | - [4.025474163363873, 4.553472186662641] 1370 | shape: polygon 1371 | obstacle0171: 1372 | corners: 1373 | - [0.07794487500758507, 5.476045020570417] 1374 | - [0.07794487500758507, 5.929984633062892] 1375 | - [0.45287961849581077, 5.727722077327881] 1376 | - [0.09197161080247468, 5.476045020570417] 1377 | - [0.07794487500758507, 5.476045020570417] 1378 | shape: polygon 1379 | obstacle0172: 1380 | corners: 1381 | - [5.921626270181106, 5.891055117389254] 1382 | - [5.921626270181106, 6.12086102580874] 1383 | - [6.111917495684385, 6.267478040407602] 1384 | - [6.08938247638823, 5.891055117389254] 1385 | - [5.921626270181106, 5.891055117389254] 1386 | shape: polygon 1387 | obstacle0173: 1388 | corners: 1389 | - [-1.0338695338734594, 0.041945631252841675] 1390 | - [-1.0338695338734594, 0.20590756380444186] 1391 | - [-0.9375394553433302, 0.3638669839295482] 1392 | - [-0.6993381101437255, 0.041945631252841675] 1393 | - [-1.0338695338734594, 0.041945631252841675] 1394 | shape: polygon 1395 | obstacle0174: 1396 | corners: 1397 | - [4.68588320726191, -1.0125264475386215] 1398 | - [4.68588320726191, -0.8483526092091007] 1399 | - [4.789913856750283, -0.9046506200180319] 1400 | - [4.8694306505057785, -1.0125264475386215] 1401 | - [4.68588320726191, -1.0125264475386215] 1402 | shape: polygon 1403 | obstacle0175: 1404 | corners: 1405 | - [0.2717332313121874, 6.305681066731985] 1406 | - [0.2717332313121874, 6.529194471377043] 1407 | - [0.6813795418270016, 6.4340950260027245] 1408 | - [0.47646123152746583, 6.305681066731985] 1409 | - [0.2717332313121874, 6.305681066731985] 1410 | shape: polygon 1411 | obstacle0176: 1412 | corners: 1413 | - [4.652677895712486, 5.032658965768002] 1414 | - [4.652677895712486, 5.415647800676527] 1415 | - [4.813490444676051, 5.464051284602282] 1416 | - [5.040662262113612, 5.032658965768002] 1417 | - [4.652677895712486, 5.032658965768002] 1418 | shape: polygon 1419 | obstacle0177: 1420 | corners: 1421 | - [10.246310193581031, -2.715267915814572] 1422 | - [10.246310193581031, -2.6905445720766537] 1423 | - [10.499143800827316, -2.62063139475731] 1424 | - [10.264846392161784, -2.715267915814572] 1425 | - [10.246310193581031, -2.715267915814572] 1426 | shape: polygon 1427 | obstacle0178: 1428 | corners: 1429 | - [6.582443141479764, 2.1419992768723297] 1430 | - [6.582443141479764, 2.518913070984533] 1431 | - [6.61473304111136, 2.631420968493458] 1432 | - [6.843511794573877, 2.1419992768723297] 1433 | - [6.582443141479764, 2.1419992768723297] 1434 | shape: polygon 1435 | obstacle0179: 1436 | corners: 1437 | - [6.7900812866394915, 1.0829761833421623] 1438 | - [6.7900812866394915, 1.421113720651434] 1439 | - [7.0066380974005416, 1.5739956732960636] 1440 | - [7.136853631045379, 1.0829761833421623] 1441 | - [6.7900812866394915, 1.0829761833421623] 1442 | shape: polygon 1443 | obstacle0180: 1444 | corners: 1445 | - [11.125952560116023, -1.0654702807526961] 1446 | - [11.125952560116023, -0.6957465725396241] 1447 | - [11.41584998983632, -0.7983548499739181] 1448 | - [11.133776418689887, -1.0654702807526961] 1449 | - [11.125952560116023, -1.0654702807526961] 1450 | shape: polygon 1451 | obstacle0181: 1452 | corners: 1453 | - [3.5083149551332564, 5.242482004864135] 1454 | - [3.5083149551332564, 5.691425981076917] 1455 | - [3.6566541964925814, 5.48411841685686] 1456 | - [3.8554137857141484, 5.242482004864135] 1457 | - [3.5083149551332564, 5.242482004864135] 1458 | shape: polygon 1459 | obstacle0182: 1460 | corners: 1461 | - [11.73671811646473, 1.8870738027139646] 1462 | - [11.73671811646473, 2.2678621104614596] 1463 | - [11.923242741224561, 2.2797815659084] 1464 | - [11.920544167271624, 1.8870738027139646] 1465 | - [11.73671811646473, 1.8870738027139646] 1466 | shape: polygon 1467 | obstacle0183: 1468 | corners: 1469 | - [3.072177526639276, -0.8352932602759893] 1470 | - [3.072177526639276, -0.6815732606536729] 1471 | - [3.273435820479336, -0.6525446542710078] 1472 | - [3.1826682533235355, -0.8352932602759893] 1473 | - [3.072177526639276, -0.8352932602759893] 1474 | shape: polygon 1475 | obstacle0184: 1476 | corners: 1477 | - [-0.13671797130808017, 1.5242633988002172] 1478 | - [-0.13671797130808017, 1.7270546281917398] 1479 | - [-0.009918040804374306, 1.6840383569920878] 1480 | - [0.23493794032382592, 1.5242633988002172] 1481 | - [-0.13671797130808017, 1.5242633988002172] 1482 | shape: polygon 1483 | obstacle0185: 1484 | corners: 1485 | - [1.494797420520328, -0.6219391687492521] 1486 | - [1.494797420520328, -0.4725003710652152] 1487 | - [1.7036858023577663, -0.6138212561347931] 1488 | - [1.8510771010494773, -0.6219391687492521] 1489 | - [1.494797420520328, -0.6219391687492521] 1490 | shape: polygon 1491 | obstacle0186: 1492 | corners: 1493 | - [1.0800075663127853, 4.333080217590957] 1494 | - [1.0800075663127853, 4.559037482716235] 1495 | - [1.4550479073360705, 4.532220646748474] 1496 | - [1.1412223390099776, 4.333080217590957] 1497 | - [1.0800075663127853, 4.333080217590957] 1498 | shape: polygon 1499 | obstacle0187: 1500 | corners: 1501 | - [-0.49986257017661995, -1.9348834911188741] 1502 | - [-0.49986257017661995, -1.7171869690540849] 1503 | - [-0.28971604307555515, -1.5806901692311683] 1504 | - [-0.3255105872362136, -1.9348834911188741] 1505 | - [-0.49986257017661995, -1.9348834911188741] 1506 | shape: polygon 1507 | obstacle0188: 1508 | corners: 1509 | - [3.851227111017055, 0.2638789546715916] 1510 | - [3.851227111017055, 0.5425131385989573] 1511 | - [4.1663570403498555, 0.5513647463015208] 1512 | - [3.880142753450126, 0.2638789546715916] 1513 | - [3.851227111017055, 0.2638789546715916] 1514 | shape: polygon 1515 | obstacle0189: 1516 | corners: 1517 | - [7.460036421457188, 3.5897632805682322] 1518 | - [7.460036421457188, 3.959226319773275] 1519 | - [7.471957270706825, 4.001104986014084] 1520 | - [7.463572482609557, 3.5897632805682322] 1521 | - [7.460036421457188, 3.5897632805682322] 1522 | shape: polygon 1523 | obstacle0190: 1524 | corners: 1525 | - [-0.3074909693854644, 4.613183520067679] 1526 | - [-0.3074909693854644, 4.743581167158684] 1527 | - [-0.2655767275423339, 5.07625727874482] 1528 | - [-0.22133535887541667, 4.613183520067679] 1529 | - [-0.3074909693854644, 4.613183520067679] 1530 | shape: polygon 1531 | obstacle0191: 1532 | corners: 1533 | - [7.064835126188759, 0.09334303907980068] 1534 | - [7.064835126188759, 0.5239832089992582] 1535 | - [7.1685549805186985, 0.310377679594936] 1536 | - [7.164519740392078, 0.09334303907980068] 1537 | - [7.064835126188759, 0.09334303907980068] 1538 | shape: polygon 1539 | obstacle0192: 1540 | corners: 1541 | - [1.5469527098459577, -2.7497090231086263] 1542 | - [1.5469527098459577, -2.427507964679255] 1543 | - [1.6125112080615769, -2.430254868476022] 1544 | - [1.6412090399920065, -2.7497090231086263] 1545 | - [1.5469527098459577, -2.7497090231086263] 1546 | shape: polygon 1547 | obstacle0193: 1548 | corners: 1549 | - [0.7686683695887169, 2.1237641215476355] 1550 | - [0.7686683695887169, 2.405118096084768] 1551 | - [0.9698750836760051, 2.430197551717023] 1552 | - [1.0971534369240354, 2.1237641215476355] 1553 | - [0.7686683695887169, 2.1237641215476355] 1554 | shape: polygon 1555 | obstacle0194: 1556 | corners: 1557 | - [2.42006812923323, 4.29974766432942] 1558 | - [2.42006812923323, 4.458057145520804] 1559 | - [2.6557284604296107, 4.496567428947238] 1560 | - [2.6062838891835853, 4.29974766432942] 1561 | - [2.42006812923323, 4.29974766432942] 1562 | shape: polygon 1563 | obstacle0195: 1564 | corners: 1565 | - [5.4827856341531165, 0.21805164794901444] 1566 | - [5.4827856341531165, 0.6534554441072067] 1567 | - [5.776963643489893, 0.28160277191474653] 1568 | - [5.843747177606736, 0.21805164794901444] 1569 | - [5.4827856341531165, 0.21805164794901444] 1570 | shape: polygon 1571 | obstacle0196: 1572 | corners: 1573 | - [3.661396173951056, 0.15098878337344157] 1574 | - [3.661396173951056, 0.6497791157796386] 1575 | - [3.731457252699875, 0.29097931360174933] 1576 | - [4.061757280863413, 0.15098878337344157] 1577 | - [3.661396173951056, 0.15098878337344157] 1578 | shape: polygon 1579 | obstacle0197: 1580 | corners: 1581 | - [7.287767986695707, -2.4228561990879505] 1582 | - [7.287767986695707, -2.129287966834446] 1583 | - [7.721402515218304, -2.2648112089923096] 1584 | - [7.479489417741609, -2.4228561990879505] 1585 | - [7.287767986695707, -2.4228561990879505] 1586 | shape: polygon 1587 | obstacle0198: 1588 | corners: 1589 | - [0.47222712601620387, 7.015459318387208] 1590 | - [0.47222712601620387, 7.395715037057356] 1591 | - [0.6713245444515654, 7.356468738140385] 1592 | - [0.477980809080492, 7.015459318387208] 1593 | - [0.47222712601620387, 7.015459318387208] 1594 | shape: polygon 1595 | obstacle0199: 1596 | corners: 1597 | - [6.11621746478637, 3.4221158406790337] 1598 | - [6.11621746478637, 3.6017971812605407] 1599 | - [6.568249123490946, 3.726021922804036] 1600 | - [6.411904196311261, 3.4221158406790337] 1601 | - [6.11621746478637, 3.4221158406790337] 1602 | shape: polygon 1603 | obstacle0200: 1604 | corners: 1605 | - [-0.9629786542094048, 1.003524177301947] 1606 | - [-0.9629786542094048, 1.1564266969014603] 1607 | - [-0.8163629134437413, 1.0797375734263914] 1608 | - [-0.49584525591811707, 1.003524177301947] 1609 | - [-0.9629786542094048, 1.003524177301947] 1610 | shape: polygon 1611 | obstacle0201: 1612 | corners: 1613 | - [4.010885419987513, 6.540271259707925] 1614 | - [4.010885419987513, 6.735070081366648] 1615 | - [4.4256931135654, 7.004023228821613] 1616 | - [4.422907010149477, 6.540271259707925] 1617 | - [4.010885419987513, 6.540271259707925] 1618 | shape: polygon 1619 | obstacle0202: 1620 | corners: 1621 | - [11.331385184670786, 3.696007497450145] 1622 | - [11.331385184670786, 3.9663202004733806] 1623 | - [11.695094081012904, 3.768993670521832] 1624 | - [11.67920598749173, 3.696007497450145] 1625 | - [11.331385184670786, 3.696007497450145] 1626 | shape: polygon 1627 | obstacle0203: 1628 | corners: 1629 | - [-1.6179230870030554, 2.1072913261147823] 1630 | - [-1.6179230870030554, 2.192250742265613] 1631 | - [-1.1719742049987767, 2.1698504639077694] 1632 | - [-1.5259167624053478, 2.1072913261147823] 1633 | - [-1.6179230870030554, 2.1072913261147823] 1634 | shape: polygon 1635 | obstacle0204: 1636 | corners: 1637 | - [1.7865972311434155, 4.376508095308532] 1638 | - [1.7865972311434155, 4.687785596760897] 1639 | - [1.9489817648231522, 4.765849422947507] 1640 | - [2.1295894668063005, 4.376508095308532] 1641 | - [1.7865972311434155, 4.376508095308532] 1642 | shape: polygon 1643 | obstacle0205: 1644 | corners: 1645 | - [5.386516308590439, 6.136143734902221] 1646 | - [5.386516308590439, 6.157248115836639] 1647 | - [5.885812774844774, 6.201943709602717] 1648 | - [5.6727298626141796, 6.136143734902221] 1649 | - [5.386516308590439, 6.136143734902221] 1650 | shape: polygon 1651 | obstacle0206: 1652 | corners: 1653 | - [-1.1715486577517342, 1.8295383900261255] 1654 | - [-1.1715486577517342, 2.265989729398672] 1655 | - [-1.021875887712755, 2.1554395031608204] 1656 | - [-0.6884877426795852, 1.8295383900261255] 1657 | - [-1.1715486577517342, 1.8295383900261255] 1658 | shape: polygon 1659 | obstacle0207: 1660 | corners: 1661 | - [0.5906944952014337, 1.7517638773959447] 1662 | - [0.5906944952014337, 2.1706110471244773] 1663 | - [0.9880205357497098, 1.923617280512578] 1664 | - [0.8418970528629397, 1.7517638773959447] 1665 | - [0.5906944952014337, 1.7517638773959447] 1666 | shape: polygon 1667 | obstacle0208: 1668 | corners: 1669 | - [-0.2880099596400154, 4.8998235180772465] 1670 | - [-0.2880099596400154, 5.030346543814966] 1671 | - [-0.16232062753992338, 4.948831051350373] 1672 | - [-0.28551399500234415, 4.8998235180772465] 1673 | - [-0.2880099596400154, 4.8998235180772465] 1674 | shape: polygon 1675 | obstacle0209: 1676 | corners: 1677 | - [-1.6538341080726375, -1.4124201306904283] 1678 | - [-1.6538341080726375, -1.0820765200290845] 1679 | - [-1.3710587391638152, -1.0593674803409225] 1680 | - [-1.4926809892669208, -1.4124201306904283] 1681 | - [-1.6538341080726375, -1.4124201306904283] 1682 | shape: polygon 1683 | obstacle0210: 1684 | corners: 1685 | - [4.042451233820499, -1.8014261352169212] 1686 | - [4.042451233820499, -1.3030383361689508] 1687 | - [4.219610203908408, -1.5789345875089975] 1688 | - [4.536602676452733, -1.8014261352169212] 1689 | - [4.042451233820499, -1.8014261352169212] 1690 | shape: polygon 1691 | obstacle0211: 1692 | corners: 1693 | - [8.658417089383597, -1.2819814690139673] 1694 | - [8.658417089383597, -1.2638321647371396] 1695 | - [8.838004226446907, -1.1746464607326703] 1696 | - [9.057147713046247, -1.2819814690139673] 1697 | - [8.658417089383597, -1.2819814690139673] 1698 | shape: polygon 1699 | obstacle0212: 1700 | corners: 1701 | - [7.639460492249929, -0.9858485313575827] 1702 | - [7.639460492249929, -0.783598730315028] 1703 | - [7.711957407438296, -0.5432601535015122] 1704 | - [7.7046379325934184, -0.9858485313575827] 1705 | - [7.639460492249929, -0.9858485313575827] 1706 | shape: polygon 1707 | obstacle0213: 1708 | corners: 1709 | - [5.19080402546914, 0.5973395524935907] 1710 | - [5.19080402546914, 0.7879727694222428] 1711 | - [5.67680332969522, 0.7737112243151083] 1712 | - [5.516713021241779, 0.5973395524935907] 1713 | - [5.19080402546914, 0.5973395524935907] 1714 | shape: polygon 1715 | obstacle0214: 1716 | corners: 1717 | - [7.94612827816543, 4.711372748914552] 1718 | - [7.94612827816543, 5.173788285762529] 1719 | - [8.022411059845453, 5.069518531492713] 1720 | - [8.234082284350936, 4.711372748914552] 1721 | - [7.94612827816543, 4.711372748914552] 1722 | shape: polygon 1723 | obstacle0215: 1724 | corners: 1725 | - [11.846226742226207, -0.11820905090153655] 1726 | - [11.846226742226207, -0.07884072352906402] 1727 | - [12.019405121731523, -0.04773131688637111] 1728 | - [12.00547154366357, -0.11820905090153655] 1729 | - [11.846226742226207, -0.11820905090153655] 1730 | shape: polygon 1731 | obstacle0216: 1732 | corners: 1733 | - [4.65271241830715, 2.058771334430708] 1734 | - [4.65271241830715, 2.3890831234688767] 1735 | - [4.782064331239152, 2.468615621835797] 1736 | - [5.066147320603725, 2.058771334430708] 1737 | - [4.65271241830715, 2.058771334430708] 1738 | shape: polygon 1739 | obstacle0217: 1740 | corners: 1741 | - [5.079973459513704, 1.1326518187491406] 1742 | - [5.079973459513704, 1.2430830241684168] 1743 | - [5.273861240811534, 1.149976459972606] 1744 | - [5.243725765484528, 1.1326518187491406] 1745 | - [5.079973459513704, 1.1326518187491406] 1746 | shape: polygon 1747 | obstacle0218: 1748 | corners: 1749 | - [4.643547830830108, 2.8892809500786445] 1750 | - [4.643547830830108, 3.067301501166653] 1751 | - [4.997951005763321, 3.081781525689543] 1752 | - [5.069636193465442, 2.8892809500786445] 1753 | - [4.643547830830108, 2.8892809500786445] 1754 | shape: polygon 1755 | obstacle0219: 1756 | corners: 1757 | - [2.3919160736474687, 4.168407106957048] 1758 | - [2.3919160736474687, 4.29708370629297] 1759 | - [2.518693287152773, 4.278797778430159] 1760 | - [2.7040828617053845, 4.168407106957048] 1761 | - [2.3919160736474687, 4.168407106957048] 1762 | shape: polygon 1763 | obstacle0220: 1764 | corners: 1765 | - [6.580499475195001, 0.6001178513615284] 1766 | - [6.580499475195001, 0.9966190221867465] 1767 | - [6.779514565666149, 0.6943744350744686] 1768 | - [6.658083032487807, 0.6001178513615284] 1769 | - [6.580499475195001, 0.6001178513615284] 1770 | shape: polygon 1771 | obstacle0221: 1772 | corners: 1773 | - [5.592499050364349, 5.440822404426312] 1774 | - [5.592499050364349, 5.68886274070174] 1775 | - [5.637939909125114, 5.764064680114505] 1776 | - [5.8117709882577335, 5.440822404426312] 1777 | - [5.592499050364349, 5.440822404426312] 1778 | shape: polygon 1779 | obstacle0222: 1780 | corners: 1781 | - [-1.8590849768883637, -0.621657910852329] 1782 | - [-1.8590849768883637, -0.15101733494995684] 1783 | - [-1.7372965072169788, -0.5644750795466498] 1784 | - [-1.659682100161008, -0.621657910852329] 1785 | - [-1.8590849768883637, -0.621657910852329] 1786 | shape: polygon 1787 | obstacle0223: 1788 | corners: 1789 | - [10.858157481211123, 6.762032010504814] 1790 | - [10.858157481211123, 6.936366947227611] 1791 | - [11.03976356867333, 7.159585712553808] 1792 | - [10.916050079274873, 6.762032010504814] 1793 | - [10.858157481211123, 6.762032010504814] 1794 | shape: polygon 1795 | obstacle0224: 1796 | corners: 1797 | - [-0.028970715880946463, 7.146659944076378] 1798 | - [-0.028970715880946463, 7.248884192422373] 1799 | - [0.08971201360360576, 7.353660191059889] 1800 | - [0.2611517065526391, 7.146659944076378] 1801 | - [-0.028970715880946463, 7.146659944076378] 1802 | shape: polygon 1803 | obstacle0225: 1804 | corners: 1805 | - [6.1455164672350335, 1.120842077482986] 1806 | - [6.1455164672350335, 1.250351956335177] 1807 | - [6.444161052086911, 1.2194224549516988] 1808 | - [6.421396567611003, 1.120842077482986] 1809 | - [6.1455164672350335, 1.120842077482986] 1810 | shape: polygon 1811 | obstacle0226: 1812 | corners: 1813 | - [6.614513406453618, 4.76831394691486] 1814 | - [6.614513406453618, 4.920364929041015] 1815 | - [7.041622674135702, 5.155045670359697] 1816 | - [7.01514687958474, 4.76831394691486] 1817 | - [6.614513406453618, 4.76831394691486] 1818 | shape: polygon 1819 | obstacle0227: 1820 | corners: 1821 | - [-0.10035910021448768, -1.7164507242090745] 1822 | - [-0.10035910021448768, -1.2804569306019864] 1823 | - [0.10156095004088778, -1.3139241486810653] 1824 | - [0.06074153383166736, -1.7164507242090745] 1825 | - [-0.10035910021448768, -1.7164507242090745] 1826 | shape: polygon 1827 | obstacle0228: 1828 | corners: 1829 | - [3.8413245722246607, 4.436484411249559] 1830 | - [3.8413245722246607, 4.49868101352964] 1831 | - [4.232925629852137, 4.905097804043194] 1832 | - [4.295298991236252, 4.436484411249559] 1833 | - [3.8413245722246607, 4.436484411249559] 1834 | shape: polygon 1835 | obstacle0229: 1836 | corners: 1837 | - [5.337933230083101, 1.9202853016872208] 1838 | - [5.337933230083101, 2.397499236094364] 1839 | - [5.603535254922614, 1.9723876153394757] 1840 | - [5.473136217434222, 1.9202853016872208] 1841 | - [5.337933230083101, 1.9202853016872208] 1842 | shape: polygon 1843 | obstacle0230: 1844 | corners: 1845 | - [9.839933741997942, 0.45830147895065076] 1846 | - [9.839933741997942, 0.512942052046775] 1847 | - [10.263352117950797, 0.8045883381730237] 1848 | - [10.268002277708208, 0.45830147895065076] 1849 | - [9.839933741997942, 0.45830147895065076] 1850 | shape: polygon 1851 | obstacle0231: 1852 | corners: 1853 | - [9.588134684910239, 7.717857919096996] 1854 | - [9.588134684910239, 7.936691154506958] 1855 | - [10.004757138376615, 7.725406746440797] 1856 | - [9.660895790721517, 7.717857919096996] 1857 | - [9.588134684910239, 7.717857919096996] 1858 | shape: polygon 1859 | obstacle0232: 1860 | corners: 1861 | - [5.362823871382723, 5.258846233967336] 1862 | - [5.362823871382723, 5.740619154634836] 1863 | - [5.809822622258879, 5.565823993142133] 1864 | - [5.851425399134002, 5.258846233967336] 1865 | - [5.362823871382723, 5.258846233967336] 1866 | shape: polygon 1867 | obstacle0233: 1868 | corners: 1869 | - [2.674509007257983, 0.8821618158216911] 1870 | - [2.674509007257983, 1.0928157604607176] 1871 | - [2.987194046772951, 1.0742051782889872] 1872 | - [2.696720403083075, 0.8821618158216911] 1873 | - [2.674509007257983, 0.8821618158216911] 1874 | shape: polygon 1875 | obstacle0234: 1876 | corners: 1877 | - [0.9797891188076209, -2.070638557107969] 1878 | - [0.9797891188076209, -1.9984491868376884] 1879 | - [1.2285914830637483, -1.837090593828498] 1880 | - [1.046614891419494, -2.070638557107969] 1881 | - [0.9797891188076209, -2.070638557107969] 1882 | shape: polygon 1883 | obstacle0235: 1884 | corners: 1885 | - [11.421975965062673, -0.489287922931231] 1886 | - [11.421975965062673, -0.4757149435003627] 1887 | - [11.780138955577694, -0.013118141920690829] 1888 | - [11.78617678123596, -0.489287922931231] 1889 | - [11.421975965062673, -0.489287922931231] 1890 | shape: polygon 1891 | obstacle0236: 1892 | corners: 1893 | - [5.400469687047118, -2.270777060801072] 1894 | - [5.400469687047118, -2.0539352894584493] 1895 | - [5.4238435386514245, -1.9898218376506696] 1896 | - [5.639403260251896, -2.270777060801072] 1897 | - [5.400469687047118, -2.270777060801072] 1898 | shape: polygon 1899 | obstacle0237: 1900 | corners: 1901 | - [-1.8373843820970486, -0.5935895030323675] 1902 | - [-1.8373843820970486, -0.4924525085330739] 1903 | - [-1.3517724591606732, -0.3578058681907213] 1904 | - [-1.4519636830863107, -0.5935895030323675] 1905 | - [-1.8373843820970486, -0.5935895030323675] 1906 | shape: polygon 1907 | obstacle0238: 1908 | corners: 1909 | - [11.217851596461678, 2.1140127215936726] 1910 | - [11.217851596461678, 2.4384506069105774] 1911 | - [11.638706527288301, 2.4184112018597452] 1912 | - [11.688271675735376, 2.1140127215936726] 1913 | - [11.217851596461678, 2.1140127215936726] 1914 | shape: polygon 1915 | obstacle0239: 1916 | corners: 1917 | - [6.875725169975503, 6.4998182225256045] 1918 | - [6.875725169975503, 6.801394208401502] 1919 | - [7.203243355670436, 6.842981454329193] 1920 | - [7.105452450760318, 6.4998182225256045] 1921 | - [6.875725169975503, 6.4998182225256045] 1922 | shape: polygon 1923 | obstacle0240: 1924 | corners: 1925 | - [5.0774698091732375, 3.3331695716683534] 1926 | - [5.0774698091732375, 3.3724293421848386] 1927 | - [5.45219954269684, 3.440704856019055] 1928 | - [5.1119155931559765, 3.3331695716683534] 1929 | - [5.0774698091732375, 3.3331695716683534] 1930 | shape: polygon 1931 | obstacle0241: 1932 | corners: 1933 | - [11.75281763648337, 5.39250323124344] 1934 | - [11.75281763648337, 5.8464230825259715] 1935 | - [12.009026684540071, 5.560430529135169] 1936 | - [12.119375891807863, 5.39250323124344] 1937 | - [11.75281763648337, 5.39250323124344] 1938 | shape: polygon 1939 | obstacle0242: 1940 | corners: 1941 | - [-1.9441063884021665, -2.999798203220033] 1942 | - [-1.9441063884021665, -2.540962457344431] 1943 | - [-1.730956435624093, -2.690309949446872] 1944 | - [-1.639987815356724, -2.999798203220033] 1945 | - [-1.9441063884021665, -2.999798203220033] 1946 | shape: polygon 1947 | obstacle0243: 1948 | corners: 1949 | - [9.007348195292353, -0.8369023060844802] 1950 | - [9.007348195292353, -0.7295934442004799] 1951 | - [9.229793460355834, -0.6185252004409545] 1952 | - [9.037163773482074, -0.8369023060844802] 1953 | - [9.007348195292353, -0.8369023060844802] 1954 | shape: polygon 1955 | obstacle0244: 1956 | corners: 1957 | - [7.348615266020001, 0.813536729751565] 1958 | - [7.348615266020001, 0.983805061955638] 1959 | - [7.378416833078235, 1.2174059269506758] 1960 | - [7.71987793998311, 0.813536729751565] 1961 | - [7.348615266020001, 0.813536729751565] 1962 | shape: polygon 1963 | obstacle0245: 1964 | corners: 1965 | - [4.574272122143372, 4.76968856148266] 1966 | - [4.574272122143372, 5.227850501123854] 1967 | - [4.754252833892679, 4.856175028140501] 1968 | - [4.673863967639464, 4.76968856148266] 1969 | - [4.574272122143372, 4.76968856148266] 1970 | shape: polygon 1971 | obstacle0246: 1972 | corners: 1973 | - [1.6475246126538456, 4.1243807076766235] 1974 | - [1.6475246126538456, 4.276539311971784] 1975 | - [1.6676410120710357, 4.282220447751271] 1976 | - [1.9231952610722478, 4.1243807076766235] 1977 | - [1.6475246126538456, 4.1243807076766235] 1978 | shape: polygon 1979 | obstacle0247: 1980 | corners: 1981 | - [0.7965665759634044, 1.2655148767528397] 1982 | - [0.7965665759634044, 1.7385580494669268] 1983 | - [1.2172108549453458, 1.69963262038671] 1984 | - [0.8090492939473086, 1.2655148767528397] 1985 | - [0.7965665759634044, 1.2655148767528397] 1986 | shape: polygon 1987 | obstacle0248: 1988 | corners: 1989 | - [4.281773567628154, -1.7594699818846817] 1990 | - [4.281773567628154, -1.6447813864398526] 1991 | - [4.41589296744469, -1.3128667875438969] 1992 | - [4.622937610712454, -1.7594699818846817] 1993 | - [4.281773567628154, -1.7594699818846817] 1994 | shape: polygon 1995 | obstacle0249: 1996 | corners: 1997 | - [1.1634095360053216, 4.960400464319412] 1998 | - [1.1634095360053216, 5.370132032417716] 1999 | - [1.5372925007654188, 5.17519176082657] 2000 | - [1.5581110323326681, 4.960400464319412] 2001 | - [1.1634095360053216, 4.960400464319412] 2002 | shape: polygon 2003 | obstacle0250: 2004 | corners: 2005 | - [5.631718686005424, -1.129419852086369] 2006 | - [5.631718686005424, -0.8576170421342806] 2007 | - [5.719646769443692, -0.8731359275478624] 2008 | - [5.651376809284834, -1.129419852086369] 2009 | - [5.631718686005424, -1.129419852086369] 2010 | shape: polygon 2011 | obstacle0251: 2012 | corners: 2013 | - [4.616006665775574, -0.763052004254706] 2014 | - [4.616006665775574, -0.5615206022693507] 2015 | - [4.974321222864695, -0.6986562404822998] 2016 | - [4.798067126314309, -0.763052004254706] 2017 | - [4.616006665775574, -0.763052004254706] 2018 | shape: polygon 2019 | obstacle0252: 2020 | corners: 2021 | - [11.615207802303381, 4.089723273816122] 2022 | - [11.615207802303381, 4.453306201860787] 2023 | - [11.782246265151372, 4.195967025538077] 2024 | - [11.888349974609987, 4.089723273816122] 2025 | - [11.615207802303381, 4.089723273816122] 2026 | shape: polygon 2027 | obstacle0253: 2028 | corners: 2029 | - [-0.09908136870693296, 1.8706162351011653] 2030 | - [-0.09908136870693296, 1.9810324247386184] 2031 | - [0.1879756190925438, 1.9620798089848654] 2032 | - [0.047261768343116684, 1.8706162351011653] 2033 | - [-0.09908136870693296, 1.8706162351011653] 2034 | shape: polygon 2035 | obstacle0254: 2036 | corners: 2037 | - [-0.3407848646177931, 0.07654466693084139] 2038 | - [-0.3407848646177931, 0.538516289299678] 2039 | - [-0.2273157629971333, 0.41417893446574694] 2040 | - [-0.059996229413848856, 0.07654466693084139] 2041 | - [-0.3407848646177931, 0.07654466693084139] 2042 | shape: polygon 2043 | obstacle0255: 2044 | corners: 2045 | - [6.007864761927195, 4.760477083504921] 2046 | - [6.007864761927195, 5.172172450992295] 2047 | - [6.104600284077471, 4.855713791156087] 2048 | - [6.392982145526371, 4.760477083504921] 2049 | - [6.007864761927195, 4.760477083504921] 2050 | shape: polygon 2051 | obstacle0256: 2052 | corners: 2053 | - [11.656305658270302, 4.80261142004748] 2054 | - [11.656305658270302, 5.019709534378377] 2055 | - [11.843846515047778, 5.160881479808479] 2056 | - [11.772023279839937, 4.80261142004748] 2057 | - [11.656305658270302, 4.80261142004748] 2058 | shape: polygon 2059 | obstacle0257: 2060 | corners: 2061 | - [11.691421625189907, 3.4965178545792472] 2062 | - [11.691421625189907, 3.6705386083517615] 2063 | - [11.760633663736533, 3.691720244385823] 2064 | - [11.717890716876749, 3.4965178545792472] 2065 | - [11.691421625189907, 3.4965178545792472] 2066 | shape: polygon 2067 | obstacle0258: 2068 | corners: 2069 | - [2.931129122465996, -0.2819219760726037] 2070 | - [2.931129122465996, 0.044482470155946585] 2071 | - [3.0081904290557584, -0.016427777846403258] 2072 | - [3.060678676024196, -0.2819219760726037] 2073 | - [2.931129122465996, -0.2819219760726037] 2074 | shape: polygon 2075 | obstacle0259: 2076 | corners: 2077 | - [4.334387824480771, 6.531573979914796] 2078 | - [4.334387824480771, 6.9271876669499] 2079 | - [4.414761862627339, 6.83864768485827] 2080 | - [4.721314537320234, 6.531573979914796] 2081 | - [4.334387824480771, 6.531573979914796] 2082 | shape: polygon 2083 | obstacle0260: 2084 | corners: 2085 | - [2.497021214086308, 1.7028951288342338] 2086 | - [2.497021214086308, 1.9507893614848149] 2087 | - [2.776252430524439, 1.8616937117073769] 2088 | - [2.5197928521678556, 1.7028951288342338] 2089 | - [2.497021214086308, 1.7028951288342338] 2090 | shape: polygon 2091 | obstacle0261: 2092 | corners: 2093 | - [1.2055179411306325, 5.966072674603527] 2094 | - [1.2055179411306325, 6.1820011481326995] 2095 | - [1.4329284650798346, 6.06095042979543] 2096 | - [1.4825898339105696, 5.966072674603527] 2097 | - [1.2055179411306325, 5.966072674603527] 2098 | shape: polygon 2099 | obstacle0262: 2100 | corners: 2101 | - [6.278065591847096, 6.19275681366401] 2102 | - [6.278065591847096, 6.257112879467987] 2103 | - [6.6828002084269755, 6.452101370367026] 2104 | - [6.322918282290331, 6.19275681366401] 2105 | - [6.278065591847096, 6.19275681366401] 2106 | shape: polygon 2107 | obstacle0263: 2108 | corners: 2109 | - [10.24142745193333, 2.442371287863881] 2110 | - [10.24142745193333, 2.8356020894814753] 2111 | - [10.459203186662927, 2.84654732334043] 2112 | - [10.506349601717885, 2.442371287863881] 2113 | - [10.24142745193333, 2.442371287863881] 2114 | shape: polygon 2115 | obstacle0264: 2116 | corners: 2117 | - [3.288160897331716, -1.6349257714524286] 2118 | - [3.288160897331716, -1.4005497892523162] 2119 | - [3.4040819044936534, -1.1909483916979968] 2120 | - [3.5917677548290157, -1.6349257714524286] 2121 | - [3.288160897331716, -1.6349257714524286] 2122 | shape: polygon 2123 | obstacle0265: 2124 | corners: 2125 | - [9.036336120421879, -1.936025156256286] 2126 | - [9.036336120421879, -1.4737079646318576] 2127 | - [9.157701605932425, -1.7626575597181964] 2128 | - [9.292053328155898, -1.936025156256286] 2129 | - [9.036336120421879, -1.936025156256286] 2130 | shape: polygon 2131 | obstacle0266: 2132 | corners: 2133 | - [2.3040720081067914, 3.5487592549610003] 2134 | - [2.3040720081067914, 3.567439584659207] 2135 | - [2.44174079391662, 3.7805528257268795] 2136 | - [2.5177709361077283, 3.5487592549610003] 2137 | - [2.3040720081067914, 3.5487592549610003] 2138 | shape: polygon 2139 | obstacle0267: 2140 | corners: 2141 | - [5.630306233770097, -2.9121392974961102] 2142 | - [5.630306233770097, -2.9002169240230553] 2143 | - [5.693643163745947, -2.579729636852109] 2144 | - [5.675095827045445, -2.9121392974961102] 2145 | - [5.630306233770097, -2.9121392974961102] 2146 | shape: polygon 2147 | obstacle0268: 2148 | corners: 2149 | - [0.17460402726787194, 0.6981730219929938] 2150 | - [0.17460402726787194, 1.0721075287588704] 2151 | - [0.36308522819912836, 0.93943187071157] 2152 | - [0.5165961291020702, 0.6981730219929938] 2153 | - [0.17460402726787194, 0.6981730219929938] 2154 | shape: polygon 2155 | obstacle0269: 2156 | corners: 2157 | - [4.472507072933341, 4.469118010646438] 2158 | - [4.472507072933341, 4.733110343344958] 2159 | - [4.7695602562234765, 4.620009436447113] 2160 | - [4.735429842038133, 4.469118010646438] 2161 | - [4.472507072933341, 4.469118010646438] 2162 | shape: polygon 2163 | obstacle0270: 2164 | corners: 2165 | - [-1.6330970587832228, 2.4656338546818732] 2166 | - [-1.6330970587832228, 2.598532385882769] 2167 | - [-1.2301778196185373, 2.536827978627419] 2168 | - [-1.3019806411313317, 2.4656338546818732] 2169 | - [-1.6330970587832228, 2.4656338546818732] 2170 | shape: polygon 2171 | obstacle0271: 2172 | corners: 2173 | - [9.414894939218865, -1.3966185417664345] 2174 | - [9.414894939218865, -1.2584737000450417] 2175 | - [9.702127619162246, -1.3242907841468385] 2176 | - [9.672989226482683, -1.3966185417664345] 2177 | - [9.414894939218865, -1.3966185417664345] 2178 | shape: polygon 2179 | obstacle0272: 2180 | corners: 2181 | - [5.436301927342793, -2.2514704346442613] 2182 | - [5.436301927342793, -2.0439051261750336] 2183 | - [5.726010181338337, -2.051873089479934] 2184 | - [5.574701248834282, -2.2514704346442613] 2185 | - [5.436301927342793, -2.2514704346442613] 2186 | shape: polygon 2187 | obstacle0273: 2188 | corners: 2189 | - [4.411630191571403, -0.11057489926794473] 2190 | - [4.411630191571403, 0.026452075147729737] 2191 | - [4.811291974593227, 0.03716578730089587] 2192 | - [4.708043020393911, -0.11057489926794473] 2193 | - [4.411630191571403, -0.11057489926794473] 2194 | shape: polygon 2195 | obstacle0274: 2196 | corners: 2197 | - [1.0298401514229787, -0.8701062979891336] 2198 | - [1.0298401514229787, -0.5012605756131407] 2199 | - [1.3540879487515076, -0.4531984762964025] 2200 | - [1.4078462114362973, -0.8701062979891336] 2201 | - [1.0298401514229787, -0.8701062979891336] 2202 | shape: polygon 2203 | obstacle0275: 2204 | corners: 2205 | - [8.425366636220152, -0.5937808590643829] 2206 | - [8.425366636220152, -0.11466859145496056] 2207 | - [8.477283611285502, -0.20376278060976338] 2208 | - [8.526809441016068, -0.5937808590643829] 2209 | - [8.425366636220152, -0.5937808590643829] 2210 | shape: polygon 2211 | obstacle0276: 2212 | corners: 2213 | - [8.530162414535308, 0.7784839774381918] 2214 | - [8.530162414535308, 0.9937325653295084] 2215 | - [8.732631310238094, 0.865730104230338] 2216 | - [8.857562048548987, 0.7784839774381918] 2217 | - [8.530162414535308, 0.7784839774381918] 2218 | shape: polygon 2219 | obstacle0277: 2220 | corners: 2221 | - [10.842907139985845, 1.5769968110394181] 2222 | - [10.842907139985845, 1.984819450650444] 2223 | - [10.891571494647971, 1.987153949601063] 2224 | - [11.096031712854142, 1.5769968110394181] 2225 | - [10.842907139985845, 1.5769968110394181] 2226 | shape: polygon 2227 | obstacle0278: 2228 | corners: 2229 | - [11.030384962725716, 3.6026690133139985] 2230 | - [11.030384962725716, 3.8230654742277834] 2231 | - [11.435447119847787, 3.9225191069485] 2232 | - [11.449045466453345, 3.6026690133139985] 2233 | - [11.030384962725716, 3.6026690133139985] 2234 | shape: polygon 2235 | obstacle0279: 2236 | corners: 2237 | - [2.7975079269905425, 2.7794464134627983] 2238 | - [2.7975079269905425, 2.870956659123211] 2239 | - [2.8459130526435734, 2.9004940201444187] 2240 | - [3.034125020228145, 2.7794464134627983] 2241 | - [2.7975079269905425, 2.7794464134627983] 2242 | shape: polygon 2243 | obstacle0280: 2244 | corners: 2245 | - [9.529882231736556, 6.6556000510612705] 2246 | - [9.529882231736556, 6.961266612076214] 2247 | - [9.900163088477338, 6.855804868830333] 2248 | - [9.799425245136364, 6.6556000510612705] 2249 | - [9.529882231736556, 6.6556000510612705] 2250 | shape: polygon 2251 | obstacle0281: 2252 | corners: 2253 | - [6.925313537813375, -2.8665058660035325] 2254 | - [6.925313537813375, -2.625071528117981] 2255 | - [6.988364786300779, -2.8335809721893637] 2256 | - [7.334280162566131, -2.8665058660035325] 2257 | - [6.925313537813375, -2.8665058660035325] 2258 | shape: polygon 2259 | obstacle0282: 2260 | corners: 2261 | - [9.996689711252817, -2.5943791818149724] 2262 | - [9.996689711252817, -2.5108354419445873] 2263 | - [10.358236560550903, -2.560642914624444] 2264 | - [10.199850498697018, -2.5943791818149724] 2265 | - [9.996689711252817, -2.5943791818149724] 2266 | shape: polygon 2267 | obstacle0283: 2268 | corners: 2269 | - [-0.269998687525157, 2.4680449192566574] 2270 | - [-0.269998687525157, 2.476371060026938] 2271 | - [0.09982819419519057, 2.581937811163005] 2272 | - [-0.26597352193362467, 2.4680449192566574] 2273 | - [-0.269998687525157, 2.4680449192566574] 2274 | shape: polygon 2275 | obstacle0284: 2276 | corners: 2277 | - [1.9676368563005395, -2.0967412396782277] 2278 | - [1.9676368563005395, -2.0657077029963857] 2279 | - [2.044965511985463, -2.0724696720783617] 2280 | - [2.312049257797925, -2.0967412396782277] 2281 | - [1.9676368563005395, -2.0967412396782277] 2282 | shape: polygon 2283 | obstacle0285: 2284 | corners: 2285 | - [7.773683557118446, 5.881737033743054] 2286 | - [7.773683557118446, 5.94303177042315] 2287 | - [8.035689071531595, 6.1556682740576685] 2288 | - [8.240126846535816, 5.881737033743054] 2289 | - [7.773683557118446, 5.881737033743054] 2290 | shape: polygon 2291 | obstacle0286: 2292 | corners: 2293 | - [6.049942977996903, 3.5985797774855914] 2294 | - [6.049942977996903, 4.050569470335185] 2295 | - [6.285682734477527, 3.7560026847949475] 2296 | - [6.4829684718081335, 3.5985797774855914] 2297 | - [6.049942977996903, 3.5985797774855914] 2298 | shape: polygon 2299 | obstacle0287: 2300 | corners: 2301 | - [1.8510483345614408, 2.2252315561628144] 2302 | - [1.8510483345614408, 2.5300952972744737] 2303 | - [2.2277649187762085, 2.5599550230216908] 2304 | - [2.1989619345531715, 2.2252315561628144] 2305 | - [1.8510483345614408, 2.2252315561628144] 2306 | shape: polygon 2307 | obstacle0288: 2308 | corners: 2309 | - [-1.8338039498636258, 2.203591278949288] 2310 | - [-1.8338039498636258, 2.344789101255346] 2311 | - [-1.3715547514846171, 2.298991743822811] 2312 | - [-1.3783986473922454, 2.203591278949288] 2313 | - [-1.8338039498636258, 2.203591278949288] 2314 | shape: polygon 2315 | obstacle0289: 2316 | corners: 2317 | - [8.992720216071536, -2.5767186149990184] 2318 | - [8.992720216071536, -2.303578118620425] 2319 | - [9.262007817515135, -2.1289767944638993] 2320 | - [9.181850135819488, -2.5767186149990184] 2321 | - [8.992720216071536, -2.5767186149990184] 2322 | shape: polygon 2323 | obstacle0290: 2324 | corners: 2325 | - [11.699084425680422, 2.9868220244749706] 2326 | - [11.699084425680422, 3.187750163194904] 2327 | - [12.161269621957317, 2.998869421111216] 2328 | - [11.996865455678707, 2.9868220244749706] 2329 | - [11.699084425680422, 2.9868220244749706] 2330 | shape: polygon 2331 | obstacle0291: 2332 | corners: 2333 | - [9.38106969203397, 0.39979012447893236] 2334 | - [9.38106969203397, 0.5927343103781855] 2335 | - [9.838364647583374, 0.6619717848387879] 2336 | - [9.606860442801919, 0.39979012447893236] 2337 | - [9.38106969203397, 0.39979012447893236] 2338 | shape: polygon 2339 | obstacle0292: 2340 | corners: 2341 | - [11.682109093664605, -1.8771600558020207] 2342 | - [11.682109093664605, -1.5985154795624028] 2343 | - [11.68784306295423, -1.8354827183016091] 2344 | - [12.113853015351873, -1.8771600558020207] 2345 | - [11.682109093664605, -1.8771600558020207] 2346 | shape: polygon 2347 | obstacle0293: 2348 | corners: 2349 | - [3.2952559416170892, 7.4080416392984905] 2350 | - [3.2952559416170892, 7.666195036913894] 2351 | - [3.629442190563699, 7.504796679325995] 2352 | - [3.446299390465536, 7.4080416392984905] 2353 | - [3.2952559416170892, 7.4080416392984905] 2354 | shape: polygon 2355 | obstacle0294: 2356 | corners: 2357 | - [1.8218239252786477, 3.9807127006089447] 2358 | - [1.8218239252786477, 4.474524122317429] 2359 | - [2.2847804246749037, 3.981124307711099] 2360 | - [1.8489734076012807, 3.9807127006089447] 2361 | - [1.8218239252786477, 3.9807127006089447] 2362 | shape: polygon 2363 | obstacle0295: 2364 | corners: 2365 | - [2.945764709428987, 5.483286444920207] 2366 | - [2.945764709428987, 5.910161437404996] 2367 | - [3.4339366696286033, 5.941449468114807] 2368 | - [3.2940489836564253, 5.483286444920207] 2369 | - [2.945764709428987, 5.483286444920207] 2370 | shape: polygon 2371 | obstacle0296: 2372 | corners: 2373 | - [7.951467179214568, 0.7020067818488607] 2374 | - [7.951467179214568, 0.8101889575536179] 2375 | - [8.01234714527694, 1.0243826636677196] 2376 | - [8.362628578749227, 0.7020067818488607] 2377 | - [7.951467179214568, 0.7020067818488607] 2378 | shape: polygon 2379 | obstacle0297: 2380 | corners: 2381 | - [4.9201651482625826, 4.905639497121584] 2382 | - [4.9201651482625826, 5.005067485657152] 2383 | - [5.055751124406864, 5.3136793518888945] 2384 | - [5.091048541372625, 4.905639497121584] 2385 | - [4.9201651482625826, 4.905639497121584] 2386 | shape: polygon 2387 | obstacle0298: 2388 | corners: 2389 | - [9.802759113392376, -2.4679528617358386] 2390 | - [9.802759113392376, -2.4503375531699416] 2391 | - [10.301121117431425, -2.0842234023190094] 2392 | - [9.878477688390689, -2.4679528617358386] 2393 | - [9.802759113392376, -2.4679528617358386] 2394 | shape: polygon 2395 | obstacle0299: 2396 | corners: 2397 | - [-0.7600421555292076, 0.13712178530141284] 2398 | - [-0.7600421555292076, 0.3671080440558885] 2399 | - [-0.6370814218043783, 0.23548850793543497] 2400 | - [-0.29228208575299947, 0.13712178530141284] 2401 | - [-0.7600421555292076, 0.13712178530141284] 2402 | shape: polygon 2403 | obstacle0300: 2404 | corners: 2405 | - [-0.3085870139437381, -0.743532240967002] 2406 | - [-0.3085870139437381, -0.6649105536303943] 2407 | - [-0.11879920171233677, -0.6568305487181425] 2408 | - [0.16156701876038276, -0.743532240967002] 2409 | - [-0.3085870139437381, -0.743532240967002] 2410 | shape: polygon 2411 | obstacle0301: 2412 | corners: 2413 | - [8.112463753264887, 4.032936306508104] 2414 | - [8.112463753264887, 4.46217690985364] 2415 | - [8.175971399023904, 4.311614867408321] 2416 | - [8.409525271217547, 4.032936306508104] 2417 | - [8.112463753264887, 4.032936306508104] 2418 | shape: polygon 2419 | obstacle0302: 2420 | corners: 2421 | - [3.9385761075028647, 7.520710079018297] 2422 | - [3.9385761075028647, 7.5820177923793395] 2423 | - [4.246400492574052, 7.9919420349665495] 2424 | - [4.228053681583447, 7.520710079018297] 2425 | - [3.9385761075028647, 7.520710079018297] 2426 | shape: polygon 2427 | obstacle0303: 2428 | corners: 2429 | - [8.059135196022806, -1.483760547664568] 2430 | - [8.059135196022806, -1.1835718257021068] 2431 | - [8.28398332015004, -1.0296889695890852] 2432 | - [8.118634127003439, -1.483760547664568] 2433 | - [8.059135196022806, -1.483760547664568] 2434 | shape: polygon 2435 | obstacle0304: 2436 | corners: 2437 | - [2.17921396161813, 3.929567172724566] 2438 | - [2.17921396161813, 3.9986079225949123] 2439 | - [2.456176725664067, 4.091935667913402] 2440 | - [2.320022760116091, 3.929567172724566] 2441 | - [2.17921396161813, 3.929567172724566] 2442 | shape: polygon 2443 | obstacle0305: 2444 | corners: 2445 | - [-1.3996390078988556, 4.233010518462433] 2446 | - [-1.3996390078988556, 4.32572302250183] 2447 | - [-1.0742951830223206, 4.692441548257995] 2448 | - [-0.9332769490572074, 4.233010518462433] 2449 | - [-1.3996390078988556, 4.233010518462433] 2450 | shape: polygon 2451 | obstacle0306: 2452 | corners: 2453 | - [3.9832920843235344, -2.0000144631159356] 2454 | - [3.9832920843235344, -1.539800593798602] 2455 | - [4.074572110158768, -1.795883985140849] 2456 | - [4.472829964289862, -2.0000144631159356] 2457 | - [3.9832920843235344, -2.0000144631159356] 2458 | shape: polygon 2459 | obstacle0307: 2460 | corners: 2461 | - [8.974005543337938, 6.038275691911302] 2462 | - [8.974005543337938, 6.319392477449389] 2463 | - [9.441498217269205, 6.197468472077233] 2464 | - [9.015745742155598, 6.038275691911302] 2465 | - [8.974005543337938, 6.038275691911302] 2466 | shape: polygon 2467 | obstacle0308: 2468 | corners: 2469 | - [11.391236198909892, 6.4903885317865715] 2470 | - [11.391236198909892, 6.987059319189188] 2471 | - [11.584058880224083, 6.874928510953459] 2472 | - [11.75143567159264, 6.4903885317865715] 2473 | - [11.391236198909892, 6.4903885317865715] 2474 | shape: polygon 2475 | obstacle0309: 2476 | corners: 2477 | - [7.572893158443801, -0.8321040027385349] 2478 | - [7.572893158443801, -0.7608094232446718] 2479 | - [7.8289562791868645, -0.6945639415851117] 2480 | - [7.608921656228392, -0.8321040027385349] 2481 | - [7.572893158443801, -0.8321040027385349] 2482 | shape: polygon 2483 | obstacle0310: 2484 | corners: 2485 | - [10.38395395865551, 6.9903195403645935] 2486 | - [10.38395395865551, 7.113375417915733] 2487 | - [10.599459375500352, 7.394798072465845] 2488 | - [10.689541605833757, 6.9903195403645935] 2489 | - [10.38395395865551, 6.9903195403645935] 2490 | shape: polygon 2491 | obstacle0311: 2492 | corners: 2493 | - [10.463215090259336, 2.0819576439343424] 2494 | - [10.463215090259336, 2.27699763736663] 2495 | - [10.731131527845207, 2.3214230931973945] 2496 | - [10.51222088752761, 2.0819576439343424] 2497 | - [10.463215090259336, 2.0819576439343424] 2498 | shape: polygon 2499 | obstacle0312: 2500 | corners: 2501 | - [6.13754936737711, -2.4316103303588887] 2502 | - [6.13754936737711, -2.1752415972508015] 2503 | - [6.256025553137533, -2.3156513642242027] 2504 | - [6.283872318257809, -2.4316103303588887] 2505 | - [6.13754936737711, -2.4316103303588887] 2506 | shape: polygon 2507 | obstacle0313: 2508 | corners: 2509 | - [5.87378834892266, -2.253732181062451] 2510 | - [5.87378834892266, -2.0774786557320133] 2511 | - [6.077524039720348, -2.075985311687054] 2512 | - [6.17635732367434, -2.253732181062451] 2513 | - [5.87378834892266, -2.253732181062451] 2514 | shape: polygon 2515 | obstacle0314: 2516 | corners: 2517 | - [9.859360746090202, -1.4118106286164682] 2518 | - [9.859360746090202, -1.206521036456326] 2519 | - [10.204159001176365, -1.2327901979985687] 2520 | - [10.261611640241405, -1.4118106286164682] 2521 | - [9.859360746090202, -1.4118106286164682] 2522 | shape: polygon 2523 | obstacle0315: 2524 | corners: 2525 | - [0.6734564483347825, 6.090123688150717] 2526 | - [0.6734564483347825, 6.420034428757718] 2527 | - [0.9787290463298977, 6.330170978879691] 2528 | - [0.9020178556537664, 6.090123688150717] 2529 | - [0.6734564483347825, 6.090123688150717] 2530 | shape: polygon 2531 | obstacle0316: 2532 | corners: 2533 | - [6.055653973385262, 4.594963322518146] 2534 | - [6.055653973385262, 4.727296497792008] 2535 | - [6.531477110261995, 4.661872218554072] 2536 | - [6.391350251875866, 4.594963322518146] 2537 | - [6.055653973385262, 4.594963322518146] 2538 | shape: polygon 2539 | obstacle0317: 2540 | corners: 2541 | - [7.341400077802582, 2.568845237648212] 2542 | - [7.341400077802582, 2.945588983403528] 2543 | - [7.581328497808879, 2.9808183116014706] 2544 | - [7.407181884832106, 2.568845237648212] 2545 | - [7.341400077802582, 2.568845237648212] 2546 | shape: polygon 2547 | obstacle0318: 2548 | corners: 2549 | - [4.530055016858265, 1.7255735639664378] 2550 | - [4.530055016858265, 1.9394704588771372] 2551 | - [5.012609596993826, 2.196959476939015] 2552 | - [4.746084118732834, 1.7255735639664378] 2553 | - [4.530055016858265, 1.7255735639664378] 2554 | shape: polygon 2555 | obstacle0319: 2556 | corners: 2557 | - [1.751335397326426, 5.834394840972697] 2558 | - [1.751335397326426, 6.233017540095857] 2559 | - [1.8763254518957773, 6.193344293662682] 2560 | - [1.9102373335795122, 5.834394840972697] 2561 | - [1.751335397326426, 5.834394840972697] 2562 | shape: polygon 2563 | obstacle0320: 2564 | corners: 2565 | - [3.6440385446445003, 7.641553297155399] 2566 | - [3.6440385446445003, 7.826744195962036] 2567 | - [4.080797357865631, 7.89034022933529] 2568 | - [3.6864331551417795, 7.641553297155399] 2569 | - [3.6440385446445003, 7.641553297155399] 2570 | shape: polygon 2571 | obstacle0321: 2572 | corners: 2573 | - [6.474112311513352, 3.843812219239328] 2574 | - [6.474112311513352, 4.027913864738781] 2575 | - [6.832578573865764, 4.14921478490189] 2576 | - [6.948330418169198, 3.843812219239328] 2577 | - [6.474112311513352, 3.843812219239328] 2578 | shape: polygon 2579 | obstacle0322: 2580 | corners: 2581 | - [5.647556084392625, 2.2810587080754683] 2582 | - [5.647556084392625, 2.6587815222604436] 2583 | - [6.135066292363261, 2.483838267114416] 2584 | - [5.818623740953019, 2.2810587080754683] 2585 | - [5.647556084392625, 2.2810587080754683] 2586 | shape: polygon 2587 | obstacle0323: 2588 | corners: 2589 | - [2.0002864724093072, 3.7205896897688238] 2590 | - [2.0002864724093072, 4.001720933817842] 2591 | - [2.1009739884089846, 3.7237889099515575] 2592 | - [2.1107271484936097, 3.7205896897688238] 2593 | - [2.0002864724093072, 3.7205896897688238] 2594 | shape: polygon 2595 | obstacle0324: 2596 | corners: 2597 | - [-0.010691203429574525, 4.086818646285602] 2598 | - [-0.010691203429574525, 4.560347281470769] 2599 | - [0.13424573109567067, 4.464256611639443] 2600 | - [0.3327251058592846, 4.086818646285602] 2601 | - [-0.010691203429574525, 4.086818646285602] 2602 | shape: polygon 2603 | obstacle0325: 2604 | corners: 2605 | - [9.385134581540033, 7.158506190330229] 2606 | - [9.385134581540033, 7.260597049154231] 2607 | - [9.677188403006314, 7.494967901219084] 2608 | - [9.62655245736823, 7.158506190330229] 2609 | - [9.385134581540033, 7.158506190330229] 2610 | shape: polygon 2611 | obstacle0326: 2612 | corners: 2613 | - [3.275137282784023, -0.9592865611859156] 2614 | - [3.275137282784023, -0.9044652227374401] 2615 | - [3.7718316303880473, -0.6249290914070957] 2616 | - [3.2879201919617795, -0.9592865611859156] 2617 | - [3.275137282784023, -0.9592865611859156] 2618 | shape: polygon 2619 | obstacle0327: 2620 | corners: 2621 | - [9.686491963746574, -2.5813612478015027] 2622 | - [9.686491963746574, -2.4549442852280716] 2623 | - [9.804948482712092, -2.5458007859515717] 2624 | - [9.798383077683988, -2.5813612478015027] 2625 | - [9.686491963746574, -2.5813612478015027] 2626 | shape: polygon 2627 | obstacle0328: 2628 | corners: 2629 | - [8.936762320520454, -2.16463751650283] 2630 | - [8.936762320520454, -1.962429488017251] 2631 | - [9.083503424080254, -1.9222876421533701] 2632 | - [9.432668503261183, -2.16463751650283] 2633 | - [8.936762320520454, -2.16463751650283] 2634 | shape: polygon 2635 | obstacle0329: 2636 | corners: 2637 | - [4.8052445010925195, -2.399090845205888] 2638 | - [4.8052445010925195, -1.9899764673688793] 2639 | - [4.869540024034459, -2.3833581823220986] 2640 | - [5.244694197869367, -2.399090845205888] 2641 | - [4.8052445010925195, -2.399090845205888] 2642 | shape: polygon 2643 | obstacle0330: 2644 | corners: 2645 | - [5.335567014163843, 4.158055447662976] 2646 | - [5.335567014163843, 4.35616685397618] 2647 | - [5.445056705786724, 4.4054583457059024] 2648 | - [5.682835333553239, 4.158055447662976] 2649 | - [5.335567014163843, 4.158055447662976] 2650 | shape: polygon 2651 | obstacle0331: 2652 | corners: 2653 | - [-1.1646616448189429, -0.1474080479892952] 2654 | - [-1.1646616448189429, 0.010284775896047704] 2655 | - [-0.737021460219367, 0.001003928756875161] 2656 | - [-0.7397483148477896, -0.1474080479892952] 2657 | - [-1.1646616448189429, -0.1474080479892952] 2658 | shape: polygon 2659 | obstacle0332: 2660 | corners: 2661 | - [3.9443158285605486, -2.7681770310031464] 2662 | - [3.9443158285605486, -2.4956905288482236] 2663 | - [4.1345716724756345, -2.692691012347993] 2664 | - [4.109781506531307, -2.7681770310031464] 2665 | - [3.9443158285605486, -2.7681770310031464] 2666 | shape: polygon 2667 | obstacle0333: 2668 | corners: 2669 | - [9.951352449896957, 4.610849072372558] 2670 | - [9.951352449896957, 5.039777931905377] 2671 | - [10.051733837344536, 5.1008947701314] 2672 | - [10.419624820476063, 4.610849072372558] 2673 | - [9.951352449896957, 4.610849072372558] 2674 | shape: polygon 2675 | obstacle0334: 2676 | corners: 2677 | - [8.128079498319421, 1.4476882641962998] 2678 | - [8.128079498319421, 1.887544091511945] 2679 | - [8.184347874143194, 1.4786595625154355] 2680 | - [8.203011552825027, 1.4476882641962998] 2681 | - [8.128079498319421, 1.4476882641962998] 2682 | shape: polygon 2683 | obstacle0335: 2684 | corners: 2685 | - [-0.5054547377382359, 2.8814725575490687] 2686 | - [-0.5054547377382359, 3.0697601104486663] 2687 | - [-0.15116250667425996, 2.9759794368591157] 2688 | - [-0.32770831946631157, 2.8814725575490687] 2689 | - [-0.5054547377382359, 2.8814725575490687] 2690 | shape: polygon 2691 | obstacle0336: 2692 | corners: 2693 | - [11.355469492664856, 5.172128050397209] 2694 | - [11.355469492664856, 5.187088684768146] 2695 | - [11.71311575065201, 5.217238011980497] 2696 | - [11.380112011441284, 5.172128050397209] 2697 | - [11.355469492664856, 5.172128050397209] 2698 | shape: polygon 2699 | obstacle0337: 2700 | corners: 2701 | - [4.184273942458961, 2.19593002733843] 2702 | - [4.184273942458961, 2.458034683086063] 2703 | - [4.383021045965497, 2.2512662333456026] 2704 | - [4.521664079875406, 2.19593002733843] 2705 | - [4.184273942458961, 2.19593002733843] 2706 | shape: polygon 2707 | obstacle0338: 2708 | corners: 2709 | - [0.38481529492294975, 0.9414635081921592] 2710 | - [0.38481529492294975, 1.3380654297819525] 2711 | - [0.7625706096816001, 1.2936146103333968] 2712 | - [0.8220307354194931, 0.9414635081921592] 2713 | - [0.38481529492294975, 0.9414635081921592] 2714 | shape: polygon 2715 | obstacle0339: 2716 | corners: 2717 | - [2.4695315603438495, -1.3743510378138013] 2718 | - [2.4695315603438495, -1.133555602071664] 2719 | - [2.716795685782643, -1.2364887457554459] 2720 | - [2.63005329023828, -1.3743510378138013] 2721 | - [2.4695315603438495, -1.3743510378138013] 2722 | shape: polygon 2723 | obstacle0340: 2724 | corners: 2725 | - [2.8287101568378876, 1.975575979328151] 2726 | - [2.8287101568378876, 2.168022997645517] 2727 | - [3.009498137233022, 2.107238207748651] 2728 | - [2.901401764982759, 1.975575979328151] 2729 | - [2.8287101568378876, 1.975575979328151] 2730 | shape: polygon 2731 | obstacle0341: 2732 | corners: 2733 | - [4.3892673911201285, 5.0054087263511775] 2734 | - [4.3892673911201285, 5.241733433481926] 2735 | - [4.7194062388702225, 5.1476459392358676] 2736 | - [4.641076703364705, 5.0054087263511775] 2737 | - [4.3892673911201285, 5.0054087263511775] 2738 | shape: polygon 2739 | obstacle0342: 2740 | corners: 2741 | - [-0.4340906818132857, 1.8993106155880781] 2742 | - [-0.4340906818132857, 1.9206476082429913] 2743 | - [-0.3142980284746227, 2.3552365518074376] 2744 | - [0.03404073601041252, 1.8993106155880781] 2745 | - [-0.4340906818132857, 1.8993106155880781] 2746 | shape: polygon 2747 | obstacle0343: 2748 | corners: 2749 | - [-1.949986515780034, 3.8411324800694118] 2750 | - [-1.949986515780034, 4.197369277237087] 2751 | - [-1.821631700554942, 4.278651143141268] 2752 | - [-1.5906439905887446, 3.8411324800694118] 2753 | - [-1.949986515780034, 3.8411324800694118] 2754 | shape: polygon 2755 | obstacle0344: 2756 | corners: 2757 | - [2.8074367147302217, -1.508399912716095] 2758 | - [2.8074367147302217, -1.4519429745785883] 2759 | - [3.1949497002453917, -1.3455588933073896] 2760 | - [2.836960173057503, -1.508399912716095] 2761 | - [2.8074367147302217, -1.508399912716095] 2762 | shape: polygon 2763 | obstacle0345: 2764 | corners: 2765 | - [11.925382969537722, -2.310956543522364] 2766 | - [11.925382969537722, -2.0671326339538676] 2767 | - [11.945529109746005, -1.899727366135704] 2768 | - [12.030035885376918, -2.310956543522364] 2769 | - [11.925382969537722, -2.310956543522364] 2770 | shape: polygon 2771 | obstacle0346: 2772 | corners: 2773 | - [-1.9639774897546254, -1.2291785320316335] 2774 | - [-1.9639774897546254, -0.7906143355285218] 2775 | - [-1.9497365176445638, -1.2140674979236377] 2776 | - [-1.7035238925789082, -1.2291785320316335] 2777 | - [-1.9639774897546254, -1.2291785320316335] 2778 | shape: polygon 2779 | obstacle0347: 2780 | corners: 2781 | - [10.504489512783335, 1.9712807674588415] 2782 | - [10.504489512783335, 2.2527879985327397] 2783 | - [10.994333094523073, 2.0824554900915286] 2784 | - [10.879124146673172, 1.9712807674588415] 2785 | - [10.504489512783335, 1.9712807674588415] 2786 | shape: polygon 2787 | obstacle0348: 2788 | corners: 2789 | - [-0.21129649006586848, 0.9964551154864063] 2790 | - [-0.21129649006586848, 1.1506370362682183] 2791 | - [0.004754727697794003, 1.3873583154135725] 2792 | - [0.2467384979766788, 0.9964551154864063] 2793 | - [-0.21129649006586848, 0.9964551154864063] 2794 | shape: polygon 2795 | obstacle0349: 2796 | corners: 2797 | - [0.8703395413860502, 7.800669220497534] 2798 | - [0.8703395413860502, 8.18340166985215] 2799 | - [1.3011210064222198, 7.829749656462661] 2800 | - [0.9867530899552981, 7.800669220497534] 2801 | - [0.8703395413860502, 7.800669220497534] 2802 | shape: polygon 2803 | -------------------------------------------------------------------------------- /search_classes.py: -------------------------------------------------------------------------------- 1 | class SearchNode(object): 2 | def __init__(self, state, parent_node=None, cost=0.0, action=None): 3 | self._parent = parent_node 4 | self._state = state 5 | self._action = action 6 | self._cost = cost 7 | 8 | def __repr__(self): 9 | return "" % (id(self), self.state, 10 | self.cost,id(self.parent)) 11 | 12 | @property 13 | def state(self): 14 | """Get the state represented by this SearchNode""" 15 | return self._state 16 | 17 | @property 18 | def parent(self): 19 | """Get the parent search node that we are coming from.""" 20 | return self._parent 21 | 22 | @property 23 | def cost(self): 24 | """Get the cost to this search state""" 25 | return self._cost 26 | 27 | @property 28 | def action(self): 29 | """Get the action that was taken to get from parent to the state represented by this node.""" 30 | return self._action 31 | 32 | def __eq__(self, other): 33 | return isinstance(other, SearchNode) and self._state == other._state 34 | 35 | def __hash__(self): 36 | return hash(self._state) 37 | 38 | class Path(object): 39 | """This class computes the path from the starting state until the state specified by the search_node 40 | parameter by iterating backwards.""" 41 | def __init__(self, search_node): 42 | self.path = [] 43 | node = search_node 44 | while node is not None: 45 | self.path.append(node.state) 46 | node = node.parent 47 | self.path.reverse() 48 | self.cost = search_node.cost 49 | 50 | def __repr__(self): 51 | return "Path of length %d, cost: %.3f: %s" % (len(self.path),self.cost, self.path) 52 | 53 | def edges(self): 54 | return zip(self.path[0:-1], self.path[1:]) 55 | 56 | def display(self, graph): 57 | dot_graph = graph._create_dot_graph() 58 | for n in dot_graph.get_nodes(): 59 | if n.get_name() == self.path[0]: 60 | n.set_color('blue') 61 | elif n.get_name() == self.path[-1]: 62 | n.set_color('green') 63 | elif n.get_name() in self.path: 64 | n.set_color('red') 65 | edges = self.edges() 66 | for e in dot_graph.get_edges(): 67 | if (e.get_source(), e.get_destination()) in edges: 68 | e.set_color('red') 69 | dot_graph.set_concentrate(False) 70 | display_svg(dot_graph.create_svg(), raw=True) 71 | -------------------------------------------------------------------------------- /simple.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | obstacles: 3 | obstacle0000: 4 | corners: 5 | - [7.895148675115022, -2.993701667329025] 6 | - [7.871221127011549, -2.1986781206137134] 7 | - [6.811033070038029, -1.1758150738318454] 8 | - [5.66844595160773, -2.680272473096988] 9 | - [7.895148675115022, -2.993701667329025] 10 | shape: polygon 11 | obstacle0001: 12 | corners: 13 | - [10.939543030879976, 1.4648026782628873] 14 | - [11.329069146948125, 3.5025692000130912] 15 | - [9.864210812673537, 3.163710213946939] 16 | - [10.080586974127355, 1.37565517221979] 17 | - [10.939543030879976, 1.4648026782628873] 18 | shape: polygon 19 | obstacle0002: 20 | corners: 21 | - [8.133466334048054, 3.2013076329201766] 22 | - [8.990451499884017, 3.3537416372419324] 23 | - [9.159452587730774, 4.967994835159924] 24 | - [7.293143686543085, 5.18782905009151] 25 | - [8.133466334048054, 3.2013076329201766] 26 | shape: polygon 27 | obstacle0003: 28 | corners: 29 | - [8.5314686453602, -2.9091602121879063] 30 | - [10.574721532473552, -2.865163015833325] 31 | - [10.675047801693015, -2.5313969979391486] 32 | - [8.50634666498255, -2.0962681927948976] 33 | - [8.5314686453602, -2.9091602121879063] 34 | shape: polygon 35 | obstacle0004: 36 | corners: 37 | - [4.801492910924698, 4.893471466623873] 38 | - [6.128839441771457, 3.5694991773209126] 39 | - [7.245447293189472, 5.466692884285398] 40 | - [5.221457135085937, 5.934116423872871] 41 | - [4.801492910924698, 4.893471466623873] 42 | shape: polygon 43 | obstacle0005: 44 | corners: 45 | - [0.9781916467954618, -0.7739662741168445] 46 | - [-0.3891761138833436, -2.0797759287726603] 47 | - [1.7549031473346566, -2.768745288611158] 48 | - [2.2916782343371276, -2.600033891473674] 49 | - [0.9781916467954618, -0.7739662741168445] 50 | shape: polygon 51 | obstacle0006: 52 | corners: 53 | - [2.075939919854884, -2.554171347971249] 54 | - [2.921262302620707, -3.9665983595418206] 55 | - [4.1595212179023715, -3.5682081815054945] 56 | - [4.6630139347837245, -2.6994690588432264] 57 | - [2.075939919854884, -2.554171347971249] 58 | shape: polygon 59 | obstacle0007: 60 | corners: 61 | - [-1.4925366825679407, -1.0336311871441424] 62 | - [-2.777732343480734, -1.8264399697520508] 63 | - [-1.7411393273338625, -3.8043506990754414] 64 | - [-1.039965127240421, -1.3494655876651405] 65 | - [-1.4925366825679407, -1.0336311871441424] 66 | shape: polygon 67 | obstacle0008: 68 | corners: 69 | - [2.4743363687458073, 0.3164980307492615] 70 | - [2.264355134901594, 1.7826534952664645] 71 | - [0.394770633407417, 1.7611969575014763] 72 | - [0.8423970374700697, -0.2318468225745075] 73 | - [2.4743363687458073, 0.3164980307492615] 74 | shape: polygon 75 | obstacle0009: 76 | corners: 77 | - [-0.5494901854637353, 5.556479436840109] 78 | - [1.0605494548909842, 7.446615177577986] 79 | - [-0.024604830439973968, 8.064829138096806] 80 | - [-1.2884986435628114, 6.219947236562941] 81 | - [-0.5494901854637353, 5.556479436840109] 82 | shape: polygon 83 | -------------------------------------------------------------------------------- /utils.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | Parts of the code in this file are modified and/or original code by Peter Norvig. 5 | 6 | Original Software License Agreement 7 | 8 | Copyright © 1998-2002 by Peter Norvig. 9 | Permission is granted to anyone to use this software, in source or object code form, on any computer system, and to modify, compile, decompile, run, and redistribute it to anyone else, subject to the following restrictions: 10 | 11 | The author makes no warranty of any kind, either expressed or implied, about the suitability of this software for any purpose. 12 | The author accepts no liability of any kind for damages or other consequences of the use of this software, even if they arise from defects in the software. 13 | The origin of this software must not be misrepresented, either by explicit claim or by omission. 14 | Altered versions must be plainly marked as such, and must not be misrepresented as being the original software. Altered versions may be distributed in packages under other licenses (such as the GNU license). 15 | If you find this software useful, it would be nice if you let me (peter@norvig.com) know about it, and nicer still if you send me modifications that you are willing to share. However, you are not required to do so. 16 | 17 | http://www.norvig.com/license.html 18 | """ 19 | 20 | import bisect 21 | 22 | 23 | def test_ok(): 24 | try: 25 | from IPython.display import display_html 26 | display_html("""
27 | Test passed!! 28 |
""", raw=True) 29 | except: 30 | print "test ok!!" 31 | 32 | 33 | def update(x, **entries): 34 | """Update a dict; or an object with slots; according to entries. 35 | >>> update({'a': 1}, a=10, b=20) 36 | {'a': 10, 'b': 20} 37 | >>> update(Struct(a=1), a=10, b=20) 38 | Struct(a=10, b=20) 39 | """ 40 | if isinstance(x, dict): 41 | x.update(entries) 42 | else: 43 | x.__dict__.update(entries) 44 | return x 45 | 46 | def some(predicate, seq): 47 | """If some element x of seq satisfies predicate(x), return predicate(x). 48 | >>> some(callable, [min, 3]) 49 | 1 50 | >>> some(callable, [2, 3]) 51 | 0 52 | """ 53 | for x in seq: 54 | px = predicate(x) 55 | if px: return px 56 | return False 57 | 58 | class Queue: 59 | """Queue is an abstract class/interface. There are three types: 60 | Stack(): A Last In First Out Queue. 61 | FIFOQueue(): A First In First Out Queue. 62 | PriorityQueue(order, f): Queue in sorted order (default min-first). 63 | Each type supports the following methods and functions: 64 | q.append(item) -- add an item to the queue 65 | q.extend(items) -- equivalent to: for item in items: q.append(item) 66 | q.pop() -- return the top item from the queue 67 | len(q) -- number of items in q (also q.__len()) 68 | item in q -- does q contain item? 69 | Note that isinstance(Stack(), Queue) is false, because we implement stacks 70 | as lists. If Python ever gets interfaces, Queue will be an interface.""" 71 | 72 | def __init__(self): 73 | abstract 74 | 75 | def extend(self, items): 76 | for item in items: self.append(item) 77 | 78 | class PriorityQueue(Queue): 79 | """A queue in which the minimum (or maximum) element (as determined by f and 80 | order) is returned first. If order is min, the item with minimum f(x) is 81 | returned first; if order is max, then it is the item with maximum f(x). 82 | Also supports dict-like lookup.""" 83 | def __init__(self, order=min, f=lambda x: x): 84 | update(self, A=[], order=order, f=f) 85 | def append(self, item): 86 | bisect.insort(self.A, (self.f(item), item)) 87 | def __len__(self): 88 | return len(self.A) 89 | def pop(self): 90 | if self.order == min: 91 | return self.A.pop(0)[1] 92 | else: 93 | return self.A.pop()[1] 94 | def __contains__(self, item): 95 | return some(lambda (_, x): x == item, self.A) 96 | def __getitem__(self, key): 97 | for _, item in self.A: 98 | if item == key: 99 | return item 100 | def __delitem__(self, key): 101 | for i, (value, item) in enumerate(self.A): 102 | if item == key: 103 | self.A.pop(i) 104 | return 105 | 106 | --------------------------------------------------------------------------------