Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
00059
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)
00064 self.papm.move_arm_to_side(1)
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
00088
00089
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
00113 table_height = .72
00114
00115
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)
00119
00120 if success:
00121
00122
00123 place_rect_dims = [.3, .3]
00124
00125
00126 center_xyz = [.5, -.15, table_height-.05]
00127
00128
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