From 9f67c6ac68483863758916358384a6101aa483fe Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 6 Jun 2017 11:30:50 +0900 Subject: [PATCH] Enable to ignore fatal errors --- .../dynamixel_controllers/joint_controller.py | 4 +++ .../joint_position_controller.py | 6 ++++ .../joint_position_controller_dual_motor.py | 7 +++++ .../joint_torque_controller.py | 6 ++++ .../joint_torque_controller_dual_motor.py | 8 +++++ .../src/dynamixel_driver/dynamixel_io.py | 31 ++++++++++++++----- 6 files changed, 55 insertions(+), 7 deletions(-) diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py b/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py index 7ecfb70..9746adf 100644 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py @@ -70,6 +70,7 @@ def __init__(self, dxl_io, controller_namespace, port_namespace): self.compliance_margin = rospy.get_param(self.controller_namespace + '/joint_compliance_margin', None) self.compliance_punch = rospy.get_param(self.controller_namespace + '/joint_compliance_punch', None) self.torque_limit = rospy.get_param(self.controller_namespace + '/joint_torque_limit', None) + self.ignored_errors = rospy.get_param(self.controller_namespace + '/ignored_errors', None) self.__ensure_limits() @@ -136,6 +137,9 @@ def set_compliance_punch(self, punch): def set_torque_limit(self, max_torque): raise NotImplementedError + def set_ignored_errors(self, errs): + raise NotImplementedError + def process_set_speed(self, req): self.set_speed(req.speed) return [] # success diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py index bb3a637..3c26812 100755 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py @@ -101,6 +101,9 @@ def initialize(self): rospy.loginfo("Setting acceleration of %d to %d" % (self.motor_id, self.acceleration)) self.dxl_io.set_acceleration(self.motor_id, self.acceleration) + if self.ignored_errors is not None: + self.set_ignored_errors(self.ignored_errors) + self.joint_max_speed = rospy.get_param(self.controller_namespace + '/joint_max_speed', self.MAX_VELOCITY) if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY @@ -159,6 +162,9 @@ def set_torque_limit(self, max_torque): mcv = (self.motor_id, raw_torque_val) self.dxl_io.set_multi_torque_limit([mcv]) + def set_ignored_errors(self, errs): + self.dxl_io.set_ignored_errors(self.motor_id, errs) + def set_acceleration_raw(self, acc): if acc < 0: acc = 0 elif acc > 254: acc = 254 diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller_dual_motor.py b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller_dual_motor.py index f58aad9..703d7e0 100644 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller_dual_motor.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller_dual_motor.py @@ -98,6 +98,9 @@ def initialize(self): if self.compliance_punch is not None: self.set_compliance_punch(self.compliance_punch) if self.torque_limit is not None: self.set_torque_limit(self.torque_limit) + if self.ignored_errors is not None: + self.set_ignored_errors(self.ignored_errors) + self.joint_max_speed = rospy.get_param(self.controller_namespace + '/joint_max_speed', self.MAX_VELOCITY) if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY @@ -175,6 +178,10 @@ def set_torque_limit(self, max_torque): mcv_slave = (self.slave_id, raw_torque_val) self.dxl_io.set_multi_torque_limit([mcv_master, mcv_slave]) + def set_ignored_errors(self, errs): + self.dxl_io.set_ignored_errors(self.master_id, errs) + self.dxl_io.set_ignored_errors(self.slave_id, errs) + def process_motor_states(self, state_list): if self.running: states = {} diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller.py b/dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller.py index 076bf0f..6c88372 100644 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller.py @@ -96,6 +96,9 @@ def initialize(self): if self.compliance_punch is not None: self.set_compliance_punch(self.compliance_punch) if self.torque_limit is not None: self.set_torque_limit(self.torque_limit) + if self.ignored_errors is not None: + self.set_ignored_errors(self.ignored_errors) + self.joint_max_speed = rospy.get_param(self.controller_namespace + '/joint_max_speed', self.MAX_VELOCITY) if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY @@ -149,6 +152,9 @@ def set_torque_limit(self, max_torque): mcv = (self.motor_id, raw_torque_val) self.dxl_io.set_multi_torque_limit([mcv]) + def set_ignored_errors(self, errs): + self.dxl_io.set_ignored_errors(self.motor_id, errs) + def process_motor_states(self, state_list): if self.running: state = filter(lambda state: state.id == self.motor_id, state_list.motor_states) diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller_dual_motor.py b/dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller_dual_motor.py index 018586a..f289d71 100644 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller_dual_motor.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller_dual_motor.py @@ -98,6 +98,9 @@ def initialize(self): if self.compliance_punch is not None: self.set_compliance_punch(self.compliance_punch) if self.torque_limit is not None: self.set_torque_limit(self.torque_limit) + if self.ignored_errors is not None: + self.set_ignored_errors(self.ignored_errors) + self.joint_max_speed = rospy.get_param(self.controller_namespace + '/joint_max_speed', self.MAX_VELOCITY) if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY @@ -162,6 +165,11 @@ def set_torque_limit(self, max_torque): self.dxl_io.set_multi_torque_limit([mcv_master, mcv_slave]) + def set_ignored_errors(self, errs): + self.dxl_io.set_ignored_errors(self.master_id, errs) + self.dxl_io.set_ignored_errors(self.slave_id, errs) + + def process_motor_states(self, state_list): if self.running: states = {} diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py index c791bdd..a316ebc 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py @@ -67,6 +67,7 @@ def __init__(self, port, baudrate, readback_echo=False): self.ser = serial.Serial(port, baudrate, timeout=0.015) self.port_name = port self.readback_echo = readback_echo + self.ignored_errors = {} except SerialOpenError: raise SerialOpenError(port, baudrate) @@ -973,6 +974,18 @@ def get_led(self, servo_id): return bool(response[5]) + def set_ignored_errors(self, servo_id, errs): + """ + Sets which errors are ignored. + errs is a list of ignored errors' names. + """ + try: + self.ignored_errors[servo_id] + except KeyError: + self.ignored_errors[servo_id] = 0 + for err in errs: + self.ignored_errors[servo_id] += eval(err) + def exception_on_error(self, error_code, servo_id, command_failed): global exception exception = None @@ -982,25 +995,29 @@ def exception_on_error(self, error_code, servo_id, command_failed): msg = 'Communcation Error ' + ex_message exception = NonfatalErrorCodeError(msg, 0) return - if not error_code & DXL_OVERHEATING_ERROR == 0: + try: + detectable_errs = 0xff & ~self.ignored_errors[servo_id] + except KeyError: + detectable_errs = 0xff + if not error_code & DXL_OVERHEATING_ERROR & detectable_errs == 0: msg = 'Overheating Error ' + ex_message exception = FatalErrorCodeError(msg, error_code) - if not error_code & DXL_OVERLOAD_ERROR == 0: + if not error_code & DXL_OVERLOAD_ERROR & detectable_errs == 0: msg = 'Overload Error ' + ex_message exception = FatalErrorCodeError(msg, error_code) - if not error_code & DXL_INPUT_VOLTAGE_ERROR == 0: + if not error_code & DXL_INPUT_VOLTAGE_ERROR & detectable_errs == 0: msg = 'Input Voltage Error ' + ex_message exception = NonfatalErrorCodeError(msg, error_code) - if not error_code & DXL_ANGLE_LIMIT_ERROR == 0: + if not error_code & DXL_ANGLE_LIMIT_ERROR & detectable_errs == 0: msg = 'Angle Limit Error ' + ex_message exception = NonfatalErrorCodeError(msg, error_code) - if not error_code & DXL_RANGE_ERROR == 0: + if not error_code & DXL_RANGE_ERROR & detectable_errs == 0: msg = 'Range Error ' + ex_message exception = NonfatalErrorCodeError(msg, error_code) - if not error_code & DXL_CHECKSUM_ERROR == 0: + if not error_code & DXL_CHECKSUM_ERROR & detectable_errs == 0: msg = 'Checksum Error ' + ex_message exception = NonfatalErrorCodeError(msg, error_code) - if not error_code & DXL_INSTRUCTION_ERROR == 0: + if not error_code & DXL_INSTRUCTION_ERROR & detectable_errs == 0: msg = 'Instruction Error ' + ex_message exception = NonfatalErrorCodeError(msg, error_code)