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 ExpandVertex(self, v): self.QV.remove(v) Xnear = {x for x in self.Xsamples if getDist(x, v) <= self.r} self.QE.update({(v, x) for x in Xnear if self.g_hat(v) + self.c_hat(v, x) + self.h_hat(x) < self.g_T(self.xgoal)}) if v not in self.Vold: Vnear = {w for w in self.V if getDist(w, v) <= self.r} self.QE.update({(v,w) for w in Vnear if \ ((v,w) not in self.E) and \ (self.g_hat(v) + self.c_hat(v, w) + self.h_hat(w) < self.g_T(self.xgoal)) and \ (self.g_T(v) + self.c_hat(v, w) < self.g_T(w))})
def Sample(self, xstart, xgoal, cmax, bias=0.05): # sample within a eclipse if cmax < np.inf: cmin = getDist(xgoal, xstart) xcenter = np.array([(xgoal[0] + xstart[0]) / 2, (xgoal[1] + xstart[1]) / 2, (xgoal[2] + xstart[2]) / 2]) C = self.RotationToWorldFrame(xstart, xgoal) r = np.zeros(3) r[0] = cmax / 2 for i in range(1, 3): r[i] = np.sqrt(cmax**2 - cmin**2) / 2 L = np.diag(r) # R3*3 xball = self.SampleUnitBall() # np.array x = C @ L @ xball + xcenter self.C = C # save to global var self.xcenter = xcenter self.L = L if not isinside(self, x): # intersection with the state space xrand = x else: return self.Sample(xstart, xgoal, cmax) else: xrand = sampleFree(self, bias=bias) return xrand
def Sample(self, m, cmax, bias=0.05, xrand=set()): # sample within a eclipse print('new sample') if cmax < np.inf: cmin = getDist(self.xgoal, self.xstart) xcenter = np.array([(self.xgoal[0] + self.xstart[0]) / 2, (self.xgoal[1] + self.xstart[1]) / 2, (self.xgoal[2] + self.xstart[2]) / 2]) C = self.RotationToWorldFrame(self.xstart, self.xgoal) r = np.zeros(3) r[0] = cmax / 2 for i in range(1, 3): r[i] = np.sqrt(cmax**2 - cmin**2) / 2 L = np.diag(r) # R3*3 xball = self.SampleUnitBall(m) # np.array x = (C @ L @ xball).T + repmat(xcenter, len(xball.T), 1) # x2 = set(map(tuple, x)) self.C = C # save to global var self.xcenter = xcenter self.L = L x2 = set( map( tuple, x[np.array([ not isinside(self, state) and isinbound(self.env.boundary, state) for state in x ])])) # intersection with the state space xrand.update(x2) # if there are samples inside obstacle: recursion if len(x2) < m: return self.Sample(m - len(x2), cmax, bias=bias, xrand=xrand) else: for i in range(m): xrand.add(tuple(sampleFree(self, bias=bias))) return xrand
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 cost(self, x): # actual cost '''here use the additive recursive cost function''' if x == self.xstart: return 0.0 if x not in self.Parent: return np.inf return self.cost(self.Parent[x]) + getDist(x, self.Parent[x])
def c_hat(self, v, w): # c_hat < c < np.inf # heuristic estimate of the edge cost, since c is expensive if (v, w) in self.heuristic_edgeCost: pass else: self.heuristic_edgeCost[(v, w)] = getDist(v, w) return self.heuristic_edgeCost[(v, w)]
def RotationToWorldFrame(self, xstart, xgoal): # S0(n): such that the xstart and xgoal are the center points d = getDist(xstart, xgoal) xstart, xgoal = np.array(xstart), np.array(xgoal) a1 = (xgoal - xstart) / d M = np.outer(a1, [1, 0, 0]) U, S, V = np.linalg.svd(M) C = U @ np.diag([1, 1, np.linalg.det(U) * np.linalg.det(V)]) @ V.T return C
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 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 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()
def expand(self, set_xi, T, Xunconnected, r): V, E = T Eout = set() for xp in set_xi: Eout.update({(x1, x2) for (x1, x2) in E if x1 == xp}) for xc in { x for x in Xunconnected.union(V) if getDist(xp, x) <= r }: if self.g_hat(xp) + self.c_hat( xp, xc) + self.h_hat(xc) <= self.g_T(self.xgoal): if self.g_hat(xp) + self.c_hat(xp, xc) <= self.g_hat(xc): Eout.add((xp, xc)) return Eout
def path(self, dist=0): Path=[] x = self.xt i = 0 while x != self.x0: x2 = self.Parent[x] Path.append(np.array([x, x2])) dist += getDist(x, x2) x = x2 if i > 10000: print('Path is not found') return i+= 1 return Path, dist
def GrowRRT(self): print('growing...') qnew = self.x0 distance_threshold = self.stepsize self.ind = 0 while self.ind <= self.maxiter: qtarget = self.ChooseTarget() qnearest = self.Nearest(qtarget) qnew, collide = self.Extend(qnearest, qtarget) if not collide: self.AddNode(qnearest, qnew) if getDist(qnew, self.xt) < distance_threshold: self.AddNode(qnearest, self.xt) self.flag[self.xt] = 'Valid' break self.i += 1 self.ind += 1
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 g_hat(self, v): return getDist(self.xstart, v)
def line(self, x, y): return getDist(x, y)
def Near(self, nodeset, node, rn): if node in self.neighbors: return self.neighbors[node] validnodes = {i for i in nodeset if getDist(i, node) < rn} return validnodes
def InGoalRegion(self, x): # Xgoal = {x in Xfree | \\x-xgoal\\2 <= rgoal} return getDist(x, self.xgoal) <= self.rgoal
def Cost(self, x, y): # collide, dist = isCollide(self, x, y) # if collide: # return np.inf # return dist return getDist(x, y)
def h_hat(self, v): return getDist(self.xgoal, v)
def c_hat(self, v, w): # c_hat < c < np.inf # heuristic estimate of the edge cost, since c is expensive return getDist(v, w)