From a29fcac0c6a87d2768b6b718e54cb4fee62f2777 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 19 Feb 2016 11:38:29 +0100 Subject: [PATCH] Added test_move.py with joint_states subscriber --- test_move.py | 131 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100755 test_move.py diff --git a/test_move.py b/test_move.py new file mode 100755 index 0000000..947d9d1 --- /dev/null +++ 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()