00001
00002
00003 import rospy
00004 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00005 from sr_robot_commander.sr_arm_commander import SrArmCommander
00006
00007 rospy.init_node("basic_arm_examples", anonymous=True)
00008
00009 arm_commander = SrArmCommander(set_ground=False)
00010
00011 rospy.sleep(rospy.Duration(2))
00012
00013 pose_1 = [0.5, -0.3, 1.2, 0, 1.57, 0]
00014 print("Planning the move to the first pose\n" + str(pose_1) + "\n")
00015 arm_commander.plan_to_pose_target(pose_1)
00016 print("Finished planning, moving the arm now.")
00017 arm_commander.execute()
00018
00019 rospy.sleep(6.0)
00020
00021 pose_2 = [0.5, 0.3, 1.2, 0, 1.57, 0]
00022 print("Planning the move to the second pose\n" + str(pose_2) + "\n")
00023 arm_commander.plan_to_pose_target(pose_2)
00024 print("Finished planning, moving the arm now.")
00025 arm_commander.execute()
00026
00027 pose_1 = [0.5, -0.3, 1.2, 0, 1.57, 0]
00028 print("Moving arm to pose\n" + str(pose_1) + "\n")
00029 arm_commander.move_to_pose_target(pose_1, wait=True)
00030
00031 rospy.sleep(6.0)
00032
00033 pose_2 = [0.5, 0.3, 1.2, 0, 1.57, 0]
00034 print("Moving arm to pose\n" + str(pose_2) + "\n")
00035 arm_commander.move_to_pose_target(pose_2, wait=True)
00036
00037 position_1 = [0.5, -0.3, 1.2]
00038 print("Moving arm to position\n" + str(position_1) + "\n")
00039 arm_commander.move_to_position_target(position_1)
00040
00041 rospy.sleep(rospy.Duration(5))
00042
00043 position_2 = [0.5, 0.3, 1.2]
00044 print("Planning the move to the second position\n" + str(position_2) + "\n")
00045 arm_commander.plan_to_position_target(position_2)
00046 print("Finished planning, moving the arm now.")
00047 arm_commander.execute()
00048
00049 rospy.sleep(rospy.Duration(5))
00050
00051 print("Arm joints position\n" + str(arm_commander.get_joints_position()) + "\n")
00052
00053 joints_states_1 = {'ra_shoulder_pan_joint': 0.43221632746577665, 'ra_elbow_joint': 2.118891128999479,
00054 'ra_wrist_1_joint': -1.711370650686752, 'ra_wrist_2_joint': 1.4834244535003318,
00055 'ra_shoulder_lift_joint': -2.5813317754982474, 'ra_wrist_3_joint': 1.6175960918705412}
00056 print("Moving arm to joints state\n" + str(joints_states_1) + "\n")
00057
00058 arm_commander.move_to_joint_value_target(joints_states_1)
00059
00060 rospy.sleep(rospy.Duration(5))
00061
00062 joints_states_2 = {'ra_shoulder_pan_joint': 0.4225743596855942, 'ra_elbow_joint': 1.9732180863151747,
00063 'ra_wrist_1_joint': -0.8874321427449576, 'ra_wrist_2_joint': -0.9214312892819567,
00064 'ra_shoulder_lift_joint': -1.9299519748391978, 'ra_wrist_3_joint': 0.7143446787498702}
00065
00066 print("Moving arm to joints state\n" + str(joints_states_2) + "\n")
00067 arm_commander.move_to_joint_value_target(joints_states_2)
00068
00069 rospy.sleep(rospy.Duration(5))
00070
00071 print("Arm joints position\n" + str(arm_commander.get_joints_position()) + "\n")
00072
00073 joints_states_3 = {'ra_shoulder_pan_joint': 1.6113530596480121, 'ra_elbow_joint': 1.1552231775506083,
00074 'ra_wrist_1_joint': -0.2393325455779891, 'ra_wrist_2_joint': 0.4969532212998553,
00075 'ra_shoulder_lift_joint': -1.5826889903403423, 'ra_wrist_3_joint': 2.1117520537195738}
00076
00077 print("Running joints trajectory")
00078
00079 joint_trajectory = JointTrajectory()
00080 joint_trajectory.header.stamp = rospy.Time.now()
00081 joint_trajectory.joint_names = list(joints_states_1.keys())
00082 joint_trajectory.points = []
00083 time_from_start = rospy.Duration(5)
00084
00085 for joints_states in [joints_states_1, joints_states_2, joints_states_3]:
00086 trajectory_point = JointTrajectoryPoint()
00087 trajectory_point.time_from_start = time_from_start
00088 time_from_start = time_from_start + rospy.Duration(5)
00089
00090 trajectory_point.positions = []
00091 trajectory_point.velocities = []
00092 trajectory_point.accelerations = []
00093 trajectory_point.effort = []
00094 for key in joint_trajectory.joint_names:
00095 trajectory_point.positions.append(joints_states[key])
00096 trajectory_point.velocities.append(0.0)
00097 trajectory_point.accelerations.append(0.0)
00098 trajectory_point.effort.append(0.0)
00099
00100 joint_trajectory.points.append(trajectory_point)
00101
00102 arm_commander.run_joint_trajectory(joint_trajectory)
00103
00104 rospy.sleep(rospy.Duration(15))
00105
00106 named_target = "gamma"
00107 print("Moving arm to named target " + named_target)
00108 arm_commander.move_to_named_target(named_target)
00109
00110 rospy.sleep(rospy.Duration(3))
00111
00112 print("Arm joints position\n" + str(arm_commander.get_joints_position()) + "\n")
00113 print("Arm joints velocities\n" + str(arm_commander.get_joints_velocity()) + "\n")