Merge pull request #2334 from RyanDavies19/body_kinematics

Updating MD Body Kinematics

modules/moordyn/src/MoorDyn.f90

@@ -2127,7 +2127,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er
             J = J + 1
 
             rRef = m%BodyList(m%CpldBodyIs(l,iTurb))%r6  ! set reference position as per input file
-            OrMatRef = ( m%RodList(m%CpldBodyIs(l,iTurb))%OrMat )  ! set reference orientation as per input file
+            OrMatRef = ( m%BodyList(m%CpldBodyIs(l,iTurb))%OrMat )  ! set reference orientation as per input file
             CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2, OrMatRef)
 
             ! set absolute initial positions in MoorDyn 
@@ -2139,7 +2139,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er
             u%CoupledKinematics(iTurb)%TranslationDisp(2,J) = InitInp%PtfmInit(2,iTurb) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2)
             u%CoupledKinematics(iTurb)%TranslationDisp(3,J) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3)
             m%BodyList(m%CpldBodyIs(l,iTurb))%r6(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb)
-            m%BodyList(m%CpldBodyIs(l,iTurb))%r6(4:6) = EulerExtract(OrMat2)     ! apply rotation from PtfmInit onto input file's body orientation to get its true initial orientation
+            m%BodyList(m%CpldBodyIs(l,iTurb))%r6(4:6) = EulerExtract( TRANSPOSE(OrMat2) )   ! apply rotation from PtfmInit onto input file's body orientation to get its true initial orientation
 
             CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J)      ! set node as point element
 
@@ -3344,8 +3344,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er
          DO l = 1,p%nCpldBodies(iTurb)
             J = J + 1
             r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb)
-            !r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics(iTurb)%Orientation(:,:,J) ) )
-            r6_in(4:6) = EulerExtract( u%CoupledKinematics(iTurb)%Orientation(:,:,J) )   ! <<< changing back
+            r6_in(4:6) = EulerExtract( u%CoupledKinematics(iTurb)%Orientation(:,:,J) )   ! No Transpose becasue these are extrinsic
             v6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationVel(:,J)
             v6_in(4:6) = u%CoupledKinematics(iTurb)%RotationVel(:,J)
             a6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J)

modules/moordyn/src/MoorDyn_Body.f90

@@ -439,6 +439,10 @@ SUBROUTINE Body_DoRHS(Body, m, p)
       Real(DbKi)                 :: cda(6)             ! body drag coefficients
       Real(DbKi)                 :: cda_t(3,3) = 0.0         ! matrix with translational drag coefficients as diagonals
       Real(DbKi)                 :: cda_r(3,3) = 0.0         ! matrix with rotational drag coefficients as diagonals
+      Real(DbKi)                 :: w(3)                     ! body angular velocity vector
+      Real(DbKi)                 :: Fcentripetal(3)        ! centripetal force
+      Real(DbKi)                 :: Mcentripetal(3)        ! centripetal moment     
+
 
       ! Initialize variables
       U = 0.0_DbKi      ! Set to zero for now
@@ -456,6 +460,13 @@ SUBROUTINE Body_DoRHS(Body, m, p)
       body_rCGrotated = MATMUL(Body%OrMat, Body%rCG) ! rotateVector3(body_rCG, OrMat, body_rCGrotated); ! relative vector to body CG in inertial orientation
       CALL translateForce3to6DOF(body_rCGrotated, Fgrav, Body%F6net)  ! gravity forces and moments about body ref point given CG location
 
+      ! Centripetal force and moment due to COM not being at body origin plus gyroscopic moment
+      w = Body%v6(4:6)
+      Fcentripetal = - MATMUL(Body%M(1:3,1:3), CROSS_PRODUCT(w, CROSS_PRODUCT(w, body_rCGrotated)))
+      Mcentripetal = - CROSS_PRODUCT(w, MATMUL(Body%M(4:6,4:6), w)) 
+
+      Body%F6net(1:3) = Body%F6net(1:3) + Fcentripetal
+      Body%F6net(4:6) = Body%F6net(4:6) + Mcentripetal
 
       ! --------------------------------- apply wave kinematics ------------------------------------
       !env->waves->getU(r6, t, U); ! call generic function to get water velocities <<<<<<<<< all needs updating
