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, goal_config)
rtree = RRTTree(self.planning_env, goal_config, goal_config)
plan = []
start_time = time.time()
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
count = 0
start = [round(q, 3) for q in start_config]
goal = [round(f, 3) for f in goal_config]
num_turns = 1 / epsilon
while count != num_turns:
count = count + 1
rand = self.planning_env.generate_random_configuration()
#print "rand",rand
#get neighbours
fNid, fneighbour = ftree.GetNearestVertex(rand)
rNid, rneighbour = rtree.GetNearestVertex(rand)
fnew_guy = self.planning_env.connect(fneighbour, rand)
rnew_guy = self.planning_env.connect(rneighbour, rand)
fNew_id = ftree.AddVertex(fnew_guy)
rNew_id = rtree.AddVertex(rnew_guy)
#print "neighbour",neighbour,"new_guy",new_guy
ftree.AddEdge(fNid, fNew_id)
rtree.AddEdge(rNid, rNew_id)
#print "new_guy",new_guy
if self.planning_env.dimension == 2:
self.planning_env.PlotEdge(fneighbour, fnew_guy)
self.planning_env.PlotEdge(rneighbour, rnew_guy)
if fnew_guy == rnew_guy:
print "Goal Reached"
last = fNew_id
book = []
while last != 0:
book.append(ftree.vertices[last])
last = ftree.edges[last]
book.append(ftree.vertices[last])
x = len(book)
nicebook = [0] * x
for i in range(x):
nicebook[i] = book[x - i - 1]
last = rNew_id
while last != 0:
nicebook.append(rtree.vertices[last])
last = rtree.edges[last]
nicebook.append(rtree.vertices[last])
print("--- %s seconds ---" % (time.time() - start_time))
print "count", count
dist = 0
f = start
for i in nicebook:
dist = dist + self.planning_env.compute_distance(f, i)
f = i
print "total path =", dist
return nicebook
print "Goal NOT foud"
return 0
return plan
class RRTConnectPlanner(object):
def __init__(self, planning_env, visualize):
self.planning_env = planning_env
self.visualize = visualize
def Plan(self, start_config, goal_config, epsilon=0.01):
start_config = numpy.copy(start_config)
goal_config = numpy.copy(goal_config)
self.ftree = RRTTree(self.planning_env, start_config)
self.rtree = RRTTree(self.planning_env, goal_config)
plan = []
#pdb.set_trace()
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
self.ftree.AddEdge(self.ftree.GetRootId, self.ftree.GetRootId)
self.rtree.AddEdge(self.rtree.GetRootId, self.rtree.GetRootId)
q_nearest_f = start_config
q_nearest_r = goal_config
goal_index_f = 0
goal_index_r = 0
while (self.planning_env.ComputeConfigDistance(q_nearest_f,
q_nearest_r) > epsilon):
#pdb.set_trace()
q_target = self.planning_env.GenerateRandomConfiguration()
vid_f, q_nearest_f = self.ftree.GetNearestVertex(q_target)
vid_r, q_nearest_r = self.rtree.GetNearestVertex(q_target)
q_extended_f = self.planning_env.ExtendN(q_nearest_f, q_target)
q_extended_r = self.planning_env.ExtendN(q_nearest_r, q_target)
if q_extended_f is not None:
if numpy.array_equal(q_nearest_f, q_extended_f) == False:
self.ftree.AddVertex(q_extended_f)
self.ftree.AddEdge(vid_f, len(self.ftree.vertices) - 1)
#self.planning_env.PlotEdge(q_nearest_f,q_extended_f)
if q_extended_r is not None:
if numpy.array_equal(q_nearest_r, q_extended_r) == False:
self.rtree.AddVertex(q_extended_r)
self.rtree.AddEdge(vid_r, len(self.rtree.vertices) - 1)
#self.planning_env.PlotEdge(q_nearest_r,q_extended_r)
if q_extended_f is not None and q_extended_r is not None:
if numpy.array_equal(q_extended_f, q_extended_r):
goal_index_f = len(self.ftree.vertices) - 1
goal_index_r = len(self.rtree.vertices) - 1
break
#build tree
current_index_f = goal_index_f
current_index_r = goal_index_r
while current_index_f != 0:
plan.append(self.ftree.vertices[current_index_f])
current_index_f = self.ftree.edges[current_index_f]
plan.append(self.ftree.vertices[current_index_f])
plan = plan[::-1]
while current_index_r != 0:
plan.append(self.rtree.vertices[current_index_r])
current_index_r = self.rtree.edges[current_index_r]
plan.append(self.rtree.vertices[current_index_r])
print "Tree Size: " + str(
len(self.rtree.vertices) + len(self.ftree.vertices))
return plan
class RRTConnectPlanner(object):
def __init__(self, planning_env, visualize):
self.planning_env = planning_env
self.visualize = visualize
self.tree1 = []
self.tree2 = []
self.path1 = []
self.path2 = []
self.result1 = []
self.result2 = []
def Plan(self, start_config, goal_config, epsilon = 0.5):
self.tree1 = RRTTree(self.planning_env, start_config)
self.tree2 = RRTTree(self.planning_env, goal_config)
plan = []
start_time = time.time()
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)
# plan.append(goal_config)
# simple:
# goal_tf = numpy.eye(4,dtype = float)
# goal_tf[0,3] = goal_config[0]
# goal_tf[1,3] = goal_config[1]
#
# start_tf = numpy.eye(4,dtype = float)
# start_tf[0,3] = start_config[0]
# start_tf[1,3] = start_config[1]
self.tree1.vertices = []
#simple:
# self.tree1.AddVertex(start_tf)
self.tree1.AddVertex(start_config)
self.tree2.vertices = []
# simple:
# self.tree2.AddVertex(goal_tf)
self.tree2.AddVertex(goal_config)
count = 0
while (True):
count += 1
# Generate random point
qr = self.planning_env.GenerateRandomConfiguration()
# Find the nearest point to the random point
# qi: nearest point
#simple:
# if(numpy.shape(qr) != (4, 4)):
if (numpy.shape(qr) != (1, 7)):
qr = self.planning_env.GenerateRandomConfiguration()
qi_id1, qi1 = self.tree1.GetNearestVertex(qr)
qi_id2, qi2 = self.tree2.GetNearestVertex(qr)
else:
qi_id1, qi1 = self.tree1.GetNearestVertex(qr)
qi_id2, qi2 = self.tree2.GetNearestVertex(qr)
qc1 = self.planning_env.Extend(qi1, qr)
qc2 = self.planning_env.Extend(qi2, qr)
qc_id1 = self.tree1.AddVertex(qc1)
qc_id2 = self.tree2.AddVertex(qc2)
self.tree1.AddEdge(qi_id1, qc_id1)
self.tree2.AddEdge(qi_id2, qc_id2)
qc1_qc2_dist = self.planning_env.ComputeDistance(qc1, qc2)
# when reach the goal, break the while loop
if(qc1_qc2_dist < epsilon):
print("Reach the goal is qc_1 {} and qc_2 {} with dist {}".format(qc_id1, qc_id2 , qc1_qc2_dist))
break
path1 = []
plan1 = []
level1 = 0
visited1 = [False] * len(self.tree1.vertices)
path2 = []
plan2 = []
level2 = 0
visited2 = [False] * len(self.tree2.vertices)
# use DFS to find the path from the start_config to the goal_config
# print("Print all paths1 is {} ".format(self.tree1.vertices))
# print("Print all paths2 is {} ".format(self.tree2.vertices))
self.printAllPathsUtil1(0, qc_id1,visited1,path1,level1)
self.printAllPathsUtil2(0, qc_id2, visited2, path2, level2)
# Hacky way to truncate the extra path return by the DFS
idx1 = 0
while(self.path1[idx1] is not qc_id1):
self.result1.append(self.path1[idx1])
idx1 += 1
self.result1.append(qc_id1)
# Look up the vertices given the vertices' id
for i in self.result1:
v = self.tree1.vertices[i]
#simple
# plan1.append((v[0,3],v[1,3]))
plan1.append(v)
idx2 = 0
while(self.path2[idx2] is not qc_id2):
self.result2.append(self.path2[idx2])
idx2 += 1
self.result2.append(qc_id2)
# Look up the vertices given the vertices' id
for i in self.result2:
v = self.tree2.vertices[i]
#simple:
# plan2.append((v[0,3],v[1,3]))
plan2.append(v)
# given two path from two trees, plan1 and plan2, concat plan1 with plan2 (reverse) to give the final path
reversed_plan2 = plan2[::-1]
all_plan = numpy.concatenate((plan1,reversed_plan2),0)
step_dist = self.planning_env.step
all_step_dist = numpy.sum(len(all_plan)*step_dist)
elapsed_time = time.time() - start_time
vertices = len(numpy.concatenate((self.tree1.vertices,self.tree2.vertices),0))
print("RRT CONNECT: All step dist: {}, elapsed time is {} and vertices number {} ".format(all_step_dist, elapsed_time, vertices))
return all_plan
def printAllPathsUtil1(self, u, d, visited, path, level):
level += 1
visited[u] = True
path.append(u)
if u == d:
print("---------------------------------------------")
print("This is path1 {}".format(path))
# self.result.append(path)
self.path1 = path
else:
for i in self.tree1.edges[u]:
if visited[i] == False:
print("level %d, val %d " % (level, i))
self.printAllPathsUtil1(i, d, visited, path, level)
def printAllPathsUtil2(self, u, d, visited, path, level):
level += 1
visited[u] = True
path.append(u)
if u == d:
print("---------------------------------------------")
print("This is path2 {}".format(path))
# self.result.append(path)
self.path2 = path
else:
for i in self.tree2.edges[u]:
if visited[i] == False:
print("level %d, val %d " % (level, i))
self.printAllPathsUtil2(i, d, visited, path, level)
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 = []
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
while (1):
#import IPython
#IPython.embed()
#drop one vertex while
v_drop = self.planning_env.GenerateRandomConfiguration()
# compare the distance from ftree and the rtree
i_nearst_f, v_nearst_f = ftree.GetNearestVertex(v_drop)
i_nearst_r, v_nearst_r = rtree.GetNearestVertex(v_drop)
ext_v_drop_to_f = self.planning_env.Extend(v_nearst_f, v_drop)
ext_v_drop_to_r = self.planning_env.Extend(v_nearst_r, v_drop)
if (ext_v_drop_to_f == None or ext_v_drop_to_r == None): continue
dist_f = self.planning_env.ComputeDistance(ext_v_drop_to_f, v_drop)
dist_r = self.planning_env.ComputeDistance(ext_v_drop_to_r, v_drop)
# if the v_drop is close enough to both two tree,
# add this vertice into both trees and then terminate.
if (dist_f <= epsilon and dist_r <= epsilon):
final_vid_v_drop_r = rtree.AddVertex(v_drop)
rtree.AddEdge(i_nearst_r, final_vid_v_drop_r)
if (len(v_nearst_r) == 2
): #make sure there is 2D config will show the plot
self.planning_env.PlotEdge(v_nearst_r, v_drop)
final_vid_v_drop_f = ftree.AddVertex(v_drop)
ftree.AddEdge(i_nearst_f, final_vid_v_drop_f)
if (len(v_nearst_f) == 2
): #make sure there is 2D config will show the plot
self.planning_env.PlotEdge(v_nearst_f, v_drop)
break
elif (dist_f <= dist_r): # v_drop more close to the ftree
vid_v_drop_f = ftree.AddVertex(ext_v_drop_to_f)
ftree.AddEdge(i_nearst_f, vid_v_drop_f)
if (len(v_nearst_f) == 2):
self.planning_env.PlotEdge(v_nearst_f, ext_v_drop_to_f)
else: # v_drop more close to the rtree
vid_v_drop_r = rtree.AddVertex(ext_v_drop_to_r)
rtree.AddEdge(i_nearst_r, vid_v_drop_r)
if (len(v_nearst_r) == 2):
self.planning_env.PlotEdge(v_nearst_r, ext_v_drop_to_r)
#assert(v_drop != None)
# Terminate condition : the distance from v_drop to both ftree and rtree is less than epsilon
# use tree.edge one both tree to find the valid path
# for ftree's path : [start_config -> v_drop)
total_time = time.time() - start_time
plan_ftree = self.find_path(ftree, 0, final_vid_v_drop_f)
plan_ftree.reverse()
# for rtree's path: [Goal_config -> v_drop) then reverse
plan_rtree = self.find_path(rtree, 0, final_vid_v_drop_r)
# combine ftree's path + v_drop + rtree's path
for i in plan_ftree:
plan.append(i)
plan.append(v_drop)
for i in plan_rtree:
plan.append(i)
dist_plan = self.planning_env.ComputePathLength(plan)
print "total plan distance"
print dist_plan
print "total vertice in tree "
print len(ftree.vertices) + len(rtree.vertices) - 1
# end of implement
print "total plan time = "
print total_time
print " "
return plan
def Plan(self, start_config, goal_config, epsilon=0.001):
# count for executing timing
import time
t0 = time.clock()
# ftree start from start_config
ftree = RRTTree(self.planning_env, start_config)
# rtree start from goal_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
# start the Bidirection RRT algorithm
# set up the number of iterations we want to try before we give up
num_iter = 1000
# if two trees, ftree and rtree suceeed to connect each other, make isFail = False
isFail = True
import random
import time
num_vertex = 0
for i in range(num_iter):
# generate random configuration, config_q
config_q = self.planning_env.GenerateRandomConfiguration()
# get the nearest neighbor from the ftree and rtree, based on config_q
f_mid, f_mdist = ftree.GetNearestVertex(config_q)
r_mid, r_mdist = rtree.GetNearestVertex(config_q)
# get the nearest neighbor from ftree and rtree, f_config_m and r_config_m
f_config_m = ftree.vertices[f_mid]
r_config_m = rtree.vertices[r_mid]
# append f_config_n/r_config_n to the tree ftree/rtree if they are addable
# where f/r_config_n is the extended node between f/r_config_m and config_q
f_config_n = self.planning_env.Extend(f_config_m, config_q)
r_config_n = self.planning_env.Extend(r_config_m, config_q)
# add f_config_n to ftree if addable
if not numpy.array_equal(f_config_n, f_config_m):
num_vertex = num_vertex + 1
ftree.AddVertex(f_config_n)
ftree.AddEdge(f_mid,
len(ftree.vertices) -
1) # id of config_n is the last one in vertices
# draw the expended edge
if len(f_config_m) == 2:
self.planning_env.PlotEdge(f_config_m, f_config_n)
# add r_config_n to rtree if addable
if not numpy.array_equal(r_config_n, r_config_m):
num_vertex = num_vertex + 1
rtree.AddVertex(r_config_n)
rtree.AddEdge(r_mid,
len(rtree.vertices) -
1) # id of config_n is the last one in vertices
# draw the expended edge
if len(r_config_m) == 2:
self.planning_env.PlotEdge(r_config_m, r_config_n)
# break the loop if ftree and rtree connected
if numpy.array_equal(
f_config_n, r_config_n) and not numpy.array_equal(
f_config_n, f_config_m) and not numpy.array_equal(
r_config_n, r_config_m):
isFail = False
f_goal_id = len(ftree.vertices) - 1
r_goal_id = len(rtree.vertices) - 1
break
# recursive append the waypoints to the tree based on the tree.edges information
# stop if we trace back to root, id = 0
# we need to reverse for ftree not for rtree
print "number of vertices: %d" % (num_vertex)
# start here
if isFail:
print "path length: 0"
print "plan time: %f" % (time.clock() - t0)
return []
else:
# start to append waypoints from ftree
current_id = f_goal_id
while current_id != 0:
plan.append(ftree.vertices[current_id])
current_id = ftree.edges[current_id]
plan.append(
ftree.vertices[current_id]) # add start config to the plan
# reverse the order of plan
plan = plan[::-1]
# keep adding waypoints from rtree, and ignore the overlapped config
current_id = rtree.edges[r_goal_id]
while current_id != 0:
plan.append(rtree.vertices[current_id])
current_id = rtree.edges[current_id]
plan.append(
rtree.vertices[current_id]) # add goal config to the plan
path_length = 0
for i in xrange(len(plan) - 1):
path_length = path_length + self.planning_env.ComputeDistance(
plan[i], plan[i + 1])
print "path length: %f" % (path_length)
print "plan time: %f" % (time.clock() - t0)
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
# OUR TODOs:
# 1) Intermediate nodes are not being added to the tree when making connection between the node of one tree with the other tree.
#
# 2) Path Shortening with linear interpolation in between two nodes
#
# 3) May have to think over the Extend function if we want to change this or not. Extend function currently is
# essentially taking a step size of infinity i.e. reach q_rand as much as possible with a specific resolution
# of 'max_step_size'. As a result, potentially not many intermediate nodes on an edge between q_rand and q_near.
#
# 4) Carry out more testings to validate the implementation. If collision with obstacle observed at corners or whatever, try
# decreasing 'max_step_size' in Extend function and test.
self.q_id_parent_dict_ftree = {}
self.q_id_config_dict_ftree = {}
self.q_id_parent_dict_rtree = {}
self.q_id_config_dict_rtree = {}
q_goal_id = -1
q_start_id = 0
goal_found = False
swap_trees = False
NUM_ITERATIONS = 5000
self.env = self.planning_env.robot.GetEnv()
with self.env:
with self.planning_env.robot.CreateRobotStateSaver():
for k in range(0, NUM_ITERATIONS):
print "Iteration: " + str(k)
q_rand = self.planning_env.GenerateRandomConfiguration()
if (swap_trees == False):
#Picking q_near in the tree
q_near_id, q_near = ftree.GetNearestVertex(q_rand)
q_new = self.planning_env.Extend(q_near, q_rand)
if (q_new is not None):
q_new_id = ftree.AddVertex(q_new)
self.q_id_config_dict_ftree[q_new_id] = q_new
ftree.AddEdge(q_new_id, q_near_id)
self.q_id_parent_dict_ftree[q_new_id] = q_near_id
# self.planning_env.PlotEdge(q_near, q_new)
#Function 'Connect'
q_connect = q_new
q_connect_id = q_new_id
q_near_id_connect, q_near_connect = rtree.GetNearestVertex(
q_connect)
q_new_connect = self.planning_env.Extend(
q_near_connect, q_connect)
if (q_new_connect is not None):
q_new_id_connect = rtree.AddVertex(
q_new_connect)
self.q_id_config_dict_rtree[
q_new_id_connect] = q_new_connect
rtree.AddEdge(q_new_id_connect,
q_near_id_connect)
self.q_id_parent_dict_rtree[
q_new_id_connect] = q_near_id_connect
# self.planning_env.PlotEdge(q_near, q_new)
if (self.planning_env.ComputeDistance(
q_new_connect, q_connect) < epsilon):
print "Path Found!"
goal_found = True
# self.planning_env.PlotEdge(q_connect, q_near_connect)
q_ftree_connect_id = q_connect_id
q_rtree_connect_id = q_near_id_connect
plan = self.getPlan(
q_ftree_connect_id, start_config,
q_rtree_connect_id, goal_config)
break
else:
swap_trees = True
else:
swap_trees = True
else:
swap_trees = True
else:
#Picking q_near in the tree
q_near_id, q_near = rtree.GetNearestVertex(q_rand)
q_new = self.planning_env.Extend(q_near, q_rand)
if (q_new is not None):
q_new_id = rtree.AddVertex(q_new)
self.q_id_config_dict_rtree[q_new_id] = q_new
rtree.AddEdge(q_new_id, q_near_id)
self.q_id_parent_dict_rtree[q_new_id] = q_near_id
# self.planning_env.PlotEdge(q_near, q_new)
#Function 'Connect'
q_connect = q_new
q_connect_id = q_new_id
q_near_id_connect, q_near_connect = ftree.GetNearestVertex(
q_connect)
q_new_connect = self.planning_env.Extend(
q_near_connect, q_connect)
if (q_new_connect is not None):
q_new_id_connect = ftree.AddVertex(
q_new_connect)
self.q_id_config_dict_ftree[
q_new_id_connect] = q_new_connect
ftree.AddEdge(q_new_id_connect,
q_near_id_connect)
self.q_id_parent_dict_ftree[
q_new_id_connect] = q_near_id_connect
# self.planning_env.PlotEdge(q_near, q_new)
if (self.planning_env.ComputeDistance(
q_new_connect, q_connect) < epsilon):
print "Path Found!"
goal_found = True
# self.planning_env.PlotEdge(q_connect, q_near_connect)
q_rtree_connect_id = q_connect_id
q_ftree_connect_id = q_near_id_connect
plan = self.getPlan(
q_ftree_connect_id, start_config,
q_rtree_connect_id, goal_config)
break
else:
swap_trees = False
else:
swap_trees = False
else:
swap_trees = False
if (not goal_found):
print "Goal not found in " + str(
NUM_ITERATIONS) + "iterations."
else:
num_vertices = len(plan)
print "Number of Vertices: " + str(
num_vertices) + " nodes."
return plan, num_vertices
def Plan(self, start_config, goal_config, epsilon=0.5):
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
# initial conditions
ftree.AddVertex(start_config)
rtree.AddVertex(goal_config)
while (True):
# generate 2 points, one for each side
new_f = self.planning_env.GenerateRandomConfiguration()
# find nearest vertex
f_id, f_vert = ftree.GetNearestVertex(new_f)
# try to extend
pot_f = self.planning_env.Extend(f_vert, new_f)
if (pot_f != None):
dist_f = self.planning_env.ComputeDistance(pot_f, start_config)
# add tree elements
temp = ftree.AddVertex(pot_f)
ftree.AddEdge(f_id, temp)
self.planning_env.PlotEdge(f_vert, pot_f)
new_r = self.planning_env.GenerateRandomConfiguration()
r_id, r_vert = rtree.GetNearestVertex(new_r)
pot_r = self.planning_env.Extend(r_vert, new_r)
if (pot_r != None):
dist_r = self.planning_env.ComputeDistance(pot_r, goal_config)
# add tree elements
temp = rtree.AddVertex(pot_r)
rtree.AddEdge(r_id, temp)
self.planning_env.PlotEdge(r_vert, pot_r)
if (dist_f <= epsilon
and dist_r <= epsilon) or self.planning_env.Extend(
f_vert, r_vert) != None:
break
# back prop, from middle to the front
get_keys = ftree.edges.keys()
tree_key = get_keys[len(ftree.edges) - 1]
while (True):
plan.append(ftree.vertices[tree_key])
tree_key = ftree.edges.get(tree_key)
if (tree_key == 0):
break
plan.append(start_config)
plan.reverse() # now it's from beginning to the middle
# from the middle to end, no need to reverse
get_keys = rtree.edges.keys()
tree_key = get_keys[len(rtree.edges) - 1]
while (True):
plan.append(rtree.vertices[tree_key])
tree_key = rtree.edges.get(tree_key)
if (tree_key == 0):
break
plan.append(goal_config)
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