Added test_move.py with joint_states subscriber

2026-04-10 10:00:48 +02:00 · 2016-02-19 11:38:29 +01:00
parent d1905d6745
commit a29fcac0c6
1 changed files with 131 additions and 0 deletions
--- a/test_move.py
+++ b/test_move.py
@@ -0,0 +1,131 @@
+#!/usr/bin/env python
+import time
+import roslib; roslib.load_manifest('ur_driver')
+import rospy
+import actionlib
+from control_msgs.msg import *
+from trajectory_msgs.msg import *
+from sensor_msgs.msg import JointState
+from math import pi
+
+JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
+               'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
+Q1 = [2.2,0,-1.57,0,0,0]
+Q2 = [1.5,0,-1.57,0,0,0]
+Q3 = [1.5,-0.2,-1.57,0,0,0]
+joints_pos = [0.0]*6
+    
+client = None
+
+def move1():
+    global joints_pos
+    g = FollowJointTrajectoryGoal()
+    g.trajectory = JointTrajectory()
+    g.trajectory.joint_names = JOINT_NAMES
+    g.trajectory.points = [
+        JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
+        JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
+        JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
+        JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
+    client.send_goal(g)
+    try:
+        client.wait_for_result()
+    except KeyboardInterrupt:
+        client.cancel_goal()
+        raise
+
+def move_disordered():
+    global joints_pos
+    order = [4, 2, 3, 1, 5, 0]
+    g = FollowJointTrajectoryGoal()
+    g.trajectory = JointTrajectory()
+    g.trajectory.joint_names = [JOINT_NAMES[i] for i in order]
+    q1 = [Q1[i] for i in order]
+    q2 = [Q2[i] for i in order]
+    q3 = [Q3[i] for i in order]
+    g.trajectory.points = [
+        JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
+        JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
+        JointTrajectoryPoint(positions=q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
+        JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
+    client.send_goal(g)
+    client.wait_for_result()
+    
+def move_repeated():
+    global joints_pos
+    g = FollowJointTrajectoryGoal()
+    g.trajectory = JointTrajectory()
+    g.trajectory.joint_names = JOINT_NAMES
+    
+    d = 2.0
+    g.trajectory.points = [JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0))]
+    for i in range(10):
+        g.trajectory.points.append(
+            JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d)))
+        d += 1
+        g.trajectory.points.append(
+            JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d)))
+        d += 1
+        g.trajectory.points.append(
+            JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d)))
+        d += 2
+    client.send_goal(g)
+    try:
+        client.wait_for_result()
+    except KeyboardInterrupt:
+        client.cancel_goal()
+        raise
+
+def move_interrupt():
+    global joints_pos
+    g = FollowJointTrajectoryGoal()
+    g.trajectory = JointTrajectory()
+    g.trajectory.joint_names = JOINT_NAMES
+    g.trajectory.points = [
+        JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
+        JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
+        JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
+        JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
+    
+    client.send_goal(g)
+    time.sleep(2.0)
+    print "Interrupting"
+    client.send_goal(g)
+    try:
+        client.wait_for_result()
+    except KeyboardInterrupt:
+        client.cancel_goal()
+        raise
+        
+
+def joint_subscriber(msg):
+    global joints_pos
+    joints_pos = msg.position
+   
+def main():
+    global client
+    try:
+        rospy.init_node("test_move", anonymous=True, disable_signals=True)
+        client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
+        print "Waiting for server..."
+        client.wait_for_server()
+        print "Connected to server"
+        rospy.Subscriber("joint_states", JointState, joint_subscriber)
+        print "This program makes the robot move between the following three poses:"
+        print str([Q1[i]*180./pi for i in xrange(0,6)])
+        print str([Q2[i]*180./pi for i in xrange(0,6)])
+        print str([Q3[i]*180./pi for i in xrange(0,6)])
+        print "Please make sure that your robot can move freely between these poses before proceeding!"
+        inp = raw_input("Continue? y/n: ")[0]
+        if (inp == 'y'):
+            #move1()
+            move_repeated()
+            #move_disordered()
+            #move_interrupt()
+        else:
+            print "Halting program"
+    except KeyboardInterrupt:
+        rospy.signal_shutdown("KeyboardInterrupt")
+        raise
+
+if __name__ == '__main__': main()