saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-29 08:34:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

'OBB_line'

Browse Source
This commit is contained in:
yue qi
2020-07-18 23:37:52 -07:00
parent b1cb24665e
commit d57aea0758
7 changed files with 62 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/Search_3D/DstarLite3D.py
+20 -21
View File
@@ -28,9 +28,9 @@ class D_star_Lite(object):
# (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
# (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
self.env = env(resolution=resolution)
self.X = StateSpace(self.env)
self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
#self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
#self.X = StateSpace(self.env)
#self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
# self.OPEN = queue.QueuePrior()
self.OPEN = queue.MinheapPQ()
self.km = 0
@@ -64,7 +64,7 @@ class D_star_Lite(object):
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.X:
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):
@@ -77,7 +77,6 @@ class D_star_Lite(object):
for xj in intersection.union(added):
self.COST[xi][xj] = cost(self, xi, xj)
CHANGED.add(xi)
self.CHILDREN[xi] = newchildren
else:
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
CHANGED.add(xi)
@@ -116,8 +115,10 @@ class D_star_Lite(object):
def UpdateVertex(self, u):
# if still in the hunt
if not getDist(self.xt, u) <= self.env.resolution: # originally: u != s_goal
cdren = self.getchildren(u)
self.rhs[u] = min([self.getcost(s, u) + self.getg(s) for s in self.getchildren(u)])
if u in self.CHILDREN and len(self.CHILDREN[u]) == 0:
self.rhs[u] = np.inf
else:
self.rhs[u] = min([self.getcost(s, u) + self.getg(s) for s in self.getchildren(u)])
# if u is in OPEN, remove it
self.OPEN.check_remove(u)
# if rhs(u) not equal to g(u)
@@ -130,10 +131,10 @@ class D_star_Lite(object):
u = self.OPEN.get()
self.V.add(u)
self.CLOSED.add(u)
# if not self.done: # first time running, we need to stop on this condition
# if getDist(self.x0,u) < 1.5*self.env.resolution:
# self.x0 = u
# break
if not self.done: # first time running, we need to stop on this condition
if getDist(self.x0,u) < 1*self.env.resolution:
self.x0 = u
break
if kold < self.CalculateKey(u):
self.OPEN.put(u, self.CalculateKey(u))
if self.getg(u) > self.getrhs(u):
@@ -143,6 +144,7 @@ class D_star_Lite(object):
self.UpdateVertex(u)
for s in self.getchildren(u):
self.UpdateVertex(s)
# visualization(self)
self.ind += 1
def main(self):
@@ -159,17 +161,14 @@ class D_star_Lite(object):
ischanged = False
self.V = set()
while getDist(self.x0, self.xt) > 2*self.env.resolution:
#---------------------------------- at 5th node, the environment is changed and cost is updated
if t % 1 == 0:
# new1,old1 = self.env.move_block(a=[0, 0, -2], s=0.5, block_to_move=0, mode='translation')
#---------------------------------- at specific times, the environment is changed and cost is updated
if t % 2 == 0:
new0,old0 = self.env.move_block(a=[-0.1, 0, -0.2], s=0.5, block_to_move=1, mode='translation')
new1,old1 = self.env.move_block(a=[0, 0, -0.2], s=0.5, block_to_move=0, mode='translation')
new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation')
#new2,old2 = self.env.move_block(a=[-0.3, 0, -0.1], s=0.5, block_to_move=1, mode='translation')
ischanged = True
self.Path = []
# if t == 5:
# new1,old1 = self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation')
# ischanged = True
#----------------------------------- traverse the route as originally planned
if t == 0:
children_new = [i for i in self.CLOSED if getDist(self.x0, i) <= self.env.resolution*np.sqrt(3)]
@@ -184,16 +183,16 @@ class D_star_Lite(object):
if ischanged:
self.km += heuristic_fun(self, self.x0, s_last)
s_last = self.x0
CHANGED = self.updatecost(True, new1, old1)
CHANGED = self.updatecost(True, new0, old0)
CHANGED1 = self.updatecost(True, new1, old1)
CHANGED2 = self.updatecost(True, new2, old2, mode='obb')
CHANGED = CHANGED.union(CHANGED2)
CHANGED = CHANGED.union(CHANGED1, CHANGED2)
# self.V = set()
for u in CHANGED:
self.UpdateVertex(u)
self.ComputeShortestPath()
ischanged = False
# self.Path = self.path(s_start)
self.Path = self.path(self.x0)
visualization(self)
t += 1
@@ -217,7 +216,7 @@ class D_star_Lite(object):
children = [i for i in self.CLOSED if getDist(s, i) <= self.env.resolution*np.sqrt(3)]
else:
children = list(self.CHILDREN[s])
snext = children[np.argmin([cost(self,s,s_p) + self.getg(s_p) for s_p in children])]
snext = children[np.argmin([self.getcost(s,s_p) + self.getg(s_p) for s_p in children])]
path.append([s, snext])
s = snext
if ind > 100:
Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_3D/env3D.py
+3 -1
View File
@@ -59,6 +59,7 @@ class obb(object):
self.P = P
self.E = E
self.O = O
self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]])
def getAABB2(blocks):
# used in lineAABB
@@ -85,7 +86,7 @@ class env():
self.AABB = getAABB2(self.blocks)
self.AABB_pyrr = getAABB(self.blocks)
self.balls = getballs()
self.OBB = np.array([obb([2.6,2.5,1],[0.2,1,1],R_matrix(0,0,45))])
self.OBB = np.array([obb([2.6,2.5,1],[0.2,2,1],R_matrix(0,0,45))])
#self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])]))
#print(self.OBB)
# self.OBB = []
@@ -154,6 +155,7 @@ class env():
if mode == 'rotation': # this makes an OBB rotate
ori = [self.OBB[obb_to_move]]
self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2])
self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,-self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[0,0,0,1]])
return self.OBB[obb_to_move], ori[0]
Search-based Planning/Search_3D/plot_util3D.py
+1 -1
View File
@@ -91,7 +91,7 @@ def visualization(initparams):
ax = plt.subplot(111, projection='3d')
#ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
#ax.view_init(elev=0., azim=90.)
ax.view_init(elev=8., azim=105.)
ax.view_init(elev=8., azim=120.)
#ax.view_init(elev=-8., azim=180)
ax.clear()
# drawing objects
Search-based Planning/Search_3D/utils3D.py
+38 -23
View File
@@ -52,12 +52,9 @@ def isinball(i, x):
return False
def isinobb(i, x):
# pt = i.O.T@np.array(x) # transform the point from {W} to {body}
# minx,miny,minz,maxx,maxy,maxz = i.P[0] - i.E[0],i.P[1] - i.E[1],i.P[2] - i.E[2],i.P[0] + i.E[0],i.P[1] + i.E[1],i.P[2] + i.E[2]
T = np.vstack([np.column_stack([i.O.T,-i.O.T@i.P]),[0,0,0,1]])
pt = T@np.append(x,1) # transform the point from {W} to {body}
minx,miny,minz,maxx,maxy,maxz = - i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]
block = [minx,miny,minz,maxx,maxy,maxz]
# 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)
def OBB2AABB(obb):
@@ -100,30 +97,33 @@ def lineAABB(p0, p1, dist, aabb):
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
P = aabb.P # center of the AABB
E = aabb.E # extents of AABB
T = [P[0] - mid[0], P[1] - mid[1], P[2] - mid[2]]
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]) > (E[0] + hl * abs(I[0])): return False
if abs(T[1]) > (E[1] + hl * abs(I[1])): return False
if abs(T[2]) > (E[2] + hl * abs(I[2])): return False
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 = E[1] * abs(I[2]) + E[2] * abs(I[1])
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 = E[0] * abs(I[2]) + E[2] * abs(I[0])
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 = E[0] * abs(I[1]) + E[1] * abs(I[0])
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):
T = np.vstack([np.column_stack([obb.O.T,-obb.O.T@obb.P]),[0,0,0,1]])
p0 = T@np.append(p0,1) # transform the points to the box
p1 = T@np.append(p1,1)
return lineAABB(p0[0:3],p1[0:3],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 OBBOBB(obb1, obb2):
# https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
@@ -311,7 +311,12 @@ def obstacleFree(initparams, x):
def cost(initparams, i, j, dist=None, settings='Euclidean'):
collide, dist = isCollide(initparams, i, j, dist)
if initparams.env.resolution < 0.25:
if dist==None:
dist = getDist(i,j)
collide = False
else:
collide, dist = isCollide(initparams, i, j, dist)
# collide, dist= False, getDist(i, j)
if settings == 'Euclidean':
if collide:
@@ -335,7 +340,17 @@ def initcost(initparams):
return c
if __name__ == "__main__":
pass
# obb1 = obb([0,0,0],[1,1,1],[[1,0,0],[0,1,0],[0,0,1]])
import time
from env3D import R_matrix, obb
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]])
# print(OBBOBB(obb1, obb2))
p0, p1 = [2.9,2.5,1],[1.9,2.5,1]
dist = getDist(p0,p1)
start = time.time()
for i in range(3000*27):
lineAABB(p0,p1,dist,obb1)
#lineOBB(p0,p1,dist,obb1)
print(time.time() - start)