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]