anchor.diff:     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)) 
  +      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
  
positive.code: 
     if (ExtraMoment) then
        ! Compute node displacements for lever arm
        call Displacements(u, p, x, m, m%DU_full, bGuyan=.True., bElastic=.False.)
        if (p%Floating) then ! remove u_TP
           do iNode = 1,p%nNodes
              m%DU_full(p%NodesDOF(iNode)%List(1:3)) =m%DU_full(p%NodesDOF(iNode)%List(1:3)) - m%u_TP(1:3)
           enddo
        endif
     endif
  
     ! --- Build vector of external forces (including gravity) (Moment done below)  
  
negative.code:          Y1_CB_L = - (matmul(p%D1_141, m%F_L)) ! Uses non rotated loads
        endif
        if (p%Floating) then
           Y1_CB_L = matmul(RRb2g, Y1_CB_L) !>>> New
        endif
  
        Y1 = Y1_CB + Y1_Utp + Y1_CB_L+ Y1_Guy_L + Y1_Guy_R 
  