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_trajectory(cls,
time,
X,
U,
figure=None,
window_title="Trajectory be",
legend=None,
label='',
filename=None,
atm=None):
margins = (0.04, 0.05, 0.98, 0.96, 0.20, 0.34)
figure = p3_pu.prepare_fig(figure,
window_title,
figsize=(20.48, 10.24),
margins=margins)
plots = [("x", "m", X[:, cls.sv_x]), ("y", "m", X[:, cls.sv_y]),
("z", "m", X[:, cls.sv_z]), ("$u$", "m/s", X[:, cls.sv_u]),
("$v$", "m/s", X[:, cls.sv_v]), ("$w$", "m/s", X[:,
cls.sv_w]),
("$\phi$", "deg", np.rad2deg(X[:, cls.sv_phi])),
("$\\theta$", "deg", np.rad2deg(X[:, cls.sv_theta])),
("$\\psi$", "deg", np.rad2deg(X[:, cls.sv_psi])),
("$p$", "deg/s", np.rad2deg(X[:, cls.sv_p])),
("$q$", "deg/s", np.rad2deg(X[:, cls.sv_q])),
("$r$", "deg/s", np.rad2deg(X[:, cls.sv_r]))]
nrow = 5 if U is not None else 4
for i, (title, ylab, data) in enumerate(plots):
ax = plt.subplot(nrow, 3, i + 1)
plt.plot(time, data, label=label)
p3_pu.decorate(ax, title=title, ylab=ylab, legend=legend)
for i, min_yspan in enumerate(
[.5, .5, .5, .1, .1, 1., 1., 1., 1., 1., 1., 1.]):
p3_pu.ensure_yspan(plt.subplot(nrow, 3, i + 1), min_yspan)
return figure
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 plot2(time,
X,
U,
Yc=None,
P=None,
window_title="trajectory",
figure=None,
axs=None):
margins = (0.04, 0.05, 0.98, 0.96, 0.20, 0.34)
figure = figure or p3_pu.prepare_fig(figure,
window_title,
figsize=(0.75 * 20.48, 0.75 * 10.24),
margins=margins)
plots = [("$x$", "m", X[:, pmip.s_x]),
("$\\theta$", "deg", np.rad2deg(X[:, pmip.s_theta])),
("$\dot{x}$", "m/s", X[:, pmip.s_xd]),
("$\dot{\\theta}$", "deg/s", np.rad2deg(X[:, pmip.s_thetad])),
("$\\tau$", "N.m", U[:, pmip.iv_t])]
axs = axs if axs is not None else figure.subplots(3, 2).flatten()
for ax, (title, ylab, data) in zip(axs, plots):
ax.plot(time, data, linewidth=2)
p3_pu.decorate(ax, title=title, ylab=ylab)
if Yc is not None:
axs[0].plot(time, Yc, 'g', label='Setpoint')
return figure
def plot(time,
X,
U,
Yc=None,
P=None,
window_title="trajectory",
figure=None,
axs=None,
label=None):
#print('in plot {} {}'.format(figure, axs))
margins = (0.04, 0.05, 0.98, 0.96, 0.20, 0.34)
figure = figure or p3_pu.prepare_fig(figure,
window_title,
figsize=(0.75 * 20.48, 0.75 * 10.24),
margins=margins)
plots = [("x", "m", X[:, pmip.s_x]),
("$\\theta$", "deg", np.rad2deg(X[:, pmip.s_theta])),
("$\\tau$", "N.m", U[:, pmip.iv_t])]
axs = axs if axs is not None else figure.subplots(3, 1)
#pdb.set_trace()
for ax, (title, ylab, data) in zip(axs, plots):
ax.plot(time, data, linewidth=2, label=label)
p3_pu.decorate(ax, title=title, ylab=ylab)
if Yc is not None:
axs[0].plot(time, Yc, 'g', label='Setpoint')
if P is not None and P.tsat != float('inf'):
axs[2].plot(time, P.tsat * np.ones_like(time), 'k', label='sat')
axs[2].plot(time, -P.tsat * np.ones_like(time), 'k', label='-sat')
return figure, axs
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
def plot3d(time, Yc, figure=None, window_title="Flat Output Trajectory"):
figure = ppu.prepare_fig(figure, window_title, (20.48, 10.24))
ax = plt.gca()
points = Yc[:, _x:_z, 0].reshape(-1, 1, 2)
#pdb.set_trace()
segments = np.concatenate([points[:-1], points[1:]], axis=1)
norm = plt.Normalize(0, len(points))
lc = matplotlib.collections.LineCollection(segments, cmap='jet', norm=norm)
lc.set_array(np.arange(len(points)))
lc.set_linewidth(2)
line = ax.add_collection(lc)
figure.colorbar(line, ax=ax)
ax.set_xlim(points[:,0, _x].min(), points[:,0, _x].max())
ax.set_ylim(points[:,0, _y].min(), points[:,0, _y].max())
ax.set_aspect('equal'); plt.title('2D')
def plot_trajectory_ae(time,
X,
U=None,
figure=None,
window_title="Trajectory",
legend=None,
label='',
filename=None,
atm=None):
s = p3_fr.SixDOFAeroEuler
margins = (0.04, 0.05, 0.98, 0.96, 0.20, 0.34)
figure = p3_pu.prepare_fig(figure,
window_title,
figsize=(20.48, 10.24),
margins=margins)
nrow, ncol = 5 if U is not None else 4, 3
axes = figure.subplots(nrow, ncol, sharex=True)
plots = [("x", "m", X[:, s.sv_x]), ("y", "m", X[:, s.sv_y]),
("z", "m", X[:, s.sv_z]), ("v", "m/s", X[:, s.sv_va]),
("$\\alpha$", "deg", np.rad2deg(X[:, s.sv_alpha])),
("$\\beta$", "deg", np.rad2deg(X[:, s.sv_beta])),
("$\phi$", "deg", np.rad2deg(X[:, s.sv_phi])),
("$\\theta$", "deg", np.rad2deg(X[:, s.sv_theta])),
("$\\psi$", "deg", np.rad2deg(X[:, s.sv_psi])),
("$p$", "deg/s", np.rad2deg(X[:, s.sv_p])),
("$q$", "deg/s", np.rad2deg(X[:, s.sv_q])),
("$r$", "deg/s", np.rad2deg(X[:, s.sv_r]))]
for (title, ylab, data), ax in zip(plots, axes.flatten()):
ax.plot(time, data, label=label)
p3_pu.decorate(ax, title=title, ylab=ylab)
if legend != None:
plt.legend(legend, loc='best')
for min_yspan, ax in zip([1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.],
axes.flatten()):
p3_pu.ensure_yspan(ax, min_yspan)
iv_da, iv_de, iv_dr, iv_df = 1, 2, 3, 4 # FIXME... needs P?
if U is not None:
ax = axes[4, 0]
ax.plot(time, 100 * U[:, 0])
p3_pu.decorate(ax, title="$d_{th}$", ylab="%", min_yspan=1.)
ax = axes[4, 1]
ax.plot(time, np.rad2deg(U[:, iv_da]), label="aileron")
ax.plot(time, np.rad2deg(U[:, iv_dr]), label="rudder")
p3_pu.decorate(ax,
title="$d_a/d_r$",
ylab="deg",
min_yspan=1.,
legend=True)
ax = axes[4, 2]
ax.plot(time, np.rad2deg(U[:, iv_de]), label="elevator")
if U.shape[1] > iv_df:
ax.plot(time, np.rad2deg(U[:, iv_df]), label="flap")
p3_pu.decorate(ax,
title="$d_e/d_f$",
ylab="deg",
min_yspan=1.,
legend=True)
return figure, axes
def plot_trajectory_ee(time,
Xee,
U=None,
figure=None,
window_title="Trajectory",
legend=None,
label='',
filename=None,
atm=None):
s = p3_fr.SixDOFEuclidianEuler
margins = (0.04, 0.05, 0.98, 0.96, 0.20, 0.34)
figure = p3_pu.prepare_fig(figure,
window_title,
figsize=(20.48, 10.24),
margins=margins)
plots = [("x", "m", Xee[:, s.sv_x]), ("y", "m", Xee[:, s.sv_y]),
("z", "m", Xee[:, s.sv_z]), ("$\dot{x}$", "m/s", Xee[:, s.sv_xd]),
("$\dot{y}$", "m/s", Xee[:, s.sv_yd]),
("$\dot{z}$", "m/s", Xee[:, s.sv_zd]),
("$\phi$", "deg", np.rad2deg(Xee[:, s.sv_phi])),
("$\\theta$", "deg", np.rad2deg(Xee[:, s.sv_theta])),
("$\\psi$", "deg", np.rad2deg(Xee[:, s.sv_psi])),
("$p$", "deg/s", np.rad2deg(Xee[:, s.sv_p])),
("$q$", "deg/s", np.rad2deg(Xee[:, s.sv_q])),
("$r$", "deg/s", np.rad2deg(Xee[:, s.sv_r]))]
nrow = 5 if U is not None else 4
for i, (title, ylab, data) in enumerate(plots):
ax = plt.subplot(nrow, 3, i + 1)
plt.plot(time, data, label=label)
p3_pu.decorate(ax, title=title, ylab=ylab, legend=True)
#if legend!=None:
# plt.legend(legend, loc='best')
for i, min_yspan in enumerate(
[1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.]):
p3_pu.ensure_yspan(plt.subplot(nrow, 3, i + 1), min_yspan)
iv_da, iv_de, iv_dr, iv_df = 1, 2, 3, 4 # FIXME... needs P?
if U is not None:
ax = figure.add_subplot(5, 3, 13)
ax.plot(time, 100 * U[:, 0])
p3_pu.decorate(ax, title="$d_{th}$", ylab="%", min_yspan=0.01)
ax = figure.add_subplot(5, 3, 14)
ax.plot(time, np.rad2deg(U[:, iv_da]), label="aileron")
ax.plot(time, np.rad2deg(U[:, iv_dr]), label="rudder")
p3_pu.decorate(ax,
title="$d_a/d_r$",
ylab="deg",
min_yspan=1.,
legend=True)
ax = figure.add_subplot(5, 3, 15)
ax.plot(time, np.rad2deg(U[:, iv_de]), label="elevator")
ax.plot(time, np.rad2deg(U[:, iv_df]), label="flap")
p3_pu.decorate(ax,
title="$d_e/d_f$",
ylab="deg",
min_yspan=1.,
legend=True)
return figure
def find_best_radius(dm,
atm,
va=9.,
compute=False,
plot_atm=False,
plot_traj=False):
if plot_atm:
p3_pu.plot_slice_wind(atm, xmax=100, dx=2.5)
plt.show()
savefile_name = '/tmp/thermal_optim_{:.1f}.npz'.format(va)
if compute:
time, Xe, Ue, phi_sp, theta_sp = test_02_att_ctl.get_sim_defaults(
dm, tf=15, trim_args={
'h': 0,
'va': va,
'throttle': 0
})
vzs, phis = [], []
for _phi_sp in np.deg2rad(np.arange(0, 45, 1.)):
phi_sp = _phi_sp * np.ones(len(time))
time, X, U = test_02_att_ctl.run_simulation(dm,
time,
Xe,
Ue,
phi_sp,
theta_sp,
plot=plot_traj)
if plot_traj: plt.show()
Xee = dm.state_six_dof_euclidian_euler(X[-1], atm=None)
vzs.append(Xee[p3_fr.SixDOFEuclidianEuler.sv_zd])
phis.append(X[-1][dm.sv_phi])
#print('vz {:.2f}m/s'.format(Xee[p3_fr.SixDOFEuclidianEuler.sv_zd]))
#print('phi {}'.format(np.rad2deg(X[-1][dm.sv_phi])))
phis, vzs = np.array(phis), np.array(vzs)
g = 9.81
Rs = va**2 / g / np.tan(phis)
#atm = p3_atm.AtmosphereThermal1()
updrafts = np.array(
[atm.get_wind(pos_ned=[_r, 0, 0], t=0) for _r in Rs])
np.savez(savefile_name, vzs=vzs, phis=phis, Rs=Rs, updrafts=updrafts)
else:
_data = np.load(savefile_name)
vzs, phis, Rs, updrafts = [
_data[k] for k in ['vzs', 'phis', 'Rs', 'updrafts']
]
climb_rate = updrafts[1:, 2] + vzs[1:]
best_climb_idx = np.argmin(climb_rate)
res_txt = 'best vz {:.2f} radius {:.1f} roll {:.1f}'.format(
climb_rate[best_climb_idx], Rs[best_climb_idx],
np.rad2deg(phis[best_climb_idx]))
print('va {:.1f} '.format(va) + res_txt)
good_Rs = Rs < 100.
fig = p3_pu.prepare_fig(window_title='va = {:.1f} m/s'.format(va))
ax = plt.subplot(1, 3, 1)
plt.plot(np.rad2deg(phis), vzs, label='aircraft')
plt.plot(np.rad2deg(phis), updrafts[:, 2], label='updraft')
p3_pu.decorate(ax,
title='Vz',
xlab='roll in deg',
ylab='vz in m/s',
legend=True)
plt.subplot(1, 3, 2)
if 1:
plt.plot(np.rad2deg(phis[good_Rs]), Rs[good_Rs])
p3_pu.decorate(plt.gca(),
title='Radius',
xlab='roll in deg',
ylab='radius in m')
if 0:
plt.plot(np.rad2deg(phis), 1 / Rs)
p3_pu.decorate(plt.gca(),
title='Curvature',
xlab='roll in deg',
ylab='curvature in m^-1')
#pdb.set_trace()
ax = plt.subplot(1, 3, 3)
#plt.plot(Rs[good_Rs], vzs[good_Rs], label='aircraft')
#plt.plot(Rs[good_Rs], updrafts[good_Rs,2], label='updrafts')
plt.plot(Rs[1:], vzs[1:], label='aircraft')
plt.plot(Rs[1:], updrafts[1:, 2], label='updrafts')
plt.plot(Rs[1:], updrafts[1:, 2] + vzs[1:], label='sum')
p3_pu.decorate(ax,
title='Vz\n{}'.format(res_txt),
xlab='R in m',
ylab='vz in m/s',
legend=True)
