diff --git a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py index d703a8562749e2018c99457b3208d9a834b17d57..5421490f93af12a7583ef3819b7f86566f18006b 100644 --- a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py +++ b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py @@ -64,7 +64,6 @@ t_np = t_np -t_np[0] pose_data_np = np.array(pose_data); vel_data_np = np.array(vel_data); acc_data_np = np.array(acc_data); -print(acc_data_np) x_list = range(0,n_robots_logged*2,2); y_list = range(1,n_robots_logged*2,2); x_list_np = np.array(pose_data_np[:,x_list]); @@ -173,6 +172,75 @@ axs4[1, 0].legend(robot_names) # multple robot plot for comparision +####################### Inputs plot ################################### +fig4, axs5 = plt.subplots(nrows=6, ncols=2, figsize=(7, 7)) + +axs5[0, 0].plot(t_np,input_data_np[:,0]) +axs5[0, 1].plot(t_np,input_data_np[:,1]) +axs5[1, 0].plot(t_np,input_formation_data_np[:,0]) +axs5[1, 1].plot(t_np,input_formation_data_np[:,1]) +axs5[2, 0].plot(t_np,input_orientation_data_np[:,0]) +axs5[2, 1].plot(t_np,input_orientation_data_np[:,1]) +axs5[3, 0].plot(t_np,input_navigation_data_np[:,0]) +axs5[3, 1].plot(t_np,input_navigation_data_np[:,1]) +axs5[4, 0].plot(t_np,input_obstacle_data_np[:,0]) +axs5[4, 1].plot(t_np,input_obstacle_data_np[:,1]) +axs5[5, 0].plot(t_np,input_integration_data_np[:,0]) +axs5[5, 1].plot(t_np,input_integration_data_np[:,1]) + +axs5[0, 0].set_ylabel("a(m2/s") +axs5[0, 0].set_xlabel("time(s)") +axs5[0, 0].set_title("input") +axs5[0, 1].set_ylabel("a(m2/s") +axs5[0, 1].set_xlabel("time(s)") +axs5[0, 1].set_title("input") +axs5[1, 0].set_ylabel("a(m2/s") +axs5[1, 0].set_xlabel("time(s)") +axs5[1, 0].set_title("input_formation") +axs5[1, 1].set_ylabel("a(m2/s") +axs5[1, 1].set_xlabel("time(s)") +axs5[1, 1].set_title("input_formation") +axs5[2, 0].set_ylabel("a(m2/s") +axs5[2, 0].set_xlabel("time(s)") +axs5[2, 0].set_title("input_orientation") +axs5[2, 1].set_ylabel("a(m2/s") +axs5[2, 1].set_xlabel("time(s)") +axs5[2, 1].set_title("input_orientation") +axs5[3, 0].set_ylabel("a(m2/s") +axs5[3, 0].set_xlabel("time(s)") +axs5[3, 0].set_title("input_navigation") +axs5[3, 1].set_ylabel("a(m2/s") +axs5[3, 1].set_xlabel("time(s)") +axs5[3, 1].set_title("input_navigation") +axs5[4, 0].set_ylabel("a(m2/s") +axs5[4, 0].set_xlabel("time(s)") +axs5[4, 0].set_title("input_obstacle") +axs5[4, 1].set_ylabel("a(m2/s") +axs5[4, 1].set_xlabel("time(s)") +axs5[4, 1].set_title("input_obstacle") +axs5[5, 0].set_ylabel("a(m2/s") +axs5[5, 0].set_xlabel("time(s)") +axs5[5, 0].set_title("input_integration") +axs5[5, 1].set_ylabel("a(m2/s") +axs5[5, 1].set_xlabel("time(s)") +axs5[5, 1].set_title("input_integration") +axs5[0, 0].set_ylabel("ax(m)") +axs5[0, 0].set_xlabel("time(s)") + +####################### position plot ################################### +fig6 = plt.figure() +axs6 = plt.axes() +# st = fig6.suptitle("Distance plot", fontsize="x-large") +# plot workspace +axs6.set_xlim(-3, 3) +axs6.set_ylim(-2, 2) +# +for i in range(n_robots_logged): + axs6.plot(pose_data_np[:,i*2],pose_data_np[:,i*2+1]) +axs6.set_ylabel("y(m)") +axs6.set_xlabel("x(m)") +axs6.legend(robot_names) + plt.show()