simple_grasp_states.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib
00004 roslib.load_manifest('srs_states')
00005 import rospy
00006 import smach
00007 import smach_ros
00008 
00009 import copy
00010 
00011 from simple_script_server import *
00012 sss = simple_script_server()
00013 
00014 import tf
00015 from tf.transformations import *
00016 from kinematics_msgs.srv import *
00017 
00018 #this should be in manipulation_msgs
00019 #from cob_mmcontroller.msg import *
00020 #from cob_arm_navigation.msg import *
00021 
00022 from shared_state_information import *
00023 #import grasping_functions
00024 
00025 
00026 
00027 ## Select grasp state
00028 #
00029 # This state select a grasping strategy. A high object will be grasped from the side, a low one from top.
00030 class select_simple_grasp(smach.State):
00031 
00032     def __init__(self):
00033         smach.State.__init__(
00034             self,
00035             outcomes=['succeeded', 'failed', 'preempted'],
00036             input_keys=['object'],
00037             output_keys=['grasp_categorisation'])
00038         
00039         """
00040         Very simple grasp selection
00041         This need to be transfered into symbolic grounding service
00042         """
00043         self.height_switch = 0.5 # Switch to select top or side grasp using the height of the object over the ground in [m].
00044         
00045         #self.listener = tf.TransformListener()
00046         
00047         #default grasp categorisation
00048         self.grasp_categorisation = 'side'
00049 
00050     def execute(self, userdata):
00051         
00052         global listener
00053         
00054         rospy.loginfo('userdata is %s', userdata)
00055         try:
00056             # transform object_pose into base_link
00057             object_pose_in = userdata.object.pose
00058             print object_pose_in
00059             object_pose_in.header.stamp = listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id)
00060             object_pose_bl = listener.transformPose("/base_link", object_pose_in)
00061         except rospy.ROSException, e:
00062             print "Transformation not possible: %s"%e
00063             return 'failed'
00064         
00065         if object_pose_bl.pose.position.z >= self.height_switch: #TODO how to select grasps for objects within a cabinet or shelf?
00066             userdata.grasp_categorisation='side'
00067         else: 
00068             userdata.grasp_categorisation= 'top'
00069         
00070         if self.preempt_requested():
00071             self.service_preempt()
00072             return 'preempted'
00073         
00074         return 'succeeded'
00075 
00076 
00077 ## Grasp general state
00078 #
00079 # This state will grasp an object with a side grasp
00080 class simple_grasp(smach.State):
00081 
00082     def __init__(self, max_retries = 1):
00083         smach.State.__init__(
00084             self,
00085             outcomes=['succeeded', 'retry', 'no_more_retries', 'failed', 'preempted'],
00086             input_keys=['object','grasp_categorisation'])
00087         
00088         self.max_retries = max_retries
00089         self.retries = 0
00090         #self.iks = rospy.ServiceProxy('/arm_kinematics/get_ik', GetPositionIK)
00091         self.iks = rospy.ServiceProxy('/srs_arm_kinematics/get_ik', GetPositionIK)
00092         #self.listener = tf.TransformListener()
00093         self.stiffness = rospy.ServiceProxy('/arm_controller/set_joint_stiffness', SetJointStiffness)
00094 
00095 
00096     def callIKSolver(self, current_pose, goal_pose):
00097         req = GetPositionIKRequest()
00098         req.ik_request.ik_link_name = "sdh_grasp_link"
00099         req.ik_request.ik_seed_state.joint_state.position = current_pose
00100         req.ik_request.pose_stamped = goal_pose
00101         resp = self.iks(req)
00102         result = []
00103         for o in resp.solution.joint_state.position:
00104             result.append(o)
00105         return (result, resp.error_code)
00106 
00107     def execute(self, userdata):
00108         global current_task_info
00109         
00110         if not current_task_info.object_in_hand: #no object in hand
00111 
00112             if self.preempt_requested():
00113                 self.service_preempt()
00114                 return 'preempted'
00115             
00116             global listener
00117             # check if maximum retries reached
00118             print self.retries
00119             print self.max_retries
00120             if self.retries > self.max_retries:
00121                 self.retries = 0
00122                 return 'no_more_retries'
00123             
00124             # transform object_pose into base_link
00125             object_pose_in = userdata.object.pose
00126             object_pose_in.header.stamp = listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id)
00127             object_pose_bl = listener.transformPose("/base_link", object_pose_in)
00128             
00129             
00130             if userdata.grasp_categorisation == 'side':
00131                 # make arm soft TODO: handle stiffness for schunk arm
00132                 try:
00133                     self.stiffness([300,300,300,100,100,100,100])
00134                 except rospy.ServiceException, e:
00135                     print "Service call failed: %s"%e
00136                     self.retries = 0
00137                     return 'failed'
00138             
00139                 [new_x, new_y, new_z, new_w] = tf.transformations.quaternion_from_euler(-1.552, -0.042, 2.481) # rpy 
00140                 object_pose_bl.pose.orientation.x = new_x
00141                 object_pose_bl.pose.orientation.y = new_y
00142                 object_pose_bl.pose.orientation.z = new_z
00143                 object_pose_bl.pose.orientation.w = new_w
00144         
00145                 # FIXME: this is calibration between camera and hand and should be removed from scripting level
00146                 object_pose_bl.pose.position.x = object_pose_bl.pose.position.x #- 0.06 #- 0.08
00147                 object_pose_bl.pose.position.y = object_pose_bl.pose.position.y #- 0.05
00148                 object_pose_bl.pose.position.z = object_pose_bl.pose.position.z  #- 0.1
00149                 
00150                 # calculate pre and post grasp positions
00151                 pre_grasp_bl = PoseStamped()
00152                 post_grasp_bl = PoseStamped()
00153                 pre_grasp_bl = copy.deepcopy(object_pose_bl)
00154                 post_grasp_bl = copy.deepcopy(object_pose_bl)
00155         
00156                 #pre_grasp_bl.pose.position.x = pre_grasp_bl.pose.position.x + 0.10 # x offset for pre grasp position
00157                 #pre_grasp_bl.pose.position.y = pre_grasp_bl.pose.position.y + 0.10 # y offset for pre grasp position
00158                 #post_grasp_bl.pose.position.x = post_grasp_bl.pose.position.x + 0.05 # x offset for post grasp position
00159                 #post_grasp_bl.pose.position.z = post_grasp_bl.pose.position.z + 0.15 # z offset for post grasp position
00160         
00161                 pre_grasp_bl.pose.position.x = pre_grasp_bl.pose.position.x + 0.10 # x offset for pre grasp position
00162                 pre_grasp_bl.pose.position.y = pre_grasp_bl.pose.position.y + 0.10 # y offset for pre grasp position
00163                 pre_grasp_bl.pose.position.z = pre_grasp_bl.pose.position.z + 0.2 # z offset for pre grasp position
00164                 post_grasp_bl.pose.position.x = post_grasp_bl.pose.position.x + 0.05 # x offset for post grasp position
00165                 post_grasp_bl.pose.position.z = post_grasp_bl.pose.position.z + 0.17 # z offset for post grasp position
00166                 
00167             elif userdata.grasp_categorisation == 'top':
00168                 try:
00169                     self.stiffness([100,100,100,100,100,100,100])
00170                 except rospy.ServiceException, e:
00171                     print "Service call failed: %s"%e
00172                     self.retries = 0
00173                     return 'failed'
00174             
00175                 # use a predefined (fixed) orientation for object_pose_bl
00176                 [new_x, new_y, new_z, new_w] = tf.transformations.quaternion_from_euler(3.121, 0.077, -2.662) # rpy 
00177                 object_pose_bl.pose.orientation.x = new_x
00178                 object_pose_bl.pose.orientation.y = new_y
00179                 object_pose_bl.pose.orientation.z = new_z
00180                 object_pose_bl.pose.orientation.w = new_w
00181         
00182                 # FIXME: this is calibration between camera and hand and should be removed from scripting level
00183                 object_pose_bl.pose.position.x = object_pose_bl.pose.position.x #-0.04 #- 0.08
00184                 object_pose_bl.pose.position.y = object_pose_bl.pose.position.y# + 0.02
00185                 object_pose_bl.pose.position.z = object_pose_bl.pose.position.z #+ 0.07
00186         
00187                 # calculate pre and post grasp positions
00188                 pre_grasp_bl = PoseStamped()
00189                 post_grasp_bl = PoseStamped()
00190                 pre_grasp_bl = copy.deepcopy(object_pose_bl)
00191                 post_grasp_bl = copy.deepcopy(object_pose_bl)
00192             
00193                 pre_grasp_bl.pose.position.z = pre_grasp_bl.pose.position.z + 0.18 # z offset for pre grasp position
00194                 post_grasp_bl.pose.position.x = post_grasp_bl.pose.position.x + 0.05 # x offset for post grasp position
00195                 post_grasp_bl.pose.position.z = post_grasp_bl.pose.position.z + 0.15 # z offset for post grasp position
00196             else:
00197                 return 'failed'
00198                 #unknown categorisation
00199                
00200                 
00201             
00202             # calculate ik solutions for pre grasp configuration
00203             if userdata.grasp_categorisation == 'top':
00204                 arm_pre_grasp = rospy.get_param("/script_server/arm/pregrasp_top")
00205             else:
00206                 # userdata.grasp_categorisation == 'side':
00207                 arm_pre_grasp = rospy.get_param("/script_server/arm/pregrasp")
00208             
00209             (pre_grasp_conf, error_code) = self.callIKSolver(arm_pre_grasp[0], pre_grasp_bl)
00210                         
00211             if(error_code.val != error_code.SUCCESS):
00212                 rospy.logerr("Ik pre_grasp Failed")
00213                 self.retries += 1
00214                 return 'retry'
00215             
00216             # calculate ik solutions for grasp configuration
00217             (grasp_conf, error_code) = self.callIKSolver(pre_grasp_conf, object_pose_bl)
00218             if(error_code.val != error_code.SUCCESS):
00219                 rospy.logerr("Ik grasp Failed")
00220                 self.retries += 1
00221                 return 'retry'
00222             
00223             # calculate ik solutions for pre grasp configuration
00224             (post_grasp_conf, error_code) = self.callIKSolver(grasp_conf, post_grasp_bl)
00225             if(error_code.val != error_code.SUCCESS):
00226                 rospy.logerr("Ik post_grasp Failed")
00227                 self.retries += 1
00228                 return 'retry'    
00229         
00230             # execute grasp
00231             if self.preempt_requested():
00232                 self.service_preempt()
00233                 return 'preempted'
00234             
00235             sss.say([current_task_info.speaking_language['Grasp'] + userdata.object.label ],False)
00236             sss.move("torso","home")
00237             handle_arm = sss.move("arm", [pre_grasp_conf , grasp_conf],False)
00238             
00239             if userdata.grasp_categorisation == 'side':
00240                 sss.move("sdh", "cylopen")
00241             elif userdata.grasp_categorisation == 'top':
00242                 sss.move("sdh", "spheropen")
00243             else:
00244                 return 'failed'
00245             
00246             if self.preempt_requested():
00247                 self.service_preempt()
00248                 handle_arm.client.cancel_goal()
00249                 return 'preempted'
00250             else:
00251                 handle_arm.wait()
00252                 
00253                 
00254             if self.preempt_requested():
00255                 self.service_preempt()
00256                 return 'preempted'
00257             else:
00258                 if userdata.grasp_categorisation == 'side':
00259                     sss.move("sdh", "cylclosed")
00260                 elif userdata.grasp_categorisation == 'top':
00261                     sss.move("sdh", "spherclosed")
00262                 else:
00263                     return 'failed'
00264                     #unknown categorisation
00265                     
00266                 #object is already in hand    
00267                 current_task_info.object_in_hand = True
00268                 #object graspped, any previous detection is not valid anymore
00269                 current_task_info.set_object_identification_state(False) 
00270                 #New object grasp, post grasp adjustment might be required
00271                 current_task_info.set_post_grasp_adjustment_state(False)  
00272             
00273         sss.move("arm", [post_grasp_conf, "hold"])
00274         self.retries = 0     
00275         return 'succeeded'
00276 


srs_states
Author(s): Georg Arbeiter
autogenerated on Mon Oct 6 2014 08:34:06