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