diff --git a/sharpy/controllers/bladepitchpid.py b/sharpy/controllers/bladepitchpid.py index 92f3c6a2c..3f1a1f515 100644 --- a/sharpy/controllers/bladepitchpid.py +++ b/sharpy/controllers/bladepitchpid.py @@ -423,7 +423,7 @@ def drive_train_model(self, aero_torque, ini_rot_vel, ini_rot_acc): def compute_aero_torque(self, beam, struct_tstep): # Compute total forces - total_forces = np.zeros((6)) + total_forces = np.zeros(6) for ibody in self.settings['blade_num_body']: steady, unsteady, grav = struct_tstep.extract_resultants(beam, force_type=['steady', 'unsteady', 'grav'], @@ -436,9 +436,9 @@ def compute_aero_torque(self, beam, struct_tstep): hub_node = beam.connectivities[hub_elem, 0] hub_pos = struct_tstep.pos[hub_node, :] - hub_forces = np.zeros((6)) - hub_forces[0:3] = total_forces[0:3].copy() - hub_forces[3:6] = total_forces[3:6] - np.cross(hub_pos, total_forces[0:3]) + hub_forces = np.zeros(6) + hub_forces[:3] = total_forces[:3].copy() + hub_forces[3:6] = total_forces[3:6] - np.cross(hub_pos, total_forces[:3]) return hub_forces[5] @@ -450,11 +450,11 @@ def compute_blade_pitch(beam, struct_tstep, tower_ibody=0, blade_ibody=1): ielem, inode_in_elem = beam.node_master_elem[tt_node] ca0b = algebra.crv2rotation(struct_tstep.psi[ielem, inode_in_elem, :]) cga0 = algebra.quat2rotation(struct_tstep.mb_quat[tower_ibody, :]) - zg_tower_top = algebra.multiply_matrices(cga0, ca0b, np.array([0., 0., 1.])) + zg_tower_top = cga0 @ ca0b @ np.array([0., 0., 1.]) # blade root cga = algebra.quat2rotation(struct_tstep.mb_quat[blade_ibody, :]) - zg_hub = algebra.multiply_matrices(cga, np.array([0., 0., 1.])) + zg_hub = cga @ np.array([0., 0., 1.]) pitch = algebra.angle_between_vectors(zg_tower_top, zg_hub) return pitch diff --git a/sharpy/postproc/aerogridplot.py b/sharpy/postproc/aerogridplot.py index f860459f3..a66fc86f5 100644 --- a/sharpy/postproc/aerogridplot.py +++ b/sharpy/postproc/aerogridplot.py @@ -199,7 +199,16 @@ def plot_body(self): counter += 1 coords[counter, :] = aero_tstep.zeta[i_surf][:, i_m, i_n] if self.settings['include_rbm']: - coords[counter, :] += struct_tstep.for_pos[0:3] + #TODO: fix for lack of g frame description in nonlineardynamicmultibody.py + if struct_tstep.mb_dict is None: + coords[counter, :] += struct_tstep.for_pos[0:3] + else: + #TODO: uncomment for dynamic trim + # try: + # cga = algebra.euler2rot([0, self.data.trimmed_values[0], 0]) + # coords[counter, :] += np.dot(cga, struct_tstep.for_pos[0:3]) + # except AttributeError: + coords[counter, :] += np.dot(struct_tstep.cga(), struct_tstep.for_pos[0:3]) if self.settings['include_forward_motion']: coords[counter, 0] -= self.settings['dt']*self.ts*self.settings['u_inf'] @@ -313,7 +322,16 @@ def plot_wake(self): counter += 1 coords[counter, :] = self.data.aero.timestep_info[self.ts].zeta_star[i_surf][:, i_m, i_n] if self.settings['include_rbm']: - coords[counter, :] += self.data.structure.timestep_info[self.ts].for_pos[0:3] + #TODO: fix for lack of g frame description in nonlineardynamicmultibody.py + if self.data.structure.timestep_info[self.ts].mb_dict is None: + coords[counter, :] += self.data.structure.timestep_info[self.ts].for_pos[0:3] + else: + #TODO: uncomment for dynamic trim + # try: + # cga = algebra.euler2rot([0, self.data.trimmed_values[0], 0]) + # coords[counter, :] += np.dot(cga, self.data.structure.timestep_info[self.ts].for_pos[0:3]) + # except AttributeError: + coords[counter, :] += np.dot(self.data.structure.timestep_info[self.ts].cga(), self.data.structure.timestep_info[self.ts].for_pos[0:3]) if self.settings['include_forward_motion']: coords[counter, 0] -= self.settings['dt']*self.ts*self.settings['u_inf'] diff --git a/sharpy/postproc/beamplot.py b/sharpy/postproc/beamplot.py index a6c8e588a..0e21c0763 100644 --- a/sharpy/postproc/beamplot.py +++ b/sharpy/postproc/beamplot.py @@ -124,7 +124,6 @@ def write_beam(self, it): num_nodes = self.data.structure.num_node num_elem = self.data.structure.num_elem - coords = np.zeros((num_nodes, 3)) conn = np.zeros((num_elem, 3), dtype=int) node_id = np.zeros((num_nodes,), dtype=int) elem_id = np.zeros((num_elem,), dtype=int) @@ -148,6 +147,18 @@ def write_beam(self, it): # coordinates of corners coords = tstep.glob_pos(include_rbm=self.settings['include_rbm']) + if tstep.mb_dict is None: + pass + else: + #TODO: fix for lack of g frame description in nonlineardynamicmultibody.py + for i_node in range(tstep.num_node): + #TODO: uncomment for dynamic trim + # try: + # c = algebra.euler2rot([0, self.data.trimmed_values[0], 0]) + # except AttributeError: + c = self.data.structure.timestep_info[0].cga() + coords[i_node, :] += np.dot(c, tstep.for_pos[0:3]) + # check if I can output gravity forces with_gravity = False try: @@ -172,9 +183,6 @@ def write_beam(self, it): pass # count number of arguments - postproc_cell_keys = tstep.postproc_cell.keys() - postproc_cell_vals = tstep.postproc_cell.values() - postproc_cell_scalar = [] postproc_cell_vector = [] postproc_cell_6vector = [] for k, v in tstep.postproc_cell.items(): @@ -189,8 +197,6 @@ def write_beam(self, it): else: raise AttributeError('Only scalar and 3-vector types supported in beamplot') # count number of arguments - postproc_node_keys = tstep.postproc_node.keys() - postproc_node_vals = tstep.postproc_node.values() postproc_node_scalar = [] postproc_node_vector = [] postproc_node_6vector = [] diff --git a/sharpy/postproc/writevariablestime.py b/sharpy/postproc/writevariablestime.py index 2a3cd4a8d..c0a445536 100644 --- a/sharpy/postproc/writevariablestime.py +++ b/sharpy/postproc/writevariablestime.py @@ -265,6 +265,28 @@ def write(self, it): for ivariable in range(len(self.settings['structure_variables'])): if self.settings['structure_variables'][ivariable] == '': continue + #TODO: fix for lack of g frame description in nonlineardynamicmultibody.py + if tstep.mb_dict is None: + var = getattr(tstep, self.settings['structure_variables'][ivariable]) + else: + if self.settings['structure_variables'][ivariable] == 'for_pos': + import sharpy.utils.algebra as ag + #TODO: uncomment for dynamic trim + # try: + # # import pdb + # # pdb.set_trace() + # cga = ag.euler2rot([0, self.data.trimmed_values[0], 0]) + # cag = cga.T + # tstep.for_pos[0:3] = np.dot(cga,tstep.for_pos[0:3]) + # var = getattr(tstep, self.settings['structure_variables'][ivariable]).copy() + # tstep.for_pos[0:3] = np.dot(cag,tstep.for_pos[0:3]) + # except AttributeError: + t0step = self.data.structure.timestep_info[0] + tstep.for_pos[0:3] = np.dot(t0step.cga(), tstep.for_pos[0:3]) + var = getattr(tstep, self.settings['structure_variables'][ivariable]).copy() + tstep.for_pos[0:3] = np.dot(t0step.cag(), tstep.for_pos[0:3]) + else: + var = getattr(tstep, self.settings['structure_variables'][ivariable]) var = getattr(tstep, self.settings['structure_variables'][ivariable]) num_indices = len(var.shape) if num_indices == 1: diff --git a/sharpy/structure/utils/lagrangeconstraints.py b/sharpy/structure/utils/lagrangeconstraints.py index 9ca9e34ab..25d3ab208 100644 --- a/sharpy/structure/utils/lagrangeconstraints.py +++ b/sharpy/structure/utils/lagrangeconstraints.py @@ -63,6 +63,7 @@ def lagrangeconstraint(arg): dict_of_lc[arg._lc_id] = arg return arg + def print_available_lc(): """ Prints the available Lagrange Constraints @@ -71,12 +72,14 @@ def print_available_lc(): for name, i_lc in dict_of_lc.items(): cout.cout_wrap('%s ' % i_lc._lc_id, 2) + def lc_from_string(string): """ Returns the ``BaseLagrangeConstraint`` class associated to a constraint id (``_lc_id``) """ return dict_of_lc[string] + def lc_list_from_path(cwd): onlyfiles = [f for f in os.listdir(cwd) if os.path.isfile(os.path.join(cwd, f))] @@ -199,9 +202,10 @@ def define_node_dof(MB_beam, node_body, num_node): node_dof += MB_beam[ibody].num_dof.value if MB_beam[ibody].FoR_movement == 'free': node_dof += 10 - node_dof += 6*MB_beam[node_body].vdof[num_node] + node_dof += 6 * MB_beam[node_body].vdof[num_node] return node_dof + def define_FoR_dof(MB_beam, FoR_body): """ define_FoR_dof @@ -228,7 +232,8 @@ def define_FoR_dof(MB_beam, FoR_body): ################################################################################ # Equations ################################################################################ -def equal_pos_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, inode_in_body, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q): +def equal_pos_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, inode_in_body, node_FoR_dof, node_dof, FoR_dof, sys_size, + Lambda, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q): """ This function generates the stiffness and damping matrices and the independent vector associated to a constraint that imposes equal positions between a node and a frame of reference @@ -243,11 +248,12 @@ def equal_pos_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, inode_in_body, no Note: this equation constitutes a holonomic constraint which is not currently supported. Check ``equal_lin_vel_node_FoR`` """ - cout.cout_wrap("WARNING: this equation constitutes a holonomic constraint which is not currently supported. Check ``equal_lin_vel_node_FoR``", 3) + cout.cout_wrap( + "WARNING: this equation constitutes a holonomic constraint which is not currently supported. Check ``equal_lin_vel_node_FoR``", + 3) num_LM_eq_specific = 3 - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Simplify notation node_cga = MB_tstep[node_body].cga() @@ -256,48 +262,53 @@ def equal_pos_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, inode_in_body, no FoR_pos = MB_tstep[FoR_body].for_pos[0:3] # if MB_beam[node_body].FoR_movement == 'free': - B[:, node_FoR_dof:node_FoR_dof+3] = np.eye(3) - B[:, node_dof:node_dof+3] = node_cga - B[:, FoR_dof:FoR_dof+3] = -np.eye(3) + B[:, node_FoR_dof:node_FoR_dof + 3] = np.eye(3) + B[:, node_dof:node_dof + 3] = node_cga + B[:, FoR_dof:FoR_dof + 3] = -np.eye(3) - LM_K[sys_size + ieq : sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor*B - LM_K[:sys_size, sys_size + ieq : sys_size + ieq + num_LM_eq_specific] += scalingFactor*np.transpose(B) + LM_K[sys_size + ieq: sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * B + LM_K[:sys_size, sys_size + ieq: sys_size + ieq + num_LM_eq_specific] += scalingFactor * B.T - LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(B), Lambda[ieq:ieq+num_LM_eq_specific]) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(node_FoR_pos + - np.dot(node_cga, node_pos) - - FoR_pos) + LM_Q[:sys_size] += scalingFactor * B.T @ Lambda[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += scalingFactor * (node_FoR_pos + node_cga @ node_pos - FoR_pos) - LM_C[node_dof:node_dof+3, node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[node_body].quat, Lambda[ieq : ieq + num_LM_eq_specific]) + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] += scalingFactor * ag.der_CquatT_by_v( + MB_tstep[node_body].quat, Lambda[ieq: ieq + num_LM_eq_specific]) if penaltyFactor: - q = np.zeros((sys_size, )) - q[node_FoR_dof:node_FoR_dof+3] = node_FoR_pos - q[node_dof:node_dof+3] = node_pos - q[FoR_dof:FoR_dof+3] = FoR_pos - - LM_Q[:sys_size] += penaltyFactor*np.dot(B.T, np.dot(B, q)) - - LM_K[node_FoR_dof:node_FoR_dof+3, node_FoR_dof:node_FoR_dof+3] += penaltyFactor*np.eye(3) - LM_K[node_FoR_dof:node_FoR_dof+3, node_dof:node_dof+3] += penaltyFactor*node_cga - LM_K[node_FoR_dof:node_FoR_dof+3, FoR_dof:FoR_dof+3] += -penaltyFactor*np.eye(3) - LM_C[node_FoR_dof:node_FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*ag.der_Cquat_by_v(MB_tstep[node_body].quat, node_pos) - - LM_K[node_dof:node_dof+3, node_FoR_dof:node_FoR_dof+3] += penaltyFactor*node_cga.T - LM_K[node_dof:node_dof+3, node_dof:node_dof+3] += penaltyFactor*np.eye(3) - LM_K[node_dof:node_dof+3, FoR_dof:FoR_dof+3] += -penaltyFactor*node_cga.T - LM_C[node_dof:node_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*(ag.der_CquatT_by_v(MB_tstep[node_body].quat, node_FoR_pos - FoR_pos)) - - LM_K[FoR_dof:FoR_dof+3, node_FoR_dof:node_FoR_dof+3] += -penaltyFactor*np.eye(3) - LM_K[FoR_dof:FoR_dof+3, node_dof:node_dof+3] += -penaltyFactor*node_cga.T - LM_K[FoR_dof:FoR_dof+3, FoR_dof:FoR_dof+3] += penaltyFactor*np.eye(3) - LM_C[FoR_dof:FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += -penaltyFactor*ag.der_Cquat_by_v(MB_tstep[node_body].quat, node_pos) + q = np.zeros((sys_size,)) + q[node_FoR_dof:node_FoR_dof + 3] = node_FoR_pos + q[node_dof:node_dof + 3] = node_pos + q[FoR_dof:FoR_dof + 3] = FoR_pos + + LM_Q[:sys_size] += penaltyFactor * B.T @ B @ q + + LM_K[node_FoR_dof:node_FoR_dof + 3, node_FoR_dof:node_FoR_dof + 3] += penaltyFactor * np.eye(3) + LM_K[node_FoR_dof:node_FoR_dof + 3, node_dof:node_dof + 3] += penaltyFactor * node_cga + LM_K[node_FoR_dof:node_FoR_dof + 3, FoR_dof:FoR_dof + 3] += -penaltyFactor * np.eye(3) + LM_C[node_FoR_dof:node_FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] += penaltyFactor * ag.der_Cquat_by_v( + MB_tstep[node_body].quat, node_pos) + + LM_K[node_dof:node_dof + 3, node_FoR_dof:node_FoR_dof + 3] += penaltyFactor * node_cga.T + LM_K[node_dof:node_dof + 3, node_dof:node_dof + 3] += penaltyFactor * np.eye(3) + LM_K[node_dof:node_dof + 3, FoR_dof:FoR_dof + 3] += -penaltyFactor * node_cga.T + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] += penaltyFactor * ( + ag.der_CquatT_by_v(MB_tstep[node_body].quat, node_FoR_pos - FoR_pos)) + + LM_K[FoR_dof:FoR_dof + 3, node_FoR_dof:node_FoR_dof + 3] += -penaltyFactor * np.eye(3) + LM_K[FoR_dof:FoR_dof + 3, node_dof:node_dof + 3] += -penaltyFactor * node_cga.T + LM_K[FoR_dof:FoR_dof + 3, FoR_dof:FoR_dof + 3] += penaltyFactor * np.eye(3) + LM_C[FoR_dof:FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] += -penaltyFactor * ag.der_Cquat_by_v( + MB_tstep[node_body].quat, node_pos) ieq += 3 return ieq -def equal_lin_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_posB = np.zeros((3))): +def equal_lin_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, + sys_size, Lambda_dot, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, + rel_posB=np.zeros((3))): """ This function generates the stiffness and damping matrices and the independent vector associated to a constraint that imposes equal linear velocities between a node and a frame of reference @@ -315,8 +326,7 @@ def equal_lin_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number, """ num_LM_eq_specific = 3 - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Simplify notation node_cga = MB_tstep[node_body].cga() @@ -326,93 +336,455 @@ def equal_lin_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number, ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] psi = MB_tstep[node_body].psi[ielem, inode_in_elem, :] node_cab = ag.crv2rotation(psi) - node_Ra = MB_tstep[node_body].pos[node_number,:] + np.dot(node_cab, rel_posB) + node_Ra = MB_tstep[node_body].pos[node_number, :] + node_cab @ rel_posB - node_dot_Ra = MB_tstep[node_body].pos_dot[node_number,:] + node_dot_Ra = MB_tstep[node_body].pos_dot[node_number, :] FoR_cga = MB_tstep[FoR_body].cga() FoR_va = MB_tstep[FoR_body].for_vel[0:3] - Bnh[:, FoR_dof:FoR_dof+3] = FoR_cga - Bnh[:, node_dof:node_dof+3] = -1.0*node_cga + Bnh[:, FoR_dof:FoR_dof + 3] = FoR_cga + Bnh[:, node_dof:node_dof + 3] = -1. * node_cga if MB_beam[node_body].FoR_movement == 'free': - Bnh[:, node_FoR_dof:node_FoR_dof+3] = -1.0*node_cga - Bnh[:, node_FoR_dof+3:node_FoR_dof+6] = np.dot(node_cga,ag.skew(node_Ra)) + Bnh[:, node_FoR_dof:node_FoR_dof + 3] = -1. * node_cga + Bnh[:, node_FoR_dof + 3:node_FoR_dof + 6] = node_cga @ ag.skew(node_Ra) - LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh - LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * Bnh.T - LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(np.dot(FoR_cga, FoR_va) + - -1.0*np.dot(node_cga, - node_dot_Ra + - node_FoR_va + - -1.0*np.dot(ag.skew(node_Ra), node_FoR_wa))) + LM_Q[:sys_size] += scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += scalingFactor * (FoR_cga @ FoR_va - node_cga @ (node_dot_Ra + node_FoR_va - ag.skew(node_Ra) @ node_FoR_wa)) - LM_C[FoR_dof:FoR_dof+3, FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, Lambda_dot[ieq:ieq+num_LM_eq_specific]) + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += scalingFactor * ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, Lambda_dot[ieq:ieq + num_LM_eq_specific]) if MB_beam[node_body].FoR_movement == 'free': - LM_C[node_dof:node_dof+3,node_FoR_dof+6:node_FoR_dof+10] -= scalingFactor*ag.der_CquatT_by_v(MB_tstep[node_body].quat, Lambda_dot[ieq:ieq+num_LM_eq_specific]) + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] -= scalingFactor * ag.der_CquatT_by_v( + MB_tstep[node_body].quat, Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_C[node_FoR_dof:node_FoR_dof+3,node_FoR_dof+6:node_FoR_dof+10] -= scalingFactor*ag.der_CquatT_by_v(MB_tstep[node_body].quat,Lambda_dot[ieq:ieq+num_LM_eq_specific]) + LM_C[node_FoR_dof:node_FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] -= scalingFactor * ag.der_CquatT_by_v( + MB_tstep[node_body].quat, Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_C[node_FoR_dof+3:node_FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*np.dot(ag.skew(node_Ra).T, - ag.der_CquatT_by_v(MB_tstep[node_body].quat, - Lambda_dot[ieq:ieq+num_LM_eq_specific])) + LM_C[node_FoR_dof + 3:node_FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += (scalingFactor * ag.skew(node_Ra).T + @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, Lambda_dot[ieq:ieq + num_LM_eq_specific])) - LM_K[node_FoR_dof+3:node_FoR_dof+6,node_dof:node_dof+3] += scalingFactor*ag.skew(np.dot(node_cga.T,Lambda_dot[ieq:ieq+num_LM_eq_specific])) + # non-trivial - verified by hand (involves multiple transformations, Dynamics of Flexible Aircraft Appen. C) + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof:node_dof + 3] += scalingFactor * ag.skew( + node_cga.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific]) if penaltyFactor: - q = np.zeros((sys_size)) - q[FoR_dof:FoR_dof+3] = FoR_va - q[node_dof:node_dof+3] = node_dot_Ra if MB_beam[node_body].FoR_movement == 'free': - q[node_FoR_dof:node_FoR_dof+3] = node_FoR_va - q[node_FoR_dof+3:node_FoR_dof+6] = node_FoR_wa + # TODO: follow general approach to derive terms - first 4*4 terms, then LMC derivatives, then LMK derivatives - this is why penalty didn't work! - LM_Q[:sys_size] += penaltyFactor*np.dot(np.dot(Bnh.T, Bnh), q) + # Simplify notation + node_cga = MB_tstep[node_body].cga() + FoR_cga = MB_tstep[FoR_body].cga() + psi_dot = MB_tstep[node_body].psi_dot[ielem, inode_in_elem, :] - LM_C[:sys_size, :sys_size] += penaltyFactor*np.dot(Bnh.T, Bnh) + q = np.zeros((sys_size)) + q[FoR_dof:FoR_dof + 3] = FoR_va + q[node_dof:node_dof + 3] = node_dot_Ra + q[node_dof + 3:node_dof + 6] = psi_dot + q[node_FoR_dof:node_FoR_dof + 3] = node_FoR_va + q[node_FoR_dof + 3:node_FoR_dof + 6] = node_FoR_wa - # Derivatives wrt the FoR quaterion - LM_C[FoR_dof:FoR_dof+3, FoR_dof+6:FoR_dof+10] -= penaltyFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, - np.dot(node_cga, node_dot_Ra + node_FoR_va + - np.dot(ag.skew(node_Ra), node_FoR_wa))) + LM_Q[:sys_size] += penaltyFactor * Bnh.T @ Bnh @ q - LM_C[node_dof:node_dof+3, FoR_dof+6:FoR_dof+10] -= penaltyFactor*np.dot(node_cga.T, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, - FoR_va)) + # # 16 canonical terms for (abcd)^T(abcd) + LM_C[:sys_size, :sys_size] += penaltyFactor * Bnh.T @ Bnh - if MB_beam[node_body].FoR_movement == 'free': - LM_C[node_FoR_dof:node_FoR_dof+3, FoR_dof+6:FoR_dof+10] -= penaltyFactor*np.dot(node_cga.T, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, - FoR_va)) + # other LM_C derivatives for c dependencies in x1 and x2 + # term 1-x1 - \frac{\partial}{\partial x_1}(a^Taq_1 + a^Tbq2 + a^Tcq3 + a^Tdq4) + # da^Tdxaq_1 + a^Tdadxaq_1 + da^Tdxbq_2 + a^Tdbdxq_2 + da^Tdxcq_3 + a^Tdcdxq_3 + da^Tdxdq_4 - mat = ag.multiply_matrices(ag.skew(node_Ra).T, node_cga.T) - LM_C[node_FoR_dof+3:node_FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, - FoR_va)) + mat = -np.eye(3) + vec = -node_cga @ node_dot_Ra - # Derivatives wrt the node quaternion - if MB_beam[node_body].FoR_movement == 'free': - vec = -node_dot_Ra - node_FoR_va + np.dot(ag.skew(node_Ra), node_FoR_wa) - LM_C[FoR_dof:FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec)) + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) - derivative = -ag.der_CquatT_by_v(MB_tstep[node_body].quat, np.dot(FoR_cga, FoR_va)) - LM_C[node_dof:node_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*derivative - LM_C[node_FoR_dof:node_FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*derivative - LM_C[node_FoR_dof+3:node_FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] -= penaltyFactor*np.dot(ag.skew(node_Ra), derivative) + mat = node_cga.T + vec = node_dot_Ra - # Derivatives wrt the node Ra - LM_K[FoR_dof:FoR_dof+3, node_dof:node_dof+3] -= penaltyFactor*ag.multiply_matrices(FoR_cga.T, node_cga, ag.skew(node_FoR_wa)) - LM_K[node_dof:node_dof+3, node_dof:node_dof+3] += penaltyFactor*ag.skew(node_FoR_wa) - if MB_beam[node_body].FoR_movement == 'free': - LM_K[node_FoR_dof:node_FoR_dof+3, node_dof:node_dof+3] += penaltyFactor*ag.skew(node_FoR_wa) - vec = ag.multiply_matrices(node_cga.T, FoR_cga, FoR_va) - node_dot_Ra - node_FoR_va - LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof:node_dof+3] += penaltyFactor*ag.skew(vec) - LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof:node_dof+3] -= penaltyFactor*ag.der_skewp_skewp_v(node_Ra, node_FoR_wa) + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = -np.eye(3) + vec = -node_cga @ node_FoR_va + + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + mat = node_cga.T + vec = node_FoR_va + + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = -np.eye(3) + vec = node_cga @ ag.skew(node_Ra) @ node_FoR_wa + + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + mat = -node_cga.T + vec = ag.skew(node_Ra) @ node_FoR_wa + + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = -np.eye(3) + vec = FoR_cga @ FoR_va + + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + # term 2-x1 - \frac{\partial}{\partial x_1}(b^Taq_1 + b^Tbq2 + b^Tcq3 + b^Tdq4) + # db^Tdxaq_1 + b^Tdadxaq_1 + db^Tdxbq_2 + b^Tdbdxq_2 + db^Tdxcq_3 + b^Tdcdxq_3 + db^Tdxdq_4 + + mat = -np.eye(3) + vec = -node_cga @ node_dot_Ra + + LM_C[node_FoR_dof:node_FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + mat = node_cga.T + vec = node_dot_Ra + + LM_C[node_FoR_dof:node_FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = -np.eye(3) + vec = -node_cga @ node_FoR_va + + LM_C[node_FoR_dof:node_FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + mat = node_cga.T + vec = node_FoR_va + + LM_C[node_FoR_dof:node_FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = -np.eye(3) + vec = node_cga @ ag.skew(node_Ra) @ node_FoR_wa + + LM_C[node_FoR_dof:node_FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + mat = -node_cga.T + vec = ag.skew(node_Ra) @ node_FoR_wa + + LM_C[node_FoR_dof:node_FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = -np.eye(3) + vec = FoR_cga @ FoR_va + + LM_C[node_FoR_dof:node_FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + # term 3-x1 - \frac{\partial}{\partial x_1}(c^Taq_1 + c^Tbq2 + c^Tcq3 + c^Tdq4) + # dc^Tdxaq_1 + c^Tdadxaq_1 + dc^Tdxbq_2 + c^Tdbdxq_2 + dc^Tdxcq_3 + c^Tdcdxq_3 + dc^Tdxdq_4 + + mat = ag.skew(node_Ra).T + vec = -node_cga @ node_dot_Ra + + LM_C[node_FoR_dof + 3:node_FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + mat = -ag.skew(node_Ra).T @ node_cga.T + vec = node_dot_Ra + + LM_C[node_FoR_dof + 3:node_FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = ag.skew(node_Ra).T + vec = -node_cga @ node_FoR_va + + LM_C[node_FoR_dof + 3:node_FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + mat = -ag.skew(node_Ra).T @ node_cga.T + vec = node_FoR_va + + LM_C[node_FoR_dof + 3:node_FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = ag.skew(node_Ra).T + vec = node_cga @ ag.skew(node_Ra) @ node_FoR_wa + + LM_C[node_FoR_dof + 3:node_FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + mat = ag.skew(node_Ra).T @ node_cga.T + vec = ag.skew(node_Ra) @ node_FoR_wa + + LM_C[node_FoR_dof + 3:node_FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = ag.skew(node_Ra).T + vec = FoR_cga @ FoR_va + + LM_C[node_FoR_dof + 3:node_FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + # term 4-x1 - \frac{\partial}{\partial x_1}(d^Taq_1 + d^Tbq2 + d^Tcq3 + d^Tdq4) + # d^Tdadxaq_1 + d^Tdbdxq_2 + d^Tdcdxq_3 + + mat = -FoR_cga.T + vec = node_dot_Ra + + LM_C[FoR_dof:FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = -FoR_cga.T + vec = node_FoR_va + + LM_C[FoR_dof:FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = FoR_cga.T + vec = ag.skew(node_Ra) @ node_FoR_wa + + LM_C[FoR_dof:FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + # term 1-x2 - \frac{\partial}{\partial x_2}(a^Taq_1 + a^Tbq2 + a^Tcq3 + a^Tdq4) + # a^Tdddxq_4 + + mat = -node_cga.T + vec = FoR_va + + LM_C[node_dof:node_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, vec) + + # term 2-x2 - \frac{\partial}{\partial x_2}(b^Taq_1 + b^Tbq2 + b^Tcq3 + b^Tdq4) + # b^Tdddxq_4 + + mat = -node_cga.T + vec = FoR_va + + LM_C[node_FoR_dof:node_FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, vec) + + # term 3-x2 - \frac{\partial}{\partial x_2}(c^Taq_1 + c^Tbq2 + c^Tcq3 + c^Tdq4) + # c^Tdddxq_4 + + mat = ag.skew(node_Ra).T @ node_cga.T + vec = FoR_va + + LM_C[node_FoR_dof + 3:node_FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, vec) + + # term 4-x2 - \frac{\partial}{\partial x_2}(d^Taq_1 + d^Tbq2 + d^Tcq3 + d^Tdq4) + # dd^Tdxaq_1 + dd^Tdxbq_2 + dd^Tdxcq_3 + dd^Tdxdq_4 + d^Tdddxq_4 + + mat = np.eye(3) + vec = -node_cga @ node_dot_Ra + + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + + mat = np.eye(3) + vec = -node_cga @ node_FoR_va + + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + + mat = np.eye(3) + vec = node_cga @ ag.skew(node_Ra) @ node_FoR_wa + + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + + mat = np.eye(3) + vec = FoR_cga @ FoR_va + + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + + mat = FoR_cga.T + vec = FoR_va + + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, vec) + + # other LM_K derivatives for a/b/c/d dependencies in Ra + # term 1-Ra - \frac{\partial}{\partial Ra}(a^Taq_1 + a^Tbq2 + a^Tcq3 + a^Tdq4) + # a^Tdcdrq_3 + + mat = -node_cga.T @ node_cga + vec = node_FoR_wa + + LM_K[node_dof:node_dof + 3, node_dof:node_dof + 3] \ + += penaltyFactor * mat @ ag.der_skewp_v(ag.skew(node_Ra), vec) + + # term 2-Ra - \frac{\partial}{\partial Ra}(b^Taq_1 + b^Tbq2 + b^Tcq3 + b^Tdq4) + # b^Tdcdrq_3 + + mat = -node_cga.T @ node_cga + vec = node_FoR_wa + + LM_K[node_FoR_dof:node_FoR_dof + 3, node_dof:node_dof + 3] \ + += penaltyFactor * mat @ ag.der_skewp_v(ag.skew(node_Ra), vec) + + # term 3-Ra - \frac{\partial}{\partial Ra}(c^Taq_1 + c^Tbq2 + c^Tcq3 + c^Tdq4) + # dc^Tdraq_1 + dc^Tdrbq_2 + dc^Tdrcq_3 + c^Tdcdrq_3 + dc^Tdrdq_4 + + mat = np.eye(3) + vec = node_cga.T @ -node_cga @ node_dot_Ra + + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof:node_dof + 3] \ + += penaltyFactor * mat @ ag.der_skewpT_v(ag.skew(node_Ra), vec) + + mat = np.eye(3) + vec = node_cga.T @ -node_cga @ node_FoR_va + + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof:node_dof + 3] \ + += penaltyFactor * mat @ ag.der_skewpT_v(ag.skew(node_Ra), vec) + + mat = np.eye(3) + vec = node_cga.T @ node_cga @ ag.skew(node_Ra) @ node_FoR_wa + + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof:node_dof + 3] \ + += penaltyFactor * mat @ ag.der_skewpT_v(ag.skew(node_Ra), vec) + + mat = ag.skew(node_Ra).T @ node_cga.T @ node_cga + vec = node_FoR_wa + + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof:node_dof + 3] \ + += penaltyFactor * mat @ ag.der_skewp_v(ag.skew(node_Ra), vec) + + mat = np.eye(3) + vec = node_cga.T @ FoR_cga @ FoR_va + + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof:node_dof + 3] \ + += penaltyFactor * mat @ ag.der_skewpT_v(ag.skew(node_Ra), vec) + + # term 4-Ra - \frac{\partial}{\partial Ra}(d^Taq_1 + d^Tbq2 + d^Tcq3 + d^Tdq4) + # d^Tdcdrq_3 + + mat = FoR_cga.T @ node_cga + vec = node_FoR_wa + + LM_K[FoR_dof:FoR_dof + 3, node_dof:node_dof + 3] \ + += penaltyFactor * mat @ ag.der_skewp_v(ag.skew(node_Ra), vec) + + # a^T -node_cga.T + # a -node_cga + # b^T -node_cga.T + # b -node_cga + # c^T ag.skew(node_Ra).T, node_cga.T + # c node_cga, ag.skew(node_Ra) + # d^T FoR_cga.T + # d FoR_cga + # q1 node_dof:node_dof+3 node_dot_Ra + # q2 node_FoR_dof:node_FoR_dof+3 node_FoR_va + # q3 node_FoR_dof+3:node_FoR_dof+6 node_FoR_wa + # q4 FoR_dof:FoR_dof+3 FoR_va + + else: + # TODO: follow general approach to derive terms - first 4*4 terms, then LMC derivatives, + # then LMK derivatives - this is why penalty didn't work! + # if A1 is clamped, then remove the related DoFs from the description above (commented sections below) + + # Simplify notation + node_cga = MB_tstep[node_body].cga() + FoR_cga = MB_tstep[FoR_body].cga() + psi_dot = MB_tstep[node_body].psi_dot[ielem, inode_in_elem, :] + + q = np.zeros((sys_size)) + q[FoR_dof:FoR_dof + 3] = FoR_va + q[node_dof:node_dof + 3] = node_dot_Ra + q[node_dof + 3:node_dof + 6] = psi_dot + + LM_Q[:sys_size] += penaltyFactor * Bnh.T @ Bnh @ q + + # # 16 canonical terms for (abcd)^T(abcd) + LM_C[:sys_size, :sys_size] += penaltyFactor * Bnh.T @ Bnh + + # other LM_C derivatives for c dependencies in x1 and x2 + # term 1-x1 - \frac{\partial}{\partial x_1}(a^Taq_1 + a^Tbq2 + a^Tcq3 + a^Tdq4) + # da^Tdxaq_1 + a^Tdadxaq_1 + da^Tdxbq_2 + a^Tdbdxq_2 + da^Tdxcq_3 + a^Tdcdxq_3 + da^Tdxdq_4 + + mat = -np.eye(3) + vec = -node_cga @ node_dot_Ra + + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, ec) + + mat = node_cga.T + vec = node_dot_Ra + + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = -np.eye(3) + vec = FoR_cga @ FoR_va + + LM_C[node_dof:node_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + # term 4-x1 - \frac{\partial}{\partial x_1}(d^Taq_1 + d^Tbq2 + d^Tcq3 + d^Tdq4) + # d^Tdadxaq_1 + d^Tdbdxq_2 + d^Tdcdxq_3 + + mat = -FoR_cga.T + vec = node_dot_Ra + + LM_C[FoR_dof:FoR_dof + 3, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + # term 1-x2 - \frac{\partial}{\partial x_2}(a^Taq_1 + a^Tbq2 + a^Tcq3 + a^Tdq4) + # a^Tdddxq_4 + + mat = -node_cga.T + vec = FoR_va + + LM_C[node_dof:node_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, vec) + + # term 4-x2 - \frac{\partial}{\partial x_2}(d^Taq_1 + d^Tbq2 + d^Tcq3 + d^Tdq4) + # dd^Tdxaq_1 + dd^Tdxbq_2 + dd^Tdxcq_3 + dd^Tdxdq_4 + d^Tdddxq_4 + + mat = np.eye(3) + vec = -node_cga @ node_dot_Ra + + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + + mat = np.eye(3) + vec = FoR_cga @ FoR_va + + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + + mat = FoR_cga.T + vec = FoR_va + + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, vec) + + # a^T -node_cga.T + # a -node_cga + # b^T -node_cga.T + # b -node_cga + # c^T ag.skew(node_Ra).T, node_cga.T + # c node_cga, ag.skew(node_Ra) + # d^T FoR_cga.T + # d FoR_cga + # q1 node_dof:node_dof+3 node_dot_Ra + # q2 node_FoR_dof:node_FoR_dof+3 node_FoR_va + # q3 node_FoR_dof+3:node_FoR_dof+6 node_FoR_wa + # q4 FoR_dof:FoR_dof+3 FoR_va ieq += 3 return ieq -def rel_rot_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_vel=np.zeros((3))): +def rel_rot_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, + Lambda_dot, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_vel=np.zeros((3))): """ This function generates the stiffness and damping matrices and the independent vector associated to a constraint that imposes equal rotation velocities between a node and a frame of reference @@ -430,48 +802,56 @@ def rel_rot_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number, no """ num_LM_eq_specific = 3 - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Simplify notation ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] node_cga = MB_tstep[node_body].cga() node_FoR_wa = MB_tstep[node_body].for_vel[3:6] - psi = MB_tstep[node_body].psi[ielem,inode_in_elem,:] + psi = MB_tstep[node_body].psi[ielem, inode_in_elem, :] cab = ag.crv2rotation(psi) tan = ag.crv2tan(psi) FoR_cga = MB_tstep[FoR_body].cga() FoR_wa = MB_tstep[FoR_body].for_vel[3:6] - Bnh[:, node_dof+3:node_dof+6] += tan.copy() - Bnh[:, FoR_dof+3:FoR_dof+6] -= ag.multiply_matrices(cab.T, node_cga.T, FoR_cga) + Bnh[:, node_dof + 3:node_dof + 6] += tan.copy() + Bnh[:, FoR_dof + 3:FoR_dof + 6] -= cab.T @ node_cga.T @ FoR_cga if MB_beam[node_body].FoR_movement == 'free': - Bnh[:, node_FoR_dof+3:node_FoR_dof+6] += cab.T + Bnh[:, node_FoR_dof + 3:node_FoR_dof + 6] += cab.T - LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh - LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * Bnh.T - LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(np.dot(tan, MB_tstep[node_body].psi_dot[ielem, inode_in_elem, :]) + - np.dot(cab.T, node_FoR_wa) - - ag.multiply_matrices(cab.T, node_cga.T, FoR_cga, FoR_wa) + rel_vel) + LM_Q[:sys_size] += scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += scalingFactor * (tan @ MB_tstep[node_body].psi_dot[ielem, inode_in_elem, :] + + cab.T @ node_FoR_wa - cab.T @ node_cga.T @ FoR_cga @ FoR_wa + rel_vel) - LM_K[node_dof+3:node_dof+6, node_dof+3:node_dof+6] += scalingFactor*ag.der_TanT_by_xv(psi, Lambda_dot[ieq:ieq+num_LM_eq_specific]) + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += scalingFactor * ag.der_TanT_by_xv(psi, Lambda_dot[ieq:ieq + num_LM_eq_specific]) if MB_beam[node_body].FoR_movement == 'free': - LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof+3:node_dof+6] += scalingFactor*ag.der_Ccrv_by_v(psi, Lambda_dot[ieq:ieq+num_LM_eq_specific]) + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += scalingFactor * ag.der_Ccrv_by_v(psi, Lambda_dot[ieq:ieq + num_LM_eq_specific]) + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + -= scalingFactor * ag.der_Ccrv_by_v(psi, node_cga @ FoR_cga.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] -= scalingFactor*ag.der_Ccrv_by_v(psi, - ag.multiply_matrices(node_cga, FoR_cga.T, Lambda_dot[ieq:ieq+num_LM_eq_specific])) + LM_C[FoR_dof + 3:FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + -= scalingFactor * cab @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, + FoR_cga.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] -= scalingFactor*np.dot(cab, ag.der_Cquat_by_v(MB_tstep[node_body].quat, - np.dot(FoR_cga.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]))) - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] -= scalingFactor*ag.multiply_matrices(cab, node_cga, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, - Lambda_dot[ieq:ieq+num_LM_eq_specific])) + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + -= scalingFactor * cab @ node_cga @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, + Lambda_dot[ieq:ieq + num_LM_eq_specific]) ieq += 3 return ieq -def def_rot_axis_FoR_wrt_node_general(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, rot_axisB, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, indep): + +def def_rot_axis_FoR_wrt_node_general(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, + FoR_dof, sys_size, Lambda_dot, rot_axisB, rot_axisA2, scalingFactor, + penaltyFactor, ieq, LM_K, LM_C, LM_Q, indep): """ This function generates the stiffness and damping matrices and the independent vector associated to a joint that forces the rotation axis of a FoR to be parallel to a certain direction. This direction is defined in the @@ -481,6 +861,7 @@ def def_rot_axis_FoR_wrt_node_general(MB_tstep, MB_beam, FoR_body, node_body, no Args: rot_axisB (np.ndarray): Rotation axis with respect to the node B FoR + rot_axisA2 (np.ndarray): Rotation axis with respect to the node A2 FoR indep (np.ndarray): Number of the equations that are used as independent node_number (int): number of the "node" within its own body node_body (int): body number of the "node" @@ -489,147 +870,395 @@ def def_rot_axis_FoR_wrt_node_general(MB_tstep, MB_beam, FoR_body, node_body, no FoR_body (int): body number of the "FoR" FoR_dof (int): position of the first degree of freedom associated to the "FoR" - Notes: this function is missing the contribution of the rotation velocity of the reference node. See ``def_rot_axis_FoR_wrt_node_general`` """ - cout.cout_wrap("WARNING: this function is missing the contribution of the rotation velocity of the reference node. See ``def_rot_axis_FoR_wrt_node_general``", 3) ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] # Simplify notation - cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]) + cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem, inode_in_elem, :]) node_cga = MB_tstep[node_body].cga() FoR_cga = MB_tstep[FoR_body].cga() FoR_wa = MB_tstep[FoR_body].for_vel[3:6] + node_wa = MB_tstep[node_body].for_vel[3:6] + psi = MB_tstep[node_body].psi[ielem, inode_in_elem, :] + psi_dot = MB_tstep[node_body].psi_dot[ielem, inode_in_elem, :] - if not indep: - aux_Bnh = ag.multiply_matrices(ag.skew(rot_axisB), - cab.T, - node_cga.T, - FoR_cga) + if MB_beam[node_body].FoR_movement == 'free': + if not indep: + aux_Bnh = cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) - # indep = None - n0 = np.linalg.norm(aux_Bnh[0,:]) - n1 = np.linalg.norm(aux_Bnh[1,:]) - n2 = np.linalg.norm(aux_Bnh[2,:]) - if ((n0 < n1) and (n0 < n2)): - indep[:] = [1, 2] - elif ((n1 < n0) and (n1 < n2)): - indep[:] = [0, 2] - elif ((n2 < n0) and (n2 < n1)): - indep[:] = [0, 1] + # indep = None + n0 = np.linalg.norm(aux_Bnh[0, :]) + n1 = np.linalg.norm(aux_Bnh[1, :]) + n2 = np.linalg.norm(aux_Bnh[2, :]) + if ((n0 < n1) and (n0 < n2)): + indep[:] = [1, 2] + elif ((n1 < n0) and (n1 < n2)): + indep[:] = [0, 2] + elif ((n2 < n0) and (n2 < n1)): + indep[:] = [0, 1] - new_Lambda_dot = np.zeros(3) - new_Lambda_dot[indep[0]] = Lambda_dot[ieq] - new_Lambda_dot[indep[1]] = Lambda_dot[ieq+1] + new_Lambda_dot = np.zeros(3) + new_Lambda_dot[indep[0]] = Lambda_dot[ieq] + new_Lambda_dot[indep[1]] = Lambda_dot[ieq + 1] - num_LM_eq_specific = 2 - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + num_LM_eq_specific = 2 + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') + Bnh[:, FoR_dof + 3:FoR_dof + 6] -= (cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2))[indep, :] + Bnh[:, node_dof + 3:node_dof + 6] += (ag.skew(rot_axisB) @ ag.crv2tan(psi))[indep, :] + Bnh[:, node_FoR_dof + 3:node_FoR_dof + 6] += (ag.skew(rot_axisB) @ cab.T)[indep, :] + + # Constrain angular velocities + LM_Q[:sys_size] += scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + -= scalingFactor * (cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa)[indep] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += scalingFactor * (ag.skew(rot_axisB) @ ag.crv2tan(psi) @ psi_dot)[indep] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += scalingFactor * (ag.skew(rot_axisB) @ cab.T @ MB_tstep[node_body].for_vel[3:6])[indep] + + # # for initial omega A2 + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * Bnh.T + + # term 3 x1 + LM_C[FoR_dof + 3:FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += scalingFactor * ag.skew(rot_axisA2) @ FoR_cga.T @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, + cab @ new_Lambda_dot) + + # term 3 x2 + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += scalingFactor * ag.skew(rot_axisA2) @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, + node_cga @ cab @ new_Lambda_dot) + + # term 3 K(psi) + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += scalingFactor * ag.skew(rot_axisA2) @ FoR_cga.T @ node_cga @ ag.der_CcrvT_by_v(psi, new_Lambda_dot) + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + -= scalingFactor * ag.der_TanT_by_xv(psi, ag.skew(rot_axisB) @ new_Lambda_dot) + + # term 1 + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof + 3:node_dof + 6] \ + -= scalingFactor * ag.der_Ccrv_by_v(psi, ag.skew(rot_axisB) @ new_Lambda_dot) + else: + if not indep: + aux_Bnh = cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) + + # indep = None + n0 = np.linalg.norm(aux_Bnh[0, :]) + n1 = np.linalg.norm(aux_Bnh[1, :]) + n2 = np.linalg.norm(aux_Bnh[2, :]) + if ((n0 < n1) and (n0 < n2)): + indep[:] = [1, 2] + elif ((n1 < n0) and (n1 < n2)): + indep[:] = [0, 2] + elif ((n2 < n0) and (n2 < n1)): + indep[:] = [0, 1] - Bnh[:, FoR_dof+3:FoR_dof+6] = ag.multiply_matrices(ag.skew(rot_axisB), - cab.T, - node_cga.T, - FoR_cga)[indep,:] + new_Lambda_dot = np.zeros(3) + new_Lambda_dot[indep[0]] = Lambda_dot[ieq] + new_Lambda_dot[indep[1]] = Lambda_dot[ieq + 1] - # Constrain angular velocities - LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*ag.multiply_matrices(ag.skew(rot_axisB), - cab.T, - node_cga.T, - FoR_cga, - FoR_wa)[indep] + num_LM_eq_specific = 2 + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') - LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh - LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) + Bnh[:, FoR_dof + 3:FoR_dof + 6] -= (cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2))[indep, :] + Bnh[:, node_dof + 3:node_dof + 6] += (ag.skew(rot_axisB) @ ag.crv2tan(psi))[indep, :] - if MB_beam[node_body].FoR_movement == 'free': - LM_C[FoR_dof+3:FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*np.dot(FoR_cga.T, - ag.der_Cquat_by_v(MB_tstep[node_body].quat, - ag.multiply_matrices(cab, - ag.skew(rot_axisB).T, - new_Lambda_dot))) - - LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, - ag.multiply_matrices(node_cga, - cab, - ag.skew(rot_axisB).T, - new_Lambda_dot)) - - LM_K[FoR_dof+3:FoR_dof+6,node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T, - node_cga, - cab, - ag.skew(rot_axisB).T, - new_Lambda_dot) + # Constrain angular velocities + LM_Q[:sys_size] += scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + -= scalingFactor * (cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa)[indep] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += scalingFactor * (ag.skew(rot_axisB) @ ag.crv2tan(psi) @ psi_dot)[indep] + + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * Bnh.T + + # term 3 x2 + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += scalingFactor * ag.skew(rot_axisA2) @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, + node_cga @ cab @ new_Lambda_dot) + # term 3 K(psi) + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += scalingFactor * ag.skew(rot_axisA2) @ FoR_cga.T @ node_cga, ag.der_CcrvT_by_v(psi, new_Lambda_dot) + # term 2 + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + -= scalingFactor * ag.der_TanT_by_xv(psi, ag.skew(rot_axisB) @ new_Lambda_dot) + + # TODO: penalty factor formulation to be verified if penaltyFactor: - q = np.zeros((sys_size,)) - q[FoR_dof+3:FoR_dof+6] = MB_tstep[FoR_body].for_vel[3:6] + if MB_beam[node_body].FoR_movement == 'free': + q = np.zeros((sys_size,)) + q[FoR_dof + 3:FoR_dof + 6] = FoR_wa + q[node_dof + 3:node_dof + 6] = psi_dot + q[node_FoR_dof + 3:node_FoR_dof + 6] = node_wa - LM_Q[:sys_size] += penaltyFactor*np.dot(Bnh.T, np.dot(Bnh, q)) + LM_Q[:sys_size] += penaltyFactor * Bnh.T @ Bnh @ q + LM_C[:sys_size, :sys_size] += penaltyFactor * Bnh.T @ Bnh - LM_C[:sys_size, :sys_size] += penaltyFactor*np.dot(Bnh.T, Bnh) + # other LM_K derivatives for a/b/c dependencies in C(psi) and T(psi) + # term 2-psi - \frac{\partial}{\partial psi}(a^2q_2 + abq_5 + acq_7) + # da^Tdpsiaq_2 + a^Tdadpsiq_2 + da^Tdpsibq_5 + a^Tdbdpsiq_5 + da^Tdpsicq_7 + a^Tdcdpsiq_7 - sq_rot_axisB = np.dot(ag.skew(rot_axisB).T, ag.skew(rot_axisB)) + mat = np.eye(3) + vec = ag.skew(rot_axisB).T @ ag.skew(rot_axisB) @ cab.T @ node_wa - # Derivatives with the quaternion of the FoR - vec = ag.multiply_matrices(node_cga, - cab, - sq_rot_axisB, - cab.T, - node_cga.T, - FoR_cga, - FoR_wa) - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) - - mat = ag.multiply_matrices(FoR_cga.T, - node_cga, - cab, - sq_rot_axisB, - cab.T, - node_cga.T) - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, FoR_wa)) + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Ccrv_by_v(psi, vec) - if MB_beam[node_body].FoR_movement == 'free': - # Derivatives with the quaternion of the FoR of the node - vec = ag.multiply_matrices(cab, - sq_rot_axisB, - cab.T, - node_cga.T, - FoR_cga, - FoR_wa) - LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(FoR_cga.T, - ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec)) - mat = ag.multiply_matrices(FoR_cga.T, - node_cga, - cab, - sq_rot_axisB, - cab.T) - vec = np.dot(FoR_cga, FoR_wa) - LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec)) + mat = cab @ ag.skew(rot_axisB).T @ ag.skew(rot_axisB) + vec = node_wa - # Derivatives with the CRV - mat = np.dot(FoR_cga.T, node_cga) - vec = ag.multiply_matrices(sq_rot_axisB, - cab.T, - node_cga.T, - FoR_cga, - FoR_wa) - LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec)) - - mat = ag.multiply_matrices(FoR_cga.T, - node_cga, - cab, - sq_rot_axisB) - vec = ag.multiply_matrices(node_cga.T, - FoR_cga, - FoR_wa) - LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_CcrvT_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec)) + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_CcrvT_by_v(psi, vec) + + mat = np.eye(3) + vec = ag.skew(rot_axisB).T @ ag.skew(rot_axisB) @ ag.crv2tan(psi) @ psi_dot + + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Ccrv_by_v(psi, vec) + + mat = cab @ ag.skew(rot_axisB).T @ ag.skew(rot_axisB) + vec = psi_dot + + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Tan_by_xv(psi, vec) + + mat = np.eye(3) + vec = ag.skew(rot_axisB).T @ -cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Ccrv_by_v(psi, vec) + + mat = -cab @ ag.skew(rot_axisB).T + vec = node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_CcrvT_by_v(psi, vec) + + # term 5-psi - \frac{\partial}{\partial psi}(ba q_2 + b^2 q_5 + bc q_7) + # db^Tdpsiaq_2 + b^Tdadpsiq_2 + db^Tdpsibq_5 + b^Tdbdpsiq_5 + db^Tdpsicq_7 + b^Tdcdpsiq_7 + + mat = np.eye(3) + vec = ag.skew(rot_axisB).T @ ag.skew(rot_axisB) @ cab.T @ node_wa + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_TanT_by_xv(psi, vec) + + mat = ag.crv2tan(psi).T @ ag.skew(rot_axisB).T @ ag.skew(rot_axisB) + vec = node_wa + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_CcrvT_by_v(psi, vec) + + mat = np.eye(3) + vec = ag.skew(rot_axisB).T @ ag.skew(rot_axisB) @ ag.crv2tan(psi) @ psi_dot + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_TanT_by_xv(psi, vec) + + mat = ag.crv2tan(psi).T @ ag.skew(rot_axisB).T @ ag.skew(rot_axisB) + vec = psi_dot + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Tan_by_xv(psi, vec) + + mat = np.eye(3) + vec = ag.skew(rot_axisB).T @ -cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_TanT_by_xv(psi, vec) + + mat = -ag.crv2tan(psi).T @ ag.skew(rot_axisB).T + vec = node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_CcrvT_by_v(psi, vec) + + # term 7-psi - \frac{\partial}{\partial psi}(ca q_2 + cb q_5 + c^2 q_7) + # dc^Tdpsiaq_2 + c^Tdadpsiq_2 + dc^Tdpsibq_5 + c^Tdbdpsiq_5 + dc^Tdpsicq_7 + c^Tdcdpsiq_7 + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga + vec = ag.skew(rot_axisB) @ cab.T @ node_wa + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Ccrv_by_v(psi, vec) + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga @ cab @ ag.skew(rot_axisB) + vec = node_wa + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_CcrvT_by_v(psi, vec) + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga + vec = ag.skew(rot_axisB) @ ag.crv2tan(psi) @ psi_dot + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Ccrv_by_v(psi, vec) + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga @ cab @ ag.skew(rot_axisB) + vec = psi_dot + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Tan_by_xv(psi, vec) + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga + vec = -cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Ccrv_by_v(psi, vec) + + mat = ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga @ cab + vec = node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_CcrvT_by_v(psi, vec) + + # a^T cab, ag.skew(rot_axisB).T + # a ag.skew(rot_axisB), cab.T + # b^T ag.crv2tan(psi).T, ag.skew(rot_axisB).T + # b ag.skew(rot_axisB), ag.crv2tan(psi) + # c^T -ag.skew(rot_axisA2).T, FoR_cga.T, node_cga, cab + # c -cab.T, node_cga.T, FoR_cga, ag.skew(rot_axisA2) + + else: + q = np.zeros((sys_size,)) + q[FoR_dof + 3:FoR_dof + 6] = MB_tstep[FoR_body].for_vel[3:6] + q[node_dof + 3:node_dof + 6] = psi_dot + + LM_Q[:sys_size] += penaltyFactor * Bnh.T @ Bnh @ q + LM_C[:sys_size, :sys_size] += penaltyFactor * Bnh.T @ Bnh + + # term 5-x1 - \frac{\partial}{\partial x1}(b^2 q_5 + bc q_7) + # b^Tdcdx1q_7 + + mat = ag.crv2tan(psi).T @ ag.skew(rot_axisB).T @ -cab.T + vec = FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_C[node_dof + 3:node_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + # term 5-x2 - \frac{\partial}{\partial x2}(b^2 q_5 + bc q_7) + # b^Tdcdx2q_7 + + mat = ag.crv2tan(psi).T @ ag.skew(rot_axisB).T @ -cab.T @ node_cga.T + vec = ag.skew(rot_axisA2) @ FoR_wa + + LM_C[node_dof + 3:node_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, vec) + + # term 7-x1 - \frac{\partial}{\partial x1}(cb q_5 + c^2 q_7) + # dc^Tdx1aq_2 -> 0!!! + dc^Tdx1bq_5 + dc^Tdx1cq_7 + c^Tdcdx1q_7 + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T + vec = cab @ ag.skew(rot_axisB) @ ag.crv2tan(psi) @ psi_dot + + LM_C[FoR_dof + 3:FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T + vec = cab @ -cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_C[FoR_dof + 3:FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga @ cab @ -cab.T + vec = FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_C[FoR_dof + 3:FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) + + # term 7-x2 - \frac{\partial}{\partial x2}(cb q_5 + c^2 q_7) + # dc^Tdx2aq_2 -> 0!!! + dc^Tdx2bq_5 + dc^Tdx2cq_7 + c^Tdcdx2q_7 + + mat = -ag.skew(rot_axisA2).T + vec = node_cga @ cab @ ag.skew(rot_axisB) @ ag.crv2tan(psi) @ psi_dot + + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + + mat = -ag.skew(rot_axisA2).T + vec = node_cga @ cab @ -cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga @ cab @ -cab.T @ node_cga.T + vec = ag.skew(rot_axisA2) @ FoR_wa + + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, vec) + + # other LM_K derivatives for a/b/c dependencies in C(psi) and T(psi) + + mat = np.eye(3) + vec = ag.skew(rot_axisB).T @ ag.skew(rot_axisB) @ ag.crv2tan(psi) @ psi_dot + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_TanT_by_xv(psi, vec) + + mat = ag.crv2tan(psi).T @ ag.skew(rot_axisB).T @ ag.skew(rot_axisB) + vec = psi_dot + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Tan_by_xv(psi, vec) + + mat = np.eye(3) + vec = ag.skew(rot_axisB).T @ -cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_TanT_by_xv(psi, vec) + + mat = -ag.crv2tan(psi).T @ ag.skew(rot_axisB).T + vec = node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_CcrvT_by_v(psi, vec) + + # term 7-psi - \frac{\partial}{\partial psi}(cb q_5 + c^2 q_7) + # dc^Tdpsiaq_2 -> 0!!! + c^Tdadpsiq_2 -> 0!!! + dc^Tdpsibq_5 + c^Tdbdpsiq_5 + dc^Tdpsicq_7 + c^Tdcdpsiq_7 + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga + vec = ag.skew(rot_axisB) @ ag.crv2tan(psi) @ psi_dot + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Ccrv_by_v(psi, vec) + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga @ cab @ ag.skew(rot_axisB) + vec = psi_dot + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Tan_by_xv(psi, vec) + + mat = -ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga + vec = -cab.T @ node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Ccrv_by_v(psi, vec) + + mat = ag.skew(rot_axisA2).T @ FoR_cga.T @ node_cga @ cab + vec = node_cga.T @ FoR_cga @ ag.skew(rot_axisA2) @ FoR_wa + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_CcrvT_by_v(psi, vec) + + # a^T cab, ag.skew(rot_axisB).T + # a ag.skew(rot_axisB), cab.T + # b^T ag.crv2tan(psi).T, ag.skew(rot_axisB).T + # b ag.skew(rot_axisB), ag.crv2tan(psi) + # c^T -ag.skew(rot_axisA2).T, FoR_cga.T, node_cga, cab + # c -cab.T, node_cga.T, FoR_cga, ag.skew(rot_axisA2) ieq += 2 return ieq -def def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, rot_axisB, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, zero_comp): +def def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, + sys_size, Lambda_dot, rot_axisB, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, + zero_comp): """ This function generates the stiffness and damping matrices and the independent vector associated to a joint that forces the rotation axis of a FoR to be parallel to a certain direction. This direction is defined in the @@ -651,117 +1280,101 @@ def def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, FoR_body, node_body, node_n ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] num_LM_eq_specific = 2 - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Simplify notation - cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]) + cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem, inode_in_elem, :]) node_cga = MB_tstep[node_body].cga() FoR_cga = MB_tstep[FoR_body].cga() FoR_wa = MB_tstep[FoR_body].for_vel[3:6] - psi = MB_tstep[node_body].psi[ielem,inode_in_elem,:] - psi_dot = MB_tstep[node_body].psi_dot[ielem,inode_in_elem,:] + psi = MB_tstep[node_body].psi[ielem, inode_in_elem, :] + psi_dot = MB_tstep[node_body].psi_dot[ielem, inode_in_elem, :] # Components to be zero - Z = np.zeros((2,3)) + Z = np.zeros((2, 3)) Z[:, zero_comp] = np.eye(2) - Bnh[:, FoR_dof+3:FoR_dof+6] += ag.multiply_matrices(Z, cab.T, node_cga.T, FoR_cga) - Bnh[:, node_dof+3:node_dof+6] -= ag.multiply_matrices(Z, ag.crv2tan(psi)) - Bnh[:, node_FoR_dof+3:node_FoR_dof+6] -= ag.multiply_matrices(Z, cab.T) + Bnh[:, FoR_dof + 3:FoR_dof + 6] += Z @ cab.T @ node_cga.T @ FoR_cga + Bnh[:, node_dof + 3:node_dof + 6] -= Z @ ag.crv2tan(psi) + Bnh[:, node_FoR_dof + 3:node_FoR_dof + 6] -= Z @ cab.T # Constrain angular velocities - LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*ag.multiply_matrices(Z, cab.T, node_cga.T, FoR_cga, FoR_wa) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] -= scalingFactor*ag.multiply_matrices(Z, ag.crv2tan(psi), psi_dot) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] -= scalingFactor*ag.multiply_matrices(Z, cab.T, MB_tstep[node_body].for_vel[3:6]) + LM_Q[:sys_size] += scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += scalingFactor * Z @ cab.T @ node_cga.T @ FoR_cga @ FoR_wa + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + -= scalingFactor * Z @ ag.crv2tan(psi) @ psi_dot + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + -= scalingFactor * Z @ cab.T @ MB_tstep[node_body].for_vel[3:6] - LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh - LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * Bnh.T - vec = ag.multiply_matrices(node_cga, cab, Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + vec = node_cga @ cab @ Z.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += scalingFactor * ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) if MB_beam[node_body].FoR_movement == 'free': - vec = ag.multiply_matrices(cab, Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*ag.multiply_matrices(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec)) + vec = cab @ Z.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_C[FoR_dof + 3:FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += scalingFactor * FoR_cga.T @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) - LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T, node_cga, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], - np.dot(Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]))) + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += scalingFactor * FoR_cga.T @ node_cga @ ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem, inode_in_elem, :], + Z.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_K[node_dof+3:node_dof+6, node_dof+3:node_dof+6] -= scalingFactor*ag.der_TanT_by_xv(psi, ag.multiply_matrices(Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific])) - LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof+3:node_dof+6] -= scalingFactor*ag.der_Ccrv_by_v(psi, ag.multiply_matrices(Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific])) + LM_K[node_dof + 3:node_dof + 6, node_dof + 3:node_dof + 6] \ + -= scalingFactor * ag.der_TanT_by_xv(psi, Z.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific]) + LM_K[node_FoR_dof + 3:node_FoR_dof + 6, node_dof + 3:node_dof + 6] \ + -= scalingFactor * ag.der_Ccrv_by_v(psi, Z.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific]) if penaltyFactor: q = np.zeros((sys_size,)) - q[FoR_dof+3:FoR_dof+6] = FoR_wa - - LM_Q[:sys_size] += penaltyFactor*np.dot(Bnh.T, np.dot(Bnh, q)) + q[FoR_dof + 3:FoR_dof + 6] = FoR_wa - LM_C[:sys_size, :sys_size] += penaltyFactor*np.dot(Bnh.T, Bnh) + LM_Q[:sys_size] += penaltyFactor * Bnh.T @ Bnh @ q + LM_C[:sys_size, :sys_size] += penaltyFactor * Bnh.T @ Bnh - ZTZ = np.dot(Z.T, Z) + ZTZ = Z.T @ Z # Derivatives with the quaternion of the FoR - vec = ag.multiply_matrices(node_cga, - cab, - ZTZ, - cab.T, - node_cga.T, - FoR_cga, - FoR_wa) - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) - - mat = ag.multiply_matrices(FoR_cga.T, - node_cga, - cab, - ZTZ, - cab.T, - node_cga.T) - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, FoR_wa)) + vec = node_cga @ cab @ ZTZ @ cab.T @ node_cga.T @ FoR_cga @ FoR_wa + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + + mat = FoR_cga.T @ node_cga @ cab @ ZTZ @ cab.T @ node_cga.T + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, FoR_wa) if MB_beam[node_body].FoR_movement == 'free': # Derivatives with the quaternion of the FoR of the node - vec = ag.multiply_matrices(cab, - ZTZ, - cab.T, - node_cga.T, - FoR_cga, - FoR_wa) - LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(FoR_cga.T, - ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec)) - - mat = ag.multiply_matrices(FoR_cga.T, - node_cga, - cab, - ZTZ, - cab.T) - vec = np.dot(FoR_cga, FoR_wa) - LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec)) + vec = cab @ ZTZ @ cab.T @ node_cga.T @ FoR_cga @ FoR_wa + LM_C[FoR_dof + 3:FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * FoR_cga.T @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) + + mat = FoR_cga.T @ node_cga @ cab @ ZTZ @ cab.T + vec = FoR_cga @ FoR_wa + LM_C[FoR_dof + 3:FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += penaltyFactor * mat @ ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec) # Derivatives with the CRV - mat = np.dot(FoR_cga.T, node_cga) - vec = ag.multiply_matrices(ZTZ, - cab.T, - node_cga.T, - FoR_cga, - FoR_wa) - LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec)) - - mat = ag.multiply_matrices(FoR_cga.T, - node_cga, - cab, - ZTZ) - vec = ag.multiply_matrices(node_cga.T, - FoR_cga, - FoR_wa) - LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_CcrvT_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec)) + mat = FoR_cga.T @ node_cga + vec = ZTZ @ cab.T @ node_cga.T @ FoR_cga @ FoR_wa + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem, inode_in_elem, :], vec) + + mat = FoR_cga.T @ node_cga @ cab @ ZTZ + vec = node_cga.T @ FoR_cga @ FoR_wa + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += penaltyFactor * mat @ ag.der_CcrvT_by_v(MB_tstep[node_body].psi[ielem, inode_in_elem, :], vec) ieq += 2 return ieq -def def_rot_vel_mod_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, nonzero_comp, rot_vel, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q): +def def_rot_vel_mod_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, + sys_size, Lambda_dot, nonzero_comp, rot_vel, scalingFactor, penaltyFactor, ieq, LM_K, + LM_C, LM_Q): """ This function generates the stiffness and damping matrices and the independent vector associated to a joint that forces the rotation velocity of a FoR with respect to a node @@ -781,89 +1394,92 @@ def def_rot_vel_mod_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_nu ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] num_LM_eq_specific = 1 - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Simplify notation - cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]) + cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem, inode_in_elem, :]) node_cga = MB_tstep[node_body].cga() FoR_cga = MB_tstep[FoR_body].cga() FoR_wa = MB_tstep[FoR_body].for_vel[3:6] # Components to be zero - Znon = np.zeros((1,3)) + Znon = np.zeros((1, 3)) Znon[:, nonzero_comp] = 1 - Bnh[:, FoR_dof+3:FoR_dof+6] += ag.multiply_matrices(Znon, cab.T, node_cga.T, FoR_cga) + Bnh[:, FoR_dof + 3:FoR_dof + 6] += Znon @ cab.T @ node_cga.T @ FoR_cga # Constrain angular velocities - LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*ag.multiply_matrices(Znon, cab.T, node_cga.T, FoR_cga, FoR_wa) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] -= scalingFactor*rot_vel + LM_Q[:sys_size] += scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += scalingFactor * Znon @ cab.T @ node_cga.T @ FoR_cga @ FoR_wa + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] -= scalingFactor * rot_vel - LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh - LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * Bnh.T - vec = ag.multiply_matrices(node_cga, cab, Znon.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) + vec = node_cga @ cab @ Znon.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += scalingFactor * ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec) if MB_beam[node_body].FoR_movement == 'free': - vec = ag.multiply_matrices(cab, Znon.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*ag.multiply_matrices(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec)) + vec = cab @ Znon.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_C[FoR_dof + 3:FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += scalingFactor * FoR_cga.T @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec) - LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T, node_cga, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], - np.dot(Znon.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]))) + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += scalingFactor * FoR_cga.T @ node_cga @ ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem, inode_in_elem, :], + Znon.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific]) ieq += 1 return ieq -def def_rot_vect_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, rot_vect, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q): + +def def_rot_vect_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, + sys_size, Lambda_dot, rot_vect, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q): """ This function fixes the rotation velocity VECTOR of a FOR equal to a velocity vector defined in the B FoR of a node This function is a new implementation that combines and simplifies the use of 'def_rot_vel_mod_FoR_wrt_node' and 'def_rot_axis_FoR_wrt_node' together """ num_LM_eq_specific = 3 - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Simplify notation ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number] node_cga = MB_tstep[node_body].cga() - cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]) + cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem, inode_in_elem, :]) FoR_cga = MB_tstep[FoR_body].cga() FoR_wa = MB_tstep[FoR_body].for_vel[3:6] - Bnh[:, FoR_dof+3:FoR_dof+6] = ag.multiply_matrices(cab.T, - node_cga.T, - FoR_cga) + Bnh[:, FoR_dof + 3:FoR_dof + 6] = cab.T @ node_cga.T @ FoR_cga # Constrain angular velocities - LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(np.dot(Bnh[:, FoR_dof+3:FoR_dof+6], FoR_wa) - - rot_vect) + LM_Q[:sys_size] += scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += scalingFactor * (Bnh[:, FoR_dof + 3:FoR_dof + 6] @ FoR_wa - rot_vect) - LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh - LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh) + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * Bnh.T if MB_beam[node_body].FoR_movement == 'free': - LM_C[FoR_dof+3:FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*np.dot(FoR_cga.T, - ag.der_Cquat_by_v(MB_tstep[node_body].quat, - np.dot(cab, Lambda_dot[ieq:ieq+num_LM_eq_specific]))) + LM_C[FoR_dof + 3:FoR_dof + 6, node_FoR_dof + 6:node_FoR_dof + 10] \ + += scalingFactor * FoR_cga.T @ ag.der_Cquat_by_v(MB_tstep[node_body].quat, + cab @ Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, - ag.multiply_matrices(node_cga, cab, Lambda_dot[ieq:ieq+num_LM_eq_specific])) + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += scalingFactor * ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, + node_cga @ cab @ Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_K[FoR_dof+3:FoR_dof+6,node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T, - node_cga, - ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], - Lambda_dot[ieq:ieq+num_LM_eq_specific])) + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof + 3:node_dof + 6] \ + += scalingFactor * FoR_cga.T @ node_cga @ ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem, inode_in_elem, :], + Lambda_dot[ieq:ieq + num_LM_eq_specific]) if penaltyFactor: - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+3:FoR_dof+6] += penaltyFactor*np.eye(3) + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 3:FoR_dof + 6] += penaltyFactor * np.eye(3) - q = np.zeros((sys_size)) - q[FoR_dof+3:FoR_dof+6] = FoR_wa - LM_Q[:sys_size] += penaltyFactor*np.dot(np.dot(Bnh.T, Bnh), q) + q = np.zeros(sys_size) + q[FoR_dof + 3:FoR_dof + 6] = FoR_wa + LM_Q[:sys_size] += penaltyFactor * Bnh.T @ Bnh @ q ieq += 3 return ieq @@ -886,6 +1502,7 @@ class hinge_node_FoR(BaseLagrangeConstraint): node_body (int): body number of the "node" FoR_body (int): body number of the "FoR" rot_axisB (np.ndarray): Rotation axis with respect to the node B FoR + rot_axisA2 (np.ndarray): Rotation axis with respect to the node A2 FoR """ _lc_id = 'hinge_node_FoR' @@ -901,32 +1518,33 @@ def initialise(self, MBdict_entry, ieq, print_info=True): self.node_body = MBdict_entry['body'] self.FoR_body = MBdict_entry['body_FoR'] self.rot_axisB = MBdict_entry['rot_axisB'] + self.rot_axisA2 = set_value_or_default(MBdict_entry, "rot_axisA2", self.rot_axisB) self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) - - if (self.rot_axisB[[1, 2]] == 0).all(): + self.indep = [] + if (self.rot_axisB[[1, 2]] == 0).all(): self.rot_dir = 'x' self.zero_comp = np.array([1, 2], dtype=int) - elif (self.rot_axisB[[0, 2]] == 0).all(): + elif (self.rot_axisB[[0, 2]] == 0).all(): self.rot_dir = 'y' self.zero_comp = np.array([0, 2], dtype=int) - elif (self.rot_axisB[[0, 1]] == 0).all(): + elif (self.rot_axisB[[0, 1]] == 0).all(): self.rot_dir = 'z' self.zero_comp = np.array([0, 1], dtype=int) else: - raise NotImplementedError("Hinges should be parallel to the xB, yB or zB of the reference node") - #self.rot_dir = 'general' - #self.indep = [] + # raise NotImplementedError("Hinges should be parallel to the xB, yB or zB of the reference node") + self.rot_dir = 'general' + self.indep = [] return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): # Define the position of the first degree of freedom associated to the node node_dof = define_node_dof(MB_beam, self.node_body, self.node_number) @@ -935,12 +1553,13 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, ieq = self._ieq # Define the equations - # ieq = equal_pos_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) - ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) - if self.rot_dir == 'general': - ieq = def_rot_axis_FoR_wrt_node_general(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.indep) - else: - ieq = def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.zero_comp) + ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, + node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, + ieq, LM_K, LM_C, LM_Q) + ieq = def_rot_axis_FoR_wrt_node_general(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, + node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, + self.rot_axisA2, self.scalingFactor, self.penaltyFactor, ieq, LM_K, + LM_C, LM_Q, self.indep) return @@ -948,7 +1567,9 @@ def staticpost(self, lc_list, MB_beam, MB_tstep): return def dynamicpost(self, lc_list, MB_beam, MB_tstep): - MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(MB_tstep[self.node_body].cga(), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3] + MB_tstep[self.FoR_body].for_pos[:3] \ + = (MB_tstep[self.node_body].cga() @ MB_tstep[self.node_body].pos[self.node_number, :] + + MB_tstep[self.node_body].for_pos[:3]) return @@ -990,15 +1611,15 @@ def initialise(self, MBdict_entry, ieq, print_info=True): self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) self.rot_axisB = ag.unit_vector(MBdict_entry['rot_vect']) - if (self.rot_axisB[[1, 2]] == 0).all(): + if (self.rot_axisB[[1, 2]] == 0).all(): self.rot_dir = 'x' self.zero_comp = np.array([1, 2], dtype=int) self.nonzero_comp = 0 - elif (self.rot_axisB[[0, 2]] == 0).all(): + elif (self.rot_axisB[[0, 2]] == 0).all(): self.rot_dir = 'y' self.zero_comp = np.array([0, 2], dtype=int) self.nonzero_comp = 1 - elif (self.rot_axisB[[0, 1]] == 0).all(): + elif (self.rot_axisB[[0, 1]] == 0).all(): self.rot_dir = 'z' self.zero_comp = np.array([0, 1], dtype=int) self.nonzero_comp = 2 @@ -1008,17 +1629,15 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq - def set_rot_vel(self, rot_vel): self.rot_vel = rot_vel - def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): # Define the position of the first degree of freedom associated to the node node_dof = define_node_dof(MB_beam, self.node_body, self.node_number) @@ -1027,11 +1646,16 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, ieq = self._ieq # Define the equations - # ieq = equal_pos_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) - ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_posB=self.rel_posB) - # ieq = def_rot_vect_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_vect, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) - ieq = def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.zero_comp) - ieq = def_rot_vel_mod_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.nonzero_comp, self.rot_vel, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) + ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, + node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, + ieq, LM_K, LM_C, LM_Q, rel_posB=self.rel_posB) + ieq = def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, + node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, + self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, + self.zero_comp) + ieq = def_rot_vel_mod_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, + node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.nonzero_comp, + self.rot_vel, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) return def staticpost(self, lc_list, MB_beam, MB_tstep): @@ -1043,9 +1667,8 @@ def dynamicpost(self, lc_list, MB_beam, MB_tstep): node_cga = MB_tstep[self.node_body].cga() cab = ag.crv2rotation(MB_tstep[self.node_body].psi[ielem, inode_in_elem, :]) - MB_tstep[self.FoR_body].for_pos[0:3] = (np.dot(node_cga, - MB_tstep[self.node_body].pos[self.node_number,:] + np.dot(cab, self.rel_posB)) + - MB_tstep[self.node_body].for_pos[0:3]) + MB_tstep[self.FoR_body].for_pos[:3] = (node_cga @ MB_tstep[self.node_body].pos[self.node_number, :] + + cab @ self.rel_posB + MB_tstep[self.node_body].for_pos[0:3]) return @@ -1076,7 +1699,6 @@ def get_n_eq(self): return self._n_eq def initialise(self, MBdict_entry, ieq, print_info=True): - self.node_number = MBdict_entry['node_in_body'] self.node_body = MBdict_entry['body'] self.FoR_body = MBdict_entry['body_FoR'] @@ -1092,22 +1714,18 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq - def set_rotor_vel(self, rotor_vel): self.rotor_vel = rotor_vel - def set_pitch_vel(self, pitch_vel): self.pitch_vel = pitch_vel - def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): - + sys_size, dt, Lambda, Lambda_dot): # Define the position of the first degree of freedom associated to the node node_dof = define_node_dof(MB_beam, self.node_body, self.node_number) node_FoR_dof = define_FoR_dof(MB_beam, self.node_body) @@ -1122,26 +1740,27 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, # rel_vel in B FoR rel_vel = np.array([0., 0., self.rotor_vel]) - rel_vel += ag.multiply_matrices(cab.T, node_cga.T, FoR_cga, - np.array([self.pitch_vel, 0., 0.])) + rel_vel += cab.T @ node_cga.T @ FoR_cga @ np.array([self.pitch_vel, 0., 0.]) # Define the equations - ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_posB=self.rel_posB) - ieq = rel_rot_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_vel=rel_vel) + ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, + node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, + ieq, LM_K, LM_C, LM_Q, rel_posB=self.rel_posB) + ieq = rel_rot_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, + node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, + LM_K, LM_C, LM_Q, rel_vel=rel_vel) return def staticpost(self, lc_list, MB_beam, MB_tstep): return def dynamicpost(self, lc_list, MB_beam, MB_tstep): - ielem, inode_in_elem = MB_beam[self.node_body].node_master_elem[self.node_number] node_cga = MB_tstep[self.node_body].cga() cab = ag.crv2rotation(MB_tstep[self.node_body].psi[ielem, inode_in_elem, :]) - MB_tstep[self.FoR_body].for_pos[0:3] = (np.dot(node_cga, - MB_tstep[self.node_body].pos[self.node_number,:] + np.dot(cab, self.rel_posB)) + - MB_tstep[self.node_body].for_pos[0:3]) + MB_tstep[self.FoR_body].for_pos[:3] = node_cga @ (MB_tstep[self.node_body].pos[self.node_number, :] + + cab @ self.rel_posB) + MB_tstep[self.node_body].for_pos[:3] return @@ -1170,7 +1789,6 @@ def get_n_eq(self): return self._n_eq def initialise(self, MBdict_entry, ieq, print_info=True): - self.node_number = MBdict_entry['node_in_body'] self.node_body = MBdict_entry['body'] self.FoR_body = MBdict_entry['body_FoR'] @@ -1181,12 +1799,11 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): - + sys_size, dt, Lambda, Lambda_dot): # Define the position of the first degree of freedom associated to the node node_dof = define_node_dof(MB_beam, self.node_body, self.node_number) node_FoR_dof = define_FoR_dof(MB_beam, self.node_body) @@ -1194,7 +1811,9 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, ieq = self._ieq # Define the equations - ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q) + ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, + node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, + ieq, LM_K, LM_C, LM_Q) return @@ -1202,7 +1821,9 @@ def staticpost(self, lc_list, MB_beam, MB_tstep): return def dynamicpost(self, lc_list, MB_beam, MB_tstep): - MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(MB_tstep[self.node_body].cga(), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3] + MB_tstep[self.FoR_body].for_pos[:3] = (MB_tstep[self.node_body].cga() + @ MB_tstep[self.node_body].pos[self.node_number, :] + + MB_tstep[self.node_body].for_pos[:3]) return @@ -1219,16 +1840,15 @@ def get_n_eq(self): return self._n_eq def initialise(self, MBdict_entry, ieq, print_info=True): - self._ieq = ieq return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def staticpost(self, lc_list, MB_beam, MB_tstep): @@ -1260,7 +1880,6 @@ def get_n_eq(self): return self._n_eq def initialise(self, MBdict_entry, ieq, print_info=True): - self.body_FoR = MBdict_entry['body_FoR'] self._ieq = ieq self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) @@ -1269,27 +1888,27 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.body_FoR) ieq = self._ieq - Bnh[:3, FoR_dof:FoR_dof+3] = 1.0*np.eye(3) + Bnh[:3, FoR_dof:FoR_dof + 3] = np.eye(3) - LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh - LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh) + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * Bnh.T - LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific]) + LM_Q[:sys_size] += self.scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] - LM_Q[sys_size+ieq:sys_size+ieq+3] += self.scalingFactor*MB_tstep[self.body_FoR].for_vel[0:3].astype(dtype=ct.c_double, copy=True, order='F') + LM_Q[sys_size + ieq:sys_size + ieq + 3] \ + += self.scalingFactor * MB_tstep[self.body_FoR].for_vel[:3].astype(dtype=ct.c_double, copy=True, order='F') ieq += 3 return @@ -1331,13 +1950,13 @@ def initialise(self, MBdict_entry, ieq, print_info=True): self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.) self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.) - if (self.rot_axis[[1, 2]] == 0).all(): + if (self.rot_axis[[1, 2]] == 0).all(): self.rot_dir = 'x' self.zero_comp = np.array([1, 2], dtype=int) - elif (self.rot_axis[[0, 2]] == 0).all(): + elif (self.rot_axis[[0, 2]] == 0).all(): self.rot_dir = 'y' self.zero_comp = np.array([0, 2], dtype=int) - elif (self.rot_axis[[0, 1]] == 0).all(): + elif (self.rot_axis[[0, 1]] == 0).all(): self.rot_dir = 'z' self.zero_comp = np.array([0, 1], dtype=int) else: @@ -1346,62 +1965,57 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.body_FoR) ieq = self._ieq - Bnh[:3, FoR_dof:FoR_dof+3] = 1.0*np.eye(3) + Bnh[:3, FoR_dof:FoR_dof + 3] = np.eye(3) - if self.rot_dir == 'general': - # Only two of these equations are linearly independent - skew_rot_axis = ag.skew(self.rot_axis) - n0 = np.linalg.norm(skew_rot_axis[0,:]) - n1 = np.linalg.norm(skew_rot_axis[1,:]) - n2 = np.linalg.norm(skew_rot_axis[2,:]) - if ((n0 < n1) and (n0 < n2)): - row0 = 1 - row1 = 2 - elif ((n1 < n0) and (n1 < n2)): - row0 = 0 - row1 = 2 - elif ((n2 < n0) and (n2 < n1)): - row0 = 0 - row1 = 1 - Bnh[3:5, FoR_dof+3:FoR_dof+6] = skew_rot_axis[[row0,row1],:] - else: - Bnh[3:5, FoR_dof+3+self.zero_comp] = np.eye(2) + # TODO: general logic removed since that implies local beam direction coincident with global axis direction + skew_rot_axis = ag.skew(self.rot_axis) + n0 = np.linalg.norm(skew_rot_axis[0, :]) + n1 = np.linalg.norm(skew_rot_axis[1, :]) + n2 = np.linalg.norm(skew_rot_axis[2, :]) + if ((n0 < n1) and (n0 < n2)): + row0 = 1 + row1 = 2 + elif ((n1 < n0) and (n1 < n2)): + row0 = 0 + row1 = 2 + elif ((n2 < n0) and (n2 < n1)): + row0 = 0 + row1 = 1 + Bnh[3:5, FoR_dof + 3:FoR_dof + 6] = skew_rot_axis[[row0, row1], :] - LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh - LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh) + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * Bnh.T - LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific]) + LM_Q[:sys_size] += self.scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] - LM_Q[sys_size+ieq:sys_size+ieq+3] += self.scalingFactor*MB_tstep[self.body_FoR].for_vel[0:3].astype(dtype=ct.c_double, copy=True, order='F') - if self.rot_dir == 'general': - LM_Q[sys_size+ieq+3:sys_size+ieq+5] += self.scalingFactor*np.dot(skew_rot_axis[[row0,row1],:], MB_tstep[self.body_FoR].for_vel[3:6]) - else: - LM_Q[sys_size+ieq+3:sys_size+ieq+5] += self.scalingFactor*MB_tstep[self.body_FoR].for_vel[3 + self.zero_comp] + LM_Q[sys_size + ieq:sys_size + ieq + 3] \ + += self.scalingFactor * MB_tstep[self.body_FoR].for_vel[:3].astype(dtype=ct.c_double, copy=True, order='F') + + # TODO: general logic removed since that implies local beam direction coincident with global axis direction + LM_Q[sys_size + ieq + 3:sys_size + ieq + 5] \ + += self.scalingFactor * skew_rot_axis[[row0, row1], :] @ MB_tstep[self.body_FoR].for_vel[3:6] if self.penaltyFactor: - LM_Q[FoR_dof:FoR_dof+3] += self.penaltyFactor*MB_tstep[self.body_FoR].for_vel[0:3] - LM_C[FoR_dof:FoR_dof+3, FoR_dof:FoR_dof+3] += self.penaltyFactor*np.eye(3) + LM_Q[FoR_dof:FoR_dof + 3] += self.penaltyFactor * MB_tstep[self.body_FoR].for_vel[:3] + LM_C[FoR_dof:FoR_dof + 3, FoR_dof:FoR_dof + 3] += self.penaltyFactor * np.eye(3) - if self.rot_dir == 'general': - sq_rot_axis = np.dot(ag.skew(self.rot_axis).T, ag.skew(self.rot_axis)) - LM_Q[FoR_dof+3:FoR_dof+6] += self.penaltyFactor*np.dot(sq_rot_axis, MB_tstep[self.body_FoR].for_vel[3:6]) - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+3:FoR_dof+6] += self.penaltyFactor*sq_rot_axis - else: - LM_Q[FoR_dof+3:FoR_dof+6] += self.penaltyFactor*MB_tstep[self.body_FoR].for_vel[3:6] - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+3:FoR_dof+6] += self.penaltyFactor*np.eye(3) + # TODO: general logic removed since that implies local beam direction coincident with global axis direction + sq_rot_axis = ag.skew(self.rot_axis).T @ ag.skew(self.rot_axis) + LM_Q[FoR_dof + 3:FoR_dof + 6] \ + += self.penaltyFactor * sq_rot_axis @ MB_tstep[self.body_FoR].for_vel[3:6] + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 3:FoR_dof + 6] += self.penaltyFactor * sq_rot_axis ieq += 5 return @@ -1446,26 +2060,26 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') + B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.body_FoR) ieq = self._ieq - Bnh[:3, FoR_dof:FoR_dof+3] = MB_tstep[self.body_FoR].cga() + Bnh[:3, FoR_dof:FoR_dof + 3] = MB_tstep[self.body_FoR].cga() # Only two of these equations are linearly independent skew_rot_axis = ag.skew(self.rot_axis) - n0 = np.linalg.norm(skew_rot_axis[0,:]) - n1 = np.linalg.norm(skew_rot_axis[1,:]) - n2 = np.linalg.norm(skew_rot_axis[2,:]) + n0 = np.linalg.norm(skew_rot_axis[0, :]) + n1 = np.linalg.norm(skew_rot_axis[1, :]) + n2 = np.linalg.norm(skew_rot_axis[2, :]) if ((n0 < n1) and (n0 < n2)): row0 = 1 row1 = 2 @@ -1476,17 +2090,20 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, row0 = 0 row1 = 1 - Bnh[3:5, FoR_dof+3:FoR_dof+6] = skew_rot_axis[[row0,row1],:] + Bnh[3:5, FoR_dof + 3:FoR_dof + 6] = skew_rot_axis[[row0, row1], :] - LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh - LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh) + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * Bnh.T - LM_C[FoR_dof:FoR_dof+3,FoR_dof+6:FoR_dof+10] += self.scalingFactor*ag.der_CquatT_by_v(MB_tstep[self.body_FoR].quat,Lambda_dot[ieq:ieq+3]) + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] \ + += self.scalingFactor * ag.der_CquatT_by_v(MB_tstep[self.body_FoR].quat, Lambda_dot[ieq:ieq + 3]) - LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific]) + LM_Q[:sys_size] += self.scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] - LM_Q[sys_size+ieq:sys_size+ieq+3] += self.scalingFactor*np.dot(MB_tstep[self.body_FoR].cga(),MB_tstep[self.body_FoR].for_vel[0:3]) - LM_Q[sys_size+ieq+3:sys_size+ieq+5] += self.scalingFactor*np.dot(skew_rot_axis[[row0,row1],:], MB_tstep[self.body_FoR].for_vel[3:6]) + LM_Q[sys_size + ieq:sys_size + ieq + 3] \ + += self.scalingFactor * MB_tstep[self.body_FoR].cga() @ MB_tstep[self.body_FoR].for_vel[:3] + LM_Q[sys_size + ieq + 3:sys_size + ieq + 5] \ + += self.scalingFactor * skew_rot_axis[[row0, row1], :] @ MB_tstep[self.body_FoR].for_vel[3:6] ieq += 5 return @@ -1523,8 +2140,6 @@ def get_n_eq(self): return self._n_eq def initialise(self, MBdict_entry, ieq, print_info=True): - - # cout.cout_wrap("WARNING: do not use fully_constrained_node_FoR. It is outdated. Definetly not working if 'body' has velocity", 3) self.node_number = MBdict_entry['node_in_body'] self.node_body = MBdict_entry['body'] self.FoR_body = MBdict_entry['body_FoR'] @@ -1536,12 +2151,11 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): - + sys_size, dt, Lambda, Lambda_dot): # Define the position of the first degree of freedom associated to the node node_dof = define_node_dof(MB_beam, self.node_body, self.node_number) node_FoR_dof = define_FoR_dof(MB_beam, self.node_body) @@ -1549,8 +2163,12 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, ieq = self._ieq # Define the equations - ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_posB=self.rel_posB) - ieq = rel_rot_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, rel_vel=np.zeros((3))) + ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, + node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, + ieq, LM_K, LM_C, LM_Q, rel_posB=self.rel_posB) + ieq = rel_rot_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, + node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, + LM_K, LM_C, LM_Q, rel_vel=np.zeros(3)) return @@ -1562,9 +2180,8 @@ def dynamicpost(self, lc_list, MB_beam, MB_tstep): node_cga = MB_tstep[self.node_body].cga() cab = ag.crv2rotation(MB_tstep[self.node_body].psi[ielem, inode_in_elem, :]) - MB_tstep[self.FoR_body].for_pos[0:3] = (np.dot(node_cga, - MB_tstep[self.node_body].pos[self.node_number,:] + np.dot(cab, self.rel_posB)) + - MB_tstep[self.node_body].for_pos[0:3]) + MB_tstep[self.FoR_body].for_pos[:3] = node_cga @ (MB_tstep[self.node_body].pos[self.node_number, :] + + cab @ self.rel_posB) + MB_tstep[self.node_body].for_pos[:3] return @@ -1590,7 +2207,6 @@ def get_n_eq(self): return self._n_eq def initialise(self, MBdict_entry, ieq, print_info=True): - self.rot_vel = MBdict_entry['rot_vel'] self.FoR_body = MBdict_entry['FoR_body'] self._ieq = ieq @@ -1600,26 +2216,26 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq - Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F') + Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq - Bnh[:3,FoR_dof+3:FoR_dof+6] = np.eye(3) + Bnh[:3, FoR_dof + 3:FoR_dof + 6] = np.eye(3) - LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh - LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh) + LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * Bnh.T - LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific]) - LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.FoR_body].for_vel[3:6] - self.rot_vel) + LM_Q[:sys_size] += self.scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += self.scalingFactor * (MB_tstep[self.FoR_body].for_vel[3:6] - self.rot_vel) ieq += 3 return @@ -1630,6 +2246,7 @@ def staticpost(self, lc_list, MB_beam, MB_tstep): def dynamicpost(self, lc_list, MB_beam, MB_tstep): return + @lagrangeconstraint class constant_vel_FoR(BaseLagrangeConstraint): __doc__ = """ @@ -1653,7 +2270,6 @@ def get_n_eq(self): return self._n_eq def initialise(self, MBdict_entry, ieq, print_info=True): - self.vel = MBdict_entry['vel'] self.FoR_body = MBdict_entry['FoR_body'] self._ieq = ieq @@ -1663,26 +2279,26 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq - Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof+6] = np.eye(6) + Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof + 6] = np.eye(6) LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh - LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh) + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * Bnh.T - LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.FoR_body].for_vel - self.vel) + LM_Q[:sys_size] += self.scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += self.scalingFactor * (MB_tstep[self.FoR_body].for_vel - self.vel) ieq += 6 return @@ -1738,29 +2354,29 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.FoR_body) ieq = self._ieq - vel = np.zeros((6)) - vel[3 + self.xyz_index] = self.vel_amp*np.sin(self.omega*ts*dt) + vel = np.zeros(6) + vel[3 + self.xyz_index] = self.vel_amp * np.sin(self.omega * ts * dt) - Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof+6] = np.eye(6) + Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof + 6] = np.eye(6) LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh - LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh) + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * Bnh.T - LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.FoR_body].for_vel - vel) + LM_Q[:sys_size] += self.scalingFactor * Bnh.T, Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += self.scalingFactor * (MB_tstep[self.FoR_body].for_vel - vel) ieq += 6 return @@ -1808,7 +2424,7 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') @@ -1817,42 +2433,42 @@ def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, node_dof = define_node_dof(MB_beam, self.body_number, self.node_number) ieq = self._ieq - B[:num_LM_eq_specific, node_dof:node_dof+3] = np.eye(3) + B[:num_LM_eq_specific, node_dof:node_dof + 3] = np.eye(3) LM_K[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * B - LM_K[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(B) + LM_K[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * B.T - LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(B), Lambda[ieq:ieq + num_LM_eq_specific]) - LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.body_number].pos[self.node_number,:] - - MB_beam[self.body_number].ini_info.pos[self.node_number,:]) + LM_Q[:sys_size] += self.scalingFactor * B.T @ Lambda[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += self.scalingFactor * (MB_tstep[self.body_number].pos[self.node_number, :] + - MB_beam[self.body_number].ini_info.pos[self.node_number, :]) ieq += 3 - return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): if len(self.vel.shape) > 1: - current_vel = self.vel[ts-1, :] + current_vel = self.vel[ts - 1, :] else: current_vel = self.vel num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR node_dof = define_node_dof(MB_beam, self.body_number, self.node_number) ieq = self._ieq - Bnh[:num_LM_eq_specific, node_dof:node_dof+3] = np.eye(3) + Bnh[:num_LM_eq_specific, node_dof:node_dof + 3] = np.eye(3) LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh - LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh) + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * Bnh.T - LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.body_number].pos_dot[self.node_number,:] - current_vel) + LM_Q[:sys_size] += self.scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += self.scalingFactor * (MB_tstep[self.body_number].pos_dot[self.node_number, :] - current_vel) ieq += 3 return @@ -1863,6 +2479,7 @@ def staticpost(self, lc_list, MB_beam, MB_tstep): def dynamicpost(self, lc_list, MB_beam, MB_tstep): return + @lagrangeconstraint class lin_vel_node_wrtG(BaseLagrangeConstraint): __doc__ = """ @@ -1899,7 +2516,7 @@ def initialise(self, MBdict_entry, ieq, print_info=True): return self._ieq + self._n_eq def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): num_LM_eq_specific = self._n_eq B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') @@ -1908,31 +2525,32 @@ def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, node_dof = define_node_dof(MB_beam, self.body_number, self.node_number) ieq = self._ieq - B[:num_LM_eq_specific, node_dof:node_dof+3] = MB_tstep[self.body_number].cga() + B[:num_LM_eq_specific, node_dof:node_dof + 3] = MB_tstep[self.body_number].cga() LM_K[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * B - LM_K[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(B) + LM_K[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * B.T - LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(B), Lambda[ieq:ieq + num_LM_eq_specific]) - LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(np.dot(MB_tstep[self.body_number].cga(), MB_tstep[self.body_number].pos[self.node_number,:]) + - MB_tstep[self.body_number].for_pos) - LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] -= self.scalingFactor*(np.dot(MB_beam[self.body_number].ini_info.cga(), MB_beam[self.body_number].ini_info.pos[self.node_number,:]) + - MB_beam[self.body_number].ini_info.for_pos) + LM_Q[:sys_size] += self.scalingFactor * B.T @ Lambda[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += (self.scalingFactor * MB_tstep[self.body_number].cga() + @ MB_tstep[self.body_number].pos[self.node_number, :] + MB_tstep[self.body_number].for_pos) + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + -= (self.scalingFactor * MB_beam[self.body_number].ini_info.cga() + @ MB_beam[self.body_number].ini_info.pos[self.node_number, :] + + MB_beam[self.body_number].ini_info.for_pos) ieq += 3 - return def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, - sys_size, dt, Lambda, Lambda_dot): + sys_size, dt, Lambda, Lambda_dot): if len(self.vel.shape) > 1: - current_vel = self.vel[ts-1, :] + current_vel = self.vel[ts - 1, :] else: current_vel = self.vel num_LM_eq_specific = self._n_eq Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') - B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F') # Define the position of the first degree of freedom associated to the FoR FoR_dof = define_FoR_dof(MB_beam, self.body_number) @@ -1940,26 +2558,34 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq, ieq = self._ieq if MB_beam[self.body_number].FoR_movement == 'free': - Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof+3] = MB_tstep[self.body_number].cga() - Bnh[:num_LM_eq_specific, FoR_dof+3:FoR_dof+6] = -np.dot(MB_tstep[self.body_number].cga(), ag.skew(MB_tstep[self.body_number].pos[self.node_number,:])) - Bnh[:num_LM_eq_specific, node_dof:node_dof+3] = MB_tstep[self.body_number].cga() + Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof + 3] = MB_tstep[self.body_number].cga() + Bnh[:num_LM_eq_specific, FoR_dof + 3:FoR_dof + 6] \ + = -MB_tstep[self.body_number].cga() @ ag.skew(MB_tstep[self.body_number].pos[self.node_number, :]) + Bnh[:num_LM_eq_specific, node_dof:node_dof + 3] = MB_tstep[self.body_number].cga() LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh - LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh) + LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * Bnh.T if MB_beam[self.body_number].FoR_movement == 'free': - LM_C[FoR_dof:FoR_dof+3, FoR_dof+6:FoR_dof+10] += self.scalingFactor*ag.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_C[node_dof:node_dof+3, FoR_dof+6:FoR_dof+10] += self.scalingFactor*ag.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += self.scalingFactor*np.dot(ag.skew(MB_tstep[self.body_number].pos[self.node_number,:]), ag.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific])) - - LM_K[FoR_dof+3:FoR_dof+6, node_dof:node_dof+3] -= self.scalingFactor*ag.skew(np.dot(MB_tstep[self.body_number].cga().T, Lambda_dot[ieq:ieq + num_LM_eq_specific])) - - LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific]) - LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(np.dot( MB_tstep[self.body_number].cga(), ( - MB_tstep[self.body_number].for_vel[0:3] + - np.dot(ag.skew(MB_tstep[self.body_number].for_vel[3:6]), MB_tstep[self.body_number].pos[self.node_number,:]) + - MB_tstep[self.body_number].pos_dot[self.node_number,:])) - - current_vel) + LM_C[FoR_dof:FoR_dof + 3, FoR_dof + 6:FoR_dof + 10] += self.scalingFactor * ag.der_CquatT_by_v( + MB_tstep[self.body_number].quat, Lambda_dot[ieq:ieq + num_LM_eq_specific]) + LM_C[node_dof:node_dof + 3, FoR_dof + 6:FoR_dof + 10] += self.scalingFactor * ag.der_CquatT_by_v( + MB_tstep[self.body_number].quat, Lambda_dot[ieq:ieq + num_LM_eq_specific]) + LM_C[FoR_dof + 3:FoR_dof + 6, FoR_dof + 6:FoR_dof + 10] \ + += (self.scalingFactor * ag.skew(MB_tstep[self.body_number].pos[self.node_number, :]) + @ ag.der_CquatT_by_v(MB_tstep[self.body_number].quat, Lambda_dot[ieq:ieq + num_LM_eq_specific])) + + LM_K[FoR_dof + 3:FoR_dof + 6, node_dof:node_dof + 3] \ + -= self.scalingFactor * ag.skew(MB_tstep[self.body_number].cga().T + @ Lambda_dot[ieq:ieq + num_LM_eq_specific]) + + LM_Q[:sys_size] += self.scalingFactor * Bnh.T @ Lambda_dot[ieq:ieq + num_LM_eq_specific] + LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] \ + += self.scalingFactor * (MB_tstep[self.body_number].cga() + @ (MB_tstep[self.body_number].for_vel[:3] + + ag.skew(MB_tstep[self.body_number].for_vel[3:6]) + @ MB_tstep[self.body_number].pos[self.node_number, :] + + MB_tstep[self.body_number].pos_dot[self.node_number, :]) - current_vel) ieq += 3 return @@ -1975,7 +2601,6 @@ def dynamicpost(self, lc_list, MB_beam, MB_tstep): # Funtions to interact with this Library ################################################################################ def initialize_constraints(MBdict): - index_eq = 0 num_constraints = MBdict['num_constraints'] lc_list = list() @@ -1986,11 +2611,13 @@ def initialize_constraints(MBdict): MBdict_entry = MBdict["constraint_%02d" % iconstraint] if "penaltyFactor" in MBdict_entry.keys(): if not MBdict_entry['penaltyFactor'] == 0.: - raise NotImplementedError("Penalty method not completely implemented for Lagrange Constraints") + print("Penalty method not completely implemented for Lagrange Constraints") + index_eq = lc_list[-1].initialise(MBdict_entry, index_eq) return lc_list + def define_num_LM_eq(lc_list): """ define_num_LM_eq @@ -2018,7 +2645,8 @@ def define_num_LM_eq(lc_list): return num_LM_eq -def generate_lagrange_matrix(lc_list, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot, dynamic_or_static): +def generate_lagrange_matrix(lc_list, MB_beam, MB_tstep, ts, num_LM_eq, sys_size, dt, Lambda, Lambda_dot, + dynamic_or_static): """ generate_lagrange_matrix @@ -2042,39 +2670,20 @@ def generate_lagrange_matrix(lc_list, MB_beam, MB_tstep, ts, num_LM_eq, sys_size LM_Q (np.ndarray): Vector of independent terms associated to the Lagrange Multipliers equations """ # Initialize matrices - LM_C = np.zeros((sys_size + num_LM_eq,sys_size + num_LM_eq), dtype=ct.c_double, order = 'F') - LM_K = np.zeros((sys_size + num_LM_eq,sys_size + num_LM_eq), dtype=ct.c_double, order = 'F') - LM_Q = np.zeros((sys_size + num_LM_eq,),dtype=ct.c_double, order = 'F') + LM_C = np.zeros((sys_size + num_LM_eq, sys_size + num_LM_eq), dtype=ct.c_double, order='F') + LM_K = np.zeros((sys_size + num_LM_eq, sys_size + num_LM_eq), dtype=ct.c_double, order='F') + LM_Q = np.zeros((sys_size + num_LM_eq,), dtype=ct.c_double, order='F') # Define the matrices associated to the constratints # TODO: Is there a better way to deal with ieq? - # ieq = 0 for lc in lc_list: if dynamic_or_static.lower() == "static": - lc.staticmat(LM_C=LM_C, - LM_K=LM_K, - LM_Q=LM_Q, - MB_beam=MB_beam, - MB_tstep=MB_tstep, - ts=ts, - num_LM_eq=num_LM_eq, - sys_size=sys_size, - dt=dt, - Lambda=Lambda, - Lambda_dot=Lambda_dot) + lc.staticmat(LM_C=LM_C, LM_K=LM_K, LM_Q=LM_Q, MB_beam=MB_beam, MB_tstep=MB_tstep, ts=ts, + num_LM_eq=num_LM_eq, sys_size=sys_size, dt=dt, Lambda=Lambda, Lambda_dot=Lambda_dot) elif dynamic_or_static.lower() == "dynamic": - lc.dynamicmat(LM_C=LM_C, - LM_K=LM_K, - LM_Q=LM_Q, - MB_beam=MB_beam, - MB_tstep=MB_tstep, - ts=ts, - num_LM_eq=num_LM_eq, - sys_size=sys_size, - dt=dt, - Lambda=Lambda, - Lambda_dot=Lambda_dot) + lc.dynamicmat(LM_C=LM_C, LM_K=LM_K, LM_Q=LM_Q, MB_beam=MB_beam, MB_tstep=MB_tstep, ts=ts, + num_LM_eq=num_LM_eq, sys_size=sys_size, dt=dt, Lambda=Lambda, Lambda_dot=Lambda_dot) return LM_C, LM_K, LM_Q @@ -2085,15 +2694,10 @@ def postprocess(lc_list, MB_beam, MB_tstep, dynamic_or_static): """ for lc in lc_list: if dynamic_or_static.lower() == "static": - lc.staticpost(lc_list = lc_list, - MB_beam = MB_beam, - MB_tstep = MB_tstep) + lc.staticpost(lc_list=lc_list, MB_beam=MB_beam, MB_tstep=MB_tstep) elif dynamic_or_static.lower() == "dynamic": - lc.dynamicpost(lc_list = lc_list, - MB_beam = MB_beam, - MB_tstep = MB_tstep) - + lc.dynamicpost(lc_list=lc_list, MB_beam=MB_beam, MB_tstep=MB_tstep) return @@ -2104,15 +2708,12 @@ def remove_constraint(MBdict, constraint): a dynamic simulation """ try: - del(MBdict[constraint]) + del (MBdict[constraint]) MBdict['num_constraints'] -= 1 except KeyError: - # The entry did not exist in the dict, pass without substracting 1 to - # num_constraints pass ################################################################################ -################################################################################ -################################################################################ + print_available_lc() diff --git a/sharpy/structure/utils/modalutils.py b/sharpy/structure/utils/modalutils.py index b7698cb86..355cc51f2 100644 --- a/sharpy/structure/utils/modalutils.py +++ b/sharpy/structure/utils/modalutils.py @@ -376,7 +376,7 @@ def principal_axes_inertia(j_a, r_cg, m): """ - j_p, t_pa = np.linalg.eig(j_a + algebra.multiply_matrices(algebra.skew(r_cg), algebra.skew(r_cg)) * m) + j_p, t_pa = np.linalg.eig(j_a + algebra.skew(r_cg) @ (algebra.skew(r_cg)) * m) t_pa, j_p = order_eigenvectors(t_pa, j_p) diff --git a/sharpy/utils/algebra.py b/sharpy/utils/algebra.py index f6dff6254..bb2e210e0 100644 --- a/sharpy/utils/algebra.py +++ b/sharpy/utils/algebra.py @@ -8,9 +8,9 @@ """ import numpy as np -import scipy.linalg from warnings import warn + ####### # functions for back compatibility def quat2rot(quat): @@ -28,9 +28,9 @@ def rot2crv(rot): return rotation2crv(rot.T) -def triad2rot(xb,yb,zb): +def triad2rot(xb, yb, zb): warn('triad2rot(xb,yb,zb) is obsolete! Use triad2rotation(xb,yb,zb).T instead!', stacklevel=2) - return triad2rotation(xb,yb,zb).T + return triad2rotation(xb, yb, zb).T def mat2quat(rot): @@ -49,7 +49,6 @@ def mat2quat(rot): warn('mat2quat(rot) is obsolete! Use rotation2quat(rot.T) instead!', stacklevel=2) return rotation2quat(rot.T) -####### def tangent_vector(in_coord, ordering=None): @@ -101,11 +100,7 @@ def tangent_vector(in_coord, ordering=None): vector = [] for idim in range(ndim): vector.append((polyfit_der_vec[idim])(inode)) - # vector = np.array([polyfit_der_vec[0](inode), - # DEBUG vector = np.array(vector) - if (np.linalg.norm(vector)) == 0.0: - print(vector) vector /= np.linalg.norm(vector) tangent[inode, :] = vector @@ -116,7 +111,7 @@ def tangent_vector(in_coord, ordering=None): # use previous vector fake_tangent[inode, :] = fake_tangent[inode - 1, :] continue - fake_tangent[inode, :] = coord[inode+1, :] - coord[inode, :] + fake_tangent[inode, :] = coord[inode + 1, :] - coord[inode, :] for inode in range(n_nodes): if np.dot(tangent[inode, :], fake_tangent[inode, :]) < 0: @@ -163,14 +158,14 @@ def unit_vector(vector): """ if np.linalg.norm(vector) < 1e-6: return np.zeros_like(vector) - return vector/np.linalg.norm(vector) + return vector / np.linalg.norm(vector) def rotation_matrix_around_axis(axis, angle): axis = unit_vector(axis) - rot = np.cos(angle)*np.eye(3) - rot += np.sin(angle)*skew(axis) - rot += (1 - np.cos(angle))*np.outer(axis, axis) + rot = np.cos(angle) * np.eye(3) + rot += np.sin(angle) * skew(axis) + rot += (1 - np.cos(angle)) * np.outer(axis, axis) return rot @@ -229,9 +224,9 @@ def quadskew(vector): raise ValueError('The input vector is not 3D') matrix = np.zeros((4, 4)) - matrix[0,1:4] = vector - matrix[1:4,0] = -vector - matrix[1:4,1:4] = skew(vector) + matrix[0, 1:4] = vector + matrix[1:4, 0] = -vector + matrix[1:4, 1:4] = skew(vector) return matrix @@ -265,10 +260,11 @@ def angle_between_vectors_sign(vec_a, vec_b, plane_normal=np.array([0, 0, 1])): def angle_between_vector_and_plane(vector, plane_normal): - angle = np.arcsin((np.linalg.norm(np.dot(vector, plane_normal)))/ - (np.linalg.norm(vector)*np.linalg.norm(plane_normal))) + angle = np.arcsin((np.linalg.norm(np.dot(vector, plane_normal))) / + (np.linalg.norm(vector) * np.linalg.norm(plane_normal))) return angle + def panel_area(A, B, C, D): """ Calculates the area of a quadrilateral panel from the corner @@ -282,52 +278,16 @@ def panel_area(A, B, C, D): Returns: float: Area of quadrilateral panel - """ - Theta_1 = angle_between_vectors(A-B, A-D) - Theta_2 = angle_between_vectors(B-C, B-D) - a = np.linalg.norm(D-A) - b = np.linalg.norm(A-B) - c = np.linalg.norm(B-C) - d = np.linalg.norm(C-D) - s = (a+b+c+d)/2 - area = np.sqrt((s-a)*(s-b)*(s-c)*(s-d)-a*b*c*d*np.cos(0.5*(Theta_1+Theta_2))**2) + """ + Theta_1 = angle_between_vectors(A - B, A - D) + Theta_2 = angle_between_vectors(B - C, B - D) + a = np.linalg.norm(D - A) + b = np.linalg.norm(A - B) + c = np.linalg.norm(B - C) + d = np.linalg.norm(C - D) + s = (a + b + c + d) / 2 + area = np.sqrt((s - a) * (s - b) * (s - c) * (s - d) - a * b * c * d * np.cos(0.5 * (Theta_1 + Theta_2)) ** 2) return area - -# def mat2quat(mat): -# matT = mat.T - -# s = np.zeros((4, 4)) - -# s[0, 0] = 1.0 + np.trace(matT) -# s[0, 1:] = matrix2skewvec(matT) - -# s[1, 0] = matT[2, 1] - matT[1, 2] -# s[1, 1] = 1.0 + matT[0, 0] - matT[1, 1] - matT[2, 2] -# s[1, 2] = matT[0, 1] + matT[1, 0] -# s[1, 3] = matT[0, 2] + matT[2, 0] - -# s[2, 0] = matT[0, 2] - matT[2, 0] -# s[2, 1] = matT[1, 0] + matT[0, 1] -# s[2, 2] = 1.0 - matT[0, 0] + matT[1, 1] - matT[2, 2] -# s[2, 3] = matT[1, 2] + matT[2, 1] - -# s[3, 0] = matT[1, 0] - matT[0, 1] -# s[3, 1] = matT[0, 2] + matT[2, 0] -# s[3, 2] = matT[1, 2] + matT[2, 1] -# s[3, 3] = 1.0 - matT[0, 0] - matT[1, 1] + matT[2, 2] - -# smax = np.max(np.diag(s)) -# ismax = np.argmax(np.diag(s)) - -# # compute quaternion angles -# quat = np.zeros((4,)) -# quat[ismax] = 0.5*np.sqrt(smax) -# for i in range(4): -# if i == ismax: -# continue -# quat[i] = 0.25*s[ismax, i]/quat[ismax] - -# return quat def rotation2quat(Cab): @@ -386,11 +346,11 @@ def rotation2quat(Cab): # compute quaternion angles quat = np.zeros((4,)) - quat[ismax] = 0.5*np.sqrt(smax) + quat[ismax] = 0.5 * np.sqrt(smax) for i in range(4): if i == ismax: continue - quat[i] = 0.25*s[ismax, i]/quat[ismax] + quat[i] = 0.25 * s[ismax, i] / quat[ismax] return quat_bound(quat) @@ -431,13 +391,13 @@ def matrix2skewvec(matrix): def quat2crv(quat): - crv_norm = 2.0*np.arccos(max(-1.0, min(quat[0], 1.0))) + crv_norm = 2.0 * np.arccos(max(-1.0, min(quat[0], 1.0))) # normal vector if abs(crv_norm) < 1e-15: psi = np.zeros((3,)) else: - psi = crv_norm*quat[1:4]/np.sin(crv_norm*0.5) + psi = crv_norm * quat[1:4] / np.sin(crv_norm * 0.5) return psi @@ -501,16 +461,16 @@ def crv_bounds(crv_ini): norm_ini = np.linalg.norm(crv_ini) # force the norm to be in [-pi, pi] - norm = norm_ini - 2.0*np.pi*int(norm_ini/(2*np.pi)) + norm = norm_ini - 2.0 * np.pi * int(norm_ini / (2 * np.pi)) if norm == 0.0: crv *= 0.0 else: if norm > np.pi: - norm -= 2.0*np.pi + norm -= 2.0 * np.pi elif norm < -np.pi: - norm += 2.0*np.pi - crv *= (norm/norm_ini) + norm += 2.0 * np.pi + crv *= (norm / norm_ini) return crv # return crv_ini @@ -556,14 +516,14 @@ def crv2rotation(psi): if norm_psi < 1e-15: skew_psi = skew(psi) - rot_matrix = np.eye(3) + skew_psi + 0.5*np.dot(skew_psi, skew_psi) + rot_matrix = np.eye(3) + skew_psi + 0.5 * np.dot(skew_psi, skew_psi) else: - normal = psi/norm_psi + normal = psi / norm_psi skew_normal = skew(normal) rot_matrix = np.eye(3) - rot_matrix += np.sin(norm_psi)*skew_normal - rot_matrix += (1.0 - np.cos(norm_psi))*np.dot(skew_normal, skew_normal) + rot_matrix += np.sin(norm_psi) * skew_normal + rot_matrix += (1.0 - np.cos(norm_psi)) * np.dot(skew_normal, skew_normal) return rot_matrix @@ -641,11 +601,11 @@ def crv2tan(psi): eps = 1e-8 if norm_psi < eps: - return np.eye(3) - 0.5*psi_skew + 1.0/6.0*np.dot(psi_skew, psi_skew) + return np.eye(3) - 0.5 * psi_skew + 1.0 / 6.0 * np.dot(psi_skew, psi_skew) else: - k1 = (np.cos(norm_psi) - 1.0)/(norm_psi*norm_psi) - k2 = (1.0 - np.sin(norm_psi)/norm_psi)/(norm_psi*norm_psi) - return np.eye(3) + k1*psi_skew + k2*np.dot(psi_skew, psi_skew) + k1 = (np.cos(norm_psi) - 1.0) / (norm_psi * norm_psi) + k2 = (1.0 - np.sin(norm_psi) / norm_psi) / (norm_psi * norm_psi) + return np.eye(3) + k1 * psi_skew + k2 * np.dot(psi_skew, psi_skew) def crv2invtant(psi): @@ -715,18 +675,18 @@ def quat2rotation(q1): rot_mat = np.zeros((3, 3), order='F') - rot_mat[0, 0] = q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2 - rot_mat[1, 1] = q[0]**2 - q[1]**2 + q[2]**2 - q[3]**2 - rot_mat[2, 2] = q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2 + rot_mat[0, 0] = q[0] ** 2 + q[1] ** 2 - q[2] ** 2 - q[3] ** 2 + rot_mat[1, 1] = q[0] ** 2 - q[1] ** 2 + q[2] ** 2 - q[3] ** 2 + rot_mat[2, 2] = q[0] ** 2 - q[1] ** 2 - q[2] ** 2 + q[3] ** 2 - rot_mat[1, 0] = 2.*(q[1]*q[2] + q[0]*q[3]) - rot_mat[0, 1] = 2.*(q[1]*q[2] - q[0]*q[3]) + rot_mat[1, 0] = 2. * (q[1] * q[2] + q[0] * q[3]) + rot_mat[0, 1] = 2. * (q[1] * q[2] - q[0] * q[3]) - rot_mat[2, 0] = 2.*(q[1]*q[3] - q[0]*q[2]) - rot_mat[0, 2] = 2.*(q[1]*q[3] + q[0]*q[2]) + rot_mat[2, 0] = 2. * (q[1] * q[3] - q[0] * q[2]) + rot_mat[0, 2] = 2. * (q[1] * q[3] + q[0] * q[2]) - rot_mat[2, 1] = 2.*(q[2]*q[3] + q[0]*q[1]) - rot_mat[1, 2] = 2.*(q[2]*q[3] - q[0]*q[1]) + rot_mat[2, 1] = 2. * (q[2] * q[3] + q[0] * q[1]) + rot_mat[1, 2] = 2. * (q[2] * q[3] - q[0] * q[1]) return rot_mat @@ -761,9 +721,9 @@ def rotation3d_x(angle): c = np.cos(angle) s = np.sin(angle) mat = np.zeros((3, 3)) - mat[0, :] = [1.0, 0.0, 0.0] - mat[1, :] = [0.0, c, -s] - mat[2, :] = [0.0, s, c] + mat[0, :] = [1., 0., 0.] + mat[1, :] = [0., c, -s] + mat[2, :] = [0., s, c] return mat @@ -791,9 +751,9 @@ def rotation3d_y(angle): c = np.cos(angle) s = np.sin(angle) mat = np.zeros((3, 3)) - mat[0, :] = [c, 0.0, s] - mat[1, :] = [0.0, 1.0, 0.0] - mat[2, :] = [-s, 0.0, c] + mat[0, :] = [c, 0., s] + mat[1, :] = [0., 1., 0.] + mat[2, :] = [-s, 0., c] return mat @@ -819,14 +779,13 @@ def rotation3d_z(angle): c = np.cos(angle) s = np.sin(angle) mat = np.zeros((3, 3)) - mat[0, :] = [ c, -s, 0.0] - mat[1, :] = [ s, c, 0.0] - mat[2, :] = [0.0, 0.0, 1.0] + mat[0, :] = [c, -s, 0.] + mat[1, :] = [s, c, 0.] + mat[2, :] = [0., 0., 1.] return mat def rotate_crv(crv_in, axis, angle): - crv = np.zeros_like(crv_in) C = crv2rotation(crv_in).T rot = rotation_matrix_around_axis(axis, angle) C = np.dot(C, rot) @@ -857,9 +816,6 @@ def euler2rot(euler): """ - # rot = rotation3d_z(euler[2]) - # rot = np.dot(rotation3d_y(euler[1]), rot) - # rot = np.dot(rotation3d_x(euler[0]), rot) rot = rotation3d_z(euler[2]).dot(rotation3d_y(euler[1]).dot(rotation3d_x(euler[0]))) return rot @@ -909,21 +865,21 @@ def quat2euler(quat): Report 012010. ETS Ingenieria Informatica. Universidad de Malaga. 2013. """ - assert np.abs(np.linalg.norm(quat)-1.0) < 1.e6, 'Input quaternion is not normalised' + assert np.abs(np.linalg.norm(quat) - 1.0) < 1.e6, 'Input quaternion is not normalised' q0 = quat[0] q1 = quat[1] q2 = quat[2] q3 = quat[3] - delta = quat[0]*quat[2] - quat[1]*quat[3] + delta = quat[0] * quat[2] - quat[1] * quat[3] if np.abs(delta) > 0.9 * 0.5: warn('Warning, approaching singularity. Delta {:.3f} for singularity at Delta=0.5'.format(np.abs(delta))) - yaw = np.arctan2(2*(q0*q3+q1*q2), (1-2*(q2**2+q3**2))) - pitch = np.arcsin(2*delta) - roll = np.arctan2(2*(q0*q1+q2*q3), (1-2*(q1**2+q2**2))) + yaw = np.arctan2(2 * (q0 * q3 + q1 * q2), (1 - 2 * (q2 ** 2 + q3 ** 2))) + pitch = np.arcsin(2 * delta) + roll = np.arctan2(2 * (q0 * q1 + q2 * q3), (1 - 2 * (q1 ** 2 + q2 ** 2))) return np.array([roll, pitch, yaw]) @@ -938,10 +894,10 @@ def crv_dot2Omega(crv, crv_dot): def quaternion_product(q, r): result = np.zeros((4,)) - result[0] = q[0]*r[0] - q[1]*r[1] - q[2]*r[2] - q[3]*r[3] - result[1] = q[0]*r[1] + q[1]*r[0] + q[2]*r[3] - q[3]*r[2] - result[2] = q[0]*r[2] - q[1]*r[3] + q[2]*r[0] + q[3]*r[1] - result[3] = q[0]*r[3] + q[1]*r[2] - q[2]*r[1] + q[3]*r[0] + result[0] = q[0] * r[0] - q[1] * r[1] - q[2] * r[2] - q[3] * r[3] + result[1] = q[0] * r[1] + q[1] * r[0] + q[2] * r[3] - q[3] * r[2] + result[2] = q[0] * r[2] - q[1] * r[3] + q[2] * r[0] + q[3] * r[1] + result[3] = q[0] * r[3] + q[1] * r[2] - q[2] * r[1] + q[3] * r[0] return result @@ -949,8 +905,8 @@ def omegadt2quat(omegadt): quat = np.zeros((4,)) omegadt_norm = np.linalg.norm(omegadt) - quat[0] = np.cos(0.5*omegadt_norm) - quat[1:4] = unit_vector(omegadt)*np.sin(0.5*omegadt_norm) + quat[0] = np.cos(0.5 * omegadt_norm) + quat[1:4] = unit_vector(omegadt) * np.sin(0.5 * omegadt_norm) return quat @@ -977,30 +933,24 @@ def get_triad(coordinates_def, frame_of_reference_delta, twist=None, n_nodes=3, for inode in range(n_nodes): v_vector = frame_of_reference_delta[inode, :] normal[inode, :] = unit_vector(np.cross( - tangent[inode, :], - v_vector - ) - ) + tangent[inode, :], + v_vector + ) + ) binormal[inode, :] = -unit_vector(np.cross( - tangent[inode, :], - normal[inode, :] - ) - ) + tangent[inode, :], + normal[inode, :] + ) + ) if twist is not None: - raise NotImplementedError('Structural twist is not yet supported in algebra.get_triad, but it is in beamstructures.py') - # # we apply twist now - # for inode in range(self.n_nodes): - # if not self.structural_twist[inode] == 0.0: - # rotation_mat = algebra.rotation_matrix_around_axis(tangent[inode, :], - # self.structural_twist[inode]) - # normal[inode, :] = np.dot(rotation_mat, normal[inode, :]) - # binormal[inode, :] = np.dot(rotation_mat, binormal[inode, :]) + raise NotImplementedError( + 'Structural twist is not yet supported in algebra.get_triad, but it is in beamstructures.py') return tangent, binormal, normal -def der_Cquat_by_v(q,v): +def der_Cquat_by_v(q, v): """ Being C=C(quat) the rotational matrix depending on the quaternion q and defined as C=quat2rotation(q), the function returns the derivative, w.r.t. the @@ -1014,19 +964,18 @@ def der_Cquat_by_v(q,v): where :math:`d(.)` is a delta operator. """ - vx,vy,vz=v - q0,q1,q2,q3=q - - return 2.*np.array( [[ q0*vx + q2*vz - q3*vy, q1*vx + q2*vy + q3*vz, - q0*vz + q1*vy - q2*vx, -q0*vy + q1*vz - q3*vx], - [ q0*vy - q1*vz + q3*vx, -q0*vz - q1*vy + q2*vx, - q1*vx + q2*vy + q3*vz, q0*vx + q2*vz - q3*vy], - [ q0*vz + q1*vy - q2*vx, q0*vy - q1*vz + q3*vx, - -q0*vx - q2*vz + q3*vy, q1*vx + q2*vy + q3*vz]]) + vx, vy, vz = v + q0, q1, q2, q3 = q + return 2. * np.array([[q0 * vx + q2 * vz - q3 * vy, q1 * vx + q2 * vy + q3 * vz, + q0 * vz + q1 * vy - q2 * vx, -q0 * vy + q1 * vz - q3 * vx], + [q0 * vy - q1 * vz + q3 * vx, -q0 * vz - q1 * vy + q2 * vx, + q1 * vx + q2 * vy + q3 * vz, q0 * vx + q2 * vz - q3 * vy], + [q0 * vz + q1 * vy - q2 * vx, q0 * vy - q1 * vz + q3 * vx, + -q0 * vx - q2 * vz + q3 * vy, q1 * vx + q2 * vy + q3 * vz]]) -def der_CquatT_by_v(q,v): +def der_CquatT_by_v(q, v): r""" Returns the derivative with respect to quaternion components of a projection matrix times a constant vector. @@ -1072,18 +1021,18 @@ def der_CquatT_by_v(q,v): np.array: :math:`\mathbf{D}` matrix. """ - vx,vy,vz=v - q0,q1,q2,q3=q + vx, vy, vz = v + q0, q1, q2, q3 = q - return 2.*np.array( [[ q0*vx - q2*vz + q3*vy, q1*vx + q2*vy + q3*vz, - - q0*vz + q1*vy - q2*vx, q0*vy + q1*vz - q3*vx], - [q0*vy + q1*vz - q3*vx, q0*vz - q1*vy + q2*vx, - q1*vx + q2*vy + q3*vz,-q0*vx + q2*vz - q3*vy], - [q0*vz - q1*vy + q2*vx, -q0*vy - q1*vz + q3*vx, - q0*vx - q2*vz + q3*vy, q1*vx + q2*vy + q3*vz]]) + return 2. * np.array([[q0 * vx - q2 * vz + q3 * vy, q1 * vx + q2 * vy + q3 * vz, + - q0 * vz + q1 * vy - q2 * vx, q0 * vy + q1 * vz - q3 * vx], + [q0 * vy + q1 * vz - q3 * vx, q0 * vz - q1 * vy + q2 * vx, + q1 * vx + q2 * vy + q3 * vz, -q0 * vx + q2 * vz - q3 * vy], + [q0 * vz - q1 * vy + q2 * vx, -q0 * vy - q1 * vz + q3 * vx, + q0 * vx - q2 * vz + q3 * vy, q1 * vx + q2 * vy + q3 * vz]]) -def der_Tan_by_xv(fv0,xv): +def der_Tan_by_xv(fv0, xv): """ Being fv0 a cartesian rotation vector and Tan the corresponding tangential operator (computed through crv2tan(fv)), the function returns the derivative @@ -1101,6 +1050,12 @@ def der_Tan_by_xv(fv0,xv): """ f0 = np.linalg.norm(fv0) + if f0 < (1e9 * np.finfo(float).eps): + tensor1 = np.array([[0., 0., 0.], [0., 0., -0.5], [0., 0.5, 0.]]) * xv[0] + tensor2 = np.array([[0., 0., 0.5], [0., 0., 0.], [-0.5, 0., 0.]]) * xv[1] + tensor3 = np.array([[0., -0.5, 0.], [0.5, 0., 0.], [0., 0., 0.]]) * xv[2] + return tensor1 + tensor2 + tensor3 + sf0, cf0 = np.sin(f0), np.cos(f0) fv0_x, fv0_y, fv0_z = fv0 @@ -1118,70 +1073,82 @@ def der_Tan_by_xv(fv0,xv): Ts02 = (1 - rs01) / f0p2 Ts04 = (1 - rs01) / f0p4 - # if f0<1e-8: rs01=1.0 # no need return np.array( - [[xv_x*((-fv0_y**2 - fv0_z**2)*(-cf0*fv0_x/f0p2 + fv0_x*rs03)/f0p2 - - 2*fv0_x*(1 - rs01)*(-fv0_y**2 - fv0_z**2)/f0p4) + xv_y*(fv0_x*fv0_y*( - -cf0*fv0_x/f0p2 + fv0_x*rs03)/f0p2 + fv0_y*Ts02 + - fv0_x*fv0_z*rs03 - 2*fv0_x**2*fv0_y*Ts04 + 2*fv0_x*fv0_z* - rc04) + xv_z*(fv0_x*fv0_z*(-cf0*fv0_x/f0p2 + fv0_x*rs03)/f0p2 + - fv0_z*Ts02 - fv0_x*fv0_y*rs03 - 2*fv0_x**2*fv0_z*Ts04 - - 2*fv0_x*fv0_y*rc04), - # - xv_x*(-2*fv0_y*Ts02 + (-fv0_y**2 - fv0_z**2)*(-cf0*fv0_y/f0p2 + - fv0_y*rs03)/f0p2 - 2*fv0_y*(1 - rs01)*(-fv0_y**2 - fv0_z**2)/f0p4) + - xv_y*(fv0_x*fv0_y*(-cf0*fv0_y/f0p2 + fv0_y*rs03)/f0p2 + fv0_x*Ts02 + - fv0_y*fv0_z*rs03 - 2*fv0_x*fv0_y**2*Ts04 + 2*fv0_y*fv0_z*rc04) - + xv_z*(fv0_x*fv0_z*(-cf0*fv0_y/f0p2 + fv0_y*rs03)/f0p2 + rc02 - - fv0_y**2*rs03 - 2*fv0_x*fv0_y*fv0_z*Ts04 - 2*fv0_y**2*rc04), + [[xv_x * ((-fv0_y ** 2 - fv0_z ** 2) * (-cf0 * fv0_x / f0p2 + fv0_x * rs03) / f0p2 - + 2 * fv0_x * (1 - rs01) * (-fv0_y ** 2 - fv0_z ** 2) / f0p4) + xv_y * (fv0_x * fv0_y * ( + -cf0 * fv0_x / f0p2 + fv0_x * rs03) / f0p2 + fv0_y * Ts02 + + fv0_x * fv0_z * rs03 - 2 * fv0_x ** 2 * fv0_y * Ts04 + 2 * fv0_x * fv0_z * + rc04) + xv_z * ( + fv0_x * fv0_z * (-cf0 * fv0_x / f0p2 + fv0_x * rs03) / f0p2 + + fv0_z * Ts02 - fv0_x * fv0_y * rs03 - 2 * fv0_x ** 2 * fv0_z * Ts04 + - 2 * fv0_x * fv0_y * rc04), + # + xv_x * (-2 * fv0_y * Ts02 + (-fv0_y ** 2 - fv0_z ** 2) * (-cf0 * fv0_y / f0p2 + + fv0_y * rs03) / f0p2 - 2 * fv0_y * (1 - rs01) * ( + -fv0_y ** 2 - fv0_z ** 2) / f0p4) + + xv_y * (fv0_x * fv0_y * (-cf0 * fv0_y / f0p2 + fv0_y * rs03) / f0p2 + fv0_x * Ts02 + + fv0_y * fv0_z * rs03 - 2 * fv0_x * fv0_y ** 2 * Ts04 + 2 * fv0_y * fv0_z * rc04) + + xv_z * (fv0_x * fv0_z * (-cf0 * fv0_y / f0p2 + fv0_y * rs03) / f0p2 + rc02 - + fv0_y ** 2 * rs03 - 2 * fv0_x * fv0_y * fv0_z * Ts04 - 2 * fv0_y ** 2 * rc04), # - xv_x*(-2*fv0_z*Ts02 + (-fv0_y**2 - fv0_z**2)*(-cf0*fv0_z/f0p2 - + fv0_z*rs03)/f0p2 - 2*fv0_z*(1 - rs01)*(-fv0_y**2 - fv0_z**2)/f0p4) + - xv_y*(fv0_x*fv0_y*(-cf0*fv0_z/f0p2 + fv0_z*rs03)/f0p2 - rc02 - + fv0_z**2*rs03 - 2*fv0_x*fv0_y*fv0_z*Ts04 + 2*fv0_z**2*rc04) - + xv_z*(fv0_x*fv0_z*(-cf0*fv0_z/f0p2 + fv0_z*rs03)/f0p2 + fv0_x*Ts02 - - fv0_y*fv0_z*rs03 - 2*fv0_x*fv0_z**2*Ts04 - 2*fv0_y*fv0_z*rc04)], - [xv_x*(fv0_x*fv0_y*(-cf0*fv0_x/f0p2 + fv0_x*rs03)/f0p2 + - fv0_y*Ts02 - fv0_x*fv0_z*rs03 - 2*fv0_x**2*fv0_y*Ts04 - - 2*fv0_x*fv0_z*rc04) + xv_y*(-2*fv0_x*Ts02 + - (-fv0_x**2 - fv0_z**2)*(-cf0*fv0_x/f0p2 + fv0_x*rs03)/f0p2 - - 2*fv0_x*(1 - rs01)*(-fv0_x**2 - fv0_z**2)/f0p4) + - xv_z*(fv0_y*fv0_z*(-cf0*fv0_x/f0p2 + fv0_x*rs03)/f0p2 - rc02 - + fv0_x**2*rs03 + 2*fv0_x**2*rc04 - 2*fv0_x*fv0_y*fv0_z*Ts04), - xv_x*(fv0_x*fv0_y*(-cf0*fv0_y/f0p2 + fv0_y*rs03)/f0p2 + - fv0_x*Ts02 - fv0_y*fv0_z*rs03 - 2*fv0_x*fv0_y**2*Ts04 - - 2*fv0_y*fv0_z*rc04) + xv_y*((-fv0_x**2 - fv0_z**2)*(-cf0*fv0_y/f0p2 - + fv0_y*rs03)/f0p2 - 2*fv0_y*(1 - rs01)*(-fv0_x**2 - fv0_z**2)/f0p4) - + xv_z*(fv0_y*fv0_z*(-cf0*fv0_y/f0p2 + fv0_y*rs03)/f0p2 + fv0_z*Ts02 - + fv0_x*fv0_y*rs03 + 2*fv0_x*fv0_y*rc04 - 2*fv0_y**2*fv0_z*Ts04), - xv_x*(fv0_x*fv0_y*(-cf0*fv0_z/f0p2 + fv0_z*rs03)/f0p2 + rc02 - fv0_z**2*rs03 - - 2*fv0_x*fv0_y*fv0_z*Ts04 - 2*fv0_z**2*rc04) + xv_y*(-2*fv0_z*Ts02 - + (-fv0_x**2 - fv0_z**2)*(-cf0*fv0_z/f0p2 + fv0_z*rs03)/f0p2 - - 2*fv0_z*(1 - rs01)*(-fv0_x**2 - fv0_z**2)/f0p4) + xv_z*(fv0_y*fv0_z*(-cf0*fv0_z/f0p2 - + fv0_z*rs03)/f0p2 + fv0_y*Ts02 + fv0_x*fv0_z*rs03 + 2*fv0_x*fv0_z*rc04 - - 2*fv0_y*fv0_z**2*Ts04)], - [xv_x*(fv0_x*fv0_z*(-cf0*fv0_x/f0p2 + fv0_x*rs03)/f0p2 + fv0_z*Ts02 - + fv0_x*fv0_y*rs03 - 2*fv0_x**2*fv0_z*Ts04 + 2*fv0_x*fv0_y*rc04) - + xv_y*(fv0_y*fv0_z*(-cf0*fv0_x/f0p2 + fv0_x*rs03)/f0p2 + rc02 - fv0_x**2*rs03 - - 2*fv0_x**2*rc04 - 2*fv0_x*fv0_y*fv0_z*Ts04) + xv_z*(-2*fv0_x*Ts02 - + (-fv0_x**2 - fv0_y**2)*(-cf0*fv0_x/f0p2 + fv0_x*rs03)/f0p2 - - 2*fv0_x*(1 - rs01)*(-fv0_x**2 - fv0_y**2)/f0p4), - xv_x*(fv0_x*fv0_z*(-cf0*fv0_y/f0p2 + fv0_y*rs03)/f0p2 - rc02 + fv0_y**2*rs03 - - 2*fv0_x*fv0_y*fv0_z*Ts04 + 2*fv0_y**2*rc04) + xv_y*(fv0_y*fv0_z*(-cf0*fv0_y/f0p2 - + fv0_y*rs03)/f0p2 + fv0_z*Ts02 - fv0_x*fv0_y*rs03 - 2*fv0_x*fv0_y*rc04 - - 2*fv0_y**2*fv0_z*Ts04) + xv_z*(-2*fv0_y*Ts02 + (-fv0_x**2 - - fv0_y**2)*(-cf0*fv0_y/f0p2 + fv0_y*rs03)/f0p2 - 2*fv0_y*(1 - rs01)*(-fv0_x**2 - - fv0_y**2)/f0p4), - xv_x*(fv0_x*fv0_z*(-cf0*fv0_z/f0p2 + fv0_z*rs03)/f0p2 + fv0_x*Ts02 + - fv0_y*fv0_z*rs03 - 2*fv0_x*fv0_z**2*Ts04 + 2*fv0_y*fv0_z*rc04) + - xv_y*(fv0_y*fv0_z*(-cf0*fv0_z/f0p2 + fv0_z*rs03)/f0p2 + fv0_y*Ts02 - - fv0_x*fv0_z*rs03 - 2*fv0_x*fv0_z*rc04 - 2*fv0_y*fv0_z**2*Ts04) + - xv_z*((-fv0_x**2 - fv0_y**2)*(-cf0*fv0_z/f0p2 + fv0_z*rs03)/f0p2 - - 2*fv0_z*(1 - rs01)*(-fv0_x**2 - fv0_y**2)/f0p4)]]) - # end der_Tan_by_xv - - -def der_TanT_by_xv(fv0,xv): + xv_x * (-2 * fv0_z * Ts02 + (-fv0_y ** 2 - fv0_z ** 2) * (-cf0 * fv0_z / f0p2 + + fv0_z * rs03) / f0p2 - 2 * fv0_z * (1 - rs01) * ( + -fv0_y ** 2 - fv0_z ** 2) / f0p4) + + xv_y * (fv0_x * fv0_y * (-cf0 * fv0_z / f0p2 + fv0_z * rs03) / f0p2 - rc02 + + fv0_z ** 2 * rs03 - 2 * fv0_x * fv0_y * fv0_z * Ts04 + 2 * fv0_z ** 2 * rc04) + + xv_z * (fv0_x * fv0_z * (-cf0 * fv0_z / f0p2 + fv0_z * rs03) / f0p2 + fv0_x * Ts02 + - fv0_y * fv0_z * rs03 - 2 * fv0_x * fv0_z ** 2 * Ts04 - 2 * fv0_y * fv0_z * rc04)], + [xv_x * (fv0_x * fv0_y * (-cf0 * fv0_x / f0p2 + fv0_x * rs03) / f0p2 + + fv0_y * Ts02 - fv0_x * fv0_z * rs03 - 2 * fv0_x ** 2 * fv0_y * Ts04 - + 2 * fv0_x * fv0_z * rc04) + xv_y * (-2 * fv0_x * Ts02 + + (-fv0_x ** 2 - fv0_z ** 2) * ( + -cf0 * fv0_x / f0p2 + fv0_x * rs03) / f0p2 + - 2 * fv0_x * (1 - rs01) * (-fv0_x ** 2 - fv0_z ** 2) / f0p4) + + xv_z * (fv0_y * fv0_z * (-cf0 * fv0_x / f0p2 + fv0_x * rs03) / f0p2 - rc02 + + fv0_x ** 2 * rs03 + 2 * fv0_x ** 2 * rc04 - 2 * fv0_x * fv0_y * fv0_z * Ts04), + xv_x * (fv0_x * fv0_y * (-cf0 * fv0_y / f0p2 + fv0_y * rs03) / f0p2 + + fv0_x * Ts02 - fv0_y * fv0_z * rs03 - 2 * fv0_x * fv0_y ** 2 * Ts04 + - 2 * fv0_y * fv0_z * rc04) + xv_y * ((-fv0_x ** 2 - fv0_z ** 2) * (-cf0 * fv0_y / f0p2 + + fv0_y * rs03) / f0p2 - 2 * fv0_y * ( + 1 - rs01) * (-fv0_x ** 2 - fv0_z ** 2) / f0p4) + + xv_z * (fv0_y * fv0_z * (-cf0 * fv0_y / f0p2 + fv0_y * rs03) / f0p2 + fv0_z * Ts02 + + fv0_x * fv0_y * rs03 + 2 * fv0_x * fv0_y * rc04 - 2 * fv0_y ** 2 * fv0_z * Ts04), + xv_x * (fv0_x * fv0_y * (-cf0 * fv0_z / f0p2 + fv0_z * rs03) / f0p2 + rc02 - fv0_z ** 2 * rs03 + - 2 * fv0_x * fv0_y * fv0_z * Ts04 - 2 * fv0_z ** 2 * rc04) + xv_y * (-2 * fv0_z * Ts02 + + (-fv0_x ** 2 - fv0_z ** 2) * ( + -cf0 * fv0_z / f0p2 + fv0_z * rs03) / f0p2 - + 2 * fv0_z * (1 - rs01) * ( + -fv0_x ** 2 - fv0_z ** 2) / f0p4) + xv_z * ( + fv0_y * fv0_z * (-cf0 * fv0_z / f0p2 + + fv0_z * rs03) / f0p2 + fv0_y * Ts02 + fv0_x * fv0_z * rs03 + 2 * fv0_x * fv0_z * rc04 + - 2 * fv0_y * fv0_z ** 2 * Ts04)], + [xv_x * (fv0_x * fv0_z * (-cf0 * fv0_x / f0p2 + fv0_x * rs03) / f0p2 + fv0_z * Ts02 + + fv0_x * fv0_y * rs03 - 2 * fv0_x ** 2 * fv0_z * Ts04 + 2 * fv0_x * fv0_y * rc04) + + xv_y * (fv0_y * fv0_z * (-cf0 * fv0_x / f0p2 + fv0_x * rs03) / f0p2 + rc02 - fv0_x ** 2 * rs03 + - 2 * fv0_x ** 2 * rc04 - 2 * fv0_x * fv0_y * fv0_z * Ts04) + xv_z * (-2 * fv0_x * Ts02 + + ( + -fv0_x ** 2 - fv0_y ** 2) * ( + -cf0 * fv0_x / f0p2 + fv0_x * rs03) / f0p2 - + 2 * fv0_x * (1 - rs01) * ( + -fv0_x ** 2 - fv0_y ** 2) / f0p4), + xv_x * (fv0_x * fv0_z * (-cf0 * fv0_y / f0p2 + fv0_y * rs03) / f0p2 - rc02 + fv0_y ** 2 * rs03 - + 2 * fv0_x * fv0_y * fv0_z * Ts04 + 2 * fv0_y ** 2 * rc04) + xv_y * ( + fv0_y * fv0_z * (-cf0 * fv0_y / f0p2 + + fv0_y * rs03) / f0p2 + fv0_z * Ts02 - fv0_x * fv0_y * rs03 - 2 * fv0_x * fv0_y * rc04 + - 2 * fv0_y ** 2 * fv0_z * Ts04) + xv_z * (-2 * fv0_y * Ts02 + (-fv0_x ** 2 + - fv0_y ** 2) * ( + -cf0 * fv0_y / f0p2 + fv0_y * rs03) / f0p2 - 2 * fv0_y * ( + 1 - rs01) * (-fv0_x ** 2 + - fv0_y ** 2) / f0p4), + xv_x * (fv0_x * fv0_z * (-cf0 * fv0_z / f0p2 + fv0_z * rs03) / f0p2 + fv0_x * Ts02 + + fv0_y * fv0_z * rs03 - 2 * fv0_x * fv0_z ** 2 * Ts04 + 2 * fv0_y * fv0_z * rc04) + + xv_y * (fv0_y * fv0_z * (-cf0 * fv0_z / f0p2 + fv0_z * rs03) / f0p2 + fv0_y * Ts02 + - fv0_x * fv0_z * rs03 - 2 * fv0_x * fv0_z * rc04 - 2 * fv0_y * fv0_z ** 2 * Ts04) + + xv_z * ((-fv0_x ** 2 - fv0_y ** 2) * (-cf0 * fv0_z / f0p2 + fv0_z * rs03) / f0p2 - + 2 * fv0_z * (1 - rs01) * (-fv0_x ** 2 - fv0_y ** 2) / f0p4)]]) + + +def der_TanT_by_xv(fv0, xv): """ Being fv0 a cartesian rotation vector and Tan the corresponding tangential operator (computed through crv2tan(fv)), the function returns the derivative @@ -1209,49 +1176,58 @@ def der_TanT_by_xv(fv0,xv): # Defining useful functions eps = 1e-15 - f0=np.linalg.norm(fv0) + f0 = np.linalg.norm(fv0) if f0 < eps: - f1 = -1.0/2.0 - f2 = 1.0/6.0 - g1 = -1.0/12.0 - g2 = 0.0 # TODO: check this + f1 = -1.0 / 2.0 + f2 = 1.0 / 6.0 + g1 = -1.0 / 12.0 + g2 = 0.0 # TODO: check this else: - f1 = (np.cos(f0)-1.0)/f0**2.0 - f2 = (1.0-np.sin(f0)/f0)/f0**2.0 - g1 = (f0*np.sin(f0)+2.0*(np.cos(f0)-1.0))/f0**4.0 - g2 = (2.0/f0**4 + np.cos(f0)/f0**4 - 3.0*np.sin(f0)/f0**5) + f1 = (np.cos(f0) - 1.0) / f0 ** 2.0 + f2 = (1.0 - np.sin(f0) / f0) / f0 ** 2.0 + g1 = (f0 * np.sin(f0) + 2.0 * (np.cos(f0) - 1.0)) / f0 ** 4.0 + g2 = (2.0 / f0 ** 4 + np.cos(f0) / f0 ** 4 - 3.0 * np.sin(f0) / f0 ** 5) # Computing the derivatives of the functions - df1dpx = -1.0*px*g1 - df1dpy = -1.0*py*g1 - df1dpz = -1.0*pz*g1 + df1dpx = -1.0 * px * g1 + df1dpy = -1.0 * py * g1 + df1dpz = -1.0 * pz * g1 - df2dpx = -1.0*px*g2 - df2dpy = -1.0*py*g2 - df2dpz = -1.0*pz*g2 + df2dpx = -1.0 * px * g2 + df2dpy = -1.0 * py * g2 + df2dpz = -1.0 * pz * g2 # Compute the output matrix - der_TanT_by_xv = np.zeros((3,3),) + der_TanT_by_xv = np.zeros((3, 3), ) # First column (derivatives with psi_x) - der_TanT_by_xv[0,0] = -1.0*df2dpx*(py**2+pz**2)*vx + df1dpx*pz*vy + df2dpx*px*py*vy + f2*py*vy - df1dpx*py*vz + df2dpx*px*pz*vz + f2*pz*vz - der_TanT_by_xv[1,0] = -1.0*df1dpx*pz*vx + df2dpx*px*py*vx + f2*py*vx - df2dpx*px**2*vy - 2.0*f2*px*vy - df2dpx*pz**2*vy + df1dpx*px*vz+f1*vz + df2dpx*py*pz*vz - der_TanT_by_xv[2,0] = df1dpx*py*vx + df2dpx*px*pz*vx + f2*pz*vx - df1dpx*px*vy -f1*vy + df2dpx*py*pz*vy - df2dpx*px**2*vz - 2.0*f2*px*vz - df2dpx*py**2*vz + der_TanT_by_xv[0, 0] = -1.0 * df2dpx * ( + py ** 2 + pz ** 2) * vx + df1dpx * pz * vy + df2dpx * px * py * vy + f2 * py * vy - df1dpx * py * vz + df2dpx * px * pz * vz + f2 * pz * vz + der_TanT_by_xv[ + 1, 0] = -1.0 * df1dpx * pz * vx + df2dpx * px * py * vx + f2 * py * vx - df2dpx * px ** 2 * vy - 2.0 * f2 * px * vy - df2dpx * pz ** 2 * vy + df1dpx * px * vz + f1 * vz + df2dpx * py * pz * vz + der_TanT_by_xv[ + 2, 0] = df1dpx * py * vx + df2dpx * px * pz * vx + f2 * pz * vx - df1dpx * px * vy - f1 * vy + df2dpx * py * pz * vy - df2dpx * px ** 2 * vz - 2.0 * f2 * px * vz - df2dpx * py ** 2 * vz # Second column (derivatives with psi_y) - der_TanT_by_xv[0,1] = -df2dpy*py**2*vx -f2*2*py*vx - df2dpy*pz**2*vx + df1dpy*pz*vy + df2dpy*px*py*vy +f2*px*vy - df1dpy*py*vz - f1*vz + df2dpy*px*pz*vz - der_TanT_by_xv[1,1] = -df1dpy*pz*vx + df2dpy*px*py*vx + f2*px*vx - df2dpy*px**2*vy - df2dpy*pz**2*vy + df1dpy*px*vz + df2dpy*py*pz*vz + f2*pz*vz - der_TanT_by_xv[2,1] = df1dpy*py*vx + f1*vx + df2dpy*px*pz*vx - df1dpy*px*vy + df2dpy*py*pz*vy + f2*pz*vy - df2dpy*px**2*vz - df2dpy*py**2*vz - 2.0*f2*py*vz + der_TanT_by_xv[ + 0, 1] = -df2dpy * py ** 2 * vx - f2 * 2 * py * vx - df2dpy * pz ** 2 * vx + df1dpy * pz * vy + df2dpy * px * py * vy + f2 * px * vy - df1dpy * py * vz - f1 * vz + df2dpy * px * pz * vz + der_TanT_by_xv[ + 1, 1] = -df1dpy * pz * vx + df2dpy * px * py * vx + f2 * px * vx - df2dpy * px ** 2 * vy - df2dpy * pz ** 2 * vy + df1dpy * px * vz + df2dpy * py * pz * vz + f2 * pz * vz + der_TanT_by_xv[ + 2, 1] = df1dpy * py * vx + f1 * vx + df2dpy * px * pz * vx - df1dpy * px * vy + df2dpy * py * pz * vy + f2 * pz * vy - df2dpy * px ** 2 * vz - df2dpy * py ** 2 * vz - 2.0 * f2 * py * vz # Second column (derivatives with psi_z) - der_TanT_by_xv[0,2] = -df2dpz*py**2*vx - df2dpz*pz**2*vx - 2.0*f2*pz*vx + df1dpz*pz*vy + f1*vy + df2dpz*px*py*vy - df1dpz*py*vz + df2dpz*px*pz*vz + f2*px*vz - der_TanT_by_xv[1,2] = -df1dpz*pz*vx - f1*vx + df2dpz*px*py*vx - df2dpz*px**2*vy - df2dpz*pz**2*vy - 2.0*f2*pz*vy + df1dpz*px*vz + df2dpz*py*pz*vz + f2*py*vz - der_TanT_by_xv[2,2] = df1dpz*py*vx + df2dpz*px*pz*vx + f2*px*vx - df1dpz*px*vy + df2dpz*py*pz*vy + f2*py*vy - df2dpz*px**2*vz - df2dpz*py**2*vz + der_TanT_by_xv[ + 0, 2] = -df2dpz * py ** 2 * vx - df2dpz * pz ** 2 * vx - 2.0 * f2 * pz * vx + df1dpz * pz * vy + f1 * vy + df2dpz * px * py * vy - df1dpz * py * vz + df2dpz * px * pz * vz + f2 * px * vz + der_TanT_by_xv[ + 1, 2] = -df1dpz * pz * vx - f1 * vx + df2dpz * px * py * vx - df2dpz * px ** 2 * vy - df2dpz * pz ** 2 * vy - 2.0 * f2 * pz * vy + df1dpz * px * vz + df2dpz * py * pz * vz + f2 * py * vz + der_TanT_by_xv[ + 2, 2] = df1dpz * py * vx + df2dpz * px * pz * vx + f2 * px * vx - df1dpz * px * vy + df2dpz * py * pz * vy + f2 * py * vy - df2dpz * px ** 2 * vz - df2dpz * py ** 2 * vz return der_TanT_by_xv -def der_Ccrv_by_v(fv0,v): +def der_Ccrv_by_v(fv0, v): r""" Being C=C(fv0) the rotational matrix depending on the Cartesian rotation vector fv0 and defined as C=crv2rotation(fv0), the function returns the @@ -1265,14 +1241,14 @@ def der_Ccrv_by_v(fv0,v): where :math:`d(.)` is a delta operator. """ - Cab0=crv2rotation(fv0) - T0=crv2tan(fv0) - vskew=skew(v) + Cab0 = crv2rotation(fv0) + T0 = crv2tan(fv0) + vskew = skew(v) - return -np.dot(Cab0,np.dot(vskew,T0)) + return -np.dot(Cab0, np.dot(vskew, T0)) -def der_CcrvT_by_v(fv0,v): +def der_CcrvT_by_v(fv0, v): """ Being C=C(fv0) the rotation matrix depending on the Cartesian rotation vector fv0 and defined as C=crv2rotation(fv0), the function returns the @@ -1286,10 +1262,10 @@ def der_CcrvT_by_v(fv0,v): where :math:`d(.)` is a delta operator. """ - Cba0=crv2rotation(fv0).T - T0=crv2tan(fv0) + Cba0 = crv2rotation(fv0).T + T0 = crv2tan(fv0) - return np.dot( skew( np.dot(Cba0,v) ),T0) + return np.dot(skew(np.dot(Cba0, v)), T0) def der_quat_wrt_crv(quat0): @@ -1403,16 +1379,16 @@ def der_Ceuler_by_v(euler, v): v2 = v[1] v3 = v[2] - res[0, 0] = v2*(sp*ss + cp*st*cp) + v3*(cp*ss - sp*st*cs) - res[0, 1] = v1*(-st*cs) + v2*(sp*ct*cs) + v3*(cp*ct*cs) - res[0, 2] = v1*(ct*ss) + v2*(-cp*cs - sp*st*ss) + v3*(sp*cs-cp*st*ss) + res[0, 0] = v2 * (sp * ss + cp * st * cp) + v3 * (cp * ss - sp * st * cs) + res[0, 1] = v1 * (-st * cs) + v2 * (sp * ct * cs) + v3 * (cp * ct * cs) + res[0, 2] = v1 * (ct * ss) + v2 * (-cp * cs - sp * st * ss) + v3 * (sp * cs - cp * st * ss) - res[1, 0] = v2*(-sp*cs+cp*st*ss) + v3*(-cp*cs + sp*st*ss) - res[1, 1] = v1*(-st*ss) + v2*(sp*ct*ss) + v3*(-cp*ct*ss) - res[1, 2] = v1*(ct*cs) + v2*(-cp*ss + sp*st*cs) + v3*(sp*ss + cp*st*cs) + res[1, 0] = v2 * (-sp * cs + cp * st * ss) + v3 * (-cp * cs + sp * st * ss) + res[1, 1] = v1 * (-st * ss) + v2 * (sp * ct * ss) + v3 * (-cp * ct * ss) + res[1, 2] = v1 * (ct * cs) + v2 * (-cp * ss + sp * st * cs) + v3 * (sp * ss + cp * st * cs) - res[2, 0] = v2*(cp*ct) + v3*(-sp*ct) - res[2, 1] = v1*(-ct) + v2*(-sp*st) + v3*(-cp*st) + res[2, 0] = v2 * (cp * ct) + v3 * (-sp * ct) + res[2, 1] = v1 * (-ct) + v2 * (-sp * st) + v3 * (-cp * st) return res @@ -1505,16 +1481,16 @@ def der_Peuler_by_v(euler, v): v2 = v[1] v3 = v[2] - res[0, 1] = v1*(-st*cs) + v2*(-st*ss) - v3*ct - res[0, 2] = -v1*(ct*ss) + v2*ct*cs + res[0, 1] = v1 * (-st * cs) + v2 * (-st * ss) - v3 * ct + res[0, 2] = -v1 * (ct * ss) + v2 * ct * cs - res[1, 0] = v1*(sp*ss + cp*st*cs) + v2*(-sp*cs + cp*st*ss) + v3 * (cp*ct) - res[1, 1] = v1*(sp*ct*cs) + v2*(sp*ct*ss) + v3*(-sp*st) - res[1, 2] = v1*(-cp*cs - sp*st*ss) + v2*(-cp*ss + sp*st*cs) + res[1, 0] = v1 * (sp * ss + cp * st * cs) + v2 * (-sp * cs + cp * st * ss) + v3 * (cp * ct) + res[1, 1] = v1 * (sp * ct * cs) + v2 * (sp * ct * ss) + v3 * (-sp * st) + res[1, 2] = v1 * (-cp * cs - sp * st * ss) + v2 * (-cp * ss + sp * st * cs) - res[2, 0] = v1*(cp*ss-sp*st*cs) + v2*(-cp*cs-sp*st*ss) + v3*(-sp*ct) - res[2, 1] = v1*(cp*ct*cs) + v2*(cp*ct*ss) + v3*(-cp*st) - res[2, 2] = v1*(sp*ss + -cp*st*ss) + v2*(sp*ss+cp*st*cs) + res[2, 0] = v1 * (cp * ss - sp * st * cs) + v2 * (-cp * cs - sp * st * ss) + v3 * (-sp * ct) + res[2, 1] = v1 * (cp * ct * cs) + v2 * (cp * ct * ss) + v3 * (-cp * st) + res[2, 2] = v1 * (sp * ss + -cp * st * ss) + v2 * (sp * ss + cp * st * cs) return res @@ -1602,29 +1578,29 @@ def der_Ceuler_by_v_NED(euler, v): v3 = v[2] res[0, 0] = 0 - res[0, 1] = - v1*st*cs - v2*st*ss - v3*ct - res[0, 2] = -v1*ct*ss + v2*ct*cs + res[0, 1] = - v1 * st * cs - v2 * st * ss - v3 * ct + res[0, 2] = -v1 * ct * ss + v2 * ct * cs - res[1, 0] = v1*(sp*ss + cp*st*cs) + v2*(-sp*cs + cp*st*ss) + v3*cp*ct - res[1, 1] = v1*(sp*ct*cs) + v2*sp*ct*ss - v3*sp*st - res[1, 2] = v1*(-cp*cs - sp*st*ss) + v2*(-cp*ss + sp*st*cs) + res[1, 0] = v1 * (sp * ss + cp * st * cs) + v2 * (-sp * cs + cp * st * ss) + v3 * cp * ct + res[1, 1] = v1 * (sp * ct * cs) + v2 * sp * ct * ss - v3 * sp * st + res[1, 2] = v1 * (-cp * cs - sp * st * ss) + v2 * (-cp * ss + sp * st * cs) - res[2, 0] = v1*(cp*ss - sp*st*cs) + v2*(-cp*cs-sp*st*ss) + v3*(-sp*ct) - res[2, 1] = v1*cp*ct*cs + v2*cp*ct*ss - v3*cp*st - res[2, 2] = v1*(sp*cs - cp*st*ss) + v2*(sp*ss + cp*st*cs) + res[2, 0] = v1 * (cp * ss - sp * st * cs) + v2 * (-cp * cs - sp * st * ss) + v3 * (-sp * ct) + res[2, 1] = v1 * cp * ct * cs + v2 * cp * ct * ss - v3 * cp * st + res[2, 2] = v1 * (sp * cs - cp * st * ss) + v2 * (sp * ss + cp * st * cs) return res -def cross3(v,w): +def cross3(v, w): """ Computes the cross product of two vectors (v and w) with size 3 """ - res = np.zeros((3,),) - res[0] = v[1]*w[2] - v[2]*w[1] - res[1] = -v[0]*w[2] + v[2]*w[0] - res[2] = v[0]*w[1] - v[1]*w[0] + res = np.zeros((3,), ) + res[0] = v[1] * w[2] - v[2] * w[1] + res[1] = -v[0] * w[2] + v[2] * w[0] + res[2] = v[0] * w[1] - v[1] * w[0] return res @@ -1757,8 +1733,6 @@ def der_Teuler_by_w(euler, w): np.ndarray: Computed :math:`\frac{\partial}{\partial\Theta}\left.\left(T^{GA}(\mathbf{\Theta})\mathbf{\omega}^A\right)\right|_{\Theta_0,\omega^A_0}` """ - - p = w[0] q = w[1] r = w[2] @@ -1854,30 +1828,6 @@ def der_Teuler_by_w_NED(euler, w): return derT -def multiply_matrices(*argv): - """ - multiply_matrices - - Multiply a series of matrices from left to right - - Args: - *argv: series of numpy arrays - Returns: - sol(numpy array): product of all the given matrices - - Examples: - solution = multiply_matrices(A, B, C) - """ - - size = np.shape(argv[0]) - nrow = size[0] - - sol = np.eye(nrow) - for M in argv: - sol = np.dot(sol, M) - return sol - - def norm3d(v): """ Norm of a 3D vector @@ -1891,7 +1841,7 @@ def norm3d(v): Returns: np.ndarray: Norm of the vector """ - return np.sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]) + return np.sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]) def normsq3d(v): @@ -1904,7 +1854,7 @@ def normsq3d(v): Returns: np.ndarray: Square of the norm of the vector """ - return v[0]*v[0]+v[1]*v[1]+v[2]*v[2] + return v[0] * v[0] + v[1] * v[1] + v[2] * v[2] def get_transformation_matrix(transformation): @@ -1981,18 +1931,36 @@ def der_skewp_skewp_v(p, v): .. math:: \frac{d}{d\boldsymbol{p}} (\tilde{\boldsymbol{p}} \tilde{\boldsymbol{p}} v) """ - der = np.zeros((3,3)) + der = np.zeros((3, 3)) - der[0, 0] = v[1]*p[1] + v[2]*p[2] - der[0, 1] = -2*v[0]*p[1] + v[1]*p[0] - der[0, 2] = -2*v[0]*p[2] + v[2]*p[0] + der[0, 0] = v[1] * p[1] + v[2] * p[2] + der[0, 1] = -2 * v[0] * p[1] + v[1] * p[0] + der[0, 2] = -2 * v[0] * p[2] + v[2] * p[0] - der[1, 0] = v[0]*p[1] - 2*v[1]*p[0] - der[1, 1] = v[0]*p[0] + v[2]*p[2] - der[1, 2] = -2*v[1]*p[2] + v[2]*p[1] + der[1, 0] = v[0] * p[1] - 2 * v[1] * p[0] + der[1, 1] = v[0] * p[0] + v[2] * p[2] + der[1, 2] = -2 * v[1] * p[2] + v[2] * p[1] - der[2, 0] = v[0]*p[2] - 2*v[2]*p[0] - der[2, 1] = v[1]*p[2] - 2*v[2]*p[1] - der[2, 2] = v[0]*p[0] + v[1]*p[1] + der[2, 0] = v[0] * p[2] - 2 * v[2] * p[0] + der[2, 1] = v[1] * p[2] - 2 * v[2] * p[1] + der[2, 2] = v[0] * p[0] + v[1] * p[1] return der + + +def der_skewpT_v(p, v): + """ + This function computes: + + .. math:: \frac{d}{d\boldsymbol{p}} \tilde{\boldsymbol{p}}^T v) + """ + return skew(v) + + +def der_skewp_v(p, v): + """ + This function computes: + + .. math:: \frac{d}{d\boldsymbol{p}} \tilde{\boldsymbol{p}} v) + """ + return -skew(v) diff --git a/sharpy/utils/generate_cases.py b/sharpy/utils/generate_cases.py index cb2a59b15..a5299e116 100644 --- a/sharpy/utils/generate_cases.py +++ b/sharpy/utils/generate_cases.py @@ -588,7 +588,10 @@ def create_frame_of_reference_delta(self, y_BFoR='y_AFoR'): y_BFoR (string): Direction of the yB axis """ - if y_BFoR == 'x_AFoR': + if isinstance(y_BFoR,(list,np.ndarray)): + yB = np.asarray(y_BFoR) + cout.cout_wrap(("WARNING: custom FoR delta defined, using the given value: y_BFoR = {y_BFoR}" % (y_BFoR)), 3) + elif y_BFoR == 'x_AFoR': yB = np.array([1.0, 0.0, 0.0]) elif y_BFoR == 'y_AFoR': yB = np.array([0.0, 1.0, 0.0]) @@ -1979,7 +1982,7 @@ def check(self): raise RuntimeError(("'%s' parameter required in '%s' lagrange constraint" % (param, self.behaviour))) has_behaviour = False for param, value in self.__dict__.items(): - if not param in ['behaviour', 'scalingFactor', 'penaltyFactor']: + if not param in ['behaviour', 'scalingFactor', 'penaltyFactor', 'rot_axisA2']: if param not in required_parameters: raise RuntimeError(("'%s' parameter is not required in '%s' lagrange constraint" % (param, self.behaviour))) if param == 'behaviour': @@ -2026,7 +2029,11 @@ def generate_multibody_file(list_LagrangeConstraints, list_Bodies, route, case_n data=getattr(constraint, "penaltyFactor")) except: pass - + try: + constraint_id.create_dataset("rot_axisA2", + data=getattr(constraint, "rot_axisA2")) + except: + pass iconstraint += 1 # Write the body information diff --git a/tests/coupled/multibody/test_double_pendulum_slanted.py b/tests/coupled/multibody/test_double_pendulum_slanted.py new file mode 100644 index 000000000..1e2c8eb35 --- /dev/null +++ b/tests/coupled/multibody/test_double_pendulum_slanted.py @@ -0,0 +1,338 @@ +import numpy as np +import unittest +import os +import shutil +from sharpy.utils.constants import deg2rad + + +class TestDoublePendulumSlanted(unittest.TestCase): + """ + Validation of a double pendulum with distributed mass and flared hinge axis at the connection + + As given in https://dx.doi.org/10.2514/6.2024-1441 + """ + + def _setUp(self, lateral): + import sharpy.utils.generate_cases as gc + import sharpy.utils.algebra as ag + + # Structural properties + + length_beam = 0.5 #meters + cx_length = 0.02 #meters + + A = cx_length * cx_length #assume rectangular cross section a= d^2 + material_density = 2700.0 #kg/m^3 + + mass_per_unit_length = material_density * A #kg/m + mass_iner = (mass_per_unit_length) * (cx_length * cx_length + cx_length * cx_length) / (12.0) + + EA = 2.800e7 + GA = 1.037e7 + GJ = 6.914e2 + EI = 9.333e2 + + lateral_ini = lateral + + # Beam1 + global nnodes1 + nnodes1 = 11 + l1 = length_beam + m1 = 0.0 + theta_ini1 = 90. * deg2rad + + # Beam2 + nnodes2 = nnodes1 + l2 = l1 + m2 = m1 + theta_ini2 = 90. * deg2rad + + # airfoils + airfoil = np.zeros((1, 20, 2), ) + airfoil[0, :, 0] = np.linspace(0., 1., 20) + + # Simulation + numtimesteps = 30 + dt = 0.01 + + # Create the structure + beam1 = gc.AeroelasticInformation() + r1 = np.linspace(0.0, l1, nnodes1) + node_pos1 = np.zeros((nnodes1, 3), ) + node_pos1[:, 0] = r1 * np.sin(theta_ini1) * np.cos(lateral_ini) + node_pos1[:, 1] = r1 * np.sin(theta_ini1) * np.sin(lateral_ini) + node_pos1[:, 2] = -r1 * np.cos(theta_ini1) + beam1.StructuralInformation.generate_uniform_beam(node_pos1, mass_per_unit_length, mass_iner, mass_iner / 2.0, + mass_iner / 2.0, np.zeros((3,), ), EA, GA, GA, GJ, EI, EI, + num_node_elem=3, y_BFoR='y_AFoR', num_lumped_mass=1) + beam1.StructuralInformation.body_number = np.zeros((beam1.StructuralInformation.num_elem,), dtype=int) + beam1.StructuralInformation.boundary_conditions[0] = 1 + beam1.StructuralInformation.boundary_conditions[-1] = -1 + beam1.StructuralInformation.lumped_mass_nodes = np.array([nnodes1 - 1], dtype=int) + beam1.StructuralInformation.lumped_mass = np.ones((1,)) * m1 + beam1.StructuralInformation.lumped_mass_inertia = np.zeros((1, 3, 3)) + beam1.StructuralInformation.lumped_mass_position = np.zeros((1, 3)) + beam1.AerodynamicInformation.create_one_uniform_aerodynamics( + beam1.StructuralInformation, + chord=1., + twist=0., + sweep=0., + num_chord_panels=4, + m_distribution='uniform', + elastic_axis=0.25, + num_points_camber=20, + airfoil=airfoil) + + beam2 = gc.AeroelasticInformation() + r2 = np.linspace(0.0, l2, nnodes2) + node_pos2 = np.zeros((nnodes2, 3), ) + node_pos2[:, 0] = r2 * np.sin(theta_ini2) * np.cos(lateral_ini) + node_pos1[-1, 0] + node_pos2[:, 1] = r2 * np.sin(theta_ini2) * np.sin(lateral_ini) + node_pos1[-1, 1] + node_pos2[:, 2] = -r2 * np.cos(theta_ini2) + node_pos1[-1, 2] + 0.00000001 + beam2.StructuralInformation.generate_uniform_beam(node_pos2, mass_per_unit_length, mass_iner, mass_iner / 2.0, + mass_iner / 2.0, np.zeros((3,), ), EA, GA, GA, GJ, EI, EI, + num_node_elem=3, y_BFoR='y_AFoR', num_lumped_mass=1) + beam2.StructuralInformation.body_number = np.zeros((beam1.StructuralInformation.num_elem,), dtype=int) + beam2.StructuralInformation.boundary_conditions[0] = 1 + beam2.StructuralInformation.boundary_conditions[-1] = -1 + beam2.StructuralInformation.lumped_mass_nodes = np.array([nnodes2 - 1], dtype=int) + beam2.StructuralInformation.lumped_mass = np.ones((1,)) * m2 + beam2.StructuralInformation.lumped_mass_inertia = np.zeros((1, 3, 3)) + beam2.StructuralInformation.lumped_mass_position = np.zeros((1, 3)) + beam2.AerodynamicInformation.create_one_uniform_aerodynamics( + beam2.StructuralInformation, + chord=1., + twist=0., + sweep=0., + num_chord_panels=4, + m_distribution='uniform', + elastic_axis=0.25, + num_points_camber=20, + airfoil=airfoil) + + beam1.assembly(beam2) + + # Simulation details + SimInfo = gc.SimulationInformation() + SimInfo.set_default_values() + + SimInfo.define_uinf(np.array([0.0, 1.0, 0.0]), 1.) + + SimInfo.solvers['SHARPy']['flow'] = ['BeamLoader', + 'AerogridLoader', + 'DynamicCoupled'] + + global name_hinge_slanted + name_hinge_slanted = 'name_hinge_slanted' + SimInfo.solvers['SHARPy']['case'] = name_hinge_slanted + SimInfo.solvers['SHARPy']['write_screen'] = 'off' + SimInfo.solvers['SHARPy']['route'] = os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + '/' + SimInfo.solvers['SHARPy']['log_folder'] = os.path.abspath( + os.path.dirname(os.path.realpath(__file__))) + '/output/' + SimInfo.set_variable_all_dicts('dt', dt) + SimInfo.define_num_steps(numtimesteps) + SimInfo.set_variable_all_dicts('rho', 0.0) + SimInfo.set_variable_all_dicts('velocity_field_input', SimInfo.solvers['SteadyVelocityField']) + SimInfo.set_variable_all_dicts('output', + os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + '/output/') + + SimInfo.solvers['BeamLoader']['unsteady'] = 'on' + + SimInfo.solvers['AerogridLoader']['unsteady'] = 'on' + SimInfo.solvers['AerogridLoader']['initial_align'] = 'off' + SimInfo.solvers['AerogridLoader']['aligned_grid'] = 'off' + SimInfo.solvers['AerogridLoader']['mstar'] = 2 + SimInfo.solvers['AerogridLoader']['wake_shape_generator'] = 'StraightWake' + SimInfo.solvers['AerogridLoader']['wake_shape_generator_input'] = {'u_inf': 1., + 'u_inf_direction': np.array([0., 1., 0.]), + 'dt': dt} + + SimInfo.solvers['WriteVariablesTime']['FoR_number'] = np.array([0, 1], dtype=int) + SimInfo.solvers['WriteVariablesTime']['FoR_variables'] = ['for_pos', 'mb_quat'] + SimInfo.solvers['WriteVariablesTime']['structure_nodes'] = np.array([nnodes1 - 1, nnodes1 + nnodes2 - 1], + dtype=int) + SimInfo.solvers['WriteVariablesTime']['structure_variables'] = ['pos'] + + SimInfo.solvers['NonLinearDynamicMultibody']['gravity_on'] = True + SimInfo.solvers['NonLinearDynamicMultibody']['gravity'] = 9.81 + SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator'] = 'NewmarkBeta' + SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator_settings'] = {'newmark_damp': 0.0, + 'dt': dt} + SimInfo.solvers['NonLinearDynamicMultibody']['write_lm'] = True + + SimInfo.solvers['BeamPlot']['include_FoR'] = True + + SimInfo.solvers['DynamicCoupled']['structural_solver'] = 'NonLinearDynamicMultibody' + SimInfo.solvers['DynamicCoupled']['structural_solver_settings'] = SimInfo.solvers['NonLinearDynamicMultibody'] + SimInfo.solvers['DynamicCoupled']['aero_solver'] = 'NoAero' + SimInfo.solvers['DynamicCoupled']['aero_solver_settings'] = SimInfo.solvers['NoAero'] + SimInfo.solvers['DynamicCoupled']['postprocessors'] = ['WriteVariablesTime', 'BeamPlot', 'AerogridPlot'] + SimInfo.solvers['DynamicCoupled']['postprocessors_settings'] = { + 'WriteVariablesTime': SimInfo.solvers['WriteVariablesTime'], + 'BeamPlot': SimInfo.solvers['BeamPlot'], + 'AerogridPlot': SimInfo.solvers['AerogridPlot'] + } + + SimInfo.with_forced_vel = False + SimInfo.with_dynamic_forces = False + + # Create the MB and BC files + LC1 = gc.LagrangeConstraint() + LC1.behaviour = 'hinge_FoR' + LC1.body_FoR = 0 + LC1.rot_axis_AFoR = np.array([0.0, 1.0, 0.0]) + LC1.scalingFactor = 1e6 + LC1.penaltyFactor = 0. + + LC2 = gc.LagrangeConstraint() + LC2.behaviour = 'hinge_node_FoR' + LC2.node_in_body = nnodes1 - 1 + LC2.body = 0 + LC2.body_FoR = 1 + LC2.rot_axisB = np.array([np.sin(45.0 * deg2rad), np.cos(45.0 * deg2rad), 0.0]) + LC2.rot_axisA2 = np.array([np.sin(45.0 * deg2rad), np.cos(45.0 * deg2rad), 0.0]) + LC2.scalingFactor = 1e6 + LC2.penaltyFactor = 0. + + LC = [] + LC.append(LC1) + LC.append(LC2) + + MB1 = gc.BodyInformation() + MB1.body_number = 0 + MB1.FoR_position = np.zeros(6) + MB1.FoR_velocity = np.zeros(6) + MB1.FoR_acceleration = np.zeros(6) + MB1.FoR_movement = 'free' + MB1.quat = ag.rotation2quat(ag.rotation3d_z(lateral_ini) @ ag.quat2rotation(np.array([1., 0., 0., 0.]))) + + MB2 = gc.BodyInformation() + MB2.body_number = 1 + MB2.FoR_position = np.array([node_pos2[0, 0], node_pos2[0, 1], node_pos2[0, 2], 0., 0., 0.]) + MB2.FoR_velocity = np.zeros(6) + MB2.FoR_acceleration = np.zeros(6) + MB2.FoR_movement = 'free' + MB2.quat = ag.rotation2quat(ag.rotation3d_z(lateral_ini) @ ag.quat2rotation(np.array([1., 0., 0., 0.]))) + + MB = [] + MB.append(MB1) + MB.append(MB2) + + # Write files + gc.clean_test_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + SimInfo.generate_solver_file() + SimInfo.generate_dyn_file(numtimesteps) + beam1.generate_h5_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + gc.generate_multibody_file(LC, MB, SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + + # Same case with penalty weights + global name_hinge_slanted_pen + name_hinge_slanted_pen = 'name_hinge_slanted_pen' + SimInfo.solvers['SHARPy']['case'] = name_hinge_slanted_pen + + LC1.scalingFactor = 1e-24 + LC1.penaltyFactor = 1e0 + LC2.scalingFactor = 1e-24 + LC2.penaltyFactor = 1e0 + + gc.clean_test_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + SimInfo.generate_solver_file() + SimInfo.generate_dyn_file(numtimesteps) + beam1.generate_h5_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + gc.generate_multibody_file(LC, MB, SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + + # Same case with rotated global reference + global name_hinge_slanted_lateralrot + name_hinge_slanted_lateralrot = 'name_hinge_slanted_lateralrot' + SimInfo.solvers['SHARPy']['case'] = name_hinge_slanted_lateralrot + + LC2.rot_axisB = np.array([np.sin(45.0 * deg2rad), np.cos(45.0 * deg2rad), 0.0]) + LC2.rot_axisA2 = np.array([np.sin(45.0 * deg2rad), np.cos(45.0 * deg2rad), 0.0]) + + gc.clean_test_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + SimInfo.generate_solver_file() + SimInfo.generate_dyn_file(numtimesteps) + beam1.generate_h5_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + gc.generate_multibody_file(LC, MB, SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + + # # Same case with spherical joints + # global name_spherical + # name_spherical = 'dpg_spherical' + # SimInfo.solvers['SHARPy']['case'] = name_spherical + + # SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator'] = 'NewmarkBeta' + # SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator_settings'] = {'newmark_damp': 0.15, + # 'dt': dt} + + # LC1 = gc.LagrangeConstraint() + # LC1.behaviour = 'spherical_FoR' + # LC1.body_FoR = 0 + # LC1.scalingFactor = 1e6 + + # LC2 = gc.LagrangeConstraint() + # LC2.behaviour = 'spherical_node_FoR' + # LC2.node_in_body = nnodes1-1 + # LC2.body = 0 + # LC2.body_FoR = 1 + # LC2.scalingFactor = 1e6 + + gc.clean_test_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + SimInfo.generate_solver_file() + SimInfo.generate_dyn_file(numtimesteps) + beam1.generate_h5_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + gc.generate_multibody_file(LC, MB, SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case']) + + def run_and_assert(self, name, lateral): + import sharpy.sharpy_main + import sharpy.utils.algebra as ag + + solver_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__)) + '/' + name + '.sharpy') + sharpy.sharpy_main.main(['', solver_path]) + + # read output and compare + output_path = os.path.abspath( + os.path.dirname(os.path.realpath(__file__))) + '/output/' + name + '/WriteVariablesTime/' + pos_tip_data = np.loadtxt(("%sstruct_pos_node%d.dat" % (output_path, nnodes1 * 2 - 1)), ) + for_pos_tip_data = np.loadtxt(("%sFoR_%02d_for_pos.dat" % (output_path, 0)), ) + quat_tip_data = np.loadtxt(("%sFoR_%02d_mb_quat.dat" % (output_path, 0)), ) + + calc_pos_tip_data = ag.rotation3d_z(-lateral) @ (for_pos_tip_data[-1, 1:4] + + ag.quat2rotation(quat_tip_data[-1, 1:]) + @ pos_tip_data[-1, 1:]) + + self.assertAlmostEqual(calc_pos_tip_data[0], 0.80954978, 4) + self.assertAlmostEqual(calc_pos_tip_data[1], 0.1024842, 4) + self.assertAlmostEqual(calc_pos_tip_data[2], -0.48183994, 4) + + def test_doublependulum_hinge_slanted(self): + lateral_hinge = 0. * deg2rad + self._setUp(lateral_hinge) + self.run_and_assert(name_hinge_slanted, lateral_hinge) + + def test_doublependulum_hinge_slanted_pen(self): + lateral_hinge = 0. * deg2rad + self._setUp(lateral_hinge) + self.run_and_assert(name_hinge_slanted_pen, lateral_hinge) + + def test_doublependulum_hinge_slanted_lateralrot(self): + lateral_hinge = 30. * deg2rad + self._setUp(lateral_hinge) + self.run_and_assert(name_hinge_slanted_lateralrot, lateral_hinge) + + def tearDown(self): + solver_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + solver_path += '/' + for name in [name_hinge_slanted, name_hinge_slanted_pen, name_hinge_slanted_lateralrot]: + files_to_delete = [name + '.aero.h5', + name + '.dyn.h5', + name + '.fem.h5', + name + '.mb.h5', + name + '.sharpy'] + for f in files_to_delete: + os.remove(solver_path + f) + + shutil.rmtree(solver_path + 'output/') + + +if __name__ == '__main__': + unittest.main()