def cont_dyn(self, X, t, U):
#Fb = np.array([0, 0, 0])
#Mb = np.array([0, 0, 0])
Ueng, Usfc = U[:self.P.eng_nb], U[
self.P.
eng_nb:] # engines and control surfaces parts of input vector
#LOG.info(" engine {} sfc {}".format(Ueng, Usfc))
euler = pal.euler_of_quat(X[self.sv_slice_quat])
omega = X[self.sv_slice_rvel]
R_b2w = pal.rmat_of_quat(X[pat_dyn.SolidFDM.sv_slice_quat])
#pdb.set_trace()
vi_e = X[self.sv_slice_vel]
w_e = [0, 0, 0]
va, alpha, beta = pfw.get_va_alpha_beta(
vi_e, w_e, euler) # fixme: make a fonction that takes vi_b
rho = patm.get_rho(-X[self.sv_z])
Pdyn = 0.5 * rho * va**2
Fb, f_eng_body = get_forces_body(rho, va, alpha, beta, euler, omega,
Pdyn, Ueng, Usfc, self.P)
Mb = get_moments_body(alpha, beta, euler, omega, Pdyn, f_eng_body,
Usfc, self.P)
print(Usfc, Mb)
# try:
# return ps.s1_dyn(X, t, Fb, Mb, P.m, P.I, P.invI, P.inv_mamat)
# except AttributeError:
# return ps.s1_dyn(X, t, Fb, Mb, P.m, P.I, P.invI, None)
return pat_dyn.SolidFDM.cont_dyn(self, X, t, np.concatenate((Fb, Mb)))
def dyn(self, X, t, U, atm):
Ueng = U[self.P.u_slice_eng()] # engines part of input vector
Usfc = U[self.P.u_slice_sfc()] # control surfaces part of input vector
X_pos = X[self.sv_slice_pos] #
X_vel = X[self.sv_slice_vel] #
X_quat = X[self.sv_slice_quat] # quaternion
X_rvel_body = X[self.sv_slice_rvel] # body rotational velocities
earth_to_body_R = p3_alg.rmat_of_quat(X_quat)
wind_ned = atm.get_wind_ned(X_pos, t) if atm is not None else [0, 0, 0]
va, alpha, beta = p3_fr.vel_world_to_aero_quat(X_vel, X_quat, wind_ned)
h = -X[self.sv_z]
rho = p3_atm.get_rho(h)
Pdyn = 0.5 * rho * va**2
f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta, X_rvel_body,
Usfc, self.P, Pdyn)
f_eng_body = p3_fw_aero.get_f_eng_body(h, va, Ueng, self.P)
f_weight_body = np.dot(earth_to_body_R, [0., 0., self.P.m * self.P.g])
forces_body = f_aero_body + np.sum(f_eng_body, axis=0) + f_weight_body
m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta, X_rvel_body,
Usfc, self.P, Pdyn)
m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P)
moments_body = m_aero_body + m_eng_body
_U = np.concatenate((forces_body, moments_body))
return p3_fr.SixDOFEuclidianQuat.cont_dyn(X, t, _U, self.P)
def dyn(self, X, t, U, wind_ned=[0, 0, 0]):
Ueng = U[self.P.u_slice_eng()] # engines part of input vector
Usfc = U[self.P.u_slice_sfc()] # control surfaces part of input vector
X_pos = X[self.sv_slice_pos] # ned pos
X_avel = va, alpha, beta = X[
self.sv_slice_vaero] # airvel, alpha, beta
X_euler = X[self.sv_slice_eul] # euler angles
X_rvel_body = X[self.sv_slice_rvel] # body rotational velocities
earth_to_body_R = p3_alg.rmat_of_euler(X_euler)
rho = p3_atm.get_rho(-X[self.sv_z])
Pdyn = 0.5 * rho * va**2
# Forces
f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta, X_rvel_body,
Usfc, self.P, Pdyn)
f_eng_body = p3_fw_aero.get_f_eng_body(-X[self.sv_z], va, Ueng, self.P)
f_weight_body = np.dot(earth_to_body_R, [0., 0., self.P.m * self.P.g])
forces_body = f_aero_body + np.sum(f_eng_body, axis=0) + f_weight_body
# Moments
m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta, X_rvel_body,
Usfc, self.P, Pdyn)
m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P)
m_body = m_aero_body + m_eng_body
return p3_fr.SixDOFAeroEuler.cont_dyn(
X, t, np.concatenate((forces_body, m_body)), self.P, wind_ned)
def fit_va_and_phi(atm, dm, compute=False):
savefile_name = '/home/poine/tmp/pat_glider_trims.npz'
h = 0.
if compute:
phis = np.deg2rad(np.arange(-45, 46, 1.))
vas = np.arange(6, 20, 1.)
vzs = np.zeros((len(vas), len(phis)))
for iphi, phi in enumerate(phis):
for iva, va in enumerate(vas):
Xe, Ue = dm.foo_trim_aero_banked_cst_throttle(h=h, va=va, throttle=0., flaps=0., phi=phi, debug=True)
vzs[iva, iphi] = Xe[p3_fr.SixDOFEuclidianEuler.sv_zd]
np.savez(savefile_name, phis=phis, vas=vas, vzs=vzs)
print('saved {}'.format(savefile_name))
else:
_data = np.load(savefile_name)
phis, vas, vzs = [_data[k] for k in ['phis', 'vas', 'vzs']]
# fit
rho = p3_atm.get_rho(h)
K = 2.*dm.P.m*dm.P.g/rho/dm.P.Sref
H,Y = [],[]
for iphi, phi in enumerate(phis):
for iva, va in enumerate(vas):
CL = K/va**2
#H = np.array([[va**3/K, K/va] for va in vas])
H.append([va/CL, va*CL/np.cos(phi)**2])
Y.append(vzs[iva, iphi])
H=np.array(H)
Y=np.array(Y)
CD0, B = np.dot(np.linalg.pinv(H), Y)
#pdb.set_trace()
print('K {} CD0 {} B {}'.format(K, CD0, B))
plt.subplot(1,2,1)
for iva, va in enumerate(vas):
plt.plot(np.rad2deg(phis), vzs[iva,:], label='va {}'.format(va))
p3_pu.decorate(plt.gca(), title='zd', xlab='phi (deg)', ylab='m/s', legend=True)
plt.subplot(1,2,2)
for iphi, phi in enumerate(phis):
plt.plot(vas, vzs[:,iphi], label='phi {}'.format(np.rad2deg(phi)))
p3_pu.decorate(plt.gca(), title='zd', xlab='va (m/s)', ylab='m/s', legend=True)
vzs2 = np.zeros((len(vas), len(phis)))
for iphi, phi in enumerate(phis):
for iva, va in enumerate(vas):
CL = K/va**2
vzs2[iva, iphi] = va*(CD0/CL+B*CL/np.cos(phi)**2)
fig = plt.figure()
ax = fig.gca(projection='3d')
_phis, _vas = np.meshgrid(phis, vas)
surf = ax.plot_surface(_vas, np.rad2deg(phis), vzs)
surf2 = ax.plot_surface(_vas, np.rad2deg(phis), vzs2)
ax.set_xlabel('va (m/s)')
ax.set_ylabel('phi (deg)')
ax.set_zlabel('vz (m/s)')
plt.show()
def trim(self,
args={},
UengFun=Ueng_of_throttle,
UsfcFun=Usfc_of_elevator,
debug=False):
#Xe = np.zeros(self.sv_size); Xe[self.sv_qi] = 1.; Xe[self.sv_xd] = 15.
#Ue = np.zeros(self.iv_size)
va, gamma, h = args.get('va',
self.P.Vref), args.get('gamma',
0.), args.get('h', 0.)
if debug:
print("searching for constant path trajectory with")
print(" h {:f} m".format(h))
print(" va {:f} m/s".format(va))
print(" gamma {:f} deg".format(np.rad2deg(gamma)))
LOG.info(" Running trim for va {} gamma {} h {}".format(va, gamma, h))
rho = patm.get_rho(h)
Pdyn, beta, omega = 0.5 * rho * va**2, 0, [0, 0, 0]
def err_func(args):
ele, throttle, alpha = args
Ueng, Usfc = UengFun(throttle, self.P), UsfcFun(ele, self.P)
eulers = [0, gamma + alpha, 0]
Fb, f_eng_body = get_forces_body(rho, va, alpha, beta, eulers,
omega, Pdyn, Ueng, Usfc, self.P)
f_weight_body = np.dot(pfw.earth_to_body(eulers),
[0., 0., self.P.m * self.P.g])
Fb = Fb + f_weight_body
Mb = get_moments_body(alpha, beta, eulers, omega, Pdyn, f_eng_body,
Usfc, self.P)
return [Fb[0], Fb[2], Mb[1]]
sol = scipy.optimize.root(err_func, [0., 0.5, 0.], method='hybr')
ele, throttle, alpha = sol.x
#print sol
LOG.info(
" elevator {:.1f} deg throttle {:.1f} % alpha {:.1f} deg".format(
np.rad2deg(ele), 100 * throttle, np.rad2deg(alpha)))
if debug:
print("""result:
throttle : {:f} %
elevator : {:f} deg
angle of attack : {:f} deg""".format(100. * throttle, np.rad2deg(ele),
np.rad2deg(alpha)))
#Xe = [0, 0, -h, va*math.cos(alpha), 0, va*math.sin(alpha), 0, gamma+alpha, 0, 0, 0, 0]
q = pal.quat_of_euler([0, gamma + alpha, 0])
Xe = [
0, 0, -h, va * math.cos(gamma), 0, va * math.sin(gamma), q[0],
q[1], q[2], q[3], 0, 0, 0
]
Ue = UengFun(throttle, self.P) + UsfcFun(ele, self.P)
return Xe, Ue
def trim_aero_cst_throttle(self,
h=0.,
va=12.,
throttle=0.,
flaps=0.,
report=True,
debug=False,
atm=None):
if report:
print("searching (aero) for constant throttle trajectory with")
print(" h {:.1f} m".format(h))
print(" va {:.2f} m/s".format(va))
print(" throttle {:.1f} %".format(100 * throttle))
print(" flap {:.1f} deg".format(np.rad2deg(flaps)))
beta = 0.
rho = p3_atm.get_rho(h)
Pdyn = 0.5 * rho * va**2
X_rvel_body = [0, 0, 0]
def err_func(args):
gamma, elevator, alpha = args
Ueng, Usfc = [throttle], [0, elevator, 0, flaps]
f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta,
X_rvel_body, Usfc, self.P,
Pdyn)
f_eng_body = p3_fw_aero.get_f_eng_body(h, va, Ueng, self.P)
eul = phi, theta, psi = [0, gamma + alpha, 0]
earth_to_body_R = p3_alg.rmat_of_euler(eul)
f_weight_body = np.dot(earth_to_body_R,
[0., 0., self.P.m * self.P.g])
forces_body = f_aero_body + np.sum(f_eng_body,
axis=0) + f_weight_body
m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta,
X_rvel_body, Usfc, self.P,
Pdyn)
m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P)
m_body = m_aero_body + m_eng_body
return np.linalg.norm(np.concatenate((forces_body, m_body)))
p0 = [0., np.deg2rad(-2.), np.deg2rad(1.)]
gamma_e, ele_e, alpha_e = scipy.optimize.fmin_powell(err_func,
p0,
disp=debug,
ftol=1e-9)
if report:
print("""result:
gamma : {:.2f} deg
elevator : {:.2f} deg
angle of attack : {:.2f} deg""".format(np.rad2deg(gamma_e),
np.rad2deg(ele_e),
np.rad2deg(alpha_e)))
return gamma_e, ele_e, alpha_e
def get_f_eng_body(h, va, U, P):
"""
return propulsion forces expressed in body frame
"""
rho = p3_atm.get_rho(h)
thrusts = [
thrust_of_throttle(U[i], P.fmaxs[i], rho, P.rhois[i], P.nrhos[i], va,
P.Vis[i], P.nVs[i]) for i in range(P.eng_nb)
]
f_engines_body = np.array([
np.dot(P.eng_to_body[i], np.array([thrusts[i], 0., 0.]))
for i in range(P.eng_nb)
])
return f_engines_body
def cont_dyn(self, X, t, U):
Ueng, Usfc = U[:self.P.eng_nb], U[
self.P.
eng_nb:] # engines and control surfaces parts of input vector
rho = patm.get_rho(-X[self.sv_z])
Pdyn = 0.5 * rho * X[self.sv_v]**2
(va, alpha, beta), euler, omega = X[self.sv_slice_vaero], X[
self.sv_slice_eul], X[self.sv_slice_rvel]
Fb, f_eng_body = get_forces_body(rho, va, alpha, beta, euler, omega,
Pdyn, Ueng, Usfc, self.P)
Mb = get_moments_body(alpha, beta, euler, omega, Pdyn, f_eng_body,
Usfc, self.P)
#pdb.set_trace()
#print Usfc, Mb
return pat_dyn.SolidDM3.cont_dyn(self, X, t, np.concatenate((Fb, Mb)))
def dyn(self, X, t, U, wind_ned=[0, 0, 0]):
Ueng = U[self.P.u_slice_eng()] # engines part of input vector
Usfc = U[self.P.u_slice_sfc()] # control surfaces part of input vector
X_pos = X[self.sv_slice_pos] #
X_vel = X[self.sv_slice_vel] #
X_euler = X[self.sv_slice_eul] # euler angles
X_rvel_body = X[self.sv_slice_rvel] # body rotational velocities
earth_to_body_R = p3_alg.rmat_of_euler(X_euler)
va, alpha, beta = p3_fr.vel_world_to_aero_eul(X_vel, X_euler, wind_ned)
h = -X[self.sv_z]
rho = p3_atm.get_rho(h)
Pdyn = 0.5 * rho * va**2
f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta, X_rvel_body,
Usfc, self.P, Pdyn)
f_eng_body = p3_fw_aero.get_f_eng_body(h, va, Ueng, self.P)
f_weight_body = np.dot(earth_to_body_R, [0., 0., self.P.m * self.P.g])
forces_body = f_aero_body + np.sum(f_eng_body, axis=0) + f_weight_body
m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta, X_rvel_body,
Usfc, self.P, Pdyn)
m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P)
moments_body = m_aero_body + m_eng_body
if 1:
_U = np.concatenate((forces_body, moments_body))
return p3_fr.SixDOFEuclidianEuler.cont_dyn(X, t, _U, self.P)
else:
Xdot = np.zeros(self.sv_size)
# Position kinematics
Xdot[self.sv_slice_pos] = X[self.sv_slice_vel]
# Newton for forces in world frame
forces_world = np.dot(earth_to_body_R.T, forces_body)
Xdot[self.sv_slice_vel] = 1. / self.P.m * forces_world
# Orientation kinematics
Xdot[self.sv_slice_eul] = p3_alg.euler_derivatives(
X_euler, X_rvel_body)
# Newton for moments in body frame
raccel_body = np.dot(
self.P.invI, moments_body -
np.cross(X_rvel_body, np.dot(self.P.I, X_rvel_body)))
Xdot[self.sv_slice_rvel] = raccel_body
return Xdot
def test0():
h = 0
aero_vel = va, alpha, beta = [10, 0, 0]
euler, rvel = [0., 0, 0], [0.1, 0, 0]
Usfc = [0, 0, 0, 0]
Ueng = [0.1]
p_filename = os.path.join(p3_u.pat_dir(), 'data/vehicles/cularis.xml')
P = p3_fwparam.Param(p_filename)
rho = p3_atm.get_rho(h)
Pdyn = 0.5*rho*va**2
X = np.concatenate(([0, 0, -h], aero_vel, euler, rvel))
print('f_eng new: {}'.format(p3_fw_aero.get_f_eng_body(h, va, Ueng, P)))
print('f_eng old: {}'.format(p1_fw_dyn.get_f_eng_body(X, Ueng, P)))
print('new: {}'.format(p1_fw_dyn.get_m_aero_body2(va, alpha, beta, rvel, Usfc, P, Pdyn)))
print('old {}'.format(p1_fw_dyn.get_m_aero_body(X, Usfc, P, Pdyn)))
def trim(self,
args={},
UengFun=Ueng_of_throttle,
UsfcFun=Usfc_of_elevator,
debug=False):
va, gamma, h = args.get('va',
self.P.Vref), args.get('gamma',
0), args.get('h', 0)
if debug:
print("searching for constant path trajectory with")
print(" h {:f} m".format(h))
print(" va {:f} m/s".format(va))
print(" gamma {:f} deg".format(np.rad2deg(gamma)))
rho = patm.get_rho(h)
Pdyn, beta, omega = 0.5 * rho * va**2, 0, [0, 0, 0]
def err_func(args):
ele, throttle, alpha = args
Ueng, Usfc = UengFun(throttle, self.P), UsfcFun(ele, self.P)
eulers = [0, gamma + alpha, 0]
Fb, f_eng_body = get_forces_body(rho, va, alpha, beta, eulers,
omega, Pdyn, Ueng, Usfc, self.P)
f_weight_body = np.dot(pfw.earth_to_body(eulers),
[0., 0., self.P.m * self.P.g])
Fb = Fb + f_weight_body
Mb = get_moments_body(alpha, beta, eulers, omega, Pdyn, f_eng_body,
Usfc, self.P)
return [Fb[0], Fb[2], Mb[1]]
sol = scipy.optimize.root(err_func, [0., 0.5, 0.], method='hybr')
ele, throttle, alpha = sol.x
if debug:
print("""result:
throttle : {:f} %
elevator : {:f} deg
angle of attack : {:f} deg""".format(100. * throttle, np.rad2deg(ele),
np.rad2deg(alpha)))
Ue = np.zeros(self.P.input_nb)
Ue[:self.P.eng_nb] = UengFun(throttle, self.P)
Ue[self.P.eng_nb:self.P.input_nb] = UsfcFun(ele, self.P)
Xe = [0., 0., -h, va, alpha, beta, 0., gamma + alpha, 0., 0., 0., 0.]
return Xe, Ue
def fit_va(atm, dm, h=0):
vas, vzs = np.arange(6, 20, 1.), []
# compute simulator trims
for va in vas:
Xae_e, Uae_e = dm.trim({'h':h, 'va':va, 'throttle':0}, report=False, debug=False)
vzs.append(-Xae_e[p3_fr.SixDOFEuclidianEuler.sv_zd])
# fit polynomial
rho = p3_atm.get_rho(h)
K = 2.*dm.P.m*dm.P.g/rho/dm.P.Sref
H = np.array([[va**3/K, K/va] for va in vas])
CD0, B = np.dot(np.linalg.pinv(H), vzs)
print(f'K {K} CD0 {CD0} B {B}')
# display simulator points and polynomial model
vas2 = np.arange(6, 20, 0.1)
vzs2 = [va**3/K*CD0+K/va*B for va in vas2]
plt.plot(vas, vzs, '*')
plt.plot(vas2, vzs2)
p3_pu.decorate(plt.gca(), xlab='va (m/s)', ylab='vz (m/s)')
plt.show()
def trim_aero_banked_cst_throttle(self,
h=0.,
va=12.,
throttle=0.,
flaps=0.,
phi=0.,
report=True,
debug=False,
atm=None):
beta = 0.
rho = p3_atm.get_rho(h)
Pdyn = 0.5 * rho * va**2
psi, psid = 0., self.P.g * np.tan(phi) / va
def err_func(args):
theta, elevator, aileron, rudder, alpha = args
Ueng, Usfc = [throttle], [aileron, elevator, rudder]
if self.P.sfc_nb >= 4:
Usfc.append(flaps)
X_rvel_body = p, q, r = 0., np.sin(phi) * np.cos(
theta) * psid, np.cos(phi) * np.cos(theta) * psid
#X_rvel_body = p, q, r = 0, np.sin(phi)*np.cos(theta)*va/R, np.cos(phi)*np.cos(theta)*va/R
wind_ned = [0, 0, 0]
earth_to_body_R = p3_alg.rmat_of_euler([phi, theta, psi])
ivel_b = p3_fr.vel_aero_to_body_R([va, alpha, beta],
earth_to_body_R, wind_ned)
f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta,
X_rvel_body, Usfc, self.P,
Pdyn)
f_eng_body = p3_fw_aero.get_f_eng_body(h, va, Ueng, self.P)
#f_body = f_aero_body + np.sum(f_eng_body, axis=0)
#f_ned = np.dot(earth_to_body_R.T, f_body).squeeze()
#f_ned[2] += self.P.m*self.P.g
#f_ned[1] -= self.P.m*va*psid
f_weight_body = np.dot(earth_to_body_R,
[0., 0., self.P.m * self.P.g])
f_body = f_aero_body + np.sum(f_eng_body, axis=0) + f_weight_body
uvwd = 1 / self.P.m * f_body - np.cross(X_rvel_body, ivel_b)
m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta,
X_rvel_body, Usfc, self.P,
Pdyn)
m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P)
m_body = m_aero_body + m_eng_body
#pdb.set_trace()
#pqrd = np.dot(self.P.invI, m_body - np.cross(X_rvel_body, np.dot(self.P.I, X_rvel_body)))
pqrd = m_body - np.cross(X_rvel_body, np.dot(
self.P.I, X_rvel_body))
foo = np.concatenate((uvwd, pqrd))
#return np.linalg.norm(np.concatenate((f_ned, m_body)))
#print foo
return np.linalg.norm(foo) #+100.*abs(aileron)
p0 = [0., np.deg2rad(-2.), np.deg2rad(-1.), np.deg2rad(2.), 0.]
theta, elevator, aileron, rudder, alpha = scipy.optimize.fmin_powell(
err_func, p0, disp=debug, ftol=1e-12)
gamma = theta - alpha
if report:
print("""result:
gamma : {:.2f} deg
theta : {:.2f} deg
elevator : {:.2f} deg
aileron : {:.2f} deg
rudder : {:.2f} deg
angle of attack : {:.2f} deg""".format(np.rad2deg(gamma), np.rad2deg(theta),
np.rad2deg(elevator),
np.rad2deg(aileron),
np.rad2deg(rudder),
np.rad2deg(alpha)))
return gamma, elevator, aileron, rudder, alpha
