srs_states: srs_states: srs_grasp

00001 #!/usr/bin/python
00002 
00003 import roslib
00004 roslib.load_manifest('srs_grasping')
00005 import rospy
00006 import grasping_functions
00007 from shared_state_information import *
00008 import copy
00009 from srs_knowledge.srv import *
00010 from srs_symbolic_grounding.srv import *
00011 from srs_symbolic_grounding.msg import *
00012 from simple_script_server import *
00013 sss = simple_script_server()
00014 
00015 from srs_grasping.srv import *
00016 from srs_knowledge.srv import *
00017 from pr2_controllers_msgs.msg import JointTrajectoryControllerState
00018 from srs_msgs.msg import SRSSpatialInfo 
00019 
00020 class select_srs_grasp(smach.State):
00021 
00022     def __init__(self):
00023         smach.State.__init__(
00024             self,
00025             outcomes=['succeeded', 'not_possible', 'failed', 'preempted'],
00026             input_keys=['object', 'target_object_id'],
00027             output_keys=['grasp_configuration', 'poses'])
00028   
00029 
00030     def execute(self, userdata):
00031         
00032         global listener
00033         print "Object pose before transformation:", userdata.object.pose
00034         try:
00035             # transform object_pose into base_link
00036             object_pose_in = userdata.object.pose
00037             object_pose_in.header.stamp = listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id)
00038             object_pose_bl = listener.transformPose("/base_link", object_pose_in)
00039 
00040         except rospy.ROSException, e:
00041             print ("Transformation not possible: %s", e)
00042             return 'failed'
00043         
00044         get_grasps_from_position = rospy.ServiceProxy('get_feasible_grasps', GetFeasibleGrasps)
00045         req = GetFeasibleGraspsRequest(userdata.target_object_id, object_pose_bl.pose, [0.0, 0.0]) #[X,Z]
00046         grasp_configuration = copy.deepcopy((get_grasps_from_position(req)).grasp_configuration)
00047 
00048         if len(grasp_configuration) < 1: # no valid configurations found
00049             print "grasp not possible"
00050             return 'not_possible'
00051         else:       
00052             poses=list()
00053             for index in range(len(grasp_configuration)):
00054                 pose = PoseStamped()
00055                 pose.header.frame_id = "/base_link"
00056                 pose.pose = grasp_configuration[index].pre_grasp.pose
00057                 poses.append(pose)
00058             userdata.poses = poses
00059             userdata.grasp_configuration = grasp_configuration
00060         return 'succeeded'
00061      
00062 
00063 class srs_grasp(smach.State):
00064     def __init__(self):
00065         smach.State.__init__(self, outcomes=['succeeded','not_completed','failed', 'preempted'], 
00066                              input_keys=['grasp_configuration','grasp_configuration_id'], 
00067                              output_keys=['grasp_categorisation'])
00068 
00069         self.current_arm_state = [];
00070         self.arm_state = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state)
00071 
00072     def get_joint_state(self, msg):
00073         self.current_arm_state = list(msg.desired.positions)
00074 
00075     def get_fake_id(self, grasp_configuration):
00076         #TOP grasps priority.
00077         for i in range(0, len(grasp_configuration)):
00078                 if grasp_configuration[i].category == "TOP":
00079                         return i;
00080         return (len(grasp_configuration)-1);
00081 
00082     def execute(self, userdata): 
00083 
00084         grasp_configuration_id = userdata.grasp_configuration_id
00085         #grasp_configuration_id = self.get_fake_id(userdata.grasp_configuration);
00086         
00087         category = userdata.grasp_configuration[grasp_configuration_id].category
00088         if category == "TOP":
00089             userdata.grasp_categorisation = 'top'
00090             sdh_handle=sss.move("sdh", "spheropen")
00091         elif category == "SIDE" or category == "-SIDE":
00092             userdata.grasp_categorisation = 'side'
00093             sdh_handle=sss.move("sdh", "cylopen")
00094         else:
00095             userdata.grasp_categorisation = 'front'
00096             sdh_handle=sss.move("sdh", "cylopen")
00097         sdh_handle.wait()
00098         
00099 
00100         #Get the current arm joint states.
00101         rospy.sleep(3)
00102         while self.arm_state.get_num_connections() == 0:
00103                 time.sleep(0.3)
00104                 continue
00105 
00106         #pregrasps
00107         pre_grasp_stamped = copy.deepcopy(userdata.grasp_configuration[grasp_configuration_id].pre_grasp);
00108 
00109         #grasp
00110         grasp_stamped = copy.deepcopy(userdata.grasp_configuration[grasp_configuration_id].grasp);
00111 
00112         #postgrasp
00113         post_grasp_stamped = copy.deepcopy(grasp_stamped);
00114         post_grasp_stamped.pose.position.x += 0.15;
00115         post_grasp_stamped.pose.position.z += 0.2;
00116 
00117         grasp_trajectory = [];
00118         postgrasp_trajectory = [];
00119 
00120         class BadGrasp(Exception): pass
00121         try:
00122                 try:
00123                         ipa_arm_navigation = rospy.get_param("srs/ipa_arm_navigation")
00124                 except Exception, e:
00125                         rospy.INFO('can not read parameter of srs/ipa_arm_navigation, use the default value planned arm navigation disabled')
00126 
00127                 if ipa_arm_navigation.lower() == 'true':
00128                         #mode = "planned"
00129                         grasp_trajectory.append(self.current_arm_state)
00130                 else:
00131                         #pre-grasp
00132                         (pgc1, error_code) = grasping_functions.graspingutils.callIKSolver(self.current_arm_state, pre_grasp_stamped)
00133                         if(error_code.val != error_code.SUCCESS):
00134                                 sss.say(["I can not move the arm to the pregrasp position!"])
00135                                 raise BadGrasp();
00136                         grasp_trajectory.append(pgc1)
00137 
00138                         #second pre-grasp
00139                         aux_pre = pre_grasp_stamped.pose.position.x;
00140                         aux = 0.0;
00141                         for i in range(0,5):
00142                                 aux += 0.02;
00143                                 pre_grasp_stamped.pose.position.x = aux_pre + aux;
00144                                 (pgc2, error_code) = grasping_functions.graspingutils.callIKSolver(pgc1, pre_grasp_stamped)
00145                                 if(error_code.val == error_code.SUCCESS):
00146                                         grasp_trajectory.append(pgc2);
00147                                         break;  
00148 
00149                 #grasp
00150                 (gc, error_code) = grasping_functions.graspingutils.callIKSolver(grasp_trajectory[len(grasp_trajectory)-1], grasp_stamped)
00151                 if(error_code.val != error_code.SUCCESS):
00152                         sss.say(["I can not move the arm to the grasp position!"])
00153                         raise BadGrasp();               
00154                 grasp_trajectory.append(gc);
00155 
00156                 #Move arm to pregrasp->grasp position.
00157                 arm_handle = sss.move("arm", grasp_trajectory, False)
00158                 rospy.sleep(4)
00159                 arm_handle.wait(6)
00160 
00161 
00162                 # wait while movement
00163                 r = rospy.Rate(10)
00164                 preempted = False
00165                 arm_state = -1
00166                 while True:
00167                         preempted = self.preempt_requested()
00168                         arm_state = arm_handle.get_state()
00169                         if preempted or ( arm_state == 3) or (arm_state == 4):
00170                                 break # stop waiting  
00171                         r.sleep()
00172                 
00173 
00174                 #Close SDH based on the grasp configuration to grasp.
00175                 sdh_handle = sss.move("sdh", [list(userdata.grasp_configuration[grasp_configuration_id].sdh_joint_values)], False)
00176                 sss.say(["I am grasping the object now!"])
00177                 rospy.sleep(3);
00178                 sdh_handle.wait(4)
00179                 
00180                 r = rospy.Rate(10)
00181                 preempted = False
00182                 sdh_state = -1
00183                 while True:
00184                         preempted = self.preempt_requested()
00185                         sdh_state = sdh_handle.get_state()
00186                         if preempted or ( sdh_state == 3) or (sdh_state == 4):
00187                                 break # stop waiting  
00188                         r.sleep()
00189 
00190                 #Confirm the grasp based on force feedback
00191                 if not grasping_functions.graspingutils.sdh_tactil_sensor_result():
00192                         #Regrasp (close MORE the fingers)
00193                         regrasp = list(userdata.grasp_configuration[grasp_configuration_id].sdh_joint_values)
00194                         print "Current config, trying regrasp:\n", regrasp
00195                         regrasp[1] += 0.07
00196                         regrasp[3] += 0.07
00197                         regrasp[5] += 0.07
00198                         print "to:\n", regrasp
00199 
00200                         sdh_handle = sss.move("sdh", [regrasp])
00201                         rospy.sleep(2)
00202                         sdh_handle.wait(3)
00203 
00204                         if not grasping_functions.graspingutils.sdh_tactil_sensor_result():
00205                                 sss.say(["I can not fix the object correctly!"])
00206                                 raise BadGrasp();
00207 
00208                 #post-grasp
00209                 aux_x = post_grasp_stamped.pose.position.x;
00210                 aux = 0.0;
00211 
00212                 for i in range(0,5):
00213                         post_grasp_stamped.pose.position.x = aux_x + aux;
00214                         (post_grasp_conf, error_code) = grasping_functions.graspingutils.callIKSolver(self.current_arm_state, post_grasp_stamped)
00215                         aux += 0.01;
00216                         if(error_code.val == error_code.SUCCESS):
00217                                 postgrasp_trajectory.append(post_grasp_conf);
00218                                 break;
00219 
00220                 if len(postgrasp_trajectory) == 0:
00221                         sss.say(["I can not move the object to the postgrasp position!"])
00222                 else:
00223                         #second post-grasp
00224                         aux_x = post_grasp_stamped.pose.position.x;
00225                         aux = 0.0;
00226                         for i in range(0,5):
00227                                 post_grasp_stamped.pose.position.x = aux_x + aux;
00228                                 (post_grasp_conf2, error_code) = grasping_functions.graspingutils.callIKSolver(post_grasp_conf, post_grasp_stamped)
00229                                 aux += 0.02;
00230                                 if(error_code.val == error_code.SUCCESS):
00231                                         postgrasp_trajectory.append(post_grasp_conf2);
00232                                         break;
00233 
00234                 arm_handle = sss.move("arm",postgrasp_trajectory, False)
00235                 sss.say(["I have grasped the object with success!"])
00236                 rospy.sleep(2)
00237                 arm_handle.wait(3)
00238 
00239                 return 'succeeded'
00240 
00241 
00242         except BadGrasp:
00243                 sss.say(["I can not catch the object!"], False)
00244                 return 'not_completed';
00245 
00246 # estimate the best grasp position
00247 class grasp_base_pose_estimation(smach.State):
00248     
00249     def __init__(self):
00250         smach.State.__init__(
00251             self,
00252             outcomes=['retry', 'not_retry', 'failed', 'preempted'],
00253             input_keys=['object','target_workspace_name'],
00254             output_keys=['base_pose'])
00255         
00256         self.counter = 0 
00257 
00258 
00259     def execute(self, userdata):
00260         
00261         self.counter = self.counter + 1
00262         if self.counter > 1:
00263             self.counter = 0
00264             #only need to retry once
00265             return 'not_retry'
00266         
00267         global listener
00268 
00269         try:
00270             #transform object_pose into base_link
00271             object_pose_in = userdata.object.pose
00272             object_pose_in.header.stamp = listener.getLatestCommonTime("/map",object_pose_in.header.frame_id)
00273             object_pose_map = listener.transformPose("/map", object_pose_in)
00274         except rospy.ROSException, e:
00275             print "Transformation not possible: %s"%e
00276             return 'failed'
00277 
00278         all_workspaces_on_map = ''
00279         index_of_the_target_workspace = ''
00280 
00281         try:
00282             rospy.wait_for_service('get_workspace_on_map')
00283             getWorkspace = rospy.ServiceProxy('get_workspace_on_map', GetWorkspaceOnMap)
00284             all_workspaces_on_map = getWorkspace(os.environ['ROBOT_ENV'], True)
00285             
00286             #get the index of the target workspace e.g. table0    
00287             index_of_the_target_workspace = all_workspaces_on_map.objects.index(userdata.target_workspace_name)
00288             #get the pose of the workspace from knowledge service
00289             target_object_pose = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].pose
00290             #get the houseHoldID of the workspace 
00291             object_id = all_workspaces_on_map.houseHoldId[index_of_the_target_workspace]
00292             
00293             #rospy.loginfo ("target name: %s", userdata.target_workspace_name)      
00294             #rospy.loginfo ("target pose: %s", target_object_pose)
00295             #rospy.loginfo ("target id: %s", object_id)
00296 
00297         except rospy.ServiceException, e:
00298             print "Service call failed: %s"%e
00299             return 'failed'
00300 
00301         parent_obj_geometry = SRSSpatialInfo()
00302         parent_obj_geometry.pose = target_object_pose
00303         parent_obj_geometry.l = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].l
00304         parent_obj_geometry.w = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].w
00305         parent_obj_geometry.h = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].h
00306         
00307         rospy.wait_for_service('symbol_grounding_grasp_base_pose_experimental')
00308         symbol_grounding_grasp_base_pose_experimental = rospy.ServiceProxy('symbol_grounding_grasp_base_pose_experimental', SymbolGroundingGraspBasePoseExperimental)
00309         try:
00310             result = symbol_grounding_grasp_base_pose_experimental(object_pose_map.pose, parent_obj_geometry, all_workspaces_on_map.objectsInfo)
00311             userdata.base_pose = [result.grasp_base_pose.x, result.grasp_base_pose.y, result.grasp_base_pose.theta]
00312             return 'retry'
00313         except rospy.ServiceException, e:
00314             print "Service call failed: %s" %e
00315             return 'failed'
00316
srs_grasp_states.py
