anchor.diff:      type(matrix) :: SMAT_DOT
   
       real(dp)     :: theta(NUM_SPAT_DIM), dtheta(NUM_SPAT_DIM)
  -    
  +
       ! convert vec to array
       theta = array(thetain); dtheta=array(dthetain);   
   
  
positive.code:     ! Assuming the body axis to be at the centre of mass of the body
      ! Inertial or body frame?
      alpha%KE = 0.5_dp*(mass * alpha%v *  alpha%v &
           & + alpha%omega*alpha%J* alpha%omega) 
  
      ! potential energy due to position
      ! include strain energy later
  
negative.code:   ! mass   : mass of the body
    ! re     : interested point on the body measure in body frame
    !*******************************************************************!
    function create_body(mass, re, q, qdotin) 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)
      ! output
      type(body)                  :: alpha
  
      ! local variables
      real(dp), dimension(NUM_SPAT_DIM):: theta
  
      ! make a local copy
      theta = q(4:6)
  