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 d34d296..8dc318e 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 231d1af..c273b53 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__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc index 6e99341..d4129a7 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/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py index 0261b2b..5c35ea5 100644 --- a/Sampling-based Planning/rrt_3D/rrt3D.py +++ b/Sampling-based Planning/rrt_3D/rrt3D.py @@ -45,7 +45,7 @@ class rrtstar(): if not isCollide(self, xnearest, xnew): self.V.append(xnew) # add point self.wireup(xnew, xnearest) - # visualization(self) + visualization(self) self.i += 1 self.ind += 1 if getDist(xnew, self.env.goal) <= 1: diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py index 3734e8b..318f38a 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py @@ -26,7 +26,7 @@ class rrtstar(): self.maxiter = 10000 # at least 4000 in this env self.stepsize = 0.5 self.gamma = 500 - self.eta = 1.1*self.stepsize + self.eta = 2*self.stepsize self.Path = [] self.done = False @@ -61,7 +61,7 @@ class rrtstar(): if not isCollide(self,xnearest,xnew): Xnear = near(self,xnew) self.V.append(xnew) # add point - # visualization(self) + visualization(self) # minimal path and minimal cost xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew) # connecting along minimal cost path diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index 75ee073..a63385e 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -40,9 +40,9 @@ def sampleFree(initparams): if isinside(initparams, x): return sampleFree(initparams) else: - #if i < 0.05: - # return initparams.env.goal+0.01 - #else: return np.array(x) + if i < 0.05: + return initparams.env.goal+0.01 + else: return np.array(x) return np.array(x) diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index c0a8df6..85c5f06 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -12,11 +12,11 @@ import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") from Search_3D.env3D import env -from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ +from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ cost from Search_3D.plot_util3D import visualization import queue - +import time class Weighted_A_star(object): def __init__(self, resolution=0.5): @@ -29,7 +29,7 @@ class Weighted_A_star(object): self.env = env(resolution=resolution) self.Space = StateSpace(self) # key is the point, store g value self.start, self.goal = getNearest(self.Space, self.env.start), getNearest(self.Space, self.env.goal) - self.AABB = getAABB(self.env.blocks) + # self.AABB = getAABB(self.env.blocks) self.Space[hash3D(getNearest(self.Space, self.start))] = 0 # set g(x0) = 0 self.h = Heuristic(self.Space, self.goal) @@ -120,5 +120,7 @@ class Weighted_A_star(object): if __name__ == '__main__': + sta = time.time() Astar = Weighted_A_star(1) Astar.run() + print(time.time() - sta) \ No newline at end of file diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search-based Planning/Search_3D/LP_Astar3D.py new file mode 100644 index 0000000..e465ed0 --- /dev/null +++ b/Search-based Planning/Search_3D/LP_Astar3D.py @@ -0,0 +1,190 @@ +import numpy as np +import matplotlib.pyplot as plt + +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") +from Search_3D.env3D import env +from Search_3D import Astar3D +from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isinbound, isinball, hash3D, dehash, \ + cost, obstacleFree +from Search_3D.plot_util3D import visualization +import queue +import pyrr +import time + + +class Lifelong_Astar(object): + def __init__(self,resolution = 1): + 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.g = StateSpace(self) + self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal) + self.x0, self.xt = hash3D(self.start), hash3D(self.goal) + self.v = StateSpace(self) # rhs(.) = g(.) = inf + self.v[hash3D(self.start)] = 0 # rhs(x0) = 0 + self.h = Heuristic(self.g, self.goal) + + self.OPEN = queue.QueuePrior() # store [point,priority] + self.OPEN.put(self.x0, [self.h[self.x0],0]) + self.CLOSED = set() + + # used for A* + self.done = False + self.Path = [] + self.V = [] + self.ind = 0 + + # initialize children list + self.CHILDREN = {} + self.getCHILDRENset() + + # initialize cost list + self.COST = {} + _ = self.costset() + + def costset(self): + NodeToChange = set() + for strxi in self.CHILDREN.keys(): + children = self.CHILDREN[strxi] + xi = dehash(strxi) + toUpdate = [self.cost(xj,xi) for xj in children] + if strxi in self.COST: + # if the old cost not equal to new cost + diff = np.not_equal(self.COST[strxi],toUpdate) + cd = np.array(children)[diff] + for i in cd: + NodeToChange.add(hash3D(i)) + self.COST[strxi] = toUpdate + else: + self.COST[strxi] = toUpdate + return NodeToChange + + def getCOSTset(self,strxi,xj): + ind, children = 0, self.CHILDREN[strxi] + for i in children: + if all(i == xj): + return self.COST[strxi][ind] + ind += 1 + + + def children(self, x): + allchild = [] + resolution = self.env.resolution + for direc in self.Alldirec: + child = np.array(list(map(np.add,x,np.multiply(direc,resolution)))) + if isinbound(self.env.boundary,child): + allchild.append(child) + return allchild + + def getCHILDRENset(self): + for strxi in self.g.keys(): + xi = dehash(strxi) + self.CHILDREN[strxi] = self.children(xi) + + def isCollide(self, x, child): + ray , dist = getRay(x, child) , getDist(x, child) + if not isinbound(self.env.boundary,child): + return True, dist + for i in self.env.AABB: + 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, dist + for i in self.env.balls: + if isinball(i, child): + return True, dist + 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, dist + return False, dist + + def cost(self, x, y): + collide, dist = self.isCollide(x, y) + if collide: return np.inf + else: return dist + + def key(self,strxi,epsilion = 1): + return [min(self.g[strxi],self.v[strxi]) + epsilion*self.h[strxi],min(self.g[strxi],self.v[strxi])] + + def path(self): + path = [] + strx = self.xt + strstart = self.x0 + ind = 0 + while strx != strstart: + j = dehash(strx) + nei = self.CHILDREN[strx] + gset = [self.g[hash3D(xi)] for xi in nei] + # collision check and make g cost inf + for i in range(len(nei)): + if self.isCollide(nei[i],j)[0]: + gset[i] = np.inf + parent = nei[np.argmin(gset)] + path.append([dehash(strx), parent]) + strx = hash3D(parent) + if ind > 100: + break + ind += 1 + return path + + #------------------Lifelong Plannning A* + def UpdateMembership(self,strxi, xi, xparent=None): + if strxi != self.x0: + self.v[strxi] = min([self.g[hash3D(j)] + self.getCOSTset(strxi,j) for j in self.CHILDREN[strxi]]) + self.OPEN.check_remove(strxi) + if self.g[strxi] != self.v[strxi]: + self.OPEN.put(strxi,self.key(strxi)) + + def ComputePath(self): + print('computing path ...') + while self.key(self.xt) > self.OPEN.top_key() or self.v[self.xt] != self.g[self.xt]: + strxi = self.OPEN.get() + xi = dehash(strxi) + # if g > rhs, overconsistent + if self.g[strxi] > self.v[strxi]: + self.g[strxi] = self.v[strxi] + # add xi to expanded node set + if strxi not in self.CLOSED: + self.V.append(xi) + self.CLOSED.add(strxi) + else: # underconsistent and consistent + self.g[strxi] = np.inf + self.UpdateMembership(strxi, xi) + for xj in self.CHILDREN[strxi]: + strxj = hash3D(xj) + self.UpdateMembership(strxj, xj) + + # visualization(self) + self.ind += 1 + self.Path = self.path() + self.done = True + visualization(self) + plt.pause(2) + + def change_env(self): + self.env.change() + self.done = False + self.Path = [] + self.CLOSED = set() + N = self.costset() + for strxi in N: + xi = dehash(strxi) + self.UpdateMembership(strxi,xi) + +if __name__ == '__main__': + sta = time.time() + Astar = Lifelong_Astar(1) + Astar.ComputePath() + Astar.change_env() + Astar.ComputePath() + plt.show() + print(time.time() - sta) \ No newline at end of file diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index 51a805c..d2dd224 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -13,7 +13,7 @@ import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") from Search_3D.env3D import env from Search_3D import Astar3D -from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ +from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ cost, obstacleFree from Search_3D.plot_util3D import visualization import queue @@ -79,5 +79,5 @@ class LRT_A_star2: if __name__ == '__main__': - T = LRT_A_star2(resolution=0.5, N=1) + T = LRT_A_star2(resolution=0.5, N=150) T.run() diff --git a/Search-based Planning/Search_3D/RTA_Astar3D.py b/Search-based Planning/Search_3D/RTA_Astar3D.py index 728b25d..aaf41aa 100644 --- a/Search-based Planning/Search_3D/RTA_Astar3D.py +++ b/Search-based Planning/Search_3D/RTA_Astar3D.py @@ -13,7 +13,7 @@ import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") from Search_3D.env3D import env from Search_3D import Astar3D -from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ +from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ cost, obstacleFree from Search_3D.plot_util3D import visualization import queue @@ -74,5 +74,5 @@ class RTA_A_star: if __name__ == '__main__': - T = RTA_A_star(resolution=0.5, N=500) + T = RTA_A_star(resolution=0.5, N=100) T.run() \ No newline at end of file diff --git a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc index 32a2674..fa68160 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc differ 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 7b46412..07487bb 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 3be010f..382a864 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 e2fddd0..e89e676 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 f20c254..91f0836 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/env3D.py b/Search-based Planning/Search_3D/env3D.py index f3700fa..754e208 100644 --- a/Search-based Planning/Search_3D/env3D.py +++ b/Search-based Planning/Search_3D/env3D.py @@ -22,6 +22,13 @@ def getblocks(): Obstacles.append([j for j in i]) return np.array(Obstacles) +def getAABB(blocks): + # used for Pyrr package for detecting collision + AABB = [] + for i in blocks: + AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger + return AABB + def getballs(): spheres = [[16,2.5,4,2],[10,2.5,1,1]] Obstacles = [] @@ -29,19 +36,23 @@ def getballs(): Obstacles.append([j for j in i]) return np.array(Obstacles) +def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): + return block + class env(): def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): self.resolution = resolution self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) self.blocks = getblocks() + self.AABB = getAABB(self.blocks) self.balls = getballs() self.start = np.array([0.5, 2.5, 5.5]) self.goal = np.array([19.0, 2.5, 5.5]) - def visualize(self): - # fig = plt.figure() - # TODO: do visualizations - return + def change(self): + newblock = add_block() + self.blocks = np.vstack([self.blocks,newblock]) + self.AABB = getAABB(self.blocks) if __name__ == '__main__': diff --git a/Search-based Planning/Search_3D/queue.py b/Search-based Planning/Search_3D/queue.py index 8f481ae..9531c36 100644 --- a/Search-based Planning/Search_3D/queue.py +++ b/Search-based Planning/Search_3D/queue.py @@ -60,3 +60,48 @@ class QueuePrior: def enumerate(self): return self.queue + + def check_remove(self, item): + for (p, x) in self.queue: + if item == x: + self.queue.remove((p, x)) + + def top_key(self): + return self.queue[0][0] + + +# class QueuePrior: +# """ +# Class: QueuePrior +# Description: QueuePrior reorders elements using value [priority] +# """ + +# def __init__(self): +# self.queue = [] + +# def empty(self): +# return len(self.queue) == 0 + +# def put(self, item, priority): +# count = 0 +# for (p, x) in self.queue: +# if x == item: +# self.queue[count] = (priority, item) +# break +# count += 1 +# if count == len(self.queue): +# heapq.heappush(self.queue, (priority, item)) # reorder x using priority + +# def get(self): +# return heapq.heappop(self.queue)[1] # pop out the smallest item + +# def enumerate(self): +# return self.queue + +# def check_remove(self, item): +# for (p, x) in self.queue: +# if item == x: +# self.queue.remove((p, x)) + +# def top_key(self): +# return self.queue[0][0] \ No newline at end of file diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index 2ac5aa8..1e2330b 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -5,12 +5,6 @@ def getRay(x, y): direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] return np.array([x, direc]) -def getAABB(blocks): - AABB = [] - for i in blocks: - AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger - return AABB - def getDist(pos1, pos2): return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) @@ -75,7 +69,7 @@ def isCollide(initparams, x, direc): ray , dist = getRay(x, child) , getDist(x, child) if not isinbound(initparams.env.boundary,child): return True, child - for i in initparams.AABB: + for i in initparams.env.AABB: shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) if shot is not None: dist_wall = getDist(x, shot)