def Plan(self, start_config, goal_config, epsilon=0.2):
ftree = RRTTree(self.planning_env, start_config)
rtree = RRTTree(self.planning_env, goal_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
NUM_ITERATIONS = 100000
for iter in range(NUM_ITERATIONS):
# pick random goal tree and connect path tree
rand_tree, rand_goal_config = (ftree, goal_config) if len(
ftree.vertices) < len(rtree.vertices) else (rtree,
start_config)
connect_tree = ftree if len(ftree.vertices) >= len(
rtree.vertices) else rtree
# random goal plan
v_new = self.RandomGoalPlan(rand_tree, rand_goal_config)
# connect path plan
if (v_new is not None) and self.ConnectPlan(
connect_tree, v_new, epsilon):
break
if (iter % 10 == 0):
d_f = self.planning_env.ComputeDistance(
ftree.GetNearestVertex(goal_config)[1], goal_config)
d_r = self.planning_env.ComputeDistance(
rtree.GetNearestVertex(start_config)[1], start_config)
print(iter, ' Closest ftree dist to end goal :', d_f,
'; Closest rtree dist to start goal :', d_r)
dist = self.planning_env.ComputeDistance(ftree.vertices[-1],
rtree.vertices[-1])
print("Dist=", dist)
if dist < epsilon:
plan_f, fid = [ftree.vertices[-1]], len(ftree.vertices) - 1
while (fid != ftree.GetRootId()):
plan_f.append(ftree.vertices[ftree.edges[fid]])
fid = ftree.edges[fid]
plan_r, rid = [rtree.vertices[-1]], len(rtree.vertices) - 1
while (rid != rtree.GetRootId()):
plan_r.append(rtree.vertices[rtree.edges[rid]])
rid = rtree.edges[rid]
plan = plan_f[::-1]
plan.extend(plan_r)
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
if self.planning_env.visualize:
[
self.planning_env.PlotEdge(plan[i - 1], plan[i])
for i in range(1, len(plan))
]
return plan
def Plan(self, start_config, goal_config, epsilon = 0.001):
ftree = RRTTree(self.planning_env, start_config)
rtree = RRTTree(self.planning_env, goal_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
while True:
sample = self.planning_env.GenerateRandomConfiguration()
f_vid, f_vtx = ftree.GetNearestVertex(sample)
r_vid, r_vtx = rtree.GetNearestVertex(sample)
f_best = self.planning_env.Extend(f_vtx, sample)
# grow towards f_best if possible
sample = f_best if f_best != None else sample
r_best = self.planning_env.Extend(r_vtx, sample)
if f_best is not None:
f_new_vid = ftree.AddVertex(f_best)
ftree.AddEdge(f_vid, f_new_vid)
self.planning_env.PlotEdge(f_vtx, f_best)
if r_best is not None:
r_new_vid = rtree.AddVertex(r_best)
rtree.AddEdge(r_vid, r_new_vid)
self.planning_env.PlotEdge(r_vtx, r_best)
if f_best != None and r_best != None:
dist = self.planning_env.ComputeDistance(f_best, r_best)
if dist < epsilon:
# build plan here
cursor = f_new_vid
while(cursor != ftree.GetRootId()):
plan.append(ftree.vertices[cursor])
cursor = ftree.edges[cursor]
plan.append(ftree.vertices[cursor])
plan.reverse()
cursor = r_new_vid
while(cursor != rtree.GetRootId()):
cursor = rtree.edges[cursor]
plan.append(rtree.vertices[cursor])
break
return plan
def Plan(self, start_config, goal_config, epsilon=0.001):
start_time = time.time()
ftree = RRTTree(self.planning_env, start_config)
rtree = RRTTree(self.planning_env, goal_config)
plan = []
rplan = []
fplan = []
expansions = 0
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt connect planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
print "startConfig = [%.2f, %.2f]" % (start_config[0], start_config[1])
print "goalConfig = [%.2f, %.2f]" % (goal_config[0], goal_config[1])
disc = True
while (disc):
if (random.random() < 0.5):
desConfig = self.planning_env.GenerateRandomConfiguration()
else:
desConfig = goal_config
expansions += 1
[nearID, nearConfig] = ftree.GetNearestVertex(desConfig)
extension = self.planning_env.Extend(nearConfig, desConfig)
if (extension != None):
extIDf = ftree.AddVertex(extension)
ftree.AddEdge(nearID, extIDf)
#self.planning_env.PlotEdge(nearConfig, extension);
#if(numpy.array_equal(extension, goal_config)):
# disc = False
if (random.random() < 0.95):
desConfig = self.planning_env.GenerateRandomConfiguration()
else:
desConfig = start_config
expansions += 1
[nearID, nearConfig] = rtree.GetNearestVertex(desConfig)
extension = self.planning_env.Extend(nearConfig, desConfig)
if (extension != None):
extIDr = rtree.AddVertex(extension)
rtree.AddEdge(nearID, extIDr)
#self.planning_env.PlotEdge(nearConfig, extension);
#if(numpy.array_equal(extension, start_config)):
# disc = False
for i in range(len(ftree.vertices)):
if (numpy.linalg.norm(
numpy.array(desConfig) -
numpy.array(ftree.vertices[i])) < 5):
lastlink = self.planning_env.Extend(
extension, ftree.vertices[i])
if (numpy.array_equal(lastlink, ftree.vertices[i])):
lastIDr = rtree.AddVertex(lastlink)
rtree.AddEdge(extIDr, lastIDr)
#self.planning_env.PlotEdge(extension, lastlink);
disc = False
extIDf = i
expansions += 1
break
lastlink_cp = lastlink
rplan.insert(0, goal_config)
while 1:
if (extIDr == rtree.GetRootId()):
break
else:
rplan.insert(1, extension)
extIDr = rtree.edges[extIDr]
extension = rtree.vertices[extIDr]
plan = list(reversed(rplan))
plan.insert(0, start_config)
while 1:
if (extIDf == ftree.GetRootId()):
break
else:
plan.insert(1, lastlink)
extIDf = ftree.edges[extIDf]
lastlink = ftree.vertices[extIDf]
for config in plan:
print "fconfig = [%.2f, %.2f]" % (config[0], config[1])
print("--- %s seconds ---" % (time.time() - start_time))
print "Total expansions = %d" % (expansions)
print("--- %s seconds ---" % (time.time() - start_time))
print("--- %s path length ---" % len(plan))
print("--- %s vertices ---" %
(len(ftree.vertices) + len(rtree.vertices) - 1))
return plan
def Plan(self, start_config, goal_config, epsilon = 0.001):
ftree = RRTTree(self.planning_env, start_config)
rtree = RRTTree(self.planning_env, goal_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt connect planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
#plan.append(start_config)
#RRT
dist = float('inf')
count = 0
while dist > epsilon:
# Bias Sampling with P of sampling towards the Goal = 0.1
if not count % 2:
p = random.random()
if p < 0.1:
f_q_r = goal_config
else:
f_q_r = self.planning_env.GenerateRandomConfiguration()
else:
f_q_r = numpy.copy(r_q_c)
# Get the Vertex closest to q_r
f_sid, f_q_n = ftree.GetNearestVertex(f_q_r)
# Using linear interpolation to get the furthest vertex without collision
f_q_c = self.planning_env.Extend(f_q_n, f_q_r)
# Add vertex to the tree
f_eid = ftree.AddVertex(f_q_c)
# Add an edge on the tree
ftree.AddEdge(f_sid, f_eid)
if self.visualize:
self.planning_env.PlotEdge(f_q_n, f_q_c)
if count % 2:
p = random.random()
if p < 0.1:
r_q_r = start_config
else:
r_q_r = self.planning_env.GenerateRandomConfiguration()
else:
r_q_r = numpy.copy(f_q_c)
# r_q_r = numpy.copy(f_q_c)
#for reverse tree
r_sid, r_q_n = rtree.GetNearestVertex(r_q_r)
# Using linear interpolation to get the furthest vertex without collision
r_q_c = self.planning_env.Extend(r_q_n, r_q_r)
if(numpy.array_equal(r_q_n,r_q_c)):
print("collision")
# Add vertex to the r tree
r_eid = rtree.AddVertex(r_q_c)
# Add an edge on the r tree
rtree.AddEdge(r_sid, r_eid)
# Check how close we are to the goal
dist = self.planning_env.ComputeDistance(f_q_c, r_q_c)
count += 1
print(dist)
# print q_n
print f_q_c, f_q_n, r_q_c, r_q_n, r_q_r
print rtree, ftree
if self.visualize:
self.planning_env.PlotEdge(r_q_n, r_q_c)
print("trees constructed")
vid = f_eid
while (vid != ftree.GetRootId()):
plan = [ftree.vertices[vid]] + plan
vid = ftree.edges[vid]
vid = r_eid
while (vid != rtree.GetRootId()):
vid = rtree.edges[vid]
plan = plan + [rtree.vertices[vid]]
plan.append(goal_config)
print("finished adding plan")
print plan
return plan