$search
00001 #!/usr/bin/python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2009, Bosch LLC 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: Sarah Osentoski 00035 00036 ## @package pick_and_place_manager 00037 # Functions to grasp objects off flat tables and place them down in 00038 # specified rectangular regions in space 00039 00040 import roslib 00041 roslib.load_manifest('pr2_pick_and_place_service') 00042 import rospy 00043 from object_manipulation_msgs.msg import PickupAction, PickupGoal, \ 00044 PlaceAction, PlaceGoal, GripperTranslation, ReactivePlaceAction, ReactivePlaceGoal 00045 from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxRequest 00046 from object_manipulation_msgs.msg import ManipulationResult, GraspableObject 00047 from tabletop_object_detector.srv import TabletopDetection, TabletopDetectionRequest 00048 from tabletop_object_detector.msg import TabletopDetectionResult 00049 from tabletop_collision_map_processing.srv import \ 00050 TabletopCollisionMapProcessing, TabletopCollisionMapProcessingRequest 00051 from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal, Pr2GripperCommand 00052 from pr2_pick_and_place_service.srv import * 00053 from pr2_pick_and_place_service.msg import * 00054 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 00055 import tf 00056 import actionlib 00057 00058 00059 from pr2_pick_and_place_demos.pick_and_place_manager import * 00060 00061 ##Manager for pick and place actions 00062 class PickAndPlaceService(): 00063 00064 00065 def __init__(self, use_slip_controller = 0, use_slip_detection = 0): 00066 00067 #pick and place manager from demos 00068 self.pickandplacemanager=PickAndPlaceManager() 00069 00070 self.whicharm=1 #gave default arm 00071 self. lgripper=rospy.Publisher('/l_gripper_controller/command', Pr2GripperCommand) 00072 self. rgripper=rospy.Publisher('/r_gripper_controller/command', Pr2GripperCommand) 00073 00074 self.headpub=rospy.Publisher('/head_traj_controller/command', JointTrajectory, latch=True) 00075 self.headx=0 00076 self.heady=0 00077 self.table_detected=False 00078 00079 #Definition for Provided Services 00080 self.pickplaceservice_detachobject=rospy.Service('pick_and_place_detach_object', DetachObjectFromGripper, self.detach_object_service_interface) 00081 self.pickplaceservice_detectobjects=rospy.Service('pick_and_place_detect_object', DetectObjects, self.detect_object_service_interface) 00082 self.pickplaceservice_detecttable=rospy.Service('pick_and_place_detect_table', DetectTable, self.detect_table_service_interface) 00083 self.pickplaceservice_pickupobject=rospy.Service('pick_and_place_pickup_object', PickUpObject, self.pickup_object_service_interface) 00084 self.pickplaceservice_pointhead=rospy.Service('pick_and_place_point_head', PointHead, self.point_head_service_interface) 00085 self.pickplaceservice_selectarm=rospy.Service('pick_and_place_select_arm', SelectArm, self.select_arm_service_interface) 00086 self.pickplaceservice_switchsides=rospy.Service('pick_and_place_switch_sides', SwitchPickupPutdownSides, self.switch_sides_service_interface) 00087 self.pickplaceservice_placeobject=rospy.Service('pick_and_place_place_object', PlaceObject, self.place_object_service_interface) 00088 self.pickplaceservice_armtoside=rospy.Service('pick_and_place_move_arm_to_side', MoveArmToSide, self.move_arm_to_side_service_interface) 00089 self.pickplaceservice_armtofront=rospy.Service('pick_and_place_move_arm_to_front', MoveArmToFront, self.move_arm_to_front_service_interface) 00090 00091 00092 #arm-away joint angles 00093 self.arm_above_and_to_side_angles = [[-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501], 00094 [0.968, 0.729, 0.554, -1.891, 1.786, -1.127, 0.501]] 00095 self.arm_to_side_angles = [[-2.135, 0.803, -1.732, -1.905, -2.369, -1.680, 1.398], 00096 [2.135, 0.803, 1.732, -1.905, 2.369, -1.680, 1.398]] 00097 self.arm_above_and_to_front_angles=[[-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501], [0.968, 0.729, 0.554, -1.891, 1.786, -1.127, 0.501]] 00098 self.arm_to_front_angles=[[0.0, 1.203, 0.0, -2.105, -3.13, -1.680, 1.398],[0.0, 1.203, 0.0, -2.105, 3.13, -1.680, 1.398]] 00099 00100 00101 def getObjects(self): 00102 objectlist=[]; 00103 print objectlist 00104 print type(self.pickandplacemanager.detected_objects) 00105 for (ind, object) in enumerate(self.pickandplacemanager.detected_objects): 00106 print "indext:" 00107 print ind 00108 ob=PickPlaceObject() 00109 ob.objectid=ind 00110 ob.pose=object.pose 00111 ob.object=object.object 00112 print type(ob) 00113 print type(ob.object) 00114 print type(object.pose) 00115 print type(object.box_dims) 00116 ob.boundingbox=object.box_dims 00117 00118 objectlist.append(ob) 00119 print len(objectlist) 00120 print type(objectlist) 00121 return objectlist 00122 00123 #opens hand to drop object 00124 def drop_object(self, arm): 00125 gripper_command=Pr2GripperCommand() 00126 gripper_command.position=.08 00127 gripper_command.max_effort=-1; 00128 if arm=='l': 00129 self.lgripper.publish(gripper_command) 00130 else: 00131 self. rgripper.publish(gripper_command) 00132 00133 #opens hand to drop object 00134 #future improvement: check that something is in the hand 00135 def detach_object_service_interface(self, req): 00136 #set the current positions of the wrists as the current 'goals' relative to which Cartesian movements are expressed 00137 arm=req.arm 00138 00139 if arm=='r'or arm=='l': 00140 self.drop_object(arm) 00141 00142 else: 00143 return DetachObjectFromGripperResponse(False) 00144 00145 return DetachObjectFromGripperResponse(True) 00146 00147 #detects object 00148 def detect_object_service_interface(self, req): 00149 command=req.command 00150 if not self.table_detected: 00151 self.pickandplacemanager.find_table() 00152 00153 if command=='d': 00154 rospy.loginfo("detecting objects") 00155 self.pickandplacemanager.call_tabletop_detection(update_table = 0, clear_attached_objects = 0) 00156 elif input == 'da': 00157 rospy.loginfo("detecting objects and clearing attached objects") 00158 self.pickandplacemanager.call_tabletop_detection( update_table = 0, clear_attached_objects = 1) 00159 elif input == 'dp': 00160 rospy.loginfo("detecting objects, taking a new static collision map, and clearing attached objects") 00161 self.pickandplacemanager.call_tabletop_detection(update_table = 0, clear_attached_objects = 1, num_models = 5) 00162 00163 #update current arm goals 00164 print('returned from service call') 00165 objectlist=self.getObjects() 00166 print('length of objectlist') 00167 00168 print(len(objectlist)) 00169 rvar= DetectObjectsResponse(objectlist) 00170 print(len(rvar.objects)) 00171 00172 return DetectObjectsResponse(objectlist) #change this to 00173 00174 def detect_table_service_interface(self, req): 00175 rospy.loginfo("finding the table") 00176 self.pickandplacemanager.find_table() 00177 print "found table" 00178 return DetectTableResponse(self.pickandplacemanager.table_height, self.pickandplacemanager.table_front_edge_x, self.pickandplacemanager.place_rect_dims) #change this to 00179 00180 def move_head(self, dir): 00181 delta=.01 00182 negdelta=-.01 00183 if dir=='l': 00184 self.headx=self.headx+negdelta 00185 elif dir=='r': 00186 self.headx=self.headx+delta 00187 elif dir=='u': 00188 print "moving head down" 00189 print negdelta 00190 self.heady=self.heady+negdelta 00191 elif dir =='d' : 00192 self.heady=self.heady+delta 00193 00194 if self.headx < -2.8: 00195 self.headx=-2.8 00196 elif self.headx> 2.8: 00197 self.headx=2.8 00198 00199 if self.heady< -.24: 00200 self.heady=-.24 00201 elif self.heady> 1.16: 00202 self.heady=1.16 00203 print self.headx 00204 print self.heady 00205 traj=JointTrajectory() 00206 traj.joint_names = ["head_pan_joint", "head_tilt_joint"] 00207 traj.points = [] 00208 positions=[self.headx, self.heady] 00209 velocities=[0.0]*len(positions) 00210 accelerations=[] 00211 time_from_start=rospy.Duration(0, 0) 00212 traj.points.append(JointTrajectoryPoint(positions, velocities, 00213 accelerations, time_from_start)) 00214 self.headpub.publish(traj) 00215 00216 00217 def point_head_service_interface(self, req): 00218 side=req.dir 00219 rospy.loginfo("pointing the head at the current place location and draw the place area") 00220 if side=='r' or side=='l' or side=='d' or side =='u' : 00221 self.move_head(side) 00222 00223 00224 else: 00225 self.point_head_at_place_rect() 00226 self.draw_place_area() 00227 00228 return PointHeadResponse(True) 00229 00230 def pickup_object_service_interface(self, req): 00231 object_num=req.objectid 00232 arm=req.arm 00233 00234 if arm=='r': 00235 armnum=0 00236 elif arm=='l': 00237 armnum=1 00238 else: 00239 return PickUpObjectResponse(False) 00240 00241 (result, arm_used)=self.pickandplacemanager.grasp_object_and_check_success(self.pickandplacemanager.detected_objects[object_num], armnum) 00242 if result=="succeeded": 00243 return PickUpObjectResponse(True) 00244 elif result=="attempt failed": 00245 return PickUpObjectResponse(False) 00246 elif result == "no feasible grasp": 00247 rospy.loginfo("no feasible grasp for this object with the %s arm"%self.arm_dict[self.whicharm]) 00248 return PickUpObjectResponse(False) 00249 00250 #Service Changes arm that is being used for tasks. 00251 def select_arm_service_interface(self, req): 00252 arm=req.side 00253 if arm=='r': 00254 self.whicharm=0 00255 elif arm=='l': 00256 self.whicharm =1 00257 else: 00258 return SelectArmResponse(False) 00259 00260 return SelectArmResponse(True) 00261 00262 #switch the pickup and place sides of the table 00263 def switch_sides(self): 00264 self.pickandplacemanager.put_down_side=self.pickandplacemanager.pick_up_side 00265 self.pickandplacemanager.pick_up_side=1-self.pickandplacemanager.put_down_side 00266 rospy.loginfo("switching sides! pick-up side is now %s, put-down is %s"%(self.pickandplacemanager.arm_dict[self.pick_up_side], self.pickandplacemanager.arm_dict[self.put_down_side])) 00267 00268 #update the place rectangle 00269 self.set_table_place_rectangle(self.pickandplacemanager.put_down_side) 00270 00271 #set the place rectangle to one side of the table 00272 def set_table_place_rectangle(self, side): 00273 rect_pose_mat = scipy.identity(4) 00274 rect_pose_mat[0:3, 3] = scipy.matrix(self.pickandplacemanager.table_centers[side]) 00275 rect_pose_stamped = stamp_pose(mat_to_pose(rect_pose_mat), 'base_link') 00276 self.pickandplacemanager.set_place_area(rect_pose_stamped, self.pickandplacemanager.table_side_dims) 00277 00278 def switch_sides_service_interface(self, req): 00279 self.switch_sides() 00280 return SwitchPickupPutdownSidesResponse(True) 00281 00282 00283 00284 def place_object_service_interface(self, req): 00285 command=req.command 00286 arm=req.arm 00287 rightarm=0 00288 leftarm=1 00289 00290 if arm=='r': 00291 armnum=0 00292 else: 00293 armnum=1 00294 00295 if command=='w' or command=='wo': 00296 if not any(self.pickandplacemanager.held_objects): 00297 print "the robot doesn't think it's holding any objects \n Use manual control to do an open loop place anyway" 00298 return PlaceObjectResponse(False) 00299 elif arm=='l' and not self.pickandplacemanager.held_objects[leftarm]: 00300 print "the robot doesn't think it's holding any objects in its left arm \n Use manual control to do an open loop place anyway" 00301 00302 elif arm=='r' and not self.pickandplacemanager.held_objects[rightarm]: 00303 print "the robot doesn't think it's holding any objects in its rigt arm \n Use manual control to do an open loop place anyway" 00304 return PlaceObjectResponse(False) 00305 if command =='wo': 00306 if not self.pickandplacemanager.held_objects[armnum]: 00307 print "no recorded pose for an object in that gripper!" 00308 return PlaceObjectResponse(False) 00309 self.pickandplacemanager.place_object(armnum, self.held_objects[armnum].pose) 00310 else: 00311 self.pickandplacemanager.put_down_object(armnum, use_place_override = 1) 00312 00313 return PlaceObjectResponse(True) 00314 else: 00315 return PlaceObjectResponse(False) 00316 def move_arm_to_side_service_interface(self, req): 00317 arm=req.arm 00318 if arm=='r': 00319 armnum=0 00320 else: 00321 armnum=1 00322 00323 if arm=='l' or arm=='r': 00324 self.pickandplacemanager.move_arm_to_side(armnum) 00325 else: 00326 return MoveArmToSideResponse(False) 00327 return MoveArmToSideResponse(True) 00328 00329 def move_arm_to_front(self, whicharm, try_constrained=0): 00330 if try_constrained: 00331 if whicharm == 1: 00332 start_angles = [0, 1.203, 0, -2.105, 3.13, -1.680, 1.398] 00333 else: 00334 start_angles = [0, 1.203, 0, -2.105, -3.13, -1.680, 1.398] 00335 00336 #default location is arm-to-the-side 00337 if whicharm == 1: 00338 rospy.loginfo("Planning for the left arm") 00339 location = [0.05, 0.576, 0.794] 00340 else: 00341 rospy.loginfo("Planning for the right arm") 00342 location = [0.05, -0.576, 0.794] 00343 00344 current_pose = self.pickandplacemanager.cms[whicharm].get_current_wrist_pose_stamped('base_link') 00345 orientation_constraint = self.pickandplacemanager.get_keep_object_level_constraint(whicharm,current_pose) 00346 constraint = Constraints() 00347 constraint.orientation_constraints.append(orientation_constraint) 00348 result = self.pickandplacemanager.try_to_move_constrained(whicharm,constraint,3,start_angles,location) 00349 if result == 1: 00350 return 1 00351 00352 #either constrained move didn't work or we didn't request a constrained move 00353 result = self.pickandplacemanager.try_hard_to_move_joint(whicharm, [self.arm_above_and_to_front_angles[whicharm],self.arm_to_front_angles[whicharm]], use_open_loop = 1) 00354 return result 00355 00356 00357 def move_arm_to_front_service_interface(self, req): 00358 arm=req.arm 00359 if arm=='r': 00360 armnum=0 00361 else: 00362 armnum=1; 00363 if arm=='l' or arm=='r': 00364 self.move_arm_to_front(armnum) 00365 else: 00366 return MoveArmToFrontResponse(False) 00367 return MoveArmToFrontResponse(True) 00368 00369 if __name__ == '__main__': 00370 00371 rospy.init_node('pick_and_place_manager', anonymous=True) 00372 pick_and_place_manager = PickAndPlaceService() 00373 rospy.spin() 00374 # pick_and_place_manager.keyboard_interface()