diff --git a/modules/subdyn/src/SD_FEM.f90 b/modules/subdyn/src/SD_FEM.f90 index f2284d8ba8..27e72964b0 100644 --- a/modules/subdyn/src/SD_FEM.f90 +++ b/modules/subdyn/src/SD_FEM.f90 @@ -1249,6 +1249,9 @@ SUBROUTINE AssembleKM(Init, p, ErrStat, ErrMsg) ENDDO ! Add concentrated mass to mass matrix + CALL AllocAry( p%CMassNode, Init%nCMass, 'p%CMassNode', ErrStat2, ErrMsg2); if(Failed()) return; + CALL AllocAry( p%CMassWeight, Init%nCMass, 'p%CMassWeight', ErrStat2, ErrMsg2); if(Failed()) return; + CALL AllocAry( p%CMassOffset, Init%nCMass, 3, 'p%CMassOffset', ErrStat2, ErrMsg2); if(Failed()) return; DO I = 1, Init%nCMass iNode = NINT(Init%CMass(I, 1)) ! Note index where concentrated mass is to be added ! Safety check (otherwise we might have more than 6 DOF) @@ -1271,14 +1274,20 @@ SUBROUTINE AssembleKM(Init, p, ErrStat, ErrMsg) Init%M(jGlob, kGlob) = Init%M(jGlob, kGlob) + M66(J,K) ENDDO ENDDO - ENDDO ! Loop on concentrated mass - ! Add concentrated mass induced gravity force - DO I = 1, Init%nCMass - iNode = NINT(Init%CMass(I, 1)) ! Note index where concentrated mass is to be added - iGlob = p%NodesDOF(iNode)%List(3) ! uz - p%FG(iGlob) = p%FG(iGlob) - Init%CMass(I, 2)*Init%g - ENDDO + ! Add concentrated mass contribution to gravity force and moment + iGlob = p%NodesDOF(iNode)%List(3); p%FG(iGlob) = p%FG(iGlob) - m*Init%g ! uz: -mg + iGlob = p%NodesDOF(iNode)%List(4); p%FG(iGlob) = p%FG(iGlob) - m*Init%g * y ! tx: -mgy + iGlob = p%NodesDOF(iNode)%List(5); p%FG(iGlob) = p%FG(iGlob) + m*Init%g * x ! ty: mgx + + ! Save concentrated mass information for GuyanLoadCorrection + p%CMassNode(I) = iNode + p%CMassWeight(I) = m*Init%g + p%CMassOffset(I,1) = x + p%CMassOffset(I,2) = y + p%CMassOffset(I,3) = z + + ENDDO ! Loop on concentrated mass CALL CleanUp_AssembleKM() diff --git a/modules/subdyn/src/SubDyn.f90 b/modules/subdyn/src/SubDyn.f90 index e10cf1a022..eff2ca3a23 100644 --- a/modules/subdyn/src/SubDyn.f90 +++ b/modules/subdyn/src/SubDyn.f90 @@ -914,7 +914,7 @@ SUBROUTINE SD_Input(SDInputFile, Init, p, ErrStat,ErrMsg) CHARACTER(64), ALLOCATABLE :: StrArray(:) ! Array of strings, for better control of table inputs LOGICAL :: Echo LOGICAL :: LegacyFormat -LOGICAL :: bNumeric, bInteger +LOGICAL :: bNumeric, bInteger, bCableHasPretension INTEGER(IntKi) :: UnIn INTEGER(IntKi) :: nColumns, nColValid, nColNumeric INTEGER(IntKi) :: IOS @@ -1297,6 +1297,7 @@ SUBROUTINE SD_Input(SDInputFile, Init, p, ErrStat,ErrMsg) CALL ReadCom ( UnIn, SDInputFile, 'Cable properties Unit ' ,ErrStat2, ErrMsg2, UnEc ); if(Failed()) return IF (Check( Init%NPropSetsC < 0, 'NPropSetsCable must be >=0')) return CALL AllocAry(Init%PropSetsC, Init%NPropSetsC, PropSetsCCol, 'PropSetsC', ErrStat2, ErrMsg2); if(Failed()) return + bCableHasPretension = .false. DO I = 1, Init%NPropSetsC !CALL ReadAry( UnIn, SDInputFile, Init%PropSetsC(I,:), PropSetsCCol, 'PropSetsC', 'PropSetsC ID and values ', ErrStat2, ErrMsg2, UnEc ); if(Failed()) return READ(UnIn, FMT='(A)', IOSTAT=ErrStat2) Line; ErrMsg2='Error reading cable property line'; if (Failed()) return @@ -1309,7 +1310,18 @@ SUBROUTINE SD_Input(SDInputFile, Init, p, ErrStat,ErrMsg) call LegacyWarning('Using 4 values instead of 5 for cable properties. Cable will have constant properties and wont be controllable.') Init%PropSetsC(:,5:PropSetsCCol)=0 ! No CtrlChannel endif + if (Init%PropSetsC(I,4)>0.0) then + bCableHasPretension = .true. + end if ENDDO + if (bCableHasPretension) then + call WrScr('!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!') + call WrScr('Warning: Cable with non-zero pretension specified.') + call WrScr(' SubDyn currently does not account for geometric stiffness from pretension.' ) + call WrScr(' Avoid non-zero cable pretension if possible.' ) + call WrScr('!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!') + end if + !----------------------- RIGID LINK PROPERTIES ------------------------------------ CALL ReadCom ( UnIn, SDInputFile, 'Rigid link properties' ,ErrStat2, ErrMsg2, UnEc ); if(Failed()) return CALL ReadIVar ( UnIn, SDInputFile, Init%NPropSetsR, 'NPropSetsR', 'Number of rigid link properties' ,ErrStat2, ErrMsg2, UnEc ); if(Failed()) return @@ -3255,7 +3267,7 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC real(ReKi) :: CableTension ! Controllable Cable force real(ReKi) :: DeltaL ! Change of length real(ReKi) :: rotations(3) - real(ReKi) :: du(3), Moment(3), Force(3) + real(ReKi) :: du(3), Moment(3), Force(3), CMassOffset(3), CMassWeight(3) real(ReKi) :: u_TP(6) real(FEKi) :: FGe(12) ! element gravity force vector ! Variables for Guyan Rigid motion @@ -3264,6 +3276,10 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC real(ReKi), dimension(3) :: duP ! Displacement of node due to rigid rotation real(R8Ki), dimension(3,3) :: Rb2g ! Rotation matrix body 2 global real(R8Ki), dimension(3,3) :: Rg2b ! Rotation matrix global 2 body coordinates + real(ReKi), dimension(3,3) :: orientation ! Nodal orientation matrix + + INTEGER(IntKi) :: ErrStat2 ! Error status of the operation + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat /= ErrID_None ErrStat = ErrID_None ErrMsg = "" @@ -3327,9 +3343,9 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC ! --- Build vector of external moment ! For floating structure with potentially large Guyan (rigid-body) rotation, nodal self-weight needs to be recomputed based on the current rigid-body orientation m%FG = 0.0_R8Ki - if ( RotateLoads ) then + if ( RotateLoads ) then ! if and only if floating Rb2g = transpose(Rg2b) ! Body (Guyan) to global - do i = 1, size(p%ElemProps) + do i = 1, size(p%ElemProps) ! Loop through all elements ! --- Element Fg in the earth-fixed frame CALL ElemG(p%ElemProps(i)%Area, p%ElemProps(i)%Length, p%ElemProps(i)%Rho, matmul(Rb2g,p%ElemProps(i)%DirCos), FGe, p%g) ! --- Element Fg in the Guyan rigid-body frame @@ -3341,6 +3357,25 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC IDOF = p%ElemsDOF(1:12,i) m%FG( IDOF ) = m%FG( IDOF ) + FGe(1:12) end do + do i = 1,size(p%CMassNode) ! Loop through all concentrated masses + iNode = p%CMassNode(i) + IDOF(1:6) = p%NodesDOF(iNode)%List(1:6) + CMassOffset = p%CMassOffset(i,:) + CMassWeight = matmul(Rg2b, (/0.0,0.0,-p%CMassWeight(i)/) ) + m%FG(IDOF(1:3)) = m%FG(IDOF(1:3)) + CMassWeight + m%FG(IDOF(4:6)) = m%FG(IDOF(4:6)) + cross_product(CMassOffset,CMassWeight) + end do + end if + + if (GuyanLoadCorrection) then ! if and only if fixed-bottom + ! Additional GuyanLoadCorrection coming from the weight of concentrated masses with CoG offset + do i = 1,size(p%CMassNode) ! Loop through all concentrated masses + iNode = p%CMassNode(i) + IDOF(4:6) = p%NodesDOF(iNode)%List(4:6) + call SmllRotTrans('Nodal rotation',m%DU_full(IDOF(4)),m%DU_full(IDOF(5)),m%DU_full(IDOF(6)),orientation,'',ErrStat2,ErrMsg2); if(Failed()) return + CMassOffset = matmul(p%CMassOffset(i,:),orientation) + m%Fext(IDOF(4:6)) = m%Fext(IDOF(4:6)) + cross_product( CMassOffset-p%CMassOffset(i,:), (/0.0,0.0,-p%CMassWeight(i)/) ) + end do end if do iNode = 1,p%nNodes @@ -3355,7 +3390,7 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC endif ! Extra moment dm = Delta u x (fe + fg) - if (GuyanLoadCorrection) then + if (GuyanLoadCorrection) then ! if and only if fixed-bottom du = m%DU_full(p%NodesDOF(iNode)%List(1:3)) ! Lever arm Moment(1) = Moment(1) + du(2) * Force(3) - du(3) * Force(2) Moment(2) = Moment(2) + du(3) * Force(1) - du(1) * Force(3) @@ -3380,8 +3415,14 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC contains subroutine Fatal(ErrMsg_in) character(len=*), intent(in) :: ErrMsg_in - call SetErrStat(ErrID_Fatal, ErrMsg_in, ErrStat, ErrMsg, 'GetExtForce'); + call SetErrStat(ErrID_Fatal, ErrMsg_in, ErrStat, ErrMsg, 'GetExtForceOnInternalDOF'); end subroutine Fatal + + logical function Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'GetExtForceOnInternalDOF') + Failed = ErrStat >= AbortErrLev + end function Failed + END SUBROUTINE GetExtForceOnInternalDOF !------------------------------------------------------------------------------------------------------ diff --git a/modules/subdyn/src/SubDyn_Registry.txt b/modules/subdyn/src/SubDyn_Registry.txt index 1fb738e12d..80af480766 100644 --- a/modules/subdyn/src/SubDyn_Registry.txt +++ b/modules/subdyn/src/SubDyn_Registry.txt @@ -207,10 +207,13 @@ typedef ^ ParameterType IntKi Nmembers - - - "Number of mem typedef ^ ParameterType IntKi Elems {:}{:} - - "Element nodes connections" typedef ^ ParameterType ElemPropType ElemProps {:} - - "List of element properties" typedef ^ ParameterType R8Ki FC {:} - - "Initial cable force T0, not reduced" N -typedef ^ ParameterType R8Ki FG {:} - - "Gravity force vector (with initial cable force T0), not reduced" N +typedef ^ ParameterType R8Ki FG {:} - - "Gravity force vector, not reduced" N typedef ^ ParameterType ReKi DP0 {:}{:} - - "Vector from TP to a Node at t=0, used for Floating Rigid Body motion" m typedef ^ ParameterType ReKi rPG {:} - - "Vector from TP to rigid-body CoG in the Guyan (rigid-body) frame, used for Floating Rigid Body Motion" m typedef ^ ParameterType IntKi NodeID2JointID {:} - - "Store Joint ID for each NodeID since SubDyn re-label nodes (and add more nodes)" "-" +typedef ^ ParameterType IntKi CMassNode {:} - - "Node indices for concentrated masses" +typedef ^ ParameterType ReKi CMassWeight {:} - - "Weight of concentrated masses" N +typedef ^ ParameterType ReKi CMassOffset {:}{:} - - "Concentrated mass CoG offset from attached nodes" m # --- Parameters - Constraints reduction typedef ^ ParameterType Logical reduced - - - "True if system has been reduced to account for constraints" "-" typedef ^ ParameterType R8Ki T_red {:}{:} - - "Transformation matrix performing the constraint reduction x = T. xtilde" "-" diff --git a/modules/subdyn/src/SubDyn_Types.f90 b/modules/subdyn/src/SubDyn_Types.f90 index 7b8a836d20..c24237f6b9 100644 --- a/modules/subdyn/src/SubDyn_Types.f90 +++ b/modules/subdyn/src/SubDyn_Types.f90 @@ -256,10 +256,13 @@ MODULE SubDyn_Types INTEGER(IntKi) , DIMENSION(:,:), ALLOCATABLE :: Elems !< Element nodes connections [-] TYPE(ElemPropType) , DIMENSION(:), ALLOCATABLE :: ElemProps !< List of element properties [-] REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: FC !< Initial cable force T0, not reduced [N] - REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: FG !< Gravity force vector (with initial cable force T0), not reduced [N] + REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: FG !< Gravity force vector, not reduced [N] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: DP0 !< Vector from TP to a Node at t=0, used for Floating Rigid Body motion [m] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: rPG !< Vector from TP to rigid-body CoG in the Guyan (rigid-body) frame, used for Floating Rigid Body Motion [m] INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: NodeID2JointID !< Store Joint ID for each NodeID since SubDyn re-label nodes (and add more nodes) [-] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: CMassNode !< Node indices for concentrated masses [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: CMassWeight !< Weight of concentrated masses [N] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: CMassOffset !< Concentrated mass CoG offset from attached nodes [m] LOGICAL :: reduced = .false. !< True if system has been reduced to account for constraints [-] REAL(R8Ki) , DIMENSION(:,:), ALLOCATABLE :: T_red !< Transformation matrix performing the constraint reduction x = T. xtilde [-] REAL(R8Ki) , DIMENSION(:,:), ALLOCATABLE :: T_red_T !< Transpose of T_red [-] @@ -2637,6 +2640,42 @@ subroutine SD_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg) end if DstParamData%NodeID2JointID = SrcParamData%NodeID2JointID end if + if (allocated(SrcParamData%CMassNode)) then + LB(1:1) = lbound(SrcParamData%CMassNode, kind=B8Ki) + UB(1:1) = ubound(SrcParamData%CMassNode, kind=B8Ki) + if (.not. allocated(DstParamData%CMassNode)) then + allocate(DstParamData%CMassNode(LB(1):UB(1)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%CMassNode.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstParamData%CMassNode = SrcParamData%CMassNode + end if + if (allocated(SrcParamData%CMassWeight)) then + LB(1:1) = lbound(SrcParamData%CMassWeight, kind=B8Ki) + UB(1:1) = ubound(SrcParamData%CMassWeight, kind=B8Ki) + if (.not. allocated(DstParamData%CMassWeight)) then + allocate(DstParamData%CMassWeight(LB(1):UB(1)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%CMassWeight.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstParamData%CMassWeight = SrcParamData%CMassWeight + end if + if (allocated(SrcParamData%CMassOffset)) then + LB(1:2) = lbound(SrcParamData%CMassOffset, kind=B8Ki) + UB(1:2) = ubound(SrcParamData%CMassOffset, kind=B8Ki) + if (.not. allocated(DstParamData%CMassOffset)) then + allocate(DstParamData%CMassOffset(LB(1):UB(1),LB(2):UB(2)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%CMassOffset.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstParamData%CMassOffset = SrcParamData%CMassOffset + end if DstParamData%reduced = SrcParamData%reduced if (allocated(SrcParamData%T_red)) then LB(1:2) = lbound(SrcParamData%T_red, kind=B8Ki) @@ -3388,6 +3427,15 @@ subroutine SD_DestroyParam(ParamData, ErrStat, ErrMsg) if (allocated(ParamData%NodeID2JointID)) then deallocate(ParamData%NodeID2JointID) end if + if (allocated(ParamData%CMassNode)) then + deallocate(ParamData%CMassNode) + end if + if (allocated(ParamData%CMassWeight)) then + deallocate(ParamData%CMassWeight) + end if + if (allocated(ParamData%CMassOffset)) then + deallocate(ParamData%CMassOffset) + end if if (allocated(ParamData%T_red)) then deallocate(ParamData%T_red) end if @@ -3616,6 +3664,9 @@ subroutine SD_PackParam(RF, Indata) call RegPackAlloc(RF, InData%DP0) call RegPackAlloc(RF, InData%rPG) call RegPackAlloc(RF, InData%NodeID2JointID) + call RegPackAlloc(RF, InData%CMassNode) + call RegPackAlloc(RF, InData%CMassWeight) + call RegPackAlloc(RF, InData%CMassOffset) call RegPack(RF, InData%reduced) call RegPackAlloc(RF, InData%T_red) call RegPackAlloc(RF, InData%T_red_T) @@ -3794,6 +3845,9 @@ subroutine SD_UnPackParam(RF, OutData) call RegUnpackAlloc(RF, OutData%DP0); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%rPG); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%NodeID2JointID); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%CMassNode); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%CMassWeight); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%CMassOffset); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%reduced); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%T_red); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%T_red_T); if (RegCheckErr(RF, RoutineName)) return