test_move.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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             #move1()
00144             move_repeated()
00145             #move_disordered()
00146             #move_interrupt()
00147         else:
00148             print "Halting program"
00149     except KeyboardInterrupt:
00150         rospy.signal_shutdown("KeyboardInterrupt")
00151         raise
00152 
00153 if __name__ == '__main__': main()


ur_modern_driver
Author(s): Thomas Timm Andersen
autogenerated on Wed Apr 3 2019 02:55:31