@@ -487,7 +498,7 @@ SUBROUTINE Body_DoRHS(Body, m, p)
       do l = 1,Body%nAttachedC
 
          ! get net force and mass from Point on body ref point (global orientation)
-         CALL Point_GetNetForceAndMass( m%PointList(Body%attachedC(l)), Body%r6(1:3), F6_i, M6_i, m, p)
+         CALL Point_GetNetForceAndMass( m%PointList(Body%attachedC(l)), Body%r6(1:3), Body%v6(4:6), F6_i, M6_i, m, p)
 
          if (ABS(F6_i(5)) > 1.0E12) then
             Call WrScr( "Warning: extreme pitch moment from body-attached Point "//trim(num2lstr(l)))
@@ -502,7 +513,7 @@ SUBROUTINE Body_DoRHS(Body, m, p)
       do l=1,Body%nAttachedR
 
          ! get net force and mass from Rod on body ref point (global orientation)
-         CALL Rod_GetNetForceAndMass(m%RodList(Body%attachedR(l)), Body%r6(1:3), F6_i, M6_i, m, p)
+         CALL Rod_GetNetForceAndMass(m%RodList(Body%attachedR(l)), Body%r6(1:3), Body%v6(4:6), F6_i, M6_i, m, p)
 
          if (ABS(F6_i(5)) > 1.0E12) then
             Call WrScr("Warning: extreme pitch moment from body-attached Rod "//trim(num2lstr(l)))

modules/moordyn/src/MoorDyn_Driver.f90

@@ -632,7 +632,19 @@ PROGRAM MoorDyn_Driver
       end if   ! InputsMod == 1 
 
       ! >>> otherwise, mesh kinematics should all still be zero ... maybe worth checking <<<
-
+
+      ! ! set free body state for kinematics debugging
+      ! if (i==1) then
+      !    DO l = 1,MD_p%nFreeBodies
+      !       IF (l==1) THEN 
+      !          MD_x%states(MD_m%BodyStateIs1(l):MD_m%BodyStateIsN(l)) = [0.0, 0.0, 0.0, 0.2, 0.0, 0.0,  0.0, 0.0, -2.0, 0.0, 0.0, 0.0]
+      !          print*, "vel set for body1"
+      !       ELSEIF (l==2) THEN
+      !          MD_x%states(MD_m%BodyStateIs1(l):MD_m%BodyStateIsN(l)) = [0.0, 0.0, 10.0*0.2, 0.2, 0.0, 0.0,  0.0, 10.0, -2.0, 0.0, 0.0, 0.0]
+      !          print*, "vel set for body2"
+      !       ENDIF
+      !    ENDDO
+      ! endif
 
       ! --------------------------------- update states ---------------------------------
       CALL MD_UpdateStates( t, nt, MD_u, MD_uTimes, MD_p, MD_x, MD_xd, MD_xc, MD_xo, MD_m, ErrStat2, ErrMsg2 ); call AbortIfFailed()

modules/moordyn/src/MoorDyn_Line.f90

@@ -161,7 +161,7 @@ SUBROUTINE SetupLine (Line, LineProp, p, ErrStat, ErrMsg)
       END IF
 
       ! allocate segment scalar quantities
-      ALLOCATE ( Line%l(N), Line%ld(N), Line%lstr(N), Line%lstrd(N), Line%Kurv(0:N), Line%V(N), STAT = ErrStat )
+      ALLOCATE ( Line%l(N), Line%ld(N), Line%lstr(N), Line%lstrd(N), Line%Kurv(0:N), Line%V(N), Line%F(N), STAT = ErrStat )
       IF ( ErrStat /= ErrID_None ) THEN
          ErrMsg  = ' Error allocating segment scalar quantity arrays.'
          !CALL CleanUp()
@@ -1032,6 +1032,8 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p)  !, FairFtot, FairMtot, AnchFtot,
       Real(DbKi)                       :: Vi(3)          ! relative water velocity at a given node
       Real(DbKi)                       :: Vp(3)          ! transverse relative water velocity component at a given node
       Real(DbKi)                       :: Vq(3)          ! tangential relative water velocity component at a given node
+      Real(DbKi)                       :: ap(3)          ! transverse fluid acceleration component at a given node
+      Real(DbKi)                       :: aq(3)          ! tangential fluid acceleration component at a given node
       Real(DbKi)                       :: SumSqVp        !
       Real(DbKi)                       :: SumSqVq        !
       Real(DbKi)                       :: MagVp          !
@@ -1044,6 +1046,15 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p)  !, FairFtot, FairMtot, AnchFtot,
       Real(DbKi)                       :: ld_1           ! rate of change of static stiffness portion of segment [m/s]
       Real(DbKi)                       :: EA_1           ! stiffness of 'static stiffness' portion of segment, combines with dynamic stiffness to give static stiffnes [m/s]
 
