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)
