$search
00001 #!/usr/bin/python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2009, Willow Garage, Inc. 00005 # All rights reserved. 00006 # 00007 # Redistribution and use in source and binary forms, with or without 00008 # modification, are permitted provided that the following conditions 00009 # are met: 00010 # 00011 # * Redistributions of source code must retain the above copyright 00012 # notice, this list of conditions and the following disclaimer. 00013 # * Redistributions in binary form must reproduce the above 00014 # copyright notice, this list of conditions and the following 00015 # disclaimer in the documentation and/or other materials provided 00016 # with the distribution. 00017 # * Neither the name of the Willow Garage nor the names of its 00018 # contributors may be used to endorse or promote products derived 00019 # from this software without specific prior written permission. 00020 # 00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 # POSSIBILITY OF SUCH DAMAGE. 00033 # 00034 # author: Kaijen Hsiao 00035 00036 """ 00037 Simple example of using the pick_and_place_manager to pick up an object 00038 near a specified point, and to put it back down in a desired region. 00039 Launch pr2_tabletop_manipulation_launch pr2_tabletop_manipulation.launch 00040 before running. Works better with fingertip sensors installed. 00041 """ 00042 00043 import roslib 00044 roslib.load_manifest('pr2_pick_and_place_demos') 00045 import rospy 00046 from pr2_pick_and_place_demos.pick_and_place_manager import * 00047 from object_manipulator.convert_functions import * 00048 00049 class SimplePickAndPlaceExample(): 00050 00051 def __init__(self): 00052 00053 rospy.loginfo("initializing pick and place manager") 00054 self.papm = PickAndPlaceManager() 00055 rospy.loginfo("finished initializing pick and place manager") 00056 00057 00058 #pick up the nearest object to PointStamped target_point with whicharm 00059 #(0=right, 1=left) 00060 def pick_up_object_near_point(self, target_point, whicharm): 00061 00062 rospy.loginfo("moving the arms to the side") 00063 self.papm.move_arm_to_side(0) #right arm 00064 self.papm.move_arm_to_side(1) #left arm 00065 00066 rospy.loginfo("pointing the head at the target point") 00067 self.papm.point_head(get_xyz(target_point.point), 00068 target_point.header.frame_id) 00069 00070 rospy.loginfo("detecting the table and objects") 00071 self.papm.call_tabletop_detection(update_table = 1, update_place_rectangle = 1, 00072 clear_attached_objects = 1) 00073 00074 rospy.loginfo("picking up the nearest object to the target point") 00075 success = self.papm.pick_up_object_near_point(target_point, 00076 whicharm) 00077 00078 if success: 00079 rospy.loginfo("pick-up was successful! Moving arm to side") 00080 self.papm.move_arm_to_side(whicharm) 00081 else: 00082 rospy.loginfo("pick-up failed.") 00083 00084 return success 00085 00086 00087 #place the object held in whicharm (0=right, 1=left) down in the 00088 #place rectangle defined by place_rect_dims (x,y) 00089 #and place_rect_center (PoseStamped) 00090 def place_object(self, whicharm, place_rect_dims, place_rect_center): 00091 00092 self.papm.set_place_area(place_rect_center, place_rect_dims) 00093 00094 rospy.loginfo("putting down the object in the %s gripper"\ 00095 %self.papm.arm_dict[whicharm]) 00096 success = self.papm.put_down_object(whicharm, 00097 max_place_tries = 25, 00098 use_place_override = 1) 00099 00100 if success: 00101 rospy.loginfo("place returned success") 00102 else: 00103 rospy.loginfo("place returned failure") 00104 00105 return success 00106 00107 00108 if __name__ == "__main__": 00109 rospy.init_node('simple_pick_and_place_example') 00110 sppe = SimplePickAndPlaceExample() 00111 00112 #adjust for your table 00113 table_height = .72 00114 00115 #.5 m in front of robot, centered 00116 target_point_xyz = [.5, 0, table_height-.05] 00117 target_point = create_point_stamped(target_point_xyz, 'base_link') 00118 success = sppe.pick_up_object_near_point(target_point, 0) #right arm 00119 00120 if success: 00121 00122 #square of size 30 cm by 30 cm 00123 place_rect_dims = [.3, .3] 00124 00125 #.5 m in front of robot, to the right 00126 center_xyz = [.5, -.15, table_height-.05] 00127 00128 #aligned with axes of frame_id 00129 center_quat = [0,0,0,1] 00130 place_rect_center = create_pose_stamped(center_xyz+center_quat, 00131 'base_link') 00132 00133 sppe.place_object(0, place_rect_dims, place_rect_center) 00134 00135