test_move.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #
3 # Copyright 2015, 2016 Thomas Timm Andersen (original version)
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16 
17 import time
18 import roslib; roslib.load_manifest('ur_driver')
19 import rospy
20 import actionlib
21 from control_msgs.msg import *
22 from trajectory_msgs.msg import *
23 from sensor_msgs.msg import JointState
24 from math import pi
25 
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]
31 
32 client = None
33 
34 def move1():
35  global joints_pos
36  g = FollowJointTrajectoryGoal()
37  g.trajectory = JointTrajectory()
38  g.trajectory.joint_names = JOINT_NAMES
39  try:
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))]
47  client.send_goal(g)
48  client.wait_for_result()
49  except KeyboardInterrupt:
50  client.cancel_goal()
51  raise
52  except:
53  raise
54 
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]
63  try:
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))]
71  client.send_goal(g)
72  client.wait_for_result()
73  except KeyboardInterrupt:
74  client.cancel_goal()
75  raise
76  except:
77  raise
78 
80  g = FollowJointTrajectoryGoal()
81  g.trajectory = JointTrajectory()
82  g.trajectory.joint_names = JOINT_NAMES
83  try:
84  joint_states = rospy.wait_for_message("joint_states", JointState)
85  joints_pos = joint_states.position
86  d = 2.0
87  g.trajectory.points = [JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0))]
88  for i in range(10):
89  g.trajectory.points.append(
90  JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d)))
91  d += 1
92  g.trajectory.points.append(
93  JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d)))
94  d += 1
95  g.trajectory.points.append(
96  JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d)))
97  d += 2
98  client.send_goal(g)
99  client.wait_for_result()
100  except KeyboardInterrupt:
101  client.cancel_goal()
102  raise
103  except:
104  raise
105 
107  g = FollowJointTrajectoryGoal()
108  g.trajectory = JointTrajectory()
109  g.trajectory.joint_names = JOINT_NAMES
110  try:
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))]
118 
119  client.send_goal(g)
120  time.sleep(3.0)
121  print "Interrupting"
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))]
129  client.send_goal(g)
130  client.wait_for_result()
131  except KeyboardInterrupt:
132  client.cancel_goal()
133  raise
134  except:
135  raise
136 
137 def main():
138  global client
139  try:
140  rospy.init_node("test_move", anonymous=True, disable_signals=True)
141  client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
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')
147  if (index > 0):
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]
157  if (inp == 'y'):
158  #move1()
159  move_repeated()
160  #move_disordered()
161  #move_interrupt()
162  else:
163  print "Halting program"
164  except KeyboardInterrupt:
165  rospy.signal_shutdown("KeyboardInterrupt")
166  raise
167 
168 if __name__ == '__main__': main()
def move1()
Definition: test_move.py:34
def move_repeated()
Definition: test_move.py:79
def move_disordered()
Definition: test_move.py:55
def main()
Definition: test_move.py:137
def move_interrupt()
Definition: test_move.py:106


ur_modern_driver
Author(s): Thomas Timm Andersen, Simon Rasmussen
autogenerated on Fri Jun 26 2020 03:37:01