sm_generate_grasp.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('estirabot_apps_base')
00004 import rospy
00005 import smach
00006 import smach_ros
00007 
00008 from pprint import pprint
00009 from std_msgs.msg import Int8
00010 from geometry_msgs.msg                    import Transform, PoseStamped, Pose
00011 from estirabot_msgs.msg                   import PomdpGraspConfig
00012 from iri_common_smach.transform_manager   import TransformManager
00013 from iri_common_smach.homogeneous_product import homogeneous_product_pose_transform
00014 from estirabot_apps_base.pickup           import build_simple_pickup_goal
00015 
00016 # DEPRECATED: do not use this code outside this app
00017 # Should be migrated to iri_wam_smach st_get_joints_from_pose
00018 from iri_wam_common_msgs.srv import wamInverseKinematics
00019 def get_inverse_kinematics_from_pose(pose):
00020     handler = rospy.ServiceProxy('/estirabot/iri_wam_tcp_ik/get_wam_ik', wamInverseKinematics)
00021     response = handler(pose)
00022 
00023     if (response):
00024         return response.joints
00025 
00026     # Solution could not be found
00027     return NoKinematicSolutionException()
00028 
00029 class CalculateGrasp(smach.State):
00030     """
00031     TODO: This state should be converted to use different smaller states
00032     """
00033     def __init__(self):
00034         smach.State.__init__(self, 
00035                              outcomes=['success','fail','no_ik_solution','all_ik_fail'],
00036                            input_keys=['grasp_pose','pomdp_grasp_config'],
00037                           output_keys=['pomdp_grasp_config','grasp_goal'])
00038         self.tf_manager = TransformManager()
00039 
00040     def execute(self, userdata):
00041         # shorcuts
00042         pomdp_config  = userdata.pomdp_grasp_config
00043         grasp_pose_st = userdata.grasp_pose
00044 
00045         rospy.set_param('/estirabot/skills/grasp/fingers_configuration', pomdp_config.fingers_grasp_configs)
00046 
00047         # grasp_pose_st comes referecence to camera frame_id
00048         # we need to transform the message to wam base frame /wam_link0
00049         grasp_pose_st_wam = self.tf_manager.transform_pose('/wam_link0', grasp_pose_st)
00050 
00051         # Orientation is fixed so ignoring previous
00052         grasp_pose_st_wam.pose.orientation.x = 0
00053         grasp_pose_st_wam.pose.orientation.y = 0
00054         grasp_pose_st_wam.pose.orientation.z = 0
00055         grasp_pose_st_wam.pose.orientation.w = 1.0
00056 
00057         pprint("Grasp original")
00058         pub = rospy.Publisher('/debug/grasp/original', PoseStamped, None, False, True)
00059         pub.publish(grasp_pose_st_wam)
00060 
00061         # real grasp is build applying grasp modifier to grasp_pose
00062         current_modifier     = pomdp_config.approach_config.grasp_modifier_used
00063         pprint("Using grasp modifier number: " + str(current_modifier))
00064         real_grasp_st        = PoseStamped()
00065         real_grasp_st.header = grasp_pose_st_wam.header
00066         real_grasp_st.pose   = homogeneous_product_pose_transform(grasp_pose_st_wam.pose,
00067                                                           pomdp_config.approach_config.grasp_modifiers[current_modifier])
00068         if (real_grasp_st.pose.position.z < -0.282):
00069             real_grasp_st.pose.position.z = -0.282
00070 
00071         pprint("Grasp modifier applied")
00072         pub = rospy.Publisher('/debug/grasp/real', PoseStamped, None, False, True)
00073         pub.publish(real_grasp_st)
00074 
00075         # pre-grasp posture is relative to real_grasp pose
00076         pre_grasp_pose_st        = PoseStamped()
00077         pre_grasp_pose_st.header = grasp_pose_st_wam.header
00078 
00079         pre_grasp_pose_st.pose = homogeneous_product_pose_transform(real_grasp_st.pose,
00080                                                           pomdp_config.approach_config.pre_grasp_modifier)
00081         pprint("PRE-Grasp modifier applied")
00082         pub = rospy.Publisher('/debug/grasp/pre_grasp', PoseStamped, None, False, True)
00083         pub.publish(pre_grasp_pose_st)
00084 
00085         # GRASP is Kinematic compliant so save and log the final configuration
00086         pub_config = rospy.Publisher('/log/pomdp_config', PomdpGraspConfig, None, False, True)
00087         pub_config.publish(pomdp_config)
00088 
00089         # For both grasp and pre-grasp we need the inverse kinematics and get joinst positions
00090         try:
00091             joints_grasp     = get_inverse_kinematics_from_pose(real_grasp_st)
00092             joints_pre_grasp = get_inverse_kinematics_from_pose(pre_grasp_pose_st)
00093 
00094         #except NoKinematicSolutionException, e:
00095         except rospy.ServiceException, e:
00096             # Let's go to next grasp_modifier (if any)
00097             if (current_modifier < len(pomdp_config.approach_config.grasp_modifiers)-1):
00098                 # Mark next configuration to use 
00099                 pomdp_config.approach_config.grasp_modifier_used = pomdp_config.approach_config.grasp_modifier_used + 1
00100                 pomdp_config.approach_config.pre_grasp_modifier
00101                 return 'no_ik_solution'
00102             else:
00103                 # all modifiers were tested and are not valid. Return error
00104                 pub = rospy.Publisher('/log/number_of_clothes', Int8, None, False, True)
00105                 pub.publish(0)
00106                 rospy.logerr("Inverse Kinematics has no solution")
00107                 return 'all_ik_fail'
00108 
00109         #     rospy.logerr("WamIK service failed")
00110         #           return 'fail'
00111 
00112         userdata.grasp_goal =  build_simple_pickup_goal(joints_grasp,
00113                                                         joints_pre_grasp,
00114                                                         pomdp_config.fingers_grasp_configs)
00115         return 'success'
00116 
00117 class SM_ESTIRABOT_GenerateGraspMsg:
00118     def __init__(self):
00119         self.sm = smach.StateMachine(
00120                   outcomes=['success','fail','no_ik_solution','all_ik_fail'],
00121                 input_keys=['grasp_pose','pomdp_grasp_config'],
00122                output_keys=['pomdp_grasp_config','grasp_goal'])
00123 
00124     def build_sm(self):
00125         with self.sm:
00126             smach.StateMachine.add('CALCULATE_GRASP', CalculateGrasp(),
00127                 transitions = {'success'        : 'success',
00128                                'no_ik_solution' : 'no_ik_solution',
00129                                'fail'           : 'fail',
00130                                'all_ik_fail'    : 'all_ik_fail'},
00131                 remapping   = {'grasp_pose'     : 'grasp_pose',
00132                                'grasp_goal'     : 'grasp_goal'})
00133 
00134         return self.sm;


estirabot_apps_base
Author(s): Jose Luis Rivero
autogenerated on Fri Dec 6 2013 21:07:07