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

BITstar

Browse Source
This commit is contained in:
yue qi
2020-08-12 21:23:03 -07:00
parent 1f06667394
commit dbb653ceca
1 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Sampling_based_Planning/rrt_3D/BIT_star3D.py
+7 -3
View File
@@ -83,6 +83,7 @@ class BIT_star:
self.QV = set() # nodes in queue self.QV = set() # nodes in queue
self.r = np.inf # radius for evaluation self.r = np.inf # radius for evaluation
self.ind = 0 self.ind = 0
num_resample = 0
while True: while True:
# for the first round # for the first round
print('round '+str(self.ind)) print('round '+str(self.ind))
@@ -96,9 +97,10 @@ class BIT_star:
self.QV = {v for v in self.V} self.QV = {v for v in self.V}
# setting the radius # setting the radius
if self.done: if self.done:
self.r = 1 self.r = 1 # sometimes the original radius criteria makes the radius too small to improve existing tree
num_resample += 1
else: else:
self.r = self.radius(len(self.V) + len(self.Xsamples)) self.r = self.radius(len(self.V) + len(self.Xsamples)) # radius determined with the sample size and dimension of conf space
while self.BestQueueValue(self.QV, mode = 'QV') <= self.BestQueueValue(self.QE, mode = 'QE'): while self.BestQueueValue(self.QV, mode = 'QV') <= self.BestQueueValue(self.QE, mode = 'QE'):
self.ExpandVertex(self.BestInQueue(self.QV, mode = 'QV')) self.ExpandVertex(self.BestInQueue(self.QV, mode = 'QV'))
(vm, xm) = self.BestInQueue(self.QE, mode = 'QE') (vm, xm) = self.BestInQueue(self.QE, mode = 'QE')
@@ -133,7 +135,9 @@ class BIT_star:
# if the iteration is bigger # if the iteration is bigger
if self.ind > self.maxiter: if self.ind > self.maxiter:
break break
return self.T
print('complete')
print('number of times resampling ' + str(num_resample))
# ---------IRRT utils # ---------IRRT utils
def Sample(self, m, cmax, bias = 0.05, xrand = set()): def Sample(self, m, cmax, bias = 0.05, xrand = set()):