Guest User

move.py

a guest
Apr 24th, 2018
91
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import sys
  2. import copy
  3. import rospy
  4. import moveit_commander
  5. import moveit_msgs.msg
  6. import geometry_msgs.msg
  7.  
  8. # Select forward or inverse here
  9. IK = True
  10.  
  11. # Enable/disable random pose for IK
  12. RANDOM_POSE = False
  13.  
  14. # Joint poses
  15. ARM_DEFAULT = [0]*6
  16.  
  17. print "\nStarting setup"
  18. moveit_commander.roscpp_initialize(sys.argv)
  19. rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
  20.  
  21. robot = moveit_commander.RobotCommander()
  22. scene = moveit_commander.PlanningSceneInterface()
  23. group = moveit_commander.MoveGroupCommander("arm")
  24. end_effector_link =  group.get_end_effector_link()
  25.  
  26. display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
  27.     moveit_msgs.msg.DisplayTrajectory)
  28.  
  29. # print "\nWaiting for RVIZ..."
  30. # rospy.sleep(10)
  31.  
  32. print "\nReference frame: %s" % group.get_planning_frame()
  33. print "\nRobot Groups:"
  34. print robot.get_group_names()
  35. print "\nCurrent Pose:"
  36. print group.get_current_pose()
  37. # print "\nRobot State:"
  38. # print robot.get_current_state()
  39.  
  40. # group.set_goal_tolerance(10)
  41. # group.set_planning_time(10)
  42. # group.set_num_planning_attempts(20)
  43. # group.allow_replanning(True)
  44.  
  45. if IK:
  46.     print "\nGenerating IK..."
  47.    
  48.     pose_target = geometry_msgs.msg.Pose()
  49.     # pose_target = group.get_current_pose()
  50.  
  51.     # Set header
  52.     # pose_target.header.frame_id = "/world"
  53.  
  54.     # Set position
  55.     pose_target.position.x = -0.342596945962
  56.     pose_target.position.y = -0.350590067998
  57.     pose_target.position.z = 0.684848289954
  58.  
  59.     # Set orientation
  60.     # pose_target.orientation.x = 0.778503111668
  61.     # pose_target.orientation.y = -0.601031377053
  62.     # pose_target.orientation.z = 0.179308816961
  63.     # pose_target.orientation.w = 0.0232924253989
  64.  
  65.     random_pose_target = group.get_random_pose()
  66.  
  67.     # random_pose_target.pose.position.x = -0.342596945962
  68.     # random_pose_target.pose.position.y = -0.350590067998
  69.     # random_pose_target.pose.position.z = 0.684848289954
  70.    
  71.     xyz = [random_pose_target.pose.position.x,
  72.            random_pose_target.pose.position.y,
  73.            random_pose_target.pose.position.z]
  74.     print "\nRandom XYZ:",  xyz
  75.  
  76.     if RANDOM_POSE:    
  77.         print "\nRandom Pose:\n", random_pose_target
  78.         group.set_pose_target(random_pose_target, end_effector_link)
  79.     else:
  80.         print "\nSet Pose:\n", pose_target
  81.         # group.set_pose_target(pose_target, end_effector_link)
  82.         group.set_position_target(xyz, end_effector_link)
  83.    
  84.     plan = group.plan()
  85.  
  86. else:
  87.     print "\nGenerating Forward Kinematics..."
  88.     pose_target = geometry_msgs.msg.Pose()
  89.     group_variable_values = group.get_current_joint_values()
  90.     print "\nJoint Values:", group_variable_values
  91.    
  92.     # group_variable_values[3] = 0
  93.     group_variable_values = ARM_DEFAULT
  94.     group.set_joint_value_target(group_variable_values)
  95.     plan = group.plan()
  96.  
  97. print "\nWaiting while Rviz displays the plan..."
  98. rospy.sleep(1)
  99.  
  100. print "\n>>> Visualizing plan"
  101. display_trajectory = moveit_msgs.msg.DisplayTrajectory()
  102. display_trajectory.trajectory_start = robot.get_current_state()
  103. display_trajectory.trajectory.append(plan)
  104. display_trajectory_publisher.publish(display_trajectory);
  105.  
  106. print "\nWaiting while plan is visualized (again)..."
  107. rospy.sleep(1)
  108.  
  109. print "\n>>> Executing plan"
  110. group.go(wait=True)
  111.  
  112. moveit_commander.roscpp_shutdown()
RAW Paste Data