anchor.diff:     REAL(ReKi)               :: r1, r2, t, Iyy, Jzz, Ixx, A, kappa, nu, ratioSq, D_inner, D_outer
      LOGICAL                  :: shear
      REAL(ReKi), ALLOCATABLE  :: Ke(:,:), Me(:, :), FGe(:) ! element stiffness and mass matrices gravity force vector
  -   INTEGER, ALLOCATABLE     :: nn(:)                     ! node number in element 
  +   INTEGER, DIMENSION(NNE)  :: nn                        ! node number in element 
      INTEGER                  :: r
  -   
  -   
      INTEGER(IntKi)           :: ErrStat2
      CHARACTER(1024)          :: ErrMsg2
  -
  -   
  -      ! for current application
  -   IF ( (Init%FEMMod .LE. 3) .and. (Init%FEMMod .GE. 0)) THEN
  -      NNE = 2
  -   ELSE
  -      ErrStat = ErrID_Fatal
  -      ErrMsg = 'Invalid FEMMod in AssembleKM'
  -      RETURN
  -   ENDIF                              
  +   
  +   ! for current application
  +   if    (Init%FEMMod == 2) THEN ! tapered Euler-Bernoulli
  +       CALL Abort ('FEMMod = 2 is not implemented.')
  +       return
  +   elseif (Init%FEMMod == 4) THEN ! tapered Timoshenko
  +       CALL Abort ('FEMMod = 2 is not implemented.')
  +       return
  +   elseif ((Init%FEMMod == 1) .or. (Init%FEMMod == 3)) THEN !
  +      ! 1: uniform Euler-Bernouli,  3: uniform Timoshenko
  +   else
  +       CALL Abort('FEMMod is not valid. Please choose from 1, 2, 3, and 4. ')
  +       return
  +   endif
      
      ! total degrees of freedom of the system 
      Init%TDOF = 6*Init%NNode
      
  -         ! Assemble system stiffness and mass matrices with gravity force vector
  -   
      ALLOCATE( p%ElemProps(Init%NElem), STAT=ErrStat2)
  -      IF (ErrStat2 /= 0) THEN
  -         CALL SetErrStat ( ErrID_Fatal, 'Error allocating p%ElemProps', ErrStat, ErrMsg, 'AssembleKM' )
  -         CALL CleanUp_AssembleKM()
  -      RETURN
  +   IF (ErrStat2 /= 0) THEN
  +       CALL Abort('Error allocating p%ElemProps')
  +       return
      ENDIF
   
  -   CALL AllocAry( Ke,     NNE*6,         NNE*6 , 'Ke',      ErrStat2, ErrMsg2); CALL SetErrStat ( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'AssembleKM' )    ! element stiffness matrix
  -   CALL AllocAry( Me,     NNE*6,         NNE*6 , 'Me',      ErrStat2, ErrMsg2); CALL SetErrStat ( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'AssembleKM' )    ! element mass matrix 
  -   CALL AllocAry( FGe,    NNE*6,                 'FGe',     ErrStat2, ErrMsg2); CALL SetErrStat ( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'AssembleKM' )    ! element gravity force vector 
  -   CALL AllocAry( nn,     NNE,                   'nn',      ErrStat2, ErrMsg2); CALL SetErrStat ( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'AssembleKM' )    ! node number in element array 
  -   
  -   CALL AllocAry( Init%K, Init%TDOF, Init%TDOF , 'Init%K',  ErrStat2, ErrMsg2); CALL SetErrStat ( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'AssembleKM' )    ! system stiffness matrix 
  -   CALL AllocAry( Init%m, Init%TDOF, Init%TDOF , 'Init%M',  ErrStat2, ErrMsg2); CALL SetErrStat ( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'AssembleKM' )    ! system mass matrix 
  -   CALL AllocAry( Init%FG,Init%TDOF,             'Init%FG', ErrStat2, ErrMsg2); CALL SetErrStat ( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'AssembleKM' )    ! system gravity force vector 
  -    
  -   IF (ErrStat >= AbortErrLev) THEN
  -      CALL CleanUp_AssembleKM()
  -      RETURN
  -   ENDIF
  -   
  -   Init%K = 0.0_ReKi
  -   Init%M = 0.0_ReKi
  +   CALL AllocAry( Ke,     NNE*6,         NNE*6 , 'Ke',      ErrStat2, ErrMsg2); if(Failed()) return; ! element stiffness matrix
  +   CALL AllocAry( Me,     NNE*6,         NNE*6 , 'Me',      ErrStat2, ErrMsg2); if(Failed()) return; ! element mass matrix 
  +   CALL AllocAry( FGe,    NNE*6,                 'FGe',     ErrStat2, ErrMsg2); if(Failed()) return; ! element gravity force vector 
  +   CALL AllocAry( Init%K, Init%TDOF, Init%TDOF , 'Init%K',  ErrStat2, ErrMsg2); if(Failed()) return; ! system stiffness matrix 
  +   CALL AllocAry( Init%m, Init%TDOF, Init%TDOF , 'Init%M',  ErrStat2, ErrMsg2); if(Failed()) return; ! system mass matrix 
  +   CALL AllocAry( Init%FG,Init%TDOF,             'Init%FG', ErrStat2, ErrMsg2); if(Failed()) return; ! system gravity force vector 
  +   Init%K  = 0.0_ReKi
  +   Init%M  = 0.0_ReKi
      Init%FG = 0.0_ReKi
   
      
  -      ! loop over all elements
  +   ! loop over all elements
      DO I = 1, Init%NElem
      
         DO J = 1, NNE
  