+      REAL(DbKi)                       :: surface_height ! Average the surface heights at the two nodes
+      REAL(DbKi)                       :: firstNodeZ     ! Difference of first node depth from surface height
+      REAL(DbKi)                       :: secondNodeZ    ! Difference of second node depth from surface height
+      REAL(DbKi)                       :: lowerEnd(3)    ! XYZ location of lower segment end   
+      REAL(DbKi)                       :: upperEnd(3)    ! XYZ location of upper segment end
+      REAL(DbKi)                       :: segmentAxis(3) ! Vector from segment lower end to upper end
+      REAL(DbKi)                       :: upVec(3)       ! Universal up unit vector = (0,0,1)
+      REAL(DbKi)                       :: normVec(3)     ! Normal vector to segment
+
       Real(DbKi)                       :: Kurvi          ! temporary curvature value [1/m]
       Real(DbKi)                       :: pvec(3)        ! the p vector used in bending stiffness calcs
       Real(DbKi)                       :: Mforce_im1(3)  ! force vector for a contributor to the effect of a bending moment [N]
@@ -1122,18 +1133,80 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p)  !, FairFtot, FairMtot, AnchFtot,
          CALL getWaterKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, m%WaveTi, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i))
       END DO
 
+      ! --------- calculate line partial submergence (Line::calcSubSeg from MD-C) ---------
+      DO i=1,N
+
+         Line%F(i) = 1.0_DbKi
+
+         ! TODO: Is the below the right way to handle partial submergence
+
+         ! ! TODO - figure out the best math to do here
+         ! ! Averaging the surface heights at the two nodes is probably never
+         ! ! correct
+         ! surface_height = 0.5 * (Line%zeta(i-1) + Line%zeta(i)) 
+
+         ! ! The below could also be made an inline function with surface height and node indicies as inputs, same as MD-C
+         ! firstNodeZ = Line%r(3,i-1) - surface_height 
+         ! secondNodeZ = Line%r(3,i) - surface_height 
+         ! if ((firstNodeZ <= 0.0) .AND. (secondNodeZ < 0.0)) then
+         !    Line%F(i) = 1.0 ! Both nodes below water; segment must be too
+         ! else if ((firstNodeZ > 0.0) .AND. (secondNodeZ > 0.0)) then
+         !    Line%F(i) = 0.0 ! Both nodes above water; segment must be too
+         ! else if (firstNodeZ == -secondNodeZ) then
+         !    Line%F(i) = 0.5 ! Segment halfway submerged
+         ! else 
+         !    ! Segment partially submerged - figure out which node is above water
+         !    if (firstNodeZ < 0.0) then
+         !       lowerEnd = Line%r(:,i-1)
+         !    else
+         !       lowerEnd = Line%r(:,i) 
+         !    endif
+         !    if (firstNodeZ < 0.0) then
+         !       upperEnd = Line%r(:,i)
+         !    else   
+         !       upperEnd = Line%r(:,i-1)
+
+         !    endif
+         !    lowerEnd(3) = lowerEnd(3) - surface_height
+         !    upperEnd(3) = upperEnd(3) - surface_height
+
+         !    ! segment submergence is calculated by calculating submergence of
+         !    ! hypotenuse across segment from upper corner to lower corner
+         !    ! To calculate this, we need the coordinates of these corners.
+         !    ! first step is to get vector from lowerEnd to upperEnd
+         !    segmentAxis = upperEnd - lowerEnd
+
+         !    ! Next, find normal vector in z-plane, i.e. the normal vecto that
+         !    ! points "up" the most. See the following stackexchange:
+         !    ! https://math.stackexchange.com/questions/2283842/
+         !    upVec = (/0.0_DbKi, 0.0_DbKi, 1.0_DbKi/) ! the global up-unit vector 
+         !    normVec = Cross_Product(segmentAxis, (Cross_Product(upVec, segmentAxis)))
+         !    normVec = normVec / SQRT(normVec(1)**2+normVec(2)**2+normVec(3)**2) ! normalize
+
+         !    ! make sure normal vector has length equal to radius of segment
+         !    call scalevector(normVec, Line%d / 2, normVec)
+
+         !    ! Calculate and return submerged ratio:
+         !    lowerEnd = lowerEnd - normVec
+         !    upperEnd = upperEnd + normVec
+
+         !    Line%F(i) = abs(lowerEnd(3)) / (abs(lowerEnd(3)) + upperEnd(3))
+
+         ! endif
+
+      END DO
 
       ! --------------- calculate mass (including added mass) matrix for each node -----------------
       DO I = 0, N
          IF (I==0) THEN
             m_i = Pi/8.0 *d*d*Line%l(1)*rho
