diff --git a/minihawk/minihawk_scenario.py b/minihawk/minihawk_scenario.py index fd35542b..691a05ed 100644 --- a/minihawk/minihawk_scenario.py +++ b/minihawk/minihawk_scenario.py @@ -12,6 +12,7 @@ import matplotlib.pyplot as plt import yaml import pandas as pd +import seaborn as sns def plot_polytope_3d(A, b, ax=None, color="red", trans=0.2, edge=True): if ax is None: @@ -143,6 +144,10 @@ class AgentMode(Enum): plt.plot(trace[::2,0], trace[1::2,3], 'r') plt.title('z') plt.show() + + sns.color_palette('deep', n_colors=2) + trajectory_color, reachtube_color, _, obstacle_color = sns.color_palette()[:4] + plotter = pv.Plotter() trace_array = np.array(traces.nodes[0].trace['quad']) @@ -202,7 +207,7 @@ class AgentMode(Enum): obs_intersect[obs_idx] = 1 plot3dRect(trace_lb[j], trace_ub[j], plotter, color='red') else: - plot3dRect(trace_lb[j], trace_ub[j], plotter, color='blue') + plot3dRect(trace_lb[j], trace_ub[j], plotter, color=reachtube_color) # if j>620: # print("stop") # Condition for intersection @@ -230,18 +235,18 @@ class AgentMode(Enum): # else: # plotter.add_mesh(trajectory, color='blue', line_width=3) - plot3dRect( - [-78.6-4.5, 71.8-4.5, 30-2], - [-78.6+4.5, 71.8+4.5, 30+2], - plotter, color='green' - ) - # plot3dRect( - # [-83.2235, 69.56748, 60.03582], - # [-76.77987, 80.43198, 62.21454], - # plotter, color='red', trans=1 + # [-78.6-4.5, 71.8-4.5, 30-2], + # [-78.6+4.5, 71.8+4.5, 30+2], + # plotter, color='green' # ) + plot3dRect( + [-83.2235, 69.56748, 60.03582], + [-76.77987, 80.43198, 62.21454], + plotter, color=obstacle_color, trans=1 + ) + min_ub = np.inf for i in range(0, len(obstacle_data_removed)): # if i==66598: @@ -265,7 +270,7 @@ class AgentMode(Enum): continue if lb[0]<-500 or ub[0]>500: continue - print(i) + # print(i) # lb[2] += 77.01757264137268 # ub[2] += 77.01757264137268 if obs_intersect[i] == 1: diff --git a/minihawk/minihawk_scenario4.py b/minihawk/minihawk_scenario4.py index aa868ca2..9cb6cf54 100644 --- a/minihawk/minihawk_scenario4.py +++ b/minihawk/minihawk_scenario4.py @@ -14,6 +14,10 @@ import pandas as pd from verse.plotter.plotter2D import * import plotly.graph_objects as go +import seaborn as sns + +sns.set_theme() +sns.set_context('paper') def plot_polytope_3d(A, b, ax=None, color="red", trans=0.2, edge=True): if ax is None: @@ -197,6 +201,13 @@ class AgentMode(Enum): # plotter.add_mesh(trajectory, color='red', line_width=3) # idx_start = j-1 # prev_intersect = False + # fig0 = plt.figure(0, figsize=(2,2)) + # fig1 = plt.figure(1, figsize=(2,2)) + # fig2 = plt.figure(2, figsize=(2,2)) + + sns.color_palette('deep', n_colors=2) + trajectory_color, reachtube_color = sns.color_palette()[:2] + obstacles_idxes = np.where(obs_intersect)[0] for i in range(quad.all_traces.shape[0]): # if np.linalg.norm(quad.all_traces[i,0,1:]-init_c)<=0.01: @@ -209,18 +220,18 @@ class AgentMode(Enum): # else: if i==0: plt.figure(0) - plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,1]+79.5,'b', label='Simulated Trajectory') + plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,1]+79.5,color=trajectory_color, label='Simulated Trajectory') plt.figure(1) - plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,2]-71.8,'b', label='Simulated Trajectory') + plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,2]-71.8,color=trajectory_color, label='Simulated Trajectory') plt.figure(2) - plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,3],'b', label='Simulated Trajectory') + plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,3],color=trajectory_color, label='Simulated Trajectory') else: plt.figure(0) - plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,1]+79.5,'b') + plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,1]+79.5,color=trajectory_color) plt.figure(1) - plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,2]-71.8,'b') + plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,2]-71.8,color=trajectory_color) plt.figure(2) - plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,3],'b') + plt.plot(quad.all_traces[i,:,0],quad.all_traces[i,:,3],color=trajectory_color) # plt.figure(0) # plt.plot(quad.nominal_trace[:,0],quad.nominal_trace[:,1],'g') # plt.figure(1) @@ -228,36 +239,35 @@ class AgentMode(Enum): # plt.figure(2) # plt.plot(quad.nominal_trace[:,0],quad.nominal_trace[:,3],'g') - trace = np.array(traces.nodes[0].trace['quad']) plt.figure(0) - plt.plot(trace[::2,0], trace[::2,1]+79.5, 'r') - plt.plot(trace[::2,0], trace[1::2,1]+79.5, 'r') - plt.fill_between(trace[::2,0], trace[::2,1]+79.5, trace[1::2,1]+79.5, color='r' ,label='Reachtube') + plt.plot(trace[::2,0], trace[::2,1]+79.5, color=reachtube_color) + plt.plot(trace[::2,0], trace[1::2,1]+79.5, color=reachtube_color) + plt.fill_between(trace[::2,0], trace[::2,1]+79.5, trace[1::2,1]+79.5, color=reachtube_color ,label='Reachtube') # plt.title('x') plt.figure(1) - plt.plot(trace[::2,0], trace[::2,2]-71.8, 'r') - plt.plot(trace[::2,0], trace[1::2,2]-71.8, 'r') - plt.fill_between(trace[::2,0], trace[::2,2]-71.8, trace[1::2,2]-71.8, color='r' ,label='Reachtube') + plt.plot(trace[::2,0], trace[::2,2]-71.8, color=reachtube_color) + plt.plot(trace[::2,0], trace[1::2,2]-71.8, color=reachtube_color) + plt.fill_between(trace[::2,0], trace[::2,2]-71.8, trace[1::2,2]-71.8, color=reachtube_color ,label='Reachtube') # plt.title('y') plt.figure(2) - plt.plot(trace[::2,0], trace[::2,3], 'r') - plt.plot(trace[::2,0], trace[1::2,3], 'r') - plt.fill_between(trace[::2,0], trace[::2,3], trace[1::2,3], color='r' ,label='Reachtube') + plt.plot(trace[::2,0], trace[::2,3], color=reachtube_color) + plt.plot(trace[::2,0], trace[1::2,3], color=reachtube_color) + plt.fill_between(trace[::2,0], trace[::2,3], trace[1::2,3], color=reachtube_color ,label='Reachtube') # plt.title('z') goal_lb = [-79.5-5, 71.8-5, 30-2] goal_ub = [-79.5+5, 71.8+5, 30+2] - plt.figure(0) - plt.plot([trace[0,0], trace[-1,0]], [goal_lb[0]+79.5,goal_lb[0]+79.5], 'g', label = 'goal set') - plt.plot([trace[0,0], trace[-1,0]], [goal_ub[0]+79.5,goal_ub[0]+79.5], 'g') - plt.figure(1) - plt.plot([trace[0,0], trace[-1,0]], [goal_lb[1]-71.8,goal_lb[1]-71.8], 'g', label = 'goal set') - plt.plot([trace[0,0], trace[-1,0]], [goal_ub[1]-71.8,goal_ub[1]-71.8], 'g') - plt.figure(2) - plt.plot([trace[0,0], trace[-1,0]], [goal_lb[2],goal_lb[2]], 'g', label = 'goal set') - plt.plot([trace[0,0], trace[-1,0]], [goal_ub[2],goal_ub[2]], 'g') + # plt.figure(0) + # plt.plot([trace[0,0], trace[-1,0]], [goal_lb[0]+79.5,goal_lb[0]+79.5], 'g', label = 'goal set') + # plt.plot([trace[0,0], trace[-1,0]], [goal_ub[0]+79.5,goal_ub[0]+79.5], 'g') + # plt.figure(1) + # plt.plot([trace[0,0], trace[-1,0]], [goal_lb[1]-71.8,goal_lb[1]-71.8], 'g', label = 'goal set') + # plt.plot([trace[0,0], trace[-1,0]], [goal_ub[1]-71.8,goal_ub[1]-71.8], 'g') + # plt.figure(2) + # plt.plot([trace[0,0], trace[-1,0]], [goal_lb[2],goal_lb[2]], 'g', label = 'goal set') + # plt.plot([trace[0,0], trace[-1,0]], [goal_ub[2],goal_ub[2]], 'g') for obs_idx in obstacles_idxes: print(obs_idx, obs_lb[obs_idx], obs_ub[obs_idx]) @@ -270,25 +280,32 @@ class AgentMode(Enum): plt.figure(2) plt.plot([trace[0,0], trace[-1,0]], [obs_lb[obs_idx, 2],obs_lb[obs_idx, 2]], 'r') plt.plot([trace[0,0], trace[-1,0]], [obs_ub[obs_idx, 2],obs_ub[obs_idx, 2]], 'r') - plt.figure(0) + plt.figure(0,figsize=(2,2)) plt.xlabel('t',fontsize=14) plt.ylabel('x',fontsize=14) - plt.legend(prop={'size': 14}) - plt.xticks(fontsize=12) - plt.yticks(fontsize=12) - plt.figure(1) + plt.legend(prop={'size': 14},loc='upper right') + plt.xticks(fontsize=14) + plt.yticks(fontsize=14) + plt.tight_layout() + plt.savefig('sc4_x.png', bbox_inches='tight', pad_inches=0.0, dpi=300) + plt.figure(1,figsize=(2,2)) plt.xlabel('t',fontsize=14) plt.ylabel('y',fontsize=14) - plt.xticks(fontsize=12) - plt.yticks(fontsize=12) - plt.legend(prop={'size': 14}) - plt.figure(2) + plt.xticks(fontsize=14) + plt.yticks(fontsize=14) + plt.legend(prop={'size': 14},loc='upper right') + plt.tight_layout() + plt.savefig('sc4_y.png', bbox_inches='tight', pad_inches=0.0, dpi=300) + plt.figure(2,figsize=(2,2)) plt.xlabel('t',fontsize=14) plt.ylabel('z',fontsize=14) - plt.xticks(fontsize=12) - plt.yticks(fontsize=12) - plt.legend(prop={'size': 14}) - plt.show() + plt.xticks(fontsize=14) + plt.yticks(fontsize=14) + plt.legend(prop={'size': 14},loc='upper right') + plt.tight_layout() + plt.savefig('sc4_z.png', bbox_inches='tight', pad_inches=0.0, dpi=300) + # plt.show() + # if idx_start < pos.shape[0]: # color = 'red' if prev_intersect else 'blue' # trajectory = pv.lines_from_points(pos[idx_start:]) diff --git a/minihawk/sc1_x.png b/minihawk/sc1_x.png new file mode 100644 index 00000000..c4b61d97 Binary files /dev/null and b/minihawk/sc1_x.png differ diff --git a/minihawk/sc1_y.png b/minihawk/sc1_y.png new file mode 100644 index 00000000..663aabe4 Binary files /dev/null and b/minihawk/sc1_y.png differ diff --git a/minihawk/sc1_z.png b/minihawk/sc1_z.png new file mode 100644 index 00000000..55e0fdac Binary files /dev/null and b/minihawk/sc1_z.png differ diff --git a/minihawk/sc2_x.png b/minihawk/sc2_x.png new file mode 100644 index 00000000..49379e37 Binary files /dev/null and b/minihawk/sc2_x.png differ diff --git a/minihawk/sc2_y.png b/minihawk/sc2_y.png new file mode 100644 index 00000000..1723d25b Binary files /dev/null and b/minihawk/sc2_y.png differ diff --git a/minihawk/sc2_z.png b/minihawk/sc2_z.png new file mode 100644 index 00000000..e70dbe6f Binary files /dev/null and b/minihawk/sc2_z.png differ diff --git a/minihawk/sc3_x.png b/minihawk/sc3_x.png new file mode 100644 index 00000000..8ee8c43a Binary files /dev/null and b/minihawk/sc3_x.png differ diff --git a/minihawk/sc3_y.png b/minihawk/sc3_y.png new file mode 100644 index 00000000..23457f5a Binary files /dev/null and b/minihawk/sc3_y.png differ diff --git a/minihawk/sc3_z.png b/minihawk/sc3_z.png new file mode 100644 index 00000000..1ba642e9 Binary files /dev/null and b/minihawk/sc3_z.png differ diff --git a/minihawk/sc4_x.png b/minihawk/sc4_x.png new file mode 100644 index 00000000..286a7adf Binary files /dev/null and b/minihawk/sc4_x.png differ diff --git a/minihawk/sc4_y.png b/minihawk/sc4_y.png new file mode 100644 index 00000000..962534dd Binary files /dev/null and b/minihawk/sc4_y.png differ diff --git a/minihawk/sc4_z.png b/minihawk/sc4_z.png new file mode 100644 index 00000000..d49bcafd Binary files /dev/null and b/minihawk/sc4_z.png differ diff --git a/minihawk/sc5_x.png b/minihawk/sc5_x.png new file mode 100644 index 00000000..d4083398 Binary files /dev/null and b/minihawk/sc5_x.png differ diff --git a/minihawk/sc5_y.png b/minihawk/sc5_y.png new file mode 100644 index 00000000..7c49d2fd Binary files /dev/null and b/minihawk/sc5_y.png differ diff --git a/minihawk/sc5_z.png b/minihawk/sc5_z.png new file mode 100644 index 00000000..c22dcaf1 Binary files /dev/null and b/minihawk/sc5_z.png differ