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"