def testAngleToDcmTrival(self):
for a1 in ['x', 'y', 'z']:
for a2 in ['x', 'y', 'z']:
if a1 == a2:
continue
order = ''.join([a1, a2, a1])
flip_order = ''.join([a2, a1, a2])
r = np.random.rand(1)
self.assertTrue(
np.all(
geometry.AngleToDcm(r, 0.0, 0.0, order) ==
geometry.AngleToDcm(0.0, 0.0, r, order)))
self.assertTrue(
np.all(
geometry.AngleToDcm(0.0, r, 0.0, flip_order) ==
geometry.AngleToDcm(0.0, 0.0, r, order)))
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 GetTrimStateAndInputs(x):
"""Returns a WingState from trim variables."""
# Unpack trim variables.
glide_angle, dv_app, roll, d_aileron, d_elevator, d_rudder = x
# Calculate trim state.
v_app = 20.0 + dv_app
pitch = -glide_angle + angle_of_attack
yaw = 0.0
state = dynamics.WingState(omega_b=state_0.omega_b,
dcm_g2b=geometry.AngleToDcm(
yaw, pitch, roll),
wing_vel_g=np.matrix([
v_app * np.cos(glide_angle), 0.0,
v_app * np.sin(glide_angle)
]).T,
wing_pos_g=state_0.wing_pos_g)
# Calculate trim inputs.
flaps = inputs_0.flaps.copy()
flaps[[control_types.kFlapA1, control_types.kFlapA2]] += -d_aileron
flaps[[control_types.kFlapA7, control_types.kFlapA8]] += d_aileron
flaps[control_types.kFlapEle] += d_elevator
flaps[control_types.kFlapRud] += d_rudder
inputs = dynamics.WingInputs(thrust=inputs_0.thrust,
motor_moment=inputs_0.motor_moment,
flaps=flaps,
wind_g=inputs_0.wind_g)
return state, inputs
def _GetState(omega_b=None,
yaw=0.0,
pitch=0.0,
roll=0.0,
dwing_vel_ti_x=0.0,
dwing_vel_ti_z=0.0):
"""Produce a TransInState with default values."""
if omega_b is None:
omega_b = np.matrix(np.zeros((3, 1)))
dcm_ti2b = geometry.AngleToDcm(yaw, pitch, roll)
wing_vel_ti = wing_vel_ti_0 + [[dwing_vel_ti_x], [0.0],
[dwing_vel_ti_z]]
if on_tether:
radial_pos_ti = self._tether_length + 4.0
else:
radial_pos_ti = self._tether_length - 5.0
wing_pos_ti = radial_pos_ti * np.matrix([[
-np.cos(elevation_angle_ti)
], [0.0], [-np.sin(elevation_angle_ti)]])
return TransInState(omega_b=omega_b,
dcm_ti2b=dcm_ti2b,
wing_vel_ti=wing_vel_ti,
wing_pos_ti=wing_pos_ti)
def PrintTrim(self, state, inputs):
"""Print information relevant to a trimmed state.
Args:
state: WingState structure.
inputs: dynamics.WingInputs structure.
"""
state_dot = self._wing.CalcDeriv(state, inputs)
v_rel, alpha, beta = state.CalcAerodynamicAngles(inputs.wind_g)
# Fixing total thrust coefficient to 0.0 for this application.
thrust_coeff = 0.0
_, cf, _ = self._wing.CalcAeroForceMomentPos(v_rel, alpha, beta,
state.omega_b,
inputs.flaps,
thrust_coeff)
yaw, pitch, roll = geometry.DcmToAngle(state.dcm_g2b)
dcm_w2b = geometry.AngleToDcm(-beta, alpha, 0.0, order='ZYX')
cf_w = dcm_w2b.T * cf
values = [[
('Roll [deg]', np.rad2deg(roll)),
('Pitch [deg]', np.rad2deg(pitch)),
('Yaw [deg]', np.rad2deg(yaw)),
],
[('Port ail. [deg]',
np.rad2deg(inputs.flaps[control_types.kFlapA1])),
('Starboard ail. [deg]',
np.rad2deg(inputs.flaps[control_types.kFlapA8])),
('Ele. [deg]',
np.rad2deg(inputs.flaps[control_types.kFlapEle])),
('Rud. [deg]',
np.rad2deg(inputs.flaps[control_types.kFlapRud]))],
[('Vrel [m/s]', v_rel), ('Alpha [deg]', np.rad2deg(alpha)),
('Beta [deg]', np.rad2deg(beta))],
[('V X [m/s]', state.wing_vel_g[0]),
('V Y [m/s]', state.wing_vel_g[1]),
('V Z [m/s]', state.wing_vel_g[2])],
[('A X [m/s^2]', state_dot.dwing_vel_g[0]),
('A Y [m/s^2]', state_dot.dwing_vel_g[1]),
('A Z [m/s^2]', state_dot.dwing_vel_g[2])],
[('Pdot [rad/s^2]', state_dot.domega_b[0]),
('Qdot [rad/s^2]', state_dot.domega_b[1]),
('Rdot [rad/s^2]', state_dot.domega_b[2])],
[
('CL [#]', -cf_w[2]),
('CD [#]', -cf_w[0]),
('Glide ratio [#]', cf_w[2] / cf_w[0]),
]]
for line_values in values:
for name, value in line_values:
print '%20s: %10.3f' % (name, value)
def HomogeneousTransform(euler_angles, axis_order, translate=None):
"""Apply a homogeneous transformation."""
# We only use the DCM part of the homogeneous transform matrix, but
# the entire matrix is used for testing and debugging purposes.
result = np.identity(4)
if euler_angles is not None:
result[:3, :3] = geometry.AngleToDcm(*euler_angles, order=axis_order)
if translate is not None:
result[0:3, 3] = translate[:]
return result
def setUp(self):
all_params = mconfig.MakeParams('common.all_params')
self._sim_params = all_params['sim']
self._system_params = all_params['system']
self._dcm_g2control = geometry.AngleToDcm(0.1, 0.2, 0.3)
motor_model = dynamics.PureForceMomentMotorModel(
self._system_params['rotors'],
self._system_params['wing']['center_of_mass_pos'])
self._wing = dynamics.Wing(
self._system_params, self._sim_params, VacuumAeroModel(), motor_model,
dynamics.ConstantTetherForceModel(np.matrix(np.zeros((3, 1)))))
def testAngleToDcmRecovery(self):
for a1 in ['x', 'y', 'z']:
for a3 in ['x', 'y', 'z']:
for a2 in set(['x', 'y', 'z']) - set([a1, a3]):
order = ''.join([a1, a2, a3])
eulers = np.random.rand(3)
dcm = geometry.AngleToDcm(eulers[2], eulers[1], eulers[0],
order)
(r1, r2, r3) = geometry.DcmToAngle(dcm, order)
self.assertAlmostEqual(eulers[2], r1)
self.assertAlmostEqual(eulers[1], r2)
self.assertAlmostEqual(eulers[0], r3)
def testConservation(self):
"""Test that energy and angular momentum are conserved."""
np.random.seed(22)
for _ in range(1000):
eulers = _GetRandomVec(3)
state = dynamics.WingState(
omega_b=_GetRandomVec(3),
dcm_g2b=geometry.AngleToDcm(eulers[2], eulers[1], eulers[0]),
wing_pos_g=_GetRandomVec(3),
wing_vel_g=_GetRandomVec(3))
inputs = dynamics.WingInputs(
thrust=np.matrix([[0.0]]),
motor_moment=np.matrix([[0.0] for _ in range(3)]),
flaps=_GetRandomVec(system_types.kNumFlaps),
wind_g=_GetRandomVec(3))
state_dot = self._wing.CalcDeriv(state, inputs)
# Step-size for finite-difference comparisons.
h = 1e-4
# Check that the kinematics are correct.
state_pre = state.Increment(state_dot, step=-h)
state_post = state.Increment(state_dot, step=h)
dcm_g2b_diff = (state_post.dcm_g2b - state_pre.dcm_g2b) / (2.0 * h)
dcm_g2b_dot = -geometry.CrossMatrix(state.omega_b) * state.dcm_g2b
for i in range(3):
for j in range(3):
self.assertAlmostEqual(dcm_g2b_dot[i, j],
dcm_g2b_diff[i, j])
for i in range(3):
self.assertEqual(state.wing_vel_g[i, 0], state_dot.dwing_pos_g[i, 0])
# Check conservation of angular momentum (the only force here is gravity).
angular_momentum_dot = (
(geometry.CrossMatrix(state.omega_b)
* self._wing._wing_inertia_matrix
* state.omega_b)
+ self._wing._wing_inertia_matrix * state_dot.domega_b)
for i in range(3):
self.assertAlmostEqual(0.0, angular_momentum_dot[i])
# Check that energy is conserved.
energies = [
self._wing.CalcEnergy(state_pre),
self._wing.CalcEnergy(state_post)
]
energy_dot = (energies[1] - energies[0]) / (2.0 * h)
self.assertLess(np.abs(energy_dot),
np.fmax(1e-6, h * np.abs(energies[0])))
def CalcDcmWToB(alpha, beta):
"""Calculate the wind-to-body DCM given alpha and beta.
Args:
alpha: Angle of attack [rad]
beta: Angle of sideslip [rad]
Returns:
dcm_w2b
Compare to CalcDcmWToB in sim/physics/aero_frame.cc.
"""
return geometry.AngleToDcm(-beta, alpha, 0.0, 'ZYX')
def testCalcGravityForceMomentPos(self):
"""Test that gravity does not apply a moment."""
eulers = _GetRandomVec(3)
dcm_g2b = geometry.AngleToDcm(eulers[2], eulers[1], eulers[0])
force_moment_pos = self._wing._CalcGravityForceMomentPos(dcm_g2b)
force_moment = self._wing._BodyForceMomentPosToComForceMoment(
[force_moment_pos])
self.assertAlmostEqual(self._wing._wing_mass
* self._system_params['phys']['g'],
np.linalg.norm(force_moment.force))
self.assertAlmostEqual(0.0, force_moment.moment[0])
self.assertAlmostEqual(0.0, force_moment.moment[1])
self.assertAlmostEqual(0.0, force_moment.moment[2])
self.assertTrue(isinstance(force_moment.force, np.matrix))
self.assertTrue(isinstance(force_moment.moment, np.matrix))
def testGetAngleDerivatives(self):
for _ in range(22):
eulers_zyx = np.matrix([np.random.rand(1) for _ in range(3)])
pqr = np.matrix([np.random.randn(1) for _ in range(3)])
dcm = geometry.AngleToDcm(eulers_zyx[2], eulers_zyx[1],
eulers_zyx[0])
h = 1e-8
dcm_p = scipy.linalg.expm(-h * geometry.CrossMatrix(pqr)) * dcm
(r3, r2, r1) = geometry.DcmToAngle(dcm_p)
eulers_dot_approx = ([[r1], [r2], [r3]] - eulers_zyx) / h
eulers_dot = geometry.GetAngleDerivative(eulers_zyx, pqr)
for i in range(3):
self.assertAlmostEqual(eulers_dot_approx[i, 0],
eulers_dot[i, 0],
places=6)
def __init__(self,
rotor_databases,
air_density,
weights,
rotor_params,
rotor_control_params,
hover_flight_mode=False):
self._dcm_r2b = geometry.AngleToDcm(
0.0,
np.arctan2(rotor_params[0]['axis'][2], rotor_params[0]['axis'][0]),
0.0)
self._rotor_databases = rotor_databases
self._air_density = air_density
self._weights = weights
self._rotor_params = rotor_params
self._rotor_control_params = rotor_control_params
self._hover_flight_mode = hover_flight_mode
def testVelocitiesToAerodynamicAngles(self):
v_rel_in = 5.0
alpha_in = 0.1
beta_in = -0.05
v_app_b = v_rel_in * np.matrix([[np.cos(alpha_in) * np.cos(beta_in)],
[np.sin(beta_in)],
[np.sin(alpha_in) * np.cos(beta_in)]])
v_zero = np.matrix([[0.0], [0.0], [0.0]])
for _ in range(22):
dcm_g2b = geometry.AngleToDcm(np.random.rand(1), np.random.rand(1),
np.random.rand(1))
v_app_g = np.transpose(dcm_g2b) * v_app_b
(v_rel, alpha, beta) = geometry.VelocitiesToAerodynamicAngles(
dcm_g2b, v_app_g, v_zero)
self.assertAlmostEqual(v_rel, v_rel_in)
self.assertAlmostEqual(alpha, alpha_in)
self.assertAlmostEqual(beta, beta_in)
(v_rel, alpha, beta) = geometry.VelocitiesToAerodynamicAngles(
dcm_g2b, v_zero, -v_app_g)
self.assertAlmostEqual(v_rel, v_rel_in)
self.assertAlmostEqual(alpha, alpha_in)
self.assertAlmostEqual(beta, beta_in)
def MakeParams(params):
flight_plan = params['system']['flight_plan']
# Set initial wing position and orientation in the perch frame.
if flight_plan == m.kFlightPlanStartDownwind:
Xp_0 = np.array([-0.98, 0.0, -0.022]) * params['tether']['length']
wing_azi_p = np.pi
start_perched = False
elif flight_plan == m.kFlightPlanManual:
# The sim begins in OffTether; release the kite from a 60 degree
# elevation.
# TODO(b/132663390): Set a realistic initial velocity and attitude.
Xp_0 = np.array([-0.5, 0.0, -0.866]) * params['tether']['length']
wing_azi_p = np.pi
start_perched = False
elif flight_plan in [
m.kFlightPlanTurnKey, m.kFlightPlanLaunchPerch,
m.kFlightPlanHighHover
]:
# We drop the wing from slightly above the perched position, so its
# contactors are not initially in contact with the perch. The wing must be
# moved towards the perch by the same distance, so the tether's initial
# length is short enough to keep it on the perch.
drop_height = 0.06
if params['gs_model'] == m.kGroundStationModelGSv2:
Xp_0 = (np.array(
params['ground_station']['gs02']['perched_wing_pos_p']) +
np.array([0.0, -drop_height, -drop_height]))
else:
Xp_0 = (np.array(params['perch']['perched_wing_pos_p']) +
np.array([-drop_height, 0.0, -drop_height]))
wing_azi_p = 0.0
start_perched = True
elif flight_plan == m.kFlightPlanDisengageEngage:
# DisengageEngage starts with the tether almost fully payed out.
Xp_0 = [0.0, 428.0, -6.0]
wing_azi_p = np.pi / 2.0
start_perched = False
elif flight_plan == m.kFlightPlanHoverInPlace:
Xp_0 = [0.0, 55.0, -6.0] # Hand-tuned for GS02 hover simulations.
wing_azi_p = np.pi / 2.0
start_perched = False
else:
assert False
if params['gs_model'] == m.kGroundStationModelGSv2:
_, pitch, roll = geometry.QuatToAngle(
np.matrix([[d] for d in params['buoy_sim']['q_0']]))
yaw = (params['buoy_sim']['mooring_lines']['yaw_equilibrium_heading'] -
params['ground_frame']['heading'])
dcm_g2v = geometry.AngleToDcm(yaw, pitch, roll)
dcm_v2p = geometry.AngleToDcm(
params['gs02_sim']['initial_platform_azi'], 0.0, 0.0)
dcm_g2p = dcm_g2v * dcm_v2p
else:
# TODO: Make a SWIG wrapper for CalcDcmGToP.
dcm_g2p = geometry.AngleToDcm(params['perch_sim']['theta_p_0'], 0.0,
0.0)
if start_perched:
if params['gs_model'] == m.kGroundStationModelGSv2:
dcm_p2panel = np.matrix(
params['gs02_sim']['panel']['dcm_p2panel']['d'])
else:
assert wing_azi_p == 0.0
dcm_p2panel = np.matrix(
params['perch_sim']['panel']['dcm_p2panel']['d'])
dcm_panel2b = geometry.AngleToDcm(np.pi, np.pi / 2.0, 0.0)
dcm_p2b = dcm_panel2b * dcm_p2panel
else:
dcm_p2b = geometry.AngleToDcm(np.pi + wing_azi_p, np.pi / 2.0, 0.0)
q_g2b = geometry.DcmToQuat(dcm_p2b * dcm_g2p)
return {
'mass_prop_uncertainties': {
# Offset [m] for the simulated wing's center-of-mass in body
# coordinates.
'center_of_mass_offset': [0.0, 0.0, 0.0],
# Multiplier [#] on the kite mass for simulation.
'mass_scale': 1.0,
# Multipliers [#] on the kite inertia tensor for simulation.
'moment_of_inertia_scale': [1.0, 1.0, 1.0],
},
'Xg_0': np.dot(dcm_g2p.T, Xp_0).tolist()[0],
'Vb_0': [0.0, 0.0, 0.0],
'q_0': q_g2b.T.tolist()[0],
'omega_0': [0.0, 0.0, 0.0]
}
def MakeParams(params):
"""Make perch simulator parameters."""
# pylint: disable=invalid-name
A, B = physics_util.CalcTwoLinkageDynamics(
params['perch']['I_perch_and_drum'], params['perch']['I_drum'],
params['perch']['b_perch'], params['perch']['b_drum'])
flight_plan = params['flight_plan']
# Pick an initial azimuth [rad] in the ground frame to rotate the
# wing and the perch. azi_g is 0 when the wing is on the negative
# xg axis.
azi_g = 0.0
# Set perch angle based on flight plan.
if (flight_plan in [m.kFlightPlanStartDownwind]
and not params['sim_options'] & m.kSimOptConstraintSystem):
theta_p_0 = m.Wrap(azi_g, -np.pi, np.pi)
initialize_in_crosswind_config = True
elif flight_plan in [m.kFlightPlanDisengageEngage,
m.kFlightPlanHighHover,
m.kFlightPlanHoverInPlace,
m.kFlightPlanLaunchPerch,
m.kFlightPlanManual,
m.kFlightPlanTurnKey]:
theta_p_0 = m.Wrap(azi_g + np.pi, -np.pi, np.pi)
initialize_in_crosswind_config = False
else:
assert False
# The tophat has one perch azimuth encoder; GSv1 has none.
perch_azi_enabled = [
params['gs_model'] == system_types.kGroundStationModelTopHat, False]
return {
# Radius [m] of the levelwind.
'levelwind_radius': 1.5,
# Position [m] of the levelwind in perch x and y coordinates
# when the levelwind elevation is zero.
'levelwind_hub_p': [1.8, -1.5],
# Minimum tension [N] for the levelwind to engage.
'levelwind_engage_min_tension': 1e3,
# The matrices describing the linearized dynamics of the perch and
# winch drum system.
'A': {'d': A.tolist()},
'B': {'d': B.tolist()},
# Initial angle [rad] of the perch relative to ground coordinates.
'theta_p_0': theta_p_0,
# Boolean [#] that describes whether the perch begins in the
# crosswind configuration (i.e. winch drum angle is 0.0 rad) or
# a reeled-in configuration.
'initialize_in_crosswind_config': initialize_in_crosswind_config,
# Properties of the perch panel. The perch panel is modeled as
# the union of two cylinders, which are pitched and rolled about
# the perch axes by the specified angles, then truncated at
# planes parallel to the perch z-plane. The port cylinder
# corresponds to the port wing side and vice versa.
#
# Parameters are from hachtmann on 2015-08-21 (with corrections on
# 2015-09-22), and are confirmed using the drawings located here:
# go/makaniwiki/perch-geometry.
'panel': {
# For each panel, the center [m] and radius [m] describe the
# cylinder modeling it. The z_extents_p [m] specify the planes,
# parallel to the perch z-plane, at which the cylinders are
# cut.
'port': {
'center_panel': [3.122, 0.763],
'radius': 4.0,
'z_extents_p': [-0.625, 2.656],
},
'starboard': {
'center_panel': [3.203, -1.103],
'radius': 4.0,
'z_extents_p': [-0.306, 2.656],
},
'origin_pos_p': [0.0, 0.0, 0.0],
# Rotation matrix from the perch frame to the panel frame.
'dcm_p2panel': {'d': geometry.AngleToDcm(
np.deg2rad(0.0), np.deg2rad(6.0), np.deg2rad(-7.0)).tolist()},
# Y extents [m] of the panel apparatus in the panel coordinate
# system.
'y_extents_panel': [-1.9, 2.0],
},
# Sensor parameters for perch encoders. The biases of one or
# two degrees are estimated typical biases. The noise level is
# chosen to be pessimistic but not completely unrealistic.
'ts': params['common_params']['ts'],
'levelwind_ele_sensor': [MakeEncoderParams(), MakeEncoderParams()],
'perch_azi_sensor': [
MakeEncoderParams(bias=np.deg2rad(-1.0), noise_level_counts=0.25,
scale=1.0 if perch_azi_enabled[0] else 0.0),
MakeEncoderParams(bias=np.deg2rad(2.0), noise_level_counts=0.25,
scale=1.0 if perch_azi_enabled[1] else 0.0)]
}
def CalcTrim(self):
"""Find trim conditions for the wing.
Determines attitude trim for stabilized manual flight, assuming zero ambient
wind speed. The table below provides a rough idea of the expected trim
relationships.
Trim Input | Trim Output
----------------------+----------------------
roll | lateral acceleration
(glide angle, | (vertical acceleration,
freestream velocity) | horizontal acceleration)
aileron | roll moment
elevator | pitch moment
rudder | yaw moment
Returns:
A tuple (state, inputs) where state is a WingState and inputs is a
dynamics.WingInputs.
"""
state_0 = dynamics.WingState(omega_b=_Vec3Zero(),
dcm_g2b=geometry.AngleToDcm(
0.0, 0.0, 0.0),
wing_vel_g=np.matrix([20.0, 0.0, 0.0]).T,
wing_pos_g=_Vec3Zero())
inputs_0 = dynamics.WingInputs(thrust=np.matrix([[0.0]]),
motor_moment=_Vec3Zero(),
flaps=self._initial_flap_offsets.copy(),
wind_g=_Vec3Zero())
angle_of_attack = self._angle_of_attack
def GetTrimStateAndInputs(x):
"""Returns a WingState from trim variables."""
# Unpack trim variables.
glide_angle, dv_app, roll, d_aileron, d_elevator, d_rudder = x
# Calculate trim state.
v_app = 20.0 + dv_app
pitch = -glide_angle + angle_of_attack
yaw = 0.0
state = dynamics.WingState(omega_b=state_0.omega_b,
dcm_g2b=geometry.AngleToDcm(
yaw, pitch, roll),
wing_vel_g=np.matrix([
v_app * np.cos(glide_angle), 0.0,
v_app * np.sin(glide_angle)
]).T,
wing_pos_g=state_0.wing_pos_g)
# Calculate trim inputs.
flaps = inputs_0.flaps.copy()
flaps[[control_types.kFlapA1, control_types.kFlapA2]] += -d_aileron
flaps[[control_types.kFlapA7, control_types.kFlapA8]] += d_aileron
flaps[control_types.kFlapEle] += d_elevator
flaps[control_types.kFlapRud] += d_rudder
inputs = dynamics.WingInputs(thrust=inputs_0.thrust,
motor_moment=inputs_0.motor_moment,
flaps=flaps,
wind_g=inputs_0.wind_g)
return state, inputs
def TrimFunction(x):
"""Wrapper function for trimming the wing."""
state, inputs = GetTrimStateAndInputs(x)
state_dot = self._wing.CalcDeriv(state, inputs)
return [
state_dot.dwing_vel_g[0, 0], state_dot.dwing_vel_g[1, 0],
state_dot.dwing_vel_g[2, 0], state_dot.domega_b[0, 0],
state_dot.domega_b[1, 0], state_dot.domega_b[2, 0]
]
x = optimize.fsolve(TrimFunction, np.zeros((6, 1)))
return GetTrimStateAndInputs(x)
def MakeParams(params):
"""Make ground station GS02 parameters."""
# Origin of the panel frame in platform coordinates [m].
origin_pos_p = [0.288, 7.15, -4.332]
# Radius [m] of the cylinders that define the panels.
radius = 4.0
# For each panel, this is the angle [rad] the panel_origin-to-cylinder_center
# vector makes with the +x_panel direction, about the panel z-axis.
port_panel_angle = np.deg2rad(180.0 - 11.0)
star_panel_angle = np.deg2rad(180.0 + 16.0)
mclaren = params['mclaren_params']
mc = mclaren['mclaren']
d2r = lambda x: np.deg2rad(x).tolist()
# The setpoint for the detwist angle at start [deg].
detwist_setpoint = 203.0
# Tether free length [m] below which the proximity sensor will fire.
# This is hand-tuned in the sim to be 10cm past the free length at which the
# kite initializes on the perch, and depends on the bridle geometry.
if mconfig.WING_MODEL == 'oktoberkite':
# Manually tuned to get kite to perch.
prox_sensor_tether_free_length = 6.52
else:
prox_sensor_tether_free_length = 3.02
return {
# These parameters correspond to the model described in
# the "Azimuth response" section of go/makani-controls-gs02.
'mclaren': {
# Sample time [s] for the McLaren controller. The true sample time is
# 0.0096s, but that's close enough to the simulator sample time that
# we ignore the difference.
'ts': 0.01,
'detwist_setpoint': np.deg2rad(detwist_setpoint),
'reel': {
# Natural Frequency [rad/sec] and damping ratio [Nondim] for the
# azimuth command prefilter.
'azi_cmd_filter_omega':
5.0,
'azi_cmd_filter_zeta':
1.0,
# Gain [nondim] for the azimuth velocity feedforward path.
'azi_vel_cmd_ff_gain':
0.75,
# Saturation value for azimuth error [rad].
'azi_error_max':
np.deg2rad(2.0),
# Proportional gain [1/s] for the McLaren controller.
'azi_vel_cmd_kp':
1.5,
# Rate limit [rad/s^2] for the McLaren controller.
'azi_vel_cmd_rate_limit':
0.11,
# Angle [rad] by which the ground station azimuth is offset from
# the wing azimuth.
'azi_offset_from_wing':
(params['ground_station']['gs02']['reel_azi_offset_from_wing']
),
# Maximum drum angle [rad].
'drum_angle_upper_limit':
-1.977 * np.pi,
# Max commanded drum acceleration [rad/s^2].
'max_drum_accel':
(params['ground_station']['gs02']['max_drum_accel_in_reel']),
# Fraction of dead zone at which to zero azimuth position
# error. See the MAT controller parameter rAziLittleDeadzone.
'little_dead_zone':
1.0 / 3.0,
},
'transform': {
# Half-width [rad] of the dead zone to apply about azimuth target
# angles.
'azi_dead_zone_half_width':
d2r(mc['aDead_Azi_Transform']),
# Fraction [#] of the maximum acceleration to use when attenuating
# azimuth velocity commands.
'azi_decel_ratio':
mc['rDecel_Azi_Transform'],
# Maximum acceleration [rad/s^2] for azimuth commands.
'azi_max_accel':
mc['dnMax_Azi_Transform'],
# Nominal velocity command [rad/s] for the azimuth.
'azi_nominal_vel':
mc['nNominal_Azi_Transform'],
# Base offset angle [rad] of the platform from the wing azimuth.
'azi_offset_from_wing':
-np.pi,
# Target azimuth angles [rad] relative to azi_offset_from_wing for
# each stage of a HighTension to Reel transform.
'azi_targets_ht2reel':
d2r(mc['aTargetHT2Reel_Azi_Transform']),
# Target azimuth angles [rad] for each stage of a Reel to
# HighTension transform.
'azi_targets_reel2ht':
d2r(mc['aTargetReel2HT_Azi_Transform']),
# Tolerances [rad] for which to consider the azimuth target
# satisfied for each stage of a HighTension to Reel transform.
'azi_tols_ht2reel':
d2r(mc['aTolHT2Reel_Azi_Transform']),
# Tolerances [rad] for which to consider the azimuth target
# satisfied for each stage of a Reel to HighTension transform.
'azi_tols_reel2ht':
d2r(mc['aTolReel2HT_Azi_Transform']),
# Half-width [rad] of the dead zone to apply about winch target
# angles.
'winch_dead_zone_half_width':
d2r(mc['aDead_Winch_Transform']),
# Fraction [#] of the maximum acceleration to use when attenuating
# winch velocity commands.
'winch_decel_ratio':
mc['rDecel_Winch_Transform'],
# Maximum acceleration [rad/s^2] for winch commands.
'winch_max_accel':
mc['dnMax_Winch_Transform'],
# Nominal velocity command [rad/s] for the winch.
'winch_nominal_vel':
mc['nNominal_Winch_Transform'],
# Target winch angles [rad] for each stage of a HighTension to
# Reel transform.
'winch_targets_ht2reel':
d2r(mc['aTargetHT2Reel_Winch_Transform']),
# Target winch angles [rad] for each stage of a Reel to
# HighTension transform.
'winch_targets_reel2ht':
d2r(mc['aTargetReel2HT_Winch_Transform']),
# Tolerances [rad] for which to consider the winch target
# satisfied for each stage of a HighTension to Reel transform.
'winch_tols_ht2reel':
d2r(mc['aTolHT2Reel_Winch_Transform']),
# Tolerances [rad] for which to consider the winch target
# satisfied for each stage of a Reel to HighTension transform.
'winch_tols_reel2ht':
d2r(mc['aTolReel2HT_Winch_Transform']),
# Target detwist angles [rad] for each stage of a HighTension to
# Reel transform.
# The targets are ordered as [0, 1, 2, 3, 4]
'detwist_targets_ht2reel':
d2r([0.0, 165.0, 165.0, detwist_setpoint, detwist_setpoint]),
# Target detwist angles [rad] for each stage of a Reel to
# HighTension transform.
# The targets are ordered as [0, 4, 3, 2, 1]
'detwist_targets_reel2ht':
d2r([detwist_setpoint, 0.0, 165.0, 165.0, detwist_setpoint]),
# Max velocity [rad/s] for the detwist.
# See DetwistPro:Configuration.MaxTrackingVelocity_rps in file
# configuration.csv in
# Makani_Groundstation/UserPartition/permBackup/GS02-0x/
'detwist_max_vel':
1.257,
},
'high_tension': {
# Threshold brake torque [N-m] to switch from state machine
# case 0 to either case 1 or 6.
'm_max_azi_ht': 5000.0,
# Threshold azimuth error [rad] to switch from state machine
# case 0 to either case 1 or 6.
'a_control_threshold_azi_ht': 0.0873,
# Threshold total torque [N-m] to switch from state machine case 1
# to 2 or from case 6 to 7.
'm_control_threshold_azi_ht': 500.0,
# Nominal commanded azimuth angular rate [rad/s] in state machine
# cases 3 or 8.
'n_demand_azi_ht': 1.0,
# Threshold azimuth error [rad] to command state machine case 5
# from either cases 3 or 8.
'a_control_tolerance_azi_ht': 0.0262,
# Threshold azimuth rate [rad/s] to command state machine case 0.
'n_control_tolerance_azi_ht': 0.05,
# Maximum time [s] in state machine case 5 before switching to
# case 0.
't_threshold_wait_azi_ht': 10.0,
# Steady state non-zero azimuth rate [rad/s] based on GS02 test
# data.
'omega_nom': 0.122,
# Threshold azimuth rate [rad/s] for comparing the difference
# between azimuth rates of type double.
'test_threshold': 0.001,
# Desired azimuth 1st order spin up loop time constant [s].
'tau_spin': 0.6,
# Desired azimuth 1st order stopping loop time constant [s].
'tau_stop': 0.3,
# Ground station inertia about the azimuth axis [kg-m^2].
'Iz_gndstation': 9.44e4,
# 1st order spin up loop gain: Iz_gndstation/tau_spin [kg-m^2/s].
'k_spin': 1.573e5,
# 1st order stopping loop gain: Iz_gndstation/tau_stop [kg-m^2/s].
'k_stop': 3.147e5,
# Max velocity [rad/s] for the detwist.
# See DetwistPro:Configuration.MaxTrackingVelocity_rps in file
# configuration.csv in
# Makani_Groundstation/UserPartition/permBackup/GS02-0x/
'detwist_max_vel': 1.257,
},
},
# Proportional gain [1/s] for the drive controller model.
'azi_accel_kp':
9.5,
# Proportional gain [1/s] for the detwist controller model.
# The kp is retrieved from the flight log, by comparing detwist motor
# command and the detwist angle.
'detwist_angle_kp':
46.8,
# Natural frequency [Hz] and damping ratio [#] for a second-order model of
# the winch drive response. See the "Winch response" section of
# go/makani-controls-gs02.
'winch_drive_natural_freq':
np.sqrt(250.0),
'winch_drive_damping_ratio':
0.55,
# Rates [m/rad] at which the main- and wide-wrap sections of the
# tether track traverse the drum x-direction with respect to drum
# angle. These are derived from the camming chart:
# https://docs.google.com/spreadsheets/d/1Zbgf6RQRHo1KDB0dMvGqBsKi1cp5f8RNb6y5Gb4Mz6I"
#
# For a given lead angle alpha, a positive drum rotation of dtheta
# corresponds to a translation of the wrapping along the negative
# x-direction by r*tan(alpha).
'dx_dtheta_main_wrap':
-0.00696,
'dx_dtheta_wide_wrap':
-0.0733,
'panel': {
# For each panel, the center [m] in the panel frame and radius [m]
# describe the cylinder modeling it. The z_extents_p [m] specify the
# planes, parallel to the platform xy-plane, at which the cylinders
# are cut.
'port': {
'center_panel': [
radius * np.cos(port_panel_angle),
radius * np.sin(port_panel_angle)
],
'radius':
radius,
'z_extents_p': [origin_pos_p[2], origin_pos_p[2] + 3.0],
},
'starboard': {
'center_panel': [
radius * np.cos(star_panel_angle),
radius * np.sin(star_panel_angle)
],
'radius':
radius,
'z_extents_p': [origin_pos_p[2], origin_pos_p[2] + 3.0],
},
# These parameters define the panel frame relative to the platform
# frame. Its origin is the top of the panels along the bookseam (the
# seam between the panels). Then we apply a yaw rotation so that +x
# points along the nominal boom direction (see note below), and
# finally a pitch rotation corresponding to the inclination of the
# panels.
#
# Note: The actual boom direction is another 4 degrees from the
# platform +x-direction, but this is compensated up to 0.1 degrees
# by rotation of the panels. We ignore both the boom misalignment and
# the compensation.
'origin_pos_p': origin_pos_p,
'dcm_p2panel': {
'd':
geometry.AngleToDcm(np.deg2rad(90.0), np.deg2rad(-14.0),
0.0).tolist()
},
# Y extents [m] of the panel apparatus in the panel coordinate
# system. Visualizer only.
'y_extents_panel': [-1.9, 2.0],
},
# Radius [m] of the visualized platform.
'platform_radius':
3.0,
# Drum frame x-coordinate [m] at which the tether wrapping on the main
# body of the drum begins.
'wrap_start_posx_drum':
-1.985,
# Drum frame x-coordinate [m] of the transition between the wide-wrap
# section and the main-wrap section.
'wrap_transition_posx_drum':
-1.634,
# Initial angle [rad] for the platform azimuth.
'initial_platform_azi':
(params['phys_sim']['wind_direction'] + np.deg2rad(90.0) -
params['buoy_sim']['mooring_lines']['yaw_equilibrium_heading']),
# Initial angles [rad] for the drum. Has no effect if init_tether_tension
# is true.
'initial_drum_angle':
np.deg2rad(-170.0),
# Whether to choose an initial drum angle that yields a reasonable tether
# tension. Pre-empts initial_drum_angle.
'init_tether_tension':
True,
# Tether free length [m] below which the proximity sensor will fire.
'prox_sensor_tether_free_length':
prox_sensor_tether_free_length,
# The minimum tether elevation [deg] in the platform frame to engage the
# levelwind.
'min_levelwind_angle_for_tether_engagement':
np.deg2rad(-2.0),
}
def MakeParams(params):
"""Creates a dictionary containing buoy simulation parameters.
Args:
params: dict containing information about the physical buoy and sea model.
Returns:
dict containing information about the buoy position, hydrodynamic
characteristics, mooring line connectors, mass, and initial conditions.
"""
# Parameters for the 2019 off-shore demo buoy. See go/makani-buoy-sim-params
# for details.
# Initial buoy (vessel) attitude relative to its parent (ground) frame.
dcm_g2v_0 = geometry.AngleToDcm(0.0, 0.0, 0.0)
q_g2v_0 = geometry.DcmToQuat(dcm_g2v_0)
# Yaw restoring torque coefficients [s^-2]. See the section "Buoy Model
# Development" in go/makani-buoy-sim-params.
restoring_torque_coeff_z = 0.3843
# Damping torque coefficients [unitless].
damping_torque_coeff_x = -1.1649
damping_torque_coeff_y = -1.1649
damping_torque_coeff_z = -0.8328
# Effective mooring line attachment point [m].
mooring_attach_z_offset_from_com_v = 10.002
mooring_attach_pos_z_v = (params['buoy']['center_of_mass_pos'][2] +
mooring_attach_z_offset_from_com_v)
# Vertical position of the mean sea level.
# Assumes vessel and ground frames are coincidental at initialization.
water_density = params['sea']['water_density']
wet_height = (4.0 * params['buoy']['mass'] / np.pi / water_density /
params['buoy']['spar_diameter']**2)
msl_pos_z_g = params['buoy']['bottom_deck_pos_z_v'] - wet_height
return {
'msl_pos_z_g': msl_pos_z_g,
'hydrodynamics': {
# Torsional damping coefficients [N*m/(rad/s)^2].
'torsional_damping_x':
(damping_torque_coeff_x *
params['buoy']['inertia_tensor']['d'][0][0]),
'torsional_damping_y':
(damping_torque_coeff_y *
params['buoy']['inertia_tensor']['d'][1][1]),
'torsional_damping_z':
(damping_torque_coeff_z *
params['buoy']['inertia_tensor']['d'][2][2]),
# Buoyancy damping coefficient [N*s^2/m^2].
'buoyancy_damping_coeff':
9.0e4,
# Spar segment drag coefficient.
# Setting this to zero now (no hydrodynamic drag) and relying on
# damping coefficients instead.
# TODO: Obtain values for the drag coefficient.
'Cd':
0.0,
# Added mass parameters.
'Ca':
1.0, # Added mass coefficient [-].
'Dh':
6.8219, # Heave added mass efective diameter [m].
'ki':
1.0188, # Added inertia scaling factor [-].
'uncertainties': {
'torsional_damping_x_scale': 1.0,
'torsional_damping_y_scale': 1.0,
'torsional_damping_z_scale': 1.0,
'buoyancy_damping_coeff_scale': 1.0,
'Ca_scale': 1.0,
'Dh_scale': 1.0,
'ki_scale': 1.0,
},
},
'mooring_lines': {
# Definition of the heading [rad] of the +X axis of the vessel frame
# for which there is no restoring yaw torque, clockwise from North.
# This is assumed to be derived from the mooring system installation.
# The value in this parameter will override the yaw angle specified in
# the initial quaternion q_0 below, in order to have the buoy at rest
# at the initial time.
# TODO(b/144859123): Currently this is set to zero to work around a
# bug in which the vessel heading is used by the sim even in onshore
# scenarios, where it should have no effect. The long-term fix should
# be to remove the buoy model from onshore simulations entirely.
'yaw_equilibrium_heading':
np.deg2rad(0.0),
# For the coefficients below, see derivation in
# go/makani-buoy-sim-params. Equivalent yaw torsional stiffness
# coefficients [N*m/rad] accounting for hydrostatic action and for the
# mooring lines. The subscript indicates the axis of rotation, in
# vessel frame (v).
'torsional_stiffness_z':
restoring_torque_coeff_z *
params['buoy']['inertia_tensor']['d'][2][2],
# Mooring line attachment point [m] in vessel frame.
'mooring_attach_v': [0., 0., mooring_attach_pos_z_v],
# Mooring line model parameters.
'kt0':
2.2e+03, # Proportional spring coefficient [N/m].
'kt1':
32.0, # Quadratic spring coefficient [N/m^2].
'ct':
1.1078e+05, # Translational damping coefficient [N*s^2/m^2].
'uncertainties': {
'yaw_equilibrium_heading_delta': 0.0,
'torsional_stiffness_z_scale': 1.0,
'mooring_attach_pos_x_delta': 0.0,
'mooring_attach_pos_y_delta': 0.0,
'mooring_attach_pos_z_delta': 0.0,
'kt0_scale': 1.0,
'kt1_scale': 1.0,
'ct_scale': 1.0,
},
},
'mass_prop_uncertainties': {
# Offset [m] for the center-of-mass in vessel coordinates.
'center_of_mass_offset': [0.0, 0.0, 0.0],
# Buoy mass multiplier [#].
'mass_scale': 1.0,
# Buoy moment of inertia multipliers [#].
'moment_of_inertia_scale': [1.0, 1.0, 1.0],
},
# Initial buoy attitude.
# This attitude will be modified according to the value of the parameter
# yaw_equilibrium_heading above.
'q_0': q_g2v_0.T.tolist()[0],
# Initial buoy angular velocity [rad/s] relative to its parent (ground)
# frame, expressed in vessel coordinates.
'omega_0': [0.0, 0.0, 0.0],
# Initial buoy position [m] relative to the ground frame, expressed in
# ground coordinates.
'Xg_0': [0.0, 0.0, 0.0],
# Initial buoy velocity [m/s] relative to the ground frame, expressed in
# ground coordinates.
'Vg_0': [0.0, 0.0, 0.0],
}
def __init__(self, rotor_params, pos_com_b):
self._dcm_r2b = geometry.AngleToDcm(
0.0,
np.arctan2(rotor_params[0]['axis'][2], rotor_params[0]['axis'][0]),
0.0)
self._pos_com_b = np.matrix(pos_com_b).T
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}
def __init__(self, system_params, control_params, sim_params):
"""Constructs a ControlDesign object from parameters."""
self._tether_length = system_params['tether']['length']
# Determine the path parameters.
ti_origin_g = np.matrix([[0.0], [0.0], [0.0]])
# We currently assume no wind in the trans-in script, so the trans-in start
# azimuth should not matter. A starting azimuth should be specified if we
# want to account for wind speed in this script.
# TODO: Use playbook entry to calculate accel start azimuth.
dcm_g2ti = geometry.AngleToDcm(0.0, 0.0, 0.0)
self._trans_in_frame = TransInFrame(dcm_g2ti, ti_origin_g)
self._wing_area = system_params['wing']['A']
self._wing_span = system_params['wing']['b']
self._wing_chord = system_params['wing']['c']
self._air_density = system_params['phys']['rho']
rotor_databases = [
physics.RotorDatabase(
physics.GetRotorDatabase(
sim_params['rotor_sim']['database_names'][i]['name']))
for i in range(control_types.kNumMotors)
]
self._motor_model = dynamics.MotorMixerMotorModel(
rotor_databases, self._air_density,
control_params['trans_in']['output']['thrust_moment_weights'],
system_params['rotors'], control_params['rotor_control'])
self._thrust_cmd = control_params['trans_in']['longitudinal'][
'thrust_cmd']
if system_params['gs_model'] == system_types.kGroundStationModelTopHat:
gsg_pos_g = (
np.array(system_params['perch']['winch_drum_origin_p']) +
np.array(system_params['perch']['gsg_pos_wd']))
elif system_params['gs_model'] == system_types.kGroundStationModelGSv2:
gs02_params = system_params['ground_station']['gs02']
# We assume the platform frame is aligned with the g-frame, i.e. the
# platform azimuth is zero.
gsg_pos_g = (np.array(gs02_params['drum_origin_p']) +
np.array(gs02_params['gsg_pos_drum']))
else:
assert False, 'Invalid GS model.'
tether_model = dynamics.CatenaryTetherForceModel(
system_params['tether'], gsg_pos_g,
system_params['wing']['bridle_rad'], system_params['phys']['g'],
system_params['phys']['rho'])
self._wing = dynamics.Wing(system_params, sim_params,
dynamics.SwigAeroModel(), self._motor_model,
tether_model)
self._wing_weight = system_params['wing']['m'] * system_params['phys'][
'g']
self._motor_yaw_lever_arm = np.abs(
system_params['rotors'][control_types.kMotorSbo]['pos'][1] +
system_params['rotors'][control_types.kMotorSbi]['pos'][1]) / 2.0
flap_offsets = np.deg2rad(
np.array([-11.5, -11.5, -11.5, -11.5, -11.5, -11.5, 0.0, 0.0]))
assert len(flap_offsets) == 8
self._midboard_flap_ratio = control_params['trans_in']['attitude'][
'midboard_flap_ratio']
self._flap_offsets = np.matrix(np.zeros((control_types.kNumFlaps, 1)))
self._flap_offsets[control_types.kFlapA1] = flap_offsets[0]
self._flap_offsets[control_types.kFlapA2] = flap_offsets[1]
self._flap_offsets[control_types.kFlapA4] = flap_offsets[2]
self._flap_offsets[control_types.kFlapA5] = flap_offsets[3]
self._flap_offsets[control_types.kFlapA7] = flap_offsets[4]
self._flap_offsets[control_types.kFlapA8] = flap_offsets[5]
self._flap_offsets[control_types.kFlapEle] = flap_offsets[6]
self._flap_offsets[control_types.kFlapRud] = flap_offsets[7]