def test_rotation_G_to_A(self):
"""
Tests the rotation of a vector in G frame to a vector in A frame by a roll, pitch and yaw considering that
SHARPy employs an SEU frame.
In the inertial frame, the ``x_g`` axis is aligned with the longitudinal axis of the aircraft pointing backwards,
``z_g`` points upwards and ``y_g`` completes the set
Returns:
"""
aircraft_nose = np.array([-1, 0, 0])
z_A = np.array([0, 0, 1])
euler = np.array([90, 90, 90]) * np.pi / 180
quat = algebra.euler2quat(euler)
aircraft_nose_rotated = np.array([0, 0,
1]) # in G frame pointing upwards
z_A_rotated_g = np.array([1, 0, 0])
Cga = algebra.euler2rot(euler)
Cga_quat = algebra.quat2rotation(quat)
np.testing.assert_array_almost_equal(
Cga.dot(aircraft_nose),
aircraft_nose_rotated,
err_msg='Rotation using Euler angles not performed properly')
np.testing.assert_array_almost_equal(
Cga_quat.dot(aircraft_nose),
aircraft_nose_rotated,
err_msg='Rotation using quaternions not performed properly')
np.testing.assert_array_almost_equal(
Cga.dot(z_A),
z_A_rotated_g,
err_msg='Rotation using Euler angles not performed properly')
np.testing.assert_array_almost_equal(
Cga_quat.dot(z_A),
z_A_rotated_g,
err_msg='Rotation using quaternions not performed properly')
# Check projections
Pag = Cga.T
Pag_quat = Cga_quat.T
np.testing.assert_array_almost_equal(
Pag.dot(z_A_rotated_g),
z_A,
err_msg='Error in projection from A to G using Euler angles')
np.testing.assert_array_almost_equal(
Pag.dot(aircraft_nose_rotated),
aircraft_nose,
err_msg='Error in projection from A to G using Euler angles')
np.testing.assert_array_almost_equal(
Pag_quat.dot(z_A_rotated_g),
z_A,
err_msg='Error in projection from A to G using quaternions')
np.testing.assert_array_almost_equal(
Pag_quat.dot(aircraft_nose_rotated),
aircraft_nose,
err_msg='Error in projection from A to G using quaternions')
def update_step(self):
self.data.aero.generate_zeta(self.data.structure,
self.data.aero.aero_settings,
self.data.ts)
# for i_surf in range(self.data.aero.timestep_info[self.ts].n_surf):
# self.data.aero.timestep_info[self.ts].forces[i_surf].fill(0.0)
# self.data.aero.timestep_info[self.ts].dynamic_forces[i_surf].fill(0.0)
euler = np.array([
self.settings['roll'].value, self.settings['alpha'].value,
self.settings['beta'].value
])
euler_rot = algebra.euler2rot(euler) # this is Cag
quat = algebra.mat2quat(euler_rot.T)
self.data.structure.update_orientation(
quat, self.data.ts) # quat corresponding to Cga
self.data.aero.update_orientation(
quat, self.data.ts) # quat corresponding to Cga
def __init__(
self,
M, # chordwise panelling
Nv, # panelling VTP
Nh, # panelling HTP (total, not half)
Mstar_fact,
u_inf, # flight cond
rho,
# size
chord_htp_root,
chord_htp_tip,
chord_vtp_root,
span_htp,
height_vtp,
# elastic axis
main_ea, # from LE.
# angles
alpha_htp_deg=0.0, # positive if upward
sweep_htp_deg=0.0, # positive if backward
sweep_vtp_deg=0.0, # positive if backward
# others
alpha_inf_deg=0.0,
beta_inf_deg=0.0,
rotate_frame_A=True,
route='.', # saving
case_name='Ttail'):
### parametrisation
assert Nv%2 != 1,\
'vertical panelling of VTP must be divisible by 2 (using 3-noded FEs!)'
assert Nh%4 != 1,\
'spanwise panelling of full HTP must be divisible by 4 (using 3-noded FEs!)'
# chordwise
self.M = M
# vtp
self.Nv = Nv
# htp
self.Nh = Nh
self.Nh_semi = Nh // 2
# wake
self.Mstar_fact = Mstar_fact # wake chord-wise panel factor
# beam elements
self.n_surfaces = 3
self.num_nodes_elem = 3
self.num_elem_vtp = self.Nv // 2
self.num_elem_htpL = self.Nh_semi // 2 # port
self.num_elem_htpR = self.Nh_semi // 2 # starboard
self.num_elem_tot = self.num_elem_vtp + self.num_elem_htpL + self.num_elem_htpR
self.num_nodes_vtp = 2 * self.num_elem_vtp + 1
self.num_nodes_htpL = 2 * self.num_elem_htpL + 1
self.num_nodes_htpR = 2 * self.num_elem_htpR + 1
self.num_nodes_tot = 2 * self.num_elem_vtp + 2 * self.num_elem_htpL + 2 * self.num_elem_htpR + 1
### store input
self.u_inf = u_inf # flight cond
self.rho = rho
self.chord_htp_root = chord_htp_root
self.chord_htp_tip = chord_htp_tip
self.chord_vtp_root = chord_vtp_root
self.span_htp = span_htp
self.height_vtp = height_vtp
self.main_ea = main_ea
self.alpha_htp_deg = alpha_htp_deg
self.sweep_htp_deg = sweep_htp_deg
self.sweep_vtp_deg = sweep_vtp_deg
# FoR A orientation
self.alpha_inf_deg = alpha_inf_deg
self.beta_inf_deg = beta_inf_deg
self.rotate_frame_A = rotate_frame_A
if self.rotate_frame_A:
self.quat = algebra.euler2quat(
np.pi / 180. * np.array([0.0, alpha_inf_deg, beta_inf_deg]))
self.u_inf_direction = np.array([1., 0., 0.])
else:
self.quat = algebra.euler2quat(np.array([0., 0., 0.]))
self.u_inf_direction = np.dot(
algebra.euler2rot(
np.pi / 180. *
np.array([0.0, alpha_inf_deg, beta_inf_deg])),
np.array([1., 0., 0.]))
# time-step
self.dt = self.chord_htp_root / self.M / self.u_inf
self.route = route + '/'
self.case_name = case_name
def derivatives(self, Y_freq):
Cng = np.array([[-1, 0, 0], [0, 1, 0],
[0, 0,
-1]]) # Project SEU on NED - TODO implementation
u_inf = self.settings['u_inf'].value
s_ref = self.settings['S_ref'].value
b_ref = self.settings['b_ref'].value
c_ref = self.settings['c_ref'].value
rho = self.data.linear.tsaero0.rho
# Inertial frame
try:
euler = self.data.linear.tsstruct0.euler
Pga = algebra.euler2rot(euler)
rig_dof = 9
except AttributeError:
quat = self.data.linear.tsstruct0.quat
Pga = algebra.quat2rotation(quat)
rig_dof = 10
derivatives_g = np.zeros((6, Y_freq.shape[1] + 2))
coefficients = {
'force':
0.5 * rho * u_inf**2 * s_ref,
'moment_lon':
0.5 * rho * u_inf**2 * s_ref * c_ref,
'moment_lat':
0.5 * rho * u_inf**2 * s_ref * b_ref,
'force_angular_vel':
0.5 * rho * u_inf**2 * s_ref * c_ref / u_inf,
'moment_lon_angular_vel':
0.5 * rho * u_inf**2 * s_ref * c_ref * c_ref / u_inf
} # missing rates
for in_channel in range(Y_freq.shape[1]):
derivatives_g[:3, in_channel] = Pga.dot(Y_freq[:3, in_channel])
derivatives_g[3:, in_channel] = Pga.dot(Y_freq[3:, in_channel])
derivatives_g[:3, :3] /= coefficients['force']
derivatives_g[:3, 3:6] /= coefficients['force_angular_vel']
derivatives_g[4, :3] /= coefficients['moment_lon']
derivatives_g[4, 3:6] /= coefficients['moment_lon_angular_vel']
derivatives_g[[3, 5], :] /= coefficients['moment_lat']
derivatives_g[:, -2] = derivatives_g[:, 2] * u_inf # ders wrt alpha
derivatives_g[:, -1] = derivatives_g[:, 1] * u_inf # ders wrt beta
der_matrix = np.zeros((6, self.inputs - (rig_dof - 6)))
der_col = 0
for i in list(range(6)) + list(range(rig_dof, self.inputs)):
der_matrix[:3, der_col] = Y_freq[:3, i]
der_matrix[3:6, der_col] = Y_freq[3:6, i]
der_col += 1
labels_force = {0: 'X', 1: 'Y', 2: 'Z', 3: 'L', 4: 'M', 5: 'N'}
labels_velocity = {
0: 'u',
1: 'v',
2: 'w',
3: 'p',
4: 'q',
5: 'r',
6: 'flap1',
7: 'flap2',
8: 'flap3'
}
table = cout.TablePrinter(
n_fields=7,
field_length=12,
field_types=['s', 'f', 'f', 'f', 'f', 'f', 'f'])
table.print_header(['der'] + list(labels_force.values()))
for i in range(der_matrix.shape[1]):
table.print_line([labels_velocity[i]] + list(der_matrix[:, i]))
table_coeff = cout.TablePrinter(n_fields=7,
field_length=12,
field_types=['s'] + 6 * ['f'])
labels_out = {
0: 'C_D',
1: 'C_Y',
2: 'C_L',
3: 'C_l',
4: 'C_m',
5: 'C_n'
}
labels_der = {
0: 'u',
1: 'v',
2: 'w',
3: 'p',
4: 'q',
5: 'r',
6: 'alpha',
7: 'beta'
}
table_coeff.print_header(['der'] + list(labels_out.values()))
for i in range(6):
table_coeff.print_line([labels_der[i]] + list(derivatives_g[:, i]))
table_coeff.print_line([labels_der[6]] + list(derivatives_g[:, -2]))
table_coeff.print_line([labels_der[7]] + list(derivatives_g[:, -1]))
return der_matrix, derivatives_g
def test_rotation_matrices_derivatives(self):
"""
Checks derivatives of rotation matrix derivatives with respect to
quaternions and Cartesian rotation vectors
Note: test only includes CRV <-> quaternions conversions
"""
### linearisation point
# fi0=np.pi/6
# nv0=np.array([1,3,1])
fi0 = 2.0 * np.pi * random.random() - np.pi
nv0 = np.array([random.random(), random.random(), random.random()])
nv0 = nv0 / np.linalg.norm(nv0)
fv0 = fi0 * nv0
qv0 = algebra.crv2quat(fv0)
ev0 = algebra.quat2euler(qv0)
# direction of perturbation
# fi1=np.pi/3
# nv1=np.array([-2,4,1])
fi1 = 2.0 * np.pi * random.random() - np.pi
nv1 = np.array([random.random(), random.random(), random.random()])
nv1 = nv1 / np.linalg.norm(nv1)
fv1 = fi1 * nv1
qv1 = algebra.crv2quat(fv1)
ev1 = algebra.quat2euler(qv1)
# linearsation point
Cga0 = algebra.quat2rotation(qv0)
Cag0 = Cga0.T
Cab0 = algebra.crv2rotation(fv0)
Cba0 = Cab0.T
Cga0_euler = algebra.euler2rot(ev0)
Cag0_euler = Cga0_euler.T
# derivatives
# xv=np.ones((3,)) # dummy vector
xv = np.array([random.random(),
random.random(),
random.random()]) # dummy vector
derCga = algebra.der_Cquat_by_v(qv0, xv)
derCag = algebra.der_CquatT_by_v(qv0, xv)
derCab = algebra.der_Ccrv_by_v(fv0, xv)
derCba = algebra.der_CcrvT_by_v(fv0, xv)
derCga_euler = algebra.der_Ceuler_by_v(ev0, xv)
derCag_euler = algebra.der_Peuler_by_v(ev0, xv)
A = np.array([1e-1, 1e-2, 1e-3, 1e-4, 1e-5, 1e-6])
er_ag = 10.
er_ga = 10.
er_ab = 10.
er_ba = 10.
er_ag_euler = 10.
er_ga_euler = 10.
for a in A:
# perturbed
qv = a * qv1 + (1. - a) * qv0
fv = a * fv1 + (1. - a) * fv0
ev = a * ev1 + (1. - a) * ev0
dqv = qv - qv0
dfv = fv - fv0
dev = ev - ev0
Cga = algebra.quat2rotation(qv)
Cag = Cga.T
Cab = algebra.crv2rotation(fv)
Cba = Cab.T
Cga_euler = algebra.euler2rot(ev)
Cag_euler = Cga_euler.T
dCag_num = np.dot(Cag - Cag0, xv)
dCga_num = np.dot(Cga - Cga0, xv)
dCag_an = np.dot(derCag, dqv)
dCga_an = np.dot(derCga, dqv)
er_ag_new = np.max(np.abs(dCag_num - dCag_an))
er_ga_new = np.max(np.abs(dCga_num - dCga_an))
dCab_num = np.dot(Cab - Cab0, xv)
dCba_num = np.dot(Cba - Cba0, xv)
dCab_an = np.dot(derCab, dfv)
dCba_an = np.dot(derCba, dfv)
er_ab_new = np.max(np.abs(dCab_num - dCab_an))
er_ba_new = np.max(np.abs(dCba_num - dCba_an))
dCag_num_euler = np.dot(Cag_euler - Cag0_euler, xv)
dCga_num_euler = np.dot(Cga_euler - Cga0_euler, xv)
dCag_an_euler = np.dot(derCag_euler, dev)
dCga_an_euler = np.dot(derCga_euler, dev)
er_ag_euler_new = np.max(np.abs(dCag_num_euler - dCag_an_euler))
er_ga_euler_new = np.max(np.abs(dCga_num_euler - dCga_an_euler))
assert er_ga_new < er_ga, 'der_Cquat_by_v error not converging to 0'
assert er_ag_new < er_ag, 'der_CquatT_by_v error not converging to 0'
assert er_ab_new < er_ab, 'der_Ccrv_by_v error not converging to 0'
assert er_ba_new < er_ba, 'der_CcrvT_by_v error not converging to 0'
assert er_ga_euler_new < er_ga_euler, 'der_Ceuler_by_v error not converging to 0'
assert er_ag_euler_new < er_ag_euler, 'der_Peuler_by_v error not converging to 0'
er_ag = er_ag_new
er_ga = er_ga_new
er_ab = er_ab_new
er_ba = er_ba_new
er_ag_euler = er_ag_euler_new
er_ga_euler = er_ga_euler_new
assert er_ga < A[-2], 'der_Cquat_by_v error too large'
assert er_ag < A[-2], 'der_CquatT_by_v error too large'
assert er_ab < A[-2], 'der_Ccrv_by_v error too large'
assert er_ba < A[-2], 'der_CcrvT_by_v error too large'
assert er_ag_euler < A[-2], 'der_Peuler_by_v error too large'
assert er_ga_euler < A[-2], 'der_Ceuler_by_v error too large'