lib_robotis.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #
00003 # Copyright (c) 2009, Georgia Tech Research Corporation
00004 # All rights reserved.
00005 # 
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Georgia Tech Research Corporation nor the
00014 #       names of its contributors may be used to endorse or promote products
00015 #       derived from this software without specific prior written permission.
00016 # 
00017 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00018 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00021 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00022 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00023 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00024 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00025 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00026 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 #
00028 
00029 ## Controlling Robotis Dynamixel RX-28 & RX-64 servos from python
00030 ## using the USB2Dynamixel adaptor.
00031 
00032 ## Authors: Travis Deyle, Advait Jain & Marc Killpack (Healthcare Robotics Lab, Georgia Tech.)
00033 
00034 import serial
00035 import time
00036 import thread
00037 import sys, optparse
00038 import math
00039 
00040 class USB2Dynamixel_Device():
00041     ''' Class that manages serial port contention between servos on same bus
00042     '''
00043     def __init__( self, dev_name = '/dev/ttyUSB0', baudrate = 57600 ):
00044         try:
00045             self.dev_name = string.atoi( dev_name ) # stores the serial port as 0-based integer for Windows
00046         except:
00047             self.dev_name = dev_name # stores it as a /dev-mapped string for Linux / Mac
00048 
00049         self.mutex = thread.allocate_lock()
00050         self.servo_dev = None
00051 
00052         self.acq_mutex()
00053         self._open_serial( baudrate )
00054         self.rel_mutex()
00055 
00056     def acq_mutex(self):
00057         self.mutex.acquire()
00058 
00059     def rel_mutex(self):
00060         self.mutex.release()
00061 
00062     def send_serial(self, msg):
00063         # It is up to the caller to acquire / release mutex
00064         self.servo_dev.write( msg )
00065 
00066     def read_serial(self, nBytes=1):
00067         # It is up to the caller to acquire / release mutex
00068         rep = self.servo_dev.read( nBytes )
00069         return rep
00070 
00071     def _open_serial(self, baudrate):
00072         try:
00073             self.servo_dev = serial.Serial(self.dev_name, baudrate, timeout=1.0)
00074             # Closing the device first seems to prevent "Access Denied" errors on WinXP
00075             # (Conversations with Brian Wu @ MIT on 6/23/2010)
00076             self.servo_dev.close()  
00077             self.servo_dev.setParity('N')
00078             self.servo_dev.setStopbits(1)
00079             self.servo_dev.open()
00080 
00081             self.servo_dev.flushOutput()
00082             self.servo_dev.flushInput()
00083 
00084         except (serial.serialutil.SerialException), e:
00085             raise RuntimeError('lib_robotis: Serial port not found!\n')
00086         if(self.servo_dev == None):
00087             raise RuntimeError('lib_robotis: Serial port not found!\n')
00088 
00089 
00090 
00091 
00092 
00093 class Robotis_Servo():
00094     ''' Class to use a robotis RX-28 or RX-64 servo.
00095     '''
00096     def __init__(self, USB2Dynamixel, servo_id, series = None ):
00097         ''' USB2Dynamixel - USB2Dynamixel_Device object to handle serial port.
00098                             Handles threadsafe operation for multiple servos
00099             servo_id - servo ids connected to USB2Dynamixel 1,2,3,4 ... (1 to 253)
00100                        [0 is broadcast if memory serves]
00101             series - Just a convenience for defining "good" defaults on MX series.
00102                      When set to "MX" it uses these values, otherwise it uses values
00103                      better for AX / RX series.  Any of the defaults can be overloaded
00104                      on a servo-by-servo bases in servo_config.py
00105         '''
00106 
00107         # To change the defaults, load some or all changes into servo_config.py
00108         if series == 'MX':
00109             defaults = {
00110                 'home_encoder': 0x7FF,
00111                 'max_encoder': 0xFFF,
00112                 'rad_per_enc': 2*math.pi / 0xFFF, 
00113                 'max_ang': math.pi,
00114                 'min_ang': -math.pi,
00115                 'flipped': False,
00116                 'max_speed': math.radians(180)
00117                 }
00118         else: # Common settings for RX-series.  Can overload in servo_config.py
00119             defaults = {
00120                 'home_encoder': 0x200,
00121                 'max_encoder': 0x3FF,  # Assumes 0 is min.
00122                 'rad_per_enc': math.radians(300.0) / 1024.0, 
00123                 'max_ang': math.radians(148),
00124                 'min_ang': math.radians(-148),
00125                 'flipped': False,
00126                 'max_speed': math.radians(180)
00127                 }
00128 
00129         # Error Checking
00130         if USB2Dynamixel == None:
00131             raise RuntimeError('lib_robotis: Robotis Servo requires USB2Dynamixel!\n')
00132         else:
00133             self.dyn = USB2Dynamixel
00134 
00135         # ID exists on bus?
00136         self.servo_id = servo_id
00137         try:
00138             self.read_address(3)
00139         except:
00140             raise RuntimeError('lib_robotis: Error encountered.  Could not find ID (%d) on bus (%s), or USB2Dynamixel 3-way switch in wrong position.\n' %
00141                                ( servo_id, self.dyn.dev_name ))
00142 
00143         # Set Return Delay time - Used to determine when next status can be requested
00144         data = self.read_address( 0x05, 1)
00145         self.return_delay = data[0] * 2e-6
00146 
00147         # Set various parameters.  Load from servo_config.
00148         self.settings = {}
00149         try:
00150             import servo_config as sc
00151             if sc.servo_param.has_key( self.servo_id ):
00152                 self.settings = sc.servo_param[ self.servo_id ]
00153             else:
00154                 print 'Warning: servo_id ', self.servo_id, ' not found in servo_config.py.  Using defaults.'
00155         except:
00156             print 'Warning: servo_config.py not found.  Using defaults.'
00157 
00158         # Set to default any parameter not specified in servo_config
00159         for key in defaults.keys():
00160             if self.settings.has_key( key ):
00161                 pass
00162             else:
00163                 self.settings[ key ] = defaults[ key ]
00164 
00165     def init_cont_turn(self):
00166         '''sets CCW angle limit to zero and allows continuous turning (good for wheels).
00167         After calling this method, simply use 'set_angvel' to command rotation.  This 
00168         rotation is proportional to torque according to Robotis documentation.
00169         '''
00170         self.write_address(0x08, [0,0])
00171 
00172     def kill_cont_turn(self):
00173         '''resets CCW angle limits to allow commands through 'move_angle' again
00174         '''
00175         self.write_address(0x08, [255, 3])
00176 
00177     def is_moving(self):
00178         ''' returns True if servo is moving.
00179         '''
00180         data = self.read_address( 0x2e, 1 )
00181         return data[0] != 0
00182 
00183     def read_voltage(self):
00184         ''' returns voltage (Volts)
00185         '''
00186         data = self.read_address( 0x2a, 1 )
00187         return data[0] / 10.
00188 
00189     def read_temperature(self):
00190         ''' returns the temperature (Celcius)
00191         '''
00192         data = self.read_address( 0x2b, 1 )
00193         return data[0]
00194 
00195     def read_load(self):
00196         ''' number proportional to the torque applied by the servo.
00197             sign etc. might vary with how the servo is mounted.
00198         '''
00199         data = self.read_address( 0x28, 2 )
00200         load = data[0] + (data[1] >> 6) * 256
00201         if data[1] >> 2 & 1 == 0:
00202             return -1.0 * load
00203         else:
00204             return 1.0 * load
00205 
00206     def read_encoder(self):
00207         ''' returns position in encoder ticks
00208         '''
00209         data = self.read_address( 0x24, 2 )
00210         enc_val = data[0] + data[1] * 256
00211         return enc_val
00212 
00213     def read_angle(self):
00214         ''' returns the current servo angle (radians)
00215         '''
00216         ang = (self.read_encoder() - self.settings['home_encoder']) * self.settings['rad_per_enc']
00217         if self.settings['flipped']:
00218             ang = ang * -1.0
00219         return ang
00220 
00221     def move_angle(self, ang, angvel=None, blocking=True):
00222         ''' move to angle (radians)
00223         '''
00224         if angvel == None:
00225             angvel = self.settings['max_speed']
00226 
00227         if angvel > self.settings['max_speed']:
00228             print 'lib_robotis.move_angle: angvel too high - %.2f deg/s' % (math.degrees(angvel))
00229             print 'lib_robotis.ignoring move command.'
00230             return
00231 
00232         if ang > self.settings['max_ang'] or ang < self.settings['min_ang']:
00233             print 'lib_robotis.move_angle: angle out of range- ', math.degrees(ang)
00234             print 'lib_robotis.ignoring move command.'
00235             return
00236         
00237         self.set_angvel(angvel)
00238 
00239         if self.settings['flipped']:
00240             ang = ang * -1.0
00241         enc_tics = int(round( ang / self.settings['rad_per_enc'] ))
00242         enc_tics += self.settings['home_encoder']
00243         self.move_to_encoder( enc_tics )
00244 
00245         if blocking == True:
00246             while(self.is_moving()):
00247                 continue
00248 
00249     def move_to_encoder(self, n):
00250         ''' move to encoder position n
00251         '''
00252         # In some border cases, we can end up above/below the encoder limits.
00253         #   eg. int(round(math.radians( 180 ) / ( math.radians(360) / 0xFFF ))) + 0x7FF => -1
00254         n = min( max( n, 0 ), self.settings['max_encoder'] ) 
00255         hi,lo = n / 256, n % 256
00256         return self.write_address( 0x1e, [lo,hi] )
00257 
00258     def enable_torque(self):
00259         return self.write_address(0x18, [1])
00260 
00261     def disable_torque(self):
00262         return self.write_address(0x18, [0])
00263 
00264     def set_angvel(self, angvel):
00265         ''' angvel - in rad/sec
00266         '''     
00267         rpm = angvel / (2 * math.pi) * 60.0
00268         angvel_enc = int(round( rpm / 0.111 ))
00269         if angvel_enc<0:
00270             hi,lo = abs(angvel_enc) / 256 + 4, abs(angvel_enc) % 256
00271         else:
00272             hi,lo = angvel_enc / 256, angvel_enc % 256
00273         
00274         return self.write_address( 0x20, [lo,hi] )
00275 
00276     def write_id(self, id):
00277         ''' changes the servo id
00278         '''
00279         return self.write_address( 0x03, [id] )
00280 
00281     def __calc_checksum(self, msg):
00282         chksum = 0
00283         for m in msg:
00284             chksum += m
00285         chksum = ( ~chksum ) % 256
00286         return chksum
00287 
00288     def read_address(self, address, nBytes=1):
00289         ''' reads nBytes from address on the servo.
00290             returns [n1,n2 ...] (list of parameters)
00291         '''
00292         msg = [ 0x02, address, nBytes ]
00293         return self.send_instruction( msg, self.servo_id )
00294 
00295     def write_address(self, address, data):
00296         ''' writes data at the address.
00297             data = [n1,n2 ...] list of numbers.
00298             return [n1,n2 ...] (list of return parameters)
00299         '''
00300         msg = [ 0x03, address ] + data
00301         return self.send_instruction( msg, self.servo_id )
00302 
00303     def send_instruction(self, instruction, id):
00304         msg = [ id, len(instruction) + 1 ] + instruction # instruction includes the command (1 byte + parameters. length = parameters+2)
00305         chksum = self.__calc_checksum( msg )
00306         msg = [ 0xff, 0xff ] + msg + [chksum]
00307         
00308         self.dyn.acq_mutex()
00309         try:
00310             self.send_serial( msg )
00311             data, err = self.receive_reply()
00312         except:
00313             self.dyn.rel_mutex()
00314             raise
00315         self.dyn.rel_mutex()
00316         
00317         if err != 0:
00318             self.process_err( err )
00319 
00320         return data
00321 
00322     def process_err( self, err ):
00323         raise RuntimeError('lib_robotis: An error occurred: %d\n' % err)
00324 
00325     def receive_reply(self):
00326         start = self.dyn.read_serial( 2 )
00327         if start != '\xff\xff':
00328             raise RuntimeError('lib_robotis: Failed to receive start bytes\n')
00329         servo_id = self.dyn.read_serial( 1 )
00330         if ord(servo_id) != self.servo_id:
00331             raise RuntimeError('lib_robotis: Incorrect servo ID received: %d\n' % ord(servo_id))
00332         data_len = self.dyn.read_serial( 1 )
00333         err = self.dyn.read_serial( 1 )
00334         data = self.dyn.read_serial( ord(data_len) - 2 )
00335         checksum = self.dyn.read_serial( 1 ) # I'm not going to check...
00336         return [ord(v) for v in data], ord(err)
00337         
00338 
00339     def send_serial(self, msg):
00340         """ sends the command to the servo
00341         """
00342         out = ''
00343         for m in msg:
00344             out += chr(m)
00345         self.dyn.send_serial( out )
00346 
00347 
00348 
00349 
00350 
00351 def find_servos(dyn):
00352     ''' Finds all servo IDs on the USB2Dynamixel '''
00353     print 'Scanning for Servos.'
00354     servos = []
00355     dyn.servo_dev.setTimeout( 0.03 ) # To make the scan faster
00356     for i in xrange(254):
00357         try:
00358             s = Robotis_Servo( dyn, i )
00359             print '\n FOUND A SERVO @ ID %d\n' % i
00360             servos.append( i )
00361         except:
00362             pass
00363     dyn.servo_dev.setTimeout( 1.0 ) # Restore to original
00364     return servos
00365 
00366 
00367 def recover_servo(dyn):
00368     ''' Recovers a bricked servo by booting into diagnostic bootloader and resetting '''
00369     raw_input('Make sure only one servo connected to USB2Dynamixel Device [ENTER]')
00370     raw_input('Disconnect power from the servo, but leave USB2Dynamixel connected to USB. [ENTER]')
00371 
00372     dyn.servo_dev.setBaudrate( 57600 )
00373     
00374     print 'Get Ready.  Be ready to reconnect servo power when I say \'GO!\''
00375     print 'After a second, the red LED should become permanently lit.'
00376     print 'After that happens, Ctrl + C to kill this program.'
00377     print
00378     print 'Then, you will need to use a serial terminal to issue additional commands.',
00379     print 'Here is an example using screen as serial terminal:'
00380     print
00381     print 'Command Line:  screen /dev/robot/servo_left 57600'
00382     print 'Type: \'h\''
00383     print 'Response: Command : L(oad),G(o),S(ystem),A(pplication),R(eset),D(ump),C(lear)'
00384     print 'Type: \'C\''
00385     print 'Response:  * Clear EEPROM '
00386     print 'Type: \'A\''
00387     print 'Response: * Application Mode'
00388     print 'Type: \'G\''
00389     print 'Response:  * Go'
00390     print
00391     print 'Should now be able to reconnect to the servo using ID 1'
00392     print
00393     print
00394     raw_input('Ready to reconnect power? [ENTER]')
00395     print 'GO!'
00396 
00397     while True:
00398         s.write('#')
00399         time.sleep(0.0001)
00400 
00401 
00402 if __name__ == '__main__':
00403     p = optparse.OptionParser()
00404     p.add_option('-d', action='store', type='string', dest='dev_name',
00405                  help='Required: Device string for USB2Dynamixel. [i.e. /dev/ttyUSB0 for Linux, \'0\' (for COM1) on Windows]')
00406     p.add_option('--scan', action='store_true', dest='scan', default=False,
00407                  help='Scan the device for servo IDs attached.')
00408     p.add_option('--recover', action='store_true', dest='recover', default=False,
00409                  help='Recover from a bricked servo (restores to factory defaults).')
00410     p.add_option('--ang', action='store', type='float', dest='ang',
00411                  help='Angle to move the servo to (degrees).')
00412     p.add_option('--ang_vel', action='store', type='float', dest='ang_vel',
00413                  help='angular velocity. (degrees/sec) [default = 50]', default=50)
00414     p.add_option('--id', action='store', type='int', dest='id',
00415                  help='id of servo to connect to, [default = 1]', default=1)
00416     p.add_option('--baud', action='store', type='int', dest='baud',
00417                  help='baudrate for USB2Dynamixel connection [default = 57600]', default=57600)
00418 
00419     opt, args = p.parse_args()
00420 
00421     if opt.dev_name == None:
00422         p.print_help()
00423         sys.exit(0)
00424 
00425     dyn = USB2Dynamixel_Device(opt.dev_name, opt.baud)
00426 
00427     if opt.scan:
00428         find_servos( dyn )
00429 
00430     if opt.recover:
00431         recover_servo( dyn )
00432 
00433     if opt.ang != None:
00434         servo = Robotis_Servo( dyn, opt.id )
00435         servo.move_angle( math.radians(opt.ang), math.radians(opt.ang_vel) )
00436     


robotis
Author(s): Travis Deyle, Advait Jain, Marc Killpack, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:37:53