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