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

'dynamic_rrt'

Browse Source
This commit is contained in:
yue qi
2020-08-04 15:34:15 -07:00
parent 36cc2ff837
commit b3f190ec36
14 changed files with 110 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.idea/PathPlanning.iml
Generated
+1 -2
View File
@@ -2,11 +2,10 @@
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="inheritedJdk" />
<orderEntry type="jdk" jdkName="Python 3.7 (base)" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="TestRunnerService">
<option name="projectConfiguration" value="pytest" />
<option name="PROJECT_TEST_RUNNER" value="pytest" />
</component>
</module>
.idea/misc.xml
Generated
+1 -1
View File
@@ -1,4 +1,4 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (base)" project-jdk-type="Python SDK" />
</project>
Sampling_based_Planning/rrt_2D/__pycache__/env.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling_based_Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling_based_Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling_based_Planning/rrt_2D/rrt.py
+2 -1
View File
@@ -41,6 +41,7 @@ class Rrt:
self.obs_boundary = self.env.obs_boundary
def planning(self):
print("z")
for i in range(self.iter_max):
node_rand = self.generate_random_node(self.goal_sample_rate)
node_near = self.nearest_neighbor(self.vertex, node_rand)
@@ -102,7 +103,7 @@ def main():
x_start = (2, 2) # Starting node
x_goal = (49, 24) # Goal node
rrt = Rrt(x_start, x_goal, 0.5, 0.00, 10000)
rrt = Rrt(x_start, x_goal, 0.5, 0.05, 10000)
path = rrt.planning()
if path:
Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling_based_Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
+58 -49
View File
@@ -12,14 +12,16 @@ import matplotlib.pyplot as plt
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based_Planning/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
from rrt_3D.env3D import env
from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, cost, path, edgeset, isinbound, isinside
from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, cost, path, edgeset, isinbound, \
isinside
from rrt_3D.rrt3D import rrt
from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
class dynamic_rrt_3D():
class dynamic_rrt_3D:
def __init__(self):
self.env = env()
self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
@@ -27,18 +29,20 @@ class dynamic_rrt_3D():
self.current = tuple(self.env.start)
self.stepsize = 0.25
self.maxiter = 10000
self.GoalProb = 0.05 # probability biased to the goal
self.WayPointProb = 0.05 # probability falls back on to the way points
self.GoalProb = 0.05 # probability biased to the goal
self.WayPointProb = 0.02 # probability falls back on to the way points
self.done = False
self.invalid = False
self.V = [] # vertices
self.Parent = {} # parent child relation
self.Edge = set() # edge relation (node, parent node) tuple
self.V = [] # vertices
self.Parent = {} # parent child relation
self.Edge = set() # edge relation (node, parent node) tuple
self.Path = []
self.flag = {}# flag dictionary
self.flag = {} # flag dictionary
self.ind = 0
self.i = 0
#--------Dynamic RRT algorithm
# --------Dynamic RRT algorithm
def RegrowRRT(self):
self.TrimRRT()
self.GrowRRT()
@@ -57,14 +61,14 @@ class dynamic_rrt_3D():
i += 1
self.CreateTreeFromNodes(S)
print('trimming complete...')
def InvalidateNodes(self, obstacle):
Edges = self.FindAffectedEdges(obstacle)
for edge in Edges:
qe = self.ChildEndpointNode(edge)
self.flag[qe] = 'Invalid'
#--------Extend RRT algorithm-----
# --------Extend RRT algorithm-----
def initRRT(self):
self.V.append(self.x0)
self.flag[self.x0] = 'Valid'
@@ -72,12 +76,11 @@ class dynamic_rrt_3D():
def GrowRRT(self):
print('growing')
qnew = self.x0
tree = None
distance_threshold = self.stepsize
self.ind = 0
while self.ind <= self.maxiter:
qtarget = self.ChooseTarget()
qnearest = self.Nearest(tree, qtarget)
qnearest = self.Nearest(qtarget)
qnew, collide = self.Extend(qnearest, qtarget)
if not collide:
self.AddNode(qnearest, qnew)
@@ -96,14 +99,14 @@ class dynamic_rrt_3D():
if len(self.V) == 1:
i = 0
else:
i = np.random.randint(0, high = len(self.V) - 1)
i = np.random.randint(0, high=len(self.V) - 1)
if 0 < p < self.GoalProb:
return self.xt
elif self.GoalProb < p < self.GoalProb + self.WayPointProb:
return self.V[i]
elif self.GoalProb + self.WayPointProb < p < 1:
return tuple(self.RandomState())
def RandomState(self):
# generate a random, obstacle free state
xrand = sampleFree(self, bias=0)
@@ -115,16 +118,16 @@ class dynamic_rrt_3D():
self.Edge.add((extended, nearest))
self.flag[extended] = 'Valid'
def Nearest(self, tree, target):
def Nearest(self, target):
# TODO use kdTree to speed up search
return nearest(self, target, isset=True)
def Extend(self, nearest, target):
extended, dist = steer(self, nearest, target, DIST = True)
extended, dist = steer(self, nearest, target, DIST=True)
collide, _ = isCollide(self, nearest, target, dist)
return extended, collide
#--------Main function
# --------Main function
def Main(self):
# qstart = qgoal
self.x0 = tuple(self.env.goal)
@@ -132,33 +135,36 @@ class dynamic_rrt_3D():
self.xt = tuple(self.env.start)
self.initRRT()
self.GrowRRT()
self.Path, D = path(self)
self.Path, D = self.path()
self.done = True
self.visualization()
plt.show()
self.visualization()
t = 0
while True:
# move the block while the robot is moving
new, _ = self.env.move_block(a=[0, 0, -0.2], mode='translation')
self.InvalidateNodes(new)
self.TrimRRT()
# if solution path contains invalid node
self.done = True
self.visualization()
plt.show()
invalid = self.PathisInvalid(self.Path)
if invalid:
self.invalid = self.PathisInvalid(self.Path)
if self.invalid:
self.done = False
self.RegrowRRT()
self.Path = []
self.Path, D = path(self)
self.Path, D = self.path()
self.done = True
self.visualization()
if t == 8:
break
t += 1
self.visualization()
plt.show()
#--------Additional utility functions
# --------Additional utility functions
def FindAffectedEdges(self, obstacle):
# scan the graph for the changed edges in the tree.
# return the end point and the affected
print('finding affected edges')
Affectededges = []
for e in self.Edge:
child, parent = e
@@ -171,44 +177,48 @@ class dynamic_rrt_3D():
return edge[0]
def CreateTreeFromNodes(self, Nodes):
self.V = []
Parent = {}
edges = set()
for v in Nodes:
self.V.append(v)
Parent[v] = self.Parent[v]
edges.add((v, Parent[v]))
self.Parent = Parent
self.Edge = edges
print('creating tree')
# self.Parent = {node: self.Parent[node] for node in Nodes}
self.V = [node for node in Nodes]
self.Edge = {(node, self.Parent[node]) for node in Nodes}
# if self.invalid:
# del self.Parent[self.xt]
def PathisInvalid(self, path):
for edge in path:
if self.flag[tuple(edge[0])] == 'Invalid' or self.flag[tuple(edge[1])] == 'Invalid':
return True
def path(self, Path=[], dist=0):
def path(self, dist=0):
Path=[]
x = self.xt
i = 0
while x != self.x0:
x2 = self.Parent[x]
Path.append(np.array([x, x2]))
dist += getDist(x, x2)
x = x2
if i > 10000:
print('Path is not found')
return
i+= 1
return Path, dist
#--------Visualization specialized for dynamic RRT
# --------Visualization specialized for dynamic RRT
def visualization(self):
if self.ind % 100 == 0 or self.done:
V = np.array(self.V)
Path = np.array(self.Path)
start = self.env.start
goal = self.env.goal
edges = []
for i in self.Parent:
edges.append([i,self.Parent[i]])
# edges = []
# for i in self.Parent:
# edges.append([i, self.Parent[i]])
edges = np.array([list(i) for i in self.Edge])
ax = plt.subplot(111, projection='3d')
# ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
# ax.view_init(elev=0., azim=90.)
ax.view_init(elev=8., azim=120.)
ax.view_init(elev=0., azim=90.)
ax.clear()
# drawing objects
draw_Spheres(ax, self.env.balls)
@@ -229,13 +239,12 @@ class dynamic_rrt_3D():
dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
make_transparent(ax)
#plt.xlabel('x')
#plt.ylabel('y')
# plt.xlabel('x')
# plt.ylabel('y')
ax.set_axis_off()
plt.pause(0.0001)
if __name__ == '__main__':
rrt = dynamic_rrt_3D()
rrt.Main()
Sampling_based_Planning/rrt_3D/plot_util3D.py
+1 -1
View File
@@ -106,7 +106,7 @@ def visualization(initparams):
# ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
# ax.view_init(elev=0., azim=90.)
ax.view_init(elev=8., azim=120.)
ax.view_init(elev=8., azim=90.)
# ax.view_init(elev=-8., azim=180)
ax.clear()
# drawing objects
Sampling_based_Planning/rrt_3D/rrt_connect3D.py
+46 -21
View File
@@ -18,6 +18,23 @@ from rrt_3D.env3D import env
from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
class Tree():
def __init__(self, node):
self.V = []
self.Parent = {}
self.V.append(node)
# self.Parent[node] = None
def add_vertex(self, node):
if node not in self.V:
self.V.append(node)
def add_edge(self, parent, child):
# here edge is defined a tuple of (parent, child) (qnear, qnew)
self.Parent[child] = parent
class rrt_connect():
def __init__(self):
self.env = env()
@@ -33,6 +50,7 @@ class rrt_connect():
self.qgoal = tuple(self.env.goal)
self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
self.qnew = None
self.done = False
self.ind = 0
self.fig = plt.figure(figsize=(10, 8))
@@ -78,7 +96,7 @@ class rrt_connect():
collide, _ = isCollide(self, qnear, qnew, dist = dist)
return not collide
#----------RRT connect algorithm
#----------RRT connect algorithm
def CONNECT(self, Tree, q):
print('in connect')
while True:
@@ -97,13 +115,14 @@ class rrt_connect():
qnew = self.qnew # get qnew from outside
if self.CONNECT(Tree_B, qnew) == 'Reached':
print('reached')
# return self.PATH(Tree_A, Tree_B)
self.done = True
self.Path = self.PATH(Tree_A, Tree_B)
self.visualization(Tree_A, Tree_B, k)
plt.show()
return
else:
print('not reached')
# return
Tree_A, Tree_B = self.SWAP(Tree_A, Tree_B)
self.visualization(Tree_A, Tree_B, k)
print('Failure')
return 'Failure'
# def PATH(self, tree_a, tree_b):
@@ -111,10 +130,29 @@ class rrt_connect():
tree_a, tree_b = tree_b, tree_a
return tree_a, tree_b
def PATH(self, tree_a, tree_b):
qnew = self.qnew
patha = []
pathb = []
while True:
patha.append((tree_a.Parent[qnew], qnew))
qnew = tree_a.Parent[qnew]
if qnew == self.qinit or qnew == self.qgoal:
break
qnew = self.qnew
while True:
pathb.append((tree_b.Parent[qnew], qnew))
qnew = tree_b.Parent[qnew]
if qnew == self.qinit or qnew == self.qgoal:
break
return patha + pathb
#----------RRT connect algorithm
def visualization(self, tree_a, tree_b, index):
if (index % 10 == 0 and index != 0) or self.done:
if (index % 20 == 0 and index != 0) or self.done:
# a_V = np.array(tree_a.V)
# b_V = np.array(tree_b.V)
Path = self.Path
start = self.env.start
goal = self.env.goal
a_edges, b_edges = [], []
@@ -132,32 +170,19 @@ class rrt_connect():
draw_block_list(ax, np.array([self.env.boundary]), alpha=0)
draw_line(ax, a_edges, visibility=0.75, color='g')
draw_line(ax, b_edges, visibility=0.75, color='y')
# draw_line(ax, Path, color='r')
draw_line(ax, Path, color='r')
ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
xmin, xmax = self.env.boundary[0], self.env.boundary[3]
ymin, ymax = self.env.boundary[1], self.env.boundary[4]
zmin, zmax = self.env.boundary[2], self.env.boundary[5]
dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
dx, dy, _ = xmax - xmin, ymax - ymin, zmax - zmin
ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
make_transparent(ax)
ax.set_axis_off()
plt.pause(0.0001)
class Tree():
def __init__(self, node):
self.V = []
self.Parent = {}
self.V.append(node)
# self.Parent[node] = None
def add_vertex(self, node):
if node not in self.V:
self.V.append(node)
def add_edge(self, parent, child):
# here edge is defined a tuple of (parent, child) (qnear, qnew)
self.Parent[child] = parent
if __name__ == '__main__':
p = rrt_connect()
Sampling_based_Planning/rrt_3D/utils3D.py
+1
View File
@@ -196,6 +196,7 @@ def steer(initparams, x, y, DIST=False):
if np.equal(x, y).all():
return x, 0.0
dist, step = getDist(y, x), initparams.stepsize
step = min(dist, step)
increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step)
xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2])
# direc = (y - x) / np.linalg.norm(y - x)