anchor.diff:        CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )   
      CALL BD_CopyInput(u, u_tmp, MESH_NEWCOPY, ErrStat2, ErrMsg2)
         CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  -   CALL BD_CopyInput(u, u_tmp2, MESH_NEWCOPY, ErrStat2, ErrMsg2)
  -      CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  +      
      ! Actuator
      IF( p%torq ) THEN
  -       temp_R(:,:) = MATMUL(u%RootMotion%Orientation(:,:,1),TRANSPOSE(u%HubMotion%Orientation(:,:,1)))
  -       temp_cc(:) = EulerExtract(temp_R)
  -       temp_thetaP = xd%thetaP
  -       temp_omegaP = xd%thetaPD
  -       temp_alphaP = -(p%pitchK/p%pitchJ) * xd%thetaP - (p%pitchC/p%pitchJ) * xd%thetaPD + &
  -          (p%pitchK/p%pitchJ) * (-temp_cc(3))
  -       temp_cc(3) = -temp_thetaP
  -       temp_R(:,:) = EulerConstruct(temp_cc)
  -       u_tmp%RootMotion%Orientation(:,:,1) = MATMUL(temp_R,u%HubMotion%Orientation(:,:,1))
  -       u_tmp%RootMotion%RotationVel(:,1) = u%RootMotion%RotationVel(:,1) - &
  -         temp_omegaP * u%RootMotion%Orientation(3,1:3,1)
  -       u_tmp%RootMotion%RotationAcc(:,1) = u%RootMotion%RotationAcc(:,1) - &
  -         temp_alphaP * u%RootMotion%Orientation(3,1:3,1)
  -       CALL BD_CopyInput(u_tmp, u_tmp2, MESH_NEWCOPY, ErrStat2, ErrMsg2)
  -          CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  +       CALL PitchActuator_SetBC(p, u_tmp, xd)
      ENDIF
      ! END Actuator
  +   
  +   CALL BD_CopyInput(u_tmp, u_tmp2, MESH_NEWCOPY, ErrStat2, ErrMsg2)
  +      CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  +      
      CALL BD_CrvExtractCrv(p%GlbRot,temp_glb,ErrStat2,ErrMsg2)
         CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
         if (ErrStat >= AbortErrLev) then
  
positive.code:    INTEGER(IntKi),                    INTENT(  OUT)  :: ErrStat     ! Error status of the operation
     CHARACTER(*),                      INTENT(  OUT)  :: ErrMsg      ! Error message if ErrStat /= ErrID_None
  
        ! Initialize ErrStat
  
        ErrStat = ErrID_None
        ErrMsg  = ""
  
        ! Update discrete states here:
  
  !      xd%DummyDiscState = 0.0
  
  END SUBROUTINE BD_UpdateDiscState
  !-----------------------------------------------------------------------------------------------------------------------------------
  SUBROUTINE BD_CalcConstrStateResidual( t, u, p, x, xd, z, OtherState, Z_residual, ErrStat, ErrMsg )
  
negative.code:    p%IniDisp(:) = x%q(:)
     p%IniVelo(:) = x%dqdt(:)
  
  ! Actuator
  !for S testing: k = 2.0E+7 kgm^2/s^2, c = 5.0E+5 kgm^2/s, and J = 200 kgm^2,    
     p%torq = .TRUE. 
     p%pitchK = 2.0D+07  !kgm^2/s^2
     p%pitchC = 5.0D+05  !kgm^2/s
     p%pitchJ = 2.0D+02  !kgm^2
     TmpDCM(:,:) = MATMUL(u%RootMotion%Orientation(:,:,1),TRANSPOSE(u%HubMotion%Orientation(:,:,1)))
     temp_CRV(:) = EulerExtract(TmpDCM)
     xd%thetaP = -temp_CRV(3)    
  