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)