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

update

Browse Source
This commit is contained in:
zhm-real
2020-07-09 16:46:57 -07:00
parent c91e420633
commit a48333ae14
3 changed files with 124 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/.idea/workspace.xml
Generated
+25 -24
View File
@@ -22,6 +22,7 @@
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_3D/Dstar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/Dstar3D.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_3D/utils3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/utils3D.py" afterDir="false" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
@@ -73,7 +74,28 @@
</list>
</option>
</component>
<component name="RunManager" selected="Python.Dstar3D">
<component name="RunManager" selected="Python.Astar3D">
<configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs>
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
<option name="IS_MODULE_SDK" value="true" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/Astar3D.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
<option name="MODULE_MODE" value="false" />
<option name="REDIRECT_INPUT" value="false" />
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
@@ -116,27 +138,6 @@
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="Field_D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs>
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
<option name="IS_MODULE_SDK" value="true" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Field_D_star.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
<option name="MODULE_MODE" value="false" />
<option name="REDIRECT_INPUT" value="false" />
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="ReedsShepp" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
@@ -203,18 +204,18 @@
<list>
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.D_star" />
<item itemvalue="Python.Field_D_star" />
<item itemvalue="Python.ReedsShepp" />
<item itemvalue="Python.simulation" />
<item itemvalue="Python.Dstar3D" />
<item itemvalue="Python.Astar3D" />
</list>
<recent_temporary>
<list>
<item itemvalue="Python.Astar3D" />
<item itemvalue="Python.Dstar3D" />
<item itemvalue="Python.D_star" />
<item itemvalue="Python.simulation" />
<item itemvalue="Python.ReedsShepp" />
<item itemvalue="Python.Field_D_star" />
</list>
</recent_temporary>
</component>
Search-based Planning/Search_3D/Dstar3D.py
+28 -33
View File
@@ -74,17 +74,12 @@ class D_star(object):
# if empty, returns None and -1
# it also removes this min value form the OPEN set.
if self.OPEN:
mink = min(self.OPEN, key=self.OPEN.get)
minv = self.OPEN[mink]
_ = self.OPEN.pop(mink)
# #
# mink = -1
# minv = np.inf
# for v, k in enumerate(self.OPEN):
# if v < minv:
# mink, minv = k, v
# return mink, self.OPEN.pop(mink)
return mink, minv
mink = -1
minv = np.inf
for v, k in enumerate(self.OPEN):
if v < minv:
mink, minv = k, v
return mink, self.OPEN.pop(mink)
return None, -1
def insert(self, x, h_new):
@@ -167,7 +162,7 @@ class D_star(object):
while True:
# TODO: self.x0 =
self.process_state()
visualization(self)
# visualization(self)
if self.tag[self.x0] == "Closed":
break
self.ind += 1
@@ -178,29 +173,29 @@ class D_star(object):
# plt.show()
# when the environemnt changes over time
for i in range(2):
self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation')
visualization(self)
s = tuple(self.env.start)
while s != self.xt:
if s == tuple(self.env.start):
sparent = self.b[self.x0]
else:
sparent = self.b[s]
# self.update_obs()
if cost(self, s, sparent) == np.inf:
# print(s, " ", sparent)
self.modify(s)
continue
self.ind += 1
s = sparent
self.Path = self.path()
visualization(self)
# for i in range(2):
# self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation')
# visualization(self)
# s = tuple(self.env.start)
#
# while s != self.xt:
# if s == tuple(self.env.start):
# sparent = self.b[self.x0]
# else:
# sparent = self.b[s]
# # self.update_obs()
#
# if cost(self, s, sparent) == np.inf:
# # print(s, " ", sparent)
# self.modify(s)
# continue
# self.ind += 1
# s = sparent
# self.Path = self.path()
# visualization(self)
plt.show()
if __name__ == '__main__':
D = D_star(1)
D = D_star(0.5)
D.run()
Search-based Planning/Search_3D/utils3D.py
+71 -56
View File
@@ -2,72 +2,76 @@ 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]]
return np.array([x, direc])
def getDist(pos1, pos2):
return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
def getManDist(pos1, pos2):
return sum([abs(pos1[0] - pos2[0]),abs(pos1[1] - pos2[1]),abs(pos1[2] - pos2[2])])
def getNearest(Space,pt):
def getManDist(pos1, pos2):
return sum([abs(pos1[0] - pos2[0]), abs(pos1[1] - pos2[1]), abs(pos1[2] - pos2[2])])
def getNearest(Space, pt):
'''get the nearest point on the grid'''
mindis,minpt = 1000,None
for pts in Space:
dis = getDist(pts,pt)
mindis, minpt = 1000, None
for pts in Space:
dis = getDist(pts, pt)
if dis < mindis:
mindis,minpt = dis,pts
mindis, minpt = dis, pts
return minpt
def Heuristic(Space,t):
def Heuristic(Space, t):
'''Max norm distance'''
h = {}
for k in Space.keys():
h[k] = max(abs(np.array([t[0]-k[0],t[1]-k[1],t[2]-k[2]])))
h[k] = max(abs(np.array([t[0] - k[0], t[1] - k[1], t[2] - k[2]])))
return h
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(' ')])
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]:
return True
return False
def isinball(i, x):
if getDist(i[0:3], x) <= i[3]:
return True
return False
def lineSphere(p0,p1,ball):
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]
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:
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:
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:
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
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
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
def lineAABB(p0, p1, dist, aabb):
# https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
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]]
# do any of the principal axis form a separting axis?
if abs(T[0]) > (E[0] + hl * abs(I[0])): return False
@@ -84,46 +88,48 @@ def lineAABB(p0,p1,dist,aabb):
if abs(T[0] * I[1] - T[1] * I[0]) > r: return False
return True
def StateSpace(env, factor = 0):
def StateSpace(env, factor=0):
boundary = env.boundary
resolution = env.resolution
xmin,xmax = boundary[0]+factor*resolution,boundary[3]-factor*resolution
ymin,ymax = boundary[1]+factor*resolution,boundary[4]-factor*resolution
zmin,zmax = boundary[2]+factor*resolution,boundary[5]-factor*resolution
xarr = np.arange(xmin,xmax,resolution).astype(float)
yarr = np.arange(ymin,ymax,resolution).astype(float)
zarr = np.arange(zmin,zmax,resolution).astype(float)
xmin, xmax = boundary[0] + factor * resolution, boundary[3] - factor * resolution
ymin, ymax = boundary[1] + factor * resolution, boundary[4] - factor * resolution
zmin, zmax = boundary[2] + factor * resolution, boundary[5] - factor * resolution
xarr = np.arange(xmin, xmax, resolution).astype(float)
yarr = np.arange(ymin, ymax, resolution).astype(float)
zarr = np.arange(zmin, zmax, resolution).astype(float)
Space = set()
for x in xarr:
for y in yarr:
for z in zarr:
Space.add((x,y,z))
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.'''
g = {}
Space = StateSpace(initparams.env)
for v in Space:
g[v] = np.inf # this hashmap initialize all g values at inf
g[v] = np.inf # this hashmap initialize all g values at inf
return g
def isCollide(initparams, x, child):
'''see if line intersects obstacle'''
dist = getDist(x, child)
if not isinbound(initparams.env.boundary,child): return True, dist
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
if lineAABB(x, child, dist, i):return True, dist
if lineAABB(x, child, dist, i): return True, dist
for i in initparams.env.balls:
if isinball(i, child):return True, dist
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]
@@ -132,43 +138,52 @@ def isCollide(initparams, x, child):
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):
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):
def obstacleFree(initparams, x):
for i in initparams.env.blocks:
if isinbound(i,x):
if isinbound(i, x):
return False
for i in initparams.env.balls:
if isinball(i,x):
if isinball(i, x):
return False
return True
def cost(initparams, i,j,settings=0):
collide, dist = isCollide(initparams,i,j)
def cost(initparams, i, j, settings=0):
collide, dist = isCollide(initparams, i, j)
if settings == 0:
if collide: return np.inf
else: return dist
if collide:
return np.inf
else:
return dist
if settings == 1:
if collide: return np.inf
else: 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
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__":
a = '()'
print(list(a))
print(list(a))