def __init__(self):
smach.State.__init__(self,
outcomes=['success','fail','no_ik_solution','all_ik_fail'],
input_keys=['grasp_pose','pomdp_grasp_config'],
output_keys=['pomdp_grasp_config','grasp_goal'])
self.tf_manager = TransformManager()
class CalculateGrasp(smach.State):
"""
TODO: This state should be converted to use different smaller states
"""
def __init__(self):
smach.State.__init__(self,
outcomes=['success','fail','no_ik_solution','all_ik_fail'],
input_keys=['grasp_pose','pomdp_grasp_config'],
output_keys=['pomdp_grasp_config','grasp_goal'])
self.tf_manager = TransformManager()
def execute(self, userdata):
# shorcuts
pomdp_config = userdata.pomdp_grasp_config
grasp_pose_st = userdata.grasp_pose
rospy.set_param('/estirabot/skills/grasp/fingers_configuration', pomdp_config.fingers_grasp_configs)
# grasp_pose_st comes referecence to camera frame_id
# we need to transform the message to wam base frame /wam_link0
grasp_pose_st_wam = self.tf_manager.transform_pose('/wam_link0', grasp_pose_st)
# Orientation is fixed so ignoring previous
grasp_pose_st_wam.pose.orientation.x = 0
grasp_pose_st_wam.pose.orientation.y = 0
grasp_pose_st_wam.pose.orientation.z = 0
grasp_pose_st_wam.pose.orientation.w = 1.0
pprint("Grasp original")
pub = rospy.Publisher('/debug/grasp/original', PoseStamped, None, False, True)
pub.publish(grasp_pose_st_wam)
# real grasp is build applying grasp modifier to grasp_pose
current_modifier = pomdp_config.approach_config.grasp_modifier_used
pprint("Using grasp modifier number: " + str(current_modifier))
real_grasp_st = PoseStamped()
real_grasp_st.header = grasp_pose_st_wam.header
real_grasp_st.pose = homogeneous_product_pose_transform(grasp_pose_st_wam.pose,
pomdp_config.approach_config.grasp_modifiers[current_modifier])
if (real_grasp_st.pose.position.z < -0.282):
real_grasp_st.pose.position.z = -0.282
pprint("Grasp modifier applied")
pub = rospy.Publisher('/debug/grasp/real', PoseStamped, None, False, True)
pub.publish(real_grasp_st)
# pre-grasp posture is relative to real_grasp pose
pre_grasp_pose_st = PoseStamped()
pre_grasp_pose_st.header = grasp_pose_st_wam.header
pre_grasp_pose_st.pose = homogeneous_product_pose_transform(real_grasp_st.pose,
pomdp_config.approach_config.pre_grasp_modifier)
pprint("PRE-Grasp modifier applied")
pub = rospy.Publisher('/debug/grasp/pre_grasp', PoseStamped, None, False, True)
pub.publish(pre_grasp_pose_st)
# GRASP is Kinematic compliant so save and log the final configuration
pub_config = rospy.Publisher('/log/pomdp_config', PomdpGraspConfig, None, False, True)
pub_config.publish(pomdp_config)
# For both grasp and pre-grasp we need the inverse kinematics and get joinst positions
try:
joints_grasp = get_inverse_kinematics_from_pose(real_grasp_st)
joints_pre_grasp = get_inverse_kinematics_from_pose(pre_grasp_pose_st)
#except NoKinematicSolutionException, e:
except rospy.ServiceException, e:
# Let's go to next grasp_modifier (if any)
if (current_modifier < len(pomdp_config.approach_config.grasp_modifiers)-1):
# Mark next configuration to use
pomdp_config.approach_config.grasp_modifier_used = pomdp_config.approach_config.grasp_modifier_used + 1
pomdp_config.approach_config.pre_grasp_modifier
return 'no_ik_solution'
else:
# all modifiers were tested and are not valid. Return error
pub = rospy.Publisher('/log/number_of_clothes', Int8, None, False, True)
pub.publish(0)
rospy.logerr("Inverse Kinematics has no solution")
return 'all_ik_fail'
# rospy.logerr("WamIK service failed")
# return 'fail'
userdata.grasp_goal = build_simple_pickup_goal(joints_grasp,
joints_pre_grasp,
pomdp_config.fingers_grasp_configs)
return 'success'
