wg_manipulation.py
Go to the documentation of this file.
00001 #
00002 # Copyright (c) 2009, Georgia Tech Research Corporation
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Georgia Tech Research Corporation nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00021 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00022 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00023 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00024 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00025 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 #
00027 #Based on some code by Kaijen, modified heavily by Marc Killpack
00028 
00029 
00030 import roslib
00031 roslib.load_manifest('pr2_playpen')
00032 import rospy
00033 from pr2_playpen.pick_and_place_manager import *
00034 from object_manipulator.convert_functions import *
00035 from pr2_playpen.srv import Playpen
00036 from pr2_playpen.srv import Conveyor
00037 from pr2_playpen.srv import Check
00038 from pr2_playpen.srv import Train
00039 from UI_segment_object.srv import Save
00040 import os
00041 import datetime
00042 import cPickle
00043 
00044 import math
00045 import numpy as np
00046 
00047 class SimplePickAndPlaceExample():
00048 
00049     def __init__(self):
00050 
00051         rospy.loginfo("initializing pick and place manager")
00052         self.papm = PickAndPlaceManager()
00053         rospy.loginfo("finished initializing pick and place manager")
00054         rospy.wait_for_service('playpen')
00055         rospy.wait_for_service('conveyor')
00056         self.playpen = rospy.ServiceProxy('playpen', Playpen)
00057         self.conveyor = rospy.ServiceProxy('conveyor', Conveyor)
00058         self.objects_dist = [.135, .26-.135, .405-.26, .545-.405, 
00059                              .70-.545, .865-.70, .995-.865, 1.24-.995]
00060         self.tries = 0
00061         self.successes = 0
00062     #pick up the nearest object to PointStamped target_point with whicharm 
00063     #(0=right, 1=left)
00064     def pick_up_object_near_point(self, target_point, whicharm):
00065 
00066         rospy.loginfo("moving the arms to the side")
00067         self.papm.move_arm_out_of_way(0)
00068         self.papm.move_arm_out_of_way(1)
00069 
00070 #############once is it positioned, we don't want to move the head at all !!!#############
00071 #        rospy.loginfo("pointing the head at the target point")
00072 #        self.papm.point_head(get_xyz(target_point.point),
00073 #                             target_point.header.frame_id)
00074 #########################################################################################        
00075 
00076 
00077         rospy.loginfo("detecting the table and objects")
00078         self.papm.call_tabletop_detection(take_static_collision_map = 1,
00079                              update_table = 1, clear_attached_objects = 1)
00080 
00081         rospy.loginfo("picking up the nearest object to the target point")
00082         success = self.papm.pick_up_object_near_point(target_point,
00083                                                       whicharm)
00084 
00085         if success:
00086             rospy.loginfo("pick-up was successful!  Moving arm to side")
00087             #self.papm.move_arm_to_side(whicharm)
00088             self.papm.move_arm_out_of_way(whicharm)
00089         else:
00090             rospy.loginfo("pick-up failed.")
00091 
00092         return success
00093 
00094 
00095     #place the object held in whicharm (0=right, 1=left) down in the 
00096     #place rectangle defined by place_rect_dims (x,y) 
00097     #and place_rect_center (PoseStamped)
00098     def place_object(self, whicharm, place_rect_dims, place_rect_center):
00099 
00100         self.papm.set_place_area(place_rect_center, place_rect_dims)
00101 
00102         rospy.loginfo("putting down the object in the %s gripper"\
00103                       %self.papm.arm_dict[whicharm])
00104         success = self.papm.put_down_object(whicharm,
00105                       max_place_tries = 5,
00106                       use_place_override = 1)
00107 
00108         if success:
00109             rospy.loginfo("place returned success")
00110         else:
00111             rospy.loginfo("place returned failure")
00112             self.papm.open_gripper(whicharm)
00113         return success
00114 
00115 
00116 if __name__ == "__main__":
00117     rospy.init_node('simple_pick_and_place_example')
00118 
00119     sppe = SimplePickAndPlaceExample()
00120 
00121     #adjust for your table 
00122     table_height = 0.529
00123     date = datetime.datetime.now()
00124 
00125     f_name = date.strftime("%Y-%m-%d_%H-%M-%S")
00126 
00127     save_dir = os.getcwd()+'/../../data/'+f_name
00128     playpen_dir = '/home/mkillpack/svn/gt-ros-pkg/hrl/pr2_playpen/data/' #should add way to sync
00129     os.mkdir(save_dir)
00130 
00131     print "CHECING FOR DIRECTORY :  ", os.getcwd()+'/../../data/'+f_name
00132     #.5 m in front of robot, centered
00133     target_point_xyz = [.625, 0, table_height]   #this is currently an approximation/hack should use ar tag
00134     target_point = create_point_stamped(target_point_xyz, 'base_link')
00135     arm = 0
00136     rospy.wait_for_service('playpen_train_success')
00137     rospy.wait_for_service('playpen_check_success')
00138     rospy.wait_for_service('playpen_save_pt_cloud')
00139     rospy.wait_for_service('playpen_save_image')
00140 #    rospy.wait_for_service('pr2_save_pt_cloud')
00141 
00142     try:
00143         train_success = rospy.ServiceProxy('playpen_train_success', Train)
00144         check_success = rospy.ServiceProxy('playpen_check_success', Check)
00145 #        save_pr2_cloud = rospy.ServiceProxy('pr2_save_pt_cloud', Save)
00146         save_playpen_cloud = rospy.ServiceProxy('playpen_save_pt_cloud', Save)
00147         save_playpen_image = rospy.ServiceProxy('playpen_save_image', Save)
00148     except rospy.ServiceException, e:
00149         print "Service call failed: %s"%e
00150     num_samples = train_success()
00151 
00152 
00153     for i in xrange(len(sppe.objects_dist)):
00154         file_handle = open(save_dir+'/object'+str(i).zfill(3)+'.pkl', 'wb')
00155         data = {}
00156         sppe.playpen(0)
00157         sppe.conveyor(sppe.objects_dist[i])
00158         # data['object'+str(i).zfill(3)] = {}
00159         # data['object'+str(i).zfill(3)]['success'] = []
00160         # data['object'+str(i).zfill(3)]['frames'] = []
00161 
00162         data['success'] = []
00163         data['frames'] = []
00164 
00165         while sppe.tries<6:
00166             print "arm is ", arm
00167 #            print "target _point is ", target_point.x
00168 #            save_pr2_cloud(save_dir+'/object'+str(i).zfill(3)+'_try'+str(sppe.tries).zfill(3)+'_before_pr2.pcd')
00169             save_playpen_cloud(playpen_dir+'object'+str(i).zfill(3)+'_try'+str(sppe.tries).zfill(3)+'_before_playpen.pcd')
00170             save_playpen_image(playpen_dir+'object'+str(i).zfill(3)+'_try'+str(sppe.tries).zfill(3)+'_before_playpen.png')
00171             success = sppe.pick_up_object_near_point(target_point, arm)
00172 
00173             result = []
00174             rospy.sleep(5)
00175             for j in xrange(5):
00176                 result.append(check_success('').result)
00177                 rospy.sleep(5)
00178             result.sort()
00179             if result[2] == 'table':
00180                 success = True
00181             elif result[2] == 'object':
00182                 success = False
00183             # else:
00184             #     success = False
00185             #     sppe.tries = sppe.tries-1 # this is to compensate for failures in perception hopefully
00186 
00187             print "SUCCESS IS :", success, '    ', result
00188             data['success'].append(success)
00189                 
00190 #            save_pr2_cloud(save_dir+'/object'+str(i).zfill(3)+'try'+str(sppe.tries).zfill(3)+'_after_pr2.pcd')
00191             save_playpen_cloud(playpen_dir+'object'+str(i).zfill(3)+'_try'+str(sppe.tries).zfill(3)+'_after_playpen.pcd')
00192             save_playpen_image(playpen_dir+'object'+str(i).zfill(3)+'_try'+str(sppe.tries).zfill(3)+'_after_playpen.png')
00193 #            save_playpen_cloud(save_dir+'/object'+str(i).zfill(3)+'try'+str(sppe.tries).zfill(3)+'_after_playpen.pcd')
00194 
00195 
00196             if success:
00197                 sppe.successes=sppe.successes + 1
00198                 #square of size 30 cm by 30 cm
00199                 place_rect_dims = [.1, .1]
00200 
00201                 x = np.random.uniform(-0.18, 0.18, 1)[0]
00202                 y = np.random.uniform(-0.18, 0.18, 1)[0]
00203                 center_xyz = [.625+x, y, table_height+.10]
00204 
00205 
00206                 #aligned with axes of frame_id
00207                 center_quat = [0,0,0,1]
00208                 place_rect_center = create_pose_stamped(center_xyz+center_quat,
00209                                                         'base_link')
00210 
00211                 sppe.place_object(arm, place_rect_dims, place_rect_center)
00212 
00213 
00214             arm = arm.__xor__(1)
00215             sppe.tries = sppe.tries+1
00216 
00217         sppe.playpen(1)
00218         sppe.successes = 0
00219         sppe.tries = 0
00220         cPickle.dump(data, file_handle)
00221         file_handle.close()
00222     
00223 #    sppe.playpen(0)


pr2_playpen
Author(s): Marc Killpack / mkillpack3@gatech.edu, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 12:18:32