text: #! /usr/bin/env python
  
  import rospy
  import sys
  import copy
  import moveit_commander
  import moveit_msgs.msg
  import geometry_msgs.msg
  import actionlib
  import math
  from tf.transformations import quaternion_from_euler
  from math import pi
  
  import tf2_ros
  import tf2_geometry_msgs
  
  def transform_pose(input_pose, from_frame, to_frame):
  
      # **Assuming /tf2 topic is being broadcasted
      tf_buffer = tf2_ros.Buffer()
      listener = tf2_ros.TransformListener(tf_buffer)
  
      pose_stamped = tf2_geometry_msgs.PoseStamped()
      pose_stamped.pose = input_pose
      pose_stamped.header.frame_id = from_frame
      pose_stamped.header.stamp = rospy.Time.now()
  
      try:
          # ** It is important to wait for the listener to start listening. Hence the rospy.Duration(1)
          output_pose_stamped = tf_buffer.transform(pose_stamped, to_frame, rospy.Duration(1))
          return output_pose_stamped.pose
  
      except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
          raise
  
  class Ur5Moveit:
  
      # Constructor
      def __init__(self):
  
          rospy.init_node('node_go_to_pose', anonymous=True)
  
          self._planning_group = "ur5_planning_group"
          self._commander = moveit_commander.roscpp_initialize(sys.argv)
          self._robot = moveit_commander.RobotCommander()
          self._scene = moveit_commander.PlanningSceneInterface()
          self._group = moveit_commander.MoveGroupCommander(self._planning_group)
          self._display_trajectory_publisher = rospy.Publisher(
              '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
  
          self._exectute_trajectory_client = actionlib.SimpleActionClient(
              'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
          self._exectute_trajectory_client.wait_for_server()
  
          self._planning_frame = self._group.get_planning_frame()
          self._eef_link = self._group.get_end_effector_link()
          self._group_names = self._robot.get_group_names()
          self._box_name = ''
  
          # Current State of the Robot is needed to add box to planning scene
          self._curr_state = self._robot.get_current_state()
  
          rospy.loginfo(
              '\033[94m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m')
          rospy.loginfo(
              '\033[94m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m')
          rospy.loginfo(
              '\033[94m' + "Group Names: {}".format(self._group_names) + '\033[0m')
  
          rospy.loginfo('\033[94m' + " >>> Ur5Moveit init done." + '\033[0m')
  
      def go_to_pose(self, arg_pose):
  
          pose_values = self._group.get_current_pose().pose
          rospy.loginfo('\033[94m' + ">>> Current Pose:" + '\033[0m')
          rospy.loginfo(pose_values)
  
          self._group.set_pose_target(arg_pose)
          flag_plan = self._group.go(wait=True)  # wait=False for Async Move
  
          pose_values = self._group.get_current_pose().pose
          rospy.loginfo('\033[94m' + ">>> Final Pose:" + '\033[0m')
          rospy.loginfo(pose_values)
  
          list_joint_values = self._group.get_current_joint_values()
          rospy.loginfo('\033[94m' + ">>> Final Joint Values:" + '\033[0m')
          rospy.loginfo(list_joint_values)
  
          if (flag_plan == True):
              rospy.loginfo(
                  '\033[94m' + ">>> go_to_pose() Success" + '\033[0m')
          else:
              rospy.logerr(
                  '\033[94m' + ">>> go_to_pose() Failed. Solution for Pose not Found." + '\033[0m')
  
          return flag_plan
  
      # Destructor
      def __del__(self):
          moveit_commander.roscpp_shutdown()
          rospy.loginfo(
              '\033[94m' + "Object of class Ur5Moveit Deleted." + '\033[0m')
  
  if __name__ == '__main__':
      ur5 = Ur5Moveit()
      cur_pos = ur5._group.get_current_pose().pose.position
     
      ur5_pose_1 = ur5._group.get_current_pose().pose
      ur5_pose_1.position.x = 8.1085 -7.6509
      ur5_pose_1.position.y = 3.24 - 2.677
      ur5_pose_1.position.z = 1
  
      ur5._group.set_pose_target(ur5_pose_1)
      flag_plan = ur5._group.go(wait=True)
      if (flag_plan == True):
          rospy.loginfo(
              '\033[94m' + ">>> go_to_pose() Success" + '\033[0m')
      else:
          rospy.logerr(
              '\033[94m' + ">>> go_to_pose() Failed. Solution for Pose not Found." + '\033[0m')
  
      rospy.sleep(2)
  
      # while not rospy.is_shutdown():
      #     ur5.go_to_pose(ur5_pose_1)
      #     rospy.sleep(2)
      #     ur5.go_to_pose(ur5_pose_2)
      #     rospy.sleep(2)
      #     ur5.go_to_pose(ur5_pose_3)
      #     rospy.sleep(2)
  
      del ur5