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 
00009 JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
00010                'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
00011 Q1 = [2.2,0,-1.57,0,0,0]
00012 Q2 = [1.5,0,-1.57,0,0,0]
00013 Q3 = [1.5,-0.2,-1.57,0,0,0]
00014 
00015 client = None
00016 
00017 def move1():
00018     g = FollowJointTrajectoryGoal()
00019     g.trajectory = JointTrajectory()
00020     g.trajectory.joint_names = JOINT_NAMES
00021     g.trajectory.points = [
00022         JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
00023         JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
00024         JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
00025     client.send_goal(g)
00026     try:
00027         client.wait_for_result()
00028     except KeyboardInterrupt:
00029         client.cancel_goal()
00030         raise
00031 
00032 def move_disordered():
00033     order = [4, 2, 3, 1, 5, 0]
00034     g = FollowJointTrajectoryGoal()
00035     g.trajectory = JointTrajectory()
00036     g.trajectory.joint_names = [JOINT_NAMES[i] for i in order]
00037     q1 = [Q1[i] for i in order]
00038     q2 = [Q2[i] for i in order]
00039     q3 = [Q3[i] for i in order]
00040     g.trajectory.points = [
00041         JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
00042         JointTrajectoryPoint(positions=q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
00043         JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
00044     client.send_goal(g)
00045     client.wait_for_result()
00046     
00047 def move_repeated():
00048     g = FollowJointTrajectoryGoal()
00049     g.trajectory = JointTrajectory()
00050     g.trajectory.joint_names = JOINT_NAMES
00051     
00052     d = 2.0
00053     g.trajectory.points = []
00054     for i in range(10):
00055         g.trajectory.points.append(
00056             JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d)))
00057         d += 1
00058         g.trajectory.points.append(
00059             JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d)))
00060         d += 1
00061         g.trajectory.points.append(
00062             JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d)))
00063         d += 2
00064     client.send_goal(g)
00065     try:
00066         client.wait_for_result()
00067     except KeyboardInterrupt:
00068         client.cancel_goal()
00069         raise
00070 
00071 def move_interrupt():
00072     g = FollowJointTrajectoryGoal()
00073     g.trajectory = JointTrajectory()
00074     g.trajectory.joint_names = JOINT_NAMES
00075     g.trajectory.points = [
00076         JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
00077         JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
00078         JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
00079     
00080     client.send_goal(g)
00081     time.sleep(2.0)
00082     print "Interrupting"
00083     client.send_goal(g)
00084     try:
00085         client.wait_for_result()
00086     except KeyboardInterrupt:
00087         client.cancel_goal()
00088         raise
00089 
00090 def main():
00091     global client
00092     try:
00093         rospy.init_node("test_move", anonymous=True, disable_signals=True)
00094         client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
00095         print "Waiting for server..."
00096         client.wait_for_server()
00097         print "Connected to server"
00098         #move1()
00099         move_repeated()
00100         #move_disordered()
00101         #move_interrupt()
00102     except KeyboardInterrupt:
00103         rospy.signal_shutdown("KeyboardInterrupt")
00104         raise
00105 
00106 if __name__ == '__main__': main()


ur_driver
Author(s): Stuart Glaser
autogenerated on Mon Oct 6 2014 08:25:32