From 263ead4178477b233f4ab6991ade4782b149bb4a Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 10 Sep 2022 18:38:51 +0900 Subject: [PATCH] add resolution_divider --- .../dynamixel_controllers/calib_required_joint_controller.py | 2 +- .../src/dynamixel_controllers/joint_position_controller.py | 3 +++ dynamixel_driver/src/dynamixel_driver/dynamixel_io.py | 3 +++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/dynamixel_controllers/src/dynamixel_controllers/calib_required_joint_controller.py b/dynamixel_controllers/src/dynamixel_controllers/calib_required_joint_controller.py index d27f5bd..7edbb1c 100644 --- a/dynamixel_controllers/src/dynamixel_controllers/calib_required_joint_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/calib_required_joint_controller.py @@ -73,7 +73,6 @@ def __calib(self): rate = rospy.Rate(50) while not rospy.is_shutdown(): try: - init_pos = self.motor_states_for_init['position'] if abs(self.motor_states_for_init['load']) > self.detect_limit_load: break except KeyError: @@ -92,6 +91,7 @@ def __calib(self): if self.torque_limit is not None: self.set_torque_limit(self.torque_limit) motor_states_sub_for_init.unregister() + init_pos = self.motor_states_for_init['position'] # Remember initial joint position diff = init_pos - self.initial_position_raw diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py index 437abb8..3ac8d0a 100755 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py @@ -56,6 +56,9 @@ def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id') + self.resolution_divider = rospy.get_param(self.controller_namespace + '/motor/resolution_divider', 1) + self.dxl_io.set_resolution_divider(self.motor_id, self.resolution_divider) + self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init') self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min') self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py index 797bb89..6f73c59 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py @@ -68,6 +68,7 @@ def __init__(self, port, baudrate, readback_echo=False): self.port_name = port self.readback_echo = readback_echo self.ignored_errors = {} + self.resolution_divider = 1 except SerialOpenError: raise SerialOpenError(port, baudrate) @@ -416,6 +417,7 @@ def set_resolution_divider(self, servo_id, divider): 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) + self.resolution_divider = divider return response else: raise UnsupportedFeatureError(model, DXL_RESOLUTION_DIVIDER) @@ -1018,6 +1020,7 @@ def get_feedback(self, servo_id): position = response[11] + (response[12] << 8) if position & 0x8000: position += -0x10000 + position = position * self.resolution_divider error = position - goal speed = response[13] + ( response[14] << 8) if speed > 1023: speed = 1023 - speed