sr_example: sr_right_mix_examples

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from sr_robot_commander.sr_arm_commander import SrArmCommander
00005 from sr_robot_commander.sr_hand_commander import SrHandCommander
00006 
00007 
00008 rospy.init_node("basic_arm_examples", anonymous=True)
00009 
00010 hand_commander = SrHandCommander()
00011 arm_commander = SrArmCommander()
00012 
00013 rospy.loginfo("Set arm teach mode ON")
00014 arm_commander.set_teach_mode(True)
00015 # sleep for some time during which the arm can be moved around by pushing it
00016 # but be careful to get away before the time runs out. You are warned
00017 rospy.sleep(20.0)
00018 rospy.loginfo("Set arm teach mode OFF")
00019 arm_commander.set_teach_mode(False)
00020 
00021 hand_joint_states_1 = {'rh_FFJ1': 0.35, 'rh_FFJ2': 0.0, 'rh_FFJ3': 0.0, 'rh_FFJ4': 0.0,
00022                        'rh_MFJ1': 0.35, 'rh_MFJ2': 0.0, 'rh_MFJ3': 0.0, 'rh_MFJ4': 0.0,
00023                        'rh_RFJ1': 0.35, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0,
00024                        'rh_LFJ1': 0.35, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0,
00025                        'rh_THJ1': 0.35, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0, 'rh_THJ4': 0.0, 'rh_THJ5': 0.0,
00026                        'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
00027 hand_joint_states_2 = {'rh_FFJ1': 0.35, 'rh_FFJ2': 1.5707, 'rh_FFJ3': 1.5707, 'rh_FFJ4': 0.0,
00028                        'rh_MFJ1': 0.35, 'rh_MFJ2': 1.5707, 'rh_MFJ3': 1.5707, 'rh_MFJ4': 0.0,
00029                        'rh_RFJ1': 0.35, 'rh_RFJ2': 1.5707, 'rh_RFJ3': 1.5707, 'rh_RFJ4': 0.0,
00030                        'rh_LFJ1': 0.35, 'rh_LFJ2': 1.5707, 'rh_LFJ3': 1.5707, 'rh_LFJ4': 0.0,
00031                        'rh_THJ1': 0.35, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0, 'rh_THJ4': 0.0, 'rh_THJ5': 0.0,
00032                        'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
00033 hand_joint_states_3 = {'rh_FFJ1': 0.35, 'rh_FFJ2': 0.0, 'rh_MFJ3': 0.0}
00034 
00035 
00036 joint_states_1 = {'ra_shoulder_pan_joint': 0.43221632746577665, 'ra_elbow_joint': 2.118891128999479,
00037                    'ra_wrist_1_joint': -1.711370650686752, 'ra_wrist_2_joint': 1.4834244535003318,
00038                    'ra_shoulder_lift_joint': -2.5813317754982474, 'ra_wrist_3_joint': 1.6175960918705412}
00039 
00040 joint_states_2 = {'ra_shoulder_pan_joint': 0.4225743596855942, 'ra_elbow_joint': 1.9732180863151747,
00041                    'ra_wrist_1_joint': -0.8874321427449576, 'ra_wrist_2_joint': -0.9214312892819567,
00042                    'ra_shoulder_lift_joint': -1.9299519748391978, 'ra_wrist_3_joint': 0.7143446787498702}
00043 # Move hand
00044 joint_states = hand_joint_states_1
00045 rospy.loginfo("Moving hand to joint states\n" + str(joint_states) + "\n")
00046 hand_commander.move_to_joint_value_target_unsafe(joint_states, 1.0, False)
00047 
00048 
00049 
00050 
00051 # Move arm
00052 joint_states = joint_states_1
00053 rospy.loginfo("Moving arm to joint states\n" + str(joint_states) + "\n")
00054 arm_commander.move_to_joint_value_target_unsafe(joint_states, 3.0, True)
00055 
00056 # Move hand
00057 joint_states = hand_joint_states_2
00058 rospy.loginfo("Moving hand to joint states\n" + str(joint_states) + "\n")
00059 hand_commander.move_to_joint_value_target_unsafe(joint_states, 3.0, True)
00060 
00061 
00062 # Move hand
00063 joint_states = hand_joint_states_3
00064 rospy.loginfo("Moving hand to joint states\n" + str(joint_states) + "\n")
00065 hand_commander.move_to_joint_value_target_unsafe(joint_states, 3.0, False)
00066 
00067 # Move arm
00068 joint_states = joint_states_2
00069 rospy.loginfo("Moving arm to joint states\n" + str(joint_states) + "\n")
00070 arm_commander.move_to_joint_value_target_unsafe(joint_states, 3.0, wait= True)
00071 
00072 
00073 hand_commander.set_teach_mode(True)
00074 # sleep for some time during which the hand joints can be moved manually
00075 rospy.sleep(20.0)
00076 rospy.loginfo("Set hand teach mode OFF")
00077 hand_commander.set_teach_mode(False)
00078 
00079 
00080 
00081