11"""
22 *
3- * Copyright (c) 2021 Manuel Galliker
3+ * Copyright (c) 2021 Manuel Yves Galliker
44 * 2021 Autonomous Systems Lab ETH Zurich
55 * All rights reserved.
66 * Redistribution and use in source and binary forms, with or without
@@ -57,14 +57,17 @@ def compute_actuator_force_features(self, index, v_airspeed, angle_of_attack):
5757 Model description:
5858 """
5959 actuator_input = self .actuator_input_vec [index ]
60- q_xz = 0.5 * self .air_density * (v_airspeed [0 ]** 2 + v_airspeed [2 ]** 2 ) #TODO Take dynamic pressure
60+ q_xz = 0.5 * self .air_density * \
61+ (v_airspeed [0 ]** 2 + v_airspeed [2 ]** 2 ) # TODO Take dynamic pressure
6162 # TODO Compute lift axis and drag axis
6263 lift_axis = np .array ([v_airspeed [0 ], 0.0 , v_airspeed [2 ]])
6364 lift_axis = (lift_axis / np .linalg .norm (lift_axis )).reshape ((3 , 1 ))
64- drag_axis = (- 1.0 * v_airspeed / np .linalg .norm (v_airspeed )).reshape ((3 , 1 ))
65- X_lift = lift_axis @ np .array ([[actuator_input ]]) * q_xz * self .area
66- X_drag = drag_axis @ np .array ([[actuator_input ]]) * q_xz * self .area
67- R_aero_to_body = Rotation .from_rotvec ([0 , - angle_of_attack , 0 ]).as_matrix ()
65+ drag_axis = (- 1.0 * v_airspeed /
66+ np .linalg .norm (v_airspeed )).reshape ((3 , 1 ))
67+ X_lift = lift_axis @ np .array ([[actuator_input ]]) * q_xz * self .area
68+ X_drag = drag_axis @ np .array ([[actuator_input ]]) * q_xz * self .area
69+ R_aero_to_body = Rotation .from_rotvec (
70+ [0 , - angle_of_attack , 0 ]).as_matrix ()
6871 X_lift_body = R_aero_to_body @ X_lift
6972 X_drag_body = R_aero_to_body @ X_drag
7073
@@ -75,15 +78,21 @@ def compute_actuator_moment_features(self, index, v_airspeed, angle_of_attack):
7578 Model description:
7679 """
7780 actuator_input = self .actuator_input_vec [index ]
78- q_xz = 0.5 * self .air_density * (v_airspeed [0 ]** 2 + v_airspeed [2 ]** 2 ) #TODO Take dynamic pressure
81+ q_xz = 0.5 * self .air_density * \
82+ (v_airspeed [0 ]** 2 + v_airspeed [2 ]** 2 ) # TODO Take dynamic pressure
7983 lift_axis = np .array ([v_airspeed [0 ], 0.0 , v_airspeed [2 ]])
8084 yaw_axis = (lift_axis / np .linalg .norm (lift_axis )).reshape ((3 , 1 ))
81- roll_axis = ( v_airspeed / np .linalg .norm (v_airspeed )).reshape ((3 , 1 ))
82- pitch_axis = np .cross (np .transpose (yaw_axis ), np .transpose (roll_axis )).reshape ((3 , 1 ))
83- X_roll_moment = roll_axis @ np .array ([[actuator_input ]]) * q_xz * self .area
84- X_pitch_moment = pitch_axis @ np .array ([[actuator_input ]]) * q_xz * self .area
85- X_yaw_moment = yaw_axis @ np .array ([[actuator_input ]]) * q_xz * self .area
86- R_aero_to_body = Rotation .from_rotvec ([0 , - angle_of_attack , 0 ]).as_matrix ()
85+ roll_axis = (v_airspeed / np .linalg .norm (v_airspeed )).reshape ((3 , 1 ))
86+ pitch_axis = np .cross (np .transpose (yaw_axis ),
87+ np .transpose (roll_axis )).reshape ((3 , 1 ))
88+ X_roll_moment = roll_axis @ np .array (
89+ [[actuator_input ]]) * q_xz * self .area
90+ X_pitch_moment = pitch_axis @ np .array (
91+ [[actuator_input ]]) * q_xz * self .area
92+ X_yaw_moment = yaw_axis @ np .array ([[actuator_input ]]
93+ ) * q_xz * self .area
94+ R_aero_to_body = Rotation .from_rotvec (
95+ [0 , - angle_of_attack , 0 ]).as_matrix ()
8796 X_roll_moment_body = R_aero_to_body @ X_roll_moment
8897 X_pitch_moment_body = R_aero_to_body @ X_pitch_moment
8998 X_yaw_moment_body = R_aero_to_body @ X_yaw_moment
@@ -93,11 +102,13 @@ def compute_actuator_moment_features(self, index, v_airspeed, angle_of_attack):
93102 def compute_actuator_force_matrix (self , v_airspeed_mat , angle_of_attack_vec ):
94103 print ("Computing force features for control surface:" , self .name )
95104
96- X_forces = self .compute_actuator_force_features (0 , v_airspeed_mat [0 , :], angle_of_attack_vec [0 , :])
105+ X_forces = self .compute_actuator_force_features (
106+ 0 , v_airspeed_mat [0 , :], angle_of_attack_vec [0 , :])
97107 rotor_features_bar = Bar (
98108 'Feature Computatiuon' , max = self .actuator_input_vec .shape [0 ])
99109 for index in range (1 , self .n_timestamps ):
100- X_force_curr = self .compute_actuator_force_features (index , v_airspeed_mat [index , :], angle_of_attack_vec [index , :])
110+ X_force_curr = self .compute_actuator_force_features (
111+ index , v_airspeed_mat [index , :], angle_of_attack_vec [index , :])
101112 X_forces = np .vstack ((X_forces , X_force_curr ))
102113 rotor_features_bar .next ()
103114 rotor_features_bar .finish ()
@@ -109,11 +120,13 @@ def compute_actuator_force_matrix(self, v_airspeed_mat, angle_of_attack_vec):
109120 def compute_actuator_moment_matrix (self , v_airspeed_mat , angle_of_attack_vec ):
110121 print ("Computing moment features for control surface:" , self .name )
111122
112- X_moments = self .compute_actuator_moment_features (0 , v_airspeed_mat [0 , :], angle_of_attack_vec [0 , :])
123+ X_moments = self .compute_actuator_moment_features (
124+ 0 , v_airspeed_mat [0 , :], angle_of_attack_vec [0 , :])
113125 rotor_features_bar = Bar (
114126 'Feature Computatiuon' , max = self .actuator_input_vec .shape [0 ])
115127 for index in range (1 , self .n_timestamps ):
116- X_moment_curr = self .compute_actuator_moment_features (index , v_airspeed_mat [index , :], angle_of_attack_vec [index , :])
128+ X_moment_curr = self .compute_actuator_moment_features (
129+ index , v_airspeed_mat [index , :], angle_of_attack_vec [index , :])
117130 X_moments = np .vstack ((X_moments , X_moment_curr ))
118131 rotor_features_bar .next ()
119132 rotor_features_bar .finish ()
0 commit comments