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

'update'

Browse Source
This commit is contained in:
yue qi
2020-07-26 14:44:35 -07:00
parent 256812d165
commit d229bceaaf
9 changed files with 128 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Sampling-based Planning/rrt_3D/utils3D.py
+4 -5
View File
@@ -37,14 +37,14 @@ 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:
if i < bias:
return initparams.env.goal + 1
else:
return x
@@ -172,10 +172,9 @@ def steer(initparams, x, y):
def cost(initparams, x):
'''here use the additive recursive cost function'''
if x == tuple(initparams.env.start):
if x == initparams.x0:
return 0
xparent = initparams.Parent[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):