saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-30 00:50:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

update informed rrt*

Browse Source
This commit is contained in:
zhm-real
2020-08-03 11:11:34 -07:00
parent 9a5f8199c5
commit db149a2d63
4 changed files with 69 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Sampling_based_Planning/rrt_2D/batch_informed_trees.py
+18 -26
View File
@@ -6,7 +6,6 @@ Batch Informed Trees (BIT*)
import os
import sys
import math
import copy
import random
import numpy as np
import matplotlib.pyplot as plt
@@ -66,7 +65,6 @@ class BITStar:
self.g_T = dict()
def init(self):
print("init")
self.Tree.V.add(self.x_start)
self.X_sample.add(self.x_goal)
@@ -141,33 +139,13 @@ class BITStar:
self.Tree.QV = set()
if k % 5 == 0:
self.draw(xCenter, self.g_T[self.x_goal], cMin, theta)
self.animation(xCenter, self.g_T[self.x_goal], cMin, theta)
path_x, path_y = self.ExtractPath()
plt.plot(path_x, path_y, linewidth=2, color='r')
plt.pause(0.01)
# test
plt.show()
def draw(self, xCenter, cMax, cMin, theta):
plt.cla()
self.plot_grid("Batch Informed Trees (BIT*)")
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
for v in self.X_sample:
plt.plot(v.x, v.y, marker='.', color='lightgrey', markersize='2')
if cMax < np.inf:
self.draw_ellipse(xCenter, cMax, cMin, theta)
for v, w in self.Tree.E:
plt.plot([v.x, w.x], [v.y, w.y], '-g')
plt.pause(0.01)
def ExtractPath(self):
node = self.x_goal
path_x, path_y = [node.x], [node.y]
@@ -335,9 +313,23 @@ class BITStar:
dy = node_end.y - node_start.y
return math.hypot(dx, dy), math.atan2(dy, dx)
def animation(self, name, cBest):
theta, cMin, xCenter, C = self.init()
self.draw_ellipse(xCenter, cBest, cMin, theta)
def animation(self, xCenter, cMax, cMin, theta):
plt.cla()
self.plot_grid("Batch Informed Trees (BIT*)")
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
for v in self.X_sample:
plt.plot(v.x, v.y, marker='.', color='lightgrey', markersize='2')
if cMax < np.inf:
self.draw_ellipse(xCenter, cMax, cMin, theta)
for v, w in self.Tree.E:
plt.plot([v.x, w.x], [v.y, w.y], '-g')
plt.pause(0.001)
def plot_grid(self, name):
Sampling_based_Planning/rrt_2D/informed_rrt_star.py
+51 -38
View File
@@ -51,42 +51,55 @@ class IRrtStar:
self.X_soln = set()
self.path = None
def planning(self):
c_best = np.inf
dist, theta = self.get_distance_and_angle(self.x_start, self.x_goal)
C = self.RotationToWorldFrame(self.x_start, self.x_goal, dist)
x_center = np.array([[(self.x_start.x + self.x_goal.x) / 2.0],
[(self.x_start.y + self.x_goal.y) / 2.0], [0.0]])
def init(self):
cMin, theta = self.get_distance_and_angle(self.x_start, self.x_goal)
C = self.RotationToWorldFrame(self.x_start, self.x_goal, cMin)
xCenter = np.array([[(self.x_start.x + self.x_goal.x) / 2.0],
[(self.x_start.y + self.x_goal.y) / 2.0], [0.0]])
x_best = self.x_start
return theta, cMin, xCenter, C, x_best
def planning(self):
theta, dist, x_center, C, x_best = self.init()
c_best = np.inf
for k in range(self.iter_max):
x_rand = self.Sample(self.x_start, self.x_goal, c_best, x_center, C)
if self.X_soln:
cost = {node: self.Cost(node) for node in self.X_soln}
x_best = min(cost, key=cost.get)
c_best = cost[x_best]
x_rand = self.Sample(c_best, dist, x_center, C)
x_nearest = self.Nearest(self.V, x_rand)
x_new = self.Steer(x_nearest, x_rand)
if x_new and not self.utils.is_collision(x_nearest, x_new):
X_near = self.Near(self.V, x_new)
c_min = self.Cost(x_new)
c_min = self.Cost(x_nearest) + self.Line(x_nearest, x_new)
self.V.append(x_new)
# choose parent
for x_near in X_near:
c_new = self.Cost(x_near) + self.Line(x_near, x_new)
if c_new < c_min:
x_new.parent = x_near
c_min = c_new
# rewire
for x_near in X_near:
c_near = self.Cost(x_near)
c_new = c_min + self.Line(x_new, x_near)
c_new = self.Cost(x_new) + self.Line(x_new, x_near)
if c_new < c_near:
x_near.parent = x_new
if self.InGoalRegion(x_new):
self.X_soln.add(x_new)
new_cost = self.Cost(x_new) + self.Line(x_new, self.x_goal)
if new_cost < c_best:
c_best = new_cost
x_best = x_new
if not self.utils.is_collision(x_new, self.x_goal):
self.X_soln.add(x_new)
# new_cost = self.Cost(x_new) + self.Line(x_new, self.x_goal)
# if new_cost < c_best:
# c_best = new_cost
# x_best = x_new
if k % 20 == 0:
self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta)
@@ -108,34 +121,40 @@ class IRrtStar:
def Near(self, nodelist, node):
n = len(nodelist) + 1
r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
r = 50 * math.sqrt((math.log(n) / n))
dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]
X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and
not self.utils.is_collision(node, nodelist[ind])]
not self.utils.is_collision(nodelist[ind], node)]
return X_near
def Sample(self, x_start, x_goal, c_max, x_center, C):
def Sample(self, c_max, c_min, x_center, C):
if c_max < np.inf:
c_min = self.Line(x_start, x_goal)
r = [c_max / 2.0,
math.sqrt(c_max ** 2 - c_min ** 2) / 2.0,
math.sqrt(c_max ** 2 - c_min ** 2) / 2.0]
L = np.diag(r)
while True:
x_ball = self.SampleUnitNBall()
x_rand = C @ L @ x_ball + x_center
x_ball = self.SampleUnitBall()
x_rand = np.dot(np.dot(C, L), x_ball) + x_center
if self.x_range[0] + self.delta <= x_rand[0] <= self.x_range[1] - self.delta and \
self.y_range[0] + self.delta <= x_rand[1] <= self.y_range[1] - self.delta:
break
x_rand = Node((x_rand[0], x_rand[1]))
x_rand = Node((x_rand[(0, 0)], x_rand[(1, 0)]))
else:
x_rand = self.SampleFreeSpace()
return x_rand
@staticmethod
def SampleUnitBall():
while True:
x, y = random.uniform(-1, 1), random.uniform(-1, 1)
if x ** 2 + y ** 2 < 1:
return np.array([[x], [y], [0.0]])
def SampleFreeSpace(self):
delta = self.delta
@@ -173,14 +192,6 @@ class IRrtStar:
return C
@staticmethod
def SampleUnitNBall():
while True:
x, y = random.uniform(-1, 1), random.uniform(-1, 1)
if x ** 2 + y ** 2 < 1:
return np.array([[x], [y], [0.0]])
@staticmethod
def Nearest(nodelist, n):
return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2
@@ -190,12 +201,14 @@ class IRrtStar:
def Line(x_start, x_goal):
return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y)
@staticmethod
def Cost(node):
cost = 0.0
if node.parent is None:
return cost
def Cost(self, node):
if node == self.x_start:
return 0.0
if node.parent is None:
return np.inf
cost = 0.0
while node.parent:
cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
node = node.parent
@@ -215,13 +228,13 @@ class IRrtStar:
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if c_best != np.inf:
self.draw_ellipse(x_center, c_best, dist, theta)
for node in self.V:
if node.parent:
plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
if c_best != np.inf:
self.draw_ellipse(x_center, c_best, dist, theta)
plt.pause(0.01)
def plot_grid(self, name):
@@ -284,7 +297,7 @@ def main():
x_start = (18, 8) # Starting node
x_goal = (37, 18) # Goal node
rrt_star = IRrtStar(x_start, x_goal, 10, 0.10, 20, 1000)
rrt_star = IRrtStar(x_start, x_goal, 1, 0.10, 12, 1000)
rrt_star.planning()