00001
00002 import time
00003 import roslib; roslib.load_manifest('ur_driver')
00004 import rospy
00005 import actionlib
00006 from control_msgs.msg import *
00007 from trajectory_msgs.msg import *
00008 from sensor_msgs.msg import JointState
00009 from math import pi
00010
00011 JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
00012 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
00013 Q1 = [2.2,0,-1.57,0,0,0]
00014 Q2 = [1.5,0,-1.57,0,0,0]
00015 Q3 = [1.5,-0.2,-1.57,0,0,0]
00016
00017 client = None
00018
00019 def move1():
00020 global joints_pos
00021 g = FollowJointTrajectoryGoal()
00022 g.trajectory = JointTrajectory()
00023 g.trajectory.joint_names = JOINT_NAMES
00024 try:
00025 joint_states = rospy.wait_for_message("joint_states", JointState)
00026 joints_pos = joint_states.position
00027 g.trajectory.points = [
00028 JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
00029 JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
00030 JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
00031 JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
00032 client.send_goal(g)
00033 client.wait_for_result()
00034 except KeyboardInterrupt:
00035 client.cancel_goal()
00036 raise
00037 except:
00038 raise
00039
00040 def move_disordered():
00041 order = [4, 2, 3, 1, 5, 0]
00042 g = FollowJointTrajectoryGoal()
00043 g.trajectory = JointTrajectory()
00044 g.trajectory.joint_names = [JOINT_NAMES[i] for i in order]
00045 q1 = [Q1[i] for i in order]
00046 q2 = [Q2[i] for i in order]
00047 q3 = [Q3[i] for i in order]
00048 try:
00049 joint_states = rospy.wait_for_message("joint_states", JointState)
00050 joints_pos = joint_states.position
00051 g.trajectory.points = [
00052 JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
00053 JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
00054 JointTrajectoryPoint(positions=q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
00055 JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
00056 client.send_goal(g)
00057 client.wait_for_result()
00058 except KeyboardInterrupt:
00059 client.cancel_goal()
00060 raise
00061 except:
00062 raise
00063
00064 def move_repeated():
00065 g = FollowJointTrajectoryGoal()
00066 g.trajectory = JointTrajectory()
00067 g.trajectory.joint_names = JOINT_NAMES
00068 try:
00069 joint_states = rospy.wait_for_message("joint_states", JointState)
00070 joints_pos = joint_states.position
00071 d = 2.0
00072 g.trajectory.points = [JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0))]
00073 for i in range(10):
00074 g.trajectory.points.append(
00075 JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d)))
00076 d += 1
00077 g.trajectory.points.append(
00078 JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d)))
00079 d += 1
00080 g.trajectory.points.append(
00081 JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d)))
00082 d += 2
00083 client.send_goal(g)
00084 client.wait_for_result()
00085 except KeyboardInterrupt:
00086 client.cancel_goal()
00087 raise
00088 except:
00089 raise
00090
00091 def move_interrupt():
00092 g = FollowJointTrajectoryGoal()
00093 g.trajectory = JointTrajectory()
00094 g.trajectory.joint_names = JOINT_NAMES
00095 try:
00096 joint_states = rospy.wait_for_message("joint_states", JointState)
00097 joints_pos = joint_states.position
00098 g.trajectory.points = [
00099 JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
00100 JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
00101 JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
00102 JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
00103
00104 client.send_goal(g)
00105 time.sleep(3.0)
00106 print "Interrupting"
00107 joint_states = rospy.wait_for_message("joint_states", JointState)
00108 joints_pos = joint_states.position
00109 g.trajectory.points = [
00110 JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
00111 JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
00112 JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
00113 JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
00114 client.send_goal(g)
00115 client.wait_for_result()
00116 except KeyboardInterrupt:
00117 client.cancel_goal()
00118 raise
00119 except:
00120 raise
00121
00122 def main():
00123 global client
00124 try:
00125 rospy.init_node("test_move", anonymous=True, disable_signals=True)
00126 client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
00127 print "Waiting for server..."
00128 client.wait_for_server()
00129 print "Connected to server"
00130 parameters = rospy.get_param(None)
00131 index = str(parameters).find('prefix')
00132 if (index > 0):
00133 prefix = str(parameters)[index+len("prefix': '"):(index+len("prefix': '")+str(parameters)[index+len("prefix': '"):-1].find("'"))]
00134 for i, name in enumerate(JOINT_NAMES):
00135 JOINT_NAMES[i] = prefix + name
00136 print "This program makes the robot move between the following three poses:"
00137 print str([Q1[i]*180./pi for i in xrange(0,6)])
00138 print str([Q2[i]*180./pi for i in xrange(0,6)])
00139 print str([Q3[i]*180./pi for i in xrange(0,6)])
00140 print "Please make sure that your robot can move freely between these poses before proceeding!"
00141 inp = raw_input("Continue? y/n: ")[0]
00142 if (inp == 'y'):
00143
00144 move_repeated()
00145
00146
00147 else:
00148 print "Halting program"
00149 except KeyboardInterrupt:
00150 rospy.signal_shutdown("KeyboardInterrupt")
00151 raise
00152
00153 if __name__ == '__main__': main()