MoorDyn: Coupled Pinned Bodies and bug fixes

modules/moordyn/src/MoorDyn_Body.f90

@@ -155,12 +155,25 @@ SUBROUTINE Body_Initialize(Body, states, m)
       INTEGER(IntKi)                        :: l               ! index of segments or nodes along line
       REAL(DbKi)                            :: dummyStates(12) ! dummy vector to mimic states when initializing a rigidly attached rod
 
-
-      ! assign initial body kinematics to state vector
-      states(7:12) = Body%r6
-      states(1:6 ) = Body%v6
-
+      IF (wordy > 0) print *, "initializing Body ", Body%idNum
+
+      ! the r6 and v6 vectors should have already been set
+      ! r and rd of ends have already been set by setup function or by parent object   <<<<< right? <<<<<
 
+
+      if (Body%typeNum == 0) then               ! free body type
+
+         ! assign initial body kinematics to state vector
+         states(1:6 ) = Body%v6 ! zero velocities for initialization (set to 0 in Body_Setup)
+         states(7:12) = Body%r6
+
+      else if (Body%typeNum ==2 ) then           ! pinned rod type (coupled or attached to something previously via setPinKin)
+
+         states(1:3)   = Body%v6(4:6) ! zero velocities for initialization (set to 0 in Body_Setup)
+         states(4:6)   = Body%r6(4:6) ! body orentations
+
+      end if
+
       ! set positions of any dependent points and rods now (before they are initialized)
       CALL Body_SetDependentKin(Body, 0.0_DbKi, m)
 
@@ -203,12 +216,12 @@ END SUBROUTINE Body_InitializeUnfree
 
    ! set kinematics for Bodies if they are coupled (or ground)
    !--------------------------------------------------------------
-   SUBROUTINE Body_SetKinematics(Body, r_in, v_in, a_in, t, m)
+   SUBROUTINE Body_SetKinematics(Body, r6_in, v6_in, a6_in, t, m)
 
       Type(MD_Body),         INTENT(INOUT)  :: Body       ! the Body object
-      Real(DbKi),            INTENT(IN   )  :: r_in(6)   ! 6-DOF position
-      Real(DbKi),            INTENT(IN   )  :: v_in(6)   ! 6-DOF velocity
-      Real(DbKi),             INTENT(IN   ) :: a_in(6)       ! 6-DOF acceleration (only used for coupled rods)
+      Real(DbKi),            INTENT(IN   )  :: r6_in(6)   ! 6-DOF position
+      Real(DbKi),            INTENT(IN   )  :: v6_in(6)   ! 6-DOF velocity
+      Real(DbKi),             INTENT(IN   ) :: a6_in(6)       ! 6-DOF acceleration 
       Real(DbKi),            INTENT(IN   )  :: t         ! instantaneous time
       TYPE(MD_MiscVarType),  INTENT(INOUT)  :: m         ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Points)
 
@@ -218,26 +231,24 @@ SUBROUTINE Body_SetKinematics(Body, r_in, v_in, a_in, t, m)
       ! store current time
       Body%time = t
 
-   !   if (abs(Body%typeNum) == 2) then ! body coupled in 6 DOF, or ground
-         Body%r6 = r_in
-         Body%v6 = v_in
-         Body%a6 = a_in
+      if (Body%typeNum == 2) then ! body pinned to coupling point
+
+         ! set Body translational kinematics based on BCs (linear model for now) 
+         Body%r6(1:3) = r6_in(1:3)
+         Body%v6(1:3) = v6_in(1:3)
+         Body%a6(1:3) = a6_in(1:3)
+
+         ! Body rotations are left alone and will be handled, along with passing kinematics to dependent objects, by separate call to setState
+
+      else ! body rigidly coupled to coupling point
+         Body%r6 = r6_in
+         Body%v6 = v6_in
+         Body%a6 = a6_in
 
          ! since this body has no states and all DOFs have been set, pass its kinematics to dependent attachments
          CALL Body_SetDependentKin(Body, t, m)
-
-   !   else if (abs(Body%typeNum) == 1) then ! body pinned at reference point
-   !   
-   !      ! set Body *end A only* kinematics based on BCs (linear model for now) 
-   !      Body%r6(1:3) = r_in(1:3)
-   !      Body%v6(1:3) = v_in(1:3)
-   !      
-   !      ! Body is pinned so only ref point posiiton is specified, rotations are left alone and will be 
-   !      ! handled, along with passing kinematics to attached objects, by separate call to setState
-   !   
-   !   else
-   !      print *, "Error: Body_SetKinematics called for a free Body."  ! <<<
-   !   end if
+
+      end if
 
    END SUBROUTINE Body_SetKinematics
    !--------------------------------------------------------------
@@ -257,14 +268,26 @@ SUBROUTINE Body_SetState(Body, X, t, m)
       ! store current time
       Body%time = t
 
+      if (Body%typeNum == 0) then ! free Body type
 
-
-      Body%r6 = X(7:12)   ! get positions      
-      Body%v6 = X(1:6)    ! get velocities
-
+         Body%r6 = X(7:12)   ! get positions      
+         Body%v6 = X(1:6)    ! get velocities
+
+         ! set positions of any dependent points and rods
+         CALL Body_SetDependentKin(Body, t, m) 
+
+      else if (Body%typeNum == 2) then
+
+         Body%r6(4:6) = X(4:6) ! get positions
+         Body%v6(4:6) = X(1:3) ! get velocities
 
