anchor.diff:  
         ! find xdot at t
         CALL SD_CalcContStateDeriv( t, u_interp, p, x, xd, z, OtherState, m, xdot, ErrStat, ErrMsg ) !initializes xdot
  -
  -      k1%qm    = p%SDDeltaT * xdot%qm
  -      k1%qmdot = p%SDDeltaT * xdot%qmdot
  -  
  +      k1%qm       = p%SDDeltaT * xdot%qm
  +      k1%qmdot    = p%SDDeltaT * xdot%qmdot
         x_tmp%qm    = x%qm    + 0.5 * k1%qm
         x_tmp%qmdot = x%qmdot + 0.5 * k1%qmdot
  -
         ! interpolate u to find u_interp = u(t + dt/2)
         CALL SD_Input_ExtrapInterp(u, utimes, u_interp, t+0.5*p%SDDeltaT, ErrStat, ErrMsg)
   
         ! find xdot at t + dt/2
         CALL SD_CalcContStateDeriv( t + 0.5*p%SDDeltaT, u_interp, p, x_tmp, xd, z, OtherState, m, xdot, ErrStat, ErrMsg )
  -
         k2%qm    = p%SDDeltaT * xdot%qm
         k2%qmdot = p%SDDeltaT * xdot%qmdot
  -
         x_tmp%qm    = x%qm    + 0.5 * k2%qm
         x_tmp%qmdot = x%qmdot + 0.5 * k2%qmdot
   
         ! find xdot at t + dt/2
         CALL SD_CalcContStateDeriv( t + 0.5*p%SDDeltaT, u_interp, p, x_tmp, xd, z, OtherState, m, xdot, ErrStat, ErrMsg )
  -     
  -      k3%qm    = p%SDDeltaT * xdot%qm
  -      k3%qmdot = p%SDDeltaT * xdot%qmdot
  -
  +      k3%qm       = p%SDDeltaT * xdot%qm
  +      k3%qmdot    = p%SDDeltaT * xdot%qmdot
         x_tmp%qm    = x%qm    + k3%qm
         x_tmp%qmdot = x%qmdot + k3%qmdot
  -
         ! interpolate u to find u_interp = u(t + dt)
         CALL SD_Input_ExtrapInterp(u, utimes, u_interp, t + p%SDDeltaT, ErrStat, ErrMsg)
   
         ! find xdot at t + dt
         CALL SD_CalcContStateDeriv( t + p%SDDeltaT, u_interp, p, x_tmp, xd, z, OtherState, m, xdot, ErrStat, ErrMsg )
  -
         k4%qm    = p%SDDeltaT * xdot%qm
         k4%qmdot = p%SDDeltaT * xdot%qmdot
  -
  -      x%qm    = x%qm    +  ( k1%qm    + 2. * k2%qm    + 2. * k3%qm    + k4%qm    ) / 6.      
  -      x%qmdot = x%qmdot +  ( k1%qmdot + 2. * k2%qmdot + 2. * k3%qmdot + k4%qmdot ) / 6.      
  -
  -      CALL ExitThisRoutine()
  -      
  -CONTAINS      
  -   !...............................................................................................................................
  -   SUBROUTINE ExitThisRoutine()
  -   ! This subroutine destroys all the local variables
  -   !...............................................................................................................................
  -
  -         ! local variables
  +      x%qm     = x%qm    +  ( k1%qm    + 2. * k2%qm    + 2. * k3%qm    + k4%qm    ) / 6.
  +      x%qmdot  = x%qmdot +  ( k1%qmdot + 2. * k2%qmdot + 2. * k3%qmdot + k4%qmdot ) / 6.
  +
  +      CALL CleanUp()
  +      
  +CONTAINS       
  +
  +   SUBROUTINE CleanUp()
         INTEGER(IntKi)             :: ErrStat3    ! The error identifier (ErrStat)
         CHARACTER(1024)            :: ErrMsg3     ! The error message (ErrMsg)
  -   
  -   
         CALL SD_DestroyContState( xdot,     ErrStat3, ErrMsg3 )
         CALL SD_DestroyContState( k1,       ErrStat3, ErrMsg3 )
         CALL SD_DestroyContState( k2,       ErrStat3, ErrMsg3 )
         CALL SD_DestroyContState( k3,       ErrStat3, ErrMsg3 )
         CALL SD_DestroyContState( k4,       ErrStat3, ErrMsg3 )
         CALL SD_DestroyContState( x_tmp,    ErrStat3, ErrMsg3 )
  -
         CALL SD_DestroyInput(     u_interp, ErrStat3, ErrMsg3 )
  -         
  -   END SUBROUTINE ExitThisRoutine            
  +   END SUBROUTINE CleanUp            
         
   END SUBROUTINE SD_RK4
   
  
positive.code:             !OtherState%xdot(4)    = OtherState%xdot(3)
              !OtherState%xdot(3)    = OtherState%xdot(2)
              !OtherState%xdot(2)    = OtherState%xdot(1)
  
           elseif (OtherState%n .gt. n) then
   
              ErrStat = ErrID_Fatal
              ErrMsg = ' Backing up in time is not supported with a multistep method '
              RETURN
  
           endif
  
           CALL SD_CopyContState( xdot, OtherState%xdot ( 1 ), MESH_UPDATECOPY, ErrStat, ErrMsg )
           !OtherState%xdot ( 1 )     = xdot  ! make sure this is most up to date
  
           x%qm    = x%qm    + (p%SDDeltaT / 24.) * ( 55.*OtherState%xdot(1)%qm - 59.*OtherState%xdot(2)%qm    + 37.*OtherState%xdot(3)%qm  &
                                         - 9. * OtherState%xdot(4)%qm )
  
           x%qmdot = x%qmdot + (p%SDDeltaT / 24.) * ( 55.*OtherState%xdot(1)%qmdot - 59.*OtherState%xdot(2)%qmdot  &
                                            + 37.*OtherState%xdot(3)%qmdot  - 9.*OtherState%xdot(4)%qmdot )
  
        endif
  
  
        CALL SD_DestroyContState(xdot, ErrStat, ErrMsg)
        CALL SD_DestroyInput(u_interp, ErrStat, ErrMsg)
        
  END SUBROUTINE SD_AB4
  !----------------------------------------------------------------------------------------------------------------------------------
  !> This subroutine implements the fourth-order Adams-Bashforth-Moulton Method (RK4) for numerically integrating ordinary 
  !! differential equations:
  
negative.code: 
  
  !----------------------------------------------------------------------------------------------------------------------------------
  !> Rotate the joint coordinates with respect to global z
  SUBROUTINE SubRotate(Joints,NJoints,SubRotZ)
     REAL(ReKi),                       INTENT(IN)       :: SubRotZ    ! Rotational angle in degrees
     INTEGER(IntKi),                   INTENT(IN)       :: NJOINTS    ! Row size of Joints 
     REAL(ReKi), DIMENSION(NJOINTS,3), INTENT(INOUT)    :: JOINTS     ! Rotational angle in degrees (Njoints,4)
     !locals
     REAL(ReKi)                 :: rot  !angle in rad
     REAL(ReKi), DIMENSION(2,2) :: ROTM !rotational matrix (cos matrix with -theta)
     
     rot=pi*SubRotz/180.
     ROTM=transpose(reshape([ COS(rot),    -SIN(rot) , &
  