pick_and_place.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright 2013-2014, Unbounded Robotics, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Unbounded Robotics, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 #
33 # Author: Michael Ferguson
34 
35 import argparse
36 import copy
37 import math
38 import sys
39 
40 import rospy
41 import actionlib
42 from moveit_python import *
43 from moveit_python.geometry import rotate_pose_msg_by_euler_angles
44 
45 from grasping_msgs.msg import *
46 from moveit_msgs.msg import MoveItErrorCodes, PlaceLocation
47 
48 joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
49  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
50 ready_pose = [-1.393150, -0.103543, 0, -1.608378, -0.458660, -0.1, -2.611218]
51 
52 def move_to_ready(interface):
53  result = interface.moveToJointPosition(joint_names, ready_pose)
54  if result.error_code.val != 1:
55  rospy.sleep(1.0)
56  rospy.logerr("Move arm to ready position failed, trying again...")
57  result = interface.moveToJointPosition(joint_names, ready_pose, 0.02)
58 
59 if __name__=="__main__":
60  parser = argparse.ArgumentParser(description="Simple demo of pick and place")
61  parser.add_argument("--objects", help="Just do object perception", action="store_true")
62  parser.add_argument("--all", help="Just do object perception, but insert all objects", action="store_true")
63  parser.add_argument("--once", help="Run once.", action="store_true")
64  parser.add_argument("--ready", help="Move the arm to the ready position.", action="store_true")
65  parser.add_argument("--plan", help="Only do planning, no execution", action="store_true")
66  parser.add_argument("--x", help="Recommended x offset, how far out an object should roughly be.", type=float, default=0.5)
67  args, unknown = parser.parse_known_args()
68 
69  rospy.init_node("pick_and_place_demo")
70  move_group = MoveGroupInterface("arm", "base_link", plan_only = args.plan)
71 
72  # if all we want to do is prepare the arm, do it now
73  if args.ready:
74  move_to_ready(move_group)
75  exit(0)
76 
77  scene = PlanningSceneInterface("base_link")
78  pickplace = PickPlaceInterface("arm", "gripper", plan_only = args.plan, verbose = True)
79 
80  rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
81  find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
82  find_objects.wait_for_server()
83  rospy.loginfo("...connected")
84 
85  while not rospy.is_shutdown():
86  goal = FindGraspableObjectsGoal()
87  goal.plan_grasps = True
88  find_objects.send_goal(goal)
89  find_objects.wait_for_result(rospy.Duration(5.0))
90  find_result = find_objects.get_result()
91 
92  rospy.loginfo("Found %d objects" % len(find_result.objects))
93 
94  # remove all previous objects
95  for name in scene.getKnownCollisionObjects():
96  scene.removeCollisionObject(name, False)
97  for name in scene.getKnownAttachedObjects():
98  scene.removeAttachedObject(name, False)
99  scene.waitForSync()
100  # clear colors
101  scene._colors = dict()
102 
103  # insert objects, find the one to grasp
104  the_object = None
105  the_object_dist = 0.35
106  count = -1
107  for obj in find_result.objects:
108  count += 1
109  scene.addSolidPrimitive("object%d"%count,
110  obj.object.primitives[0],
111  obj.object.primitive_poses[0],
112  wait = False)
113  # object must have usable grasps
114  if len(obj.grasps) < 1:
115  continue
116  # choose object in front of robot
117  dx = obj.object.primitive_poses[0].position.x - args.x
118  dy = obj.object.primitive_poses[0].position.y
119  d = math.sqrt((dx * dx) + (dy * dy))
120  if d < the_object_dist:
121  the_object_dist = d
122  the_object = count
123 
124  if the_object == None:
125  rospy.logerr("Nothing to grasp! try again...")
126  continue
127 
128  # insert table
129  for obj in find_result.support_surfaces:
130  # extend surface to floor
131  height = obj.primitive_poses[0].position.z
132  obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
133  2.0, # make table wider
134  obj.primitives[0].dimensions[2] + height]
135  obj.primitive_poses[0].position.z += -height/2.0
136 
137  # add to scene
138  scene.addSolidPrimitive(obj.name,
139  obj.primitives[0],
140  obj.primitive_poses[0],
141  wait = False)
142 
143  obj_name = "object%d"%the_object
144 
145  # sync
146  scene.waitForSync()
147 
148  # set color of object we are grabbing
149  scene.setColor(obj_name, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange
150  scene.setColor(find_result.objects[the_object].object.support_surface, 0, 0, 0) # black
151  scene.sendColors()
152 
153  # exit now if we are just doing object update
154  if args.objects:
155  if args.once:
156  exit(0)
157  else:
158  continue
159 
160  # get grasps (we checked that they exist above)
161  grasps = find_result.objects[the_object].grasps
162  support_surface = find_result.objects[the_object].object.support_surface
163 
164  # call move_group to pick the object
165  rospy.loginfo("Beginning to pick.")
166  success, pick_result = pickplace.pick_with_retry(obj_name, grasps, support_name=support_surface, scene=scene)
167  if not success:
168  exit(-1)
169 
170  # create a set of place locations for the cube
171  places = list()
172  l = PlaceLocation()
173  l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0]
174  l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id
175  # invert the y of the pose
176  l.place_pose.pose.position.y = -l.place_pose.pose.position.y
177  # copy the posture, approach and retreat from the grasp used
178  l.post_place_posture = pick_result.grasp.pre_grasp_posture
179  l.pre_place_approach = pick_result.grasp.pre_grasp_approach
180  l.post_place_retreat = pick_result.grasp.post_grasp_retreat
181  places.append(copy.deepcopy(l))
182  # create another several places, rotate each by 90 degrees in yaw direction
183  l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
184  places.append(copy.deepcopy(l))
185  l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
186  places.append(copy.deepcopy(l))
187  l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
188  places.append(copy.deepcopy(l))
189 
190  # drop it like it's hot
191  rospy.loginfo("Beginning to place.")
192  while not rospy.is_shutdown():
193  # can't fail here or moveit needs to be restarted
194  success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene)
195  if success:
196  break
197 
198  # place arm back at side
199  move_to_ready(move_group)
200  rospy.loginfo("Ready...")
201 
202  # rinse and repeat
203  if args.once:
204  exit(0)
205 
msg
pick_and_place.move_to_ready
def move_to_ready(interface)
Definition: pick_and_place.py:52
moveit_python::geometry

simple_grasping
Author(s): Michael Ferguson
autogenerated on Wed Jun 5 2019 20:04:47