diff --git a/sharpy/solvers/aerogridloader.py b/sharpy/solvers/aerogridloader.py index bc8f9a8df..af8098676 100644 --- a/sharpy/solvers/aerogridloader.py +++ b/sharpy/solvers/aerogridloader.py @@ -6,12 +6,13 @@ import sharpy.utils.settings as settings_utils import sharpy.utils.h5utils as h5utils import sharpy.utils.generator_interface as gen_interface +from sharpy.solvers.gridloader import GridLoader @solver -class AerogridLoader(BaseSolver): +class AerogridLoader(GridLoader): """ - ``AerogridLoader`` class, inherited from ``BaseSolver`` + ``AerogridLoader`` class, inherited from ``GridLoader`` Generates aerodynamic grid based on the input data @@ -36,9 +37,9 @@ class AerogridLoader(BaseSolver): 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 + file_name (str): name of the ``.aero.h5`` HDF5 file aero: empty attribute - aero_data_dict (dict): key-value pairs of aerodynamic data + data_dict (dict): key-value pairs of aerodynamic data wake_shape_generator (class): Wake shape generator """ @@ -89,28 +90,13 @@ class AerogridLoader(BaseSolver): settings_options=settings_options) def __init__(self): - self.data = None - self.settings = None - self.aero_file_name = '' - # storage of file contents - self.aero_data_dict = dict() - - # aero storage + super().__init__ + self.file_name = '.aero.h5' self.aero = None - self.wake_shape_generator = None def initialise(self, data, restart=False): - 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, options=self.settings_options) - - # read input file (aero) - self.read_files() + super().initialise(data) wake_shape_generator_type = gen_interface.generator_from_string( self.settings['wake_shape_generator']) @@ -119,25 +105,9 @@ def initialise(self, data, restart=False): self.settings['wake_shape_generator_input'], restart=restart) - 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, **kwargs): self.data.aero = aerogrid.Aerogrid() - self.data.aero.generate(self.aero_data_dict, + self.data.aero.generate(self.data_dict, self.data.structure, self.settings, self.data.ts) diff --git a/sharpy/solvers/dynamiccoupled.py b/sharpy/solvers/dynamiccoupled.py index 0e5950d22..719a58380 100644 --- a/sharpy/solvers/dynamiccoupled.py +++ b/sharpy/solvers/dynamiccoupled.py @@ -178,6 +178,10 @@ class DynamicCoupled(BaseSolver): 'The dictionary values are dictionaries with the settings ' \ 'needed by each generator.' + settings_types['nonlifting_body_interactions'] = 'bool' + settings_default['nonlifting_body_interactions'] = False + settings_description['nonlifting_body_interactions'] = 'Effect of Nonlifting Bodies on Lifting bodies are considered' + settings_table = settings_utils.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description, settings_options) @@ -365,17 +369,20 @@ def initialise(self, data, custom_settings=None, restart=False): 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.remove_old_timestep_info(self.data.structure.timestep_info) + self.remove_old_timestep_info(self.data.aero.timestep_info) + if self.settings['nonlifting_body_interactions']: + self.remove_old_timestep_info(self.data.nonlifting_body.timestep_info) self.data.ts = 0 + def remove_old_timestep_info(self, tstep_info): + # copy last info to first + tstep_info[0] = tstep_info[-1].copy() + # delete all the rest + while len(tstep_info) - 1: + del tstep_info[-1] + def process_controller_output(self, controlled_state): """ This function modified the solver properties and parameters as @@ -508,8 +515,6 @@ def network_loop(self, in_queue, out_queue, finish_event): out_network.close() def time_loop(self, in_queue=None, out_queue=None, finish_event=None, solvers=None): - # import pdb - # pdb.set_trace() self.logger.debug('Inside time loop') # dynamic simulations start at tstep == 1, 0 is reserved for the initial state for self.data.ts in range( @@ -527,6 +532,10 @@ def time_loop(self, in_queue=None, out_queue=None, finish_event=None, solvers=No structural_kstep = self.data.structure.timestep_info[-1].copy() aero_kstep = self.data.aero.timestep_info[-1].copy() + if self.settings['nonlifting_body_interactions']: + nl_body_kstep = self.data.nonlifting_body.timestep_info[-1].copy() + else: + nl_body_kstep = None self.logger.debug('Time step {}'.format(self.data.ts)) # Add the controller here @@ -534,13 +543,10 @@ def time_loop(self, in_queue=None, out_queue=None, finish_event=None, solvers=No state = {'structural': structural_kstep, 'aero': aero_kstep} for k, v in self.controllers.items(): - state, control = v.control(self.data, state) + 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.aero_solver.update_custom_grid(state['structural'], state['aero']) - # import pdb - # pdb.set_trace() # Add external forces if self.with_runtime_generators: @@ -570,16 +576,17 @@ def time_loop(self, in_queue=None, out_queue=None, finish_event=None, solvers=No cout.cout_wrap(("The FSI solver did not converge!!! residuals: %f %f" % (print_res, print_res_dqdt))) self.aero_solver.update_custom_grid( structural_kstep, - aero_kstep) + aero_kstep, + nl_body_kstep) break # generate new grid (already rotated) aero_kstep = controlled_aero_kstep.copy() - # import pdb - # pdb.set_trace() + self.aero_solver.update_custom_grid( - structural_kstep, - aero_kstep) + structural_kstep, + aero_kstep, + nl_body_kstep) # compute unsteady contribution force_coeff = 0.0 @@ -611,7 +618,8 @@ def time_loop(self, in_queue=None, out_queue=None, finish_event=None, solvers=No self.data = self.aero_solver.run(aero_step=aero_kstep, structural_step=structural_kstep, convect_wake=True, - unsteady_contribution=unsteady_contribution) + unsteady_contribution=unsteady_contribution, + nl_body_tstep = nl_body_kstep) self.time_aero += time.perf_counter() - ini_time_aero previous_kstep = structural_kstep.copy() @@ -622,12 +630,15 @@ def time_loop(self, in_queue=None, out_queue=None, finish_event=None, solvers=No previous_kstep.runtime_unsteady_forces = previous_runtime_unsteady_forces.astype(dtype=ct.c_double, order='F', copy=True) # move the aerodynamic surface according the the structural one - self.aero_solver.update_custom_grid(structural_kstep, - aero_kstep) - + self.aero_solver.update_custom_grid( + structural_kstep, + aero_kstep, + nl_body_kstep) + self.map_forces(aero_kstep, - structural_kstep, - force_coeff) + structural_kstep, + nl_body_kstep = nl_body_kstep, + unsteady_forces_coeff = force_coeff) # relaxation relax_factor = self.relaxation_factor(k) @@ -671,16 +682,20 @@ def time_loop(self, in_queue=None, out_queue=None, finish_event=None, solvers=No self.aero_solver, self.with_runtime_generators): # move the aerodynamic surface according to the structural one - self.aero_solver.update_custom_grid( - structural_kstep, - aero_kstep) + self.aero_solver.update_custom_grid(structural_kstep, + aero_kstep, + nl_body_tstep = nl_body_kstep) break # move the aerodynamic surface according the the structural one - self.aero_solver.update_custom_grid(structural_kstep, aero_kstep) + self.aero_solver.update_custom_grid(structural_kstep, + aero_kstep, + nl_body_tstep = nl_body_kstep) self.aero_solver.add_step() self.data.aero.timestep_info[-1] = aero_kstep.copy() + if self.settings['nonlifting_body_interactions']: + self.data.nonlifting_body.timestep_info[-1] = nl_body_kstep.copy() self.structural_solver.add_step() self.data.structure.timestep_info[-1] = structural_kstep.copy() @@ -698,11 +713,9 @@ def time_loop(self, in_queue=None, out_queue=None, finish_event=None, solvers=No structural_kstep.for_vel[2], np.sum(structural_kstep.steady_applied_forces[:, 0]), np.sum(structural_kstep.steady_applied_forces[:, 2])]) - # import pdb - # pdb.set_trace() (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.structural_solver.extract_resultants(self.data.structure.timestep_info[self.data.ts])) + self.structural_solver.extract_resultants(self.data.structure.timestep_info[self.data.ts])) # run postprocessors if self.with_postprocessors: for postproc in self.postprocessors: @@ -757,7 +770,9 @@ def convergence(self, k, tstep, previous_tstep, return False # Check the special case of no aero and no runtime generators - if aero_solver.solver_id.lower() == "noaero" and not with_runtime_generators: + if (aero_solver.solver_id.lower() == "noaero"\ + or struct_solver.solver_id.lower() == "nostructural")\ + and not with_runtime_generators: return True # relative residuals @@ -796,7 +811,7 @@ def convergence(self, k, tstep, previous_tstep, return True - def map_forces(self, aero_kstep, structural_kstep, unsteady_forces_coeff=1.0): + def map_forces(self, aero_kstep, structural_kstep, nl_body_kstep = None, 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) @@ -811,8 +826,8 @@ def map_forces(self, aero_kstep, structural_kstep, unsteady_forces_coeff=1.0): self.data.structure.node_master_elem, self.data.structure.connectivities, structural_kstep.cag(), - self.data.aero.aero_dict) - dynamic_struct_forces = mapping.aero2struct_force_mapping( + self.data.aero.data_dict) + dynamic_struct_forces = unsteady_forces_coeff*mapping.aero2struct_force_mapping( aero_kstep.dynamic_forces, self.data.aero.struct2aero_mapping, aero_kstep.zeta, @@ -821,7 +836,7 @@ def map_forces(self, aero_kstep, structural_kstep, unsteady_forces_coeff=1.0): self.data.structure.node_master_elem, self.data.structure.connectivities, structural_kstep.cag(), - self.data.aero.aero_dict) + self.data.aero.data_dict) if self.correct_forces: struct_forces = \ @@ -834,6 +849,18 @@ def map_forces(self, aero_kstep, structural_kstep, unsteady_forces_coeff=1.0): structural_kstep.postproc_node['aero_steady_forces'] = struct_forces structural_kstep.postproc_node['aero_unsteady_forces'] = dynamic_struct_forces + # if self.settings['nonlifting_body_interactions']: + # struct_forces += mapping.aero2struct_force_mapping( + # nl_body_kstep.forces, + # self.data.nonlifting_body.struct2aero_mapping, + # nl_body_kstep.zeta, + # structural_kstep.pos, + # structural_kstep.psi, + # self.data.structure.node_master_elem, + # self.data.structure.connectivities, + # structural_kstep.cag(), + # self.data.nonlifting_body.data_dict) + # prescribed forces + aero forces # prescribed forces + aero forces + runtime generated structural_kstep.steady_applied_forces += struct_forces structural_kstep.steady_applied_forces += self.data.structure.ini_info.steady_applied_forces @@ -859,296 +886,6 @@ def relaxation_factor(self, k): value = initial + (final - initial)/self.settings['relaxation_steps']*k return value - def change_trim(self, 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 - - - 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_grid(self.data.structure) - - def time_step(self, in_queue=None, out_queue=None, finish_event=None, solvers=None): - # import pdb - # pdb.set_trace() - self.logger.debug('Inside time step') - # dynamic simulations start at tstep == 1, 0 is reserved for the initial state - self.data.ts = len(self.data.structure.timestep_info) - initial_time = time.perf_counter() - - # network only - # get input from the other thread - if in_queue: - self.logger.info('Time Loop - Waiting for input') - values = in_queue.get() # should be list of tuples - self.logger.debug('Time loop - received {}'.format(values)) - self.set_of_variables.update_timestep(self.data, values) - - structural_kstep = self.data.structure.timestep_info[-1].copy() - aero_kstep = self.data.aero.timestep_info[-1].copy() - self.logger.debug('Time step {}'.format(self.data.ts)) - - # Add the controller here - if self.with_controllers: - control = [] - state = {'structural': structural_kstep, - 'aero': aero_kstep} - for k, v in self.controllers.items(): - state, control_command = 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) - control.append(control_command) - self.aero_solver.update_custom_grid(state['structural'], state['aero']) - # import pdb - # pdb.set_trace() - - # Add external forces - if self.with_runtime_generators: - structural_kstep.runtime_steady_forces.fill(0.) - structural_kstep.runtime_unsteady_forces.fill(0.) - params = dict() - params['data'] = self.data - params['struct_tstep'] = structural_kstep - params['aero_tstep'] = aero_kstep - params['fsi_substep'] = -1 - for id, runtime_generator in self.runtime_generators.items(): - runtime_generator.generate(params) - - 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() - - for k in range(self.settings['fsi_substeps'] + 1): - if (k == self.settings['fsi_substeps'] and - self.settings['fsi_substeps']): - print_res = 0 if self.res == 0. else np.log10(self.res) - print_res_dqdt = 0 if self.res_dqdt == 0. else np.log10(self.res_dqdt) - cout.cout_wrap(("The FSI solver did not converge!!! residuals: %f %f" % (print_res, print_res_dqdt))) - self.aero_solver.update_custom_grid( - structural_kstep, - aero_kstep) - 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']: - if self.data.ts > self.settings['steps_without_unsteady_force']: - unsteady_contribution = True - if k < self.settings['pseudosteps_ramp_unsteady_force']: - force_coeff = k/self.settings['pseudosteps_ramp_unsteady_force'] - else: - force_coeff = 1. - - previous_runtime_steady_forces = structural_kstep.runtime_steady_forces.astype(dtype=ct.c_double, order='F', copy=True) - previous_runtime_unsteady_forces = structural_kstep.runtime_unsteady_forces.astype(dtype=ct.c_double, order='F', copy=True) - # Add external forces - if self.with_runtime_generators: - structural_kstep.runtime_steady_forces.fill(0.) - structural_kstep.runtime_unsteady_forces.fill(0.) - params = dict() - params['data'] = self.data - params['struct_tstep'] = structural_kstep - params['aero_tstep'] = aero_kstep - params['fsi_substep'] = k - for id, runtime_generator in self.runtime_generators.items(): - runtime_generator.generate(params) - - # run the solver - ini_time_aero = time.perf_counter() - self.data = self.aero_solver.run(aero_step=aero_kstep, - structural_step=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() - structural_kstep.runtime_steady_forces = previous_kstep.runtime_steady_forces.astype(dtype=ct.c_double, order='F', copy=True) - structural_kstep.runtime_unsteady_forces = previous_kstep.runtime_unsteady_forces.astype(dtype=ct.c_double, order='F', copy=True) - previous_kstep.runtime_steady_forces = previous_runtime_steady_forces.astype(dtype=ct.c_double, order='F', copy=True) - previous_kstep.runtime_unsteady_forces = previous_runtime_unsteady_forces.astype(dtype=ct.c_double, order='F', copy=True) - - # 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'] + 1): - # run structural solver - coeff = ((i_substep + 1)/ - (self.settings['structural_substeps'] + 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.aero_solver.update_custom_grid( - # structural_kstep, - # aero_kstep) - self.time_struc += time.perf_counter() - ini_time_struc - - # check convergence - if self.convergence(k, - structural_kstep, - previous_kstep, - self.structural_solver, - self.aero_solver, - self.with_runtime_generators): - # 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: - print_res = 0 if self.res_dqdt == 0. else np.log10(self.res_dqdt) - self.residual_table.print_line([self.data.ts, - self.data.ts*self.dt, - k, - self.time_struc/(self.time_aero + self.time_struc), - final_time - initial_time, - print_res, - 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.data.structure.timestep_info[self.data.ts].total_forces[0:3], - self.data.structure.timestep_info[self.data.ts].total_forces[3:6]) = ( - self.structural_solver.extract_resultants(self.data.structure.timestep_info[self.data.ts])) - # run postprocessors - if self.with_postprocessors: - for postproc in self.postprocessors: - self.data = self.postprocessors[postproc].run(online=True, solvers=solvers) - - # network only - # put result back in queue - if out_queue: - self.logger.debug('Time loop - about to get out variables from data') - self.set_of_variables.get_value(self.data) - if out_queue.full(): - # clear the queue such that it always contains the latest time step - out_queue.get() # clear item from queue - self.logger.debug('Data output Queue is full - clearing output') - out_queue.put(self.set_of_variables) - - if finish_event: - finish_event.set() - self.logger.info('Time loop - Complete') - - return control - - def extract_resultants(self, tstep=None): - return self.structural_solver.extract_resultants(tstep) - - # def extract_controlcommand(self, tstep=None): - # if self.with_controllers: - # structural_kstep = self.data.structure.timestep_info[-1].copy() - # aero_kstep = self.data.aero.timestep_info[-1].copy() - # state = {'structural': structural_kstep, - # 'aero': aero_kstep} - # control = [] - # for k, v in self.controllers.items(): - # state, control_command = v.control(self.data, state) - # control.append(control_command) - # return control - - def get_direction(self, thrust_nodes, tstep=None): - self.force_orientation_G = np.zeros((len(thrust_nodes), 3)) - for i_node, node in enumerate(thrust_nodes): - # import pdb - # pdb.set_trace() - elem_thrust_node = np.where(self.data.structure.connectivities == 0)[0][0] - node_thrust_node = np.where(self.data.structure.connectivities == 0)[1][0] - self.force_orientation_G[i_node, :] = np.dot(algebra.quat2rotation(self.data.structure.ini_info.quat), - np.dot(algebra.crv2rotation(self.data.structure.ini_info.psi[elem_thrust_node, node_thrust_node, :]), - algebra.unit_vector(self.data.structure.ini_info.steady_applied_forces[node, 0:3]))) - return self.force_orientation_G - - @staticmethod def interpolate_timesteps(step0, step1, out_step, coeff): """ diff --git a/sharpy/solvers/dynamictrim.py b/sharpy/solvers/dynamictrim.py deleted file mode 100644 index 0f85c3b28..000000000 --- a/sharpy/solvers/dynamictrim.py +++ /dev/null @@ -1,732 +0,0 @@ -import numpy as np - -import sharpy.utils.cout_utils as cout -import sharpy.utils.solver_interface as solver_interface -from sharpy.utils.solver_interface import solver, BaseSolver -import sharpy.utils.settings as settings_utils -import os - - -@solver -class DynamicTrim(BaseSolver): - """ - The ``StaticTrim`` solver determines the longitudinal state of trim (equilibrium) for an aeroelastic system in - static conditions. It wraps around the desired solver to yield the state of trim of the system, in most cases - the :class:`~sharpy.solvers.staticcoupled.StaticCoupled` solver. - - It calculates the required angle of attack, elevator deflection and thrust required to achieve longitudinal - equilibrium. The output angles are shown in degrees. - - The results from the trimming iteration can be saved to a text file by using the `save_info` option. - """ - solver_id = 'DynamicTrim' - solver_classification = 'Flight Dynamics' - - settings_types = dict() - settings_default = dict() - settings_description = dict() - - settings_types['print_info'] = 'bool' - settings_default['print_info'] = True - settings_description['print_info'] = 'Print info to screen' - - settings_types['solver'] = 'str' - settings_default['solver'] = '' - settings_description['solver'] = 'Solver to run in trim routine' - - settings_types['solver_settings'] = 'dict' - settings_default['solver_settings'] = dict() - settings_description['solver_settings'] = 'Solver settings dictionary' - - settings_types['max_iter'] = 'int' - settings_default['max_iter'] = 40000 - settings_description['max_iter'] = 'Maximum number of iterations of trim routine' - - settings_types['fz_tolerance'] = 'float' - settings_default['fz_tolerance'] = 0.01 - settings_description['fz_tolerance'] = 'Tolerance in vertical force' - - settings_types['fx_tolerance'] = 'float' - settings_default['fx_tolerance'] = 0.01 - settings_description['fx_tolerance'] = 'Tolerance in horizontal force' - - settings_types['m_tolerance'] = 'float' - settings_default['m_tolerance'] = 0.01 - settings_description['m_tolerance'] = 'Tolerance in pitching moment' - - settings_types['tail_cs_index'] = ['int', 'list(int)'] - settings_default['tail_cs_index'] = 0 - settings_description['tail_cs_index'] = 'Index of control surfaces that move to achieve trim' - - settings_types['thrust_nodes'] = 'list(int)' - settings_default['thrust_nodes'] = [0] - settings_description['thrust_nodes'] = 'Nodes at which thrust is applied' - - settings_types['initial_alpha'] = 'float' - settings_default['initial_alpha'] = 0. - settings_description['initial_alpha'] = 'Initial angle of attack' - - settings_types['initial_deflection'] = 'float' - settings_default['initial_deflection'] = 0. - settings_description['initial_deflection'] = 'Initial control surface deflection' - - settings_types['initial_thrust'] = 'float' - settings_default['initial_thrust'] = 0.0 - settings_description['initial_thrust'] = 'Initial thrust setting' - - settings_types['initial_angle_eps'] = 'float' - settings_default['initial_angle_eps'] = 0.05 - settings_description['initial_angle_eps'] = 'Initial change of control surface deflection' - - settings_types['initial_thrust_eps'] = 'float' - settings_default['initial_thrust_eps'] = 2. - settings_description['initial_thrust_eps'] = 'Initial thrust setting change' - - settings_types['relaxation_factor'] = 'float' - settings_default['relaxation_factor'] = 0.2 - settings_description['relaxation_factor'] = 'Relaxation factor' - - settings_types['notrim_relax'] = 'bool' - settings_default['notrim_relax'] = False - settings_description['notrim_relax'] = 'Disable gains for trim - releases internal loads at initial values' - - settings_types['notrim_relax_iter'] = 'int' - settings_default['notrim_relax_iter'] = 10000000 - settings_description['notrim_relax_iter'] = 'Terminate notrim_relax at defined number of steps' - - - settings_types['speed_up_factor'] = 'float' - settings_default['speed_up_factor'] = 1.0 - settings_description['speed_up_factor'] = 'increase dt in trim iterations' - - settings_types['save_info'] = 'bool' - settings_default['save_info'] = False - settings_description['save_info'] = 'Save trim results to text file' - - 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.solver = None - - # The order is - # [0]: alpha/fz - # [1]: alpha + delta (gamma)/moment - # [2]: thrust/fx - - self.n_input = 3 - self.i_iter = 0 - - self.input_history = [] - self.output_history = [] - self.gradient_history = [] - self.trimmed_values = np.zeros((3,)) - - self.table = None - self.folder = None - - def initialise(self, data, restart=False): - self.data = data - self.settings = data.settings[self.solver_id] - settings_utils.to_custom_types(self.settings, self.settings_types, self.settings_default) - - self.solver = solver_interface.initialise_solver(self.settings['solver']) - - if self.settings['solver_settings']['structural_solver'] == "NonLinearDynamicCoupledStep": - #replace free flying with clamped - oldsettings = self.settings['solver_settings'] - self.settings['solver_settings']['structural_solver'] = 'NonLinearDynamicPrescribedStep' - # self.settings['solver_settings']['structural_solver_settings'] = {'print_info': 'off', - # 'max_iterations': 950, - # 'delta_curved': 1e-1, - # 'min_delta': tolerance, - # 'newmark_damp': 5e-3, - # 'gravity_on': gravity, - # 'gravity': 9.81, - # 'num_steps': n_tstep, - # 'dt': dt, - # 'initial_velocity': u_inf * int(free_flight)} commented since both solvers take (almost) same inputs - u_inf = self.settings['solver_settings']['structural_solver_settings']['initial_velocity'] - self.settings['solver_settings']['structural_solver_settings'].pop('initial_velocity') - self.settings['solver_settings']['structural_solver_settings']['newmark_damp'] = 1.0 - # settings['StepUvlm'] = {'print_info': 'off', - # 'num_cores': num_cores, - # 'convection_scheme': 2, - # 'gamma_dot_filtering': 6, - # 'velocity_field_generator': 'GustVelocityField', - # 'velocity_field_input': {'u_inf': int(not free_flight) * u_inf, - # 'u_inf_direction': [1., 0, 0], - # 'gust_shape': '1-cos', - # 'gust_parameters': {'gust_length': gust_length, - # 'gust_intensity': gust_intensity * u_inf}, - # 'offset': gust_offset, - # 'relative_motion': relative_motion}, - # 'rho': rho, - # 'n_time_steps': n_tstep, - # 'dt': dt} - self.settings['solver_settings']['aero_solver_settings']['velocity_field_generator'] = 'SteadyVelocityField' - u_inf_direction = self.settings['solver_settings']['aero_solver_settings']['velocity_field_input']['u_inf_direction'] - self.settings['solver_settings']['aero_solver_settings']['velocity_field_input'].clear() - self.settings['solver_settings']['aero_solver_settings']['velocity_field_input']['u_inf'] = u_inf - self.settings['solver_settings']['aero_solver_settings']['velocity_field_input']['u_inf_direction'] = u_inf_direction - - self.settings['solver_settings'] - # TODO: plus dynamic coupled to add controller - - # import pdb - # pdb.set_trace() - - gains_elev = -np.array([0.00015, 0.00015, 0.0]) #we can pick them! dimensional force/moment versus radians - gains_alpha = np.array([0.00015, 0.00015, 0.0]) - gains_thrust = -np.array([0.1, 0.05, 0.0]) #we can pick them too! this is 1-1 almost - route = data.settings['SHARPy']['route'] - n_tstep = int(self.settings['solver_settings']['n_time_steps']) - dt = float(self.settings['solver_settings']['dt']) - elev_file = route + '/elev.csv' - alpha_file = route + '/alpha.csv' - thrust_file = route + '/thrust.csv' - - elev_hist = np.linspace(0, n_tstep*dt, n_tstep) - elev_hist = 0.0/180.0*np.pi*elev_hist - - alpha_hist = np.linspace(1, 1, n_tstep) - alpha_hist = 0.0/180.0*np.pi*alpha_hist - - thrust_hist = np.linspace(1, 1, n_tstep) - thrust_hist = 0.0/180.0*np.pi*thrust_hist - - np.savetxt(elev_file, elev_hist) - np.savetxt(alpha_file, alpha_hist) - np.savetxt(thrust_file, thrust_hist) - - - try: - self.settings['solver_settings']['controller_id'].clear() - self.settings['solver_settings']['controller_settings'].clear() - except: - print("original no controller") - - self.settings['solver_settings']['controller_id']= {'controller_elevator': 'ControlSurfacePidController', - 'controller_alpha': 'AlphaController', - 'controller_thrust': 'ThrustController'} - self.settings['solver_settings']['controller_settings']= {'controller_elevator': {'P': gains_elev[0], - 'I': gains_elev[1], - 'D': gains_elev[2], - 'dt': dt, - 'input_type': 'm_y', - 'write_controller_log': True, - 'controlled_surfaces': [0], - 'time_history_input_file': route + '/elev.csv', - 'use_initial_value': True, - 'initial_value': self.settings['initial_deflection'] - } - , - 'controller_alpha': {'P': gains_alpha[0], - 'I': gains_alpha[1], - 'D': gains_alpha[2], - 'dt': dt, - 'input_type': 'f_z', - 'write_controller_log': True, - 'time_history_input_file': route + '/alpha.csv', - 'use_initial_value': True, - 'initial_value': self.settings['initial_alpha']} - , - 'controller_thrust': {'thrust_nodes': self.settings['thrust_nodes'], - 'P': gains_thrust[0], - 'I': gains_thrust[1], - 'D': gains_thrust[2], - 'dt': dt, - 'input_type': 'f_x', - 'write_controller_log': True, - 'time_history_input_file': route + '/thrust.csv', - 'use_initial_value': True, - 'initial_value': self.settings['initial_thrust']} - } - # import pdb - # pdb.set_trace() - # #end - - self.solver.initialise(self.data, self.settings['solver_settings'], restart=restart) - - self.folder = data.output_folder + '/statictrim/' - if not os.path.exists(self.folder): - os.makedirs(self.folder) - - self.table = cout.TablePrinter(10, 8, ['g', 'f', 'f', 'f', 'f', 'f', 'f', 'f', 'f', 'f'], - filename=self.folder+'trim_iterations.txt') - self.table.print_header(['iter', 'alpha[deg]', 'elev[deg]', 'thrust', 'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz']) - - - elif self.settings['solver_settings']['structural_solver'] == "NonLinearDynamicMultibody": - # import pdb - # pdb.set_trace() - self.data.structure.ini_mb_dict['body_00']['FoR_movement'] = 'prescribed_trim' - self.data.structure.timestep_info[0].mb_dict['body_00']['FoR_movement'] = 'prescribed_trim' - self.data.structure.ini_info.mb_dict['body_00']['FoR_movement'] = 'prescribed_trim' - # self.data.structure.ini_mb_dict['body_00']['FoR_movement'] = 'prescribed_trim' - # self.data.structure.timestep_info[0].mb_dict['body_00']['FoR_movement'] = 'prescribed_trim' - # self.data.structure.ini_info.mb_dict['body_00']['FoR_movement'] = 'prescribed_trim' - #replace free flying with clamped - oldsettings = self.settings['solver_settings'] - self.settings['solver_settings']['structural_solver'] = 'NonLinearDynamicMultibody' - # self.settings['solver_settings']['structural_solver_settings'] = {'print_info': 'off', - # 'max_iterations': 950, - # 'delta_curved': 1e-1, - # 'min_delta': tolerance, - # 'newmark_damp': 5e-3, - # 'gravity_on': gravity, - # 'gravity': 9.81, - # 'num_steps': n_tstep, - # 'dt': dt, - # 'initial_velocity': u_inf * int(free_flight)} commented since both solvers take (almost) same inputs - self.settings['solver_settings']['structural_solver_settings'].pop('dyn_trim') - u_inf = self.settings['solver_settings']['structural_solver_settings']['initial_velocity'] - self.settings['solver_settings']['structural_solver_settings'].pop('initial_velocity') - # import pdb - # pdb.set_trace() - dt = self.settings['solver_settings']['structural_solver_settings']['time_integrator_settings']['dt'] - # self.settings['solver_settings']['structural_solver_settings']['time_integrator_settings']['dt'] = float(dt)/5. - self.settings['solver_settings']['structural_solver_settings']['time_integrator_settings']['newmark_damp'] = 0.1 - # settings['StepUvlm'] = {'print_info': 'off', - # 'num_cores': num_cores, - # 'convection_scheme': 2, - # 'gamma_dot_filtering': 6, - # 'velocity_field_generator': 'GustVelocityField', - # 'velocity_field_input': {'u_inf': int(not free_flight) * u_inf, - # 'u_inf_direction': [1., 0, 0], - # 'gust_shape': '1-cos', - # 'gust_parameters': {'gust_length': gust_length, - # 'gust_intensity': gust_intensity * u_inf}, - # 'offset': gust_offset, - # 'relative_motion': relative_motion}, - # 'rho': rho, - # 'n_time_steps': n_tstep, - # 'dt': dt} - # self.settings['solver_settings']['aero_solver_settings']['convection_scheme'] = 2 - - self.settings['solver_settings']['aero_solver_settings']['velocity_field_generator'] = 'SteadyVelocityField' - u_inf_direction = self.settings['solver_settings']['aero_solver_settings']['velocity_field_input']['u_inf_direction'] - self.settings['solver_settings']['aero_solver_settings']['velocity_field_input'].clear() - self.settings['solver_settings']['aero_solver_settings']['velocity_field_input']['u_inf'] = u_inf - self.settings['solver_settings']['aero_solver_settings']['velocity_field_input']['u_inf_direction'] = u_inf_direction - - self.settings['solver_settings'] - # TODO: plus dynamic coupled to add controller - - # import pdb - # pdb.set_trace() - - # gains_elev = -np.array([0.000015, 0.000015, 0.0]) #we can pick them! dimensional force/moment versus radians - # gains_alpha = np.array([0.000010, 0.000010, 0.0]) - # # gains_elev = -np.array([0.00015, 0.00015, 0.0]) #we can pick them! dimensional force/moment versus radians - # # gains_alpha = np.array([0.00010, 0.00010, 0.0]) - # gains_thrust = -np.array([0.1, 0.05, 0.0]) #we can pick them too! this is 1-1 almost - gains_elev = (not self.settings['notrim_relax'])*-np.array([0.000015, 0.000010, 0.0]) #we can pick them! dimensional force/moment versus radians - gains_alpha = (not self.settings['notrim_relax'])*np.array([0.000015, 0.000010, 0.0]) - # gains_elev = -np.array([0.00015, 0.00015, 0.0]) #we can pick them! dimensional force/moment versus radians - # gains_alpha = np.array([0.00010, 0.00010, 0.0]) - gains_thrust = (not self.settings['notrim_relax'])*-np.array([0.1, 0.05, 0.0]) - - - - #we can pick them too! this is 1-1 almost - route = data.settings['SHARPy']['route'] - n_tstep = int(self.settings['solver_settings']['n_time_steps']) - n_tstep *=40 - self.settings['solver_settings']['n_time_steps'] = n_tstep - self.settings['solver_settings']['structural_solver_settings']['num_steps'] = n_tstep - self.settings['solver_settings']['aero_solver_settings']['n_time_steps'] = n_tstep - # import pdb - # pdb.set_trace() - - dt = float(self.settings['solver_settings']['dt'])*self.settings['speed_up_factor'] - # import pdb - # pdb.set_trace() - self.settings['solver_settings']['dt'] = dt - self.settings['solver_settings']['structural_solver_settings']['time_integrator_settings']['dt'] = dt - self.settings['solver_settings']['aero_solver_settings']['dt'] = dt - elev_file = route + '/elev.csv' - alpha_file = route + '/alpha.csv' - thrust_file = route + '/thrust.csv' - - elev_hist = np.linspace(0, n_tstep*dt, n_tstep) - elev_hist = 0.0/180.0*np.pi*elev_hist - - alpha_hist = np.linspace(1, 1, n_tstep) - alpha_hist = 0.0/180.0*np.pi*alpha_hist - - thrust_hist = np.linspace(1, 1, n_tstep) - thrust_hist = 0.0/180.0*np.pi*thrust_hist - - np.savetxt(elev_file, elev_hist) - np.savetxt(alpha_file, alpha_hist) - np.savetxt(thrust_file, thrust_hist) - - try: - self.settings['solver_settings']['controller_id'].clear() - self.settings['solver_settings']['controller_settings'].clear() - except: - print("original no controller") - - self.settings['solver_settings']['controller_id']= {'controller_elevator': 'ControlSurfacePidController' - , - 'controller_alpha': 'AlphaController' - , - 'controller_thrust': 'ThrustController' - } - self.settings['solver_settings']['controller_settings']= {'controller_elevator': {'P': gains_elev[0], - 'I': gains_elev[1], - 'D': gains_elev[2], - 'dt': dt, - 'input_type': 'm_y', - 'write_controller_log': True, - 'controlled_surfaces': [0], - 'time_history_input_file': route + '/elev.csv', - 'use_initial_value': True, - 'initial_value': self.settings['initial_deflection'] - } - , - 'controller_alpha': {'P': gains_alpha[0], - 'I': gains_alpha[1], - 'D': gains_alpha[2], - 'dt': dt, - 'input_type': 'f_z', - 'write_controller_log': True, - 'time_history_input_file': route + '/alpha.csv', - 'use_initial_value': True, - 'initial_value': self.settings['initial_alpha']} - , - 'controller_thrust': {'thrust_nodes': self.settings['thrust_nodes'], - 'P': gains_thrust[0], - 'I': gains_thrust[1], - 'D': gains_thrust[2], - 'dt': dt, - 'input_type': 'f_x', - 'write_controller_log': True, - 'time_history_input_file': route + '/thrust.csv', - 'use_initial_value': True, - 'initial_value': self.settings['initial_thrust']} - } - # import pdb - # pdb.set_trace() - # #end - - self.solver.initialise(self.data, self.settings['solver_settings'], restart=restart) - - self.folder = data.output_folder + '/statictrim/' - if not os.path.exists(self.folder): - os.makedirs(self.folder) - - self.table = cout.TablePrinter(10, 8, ['g', 'f', 'f', 'f', 'f', 'f', 'f', 'f', 'f', 'f'], - filename=self.folder+'trim_iterations.txt') - self.table.print_header(['iter', 'alpha[deg]', 'elev[deg]', 'thrust', 'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz']) - else: - raise NotImplementedError('Dynamic trim is only working with nonlinearcoupled or multibody!') - - - def undo_changes(self, data): - - # import pdb - # pdb.set_trace() - if self.settings['solver_settings']['structural_solver'] == "NonLinearDynamicMultibody": - data.structure.ini_mb_dict['body_00']['FoR_movement'] = 'free' - data.structure.timestep_info[-1].mb_dict['body_00']['FoR_movement'] = 'free' - data.structure.ini_info.mb_dict['body_00']['FoR_movement'] = 'free' - print("HARDCODED!! 16723") - - # data.structure.ini_info.pos_dot *= 0. - # data.structure.ini_info.pos_ddot *= 0. - # data.structure.ini_info.psi_dot *= 0. - # data.structure.ini_info.psi_dot_local *= 0. - # data.structure.ini_info.psi_ddot *= 0. - # data.structure.timestep_info[-1].pos_dot *= 0. - # data.structure.timestep_info[-1].pos_ddot *= 0. - # data.structure.timestep_info[-1].psi_dot *= 0. - # data.structure.timestep_info[-1].psi_dot_local *= 0. - # data.structure.timestep_info[-1].psi_ddot *= 0. - # data.structure.timestep_info[-1].mb_FoR_vel *= 0. - # data.structure.timestep_info[-1].mb_FoR_acc *= 0. - # data.structure.timestep_info[-1].mb_dquatdt *= 0. - # data.structure.timestep_info[-1].dqddt *= 0. - # data.structure.timestep_info[-1].forces_constraints_FoR *= 0. - # data.structure.timestep_info[-1].forces_constraints_nodes *= 0. - - # data.structure.timestep_info[-1].dqdt *= 0. - # data.structure.timestep_info[-1].q *= 0. - # data.structure.timestep_info[-1].steady_applied_forces *= 0. - # data.structure.timestep_info[-1].unsteady_applied_forces *= 0. - - # import pdb - # pdb.set_trace() - # data.structure.timestep_info[0].q = np.append(data.structure.timestep_info[0].q, np.zeros(10)) - # data.structure.timestep_info[0].dqdt = np.append(data.structure.timestep_info[0].dqdt, np.zeros(10)) - # data.structure.timestep_info[0].dqddt = np.append(data.structure.timestep_info[0].dqddt, np.zeros(10)) - - - def increase_ts(self): - self.data.ts += 1 - self.structural_solver.next_step() - self.aero_solver.next_step() - - def run(self, **kwargs): - - # In the event the modal solver has been run prior to StaticCoupled (i.e. to get undeformed modes), copy - # results and then attach to the resulting timestep - try: - modal = self.data.structure.timestep_info[-1].modal.copy() - modal_exists = True - except AttributeError: - modal_exists = False - - self.trim_algorithm() - - if modal_exists: - self.data.structure.timestep_info[-1].modal = modal - - if self.settings['save_info']: - np.savetxt(self.folder + '/trim_values.txt', self.trimmed_values) - - # save trimmed values for dynamic coupled multibody access if needed - self.data.trimmed_values = self.trimmed_values - self.undo_changes(self.data) - - return self.data - - def convergence(self, fz, m, fx, thrust): - return_value = np.array([False, False, False]) - - if np.abs(fz) < self.settings['fz_tolerance']: - return_value[0] = True - - if np.abs(m) < self.settings['m_tolerance']: - return_value[1] = True - - # print(fx) - # print(thrust) - if np.abs(fx) < self.settings['fx_tolerance']: - return_value[2] = True - - return return_value - - def trim_algorithm(self): - """ - Trim algorithm method - - The trim condition is found iteratively. - - Returns: - np.array: array of trim values for angle of attack, control surface deflection and thrust. - """ - for self.i_iter in range(self.settings['max_iter'] + 1): - if self.i_iter == self.settings['max_iter']: - raise Exception('The Trim routine reached max iterations without convergence!') - - self.input_history.append([]) - self.output_history.append([]) - # self.gradient_history.append([]) - for i in range(self.n_input): - self.input_history[self.i_iter].append(0) - self.output_history[self.i_iter].append(0) - # self.gradient_history[self.i_iter].append(0) - - # the first iteration requires computing gradients - if not self.i_iter: - # add to input history the initial estimation - self.input_history[self.i_iter][0] = self.settings['initial_alpha'] - self.input_history[self.i_iter][1] = (self.settings['initial_deflection'] + - self.settings['initial_alpha']) - self.input_history[self.i_iter][2] = self.settings['initial_thrust'] - - # compute output - (self.output_history[self.i_iter][0], - self.output_history[self.i_iter][1], - self.output_history[self.i_iter][2]) = self.evaluate(self.input_history[self.i_iter][0], - self.input_history[self.i_iter][1], - self.input_history[self.i_iter][2]) - - # # do not check for convergence in first step to let transient take effect! - # # check for convergence (in case initial values are ok) - # if all(self.convergence(self.output_history[self.i_iter][0], - # self.output_history[self.i_iter][1], - # self.output_history[self.i_iter][2], - # self.input_history[self.i_iter][2])): - # self.trimmed_values = self.input_history[self.i_iter] - # return - - # # compute gradients - # # dfz/dalpha - # (l, m, d) = self.evaluate(self.input_history[self.i_iter][0] + self.settings['initial_angle_eps'], - # self.input_history[self.i_iter][1], - # self.input_history[self.i_iter][2]) - - # self.gradient_history[self.i_iter][0] = ((l - self.output_history[self.i_iter][0]) / - # self.settings['initial_angle_eps']) - - # # dm/dgamma - # (l, m, d) = self.evaluate(self.input_history[self.i_iter][0], - # self.input_history[self.i_iter][1] + self.settings['initial_angle_eps'], - # self.input_history[self.i_iter][2]) - - # self.gradient_history[self.i_iter][1] = ((m - self.output_history[self.i_iter][1]) / - # self.settings['initial_angle_eps']) - - # # dfx/dthrust - # (l, m, d) = self.evaluate(self.input_history[self.i_iter][0], - # self.input_history[self.i_iter][1], - # self.input_history[self.i_iter][2] + - # self.settings['initial_thrust_eps']) - - # self.gradient_history[self.i_iter][2] = ((d - self.output_history[self.i_iter][2]) / - # self.settings['initial_thrust_eps']) - - continue - - # if not all(np.isfinite(self.gradient_history[self.i_iter - 1])) - # now back to normal evaluation (not only the i_iter == 0 case) - # compute next alpha with the previous gradient - # convergence = self.convergence(self.output_history[self.i_iter - 1][0], - # self.output_history[self.i_iter - 1][1], - # self.output_history[self.i_iter - 1][2]) - convergence = np.full((3, ), False) - if convergence[0]: - # fz is converged, don't change it - self.input_history[self.i_iter][0] = self.input_history[self.i_iter - 1][0] - # self.gradient_history[self.i_iter][0] = self.gradient_history[self.i_iter - 1][0] - else: - self.input_history[self.i_iter][0] = self.input_history[self.i_iter - 1][0] - - if convergence[1]: - # m is converged, don't change it - self.input_history[self.i_iter][1] = self.input_history[self.i_iter - 1][1] - # self.gradient_history[self.i_iter][1] = self.gradient_history[self.i_iter - 1][1] - else: - # compute next gamma with the previous gradient - self.input_history[self.i_iter][1] = self.input_history[self.i_iter - 1][1] - - if convergence[2]: - # fx is converged, don't change it - self.input_history[self.i_iter][2] = self.input_history[self.i_iter - 1][2] - # self.gradient_history[self.i_iter][2] = self.gradient_history[self.i_iter - 1][2] - else: - # compute next gamma with the previous gradient - self.input_history[self.i_iter][2] = self.input_history[self.i_iter - 1][2] - # else: - # if convergence[0] and convergence[1]: - - # # compute next gamma with the previous gradient - # self.input_history[self.i_iter][2] = self.balance_thrust(self.output_history[self.i_iter - 1][2]) - # else: - # self.input_history[self.i_iter][2] = self.input_history[self.i_iter - 1][2] - - if self.settings['relaxation_factor']: - for i_dim in range(3): - self.input_history[self.i_iter][i_dim] = (self.input_history[self.i_iter][i_dim]*(1 - self.settings['relaxation_factor']) + - self.input_history[self.i_iter][i_dim]*self.settings['relaxation_factor']) - - # evaluate - (self.output_history[self.i_iter][0], - self.output_history[self.i_iter][1], - self.output_history[self.i_iter][2]) = self.evaluate(self.input_history[self.i_iter][0], - self.input_history[self.i_iter][1], - self.input_history[self.i_iter][2]) - - if not convergence[0]: - # self.gradient_history[self.i_iter][0] = ((self.output_history[self.i_iter][0] - - # self.output_history[self.i_iter - 1][0]) / - # (self.input_history[self.i_iter][0] - - # self.input_history[self.i_iter - 1][0])) - pass - - if not convergence[1]: - # self.gradient_history[self.i_iter][1] = ((self.output_history[self.i_iter][1] - - # self.output_history[self.i_iter - 1][1]) / - # (self.input_history[self.i_iter][1] - - # self.input_history[self.i_iter - 1][1])) - pass - - if not convergence[2]: - # self.gradient_history[self.i_iter][2] = ((self.output_history[self.i_iter][2] - - # self.output_history[self.i_iter - 1][2]) / - # (self.input_history[self.i_iter][2] - - # self.input_history[self.i_iter - 1][2])) - pass - - # check convergence - convergence = self.convergence(self.output_history[self.i_iter][0], - self.output_history[self.i_iter][1], - self.output_history[self.i_iter][2], - self.input_history[self.i_iter][2]) - # print(convergence) - if all(convergence) or ((self.settings['notrim_relax']) or (self.i_iter > self.settings['notrim_relax_iter'])): - self.trimmed_values = self.input_history[self.i_iter] - self.table.close_file() - return - - # def balance_thrust(self, drag): - # thrust_nodes = self.settings['thrust_nodes'] - # thrust_nodes_num = len(thrust_nodes) - # orientation = self.solver.get_direction(thrust_nodes) - # thrust = -drag/np.sum(orientation, axis=0)[0] - # return thrust - - - def evaluate(self, alpha, deflection_gamma, thrust): - if not np.isfinite(alpha): - import pdb; pdb.set_trace() - if not np.isfinite(deflection_gamma): - import pdb; pdb.set_trace() - if not np.isfinite(thrust): - import pdb; pdb.set_trace() - - # cout.cout_wrap('--', 2) - # cout.cout_wrap('Trying trim: ', 2) - # cout.cout_wrap('Alpha: ' + str(alpha*180/np.pi), 2) - # cout.cout_wrap('CS deflection: ' + str((deflection_gamma - alpha)*180/np.pi), 2) - # cout.cout_wrap('Thrust: ' + str(thrust), 2) - # modify the trim in the static_coupled solver - # self.solver.change_trim(thrust, - # self.settings['thrust_nodes'], - # deflection_gamma - alpha, - # self.settings['tail_cs_index']) - # run the solver - # import pdb - # pdb.set_trace() - control = self.solver.time_step() - # extract resultants - forces, moments = self.solver.extract_resultants() - # extract controller inputs - # control = self.solver.extract_controlcommand() - alpha = control[1] - self.input_history[self.i_iter][0] = alpha - - deflection_gamma = control[0] - self.input_history[self.i_iter][1] = deflection_gamma - - thrust = control[2] - self.input_history[self.i_iter][2] = thrust - - # deflection_gamma = control[0] - # thrust = control[1] - - forcez = forces[2] - forcex = forces[0] - moment = moments[1] - # cout.cout_wrap('Forces and moments:', 2) - # cout.cout_wrap('fx = ' + str(forces[0]) + ' mx = ' + str(moments[0]), 2) - # cout.cout_wrap('fy = ' + str(forces[1]) + ' my = ' + str(moments[1]), 2) - # cout.cout_wrap('fz = ' + str(forces[2]) + ' mz = ' + str(moments[2]), 2) - - self.table.print_line([self.i_iter, - alpha*180/np.pi, - (deflection_gamma)*180/np.pi, - thrust, - forces[0], - forces[1], - forces[2], - moments[0], - moments[1], - moments[2]]) - - return forcez, moment, forcex diff --git a/sharpy/solvers/noaero.py b/sharpy/solvers/noaero.py index 5a18a68d9..562bbe3a4 100644 --- a/sharpy/solvers/noaero.py +++ b/sharpy/solvers/noaero.py @@ -78,7 +78,7 @@ def update_grid(self, beam): -1, beam_ts=-1) - def update_custom_grid(self, structure_tstep, aero_tstep): + def update_custom_grid(self, structure_tstep, aero_tstep, nl_body_tstep = None): # called by DynamicCoupled if self.settings['update_grid']: self.data.aero.generate_zeta_timestep_info(structure_tstep, diff --git a/sharpy/solvers/nonlineardynamiccoupledstep.py b/sharpy/solvers/nonlineardynamiccoupledstep.py index 621686ba5..c6a665b83 100644 --- a/sharpy/solvers/nonlineardynamiccoupledstep.py +++ b/sharpy/solvers/nonlineardynamiccoupledstep.py @@ -77,8 +77,7 @@ def initialise(self, data, custom_settings=None, restart=False): def run(self, **kwargs): structural_step = settings_utils.set_value_or_default(kwargs, 'structural_step', self.data.structure.timestep_info[-1]) - # TODO: previous_structural_step never used - previous_structural_step = settings_utils.set_value_or_default(kwargs, 'previous_structural_step', self.data.structure.timestep_info[-1]) + dt= settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt']) xbeamlib.xbeam_step_couplednlndyn(self.data.structure, diff --git a/sharpy/solvers/nonlineardynamicmultibody.py b/sharpy/solvers/nonlineardynamicmultibody.py index ba4046491..bcbd4be7d 100644 --- a/sharpy/solvers/nonlineardynamicmultibody.py +++ b/sharpy/solvers/nonlineardynamicmultibody.py @@ -62,25 +62,6 @@ class NonLinearDynamicMultibody(_BaseStructural): settings_default['zero_ini_dot_ddot'] = False settings_description['zero_ini_dot_ddot'] = 'Set to zero the position and crv derivatives at the first time step' - settings_types['fix_prescribed_quat_ini'] = 'bool' - settings_default['fix_prescribed_quat_ini'] = False - settings_description['fix_prescribed_quat_ini'] = 'Set to initial the quaternion for prescibed bodies' - - # initial speed direction is given in inertial FOR!!! also in a lot of cases coincident with global A frame - settings_types['initial_velocity_direction'] = 'list(float)' - settings_default['initial_velocity_direction'] = [-1.0, 0.0, 0.0] - settings_description['initial_velocity_direction'] = 'Initial velocity of the reference node given in the inertial FOR' - - settings_types['initial_velocity'] = 'float' - settings_default['initial_velocity'] = 0 - settings_description['initial_velocity'] = 'Initial velocity magnitude of the reference node' - - # restart sim after dynamictrim - settings_types['dyn_trim'] = 'bool' - settings_default['dyn_trim'] = False - settings_description['dyn_trim'] = 'flag for dyntrim prior to dyncoup' - - settings_table = settings_utils.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) @@ -122,302 +103,42 @@ def initialise(self, data, custom_settings=None, restart=False): self.data.structure.add_unsteady_information( self.data.structure.dyn_dict, self.settings['num_steps']) - # import pdb - # pdb.set_trace() - if self.settings['dyn_trim']: - # import pdb - # pdb.set_trace() - - self.data = self.data.previousndm.data - - self.Lambda = self.data.Lambda - self.Lambda_dot = self.data.Lambda_dot - self.Lambda_ddot = np.zeros_like(self.data.Lambda) - - num_body = self.data.structure.timestep_info[0].mb_FoR_vel.shape[0] - - new_quat = np.zeros([num_body,4]) - ini_quat = np.zeros([num_body,4]) - new_direction = np.zeros([num_body,3]) - - # import pdb - # pdb.set_trace() - # self.settings['initial_velocity_direction'] = [-0.8, 0, 0.6] - - # if self.settings['initial_velocity']: - # for ibody in range(num_body): - # new_quat[ibody] = self.data.structure.timestep_info[-1].mb_quat[ibody] - # ini_quat[ibody] = self.data.structure.ini_mb_dict['body_%02d' % ibody]['quat'] - # b = algebra.multiply_matrices(algebra.quat2rotation(new_quat[0]),self.settings['initial_velocity_direction']) - # new_direction[ibody] = algebra.multiply_matrices(algebra.quat2rotation(new_quat[ibody]).T,b) - # # new_direction = np.dot(self.data.structure.timestep_info[-1].cag(), - # # self.settings['initial_velocity_direction']) - # self.data.structure.timestep_info[-1].for_vel[0:3] += new_direction[0]*self.settings['initial_velocity'] - # # num_body = self.data.structure.timestep_info[0].mb_FoR_vel.shape[0] - # # # self.data.structure.num_bodies - # for ibody in range(num_body): - # self.data.structure.timestep_info[-1].mb_FoR_vel[ibody,0:3] += new_direction[ibody]*self.settings['initial_velocity'] - # print(self.data.structure.timestep_info[-1].mb_FoR_vel) - - if self.settings['initial_velocity']: - for ibody in range(num_body): - new_quat[ibody] = self.data.structure.timestep_info[-1].mb_quat[ibody] - ini_quat[ibody] = self.data.structure.ini_mb_dict['body_%02d' % ibody]['quat'] - new_direction[ibody] = algebra.multiply_matrices(algebra.quat2rotation(new_quat[ibody]).T,self.settings['initial_velocity_direction']) - # new_direction = np.dot(self.data.structure.timestep_info[-1].cag(), - # self.settings['initial_velocity_direction']) - self.data.structure.timestep_info[-1].for_vel[0:3] += new_direction[0]*self.settings['initial_velocity'] - # num_body = self.data.structure.timestep_info[0].mb_FoR_vel.shape[0] - # # self.data.structure.num_bodies - for ibody in range(num_body): - self.data.structure.timestep_info[-1].mb_FoR_vel[ibody,0:3] += new_direction[ibody]*self.settings['initial_velocity'] - print(self.data.structure.timestep_info[-1].mb_FoR_vel) - - # import pdb - # pdb.set_trace() - # if self.settings['initial_velocity']: - # new_direction = np.dot(self.data.structure.timestep_info[-1].cag(), - # self.settings['initial_velocity_direction']) - # self.data.structure.timestep_info[-1].for_vel[0:3] = new_direction*self.settings['initial_velocity'] - # num_body = self.data.structure.timestep_info[0].mb_FoR_vel.shape[0] - # # self.data.structure.num_bodies - # for ibody in range(num_body): - # self.data.structure.timestep_info[-1].mb_FoR_vel[ibody,:] = self.data.structure.timestep_info[-1].for_vel - - - # nowquat = np.zeros([num_body,4]) - # iniquat = np.zeros([num_body,4]) - # # reset the a2 rot axis for hinge axes onlyyyyyyy!!!!!!! TODO: - # for ibody in range(num_body): - # nowquat[ibody] = self.data.structure.timestep_info[-1].mb_quat[ibody] - # iniquat[ibody] = self.data.structure.ini_mb_dict['body_%02d' % ibody]['quat'] - - - # # hardcodeeeeeee - # for iconstraint in range(2): - # self.data.structure.ini_mb_dict['constraint_%02d' % iconstraint]['rot_axisA2'] = algebra.multiply_matrices(algebra.quat2rotation(self.data.structure.timestep_info[-1].mb_quat[iconstraint+1]).T, - # algebra.quat2rotation(self.data.structure.ini_mb_dict['body_%02d' % (iconstraint+1)]['quat']), - # self.data.structure.ini_mb_dict['constraint_%02d' % iconstraint]['rot_axisA2']) - - # # reset quat, pos, vel, acc - # for ibody in range(num_body): - # self.data.structure.ini_mb_dict['body_%02d' % ibody]['quat'] = self.data.structure.timestep_info[-1].mb_quat[ibody] - # # self.data.structure.ini_mb_dict['body_%02d' % ibody]['FoR_acceleration'] = self.data.structure.timestep_info[-1].mb_FoR_acc[ibody,:] - # self.data.structure.timestep_info[-1].mb_FoR_acc[ibody,:] = np.zeros([1,6]) - - # self.data.structure.ini_mb_dict['body_%02d' % ibody]['FoR_velocity'] = self.data.structure.timestep_info[-1].mb_FoR_vel[ibody,:] - # self.data.structure.ini_mb_dict['body_%02d' % ibody]['FoR_position'] = self.data.structure.timestep_info[-1].mb_FoR_pos[ibody,:] - - # self.data.structure.timestep_info[-1].mb_dict = self.data.structure.ini_mb_dict - # self.data.structure.ini_info.mb_dict = self.data.structure.ini_mb_dict - - # # Define the number of equations - self.lc_list = lagrangeconstraints.initialize_constraints(self.data.structure.ini_mb_dict) - - # import pdb - # pdb.set_trace() - # Define the number of dofs - self.define_sys_size() #check - self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list) - num_LM_eq = self.num_LM_eq - - - self.prev_Dq = np.zeros((self.sys_size + self.num_LM_eq)) - - self.settings['time_integrator_settings']['sys_size'] = self.sys_size - self.settings['time_integrator_settings']['num_LM_eq'] = self.num_LM_eq - - # Initialise time integrator - self.time_integrator = solver_interface.initialise_solver( - self.settings['time_integrator']) - self.time_integrator.initialise( - self.data, self.settings['time_integrator_settings']) - - if self.settings['write_lm']: - dire = self.data.output_folder + '/NonLinearDynamicMultibody/' - if not os.path.isdir(dire): - os.makedirs(dire) - - self.out_files = {'lambda': dire + 'lambda.dat', - 'lambda_dot': dire + 'lambda_dot.dat', - 'lambda_ddot': dire + 'lambda_ddot.dat', - 'cond_number': dire + 'cond_num.dat'} - - - - # # add initial speed to RBM - # if self.settings['initial_velocity']: - # new_direction = np.dot(self.data.structure.timestep_info[-1].cag(), - # self.settings['initial_velocity_direction']) - # self.data.structure.timestep_info[-1].for_vel[0:3] = new_direction*self.settings['initial_velocity'] - # num_body = self.data.structure.timestep_info[0].mb_FoR_vel.shape[0] - # # self.data.structure.num_bodies - # for ibody in range(num_body): - # self.data.structure.timestep_info[-1].mb_FoR_vel[ibody,:] = self.data.structure.timestep_info[-1].for_vel - # # import pdb - # # pdb.set_trace() - - # # nowquat = np.zeros([num_body,4]) - # # iniquat = np.zeros([num_body,4]) - # # # reset the a2 rot axis for hinge axes onlyyyyyyy!!!!!!! TODO: - # # for ibody in range(num_body): - # # nowquat[ibody] = self.data.structure.timestep_info[-1].mb_quat[ibody] - # # iniquat[ibody] = self.data.structure.ini_mb_dict['body_%02d' % ibody]['quat'] - - - # # # hardcodeeeeeee - # # for iconstraint in range(2): - # # self.data.structure.ini_mb_dict['constraint_%02d' % iconstraint]['rot_axisA2'] = algebra.multiply_matrices(algebra.quat2rotation(self.data.structure.timestep_info[-1].mb_quat[iconstraint+1]).T, - # # algebra.quat2rotation(self.data.structure.ini_mb_dict['body_%02d' % (iconstraint+1)]['quat']), - # # self.data.structure.ini_mb_dict['constraint_%02d' % iconstraint]['rot_axisA2']) - - # # # reset quat, pos, vel, acc - # # for ibody in range(num_body): - # # self.data.structure.ini_mb_dict['body_%02d' % ibody]['quat'] = self.data.structure.timestep_info[-1].mb_quat[ibody] - # # # self.data.structure.ini_mb_dict['body_%02d' % ibody]['FoR_acceleration'] = self.data.structure.timestep_info[-1].mb_FoR_acc[ibody,:] - # # self.data.structure.timestep_info[-1].mb_FoR_acc[ibody,:] = np.zeros([1,6]) - - # # self.data.structure.ini_mb_dict['body_%02d' % ibody]['FoR_velocity'] = self.data.structure.timestep_info[-1].mb_FoR_vel[ibody,:] - # # self.data.structure.ini_mb_dict['body_%02d' % ibody]['FoR_position'] = self.data.structure.timestep_info[-1].mb_FoR_pos[ibody,:] - - - - - # # Define the number of equations - # self.lc_list = lagrangeconstraints.initialize_constraints(self.data.structure.ini_mb_dict) - # print(self.data.structure.ini_mb_dict) - # # import pdb - # # pdb.set_trace() - # self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list) - - # # self.num_LM_eq = 10 - - # structural_step = self.data.structure.timestep_info[-1] - # # dt= self.settings['dt'] - # # import pdb - # # pdb.set_trace() - - - # MBdict = structural_step.mb_dict - - - # # import pdb - # # pdb.set_trace() - - # MB_beam, MB_tstep = mb.split_multibody( - # self.data.structure, - # structural_step, - # MBdict, - # -1) - - # # import pdb - # # pdb.set_trace() - - # q = [] - # dqdt = [] - # dqddt = [] - - # for ibody in range(num_body): - # q = np.append(q, MB_tstep[ibody].q) - # dqdt = np.append(dqdt, MB_tstep[ibody].dqdt) - # dqddt = np.append(dqddt, MB_tstep[ibody].dqddt) - - # q = np.append(q, self.data.Lambda) - # dqdt = np.append(dqdt, self.data.Lambda_dot) - # dqddt = np.append(dqddt, np.zeros([10])) - - # # q = np.insert(q, 336, np.zeros([10])) - # # dqdt = np.insert(dqdt, 336, np.zeros([10])) - # # dqddt = np.insert(dqddt, 336, np.zeros([10])) - - - - # self.Lambda, self.Lambda_dot = mb.state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep, self.num_LM_eq) - - # self.Lambda_ddot = np.zeros_like(self.Lambda) - - # # self.Lambda = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F') - # # self.Lambda_dot = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F') - # # self.Lambda_ddot = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F') + # 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) - # if self.settings['write_lm']: - # dire = self.data.output_folder + '/NonLinearDynamicMultibody/' - # if not os.path.isdir(dire): - # os.makedirs(dire) + self.Lambda = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F') + self.Lambda_dot = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F') + self.Lambda_ddot = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F') - # self.out_files = {'lambda': dire + 'lambda.dat', - # 'lambda_dot': dire + 'lambda_dot.dat', - # 'lambda_ddot': dire + 'lambda_ddot.dat', - # 'cond_number': dire + 'cond_num.dat'} - # # clean up files - # for file in self.out_files.values(): - # if os.path.isfile(file): - # os.remove(file) + if self.settings['write_lm']: + dire = self.data.output_folder + '/NonLinearDynamicMultibody/' + if not os.path.isdir(dire): + os.makedirs(dire) - # # Define the number of dofs - # self.define_sys_size() + self.out_files = {'lambda': dire + 'lambda.dat', + 'lambda_dot': dire + 'lambda_dot.dat', + 'lambda_ddot': dire + 'lambda_ddot.dat', + 'cond_number': dire + 'cond_num.dat'} + # clean up files + for file in self.out_files.values(): + if os.path.isfile(file): + os.remove(file) - # self.prev_Dq = np.zeros((self.sys_size + self.num_LM_eq)) + # Define the number of dofs + self.define_sys_size() - # self.settings['time_integrator_settings']['sys_size'] = self.sys_size - # self.settings['time_integrator_settings']['num_LM_eq'] = self.num_LM_eq + self.prev_Dq = np.zeros((self.sys_size + self.num_LM_eq)) - # # Initialise time integrator - # if not restart: - # self.time_integrator = solver_interface.initialise_solver( - # self.settings['time_integrator']) - # self.time_integrator.initialise( - # self.data, self.settings['time_integrator_settings'], restart=restart) + self.settings['time_integrator_settings']['sys_size'] = self.sys_size + self.settings['time_integrator_settings']['num_LM_eq'] = self.num_LM_eq - else: - # add initial speed to RBM - if self.settings['initial_velocity']: - new_direction = np.dot(self.data.structure.timestep_info[-1].cag(), - self.settings['initial_velocity_direction']) - self.data.structure.timestep_info[-1].for_vel[0:3] = new_direction*self.settings['initial_velocity'] - num_body = self.data.structure.timestep_info[0].mb_FoR_vel.shape[0] - # self.data.structure.num_bodies - for ibody in range(num_body): - self.data.structure.timestep_info[-1].mb_FoR_vel[ibody,:] = self.data.structure.timestep_info[-1].for_vel - # import pdb - # pdb.set_trace() - - # 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) - - self.Lambda = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F') - self.Lambda_dot = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F') - self.Lambda_ddot = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F') - - if self.settings['write_lm']: - dire = self.data.output_folder + '/NonLinearDynamicMultibody/' - if not os.path.isdir(dire): - os.makedirs(dire) - - self.out_files = {'lambda': dire + 'lambda.dat', - 'lambda_dot': dire + 'lambda_dot.dat', - 'lambda_ddot': dire + 'lambda_ddot.dat', - 'cond_number': dire + 'cond_num.dat'} - # clean up files - for file in self.out_files.values(): - if os.path.isfile(file): - os.remove(file) - - # Define the number of dofs - self.define_sys_size() - - self.prev_Dq = np.zeros((self.sys_size + self.num_LM_eq)) - - self.settings['time_integrator_settings']['sys_size'] = self.sys_size - self.settings['time_integrator_settings']['num_LM_eq'] = self.num_LM_eq - - # Initialise time integrator - if not restart: - self.time_integrator = solver_interface.initialise_solver( - self.settings['time_integrator']) - self.time_integrator.initialise( - self.data, self.settings['time_integrator_settings'], restart=restart) + # Initialise time integrator + if not restart: + self.time_integrator = solver_interface.initialise_solver( + self.settings['time_integrator']) + self.time_integrator.initialise( + self.data, self.settings['time_integrator_settings'], restart=restart) def add_step(self): self.data.structure.next_step() @@ -483,8 +204,6 @@ def assembly_MB_eq_system(self, MB_beam, MB_tstep, ts, dt, Lambda, Lambda_dot, M first_dof = 0 last_dof = 0 - # import pdb - # pdb.set_trace() # Loop through the different bodies for ibody in range(len(MB_beam)): @@ -494,22 +213,10 @@ def assembly_MB_eq_system(self, MB_beam, MB_tstep, ts, dt, Lambda, Lambda_dot, M K = None Q = None - - # import pdb - # pdb.set_trace() # Generate the matrices for each body if MB_beam[ibody].FoR_movement == 'prescribed': last_dof = first_dof + MB_beam[ibody].num_dof.value - # old_quat = MB_tstep[ibody].quat.copy() - M, C, K, Q = xbeamlib.cbeam3_asbly_dynamic(MB_beam[ibody], MB_tstep[ibody], self.settings) - # MB_tstep[ibody].quat = old_quat - - elif MB_beam[ibody].FoR_movement == 'prescribed_trim': - last_dof = first_dof + MB_beam[ibody].num_dof.value - # old_quat = MB_tstep[ibody].quat.copy() M, C, K, Q = xbeamlib.cbeam3_asbly_dynamic(MB_beam[ibody], MB_tstep[ibody], self.settings) - # MB_tstep[ibody].quat = old_quat - elif MB_beam[ibody].FoR_movement == 'free': last_dof = first_dof + MB_beam[ibody].num_dof.value + 10 @@ -576,36 +283,8 @@ def integrate_position(self, MB_beam, MB_tstep, dt): MB_tstep[ibody].for_pos[0:3] += dt*np.dot(MB_tstep[ibody].cga(),MB_tstep[ibody].for_vel[0:3]) def extract_resultants(self, tstep): - # import pdb - # pdb.set_trace() - # TODO: code - if tstep is None: - tstep = self.data.structure.timestep_info[self.data.ts] - steady, unsteady, grav = tstep.extract_resultants(self.data.structure, force_type=['steady', 'unsteady', 'grav']) - totals = steady + unsteady + grav - return totals[0:3], totals[3:6] - - def extract_resultants_not_required(self, tstep): - # as ibody = None returns for entire structure! How convenient! - import pdb - pdb.set_trace() # TODO: code - if tstep is None: - tstep = self.data.structure.timestep_info[self.data.ts] - steady_running = 0 - unsteady_running = 0 - grav_running = 0 - for body in range (0, self.data.structure.ini_mb_dict['num_bodies']): - steady, unsteady, grav = tstep.extract_resultants(self.data.structure, force_type=['steady', 'unsteady', 'grav'], ibody = body) - steady_running += steady - unsteady_running += unsteady - grav_running += grav - totals = steady_running + unsteady_running + grav_running - return totals[0:3], totals[3:6] - - - - # return np.zeros((3)), np.zeros((3)) + return np.zeros((3)), np.zeros((3)) def compute_forces_constraints(self, MB_beam, MB_tstep, ts, dt, Lambda, Lambda_dot): """ @@ -673,8 +352,6 @@ def write_lm_cond_num(self, iteration, Lambda, Lambda_dot, Lambda_ddot, cond_num def run(self, **kwargs): structural_step = settings_utils.set_value_or_default(kwargs, 'structural_step', self.data.structure.timestep_info[-1]) dt= settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt']) - # import pdb - # pdb.set_trace() if structural_step.mb_dict is not None: MBdict = structural_step.mb_dict @@ -703,16 +380,6 @@ def run(self, **kwargs): MB_tstep[ibody].psi_dot_local *= 0. MB_tstep[ibody].psi_ddot *= 0. - - # # Initialize - # # TODO: i belive this can move into disp_and_accel2 state as self.Lambda, self.Lambda_dot - - - # # Predictor step - # q, dqdt, dqddt = mb.disp_and_accel2state(MB_beam, MB_tstep, self.Lambda, self.Lambda_dot, self.sys_size, num_LM_eq) - # self.time_integrator.predictor(q, dqdt, dqddt) - - # else: # Initialize # TODO: i belive this can move into disp_and_accel2 state as self.Lambda, self.Lambda_dot if not num_LM_eq == 0: @@ -726,7 +393,7 @@ def run(self, **kwargs): # Predictor step q, dqdt, dqddt = mb.disp_and_accel2state(MB_beam, MB_tstep, Lambda, Lambda_dot, self.sys_size, num_LM_eq) - self.time_integrator.predictor(q, dqdt, dqddt) + self.time_integrator.predictor(q, dqdt, dqddt) # Reference residuals old_Dq = 1.0 @@ -738,9 +405,7 @@ def run(self, **kwargs): if iteration == self.settings['max_iterations'] - 1: error = ('Solver did not converge in %d iterations.\n res = %e \n LM_res = %e' % (iteration, res, LM_res)) - print(error) - break - # raise exc.NotConvergedSolver(error) + raise exc.NotConvergedSolver(error) # Update positions and velocities Lambda, Lambda_dot = mb.state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep, num_LM_eq) @@ -759,18 +424,6 @@ def run(self, **kwargs): Asys, Q = self.time_integrator.build_matrix(MB_M, MB_C, MB_K, MB_Q, kBnh, LM_Q) - np.set_printoptions(threshold=np.inf) - # import pdb - # pdb.set_trace() - - # print(Asys) - # print(Q) - # import sympy - # rref, inds = sympy.Matrix(Asys).rref() - # print(inds) - # print(rref) - # print(np.linalg.inv(Asys)) - if self.settings['write_lm']: cond_num = np.linalg.cond(Asys[:self.sys_size, :self.sys_size]) cond_num_lm = np.linalg.cond(Asys) @@ -826,13 +479,7 @@ def run(self, **kwargs): if (res < self.settings['min_delta']) and (LM_res < self.settings['min_delta']): break - # if (res < self.settings['min_delta']) and (np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq])) < self.settings['abs_threshold']): - # print(f'Relative \'min_delta\' threshold not reached - LM_res is {LM_res} >= \'min_delta\' {self.settings["min_delta"]} abs res is {np.max(np.abs(Dq[0:self.sys_size]))}, abs LM_res is {np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq]))} < {self.settings["abs_threshold"]}') - # break - # import pdb - # pdb.set_trace() - # print("end - check") Lambda, Lambda_dot = mb.state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep, num_LM_eq) if self.settings['write_lm']: self.write_lm_cond_num(iteration, Lambda, Lambda_dot, Lambda_ddot, cond_num, cond_num_lm) @@ -852,7 +499,6 @@ def run(self, **kwargs): MB_tstep[ibody].quat = np.dot(Temp, np.dot(np.eye(4) - 0.25*algebra.quadskew(MB_tstep[ibody].for_vel[3:6])*dt, MB_tstep[ibody].quat)) # End of Newmark-beta iterations - # self.beta = self.time_integrator.beta # self.integrate_position(MB_beam, MB_tstep, dt) 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) @@ -865,16 +511,4 @@ def run(self, **kwargs): self.Lambda_dot = Lambda_dot.astype(dtype=ct.c_double, copy=True, order='F') self.Lambda_ddot = Lambda_ddot.astype(dtype=ct.c_double, copy=True, order='F') - self.data.Lambda = Lambda.astype(dtype=ct.c_double, copy=True, order='F') - self.data.Lambda_dot = Lambda_dot.astype(dtype=ct.c_double, copy=True, order='F') - self.data.Lambda_ddot = Lambda_ddot.astype(dtype=ct.c_double, copy=True, order='F') - - self.data.previousndm = self - - # name = "struc_" + str(self.data.ts) + ".txt" - # from pprint import pprint as ppr - # file = open(name,'wt') - # ppr(self.data.structure.timestep_info[-1].__dict__, stream=file) - # file.close() - return self.data diff --git a/sharpy/solvers/staticcoupled.py b/sharpy/solvers/staticcoupled.py index 74f05a159..e3fde1bae 100644 --- a/sharpy/solvers/staticcoupled.py +++ b/sharpy/solvers/staticcoupled.py @@ -74,6 +74,10 @@ class StaticCoupled(BaseSolver): settings_description['runtime_generators'] = 'The dictionary keys are the runtime generators to be used. ' \ 'The dictionary values are dictionaries with the settings ' \ 'needed by each generator.' + + settings_types['nonlifting_body_interactions'] = 'bool' + settings_default['nonlifting_body_interactions'] = False + settings_description['nonlifting_body_interactions'] = 'Consider forces induced by nonlifting bodies' settings_table = settings_utils.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description, settings_options) @@ -149,17 +153,20 @@ def increase_ts(self): 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.remove_old_timestep_info(self.data.structure.timestep_info) + self.remove_old_timestep_info(self.data.aero.timestep_info) + if self.settings['nonlifting_body_interactions']: + self.remove_old_timestep_info(self.data.nonlifting_body.timestep_info) self.data.ts = 0 + def remove_old_timestep_info(self, tstep_info): + # copy last info to first + tstep_info[0] = tstep_info[-1].copy() + # delete all the rest + while len(tstep_info) - 1: + del tstep_info[-1] + def run(self, **kwargs): for i_step in range(self.settings['n_load_steps'] + 1): if (i_step == self.settings['n_load_steps'] and @@ -189,14 +196,29 @@ def run(self, **kwargs): self.data.structure.node_master_elem, self.data.structure.connectivities, self.data.structure.timestep_info[self.data.ts].cag(), - self.data.aero.aero_dict) - + self.data.aero.data_dict) + if self.correct_forces: struct_forces = \ self.correct_forces_generator.generate(aero_kstep=self.data.aero.timestep_info[self.data.ts], structural_kstep=self.data.structure.timestep_info[self.data.ts], struct_forces=struct_forces, ts=0) + + # map nonlifting forces to structural nodes + if self.settings['nonlifting_body_interactions']: + struct_forces += mapping.aero2struct_force_mapping( + self.data.nonlifting_body.timestep_info[self.data.ts].forces, + self.data.nonlifting_body.struct2aero_mapping, + self.data.nonlifting_body.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.nonlifting_body.data_dict, + skip_moments_generated_by_forces = True) + self.data.aero.timestep_info[self.data.ts].aero_steady_forces_beam_dof = struct_forces self.data.structure.timestep_info[self.data.ts].postproc_node['aero_steady_forces'] = struct_forces # B @@ -239,6 +261,7 @@ def run(self, **kwargs): # update grid self.aero_solver.update_step() + self.structural_solver.update(self.data.structure.timestep_info[self.data.ts]) # convergence if self.convergence(i_iter, i_step): # create q and dqdt vectors @@ -322,10 +345,7 @@ def change_trim(self, alpha, thrust, thrust_nodes, tail_deflection, tail_cs_inde 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) - #TODO: HARDCODE - self.force_orientation = np.array([[0., 1., 0.],[0., -1., 0.]]) - print(self.force_orientation) + # print(self.force_orientation) # thrust # thrust is scaled so that the direction of the forces is conserved @@ -345,7 +365,7 @@ def change_trim(self, alpha, thrust, thrust_nodes, tail_deflection, tail_cs_inde # tail deflection try: - self.data.aero.aero_dict['control_surface_deflection'][tail_cs_index] = tail_deflection + self.data.aero.data_dict['control_surface_deflection'][tail_cs_index] = tail_deflection except KeyError: raise Exception('This model has no control surfaces') except IndexError: diff --git a/sharpy/solvers/staticuvlm.py b/sharpy/solvers/staticuvlm.py index 66fcd7d35..e9fa9b859 100644 --- a/sharpy/solvers/staticuvlm.py +++ b/sharpy/solvers/staticuvlm.py @@ -43,6 +43,18 @@ class StaticUvlm(BaseSolver): settings_default['horseshoe'] = False settings_description['horseshoe'] = 'Horseshoe wake modelling for steady simulations.' + settings_types['nonlifting_body_interactions'] = 'bool' + settings_default['nonlifting_body_interactions'] = False + settings_description['nonlifting_body_interactions'] = 'Consider nonlifting body interactions' + + settings_types['only_nonlifting'] = 'bool' + settings_default['only_nonlifting'] = False + settings_description['only_nonlifting'] = 'Consider only nonlifting bodies' + + settings_types['phantom_wing_test'] = 'bool' + settings_default['phantom_wing_test'] = False + settings_description['phantom_wing_test'] = 'Debug option' + settings_types['num_cores'] = 'int' settings_default['num_cores'] = 0 settings_description['num_cores'] = 'Number of cores to use in the VLM lib' @@ -111,6 +123,10 @@ class StaticUvlm(BaseSolver): settings_default['map_forces_on_struct'] = False settings_description['map_forces_on_struct'] = 'Maps the forces on the structure at the end of the timestep. Only usefull if the solver is used outside StaticCoupled' + settings_types['ignore_first_x_nodes_in_force_calculation'] = 'int' + settings_default['ignore_first_x_nodes_in_force_calculation'] = 0 + settings_description['ignore_first_x_nodes_in_force_calculation'] = 'Ignores the forces on the first user-specified number of nodes of all surfaces.' + settings_table = settings_utils.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description) @@ -138,60 +154,84 @@ def initialise(self, data, custom_settings=None, restart=False): def add_step(self): self.data.aero.add_timestep() + if self.settings['nonlifting_body_interactions']: + self.data.nonlifting_body.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): + if not self.settings['only_nonlifting']: + self.data.aero.generate_zeta(beam, + self.data.aero.aero_settings, + -1, + beam_ts=-1) + if self.settings['nonlifting_body_interactions'] or self.settings['only_nonlifting']: + self.data.nonlifting_body.generate_zeta(beam, + self.data.nonlifting_body.aero_settings, + -1, + beam_ts=-1) + + def update_custom_grid(self, structure_tstep, aero_tstep, nonlifting_tstep=None): self.data.aero.generate_zeta_timestep_info(structure_tstep, aero_tstep, self.data.structure, self.data.aero.aero_settings, dt=self.settings['rollup_dt']) + if self.settings['nonlifting_body_interactions']: + self.data.nonlifting_body.generate_zeta_timestep_info(structure_tstep, + nonlifting_tstep, + self.data.structure, + self.data.nonlifting_body.aero_settings) def run(self, **kwargs): - aero_tstep = settings_utils.set_value_or_default(kwargs, 'aero_step', self.data.aero.timestep_info[-1]) - structure_tstep = settings_utils.set_value_or_default(kwargs, 'structural_step', self.data.structure.timestep_info[-1]) - dt = settings_utils.set_value_or_default(kwargs, 'dt', self.settings['rollup_dt']) - t = settings_utils.set_value_or_default(kwargs, 't', self.data.ts*dt) - - unsteady_contribution = False - convect_wake = False - - if not aero_tstep.zeta: - return self.data - - # generate the wake because the solid shape might change - 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}) - - # generate uext - self.velocity_generator.generate({'zeta': aero_tstep.zeta, - 'override': True, - 'for_pos': structure_tstep.for_pos[0:3]}, - aero_tstep.u_ext) - # grid orientation - uvlmlib.vlm_solver(aero_tstep, - self.settings) - - if self.settings['map_forces_on_struct']: - structure_tstep.steady_applied_forces[:] = mapping.aero2struct_force_mapping( - aero_tstep.forces, - self.data.aero.struct2aero_mapping, - self.data.aero.timestep_info[self.data.ts].zeta, - structure_tstep.pos, - structure_tstep.psi, - self.data.structure.node_master_elem, - self.data.structure.connectivities, - structure_tstep.cag(), - self.data.aero.aero_dict) + structure_tstep = settings_utils.set_value_or_default(kwargs, 'structural_step', self.data.structure.timestep_info[self.data.ts]) + + if not self.settings['only_nonlifting']: + aero_tstep = settings_utils.set_value_or_default(kwargs, 'aero_step', self.data.aero.timestep_info[self.data.ts]) + if not self.data.aero.timestep_info[self.data.ts].zeta: + return self.data + + # generate the wake because the solid shape might change + 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}) + + if self.settings['nonlifting_body_interactions']: + # generate uext + self.velocity_generator.generate({'zeta': self.data.nonlifting_body.timestep_info[self.data.ts].zeta, + 'override': True, + 'for_pos': structure_tstep.for_pos[0:3]}, + self.data.nonlifting_body.timestep_info[self.data.ts].u_ext) + # generate uext + self.velocity_generator.generate({'zeta': self.data.aero.timestep_info[self.data.ts].zeta, + 'override': True, + 'for_pos': self.data.structure.timestep_info[self.data.ts].for_pos[0:3]}, + self.data.aero.timestep_info[self.data.ts].u_ext) + # grid orientation + uvlmlib.vlm_solver_lifting_and_nonlifting_bodies(self.data.aero.timestep_info[self.data.ts], + self.data.nonlifting_body.timestep_info[self.data.ts], + self.settings) + else: + # generate uext + self.velocity_generator.generate({'zeta': self.data.aero.timestep_info[self.data.ts].zeta, + 'override': True, + 'for_pos': self.data.structure.timestep_info[self.data.ts].for_pos[0:3]}, + self.data.aero.timestep_info[self.data.ts].u_ext) + + + # grid orientation + uvlmlib.vlm_solver(self.data.aero.timestep_info[self.data.ts], + self.settings) + else: + self.velocity_generator.generate({'zeta': self.data.nonlifting_body.timestep_info[self.data.ts].zeta, + 'override': True, + 'for_pos': self.data.structure.timestep_info[self.data.ts].for_pos[0:3]}, + self.data.nonlifting_body.timestep_info[self.data.ts].u_ext) + uvlmlib.vlm_solver_nonlifting_body(self.data.nonlifting_body.timestep_info[self.data.ts], + self.settings) return self.data @@ -199,12 +239,17 @@ def next_step(self): """ Updates de aerogrid based on the info of the step, and increases the self.ts counter """ self.data.aero.add_timestep() + if self.settings['nonlifting_body_interactions']: + self.data.nonlifting_body.add_timestep() self.update_step() def update_step(self): - self.data.aero.generate_zeta(self.data.structure, - self.data.aero.aero_settings, - self.data.ts) - # for i_surf in range(self.data.aero.timestep_info[self.data.ts].n_surf): - # self.data.aero.timestep_info[self.data.ts].forces[i_surf].fill(0.0) - # self.data.aero.timestep_info[self.data.ts].dynamic_forces[i_surf].fill(0.0) + if not self.settings['only_nonlifting']: + self.data.aero.generate_zeta(self.data.structure, + self.data.aero.aero_settings, + self.data.ts) + if self.settings['nonlifting_body_interactions'] or self.settings['only_nonlifting']: + self.data.nonlifting_body.generate_zeta(self.data.structure, + self.data.nonlifting_body.aero_settings, + self.data.ts) + diff --git a/sharpy/solvers/stepuvlm.py b/sharpy/solvers/stepuvlm.py index d4cd9ad40..ba4b2a686 100644 --- a/sharpy/solvers/stepuvlm.py +++ b/sharpy/solvers/stepuvlm.py @@ -130,6 +130,26 @@ class StepUvlm(BaseSolver): settings_types['quasi_steady'] = 'bool' settings_default['quasi_steady'] = False settings_description['quasi_steady'] = 'Use quasi-steady approximation in UVLM' + + settings_types['only_nonlifting'] = 'bool' + settings_default['only_nonlifting'] = False + settings_description['only_nonlifting'] = 'Consider nonlifting body interactions' + + settings_types['nonlifting_body_interactions'] = 'bool' + settings_default['nonlifting_body_interactions'] = False + settings_description['nonlifting_body_interactions'] = 'Consider nonlifting body interactions' + + settings_types['phantom_wing_test'] = 'bool' + settings_default['phantom_wing_test'] = False + settings_description['phantom_wing_test'] = 'Debug option' + + settings_types['centre_rot_g'] = 'list(float)' + settings_default['centre_rot_g'] = [0., 0., 0.] + settings_description['centre_rot_g'] = 'Centre of rotation in G FoR around which ``rbm_vel_g`` is applied' + + settings_types['ignore_first_x_nodes_in_force_calculation'] = 'int' + settings_default['ignore_first_x_nodes_in_force_calculation'] = 0 + settings_description['ignore_first_x_nodes_in_force_calculation'] = 'Ignores the forces on the first user-specified number of nodes of all surfaces.' settings_table = settings_utils.SettingsTable() __doc__ += settings_table.generate(settings_types, settings_default, settings_description, settings_options) @@ -218,13 +238,32 @@ def run(self, **kwargs): 'for_pos': structure_tstep.for_pos, 'is_wake': True}, aero_tstep.u_ext_star) + if self.settings['nonlifting_body_interactions']: - uvlmlib.uvlm_solver(self.data.ts, - aero_tstep, - structure_tstep, - self.settings, - convect_wake=convect_wake, - dt=dt) + nl_body_tstep = settings_utils.set_value_or_default(kwargs, 'nl_body_tstep', self.data.nonlifting_body.timestep_info[-1]) + self.velocity_generator.generate({'zeta': nl_body_tstep.zeta, + 'override': True, + 'ts': self.data.ts, + 'dt': dt, + 't': t, + 'for_pos': structure_tstep.for_pos, + 'is_wake': False}, + nl_body_tstep.u_ext) + + uvlmlib.uvlm_solver_lifting_and_nonlifting(self.data.ts, + aero_tstep, + nl_body_tstep, + structure_tstep, + self.settings, + convect_wake=convect_wake, + dt=dt) + else: + uvlmlib.uvlm_solver(self.data.ts, + aero_tstep, + structure_tstep, + self.settings, + convect_wake=convect_wake, + dt=dt) if unsteady_contribution and not self.settings['quasi_steady']: # calculate unsteady (added mass) forces: @@ -248,24 +287,38 @@ def run(self, **kwargs): 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() + if self.settings['nonlifting_body_interactions']: + self.data.nonlifting_body.add_timestep() def update_grid(self, beam): self.data.aero.generate_zeta(beam, self.data.aero.aero_settings, -1, beam_ts=-1) + if self.settings['nonlifting_body_interactions']: + self.data.nonlifting_body.generate_zeta(beam, + self.data.aero.aero_settings, + -1, + beam_ts=-1) - def update_custom_grid(self, structure_tstep, aero_tstep): + def update_custom_grid(self, structure_tstep, aero_tstep, nl_body_tstep = None): self.data.aero.generate_zeta_timestep_info(structure_tstep, aero_tstep, self.data.structure, self.data.aero.aero_settings, dt=self.settings['dt']) + if self.settings['nonlifting_body_interactions']: + if nl_body_tstep is None: + nl_body_tstep = self.data.nonlifting_body.timestep_info[-1] + self.data.nonlifting_body.generate_zeta_timestep_info(structure_tstep, + nl_body_tstep, + self.data.structure, + self.data.nonlifting_body.aero_settings, + dt = self.settings['dt']) @staticmethod def filter_gamma_dot(tstep, history, filter_param): diff --git a/sharpy/solvers/trim.py b/sharpy/solvers/trim.py index b8543f21e..4cf0a5a05 100644 --- a/sharpy/solvers/trim.py +++ b/sharpy/solvers/trim.py @@ -291,7 +291,7 @@ def solver_wrapper(x, x_info, solver_data, i_dim=-1): tstep.quat[:] = orientation_quat # control surface deflection for i_cs in range(len(x_info['i_control_surfaces'])): - solver_data.data.aero.aero_dict['control_surface_deflection'][x_info['control_surfaces_id'][i_cs]] = x[x_info['i_control_surfaces'][i_cs]] + solver_data.data.aero.data_dict['control_surface_deflection'][x_info['control_surfaces_id'][i_cs]] = x[x_info['i_control_surfaces'][i_cs]] # thrust input tstep.steady_applied_forces[:] = 0.0 try: