diff --git a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc index 66c29a4..71f8a09 100644 Binary files a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc and b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc index 754c2e4..7cb6a21 100644 Binary files a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc new file mode 100644 index 0000000..f8582b6 Binary files /dev/null and b/Sampling-based Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc differ 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 cadb6c7..f8a989c 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..c1654aa --- /dev/null +++ b/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py @@ -0,0 +1,121 @@ +""" +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, isinbound +from rrt_3D.rrt3D import rrt + +class dynamic_rrt_3D(rrt): + + def __init__(self): + # variables in rrt + self.env = env() + self.Parent = {} + self.E = edgeset() # edgeset + self.V = [] # nodeset + self.i = 0 + self.maxiter = 10000 # 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.goal) + self.xt = tuple(self.env.start) + # additional variables + self.Flag = {} + self.xrobot = tuple(self.env.start) + + self.V.append(self.x0) + self.ind = 0 + self.fig = plt.figure(figsize=(10, 8)) + + def RegrowRRT(self, xrobot): + self.TrimRRT() + self.GrowRRT(xrobot = xrobot) + + def TrimRRT(self): + S = [] + i = 1 + for qi in self.V: + if qi == self.x0: + continue + qp = self.Parent[qi] + if qp == self.x0: + continue + if self.Flag[qp] == 'Invalid': + self.Flag[qi] = 'Invalid' + self.E.remove_edge([qi,qp]) # REMOVE edge that parent is invalid + if self.Flag[qi] != 'Invalid': + S.append(qi) + i += 1 + self.V, self.E = self.CreateTreeFromNodes(S) + + def InvalidateNodes(self, obstacle, mode): + E = self.FindAffectedEdges(obstacle, mode) + for e in E: + qe = self.ChildEndpointNode(e) + self.Flag[qe] = 'Invalid' + + def Main(self): + qgoal = tuple(self.env.start) + qstart = tuple(self.env.goal) + self.GrowRRT() + self.done = True + # visualization(self) + self.done = False + # change the enviroment + new0,old0 = self.env.move_block(a=[0, 0, -2], s=0.5, block_to_move=1, mode='translation') + while qgoal != qstart: + qgoal = self.Parent[qgoal] + # TODO move to qgoal and check for new obs + xrobot = qstart + # TODO if any new obstacle are observed + self.InvalidateNodes(new0, mode = 'translation') + for xi in self.Path: + if self.Flag[tuple(xi[0])] == 'Invalid': + self.RegrowRRT(tuple(self.env.start)) + self.done = True + self.Path, D = path(self) + visualization(self) + plt.show() + + def GrowRRT(self, Reversed = True, xrobot = None): + # rrt.run() + self.run(Reversed = Reversed, xrobot = xrobot) + + def CreateTreeFromNodes(self, S): + return S, self.E + + def FindAffectedEdges(self, obstacle, mode): + # if the nodes appears inside of the new range an obstacle moved, + # find the edges associated with this node. (the node is the parent) + nodes = np.array(self.V) + if mode == 'translation': + affected_nodes = nodes[isinbound(obstacle, nodes, isarray = True)] + elif mode == 'rotation': + affected_nodes = nodes[isinbound(obstacle, nodes, mode = 'obb', isarray = True)] + if len(affected_nodes) == 0: + return [] + return self.E.get_edge(affected_nodes) + + def ChildEndpointNode(self, e): + # if self.E.isEndNode(e[1]): + return e[1] + #else: + # return self.ChildEndpointNode(e[1]) + +if __name__ == '__main__': + rrt = dynamic_rrt_3D() + rrt.Main() diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py index 3ea9729..c128965 100644 --- a/Sampling-based Planning/rrt_3D/rrt3D.py +++ b/Sampling-based Planning/rrt_3D/rrt3D.py @@ -14,52 +14,74 @@ 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, \ - hash3D, dehash +from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset -class rrtstar(): +class rrt(): def __init__(self): self.env = env() - self.Parent = defaultdict(lambda: defaultdict(dict)) + self.Parent = {} self.V = [] self.E = edgeset() self.i = 0 self.maxiter = 10000 - self.stepsize = 1.0 + self.stepsize = 0.5 self.Path = [] self.done = False + self.x0 = tuple(self.env.start) + self.xt = tuple(self.env.goal) + self.Flag = None + + + self.ind = 0 + self.fig = plt.figure(figsize=(10, 8)) def wireup(self, x, y): self.E.add_edge([x, y]) # add edge - self.Parent[hash3D(x)] = y + self.Parent[x] = y - def run(self): - self.V.append(self.env.start) - self.ind = 0 - self.fig = plt.figure(figsize=(10, 8)) - xnew = self.env.start - while self.ind < self.maxiter and getDist(xnew, self.env.goal) > 1: + def run(self, Reversed = True, xrobot = None): + if Reversed: + if xrobot is None: + self.x0 = tuple(self.env.goal) + self.xt = tuple(self.env.start) + else: + self.x0 = tuple(self.env.goal) + self.xt = xrobot + xnew = self.env.goal + else: + xnew = self.env.start + self.V.append(self.x0) + while self.ind < self.maxiter: xrand = sampleFree(self) xnearest = nearest(self, xrand) xnew = steer(self, xnearest, xrand) - if not isCollide(self, xnearest, xnew): + collide, _ = isCollide(self, xnearest, xnew) + if not collide: self.V.append(xnew) # add point + if self.Flag is not None: + self.Flag[xnew] = 'Valid' self.wireup(xnew, xnearest) - visualization(self) + + if getDist(xnew, self.xt) <= self.stepsize: + self.wireup(self.xt, xnew) + self.Path, D = path(self) + print('Total distance = ' + str(D)) + if self.Flag is not None: + self.Flag[self.xt] = 'Valid' + break + # visualization(self) self.i += 1 self.ind += 1 - if getDist(xnew, self.env.goal) <= 1: - self.wireup(self.env.goal, xnew) - self.Path, D = path(self) - print('Total distance = ' + str(D)) - self.done = True - visualization(self) - plt.show() + # if the goal is really reached + + # self.done = True + # visualization(self) + # plt.show() if __name__ == '__main__': - p = rrtstar() + p = rrt() starttime = time.time() p.run() print('time used = ' + str(time.time() - starttime)) diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py index f712bf0..e22e781 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py @@ -13,8 +13,7 @@ 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, \ - hash3D, dehash +from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset class rrtstar(): @@ -24,70 +23,79 @@ class rrtstar(): self.E = edgeset() self.V = [] self.i = 0 - self.maxiter = 10000 # at least 4000 in this env + self.maxiter = 5000 # at least 2000 in this env self.stepsize = 0.5 self.gamma = 500 - self.eta = 2 * self.stepsize + self.eta = 2*self.stepsize self.Path = [] self.done = False + self.x0 = tuple(self.env.start) + self.xt = tuple(self.env.goal) - def wireup(self, x, y): - self.E.add_edge([x, y]) # add edge - self.Parent[hash3D(x)] = y + self.V.append(self.x0) + self.ind = 0 + def wireup(self,x,y): + self.E.add_edge([x,y]) # add edge + self.Parent[x] = y - def removewire(self, xnear): - xparent = self.Parent[hash3D(xnear)] - a = [xnear, xparent] - self.E.remove_edge(a) # remove and replace old the connection + def removewire(self,xnear): + xparent = self.Parent[xnear] + a = [xnear,xparent] + self.E.remove_edge(a) # remove and replace old the connection def reached(self): self.done = True - xn = near(self, self.env.goal) - c = [cost(self, x) for x in xn] + goal = self.xt + xn = near(self,self.env.goal) + c = [cost(self,tuple(x)) for x in xn] xncmin = xn[np.argmin(c)] - self.wireup(self.env.goal, xncmin) - self.V.append(self.env.goal) - self.Path, self.D = path(self) + self.wireup(goal , tuple(xncmin)) + self.V.append(goal) + self.Path,self.D = path(self) def run(self): - self.V.append(self.env.start) - self.ind = 0 - xnew = self.env.start + xnew = self.x0 print('start rrt*... ') - self.fig = plt.figure(figsize=(10, 8)) + self.fig = plt.figure(figsize = (10,8)) while self.ind < self.maxiter: - xrand = sampleFree(self) - xnearest = nearest(self, xrand) - xnew = steer(self, xnearest, xrand) - if not isCollide(self, xnearest, xnew): - Xnear = near(self, xnew) - self.V.append(xnew) # add point - visualization(self) + xrand = sampleFree(self) + xnearest = nearest(self,xrand) + xnew = steer(self,xnearest,xrand) + collide, _ = isCollide(self,xnearest,xnew) + if not collide: + Xnear = near(self,xnew) + self.V.append(xnew) # add point + # visualization(self) # minimal path and minimal cost xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew) # connecting along minimal cost path for xnear in Xnear: + xnear = tuple(xnear) c1 = cost(self, xnear) + getDist(xnew, xnear) - if not isCollide(self, xnew, xnear) and c1 < cmin: + collide, _ = isCollide(self, xnew, xnear) + if not collide and c1 < cmin: xmin, cmin = xnear, c1 self.wireup(xnew, xmin) # rewire for xnear in Xnear: + xnear = tuple(xnear) c2 = cost(self, xnew) + getDist(xnew, xnear) - if not isCollide(self, xnew, xnear) and c2 < cost(self, xnear): + collide, _ = isCollide(self, xnew, xnear) + if not collide and c2 < cost(self, xnear): self.removewire(xnear) self.wireup(xnear, xnew) self.i += 1 self.ind += 1 # max sample reached self.reached() - print('time used = ' + str(time.time() - starttime)) - print('Total distance = ' + str(self.D)) + print('time used = ' + str(time.time()-starttime)) + print('Total distance = '+str(self.D)) visualization(self) plt.show() - + if __name__ == '__main__': p = rrtstar() starttime = time.time() p.run() + diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index ac7c45f..107e16a 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -37,77 +37,152 @@ 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: - return initparams.env.goal + 1 + if i < bias: + return np.array(initparams.xt) + 1 else: - return np.array(x) - return np.array(x) - + return x + return x +# ---------------------- Collision checking algorithms def isinside(initparams, x): '''see if inside obstacle''' for i in initparams.env.blocks: - if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: + if isinbound(i, x): + return True + for i in initparams.env.OBB: + if isinbound(i, x, mode = 'obb'): + return True + for i in initparams.env.balls: + if isinball(i, x): return True return False +def isinbound(i, x, mode = False, factor = 0, isarray = False): + if mode == 'obb': + 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[:,1] < i[4] + factor) + compz = (i[2] - factor <= x[:,2]) & (x[:,2] < 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] -def isinbound(i, x): - if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: +def isinobb(i, x, isarray = False): + # transform the point from {W} to {body} + 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 isinball(i, x, factor = 0): + if getDist(i[0:3], x) <= i[3] + factor: return True return False +def lineSphere(p0, p1, ball): + # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf + c, r = ball[0:3], ball[-1] + line = [p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]] + d1 = [c[0] - p0[0], c[1] - p0[1], c[2] - p0[2]] + t = (1 / (line[0] * line[0] + line[1] * line[1] + line[2] * line[2])) * ( + line[0] * d1[0] + line[1] * d1[1] + line[2] * d1[2]) + if t <= 0: + if (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]) <= r ** 2: return True + elif t >= 1: + d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]] + if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True + elif 0 < t < 1: + x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]] + k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]] + if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True + return False -def isCollide(initparams, x, y): +def lineAABB(p0, p1, dist, aabb): + # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 + # aabb should have the attributes of P, E as center point and extents + mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point + I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction + hl = dist / 2 # radius + T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]] + # do any of the principal axis form a separting axis? + if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False + if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False + if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False + # I.cross(x axis) ? + r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1]) + if abs(T[1] * I[2] - T[2] * I[1]) > r: return False + # I.cross(y axis) ? + r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0]) + if abs(T[2] * I[0] - T[0] * I[2]) > r: return False + # I.cross(z axis) ? + r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0]) + if abs(T[0] * I[1] - T[1] * I[0]) > r: return False + + return True + +def lineOBB(p0, p1, dist, obb): + # transform points to obb frame + res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T + # record old position and set the position to origin + oldP, obb.P= obb.P, [0,0,0] + # calculate segment-AABB testing + ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb) + # reset the position + obb.P = oldP + return ans + +def isCollide(initparams, x, child, dist=None): '''see if line intersects obstacle''' - ray = getRay(x, y) - dist = getDist(x, y) - if not isinbound(initparams.env.boundary, y): - return True - for i in getAABB(initparams.env.blocks): - shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) - if shot is not None: - dist_wall = getDist(x, shot) - if dist_wall <= dist: # collide - return True + '''specified for expansion in A* 3D lookup table''' + if dist==None: + dist = getDist(x, child) + # check in bound + if not isinbound(initparams.env.boundary, child): + return True, dist + # check collision in AABB + for i in range(len(initparams.env.AABB)): + if lineAABB(x, child, dist, initparams.env.AABB[i]): + return True, dist + # check collision in ball for i in initparams.env.balls: - shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i) - if shot != []: - dists_ball = [getDist(x, j) for j in shot] - if all(dists_ball <= dist): # collide - return True - return False - + if lineSphere(x, child, i): + return True, dist + # check collision with obb + for i in initparams.env.OBB: + if lineOBB(x, child, dist, i): + return True, dist + return False, dist +# ---------------------- leaf node extending algorithms def nearest(initparams, x): V = np.array(initparams.V) if initparams.i == 0: return initparams.V[0] xr = repmat(x, len(V), 1) dists = np.linalg.norm(xr - V, axis=1) - return initparams.V[np.argmin(dists)] - - -def steer(initparams, x, y): - direc = (y - x) / np.linalg.norm(y - x) - xnew = x + initparams.stepsize * direc - return xnew - + return tuple(initparams.V[np.argmin(dists)]) def near(initparams, x): + x = np.array(x) + V = np.array(initparams.V) cardV = len(initparams.V) eta = initparams.eta gamma = initparams.gamma r = min(gamma * (np.log(cardV) / cardV), eta) - if initparams.done: r = 1 - V = np.array(initparams.V) + if initparams.done: + r = 1 if initparams.i == 0: return [initparams.V[0]] xr = repmat(x, len(V), 1) @@ -115,52 +190,80 @@ def near(initparams, x): nearpoints = V[inside] return np.array(nearpoints) +def steer(initparams, x, y): + dist, step = getDist(y, x), initparams.stepsize + 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) + # xnew = x + initparams.stepsize * direc + return xnew def cost(initparams, x): '''here use the additive recursive cost function''' - if all(x == initparams.env.start): + if x == initparams.x0: return 0 - xparent = initparams.Parent[hash3D(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): - x = initparams.env.goal - while not all(x == initparams.env.start): - x2 = initparams.Parent[hash3D(x)] + x = initparams.xt + while x != initparams.x0: + x2 = initparams.Parent[x] Path.append(np.array([x, x2])) dist += getDist(x, x2) x = x2 return Path, dist - -def hash3D(x): - return str(x[0]) + ' ' + str(x[1]) + ' ' + str(x[2]) - - -def dehash(x): - return np.array([float(i) for i in x.split(' ')]) - - class edgeset(object): def __init__(self): self.E = {} def add_edge(self, edge): - x, y = hash3D(edge[0]), hash3D(edge[1]) + x, y = edge[0], edge[1] if x in self.E: - self.E[x].append(y) + self.E[x].add(y) else: - self.E[x] = [y] + self.E[x] = set() + self.E[x].add(y) def remove_edge(self, edge): x, y = edge[0], edge[1] - self.E[hash3D(x)].remove(hash3D(y)) + self.E[x].remove(y) - def get_edge(self): + def get_edge(self, nodes = None): edges = [] - for v in self.E: - for n in self.E[v]: - # if (n,v) not in edges: - edges.append((dehash(v), dehash(n))) + if nodes is None: + for v in self.E: + for n in self.E[v]: + # if (n,v) not in edges: + edges.append((v, n)) + else: + for v in nodes: + for n in self.E[tuple(v)]: + edges.append((v, n)) return edges + + def isEndNode(self, node): + return node not in self.E + + +class Node: + def __init__(self, data): + self.data = data + self.sibling = None + self.child = None + +class Tree: + def __init__(self, start): + self.root = Node(start) + self.ind = 0 + self.index = {start:self.ind} + + def add_edge(self, edge): + # y exists in the tree while x does not + x, y = edge[0], edge[1] + + + + + diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search-based Planning/Search_3D/Anytime_Dstar3D.py index 026a1c0..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): @@ -197,7 +199,7 @@ class Anytime_Dstar(object): self.INCONS = set() self.CLOSED = set() self.ComputeorImprovePath() - # TODO publish current epsilon sub optimal solution + # publish current epsilon sub optimal solution self.Path = self.path() # if epsilon == 1: # wait for change to occur @@ -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 47bdd89..d328f64 100644 --- a/Search-based Planning/Search_3D/DstarLite3D.py +++ b/Search-based Planning/Search_3D/DstarLite3D.py @@ -50,29 +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) - 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__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index 727f29d..4b7036f 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc index 8478a15..af8baf3 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc index 4306b48..f0e6250 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc differ 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 2ad0952..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..b421f35 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[:,1] < i[4] + factor) + compz = (i[2] - factor <= x[:,2]) & (x[:,2] < 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)