def calculate_forces(self): for self.ts in range(self.ts_max): rot = algebra.quat2rotation( self.data.structure.timestep_info[self.ts].quat) force = self.data.aero.timestep_info[self.ts].forces unsteady_force = self.data.aero.timestep_info[ self.ts].dynamic_forces n_surf = len(force) for i_surf in range(n_surf): total_steady_force = np.zeros((3, )) total_unsteady_force = np.zeros((3, )) _, n_rows, n_cols = force[i_surf].shape for i_m in range(n_rows): for i_n in range(n_cols): total_steady_force += force[i_surf][0:3, i_m, i_n] total_unsteady_force += unsteady_force[i_surf][0:3, i_m, i_n] self.data.aero.timestep_info[self.ts].inertial_steady_forces[ i_surf, 0:3] = total_steady_force self.data.aero.timestep_info[self.ts].inertial_unsteady_forces[ i_surf, 0:3] = total_unsteady_force self.data.aero.timestep_info[self.ts].body_steady_forces[ i_surf, 0:3] = np.dot(rot.T, total_steady_force) self.data.aero.timestep_info[self.ts].body_unsteady_forces[ i_surf, 0:3] = np.dot(rot.T, total_unsteady_force)
def wing_tip_deflection(self, frame='a', alpha=0, reference_line=np.array([0, 0, 0], dtype=float)): param_array = [] deflection = [] if frame == 'g': try: import sharpy.utils.algebra as algebra except ModuleNotFoundError: raise (ModuleNotFoundError('Please load sharpy')) else: cga = algebra.quat2rotation( algebra.euler2quat(np.array([0, alpha * np.pi / 180, 0]))) for case in self.cases['aeroelastic']: param_array.append(case.parameter_value) if frame == 'a': deflection.append( case.get_deflection_at_line(reference_line)[-1, -3:]) elif frame == 'g': deflection.append( cga.dot( case.get_deflection_at_line(reference_line)[-1, -3:])) param_array = np.array(param_array) order = np.argsort(param_array) param_array = param_array[order] deflection = np.array([deflection[ith] for ith in order]) return param_array, deflection
def test_rotation_G_to_A(self): """ Tests the rotation of a vector in G frame to a vector in A frame by a roll, pitch and yaw considering that SHARPy employs an SEU frame. In the inertial frame, the ``x_g`` axis is aligned with the longitudinal axis of the aircraft pointing backwards, ``z_g`` points upwards and ``y_g`` completes the set Returns: """ aircraft_nose = np.array([-1, 0, 0]) z_A = np.array([0, 0, 1]) euler = np.array([90, 90, 90]) * np.pi / 180 quat = algebra.euler2quat(euler) aircraft_nose_rotated = np.array([0, 0, 1]) # in G frame pointing upwards z_A_rotated_g = np.array([1, 0, 0]) Cga = algebra.euler2rot(euler) Cga_quat = algebra.quat2rotation(quat) np.testing.assert_array_almost_equal( Cga.dot(aircraft_nose), aircraft_nose_rotated, err_msg='Rotation using Euler angles not performed properly') np.testing.assert_array_almost_equal( Cga_quat.dot(aircraft_nose), aircraft_nose_rotated, err_msg='Rotation using quaternions not performed properly') np.testing.assert_array_almost_equal( Cga.dot(z_A), z_A_rotated_g, err_msg='Rotation using Euler angles not performed properly') np.testing.assert_array_almost_equal( Cga_quat.dot(z_A), z_A_rotated_g, err_msg='Rotation using quaternions not performed properly') # Check projections Pag = Cga.T Pag_quat = Cga_quat.T np.testing.assert_array_almost_equal( Pag.dot(z_A_rotated_g), z_A, err_msg='Error in projection from A to G using Euler angles') np.testing.assert_array_almost_equal( Pag.dot(aircraft_nose_rotated), aircraft_nose, err_msg='Error in projection from A to G using Euler angles') np.testing.assert_array_almost_equal( Pag_quat.dot(z_A_rotated_g), z_A, err_msg='Error in projection from A to G using quaternions') np.testing.assert_array_almost_equal( Pag_quat.dot(aircraft_nose_rotated), aircraft_nose, err_msg='Error in projection from A to G using quaternions')
def update_mb_dB_before_merge(tstep, MB_tstep): """ update_mb_db_before_merge Updates the FoR information database before merge the bodies Longer description Args: tstep (StructTimeStepInfo): timestep information of the multibody system MB_tstep (list of StructTimeStepInfo): each entry represents a body Returns: Examples: Notes: """ for ibody in range(len(MB_tstep)): CAslaveG = algebra.quat2rotation(MB_tstep[ibody].quat).T tstep.mb_FoR_pos[ibody, :] = MB_tstep[ibody].for_pos tstep.mb_FoR_vel[ibody, 0:3] = np.dot(np.transpose(CAslaveG), MB_tstep[ibody].for_vel[0:3]) tstep.mb_FoR_vel[ibody, 3:6] = np.dot(np.transpose(CAslaveG), MB_tstep[ibody].for_vel[3:6]) tstep.mb_FoR_acc[ibody, 0:3] = np.dot(np.transpose(CAslaveG), MB_tstep[ibody].for_acc[0:3]) tstep.mb_FoR_acc[ibody, 3:6] = np.dot(np.transpose(CAslaveG), MB_tstep[ibody].for_acc[3:6]) tstep.mb_quat[ibody, :] = MB_tstep[ibody].quat.astype( dtype=ct.c_double, order='F', copy=True) tstep.mb_dqddt_quat[ibody, :] = MB_tstep[ibody].dqddt[-4:].astype( dtype=ct.c_double, order='F', copy=True) # TODO: Is it convenient to do this? for ibody in range(len(MB_tstep)): MB_tstep[ibody].mb_FoR_pos = tstep.mb_FoR_pos.astype(dtype=ct.c_double, order='F', copy=True) MB_tstep[ibody].mb_FoR_vel = tstep.mb_FoR_vel.astype(dtype=ct.c_double, order='F', copy=True) MB_tstep[ibody].mb_FoR_acc = tstep.mb_FoR_acc.astype(dtype=ct.c_double, order='F', copy=True) MB_tstep[ibody].mb_quat = tstep.mb_quat.astype(dtype=ct.c_double, order='F', copy=True) MB_tstep[ibody].mb_dqddt_quat = tstep.mb_dqddt_quat.astype( dtype=ct.c_double, order='F', copy=True)
def update_mb_db_before_split(tstep, beam, mb_data_dict, ts): """ update_mb_db_before_split Updates the FoR information database before splitting system Args: tstep (:class:`~sharpy.utils.datastructures.StructTimeStepInfo`): timestep information of the multibody system beam (:class:`~sharpy.structure.models.beam.Beam`): structural information of the multibody system mb_data_dict (dict): Dictionary including the multibody information ts (int): time step number Notes: At this point, this function does nothing, but we might need it at some point """ return raise NotImplementedError("This function is useless right now") # TODO: Right now, the Amaster FoR is not expected to move # when it does, the rest of FoR positions should be updated accordingly # right now, this function should be useless (I check it below) # if mb_data_dict['body_%02d' % 0]['FoR_movement']: # CGAmaster = algebra.quat2rotation(tstep.quat) # tstep.mb_FoR_vel[0, 0:3] = np.dot(CGAmaster, tstep.for_vel[0:3]) # tstep.mb_FoR_vel[0, 3:6] = np.dot(CGAmaster, tstep.for_vel[3:6]) # tstep.mb_FoR_acc[0, 0:3] = np.dot(CGAmaster, tstep.for_acc[0:3]) # tstep.mb_FoR_acc[0, 3:6] = np.dot(CGAmaster, tstep.for_acc[3:6]) if ((mb_data_dict['body_00']['FoR_movement'] == 'prescribed') and (ts > 0)): tstep.for_vel[:] = beam.dynamic_input[ts - 1]['for_vel'].astype( dtype=ct.c_double, order='F', copy=True) tstep.for_acc[:] = beam.dynamic_input[ts - 1]['for_acc'].astype( dtype=ct.c_double, order='F', copy=True) if True: CGAmaster = algebra.quat2rotation(tstep.quat) tstep.mb_FoR_pos[0, :] = tstep.for_pos.astype(dtype=ct.c_double, order='F', copy=True) tstep.mb_FoR_vel[0, 0:3] = np.dot(CGAmaster, tstep.for_vel[0:3]) tstep.mb_FoR_vel[0, 3:6] = np.dot(CGAmaster, tstep.for_vel[3:6]) tstep.mb_FoR_acc[0, 0:3] = np.dot(CGAmaster, tstep.for_acc[0:3]) tstep.mb_FoR_acc[0, 3:6] = np.dot(CGAmaster, tstep.for_acc[3:6]) tstep.mb_quat[0, :] = tstep.quat.astype(dtype=ct.c_double, order='F', copy=True) else: pass
def update_mb_db_before_split(tstep, beam, mb_data_dict, ts): """ update_mb_db_before_split Updates the FoR information database before split the system Longer description Args: tstep (StructTimeStepInfo): timestep information of the multibody system Returns: Examples: Notes: At this point, this function does nothing, but we might need it at some point """ # TODO: Right now, the Amaster FoR is not expected to move # when it does, the rest of FoR positions should be updated accordingly # right now, this function should be useless (I check it below) # if mb_data_dict['body_%02d' % 0]['FoR_movement']: # CGAmaster = algebra.quat2rotation(tstep.quat) # tstep.mb_FoR_vel[0, 0:3] = np.dot(CGAmaster, tstep.for_vel[0:3]) # tstep.mb_FoR_vel[0, 3:6] = np.dot(CGAmaster, tstep.for_vel[3:6]) # tstep.mb_FoR_acc[0, 0:3] = np.dot(CGAmaster, tstep.for_acc[0:3]) # tstep.mb_FoR_acc[0, 3:6] = np.dot(CGAmaster, tstep.for_acc[3:6]) if ((mb_data_dict['body_00']['FoR_movement'] == 'prescribed') and (ts > 0)): tstep.for_vel[:] = beam.dynamic_input[ts - 1]['for_vel'].astype( dtype=ct.c_double, order='F', copy=True) tstep.for_acc[:] = beam.dynamic_input[ts - 1]['for_acc'].astype( dtype=ct.c_double, order='F', copy=True) if True: CGAmaster = algebra.quat2rotation(tstep.quat) tstep.mb_FoR_pos[0, :] = tstep.for_pos.astype(dtype=ct.c_double, order='F', copy=True) tstep.mb_FoR_vel[0, 0:3] = np.dot(CGAmaster, tstep.for_vel[0:3]) tstep.mb_FoR_vel[0, 3:6] = np.dot(CGAmaster, tstep.for_vel[3:6]) tstep.mb_FoR_acc[0, 0:3] = np.dot(CGAmaster, tstep.for_acc[0:3]) tstep.mb_FoR_acc[0, 3:6] = np.dot(CGAmaster, tstep.for_acc[3:6]) tstep.mb_quat[0, :] = tstep.quat.astype(dtype=ct.c_double, order='F', copy=True) else: pass
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)
data0 = sharpy.sharpy_main.main( ['...', route_main + case_main + '.solver.txt']) tsaero0 = data0.aero.timestep_info[0] tsaero0.rho = ws.config['LinearUvlm']['density'] ### ----- retrieve transformation matrices # this is necessary so as to project input motion and output forces in the # rolled frame of reference R tsstr0 = data0.structure.timestep_info[0] ### check all CRV are the same crv_ref = tsstr0.psi[0][0] for ee in range(data0.structure.num_elem): for nn_node in range(3): assert np.linalg.norm(crv_ref - tsstr0.psi[ee, nn_node, :]) < 1e-13, \ 'CRV distribution along beam nodes not uniform!' Cga = algebra.quat2rotation(tsstr0.quat) Cab = algebra.crv2rotation(tsstr0.psi[0][0]) Cgb = np.dot(Cga, Cab) ### rolled FoR: # note that, if RollNodes is False, this is equivalent to the FoR A. While # this transformation is redundant for RollNodes=False, we keep it for debug Roll0Rad = np.pi / 180. * Roll0Deg crv_roll = Roll0Rad * np.array([1., 0., 0.]) Cgr = algebra.crv2rotation(crv_roll) Crg = Cgr.T Crb = np.dot(Crg, Cgb) Cra = np.dot(Crg, Cga) ### ----- linearisation Sol = lin_aeroelastic.LinAeroEla(data0)
def initialise(self, data, custom_settings=None): r""" Initialises the Linear UVLM aerodynamic solver and the chosen velocity generator. Settings are parsed into the standard SHARPy settings format for solvers. It then checks whether there is any previous information about the linearised system (in order for a solution to be restarted without overwriting the linearisation). If a linearised system does not exist, a linear UVLM system is created linearising about the current time step. The reference values for the input and output are transformed into column vectors :math:`\mathbf{u}` and :math:`\mathbf{y}`, respectively. The information pertaining to the linear system is stored in a dictionary ``self.data.aero.linear`` within the main ``data`` variable. Args: data (PreSharpy): class containing the problem information custom_settings (dict): custom settings dictionary """ 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) settings.to_custom_types(self.settings['ScalingDict'], self.scaling_settings_types, self.scaling_settings_default, no_ctype=True) # Check whether linear UVLM has been initialised try: self.data.aero.linear except AttributeError: self.data.aero.linear = dict() aero_tstep = self.data.aero.timestep_info[-1] ### Record body orientation/velocities at time 0 # This option allows to rotate the linearised UVLM with the A frame # or a specific body (multi-body solution) if self.settings['track_body']: self.num_body_track = self.settings['track_body_number'] # track A frame if self.num_body_track == -1: self.quat0 = self.data.structure.timestep_info[ -1].quat.copy() self.for_vel0 = self.data.structure.timestep_info[ -1].for_vel.copy() else: # track a specific body self.quat0 = \ self.data.structure.timestep_info[-1].mb_quat[self.num_body_track,:].copy() self.for_vel0 = \ self.data.structure.timestep_info[-1].mb_FoR_vel[self.num_body_track ,:].copy() # convert to G frame self.Cga0 = algebra.quat2rotation(self.quat0) self.Cga = self.Cga0.copy() self.for_vel0[:3] = self.Cga0.dot(self.for_vel0[:3]) self.for_vel0[3:] = self.Cga0.dot(self.for_vel0[3:]) else: # check/record initial rotation speed self.num_body_track = None self.quat0 = None self.Cag0 = None self.Cga = None self.for_vel0 = np.zeros((6, )) # TODO: verify of a better way to implement rho aero_tstep.rho = self.settings['density'] # Generate instance of linuvlm.Dynamic() lin_uvlm_system = linuvlm.DynamicBlock( aero_tstep, dynamic_settings=self.settings, # dt=self.settings['dt'].value, # integr_order=self.settings['integr_order'].value, # ScalingDict=self.settings['ScalingDict'], # RemovePredictor=self.settings['remove_predictor'].value, # UseSparse=self.settings['use_sparse'].value, for_vel=self.for_vel0) # add rotational speed for ii in range(lin_uvlm_system.MS.n_surf): lin_uvlm_system.MS.Surfs[ii].omega = self.for_vel0[3:] # Save reference values # System Inputs u_0 = self.pack_input_vector() # Linearised state dt = self.settings['dt'] x_0 = self.pack_state_vector(aero_tstep, None, dt, self.settings['integr_order']) # Reference forces f_0 = np.concatenate([ aero_tstep.forces[ss][0:3].reshape(-1, order='C') for ss in range(aero_tstep.n_surf) ]) # Assemble the state space system lin_uvlm_system.assemble_ss() self.data.aero.linear['System'] = lin_uvlm_system self.data.aero.linear['SS'] = lin_uvlm_system.SS self.data.aero.linear['x_0'] = x_0 self.data.aero.linear['u_0'] = u_0 self.data.aero.linear['y_0'] = f_0 # self.data.aero.linear['gamma_0'] = gamma # self.data.aero.linear['gamma_star_0'] = gamma_star # self.data.aero.linear['gamma_dot_0'] = gamma_dot # TODO: Implement in AeroTimeStepInfo a way to store the state vectors # aero_tstep.linear.x = x_0 # aero_tstep.linear.u = u_0 # aero_tstep.linear.y = f_0 # Initialise 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'])
def get_body(self, beam, num_dof_ibody, ibody): """ get_body Extract the body number 'ibody' from a multibody system Given 'self' as a StructTimeStepInfo class of a multibody system, this function returns another StructTimeStepInfo class (ibody_StructTimeStepInfo) that only includes the body number 'ibody' of the original system Args: self(StructTimeStepInfo): timestep information of the multibody system beam(Beam): beam information of the multibody system ibody(int): body number to be extracted Returns: ibody_StructTimeStepInfo(StructTimeStepInfo): timestep information of the isolated body Examples: Notes: """ # Define the nodes and elements belonging to the body ibody_elems, ibody_nodes = mb.get_elems_nodes_list(beam, ibody) ibody_num_node = len(ibody_nodes) ibody_num_elem = len(ibody_elems) ibody_first_dof = 0 for index_body in range(ibody - 1): aux_elems, aux_nodes = mb.get_elems_nodes_list(beam, index_body) ibody_first_dof += np.sum(beam.vdof[aux_nodes] > -1) * 6 # Initialize the new StructTimeStepInfo ibody_StructTimeStepInfo = StructTimeStepInfo( ibody_num_node, ibody_num_elem, self.num_node_elem, num_dof=num_dof_ibody, num_bodies=beam.num_bodies) # Assign all the variables # ibody_StructTimeStepInfo.quat = self.quat.astype(dtype=ct.c_double, order='F', copy=True) # ibody_StructTimeStepInfo.for_pos = self.for_pos.astype(dtype=ct.c_double, order='F', copy=True) # ibody_StructTimeStepInfo.for_vel = self.for_vel.astype(dtype=ct.c_double, order='F', copy=True) # ibody_StructTimeStepInfo.for_acc = self.for_acc.astype(dtype=ct.c_double, order='F', copy=True) CAslaveG = algebra.quat2rotation(self.mb_quat[ibody, :]).T ibody_StructTimeStepInfo.quat = self.mb_quat[ibody, :].astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.for_pos = self.mb_FoR_pos[ibody, :].astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.for_vel[0:3] = np.dot( CAslaveG, self.mb_FoR_vel[ibody, 0:3]) ibody_StructTimeStepInfo.for_vel[3:6] = np.dot( CAslaveG, self.mb_FoR_vel[ibody, 3:6]) ibody_StructTimeStepInfo.for_acc[0:3] = np.dot( CAslaveG, self.mb_FoR_acc[ibody, 0:3]) ibody_StructTimeStepInfo.for_acc[3:6] = np.dot( CAslaveG, self.mb_FoR_acc[ibody, 3:6]) ibody_StructTimeStepInfo.pos = self.pos[ibody_nodes, :].astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.pos_dot = self.pos_dot[ibody_nodes, :].astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.psi = self.psi[ibody_elems, :, :].astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.psi_dot = self.psi_dot[ ibody_elems, :, :].astype(dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.gravity_vector_inertial = self.gravity_vector_inertial.astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.gravity_vector_body = self.gravity_vector_body.astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.steady_applied_forces = self.steady_applied_forces[ ibody_nodes, :].astype(dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.unsteady_applied_forces = self.unsteady_applied_forces[ ibody_nodes, :].astype(dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.gravity_forces = self.gravity_forces[ ibody_nodes, :].astype(dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.total_gravity_forces = self.total_gravity_forces.astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.q[0:num_dof_ibody.value] = self.q[ ibody_first_dof:ibody_first_dof + num_dof_ibody.value].astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.dqdt[0:num_dof_ibody.value] = self.dqdt[ ibody_first_dof:ibody_first_dof + num_dof_ibody.value].astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.dqddt[0:num_dof_ibody.value] = self.dqddt[ ibody_first_dof:ibody_first_dof + num_dof_ibody.value].astype( dtype=ct.c_double, order='F', copy=True) # ibody_StructTimeStepInfo.q[-10:] = self.q[-10:].astype(dtype=ct.c_double, order='F', copy=True) # ibody_StructTimeStepInfo.dqdt[-10:] = self.dqdt[-10:].astype(dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.dqdt[ -4:] = ibody_StructTimeStepInfo.quat.astype(dtype=ct.c_double, order='F', copy=True) # ibody_StructTimeStepInfo.dqddt[-10:] = self.dqddt[-10:].astype(dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.mb_quat = self.mb_quat.astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.mb_FoR_pos = self.mb_FoR_pos.astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.mb_FoR_vel = self.mb_FoR_vel.astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.mb_FoR_acc = self.mb_FoR_acc.astype( dtype=ct.c_double, order='F', copy=True) ibody_StructTimeStepInfo.mb_dqddt_quat = self.mb_dqddt_quat.astype( dtype=ct.c_double, order='F', copy=True) return ibody_StructTimeStepInfo
def change_to_global_AFoR(self, global_ibody): """ change_to_global_AFoR Reference a StructTimeStepInfo to the global A frame of reference Given 'self' as a StructTimeStepInfo class, this function references it to the global A frame of reference Args: self(StructTimeStepInfo): timestep information global_ibody(int): body number (as defined in the mutibody system) to be modified Returns: Examples: Notes: """ # Define the rotation matrices between the different FoR CAslaveG = algebra.quat2rotation(self.mb_quat[global_ibody, :]).T CGAmaster = algebra.quat2rotation(self.mb_quat[0, :]) Csm = np.dot(CAslaveG, CGAmaster) delta_pos_ms = self.mb_FoR_pos[global_ibody, :] - self.mb_FoR_pos[0, :] delta_vel_ms = self.mb_FoR_vel[global_ibody, :] - self.mb_FoR_vel[0, :] for inode in range(self.pos.shape[0]): pos_previous = self.pos[inode, :] + np.zeros((3, ), ) self.pos[inode, :] = ( np.dot(np.transpose(Csm), self.pos[inode, :]) + np.dot(np.transpose(CGAmaster), delta_pos_ms[0:3])) self.pos_dot[inode, :] = ( np.dot(np.transpose(Csm), self.pos_dot[inode, :]) + np.dot(np.transpose(CGAmaster), delta_vel_ms[0:3]) + np.dot( Csm.T, np.dot( algebra.skew( np.dot(CAslaveG, self.mb_FoR_vel[global_ibody, 3:6])), pos_previous)) - np.dot( algebra.skew(np.dot(CGAmaster.T, self.mb_FoR_vel[0, 3:6])), self.pos[inode, :])) self.gravity_forces[inode, 0:3] = np.dot(Csm.T, self.gravity_forces[inode, 0:3]) self.gravity_forces[inode, 3:6] = np.dot(Csm.T, self.gravity_forces[inode, 3:6]) # np.cross(np.dot(CGAmaster.T, delta_vel_ms[3:6]), pos_previous)) for ielem in range(self.psi.shape[0]): for inode in range(3): psi_previous = self.psi[ielem, inode, :] + np.zeros((3, ), ) self.psi[ielem, inode, :] = algebra.rotation2crv( np.dot(Csm.T, algebra.crv2rotation(self.psi[ielem, inode, :]))) self.psi_dot[ielem, inode, :] = np.dot( algebra.crv2tan(self.psi[ielem, inode, :]), (np.dot( Csm.T, np.dot( algebra.crv2tan(psi_previous).T, self.psi_dot[ielem, inode, :])) + np.dot( algebra.quat2rotation(self.mb_quat[0, :]).T, delta_vel_ms[3:6]))) # Set the output FoR variables self.for_pos = self.mb_FoR_pos[0, :].astype(dtype=ct.c_double, order='F', copy=True) self.for_vel[0:3] = np.dot(np.transpose(CGAmaster), self.mb_FoR_vel[0, 0:3]) self.for_vel[3:6] = np.dot(np.transpose(CGAmaster), self.mb_FoR_vel[0, 3:6]) self.for_acc[0:3] = np.dot(np.transpose(CGAmaster), self.mb_FoR_acc[0, 0:3]) self.for_acc[3:6] = np.dot(np.transpose(CGAmaster), self.mb_FoR_acc[0, 3:6]) self.quat = self.mb_quat[0, :].astype(dtype=ct.c_double, order='F', copy=True)
def polars(data, aero_kstep, structural_kstep, struct_forces, **kwargs): r""" This function corrects the aerodynamic forces from UVLM based on the airfoil polars provided by the user in the aero.h5 file These are the steps needed to correct the forces: * The force coming from UVLM is divided into induced drag (parallel to the incoming flow velocity) and lift (the remaining force). * The angle of attack is computed based on that lift force and the angle of zero lift computed form the airfoil polar and assuming a slope of :math:`2 \pi` * The drag force is computed based on the angle of attack and the polars provided by the user Args: data (:class:`sharpy.PreSharpy`): SHARPy data aero_kstep (:class:`sharpy.utils.datastructures.AeroTimeStepInfo`): Current aerodynamic substep structural_kstep (:class:`sharpy.utils.datastructures.StructTimeStepInfo`): Current structural substep struct_forces (np.array): Array with the aerodynamic forces mapped on the structure in the B frame of reference Keyword Arguments: rho (float): air density correct_lift (bool): correct also lift coefficient according to the polars cd_from_cl (bool): interpolate drag from lift instead of computing the AoA first Returns: np.ndarray: corresponding aerodynamic force at the structural node from the force and moment at a grid vertex """ aerogrid = data.aero beam = data.structure rho = kwargs.get('rho', 1.225) correct_lift = kwargs.get('correct_lift', False) cd_from_cl = kwargs.get('cd_from_cl', False) aero_dict = aerogrid.aero_dict if aerogrid.polars is None: return struct_forces new_struct_forces = np.zeros_like(struct_forces) nnode = struct_forces.shape[0] for inode in range(nnode): new_struct_forces[inode, :] = struct_forces[inode, :].copy() if aero_dict['aero_node'][inode]: ielem, inode_in_elem = beam.node_master_elem[inode] iairfoil = aero_dict['airfoil_distribution'][ielem, inode_in_elem] isurf = aerogrid.struct2aero_mapping[inode][0]['i_surf'] i_n = aerogrid.struct2aero_mapping[inode][0]['i_n'] N = aerogrid.aero_dimensions[isurf, 1] polar = aerogrid.polars[iairfoil] cab = algebra.crv2rotation(structural_kstep.psi[ielem, inode_in_elem, :]) cga = algebra.quat2rotation(structural_kstep.quat) cgb = np.dot(cga, cab) # Deal with the extremes if i_n == 0: node1 = 0 node2 = 1 elif i_n == N: node1 = nnode - 1 node2 = nnode - 2 else: node1 = inode + 1 node2 = inode - 1 # Define the span and the span direction dir_span = 0.5*np.dot(cga, structural_kstep.pos[node1, :] - structural_kstep.pos[node2, :]) span = np.linalg.norm(dir_span) dir_span = algebra.unit_vector(dir_span) # Define the chord and the chord direction dir_chord = aero_kstep.zeta[isurf][:, -1, i_n] - aero_kstep.zeta[isurf][:, 0, i_n] chord = np.linalg.norm(dir_chord) dir_chord = algebra.unit_vector(dir_chord) # Define the relative velocity and its direction urel = (structural_kstep.pos_dot[inode, :] + structural_kstep.for_vel[0:3] + np.cross(structural_kstep.for_vel[3:6], structural_kstep.pos[inode, :])) urel = -np.dot(cga, urel) urel += np.average(aero_kstep.u_ext[isurf][:, :, i_n], axis=1) # uind = uvlmlib.uvlm_calculate_total_induced_velocity_at_points(aero_kstep, # np.array([structural_kstep.pos[inode, :] - np.array([0, 0, 1])]), # structural_kstep.for_pos, # ct.c_uint(8))[0] # print(inode, urel, uind) # urel -= uind dir_urel = algebra.unit_vector(urel) # Force in the G frame of reference force = np.dot(cgb, struct_forces[inode, 0:3]) dir_force = algebra.unit_vector(force) # Coefficient to change from aerodynamic coefficients to forces (and viceversa) coef = 0.5*rho*np.linalg.norm(urel)**2*chord*span # Divide the force in drag and lift drag_force = np.dot(force, dir_urel)*dir_urel lift_force = force - drag_force # Compute the associated lift cl = np.linalg.norm(lift_force)/coef cd_sharpy = np.linalg.norm(drag_force)/coef if cd_from_cl: # Compute the drag from the lift cd, cm = polar.get_cdcm_from_cl(cl) else: # Compute the angle of attack assuming that UVLM gives a 2pi polar aoa_deg_2pi = polar.get_aoa_deg_from_cl_2pi(cl) # Compute the coefficients assocaited to that angle of attack cl_new, cd, cm = polar.get_coefs(aoa_deg_2pi) # print(cl, cl_new) if correct_lift: cl = cl_new # Recompute the forces based on the coefficients lift_force = cl*algebra.unit_vector(lift_force)*coef drag_force += cd*dir_urel*coef force = lift_force + drag_force new_struct_forces[inode, 0:3] = np.dot(cgb.T, force) return new_struct_forces
def cga(self): return algebra.quat2rotation(self.quat)
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
def test_quat_wrt_rot(self): """ We define: - G: initial frame - A: frame obtained upon rotation, Cga, defined by the quaternion q0 - B: frame obtained upon further rotation, Cab, of A defined by the "infinitesimal" Cartesian rotation vector dcrv The test verifies that: 1. the total rotation matrix Cgb(q0+dq) is equal to Cgb = Cga(q0) Cab(dcrv) where dq = algebra.der_quat_wrt_crv(q0) 2. the difference between analytically computed delta quaternion, dq, and the numerical delta dq_num = algebra.crv2quat(algebra.rotation2crv(Cgb_ref))-q0 is comparable to the step used to compute the delta dcrv 3. The equality: d(Cga(q0)*v)*dq = Cga(q0) * d(Cab*dv)*dcrv where d(Cga(q0)*v) and d(Cab*dv) are the derivatives computed through algebra.der_Cquat_by_v and algebra.der_Ccrv_by_v for a random vector v. Warning: - The relation dcrv->dquat is not uniquely defined. However, the resulting rotation matrix is, namely: Cga(q0+dq)=Cga(q0)*Cab(dcrv) """ ### case 1: simple rotation about the same axis # linearisation point a0 = 30. * np.pi / 180 n0 = np.array([0, 0, 1]) n0 = n0 / np.linalg.norm(n0) q0 = algebra.crv2quat(a0 * n0) Cga = algebra.quat2rotation(q0) # direction of perturbation n2 = n0 A = np.array([1e-2, 1e-3, 1e-4, 1e-5, 1e-6]) for a in A: drot = a * n2 # build rotation manually atot = a0 + a Cgb_exp = algebra.crv2rotation(atot * n0) # ok # build combined rotation Cab = algebra.crv2rotation(drot) Cgb_ref = np.dot(Cga, Cab) # verify expected vs combined rotation matrices assert np.linalg.norm(Cgb_exp - Cgb_ref) / a < 1e-8, \ 'Verify test case - these matrices need to be identical' # verify analytical rotation matrix dq_an = np.dot(algebra.der_quat_wrt_crv(q0), drot) Cgb_an = algebra.quat2rotation(q0 + dq_an) erel_rot = np.linalg.norm(Cgb_an - Cgb_ref) / a assert erel_rot < 3e-3, \ 'Relative error of rotation matrix (%.2e) too large!' % erel_rot # verify delta quaternion erel_dq = np.linalg.norm(Cgb_an - Cgb_ref) dq_num = algebra.crv2quat(algebra.rotation2crv(Cgb_ref)) - q0 erel_dq = np.linalg.norm(dq_num - dq_an) / np.linalg.norm(dq_an) / a assert erel_dq < .3, \ 'Relative error delta quaternion (%.2e) too large!' % erel_dq # verify algebraic relation v = np.ones((3, )) D1 = algebra.der_Cquat_by_v(q0, v) D2 = algebra.der_Ccrv_by_v(np.zeros((3, )), v) res = np.dot(D1, dq_num) - np.dot(np.dot(Cga, D2), drot) erel_res = np.linalg.norm(res) / a assert erel_res < 5e-1 * a, \ 'Relative error of residual (%.2e) too large!' % erel_res ### case 2: random rotation # linearisation point a0 = 30. * np.pi / 180 n0 = np.array([-2, -1, 1]) n0 = n0 / np.linalg.norm(n0) q0 = algebra.crv2quat(a0 * n0) Cga = algebra.quat2rotation(q0) # direction of perturbation n2 = np.array([0.5, 1., -2.]) n2 = n2 / np.linalg.norm(n2) A = np.array([1e-2, 1e-3, 1e-4, 1e-5, 1e-6]) for a in A: drot = a * n2 # build combined rotation Cab = algebra.crv2rotation(drot) Cgb_ref = np.dot(Cga, Cab) # verify analytical rotation matrix dq_an = np.dot(algebra.der_quat_wrt_crv(q0), drot) Cgb_an = algebra.quat2rotation(q0 + dq_an) erel_rot = np.linalg.norm(Cgb_an - Cgb_ref) / a assert erel_rot < 3e-3, \ 'Relative error of rotation matrix (%.2e) too large!' % erel_rot # verify delta quaternion erel_dq = np.linalg.norm(Cgb_an - Cgb_ref) dq_num = algebra.crv2quat(algebra.rotation2crv(Cgb_ref)) - q0 erel_dq = np.linalg.norm(dq_num - dq_an) / np.linalg.norm(dq_an) / a assert erel_dq < .3, \ 'Relative error delta quaternion (%.2e) too large!' % erel_dq # verify algebraic relation v = np.ones((3, )) D1 = algebra.der_Cquat_by_v(q0, v) D2 = algebra.der_Ccrv_by_v(np.zeros((3, )), v) res = np.dot(D1, dq_num) - np.dot(np.dot(Cga, D2), drot) erel_res = np.linalg.norm(res) / a assert erel_res < 5e-1 * a, \ 'Relative error of residual (%.2e) too large!' % erel_res
def run(self, aero_tstep, structure_tstep, convect_wake=False, dt=None, t=None, unsteady_contribution=False): r""" Solve the linear aerodynamic UVLM model at the current time step ``n``. The step increment is solved as: .. math:: \mathbf{x}^n &= \mathbf{A\,x}^{n-1} + \mathbf{B\,u}^n \\ \mathbf{y}^n &= \mathbf{C\,x}^n + \mathbf{D\,u}^n A change of state is possible in order to solve the system without the predictor term. In which case the system is solved by: .. math:: \mathbf{h}^n &= \mathbf{A\,h}^{n-1} + \mathbf{B\,u}^{n-1} \\ \mathbf{y}^n &= \mathbf{C\,h}^n + \mathbf{D\,u}^n Variations are taken with respect to initial reference state. The state and input vectors for the linear UVLM system are of the form: If ``integr_order==1``: .. math:: \mathbf{x}_n = [\delta\mathbf{\Gamma}^T_n,\, \delta\mathbf{\Gamma_w}_n^T,\, \Delta t \,\delta\mathbf{\dot{\Gamma}}_n^T]^T Else, if ``integr_order==2``: .. math:: \mathbf{x}_n = [\delta\mathbf{\Gamma}_n^T,\, \delta\mathbf{\Gamma_w}_n^T,\, \Delta t \,\delta\mathbf{\dot{\Gamma}}_n^T,\, \delta\mathbf{\Gamma}_{n-1}^T]^T And the input vector: .. math:: \mathbf{u}_n = [\delta\mathbf{\zeta}_n^T,\, \delta\dot{\mathbf{\zeta}}_n^T,\,\delta\mathbf{u_{ext}}^T_n]^T where the subscript ``n`` refers to the time step. The linear UVLM system is then solved as detailed in :func:`sharpy.linear.src.linuvlm.Dynamic.solve_step`. The output is a column vector containing the aerodynamic forces at the panel vertices. To Do: option for impulsive start? Args: aero_tstep (AeroTimeStepInfo): object containing the aerodynamic data at the current time step structure_tstep (StructTimeStepInfo): object containing the structural data at the current time step convect_wake (bool): for backward compatibility only. The linear UVLM assumes a frozen wake geometry dt (float): time increment t (float): current time unsteady_contribution (bool): (backward compatibily). Unsteady aerodynamic effects are always included Returns: PreSharpy: updated ``self.data`` class with the new forces and circulation terms of the system """ 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'] if t is None: t = self.data.ts * dt integr_order = self.settings['integr_order'] ### Define Input # Generate external velocity field u_ext 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) ### Proj from FoR G to linearisation frame # - proj happens in self.pack_input_vector and unpack_ss_vectors if self.settings['track_body']: # track A frame if self.num_body_track == -1: self.Cga = algebra.quat2rotation(structure_tstep.quat) else: # track a specific body self.Cga = algebra.quat2rotation( structure_tstep.mb_quat[self.num_body_track, :]) # Column vector that will be the input to the linearised UVLM system # Input is at time step n, since it is updated in the aeroelastic solver prior to aerodynamic solver u_n = self.pack_input_vector() du_n = u_n - self.data.aero.linear['u_0'] if self.settings['remove_predictor']: u_m1 = self.pack_input_vector() du_m1 = u_m1 - self.data.aero.linear['u_0'] else: du_m1 = None # Retrieve State vector at time n-1 if len(self.data.aero.timestep_info) < 2: x_m1 = self.pack_state_vector(aero_tstep, None, dt, integr_order) else: x_m1 = self.pack_state_vector(aero_tstep, self.data.aero.timestep_info[-2], dt, integr_order) # dx is at timestep n-1 dx_m1 = x_m1 - self.data.aero.linear['x_0'] ### Solve system - output is the variation in force dx_n, dy_n = self.data.aero.linear['System'].solve_step( dx_m1, du_m1, du_n, transform_state=True) x_n = self.data.aero.linear['x_0'] + dx_n y_n = self.data.aero.linear['y_0'] + dy_n # if self.settings['physical_model']: forces, gamma, gamma_dot, gamma_star = self.unpack_ss_vectors( y_n, x_n, u_n, aero_tstep) aero_tstep.forces = forces aero_tstep.gamma = gamma aero_tstep.gamma_dot = gamma_dot aero_tstep.gamma_star = gamma_star return self.data
def __init__(self, data, custom_settings_linear=None, uvlm_block=False, chosen_ts=None): self.data = data if custom_settings_linear is None: settings_here = data.settings['LinearUvlm'] else: settings_here = custom_settings_linear sharpy.utils.settings.to_custom_types(settings_here, linuvlm.settings_types_dynamic, linuvlm.settings_default_dynamic) if chosen_ts is None: self.chosen_ts = self.data.ts else: self.chosen_ts = chosen_ts ## TEMPORARY - NEED TO INCLUDE PROPER INTEGRATION OF SETTINGS try: self.rigid_body_motions = settings_here['rigid_body_motion'] except KeyError: self.rigid_body_motions = False try: self.use_euler = settings_here['use_euler'] except KeyError: self.use_euler = False if self.rigid_body_motions and settings_here['track_body']: self.track_body = True else: self.track_body = False ## ------- ### extract aeroelastic info self.dt = settings_here['dt'].value ### reference to timestep_info # aero aero = data.aero self.tsaero = aero.timestep_info[self.chosen_ts] # structure structure = data.structure self.tsstr = structure.timestep_info[self.chosen_ts] # --- backward compatibility try: rho = settings_here['density'].value except KeyError: warnings.warn( "Key 'density' not found in 'LinearUvlm' solver settings. '\ 'Trying to read it from 'StaticCoupled'." ) rho = data.settings['StaticCoupled']['aero_solver_settings']['rho'] if type(rho) == str: rho = np.float(rho) if hasattr(rho, 'value'): rho = rho.value self.tsaero.rho = rho # --- backward compatibility ### gebm if self.use_euler: self.num_dof_rig = 9 else: self.num_dof_rig = 10 self.num_dof_flex = np.sum(self.data.structure.vdof >= 0) * 6 self.num_dof_str = self.num_dof_flex + self.num_dof_rig self.reshape_struct_input() try: beam_settings = settings_here['beam_settings'] except KeyError: beam_settings = dict() self.lingebm_str = lingebm.FlexDynamic(self.tsstr, structure, beam_settings) cga = algebra.quat2rotation(self.tsstr.quat) ### uvlm if uvlm_block: self.linuvlm = linuvlm.DynamicBlock( self.tsaero, dt=settings_here['dt'].value, dynamic_settings=settings_here, RemovePredictor=settings_here['remove_predictor'].value, UseSparse=settings_here['use_sparse'].value, integr_order=settings_here['integr_order'].value, ScalingDict=settings_here['ScalingDict'], for_vel=np.hstack((cga.dot(self.tsstr.for_vel[:3]), cga.dot(self.tsstr.for_vel[3:])))) else: self.linuvlm = linuvlm.Dynamic( self.tsaero, dt=settings_here['dt'].value, dynamic_settings=settings_here, RemovePredictor=settings_here['remove_predictor'].value, UseSparse=settings_here['use_sparse'].value, integr_order=settings_here['integr_order'].value, ScalingDict=settings_here['ScalingDict'], for_vel=np.hstack((cga.dot(self.tsstr.for_vel[:3]), cga.dot(self.tsstr.for_vel[3:]))))
def derivatives(self, Y_freq): Cng = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]]) # Project SEU on NED - TODO implementation u_inf = self.settings['u_inf'].value s_ref = self.settings['S_ref'].value b_ref = self.settings['b_ref'].value c_ref = self.settings['c_ref'].value rho = self.data.linear.tsaero0.rho # Inertial frame try: euler = self.data.linear.tsstruct0.euler Pga = algebra.euler2rot(euler) rig_dof = 9 except AttributeError: quat = self.data.linear.tsstruct0.quat Pga = algebra.quat2rotation(quat) rig_dof = 10 derivatives_g = np.zeros((6, Y_freq.shape[1] + 2)) coefficients = { 'force': 0.5 * rho * u_inf**2 * s_ref, 'moment_lon': 0.5 * rho * u_inf**2 * s_ref * c_ref, 'moment_lat': 0.5 * rho * u_inf**2 * s_ref * b_ref, 'force_angular_vel': 0.5 * rho * u_inf**2 * s_ref * c_ref / u_inf, 'moment_lon_angular_vel': 0.5 * rho * u_inf**2 * s_ref * c_ref * c_ref / u_inf } # missing rates for in_channel in range(Y_freq.shape[1]): derivatives_g[:3, in_channel] = Pga.dot(Y_freq[:3, in_channel]) derivatives_g[3:, in_channel] = Pga.dot(Y_freq[3:, in_channel]) derivatives_g[:3, :3] /= coefficients['force'] derivatives_g[:3, 3:6] /= coefficients['force_angular_vel'] derivatives_g[4, :3] /= coefficients['moment_lon'] derivatives_g[4, 3:6] /= coefficients['moment_lon_angular_vel'] derivatives_g[[3, 5], :] /= coefficients['moment_lat'] derivatives_g[:, -2] = derivatives_g[:, 2] * u_inf # ders wrt alpha derivatives_g[:, -1] = derivatives_g[:, 1] * u_inf # ders wrt beta der_matrix = np.zeros((6, self.inputs - (rig_dof - 6))) der_col = 0 for i in list(range(6)) + list(range(rig_dof, self.inputs)): der_matrix[:3, der_col] = Y_freq[:3, i] der_matrix[3:6, der_col] = Y_freq[3:6, i] der_col += 1 labels_force = {0: 'X', 1: 'Y', 2: 'Z', 3: 'L', 4: 'M', 5: 'N'} labels_velocity = { 0: 'u', 1: 'v', 2: 'w', 3: 'p', 4: 'q', 5: 'r', 6: 'flap1', 7: 'flap2', 8: 'flap3' } table = cout.TablePrinter( n_fields=7, field_length=12, field_types=['s', 'f', 'f', 'f', 'f', 'f', 'f']) table.print_header(['der'] + list(labels_force.values())) for i in range(der_matrix.shape[1]): table.print_line([labels_velocity[i]] + list(der_matrix[:, i])) table_coeff = cout.TablePrinter(n_fields=7, field_length=12, field_types=['s'] + 6 * ['f']) labels_out = { 0: 'C_D', 1: 'C_Y', 2: 'C_L', 3: 'C_l', 4: 'C_m', 5: 'C_n' } labels_der = { 0: 'u', 1: 'v', 2: 'w', 3: 'p', 4: 'q', 5: 'r', 6: 'alpha', 7: 'beta' } table_coeff.print_header(['der'] + list(labels_out.values())) for i in range(6): table_coeff.print_line([labels_der[i]] + list(derivatives_g[:, i])) table_coeff.print_line([labels_der[6]] + list(derivatives_g[:, -2])) table_coeff.print_line([labels_der[7]] + list(derivatives_g[:, -1])) return der_matrix, derivatives_g
def merge_multibody(MB_tstep, MB_beam, beam, tstep, mb_data_dict, dt): """ merge_multibody This functions merges a series of bodies into a multibody system at a certain time step Longer description Args: MB_beam (list of beam): each entry represents a body MB_tstep (list of StructTimeStepInfo): each entry represents a body beam (beam): structural information of the multibody system tstep (StructTimeStepInfo): timestep information of the multibody system mb_data_dict (): Dictionary including the multibody information dt(int): time step Returns: beam (beam): structural information of the multibody system tstep (StructTimeStepInfo): timestep information of the multibody system Examples: Notes: """ update_mb_dB_before_merge(tstep, MB_tstep) first_dof = 0 for ibody in range(beam.num_bodies): # Renaming for clarity ibody_elems = MB_beam[ibody].global_elems_num ibody_nodes = MB_beam[ibody].global_nodes_num # Merge tstep MB_tstep[ibody].change_to_global_AFoR(ibody) tstep.pos[ibody_nodes, :] = MB_tstep[ibody].pos.astype( dtype=ct.c_double, order='F', copy=True) tstep.pos_dot[ibody_nodes, :] = MB_tstep[ibody].pos_dot.astype( dtype=ct.c_double, order='F', copy=True) tstep.psi[ibody_elems, :, :] = MB_tstep[ibody].psi.astype( dtype=ct.c_double, order='F', copy=True) tstep.psi_dot[ibody_elems, :, :] = MB_tstep[ibody].psi_dot.astype( dtype=ct.c_double, order='F', copy=True) tstep.gravity_forces[ ibody_nodes, :] = MB_tstep[ibody].gravity_forces.astype( dtype=ct.c_double, order='F', copy=True) # TODO: Do I need a change in FoR for the following variables? Maybe for the FoR ones. tstep.forces_constraints_nodes[ ibody_nodes, :] = MB_tstep[ibody].forces_constraints_nodes.astype( dtype=ct.c_double, order='F', copy=True) tstep.forces_constraints_FoR[ibody, :] = MB_tstep[ ibody].forces_constraints_FoR[ibody, :].astype(dtype=ct.c_double, order='F', copy=True) # Merge states ibody_num_dof = MB_beam[ibody].num_dof.value tstep.q[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].q[:-10].astype( dtype=ct.c_double, order='F', copy=True) tstep.dqdt[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].dqdt[:-10].astype( dtype=ct.c_double, order='F', copy=True) tstep.dqddt[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].dqddt[:-10].astype( dtype=ct.c_double, order='F', copy=True) first_dof += ibody_num_dof tstep.q[-10:] = MB_tstep[0].q[-10:].astype(dtype=ct.c_double, order='F', copy=True) tstep.dqdt[-10:] = MB_tstep[0].dqdt[-10:].astype(dtype=ct.c_double, order='F', copy=True) tstep.dqddt[-10:] = MB_tstep[0].dqddt[-10:].astype(dtype=ct.c_double, order='F', copy=True) # Define the new FoR information CAG = algebra.quat2rotation(tstep.quat).T tstep.for_pos = tstep.mb_FoR_pos[0, :].astype(dtype=ct.c_double, order='F', copy=True) tstep.for_vel[0:3] = np.dot(CAG, tstep.mb_FoR_vel[0, 0:3]) tstep.for_vel[3:6] = np.dot(CAG, tstep.mb_FoR_vel[0, 3:6]) tstep.for_acc[0:3] = np.dot(CAG, tstep.mb_FoR_acc[0, 0:3]) tstep.for_acc[3:6] = np.dot(CAG, tstep.mb_FoR_acc[0, 3:6]) tstep.quat = tstep.mb_quat[0, :].astype(dtype=ct.c_double, order='F', copy=True)
def state2disp(q, dqdt, dqddt, MB_beam, MB_tstep): """ state2disp Recovers the displacements from the states Longer description Args: MB_beam (list of beam): each entry represents a body MB_tstep (list of StructTimeStepInfo): each entry represents a body q(numpy array): Vector of states dqdt(numpy array): Time derivatives of states dqddt(numpy array): Second time derivatives of states Returns: Examples: Notes: """ first_dof = 0 for ibody in range(len(MB_beam)): ibody_num_dof = MB_beam[ibody].num_dof.value if (MB_beam[ibody].FoR_movement == 'prescribed'): MB_tstep[ibody].q[:-10] = q[first_dof:first_dof + ibody_num_dof].astype( dtype=ct.c_double, order='F', copy=True) MB_tstep[ibody].dqdt[:-10] = dqdt[first_dof:first_dof + ibody_num_dof].astype( dtype=ct.c_double, order='F', copy=True) MB_tstep[ibody].dqddt[:-10] = dqddt[first_dof:first_dof + ibody_num_dof].astype( dtype=ct.c_double, order='F', copy=True) xbeamlib.cbeam3_solv_state2disp(MB_beam[ibody], MB_tstep[ibody]) first_dof += ibody_num_dof elif (MB_beam[ibody].FoR_movement == 'free'): MB_tstep[ibody].q = q[first_dof:first_dof + ibody_num_dof + 10].astype(dtype=ct.c_double, order='F', copy=True) # dqdt[first_dof+ibody_num_dof+6:first_dof+ibody_num_dof+10] = algebra.unit_quat(dqdt[first_dof+ibody_num_dof+6:first_dof+ibody_num_dof+10]) MB_tstep[ibody].dqdt = dqdt[first_dof:first_dof + ibody_num_dof + 10].astype(dtype=ct.c_double, order='F', copy=True) MB_tstep[ibody].dqddt = dqddt[first_dof:first_dof + ibody_num_dof + 10].astype(dtype=ct.c_double, order='F', copy=True) xbeamlib.xbeam_solv_state2disp(MB_beam[ibody], MB_tstep[ibody]) # if onlyFlex: # xbeamlib.cbeam3_solv_state2disp(MB_beam[ibody], MB_tstep[ibody]) # else: # xbeamlib.xbeam_solv_state2disp(MB_beam[ibody], MB_tstep[ibody]) first_dof += ibody_num_dof + 10 for ibody in range(len(MB_beam)): CAslaveG = algebra.quat2rotation(MB_tstep[ibody].quat).T # MB_tstep[0].mb_FoR_pos[ibody,:] = MB_tstep[ibody].for_pos.astype(dtype=ct.c_double, order='F', copy=True) MB_tstep[0].mb_FoR_vel[ibody, 0:3] = np.dot(CAslaveG.T, MB_tstep[ibody].for_vel[0:3]) MB_tstep[0].mb_FoR_vel[ibody, 3:6] = np.dot(CAslaveG.T, MB_tstep[ibody].for_vel[3:6]) MB_tstep[0].mb_FoR_acc[ibody, 0:3] = np.dot(CAslaveG.T, MB_tstep[ibody].for_acc[0:3]) MB_tstep[0].mb_FoR_acc[ibody, 3:6] = np.dot(CAslaveG.T, MB_tstep[ibody].for_acc[3:6]) MB_tstep[0].mb_quat[ibody, :] = MB_tstep[ibody].quat.astype( dtype=ct.c_double, order='F', copy=True) for ibody in range(len(MB_beam)): # MB_tstep[ibody].mb_FoR_pos = MB_tstep[0].mb_FoR_pos.astype(dtype=ct.c_double, order='F', copy=True) MB_tstep[ibody].mb_FoR_vel = MB_tstep[0].mb_FoR_vel.astype( dtype=ct.c_double, order='F', copy=True) MB_tstep[ibody].mb_FoR_acc = MB_tstep[0].mb_FoR_acc.astype( dtype=ct.c_double, order='F', copy=True) MB_tstep[ibody].mb_quat = MB_tstep[0].mb_quat.astype(dtype=ct.c_double, order='F', copy=True)
def test_rotation_matrices(self): """ Checks routines and consistency of functions to generate rotation matrices. Note: test only includes triad <-> CRV <-> quaternions conversions """ ### Verify that function build rotation matrix (not projection matrix) # set an easy rotation (x axis) a = np.pi / 6. nv = np.array([1, 0, 0]) sa, ca = np.sin(a), np.cos(a) Cab_exp = np.array([ [1, 0, 0], [0, ca, -sa], [0, sa, ca], ]) ### rot from triad Cab_num = algebra.triad2rotation(Cab_exp[:, 0], Cab_exp[:, 1], Cab_exp[:, 2]) assert np.linalg.norm(Cab_num - Cab_exp) < 1e-15, \ 'crv2rotation not producing the right result' ### rot from crv fv = a * nv Cab_num = algebra.crv2rotation(fv) assert np.linalg.norm(Cab_num - Cab_exp) < 1e-15, \ 'crv2rotation not producing the right result' ### rot from quat quat = algebra.crv2quat(fv) Cab_num = algebra.quat2rotation(quat) assert np.linalg.norm(Cab_num - Cab_exp) < 1e-15, \ 'quat2rotation not producing the right result' ### inverse relations # check crv2rotation and rotation2crv are biunivolcal in [-pi,pi] # check quat2rotation and rotation2quat are biunivocal in [-pi,pi] N = 100 for nn in range(N): # def random rotation in [-pi,pi] a = np.pi * (2. * np.random.rand() - 1) nv = 2. * np.random.rand(3) - 1 nv = nv / np.linalg.norm(nv) # inverse crv fv0 = a * nv Cab = algebra.crv2rotation(fv0) fv = algebra.rotation2crv(Cab) assert np.linalg.norm(fv - fv0) < 1e-12, \ 'rotation2crv not producing the right result' # triad2crv xa, ya, za = Cab[:, 0], Cab[:, 1], Cab[:, 2] assert np.linalg.norm( algebra.triad2crv(xa, ya, za) - fv0) < 1e-12, \ 'triad2crv not producing the right result' # inverse quat quat0 = np.zeros((4, )) quat0[0] = np.cos(.5 * a) quat0[1:] = np.sin(.5 * a) * nv quat = algebra.rotation2quat(algebra.quat2rotation(quat0)) assert np.linalg.norm(quat - quat0) < 1e-12, \ 'rotation2quat not producing the right result' ### combined rotation # assume 3 FoR, G, A and B where: # - G is the initial FoR # - A derives from a 90 deg rotation about zG # - B derives from a 90 deg rotation about yA crv_G_to_A = .5 * np.pi * np.array([0, 0, 1]) crv_A_to_B = .5 * np.pi * np.array([0, 1, 0]) Cga = algebra.crv2rotation(crv_G_to_A) Cab = algebra.crv2rotation(crv_A_to_B) # rotation G to B (i.e. projection B onto G) Cgb = np.dot(Cga, Cab) Cgb_exp = np.array([[0, -1, 0], [0, 0, 1], [-1, 0, 0]]) assert np.linalg.norm(Cgb - Cgb_exp) < 1e-15, \ 'combined rotation not as expected!'
def get_gebm2uvlm_gains(self): """ Gain matrix to transfer GEBM dofs to UVLM lattice vertices and stiffening term due to non-zero forces at the linearisation point. The function produces the matrices: - ``Kdisp``: 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. - ``Kss``: stiffness factor accounting for non-zero forces at the linearisation point. (flexible dof -> flexible dof) - ``Ksr``: stiffness factor accounting for non-zero forces at the linearisation point. (rigid dof -> flexible dof) Notes: - The following terms have been verified against SHARPy (to ensure same sign conventions and accuracy): - :math:`\\mathbf{C}^{AB}` - accuracy of :math:`X^B=\\mathbf{C}^{AB}*X^A` - accuracy of :math:`X^G` and :math:`X^A` """ data = self.data aero = self.data.aero structure = self.data.structure # data.aero.beam tsaero = self.tsaero tsstr = self.tsstr # allocate output Kdisp = np.zeros((3 * self.linuvlm.Kzeta, self.num_dof_str)) Kvel_disp = np.zeros((3 * self.linuvlm.Kzeta, self.num_dof_str)) Kvel_vel = np.zeros((3 * self.linuvlm.Kzeta, self.num_dof_str)) Kforces = np.zeros((self.num_dof_str, 3 * self.linuvlm.Kzeta)) Kss = np.zeros((self.num_dof_flex, self.num_dof_flex)) Ksr = np.zeros((self.num_dof_flex, self.num_dof_rig)) # get projection matrix A->G # (and other quantities indep. from nodal position) Cga = algebra.quat2rotation(tsstr.quat) Cag = Cga.T # for_pos=tsstr.for_pos for_tra = 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) # GEBM degrees of freedom jj_for_tra = range(self.num_dof_str - 10, self.num_dof_str - 7) jj_for_rot = range(self.num_dof_str - 7, self.num_dof_str - 4) jj_quat = range(self.num_dof_str - 4, self.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) # 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 # 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) ### 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.linuvlm.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 aero force faero = tsaero.forces[ss][:3, mm, nn] # 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) Tan = algebra.crv2tan(psi) XbskewTan = np.dot(Xbskew, Tan) # get velocity terms zetag_dot = tsaero.zeta_dot[ ss][:, mm, nn] # 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 ### ---------------------------------------- allocate Kdisp if bc_here != 1: # wrt pos Kdisp[np.ix_(ii_vert, jj_tra)] += Cga # wrt psi 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) Kdisp[np.ix_(ii_vert, jj_quat)] = \ algebra.der_Cquat_by_v(tsstr.quat, zetaa) ### ------------------------------------ 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)) 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) Kvel_disp[np.ix_(ii_vert, jj_quat)] = \ algebra.der_Cquat_by_v(tsstr.quat, zetaa_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)] += Cbg # nodal moments Kforces[np.ix_(jj_rot, ii_vert)] += \ 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 if bc_here != 1: # forces Dfdcrv = algebra.der_CcrvT_by_v( psi, np.dot(Cag, faero)) Dfdquat = np.dot( Cba, algebra.der_CquatT_by_v(tsstr.quat, faero)) Kss[np.ix_(jj_tra, jj_rot)] -= Dfdcrv Ksr[jj_tra, -4:] -= Dfdquat # moments Kss[np.ix_(jj_rot, jj_rot)] -= np.dot(Xbskew, Dfdcrv) Ksr[jj_rot, -4:] -= np.dot(Xbskew, Dfdquat) # embed() # transfer self.Kdisp = Kdisp self.Kvel_disp = Kvel_disp self.Kvel_vel = Kvel_vel self.Kforces = Kforces # stiffening factors self.Kss = Kss self.Ksr = Ksr
def get_mode_zeta(data, eigvect): """ Retrieves the UVLM grid nodal displacements associated to the eigenvector ``eigvect`` """ ### initialise aero = data.aero struct = data.structure tsaero = data.aero.timestep_info[data.ts] tsstr = data.structure.timestep_info[data.ts] try: num_dof = struct.num_dof.value except AttributeError: num_dof = struct.num_dof eigvect = eigvect[:num_dof] zeta_mode = [] for ss in range(aero.n_surf): zeta_mode.append(tsaero.zeta[ss].copy()) jj = 0 # structural dofs index Cga0 = algebra.quat2rotation(tsstr.quat) Cag0 = Cga0.T for node_glob in range(struct.num_node): ### detect bc at node (and no. of dofs) bc_here = struct.boundary_conditions[node_glob] if bc_here == 1: # clamp dofs_here = 0 continue elif bc_here == -1 or bc_here == 0: dofs_here = 6 jj_tra = [jj, jj + 1, jj + 2] jj_rot = [jj + 3, jj + 4, jj + 5] jj += dofs_here # retrieve element and local index ee, node_loc = struct.node_master_elem[node_glob, :] # get original position and crv Ra0 = tsstr.pos[node_glob, :] psi0 = tsstr.psi[ee, node_loc, :] Rg0 = np.dot(Cga0, Ra0) Cab0 = algebra.crv2rotation(psi0) Cbg0 = np.dot(Cab0.T, Cag0) # update position and crv of mode Ra = tsstr.pos[node_glob, :] + eigvect[jj_tra] psi = tsstr.psi[ee, node_loc, :] + eigvect[jj_rot] Rg = np.dot(Cga0, Ra) Cab = algebra.crv2rotation(psi) Cbg = np.dot(Cab.T, Cag0) ### 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] for mm in range(M + 1): # get position of vertex in B FoR zetag0 = tsaero.zeta[ss][:, mm, nn] # in G FoR, w.r.t. origin A-G Xb = np.dot(Cbg0, zetag0 - Rg0) # in B FoR, w.r.t. origin B # update vertex position zeta_mode[ss][:, mm, nn] = Rg + np.dot(np.dot(Cga0, Cab), Xb) return zeta_mode
def generate(self, linuvlm=None, tsaero0=None, tsstruct0=None, aero=None, structure=None): """ Generates a matrix mapping a linear control surface deflection onto the aerodynamic grid. The parsing of arguments is temporary since this state space element will include a full actuator model. The parsing of arguments is optional if the class has been previously initialised. Args: linuvlm: tsaero0: tsstruct0: aero: structure: Returns: """ if self.aero is not None: aero = self.aero structure = self.structure linuvlm = self.linuvlm tsaero0 = self.tsaero0 tsstruct0 = self.tsstruct0 # Find the vertices corresponding to a control surface from beam coordinates to aerogrid aero_dict = aero.aero_dict n_surf = aero.timestep_info[0].n_surf n_control_surfaces = self.n_control_surfaces if self.under_development: import matplotlib.pyplot as plt # Part of the testing process Kdisp = np.zeros((3 * linuvlm.Kzeta, n_control_surfaces)) Kvel = np.zeros((3 * linuvlm.Kzeta, n_control_surfaces)) Kmom = np.zeros((3 * linuvlm.Kzeta, n_control_surfaces)) zeta0 = np.concatenate([tsaero0.zeta[i_surf].reshape(-1, order='C') for i_surf in range(n_surf)]) Cga = algebra.quat2rotation(tsstruct0.quat).T Cag = Cga.T # Initialise these parameters hinge_axis = None # Will be set once per control surface to the hinge axis with_control_surface = False # Will be set to true if the spanwise node contains a control surface for global_node in range(structure.num_node): # Retrieve elements and local nodes to which a single node is attached for i_elem in range(structure.num_elem): if global_node in structure.connectivities[i_elem, :]: i_local_node = np.where(structure.connectivities[i_elem, :] == global_node)[0][0] for_delta = structure.frame_of_reference_delta[i_elem, :, 0] # CRV to transform from G to B frame psi = tsstruct0.psi[i_elem, i_local_node] Cab = algebra.crv2rotation(psi) Cba = Cab.T Cbg = np.dot(Cab.T, Cag) Cgb = Cbg.T # print(global_node) if self.under_development: print('Node -- ' + str(global_node)) # Map onto aerodynamic coordinates. Some nodes may be part of two aerodynamic surfaces. This will happen # at the surface boundary for structure2aero_node in aero.struct2aero_mapping[global_node]: # Retrieve surface and span-wise coordinate i_surf, i_node_span = structure2aero_node['i_surf'], structure2aero_node['i_n'] # Surface panelling M = aero.aero_dimensions[i_surf][0] N = aero.aero_dimensions[i_surf][1] K_zeta_start = 3 * sum(linuvlm.MS.KKzeta[:i_surf]) shape_zeta = (3, M + 1, N + 1) i_control_surface = aero_dict['control_surface'][i_elem, i_local_node] if i_control_surface >= 0: if not with_control_surface: i_start_of_cs = i_node_span.copy() with_control_surface = True control_surface_chord = aero_dict['control_surface_chord'][i_control_surface] i_node_hinge = M - control_surface_chord i_vertex_hinge = [K_zeta_start + np.ravel_multi_index((i_axis, i_node_hinge, i_node_span), shape_zeta) for i_axis in range(3)] i_vertex_next_hinge = [K_zeta_start + np.ravel_multi_index((i_axis, i_node_hinge, i_start_of_cs + 1), shape_zeta) for i_axis in range(3)] zeta_hinge = zeta0[i_vertex_hinge] zeta_next_hinge = zeta0[i_vertex_next_hinge] if hinge_axis is None: # Hinge axis not yet set for current control surface # Hinge axis is in G frame hinge_axis = zeta_next_hinge - zeta_hinge hinge_axis = hinge_axis / np.linalg.norm(hinge_axis) for i_node_chord in range(M + 1): i_vertex = [K_zeta_start + np.ravel_multi_index((i_axis, i_node_chord, i_node_span), shape_zeta) for i_axis in range(3)] if i_node_chord > i_node_hinge: # Zeta in G frame zeta_node = zeta0[i_vertex] # Gframe zeta_nodeA = Cag.dot(zeta_node) zeta_hingeA = Cag.dot(zeta_hinge) zeta_hingeB = Cbg.dot(zeta_hinge) zeta_nodeB = Cbg.dot(zeta_node) chord_vec = (zeta_node - zeta_hinge) if self.under_development: print('G Frame') print('Hinge axis = ' + str(hinge_axis)) print('\tHinge = ' + str(zeta_hinge)) print('\tNode = ' + str(zeta_node)) print('A Frame') print('\tHinge = ' + str(zeta_hingeA)) print('\tNode = ' + str(zeta_nodeA)) print('B Frame') print('\tHinge axis = ' + str(Cbg.dot(hinge_axis))) print('\tHinge = ' + str(zeta_hingeB)) print('\tNode = ' + str(zeta_nodeB)) print('Chordwise Vector') print('GVec = ' + str(chord_vec/np.linalg.norm(chord_vec))) print('BVec = ' + str(Cbg.dot(chord_vec/np.linalg.norm(chord_vec)))) # pass # Removing the += because cs where being added twice Kdisp[i_vertex, i_control_surface] = \ Cgb.dot(der_R_arbitrary_axis_times_v(Cbg.dot(hinge_axis), 0, -for_delta * Cbg.dot(chord_vec))) # Kdisp[i_vertex, i_control_surface] = \ # der_R_arbitrary_axis_times_v(hinge_axis, 0, chord_vec) # Flap velocity Kvel[i_vertex, i_control_surface] = -algebra.skew(chord_vec).dot( hinge_axis) # Flap hinge moment - future work # Kmom[i_vertex, i_control_surface] += algebra.skew(chord_vec) # Testing progress if self.under_development: plt.scatter(zeta_hingeB[1], zeta_hingeB[2], color='k') plt.scatter(zeta_nodeB[1], zeta_nodeB[2], color='b') # plt.scatter(zeta_hinge[1], zeta_hinge[2], color='k') # plt.scatter(zeta_node[1], zeta_node[2], color='b') # Testing out delta = 5*np.pi/180 # zeta_newB = Cbg.dot(Kdisp[i_vertex, 1].dot(delta)) + zeta_nodeB zeta_newB = Cbg.dot(Kdisp[i_vertex, -1].dot(delta)) + zeta_nodeB plt.scatter(zeta_newB[1], zeta_newB[2], color='r') old_vector = zeta_nodeB - zeta_hingeB new_vector = zeta_newB - zeta_hingeB angle = np.arccos(new_vector.dot(old_vector) / (np.linalg.norm(new_vector) * np.linalg.norm(old_vector))) print(angle) if self.under_development: plt.axis('equal') plt.show() else: with_control_surface = False hinge_axis = None # Reset for next control surface self.Kzeta_delta = Kdisp self.Kdzeta_ddelta = Kvel # self.Kmom = Kmom return Kdisp, Kvel
def generate(self): """ Generates a matrix mapping a linear control surface deflection onto the aerodynamic grid. This generates two matrices: * `Kzeta_delta` maps the deflection angle onto displacements. It has as many columns as independent control surfaces. * `Kdzeta_ddelta` maps the deflection rate onto grid velocities. Again, it has as many columns as independent control surfaces. Returns: tuple: Tuple containing `Kzeta_delta` and `Kdzeta_ddelta`. """ # For future development # In hindsight, building this matrix iterating through structural node was a big mistake that # has led to very messy code. Would rework by element and in B frame aero = self.aero structure = self.structure linuvlm = self.linuvlm tsaero0 = self.tsaero0 tsstruct0 = self.tsstruct0 # Find the vertices corresponding to a control surface from beam coordinates to aerogrid aero_dict = aero.aero_dict n_surf = tsaero0.n_surf n_control_surfaces = self.n_control_surfaces Kdisp = np.zeros((3 * linuvlm.Kzeta, n_control_surfaces)) Kvel = np.zeros((3 * linuvlm.Kzeta, n_control_surfaces)) zeta0 = np.concatenate([tsaero0.zeta[i_surf].reshape(-1, order='C') for i_surf in range(n_surf)]) Cga = algebra.quat2rotation(tsstruct0.quat).T Cag = Cga.T # Initialise these parameters hinge_axis = None # Will be set once per control surface to the hinge axis with_control_surface = False # Will be set to true if the spanwise node contains a control surface for global_node in range(structure.num_node): # Retrieve elements and local nodes to which a single node is attached for i_elem in range(structure.num_elem): if global_node in structure.connectivities[i_elem, :]: i_local_node = np.where(structure.connectivities[i_elem, :] == global_node)[0][0] for_delta = structure.frame_of_reference_delta[i_elem, :, 0] # CRV to transform from G to B frame psi = tsstruct0.psi[i_elem, i_local_node] Cab = algebra.crv2rotation(psi) Cba = Cab.T Cbg = np.dot(Cab.T, Cag) Cgb = Cbg.T # Map onto aerodynamic coordinates. Some nodes may be part of two aerodynamic surfaces. for structure2aero_node in aero.struct2aero_mapping[global_node]: # Retrieve surface and span-wise coordinate i_surf, i_node_span = structure2aero_node['i_surf'], structure2aero_node['i_n'] # Although a node may be part of 2 aerodynamic surfaces, we need to ensure that the current # element for the given node is indeed part of that surface. elems_in_surf = np.where(aero_dict['surface_distribution'] == i_surf)[0] if i_elem not in elems_in_surf: continue # Surface panelling M = aero.aero_dimensions[i_surf][0] N = aero.aero_dimensions[i_surf][1] K_zeta_start = 3 * sum(linuvlm.MS.KKzeta[:i_surf]) shape_zeta = (3, M + 1, N + 1) i_control_surface = aero_dict['control_surface'][i_elem, i_local_node] if i_control_surface >= 0: if not with_control_surface: i_start_of_cs = i_node_span.copy() with_control_surface = True control_surface_chord = aero_dict['control_surface_chord'][i_control_surface] try: control_surface_hinge_coord = \ aero_dict['control_surface_hinge_coord'][i_control_surface] * \ aero_dict['chord'][i_elem, i_local_node] except KeyError: control_surface_hinge_coord = None i_node_hinge = M - control_surface_chord i_vertex_hinge = [K_zeta_start + np.ravel_multi_index((i_axis, i_node_hinge, i_node_span), shape_zeta) for i_axis in range(3)] i_vertex_next_hinge = [K_zeta_start + np.ravel_multi_index((i_axis, i_node_hinge, i_start_of_cs + 1), shape_zeta) for i_axis in range(3)] if control_surface_hinge_coord is not None and M == control_surface_chord: # fully articulated control surface zeta_hinge = Cgb.dot(Cba.dot(tsstruct0.pos[global_node]) + for_delta * np.array([0, control_surface_hinge_coord, 0])) zeta_next_hinge = Cgb.dot(Cbg.dot(zeta_hinge) + np.array([1, 0, 0])) # parallel to the x_b vector else: zeta_hinge = zeta0[i_vertex_hinge] zeta_next_hinge = zeta0[i_vertex_next_hinge] if hinge_axis is None: # Hinge axis not yet set for current control surface # Hinge axis is in G frame hinge_axis = zeta_next_hinge - zeta_hinge hinge_axis = hinge_axis / np.linalg.norm(hinge_axis) for i_node_chord in range(M + 1): i_vertex = [K_zeta_start + np.ravel_multi_index((i_axis, i_node_chord, i_node_span), shape_zeta) for i_axis in range(3)] if i_node_chord >= i_node_hinge: # Zeta in G frame zeta_node = zeta0[i_vertex] # Gframe chord_vec = (zeta_node - zeta_hinge) Kdisp[i_vertex, i_control_surface] = \ Cgb.dot(der_R_arbitrary_axis_times_v(Cbg.dot(hinge_axis), 0, -for_delta * Cbg.dot(chord_vec))) * for_delta * -1 # Flap velocity Kvel[i_vertex, i_control_surface] = -algebra.skew(-for_delta * chord_vec).dot( hinge_axis) * for_delta * -1 else: with_control_surface = False hinge_axis = None # Reset for next control surface # >>>> Merge control surfaces 0 and 1 # Kdisp[:, 0] -= Kdisp[:, 1] # Kvel[:, 0] -= Kvel[:, 1] self.Kzeta_delta = Kdisp self.Kdzeta_ddelta = Kvel return Kdisp, Kvel
int(np.round(100*RollVecDeg[ii])) ) case_here=case_main+'_ainf%.4da%.4ds%.4dr%.4d'%tplparams route_here=route_main os.system('mkdir -p %s'%(route_here,)) ### ------------------------------------------------------ Build wing model ws=flying_wings.Goland( M=M,N=N,Mstar_fact=Mstar_fact,n_surfaces=Nsurf, u_inf=u_inf, alpha=AlphaVecDeg[ii], beta=-SideVecDeg[ii], route=route_here, case_name=case_here) # updte wind direction quat_wind=algebra.euler2quat(-np.pi/180.*np.array([0.,AlphaInfVecDeg[ii],0.])) u_inf_dir=np.dot( algebra.quat2rotation(quat_wind),np.array([1.,0.,0.])) ws.main_ea-=.25/M ws.main_cg-=.25/M ws.root_airfoil_P = 4 ws.root_airfoil_M = 2 ws.tip_airfoil_P = 4 ws.tip_airfoil_M = 2 ws.clean_test_files() ws.update_derived_params() ws.generate_fem_file() ws.generate_aero_file() ### solution flow
def update_orientation(self, quat, ts=-1): rot = algebra.quat2rotation(quat) self.timestep_info[ts].update_orientation(rot.T)
def test_rotation_matrices_derivatives(self): """ Checks derivatives of rotation matrix derivatives with respect to quaternions and Cartesian rotation vectors Note: test only includes CRV <-> quaternions conversions """ ### linearisation point # fi0=np.pi/6 # nv0=np.array([1,3,1]) fi0 = 2.0 * np.pi * random.random() - np.pi nv0 = np.array([random.random(), random.random(), random.random()]) nv0 = nv0 / np.linalg.norm(nv0) fv0 = fi0 * nv0 qv0 = algebra.crv2quat(fv0) ev0 = algebra.quat2euler(qv0) # direction of perturbation # fi1=np.pi/3 # nv1=np.array([-2,4,1]) fi1 = 2.0 * np.pi * random.random() - np.pi nv1 = np.array([random.random(), random.random(), random.random()]) nv1 = nv1 / np.linalg.norm(nv1) fv1 = fi1 * nv1 qv1 = algebra.crv2quat(fv1) ev1 = algebra.quat2euler(qv1) # linearsation point Cga0 = algebra.quat2rotation(qv0) Cag0 = Cga0.T Cab0 = algebra.crv2rotation(fv0) Cba0 = Cab0.T Cga0_euler = algebra.euler2rot(ev0) Cag0_euler = Cga0_euler.T # derivatives # xv=np.ones((3,)) # dummy vector xv = np.array([random.random(), random.random(), random.random()]) # dummy vector derCga = algebra.der_Cquat_by_v(qv0, xv) derCag = algebra.der_CquatT_by_v(qv0, xv) derCab = algebra.der_Ccrv_by_v(fv0, xv) derCba = algebra.der_CcrvT_by_v(fv0, xv) derCga_euler = algebra.der_Ceuler_by_v(ev0, xv) derCag_euler = algebra.der_Peuler_by_v(ev0, xv) A = np.array([1e-1, 1e-2, 1e-3, 1e-4, 1e-5, 1e-6]) er_ag = 10. er_ga = 10. er_ab = 10. er_ba = 10. er_ag_euler = 10. er_ga_euler = 10. for a in A: # perturbed qv = a * qv1 + (1. - a) * qv0 fv = a * fv1 + (1. - a) * fv0 ev = a * ev1 + (1. - a) * ev0 dqv = qv - qv0 dfv = fv - fv0 dev = ev - ev0 Cga = algebra.quat2rotation(qv) Cag = Cga.T Cab = algebra.crv2rotation(fv) Cba = Cab.T Cga_euler = algebra.euler2rot(ev) Cag_euler = Cga_euler.T dCag_num = np.dot(Cag - Cag0, xv) dCga_num = np.dot(Cga - Cga0, xv) dCag_an = np.dot(derCag, dqv) dCga_an = np.dot(derCga, dqv) er_ag_new = np.max(np.abs(dCag_num - dCag_an)) er_ga_new = np.max(np.abs(dCga_num - dCga_an)) dCab_num = np.dot(Cab - Cab0, xv) dCba_num = np.dot(Cba - Cba0, xv) dCab_an = np.dot(derCab, dfv) dCba_an = np.dot(derCba, dfv) er_ab_new = np.max(np.abs(dCab_num - dCab_an)) er_ba_new = np.max(np.abs(dCba_num - dCba_an)) dCag_num_euler = np.dot(Cag_euler - Cag0_euler, xv) dCga_num_euler = np.dot(Cga_euler - Cga0_euler, xv) dCag_an_euler = np.dot(derCag_euler, dev) dCga_an_euler = np.dot(derCga_euler, dev) er_ag_euler_new = np.max(np.abs(dCag_num_euler - dCag_an_euler)) er_ga_euler_new = np.max(np.abs(dCga_num_euler - dCga_an_euler)) assert er_ga_new < er_ga, 'der_Cquat_by_v error not converging to 0' assert er_ag_new < er_ag, 'der_CquatT_by_v error not converging to 0' assert er_ab_new < er_ab, 'der_Ccrv_by_v error not converging to 0' assert er_ba_new < er_ba, 'der_CcrvT_by_v error not converging to 0' assert er_ga_euler_new < er_ga_euler, 'der_Ceuler_by_v error not converging to 0' assert er_ag_euler_new < er_ag_euler, 'der_Peuler_by_v error not converging to 0' er_ag = er_ag_new er_ga = er_ga_new er_ab = er_ab_new er_ba = er_ba_new er_ag_euler = er_ag_euler_new er_ga_euler = er_ga_euler_new assert er_ga < A[-2], 'der_Cquat_by_v error too large' assert er_ag < A[-2], 'der_CquatT_by_v error too large' assert er_ab < A[-2], 'der_Ccrv_by_v error too large' assert er_ba < A[-2], 'der_CcrvT_by_v error too large' assert er_ag_euler < A[-2], 'der_Peuler_by_v error too large' assert er_ga_euler < A[-2], 'der_Ceuler_by_v error too large'