class lateral_one_minus_cos(BaseGust): r""" This gust can be used by using the setting ``gust_shape = 'lateral 1-cos'`` in :class:`GustVelocityField`. """ gust_id = 'lateral 1-cos' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['gust_length'] = 'float' settings_default['gust_length'] = 0.0 settings_description['gust_length'] = 'Length of gust' settings_types['gust_intensity'] = 'float' settings_default['gust_intensity'] = 0.0 settings_description['gust_intensity'] = 'Intensity of the gust' setting_table = settings.SettingsTable() __doc__ += setting_table.generate(settings_types, settings_default, settings_description, doc_settings_description) def initialise(self, in_dict): self.settings = in_dict settings.to_custom_types(self.settings, self.settings_types, self.settings_default) def gust_shape(self, x, y, z, time=0): gust_length = self.settings['gust_length'].value gust_intensity = self.settings['gust_intensity'].value vel = np.zeros((3,)) if x > 0.0 or x < -gust_length: return vel vel[1] = (1.0 - np.cos(2.0 * np.pi * x / gust_length)) * gust_intensity * 0.5 return vel
def __doc__(self): # Generate documentation table settings_table = settings.SettingsTable() _doc = inspect.getdoc(self) _doc += settings_table.generate(settings_types, settings_default, settings_description) return _doc
class PickleData(BaseSolver): """ This postprocessor writes the SHARPy ``data`` structure in a pickle file, such that classes and methods from SHARPy are retained for restarted solutions or further post-processing. """ solver_id = 'PickleData' solver_classification = 'post-processor' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['folder'] = 'str' settings_default['folder'] = './output' settings_description['folder'] = 'Folder to output pickle file' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): import sharpy self.settings = None self.data = None self.filename = None self.folder = None self.caller = None def initialise(self, data, custom_settings=None, caller=None): self.data = data if custom_settings is None: self.settings = data.settings[self.solver_id] else: self.settings = custom_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) # create folder for containing files if necessary if not os.path.exists(self.settings['folder']): os.makedirs(self.settings['folder']) self.folder = self.settings['folder'] + '/' if not os.path.exists(self.folder): os.makedirs(self.folder) self.filename = self.folder + self.data.settings['SHARPy'][ 'case'] + '.pkl' self.caller = caller def run(self, online=False): for it in range(len(self.data.structure.timestep_info)): tstep_p = self.data.structure.timestep_info[it] if tstep_p is not None: if not tstep_p.in_global_AFoR: tstep_p.whole_structure_to_global_AFoR(self.data.structure) with open(self.filename, 'wb') as f: pickle.dump(self.data, f, protocol=pickle.HIGHEST_PROTOCOL) return self.data
class Iterative(BaseBalancedRom): __doc__ = librom.balreal_iter.__doc__ _bal_rom_id = 'Iterative' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['lowrank'] = 'bool' settings_default['lowrank'] = True settings_description['lowrank'] = 'Use low rank methods' settings_types['smith_tol'] = 'float' settings_default['smith_tol'] = 1e-10 settings_description['smith_tol'] = 'Smith tolerance' settings_types['tolSVD'] = 'float' settings_default['tolSVD'] = 1e-6 settings_description['tolSVD'] = 'SVD threshold' settings_types['tolSVD'] = 'float' settings_default['tolSVD'] = 1e-6 settings_description['tolSVD'] = 'SVD threshold' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.settings = dict() def initialise(self, in_settings=None): if in_settings is not None: self.settings = in_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) def run(self, ss): A, B, C, D = ss.get_mats() s, T, Tinv, rcmax, romax = librom.balreal_iter( A, B, C, lowrank=self.settings['lowrank'], tolSmith=self.settings['smith_tol'].value, tolSVD=self.settings['tolSVD'].value) Ar = Tinv.dot(A.dot(T)) Br = Tinv.dot(B) Cr = C.dot(T) ssrom = libss.ss(Ar, Br, Cr, D, dt=ss.dt) return ssrom
class span_sine(BaseGust): r""" This gust can be used by using the setting ``gust_shape = 'span sine'`` in :class:`GustVelocityField`. """ gust_id = 'span sine' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['gust_intensity'] = 'float' settings_default['gust_intensity'] = 0.0 settings_description['gust_intensity'] = 'Intensity of the gust' settings_types['span'] = 'float' settings_default['span'] = 0. settings_description['span'] = 'Wing span' settings_types['periods_per_span'] = 'int' settings_default['periods_per_span'] = 1 settings_description['periods_per_span'] = 'Number of times that the sine is repeated in the span of the wing' settings_types['perturbation_dir'] = 'list(float)' settings_default['perturbation_dir'] = np.array([0, 0, 1.]) settings_description['perturbation_dir'] = 'Direction in which the perturbation will be applied in A FoR' settings_types['span_dir'] = 'list(float)' settings_default['span_dir'] = np.array([0, 1., 0]) settings_description['span_dir'] = 'Direction of the span of the wing' settings_types['span_with_gust'] = 'float' settings_default['span_with_gust'] = 0. settings_description['span_with_gust'] = 'Extension of the span to which the gust will be applied' setting_table = settings.SettingsTable() __doc__ += setting_table.generate(settings_types, settings_default, settings_description, doc_settings_description) def initialise(self, in_dict): self.settings = in_dict settings.to_custom_types(self.settings, self.settings_types, self.settings_default) if self.settings['span_with_gust'].value == 0: self.settings['span_with_gust'] = self.settings['span'] def gust_shape(self, x, y, z, time=0): d = np.dot(np.array([x, y, z]), self.settings['span_dir']) if np.abs(d) <= self.settings['span_with_gust'].value / 2: vel = 0.5 * self.settings['gust_intensity'].value * np.sin( d * 2. * np.pi / (self.settings['span'].value / self.settings['periods_per_span'].value)) else: vel = np.zeros((3,)) return vel * self.settings['perturbation_dir']
class DARPA(BaseGust): r""" Discrete, non-uniform span model .. math:: U_z = \frac{u_{de}}{2}\left[1-\cos\left(\frac{2\pi x}{S}\right)\right]\cos\left(\frac{\pi y}{b}\right) This gust can be used by using the setting ``gust_shape = 'DARPA'`` in :class:`.GustVelocityField`. """ gust_id = 'DARPA' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['gust_length'] = 'float' settings_default['gust_length'] = 0.0 settings_description['gust_length'] = 'Length of gust' settings_types['gust_intensity'] = 'float' settings_default['gust_intensity'] = 0.0 settings_description['gust_intensity'] = 'Intensity of the gust' settings_types['span'] = 'float' settings_default['span'] = 0. settings_description['span'] = 'Wing span' setting_table = settings.SettingsTable() __doc__ += setting_table.generate(settings_types, settings_default, settings_description, header_line=doc_settings_description) def initialise(self, in_dict): self.settings = in_dict settings.to_custom_types(self.settings, self.settings_types, self.settings_default) def gust_shape(self, x, y, z, time=0): gust_length = self.settings['gust_length'].value gust_intensity = self.settings['gust_intensity'].value span = self.settings['span'].value vel = np.zeros((3, )) if x > 0.0 or x < -gust_length: return vel vel[2] = (1.0 - np.cos(2.0 * np.pi * x / gust_length)) * gust_intensity * 0.5 vel[2] *= -np.cos(y / span * np.pi) return vel
class time_varying(BaseGust): r""" The inflow velocity changes with time but it is uniform in space. It is read from a 4 column file: .. math:: time[s] \Delta U_x \Delta U_y \Delta U_z This gust can be used by using the setting ``gust_shape = 'time varying'`` in :class:.`GustVelocityField`. """ gust_id = 'time varying' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['file'] = 'str' settings_default['file'] = '' settings_description['file'] = 'File with the information' setting_table = settings.SettingsTable() __doc__ += setting_table.generate(settings_types, settings_default, settings_description, header_line=doc_settings_description) def __init__(self): super().__init__() self.file_info = None def initialise(self, in_dict): self.settings = in_dict settings.to_custom_types(self.settings, self.settings_types, self.settings_default) self.file_info = np.loadtxt(self.settings['file']) def gust_shape(self, x, y, z, time=0): vel = np.zeros((3, )) d = np.dot(np.array([x, y, z]), self.u_inf_direction) if d > 0.0: return vel vel[0] = np.interp(d, -self.file_info[::-1, 0] * self.u_inf, self.file_info[::-1, 1]) vel[1] = np.interp(d, -self.file_info[::-1, 0] * self.u_inf, self.file_info[::-1, 2]) vel[2] = np.interp(d, -self.file_info[::-1, 0] * self.u_inf, self.file_info[::-1, 3]) return vel
class NoAero(BaseSolver): """ Solver to be used with DynamicCoupled when aerodynamics are not of interest """ solver_id = 'NoAero' solver_classification = 'Aero' settings_types = dict() settings_default = dict() settings_description = dict() settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.data = None self.settings = None def initialise(self, data, custom_settings=None): """ To be called just once per simulation. """ self.data = data if custom_settings is None: self.settings = data.settings[self.solver_id] else: self.settings = custom_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) def run(self, aero_tstep=None, structure_tstep=None, convect_wake=True, dt=None, t=None, unsteady_contribution=False): return self.data def add_step(self): self.data.aero.add_timestep() def update_grid(self, beam): pass def update_custom_grid(self, structure_tstep, aero_tstep): pass
class InNetwork(Network): """ Input Network socket settings Note: If sending/receiving data across the net or LAN, make sure that your firewall has the desired ports open, otherwise the signals will not make it through. """ settings_types = Network.settings_types.copy() settings_default = Network.settings_default.copy() settings_description = Network.settings_description.copy() settings_types['port'] = 'int' settings_default['port'] = 65001 settings_description['port'] = 'Own port for input network' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): super().__init__() self._in_message_length = 1024 self._recv_buffer = b'' def set_message_length(self, value): self._in_message_length = value logger.debug('Set input signal message size to {} bytes'.format( self._in_message_length)) def process_events(self, mask): self.sock.setblocking(False) if mask and selectors.EVENT_READ: logger.info( 'In Network - waiting for input data of size {} bytes'.format( self._in_message_length)) msg = self.receive(self._in_message_length) self._recv_buffer += msg # any required processing # send list of tuples if len(self._recv_buffer) == self._in_message_length: logger.info('In Network - {}/{} bytes read'.format( len(self._recv_buffer), self._in_message_length)) list_of_variables = message_interface.decoder( self._recv_buffer, byte_ordering=self._byte_ordering) self.queue.put(list_of_variables) logger.debug('In Network - put data in the queue') self._recv_buffer = b'' # clean up
class one_minus_cos(BaseGust): r""" One minus cos gust (single bump) .. math:: U_z = \frac{u_{de}}{2}\left[1-\cos\left(\frac{2\pi x}{S}\right)\right] This gust can be used by using the setting ``gust_shape = '1-cos'`` in :class:`.GustVelocityField`. """ gust_id = '1-cos' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['gust_length'] = 'float' settings_default['gust_length'] = 0.0 settings_description['gust_length'] = 'Length of gust, :math:`S`.' settings_types['gust_intensity'] = 'float' settings_default['gust_intensity'] = 0.0 settings_description[ 'gust_intensity'] = 'Intensity of the gust :math:`u_{de}`.' setting_table = settings.SettingsTable() __doc__ += setting_table.generate(settings_types, settings_default, settings_description, header_line=doc_settings_description) def initialise(self, in_dict): self.settings = in_dict settings.to_custom_types(self.settings, self.settings_types, self.settings_default) def gust_shape(self, x, y, z, time=0): gust_length = self.settings['gust_length'].value gust_intensity = self.settings['gust_intensity'].value vel = np.zeros((3, )) if x > 0.0 or x < -gust_length: return vel vel[2] = (1.0 - np.cos(2.0 * np.pi * x / gust_length)) * gust_intensity * 0.5 return vel
class time_varying_global(BaseGust): r""" Similar to the previous one but the velocity changes instanteneously in the whole flow field. It is not fed into the solid. This gust can be used by using the setting ``gust_shape = 'time varying global'`` in :class:`GustVelocityField`. """ gust_id = 'time varying global' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['file'] = 'str' settings_default['file'] = '' settings_description[ 'file'] = 'File with the information (only for time varying)' setting_table = settings.SettingsTable() __doc__ += setting_table.generate(settings_types, settings_default, settings_description, header_line=doc_settings_description) def __init__(self): super().__init__() self.file_info = None def initialise(self, in_dict): self.settings = in_dict settings.to_custom_types(self.settings, self.settings_types, self.settings_default) self.file_info = np.loadtxt(self.settings['file']) def gust_shape(self, x, y, z, time=0): vel = np.zeros((3, )) vel[0] = np.interp(time, self.file_info[:, 0], self.file_info[:, 1]) vel[1] = np.interp(time, self.file_info[:, 0], self.file_info[:, 2]) vel[2] = np.interp(time, self.file_info[:, 0], self.file_info[:, 3]) return vel
class continuous_sin(BaseGust): r""" Continuous sinusoidal gust model .. math:: U_z = \frac{u_{de}}{2}\sin\left(\frac{2\pi x}{S}\right) This gust can be used by using the setting ``gust_shape = 'continuous_sin'`` in :class:`GustVelocityField`. """ gust_id = 'continuous_sin' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['gust_length'] = 'float' settings_default['gust_length'] = 0.0 settings_description['gust_length'] = 'Length of gust' settings_types['gust_intensity'] = 'float' settings_default['gust_intensity'] = 0.0 settings_description['gust_intensity'] = 'Intensity of the gust' setting_table = settings.SettingsTable() __doc__ += setting_table.generate(settings_types, settings_default, settings_description, header_line=doc_settings_description) def initialise(self, in_dict): self.settings = in_dict settings.to_custom_types(self.settings, self.settings_types, self.settings_default) def gust_shape(self, x, y, z, time=0): gust_length = self.settings['gust_length'].value gust_intensity = self.settings['gust_intensity'].value vel = np.zeros((3, )) if x > 0.0: return vel vel[2] = 0.5 * gust_intensity * np.sin(2 * np.pi * x / gust_length) return vel
class SHWUvlm(BaseSolver): """ Steady vortex method assuming helicoidal wake shape """ solver_id = 'SHWUvlm' solver_classification = 'aero' # settings list settings_types = dict() settings_default = dict() settings_description = dict() settings_types['print_info'] = 'bool' settings_default['print_info'] = True settings_description['print_info'] = 'Output run-time information' settings_types['num_cores'] = 'int' settings_default['num_cores'] = 0 settings_description['num_cores'] = 'Number of cores to used in parallelisation' settings_types['convection_scheme'] = 'int' settings_default['convection_scheme'] = 2 settings_description['convection_scheme'] = 'Convection scheme for the wake (only 2 tested for this solver)' settings_types['dt'] = 'float' settings_default['dt'] = 0.1 settings_description['dt'] = 'time step used to discretise the wake' settings_types['iterative_solver'] = 'bool' settings_default['iterative_solver'] = False settings_description['iterative_solver'] = '' settings_types['iterative_tol'] = 'float' settings_default['iterative_tol'] = 1e-4 settings_description['iterative_tol'] = '' settings_types['iterative_precond'] = 'bool' settings_default['iterative_precond'] = False settings_description['iterative_precond'] = '' settings_types['velocity_field_generator'] = 'str' settings_default['velocity_field_generator'] = 'SteadyVelocityField' settings_description['velocity_field_generator'] = 'Name of the velocity field generator' settings_types['velocity_field_input'] = 'dict' settings_default['velocity_field_input'] = {} settings_description['velocity_field_input'] = 'Dictionary of inputs needed by the velocity field generator' settings_types['gamma_dot_filtering'] = 'int' settings_default['gamma_dot_filtering'] = 0 settings_description['gamma_dot_filtering'] = 'Parameter used to filter gamma dot (only odd numbers bigger than one allowed)' settings_types['rho'] = 'float' settings_default['rho'] = 1.225 settings_description['rho'] = 'Density' settings_types['rot_vel'] = 'float' settings_default['rot_vel'] = 0.0 settings_description['rot_vel'] = 'Rotation velocity in rad/s' settings_types['rot_axis'] = 'list(float)' settings_default['rot_axis'] = np.array([1.,0.,0.]) settings_description['rot_axis'] = 'Axis of rotation of the wake' settings_types['rot_center'] = 'list(float)' settings_default['rot_center'] = np.array([0.,0.,0.]) settings_description['rot_center'] = 'Center of rotation of the wake' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.data = None self.settings = None self.velocity_generator = None def initialise(self, data, custom_settings=None): self.data = data if custom_settings is None: self.settings = data.settings[self.solver_id] else: self.settings = custom_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) # self.data.structure.add_unsteady_information(self.data.structure.dyn_dict, self.settings['n_time_steps'].value) # init velocity generator velocity_generator_type = gen_interface.generator_from_string( self.settings['velocity_field_generator']) self.velocity_generator = velocity_generator_type() self.velocity_generator.initialise(self.settings['velocity_field_input']) # Checks if not self.settings['convection_scheme'].value == 2: sys.exit("ERROR: convection_scheme: %u. Only 2 supported" % self.settings['convection_scheme'].value) def run(self, aero_tstep=None, structure_tstep=None, convect_wake=False, dt=None, t=None, unsteady_contribution=False): # Checks if convect_wake: sys.exit("ERROR: convect_wake should be set to False") if aero_tstep is None: aero_tstep = self.data.aero.timestep_info[-1] if structure_tstep is None: structure_tstep = self.data.structure.timestep_info[-1] if dt is None: dt = self.settings['dt'].value if t is None: t = self.data.ts*dt # generate uext self.velocity_generator.generate({'zeta': aero_tstep.zeta, 'override': True, 't': t, 'ts': self.data.ts, 'dt': dt, 'for_pos': structure_tstep.for_pos}, aero_tstep.u_ext) # if self.settings['convection_scheme'].value > 1 and convect_wake: # # generate uext_star # self.velocity_generator.generate({'zeta': aero_tstep.zeta_star, # 'override': True, # 'ts': self.data.ts, # 'dt': dt, # 't': t, # 'for_pos': structure_tstep.for_pos}, # aero_tstep.u_ext_star) # previous_ts = max(len(self.data.aero.timestep_info) - 1, 0) - 1 # previous_ts = -1 # print('previous_step max circulation: %f' % previous_aero_tstep.gamma[0].min()) # print('current step max circulation: %f' % aero_tstep.gamma[0].min()) uvlmlib.shw_solver(self.data.ts, aero_tstep, structure_tstep, self.settings, convect_wake=False, dt=dt) # print('current step max unsforce: %f' % aero_tstep.dynamic_forces[0].max()) if unsteady_contribution: # calculate unsteady (added mass) forces: self.data.aero.compute_gamma_dot(dt, aero_tstep, self.data.aero.timestep_info[-3:]) if self.settings['gamma_dot_filtering'].value > 0: self.filter_gamma_dot(aero_tstep, self.data.aero.timestep_info, self.settings['gamma_dot_filtering'].value) uvlmlib.uvlm_calculate_unsteady_forces(aero_tstep, structure_tstep, self.settings, convect_wake=convect_wake, dt=dt) else: for i_surf in range(len(aero_tstep.gamma)): aero_tstep.gamma_dot[i_surf][:] = 0.0 return self.data def add_step(self): self.data.aero.add_timestep() def update_grid(self, beam): self.data.aero.generate_zeta(beam, self.data.aero.aero_settings, -1, beam_ts=-1) def update_custom_grid(self, structure_tstep, aero_tstep): self.data.aero.generate_zeta_timestep_info(structure_tstep, aero_tstep, self.data.structure, self.data.aero.aero_settings) def update_step(self): self.data.aero.generate_zeta(self.data.structure, self.data.aero.aero_settings, self.data.ts) @staticmethod def filter_gamma_dot(tstep, history, filter_param): series_length = len(history) + 1 for i_surf in range(len(tstep.zeta)): n_rows, n_cols = tstep.gamma[i_surf].shape for i in range(n_rows): for j in range(n_cols): series = np.zeros((series_length,)) for it in range(series_length - 1): series[it] = history[it].gamma_dot[i_surf][i, j] series[-1] = tstep.gamma_dot[i_surf][i, j] # filter tstep.gamma_dot[i_surf][i, j] = scipy.signal.wiener(series, filter_param)[-1]
class NoAero(BaseSolver): """ Skip UVLM evaluation Solver to be used with either :class:`~sharpy.solvers.staticcoupled.StaticCoupled` or :class:`~sharpy.solvers.dynamiccoupled.DynamicCoupled` when aerodynamics are not of interest. An example would be running a structural-only simulation where you would like to keep an aerodynamic grid for visualisation purposes. """ solver_id = 'NoAero' solver_classification = 'Aero' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['dt'] = 'float' settings_default['dt'] = 0.1 settings_description['dt'] = 'Time step' settings_types['update_grid'] = 'bool' settings_default['update_grid'] = True settings_description[ 'update_grid'] = 'Update aerodynamic grid as the structure deforms.' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.data = None self.settings = None def initialise(self, data, custom_settings=None): self.data = data if custom_settings is None: self.settings = data.settings[self.solver_id] else: self.settings = custom_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default, no_ctype=True) if len(self.data.aero.timestep_info ) == 0: # initialise with zero timestep for static sims self.update_step() def run(self, aero_tstep=None, structure_tstep=None, convect_wake=True, dt=None, t=None, unsteady_contribution=False): # generate the wake because the solid shape might change if aero_tstep is None: aero_tstep = self.data.aero.timestep_info[self.data.ts] self.data.aero.wake_shape_generator.generate({ 'zeta': aero_tstep.zeta, 'zeta_star': aero_tstep.zeta_star, 'gamma': aero_tstep.gamma, 'gamma_star': aero_tstep.gamma_star, 'dist_to_orig': aero_tstep.dist_to_orig }) return self.data def add_step(self): self.data.aero.add_timestep() def update_grid(self, beam): # called by DynamicCoupled if self.settings['update_grid']: self.data.aero.generate_zeta(beam, self.data.aero.aero_settings, -1, beam_ts=-1) def update_custom_grid(self, structure_tstep, aero_tstep): # called by DynamicCoupled if self.settings['update_grid']: self.data.aero.generate_zeta_timestep_info( structure_tstep, aero_tstep, self.data.structure, self.data.aero.aero_settings, dt=None) def update_step(self): # called by StaticCoupled if self.settings['update_grid']: self.data.aero.generate_zeta(self.data.structure, self.data.aero.aero_settings, self.data.ts) else: self.add_step() def next_step(self): # called by StaticCoupled self.add_step()
class GridBox(generator_interface.BaseGenerator): """ GridBox Generatex a grid within a box to be used to generate the flow field during the postprocessing """ generator_id = 'GridBox' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['coords_0'] = 'list(float)' settings_default['coords_0'] = [0., 0., 0.] settings_description['coords_0'] = 'First bounding box corner' settings_types['coords_1'] = 'list(float)' settings_default['coords_1'] = [10., 0., 10.] settings_description['coords_1'] = 'Second bounding box corner' settings_types['spacing'] = 'list(float)' settings_default['spacing'] = [1., 1., 1.] settings_description['spacing'] = 'Spacing parameters of the bbox' settings_types['moving'] = 'bool' settings_default['moving'] = False settings_description['moving'] = 'If ``True``, the box moves with the body frame of reference. It does not rotate with it, though' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.in_dict = dict() self.settings = None def initialise(self, in_dict): self.in_dict = in_dict settings.to_custom_types(self.in_dict, self.settings_types, self.settings_default) self.settings = copy.deepcopy(self.in_dict) self.x0 = self.in_dict['coords_0'][0] self.y0 = self.in_dict['coords_0'][1] self.z0 = self.in_dict['coords_0'][2] self.x1 = self.in_dict['coords_1'][0] self.y1 = self.in_dict['coords_1'][1] self.z1 = self.in_dict['coords_1'][2] self.dx = self.in_dict['spacing'][0] self.dy = self.in_dict['spacing'][1] self.dz = self.in_dict['spacing'][2] def generate(self, params): if self.settings['moving']: for_pos = params['for_pos'] else: for_pos = np.zeros((3,)) nx = np.abs(int((self.x1-self.x0)/self.dx + 1)) ny = np.abs(int((self.y1-self.y0)/self.dy + 1)) nz = np.abs(int((self.z1-self.z0)/self.dz + 1)) xarray = np.linspace(self.x0, self.x1, nx) + for_pos[0] yarray = np.linspace(self.y0, self.y1, ny) + for_pos[1] zarray = np.linspace(self.z0, self.z1, nz) + for_pos[2] grid = [] for iz in range(nz): grid.append(np.zeros((3, nx, ny), dtype=ct.c_double)) for ix in range(nx): for iy in range(ny): grid[iz][0, ix, iy] = xarray[ix] grid[iz][1, ix, iy] = yarray[iy] grid[iz][2, ix, iy] = zarray[iz] vtk_info = tvtk.RectilinearGrid() vtk_info.dimensions = np.array([nx, ny, nz], dtype=int) vtk_info.x_coordinates = xarray vtk_info.y_coordinates = yarray vtk_info.z_coordinates = zarray return vtk_info, grid
class RigidDynamicCoupledStep(_BaseStructural): """ Structural solver used for the dynamic simulation of free-flying rigid structures. This solver provides an interface to the structural library (``xbeam``) and updates the structural parameters for every k-th step in the FSI iteration. This solver can be called as part of a standalone structural simulation or as the structural solver of a coupled aeroelastic simulation. """ solver_id = 'RigidDynamicCoupledStep' solver_classification = 'structural' settings_types = _BaseStructural.settings_types.copy() settings_default = _BaseStructural.settings_default.copy() settings_description = _BaseStructural.settings_description.copy() settings_types['balancing'] = 'bool' settings_default['balancing'] = False settings_types['relaxation_factor'] = 'float' settings_default['relaxation_factor'] = 0.3 settings_description['relaxation factor'] = 'Relaxation factor' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.data = None self.settings = None def initialise(self, data, custom_settings=None): self.data = data if custom_settings is None: self.settings = data.settings[self.solver_id] else: self.settings = custom_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) # load info from dyn dictionary self.data.structure.add_unsteady_information( self.data.structure.dyn_dict, self.settings['num_steps'].value) # generate q, dqdt and dqddt xbeamlib.xbeam_solv_disp2state(self.data.structure, self.data.structure.timestep_info[-1]) def run(self, structural_step, previous_structural_step=None, dt=None): if dt is None: dt = self.settings['dt'] xbeamlib.xbeam_step_coupledrigid(self.data.structure, self.settings, self.data.ts, structural_step, dt=dt) self.extract_resultants(structural_step) self.data.structure.integrate_position(structural_step, dt) return self.data def add_step(self): self.data.structure.next_step() def next_step(self): pass def extract_resultants(self, step=None): if step is None: step = self.data.structure.timestep_info[-1] applied_forces = self.data.structure.nodal_b_for_2_a_for( step.steady_applied_forces + step.unsteady_applied_forces, step) applied_forces_copy = applied_forces.copy() gravity_forces_copy = step.gravity_forces.copy() for i_node in range(self.data.structure.num_node): applied_forces_copy[i_node, 3:6] += algebra.cross3( step.pos[i_node, :], applied_forces_copy[i_node, 0:3]) gravity_forces_copy[i_node, 3:6] += algebra.cross3( step.pos[i_node, :], gravity_forces_copy[i_node, 0:3]) totals = np.sum(applied_forces_copy + gravity_forces_copy, axis=0) step.total_forces = np.sum(applied_forces_copy, axis=0) step.total_gravity_forces = np.sum(gravity_forces_copy, axis=0) return totals[0:3], totals[3:6]
class TurbVelocityFieldBts(generator_interface.BaseGenerator): r""" Turbulent Velocity Field Generator from TurbSim bts files ``TurbVelocitityFieldBts`` is a class inherited from ``BaseGenerator`` The ``TurbVelocitityFieldBts`` class generates a velocity field based on the input from a bts file generated by TurbSim. https://nwtc.nrel.gov/TurbSim To call this generator, the ``generator_id = TurbVelocityField`` shall be used. This is parsed as the value for the ``velocity_field_generator`` key in the desired aerodynamic solver's settings. """ generator_id = 'TurbVelocityFieldBts' generator_classification = 'TurbVelocityFieldBts' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['print_info'] = 'bool' settings_default['print_info'] = True settings_description[ 'print_info'] = 'Output solver-specific information in runtime' settings_types['turbulent_field'] = 'str' settings_default['turbulent_field'] = None settings_description[ 'turbulent_field'] = 'BTS file path of the velocity file' settings_types['new_orientation'] = 'str' settings_default['new_orientation'] = 'xyz' settings_description['new_orientation'] = 'New order of the axes' settings_types['u_fed'] = 'list(float)' settings_default['u_fed'] = np.zeros((3, )) settings_description[ 'u_fed'] = 'Velocity at which the turbulence field is fed into the solid' settings_types['u_out'] = 'list(float)' settings_default['u_out'] = np.zeros((3, )) settings_description[ 'u_out'] = 'Velocity to set for points outside the interpolating box' settings_types['case_with_tower'] = 'bool' settings_default['case_with_tower'] = False settings_description[ 'case_with_tower'] = 'Does the SHARPy case will include the tower in the simulation?' setting_table = settings.SettingsTable() __doc__ += setting_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.interpolator = [] self.bbox = None self.x_grid = None self.y_grid = None self.z_grid = None self.vel = None def initialise(self, in_dict): self.in_dict = in_dict settings.to_custom_types(self.in_dict, self.settings_types, self.settings_default) self.settings = self.in_dict self.x_grid, self.y_grid, self.z_grid, self.vel = self.read_turbsim_bts( self.settings['turbulent_field']) if not self.settings['new_orientation'] == 'xyz': # self.settings['new_orientation'] = 'zyx' self.x_grid, self.y_grid, self.z_grid, self.vel = self.change_orientation( self.x_grid, self.y_grid, self.z_grid, self.vel, self.settings['new_orientation']) self.bbox = self.get_field_bbox(self.x_grid, self.y_grid, self.z_grid) if self.settings['print_info']: cout.cout_wrap('The domain bbox is:', 1) cout.cout_wrap( ' x = [' + str(self.bbox[0, 0]) + ', ' + str(self.bbox[0, 1]) + ']', 1) cout.cout_wrap( ' y = [' + str(self.bbox[1, 0]) + ', ' + str(self.bbox[1, 1]) + ']', 1) cout.cout_wrap( ' z = [' + str(self.bbox[2, 0]) + ', ' + str(self.bbox[2, 1]) + ']', 1) # self.init_interpolator(x_grid, y_grid, z_grid, vel) def init_interpolator(self, x_grid, y_grid, z_grid, vel): pass # for ivel in range(3): # self.interpolator.append(interpolate.RegularGridInterpolator((x_grid, y_grid, z_grid), # vel[ivel,:,:,:], # bounds_error=False, # fill_value=self.settings['u_out'][ivel])) def generate(self, params, uext): zeta = params['zeta'] for_pos = params['for_pos'] t = params['t'] # Through "offstet" zeta can be modified to simulate the turbulence being fed to the solid # Usual method for wind turbines self.interpolate_zeta(zeta, for_pos, uext, offset=-self.settings['u_fed'] * t) def interpolate_zeta(self, zeta, for_pos, u_ext, interpolator=None, offset=np.zeros((3))): # if interpolator is None: # interpolator = self.interpolator for isurf in range(len(zeta)): _, n_m, n_n = zeta[isurf].shape # Reorder the coordinates points_list = np.zeros((n_m * n_n, 3)) ipoint = 0 for i_m in range(n_m): for i_n in range(n_n): points_list[ ipoint, :] = zeta[isurf][:, i_m, i_n] + for_pos[0:3] + offset ipoint += 1 # Interpolate list_uext = interp_rectgrid_vectorfield( points_list, (self.x_grid, self.y_grid, self.z_grid), self.vel, self.settings['u_out'], regularGrid=True, num_cores=1) # Reorder the values ipoint = 0 for i_m in range(n_m): for i_n in range(n_n): u_ext[isurf][:, i_m, i_n] = list_uext[ipoint, :] ipoint += 1 def read_turbsim_bts(self, fname): # This post may be useful to understand the function: # https://wind.nrel.gov/forum/wind/viewtopic.php?t=1384 dtype = np.dtype([ ("id", np.int16), ("nz", np.int32), ("ny", np.int32), ("tower_points", np.int32), ("ntime_steps", np.int32), ("dz", np.float32), ("dy", np.float32), ("dt", np.float32), ("u_mean", np.float32), ("HubHt", np.float32), ("Zbottom", np.float32), ("u_slope_scaling", np.float32), ("u_offset_scaling", np.float32), ("v_slope_scaling", np.float32), ("v_offset_scaling", np.float32), ("w_slope_scaling", np.float32), ("w_offset_scaling", np.float32), ("n_char_description", np.int32), ("description", np.dtype((bytes, 73))), # ("data", np.dtype((bytes, 12865632))) ("data", np.dtype((bytes, 2))) ]) fileContent = np.fromfile(fname, dtype=dtype) n_char_description = fileContent[0][17] nbytes_data = 2 * 3 * fileContent[0][4] * fileContent[0][ 1] * fileContent[0][2] dtype = np.dtype([("id", np.int16), ("nz", np.int32), ("ny", np.int32), ("tower_points", np.int32), ("ntime_steps", np.int32), ("dz", np.float32), ("dy", np.float32), ("dt", np.float32), ("u_mean", np.float32), ("HubHt", np.float32), ("Zbottom", np.float32), ("u_slope_scaling", np.float32), ("u_offset_scaling", np.float32), ("v_slope_scaling", np.float32), ("v_offset_scaling", np.float32), ("w_slope_scaling", np.float32), ("w_offset_scaling", np.float32), ("n_char_description", np.int32), ("description", np.dtype( (bytes, n_char_description))), ("data", np.dtype((bytes, nbytes_data)))]) fileContent = np.fromfile(fname, dtype=dtype) dictionary = {} for i in range(len(fileContent.dtype.names)): dictionary[fileContent.dtype.names[i]] = fileContent[0][i] scaling = np.array([ dictionary['u_slope_scaling'], dictionary['v_slope_scaling'], dictionary['w_slope_scaling'] ]) offset = np.array([ dictionary['u_offset_scaling'], dictionary['v_offset_scaling'], dictionary['w_offset_scaling'] ]) # Checks # print("Case description: ", dictionary['description']) if dictionary['description'][-1] == ".": print( "WARNING: I think there is something wrong with the case description. The length is not", n_char_description, "characters.") # print("Input", dictionary['n_char_description'], "as the number of characters of the case description") # vel_aux = np.fromstring(dictionary['data'], dtype='>i2') vel_aux = np.fromstring(dictionary['data'], dtype=np.int16) vel = np.zeros( (3, dictionary['ntime_steps'], dictionary['ny'], dictionary['nz'])) counter = -1 for ix in range(dictionary['ntime_steps']): for iz in range(dictionary['nz']): for iy in range(dictionary['ny']): for ivel in range(3): counter += 1 vel[ivel, -ix, iy, iz] = (vel_aux[counter] - offset[ivel]) / scaling[ivel] # Generate the grid height = dictionary['dz'] * (dictionary['nz'] - 1) width = dictionary['dy'] * (dictionary['ny'] - 1) x_grid = np.linspace(-dictionary['ntime_steps'] + 1, 0, dictionary['ntime_steps'] ) * dictionary['dt'] * dictionary['u_mean'] y_grid = np.linspace(-width / 2, width / 2, dictionary['ny']) if self.settings['case_with_tower']: z_grid = np.linspace(dictionary['Zbottom'], dictionary['Zbottom'] + height, dictionary['nz']) else: z_grid = np.linspace(-height / 2, height / 2, dictionary['nz']) return x_grid, y_grid, z_grid, vel def change_orientation(self, old_xgrid, old_ygrid, old_zgrid, old_vel, new_orientation_input): old_grid = [] old_grid.append(old_xgrid.copy()) old_grid.append(old_ygrid.copy()) old_grid.append(old_zgrid.copy()) new_orientation = ("%s." % new_orientation_input)[:-1] # Generate information for new_orientation if not old_vel.shape[0] == 3: print("ERROR: velocity must have three dimension") if (not (len(old_vel[0, :, 0, 0]) == len(old_xgrid))) or (not (len( old_vel[0, 0, :, 0]) == len(old_ygrid))) or (not (len( old_vel[0, 0, 0, :]) == len(old_zgrid))): print("ERROR: dimensions mismatch") return old_dim = np.array([len(old_xgrid), len(old_ygrid), len(old_zgrid)]) position_in_old = np.zeros((3), dtype=int) sign = np.array([1, 1, 1], dtype=int) for ivel in range(3): if new_orientation[0] == "-": sign[ivel] = -1 new_orientation = new_orientation[1:] if new_orientation[0] == "x": position_in_old[ivel] = 0 elif new_orientation[0] == "y": position_in_old[ivel] = 1 elif new_orientation[0] == "z": position_in_old[ivel] = 2 new_orientation = new_orientation[1:] # print("position_in_old: ", position_in_old) # Check the new orientation system new_ux = np.zeros((3), dtype=int) new_uy = np.zeros((3), dtype=int) new_uz = np.zeros((3), dtype=int) new_ux[position_in_old[0]] = sign[0] new_uy[position_in_old[1]] = sign[1] new_uz[position_in_old[2]] = sign[2] aux_ux = np.cross(new_uy, new_uz) aux_uy = np.cross(new_uz, new_ux) aux_uz = np.cross(new_ux, new_uy) if (not (np.abs(aux_ux - new_ux) < 1e-9).all()) or ( not (np.abs(aux_uy - new_uy) < 1e-9).all()) or ( not (np.abs(aux_uz - new_uz) < 1e-9).all()): print("ERROR: The new coordinate system is not coherent") print("ux error: ", aux_ux - new_ux) print("uy error: ", aux_uy - new_uy) print("uz error: ", aux_uz - new_uz) # Output variables new_grid = [None] * 3 new_dim = old_dim[position_in_old] for ivel in range(3): new_grid[ivel] = old_grid[position_in_old[ivel]] * sign[ivel] if sign[ivel] == -1: new_grid[ivel] = new_grid[ivel][::-1] new_vel = np.zeros((3, new_dim[0], new_dim[1], new_dim[2])) # print("old_dim:", old_dim) # print("new_dim:", new_dim) # print("old_vel.shape:", old_vel.shape) # print("new_vel.shape:", new_vel.shape) # These loops will index variables associated with the new grid for ix in range(new_dim[0]): for iy in range(new_dim[1]): for iz in range(new_dim[2]): new_i = np.array([ix, iy, iz]) # old_i = np.array([new_i[position_in_old[0]]*sign[0], new_i[position_in_old[1]]*sign[1], new_i[position_in_old[2]]*sign[2]]) old_i = np.array([ new_i[position_in_old[0]], new_i[position_in_old[1]], new_i[position_in_old[2]] ]) for icoord in range(3): if sign[icoord] == -1: old_i[icoord] = -1 * old_i[icoord] - 1 for ivel in range(3): # print("moving: ", position_in_old[ivel], ix, iy, iz, "to: ", ivel,new_i[0],new_i[1],new_i[2]) new_vel[ivel, new_i[0], new_i[1], new_i[2]] = old_vel[position_in_old[ivel], old_i[0], old_i[1], old_i[2]] * sign[ivel] return new_grid[0], new_grid[1], new_grid[2], new_vel def get_field_bbox(self, x_grid, y_grid, z_grid): bbox = np.zeros((3, 2)) bbox[0, :] = [np.min(x_grid), np.max(x_grid)] bbox[1, :] = [np.min(y_grid), np.max(y_grid)] bbox[2, :] = [np.min(z_grid), np.max(z_grid)] return bbox
class SaveData(BaseSolver): """ The ``SaveData`` postprocessor writes the SHARPy variables into ``hdf5`` files. The linear state space files may be saved to ``.mat`` if desired instead. It has options to save the following classes: * :class:`~sharpy.sharpy.aero.models.Aerogrid` including :class:`sharpy.sharpy.utils.datastructures.AeroTimeStepInfo` * :class:`~sharpy.sharpy.structure.beam.Beam` including :class:`sharpy.sharpy.utils.datastructures.StructTimeStepInfo` * :class:`sharpy.solvers.linearassembler.Linear` including classes in :exc:`sharpy.linear.assembler` Notes: This method saves simply the data. If you would like to preserve the SHARPy methods of the relevant classes see also :class:`sharpy.solvers.pickledata.PickleData`. """ solver_id = 'SaveData' solver_classification = 'post-processor' settings_types = dict() settings_default = dict() settings_description = dict() settings_options = dict() settings_types['folder'] = 'str' settings_default['folder'] = './output' settings_description['folder'] = 'Folder to save data' settings_types['save_aero'] = 'bool' settings_default['save_aero'] = True settings_description['save_aero'] = 'Save aerodynamic classes.' settings_types['save_struct'] = 'bool' settings_default['save_struct'] = True settings_description['save_struct'] = 'Save structural classes.' settings_types['save_linear'] = 'bool' settings_default['save_linear'] = False settings_description['save_linear'] = 'Save linear state space system.' settings_types['save_linear_uvlm'] = 'bool' settings_default['save_linear_uvlm'] = False settings_description['save_linear_uvlm'] = 'Save linear UVLM state space system. Use with caution when dealing with ' \ 'large systems.' settings_types['save_rom'] = 'bool' settings_default['save_rom'] = False settings_description['save_rom'] = 'Saves the ROM matrices and the reduced order model' settings_types['skip_attr'] = 'list(str)' settings_default['skip_attr'] = ['fortran', 'airfoils', 'airfoil_db', 'settings_types', # 'beam', 'ct_dynamic_forces_list', # 'ct_forces_list', 'ct_gamma_dot_list', 'ct_gamma_list', 'ct_gamma_star_list', 'ct_normals_list', 'ct_u_ext_list', 'ct_u_ext_star_list', 'ct_zeta_dot_list', 'ct_zeta_list', 'ct_zeta_star_list', 'dynamic_input'] settings_description['skip_attr'] = 'List of attributes to skip when writing file' settings_types['compress_float'] = 'bool' settings_default['compress_float'] = False settings_description['compress_float'] = 'Compress float' settings_types['format'] = 'str' settings_default['format'] = 'h5' settings_description['format'] = 'Save linear state space to hdf5 ``.h5`` or Matlab ``.mat`` format.' settings_options['format'] = ['h5', 'mat'] settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description, settings_options=settings_options) def __init__(self): import sharpy self.settings = None self.data = None self.folder = '' self.filename = '' self.filename_linear = '' self.ts_max = 0 self.caller = None ### specify which classes are saved as hdf5 group # see initialise and add_as_grp self.ClassesToSave = (sharpy.presharpy.presharpy.PreSharpy,) def initialise(self, data, custom_settings=None, caller=None): self.data = data if custom_settings is None: self.settings = data.settings[self.solver_id] else: self.settings = custom_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default, options=self.settings_options) # Add these anyway - therefore if you add your own skip_attr you don't have to retype all of these self.settings['skip_attr'].extend(['fortran', 'airfoils', 'airfoil_db', 'settings_types', 'ct_dynamic_forces_list', 'ct_forces_list', 'ct_gamma_dot_list', 'ct_gamma_list', 'ct_gamma_star_list', 'ct_normals_list', 'ct_u_ext_list', 'ct_u_ext_star_list', 'ct_zeta_dot_list', 'ct_zeta_list', 'ct_zeta_star_list', 'dynamic_input']) self.ts_max = self.data.ts + 1 # create folder for containing files if necessary if not os.path.exists(self.settings['folder']): os.makedirs(self.settings['folder']) self.folder = self.settings['folder'] + '/' + self.data.settings['SHARPy']['case'] + '/savedata/' if not os.path.exists(self.folder): os.makedirs(self.folder) self.filename = self.folder + self.data.settings['SHARPy']['case'] + '.data.h5' self.filename_linear = self.folder + self.data.settings['SHARPy']['case'] + '.linss.h5' if os.path.isfile(self.filename): os.remove(self.filename) # check that there is a linear system - else return setting to false if self.settings['save_linear'] or self.settings['save_linear_uvlm']: try: self.data.linear except AttributeError: cout.cout_wrap('SaveData variables ``save_linear`` and/or ``save_linear`` are True but no linear system' 'been found', 3) self.settings['save_linear'] = False self.settings['save_linear_uvlm'] = False if self.settings['format'] == 'h5': # allocate list of classes to be saved if self.settings['save_aero']: self.ClassesToSave += (sharpy.aero.models.aerogrid.Aerogrid, sharpy.utils.datastructures.AeroTimeStepInfo,) if self.settings['save_struct']: self.ClassesToSave += ( sharpy.structure.models.beam.Beam, sharpy.utils.datastructures.StructTimeStepInfo,) if self.settings['save_linear']: self.ClassesToSave += (sharpy.solvers.linearassembler.Linear, sharpy.linear.assembler.linearaeroelastic.LinearAeroelastic, sharpy.linear.assembler.linearbeam.LinearBeam, sharpy.linear.assembler.linearuvlm.LinearUVLM, sharpy.linear.src.libss.ss, sharpy.linear.src.lingebm.FlexDynamic,) if self.settings['save_linear_uvlm']: self.ClassesToSave += (sharpy.solvers.linearassembler.Linear, sharpy.linear.src.libss.ss) self.caller = caller def run(self, online=False): # Use the following statement in case the ct types are not defined and # you need them on uvlm3d # self.data.aero.timestep_info[-1].generate_ctypes_pointers() if self.settings['format'] == 'h5': file_exists = os.path.isfile(self.filename) hdfile = h5py.File(self.filename, 'a') if (online and file_exists): self.save_timestep(self.data, self.settings, self.data.ts, hdfile) else: skip_attr_init = copy.deepcopy(self.settings['skip_attr']) skip_attr_init.append('timestep_info') h5utils.add_as_grp(self.data, hdfile, grpname='data', ClassesToSave=self.ClassesToSave, SkipAttr=skip_attr_init, compress_float=self.settings['compress_float']) if self.settings['save_struct']: h5utils.add_as_grp(list(), hdfile['data']['structure'], grpname='timestep_info') if self.settings['save_aero']: h5utils.add_as_grp(list(), hdfile['data']['aero'], grpname='timestep_info') for it in range(len(self.data.structure.timestep_info)): tstep_p = self.data.structure.timestep_info[it] if tstep_p is not None: self.save_timestep(self.data, self.settings, it, hdfile) hdfile.close() if self.settings['save_linear_uvlm']: linhdffile = h5py.File(self.filename.replace('.data.h5', '.uvlmss.h5'), 'a') h5utils.add_as_grp(self.data.linear.linear_system.uvlm.ss, linhdffile, grpname='ss', ClassesToSave=self.ClassesToSave, SkipAttr=self.settings['skip_attr'], compress_float=self.settings['compress_float']) h5utils.add_as_grp(self.data.linear.linear_system.linearisation_vectors, linhdffile, grpname='linearisation_vectors', ClassesToSave=self.ClassesToSave, SkipAttr=self.settings['skip_attr'], compress_float=self.settings['compress_float']) linhdffile.close() if self.settings['save_linear']: with h5py.File(self.filename_linear, 'a') as linfile: h5utils.add_as_grp(self.data.linear.linear_system.linearisation_vectors, linfile, grpname='linearisation_vectors', ClassesToSave=self.ClassesToSave, SkipAttr=self.settings['skip_attr'], compress_float=self.settings['compress_float']) h5utils.add_as_grp(self.data.linear.ss, linfile, grpname='ss', ClassesToSave=self.ClassesToSave, SkipAttr=self.settings['skip_attr'], compress_float=self.settings['compress_float']) if self.settings['save_rom']: try: for k, rom in self.data.linear.linear_system.uvlm.rom.items(): rom.save(self.filename.replace('.data.h5', '_{:s}.rom.h5'.format(k.lower()))) except AttributeError: cout.cout_wrap('Could not locate a reduced order model to save') elif self.settings['format'] == 'mat': from scipy.io import savemat if self.settings['save_linear']: # reference-forces linearisation_vectors = self.data.linear.linear_system.linearisation_vectors matfilename = self.filename.replace('.data.h5', '.linss.mat') A, B, C, D = self.data.linear.ss.get_mats() savedict = {'A': A, 'B': B, 'C': C, 'D': D} for k, v in linearisation_vectors.items(): savedict[k] = v try: dt = self.data.linear.ss.dt savedict['dt'] = dt except AttributeError: pass savemat(matfilename, savedict) if self.settings['save_linear_uvlm']: matfilename = self.filename.replace('.data.h5', '.uvlmss.mat') linearisation_vectors = self.data.linear.linear_system.uvlm.linearisation_vectors A, B, C, D = self.data.linear.linear_system.uvlm.ss.get_mats() savedict = {'A': A, 'B': B, 'C': C, 'D': D} for k, v in linearisation_vectors.items(): savedict[k] = v try: dt = self.data.linear.ss.dt savedict['dt'] = dt except AttributeError: pass savemat(matfilename, savedict) return self.data @staticmethod def save_timestep(data, settings, ts, hdfile): if settings['save_aero']: h5utils.add_as_grp(data.aero.timestep_info[ts], hdfile['data']['aero']['timestep_info'], grpname=("%05d" % ts), ClassesToSave=(sharpy.utils.datastructures.AeroTimeStepInfo,), SkipAttr=settings['skip_attr'], compress_float=settings['compress_float']) if settings['save_struct']: if data.structure.timestep_info[ts].in_global_AFoR: tstep = data.structure.timestep_info[ts] else: tstep = data.structure.timestep_info[data.ts].copy() tstep.whole_structure_to_global_AFoR(data.structure) h5utils.add_as_grp(tstep, hdfile['data']['structure']['timestep_info'], grpname=("%05d" % ts), ClassesToSave=(sharpy.utils.datastructures.StructTimeStepInfo,), SkipAttr=settings['skip_attr'], compress_float=settings['compress_float'])
class Modal(BaseSolver): """ ``Modal`` solver class, inherited from ``BaseSolver`` Extracts the ``M``, ``K`` and ``C`` matrices from the ``Fortran`` library for the beam. Depending on the choice of modal projection, these may or may not be transformed to a state-space form to compute the eigenvalues and mode shapes of the structure. """ solver_id = 'Modal' solver_classification = 'Linear' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['print_info'] = 'bool' settings_default['print_info'] = True settings_description['print_info'] = 'Write status to screen' settings_types['folder'] = 'str' settings_default['folder'] = './output' settings_description['folder'] = 'Output folder' # solution options settings_types['rigid_body_modes'] = 'bool' settings_default['rigid_body_modes'] = False settings_description['rigid_body_modes'] = 'Write modes with rigid body mode shapes' settings_types['use_undamped_modes'] = 'bool' # basis for modal projection settings_default['use_undamped_modes'] = True settings_description['use_undamped_modes'] = 'Project the modes onto undamped mode shapes' settings_types['NumLambda'] = 'int' # no. of different modes to retain settings_default['NumLambda'] = 20 # doubles if use_undamped_modes is False settings_description['NumLambda'] = 'Number of modes to retain' settings_types['keep_linear_matrices'] = 'bool' # attach linear M,C,K matrices to output dictionary settings_default['keep_linear_matrices'] = True settings_description['keep_linear_matrices'] = 'Save M, C and K matrices to output dictionary' # output options settings_types['write_modes_vtk'] = 'bool' # write displacements mode shapes in vtk file settings_default['write_modes_vtk'] = True settings_description['write_modes_vtk'] = 'Write Paraview files with mode shapes' settings_types['print_matrices'] = 'bool' # print M,C,K matrices to dat file settings_default['print_matrices'] = False settings_description['print_matrices'] = 'Write M, C and K matrices to file' settings_types['write_dat'] = 'bool' # write modes shapes/freq./damp. to dat file settings_default['write_dat'] = True settings_description['write_dat'] = 'Write mode shapes, frequencies and damping to file' settings_types['continuous_eigenvalues'] = 'bool' settings_default['continuous_eigenvalues'] = False settings_description['continuous_eigenvalues'] = 'Use continuous time eigenvalues' settings_types['dt'] = 'float' settings_default['dt'] = 0 settings_description['dt'] = 'Time step to compute discrete time eigenvalues' settings_types['delta_curved'] = 'float' settings_default['delta_curved'] = 1e-2 settings_description['delta_curved'] = 'Threshold for linear expressions in rotation formulas' settings_types['plot_eigenvalues'] = 'bool' settings_default['plot_eigenvalues'] = False settings_description['plot_eigenvalues'] = 'Plot to screen root locus diagram' settings_types['max_rotation_deg'] = 'float' settings_default['max_rotation_deg'] = 15. settings_description['max_rotation_deg'] = 'Scale mode shape to have specified maximum rotation' settings_types['max_displacement'] = 'float' settings_default['max_displacement'] = 0.15 settings_description['max_displacement'] = 'Scale mode shape to have specified maximum displacement' settings_types['use_custom_timestep'] = 'int' settings_default['use_custom_timestep'] = -1 settings_description['use_custom_timestep'] = 'If > -1, it will use that time step geometry for calculating the modes' settings_types['rigid_modes_cg'] = 'bool' settings_default['rigid_modes_cg'] = False settings_description['rigid_modes_cg'] = 'Modify the ridid body modes such that they are defined wrt to the CG' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.data = None self.settings = None self.folder = None self.eigenvalue_table = None self.filename_freq = None self.filename_damp = None self.filename_shapes = None self.rigid_body_motion = None def initialise(self, data, custom_settings=None): self.data = data if custom_settings is None: self.settings = data.settings[self.solver_id] else: self.settings = custom_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) self.rigid_body_motion = self.settings['rigid_body_modes'].value self.data.ts = len(self.data.structure.timestep_info) - 1 if self.settings['use_custom_timestep'].value > -1: self.data.ts = self.settings['use_custom_timestep'].value # load info from dyn dictionary self.data.structure.add_unsteady_information( self.data.structure.dyn_dict, self.data.ts) # create folder for containing files if necessary if not os.path.exists(self.settings['folder']): os.makedirs(self.settings['folder']) self.folder = (self.settings['folder'] + '/' + self.data.settings['SHARPy']['case'] + '/beam_modal_analysis/') if not os.path.exists(self.folder): os.makedirs(self.folder) self.filename_freq = (self.folder + 'tstep' + ("%06d" % self.data.ts) + '_ModalFrequencies.dat') self.filename_damp = (self.folder + 'tstep' + ("%06d" % self.data.ts) + '_ModalDamping.dat') self.filename_shapes = (self.folder + 'tstep' + ("%06d" % self.data.ts) + '_ModalShape') if self.settings['print_info']: cout.cout_wrap('Structural eigenvalues') # self.eigenvalue_table = cout.TablePrinter(7, 12, ['d', 'f', 'f', 'f', 'f', 'f', 'f']) # self.eigenvalue_table.print_header(['mode', 'eval_real', 'eval_imag', 'freq_n (Hz)', 'freq_d (Hz)', # 'damping', 'period (s)']) self.eigenvalue_table = modalutils.EigenvalueTable() self.eigenvalue_table.print_header(self.eigenvalue_table.headers) def run(self): r""" Extracts the eigenvalues and eigenvectors of the clamped structure. If ``use_undamped_modes == True`` then the free vibration modes of the clamped structure are found solving: .. math:: \mathbf{M\,\ddot{\eta}} + \mathbf{K\,\eta} = 0 that flows down to solving the non-trivial solutions to: .. math:: (-\omega_n^2\,\mathbf{M} + \mathbf{K})\mathbf{\Phi} = 0 On the other hand, if the damped modes are chosen because the system has damping, the free vibration modes are found solving the equation of motion of the form: .. math:: \mathbf{M\,\ddot{\eta}} + \mathbf{C\,\dot{\eta}} + \mathbf{K\,\eta} = 0 which can be written in state space form, with the state vector :math:`\mathbf{x} = [\eta^T,\,\dot{\eta}^T]^T` as .. math:: \mathbf{\dot{x}} = \begin{bmatrix} 0 & \mathbf{I} \\ -\mathbf{M^{-1}K} & -\mathbf{M^{-1}C} \end{bmatrix} \mathbf{x} and therefore the mode shapes and frequencies correspond to the solution of the eigenvalue problem .. math:: \mathbf{A\,\Phi} = \mathbf{\Lambda\,\Phi}. From the eigenvalues, the following system characteristics are provided: * Natural Frequency: :math:`\omega_n = |\lambda|` * Damped natural frequency: :math:`\omega_d = \text{Im}(\lambda) = \omega_n \sqrt{1-\zeta^2}` * Damping ratio: :math:`\zeta = -\frac{\text{Re}(\lambda)}{\omega_n}` In addition to the above, the modal output dictionary includes the following: * ``M``: Tangent mass matrix * ``C``: Tangent damping matrix * ``K``: Tangent stiffness matrix * ``Ccut``: Modal damping matrix :math:`\mathbf{C}_m = \mathbf{\Phi}^T\mathbf{C}\mathbf{\Phi}` * ``Kin_damp``: Forces gain matrix (when damped): :math:`K_{in} = \mathbf{\Phi}_L^T \mathbf{M}^{-1}` * ``eigenvectors``: Right eigenvectors * ``eigenvectors_left``: Left eigenvectors given when the system is damped Returns: PreSharpy: updated data object with modal analysis as part of the last structural time step. """ # Number of degrees of freedom num_str_dof = self.data.structure.num_dof.value if self.rigid_body_motion: num_rigid_dof = 10 else: num_rigid_dof = 0 num_dof = num_str_dof + num_rigid_dof # if NumLambda # Initialize matrices FullMglobal = np.zeros((num_dof, num_dof), dtype=ct.c_double, order='F') FullKglobal = np.zeros((num_dof, num_dof), dtype=ct.c_double, order='F') FullCglobal = np.zeros((num_dof, num_dof), dtype=ct.c_double, order='F') if self.rigid_body_motion: # Settings for the assembly of the matrices # try: # full_matrix_settings = self.data.settings['StaticCoupled']['structural_solver_settings'] # full_matrix_settings['dt'] = ct.c_double(0.01) # Dummy: required but not used # full_matrix_settings['newmark_damp'] = ct.c_double(1e-2) # Dummy: required but not used # except KeyError: # full_matrix_settings = self.data.settings['DynamicCoupled']['structural_solver_settings'] import sharpy.solvers._basestructural as basestructuralsolver full_matrix_settings = basestructuralsolver._BaseStructural().settings_default settings.to_custom_types(full_matrix_settings, basestructuralsolver._BaseStructural().settings_types, full_matrix_settings) # Obtain the tangent mass, damping and stiffness matrices FullMglobal, FullCglobal, FullKglobal, FullQ = xbeamlib.xbeam3_asbly_dynamic(self.data.structure, self.data.structure.timestep_info[self.data.ts], full_matrix_settings) else: xbeamlib.cbeam3_solv_modal(self.data.structure, self.settings, self.data.ts, FullMglobal, FullCglobal, FullKglobal) # Print matrices if self.settings['print_matrices'].value: np.savetxt(self.folder + "Mglobal.dat", FullMglobal, fmt='%.12f', delimiter='\t', newline='\n') np.savetxt(self.folder + "Cglobal.dat", FullCglobal, fmt='%.12f', delimiter='\t', newline='\n') np.savetxt(self.folder + "Kglobal.dat", FullKglobal, fmt='%.12f', delimiter='\t', newline='\n') # Check if the damping matrix is zero (issue working) if self.settings['use_undamped_modes'].value: zero_FullCglobal = True for i,j in itertools.product(range(num_dof),range(num_dof)): if(np.absolute(FullCglobal[i, j]) > np.finfo(float).eps): zero_FullCglobal = False warnings.warn( 'Projecting a system with damping on undamped modal shapes') break # Check if the damping matrix is skew-symmetric # skewsymmetric_FullCglobal = True # for i in range(num_dof): # for j in range(i:num_dof): # if((i==j) and (np.absolute(FullCglobal[i, j]) > np.finfo(float).eps)): # skewsymmetric_FullCglobal = False # elif(np.absolute(FullCglobal[i, j] + FullCglobal[j, i]) > np.finfo(float).eps): # skewsymmetric_FullCglobal = False NumLambda = min(num_dof, self.settings['NumLambda'].value) if self.settings['use_undamped_modes'].value: # Solve for eigenvalues (with unit eigenvectors) eigenvalues,eigenvectors=np.linalg.eig( np.linalg.solve(FullMglobal,FullKglobal)) eigenvectors_left=None # Define vibration frequencies and damping freq_natural = np.sqrt(eigenvalues) order = np.argsort(freq_natural)[:NumLambda] freq_natural = freq_natural[order] #freq_damped = freq_natural eigenvalues = eigenvalues[order] eigenvectors = eigenvectors[:,order] damping = np.zeros((NumLambda,)) else: # State-space model Minv_neg = -np.linalg.inv(FullMglobal) A = np.zeros((2*num_dof, 2*num_dof), dtype=ct.c_double, order='F') A[:num_dof, num_dof:] = np.eye(num_dof) A[num_dof:, :num_dof] = np.dot(Minv_neg, FullKglobal) A[num_dof:, num_dof:] = np.dot(Minv_neg, FullCglobal) # Solve the eigenvalues problem eigenvalues, eigenvectors_left, eigenvectors = \ sc.linalg.eig(A,left=True,right=True) freq_natural = np.abs(eigenvalues) damping = np.zeros_like(freq_natural) iiflex = freq_natural > 1e-16*np.mean(freq_natural) # Pick only structural modes damping[iiflex] = -eigenvalues[iiflex].real/freq_natural[iiflex] freq_damped = freq_natural * np.sqrt(1-damping**2) # Order & downselect complex conj: # this algorithm assumes that complex conj eigenvalues appear consecutively # in eigenvalues. For symmetrical systems, this relies on the fact that: # - complex conj eigenvalues have the same absolute value (to machine # precision) # - couples of eigenvalues with multiplicity higher than 1, show larger # numerical difference order = np.argsort(freq_damped)[:2*NumLambda] freq_damped = freq_damped[order] freq_natural = freq_natural[order] eigenvalues = eigenvalues[order] include = np.ones((2*NumLambda,), dtype=np.bool) ii = 0 tol_rel = np.finfo(float).eps * freq_damped[ii] while ii < 2*NumLambda: # check complex if np.abs(eigenvalues[ii].imag) > 0.: if np.abs(eigenvalues[ii+1].real-eigenvalues[ii].real) > tol_rel or\ np.abs(eigenvalues[ii+1].imag+eigenvalues[ii].imag) > tol_rel: raise NameError('Complex conjugate expected but not found!') ii += 1 try: include[ii] = False except IndexError: pass ii += 1 freq_damped = freq_damped[include] eigenvalues = eigenvalues[include] if self.settings['continuous_eigenvalues']: if self.settings['dt'].value == 0.: raise ValueError('Cannot compute the continuous eigenvalues without a dt value') eigenvalues = np.log(eigenvalues)/self.settings['dt'].value order = order[include] damping = damping[order] eigenvectors = eigenvectors[:, order] eigenvectors_left = eigenvectors_left[:, order].conj() # Modify rigid body modes for them to be defined wrt the CG if self.settings['rigid_modes_cg']: if not eigenvectors_left: eigenvectors = self.free_free_modes(eigenvectors, FullMglobal) # Scaling eigenvectors, eigenvectors_left = self.scale_modes_unit_mass_matrix(eigenvectors, FullMglobal, eigenvectors_left) # Other terms required for state-space realisation # non-zero damping matrix # Modal damping matrix if self.settings['use_undamped_modes'] and not(zero_FullCglobal): Ccut = np.dot(eigenvectors.T, np.dot(FullCglobal, eigenvectors)) else: Ccut = None # forces gain matrix (nodal -> modal) if not self.settings['use_undamped_modes']: Kin_damp = np.dot(eigenvectors_left[num_dof:, :].T, -Minv_neg) else: Kin_damp = None # Plot eigenvalues using matplotlib if specified in settings if self.settings['plot_eigenvalues']: import matplotlib.pyplot as plt fig = plt.figure() plt.scatter(eigenvalues.real, eigenvalues.imag) plt.show() plt.savefig(self.folder + 'eigenvalues.png', transparent=True, bbox_inches='tight') # Write dat files if self.settings['write_dat'].value: if type(eigenvalues) == complex: np.savetxt(self.folder + "eigenvalues.dat", eigenvalues.view(float).reshape(-1, 2), fmt='%.12f', delimiter='\t', newline='\n') else: np.savetxt(self.folder + "eigenvalues.dat", eigenvalues.view(float), fmt='%.12f', delimiter='\t', newline='\n') np.savetxt(self.folder + "eigenvectors.dat", eigenvectors[:num_dof].real, fmt='%.12f', delimiter='\t', newline='\n') if not self.settings['use_undamped_modes'].value: np.savetxt(self.folder + 'frequencies.dat', freq_damped[:NumLambda], fmt='%e', delimiter='\t', newline='\n') else: np.savetxt(self.folder + 'frequencies.dat', freq_natural[:NumLambda], fmt='%e', delimiter='\t', newline='\n') np.savetxt(self.filename_damp, damping[:NumLambda], fmt='%e', delimiter='\t', newline='\n') # Write vtk if self.settings['write_modes_vtk'].value: try: self.data.aero aero_model = True except AttributeError: warnings.warn('No aerodynamic model found - unable to project the mode onto aerodynamic grid') aero_model = False if aero_model: modalutils.write_modes_vtk( self.data, eigenvectors[:num_dof], NumLambda, self.filename_shapes, self.settings['max_rotation_deg'], self.settings['max_displacement'], ts=self.settings['use_custom_timestep'].value) outdict = dict() if self.settings['use_undamped_modes']: outdict['modes'] = 'undamped' outdict['freq_natural'] = freq_natural if not zero_FullCglobal: outdict['warning'] =\ 'system with damping: mode shapes and natural frequencies do not account for damping!' else: outdict['modes'] = 'damped' outdict['freq_damped'] = freq_damped outdict['freq_natural'] = freq_natural outdict['damping'] = damping outdict['eigenvalues'] = eigenvalues outdict['eigenvectors'] = eigenvectors if Ccut is not None: outdict['Ccut'] = Ccut if Kin_damp is not None: outdict['Kin_damp'] = Kin_damp if not self.settings['use_undamped_modes']: outdict['eigenvectors_left'] = eigenvectors_left if self.settings['keep_linear_matrices'].value: outdict['M'] = FullMglobal outdict['C'] = FullCglobal outdict['K'] = FullKglobal self.data.structure.timestep_info[self.data.ts].modal = outdict if self.settings['print_info']: if self.settings['use_undamped_modes']: self.eigenvalue_table.print_evals(np.sqrt(eigenvalues[:NumLambda])*1j) else: self.eigenvalue_table.print_evals(eigenvalues[:NumLambda]) return self.data def scale_modes_unit_mass_matrix(self, eigenvectors, FullMglobal, eigenvectors_left=None): if self.settings['use_undamped_modes']: # mass normalise (diagonalises M and K) dfact = np.diag(np.dot(eigenvectors.T, np.dot(FullMglobal, eigenvectors))) eigenvectors = (1./np.sqrt(dfact))*eigenvectors else: # unit normalise (diagonalises A) if not self.rigid_body_motion: for ii in range(eigenvectors.shape[1]): # Issue - dot product = 0 when you have arbitrary damping fact = 1./np.sqrt(np.dot(eigenvectors_left[:, ii], eigenvectors[:, ii])) eigenvectors_left[:, ii] = fact*eigenvectors_left[:, ii] eigenvectors[:, ii] = fact*eigenvectors[:, ii] return eigenvectors, eigenvectors_left def free_free_modes(self, phi, M): r""" Returns the rigid body modes defined with respect to the centre of gravity The transformation from the modes defined at the FoR A origin, :math:`\boldsymbol{\Phi}`, to the modes defined using the centre of gravity as a reference is .. math:: \boldsymbol{\Phi}_{rr,CG}|_{TRA} = \boldsymbol{\Phi}_{RR}|_{TRA} + \tilde{\mathbf{r}}_{CG} \boldsymbol{\Phi}_{RR}|_{ROT} .. math:: \boldsymbol{\Phi}_{rr,CG}|_{ROT} = \boldsymbol{\Phi}_{RR}|_{ROT} Returns: (np.array): Transformed eigenvectors """ # NG - 26/7/19 This is the transformation being performed by K_vec # Leaving this here for now in case it becomes necessary # .. math:: \boldsymbol{\Phi}_{ss,CG}|_{TRA} = \boldsymbol{\Phi}_{SS}|_{TRA} +\boldsymbol{\Phi}_{RS}|_{TRA} - # \tilde{\mathbf{r}}_{A}\boldsymbol{\Phi}_{RS}|_{ROT} # # .. math:: \boldsymbol{\Phi}_{ss,CG}|_{ROT} = \boldsymbol{\Phi}_{SS}|_{ROT} # + (\mathbf{T}(\boldsymbol{\Psi})^\top)^{-1}\boldsymbol{\Phi}_{RS}|_{ROT} if not self.rigid_body_motion: warnings.warn('No rigid body modes to transform because the structure is clamped') return phi else: pos = self.data.structure.timestep_info[self.data.ts].pos r_cg = modalutils.cg(M) jj = 0 K_vec = np.zeros((phi.shape[0], phi.shape[0])) jj_for_vel = range(self.data.structure.num_dof.value, self.data.structure.num_dof.value + 3) jj_for_rot = range(self.data.structure.num_dof.value + 3, self.data.structure.num_dof.value + 6) for node_glob in range(self.data.structure.num_node): ### detect bc at node (and no. of dofs) bc_here = self.data.structure.boundary_conditions[node_glob] if bc_here == 1: # clamp (only rigid-body) dofs_here = 0 jj_tra, jj_rot = [], [] continue elif bc_here == -1 or bc_here == 0: # (rigid+flex body) dofs_here = 6 jj_tra = 6 * self.data.structure.vdof[node_glob] + np.array([0, 1, 2], dtype=int) jj_rot = 6 * self.data.structure.vdof[node_glob] + np.array([3, 4, 5], dtype=int) # jj_tra=[jj ,jj+1,jj+2] # jj_rot=[jj+3,jj+4,jj+5] else: raise NameError('Invalid boundary condition (%d) at node %d!' \ % (bc_here, node_glob)) jj += dofs_here ee, node_loc = self.data.structure.node_master_elem[node_glob, :] psi = self.data.structure.timestep_info[self.data.ts].psi[ee, node_loc, :] Ra = pos[node_glob, :] # in A FoR with respect to G K_vec[np.ix_(jj_tra, jj_tra)] += np.eye(3) K_vec[np.ix_(jj_tra, jj_for_vel)] += np.eye(3) K_vec[np.ix_(jj_tra, jj_for_rot)] -= algebra.skew(Ra) K_vec[np.ix_(jj_rot, jj_rot)] += np.eye(3) K_vec[np.ix_(jj_rot, jj_for_rot)] += np.linalg.inv(algebra.crv2tan(psi).T) # Rigid-Rigid modes transform Krr = np.eye(10) Krr[np.ix_([0, 1, 2], [3, 4, 5])] += algebra.skew(r_cg) # Assemble transformed modes phirr = Krr.dot(phi[-10:, :10]) phiss = K_vec.dot(phi[:, 10:]) # Get rigid body modes to be positive in translation and rotation for i in range(10): ind = np.argmax(np.abs(phirr[:, i])) phirr[:, i] = np.sign(phirr[ind, i]) * phirr[:, i] # NG - 26/7/19 - Transformation of the rigid part of the elastic modes ended up not being necessary but leaving # here in case it becomes useful in the future phit = np.block([np.zeros((phi.shape[0], 10)), phi[:, 10:]]) phit[-10:, :10] = phirr return phit
class StraightWake(generator_interface.BaseGenerator): r""" Straight wake shape generator ``StraightWake`` class inherited from ``BaseGenerator`` The object creates a straight wake shedding from the trailing edge based on the time step ``dt``, the incoming velocity magnitude ``u_inf`` and direction ``u_inf_direction`` """ generator_id = 'StraightWake' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['u_inf'] = 'float' settings_default['u_inf'] = 1. # None settings_description['u_inf'] = 'Free stream velocity magnitude' settings_types['u_inf_direction'] = 'list(float)' settings_default['u_inf_direction'] = np.array([1.0, 0, 0]) # None settings_description[ 'u_inf_direction'] = '``x``, ``y`` and ``z`` relative components of the free stream velocity' settings_types['dt'] = 'float' settings_default['dt'] = 0.1 # None settings_description['dt'] = 'Time step' settings_types['dx1'] = 'float' settings_default['dx1'] = -1.0 settings_description['dx1'] = 'Size of the first wake panel' settings_types['ndx1'] = 'int' settings_default['ndx1'] = 1 settings_description['ndx1'] = 'Number of panels with size ``dx1``' settings_types['r'] = 'float' settings_default['r'] = 1. settings_description['r'] = 'Growth rate after ``ndx1`` panels' settings_types['dxmax'] = 'float' settings_default['dxmax'] = -1.0 settings_description['dxmax'] = 'Maximum panel size' setting_table = settings.SettingsTable() __doc__ += setting_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.in_dict = dict() self.u_inf = 0. self.u_inf_direction = None self.dt = None def initialise(self, data, in_dict=None): self.in_dict = in_dict # For backwards compatibility if len(self.in_dict.keys()) == 0: cout.cout_wrap( "WARNING: The code will run for backwards compatibility. \ In future releases you will need to define a 'wake_shape_generator' in ``AerogridLoader''. \ Please, check the documentation", 3) # Look for an aerodynamic solver if 'StaticUvlm' in data.settings: aero_solver_name = 'StaticUvlm' aero_solver_settings = data.settings['StaticUvlm'] elif 'SHWUvlm' in data.settings: aero_solver_name = 'SHWUvlm' aero_solver_settings = data.settings['SHWUvlm'] elif 'StaticCoupled' in data.settings: aero_solver_name = data.settings['StaticCoupled'][ 'aero_solver'] aero_solver_settings = data.settings['StaticCoupled'][ 'aero_solver_settings'] elif 'StaticCoupledRBM' in data.settings: aero_solver_name = data.settings['StaticCoupledRBM'][ 'aero_solver'] aero_solver_settings = data.settings['StaticCoupledRBM'][ 'aero_solver_settings'] elif 'DynamicCoupled' in data.settings: aero_solver_name = data.settings['DynamicCoupled'][ 'aero_solver'] aero_solver_settings = data.settings['DynamicCoupled'][ 'aero_solver_settings'] elif 'StepUvlm' in data.settings: aero_solver_name = 'StepUvlm' aero_solver_settings = data.settings['StepUvlm'] else: raise RuntimeError("ERROR: aerodynamic solver not found") # Get the minimum parameters needed to define the wake aero_solver = solver_interface.solver_from_string(aero_solver_name) settings.to_custom_types(aero_solver_settings, aero_solver.settings_types, aero_solver.settings_default) if 'dt' in aero_solver_settings.keys(): dt = aero_solver_settings['dt'].value elif 'rollup_dt' in aero_solver_settings.keys(): dt = aero_solver_settings['rollup_dt'].value else: # print(aero_solver['velocity_field_input']['u_inf']) dt = 1. / aero_solver_settings['velocity_field_input'][ 'u_inf'].value self.in_dict = { 'u_inf': aero_solver_settings['velocity_field_input']['u_inf'], 'u_inf_direction': aero_solver_settings['velocity_field_input'] ['u_inf_direction'], 'dt': dt } settings.to_custom_types(self.in_dict, self.settings_types, self.settings_default, no_ctype=True) self.u_inf = self.in_dict['u_inf'] self.u_inf_direction = self.in_dict['u_inf_direction'] self.dt = self.in_dict['dt'] if self.in_dict['dx1'] == -1: self.dx1 = self.u_inf * self.dt else: self.dx1 = self.in_dict['dx1'] self.ndx1 = self.in_dict['ndx1'] self.r = self.in_dict['r'] if self.in_dict['dxmax'] == -1: self.dxmax = self.dx1 def generate(self, params): # Renaming for convenience zeta = params['zeta'] zeta_star = params['zeta_star'] gamma = params['gamma'] gamma_star = params['gamma_star'] nsurf = len(zeta) for isurf in range(nsurf): M, N = zeta_star[isurf][0, :, :].shape for j in range(N): zeta_star[isurf][:, 0, j] = zeta[isurf][:, -1, j] zeta_star[isurf][:, 1, j] = zeta_star[ isurf][:, 0, j] + self.dx1 * self.u_inf_direction for i in range(2, self.ndx1 + 1): zeta_star[isurf][:, i, j] = zeta_star[ isurf][:, i - 1, j] + self.dx1 * self.u_inf_direction for i in range(self.ndx1 + 1, M): # print(self.dx1, self.r, i, self.ndx1) deltax = self.dx1 * self.r**(i - self.ndx1) if deltax > self.dxmax: deltax = self.dxmax zeta_star[isurf][:, i, j] = zeta_star[ isurf][:, i - 1, j] + deltax * self.u_inf_direction gamma[isurf] *= 0. gamma_star[isurf] *= 0.
class RigidDynamicPrescribedStep(BaseSolver): solver_id = 'RigidDynamicPrescribedStep' solver_classification = 'structural' settings_types = _BaseStructural.settings_types.copy() settings_default = _BaseStructural.settings_default.copy() settings_description = _BaseStructural.settings_description.copy() settings_types['dt'] = 'float' settings_default['dt'] = 0.01 settings_description['dt'] = 'Time step of simulation' settings_types['num_steps'] = 'int' settings_default['num_steps'] = 500 settings_description['num_steps'] = 'Number of timesteps to be run' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.data = None self.settings = None def initialise(self, data, custom_settings=None): self.data = data if custom_settings is None: self.settings = data.settings[self.solver_id] else: self.settings = custom_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) # load info from dyn dictionary self.data.structure.add_unsteady_information( self.data.structure.dyn_dict, self.settings['num_steps'].value) def run(self, structural_step=None, dt=None): if structural_step is None: structural_step = self.data.structure.timestep_info[-1] if dt is None: dt = self.settings['dt'].value if self.data.ts > 0: try: structural_step.for_vel[:] = self.data.structure.dynamic_input[ self.data.ts - 1]['for_vel'] structural_step.for_acc[:] = self.data.structure.dynamic_input[ self.data.ts - 1]['for_acc'] except IndexError: pass Temp = np.linalg.inv( np.eye(4) + 0.25 * algebra.quadskew(structural_step.for_vel[3:6]) * dt) structural_step.quat = np.dot( Temp, np.dot( np.eye(4) - 0.25 * algebra.quadskew(structural_step.for_vel[3:6]) * dt, structural_step.quat)) xbeamlib.cbeam3_solv_disp2state(self.data.structure, structural_step) # xbeamlib.cbeam3_step_nlndyn(self.data.structure, # self.settings, # self.data.ts, # structural_step, # dt=dt) self.extract_resultants(structural_step) if self.data.ts > 0: self.data.structure.integrate_position(structural_step, self.settings['dt'].value) return self.data def add_step(self): self.data.structure.next_step() def next_step(self): pass # self.data.structure.next_step() # ts = len(self.data.structure.timestep_info) - 1 # if ts > 0: # self.data.structure.timestep_info[ts].for_vel[:] = self.data.structure.dynamic_input[ts - 1]['for_vel'] # self.data.structure.timestep_info[ts].for_acc[:] = self.data.structure.dynamic_input[ts - 1]['for_acc'] # self.data.structure.timestep_info[ts].unsteady_applied_forces[:] = self.data.structure.dynamic_input[ts - 1]['dynamic_forces'] def extract_resultants(self, step=None): if step is None: step = self.data.structure.timestep_info[-1] applied_forces = self.data.structure.nodal_b_for_2_a_for( step.steady_applied_forces + step.unsteady_applied_forces, step) applied_forces_copy = applied_forces.copy() gravity_forces_copy = step.gravity_forces.copy() for i_node in range(self.data.structure.num_node): applied_forces_copy[i_node, 3:6] += algebra.cross3( step.pos[i_node, :], applied_forces_copy[i_node, 0:3]) gravity_forces_copy[i_node, 3:6] += algebra.cross3( step.pos[i_node, :], gravity_forces_copy[i_node, 0:3]) totals = np.sum(applied_forces_copy + gravity_forces_copy, axis=0) step.total_forces = np.sum(applied_forces_copy, axis=0) step.total_gravity_forces = np.sum(gravity_forces_copy, axis=0) return totals[0:3], totals[3:6] def update(self, tstep=None): self.create_q_vector(tstep) def create_q_vector(self, tstep=None): import sharpy.structure.utils.xbeamlib as xb if tstep is None: tstep = self.data.structure.timestep_info[-1] xb.xbeam_solv_disp2state(self.data.structure, tstep)
class DynamicCoupled(BaseSolver): """ The ``DynamicCoupled`` solver couples the aerodynamic and structural solvers of choice to march forward in time the aeroelastic system's solution. Using the ``DynamicCoupled`` solver requires that an instance of the ``StaticCoupled`` solver is called in the SHARPy solution ``flow`` when defining the problem case. """ solver_id = 'DynamicCoupled' solver_classification = 'Coupled' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['print_info'] = 'bool' settings_default['print_info'] = True settings_description['print_info'] = 'Write status to screen' settings_types['structural_solver'] = 'str' settings_default['structural_solver'] = None settings_description[ 'structural_solver'] = 'Structural solver to use in the coupled simulation' settings_types['structural_solver_settings'] = 'dict' settings_default['structural_solver_settings'] = None settings_description[ 'structural_solver_settings'] = 'Dictionary of settings for the structural solver' settings_types['aero_solver'] = 'str' settings_default['aero_solver'] = None settings_description[ 'aero_solver'] = 'Aerodynamic solver to use in the coupled simulation' settings_types['aero_solver_settings'] = 'dict' settings_default['aero_solver_settings'] = None settings_description[ 'aero_solver_settings'] = 'Dictionary of settings for the aerodynamic solver' settings_types['n_time_steps'] = 'int' settings_default['n_time_steps'] = None settings_description[ 'n_time_steps'] = 'Number of time steps for the simulation' settings_types['dt'] = 'float' settings_default['dt'] = None settings_description['dt'] = 'Time step' settings_types['fsi_substeps'] = 'int' settings_default['fsi_substeps'] = 70 settings_description['fsi_substeps'] = 'Max iterations in the FSI loop' settings_types['fsi_tolerance'] = 'float' settings_default['fsi_tolerance'] = 1e-5 settings_description[ 'fsi_tolerance'] = 'Convergence threshold for the FSI loop' settings_types['structural_substeps'] = 'int' settings_default['structural_substeps'] = 0 # 0 is normal coupled sim. settings_description[ 'structural_substeps'] = 'Number of extra structural time steps per aero time step. 0 is a fully coupled simulation.' settings_types['relaxation_factor'] = 'float' settings_default['relaxation_factor'] = 0.2 settings_description[ 'relaxation_factor'] = 'Relaxation parameter in the FSI iteration. 0 is no relaxation and -> 1 is very relaxed' settings_types['final_relaxation_factor'] = 'float' settings_default['final_relaxation_factor'] = 0.0 settings_description[ 'final_relaxation_factor'] = 'Relaxation factor reached in ``relaxation_steps`` with ``dynamic_relaxation`` on' settings_types['minimum_steps'] = 'int' settings_default['minimum_steps'] = 3 settings_description[ 'minimum_steps'] = 'Number of minimum FSI iterations before convergence' settings_types['relaxation_steps'] = 'int' settings_default['relaxation_steps'] = 100 settings_description[ 'relaxation_steps'] = 'Length of the relaxation factor ramp between ``relaxation_factor`` and ``final_relaxation_factor`` with ``dynamic_relaxation`` on' settings_types['dynamic_relaxation'] = 'bool' settings_default['dynamic_relaxation'] = False settings_description[ 'dynamic_relaxation'] = 'Controls if relaxation factor is modified during the FSI iteration process' settings_types['postprocessors'] = 'list(str)' settings_default['postprocessors'] = list() settings_description[ 'postprocessors'] = 'List of the postprocessors to run at the end of every time step' settings_types['postprocessors_settings'] = 'dict' settings_default['postprocessors_settings'] = dict() settings_description[ 'postprocessors_settings'] = 'Dictionary with the applicable settings for every ``psotprocessor``. Every ``postprocessor`` needs its entry, even if empty' settings_types['controller_id'] = 'dict' settings_default['controller_id'] = dict() settings_description[ 'controller_id'] = 'Dictionary of id of every controller (key) and its type (value)' settings_types['controller_settings'] = 'dict' settings_default['controller_settings'] = dict() settings_description[ 'controller_settings'] = 'Dictionary with settings (value) of every controller id (key)' settings_types['cleanup_previous_solution'] = 'bool' settings_default['cleanup_previous_solution'] = False settings_description[ 'cleanup_previous_solution'] = 'Controls if previous ``timestep_info`` arrays are reset before running the solver' settings_types['include_unsteady_force_contribution'] = 'bool' settings_default['include_unsteady_force_contribution'] = False settings_description[ 'include_unsteady_force_contribution'] = 'If on, added mass contribution is added to the forces. This depends on the time derivative of the bound circulation. Check ``filter_gamma_dot`` in the aero solver' settings_types['steps_without_unsteady_force'] = 'int' settings_default['steps_without_unsteady_force'] = 0 settings_description[ 'steps_without_unsteady_force'] = 'Number of initial timesteps that don\'t include unsteady forces contributions. This avoids oscillations due to no perfectly trimmed initial conditions' settings_types['pseudosteps_ramp_unsteady_force'] = 'int' settings_default['pseudosteps_ramp_unsteady_force'] = 0 settings_description[ 'pseudosteps_ramp_unsteady_force'] = 'Length of the ramp with which unsteady force contribution is introduced every time step during the FSI iteration process' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.data = None self.settings = None self.structural_solver = None self.aero_solver = None self.print_info = False self.res = 0.0 self.res_dqdt = 0.0 self.res_dqddt = 0.0 self.previous_force = None self.dt = 0. self.substep_dt = 0. self.initial_n_substeps = None self.predictor = False self.residual_table = None self.postprocessors = dict() self.with_postprocessors = False self.controllers = None self.time_aero = 0. self.time_struc = 0. def get_g(self): """ Getter for ``g``, the gravity value """ return self.structural_solver.settings['gravity'].value def set_g(self, new_g): """ Setter for ``g``, the gravity value """ self.structural_solver.settings['gravity'] = ct.c_double(new_g) def get_rho(self): """ Getter for ``rho``, the density value """ return self.aero_solver.settings['rho'].value def set_rho(self, new_rho): """ Setter for ``rho``, the density value """ self.aero_solver.settings['rho'] = ct.c_double(new_rho) def initialise(self, data, custom_settings=None): """ Controls the initialisation process of the solver, including processing the settings and initialising the aero and structural solvers, postprocessors and controllers. """ self.data = data if custom_settings is None: self.settings = data.settings[self.solver_id] else: self.settings = custom_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) self.original_settings = copy.deepcopy(self.settings) self.dt = self.settings['dt'] self.substep_dt = (self.dt.value / (self.settings['structural_substeps'].value + 1)) self.initial_n_substeps = self.settings['structural_substeps'].value self.print_info = self.settings['print_info'] if self.settings['cleanup_previous_solution']: # if there's data in timestep_info[>0], copy the last one to # timestep_info[0] and remove the rest self.cleanup_timestep_info() self.structural_solver = solver_interface.initialise_solver( self.settings['structural_solver']) self.structural_solver.initialise( self.data, self.settings['structural_solver_settings']) self.aero_solver = solver_interface.initialise_solver( self.settings['aero_solver']) self.aero_solver.initialise(self.structural_solver.data, self.settings['aero_solver_settings']) self.data = self.aero_solver.data # initialise postprocessors self.postprocessors = dict() if self.settings['postprocessors']: self.with_postprocessors = True for postproc in self.settings['postprocessors']: self.postprocessors[postproc] = solver_interface.initialise_solver( postproc) self.postprocessors[postproc].initialise( self.data, self.settings['postprocessors_settings'][postproc]) # initialise controllers self.controllers = dict() self.with_controllers = False if self.settings['controller_id']: self.with_controllers = True for controller_id, controller_type in self.settings[ 'controller_id'].items(): self.controllers[controller_id] = ( controller_interface.initialise_controller(controller_type)) self.controllers[controller_id].initialise( self.settings['controller_settings'][controller_id], controller_id) # print information header if self.print_info: self.residual_table = cout.TablePrinter( 8, 12, ['g', 'f', 'g', 'f', 'f', 'f', 'e', 'e']) self.residual_table.field_length[0] = 5 self.residual_table.field_length[1] = 6 self.residual_table.field_length[2] = 4 self.residual_table.print_header([ 'ts', 't', 'iter', 'struc ratio', 'iter time', 'residual vel', 'FoR_vel(x)', 'FoR_vel(z)' ]) def cleanup_timestep_info(self): if max(len(self.data.aero.timestep_info), len(self.data.structure.timestep_info)) > 1: # copy last info to first self.data.aero.timestep_info[0] = self.data.aero.timestep_info[-1] self.data.structure.timestep_info[ 0] = self.data.structure.timestep_info[-1] # delete all the rest while len(self.data.aero.timestep_info) - 1: del self.data.aero.timestep_info[-1] while len(self.data.structure.timestep_info) - 1: del self.data.structure.timestep_info[-1] self.data.ts = 0 def process_controller_output(self, controlled_state): """ This function modified the solver properties and parameters as requested from the controller. This keeps the main loop much cleaner, while allowing for flexibility Please, if you add options in here, always code the possibility of that specific option not being there without the code complaining to the user. If it possible, use the same Key for the new setting as for the setting in the solver. For example, if you want to modify the `structural_substeps` variable in settings, use that Key in the `info` dictionary. As a convention: a value of None returns the value to the initial one specified in settings, while the key not being in the dict is ignored, so if any change was made before, it will stay there. """ try: info = controlled_state['info'] except KeyError: return controlled_state['structural'], controlled_state['aero'] # general copy-if-exists, restore if == None for info_k, info_v in info.items(): if info_k in self.settings: if info_v is not None: self.settings[info_k] = info_v else: self.settings[info_k] = self.original_settings[info_k] # specifics of every option for info_k, info_v in info.items(): if info_k in self.settings: if info_k == 'structural_substeps': if info_v is not None: self.substep_dt = ( self.settings['dt'].value / (self.settings['structural_substeps'].value + 1)) if info_k == 'structural_solver': if info_v is not None: self.structural_solver = solver_interface.initialise_solver( info['structural_solver']) self.structural_solver.initialise( self.data, self.settings['structural_solver_settings']) return controlled_state['structural'], controlled_state['aero'] def run(self): """ Run the time stepping procedure with controllers and postprocessors included. """ # dynamic simulations start at tstep == 1, 0 is reserved for the initial state for self.data.ts in range( len(self.data.structure.timestep_info), self.settings['n_time_steps'].value + len(self.data.structure.timestep_info)): initial_time = time.perf_counter() structural_kstep = self.data.structure.timestep_info[-1].copy() aero_kstep = self.data.aero.timestep_info[-1].copy() # Add the controller here if self.with_controllers: state = {'structural': structural_kstep, 'aero': aero_kstep} for k, v in self.controllers.items(): state = v.control(self.data, state) # this takes care of the changes in options for the solver structural_kstep, aero_kstep = self.process_controller_output( state) self.time_aero = 0.0 self.time_struc = 0.0 # Copy the controlled states so that the interpolation does not # destroy the previous information controlled_structural_kstep = structural_kstep.copy() controlled_aero_kstep = aero_kstep.copy() k = 0 for k in range(self.settings['fsi_substeps'].value + 1): if (k == self.settings['fsi_substeps'].value and self.settings['fsi_substeps']): cout.cout_wrap('The FSI solver did not converge!!!') break # generate new grid (already rotated) aero_kstep = controlled_aero_kstep.copy() self.aero_solver.update_custom_grid(structural_kstep, aero_kstep) # compute unsteady contribution force_coeff = 0.0 unsteady_contribution = False if self.settings['include_unsteady_force_contribution'].value: if self.data.ts > self.settings[ 'steps_without_unsteady_force'].value: unsteady_contribution = True if k < self.settings[ 'pseudosteps_ramp_unsteady_force'].value: force_coeff = k / self.settings[ 'pseudosteps_ramp_unsteady_force'].value else: force_coeff = 1. # run the solver ini_time_aero = time.perf_counter() self.data = self.aero_solver.run( aero_kstep, structural_kstep, convect_wake=True, unsteady_contribution=unsteady_contribution) self.time_aero += time.perf_counter() - ini_time_aero previous_kstep = structural_kstep.copy() structural_kstep = controlled_structural_kstep.copy() # move the aerodynamic surface according the the structural one self.aero_solver.update_custom_grid(structural_kstep, aero_kstep) self.map_forces(aero_kstep, structural_kstep, force_coeff) # relaxation relax_factor = self.relaxation_factor(k) relax(self.data.structure, structural_kstep, previous_kstep, relax_factor) # check if nan anywhere. # if yes, raise exception if np.isnan(structural_kstep.steady_applied_forces).any(): raise exc.NotConvergedSolver( 'NaN found in steady_applied_forces!') if np.isnan(structural_kstep.unsteady_applied_forces).any(): raise exc.NotConvergedSolver( 'NaN found in unsteady_applied_forces!') copy_structural_kstep = structural_kstep.copy() ini_time_struc = time.perf_counter() for i_substep in range( self.settings['structural_substeps'].value + 1): # run structural solver coeff = ((i_substep + 1) / (self.settings['structural_substeps'].value + 1)) structural_kstep = self.interpolate_timesteps( step0=self.data.structure.timestep_info[-1], step1=copy_structural_kstep, out_step=structural_kstep, coeff=coeff) self.data = self.structural_solver.run( structural_step=structural_kstep, dt=self.substep_dt) self.time_struc += time.perf_counter() - ini_time_struc # check convergence if self.convergence(k, structural_kstep, previous_kstep): # move the aerodynamic surface according to the structural one self.aero_solver.update_custom_grid( structural_kstep, aero_kstep) break # move the aerodynamic surface according the the structural one self.aero_solver.update_custom_grid(structural_kstep, aero_kstep) self.aero_solver.add_step() self.data.aero.timestep_info[-1] = aero_kstep.copy() self.structural_solver.add_step() self.data.structure.timestep_info[-1] = structural_kstep.copy() final_time = time.perf_counter() if self.print_info: self.residual_table.print_line([ self.data.ts, self.data.ts * self.dt.value, k, self.time_struc / (self.time_aero + self.time_struc), final_time - initial_time, np.log10(self.res_dqdt), structural_kstep.for_vel[0], structural_kstep.for_vel[2], np.sum(structural_kstep.steady_applied_forces[:, 0]), np.sum(structural_kstep.steady_applied_forces[:, 2]) ]) self.structural_solver.extract_resultants() # run postprocessors if self.with_postprocessors: for postproc in self.postprocessors: self.data = self.postprocessors[postproc].run(online=True) if self.print_info: cout.cout_wrap('...Finished', 1) return self.data def convergence(self, k, tstep, previous_tstep): r""" Check convergence in the FSI loop. Convergence is determined as: .. math:: \epsilon_q^k = \frac{|| q^k - q^{k - 1} ||}{q^0} .. math:: \epsilon_\dot{q}^k = \frac{|| \dot{q}^k - \dot{q}^{k - 1} ||}{\dot{q}^0} FSI converged if :math:`\epsilon_q^k < \mathrm{FSI\ tolerance}` and :math:`\epsilon_\dot{q}^k < \mathrm{FSI\ tolerance}` """ # check for non-convergence if not all(np.isfinite(tstep.q)): import pdb pdb.set_trace() raise Exception( '***Not converged! There is a NaN value in the forces!') if not k: # save the value of the vectors for normalising later self.base_q = np.linalg.norm(tstep.q.copy()) self.base_dqdt = np.linalg.norm(tstep.dqdt.copy()) if self.base_dqdt == 0: self.base_dqdt = 1. return False # relative residuals self.res = (np.linalg.norm(tstep.q - previous_tstep.q) / self.base_q) self.res_dqdt = (np.linalg.norm(tstep.dqdt - previous_tstep.dqdt) / self.base_dqdt) # we don't want this to converge before introducing the gamma_dot forces! if self.settings['include_unsteady_force_contribution'].value: if k < self.settings['pseudosteps_ramp_unsteady_force'].value: return False # convergence if k > self.settings['minimum_steps'].value - 1: if self.res < self.settings['fsi_tolerance'].value: if self.res_dqdt < self.settings['fsi_tolerance'].value: return True return False def map_forces(self, aero_kstep, structural_kstep, unsteady_forces_coeff=1.0): # set all forces to 0 structural_kstep.steady_applied_forces.fill(0.0) structural_kstep.unsteady_applied_forces.fill(0.0) # aero forces to structural forces struct_forces = mapping.aero2struct_force_mapping( aero_kstep.forces, self.data.aero.struct2aero_mapping, aero_kstep.zeta, structural_kstep.pos, structural_kstep.psi, self.data.structure.node_master_elem, self.data.structure.connectivities, structural_kstep.cag(), self.data.aero.aero_dict) dynamic_struct_forces = unsteady_forces_coeff * mapping.aero2struct_force_mapping( aero_kstep.dynamic_forces, self.data.aero.struct2aero_mapping, aero_kstep.zeta, structural_kstep.pos, structural_kstep.psi, self.data.structure.node_master_elem, self.data.structure.connectivities, structural_kstep.cag(), self.data.aero.aero_dict) # prescribed forces + aero forces try: structural_kstep.steady_applied_forces = ( (struct_forces + self.data.structure.ini_info.steady_applied_forces).astype( dtype=ct.c_double, order='F', copy=True)) structural_kstep.unsteady_applied_forces = (( dynamic_struct_forces + self.data.structure.dynamic_input[max( self.data.ts - 1, 0)]['dynamic_forces']).astype( dtype=ct.c_double, order='F', copy=True)) except KeyError: structural_kstep.steady_applied_forces = ( (struct_forces + self.data.structure.ini_info.steady_applied_forces).astype( dtype=ct.c_double, order='F', copy=True)) structural_kstep.unsteady_applied_forces = dynamic_struct_forces def relaxation_factor(self, k): initial = self.settings['relaxation_factor'].value if not self.settings['dynamic_relaxation'].value: return initial final = self.settings['final_relaxation_factor'].value if k >= self.settings['relaxation_steps'].value: return final value = initial + ( final - initial) / self.settings['relaxation_steps'].value * k return value @staticmethod def interpolate_timesteps(step0, step1, out_step, coeff): """ Performs a linear interpolation between step0 and step1 based on coeff in [0, 1]. 0 means info in out_step == step0 and 1 out_step == step1. Quantities interpolated: * `steady_applied_forces` * `unsteady_applied_forces` * `velocity` input in Lagrange constraints """ if not 0.0 <= coeff <= 1.0: return out_step # forces out_step.steady_applied_forces[:] = ( (1.0 - coeff) * step0.steady_applied_forces + (coeff) * (step1.steady_applied_forces)) out_step.unsteady_applied_forces[:] = ( (1.0 - coeff) * step0.unsteady_applied_forces + (coeff) * (step1.unsteady_applied_forces)) # multibody if necessary if out_step.mb_dict is not None: for key in step1.mb_dict.keys(): if 'constraint_' in key: try: out_step.mb_dict[key]['velocity'][:] = ( (1.0 - coeff) * step0.mb_dict[key]['velocity'] + (coeff) * step1.mb_dict[key]['velocity']) except KeyError: pass return out_step
class NonLinearStaticMultibody(_BaseStructural): """ Nonlinear static multibody Nonlinear static solver for multibody structures. """ solver_id = 'NonLinearStaticMultibody' solver_classification = 'structural' settings_types = _BaseStructural.settings_types.copy() settings_default = _BaseStructural.settings_default.copy() settings_description = _BaseStructural.settings_description.copy() settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.data = None self.settings = None # Total number of unknowns in the Multybody sistem self.sys_size = None # Total number of equations associated to the Lagrange multipliers self.lc_list = None self.num_LM_eq = None # self.gamma = None # self.beta = None def initialise(self, data, custom_settings=None): self.data = data if custom_settings is None: self.settings = data.settings[self.solver_id] else: self.settings = custom_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) # load info from dyn dictionary # self.data.structure.add_unsteady_information( # self.data.structure.dyn_dict, self.settings['num_steps'].value) # # # Define Newmark constants # self.gamma = 0.5 + self.settings['newmark_damp'].value # self.beta = 0.25*(self.gamma + 0.5)*(self.gamma + 0.5) # Define the number of equations self.lc_list = lagrangeconstraints.initialize_constraints( self.data.structure.ini_mb_dict) self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list) # Define the number of dofs self.define_sys_size() def add_step(self): self.data.structure.next_step() def next_step(self): pass def define_sys_size(self): MBdict = self.data.structure.ini_mb_dict self.sys_size = self.data.structure.num_dof.value for ibody in range(self.data.structure.num_bodies): if (MBdict['body_%02d' % ibody]['FoR_movement'] == 'free'): self.sys_size += 10 def assembly_MB_eq_system(self, MB_beam, MB_tstep, Lambda, MBdict, iLoadStep): self.lc_list = lagrangeconstraints.initialize_constraints(MBdict) self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list) # MB_M = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F') # MB_C = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F') MB_K = np.zeros( (self.sys_size + self.num_LM_eq, self.sys_size + self.num_LM_eq), dtype=ct.c_double, order='F') # MB_Asys = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F') MB_Q = np.zeros((self.sys_size + self.num_LM_eq, ), dtype=ct.c_double, order='F') #ipdb.set_trace() first_dof = 0 last_dof = 0 # Loop through the different bodies for ibody in range(len(MB_beam)): # Initialize matrices # M = None # C = None K = None Q = None # Generate the matrices for each body if MB_beam[ibody].FoR_movement == 'prescribed': last_dof = first_dof + MB_beam[ibody].num_dof.value elif MB_beam[ibody].FoR_movement == 'free': last_dof = first_dof + MB_beam[ibody].num_dof.value + 10 K, Q = xbeamlib.cbeam3_asbly_static(MB_beam[ibody], MB_tstep[ibody], self.settings, iLoadStep) ############### Assembly into the global matrices # Flexible and RBM contribution to Asys # MB_M[first_dof:last_dof, first_dof:last_dof] = M.astype(dtype=ct.c_double, copy=True, order='F') # MB_C[first_dof:last_dof, first_dof:last_dof] = C.astype(dtype=ct.c_double, copy=True, order='F') MB_K[first_dof:last_dof, first_dof:last_dof] = K.astype(dtype=ct.c_double, copy=True, order='F') #Q MB_Q[first_dof:last_dof] = Q first_dof = last_dof # Define the number of equations # Generate matrices associated to Lagrange multipliers LM_C, LM_K, LM_Q = lagrangeconstraints.generate_lagrange_matrix( self.lc_list, MB_beam, MB_tstep, 0, self.num_LM_eq, self.sys_size, 0., Lambda, np.zeros_like(Lambda), "static") # Include the matrices associated to Lagrange Multipliers # MB_C += LM_C MB_K += LM_K MB_Q += LM_Q # MB_Asys = MB_K + MB_C*self.gamma/(self.beta*dt) + MB_M/(self.beta*dt*dt) return MB_K, MB_Q def integrate_position(self, MB_beam, MB_tstep, dt): pass # vel = np.zeros((6,),) # acc = np.zeros((6,),) # for ibody in range(0, len(MB_tstep)): # # I think this is the right way to do it, but to make it match the rest I change it temporally # if True: # # MB_tstep[ibody].mb_quat[ibody,:] = algebra.quaternion_product(MB_tstep[ibody].quat, MB_tstep[ibody].mb_quat[ibody,:]) # acc[0:3] = (0.5-self.beta)*np.dot(MB_beam[ibody].timestep_info.cga(),MB_beam[ibody].timestep_info.for_acc[0:3])+self.beta*np.dot(MB_tstep[ibody].cga(),MB_tstep[ibody].for_acc[0:3]) # vel[0:3] = np.dot(MB_beam[ibody].timestep_info.cga(),MB_beam[ibody].timestep_info.for_vel[0:3]) # MB_tstep[ibody].for_pos[0:3] += dt*(vel[0:3] + dt*acc[0:3]) # else: # MB_tstep[ibody].for_pos[0:3] += dt*np.dot(MB_tstep[ibody].cga(),MB_tstep[ibody].for_vel[0:3]) # # # Use next line for double pendulum (fix position of the second FoR) # # MB_tstep[ibody].for_pos[0:3] = np.dot(algebra.quat2rotation(MB_tstep[0].quat), MB_tstep[0].pos[-1,:]) # # print("tip final pos: ", np.dot(algebra.quat2rotation(MB_tstep[0].quat), MB_tstep[0].pos[-1,:])) # # print("FoR final pos: ", MB_tstep[ibody].for_pos[0:3]) # # print("pause") def extract_resultants(self, tstep): # TODO: code return np.zeros((3, )), np.zeros((3, )) def update(self, tstep=None): self.create_q_vector(tstep) def create_q_vector(self, tstep=None): import sharpy.structure.utils.xbeamlib as xb if tstep is None: tstep = self.data.structure.timestep_info[-1] xb.xbeam_solv_disp2state(self.data.structure, tstep) def compute_forces_constraints(self, MB_beam, MB_tstep, Lambda): try: self.lc_list[0] except IndexError: return # TODO the output of this routine is wrong. check at some point. LM_C, LM_K, LM_Q = lagrangeconstraints.generate_lagrange_matrix( self.lc_list, MB_beam, MB_tstep, 0, self.num_LM_eq, self.sys_size, 0., Lambda, np.zeros_like(Lambda), "static") F = -np.dot(LM_K[:, -self.num_LM_eq:], Lambda) first_dof = 0 for ibody in range(len(MB_beam)): # Forces associated to nodes body_numdof = MB_beam[ibody].num_dof.value body_freenodes = np.sum(MB_beam[ibody].vdof > -1) last_dof = first_dof + body_numdof MB_tstep[ibody].forces_constraints_nodes[( MB_beam[ibody].vdof > -1), :] = F[first_dof:last_dof].reshape( body_freenodes, 6, order='C') # Forces associated to the frame of reference if MB_beam[ibody].FoR_movement == 'free': # TODO: How are the forces in the quaternion equation interpreted? MB_tstep[ibody].forces_constraints_FoR[ ibody, :] = F[last_dof:last_dof + 10] last_dof += 10 first_dof = last_dof # print(MB_tstep[ibody].forces_constraints_nodes) # TODO: right now, these forces are only used as an output, they are not read when the multibody is splitted def run(self, structural_step=None, dt=None): if structural_step is None: structural_step = self.data.structure.timestep_info[-1] if structural_step.mb_dict is not None: MBdict = structural_step.mb_dict else: MBdict = self.data.structure.ini_mb_dict if dt is None: dt = ct.c_float(0.) self.lc_list = lagrangeconstraints.initialize_constraints(MBdict) self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list) # TODO: only working for constant forces MB_beam, MB_tstep = mb.split_multibody(self.data.structure, structural_step, MBdict, 0) q = np.zeros((self.sys_size + self.num_LM_eq, ), dtype=ct.c_double, order='F') dqdt = np.zeros((self.sys_size + self.num_LM_eq, ), dtype=ct.c_double, order='F') dqddt = np.zeros((self.sys_size + self.num_LM_eq, ), dtype=ct.c_double, order='F') mb.disp2state(MB_beam, MB_tstep, q, dqdt, dqddt) # Lagrange multipliers parameters num_LM_eq = self.num_LM_eq Lambda = np.zeros((num_LM_eq, ), dtype=ct.c_double, order='F') # Lambda_dot = np.zeros((num_LM_eq,), dtype=ct.c_double, order='F') Dq_old = 0. Dq = np.zeros((self.sys_size, )) for iLoadStep in range(0, self.settings['num_load_steps'].value + 1): iter = -1 # delta = settings.min_delta + 1. converged = False while converged == False: iter += 1 if (iter == self.settings['max_iterations'].value - 1): cout.cout_wrap(("Residual is: %f" % np.amax(np.abs(Dq))), 4) cout.cout_wrap("Static equations did not converge", 4) break MB_K, MB_Q = self.assembly_MB_eq_system( MB_beam, MB_tstep, Lambda, MBdict, iLoadStep) Dq = np.linalg.solve(MB_K, -MB_Q) # Dq *= 0.7 q += Dq mb.state2disp(q, dqdt, dqddt, MB_beam, MB_tstep) if (iter > 0): if (np.amax(np.abs(Dq)) < Dq_old): converged = True if iter == 0: Dq_old = np.amax(np.array([1., np.amax( np.abs(Dq))])) * self.settings['min_delta'].value lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep, "static") self.compute_forces_constraints(MB_beam, MB_tstep, Lambda) if self.settings['gravity_on']: for ibody in range(len(MB_beam)): xbeamlib.cbeam3_correct_gravity_forces(MB_beam[ibody], MB_tstep[ibody], self.settings) mb.merge_multibody(MB_tstep, MB_beam, self.data.structure, structural_step, MBdict, 0.) # # Initialize # q = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F') # dqdt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F') # dqddt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F') # # # Predictor step # mb.disp2state(MB_beam, MB_tstep, q, dqdt, dqddt) # # q += dt*dqdt + (0.5 - self.beta)*dt*dt*dqddt # dqdt += (1.0 - self.gamma)*dt*dqddt # dqddt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F') # if not num_LM_eq == 0: # Lambda = q[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F') # Lambda_dot = dqdt[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F') # else: # Lambda = 0 # Lambda_dot = 0 # # # Newmark-beta iterations # old_Dq = 1.0 # LM_old_Dq = 1.0 # # converged = False # for iter in range(self.settings['max_iterations'].value): # # Check if the maximum of iterations has been reached # if (iter == self.settings['max_iterations'].value - 1): # print('Solver did not converge in ', iter, ' iterations.') # print('res = ', res) # print('LM_res = ', LM_res) # import pdb; pdb.set_trace() # break # # # Update positions and velocities # mb.state2disp(q, dqdt, dqddt, MB_beam, MB_tstep) # MB_Asys, MB_Q = self.assembly_MB_eq_system(MB_beam, MB_tstep, self.data.ts, dt, Lambda, Lambda_dot, MBdict) # # # Compute the correction # # ADC next line not necessary # # Dq = np.zeros((self.sys_size+num_LM_eq,), dtype=ct.c_double, order='F') # # MB_Asys_balanced, T = scipy.linalg.matrix_balance(MB_Asys) # # invT = np.matrix(T).I # # MB_Q_balanced = np.dot(invT, MB_Q).T # # Dq = np.linalg.solve(MB_Asys, -MB_Q) # # least squares solver # # Dq = np.linalg.lstsq(np.dot(MB_Asys_balanced, invT), -MB_Q_balanced, rcond=None)[0] # # # Evaluate convergence # if (iter > 0): # res = np.max(np.abs(Dq[0:self.sys_size]))/old_Dq # if not num_LM_eq == 0: # LM_res = np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq]))/LM_old_Dq # else: # LM_res = 0.0 # if (res < self.settings['min_delta'].value) and (LM_res < self.settings['min_delta'].value*1e-2): # converged = True # # # Compute variables from previous values and increments # # TODO:decide If I want other way of updating lambda # # this for least sq # # q[:, np.newaxis] += Dq # # dqdt[:, np.newaxis] += self.gamma/(self.beta*dt)*Dq # # dqddt[:, np.newaxis] += 1.0/(self.beta*dt*dt)*Dq # # # this for direct solver # q += Dq # dqdt += self.gamma/(self.beta*dt)*Dq # dqddt += 1.0/(self.beta*dt*dt)*Dq # # if not num_LM_eq == 0: # Lambda = q[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F') # Lambda_dot = dqdt[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F') # else: # Lambda = 0 # Lambda_dot = 0 # # if converged: # break # # if iter == 0: # old_Dq = np.max(np.abs(Dq[0:self.sys_size])) # if old_Dq < 1.0: # old_Dq = 1.0 # if num_LM_eq: # LM_old_Dq = np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq])) # else: # LM_old_Dq = 1.0 # # mb.state2disp(q, dqdt, dqddt, MB_beam, MB_tstep) # # end: comment time stepping # # # End of Newmark-beta iterations # self.integrate_position(MB_beam, MB_tstep, dt) # # lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep, MBdict, "dynamic") # lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep, "dynamic") # self.compute_forces_constraints(MB_beam, MB_tstep, self.data.ts, dt, Lambda, Lambda_dot) # if self.settings['gravity_on']: # for ibody in range(len(MB_beam)): # xbeamlib.cbeam3_correct_gravity_forces(MB_beam[ibody], MB_tstep[ibody], self.settings) # mb.merge_multibody(MB_tstep, MB_beam, self.data.structure, structural_step, MBdict, dt) # structural_step.q[:] = q[:self.sys_size].copy() # structural_step.dqdt[:] = dqdt[:self.sys_size].copy() # structural_step.dqddt[:] = dqddt[:self.sys_size].copy() return self.data
class LinearBeam(BaseElement): r""" State space member Define class for linear state-space realisation of GEBM flexible-body equations from SHARPy``timestep_info`` class and with the nonlinear structural information. State-space models can be defined in continuous or discrete time (dt required). Modal projection, either on the damped or undamped modal shapes, is also avaiable. To produce the state-space equations: Notes on the settings: a. ``modal_projection={True,False}``: determines whether to project the states onto modal coordinates. Projection over damped or undamped modal shapes can be obtained selecting: - ``proj_modes={'damped','undamped'}`` while - ``inout_coords={'modes','nodal'}`` determines whether the modal state-space inputs/outputs are modal coords or nodal degrees-of-freedom. If ``modes`` is selected, the ``Kin`` and ``Kout`` gain matrices are generated to transform nodal to modal dofs b. ``dlti={True,False}``: if true, generates discrete-time system. The continuous to discrete transformation method is determined by:: discr_method={ 'newmark', # Newmark-beta 'zoh', # Zero-order hold 'bilinear'} # Bilinear (Tustin) transformation DLTIs can be obtained directly using the Newmark-:math:`\beta` method ``discr_method='newmark'`` ``newmark_damp=xx`` with ``xx<<1.0`` for full-states descriptions (``modal_projection=False``) and modal projection over the undamped structural modes (``modal_projection=True`` and ``proj_modes``). The Zero-order holder and bilinear methods, instead, work in all descriptions, but require the continuous state-space equations. """ sys_id = "LinearBeam" settings_types = dict() settings_default = dict() settings_description = dict() settings_default['modal_projection'] = True settings_types['modal_projection'] = 'bool' settings_description['modal_projection'] = 'Use modal projection' settings_default['inout_coords'] = 'nodes' settings_types['inout_coords'] = 'str' settings_description[ 'inout_coords'] = 'Beam state space input/output coordinates. ``modes`` or ``nodes``' settings_types['num_modes'] = 'int' settings_default['num_modes'] = 10 settings_description['num_modes'] = 'Number of modes to retain' settings_default['discrete_time'] = True settings_types['discrete_time'] = 'bool' settings_description['discrete_time'] = 'Assemble beam in discrete time' settings_default['dt'] = 0.001 settings_types['dt'] = 'float' settings_description['dt'] = 'Discrete time system integration time step' settings_default['proj_modes'] = 'undamped' settings_types['proj_modes'] = 'str' settings_description['proj_modes'] = 'Use ``undamped`` or ``damped`` modes' settings_default['discr_method'] = 'newmark' settings_types['discr_method'] = 'str' settings_description[ 'discr_method'] = 'Discrete time assembly system method: ``newmark`` or ``zoh``' settings_default['newmark_damp'] = 1e-4 settings_types['newmark_damp'] = 'float' settings_description[ 'newmark_damp'] = 'Newmark damping value. For systems assembled using ``newmark``' settings_default['use_euler'] = False settings_types['use_euler'] = 'bool' settings_description[ 'use_euler'] = 'Use euler angles for rigid body parametrisation' settings_default['print_info'] = True settings_types['print_info'] = 'bool' settings_description['print_info'] = 'Display information on screen' settings_default['gravity'] = False settings_types['gravity'] = 'bool' settings_description['gravity'] = 'Linearise gravitational forces' settings_types['remove_dofs'] = 'list(str)' settings_default['remove_dofs'] = [] settings_description[ 'remove_dofs'] = 'Remove desired degrees of freedom: ``eta``, ``V``, ``W`` or ``orient``' settings_types['remove_sym_modes'] = 'bool' settings_default['remove_sym_modes'] = False settings_description[ 'remove_sym_modes'] = 'Remove symmetric modes if wing is clamped' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.sys = None # The actual object self.ss = None # The state space object self.clamped = None self.tsstruct0 = None self.settings = dict() self.state_variables = None self.linearisation_vectors = dict() def initialise(self, data, custom_settings=None): if custom_settings: self.settings = custom_settings else: try: self.settings = data.settings['LinearAssembler'][ 'linear_system_settings'] except KeyError: pass settings.to_custom_types(self.settings, self.settings_types, self.settings_default, no_ctype=True) beam = lingebm.FlexDynamic(data.linear.tsstruct0, data.structure, self.settings) self.sys = beam self.tsstruct0 = data.linear.tsstruct0 # State variables num_dof_flex = self.sys.structure.num_dof.value num_dof_rig = self.sys.Mstr.shape[0] - num_dof_flex state_db = { 'eta': [0, num_dof_flex], 'V_bar': [num_dof_flex, num_dof_flex + 3], 'W_bar': [num_dof_flex + 3, num_dof_flex + 6], 'orient_bar': [num_dof_flex + 6, num_dof_flex + num_dof_rig], 'dot_eta': [num_dof_flex + num_dof_rig, 2 * num_dof_flex + num_dof_rig], 'V': [ 2 * num_dof_flex + num_dof_rig, 2 * num_dof_flex + num_dof_rig + 3 ], 'W': [ 2 * num_dof_flex + num_dof_rig + 3, 2 * num_dof_flex + num_dof_rig + 6 ], 'orient': [ 2 * num_dof_flex + num_dof_rig + 6, 2 * num_dof_flex + 2 * num_dof_rig ] } self.state_variables = LinearVector(state_db, self.sys_id) if num_dof_rig == 0: self.clamped = True self.linearisation_vectors['eta'] = self.tsstruct0.q self.linearisation_vectors['eta_dot'] = self.tsstruct0.dqdt self.linearisation_vectors[ 'forces_struct'] = self.tsstruct0.steady_applied_forces.reshape( -1, order='C') def assemble(self, t_ref=None): """ Assemble the beam state-space system. Args: t_ref (float): Scaling factor to non-dimensionalise the beam's time step. Returns: """ if self.settings['gravity']: self.sys.linearise_gravity_forces() if self.settings['remove_dofs']: self.trim_nodes(self.settings['remove_dofs']) if self.settings['modal_projection'] and self.settings[ 'remove_sym_modes'] and self.clamped: self.remove_symmetric_modes() if t_ref is not None: self.sys.scale_system_normalised_time(t_ref) # import sharpy.linear.assembler.linearthrust as linearthrust # engine = linearthrust.LinearThrust() # engine.initialise() # K_thrust = engine.generate(self.tsstruct0, self.sys) # # self.sys.Kstr += K_thrust self.sys.assemble() # TODO: remove integrals of the rigid body modes (and change mode shapes to account for this in the coupling matrices) # Option to remove certain dofs via dict: i.e. dofs to remove # Map dofs to equations # Boundary conditions # Same with modal, remove certain modes. Need to specify that modes to keep refer to flexible ones only if self.sys.SSdisc: self.ss = self.sys.SSdisc elif self.sys.SScont: self.ss = self.sys.SScont return self.ss def x0(self): x = np.concatenate((self.tsstruct0.q, self.tsstruct0.dqdt)) return x def trim_nodes(self, trim_list=list): num_dof_flex = self.sys.structure.num_dof.value num_dof_rig = self.sys.Mstr.shape[0] - num_dof_flex # Dictionary containing DOFs and corresponding equations dof_db = { 'eta': [0, num_dof_flex, 1], 'V': [num_dof_flex, num_dof_flex + 3, 2], 'W': [num_dof_flex + 3, num_dof_flex + 6, 3], 'orient': [num_dof_flex + 6, num_dof_flex + num_dof_rig, 4], 'yaw': [num_dof_flex + 8, num_dof_flex + num_dof_rig, 1] } # ----------------------------------------------------------------------- # Better to place in a function available to all elements since it will equally apply # Therefore, the dof_db should be a class attribute # Take away alongside the vector variable class # All variables vec_db = dict() for item in dof_db: vector_var = VectorVariable(item, dof_db[item], 'LinearBeam') vec_db[item] = vector_var used_vars_db = vec_db.copy() # Variables to remove removed_dofs = 0 removed_db = dict() for item in trim_list: removed_db[item] = vec_db[item] removed_dofs += vec_db[item].size del used_vars_db[item] # Update variables position for rem_item in removed_db: for item in used_vars_db: if used_vars_db[item].rows_loc[0] < removed_db[ rem_item].first_pos: continue else: # Update order and position used_vars_db[item].first_pos -= removed_db[rem_item].size used_vars_db[item].end_pos -= removed_db[rem_item].size self.state_variables = used_vars_db # TODO: input and output variables ### ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # Map dofs to equations trim_matrix = np.zeros((num_dof_rig + num_dof_flex, num_dof_flex + num_dof_rig - removed_dofs)) for item in used_vars_db: trim_matrix[used_vars_db[item].rows_loc, used_vars_db[item].cols_loc] = 1 # Update matrices self.sys.Mstr = trim_matrix.T.dot(self.sys.Mstr.dot(trim_matrix)) self.sys.Cstr = trim_matrix.T.dot(self.sys.Cstr.dot(trim_matrix)) self.sys.Kstr = trim_matrix.T.dot(self.sys.Kstr.dot(trim_matrix)) def remove_symmetric_modes(self): """ Removes symmetric modes when the wing is clamped at the midpoint. It will force the wing tip displacements in ``z`` to be postive for all modes. Updates the mode shapes matrix, the natural frequencies and the number of modes. """ # Group modes into symmetric and anti-symmetric modes modes_sym = np.zeros_like(self.sys.U) # grouped modes total_modes = self.sys.num_modes for i in range(total_modes // 2): je = 2 * i jo = 2 * i + 1 modes_sym[:, je] = 1. / np.sqrt(2) * (self.sys.U[:, je] + self.sys.U[:, jo]) modes_sym[:, jo] = 1. / np.sqrt(2) * (self.sys.U[:, je] - self.sys.U[:, jo]) self.sys.U = modes_sym # Remove anti-symmetric modes # Wing 1 and 2 nodes # z-displacement index ind_w1 = [6 * i + 2 for i in range(self.sys.structure.num_node // 2) ] # Wing 1 nodes are in the first half rows ind_w1_x = [6 * i for i in range(self.sys.structure.num_node // 2) ] # Wing 1 nodes are in the first half rows ind_w1_y = [ 6 * i + 1 for i in range(self.sys.structure.num_node // 2) ] # Wing 1 nodes are in the first half rows ind_w2 = [ 6 * i + 2 for i in range(self.sys.structure.num_node // 2, self.sys.structure.num_node - 1) ] # Wing 2 nodes are in the second half rows sym_mode_index = [] for i in range(self.sys.num_modes // 2): found_symmetric = False for j in range(2): ind = 2 * i + j # Maximum z displacement for wings 1 and 2 ind_max_w1 = np.argmax(np.abs(modes_sym[ind_w1, ind])) ind_max_w2 = np.argmax(np.abs(modes_sym[ind_w2, ind])) z_max_w1 = modes_sym[ind_w1, ind][ind_max_w1] z_max_w2 = modes_sym[ind_w2, ind][ind_max_w2] z_max_diff = np.abs(z_max_w1 - z_max_w2) if z_max_diff < np.abs(z_max_w1 + z_max_w2): sym_mode_index.append(ind) if found_symmetric: raise NameError('Symmetric Mode previously found') found_symmetric = True self.sys.U = modes_sym[:, sym_mode_index] # make all elastic modes have a positive z component at the wingtip for i in range(self.sys.U.shape[1]): if np.abs(self.sys.U[ind_w1[-1], i]) > 1e-10: self.sys.U[:, i] = np.sign(self.sys.U[ind_w1[-1], i]) * self.sys.U[:, i] elif np.abs(self.sys.U[ind_w1_y, i][-1]) > 1e-4: self.sys.U[:, i] = np.sign(self.sys.U[ind_w1_y[-1], i]) * self.sys.U[:, i] self.sys.freq_natural = self.sys.freq_natural[sym_mode_index] self.sys.num_modes = len(self.sys.freq_natural) def unpack_ss_vector(self, x_n, u_n, struct_tstep): """ Warnings: Under development. Missing: * Accelerations * Double check the cartesian rotation vector * Tangential operator for the moments Takes the state :math:`x = [\eta, \dot{\eta}]` and input vectors :math:`u = N` of a linearised beam and returns a SHARPy timestep instance, including the reference values. Args: x_n (np.ndarray): Structural beam state vector in nodal space y_n (np.ndarray): Beam input vector (nodal forces) struct_tstep (utils.datastructures.StructTimeStepInfo): Reference timestep used for linearisation Returns: utils.datastructures.StructTimeStepInfo: new timestep with linearised values added to the reference value """ # check if clamped vdof = self.sys.structure.vdof num_node = struct_tstep.num_node num_dof = 6 * sum(vdof >= 0) if self.sys.clamped: clamped = True rig_dof = 0 else: clamped = False if self.settings['use_euler']: rig_dof = 9 else: rig_dof = 10 q = np.zeros_like(struct_tstep.q) q = np.zeros((num_dof + rig_dof)) dqdt = np.zeros_like(q) dqddt = np.zeros_like(q) pos = np.zeros_like(struct_tstep.pos) pos_dot = np.zeros_like(struct_tstep.pos_dot) psi = np.zeros_like(struct_tstep.psi) psi_dot = np.zeros_like(struct_tstep.psi_dot) for_pos = np.zeros_like(struct_tstep.for_pos) for_vel = np.zeros_like(struct_tstep.for_vel) for_acc = np.zeros_like(struct_tstep.for_acc) quat = np.zeros_like(struct_tstep.quat) gravity_forces = np.zeros_like(struct_tstep.gravity_forces) total_gravity_forces = np.zeros_like(struct_tstep.total_gravity_forces) steady_applied_forces = np.zeros_like( struct_tstep.steady_applied_forces) unsteady_applied_forces = np.zeros_like( struct_tstep.unsteady_applied_forces) q[:num_dof + rig_dof] = x_n[:num_dof + rig_dof] dqdt[:num_dof + rig_dof] = x_n[num_dof + rig_dof:] # Missing the forces # dqddt = self.sys.Minv.dot(-self.sys.Cstr.dot(dqdt) - self.sys.Kstr.dot(q)) for i_node in vdof[vdof >= 0]: pos[i_node + 1, :] = q[6 * i_node:6 * i_node + 3] pos_dot[i_node + 1, :] = dqdt[6 * i_node + 0:6 * i_node + 3] # TODO: CRV of clamped node and double check that the CRV takes this form for i_elem in range(struct_tstep.num_elem): for i_node in range(struct_tstep.num_node_elem): psi[i_elem, i_node, :] = np.linalg.inv( algebra.crv2tan(struct_tstep.psi[i_elem, i_node]).T).dot( q[i_node + 3:i_node + 6]) psi_dot[i_elem, i_node, :] = dqdt[i_node + 3:i_node + 6] if not clamped: for_vel = dqdt[-rig_dof:-rig_dof + 6] if self.settings['use_euler']: euler = dqdt[-4:-1] quat = algebra.euler2quat(euler) else: quat = dqdt[-4:] for_pos = q[-rig_dof:-rig_dof + 6] for_acc = dqddt[-rig_dof:-rig_dof + 6] if u_n is not None: for i_node in vdof[vdof >= 0]: steady_applied_forces[i_node + 1] = u_n[6 * i_node:6 * i_node + 6] steady_applied_forces[0] = u_n[-10:-4] - np.sum( steady_applied_forces[1:, :], 0) # gravity forces - careful - debug C_grav = np.zeros((q.shape[0], q.shape[0])) K_grav = np.zeros_like(C_grav) try: Crr = self.sys.Crr_grav Csr = self.sys.Csr_grav C_grav[:-rig_dof, -rig_dof:] = Csr # TODO: sort out changing q vector with euler C_grav[-rig_dof:, -rig_dof:] = Crr K_grav[-rig_dof:, :-rig_dof] = self.sys.Krs_grav K_grav[:-rig_dof, :-rig_dof] = self.sys.Kss_grav fgrav = -C_grav.dot(dqdt) - K_grav.dot(q) for i in range(gravity_forces.shape[0] - 1): #add bc at node - doing it manually here gravity_forces[i + 1, :] = fgrav[6 * i:6 * (i + 1)] gravity_forces[0, :] = fgrav[-rig_dof:-rig_dof + 6] - np.sum( gravity_forces[1:], 0) except AttributeError: pass current_time_step = struct_tstep.copy() current_time_step.q[:len(q)] = q + struct_tstep.q[:len(q)] current_time_step.dqdt[:len(q)] = dqdt + struct_tstep.dqdt[:len(q)] current_time_step.dqddt[:len(q)] = dqddt + struct_tstep.dqddt[:len(q)] current_time_step.pos = pos + struct_tstep.pos current_time_step.pos_dot = pos + struct_tstep.pos_dot current_time_step.psi = psi + struct_tstep.psi current_time_step.psi_dot = psi_dot + struct_tstep.psi_dot current_time_step.for_vel = for_vel + struct_tstep.for_vel current_time_step.for_acc = for_acc + struct_tstep.for_acc current_time_step.for_pos = for_pos + struct_tstep.for_pos current_time_step.gravity_forces = gravity_forces + struct_tstep.gravity_forces current_time_step.total_gravity_forces = total_gravity_forces + struct_tstep.total_gravity_forces current_time_step.unsteady_applied_forces = unsteady_applied_forces + struct_tstep.unsteady_applied_forces new_quat = quat + struct_tstep.quat current_time_step.quat = new_quat / np.linalg.norm(new_quat) current_time_step.steady_applied_forces = steady_applied_forces + struct_tstep.steady_applied_forces return current_time_step def rigid_aero_forces(self): # Debug adding rigid forces from tornado derivatives_alpha = np.zeros((6, 5)) derivatives_alpha[0, :] = np.array([0.0511, 0, 0, 0.08758, 0]) # drag derivatives derivatives_alpha[1, :] = np.array([0, 0, -0.05569, 0, 0]) # Y derivatives derivatives_alpha[2, :] = np.array([5.53, 0, 0, 11.35, 0]) # lift derivatives derivatives_alpha[3, :] = np.array([0, 0, -0.609, 0, 0]) # roll derivatives derivatives_alpha[4, :] = np.array([-9.9988, 0, 0, -37.61, 0]) # pitch derivatives derivatives_alpha[5, :] = np.array([0, 0, -0.047, 0, 0]) # yaw derivatives Cx0 = -0.0324 Cz0 = 0.436 Cm0 = -0.78966 quat = self.tsstruct0.quat Cga = algebra.quat2rotation(quat)
class AerogridLoader(BaseSolver): """ ``AerogridLoader`` class, inherited from ``BaseSolver`` Generates aerodynamic grid based on the input data Args: data (PreSharpy): ``ProblemData`` class structure Attributes: settings (dict): Name-value pair of the settings employed by the aerodynamic solver settings_types (dict): Acceptable types for the values in ``settings`` settings_default (dict): Name-value pair of default values for the aerodynamic settings data (ProblemData): class structure aero_file_name (str): name of the ``.aero.h5`` HDF5 file aero: empty attribute aero_data_dict (dict): key-value pairs of aerodynamic data wake_shape_generator (class): Wake shape generator Notes: The ``control_surface_deflection`` setting allows the user to use a time specific control surface deflection, should the problem include them. This setting takes a list of strings, each for the required control surface generator. The ``control_surface_deflection_generator_settings`` setting is a list of dictionaries, one for each control surface. The dictionaries specify the settings for the generator ``DynamicControlSurface``. If the relevant control surface is simply static, an empty string should be parsed. See the documentation for ``DynamicControlSurface`` generators for accepted key-value pairs as settings. The initial wake shape is now defined in SHARPy (instead of UVLM) through a wake shape generator ``wake_shape_generator`` and the required inputs ``wake_shape_generator_input`` """ solver_id = 'AerogridLoader' solver_classification = 'loader' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['unsteady'] = 'bool' settings_default['unsteady'] = False settings_description['unsteady'] = 'Unsteady effects' settings_types['aligned_grid'] = 'bool' settings_default['aligned_grid'] = True settings_description['aligned_grid'] = 'Align grid' settings_types['freestream_dir'] = 'list(float)' settings_default['freestream_dir'] = [1.0, 0.0, 0.0] settings_description['freestream_dir'] = 'Free stream flow direction' settings_types['mstar'] = 'int' settings_default['mstar'] = 10 settings_description['mstar'] = 'Number of chordwise wake panels' settings_types['control_surface_deflection'] = 'list(str)' settings_default['control_surface_deflection'] = [] settings_description[ 'control_surface_deflection'] = 'List of control surface generators for each control surface' settings_types[ 'control_surface_deflection_generator_settings'] = 'list(dict)' settings_default['control_surface_deflection_generator_settings'] = list( dict()) settings_description['control_surface_deflection_generator_settings'] = 'List of dictionaries with the settings ' \ 'for each generator' settings_types['wake_shape_generator'] = 'str' settings_default['wake_shape_generator'] = 'StraightWake' settings_description[ 'wake_shape_generator'] = 'ID of the generator to define the initial wake shape' settings_types['wake_shape_generator_input'] = 'dict' settings_default['wake_shape_generator_input'] = dict() settings_description[ 'wake_shape_generator_input'] = 'Dictionary of inputs needed by the wake shape generator' settings_table = settings_utils.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.data = None self.settings = None self.aero_file_name = '' # storage of file contents self.aero_data_dict = dict() # aero storage self.aero = None def initialise(self, data): self.data = data self.settings = data.settings[self.solver_id] # init settings settings_utils.to_custom_types(self.settings, self.settings_types, self.settings_default) # read input file (aero) self.read_files() wake_shape_generator_type = gen_interface.generator_from_string( self.settings['wake_shape_generator']) self.wake_shape_generator = wake_shape_generator_type() self.wake_shape_generator.initialise( data, self.settings['wake_shape_generator_input']) def read_files(self): # open aero file # first, file names self.aero_file_name = (self.data.case_route + '/' + self.data.case_name + '.aero.h5') # then check that the file exists h5utils.check_file_exists(self.aero_file_name) # read and store the hdf5 file with h5.File(self.aero_file_name, 'r') as aero_file_handle: # store files in dictionary self.aero_data_dict = h5utils.load_h5_in_dict(aero_file_handle) def run(self): self.data.aero = aerogrid.Aerogrid() self.data.aero.generate(self.aero_data_dict, self.data.structure, self.settings, self.data.ts) aero_tstep = self.data.aero.timestep_info[self.data.ts] self.wake_shape_generator.generate({ 'zeta': aero_tstep.zeta, 'zeta_star': aero_tstep.zeta_star, 'gamma': aero_tstep.gamma, 'gamma_star': aero_tstep.gamma_star }) # keep the call to the wake generator # because it might be needed by other solvers self.data.aero.wake_shape_generator = self.wake_shape_generator return self.data
class FrequencyLimited(BaseBalancedRom): __doc__ = librom.balfreq.__doc__ _bal_rom_id = 'FrequencyLimited' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['frequency'] = 'float' settings_default['frequency'] = 1. settings_description['frequency'] = 'defines limit frequencies for balancing. The balanced model will be accurate ' \ 'in the range ``[0,F]``, where ``F`` is the value of this key. Note that ``F`` ' \ 'units must be consistent with the units specified in the in ' \ 'the ``self.ScalingFacts`` dictionary.' settings_types['method_low'] = 'str' settings_default['method_low'] = 'trapz' settings_description['method_low'] = '``gauss`` or ``trapz`` specifies whether to use gauss quadrature or ' \ 'trapezoidal rule in the low-frequency range ``[0,F]``' settings_types['options_low'] = 'dict' settings_default['options_low'] = dict() settings_description['options_low'] = 'Settings for the low frequency integration. See Notes.' settings_types['method_high'] = 'str' settings_default['method_high'] = 'trapz' settings_description['method_high'] = '``gauss`` or ``trapz`` specifies whether to use gauss quadrature or ' \ 'trapezoidal rule in the high-frequency range ``[F,FN]``' settings_types['options_high'] = 'dict' settings_default['options_high'] = dict() settings_description['options_high'] = 'Settings for the high frequency integration. See Notes.' settings_types['check_stability'] = 'bool' settings_default['check_stability'] = True settings_description['check_stability'] = 'if True, the balanced model is truncated to eliminate ' \ 'unstable modes - if any is found. Note that very accurate ' \ 'balanced model can still be obtained, even if high order ' \ 'modes are unstable.' settings_types['get_frequency_response'] = 'bool' settings_default['get_frequency_response'] = False settings_description['get_frequency_response'] = 'if True, the function also returns the frequency ' \ 'response evaluated at the low-frequency range integration' \ ' points. If True, this option also allows to automatically' \ ' tune the balanced model.' # Integrator options settings_options_types = dict() settings_options_default = dict() settings_options_description = dict() settings_options_types['points'] = 'int' settings_options_default['points'] = 12 settings_options_description['points'] = 'Trapezoidal points of integration' settings_options_types['partitions'] = 'int' settings_options_default['partitions'] = 2 settings_options_description['partitions'] = 'Number of Gauss-Lobotto quadratures' settings_options_types['order'] = 'int' settings_options_default['order'] = 2 settings_options_description['order'] = 'Order of Gauss-Lobotto quadratures' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) options_table = settings.SettingsTable() __doc__ += options_table.generate(settings_options_types, settings_options_default, settings_options_description, 'The parameters of integration take the following options:\n') def __init__(self): self.settings = dict() def initialise(self, in_settings=None): if in_settings is not None: self.settings = in_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default, no_ctype=True) settings.to_custom_types(self.settings['options_low'], self.settings_options_types, self.settings_options_default, no_ctype=True) settings.to_custom_types(self.settings['options_high'], self.settings_options_types, self.settings_options_default, no_ctype=True) def run(self, ss): output_results = librom.balfreq(ss, self.settings) return output_results[0]
class LinearAeroelastic(ss_interface.BaseElement): r""" Assemble a linearised aeroelastic system The aeroelastic system can be seen as the coupling between a linearised aerodynamic system (System 1) and a linearised beam system (System 2). The coupled system retains inputs and outputs from both systems such that .. math:: \mathbf{u} = [\mathbf{u}_1;\, \mathbf{u}_2] and the outputs are also ordered in a similar fashion .. math:: \mathbf{y} = [\mathbf{y}_1;\, \mathbf{y}_2] Reference the individual systems for the particular ordering of the respective input and output variables. """ sys_id = 'LinearAeroelastic' settings_default = dict() settings_types = dict() settings_description = dict() settings_types['aero_settings'] = 'dict' settings_default['aero_settings'] = None settings_description['aero_settings'] = 'Linear UVLM settings' settings_types['beam_settings'] = 'dict' settings_default['beam_settings'] = None settings_description['beam_settings'] = 'Linear Beam settings' settings_types['uvlm_filename'] = 'str' settings_default['uvlm_filename'] = '' settings_description[ 'uvlm_filename'] = 'Path to .data.h5 file containing UVLM/ROM state space to load' settings_types['track_body'] = 'bool' settings_default['track_body'] = True settings_description[ 'track_body'] = 'UVLM inputs and outputs projected to coincide with lattice at linearisation' settings_types['use_euler'] = 'bool' settings_default['use_euler'] = False settings_description[ 'use_euler'] = 'Parametrise orientations in terms of Euler angles' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.ss = None # The state space object self.lsys = dict() # Contains the underlying objects self.uvlm = None self.beam = None self.load_uvlm_from_file = False self.settings = dict() self.state_variables = None self.couplings = dict() self.linearisation_vectors = dict() # Aeroelastic coupling gains # transfer self.Kdisp = None self.Kvel_disp = None self.Kdisp_vel = None self.Kvel_vel = None self.Kforces = None # stiffening factors self.Kss = None self.Krs = None self.Csr = None self.Crs = None self.Crr = None def initialise(self, data): try: self.settings = data.settings['LinearAssembler'][ 'linear_system_settings'] except KeyError: self.settings = None settings.to_custom_types(self.settings, self.settings_types, self.settings_default, no_ctype=True) if self.settings['use_euler']: self.settings['aero_settings']['use_euler'] = True self.settings['beam_settings']['use_euler'] = True # Create Linear UVLM self.uvlm = ss_interface.initialise_system('LinearUVLM') self.uvlm.initialise(data, custom_settings=self.settings['aero_settings']) if self.settings['uvlm_filename'] == '': self.uvlm.assemble(track_body=self.settings['track_body']) else: self.load_uvlm_from_file = True # Create beam self.beam = ss_interface.initialise_system('LinearBeam') self.beam.initialise(data, custom_settings=self.settings['beam_settings']) for k, v in self.uvlm.linearisation_vectors.items(): self.linearisation_vectors[k] = v for k, v in self.beam.linearisation_vectors.items(): self.linearisation_vectors[k] = v self.get_gebm2uvlm_gains(data) def assemble(self): r""" Assembly of the linearised aeroelastic system. The UVLM state-space system has already been assembled. Prior to assembling the beam's first order state-space, the damping and stiffness matrices have to be modified to include the damping and stiffenning terms that arise from the linearisation of the aeordynamic forces with respect to the A frame of reference. See :func:`sharpy.linear.src.lin_aeroela.get_gebm2uvlm_gains()` for details on the linearisation. Then the beam is assembled as per the given settings in normalised time if the aerodynamic system has been scaled. The discrete time systems of the UVLM and the beam must have the same time step. The UVLM inputs and outputs are then projected onto the structural degrees of freedom (obviously with the exception of external gusts and control surfaces). Hence, the gains :math:`\mathbf{K}_{sa}` and :math:`\mathbf{K}_{as}` are added to the output and input of the UVLM system, respectively. These gains perform the following relation: .. math:: \begin{bmatrix}\zeta \\ \zeta' \\ u_g \\ \delta \end{bmatrix} = \mathbf{K}_{as} \begin{bmatrix} \eta \\ \eta' \\ u_g \\ \delta \end{bmatrix} = .. math:: \mathbf{N}_{nodes} = \mathbf{K}_{sa} \mathbf{f}_{vertices} If the beam is expressed in modal form, the UVLM is further projected onto the beam's modes to have the following input/output structure: Returns: """ uvlm = self.uvlm beam = self.beam # Linearisation of the aerodynamic forces introduces stiffenning and damping terms into the beam matrices flex_nodes = self.beam.sys.num_dof_flex stiff_aero = np.zeros_like(beam.sys.Kstr) damping_aero = np.zeros_like(beam.sys.Cstr) stiff_aero[:flex_nodes, :flex_nodes] = self.Kss rigid_dof = beam.sys.Kstr.shape[0] - flex_nodes total_dof = flex_nodes + rigid_dof if rigid_dof > 0: rigid_dof = beam.sys.Kstr.shape[0] - self.Kss.shape[0] stiff_aero[flex_nodes:, :flex_nodes] = self.Krs damping_aero[:flex_nodes, flex_nodes:] = self.Csr damping_aero[flex_nodes:, flex_nodes:] = self.Crr damping_aero[flex_nodes:, :flex_nodes] = self.Crs beam.sys.Cstr += damping_aero beam.sys.Kstr += stiff_aero if uvlm.scaled: beam.assemble(t_ref=uvlm.sys.ScalingFacts['time']) else: beam.assemble() if not self.load_uvlm_from_file: # Projecting the UVLM inputs and outputs onto the structural degrees of freedom Ksa = self.Kforces[:beam.sys. num_dof, :] # maps aerodynamic grid forces to nodal forces # Map the nodal displacement and velocities onto the grid displacements and velocities Kas = np.zeros((uvlm.ss.inputs, 2 * beam.sys.num_dof + (uvlm.ss.inputs - 2 * self.Kdisp.shape[0]))) Kas[:2*self.Kdisp.shape[0], :2*beam.sys.num_dof] = \ np.block([[self.Kdisp[:, :beam.sys.num_dof], self.Kdisp_vel[:, :beam.sys.num_dof]], [self.Kvel_disp[:, :beam.sys.num_dof], self.Kvel_vel[:, :beam.sys.num_dof]]]) # Retain other inputs Kas[2 * self.Kdisp.shape[0]:, 2 * beam.sys.num_dof:] = np.eye(uvlm.ss.inputs - 2 * self.Kdisp.shape[0]) # Scaling if uvlm.scaled: Kas /= uvlm.sys.ScalingFacts['length'] uvlm.ss.addGain(Ksa, where='out') uvlm.ss.addGain(Kas, where='in') self.couplings['Ksa'] = Ksa self.couplings['Kas'] = Kas if self.settings['beam_settings']['modal_projection'] == True and \ self.settings['beam_settings']['inout_coords'] == 'modes': # Project UVLM onto modal space phi = beam.sys.U in_mode_matrix = np.zeros( (uvlm.ss.inputs, beam.ss.outputs + (uvlm.ss.inputs - 2 * beam.sys.num_dof))) in_mode_matrix[:2 * beam.sys.num_dof, :2 * beam.sys.num_modes] = sclalg.block_diag( phi, phi) in_mode_matrix[2 * beam.sys.num_dof:, 2 * beam.sys.num_modes:] = np.eye(uvlm.ss.inputs - 2 * beam.sys.num_dof) out_mode_matrix = phi.T uvlm.ss.addGain(in_mode_matrix, where='in') uvlm.ss.addGain(out_mode_matrix, where='out') # Reduce uvlm projected onto structural coordinates if uvlm.rom: if rigid_dof != 0: self.runrom_rbm(uvlm) else: for k, rom in uvlm.rom.items(): uvlm.ss = rom.run(uvlm.ss) else: uvlm.ss = self.load_uvlm(self.settings['uvlm_filename']) # Coupling matrices Tas = np.eye(uvlm.ss.inputs, beam.ss.outputs) Tsa = np.eye(beam.ss.inputs, uvlm.ss.outputs) # Scale coupling matrices if uvlm.scaled: Tsa *= uvlm.sys.ScalingFacts['force'] * uvlm.sys.ScalingFacts[ 'time']**2 if rigid_dof > 0: warnings.warn( 'Time scaling for problems with rigid body motion under development.' ) Tas[:flex_nodes + 6, :flex_nodes + 6] /= uvlm.sys.ScalingFacts['length'] Tas[total_dof:total_dof + flex_nodes + 6] /= uvlm.sys.ScalingFacts['length'] else: if not self.settings['beam_settings']['modal_projection']: Tas /= uvlm.sys.ScalingFacts['length'] ss = libss.couple(ss01=uvlm.ss, ss02=beam.ss, K12=Tas, K21=Tsa) # Conditioning of A matrix # cond_a = np.linalg.cond(ss.A) # if type(uvlm.ss.A) != np.ndarray: # cond_a_uvlm = np.linalg.cond(uvlm.ss.A.todense()) # else: # cond_a_uvlm = np.linalg.cond(uvlm.ss.A) # cond_a_beam = np.linalg.cond(beam.ss.A) # cout.cout_wrap('Matrix A condition = %e' % cond_a) # cout.cout_wrap('Matrix A_uvlm condition = %e' % cond_a_uvlm) # cout.cout_wrap('Matrix A_beam condition = %e' % cond_a_beam) self.couplings['Tas'] = Tas self.couplings['Tsa'] = Tsa self.state_variables = {'aero': uvlm.ss.states, 'beam': beam.ss.states} # Save zero force reference self.linearisation_vectors['forces_aero_beam_dof'] = Ksa.dot( self.linearisation_vectors['forces_aero']) if self.settings['beam_settings']['modal_projection'] == True and \ self.settings['beam_settings']['inout_coords'] == 'modes': self.linearisation_vectors[ 'forces_aero_beam_dof'] = out_mode_matrix.dot( self.linearisation_vectors['forces_aero_beam_dof']) cout.cout_wrap('Aeroelastic system assembled:') cout.cout_wrap('\tAerodynamic states: %g' % uvlm.ss.states, 1) cout.cout_wrap('\tStructural states: %g' % beam.ss.states, 1) cout.cout_wrap('\tTotal states: %g' % ss.states, 1) cout.cout_wrap('\tInputs: %g' % ss.inputs, 1) cout.cout_wrap('\tOutputs: %g' % ss.outputs, 1) return ss def update(self, u_infty): """ Updates the aeroelastic scaled system with the new reference velocity. Only the beam equations need updating since the only dependency in the forward flight velocity resides there. Args: u_infty (float): New reference velocity Returns: sharpy.linear.src.libss.ss: Updated aeroelastic state-space system """ t_ref = self.uvlm.sys.ScalingFacts['length'] / u_infty self.beam.sys.update_matrices_time_scale(t_ref) self.beam.sys.assemble() self.beam.ss = self.beam.sys.SSdisc self.ss = libss.couple(ss01=self.uvlm.ss, ss02=self.beam.ss, K12=self.couplings['Tas'], K21=self.couplings['Tsa']) return self.ss def runrom_rbm(self, uvlm): ss = uvlm.ss rig_nodes = self.beam.sys.num_dof_rig if rig_nodes == 9: orient_dof = 3 else: orient_dof = 4 # Input side if self.settings['beam_settings']['modal_projection'] == True and \ self.settings['beam_settings']['inout_coords'] == 'modes': rem_int_modes = np.zeros((ss.inputs, ss.inputs - rig_nodes)) rem_int_modes[rig_nodes:, :] = np.eye(ss.inputs - rig_nodes) # Output side - remove quaternion equations output rem_quat_out = np.zeros((ss.outputs - orient_dof, ss.outputs)) # find quaternion indices U = self.beam.sys.U indices = np.where(U[-orient_dof:, :] == 1.)[1] j = 0 for i in range(ss.outputs): if i in indices: continue rem_quat_out[j, i] = 1 j += 1 else: rem_int_modes = np.zeros((ss.inputs, ss.inputs - rig_nodes)) rem_int_modes[:self.beam.sys.num_dof_flex, :self.beam.sys.num_dof_flex] = \ np.eye(self.beam.sys.num_dof_flex) rem_int_modes[self.beam.sys.num_dof_flex+rig_nodes:, self.beam.sys.num_dof_flex:] = \ np.eye(ss.inputs - self.beam.sys.num_dof_flex - rig_nodes) rem_quat_out = np.zeros((ss.outputs - orient_dof, ss.outputs)) rem_quat_out[:, :-orient_dof] = np.eye(ss.outputs - orient_dof) ss.addGain(rem_int_modes, where='in') ss.addGain(rem_quat_out, where='out') for k, rom in uvlm.rom.items(): uvlm.ss = rom.run(uvlm.ss) uvlm.ss.addGain(rem_int_modes.T, where='in') uvlm.ss.addGain(rem_quat_out.T, where='out') def get_gebm2uvlm_gains(self, data): r""" Provides: - the gain matrices required to connect the linearised GEBM and UVLM inputs/outputs - the stiffening and damping factors to be added to the linearised GEBM equations in order to account for non-zero aerodynamic loads at the linearisation point. The function produces the gain matrices: - ``Kdisp``: gains from GEBM to UVLM grid displacements - ``Kvel_disp``: influence of GEBM dofs displacements to UVLM grid velocities. - ``Kvel_vel``: influence of GEBM dofs displacements to UVLM grid displacements. - ``Kforces`` (UVLM->GEBM) dimensions are the transpose than the Kdisp and Kvel* matrices. Hence, when allocation this term, ``ii`` and ``jj`` indices will unintuitively refer to columns and rows, respectively. And the stiffening/damping terms accounting for non-zero aerodynamic forces at the linearisation point: - ``Kss``: stiffness factor (flexible dof -> flexible dof) accounting for non-zero forces at the linearisation point. - ``Csr``: damping factor (rigid dof -> flexible dof) - ``Crs``: damping factor (flexible dof -> rigid dof) - ``Crr``: damping factor (rigid dof -> rigid dof) Stiffening and damping related terms due to the non-zero aerodynamic forces at the linearisation point: .. math:: \mathbf{F}_{A,n} = C^{AG}(\mathbf{\chi})\sum_j \mathbf{f}_{G,j} \rightarrow \delta\mathbf{F}_{A,n} = C^{AG}_0 \sum_j \delta\mathbf{f}_{G,j} + \frac{\partial}{\partial\chi}(C^{AG}\sum_j \mathbf{f}_{G,j}^0)\delta\chi The term multiplied by the variation in the quaternion, :math:`\delta\chi`, couples the forces with the rigid body equations and becomes part of :math:`\mathbf{C}_{sr}`. Similarly, the linearisation of the moments results in expression that contribute to the stiffness and damping matrices. .. math:: \mathbf{M}_{B,n} = \sum_j \tilde{X}_B C^{BA}(\Psi)C^{AG}(\chi)\mathbf{f}_{G,j} .. math:: \delta\mathbf{M}_{B,n} = \sum_j \tilde{X}_B\left(C_0^{BG}\delta\mathbf{f}_{G,j} + \frac{\partial}{\partial\Psi}(C^{BA}\delta\mathbf{f}^0_{A,j})\delta\Psi + \frac{\partial}{\partial\chi}(C^{BA}_0 C^{AG} \mathbf{f}_{G,j})\delta\chi\right) The linearised equations of motion for the geometrically exact beam model take the input term :math:`\delta \mathbf{Q}_n = \{\delta\mathbf{F}_{A,n},\, T_0^T\delta\mathbf{M}_{B,n}\}`, which means that the moments should be provided as :math:`T^T(\Psi)\mathbf{M}_B` instead of :math:`\mathbf{M}_A = C^{AB}\mathbf{M}_B`, where :math:`T(\Psi)` is the tangential operator. .. math:: \delta(T^T\mathbf{M}_B) = T^T_0\delta\mathbf{M}_B + \frac{\partial}{\partial\Psi}(T^T\delta\mathbf{M}_B^0)\delta\Psi is the linearised expression for the moments, where the first term would correspond to the input terms to the beam equations and the second arises due to the non-zero aerodynamic moment at the linearisation point and must be subtracted (since it comes from the forces) to form part of :math:`\mathbf{K}_{ss}`. In addition, the :math:`\delta\mathbf{M}_B` term depends on both :math:`\delta\Psi` and :math:`\delta\chi`, therefore those terms would also contribute to :math:`\mathbf{K}_{ss}` and :math:`\mathbf{C}_{sr}`, respectively. The contribution from the total forces and moments will be accounted for in :math:`\mathbf{C}_{rr}` and :math:`\mathbf{C}_{rs}`. .. math:: \delta\mathbf{F}_{tot,A} = \sum_n\left(C^{GA}_0 \sum_j \delta\mathbf{f}_{G,j} + \frac{\partial}{\partial\chi}(C^{AG}\sum_j \mathbf{f}_{G,j}^0)\delta\chi\right) Therefore, after running this method, the beam matrices will be updated as: >>> K_beam[:flex_dof, :flex_dof] += Kss >>> C_beam[:flex_dof, -rigid_dof:] += Csr >>> C_beam[-rigid_dof:, :flex_dof] += Crs >>> C_beam[-rigid_dof:, -rigid_dof:] += Crr Track body option The ``track_body`` setting restricts the UVLM grid to linear translation motions and therefore should be used to ensure that the forces are computed using the reference linearisation frame. The UVLM and beam are linearised about a reference equilibrium condition. The UVLM is defined in the inertial reference frame while the beam employs the body attached frame and therefore a projection from one frame onto another is required during the coupling process. However, the inputs to the UVLM (i.e. the lattice grid coordinates) are obtained from the beam deformation which is expressed in A frame and therefore the grid coordinates need to be projected onto the inertial frame ``G``. As the beam rotates, the projection onto the ``G`` frame of the lattice grid coordinates will result in a grid that is not coincident with that at the linearisation reference and therefore the grid coordinates must be projected onto the original frame, which will be referred to as ``U``. The transformation between the inertial frame ``G`` and the ``U`` frame is a function of the rotation of the ``A`` frame and the original position: .. math:: C^{UG}(\chi) = C^{GA}(\chi_0)C^{AG}(\chi) Therefore, the grid coordinates obtained in ``A`` frame and projected onto the ``G`` frame can be transformed to the ``U`` frame using .. math:: \zeta_U = C^{UG}(\chi) \zeta_G which allows the grid lattice coordinates to be projected onto the original linearisation frame. In a similar fashion, the output lattice vertex forces of the UVLM are defined in the original linearisation frame ``U`` and need to be transformed onto the inertial frame ``G`` prior to projecting them onto the ``A`` frame to use them as the input forces to the beam system. .. math:: \boldsymbol{f}_G = C^{GU}(\chi)\boldsymbol{f}_U The linearisation of the above relations lead to the following expressions that have to be added to the coupling matrices: * ``Kdisp_vel`` terms: .. math:: \delta\boldsymbol{\zeta}_U= C^{GA}_0 \frac{\partial}{\partial \boldsymbol{\chi}} \left(C^{AG}\boldsymbol{\zeta}_{G,0}\right)\delta\boldsymbol{\chi} + \delta\boldsymbol{\zeta}_G * ``Kvel_vel`` terms: .. math:: \delta\dot{\boldsymbol{\zeta}}_U= C^{GA}_0 \frac{\partial}{\partial \boldsymbol{\chi}} \left(C^{AG}\dot{\boldsymbol{\zeta}}_{G,0}\right)\delta\boldsymbol{\chi} + \delta\dot{\boldsymbol{\zeta}}_G The transformation of the forces and moments introduces terms that are functions of the orientation and are included as stiffening and damping terms in the beam's matrices: * ``Csr`` damping terms relating to translation forces: .. math:: C_{sr}^{tra} -= \frac{\partial}{\partial\boldsymbol{\chi}} \left(C^{GA} C^{AG}_0 \boldsymbol{f}_{G,0}\right)\delta\boldsymbol{\chi} * ``Csr`` damping terms related to moments: .. math:: C_{sr}^{rot} -= T^\top\widetilde{\mathbf{X}}_B C^{BG} \frac{\partial}{\partial\boldsymbol{\chi}} \left(C^{GA} C^{AG}_0 \boldsymbol{f}_{G,0}\right)\delta\boldsymbol{\chi} The ``track_body`` setting. When ``track_body`` is enabled, the UVLM grid is no longer coincident with the inertial reference frame throughout the simulation but rather it is able to rotate as the ``A`` frame rotates. This is to simulate a free flying vehicle, where, for instance, the orientation does not affect the aerodynamics. The UVLM defined in this frame of reference, named ``U``, satisfies the following convention: * The ``U`` frame is coincident with the ``G`` frame at the time of linearisation. * The ``U`` frame rotates as the ``A`` frame rotates. Transformations related to the ``U`` frame of reference: * The angle between the ``U`` frame and the ``A`` frame is always constant and equal to :math:`\boldsymbol{\Theta}_0`. * The angle between the ``A`` frame and the ``G`` frame is :math:`\boldsymbol{\Theta}=\boldsymbol{\Theta}_0 + \delta\boldsymbol{\Theta}` * The projection of a vector expressed in the ``G`` frame onto the ``U`` frame is expressed by: .. math:: \boldsymbol{v}^U = C^{GA}_0 C^{AG} \boldsymbol{v}^G * The reverse, a projection of a vector expressed in the ``U`` frame onto the ``G`` frame, is expressed by .. math:: \boldsymbol{v}^U = C^{GA} C^{AG}_0 \boldsymbol{v}^U The effect this has on the aeroelastic coupling between the UVLM and the structural dynamics is that the orientation and change of orientation of the vehicle has no effect on the aerodynamics. The aerodynamics are solely affected by the contribution of the 6-rigid body velocities (as well as the flexible DOFs velocities). """ aero = data.aero structure = data.structure tsaero = self.uvlm.tsaero0 tsstr = self.beam.tsstruct0 Kzeta = self.uvlm.sys.Kzeta num_dof_str = self.beam.sys.num_dof_str num_dof_rig = self.beam.sys.num_dof_rig num_dof_flex = self.beam.sys.num_dof_flex use_euler = self.beam.sys.use_euler # allocate output Kdisp = np.zeros((3 * Kzeta, num_dof_str)) Kdisp_vel = np.zeros( (3 * Kzeta, num_dof_str)) # Orientation is in velocity DOFs Kvel_disp = np.zeros((3 * Kzeta, num_dof_str)) Kvel_vel = np.zeros((3 * Kzeta, num_dof_str)) Kforces = np.zeros((num_dof_str, 3 * Kzeta)) Kss = np.zeros((num_dof_flex, num_dof_flex)) Csr = np.zeros((num_dof_flex, num_dof_rig)) Crs = np.zeros((num_dof_rig, num_dof_flex)) Crr = np.zeros((num_dof_rig, num_dof_rig)) Krs = np.zeros((num_dof_rig, num_dof_flex)) # get projection matrix A->G # (and other quantities indep. from nodal position) Cga = algebra.quat2rotation(tsstr.quat) # NG 6-8-19 removing .T Cag = Cga.T # for_pos=tsstr.for_pos for_vel = tsstr.for_vel[:3] for_rot = tsstr.for_vel[3:] skew_for_rot = algebra.skew(for_rot) Der_vel_Ra = np.dot(Cga, skew_for_rot) Faero = np.zeros(3) FaeroA = np.zeros(3) # GEBM degrees of freedom jj_for_tra = range(num_dof_str - num_dof_rig, num_dof_str - num_dof_rig + 3) jj_for_rot = range(num_dof_str - num_dof_rig + 3, num_dof_str - num_dof_rig + 6) if use_euler: jj_euler = range(num_dof_str - 3, num_dof_str) euler = algebra.quat2euler(tsstr.quat) tsstr.euler = euler else: jj_quat = range(num_dof_str - 4, num_dof_str) jj = 0 # nodal dof index for node_glob in range(structure.num_node): ### detect bc at node (and no. of dofs) bc_here = structure.boundary_conditions[node_glob] if bc_here == 1: # clamp (only rigid-body) dofs_here = 0 jj_tra, jj_rot = [], [] # continue elif bc_here == -1 or bc_here == 0: # (rigid+flex body) dofs_here = 6 jj_tra = 6 * structure.vdof[node_glob] + np.array([0, 1, 2], dtype=int) jj_rot = 6 * structure.vdof[node_glob] + np.array([3, 4, 5], dtype=int) else: raise NameError('Invalid boundary condition (%d) at node %d!' \ % (bc_here, node_glob)) jj += dofs_here # retrieve element and local index ee, node_loc = structure.node_master_elem[node_glob, :] # get position, crv and rotation matrix Ra = tsstr.pos[node_glob, :] # in A FoR, w.r.t. origin A-G Rg = np.dot(Cag.T, Ra) # in G FoR, w.r.t. origin A-G psi = tsstr.psi[ee, node_loc, :] psi_dot = tsstr.psi_dot[ee, node_loc, :] Cab = algebra.crv2rotation(psi) Cba = Cab.T Cbg = np.dot(Cab.T, Cag) Tan = algebra.crv2tan(psi) track_body = self.settings['track_body'] ### str -> aero mapping # some nodes may be linked to multiple surfaces... for str2aero_here in aero.struct2aero_mapping[node_glob]: # detect surface/span-wise coordinate (ss,nn) nn, ss = str2aero_here['i_n'], str2aero_here['i_surf'] # print('%.2d,%.2d'%(nn,ss)) # surface panelling M = aero.aero_dimensions[ss][0] N = aero.aero_dimensions[ss][1] Kzeta_start = 3 * sum(self.uvlm.sys.MS.KKzeta[:ss]) shape_zeta = (3, M + 1, N + 1) for mm in range(M + 1): # get bound vertex index ii_vert = [ Kzeta_start + np.ravel_multi_index( (cc, mm, nn), shape_zeta) for cc in range(3) ] # get position vectors zetag = tsaero.zeta[ss][:, mm, nn] # in G FoR, w.r.t. origin A-G zetaa = np.dot(Cag, zetag) # in A FoR, w.r.t. origin A-G Xg = zetag - Rg # in G FoR, w.r.t. origin B Xb = np.dot(Cbg, Xg) # in B FoR, w.r.t. origin B # get rotation terms Xbskew = algebra.skew(Xb) XbskewTan = np.dot(Xbskew, Tan) # get velocity terms zetag_dot = tsaero.zeta_dot[ss][:, mm, nn] - Cga.dot( for_vel) # in G FoR, w.r.t. origin A-G zetaa_dot = np.dot( Cag, zetag_dot) # in A FoR, w.r.t. origin A-G # get aero force faero = tsaero.forces[ss][:3, mm, nn] Faero += faero faero_a = np.dot(Cag, faero) FaeroA += faero_a maero_g = np.cross(Xg, faero) maero_b = np.dot(Cbg, maero_g) ### ---------------------------------------- allocate Kdisp if bc_here != 1: # wrt pos - Eq 25 second term Kdisp[np.ix_(ii_vert, jj_tra)] += Cga # wrt psi - Eq 26 Kdisp[np.ix_(ii_vert, jj_rot)] -= np.dot(Cbg.T, XbskewTan) # w.r.t. position of FoR A (w.r.t. origin G) # null as A and G have always same origin in SHARPy # # ### w.r.t. quaternion (attitude changes) if use_euler: Kdisp_vel[np.ix_(ii_vert, jj_euler)] += \ algebra.der_Ceuler_by_v(tsstr.euler, zetaa) # Track body - project inputs as for A not moving if track_body: Kdisp_vel[np.ix_(ii_vert, jj_euler)] += \ Cga.dot(algebra.der_Peuler_by_v(tsstr.euler, zetag)) else: # Equation 25 # Kdisp[np.ix_(ii_vert, jj_quat)] += \ # algebra.der_Cquat_by_v(tsstr.quat, zetaa) Kdisp_vel[np.ix_(ii_vert, jj_quat)] += \ algebra.der_Cquat_by_v(tsstr.quat, zetaa) # Track body - project inputs as for A not moving if track_body: Kdisp_vel[np.ix_(ii_vert, jj_quat)] += \ Cga.dot(algebra.der_CquatT_by_v(tsstr.quat, zetag)) ### ------------------------------------ allocate Kvel_disp if bc_here != 1: # # wrt pos Kvel_disp[np.ix_(ii_vert, jj_tra)] += Der_vel_Ra # wrt psi (at zero psi_dot) Kvel_disp[np.ix_(ii_vert, jj_rot)] -= \ np.dot(Cga, np.dot(skew_for_rot, np.dot(Cab, XbskewTan))) # # wrt psi (psi_dot contributions - verified) Kvel_disp[np.ix_(ii_vert, jj_rot)] += np.dot( Cbg.T, np.dot(algebra.skew(np.dot(XbskewTan, psi_dot)), Tan)) if np.linalg.norm(psi) >= 1e-6: Kvel_disp[np.ix_(ii_vert, jj_rot)] -= \ np.dot(Cbg.T, np.dot(Xbskew, algebra.der_Tan_by_xv(psi, psi_dot))) # # w.r.t. position of FoR A (w.r.t. origin G) # # null as A and G have always same origin in SHARPy # # ### w.r.t. quaternion (attitude changes) - Eq 30 if use_euler: Kvel_vel[np.ix_(ii_vert, jj_euler)] += \ algebra.der_Ceuler_by_v(tsstr.euler, zetaa_dot) # Track body if ForA is rotating if track_body: Kvel_vel[np.ix_(ii_vert, jj_euler)] += \ Cga.dot(algebra.der_Peuler_by_v(tsstr.euler, zetag_dot)) else: Kvel_vel[np.ix_(ii_vert, jj_quat)] += \ algebra.der_Cquat_by_v(tsstr.quat, zetaa_dot) # Track body if ForA is rotating if track_body: Kvel_vel[np.ix_(ii_vert, jj_quat)] += \ Cga.dot(algebra.der_CquatT_by_v(tsstr.quat, zetag_dot)) ### ------------------------------------- allocate Kvel_vel if bc_here != 1: # wrt pos_dot Kvel_vel[np.ix_(ii_vert, jj_tra)] += Cga # # wrt crv_dot Kvel_vel[np.ix_(ii_vert, jj_rot)] -= np.dot(Cbg.T, XbskewTan) # # wrt velocity of FoR A Kvel_vel[np.ix_(ii_vert, jj_for_tra)] += Cga Kvel_vel[np.ix_(ii_vert, jj_for_rot)] -= \ np.dot(Cga, algebra.skew(zetaa)) # wrt rate of change of quaternion: not implemented! ### -------------------------------------- allocate Kforces if bc_here != 1: # nodal forces Kforces[np.ix_(jj_tra, ii_vert)] += Cag # nodal moments Kforces[np.ix_(jj_rot, ii_vert)] += \ np.dot(Tan.T, np.dot(Cbg, algebra.skew(Xg))) # or, equivalently, np.dot( algebra.skew(Xb),Cbg) # total forces Kforces[np.ix_(jj_for_tra, ii_vert)] += Cag # total moments Kforces[np.ix_(jj_for_rot, ii_vert)] += \ np.dot(Cag, algebra.skew(zetag)) # quaternion equation # null, as not dep. on external forces ### --------------------------------------- allocate Kstiff ### flexible dof equations (Kss and Csr) if bc_here != 1: # nodal forces if use_euler: if not track_body: Csr[jj_tra, -3:] -= algebra.der_Peuler_by_v( tsstr.euler, faero) # Csr[jj_tra, -3:] -= algebra.der_Ceuler_by_v(tsstr.euler, Cga.T.dot(faero)) else: if not track_body: Csr[jj_tra, -4:] -= algebra.der_CquatT_by_v( tsstr.quat, faero) # Track body # if track_body: # Csr[jj_tra, -4:] -= algebra.der_Cquat_by_v(tsstr.quat, Cga.T.dot(faero)) ### moments TanTXbskew = np.dot(Tan.T, Xbskew) # contrib. of TanT (dpsi) - Eq 37 - Integration of UVLM and GEBM Kss[np.ix_(jj_rot, jj_rot)] -= algebra.der_TanT_by_xv( psi, maero_b) # contrib of delta aero moment (dpsi) - Eq 36 Kss[np.ix_(jj_rot, jj_rot)] -= \ np.dot(TanTXbskew, algebra.der_CcrvT_by_v(psi, np.dot(Cag, faero))) # contribution of delta aero moment (dquat) if use_euler: if not track_body: Csr[jj_rot, -3:] -= \ np.dot(TanTXbskew, np.dot(Cba, algebra.der_Peuler_by_v(tsstr.euler, faero))) # if track_body: # Csr[jj_rot, -3:] -= \ # np.dot(TanTXbskew, # np.dot(Cbg, # algebra.der_Peuler_by_v(tsstr.euler, Cga.T.dot(faero)))) else: if not track_body: Csr[jj_rot, -4:] -= \ np.dot(TanTXbskew, np.dot(Cba, algebra.der_CquatT_by_v(tsstr.quat, faero))) # Track body # if track_body: # Csr[jj_rot, -4:] -= \ # np.dot(TanTXbskew, # np.dot(Cbg, # algebra.der_CquatT_by_v(tsstr.quat, Cga.T.dot(faero)))) ### rigid body eqs (Crs and Crr) if bc_here != 1: # Changed Crs to Krs - NG 14/5/19 # moments contribution due to delta_Ra (+ sign intentional) Krs[3:6, jj_tra] += algebra.skew(faero_a) # moment contribution due to delta_psi (+ sign intentional) Krs[3:6, jj_rot] += np.dot(algebra.skew(faero_a), algebra.der_Ccrv_by_v(psi, Xb)) if use_euler: if not track_body: # total force Crr[:3, -3:] -= algebra.der_Peuler_by_v( tsstr.euler, faero) # total moment contribution due to change in euler angles Crr[3:6, -3:] -= algebra.der_Peuler_by_v( tsstr.euler, np.cross(zetag, faero)) Crr[3:6, -3:] += np.dot( np.dot(Cag, algebra.skew(faero)), algebra.der_Peuler_by_v( tsstr.euler, np.dot(Cab, Xb))) else: if not track_body: # total force Crr[:3, -4:] -= algebra.der_CquatT_by_v( tsstr.quat, faero) # total moment contribution due to quaternion Crr[3:6, -4:] -= algebra.der_CquatT_by_v( tsstr.quat, np.cross(zetag, faero)) Crr[3:6, -4:] += np.dot( np.dot(Cag, algebra.skew(faero)), algebra.der_CquatT_by_v( tsstr.quat, np.dot(Cab, Xb))) # # Track body # if track_body: # # NG 20/8/19 - is the Cag needed here? Verify # Crr[:3, -4:] -= Cag.dot(algebra.der_Cquat_by_v(tsstr.quat, Cga.T.dot(faero))) # # Crr[3:6, -4:] -= Cag.dot(algebra.skew(zetag).dot(algebra.der_Cquat_by_v(tsstr.quat, Cga.T.dot(faero)))) # Crr[3:6, -4:] += Cag.dot(algebra.skew(faero)).dot(algebra.der_Cquat_by_v(tsstr.quat, Cga.T.dot(zetag))) # transfer self.Kdisp = Kdisp self.Kvel_disp = Kvel_disp self.Kdisp_vel = Kdisp_vel self.Kvel_vel = Kvel_vel self.Kforces = Kforces # stiffening factors self.Kss = Kss self.Krs = Krs self.Csr = Csr self.Crs = Crs self.Crr = Crr @staticmethod def load_uvlm(filename): import sharpy.utils.h5utils as h5 cout.cout_wrap( 'Loading UVLM state space system projected onto structural DOFs from file' ) read_data = h5.readh5(filename).ss # uvlm_ss_read = read_data.linear.linear_system.uvlm.ss uvlm_ss_read = read_data return libss.ss(uvlm_ss_read.A, uvlm_ss_read.B, uvlm_ss_read.C, uvlm_ss_read.D, dt=uvlm_ss_read.dt)
class Balanced(rom_interface.BaseRom): """Balancing ROM methods Main class to load a balancing ROM. See below for the appropriate settings to be parsed in the ``algorithm_settings`` based on your selection. Supported algorithms: * Direct balancing :class:`.Direct` * Iterative balancing * Frequency limited balancing :class:`.FrequencyLimited` """ rom_id = 'Balanced' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['algorithm'] = 'str' settings_default['algorithm'] = '' settings_description['algorithm'] = 'Balanced realisation method' settings_types['algorithm_settings'] = 'dict' settings_default['algorithm_settings'] = dict() settings_description['algorithm_settings'] = 'Settings for the desired algorithm' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.settings = dict() self.algorithm = None self.ssrom = None self.ss = None self.dtsystem = None def initialise(self, in_settings=None): if in_settings is not None: self.settings = in_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) if not (self.settings['algorithm'] in dict_of_balancing_roms): raise AttributeError('Balancing algorithm %s is not yet implemented' % self.settings['algorithm']) self.algorithm = dict_of_balancing_roms[self.settings['algorithm']]() self.algorithm.initialise(self.settings['algorithm_settings']) def run(self, ss): self.ss = ss A, B, C, D = self.ss.get_mats() if self.ss.dt: self.dtsystem = True else: self.dtsystem = False out = self.algorithm.run(ss) if type(out) == libss.ss: self.ssrom = out else: Ar, Br, Cr = out if self.dtsystem: self.ssrom = libss.ss(Ar, Br, Cr, D, dt=self.ss.dt) else: self.ssrom = libss.ss(Ar, Br, Cr, D) return self.ssrom
class StaticCoupled(BaseSolver): """ This class is the main FSI driver for static simulations. It requires a ``structural_solver`` and a ``aero_solver`` to be defined. """ solver_id = 'StaticCoupled' solver_classification = 'Coupled' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['print_info'] = 'bool' settings_default['print_info'] = True settings_description['print_info'] = 'Write status to screen' settings_types['structural_solver'] = 'str' settings_default['structural_solver'] = None settings_description[ 'structural_solver'] = 'Structural solver to use in the coupled simulation' settings_types['structural_solver_settings'] = 'dict' settings_default['structural_solver_settings'] = None settings_description[ 'structural_solver_settings'] = 'Dictionary of settings for the structural solver' settings_types['aero_solver'] = 'str' settings_default['aero_solver'] = None settings_description[ 'aero_solver'] = 'Aerodynamic solver to use in the coupled simulation' settings_types['aero_solver_settings'] = 'dict' settings_default['aero_solver_settings'] = None settings_description[ 'aero_solver_settings'] = 'Dictionary of settings for the aerodynamic solver' settings_types['max_iter'] = 'int' settings_default['max_iter'] = 100 settings_description['max_iter'] = 'Max iterations in the FSI loop' settings_types['n_load_steps'] = 'int' settings_default['n_load_steps'] = 0 settings_description[ 'n_load_steps'] = 'Length of ramp for forces and gravity during FSI iteration' settings_types['tolerance'] = 'float' settings_default['tolerance'] = 1e-5 settings_description[ 'tolerance'] = 'Convergence threshold for the FSI loop' settings_types['relaxation_factor'] = 'float' settings_default['relaxation_factor'] = 0. settings_description[ 'relaxation_factor'] = 'Relaxation parameter in the FSI iteration. 0 is no relaxation and -> 1 is very relaxed' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.data = None self.settings = None self.structural_solver = None self.aero_solver = None self.previous_force = None self.residual_table = None def initialise(self, data, input_dict=None): self.data = data if input_dict is None: self.settings = data.settings[self.solver_id] else: self.settings = input_dict settings.to_custom_types(self.settings, self.settings_types, self.settings_default) self.print_info = self.settings['print_info'] self.structural_solver = solver_interface.initialise_solver( self.settings['structural_solver']) self.structural_solver.initialise( self.data, self.settings['structural_solver_settings']) self.aero_solver = solver_interface.initialise_solver( self.settings['aero_solver']) self.aero_solver.initialise(self.structural_solver.data, self.settings['aero_solver_settings']) self.data = self.aero_solver.data if self.print_info: self.residual_table = cout.TablePrinter( 9, 8, ['g', 'g', 'f', 'f', 'f', 'f', 'f', 'f', 'f']) self.residual_table.field_length[0] = 3 self.residual_table.field_length[1] = 3 self.residual_table.field_length[2] = 10 self.residual_table.print_header([ 'iter', 'step', 'log10(res)', 'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz' ]) def increase_ts(self): self.data.ts += 1 self.structural_solver.next_step() self.aero_solver.next_step() def cleanup_timestep_info(self): if max(len(self.data.aero.timestep_info), len(self.data.structure.timestep_info)) > 1: # copy last info to first self.data.aero.timestep_info[0] = self.data.aero.timestep_info[ -1].copy() self.data.structure.timestep_info[ 0] = self.data.structure.timestep_info[-1].copy() # delete all the rest while len(self.data.aero.timestep_info) - 1: del self.data.aero.timestep_info[-1] while len(self.data.structure.timestep_info) - 1: del self.data.structure.timestep_info[-1] self.data.ts = 0 def run(self): for i_step in range(self.settings['n_load_steps'].value + 1): if (i_step == self.settings['n_load_steps'].value and self.settings['n_load_steps'].value > 0): break # load step coefficient if not self.settings['n_load_steps'].value == 0: load_step_multiplier = ( i_step + 1.0) / self.settings['n_load_steps'].value else: load_step_multiplier = 1.0 # new storage every load step if i_step > 0: self.increase_ts() for i_iter in range(self.settings['max_iter'].value): # run aero self.data = self.aero_solver.run() # map force struct_forces = mapping.aero2struct_force_mapping( self.data.aero.timestep_info[self.data.ts].forces, self.data.aero.struct2aero_mapping, self.data.aero.timestep_info[self.data.ts].zeta, self.data.structure.timestep_info[self.data.ts].pos, self.data.structure.timestep_info[self.data.ts].psi, self.data.structure.node_master_elem, self.data.structure.connectivities, self.data.structure.timestep_info[self.data.ts].cag(), self.data.aero.aero_dict) if not self.settings['relaxation_factor'].value == 0.: if i_iter == 0: self.previous_force = struct_forces.copy() temp = struct_forces.copy() struct_forces = ( (1.0 - self.settings['relaxation_factor'].value) * struct_forces + self.settings['relaxation_factor'].value * self.previous_force) self.previous_force = temp # copy force in beam old_g = self.structural_solver.settings['gravity'].value self.structural_solver.settings[ 'gravity'] = old_g * load_step_multiplier temp1 = load_step_multiplier * ( struct_forces + self.data.structure.ini_info.steady_applied_forces) self.data.structure.timestep_info[ self.data.ts].steady_applied_forces[:] = temp1 # run beam self.data = self.structural_solver.run() self.structural_solver.settings['gravity'] = ct.c_double(old_g) (self.data.structure.timestep_info[ self.data.ts].total_forces[0:3], self.data.structure.timestep_info[self.data.ts]. total_forces[3:6]) = (self.extract_resultants( self.data.structure.timestep_info[self.data.ts])) # update grid self.aero_solver.update_step() # convergence if self.convergence(i_iter, i_step): # create q and dqdt vectors self.structural_solver.update( self.data.structure.timestep_info[self.data.ts]) self.cleanup_timestep_info() break return self.data def convergence(self, i_iter, i_step): if i_iter == self.settings['max_iter'].value - 1: cout.cout_wrap('StaticCoupled did not converge!', 0) # quit(-1) return_value = None if i_iter == 0: self.initial_residual = np.linalg.norm( self.data.structure.timestep_info[self.data.ts].pos) self.previous_residual = self.initial_residual self.current_residual = self.initial_residual if self.print_info: forces = self.data.structure.timestep_info[ self.data.ts].total_forces self.residual_table.print_line([ i_iter, i_step, 0.0, forces[0], forces[1], forces[2], forces[3], forces[4], forces[5], ]) return False self.current_residual = np.linalg.norm( self.data.structure.timestep_info[self.data.ts].pos) if self.print_info: forces = self.data.structure.timestep_info[ self.data.ts].total_forces res_print = np.NINF if (np.abs(self.current_residual - self.previous_residual) > sys.float_info.epsilon * 10): res_print = np.log10( np.abs(self.current_residual - self.previous_residual) / self.initial_residual) self.residual_table.print_line([ i_iter, i_step, res_print, forces[0], forces[1], forces[2], forces[3], forces[4], forces[5], ]) if return_value is None: if np.abs( self.current_residual - self.previous_residual ) / self.initial_residual < self.settings['tolerance'].value: return_value = True else: self.previous_residual = self.current_residual return_value = False if return_value is None: return_value = False return return_value def change_trim(self, alpha, thrust, thrust_nodes, tail_deflection, tail_cs_index): # self.cleanup_timestep_info() self.data.structure.timestep_info = [] self.data.structure.timestep_info.append( self.data.structure.ini_info.copy()) aero_copy = self.data.aero.timestep_info[-1] self.data.aero.timestep_info = [] self.data.aero.timestep_info.append(aero_copy) self.data.ts = 0 # alpha orientation_quat = algebra.euler2quat(np.array([0.0, alpha, 0.0])) self.data.structure.timestep_info[0].quat[:] = orientation_quat[:] try: self.force_orientation except AttributeError: self.force_orientation = np.zeros((len(thrust_nodes), 3)) for i_node, node in enumerate(thrust_nodes): self.force_orientation[i_node, :] = (algebra.unit_vector( self.data.structure.ini_info.steady_applied_forces[node, 0:3])) # print(self.force_orientation) # thrust # thrust is scaled so that the direction of the forces is conserved # in all nodes. # the `thrust` parameter is the force PER node. # if there are two or more nodes in thrust_nodes, the total forces # is n_nodes_in_thrust_nodes*thrust # thrust forces have to be indicated in structure.ini_info # print(algebra.unit_vector(self.data.structure.ini_info.steady_applied_forces[0, 0:3])*thrust) for i_node, node in enumerate(thrust_nodes): # self.data.structure.ini_info.steady_applied_forces[i_node, 0:3] = ( # algebra.unit_vector(self.data.structure.ini_info.steady_applied_forces[i_node, 0:3])*thrust) self.data.structure.ini_info.steady_applied_forces[node, 0:3] = ( self.force_orientation[i_node, :] * thrust) self.data.structure.timestep_info[0].steady_applied_forces[ node, 0:3] = (self.force_orientation[i_node, :] * thrust) # tail deflection try: self.data.aero.aero_dict['control_surface_deflection'][ tail_cs_index] = tail_deflection except KeyError: raise Exception('This model has no control surfaces') except IndexError: raise Exception( 'The tail control surface index > number of surfaces') # update grid self.aero_solver.update_step() def extract_resultants(self, tstep=None): return self.structural_solver.extract_resultants(tstep)
class Direct(BaseBalancedRom): __doc__ = librom.balreal_direct_py.__doc__ _bal_rom_id = 'Direct' settings_types = dict() settings_default = dict() settings_description = dict() settings_types['tune'] = 'bool' settings_default['tune'] = True settings_description['tune'] = 'Tune ROM to specified tolerance' settings_types['rom_tolerance'] = 'float' settings_default['rom_tolerance'] = 1e-2 settings_description['rom_tolerance'] = 'Absolute accuracy with respect to full order frequency response' settings_types['rom_tune_freq_range'] = 'list(float)' settings_default['rom_tune_freq_range'] = [0, 1] settings_description['rom_tune_freq_range'] = 'Beginning and end of frequency range where to tune ROM' settings_types['convergence'] = 'str' settings_default['convergence'] = 'min' settings_description['convergence'] = 'ROM tuning convergence. If ``min`` attempts to find minimal number of states.' \ 'If ``all`` it starts from larger size ROM until convergence to ' \ 'specified tolerance is found.' settings_types['reduction_method'] = 'str' settings_default['reduction_method'] = 'realisation' settings_description['reduction_method'] = 'Reduction method. ``realisation`` or ``truncation``' settings_table = settings.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) def __init__(self): self.settings = dict() def initialise(self, in_settings=None): if in_settings is not None: self.settings = in_settings settings.to_custom_types(self.settings, self.settings_types, self.settings_default) def run(self, ss): A, B, C, D = ss.get_mats() try: if ss.dt is not None: dtsystem = True else: dtsystem = False except AttributeError: dtsystem = False S, T, Tinv = librom.balreal_direct_py(A, B, C, DLTI=dtsystem) Ar = T.dot(A.dot(Tinv)) Br = T.dot(B) Cr = C.dot(Tinv) if self.dtsystem: ss_bal = libss.ss(Ar, Br, Cr, self.ss.D, dt=self.ss.dt) else: ss_bal = libss.ss(Ar, Br, Cr, self.ss.D) if self.settings['tune']: kv = np.linspace(self.settings['rom_tune_freq_range'][0], self.settings['rom_tune_freq_range'][1]) ssrom = librom.tune_rom(ss_bal, kv=kv, tol=self.settings['rom_tolerance'], gv=S, convergence=self.settings['convergence'], method=self.settings['reduction_method']) return ssrom else: return ss_bal