def GetTimeSeries(self, params, sim, control):
time, tether_pitch = self._SelectTelemetry(sim, control,
['time', 'tether_pitch'])
if (not scoring_util.IsSelectionValid(time)
or not scoring_util.IsSelectionValid(tether_pitch)):
return {
'min_sustained_tether_pitch': float('nan'),
'max_sustained_tether_pitch': float('nan')
}
tether_pitch_deg = np.rad2deg(tether_pitch)
# Low pass, symmetrically (2nd order) filter the tether pitch.
cut_off_freq = 2.0
tether_pitch_deg_f = scoring_util.LpFiltFiltTimeSeries(
time, tether_pitch_deg, cut_off_freq)
min_sustained_tether_pitch, max_sustained_tether_pitch = (
scoring_util.GetSustainedValue(tether_pitch_deg_f,
self._good_lower_limit,
self._good_upper_limit,
self._sustained_duration,
scoring_util.GetTimeSamp(time)))
return {
'min_sustained_tether_pitch': min_sustained_tether_pitch,
'max_sustained_tether_pitch': max_sustained_tether_pitch
}
def GetTimeSeries(self, params, sim, control):
time, tether_elevation, tether_elevation_valid = self._SelectTelemetry(
sim, control, ['time', 'tether_elevation', 'tether_elevation_valid'])
if not (scoring_util.IsSelectionValid(tether_elevation) or
scoring_util.IsSelectionValid(time)):
return {'tether_elevation_std': np.array([float('nan')])}
tether_elevation_deg = np.rad2deg(
tether_elevation[tether_elevation_valid == 1])
# Low pass, symmetrically (2nd order) filter the tether elevation.
tether_elevation_deg_f = scoring_util.LpFiltFiltTimeSeries(
time, tether_elevation_deg, self._cut_off_freq)
# Calculate a rolling StDev of the difference between the raw signal and
# the filtered signal.
t_samp = scoring_util.GetTimeSamp(time)
if np.isnan(t_samp):
return {'tether_elevation_std': np.array([float('nan')])}
n_window = int(self._t_window / t_samp)
elevation_deviation_df = pd.DataFrame(
tether_elevation_deg - tether_elevation_deg_f)
tether_elevation_std = elevation_deviation_df.rolling(
n_window, min_periods=n_window).std().values.flatten()
return {'tether_elevation_std': tether_elevation_std}
def GetTimeSeries(self, params, sim, control):
# Tether sphere radius at mean tether tension.
# - The 0.75 factor applied to the tether tensile stiffness is used to
# account for catenary effects.
# - The extra 0.16 meter offset is used to account for the distance from
# the bridle pivot to the origin of the body frame.
tether_params = params['system_params']['tether']
wing_params = params['system_params']['wing']
mean_tether_sphere_radius = (
tether_params['length'] + self._mean_tether_tension /
(0.75 * tether_params['tensile_stiffness'] /
tether_params['length']) + wing_params['bridle_rad'] + 0.16)
wing_xg, tether_xg_start = self._SelectTelemetry(
sim, control, ['wing_xg', 'tether_xg_start'])
if (scoring_util.IsSelectionValid(wing_xg)
and scoring_util.IsSelectionValid(tether_xg_start)):
tether_norm = np.linalg.norm(
numpy_utils.Vec3ToArray(wing_xg) -
numpy_utils.Vec3ToArray(tether_xg_start),
axis=1)
deviation = tether_norm - mean_tether_sphere_radius
else:
deviation = np.array([], dtype=float)
return {'tether_sphere_deviation': deviation}
def GetTimeSeries(self, params, sim, control):
# Exclude Perched, PilotHover, HoverAscend and HoverDescend flight modes.
flight_mode_exclusion_list = [
'kFlightModePilotHover', 'kFlightModePerched',
'kFlightModeHoverAscend', 'kFlightModeHoverDescend'
]
flight_modes = []
for name in _FLIGHT_MODE_HELPER.Names():
if name not in flight_mode_exclusion_list:
flight_modes.append(name)
tension, wing_acc, omega_i, domega_i, time_i = (self._SelectTelemetry(
sim,
control, [
'tether_tension', 'wing_acc', 'body_rates', 'angular_acc',
'time'
],
flight_modes=flight_modes))
# Check if body rates data exist. These may not exist if the relevant
# flight modes do not exist.
if not (scoring_util.IsSelectionValid(omega_i)
and scoring_util.IsSelectionValid(wing_acc)):
return {'wing_bending_failure_indices': np.array(float('nan'))}
# Create angular acceleration data for flight logs.
if scoring_util.IsSelectionValid(domega_i):
domega = domega_i
else:
domega = {
'x': np.gradient(omega_i['x'], time_i),
'y': np.gradient(omega_i['y'], time_i),
'z': np.gradient(omega_i['z'], time_i)
}
wing_acc_z = wing_acc['z']
ang_acc_x = domega['x']
kite_mass = params['system_params']['wing']['m']
# Scoring function is a linear interaction equation of aerodynamic lift
# loads (adjusted for roll acceleration contribution) and inertial loads
# relative to their respective limits. It returns a failure index (ratio
# of interaction equation output to max allowable value, a value of 1.0
# being the max allowable value). Source derivation here:
# https://goo.gl/qUjCgy
wing_bending_failure_indices = (
(tension - kite_mass * wing_acc_z) /
(self._aero_tension_limit *
(1 - 0.484 * abs(ang_acc_x / self._domega_limit))) +
wing_acc_z / self._accel_limit)
return {'wing_bending_failure_indices': wing_bending_failure_indices}
def GetOutput(self, timeseries):
tension = timeseries['tension_kn']
time = timeseries['time']
if (scoring_util.IsSelectionValid(tension)
and scoring_util.IsSelectionValid(time)):
# Low pass, symmetrically (2nd order) filter the tension signal.
cut_off_freq = 4.0
tension_f = scoring_util.LpFiltFiltTimeSeries(
time, tension, cut_off_freq)
tension_min = np.min(tension_f)
else:
tension_min = float('nan')
return {'tension_min': tension_min}
def GetTimeSeries(self, params, sim, control):
hover_params = params['control_params']['hover']
thrust_moment, thrust_moment_avail, control_time = self._SelectTelemetry(
sim, control, ['thrust_moment', 'thrust_moment_avail', 'time'])
if (not scoring_util.IsSelectionValid(thrust_moment)
or not scoring_util.IsSelectionValid(control_time)):
return {'max_saturation_duration': float('nan')}
if self._axis == 'thrust':
cmd = thrust_moment['thrust']
cmd_avail = thrust_moment_avail['thrust']
min_software_limit = float('-infinity')
max_software_limit = hover_params['altitude']['max_thrust']
elif self._axis == 'moment_y':
cmd = thrust_moment['moment']['y']
cmd_avail = thrust_moment_avail['moment']['y']
min_software_limit = hover_params['angles']['min_moment']['y']
max_software_limit = hover_params['angles']['max_moment']['y']
elif self._axis == 'moment_z':
cmd = thrust_moment['moment']['z']
cmd_avail = thrust_moment_avail['moment']['z']
min_software_limit = hover_params['angles']['min_moment']['z']
max_software_limit = hover_params['angles']['max_moment']['z']
else:
assert False, 'The axis %s is not supported.' % self._axis
t_samp = scoring_util.GetTimeSamp(control_time)
if np.isnan(t_samp):
return {'max_saturation_duration': np.array([float('nan')])}
saturation_mask = np.argwhere(
np.logical_or(
np.abs(cmd - cmd_avail) > 1e-2,
np.logical_or(
np.abs(cmd - min_software_limit) < 1e-2,
np.abs(cmd - max_software_limit) < 1e-2)))
if saturation_mask.size > 0:
saturated_intervals = scoring_util.GetIntervals(saturation_mask)
max_saturation_size = np.max([
interval[1] - interval[0] for interval in saturated_intervals
])
else:
max_saturation_size = 0.0
return {'max_saturation_duration': max_saturation_size * t_samp}
def GetTimeSeries(self, params, sim, control):
wing_xg, buoy_xg, dcm_g2v, platform_azi, gain_ramp = self._SelectTelemetry(
sim, control, ['wing_xg', 'buoy_xg', 'dcm_g2v', 'platform_azi',
'hover_gain_ramp_scale'],
flight_modes='kFlightModeHoverDescend')
hover_path_params = params['control_params']['hover']['path']
gain_ramp_down_idx = np.where(gain_ramp < 1e-8)
if (np.size(gain_ramp_down_idx) == 0 or
not scoring_util.IsSelectionValid(platform_azi)):
xy_perched_pos_err = float('nan')
else:
last_gain_ramp_down_idx = gain_ramp_down_idx[0][0]
dcm_v2p = geometry.AngleToDcm(
platform_azi[last_gain_ramp_down_idx], 0.0, 0.0, 'ZYX')
dcm_g2p = np.matmul(np.matrix(dcm_g2v[last_gain_ramp_down_idx, :, :]),
dcm_v2p)
final_wing_pos_g = np.array(wing_xg[last_gain_ramp_down_idx].tolist())
final_buoy_pos_g = np.array(buoy_xg[last_gain_ramp_down_idx].tolist())
final_wing_pos_p = np.matmul(dcm_g2p, final_wing_pos_g - final_buoy_pos_g)
perch_wing_pos_p = hover_path_params['perched_wing_pos_p'].tolist()
perched_wing_pos_err = final_wing_pos_p - perch_wing_pos_p
xy_perched_pos_err = np.sqrt(
perched_wing_pos_err[0, 0]**2 + perched_wing_pos_err[0, 1]**2)
return {'xy_perched_pos_err': xy_perched_pos_err}
def GetTimeSeries(self, params, sim, control):
tension, tether_roll, tether_pitch, time = self._SelectTelemetry(
sim, control,
['tether_tension', 'tether_roll', 'tether_pitch', 'time'])
bridle_y_offset = params['system_params']['wing']['bridle_y_offset']
bridle_pos_z = params['system_params']['wing']['bridle_pos']['z'][0, 0]
bridle_rad = params['system_params']['wing']['bridle_rad']
center_of_mass = params['system_params']['wing']['center_of_mass_pos']
roll_inertia = np.matrix(params['system_params']['wing']['I']['d'])[0,
0]
if scoring_util.IsSelectionValid(tension):
bridle_roll_stiffness = tension * (
-bridle_y_offset * np.sin(tether_roll) * np.cos(tether_pitch) +
bridle_rad * np.cos(tether_roll) * np.cos(tether_pitch) +
center_of_mass['y'] * np.sin(tether_roll) *
np.cos(tether_pitch) -
(center_of_mass['z'] - bridle_pos_z) * np.cos(tether_roll))
# Low pass, symmetrically (2nd order) filter the computed roll stiffness.
cut_off_freq = 0.5
bridle_roll_stiffness_f = scoring_util.LpFiltFiltTimeSeries(
time, bridle_roll_stiffness, cut_off_freq)
roll_period = np.divide(
2 * np.pi,
np.sqrt(
np.maximum(bridle_roll_stiffness_f, 1e-3) / roll_inertia))
return {'roll_period': roll_period}
else:
return {'roll_period': np.array([float('nan')])}
def GetTimeSeries(self, params, sim, control):
time = self._SelectTelemetry(sim, control, 'time')
if scoring_util.IsSelectionValid(time):
t_samp = scoring_util.GetTimeSamp(time)
return {'sample_time_error': np.abs(np.diff(time) - t_samp)}
else:
return {'sample_time_error': np.array([float('nan')])}
def GetTimeSeries(self, params, sim, control):
# TODO: Use the Gs02TransformStageFilter.
if self._transform_stages:
gs02_mode, gs02_transform_stage, tether_elevation = (
self._SelectTelemetry(
sim, control, ['gs02_mode', 'gs02_transform_stage',
'tether_elevation']))
if (not scoring_util.IsSelectionValid(gs02_mode) or
not scoring_util.IsSelectionValid(gs02_transform_stage) or
not scoring_util.IsSelectionValid(tether_elevation)):
return {
'gs02_mode': None,
'gs02_transform_stage': None,
'tether_elevation': None
}
else:
tether_elevation_deg = np.rad2deg(tether_elevation)
return {
'gs02_mode': gs02_mode,
'gs02_transform_stage': gs02_transform_stage,
'tether_elevation': tether_elevation_deg
}
else:
time, tether_elevation = self._SelectTelemetry(
sim, control, ['time', 'tether_elevation'])
if not (scoring_util.IsSelectionValid(tether_elevation) or
scoring_util.IsSelectionValid(time)):
return {
'tether_elevation': None
}
tether_elevation_deg = np.rad2deg(tether_elevation)
# Low pass, symmetrically (2nd order) filter the tether elevation.
cut_off_freq = 0.4
tether_elevation_deg_f = scoring_util.LpFiltFiltTimeSeries(
time, tether_elevation_deg, cut_off_freq)
self._t_samp = scoring_util.GetTimeSamp(time)
# Return filtered data.
return {
'tether_elevation': tether_elevation_deg_f
}
def GetTimeSeries(self, params, sim, control):
tether_tension, time = self._SelectTelemetry(
sim, control, ['tether_tension', 'time'])
if not scoring_util.IsSelectionValid(tether_tension):
return {'time': [], 'tension_kn': []}
else:
return {
'time': time,
'tension_kn': tether_tension.flatten() * 1e-3
}
def GetTimeSeries(self, params, sim, control):
area = params['system_params']['wing']['A']
c = params['system_params']['wing']['c']
airspeed, tether_moment = self._SelectTelemetry(
sim, control, ['airspeed', 'tether_moment'])
dynamic_pressure = (0.5 * params['system_params']['phys']['rho'] *
airspeed**2.0)
if scoring_util.IsSelectionValid(tether_moment):
cm_tether = tether_moment['y'] / (dynamic_pressure * area * c)
else:
cm_tether = np.array([float('nan')])
return {'cm_tether': cm_tether}
def GetTimeSeries(self, params, sim, control):
servo_torques = self._SelectTelemetry(sim, control,
'servo_shaft_torques')
if scoring_util.IsSelectionValid(servo_torques):
rud_hm_1 = servo_torques[:, 8]
rud_hm_2 = servo_torques[:, 9]
else:
# Servo_torques are returned as np.array([float('nan')]) for flight logs.
rud_hm_1 = np.array([float('nan')])
rud_hm_2 = np.array([float('nan')])
summed_moment = np.abs(rud_hm_1) + np.abs(rud_hm_2)
return {'rud_hm': summed_moment}
def GetTimeSeries(self, params, sim, control):
tether_azimuth, platform_azi = (
self._SelectTelemetry(sim, control, ['tether_azimuth', 'platform_azi']))
if not scoring_util.IsSelectionValid(tether_azimuth):
return {
'tether_azimuth_p': None,
}
else:
# Only evaluate panel azimuth tracking when the kite is in the air.
tether_azimuth = numpy_utils.Wrap(
(tether_azimuth - platform_azi), -np.pi, np.pi)
return {
'tether_azimuth_p': np.rad2deg(tether_azimuth),
}
def GetTimeSeries(self, params, sim, control):
block_voltages, ground_voltage, tether_current, loop_angle, time = (
self._SelectTelemetry(sim, control,
['block_voltages', 'ground_voltage',
'tether_current', 'loop_angle', 'time'],
flight_modes='kFlightModeCrosswindNormal'))
crosswind_ground_power_mean = float('nan')
crosswind_stable_ground_power_mean = float('nan')
crosswind_kite_power_mean = float('nan')
crosswind_stable_kite_power_mean = float('nan')
if scoring_util.IsSelectionValid(block_voltages):
kite_powers = np.sum(block_voltages, 1) * tether_current
ground_powers = ground_voltage * tether_current
loop_indexes = []
last_time = None
# Determine times of each loop 9 o'clock.
for ind in range(1, len(loop_angle)):
if loop_angle[ind] > 2 * np.pi - 0.5 and loop_angle[ind - 1] < 0.5:
if not last_time or time[ind] - last_time > 5.0:
loop_indexes.append(ind)
last_time = time[ind]
# Compute average power across all whole loops flown.
if len(loop_indexes) >= 2:
crosswind_kite_power_mean = np.mean(
kite_powers[loop_indexes[0]:loop_indexes[-1]])
crosswind_ground_power_mean = np.mean(
ground_powers[loop_indexes[0]:loop_indexes[-1]])
# Throw away the first 4 loops to allow measurement to stabilize.
if len(loop_indexes) >= 6:
crosswind_stable_kite_power_mean = np.mean(
kite_powers[loop_indexes[4]:loop_indexes[-1]])
crosswind_stable_ground_power_mean = np.mean(
ground_powers[loop_indexes[4]:loop_indexes[-1]])
return {
'crosswind_ground_power_mean': crosswind_ground_power_mean,
'crosswind_stable_ground_power_mean':
crosswind_stable_ground_power_mean,
'crosswind_kite_power_mean': crosswind_kite_power_mean,
'crosswind_stable_kite_power_mean': crosswind_stable_kite_power_mean,
}
def GetTimeSeries(self, params, sim, control):
flight_modes = [
'kFlightModeCrosswindNormal', 'kFlightModeCrosswindPrepTransOut'
]
path_radius_target, wing_pos_cw = self._SelectTelemetry(
sim,
control, ['path_radius_target', 'wing_pos_cw'],
flight_modes=flight_modes)
if not scoring_util.IsSelectionValid(wing_pos_cw):
return {
'crosswind_radius_err': np.array([]),
}
radii = np.hypot(wing_pos_cw['x'], wing_pos_cw['y'])
radius_cmds = path_radius_target
error = radii - radius_cmds
return {'crosswind_radius_err': error}
def GetTimeSeries(self, params, sim, control):
ground_voltage, tether_current = self._SelectTelemetry(
sim, control, ['ground_voltage', 'tether_current'])
power = np.multiply(ground_voltage, tether_current)
if not scoring_util.IsSelectionValid(power):
return {
'power_slope': np.array([float('nan')])
}
# Now figure out the indexing to support desired window time (s).
window_time = 1.0
sample_step = params['system_params']['ts'][0]
window_size = int(window_time / sample_step)
# Recalculate the actual window time we get for an achievable window size.
window_time = float(window_size) * sample_step
delta_power = np.subtract(power[0:-window_size], power[window_size:])
return {
# 1.0 factor accounts for 1 / 1.0 seconds.
'power_slope': 1.0 / window_time * np.abs(delta_power) / 1e3
}
def GetTimeSeries(self, params, sim, control):
# "_i" is used to denote the as-imported variable.
(omega_i, airspeed_i, alpha_i, beta_i, flaps,
loop_angles) = (self._SelectTelemetry(sim, control, [
'body_rates', 'airspeed', 'alpha', 'beta', 'flaps', 'loop_angle'
]))
if not scoring_util.IsSelectionValid(flaps):
return {
'saturation_mask': None,
'largest_saturation_loop_mask': None
}
deflections = np.rad2deg(flaps[:, self._flap_indices])
# Observations of actual control surface deflections show that when
# saturated, they are not exactly at the control limit. Adding or
# subtracting 0.25 degrees from the limit allows for most saturations
# to be flagged by the criteria. A4 and A5 are often very close to the
# upper limit of zero with very small magnitudes of deflection. An offset
# of only 0.005 degrees is applied to the upper flap limit for flaps A4
# and A5 to prevent normal operation from being flagged erroneously.
# The deflection limits of the rudder are altered to account for the
# wing-fuselage junction loads limit, per b/112267831.
lower_deflection_limit = (np.rad2deg(
np.array(params['control_params']['crosswind']['output']
['lower_flap_limits'][0, self._flap_indices])) + 0.25)
if (self._flap_indices == [system_labels.kFlapA4]
or self._flap_indices == [system_labels.kFlapA5]):
upper_deflection_limit = (np.rad2deg(
np.array(params['control_params']['crosswind']['output']
['upper_flap_limits'][0, self._flap_indices])) -
0.005)
elif self._flap_indices == [system_labels.kFlapRud]:
# Check if body rates data exist. These may not exist if the relevant
# flight modes do not exist.
if not scoring_util.IsSelectionValid(omega_i):
return {'saturation_mask': float('nan')}
# Position [m] of the vtail aerodynamic center. Only the x-coordinate is
# relevant here.
# TODO: Add position of aerodynamic center to params in the h5 log.
r_vtail = np.array([-7.0, 0.0, 0.0])
vapp = np.array(airspeed_i)
alpha = np.array(alpha_i)
beta = np.array(beta_i)
omega = np.array([omega_i['x'], omega_i['y'], omega_i['z']])
# Account for motion of empennage relative to kite origin as it affects
# apparent wind, alpha, beta.
v_kite = apparent_wind_util.ApparentWindSphToCart(
vapp, alpha, beta).T
v_omega = np.cross(omega, r_vtail, axis=0)
vapp, alpha, beta = (apparent_wind_util.ApparentWindCartToSph(
(v_kite + v_omega).T))
rudder_limits = self._GetRudderLimits(beta, vapp)
lower_deflection_limit = rudder_limits['lower_limit']
upper_deflection_limit = rudder_limits['upper_limit']
# Check that there is not a dimension problem coming out of the rudder
# deflection limit table. Deflections is expected shape (N,1). The rudder
# table uses the apparent_wind_util that may create arrays of shape (N,).
if np.shape(lower_deflection_limit) != np.shape(deflections):
lower_deflection_limit = np.reshape(lower_deflection_limit,
np.shape(deflections))
if np.shape(upper_deflection_limit) != np.shape(deflections):
upper_deflection_limit = np.reshape(upper_deflection_limit,
np.shape(deflections))
else:
upper_deflection_limit = (np.rad2deg(
np.array(params['control_params']['crosswind']['output']
['upper_flap_limits'][0, self._flap_indices])) - 0.25)
# Fraction of a limit at which a surface is considered nearly saturated.
saturation_fraction = 0.9
new_upper_limit = saturation_fraction * upper_deflection_limit
new_lower_limit = saturation_fraction * lower_deflection_limit
is_saturated_upper = np.greater_equal(deflections, new_upper_limit)
is_saturated_lower = np.less_equal(deflections, new_lower_limit)
saturation_mask = np.logical_or(is_saturated_upper, is_saturated_lower)
largest_saturation_loop_mask = saturation_mask
# Identify the number of loops in the data and cycle over them to find which
# loop has the highest percentage of saturation. If there are no loops then
# simply take all the data as the scoring function is likely operating in a
# non-crosswind mode, e.g. hover.
endloop_indices = loop_averager.GetEndLoopIndices(loop_angles)
if np.size(endloop_indices):
# Go through each independent loop to find the mask array that contains
# the largest amount of masked values and return that array for scoring.
for i, endloop_indx in enumerate(endloop_indices):
if i == 0:
start_loop_indx = 0
largest_pct_saturated = 0.0
largest_saturation_loop_mask = saturation_mask[
start_loop_indx:endloop_indx + 1]
else:
start_loop_indx = endloop_indices[i - 1]
# Figure out how long the surface has been saturated during this loop.
this_loop_mask = saturation_mask[start_loop_indx:endloop_indx +
1]
saturation_counter = np.where(this_loop_mask)[0]
percent_saturated = (float(np.size(saturation_counter)) /
np.size(this_loop_mask) * 100.0)
if percent_saturated > largest_pct_saturated:
largest_saturation_loop_mask = this_loop_mask
largest_pct_saturated = percent_saturated
return {
'saturation_mask': saturation_mask,
'largest_saturation_loop_mask': largest_saturation_loop_mask
}
def GetTimeSeries(self, params, sim, control):
# Exclude Perched, PilotHover, HoverAscend and HoverDescend flight modes.
flight_mode_exclusion_list = [
'kFlightModePilotHover', 'kFlightModePerched',
'kFlightModeHoverAscend', 'kFlightModeHoverDescend'
]
flight_modes = []
for name in _FLIGHT_MODE_HELPER.Names():
if name not in flight_mode_exclusion_list:
flight_modes.append(name)
# Constants.
rhoair = params['sim_params']['phys_sim']['air_density']
g_g = params['system_params']['phys']['g_g'][0]
gravity = np.array([g_g['x'], g_g['y'], g_g['z']])
sref = params['system_params']['wing']['A']
bref = params['system_params']['wing']['b']
cref = params['system_params']['wing']['c']
# Maintain backwards compatibility with RPX-09 and previous logs.
wing_params_names = params['system_params']['wing'].dtype.names
tail_params_names = [
'm_tail', 'i_tail', 'tail_cg_pos', 'horizontal_tail_pos'
]
if all(name in wing_params_names for name in tail_params_names):
system_params = params['system_params'][0]
m_tail = system_params['wing']['m_tail']
i_tail = np.array(system_params['wing']['i_tail']['d'])
tail_cg_pos_i = system_params['wing']['tail_cg_pos']
tail_cg_pos = np.array(
[tail_cg_pos_i['x'], tail_cg_pos_i['y'], tail_cg_pos_i['z']])
r_empennage_i = system_params['wing']['horizontal_tail_pos']
r_empennage = np.array(
[r_empennage_i['x'], r_empennage_i['y'], r_empennage_i['z']])
else:
# Tail CG [m] in body coordinates (tail = tub + fuselage + empennage).
# Source: full kite finite element model m600assy_sn2_fem_r13_s.fem
tail_cg_pos = np.array([-5.139, 0.003, 0.181])
# Tail mass [kg] (tail = tub + fuselage + empennage).
# Source: full kite finite element model m600assy_sn2_fem_r13_s.fem
m_tail = 189.2
# Tail matrix of inertia [kg-m^2] about tail CG
# (tail = tub + fuselage + empennage).
# Source: full kite finite element model m600assy_sn2_fem_r13_s.fem
i_tail = np.array([[152.20, -0.5964, 61.80],
[-0.5964, 1356.5, 0.7337],
[61.80, 0.7337, 1280.2]])
# Position [m] of fuselage-empennage junction
r_empennage = np.array([-6.776, 0.045, 0.8165])
# "_i" is used to denote the as-imported variable.
(acc_b_i, omega_i, domega_i, airspeed_i, alpha_i, beta_i, flaps_i,
time_i, dcm_g2b_i) = self._SelectTelemetry(
sim,
control, [
'wing_acc', 'body_rates', 'angular_acc', 'airspeed', 'alpha',
'beta', 'flaps', 'time', 'dcm_g2b'
],
flight_modes=flight_modes)
# Check if body rates data exist. These may not exist if the relevant
# flight modes do not exist.
if not scoring_util.IsSelectionValid(omega_i):
return {'wing_fuse_junction_moment': np.array([float('nan')])}
vapp = np.array(airspeed_i)
alpha = np.array(alpha_i)
beta = np.array(beta_i)
d_elev = flaps_i[:, 6]
d_rud = flaps_i[:, 7]
dcm_g2b = np.array(dcm_g2b_i)
acc_b = np.array([acc_b_i['x'], acc_b_i['y'], acc_b_i['z']])
# Add gravity vector to inertial acceleration.
acc_b -= np.matmul(dcm_g2b, gravity).T
omega = np.array([omega_i['x'], omega_i['y'], omega_i['z']])
# Create angular acceleration data for flight logs.
if scoring_util.IsSelectionValid(domega_i):
domega = np.array([domega_i['x'], domega_i['y'], domega_i['z']])
else:
domega = np.array([
np.gradient(omega_i['x'], time_i),
np.gradient(omega_i['y'], time_i),
np.gradient(omega_i['z'], time_i)
])
# Account for motion of empennage relative to kite origin as it affects
# apparent wind, alpha, beta.
v_kite = apparent_wind_util.ApparentWindSphToCart(vapp, alpha, beta).T
v_omega = np.cross(omega, r_empennage, axis=0)
vapp, alpha, beta = (apparent_wind_util.ApparentWindCartToSph(
(v_kite + v_omega).T))
# Tail aerodynamic moment lookup tables (tail = empennage only).
cl_lookup = interpolate.interp2d(self._d_rudder,
self._betas,
self._cl_tail,
kind='linear')
cm_lookup = interpolate.interp2d(self._d_elev,
self._alphas,
self._cm_tail,
kind='linear')
cn_lookup = interpolate.interp2d(self._d_rudder,
self._betas,
self._cn_tail,
kind='linear')
# Initialize arrays filled out in for-loop
cl = np.zeros(time_i.shape[0], dtype=float)
cm = np.zeros(time_i.shape[0], dtype=float)
cn = np.zeros(time_i.shape[0], dtype=float)
# Tail aerodynamic moment at the wing/fuselage junction looped on all
# datapoints. For cm reduce alpha by 3 deg for wing downwash at empennage.
# This is true for higher wing CL's which helps capture the peak bending
# moments, but is not necessarily valid for all data in the time series.
for ni in range(0, time_i.shape[0]):
cl[ni] = cl_lookup(np.rad2deg(d_rud[ni]), np.rad2deg(beta[ni]))
cm[ni] = cm_lookup(np.rad2deg(d_elev[ni]),
np.rad2deg(alpha[ni]) - 3.0)
cn[ni] = cn_lookup(np.rad2deg(d_rud[ni]), np.rad2deg(beta[ni]))
aero_moment_junct = 0.5 * rhoair * vapp**2.0 * sref * np.array(
[cl * bref, cm * cref, cn * bref])
# Inertial moment at the wing/fuselage junction.
inertial_force_cg = m_tail * (
acc_b + np.cross(domega, tail_cg_pos, axis=0) +
np.cross(omega, np.cross(omega, tail_cg_pos, axis=0), axis=0))
inertial_moment_cg = (np.dot(i_tail, domega) +
np.cross(omega, np.dot(i_tail, omega), axis=0))
inertial_moment_junct = inertial_moment_cg + np.cross(
tail_cg_pos, inertial_force_cg, axis=0)
# Total moment at the wing/fuselage junction.
moment_junct = {
'x': aero_moment_junct[0, :] - inertial_moment_junct[0, :],
'y': aero_moment_junct[1, :] - inertial_moment_junct[1, :],
'z': aero_moment_junct[2, :] - inertial_moment_junct[2, :]
}
return {'wing_fuse_junction_moment': moment_junct[self._axis] / 1000.0}
def GetTimeSeries(self, params, sim, control):
# TODO: This scoring function takes a while to evaluate, needs
# some attention to reduce execution time.
# Table look-up
v = self._v_freestreams
o = self._omegas
my_cw_lookup = interpolate.RectBivariateSpline(v,
o,
self._my_a_cw.T,
kx=1,
ky=1)
mz_cw_lookup = interpolate.RectBivariateSpline(v,
o,
self._mz_a_cw.T,
kx=1,
ky=1)
my_ccw_lookup = interpolate.RectBivariateSpline(v,
o,
self._my_a_ccw.T,
kx=1,
ky=1)
mz_ccw_lookup = interpolate.RectBivariateSpline(v,
o,
self._mz_a_ccw.T,
kx=1,
ky=1)
rotors_dir = params['system_params']['rotors']['dir']
rotors_axis = params['system_params']['rotors']['axis']
# Note: rotors_inertia is rotational inertia of rotor and motor.
rotors_inertia = params['system_params']['rotors']['I']
param_names = [
'airspeed', 'alpha', 'beta', 'rotor_speeds', 'rotor_gyro_moments',
'body_rates'
]
(vapp, alpha, beta, rotor_omega, gyro_moments_xyz,
body_rates) = self._SelectTelemetry(sim, control, param_names)
# Transformation: Standard kite body (b) to standard hub fixed (h)
# (x-forward, z-downward)
dcm_b2h = np.array(geometry.AngleToDcm(0.0, np.deg2rad(-3.0), 0.0))
# Transformation: Geometric hub fixed (gh, X-rearward, Z-upward)) to
# standard hub fixed (h)
dcm_gh2h = np.array(geometry.AngleToDcm(0.0, np.pi, 0.0))
# Kite apparent speed components in (h)
vk = vapp[np.newaxis].T * np.matmul(
dcm_b2h,
np.transpose(
np.array([[-np.cos(alpha) * np.cos(beta)], [-np.sin(beta)],
[-np.sin(alpha) * np.cos(beta)]]),
(2, 0, 1)))[:, :, 0]
# Rotor apparent speed in spherical coordinates
va = np.linalg.norm(vk, axis=1)
a = -np.arctan2(np.hypot(vk[:, 1], vk[:, 2]), vk[:, 0])
t = -np.arctan2(vk[:, 2], vk[:, 1])
# Geometric wind aligned (gw) to geometric hub fixed (gh)
# TODO: Vectorize this function.
dcm_gw2gh = np.ndarray((len(t), 3, 3))
for i in range(len(t)):
dcm_gw2gh[i, :, :] = geometry.AngleToDcm(0.0, 0.0, t[i])
# Gyroscopic moment components in (h)
if scoring_util.IsSelectionValid(gyro_moments_xyz):
gyro_moment_y = gyro_moments_xyz['y']
gyro_moment_z = gyro_moments_xyz['z']
else:
angular_momentum_h = (rotors_inertia[0, :] * rotor_omega *
rotors_dir[0, :])
axis = np.concatenate(
[rotors_axis['x'], rotors_axis['y'], rotors_axis['z']])
angular_momentum_b = np.multiply(
np.transpose(angular_momentum_h[np.newaxis], (1, 0, 2)),
axis[np.newaxis])
body_omega = np.concatenate([[body_rates['x']], [body_rates['y']],
[body_rates['z']]])
gyro_moment_b = np.cross(angular_momentum_b,
np.transpose(body_omega[np.newaxis],
(2, 1, 0)),
axis=1)
gyro_moment_y = gyro_moment_b[:, 1, :]
gyro_moment_z = gyro_moment_b[:, 2, :]
gyro_moment = np.zeros(
(gyro_moment_y.shape[0], gyro_moment_y.shape[1], 3, 1))
gyro_moment[:, :, 1, 0] = gyro_moment_y
gyro_moment[:, :, 2, 0] = gyro_moment_z
m_gyro = np.matmul(dcm_b2h, gyro_moment)[:, :, :, 0]
v_freestream_in_range = np.logical_and(
va >= np.min(self._v_freestreams),
va <= np.max(self._v_freestreams))
# Loop on 8 rotors
m_totals = {}
for nr in range(0, 8):
rotor_omega_cur = rotor_omega[:, nr]
omega_in_range = np.logical_and(
rotor_omega_cur >= np.min(self._omegas),
rotor_omega_cur <= np.max(self._omegas))
in_range = np.logical_and(omega_in_range, v_freestream_in_range)
# Table look-up
if rotors_dir[0, nr] > 0:
my_aero_w = (my_cw_lookup(
va[in_range], rotor_omega_cur[in_range], grid=False) *
a[in_range])
mz_aero_w = (mz_cw_lookup(
va[in_range], rotor_omega_cur[in_range], grid=False) *
a[in_range])
else:
my_aero_w = (my_ccw_lookup(
va[in_range], rotor_omega_cur[in_range], grid=False) *
a[in_range])
mz_aero_w = (mz_ccw_lookup(
va[in_range], rotor_omega_cur[in_range], grid=False) *
a[in_range])
# Aerodynamic moment components in (h)
m_aero_w = np.zeros((my_aero_w.shape[0], 3, 1))
m_aero_w[:, 1, 0] = my_aero_w
m_aero_w[:, 2, 0] = mz_aero_w
m_aero = np.matmul(dcm_gh2h,
np.matmul(dcm_gw2gh[in_range, :, :],
m_aero_w))[:, :, 0]
# Total resultant in-plane moment
m_totals[nr] = m_aero + m_gyro[in_range, nr]
return {'rotor_moments': m_totals}