-      ! set positions of any dependent points and rods
-      CALL Body_SetDependentKin(Body, t, m)
+
+         ! set positions of any dependent points and rods
+         CALL Body_SetDependentKin(Body, t, m) 
+
+      else 
+         print *, "Error: Body::setState called for a non-free Body type in MoorDyn"   ! <<<
+      end if
 
    END SUBROUTINE Body_SetState
    !--------------------------------------------------------------
@@ -336,6 +359,8 @@ SUBROUTINE Body_GetStateDeriv(Body, Xd, m, p)
 
       INTEGER(IntKi)                        :: J                ! index
 
+      Real(DbKi)                            :: Fnet     (6)     ! net force and moment about reference point
+
       Real(DbKi)                            :: acc(6)           ! 6DOF acceleration vector
 
       Real(DbKi)                            :: y_temp (6)       ! temporary vector for LU decomposition
@@ -349,15 +374,35 @@ SUBROUTINE Body_GetStateDeriv(Body, Xd, m, p)
 
       CALL Body_DoRHS(Body, m, p)
 
-      ! solve for accelerations in [M]{a}={f} using LU decomposition
-      CALL LUsolve(6, Body%M, LU_temp, Body%F6net, y_temp, acc)
+      IF (Body%typeNum == 0) THEN ! Free body
 
-      ! fill in state derivatives
-      Xd(7:12) = Body%v6       ! dxdt = V   (velocities)
-      Xd(1:6)  = acc           ! dVdt = a   (accelerations) 
+         ! solve for accelerations in [M]{a}={f} using LU decomposition
+         CALL LUsolve(6, Body%M, LU_temp, Body%F6net, y_temp, acc)
 
-      ! store accelerations in case they're useful as output
-      Body%a6 = acc
+         ! fill in state derivatives
+         Xd(7:12) = Body%v6       ! dxdt = V   (velocities)
+         Xd(1:6)  = acc           ! dVdt = a   (accelerations) 
+
+         ! store accelerations in case they're useful as output
+         Body%a6 = acc
+
+      ELSE ! Pinned Body, 3 states (rotational only)
+
+         ! Account for moment response due to inertial coupling
+         Fnet = Body%F6net
+         Fnet(4:6) = Fnet(4:6) - MATMUL(Body%M(4:6,1:3), Body%a6(1:3))  
+
+         ! solve for accelerations in [M]{a}={f} using LU decomposition
+         CALL LUsolve(3, Body%M(4:6,4:6), LU_temp(4:6,4:6), Fnet(4:6), y_temp(4:6), acc(4:6))
+
+         ! fill in state derivatives
+         Xd(4:6) = Body%v6(4:6)       ! dxdt = V   (velocities)
+         Xd(1:3)  = acc(4:6)           ! dVdt = a   (accelerations) 
+
+         ! store accelerations in case they're useful as output
+         Body%a6(4:6) = acc(4:6)
+
+      ENDIF
 
       ! check for NaNs (should check all state derivatives, not just first 6)
       DO J = 1, 6
@@ -477,9 +522,29 @@ SUBROUTINE Body_GetCoupledForce(Body, Fnet_out, m, p)
       ! add inertial loads as appropriate 
       if (Body%typeNum == -1) then                          
 
-         F6_iner = 0.0_DbKi !-MATMUL(Body%M, Body%a6)     <<<<<<<< why does including F6_iner cause instability???
-         Fnet_out = Body%F6net + F6_iner        ! add inertial loads
+         if (p%inertialF == 1) then      ! include inertial components 
+            F6_iner = -MATMUL(Body%M, Body%a6)     ! unstable in OpenFAST v4 and below becasue of loose coupling with ED and SD. Transients in acceleration can cause issues
+         else
+            ! When OpenFAST v5 is released w/ tight coupling, remove this hack and just use the inertial term above 
+            F6_iner = 0.0
+         endif
+
+         Body%F6net = Body%F6net + F6_iner        ! add inertial loads
+         Fnet_out = Body%F6net
+
+      else if (Body%typeNum == 2) then  ! pinned coupled body  
+
+         if (p%inertialF == 1) then      ! include inertial components  
+            ! inertial loads ... from input translational ... and solved rotational ... acceleration
+            F6_iner(1:3)  = -MATMUL(Body%M(1:3,1:3), Body%a6(1:3)) - MATMUL(Body%M(1:3,4:6), Body%a6(4:6))
+         else 
+            F6_iner(1:3) = 0.0
+         endif
 
+         Body%F6net(1:3) = Body%F6net(1:3) + F6_iner(1:3)     ! add translational inertial loads
+         Body%F6net(4:6) = 0.0_DbKi
+         Fnet_out = Body%F6net
+
       else
          print *, "ERROR, Body_GetCoupledForce called for wrong (non-coupled) body type in MoorDyn!"
       end if
@@ -505,7 +570,7 @@ SUBROUTINE Body_AddPoint(Body, pointID, coords)
          Body%AttachedC(Body%nAttachedC) = pointID
          Body%rPointRel(:,Body%nAttachedC) = coords  ! store relative position of point on body
       ELSE
-         Print*, "too many Points attached to Body ", Body%IdNum, " in MoorDyn!"
+         call WrScr("too many Points attached to Body "//trim(num2lstr(Body%IdNum))//" in MoorDyn!")
       END IF
 
    END SUBROUTINE Body_AddPoint
@@ -536,7 +601,7 @@ SUBROUTINE Body_AddRod(Body, rodID, coords)
          Body%r6RodRel(4:6, Body%nAttachedR) = tempUnitVec
 
       ELSE
-         Print*, "too many rods attached to Body ", Body%IdNum, " in MoorDyn"
+         call WrScr("too many rods attached to Body "//trim(num2lstr(Body%IdNum))//" in MoorDyn")
       END IF
 
    END SUBROUTINE Body_AddRod