18 import roslib; roslib.load_manifest(
'ur_driver')
23 from sensor_msgs.msg
import JointState
26 JOINT_NAMES = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'elbow_joint',
27 'wrist_1_joint',
'wrist_2_joint',
'wrist_3_joint']
28 Q1 = [2.2,0,-1.57,0,0,0]
29 Q2 = [1.5,0,-1.57,0,0,0]
30 Q3 = [1.5,-0.2,-1.57,0,0,0]
36 g = FollowJointTrajectoryGoal()
37 g.trajectory = JointTrajectory()
38 g.trajectory.joint_names = JOINT_NAMES
40 joint_states = rospy.wait_for_message(
"joint_states", JointState)
41 joints_pos = joint_states.position
42 g.trajectory.points = [
43 JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
44 JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
45 JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
46 JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
48 client.wait_for_result()
49 except KeyboardInterrupt:
56 order = [4, 2, 3, 1, 5, 0]
57 g = FollowJointTrajectoryGoal()
58 g.trajectory = JointTrajectory()
59 g.trajectory.joint_names = [JOINT_NAMES[i]
for i
in order]
60 q1 = [Q1[i]
for i
in order]
61 q2 = [Q2[i]
for i
in order]
62 q3 = [Q3[i]
for i
in order]
64 joint_states = rospy.wait_for_message(
"joint_states", JointState)
65 joints_pos = joint_states.position
66 g.trajectory.points = [
67 JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
68 JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
69 JointTrajectoryPoint(positions=q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
70 JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
72 client.wait_for_result()
73 except KeyboardInterrupt:
80 g = FollowJointTrajectoryGoal()
81 g.trajectory = JointTrajectory()
82 g.trajectory.joint_names = JOINT_NAMES
84 joint_states = rospy.wait_for_message(
"joint_states", JointState)
85 joints_pos = joint_states.position
87 g.trajectory.points = [JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0))]
89 g.trajectory.points.append(
90 JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d)))
92 g.trajectory.points.append(
93 JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d)))
95 g.trajectory.points.append(
96 JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d)))
99 client.wait_for_result()
100 except KeyboardInterrupt:
107 g = FollowJointTrajectoryGoal()
108 g.trajectory = JointTrajectory()
109 g.trajectory.joint_names = JOINT_NAMES
111 joint_states = rospy.wait_for_message(
"joint_states", JointState)
112 joints_pos = joint_states.position
113 g.trajectory.points = [
114 JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
115 JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
116 JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
117 JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
122 joint_states = rospy.wait_for_message(
"joint_states", JointState)
123 joints_pos = joint_states.position
124 g.trajectory.points = [
125 JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
126 JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
127 JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
128 JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
130 client.wait_for_result()
131 except KeyboardInterrupt:
140 rospy.init_node(
"test_move", anonymous=
True, disable_signals=
True)
142 print "Waiting for server..." 143 client.wait_for_server()
144 print "Connected to server" 145 parameters = rospy.get_param(
None)
146 index = str(parameters).find(
'prefix')
148 prefix = str(parameters)[index+len(
"prefix': '"):(index+len(
"prefix': '")+str(parameters)[index+len(
"prefix': '"):-1].find(
"'"))]
149 for i, name
in enumerate(JOINT_NAMES):
150 JOINT_NAMES[i] = prefix + name
151 print "This program makes the robot move between the following three poses:" 152 print str([Q1[i]*180./pi
for i
in xrange(0,6)])
153 print str([Q2[i]*180./pi
for i
in xrange(0,6)])
154 print str([Q3[i]*180./pi
for i
in xrange(0,6)])
155 print "Please make sure that your robot can move freely between these poses before proceeding!" 156 inp = raw_input(
"Continue? y/n: ")[0]
163 print "Halting program" 164 except KeyboardInterrupt:
165 rospy.signal_shutdown(
"KeyboardInterrupt")
168 if __name__ ==
'__main__':
main()