def test_crv_tangetial_operator_transpose_derivative(self): """ Checks Cartesian rotation vector tangential operator transpose""" # dummy vector xv = np.random.rand(3) # linearisation point fi0 = 2.0 * np.pi * np.random.rand(1) nv0 = np.random.rand(3) nv0 = nv0 / np.linalg.norm(nv0) fv0 = fi0 * nv0 T0_T = np.transpose(algebra.crv2tan(fv0)) T0_Txv = np.dot(T0_T, xv) # Analytical solution derT_T_an = algebra.der_TanT_by_xv(fv0, xv) # End point fi1 = 2.0 * np.pi * np.random.rand() nv1 = np.random.rand(3) nv1 = nv1 / np.linalg.norm(nv1) fv1 = fi1 * nv1 er = 10. A = np.array([1e-1, 1e-2, 1e-3, 1e-4, 1e-5, 1e-6]) for a in A: # perturbed fv = a * fv1 + (1. - a) * fv0 dfv = fv - fv0 Tpert_T = np.transpose(algebra.crv2tan(fv)) Tpert_Txv = np.dot(Tpert_T, xv) dT_T_num = Tpert_Txv - T0_Txv dT_T_an = np.dot(derT_T_an, dfv) # Error er_new = np.max(np.abs(dT_T_num - dT_T_an)) / np.max( np.abs(dT_T_an)) assert er_new < er, 'der_TanT_by_xv error not converging to 0' er = er_new assert er < A[-2], 'der_TanT_by_xv error too large'
def test_crv_tangetial_operator_derivative(self): """ Checks Cartesian rotation vector tangential operator """ # linearisation point fi0 = np.pi / 6 nv0 = np.array([1, 3, 1]) nv0 = nv0 / np.linalg.norm(nv0) fv0 = fi0 * nv0 T0 = algebra.crv2tan(fv0) # dummy vector xv = np.ones((3, )) T0xv = np.dot(T0, xv) # derT_an=dTxv(fv0,xv) derT_an = algebra.der_Tan_by_xv(fv0, xv) # derT_an=algebra.der_Tan_by_xv_an(fv0,xv) # dummy fi1 = np.pi / 3 nv1 = np.array([4, 1, -2]) nv1 = nv1 / np.linalg.norm(nv1) fv1 = fi1 * nv1 er = 10. A = np.array([1e-1, 1e-2, 1e-3, 1e-4, 1e-5, 1e-6]) for a in A: # perturbed fv = a * fv1 + (1. - a) * fv0 dfv = fv - fv0 Tpert = algebra.crv2tan(fv) Tpertxv = np.dot(Tpert, xv) dT_num = Tpertxv - T0xv dT_an = np.dot(derT_an, dfv) er_new = np.max(np.abs(dT_num - dT_an)) / np.max(np.abs(dT_an)) assert er_new < er, 'der_Tan_by_xv error not converging to 0' er = er_new assert er < A[-2], 'der_Tan_by_xv error too large'
def test_crv_tangential_operator(self): """ Checks Cartesian rotation vector tangential operator """ # linearisation point fi0 = -np.pi / 6 nv0 = np.array([1, 3, 1]) nv0 = np.array([1, 0, 0]) nv0 = nv0 / np.linalg.norm(nv0) fv0 = fi0 * nv0 Cab = algebra.crv2rotation(fv0) # fv0 is rotation from A to B # dummy fi1 = np.pi / 3 nv1 = np.array([2, 4, 1]) nv1 = nv1 / np.linalg.norm(nv1) fv1 = fi1 * nv1 er_tan = 10. A = np.array([1e-1, 1e-2, 1e-3, 1e-4, 1e-5, 1e-6]) for a in A: # perturbed fv = a * fv1 + (1. - a) * fv0 dfv = fv - fv0 ### Compute relevant quantities dCab = algebra.crv2rotation(fv0 + dfv) - Cab T = algebra.crv2tan(fv0) Tdfv = np.dot(T, dfv) Tdfv_skew = algebra.skew(Tdfv) dCab_an = np.dot(Cab, Tdfv_skew) er_tan_new = np.max(np.abs(dCab - dCab_an)) / np.max( np.abs(dCab_an)) assert er_tan_new < er_tan, 'crv2tan error not converging to 0' er_tan = er_tan_new assert er_tan < A[-2], 'crv2tan error too large'
def get_gebm2uvlm_gains(self, data): r""" Provides: - the gain matrices required to connect the linearised GEBM and UVLM inputs/outputs - the stiffening and damping factors to be added to the linearised GEBM equations in order to account for non-zero aerodynamic loads at the linearisation point. The function produces the gain matrices: - ``Kdisp``: gains from GEBM to UVLM grid displacements - ``Kvel_disp``: influence of GEBM dofs displacements to UVLM grid velocities. - ``Kvel_vel``: influence of GEBM dofs displacements to UVLM grid displacements. - ``Kforces`` (UVLM->GEBM) dimensions are the transpose than the Kdisp and Kvel* matrices. Hence, when allocation this term, ``ii`` and ``jj`` indices will unintuitively refer to columns and rows, respectively. And the stiffening/damping terms accounting for non-zero aerodynamic forces at the linearisation point: - ``Kss``: stiffness factor (flexible dof -> flexible dof) accounting for non-zero forces at the linearisation point. - ``Csr``: damping factor (rigid dof -> flexible dof) - ``Crs``: damping factor (flexible dof -> rigid dof) - ``Crr``: damping factor (rigid dof -> rigid dof) Stiffening and damping related terms due to the non-zero aerodynamic forces at the linearisation point: .. math:: \mathbf{F}_{A,n} = C^{AG}(\mathbf{\chi})\sum_j \mathbf{f}_{G,j} \rightarrow \delta\mathbf{F}_{A,n} = C^{AG}_0 \sum_j \delta\mathbf{f}_{G,j} + \frac{\partial}{\partial\chi}(C^{AG}\sum_j \mathbf{f}_{G,j}^0)\delta\chi The term multiplied by the variation in the quaternion, :math:`\delta\chi`, couples the forces with the rigid body equations and becomes part of :math:`\mathbf{C}_{sr}`. Similarly, the linearisation of the moments results in expression that contribute to the stiffness and damping matrices. .. math:: \mathbf{M}_{B,n} = \sum_j \tilde{X}_B C^{BA}(\Psi)C^{AG}(\chi)\mathbf{f}_{G,j} .. math:: \delta\mathbf{M}_{B,n} = \sum_j \tilde{X}_B\left(C_0^{BG}\delta\mathbf{f}_{G,j} + \frac{\partial}{\partial\Psi}(C^{BA}\delta\mathbf{f}^0_{A,j})\delta\Psi + \frac{\partial}{\partial\chi}(C^{BA}_0 C^{AG} \mathbf{f}_{G,j})\delta\chi\right) The linearised equations of motion for the geometrically exact beam model take the input term :math:`\delta \mathbf{Q}_n = \{\delta\mathbf{F}_{A,n},\, T_0^T\delta\mathbf{M}_{B,n}\}`, which means that the moments should be provided as :math:`T^T(\Psi)\mathbf{M}_B` instead of :math:`\mathbf{M}_A = C^{AB}\mathbf{M}_B`, where :math:`T(\Psi)` is the tangential operator. .. math:: \delta(T^T\mathbf{M}_B) = T^T_0\delta\mathbf{M}_B + \frac{\partial}{\partial\Psi}(T^T\delta\mathbf{M}_B^0)\delta\Psi is the linearised expression for the moments, where the first term would correspond to the input terms to the beam equations and the second arises due to the non-zero aerodynamic moment at the linearisation point and must be subtracted (since it comes from the forces) to form part of :math:`\mathbf{K}_{ss}`. In addition, the :math:`\delta\mathbf{M}_B` term depends on both :math:`\delta\Psi` and :math:`\delta\chi`, therefore those terms would also contribute to :math:`\mathbf{K}_{ss}` and :math:`\mathbf{C}_{sr}`, respectively. The contribution from the total forces and moments will be accounted for in :math:`\mathbf{C}_{rr}` and :math:`\mathbf{C}_{rs}`. .. math:: \delta\mathbf{F}_{tot,A} = \sum_n\left(C^{GA}_0 \sum_j \delta\mathbf{f}_{G,j} + \frac{\partial}{\partial\chi}(C^{AG}\sum_j \mathbf{f}_{G,j}^0)\delta\chi\right) Therefore, after running this method, the beam matrices will be updated as: >>> K_beam[:flex_dof, :flex_dof] += Kss >>> C_beam[:flex_dof, -rigid_dof:] += Csr >>> C_beam[-rigid_dof:, :flex_dof] += Crs >>> C_beam[-rigid_dof:, -rigid_dof:] += Crr Track body option The ``track_body`` setting restricts the UVLM grid to linear translation motions and therefore should be used to ensure that the forces are computed using the reference linearisation frame. The UVLM and beam are linearised about a reference equilibrium condition. The UVLM is defined in the inertial reference frame while the beam employs the body attached frame and therefore a projection from one frame onto another is required during the coupling process. However, the inputs to the UVLM (i.e. the lattice grid coordinates) are obtained from the beam deformation which is expressed in A frame and therefore the grid coordinates need to be projected onto the inertial frame ``G``. As the beam rotates, the projection onto the ``G`` frame of the lattice grid coordinates will result in a grid that is not coincident with that at the linearisation reference and therefore the grid coordinates must be projected onto the original frame, which will be referred to as ``U``. The transformation between the inertial frame ``G`` and the ``U`` frame is a function of the rotation of the ``A`` frame and the original position: .. math:: C^{UG}(\chi) = C^{GA}(\chi_0)C^{AG}(\chi) Therefore, the grid coordinates obtained in ``A`` frame and projected onto the ``G`` frame can be transformed to the ``U`` frame using .. math:: \zeta_U = C^{UG}(\chi) \zeta_G which allows the grid lattice coordinates to be projected onto the original linearisation frame. In a similar fashion, the output lattice vertex forces of the UVLM are defined in the original linearisation frame ``U`` and need to be transformed onto the inertial frame ``G`` prior to projecting them onto the ``A`` frame to use them as the input forces to the beam system. .. math:: \boldsymbol{f}_G = C^{GU}(\chi)\boldsymbol{f}_U The linearisation of the above relations lead to the following expressions that have to be added to the coupling matrices: * ``Kdisp_vel`` terms: .. math:: \delta\boldsymbol{\zeta}_U= C^{GA}_0 \frac{\partial}{\partial \boldsymbol{\chi}} \left(C^{AG}\boldsymbol{\zeta}_{G,0}\right)\delta\boldsymbol{\chi} + \delta\boldsymbol{\zeta}_G * ``Kvel_vel`` terms: .. math:: \delta\dot{\boldsymbol{\zeta}}_U= C^{GA}_0 \frac{\partial}{\partial \boldsymbol{\chi}} \left(C^{AG}\dot{\boldsymbol{\zeta}}_{G,0}\right)\delta\boldsymbol{\chi} + \delta\dot{\boldsymbol{\zeta}}_G The transformation of the forces and moments introduces terms that are functions of the orientation and are included as stiffening and damping terms in the beam's matrices: * ``Csr`` damping terms relating to translation forces: .. math:: C_{sr}^{tra} -= \frac{\partial}{\partial\boldsymbol{\chi}} \left(C^{GA} C^{AG}_0 \boldsymbol{f}_{G,0}\right)\delta\boldsymbol{\chi} * ``Csr`` damping terms related to moments: .. math:: C_{sr}^{rot} -= T^\top\widetilde{\mathbf{X}}_B C^{BG} \frac{\partial}{\partial\boldsymbol{\chi}} \left(C^{GA} C^{AG}_0 \boldsymbol{f}_{G,0}\right)\delta\boldsymbol{\chi} The ``track_body`` setting. When ``track_body`` is enabled, the UVLM grid is no longer coincident with the inertial reference frame throughout the simulation but rather it is able to rotate as the ``A`` frame rotates. This is to simulate a free flying vehicle, where, for instance, the orientation does not affect the aerodynamics. The UVLM defined in this frame of reference, named ``U``, satisfies the following convention: * The ``U`` frame is coincident with the ``G`` frame at the time of linearisation. * The ``U`` frame rotates as the ``A`` frame rotates. Transformations related to the ``U`` frame of reference: * The angle between the ``U`` frame and the ``A`` frame is always constant and equal to :math:`\boldsymbol{\Theta}_0`. * The angle between the ``A`` frame and the ``G`` frame is :math:`\boldsymbol{\Theta}=\boldsymbol{\Theta}_0 + \delta\boldsymbol{\Theta}` * The projection of a vector expressed in the ``G`` frame onto the ``U`` frame is expressed by: .. math:: \boldsymbol{v}^U = C^{GA}_0 C^{AG} \boldsymbol{v}^G * The reverse, a projection of a vector expressed in the ``U`` frame onto the ``G`` frame, is expressed by .. math:: \boldsymbol{v}^U = C^{GA} C^{AG}_0 \boldsymbol{v}^U The effect this has on the aeroelastic coupling between the UVLM and the structural dynamics is that the orientation and change of orientation of the vehicle has no effect on the aerodynamics. The aerodynamics are solely affected by the contribution of the 6-rigid body velocities (as well as the flexible DOFs velocities). """ aero = data.aero structure = data.structure tsaero = self.uvlm.tsaero0 tsstr = self.beam.tsstruct0 Kzeta = self.uvlm.sys.Kzeta num_dof_str = self.beam.sys.num_dof_str num_dof_rig = self.beam.sys.num_dof_rig num_dof_flex = self.beam.sys.num_dof_flex use_euler = self.beam.sys.use_euler # allocate output Kdisp = np.zeros((3 * Kzeta, num_dof_str)) Kdisp_vel = np.zeros( (3 * Kzeta, num_dof_str)) # Orientation is in velocity DOFs Kvel_disp = np.zeros((3 * Kzeta, num_dof_str)) Kvel_vel = np.zeros((3 * Kzeta, num_dof_str)) Kforces = np.zeros((num_dof_str, 3 * Kzeta)) Kss = np.zeros((num_dof_flex, num_dof_flex)) Csr = np.zeros((num_dof_flex, num_dof_rig)) Crs = np.zeros((num_dof_rig, num_dof_flex)) Crr = np.zeros((num_dof_rig, num_dof_rig)) Krs = np.zeros((num_dof_rig, num_dof_flex)) # get projection matrix A->G # (and other quantities indep. from nodal position) Cga = algebra.quat2rotation(tsstr.quat) # NG 6-8-19 removing .T Cag = Cga.T # for_pos=tsstr.for_pos for_vel = tsstr.for_vel[:3] for_rot = tsstr.for_vel[3:] skew_for_rot = algebra.skew(for_rot) Der_vel_Ra = np.dot(Cga, skew_for_rot) Faero = np.zeros(3) FaeroA = np.zeros(3) # GEBM degrees of freedom jj_for_tra = range(num_dof_str - num_dof_rig, num_dof_str - num_dof_rig + 3) jj_for_rot = range(num_dof_str - num_dof_rig + 3, num_dof_str - num_dof_rig + 6) if use_euler: jj_euler = range(num_dof_str - 3, num_dof_str) euler = algebra.quat2euler(tsstr.quat) tsstr.euler = euler else: jj_quat = range(num_dof_str - 4, num_dof_str) jj = 0 # nodal dof index for node_glob in range(structure.num_node): ### detect bc at node (and no. of dofs) bc_here = structure.boundary_conditions[node_glob] if bc_here == 1: # clamp (only rigid-body) dofs_here = 0 jj_tra, jj_rot = [], [] # continue elif bc_here == -1 or bc_here == 0: # (rigid+flex body) dofs_here = 6 jj_tra = 6 * structure.vdof[node_glob] + np.array([0, 1, 2], dtype=int) jj_rot = 6 * structure.vdof[node_glob] + np.array([3, 4, 5], dtype=int) else: raise NameError('Invalid boundary condition (%d) at node %d!' \ % (bc_here, node_glob)) jj += dofs_here # retrieve element and local index ee, node_loc = structure.node_master_elem[node_glob, :] # get position, crv and rotation matrix Ra = tsstr.pos[node_glob, :] # in A FoR, w.r.t. origin A-G Rg = np.dot(Cag.T, Ra) # in G FoR, w.r.t. origin A-G psi = tsstr.psi[ee, node_loc, :] psi_dot = tsstr.psi_dot[ee, node_loc, :] Cab = algebra.crv2rotation(psi) Cba = Cab.T Cbg = np.dot(Cab.T, Cag) Tan = algebra.crv2tan(psi) track_body = self.settings['track_body'] ### str -> aero mapping # some nodes may be linked to multiple surfaces... for str2aero_here in aero.struct2aero_mapping[node_glob]: # detect surface/span-wise coordinate (ss,nn) nn, ss = str2aero_here['i_n'], str2aero_here['i_surf'] # print('%.2d,%.2d'%(nn,ss)) # surface panelling M = aero.aero_dimensions[ss][0] N = aero.aero_dimensions[ss][1] Kzeta_start = 3 * sum(self.uvlm.sys.MS.KKzeta[:ss]) shape_zeta = (3, M + 1, N + 1) for mm in range(M + 1): # get bound vertex index ii_vert = [ Kzeta_start + np.ravel_multi_index( (cc, mm, nn), shape_zeta) for cc in range(3) ] # get position vectors zetag = tsaero.zeta[ss][:, mm, nn] # in G FoR, w.r.t. origin A-G zetaa = np.dot(Cag, zetag) # in A FoR, w.r.t. origin A-G Xg = zetag - Rg # in G FoR, w.r.t. origin B Xb = np.dot(Cbg, Xg) # in B FoR, w.r.t. origin B # get rotation terms Xbskew = algebra.skew(Xb) XbskewTan = np.dot(Xbskew, Tan) # get velocity terms zetag_dot = tsaero.zeta_dot[ss][:, mm, nn] - Cga.dot( for_vel) # in G FoR, w.r.t. origin A-G zetaa_dot = np.dot( Cag, zetag_dot) # in A FoR, w.r.t. origin A-G # get aero force faero = tsaero.forces[ss][:3, mm, nn] Faero += faero faero_a = np.dot(Cag, faero) FaeroA += faero_a maero_g = np.cross(Xg, faero) maero_b = np.dot(Cbg, maero_g) ### ---------------------------------------- allocate Kdisp if bc_here != 1: # wrt pos - Eq 25 second term Kdisp[np.ix_(ii_vert, jj_tra)] += Cga # wrt psi - Eq 26 Kdisp[np.ix_(ii_vert, jj_rot)] -= np.dot(Cbg.T, XbskewTan) # w.r.t. position of FoR A (w.r.t. origin G) # null as A and G have always same origin in SHARPy # # ### w.r.t. quaternion (attitude changes) if use_euler: Kdisp_vel[np.ix_(ii_vert, jj_euler)] += \ algebra.der_Ceuler_by_v(tsstr.euler, zetaa) # Track body - project inputs as for A not moving if track_body: Kdisp_vel[np.ix_(ii_vert, jj_euler)] += \ Cga.dot(algebra.der_Peuler_by_v(tsstr.euler, zetag)) else: # Equation 25 # Kdisp[np.ix_(ii_vert, jj_quat)] += \ # algebra.der_Cquat_by_v(tsstr.quat, zetaa) Kdisp_vel[np.ix_(ii_vert, jj_quat)] += \ algebra.der_Cquat_by_v(tsstr.quat, zetaa) # Track body - project inputs as for A not moving if track_body: Kdisp_vel[np.ix_(ii_vert, jj_quat)] += \ Cga.dot(algebra.der_CquatT_by_v(tsstr.quat, zetag)) ### ------------------------------------ allocate Kvel_disp if bc_here != 1: # # wrt pos Kvel_disp[np.ix_(ii_vert, jj_tra)] += Der_vel_Ra # wrt psi (at zero psi_dot) Kvel_disp[np.ix_(ii_vert, jj_rot)] -= \ np.dot(Cga, np.dot(skew_for_rot, np.dot(Cab, XbskewTan))) # # wrt psi (psi_dot contributions - verified) Kvel_disp[np.ix_(ii_vert, jj_rot)] += np.dot( Cbg.T, np.dot(algebra.skew(np.dot(XbskewTan, psi_dot)), Tan)) if np.linalg.norm(psi) >= 1e-6: Kvel_disp[np.ix_(ii_vert, jj_rot)] -= \ np.dot(Cbg.T, np.dot(Xbskew, algebra.der_Tan_by_xv(psi, psi_dot))) # # w.r.t. position of FoR A (w.r.t. origin G) # # null as A and G have always same origin in SHARPy # # ### w.r.t. quaternion (attitude changes) - Eq 30 if use_euler: Kvel_vel[np.ix_(ii_vert, jj_euler)] += \ algebra.der_Ceuler_by_v(tsstr.euler, zetaa_dot) # Track body if ForA is rotating if track_body: Kvel_vel[np.ix_(ii_vert, jj_euler)] += \ Cga.dot(algebra.der_Peuler_by_v(tsstr.euler, zetag_dot)) else: Kvel_vel[np.ix_(ii_vert, jj_quat)] += \ algebra.der_Cquat_by_v(tsstr.quat, zetaa_dot) # Track body if ForA is rotating if track_body: Kvel_vel[np.ix_(ii_vert, jj_quat)] += \ Cga.dot(algebra.der_CquatT_by_v(tsstr.quat, zetag_dot)) ### ------------------------------------- allocate Kvel_vel if bc_here != 1: # wrt pos_dot Kvel_vel[np.ix_(ii_vert, jj_tra)] += Cga # # wrt crv_dot Kvel_vel[np.ix_(ii_vert, jj_rot)] -= np.dot(Cbg.T, XbskewTan) # # wrt velocity of FoR A Kvel_vel[np.ix_(ii_vert, jj_for_tra)] += Cga Kvel_vel[np.ix_(ii_vert, jj_for_rot)] -= \ np.dot(Cga, algebra.skew(zetaa)) # wrt rate of change of quaternion: not implemented! ### -------------------------------------- allocate Kforces if bc_here != 1: # nodal forces Kforces[np.ix_(jj_tra, ii_vert)] += Cag # nodal moments Kforces[np.ix_(jj_rot, ii_vert)] += \ np.dot(Tan.T, np.dot(Cbg, algebra.skew(Xg))) # or, equivalently, np.dot( algebra.skew(Xb),Cbg) # total forces Kforces[np.ix_(jj_for_tra, ii_vert)] += Cag # total moments Kforces[np.ix_(jj_for_rot, ii_vert)] += \ np.dot(Cag, algebra.skew(zetag)) # quaternion equation # null, as not dep. on external forces ### --------------------------------------- allocate Kstiff ### flexible dof equations (Kss and Csr) if bc_here != 1: # nodal forces if use_euler: if not track_body: Csr[jj_tra, -3:] -= algebra.der_Peuler_by_v( tsstr.euler, faero) # Csr[jj_tra, -3:] -= algebra.der_Ceuler_by_v(tsstr.euler, Cga.T.dot(faero)) else: if not track_body: Csr[jj_tra, -4:] -= algebra.der_CquatT_by_v( tsstr.quat, faero) # Track body # if track_body: # Csr[jj_tra, -4:] -= algebra.der_Cquat_by_v(tsstr.quat, Cga.T.dot(faero)) ### moments TanTXbskew = np.dot(Tan.T, Xbskew) # contrib. of TanT (dpsi) - Eq 37 - Integration of UVLM and GEBM Kss[np.ix_(jj_rot, jj_rot)] -= algebra.der_TanT_by_xv( psi, maero_b) # contrib of delta aero moment (dpsi) - Eq 36 Kss[np.ix_(jj_rot, jj_rot)] -= \ np.dot(TanTXbskew, algebra.der_CcrvT_by_v(psi, np.dot(Cag, faero))) # contribution of delta aero moment (dquat) if use_euler: if not track_body: Csr[jj_rot, -3:] -= \ np.dot(TanTXbskew, np.dot(Cba, algebra.der_Peuler_by_v(tsstr.euler, faero))) # if track_body: # Csr[jj_rot, -3:] -= \ # np.dot(TanTXbskew, # np.dot(Cbg, # algebra.der_Peuler_by_v(tsstr.euler, Cga.T.dot(faero)))) else: if not track_body: Csr[jj_rot, -4:] -= \ np.dot(TanTXbskew, np.dot(Cba, algebra.der_CquatT_by_v(tsstr.quat, faero))) # Track body # if track_body: # Csr[jj_rot, -4:] -= \ # np.dot(TanTXbskew, # np.dot(Cbg, # algebra.der_CquatT_by_v(tsstr.quat, Cga.T.dot(faero)))) ### rigid body eqs (Crs and Crr) if bc_here != 1: # Changed Crs to Krs - NG 14/5/19 # moments contribution due to delta_Ra (+ sign intentional) Krs[3:6, jj_tra] += algebra.skew(faero_a) # moment contribution due to delta_psi (+ sign intentional) Krs[3:6, jj_rot] += np.dot(algebra.skew(faero_a), algebra.der_Ccrv_by_v(psi, Xb)) if use_euler: if not track_body: # total force Crr[:3, -3:] -= algebra.der_Peuler_by_v( tsstr.euler, faero) # total moment contribution due to change in euler angles Crr[3:6, -3:] -= algebra.der_Peuler_by_v( tsstr.euler, np.cross(zetag, faero)) Crr[3:6, -3:] += np.dot( np.dot(Cag, algebra.skew(faero)), algebra.der_Peuler_by_v( tsstr.euler, np.dot(Cab, Xb))) else: if not track_body: # total force Crr[:3, -4:] -= algebra.der_CquatT_by_v( tsstr.quat, faero) # total moment contribution due to quaternion Crr[3:6, -4:] -= algebra.der_CquatT_by_v( tsstr.quat, np.cross(zetag, faero)) Crr[3:6, -4:] += np.dot( np.dot(Cag, algebra.skew(faero)), algebra.der_CquatT_by_v( tsstr.quat, np.dot(Cab, Xb))) # # Track body # if track_body: # # NG 20/8/19 - is the Cag needed here? Verify # Crr[:3, -4:] -= Cag.dot(algebra.der_Cquat_by_v(tsstr.quat, Cga.T.dot(faero))) # # Crr[3:6, -4:] -= Cag.dot(algebra.skew(zetag).dot(algebra.der_Cquat_by_v(tsstr.quat, Cga.T.dot(faero)))) # Crr[3:6, -4:] += Cag.dot(algebra.skew(faero)).dot(algebra.der_Cquat_by_v(tsstr.quat, Cga.T.dot(zetag))) # transfer self.Kdisp = Kdisp self.Kvel_disp = Kvel_disp self.Kdisp_vel = Kdisp_vel self.Kvel_vel = Kvel_vel self.Kforces = Kforces # stiffening factors self.Kss = Kss self.Krs = Krs self.Csr = Csr self.Crs = Crs self.Crr = Crr
def unpack_ss_vector(self, x_n, u_n, struct_tstep): """ Warnings: Under development. Missing: * Accelerations * Double check the cartesian rotation vector * Tangential operator for the moments Takes the state :math:`x = [\eta, \dot{\eta}]` and input vectors :math:`u = N` of a linearised beam and returns a SHARPy timestep instance, including the reference values. Args: x_n (np.ndarray): Structural beam state vector in nodal space y_n (np.ndarray): Beam input vector (nodal forces) struct_tstep (utils.datastructures.StructTimeStepInfo): Reference timestep used for linearisation Returns: utils.datastructures.StructTimeStepInfo: new timestep with linearised values added to the reference value """ # check if clamped vdof = self.sys.structure.vdof num_node = struct_tstep.num_node num_dof = 6 * sum(vdof >= 0) if self.sys.clamped: clamped = True rig_dof = 0 else: clamped = False if self.settings['use_euler']: rig_dof = 9 else: rig_dof = 10 q = np.zeros_like(struct_tstep.q) q = np.zeros((num_dof + rig_dof)) dqdt = np.zeros_like(q) dqddt = np.zeros_like(q) pos = np.zeros_like(struct_tstep.pos) pos_dot = np.zeros_like(struct_tstep.pos_dot) psi = np.zeros_like(struct_tstep.psi) psi_dot = np.zeros_like(struct_tstep.psi_dot) for_pos = np.zeros_like(struct_tstep.for_pos) for_vel = np.zeros_like(struct_tstep.for_vel) for_acc = np.zeros_like(struct_tstep.for_acc) quat = np.zeros_like(struct_tstep.quat) gravity_forces = np.zeros_like(struct_tstep.gravity_forces) total_gravity_forces = np.zeros_like(struct_tstep.total_gravity_forces) steady_applied_forces = np.zeros_like( struct_tstep.steady_applied_forces) unsteady_applied_forces = np.zeros_like( struct_tstep.unsteady_applied_forces) q[:num_dof + rig_dof] = x_n[:num_dof + rig_dof] dqdt[:num_dof + rig_dof] = x_n[num_dof + rig_dof:] # Missing the forces # dqddt = self.sys.Minv.dot(-self.sys.Cstr.dot(dqdt) - self.sys.Kstr.dot(q)) for i_node in vdof[vdof >= 0]: pos[i_node + 1, :] = q[6 * i_node:6 * i_node + 3] pos_dot[i_node + 1, :] = dqdt[6 * i_node + 0:6 * i_node + 3] # TODO: CRV of clamped node and double check that the CRV takes this form for i_elem in range(struct_tstep.num_elem): for i_node in range(struct_tstep.num_node_elem): psi[i_elem, i_node, :] = np.linalg.inv( algebra.crv2tan(struct_tstep.psi[i_elem, i_node]).T).dot( q[i_node + 3:i_node + 6]) psi_dot[i_elem, i_node, :] = dqdt[i_node + 3:i_node + 6] if not clamped: for_vel = dqdt[-rig_dof:-rig_dof + 6] if self.settings['use_euler']: euler = dqdt[-4:-1] quat = algebra.euler2quat(euler) else: quat = dqdt[-4:] for_pos = q[-rig_dof:-rig_dof + 6] for_acc = dqddt[-rig_dof:-rig_dof + 6] if u_n is not None: for i_node in vdof[vdof >= 0]: steady_applied_forces[i_node + 1] = u_n[6 * i_node:6 * i_node + 6] steady_applied_forces[0] = u_n[-10:-4] - np.sum( steady_applied_forces[1:, :], 0) # gravity forces - careful - debug C_grav = np.zeros((q.shape[0], q.shape[0])) K_grav = np.zeros_like(C_grav) try: Crr = self.sys.Crr_grav Csr = self.sys.Csr_grav C_grav[:-rig_dof, -rig_dof:] = Csr # TODO: sort out changing q vector with euler C_grav[-rig_dof:, -rig_dof:] = Crr K_grav[-rig_dof:, :-rig_dof] = self.sys.Krs_grav K_grav[:-rig_dof, :-rig_dof] = self.sys.Kss_grav fgrav = -C_grav.dot(dqdt) - K_grav.dot(q) for i in range(gravity_forces.shape[0] - 1): #add bc at node - doing it manually here gravity_forces[i + 1, :] = fgrav[6 * i:6 * (i + 1)] gravity_forces[0, :] = fgrav[-rig_dof:-rig_dof + 6] - np.sum( gravity_forces[1:], 0) except AttributeError: pass current_time_step = struct_tstep.copy() current_time_step.q[:len(q)] = q + struct_tstep.q[:len(q)] current_time_step.dqdt[:len(q)] = dqdt + struct_tstep.dqdt[:len(q)] current_time_step.dqddt[:len(q)] = dqddt + struct_tstep.dqddt[:len(q)] current_time_step.pos = pos + struct_tstep.pos current_time_step.pos_dot = pos + struct_tstep.pos_dot current_time_step.psi = psi + struct_tstep.psi current_time_step.psi_dot = psi_dot + struct_tstep.psi_dot current_time_step.for_vel = for_vel + struct_tstep.for_vel current_time_step.for_acc = for_acc + struct_tstep.for_acc current_time_step.for_pos = for_pos + struct_tstep.for_pos current_time_step.gravity_forces = gravity_forces + struct_tstep.gravity_forces current_time_step.total_gravity_forces = total_gravity_forces + struct_tstep.total_gravity_forces current_time_step.unsteady_applied_forces = unsteady_applied_forces + struct_tstep.unsteady_applied_forces new_quat = quat + struct_tstep.quat current_time_step.quat = new_quat / np.linalg.norm(new_quat) current_time_step.steady_applied_forces = steady_applied_forces + struct_tstep.steady_applied_forces return current_time_step
def free_free_modes(self, phi, M): r""" Returns the rigid body modes defined with respect to the centre of gravity The transformation from the modes defined at the FoR A origin, :math:`\boldsymbol{\Phi}`, to the modes defined using the centre of gravity as a reference is .. math:: \boldsymbol{\Phi}_{rr,CG}|_{TRA} = \boldsymbol{\Phi}_{RR}|_{TRA} + \tilde{\mathbf{r}}_{CG} \boldsymbol{\Phi}_{RR}|_{ROT} .. math:: \boldsymbol{\Phi}_{rr,CG}|_{ROT} = \boldsymbol{\Phi}_{RR}|_{ROT} Returns: (np.array): Transformed eigenvectors """ # NG - 26/7/19 This is the transformation being performed by K_vec # Leaving this here for now in case it becomes necessary # .. math:: \boldsymbol{\Phi}_{ss,CG}|_{TRA} = \boldsymbol{\Phi}_{SS}|_{TRA} +\boldsymbol{\Phi}_{RS}|_{TRA} - # \tilde{\mathbf{r}}_{A}\boldsymbol{\Phi}_{RS}|_{ROT} # # .. math:: \boldsymbol{\Phi}_{ss,CG}|_{ROT} = \boldsymbol{\Phi}_{SS}|_{ROT} # + (\mathbf{T}(\boldsymbol{\Psi})^\top)^{-1}\boldsymbol{\Phi}_{RS}|_{ROT} if not self.rigid_body_motion: warnings.warn('No rigid body modes to transform because the structure is clamped') return phi else: pos = self.data.structure.timestep_info[self.data.ts].pos r_cg = modalutils.cg(M) jj = 0 K_vec = np.zeros((phi.shape[0], phi.shape[0])) jj_for_vel = range(self.data.structure.num_dof.value, self.data.structure.num_dof.value + 3) jj_for_rot = range(self.data.structure.num_dof.value + 3, self.data.structure.num_dof.value + 6) for node_glob in range(self.data.structure.num_node): ### detect bc at node (and no. of dofs) bc_here = self.data.structure.boundary_conditions[node_glob] if bc_here == 1: # clamp (only rigid-body) dofs_here = 0 jj_tra, jj_rot = [], [] continue elif bc_here == -1 or bc_here == 0: # (rigid+flex body) dofs_here = 6 jj_tra = 6 * self.data.structure.vdof[node_glob] + np.array([0, 1, 2], dtype=int) jj_rot = 6 * self.data.structure.vdof[node_glob] + np.array([3, 4, 5], dtype=int) # jj_tra=[jj ,jj+1,jj+2] # jj_rot=[jj+3,jj+4,jj+5] else: raise NameError('Invalid boundary condition (%d) at node %d!' \ % (bc_here, node_glob)) jj += dofs_here ee, node_loc = self.data.structure.node_master_elem[node_glob, :] psi = self.data.structure.timestep_info[self.data.ts].psi[ee, node_loc, :] Ra = pos[node_glob, :] # in A FoR with respect to G K_vec[np.ix_(jj_tra, jj_tra)] += np.eye(3) K_vec[np.ix_(jj_tra, jj_for_vel)] += np.eye(3) K_vec[np.ix_(jj_tra, jj_for_rot)] -= algebra.skew(Ra) K_vec[np.ix_(jj_rot, jj_rot)] += np.eye(3) K_vec[np.ix_(jj_rot, jj_for_rot)] += np.linalg.inv(algebra.crv2tan(psi).T) # Rigid-Rigid modes transform Krr = np.eye(10) Krr[np.ix_([0, 1, 2], [3, 4, 5])] += algebra.skew(r_cg) # Assemble transformed modes phirr = Krr.dot(phi[-10:, :10]) phiss = K_vec.dot(phi[:, 10:]) # Get rigid body modes to be positive in translation and rotation for i in range(10): ind = np.argmax(np.abs(phirr[:, i])) phirr[:, i] = np.sign(phirr[ind, i]) * phirr[:, i] # NG - 26/7/19 - Transformation of the rigid part of the elastic modes ended up not being necessary but leaving # here in case it becomes useful in the future phit = np.block([np.zeros((phi.shape[0], 10)), phi[:, 10:]]) phit[-10:, :10] = phirr return phit
### ----- linearisation Sol = lin_aeroelastic.LinAeroEla(data0) Sol.linuvlm.assemble_ss() Sol.get_gebm2uvlm_gains() # gains, str -> aero Zblock = np.zeros((3 * Sol.linuvlm.Kzeta, Sol.num_dof_str)) Kas = np.block([[Sol.Kdisp, Sol.Kdisp_vel], [Sol.Kvel_disp, Sol.Kvel_vel], [Zblock, Zblock]]) Zblock = None # gains, aero -> str Ksa = Sol.Kforces # ----------------------- project forces at nodes and total in rolled FoR R T0 = algebra.crv2tan(tsstr0.psi[0][0]) T0Tinv = np.linalg.inv(T0.T) for nn in range(Sol.lingebm_str.num_dof // 6): iitra = [6 * nn + ii for ii in range(3)] Ksa[iitra, :] = np.dot(Cra, Ksa[iitra, :]) iirot = [6 * nn + ii for ii in range(3, 6)] Ksa[iirot, :] = np.dot(Crb, np.dot(T0Tinv, Ksa[iirot, :])) iitra = [Sol.lingebm_str.num_dof + ii for ii in range(3)] Ksa[iitra, :] = np.dot(Cra, Ksa[iitra, :]) iirot = [Sol.lingebm_str.num_dof + ii for ii in range(3, 6)] Ksa[iirot, :] = np.dot(Cra, Ksa[iirot, :]) # -------------------------------------- plunge/pitch through flex body dof Kpl_flex = np.zeros((2 * Sol.num_dof_str, 2))
def change_to_global_AFoR(self, global_ibody): """ change_to_global_AFoR Reference a StructTimeStepInfo to the global A frame of reference Given 'self' as a StructTimeStepInfo class, this function references it to the global A frame of reference Args: self(StructTimeStepInfo): timestep information global_ibody(int): body number (as defined in the mutibody system) to be modified Returns: Examples: Notes: """ # Define the rotation matrices between the different FoR CAslaveG = algebra.quat2rotation(self.mb_quat[global_ibody, :]).T CGAmaster = algebra.quat2rotation(self.mb_quat[0, :]) Csm = np.dot(CAslaveG, CGAmaster) delta_pos_ms = self.mb_FoR_pos[global_ibody, :] - self.mb_FoR_pos[0, :] delta_vel_ms = self.mb_FoR_vel[global_ibody, :] - self.mb_FoR_vel[0, :] for inode in range(self.pos.shape[0]): pos_previous = self.pos[inode, :] + np.zeros((3, ), ) self.pos[inode, :] = ( np.dot(np.transpose(Csm), self.pos[inode, :]) + np.dot(np.transpose(CGAmaster), delta_pos_ms[0:3])) self.pos_dot[inode, :] = ( np.dot(np.transpose(Csm), self.pos_dot[inode, :]) + np.dot(np.transpose(CGAmaster), delta_vel_ms[0:3]) + np.dot( Csm.T, np.dot( algebra.skew( np.dot(CAslaveG, self.mb_FoR_vel[global_ibody, 3:6])), pos_previous)) - np.dot( algebra.skew(np.dot(CGAmaster.T, self.mb_FoR_vel[0, 3:6])), self.pos[inode, :])) self.gravity_forces[inode, 0:3] = np.dot(Csm.T, self.gravity_forces[inode, 0:3]) self.gravity_forces[inode, 3:6] = np.dot(Csm.T, self.gravity_forces[inode, 3:6]) # np.cross(np.dot(CGAmaster.T, delta_vel_ms[3:6]), pos_previous)) for ielem in range(self.psi.shape[0]): for inode in range(3): psi_previous = self.psi[ielem, inode, :] + np.zeros((3, ), ) self.psi[ielem, inode, :] = algebra.rotation2crv( np.dot(Csm.T, algebra.crv2rotation(self.psi[ielem, inode, :]))) self.psi_dot[ielem, inode, :] = np.dot( algebra.crv2tan(self.psi[ielem, inode, :]), (np.dot( Csm.T, np.dot( algebra.crv2tan(psi_previous).T, self.psi_dot[ielem, inode, :])) + np.dot( algebra.quat2rotation(self.mb_quat[0, :]).T, delta_vel_ms[3:6]))) # Set the output FoR variables self.for_pos = self.mb_FoR_pos[0, :].astype(dtype=ct.c_double, order='F', copy=True) self.for_vel[0:3] = np.dot(np.transpose(CGAmaster), self.mb_FoR_vel[0, 0:3]) self.for_vel[3:6] = np.dot(np.transpose(CGAmaster), self.mb_FoR_vel[0, 3:6]) self.for_acc[0:3] = np.dot(np.transpose(CGAmaster), self.mb_FoR_acc[0, 0:3]) self.for_acc[3:6] = np.dot(np.transpose(CGAmaster), self.mb_FoR_acc[0, 3:6]) self.quat = self.mb_quat[0, :].astype(dtype=ct.c_double, order='F', copy=True)
def get_gebm2uvlm_gains(self): """ Gain matrix to transfer GEBM dofs to UVLM lattice vertices and stiffening term due to non-zero forces at the linearisation point. The function produces the matrices: - ``Kdisp``: from GEBM to UVLM grid displacements - ``Kvel_disp``: influence of GEBM dofs displacements to UVLM grid velocities. - ``Kvel_vel``: influence of GEBM dofs displacements to UVLM grid displacements. - ``Kforces`` (UVLM->GEBM) dimensions are the transpose than the Kdisp and Kvel* matrices. Hence, when allocation this term, ``ii`` and ``jj`` indices will unintuitively refer to columns and rows, respectively. - ``Kss``: stiffness factor accounting for non-zero forces at the linearisation point. (flexible dof -> flexible dof) - ``Ksr``: stiffness factor accounting for non-zero forces at the linearisation point. (rigid dof -> flexible dof) Notes: - The following terms have been verified against SHARPy (to ensure same sign conventions and accuracy): - :math:`\\mathbf{C}^{AB}` - accuracy of :math:`X^B=\\mathbf{C}^{AB}*X^A` - accuracy of :math:`X^G` and :math:`X^A` """ data = self.data aero = self.data.aero structure = self.data.structure # data.aero.beam tsaero = self.tsaero tsstr = self.tsstr # allocate output Kdisp = np.zeros((3 * self.linuvlm.Kzeta, self.num_dof_str)) Kvel_disp = np.zeros((3 * self.linuvlm.Kzeta, self.num_dof_str)) Kvel_vel = np.zeros((3 * self.linuvlm.Kzeta, self.num_dof_str)) Kforces = np.zeros((self.num_dof_str, 3 * self.linuvlm.Kzeta)) Kss = np.zeros((self.num_dof_flex, self.num_dof_flex)) Ksr = np.zeros((self.num_dof_flex, self.num_dof_rig)) # get projection matrix A->G # (and other quantities indep. from nodal position) Cga = algebra.quat2rotation(tsstr.quat) Cag = Cga.T # for_pos=tsstr.for_pos for_tra = tsstr.for_vel[:3] for_rot = tsstr.for_vel[3:] skew_for_rot = algebra.skew(for_rot) Der_vel_Ra = np.dot(Cga, skew_for_rot) # GEBM degrees of freedom jj_for_tra = range(self.num_dof_str - 10, self.num_dof_str - 7) jj_for_rot = range(self.num_dof_str - 7, self.num_dof_str - 4) jj_quat = range(self.num_dof_str - 4, self.num_dof_str) jj = 0 # nodal dof index for node_glob in range(structure.num_node): ### detect bc at node (and no. of dofs) bc_here = structure.boundary_conditions[node_glob] if bc_here == 1: # clamp (only rigid-body) dofs_here = 0 jj_tra, jj_rot = [], [] # continue elif bc_here == -1 or bc_here == 0: # (rigid+flex body) dofs_here = 6 jj_tra = 6 * structure.vdof[node_glob] + np.array([0, 1, 2], dtype=int) jj_rot = 6 * structure.vdof[node_glob] + np.array([3, 4, 5], dtype=int) # jj_tra=[jj ,jj+1,jj+2] # jj_rot=[jj+3,jj+4,jj+5] else: raise NameError('Invalid boundary condition (%d) at node %d!' \ % (bc_here, node_glob)) jj += dofs_here # retrieve element and local index ee, node_loc = structure.node_master_elem[node_glob, :] # get position, crv and rotation matrix Ra = tsstr.pos[node_glob, :] # in A FoR, w.r.t. origin A-G Rg = np.dot(Cag.T, Ra) # in G FoR, w.r.t. origin A-G psi = tsstr.psi[ee, node_loc, :] psi_dot = tsstr.psi_dot[ee, node_loc, :] Cab = algebra.crv2rotation(psi) Cba = Cab.T Cbg = np.dot(Cab.T, Cag) ### str -> aero mapping # some nodes may be linked to multiple surfaces... for str2aero_here in aero.struct2aero_mapping[node_glob]: # detect surface/span-wise coordinate (ss,nn) nn, ss = str2aero_here['i_n'], str2aero_here['i_surf'] # print('%.2d,%.2d'%(nn,ss)) # surface panelling M = aero.aero_dimensions[ss][0] N = aero.aero_dimensions[ss][1] Kzeta_start = 3 * sum(self.linuvlm.MS.KKzeta[:ss]) shape_zeta = (3, M + 1, N + 1) for mm in range(M + 1): # get bound vertex index ii_vert = [ Kzeta_start + np.ravel_multi_index( (cc, mm, nn), shape_zeta) for cc in range(3) ] # get aero force faero = tsaero.forces[ss][:3, mm, nn] # get position vectors zetag = tsaero.zeta[ss][:, mm, nn] # in G FoR, w.r.t. origin A-G zetaa = np.dot(Cag, zetag) # in A FoR, w.r.t. origin A-G Xg = zetag - Rg # in G FoR, w.r.t. origin B Xb = np.dot(Cbg, Xg) # in B FoR, w.r.t. origin B # get rotation terms Xbskew = algebra.skew(Xb) Tan = algebra.crv2tan(psi) XbskewTan = np.dot(Xbskew, Tan) # get velocity terms zetag_dot = tsaero.zeta_dot[ ss][:, mm, nn] # in G FoR, w.r.t. origin A-G zetaa_dot = np.dot( Cag, zetag_dot) # in A FoR, w.r.t. origin A-G ### ---------------------------------------- allocate Kdisp if bc_here != 1: # wrt pos Kdisp[np.ix_(ii_vert, jj_tra)] += Cga # wrt psi Kdisp[np.ix_(ii_vert, jj_rot)] -= np.dot(Cbg.T, XbskewTan) # w.r.t. position of FoR A (w.r.t. origin G) # null as A and G have always same origin in SHARPy # # ### w.r.t. quaternion (attitude changes) Kdisp[np.ix_(ii_vert, jj_quat)] = \ algebra.der_Cquat_by_v(tsstr.quat, zetaa) ### ------------------------------------ allocate Kvel_disp if bc_here != 1: # # wrt pos Kvel_disp[np.ix_(ii_vert, jj_tra)] += Der_vel_Ra # wrt psi (at zero psi_dot) Kvel_disp[np.ix_(ii_vert, jj_rot)] -= \ np.dot(Cga, np.dot(skew_for_rot, np.dot(Cab, XbskewTan))) # # wrt psi (psi_dot contributions - verified) Kvel_disp[np.ix_(ii_vert, jj_rot)] += np.dot( Cbg.T, np.dot(algebra.skew(np.dot(XbskewTan, psi_dot)), Tan)) Kvel_disp[np.ix_(ii_vert, jj_rot)] -= \ np.dot(Cbg.T, np.dot(Xbskew, algebra.der_Tan_by_xv(psi, psi_dot))) # # w.r.t. position of FoR A (w.r.t. origin G) # # null as A and G have always same origin in SHARPy # # ### w.r.t. quaternion (attitude changes) Kvel_disp[np.ix_(ii_vert, jj_quat)] = \ algebra.der_Cquat_by_v(tsstr.quat, zetaa_dot) ### ------------------------------------- allocate Kvel_vel if bc_here != 1: # wrt pos_dot Kvel_vel[np.ix_(ii_vert, jj_tra)] += Cga # # wrt crv_dot Kvel_vel[np.ix_(ii_vert, jj_rot)] -= np.dot(Cbg.T, XbskewTan) # # wrt velocity of FoR A Kvel_vel[np.ix_(ii_vert, jj_for_tra)] += Cga Kvel_vel[np.ix_(ii_vert, jj_for_rot)] -= \ np.dot(Cga, algebra.skew(zetaa)) # wrt rate of change of quaternion: not implemented! ### -------------------------------------- allocate Kforces if bc_here != 1: # nodal forces Kforces[np.ix_(jj_tra, ii_vert)] += Cbg # nodal moments Kforces[np.ix_(jj_rot, ii_vert)] += \ np.dot(Cbg, algebra.skew(Xg)) # or, equivalently, np.dot( algebra.skew(Xb),Cbg) # total forces Kforces[np.ix_(jj_for_tra, ii_vert)] += Cag # total moments Kforces[np.ix_(jj_for_rot, ii_vert)] += \ np.dot(Cag, algebra.skew(zetag)) # quaternion equation # null, as not dep. on external forces ### --------------------------------------- allocate Kstiff if bc_here != 1: # forces Dfdcrv = algebra.der_CcrvT_by_v( psi, np.dot(Cag, faero)) Dfdquat = np.dot( Cba, algebra.der_CquatT_by_v(tsstr.quat, faero)) Kss[np.ix_(jj_tra, jj_rot)] -= Dfdcrv Ksr[jj_tra, -4:] -= Dfdquat # moments Kss[np.ix_(jj_rot, jj_rot)] -= np.dot(Xbskew, Dfdcrv) Ksr[jj_rot, -4:] -= np.dot(Xbskew, Dfdquat) # embed() # transfer self.Kdisp = Kdisp self.Kvel_disp = Kvel_disp self.Kvel_vel = Kvel_vel self.Kforces = Kforces # stiffening factors self.Kss = Kss self.Ksr = Ksr