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

LRTA*

Browse Source
This commit is contained in:
yue qi
2020-06-29 11:48:17 -07:00
parent 5ba30a3a2f
commit 16e4f3ed28
3 changed files with 109 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/Search_3D/Astar3D.py
+13 -7
View File
@@ -35,6 +35,7 @@ class Weighted_A_star(object):
self.V = []
self.done = False
self.Path = []
self.ind = 0
def children(self,x):
allchild = []
@@ -44,10 +45,9 @@ class Weighted_A_star(object):
allchild.append(child)
return allchild
def run(self):
def run(self, N=None):
x0, xt = hash3D(self.start), hash3D(self.goal)
self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # item, priority = g + h
self.ind = 0
while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty
strxi = self.OPEN.get()
xi = dehash(strxi)
@@ -69,12 +69,18 @@ class Weighted_A_star(object):
else:
# add xj in to OPEN set
self.OPEN.put(strxj, a+1*self.h[strxj])
# For specified expanded nodes, used primarily in LRTA*
if N is not None:
if len(self.V) % N == 0:
break
if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
self.ind += 1
self.done = True
self.Path = self.path()
visualization(self)
plt.show()
# if the path finding is finished
if xt in self.CLOSED:
self.done = True
self.Path = self.path()
visualization(self)
plt.show()
def path(self):
path = []
@@ -87,5 +93,5 @@ class Weighted_A_star(object):
return path
if __name__ == '__main__':
Astar = Weighted_A_star(0.5)
Astar = Weighted_A_star(1)
Astar.run()
Search-based Planning/Search_3D/LRT_Astar3D.py
+96 -70
View File
@@ -1,4 +1,4 @@
# this is the three dimensional LRTA* algo
# this is the three dimensional near-sighted 1 neighborhood LRTA* algo
# !/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@@ -12,84 +12,110 @@ import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
from Search_3D.env3D import env
from Search_3D.Astar3D import Weighted_A_star
from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost
from Search_3D.plot_util3D import visualization
import queue
class LRT_A_star(object):
def __init__(self,resolution=0.5):
self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\
[-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\
[1,-1,0],[-1,1,0],[1,0,-1],[-1,0, 1],[0,1, -1],[0, -1,1],\
[1,-1,-1],[-1,1,-1],[-1,-1,1],[1,1,-1],[1,-1,1],[-1,1,1]])
self.env = env(resolution = resolution)
self.Space = StateSpace(self)
self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
self.AABB = getAABB(self.env.blocks)
self.Space[hash3D(getNearest(self.Space,self.start))] = 0
self.OPEN = queue.QueuePrior()
self.h = Heuristic(self.Space,self.goal) # 1. initialize heuristic h = h0
self.Child = {}
self.CLOSED = set()
self.V = []
self.done = False
self.Path = []
# class LRT_A_star1(object):
# def __init__(self,resolution=0.5):
# self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\
# [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\
# [1,-1,0],[-1,1,0],[1,0,-1],[-1,0, 1],[0,1, -1],[0, -1,1],\
# [1,-1,-1],[-1,1,-1],[-1,-1,1],[1,1,-1],[1,-1,1],[-1,1,1]])
# self.env = env(resolution = resolution)
# self.Space = StateSpace(self)
# self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
# self.AABB = getAABB(self.env.blocks)
# self.Space[hash3D(getNearest(self.Space,self.start))] = 0 # this is g
# self.OPEN = queue.QueuePrior()
# self.h = Heuristic(self.Space,self.goal) # 1. initialize heuristic h = h0
# self.Child = {}
# self.CLOSED = set()
# self.V = []
# self.done = False
# self.Path = []
def children(self,x):
allchild = []
for j in self.Alldirec:
collide,child = isCollide(self,x,j)
if not collide:
allchild.append(child)
return allchild
# def children(self,x):
# allchild = []
# for j in self.Alldirec:
# collide,child = isCollide(self,x,j)
# if not collide:
# allchild.append(child)
# return allchild
def step(self, xi, strxi):
childs = self.children(xi) # 4. generate depth 1 neighborhood S(s,1) = {s' in S | norm(s,s') = 1}
fvals = [cost(xi,i) + self.h[hash3D(i)] for i in childs]
xj , fmin = childs[np.argmin(fvals)], min(fvals) # 5. compute h'(s) = min(dist(s,s') + h(s'))
strxj = hash3D(xj)
# add the child of xi
self.Child[strxi] = xj
if fmin >= self.h[strxi]: # 6. if h'(s) > h(s) then update h(s) = h'(s)
self.h[strxi] = fmin
# TODO: action to move to xj
self.OPEN.put(strxj, self.h[strxj]) # 7. update current state s = argmin (dist(s,s') + h(s'))
# def step(self, xi, strxi):
# childs = self.children(xi) # 4. generate depth 1 neighborhood S(s,1) = {s' in S | norm(s,s') = 1}
# fvals = [cost(xi,i) + self.h[hash3D(i)] for i in childs]
# xj , fmin = childs[np.argmin(fvals)], min(fvals) # 5. compute h'(s) = min(dist(s,s') + h(s'))
# strxj = hash3D(xj)
# # add the child of xi
# self.Child[strxi] = xj
# if fmin >= self.h[strxi]: # 6. if h'(s) > h(s) then update h(s) = h'(s)
# self.h[strxi] = fmin
# # TODO: action to move to xj
# self.OPEN.put(strxj, self.h[strxj]) # 7. update current state s = argmin (dist(s,s') + h(s'))
# def run(self):
# x0 = hash3D(self.start)
# xt = hash3D(self.goal)
# self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # 2. reset the current state
# self.ind = 0
# while xt not in self.CLOSED and self.OPEN: # 3. while s not in Sg do
# strxi = self.OPEN.get()
# xi = dehash(strxi)
# self.CLOSED.add(strxi)
# self.V.append(xi)
# visualization(self)
# if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
# self.ind += 1
# self.done = True
# self.Path = self.path()
# visualization(self)
# plt.show()
# def path(self):
# # this is a suboptimal path.
# path = []
# strgoal = hash3D(self.goal)
# strx = hash3D(self.start)
# ind = 0
# while strx != strgoal:
# path.append([dehash(strx),self.Child[strx]])
# strx = hash3D(self.Child[strx])
# ind += 1
# if ind == 1000:
# return np.flip(path,axis=0)
# path = np.flip(path,axis=0)
# return path
class LRT_A_star2():
def __init__(self,resolution=0.5, N=7):
self.lookahead = N
self.Astar = Weighted_A_star()
self.Astar.env.resolution = resolution
def expand(self):
self.Astar.run(self.lookahead)
def updateHeuristic(self):
for strxi in self.Astar.CLOSED:
self.Astar.h[strxi] = np.inf
xi = dehash(strxi)
self.Astar.h[strxi] = min([cost(xi,xj) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)])
#def move(self):
# print(np.argmin([j[0] for j in self.Astar.OPEN.enumerate()]))
def run(self):
x0 = hash3D(self.start)
xt = hash3D(self.goal)
self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # 2. reset the current state
self.ind = 0
while xt not in self.CLOSED and self.OPEN: # 3. while s not in Sg do
strxi = self.OPEN.get()
xi = dehash(strxi)
self.CLOSED.add(strxi)
self.V.append(xi)
visualization(self)
self.step(xi , strxi)
if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
self.ind += 1
self.done = True
self.Path = self.path()
visualization(self)
plt.show()
xt = hash3D(self.Astar.goal)
while xt not in self.Astar.CLOSED:
self.expand()
#self.updateHeuristic()
def path(self):
# this is a suboptimal path.
path = []
strgoal = hash3D(self.goal)
strx = hash3D(self.start)
ind = 0
while strx != strgoal:
path.append([dehash(strx),self.Child[strx]])
strx = hash3D(self.Child[strx])
ind += 1
if ind == 1000:
return np.flip(path,axis=0)
path = np.flip(path,axis=0)
return path
if __name__ == '__main__':
Astar = LRT_A_star(0.5)
Astar.run()
T = LRT_A_star2(resolution = 1, N = 2)
T.run()
Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc
BIN
View File
Binary file not shown.