Go to the documentation of this file.00001 import roslib; roslib.load_manifest('iri_wam_smach')
00002 import rospy
00003 import smach
00004 import smach_ros
00005 import numpy
00006
00007 from st_wam_move_in_cartesian_pose import MoveWamInCartesianPose
00008 from iri_common_smach.st_get_pose_from_frames import GetPoseStampedFromFrames
00009 from iri_common_smach.homogeneous_product import *
00010 from pprint import pprint
00011 from tf.transformations import *
00012
00013 def pose_st_from_matrix(matrix, frame_id):
00014 """ Returns geometry_msgs.msg.Pose() from transformation.matrix
00015
00016 """
00017 quaternion = quaternion_from_matrix(matrix)
00018 position = translation_from_matrix(matrix)
00019
00020 pose_st = PoseStamped()
00021 pose_st.header.frame_id = frame_id
00022 pose_st.header.stamp = rospy.Time()
00023 pose_st.pose.position.x = position[0]
00024 pose_st.pose.position.y = position[1]
00025 pose_st.pose.position.z = position[2]
00026 pose_st.pose.orientation.x = quaternion[0]
00027 pose_st.pose.orientation.y = quaternion[1]
00028 pose_st.pose.orientation.z = quaternion[2]
00029 pose_st.pose.orientation.w = quaternion[3]
00030
00031 return pose_st
00032
00033
00034 class WAMGetTcpPose(smach.State):
00035 """
00036
00037 """
00038 def __init__(self):
00039 smach.State.__init__(self, outcomes = ['success'],
00040 input_keys = ['pose_st', 'tool_to_tcp_pose'],
00041 output_keys = ['tcp_pose'])
00042
00043 def execute(self, userdata):
00044
00045 T1 = translation_matrix((userdata.tool_to_tcp_pose.pose.position.x,
00046 userdata.tool_to_tcp_pose.pose.position.y,
00047 userdata.tool_to_tcp_pose.pose.position.z))
00048 R1 = quaternion_matrix((userdata.tool_to_tcp_pose.pose.orientation.x,
00049 userdata.tool_to_tcp_pose.pose.orientation.y,
00050 userdata.tool_to_tcp_pose.pose.orientation.z,
00051 userdata.tool_to_tcp_pose.pose.orientation.w))
00052 H1 = concatenate_matrices(T1,R1)
00053
00054 T2 = translation_matrix((userdata.pose_st.pose.position.x,
00055 userdata.pose_st.pose.position.y,
00056 userdata.pose_st.pose.position.z))
00057 R2 = quaternion_matrix((userdata.pose_st.pose.orientation.x,
00058 userdata.pose_st.pose.orientation.y,
00059 userdata.pose_st.pose.orientation.z,
00060 userdata.pose_st.pose.orientation.w))
00061 H2 = concatenate_matrices(T2,R2)
00062
00063 H3 = concatenate_matrices(H2,H1)
00064
00065 pose = pose_st_from_matrix(H3, userdata.pose_st.header.frame_id)
00066
00067 pprint("HOMOGENOUS PRODUCT")
00068 pprint(pose.pose)
00069 userdata.tcp_pose = pose.pose
00070
00071 return 'success'
00072
00073
00074
00075 class SM_WAM_MoveFromCartesianPose():
00076 """
00077 State machine to move the WAM given a cartesian pose.
00078
00079 It is composed by two steps:
00080 1. Transform cartesian pose to /wam_tcp
00081 2. Call to the move action using the correct cartesian pose
00082
00083 @type move_service : str
00084 @param move_service : URI of move in joinst wam service
00085 @type ik_service : str
00086 @param ik_service : URI of inverse kinematic service
00087 @type pose_st : PoseStamped [input_key]
00088 @param pose_st : Pose to move the
00089 """
00090
00091 def __init__(self, move_service, tool_frame):
00092 self.move_service = move_service
00093 self.tool_frame = tool_frame
00094
00095 def build_sm(self,tf_listener):
00096 sm = smach.StateMachine(outcomes=['success','aborted','no_kinematic_solution'],
00097 input_keys=['pose_st'])
00098
00099 sm.userdata.tcp_frame = 'wam_tcp'
00100 sm.userdata.tool_frame = self.tool_frame
00101 with sm:
00102
00103 smach.StateMachine.add('GET_POSE_FROM_FRAMES', GetPoseStampedFromFrames(tf_listener),
00104 transitions = {'success' : 'GET_TCP_POSE',
00105 'fail' : 'aborted'},
00106 remapping = { 'src_frame' : 'tool_frame',
00107 'dest_frame' : 'tcp_frame',
00108 'pose_st' : 'sm_pose_st'})
00109
00110 smach.StateMachine.add('GET_TCP_POSE', WAMGetTcpPose(),
00111 transitions = {'success' : 'MOVE_IN_CARTESIAN'},
00112 remapping = { 'pose_st' : 'pose_st',
00113 'tool_to_tcp_pose' : 'sm_pose_st',
00114 'tcp_pose' : 'sm_tcp_pose'})
00115
00116 smach.StateMachine.add('MOVE_IN_CARTESIAN', MoveWamInCartesianPose(self.move_service, 0.2, 0.3),
00117 transitions = {'success' : 'success',
00118 'fail' : 'aborted'},
00119 remapping = {'pose' : 'sm_tcp_pose'})
00120
00121 return sm