def run(self):
self.V.append(tuple(self.env.start))
self.ind = 0
self.fig = plt.figure(figsize=(10, 8))
xnew = self.env.start
while self.ind < self.maxiter and getDist(
xnew, self.env.goal) > self.stepsize:
xrand = sampleFree(self)
xnearest = nearest(self, xrand)
xnew = steer(self, xnearest, xrand)
collide, _ = isCollide(self, xnearest, xnew)
if not collide:
self.V.append(xnew) # add point
self.wireup(xnew, xnearest)
if getDist(xnew, self.env.goal) <= self.stepsize:
goal = tuple(self.env.goal)
self.wireup(goal, xnew)
self.Path, D = path(self)
print('Total distance = ' + str(D))
# visualization(self)
self.i += 1
self.ind += 1
# if the goal is really reached
self.done = True
visualization(self)
plt.show()
def run(self, Reversed = True, xrobot = None):
if Reversed:
if xrobot is None:
self.x0 = tuple(self.env.goal)
self.xt = tuple(self.env.start)
else:
self.x0 = tuple(self.env.goal)
self.xt = xrobot
xnew = self.env.goal
else:
xnew = self.env.start
self.V.append(self.x0)
while self.ind < self.maxiter:
xrand = sampleFree(self)
xnearest = nearest(self, xrand)
xnew = steer(self, xnearest, xrand)
collide, _ = isCollide(self, xnearest, xnew)
if not collide:
self.V.append(xnew) # add point
if self.Flag is not None:
self.Flag[xnew] = 'Valid'
self.wireup(xnew, xnearest)
if getDist(xnew, self.xt) <= self.stepsize:
self.wireup(self.xt, xnew)
self.Path, D = path(self)
print('Total distance = ' + str(D))
if self.Flag is not None:
self.Flag[self.xt] = 'Valid'
break
# visualization(self)
self.i += 1
self.ind += 1
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):
self.V.append(self.env.start)
self.ind = 0
self.fig = plt.figure(figsize=(10, 8))
xnew = self.env.start
while self.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
self.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))
self.done = True
visualization(self)
plt.show()
def Main(self):
qgoal = tuple(self.env.start)
qstart = tuple(self.env.goal)
self.GrowRRT()
self.done = True
# visualization(self)
self.done = False
# change the enviroment
new0,old0 = self.env.move_block(a=[0, 0, -2], s=0.5, block_to_move=1, mode='translation')
while qgoal != qstart:
qgoal = self.Parent[qgoal]
# TODO move to qgoal and check for new obs
xrobot = qstart
# TODO if any new obstacle are observed
self.InvalidateNodes(new0, mode = 'translation')
for xi in self.Path:
if self.Flag[tuple(xi[0])] == 'Invalid':
self.RegrowRRT(tuple(self.env.start))
self.done = True
self.Path, D = path(self)
visualization(self)
plt.show()
def RRTplan(self, env, initial, goal):
threshold = self.stepsize
nearest = initial # state structure
self.V.append(initial)
rrt_tree = initial # TODO KDtree structure
while self.ind <= self.maxiter:
target = self.ChooseTarget(goal)
nearest = self.Nearest(rrt_tree, target)
extended, collide = self.Extend(env, nearest, target)
if not collide:
self.AddNode(rrt_tree, nearest, extended)
if getDist(nearest, goal) <= threshold:
self.AddNode(rrt_tree, nearest, self.xt)
break
self.i += 1
self.ind += 1
visualization(self)
# return rrt_tree
self.done = True
self.Path, D = path(self)
visualization(self)
plt.show()
def run(self):
self.V.append(self.x0)
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:
self.V.append(xnew) # add point
self.wireup(xnew, xnearest)
if getDist(xnew, self.xt) <= self.stepsize:
self.wireup(self.xt, xnew)
self.Path, D = path(self)
print('Total distance = ' + str(D))
break
# visualization(self)
self.i += 1
self.ind += 1
# if the goal is really reached
self.done = True
visualization(self)
plt.show()
def Informed_rrt(self):
self.V = [self.xstart]
self.E = set()
self.Xsoln = set()
self.T = (self.V, self.E)
c = 1
while self.ind <= self.N:
print(self.ind)
self.visualization()
# print(self.i)
if len(self.Xsoln) == 0:
cbest = np.inf
else:
cbest = min({self.cost(xsln) for xsln in self.Xsoln})
xrand = self.Sample(self.xstart, self.xgoal, cbest)
xnearest = nearest(self, xrand)
xnew, dist = steer(self, xnearest, xrand)
# print(xnew)
collide, _ = isCollide(self, xnearest, xnew, dist=dist)
if not collide:
self.V.append(xnew)
Xnear = near(self, xnew)
xmin = xnearest
cmin = self.cost(xmin) + c * self.line(xnearest, xnew)
for xnear in Xnear:
xnear = tuple(xnear)
cnew = self.cost(xnear) + c * self.line(xnear, xnew)
if cnew < cmin:
collide, _ = isCollide(self, xnear, xnew)
if not collide:
xmin = xnear
cmin = cnew
self.E.add((xmin, xnew))
self.Parent[xnew] = xmin
for xnear in Xnear:
xnear = tuple(xnear)
cnear = self.cost(xnear)
cnew = self.cost(xnew) + c * self.line(xnew, xnear)
# rewire
if cnew < cnear:
collide, _ = isCollide(self, xnew, xnear)
if not collide:
xparent = self.Parent[xnear]
self.E.difference_update((xparent, xnear))
self.E.add((xnew, xnear))
self.Parent[xnear] = xnew
self.i += 1
if self.InGoalRegion(xnew):
print('reached')
self.done = True
self.Parent[self.xgoal] = xnew
self.Path, _ = path(self)
self.Xsoln.add(xnew)
# update path
if self.done:
self.Path, _ = path(self, Path=[])
self.ind += 1
# return tree
return self.T
