mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-30 00:50:46 +08:00
update format
This commit is contained in:
zhm-real
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,44 @@
|
||||
# this is the three dimensional configuration space for rrt
|
||||
# !/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
@author: yue qi
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
|
||||
def getblocks(resolution):
|
||||
# AABBs
|
||||
block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00],
|
||||
[9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00],
|
||||
[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00],
|
||||
[1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00],
|
||||
[6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00],
|
||||
[1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00],
|
||||
[1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]]
|
||||
Obstacles = []
|
||||
for i in block:
|
||||
i = np.array(i)
|
||||
Obstacles.append((i[0] / resolution, i[1] / resolution, i[2] / resolution, i[3] / resolution, i[4] / resolution,
|
||||
i[5] / resolution))
|
||||
return np.array(Obstacles)
|
||||
|
||||
|
||||
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]) / resolution
|
||||
self.blocks = getblocks(resolution)
|
||||
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
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
newenv = env()
|
||||
X = StateSpace(newenv.boundary, newenv.resolution)
|
||||
print(X)
|
||||
@@ -0,0 +1,59 @@
|
||||
|
||||
"""
|
||||
This is rrt star code for 3D
|
||||
@author: yue qi
|
||||
"""
|
||||
import numpy as np
|
||||
from numpy.matlib import repmat
|
||||
from rrt_3D.env3D import env
|
||||
from collections import defaultdict
|
||||
import pyrr as pyrr
|
||||
from utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
|
||||
import time
|
||||
|
||||
|
||||
class rrtstar():
|
||||
def __init__(self):
|
||||
self.env = env()
|
||||
self.Parent = defaultdict(lambda: defaultdict(dict))
|
||||
self.V = []
|
||||
self.E = []
|
||||
self.i = 0
|
||||
self.maxiter = 10000
|
||||
self.stepsize = 0.5
|
||||
self.Path = []
|
||||
|
||||
def wireup(self,x,y):
|
||||
self.E.append([x,y]) # add edge
|
||||
self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
|
||||
|
||||
def removewire(self,xnear):
|
||||
xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])]
|
||||
a = np.array([xnear,xparent])
|
||||
self.E = [xx for xx in self.E if not (xx==a).all()] # remove and replace old the connection
|
||||
|
||||
def run(self):
|
||||
self.V.append(self.env.start)
|
||||
ind = 0
|
||||
xnew = self.env.start
|
||||
while ind < self.maxiter and getDist(xnew,self.env.goal) > 1:
|
||||
xrand = sampleFree(self)
|
||||
xnearest = nearest(self,xrand)
|
||||
xnew = steer(self,xnearest,xrand)
|
||||
if not isCollide(self,xnearest,xnew):
|
||||
self.V.append(xnew) # add point
|
||||
self.wireup(xnew,xnearest)
|
||||
#visualization(self)
|
||||
self.i += 1
|
||||
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))
|
||||
visualization(self)
|
||||
|
||||
if __name__ == '__main__':
|
||||
p = rrtstar()
|
||||
starttime = time.time()
|
||||
p.run()
|
||||
print('time used = ' + str(time.time()-starttime))
|
||||
@@ -0,0 +1,79 @@
|
||||
|
||||
"""
|
||||
This is rrt star code for 3D
|
||||
@author: yue qi
|
||||
"""
|
||||
import numpy as np
|
||||
from numpy.matlib import repmat
|
||||
from rrt_3D.env3D import env
|
||||
from collections import defaultdict
|
||||
import pyrr as pyrr
|
||||
from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
|
||||
import time
|
||||
|
||||
|
||||
class rrtstar():
|
||||
def __init__(self):
|
||||
self.env = env()
|
||||
self.Parent = defaultdict(lambda: defaultdict(dict))
|
||||
self.V = []
|
||||
self.E = []
|
||||
self.i = 0
|
||||
self.maxiter = 10000
|
||||
self.stepsize = 0.5
|
||||
self.Path = []
|
||||
|
||||
def wireup(self,x,y):
|
||||
self.E.append([x,y]) # add edge
|
||||
self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
|
||||
|
||||
def removewire(self,xnear):
|
||||
xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])]
|
||||
a = np.array([xnear,xparent])
|
||||
self.E = [xx for xx in self.E if not (xx==a).all()] # remove and replace old the connection
|
||||
|
||||
def run(self):
|
||||
self.V.append(self.env.start)
|
||||
ind = 0
|
||||
xnew = self.env.start
|
||||
while ind < self.maxiter and getDist(xnew,self.env.goal) > 1:
|
||||
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)
|
||||
# minimal path and minimal cost
|
||||
xmin,cmin = xnearest,cost(self,xnearest) + getDist(xnearest,xnew)
|
||||
# connecting along minimal cost path
|
||||
if self.i == 0:
|
||||
c1 = cost(self,Xnear) + getDist(xnew,Xnear)
|
||||
if not isCollide(self,xnew,Xnear) and c1 < cmin:
|
||||
xmin,cmin = Xnear,c1
|
||||
self.wireup(xnew,xmin)
|
||||
else:
|
||||
for xnear in Xnear:
|
||||
c1 = cost(self,xnear) + getDist(xnew,xnear)
|
||||
if not isCollide(self,xnew,xnear) and c1 < cmin:
|
||||
xmin,cmin = xnear,c1
|
||||
self.wireup(xnew,xmin)
|
||||
# rewire
|
||||
for xnear in Xnear:
|
||||
c2 = cost(self,xnew) + getDist(xnew,xnear)
|
||||
if not isCollide(self,xnew,xnear) and c2 < cost(self,xnear):
|
||||
self.removewire(xnear)
|
||||
self.wireup(xnear,xnew)
|
||||
self.i += 1
|
||||
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))
|
||||
visualization(self)
|
||||
|
||||
if __name__ == '__main__':
|
||||
p = rrtstar()
|
||||
starttime = time.time()
|
||||
p.run()
|
||||
print('time used = ' + str(time.time()-starttime))
|
||||
@@ -0,0 +1,177 @@
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user