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
