From a21f5af45753c6ae1e8fed84c3d07ee12e0ae7b1 Mon Sep 17 00:00:00 2001 From: Karan Date: Mon, 11 Jun 2018 15:34:56 -0500 Subject: [PATCH] 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()