def test_vctl(dm, trim_args, force_recompute=False, dt=0.01, tf=80.5, atm=None,
save_filename = '/tmp/pat_glider_vctl_w.npz'):
atm = p3_atm.AtmosphereCalm()
#atm = p3_atm.AtmosphereCstWind([1, 0, 0])
#trim_args={'h':26, 'va':17, 'gamma':0}
#ref_traj = p3_traj3d.CircleRefTraj(c=[70, 0, -15], r=80)
#ref_traj = p3_traj3d.ZStepCircleRefTraj(c=[70, 0, -10], r=80)
ref_traj = p3_traj3d.ZdStepCircleRefTraj(c=[70, 0, -10], r=120)
#ref_traj = p3_traj3d.BankedCircleRefTraj(c=[70, 0, -30], r=150)
#ref_traj = p3_traj3d.SquareRefTraj()
#ref_traj = p3_traj3d.OctogonRefTraj() # kindof broken
#ref_traj = p3_traj3d.OvalRefTraj() #
ctl = p3_guid.GuidancePurePursuit(dm, ref_traj, trim_args, dt)
#ctl.set_vmode(ctl.v_mode_throttle, 0.)
#ctl.set_vmode(ctl.v_mode_vz, -1.)
#ctl.set_vmode(ctl.v_mode_alt, -12)
ctl.set_vmode(ctl.v_mode_traj, None)
ctl.set_initial_conditions((70, -80, -4), None)
ctl_logger = p3_guid.GuidancePurePursuitLogger()
time, X, U = _run_or_load_sim(dm, ctl, None, tf, dt, trim_args, atm, ctl_logger, force_recompute, save_filename)
ctl_logger.plot3D(time, X, ctl, atm, ref_traj)
ctl_logger.plot_slice_nu(time, X, U, ctl, atm)
ctl_logger.plot_chronograms(time, X, U, ctl, atm)
def main(param_filename,
force_recompute=False,
save_filename='/tmp/pat_glider_wind.npz'):
dm = p1_fw_dyn.DynamicModel(param_filename)
trim_args = {'h': 0, 'va': 11, 'gamma': 0}
ref_traj = p3_traj3d.LineRefTraj(p1=[0, 0, 0], p2=[250, 0, 0])
tf = 5.5
#atm = None
#atm = p3_atm.AtmosphereCalm()
#atm = p3_atm.AtmosphereCstWind([-1, 0, 0])
#atm = p3_atm.AtmosphereVgradient([0, 0, 3])
#atm = p3_atm.AtmosphereSinetWind([1, 0, 0])
atm = p3_atm.AtmosphereSinetWind([-5, 0, -1])
#atm = p3_atm.AtmosphereSinedWind([-5, 0, -1])
#atm = p3_atm.AtmosphereThermal1()
dt = 0.01
ctl = p3_guid.GuidancePurePursuit(dm, ref_traj, trim_args, dt)
ctl.set_vmode(ctl.v_mode_throttle, ctl.Ue[0])
#ctl.att_ctl.reset(0, ctl.Xe) # Xe must be SixDOFAeroEuler
ctl_logger = p3_guid.GuidancePurePursuitLogger()
#p3_pu.plot_3D_wind(atm)
#p3_pu.plot_slice_wind(atm)
#plt.show()
#time, X, U = test_03_guidance.run_simulation(dm, ctl, None, tf=tf, dt=dt, trim_args=trim_args, atm=atm,
# cbk=lambda:ctl_logger.record(ctl))
time, X, U = test_03_guidance._run_or_load_sim(dm, ctl, None, tf, dt,
trim_args, atm, ctl_logger,
force_recompute,
save_filename)
ctl_logger.plot_chronograms(time, X, U, ctl, atm)
#ctl_logger.plot_debug_chronograms(time, X, U, ctl, atm)
plt.show()
def test_circle(dm, trim_args, force_recompute=False, dt=0.01, tf=30.5, atm=None,
save_filename = '/tmp/pat_glider_circle.npz'):
ref_traj = p3_traj3d.CircleRefTraj(c=[0, 0, 0], r=20)
ctl = p3_guid.GuidancePurePursuit(dm, ref_traj, trim_args, dt)
ctl.v_mode = ctl.v_mode_throttle; ctl.throttle_sp = ctl.Ue[0]
ctl.set_initial_conditions((0, 20, 0), None)
ctl_logger = p3_guid.GuidancePurePursuitLogger()
time, X, U = _run_or_load_sim(dm, ctl, None, tf, dt, trim_args, atm, ctl_logger, force_recompute, save_filename)
ctl_logger.plot_chronograms(time, X, U, ctl, atm)
ctl_logger.plot3D(time, X, ctl, atm, ref_traj)
def main():
# Read aircraft trajectory and control variables from a file
ctl_logger = p3_guid.GuidancePurePursuitLogger()
time, X, U = ctl_logger.load('/tmp/pat_glider_ds.npz')
# This is needed :(
atm = p3_atm.AtmosphereShearX(wind1=7.0,
wind2=-1.0,
xlayer=60.0,
zlayer=40.0)
# Convert aircraft trajectory to euclidian/euler state vector
Xee = np.array([
p3_fr.SixDOFAeroEuler.to_six_dof_euclidian_euler(_X, atm, _t)
for _X, _t in zip(X, time)
])
# Compute energy
inertial_vel_ned = Xee[:, p3_fr.SixDOFEuclidianEuler.sv_slice_vel]
inertial_vel_norm = np.linalg.norm(inertial_vel_ned, axis=1)
mass, g = 1.7, 9.81 # we can get that from the cularis dynamic model if needed, as well as inertia
kinetic_energy = 0.5 * mass * inertial_vel_norm**2
pos_ned = Xee[:, p3_fr.SixDOFEuclidianEuler.sv_slice_pos]
potential_energy = mass * g * -Xee[:, p3_fr.SixDOFEuclidianEuler.sv_z]
#pdb.set_trace()
# Plot aircraft trajectory
#p1_fw_dyn.plot_trajectory_ae(time, X, U, window_title='chronogram', atm=atm)
_ctl = None # not needed
ctl_logger.plot_chronograms(time, X, U, _ctl, atm)
ctl_logger.plot3D(time, X, _ctl, atm)
ctl_logger.plot_slice_nu(time,
X,
U,
_ctl,
atm,
ref_traj=None,
n0=0,
n1=210,
dn=10,
h0=0,
h1=80,
dh=10)
# Plot energy
plt.figure()
plt.plot(time, kinetic_energy, label='kinetic energy')
plt.plot(time, potential_energy, label='potential energy')
plt.plot(time, potential_energy + kinetic_energy, label='total energy')
p3_pu.decorate(plt.gca(),
title='Energy',
xlab='time in s',
ylab='E in joules',
legend=True)
plt.show()
def test_slope_soaring(dm, trim_args, force_recompute=False, dt=0.01, tf=120.5,
save_filename = '/tmp/pat_glider_slope_soaring.npz'):
atm = p3_atm.AtmosphereRidge()
trim_args={'h':30, 'va':12, 'gamma':0}
#ref_traj = p3_traj3d.CircleRefTraj(c=[-10, 0, -50], r=25)
ref_traj = p3_traj3d.OvalRefTraj( _r=25., _l=200, _c=np.array([-10, 0, -50])) #
ctl = p3_guid.GuidancePurePursuit(dm, ref_traj, trim_args, dt)
ctl.v_mode = ctl.v_mode_throttle; ctl.throttle_sp = 0. # we glide
ctl_logger = p3_guid.GuidancePurePursuitLogger()
time, X, U = _run_or_load_sim(dm, ctl, None, tf, dt, trim_args, atm, ctl_logger, force_recompute, save_filename)
ctl_logger.plot3D(time, X, ctl, atm, ref_traj)
ctl_logger.plot_slice_nu(time, X, U, ctl, atm, n0=-50, n1=40)
ctl_logger.plot_chronograms(time, X, U, ctl, atm)
def test_dynamic_soaring(dm, trim_args, force_recompute=False, dt=0.005, tf=150.):
#atm = p3_atm.AtmosphereShearX(wind1=15.0, wind2=-2.0, xlayer=60.0, zlayer=40.0)
#atm = p3_atm.AtmosphereShearX(wind1=7.0, wind2=-1.0, xlayer=60.0, zlayer=40.0)
#atm = p3_atm.AtmosphereShearX(wind1=5.0, wind2=0.0, xlayer=60.0, zlayer=40.0)
atm, save_filename = _ds_exp(4)
trim_args={'h':30, 'va':17, 'gamma':0}
#ref_traj = p3_traj3d.CircleRefTraj(c=[0, 0, -20], r=20)
ref_traj = p3_traj3d.BankedCircleRefTraj(c=[100, 0, -40], r=60, slope=np.deg2rad(10))
#ctl = p3_guid.GuidancePurePursuit(dm, ref_traj, trim_args, dt, lookahead_dist=15., max_phi=np.deg2rad(45))
ctl = p3_guid.GuidanceDS(dm, ref_traj, trim_args, dt, lookahead_dist=15., max_phi=np.deg2rad(60))
ctl.v_mode = ctl.v_mode_alt
ctl_logger = p3_guid.GuidancePurePursuitLogger()
time, X, U = _run_or_load_sim(dm, ctl, None, tf, dt, trim_args, atm, ctl_logger, force_recompute, save_filename)
ctl_logger.plot3D(time, X, ctl, atm)
ctl_logger.plot_slice_nu(time, X, U, ctl, atm, ref_traj, n0=0, n1=210, dn=10, h0=0, h1=80, dh=10)
ctl_logger.plot_chronograms(time, X, U, ctl, atm)
return ref_traj, (time, X, U)
def main(dt=0.005):
param_filename = p3_u.pat_ressource('data/vehicles/cularis.xml')
dm = p1_fw_dyn.DynamicModel(param_filename)
trim_args = {'h': 30, 'va': 17, 'gamma': 0}
if 0:
atm = p3_atm.AtmosphereCalm()
save_filename = '/tmp/pat_glider_ds_nw.npz'
if 0:
atm = p3_atm.AtmosphereCstWind([1, 0, 0])
save_filename = '/tmp/pat_glider_ds_wc100.npz'
if 0:
atm = p3_atm.AtmosphereRidge()
save_filename = '/tmp/pat_glider_ds_wr.npz'
if 0:
atm = p3_atm.AtmosphereShearX(wind1=5.0,
wind2=0.0,
xlayer=60.0,
zlayer=40.0)
save_filename = '/tmp/pat_glider_ds_ws50.npz'
if 1:
atm = p3_atm.AtmosphereVgradient()
save_filename = '/tmp/pat_glider_ds_wvg.npz'
#ref_traj = p3_traj3d.BankedCircleRefTraj(c=[100, 0, -40], r=60, slope=np.deg2rad(10))
#ctl = p3_guid.GuidanceDS(dm, ref_traj, trim_args, dt, lookahead_dist=15., max_phi=np.deg2rad(60))
ctl_logger = p3_guid.GuidancePurePursuitLogger()
time, Xae, U = ctl_logger.load(save_filename)
# aliasing state variables
_pos = Xae[:, p3_fr.SixDOFAeroEuler.sv_slice_pos]
_alt = -Xae[:, p3_fr.SixDOFAeroEuler.sv_z]
_va = Xae[:, p3_fr.SixDOFAeroEuler.sv_va]
_va2 = _va**2
_alpha = Xae[:, p3_fr.SixDOFAeroEuler.sv_alpha]
_eul = Xae[:, p3_fr.SixDOFAeroEuler.sv_slice_eul]
_phi, _theta, _psi = [_eul[:, _i] for _i in range(3)]
_gamma = _theta - _alpha
# aliasing input variables
_throttle = U[:, dm.iv_dth()]
# compute state variable in euclidian/euler format
Xee = np.array([dm.to_six_dof_euclidian_euler(_X, atm, 0.) for _X in Xae])
_vi = Xee[:, p3_fr.SixDOFEuclidianEuler.sv_slice_vel]
groundspeed_3d = np.linalg.norm(_vi, axis=1) # I hate this variable name
#ctl_logger.plot_chronograms(time, X, U, ctl, atm2)
# Compute Energy
e_kin_air = 0.5 * dm.P.m * _va2
alt_0 = 30.
e_pot = dm.P.m * 9.81 * (_alt - alt_0)
e_tot_air = e_kin_air + e_pot
# Compute wind in body frame
#df.wx.iloc[i] = -V_a * cos(theta-np.deg2rad(alpha)) + V_g
#df.wz.iloc[i] = V_a * sin(theta-np.deg2rad(alpha)) + V_z
#Vg is gps ground speed, in X-Y
#and Vz is climb speed
# def estimate_inplane_wind(df_in):
# df = df_in.copy() # Be careful, this is important !!!
# for i in range(df.index.shape[0]):
# V_a = df.airspeed.iloc[i]
# theta = df.theta.iloc[i]
# #alpha = df.alpha.iloc[i]
# alpha = alpha_func(df.index[i])
# V_g = df.vel.iloc[i]
# # V_g = df.vel_3d.iloc[i]
# V_z = -df.climb.iloc[i] #
# df.wx.iloc[i] = -V_a * cos(theta-np.deg2rad(alpha)) + V_g
# df.wz.iloc[i] = V_a * sin(theta-np.deg2rad(alpha)) + V_z # going up is negative for wz
# df.alpha[i] = alpha
# return df
Wned = np.array([atm.get_wind_ned(_p, _t) for _p, _t in zip(_pos, time)])
Wb = np.array(
[p3_fr.vel_world_to_body_eul(_v, _e) for _v, _e in zip(Wned, _eul)])
Wx, Wy, Wz = [Wb[:, _i] for _i in range(3)]
dt = time[1] - time[0]
dWx, dWz = np.gradient(Wx, dt), np.gradient(Wz, dt)
# compute power
rho = 1.225
AR = 11.84
e = 0.85
# I don't have accel, let's compute it
Az = np.gradient(_vi[:, 2], dt)
Lift = -Az * dm.P.m
#Lift = -Az * mass
CL = Lift / (0.5 * rho * _va2 * dm.P.Sref)
CD = dm.P.CD0 + CL**2 / (np.pi * AR * e)
D = -0.5 * rho * _va2 * dm.P.Sref * CD
P_drag = _va * D
P_dwx = -dWx * (-_va * np.sign(_gamma) * np.cos(_gamma))
P_dwz = dWz * (_va * np.sin(_gamma))
#matplotlib.rcParams['text.usetex'] = True
plt.rcParams["font.family"] = "Times New Roman"
plt.rcParams["font.size"] = 11
fig = plt.figure(figsize=(10, 14.5))
axes = fig.subplots(6, 1, sharex=True)
fig.subplots_adjust(top=0.975,
bottom=0.035,
left=0.125,
right=0.97,
hspace=0.165,
wspace=0.205)
ax = axes[0] #fig.add_subplot(611)
ax.set_title('Energy in Air-Path Frame')
ax.plot(time, e_tot_air, label='$E_{Total}$')
ax.plot(time, e_kin_air, label='$E_{Kinetic}$')
ax.plot(time, e_pot, label='$E_{Potential}$')
ax.grid()
ax.legend()
ax.set_ylabel('Energy [J]')
ax = axes[1] #fig.add_subplot(612)
ax.plot(time, _alt - alt_0, label='Height AGL')
ax.plot(time, _va, label='$V_a$')
ax.plot(time, groundspeed_3d, label='$V_{i}$')
ax.grid()
ax.set_ylabel('Height AGL [$m$] \nSpeed [$m/s$]')
ax.legend()
ax = axes[2] #fig.add_subplot(613)
ax.plot(time, np.rad2deg(_gamma), label='$\gamma$ [deg]')
ax.plot(time, Wx, label='$W_x$')
ax.plot(time, dWx, label='$\dot{W_x}$')
ax.plot(time, Wz, label='$W_z$')
ax.plot(time, -_vi[:, 2], label='$Vi_z$')
ax.grid()
ax.legend()
ax.set_ylabel(
'Flight Path ($\gamma$) [deg] \n Wind Speed [$m/s$] \n Gradient [$m/s^2$]'
)
ax = axes[3] #fig.add_subplot(614)
ax.plot(time,
P_drag,
color='red',
label='$P_D$ [W], $\sum{P_D}$ = %0.2f [Ws]' %
(np.nansum(P_drag) / 100))
ax.plot(time,
P_dwx,
color='black',
alpha=0.6,
label='$P_{\dot{W}_X}$ [W], $\sum{P_{\dot{W}_X}}$ = %0.2f [Ws]' %
(np.nansum(P_dwx) / 100))
ax.fill_between(time,
0,
P_dwx,
where=(P_dwx >= 0),
alpha=0.50,
color='green',
interpolate=True)
ax.fill_between(time,
0,
P_dwx,
where=(P_dwx < 0),
alpha=0.50,
color='red',
interpolate=True)
ax.grid()
ax.legend()
ax.set_ylabel('Power ($P_{\dot{W}_X}\, & \, P_D$) [W]')
ax = axes[4] #fig.add_subplot(615)
ax.plot(time,
P_dwz,
color='black',
alpha=0.3,
label='$P_{\dot{W_Z}}$, $\sum{P_{\dot{W_Z}}}$ = %0.2f [Ws]' %
(np.nansum(P_dwz) / 100))
ax.fill_between(time,
0,
P_dwz,
where=(P_dwz >= 0),
alpha=0.50,
color='green',
interpolate=True) #, label='P$_{\dot{w}}$');
ax.fill_between(time,
0,
P_dwz,
where=(P_dwz < 0),
alpha=0.50,
color='red',
interpolate=True) #, label='P$_{\dot{w}}$');
ax.grid()
plt.legend()
ax.set_ylabel('Power ($P_{\dot{W}_Z}$) [W]')
ax.set_xticklabels([])
ax = axes[5] #fig.add_subplot(616);
ax.plot(time,
_throttle,
label='Throttle, Averaged = %0.2f' % (np.nanmean(_throttle)))
#ax.fill_between(time,0,electrical_power, where=(_throttle >= throttle_limit), alpha=0.20, color='grey', interpolate=True)
#ax.plot(time,electrical_power, label='$P_{Elec.}$ [W] , $\sum{P_{Elec.}}$ = %0.2f [Ws]' % (np.nansum(electrical_power[throttle>=throttle_limit])/100.* propulsion_eff) ); #np.nanmean(electrical_power),
ax.grid()
plt.legend()
ax.set_ylabel('Throttle [\%] \n Elec. Power ($P_{Elec.}$) [W]')
ax.set_xlabel('Time [s]')
#ax.set_xlim([20, 30])
plt.savefig('/tmp/traj_murat_ds.png', dpi=120, bbox_inches='tight')