anchor.diff:      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)) )
  +
  +    !   trans(get_rotation(theta))
  +
  +    !    qdot(4:6) = array(  * vector(q(10:12)) )
  +
  +    !  sets the state of the body and its time derivatives
  +    call set_state(q, qdot, alpha)
  +
       ! mass of the body
       alpha%mass  = mass
  -    
  +
       ! moment of inertia in body-fixed frame
       alpha%J = -mass*skew(re)*skew(re)
  -    
  +
       !first moment of inertia in body-fixed frame: mass*(cg location)
       alpha%c  = mass*re
   
  
positive.code:   ! mass   : mass of the body
    ! re     : interested point on the body measure in body frame
    !*******************************************************************!
    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)        :: qdot(NDOF_PBODY)
  
      ! output
      type(body)                  :: alpha
  
      ! local variables
      real(dp), dimension(NUM_SPAT_DIM):: theta
  
      !  sets the state of the body and its time derivatives
      call set_state(q, qdot, alpha)
  
      ! make a local copy
      theta = q(4:6)
  
negative.code:     type(matrix) :: SMAT_DOT
  
      real(dp)     :: theta(NUM_SPAT_DIM), dtheta(NUM_SPAT_DIM)
  
      ! convert vec to array
      theta = array(thetain); dtheta=array(dthetain);   
  
  