From 59f16d1a4cda46ac98ed618e2979bc718d7ae029 Mon Sep 17 00:00:00 2001 From: pazeshun Date: Sat, 12 Nov 2016 01:46:13 +0900 Subject: [PATCH 1/8] Add methods about multi-turn offset and resolution divider to DynamixelIO --- .../src/dynamixel_driver/dynamixel_const.py | 11 +-- .../src/dynamixel_driver/dynamixel_io.py | 72 +++++++++++++++++++ 2 files changed, 79 insertions(+), 4 deletions(-) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py index a15a46c..183aac8 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py +++ b/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 @@ -257,7 +260,7 @@ '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, @@ -265,7 +268,7 @@ '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, @@ -273,7 +276,7 @@ '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, @@ -281,7 +284,7 @@ '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] }, } diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py index c791bdd..86137f7 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py +++ b/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 # @@ -872,6 +909,41 @@ 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) From d3578095ba70be34800759974825b7afaf23856e Mon Sep 17 00:00:00 2001 From: pazeshun Date: Sat, 12 Nov 2016 01:50:26 +0900 Subject: [PATCH 2/8] Enable DynamixelIO to handle negative position --- dynamixel_driver/src/dynamixel_driver/dynamixel_io.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py index 86137f7..36e15b7 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py @@ -563,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) @@ -649,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) @@ -766,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) ) @@ -840,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) ) @@ -950,6 +954,8 @@ def get_position(self, servo_id): 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): @@ -1007,6 +1013,8 @@ def get_feedback(self, servo_id): # extract data values from the raw data goal = response[5] + (response[6] << 8) 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 From d122b3650ed27adc5129a91c43c498950a242beb Mon Sep 17 00:00:00 2001 From: pazeshun Date: Sat, 12 Nov 2016 01:58:43 +0900 Subject: [PATCH 3/8] Enable to change multi-turn offset and resolution divider by set_servo_config.py --- dynamixel_driver/scripts/set_servo_config.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/dynamixel_driver/scripts/set_servo_config.py b/dynamixel_driver/scripts/set_servo_config.py index 04fbe03..b7fae28 100755 --- a/dynamixel_driver/scripts/set_servo_config.py +++ b/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 From 461389c72466775f6a1f9d3373961b97ef4aa4e8 Mon Sep 17 00:00:00 2001 From: pazeshun Date: Sat, 12 Nov 2016 02:27:14 +0900 Subject: [PATCH 4/8] Enable to dump multi-turn offset and resolution divider by info_dump.py --- dynamixel_driver/scripts/info_dump.py | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/dynamixel_driver/scripts/info_dump.py b/dynamixel_driver/scripts/info_dump.py index c104f00..18a75c6 100755 --- a/dynamixel_driver/scripts/info_dump.py +++ b/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' From bdf9bd886f15d3e946735cc53f092dde05131ca3 Mon Sep 17 00:00:00 2001 From: pazeshun Date: Mon, 14 Nov 2016 16:50:28 +0900 Subject: [PATCH 5/8] Fix dynamixel_serial_proxy to support multi-turn mode --- dynamixel_driver/src/dynamixel_driver/dynamixel_const.py | 2 ++ .../src/dynamixel_driver/dynamixel_serial_proxy.py | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py index 183aac8..268d63e 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py @@ -144,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 diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py index fcd5867..a31ca0e 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py +++ b/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) From d90a619bd0f0551ab25615815141325254890525 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Wed, 1 Feb 2017 18:37:07 +0900 Subject: [PATCH 6/8] Adjust goal in feedback to multi-turn mode --- dynamixel_driver/src/dynamixel_driver/dynamixel_io.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py index 36e15b7..a2a4d40 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py @@ -1012,6 +1012,8 @@ 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 From a729fb363114b68d7b72d20ec4805a57db859aab Mon Sep 17 00:00:00 2001 From: Toshinori Hirose Date: Sun, 25 Oct 2020 22:06:37 +0900 Subject: [PATCH 7/8] Add DXMIO --- dynamixel_driver/src/dynamixel_driver/dynamixel_const.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py index 268d63e..8599507 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py @@ -288,5 +288,8 @@ 'rpm_per_tick': 0.114, 'features': [DXL_MULTI_TURN_OFFSET_L, DXL_RESOLUTION_DIVIDER, DXL_CURRENT_L, DXL_TORQUE_CONTROL_MODE, DXL_GOAL_ACCELERATION] }, + 16416: {'name': 'DXMIO', + 'features': [] + }, } From d2da033fb357697ccfbe87224b13f83d3a3db577 Mon Sep 17 00:00:00 2001 From: Toshinori Hirose Date: Sun, 25 Oct 2020 22:08:00 +0900 Subject: [PATCH 8/8] Add Support DXMIO --- .../dynamixel_serial_proxy.py | 58 ++++++++++--------- 1 file changed, 32 insertions(+), 26 deletions(-) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py index a31ca0e..45d4130 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py @@ -122,34 +122,40 @@ def __fill_motor_parameters(self, motor_id, model_number): rospy.set_param('dynamixel/%s/%d/model_name' %(self.port_namespace, motor_id), DXL_MODEL_TO_PARAMS[model_number]['name']) rospy.set_param('dynamixel/%s/%d/min_angle' %(self.port_namespace, motor_id), angles['min']) rospy.set_param('dynamixel/%s/%d/max_angle' %(self.port_namespace, motor_id), angles['max']) + + if 'torque_per_volt' in DXL_MODEL_TO_PARAMS[model_number]: + torque_per_volt = DXL_MODEL_TO_PARAMS[model_number]['torque_per_volt'] + rospy.set_param('dynamixel/%s/%d/torque_per_volt' %(self.port_namespace, motor_id), torque_per_volt) + rospy.set_param('dynamixel/%s/%d/max_torque' %(self.port_namespace, motor_id), torque_per_volt * voltage) - torque_per_volt = DXL_MODEL_TO_PARAMS[model_number]['torque_per_volt'] - rospy.set_param('dynamixel/%s/%d/torque_per_volt' %(self.port_namespace, motor_id), torque_per_volt) - rospy.set_param('dynamixel/%s/%d/max_torque' %(self.port_namespace, motor_id), torque_per_volt * voltage) - - velocity_per_volt = DXL_MODEL_TO_PARAMS[model_number]['velocity_per_volt'] - rpm_per_tick = DXL_MODEL_TO_PARAMS[model_number]['rpm_per_tick'] - rospy.set_param('dynamixel/%s/%d/velocity_per_volt' %(self.port_namespace, motor_id), velocity_per_volt) - rospy.set_param('dynamixel/%s/%d/max_velocity' %(self.port_namespace, motor_id), velocity_per_volt * voltage) - rospy.set_param('dynamixel/%s/%d/radians_second_per_encoder_tick' %(self.port_namespace, motor_id), rpm_per_tick * RPM_TO_RADSEC) + if 'velocity_per_volt' in DXL_MODEL_TO_PARAMS[model_number]: + velocity_per_volt = DXL_MODEL_TO_PARAMS[model_number]['velocity_per_volt'] + rospy.set_param('dynamixel/%s/%d/velocity_per_volt' %(self.port_namespace, motor_id), velocity_per_volt) + rospy.set_param('dynamixel/%s/%d/max_velocity' %(self.port_namespace, motor_id), velocity_per_volt * voltage) + + if 'rpm_per_tick' in DXL_MODEL_TO_PARAMS[model_number]: + rpm_per_tick = DXL_MODEL_TO_PARAMS[model_number]['rpm_per_tick'] + rospy.set_param('dynamixel/%s/%d/radians_second_per_encoder_tick' %(self.port_namespace, motor_id), rpm_per_tick * RPM_TO_RADSEC) - 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) - rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_degree' %(self.port_namespace, motor_id), encoder_resolution / range_degrees) - rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_radian' %(self.port_namespace, motor_id), encoder_resolution / range_radians) - rospy.set_param('dynamixel/%s/%d/degrees_per_encoder_tick' %(self.port_namespace, motor_id), range_degrees / encoder_resolution) - rospy.set_param('dynamixel/%s/%d/radians_per_encoder_tick' %(self.port_namespace, motor_id), range_radians / encoder_resolution) + if ('encoder_resolution' in DXL_MODEL_TO_PARAMS[model_number] + and 'range_degrees' in DXL_MODEL_TO_PARAMS[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) + rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_degree' %(self.port_namespace, motor_id), encoder_resolution / range_degrees) + rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_radian' %(self.port_namespace, motor_id), encoder_resolution / range_radians) + rospy.set_param('dynamixel/%s/%d/degrees_per_encoder_tick' %(self.port_namespace, motor_id), range_degrees / encoder_resolution) + rospy.set_param('dynamixel/%s/%d/radians_per_encoder_tick' %(self.port_namespace, motor_id), range_radians / encoder_resolution) # keep some parameters around for diagnostics self.motor_static_info[motor_id] = {}