anchor.diff:  
   END SUBROUTINE BD_InitshpDerJaco
   
  +SUBROUTINE PitchActuator_SetBC(p, u, xd)
  +! this routine alters the RootMotion inputs based on the pitch-actuator parameters and discrete states
  +
  +   TYPE(BD_ParameterType),    INTENT(IN   )  :: p                                 ! The module parameters
  +   TYPE(BD_InputType),        INTENT(INOUT)  :: u                                 ! inputs
  +   TYPE(BD_DiscreteStateType),INTENT(IN   )  :: xd                                ! The module discrete states
  +
  +   ! local variables
  +   REAL(ReKi)                                :: temp_R(3,3)
  +   REAL(ReKi)                                :: temp_cc(3)
  +   REAL(ReKi)                                :: u_theta_pitch
  +   REAL(ReKi)                                :: thetaP
  +   REAL(ReKi)                                :: omegaP
  +   REAL(ReKi)                                :: alphaP
  +   
  +   
  +   temp_R = MATMUL(u%RootMotion%Orientation(:,:,1),TRANSPOSE(u%HubMotion%Orientation(:,:,1)))
  +   temp_cc = EulerExtract(temp_R)   != Hub_theta_Root
  +   u_theta_pitch = -temp_cc(3)
  +       
  +   thetaP = xd%thetaP
  +   omegaP = xd%thetaPD
  +   alphaP = -(p%pitchK/p%pitchJ) * xd%thetaP - (p%pitchC/p%pitchJ) * xd%thetaPD + (p%pitchK/p%pitchJ) * u_theta_pitch
  +       
  +   ! Calculate new root motions for use as BeamDyn boundary conditions:       
  +   !note: we alter the orientation last because we need the input (before actuator) root orientation for the rotational velocity and accelerations
  +   u%RootMotion%RotationVel(:,1) = u%RootMotion%RotationVel(:,1) - omegaP * u%RootMotion%Orientation(3,:,1)
  +   u%RootMotion%RotationAcc(:,1) = u%RootMotion%RotationAcc(:,1) - alphaP * u%RootMotion%Orientation(3,:,1)
  +
  +   temp_cc(3) = -thetaP
  +   temp_R = EulerConstruct(temp_cc)       
  +   u%RootMotion%Orientation(:,:,1) = MATMUL(temp_R,u%HubMotion%Orientation(:,:,1))
  +       
  +
  +END SUBROUTINE PitchActuator_SetBC
   
   END MODULE BeamDyn
  
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)    
  