def user_prompt(pywindow): # prompt for num robots, start and end positions, mark on pywindow
num_robots = input('Enter number of robots: ')
robo_colors = color_init(num_robots)
start = [] # to be used for coordinates of robots start positions
goal_set = [] # to be used for coordinates of robots end positions
i = 0
running_ = 1
while i < num_robots and running_: # until start/stop selected for all, or exited
exit1, start = get_click_pos(i, 'start', start, pywindow, robo_colors[i]) # get start position from user, append
exit2, goal_set = get_click_pos(i, 'end', goal_set, pywindow, robo_colors[i]) # get end position from user, append
i = i + 1
if exit1 == 1 or exit2 == 1: # end loop if user exited
running_ = 0
goal_set_size = 12
goal_set2 = [] # updating goal set to be a larger box, rather than just a goal Vertex
for i in range(num_robots): # make four Vertices with goal_set at center
pt1 = Vertex(goal_set[i].x - goal_set_size, goal_set[i].y - goal_set_size)
pt2 = Vertex(goal_set[i].x - goal_set_size, goal_set[i].y + goal_set_size)
pt3 = Vertex(goal_set[i].x + goal_set_size, goal_set[i].y + goal_set_size)
pt4 = Vertex(goal_set[i].x + goal_set_size, goal_set[i].y - goal_set_size)
goal_box = create_shape([pt1, pt2, pt3, pt4])
goal_set2.append(goal_box)
draw_shape(goal_box, pywindow)
pygame.display.flip()
return start, goal_set2, num_robots, robo_colors
def sample_free(path, goal_set): # return pseudo random Vertex object
if path == []: # with coordinates uniformly in pywindow
return Vertex(random.random() * Dimensions.window_length,
random.random() * Dimensions.window_width)
else: # with coordinates biased towards goal
x_l, x_r, y_sm, y_lar = get_bias_box(
goal_set[0].x + Dimensions.goal_set_size,
goal_set[0].y + Dimensions.goal_set_size)
vert = Vertex((random.random() * (x_r - x_l)) + x_l,
(random.random() * (y_lar - y_sm)) + y_sm)
#print vert.x, vert.y
return vert
def steer4(
vertex_nearest, vertex_rand,
goal_set): # steer nearest vertex towards random Vertex, within radius
if norm(vertex_nearest,
vertex_rand) < Dimensions.tree_radius: # if already within radius
vertex_new = vertex_rand # then no calculation needed
else:
ratio1 = (vertex_rand.x - vertex_nearest.x) / (
vertex_rand.y - vertex_nearest.y
) # calculate 4-Dimensional "slopes"
ratio2 = (vertex_rand.x - vertex_nearest.x) / (vertex_rand.x_vel -
vertex_nearest.x_vel)
ratio3 = (vertex_rand.x - vertex_nearest.x) / (vertex_rand.y_vel -
vertex_nearest.y_vel)
new_delta_x = sqrt((Dimensions.tree_radius**2) /
(1 + (1 / (ratio1**2)) + (1 / (ratio2**2)) +
(1 / (ratio3**2)))) # use Euclidean norm to solve
if vertex_rand.x < vertex_nearest.x:
new_delta_x = -new_delta_x
y_new = vertex_nearest.y + (new_delta_x / ratio1)
x_v_new = vertex_nearest.x_vel + (new_delta_x / ratio2)
y_v_new = vertex_nearest.y_vel + (new_delta_x / ratio3)
vertex_new = Vertex(vertex_nearest.x + new_delta_x, y_new, x_v_new,
y_v_new) # initialize vertex
#print 'vert new before trace inclusivity', vertex_new.x, vertex_new.y, vertex_new.x_vel, vertex_new.y_vel
vertex_new_ = trace_inclusivity(
vertex_nearest, vertex_new,
goal_set) # if hits goal, adjust vertex to intersection pt
#print 'vert new after trace inclusivity', vertex_new_.x, vertex_new_.y, vertex_new_.x_vel, vertex_new_.y_vel
return vertex_new_
def steer(x_nearest, x_rand, goal_set): # steer nearest vertex towards random Vertex, within some radius
theta = atan2(x_rand.y - x_nearest.y, x_rand.x - x_nearest.x) # atan2 accounts for different quadrants
x_new = x_nearest.x + Dimensions.tree_radius * cos(theta) # new x value
y_new = x_nearest.y + Dimensions.tree_radius * sin(theta) # new y value
vertex_new = Vertex(x_new, y_new) # initialize vertex
vertex_new_ = trace_inclusivity(x_nearest, vertex_new, goal_set) # if hits goal, adjust vertex to intersection pt
return vertex_new_
def get_click_pos(
i, pt_type, pt, pywindow,
color_dot): # prompt user to click pygame window, read position
print 'Click robot', i, pt_type, 'Vertex' # prompt user
waiting = 1
exit_pressed = 0
while waiting: # loop until user has clicked, or clicked exit
event_click = pygame.event.poll(
) # read whether user event has occurred
if event_click.type == pygame.QUIT: # if user clicked exit
waiting = 0
exit_pressed = 1
elif event_click.type == pygame.MOUSEBUTTONDOWN and event_click.button == 1: # if user clicked in pywindow
x, y = pygame.mouse.get_pos() # get position of the click
x = float(x)
y = float(y)
pt.append(Vertex(x, y, 0, 0)) # array of all start/end positions
pygame.draw.circle(pywindow, color_dot, (int(x), int(y)),
Settings.robo_size,
0) # display starting Vertex with color circle
x_, y_ = world_to_x_plot(x, 0)
x_ = int(x_)
y_ = int(y_)
pygame.draw.circle(pywindow, color_dot, (x_, y_),
Settings.robo_size_vel, 0)
x_, y_ = world_to_y_plot(y, 0)
x_ = int(x_)
y_ = int(y_)
pygame.draw.circle(pywindow, color_dot, (x_, y_),
Settings.robo_size_vel, 0)
pygame.display.flip() # update display with these new shapes
waiting = 0
return exit_pressed, pt
def sample_free(path, goal_set, i, sign): # return pseudo random Vertex object
x = random.random() * Dimensions.window_width # x value is uniformly random sample
y = random.random() * Dimensions.window_length # y value is uniformly random sample
bias_iteration = False # velocity values might be biased for smoother paths
if i == Settings.sample_bias_iterator**(-1) - 1:
biasx = sign[0][1]
biasy = sign[1][1]
bias_iteration = True
i = 0
else:
biasx = False
biasy = False
i = i + 1
###############################
if biasx == False: # velocity is not biased in positive or negative direction
if bias_iteration == False: # velocity is not biased
x_vel = (random.random() * 2 * Settings.robo_vel_max) - Settings.robo_vel_max
else: # velocity is biased to be near zero
x_vel = (random.random() * .4 * Settings.robo_vel_max) - (Settings.robo_vel_max * .2)
else: # velocity is biased to be in positive or negative direction
x_vel = random.random() * Settings.robo_vel_max * sign[0][0]
###############################
if biasy == False: # velocity is not biased in positive or negative direction
if bias_iteration == False: # velocity is not biased
y_vel = (random.random() * 2 * Settings.robo_vel_max) - Settings.robo_vel_max
else: # velocity is biased to be near zero
y_vel = (random.random() * .4 * Settings.robo_vel_max) - (Settings.robo_vel_max * .2)
else: # velocity is biased to be in positive or negative direction
y_vel = random.random() * Settings.robo_vel_max * sign[1][0]
return Vertex(x, y, x_vel, y_vel), i
def collision(pt1, pt2, shape, type_): # check if pt1 to pt2 collides with any edge of shape
collision_ = 0; intersect = None
for i in range(len(shape)): # for all edges of shape
if collision_line(pt1, pt2, shape[i], shape[i].obs_next) == 1: # if collision
if type_ == 'obstacles':
collision_ = 1
break
elif type_ == 'goal':
print 'intersect with goal in x,y plane'
x, y = intersection(pt1, pt2, shape[i], shape[i].obs_next)
if abs(pt2.x_vel) < Settings.robo_finish_vel and abs(pt2.y_vel) < Settings.robo_finish_vel:
intersect = Vertex(x, y, pt2.x_vel, pt2.y_vel) # if intersection pt is also in velocity range
intersect.at_goal_set = True # then robot is at goal set
collision_ = 1
print 'vel magnitude small enough'
else:
print 'vel magnitude too large'
else:
print 'unknown collision check type'
return collision_, intersect
def dig_in(id_, depth_, mytoken, node_, nd_viz, ed_viz, style_viz):
if depth_ == 0:
return None
print("depth: ", depth_)
user = {id_: node_.links[id_]}
node_next = Vertex(user, links = getlinks(id_, user, mytoken))
node_next.links.pop(next(iter(node_.user)), None)
node_.nodes.append(node_next)
make_json(node_next, nd_viz, ed_viz, style_viz)
for match_friend in node_next.links:
dig_in(match_friend, depth_-1, mytoken, node_next, nd_viz, ed_viz, style_viz)
def sample_free(path, goal_set): # return pseudo random Vertex object
# if path == []: # with coordinates uniformly in pywindow
x = random.random() * Dimensions.window_width
y = random.random() * Dimensions.window_length
x_vel = (random.random() * 2 * Settings.robo_vel_max) - Settings.robo_vel_max
y_vel = (random.random() * 2 * Settings.robo_vel_max) - Settings.robo_vel_max
return Vertex(x, y, x_vel, y_vel)
# else: # with coordinates biased towards goal
# x_l, x_r, y_sm, y_lar = get_bias_box(goal_set[0].x + Dimensions.goal_set_size, goal_set[0].y + Dimensions.goal_set_size)
# vert = Vertex((random.random() * (x_r - x_l)) + x_l, (random.random() * (y_lar - y_sm)) + y_sm)
# print vert.x, vert.y
# return vert
##############################################################################################################
def main():
pywindow, obstacles, axis, buttons = init_pywindow(
'i-Nash Trajectory') # set up pygame window, dimensions and obstacles
start, goal_set, num_robots, robo_colors, sign, goal = user_prompt(
pywindow) # prompt for num bots, start, end positions
num_tests = input('how many tests to run? ')
success_total = 0
while success_total < num_tests:
print 'running test number:', success_total + 1
for i in range(len(start)):
print 'start:', start[i].x, start[i].y, 'goal', goal[i].x, goal[
i].y
nash_success = run_test(pywindow, obstacles, axis, buttons, start,
goal_set, num_robots, robo_colors, sign, goal,
success_total)
if nash_success:
success_total = success_total + 1
for i in range(len(goal)):
goal[i] = Vertex(goal[i].x, goal[i].y, 0, 0)
goal[i].tree_num = 2
start[i] = Vertex(start[i].x, goal[i].y, 0, 0)
print 'Done the whole thing, wow, excellent, nice! Super cool!'
return
def intersection(pt1, pt2, pt3, pt4): # find intersection point of two lines, given two points for each line
m, b = line(pt1, pt2) # get line values from 2 pts
m2, b2 = line(pt3, pt4)
if m is not None and m2 is not None: # for non vertical lines
x = (b2 - b) / (m - m2) # calculate x value
y = (m * x) + b # calculate y value
else:
if m2 is None: # if line 2 was the vertical one
x = pt3.x
y = (m * x) + b
else: # else line 1 was the vertical one
x = pt1.x
y = (m2 * x) + b2
return Vertex(x, y)
def get_click_pos(i, pt_type, pt, pywindow, color_dot): # prompt user to click pygame window, read position
print 'Click robot', i+1, pt_type, 'Vertex' # prompt user
waiting = 1; exit_pressed = 0
while waiting: # loop until user has clicked, or clicked exit
event_click = pygame.event.poll() # read whether user event has occurred
if event_click.type == pygame.QUIT: # if user clicked exit
waiting = 0
exit_pressed = 1
elif event_click.type == pygame.MOUSEBUTTONDOWN and event_click.button == 1: # if user clicked in pywindow
x, y = pygame.mouse.get_pos() # get position of the click
pt.append(Vertex(x, y)) # array of all start/end positions
pygame.draw.circle(pywindow, color_dot, (x, y), 10, 0) # display starting Vertex with color circle
pygame.display.flip() # update display with these new shapes
waiting = 0
return exit_pressed, pt
def display_generation(x, y):
x_ = x - Dimensions.button_width
displays = []
displays.append(
create_shape([
Vertex(0, 0, 0, 0),
Vertex(0, y, 0, 0),
Vertex(x, y, 0, 0),
Vertex(x, 0, 0, 0)
]))
displays.append(
create_shape([
Vertex(0, 0, 0, 0),
Vertex(0, y, 0, 0),
Vertex(x_, y, 0, 0),
Vertex(x_, 0, 0, 0)
]))
return displays
def start(base_id, mytoken, depth):
global res
res = 0
print("START DIG")
user = getnode(base_id, mytoken)
links = getlinks(base_id, user, mytoken)
node = Vertex(user, links)
print("USER: "******"LINKS: ", links)
nd_viz, ed_viz, style_viz = make_json(node)
for match_friend_id in node.links:
dig_in(match_friend_id, depth, mytoken, node, nd_viz, ed_viz, style_viz)
nodes_edges_js = {"nodes": nd_viz, "edges": ed_viz}
user_data = list()
user_data.append(nodes_edges_js)
user_data.append(style_viz)
to_file(json.dumps(user_data, ensure_ascii=False))
res = 1
def get_position(pt1, time_, traj): # difference: for a specific time, not fixed number of states with some time_step
ux = traj.u_x1
uy = traj.u_y1
v1_x = pt1.x_vel + (ux * traj.ts1_x)
v1_y = pt1.y_vel + (uy * traj.ts1_y)
x1 = pt1.x + (pt1.x_vel * traj.ts1_x) + (.5 * ux * (traj.ts1_x ** 2))
y1 = pt1.y + (pt1.y_vel * traj.ts1_y) + (.5 * uy * (traj.ts1_y ** 2))
if time_ < traj.ts1_x:
x_ = pt1.x + (pt1.x_vel * time_) + (.5 * ux * (time_ ** 2))
v_x = pt1.x_vel + (ux * time_)
else:
x_ = x1 + (v1_x * (time_ - traj.ts1_x)) - (.5 * ux * ((time_ - traj.ts1_x) ** 2))
v_x = v1_x - (ux * (time_ - traj.ts1_x))
if time_ < traj.ts1_y:
y_ = pt1.y + (pt1.y_vel * time_) + (.5 * uy * (time_ ** 2))
v_y = pt1.y_vel + (uy * time_)
else:
y_ = y1 + (v1_y * (time_ - traj.ts1_y)) - (.5 * uy * ((time_ - traj.ts1_y) ** 2))
v_y = v1_y - (uy * (time_ - traj.ts1_y))
return Vertex(x_, y_, v_x, v_y)
def sample_free(path, goal_set, i, sign): # return pseudo random Vertex object
x = random.random() * Dimensions.window_width
y = random.random() * Dimensions.window_length
if i == Settings.sample_bias_iterator**(-1) - 1:
biasx = sign[0][1]
biasy = sign[1][1]
i = 0
else:
biasx = False
biasy = False
i = i + 1
if biasx == False:
x_vel = (random.random() * 2 *
Settings.robo_vel_max) - Settings.robo_vel_max
else:
x_vel = random.random() * Settings.robo_vel_max * sign[0][
0] # bias velocity sampling
if biasy == False:
y_vel = (random.random() * 2 *
Settings.robo_vel_max) - Settings.robo_vel_max
else:
y_vel = random.random() * Settings.robo_vel_max * sign[1][
0] # to help avoid swirling paths
return Vertex(x, y, x_vel, y_vel), i
def tests():
v1 = Vertex(2,3)
v2 = Vertex(0,0)
v1.print_out()
v2.print_out()
print "\n"
dist = v1.EuclidDist(v2)
e = Edge(v1, v2, dist, False)
graph = Graph()
graph.addVertex(v1)
graph.addVertex(v2)
graph.addEdge(e)
graph.print_adjmatrix()
graph.print_vertexlst()
v3 = Vertex(4,5)
graph.addVertex(v3)
e2 = Edge(v1, v3, v3.EuclidDist(v1), True)
graph.addEdge(e2)
print("Real Print starts here ")
graph.print_graph()
def sample_free(path, goal_set): # return pseudo random Vertex object
# if path == []: # with coordinates uniformly in pywindow
return Vertex(random.random() * Dimensions.window_length, random.random() * Dimensions.window_width)
def get_box_pts(x, y, delta_x, delta_y):
pt1 = Vertex(x - delta_x, y - delta_y, 0, 0)
pt2 = Vertex(x - delta_x, y + delta_y, 0, 0)
pt3 = Vertex(x + delta_x, y + delta_y, 0, 0)
pt4 = Vertex(x + delta_x, y - delta_y, 0, 0)
return [pt1, pt2, pt3, pt4]
def sample_free(): # return pseudo random Vertex object with coordinates uniformly in pywindow
return Vertex(random.random() * Window.length, random.random() * Window.width)
def sample_free(
): # return pseudo random Vertex object with coordinates uniformly in pywindow
length = 500
width = 700
x_rand = Vertex(random.random() * length, random.random() * width)
return x_rand
def obstacle_generation(
): # return array of obstacles, obstacle is array of Vertex objects connected in a loop
window = create_shape(
[Vertex(0, 0),
Vertex(0, 700),
Vertex(500, 700),
Vertex(500, 0)])
obs1 = create_shape(
[Vertex(0, 455),
Vertex(0, 475),
Vertex(280, 475),
Vertex(280, 455)])
obs2 = create_shape([
Vertex(220, 225),
Vertex(220, 245),
Vertex(500, 245),
Vertex(500, 225)
])
obstacles = [window, obs1, obs2]
return obstacles