pr2_pick_and_place_service: pick_and_place

Go to the documentation of this file.
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()