From a2355e55ed20e32e08effab64c31577c30e04b1e Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Fri, 19 Dec 2025 10:58:55 -0700 Subject: [PATCH 01/24] Initial construction of K,M for modal damping init. --- modules/beamdyn/src/BeamDyn.f90 | 42 ++++++++++++++++++++++++++++++--- 1 file changed, 39 insertions(+), 3 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 6e245c482..33ddb2b20 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -188,7 +188,43 @@ SUBROUTINE BD_Init( InitInp, u, p, x, xd, z, OtherState, y, MiscVar, Interval, I QuasiStaticInitialized = .FALSE. ENDIF + !................................. + ! Calculation of modal damping here + + IF(p%damp_flag .EQ. 2) THEN + + PRINT *, 'In modal damping branch.' + + ! 0. Setup quadrature points + CALL BD_QuadraturePointData(p, x, MiscVar) + + ! 1. Generates K, M Matrices + ! These go into 'MiscVar%StifK' and 'MiscVar%MassM' + CALL BD_GenerateDynamicElementGA2( x, OtherState, p, MiscVar, .TRUE.) + ! Do I need to call error handling after this? + + ! 2. Copy lines from 'BD_CalcForceAcc' for M, K -> 2D and apply Boundary conditions + ! In that function, this produces LP_MassM_LU + + ! Full mass matrix (n_dof, n_dof) + MiscVar%LP_MassM = reshape(MiscVar%MassM, [p%dof_total, p%dof_total]) + + ! Mass matrix for free nodes + MiscVar%LP_MassM_LU = MiscVar%LP_MassM(7:p%dof_total, 7:p%dof_total) + ! Full stiffness matrix (n_dof, n_dof) + MiscVar%LP_StifK = reshape(MiscVar%StifK, [p%dof_total, p%dof_total]) + + ! Stiffness matrix for free nodes + MiscVar%LP_StifK_LU = MiscVar%LP_StifK(7:p%dof_total, 7:p%dof_total) + + ! 3. Do eigenanalysis + + ! 4. Save eigenvectors/eigenvalues or damping matrix in original frame + + print *, 'End of Modal damping.' + + ENDIF !................................. ! initialization of output mesh values (used for initial guess to AeroDyn) @@ -3216,7 +3252,7 @@ SUBROUTINE BD_ElementMatrixAcc( nelem, p, OtherState, m ) CALL BD_ElasticForce( nelem, p, m, .FALSE. ) ! Calculate Fc, Fd only - IF(p%damp_flag .NE. 0) THEN + IF(p%damp_flag .EQ. 1) THEN CALL BD_DissipativeForce( nelem, p, m, .FALSE. ) ! Calculate dissipative terms on Fc, Fd ENDIF CALL BD_GravityForce( nelem, p, m, MATMUL(p%gravity,OtherState%GlbRot) ) ! Calculate Fg @@ -4955,7 +4991,7 @@ SUBROUTINE BD_ElementMatrixGA2( fact, nelem, p, OtherState, m ) CALL BD_ElasticForce( nelem,p,m,fact ) ! Calculate Fc, Fd [and if(fact): Oe, Pe, and Qe for N-R algorithm] using m%qp%E1, m%qp%RR0, m%qp%kappa, m%qp%Stif CALL BD_InertialForce( nelem,p,m,fact ) ! Calculate Fi [and Mi,Gi,Ki IF(fact)] - IF(p%damp_flag .NE. 0) THEN + IF(p%damp_flag .EQ. 1) THEN CALL BD_DissipativeForce( nelem,p,m,fact ) ! Calculate dissipative terms on Fc, Fd [and Sd, Od, Pd and Qd, betaC, Gd, Xd, Yd for N-R algorithm] ENDIF @@ -5009,7 +5045,7 @@ SUBROUTINE BD_ElementMatrixGA2( fact, nelem, p, OtherState, m ) END DO ! Dissipative terms - IF (p%damp_flag .NE. 0) THEN + IF (p%damp_flag .EQ. 1) THEN DO j=1,p%nodes_per_elem DO idx_dof2=1,p%dof_node DO i=1,p%nodes_per_elem From b1150f910be71324e377c49ce995fd7085494616 Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Mon, 22 Dec 2025 11:00:51 -0700 Subject: [PATCH 02/24] Eigenanalysis moved to NWTC_Num for reuse in beamdyn modal damping. BD calculation matches openfast toolbox and analytical frequencies for cantilever. --- modules/beamdyn/src/BeamDyn.f90 | 170 +++++++++++++++++---- modules/beamdyn/src/BeamDyn_Types.f90 | 36 +++++ modules/beamdyn/src/Registry_BeamDyn.txt | 3 + modules/nwtc-library/src/NWTC_Num.f90 | 180 +++++++++++++++++++++++ modules/subdyn/src/FEM.f90 | 160 +------------------- 5 files changed, 360 insertions(+), 189 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 33ddb2b20..bb9890122 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -24,6 +24,8 @@ MODULE BeamDyn IMPLICIT NONE + INTEGER, PARAMETER :: FEKi = R8Ki ! Define the kind to be used for FEM/eigenanalysis + #ifndef UNIT_TEST PRIVATE @@ -192,38 +194,7 @@ SUBROUTINE BD_Init( InitInp, u, p, x, xd, z, OtherState, y, MiscVar, Interval, I ! Calculation of modal damping here IF(p%damp_flag .EQ. 2) THEN - - PRINT *, 'In modal damping branch.' - - ! 0. Setup quadrature points - CALL BD_QuadraturePointData(p, x, MiscVar) - - ! 1. Generates K, M Matrices - ! These go into 'MiscVar%StifK' and 'MiscVar%MassM' - CALL BD_GenerateDynamicElementGA2( x, OtherState, p, MiscVar, .TRUE.) - ! Do I need to call error handling after this? - - ! 2. Copy lines from 'BD_CalcForceAcc' for M, K -> 2D and apply Boundary conditions - ! In that function, this produces LP_MassM_LU - - ! Full mass matrix (n_dof, n_dof) - MiscVar%LP_MassM = reshape(MiscVar%MassM, [p%dof_total, p%dof_total]) - - ! Mass matrix for free nodes - MiscVar%LP_MassM_LU = MiscVar%LP_MassM(7:p%dof_total, 7:p%dof_total) - - ! Full stiffness matrix (n_dof, n_dof) - MiscVar%LP_StifK = reshape(MiscVar%StifK, [p%dof_total, p%dof_total]) - - ! Stiffness matrix for free nodes - MiscVar%LP_StifK_LU = MiscVar%LP_StifK(7:p%dof_total, 7:p%dof_total) - - ! 3. Do eigenanalysis - - ! 4. Save eigenvectors/eigenvalues or damping matrix in original frame - - print *, 'End of Modal damping.' - + call Init_ModalDamping(p, x, MiscVar, OtherState) ENDIF !................................. @@ -1823,6 +1794,141 @@ END SUBROUTINE cleanup END SUBROUTINE Init_ContinuousStates +!----------------------------------------------------------------------------------------------------------------------------------- +!> This routine initializes modal damping. +SUBROUTINE Init_ModalDamping(p, x, m, OtherState) + + ! TODO : Look at what should be INOUT v. IN + + TYPE(BD_ParameterType), INTENT(INOUT) :: p !< Parameters, output eigenpairs here + TYPE(BD_ContinuousStateType), INTENT(INOUT) :: x !< Continuous states at t on input at t + dt on output + TYPE(BD_MiscVarType), INTENT(INOUT) :: m !< misc/optimization variables + type(BD_OtherStateType), INTENT(INOUT) :: OtherState !< Global rotations are stored in otherstate + + INTEGER(IntKi) :: nDOF + INTEGER(IntKi) :: ErrStat + CHARACTER(ErrMsgLen) :: ErrMsg + + ErrStat = ErrID_None + ErrMsg = '' + + PRINT *, 'In modal damping branch.' + + ! 0. Setup quadrature points + CALL BD_QuadraturePointData(p, x, m) + + ! 1. Generates K, M Matrices + ! These go into 'm%StifK' and 'm%MassM' + CALL BD_GenerateDynamicElementGA2( x, OtherState, p, m, .TRUE.) + ! Do I need to call error handling after this? + + ! 2. Copy lines from 'BD_CalcForceAcc' for M, K -> 2D and apply Boundary conditions + ! In that function, this produces LP_MassM_LU + + ! Full mass matrix (n_dof, n_dof) + m%LP_MassM = reshape(m%MassM, [p%dof_total, p%dof_total]) + + ! Mass matrix for free nodes + m%LP_MassM_LU = m%LP_MassM(7:p%dof_total, 7:p%dof_total) + + ! Full stiffness matrix (n_dof, n_dof) + m%LP_StifK = reshape(m%StifK, [p%dof_total, p%dof_total]) + + ! Stiffness matrix for free nodes + m%LP_StifK_LU = m%LP_StifK(7:p%dof_total, 7:p%dof_total) + + ! 3. Do eigenanalysis + ! for now, calculate all eigenpairs + nDOF = p%dof_total - 6 + + ! TODO : Since these are on the parameters is deallocating automatically handled? + CALL AllocAry(p%eigenvectors, nDOF, nDOF, 'p%eigenvectors', ErrStat, ErrMsg) + CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') + + CALL AllocAry(p%omega, nDOF, 'p%omega', ErrStat, ErrMsg) + CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') + + CALL EigenSolveWrap(m%LP_StifK_LU, m%LP_MassM_LU, nDOF, nDOF, .TRUE., p%eigenvectors, p%omega, ErrStat, ErrMsg ) + + ! TODO : Mode shapes need to be mass normalized. + + ! 4. Save eigenvectors/eigenvalues or damping matrix in original frame + + print *, 'End of Modal damping.' + +END SUBROUTINE + +!> Wrapper function for eigen value analyses +SUBROUTINE EigenSolveWrap(K, M, nDOF, NOmega, bCheckSingularity, EigVect, Omega, ErrStat, ErrMsg ) + USE NWTC_Num, only: EigenSolve + + INTEGER, INTENT(IN ) :: nDOF ! Total degrees of freedom of the incoming system + REAL(FEKi), INTENT(IN ) :: K(nDOF, nDOF) ! stiffness matrix + REAL(FEKi), INTENT(IN ) :: M(nDOF, nDOF) ! mass matrix + INTEGER, INTENT(IN ) :: NOmega ! No. of requested eigenvalues + LOGICAL, INTENT(IN ) :: bCheckSingularity ! If True, the solver will fail if rigid modes are present + REAL(FEKi), INTENT( OUT) :: EigVect(nDOF, NOmega) ! Returned Eigenvectors + REAL(FEKi), INTENT( OUT) :: Omega(NOmega) ! Returned Eigenvalues + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + ! LOCALS + REAL(LaKi), ALLOCATABLE :: K_LaKi(:,:), M_LaKi(:,:) + REAL(LaKi), ALLOCATABLE :: EigVect_LaKi(:,:), Omega_LaKi(:) + INTEGER(IntKi) :: N + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + ErrStat = ErrID_None + ErrMsg = '' + EigVect=0.0_FeKi + Omega=0.0_FeKi + + ! --- Unfortunate conversion to FEKi... TODO TODO consider storing M and K in FEKi + N=size(K,1) + CALL AllocAry(K_LaKi , N, N, 'K_FEKi', ErrStat2, ErrMsg2); if(Failed()) return + CALL AllocAry(M_LaKi , N, N, 'M_FEKi', ErrStat2, ErrMsg2); if(Failed()) return + K_LaKi = real( K, LaKi ) + M_LaKi = real( M, LaKi ) + N=size(K_LaKi,1) + + ! Note: NOmega must be <= N, which is the length of Omega2, Phi! + if ( NOmega > nDOF ) then + CALL SetErrStat(ErrID_Fatal,"NOmega must be less than or equal to N",ErrStat,ErrMsg,'EigenSolveWrap') + CALL CleanupEigen() + return + end if + + ! --- Eigenvalue analysis + CALL AllocAry(EigVect_LAKi, N, N, 'EigVect', ErrStat2, ErrMsg2); if(Failed()) return; + CALL AllocAry(Omega_LaKi, N , 'Omega', ErrStat2, ErrMsg2); if(Failed()) return; ! <<< NOTE: Needed due to dimension of Omega + CALL EigenSolve(K_LaKi, M_LaKi, N, bCheckSingularity, EigVect_LaKi, Omega_LaKi, ErrStat2, ErrMsg2 ); if (Failed()) return; + + Omega(:) = huge(1.0_ReKi) + Omega(1:nOmega) = real(Omega_LaKi(1:nOmega), FEKi) !<<< nOmega= AbortErrLev + if (Failed) call CleanUpEigen() + END FUNCTION Failed + + SUBROUTINE CleanupEigen() + IF (ALLOCATED(Omega_LaKi) ) DEALLOCATE(Omega_LaKi) + IF (ALLOCATED(EigVect_LaKi)) DEALLOCATE(EigVect_LaKi) + IF (ALLOCATED(K_LaKi) ) DEALLOCATE(K_LaKi) + IF (ALLOCATED(M_LaKi) ) DEALLOCATE(M_LaKi) + END SUBROUTINE CleanupEigen + +END SUBROUTINE EigenSolveWrap + !----------------------------------------------------------------------------------------------------------------------------------- !> This routine is called at the end of the simulation. SUBROUTINE BD_End( u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) diff --git a/modules/beamdyn/src/BeamDyn_Types.f90 b/modules/beamdyn/src/BeamDyn_Types.f90 index 537c3af3a..4e20db015 100644 --- a/modules/beamdyn/src/BeamDyn_Types.f90 +++ b/modules/beamdyn/src/BeamDyn_Types.f90 @@ -212,6 +212,8 @@ MODULE BeamDyn_Types LOGICAL :: RotStates = .false. !< Orient states in rotating frame during linearization? (flag) [-] LOGICAL :: CompAeroMaps = .FALSE. !< flag to determine if BeamDyn is computing aero maps (true) or running a normal simulation (false) [-] LOGICAL :: CompAppliedLdAtRoot = .FALSE. !< flag to determine if BeamDyn should compute the applied loads at root [-] + REAL(R8Ki) , DIMENSION(:,:), ALLOCATABLE :: eigenvectors !< Mode shape eigenvectors [-] + REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: omega !< Modal frequencies [-] END TYPE BD_ParameterType ! ======================= ! ========= BD_InputType ======= @@ -1492,6 +1494,30 @@ subroutine BD_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg) DstParamData%RotStates = SrcParamData%RotStates DstParamData%CompAeroMaps = SrcParamData%CompAeroMaps DstParamData%CompAppliedLdAtRoot = SrcParamData%CompAppliedLdAtRoot + if (allocated(SrcParamData%eigenvectors)) then + LB(1:2) = lbound(SrcParamData%eigenvectors) + UB(1:2) = ubound(SrcParamData%eigenvectors) + if (.not. allocated(DstParamData%eigenvectors)) then + allocate(DstParamData%eigenvectors(LB(1):UB(1),LB(2):UB(2)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%eigenvectors.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstParamData%eigenvectors = SrcParamData%eigenvectors + end if + if (allocated(SrcParamData%omega)) then + LB(1:1) = lbound(SrcParamData%omega) + UB(1:1) = ubound(SrcParamData%omega) + if (.not. allocated(DstParamData%omega)) then + allocate(DstParamData%omega(LB(1):UB(1)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%omega.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstParamData%omega = SrcParamData%omega + end if end subroutine subroutine BD_DestroyParam(ParamData, ErrStat, ErrMsg) @@ -1597,6 +1623,12 @@ subroutine BD_DestroyParam(ParamData, ErrStat, ErrMsg) if (allocated(ParamData%FEweight)) then deallocate(ParamData%FEweight) end if + if (allocated(ParamData%eigenvectors)) then + deallocate(ParamData%eigenvectors) + end if + if (allocated(ParamData%omega)) then + deallocate(ParamData%omega) + end if end subroutine subroutine BD_PackParam(RF, Indata) @@ -1690,6 +1722,8 @@ subroutine BD_PackParam(RF, Indata) call RegPack(RF, InData%RotStates) call RegPack(RF, InData%CompAeroMaps) call RegPack(RF, InData%CompAppliedLdAtRoot) + call RegPackAlloc(RF, InData%eigenvectors) + call RegPackAlloc(RF, InData%omega) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -1794,6 +1828,8 @@ subroutine BD_UnPackParam(RF, OutData) call RegUnpack(RF, OutData%RotStates); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%CompAeroMaps); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%CompAppliedLdAtRoot); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%eigenvectors); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%omega); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine BD_CopyInput(SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg) diff --git a/modules/beamdyn/src/Registry_BeamDyn.txt b/modules/beamdyn/src/Registry_BeamDyn.txt index 48f62ee20..a8fbac5f7 100644 --- a/modules/beamdyn/src/Registry_BeamDyn.txt +++ b/modules/beamdyn/src/Registry_BeamDyn.txt @@ -222,6 +222,9 @@ typedef ^ ParameterType ^ FEweight {:}{:} typedef ^ ParameterType logical RotStates - - - "Orient states in rotating frame during linearization? (flag)" - typedef ^ ParameterType LOGICAL CompAeroMaps - .FALSE. - "flag to determine if BeamDyn is computing aero maps (true) or running a normal simulation (false)" - typedef ^ ParameterType LOGICAL CompAppliedLdAtRoot - .FALSE. - "flag to determine if BeamDyn should compute the applied loads at root" - +# .... arrays for modal damping ........................................................................................................ +typedef ^ ParameterType R8Ki eigenvectors {:}{:} - - "Mode shape eigenvectors" - +typedef ^ ParameterType ^ omega {:} - - "Modal frequencies" - # ..... Inputs diff --git a/modules/nwtc-library/src/NWTC_Num.f90 b/modules/nwtc-library/src/NWTC_Num.f90 index d4f36542d..7e246cf4b 100644 --- a/modules/nwtc-library/src/NWTC_Num.f90 +++ b/modules/nwtc-library/src/NWTC_Num.f90 @@ -81,6 +81,7 @@ MODULE NWTC_Num INTEGER, PARAMETER :: kernelType_TRICUBE = 5 INTEGER, PARAMETER :: kernelType_GAUSSIAN = 6 + INTEGER, PARAMETER :: LaKi = R8Ki ! Define the kind to be used for LaPack, Eigensolve ! constants for output formats INTEGER, PARAMETER :: Output_in_Native_Units = 0 @@ -7294,5 +7295,184 @@ SUBROUTINE fZero_R8(x, f, roots, nZeros, Period) end if END SUBROUTINE fZero_R8 +!======================================================================= + ! Copy of EigenSolve from SubDyn, migrated here to use for beamdyn modal damping. + !> Return eigenvalues, Omega, and eigenvectors + SUBROUTINE EigenSolve(K, M, N, bCheckSingularity, EigVect, Omega, ErrStat, ErrMsg ) + USE NWTC_LAPACK, only: LAPACK_ggev + + INTEGER , INTENT(IN ) :: N !< Number of degrees of freedom, size of M and K + REAL(LaKi), INTENT(INOUT) :: K(N, N) !< Stiffness matrix + REAL(LaKi), INTENT(INOUT) :: M(N, N) !< Mass matrix + LOGICAL, INTENT(IN ) :: bCheckSingularity ! If True, the solver will fail if rigid modes are present + REAL(LaKi), INTENT(INOUT) :: EigVect(N, N) !< Returned Eigenvectors + REAL(LaKi), INTENT(INOUT) :: Omega(N) !< Returned Eigenvalues + INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None + ! LOCALS + REAL(LaKi), ALLOCATABLE :: WORK (:), VL(:,:), AlphaR(:), AlphaI(:), BETA(:) ! eigensolver variables + INTEGER :: i + INTEGER :: LWORK !variables for the eigensolver + INTEGER, ALLOCATABLE :: KEY(:) + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + REAL(LaKi) :: normA + REAL(LaKi) :: Omega2(N) !< Squared eigenvalues + REAL(LaKi), parameter :: MAX_EIGENVALUE = HUGE(1.0_ReKi) ! To avoid overflow when switching to ReKi + + ErrStat = ErrID_None + ErrMsg = '' + + ! allocate working arrays and return arrays for the eigensolver + LWORK=8*N + 16 !this is what the eigensolver wants >> bjj: +16 because of MKL ?ggev documenation ( "lwork >= max(1, 8n+16) for real flavors"), though LAPACK documenation says 8n is fine + !bjj: there seems to be a memory problem in *GGEV, so I'm making the WORK array larger to see if I can figure it out + CALL AllocAry( Work, LWORK, 'Work', ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'EigenSolve') + CALL AllocAry( AlphaR, N, 'AlphaR', ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'EigenSolve') + CALL AllocAry( AlphaI, N, 'AlphaI', ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'EigenSolve') + CALL AllocAry( Beta, N, 'Beta', ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'EigenSolve') + CALL AllocAry( VL, N, N, 'VL', ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'EigenSolve') + CALL AllocAry( KEY, N, 'KEY', ErrStat2, ErrMsg2 ); if(Failed()) return + + ! --- Eigenvalue analysis + ! note: SGGEV seems to have memory issues in certain cases. The eigenvalues seem to be okay, but the eigenvectors vary wildly with different compiling options. + ! DGGEV seems to work better, so I'm making these variables LaKi (which is set to R8Ki for now) - bjj 4/25/2014 + ! bjj: This comes from the LAPACK documentation: + ! Note: the quotients AlphaR(j)/BETA(j) and AlphaI(j)/BETA(j) may easily over- or underflow, and BETA(j) may even be zero. + ! Thus, the user should avoid naively computing the ratio Alpha/beta. However, AlphaR and AlphaI will be always less + ! than and usually comparable with norm(A) in magnitude, and BETA always less than and usually comparable with norm(B). + ! Omega2=AlphaR/BETA !Note this may not be correct if AlphaI<>0 and/or BETA=0 TO INCLUDE ERROR CHECK, also they need to be sorted + CALL LAPACK_ggev('N','V',N ,K, M, AlphaR, AlphaI, Beta, VL, EigVect, WORK, LWORK, ErrStat2, ErrMsg2) + if(Failed()) return + + ! --- Determining and sorting eigen frequencies + Omega2(:) =0.0_LaKi + DO I=1,N !Initialize the key and calculate Omega + KEY(I)=I + !Omega2(I) = AlphaR(I)/Beta(I) + if ( EqualRealNos(real(Beta(I),ReKi),0.0_ReKi) ) then + ! --- Beta =0 + if (bCheckSingularity) call WrScr('[WARN] Large eigenvalue found, system may be ill-conditioned') + Omega2(I) = MAX_EIGENVALUE + elseif ( EqualRealNos(real(AlphaI(I),ReKi),0.0_ReKi) ) THEN + ! --- Real Eigenvalues + IF ( AlphaR(I)<0.0_LaKi ) THEN + if ( (AlphaR(I)/Beta(I))<1e-6_LaKi ) then + ! Tolerating very small negative eigenvalues + if (bCheckSingularity) call WrScr('[INFO] Negative eigenvalue found with small norm (system may contain rigid body mode)') + Omega2(I)=0.0_LaKi + else + if (bCheckSingularity) call WrScr('[WARN] Negative eigenvalue found, system may be ill-conditioned.') + Omega2(I)=AlphaR(I)/Beta(I) + endif + else + Omega2(I) = AlphaR(I)/Beta(I) + endif + else + ! --- Complex Eigenvalues + normA = sqrt(AlphaR(I)**2 + AlphaI(I)**2) + if ( (normA/Beta(I))<1e-6_LaKi ) then + ! Tolerating very small eigenvalues with imaginary part + if (bCheckSingularity) call WrScr('[WARN] Complex eigenvalue found with small norm, approximating as 0') + Omega2(I) = 0.0_LaKi + elseif ( abs(AlphaR(I))>1e3_LaKi*abs(AlphaI(I)) ) then + ! Tolerating very small imaginary part compared to real part... (not pretty) + if (bCheckSingularity) call WrScr('[WARN] Complex eigenvalue found with small Im compare to Re') + Omega2(I) = AlphaR(I)/Beta(I) + else + if (bCheckSingularity) call WrScr('[WARN] Complex eigenvalue found with large imaginary value)') + Omega2(I) = MAX_EIGENVALUE + endif + !call Fatal('Complex eigenvalue found, system may be ill-conditioned'); return + endif + ! Capping to avoid overflow + if (Omega2(I)> MAX_EIGENVALUE) then + Omega2(I) = MAX_EIGENVALUE + endif + enddo + + ! Sorting. LASRT has issues for double precision 64 bit on windows + !CALL ScaLAPACK_LASRT('I',N,Omega2,KEY,ErrStat2,ErrMsg2); if(Failed()) return + CALL sort_in_place(Omega2,KEY) + + ! --- Sorting eigen vectors + ! KEEP ME: scaling of the eigenvectors using generalized mass =identity criterion + ! ALLOCATE(normcoeff(N,N), STAT = ErrStat ) + ! result1 = matmul(M,EigVect) + ! result2 = matmul(transpose(EigVect),result1) + ! normcoeff=sqrt(result2) !This should be a diagonal matrix which contains the normalization factors + ! normcoeff=sqrt(matmul(transpose(EigVect),matmul(M,EigVect))) !This should be a diagonal matrix which contains the normalization factors + VL=EigVect !temporary storage for sorting EigVect + DO I=1,N + !EigVect(:,I)=VL(:,KEY(I))/normcoeff(KEY(I),KEY(I)) !reordered and normalized + EigVect(:,I)=VL(:,KEY(I)) !just reordered as Huimin had a normalization outside of this one + ENDDO + !+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++! + + ! --- Return Omega (capped by huge(ReKi)) and check for singularity + Omega(:) = 0.0_LaKi + do I=1,N + if (EqualRealNos(real(Omega2(I),ReKi), 0.0_ReKi)) then ! NOTE: may be necessary for some corner numerics + Omega(i)=0.0_LaKi + if (bCheckSingularity) then + call Fatal('Zero eigenvalue found, system may contain rigid body mode'); return + endif + elseif (Omega2(I)>0) then + Omega(i)=sqrt(Omega2(I)) + else + ! Negative eigenfrequency + print*,'>>> Wrong eigenfrequency, Omega^2=',Omega2(I) ! <<< This should never happen + Omega(i)= 0.0_LaKi + call Fatal('Negative eigenvalue found, system may be ill-conditioned'); return + endif + enddo + + CALL CleanupEigen() + RETURN + + CONTAINS + LOGICAL FUNCTION Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'EigenSolve') + Failed = ErrStat >= AbortErrLev + if (Failed) call CleanUpEigen() + END FUNCTION Failed + + SUBROUTINE Fatal(ErrMsg_in) + character(len=*), intent(in) :: ErrMsg_in + CALL SetErrStat(ErrID_Fatal, ErrMsg_in, ErrStat, ErrMsg, 'EigenSolve'); + CALL CleanUpEigen() + END SUBROUTINE Fatal + + SUBROUTINE CleanupEigen() + IF (ALLOCATED(Work) ) DEALLOCATE(Work) + IF (ALLOCATED(AlphaR)) DEALLOCATE(AlphaR) + IF (ALLOCATED(AlphaI)) DEALLOCATE(AlphaI) + IF (ALLOCATED(Beta) ) DEALLOCATE(Beta) + IF (ALLOCATED(VL) ) DEALLOCATE(VL) + IF (ALLOCATED(KEY) ) DEALLOCATE(KEY) + END SUBROUTINE CleanupEigen + + pure subroutine sort_in_place(a,key) + real(LaKi), intent(inout), dimension(:) :: a + integer(IntKi), intent(inout), dimension(:) :: key + integer(IntKi) :: tempI + real(LaKi) :: temp + integer(IntKi) :: i, j + do i = 2, size(a) + j = i - 1 + temp = a(i) + tempI = key(i) + do while (j>=1 .and. a(j)>temp) + a(j+1) = a(j) + key(j+1) = key(j) + j = j - 1 + if (j<1) then + exit + endif + end do + a(j+1) = temp + key(j+1) = tempI + end do + end subroutine sort_in_place + END SUBROUTINE EigenSolve !======================================================================= END MODULE NWTC_Num diff --git a/modules/subdyn/src/FEM.f90 b/modules/subdyn/src/FEM.f90 index 84cdff9ef..97a14a902 100644 --- a/modules/subdyn/src/FEM.f90 +++ b/modules/subdyn/src/FEM.f90 @@ -23,7 +23,7 @@ MODULE FEM IMPLICIT NONE INTEGER, PARAMETER :: FEKi = R8Ki ! Define the kind to be used for FEM - INTEGER, PARAMETER :: LaKi = R8Ki ! Define the kind to be used for LaPack + ! INTEGER, PARAMETER :: LaKi = R8Ki ! Define the kind to be used for LaPack INTERFACE FINDLOCI ! In the future, use FINDLOC from intrinsic MODULE PROCEDURE FINDLOCI_R8Ki @@ -33,162 +33,6 @@ MODULE FEM CONTAINS -!------------------------------------------------------------------------------------------------------ -!> Return eigenvalues, Omega, and eigenvectors - -SUBROUTINE EigenSolve(K, M, N, bCheckSingularity, EigVect, Omega, ErrStat, ErrMsg ) - USE NWTC_LAPACK, only: LAPACK_ggev - INTEGER , INTENT(IN ) :: N !< Number of degrees of freedom, size of M and K - REAL(LaKi), INTENT(INOUT) :: K(N, N) !< Stiffness matrix - REAL(LaKi), INTENT(INOUT) :: M(N, N) !< Mass matrix - LOGICAL, INTENT(IN ) :: bCheckSingularity ! If True, the solver will fail if rigid modes are present - REAL(LaKi), INTENT(INOUT) :: EigVect(N, N) !< Returned Eigenvectors - REAL(LaKi), INTENT(INOUT) :: Omega(N) !< Returned Eigenvalues - INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation - CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None - ! LOCALS - REAL(LaKi), ALLOCATABLE :: WORK (:), VL(:,:), AlphaR(:), AlphaI(:), BETA(:) ! eigensolver variables - INTEGER :: i - INTEGER :: LWORK !variables for the eigensolver - INTEGER, ALLOCATABLE :: KEY(:) - INTEGER(IntKi) :: ErrStat2 - CHARACTER(ErrMsgLen) :: ErrMsg2 - REAL(LaKi) :: normA - REAL(LaKi) :: Omega2(N) !< Squared eigenvalues - REAL(LaKi), parameter :: MAX_EIGENVALUE = HUGE(1.0_ReKi) ! To avoid overflow when switching to ReKi - - ErrStat = ErrID_None - ErrMsg = '' - - ! allocate working arrays and return arrays for the eigensolver - LWORK=8*N + 16 !this is what the eigensolver wants >> bjj: +16 because of MKL ?ggev documenation ( "lwork >= max(1, 8n+16) for real flavors"), though LAPACK documenation says 8n is fine - !bjj: there seems to be a memory problem in *GGEV, so I'm making the WORK array larger to see if I can figure it out - CALL AllocAry( Work, LWORK, 'Work', ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'EigenSolve') - CALL AllocAry( AlphaR, N, 'AlphaR', ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'EigenSolve') - CALL AllocAry( AlphaI, N, 'AlphaI', ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'EigenSolve') - CALL AllocAry( Beta, N, 'Beta', ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'EigenSolve') - CALL AllocAry( VL, N, N, 'VL', ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'EigenSolve') - CALL AllocAry( KEY, N, 'KEY', ErrStat2, ErrMsg2 ); if(Failed()) return - - ! --- Eigenvalue analysis - ! note: SGGEV seems to have memory issues in certain cases. The eigenvalues seem to be okay, but the eigenvectors vary wildly with different compiling options. - ! DGGEV seems to work better, so I'm making these variables LaKi (which is set to R8Ki for now) - bjj 4/25/2014 - ! bjj: This comes from the LAPACK documentation: - ! Note: the quotients AlphaR(j)/BETA(j) and AlphaI(j)/BETA(j) may easily over- or underflow, and BETA(j) may even be zero. - ! Thus, the user should avoid naively computing the ratio Alpha/beta. However, AlphaR and AlphaI will be always less - ! than and usually comparable with norm(A) in magnitude, and BETA always less than and usually comparable with norm(B). - ! Omega2=AlphaR/BETA !Note this may not be correct if AlphaI<>0 and/or BETA=0 TO INCLUDE ERROR CHECK, also they need to be sorted - CALL LAPACK_ggev('N','V',N ,K, M, AlphaR, AlphaI, Beta, VL, EigVect, WORK, LWORK, ErrStat2, ErrMsg2) - if(Failed()) return - - ! --- Determinign and sorting eigen frequencies - Omega2(:) =0.0_LaKi - DO I=1,N !Initialize the key and calculate Omega - KEY(I)=I - !Omega2(I) = AlphaR(I)/Beta(I) - if ( EqualRealNos(real(Beta(I),ReKi),0.0_ReKi) ) then - ! --- Beta =0 - if (bCheckSingularity) call WrScr('[WARN] Large eigenvalue found, system may be ill-conditioned') - Omega2(I) = MAX_EIGENVALUE - elseif ( EqualRealNos(real(AlphaI(I),ReKi),0.0_ReKi) ) THEN - ! --- Real Eigenvalues - IF ( AlphaR(I)<0.0_LaKi ) THEN - if ( (AlphaR(I)/Beta(I))<1e-6_LaKi ) then - ! Tolerating very small negative eigenvalues - if (bCheckSingularity) call WrScr('[INFO] Negative eigenvalue found with small norm (system may contain rigid body mode)') - Omega2(I)=0.0_LaKi - else - if (bCheckSingularity) call WrScr('[WARN] Negative eigenvalue found, system may be ill-conditioned.') - Omega2(I)=AlphaR(I)/Beta(I) - endif - else - Omega2(I) = AlphaR(I)/Beta(I) - endif - else - ! --- Complex Eigenvalues - normA = sqrt(AlphaR(I)**2 + AlphaI(I)**2) - if ( (normA/Beta(I))<1e-6_LaKi ) then - ! Tolerating very small eigenvalues with imaginary part - if (bCheckSingularity) call WrScr('[WARN] Complex eigenvalue found with small norm, approximating as 0') - Omega2(I) = 0.0_LaKi - elseif ( abs(AlphaR(I))>1e3_LaKi*abs(AlphaI(I)) ) then - ! Tolerating very small imaginary part compared to real part... (not pretty) - if (bCheckSingularity) call WrScr('[WARN] Complex eigenvalue found with small Im compare to Re') - Omega2(I) = AlphaR(I)/Beta(I) - else - if (bCheckSingularity) call WrScr('[WARN] Complex eigenvalue found with large imaginary value)') - Omega2(I) = MAX_EIGENVALUE - endif - !call Fatal('Complex eigenvalue found, system may be ill-conditioned'); return - endif - ! Capping to avoid overflow - if (Omega2(I)> MAX_EIGENVALUE) then - Omega2(I) = MAX_EIGENVALUE - endif - enddo - - ! Sorting. LASRT has issues for double precision 64 bit on windows - !CALL ScaLAPACK_LASRT('I',N,Omega2,KEY,ErrStat2,ErrMsg2); if(Failed()) return - CALL sort_in_place(Omega2,KEY) - - ! --- Sorting eigen vectors - ! KEEP ME: scaling of the eigenvectors using generalized mass =identity criterion - ! ALLOCATE(normcoeff(N,N), STAT = ErrStat ) - ! result1 = matmul(M,EigVect) - ! result2 = matmul(transpose(EigVect),result1) - ! normcoeff=sqrt(result2) !This should be a diagonal matrix which contains the normalization factors - ! normcoeff=sqrt(matmul(transpose(EigVect),matmul(M,EigVect))) !This should be a diagonal matrix which contains the normalization factors - VL=EigVect !temporary storage for sorting EigVect - DO I=1,N - !EigVect(:,I)=VL(:,KEY(I))/normcoeff(KEY(I),KEY(I)) !reordered and normalized - EigVect(:,I)=VL(:,KEY(I)) !just reordered as Huimin had a normalization outside of this one - ENDDO - !+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++! - - ! --- Return Omega (capped by huge(ReKi)) and check for singularity - Omega(:) = 0.0_LaKi - do I=1,N - if (EqualRealNos(real(Omega2(I),ReKi), 0.0_ReKi)) then ! NOTE: may be necessary for some corner numerics - Omega(i)=0.0_LaKi - if (bCheckSingularity) then - call Fatal('Zero eigenvalue found, system may contain rigid body mode'); return - endif - elseif (Omega2(I)>0) then - Omega(i)=sqrt(Omega2(I)) - else - ! Negative eigenfrequency - print*,'>>> Wrong eigenfrequency, Omega^2=',Omega2(I) ! <<< This should never happen - Omega(i)= 0.0_LaKi - call Fatal('Negative eigenvalue found, system may be ill-conditioned'); return - endif - enddo - - CALL CleanupEigen() - RETURN - -CONTAINS - LOGICAL FUNCTION Failed() - call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'EigenSolve') - Failed = ErrStat >= AbortErrLev - if (Failed) call CleanUpEigen() - END FUNCTION Failed - - SUBROUTINE Fatal(ErrMsg_in) - character(len=*), intent(in) :: ErrMsg_in - CALL SetErrStat(ErrID_Fatal, ErrMsg_in, ErrStat, ErrMsg, 'EigenSolve'); - CALL CleanUpEigen() - END SUBROUTINE Fatal - - SUBROUTINE CleanupEigen() - IF (ALLOCATED(Work) ) DEALLOCATE(Work) - IF (ALLOCATED(AlphaR)) DEALLOCATE(AlphaR) - IF (ALLOCATED(AlphaI)) DEALLOCATE(AlphaI) - IF (ALLOCATED(Beta) ) DEALLOCATE(Beta) - IF (ALLOCATED(VL) ) DEALLOCATE(VL) - IF (ALLOCATED(KEY) ) DEALLOCATE(KEY) - END SUBROUTINE CleanupEigen - -END SUBROUTINE EigenSolve pure subroutine sort_in_place(a,key) real(LaKi), intent(inout), dimension(:) :: a @@ -687,6 +531,8 @@ END SUBROUTINE CraigBamptonReduction_FromPartition !! Case1: K and M are taken "as is", this is used for the "LL" part of the matrix !! Case2: K and M contain some constraints lines, and they need to be removed from the Mass/Stiffness matrix. Used for full system SUBROUTINE EigenSolveWrap(K, M, nDOF, NOmega, bCheckSingularity, EigVect, Omega, ErrStat, ErrMsg, bDOF ) + USE NWTC_Num, only: EigenSolve + INTEGER, INTENT(IN ) :: nDOF ! Total degrees of freedom of the incoming system REAL(FEKi), INTENT(IN ) :: K(nDOF, nDOF) ! stiffness matrix REAL(FEKi), INTENT(IN ) :: M(nDOF, nDOF) ! mass matrix From 7910f02798b43a2633be0f28fec22e44e68b934a Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Mon, 22 Dec 2025 14:30:17 -0700 Subject: [PATCH 03/24] Construction of damping matrix in rotating frame and clean-up. --- modules/beamdyn/src/BeamDyn.f90 | 91 +++++++++++++++++++----- modules/beamdyn/src/BeamDyn_Types.f90 | 42 ++++------- modules/beamdyn/src/Registry_BeamDyn.txt | 3 +- 3 files changed, 88 insertions(+), 48 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index bb9890122..3eddce96b 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -194,7 +194,7 @@ SUBROUTINE BD_Init( InitInp, u, p, x, xd, z, OtherState, y, MiscVar, Interval, I ! Calculation of modal damping here IF(p%damp_flag .EQ. 2) THEN - call Init_ModalDamping(p, x, MiscVar, OtherState) + call Init_ModalDamping(x, OtherState, p, MiscVar) ENDIF !................................. @@ -1796,23 +1796,37 @@ END SUBROUTINE Init_ContinuousStates !----------------------------------------------------------------------------------------------------------------------------------- !> This routine initializes modal damping. -SUBROUTINE Init_ModalDamping(p, x, m, OtherState) +SUBROUTINE Init_ModalDamping(x, OtherState, p, m) ! TODO : Look at what should be INOUT v. IN - TYPE(BD_ParameterType), INTENT(INOUT) :: p !< Parameters, output eigenpairs here - TYPE(BD_ContinuousStateType), INTENT(INOUT) :: x !< Continuous states at t on input at t + dt on output + TYPE(BD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at t on input at t + dt on output + type(BD_OtherStateType), INTENT(IN ) :: OtherState !< Global rotations are stored in otherstate + TYPE(BD_ParameterType), INTENT(INOUT) :: p !< Parameters, output modal damping matrix in original frame here TYPE(BD_MiscVarType), INTENT(INOUT) :: m !< misc/optimization variables - type(BD_OtherStateType), INTENT(INOUT) :: OtherState !< Global rotations are stored in otherstate + ! Local variables INTEGER(IntKi) :: nDOF + INTEGER(IntKi) :: j ! looping indexing variable + INTEGER(IntKi) :: numZeta ! number of damping values + REAL :: Zj ! diagonal element of the modal damping matrix INTEGER(IntKi) :: ErrStat CHARACTER(ErrMsgLen) :: ErrMsg + REAL(R8Ki), ALLOCATABLE :: eigenvectors (:, :) ! mode shapes + REAL(R8Ki), ALLOCATABLE :: omega (:) ! modal frequencies (rad/s) + REAL(LaKi), ALLOCATABLE :: phiT_M (:, :) ! mode shapes transpose times mass matrix + REAL(LaKi), ALLOCATABLE :: phi0T_M_phi0 (:, :) ! normalization calculation of mass matrix + REAL, DIMENSION(10) :: zeta ! Modal damping values, should be changed to be user input. ErrStat = ErrID_None ErrMsg = '' - PRINT *, 'In modal damping branch.' + PRINT *, 'In modal damping branch. Damping values are currently hard-coded.' + + ! TODO : Take actual user input for zeta + ! zeta is fraction of critical damping. + zeta = (/ 0.001d0, 0.003d0, 0.0015d0, 0.0045d0, 0.002d0, 0.006d0, 0.007d0, 0.008d0, 0.009d0, 0.010d0 /) + numZeta = size(zeta, 1) ! 0. Setup quadrature points CALL BD_QuadraturePointData(p, x, m) @@ -1820,11 +1834,8 @@ SUBROUTINE Init_ModalDamping(p, x, m, OtherState) ! 1. Generates K, M Matrices ! These go into 'm%StifK' and 'm%MassM' CALL BD_GenerateDynamicElementGA2( x, OtherState, p, m, .TRUE.) - ! Do I need to call error handling after this? ! 2. Copy lines from 'BD_CalcForceAcc' for M, K -> 2D and apply Boundary conditions - ! In that function, this produces LP_MassM_LU - ! Full mass matrix (n_dof, n_dof) m%LP_MassM = reshape(m%MassM, [p%dof_total, p%dof_total]) @@ -1838,24 +1849,72 @@ SUBROUTINE Init_ModalDamping(p, x, m, OtherState) m%LP_StifK_LU = m%LP_StifK(7:p%dof_total, 7:p%dof_total) ! 3. Do eigenanalysis - ! for now, calculate all eigenpairs + ! For now, calculate all eigenpairs nDOF = p%dof_total - 6 - ! TODO : Since these are on the parameters is deallocating automatically handled? - CALL AllocAry(p%eigenvectors, nDOF, nDOF, 'p%eigenvectors', ErrStat, ErrMsg) + CALL AllocAry(eigenvectors, nDOF, nDOF, 'eigenvectors', ErrStat, ErrMsg) + CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') + + CALL AllocAry(omega, nDOF, 'omega', ErrStat, ErrMsg) CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') - CALL AllocAry(p%omega, nDOF, 'p%omega', ErrStat, ErrMsg) + CALL EigenSolveWrap(m%LP_StifK_LU, m%LP_MassM_LU, nDOF, nDOF, .TRUE., eigenvectors, omega, ErrStat, ErrMsg ) + + ! Mass-normalize the mode shapes + CALL AllocAry(phi0T_M_phi0, nDOF, nDof, 'phi0T_M_phi0', ErrStat, ErrMsg) CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') - CALL EigenSolveWrap(m%LP_StifK_LU, m%LP_MassM_LU, nDOF, nDOF, .TRUE., p%eigenvectors, p%omega, ErrStat, ErrMsg ) + phi0T_M_phi0 = matmul(transpose(eigenvectors), matmul(m%LP_MassM_LU, eigenvectors)) + + do j = 1, nDOF + eigenvectors(:, j) = eigenvectors(:, j) / sqrt(phi0T_M_phi0(j, j)) + end do + + ! 4. Generate damping matrix in original frame + CALL AllocAry(phiT_M, nDOF, nDof, 'phiT_M', ErrStat, ErrMsg) + CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') - ! TODO : Mode shapes need to be mass normalized. + phiT_M = matmul(transpose(eigenvectors), m%LP_MassM_LU) ! after normalization - ! 4. Save eigenvectors/eigenvalues or damping matrix in original frame + CALL AllocAry(p%ModalDampingMat, nDOF, nDOF, 'p%ModalDampingMat', ErrStat, ErrMsg) + CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') + + do j = 1, nDOF + + if( j <= numZeta) then + Zj = 2.0d0 * omega(j) * zeta(j) + else + ! Stiffness proportional damping is used past the last prescribed value + ! at a rate equal to the last prescribed value. + Zj = 2.0d0 * omega(j) * (zeta(numZeta) * omega(j) / omega(numZeta)) + endif + + p%ModalDampingMat(j, :) = Zj * phiT_M(j, :) + end do + + p%ModalDampingMat = matmul(transpose(phiT_M), p%ModalDampingMat) + + ! Debugging / verifying - recover the zeta values + ! phi0T_M_phi0 = matmul(transpose(eigenvectors), matmul(m%LP_MassM_LU, eigenvectors)) + ! print *, 'Verifying mass orthonormal here.' + ! phi0T_M_phi0 = matmul(transpose(eigenvectors), matmul(p%ModalDampingMat, eigenvectors)) + ! do j = 1,nDOF + ! phi0T_M_phi0(j,j) = phi0T_M_phi0(j,j) / omega(j) / 2.0d0 + ! end do print *, 'End of Modal damping.' + CALL CleanupModalInit() + +CONTAINS + + SUBROUTINE CleanupModalInit() + IF (ALLOCATED(eigenvectors) ) DEALLOCATE(eigenvectors) + IF (ALLOCATED(omega) ) DEALLOCATE(omega) + IF (ALLOCATED(phi0T_M_phi0) ) DEALLOCATE(phi0T_M_phi0) + IF (ALLOCATED(phiT_M) ) DEALLOCATE(phiT_M) + END SUBROUTINE CleanupModalInit + END SUBROUTINE !> Wrapper function for eigen value analyses diff --git a/modules/beamdyn/src/BeamDyn_Types.f90 b/modules/beamdyn/src/BeamDyn_Types.f90 index 4e20db015..8a0c32364 100644 --- a/modules/beamdyn/src/BeamDyn_Types.f90 +++ b/modules/beamdyn/src/BeamDyn_Types.f90 @@ -212,8 +212,7 @@ MODULE BeamDyn_Types LOGICAL :: RotStates = .false. !< Orient states in rotating frame during linearization? (flag) [-] LOGICAL :: CompAeroMaps = .FALSE. !< flag to determine if BeamDyn is computing aero maps (true) or running a normal simulation (false) [-] LOGICAL :: CompAppliedLdAtRoot = .FALSE. !< flag to determine if BeamDyn should compute the applied loads at root [-] - REAL(R8Ki) , DIMENSION(:,:), ALLOCATABLE :: eigenvectors !< Mode shape eigenvectors [-] - REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: omega !< Modal frequencies [-] + REAL(R8Ki) , DIMENSION(:,:), ALLOCATABLE :: ModalDampingMat !< Modal damping matrix in the rotating frame [-] END TYPE BD_ParameterType ! ======================= ! ========= BD_InputType ======= @@ -1494,29 +1493,17 @@ subroutine BD_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg) DstParamData%RotStates = SrcParamData%RotStates DstParamData%CompAeroMaps = SrcParamData%CompAeroMaps DstParamData%CompAppliedLdAtRoot = SrcParamData%CompAppliedLdAtRoot - if (allocated(SrcParamData%eigenvectors)) then - LB(1:2) = lbound(SrcParamData%eigenvectors) - UB(1:2) = ubound(SrcParamData%eigenvectors) - if (.not. allocated(DstParamData%eigenvectors)) then - allocate(DstParamData%eigenvectors(LB(1):UB(1),LB(2):UB(2)), stat=ErrStat2) + if (allocated(SrcParamData%ModalDampingMat)) then + LB(1:2) = lbound(SrcParamData%ModalDampingMat) + UB(1:2) = ubound(SrcParamData%ModalDampingMat) + if (.not. allocated(DstParamData%ModalDampingMat)) then + allocate(DstParamData%ModalDampingMat(LB(1):UB(1),LB(2):UB(2)), stat=ErrStat2) if (ErrStat2 /= 0) then - call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%eigenvectors.', ErrStat, ErrMsg, RoutineName) + call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ModalDampingMat.', ErrStat, ErrMsg, RoutineName) return end if end if - DstParamData%eigenvectors = SrcParamData%eigenvectors - end if - if (allocated(SrcParamData%omega)) then - LB(1:1) = lbound(SrcParamData%omega) - UB(1:1) = ubound(SrcParamData%omega) - if (.not. allocated(DstParamData%omega)) then - allocate(DstParamData%omega(LB(1):UB(1)), stat=ErrStat2) - if (ErrStat2 /= 0) then - call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%omega.', ErrStat, ErrMsg, RoutineName) - return - end if - end if - DstParamData%omega = SrcParamData%omega + DstParamData%ModalDampingMat = SrcParamData%ModalDampingMat end if end subroutine @@ -1623,11 +1610,8 @@ subroutine BD_DestroyParam(ParamData, ErrStat, ErrMsg) if (allocated(ParamData%FEweight)) then deallocate(ParamData%FEweight) end if - if (allocated(ParamData%eigenvectors)) then - deallocate(ParamData%eigenvectors) - end if - if (allocated(ParamData%omega)) then - deallocate(ParamData%omega) + if (allocated(ParamData%ModalDampingMat)) then + deallocate(ParamData%ModalDampingMat) end if end subroutine @@ -1722,8 +1706,7 @@ subroutine BD_PackParam(RF, Indata) call RegPack(RF, InData%RotStates) call RegPack(RF, InData%CompAeroMaps) call RegPack(RF, InData%CompAppliedLdAtRoot) - call RegPackAlloc(RF, InData%eigenvectors) - call RegPackAlloc(RF, InData%omega) + call RegPackAlloc(RF, InData%ModalDampingMat) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -1828,8 +1811,7 @@ subroutine BD_UnPackParam(RF, OutData) call RegUnpack(RF, OutData%RotStates); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%CompAeroMaps); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%CompAppliedLdAtRoot); if (RegCheckErr(RF, RoutineName)) return - call RegUnpackAlloc(RF, OutData%eigenvectors); if (RegCheckErr(RF, RoutineName)) return - call RegUnpackAlloc(RF, OutData%omega); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%ModalDampingMat); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine BD_CopyInput(SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg) diff --git a/modules/beamdyn/src/Registry_BeamDyn.txt b/modules/beamdyn/src/Registry_BeamDyn.txt index a8fbac5f7..dd10f46d3 100644 --- a/modules/beamdyn/src/Registry_BeamDyn.txt +++ b/modules/beamdyn/src/Registry_BeamDyn.txt @@ -223,8 +223,7 @@ typedef ^ ParameterType logical RotStates - typedef ^ ParameterType LOGICAL CompAeroMaps - .FALSE. - "flag to determine if BeamDyn is computing aero maps (true) or running a normal simulation (false)" - typedef ^ ParameterType LOGICAL CompAppliedLdAtRoot - .FALSE. - "flag to determine if BeamDyn should compute the applied loads at root" - # .... arrays for modal damping ........................................................................................................ -typedef ^ ParameterType R8Ki eigenvectors {:}{:} - - "Mode shape eigenvectors" - -typedef ^ ParameterType ^ omega {:} - - "Modal frequencies" - +typedef ^ ParameterType R8Ki ModalDampingMat {:}{:} - - "Modal damping matrix in the rotating frame" - # ..... Inputs From b041ff12dcf9a3a148e9a6355da38f18f1806ca6 Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Tue, 30 Dec 2025 13:42:53 -0700 Subject: [PATCH 04/24] Calculated modal damping force (not sure if used correct velocities/coord transforms). Works for fixed ideal beam. --- modules/beamdyn/src/BeamDyn.f90 | 99 ++++++++++++++++++++---- modules/beamdyn/src/BeamDyn_Types.f90 | 38 ++++++++- modules/beamdyn/src/Registry_BeamDyn.txt | 5 +- 3 files changed, 125 insertions(+), 17 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 3eddce96b..0edb4d5bd 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -1799,6 +1799,7 @@ END SUBROUTINE Init_ContinuousStates SUBROUTINE Init_ModalDamping(x, OtherState, p, m) ! TODO : Look at what should be INOUT v. IN + ! TODO : Error messages are probably wrong in some way. TYPE(BD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at t on input at t + dt on output type(BD_OtherStateType), INTENT(IN ) :: OtherState !< Global rotations are stored in otherstate @@ -1818,6 +1819,9 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) REAL(LaKi), ALLOCATABLE :: phi0T_M_phi0 (:, :) ! normalization calculation of mass matrix REAL, DIMENSION(10) :: zeta ! Modal damping values, should be changed to be user input. + INTEGER(IntKi) :: ErrStat2 ! Temporary Error status + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary Error message + ErrStat = ErrID_None ErrMsg = '' @@ -1852,17 +1856,17 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) ! For now, calculate all eigenpairs nDOF = p%dof_total - 6 - CALL AllocAry(eigenvectors, nDOF, nDOF, 'eigenvectors', ErrStat, ErrMsg) - CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') + CALL AllocAry(eigenvectors, nDOF, nDOF, 'eigenvectors', ErrStat2, ErrMsg2) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') - CALL AllocAry(omega, nDOF, 'omega', ErrStat, ErrMsg) - CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') + CALL AllocAry(omega, nDOF, 'omega', ErrStat2, ErrMsg2) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') CALL EigenSolveWrap(m%LP_StifK_LU, m%LP_MassM_LU, nDOF, nDOF, .TRUE., eigenvectors, omega, ErrStat, ErrMsg ) ! Mass-normalize the mode shapes - CALL AllocAry(phi0T_M_phi0, nDOF, nDof, 'phi0T_M_phi0', ErrStat, ErrMsg) - CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') + CALL AllocAry(phi0T_M_phi0, nDOF, nDof, 'phi0T_M_phi0', ErrStat2, ErrMsg2) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') phi0T_M_phi0 = matmul(transpose(eigenvectors), matmul(m%LP_MassM_LU, eigenvectors)) @@ -1871,13 +1875,13 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) end do ! 4. Generate damping matrix in original frame - CALL AllocAry(phiT_M, nDOF, nDof, 'phiT_M', ErrStat, ErrMsg) - CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') + CALL AllocAry(phiT_M, nDOF, nDof, 'phiT_M', ErrStat2, ErrMsg2) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') phiT_M = matmul(transpose(eigenvectors), m%LP_MassM_LU) ! after normalization - CALL AllocAry(p%ModalDampingMat, nDOF, nDOF, 'p%ModalDampingMat', ErrStat, ErrMsg) - CALL SetErrStat(ErrStat, ErrMsg, ErrStat, ErrMsg, 'Init_ModalDamping') + CALL AllocAry(p%ModalDampingMat, nDOF, nDOF, 'p%ModalDampingMat', ErrStat2, ErrMsg2) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') do j = 1, nDOF @@ -1902,7 +1906,15 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) ! phi0T_M_phi0(j,j) = phi0T_M_phi0(j,j) / omega(j) / 2.0d0 ! end do - print *, 'End of Modal damping.' + ! Allocate memory for the velocity vector that will be multiplied by the modal damping matrix + CALL AllocAry(m%DampedVelocities, p%dof_total-6, 'DampedVelocities', ErrStat2, ErrMsg2) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') + + ! Allocate memory for the velocity vector that will be multiplied by the modal damping matrix + CALL AllocAry(m%ModalDampingF, p%dof_total-6, 'ModalDampingF', ErrStat2, ErrMsg2) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') + + print *, 'End of Modal damping init.' CALL CleanupModalInit() @@ -2182,7 +2194,7 @@ SUBROUTINE BD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, CALL BD_QPDataVelocity( p, x_tmp, m ) ! x%dqdt --> m%qp%vvv, m%qp%vvp ! calculate accelerations and reaction loads (in m%RHS): - CALL BD_CalcForceAcc(m%u, p, OtherState, m, ErrStat2,ErrMsg2) + CALL BD_CalcForceAcc(m%u, p, x, OtherState, m, ErrStat2,ErrMsg2) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) ELSE @@ -2306,7 +2318,7 @@ SUBROUTINE BD_CalcContStateDeriv( t, u, p, x, xd, z, OtherState, m, dxdt, ErrSta CALL BD_QPDataVelocity( p, dxdt, m ) ! x%dqdt --> m%qp%vvv, m%qp%vvp ! calculate accelerations and reaction loads (in m%RHS): - CALL BD_CalcForceAcc(m%u, p, OtherState, m, ErrStat2,ErrMsg2) + CALL BD_CalcForceAcc(m%u, p, x, OtherState, m, ErrStat2,ErrMsg2) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) if (ErrStat >= AbortErrLev) return @@ -5626,7 +5638,7 @@ SUBROUTINE BD_InitAcc( u, p, x, OtherState, m, qdotdot, ErrStat, ErrMsg ) CALL BD_QPDataVelocity(p, x, m) ! set misc vars, particularly m%RHS - CALL BD_CalcForceAcc( u, p, OtherState, m, ErrStat2, ErrMsg2 ) + CALL BD_CalcForceAcc( u, p, x, OtherState, m, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) ! set accelerations with inputs from the root and BD_CalcForceAcc solution @@ -5710,10 +5722,11 @@ END SUBROUTINE BD_InitAcc !! !! The root reaction force is therefore calculated afterwards as !! \f$ F_\textrm{root} = f_1 - \sum_{i} m_{1,i} a_{i} \f$. -SUBROUTINE BD_CalcForceAcc( u, p, OtherState, m, ErrStat, ErrMsg ) +SUBROUTINE BD_CalcForceAcc( u, p, x, OtherState, m, ErrStat, ErrMsg ) TYPE(BD_InputType), INTENT(IN ) :: u !< Inputs at t TYPE(BD_ParameterType), INTENT(IN ) :: p !< Parameters + TYPE(BD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states TYPE(BD_OtherStateType), INTENT(IN ) :: OtherState !< other states (contains ref orientation) TYPE(BD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation @@ -5763,6 +5776,11 @@ SUBROUTINE BD_CalcForceAcc( u, p, OtherState, m, ErrStat, ErrMsg ) ! Add force contributions from root acceleration m%LP_RHS_LU = m%LP_RHS_LU - matmul(m%LP_MassM(7:,1:6), RootAcc) + IF(p%damp_flag .EQ. 2) THEN + ! Because modal damping is already global, it wouldn't make sense in BD_AssembleRHS. + CALL BD_AddModalDampingRHS(u, p, x, OtherState, m) + ENDIF + ! Solve linear equations A * X = B for acceleration (F=ma) for nodes 2:p%node_total CALL LAPACK_getrf(n_free, n_free, m%LP_MassM_LU, m%LP_indx, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -5894,6 +5912,57 @@ SUBROUTINE BD_ComputeElementMass(nelem,p,NQPpos,EMass0_GL,elem_mass,elem_CG,elem END SUBROUTINE BD_ComputeElementMass +!----------------------------------------------------------------------------------------------------------------------------------- +!> This subroutine calculates the modal damping force +! Adds modal damping contributions to m%LP_RHS_LU +SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) + + TYPE(BD_InputType), INTENT(IN ) :: u !< Inputs at t + TYPE(BD_ParameterType), INTENT(IN ) :: p !< Parameters + TYPE(BD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states + TYPE(BD_OtherStateType), INTENT(IN ) :: OtherState !< other states (contains ref orientation) + TYPE(BD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables + INTEGER(IntKi) :: j ! looping indexing variable + + ! 1. Velocities relative to root + do j = 1, size(x%dqdt, 2)-1 + ! TODO : Is this the correct place to get the velocities? + ! m%DampedVelocities((j-1)*6+1:j*6) = x%dqdt(:, j+1) - x%dqdt(:, 0) + + + ! TODO : Also do a rotation here? + m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(OtherState%GlbRot, x%dqdt(1:3, j+1) - x%dqdt(1:3, 1)) + m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(OtherState%GlbRot, x%dqdt(4:6, j+1) - x%dqdt(4:6, 1)) + + end do + + ! 2. Velocities represented in rotating frame + + ! OtherState%GlbRot - will use to get rotation into correct coordinate system + ! Also might be relevant - u%RootMotion%... + + ! 3. Multiply by modal damping matrix + m%ModalDampingF = matmul(p%ModalDampingMat, m%DampedVelocities) + + ! 4. Rotate to correct coordinates and add to m%LP_RHS_LU + + ! TODO: Add or subtract to m%LP_RHS_LU? + ! m%LP_RHS_LU = m%LP_RHS_LU - m%ModalDampingF + + do j = 1, size(x%dqdt, 2)-1 + m%LP_RHS_LU((j-1)*6+1:(j-1)*6+3) = m%LP_RHS_LU((j-1)*6+1:(j-1)*6+3) & + - matmul(transpose(OtherState%GlbRot), m%ModalDampingF((j-1)*6+1:(j-1)*6+3)) + + m%LP_RHS_LU((j-1)*6+4:(j-1)*6+6) = m%LP_RHS_LU((j-1)*6+4:(j-1)*6+6) & + - matmul(transpose(OtherState%GlbRot), m%ModalDampingF((j-1)*6+4:(j-1)*6+6)) + end do + + + print *, 'TODO: Add modal damping force to the RHS vector here.' + print *, 'TODO: Should force be added or subtracted here? Should be clear in testing...' + +END SUBROUTINE BD_AddModalDampingRHS + subroutine BD_InitVars(u, p, x, y, m, InitOut, Linearize, ErrStat, ErrMsg) type(BD_InputType), intent(inout) :: u !< An initial guess for the input; input mesh must be defined diff --git a/modules/beamdyn/src/BeamDyn_Types.f90 b/modules/beamdyn/src/BeamDyn_Types.f90 index 8a0c32364..7c9d978b5 100644 --- a/modules/beamdyn/src/BeamDyn_Types.f90 +++ b/modules/beamdyn/src/BeamDyn_Types.f90 @@ -70,7 +70,7 @@ MODULE BeamDyn_Types REAL(R8Ki) , DIMENSION(:,:,:), ALLOCATABLE :: stiff0 !< C/S stiffness matrix arrays [-] REAL(R8Ki) , DIMENSION(:,:,:), ALLOCATABLE :: mass0 !< C/S mass matrix arrays [-] REAL(R8Ki) , DIMENSION(1:6) :: beta = 0.0_R8Ki !< Damping Coefficient [-] - INTEGER(IntKi) :: damp_flag = 0_IntKi !< Damping Flag: 0-No Damping, 1-Damped [-] + INTEGER(IntKi) :: damp_flag = 0_IntKi !< Damping Flag: 0-No Damping, 1-Stiffness Prop. Damped, 2-Modal Damping [-] END TYPE BladeInputData ! ======================= ! ========= BD_InputFile ======= @@ -306,6 +306,8 @@ MODULE BeamDyn_Types REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: LP_RHS !< Right-hand-side vector [-] REAL(R8Ki) , DIMENSION(:,:), ALLOCATABLE :: LP_StifK_LU !< Stiffness Matrix for LU [-] REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: LP_RHS_LU !< Right-hand-side vector for LU [-] + REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: DampedVelocities !< Velocity vector for applying modal damping [-] + REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: ModalDampingF !< Modal damping force in the modal damping matrix coordinates [-] INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: LP_indx !< Index vector for LU [-] TYPE(BD_InputType) :: u !< Inputs converted to the internal BD coordinate system [-] TYPE(ModJacType) :: Jac !< Jacobian matrices and arrays corresponding to module variables [-] @@ -2920,6 +2922,30 @@ subroutine BD_CopyMisc(SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg) end if DstMiscData%LP_RHS_LU = SrcMiscData%LP_RHS_LU end if + if (allocated(SrcMiscData%DampedVelocities)) then + LB(1:1) = lbound(SrcMiscData%DampedVelocities) + UB(1:1) = ubound(SrcMiscData%DampedVelocities) + if (.not. allocated(DstMiscData%DampedVelocities)) then + allocate(DstMiscData%DampedVelocities(LB(1):UB(1)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%DampedVelocities.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstMiscData%DampedVelocities = SrcMiscData%DampedVelocities + end if + if (allocated(SrcMiscData%ModalDampingF)) then + LB(1:1) = lbound(SrcMiscData%ModalDampingF) + UB(1:1) = ubound(SrcMiscData%ModalDampingF) + if (.not. allocated(DstMiscData%ModalDampingF)) then + allocate(DstMiscData%ModalDampingF(LB(1):UB(1)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%ModalDampingF.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstMiscData%ModalDampingF = SrcMiscData%ModalDampingF + end if if (allocated(SrcMiscData%LP_indx)) then LB(1:1) = lbound(SrcMiscData%LP_indx) UB(1:1) = ubound(SrcMiscData%LP_indx) @@ -3062,6 +3088,12 @@ subroutine BD_DestroyMisc(MiscData, ErrStat, ErrMsg) if (allocated(MiscData%LP_RHS_LU)) then deallocate(MiscData%LP_RHS_LU) end if + if (allocated(MiscData%DampedVelocities)) then + deallocate(MiscData%DampedVelocities) + end if + if (allocated(MiscData%ModalDampingF)) then + deallocate(MiscData%ModalDampingF) + end if if (allocated(MiscData%LP_indx)) then deallocate(MiscData%LP_indx) end if @@ -3121,6 +3153,8 @@ subroutine BD_PackMisc(RF, Indata) call RegPackAlloc(RF, InData%LP_RHS) call RegPackAlloc(RF, InData%LP_StifK_LU) call RegPackAlloc(RF, InData%LP_RHS_LU) + call RegPackAlloc(RF, InData%DampedVelocities) + call RegPackAlloc(RF, InData%ModalDampingF) call RegPackAlloc(RF, InData%LP_indx) call BD_PackInput(RF, InData%u) call NWTC_Library_PackModJacType(RF, InData%Jac) @@ -3176,6 +3210,8 @@ subroutine BD_UnPackMisc(RF, OutData) call RegUnpackAlloc(RF, OutData%LP_RHS); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%LP_StifK_LU); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%LP_RHS_LU); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%DampedVelocities); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%ModalDampingF); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%LP_indx); if (RegCheckErr(RF, RoutineName)) return call BD_UnpackInput(RF, OutData%u) ! u call NWTC_Library_UnpackModJacType(RF, OutData%Jac) ! Jac diff --git a/modules/beamdyn/src/Registry_BeamDyn.txt b/modules/beamdyn/src/Registry_BeamDyn.txt index dd10f46d3..aefbaf929 100644 --- a/modules/beamdyn/src/Registry_BeamDyn.txt +++ b/modules/beamdyn/src/Registry_BeamDyn.txt @@ -53,7 +53,7 @@ typedef ^ BladeInputData ^ mass0 {:}{:}{:} - - typedef ^ BladeInputData ^ beta {6} - - "Damping Coefficient" - #end of BDKi-type variables #^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -typedef ^ BladeInputData IntKi damp_flag - - - "Damping Flag: 0-No Damping, 1-Damped" +typedef ^ BladeInputData IntKi damp_flag - - - "Damping Flag: 0-No Damping, 1-Stiffness Prop. Damped, 2-Modal Damping" # ..... Input file data.............................................................................. @@ -346,6 +346,9 @@ typedef ^ MiscVarType ^ LP_MassM_LU {:}{:} - - "M typedef ^ MiscVarType ^ LP_RHS {:} - - "Right-hand-side vector" - typedef ^ MiscVarType ^ LP_StifK_LU {:}{:} - - "Stiffness Matrix for LU" - typedef ^ MiscVarType ^ LP_RHS_LU {:} - - "Right-hand-side vector for LU" - +# Velocity array for modal damping calculation +typedef ^ MiscVarType ^ DampedVelocities {:} - - "Velocity vector for applying modal damping" - +typedef ^ MiscVarType ^ ModalDampingF {:} - - "Modal damping force in the modal damping matrix coordinates" - #end of BDKi-type variables #^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ typedef ^ MiscVarType IntKi LP_indx {:} - - "Index vector for LU" - From 35cd7618d7d4b9f492fc8458e811989155d1625c Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Tue, 30 Dec 2025 15:26:41 -0700 Subject: [PATCH 05/24] Updated modal damping to remove rigid body rotation velocity and have rotation over time step. Need to test both. --- modules/beamdyn/src/BeamDyn.f90 | 53 ++++++++++++++++-------- modules/beamdyn/src/BeamDyn_Types.f90 | 4 ++ modules/beamdyn/src/Registry_BeamDyn.txt | 1 + 3 files changed, 40 insertions(+), 18 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 0edb4d5bd..d88bffcdd 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -5922,44 +5922,61 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) TYPE(BD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states TYPE(BD_OtherStateType), INTENT(IN ) :: OtherState !< other states (contains ref orientation) TYPE(BD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables - INTEGER(IntKi) :: j ! looping indexing variable + + INTEGER(IntKi) :: j ! looping indexing variable (node number) + REAL(R8Ki) :: r(3) ! nodal position relative to root + INTEGER(IntKi) :: elem ! looping indexing for element number + INTEGER(IntKi) :: elem_node ! looping indexing for node in the element number + + ! 1. Velocities relative to root + ! TODO : Is x%dqdt(:, 0) correct for root velocities? Or should I use u%RootMotion%... variables do j = 1, size(x%dqdt, 2)-1 - ! TODO : Is this the correct place to get the velocities? - ! m%DampedVelocities((j-1)*6+1:j*6) = x%dqdt(:, j+1) - x%dqdt(:, 0) + ! 1.a. Velocities minus root velocity + m%DampedVelocities((j-1)*6+1:j*6) = x%dqdt(:, j+1) - x%dqdt(:, 0) + + ! 1.b. Subtract out the rigid body rotation of the root here + ! position cross rotational velocity + elem = j / (p%nodes_per_elem - 1) + 1 + elem_node = j - (elem-1) * (p%nodes_per_elem - 1) - ! TODO : Also do a rotation here? - m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(OtherState%GlbRot, x%dqdt(1:3, j+1) - x%dqdt(1:3, 1)) - m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(OtherState%GlbRot, x%dqdt(4:6, j+1) - x%dqdt(4:6, 1)) + ! Vector from root to node (inertial frame) + r = p%uuN0(1:3, elem_node, elem) + x%q(1:3, j) + m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = m%DampedVelocities((j-1)*6+1:(j-1)*6+3) & + - Cross_Product(x%dqdt(4:6, 0), r) end do - ! 2. Velocities represented in rotating frame + ! 2. rotate velocities by matmul(u%RootMotion%Orientation, OtherState%GlbRot) + ! Solve is done at the coordinate system based on time n for states at time n+1 + ! The damping matrix is technically defined for the n+1 coordinate system since that's where accel is evaluated. + ! Therefore, need to resolve the coordinate differences. + ! + ! OtherState%GlbRot = tranpose(u%RootMotion%Orientation(:, :, 1) evaluated at n) + ! here, u%RootMotion%Orientation(:, :, 1) is evaluated at n+1 + m%ModalDampingRot = matmul(u%RootMotion%Orientation(:, :, 1), OtherState%GlbRot) - ! OtherState%GlbRot - will use to get rotation into correct coordinate system - ! Also might be relevant - u%RootMotion%... + do j = 1, size(x%dqdt, 2)-1 + m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(m%ModalDampingRot, x%dqdt(1:3, j+1) - x%dqdt(1:3, 1)) + m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(m%ModalDampingRot, x%dqdt(4:6, j+1) - x%dqdt(4:6, 1)) + end do ! 3. Multiply by modal damping matrix m%ModalDampingF = matmul(p%ModalDampingMat, m%DampedVelocities) - ! 4. Rotate to correct coordinates and add to m%LP_RHS_LU - - ! TODO: Add or subtract to m%LP_RHS_LU? - ! m%LP_RHS_LU = m%LP_RHS_LU - m%ModalDampingF - + ! 4. Rotate to correct coordinates and subtract from m%LP_RHS_LU do j = 1, size(x%dqdt, 2)-1 m%LP_RHS_LU((j-1)*6+1:(j-1)*6+3) = m%LP_RHS_LU((j-1)*6+1:(j-1)*6+3) & - - matmul(transpose(OtherState%GlbRot), m%ModalDampingF((j-1)*6+1:(j-1)*6+3)) + - matmul(transpose(m%ModalDampingRot), m%ModalDampingF((j-1)*6+1:(j-1)*6+3)) m%LP_RHS_LU((j-1)*6+4:(j-1)*6+6) = m%LP_RHS_LU((j-1)*6+4:(j-1)*6+6) & - - matmul(transpose(OtherState%GlbRot), m%ModalDampingF((j-1)*6+4:(j-1)*6+6)) + - matmul(transpose(m%ModalDampingRot), m%ModalDampingF((j-1)*6+4:(j-1)*6+6)) end do - print *, 'TODO: Add modal damping force to the RHS vector here.' - print *, 'TODO: Should force be added or subtracted here? Should be clear in testing...' + print *, 'End of damping force calculation.' END SUBROUTINE BD_AddModalDampingRHS diff --git a/modules/beamdyn/src/BeamDyn_Types.f90 b/modules/beamdyn/src/BeamDyn_Types.f90 index 7c9d978b5..95154ca03 100644 --- a/modules/beamdyn/src/BeamDyn_Types.f90 +++ b/modules/beamdyn/src/BeamDyn_Types.f90 @@ -308,6 +308,7 @@ MODULE BeamDyn_Types REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: LP_RHS_LU !< Right-hand-side vector for LU [-] REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: DampedVelocities !< Velocity vector for applying modal damping [-] REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: ModalDampingF !< Modal damping force in the modal damping matrix coordinates [-] + REAL(R8Ki) , DIMENSION(1:3,1:3) :: ModalDampingRot = 0.0_R8Ki !< Rotation over the current time step for modal damping [-] INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: LP_indx !< Index vector for LU [-] TYPE(BD_InputType) :: u !< Inputs converted to the internal BD coordinate system [-] TYPE(ModJacType) :: Jac !< Jacobian matrices and arrays corresponding to module variables [-] @@ -2946,6 +2947,7 @@ subroutine BD_CopyMisc(SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg) end if DstMiscData%ModalDampingF = SrcMiscData%ModalDampingF end if + DstMiscData%ModalDampingRot = SrcMiscData%ModalDampingRot if (allocated(SrcMiscData%LP_indx)) then LB(1:1) = lbound(SrcMiscData%LP_indx) UB(1:1) = ubound(SrcMiscData%LP_indx) @@ -3155,6 +3157,7 @@ subroutine BD_PackMisc(RF, Indata) call RegPackAlloc(RF, InData%LP_RHS_LU) call RegPackAlloc(RF, InData%DampedVelocities) call RegPackAlloc(RF, InData%ModalDampingF) + call RegPack(RF, InData%ModalDampingRot) call RegPackAlloc(RF, InData%LP_indx) call BD_PackInput(RF, InData%u) call NWTC_Library_PackModJacType(RF, InData%Jac) @@ -3212,6 +3215,7 @@ subroutine BD_UnPackMisc(RF, OutData) call RegUnpackAlloc(RF, OutData%LP_RHS_LU); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%DampedVelocities); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%ModalDampingF); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%ModalDampingRot); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%LP_indx); if (RegCheckErr(RF, RoutineName)) return call BD_UnpackInput(RF, OutData%u) ! u call NWTC_Library_UnpackModJacType(RF, OutData%Jac) ! Jac diff --git a/modules/beamdyn/src/Registry_BeamDyn.txt b/modules/beamdyn/src/Registry_BeamDyn.txt index aefbaf929..d392d4621 100644 --- a/modules/beamdyn/src/Registry_BeamDyn.txt +++ b/modules/beamdyn/src/Registry_BeamDyn.txt @@ -349,6 +349,7 @@ typedef ^ MiscVarType ^ LP_RHS_LU {:} - - "R # Velocity array for modal damping calculation typedef ^ MiscVarType ^ DampedVelocities {:} - - "Velocity vector for applying modal damping" - typedef ^ MiscVarType ^ ModalDampingF {:} - - "Modal damping force in the modal damping matrix coordinates" - +typedef ^ MiscVarType ^ ModalDampingRot {3}{3} - - "Rotation over the current time step for modal damping" - #end of BDKi-type variables #^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ typedef ^ MiscVarType IntKi LP_indx {:} - - "Index vector for LU" - From 33b68ca7d373c1c806bfb684c83e096e22da07c1 Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Wed, 31 Dec 2025 14:33:55 -0700 Subject: [PATCH 06/24] Fixing index issues for modal damping transforms. --- modules/beamdyn/src/BeamDyn.f90 | 39 ++++++++++++++++++++++----------- 1 file changed, 26 insertions(+), 13 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index d88bffcdd..764ab369c 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -5932,21 +5932,34 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) ! 1. Velocities relative to root ! TODO : Is x%dqdt(:, 0) correct for root velocities? Or should I use u%RootMotion%... variables - do j = 1, size(x%dqdt, 2)-1 - ! 1.a. Velocities minus root velocity - m%DampedVelocities((j-1)*6+1:j*6) = x%dqdt(:, j+1) - x%dqdt(:, 0) + ! element loops + do elem = 1, p%elem_total + + do elem_node = 1, p%nodes_per_elem + + ! Global node index, excluding root + j = (elem - 1) * (p%nodes_per_elem - 1) + elem_node - 1 + + if (j > 0) then + + ! 1.a. Velocities minus root velocity + ! x%... is at j+1 because skipping the root node and j is 1 at the first node after root. + m%DampedVelocities((j-1)*6+1:j*6) = x%dqdt(:, j+1) - x%dqdt(:, 1) - ! 1.b. Subtract out the rigid body rotation of the root here - ! position cross rotational velocity - elem = j / (p%nodes_per_elem - 1) + 1 - elem_node = j - (elem-1) * (p%nodes_per_elem - 1) + ! 1.b. Subtract out the rigid body rotation of the root here + ! position cross rotational velocity - ! Vector from root to node (inertial frame) - r = p%uuN0(1:3, elem_node, elem) + x%q(1:3, j) + ! Vector from root to node (inertial frame) + r = p%uuN0(1:3, elem_node, elem) + x%q(1:3, j+1) + + m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = m%DampedVelocities((j-1)*6+1:(j-1)*6+3) & + - Cross_Product(x%dqdt(4:6, 1), r) + end if + + + end do - m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = m%DampedVelocities((j-1)*6+1:(j-1)*6+3) & - - Cross_Product(x%dqdt(4:6, 0), r) end do ! 2. rotate velocities by matmul(u%RootMotion%Orientation, OtherState%GlbRot) @@ -5959,8 +5972,8 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) m%ModalDampingRot = matmul(u%RootMotion%Orientation(:, :, 1), OtherState%GlbRot) do j = 1, size(x%dqdt, 2)-1 - m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(m%ModalDampingRot, x%dqdt(1:3, j+1) - x%dqdt(1:3, 1)) - m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(m%ModalDampingRot, x%dqdt(4:6, j+1) - x%dqdt(4:6, 1)) + m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(m%ModalDampingRot, x%dqdt(1:3, j+1)) + m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(m%ModalDampingRot, x%dqdt(4:6, j+1)) end do ! 3. Multiply by modal damping matrix From 1317aeadd827f227062546d34dd5f624f79f402a Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Fri, 2 Jan 2026 08:21:49 -0700 Subject: [PATCH 07/24] Fixed which velocity to rotate and set rotation matrix to Identity since previous was really wrong. --- modules/beamdyn/src/BeamDyn.f90 | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 764ab369c..5e9bcd054 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -1830,6 +1830,8 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) ! TODO : Take actual user input for zeta ! zeta is fraction of critical damping. zeta = (/ 0.001d0, 0.003d0, 0.0015d0, 0.0045d0, 0.002d0, 0.006d0, 0.007d0, 0.008d0, 0.009d0, 0.010d0 /) + ! zeta = (/ 0.1d0, 0.3d0, 0.15d0, 0.45d0, 0.2d0, 0.6d0, 0.7d0, 0.8d0, 0.9d0, 1.0d0 /) + ! zeta = (/ 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0 /) numZeta = size(zeta, 1) ! 0. Setup quadrature points @@ -1898,6 +1900,9 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) p%ModalDampingMat = matmul(transpose(phiT_M), p%ModalDampingMat) + print *, 'Frequencies at modal damping init [Hz]' + print *, omega / 2.0d0 / 3.1415926535d0 + ! Debugging / verifying - recover the zeta values ! phi0T_M_phi0 = matmul(transpose(eigenvectors), matmul(m%LP_MassM_LU, eigenvectors)) ! print *, 'Verifying mass orthonormal here.' @@ -5969,11 +5974,18 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) ! ! OtherState%GlbRot = tranpose(u%RootMotion%Orientation(:, :, 1) evaluated at n) ! here, u%RootMotion%Orientation(:, :, 1) is evaluated at n+1 - m%ModalDampingRot = matmul(u%RootMotion%Orientation(:, :, 1), OtherState%GlbRot) + + ! m%ModalDampingRot = matmul(u%RootMotion%Orientation(:, :, 1), OtherState%GlbRot) + + ! TODO : Use an actual rotation matrix here for the step, the option above is wrong and + ! diverges from identity instead of just being slightly different at all times. + m%ModalDampingRot = reshape((/ 1.0d0, 0.0d0, 0.0d0, & + 0.0d0, 1.0d0, 0.0d0, & + 0.0d0, 0.0d0, 1.0d0 /), shape(m%ModalDampingRot)) do j = 1, size(x%dqdt, 2)-1 - m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(m%ModalDampingRot, x%dqdt(1:3, j+1)) - m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(m%ModalDampingRot, x%dqdt(4:6, j+1)) + m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(m%ModalDampingRot, m%DampedVelocities((j-1)*6+1:(j-1)*6+3)) + m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(m%ModalDampingRot, m%DampedVelocities((j-1)*6+4:(j-1)*6+6)) end do ! 3. Multiply by modal damping matrix @@ -5989,7 +6001,7 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) end do - print *, 'End of damping force calculation.' + ! print *, 'End of damping force calculation.' END SUBROUTINE BD_AddModalDampingRHS From 2fdb9cbd52871fff860962ed66f1b2107e1f3716 Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Fri, 2 Jan 2026 10:44:39 -0700 Subject: [PATCH 08/24] Corrected the rotation on the time step to be in root coordinates. Rotated test looks good now. --- modules/beamdyn/src/BeamDyn.f90 | 22 ++++++++-------------- modules/beamdyn/src/BeamDyn_Types.f90 | 4 ---- modules/beamdyn/src/Registry_BeamDyn.txt | 1 - 3 files changed, 8 insertions(+), 19 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 5e9bcd054..4a01458d2 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -5929,10 +5929,10 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) TYPE(BD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables INTEGER(IntKi) :: j ! looping indexing variable (node number) - REAL(R8Ki) :: r(3) ! nodal position relative to root INTEGER(IntKi) :: elem ! looping indexing for element number INTEGER(IntKi) :: elem_node ! looping indexing for node in the element number - + REAL(R8Ki) :: r(3) ! nodal position relative to root + real(R8Ki) :: ModalDampingRot(3, 3) ! 1. Velocities relative to root @@ -5973,19 +5973,13 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) ! Therefore, need to resolve the coordinate differences. ! ! OtherState%GlbRot = tranpose(u%RootMotion%Orientation(:, :, 1) evaluated at n) - ! here, u%RootMotion%Orientation(:, :, 1) is evaluated at n+1 - - ! m%ModalDampingRot = matmul(u%RootMotion%Orientation(:, :, 1), OtherState%GlbRot) + ! here, u%RootMotion%Orientation(:, :, 1) is evaluated at n+1, but is tranposed at this point - ! TODO : Use an actual rotation matrix here for the step, the option above is wrong and - ! diverges from identity instead of just being slightly different at all times. - m%ModalDampingRot = reshape((/ 1.0d0, 0.0d0, 0.0d0, & - 0.0d0, 1.0d0, 0.0d0, & - 0.0d0, 0.0d0, 1.0d0 /), shape(m%ModalDampingRot)) + ModalDampingRot = matmul(transpose(u%RootMotion%Orientation(:, :, 1)), OtherState%GlbRot) do j = 1, size(x%dqdt, 2)-1 - m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(m%ModalDampingRot, m%DampedVelocities((j-1)*6+1:(j-1)*6+3)) - m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(m%ModalDampingRot, m%DampedVelocities((j-1)*6+4:(j-1)*6+6)) + m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(ModalDampingRot, m%DampedVelocities((j-1)*6+1:(j-1)*6+3)) + m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(ModalDampingRot, m%DampedVelocities((j-1)*6+4:(j-1)*6+6)) end do ! 3. Multiply by modal damping matrix @@ -5994,10 +5988,10 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) ! 4. Rotate to correct coordinates and subtract from m%LP_RHS_LU do j = 1, size(x%dqdt, 2)-1 m%LP_RHS_LU((j-1)*6+1:(j-1)*6+3) = m%LP_RHS_LU((j-1)*6+1:(j-1)*6+3) & - - matmul(transpose(m%ModalDampingRot), m%ModalDampingF((j-1)*6+1:(j-1)*6+3)) + - matmul(transpose(ModalDampingRot), m%ModalDampingF((j-1)*6+1:(j-1)*6+3)) m%LP_RHS_LU((j-1)*6+4:(j-1)*6+6) = m%LP_RHS_LU((j-1)*6+4:(j-1)*6+6) & - - matmul(transpose(m%ModalDampingRot), m%ModalDampingF((j-1)*6+4:(j-1)*6+6)) + - matmul(transpose(ModalDampingRot), m%ModalDampingF((j-1)*6+4:(j-1)*6+6)) end do diff --git a/modules/beamdyn/src/BeamDyn_Types.f90 b/modules/beamdyn/src/BeamDyn_Types.f90 index 95154ca03..7c9d978b5 100644 --- a/modules/beamdyn/src/BeamDyn_Types.f90 +++ b/modules/beamdyn/src/BeamDyn_Types.f90 @@ -308,7 +308,6 @@ MODULE BeamDyn_Types REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: LP_RHS_LU !< Right-hand-side vector for LU [-] REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: DampedVelocities !< Velocity vector for applying modal damping [-] REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: ModalDampingF !< Modal damping force in the modal damping matrix coordinates [-] - REAL(R8Ki) , DIMENSION(1:3,1:3) :: ModalDampingRot = 0.0_R8Ki !< Rotation over the current time step for modal damping [-] INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: LP_indx !< Index vector for LU [-] TYPE(BD_InputType) :: u !< Inputs converted to the internal BD coordinate system [-] TYPE(ModJacType) :: Jac !< Jacobian matrices and arrays corresponding to module variables [-] @@ -2947,7 +2946,6 @@ subroutine BD_CopyMisc(SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg) end if DstMiscData%ModalDampingF = SrcMiscData%ModalDampingF end if - DstMiscData%ModalDampingRot = SrcMiscData%ModalDampingRot if (allocated(SrcMiscData%LP_indx)) then LB(1:1) = lbound(SrcMiscData%LP_indx) UB(1:1) = ubound(SrcMiscData%LP_indx) @@ -3157,7 +3155,6 @@ subroutine BD_PackMisc(RF, Indata) call RegPackAlloc(RF, InData%LP_RHS_LU) call RegPackAlloc(RF, InData%DampedVelocities) call RegPackAlloc(RF, InData%ModalDampingF) - call RegPack(RF, InData%ModalDampingRot) call RegPackAlloc(RF, InData%LP_indx) call BD_PackInput(RF, InData%u) call NWTC_Library_PackModJacType(RF, InData%Jac) @@ -3215,7 +3212,6 @@ subroutine BD_UnPackMisc(RF, OutData) call RegUnpackAlloc(RF, OutData%LP_RHS_LU); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%DampedVelocities); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%ModalDampingF); if (RegCheckErr(RF, RoutineName)) return - call RegUnpack(RF, OutData%ModalDampingRot); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%LP_indx); if (RegCheckErr(RF, RoutineName)) return call BD_UnpackInput(RF, OutData%u) ! u call NWTC_Library_UnpackModJacType(RF, OutData%Jac) ! Jac diff --git a/modules/beamdyn/src/Registry_BeamDyn.txt b/modules/beamdyn/src/Registry_BeamDyn.txt index d392d4621..aefbaf929 100644 --- a/modules/beamdyn/src/Registry_BeamDyn.txt +++ b/modules/beamdyn/src/Registry_BeamDyn.txt @@ -349,7 +349,6 @@ typedef ^ MiscVarType ^ LP_RHS_LU {:} - - "R # Velocity array for modal damping calculation typedef ^ MiscVarType ^ DampedVelocities {:} - - "Velocity vector for applying modal damping" - typedef ^ MiscVarType ^ ModalDampingF {:} - - "Modal damping force in the modal damping matrix coordinates" - -typedef ^ MiscVarType ^ ModalDampingRot {3}{3} - - "Rotation over the current time step for modal damping" - #end of BDKi-type variables #^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ typedef ^ MiscVarType IntKi LP_indx {:} - - "Index vector for LU" - From 669c3e13a130e79115be2ac98767ae0a3c3a2f67 Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Fri, 2 Jan 2026 12:11:38 -0700 Subject: [PATCH 09/24] Added modal damping linearization tests for simple beams. --- reg_tests/CTestList.cmake | 3 +++ reg_tests/executeOpenfastLinearRegressionCase.py | 2 +- reg_tests/r-test | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/reg_tests/CTestList.cmake b/reg_tests/CTestList.cmake index 3f8333e8b..b825d6aa8 100644 --- a/reg_tests/CTestList.cmake +++ b/reg_tests/CTestList.cmake @@ -391,6 +391,9 @@ of_regression_linear("Fake5MW_AeroLin_B3_UA6" "-highpass=0.05" "openfas of_regression_linear("WP_Stationary_Linear" "" "openfast;linear;elastodyn") of_regression_linear("Ideal_Beam_Fixed_Free_Linear" "-highpass=0.10" "openfast;linear;beamdyn") of_regression_linear("Ideal_Beam_Free_Free_Linear" "-highpass=0.10" "openfast;linear;beamdyn") +of_regression_linear("Damped_Beam_Fixed" "-highpass=0.10" "openfast;linear;beamdyn") +of_regression_linear("Damped_Beam_Rotating" "-highpass=0.10" "openfast;linear;beamdyn") +of_regression_linear("Damped_Beam_Rotated" "-highpass=0.10" "openfast;linear;beamdyn") of_regression_linear("5MW_Land_Linear_Aero" "-highpass=0.25" "openfast;linear;elastodyn;servodyn;aerodyn") of_regression_linear("5MW_Land_Linear_Aero_CalcSteady" "-highpass=0.25" "openfast;linear;elastodyn;servodyn;aerodyn") of_regression_linear("5MW_Land_BD_Linear" "" "openfast;linear;beamdyn;servodyn") diff --git a/reg_tests/executeOpenfastLinearRegressionCase.py b/reg_tests/executeOpenfastLinearRegressionCase.py index 4af4fdf08..4f06234f9 100644 --- a/reg_tests/executeOpenfastLinearRegressionCase.py +++ b/reg_tests/executeOpenfastLinearRegressionCase.py @@ -139,7 +139,7 @@ def indent(msg, sindent='\t'): # create the local output directory if it does not already exist # and initialize it with input files for all test cases -for data in ["Ideal_Beam", "WP_Baseline"]: +for data in ["Ideal_Beam", "WP_Baseline", "Damped_Beam"]: dataDir = os.path.join(buildDirectory, data) if not os.path.isdir(dataDir): rtl.copyTree(os.path.join(moduleDirectory, data), dataDir, excludeExt=excludeExt) diff --git a/reg_tests/r-test b/reg_tests/r-test index ab6571888..cbca2b847 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit ab657188872b0f701771c423346c344a676aa265 +Subproject commit cbca2b84717dcf1017f6d867e4568eae1c28b50a From 415cf77d98d4ca0c2748a8a0c83a8cc1b6d45550 Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Tue, 6 Jan 2026 14:17:07 -0700 Subject: [PATCH 10/24] Adding output file of bd modal analysis frequencies and participation factors. --- modules/beamdyn/src/BeamDyn.f90 | 55 +++++++++++++++++++++++++++++++-- 1 file changed, 53 insertions(+), 2 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 4a01458d2..5f29f1b84 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -1817,7 +1817,11 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) REAL(R8Ki), ALLOCATABLE :: omega (:) ! modal frequencies (rad/s) REAL(LaKi), ALLOCATABLE :: phiT_M (:, :) ! mode shapes transpose times mass matrix REAL(LaKi), ALLOCATABLE :: phi0T_M_phi0 (:, :) ! normalization calculation of mass matrix - REAL, DIMENSION(10) :: zeta ! Modal damping values, should be changed to be user input. + REAL, DIMENSION(40) :: zeta ! Modal damping values, should be changed to be user input. + + REAL(LaKi), ALLOCATABLE :: modal_participation (:, :) ! Modal participation factor + INTEGER(IntKi) :: bdModesFile ! Unit numbers for file with BD modes + INTEGER(IntKi) :: ErrStat2 ! Temporary Error status CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary Error message @@ -1829,9 +1833,21 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) ! TODO : Take actual user input for zeta ! zeta is fraction of critical damping. - zeta = (/ 0.001d0, 0.003d0, 0.0015d0, 0.0045d0, 0.002d0, 0.006d0, 0.007d0, 0.008d0, 0.009d0, 0.010d0 /) ! zeta = (/ 0.1d0, 0.3d0, 0.15d0, 0.45d0, 0.2d0, 0.6d0, 0.7d0, 0.8d0, 0.9d0, 1.0d0 /) ! zeta = (/ 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0, 0.0d0 /) + + ! ! Zeta for Damped_Beam tests + ! zeta = (/ 0.001d0, 0.003d0, 0.0015d0, 0.0045d0, 0.002d0, 0.006d0, 0.007d0, 0.008d0, 0.009d0, 0.010d0 /) + + ! Zeta for IEA 22 test + zeta = (/ 0.00481431, 0.00501452, 0.01319441, 0.01365751, 0.02704056, 0.02928076, & + 0.03943607, 0.0133448, 0.05628846, 0.05935411, 0.01490789, 0.08727722, & + 0.08508602, 0.01754962, 0.12212305, 0.12607313, 0.02324255, 0.03571866, & + 0.13962916, 0.00848232, 0.16332203, 0.0405824, 0.28314572, 0.05114984, & + 0.40216838, 0.3507376, 0.022072, 0.0540725, 0.06678422, 0.03625773, & + 0.0484979, 0.8516855, 0.98869347, 0.05778878, 0.06610646, 0.0764022, & + 0.08499995, 0.09210838, 0.13037341, 0.13997288 /) + numZeta = size(zeta, 1) ! 0. Setup quadrature points @@ -1911,6 +1927,8 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) ! phi0T_M_phi0(j,j) = phi0T_M_phi0(j,j) / omega(j) / 2.0d0 ! end do + CALL CalcModalParticipation() + ! Allocate memory for the velocity vector that will be multiplied by the modal damping matrix CALL AllocAry(m%DampedVelocities, p%dof_total-6, 'DampedVelocities', ErrStat2, ErrMsg2) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') @@ -1932,6 +1950,39 @@ SUBROUTINE CleanupModalInit() IF (ALLOCATED(phiT_M) ) DEALLOCATE(phiT_M) END SUBROUTINE CleanupModalInit + SUBROUTINE CalcModalParticipation() + + ! Theory based on Abaqus documentation + ! Only using rotational DOFs for rotations and not including + ! contributions from translations + + CALL AllocAry(modal_participation, nDOF, 6, 'modal_participation', ErrStat2, ErrMsg2) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') + + do j = 1, 6 + modal_participation(:, j) = sum(phiT_M(:, j::6)*phiT_M(:, j::6), dim=2) + end do + + ! Write to a file instead + CALL GetNewUnit( bdModesFile ) + open(unit=bdModesFile, file='beamdyn_modes.csv') + + write(bdModesFile,*) '#Frequency [Hz], Zeta [Frac. Critical], & + &Participation X, Participation Y, Participation Z, & + &Participation RX, Participation RY, Participation RZ' + + ! Write to a file + do j = 1, numZeta + write(bdModesFile, ' (1F12.4,1F12.8,6E14.5) ') omega(j)/6.28318530d0, & + zeta(j), modal_participation(j, :) / sum(modal_participation(j, :)) + end do + + close(bdModesFile) + + IF (ALLOCATED(modal_participation) ) DEALLOCATE(modal_participation) + + END SUBROUTINE + END SUBROUTINE !> Wrapper function for eigen value analyses From f1529eba521292df06bdd2a455a6babe4c93dd98 Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Wed, 7 Jan 2026 13:58:51 -0700 Subject: [PATCH 11/24] Switching to alternative coordinate transforms and now is consistent for IEA 22 MW example with mu damping. --- modules/beamdyn/src/BeamDyn.f90 | 53 ++++++++++++++++++++++++++------- 1 file changed, 43 insertions(+), 10 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 5f29f1b84..bb113b9e3 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -1798,7 +1798,6 @@ END SUBROUTINE Init_ContinuousStates !> This routine initializes modal damping. SUBROUTINE Init_ModalDamping(x, OtherState, p, m) - ! TODO : Look at what should be INOUT v. IN ! TODO : Error messages are probably wrong in some way. TYPE(BD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at t on input at t + dt on output @@ -1821,7 +1820,7 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) REAL(LaKi), ALLOCATABLE :: modal_participation (:, :) ! Modal participation factor INTEGER(IntKi) :: bdModesFile ! Unit numbers for file with BD modes - + real(R8Ki) :: NodeRot(3, 3) INTEGER(IntKi) :: ErrStat2 ! Temporary Error status CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary Error message @@ -1916,8 +1915,32 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) p%ModalDampingMat = matmul(transpose(phiT_M), p%ModalDampingMat) - print *, 'Frequencies at modal damping init [Hz]' - print *, omega / 2.0d0 / 3.1415926535d0 + ! Apply the rotation of q here. When the actual dynamics are at the same position + ! as this, then this cancels with a rotation applied at the modal damping force. + do j = 1, size(x%dqdt, 2)-1 + + ! Loop over the nodes that apply to the damping matrix, so don't include the root node. + call BD_CrvMatrixR(x%q(4:6, j+1), NodeRot) + + p%ModalDampingMat(:, (j-1)*6+1:(j-1)*6+3) = matmul(p%ModalDampingMat(:, (j-1)*6+1:(j-1)*6+3), & + NodeRot) + + p%ModalDampingMat(:, (j-1)*6+4:(j-1)*6+6) = matmul(p%ModalDampingMat(:, (j-1)*6+4:(j-1)*6+6), & + NodeRot) + + end do + do j = 1, size(x%dqdt, 2)-1 + + ! Loop over the nodes that apply to the damping matrix, so don't include the root node. + call BD_CrvMatrixR(x%q(4:6, j+1), NodeRot) + + p%ModalDampingMat((j-1)*6+1:(j-1)*6+3, :) = matmul(transpose(NodeRot), & + p%ModalDampingMat((j-1)*6+1:(j-1)*6+3, :)) + + p%ModalDampingMat((j-1)*6+4:(j-1)*6+6, :) = matmul(transpose(NodeRot), & + p%ModalDampingMat((j-1)*6+4:(j-1)*6+6, :)) + + end do ! Debugging / verifying - recover the zeta values ! phi0T_M_phi0 = matmul(transpose(eigenvectors), matmul(m%LP_MassM_LU, eigenvectors)) @@ -5984,11 +6007,10 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) INTEGER(IntKi) :: elem_node ! looping indexing for node in the element number REAL(R8Ki) :: r(3) ! nodal position relative to root real(R8Ki) :: ModalDampingRot(3, 3) + real(R8Ki) :: NodeRot(3, 3) ! 1. Velocities relative to root - ! TODO : Is x%dqdt(:, 0) correct for root velocities? Or should I use u%RootMotion%... variables - ! element loops do elem = 1, p%elem_total @@ -6029,8 +6051,14 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) ModalDampingRot = matmul(transpose(u%RootMotion%Orientation(:, :, 1)), OtherState%GlbRot) do j = 1, size(x%dqdt, 2)-1 - m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(ModalDampingRot, m%DampedVelocities((j-1)*6+1:(j-1)*6+3)) - m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(ModalDampingRot, m%DampedVelocities((j-1)*6+4:(j-1)*6+6)) + + ! Loop over the nodes that apply to the damping matrix, so don't include the root node. + call BD_CrvMatrixR(x%q(4:6, j+1), NodeRot) + + m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(matmul(transpose(NodeRot), ModalDampingRot), & + m%DampedVelocities((j-1)*6+1:(j-1)*6+3)) + m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(matmul(transpose(NodeRot), ModalDampingRot), & + m%DampedVelocities((j-1)*6+4:(j-1)*6+6)) end do ! 3. Multiply by modal damping matrix @@ -6038,11 +6066,16 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) ! 4. Rotate to correct coordinates and subtract from m%LP_RHS_LU do j = 1, size(x%dqdt, 2)-1 + + call BD_CrvMatrixR(x%q(4:6, j+1), NodeRot) + m%LP_RHS_LU((j-1)*6+1:(j-1)*6+3) = m%LP_RHS_LU((j-1)*6+1:(j-1)*6+3) & - - matmul(transpose(ModalDampingRot), m%ModalDampingF((j-1)*6+1:(j-1)*6+3)) + - matmul(matmul(transpose(ModalDampingRot), NodeRot), & + m%ModalDampingF((j-1)*6+1:(j-1)*6+3)) m%LP_RHS_LU((j-1)*6+4:(j-1)*6+6) = m%LP_RHS_LU((j-1)*6+4:(j-1)*6+6) & - - matmul(transpose(ModalDampingRot), m%ModalDampingF((j-1)*6+4:(j-1)*6+6)) + - matmul(matmul(transpose(ModalDampingRot), NodeRot), & + m%ModalDampingF((j-1)*6+4:(j-1)*6+6)) end do From 4a82a09505f3c0fe70b1ba0bd2fabc0c0c2caf19 Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Thu, 8 Jan 2026 15:28:50 +0000 Subject: [PATCH 12/24] Add modal damping inputs to BeamDyn blade files --- docs/source/user/api_change.rst | 3 + modules/beamdyn/src/BeamDyn_IO.f90 | 72 +++++++++--------------- modules/beamdyn/src/BeamDyn_Types.f90 | 22 ++++++++ modules/beamdyn/src/Registry_BeamDyn.txt | 8 ++- reg_tests/r-test | 2 +- 5 files changed, 58 insertions(+), 49 deletions(-) diff --git a/docs/source/user/api_change.rst b/docs/source/user/api_change.rst index 80a973a4e..4f9269b09 100644 --- a/docs/source/user/api_change.rst +++ b/docs/source/user/api_change.rst @@ -62,6 +62,9 @@ ElastoDyn 79 PBrIner(3) 200 ElastoDyn 80 BlPIner(1) 28578 BlPIner(1) - Pitch inertia of an undeflected blade, blade 1 (kg m^2) ElastoDyn 81 BlPIner(2) 28578 BlPIner(2) - Pitch inertia of an undeflected blade, blade 2 (kg m^2) ElastoDyn 82 BlPIner(3) 28578 BlPIner(3) - Pitch inertia of an undeflected blade, blade 3 (kg m^2) [unused for 2 blades] +BeamDyn 10 ------ Modal Damping [used only if damp_type=2] -------------------------------- +BeamDyn 11 n_modes 3 n_modes - Number of modal damping coefficients (-) +BeamDyn 12 zeta 0.1, 0.2, 0.3 zeta - Damping coefficients for mode 1 through n_modes ServoDyn 9 PitNeut(1) 0 PitNeut(1) - Blade 1 neutral pitch position--pitch spring moment is zero at this position *[unused when* **PCMode>0** and **t>=TPCOn** *]* ServoDyn 10 PitNeut(2) 0 PitNeut(2) - Blade 2 neutral pitch position--pitch spring moment is zero at this position *[unused when* **PCMode>0** and **t>=TPCOn** *]* ServoDyn 11 PitNeut(3) 0 PitNeut(3) - Blade 3 neutral pitch position--pitch spring moment is zero at this position *[unused when* **PCMode>0** and **t>=TPCOn** *]* *[unused for 2 blades]* diff --git a/modules/beamdyn/src/BeamDyn_IO.f90 b/modules/beamdyn/src/BeamDyn_IO.f90 index 5adace993..e0322d6bb 100644 --- a/modules/beamdyn/src/BeamDyn_IO.f90 +++ b/modules/beamdyn/src/BeamDyn_IO.f90 @@ -1056,49 +1056,28 @@ SUBROUTINE BD_ReadBladeFile(BldFile,BladeInputFileData,UnEc,ErrStat,ErrMsg) CALL ReadCom(UnIn,BldFile,'unused beam file header line 2',ErrStat2,ErrMsg2,UnEc) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - ! -------------- BLADE PARAMETER----------------------------------------------- - CALL ReadCom(UnIn,BldFile,'beam parameters',ErrStat2,ErrMsg2,UnEc) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - - CALL ReadVar(UnIn,BldFile,BladeInputFileData%station_total,'station_total','Number of blade input stations',ErrStat2,ErrMsg2,UnEc) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - - CALL AllocAry(BladeInputFileData%stiff0,6,6,BladeInputFileData%station_total,'Cross-sectional 6 by 6 stiffness matrix',ErrStat2,ErrMsg2) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - CALL AllocAry(BladeInputFileData%mass0,6,6,BladeInputFileData%station_total,'Cross-sectional 6 by 6 mass matrix',ErrStat2,ErrMsg2) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - CALL AllocAry(BladeInputFileData%station_eta,BladeInputFileData%station_total,'Station eta array',ErrStat2,ErrMsg2) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - - !after allocating these arrays, we'll make sure it was successful: - if (ErrStat >= AbortErrLev) then - call cleanup() - return - end if - -!FIXME: any reason not to use a logical for this? - CALL ReadVar(UnIn,BldFile,BladeInputFileData%damp_flag,'damp_flag','Damping flag',ErrStat2,ErrMsg2,UnEc) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - - ! -------------- DAMPING PARAMETER----------------------------------------------- - CALL ReadCom(UnIn,BldFile,'damping parameters',ErrStat2,ErrMsg2,UnEc) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - CALL ReadCom(UnIn,BldFile,'mu1 to mu6',ErrStat2,ErrMsg2,UnEc) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - CALL ReadCom(UnIn,BldFile,'units',ErrStat2,ErrMsg2,UnEc) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - - CALL ReadAry(UnIn,BldFile,BladeInputFileData%beta,6,'damping coefficient','damping coefficient',ErrStat2,ErrMsg2,UnEc) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - - if (ErrStat >= AbortErrLev) then - call cleanup() - return - end if - - -! -------------- DISTRIBUTED PROPERTIES-------------------------------------------- - CALL ReadCom(UnIn,BldFile,'Distributed properties',ErrStat2,ErrMsg2,UnEc) + ! Blade Parameters ---------------------------------------------------------- + CALL ReadCom(UnIn, BldFile, 'Blade Parameters', ErrStat2, ErrMsg2, UnEc); if (Failed()) return + CALL ReadVar(UnIn, BldFile, BladeInputFileData%station_total,'station_total','Number of blade input stations', ErrStat2, ErrMsg2, UnEc); if (Failed()) return + CALL AllocAry(BladeInputFileData%stiff0, 6, 6, BladeInputFileData%station_total, 'Cross-sectional 6 by 6 stiffness matrix', ErrStat2, ErrMsg2); if (Failed()) return + CALL AllocAry(BladeInputFileData%mass0, 6, 6, BladeInputFileData%station_total, 'Cross-sectional 6 by 6 mass matrix', ErrStat2, ErrMsg2); if (Failed()) return + CALL AllocAry(BladeInputFileData%station_eta, BladeInputFileData%station_total, 'Station eta array', ErrStat2, ErrMsg2); if (Failed()) return + CALL ReadVar(UnIn, BldFile, BladeInputFileData%damp_flag, 'damp_flag', 'Damping type (switch) {0: None, 1: Stiffness-Proportional, 2: Modal}', ErrStat2, ErrMsg2, UnEc); if (Failed()) return + + ! Stiffness Proportional Damping -------------------------------------------- + CALL ReadCom(UnIn, BldFile, 'Stiffness-Proportional Damping Header', ErrStat2, ErrMsg2, UnEc); if (Failed()) return + CALL ReadCom(UnIn, BldFile, 'Mu Table Header 1', ErrStat2, ErrMsg2, UnEc); if (Failed()) return + CALL ReadCom(UnIn, BldFile, 'Mu Table Header 2', ErrStat2, ErrMsg2, UnEc); if (Failed()) return + CALL ReadAry(UnIn, BldFile, BladeInputFileData%beta, 6, 'beta', 'Mu damping coefficients', ErrStat2, ErrMsg2, UnEc); if (Failed()) return + + ! Modal Damping ------------------------------------------------------------- + CALL ReadCom(UnIn, BldFile, 'Modal Damping Header', ErrStat2, ErrMsg2, UnEc); if (Failed()) return + CALL ReadVar(UnIn, BldFile, BladeInputFileData%n_modes, 'n_modes', 'Number of modal damping coefficients (-)', ErrStat2, ErrMsg2, UnEc); if (Failed()) return + CALL AllocAry(BladeInputFileData%zeta, BladeInputFileData%n_modes, 'Modal damping coefficients (zeta)', ErrStat2, ErrMsg2); if (Failed()) return + CALL ReadAry(UnIn, BldFile, BladeInputFileData%zeta, BladeInputFileData%n_modes, 'zeta', 'Damping coefficients for mode 1 through n_modes', ErrStat2, ErrMsg2, UnEc); if (Failed()) return + + ! Distributed Properties ---------------------------------------------------- + CALL ReadCom(UnIn,BldFile,'Distributed Properties',ErrStat2,ErrMsg2,UnEc) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) DO i=1,BladeInputFileData%station_total @@ -1111,7 +1090,7 @@ SUBROUTINE BD_ReadBladeFile(BldFile,BladeInputFileData,UnEc,ErrStat,ErrMsg) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) DO j=1,6 - CALL ReadAry(UnIn,BldFile,temp66(j,:),6,'siffness_matrix','Blade C/S stiffness matrix',ErrStat2,ErrMsg2,UnEc) + CALL ReadAry(UnIn,BldFile,temp66(j,:),6,'stiffness_matrix','Blade C/S stiffness matrix',ErrStat2,ErrMsg2,UnEc) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) ENDDO if (ErrStat >= AbortErrLev) then @@ -1137,7 +1116,10 @@ SUBROUTINE BD_ReadBladeFile(BldFile,BladeInputFileData,UnEc,ErrStat,ErrMsg) return contains - !..................... + logical function Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + Failed = ErrStat >= AbortErrLev + end function subroutine cleanup() close(UnIn) return diff --git a/modules/beamdyn/src/BeamDyn_Types.f90 b/modules/beamdyn/src/BeamDyn_Types.f90 index 7c9d978b5..f488d2cfa 100644 --- a/modules/beamdyn/src/BeamDyn_Types.f90 +++ b/modules/beamdyn/src/BeamDyn_Types.f90 @@ -70,6 +70,8 @@ MODULE BeamDyn_Types REAL(R8Ki) , DIMENSION(:,:,:), ALLOCATABLE :: stiff0 !< C/S stiffness matrix arrays [-] REAL(R8Ki) , DIMENSION(:,:,:), ALLOCATABLE :: mass0 !< C/S mass matrix arrays [-] REAL(R8Ki) , DIMENSION(1:6) :: beta = 0.0_R8Ki !< Damping Coefficient [-] + INTEGER(IntKi) :: n_modes = 0_IntKi !< Number of modal damping coefficients [-] + REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: zeta !< Modal damping coefficient array [-] INTEGER(IntKi) :: damp_flag = 0_IntKi !< Damping Flag: 0-No Damping, 1-Stiffness Prop. Damped, 2-Modal Damping [-] END TYPE BladeInputData ! ======================= @@ -539,6 +541,19 @@ subroutine BD_CopyBladeInputData(SrcBladeInputDataData, DstBladeInputDataData, C DstBladeInputDataData%mass0 = SrcBladeInputDataData%mass0 end if DstBladeInputDataData%beta = SrcBladeInputDataData%beta + DstBladeInputDataData%n_modes = SrcBladeInputDataData%n_modes + if (allocated(SrcBladeInputDataData%zeta)) then + LB(1:1) = lbound(SrcBladeInputDataData%zeta) + UB(1:1) = ubound(SrcBladeInputDataData%zeta) + if (.not. allocated(DstBladeInputDataData%zeta)) then + allocate(DstBladeInputDataData%zeta(LB(1):UB(1)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstBladeInputDataData%zeta.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstBladeInputDataData%zeta = SrcBladeInputDataData%zeta + end if DstBladeInputDataData%damp_flag = SrcBladeInputDataData%damp_flag end subroutine @@ -558,6 +573,9 @@ subroutine BD_DestroyBladeInputData(BladeInputDataData, ErrStat, ErrMsg) if (allocated(BladeInputDataData%mass0)) then deallocate(BladeInputDataData%mass0) end if + if (allocated(BladeInputDataData%zeta)) then + deallocate(BladeInputDataData%zeta) + end if end subroutine subroutine BD_PackBladeInputData(RF, Indata) @@ -571,6 +589,8 @@ subroutine BD_PackBladeInputData(RF, Indata) call RegPackAlloc(RF, InData%stiff0) call RegPackAlloc(RF, InData%mass0) call RegPack(RF, InData%beta) + call RegPack(RF, InData%n_modes) + call RegPackAlloc(RF, InData%zeta) call RegPack(RF, InData%damp_flag) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -589,6 +609,8 @@ subroutine BD_UnPackBladeInputData(RF, OutData) call RegUnpackAlloc(RF, OutData%stiff0); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%mass0); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%beta); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%n_modes); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%zeta); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%damp_flag); if (RegCheckErr(RF, RoutineName)) return end subroutine diff --git a/modules/beamdyn/src/Registry_BeamDyn.txt b/modules/beamdyn/src/Registry_BeamDyn.txt index aefbaf929..5710ea0cb 100644 --- a/modules/beamdyn/src/Registry_BeamDyn.txt +++ b/modules/beamdyn/src/Registry_BeamDyn.txt @@ -48,9 +48,11 @@ typedef ^ BladeInputData IntKi format_index - - - #vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv #the following are BDKi = R8Ki typedef ^ BladeInputData R8Ki station_eta {:} - - "Station location in eta [0,1]" -typedef ^ BladeInputData ^ stiff0 {:}{:}{:} - - "C/S stiffness matrix arrays" -typedef ^ BladeInputData ^ mass0 {:}{:}{:} - - "C/S mass matrix arrays" -typedef ^ BladeInputData ^ beta {6} - - "Damping Coefficient" - +typedef ^ BladeInputData R8Ki stiff0 {:}{:}{:} - - "C/S stiffness matrix arrays" +typedef ^ BladeInputData R8Ki mass0 {:}{:}{:} - - "C/S mass matrix arrays" +typedef ^ BladeInputData R8Ki beta {6} - - "Damping Coefficient" - +typedef ^ BladeInputData IntKi n_modes - - - "Number of modal damping coefficients" - +typedef ^ BladeInputData R8Ki zeta : - - "Modal damping coefficient array" - #end of BDKi-type variables #^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ typedef ^ BladeInputData IntKi damp_flag - - - "Damping Flag: 0-No Damping, 1-Stiffness Prop. Damped, 2-Modal Damping" diff --git a/reg_tests/r-test b/reg_tests/r-test index cbca2b847..4aba0e855 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit cbca2b84717dcf1017f6d867e4568eae1c28b50a +Subproject commit 4aba0e8551c2573bcf30e87a7ed69059df2af432 From 951e62847b872b50f59ee9f400f0ff1f05580e2c Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Thu, 8 Jan 2026 19:49:39 +0000 Subject: [PATCH 13/24] Make BD use modal damping zeta values from input file --- modules/beamdyn/src/BeamDyn.f90 | 96 +++++++++++++----------- modules/beamdyn/src/BeamDyn_Types.f90 | 18 +++++ modules/beamdyn/src/Registry_BeamDyn.txt | 1 + 3 files changed, 70 insertions(+), 45 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index bb113b9e3..62ec9ad86 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -194,7 +194,7 @@ SUBROUTINE BD_Init( InitInp, u, p, x, xd, z, OtherState, y, MiscVar, Interval, I ! Calculation of modal damping here IF(p%damp_flag .EQ. 2) THEN - call Init_ModalDamping(x, OtherState, p, MiscVar) + call Init_ModalDamping(x, OtherState, p, MiscVar, ErrStat2, ErrMsg2); if (Failed()) return ENDIF !................................. @@ -1109,7 +1109,16 @@ subroutine SetParameters(InitInp, InputFileData, p, OtherState, ErrStat, ErrMsg) ! Physical damping flag and 6 damping coefficients !............................................... p%damp_flag = InputFileData%InpBl%damp_flag - p%beta = InputFileData%InpBl%beta + select case (p%damp_flag) + case (0) ! No damping + case (1) ! Stiffness-proportional damping + p%beta = InputFileData%InpBl%beta + case (2) ! Modal damping + p%zeta = InputFileData%InpBl%zeta + case default + call SetErrStat(ErrID_Fatal, "Invalid value for physical damping flag in input file.", ErrStat, ErrMsg, RoutineName ) + return + end select !............................................... ! set parameters for File I/O data: @@ -1796,7 +1805,7 @@ END SUBROUTINE Init_ContinuousStates !----------------------------------------------------------------------------------------------------------------------------------- !> This routine initializes modal damping. -SUBROUTINE Init_ModalDamping(x, OtherState, p, m) +SUBROUTINE Init_ModalDamping(x, OtherState, p, m, ErrStat, ErrMsg) ! TODO : Error messages are probably wrong in some way. @@ -1804,27 +1813,24 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) type(BD_OtherStateType), INTENT(IN ) :: OtherState !< Global rotations are stored in otherstate TYPE(BD_ParameterType), INTENT(INOUT) :: p !< Parameters, output modal damping matrix in original frame here TYPE(BD_MiscVarType), INTENT(INOUT) :: m !< misc/optimization variables + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(ErrMsgLen), INTENT( OUT) :: ErrMsg - ! Local variables + INTEGER(IntKi) :: ErrStat2 ! Temporary Error status + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary Error message INTEGER(IntKi) :: nDOF INTEGER(IntKi) :: j ! looping indexing variable INTEGER(IntKi) :: numZeta ! number of damping values - REAL :: Zj ! diagonal element of the modal damping matrix - INTEGER(IntKi) :: ErrStat - CHARACTER(ErrMsgLen) :: ErrMsg + REAL(R8Ki) :: Zj ! diagonal element of the modal damping matrix REAL(R8Ki), ALLOCATABLE :: eigenvectors (:, :) ! mode shapes REAL(R8Ki), ALLOCATABLE :: omega (:) ! modal frequencies (rad/s) - REAL(LaKi), ALLOCATABLE :: phiT_M (:, :) ! mode shapes transpose times mass matrix - REAL(LaKi), ALLOCATABLE :: phi0T_M_phi0 (:, :) ! normalization calculation of mass matrix - REAL, DIMENSION(40) :: zeta ! Modal damping values, should be changed to be user input. + REAL(R8Ki), ALLOCATABLE :: phiT_M (:, :) ! mode shapes transpose times mass matrix + REAL(R8Ki), ALLOCATABLE :: phi0T_M_phi0 (:, :) ! normalization calculation of mass matrix - REAL(LaKi), ALLOCATABLE :: modal_participation (:, :) ! Modal participation factor + REAL(R8Ki), ALLOCATABLE :: modal_participation (:, :) ! Modal participation factor INTEGER(IntKi) :: bdModesFile ! Unit numbers for file with BD modes real(R8Ki) :: NodeRot(3, 3) - INTEGER(IntKi) :: ErrStat2 ! Temporary Error status - CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary Error message - ErrStat = ErrID_None ErrMsg = '' @@ -1839,15 +1845,15 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) ! zeta = (/ 0.001d0, 0.003d0, 0.0015d0, 0.0045d0, 0.002d0, 0.006d0, 0.007d0, 0.008d0, 0.009d0, 0.010d0 /) ! Zeta for IEA 22 test - zeta = (/ 0.00481431, 0.00501452, 0.01319441, 0.01365751, 0.02704056, 0.02928076, & - 0.03943607, 0.0133448, 0.05628846, 0.05935411, 0.01490789, 0.08727722, & - 0.08508602, 0.01754962, 0.12212305, 0.12607313, 0.02324255, 0.03571866, & - 0.13962916, 0.00848232, 0.16332203, 0.0405824, 0.28314572, 0.05114984, & - 0.40216838, 0.3507376, 0.022072, 0.0540725, 0.06678422, 0.03625773, & - 0.0484979, 0.8516855, 0.98869347, 0.05778878, 0.06610646, 0.0764022, & - 0.08499995, 0.09210838, 0.13037341, 0.13997288 /) + ! zeta = (/ 0.00481431, 0.00501452, 0.01319441, 0.01365751, 0.02704056, 0.02928076, & + ! 0.03943607, 0.0133448, 0.05628846, 0.05935411, 0.01490789, 0.08727722, & + ! 0.08508602, 0.01754962, 0.12212305, 0.12607313, 0.02324255, 0.03571866, & + ! 0.13962916, 0.00848232, 0.16332203, 0.0405824, 0.28314572, 0.05114984, & + ! 0.40216838, 0.3507376, 0.022072, 0.0540725, 0.06678422, 0.03625773, & + ! 0.0484979, 0.8516855, 0.98869347, 0.05778878, 0.06610646, 0.0764022, & + ! 0.08499995, 0.09210838, 0.13037341, 0.13997288 /) - numZeta = size(zeta, 1) + numZeta = size(p%zeta) ! 0. Setup quadrature points CALL BD_QuadraturePointData(p, x, m) @@ -1882,7 +1888,7 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) CALL EigenSolveWrap(m%LP_StifK_LU, m%LP_MassM_LU, nDOF, nDOF, .TRUE., eigenvectors, omega, ErrStat, ErrMsg ) ! Mass-normalize the mode shapes - CALL AllocAry(phi0T_M_phi0, nDOF, nDof, 'phi0T_M_phi0', ErrStat2, ErrMsg2) + CALL AllocAry(phi0T_M_phi0, nDOF, nDOF, 'phi0T_M_phi0', ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') phi0T_M_phi0 = matmul(transpose(eigenvectors), matmul(m%LP_MassM_LU, eigenvectors)) @@ -1892,7 +1898,7 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) end do ! 4. Generate damping matrix in original frame - CALL AllocAry(phiT_M, nDOF, nDof, 'phiT_M', ErrStat2, ErrMsg2) + CALL AllocAry(phiT_M, nDOF, nDOF, 'phiT_M', ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') phiT_M = matmul(transpose(eigenvectors), m%LP_MassM_LU) ! after normalization @@ -1903,11 +1909,11 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) do j = 1, nDOF if( j <= numZeta) then - Zj = 2.0d0 * omega(j) * zeta(j) + Zj = 2.0_R8Ki * omega(j) * p%zeta(j) else ! Stiffness proportional damping is used past the last prescribed value ! at a rate equal to the last prescribed value. - Zj = 2.0d0 * omega(j) * (zeta(numZeta) * omega(j) / omega(numZeta)) + Zj = 2.0_R8Ki * omega(j) * (p%zeta(numZeta) * omega(j) / omega(numZeta)) endif p%ModalDampingMat(j, :) = Zj * phiT_M(j, :) @@ -1947,7 +1953,7 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m) ! print *, 'Verifying mass orthonormal here.' ! phi0T_M_phi0 = matmul(transpose(eigenvectors), matmul(p%ModalDampingMat, eigenvectors)) ! do j = 1,nDOF - ! phi0T_M_phi0(j,j) = phi0T_M_phi0(j,j) / omega(j) / 2.0d0 + ! phi0T_M_phi0(j,j) = phi0T_M_phi0(j,j) / omega(j) / 2.0_R8Ki ! end do CALL CalcModalParticipation() @@ -1996,8 +2002,8 @@ SUBROUTINE CalcModalParticipation() ! Write to a file do j = 1, numZeta - write(bdModesFile, ' (1F12.4,1F12.8,6E14.5) ') omega(j)/6.28318530d0, & - zeta(j), modal_participation(j, :) / sum(modal_participation(j, :)) + write(bdModesFile, ' (1F12.4,1F12.8,6E14.5) ') omega(j)/TwoPi_D, & + p%zeta(j), modal_participation(j, :) / sum(modal_participation(j, :)) end do close(bdModesFile) @@ -2023,8 +2029,8 @@ SUBROUTINE EigenSolveWrap(K, M, nDOF, NOmega, bCheckSingularity, EigVect, Omega CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None ! LOCALS - REAL(LaKi), ALLOCATABLE :: K_LaKi(:,:), M_LaKi(:,:) - REAL(LaKi), ALLOCATABLE :: EigVect_LaKi(:,:), Omega_LaKi(:) + REAL(R8Ki), ALLOCATABLE :: K_R8Ki(:,:), M_R8Ki(:,:) + REAL(R8Ki), ALLOCATABLE :: EigVect_R8Ki(:,:), Omega_R8Ki(:) INTEGER(IntKi) :: N INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 @@ -2035,11 +2041,11 @@ SUBROUTINE EigenSolveWrap(K, M, nDOF, NOmega, bCheckSingularity, EigVect, Omega ! --- Unfortunate conversion to FEKi... TODO TODO consider storing M and K in FEKi N=size(K,1) - CALL AllocAry(K_LaKi , N, N, 'K_FEKi', ErrStat2, ErrMsg2); if(Failed()) return - CALL AllocAry(M_LaKi , N, N, 'M_FEKi', ErrStat2, ErrMsg2); if(Failed()) return - K_LaKi = real( K, LaKi ) - M_LaKi = real( M, LaKi ) - N=size(K_LaKi,1) + CALL AllocAry(K_R8Ki , N, N, 'K_FEKi', ErrStat2, ErrMsg2); if(Failed()) return + CALL AllocAry(M_R8Ki , N, N, 'M_FEKi', ErrStat2, ErrMsg2); if(Failed()) return + K_R8Ki = real( K, R8Ki ) + M_R8Ki = real( M, R8Ki ) + N=size(K_R8Ki,1) ! Note: NOmega must be <= N, which is the length of Omega2, Phi! if ( NOmega > nDOF ) then @@ -2049,15 +2055,15 @@ SUBROUTINE EigenSolveWrap(K, M, nDOF, NOmega, bCheckSingularity, EigVect, Omega end if ! --- Eigenvalue analysis - CALL AllocAry(EigVect_LAKi, N, N, 'EigVect', ErrStat2, ErrMsg2); if(Failed()) return; - CALL AllocAry(Omega_LaKi, N , 'Omega', ErrStat2, ErrMsg2); if(Failed()) return; ! <<< NOTE: Needed due to dimension of Omega - CALL EigenSolve(K_LaKi, M_LaKi, N, bCheckSingularity, EigVect_LaKi, Omega_LaKi, ErrStat2, ErrMsg2 ); if (Failed()) return; + CALL AllocAry(EigVect_R8Ki, N, N, 'EigVect', ErrStat2, ErrMsg2); if(Failed()) return; + CALL AllocAry(Omega_R8Ki, N , 'Omega', ErrStat2, ErrMsg2); if(Failed()) return; ! <<< NOTE: Needed due to dimension of Omega + CALL EigenSolve(K_R8Ki, M_R8Ki, N, bCheckSingularity, EigVect_R8Ki, Omega_R8Ki, ErrStat2, ErrMsg2 ); if (Failed()) return; Omega(:) = huge(1.0_ReKi) - Omega(1:nOmega) = real(Omega_LaKi(1:nOmega), FEKi) !<<< nOmega Date: Thu, 8 Jan 2026 19:49:54 +0000 Subject: [PATCH 14/24] Update r-test pointer --- reg_tests/r-test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/reg_tests/r-test b/reg_tests/r-test index 4aba0e855..f29726004 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit 4aba0e8551c2573bcf30e87a7ed69059df2af432 +Subproject commit f2972600420f2b918ebf3ac3a3d7ced71d648eba From 3e1eea3d98b22fdaa6281df25087e787975d3859 Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Thu, 8 Jan 2026 20:19:08 +0000 Subject: [PATCH 15/24] Tidy up Init_ModalDamping --- modules/beamdyn/src/BeamDyn.f90 | 194 +++++++------------------- modules/nwtc-library/src/NWTC_Num.f90 | 44 +++--- modules/subdyn/src/FEM.f90 | 2 +- 3 files changed, 76 insertions(+), 164 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 62ec9ad86..165354340 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -1807,28 +1807,26 @@ END SUBROUTINE Init_ContinuousStates !> This routine initializes modal damping. SUBROUTINE Init_ModalDamping(x, OtherState, p, m, ErrStat, ErrMsg) - ! TODO : Error messages are probably wrong in some way. - TYPE(BD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at t on input at t + dt on output type(BD_OtherStateType), INTENT(IN ) :: OtherState !< Global rotations are stored in otherstate TYPE(BD_ParameterType), INTENT(INOUT) :: p !< Parameters, output modal damping matrix in original frame here TYPE(BD_MiscVarType), INTENT(INOUT) :: m !< misc/optimization variables INTEGER(IntKi), INTENT( OUT) :: ErrStat - CHARACTER(ErrMsgLen), INTENT( OUT) :: ErrMsg + CHARACTER(*), INTENT( OUT) :: ErrMsg - INTEGER(IntKi) :: ErrStat2 ! Temporary Error status - CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary Error message + CHARACTER(*), PARAMETER :: RoutineName = 'Init_ModalDamping' + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 INTEGER(IntKi) :: nDOF - INTEGER(IntKi) :: j ! looping indexing variable - INTEGER(IntKi) :: numZeta ! number of damping values - REAL(R8Ki) :: Zj ! diagonal element of the modal damping matrix - REAL(R8Ki), ALLOCATABLE :: eigenvectors (:, :) ! mode shapes - REAL(R8Ki), ALLOCATABLE :: omega (:) ! modal frequencies (rad/s) - REAL(R8Ki), ALLOCATABLE :: phiT_M (:, :) ! mode shapes transpose times mass matrix - REAL(R8Ki), ALLOCATABLE :: phi0T_M_phi0 (:, :) ! normalization calculation of mass matrix - - REAL(R8Ki), ALLOCATABLE :: modal_participation (:, :) ! Modal participation factor - INTEGER(IntKi) :: bdModesFile ! Unit numbers for file with BD modes + INTEGER(IntKi) :: j, k ! looping indexing variable + INTEGER(IntKi) :: numZeta ! number of damping values + REAL(R8Ki) :: Zj ! diagonal element of the modal damping matrix + REAL(R8Ki), ALLOCATABLE :: eigenvectors(:, :) ! mode shapes + REAL(R8Ki), ALLOCATABLE :: omega(:) ! modal frequencies (rad/s) + REAL(R8Ki), ALLOCATABLE :: phiT_M(:, :) ! mode shapes transpose times mass matrix + REAL(R8Ki), ALLOCATABLE :: phi0T_M_phi0(:, :) ! normalization calculation of mass matrix + REAL(R8Ki), ALLOCATABLE :: StifK(:,:) ! Copy of stiffness matrix for eigenanalysis (modified during solve) + REAL(R8Ki), ALLOCATABLE :: MassM(:,:) ! Copy of mass matrix for eigenanalysis (modified during solve) real(R8Ki) :: NodeRot(3, 3) ErrStat = ErrID_None @@ -1879,17 +1877,19 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m, ErrStat, ErrMsg) ! For now, calculate all eigenpairs nDOF = p%dof_total - 6 - CALL AllocAry(eigenvectors, nDOF, nDOF, 'eigenvectors', ErrStat2, ErrMsg2) - CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') + ! Allocate eigenvector matrix and eigenvalue arrays + call AllocAry(eigenvectors, nDOF, nDOF, 'eigenvectors', ErrStat2, ErrMsg2); if (Failed()) return + call AllocAry(omega, nDOF, 'omega', ErrStat2, ErrMsg2); if (Failed()) return + call AllocAry(StifK, nDOF, nDOF, 'StifK', ErrStat2, ErrMsg2); if (Failed()) return + call AllocAry(MassM, nDOF, nDOF, 'MassM', ErrStat2, ErrMsg2); if (Failed()) return - CALL AllocAry(omega, nDOF, 'omega', ErrStat2, ErrMsg2) - CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') - - CALL EigenSolveWrap(m%LP_StifK_LU, m%LP_MassM_LU, nDOF, nDOF, .TRUE., eigenvectors, omega, ErrStat, ErrMsg ) + ! EigenSolve modifies the input matrices, so make copies before calling + StifK = m%LP_StifK_LU + MassM = m%LP_MassM_LU + call EigenSolve(StifK, MassM, nDOF, .TRUE., eigenvectors, omega, ErrStat2, ErrMsg2); if (Failed()) return ! Mass-normalize the mode shapes - CALL AllocAry(phi0T_M_phi0, nDOF, nDOF, 'phi0T_M_phi0', ErrStat2, ErrMsg2) - CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') + call AllocAry(phi0T_M_phi0, nDOF, nDOF, 'phi0T_M_phi0', ErrStat2, ErrMsg2); if (Failed()) return phi0T_M_phi0 = matmul(transpose(eigenvectors), matmul(m%LP_MassM_LU, eigenvectors)) @@ -1898,13 +1898,11 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m, ErrStat, ErrMsg) end do ! 4. Generate damping matrix in original frame - CALL AllocAry(phiT_M, nDOF, nDOF, 'phiT_M', ErrStat2, ErrMsg2) - CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') + call AllocAry(phiT_M, nDOF, nDOF, 'phiT_M', ErrStat2, ErrMsg2); if (Failed()) return phiT_M = matmul(transpose(eigenvectors), m%LP_MassM_LU) ! after normalization - CALL AllocAry(p%ModalDampingMat, nDOF, nDOF, 'p%ModalDampingMat', ErrStat2, ErrMsg2) - CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') + call AllocAry(p%ModalDampingMat, nDOF, nDOF, 'p%ModalDampingMat', ErrStat2, ErrMsg2); if (Failed()) return do j = 1, nDOF @@ -1923,29 +1921,23 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m, ErrStat, ErrMsg) ! Apply the rotation of q here. When the actual dynamics are at the same position ! as this, then this cancels with a rotation applied at the modal damping force. - do j = 1, size(x%dqdt, 2)-1 + do j = 2, p%node_total ! Loop over the nodes that apply to the damping matrix, so don't include the root node. - call BD_CrvMatrixR(x%q(4:6, j+1), NodeRot) - - p%ModalDampingMat(:, (j-1)*6+1:(j-1)*6+3) = matmul(p%ModalDampingMat(:, (j-1)*6+1:(j-1)*6+3), & - NodeRot) - - p%ModalDampingMat(:, (j-1)*6+4:(j-1)*6+6) = matmul(p%ModalDampingMat(:, (j-1)*6+4:(j-1)*6+6), & - NodeRot) - + call BD_CrvMatrixR(x%q(4:6, j), NodeRot) + + k = (j-2)*6 + p%ModalDampingMat(:, k+1:k+3) = matmul(p%ModalDampingMat(:, k+1:k+3), NodeRot) + p%ModalDampingMat(:, k+4:k+6) = matmul(p%ModalDampingMat(:, k+4:k+6), NodeRot) end do - do j = 1, size(x%dqdt, 2)-1 + do j = 2, p%node_total ! Loop over the nodes that apply to the damping matrix, so don't include the root node. - call BD_CrvMatrixR(x%q(4:6, j+1), NodeRot) - - p%ModalDampingMat((j-1)*6+1:(j-1)*6+3, :) = matmul(transpose(NodeRot), & - p%ModalDampingMat((j-1)*6+1:(j-1)*6+3, :)) - - p%ModalDampingMat((j-1)*6+4:(j-1)*6+6, :) = matmul(transpose(NodeRot), & - p%ModalDampingMat((j-1)*6+4:(j-1)*6+6, :)) - + call BD_CrvMatrixR(x%q(4:6, j), NodeRot) + + k = (j-2)*6 + p%ModalDampingMat(k+1:k+3, :) = matmul(transpose(NodeRot), p%ModalDampingMat(k+1:k+3, :)) + p%ModalDampingMat(k+4:k+6, :) = matmul(transpose(NodeRot), p%ModalDampingMat(k+4:k+6, :)) end do ! Debugging / verifying - recover the zeta values @@ -1956,30 +1948,25 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m, ErrStat, ErrMsg) ! phi0T_M_phi0(j,j) = phi0T_M_phi0(j,j) / omega(j) / 2.0_R8Ki ! end do - CALL CalcModalParticipation() + call CalcModalParticipation() ! Allocate memory for the velocity vector that will be multiplied by the modal damping matrix - CALL AllocAry(m%DampedVelocities, p%dof_total-6, 'DampedVelocities', ErrStat2, ErrMsg2) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') + call AllocAry(m%DampedVelocities, nDOF, 'DampedVelocities', ErrStat2, ErrMsg2); if (Failed()) return ! Allocate memory for the velocity vector that will be multiplied by the modal damping matrix - CALL AllocAry(m%ModalDampingF, p%dof_total-6, 'ModalDampingF', ErrStat2, ErrMsg2) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Init_ModalDamping') - - print *, 'End of Modal damping init.' + call AllocAry(m%ModalDampingF, nDOF, 'ModalDampingF', ErrStat2, ErrMsg2); if (Failed()) return - CALL CleanupModalInit() +contains -CONTAINS + logical function Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + Failed = ErrStat >= AbortErrLev + end function Failed - SUBROUTINE CleanupModalInit() - IF (ALLOCATED(eigenvectors) ) DEALLOCATE(eigenvectors) - IF (ALLOCATED(omega) ) DEALLOCATE(omega) - IF (ALLOCATED(phi0T_M_phi0) ) DEALLOCATE(phi0T_M_phi0) - IF (ALLOCATED(phiT_M) ) DEALLOCATE(phiT_M) - END SUBROUTINE CleanupModalInit + subroutine CalcModalParticipation() - SUBROUTINE CalcModalParticipation() + REAL(R8Ki), ALLOCATABLE :: modal_participation (:, :) ! Modal participation factor + INTEGER(IntKi) :: bdModesFile ! Unit numbers for file with BD modes ! Theory based on Abaqus documentation ! Only using rotational DOFs for rotations and not including @@ -1993,12 +1980,12 @@ SUBROUTINE CalcModalParticipation() end do ! Write to a file instead - CALL GetNewUnit( bdModesFile ) + CALL GetNewUnit(bdModesFile) open(unit=bdModesFile, file='beamdyn_modes.csv') - write(bdModesFile,*) '#Frequency [Hz], Zeta [Frac. Critical], & - &Participation X, Participation Y, Participation Z, & - &Participation RX, Participation RY, Participation RZ' + write(bdModesFile,*) '#Frequency [Hz], Zeta [Frac. Critical],'// & + 'Participation X, Participation Y, Participation Z,'// & + 'Participation RX, Participation RY, Participation RZ' ! Write to a file do j = 1, numZeta @@ -2008,83 +1995,10 @@ SUBROUTINE CalcModalParticipation() close(bdModesFile) - IF (ALLOCATED(modal_participation) ) DEALLOCATE(modal_participation) - END SUBROUTINE END SUBROUTINE -!> Wrapper function for eigen value analyses -SUBROUTINE EigenSolveWrap(K, M, nDOF, NOmega, bCheckSingularity, EigVect, Omega, ErrStat, ErrMsg ) - USE NWTC_Num, only: EigenSolve - - INTEGER, INTENT(IN ) :: nDOF ! Total degrees of freedom of the incoming system - REAL(FEKi), INTENT(IN ) :: K(nDOF, nDOF) ! stiffness matrix - REAL(FEKi), INTENT(IN ) :: M(nDOF, nDOF) ! mass matrix - INTEGER, INTENT(IN ) :: NOmega ! No. of requested eigenvalues - LOGICAL, INTENT(IN ) :: bCheckSingularity ! If True, the solver will fail if rigid modes are present - REAL(FEKi), INTENT( OUT) :: EigVect(nDOF, NOmega) ! Returned Eigenvectors - REAL(FEKi), INTENT( OUT) :: Omega(NOmega) ! Returned Eigenvalues - INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation - CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None - - ! LOCALS - REAL(R8Ki), ALLOCATABLE :: K_R8Ki(:,:), M_R8Ki(:,:) - REAL(R8Ki), ALLOCATABLE :: EigVect_R8Ki(:,:), Omega_R8Ki(:) - INTEGER(IntKi) :: N - INTEGER(IntKi) :: ErrStat2 - CHARACTER(ErrMsgLen) :: ErrMsg2 - ErrStat = ErrID_None - ErrMsg = '' - EigVect=0.0_FeKi - Omega=0.0_FeKi - - ! --- Unfortunate conversion to FEKi... TODO TODO consider storing M and K in FEKi - N=size(K,1) - CALL AllocAry(K_R8Ki , N, N, 'K_FEKi', ErrStat2, ErrMsg2); if(Failed()) return - CALL AllocAry(M_R8Ki , N, N, 'M_FEKi', ErrStat2, ErrMsg2); if(Failed()) return - K_R8Ki = real( K, R8Ki ) - M_R8Ki = real( M, R8Ki ) - N=size(K_R8Ki,1) - - ! Note: NOmega must be <= N, which is the length of Omega2, Phi! - if ( NOmega > nDOF ) then - CALL SetErrStat(ErrID_Fatal,"NOmega must be less than or equal to N",ErrStat,ErrMsg,'EigenSolveWrap') - CALL CleanupEigen() - return - end if - - ! --- Eigenvalue analysis - CALL AllocAry(EigVect_R8Ki, N, N, 'EigVect', ErrStat2, ErrMsg2); if(Failed()) return; - CALL AllocAry(Omega_R8Ki, N , 'Omega', ErrStat2, ErrMsg2); if(Failed()) return; ! <<< NOTE: Needed due to dimension of Omega - CALL EigenSolve(K_R8Ki, M_R8Ki, N, bCheckSingularity, EigVect_R8Ki, Omega_R8Ki, ErrStat2, ErrMsg2 ); if (Failed()) return; - - Omega(:) = huge(1.0_ReKi) - Omega(1:nOmega) = real(Omega_R8Ki(1:nOmega), FEKi) !<<< nOmega= AbortErrLev - if (Failed) call CleanUpEigen() - END FUNCTION Failed - - SUBROUTINE CleanupEigen() - IF (ALLOCATED(Omega_R8Ki) ) DEALLOCATE(Omega_R8Ki) - IF (ALLOCATED(EigVect_R8Ki)) DEALLOCATE(EigVect_R8Ki) - IF (ALLOCATED(K_R8Ki) ) DEALLOCATE(K_R8Ki) - IF (ALLOCATED(M_R8Ki) ) DEALLOCATE(M_R8Ki) - END SUBROUTINE CleanupEigen - -END SUBROUTINE EigenSolveWrap - !----------------------------------------------------------------------------------------------------------------------------------- !> This routine is called at the end of the simulation. SUBROUTINE BD_End( u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) @@ -6056,7 +5970,7 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) ModalDampingRot = matmul(transpose(u%RootMotion%Orientation(:, :, 1)), OtherState%GlbRot) - do j = 1, size(x%dqdt, 2)-1 + do j = 1, p%node_total-1 ! Loop over the nodes that apply to the damping matrix, so don't include the root node. call BD_CrvMatrixR(x%q(4:6, j+1), NodeRot) @@ -6071,7 +5985,7 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) m%ModalDampingF = matmul(p%ModalDampingMat, m%DampedVelocities) ! 4. Rotate to correct coordinates and subtract from m%LP_RHS_LU - do j = 1, size(x%dqdt, 2)-1 + do j = 1, p%node_total-1 call BD_CrvMatrixR(x%q(4:6, j+1), NodeRot) diff --git a/modules/nwtc-library/src/NWTC_Num.f90 b/modules/nwtc-library/src/NWTC_Num.f90 index 7e246cf4b..a0ced39fa 100644 --- a/modules/nwtc-library/src/NWTC_Num.f90 +++ b/modules/nwtc-library/src/NWTC_Num.f90 @@ -80,8 +80,6 @@ MODULE NWTC_Num INTEGER, PARAMETER :: kernelType_TRIWEIGHT = 4 INTEGER, PARAMETER :: kernelType_TRICUBE = 5 INTEGER, PARAMETER :: kernelType_GAUSSIAN = 6 - - INTEGER, PARAMETER :: LaKi = R8Ki ! Define the kind to be used for LaPack, Eigensolve ! constants for output formats INTEGER, PARAMETER :: Output_in_Native_Units = 0 @@ -7302,23 +7300,23 @@ SUBROUTINE EigenSolve(K, M, N, bCheckSingularity, EigVect, Omega, ErrStat, ErrMs USE NWTC_LAPACK, only: LAPACK_ggev INTEGER , INTENT(IN ) :: N !< Number of degrees of freedom, size of M and K - REAL(LaKi), INTENT(INOUT) :: K(N, N) !< Stiffness matrix - REAL(LaKi), INTENT(INOUT) :: M(N, N) !< Mass matrix + REAL(R8Ki), INTENT(INOUT) :: K(N, N) !< Stiffness matrix + REAL(R8Ki), INTENT(INOUT) :: M(N, N) !< Mass matrix LOGICAL, INTENT(IN ) :: bCheckSingularity ! If True, the solver will fail if rigid modes are present - REAL(LaKi), INTENT(INOUT) :: EigVect(N, N) !< Returned Eigenvectors - REAL(LaKi), INTENT(INOUT) :: Omega(N) !< Returned Eigenvalues + REAL(R8Ki), INTENT(INOUT) :: EigVect(N, N) !< Returned Eigenvectors + REAL(R8Ki), INTENT(INOUT) :: Omega(N) !< Returned Eigenvalues INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None ! LOCALS - REAL(LaKi), ALLOCATABLE :: WORK (:), VL(:,:), AlphaR(:), AlphaI(:), BETA(:) ! eigensolver variables + REAL(R8Ki), ALLOCATABLE :: WORK (:), VL(:,:), AlphaR(:), AlphaI(:), BETA(:) ! eigensolver variables INTEGER :: i INTEGER :: LWORK !variables for the eigensolver INTEGER, ALLOCATABLE :: KEY(:) INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 - REAL(LaKi) :: normA - REAL(LaKi) :: Omega2(N) !< Squared eigenvalues - REAL(LaKi), parameter :: MAX_EIGENVALUE = HUGE(1.0_ReKi) ! To avoid overflow when switching to ReKi + REAL(R8Ki) :: normA + REAL(R8Ki) :: Omega2(N) !< Squared eigenvalues + REAL(R8Ki), parameter :: MAX_EIGENVALUE = HUGE(1.0_ReKi) ! To avoid overflow when switching to ReKi ErrStat = ErrID_None ErrMsg = '' @@ -7335,7 +7333,7 @@ SUBROUTINE EigenSolve(K, M, N, bCheckSingularity, EigVect, Omega, ErrStat, ErrMs ! --- Eigenvalue analysis ! note: SGGEV seems to have memory issues in certain cases. The eigenvalues seem to be okay, but the eigenvectors vary wildly with different compiling options. - ! DGGEV seems to work better, so I'm making these variables LaKi (which is set to R8Ki for now) - bjj 4/25/2014 + ! DGGEV seems to work better, so I'm making these variables R8Ki (which is set to R8Ki for now) - bjj 4/25/2014 ! bjj: This comes from the LAPACK documentation: ! Note: the quotients AlphaR(j)/BETA(j) and AlphaI(j)/BETA(j) may easily over- or underflow, and BETA(j) may even be zero. ! Thus, the user should avoid naively computing the ratio Alpha/beta. However, AlphaR and AlphaI will be always less @@ -7345,7 +7343,7 @@ SUBROUTINE EigenSolve(K, M, N, bCheckSingularity, EigVect, Omega, ErrStat, ErrMs if(Failed()) return ! --- Determining and sorting eigen frequencies - Omega2(:) =0.0_LaKi + Omega2(:) =0.0_R8Ki DO I=1,N !Initialize the key and calculate Omega KEY(I)=I !Omega2(I) = AlphaR(I)/Beta(I) @@ -7355,11 +7353,11 @@ SUBROUTINE EigenSolve(K, M, N, bCheckSingularity, EigVect, Omega, ErrStat, ErrMs Omega2(I) = MAX_EIGENVALUE elseif ( EqualRealNos(real(AlphaI(I),ReKi),0.0_ReKi) ) THEN ! --- Real Eigenvalues - IF ( AlphaR(I)<0.0_LaKi ) THEN - if ( (AlphaR(I)/Beta(I))<1e-6_LaKi ) then + IF ( AlphaR(I)<0.0_R8Ki ) THEN + if ( (AlphaR(I)/Beta(I))<1e-6_R8Ki ) then ! Tolerating very small negative eigenvalues if (bCheckSingularity) call WrScr('[INFO] Negative eigenvalue found with small norm (system may contain rigid body mode)') - Omega2(I)=0.0_LaKi + Omega2(I)=0.0_R8Ki else if (bCheckSingularity) call WrScr('[WARN] Negative eigenvalue found, system may be ill-conditioned.') Omega2(I)=AlphaR(I)/Beta(I) @@ -7370,11 +7368,11 @@ SUBROUTINE EigenSolve(K, M, N, bCheckSingularity, EigVect, Omega, ErrStat, ErrMs else ! --- Complex Eigenvalues normA = sqrt(AlphaR(I)**2 + AlphaI(I)**2) - if ( (normA/Beta(I))<1e-6_LaKi ) then + if ( (normA/Beta(I))<1e-6_R8Ki ) then ! Tolerating very small eigenvalues with imaginary part if (bCheckSingularity) call WrScr('[WARN] Complex eigenvalue found with small norm, approximating as 0') - Omega2(I) = 0.0_LaKi - elseif ( abs(AlphaR(I))>1e3_LaKi*abs(AlphaI(I)) ) then + Omega2(I) = 0.0_R8Ki + elseif ( abs(AlphaR(I))>1e3_R8Ki*abs(AlphaI(I)) ) then ! Tolerating very small imaginary part compared to real part... (not pretty) if (bCheckSingularity) call WrScr('[WARN] Complex eigenvalue found with small Im compare to Re') Omega2(I) = AlphaR(I)/Beta(I) @@ -7409,10 +7407,10 @@ SUBROUTINE EigenSolve(K, M, N, bCheckSingularity, EigVect, Omega, ErrStat, ErrMs !+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++! ! --- Return Omega (capped by huge(ReKi)) and check for singularity - Omega(:) = 0.0_LaKi + Omega(:) = 0.0_R8Ki do I=1,N if (EqualRealNos(real(Omega2(I),ReKi), 0.0_ReKi)) then ! NOTE: may be necessary for some corner numerics - Omega(i)=0.0_LaKi + Omega(i)=0.0_R8Ki if (bCheckSingularity) then call Fatal('Zero eigenvalue found, system may contain rigid body mode'); return endif @@ -7421,7 +7419,7 @@ SUBROUTINE EigenSolve(K, M, N, bCheckSingularity, EigVect, Omega, ErrStat, ErrMs else ! Negative eigenfrequency print*,'>>> Wrong eigenfrequency, Omega^2=',Omega2(I) ! <<< This should never happen - Omega(i)= 0.0_LaKi + Omega(i)= 0.0_R8Ki call Fatal('Negative eigenvalue found, system may be ill-conditioned'); return endif enddo @@ -7452,10 +7450,10 @@ SUBROUTINE CleanupEigen() END SUBROUTINE CleanupEigen pure subroutine sort_in_place(a,key) - real(LaKi), intent(inout), dimension(:) :: a + real(R8Ki), intent(inout), dimension(:) :: a integer(IntKi), intent(inout), dimension(:) :: key integer(IntKi) :: tempI - real(LaKi) :: temp + real(R8Ki) :: temp integer(IntKi) :: i, j do i = 2, size(a) j = i - 1 diff --git a/modules/subdyn/src/FEM.f90 b/modules/subdyn/src/FEM.f90 index 97a14a902..f645c10c5 100644 --- a/modules/subdyn/src/FEM.f90 +++ b/modules/subdyn/src/FEM.f90 @@ -23,7 +23,7 @@ MODULE FEM IMPLICIT NONE INTEGER, PARAMETER :: FEKi = R8Ki ! Define the kind to be used for FEM - ! INTEGER, PARAMETER :: LaKi = R8Ki ! Define the kind to be used for LaPack + INTEGER, PARAMETER :: LaKi = R8Ki ! Define the kind to be used for LaPack INTERFACE FINDLOCI ! In the future, use FINDLOC from intrinsic MODULE PROCEDURE FINDLOCI_R8Ki From 689f93819fb0ef8e67d4932aea8cfcc76b472cf0 Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Thu, 8 Jan 2026 21:43:49 +0000 Subject: [PATCH 16/24] Remove additional rotation from BD_AddModalDampingRHS --- modules/beamdyn/src/BeamDyn.f90 | 68 +++++++++++++-------------------- 1 file changed, 26 insertions(+), 42 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 165354340..7b6847993 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -5922,40 +5922,34 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) TYPE(BD_OtherStateType), INTENT(IN ) :: OtherState !< other states (contains ref orientation) TYPE(BD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables - INTEGER(IntKi) :: j ! looping indexing variable (node number) - INTEGER(IntKi) :: elem ! looping indexing for element number - INTEGER(IntKi) :: elem_node ! looping indexing for node in the element number - REAL(R8Ki) :: r(3) ! nodal position relative to root - real(R8Ki) :: ModalDampingRot(3, 3) - real(R8Ki) :: NodeRot(3, 3) - + integer(IntKi) :: j, k ! looping indexing variable + integer(IntKi) :: elem ! looping indexing for element number + integer(IntKi) :: elem_node ! looping indexing for node in the element number + real(R8Ki) :: r(3) ! nodal position relative to root + real(R8Ki) :: NodeRot(3, 3) ! 1. Velocities relative to root ! element loops do elem = 1, p%elem_total - do elem_node = 1, p%nodes_per_elem + do elem_node = 2, p%nodes_per_elem ! Global node index, excluding root - j = (elem - 1) * (p%nodes_per_elem - 1) + elem_node - 1 + j = (elem - 1) * (p%nodes_per_elem - 1) + elem_node - if (j > 0) then + ! DOF index + k = (j - 2) * 6 - ! 1.a. Velocities minus root velocity - ! x%... is at j+1 because skipping the root node and j is 1 at the first node after root. - m%DampedVelocities((j-1)*6+1:j*6) = x%dqdt(:, j+1) - x%dqdt(:, 1) + ! 1.a. Node translational velocity minus root velocity + ! x%... is at j+1 because skipping the root node and j is 1 at the first node after root. + m%DampedVelocities(k+1:k+6) = x%dqdt(:, j) - x%dqdt(:, 1) - ! 1.b. Subtract out the rigid body rotation of the root here - ! position cross rotational velocity - - ! Vector from root to node (inertial frame) - r = p%uuN0(1:3, elem_node, elem) + x%q(1:3, j+1) - - m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = m%DampedVelocities((j-1)*6+1:(j-1)*6+3) & - - Cross_Product(x%dqdt(4:6, 1), r) - end if + ! 1.b. Subtract out the rigid body rotational velocity based on the blade root rotation + ! Vector from root to node + r = p%uuN0(1:3, elem_node, elem) + x%q(1:3, j) + m%DampedVelocities(k+1:k+3) = m%DampedVelocities(k+1:k+3) - Cross_Product(x%dqdt(4:6, 1), r) end do end do @@ -5968,39 +5962,29 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) ! OtherState%GlbRot = tranpose(u%RootMotion%Orientation(:, :, 1) evaluated at n) ! here, u%RootMotion%Orientation(:, :, 1) is evaluated at n+1, but is tranposed at this point - ModalDampingRot = matmul(transpose(u%RootMotion%Orientation(:, :, 1)), OtherState%GlbRot) - - do j = 1, p%node_total-1 + do j = 2, p%node_total ! Loop over the nodes that apply to the damping matrix, so don't include the root node. - call BD_CrvMatrixR(x%q(4:6, j+1), NodeRot) + call BD_CrvMatrixR(x%q(4:6, j), NodeRot) - m%DampedVelocities((j-1)*6+1:(j-1)*6+3) = matmul(matmul(transpose(NodeRot), ModalDampingRot), & - m%DampedVelocities((j-1)*6+1:(j-1)*6+3)) - m%DampedVelocities((j-1)*6+4:(j-1)*6+6) = matmul(matmul(transpose(NodeRot), ModalDampingRot), & - m%DampedVelocities((j-1)*6+4:(j-1)*6+6)) + k = (j - 2) * 6 + m%DampedVelocities(k+1:k+3) = matmul(transpose(NodeRot), m%DampedVelocities(k+1:k+3)) + m%DampedVelocities(k+4:k+6) = matmul(transpose(NodeRot), m%DampedVelocities(k+4:k+6)) end do ! 3. Multiply by modal damping matrix m%ModalDampingF = matmul(p%ModalDampingMat, m%DampedVelocities) ! 4. Rotate to correct coordinates and subtract from m%LP_RHS_LU - do j = 1, p%node_total-1 - - call BD_CrvMatrixR(x%q(4:6, j+1), NodeRot) + do j = 2, p%node_total - m%LP_RHS_LU((j-1)*6+1:(j-1)*6+3) = m%LP_RHS_LU((j-1)*6+1:(j-1)*6+3) & - - matmul(matmul(transpose(ModalDampingRot), NodeRot), & - m%ModalDampingF((j-1)*6+1:(j-1)*6+3)) + call BD_CrvMatrixR(x%q(4:6, j), NodeRot) - m%LP_RHS_LU((j-1)*6+4:(j-1)*6+6) = m%LP_RHS_LU((j-1)*6+4:(j-1)*6+6) & - - matmul(matmul(transpose(ModalDampingRot), NodeRot), & - m%ModalDampingF((j-1)*6+4:(j-1)*6+6)) + k = (j - 2) * 6 + m%LP_RHS_LU(k+1:k+3) = m%LP_RHS_LU(k+1:k+3) - matmul(NodeRot, m%ModalDampingF(k+1:k+3)) + m%LP_RHS_LU(k+4:k+6) = m%LP_RHS_LU(k+4:k+6) - matmul(NodeRot, m%ModalDampingF(k+4:k+6)) end do - - ! print *, 'End of damping force calculation.' - END SUBROUTINE BD_AddModalDampingRHS From b5cad678fe734f052c319bf35c53acc8fc3d62fa Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Thu, 8 Jan 2026 21:44:11 +0000 Subject: [PATCH 17/24] Update openfast_io for new BeamDyn Blade file inputs --- openfast_io/openfast_io/FAST_reader.py | 8 +++++++- openfast_io/openfast_io/FAST_writer.py | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/openfast_io/openfast_io/FAST_reader.py b/openfast_io/openfast_io/FAST_reader.py index c7b2eb6c4..e67e7951f 100644 --- a/openfast_io/openfast_io/FAST_reader.py +++ b/openfast_io/openfast_io/FAST_reader.py @@ -1,3 +1,4 @@ +from numbers import Number import os, re, copy import numpy as np from functools import reduce @@ -915,7 +916,7 @@ def read_BeamDynBlade(self, beamdyn_blade_file, BladeNumber = 0): f.readline() f.readline() f.readline() - #---------------------- DAMPING COEFFICIENT------------------------------------ + #---------------------- Stiffness-Proportional Damping ----------------- ln = f.readline().split() self.fst_vt['BeamDynBlade'][BladeNumber]['mu1'] = float(ln[0]) self.fst_vt['BeamDynBlade'][BladeNumber]['mu2'] = float(ln[1]) @@ -924,6 +925,11 @@ def read_BeamDynBlade(self, beamdyn_blade_file, BladeNumber = 0): self.fst_vt['BeamDynBlade'][BladeNumber]['mu5'] = float(ln[4]) self.fst_vt['BeamDynBlade'][BladeNumber]['mu6'] = float(ln[5]) f.readline() + # ------ Modal Damping [used only if damp_type=2] -------------------------------- + + self.fst_vt['BeamDynBlade'][BladeNumber]['n_modes'] = int_read(f.readline().split()[0]) + self.fst_vt['BeamDynBlade'][BladeNumber]['zeta'] = np.array([f.readline().strip().split()[:self.fst_vt['BeamDynBlade'][BladeNumber]['n_modes']]], dtype=float) + f.readline() #---------------------- DISTRIBUTED PROPERTIES--------------------------------- self.fst_vt['BeamDynBlade'][BladeNumber]['radial_stations'] = np.zeros((self.fst_vt['BeamDynBlade'][BladeNumber]['station_total'])) diff --git a/openfast_io/openfast_io/FAST_writer.py b/openfast_io/openfast_io/FAST_writer.py index 9c3eb248d..bc1bc6746 100644 --- a/openfast_io/openfast_io/FAST_writer.py +++ b/openfast_io/openfast_io/FAST_writer.py @@ -820,7 +820,7 @@ def write_BeamDynBlade(self, bldInd = None): f.write('Generated with OpenFAST_IO\n') f.write('---------------------- BLADE PARAMETERS --------------------------------------\n') f.write('{:<22} {:<11} {:}'.format(bd_blade_dict['station_total'], 'station_total', '- Number of blade input stations (-)\n')) - f.write('{:<22} {:<11} {:}'.format(bd_blade_dict['damp_type'], 'damp_type', '- Damping type: 0: no damping; 1: damped\n')) + f.write('{:<22} {:<11} {:}'.format(bd_blade_dict['damp_type'], 'damp_type', '- Damping type (switch) {0: none, 1: stiffness-proportional, 2: modal}\n')) f.write('---------------------- DAMPING COEFFICIENT------------------------------------\n') f.write(" ".join(['{:^11s}'.format(i) for i in ['mu1','mu2','mu3','mu4','mu5','mu6']])+'\n') f.write(" ".join(['{:^11s}'.format(i) for i in ['(-)','(-)','(-)','(-)','(-)','(-)']])+'\n') From 1fa6efb76c0e5f866dd3824e31d390ccc5fbc833 Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Thu, 8 Jan 2026 21:54:13 +0000 Subject: [PATCH 18/24] Account for current root position when calculating vector from root to node in BeamDyn --- modules/beamdyn/src/BeamDyn.f90 | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index 7b6847993..aae3f68f8 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -5947,7 +5947,8 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) ! 1.b. Subtract out the rigid body rotational velocity based on the blade root rotation ! Vector from root to node - r = p%uuN0(1:3, elem_node, elem) + x%q(1:3, j) + r = OtherState%GlbPos + p%uuN0(1:3, elem_node, elem) + x%q(1:3, j) - & + (u%RootMotion%Position(:, 1) + u%RootMotion%TranslationDisp(:, 1)) m%DampedVelocities(k+1:k+3) = m%DampedVelocities(k+1:k+3) - Cross_Product(x%dqdt(4:6, 1), r) end do From d40c039144b6e2a1ae0d2b43f2deae5660666437 Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Fri, 9 Jan 2026 20:23:11 +0000 Subject: [PATCH 19/24] Revert change to BD modal damping node vector calculation --- modules/beamdyn/src/BeamDyn.f90 | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/modules/beamdyn/src/BeamDyn.f90 b/modules/beamdyn/src/BeamDyn.f90 index aae3f68f8..8c71621ec 100644 --- a/modules/beamdyn/src/BeamDyn.f90 +++ b/modules/beamdyn/src/BeamDyn.f90 @@ -1832,8 +1832,6 @@ SUBROUTINE Init_ModalDamping(x, OtherState, p, m, ErrStat, ErrMsg) ErrStat = ErrID_None ErrMsg = '' - PRINT *, 'In modal damping branch. Damping values are currently hard-coded.' - ! TODO : Take actual user input for zeta ! zeta is fraction of critical damping. ! zeta = (/ 0.1d0, 0.3d0, 0.15d0, 0.45d0, 0.2d0, 0.6d0, 0.7d0, 0.8d0, 0.9d0, 1.0d0 /) @@ -5947,10 +5945,11 @@ SUBROUTINE BD_AddModalDampingRHS(u, p, x, OtherState, m) ! 1.b. Subtract out the rigid body rotational velocity based on the blade root rotation ! Vector from root to node - r = OtherState%GlbPos + p%uuN0(1:3, elem_node, elem) + x%q(1:3, j) - & - (u%RootMotion%Position(:, 1) + u%RootMotion%TranslationDisp(:, 1)) + ! r = OtherState%GlbPos + p%uuN0(1:3, elem_node, elem) + x%q(1:3, j) - & + ! (u%RootMotion%Position(:, 1) + u%RootMotion%TranslationDisp(:, 1)) + r = p%uuN0(1:3, elem_node, elem) + x%q(1:3, j) - m%DampedVelocities(k+1:k+3) = m%DampedVelocities(k+1:k+3) - Cross_Product(x%dqdt(4:6, 1), r) + m%DampedVelocities(k+1:k+3) = m%DampedVelocities(k+1:k+3) - Cross_Product(u%RootMotion%RotationVel(:, 1), r) end do end do From 7c00d83cfebd962c16b859de72ac985a4db843dc Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Wed, 14 Jan 2026 22:19:08 +0000 Subject: [PATCH 20/24] Add some basic documentation for modal damping in BeamDyn --- docs/source/user/beamdyn/appendix.rst | 23 +++++ .../user/beamdyn/examples/nrel_5mw_blade.inp | 15 ++-- docs/source/user/beamdyn/input_files.rst | 84 ++++++++++++++----- docs/source/user/beamdyn/introduction.rst | 22 ++--- docs/source/user/beamdyn/running_bd.rst | 39 +-------- docs/source/user/beamdyn/theory.rst | 36 ++++++-- 6 files changed, 140 insertions(+), 79 deletions(-) diff --git a/docs/source/user/beamdyn/appendix.rst b/docs/source/user/beamdyn/appendix.rst index 3e282ce5a..3fabb468f 100644 --- a/docs/source/user/beamdyn/appendix.rst +++ b/docs/source/user/beamdyn/appendix.rst @@ -16,6 +16,11 @@ OpenFAST+BeamDyn and stand-alone BeamDyn (static and dynamic) simulations all re :download:`(NREL 5MW static example) `: This file includes information on the numerical-solution parameters (e.g., numerical damping, quadrature rules), and the geometric definition of the beam reference line via "members" and "key points". This file also specifies the "blade input file." 2) BeamDyn blade input file :download:`(NREL 5MW example) `: + This file specifies the blade sectional properties at various stations along the blade. + The file includes stiffness and mass matrices at each station, as well as damping parameters. + Note that the example file uses stiffness-proportional damping (damp_flag = 1). For modal + damping (damp_flag = 2), the n_modes parameter should be set to a non-zero value and + followed by the corresponding modal damping ratios (zeta values). Stand-alone BeamDyn simulation also require a driver input file; we list here examples for static and dynamic simulations: @@ -56,3 +61,21 @@ outputs are expressed in one of the following three coordinate systems: :align: center BeamDyn Output Channel List + +.. note:: + + **New Output Channels (v5.0 and later):** + + BeamDyn now includes additional output channels for applied loads mapped to the root node. + These channels provide the total applied loads (both distributed and point loads) resolved + at the root of the blade, expressed in both the root coordinate system (r) and global + inertial frame (g): + + - **RootAppliedFxr, RootAppliedFyr, RootAppliedFzr**: Applied force components in r-frame + - **RootAppliedMxr, RootAppliedMyr, RootAppliedMzr**: Applied moment components in r-frame + - **RootAppliedFxg, RootAppliedFyg, RootAppliedFzg**: Applied force components in g-frame + - **RootAppliedMxg, RootAppliedMyg, RootAppliedMzg**: Applied moment components in g-frame + + These outputs are useful for understanding the total aerodynamic and other external loads + acting on the blade, particularly when diagnosing load imbalances or validating force + distributions. diff --git a/docs/source/user/beamdyn/examples/nrel_5mw_blade.inp b/docs/source/user/beamdyn/examples/nrel_5mw_blade.inp index 24e2192f2..0a5ebced0 100644 --- a/docs/source/user/beamdyn/examples/nrel_5mw_blade.inp +++ b/docs/source/user/beamdyn/examples/nrel_5mw_blade.inp @@ -1,13 +1,16 @@ ------- BEAMDYN V1.00.* INDIVIDUAL BLADE INPUT FILE -------------------------- NREL 5MW Blade - ---------------------- BLADE PARAMETERS -------------------------------------- -49 station_total - Number of blade input stations (-) - 1 damp_flag - Damping flag: 0: no damping; 1: damped - ---------------------- DAMPING COEFFICIENT------------------------------------ +------ Blade Parameters -------------------------------------------------------- +49 station_total - Number of blade input stations (-) +1 damp_type - Damping type (switch) {0: none, 1: stiffness-proportional, 2: modal} +------ Stiffness-Proportional Damping [used only if damp_type=1] --------------- mu1 mu2 mu3 mu4 mu5 mu6 (-) (-) (-) (-) (-) (-) -1.0E-03 1.0E-03 1.0E-03 0.0014 0.0022 0.0022 - ---------------------- DISTRIBUTED PROPERTIES--------------------------------- +1.0E-03 1.0E-03 1.0E-03 0.0014 0.0022 0.0022 +------ Modal Damping [used only if damp_type=2] -------------------------------- +4 n_modes - Number of modal damping coefficients (-) +0.01 0.02 0.03 0.04 zeta - Damping coefficients for mode 1 through n_modes +------ Distributed Properties -------------------------------------------------- 0.000000 9.729480E+08 0.000000E+00 0.000000E+00 0.000000E+00 0.000000E+00 0.000000E+00 0.000000E+00 9.729480E+08 0.000000E+00 0.000000E+00 0.000000E+00 0.000000E+00 diff --git a/docs/source/user/beamdyn/input_files.rst b/docs/source/user/beamdyn/input_files.rst index 7f9741eea..26d5ecbfa 100644 --- a/docs/source/user/beamdyn/input_files.rst +++ b/docs/source/user/beamdyn/input_files.rst @@ -215,15 +215,22 @@ contents of the BeamDyn input file (useful for debugging errors in the input file). The ``QuasiStaticInit`` flag indicates if BeamDyn should perform a quasi-static -solution at initialization to better initialize its states. In general, this should -be set to true for better numerical performance (it reduces startup transients). +solution at initialization to better initialize its states. This option is only +available when coupled to OpenFAST with dynamic analysis enabled. When set to ``TRUE``, +BeamDyn will perform a steady-state startup (SSS) solve that includes centripetal +accelerations to reduce startup transients and improve numerical performance. This +is particularly useful for rotating blade simulations where the initial conditions +would otherwise cause spurious transients. The keyword "DEFAULT" sets this to ``FALSE``. ``rhoinf`` specifies the numerical damping parameter (spectral radius of the amplification matrix) in the range of :math:`[0.0,1.0]` used in the generalized-\ :math:`\alpha` time integrator implemented in BeamDyn -for dynamic analysis. For ``rhoinf = 1.0``, no -numerical damping is introduced and the generalized-\ :math:`\alpha` -scheme is identical to the Newmark scheme; for +for dynamic analysis. **Note: This parameter is only used when BeamDyn is run +in loose coupling mode (e.g., stand-alone with the driver or when loose coupling +is explicitly selected in OpenFAST). When tight coupling is used in OpenFAST, +time integration is handled by the glue code and this parameter is ignored.** +For ``rhoinf = 1.0``, no numerical damping is introduced and the +generalized-\ :math:`\alpha` scheme is identical to the Newmark scheme; for ``rhoinf = 0.0``, maximum numerical damping is introduced. Numerical damping may help produce numerically stable solutions. @@ -245,9 +252,11 @@ between two input stations into “Refine factor” of segments. The keyword This entry is not used in Gauss quadrature. ``N_Fact`` specifies a parameter used in the modified Newton-Raphson -scheme. If ``N_Fact = 1`` a full Newton -iteration scheme is used, i.e., the global tangent stiffness matrix is -computed and factorized at each iteration; if +scheme. **Note: This parameter is only used when BeamDyn is run in loose coupling +mode (stand-alone with the driver or when loose coupling is explicitly selected in +OpenFAST). When tight coupling is used in OpenFAST, this parameter is ignored.** +If ``N_Fact = 1`` a full Newton iteration scheme is used, i.e., the global tangent +stiffness matrix is computed and factorized at each iteration; if ``N_Fact > 1`` a modified Newton iteration scheme is used, i.e., the global stiffness matrix is computed and factorized every ``N_Fact`` iterations within each time step. The @@ -265,15 +274,19 @@ load steps as opposed to one single large load step which may cause divergence o Newton-Raphson scheme. The keyword “DEFAULT” sets ``load_retries = 20``. ``NRMax`` specifies the maximum number of iterations per time step in -the Newton-Raphson scheme. If convergence is not reached within this -number of iterations, BeamDyn returns an error message and terminates -the simulation. The keyword “DEFAULT” sets +the Newton-Raphson scheme. **Note: This parameter is only used when BeamDyn is run +in loose coupling mode (stand-alone with the driver or when loose coupling is explicitly +selected in OpenFAST). When tight coupling is used in OpenFAST, this parameter is ignored.** +If convergence is not reached within this number of iterations, BeamDyn returns an error +message and terminates the simulation. The keyword "DEFAULT" sets ``NRMax = 10``. ``Stop_Tol`` specifies a tolerance parameter used in convergence criteria of a nonlinear solution that is used for the termination of the -iteration. The keyword “DEFAULT” sets -``Stop_Tol = 1.0E-05``. Please refer to +iteration. **Note: This parameter is only used when BeamDyn is run in loose coupling +mode (stand-alone with the driver or when loose coupling is explicitly selected in +OpenFAST). When tight coupling is used in OpenFAST, this parameter is ignored.** +The keyword "DEFAULT" sets ``Stop_Tol = 1.0E-05``. Please refer to :numref:`convergence-criterion` for more details. ``tngt_stf_fd`` is a boolean that sets the flag to compute the tangent stiffness @@ -468,18 +481,26 @@ Blade Parameters ``Station_Total`` specifies the number cross-sectional stations along the blade axis used in the analysis. -``Damp_Type`` specifies if structural damping is considered in the -analysis. If ``Damp_Type = 0``, then no damping is -considered in the analysis and the six damping coefficient in the next -section will be ignored. If ``Damp_Type = 1``, structural -damping will be included in the analysis. +``damp_type`` specifies the type of structural damping to be used in the +analysis. There are three options: -Damping Coefficient -~~~~~~~~~~~~~~~~~~~ +- ``damp_type = 0``: No damping is considered in the analysis. The damping + coefficients in the following sections will be ignored. +- ``damp_type = 1``: Stiffness-proportional damping is applied. + The six damping coefficients (``beta``) in the Stiffness-Proportional Damping + section are used to scale the 6x6 stiffness matrix at each cross section. +- ``damp_type = 2``: Modal damping is applied. The modal damping coefficients + (``zeta``) for the first ``n_modes`` modes are used. BeamDyn internally computes + the modal properties and applies damping in the modal coordinates. + +Stiffness-Proportional Damping Coefficients +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -This section specifies six damping coefficients, :math:`\mu_{ii}` with +This section specifies six damping coefficients, :math:`\mu_{ii}` (also called ``beta``) with :math:`i \in [1,6]`, for six DOFs (three translations and three -rotations). Viscous damping is implemented in BeamDyn where the damping +rotations). These coefficients are only used when ``damp_flag = 1``. + +Viscous damping is implemented in BeamDyn where the damping forces are proportional to the strain rate. These are stiffness-proportional damping coefficients, whereby the :math:`6\times6` damping matrix at each cross section is scaled from the @@ -510,6 +531,25 @@ coefficient matrix defined as 0 & 0 & 0 & 0 & 0 & \mu_{66} \\ \end{bmatrix} +Modal Damping Coefficients +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This section specifies modal damping parameters and is only used when ``damp_flag = 2``. + +``n_modes`` specifies the number of modes for which modal damping coefficients +are provided. BeamDyn will compute the natural frequencies and mode shapes of the +blade and apply damping in the modal coordinates. + +``zeta`` is an array of ``n_modes`` modal damping ratios, one for each mode. +Each value should typically be between 0.0 (no damping) and 1.0 (critical damping), +though higher values are permitted. Common values for composite blade structures +are in the range of 0.01 to 0.05 (1% to 5% of critical damping). The damping +ratios are applied to modes 1 through ``n_modes`` in order of increasing frequency. + +If modal damping is selected, BeamDyn calculates nodal damping forces based on the node velocities, +rotated to the initial node orientation, and the mode shape after quasi-static initialization has been performed, +if it was requested. These nodal damping forces are then transformed back to the current node orientation. + Distributed Properties ~~~~~~~~~~~~~~~~~~~~~~ diff --git a/docs/source/user/beamdyn/introduction.rst b/docs/source/user/beamdyn/introduction.rst index 95e54cfc3..9cee56289 100644 --- a/docs/source/user/beamdyn/introduction.rst +++ b/docs/source/user/beamdyn/introduction.rst @@ -5,14 +5,14 @@ Introduction BeamDyn is a time-domain structural-dynamics module for slender structures created by the National Renewable Energy Laboratory (NREL) -through support from the U.S. Department of Energy Wind and Water Power +through support from the U.S. Department of Energy Wind and Water Power Program and the NREL Laboratory Directed Research and Development (LDRD) program through the grant “High-Fidelity Computational Modeling of -Wind-Turbine Structural Dynamics”, see References :cite:`Wang:SFE2013,Wang:GEBT2013,Wang:GEBT2014,Wang:2015`. +Wind-Turbine Structural Dynamics”, see References :cite:`Wang:SFE2013,Wang:GEBT2013,Wang:GEBT2014,Wang:2015`. The module has been coupled into the FAST aero-hydro-servo-elastic wind turbine multi-physics engineering tool where it used to model blade structural dynamics. The BeamDyn module follows the requirements of the -FAST modularization framework, see References :cite:`Jonkman:2013`; +FAST modularization framework, see References :cite:`Jonkman:2013`; :cite:`Sprague:2013,Sprague:2014,website:FASTModularizationFramework`, couples to FAST version 8, and provides new capabilities for modeling initially curved and twisted composite wind turbine blades undergoing @@ -64,7 +64,7 @@ outputs the blade displacements, velocities, and accelerations along the beam length, which are used by AeroDyn to calculate the local aerodynamic loads (distributed along the length) that are used as inputs for BeamDyn. In addition, BeamDyn can calculate member internal reaction -loads, as requested by the user. Please refers to Figure [fig:FlowChart] +loads, as requested by the user. Please refers to Figure [fig:FlowChart] for the coupled interactions between BeamDyn and other modules in FAST. When coupled to FAST, BeamDyn replaces the more simplified blade structural model of ElastoDyn that is still available as an option, but @@ -100,12 +100,12 @@ loads specified at FE nodes, or a combination of the two. When BeamDyn is coupled to FAST, the blade analysis node discretization may be independent between BeamDyn and AeroDyn. -This document is organized as follows. Section :ref:`running-beamdyn` details how to +This document is organized as follows. Section :ref:`running-beamdyn` details how to obtain the BeamDyn and FAST software archives and run either the stand-alone version of BeamDyn or BeamDyn coupled to FAST. -Section :ref:`bd-input-files` describes the BeamDyn input files. -Section :ref:`bd-output-files` discusses the output files generated by -BeamDyn. Section :ref:`beamdyn-theory` summarizes the BeamDyn theory. -Section :ref:`bd-future-work` outlines potential future work. Example input -files are shown in Appendix :numref:`bd_input_files`. -A summary of available output channels is found in Appendix :ref:`app-output-channel`. +Section :ref:`bd-input-files` describes the BeamDyn input files. +Section :ref:`bd-output-files` discusses the output files generated by +BeamDyn. Section :ref:`beamdyn-theory` summarizes the BeamDyn theory. +Section :ref:`bd-future-work` outlines potential future work. Example input +files are shown in Appendix :numref:`bd_input_files`. +A summary of available output channels is found in Appendix :ref:`app-output-channel`. diff --git a/docs/source/user/beamdyn/running_bd.rst b/docs/source/user/beamdyn/running_bd.rst index a2885315f..2f0d767ab 100644 --- a/docs/source/user/beamdyn/running_bd.rst +++ b/docs/source/user/beamdyn/running_bd.rst @@ -14,41 +14,10 @@ There are two forms of the BeamDyn software to choose from: stand-alone and coupled to the FAST simulator. Although the user may not necessarily need both forms, he/she would likely need to be familiar with and run the stand-alone model if building a model of the blade from scratch. The -stand-alone version is also helpful for model troubleshooting, even if +stand-alone driver is also helpful for model troubleshooting, even if the goal is to conduct aero-hydro-servo-elastic simulations of onshore/offshore wind turbines within FAST. -Stand-Alone BeamDyn Archive -~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -Users can download the stand-alone BeamDyn archive from our Web server -at https://nwtc.nrel.gov/BeamDyn. The file has a name similar to -``BD_v1.00.00a.exe``, but may have a different version number. The user -can then download the self-extracting archive (*.exe*) to expand the -archive into a folder he/she specifies. - -The archive contains the ``bin``, ``CertTest``, ``Compiling``, -``Docs``, and ``Source`` folders. The ``bin`` folder includes the -main executable file, ``BeamDyn_Driver.exe``, which is used to execute -the stand-alone BeamDyn program. The ``CertTest`` folder contains a -collection of sample BeamDyn input files and driver input files that can -be used as templates for the user’s own models. This document may be -found in the ``Docs`` folder. The ``Compiling`` folder contains files -for compiling the stand-alone ``BeamDyn_v1.00.00.exe`` file with either -Visual Studio or gFortran. The Fortran source code is located in the -``Source`` folder. - -FAST Archive -~~~~~~~~~~~~ - -Download the FAST archive, which includes BeamDyn, from our Web server -at https://nwtc.nrel.gov/FAST8. The file has a name similar to -``FAST_v8.12.00.exe``, but may have a different version number. Run the -downloaded self-extracting archive (``.exe``) to expand the archive into a -user-specified folder. The FAST executable file is located in the -archive’s ``bin`` folder. An example model using the NREL 5-MW -reference turbine is located in the ``CertTest`` folder. - Running BeamDyn --------------- @@ -56,7 +25,7 @@ Running the Stand-Alone BeamDyn Program ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ The stand-alone BeamDyn program, ``BeamDyn_Driver.exe``, simulates static -and dynamic responses of the user’s input model, without coupling to +and dynamic responses of the user's input model, without coupling to FAST. Unlike the coupled version, the stand-alone software requires the use of a driver file in addition to the primary and blade BeamDyn input files. This driver file specifies inputs normally provided to BeamDyn by @@ -78,12 +47,12 @@ file, as described in Section :ref:`driver-input-file`. Running BeamDyn Coupled to FAST ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -Run the coupled FAST software from a DOS command prompt by typing, for +Run the coupled FAST software from a command prompt by typing, for example: .. code-block:: bash - >FAST_Win32.exe Test26.fst + >openfast.exe Test26.fst where ``Test26.fst`` is the name of the primary FAST input file. This input file has a feature switch to enable or disable the BeamDyn diff --git a/docs/source/user/beamdyn/theory.rst b/docs/source/user/beamdyn/theory.rst index 78d14e693..f6c590eae 100644 --- a/docs/source/user/beamdyn/theory.rst +++ b/docs/source/user/beamdyn/theory.rst @@ -605,7 +605,32 @@ where :math:`{\underline{\underline{O}}}_{12}` and matrices of :math:`\mathcal{{\underline{\underline{O}}}}` and :math:`\mathcal{{\underline{\underline{G}}}}` as :math:`{\underline{\underline{C}}}_{12}` in Eq. :eq:`E1-PartC`. +Modal Damping +------------- +In addition to the stiffness-proportional viscous damping described above, BeamDyn +also supports modal damping. When modal damping is selected (``damp_flag = 2``), +BeamDyn computes the natural frequencies and mode shapes of the blade and applies +damping in the modal coordinates. + +The modal damping approach constructs a damping matrix :math:`{\underline{\underline{C}}}_{modal}` +based on user-specified modal damping ratios :math:`\zeta_i` for modes :math:`i = 1, 2, \ldots, n_{modes}`. +The modal damping matrix is defined in terms of the modal properties: + +.. math:: + :label: ModalDamping + + {\underline{\underline{C}}}_{modal} = \sum_{i=1}^{n_{modes}} 2 \zeta_i \omega_i \underline{\phi}_i \underline{\phi}_i^T + +where :math:`\omega_i` is the natural frequency of mode :math:`i`, :math:`\underline{\phi}_i` +is the corresponding mode shape (mass-normalized eigenvector), and :math:`\zeta_i` is the +modal damping ratio for mode :math:`i`. This damping matrix is then transformed to the +physical coordinates and applied during the time integration in the same manner as the +stiffness-proportional damping. + +The advantage of modal damping is that it allows for mode-specific damping levels, which +can be more physically representative of composite blade structures where different modes +may experience different levels of damping due to various energy dissipation mechanisms. .. _convergence-criterion: Convergence Criterion and Generalized-\ :math:`\alpha` Time Integrator @@ -637,7 +662,8 @@ Time integration is performed using the generalized-\ :math:`\alpha` scheme in BeamDyn, which is an unconditionally stable (for linear systems), second-order accurate algorithm. The scheme allows for users to choose integration parameters that introduce high-frequency numerical -dissipation. More details regarding the generalized-\ :math:`\alpha` +dissipation. The amount of numerical dissipation is controlled by the user-specified +spectral radius at infinity, :math:`\rho_{\infty}`. More details regarding the generalized-\ :math:`\alpha` method can be found in :cite:`Chung-Hulbert:1993,Bauchau:2010`. Calculation of Reaction Loads @@ -651,14 +677,14 @@ mode), the reaction loads at the root are needed to satisfy equality of the governing equations. The reaction loads at the root are also the loads passing from blade to hub in a full turbine analysis. -The governing equations in Eq. :eq:`GovernGEBT-1-2` can be recast in a compact form +The governing equations in Eq. :eq:`GovernGEBT-1-2` can be recast in a compact form .. math:: :label: CompactGovern {\underline{\mathcal{F}}}^I - {\underline{\mathcal{F}}}^{C\prime} + {\underline{\mathcal{F}}}^D = {\underline{\mathcal{F}}}^{ext} -with all the vectors defined in Section [sec:LinearProcess]. At the +with all the vectors defined in Section [sec:LinearProcess]. At the blade root, the governing equation is revised as .. math:: @@ -668,14 +694,14 @@ blade root, the governing equation is revised as where :math:`{\underline{\mathcal{F}}}^R = \left[ {\underline{F}}^R~~~{\underline{M}}^R\right]^T` is the reaction force vector and it can be solved from -Eq. :eq:`CompactGovernRoot` given that the motion fields are known at this +Eq. :eq:`CompactGovernRoot` given that the motion fields are known at this point. Calculation of Blade Loads -------------------------- BeamDyn can also calculate the blade loads at each finite element node -along the blade axis. The governing equation in Eq. :eq:`CompactGovern` are +along the blade axis. The governing equation in Eq. :eq:`CompactGovern` are recast as .. math:: From 321b8607e42b515da7c5e7141e1e4d25f55b532c Mon Sep 17 00:00:00 2001 From: Justin Porter <55218207+JustinPorter88@users.noreply.github.com> Date: Wed, 14 Jan 2026 15:51:23 -0700 Subject: [PATCH 21/24] Test of IEA 22 MW modal damping. --- reg_tests/CTestList.cmake | 1 + reg_tests/r-test | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/reg_tests/CTestList.cmake b/reg_tests/CTestList.cmake index cbd31d3e2..c6070e21e 100644 --- a/reg_tests/CTestList.cmake +++ b/reg_tests/CTestList.cmake @@ -358,6 +358,7 @@ of_regression("Tailfin_FreeYaw1DOF_Unsteady" "openfast;elastodyn;aerod of_regression("5MW_Land_DLL_WTurb_ADsk" "openfast;elastodyn;aerodisk") of_regression("5MW_Land_DLL_WTurb_ADsk_SED" "openfast;simple-elastodyn;aerodisk") of_regression("5MW_Land_DLL_WTurb_SED" "openfast;simple-elastodyn;aerodyn") +of_regression("IEA22MW_ModalDamping" "openfast;beamdyn;servodyn") of_aeromap_regression("5MW_Land_AeroMap" "aeromap;elastodyn;aerodyn") diff --git a/reg_tests/r-test b/reg_tests/r-test index f29726004..aee789b3c 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit f2972600420f2b918ebf3ac3a3d7ced71d648eba +Subproject commit aee789b3c5ab9ba2e7677baeadcf59e2e5b4a66f From d231cdbe8ea2d0e5e4c75c8a1706aa6f8f1e1107 Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Wed, 14 Jan 2026 23:16:19 +0000 Subject: [PATCH 22/24] Add modal damping input recommendations --- docs/source/user/beamdyn/input_files.rst | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/docs/source/user/beamdyn/input_files.rst b/docs/source/user/beamdyn/input_files.rst index 26d5ecbfa..04b55ace4 100644 --- a/docs/source/user/beamdyn/input_files.rst +++ b/docs/source/user/beamdyn/input_files.rst @@ -550,6 +550,13 @@ If modal damping is selected, BeamDyn calculates nodal damping forces based on t rotated to the initial node orientation, and the mode shape after quasi-static initialization has been performed, if it was requested. These nodal damping forces are then transformed back to the current node orientation. +Recommendations: + +- It is recommended to stop inputting zeta values before reaching the first axial mode (typically around 18 modes). Users may experiment with including more or fewer modes to observe the effect on results. +- Avoid prescribing a final zeta value of 1.0 (e.g., do not specify 18 modes with realistic zeta values followed by a 19th mode with zeta=1.0), as this can significantly degrade result quality. +- When attempting to match stiffness-proportional damping (:math:`\mu`), the OpenFAST toolbox may fail to provide reliable damping values matched with mode numbers once some modes become critically damped. Reducing the number of modes (e.g., from 40 to 30) can help if higher modes are indexed incorrectly. +- In some cases, axial loads appear to be driven by the axial motion of non-axial modes. + Distributed Properties ~~~~~~~~~~~~~~~~~~~~~~ From 986bc16fd746846de5a623f2fb4ddd5bbe984d7e Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Wed, 14 Jan 2026 23:22:00 +0000 Subject: [PATCH 23/24] Update r-test pointer --- reg_tests/r-test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/reg_tests/r-test b/reg_tests/r-test index aee789b3c..37cc2beb1 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit aee789b3c5ab9ba2e7677baeadcf59e2e5b4a66f +Subproject commit 37cc2beb170b521178cff3c2e2347c0d48555329 From 4c540b8ef66e29cb1a555f8bdd0861ecd7ea30a9 Mon Sep 17 00:00:00 2001 From: Derek Slaughter Date: Wed, 14 Jan 2026 18:23:27 -0500 Subject: [PATCH 24/24] Put PitchDOF in ED in the rotating frame for linearization --- modules/elastodyn/src/ElastoDyn.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/elastodyn/src/ElastoDyn.f90 b/modules/elastodyn/src/ElastoDyn.f90 index 7a0298b38..6eb40490d 100644 --- a/modules/elastodyn/src/ElastoDyn.f90 +++ b/modules/elastodyn/src/ElastoDyn.f90 @@ -11114,7 +11114,7 @@ subroutine ED_InitVars(u, p, x, y, m, Vars, InputFileData, Linearize, ErrStat, E do i = 1, p%NumBl call MV_AddVar(Vars%x, 'Blade'//trim(Num2LStr(i))//'Pitch', FieldAngularDisp, & DL=DatLoc(ED_x_QT), iAry=DOF_BP(i), & - Flags=VF_DerivOrder2, & + Flags=ior(VF_RotFrame, VF_DerivOrder2), & Perturb=2.0_R8Ki * D2R_D, & LinNames=['Blade pitch DOF (internal DOF index = DOF_BP('//trim(Num2LStr(i))//')), rad'], & Active=InputFileData%PitchDOF)