diff --git a/Tools/parametric_model/generate_parametric_model.py b/Tools/parametric_model/generate_parametric_model.py index bf9b2999..14772537 100644 --- a/Tools/parametric_model/generate_parametric_model.py +++ b/Tools/parametric_model/generate_parametric_model.py @@ -4,7 +4,7 @@ __license__ = "BSD 3" -from src.models import QuadRotorModel, QuadPlaneModel, DeltaQuadPlaneModel +from src.models import QuadRotorModel, QuadPlaneModel, DeltaQuadPlaneModel, TiltWingModel import argparse @@ -21,6 +21,9 @@ def start_model_estimation(arg_list): elif (model == "delta_quad_plane_model"): model = DeltaQuadPlaneModel(rel_ulog_path) + elif (model == "tilt_wing_model"): + model = TiltWingModel(rel_ulog_path) + else: print("no valid model selected") diff --git a/Tools/parametric_model/src/models/__init__.py b/Tools/parametric_model/src/models/__init__.py index 02724fc0..3df10672 100644 --- a/Tools/parametric_model/src/models/__init__.py +++ b/Tools/parametric_model/src/models/__init__.py @@ -16,3 +16,5 @@ from .quadrotor_model import QuadRotorModel from . import model_config from . model_config import ModelConfig +from . import tilt_wing_model +from . tilt_wing_model import TiltWingModel diff --git a/Tools/parametric_model/src/models/__init__.pyc b/Tools/parametric_model/src/models/__init__.pyc deleted file mode 100644 index 7472d6e4..00000000 Binary files a/Tools/parametric_model/src/models/__init__.pyc and /dev/null differ diff --git a/Tools/parametric_model/src/models/aerodynamic_models/__init__.py b/Tools/parametric_model/src/models/aerodynamic_models/__init__.py index ab8f6b0e..a86a772f 100644 --- a/Tools/parametric_model/src/models/aerodynamic_models/__init__.py +++ b/Tools/parametric_model/src/models/aerodynamic_models/__init__.py @@ -10,3 +10,5 @@ from .aero_model_Delta import AeroModelDelta from . import simple_drag_model from .simple_drag_model import SimpleDragModel +from . import tilt_wing_section_aero_model +from .tilt_wing_section_aero_model import TiltWingSection diff --git a/Tools/parametric_model/src/models/aerodynamic_models/__init__.pyc b/Tools/parametric_model/src/models/aerodynamic_models/__init__.pyc deleted file mode 100644 index 774d3eab..00000000 Binary files a/Tools/parametric_model/src/models/aerodynamic_models/__init__.pyc and /dev/null differ diff --git a/Tools/parametric_model/src/models/aerodynamic_models/aero_model_AAE.py b/Tools/parametric_model/src/models/aerodynamic_models/aero_model_AAE.py index e612264c..dee7b8f3 100644 --- a/Tools/parametric_model/src/models/aerodynamic_models/aero_model_AAE.py +++ b/Tools/parametric_model/src/models/aerodynamic_models/aero_model_AAE.py @@ -59,9 +59,8 @@ def compute_main_wing_feature(self, v_airspeed, angle_of_attack): 1 - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*angle_of_attack*v_xz**2 F_xz_aero_frame[2, 6] = -( 1 - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*v_xz**2 - F_xz_aero_frame[2, 7] = -2 * \ - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac) \ - * math.sin(angle_of_attack)*math.cos(angle_of_attack)*v_xz**2 + F_xz_aero_frame[2, 7] = -cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac) \ + * math.sin(2*angle_of_attack)*v_xz**2 # Transorm from stability axis frame to body FRD frame R_aero_to_body = Rotation.from_rotvec( diff --git a/Tools/parametric_model/src/models/aerodynamic_models/aero_model_Delta.py b/Tools/parametric_model/src/models/aerodynamic_models/aero_model_Delta.py index 5ffa9b08..f870f591 100644 --- a/Tools/parametric_model/src/models/aerodynamic_models/aero_model_Delta.py +++ b/Tools/parametric_model/src/models/aerodynamic_models/aero_model_Delta.py @@ -60,9 +60,8 @@ def compute_main_wing_feature(self, v_airspeed, angle_of_attack): 1 - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*angle_of_attack*v_xz**2 F_xz_aero_frame[2, 6] = -( 1 - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*v_xz**2 - F_xz_aero_frame[2, 7] = -2 * \ - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac) \ - * math.sin(angle_of_attack)*math.cos(angle_of_attack)*v_xz**2 + F_xz_aero_frame[2, 7] = -cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac) \ + * math.sin(2*angle_of_attack)*v_xz**2 # Transorm from stability axis frame to body FRD frame R_aero_to_body = Rotation.from_rotvec( diff --git a/Tools/parametric_model/src/models/aerodynamic_models/tilt_wing_section_aero_model.py b/Tools/parametric_model/src/models/aerodynamic_models/tilt_wing_section_aero_model.py new file mode 100644 index 00000000..666494f6 --- /dev/null +++ b/Tools/parametric_model/src/models/aerodynamic_models/tilt_wing_section_aero_model.py @@ -0,0 +1,152 @@ +__author__ = "Manuel Galliker" +__maintainer__ = "Manuel Galliker" +__license__ = "BSD 3" + +import math +import numpy as np + +from ...tools import cropped_sym_sigmoid +from scipy.spatial.transform import Rotation +from progress.bar import Bar + +# AAE stands for aileron, aileron, elevator + + +class TiltWingSection(): + def __init__(self, wing_section_config, v_airspeed_mat, air_density=1.225, angular_vel_mat=None, rotor=None): + self.stall_angle = wing_section_config["stall_angle_deg"]*math.pi/180.0 + self.sig_scale_fac = wing_section_config["sig_scale_factor"] + self.air_density = air_density + self.n_timestamps = v_airspeed_mat.shape[0] + self.cp_position = np.array( + wing_section_config["cp_position"]).reshape(3, 1) + self.description = wing_section_config["description"] + self.aero_coef_list = ["c_d_wing_xz_offset", "c_d_wing_xz_lin", "c_d_wing_xz_quad", "c_d_wing_xz_stall_min", "c_d_wing_xz_stall_90_deg", + "c_l_wing_xz_offset", "c_l_wing_xz_lin", "c_l_wing_xz_stall", "c_d_wing_y_offset"] + + # upstream rotor influencing the downwash over wing section. + if rotor == None: + self.in_downstream = False + else: + self.in_downstream = True + self.rotor = rotor + + self.compute_static_local_airspeed(v_airspeed_mat, angular_vel_mat) + + def compute_static_local_airspeed(self, v_airspeed_mat, angular_vel_mat): + """ + The only component of the airspeed chaning during optimization is the part due to the actuator downwash. + The other "static" components (airspeed due to groundspeed, wind and angular velocity can be precomputed in this function""" + + self.static_local_airspeed_mat = np.zeros(v_airspeed_mat.shape) + + if angular_vel_mat is not None: + assert (v_airspeed_mat.shape == + angular_vel_mat.shape), "RotorModel: v_airspeed_mat and angular_vel_mat differ in size." + for i in range(self.n_timestamps): + self.static_local_airspeed_mat[i, :] = v_airspeed_mat[i, :] + \ + np.cross(angular_vel_mat[i, :], + self.cp_position.flatten()) + + def update_local_airspeed_and_aoa(self, rotor_thrust_coef): + + self.local_airspeed_mat = np.zeros( + self.static_local_airspeed_mat.shape) + self.local_aoa_vec = np.zeros(self.static_local_airspeed_mat.shape[0]) + rotor_thrust_mat = self.rotor.predict_thrust_force(rotor_thrust_coef) + if self.in_downstream: + for i in range(self.n_timestamps): + abs_thrust = np.linalg.norm(rotor_thrust_mat[i, :]) + v_air_parr = self.rotor.v_air_parallel_abs[i] + v_downwash = self.rotor.rotor_axis * (-v_air_parr + math.sqrt(v_air_parr**2 + ( + 2*abs_thrust/(self.rotor.air_density*self.rotor.prop_area)))) + self.local_airspeed_mat[i, :] = self.static_local_airspeed_mat[i, + :] + v_downwash.flatten() + self.local_aoa_vec[i] = math.atan2( + self.local_airspeed_mat[i, 2], self.local_airspeed_mat[i, 0]) + + def predict_single_wing_segment_feature_mat(self, v_airspeed, angle_of_attack): + """ + Model description: + + Compute lift and drag forces in stability axis frame. + + This is done by interpolating two models: + 1. More suffisticated Model for abs(AoA) < stall_angle + + - Lift force coefficient as linear function of AoA: + F_Lift = 0.5 * density * area * \ + V_air_xz^2 * (c_l_0 + c_l_lin*AoA) + + - Drag force coefficient as quadratic function of AoA + F_Drag = 0.5 * density * area * V_air_xz^2 * \ + (c_d_0 + c_d_lin * AoA + c_d_quad * AoA^2) + + 2. Simple plate model for abs(AoA) > stall_angle + F_Lift = density * area * V_air_xz^2 * \ + cos(AoA) * sin(AoA) * c_l_stall + F_Drag = 0.5 * density * area * \ + V_air_xz^2 * sin(AoA) * c_d_stall + + The two models are interpolated with a symmetric sigmoid function obtained by multiplying two logistic functions: + if abs(AoA) < stall_angle: cropped_sym_sigmoid(AoA) = 0 + if abs(AoA) > stall_angle: cropped_sym_sigmoid(AoA) = 1 + """ + + v_xz = math.sqrt(v_airspeed[0]**2 + v_airspeed[2]**2) + X_xz_aero_frame = np.zeros((3, 8)) + + # Compute Drag force coeffiecients: + X_xz_aero_frame[0, 0] = -( + 1 - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*v_xz**2 + X_xz_aero_frame[0, 1] = -( + 1 - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*angle_of_attack*v_xz**2 + X_xz_aero_frame[0, 2] = -( + 1 - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*angle_of_attack**2*v_xz**2 + X_xz_aero_frame[0, 3] = -(cropped_sym_sigmoid(angle_of_attack, + x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*(1 - math.sin(angle_of_attack)**2)*v_xz**2 + X_xz_aero_frame[0, 3] = -(cropped_sym_sigmoid(angle_of_attack, + x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*(1 - math.sin(angle_of_attack)**2)*v_xz**2 + X_xz_aero_frame[0, 4] = -(cropped_sym_sigmoid(angle_of_attack, + x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*(math.sin(angle_of_attack)**2)*v_xz**2 + + # Compute Lift force coefficients: + X_xz_aero_frame[2, 5] = -( + 1 - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*angle_of_attack*v_xz**2 + X_xz_aero_frame[2, 6] = -( + 1 - cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac))*v_xz**2 + X_xz_aero_frame[2, 7] = -cropped_sym_sigmoid(angle_of_attack, x_offset=self.stall_angle, scale_fac=self.sig_scale_fac) \ + * math.sin(2*angle_of_attack)*v_xz**2 + + # Transorm from stability axis frame to body FRD frame + R_aero_to_body = Rotation.from_rotvec( + [0, -angle_of_attack, 0]).as_matrix() + X_xz_body_frame = R_aero_to_body @ X_xz_aero_frame + + """ + Compute drag in y direction of body frame using a single coefficient: + F_y = 0.5 * density * area * V_air_y^2 * c_d_y""" + X_y_body_frame = np.array([0, -math.copysign( + 1, v_airspeed[1]) * v_airspeed[1]**2, 0]).reshape(3, 1) + X_wing_segment = np.hstack((X_xz_body_frame, X_y_body_frame)) + return X_wing_segment + + def predict_wing_segment_forces(self, rotor_thrust_coef, aero_force_coef): + """ + Inputs: + + v_airspeed_mat: numpy array of dimension (n,3) with columns for [v_a_x, v_a_y, v_a_z] + angle_of_attack_vec: vector of size (n) with corresponding AoA values + flap_commands = numpy array of dimension (n,3) with columns for [u_aileron_right, u_aileron_left, u_elevator] + """ + aero_force_coef = aero_force_coef.reshape( + (aero_force_coef.shape[0], 1)) + self.update_local_airspeed_and_aoa(rotor_thrust_coef) + X_aero = self.predict_single_wing_segment_feature_mat( + self.local_airspeed_mat[0, :], self.local_aoa_vec[0]) + for i in range(1, self.n_timestamps): + X_curr = self.predict_single_wing_segment_feature_mat( + self.local_airspeed_mat[0, :], self.local_aoa_vec[0]) + X_aero = np.vstack((X_aero, X_curr)) + F_aero_force_pred = X_aero @ aero_force_coef + return F_aero_force_pred diff --git a/Tools/parametric_model/src/models/dynamics_model.py b/Tools/parametric_model/src/models/dynamics_model.py index 9d1eff91..b700373a 100644 --- a/Tools/parametric_model/src/models/dynamics_model.py +++ b/Tools/parametric_model/src/models/dynamics_model.py @@ -52,9 +52,7 @@ def __init__(self, config_dict, rel_data_path): print(rel_data_path) exit(1) - self.quat_columns = self.get_topic_list_from_topic_type( - "vehicle_attitude") - self.quaternion_df = self.data_df[self.quat_columns] + self.quaternion_df = self.data_df[["q0", "q1", "q2", "q3"]] self.q_mat = self.quaternion_df.to_numpy() # used to generate a dict with the resulting coefficients later on. @@ -180,9 +178,16 @@ def normalize_actuators(self): exit(1) self.data_df[self.actuator_columns[i]] = actuator_data + def initialize_rotor_model(self, rotor_config_dict, rotor_group, angular_vel_mat=None): + rotor_input_name = rotor_config_dict["dataframe_name"] + u_vec = self.data_df[rotor_input_name].to_numpy() + rotor = RotorModel( + rotor_config_dict, u_vec, self.v_airspeed_mat, angular_vel_mat=angular_vel_mat) + return rotor + def compute_rotor_features(self, rotors_config_dict, angular_vel_mat=None): - v_airspeed_mat = self.data_df[[ + self.v_airspeed_mat = self.data_df[[ "V_air_body_x", "V_air_body_y", "V_air_body_z"]].to_numpy() self.rotor_dict = {} @@ -190,15 +195,15 @@ def compute_rotor_features(self, rotors_config_dict, angular_vel_mat=None): rotor_group_list = rotors_config_dict[rotor_group] self.rotor_dict[rotor_group] = {} if (self.estimate_forces): - X_force_collector = np.zeros((3*v_airspeed_mat.shape[0], 3)) + X_force_collector = np.zeros( + (3*self.v_airspeed_mat.shape[0], 3)) if (self.estimate_moments): - X_moment_collector = np.zeros((3*v_airspeed_mat.shape[0], 5)) + X_moment_collector = np.zeros( + (3*self.v_airspeed_mat.shape[0], 5)) for rotor_config_dict in rotor_group_list: - rotor_input_name = rotor_config_dict["dataframe_name"] - u_vec = self.data_df[rotor_input_name] - rotor = RotorModel( - rotor_config_dict, u_vec, v_airspeed_mat, angular_vel_mat=angular_vel_mat) - self.rotor_dict[rotor_group][rotor_input_name] = rotor + rotor = self.initialize_rotor_model( + rotor_config_dict, rotor_group, angular_vel_mat) + self.rotor_dict[rotor_group][rotor_config_dict["dataframe_name"]] = rotor if (self.estimate_forces): X_force_curr, curr_rotor_forces_coef_list = rotor.compute_actuator_force_matrix() diff --git a/Tools/parametric_model/src/models/model_config/qpm_delta_vtol_config.yaml b/Tools/parametric_model/src/models/model_config/qpm_delta_vtol_config.yaml index 54bf3eaf..13a57bc4 100644 --- a/Tools/parametric_model/src/models/model_config/qpm_delta_vtol_config.yaml +++ b/Tools/parametric_model/src/models/model_config/qpm_delta_vtol_config.yaml @@ -1,9 +1,9 @@ --- # general information -model_name: "Gazebo Standart VTOL" -model_type: "Standard VTOL" +model_name: "Gazebo Delta VTOL" +model_type: "Delta VTOL" model_file: - "quad_plane_model.py" + "delta_quad_plane_model.py" # all vectors in FRD body frame if not specified otherwise model_config: diff --git a/Tools/parametric_model/src/models/model_config/tilt_wing_config.yaml b/Tools/parametric_model/src/models/model_config/tilt_wing_config.yaml new file mode 100644 index 00000000..53567ef7 --- /dev/null +++ b/Tools/parametric_model/src/models/model_config/tilt_wing_config.yaml @@ -0,0 +1,287 @@ +--- +# general information +model_name: "Tilt Wing VTOL" +model_type: "Tilt Wing" +model_file: + "tilt_wing_model.py" + + # all vectors in FRD body frame if not specified otherwise +model_config: + optimzation_parameters: + initial_coefficients: + wing_rot_drag_lin: 1.0 + wing_rot_thrust_quad: 10.0 + wing_rot_thrust_lin: -1.0 + tail_rot_drag_lin: 1.0 + tail_rot_thrust_quad: 10.0 + tail_rot_thrust_lin: -1.0 + c_d_wing_xz_offset: 1.0 + c_d_wing_xz_lin: 1.0 + c_d_wing_xz_quad: 1.0 + c_d_wing_xz_stall_min: 1.0 + c_d_wing_xz_stall_90_deg: 1.0 + c_l_wing_xz_offset: 0.0 + c_l_wing_xz_lin: 1.0 + c_l_wing_xz_stall: 1.0 + c_d_wing_y_offset: 1.0 + coefficient_bounds: + wing_rot_drag_lin: !!python/tuple [0, 10] + wing_rot_thrust_quad: !!python/tuple [0, 100] + wing_rot_thrust_lin: !!python/tuple [-10, 0] + tail_rot_drag_lin: !!python/tuple [0, 10] + tail_rot_thrust_quad: !!python/tuple [0, 100] + tail_rot_thrust_lin: !!python/tuple [-10, 0] + c_d_wing_xz_offset: !!python/tuple [-10, 10] + c_d_wing_xz_lin: !!python/tuple [0, 10] + c_d_wing_xz_quad: !!python/tuple [0, 10] + c_d_wing_xz_stall_min: !!python/tuple [0, 10] + c_d_wing_xz_stall_90_deg: !!python/tuple [0, 10] + c_l_wing_xz_offset: !!python/tuple [-1, 1] + c_l_wing_xz_lin: !!python/tuple [0, 10] + c_l_wing_xz_stall: !!python/tuple [0, 10] + c_d_wing_y_offset: !!python/tuple [0, 10] + mass: 6.1 + actuators: + rotors: + # All rotors in the same group will share the coefficients + wing_: + - rotor_0: + description: "rotor wing right, right" + dataframe_name: "u0" + # rotor_axis corresponds to having 0 wing_tilt, e.g. the wing facing forward + rotor_axis: + - 1 + - 0 + - 0 + tilt_axis: + - 0 + - 1 + - 0 + max_tilt_angle_deg: 90 + turning_direction: -1 + position: + - 0.35 + - 0.35 + - -0.07 + + - rotor_1: + description: "rotor wing right, left" + dataframe_name: "u1" + rotor_axis: + - 1 + - 0 + - 0 + tilt_axis: + - 0 + - 1 + - 0 + max_tilt_angle_deg: 90 + turning_direction: -1 + position: + - -0.35 + - -0.35 + - -0.07 + + - rotor_2: + description: "rotor wing left, right" + dataframe_name: "u2" + rotor_axis: + - 1 + - 0 + - 0 + tilt_axis: + - 0 + - 1 + - 0 + max_tilt_angle_deg: 90 + turning_direction: 1 + position: + - 0.35 + - -0.35 + - -0.07 + + - rotor_3: + description: "rotor wing left, left" + dataframe_name: "u3" + rotor_axis: + - 1 + - 0 + - 0 + tilt_axis: + - 0 + - 1 + - 0 + max_tilt_angle_deg: 90 + turning_direction: 1 + position: + - -0.35 + - 0.35 + - -0.07 + + tail_: + - rotor_4: + description: "rotor tail" + dataframe_name: "u4" + rotor_axis: + - 0 + - 0 + - -1 + turning_direction: 1 + position: + - 0.35 + - 0.35 + - -0.07 + + control_surfaces: + - aileron_right: + description: "aileron right, positive up" + dataframe_name: "u5" + + - flap_right: + description: "flap right, positive up" + dataframe_name: "u6" + + - flap_left: + description: "flap left, positive up" + dataframe_name: "u7" + + - aileron_left: + description: "aileron left, positive up" + dataframe_name: "u8" + + - elevator: + description: "elevator, positive up" + dataframe_name: "u9" + + wing_tilt: + - wing_tilt: + description: "wing tilt, -1 to 1 = cruise (0°) to hover (90°) configuration" + dataframe_name: "u_tilt" + + aerodynamics: + stall_angle_deg: 20 + sig_scale_factor: 30 + main_wing_sections: + - wing_right_right: + rotor: "u0" + cp_position: + - 0 + - 0 + - 0 + stall_angle_deg: 20 + sig_scale_factor: 30 + description: "wing segment right, right" + + - wing_right_left: + rotor: "u1" + cp_position: + - 0 + - 0 + - 0 + stall_angle_deg: 20 + sig_scale_factor: 30 + description: "wing segment right, left" + + - wing_left_right: + rotor: "u2" + cp_position: + - 0 + - 0 + - 0 + stall_angle_deg: 20 + sig_scale_factor: 30 + description: "wing segment left, right" + + - wing_left_left: + rotor: "u3" + cp_position: + - 0 + - 0 + - 0 + stall_angle_deg: 20 + sig_scale_factor: 30 + description: "wing segment left, left" + +dynamics_model_config: + estimate_forces: True + estimate_moments: False + resample_freq: 1.0 + data: + required_ulog_topics: + actuator_controls_0: + ulog_name: + - "timestamp" + - "control[0]" + - "control[1]" + - "control[2]" + - "control[4]" + - "control[5]" + - "control[6]" + - "control[7]" + dataframe_name: + - "timestamp" + - "u0" + - "u1" + - "u2" + - "u5" + - "u6" + - "u7" + - "u8" + actuator_type: + - "timestamp" + - "motor" + - "motor" + - "motor" + - "control_surcafe" + - "control_surcafe" + - "control_surcafe" + - "control_surcafe" + actuator_controls_1: + ulog_name: + - "timestamp" + - "control[0]" + - "control[1]" + - "control[4]" + - "control[5]" + dataframe_name: + - "timestamp" + - "u9" + - "u_tilt" + - "u4" + - "u3" + actuator_type: + - "timestamp" + - "control_surcafe" + - "motor" # convert wing_Tilt to 0 to 1 + - "motor" + - "motor" + vehicle_local_position: + ulog_name: + - "timestamp" + - "vx" + - "vy" + - "vz" + tw_attitude_id: + ulog_name: + - "timestamp" + - "q[0]" + - "q[1]" + - "q[2]" + - "q[3]" + dataframe_name: + - "timestamp" + - "q0" + - "q1" + - "q2" + - "q3" + vehicle_acceleration: + ulog_name: + - "timestamp" + - "xyz[0]" + - "xyz[1]" + - "xyz[2]" + dataframe_name: + - "timestamp" + - "accel_x" + - "accel_y" + - "accel_z" diff --git a/Tools/parametric_model/src/models/rotor_models/__init__.py b/Tools/parametric_model/src/models/rotor_models/__init__.py index 0926580c..916b6b54 100644 --- a/Tools/parametric_model/src/models/rotor_models/__init__.py +++ b/Tools/parametric_model/src/models/rotor_models/__init__.py @@ -4,3 +4,5 @@ from . import rotor_model from .rotor_model import RotorModel +from . import tiltingrotor_model +from .tiltingrotor_model import TiltingRotorModel diff --git a/Tools/parametric_model/src/models/rotor_models/rotor_model.py b/Tools/parametric_model/src/models/rotor_models/rotor_model.py index b45fd41e..6c530682 100644 --- a/Tools/parametric_model/src/models/rotor_models/rotor_model.py +++ b/Tools/parametric_model/src/models/rotor_models/rotor_model.py @@ -9,11 +9,12 @@ class RotorModel(): - def __init__(self, rotor_config_dict, actuator_input_vec, v_airspeed_mat, air_density=1.225, angular_vel_mat=None): + def __init__(self, rotor_config_dict, actuator_input_vec, v_airspeed_mat, air_density=1.225, angular_vel_mat=None, rotor_axis_mat=None): """ Inputs: actuator_input_vec: vector of actuator inputs (normalized between 0 and 1), numpy array of shape (n, 1) v_airspeed_mat: matrix of vertically stacked airspeed vectors, numpy array of shape (n, 3) + rotor_axis_mat: matrices of rotor axis corresponding to different timestamps. only needed for models with tilting rotors. """ # no more thrust produced at this airspeed inflow velocity @@ -23,7 +24,7 @@ def __init__(self, rotor_config_dict, actuator_input_vec, v_airspeed_mat, air_de rotor_config_dict["position"]).reshape(3, 1) self.turning_direction = rotor_config_dict["turning_direction"] self.rotor_name = rotor_config_dict["description"] - self.actuator_input_vec = actuator_input_vec + self.actuator_input_vec = np.array(actuator_input_vec) self.n_timestamps = actuator_input_vec.shape[0] # prop diameter in meters @@ -35,14 +36,13 @@ def __init__(self, rotor_config_dict, actuator_input_vec, v_airspeed_mat, air_de # air density in kg/m^3 self.air_density = air_density - self.compute_local_airspeed(v_airspeed_mat, angular_vel_mat) + self.compute_local_airspeed( + v_airspeed_mat, angular_vel_mat, rotor_axis_mat) - def compute_local_airspeed(self, v_airspeed_mat, angular_vel_mat): + def compute_local_airspeed(self, v_airspeed_mat, angular_vel_mat, rotor_axis_mat=None): # adjust airspeed with angular acceleration is angular_vel_mat is passed as argument - self.local_airspeed_mat = np.zeros(v_airspeed_mat.shape) - if angular_vel_mat is not None: assert (v_airspeed_mat.shape == angular_vel_mat.shape), "RotorModel: v_airspeed_mat and angular_vel_mat differ in size." @@ -57,18 +57,27 @@ def compute_local_airspeed(self, v_airspeed_mat, angular_vel_mat): self.v_airspeed_perpendicular_to_rotor_axis = np.zeros( v_airspeed_mat.shape) - for i in range(self.n_timestamps): - v_local_airspeed = self.local_airspeed_mat[i, :] - self.v_airspeed_parallel_to_rotor_axis[i, :] = (np.vdot( - self.rotor_axis, v_local_airspeed) * self.rotor_axis).flatten() - self.v_air_parallel_abs[i] = np.linalg.norm( - self.v_airspeed_parallel_to_rotor_axis) - self.v_airspeed_perpendicular_to_rotor_axis[i, :] = v_local_airspeed - \ - self.v_airspeed_parallel_to_rotor_axis[i, :] - - self.airspeed_initialized = True - - def compute_actuator_force_features(self, actuator_input, v_air_parallel_abs, v_airspeed_perpendicular_to_rotor_axis): + # if the rotor axis changes direction and rotor_axis_mat is specified + if rotor_axis_mat is not None: + for i in range(self.n_timestamps): + v_local_airspeed = self.local_airspeed_mat[i, :] + self.v_airspeed_parallel_to_rotor_axis[i, :] = (np.vdot( + rotor_axis_mat[i, :], v_local_airspeed) * rotor_axis_mat[i, :]).flatten() + self.v_air_parallel_abs[i] = np.linalg.norm( + self.v_airspeed_parallel_to_rotor_axis) + self.v_airspeed_perpendicular_to_rotor_axis[i, :] = v_local_airspeed - \ + self.v_airspeed_parallel_to_rotor_axis[i, :] + else: + for i in range(self.n_timestamps): + v_local_airspeed = self.local_airspeed_mat[i, :] + self.v_airspeed_parallel_to_rotor_axis[i, :] = (np.vdot( + self.rotor_axis, v_local_airspeed) * self.rotor_axis).flatten() + self.v_air_parallel_abs[i] = np.linalg.norm( + self.v_airspeed_parallel_to_rotor_axis) + self.v_airspeed_perpendicular_to_rotor_axis[i, :] = v_local_airspeed - \ + self.v_airspeed_parallel_to_rotor_axis[i, :] + + def compute_actuator_force_features(self, index, rotor_axis=None): """compute thrust model using a 2nd degree model of the normalized actuator outputs Inputs: @@ -78,9 +87,17 @@ def compute_actuator_force_features(self, actuator_input, v_air_parallel_abs, v_ For the model explanation have a look at the PDF. """ - # Thrust force computation + actuator_input = self.actuator_input_vec[index] + v_air_parallel_abs = self.v_air_parallel_abs[index] + v_airspeed_perpendicular_to_rotor_axis = \ + self.v_airspeed_perpendicular_to_rotor_axis[index, :].reshape( + (3, 1)) - X_thrust = self.rotor_axis @ np.array( + if rotor_axis is None: + rotor_axis = self.rotor_axis + + # Thrust force computation + X_thrust = rotor_axis @ np.array( [[actuator_input**2, (actuator_input*v_air_parallel_abs)]]) * self.air_density * self.prop_diameter**4 # Drag force computation if (np.linalg.norm(v_airspeed_perpendicular_to_rotor_axis) >= 0.05): @@ -93,12 +110,20 @@ def compute_actuator_force_features(self, actuator_input, v_air_parallel_abs, v_ return X_forces - def compute_actuator_moment_features(self, actuator_input, v_air_parallel_abs, v_airspeed_perpendicular_to_rotor_axis): + def compute_actuator_moment_features(self, index, rotor_axis=None): - X_moments = np.zeros((3, 5)) + actuator_input = self.actuator_input_vec[index] + v_air_parallel_abs = self.v_air_parallel_abs[index] + v_airspeed_perpendicular_to_rotor_axis = \ + self.v_airspeed_perpendicular_to_rotor_axis[index, :].reshape( + (3, 1)) + + if rotor_axis is None: + rotor_axis = self.rotor_axis + X_moments = np.zeros((3, 5)) leaver_moment_vec = np.cross( - self.rotor_position.flatten(), self.rotor_axis.flatten()) + self.rotor_position.flatten(), rotor_axis.flatten()) # Thrust leaver moment X_moments[:, 0] = leaver_moment_vec * actuator_input**2 * \ self.air_density * self.prop_diameter**4 @@ -118,18 +143,12 @@ def compute_actuator_moment_features(self, actuator_input, v_air_parallel_abs, v return X_moments def compute_actuator_force_matrix(self): - - assert (self.airspeed_initialized), "Airspeed is not initialized. Call initialize_actuator_airspeed(v_airspeed_mat) first." - print("Computing force features for rotor:", self.rotor_name) - - X_forces = self.compute_actuator_force_features( - self.actuator_input_vec[0], self.v_air_parallel_abs[0], self.v_airspeed_perpendicular_to_rotor_axis[0, :].reshape((3, 1))) + X_forces = self.compute_actuator_force_features(0) rotor_features_bar = Bar( 'Feature Computatiuon', max=self.actuator_input_vec.shape[0]) - for i in range(1, self.n_timestamps): - X_force_curr = self.compute_actuator_force_features( - self.actuator_input_vec[i], self.v_air_parallel_abs[i], self.v_airspeed_perpendicular_to_rotor_axis[i, :].reshape((3, 1))) + for index in range(1, self.n_timestamps): + X_force_curr = self.compute_actuator_force_features(index) X_forces = np.vstack((X_forces, X_force_curr)) rotor_features_bar.next() rotor_features_bar.finish() @@ -140,18 +159,12 @@ def compute_actuator_force_matrix(self): return X_forces, coef_list_forces def compute_actuator_moment_matrix(self): - - assert (self.airspeed_initialized), "Airspeed is not initialized. Call initialize_actuator_airspeed(v_airspeed_mat) first." - print("Computing moment features for rotor:", self.rotor_name) - - X_moments = self.compute_actuator_moment_features( - self.actuator_input_vec[0], self.v_air_parallel_abs[0], self.v_airspeed_perpendicular_to_rotor_axis[0, :].reshape((3, 1))) + X_moments = self.compute_actuator_moment_features(0) rotor_features_bar = Bar( 'Feature Computatiuon', max=self.actuator_input_vec.shape[0]) - for i in range(1, self.n_timestamps): - X_moment_curr = self.compute_actuator_moment_features( - self.actuator_input_vec[i], self.v_air_parallel_abs[i], self.v_airspeed_perpendicular_to_rotor_axis[i, :].reshape((3, 1))) + for index in range(1, self.n_timestamps): + X_moment_curr = self.compute_actuator_moment_features(index) X_moments = np.vstack((X_moments, X_moment_curr)) rotor_features_bar.next() rotor_features_bar.finish() @@ -165,7 +178,5 @@ def predict_thrust_force(self, thrust_coef_list): """ thrust_coef = np.array(thrust_coef_list).reshape(2, 1) stacked_force_vec = self.X_thrust @ thrust_coef - print(stacked_force_vec) force_mat = stacked_force_vec.reshape((self.n_timestamps, 3)) - print(force_mat) return force_mat diff --git a/Tools/parametric_model/src/models/rotor_models/tiltingrotor_model.py b/Tools/parametric_model/src/models/rotor_models/tiltingrotor_model.py new file mode 100644 index 00000000..93441b17 --- /dev/null +++ b/Tools/parametric_model/src/models/rotor_models/tiltingrotor_model.py @@ -0,0 +1,71 @@ +__author__ = "Manuel Galliker" +__maintainer__ = "Manuel Galliker" +__license__ = "BSD 3" + +from . import RotorModel +import numpy as np +import pandas as pd +import math +from progress.bar import Bar +from . import RotorModel +from scipy.spatial.transform import Rotation + + +class TiltingRotorModel(RotorModel): + + def __init__(self, rotor_config_dict, actuator_input_vec, v_airspeed_mat, wing_tilt_vec, air_density=1.225, angular_vel_mat=None): + print(rotor_config_dict) + self.tilt_axis = np.array(rotor_config_dict["tilt_axis"]).reshape(3, 1) + self.max_tilt_angle = rotor_config_dict["max_tilt_angle_deg"]*math.pi/180.0 + self.wing_tilt_vec = np.array(wing_tilt_vec) + self.n_timestamps = actuator_input_vec.shape[0] + self.rotor_axis = np.array( + rotor_config_dict["rotor_axis"]).reshape(3, 1) + self.compute_rotor_axis_mat() + super(TiltingRotorModel, self).__init__( + rotor_config_dict, actuator_input_vec, v_airspeed_mat, air_density, angular_vel_mat=angular_vel_mat, rotor_axis_mat=self.rotor_axis_mat) + + def compute_rotor_axis_mat(self): + self.rotor_axis_mat = np.zeros((self.n_timestamps, 3)) + for i in range(self.n_timestamps): + # Active vector rotation around tilt axis: + rotvec = self.tilt_axis.flatten() * self.max_tilt_angle * \ + self.wing_tilt_vec[i] + R_active_tilt = Rotation.from_rotvec( + rotvec).as_matrix() + self.rotor_axis_mat[i, :] = ( + R_active_tilt @ self.rotor_axis).flatten() + + def compute_actuator_force_matrix(self): + print("Computing force features for rotor:", self.rotor_name) + X_forces = self.compute_actuator_force_features( + 0, self.rotor_axis_mat[0, :].reshape((3, 1))) + rotor_features_bar = Bar( + 'Feature Computatiuon', max=self.actuator_input_vec.shape[0]) + for index in range(1, self.n_timestamps): + X_force_curr = self.compute_actuator_force_features( + index, self.rotor_axis_mat[index, :].reshape((3, 1))) + X_forces = np.vstack((X_forces, X_force_curr)) + rotor_features_bar.next() + rotor_features_bar.finish() + coef_list_forces = ["rot_drag_lin", "rot_thrust_quad", + "rot_thrust_lin"] + self.X_forces = X_forces + self.X_thrust = X_forces[:, 1:] + return X_forces, coef_list_forces + + def compute_actuator_moment_matrix(self): + print("Computing moment features for rotor:", self.rotor_name) + X_moments = self.compute_actuator_moment_features( + 0, self.rotor_axis_mat[0, :].reshape((3, 1))) + rotor_features_bar = Bar( + 'Feature Computatiuon', max=self.actuator_input_vec.shape[0]) + for index in range(1, self.n_timestamps): + X_moment_curr = self.compute_actuator_moment_features( + index, self.rotor_axis_mat[index, :].reshape((3, 1))) + X_moments = np.vstack((X_moments, X_moment_curr)) + rotor_features_bar.next() + rotor_features_bar.finish() + coef_list_moments = ["c_m_leaver_quad", "c_m_leaver_lin", + "c_m_drag_z_quad", "c_m_drag_z_lin", "c_m_rolling"] + return X_moments, coef_list_moments diff --git a/Tools/parametric_model/src/models/tilt_wing_model.py b/Tools/parametric_model/src/models/tilt_wing_model.py new file mode 100644 index 00000000..1642e2c8 --- /dev/null +++ b/Tools/parametric_model/src/models/tilt_wing_model.py @@ -0,0 +1,188 @@ +__author__ = "Manuel Galliker" +__maintainer__ = "Manuel Galliker" +__license__ = "BSD 3" + +"""Model to estimate the system parameters of gazebos standart vtol quadplane: +https://docs.px4.io/master/en/simulation/gazebo_vehicles.html#standard_vtol """ + + +import numpy as np +import math + +from .dynamics_model import DynamicsModel +from .rotor_models import TiltingRotorModel, RotorModel +from scipy.optimize import minimize +from scipy.linalg import block_diag +from .model_plots import model_plots, quad_plane_model_plots +from .model_config import ModelConfig +from .aerodynamic_models import TiltWingSection + + +"""This model estimates forces and moments for quad plane as for example the standard vtol in gazebo.""" + + +class TiltWingModel(DynamicsModel): + def __init__(self, rel_data_path, config_file="tilt_wing_config.yaml"): + self.config = ModelConfig(config_file) + super(TiltWingModel, self).__init__( + config_dict=self.config.dynamics_model_config, rel_data_path=rel_data_path) + + self.rotor_config_dict = self.config.model_config["actuators"]["rotors"] + self.stall_angle = math.pi/180 * \ + self.config.model_config["aerodynamics"]["stall_angle_deg"] + self.sig_scale_fac = self.config.model_config["aerodynamics"]["sig_scale_factor"] + self.mass = self.config.model_config["mass"] + self.u_tilt_vec = self.data_df["u_tilt"].to_numpy() + + assert (self.estimate_moments == + False), "Estimation of moments is not yet implemented in TiltWingModel. Disable in config file to estimate forces." + + def normalize_actuators(self): + # u : normalize actuator output from pwm to be scaled between 0 and 1 + # To be adjusted using parameters: + self.min_output = -1 + self.max_output = 1 + self.trim_output = 0 + self.actuator_columns = self.get_topic_list_from_topic_type( + "actuator_controls_0") + self.get_topic_list_from_topic_type( + "actuator_controls_1") + self.actuator_type = self.req_topics_dict["actuator_controls_0"]["actuator_type"] + \ + self.req_topics_dict["actuator_controls_1"]["actuator_type"] + self.actuator_type.remove("timestamp") + self.actuator_type.remove("timestamp") + for i in range(len(self.actuator_columns)): + actuator_data = self.data_df[self.actuator_columns[i]].to_numpy() + if (self.actuator_type[i] == "motor"): + for j in range(actuator_data.shape[0]): + if (actuator_data[j] < self.min_output): + actuator_data[j] = 0 + else: + actuator_data[j] = ( + actuator_data[j] - self.min_output)/(self.max_output - self.min_output) + elif (self.actuator_type[i] == "control_surcafe"): + for j in range(actuator_data.shape[0]): + if (actuator_data[j] < self.min_output): + actuator_data[j] = 0 + else: + actuator_data[j] = 2*( + actuator_data[j] - self.trim_output)/(self.max_output - self.min_output) + else: + print("actuator type unknown:", self.actuator_type[i]) + print("normalization failed") + exit(1) + self.data_df[self.actuator_columns[i]] = actuator_data + + def initialize_rotor_model(self, rotor_config_dict, rotor_group, angular_vel_mat=None): + rotor_input_name = rotor_config_dict["dataframe_name"] + u_vec = self.data_df[rotor_input_name].to_numpy() + if (rotor_group == "wing_"): + rotor = TiltingRotorModel( + rotor_config_dict, u_vec, self.v_airspeed_mat, self.u_tilt_vec, angular_vel_mat=angular_vel_mat) + else: + rotor = RotorModel( + rotor_config_dict, u_vec, self.v_airspeed_mat, angular_vel_mat=angular_vel_mat) + return rotor + + def prepare_opimization_matrices(self): + + if "V_air_body_x" not in self.data_df: + self.normalize_actuators() + self.compute_airspeed_from_groundspeed(["vx", "vy", "vz"]) + + # Rotor features as self.X_rotor_forces + self.compute_rotor_features(self.rotor_config_dict) + + # Initialize wing sections + airspeed_mat = self.data_df[["vx", "vy", "vz"]].to_numpy() + angular_vel_mat = self.data_df[["vx", "vy", "vz"]].to_numpy() + self.wing_sections = [] + wing_sections_config_list = self.config.model_config["aerodynamics"]["main_wing_sections"] + for i in range(len(wing_sections_config_list)): + curr_wing_section_config = wing_sections_config_list[i] + curr_rotor = self.rotor_dict["wing_"][( + curr_wing_section_config["rotor"])] + curr_wing_section = TiltWingSection( + curr_wing_section_config, airspeed_mat, angular_vel_mat=angular_vel_mat, rotor=curr_rotor) + self.wing_sections.append(curr_wing_section) + self.aero_coef_list = curr_wing_section.aero_coef_list + + # Accelerations + accel_body_mat = self.data_df[[ + "accel_x", "accel_y", "accel_z"]].to_numpy() + self.y_accel = accel_body_mat.flatten() + self.y_forces = self.mass * accel_body_mat.flatten() + + def predict_forces(self, x): + aero_coef = np.array(x[6:15]).reshape(9, 1) + main_wing_rotor_thrust_coef = np.array(x[[1, 2]]).reshape(2, 1) + rotor_coef = np.array(x[0:6]).reshape(6, 1) + F_aero_pred = np.zeros((self.y_forces.shape[0], 1)) + for wing_section in self.wing_sections: + F_aero_segment_pred = wing_section.predict_wing_segment_forces( + main_wing_rotor_thrust_coef, aero_coef) + F_aero_pred = np.add(F_aero_pred, F_aero_segment_pred) + + F_rotor_pred = self.X_rotor_forces @ rotor_coef + F_pred = np.add(F_aero_pred, F_rotor_pred) + return F_pred + + def objective(self, x): + print("Evaluating objective function.") + F_pred = self.predict_forces(x) + a_pred = F_pred.flatten() / self.mass + # pred_error = np.linalg.norm((a_pred - self.y_accel)) + pred_error = np.sqrt(((a_pred - self.y_accel) ** 2).mean()) + print("Current sum of squared acceleration prediction error: ", pred_error) + return pred_error + + def estimate_model(self): + print("Estimating quad plane model using the following data:") + print(self.data_df.columns) + self.data_df_len = self.data_df.shape[0] + print("resampled data contains ", self.data_df_len, "timestamps.") + self.prepare_opimization_matrices() + + # optimization_variables: + optimization_parameters = self.config.model_config["optimzation_parameters"] + self.coef_list = self.rotor_forces_coef_list + self.aero_coef_list + config_coef_list = ( + optimization_parameters["initial_coefficients"]).keys() + print("estimating coefficients: ", self.coef_list) + assert (self.coef_list == list(config_coef_list)), ( + "keys on config file differ from coefficient list: ", self.coef_list, config_coef_list) + + # initial guesses + x0 = np.array( + list(optimization_parameters["initial_coefficients"].values())) + print("Initial coefficients: ", x0) + coef_bounds = list( + optimization_parameters["coefficient_bounds"].values()) + print("Coefficients bounds: ", coef_bounds) + + solution = minimize(self.objective, x0, method='SLSQP', + bounds=coef_bounds) + self.x_opt = solution.x + print(self.x_opt) + + return + + def plot_model_predicitons(self): + + y_forces_pred = self.predict_forces(self.x_opt) + # y_moments_pred = self.reg.predict(self.X_moments) + + model_plots.plot_accel_predeictions( + self.y_forces, y_forces_pred, self.data_df["timestamp"]) + # model_plots.plot_angular_accel_predeictions( + # self.y_moments, y_moments_pred, self.data_df["timestamp"]) + # model_plots.plot_az_and_collective_input( + # self.y_forces, y_forces_pred, self.data_df[["u0", "u1", "u2", "u3"]], self.data_df["timestamp"]) + # model_plots.plot_accel_and_airspeed_in_z_direction( + # self.y_forces, y_forces_pred, self.data_df["V_air_body_z"], self.data_df["timestamp"]) + # model_plots.plot_airspeed_and_AoA( + # self.data_df[["V_air_body_x", "V_air_body_y", "V_air_body_z", "AoA"]], self.data_df["timestamp"]) + # model_plots.plot_accel_and_airspeed_in_y_direction( + # self.y_forces, y_forces_pred, self.data_df["V_air_body_y"], self.data_df["timestamp"]) + # quad_plane_model_plots.plot_accel_predeictions_with_flap_outputs( + # self.y_forces, y_forces_pred, self.data_df[["u5", "u6", "u7"]], self.data_df["timestamp"]) + return diff --git a/Tools/parametric_model/src/tools/dataframe_tools.py b/Tools/parametric_model/src/tools/dataframe_tools.py index fb940c85..10f0d5e2 100644 --- a/Tools/parametric_model/src/tools/dataframe_tools.py +++ b/Tools/parametric_model/src/tools/dataframe_tools.py @@ -7,14 +7,27 @@ from src.tools import pandas_from_topic from .ulog_utils import slerp -HOVER_PWM = 1500 +def compute_flight_time(ulog, min_hover_pwm=1500): -def compute_flight_time(ulog): - act_df = pandas_from_topic(ulog, ["actuator_outputs"]) - act_df_crp = act_df[act_df.iloc[:, 2] > HOVER_PWM] + topic_type_list = [] + for ulog_data_element in ulog._data_list: + topic_type_list.append(ulog_data_element.name) - # set start and end time of flight duration + if "actuator_outputs" in topic_type_list: + act_df = pandas_from_topic(ulog, ["actuator_outputs"]) + act_df_crp = act_df[act_df.iloc[:, 2] > min_hover_pwm] + + # special case for aero mini tilt wing for asl + elif "actuator_controls_0" in topic_type_list: + act_df = pandas_from_topic(ulog, ["actuator_controls_0"]) + act_df_crp = act_df[act_df.iloc[:, 2] > 0.2] + + else: + print("could not select flight time due to missing actuator topic") + exit(1) + + # set start and end time of flight duration t_start = act_df_crp.iloc[1, 0] t_end = act_df_crp.iloc[(act_df_crp.shape[0]-1), 0] flight_time = {"t_start": t_start, "t_end": t_end}