[dynamixel_driver] Support for Multi-turn Mode

dynamixel_driver/scripts/info_dump.py

@@ -58,17 +58,34 @@ def print_data(values):
         print '''\
     Motor %(id)d is connected:
         Freespin: True
+        Multi-Turn: False
         Model ------------------- %(model)s
         Current Speed ----------- %(speed)d
         Current Temperature ----- %(temperature)d%(degree_symbol)sC
         Current Voltage --------- %(voltage).1fv
         Current Load ------------ %(load)d
         Moving ------------------ %(moving)s
+''' %values
+    elif values['multi_turn']:
+        print '''\
+    Motor %(id)d is connected:
+        Freespin: False
+        Multi-Turn: True
+        Model ------------------- %(model)s
+        Multi-Turn Offset ------- %(multi_turn_offset)d
+        Resolution Divider ------ %(resolution_divider)d
+        Current Position -------- %(position)d
+        Current Speed ----------- %(speed)d
+        Current Temperature ----- %(temperature)d%(degree_symbol)sC
+        Current Voltage --------- %(voltage).1fv
+        Current Load ------------ %(load)d
+        Moving ------------------ %(moving)s
 ''' %values
     else:
         print '''\
     Motor %(id)d is connected:
         Freespin: False
+        Multi-Turn: False
         Model ------------------- %(model)s
         Min Angle --------------- %(min)d
         Max Angle --------------- %(max)d
@@ -127,8 +144,17 @@ def print_data(values):
                 print 'done'
                 if angles['max'] == 0 and angles['min'] == 0:
                     values['freespin'] = True
+                    values['multi_turn'] = False
                 else:
                     values['freespin'] = False
+                    if (DXL_MULTI_TURN_OFFSET_L in DXL_MODEL_TO_PARAMS[model]['features']) \
+                            and angles['max'] == 4095 and angles['min'] == 4095:
+                        values['multi_turn'] = True
+                        values['multi_turn_offset'] = dxl_io.get_multi_turn_offset(motor_id)
+                        values['resolution_divider'] = dxl_io.get_resolution_divider(motor_id)
+                    else:
+                        values['multi_turn'] = False
+
                 print_data(values)
             else:
                 print 'error'

dynamixel_driver/scripts/set_servo_config.py

@@ -72,6 +72,10 @@
                       help='set servo motor minimum voltage limit')
     parser.add_option('--max-voltage-limit', type='int', metavar='MAX_VOLTAGE', dest='max_voltage_limit',
                       help='set servo motor maximum voltage limit')
+    parser.add_option('--multi-turn-offset', type='int', metavar='OFFSET', dest='multi_turn_offset',
+                      help='set servo motor multi-turn offset')
+    parser.add_option('--resolution-divider', type='int', metavar='DIVIDER', dest='resolution_divider',
+                      help='set servo motor resolution divider')
 
     (options, args) = parser.parse_args(sys.argv)
     print options
@@ -139,6 +143,22 @@
                     print 'Setting maximum voltage limit to %d' % options.max_voltage_limit
                     dxl_io.set_voltage_limit_max(motor_id, options.max_voltage_limit)
 
+                # check if multi-turn offset needs to be changed
+                if options.multi_turn_offset is not None:
+                    if options.multi_turn_offset < -24576 or options.multi_turn_offset > 24576:
+                        print 'Requested multi-turn offset is out of valid range (-24576 - 24576)'
+                    else:
+                        print 'Setting multi-turn offset to %d' % options.multi_turn_offset
+                        dxl_io.set_multi_turn_offset(motor_id, options.multi_turn_offset)
+
+                # check if resolution divider needs to be changed
+                if options.resolution_divider is not None:
+                    if options.resolution_divider < 1 or options.resolution_divider > 4:
+                        print 'Requested resolution divider is out of valid range (1 - 4)'
+                    else:
+                        print 'Setting resolution divider to %d' % options.resolution_divider
+                        dxl_io.set_resolution_divider(motor_id, options.resolution_divider)
+
                 print 'done'
             else:
                 print 'Unable to connect to Dynamixel motor with ID %d' % motor_id

dynamixel_driver/src/dynamixel_driver/dynamixel_const.py

@@ -69,6 +69,9 @@
 DXL_DOWN_CALIBRATION_H = 21
 DXL_UP_CALIBRATION_L = 22
 DXL_UP_CALIBRATION_H = 23
+DXL_MULTI_TURN_OFFSET_L = 20
+DXL_MULTI_TURN_OFFSET_H = 21
+DXL_RESOLUTION_DIVIDER = 22
 DXL_TORQUE_ENABLE = 24
 DXL_LED = 25
 DXL_CW_COMPLIANCE_MARGIN = 26
@@ -141,6 +144,8 @@
 DXL_MIN_COMPLIANCE_SLOPE = 1
 DXL_MAX_COMPLIANCE_SLOPE = 254
 
+DXL_MULTI_TURN_RANGE_TICK = 57344           # angle range in encoder units in multi-turn mode
+
 # These are guesses as Dynamixel documentation doesn't have any info about it
 DXL_MIN_PUNCH = 0
 DXL_MAX_PUNCH = 255
@@ -257,31 +262,31 @@
            'torque_per_volt':    0.2 / 12.0,                        #  torque not specified!
            'velocity_per_volt':  (470 * RPM_TO_RADSEC) / 12.0,      #  470 RPM @ 12.0V
            'rpm_per_tick':       0.114,
-           'features':           [DXL_GOAL_ACCELERATION]
+           'features':           [DXL_MULTI_TURN_OFFSET_L, DXL_RESOLUTION_DIVIDER, DXL_GOAL_ACCELERATION]
          },
      29: { 'name':               'MX-28',
            'encoder_resolution': 4096,
            'range_degrees':      360.0,
            'torque_per_volt':    2.5 / 12.0,                       #  2.5 NM @ 12V
            'velocity_per_volt':  (55 * RPM_TO_RADSEC) / 12.0,      #  54 RPM @ 12.0V
            'rpm_per_tick':       0.114,
-           'features':           [DXL_GOAL_ACCELERATION]
+           'features':           [DXL_MULTI_TURN_OFFSET_L, DXL_RESOLUTION_DIVIDER, DXL_GOAL_ACCELERATION]
          },
     310: { 'name':               'MX-64',
            'encoder_resolution': 4096,
            'range_degrees':      360.0,
            'torque_per_volt':    6.0 / 12.0,                       #  6 NM @ 12V
            'velocity_per_volt':  (63 * RPM_TO_RADSEC) / 12.0,      #  63 RPM @ 12.0V
            'rpm_per_tick':       0.114,
-           'features':           [DXL_CURRENT_L, DXL_TORQUE_CONTROL_MODE, DXL_GOAL_ACCELERATION]
+           'features':           [DXL_MULTI_TURN_OFFSET_L, DXL_RESOLUTION_DIVIDER, DXL_CURRENT_L, DXL_TORQUE_CONTROL_MODE, DXL_GOAL_ACCELERATION]
          },
     320: { 'name':               'MX-106',
            'encoder_resolution': 4096,
            'range_degrees':      360.0,
            'torque_per_volt':    8.4 / 12.0,                       #  8.4 NM @ 12V
            'velocity_per_volt':  (45 * RPM_TO_RADSEC) / 12.0,      #  45 RPM @ 12.0V
            'rpm_per_tick':       0.114,
-           'features':           [DXL_CURRENT_L, DXL_TORQUE_CONTROL_MODE, DXL_GOAL_ACCELERATION]
+           'features':           [DXL_MULTI_TURN_OFFSET_L, DXL_RESOLUTION_DIVIDER, DXL_CURRENT_L, DXL_TORQUE_CONTROL_MODE, DXL_GOAL_ACCELERATION]
          },
 }
 

dynamixel_driver/src/dynamixel_driver/dynamixel_io.py

@@ -382,6 +382,43 @@ def set_voltage_limits(self, servo_id, min_voltage, max_voltage):
             self.exception_on_error(response[4], servo_id, 'setting min and max voltage levels to %d and %d' %(min_voltage, max_voltage))
         return response
 
+    def set_multi_turn_offset(self, servo_id, offset):
+        """
+        Set the multi-turn offset for MX motors
+        """
+
+        model = self.get_model_number(servo_id)
+        if not model in DXL_MODEL_TO_PARAMS:
+            raise UnsupportedFeatureError(model, DXL_MULTI_TURN_OFFSET_L)
+
+        if DXL_MULTI_TURN_OFFSET_L in DXL_MODEL_TO_PARAMS[model]['features']:
+            offset &= 0xffff
+            loVal = int(offset % 256)
+            hiVal = int(offset >> 8)
+            response = self.write(servo_id, DXL_MULTI_TURN_OFFSET_L, (loVal, hiVal))
+            if response:
+                self.exception_on_error(response[4], servo_id, 'setting multiturn offset to %d' % offset)
+            return response
+        else:
+            raise UnsupportedFeatureError(model, DXL_MULTI_TURN_OFFSET_L)
+
+    def set_resolution_divider(self, servo_id, divider):
+        """
+        Set resolution divider for MX motors. Valid range: 1 to 4
+        """
+
+        model = self.get_model_number(servo_id)
+        if not model in DXL_MODEL_TO_PARAMS:
+            raise UnsupportedFeatureError(model, DXL_RESOLUTION_DIVIDER)
+
+        if DXL_RESOLUTION_DIVIDER in DXL_MODEL_TO_PARAMS[model]['features']:
+            response = self.write(servo_id, DXL_RESOLUTION_DIVIDER, [divider])
+            if response:
+                self.exception_on_error(response[4], servo_id, 'setting resolution divider to %d' % divider)
+            return response
+        else:
+            raise UnsupportedFeatureError(model, DXL_RESOLUTION_DIVIDER)
+
 
     ###############################################################
     # These functions can send a single command to a single servo #
@@ -526,8 +563,9 @@ def set_acceleration(self, servo_id, acceleration):
     def set_position(self, servo_id, position):
         """
         Set the servo with servo_id to the specified goal position.
-        Position value must be positive.
+        Position can be negative only if the dynamixel is in "Multi-Turn" mode.
         """
+        position &= 0xffff
         loVal = int(position % 256)
         hiVal = int(position >> 8)
 
@@ -612,6 +650,7 @@ def set_position_and_speed(self, servo_id, position, speed):
             hiSpeedVal = int((1023 - speed) >> 8)
 
         # split position into 2 bytes
+        position &= 0xffff
         loPositionVal = int(position % 256)
         hiPositionVal = int(position >> 8)
 
@@ -729,6 +768,7 @@ def set_multi_position(self, valueTuples):
             sid = vals[0]
             position = vals[1]
             # split position into 2 bytes
+            position &= 0xffff
             loVal = int(position % 256)
             hiVal = int(position >> 8)
             writeableVals.append( (sid, loVal, hiVal) )
@@ -803,6 +843,7 @@ def set_multi_position_and_speed(self, valueTuples):
                 hiSpeedVal = int((1023 - speed) >> 8)
 
             # split position into 2 bytes
+            position &= 0xffff
             loPositionVal = int(position % 256)
             hiPositionVal = int(position >> 8)
             writeableVals.append( (sid, loPositionVal, hiPositionVal, loSpeedVal, hiSpeedVal) )
@@ -872,12 +913,49 @@ def get_voltage_limits(self, servo_id):
         # return the data in a dictionary
         return {'min':min_voltage, 'max':max_voltage}
 
+    def get_multi_turn_offset(self, servo_id):
+        """ Reads the servo's multi-turn offset(if supported by model) """
+        model = self.get_model_number(servo_id)
+        if not model in DXL_MODEL_TO_PARAMS:
+            raise UnsupportedFeatureError(model, DXL_MULTI_TURN_OFFSET_L)
+
+        if DXL_MULTI_TURN_OFFSET_L in DXL_MODEL_TO_PARAMS[model]['features']:
+            response = self.read(servo_id, DXL_MULTI_TURN_OFFSET_L, 2)
+            if response:
+                self.exception_on_error(response[4], servo_id, 'fetching multi-turn offset')
+            offset = response[5] + (response[6] << 8)
+            if offset & 0x8000:
+                offset += -0x10000
+            return offset
+
+        else:
+            raise UnsupportedFeatureError(model, DXL_MULTI_TURN_OFFSET_L)
+
+
+    def get_resolution_divider(self, servo_id):
+        """ Reads the servo's resolution divider(if supported by model) """
+        model = self.get_model_number(servo_id)
+        if not model in DXL_MODEL_TO_PARAMS:
+            raise UnsupportedFeatureError(model, DXL_RESOLUTION_DIVIDER)
+
+        if DXL_RESOLUTION_DIVIDER in DXL_MODEL_TO_PARAMS[model]['features']:
+            response = self.read(servo_id, DXL_RESOLUTION_DIVIDER, 1)
+            if response:
+                self.exception_on_error(response[4], servo_id, 'fetching resolution divider')
+            return response[5]
+
+        else:
+            raise UnsupportedFeatureError(model, DXL_RESOLUTION_DIVIDER)
+
+
     def get_position(self, servo_id):
         """ Reads the servo's position value from its registers. """
         response = self.read(servo_id, DXL_PRESENT_POSITION_L, 2)
         if response:
             self.exception_on_error(response[4], servo_id, 'fetching present position')
         position = response[5] + (response[6] << 8)
+        if position & 0x8000:
+            position += -0x10000
         return position
 
     def get_speed(self, servo_id):
@@ -934,7 +1012,11 @@ def get_feedback(self, servo_id):
         if len(response) == 24:
             # extract data values from the raw data
             goal = response[5] + (response[6] << 8)
+            if goal & 0x8000:
+                goal += -0x10000
             position = response[11] + (response[12] << 8)
+            if position & 0x8000:
+                position += -0x10000
             error = position - goal
             speed = response[13] + ( response[14] << 8)
             if speed > 1023: speed = 1023 - speed

dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py

@@ -136,6 +136,13 @@ def __fill_motor_parameters(self, motor_id, model_number):
         encoder_resolution = DXL_MODEL_TO_PARAMS[model_number]['encoder_resolution']
         range_degrees = DXL_MODEL_TO_PARAMS[model_number]['range_degrees']
         range_radians = math.radians(range_degrees)
+        if angles['max'] == 4095 and angles['min'] == 4095:  # In multi-turn mode if the servo is MX motor
+            try:
+                resolution_divider = self.dxl_io.get_resolution_divider(motor_id)
+                range_degrees = (range_degrees / encoder_resolution) * resolution_divider * DXL_MULTI_TURN_RANGE_TICK
+                range_radians = math.radians(range_degrees)
+                encoder_resolution = DXL_MULTI_TURN_RANGE_TICK
+            except dynamixel_io.UnsupportedFeatureError: pass  # The servo is not MX motor
         rospy.set_param('dynamixel/%s/%d/encoder_resolution' %(self.port_namespace, motor_id), encoder_resolution)
         rospy.set_param('dynamixel/%s/%d/range_degrees' %(self.port_namespace, motor_id), range_degrees)
         rospy.set_param('dynamixel/%s/%d/range_radians' %(self.port_namespace, motor_id), range_radians)