anchor.diff:  
   end subroutine residual
   
  +!!$
  +!!$
  +!!$
  +!!$ 
  +!!$  ! residual assembly
  +!!$  !  call disp(" >> Assembling the residuals...")
  +!!$  res  = get_residual(body1)
  +!!$  !  call disp(" >> Residual assembly complete...")
  +!!$  !call disp("   R   =   ", res)
  +!!$
  +!!$  ! jacobian assembly
  +!!$  !call disp(" >> Assembling the Jacobian...")
  +!!$  jac  = get_jacobian(body1)
  +!!$  !call disp(" >> Jacobian assembly complete...")
  +!!$  !call disp("   JAC =   ",jac)
  +!!$
  +!!$
  +!!$  ! implement FD to check if the jacobian is the same
  +!!$  qtmp = q
  +!!$  call set_state(q, q_dot, body1)
  +!!$  dh = 1.0d-8
  +!!$
  +!!$  res = get_residual(body1)
  +!!$     
  +!!$  var: do  k = 1 , TOT_NDOF
  +!!$
  +!!$     ! perturb k-th variable
  +!!$     qtmp( k ) = qtmp (k)  + dh
  +!!$     ! updating the body state
  +!!$     call set_state(qtmp, q_dot, body1)
  +!!$     ! find the solution
  +!!$     res2 = get_residual(body1)
  +!!$     !    call disp("   R   =   ", res)
  +!!$     
  +!!$     dq =  (res2-res)/dh
  +!!$     
  +!!$     jac2 (:, k) = dq
  +!!$     
  +!!$     qtmp = q
  +!!$
  +!!$  end do var
  +!!$  call disp("   dJAC =   ",jac2)
  +!!$ 
  +!!$  call disp("   Error =   ",jac2-Jac)
  +!!$
  +!!$
  +!!$
  +!!$  stop"stopped"
  
positive.code: 
          q            = get_updated_q(q, dq)
          q_dot        = get_updated_q_dot(q_dot, dq)
          !  q_double_dot = get_updated_q_double_dot(q_double_dot, dq)
  
          call disp("   q      =   ", q)
  !        call disp("   qdot   =   ", q_dot)
  
          if (l2_norm .le. REL_TOL .OR. linf_norm.le. ABS_TOL) &
               & exit newton
  
negative.code: !!$  q_dot(7:9)   = (/ 0.0_dp, 0.0_dp, 0.0_dp /)
  !!$  q_dot(10:12) = (/ 0.0_dp, 0.0_dp, 0.0_dp /)
  
    !  CALL init_random_seed()         ! see example of RANDOM_SEED
    CALL RANDOM_NUMBER(q)
    CALL RANDOM_NUMBER(q_dot)  
    q_dot = q_dot*q_dot+1.0_dp
  
    ! define the attributes of the body
    mass      = 2.0_dp
  
    ! point of interest on the body
    CALL RANDOM_NUMBER(re)
    !  re        = (/ 0.0_dp, 0.0_dp, 0.0_dp /)   
  
    !-------------------------------------------------------------------!
    ! (2) create a pendulum body using the state and attr above
    !-------------------------------------------------------------------!
  
    !  call disp(" >> Creating a body...")
  
    body1 = create_body(mass, vector(re), q, q_dot)
  
    !call disp(" >> Body created successfully...")
  
    !-------------------------------------------------------------------!
    ! Settings for time-integration
  