def reached(self):
self.done = True
xn = near(self, self.env.goal)
c = [cost(self, x) for x in xn]
xncmin = xn[np.argmin(c)]
self.wireup(self.env.goal, xncmin)
self.V.append(self.env.goal)
self.Path, self.D = path(self)
def reached(self):
self.done = True
goal = self.xt
xn = near(self, self.env.goal)
c = [cost(self, tuple(x)) for x in xn]
xncmin = xn[np.argmin(c)]
self.wireup(goal, tuple(xncmin))
self.V.append(goal)
self.Path, self.D = path(self)
def run(self):
xnew = self.x0
print('start rrt*... ')
self.fig = plt.figure(figsize=(10, 8))
while self.ind < self.maxiter:
xrand = sampleFree(self)
xnearest = nearest(self, xrand)
xnew, dist = steer(self, xnearest, xrand)
collide, _ = isCollide(self, xnearest, xnew, dist=dist)
if not collide:
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
Collide = []
for xnear in Xnear:
xnear = tuple(xnear)
c1 = cost(self, xnear) + getDist(xnew, xnear)
collide, _ = isCollide(self, xnew, xnear)
Collide.append(collide)
if not collide and c1 < cmin:
xmin, cmin = xnear, c1
self.wireup(xnew, xmin)
# rewire
for i in range(len(Xnear)):
collide = Collide[i]
xnear = tuple(Xnear[i])
c2 = cost(self, xnew) + getDist(xnew, xnear)
if not collide and c2 < cost(self, xnear):
# self.removewire(xnear)
self.wireup(xnear, xnew)
self.i += 1
self.ind += 1
# max sample reached
self.reached()
print('time used = ' + str(time.time() - starttime))
print('Total distance = ' + str(self.D))
visualization(self)
plt.show()
def run(self):
self.V.append(self.env.start)
self.ind = 0
xnew = self.env.start
print('start rrt*... ')
self.fig = plt.figure(figsize=(10, 8))
while self.ind < self.maxiter:
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
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
self.ind += 1
# max sample reached
self.reached()
print('time used = ' + str(time.time() - starttime))
print('Total distance = ' + str(self.D))
visualization(self)
plt.show()