def plot_traj(trajs_multi, number=None, title_string=None): num_trajs = len(trajs_multi) if number == None: number = np.random.randint(num_trajs) if title_string == None: title_string = 'from gen_results.py' # print 'number', number # print len(trajs_multi) # print len(trajs_multi[number]) # print trajs_multi[number][0] # print trajs_multi[number][1] # print trajs_multi[number][2] # print trajs_multi[number][0].shape # print trajs_multi[number][1].shape # print trajs_multi[number][2].shape # print 'entering plot' pedData.plot_traj_raw_multi(trajs_multi[number], title_string)
def plot_training_process(file_dir, format_str, num_agents):
plt.rcParams.update({'font.size': 30})
save_folder_dir = file_dir + "/../../pickle_files/multi/results/figures/"
# plot rvo trajs
# rvo_trajs_filename = file_dir + \
# "/../../pickle_files/multi/results/hard_rvo_trajs_raw.p"
# rvo_trajs = pickle.load(open(rvo_trajs_filename, "rb"))
# for i, traj in enumerate(rvo_trajs):
# nn_nav_multi.plot_traj_raw_multi(traj, '')
# plt.title('')
# file_name = 'rvo_case_'+str(i)+format_str
# plt.savefig(save_folder_dir+file_name,bbox_inches='tight')
# print 'saved', file_name
# plot neural network trajs
test_cases = pickle.load(open(file_dir + \
"/../../pickle_files/multi/results/%d_agents_hard_test_cases.p"%num_agents, "rb"))
iterations = [0, 50, 500, 800, 1000]
# load multiagent neural network
# load nn_rl
mode = 'no_constr'
passing_side = 'right'
for iteration in iterations:
# mode = 'rotate_constr'
filename = "%d_agents_policy_iter_%d.p" % (num_agents, iteration)
# filename=None
NN_value_net_multi = nn_nav.load_NN_navigation_value(file_dir,
mode,
passing_side,
filename=filename)
nn_trajs_filename = file_dir + \
"/../../pickle_files/multi/results/%d_agents_hard_nn_trajs_iter_%d.p"%(num_agents,iteration)
for i, test_case in enumerate(test_cases):
traj, time_to_complete = \
NN_value_net_multi.generate_traj(test_case, figure_name='%_agents_network'%num_agents)
pedData.plot_traj_raw_multi(traj,
'',
figure_name='training_process')
plt.title('')
file_name = '%d_agents_nn_iter_%d_case_%d' % (
num_agents, iteration, i) + format_str
plt.savefig(save_folder_dir + file_name, bbox_inches='tight')
print('saved', file_name)
def evaluate_on_test_case(test_case, nn_rl, if_plot=True):
traj_raw_multi, time_to_complete = \
nn_rl.value_net.generate_traj(test_case, figure_name='no_plot',stopOnCollision=True)
path_time = np.sum(time_to_complete)
# plotting (debugging)
# a1_speed = np.linalg.norm(traj_raw[0,5:7])
# a2_speed = np.linalg.norm(traj_raw[0,14:16])
# a1_len = np.sum(np.linalg.norm(traj_raw[0:-1, 1:3] - traj_raw[1:, 1:3], axis=1))
# a2_len = np.sum(np.linalg.norm(traj_raw[0:-1, 10:12] - traj_raw[1:, 10:12], axis=1))
# min_dist = np.amin(np.linalg.norm(traj_raw[:,1:3]-traj_raw[:,10:12], axis=1)) - \
# traj_raw[0,9] - traj_raw[0,18]
agents_speed, agents_time, agents_len, min_dist = nn_rlearning.compute_plot_stats(
traj_raw_multi)
title_string = 'a%d, t %.2f, sp %.2f, len %.2f \n %s; min_dist %.2f a%d t %.2f, sp %.2f, len %.2f' % \
(0, agents_time[0], agents_speed[0], agents_len[0], \
nn_rl.passing_side, min_dist, 1, agents_time[1], agents_speed[1], agents_len[1])
num_agents = len(traj_raw_multi) - 1
if num_agents > 2:
for tt in xrange(2, num_agents):
agent_string = '\n a%d, t %.2f, sp %.2f, len %.2f' % \
(tt, agents_time[tt], agents_speed[tt], agents_len[tt])
title_string += agent_string
if if_plot: # plot every time case
pedData.plot_traj_raw_multi(traj_raw_multi, title_string)
print('from nn_debug')
for tt in xrange(num_agents):
print('a%d, t %.2f, sp %.2f, len %.2f' % \
(tt, agents_time[tt], agents_speed[tt], agents_len[tt]))
# debug rawTraj_2_sample
# print 'agent 1', traj_raw[-1,1:10]
# print 'agent 2', traj_raw[-1,10:19]
# nn_rl.rawTraj_2_trainingData(traj_raw, 0.97)
# x, y, traj_raw, agent_1_time, agent_2_time = \
# nn_rl.straightLine_traj(test_case, 0.97, figure_name='straight line training traj')
# within GETTING_CLOSE_RANGE, not really collided
if_collided = min_dist < GETTING_CLOSE_RANGE / 2.0
return path_time, if_collided, traj_raw_multi, title_string
def stress_test_case(nn_rl, num_run, test_case=None, figure_name_str=None):
t_start = time.time()
path_time_vec = np.zeros((num_run, 1))
if_collided_vec = np.zeros((num_run, 1))
max_failed_trajs = 10
num_failed_trajs = 0
# whether generate random test cases
if test_case == None:
use_random_test_case = True
else:
use_random_test_case = False
# main loop
for i in xrange(num_run):
if use_random_test_case:
side_length = np.random.rand() * (6.0 - 3.0) + 3.0
# is_static=1
test_case = nn_nav.generate_rand_test_case_multi(nn_rl.num_agents, side_length, \
np.array([0.3,1.2]), np.array([0.3, 0.5]))
path_time, if_collided, traj_raw_multi, title_string = \
evaluate_on_test_case(test_case, nn_rl, if_plot=False)
path_time_vec[i] = path_time
time_to_reach_goal, traj_lengths, min_sepDist, if_completed_vec \
= pedData.computeStats(traj_raw_multi)
# print 'min_dist', min_sepDist
# raw_input()
if_collided_vec[i] = if_collided
# print traj_raw
# plotting
# first trajectory
if i == 0:
if figure_name_str == None:
figure_name_str = 'first_plot'
pedData.plot_traj_raw_multi(traj_raw_multi, title_string, \
figure_name=figure_name_str)
# if reached goal
num_agents = len(traj_raw_multi) - 1
agents_final_states = [
traj_raw_multi[t][-1, :] for t in xrange(1, num_agents + 1)
]
agents_reached_goal = []
if_all_successful = True
for tt in xrange(num_agents):
agent_final_state = agents_final_states[tt]
others_final_state = [
agents_final_states[t] for t in xrange(num_agents) if t != tt
]
tmp = nn_rl.value_net.if_terminal_state(\
agent_final_state, others_final_state)
agents_reached_goal.append(tmp)
if tmp != REACHED_GOAL:
if_all_successful = False
# if if_collided == False:
# continue
if i > 0: #if_all_successful == False:
figure_name_str = 'bad_traj_' + str(num_failed_trajs)
pedData.plot_traj_raw_multi(traj_raw_multi, title_string, \
figure_name = figure_name_str)
num_failed_trajs += 1
# print 'details of the failed test case'
# print if_collided, if_agent1_reached_goal==REACHED_GOAL, \
# if_agent2_reached_goal==REACHED_GOAL
# print 'agent1_final_state', agent1_final_state
# print 'dist_to_goal 1', np.linalg.norm(agent1_final_state[0:2]-agent1_final_state[6:8])
# print 'agent2_final_state', agent2_final_state
# print 'agent2_final 2', np.linalg.norm(agent2_final_state[0:2]-agent2_final_state[6:8])
num_plts = 1
# for plotting/debugging
time_vec = traj_raw_multi[0]
num_pts = len(time_vec)
# filter speeds
agents_filtered_vel_xy = np.zeros((num_pts, num_agents * 2))
dt_vec = time_vec.copy()
dt_vec[1:] = time_vec[1:] - time_vec[:-1]
dt_vec[0] = dt_vec[1]
time_past_one_ind = 0
agents_states = [
traj_raw_multi[tt] for tt in xrange(1, num_agents + 1)
]
for ii in xrange(num_pts):
while time_vec[ii] - time_vec[time_past_one_ind] > 0.45:
time_past_one_ind += 1
dt_past_vec = dt_vec[time_past_one_ind:ii + 1]
for jj in xrange(num_agents):
past_vel = agents_states[jj][time_past_one_ind:ii + 1, 2:5]
filter_vel_theta = nn_nav.filter_vel(dt_past_vec,
past_vel,
ifClose=True)
agents_filtered_vel_xy[ii, jj *
2] = filter_vel_theta[0] * np.cos(
filter_vel_theta[1])
agents_filtered_vel_xy[ii, jj * 2 +
1] = filter_vel_theta[0] * np.sin(
filter_vel_theta[1])
for j in xrange(num_pts):
agents_state = [
traj_raw_multi[tt][j, :]
for tt in xrange(1, num_agents + 1)
]
# print 'agents_state', agents_state
# raw_input()
agents_dist_2_goal = [
np.linalg.norm(a_s[0:2] - a_s[6:8]) for a_s in agents_state
]
# within GETTING_CLOSE_RANGE, not really collided
if_collided_tmp = False
min_dist_cur_pt = np.inf
if_too_close = False
for k in xrange(num_agents):
for h in xrange(k + 1, num_agents):
dist_tmp = np.linalg.norm(agents_state[k][0:2] - agents_state[h][0:2]) - \
agents_state[k][8] - agents_state[h][8]
if dist_tmp < min_dist_cur_pt:
min_dist_cur_pt = dist_tmp
if min_dist_cur_pt < GETTING_CLOSE_RANGE:
if_collided_tmp = True
if_no_improvement_vec = np.zeros((num_agents, ), dtype=bool)
agent_desired_speed = np.zeros((num_agents, ))
agent_actual_speed = np.zeros((num_agents, ))
for k in xrange(num_agents):
agent_desired_speed[k] = np.linalg.norm(
agents_state[k][2:4])
agent_actual_speed[k] = agents_state[k][5]
if_no_improvement_vec[k] = (agent_actual_speed[k] / agent_desired_speed[k]) < 0.2\
and (agents_dist_2_goal[k] > DIST_2_GOAL_THRES)
if True: #if_collided_tmp and True: #(if_collided or if_no_improvement_1 or if_no_improvement_2):
print('------ stress_test_case() in nn_debug.py')
for k in xrange(num_agents):
print '%dth agent' % k
agent_state = agents_state[k]
filtered_others_state = [
agents_state[tt].copy()
for tt in xrange(num_agents)
]
for tt in xrange(num_agents):
filtered_others_state[tt][
2:4] = agents_filtered_vel_xy[j, tt *
2:(tt + 1) * 2]
others_state = [
filtered_others_state[tt].copy()
for tt in xrange(num_agents) if tt != k
]
# print '~~~ agent %d state'%k, agent_state
# print 'in debug'
print('agent_state', agent_state)
# print 'others_state[0]', others_state[0]
# print '~~~~~~~~~~'
# raw_input()
print(
'current value',
nn_rl.value_net_copy.find_states_values(
agent_state, others_state))
# debug
agent_state_copy = agent_state.copy()
agent_state_copy[2:4] = 0
print(
'current value_zero',
nn_rl.value_net_copy.find_states_values(
agent_state_copy, others_state))
if if_no_improvement_vec[k]:
print('no improvement %d, pref speed: %.3f, actual_speed: %.3f ' % \
(k, agent_desired_speed[k], agent_actual_speed[k]))
# raw_input()
if True: #agents_dist_2_goal[k] > DIST_2_GOAL_THRES:
ref_prll, ref_orth, state_nn = \
pedData.rawState_2_agentCentricState(agent_state, others_state, nn_rl.num_agents)
# print 'before rotate', state_nn
# a_s, o_s = \
# pedData.agentCentricState_2_rawState_noRotate(state_nn)
# print 'a_s_after', a_s
# ref_prll_after, ref_orth_after, state_nn_after = \
# pedData.rawState_2_agentCentricState(a_s, o_s, nn_rl.num_agents)
# print 'after rotate', state_nn_after
# raw_input()
title_string = 'a%d; time: %.3f, dist_2_goal: %.3f' % (
k, traj_raw_multi[0][j], agents_dist_2_goal[k])
# nn_rl.value_net.plot_ped_testCase(state_nn, None, title_string, 'a1 '+str(j))
plt_colors_custom = copy.deepcopy(plt_colors)
plt_colors_custom[0] = plt_colors[k]
plt_colors_custom[1:k + 1] = plt_colors[0:k]
nn_rl.value_net.plot_ped_testCase(
state_nn,
None,
title_string,
'a%d' % k,
plt_colors_custom=plt_colors_custom)
num_plts += 1
raw_input()
if if_collided_tmp:
raw_input()
print(test_case)
plt.show()