positive.code:    LOGICAL                       :: found, CreateNewProp
     INTEGER(IntKi)                :: ErrStat2
     CHARACTER(1024)               :: ErrMsg2
     
     
     ErrStat = ErrID_None
     ErrMsg  = ""
     
     !~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
     
     ! number of nodes per element
     IF( ( Init%FEMMod .GE. 0 ) .and. (Init%FEMMod .LE. 3) ) THEN
        NNE = 2 
     ELSE
        CALL SetErrStat(ErrID_Fatal, 'FEMMod '//TRIM(Num2LStr(Init%FEMMod))//' not implemented.',ErrStat,ErrMsg,'SD_Discrt')
        RETURN
     ENDIF
     
     
     Init%NNode = Init%NJoints + ( Init%NDiv - 1 )*p%NMembers    ! Calculate total number of nodes according to divisions 
     Init%NElem = p%NMembers*Init%NDiv                           ! Total number of element   
  
negative.code:    DC( 7: 9,  7: 9) = DirCos
     DC(10:12, 10:12) = DirCos
     
     K = MATMUL( MATMUL(DC, K), TRANSPOSE(DC) ) ! TODO: change me if DirCos convention is  transposed
     
  END SUBROUTINE ElemK
  
  !------------------------------------------------------------------------------------------------------
  !> Element mass matrix for classical beam elements
  SUBROUTINE ElemM(A, L, Ixx, Iyy, Jzz, rho, DirCos, M)
     REAL(ReKi), INTENT( IN) :: A, L, Ixx, Iyy, Jzz, rho
     REAL(ReKi), INTENT( IN) :: DirCos(3,3)
     REAL(ReKi), INTENT(OUT) :: M(12, 12)
  
     REAL(ReKi) :: t, rx, ry, po
     REAL(ReKi) :: DC(12, 12)
     
     t = rho*A*L;
     rx = rho*Ixx;
     ry = rho*Iyy;
     po = rho*Jzz*L;   
  
     M(1:12,1:12) = 0
        
     M( 9,  9) = t/3.0
     M( 7,  7) = 13.0*t/35.0 + 6.0*ry/(5.0*L)
  