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