Advertisement
WintermuteAI

MoveIt Python pick code

Sep 18th, 2018
181
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 15.09 KB | None | 0 0
  1. #!/usr/bin/env python
  2.  
  3. """
  4.    moveit_pick_and_place_demo.py - Version 0.1 2014-01-14
  5.    
  6.    Command the gripper to grasp a target object and move it to a new location, all
  7.    while avoiding simulated obstacles.
  8.    
  9.    Created for the Pi Robot Project: http://www.pirobot.org
  10.    Copyright (c) 2014 Patrick Goebel.  All rights reserved.
  11.    This program is free software; you can redistribute it and/or modify
  12.    it under the terms of the GNU General Public License as published by
  13.    the Free Software Foundation; either version 2 of the License, or
  14.    (at your option) any later version.5
  15.    
  16.    This program is distributed in the hope that it will be useful,
  17.    but WITHOUT ANY WARRANTY; without even the implied warranty of
  18.    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19.    GNU General Public License for more details at:
  20.    
  21.    http://www.gnu.org/licenses/gpl.html
  22. """
  23.  
  24. import rospy, sys
  25. import moveit_commander
  26. from geometry_msgs.msg import PoseStamped, Pose
  27. from moveit_commander import MoveGroupCommander, PlanningSceneInterface
  28. from moveit_msgs.msg import PlanningScene, ObjectColor
  29. from moveit_msgs.msg import Grasp, GripperTranslation, MoveItErrorCodes
  30.  
  31. from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
  32. from tf.transformations import quaternion_from_euler
  33. from copy import deepcopy
  34.  
  35. GROUP_NAME_ARM = 'arm'
  36. GROUP_NAME_GRIPPER = 'gripper'
  37.  
  38. GRIPPER_FRAME = 'claw_base'
  39.  
  40. GRIPPER_OPEN = [0.3,0.3]
  41. GRIPPER_CLOSED = [0.0,0.0]
  42. GRIPPER_NEUTRAL = [0.009,0.009]
  43. GRIPPER_GRASP = [0.017,0.017]
  44.  
  45. GRIPPER_JOINT_NAMES = ['joint6','joint7']
  46.  
  47. GRIPPER_EFFORT = [1.0]
  48.  
  49. REFERENCE_FRAME = 'base_cylinder'
  50.  
  51. class MoveItDemo:
  52.     def __init__(self):
  53.         # Initialize the move_group API
  54.         moveit_commander.roscpp_initialize(sys.argv)
  55.        
  56.         rospy.init_node('moveit_demo')
  57.        
  58.         # Use the planning scene object to add or remove objects
  59.         scene = PlanningSceneInterface()
  60.        
  61.         # Create a scene publisher to push changes to the scene
  62.         self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
  63.        
  64.         # Create a publisher for displaying gripper poses
  65.         self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5)
  66.        
  67.         # Create a dictionary to hold object colors
  68.         self.colors = dict()
  69.                        
  70.         # Initialize the move group for the right arm
  71.         right_arm = MoveGroupCommander(GROUP_NAME_ARM)
  72.        
  73.         # Initialize the move group for the right gripper
  74.         right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
  75.        
  76.         # Get the name of the end-effector link
  77.         end_effector_link = right_arm.get_end_effector_link()
  78.  
  79.         # Allow some leeway in position (meters) and orientation (radians)
  80.         #right_arm.set_goal_position_tolerance(0.05)
  81.         #right_arm.set_goal_orientation_tolerance(0.1)
  82.  
  83.         # Allow replanning to increase the odds of a solution
  84.         right_arm.allow_replanning(True)
  85.        
  86.         # Set the right arm reference frame
  87.         #right_arm.set_pose_reference_frame(REFERENCE_FRAME)
  88.        
  89.         # Allow 5 seconds per planning attempt
  90.         right_arm.set_planning_time(3)
  91.        
  92.         # Set a limit on the number of pick attempts before bailing
  93.         max_pick_attempts = 5
  94.        
  95.         # Set a limit on the number of place attempts
  96.         max_place_attempts = 5
  97.                
  98.         # Give the scene a chance to catch up
  99.         rospy.sleep(2)
  100.  
  101.  
  102.  
  103.    
  104.         # clean the scene
  105.         scene.remove_world_object("table")
  106.         scene.remove_world_object("part")
  107.        
  108.         # Remove any attached objects from a previous session
  109.         scene.remove_attached_object(GRIPPER_FRAME, "part")
  110.        
  111.         # Give the scene a chance to catch up    
  112.         rospy.sleep(1)
  113.        
  114.         # Start the arm in the "grasp" pose stored in the SRDF file
  115.         right_arm.set_named_target('default')
  116.         right_arm.go()
  117.        
  118.         # Open the gripper to the neutral position
  119.         right_gripper.set_joint_value_target(GRIPPER_OPEN)
  120.         right_gripper.go()
  121.        
  122.         rospy.sleep(1)
  123.  
  124.  
  125.         place_pose = PoseStamped()
  126.         place_pose.header.frame_id = REFERENCE_FRAME
  127.         place_pose.pose.position.x = 0.170
  128.         place_pose.pose.position.y = 0.045
  129.         place_pose.pose.position.z = 0.3
  130.         scene.add_box("part", place_pose, (0.07, 0.01, 0.2))
  131.        
  132.         # Specify a pose to place the target after being picked up
  133.         target_pose = PoseStamped()
  134.         target_pose.header.frame_id = REFERENCE_FRAME
  135.         # start the gripper in a neutral pose part way to the target
  136.         target_pose.pose.position.x = 0.0733923763037
  137.         target_pose.pose.position.y = 0.0129100652412
  138.         target_pose.pose.position.z = 0.3097191751
  139.         target_pose.pose.orientation.x = -0.524236500263
  140.         target_pose.pose.orientation.y = 0.440069645643
  141.         target_pose.pose.orientation.z = -0.468739062548
  142.         target_pose.pose.orientation.w = 0.558389186859
  143.  
  144.  
  145.         # Initialize the grasp pose to the target pose
  146.         grasp_pose = target_pose
  147.     print("going to start pose")
  148.     right_arm.set_pose_target(target_pose)
  149.         right_arm.go()
  150.  
  151.         rospy.sleep(2)
  152.  
  153.        
  154.         # Shift the grasp pose by half the width of the target to center it
  155.         #grasp_pose.pose.position.y -= target_size[1] / 2.0
  156.         #grasp_pose.pose.position.x = 0.12792118579
  157.         #grasp_pose.pose.position.y = -0.285290879999
  158.         #grasp_pose.pose.position.z = 0.120301181892
  159.                
  160.         # Generate a list of grasps
  161.         grasps = self.make_grasps(grasp_pose, 'part')
  162.  
  163.         # Publish the grasp poses so they can be viewed in RViz
  164.         print "Publishing grasps"
  165.         for grasp in grasps:
  166.             self.gripper_pose_pub.publish(grasp.grasp_pose)
  167.             rospy.sleep(0.2)
  168.  
  169.         # Track success/failure and number of attempts for pick operation
  170.         result = None
  171.         n_attempts = 0
  172.        
  173.         # Repeat until we succeed or run out of attempts
  174.         while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
  175.             n_attempts += 1
  176.             rospy.loginfo("Pick attempt: " +  str(n_attempts))
  177.             result = right_arm.pick('part', grasps)
  178.             rospy.sleep(0.2)
  179.        
  180.         # If the pick was successful, attempt the place operation  
  181.         if result == MoveItErrorCodes.SUCCESS:
  182.             result = None
  183.             n_attempts = 0
  184.            
  185.             # Generate valid place poses
  186.             places = self.make_places(place_pose)
  187.            
  188.             # Repeat until we succeed or run out of attempts
  189.             while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
  190.                 n_attempts += 1
  191.                 rospy.loginfo("Place attempt: " +  str(n_attempts))
  192.                 for place in places:
  193.                     result = right_arm.place('part', place)
  194.                     if result == MoveItErrorCodes.SUCCESS:
  195.                         break
  196.                 rospy.sleep(0.2)
  197.                
  198.             if result != MoveItErrorCodes.SUCCESS:
  199.                 rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
  200.             else:
  201.                 # Return the arm to the "resting" pose stored in the SRDF file
  202.                 right_arm.set_named_target('right_arm_rest')
  203.                 right_arm.go()
  204.                
  205.                 # Open the gripper to the open position
  206.                 right_gripper.set_joint_value_target(GRIPPER_OPEN)
  207.                 right_gripper.go()
  208.         else:
  209.             rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
  210.  
  211.         rospy.sleep(1)
  212.  
  213.         # Shut down MoveIt cleanly
  214.         moveit_commander.roscpp_shutdown()
  215.        
  216.         # Exit the script
  217.         moveit_commander.os._exit(0)
  218.        
  219.     # Get the gripper posture as a JointTrajectory
  220.     def make_gripper_posture(self, joint_positions):
  221.         # Initialize the joint trajectory for the gripper joints
  222.         t = JointTrajectory()
  223.        
  224.         # Set the joint names to the gripper joint names
  225.         t.joint_names = GRIPPER_JOINT_NAMES
  226.        
  227.         # Initialize a joint trajectory point to represent the goal
  228.         tp = JointTrajectoryPoint()
  229.        
  230.         # Assign the trajectory joint positions to the input positions
  231.         tp.positions = joint_positions
  232.        
  233.         # Set the gripper effort
  234.         tp.effort = GRIPPER_EFFORT
  235.        
  236.         tp.time_from_start = rospy.Duration(1.0)
  237.        
  238.         # Append the goal point to the trajectory points
  239.         t.points.append(tp)
  240.        
  241.         # Return the joint trajectory
  242.         return t
  243.    
  244.     # Generate a gripper translation in the direction given by vector
  245.     def make_gripper_translation(self, min_dist, desired, vector):
  246.         # Initialize the gripper translation object
  247.         g = GripperTranslation()
  248.        
  249.         # Set the direction vector components to the input
  250.         g.direction.vector.x = vector[0]
  251.         g.direction.vector.y = vector[1]
  252.         g.direction.vector.z = vector[2]
  253.        
  254.         # The vector is relative to the gripper frame
  255.         g.direction.header.frame_id = GRIPPER_FRAME
  256.        
  257.         # Assign the min and desired distances from the input
  258.         g.min_distance = min_dist
  259.         g.desired_distance = desired
  260.        
  261.         return g
  262.  
  263.     # Generate a list of possible grasps
  264.     def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
  265.         # Initialize the grasp object
  266.         g = Grasp()
  267.        
  268.         # Set the pre-grasp and grasp postures appropriately
  269.         g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
  270.         g.grasp_posture = self.make_gripper_posture(GRIPPER_GRASP)
  271.                
  272.         # Set the approach and retreat parameters as desired
  273.         g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [0, 0, -1.0])
  274.         g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0, 0, 1.0])
  275.        
  276.         # Set the first grasp pose to the input pose
  277.         g.grasp_pose = initial_pose_stamped
  278.    
  279.         ideal_roll = 0
  280.         ideal_pitch = 0
  281.         ideal_yaw = -1.57
  282.        
  283.         step_size = 0.1
  284.         idx = 0.1
  285.         idx_roll = ideal_roll + idx
  286.         idx_pitch = ideal_pitch + idx
  287.         idx_yaw = ideal_yaw + idx
  288.         roll_vals = []
  289.         pitch_vals = []
  290.         yaw_vals = []
  291.         while idx >= -0.1:
  292.             roll_vals.append(idx_roll)
  293.             pitch_vals.append(idx_pitch)
  294.             yaw_vals.append(idx_yaw)
  295.             idx -= step_size
  296.             idx_roll -= step_size
  297.             idx_pitch -= step_size
  298.             idx_yaw -= step_size
  299.  
  300.         # A list to hold the grasps
  301.         grasps = []
  302.        
  303.         print "Generating Poses"
  304.        
  305.         # Generate a grasp for each roll pitch and yaw angle
  306.         for r in roll_vals:
  307.             for y in yaw_vals:
  308.                 for p in pitch_vals:
  309.                     # Create a quaternion from the Euler angles
  310.                     q = quaternion_from_euler(r, p, y)
  311.                    
  312.                     # Set the grasp pose orientation accordingly
  313.                     g.grasp_pose.pose.orientation.x = q[0]
  314.                     g.grasp_pose.pose.orientation.y = q[1]
  315.                     g.grasp_pose.pose.orientation.z = q[2]
  316.                     g.grasp_pose.pose.orientation.w = q[3]
  317.                    
  318.                     # Set and id for this grasp (simply needs to be unique)
  319.                     g.id = str(len(grasps))
  320.                    
  321.                     # Set the allowed touch objects to the input list
  322.                     g.allowed_touch_objects = allowed_touch_objects
  323.                    
  324.                     # Don't restrict contact force
  325.                     g.max_contact_force = 0
  326.                    
  327.                     # Degrade grasp quality for increasing pitch angles
  328.                     g.grasp_quality = 1.0 - abs(p)
  329.                    
  330.                     # Append the grasp to the list
  331.                     grasps.append(deepcopy(g))
  332.                    
  333.         print "Generated " + g.id + " poses"
  334.         # Return the list
  335.         return grasps
  336.    
  337.     # Generate a list of possible place poses
  338.     def make_places(self, init_pose):
  339.         # Initialize the place location as a PoseStamped message
  340.         place = PoseStamped()
  341.        
  342.         # Start with the input place pose
  343.         place = init_pose
  344.        
  345.         # A list of x shifts (meters) to try
  346.         x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
  347.        
  348.         # A list of y shifts (meters) to try
  349.         y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
  350.        
  351.         pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
  352.        
  353.         # A list of yaw angles to try
  354.         yaw_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
  355.  
  356.         # A list to hold the places
  357.         places = []
  358.        
  359.         # Generate a place pose for each angle and translation
  360.         for y in yaw_vals:
  361.             for p in pitch_vals:
  362.                 for y in y_vals:
  363.                     for x in x_vals:
  364.                         place.pose.position.x = init_pose.pose.position.x + x
  365.                         place.pose.position.y = init_pose.pose.position.y + y
  366.                        
  367.                         # Create a quaternion from the Euler angles
  368.                         q = quaternion_from_euler(0, p, y)
  369.                        
  370.                         # Set the place pose orientation accordingly
  371.                         place.pose.orientation.x = q[0]
  372.                         place.pose.orientation.y = q[1]
  373.                         place.pose.orientation.z = q[2]
  374.                         place.pose.orientation.w = q[3]
  375.                        
  376.                         # Append this place pose to the list
  377.                         places.append(deepcopy(place))
  378.        
  379.         # Return the list
  380.         return places
  381.    
  382.     # Set the color of an object
  383.     def setColor(self, name, r, g, b, a = 0.9):
  384.         # Initialize a MoveIt color object
  385.         color = ObjectColor()
  386.        
  387.         # Set the id to the name given as an argument
  388.         color.id = name
  389.        
  390.         # Set the rgb and alpha values given as input
  391.         color.color.r = r
  392.         color.color.g = g
  393.         color.color.b = b
  394.         color.color.a = a
  395.        
  396.         # Update the global color dictionary
  397.         self.colors[name] = color
  398.  
  399.     # Actually send the colors to MoveIt!
  400.     def sendColors(self):
  401.         # Initialize a planning scene object
  402.         p = PlanningScene()
  403.  
  404.         # Need to publish a planning scene diff        
  405.         p.is_diff = True
  406.        
  407.         # Append the colors from the global color dictionary
  408.         for color in self.colors.values():
  409.             p.object_colors.append(color)
  410.        
  411.         # Publish the scene diff
  412.         self.scene_pub.publish(p)
  413.  
  414. if __name__ == "__main__":
  415.     MoveItDemo()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement