def startPlanner(X1,Y1,TH1,D_goal,TH_goal): global Tree_start_to_right,Tree_start_to_left,Tree_right_to_goal,Tree_right_to_left,Tree_start_to_goal X1 = util.cm_to_feet(X1) Y1 = util.cm_to_feet(Y1) Tree_start_to_right = [] Tree_right_to_goal = [] Tree_start_to_left = [] Tree_left_to_goal = [] Tree_start_to_goal = [] path_start_to_right = [] path_right_to_goal = [] path_start_to_left = [] path_left_to_goal = [] #canvas.delete(ALL) #util.drawCourt(canvas) # compute start and end points in the global coordinate frame. X = X1 Y = Y1 TH1 = math.radians(TH1) D_goal = util.cm_to_feet(float(D_goal)) TH_goal = float(TH_goal) TH = TH1 - math.radians(TH_goal) # angle to goal in global coord # = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg] X2 = X + D_goal*math.cos(TH) Y2 = Y + D_goal*math.sin(TH) TH2 = math.radians(45.0) print "start ",X,Y,TH1," end ",X2,Y2,TH2,"radius",ROBOT_RADIUS # rescale X and Y to Computation coordinate frame (Turning radius = 1) X = X/ROBOT_RADIUS Y = Y/ROBOT_RADIUS TH = TH1 X2 = X2/ROBOT_RADIUS Y2 = Y2/ROBOT_RADIUS TH2 = math.radians(45.0) if not((util.newState((X,Y,TH))) and (util.newState((X2,Y2,TH2)))): print "Invalid start and/or goal configuration" return # ----------- Check which case the start-goal configuration falls under --------------------- # Case 1: start below and goal above => angle at net = pi/2 # Case 2: start above and goal below => angle at net = 3*pi/2 # Case 3: start and goal at same side of net => dont check net at all! t1 = time.time() if((Y <= 60/ROBOT_RADIUS) and (Y2 >= 60/ROBOT_RADIUS)): case = 1
def isSafeDubins(curve_number,t,p,q,start_p): """ Given a start position and a Dubins curve, check the generated path for collisions. Return true if there's no collision and false if there is """ v = util.stepsize/util.ROBOT_RADIUS (x,y,theta) = start_p.getVertex() (startx,starty,starttheta) = (x,y,theta) #print (x,y,theta)," curve_number ",curve_number," t ",t," p ",p," q ",q # first action if((curve_number == 0) or (curve_number == 2) or (curve_number == 5)): #turn left action = util.turnLeft_sim elif((curve_number == 1) or (curve_number == 3) or (curve_number == 4)): #turn right action = util.turnRight_sim i = 0 while(i < t): (x,y,theta) = action(x,y,theta,v) point = (x,y,theta) if not (util.newState(point)): #print point," False" return False #print point," True" i = i + v #print "first action OK, ends at ",(x,y,theta) (x,y,theta) = action(startx,starty,starttheta,t) (startx,starty,starttheta) = (x,y,theta) # second action if (curve_number <= 3): # go forward action = util.goForward_sim elif(curve_number == 4): # turn left action = util.turnLeft_sim elif(curve_number == 5): # turn right action = util.turnRight_sim i = 0 while(i < p): (x,y,theta) = action(x,y,theta,v) point = (x,y,theta) if not (util.newState(point)): #print point," False" return False #print point," True" i = i+ v (x,y,theta) = action(startx,starty,starttheta,p) (startx,starty,starttheta) = (x,y,theta) # third action if(curve_number == 0) or (curve_number == 3) or (curve_number == 5): # turn left action = util.turnLeft_sim elif(curve_number == 1) or (curve_number == 2) or (curve_number == 4): # turn right action = util.turnRight_sim i = 0 while(i < q): (x,y,theta) = action(x,y,theta,v) point = (x,y,theta) if not (util.newState(point)): #print point," False" return False i = i + v # print "Third action OK" return True
def startPlanner(X1,Y1,TH1,D_goal,TH_goal): global Tree_start_to_right,Tree_start_to_left,Tree_right_to_goal,Tree_right_to_left,Tree_start_to_goal X1 = util.cm_to_feet(X1) Y1 = util.cm_to_feet(Y1) Tree_start_to_right = [] Tree_right_to_goal = [] Tree_start_to_left = [] Tree_left_to_goal = [] Tree_start_to_goal = [] path_start_to_right = [] path_right_to_goal = [] path_start_to_left = [] path_left_to_goal = [] #canvas.delete(ALL) #util.drawCourt(canvas) # compute start and end points in the global coordinate frame. X = X1 Y = Y1 TH1 = math.radians(TH1) D_goal = util.cm_to_feet(float(D_goal)) TH_goal = float(TH_goal) TH = TH1 - math.radians(TH_goal) # angle to goal in global coord # = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg] X2 = X + D_goal*math.cos(TH) Y2 = Y + D_goal*math.sin(TH) TH2 = math.radians(45.0) print "start ",X,Y,TH1," end ",X2,Y2,TH2,"radius",ROBOT_RADIUS # rescale X and Y to Computation coordinate frame (Turning radius = 1) X = X/ROBOT_RADIUS Y = Y/ROBOT_RADIUS TH = TH1 X2 = X2/ROBOT_RADIUS Y2 = Y2/ROBOT_RADIUS TH2 = math.radians(45.0) if not((util.newState((X,Y,TH))) and (util.newState((X2,Y2,TH2)))): print "Invalid start and/or goal configuration" return # ----------- Check which case the start-goal configuration falls under --------------------- # Case 1: start below and goal above => angle at net = pi/2 # Case 2: start above and goal below => angle at net = 3*pi/2 # Case 3: start and goal at same side of net => dont check net at all! t1 = time.time() if((Y <= 60/ROBOT_RADIUS) and (Y2 >= 60/ROBOT_RADIUS)): case = 1 elif((Y >= 60/ROBOT_RADIUS) and (Y2 <= 60/ROBOT_RADIUS)): case = 2 else: case = 3 print " case ",case if(case == 1): angle_net = math.pi/2 elif(case == 2): angle_net = 3*math.pi/2 if((case == 1) or (case == 2)): # --------- find path through right side of net --------- # plan_start_to_right = plan(canvas,X,Y,TH,50/ROBOT_RADIUS,60/ROBOT_RADIUS,angle_net,Tree_start_to_right) plan_right_to_goal = plan(canvas,50/ROBOT_RADIUS,60/ROBOT_RADIUS,angle_net,X2,Y2,TH2,Tree_right_to_goal) if(plan_start_to_right != False) and (plan_right_to_goal != False): plan_right = (plan_start_to_right[0] + plan_right_to_goal[0],plan_start_to_right[1] + plan_right_to_goal[1]) else: plan_right = False # -------- find path through left side of net ----------- # plan_start_to_left = plan(X,Y,TH,10/ROBOT_RADIUS,60/ROBOT_RADIUS,angle_net,Tree_start_to_left) plan_left_to_goal = plan(10/ROBOT_RADIUS,60/ROBOT_RADIUS,angle_net,X2,Y2,TH2,Tree_left_to_goal) if(plan_start_to_left != False) and (plan_left_to_goal!= False): plan_left = (plan_start_to_left[0] + plan_left_to_goal[0],plan_start_to_left[1] + plan_left_to_goal[1]) else: plan_left = False # --------- choose shortest path ------------------------ # if (plan_left == False): finalplan = plan_right if (plan_right == False): finalplan = plan_left if (plan_left != False) and (plan_right != False): if(plan_left[1]) < (plan_right[1]): finalplan = plan_left else: finalplan = plan_right elif(case == 3): # ball is on same side of net, so try to reach it directly finalplan = plan(X,Y,TH,X2,Y2,TH2,Tree_start_to_goal) t1 = time.time() - t1 #---------- draw path -------------- # #print("Time to complete Planning ",t1," s case",case) i = 0 return finalplan[0]
def isSafeDubins(curve_number, t, p, q, start_p): """ Given a start position and a Dubins curve, check the generated path for collisions. Return true if there's no collision and false if there is """ v = util.stepsize / util.ROBOT_RADIUS (x, y, theta) = start_p.getVertex() (startx, starty, starttheta) = (x, y, theta) #print (x,y,theta)," curve_number ",curve_number," t ",t," p ",p," q ",q # first action if ((curve_number == 0) or (curve_number == 2) or (curve_number == 5)): #turn left action = util.turnLeft_sim elif ((curve_number == 1) or (curve_number == 3) or (curve_number == 4)): #turn right action = util.turnRight_sim i = 0 while (i < t): (x, y, theta) = action(x, y, theta, v) point = (x, y, theta) if not (util.newState(point)): #print point," False" return False #print point," True" i = i + v #print "first action OK, ends at ",(x,y,theta) (x, y, theta) = action(startx, starty, starttheta, t) (startx, starty, starttheta) = (x, y, theta) # second action if (curve_number <= 3): # go forward action = util.goForward_sim elif (curve_number == 4): # turn left action = util.turnLeft_sim elif (curve_number == 5): # turn right action = util.turnRight_sim i = 0 while (i < p): (x, y, theta) = action(x, y, theta, v) point = (x, y, theta) if not (util.newState(point)): #print point," False" return False #print point," True" i = i + v (x, y, theta) = action(startx, starty, starttheta, p) (startx, starty, starttheta) = (x, y, theta) # third action if (curve_number == 0) or (curve_number == 3) or (curve_number == 5): # turn left action = util.turnLeft_sim elif (curve_number == 1) or (curve_number == 2) or (curve_number == 4): # turn right action = util.turnRight_sim i = 0 while (i < q): (x, y, theta) = action(x, y, theta, v) point = (x, y, theta) if not (util.newState(point)): #print point," False" return False i = i + v # print "Third action OK" return True