scripts/pick_and_place.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 import sys
   5 import rospkg
   6 import rospy
   7 import moveit_commander
   8 import tf
   9 import numpy as np
  10 from math import *
  11 from geometry_msgs.msg import PoseStamped
  12 from moveit_msgs.msg import PlanningScene, ObjectColor
  13 from moveit_msgs.msg import Grasp, GripperTranslation
  14 from moveit_msgs.msg import MoveItErrorCodes
  15 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
  16 from tf.transformations import quaternion_from_euler
  17
  18 class MoveItDemo:
  19         def __init__(self):
  20                 # Initialize the move_group API
  21                 moveit_commander.roscpp_initialize(sys.argv)
  22
  23                 rospack = rospkg.RosPack()
  24                 rospy.init_node('moveit_demo')
  25
  26                 self.tflistener = tf.TransformListener()
  27                 scene   = moveit_commander.PlanningSceneInterface()
  28                 # Create a scene publisher to push changes to the scene
  29                 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=16)
  30                 self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
  31                 self.arm        = moveit_commander.MoveGroupCommander("arm")
  32                 self.arm.set_planner_id("RRTkConfigDefault");
  33                 self.gripper = moveit_commander.MoveGroupCommander("gripper")
  34
  35                 scene.remove_world_object("tube1")
  36                 scene.remove_attached_object("gripper_center", "tube1")
  37                 self.arm.set_goal_position_tolerance(0.04)
  38                 self.arm.set_goal_orientation_tolerance(5*pi/180)
  39
  40                 self.gripper_open()
  41                 self.arm.set_named_target('Home')
  42                 self.arm.go()
  43
  44                 tube1_pose = PoseStamped()
  45                 tube1_pose.header.frame_id = "/arm_base"
  46                 tube1_pose.pose.position.x = 0.5
  47                 tube1_pose.pose.position.y = 0.0
  48                 tube1_pose.pose.position.z = 0.03
  49                 tube1_pose.pose.orientation.w = 1.0
  50                 scene.add_mesh("tube1", tube1_pose, rospack.get_path('arm_ros_conn') + "/meshes/vrohr.stl")
  51                 self.setColor("tube1", 1.0, 1.0, 0, 1.0)
  52
  53                 place_pose = PoseStamped()
  54                 place_pose.header.frame_id = "/arm_base"
  55                 place_pose.pose.position.x = -0.5
  56                 place_pose.pose.position.y = 0.0
  57                 place_pose.pose.position.z = 0.03
  58                 place_pose.pose.orientation.w = 1.0
  59
  60                 rospy.sleep(2) # Give the scene a chance to catch up
  61
  62                 tube1_pose.pose.position.z += 0.02
  63                 if self.pick("tube1", tube1_pose):
  64                         rospy.sleep(2) # Give the scene a chance to catch up
  65                         self.place("tube1", place_pose)
  66                         rospy.sleep(2) # Give the scene a chance to catch up
  67
  68                 self.arm.set_named_target('Home')
  69                 self.arm.go()
  70                 rospy.sleep(2) # Give the scene a chance to catch up
  71
  72                 if self.pick("tube1", place_pose):
  73                         rospy.sleep(2) # Give the scene a chance to catch up
  74                         self.place("tube1", tube1_pose)
  75                         rospy.sleep(2) # Give the scene a chance to catch up
  76
  77                 # Shut down MoveIt cleanly
  78                 moveit_commander.roscpp_shutdown()
  79
  80                 # Exit the script
  81                 moveit_commander.os._exit(0)
  82
  83         def pick(self, item, pose):
  84                 # Generate a list of grasps
  85                 grasps = self.make_grasps(pose, [item], [5*pi/180] * 2)
  86                 for grasp in grasps:
  87                         self.gripper_pose_pub.publish(grasp.grasp_pose)
  88                         rospy.sleep(0.2)
  89                 # Repeat until we succeed or run out of attempts
  90                 for i in range(3):
  91                         rospy.loginfo("Pick attempt: " + str(i))
  92                         result = self.arm.pick(item, grasps)
  93                         if result == MoveItErrorCodes.SUCCESS:
  94                                 rospy.loginfo("Pick succeeded")
  95                                 return True
  96                         rospy.loginfo("Pick failed")
  97                         rospy.sleep(0.2)
  98
  99                 return False
 100
 101         def place(self, item, place_pose):
 102                 # Generate valid place poses
 103                 # Rotate the gripper around the z axis to let it point to the target, reference is the x axis
 104                 v = place_pose.pose.position.__getstate__()[:2] # x, y component of position
 105                 v /= np.linalg.norm(v) # unit vector
 106                 q = quaternion_from_euler(0, 0, acos(np.dot(v, [1, 0]))) # use the dot product to get the angle
 107                 place_pose.pose.orientation.x = q[0]
 108                 place_pose.pose.orientation.y = q[1]
 109                 place_pose.pose.orientation.z = q[2]
 110                 place_pose.pose.orientation.w = q[3]
 111
 112                 # Publish the place pose so they it be viewed in RViz
 113                 self.gripper_pose_pub.publish(place_pose)
 114                 rospy.sleep(0.2)
 115
 116                 for i in range(3):
 117                         rospy.loginfo("Place attempt: " + str(i))
 118                         if self.arm.place(item, place_pose):
 119                                 rospy.loginfo("Place succeeded")
 120                                 break
 121                         rospy.sleep(0.2)
 122
 123         def gripper_to(self, to):
 124                 self.gripper.set_joint_value_target([to, to])
 125                 self.gripper.go()
 126
 127         def gripper_open(self):
 128                 self.gripper.set_named_target('open')
 129                 self.gripper.go()
 130
 131         def grasp_tube(self):
 132                 self.gripper_to(0.05)
 133
 134         # See http://wiki.ros.org/turtlebot_arm_interactive_markers/Tutorials/UsingArmInteractiveMarkers?action=AttachFile&do=get&target=arm_markers_rviz6.jpgt the color of an object
 135         def setColor(self, name, r, g, b, a=0.9):
 136                 # Initialize a MoveIt color object
 137                 color = ObjectColor()
 138
 139                 # Set the id to the name given as an argument
 140                 color.id = name
 141
 142                 # Set the rgb and alpha values given as input
 143                 color.color.r = r
 144                 color.color.g = g
 145                 color.color.b = b
 146                 color.color.a = a
 147
 148                 # Initialize a planning scene object
 149                 p = PlanningScene()
 150                 # Need to publish a planning scene diff
 151                 p.is_diff = True
 152                 p.object_colors.append(color)
 153                 # Publish the scene diff
 154                 self.scene_pub.publish(p)
 155
 156         # Get the gripper posture as a JointTrajectory
 157         def make_gripper_posture(self, joint_positions):
 158                 # Initialize the joint trajectory for the gripper joints
 159                 t = JointTrajectory()
 160
 161                 # Set the joint names to the gripper joint names
 162                 t.joint_names = ["left_gripper_joint", "right_gripper_joint"]
 163
 164                 # Initialize a joint trajectory point to represent the goal
 165                 tp = JointTrajectoryPoint()
 166
 167                 # Assign the trajectory joint positions to the input positions
 168                 tp.positions = joint_positions
 169
 170                 # Set the gripper effort
 171                 tp.effort = [1.0, 1.0]
 172
 173                 tp.time_from_start = rospy.Duration(2.0)
 174
 175                 # Append the goal point to the trajectory points
 176                 t.points.append(tp)
 177
 178                 # Return the joint trajectory
 179                 return t
 180
 181         # Generate a gripper translation in the direction given by vector
 182         def make_gripper_translation(self, min_dist, desired, vector):
 183                 # Initialize the gripper translation object
 184                 g = GripperTranslation()
 185
 186                 # Set the direction vector components to the input
 187                 g.direction.vector.x = vector[0]
 188                 g.direction.vector.y = vector[1]
 189                 g.direction.vector.z = vector[2]
 190
 191                 # The vector is relative to the gripper frame
 192                 g.direction.header.frame_id = "gripper_center"
 193
 194                 # Assign the min and desired distances from the input
 195                 g.min_distance = min_dist
 196                 g.desired_distance = desired
 197
 198                 return g
 199
 200         # Generate a list of possible grasps
 201         def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0, 0]):
 202                 # Initialize the grasp object
 203                 g = Grasp()
 204
 205                 # Set the pre-grasp and grasp postures appropriately;
 206                 # grasp_opening should be a bit smaller than target width
 207                 g.pre_grasp_posture = self.make_gripper_posture([0.35, 0.35])
 208                 g.grasp_posture = self.make_gripper_posture(grasp_opening)
 209
 210                 # Set the approach and retreat parameters as desired
 211                 g.pre_grasp_approach = self.make_gripper_translation(0.05, 0.1, [1.0, 0.0, 0.0])
 212                 g.post_grasp_retreat = self.make_gripper_translation(0.05, 0.1, [0.0, 0.0, 1.0])
 213
 214                 # Set the first grasp pose to the input pose
 215                 g.grasp_pose = initial_pose_stamped
 216
 217                 # A list to hold the grasps
 218                 grasps = []
 219
 220                 # Rotate the gripper around the z axis to let it point to the target, reference is the x axis
 221                 v = initial_pose_stamped.pose.position.__getstate__()[:2] # x, y component of position
 222                 v /= np.linalg.norm(v) # unit vector
 223                 q = quaternion_from_euler(0, 0, acos(np.dot(v, [1, 0]))) # use the dot product to get the angle
 224
 225                 # Set the grasp pose orientation accordingly
 226                 g.grasp_pose.pose.orientation.x = q[0]
 227                 g.grasp_pose.pose.orientation.y = q[1]
 228                 g.grasp_pose.pose.orientation.z = q[2]
 229                 g.grasp_pose.pose.orientation.w = q[3]
 230
 231                 # Set and id for this grasp (simply needs to be unique)
 232                 g.id = str(len(grasps))
 233
 234                 # Set the allowed touch objects to the input list
 235                 g.allowed_touch_objects = allowed_touch_objects
 236
 237                 # Don't restrict contact force
 238                 g.max_contact_force = 0
 239
 240                 # Degrade grasp quality for increasing pitch angles
 241                 g.grasp_quality = 1.0
 242
 243                 # Append the grasp to the list
 244                 grasps.append(g)
 245
 246                 # Return the list
 247                 return grasps
 248
 249 if __name__ == "__main__":
 250         MoveItDemo()