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 test1():
param_filename = os.path.join(p3_u.pat_dir(), 'data/vehicles/skywalker_x8.xml')
dm = p1_fw_dyn.DynamicModel(param_filename)
plot_polar(dm)
#trim_throttle(dm)
plot_trims(dm)
plt.show()
def main():
logging.basicConfig(level=logging.INFO)
np.set_printoptions(linewidth=500)
param_filename = '/home/poine/work/pat/data/vehicles/cularis.xml'
trim_args = {'h': 0, 'va': 10, 'gamma': 0, 'flaps': 0}
_fdm = p1_fw_dyn.DynamicModel(param_filename)
_fms = p3_guid.FMS(_fdm, trim_args)
_atm = p3_atm.AtmosphereCalm()
#_atm = p3_atm.AtmosphereThermalMulti()
sim = p3_u.Sim(_fdm, _fms, _atm)
t0, tf, dt = 0, 5., 0.01
time = np.arange(t0, tf, dt)
Xe, Ue = _fdm.trim(trim_args, report=True)
sim.reset(time[0], Xe, Ue)
sim.ctl.set_mode(p3_guid.FMS.mod_auto1)
for i in range(1, len(time)):
sim.run(time[i])
_fdm.plot_trajectory(time,
np.array(sim.Xs),
np.array(sim.Us),
window_title="None")
plt.show()
def __init__(self):
p3_rpu.PeriodicNode.__init__(self, 'ros_pat_publish_transform')
pos_ned = [0, 0, -1]
self.T_w2b = np.eye(4)
dm_ae = p1_fw_dyn.DynamicModel()
Xe, Ue = dm_ae.trim({
'h': 0,
'va': 8,
'gamma': 0
},
report=True,
debug=True)
va, alpha, beta = Xe[p3_fr.SixDOFAeroEuler.sv_slice_vaero]
#va, alpha, beta = 10, np.deg2rad(3), np.deg2rad(0)
phi, theta, psi = Xe[p3_fr.SixDOFAeroEuler.sv_slice_eul]
#phi, theta, psi = np.deg2rad(0), np.deg2rad(20), np.deg2rad(0)
p3_u.set_rot(self.T_w2b, p3_al.rmat_of_euler([phi, theta, psi]))
self.T_b2w = np.linalg.inv(self.T_w2b)
p3_u.set_trans(self.T_b2w, pos_ned)
self.T_a2b = np.eye(4)
p3_u.set_rot(self.T_a2b, p3_fr.R_aero_to_body(alpha, beta))
self.T_b2a = self.T_a2b #np.linalg.inv(self.T_a2b) # FIXME
self.tf_pub = p3_rpu.TransformPublisher()
self.marker_pub = p3_rpu.PoseArrayPublisher(dae='glider.dae')
def main(param_filename):
dm = p1_fw_dyn.DynamicModel(param_filename)
atm = p3_atm.AtmosphereThermal1()
#atm.set_params(xc=0, yc=10, zi=1500, wstar=300)
#find_best_radius(dm, atm, va=9., compute=True, plot_atm=False, plot_traj=True)
plot_gradient(atm)
plt.show()
def main():
param_filename = os.path.join(p3_u.pat_dir(), 'data/vehicles/cularis.xml')
dm = p1_fw_dyn.DynamicModel(param_filename)
pc = ScheduledPitchControl(dm)
run_sim(dm, pc)
#old_thing(dm)
plt.show()
def get_trim_defaults(trim_args={
'h': 0,
'va': 12,
'gamma': 0
},
ac_name='cularis'):
dm = p1_fw_dyn.DynamicModel(
os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
Xe, Ue = dm.trim(trim_args, debug=True)
return dm, Xe, Ue
def main(param_filename, trim_args = {'h':0, 'va':11, 'gamma':0}, force_recompute=False, exp=3):
dm = p1_fw_dyn.DynamicModel(param_filename)
if exp==0: test_line(dm, trim_args, force_recompute)
elif exp==1: test_circle(dm, trim_args, force_recompute)
elif exp==2: test_vctl(dm, trim_args, force_recompute)
elif exp==3: test_slope_soaring(dm, trim_args, force_recompute)
elif exp==4: test_dynamic_soaring(dm, trim_args, force_recompute)
elif exp==5: test_thermal_centering(dm, trim_args, force_recompute, tf=120.2, exp=0)
elif exp==6: test_thermal_centering_aroun(dm, trim_args, force_recompute, tf=170.2, exp=1)
elif exp==7: test_ardusoaring(dm, trim_args, force_recompute, tf=170.2, exp=0)
plt.show()
def plot_poles(trim_args={'h': 0, 'va': 12, 'gamma': 0}, ac_name='cularis'):
dm = p1_fw_dyn.DynamicModel(
os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
Xe, Ue = dm.trim(trim_args, debug=True)
A, B = dm.get_jacobian(Xe, Ue)
_eval, _evel = np.linalg.eig(A)
B1 = B[:, 2][np.newaxis].T
C = np.array([0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0])
D = np.zeros((1, 1))
ss = control.ss(A, B1, C, D)
tf = control.ss2tf(ss)
pdb.set_trace()
def __init__(self, name='ros_pat_glider_trim'):
p3_rpu.PeriodicNode.__init__(self, name)
self.joint_state_pub = p3_rpu.JointStatePublisher(what=name)
#self.ivel_pub = p3_rpu.PosePublisher(what=name)
self.txt_pub = p3_rpu.TextMarkerPublisher('/ds_glider/trim_txt', 'ds_glider/base_link', scale=0.1, what=name)
self.tf_pub = p3_rpu.TransformPublisher()
self.tf_pub.send_w_enu_to_ned_transform(rospy.Time.now())
ac_name = 'cularis'
self.dm = p1_fw_dyn.DynamicModel(os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
self.trim_args = {'h':0, 'va':10, 'throttle':0}
self._trim()
self.dyn_cfg_srv = dynamic_reconfigure.server.Server(ros_pat.cfg.glider_trimConfig, self.dyn_cfg_callback)
def test1():
atm = None #p3_atm.AtmosphereCstWind([0.5, 0., 0.])
dm_ee = p1_fw_dyn.DynamicModel_ee()
dm_ae = p1_fw_dyn.DynamicModel()
U = [0., 0.0, 0, 0, 0]
Xee = np.array([0., 0., 0., 10., 0., 0., 0., 0., 0., 0., 0., 0.])
print('Xee {}'.format(Xee))
Xee_dot = dm_ee.dyn(Xee, t=0, U=U, atm=atm)
print('Xee_dot {}'.format(Xee_dot))
Xae = p3_fr.SixDOFEuclidianEuler.to_six_dof_aero_euler(Xee, atm)
print('Xae {}'.format(Xae))
Xae_dot = dm_ae.dyn(Xae, t=0, U=U, atm=atm)
print('Xae_dot {}'.format(Xae_dot))
def get_defaults():
param_filename = p3_u.pat_ressource('data/vehicles/cularis.xml')
dm = p1_fw_dyn.DynamicModel(param_filename)
atm = p3_atm.AtmosphereWharington(center=[-40., 0, 0],
radius=40,
strength=-1.5)
sensors = p3_sens.Sensors(std_va=0.)
cc = (20., 15., 0.)
p0 = 5, 0, -200
trim_args = {'h': 0, 'va': 9, 'gamma': 0}
dt = 0.01
ctl = p3_guidsoar.GuidanceSoaring(dm, None, trim_args, dt)
ctl.Xe[0], ctl.Xe[1], ctl.Xe[2] = p0 # aircraft start point
ctl.set_circle_center(cc) # initial circle center
return dm, ctl, atm, sensors, dt, p0, cc, trim_args
def test01(trim_args={
'h': 0,
'va': 15,
'throttle': 0,
'flaps': np.deg2rad(-8.)
},
ac_name='cularis'):
dm = p1_fw_dyn.DynamicModel(
os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
#dm = p1_fw_dyn.DynamicModel_ee(os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
#dm = p1_fw_dyn.DynamicModel_eq(os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))
Xe, Ue = dm.trim(trim_args, debug=True, report=True, aero=False)
print Xe, Ue
Xe, Ue = dm.trim(trim_args, debug=True, report=True, aero=True)
print Xe, Ue
return dm, Xe, Ue
def __init__(self, _traj=None, time_factor=1.):
traj = p3_traj3d.CircleRefTraj(c=[0, 0, -20], r=15)
#traj = p3_fw_guid.LineRefTraj(p1=[0, 0, 0], p2=[50, 50, 0])
#traj = p3_fw_guid.SquareRefTraj()
#traj = p3_fw_guid.OctogonRefTraj()
self.time_factor = time_factor
param_filename = '/home/poine/work/pat/data/vehicles/cularis.xml'
_fdm = p1_fw_dyn.DynamicModel(param_filename)
_fms = p3_fms.FMS(_fdm, {'h': 0, 'va': 10, 'gamma': 0})
#_ctl = p3_fw_guid.Guidance(_fdm, traj, {'h':0, 'va':10, 'gamma':0})
#_ctl = p3_guid.GuidanceThermal(_fdm, traj, {'h':0, 'va':10, 'gamma':0})
#_atm = p3_atm.AtmosphereCstWind([0, 0, -1])
#_atm = p3_atm.AtmosphereThermal1()
_atm = p3_atm.AtmosphereThermalMulti()
#_atm = p3_atm.AtmosphereWharington()#center=None, radius=50, strength=-2, cst=[0, 0, 0])
#_atm = p3_atm.AtmosphereGedeon(center=[55, 0, 0], radius=50, strength=-2, cst=[0, 0, 0])
#_atm = p3_atm.AtmosphereRidge()
self.sim = pmu.Sim(_fdm, _fms, _atm)
self.tf_pub = p3_rpu.TransformPublisher()
self.carrot_pub = p3_rpu.MarkerPublisher('guidance/goal',
'w_ned',
scale=(0.25, 0.25, 0.5))
self.traj_pub = p3_rpu.TrajectoryPublisher2(traj, ms=0.2)
#self.pose_pub = p3_rpu.PoseArrayPublisher(dae='glider.dae')
self.pose_pub = p3_rpu.PoseArrayPublisher(dae='ds_glider_full.dae')
self.atm_pub = None #p3_rpu.AtmPointCloudPublisher(_atm)
self.status_pub = p3_rpu.GuidanceStatusPublisher()
self.atm_disp_dyn_rec_client = dynamic_reconfigure.client.Client(
"display_atmosphere",
timeout=1,
config_callback=self.atm_config_callback)
#self.my_own_reconfigure_client = dynamic_reconfigure.client.Client("real_time_sim", timeout=1, config_callback=None)
self.cur_thermal_id = 0
self.dyn_cfg_srv = dyn_rec_srv.Server(ros_pat.cfg.guidanceConfig,
self.dyn_cfg_callback)
# we can not have two servers... f**k...
#self.atm_cfg_srv = dyn_rec_srv.Server(ros_pat.cfg.atmosphereConfig,
# self.atm_dyn_cfg_callback)#, subname='foo')
self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal",
geometry_msgs.msg.PoseStamped,
self.nav_goal_callback)
self.joy_sub = rospy.Subscriber('/joy', sensor_msgs.msg.Joy,
self.joy_callback)
self.periodic_cnt = 0
def test0():
atm = p3_atm.AtmosphereCstWind([0., 0., 0.])
dm_ae = p1_fw_dyn.DynamicModel()
U = [0.0, 0.0, 0, 0, 0]
print('U {}'.format(U))
pos_ned = [0, 0, 0]
#vel_aero = [10, np.deg2rad(4), 0]
vel_aero = [10, 0., -0.6]
eulers = np.deg2rad([0, 0, 45])
#eulers = [1.77592771, 0.99355044, 3.13596614]
eulers = [1.23060692, 0.14490205, -2.96400653]
#eulers = [0, 0, 0]
rvel_body = [0, 0, 0]
rvel_body = [0.1, 0, 0]
Xae = np.concatenate((pos_ned, vel_aero, eulers, rvel_body))
print('Xae {}'.format(Xae))
Xae_dot = dm_ae.dyn_new(Xae, t=0, U=U, atm=atm)
print('Xae_dot {}'.format(Xae_dot))
Xae_dot_old = dm_ae.dyn_old(Xae, t=0, U=U, atm=atm)
print('Xae_dot_old {}'.format(Xae_dot_old))
def test2():
atm = p3_atm.AtmosphereCstWind([0., 0., 0.])
#atm = p3_atm.AtmosphereSinetWind([0., 0., -0.5])
#atm = p3_atm.AtmosphereSinedWind([0., 0., -0.5])
#dm = t1dyn.get_default_dm()
dm = p1_fw_dyn.DynamicModel()
#dm = p1_fw_dyn.DynamicModel_ee()
time = np.arange(0, 30, 0.01)
Xe, Ue = dm.trim({'h': 0, 'va': 10, 'gamma': 0}, report=True)
X0, X_act0 = np.array(Xe), np.array(Ue),
U = Ue * np.ones((len(time), dm.input_nb()))
time, X, X_act = t1dyn.run_simulation(dm, time, X0, X_act0, U, atm=atm)
#dm.plot_trajectory_as_ae(time, X, X_act, window_title='aero/euler', atm=atm)
dm.plot_trajectory_as_ee(time,
X,
X_act,
window_title='euclidian/euler',
atm=atm)
plt.show()
def identify_plant(_ac, v_trim=12., dt=0.005, epochs=100, force_retrain=False, force_remake_training_set=False ):
param_filename = p3_u.pat_ressource('data/vehicles/{}.xml'.format(_ac))
plant_ann_filename='/tmp/pat_pil_plant_ann_{}.h5'.format(_ac)
dm = p1_fw_dyn.DynamicModel(param_filename)
dm.dt = dt # FIXME... why is that harcoded?
trim_args = {'h':0, 'va':v_trim, 'gamma':0.}
Xe, Ue = dm.trim(trim_args, report=True)
if force_retrain or force_remake_training_set or not os.path.exists(plant_ann_filename):
_input, _output = make_or_load_plant_id_training_set(_ac, dm, Xe, Ue, force_recompute=force_remake_training_set)
# Build the plant identification ANN
plant_i = keras.layers.Input((3,), name ="plant_i") # x1_k, x2_k, u_k
plant_l = keras.layers.Dense(2, activation='linear', kernel_initializer='uniform',
input_shape=(3,), use_bias=False, name="plant")
plant_o = plant_l(plant_i)
plant_ann = keras.models.Model(inputs=plant_i, outputs=plant_o)
plant_ann.compile(loss='mean_squared_error', optimizer='adam')
plant_ann.fit(_input, _output, epochs=epochs, batch_size=32, verbose=1, shuffle=True)
plant_ann.save(plant_ann_filename)
else:
plant_ann = keras.models.load_model(plant_ann_filename)
return dm, Xe, Ue, plant_ann
def test_ee_vs_ae():
atm = None #p3_atm.AtmosphereCstWind([0.5, 0., 0.])
dm_ee = p1_fw_dyn.DynamicModel_ee()
dm_ae = p1_fw_dyn.DynamicModel()
if 0:
U = [0., 0.0, 0, 0, 0]
Xee = np.array([0., 0., 0., 10., 0., 0., 0., 0., 0., 0., 0., 0.])
print('Xee {}'.format(Xee))
Xee_dot = dm_ee.dyn(Xee, t=0, U=U, atm=atm)
print('Xee_dot {}'.format(Xee_dot))
Xae = p3_fr.SixDOFEuclidianEuler.to_six_dof_aero_euler(Xee, atm)
print('Xae {}'.format(Xae))
Xae_dot = dm_ae.dyn(Xae, t=0, U=U, atm=atm)
print('Xae_dot {}'.format(Xae_dot))
if 1:
#Xae_e, Uae_e = dm_ae.trim({'h':0, 'va':10, 'throttle':0}, debug=True)
Xae_e, Uae_e = dm_ae.trim({
'h': 0,
'va': 10,
'gamma': 0
},
report=True,
debug=True)
print('Xae_e {}'.format(Xae_e))
print('Uae_e {}'.format(Uae_e))
print
#Xee_e, Uee_e = dm_ee.trim({'h':0, 'va':10, 'throttle':0}, debug=True)
Xee_e, Uee_e = dm_ee.trim({
'h': 0,
'va': 10,
'gamma': 0
},
report=True,
debug=True)
print('Xee_e {}'.format(Xee_e))
print('Uee_e {}'.format(Uee_e))
time = np.arange(0, 10, 0.01)
X0, X_act0 = np.array(Xee_e), np.array(Uee_e),
U = Uee_e * np.ones((len(time), dm_ee.input_nb()))
time, Xee, Xee_act = t1dyn.run_simulation(dm_ee,
time,
X0,
X_act0,
U,
atm=atm)
dm_ee.plot_trajectory_as_ee(
time,
Xee,
Xee_act,
window_title='euclidian/euler model as euclidian/euler')
dm_ee.plot_trajectory_as_ae(
time,
Xee,
Xee_act,
window_title='euclidian/euler model as aero/euler')
X0, X_act0 = np.array(Xae_e), np.array(Uae_e),
U = Uae_e * np.ones((len(time), dm_ae.input_nb()))
time, Xae, Xae_act = t1dyn.run_simulation(dm_ae,
time,
X0,
X_act0,
U,
atm=atm)
dm_ae.plot_trajectory_as_ee(
time,
Xae,
Xae_act,
window_title='aero/euler model as euclian/euler')
dm_ae.plot_trajectory_as_ae(
time, Xae, Xae_act, window_title='aero/euler model as aero/euler')
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')
def get_default_dm(ac_name='cularis'): #ac_name='skywalker_x8'):
return p1_fw_dyn.DynamicModel(
os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name)))