anchor.diff:        IF ( p%nDOFM == 0 ) RETURN
   
         ! Compute F_L, force on internal DOF
  -      CALL GetExtForceOnInternalDOF( u, p, m, m%F_L, ErrStat2, ErrMsg2, ExtraMoment=(p%ExtraMoment.and..not.p%Floating), RotateLoads=(p%ExtraMoment.and.p%Floating))
  +      CALL GetExtForceOnInternalDOF(u, p, x, m, m%F_L, ErrStat2, ErrMsg2, ExtraMoment=(p%ExtraMoment.and..not.p%Floating), RotateLoads=(p%ExtraMoment.and.p%Floating))
   
         udotdot_TP = (/u%TPMesh%TranslationAcc(:,1), u%TPMesh%RotationAcc(:,1)/)
         if (p%Floating) then
            ! >>> New udotdot_TP to body coordinates
  -         udotdot_TP(1:3) = matmul( u%TPMesh%Orientation(:,:,1), udotdot_TP(1:3) ) 
  -         udotdot_TP(4:6) = matmul( u%TPMesh%Orientation(:,:,1), udotdot_TP(4:6) ) 
  +         !udotdot_TP(1:3) = matmul( u%TPMesh%Orientation(:,:,1), udotdot_TP(1:3) ) 
  +         !udotdot_TP(4:6) = matmul( u%TPMesh%Orientation(:,:,1), udotdot_TP(4:6) ) 
         endif
         
         ! State equation
  
positive.code:       if (p%Floating) then
           ! For floating, we compute the Guyan motion directly (rigid body motion with TP as origin)
           ! This introduce non-linear "rotations" effects, where the bottom node should "go up", and not just translate horizontally
           Rot(1:3,1:3) = transpose(u%TPMesh%Orientation(:,:,1)) ! body 2 global
           Om(1:3)      = u%TPMesh%RotationVel(1:3,1)
           OmD(1:3)     = u%TPMesh%RotationAcc(1:3,1)
           do iSDNode = 1,p%nNodes
              iY2Node = iSDNode
              DOFList => p%NodesDOF(iSDNode)%List  ! Alias to shorten notations
              ! --- Guyan (rigid body) motion in global coordinates
              rIP0(1:3)   = p%DP0(1:3, iSDNode)
              rIP(1:3)    = matmul(Rot, rIP0)
              duP(1:3)    = rIP - rIP0 + m%u_TP(1:3)
              Om_X_r(1:3) = cross_product(Om, rIP)
              vP(1:3)     = u%TPMesh%TranslationVel(1:3,1) + Om_X_r
              aP(1:3)     = u%TPMesh%TranslationAcc(1:3,1) + cross_product(OmD, rIP)  + cross_product(Om, Om_X_r)
              m%U_full       (DOFList(1:3)) = m%U_full       (DOFList(1:3)) + duP(1:3)       
              m%U_full       (DOFList(4:6)) = m%U_full       (DOFList(4:6)) + rotations(1:3)
              m%U_full_dot   (DOFList(1:3)) = m%U_full_dot   (DOFList(1:3)) + vP(1:3)
              m%U_full_dot   (DOFList(4:6)) = m%U_full_dot   (DOFList(4:6)) + Om(1:3)
              m%U_full_dotdot(DOFList(1:3)) = m%U_full_dotdot(DOFList(1:3)) + aP(1:3)
              m%U_full_dotdot(DOFList(4:6)) = m%U_full_dotdot(DOFList(4:6)) + OmD(1:3)
              !m%U_full       (DOFList(1:3)) = matmul(Rb2g, m%U_full       (DOFList(1:3))) + duP(1:3)       
              !m%U_full       (DOFList(4:6)) = matmul(Rb2g, m%U_full       (DOFList(4:6))) + rotations(1:3)
              !m%U_full_dot   (DOFList(1:3)) = matmul(Rb2g, m%U_full_dot   (DOFList(1:3))) + vP(1:3)
              !m%U_full_dot   (DOFList(4:6)) = matmul(Rb2g, m%U_full_dot   (DOFList(4:6))) + Om(1:3)
              !m%U_full_dotdot(DOFList(1:3)) = matmul(Rb2g, m%U_full_dotdot(DOFList(1:3))) + aP(1:3)
              !m%U_full_dotdot(DOFList(4:6)) = matmul(Rb2g, m%U_full_dotdot(DOFList(4:6))) + OmD(1:3)
              ! TODO TODO which orientation to give for joints with more than 6 dofs?
              ! Construct the direction cosine matrix given the output angles
              call SmllRotTrans( 'UR_bar input angles', m%U_full(DOFList(4)), m%U_full(DOFList(5)), m%U_full(DOFList(6)), DCM, '', ErrStat2, ErrMsg2)
              call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'SD_CalcOutput')
              y%Y2mesh%Orientation     (:,:,iY2Node)   = DCM
              y%Y2mesh%TranslationDisp (:,iY2Node)     = m%U_full        (DOFList(1:3))
              y%Y2mesh%TranslationVel  (:,iY2Node)     = m%U_full_dot    (DOFList(1:3))
              y%Y2mesh%TranslationAcc  (:,iY2Node)     = m%U_full_dotdot (DOFList(1:3))
              y%Y2mesh%RotationVel     (:,iY2Node)     = m%U_full_dot    (DOFList(4:6))
              y%Y2mesh%RotationAcc     (:,iY2Node)     = m%U_full_dotdot (DOFList(4:6))
           enddo
        else
           ! --- Fixed bottom
           do iSDNode = 1,p%nNodes
              iY2Node = iSDNode
              DOFList => p%NodesDOF(iSDNode)%List  ! Alias to shorten notations
              ! TODO TODO which orientation to give for joints with more than 6 dofs?
              ! Construct the direction cosine matrix given the output angles
              CALL SmllRotTrans( 'UR_bar input angles', m%U_full(DOFList(4)), m%U_full(DOFList(5)), m%U_full(DOFList(6)), DCM, '', ErrStat2, ErrMsg2)
              CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'SD_CalcOutput')
              y%Y2mesh%Orientation     (:,:,iY2Node)   = DCM
              y%Y2mesh%TranslationDisp (:,iY2Node)     = m%U_full        (DOFList(1:3))
              y%Y2mesh%TranslationVel  (:,iY2Node)     = m%U_full_dot    (DOFList(1:3))
              y%Y2mesh%TranslationAcc  (:,iY2Node)     = m%U_full_dotdot (DOFList(1:3))
              y%Y2mesh%RotationVel     (:,iY2Node)     = m%U_full_dot    (DOFList(4:6))
              y%Y2mesh%RotationAcc     (:,iY2Node)     = m%U_full_dotdot (DOFList(4:6))
           enddo
        endif
                                      
  
negative.code:       ! --- Output 2, Y2Mesh: motions on all FEM nodes (R, and L DOFs, then full DOF vector)
        ! --------------------------------------------------------------------------------
        ! External force on internal nodes (F_L)
        call GetExtForceOnInternalDOF(u, p, x, m, m%F_L, ErrStat2, ErrMsg2, ExtraMoment=(p%ExtraMoment.and..not.p%Floating), RotateLoads=(p%ExtraMoment.and.p%Floating)); if(Failed()) return
        m%UR_bar        = 0.0_ReKi
        m%UR_bar_dot    = 0.0_ReKi
        m%UR_bar_dotdot = 0.0_ReKi
  