pick_and_place.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright 2013-2014, Unbounded Robotics, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Unbounded Robotics, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 # Author: Michael Ferguson
00034 
00035 import argparse
00036 import copy
00037 import math
00038 import sys
00039 
00040 import rospy
00041 import actionlib
00042 from moveit_python import *
00043 from moveit_python.geometry import rotate_pose_msg_by_euler_angles
00044 
00045 from grasping_msgs.msg import *
00046 from moveit_msgs.msg import MoveItErrorCodes, PlaceLocation
00047 
00048 joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
00049     "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
00050 ready_pose = [-1.393150, -0.103543, 0, -1.608378, -0.458660, -0.1, -2.611218]
00051 
00052 def move_to_ready(interface):
00053    result = interface.moveToJointPosition(joint_names, ready_pose)
00054    if result.error_code.val != 1:
00055        rospy.sleep(1.0)
00056        rospy.logerr("Move arm to ready position failed, trying again...")
00057        result = interface.moveToJointPosition(joint_names, ready_pose, 0.02)
00058 
00059 if __name__=="__main__":
00060     parser = argparse.ArgumentParser(description="Simple demo of pick and place")
00061     parser.add_argument("--objects", help="Just do object perception", action="store_true")
00062     parser.add_argument("--all", help="Just do object perception, but insert all objects", action="store_true")
00063     parser.add_argument("--once", help="Run once.", action="store_true")
00064     parser.add_argument("--ready", help="Move the arm to the ready position.", action="store_true")
00065     parser.add_argument("--plan", help="Only do planning, no execution", action="store_true")
00066     parser.add_argument("--x", help="Recommended x offset, how far out an object should roughly be.", type=float, default=0.5)
00067     args, unknown = parser.parse_known_args()
00068 
00069     rospy.init_node("pick_and_place_demo")
00070     move_group = MoveGroupInterface("arm", "base_link", plan_only = args.plan)
00071 
00072     # if all we want to do is prepare the arm, do it now
00073     if args.ready:
00074         move_to_ready(move_group)
00075         exit(0)
00076 
00077     scene = PlanningSceneInterface("base_link")
00078     pickplace = PickPlaceInterface("arm", "gripper", plan_only = args.plan, verbose = True)
00079 
00080     rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
00081     find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
00082     find_objects.wait_for_server()
00083     rospy.loginfo("...connected")
00084 
00085     while not rospy.is_shutdown():
00086         goal = FindGraspableObjectsGoal()
00087         goal.plan_grasps = True
00088         find_objects.send_goal(goal)
00089         find_objects.wait_for_result(rospy.Duration(5.0))
00090         find_result = find_objects.get_result()
00091 
00092         rospy.loginfo("Found %d objects" % len(find_result.objects))
00093 
00094         # remove all previous objects
00095         for name in scene.getKnownCollisionObjects():
00096             scene.removeCollisionObject(name, False)
00097         for name in scene.getKnownAttachedObjects():
00098             scene.removeAttachedObject(name, False)
00099         scene.waitForSync()
00100         # clear colors
00101         scene._colors = dict()
00102 
00103         # insert objects, find the one to grasp
00104         the_object = None
00105         the_object_dist = 0.35
00106         count = -1
00107         for obj in find_result.objects:
00108             count += 1
00109             scene.addSolidPrimitive("object%d"%count,
00110                                     obj.object.primitives[0],
00111                                     obj.object.primitive_poses[0],
00112                                     wait = False)
00113             # object must have usable grasps
00114             if len(obj.grasps) < 1:
00115                 continue
00116             # choose object in front of robot
00117             dx = obj.object.primitive_poses[0].position.x - args.x
00118             dy = obj.object.primitive_poses[0].position.y
00119             d = math.sqrt((dx * dx) + (dy * dy))
00120             if d < the_object_dist:
00121                 the_object_dist = d
00122                 the_object = count
00123 
00124         if the_object == None:
00125             rospy.logerr("Nothing to grasp! try again...")
00126             continue
00127 
00128         # insert table
00129         for obj in find_result.support_surfaces:
00130             # extend surface to floor
00131             height = obj.primitive_poses[0].position.z
00132             obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
00133                                             2.0, # make table wider
00134                                             obj.primitives[0].dimensions[2] + height]
00135             obj.primitive_poses[0].position.z += -height/2.0
00136 
00137             # add to scene
00138             scene.addSolidPrimitive(obj.name,
00139                                     obj.primitives[0],
00140                                     obj.primitive_poses[0],
00141                                     wait = False)
00142 
00143         obj_name = "object%d"%the_object
00144 
00145         # sync
00146         scene.waitForSync()
00147 
00148         # set color of object we are grabbing
00149         scene.setColor(obj_name, 223.0/256.0, 90.0/256.0, 12.0/256.0)  # orange
00150         scene.setColor(find_result.objects[the_object].object.support_surface, 0, 0, 0)  # black
00151         scene.sendColors()
00152 
00153         # exit now if we are just doing object update
00154         if args.objects:
00155             if args.once:
00156                 exit(0)
00157             else:
00158                 continue
00159 
00160         # get grasps (we checked that they exist above)
00161         grasps = find_result.objects[the_object].grasps
00162         support_surface = find_result.objects[the_object].object.support_surface
00163 
00164         # call move_group to pick the object
00165         rospy.loginfo("Beginning to pick.")
00166         success, pick_result = pickplace.pick_with_retry(obj_name, grasps, support_name=support_surface, scene=scene)
00167         if not success:
00168             exit(-1)
00169 
00170         # create a set of place locations for the cube
00171         places = list()
00172         l = PlaceLocation()
00173         l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0]
00174         l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id
00175         # invert the y of the pose
00176         l.place_pose.pose.position.y = -l.place_pose.pose.position.y
00177         # copy the posture, approach and retreat from the grasp used
00178         l.post_place_posture = pick_result.grasp.pre_grasp_posture
00179         l.pre_place_approach = pick_result.grasp.pre_grasp_approach
00180         l.post_place_retreat = pick_result.grasp.post_grasp_retreat
00181         places.append(copy.deepcopy(l))
00182         # create another several places, rotate each by 90 degrees in yaw direction
00183         l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
00184         places.append(copy.deepcopy(l))
00185         l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
00186         places.append(copy.deepcopy(l))
00187         l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
00188         places.append(copy.deepcopy(l))
00189 
00190         # drop it like it's hot
00191         rospy.loginfo("Beginning to place.")
00192         while not rospy.is_shutdown():
00193             # can't fail here or moveit needs to be restarted
00194             success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene)
00195             if success:
00196                 break
00197 
00198         # place arm back at side
00199         move_to_ready(move_group)
00200         rospy.loginfo("Ready...")
00201 
00202         # rinse and repeat
00203         if args.once:
00204             exit(0)
00205 


simple_grasping
Author(s): Michael Ferguson
autogenerated on Fri Aug 26 2016 13:17:24