relax_all_servos.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     Relax all servos by disabling the torque for each.
00005 """
00006 import roslib
00007 roslib.load_manifest('pi_head_tracking_3d_part1')
00008 import rospy, time
00009 from dynamixel_controllers.srv import TorqueEnable, SetSpeed
00010 
00011 class Relax():
00012     def __init__(self):
00013         rospy.init_node('relax_all_servos')
00014         
00015         dynamixel_namespace = rospy.get_namespace()
00016         if dynamixel_namespace == '/':
00017             dynamixel_namespace = rospy.get_param('~dynamixel_namespace', '/dynamixel_controller')
00018         dynamixels = rospy.get_param(dynamixel_namespace + '/dynamixels', dict())
00019         
00020         servo_torque_enable = list()
00021         servo_set_speed = list()
00022         
00023         for name in sorted(dynamixels):
00024             torque_enable_service = dynamixel_namespace + '/' + name + '_controller/torque_enable'
00025             rospy.wait_for_service(torque_enable_service)  
00026             servo_torque_enable.append(rospy.ServiceProxy(torque_enable_service, TorqueEnable))
00027             
00028             set_speed_service = dynamixel_namespace + '/' + name + '_controller/set_speed'
00029             rospy.wait_for_service(set_speed_service)
00030             servo_set_speed.append(rospy.ServiceProxy(set_speed_service, SetSpeed))
00031 
00032         # Give the servos an intial speed that is relatively slow for safety
00033         for set_speed in servo_set_speed:
00034             set_speed(0.3)
00035         
00036         # Relax all servos to give them a rest.
00037         for torque_enable in servo_torque_enable:
00038             torque_enable(False)
00039         
00040 if __name__=='__main__':
00041     Relax()


pi_head_tracking_3d_part1
Author(s): Patrick Goebel
autogenerated on Mon Jan 6 2014 11:33:45