gt_pr2: pr2_ears_stow_startup.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib
00004 roslib.load_manifest( 'robotis' )
00005 import rospy
00006 import robotis.lib_robotis as rs
00007 
00008 import time
00009 import math
00010 
00011 if __name__ == '__main__':
00012     rospy.logout( 'pr2_ears_stow_startup: Stowing RFID Ears Servos' )
00013     
00014     rospy.logout( 'pr2_ears_stow_startup: Connecting to servos' )
00015     dyn_left = rs.USB2Dynamixel_Device( '/dev/robot/servo1' )
00016     pan_left = rs.Robotis_Servo( dyn_left, 29 )
00017     tilt_left = rs.Robotis_Servo( dyn_left, 30 )
00018 
00019     dyn_right = rs.USB2Dynamixel_Device( '/dev/robot/servo0' )
00020     pan_right = rs.Robotis_Servo( dyn_right, 27 )
00021     tilt_right = rs.Robotis_Servo( dyn_right, 28 )
00022 
00023     # Hack to prevent the right servo from shaking.
00024     pan_right.write_address( 27, [3] ) # change the right pan compliance region
00025     pan_right.write_address( 26, [3] ) # change the right pan compliance region
00026 
00027     rospy.logout( 'pr2_ears_stow_startup: Commanding stow positions' )
00028     pan_left.move_angle( 1.370, math.radians( 10 ), False )
00029     tilt_left.move_angle( 0.0, math.radians( 10 ), False )
00030 
00031     pan_right.move_angle( -1.370, math.radians( 10 ), False )
00032     tilt_right.move_angle( 0.0, math.radians( 10 ), False )
00033 
00034     rospy.logout( 'pr2_ears_stow_startup: Done' )
00035 
00036     time.sleep(1.0)
00037