From b933c834e0d26233ff480c01999e6b5a90932836 Mon Sep 17 00:00:00 2001 From: Karan Date: Sun, 10 Jun 2018 23:30:20 -0500 Subject: [PATCH 01/12] batch informed trees first commit --- .../batch_informed_rrtstar.py | 51 +++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py new file mode 100644 index 00000000..33746832 --- /dev/null +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -0,0 +1,51 @@ +""" +Batch Informed Trees based path planning + +author: Karan Chawla(@karanchawla) + +Reference: https://arxiv.org/abs/1405.5848 +""" + +import random +import numpy as np +from math import cos, sin, atan2, pi +import copy +import matplotlib.pyplot as plt + +show_animation = True + +class BITStar(): + + def __init__(self, star, goal, + obstacleList, randArea, + expandDis=0.5, goalSampleRate=10, maxIter=200): + + + + + + + def plan(self, animation=True): + + + def expandVertex(self, vertex): + + def prune(self, c): + + def radius(self, q): + + def getNearestSample(self): + + def sample(self, m, cMax): + + def sampleUnitBall(self, m): + + def bestVertexQueueValue(self): + + def bestEdgeQueueValue(self): + + def bestInEdgeQueue(self): + + def bestInVertexQueue(self): + + def updateGraph(self): \ No newline at end of file From a777a3cbed2cb8ba7e4aca86b73c75da38970b22 Mon Sep 17 00:00:00 2001 From: Karan Date: Sun, 10 Jun 2018 23:49:53 -0500 Subject: [PATCH 02/12] batch informed trees first commit --- .../batch_informed_rrtstar.py | 47 ++++++++++++++++--- 1 file changed, 40 insertions(+), 7 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 33746832..ad2a8e64 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -14,30 +14,63 @@ import matplotlib.pyplot as plt show_animation = True +class Node(): + + def __init__(self, x, y): + self.x = x + self.y = y + self.cost = 0.0 + self.parent = None + class BITStar(): - def __init__(self, star, goal, + def __init__(self, start, goal, obstacleList, randArea, - expandDis=0.5, goalSampleRate=10, maxIter=200): - - - - - + expandDis=0.5, goalSampleRate=10, maxIter=200, eta): + self.start = Node(start[0], start[1]) + self.goal = Node(goal[0], goal[1]) + self.minrand = randArea[0] + self.maxrand = randArea[1] + self.expandDis = expandDis + self.goalSampleRate = goalSampleRate + self.maxIter = maxIter + self.obstacleList = obstacleList + self.samples = dict() + self.g_scores = dict() + self.f_scores = dict() + self.r = float('inf') + self.eta = eta # tunable parameter + self.unit_ball_measure = #TODO def plan(self, animation=True): + self.nodeList = [self.start] + + + + + + def expandVertex(self, vertex): def prune(self, c): def radius(self, q): + dim = len(start) #dimensions + space_measure = self.minrand * self.maxrand # volume of the space + + min_radius = self.eta * 2.0 * pow((1.0 + 1.0/dim) * + (space_measure/self.unit_ball_measure), 1.0/dim) + return min_radius * pow(numpy.log(q)/q, 1/dim) + # Return the closest sample def getNearestSample(self): + # Sample free space confined in the radius of ball R def sample(self, m, cMax): + # Sample point in a unit ball def sampleUnitBall(self, m): def bestVertexQueueValue(self): From 5ee656b3c68c701fe81e4414a1f879d976791978 Mon Sep 17 00:00:00 2001 From: Karan Date: Mon, 11 Jun 2018 15:17:28 -0500 Subject: [PATCH 03/12] m sample generation --- .../batch_informed_rrtstar.py | 136 +++++++++++++++++- 1 file changed, 132 insertions(+), 4 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index ad2a8e64..6709584a 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -8,8 +8,9 @@ Reference: https://arxiv.org/abs/1405.5848 import random import numpy as np -from math import cos, sin, atan2, pi import copy +import operator +from math import cos, sin, atan2, pi import matplotlib.pyplot as plt show_animation = True @@ -35,20 +36,52 @@ class BITStar(): self.goalSampleRate = goalSampleRate self.maxIter = maxIter self.obstacleList = obstacleList + self.vertex_queue = [] + self.edge_queue = [] self.samples = dict() self.g_scores = dict() self.f_scores = dict() self.r = float('inf') self.eta = eta # tunable parameter self.unit_ball_measure = #TODO + self.old_vertices = [] def plan(self, animation=True): self.nodeList = [self.start] + plan = None + iterations = 0 + # max length we expect to find in our 'informed' sample space, starts as infinite + cBest = float('inf') + pathLen = float('inf') + solutionSet = set() + path = None + # Computing the sampling space + cMin = math.sqrt(pow(self.start.x - self.goal.x, 2) + + pow(self.start.y - self.goal.y, 2)) + xCenter = np.matrix([[(self.start.x + self.goal.x) / 2.0], + [(self.start.y + self.goal.y) / 2.0], [0]]) + a1 = np.matrix([[(self.goal.x - self.start.x) / cMin], + [(self.goal.y - self.start.y) / cMin], [0]]) + etheta = math.atan2(a1[1], a1[0]) + # first column of idenity matrix transposed + id1_t = np.matrix([1.0, 0.0, 0.0]) + M = np.dot(a1, id1_t) + U, S, Vh = np.linalg.svd(M, 1, 1) + C = np.dot(np.dot(U, np.diag( + [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) + # while (iterations < self.maxIter): + if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0: + print("Batch: ", iterations) + samples = self.informedSample(100, cBest, cMin, xCenter, C) + # prune the tree + if animation: + self.drawGraph(xCenter=xCenter, cBest=cBest, + cMin=cMin, etheta=etheta, rnd=samples) @@ -68,10 +101,44 @@ class BITStar(): def getNearestSample(self): # Sample free space confined in the radius of ball R - def sample(self, m, cMax): + def informedSample(self, m, cMax, cMin, xCenter, C): + if cMax < float('inf'): + samples = [] + for i in range(m): + r = [cMax / 2.0, + math.sqrt(cMax**2 - cMin**2) / 2.0, + math.sqrt(cMax**2 - cMin**2) / 2.0] + L = np.diag(r) + xBall = self.sampleUnitBall() + rnd = np.dot(np.dot(C, L), xBall) + xCenter + rnd = [rnd[(0, 0)], rnd[(1, 0)]] + samples.append(rnd) + else: + for i in range(m): + rnd = self.sampleFreeSpace() + samples.append(rnd) + # Sample point in a unit ball - def sampleUnitBall(self, m): + def sampleUnitBall(self): + a = random.random() + b = random.random() + + if b < a: + a, b = b, a + + sample = (b * math.cos(2 * math.pi * a / b), + b * math.sin(2 * math.pi * a / b)) + return np.array([[sample[0]], [sample[1]], [0]]) + + def sampleFreeSpace(self): + if random.randint(0, 100) > self.goalSampleRate: + rnd = [random.uniform(self.minrand, self.maxrand), + random.uniform(self.minrand, self.maxrand)] + else: + rnd = [self.goal.x, self.goal.y] + + return rnd def bestVertexQueueValue(self): @@ -81,4 +148,65 @@ class BITStar(): def bestInVertexQueue(self): - def updateGraph(self): \ No newline at end of file + def updateGraph(self): + + def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None): + + plt.clf() + for rnd in samples: + if rnd is not None: + plt.plot(rnd[0], rnd[1], "^k") + if cBest != float('inf'): + self.plot_ellipse(xCenter, cBest, cMin, etheta) + + # for node in self.nodeList: + # if node.parent is not None: + # if node.x or node.y is not None: + # plt.plot([node.x, self.nodeList[node.parent].x], [ + # node.y, self.nodeList[node.parent].y], "-g") + + for (ox, oy, size) in self.obstacleList: + plt.plot(ox, oy, "ok", ms=30 * size) + + plt.plot(self.start.x, self.start.y, "xr") + plt.plot(self.goal.x, self.goal.y, "xr") + plt.axis([-2, 15, -2, 15]) + plt.grid(True) + plt.pause(0.01) + + def plot_ellipse(self, xCenter, cBest, cMin, etheta): + + a = math.sqrt(cBest**2 - cMin**2) / 2.0 + b = cBest / 2.0 + angle = math.pi / 2.0 - etheta + cx = xCenter[0] + cy = xCenter[1] + + t = np.arange(0, 2 * math.pi + 0.1, 0.1) + x = [a * math.cos(it) for it in t] + y = [b * math.sin(it) for it in t] + R = np.matrix([[math.cos(angle), math.sin(angle)], + [-math.sin(angle), math.cos(angle)]]) + fx = R * np.matrix([x, y]) + px = np.array(fx[0, :] + cx).flatten() + py = np.array(fx[1, :] + cy).flatten() + plt.plot(cx, cy, "xc") + plt.plot(px, py, "--c") + +def main(): + print("Starting Batch Informed Trees Star planning") + obstacleList = [ + (5, 5, 0.5), + (9, 6, 1), + (7, 5, 1), + (1, 5, 1), + (3, 6, 1), + (7, 9, 1) + ] + + bitStar = BITStar(start=[0, 0], goal=[5, 10], + randArea=[-2, 15], obstacleList=obstacleList) + path = bitStar.plan(animation=show_animation) + + if show_animation: + bitStar.drawGraph() From a21f5af45753c6ae1e8fed84c3d07ee12e0ae7b1 Mon Sep 17 00:00:00 2001 From: Karan Date: Mon, 11 Jun 2018 15:34:56 -0500 Subject: [PATCH 04/12] generating m samples in ellipsoid --- .../batch_informed_rrtstar.py | 97 ++++++++++--------- 1 file changed, 49 insertions(+), 48 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 6709584a..b6977efa 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -10,7 +10,7 @@ import random import numpy as np import copy import operator -from math import cos, sin, atan2, pi +import math import matplotlib.pyplot as plt show_animation = True @@ -26,8 +26,8 @@ class Node(): class BITStar(): def __init__(self, start, goal, - obstacleList, randArea, - expandDis=0.5, goalSampleRate=10, maxIter=200, eta): + obstacleList, randArea, eta=2.0, + expandDis=0.5, goalSampleRate=10, maxIter=200): self.start = Node(start[0], start[1]) self.goal = Node(goal[0], goal[1]) self.minrand = randArea[0] @@ -43,7 +43,7 @@ class BITStar(): self.f_scores = dict() self.r = float('inf') self.eta = eta # tunable parameter - self.unit_ball_measure = #TODO + self.unit_ball_measure = 1 self.old_vertices = [] def plan(self, animation=True): @@ -52,10 +52,10 @@ class BITStar(): plan = None iterations = 0 # max length we expect to find in our 'informed' sample space, starts as infinite - cBest = float('inf') - pathLen = float('inf') - solutionSet = set() - path = None + cBest = float('inf') + pathLen = float('inf') + solutionSet = set() + path = None # Computing the sampling space cMin = math.sqrt(pow(self.start.x - self.goal.x, 2) + @@ -72,22 +72,22 @@ class BITStar(): C = np.dot(np.dot(U, np.diag( [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) - # while (iterations < self.maxIter): + while (iterations < self.maxIter): + if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0: + samples = self.informedSample(100, cBest, cMin, xCenter, C) + # prune the tree - if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0: - print("Batch: ", iterations) - samples = self.informedSample(100, cBest, cMin, xCenter, C) - # prune the tree + if animation: + self.drawGraph(xCenter=xCenter, cBest=cBest, + cMin=cMin, etheta=etheta, samples=samples) - if animation: - self.drawGraph(xCenter=xCenter, cBest=cBest, - cMin=cMin, etheta=etheta, rnd=samples) + iterations += 1 + return plan + # def expandVertex(self, vertex): - def expandVertex(self, vertex): - - def prune(self, c): + # def prune(self, c): def radius(self, q): dim = len(start) #dimensions @@ -98,26 +98,26 @@ class BITStar(): return min_radius * pow(numpy.log(q)/q, 1/dim) # Return the closest sample - def getNearestSample(self): + # def getNearestSample(self): # Sample free space confined in the radius of ball R def informedSample(self, m, cMax, cMin, xCenter, C): - if cMax < float('inf'): - samples = [] - for i in range(m): - r = [cMax / 2.0, - math.sqrt(cMax**2 - cMin**2) / 2.0, - math.sqrt(cMax**2 - cMin**2) / 2.0] - L = np.diag(r) - xBall = self.sampleUnitBall() - rnd = np.dot(np.dot(C, L), xBall) + xCenter - rnd = [rnd[(0, 0)], rnd[(1, 0)]] - samples.append(rnd) - else: - for i in range(m): - rnd = self.sampleFreeSpace() - samples.append(rnd) - + samples = [] + if cMax < float('inf'): + for i in range(m): + r = [cMax / 2.0, + math.sqrt(cMax**2 - cMin**2) / 2.0, + math.sqrt(cMax**2 - cMin**2) / 2.0] + L = np.diag(r) + xBall = self.sampleUnitBall() + rnd = np.dot(np.dot(C, L), xBall) + xCenter + rnd = [rnd[(0, 0)], rnd[(1, 0)]] + samples.append(rnd) + else: + for i in range(m): + rnd = self.sampleFreeSpace() + samples.append(rnd) + return samples # Sample point in a unit ball def sampleUnitBall(self): @@ -131,27 +131,27 @@ class BITStar(): b * math.sin(2 * math.pi * a / b)) return np.array([[sample[0]], [sample[1]], [0]]) - def sampleFreeSpace(self): + def sampleFreeSpace(self): if random.randint(0, 100) > self.goalSampleRate: rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand)] - else: + else: rnd = [self.goal.x, self.goal.y] return rnd - def bestVertexQueueValue(self): + # def bestVertexQueueValue(self): - def bestEdgeQueueValue(self): + # def bestEdgeQueueValue(self): - def bestInEdgeQueue(self): + # def bestInEdgeQueue(self): - def bestInVertexQueue(self): + # def bestInVertexQueue(self): - def updateGraph(self): - - def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None): + # def updateGraph(self): + def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, samples=None): + print("Plotting Graph") plt.clf() for rnd in samples: if rnd is not None: @@ -172,7 +172,7 @@ class BITStar(): plt.plot(self.goal.x, self.goal.y, "xr") plt.axis([-2, 15, -2, 15]) plt.grid(True) - plt.pause(0.01) + plt.pause(5) def plot_ellipse(self, xCenter, cBest, cMin, etheta): @@ -207,6 +207,7 @@ def main(): bitStar = BITStar(start=[0, 0], goal=[5, 10], randArea=[-2, 15], obstacleList=obstacleList) path = bitStar.plan(animation=show_animation) + print("Done") - if show_animation: - bitStar.drawGraph() +if __name__ == '__main__': + main() From 2978ad60b6fee1d4ead00c4a12f9eaa894c275b0 Mon Sep 17 00:00:00 2001 From: Karan Date: Tue, 12 Jun 2018 00:24:52 -0500 Subject: [PATCH 05/12] tree implementation and node id stuff --- .../BatchInformedRRTStar/Untitled.ipynb | 132 ++++++++++++++++++ .../batch_informed_rrtstar.py | 42 ++++++ 2 files changed, 174 insertions(+) create mode 100644 PathPlanning/BatchInformedRRTStar/Untitled.ipynb diff --git a/PathPlanning/BatchInformedRRTStar/Untitled.ipynb b/PathPlanning/BatchInformedRRTStar/Untitled.ipynb new file mode 100644 index 00000000..73c339fd --- /dev/null +++ b/PathPlanning/BatchInformedRRTStar/Untitled.ipynb @@ -0,0 +1,132 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 51, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "def ConfigurationToGridCoord(config):\n", + " \n", + " # TODO:\n", + " # This function maps a configuration in the full configuration space\n", + " # to a grid coordinate in discrete space\n", + " #\n", + " coord = [0] * 2\n", + " lower_lims = [0, 0]\n", + " for i in range(0, len(coord)):\n", + " start = lower_lims[i] # start of the configuration space\n", + " coord[i] = np.around((config[i] - start)/ 1)\n", + " return coord" + ] + }, + { + "cell_type": "code", + "execution_count": 52, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[2.0, 1.0]" + ] + }, + "execution_count": 52, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "coord = ConfigurationToGridCoord([2,1])\n", + "coord" + ] + }, + { + "cell_type": "code", + "execution_count": 158, + "metadata": {}, + "outputs": [], + "source": [ + "def GridCoordToNodeId(coord):\n", + " upper_limits = [4, 3]\n", + " lower_limits = [0, 0]\n", + " num_cells = []\n", + " for idx in range(2):\n", + " num_cells.append(np.ceil((upper_limits[idx] - lower_limits[idx])/1))\n", + " node_id = 0\n", + " print(\"num_cells \", num_cells)\n", + " for i in range(len(coord) - 1, -1, -1):\n", + " # Get product of the grid space maximums\n", + " prod = 1\n", + " for j in range(0, i):\n", + " print(\"j: \", j)\n", + " prod = prod * num_cells[j]\n", + " print(\"prod: \", prod)\n", + " # Add the product to the sum\n", + " node_id = node_id + coord[i] * prod\n", + " print(\"n: \", node_id)\n", + " return int(node_id)" + ] + }, + { + "cell_type": "code", + "execution_count": 159, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "num_cells [4.0, 3.0]\n", + "j: 0\n", + "prod: 4.0\n", + "n: 8.0\n", + "n: 10.0\n" + ] + }, + { + "data": { + "text/plain": [ + "10" + ] + }, + "execution_count": 159, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "GridCoordToNodeId([2, 2])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.6.3" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index b6977efa..d9213db5 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -15,6 +15,43 @@ import matplotlib.pyplot as plt show_animation = True +class Tree(object): + + def __init__(self, start, lowerLimit, upperLimit, resolution): + self.vertices = dict() + vertex_id = self.gridCoordinateToNodeId(start) + self.vertices[vid] = [] + self.edges = [] + self.start = start + self.lowerLimit = lowerLimit + self.upperLimit = upperLimit + for idx in range(len(lowerLimit)): + self.num_cells[idx] = np.ceil((upperLimit[idx] - lowerLimit[idx])/resolution) + + def getRootId(self): + return 0 + + def addVertex(self, vertex): + vertex_id = self.gridCoordinateToNodeId(vertex) + self.vertices[vertex_id] = [] + return vertex_id + + def addEdge(self, v, x): + if (v, x) not in self.edges: + self.edges.append((v,x)) + self.vertices[v].append(x) + self.vertices[x].append(v) + + def gridCoordinateToNodeId(self, coord): + nodeId = 0 + for i in range(len(coord) - 1, -1, -1): + product = 1 + for j in range(0 , i): + product *= product * self.num_cells[j] + node_id = node_id + coord[i] * product + return node_id + + class Node(): def __init__(self, x, y): @@ -72,11 +109,16 @@ class BITStar(): C = np.dot(np.dot(U, np.diag( [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) + foundGoal = False + # run until done while (iterations < self.maxIter): if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0: samples = self.informedSample(100, cBest, cMin, xCenter, C) # prune the tree + + + if animation: self.drawGraph(xCenter=xCenter, cBest=cBest, cMin=cMin, etheta=etheta, samples=samples) From fd8a5fc7284e6c21503105ed1aaf6c56c61fda86 Mon Sep 17 00:00:00 2001 From: Karan Date: Tue, 12 Jun 2018 00:25:23 -0500 Subject: [PATCH 06/12] removed ipython stuff --- .../BatchInformedRRTStar/Untitled.ipynb | 132 ------------------ 1 file changed, 132 deletions(-) delete mode 100644 PathPlanning/BatchInformedRRTStar/Untitled.ipynb diff --git a/PathPlanning/BatchInformedRRTStar/Untitled.ipynb b/PathPlanning/BatchInformedRRTStar/Untitled.ipynb deleted file mode 100644 index 73c339fd..00000000 --- a/PathPlanning/BatchInformedRRTStar/Untitled.ipynb +++ /dev/null @@ -1,132 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": 51, - "metadata": {}, - "outputs": [], - "source": [ - "import numpy as np\n", - "def ConfigurationToGridCoord(config):\n", - " \n", - " # TODO:\n", - " # This function maps a configuration in the full configuration space\n", - " # to a grid coordinate in discrete space\n", - " #\n", - " coord = [0] * 2\n", - " lower_lims = [0, 0]\n", - " for i in range(0, len(coord)):\n", - " start = lower_lims[i] # start of the configuration space\n", - " coord[i] = np.around((config[i] - start)/ 1)\n", - " return coord" - ] - }, - { - "cell_type": "code", - "execution_count": 52, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "[2.0, 1.0]" - ] - }, - "execution_count": 52, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "coord = ConfigurationToGridCoord([2,1])\n", - "coord" - ] - }, - { - "cell_type": "code", - "execution_count": 158, - "metadata": {}, - "outputs": [], - "source": [ - "def GridCoordToNodeId(coord):\n", - " upper_limits = [4, 3]\n", - " lower_limits = [0, 0]\n", - " num_cells = []\n", - " for idx in range(2):\n", - " num_cells.append(np.ceil((upper_limits[idx] - lower_limits[idx])/1))\n", - " node_id = 0\n", - " print(\"num_cells \", num_cells)\n", - " for i in range(len(coord) - 1, -1, -1):\n", - " # Get product of the grid space maximums\n", - " prod = 1\n", - " for j in range(0, i):\n", - " print(\"j: \", j)\n", - " prod = prod * num_cells[j]\n", - " print(\"prod: \", prod)\n", - " # Add the product to the sum\n", - " node_id = node_id + coord[i] * prod\n", - " print(\"n: \", node_id)\n", - " return int(node_id)" - ] - }, - { - "cell_type": "code", - "execution_count": 159, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "num_cells [4.0, 3.0]\n", - "j: 0\n", - "prod: 4.0\n", - "n: 8.0\n", - "n: 10.0\n" - ] - }, - { - "data": { - "text/plain": [ - "10" - ] - }, - "execution_count": 159, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "GridCoordToNodeId([2, 2])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.3" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} From 53268f231740ff4fdf9e8b51c888eaee1d27fff6 Mon Sep 17 00:00:00 2001 From: Karan Date: Tue, 12 Jun 2018 02:20:17 -0500 Subject: [PATCH 07/12] substantial progress with plan method, todo: updateGraph() --- .../batch_informed_rrtstar.py | 253 ++++++++++++++++-- 1 file changed, 224 insertions(+), 29 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index d9213db5..7b871b80 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -25,24 +25,45 @@ class Tree(object): self.start = start self.lowerLimit = lowerLimit self.upperLimit = upperLimit + self.dimension = len(lowerLimit) + + # compute the number of grid cells based on the limits and + # resolution given for idx in range(len(lowerLimit)): self.num_cells[idx] = np.ceil((upperLimit[idx] - lowerLimit[idx])/resolution) def getRootId(self): + # return the id of the root of the tree return 0 def addVertex(self, vertex): + # add a vertex to the tree vertex_id = self.gridCoordinateToNodeId(vertex) self.vertices[vertex_id] = [] return vertex_id def addEdge(self, v, x): + # create an edge between v and x vertices if (v, x) not in self.edges: self.edges.append((v,x)) + # since the tree is undirected self.vertices[v].append(x) self.vertices[x].append(v) + def realCoordsToGridCoord(self, real_coord): + # convert real world coordinates to grid space + # depends on the resolution of the grid + # the output is the same as real world coords if the resolution + # is set to 1 + coord = [0] * self.dimension + for i in xrange(0, len(coord)): + start = self.lower_limits[i] # start of the grid space + coord[i] = np.around((real_coord[i] - start)/ self.resolution) + return coord + def gridCoordinateToNodeId(self, coord): + # This function maps a grid coordinate to a unique + # node id nodeId = 0 for i in range(len(coord) - 1, -1, -1): product = 1 @@ -51,6 +72,39 @@ class Tree(object): node_id = node_id + coord[i] * product return node_id + def realWorldToNodeId(self, real_coord): + # first convert the given coordinates to grid space and then + # convert the grid space coordinates to a unique node id + return self.gridCoordinateToNodeId(self.realCoordsToGridCoord(real_coord)) + + def gridCoordToRealWorldCoord(self, coord): + # This function smaps a grid coordinate in discrete space + # to a configuration in the full configuration space + config = [0] * self.dimension + for i in range(0, len(coord)): + start = self.lower_limits[i] # start of the real world / configuration space + grid_step = self.resolution * coord[i] # step from the coordinate in the grid + half_step = self.resolution / 2 # To get to middle of the grid + config[i] = start + grid_step # + half_step + return config + + def nodeIdToGridCoord(self, node_id): + # This function maps a node id to the associated + # grid coordinate + coord = [0] * len(self.lowerLimit) + for i in range(len(coord) - 1, -1, -1): + # Get the product of the grid space maximums + prod = 1 + for j in range(0, i): + prod = prod * self.num_cells[j] + coord[i] = np.floor(node_id / prod) + node_id = node_id - (coord[i] * prod) + return coord + + def nodeIdToRealWorldCoord(self, nid): + # This function maps a node in discrete space to a configuraiton + # in the full configuration space + return self.gridCoordToRealWorldCoord(self.nodeIdToGridCoord(nid)) class Node(): @@ -65,14 +119,16 @@ class BITStar(): def __init__(self, start, goal, obstacleList, randArea, eta=2.0, expandDis=0.5, goalSampleRate=10, maxIter=200): - self.start = Node(start[0], start[1]) - self.goal = Node(goal[0], goal[1]) + self.start = start + self.goal = goal + self.minrand = randArea[0] self.maxrand = randArea[1] self.expandDis = expandDis self.goalSampleRate = goalSampleRate self.maxIter = maxIter self.obstacleList = obstacleList + self.vertex_queue = [] self.edge_queue = [] self.samples = dict() @@ -84,12 +140,26 @@ class BITStar(): self.old_vertices = [] def plan(self, animation=True): + # initialize tree + self.tree = Tree(self.start,[self.minrand, self.minrand], + [self.maxrand, self.maxrand], 1.0) + + self.startId = self.tree.realWorldToNodeId(self.start) + self.goalId = self.tree.realWorldToNodeId(self.goal) + + # add goal to the samples + self.samples[self.goalId] = self.goal + self.g_scores[self.goalId] = float('inf') + self.f_scores[self.goalId] = 0 + + # add the start id to the tree + self.tree.addVertex(self.start) + self.g_scores[self.startId] = 0 + self.f_scores[self.startId] = self.computeHeuristicCost(self.startId, self.goalId) - self.nodeList = [self.start] - plan = None iterations = 0 # max length we expect to find in our 'informed' sample space, starts as infinite - cBest = float('inf') + cBest = self.g_scores[self.goalId] pathLen = float('inf') solutionSet = set() path = None @@ -113,9 +183,68 @@ class BITStar(): # run until done while (iterations < self.maxIter): if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0: - samples = self.informedSample(100, cBest, cMin, xCenter, C) + # Using informed rrt star way of computing the samples + self.samples.update(self.informedSample(200, cBest, cMin, xCenter, C)) # prune the tree + if iterations != 0: + self.samples.update(self.informedSample(200, cBest, cMin, xCenter, C)) + + # make the old vertices the new vertices + self.old_vertices += self.tree.vertices.keys() + # add the vertices to the vertex queue + for nid in self.tree.vertices.keys(): + if nid not in self.vertex_queue: + self.vertex_queue.append(nid) + # expand the best vertices until an edge is better than the vertex + # this is done because the vertex cost represents the lower bound + # on the edge cost + while(self.bestVertexQueueValue() <= self.bestEdgeQueueValue()): + self.expandVertex(self.bestInVertexQueue()) + + # add the best edge to the tree + bestEdge = self.bestInEdgeQueue() + self.edge_queue.remove(bestEdge) + + # Check if this can improve the current solution + estimatedCostOfVertex = self.g_scores[bestEdge[0]] + + self.computeDistanceCost(bestEdge[0], bestEdge[1]) + + self.computeHeuristicCost(bestEdge[1], self.goalId) + estimatedCostOfEdge = self.computeDistanceCost(self.startId, bestEdge[0]) + + self.computeHeuristicCost(bestEdge[0], bestEdge[1]) + + self.computeHeuristicCost(bestEdge[1], self.goalId) + actualCostOfEdge = self.g_scores[bestEdge[0]] + + self.computeDistanceCost(best_edge[0], best_edge[1]) + + if(estimatedCostOfVertex < self.g_scores[self.goalId]): + if(estimatedCostOfEdge < self.g_scores[self.goalId]): + if(actualCostOfEdge < self.g_scores[self.goalId]): + # connect this edge + firstCoord = self.tree.nodeIdToRealWorldCoord(bestEdge[0]) + secondCoord = self.tree.nodeIdToRealWorldCoord(bestEdge[1]) + path = self.connect(firstCoord, secondCoord) + if path == None or len(path) = 0: + continue + nextCoord = path[len(path) - 1, :] + nextCoordPathId = self.tree.realWorldToNodeId(nextCoord) + bestEdge = (bestEdge[0], nextCoordPathId) + try: + del self.samples[bestEdge[1]] + except(KeyError): + pass + eid = self.tree.addVertex(nextCoordPathId) + self.vertex_queue.append(eid) + if eid == self.goalId or bestEdge[0] == self.goalId or + bestEdge[1] == self.goalId: + print("Goal found") + foundGoal = True + + self.tree.addEdge(bestEdge[0], bestEdge[1]) + + g_score = self.computeDistanceCost(bestEdge[0], bestEdge[1]) + self.g_scores[bestEdge[1]] = g_score + self.g_scores[best_edge[0]] + self.f_scores[bestEdge[1]] = g_score + self.computeHeuristicCost(bestEdge[1], self.goalId) + self.updateGraph() + @@ -131,6 +260,20 @@ class BITStar(): # def prune(self, c): + def computeHeuristicCost(self, start_id, goal_id): + # Using Manhattan distance as heuristic + start = np.array(self.tree.nodeIdToRealWorldCoord(start_id)) + goal = np.array(self.tree.nodeIdToRealWorldCoord(goal_id)) + + return np.linalg.norm(start - goal, 2) + + def computeDistanceCost(self, vid, xid): + # L2 norm distance + start = np.array(self.tree.nodeIdToRealWorldCoord(vid)) + stop = np.array(self.tree.nodeIdToRealWorldCoord(xid)) + + return np.linalg.norm(stop - start, 2) + def radius(self, q): dim = len(start) #dimensions space_measure = self.minrand * self.maxrand # volume of the space @@ -144,21 +287,22 @@ class BITStar(): # Sample free space confined in the radius of ball R def informedSample(self, m, cMax, cMin, xCenter, C): - samples = [] - if cMax < float('inf'): - for i in range(m): - r = [cMax / 2.0, - math.sqrt(cMax**2 - cMin**2) / 2.0, - math.sqrt(cMax**2 - cMin**2) / 2.0] - L = np.diag(r) - xBall = self.sampleUnitBall() - rnd = np.dot(np.dot(C, L), xBall) + xCenter - rnd = [rnd[(0, 0)], rnd[(1, 0)]] - samples.append(rnd) - else: - for i in range(m): - rnd = self.sampleFreeSpace() - samples.append(rnd) + samples = dict() + for i in range(m+1): + if cMax < float('inf'): + r = [cMax / 2.0, + math.sqrt(cMax**2 - cMin**2) / 2.0, + math.sqrt(cMax**2 - cMin**2) / 2.0] + L = np.diag(r) + xBall = self.sampleUnitBall() + rnd = np.dot(np.dot(C, L), xBall) + xCenter + rnd = [rnd[(0, 0)], rnd[(1, 0)]] + random_id = self.tree.realWorldToNodeId(rnd) + samples[random_id] = rnd + else: + rnd = self.sampleFreeSpace() + random_id = self.tree.realWorldToNodeId(rnd) + samples[random_id] = rnd return samples # Sample point in a unit ball @@ -174,21 +318,72 @@ class BITStar(): return np.array([[sample[0]], [sample[1]], [0]]) def sampleFreeSpace(self): - if random.randint(0, 100) > self.goalSampleRate: - rnd = [random.uniform(self.minrand, self.maxrand), + rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand)] - else: - rnd = [self.goal.x, self.goal.y] return rnd - # def bestVertexQueueValue(self): + def bestVertexQueueValue(self): + if(len(self.vertex_queue) == 0): + return float('inf') + values = [self.g_scores[v] + self.computeHeuristicCost(v, self.goalId) for v in self.vertex_queue] + values.sort() + return values[0] - # def bestEdgeQueueValue(self): + def bestEdgeQueueValue(self): + if(len(self.edge_queue)==0): + return float('inf') + # return the best value in the queue by score g_tau[v] + c(v,x) + h(x) + values = [self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1]) + + self.computeHeuristicCost(e[1], self.goalId) for e in self.edge_queue] + values.sort(reverse=True) + return values[0] - # def bestInEdgeQueue(self): + def bestInVertexQueue(self): + # return the best value in the vertex queue + v_plus_vals = [(v, self.g_scores[v] + self.computeHeuristicCost(v, self.goalId)) for v in self.vertex_queue] + v_plus_vals = sorted(v_plus_vals, key=lambda x: x[1]) + + return v_plus_vals[0][0] + + def bestInEdgeQueue(self): + e_and_values = [(e[0], e[1], self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1]) + self.computeHeuristicCost(e[1], self.goalId)) for e in self.edge_queue] + e_and_values = sorted(e_and_values, key=lambda x : x[2]) + + return (e_and_values[0][0], e_and_values[0][1]) + + def expandVertex(self, vid): + self.vertex_queue.remove(vid) + + # get the coordinates for given vid + currCoord = np.array(self.nodeIdToRealWorldCoord(vid)) + + # get the nearest value in vertex for every one in samples where difference is + # less than the radius + neigbors = [] + for sid, scoord in self.samples.items(): + scoord = np.array(scoord) + if(np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid): + neigbors.append((sid, scoord)) + + # add the vertex to the edge queue + if vid not in self.old_vertices: + neigbors = [] + for v, edges in self.tree.vertices.items(): + if v!= vid and (v, vid) not in self.edge_queue: + vcoord = self.tree.nodeIdToRealWorldCoord(v) + if(np.linalg.norm(vcoord - currCoord, 2) <= self.r and v!=vid): + neigbors.append((vid, vcoord)) + + # add an edge to the edge queue is the path might improve the solution + for neighbor in neighbors: + sid = neighbor[0] + estimated_f_score = self.computeDistanceCost(self.startId, vid) + + self.computeHeuristicCost(sif, self.goalId) + + self.computeDistanceCost(vid, sid) + if estimated_f_score < self.g_scores[self.goalId]: + self.edge_queue.append((vid, sid)) - # def bestInVertexQueue(self): # def updateGraph(self): From 9a2fac62d55655e5b6254714d8c96fa8d2542940 Mon Sep 17 00:00:00 2001 From: Karan Date: Tue, 12 Jun 2018 03:37:07 -0500 Subject: [PATCH 08/12] todo: complete connect function and drawGraph --- .../batch_informed_rrtstar.py | 790 ++++++++++-------- 1 file changed, 445 insertions(+), 345 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 7b871b80..84cd33ef 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -7,104 +7,112 @@ Reference: https://arxiv.org/abs/1405.5848 """ import random -import numpy as np -import copy -import operator +import numpy as np +import copy +import operator import math -import matplotlib.pyplot as plt +import matplotlib.pyplot as plt -show_animation = True +show_animation = True -class Tree(object): - def __init__(self, start, lowerLimit, upperLimit, resolution): - self.vertices = dict() - vertex_id = self.gridCoordinateToNodeId(start) - self.vertices[vid] = [] - self.edges = [] - self.start = start - self.lowerLimit = lowerLimit - self.upperLimit = upperLimit - self.dimension = len(lowerLimit) +class RTree(object): - # compute the number of grid cells based on the limits and - # resolution given - for idx in range(len(lowerLimit)): - self.num_cells[idx] = np.ceil((upperLimit[idx] - lowerLimit[idx])/resolution) + def __init__(self, start=[0,0], lowerLimit=[0,0], upperLimit=[10,10], resolution=1): + self.vertices = dict() + + self.edges = [] + self.start = start + self.lowerLimit = lowerLimit + self.upperLimit = upperLimit + self.dimension = len(lowerLimit) + self.num_cells = [0] * self.dimension + self.resolution = resolution + # compute the number of grid cells based on the limits and + # resolution given + for idx in range(self.dimension): + self.num_cells[idx] = np.ceil( + (upperLimit[idx] - lowerLimit[idx]) / resolution) - def getRootId(self): - # return the id of the root of the tree - return 0 + vertex_id = self.realWorldToNodeId(start) + self.vertices[vertex_id] = [] - def addVertex(self, vertex): - # add a vertex to the tree - vertex_id = self.gridCoordinateToNodeId(vertex) - self.vertices[vertex_id] = [] - return vertex_id + def getRootId(self): + # return the id of the root of the tree + return 0 - def addEdge(self, v, x): - # create an edge between v and x vertices - if (v, x) not in self.edges: - self.edges.append((v,x)) - # since the tree is undirected - self.vertices[v].append(x) - self.vertices[x].append(v) + def addVertex(self, vertex): + # add a vertex to the tree + vertex_id = self.realWorldToNodeId(vertex) + self.vertices[vertex_id] = [] + return vertex_id - def realCoordsToGridCoord(self, real_coord): - # convert real world coordinates to grid space - # depends on the resolution of the grid - # the output is the same as real world coords if the resolution - # is set to 1 - coord = [0] * self.dimension - for i in xrange(0, len(coord)): - start = self.lower_limits[i] # start of the grid space - coord[i] = np.around((real_coord[i] - start)/ self.resolution) - return coord + def addEdge(self, v, x): + # create an edge between v and x vertices + if (v, x) not in self.edges: + self.edges.append((v, x)) + # since the tree is undirected + self.vertices[v].append(x) + self.vertices[x].append(v) - def gridCoordinateToNodeId(self, coord): - # This function maps a grid coordinate to a unique - # node id - nodeId = 0 - for i in range(len(coord) - 1, -1, -1): - product = 1 - for j in range(0 , i): - product *= product * self.num_cells[j] - node_id = node_id + coord[i] * product - return node_id + def realCoordsToGridCoord(self, real_coord): + # convert real world coordinates to grid space + # depends on the resolution of the grid + # the output is the same as real world coords if the resolution + # is set to 1 + coord = [0] * self.dimension + for i in range(0, len(coord)): + start = self.lowerLimit[i] # start of the grid space + coord[i] = np.around((real_coord[i] - start) / self.resolution) + return coord - def realWorldToNodeId(self, real_coord): - # first convert the given coordinates to grid space and then - # convert the grid space coordinates to a unique node id - return self.gridCoordinateToNodeId(self.realCoordsToGridCoord(real_coord)) + def gridCoordinateToNodeId(self, coord): + # This function maps a grid coordinate to a unique + # node id + nodeId = 0 + for i in range(len(coord) - 1, -1, -1): + product = 1 + for j in range(0, i): + product = product * self.num_cells[j] + nodeId = nodeId + coord[i] * product + return nodeId - def gridCoordToRealWorldCoord(self, coord): - # This function smaps a grid coordinate in discrete space - # to a configuration in the full configuration space - config = [0] * self.dimension - for i in range(0, len(coord)): - start = self.lower_limits[i] # start of the real world / configuration space - grid_step = self.resolution * coord[i] # step from the coordinate in the grid - half_step = self.resolution / 2 # To get to middle of the grid - config[i] = start + grid_step # + half_step - return config + def realWorldToNodeId(self, real_coord): + # first convert the given coordinates to grid space and then + # convert the grid space coordinates to a unique node id + return self.gridCoordinateToNodeId(self.realCoordsToGridCoord(real_coord)) - def nodeIdToGridCoord(self, node_id): - # This function maps a node id to the associated - # grid coordinate - coord = [0] * len(self.lowerLimit) - for i in range(len(coord) - 1, -1, -1): - # Get the product of the grid space maximums - prod = 1 - for j in range(0, i): - prod = prod * self.num_cells[j] - coord[i] = np.floor(node_id / prod) - node_id = node_id - (coord[i] * prod) - return coord + def gridCoordToRealWorldCoord(self, coord): + # This function smaps a grid coordinate in discrete space + # to a configuration in the full configuration space + config = [0] * self.dimension + for i in range(0, len(coord)): + # start of the real world / configuration space + start = self.lowerLimit[i] + # step from the coordinate in the grid + grid_step = self.resolution * coord[i] + half_step = self.resolution / 2 # To get to middle of the grid + config[i] = start + grid_step # + half_step + return config + + def nodeIdToGridCoord(self, node_id): + # This function maps a node id to the associated + # grid coordinate + coord = [0] * len(self.lowerLimit) + for i in range(len(coord) - 1, -1, -1): + # Get the product of the grid space maximums + prod = 1 + for j in range(0, i): + prod = prod * self.num_cells[j] + coord[i] = np.floor(node_id / prod) + node_id = node_id - (coord[i] * prod) + return coord + + def nodeIdToRealWorldCoord(self, nid): + # This function maps a node in discrete space to a configuraiton + # in the full configuration space + return self.gridCoordToRealWorldCoord(self.nodeIdToGridCoord(nid)) - def nodeIdToRealWorldCoord(self, nid): - # This function maps a node in discrete space to a configuraiton - # in the full configuration space - return self.gridCoordToRealWorldCoord(self.nodeIdToGridCoord(nid)) class Node(): @@ -114,325 +122,416 @@ class Node(): self.cost = 0.0 self.parent = None -class BITStar(): - def __init__(self, start, goal, - obstacleList, randArea, eta=2.0, - expandDis=0.5, goalSampleRate=10, maxIter=200): - self.start = start - self.goal = goal +class BITStar(object): - self.minrand = randArea[0] - self.maxrand = randArea[1] - self.expandDis = expandDis - self.goalSampleRate = goalSampleRate - self.maxIter = maxIter - self.obstacleList = obstacleList + def __init__(self, start, goal, + obstacleList, randArea, eta=2.0, + expandDis=0.5, goalSampleRate=10, maxIter=200): + self.start = start + self.goal = goal - self.vertex_queue = [] - self.edge_queue = [] - self.samples = dict() - self.g_scores = dict() - self.f_scores = dict() - self.r = float('inf') - self.eta = eta # tunable parameter - self.unit_ball_measure = 1 - self.old_vertices = [] + self.minrand = randArea[0] + self.maxrand = randArea[1] + self.expandDis = expandDis + self.goalSampleRate = goalSampleRate + self.maxIter = maxIter + self.obstacleList = obstacleList - def plan(self, animation=True): - # initialize tree - self.tree = Tree(self.start,[self.minrand, self.minrand], - [self.maxrand, self.maxrand], 1.0) + self.vertex_queue = [] + self.edge_queue = [] + self.samples = dict() + self.g_scores = dict() + self.f_scores = dict() + self.nodes = dict() + self.r = float('inf') + self.eta = eta # tunable parameter + self.unit_ball_measure = 1 + self.old_vertices = [] - self.startId = self.tree.realWorldToNodeId(self.start) - self.goalId = self.tree.realWorldToNodeId(self.goal) + # initialize tree + lowerLimit = [randArea[0], randArea[0]] + upperLimit = [randArea[1], randArea[1]] + self.tree = RTree(start=start,lowerLimit=lowerLimit,upperLimit=upperLimit,resolution=0.1) - # add goal to the samples - self.samples[self.goalId] = self.goal - self.g_scores[self.goalId] = float('inf') - self.f_scores[self.goalId] = 0 + def plan(self, animation=True): - # add the start id to the tree - self.tree.addVertex(self.start) - self.g_scores[self.startId] = 0 - self.f_scores[self.startId] = self.computeHeuristicCost(self.startId, self.goalId) + self.startId = self.tree.realWorldToNodeId(self.start) + self.goalId = self.tree.realWorldToNodeId(self.goal) - iterations = 0 - # max length we expect to find in our 'informed' sample space, starts as infinite - cBest = self.g_scores[self.goalId] - pathLen = float('inf') - solutionSet = set() - path = None + # add goal to the samples + self.samples[self.goalId] = self.goal + self.g_scores[self.goalId] = float('inf') + self.f_scores[self.goalId] = 0 - # Computing the sampling space - cMin = math.sqrt(pow(self.start.x - self.goal.x, 2) + - pow(self.start.y - self.goal.y, 2)) - xCenter = np.matrix([[(self.start.x + self.goal.x) / 2.0], - [(self.start.y + self.goal.y) / 2.0], [0]]) - a1 = np.matrix([[(self.goal.x - self.start.x) / cMin], - [(self.goal.y - self.start.y) / cMin], [0]]) - etheta = math.atan2(a1[1], a1[0]) - # first column of idenity matrix transposed - id1_t = np.matrix([1.0, 0.0, 0.0]) - M = np.dot(a1, id1_t) - U, S, Vh = np.linalg.svd(M, 1, 1) - C = np.dot(np.dot(U, np.diag( - [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) + # add the start id to the tree + self.tree.addVertex(self.start) + self.g_scores[self.startId] = 0 + self.f_scores[self.startId] = self.computeHeuristicCost( + self.startId, self.goalId) - foundGoal = False - # run until done - while (iterations < self.maxIter): - if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0: - # Using informed rrt star way of computing the samples - self.samples.update(self.informedSample(200, cBest, cMin, xCenter, C)) - # prune the tree + iterations = 0 + # max length we expect to find in our 'informed' sample space, starts as infinite + cBest = self.g_scores[self.goalId] + pathLen = float('inf') + solutionSet = set() + path = None - if iterations != 0: - self.samples.update(self.informedSample(200, cBest, cMin, xCenter, C)) + # Computing the sampling space + cMin = math.sqrt(pow(self.start[0] - self.goal[1], 2) + + pow(self.start[0] - self.goal[1], 2)) + xCenter = np.matrix([[(self.start[0] + self.goal[0]) / 2.0], + [(self.goal[1] - self.start[1]) / 2.0], [0]]) + a1 = np.matrix([[(self.goal[0]- self.start[0]) / cMin], + [(self.goal[1] - self.start[1]) / cMin], [0]]) + etheta = math.atan2(a1[1], a1[0]) + # first column of idenity matrix transposed + id1_t = np.matrix([1.0, 0.0, 0.0]) + M = np.dot(a1, id1_t) + U, S, Vh = np.linalg.svd(M, 1, 1) + C = np.dot(np.dot(U, np.diag( + [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) - # make the old vertices the new vertices - self.old_vertices += self.tree.vertices.keys() - # add the vertices to the vertex queue - for nid in self.tree.vertices.keys(): - if nid not in self.vertex_queue: - self.vertex_queue.append(nid) - # expand the best vertices until an edge is better than the vertex - # this is done because the vertex cost represents the lower bound - # on the edge cost - while(self.bestVertexQueueValue() <= self.bestEdgeQueueValue()): - self.expandVertex(self.bestInVertexQueue()) + foundGoal = False + # run until done + while (iterations < self.maxIter): + if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0: + # Using informed rrt star way of computing the samples + self.samples.update(self.informedSample( + 200, cBest, cMin, xCenter, C)) + # prune the tree + self.r = 2.0 + if iterations != 0: + self.samples.update(self.informedSample( + 200, cBest, cMin, xCenter, C)) - # add the best edge to the tree - bestEdge = self.bestInEdgeQueue() - self.edge_queue.remove(bestEdge) + # make the old vertices the new vertices + self.old_vertices += self.tree.vertices.keys() + # add the vertices to the vertex queue + for nid in self.tree.vertices.keys(): + if nid not in self.vertex_queue: + self.vertex_queue.append(nid) + # expand the best vertices until an edge is better than the vertex + # this is done because the vertex cost represents the lower bound + # on the edge cost + while(self.bestVertexQueueValue() <= self.bestEdgeQueueValue()): + self.expandVertex(self.bestInVertexQueue()) - # Check if this can improve the current solution - estimatedCostOfVertex = self.g_scores[bestEdge[0]] + - self.computeDistanceCost(bestEdge[0], bestEdge[1]) + - self.computeHeuristicCost(bestEdge[1], self.goalId) - estimatedCostOfEdge = self.computeDistanceCost(self.startId, bestEdge[0]) + - self.computeHeuristicCost(bestEdge[0], bestEdge[1]) + - self.computeHeuristicCost(bestEdge[1], self.goalId) - actualCostOfEdge = self.g_scores[bestEdge[0]] + + self.computeDistanceCost(best_edge[0], best_edge[1]) + # add the best edge to the tree + bestEdge = self.bestInEdgeQueue() + self.edge_queue.remove(bestEdge) - if(estimatedCostOfVertex < self.g_scores[self.goalId]): - if(estimatedCostOfEdge < self.g_scores[self.goalId]): - if(actualCostOfEdge < self.g_scores[self.goalId]): - # connect this edge - firstCoord = self.tree.nodeIdToRealWorldCoord(bestEdge[0]) - secondCoord = self.tree.nodeIdToRealWorldCoord(bestEdge[1]) - path = self.connect(firstCoord, secondCoord) - if path == None or len(path) = 0: - continue - nextCoord = path[len(path) - 1, :] - nextCoordPathId = self.tree.realWorldToNodeId(nextCoord) - bestEdge = (bestEdge[0], nextCoordPathId) - try: - del self.samples[bestEdge[1]] - except(KeyError): - pass - eid = self.tree.addVertex(nextCoordPathId) - self.vertex_queue.append(eid) - if eid == self.goalId or bestEdge[0] == self.goalId or - bestEdge[1] == self.goalId: - print("Goal found") - foundGoal = True + # Check if this can improve the current solution + estimatedCostOfVertex = self.g_scores[bestEdge[0]] + self.computeDistanceCost( + bestEdge[0], bestEdge[1]) + self.computeHeuristicCost(bestEdge[1], self.goalId) + estimatedCostOfEdge = self.computeDistanceCost(self.startId, bestEdge[0]) + self.computeHeuristicCost( + bestEdge[0], bestEdge[1]) + self.computeHeuristicCost(bestEdge[1], self.goalId) + actualCostOfEdge = self.g_scores[bestEdge[0]] + + \ + self.computeDistanceCost(bestEdge[0], bestEdge[1]) - self.tree.addEdge(bestEdge[0], bestEdge[1]) + if(estimatedCostOfVertex < self.g_scores[self.goalId]): + if(estimatedCostOfEdge < self.g_scores[self.goalId]): + if(actualCostOfEdge < self.g_scores[self.goalId]): + # connect this edge + firstCoord = self.tree.nodeIdToRealWorldCoord( + bestEdge[0]) + secondCoord = self.tree.nodeIdToRealWorldCoord( + bestEdge[1]) + path = self.connect(firstCoord, secondCoord) + if path == None or len(path) == 0: + continue + nextCoord = path[len(path) - 1, :] + nextCoordPathId = self.tree.realWorldToNodeId( + nextCoord) + bestEdge = (bestEdge[0], nextCoordPathId) + try: + del self.samples[bestEdge[1]] + except(KeyError): + pass + eid = self.tree.addVertex(nextCoordPathId) + self.vertex_queue.append(eid) + if eid == self.goalId or bestEdge[0] == self.goalId or bestEdge[1] == self.goalId: + print("Goal found") + foundGoal = True - g_score = self.computeDistanceCost(bestEdge[0], bestEdge[1]) - self.g_scores[bestEdge[1]] = g_score + self.g_scores[best_edge[0]] - self.f_scores[bestEdge[1]] = g_score + self.computeHeuristicCost(bestEdge[1], self.goalId) - self.updateGraph() + self.tree.addEdge(bestEdge[0], bestEdge[1]) + g_score = self.computeDistanceCost( + bestEdge[0], bestEdge[1]) + self.g_scores[bestEdge[1]] = g_score + \ + self.g_scores[best_edge[0]] + self.f_scores[bestEdge[1]] = g_score + \ + self.computeHeuristicCost(bestEdge[1], self.goalId) + self.updateGraph() + # visualize new edge + # if animation: + # self.drawGraph(xCenter=xCenter, cBest=cBest, + # cMin=cMin, etheta=etheta, samples=samples) + for edge in self.edge_queue: + if(edge[0] == bestEdge[1]): + if self.g_scores[edge[0]] + self.computeDistanceCost(edge[0], bestEdge[1]) >= self.g_scores[self.goalId]: + if(edge[0], best_edge[1]) in self.edge_queue: + self.edge_queue.remove( + (edge[0], bestEdge[1])) + if(edge[1] == bestEdge[1]): + if self.g_scores[edge[1]] + self.computeDistanceCost(edge[1], bestEdge[1]) >= self.g_scores[self.goalId]: + if(edge[1], best_edge[1]) in self.edge_queue: + self.edge_queue.remove( + (edge[1], bestEdge[1])) + else: + self.edge_queue = [] + self.vertex_queue = [] + iterations += 1 - if animation: - self.drawGraph(xCenter=xCenter, cBest=cBest, - cMin=cMin, etheta=etheta, samples=samples) + plan.append(self.goal) + currId = self.goalId + while (currId != self.startId): + plan.append(seld.tree.nodeIdToRealWorldCoord(currId)) + currId = self.nodes[currId] - iterations += 1 + plan.append(self.startId) + plan = plan[::-1] # reverse the plan + return np.array(plan) - return plan + # def expandVertex(self, vertex): - # def expandVertex(self, vertex): + # def prune(self, c): - # def prune(self, c): - - def computeHeuristicCost(self, start_id, goal_id): - # Using Manhattan distance as heuristic - start = np.array(self.tree.nodeIdToRealWorldCoord(start_id)) + def computeHeuristicCost(self, start_id, goal_id): + # Using Manhattan distance as heuristic + start = np.array(self.tree.nodeIdToRealWorldCoord(start_id)) goal = np.array(self.tree.nodeIdToRealWorldCoord(goal_id)) return np.linalg.norm(start - goal, 2) def computeDistanceCost(self, vid, xid): - # L2 norm distance - start = np.array(self.tree.nodeIdToRealWorldCoord(vid)) - stop = np.array(self.tree.nodeIdToRealWorldCoord(xid)) + # L2 norm distance + start = np.array(self.tree.nodeIdToRealWorldCoord(vid)) + stop = np.array(self.tree.nodeIdToRealWorldCoord(xid)) - return np.linalg.norm(stop - start, 2) + return np.linalg.norm(stop - start, 2) - def radius(self, q): - dim = len(start) #dimensions - space_measure = self.minrand * self.maxrand # volume of the space - - min_radius = self.eta * 2.0 * pow((1.0 + 1.0/dim) * - (space_measure/self.unit_ball_measure), 1.0/dim) - return min_radius * pow(numpy.log(q)/q, 1/dim) + def radius(self, q): + dim = len(start) # dimensions + space_measure = self.minrand * self.maxrand # volume of the space - # Return the closest sample - # def getNearestSample(self): + min_radius = self.eta * 2.0 * pow((1.0 + 1.0 / dim) * + (space_measure / self.unit_ball_measure), 1.0 / dim) + return min_radius * pow(numpy.log(q) / q, 1 / dim) - # Sample free space confined in the radius of ball R - def informedSample(self, m, cMax, cMin, xCenter, C): - samples = dict() - for i in range(m+1): - if cMax < float('inf'): - r = [cMax / 2.0, - math.sqrt(cMax**2 - cMin**2) / 2.0, - math.sqrt(cMax**2 - cMin**2) / 2.0] - L = np.diag(r) - xBall = self.sampleUnitBall() - rnd = np.dot(np.dot(C, L), xBall) + xCenter - rnd = [rnd[(0, 0)], rnd[(1, 0)]] - random_id = self.tree.realWorldToNodeId(rnd) - samples[random_id] = rnd - else: - rnd = self.sampleFreeSpace() - random_id = self.tree.realWorldToNodeId(rnd) - samples[random_id] = rnd - return samples + # Return the closest sample + # def getNearestSample(self): - # Sample point in a unit ball - def sampleUnitBall(self): - a = random.random() - b = random.random() + # Sample free space confined in the radius of ball R + def informedSample(self, m, cMax, cMin, xCenter, C): + samples = dict() + for i in range(m + 1): + if cMax < float('inf'): + r = [cMax / 2.0, + math.sqrt(cMax**2 - cMin**2) / 2.0, + math.sqrt(cMax**2 - cMin**2) / 2.0] + L = np.diag(r) + xBall = self.sampleUnitBall() + rnd = np.dot(np.dot(C, L), xBall) + xCenter + rnd = [rnd[(0, 0)], rnd[(1, 0)]] + random_id = self.tree.realWorldToNodeId(rnd) + samples[random_id] = rnd + else: + rnd = self.sampleFreeSpace() + random_id = self.tree.realWorldToNodeId(rnd) + samples[random_id] = rnd + return samples - if b < a: - a, b = b, a + # Sample point in a unit ball + def sampleUnitBall(self): + a = random.random() + b = random.random() - sample = (b * math.cos(2 * math.pi * a / b), - b * math.sin(2 * math.pi * a / b)) - return np.array([[sample[0]], [sample[1]], [0]]) + if b < a: + a, b = b, a - def sampleFreeSpace(self): - rnd = [random.uniform(self.minrand, self.maxrand), - random.uniform(self.minrand, self.maxrand)] + sample = (b * math.cos(2 * math.pi * a / b), + b * math.sin(2 * math.pi * a / b)) + return np.array([[sample[0]], [sample[1]], [0]]) - return rnd + def sampleFreeSpace(self): + rnd = [random.uniform(self.minrand, self.maxrand), + random.uniform(self.minrand, self.maxrand)] - def bestVertexQueueValue(self): - if(len(self.vertex_queue) == 0): - return float('inf') - values = [self.g_scores[v] + self.computeHeuristicCost(v, self.goalId) for v in self.vertex_queue] - values.sort() - return values[0] + return rnd - def bestEdgeQueueValue(self): - if(len(self.edge_queue)==0): - return float('inf') - # return the best value in the queue by score g_tau[v] + c(v,x) + h(x) - values = [self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1]) + - self.computeHeuristicCost(e[1], self.goalId) for e in self.edge_queue] - values.sort(reverse=True) - return values[0] + def bestVertexQueueValue(self): + if(len(self.vertex_queue) == 0): + return float('inf') + values = [self.g_scores[v] + + self.computeHeuristicCost(v, self.goalId) for v in self.vertex_queue] + values.sort() + return values[0] - def bestInVertexQueue(self): - # return the best value in the vertex queue - v_plus_vals = [(v, self.g_scores[v] + self.computeHeuristicCost(v, self.goalId)) for v in self.vertex_queue] - v_plus_vals = sorted(v_plus_vals, key=lambda x: x[1]) + def bestEdgeQueueValue(self): + if(len(self.edge_queue) == 0): + return float('inf') + # return the best value in the queue by score g_tau[v] + c(v,x) + h(x) + values = [self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1]) + + self.computeHeuristicCost(e[1], self.goalId) for e in self.edge_queue] + values.sort(reverse=True) + return values[0] - return v_plus_vals[0][0] + def bestInVertexQueue(self): + # return the best value in the vertex queue + v_plus_vals = [(v, self.g_scores[v] + self.computeHeuristicCost(v, self.goalId)) + for v in self.vertex_queue] + v_plus_vals = sorted(v_plus_vals, key=lambda x: x[1]) - def bestInEdgeQueue(self): - e_and_values = [(e[0], e[1], self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1]) + self.computeHeuristicCost(e[1], self.goalId)) for e in self.edge_queue] - e_and_values = sorted(e_and_values, key=lambda x : x[2]) + return v_plus_vals[0][0] - return (e_and_values[0][0], e_and_values[0][1]) + def bestInEdgeQueue(self): + e_and_values = [(e[0], e[1], self.g_scores[e[0]] + self.computeDistanceCost( + e[0], e[1]) + self.computeHeuristicCost(e[1], self.goalId)) for e in self.edge_queue] + e_and_values = sorted(e_and_values, key=lambda x: x[2]) - def expandVertex(self, vid): - self.vertex_queue.remove(vid) + return (e_and_values[0][0], e_and_values[0][1]) - # get the coordinates for given vid - currCoord = np.array(self.nodeIdToRealWorldCoord(vid)) + def expandVertex(self, vid): + self.vertex_queue.remove(vid) - # get the nearest value in vertex for every one in samples where difference is - # less than the radius - neigbors = [] - for sid, scoord in self.samples.items(): - scoord = np.array(scoord) - if(np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid): - neigbors.append((sid, scoord)) + # get the coordinates for given vid + currCoord = np.array(self.tree.nodeIdToRealWorldCoord(vid)) - # add the vertex to the edge queue - if vid not in self.old_vertices: - neigbors = [] - for v, edges in self.tree.vertices.items(): - if v!= vid and (v, vid) not in self.edge_queue: - vcoord = self.tree.nodeIdToRealWorldCoord(v) - if(np.linalg.norm(vcoord - currCoord, 2) <= self.r and v!=vid): - neigbors.append((vid, vcoord)) + # get the nearest value in vertex for every one in samples where difference is + # less than the radius + neigbors = [] + for sid, scoord in self.samples.items(): + scoord = np.array(scoord) + if(np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid): + neigbors.append((sid, scoord)) - # add an edge to the edge queue is the path might improve the solution - for neighbor in neighbors: - sid = neighbor[0] - estimated_f_score = self.computeDistanceCost(self.startId, vid) + - self.computeHeuristicCost(sif, self.goalId) + - self.computeDistanceCost(vid, sid) - if estimated_f_score < self.g_scores[self.goalId]: - self.edge_queue.append((vid, sid)) + # add the vertex to the edge queue + if vid not in self.old_vertices: + neigbors = [] + for v, edges in self.tree.vertices.items(): + if v != vid and (v, vid) not in self.edge_queue: + vcoord = self.tree.nodeIdToRealWorldCoord(v) + if(np.linalg.norm(vcoord - currCoord, 2) <= self.r): + neigbors.append((vid, vcoord)) + # add an edge to the edge queue is the path might improve the solution + for neighbor in neigbors: + sid = neighbor[0] + estimated_f_score = self.computeDistanceCost( + self.startId, vid) + self.computeHeuristicCost(sid, self.goalId) + self.computeDistanceCost(vid, sid) + if estimated_f_score < self.g_scores[self.goalId]: + self.edge_queue.append((vid, sid)) - # def updateGraph(self): + def updateGraph(self): + closedSet = [] + openSet = [] + currId = self.startId + openSet.append(currId) - def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, samples=None): - print("Plotting Graph") - plt.clf() - for rnd in samples: - if rnd is not None: - plt.plot(rnd[0], rnd[1], "^k") - if cBest != float('inf'): - self.plot_ellipse(xCenter, cBest, cMin, etheta) + # do some plotting - # for node in self.nodeList: - # if node.parent is not None: - # if node.x or node.y is not None: - # plt.plot([node.x, self.nodeList[node.parent].x], [ - # node.y, self.nodeList[node.parent].y], "-g") + foundGoal = False - for (ox, oy, size) in self.obstacleList: - plt.plot(ox, oy, "ok", ms=30 * size) + while len(openSet) != 0: + # get the element with lowest f_score + minn = float('inf') + min_node = None + min_idx = 0 + for i in range(0, len(openSet)): + try: + f_score = self.f_scores[openSet[i]] + except: + pass + if f_score < minn: + minn = f_score + min_node = openSet[i] + min_idx = i + currId = min_node - plt.plot(self.start.x, self.start.y, "xr") - plt.plot(self.goal.x, self.goal.y, "xr") - plt.axis([-2, 15, -2, 15]) - plt.grid(True) - plt.pause(5) + openSet.pop(min_idx) - def plot_ellipse(self, xCenter, cBest, cMin, etheta): + # Check if we're at the goal + if(currId == self.goalId): + foundGoal = True + break - a = math.sqrt(cBest**2 - cMin**2) / 2.0 - b = cBest / 2.0 - angle = math.pi / 2.0 - etheta - cx = xCenter[0] - cy = xCenter[1] + if(currId not in closedSet): + closedSet.append(currId) - t = np.arange(0, 2 * math.pi + 0.1, 0.1) - x = [a * math.cos(it) for it in t] - y = [b * math.sin(it) for it in t] - R = np.matrix([[math.cos(angle), math.sin(angle)], + # find a non visited successor to the current node + successors = self.tree.vertices[currId] + for succesor in successors: + if(succesor in closedSet): + continue + else: + # claculate tentative g score + succesorCoord = self.tree.nodeIdToRealWorldCoord(succesor) + g_score = self.g_scores[currId] + \ + self.computeDistanceCost(currId, succesor) + if succesor not in openSet: + # add the successor to open set + openSet.append(succesor) + elif g_score >= self.g_scores[succesor]: + continue + + # update g and f scores + self.g_scores[succesor] = g_score + self.f_scores[succesor] = f_score + \ + self.computeHeuristicCost(succesor, self.goalId) + + # store the parent and child + self.nodes[succesor] = currId + + def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, samples=None): + print("Plotting Graph") + plt.clf() + for rnd in samples: + if rnd is not None: + plt.plot(rnd[0], rnd[1], "^k") + if cBest != float('inf'): + self.plot_ellipse(xCenter, cBest, cMin, etheta) + + # for node in self.nodeList: + # if node.parent is not None: + # if node.x or node.y is not None: + # plt.plot([node.x, self.nodeList[node.parent].x], [ + # node.y, self.nodeList[node.parent].y], "-g") + + for (ox, oy, size) in self.obstacleList: + plt.plot(ox, oy, "ok", ms=30 * size) + + plt.plot(self.start.x, self.start.y, "xr") + plt.plot(self.goal.x, self.goal.y, "xr") + plt.axis([-2, 15, -2, 15]) + plt.grid(True) + plt.pause(5) + + def plot_ellipse(self, xCenter, cBest, cMin, etheta): + + a = math.sqrt(cBest**2 - cMin**2) / 2.0 + b = cBest / 2.0 + angle = math.pi / 2.0 - etheta + cx = xCenter[0] + cy = xCenter[1] + + t = np.arange(0, 2 * math.pi + 0.1, 0.1) + x = [a * math.cos(it) for it in t] + y = [b * math.sin(it) for it in t] + R = np.matrix([[math.cos(angle), math.sin(angle)], [-math.sin(angle), math.cos(angle)]]) - fx = R * np.matrix([x, y]) - px = np.array(fx[0, :] + cx).flatten() - py = np.array(fx[1, :] + cy).flatten() - plt.plot(cx, cy, "xc") - plt.plot(px, py, "--c") + fx = R * np.matrix([x, y]) + px = np.array(fx[0, :] + cx).flatten() + py = np.array(fx[1, :] + cy).flatten() + plt.plot(cx, cy, "xc") + plt.plot(px, py, "--c") + def main(): - print("Starting Batch Informed Trees Star planning") - obstacleList = [ + print("Starting Batch Informed Trees Star planning") + obstacleList = [ (5, 5, 0.5), (9, 6, 1), (7, 5, 1), @@ -441,10 +540,11 @@ def main(): (7, 9, 1) ] - bitStar = BITStar(start=[0, 0], goal=[5, 10], - randArea=[-2, 15], obstacleList=obstacleList) - path = bitStar.plan(animation=show_animation) - print("Done") + bitStar = BITStar(start=[0, 0], goal=[5, 10], obstacleList=obstacleList, + randArea=[0, 15]) + path = bitStar.plan(animation=show_animation) + print("Done") + if __name__ == '__main__': main() From c1b9710457797a89a800decda3a8c16e11077104 Mon Sep 17 00:00:00 2001 From: Karan Date: Tue, 12 Jun 2018 13:12:20 -0500 Subject: [PATCH 09/12] debuggin through the final implementation --- .../batch_informed_rrtstar.py | 87 ++++++++++++------- 1 file changed, 58 insertions(+), 29 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 84cd33ef..df317dca 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -13,14 +13,14 @@ import operator import math import matplotlib.pyplot as plt -show_animation = True +show_animation = False class RTree(object): - def __init__(self, start=[0,0], lowerLimit=[0,0], upperLimit=[10,10], resolution=1): + def __init__(self, start=[0, 0], lowerLimit=[0, 0], upperLimit=[10, 10], resolution=1): + self.vertices = dict() - self.edges = [] self.start = start self.lowerLimit = lowerLimit @@ -61,7 +61,7 @@ class RTree(object): # the output is the same as real world coords if the resolution # is set to 1 coord = [0] * self.dimension - for i in range(0, len(coord)): + for i in range(len(coord)): start = self.lowerLimit[i] # start of the grid space coord[i] = np.around((real_coord[i] - start) / self.resolution) return coord @@ -131,8 +131,8 @@ class BITStar(object): self.start = start self.goal = goal - self.minrand = randArea[0] - self.maxrand = randArea[1] + self.minrand = randArea[0] + self.maxrand = randArea[1] self.expandDis = expandDis self.goalSampleRate = goalSampleRate self.maxIter = maxIter @@ -152,7 +152,8 @@ class BITStar(object): # initialize tree lowerLimit = [randArea[0], randArea[0]] upperLimit = [randArea[1], randArea[1]] - self.tree = RTree(start=start,lowerLimit=lowerLimit,upperLimit=upperLimit,resolution=0.1) + self.tree = RTree(start=start, lowerLimit=lowerLimit, + upperLimit=upperLimit, resolution=0.1) def plan(self, animation=True): @@ -175,14 +176,14 @@ class BITStar(object): cBest = self.g_scores[self.goalId] pathLen = float('inf') solutionSet = set() - path = None + plan = [] # Computing the sampling space cMin = math.sqrt(pow(self.start[0] - self.goal[1], 2) + pow(self.start[0] - self.goal[1], 2)) xCenter = np.matrix([[(self.start[0] + self.goal[0]) / 2.0], [(self.goal[1] - self.start[1]) / 2.0], [0]]) - a1 = np.matrix([[(self.goal[0]- self.start[0]) / cMin], + a1 = np.matrix([[(self.goal[0] - self.start[0]) / cMin], [(self.goal[1] - self.start[1]) / cMin], [0]]) etheta = math.atan2(a1[1], a1[0]) # first column of idenity matrix transposed @@ -238,7 +239,7 @@ class BITStar(object): secondCoord = self.tree.nodeIdToRealWorldCoord( bestEdge[1]) path = self.connect(firstCoord, secondCoord) - if path == None or len(path) == 0: + if path is None or len(path) == 0: continue nextCoord = path[len(path) - 1, :] nextCoordPathId = self.tree.realWorldToNodeId( @@ -248,7 +249,7 @@ class BITStar(object): del self.samples[bestEdge[1]] except(KeyError): pass - eid = self.tree.addVertex(nextCoordPathId) + eid = self.tree.addVertex(nextCoord) self.vertex_queue.append(eid) if eid == self.goalId or bestEdge[0] == self.goalId or bestEdge[1] == self.goalId: print("Goal found") @@ -259,15 +260,17 @@ class BITStar(object): g_score = self.computeDistanceCost( bestEdge[0], bestEdge[1]) self.g_scores[bestEdge[1]] = g_score + \ - self.g_scores[best_edge[0]] + self.g_scores[bestEdge[0]] self.f_scores[bestEdge[1]] = g_score + \ self.computeHeuristicCost(bestEdge[1], self.goalId) self.updateGraph() # visualize new edge - # if animation: - # self.drawGraph(xCenter=xCenter, cBest=cBest, - # cMin=cMin, etheta=etheta, samples=samples) + + if animation: + self.drawGraph(xCenter=xCenter, cBest=cBest, + cMin=cMin, etheta=etheta, samples=self.samples.values(), + start=firstCoord, end=secondCoord, plan=plan) for edge in self.edge_queue: if(edge[0] == bestEdge[1]): @@ -288,14 +291,36 @@ class BITStar(object): plan.append(self.goal) currId = self.goalId while (currId != self.startId): - plan.append(seld.tree.nodeIdToRealWorldCoord(currId)) + plan.append(self.tree.nodeIdToRealWorldCoord(currId)) currId = self.nodes[currId] plan.append(self.startId) plan = plan[::-1] # reverse the plan return np.array(plan) - # def expandVertex(self, vertex): + def connect(self, start, end): + # A function which attempts to extend from a start coordinates + # to goal coordinates + steps = self.computeDistanceCost(self.tree.realWorldToNodeId( + start), self.tree.realWorldToNodeId(end)) * 25 + x = np.linspace(start[0], end[0], num=steps) + y = np.linspace(start[1], end[1], num=steps) + for i in range(len(x)): + if(self._collisionCheck(x[i], y[i])): + if(i == 0): + return None + # if collision, send path until collision + return np.vstack((x[0:i], y[0:i])).transpose() + return np.vstack((x, y)).transpose() + + def _collisionCheck(self, x, y): + for (ox, oy, size) in self.obstacleList: + dx = ox - x + dy = oy - y + d = dx * dx + dy * dy + if d <= 1.1 * size ** 2: + return True # collision + return False # def prune(self, c): @@ -485,7 +510,8 @@ class BITStar(object): # store the parent and child self.nodes[succesor] = currId - def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, samples=None): + def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, + samples=None, start=None, end=None, plan=None): print("Plotting Graph") plt.clf() for rnd in samples: @@ -494,6 +520,9 @@ class BITStar(object): if cBest != float('inf'): self.plot_ellipse(xCenter, cBest, cMin, etheta) + if start is not None and end is not None: + plt.plot([start[0], start[1]], [end[0], end[1]], "-g") + # for node in self.nodeList: # if node.parent is not None: # if node.x or node.y is not None: @@ -503,11 +532,11 @@ class BITStar(object): for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, "ok", ms=30 * size) - plt.plot(self.start.x, self.start.y, "xr") - plt.plot(self.goal.x, self.goal.y, "xr") + plt.plot(self.start[0], self.start[1], "xr") + plt.plot(self.goal[0], self.goal[1], "xr") plt.axis([-2, 15, -2, 15]) plt.grid(True) - plt.pause(5) + plt.pause(0.01) def plot_ellipse(self, xCenter, cBest, cMin, etheta): @@ -532,16 +561,16 @@ class BITStar(object): def main(): print("Starting Batch Informed Trees Star planning") obstacleList = [ - (5, 5, 0.5), - (9, 6, 1), - (7, 5, 1), - (1, 5, 1), - (3, 6, 1), - (7, 9, 1) + # (5, 5, 0.5), + # (9, 6, 1), + # (7, 5, 1), + # (1, 5, 1), + # (3, 6, 1), + # (7, 9, 1) ] - bitStar = BITStar(start=[0, 0], goal=[5, 10], obstacleList=obstacleList, - randArea=[0, 15]) + bitStar = BITStar(start=[0, 0], goal=[2, 4], obstacleList=obstacleList, + randArea=[-2, 15]) path = bitStar.plan(animation=show_animation) print("Done") From 3f0d2b3ed701399b081be7988cef10bcdc99efc9 Mon Sep 17 00:00:00 2001 From: Karan Date: Wed, 13 Jun 2018 12:08:34 -0500 Subject: [PATCH 10/12] plotting and some debugging --- .../batch_informed_rrtstar.py | 41 ++++++++++--------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index df317dca..786ff9cb 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -13,7 +13,7 @@ import operator import math import matplotlib.pyplot as plt -show_animation = False +show_animation = True class RTree(object): @@ -131,8 +131,8 @@ class BITStar(object): self.start = start self.goal = goal - self.minrand = randArea[0] - self.maxrand = randArea[1] + self.minrand = randArea[0] + self.maxrand = randArea[1] self.expandDis = expandDis self.goalSampleRate = goalSampleRate self.maxIter = maxIter @@ -158,7 +158,9 @@ class BITStar(object): def plan(self, animation=True): self.startId = self.tree.realWorldToNodeId(self.start) + print("startId: ", self.startId) self.goalId = self.tree.realWorldToNodeId(self.goal) + print("goalId: ", self.goalId) # add goal to the samples self.samples[self.goalId] = self.goal @@ -193,13 +195,15 @@ class BITStar(object): C = np.dot(np.dot(U, np.diag( [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) + self.samples.update(self.informedSample( + 200, cBest, cMin, xCenter, C)) foundGoal = False # run until done while (iterations < self.maxIter): if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0: # Using informed rrt star way of computing the samples self.samples.update(self.informedSample( - 200, cBest, cMin, xCenter, C)) + 50, cBest, cMin, xCenter, C)) # prune the tree self.r = 2.0 if iterations != 0: @@ -266,11 +270,10 @@ class BITStar(object): self.updateGraph() # visualize new edge - if animation: - self.drawGraph(xCenter=xCenter, cBest=cBest, - cMin=cMin, etheta=etheta, samples=self.samples.values(), - start=firstCoord, end=secondCoord, plan=plan) + self.drawGraph(xCenter=xCenter, cBest=cBest, + cMin=cMin, etheta=etheta, samples=self.samples.values(), + start=firstCoord, end=secondCoord, tree=self.nodes) for edge in self.edge_queue: if(edge[0] == bestEdge[1]): @@ -280,7 +283,7 @@ class BITStar(object): (edge[0], bestEdge[1])) if(edge[1] == bestEdge[1]): if self.g_scores[edge[1]] + self.computeDistanceCost(edge[1], bestEdge[1]) >= self.g_scores[self.goalId]: - if(edge[1], best_edge[1]) in self.edge_queue: + if(edge[1], bestEdge[1]) in self.edge_queue: self.edge_queue.remove( (edge[1], bestEdge[1])) else: @@ -409,7 +412,7 @@ class BITStar(object): v_plus_vals = [(v, self.g_scores[v] + self.computeHeuristicCost(v, self.goalId)) for v in self.vertex_queue] v_plus_vals = sorted(v_plus_vals, key=lambda x: x[1]) - + # print(v_plus_vals) return v_plus_vals[0][0] def bestInEdgeQueue(self): @@ -510,8 +513,8 @@ class BITStar(object): # store the parent and child self.nodes[succesor] = currId - def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, - samples=None, start=None, end=None, plan=None): + def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, + samples=None, start=None, end=None, tree=None): print("Plotting Graph") plt.clf() for rnd in samples: @@ -521,13 +524,13 @@ class BITStar(object): self.plot_ellipse(xCenter, cBest, cMin, etheta) if start is not None and end is not None: - plt.plot([start[0], start[1]], [end[0], end[1]], "-g") + plt.plot([start[0], start[1]], [end[0], end[1]], "-g") - # for node in self.nodeList: - # if node.parent is not None: - # if node.x or node.y is not None: - # plt.plot([node.x, self.nodeList[node.parent].x], [ - # node.y, self.nodeList[node.parent].y], "-g") + if tree is not None and len(tree) != 0: + for key, value in tree.items(): + keyCoord = self.tree.nodeIdToRealWorldCoord(key) + valueCoord = self.tree.nodeIdToRealWorldCoord(value) + plt.plot(keyCoord, valueCoord, "-r") for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, "ok", ms=30 * size) @@ -570,7 +573,7 @@ def main(): ] bitStar = BITStar(start=[0, 0], goal=[2, 4], obstacleList=obstacleList, - randArea=[-2, 15]) + randArea=[0, 15]) path = bitStar.plan(animation=show_animation) print("Done") From d4d45e7443d86e58f65e33d5430fbb6ea40f0174 Mon Sep 17 00:00:00 2001 From: Karan Date: Thu, 14 Jun 2018 14:00:18 -0500 Subject: [PATCH 11/12] final working version TODO: performance improvements --- .../batch_informed_rrtstar.py | 174 +++++++++--------- 1 file changed, 87 insertions(+), 87 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 786ff9cb..f54b80b7 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -1,5 +1,12 @@ """ -Batch Informed Trees based path planning +Batch Informed Trees based path planning: +Uses a heuristic to efficiently search increasingly dense +RGGs while reusing previous information. Provides faster +convergence that RRT*, Informed RRT* and other sampling based +methods. + +Uses lazy connecting by combining sampling based methods and A* +like incremental graph search algorithms. author: Karan Chawla(@karanchawla) @@ -15,6 +22,9 @@ import matplotlib.pyplot as plt show_animation = True +# Class to represent the explicit tree created +# while sampling through the state space + class RTree(object): @@ -113,28 +123,19 @@ class RTree(object): # in the full configuration space return self.gridCoordToRealWorldCoord(self.nodeIdToGridCoord(nid)) - -class Node(): - - def __init__(self, x, y): - self.x = x - self.y = y - self.cost = 0.0 - self.parent = None +# Uses Batch Informed Trees to find a path from start to goal class BITStar(object): def __init__(self, start, goal, obstacleList, randArea, eta=2.0, - expandDis=0.5, goalSampleRate=10, maxIter=200): + maxIter=80): self.start = start self.goal = goal self.minrand = randArea[0] self.maxrand = randArea[1] - self.expandDis = expandDis - self.goalSampleRate = goalSampleRate self.maxIter = maxIter self.obstacleList = obstacleList @@ -153,14 +154,12 @@ class BITStar(object): lowerLimit = [randArea[0], randArea[0]] upperLimit = [randArea[1], randArea[1]] self.tree = RTree(start=start, lowerLimit=lowerLimit, - upperLimit=upperLimit, resolution=0.1) + upperLimit=upperLimit, resolution=0.01) def plan(self, animation=True): self.startId = self.tree.realWorldToNodeId(self.start) - print("startId: ", self.startId) self.goalId = self.tree.realWorldToNodeId(self.goal) - print("goalId: ", self.goalId) # add goal to the samples self.samples[self.goalId] = self.goal @@ -182,7 +181,7 @@ class BITStar(object): # Computing the sampling space cMin = math.sqrt(pow(self.start[0] - self.goal[1], 2) + - pow(self.start[0] - self.goal[1], 2)) + pow(self.start[0] - self.goal[1], 2)) / 1.5 xCenter = np.matrix([[(self.start[0] + self.goal[0]) / 2.0], [(self.goal[1] - self.start[1]) / 2.0], [0]]) a1 = np.matrix([[(self.goal[0] - self.start[0]) / cMin], @@ -201,14 +200,21 @@ class BITStar(object): # run until done while (iterations < self.maxIter): if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0: + print("Batch: ", iterations) # Using informed rrt star way of computing the samples - self.samples.update(self.informedSample( - 50, cBest, cMin, xCenter, C)) - # prune the tree self.r = 2.0 if iterations != 0: + if foundGoal: + # a better way to do this would be to make number of samples + # a function of cMin + m = 200 + self.samples = dict() + self.samples[self.goalId] = self.goal + else: + m = 100 + cBest = self.g_scores[self.goalId] self.samples.update(self.informedSample( - 200, cBest, cMin, xCenter, C)) + m, cBest, cMin, xCenter, C)) # make the old vertices the new vertices self.old_vertices += self.tree.vertices.keys() @@ -231,7 +237,7 @@ class BITStar(object): bestEdge[0], bestEdge[1]) + self.computeHeuristicCost(bestEdge[1], self.goalId) estimatedCostOfEdge = self.computeDistanceCost(self.startId, bestEdge[0]) + self.computeHeuristicCost( bestEdge[0], bestEdge[1]) + self.computeHeuristicCost(bestEdge[1], self.goalId) - actualCostOfEdge = self.g_scores[bestEdge[0]] + + \ + actualCostOfEdge = self.g_scores[bestEdge[0]] + \ self.computeDistanceCost(bestEdge[0], bestEdge[1]) if(estimatedCostOfVertex < self.g_scores[self.goalId]): @@ -243,18 +249,22 @@ class BITStar(object): secondCoord = self.tree.nodeIdToRealWorldCoord( bestEdge[1]) path = self.connect(firstCoord, secondCoord) + lastEdge = self.tree.realWorldToNodeId(secondCoord) if path is None or len(path) == 0: continue nextCoord = path[len(path) - 1, :] nextCoordPathId = self.tree.realWorldToNodeId( nextCoord) bestEdge = (bestEdge[0], nextCoordPathId) - try: - del self.samples[bestEdge[1]] - except(KeyError): - pass - eid = self.tree.addVertex(nextCoord) - self.vertex_queue.append(eid) + if(bestEdge[1] in self.tree.vertices.keys()): + continue + else: + try: + del self.samples[bestEdge[1]] + except(KeyError): + pass + eid = self.tree.addVertex(nextCoord) + self.vertex_queue.append(eid) if eid == self.goalId or bestEdge[0] == self.goalId or bestEdge[1] == self.goalId: print("Goal found") foundGoal = True @@ -273,7 +283,7 @@ class BITStar(object): if animation: self.drawGraph(xCenter=xCenter, cBest=cBest, cMin=cMin, etheta=etheta, samples=self.samples.values(), - start=firstCoord, end=secondCoord, tree=self.nodes) + start=firstCoord, end=secondCoord, tree=self.tree.edges) for edge in self.edge_queue: if(edge[0] == bestEdge[1]): @@ -283,29 +293,31 @@ class BITStar(object): (edge[0], bestEdge[1])) if(edge[1] == bestEdge[1]): if self.g_scores[edge[1]] + self.computeDistanceCost(edge[1], bestEdge[1]) >= self.g_scores[self.goalId]: - if(edge[1], bestEdge[1]) in self.edge_queue: + if(lastEdge, bestEdge[1]) in self.edge_queue: self.edge_queue.remove( - (edge[1], bestEdge[1])) + (lastEdge, bestEdge[1])) else: + print("Nothing good") self.edge_queue = [] self.vertex_queue = [] iterations += 1 + print("Finding the path") plan.append(self.goal) currId = self.goalId while (currId != self.startId): plan.append(self.tree.nodeIdToRealWorldCoord(currId)) currId = self.nodes[currId] - plan.append(self.startId) + plan.append(self.start) plan = plan[::-1] # reverse the plan - return np.array(plan) + return plan def connect(self, start, end): # A function which attempts to extend from a start coordinates # to goal coordinates steps = self.computeDistanceCost(self.tree.realWorldToNodeId( - start), self.tree.realWorldToNodeId(end)) * 25 + start), self.tree.realWorldToNodeId(end)) * 10 x = np.linspace(start[0], end[0], num=steps) y = np.linspace(start[1], end[1], num=steps) for i in range(len(x)): @@ -314,14 +326,15 @@ class BITStar(object): return None # if collision, send path until collision return np.vstack((x[0:i], y[0:i])).transpose() - return np.vstack((x, y)).transpose() + + return np.vstack((x, y)).transpose() def _collisionCheck(self, x, y): for (ox, oy, size) in self.obstacleList: dx = ox - x dy = oy - y d = dx * dx + dy * dy - if d <= 1.1 * size ** 2: + if d <= size ** 2: return True # collision return False @@ -341,20 +354,10 @@ class BITStar(object): return np.linalg.norm(stop - start, 2) - def radius(self, q): - dim = len(start) # dimensions - space_measure = self.minrand * self.maxrand # volume of the space - - min_radius = self.eta * 2.0 * pow((1.0 + 1.0 / dim) * - (space_measure / self.unit_ball_measure), 1.0 / dim) - return min_radius * pow(numpy.log(q) / q, 1 / dim) - - # Return the closest sample - # def getNearestSample(self): - # Sample free space confined in the radius of ball R def informedSample(self, m, cMax, cMin, xCenter, C): samples = dict() + print("g_Score goal id: ", self.g_scores[self.goalId]) for i in range(m + 1): if cMax < float('inf'): r = [cMax / 2.0, @@ -436,53 +439,51 @@ class BITStar(object): if(np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid): neigbors.append((sid, scoord)) - # add the vertex to the edge queue - if vid not in self.old_vertices: - neigbors = [] - for v, edges in self.tree.vertices.items(): - if v != vid and (v, vid) not in self.edge_queue: - vcoord = self.tree.nodeIdToRealWorldCoord(v) - if(np.linalg.norm(vcoord - currCoord, 2) <= self.r): - neigbors.append((vid, vcoord)) - # add an edge to the edge queue is the path might improve the solution for neighbor in neigbors: sid = neighbor[0] + scoord = neighbor[1] estimated_f_score = self.computeDistanceCost( self.startId, vid) + self.computeHeuristicCost(sid, self.goalId) + self.computeDistanceCost(vid, sid) if estimated_f_score < self.g_scores[self.goalId]: self.edge_queue.append((vid, sid)) + # add the vertex to the edge queue + if vid not in self.old_vertices: + neigbors = [] + for v, edges in self.tree.vertices.items(): + if v != vid and (v, vid) not in self.edge_queue and (vid, v) not in self.edge_queue: + vcoord = self.tree.nodeIdToRealWorldCoord(v) + if(np.linalg.norm(vcoord - currCoord, 2) <= self.r and v != vid): + neigbors.append((vid, vcoord)) + + for neighbor in neigbors: + sid = neighbor[0] + scoord = neighbor[1] + estimated_f_score = self.computeDistanceCost(self.startId, vid) + \ + self.computeDistanceCost( + vid, sid) + self.computeHeuristicCost(sid, self.goalId) + if estimated_f_score < self.g_scores[self.goalId] and (self.g_scores[vid] + self.computeDistanceCost(vid, sid)) < self.g_scores[sid]: + self.edge_queue.append((vid, sid)) + def updateGraph(self): closedSet = [] openSet = [] currId = self.startId openSet.append(currId) - # do some plotting - foundGoal = False while len(openSet) != 0: # get the element with lowest f_score - minn = float('inf') - min_node = None - min_idx = 0 - for i in range(0, len(openSet)): - try: - f_score = self.f_scores[openSet[i]] - except: - pass - if f_score < minn: - minn = f_score - min_node = openSet[i] - min_idx = i - currId = min_node + currId = min(openSet, key=lambda x: self.f_scores[x]) - openSet.pop(min_idx) + # remove element from open set + openSet.remove(currId) # Check if we're at the goal if(currId == self.goalId): + self.nodes[self.goalId] foundGoal = True break @@ -507,7 +508,7 @@ class BITStar(object): # update g and f scores self.g_scores[succesor] = g_score - self.f_scores[succesor] = f_score + \ + self.f_scores[succesor] = g_score + \ self.computeHeuristicCost(succesor, self.goalId) # store the parent and child @@ -526,12 +527,6 @@ class BITStar(object): if start is not None and end is not None: plt.plot([start[0], start[1]], [end[0], end[1]], "-g") - if tree is not None and len(tree) != 0: - for key, value in tree.items(): - keyCoord = self.tree.nodeIdToRealWorldCoord(key) - valueCoord = self.tree.nodeIdToRealWorldCoord(value) - plt.plot(keyCoord, valueCoord, "-r") - for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, "ok", ms=30 * size) @@ -564,18 +559,23 @@ class BITStar(object): def main(): print("Starting Batch Informed Trees Star planning") obstacleList = [ - # (5, 5, 0.5), - # (9, 6, 1), - # (7, 5, 1), - # (1, 5, 1), - # (3, 6, 1), - # (7, 9, 1) + (5, 5, 0.5), + (9, 6, 1), + (7, 5, 1), + (1, 5, 1), + (3, 6, 1), + (7, 9, 1) ] - bitStar = BITStar(start=[0, 0], goal=[2, 4], obstacleList=obstacleList, - randArea=[0, 15]) + bitStar = BITStar(start=[-1, 0], goal=[3, 8], obstacleList=obstacleList, + randArea=[-2, 15]) path = bitStar.plan(animation=show_animation) - print("Done") + print(path) + if show_animation: + plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') + plt.grid(True) + plt.pause(0.05) + plt.show() if __name__ == '__main__': From addd75b4b7dd4673df4a5e1a8b3b544a3a735368 Mon Sep 17 00:00:00 2001 From: Karan Date: Thu, 14 Jun 2018 14:01:48 -0500 Subject: [PATCH 12/12] solution example --- PathPlanning/BatchInformedRRTStar/bit_star.png | Bin 0 -> 32102 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 PathPlanning/BatchInformedRRTStar/bit_star.png diff --git a/PathPlanning/BatchInformedRRTStar/bit_star.png b/PathPlanning/BatchInformedRRTStar/bit_star.png new file mode 100644 index 0000000000000000000000000000000000000000..ee3036e89881dc601cf1887146c24ed76b441717 GIT binary patch literal 32102 zcmeFZWmuH$`Zqd5iF64_g8@n_lG3S?f`D`?-JK&Pp@bMHT_PYQ-7O;0-2x&KLr6;Q zb9~l+t+n@iyx;bReeAtGp65Vdoco?D&g)m_9igtONK8OSfIuLK? zT`UpyrfyDlj&63=W-K0-F0R&&4lFl#_<8uaSghRKoJ4tf|MLPKN0-OE+4i%q5C|5; z9XV+&ucWnUZ!fLIbji()x1APPcR6q*EcuBwp;wXb)>1}$>rcr7Tr518dNy^k%>aa!WSh=T0{hV)e8Inzx{vE zXqdGaqN2Z~I(B(rMrr&$RgFr2G#AN#Jubc9yY$hmQefjdGNlVE2zGb&L z9#!?+?81+acQo!9KJ5KP8C@fI>lUks$oQdnp#l*qAU4du)@?cH;NXDo%~dbutCXUi z>(34jydE6OXNA>#O6gbRcDEGIR1;p!`?;*Y!fI~NK2WTrgwGpBnpuRv6$?x46|~vl9Q8bc*)r#j(;XPI_*z; zE-bz=_Bg8ApABSA_20Ybv%7L3;|08kJXA-yija&e{;En#)}8n%iJ$pJ4Kf}cf{{^C z{FYx|lD&EcBhOKnA&BrXE76U8`!<3-#S1GKkNEx$LuNmIX1~=*eQxe~gvpJ!@L$mV z9wnAx?suyk|BPwoYs@&YGq^u!Jz9yd!TwIjTZc+Wp#AaV2S-59*H@e+Zp-&`COy}u zB6^a=&VBMC&n+txn3Bi8SU&V>_nCxEF6Zh+f0>7)KY#v=t=G=FZ;23~B|2C&K8vjP z-IpQ5Y46o9HbF849y_mYsHmtYsj7ZX;5UzrjU{`Qp@0+L88R?%{~5C+lV~!}RQvhs z|NX8l8amcEYTa5F5t63|<_2|M z8TZ^b&ui+e$zXI!q zAol|?>ej7Wv^O6;+x4_wpTQ=*jS`yix}=TOVh9q%c!Dm-6o0Q-=4BxEZHt~AHkwe|_?=IE-TS7wS8#DE~Wme>S z>(f#DGrpvX(G1Ny%Y&`z_u-#p^hKWcLRgZ*84BZErjBu~~e7 zDp)f|HC-)LBAtT`Ga0&a_M~Z*boBJB_*f15^&6qJ9GG<=eW6?&HDhIM-Hf?G$>1c8b%Ozm*Z3%J7dMx^yd8*a6# zIZ=m_X(*qS^B;2ltL%QQ$y*e6XNTf2Y8?1HEDUdTqa%h{(cYfB#&aWj(y@^ds+J^X zQZh2w61pEHNd__@`s#f>+dh8|%OCqYsx;-u;`i_I%s)RXIXyjCh`~lMd3{IT-u`xT ztj3)aVe+dl-J^H1+Lc`)hKcc$3uae@>J8yF^=B%!PVXpOQ1-_Z>lf|*|KG#|S7GoK znf*ho2j8db6d94>AVWh#J@%tA!rP;07WO6`6?0UR`5dN%8N_`Al(YX9sO1047W%Y* zwyYK?6A&O_%oIL*m})v5tRJ{0~GTw{tmhJf1foL8kD{|_BkC(1b$p3y#0lRO!M z1A*>2AKmkj*~EA65QwYs=dZ?Js|qE#le`UP(!AdW%b#-<9V#Ikr_Xgt7ra1`L>v*f9pi$^Aj<^WRQCdbuhV~w{ z0o+%1b~deKz->2C!>TAst-tR|o55jrc6!1`OEgyN$-B2U9^Cqrbh7(ObW~Jp(-Umx zUh#dCv0_tN2|wX5qKg!pCSQ65YCdGANO`SXfBrr9?R9LE-V|{pf`-SS30*nsP=C@A zN=Pf}_Mz*4cWC(!!ZuY&6l9f@WZvD~9T|~omSjZ*Dd=mPlVHK*!E= zEx6CEom`eG5lT*u1a6HVHf55qGn>VnnwelC z2CqD06nov;EJr2q_a#4jQ<(=+Bo*(gnRs}3=o+PlXD|aAhf)vh5P*KjZ>Q;D}gUxzs^fMk)v{o+p1(B*TBJy zU7=AuUDtVp0) z_)YIs5={i?S`CRFa&vQ!)p@&CMM=wPguw5#)TrctEG+?;cUW9nI`Z3LAdkBQ`1yYQ z$ltJkFHPWb!}%~ep5#=^9E+DTzI4Ft=RM}5O&eTp zF(j&V6J;dAll^;%Aj~C-^706vLQJ(gby?g<&&#_$)y|T^(Tw>=IyyQu>uek@a9B9e zJPFUhfq}|>#}Nsq^?6TzDt@}>(e#Gd5g z9Crb&#jQN2<+Y|gccwMn3$I*@wu!%Lk6`QzvZIjKb!fTN$J=*_&NozTp4&NQ5Ll>9)Ws(_@ifI`|Ra*HA%>*V;#a!vY_Vh!%q%6@Ezg z0%vGKA!%=WIaNi;$Y})0Hf7n!$jkd8DCMPcum$K?bo067w`!SjEh(;F=QEU6WBa2< zh64p61T#-RemLRsk>e8iBJNzcz4z&Si)XVkfwf-2(NfjifvyG>zgZi;mX6M)OP6G~ z-WEug@p=t?nLZLIPX}s7iC&Fu>QF!A;Cl{zC%_< z(kwYurpNv1(~eBj8b1fkk;0!5n~(DAwhI>icW5lK1P^ zug5$w9Fc$k<8J9%mFscomEl9pR#H(ZZwRBpVZ=^17uKS%>lA{WExW%yo!&3^+4wo# zb36b`x)pX0|BfDPm)Dnm2bDR zVv*bGLKNU)YDL#Tcwgm4b02?_NM3Gk34P)2>wHvhzm>YNF=>;x(%53T0gnD29^Ocq z^&p?`-dZs!u5G!V#iK`NUteA`n!AP3bLxp|vS@nX*{N=WpSN5JR+@rK zYh5&_`N?!fc2*Y7(f-E%!P|RLf^}%zq19bjhGESn#iv#D1EpwOw{n*;vl8Lyl&#*t z7MG3skySg}MnuTjQ6uW3nRaPgo7~z=Jv|_F98_8X$?HNf+Yv55fDA-|a#U_`6TEes zSR)1cPjA_dq`}I-e0#c+BkA0ulF*4O*>R!a{-$dZ^U{~;lNq93EVOZ7!w%L}c6M_N zND#E69T*%eab8fc5}@NkvIrgV2?^26f3iHdaZ}S{aT882OUE}Y%WlhBl+u8jo{ie{ zTP~>pz7@Oh~KmY2Xq?tDS6E+o`hmQd=3q|V>Epl1)k-(d5wUG1Jh(>Z~tJ& zOQb@!3?eQ#q>{&cqV6j=(qw)T?^Jvo0FG{MwvsMDT{J-7lVi=EK)|t}^zzLc{mS`+ z%>B^hp13dgZjzg7}0JV8b6D z_^=T*Zq}riRa7`$I>bE3)zdF%tSxn%l^E|e?R;@XNlk6vOA>)mb-Ld0cECtj1gEj! z|C()-H1gY$Pks952Z(`~&oWwM+!2WFMUNWdwZI|s+%EV3V?JJPtF!Z~9~*dRC25S9 z2co0|#gVc3nfQ!Tnkgc~>ZC)FWs;N}7goj1M?FaqpFb-Mr<4{h(a6KY?EpDyfs@oGs9wv4yF=G%s6 zgvVV{%&4dsaR}GSA2tI(0;jhcpUn`*n|&Q^Z9QKkRH2wC8r~Ye0w7ER0s;rO);O0i zTg`WgacXO4&|^-G)TD&vd*C^urbNevWhw>?ds-bW$^<|SsNAK-!O)PA$!}Bo4~=01 zgZ%Px0-q@ss>%O-oGJkQAx)i;n};Epg|I|_^=CfK!%nk#ib_mOv}7NwrS}+I-Th=L zl`|-R!mQr3??W)~RjWyq%Gy^&(bCfLOJ}Dvu)xpH=^uLbn$1SR|RhW8wi36g;gUFqiCFRti{`UziEOCjnmG(@0e ziqQ&}{$YRVAM^EQG(ELy@hw0&_1YVMyw!Y;`8yC)ud|c=5}V=c$53gBJ^M2vO=`J2 zyW8=}1s((TMiV;%Jx7S%#ux-4SCx8Gr^>dDTI+s^v* zWaHN^?|sAY#={E&LcZ+To9CJwOuuY>!L_PvoM4DoNL zF>a$&yZwaxl0sTR0Wm*+VWPX4S;jP9!*q#_74vlmD=R4=*CzXxwHHtOM)1C+JU>|M z;$K)9-jnAi__`bcD6HP+mTHpl6Oe=$VGGFu(kp+w4I?h>_KOVWtZ|YgEQ%7Wn|tNR zSS&S%)N00pf`g*7L*CVm3f~ct9_M&M^XM2ARaFHSmm8?PzMGCSw;3b@rs#a?9y%}6 z3SR->cJDP;^N-GW?9!k?*5kS7La`r**{Y2t&x-ux&3mn3=~D9=anZkT@6SX_aHeOp zY?$xDteI<|$@p-4+-N!~MM#S7+Nl-BdTr}Q@k~`mPz6_^!%@W9_-iVv`B-e{M8*;ztcMs zOMPB(acnKF8H=&v2coup9|K%8Hs09#Qni|?P~O{F7m*V}MxINE&GP3Pn%uz%hs(Gn z!Qj`{dm8oSyoag1r^FfuuEVYbP3Smhg!)0Z5R#XaoZOu#30p}bjxexd`%4|M?p@~m z(N)8qY1hi8wz#Pv3WSxdZR`4U?G~&+vU;E0-}aQh$U-HXyl1l5?*ehfTWHU84m&znw>?%_no076)3xx&kv>5_ao*V z(Zp77Z|{D>l9G|Zij0gTvn@SKouazkv5ixzC|}iV`YJ6a$4H9JQhlY;dQ$+m zce^}C)xnl#vM)k|Xx?=~B{`}ST14lS+E6B;-t7l; zM1IS;+in!$mub^G9&gm2JO2Gfl~CetWR$pr4dWe#B{vFk4L5VdW%j|vy**od`Bne@ zYYkz%;_i1A8pVhQ$jPk$ z|3yqZJqG07Hk)+rYBUouu&P=N(>_sd>)gTUX@Z_82d)MG0cIvyxo#QQ zbLVxCq%St+<18Wa3@(2>CTbhuNvVk0N{rfE67E%{lK%)A1G{QcQy4Kl0s+Ty2Y4va z-rGMrd&~ie+1uNPM?{?ea=U}1G9gU#qg16K8v!Gp)P-%g6i1FfrR7lrXmh`xY{#AU zdiL}UHa0dN-(YwnY;A|e6A$2}ttNbFpa_7{Vpa0cgp!8l>C2Z?&uICvfNCo$DBywY zmQls@L{ItBm@+O4g*-VNyredhS2Fs{5L&F&_D1iKtZz4Hh@{b5E;rX!$5wR>44#2X zBNobw04M}<6{_C~2l2~IbbL@)PfxEH5Q^VcJB`=kD;)#^8;?jzRh8t%&70(ou53|B zQXi!%PM%8p$>Y8gwW?(~0|Ur(@V&~H-d;t2e{t0Ja`<@MJxDFqj!l}pcBbaw6gv-Pr)kE!`?us6g85dW?kk16pqJu~~MsFo{e<&*Qyx4He z8p#F}@!mZ`3`GG^Za$b)!pX-VO`FAYM^O6o+V$%#hMv=*Ait%<`809rmtKaZ(D=oV z$O|d!j=F5XDUMzJKxe~_^MA4c_lozXYEK5$(vxp>SwhKL#c#4aAfN(43eH#am7&~( zn~yOnSHb?BDC?}@&n}WiMN-u5-yQO0$umnz$iZ++5VWT&G^l2qj7O7t{{0lLhzCuA z2cg^Hyw&oQr^;mei*=@IA>fjbf}9XEzUGooqqDsxBcHeNnp+<;AHW(e0XdX%>bXPG z_E158%zH!J>T!3FiD2vR zRb$ypl&5ySjKR~BOA$a809q+TGem%-&=e3?QI6zC2Z}opzWwF#FJTW>G`?3+DyOu4 zty~uVQ>zW&7><-k)vJc2DB~CGrDqej^FcTHB*Pkx(L}LNKXe-zY)I&WRupwPe(x zelxLK*m)l3b2KA;G3zZ2D&(Yh5N|!zi-lE=oqtrps@O2eJA82G*E$fi*)8O!Q{5Z! z`T}!ZY|o{V4@`Bm6NejK8=ukny1H1jGP6kbI)Jk!{qQ*b=HfMGc1pla-F7zAL2 z-zr3@G%D;T!@3iNo^l`G!5a36_##Gxy8iv%K`F7Dx3{;d@o{v$|9a5Fvm6>)DA_R1^4`txL^xcH@$eCRk*gkz>1i zleB-jjD38fSR&beZ*PlB@Fc;PEnxM&_1>k^3Iq2iDR7AB1-XiGvx>uiVn);y?n6)< zU%$R;g}d~xaQCIV`;|DBO7wt;d9-N}2urzdw#G`#C5SK8uAX2c2vPAuXMtU%z1QE( zRs@~}42_N|t{DhJjO51`at5H@Q}XHft?)ldUSy6j z`2GTZ{#R7$Fe_;?D@jXBLx2?jvOan$g88l*TA1YfEV3bms@jg69N`3pqRwP#-c!IGTr>%`~jxjvxLf-;gGoP zYHtd)UDfI>I6MHI!*875%`Q3gBux3qPClri0JQ{!8a8op!!5YFD!R`~;FtGJEuJ&9 zzg2|)K2BxqmqX` zZ~@SJq>R?XhZn&)F)h)xSXvSZ6YLNPJsncKzL6y2>h!CRbxqWrAQ;(Z+&~l`w5t8! zL9@mE-(NOs+?HPq9$n!s6!AOY6SSWQ0lIws*_AByRebx=e=G8lKK~NKUc|?5fg_{u zSi2A{)}qU{5uT~2?~AO&+YA?~l}8A@Rlb@8b{!m)RRJe8T}xkyTfdeNv(7kGj zK14{tQBT4sii0@#_3h%fa}Wl)U|3pMsA?V8+Ou`?DcSK_!CJ}<%e%r+N~}W~6NNjE zZf#i!-rGHSF@hi|~IvexMCI8?c z`b2GBP6q~`ObqOT+Pds9M||JvVD9J*Jat{wzEo8Ao8W(^ zrs&OFM36Dho^}b%rt!51IzPBeC4YOXjq>1k7q57sT;1-7J}3%mmjnez4(0p)kw#jJ zZmdmK_O83Gk$nH!eKxz-BUW5>{tW;!j3{{GbD~axg9O*6BZ{{n92A}Ly;_S-imaR* zY=p$wv1{Glgl&SDClBN=*41f*Y{E0F42P($O}=$2t2lyNrsU_(LuaP3*g80T28*qK zY%JVsx%Ppj7Cpk;+R#>jC{ zJei>G5CH=0h-H@W9Q-1|tus+DWR(e*;z5(AC9! z%-TP%^$MtEpOTcHiqML$V)E6cNVdI=S2(kOd!1C+Bf&+HbuA_x~|`|0`dym#R}a8z2T09_;`oQgiJ_!|HDl zpL(M`UiD;EnjwW%+Ec8O`B9qF5lt$2F2fovkNyr=bxBON`PtdH<>lqVUYjgN{<{wl z;QV06#>WSL`=;C;i1J(;o1;2K?G>1WVI7!F3bKe#+Z!&-&x?Lje`nDidErxzBm^*M ziSX7y!a;n@=VjC}_1NJ}lRCR-Z)4K}XL@XEs^j!0rJTV|5>DJa^z{qG_ButQm-c>= zhZN!*ZA>er|FzBkCB+bb`=F8O1hu$!a%FY3ztVA*@0F4fUxn{^8nwh5oTf*(zTg=D z2f^~_aNGj~3T%4Ke<0530==LY!#mYGcee7r#WhX$8v~~YjPRwat5odlZ4&qf-G$xb zr{*n#No;`=4ry*LD^?F;T-gY2592k>o}KO=td;g|PPz7`r>7%9TWp5X90@rH2%wb$ zQ>vJ(fBEXwcycoRa+b~Aqks0o0N8mW~IenM%+iX6^!|W(P=e#z?KYo<8+qk7u znAY9;w&{|>v+#pJ@y~5-X&|<#9$oudqRWNZOd6V+=9SEs#yS5Npgdd29W?j3#|~Bz+#GiRW>bkQNquwtVTvs+EO3tBHTP+Jy)zW&{><^lD+z1uqF1K(MxkBd&3 zbpNS*MBEY6wdXY+whl7({L$=LK%3phoYM6-Is5t`sDs!H3(l3K+_BN#5cv$~>Db-{ zw>G(?9}VSdk~3qy z!XS>p98%!YCVaFXCHWEy#M5*3*U@}O9k_Qdmrd8yh{A?FcRT^)jQq&^b9=if1X}~YMl zpl7-mg>|{!ZAf&Cu%7dBssuG7}Md{%b0DHvcQ%+9|p!p%QRG2Fxh>O^8n z+`@Z*qVV5bgQ{=!b4SNQ`ctOJD#4G#HgT_BWlyMhN64rQM@g|o)SjGzsS<=s=ybSa z{ib&mU@l|F@On`Ft8xx{S0HZPh7d|5DB>w%^NUSUDQ8iKK4K1<2^VIY0Gf38FSa)Jk%}F#WMm%XQjZw>|Fh^u zpcfO%5jj@r$efp-@4We4+V2Qm`D-Yb5@U=&HGl(DamdLytK9IpLh#3rTnrL^W9v6} zMM}Nc_Xj5B1#wsMt@e4Pyfb){0H&Y%3h62Lvf z+MjYIt#F);_T8u-jiESjfKp99>7fs*llS(c+rPhDf*XBz%T^uAeFZKrKR>?`Bv7n2 zu99DCWoqIp6`$$mKmIcq%q~7U{Jwo4N!n5NS?vwTcj0n{-~@wt1}O>H_M5Q(M0e99 zcGgM}C@}##zZzem9UpNxG|<$Fuop2gDe?<|BFxvXb8=!gOvea4Rtp5p3v#m*U~G<> zLXxi4O27;R2h-*ARPy{8lROB>{hX_tc1KTKfD8TtQ|^l|R=FPc;T}2)!Ut~XiCbAk zDspQ`A~DbH{Nv?jTYGzVF~4P-o;3FLuRp9)%&HSy6T;Na?`~Nyk^cfdOG0>2o9uO;FXbe9#~fYiVnnL5*cy#bgUc0zcp^ zv{ADCeg-RGzG|-YQlRfR|9dLi#{Vp^{@-$&ChT|D6qP1x-tQX}e?o}vEVk~;< zRoj@Db3=Zj_ya?->S#lM3Gt+k4>yB-*xNt?`0E zgQ~=5*QO|SsRxr?j;&x1ZYCqokDZRQt$i{C)WB}4iX2h|vJiW0I(w2iYy$~fQYy&m zbqme@DCX5jaK;yId#|HamRih$KYOARHWa*66_^M*dt2=KuZ>(~$VRk*`6;8#WDEu^ zxJl#FkHkK57vPK}-M%K)wq8|~Q6S-e_E5zvmTyQwxI(p;@(G*CZYxlE}tJSNdgbm7Tv_Z&l zN}^}fNNUxV!vJv~B*V?QKxGSCSyVym@If+ULK}w z(LPsL#p?uHANw2$W^y(mYg+SB_6?7r(?Zx0vA*9j?#`p(q95OzgE4Zv6{()GG7-c{ zX^U5@2Z7Dl>z@yWz8Opf@;aD%>ioS4xmstYV-GRe3jmgzAjJ^}wHM<0ic_->M&-T!oeq4zQx%9n~XSXp@!SS)*GkPH&t!&Tyf&%U-`OU9;=t)bp!(ucNXTlem z-PHs4zgv{v0*t@gqhZW^6Pv>#qyRKjQKCY?no6aInGLVv;;#we0vZQqiZb zOAtdKK5(jpxSxXq$*)_6%S!e-Hk|1xMI888-~t4(%7(FUa(3n3t^n;*6psY?G+bT7 zRA?tk^0?VSj7H~&>a=c&S!lTspSp3~={hqF<)4wV!pACWE-*T7%RyaXDDKa}Xqn6w;G$UwJv%E<<5$SyMZ_ z>;m~0s+X+aF**L7cZF_Q6`YK@G}nHB#OqE(gi2?NL`x#Tlihu!mMALLBLc8-BqdJD zes@l|%|K6Y#hsQ42MH86;LQ~+sN$HA??=6I0;n(G?u062N?$_R3Ndf@*%~Isq&7Cg1*vi3vWdzBH84 z;H&5%k6j<}R%&uw9Bh;DH7k4E^;%M=G2<=jWCfKa^quW;lMztf${DnbDIVV5u*#8`fhS-+q7MrybtZoFc=vGp12_}L0!#2Nv-&IRfGk9vs zpq8(kwh4`3Qt;(X!)x+K44fY7gx|SC_V}ArduB{VX;ccWXS0EW((EKzkp)kNTxUQW zYW0NU6_lHfWY3w4#VOaM$uNUW#3(AjGOC$zsmw50^(k265TBHzy2AGO#+{cnNGVMX zB$qy(^1I7NwAJS0FFy+7PPAf@@rc=IgnqS4_OylXBnw+;Jiqy8j+znJF+1>P9S=J0M(?`* zsA67#s^JW0YHf$x#th?~J9iLuv%CY7ubQBtq@;tX6&7H zCU@^f?w)~sWZ_!|9O8$E*c(tb)wdLCL8rmoS%%Z^F5}bt{kK}B$#7zf(}2{qSGz75 zkkoha*p8jOBOK)aBB12*z2!0R8Jpf&ZvjQuDgg;caNN`aqW9rcrlbg^^V7Bo2d8Br3SR~{l?d(2I zE)1jgiYEc$fQMLXaABqFLq_$*FW+!xe2Y%@FL0uAQt@z*|IS1;Ot%orkM0Idjoi3P z^9pf#PEsFqMKGj4+ZNS_0ialbtknqdvuyKL*ec+QV3LGCe*QFDpQ<)B4y9*ij(N)D zgAda5wk?x+8ZcrDOG{P>2?=0&(4T^!wRm!1c0s;>b?#a<+8|4cWmln^Fc9L&9M`YQ z*x7Nxn>xYS+dgM8oc93J9pSY2^;J0jlYcVI9Gsl4qD2*-J`FgGO~77;WK5OcpBwL$ z-_HM;r{O^i=3x8oK@zTs-VxDwdJC~+R1Nqbjcdxuz#yaFiqHHrk==C71?X+`JenF) zldx`7Kwz`RY;O>`=YQj)~^{x4lW$Xn5|Bq?tQ(s zSXKV4-Ao!5H?&K{fbPplpbH83CRvugQ_?udb^+}ML8F;0*S-=Iy*VBNa`}rrNFpo< zNvy`qj#9|OB-Qu>w~@B7xekYpMem1K`n2=wA0Jjf=u8pjkxCGCzYd~wU5x@CB+DU# z{dMnWwcaZ@d_Y8*;>{YbF7+q>5$&+&bJ{<>~s^Z)K61({E+aXzbWyYf^1-C zvQFBnt|YtMzE=g-ANu0wbl8SZLCF-IULVXe(7;LY#)AZDlLc157GYW)Lls)Eb}5pn z$4UE8#7p$1?oM$z(f1k}r$hGCGDmgQPna0=@82#El;iz@*3BQzaGY&`T@ep1z1))~ z1Z^`UbE6r>-v27OZ_?S-MYv+*O40x5No(W<&W}a8peRB9f?mddnq^D}wp1WkN0zI$ zwfz-nwFeI_oE|SpW@a#Y~x>0hMYBMGf;q0tBy5|;F`l7d)U+$~@ zX2DKbLS5bMoY4s+AHpX#AtC(h*Smm~Lb!0?Pyz)cf$1M1NX@l#T$HvTaO!nNqNJPJ)UK5<#k7IWUjT6_B?lUW?LfgkDo_m{;5`B7dbV*$Nq}Drjw6J_*yV6?oNTIi=f#9RT`7cAV zcZ!m7T#!Ai_Y2RB3bu*%9F;9vOPKRf2g*hLSIiwk!<>em7&;4GnHNvPIeyV1`X%c2 zDjQFTWy6TJmjA4KzK+jvb7`#Q1#;;OazBV97hA)KWr1=RJi&y9Kn-9XDKxYlZM7WD zU%aFyn}kS``2ZCfqZ>hgE97ZIJ}S0v817-+ol|zb%lg;F zm6KQcN-qF5z-|VPs^Q%0nD(CIgFFlpCNXa}S;5U=uLDyKZL3f%TfoHaG&N=;I6r)@ zsi^UkuJloN3lwSU*aGU8JMY6YmNe=8h$-%~c%AHyV!E#CP7hZxnPYGFA_lm@^{;vm zorg-YF!z*M0aSLXWa_!d7fXl3tAEBHHw7M_bNYcFn=}=$w-IvQA=PY>v(f{tclpYd zYOXi=MpB-S@$M;(z-Dk8sp|cqDI=+CX2eaw313bgA2Hp^OA?Y zT*3aH86y!1xqOqUDrZ(-6}9-ICmTU>mX_HaDS{oS%pZph_Xm!aaz9p1xeyW%5ICJ4 zdxMRD=Q-n@GjBVVLd}oi1Mq`$q)cw_x|SS^ayDmtn-ch&vhPXx`1sPgjVbnBxEA6# zR*VE8-|3HVwUN{n@kZ3i(u$2lX6=5O`w$0};H|mC4;qmxa%eC~ettsxh<3^qqEnz@ z7ZBjoQcSpv>M*@@GT~XCCuXyrnj6cr#%wOKVpV%s@ip{PnHzpmtZ=k_omNZDJem7+RK;tmjtOk z-H{G<`mfGQQ0qTdo%=kvWae*R%XvscEpzlU5yjDR!}|uDG1!$e6Glv-n64yE&4oRF zkl|svTcB&gocOUqCONxWiq_D!?NK#|;FRLNdu6`Q&`Tmv>OkFS3-}#jDE8p6!A+|< zFUNfH)_?jy0cT>yrNr}}Fg?$tL)zER|8D-w3)>&2lqPC>(MR$NsQN)OX#ff$H>*>P zAh(!I2>(6jpEZ~)uK9VjqKPM-RTK==ddV4QHO+Q<> z;SI&){t$hGU!hCt(vZGGiLbJ$`6Y}Q?fmQ0rT7VlvQbKPwf{VWBpr%S9tjGP*i2#` zT+nD?;yLLUuuV?{U5{!Kf3nsXgzAlW>FwM^lQR_b4Gf5ao4>o&j@v;aQWM}JbWh~) zvKvH4w!9u~K=gsW2MqiKF+Hzg&CoK}l)hvAaQ<%&O947`Wl>y9YCG05 zM%xp^Mqi<}qxJ2TiO3`oY~^GEzf+Mm)#PG!A)3~JGvCxxHWxqMD}^(fSL*DA3MJI) zHiK^UL03vB2HZPOqJKey$3G9qP~%w5d%Zuz2u>!X{8_lUyVoo99jb8wk*Zdn3qt=0 zsQzO(5QI8Ud36jo8g@Mg!da%1X zvibO6W*?QQwMLu!#`>@5(sPgiTZpaWDYoB1!!uBx$62}65&uT>s_V98}|}9 zqxPpCS4x*c7wr}OHC_8zlH*^B=sDgixF7O@4uY8v49=->dijsX_pNdp859geN5m0y zK2MDZSE?6Bxm*vbk}kC8eZvW7wS@fuS0o#R4j(c89<&m`WDTI>-D-4_-P8Yyxb{A- z`PeY7=bYRfC+8`w9Kar6Y6ZKhedjDfY%gIyy6H6Q-&DJSHG7nz!|BiHO*!&1pl7Cn ze!O>k8t85y@4mxjHcb02txPkm1a4B}4mUL+WM4wS`>~rSJ5MTcz%t{tKnzOzq&-f7 zo0l`>evrJbd}Dh7ceXcFQzf3`5>jKzC)#w(44L9b;kBH-$1Z-rvRTiqf*-OOVsT?Xp>h-_#n~QZQWu5Q+LHRY2(V2fGsv3s0oi;&v}90^OP(8VZ?1hLDdhyeTGoH9vf45&-4qS8#YZgA;z$-}n7uR>C1cKPIUDPw4RQ!H9`V z<>6ijLIQWw5z+H&myDCsveg)JRgmq_Ffv-#d1yc^JGqDDv+XyzbjOm5lXF`)dRd4@ zz+Tpu3?5nWsU0!)9ug(ToW>}dsa+;$!%u@)0pfN*z!r&kz|?*W3=cm7FSaOO2;1FWg9_rW z3tM_YELCd}E|qy+;I5PR6LRbuf#G-wLyKzU{bClTgMl@pe1#MuBJ!KBCs{Fzx zBqYRApX?(3DOz$^h{ja-IyZMaJQszkdwaX=&OUL`Jykh5n^pv^luN#lthYVbfk1G) z>!6{W$2Y@ZSLPuXH7!vXYNS)AaE96ew4KTO?+WCuCm;F4NxSg>&~UtX?dEh$>M@bpF3{`|ld{1^!W ze@#jbz#Ho{rjNE8+@e!fp+`C`xi~Z#J>eJraaB52A5!jXkxN=!nObtLfrV7 z($Q2PpSd{Nu-QTgrMJ>5pFOYxvqA5?X{luJooT^zixZ66KGto;ZVFk#ERQkZxgpzvB8vZ*sl6kqbFg<>>B$_ zI^pJZ`A0Xj%ztSeq$`_G9N>>LGV1mFZJ(gU_yszP_;vR@HQri_EL?P(8UP21m%CPpju;1YhRea^EH|WVJxKH{#L6z(XL1lrmXO4IDeZB^ZB8s+?0q?9P>y2)H-TP z^$-OHVvoFcPTx$vuf6GB$^H;z+Y9fXTkd1$S8YI1Vy04sE&PMjj8gpPz; zKdLTQDKF3!`HnFuI$*D_uZNp@23i@9(BhP(rKMi3rxCtPWQ8xadhq)Aq5~AwE*e)p zP9Tyl4A*jWc6P$KhP7Mlb(!_i2QN!A4J*ldAduVq?$1lCbWFBYxE9*%L6+OC?-&`v zX-@)OheA-^OeKsGDCHV(aUs(gR-1C-_5-Nity zt1)|E3OZHjF*Jd=fu5b+Ec0SzALD5`xuMY|0R&RUQ!+9G3sO-C+EVRy0Kn4|3$hL@ zv=Hv^^E$N+JvFZLcyzU9vaO2bjw6&l(e7*Fht|7dXbScbM65HyE6)tMa|2aWD1F!T z&Yu?`M@$@s*|UdyC>TC{5~`mQnr-)K({dAXYLY?fc(L-AOXTAXNqJ!7fh=YWJO*f9 z8%)P6c9#x4dqK1Tg$F{H=>12|vgzAU3Dhk6D-C6GSha7p)^i*8qS(p$N->uhUWbM8 z1!6j|pBftISy@d+iE#v5+cv3G6=mgKR_@?BxIi{?Y7mIPi}t(rU{4V3dZ$5!#IbpG z%Ubz^12!4Y4plIyuzpq^xM)qakT-6y9L)1)2N>1K)e&bD#9X$)n}Qw`lJv@Vq&pQ)nrvW0v2M|MEzl4I4(-|i+-V-3H*k5LQHN+~#A!sK zXZA~%7j{tl?SMJ`aD##upUi1aeBcfo;{g6Y45ao%dEJub4dsZ4h|mq%A2w} z^C)Rv%J&$v2x)J*PKCVt*`-cZg@@z<=J6gfhYxnwQ-r9vw?o&PayRv+S1ePgw6fmO zHlrm=Q5$LyJD~l=YzI1t*tK(K&niIO4;&vet1BKJ=K@WVN-2vPLNFMN>+N^-U(Zki z#^~q28xrW|KA(D?i|fgQAq#Qz=*WgZ$OwL>ecQli>XNMXD?rxHjKUoY7?eIp?4^mH+L} zpPNXz)V(^eY&1+?hfAG-nt{ik7>zDjBP&1ej6}j_l({>xM9vIoRP zg8=tY?sgxL`&_ z$Iecxq0iJ&ls5Bs+iN%i)cunFZ1ljeF`?{hn42%NXSWIbV;pAqcEiK?=1~xA-P{D7 z@#dE8h#(#_;vF;xU6R*w{V$4-Qg6B<0kRwX@xwn^Po@lqb@-++_Qn44J-EGsivm{# zJsaEnvdLu6AV`fRO*Vh|o7=e2)6+AB%NieCGJ00=`g;u>z4jSlC>^c|5m@OG=hrQx zs_6__7;(dP9&3C;_!@WVGh~}{qTL-?9?s&ZKmk@2*|w`}@;+W`EL%rDO?&>zRX;hJ z3r9%2VCB1WW!@jL2Ca_It7j9)X{G_n-QGFkbm7gUK z$Ud^TtOb=TR)&72y%UKpvP&moIs8uzCJSAT-*kDIH$sMlk%Q2%I!i_I}}#COYeWc6OEq3As|2GPO?$Jt4o-9(?cXpb%R4o1Z#=QhI^FnJa> zEB3GR;C2`B2TCR;iB0n6`UhcQ%?!+lY~3$ST1fIeSZxqiN!QP%0mdq^bniBXq>^YM zIlcsqeoAe?8?ui(P!giTmgJNraVe1)ul&sqgNk z#%?g>`o20b+r}h3SvKwKH>*uvRHdi(@K}2MXg$BM#FmPl*Xp_1Y>3BYprk3c&(Thg-P(WK6O!ny#gk* z*3$9nI&qes_Q_R49qbzVF^%u>054G!5&`C~$fg1Y&)-q|UpahpPQFFd1)p zPTNMSsvhRxzeYJyy^+yRsIhyOm9*E8xqt7C7@Zc?ObWjbB9b|kL}M+TyR7H=deBI3 z!`0I!)o|(d1SAMmfY`t$&WrK~gKhk61M!>Zq4r>#QY-`i7~r_NG_ zMS1yrG$S-Uf5A=HPB|mtAs;ASmh8n;%z7cv6t3D&=3Q(FcOL&P6?)!VBCCO*sjMvV ze?;dE7MbTMDT_<*UBl=Vb#vm!+t`brXs#C7uHT=j6|6a{fw+u*?qTSl?m;911?o+o z?OqXiz+*_x=g0Y7JDLF5IxT*(9(al->qzjo~t&zVDr9x|Na-^Os1M=_w{l)3JhR=W zWv3qSi0xJqG^-5@NZDw3{D$G9^rmAzNAb}7($ribHvG6q4vL!XYZbEh zUUVxFW(cE;-wz^(+YUmCwk^?pCv8n}Fo55ds1oz))g}L=bd@)3rq`zzZi+-VrkR5h zfd{aa_E4ll0zos^kE34yb7g9R7!q1vY-qiDGk}wm6V!ee32Wl6 zJAD0`^8@8z{S#WB!75dH20mPQ#qv#Pv%kLA#h1d7fRp|y1GVuVl@0S7jEbn3UDNhX z*}ZOZuw;2Q#P8yeo8}WMapu?IP1Qv?6+n|?s&@wC6k|PEKY@ zpzrp#fNhR5+qog_J&X}I{B>>(L#Us;)7qBis2(XU6|O>{Al+L8SIY3h`wQGJoomjEJ3)V7R@ zxV+WVuSB4w7E)5uk21rcp1QxWlPrgmiV-w-G#WR~>*3<#qXHw-TGAm2cy(`p3N+bo zK@wzoX_v7Z0>aA@)PWKa0H8yaxNw5P!RENZ?8jIa7DlB7#8L2TJm}>BU{8pWMYHvEo!q)aZH!pqh z_{Sb1==f4$7ev(pt?z@iwY5ienytM0RSkpquSc~DrD{w})m~9i;pF4X?IOg& zQsNX8Y@c)izYa89`l&@{-KAO*B^gl23mgu?bx?wby(0=RU+F+a%9?87#ob4a?d**% z79MC{M=wt&38(kn_RrJYvIh)`7J!yP3u36G4XS~BgQpLQ-OoAC9POwOU)mh=JYx8w zlJrnQ=A$C{xdYCS0r?CmKOlqF}OnHfUow)=}#5ED?u1684; z#8ykpX=q?|WsPWV;Aad;6CE2{#M7rw?eI>7TQ#iET#uKaF!QdToM}mqV_x2)qol0r z+G4EgnY}=E;-0yJ0^&K>8w4e%@DzwCE+ul(-V!*&ZE!F?OF=`9b>07aB!T$vyg#ao zqe&&S;Qy}2yY{r*t$M$-()+G!sMf}{ecY;p_%KysTKJN$5R+U*$JF*odV0$ZqfgLf z`Do%VEhN;jVD%&H@pGPfO3Ym`^pwhAhSgy{diUMv3HvoWkV2$dDp`zQ4bNT{93Oiw z$X>6vr?=|(>tu@jzNEWR|EQ(;MBDFvNIyJ}r-$qwp?!$?6mI+t0g47N2ZO0 z&EIP3zox76TU0AkjUZ{sBBlt)Mf6&~`*_0@WBRGWqEVEePM5!#5K`M(hLmENpmy5m$gEKk}h+f)#MX9#(LeQE>Pd-R$9?{gcvCL3Jyg?@o#B z^#j~r6$n%iiH(iD<;6?RqsT%Ip5h|yVAwNXPJ~r3`lb%layfZW5lGKF>r zy|TWZhHxl)=m3i3{{of~P@HA`(Np;p@3E$3V@8&*%~)(6fcaVQQn4NMAjm4YZ1V1; z+MG-^L&U7umoI52>3<0w4dkH34{ia{u$=5Om>OD|#=X7Y@L^J*nWBYG$T4$HGsN!T+yp}9R3M(y+U8(xGtdhZ| zrZgB>acgJ5NI6tFfp(*E)Cs|s6c_dV5pG)tK&^QLGR+C$6c9`uL&X#98}p%`mMG(M z6yhuKbVbkIOG`ZlUGr)$PX#ONZhQNeFJ33Ti+$E1g=x4Qn!uO7K#M1yM%Rq4jx!7M z5n*O(RF-%z>Xu|P=(qYz$24p7TYkm6!YSGWSegq%4Ysk5eGYJS7RdHG88jLDjfXy| zN^)Gr((Fr5*S5TWYr6&%S97eVC7Z_v}|WXP6rJ0 zLKep2m8l>tx#x~r$xl@z!=Js5rbMp#oFFK(5X`4>PE&u@mj&=H#rOSSnk)c~0C?K>~bcv}J0o&XSUv%`fvE^*h~1 zy>^0{e0*ZU@>j8SSY)K9yFQ%nM(3k{hgW9aEiP6sgEoIy4i7|E#ovO(y?DZ!8ATe2 zFO4gXeRMx`ckl#FvJb{f;NWOdyqDrK_gBQiOUnclknmc~_8!7s<>G{1@CFfsp-_nAkBWZe5c|-)5$+dvT`j)w!-cm$8ib zwia(Y=Kk22a7#4i#@8d$m?uXX^|Rsi*xz$HC2d^6!GL{w!rJXMZ^%0wPK3Gvy%t^S zz;Pzid*@04v2d8_Dr4RDT2RJm*o^Az3KR|XLf_#WL|Tvmqq(?$OGSiS!By>L3;^q@ z$p_2by0BVuhd{_D0xpEtp7ZbmJ%z*6dl%Q57Y4OS6q~`LnHUDt$hgSAlXsYBt zS0Nls0rF^5TQb>~2&Do5{a--fCd5BF9E8DUJc{GM4_Nt0I8irTQC5cX7%NJg1>W~_ z06c=~+ZWd?v6P)s>5n{$mXsrN1Bcw_?z-fx z$u&l<^Lm#3o_t!lII-Y^?C2?npD}9*iAC39GY7y~pFPJ6;N^wvnQoZTQnOM5yzVL6 z;UC3S>OU&DcuNXf%5_}a8V5IWr+rLnhVBN}_<4muadz35I?&E@zpC`1>Tr$1WsGwZ zTiwPmR(=<`bgQQEKcDpZT_Ni=ocCtxxjBA#pgik#OT*ANegnP%eMmCmgE1XVK32>< zA#@<@B5~+LQt$#aJ~itnitzGq*lU`3+FRQAjX#QuxNs48eeaT^1rHp9gAL-q8BTASwRKkBtxW z_Mf~`uv$|l;fO3&^Ue>Y5wRY#iY>2$dYx+O+XhTn-ELoy_xj*<_|PE~UVkV_)0Vzs zoG-Pq|AUVee>qh2QBB{8h$~d>DL8WFD?{<)oTPBK(~mQ~1L*#;HFaQ*0%Z_rR8VOl z5(t0zNNqf+6ifav?GxskqJB(i{`WMcu02(D%>D|;eHm{$XRoD+w4iIX zBgXLf&kfPYY^FADjg5`XCqoBEznqF0&ad3UUpsHFHMaWSR0mp{##-^(KzUcOKz;qq z9??^Et42OJ3vPQ*|zDT3MZU{YB17nzUFVD0|4Jv#UUAc3X-<_c+X}$r zv$7#nPiVyp8j_^-{GJP=M_OX!%iYze{Fd*8cHFpkFl3wZzPDX}GMDQdhuiYF8t|M` z;#n=t_DYIuXl|4hx*O95AVt=Ph1{|l(_Gg|Z;HN!=|a0K?deev9gfB>WlfdtY}uO07&sCo|KLnmpCX9BS|uJ&|mb)YPSZ0tVr# z3^1i62!;>g?6pb!h?=F94 zK)D}{R&C%m_6D!)g-?2hhH}tviEK7E#alkGr(TH92oBQDkcV8)eM>q z9L}>wE<3Yk+_f84emxKiBi12L?16$qmWP~0|jcumq`&Y zRwkA~BXLdXYmGkddiOz2CSfl7K>H2BZqvlHlhVAhuQnd_Z+Z>5P@qOkidLJiKry68 zxusC0ZnXzOUQnDqG1~A}jfv5H!y2Z7kIc`AsiH4I;0el7{oFw;JD$~u)1qUhXtHvV zZ?z9PSO83BR;2U?=hP+KcIn_(b`X~q7`;bjY>d!JiH;t98$#nmCSTH+*jXlsqyo|f zaH;1=@_DLY^X3C!vC=}V1WYw81>bI3Z{Ni ze4Q`wqH(mtz}ZKPk~z?p0PRy&HlPWl+@}NQ`^yY2ncN%39j}NJ0VJ;(o$Q<(nC zTOEFP+Hwx`xA z8_3i(&pWv@ga6TG0QdB4ClLzvV)weBgmmM*Qe99`(DXpLC@|7AFyK8$5d>iE`>+$A zntG~|7zHwaBlXSc-Kmr^MT@=dt~FnYjam@WfhWIVvNd6Fh~YT0wBFy9|Fdl9flZ&- z$b4B^f}E)llS<8%4h?A007B^f>wEVvKzN5L1Y2iwXOCnV=itv=-iu!7 z5yzm-(`>1o+?e(sEIs532s+FGuyi5Nza9yqV6Q{1<6x=j&bRP-?=+0|MYDFUaHh64 ze`F-Vl4LZ-`E8Kjcsy}+D8M)#S^jC9RTTSolYdyL%5qGA#r& zU(h?G!|SOT6^%`)(g~gZ)y+lfN^t~2uXb&Zf1_Pc38#9B9Gh)>vPvi|Tajf0dBS5( z#!TXDr~CAc-RJRq^Uanua%_C>SRdD0(C?3%s=5l8D2%XqjYKcIZhr9}G|u*@K}&U|Pz&b8}g7%W=1b#Dv2G9>;IourPW^SgC*5 zyfte+XdZdz3w3zq>IoXaL(Rty{~O zSk~?A|H z5)r=Clx7A$eepJ;`vr2+);B^DF2ovRaX8wq#EbKF*h!+KX|r#$-VGE~DA*o$Sap1-f_S*FzRSpjy&2p;gb)$zr9Yjt#Ud+FR`B4yFYcEze*_xG` z+vyfEHsiEHo;aDDS6=}1i|&O!Pq;pG>o5(CHJbR`WX7gOy}(8f zimy0^)Wc?GHQxLjIs#pV+zA(dC1%Sb_lNHHDYhoDG`X%FQBy1Xlp=GVKlm6GXY0^= zeKzT*uvcmKz|^)&vn<7v7)=6k48p{+!g4p=x0OB)sg>%)ebXw+fdFae396;f3|UhW z_j_-=!##yFwmDk}`6=|X;d2(7YQTYMpIfj0V`fP@sklp4I7d!WsHc(_=pP>I*6L@~ z8U4>Err;CK%g^#ESL{qL)N&axM$qPlBsN3;M|VXSc|o>OPZL_;Xu25_#`#c?r%(Vym(o&vtVS#}NnK9%{%E@()QsHv`pBpF$BX6QOS7`$0rguG05>?^?c907p>Xp5lKvhb+}w<84e8(?4y z4a65Z6+ZAAmCZ80A-%PD{OaQHu)91|k|C!0{6;1G-z>%EMVN%srvu`5sLD&rW|fv6 zgEy23bTJT!`M!6!23S6T`5xNeUD1N_^w~jH(Bb?m;QrfRRUg-wmAjH5r-E~0Hda)q z0};@wqb``@?buhZf&t^*CldhU?ELcX!Ig}kF(@>-3~h#J#}F>DPf@=YVa1@P?Cd6h zyCDvJrNV=P=g%tx$sR3P?hYU9rF0XG3=ly$>Y&$(<{<%lhjjtj36*UW(nWJZ>_&y8 zS>}H(<^s-IelO}Mj!_W>Yfv6O{7^sd+g8!O9>yPJ|9=4mAtlSh$M?;B2aNLQC_XDI zE7agvDTBXpgAQgQ+-C>9eA+M!jSB>TN6wr7)eCsP8ojqF-!o`{7Z~{1Kp?RI=sq~+ z2c$j}6`|~1MZo1k)}7mj$G}_he0x(5aW{Z&HY$OK>qapnU|Xx2DYt2jKi4aQKEFjB zYeXGRS=dA1mZ25kOrYHT9H;%QWCz~af(&O z`^Joo&mb@=C_thDs&Kx+V}!FH=U!=-j~W@VK>K81`cMMMQ&;QqStz>0=pDn|!4()o zwbQtt1+tGufL75lF_FXP!2)6=Blxcb-wC5+&<_K$s2+UQjCKwalC!p;r9?3qAWup& ztM-Lw*no~81TFd5-rwKc){^2+f-w~!kxb`r-H5oLdz@V12q@V?p?L|mu_Z8tTkh{} zq0?@L{8uzG6ZEak@q6MR-H!TilMBQ{saL-fsp4vIm=IWw1;t~03n7GjWFwE?my&;cFK2xPCyE{&BO@OItqe50}n z9*U)Kn{2rZp2{wbykfYiX`n)f+nzQ!HPsUL5Xxt0UIE3Q90LM7l>I|Nw5IEJ zJ@1R5{PkkzE0(+qJ?Z(;O!hmAaLi+eXBP#nnTFeN@0R1I}}hk zDS&i(g!11>3Uny+k?M?`97;&q^($EP-VkS>gCh(|W3Snv@r`ne;7Cog_+FI&!ATpzSuL2`r%D0-0O9;WFK{O+;E@kr3byaUWFn1aj+EwA`Kood6X^neMfNYReh%iToz3YRf*^lho1JPwEP@KKjg^e0~};l@S23eoT+nz$D*VGUI1$XmPaIzYxrT? z7Xb?faOPnLBSWTp-*KaV8KinUdjDpYE&)T{0(w~=|J&n>-T|n-2J~pX;0c!5TkWP( zw1Btcnncss^78T$xU8%|{ssBLH3?NlW+VyqW&O_^v|qW$5D|KX)9zBJqKql(0RD@v zKgELOWYj_fs0|n#nAghQxqk{e*C@RliZSX@pxZ`0t{k{kda$^sb1$~Nmxtv6%1RM0;pFJM3wRa< z2qs_I#SoiE7h#3>mfm{_LXm*ozp5zN5_0jZCUkTHe$NB+*(d|spwK1Y38v#QWTTKr zfAqMIo(yY$9)b0P8;BZ44z0CSB0@rR@Pb}}DWq`nkSgTnql15-{zO&Q<_5i4lqCb$ z1Mm;0f#LR=#8-abjfDu<+j@S#1H{82I6LcM9_%j9E=B=vtV7JQB!KsgO7EuK2(L=d z$oM?DcmkGEdS}j!@LW~Ufpz8^io#dKlb|CXfC?%M!zvybPZ_#yN~54pr=h2?w`V4d zGoor)y_gCC}Tc@t2{gfzt6(qzk~sH zW1)L@?i@(G&z?OiG$;>D6m&`-#5UX_B1hrPZ-jE%L(mbW!(BvC)KI8VKe$zJE^(~{ zDCICzO9(Yx!Z@Eg3!&Hh0d@l)h2g!7w)4#VxUE^x-~gRa(BXynEtF?hB;?Xw(^6We z0FePn;2;Mc;vOz8Zf+F|i&SsOFh9fdg*{bOa(e;X0nv`}#I0EoAw^0Q$m0Ub?lEkc zomm