def plot_value_function(file_dir, format_str):
plt.rcParams.update({'font.size': 36})
save_folder_dir = file_dir + "/../../pickle_files/multi/results/figures/"
# define test case
dist_to_goal = 2.5
pref_speed = 1.0
cur_speed = 1.0
cur_heading = np.pi / 5.0
other_vx = 0.0
other_vy = 1.0
rel_pos_x = 1.5
rel_pos_y = -0.8
self_radius = 0.3
other_radius = 0.3
vx = pref_speed * np.cos(cur_heading)
vy = pref_speed * np.sin(cur_heading)
dist_2_other = np.sqrt(np.array([rel_pos_x, rel_pos_y])) - \
self_radius-other_radius
x = [dist_to_goal, pref_speed, cur_speed, cur_heading, \
other_vx, other_vy, rel_pos_x, rel_pos_y, self_radius, \
other_radius, self_radius+other_radius, vx, vy, dist_2_other]
y = 0.5
# load 2 agent 'no rotate' neural network
mode = 'no_constr'
passing_side = 'right'
iteration = 1000
filename = "twoAgents_policy_iter_%d.p" % iteration
nn_navigation = nn_nav.load_NN_navigation_value(file_dir, mode,
passing_side, filename)
nn_navigation.plot_ped_testCase(x, y, ' ', \
'test_case in no_constr')
plt.subplot(121)
plt.title('')
plt.subplot(122)
plt.title('')
fig = plt.gcf()
fig.tight_layout()
file_name = 'value_func_no_constr' + format_str
plt.savefig(save_folder_dir + file_name, bbox_inches='tight')
# load 2 agent 'no rotate' neural network
mode = 'rotate_constr'
passing_side = 'right'
iteration = 500
filename = "twoAgents_policy_iter_%d.p" % iteration
nn_navigation = nn_nav.load_NN_navigation_value(file_dir, mode,
passing_side, filename)
nn_navigation.plot_ped_testCase(x, y, ' ', \
'test_case rotate_constr')
plt.subplot(121)
plt.title('')
plt.subplot(122)
plt.title('')
fig = plt.gcf()
fig.tight_layout()
file_name = 'value_func_rotate_constr' + format_str
plt.savefig(save_folder_dir + file_name, bbox_inches='tight')
def generate_trajs_for_training_multinet(file_dir, format_str, num_agents,
num_test_cases):
save_folder_dir = file_dir + "/../../pickle_files/multi/results/figures/"
# generate test cases
# np.random.seed(1)
side_length = 2.5 + num_agents / 4.0
speed_bnds = np.array([0.1, 1.5])
radius_bnds = np.array([0.3, 0.5])
test_cases = gen_results.generate_test_cases(num_test_cases, num_agents, \
side_length, speed_bnds, radius_bnds, is_end_near_bnd=True)
# generate multiagent trajectories using neural networks
mode = 'no_constr'
passing_side = 'right'
# mode = 'rotate_constr'; passing_side = 'right'
iteration = 1000
filename = "twoAgents_policy_iter_%d.p" % iteration
# filename=None
value_net = nn_nav.load_NN_navigation_value(file_dir, mode, passing_side,
filename)
NN_navigation_multi = nn_nav_multi.NN_navigation_value_multi(value_net)
# generate trajs
tc_filename = "%d_agents_cadrl_raw.p" % num_agents
save_filename = 'multi_training_init'
NN_navigation_multi.generate_trajs_for_testcases(test_cases, file_dir=file_dir, \
filename=save_filename)
print("finished generating trajs for %s" % tc_filename)
def plot_non_coop_case(file_dir, format_str):
plt.rcParams.update({'font.size': 34})
save_folder_dir = file_dir + "/../../pickle_files/multi/results/figures/"
# load multiagent neural network
# load nn_rl
mode = 'no_constr'
passing_side = 'right'
# mode = 'rotate_constr'; passing_side = 'right'
iteration = 1000
filename = "twoAgents_policy_iter_%d.p" % iteration
# filename=None
value_net = nn_nav.load_NN_navigation_value(file_dir, mode, passing_side,
filename)
NN_navigation_multi = nn_nav_multi.NN_navigation_value_multi(value_net)
test_case = np.array([[-3.0, 0.0, 3.0, 0.0, 1.0, 0.5],\
[3.0, 0.0, -3.0, 0.0, 1.0, 0.5]])
traj_raw_multi, time_to_complete = \
NN_navigation_multi.generate_traj(test_case, figure_name='method 2', method=1)
plt.title('')
plt.locator_params(axis='y', nbins=5)
plt.locator_params(axis='x', nbins=5)
file_name = 'multi_traj_non_coop' + format_str
plt.savefig(save_folder_dir + file_name, bbox_inches='tight')
pass
def plot_multi_agent_cases(file_dir, format_str):
plt.rcParams.update({'font.size': 28})
save_folder_dir = file_dir + "/../../pickle_files/multi/results/figures/"
# load multiagent neural network
# load nn_rl
# mode = 'no_constr'
mode = 'rotate_constr'
passing_side = 'right'
iteration = 1000
filename = "twoAgents_policy_iter_%d.p" % iteration
# filename=None
value_net = nn_nav.load_NN_navigation_value(file_dir, mode, passing_side,
filename)
NN_navigation_multi = nn_nav_multi.NN_navigation_value_multi(value_net)
# six agent swap
test_cases = nn_nav_multi.preset_testCases()
traj_raw_multi, time_to_complete = \
NN_navigation_multi.generate_traj(test_cases[2], figure_name='method 1', method=1)
# raw_input()
plt.title('')
file_name = 'multi_traj_0' + format_str
plt.savefig(save_folder_dir + file_name, bbox_inches='tight')
traj_raw_multi, time_to_complete = \
NN_navigation_multi.generate_traj(test_cases[3], figure_name='method 1', method=1)
# raw_input()
plt.title('')
file_name = 'multi_traj_1' + format_str
plt.savefig(save_folder_dir + file_name, bbox_inches='tight')
# random test cases
for i in xrange(2, 10):
# np.random.seed(seed)
# seed+=1
# print 'seed', seed
# is_end_near_bnd = np.random.binomial(1, 0.5)
num_agents = 4
side_length = 3
test_case = nn_nav_multi.generate_rand_test_case_multi( num_agents, side_length,\
np.array([0.5, 1.2]), \
np.array([0.3, 0.5]), is_end_near_bnd=True)
traj_raw_multi, time_to_complete = \
NN_navigation_multi.generate_traj(test_case, figure_name='method 1', method=1)
plt.title('')
file_name = 'multi_traj_%d' % i + format_str
plt.savefig(save_folder_dir + file_name, bbox_inches='tight')
print('generated traj %d' % i)
raw_input()
pass
def generate_trajs_for_comparison_cases(file_dir, format_str):
save_folder_dir = file_dir + "/../../pickle_files/multi/results/figures/"
# generate test cases
num_agents_vec = [2, 4, 6, 8]
side_length_vec = [2.0, 2.5, 3.0, 3.5]
num_test_cases = 100
for i, num_agents in enumerate(num_agents_vec):
np.random.seed(1)
# side_length = 0.5 + num_agents/2.0
# print side_length
side_length = side_length_vec[i]
# print side_length
speed_bnds = np.array([0.5, 1.2])
radius_bnds = np.array([0.3, 0.5])
test_cases = gen_results.generate_test_cases(num_test_cases, num_agents, \
side_length, speed_bnds, radius_bnds, is_end_near_bnd=True)
# print test_cases[85]
# raw_input()
# test_cases = nn_rl.preset_testCases()
filename = file_dir + "/../../pickle_files/multi/results/%d_agents_test_cases.p" % num_agents
pickle.dump(test_cases, open(filename, "wb"))
print('saved %s' % filename)
# generate multiagent trajectories using neural networks
mode = 'no_constr'
passing_side = 'right'
# mode = 'rotate_constr'; passing_side = 'right'
iteration = 1000
filename = "twoAgents_policy_iter_%d.p" % iteration
# filename=None
value_net = nn_nav.load_NN_navigation_value(file_dir, mode, passing_side,
filename)
NN_navigation_multi = nn_nav_multi.NN_navigation_value_multi(value_net)
# generate trajs
for num_agents in num_agents_vec:
tc_filename = file_dir + "/../../pickle_files/multi/results/%d_agents_test_cases.p" % num_agents
save_filename = None
test_cases = pickle.load(open(tc_filename, "rb"))
# hard test cases for plotting
# test_cases = pickle.load(open(file_dir + \
# "/../../pickle_files/multi/results/hard_test_cases.p", "rb"))
# save_filename = 'hard_nn_trajs_iter_%d.p' % iteration
NN_navigation_multi.generate_trajs_for_testcases(test_cases, file_dir=file_dir, \
filename=save_filename)
print("finished generating trajs for %s" % tc_filename)
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 genenerate_intersection_cases(file_dir, format_str):
# generate_test_cases
sl = 2 # side_length
angles_vec = np.linspace(-np.pi / 6.0 * 5.0, 0.0, num=31)
angles_vec = np.squeeze(np.matlib.repmat(angles_vec, 1, 5))
print(angles_vec.shape)
# print angles_vec
test_cases = []
for angle in angles_vec:
test_case = np.array([[-sl, 0.0, sl, 0.0, 1.0, 0.3], \
[sl*np.cos(angle), sl*np.sin(angle), \
sl*np.cos(angle+np.pi), sl*np.sin(angle+np.pi),\
1.0, 0.3]])
test_cases.append(test_case)
filename = file_dir + "/../../pickle_files/multi/results/intersection_test_cases.p"
pickle.dump(test_cases, open(filename, "wb"))
print('saved %s' % filename)
# generate multiagent trajectories using neural networks
mode_vec = ['no_constr', 'rotate_constr']
passing_side = 'right'
for mode in mode_vec:
# load neural network
iteration = 1000
filename = "twoAgents_policy_iter_%d.p" % iteration
# filename=None
value_net = nn_nav.load_NN_navigation_value(file_dir, mode,
passing_side, filename)
NN_navigation_multi = nn_nav_multi.NN_navigation_value_multi(value_net)
print(test_cases)
# generate trajs
save_filename = 'intersection_%s_trajs_raw.p' % mode
NN_navigation_multi.generate_trajs_for_testcases(test_cases, file_dir=file_dir, \
filename=save_filename)
print("finished generating intersection trajs for %s" % mode)
# load value_net
num_agents = 4
# mode = 'no_constr'; passing_side = 'none'; iteration = 500
# mode = 'no_constr'; passing_side = 'left'; iteration = 500
# mode = 'no_constr'; passing_side = 'right'; iteration = 500
# mode = 'rotate_constr'; passing_side = 'none'; iteration = 1000
# mode = 'rotate_constr'; passing_side = 'left'; iteration = 500
mode = 'rotate_constr'
passing_side = 'right'
iteration = 600
filename = "/%d_agents_policy_iter_" % num_agents + str(iteration) + ".p"
# filename="/%d_agents_policy_iter_"%num_agents + str(2000) + ".p"
value_net = nn_nav.load_NN_navigation_value(file_dir,
num_agents,
mode,
passing_side,
filename=filename)
rospy.init_node('nn_jackal', anonymous=False)
veh_name = 'JA01'
veh_data = {
'goal': np.zeros((2, )),
'radius': 0.5,
'pref_speed': 1.2,
'kw': 10.0,
'kp': 1.0,
'name': 'JA01'
}
nn_jackal = NN_jackal(veh_name, veh_data, value_net)