mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-29 16:40:46 +08:00
Merge branch 'master' of https://github.com/zhm-real/PathPlanning
This commit is contained in:
zhm-real
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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()
|
||||
@@ -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))
|
||||
|
||||
@@ -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()
|
||||
|
||||
|
||||
@@ -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]
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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):
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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)
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user