def get_new_path_list(robots, paths, costs): # returns list of pts with equal time step for all robots final_time = max(costs) time_step = Settings.time_step num_steps = int(math.ceil(final_time / time_step)) positions = [None] * len(robots) for i in range(len(robots)): positions[i] = [None] * num_steps path_pts = [None] * len(robots) # used for current path point (position = path point + trajectory value) time_pts = [None] * len(robots) # stores time at which robot is at pt in path_pts for j in range(len(robots)): # for all robots #print 'getting new path list for robot', robots[j] path_traj= [None] * len(paths[j]) #print 'size of path for robot', robots[j], 'is', len(paths[j]) for i in range(len(paths[j]) - 1): #print 'robot', robots[j], 'vertex number', i traj_num = get_traj_num(paths[j][i], paths[j][i + 1]) path_traj[i] = traj_num path_pts[j] = 0 time_pts[j] = 0 #print 'entering time_step loop. final time:', final_time for i in range(num_steps): # for all time steps time_ = time_step * i if time_ > costs[j]: # if robot j arrives sooner than other robots positions[j][i] = paths[j][len(paths[j]) - 1] # then keep it at the final goalset point else: # if time > (pt in path).(trajectory for that pt).(final time of that trajectory) trajectory = paths[j][path_pts[j]].trajectories[path_traj[path_pts[j]]] if time_ - time_pts[j] > trajectory.t_f: # if time > arrival time of trajectory between these two points path_pts[j] = path_pts[j] + 1 # then update pt and time to next pt and next time time_pts[j] = time_pts[j] + trajectory.t_f positions[j][i] = get_position(paths[j][path_pts[j]], time_ - time_pts[j], trajectory) # return discrete states del trajectory, time_pts, path_pts, path_traj return positions
def path_generation(vertex, robot_root): paths_parents = [] # list of paths, path = list of vertices costs_parents = [] # list of costs associated with some paths paths = [] costs = [] for i in range(len(vertex.parents)): # for all parents parent = vertex.parents[i] pths, csts = path_generation2( parent, robot_root) # get all paths and costs for that parent paths_parents.append(pths) # append paths to path list costs_parents.append(csts) # append costs to cost list # separating a list of lists of paths into one list of paths: for i in range(len(paths_parents) ): # for number of lists of paths (number of parents) traj_num = get_traj_num(vertex.parents[i], vertex) cost = vertex.parents[i].trajectories[ traj_num].t_f # additional_cost is arrival time of trajectory from parent for j in range(len(paths_parents[i]) ): # for number of paths in one of those lists paths.append( paths_parents[i][j]) # append that path to new total list costs.append( costs_parents[i][j] + cost ) # append that (parent cost) + (additional_cost) to new total cost list for i in range(len(paths)): # for every path in paths paths[i].append(vertex) # add the current vertex to that path if len(paths) == 0: # if this vertex is the root node paths = [[vertex]] # only "path" is itself, and has zero cost costs = [0.0] return paths, costs
def get_new_path(path): new_path = [] for i in range(len(path) - 1): traj_num = get_traj_num(path[i], path[i + 1]) for j in range(len(path[i].trajectories[traj_num].states2) - 1): new_path.append(path[i].trajectories[traj_num].states2[j]) return new_path
def get_new_path(path): new_path = [] for i in range(len(path) - 1): traj_num = get_traj_num( path[i], path[i + 1] ) # returns index of trajectory for point i that leads to point i+1 for j in range(len(path[i].trajectories[traj_num].states2) - 1): new_path.append(path[i].trajectories[traj_num].states2[j]) return new_path
def display_path(pts, pywindow, color_): for i in range(len(pts) - 1): traj_num = get_traj_num(pts[i], pts[i + 1]) draw_traj(pywindow, color_, pts[i], pts[i].trajectories[traj_num], 6) pygame.draw.line(pywindow, color_, (pts[i].x, pts[i].y), (pts[i + 1].x, pts[i + 1].y), 2) pygame.draw.line(pywindow, color_, (world_to_x_plot(pts[i].x, pts[i].x_vel)), (world_to_x_plot(pts[i + 1].x, pts[i + 1].x_vel)), 2) pygame.draw.line(pywindow, color_, (world_to_y_plot(pts[i].y, pts[i].y_vel)), (world_to_y_plot(pts[i + 1].y, pts[i + 1].y_vel)), 2) pygame.display.flip() return
def get_new_path(path): new_path = [] for i in range(len(path) - 1): traj_num = get_traj_num(path[i], path[i + 1]) # returns index of trajectory for point i that leads to point i+1 if len(path[i].trajectories[traj_num].statevals) == 0: get_equi_time_discrete_states(path[i], path[i + 1], path[i].trajectories[traj_num]) for j in range(len(path[i].trajectories[traj_num].statevals) - 1): new_path.append(path[i].trajectories[traj_num].statevals[j]) return new_path
def get_time(path, robo_num, times, traj__): # return list of times that robot will be at corresponding vertex if len(times) == 0: times.append(0) for i in range(len(path) - 1): # for all vertices in path traj_num = get_traj_num(path[i], path[i + 1]) add_time = path[i].trajectories[traj_num].t_f / traj__.num_disc_vals for j in range(traj__.num_disc_vals - 1): # for all discrete trajectory points between two pts times.append(times[i] + add_time) # add a time value # else times is already calculated return