mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-30 00:50:46 +08:00
391311qy
162 lines
5.0 KiB
Python
162 lines
5.0 KiB
Python
import numpy as np
|
|
from numpy.matlib import repmat
|
|
import pyrr as pyrr
|
|
# plotting
|
|
import matplotlib.pyplot as plt
|
|
from mpl_toolkits.mplot3d import Axes3D
|
|
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
|
|
import mpl_toolkits.mplot3d as plt3d
|
|
|
|
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]))
|
|
|
|
def draw_block_list(ax,blocks):
|
|
'''
|
|
Subroutine used by draw_map() to display the environment blocks
|
|
'''
|
|
v = np.array([[0,0,0],[1,0,0],[1,1,0],[0,1,0],[0,0,1],[1,0,1],[1,1,1],[0,1,1]],dtype='float')
|
|
f = np.array([[0,1,5,4],[1,2,6,5],[2,3,7,6],[3,0,4,7],[0,1,2,3],[4,5,6,7]])
|
|
#clr = blocks[:,6:]/255
|
|
n = blocks.shape[0]
|
|
d = blocks[:,3:6] - blocks[:,:3]
|
|
vl = np.zeros((8*n,3))
|
|
fl = np.zeros((6*n,4),dtype='int64')
|
|
#fcl = np.zeros((6*n,3))
|
|
for k in range(n):
|
|
vl[k*8:(k+1)*8,:] = v * d[k] + blocks[k,:3]
|
|
fl[k*6:(k+1)*6,:] = f + k*8
|
|
#fcl[k*6:(k+1)*6,:] = clr[k,:]
|
|
|
|
if type(ax) is Poly3DCollection:
|
|
ax.set_verts(vl[fl])
|
|
else:
|
|
pc = Poly3DCollection(vl[fl], alpha=0.15, linewidths=1, edgecolors='k')
|
|
#pc.set_facecolor(fcl)
|
|
h = ax.add_collection3d(pc)
|
|
return h
|
|
|
|
''' The following utils can be used for rrt or rrt*,
|
|
required param initparams should have
|
|
env, environement generated from env3D
|
|
V, node set
|
|
E, edge set
|
|
i, nodes added
|
|
maxiter, maximum iteration allowed
|
|
stepsize, leaf growth restriction
|
|
|
|
'''
|
|
|
|
def sampleFree(initparams):
|
|
x = np.random.uniform(initparams.env.boundary[0:3],initparams.env.boundary[3:6])
|
|
if isinside(initparams,x):
|
|
return sampleFree(initparams)
|
|
else: return np.array(x)
|
|
|
|
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]:
|
|
return True
|
|
return False
|
|
|
|
def isCollide(initparams,x,y):
|
|
'''see if line intersects obstacle'''
|
|
ray = getRay(x,y)
|
|
dist = getDist(x,y)
|
|
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
|
|
return False
|
|
|
|
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
|
|
|
|
def near(initparams,x,r=2):
|
|
#TODO: r = min{gamma*log(card(V)/card(V)1/d),eta}
|
|
V = np.array(initparams.V)
|
|
if initparams.i == 0:
|
|
return initparams.V[0]
|
|
xr = repmat(x,len(V),1)
|
|
inside = np.linalg.norm(xr - V,axis = 1) < r
|
|
nearpoints = V[inside]
|
|
return np.array(nearpoints)
|
|
|
|
def cost(initparams,x):
|
|
'''here use the additive recursive cost function'''
|
|
if all(x == initparams.env.start):
|
|
return 0
|
|
xparent = initparams.Parent[str(x[0])][str(x[1])][str(x[2])]
|
|
return cost(initparams,xparent) + getDist(x,xparent)
|
|
|
|
def visualization(initparams):
|
|
V = np.array(initparams.V)
|
|
E = np.array(initparams.E)
|
|
Path = np.array(initparams.Path)
|
|
start = initparams.env.start
|
|
goal = initparams.env.goal
|
|
ax = plt.subplot(111,projection='3d')
|
|
ax.view_init(elev=0., azim=90)
|
|
ax.clear()
|
|
draw_block_list(ax,initparams.env.blocks)
|
|
if E != []:
|
|
for i in E:
|
|
xs = i[0][0],i[1][0]
|
|
ys = i[0][1],i[1][1]
|
|
zs = i[0][2],i[1][2]
|
|
line = plt3d.art3d.Line3D(xs, ys, zs)
|
|
ax.add_line(line)
|
|
|
|
if Path != []:
|
|
for i in Path:
|
|
xs = i[0][0],i[1][0]
|
|
ys = i[0][1],i[1][1]
|
|
zs = i[0][2],i[1][2]
|
|
line = plt3d.art3d.Line3D(xs, ys, zs,color='r')
|
|
ax.add_line(line)
|
|
|
|
ax.plot(start[0:1],start[1:2],start[2:],'go',markersize=7,markeredgecolor='k')
|
|
ax.plot(goal[0:1],goal[1:2],goal[2:],'ro',markersize=7,markeredgecolor='k')
|
|
ax.scatter3D(V[:,0], V[:,1], V[:,2])
|
|
plt.xlim(initparams.env.boundary[0],initparams.env.boundary[3])
|
|
plt.ylim(initparams.env.boundary[1],initparams.env.boundary[4])
|
|
ax.set_zlim(initparams.env.boundary[2],initparams.env.boundary[5])
|
|
plt.xlabel('x')
|
|
plt.ylabel('y')
|
|
if not Path != []:
|
|
plt.pause(0.001)
|
|
else: plt.show()
|
|
|
|
def path(initparams,Path=[],dist=0):
|
|
x = initparams.env.goal
|
|
while not all(x==initparams.env.start):
|
|
x2 = initparams.Parent[str(x[0])][str(x[1])][str(x[2])]
|
|
Path.append(np.array([x,x2]))
|
|
dist += getDist(x,x2)
|
|
x = x2
|
|
return Path,dist
|
|
|
|
|