anchor.diff:     CALL SD_Input_ExtrapInterp( u, utimes, u_interp, t, ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'SD_AM2')
      CALL GetExtForceOnInternalDOF(u_interp, p, x, m, m%F_L, ErrStat2, ErrMsg2, ExtraMoment=(p%ExtraMoment.and..not.p%Floating), RotateLoads=(p%ExtraMoment.and.p%Floating))
      m%udotdot_TP = (/u_interp%TPMesh%TranslationAcc(:,1), u_interp%TPMesh%RotationAcc(:,1)/)
  -   if (p%Floating) then
  -      ! >>> New udotdot_TP to body coordinates
  -      m%udotdot_TP(1:3) = matmul(u_interp%TPMesh%Orientation(:,:,1), m%udotdot_TP(1:3)) 
  -      m%udotdot_TP(4:6) = matmul(u_interp%TPMesh%Orientation(:,:,1), m%udotdot_TP(4:6)) 
  -   endif
  +   !if (p%Floating) then
  +   !   ! >>> Rotate All - udotdot_TP to body coordinates
  +   !   m%udotdot_TP(1:3) = matmul(u_interp%TPMesh%Orientation(:,:,1), m%udotdot_TP(1:3)) 
  +   !   m%udotdot_TP(4:6) = matmul(u_interp%TPMesh%Orientation(:,:,1), m%udotdot_TP(4:6)) 
  +   !endif
                   
      ! extrapolate u to find u_interp = u(t + dt)=u_n+1
      CALL SD_Input_ExtrapInterp(u, utimes, u_interp, t+p%SDDeltaT, ErrStat2, ErrMsg2); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'SD_AM2')
      CALL GetExtForceOnInternalDOF(u_interp, p, x, m, F_L2, ErrStat2, ErrMsg2, ExtraMoment=(p%ExtraMoment.and..not.p%Floating), RotateLoads=(p%ExtraMoment.and.p%Floating))
      udotdot_TP2 = (/u_interp%TPMesh%TranslationAcc(:,1), u_interp%TPMesh%RotationAcc(:,1)/)
  -   if (p%Floating) then
  -      ! >>> New udotdot_TP to body coordinates
  -      udotdot_TP2(1:3) = matmul(u_interp%TPMesh%Orientation(:,:,1), udotdot_TP2(1:3)) 
  -      udotdot_TP2(4:6) = matmul(u_interp%TPMesh%Orientation(:,:,1), udotdot_TP2(4:6)) 
  -   endif
  +   !if (p%Floating) then
  +   !   ! >>> Rotate All - udotdot_TP to body coordinates
  +   !   udotdot_TP2(1:3) = matmul(u_interp%TPMesh%Orientation(:,:,1), udotdot_TP2(1:3)) 
  +   !   udotdot_TP2(4:6) = matmul(u_interp%TPMesh%Orientation(:,:,1), udotdot_TP2(4:6)) 
  +   !endif
      
      ! calculate (u_n + u_n+1)/2
      udotdot_TP2 = 0.5_ReKi * ( udotdot_TP2 + m%udotdot_TP )
  
positive.code:       if (p%nDOFM>0) then
           if (p%Floating .and. present(u)) then
              ! TODO TODO rotate it A MBmmB A^t
              !DD(1:3,:) = DD(1:3,:) + matmul(transpose(u%TPMesh%Orientation(:,:,1)), p%MBmmB(1:3,:) ! >>> New 
              DD(1:nY,13:18 ) = DD(1:nY,13:18 )+ p%MBmmB
           else
              DD(1:nY,13:18 ) = DD(1:nY,13:18 )+ p%MBmmB
  
negative.code:       ! Compute reaction/coupling force at TP
        Y1_Utp  = - (matmul(p%KBB, m%u_TP) + matmul(p%CBB, m%udot_TP) + matmul(p%MBB, m%udotdot_TP) )
        if (p%nDOFM>0) then
           !>>> Rotate All
           ! NOTE: this introduces some hysteresis
           !if (p%Floating) then
           !   udotdot_TP(1:3) = matmul(Rg2b, u%TPMesh%TranslationAcc( :,1))
           !   udotdot_TP(4:6) = matmul(Rg2b, u%TPMesh%RotationAcc(:,1)    )
           !   Y1_Utp  = Y1_Utp + matmul(RRb2g, matmul(p%MBmmB, udotdot_TP))  
           !else
           Y1_Utp  = Y1_Utp + matmul(p%MBmmB, m%udotdot_TP)  
           !endif
        endif
        if ( p%nDOFM > 0) then
           Y1_CB = -( matmul(p%C1_11, x%qm) + matmul(p%C1_12, x%qmdot) )
           !if (p%Floating) then
           !   Y1_CB = matmul(RRb2g, Y1_CB) !>>> Rotate All
           !endif
        else
           Y1_CB = 0.0_ReKi
        endif
  