-            v_i = 0.5 *Line%V(1)
+            v_i = 0.5 * Line%F(1) * Line%V(1)
          ELSE IF (I==N) THEN
             m_i = pi/8.0 *d*d*Line%l(N)*rho;
-            v_i = 0.5*Line%V(N)
+            v_i = 0.5 * Line%F(N) * Line%V(N)
          ELSE
             m_i = pi/8.0 * d*d*rho*(Line%l(I) + Line%l(I+1))
-            v_i = 0.5 *(Line%V(I) + Line%V(I+1))
+            v_i = 0.5 *(Line%F(I) * Line%V(I) + Line%F(I+1) * Line%V(I+1))
          END IF
 
          DO J=1,3
@@ -1315,11 +1388,11 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p)  !, FairFtot, FairMtot, AnchFtot,
 
          !submerged weight (including buoyancy)
          IF (I==0) THEN
-            Line%W(3,I) = Pi/8.0*d*d* Line%l(1)*(rho - p%rhoW) *(-p%g)   ! assuming g is positive
+            Line%W(3,I) = Pi/8.0*d*d* Line%l(1)*(rho - Line%F(1) * p%rhoW) *(-p%g)   ! assuming g is positive
          ELSE IF (i==N)  THEN
-            Line%W(3,I) = pi/8.0*d*d* Line%l(N)*(rho - p%rhoW) *(-p%g)
+            Line%W(3,I) = pi/8.0*d*d* Line%l(N)*(rho - Line%F(N) * p%rhoW) *(-p%g)
          ELSE
-            Line%W(3,I) = pi/8.0*d*d* (Line%l(I)*(rho - p%rhoW) + Line%l(I+1)*(rho - p%rhoW) )*(-p%g)  ! left in this form for future free surface handling
+            Line%W(3,I) = pi/8.0*d*d* (Line%l(I)*(rho - Line%F(I) * p%rhoW) + Line%l(I+1)*(rho - Line%F(I+1) * p%rhoW) )*(-p%g)  ! left in this form for future free surface handling
          END IF
 
          ! relative flow velocities
@@ -1341,17 +1414,32 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p)  !, FairFtot, FairMtot, AnchFtot,
 
          ! transverse and tangenential drag
          IF (I==0) THEN
