-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathplotData.py
executable file
·149 lines (131 loc) · 6.29 KB
/
plotData.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
import matplotlib.pyplot as plt
from matplotlib.lines import Line2D
import autograd.numpy as np
plt.ion() # enable interactive drawing
class plotData:
'''
This class plots the time histories for the pendulum data.
'''
def __init__(self):
# Number of subplots = num_of_rows*num_of_cols
self.num_rows = 5 # Number of subplot rows
self.num_cols = 1 # Number of subplot columns
# Crete figure and axes handles
self.fig, self.ax = plt.subplots(self.num_rows, self.num_cols, sharex=True)
# Instantiate lists to hold the time and data histories
self.time_history = [] # time
self.zref_history = [] # reference position z_r
self.z_history = [] # position z
self.href_history = [] # reference altitude h_r
self.h_history = [] # altitude h
self.theta_history = [] # angle theta
self.Force_history = [] # control force
self.Torque_history = [] # control torque
# create a handle for every subplot.
self.handle = []
self.handle.append(myPlot(self.ax[0], ylabel='z(m)', title='Planar VTOL'))
self.handle.append(myPlot(self.ax[1], ylabel='h(m)'))
self.handle.append(myPlot(self.ax[2], ylabel='theta(deg)'))
self.handle.append(myPlot(self.ax[3], ylabel='fr(N)'))
self.handle.append(myPlot(self.ax[4], xlabel='t(s)', ylabel='fl(N)'))
def updatePlots(self, tt, states, z_ref, h_ref, thrust_right, thrust_left):
'''
Add to the time and data histories, and update the plots.
'''
# update the time history of all plot variables
self.time_history.append(tt) # time
self.zref_history.append(z_ref) # reference position
self.z_history.append(states[0]) # position
self.href_history.append(h_ref) # reference position
self.h_history.append(states[1]) # position
self.theta_history.append(180.0/np.pi*states[2]) # VTOL angle (converted to degrees)
self.Force_history.append(thrust_right) # thrust_right
self.Torque_history.append(thrust_left) # thrust_left
# update the plots with associated histories
self.handle[0].updatePlot(self.time_history, [self.z_history, self.zref_history])
self.handle[1].updatePlot(self.time_history, [self.h_history, self.href_history])
self.handle[2].updatePlot(self.time_history, [self.theta_history])
self.handle[3].updatePlot(self.time_history, [self.Force_history])
self.handle[4].updatePlot(self.time_history, [self.Torque_history])
def batchUpdatePlots(self, tt, states, z_ref, h_ref, thrust_right, thrust_left):
'''
Add to the time and data histories, and update the plots.
'''
# update the time history of all plot variables
self.time_history = tt # time
self.zref_history = z_ref # reference position
self.z_history = states[0] # position
self.href_history = h_ref # reference position
self.h_history = states[1] # position
self.theta_history = 180.0/np.pi*states[2] # VTOL angle (converted to degrees)
self.Force_history = thrust_right # thrust_right
self.Torque_history = thrust_left # thrust_left
# update the plots with associated histories
self.handle[0].updatePlot(self.time_history, [self.z_history, self.zref_history])
self.handle[1].updatePlot(self.time_history, [self.h_history, self.href_history])
self.handle[2].updatePlot(self.time_history, [self.theta_history])
self.handle[3].updatePlot(self.time_history, [self.Force_history])
self.handle[4].updatePlot(self.time_history, [self.Torque_history])
class myPlot:
'''
Create each individual subplot.
'''
def __init__(self, ax,
xlabel='',
ylabel='',
title='',
legend=None):
'''
ax - This is a handle to the axes of the figure
xlable - Label of the x-axis
ylable - Label of the y-axis
title - Plot title
legend - A tuple of strings that identify the data.
EX: ("data1","data2", ... , "dataN")
'''
self.legend = legend
self.ax = ax # Axes handle
self.colors = ['b', 'g', 'r', 'c', 'm', 'y', 'b']
# A list of colors. The first color in the list corresponds
# to the first line object, etc.
# 'b' - blue, 'g' - green, 'r' - red, 'c' - cyan, 'm' - magenta
# 'y' - yellow, 'k' - black
self.line_styles = ['-', '-', '--', '-.', ':']
# A list of line styles. The first line style in the list
# corresponds to the first line object.
# '-' solid, '--' dashed, '-.' dash_dot, ':' dotted
self.line = []
# Configure the axes
self.ax.set_ylabel(ylabel)
self.ax.set_xlabel(xlabel)
self.ax.set_title(title)
self.ax.grid(True)
# Keeps track of initialization
self.init = True
def updatePlot(self, time, data):
'''
Adds data to the plot.
time is a list,
data is a list of lists, each list corresponding to a line on the plot
'''
if self.init == True: # Initialize the plot the first time routine is called
for i in range(len(data)):
# Instantiate line object and add it to the axes
self.line.append(Line2D(time,
data[i],
color=self.colors[np.mod(i, len(self.colors) - 1)],
ls=self.line_styles[np.mod(i, len(self.line_styles) - 1)],
label=self.legend if self.legend != None else None))
self.ax.add_line(self.line[i])
self.init = False
# add legend if one is specified
if self.legend != None:
plt.legend(handles=self.line)
else: # Add new data to the plot
# Updates the x and y data of each line.
for i in range(len(self.line)):
self.line[i].set_xdata(time)
self.line[i].set_ydata(data[i])
# Adjusts the axis to fit all of the data
self.ax.relim()
self.ax.autoscale()