anchor.diff:    ! mass   : mass of the body
     ! re     : interested point on the body measure in body frame
     !*******************************************************************!
  -  function create_body(mass, re, q, qdotin) result(alpha)
  +  function create_body(mass, re, q, qdot) result(alpha)
   
       ! inputs
       real(dp), intent(in)        :: mass 
       type(vector), intent(in)    :: re 
       real(dp), intent(in)        :: q(NDOF_PBODY)
  -    real(dp), intent(in)        :: qdotin(NDOF_PBODY)
  -    real(dp)                   :: qdot(NDOF_PBODY)
  +    real(dp), intent(in)        :: qdot(NDOF_PBODY)
       ! output
       type(body)                  :: alpha
   
  
positive.code:     SMAT_INV= matrix( (/  1.0_dp, s1*s2/c2, c1*s2/c2, &
           &0.0_dp, c1, -s1, 0.0_dp,&
           & s1/c2, c1/c2 /))
  
    end function get_angrate_inv
  
    !*******************************************************************!
    ! routine that prints the state and properties of the body
  
negative.code:     alpha%S         = get_angrate(theta)
      alpha%S_dot     = get_angrate_dot(theta, qdot(4:6))
  
      !qdot =qdotin
      !qdot(1:3) = array(trans(get_rotation(theta)) * vector(q(7:9)))
      !qdot(4:6) = array(get_angrate_inv(theta) * vector(q(10:12)))
  
      !  sets the state of the body and its time derivatives
      call set_state(q, qdot, alpha)
  