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


aubo_new_driver
Author(s): liuxin
autogenerated on Sat Jun 8 2019 19:06:02