def draw_astar(mission,crew,proxemics): path = astar(mission,crew,proxemics) cp = p.select_crew(crew) waypoints = p.select_mission(mission) drawablePath = viz.path_to_trajectory(path) viz.draw_path(drawablePath,waypoints,cp)
def astar(mission,crew,proxemics): cp = p.select_crew(crew) waypoints = p.select_mission(mission) #endpoint = waypoints[-1] #endpoint is last waypoint goal_i = 0 goal0 = waypoints[0] x0 = p.robot_x0 y0 = p.robot_y0 startVel = 0 startpoint = np.array((x0,y0)) visitedNodes = [(x0,y0)] #list of tuples cost0 = 0 paths = q.PriorityQueue() # each path gets added to the queue as (priority,[nodes]), where nodes are (cost,visitedNodes,x,y,v) paths.put((0,[(cost0,visitedNodes,x0,y0,startVel)])) #paths.get() should return list of tuples useProxemics = proxemics fullPath = [] for goal in waypoints: bestPath = astarPath(startpoint,goal,paths,cp,useProxemics) startpoint = waypoints[goal_i] goal_i += 1 fullPath.append(bestPath) return fullPath[-1] #warning this isn't the right format for viz
def astar(mission,crew,proxemics):
cp = p.select_crew(crew)
waypoints = p.select_mission(mission)
x0 = p.robot_x0
y0 = p.robot_y0
v0 = 0
visitedNodes = [(x0,y0)] #list of tuples
cost0 = 0
node0 = (cost0,visitedNodes,x0,y0,v0)
goal_i = 0
max_time = 0
fullPath = []
start_time = time.time()
for goal in waypoints:
paths = q.PriorityQueue()
if goal_i > 0:
#print ("goal index =", goal_i)
#start again at the last node from the last segment
lastNode = bestPath[-1]
#print ("lastNode =", (lastNode[X_I],lastNode[Y_I]))
lastX = lastNode[X_I]
lastY = lastNode[Y_I]
newVisitedNodes = [(lastX,lastY)]
lastVel = lastNode[V_I]
node0 = (0,newVisitedNodes,lastX,lastY,lastVel)
path0 = [node0] #should now be a list of nodes
# each path gets added to the queue as (priority,[nodes]), where nodes are (cost,visitedNodes,x,y,v)
# if not paths.empty():
# print("Path not empty!")
paths.put((0,path0))
#paths.get() should return list of tuples
useProxemics = proxemics
#astarPath(goal,paths,crew,useProxemics)
bestPath, poss_max_time, timed_out = astarPath(goal,paths,cp,useProxemics, start_time, max_time)
if poss_max_time > max_time:
max_time = poss_max_time
#bestPath should be a single_path = [nodes]
goal_i += 1
fullPath.extend(bestPath)
if timed_out:
break
#fullpath should be an array of single_paths
return fullPath, max_time
def draw_astar(mission,crew,proxemics):
tic = time.time()
path = astar(mission,crew,proxemics)
cp = p.select_crew(crew)
waypoints = p.select_mission(mission)
drawablePath = viz.path_to_trajectory(path)
#print(drawablePath)
viz.draw_path(drawablePath,waypoints,cp)
toc = time.time()
tElapsed = toc - tic
tmins = np.floor(tElapsed/60)
tsecs = np.mod(tElapsed,60)
print ("Took", toc - tic, "seconds")
print ("or", tmins, "mins and", tsecs, "seconds")
def color_quiver_plt():
for mission_idx, crew_idx, wp_idx in [
(1, 1, 0)
]: # (2,2,1),(2,2,2),(2,3,1),(3,4,3)]:
mission = parameters.select_mission(mission_idx)
crew = parameters.select_crew(crew_idx)
for func_name in funcs:
out = apf.linear_goal_force_function(grid, mission[wp_idx], use_k)
out += apf.gaussian_boundary_force_function(
grid, parameters.module_size)
out += funcs[func_name](grid, crew)
norm = (out[:, 0]**2 + out[:, 1]**2)**0.5
norm = norm / norm.max().max()
plt.figure()
plt.quiver(xx, yy, out[:, 0], out[:, 1], norm, cmap=plt.cm.viridis)
# plt.colorbar()
plt.colorbar(fraction=0.046, pad=0.04)
# norm = Normalize()
# norm.autoscale()
plt.axis('scaled')
# viz.draw_crew(crew)
viz.draw_waypoints(mission[[wp_idx]])
plt.xlim((0, parameters.module_size[0]))
plt.ylim((0, parameters.module_size[1]))
plt.xlabel('x, m')
plt.ylabel('y, m')
plt.title(
'Potential Field for Goal %d Mission %d with Crew %d - %s' %
((wp_idx + 1), mission_idx, crew_idx,
'Non-Proxemic' if func_name == 'nonproxemic' else 'Proxemic'))
# plt.gcf().set_size_inches(3.5,3)
plt.tight_layout()
def astar_generate(args):
mission_idx, crew_idx, use_proxemic = args
idx = 100 * mission_idx + 10 * crew_idx + int(use_proxemic)
mission = parameters.select_mission(mission_idx)
crew = parameters.select_crew(crew_idx)
func_name = 'proxemic' if use_proxemic else 'nonproxemic'
tic = time.time()
nodelist, max_node_time = astar.astar(mission_idx, crew_idx,
func_name == 'proxemic')
toc = time.time()
path = viz.path_to_trajectory(nodelist)
viz.draw_path(path, mission, crew)
plt.xlabel('x, m')
plt.ylabel('y, m')
plt.title('Robot Path on Mission %d with Crew %d, A* - %s' %
(mission_idx, crew_idx,
'Non-Proxemic' if func_name == 'nonproxemic' else 'Proxemic'))
plt.savefig('data/astar_path__mission%d_crew%d_%s.png' %
(mission_idx, crew_idx, func_name))
plt.close()
path_df = pd.DataFrame(data=path[:, :-1],
columns=astar_path_columns,
index=path[:, -1])
path_df['vn'] = (path_df['vx']**2 + path_df['vy']**2)**0.5
path_df['an'] = path_df['vn'].diff(
) #(path_df['vx'].diff()**2 + path_df['vy'].diff()**2)**0.5
path_df[['vn', 'an']].plot()
plt.tight_layout()
plt.savefig('data/astar_velocity_accel__mission%d_crew%d_%s.png' %
(mission_idx, crew_idx, func_name))
plt.close()
count_crew_collisions = 0
for crewnum, crewmember in enumerate(crew):
path_df['distance from crew %d' %
(crewnum + 1)] = ((path_df['x'] - crewmember[0])**2 +
(path_df['y'] - crewmember[1])**2)**0.5
count_crew_collisions += (
(path_df['distance from crew %d' % (crewnum + 1)] <=
parameters.collision_distance).astype(int).diff() > 0).sum()
count_complete_wp = 0
for wp in mission:
if (((path_df['x'] - wp[0])**2 + (path_df['y'] - wp[1])**2)**0.5
).min().min() <= parameters.goal_cutoff * parameters.robot_length:
count_complete_wp += 1
percent_complete_wp = count_complete_wp / mission.shape[0]
path_df[path_df.columns[path_df.columns.str.startswith(
'distance from crew ')]].plot()
for line_y in dists_to_bin[1:-1]:
plt.plot(path_df.index[[0, -1]], [line_y] * 2, '--')
plt.xlabel('t, sec')
plt.ylabel('distance, m')
plt.title(
'Distance Between Robot and Crew\n for Mission %d, Crew Configuration %d, A* - %s'
% (crew_idx, mission_idx,
'Non-Proxemic' if func_name == 'nonproxemic' else 'Proxemic'))
plt.tight_layout()
plt.savefig('data/astar_crew_dist__mission%d_crew%d_%s.png' %
(mission_idx, crew_idx, func_name))
plt.close()
count_boundary_collisions = ((
(path_df['x'] <= parameters.robot_length / 2) |
(path_df['x'] >= parameters.module_width - parameters.robot_length / 2)
| (path_df['y'] <= parameters.robot_length / 2) |
(path_df['y'] >= parameters.module_height -
parameters.robot_length / 2)).astype(int).diff() > 0).sum()
# time to complete
# avg vel
# distance travelled
# collisions
# entered waypoint zone %
# max thrust, max velocity
df = [
func_name, # proxemic or not
mission_idx,
crew_idx,
path_df.index[-1], # time
path_df['vn'].max(), # max vel
path_df['vn'].mean(), # average velocity
(path_df['vn'] * path_df.index).sum(), # path length
path_df['an'].max(), # maximum accel?
count_boundary_collisions, # boundary collisions
count_crew_collisions, # crew colissions
path_df[path_df.columns[path_df.columns.str.startswith(
'distance from crew ')]].min().min(), # min crew distance
percent_complete_wp,
toc - tic, # time to compute
max_node_time,
]
for crewnum, crewmember in enumerate(crew):
check_time = 0
for dist_idx, dist in enumerate(dists_to_bin[1:]):
in_bin_selector = (
(path_df['distance from crew %d' % (crewnum+1)] >= dists_to_bin[dist_idx]) & \
(path_df['distance from crew %d' % (crewnum+1)] < dist)
).astype(int).diff().abs().astype(bool)
for end_pt in [0, -1]:
in_bin_selector.iloc[end_pt] = (
(path_df['distance from crew %d' %
(crewnum + 1)].iloc[end_pt] >=
dists_to_bin[dist_idx])
and (path_df['distance from crew %d' %
(crewnum + 1)].iloc[end_pt] < dist))
df += [np.diff(path_df.index[in_bin_selector])[::2].sum()]
check_time += df[-1]
print(
"check", path_df.index[-1], check_time, "good"
if path_df.index[-1] == check_time else "ERROR! ERROR! ERROR!")
df += [0] * (4 - crew.shape[0]) * (len(dists_to_bin) - 1)
path_df.to_csv('data/%d.csv' % idx)
# out = apf.linear_goal_force_function(grid,mission[0])
# out += apf.gaussian_boundary_force_function(grid, parameters.module_size)
# out += funcs[func_name](grid, crew)
# plt.figure()
# plt.quiver(xx,yy,out[:,0],out[:,1])
# plt.axis('scaled')
# # viz.draw_crew(crew)
# viz.draw_waypoints(mission[[0]])
# plt.xlim((0,parameters.module_size[0]))
# plt.ylim((0,parameters.module_size[1]))
# plt.tight_layout()
# plt.savefig('data/quiver_mission%d_crew%d_%s.png' % (mission_idx,crew_idx,func_name))
# plt.close()
return (df, path_df)
def do_generate(do_path=True,
do_distance=True,
do_speed=False,
do_paired_velocity=False):
df_columns = ['type','mission','crew','time','max vel', 'average vel', 'distance', 'max accel', 'boundary collisions', 'crew collisions', 'min crew distance', '%% complete'] + \
['time in distance bin %d from crew %d' % (dist_idx, crewnum+1) for crewnum in range(4) for dist_idx in range(len(dists_to_bin[1:]))]
idx = 0
df = pd.DataFrame(columns=df_columns, )
for mission_idx in range(1, n_mission + 1): #4
# mission_idx = 2
mission = parameters.select_mission(mission_idx)
for crew_idx in range(1, n_crew + 1): #5
# crew_idx = 2
if (crew_idx == 5):
if (mission_idx == 1):
crew = parameters.select_crew(1)
crew[:, parameters.Y_POS] = 2.0
else:
continue
else:
crew = parameters.select_crew(crew_idx)
paths_df_list = []
for func_name in func_names_list:
path = apf.apf_path_planner(parameters.robot_initial_condition,
mission,
crew,
funcs[func_name],
goal_k=use_k,
damping_b=use_b)
if do_path:
viz.draw_path(path, mission, crew, True)
plt.xlabel('x, m')
plt.ylabel('y, m')
plt.title(
'Robot Path on Mission %d with Crew %d, APF - %s' %
(mission_idx, crew_idx, 'Non-Proxemic'
if func_name == 'nonproxemic' else 'Proxemic'))
# plt.gcf().set_size_inches(3.5,3)
plt.tight_layout()
plt.savefig('data/path_mission%d_crew%d_%s.png' %
(mission_idx, crew_idx, func_name))
plt.close()
path_df = pd.DataFrame(data=path, columns=path_columns)
path_df.index = path_df.index * apf.dt
path_df['vn'] = (path_df['vx']**2 + path_df['vy']**2)**0.5
path_df['an'] = (path_df['fx']**2 +
path_df['fy']**2)**0.5 / parameters.robot_mass
if do_speed:
path_df[['vn', 'an']].plot()
plt.tight_layout()
plt.savefig(
'data/velocity_force__mission%d_crew%d_%s.png' %
(mission_idx, crew_idx, func_name))
plt.close()
count_crew_collisions = 0
for crewnum, crewmember in enumerate(crew):
path_df['distance from crew %d' % (crewnum + 1)] = (
(path_df['x'] - crewmember[0])**2 +
(path_df['y'] - crewmember[1])**2)**0.5
for dist_idx, dist in enumerate(dists_to_bin[1:]):
path_df['time in distance bin %d from crew %d' % (dist_idx, crewnum+1)] = np.diff(path_df.index[((path_df['distance from crew %d' % (crewnum+1)] >= dists_to_bin[dist_idx]) & \
(path_df['distance from crew %d' % (crewnum+1)] < dist)).astype(int).diff().abs().astype(bool)])[1::2].sum()
count_crew_collisions += (
(path_df['distance from crew %d' % (crewnum + 1)] <=
parameters.collision_distance).astype(int).diff() >
0).sum()
count_complete_wp = 0
for wp in mission:
if (((path_df['x'] - wp[0])**2 +
(path_df['y'] - wp[1])**2)**0.5).min().min(
) <= parameters.goal_cutoff * parameters.robot_length:
count_complete_wp += 1
percent_complete_wp = count_complete_wp / mission.shape[0]
if do_distance:
path_df[path_df.columns[path_df.columns.str.startswith(
'distance from crew ')]].plot()
# plt.legend(bbox_to_anchor=(0, -0.02, 1, -0.002),legends=['crew 1'])
for line_y in dists_to_bin[1:-1]:
plt.plot(path_df.index[[0, -1]], [line_y] * 2, '--')
plt.xlabel('t, sec')
plt.ylabel('distance, m')
plt.title(
'Distance Between Robot and Crew\n for Mission %d, Crew Configuration %d, APF - %s'
% (crew_idx, mission_idx, 'Non-Proxemic'
if func_name == 'nonproxemic' else 'Proxemic'))
# plt.gcf().set_size_inches(3.5,3)
plt.tight_layout()
plt.savefig(
'data/apf_crew_distance__mission%d_crew%d_%s.png' %
(mission_idx, crew_idx, func_name))
plt.close()
count_boundary_collisions = (
((path_df['x'] <= parameters.robot_length / 2) |
(path_df['x'] >=
parameters.module_width - parameters.robot_length / 2) |
(path_df['y'] <= parameters.robot_length / 2) |
(path_df['y'] >= parameters.module_height -
parameters.robot_length / 2)).astype(int).diff() >
0).sum()
# time to complete
# avg vel
# distance travelled
# collisions
# entered waypoint zone %
# max thrust, max velocity
row_data = [
func_name, # proxemic or not
mission_idx,
crew_idx,
path_df.index[-1], # time
path_df['vn'].max(), # max vel
path_df['vn'].mean(), # average velocity
path_df['vn'].sum() * apf.dt, # path length
path_df['an'].max(), # maximum accel?
count_boundary_collisions, # boundary collisions
count_crew_collisions, # crew colissions
path_df[path_df.columns[path_df.columns.str.startswith(
'distance from crew ')]].min().min(
), # min crew distance
percent_complete_wp
]
for crewnum, crewmember in enumerate(crew):
check_time = 0
for dist_idx, dist in enumerate(dists_to_bin[1:]):
in_bin_selector = (
(path_df['distance from crew %d' % (crewnum+1)] >= dists_to_bin[dist_idx]) & \
(path_df['distance from crew %d' % (crewnum+1)] < dist)
).astype(int).diff().abs().astype(bool)
for end_pt in [0, -1]:
in_bin_selector.iloc[end_pt] = (
(path_df['distance from crew %d' %
(crewnum + 1)].iloc[end_pt] >=
dists_to_bin[dist_idx]) and
(path_df['distance from crew %d' %
(crewnum + 1)].iloc[end_pt] < dist))
row_data += [
np.diff(path_df.index[in_bin_selector])[::2].sum()
]
check_time += row_data[-1]
print(
"check", path_df.index[-1], check_time,
"good" if path_df.index[-1] == check_time else
"ERROR! ERROR! ERROR!")
row_data += [0] * (4 - crew.shape[0]) * (len(dists_to_bin) - 1)
df.loc[idx] = row_data
paths_df_list.append(path_df)
idx += 1
# out = apf.linear_goal_force_function(grid,mission[0])
# out += apf.gaussian_boundary_force_function(grid, parameters.module_size)
# out += funcs[func_name](grid, crew)
# norm = (out[:,0]**2 + out[:,1]**2)**0.5
# norm = norm/norm.max().max()
# plt.figure()
# plt.quiver(xx,yy,out[:,0],out[:,1],norm)
# plt.axis('scaled')
# # viz.draw_crew(crew)
# viz.draw_waypoints(mission[[0]])
# plt.xlim((0,parameters.module_size[0]))
# plt.ylim((0,parameters.module_size[1]))
# plt.xlabel('x, m')
# plt.ylabel('y, m')
# plt.title('Potential Field for Goal %d Mission %d with Crew %d APF - %s' % (mission_idx, crew_idx, 'Non-Proxemic' if func_name=='nonproxemic' else 'Proxemic'))
# plt.tight_layout()
# plt.savefig('data/quiver_mission%d_crew%d_%s.png' % (mission_idx,crew_idx,func_name))
# plt.close()
colors = 'bg'
if do_paired_velocity:
new_df = pd.DataFrame()
new_df['proxemic'] = paths_df_list[0]['vn']
new_df['non-proxemic'] = paths_df_list[1]['vn']
# plt.plot(new_df['proxemic'], 'b')
# plt.plot(new_df['non-proxemic'], 'g')
# new_df['proxemic goals'] = 0
# new_df['nonproxemic goals'] = 0
plt.figure()
# for wp_idx,wp in enumerate(mission):
# for func_idx,func_name in enumerate(func_names_list):
# path_df = paths_df_list[func_idx]
# selector = (((((path_df['x']-wp[0])**2 + (path_df['y']-wp[1])**2)**0.5) <= parameters.goal_cutoff*parameters.robot_length).astype(int)).diff().astype(bool)
# if ((((path_df['x'].iloc[0]-wp[0])**2 + (path_df['y'].iloc[0]-wp[1])**2)**0.5) <= parameters.goal_cutoff*parameters.robot_length):
# selector[0] = True
# else:
# selector[0] = False
# for time_idxidx,time_idx in enumerate(path_df.index[selector]):
# plt.plot([time_idx]*2,[0, 4.5], colors[func_idx]+'-', linewidth=0.5)
# # if len(path_df.index[selector]) == 2:
# print(path_df.index[selector])
# # text_xpos = np.mean(path_df.index[selector])
# # else:
# # text_xpos = path_df.index[selector]
# if len(path_df.index[selector]) > 0:
# plt.text(path_df.index[selector].min()+5, 4.25, '%d' % wp_idx, color=colors[func_idx])
# plt.plot(paths_df_list[0]['distance from crew 1'],'b')
# plt.plot(paths_df_list[0]['distance from crew 2'],'b--')
# plt.plot(paths_df_list[1]['distance from crew 1'],'g')
# plt.plot(paths_df_list[1]['distance from crew 2'],'g--')
# zone_label = {0.15: 'Intimate', 0.45: 'Close Intimate', 1.2: 'Social', 3.6: 'Public'}
# max_t = np.max([paths_df_list[0].index[-1],paths_df_list[1].index[-1]])
# for line_y in dists_to_bin[1:-1]:
# plt.plot([0, max_t],[line_y]*2,'k--')
# plt.text(max_t-2, line_y, zone_label[line_y], horizontalalignment='right')
# plt.xlabel('t, sec')
# plt.ylabel('distance, m')
# plt.xlim(0, max_t)
# # plt.title('Robot Distance from Crew Member 2\nOn Mission %d with Crew Configuration %d' % (mission_idx, crew_idx))
# plt.title('Robot Distance from Crew\nOn Mission %d with Crew Configuration %d' % (mission_idx, crew_idx))
# plt.savefig('data/apfpaired_distance__mission%d_crew%d.png' % (mission_idx,crew_idx),bbox_inches='tight')
# plt.tight_layout()
# plt.close()
new_df.plot()
plt.tight_layout()
plt.savefig('data/apfpaired_v_plot__mission%d_crew%d.png' %
(mission_idx, crew_idx),
bbox_inches='tight')
plt.close()
viz.draw_path(
[paths_df_list[0].values, paths_df_list[1].values],
mission, crew)
plt.xlabel('x, m')
plt.ylabel('y, m')
plt.title('APF Paths on Mission %d with Crew %d' %
(mission_idx, crew_idx))
plt.savefig('data/apfpaired_path__mission%d_crew%d.png' %
(mission_idx, crew_idx),
bbox_inches='tight')
plt.tight_layout()
plt.close()
# if mission_idx == 2 and crew_idx == 3:
# plt.figure()
# plt.plot(paths_df_list[0]['distance from crew 2'],)
# plt.plot( paths_df_list[1]['distance from crew 2'])
# # plt.legend(['proxemic', 'non-proxemic'])
# plt.xlabel('t, sec')
# plt.ylabel('distance, m')
# plt.title('Robot Distance from Crew Member 2\nOn Mission %d with Crew Configuration %d' % (mission_idx, crew_idx))
# plt.savefig('data/apfpaired_distance__mission%d_crew%d.png' % (mission_idx,crew_idx),bbox_inches='tight')
# plt.tight_layout()
# plt.close()
return df
def compare_damp_amp():
df_columns = [
'type', 'k_exp', 'b_val', 'time', 'max vel', 'average vel', 'distance',
'max accel', 'boundary collisions', 'crew collisions',
'min crew distance', '%% complete'
]
idx = 0
df = pd.DataFrame(columns=df_columns, )
mission = parameters.select_mission(1)
crew = parameters.select_crew(1)
func_name = 'nonproxemic'
for k_exp in range(-4, 1):
for b_val in [10, 5, 1, 0]:
path = apf.apf_path_planner(parameters.robot_initial_condition,
mission,
crew,
funcs[func_name],
goal_k=2**k_exp,
damping_b=b_val)
viz.draw_path(path, mission, crew, True)
path_df = pd.DataFrame(data=path, columns=path_columns)
path_df.index = path_df.index * apf.dt
plt.savefig('data/apf_path__amp%d_damp%d.png' % (k_exp, b_val))
plt.close()
path_df['vn'] = (path_df['vx']**2 + path_df['vy']**2)**0.5
path_df['fn'] = (path_df['fx']**2 +
path_df['fy']**2)**0.5 / parameters.robot_mass
path_df[['vn', 'fn']].plot()
plt.tight_layout()
plt.savefig('data/velocity_force__amp%d_damp%d.png' %
(k_exp, b_val))
plt.close()
count_crew_collisions = 0
for crewnum, crewmember in enumerate(crew):
path_df['distance from crew %d' %
crewnum] = ((path_df['x'] - crewmember[0])**2 +
(path_df['y'] - crewmember[1])**2)**0.5
count_crew_collisions += (
(path_df['distance from crew %d' % crewnum] <=
parameters.collision_distance).astype(int).diff() >
0).sum()
count_complete_wp = 0
for wp in mission:
if (((path_df['x'] - wp[0])**2 +
(path_df['y'] - wp[1])**2)**0.5).min().min(
) <= parameters.goal_cutoff * parameters.robot_length:
count_complete_wp += 1
percent_complete_wp = count_complete_wp / mission.shape[0]
path_df[path_df.columns[path_df.columns.str.startswith(
'distance from crew ')]].plot()
plt.tight_layout()
plt.savefig('data/crew_distance__amp%d_damp%d.png' %
(k_exp, b_val))
plt.close()
count_boundary_collisions = (
((path_df['x'] <= parameters.robot_length / 2) |
(path_df['x'] >=
parameters.module_width - parameters.robot_length / 2) |
(path_df['y'] <= parameters.robot_length / 2) |
(path_df['y'] >= parameters.module_height -
parameters.robot_length / 2)).astype(int).diff() > 0).sum()
# time to complete
# avg vel
# distance travelled
# collisions
# entered waypoint zone %
# max thrust, max velocity
df.loc[idx] = [
func_name, # proxemic or not
k_exp,
b_val,
path_df.index[-1], # time
path_df['vn'].max(), # max vel
path_df['vn'].mean(), # average velocity
path_df['vn'].sum() * apf.dt, # path length
path_df['fn'].max() * apf.dt /
parameters.robot_mass, # maximum accel?
count_boundary_collisions, # boundary collisions
count_crew_collisions, # crew colissions
path_df[path_df.columns[path_df.columns.str.startswith(
'distance from crew ')]].min().min(), # min crew distance
percent_complete_wp
]
idx += 1
# out = apf.linear_goal_force_function(grid,mission[0])
# out += apf.gaussian_boundary_force_function(grid, parameters.module_size)
# out += funcs[func_name](grid, crew)
# plt.figure()
# plt.quiver(xx,yy,out[:,0],out[:,1])
# plt.axis('scaled')
# # viz.draw_crew(crew)
# viz.draw_waypoints(mission[[0]])
# plt.xlim((0,parameters.module_size[0]))
# plt.ylim((0,parameters.module_size[1]))
# plt.tight_layout()
# plt.savefig('data/quiver__amp%d_damp%d.png' % (k_exp,b_val))
# plt.close()
return df
if False: #do_legends:
plt.legend(key)
plt.axis('scaled')
draw_crew(crew)
draw_waypoints(mission)
plt.xlim((0,parameters.module_size[0]))
plt.ylim((0,parameters.module_size[1]))
plt.tight_layout()
# plt.show()
if __name__ == '__main__':
for crew_idx in range(1,5):
crew = parameters.select_crew(crew_idx)
plt.figure()
plt.axis('scaled')
plt.gca().add_patch(plt.Rectangle(
parameters.robot_initial_condition[0,parameters.XY_POS] - parameters.robot_length/2,
width=parameters.robot_length,
height=parameters.robot_length,
angle=0,
ec='b',
fill=False))
draw_crew(crew, True)
plt.xlim((0,parameters.module_size[0]))
plt.ylim((0,parameters.module_size[1]))
# plt.xlabel('x, m')
# plt.ylabel('y, m')
def astar_analyze(do_path=True,
do_distance=True,
do_speed=False,
do_contours=False,
save_summary=True):
df = pd.DataFrame(columns=df_columns, )
for mission_idx in range(1, n_mission + 1): #4
# mission_idx = 3
mission = parameters.select_mission(mission_idx)
for crew_idx in range(1, n_crew + 1): #5
# crew_idx = 3
if (crew_idx == 5):
if (mission_idx == 1):
crew = parameters.select_crew(1)
crew[:, parameters.Y_POS] = 2.0
else:
continue
else:
crew = parameters.select_crew(crew_idx)
paths_df_list = []
for func_name in funcs:
idx = 100 * mission_idx + 10 * crew_idx + int(
func_name == 'proxemic')
use_proxemic = func_name == 'proxemic'
path_df = pd.DataFrame.from_csv(
os.path.join(path_df_dir,
str(idx) + '.csv'))
path = path_df[astar_path_columns].values
# func_name = 'proxemic' if use_proxemic else 'nonproxemic'
tic = time.time()
# nodelist, max_node_time = astar.astar(mission_idx,crew_idx,func_name=='proxemic')
toc = time.time()
# path = viz.path_to_trajectory(nodelist)
max_node_time = 0
if do_path:
viz.draw_path(path, mission, crew, True)
plt.xlabel('x, m')
plt.ylabel('y, m')
plt.title(
'Robot Path on Mission %d with Crew %d, A* - %s' %
(mission_idx, crew_idx, 'Non-Proxemic'
if func_name == 'nonproxemic' else 'Proxemic'))
plt.savefig('data/astar_path__mission%d_crew%d_%s.png' %
(mission_idx, crew_idx, func_name))
plt.close()
# path_df = pd.DataFrame(data=path[:,:-1], columns=astar_path_columns, index=path[:,-1])
path_df['vn'] = (path_df['vx']**2 + path_df['vy']**2)**0.5
path_df['an'] = path_df['vn'].diff(
) #(path_df['vx'].diff()**2 + path_df['vy'].diff()**2)**0.5
if do_speed:
path_df[['vn', 'an']].plot()
plt.tight_layout()
plt.savefig(
'data/astar_velocity_accel__mission%d_crew%d_%s.png' %
(mission_idx, crew_idx, func_name))
plt.close()
count_crew_collisions = 0
for crewnum, crewmember in enumerate(crew):
path_df['distance from crew %d' % (crewnum + 1)] = (
(path_df['x'] - crewmember[0])**2 +
(path_df['y'] - crewmember[1])**2)**0.5
count_crew_collisions += (
(path_df['distance from crew %d' % (crewnum + 1)] <=
parameters.collision_distance).astype(int).diff() >
0).sum()
count_complete_wp = 0
for wp in mission:
if (((path_df['x'] - wp[0])**2 +
(path_df['y'] - wp[1])**2)**0.5).min().min(
) <= parameters.goal_cutoff * parameters.robot_length:
count_complete_wp += 1
percent_complete_wp = count_complete_wp / mission.shape[0]
if do_distance:
path_df[path_df.columns[path_df.columns.str.startswith(
'distance from crew ')]].plot()
for line_y in dists_to_bin[1:-1]:
plt.plot(path_df.index[[0, -1]], [line_y] * 2, '--')
plt.xlabel('t, sec')
plt.ylabel('distance, m')
plt.title(
'Distance Between Robot and Crew\n for Mission %d, Crew Configuration %d, A* - %s'
% (crew_idx, mission_idx, 'Non-Proxemic'
if func_name == 'nonproxemic' else 'Proxemic'))
plt.tight_layout()
plt.savefig(
'data/astar_crew_dist__mission%d_crew%d_%s.png' %
(mission_idx, crew_idx, func_name))
plt.close()
count_boundary_collisions = (
((path_df['x'] <= parameters.robot_length / 2) |
(path_df['x'] >=
parameters.module_width - parameters.robot_length / 2) |
(path_df['y'] <= parameters.robot_length / 2) |
(path_df['y'] >= parameters.module_height -
parameters.robot_length / 2)).astype(int).diff() >
0).sum()
# time to complete
# avg vel
# distance travelled
# collisions
# entered waypoint zone %
# max thrust, max velocity
row_data = [
func_name, # proxemic or not
mission_idx,
crew_idx,
path_df.index[-1], # time
path_df['vn'].max(), # max vel
path_df['vn'].mean(), # average velocity
(path_df['vn'] * path_df.index).sum(), # path length
path_df['an'].max(), # maximum accel?
count_boundary_collisions, # boundary collisions
count_crew_collisions, # crew colissions
path_df[path_df.columns[path_df.columns.str.startswith(
'distance from crew ')]].min().min(
), # min crew distance
percent_complete_wp,
0, #toc-tic, # time to compute
max_node_time,
]
for crewnum, crewmember in enumerate(crew):
check_time = 0
for dist_idx, dist in enumerate(dists_to_bin[1:]):
in_bin_selector = (
(path_df['distance from crew %d' % (crewnum+1)] >= dists_to_bin[dist_idx]) & \
(path_df['distance from crew %d' % (crewnum+1)] < dist)
).astype(int).diff().abs().astype(bool)
for end_pt in [0, -1]:
in_bin_selector.iloc[end_pt] = (
(path_df['distance from crew %d' %
(crewnum + 1)].iloc[end_pt] >=
dists_to_bin[dist_idx]) and
(path_df['distance from crew %d' %
(crewnum + 1)].iloc[end_pt] < dist))
if (mission_idx, crew_idx, crewnum + 1,
use_proxemic) in [(1, 2, 2, True),
(2, 2, 1, True),
(2, 3, 2, True),
(3, 3, 3, False),
(3, 4, 1, True)]:
print("\n\n CHECKING ERRORS BIN:",dist,"\n",(
(path_df['distance from crew %d' % (crewnum+1)] >= dists_to_bin[dist_idx]) & \
(path_df['distance from crew %d' % (crewnum+1)] < dist)
),)
row_data += [
np.diff(path_df.index[in_bin_selector])[::2].sum()
]
check_time += row_data[-1]
print(
"mission", mission_idx, "crew cfg", crew_idx, "model",
func_name, "crew #", crewnum + 1, "time check",
path_df.index[-1], check_time,
"good" if path_df.index[-1] == check_time else
"ERROR! ERROR! ERROR!")
# if check_time != path_df.index[-1]:
# return
row_data += [0] * (4 - crew.shape[0]) * (len(dists_to_bin) - 1)
df.loc[idx] = row_data
paths_df_list.append(path_df)
if do_contours:
if use_proxemic:
r = parameters.determine_constants(grid, crew)
cost = parameters.proxemic_astar_function(
grid, crew, r)
else:
cost = parameters.nonproxemic_astar_function(
grid, crew)
# plot contours
plt.figure()
plt.contour(xx, yy, cost, 20)
plt.axis('scaled')
viz.draw_crew(crew)
plt.xlim((0, parameters.module_size[0]))
plt.ylim((0, parameters.module_size[1]))
plt.tick_params(
axis='both', # changes apply to the x-axis
which='both', # both major and minor ticks are affected
bottom='off', # ticks along the bottom edge are off
top='off', # ticks along the top edge are off
left='off',
right='off',
labelleft='off',
labelbottom='off'
) # labels along the bottom edge are off
plt.tight_layout()
plt.savefig('data/con%d%s.png' %
(crew_idx, 'p' if use_proxemic else 'np'),
bbox_inches='tight')
plt.close()
colors = 'bg'
viz.draw_path([paths_df_list[0].values, paths_df_list[1].values],
mission, crew)
plt.xlabel('x, m')
plt.ylabel('y, m')
plt.title('A* Paths on Mission %d with Crew %d' %
(mission_idx, crew_idx))
plt.savefig('data/astarpaired_path__mission%d_crew%d.png' %
(mission_idx, crew_idx),
bbox_inches='tight')
plt.tight_layout()
plt.close()
if save_summary:
df.to_csv(datetime.now().__format__(
'data/astar_summary_%Y_%m_%d__%H_%M_%S.csv'))