Go to the documentation of this file.00001
00002 """A helper program to test cartesian goals for the JACO and MICO arms."""
00003
00004 import roslib; roslib.load_manifest('jaco_demo')
00005 import rospy
00006
00007 import sys
00008 import numpy as np
00009
00010 import actionlib
00011 import jaco_msgs.msg
00012 import std_msgs.msg
00013 import geometry_msgs.msg
00014
00015 import goal_generators
00016
00017
00018 def cartesian_pose_client(position, orientation):
00019 """Send a cartesian goal to the action server."""
00020 action_address = '/' + str(sys.argv[1]) + '_arm_driver/arm_pose/arm_pose'
00021 client = actionlib.SimpleActionClient(action_address, jaco_msgs.msg.ArmPoseAction)
00022 client.wait_for_server()
00023
00024 goal = jaco_msgs.msg.ArmPoseGoal()
00025 goal.pose.header = std_msgs.msg.Header(frame_id=(str(sys.argv[1]) + '_api_origin'))
00026 goal.pose.pose.position = geometry_msgs.msg.Point(
00027 x=position[0], y=position[1], z=position[2])
00028 goal.pose.pose.orientation = geometry_msgs.msg.Quaternion(
00029 x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3])
00030
00031 client.send_goal(goal)
00032
00033 if client.wait_for_result(rospy.Duration(10.0)):
00034 return client.get_result()
00035 else:
00036 client.cancel_all_goals()
00037 print(' the cartesian action timed-out')
00038 return None
00039
00040
00041 if __name__ == '__main__':
00042 if len(sys.argv) not in [3, 4, 9] or 'help' in str(sys.argv):
00043 print('Usage:')
00044 print(' cartesian_workout.py node_name random num - randomly generate num poses')
00045 print(' cartesian_workout.py node_name file_path - use poses from file')
00046 print(' cartesian_workout.py node_name x y z qx qy qz qw - use that specific pose')
00047 exit()
00048
00049 try:
00050 rospy.init_node(str(sys.argv[1]) + '_cartesian_workout')
00051
00052 if str(sys.argv[2]) == 'random' and len(sys.argv) == 4:
00053 print('Using {} randomly generated poses'.format(int(sys.argv[3])))
00054 poses = goal_generators.random_pose_generator(int(sys.argv[3]))
00055 elif len(sys.argv) == 3:
00056 print('Using poses from file: {}'.format(sys.argv[2]))
00057 poses = goal_generators.poses_from_file(str(sys.argv[2]))
00058 else:
00059 print('Using the specified pose:')
00060 raw_pose = [float(n) for n in sys.argv[2:]]
00061 mag = np.sqrt(sum(np.power(raw_pose[3:], 2)))
00062 """poses = [(raw_pose[:3], raw_pose[3:] / mag)]"""
00063 poses = [(raw_pose[:3], raw_pose[3:])]
00064
00065 for pos, orient in poses:
00066 print(' position: {}, orientation: {}'.format(pos, orient))
00067 result = cartesian_pose_client(pos, orient)
00068
00069 print('Done!')
00070
00071 except rospy.ROSInterruptException:
00072 print "program interrupted before completion"