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                  readback_echo=False):
00078         self.port_name = port_name
00079         self.port_namespace = port_namespace
00080         self.baud_rate = baud_rate
00081         self.min_motor_id = min_motor_id
00082         self.max_motor_id = max_motor_id
00083         self.update_rate = update_rate
00084         self.diagnostics_rate = diagnostics_rate
00085         self.error_level_temp = error_level_temp
00086         self.warn_level_temp = warn_level_temp
00087         self.readback_echo = readback_echo
00088         
00089         self.actual_rate = update_rate
00090         self.error_counts = {'non_fatal': 0, 'checksum': 0, 'dropped': 0}
00091         self.current_state = MotorStateList()
00092         self.num_ping_retries = 5
00093         
00094         self.motor_states_pub = rospy.Publisher('motor_states/%s' % self.port_namespace, MotorStateList, queue_size=1)
00095         self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
00096 
00097     def connect(self):
00098         try:
00099             self.dxl_io = dynamixel_io.DynamixelIO(self.port_name, self.baud_rate, self.readback_echo)
00100             self.__find_motors()
00101         except dynamixel_io.SerialOpenError, e:
00102             rospy.logfatal(e.message)
00103             sys.exit(1)
00104             
00105         self.running = True
00106         if self.update_rate > 0: Thread(target=self.__update_motor_states).start()
00107         if self.diagnostics_rate > 0: Thread(target=self.__publish_diagnostic_information).start()
00108 
00109     def disconnect(self):
00110         self.running = False
00111 
00112     def __fill_motor_parameters(self, motor_id, model_number):
00113         """
00114         Stores some extra information about each motor on the parameter server.
00115         Some of these paramters are used in joint controller implementation.
00116         """
00117         angles = self.dxl_io.get_angle_limits(motor_id)
00118         voltage = self.dxl_io.get_voltage(motor_id)
00119         voltages = self.dxl_io.get_voltage_limits(motor_id)
00120         
00121         rospy.set_param('dynamixel/%s/%d/model_number' %(self.port_namespace, motor_id), model_number)
00122         rospy.set_param('dynamixel/%s/%d/model_name' %(self.port_namespace, motor_id), DXL_MODEL_TO_PARAMS[model_number]['name'])
00123         rospy.set_param('dynamixel/%s/%d/min_angle' %(self.port_namespace, motor_id), angles['min'])
00124         rospy.set_param('dynamixel/%s/%d/max_angle' %(self.port_namespace, motor_id), angles['max'])
00125         
00126         torque_per_volt = DXL_MODEL_TO_PARAMS[model_number]['torque_per_volt']
00127         rospy.set_param('dynamixel/%s/%d/torque_per_volt' %(self.port_namespace, motor_id), torque_per_volt)
00128         rospy.set_param('dynamixel/%s/%d/max_torque' %(self.port_namespace, motor_id), torque_per_volt * voltage)
00129         
00130         velocity_per_volt = DXL_MODEL_TO_PARAMS[model_number]['velocity_per_volt']
00131         rpm_per_tick = DXL_MODEL_TO_PARAMS[model_number]['rpm_per_tick']
00132         rospy.set_param('dynamixel/%s/%d/velocity_per_volt' %(self.port_namespace, motor_id), velocity_per_volt)
00133         rospy.set_param('dynamixel/%s/%d/max_velocity' %(self.port_namespace, motor_id), velocity_per_volt * voltage)
00134         rospy.set_param('dynamixel/%s/%d/radians_second_per_encoder_tick' %(self.port_namespace, motor_id), rpm_per_tick * RPM_TO_RADSEC)
00135         
00136         encoder_resolution = DXL_MODEL_TO_PARAMS[model_number]['encoder_resolution']
00137         range_degrees = DXL_MODEL_TO_PARAMS[model_number]['range_degrees']
00138         range_radians = math.radians(range_degrees)
00139         rospy.set_param('dynamixel/%s/%d/encoder_resolution' %(self.port_namespace, motor_id), encoder_resolution)
00140         rospy.set_param('dynamixel/%s/%d/range_degrees' %(self.port_namespace, motor_id), range_degrees)
00141         rospy.set_param('dynamixel/%s/%d/range_radians' %(self.port_namespace, motor_id), range_radians)
00142         rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_degree' %(self.port_namespace, motor_id), encoder_resolution / range_degrees)
00143         rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_radian' %(self.port_namespace, motor_id), encoder_resolution / range_radians)
00144         rospy.set_param('dynamixel/%s/%d/degrees_per_encoder_tick' %(self.port_namespace, motor_id), range_degrees / encoder_resolution)
00145         rospy.set_param('dynamixel/%s/%d/radians_per_encoder_tick' %(self.port_namespace, motor_id), range_radians / encoder_resolution)
00146         
00147         # keep some parameters around for diagnostics
00148         self.motor_static_info[motor_id] = {}
00149         self.motor_static_info[motor_id]['model'] = DXL_MODEL_TO_PARAMS[model_number]['name']
00150         self.motor_static_info[motor_id]['firmware'] = self.dxl_io.get_firmware_version(motor_id)
00151         self.motor_static_info[motor_id]['delay'] = self.dxl_io.get_return_delay_time(motor_id)
00152         self.motor_static_info[motor_id]['min_angle'] = angles['min']
00153         self.motor_static_info[motor_id]['max_angle'] = angles['max']
00154         self.motor_static_info[motor_id]['min_voltage'] = voltages['min']
00155         self.motor_static_info[motor_id]['max_voltage'] = voltages['max']
00156 
00157     def __find_motors(self):
00158         rospy.loginfo('%s: Pinging motor IDs %d through %d...' % (self.port_namespace, self.min_motor_id, self.max_motor_id))
00159         self.motors = []
00160         self.motor_static_info = {}
00161         
00162         for motor_id in range(self.min_motor_id, self.max_motor_id + 1):
00163             for trial in range(self.num_ping_retries):
00164                 try:
00165                     result = self.dxl_io.ping(motor_id)
00166                 except Exception as ex:
00167                     rospy.logerr('Exception thrown while pinging motor %d - %s' % (motor_id, ex))
00168                     continue
00169                     
00170                 if result:
00171                     self.motors.append(motor_id)
00172                     break
00173                     
00174         if not self.motors:
00175             rospy.logfatal('%s: No motors found.' % self.port_namespace)
00176             sys.exit(1)
00177             
00178         counts = defaultdict(int)
00179         
00180         to_delete_if_error = []
00181         for motor_id in self.motors:
00182             for trial in range(self.num_ping_retries):
00183                 try:
00184                     model_number = self.dxl_io.get_model_number(motor_id)
00185                     self.__fill_motor_parameters(motor_id, model_number)
00186                 except Exception as ex:
00187                     rospy.logerr('Exception thrown while getting attributes for motor %d - %s' % (motor_id, ex))
00188                     if trial == self.num_ping_retries - 1: to_delete_if_error.append(motor_id)
00189                     continue
00190                     
00191                 counts[model_number] += 1
00192                 break
00193                 
00194         for motor_id in to_delete_if_error:
00195             self.motors.remove(motor_id)
00196             
00197         rospy.set_param('dynamixel/%s/connected_ids' % self.port_namespace, self.motors)
00198         
00199         status_str = '%s: Found %d motors - ' % (self.port_namespace, len(self.motors))
00200         for model_number,count in counts.items():
00201             if count:
00202                 model_name = DXL_MODEL_TO_PARAMS[model_number]['name']
00203                 status_str += '%d %s [' % (count, model_name)
00204                 
00205                 for motor_id in self.motors:
00206                     if self.motor_static_info[motor_id]['model'] == model_name:
00207                         status_str += '%d, ' % motor_id
00208                         
00209                 status_str = status_str[:-2] + '], '
00210                 
00211         rospy.loginfo('%s, initialization complete.' % status_str[:-2])
00212 
00213     def __update_motor_states(self):
00214         num_events = 50
00215         rates = deque([float(self.update_rate)]*num_events, maxlen=num_events)
00216         last_time = rospy.Time.now()
00217         
00218         rate = rospy.Rate(self.update_rate)
00219         while not rospy.is_shutdown() and self.running:
00220             # get current state of all motors and publish to motor_states topic
00221             motor_states = []
00222             for motor_id in self.motors:
00223                 try:
00224                     state = self.dxl_io.get_feedback(motor_id)
00225                     if state:
00226                         motor_states.append(MotorState(**state))
00227                         if dynamixel_io.exception: raise dynamixel_io.exception
00228                 except dynamixel_io.FatalErrorCodeError, fece:
00229                     rospy.logerr(fece)
00230                 except dynamixel_io.NonfatalErrorCodeError, nfece:
00231                     self.error_counts['non_fatal'] += 1
00232                     rospy.logdebug(nfece)
00233                 except dynamixel_io.ChecksumError, cse:
00234                     self.error_counts['checksum'] += 1
00235                     rospy.logdebug(cse)
00236                 except dynamixel_io.DroppedPacketError, dpe:
00237                     self.error_counts['dropped'] += 1
00238                     rospy.logdebug(dpe.message)
00239                 except OSError, ose:
00240                     if ose.errno != errno.EAGAIN:
00241                         rospy.logfatal(errno.errorcode[ose.errno])
00242                         rospy.signal_shutdown(errno.errorcode[ose.errno])
00243                         
00244             if motor_states:
00245                 msl = MotorStateList()
00246                 msl.motor_states = motor_states
00247                 self.motor_states_pub.publish(msl)
00248                 
00249                 self.current_state = msl
00250                 
00251                 # calculate actual update rate
00252                 current_time = rospy.Time.now()
00253                 rates.append(1.0 / (current_time - last_time).to_sec())
00254                 self.actual_rate = round(sum(rates)/num_events, 2)
00255                 last_time = current_time
00256                 
00257             rate.sleep()
00258 
00259     def __publish_diagnostic_information(self):
00260         diag_msg = DiagnosticArray()
00261         
00262         rate = rospy.Rate(self.diagnostics_rate)
00263         while not rospy.is_shutdown() and self.running:
00264             diag_msg.status = []
00265             diag_msg.header.stamp = rospy.Time.now()
00266             
00267             status = DiagnosticStatus()
00268             
00269             status.name = 'Dynamixel Serial Bus (%s)' % self.port_namespace
00270             status.hardware_id = 'Dynamixel Serial Bus on port %s' % self.port_name
00271             status.values.append(KeyValue('Baud Rate', str(self.baud_rate)))
00272             status.values.append(KeyValue('Min Motor ID', str(self.min_motor_id)))
00273             status.values.append(KeyValue('Max Motor ID', str(self.max_motor_id)))
00274             status.values.append(KeyValue('Desired Update Rate', str(self.update_rate)))
00275             status.values.append(KeyValue('Actual Update Rate', str(self.actual_rate)))
00276             status.values.append(KeyValue('# Non Fatal Errors', str(self.error_counts['non_fatal'])))
00277             status.values.append(KeyValue('# Checksum Errors', str(self.error_counts['checksum'])))
00278             status.values.append(KeyValue('# Dropped Packet Errors', str(self.error_counts['dropped'])))
00279             status.level = DiagnosticStatus.OK
00280             status.message = 'OK'
00281             
00282             if self.actual_rate - self.update_rate < -5:
00283                 status.level = DiagnosticStatus.WARN
00284                 status.message = 'Actual update rate is lower than desired'
00285                 
00286             diag_msg.status.append(status)
00287             
00288             for motor_state in self.current_state.motor_states:
00289                 mid = motor_state.id
00290                 
00291                 status = DiagnosticStatus()
00292                 
00293                 status.name = 'Robotis Dynamixel Motor %d on port %s' % (mid, self.port_namespace)
00294                 status.hardware_id = 'DXL-%d@%s' % (motor_state.id, self.port_namespace)
00295                 status.values.append(KeyValue('Model Name', str(self.motor_static_info[mid]['model'])))
00296                 status.values.append(KeyValue('Firmware Version', str(self.motor_static_info[mid]['firmware'])))
00297                 status.values.append(KeyValue('Return Delay Time', str(self.motor_static_info[mid]['delay'])))
00298                 status.values.append(KeyValue('Minimum Voltage', str(self.motor_static_info[mid]['min_voltage'])))
00299                 status.values.append(KeyValue('Maximum Voltage', str(self.motor_static_info[mid]['max_voltage'])))
00300                 status.values.append(KeyValue('Minimum Position (CW)', str(self.motor_static_info[mid]['min_angle'])))
00301                 status.values.append(KeyValue('Maximum Position (CCW)', str(self.motor_static_info[mid]['max_angle'])))
00302                 
00303                 status.values.append(KeyValue('Goal', str(motor_state.goal)))
00304                 status.values.append(KeyValue('Position', str(motor_state.position)))
00305                 status.values.append(KeyValue('Error', str(motor_state.error)))
00306                 status.values.append(KeyValue('Velocity', str(motor_state.speed)))
00307                 status.values.append(KeyValue('Load', str(motor_state.load)))
00308                 status.values.append(KeyValue('Voltage', str(motor_state.voltage)))
00309                 status.values.append(KeyValue('Temperature', str(motor_state.temperature)))
00310                 status.values.append(KeyValue('Moving', str(motor_state.moving)))
00311                 
00312                 if motor_state.temperature >= self.error_level_temp:
00313                     status.level = DiagnosticStatus.ERROR
00314                     status.message = 'OVERHEATING'
00315                 elif motor_state.temperature >= self.warn_level_temp:
00316                     status.level = DiagnosticStatus.WARN
00317                     status.message = 'VERY HOT'
00318                 else:
00319                     status.level = DiagnosticStatus.OK
00320                     status.message = 'OK'
00321                     
00322                 diag_msg.status.append(status)
00323                 
00324             self.diagnostics_pub.publish(diag_msg)
00325             rate.sleep()
00326 
00327 if __name__ == '__main__':
00328     try:
00329         serial_proxy = SerialProxy()
00330         serial_proxy.connect()
00331         rospy.spin()
00332         serial_proxy.disconnect()
00333     except rospy.ROSInterruptException: pass
00334