$search
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_part2') 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 dynamixels = rospy.get_param('dynamixels', '') 00016 00017 torque_services = list() 00018 speed_services = list() 00019 00020 for name in sorted(dynamixels): 00021 controller = name.replace("_joint", "") + "_controller" 00022 00023 torque_service = '/' + controller + '/torque_enable' 00024 rospy.wait_for_service(torque_service) 00025 torque_services.append(rospy.ServiceProxy(torque_service, TorqueEnable)) 00026 00027 speed_service = '/' + controller + '/set_speed' 00028 rospy.wait_for_service(speed_service) 00029 speed_services.append(rospy.ServiceProxy(speed_service, SetSpeed)) 00030 00031 # Set the default speed to something small 00032 for set_speed in speed_services: 00033 set_speed(0.1) 00034 00035 # Relax all servos to give them a rest. 00036 for torque_enable in torque_services: 00037 torque_enable(False) 00038 00039 if __name__=='__main__': 00040 Relax()