def nearestNeighbor_Dubins_Cost(p,Tree):
"""
returns the node which has shortest dubins distance to p
"""
# --------- Compute arguments for dubins curve -----------------#
d = math.sqrt(distance_Euclidian(p,Tree[0].getVertex()))
angle_goal = math.atan2(p[1] - Tree[0].getVertex()[1],p[0] - Tree[0].getVertex()[0])
if(angle_goal < 0):
angle_goal = 2*math.pi + angle_goal
alpha = Tree[0].getVertex()[2] - angle_goal
if(alpha < 0):
alpha = 2*math.pi + alpha
beta = math.atan2(p[1] - Tree[0].getVertex()[1], p[0] - Tree[0].getVertex()[0])
if (beta < 0):
beta = 2* math.pi + beta
beta = p[2] - beta
if(beta < 0):
beta = 2*math.pi + beta
dubinscurve = dubins(alpha,beta,d,Tree[0])
min_d = dubinscurve[1] + dubinscurve[2] + dubinscurve[3]
if(min_d >= 0):
min_d = min_d + Tree[0].getCost()
# --------------------------------------------------------------------- #
nN = Tree[0]
count = 0
for node in Tree:
d = math.sqrt(distance_Euclidian(p,node.getVertex()))
angle_goal = math.atan2(p[1] - node.getVertex()[1], p[0] - node.getVertex()[0])
alpha = node.getVertex()[2] - angle_goal
if(alpha < 0):
alpha = 2*math.pi + alpha
beta = math.atan2(p[1] - node.getVertex()[1], p[0] - node.getVertex()[0])
if (beta < 0):
beta = 2* math.pi + beta
beta = p[2] - beta
if(beta < 0):
beta = 2*math.pi + beta
dubinscurve = dubins(alpha,beta,d,node)
d = dubinscurve[1] + dubinscurve[2] + dubinscurve[3]
if(d >= 0):
d = d + node.getCost()
if (d >= 0) and ((d < min_d) or (min_d < 0)):
nN = node
min_d = d
count = count + 1
print "nearest neighbor found ",nN.getVertex()
return nN
def nearestNeighbor_Dubins(p, Tree):
"""
returns the node which has shortest dubins distance to p
"""
# --------- Compute arguments for dubins curve -----------------#
d = math.sqrt(distance_Euclidian(p, Tree[0].getVertex()))
angle_goal = math.atan2(p[1] - Tree[0].getVertex()[1],
p[0] - Tree[0].getVertex()[0])
if (angle_goal < 0):
angle_goal = 2 * math.pi + angle_goal
alpha = Tree[0].getVertex()[2] - angle_goal
if (alpha < 0):
alpha = 2 * math.pi + alpha
beta = math.atan2(p[1] - Tree[0].getVertex()[1],
p[0] - Tree[0].getVertex()[0])
if (beta < 0):
beta = 2 * math.pi + beta
beta = p[2] - beta
if (beta < 0):
beta = 2 * math.pi + beta
dubinscurve = dubins(alpha, beta, d, Tree[0])
min_d = dubinscurve[1] + dubinscurve[2] + dubinscurve[3]
nN = Tree[0]
count = 0
for node in Tree:
d = math.sqrt(distance_Euclidian(p, node.getVertex()))
angle_goal = math.atan2(p[1] - node.getVertex()[1],
p[0] - node.getVertex()[0])
alpha = node.getVertex()[2] - angle_goal
if (alpha < 0):
alpha = 2 * math.pi + alpha
beta = math.atan2(p[1] - node.getVertex()[1],
p[0] - node.getVertex()[0])
if (beta < 0):
beta = 2 * math.pi + beta
beta = p[2] - beta
if (beta < 0):
beta = 2 * math.pi + beta
dubinscurve = dubins(alpha, beta, d, node)
d = dubinscurve[1] + dubinscurve[2] + dubinscurve[3]
if (d >= 0) and ((d < min_d) or (min_d < 0)):
nN = node
min_d = d
count = count + 1
print "nearest neighbor found ", nN.getVertex()
return nN
def build_RRT(x1, y1, th1, x2, y2, th2, Tree):
global RRT_goalfound
p_init = (x1, y1, th1)
p_goal = (x2, y2, th2)
#print ("init ",p_init," goal ", p_goal)
init_Node = RRTNode(p_init)
Tree.append(init_Node)
for i in range(1000):
#print i
# 50% of the time, sample the goal
# 50% of the time, sample other half randomly
# extend right Tree to goal
if (RRT_goalfound == False):
success = False
while (success == False):
choice = random.random()
if (choice >= 0.3):
p_rand = (x2, y2, th2)
else:
p_rand = (random.random() * 60 / ROBOT_RADIUS,
(random.random() * 120) / ROBOT_RADIUS,
random.random() * 2 * math.pi)
#elif(choice>=0.1):
# p_rand = (random.random()*60/ROBOT_RADIUS,(60+random.random()*60)/ROBOT_RADIUS,random.random()*2*math.pi)
#else:
# p_rand = (random.random()*60/ROBOT_RADIUS,(random.random()*120)/ROBOT_RADIUS,random.random()*2*math.pi)
#print p_rand
success = extend_RRT(p_rand, Tree)
success_vertex = success.getVertex()
# try to connect this new vertex to the goal using a dubins curve.
#if this succeeds, stop building tree and return the node that connects to the goal via dubins
(alpha, beta, d) = dubinsparams_calc(success_vertex, p_goal)
dubinscurve = dubins(alpha, beta, d, success)
min_d = dubinscurve[1] + dubinscurve[2] + dubinscurve[3]
if (min_d >= 0):
#print "goal node ",success_vertex," start node ",p_goal, "curve number ",dubinscurve[0]
RRT_goalfound = True
Tree_end = success
return Tree_end
#print "build_RRT returning false"
return False
def build_RRT(x1,y1,th1,x2,y2,th2,Tree):
global RRT_goalfound
p_init = (x1,y1,th1)
p_goal = (x2,y2,th2)
#print ("init ",p_init," goal ", p_goal)
init_Node = RRTNode(p_init)
Tree.append(init_Node)
for i in range(1000):
#print i
# 50% of the time, sample the goal
# 50% of the time, sample other half randomly
# extend right Tree to goal
if(RRT_goalfound == False):
success = False
while (success == False) :
choice = random.random()
if(choice >= 0.3):
p_rand = (x2,y2,th2)
else:
p_rand = (random.random()*60/ROBOT_RADIUS,(random.random()*120)/ROBOT_RADIUS,random.random()*2*math.pi)
#elif(choice>=0.1):
# p_rand = (random.random()*60/ROBOT_RADIUS,(60+random.random()*60)/ROBOT_RADIUS,random.random()*2*math.pi)
#else:
# p_rand = (random.random()*60/ROBOT_RADIUS,(random.random()*120)/ROBOT_RADIUS,random.random()*2*math.pi)
#print p_rand
success = extend_RRT(p_rand,Tree)
success_vertex = success.getVertex()
# try to connect this new vertex to the goal using a dubins curve.
#if this succeeds, stop building tree and return the node that connects to the goal via dubins
(alpha,beta,d) = dubinsparams_calc(success_vertex,p_goal)
dubinscurve = dubins(alpha,beta,d,success)
min_d = dubinscurve[1] + dubinscurve[2] + dubinscurve[3]
if(min_d >= 0):
#print "goal node ",success_vertex," start node ",p_goal, "curve number ",dubinscurve[0]
RRT_goalfound = True
Tree_end = success
return Tree_end
#print "build_RRT returning false"
return False
def plan(canvas, X, Y, TH, X2, Y2, TH2, Tree):
global RRT_goalfound
t1 = time.time()
RRT_goalfound = False
query_node = build_RRT(X, Y, TH, X2, Y2, TH2, Tree)
if (query_node == False):
return False
t1 = time.time() - t1
canvas.create_oval((X2) * 5 * 1.91 - 2,
600 - (Y2 * 5 * 1.91) - 2, (X2 * 5 * 1.91) + 2,
600 - (Y2 * 5 * 1.91) + 2,
width=1,
outline='green',
fill='green')
count = 0
displayTree(canvas, Tree)
t2 = time.time()
path = query_RRT(query_node, Tree)
path.reverse()
count = len(path)
print "finish searching RRT for solution"
RRT_goal_node = path[-1]
#RRT_goal_node.display()
RRT_goal = RRT_goal_node.getVertex()
goal = (X2, Y2, TH2)
# --------- Compute arguments for dubins curve -----------------#
(alpha, beta, d) = dubinsparams_calc(RRT_goal, (X2, Y2, TH2))
#print "Dubins: d ",d," alpha ",alpha
(curve_number, t, p, q) = dubins(alpha, beta, d, RRT_goal_node)
t2 = time.time() - t2
#print "Dubins ",curve_number," t ",t," p ",p," q ",q
appendDubins(path, curve_number, t, p, q)
#print "EXECUTION TIME\n RRT built in ",t1,"seconds\n Path found in ",t2,"seconds"
return path
def build_RRT(x1, y1, th1, x2, y2, th2, Tree):
global RRT_goalfound
p_init = (x1, y1, th1)
p_goal = (x2, y2, th2)
init_Node = RRTNode(p_init)
Tree.append(init_Node)
for i in range(1000):
print i
# 50% of the time, sample the goal
# 50% of the time, sample other half randomly
# extend right Tree to goal
if (RRT_goalfound == False):
success = False
while (success == False):
choice = random.random()
if (choice >= 0.5):
p_rand = (x2, y2, th2)
elif (choice >= 0.3):
p_rand = (random.random() * 60 / 1.91,
(60 + random.random() * 60) / 1.91,
random.random() * 2 * math.pi)
else:
p_rand = (random.random() * 60 / 1.91,
(random.random() * 120) / 1.91,
random.random() * 2 * math.pi)
#print p_rand
success = extend_RRT(p_rand, Tree)
success_vertex = success.getVertex()
(alpha, beta, d) = dubinsparams_calc(success_vertex, p_goal)
dubinscurve = dubins(alpha, beta, d, success)
min_d = dubinscurve[1] + dubinscurve[2] + dubinscurve[3]
if (min_d >= 0):
print "goal node ", success_vertex, " start node ", p_goal, "curve number ", dubinscurve[
0]
RRT_goalfound = True
RightTree_goal = success
return RightTree_goal
return False
def plan(canvas,X,Y,TH,X2,Y2,TH2,Tree):
global RRT_goalfound
t1 = time.time()
RRT_goalfound = False
query_node = build_RRT(X,Y,TH,X2,Y2,TH2,Tree)
if(query_node == False):
return False
t1 = time.time() - t1
canvas.create_oval((X2)*5*1.91 - 2,600-(Y2*5*1.91)-2,(X2*5*1.91) +2,600-(Y2*5*1.91) + 2,width=1,outline = 'green',fill = 'green')
count = 0
displayTree(canvas,Tree)
t2 = time.time()
path = query_RRT(query_node,Tree)
path.reverse()
count = len(path)
print "finish searching RRT for solution"
RRT_goal_node = path[-1]
#RRT_goal_node.display()
RRT_goal = RRT_goal_node.getVertex()
goal = (X2,Y2,TH2)
# --------- Compute arguments for dubins curve -----------------#
(alpha,beta,d) = dubinsparams_calc(RRT_goal,(X2,Y2,TH2))
#print "Dubins: d ",d," alpha ",alpha
(curve_number,t,p,q) = dubins(alpha,beta,d,RRT_goal_node)
t2 = time.time() - t2
#print "Dubins ",curve_number," t ",t," p ",p," q ",q
appendDubins(path,curve_number,t,p,q)
#print "EXECUTION TIME\n RRT built in ",t1,"seconds\n Path found in ",t2,"seconds"
return path
def build_RRT(x1,y1,th1,x2,y2,th2,Tree):
global RRT_goalfound
p_init = (x1,y1,th1)
p_goal = (x2,y2,th2)
init_Node = RRTNode(p_init)
Tree.append(init_Node)
for i in range(1000):
print i
# 50% of the time, sample the goal
# 50% of the time, sample other half randomly
# extend right Tree to goal
if(RRT_goalfound == False):
success = False
while (success == False) :
choice = random.random()
if(choice >= 0.5):
p_rand = (x2,y2,th2)
elif(choice>=0.3):
p_rand = (random.random()*60/1.91,(60+random.random()*60)/1.91,random.random()*2*math.pi)
else:
p_rand = (random.random()*60/1.91,(random.random()*120)/1.91,random.random()*2*math.pi)
#print p_rand
success = extend_RRT(p_rand,Tree)
success_vertex = success.getVertex()
(alpha,beta,d) = dubinsparams_calc(success_vertex,p_goal)
dubinscurve = dubins(alpha,beta,d,success)
min_d = dubinscurve[1] + dubinscurve[2] + dubinscurve[3]
if(min_d >= 0):
print "goal node ",success_vertex," start node ",p_goal, "curve number ",dubinscurve[0]
RRT_goalfound = True
RightTree_goal = success
return RightTree_goal
return False
def startPlanner(X,Y,TH,X2,Y2,TH2):
global Tree
Tree = []
#canvas.delete(ALL)
#drawCourt(canvas)
X = float(X)
Y = float(Y)
TH = math.radians(float(TH))
X2 = float(X2)
Y2 = float(Y2)
TH2 = math.radians(float(TH2))
if not((newState((X,Y,TH))) and (newState((X2,Y2,TH2)))):
print "Invalid start and/or goal configuration"
return
t1 = time.time()
build_RRT(X,Y,TH)
t1 = time.time() - t1
count = 0
"""
for v in Tree:
x = v.getVertex()[0]*50
y = 600 - v.getVertex()[1]*50
canvas.create_oval(x-1,y-1,x+1,y+1,width = 1,outline = 'red',fill = 'red')
count = count + 1
#v.display()
"""
print "finished building RRT in time ",t1
t2 = time.time()
path = query_RRT((X,Y,TH),(X2,Y2,TH2))
path.reverse()
count = len(path)
print "finish searching RRT for solution"
RRT_goal_node = path[-1]
#RRT_goal_node.display()
RRT_goal = RRT_goal_node.getVertex()
goal = (X2,Y2,TH2)
# --------- Compute arguments for dubins curve -----------------#
d = math.sqrt(distance_Euclidian(RRT_goal,goal))
angle_goal = math.atan2(goal[1]-RRT_goal[1],goal[0]-RRT_goal[0])
if(angle_goal < 0):
angle_goal = 2*math.pi + angle_goal
alpha = RRT_goal[2] - angle_goal
if(alpha < 0):
alpha = 2*math.pi + alpha
beta = math.atan2(goal[1] - RRT_goal[1], goal[0] - RRT_goal[0])
if(beta < 0):
beta = 2*math.pi + beta
beta = goal[2] - beta
if(beta < 0):
beta = 2*math.pi + beta
print "Dubins: d ",d," alpha ",alpha
(curve_number,t,p,q) = dubins(alpha,beta,d,RRT_goal_node)
t2 = time.time() - t2
print "Dubins ",curve_number," t ",t," p ",p," q ",q
appendDubins(path,curve_number,t,p,q)
# --------------------------------------------------------------#
"""
i = 0
print "Total path length = ", len(path)
for v in path:
x = v.getVertex()[0]*50
y = 600 - v.getVertex()[1]*50
if(i < count):
color = 'blue'
else:
color = 'black'
canvas.create_oval(x - 1, y-1,x+1,y+1,width=1,outline = color, fill = color)
i = i+1
"""
#canvas.create_oval((X2)*50 - 2,600-(Y2)*50 -2,(X2)*50 +2,600-(Y2)*50 + 2,width=1,outline = 'green',fill = 'green')
print "EXECUTION TIME\n RRT built in ",t1,"seconds\n Path found in ",t2,"seconds"
