dynamixel_driver: dynamixel_serial

Go to the documentation of this file.
00001 # -*- coding: utf-8 -*-
00002 #
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2010-2011, Antons Rebguns.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of University of Arizona nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 
00036 __author__ = 'Antons Rebguns'
00037 __copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
00038 __credits__ = 'Cody Jorgensen, Cara Slutter'
00039 
00040 __license__ = 'BSD'
00041 __maintainer__ = 'Antons Rebguns'
00042 __email__ = 'anton@email.arizona.edu'
00043 
00044 
00045 import math
00046 import sys
00047 import errno
00048 from collections import deque
00049 from threading import Thread
00050 from collections import defaultdict
00051 
00052 import roslib
00053 roslib.load_manifest('dynamixel_driver')
00054 
00055 import rospy
00056 import dynamixel_io
00057 from dynamixel_driver.dynamixel_const import *
00058 
00059 from diagnostic_msgs.msg import DiagnosticArray
00060 from diagnostic_msgs.msg import DiagnosticStatus
00061 from diagnostic_msgs.msg import KeyValue
00062 
00063 from dynamixel_msgs.msg import MotorState
00064 from dynamixel_msgs.msg import MotorStateList
00065 
00066 class SerialProxy():
00067     def __init__(self,
00068                  port_name='/dev/ttyUSB0',
00069                  port_namespace='ttyUSB0',
00070                  baud_rate='1000000',
00071                  min_motor_id=1,
00072                  max_motor_id=25,
00073                  update_rate=5,
00074                  diagnostics_rate=1,
00075                  error_level_temp=75,
00076                  warn_level_temp=70):
00077         self.port_name = port_name
00078         self.port_namespace = port_namespace
00079         self.baud_rate = baud_rate
00080         self.min_motor_id = min_motor_id
00081         self.max_motor_id = max_motor_id
00082         self.update_rate = update_rate
00083         self.diagnostics_rate = diagnostics_rate
00084         self.error_level_temp = error_level_temp
00085         self.warn_level_temp = warn_level_temp
00086         
00087         self.actual_rate = update_rate
00088         self.error_counts = {'non_fatal': 0, 'checksum': 0, 'dropped': 0}
00089         self.current_state = MotorStateList()
00090         self.num_ping_retries = 5
00091         
00092         self.motor_states_pub = rospy.Publisher('motor_states/%s' % self.port_namespace, MotorStateList)
00093         self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
00094 
00095     def connect(self):
00096         try:
00097             self.dxl_io = dynamixel_io.DynamixelIO(self.port_name, self.baud_rate)
00098             self.__find_motors()
00099         except dynamixel_io.SerialOpenError, e:
00100             rospy.logfatal(e.message)
00101             sys.exit(1)
00102             
00103         self.running = True
00104         if self.update_rate > 0: Thread(target=self.__update_motor_states).start()
00105         if self.diagnostics_rate > 0: Thread(target=self.__publish_diagnostic_information).start()
00106 
00107     def disconnect(self):
00108         self.running = False
00109 
00110     def __fill_motor_parameters(self, motor_id, model_number):
00111         """
00112         Stores some extra information about each motor on the parameter server.
00113         Some of these paramters are used in joint controller implementation.
00114         """
00115         angles = self.dxl_io.get_angle_limits(motor_id)
00116         voltage = self.dxl_io.get_voltage(motor_id)
00117         voltages = self.dxl_io.get_voltage_limits(motor_id)
00118         
00119         rospy.set_param('dynamixel/%s/%d/model_number' %(self.port_namespace, motor_id), model_number)
00120         rospy.set_param('dynamixel/%s/%d/model_name' %(self.port_namespace, motor_id), DXL_MODEL_TO_PARAMS[model_number]['name'])
00121         rospy.set_param('dynamixel/%s/%d/min_angle' %(self.port_namespace, motor_id), angles['min'])
00122         rospy.set_param('dynamixel/%s/%d/max_angle' %(self.port_namespace, motor_id), angles['max'])
00123         
00124         torque_per_volt = DXL_MODEL_TO_PARAMS[model_number]['torque_per_volt']
00125         rospy.set_param('dynamixel/%s/%d/torque_per_volt' %(self.port_namespace, motor_id), torque_per_volt)
00126         rospy.set_param('dynamixel/%s/%d/max_torque' %(self.port_namespace, motor_id), torque_per_volt * voltage)
00127         
00128         velocity_per_volt = DXL_MODEL_TO_PARAMS[model_number]['velocity_per_volt']
00129         rospy.set_param('dynamixel/%s/%d/velocity_per_volt' %(self.port_namespace, motor_id), velocity_per_volt)
00130         rospy.set_param('dynamixel/%s/%d/max_velocity' %(self.port_namespace, motor_id), velocity_per_volt * voltage)
00131         rospy.set_param('dynamixel/%s/%d/radians_second_per_encoder_tick' %(self.port_namespace, motor_id), velocity_per_volt * voltage / DXL_MAX_SPEED_TICK)
00132         
00133         encoder_resolution = DXL_MODEL_TO_PARAMS[model_number]['encoder_resolution']
00134         range_degrees = DXL_MODEL_TO_PARAMS[model_number]['range_degrees']
00135         range_radians = math.radians(range_degrees)
00136         rospy.set_param('dynamixel/%s/%d/encoder_resolution' %(self.port_namespace, motor_id), encoder_resolution)
00137         rospy.set_param('dynamixel/%s/%d/range_degrees' %(self.port_namespace, motor_id), range_degrees)
00138         rospy.set_param('dynamixel/%s/%d/range_radians' %(self.port_namespace, motor_id), range_radians)
00139         rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_degree' %(self.port_namespace, motor_id), encoder_resolution / range_degrees)
00140         rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_radian' %(self.port_namespace, motor_id), encoder_resolution / range_radians)
00141         rospy.set_param('dynamixel/%s/%d/degrees_per_encoder_tick' %(self.port_namespace, motor_id), range_degrees / encoder_resolution)
00142         rospy.set_param('dynamixel/%s/%d/radians_per_encoder_tick' %(self.port_namespace, motor_id), range_radians / encoder_resolution)
00143         
00144         # keep some parameters around for diagnostics
00145         self.motor_static_info[motor_id] = {}
00146         self.motor_static_info[motor_id]['model'] = DXL_MODEL_TO_PARAMS[model_number]['name']
00147         self.motor_static_info[motor_id]['firmware'] = self.dxl_io.get_firmware_version(motor_id)
00148         self.motor_static_info[motor_id]['delay'] = self.dxl_io.get_return_delay_time(motor_id)
00149         self.motor_static_info[motor_id]['min_angle'] = angles['min']
00150         self.motor_static_info[motor_id]['max_angle'] = angles['max']
00151         self.motor_static_info[motor_id]['min_voltage'] = voltages['min']
00152         self.motor_static_info[motor_id]['max_voltage'] = voltages['max']
00153 
00154     def __find_motors(self):
00155         rospy.loginfo('%s: Pinging motor IDs %d through %d...' % (self.port_namespace, self.min_motor_id, self.max_motor_id))
00156         self.motors = []
00157         self.motor_static_info = {}
00158         
00159         for motor_id in range(self.min_motor_id, self.max_motor_id + 1):
00160             for trial in range(self.num_ping_retries):
00161                 try:
00162                     result = self.dxl_io.ping(motor_id)
00163                 except Exception as ex:
00164                     rospy.logerr('Exception thrown while pinging motor %d - %s' % (motor_id, ex))
00165                     continue
00166                     
00167                 if result:
00168                     self.motors.append(motor_id)
00169                     break
00170                     
00171         if not self.motors:
00172             rospy.logfatal('%s: No motors found.' % self.port_namespace)
00173             sys.exit(1)
00174             
00175         counts = defaultdict(int)
00176         
00177         to_delete_if_error = []
00178         for motor_id in self.motors:
00179             for trial in range(self.num_ping_retries):
00180                 try:
00181                     model_number = self.dxl_io.get_model_number(motor_id)
00182                     self.__fill_motor_parameters(motor_id, model_number)
00183                 except Exception as ex:
00184                     rospy.logerr('Exception thrown while getting attributes for motor %d - %s' % (motor_id, ex))
00185                     if trial == self.num_ping_retries - 1: to_delete_if_error.append(motor_id)
00186                     continue
00187                     
00188                 counts[model_number] += 1
00189                 break
00190                 
00191         for motor_id in to_delete_if_error:
00192             self.motors.remove(motor_id)
00193             
00194         rospy.set_param('dynamixel/%s/connected_ids' % self.port_namespace, self.motors)
00195         
00196         status_str = '%s: Found %d motors - ' % (self.port_namespace, len(self.motors))
00197         for model_number,count in counts.items():
00198             if count:
00199                 model_name = DXL_MODEL_TO_PARAMS[model_number]['name']
00200                 status_str += '%d %s [' % (count, model_name)
00201                 
00202                 for motor_id in self.motors:
00203                     if self.motor_static_info[motor_id]['model'] == model_name:
00204                         status_str += '%d, ' % motor_id
00205                         
00206                 status_str = status_str[:-2] + '], '
00207                 
00208         rospy.loginfo('%s, initialization complete.' % status_str[:-2])
00209 
00210     def __update_motor_states(self):
00211         num_events = 50
00212         rates = deque([float(self.update_rate)]*num_events, maxlen=num_events)
00213         last_time = rospy.Time.now()
00214         
00215         rate = rospy.Rate(self.update_rate)
00216         while not rospy.is_shutdown() and self.running:
00217             # get current state of all motors and publish to motor_states topic
00218             motor_states = []
00219             for motor_id in self.motors:
00220                 try:
00221                     state = self.dxl_io.get_feedback(motor_id)
00222                     if state:
00223                         motor_states.append(MotorState(**state))
00224                         if dynamixel_io.exception: raise dynamixel_io.exception
00225                 except dynamixel_io.FatalErrorCodeError, fece:
00226                     rospy.logerr(fece)
00227                 except dynamixel_io.NonfatalErrorCodeError, nfece:
00228                     self.error_counts['non_fatal'] += 1
00229                     rospy.logdebug(nfece)
00230                 except dynamixel_io.ChecksumError, cse:
00231                     self.error_counts['checksum'] += 1
00232                     rospy.logdebug(cse)
00233                 except dynamixel_io.DroppedPacketError, dpe:
00234                     self.error_counts['dropped'] += 1
00235                     rospy.logdebug(dpe.message)
00236                 except OSError, ose:
00237                     if ose.errno != errno.EAGAIN:
00238                         rospy.logfatal(errno.errorcode[ose.errno])
00239                         rospy.signal_shutdown(errno.errorcode[ose.errno])
00240                         
00241             if motor_states:
00242                 msl = MotorStateList()
00243                 msl.motor_states = motor_states
00244                 self.motor_states_pub.publish(msl)
00245                 
00246                 self.current_state = msl
00247                 
00248                 # calculate actual update rate
00249                 current_time = rospy.Time.now()
00250                 rates.append(1.0 / (current_time - last_time).to_sec())
00251                 self.actual_rate = round(sum(rates)/num_events, 2)
00252                 last_time = current_time
00253                 
00254             rate.sleep()
00255 
00256     def __publish_diagnostic_information(self):
00257         diag_msg = DiagnosticArray()
00258         
00259         rate = rospy.Rate(self.diagnostics_rate)
00260         while not rospy.is_shutdown() and self.running:
00261             diag_msg.status = []
00262             diag_msg.header.stamp = rospy.Time.now()
00263             
00264             status = DiagnosticStatus()
00265             
00266             status.name = 'Dynamixel Serial Bus (%s)' % self.port_namespace
00267             status.hardware_id = 'Dynamixel Serial Bus on port %s' % self.port_name
00268             status.values.append(KeyValue('Baud Rate', str(self.baud_rate)))
00269             status.values.append(KeyValue('Min Motor ID', str(self.min_motor_id)))
00270             status.values.append(KeyValue('Max Motor ID', str(self.max_motor_id)))
00271             status.values.append(KeyValue('Desired Update Rate', str(self.update_rate)))
00272             status.values.append(KeyValue('Actual Update Rate', str(self.actual_rate)))
00273             status.values.append(KeyValue('# Non Fatal Errors', str(self.error_counts['non_fatal'])))
00274             status.values.append(KeyValue('# Checksum Errors', str(self.error_counts['checksum'])))
00275             status.values.append(KeyValue('# Dropped Packet Errors', str(self.error_counts['dropped'])))
00276             status.level = DiagnosticStatus.OK
00277             status.message = 'OK'
00278             
00279             if self.actual_rate - self.update_rate < -5:
00280                 status.level = DiagnosticStatus.WARN
00281                 status.message = 'Actual update rate is lower than desired'
00282                 
00283             diag_msg.status.append(status)
00284             
00285             for motor_state in self.current_state.motor_states:
00286                 mid = motor_state.id
00287                 
00288                 status = DiagnosticStatus()
00289                 
00290                 status.name = 'Robotis Dynamixel Motor %d on port %s' % (mid, self.port_namespace)
00291                 status.hardware_id = 'DXL-%d@%s' % (motor_state.id, self.port_namespace)
00292                 status.values.append(KeyValue('Model Name', str(self.motor_static_info[mid]['model'])))
00293                 status.values.append(KeyValue('Firmware Version', str(self.motor_static_info[mid]['firmware'])))
00294                 status.values.append(KeyValue('Return Delay Time', str(self.motor_static_info[mid]['delay'])))
00295                 status.values.append(KeyValue('Minimum Voltage', str(self.motor_static_info[mid]['min_voltage'])))
00296                 status.values.append(KeyValue('Maximum Voltage', str(self.motor_static_info[mid]['max_voltage'])))
00297                 status.values.append(KeyValue('Minimum Position (CW)', str(self.motor_static_info[mid]['min_angle'])))
00298                 status.values.append(KeyValue('Maximum Position (CCW)', str(self.motor_static_info[mid]['max_angle'])))
00299                 
00300                 status.values.append(KeyValue('Goal', str(motor_state.goal)))
00301                 status.values.append(KeyValue('Position', str(motor_state.position)))
00302                 status.values.append(KeyValue('Error', str(motor_state.error)))
00303                 status.values.append(KeyValue('Velocity', str(motor_state.speed)))
00304                 status.values.append(KeyValue('Load', str(motor_state.load)))
00305                 status.values.append(KeyValue('Voltage', str(motor_state.voltage)))
00306                 status.values.append(KeyValue('Temperature', str(motor_state.temperature)))
00307                 status.values.append(KeyValue('Moving', str(motor_state.moving)))
00308                 
00309                 if motor_state.temperature >= self.error_level_temp:
00310                     status.level = DiagnosticStatus.ERROR
00311                     status.message = 'OVERHEATING'
00312                 elif motor_state.temperature >= self.warn_level_temp:
00313                     status.level = DiagnosticStatus.WARN
00314                     status.message = 'VERY HOT'
00315                 else:
00316                     status.level = DiagnosticStatus.OK
00317                     status.message = 'OK'
00318                     
00319                 diag_msg.status.append(status)
00320                 
00321             self.diagnostics_pub.publish(diag_msg)
00322             rate.sleep()
00323 
00324 if __name__ == '__main__':
00325     try:
00326         serial_proxy = SerialProxy()
00327         serial_proxy.connect()
00328         rospy.spin()
00329         serial_proxy.disconnect()
00330     except rospy.ROSInterruptException: pass
00331