diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 12b5e2e..da4c5c6 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/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.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ - cost + cost, children, StateSpace from Search_3D.plot_util3D import visualization import queue import time @@ -27,6 +27,7 @@ class Weighted_A_star(object): [1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 1]]) self.env = env(resolution=resolution) + self.X = StateSpace(self.env) self.g = g_Space(self) # key is the point, store g value self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal) # self.AABB = getAABB(self.env.blocks) @@ -44,13 +45,13 @@ class Weighted_A_star(object): self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0]) # item, priority = g + h self.lastpoint = self.x0 - 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 run(self, N=None): xt = self.xt @@ -60,12 +61,12 @@ class Weighted_A_star(object): if xi not in self.CLOSED: self.V.append(np.array(xi)) self.CLOSED.add(xi) # add the point in CLOSED set - visualization(self) - allchild = self.children(xi) + # visualization(self) + allchild = children(self,xi) for xj in allchild: if xj not in self.CLOSED: gi, gj = self.g[xi], self.g[xj] - a = gi + cost(xi, xj) + a = gi + cost(self, xi, xj) if a < gj: self.g[xj] = a self.Parent[xj] = xi @@ -87,9 +88,9 @@ class Weighted_A_star(object): if xt in self.CLOSED: self.done = True self.Path = self.path() - if N is None: - visualization(self) - plt.show() + # if N is None: + # visualization(self) + # plt.show() return True return False @@ -118,6 +119,6 @@ class Weighted_A_star(object): if __name__ == '__main__': sta = time.time() - Astar = Weighted_A_star(1) + Astar = Weighted_A_star(0.5) Astar.run() print(time.time() - sta) \ No newline at end of file diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index 2991238..3534379 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -3,64 +3,14 @@ import matplotlib.pyplot as plt import os import sys -from collections import defaultdict 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 StateSpace, getDist, getRay, isinbound, isinball +from Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, initcost import pyrr - -def isCollide(initparams, x, child): - '''see if line intersects obstacle''' - ray , dist = getRay(x, child) , getDist(x, child) - if not isinbound(initparams.env.boundary,child): - return True, dist - 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) - if dist_wall <= dist: # collide - return True, dist - for i in initparams.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 children(initparams, x): - # get the neighbor of a specific state - allchild = [] - resolution = initparams.env.resolution - for direc in initparams.Alldirec: - child = tuple(map(np.add,x,np.multiply(direc,resolution))) - if isinbound(initparams.env.boundary,child): - allchild.append(child) - return allchild - -def cost(initparams, x, y): - # get the cost between two points, - # do collision check here - collide, dist = isCollide(initparams,x,y) - if collide: return np.inf - else: return dist - -def initcost(initparams): - # initialize cost dictionary, could be modifed lateron - c = defaultdict(lambda: defaultdict(dict)) # two key dicionary - for xi in initparams.X: - cdren = children(initparams, xi) - for child in cdren: - c[xi][child] = cost(initparams, xi, child) - return c - - class D_star(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], diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index 528bbbb..dac6c0e 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -14,7 +14,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-base from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ - cost, obstacleFree + cost, obstacleFree, children from Search_3D.plot_util3D import visualization import queue @@ -34,8 +34,8 @@ class LRT_A_star2: for xi in self.Astar.CLOSED: lasthvals.append(self.Astar.h[xi]) # update h values if they are smaller - Children = self.Astar.children(xi) - minfval = min([cost(xi, xj, settings=0) + self.Astar.h[xj] for xj in Children]) + Children = children(self.Astar,xi) + minfval = min([cost(self.Astar,xi, xj, settings=0) + self.Astar.h[xj] for xj in Children]) # h(s) = h(s') if h(s) > c(s,s') + h(s') if self.Astar.h[xi] >= minfval: self.Astar.h[xi] = minfval @@ -47,9 +47,13 @@ class LRT_A_star2: ind = 0 # find the lowest path down hill while st in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop - Children = [i for i in self.Astar.children(st)] + Children = children(self.Astar,st) minh, minchild = np.inf, None for child in Children: + # check collision here, not a supper efficient + collide, _ = isCollide(self.Astar,st, child) + if collide: + continue h = self.Astar.h[child] if h <= minh: minh, minchild = h, child @@ -76,5 +80,5 @@ class LRT_A_star2: if __name__ == '__main__': - T = LRT_A_star2(resolution=0.5, N=150) + T = LRT_A_star2(resolution=0.5, N=100) T.run() diff --git a/Search-based Planning/Search_3D/RTA_Astar3D.py b/Search-based Planning/Search_3D/RTA_Astar3D.py index 88c18e0..818dd24 100644 --- a/Search-based Planning/Search_3D/RTA_Astar3D.py +++ b/Search-based Planning/Search_3D/RTA_Astar3D.py @@ -14,7 +14,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-base from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ - cost, obstacleFree + cost, obstacleFree, children from Search_3D.plot_util3D import visualization import queue @@ -48,7 +48,7 @@ class RTA_A_star: while sthval < maxhval: parentsvals , parents = [] , [] # find the max child - for xi in self.Astar.children(st): + for xi in children(self.Astar,st): if xi in self.Astar.CLOSED: parents.append(xi) parentsvals.append(self.Astar.h[xi]) 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 1e8e448..d3052cb 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__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc index 890cc05..698ea52 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/bidirectional_Astar3D.py b/Search-based Planning/Search_3D/bidirectional_Astar3D.py index 0796cc5..e93ee6a 100644 --- a/Search-based Planning/Search_3D/bidirectional_Astar3D.py +++ b/Search-based Planning/Search_3D/bidirectional_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.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost +from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children from Search_3D.plot_util3D import visualization import queue @@ -39,14 +39,6 @@ class Weighted_A_star(object): 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 run(self): x0, xt = self.start, self.goal self.OPEN1.put(x0, self.g[x0] + self.h1[x0]) # item, priority = g + h @@ -59,7 +51,7 @@ class Weighted_A_star(object): self.V.append(xi1) self.V.append(xi2) visualization(self) - allchild1, allchild2 = self.children(xi1), self.children(xi2) + allchild1, allchild2 = children(self,xi1), children(self,xi2) self.evaluation(allchild1,xi1,conf=1) self.evaluation(allchild2,xi2,conf=2) if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) @@ -75,7 +67,7 @@ class Weighted_A_star(object): if conf == 1: if xj not in self.CLOSED1: gi, gj = self.g[xi], self.g[xj] - a = gi + cost(xi,xj) + a = gi + cost(self,xi,xj) if a < gj: self.g[xj] = a self.Parent1[xj] = xi @@ -86,7 +78,7 @@ class Weighted_A_star(object): if conf == 2: if xj not in self.CLOSED2: gi, gj = self.g[xi], self.g[xj] - a = gi + cost(xi,xj) + a = gi + cost(self,xi,xj) if a < gj: self.g[xj] = a self.Parent2[xj] = xi diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index d1f3efc..f91fe6c 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -1,5 +1,6 @@ import numpy as np import pyrr +from collections import defaultdict def getRay(x, y): direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] @@ -43,6 +44,31 @@ def isinball(i, x): 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 lineAABB(p0,p1,dist,AABB): + #https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 + P = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point + D = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction + t = dist / 2 # radius + # TODO: implement this + + def StateSpace(env, factor = 0): boundary = env.boundary resolution = env.resolution @@ -68,28 +94,37 @@ def g_Space(initparams): g[v] = np.inf # this hashmap initialize all g values at inf return g -def isCollide(initparams, x, direc): +def isCollide(initparams, x, child): '''see if line intersects obstacle''' - resolution = initparams.env.resolution - child = tuple(map(np.add,x,np.multiply(direc,resolution))) ray , dist = getRay(x, child) , getDist(x, child) if not isinbound(initparams.env.boundary,child): - return True, child + return True, dist 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) if dist_wall <= dist: # collide - return True, child + return True, dist for i in initparams.env.balls: if isinball(i, child): - return True, child - 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, child - return False, 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 + if lineSphere(x, child, i): return True, dist + return False, dist + +def children(initparams, x): + # get the neighbor of a specific state + allchild = [] + resolution = initparams.env.resolution + for direc in initparams.Alldirec: + child = tuple(map(np.add,x,np.multiply(direc,resolution))) + if isinbound(initparams.env.boundary,child): + allchild.append(child) + return allchild def obstacleFree(initparams,x): for i in initparams.env.blocks: @@ -100,11 +135,24 @@ def obstacleFree(initparams,x): return False return True -def cost(i,j,settings=0): +def cost(initparams, i,j,settings=0): + collide, dist = isCollide(initparams,i,j) if settings == 0: - return getDist(i,j) + if collide: return np.inf + else: return dist if settings == 1: - return getManDist(i,j) + if collide: return np.inf + else: return getManDist(i,j) + +def initcost(initparams): + # initialize cost dictionary, could be modifed lateron + c = defaultdict(lambda: defaultdict(dict)) # two key dicionary + for xi in initparams.X: + cdren = children(initparams, xi) + for child in cdren: + c[xi][child] = cost(initparams, xi, child) + return c if __name__ == "__main__": - from env3D import env \ No newline at end of file + a = [10,2.5,1,1] + lineAABB([0,0,0],[1,1,1],) \ No newline at end of file