00001
00002
00003
00004
00005 import roslib
00006 roslib.load_manifest('pr2_laban_gazebo_demo')
00007 import rospy
00008 from pr2_laban_gazebo_demo.srv import ExecuteCartesianIKTrajectory
00009 from geometry_msgs.msg import Pose
00010 import time
00011 import sys
00012 import pdb
00013 import tf
00014
00015
00016
00017 def call_execute_cartesian_ik_trajectory(frame, positions, orientations):
00018 rospy.wait_for_service("execute_cartesian_ik_trajectory")
00019
00020
00021 header = roslib.msg.Header()
00022 header.frame_id = frame
00023 header.stamp = rospy.get_rostime()
00024
00025
00026 poses = []
00027 for (position, orientation) in zip(positions, orientations):
00028 pose = Pose()
00029 pose.position.x = position[0]
00030 pose.position.y = position[1]
00031 pose.position.z = position[2]
00032 pose.orientation.x = orientation[0]
00033 pose.orientation.y = orientation[1]
00034 pose.orientation.z = orientation[2]
00035 pose.orientation.w = orientation[3]
00036 poses.append(pose)
00037
00038
00039 print "calling execute_cartesian_ik_trajectory"
00040 try:
00041 s = rospy.ServiceProxy("execute_cartesian_ik_trajectory", \
00042 ExecuteCartesianIKTrajectory)
00043 resp = s(header, poses)
00044 except rospy.ServiceException, e:
00045 print "error when calling execute_cartesian_ik_trajectory: %s"%e
00046 return 0
00047 return resp.success
00048
00049
00050 def pplist(list):
00051 return ' '.join(['%2.3f'%x for x in list])
00052
00053
00054 if __name__ == "__main__":
00055 rospy.init_node("test_cartesian_ik_trajectory_executer")
00056 tf_listener = tf.TransformListener()
00057 time.sleep(.5)
00058
00059 joint_names = ["r_shoulder_pan_joint",
00060 "r_shoulder_lift_joint",
00061 "r_upper_arm_roll_joint",
00062 "r_elbow_flex_joint",
00063 "r_forearm_roll_joint",
00064 "r_wrist_flex_joint",
00065 "r_wrist_roll_joint"]
00066
00067 positions = [[.76, -.19, .83], [0.59, -0.36, 0.93]]
00068 orientations = [[.02, -.09, 0.0, 1.0], [0.65, -0.21, .38, .62]]
00069
00070 success = call_execute_cartesian_ik_trajectory("base_link", \
00071 positions, orientations)
00072
00073
00074 (trans, rot) = tf_listener.lookupTransform('base_link', 'r_wrist_roll_link', rospy.Time(0))
00075 print "end Cartesian pose: trans", pplist(trans), "rot", pplist(rot)
00076
00077 if success:
00078 print "trajectory succeeded!"
00079 else:
00080 print "trajectory failed."
00081