def plot(time, Yc, figure=None, window_title="Flat Output Trajectory"):
figure = ppu.prepare_fig(figure, window_title, (20.48, 10.24))
#pdb.set_trace()
plots = [
("$x$", "m", 0.5, Yc[:, _x, 0]),
("$y$", "m", 0.5, Yc[:, _y, 0]),
("$z$", "m", 0.5, Yc[:, _z, 0]),
("$\psi$", "deg", 0.5, np.rad2deg(Yc[:, _psi, 0])),
("$x^{(1)}$", "m/s", 0.5, Yc[:, _x, 1]),
("$y^{(1)}$", "m/s", 0.5, Yc[:, _y, 1]),
("$z^{(1)}$", "m/s", 0.5, Yc[:, _z, 1]),
("$\psi^{(1)}$", "deg/s", 0.5, np.rad2deg(Yc[:, _psi, 1])),
("$x^{(2)}$", "m/s2", 0.5, Yc[:, _x, 2]),
("$y^{(2)}$", "m/s2", 0.5, Yc[:, _y, 2]),
("$z^{(2)}$", "m/s2", 0.5, Yc[:, _z, 2]),
("$\psi^{(2)}$", "deg/s2", 0.5, np.rad2deg(Yc[:, _psi, 2])),
("$x^{(3)}$", "m/s3", 0.5, Yc[:, _x, 3]),
("$y^{(3)}$", "m/s3", 0.5, Yc[:, _y, 3]),
("$z^{(3)}$", "m/s3", 0.5, Yc[:, _z, 3]),
("$\psi^{(3)}$", "deg/s3", 0.5, np.rad2deg(Yc[:, _psi, 3])),
("$x^{(4)}$", "m/s4", 0.5, Yc[:, _x, 4]),
("$y^{(4)}$", "m/s4", 0.5, Yc[:, _y, 4]),
("$z^{(4)}$", "m/s4", 0.5, Yc[:, _z, 4]),
("$\psi^{(4)}$", "deg/s4", 0.5, np.rad2deg(Yc[:, _psi, 4])),
]
figure = ppu.plot_in_grid(time, plots, 4, figure, window_title)
return figure
def plot(time, X, U=None, figure=None, window_title="Trajectory"):
figure = ppu.prepare_fig(figure, window_title, (20.48, 10.24))
eulers = np.array([pal.euler_of_quat(_q) for _q in X[:, sv_slice_quat]])
phi, theta, psi = eulers[:, 0], eulers[:, 1], eulers[:, 2]
plots = [
("$x$", "m", 0.5, X[:, sv_x]),
("$y$", "m", 0.5, X[:, sv_y]),
("$z$", "m", 0.5, X[:, sv_z]),
("$\dot{x}$", "m/s", 0.5, X[:, sv_xd]),
("$\dot{y}$", "m/s", 0.5, X[:, sv_yd]),
("$\dot{z}$", "m/s", 0.5, X[:, sv_zd]),
("$\phi$", "deg", 0.5, np.rad2deg(phi)),
("$\\theta$", "deg", 0.5, np.rad2deg(theta)),
("$\psi$", "deg", 0.5, np.rad2deg(psi)),
("$p$", "deg/s", 0.5, np.rad2deg(X[:, sv_p])),
("$q$", "deg/s", 0.5, np.rad2deg(X[:, sv_q])),
("$r$", "deg/s", 0.5, np.rad2deg(X[:, sv_r])),
]
if U is not None:
foo = np.empty((len(time)))
foo.fill(np.nan)
plots += [("$U$", "N", 0.1, foo)]
figure = ppu.plot_in_grid(time, plots, 3, figure, window_title)
if U is not None:
ax = plt.subplot(5, 3, 13)
for i, txt in enumerate(('fr', 'br', 'bl', 'fl')):
plt.plot(time, U[:, i], label=txt)
plt.legend()
return figure
pass
def plot(self, time, X, U=None, figure=None, window_title="Trajectory", extra_rows=0):
figure = ppu.prepare_fig(figure, window_title, (20.48, 10.24))
phi, theta, psi = X[:,self.sv_phi], X[:,self.sv_theta], X[:,self.sv_psi]
plots = [("$x$", "m", 0.5, X[:,self.sv_x]),
("$y$", "m", 0.5, X[:,self.sv_y]),
("$z$", "m", 0.5, X[:,self.sv_z]),
("$v$", "m/s", 0.5, X[:,self.sv_v]),
("$\\alpha$", "deg", 0.5, np.rad2deg(X[:,self.sv_alpha])),
("$\\beta$", "deg", 0.5, np.rad2deg(X[:,self.sv_beta])),
("$\phi$", "deg", 0.5, np.rad2deg(phi)),
("$\\theta$", "deg", 0.5, np.rad2deg(theta)),
("$\psi$", "deg", 0.5, np.rad2deg(psi)),
("$p$", "deg/s", 0.5, np.rad2deg(X[:,self.sv_p])),
("$q$", "deg/s", 0.5, np.rad2deg(X[:,self.sv_q])),
("$r$", "deg/s", 0.5, np.rad2deg(X[:,self.sv_r])),
]
# if U is not None: # force an extra row of plots
# foo = np.empty((len(time))); foo.fill(np.nan)
# plots += [("$Fb$", "N", 0.1, foo), ("$Mb$", "Nm", 0.1, foo)]
figure = ppu.plot_in_grid(time, plots, 3, figure, window_title, extra_rows=1+extra_rows if U is not None else extra_rows)
if U is not None:
ax = plt.subplot(5, 3, 13)
for i,txt in enumerate(('fx', 'fy', 'fz')):
plt.plot(time, U[:,i], label=txt)
plt.legend()
ax = plt.subplot(5, 3, 14)
for i,txt in enumerate(('mx', 'my', 'mz')):
plt.plot(time, U[:,i+3], label=txt)
plt.legend()
return figure