diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc index 59a5593..083efd8 100644 Binary files a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc and b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py b/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py new file mode 100644 index 0000000..7a8b934 --- /dev/null +++ b/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py @@ -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 diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py index bc58a02..3edaa9c 100644 --- a/Sampling-based Planning/rrt_3D/rrt3D.py +++ b/Sampling-based Planning/rrt_3D/rrt3D.py @@ -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 diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py index 6e6eb43..37013e9 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py @@ -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: diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index ddf2510..7df2910 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -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): diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search-based Planning/Search_3D/Anytime_Dstar3D.py index 3bc1ad1..43c0307 100644 --- a/Search-based Planning/Search_3D/Anytime_Dstar3D.py +++ b/Search-based Planning/Search_3D/Anytime_Dstar3D.py @@ -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() diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search-based Planning/Search_3D/DstarLite3D.py index 7982d44..d328f64 100644 --- a/Search-based Planning/Search_3D/DstarLite3D.py +++ b/Search-based Planning/Search_3D/DstarLite3D.py @@ -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): diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc index 5f01f13..1db5f8f 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index 0cd9905..5c51c89 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -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)