-            Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn*    d*Line%l(1) * MagVp * Vp
-            Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*Line%l(1) * MagVq * Vq
+            Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn*    d*Line%F(1)*Line%l(1) * MagVp * Vp
+            Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*Line%F(1)*Line%l(1) * MagVq * Vq
          ELSE IF (I==N)  THEN
-            Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn*    d*Line%l(N) * MagVp * Vp
-            Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*Line%l(N) * MagVq * Vq
+            Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn*    d*Line%F(N)*Line%l(N) * MagVp * Vp
+            Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*Line%F(N)*Line%l(N) * MagVq * Vq
          ELSE
-            Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn*    d*(Line%l(I) + Line%l(I+1)) * MagVp * vp
-            Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*(Line%l(I) + Line%l(I+1)) * MagVq * vq
+            Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn*    d*(Line%F(I)*Line%l(I) + Line%F(I+1)*Line%l(I+1)) * MagVp * Vp
+            Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*(Line%F(I)*Line%l(I) + Line%F(I+1)*Line%l(I+1)) * MagVq * Vq
          END IF
 
-         ! F-K force from fluid acceleration not implemented yet
+         ! ------ fluid acceleration components for current node (from MD-C) ------
+         DO J = 1, 3
+            aq(J) = DOT_PRODUCT( Line%Ud(:,I) , Line%q(:,I) ) * Line%q(J,I);   ! tangential fluid acceleration component
+            ap(J) = Line%Ud(J,I) - aq(J)                                    ! transverse fluid acceleration component
+         ENDDO
+
+         if (I == 0) then
+            Line%Ap(:,I) = p%rhoW * (1. + Line%Can) * 0.5 * (Line%F(1)* Line%V(1)) * ap
+            Line%Aq(:,I) = p%rhoW * (1. + Line%Cat) * 0.5 * (Line%F(1)* Line%V(1)) * aq
+         else if (I == N) then
+            Line%Ap(:,I) = p%rhoW * (1. + Line%Can) * 0.5 * (Line%F(N)* Line%V(N)) * ap
+            Line%Aq(:,I) = p%rhoW * (1. + Line%Cat) * 0.5 * (Line%F(N)* Line%V(N)) * aq
+         else
+            Line%Ap(:,I) = p%rhoW * (1. + Line%Can) * 0.5 * (Line%F(I)* Line%V(I) + Line%F(I+1)* Line%V(I+1)) * ap
+            Line%Aq(:,I) = p%rhoW * (1. + Line%Cat) * 0.5 * (Line%F(I)* Line%V(I) + Line%F(I+1)* Line%V(I+1)) * aq
+         endif
 
          ! bottom contact (stiffness and damping, vertical-only for now)  - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces
          ! bottom contact - updated throughout October 2021 for seabed bathymetry and friction models
@@ -1430,11 +1518,11 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p)  !, FairFtot, FairMtot, AnchFtot,
 
          ! total forces
          IF (I==0)  THEN
-            Line%Fnet(:,I) = Line%T(:,1)                 + Line%Td(:,1)                  + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + Line%Bs(:,I)
+            Line%Fnet(:,I) = Line%T(:,1)                 + Line%Td(:,1)                  + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%Ap(:,I) + Line%Aq(:,I) + Line%B(:,I) + Line%Bs(:,I)
          ELSE IF (I==N)  THEN
-            Line%Fnet(:,I) =                -Line%T(:,N)                  - Line%Td(:,N) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + Line%Bs(:,I)
+            Line%Fnet(:,I) =                -Line%T(:,N)                  - Line%Td(:,N) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%Ap(:,I) + Line%Aq(:,I) + Line%B(:,I) + Line%Bs(:,I)
          ELSE
-            Line%Fnet(:,I) = Line%T(:,I+1) - Line%T(:,I) + Line%Td(:,I+1) - Line%Td(:,I) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + Line%Bs(:,I)
+            Line%Fnet(:,I) = Line%T(:,I+1) - Line%T(:,I) + Line%Td(:,I+1) - Line%Td(:,I) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%Ap(:,I) + Line%Aq(:,I) + Line%B(:,I) + Line%Bs(:,I)
          END IF
 
       END DO  ! I  - done looping through nodes