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'

Browse Source
This commit is contained in:
yue qi
2020-07-26 14:44:35 -07:00
parent 256812d165
commit d229bceaaf
9 changed files with 128 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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
+70
View File
@@ -0,0 +1,70 @@
"""
This is dynamic rrt code for 3D
@author: yue qi
"""
import numpy as np
from numpy.matlib import repmat
from collections import defaultdict
import time
import matplotlib.pyplot as plt
import os
import sys
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, visualization, cost, path, edgeset
class dynamic_rrt_3D:
def __init__(self):
self.env = env()
self.Parent = {}
self.E = edgeset() # edgeset
self.V = [] # nodeset
self.i = 0
self.maxiter = 2000 # at least 2000 in this env
self.stepsize = 0.5
self.gamma = 500
self.eta = 2*self.stepsize
self.Path = []
self.done = False
def RegrowRRT(self):
self.TrimRRT()
self.GrowRRT()
def TrimRRT(self):
S = set()
i = 1
for qi in self.V:
qp = self.Parent(qi)
if qp.flag == 'Invalid':
qi.flag = 'Invalid'
if qi.flag != 'Invalid':
S.add(qi)
i += 1
self.V, self.E = self.CreateTreeFromNodes(S)
def InvalidateNodes(self, obstacle):
E = self.FindAffectedEdges(obstacle)
for e in E:
qe = self.ChildEndpointNode(e)
qe.flag = 'Invalid'
def GrowRRT(self):
# TODO
pass
def CreateTreeFromNodes(self, S):
#TODO
pass
def FindAffectedEdges(self, obstacle):
#TODO
pass
def ChildEndpointNode(self):
#TODO
pass
Sampling-based Planning/rrt_3D/rrt3D.py
+1
View File
@@ -29,6 +29,7 @@ class rrtstar():
self.stepsize = 0.5
self.Path = []
self.done = False
self.x0 = tuple(self.env.start)
def wireup(self, x, y):
self.E.add_edge([x, y]) # add edge
Sampling-based Planning/rrt_3D/rrtstar3D.py
+3 -2
View File
@@ -23,12 +23,13 @@ class rrtstar():
self.E = edgeset()
self.V = []
self.i = 0
self.maxiter = 2000 # at least 2000 in this env
self.maxiter = 12000 # at least 2000 in this env
self.stepsize = 0.5
self.gamma = 500
self.eta = 2*self.stepsize
self.Path = []
self.done = False
self.x0 = tuple(self.env.start)
def wireup(self,x,y):
self.E.add_edge([x,y]) # add edge
@@ -52,7 +53,7 @@ class rrtstar():
def run(self):
self.V.append(tuple(self.env.start))
self.ind = 0
xnew = tuple(self.env.start)
xnew = self.x0
print('start rrt*... ')
self.fig = plt.figure(figsize = (10,8))
while self.ind < self.maxiter:
Sampling-based Planning/rrt_3D/utils3D.py
+4 -5
View File
@@ -37,14 +37,14 @@ def getDist(pos1, pos2):
'''
def sampleFree(initparams):
def sampleFree(initparams, bias = 0.1):
'''biased sampling'''
x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6])
i = np.random.random()
if isinside(initparams, x):
return sampleFree(initparams)
else:
if i < 0.1:
if i < bias:
return initparams.env.goal + 1
else:
return x
@@ -172,10 +172,9 @@ def steer(initparams, x, y):
def cost(initparams, x):
'''here use the additive recursive cost function'''
if x == tuple(initparams.env.start):
if x == initparams.x0:
return 0
xparent = initparams.Parent[x]
return cost(initparams, xparent) + getDist(x, xparent)
return cost(initparams, initparams.Parent[x]) + getDist(x, initparams.Parent[x])
def path(initparams, Path=[], dist=0):
Search-based Planning/Search_3D/Anytime_Dstar3D.py
+23 -21
View File
@@ -91,28 +91,30 @@ class Anytime_Dstar(object):
# scan graph for changed cost, if cost is changed update it
CHANGED = set()
for xi in self.CLOSED:
if xi in self.CHILDREN:
oldchildren = self.CHILDREN[xi] # A
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
newchildren = set(children(self, xi)) # B
removed = oldchildren.difference(newchildren)
intersection = oldchildren.intersection(newchildren)
added = newchildren.difference(oldchildren)
self.CHILDREN[xi] = newchildren
for xj in removed:
self.COST[xi][xj] = cost(self, xi, xj)
for xj in intersection.union(added):
self.COST[xi][xj] = cost(self, xi, xj)
CHANGED.add(xi)
else:
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
CHANGED.add(xi)
children_added = set(children(self, xi))
self.CHILDREN[xi] = children_added
for xj in children_added:
self.COST[xi][xj] = cost(self, xi, xj)
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
newchildren = set(children(self, xi)) # B
self.CHILDREN[xi] = newchildren
for xj in newchildren:
self.COST[xi][xj] = cost(self, xi, xj)
CHANGED.add(xi)
return CHANGED
# def updateGraphCost(self, range_changed=None, new=None, old=None, mode=False):
# # TODO scan graph for changed cost, if cost is changed update it
# # make the graph cost via vectorization
# CHANGED = set()
# Allnodes = np.array(list(self.CLOSED))
# isChanged = isinbound(old, Allnodes, mode = mode, isarray = True) & \
# isinbound(new, Allnodes, mode = mode, isarray = True)
# Changednodes = Allnodes[isChanged]
# for xi in Changednodes:
# xi = tuple(xi)
# CHANGED.add(xi)
# self.CHILDREN[xi] = set(children(self, xi))
# for xj in self.CHILDREN:
# self.COST[xi][xj] = cost(self, xi, xj)
# --------------main functions for Anytime D star
def key(self, s, epsilon=1):
@@ -228,5 +230,5 @@ class Anytime_Dstar(object):
if __name__ == '__main__':
AD = Anytime_Dstar(resolution=0.5)
AD = Anytime_Dstar(resolution=1)
AD.Main()
Search-based Planning/Search_3D/DstarLite3D.py
+7 -21
View File
@@ -50,30 +50,16 @@ class D_star_Lite(object):
self.Path = []
self.done = False
def updatecost(self,range_changed=None, new=None, old=None, mode=False):
def updatecost(self, range_changed=None, new=None, old=None, mode=False):
# scan graph for changed cost, if cost is changed update it
CHANGED = set()
for xi in self.CLOSED:
if xi in self.CHILDREN:
oldchildren = self.CHILDREN[xi]# A
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
newchildren = set(children(self,xi))# B
removed = oldchildren.difference(newchildren)
intersection = oldchildren.intersection(newchildren)
added = newchildren.difference(oldchildren)
self.CHILDREN[xi] = newchildren
for xj in removed:
self.COST[xi][xj] = cost(self, xi, xj)
for xj in intersection.union(added):
self.COST[xi][xj] = cost(self, xi, xj)
CHANGED.add(xi)
else:
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
CHANGED.add(xi)
children_added = set(children(self,xi))
self.CHILDREN[xi] = children_added
for xj in children_added:
self.COST[xi][xj] = cost(self, xi, xj)
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
newchildren = set(children(self, xi)) # B
self.CHILDREN[xi] = newchildren
for xj in newchildren:
self.COST[xi][xj] = cost(self, xi, xj)
CHANGED.add(xi)
return CHANGED
def getcost(self, xi, xj):
Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_3D/utils3D.py
+20 -17
View File
@@ -39,23 +39,32 @@ def heuristic_fun(initparams, k, t=None):
t = initparams.goal
return max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])])
def isinbound(i, x, mode=False, factor = 0):
def isinbound(i, x, mode = False, factor = 0, isarray = False):
if mode == 'obb':
return isinobb(i, x)
if i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] + factor:
return True
return False
return isinobb(i, x, isarray)
if isarray:
compx = (i[0] - factor <= x[:,0]) & (x[:,0] < i[3] + factor)
compy = (i[1] - factor <= x[:,1]) & (x[:,0] < i[4] + factor)
compz = (i[2] - factor <= x[:,2]) & (x[:,0] < i[5] + factor)
return compx & compy & compz
else:
return i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] + factor
def isinball(i, x, factor = 0):
if getDist(i[0:3], x) <= i[3] + factor:
return True
return False
def isinobb(i, x):
def isinobb(i, x, isarray = False):
# transform the point from {W} to {body}
pt = i.T@np.append(x,1)
block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]]
return isinbound(block, pt)
if isarray:
pts = (i.T@np.column_stack((x, np.ones(len(x)))).T).T[:,0:3]
block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]]
return isinbound(block, pts, isarray = isarray)
else:
pt = i.T@np.append(x,1)
block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]]
return isinbound(block, pt)
def OBB2AABB(obb):
# https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
@@ -244,7 +253,6 @@ def StateSpace(env, factor=0):
Space.add((x, y, z))
return Space
def g_Space(initparams):
'''This function is used to get nodes and discretize the space.
State space is by x*y*z,3 where each 3 is a point in 3D.'''
@@ -254,7 +262,6 @@ def g_Space(initparams):
g[v] = np.inf # this hashmap initialize all g values at inf
return g
def isCollide(initparams, x, child, dist):
'''see if line intersects obstacle'''
'''specified for expansion in A* 3D lookup table'''
@@ -277,7 +284,6 @@ def isCollide(initparams, x, child, dist):
return True, dist
return False, dist
def children(initparams, x, settings = 0):
# get the neighbor of a specific state
allchild = []
@@ -299,7 +305,6 @@ def children(initparams, x, settings = 0):
if settings == 1:
return allcost
def obstacleFree(initparams, x):
for i in initparams.env.blocks:
if isinbound(i, x):
@@ -345,11 +350,9 @@ if __name__ == "__main__":
obb1 = obb([2.6,2.5,1],[0.2,2,2],R_matrix(0,0,45))
# obb2 = obb([1,1,0],[1,1,1],[[1/np.sqrt(3)*1,1/np.sqrt(3)*1,1/np.sqrt(3)*1],[np.sqrt(3/2)*(-1/3),np.sqrt(3/2)*2/3,np.sqrt(3/2)*(-1/3)],[np.sqrt(1/8)*(-2),0,np.sqrt(1/8)*2]])
p0, p1 = [2.9,2.5,1],[1.9,2.5,1]
dist = getDist(p0,p1)
pts = np.array([[1,2,3],[4,5,6],[7,8,9],[2,2,2],[1,1,1],[3,3,3]])
start = time.time()
for i in range(3000*27):
lineAABB(p0,p1,dist,obb1)
#lineOBB(p0,p1,dist,obb1)
isinbound(obb1, pts, mode='obb', factor = 0, isarray = True)
print(time.time() - start)