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)