From 6cd1975a609ff009036f5e098f38f6db904fa050 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 5 Mar 2014 17:34:13 -0800 Subject: [PATCH 1/6] Added an Enable service to joint_position_controller.py that can enable/disable a servo so that it ignores position requests --- dynamixel_controllers/srv/Enable.srv | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 dynamixel_controllers/srv/Enable.srv diff --git a/dynamixel_controllers/srv/Enable.srv b/dynamixel_controllers/srv/Enable.srv new file mode 100644 index 0000000..a08c49e --- /dev/null +++ b/dynamixel_controllers/srv/Enable.srv @@ -0,0 +1,3 @@ +bool enable +--- + From ad1398f2cac27262f1d4501a891bcb00de30be71 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 5 Mar 2014 17:34:13 -0800 Subject: [PATCH 2/6] Added an Enable service to joint_position_controller.py that can enable/disable a servo so that it ignores position requests --- dynamixel_controllers/CMakeLists.txt | 1 + .../joint_position_controller.py | 12 +++++++++--- dynamixel_controllers/srv/Enable.srv | 3 +++ 3 files changed, 13 insertions(+), 3 deletions(-) create mode 100644 dynamixel_controllers/srv/Enable.srv diff --git a/dynamixel_controllers/CMakeLists.txt b/dynamixel_controllers/CMakeLists.txt index 38ca87f..e31a23b 100644 --- a/dynamixel_controllers/CMakeLists.txt +++ b/dynamixel_controllers/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation) add_service_files( FILES + Enable.srv RestartController.srv SetComplianceMargin.srv SetCompliancePunch.srv diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py index 9f5aabf..debaac0 100644 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py @@ -104,6 +104,8 @@ def initialize(self): self.set_speed(self.joint_speed) + self.enabled = True + return True def pos_rad_to_raw(self, pos_rad): @@ -120,6 +122,9 @@ def spd_rad_to_raw(self, spd_rad): def set_torque_enable(self, torque_enable): mcv = (self.motor_id, torque_enable) self.dxl_io.set_multi_torque_enabled([mcv]) + + def set_enable(self, enable): + self.enabled = enable def set_speed(self, speed): mcv = (self.motor_id, self.spd_rad_to_raw(speed)) @@ -169,7 +174,8 @@ def process_motor_states(self, state_list): self.joint_state_pub.publish(self.joint_state) def process_command(self, msg): - angle = msg.data - mcv = (self.motor_id, self.pos_rad_to_raw(angle)) - self.dxl_io.set_multi_position([mcv]) + if self.enabled: + angle = msg.data + mcv = (self.motor_id, self.pos_rad_to_raw(angle)) + self.dxl_io.set_multi_position([mcv]) diff --git a/dynamixel_controllers/srv/Enable.srv b/dynamixel_controllers/srv/Enable.srv new file mode 100644 index 0000000..a08c49e --- /dev/null +++ b/dynamixel_controllers/srv/Enable.srv @@ -0,0 +1,3 @@ +bool enable +--- + From 17832f045c894757660f69a6b7d298335fa760b3 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 5 Mar 2014 17:40:48 -0800 Subject: [PATCH 3/6] Merge branch 'master' of https://github.com/pirobot/dynamixel_motor --- .../src/dynamixel_controllers/joint_controller.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py b/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py index 45c05ba..b1f97ae 100644 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py @@ -49,6 +49,7 @@ from dynamixel_controllers.srv import SetSpeed from dynamixel_controllers.srv import TorqueEnable +from dynamixel_controllers.srv import Enable from dynamixel_controllers.srv import SetComplianceSlope from dynamixel_controllers.srv import SetComplianceMargin from dynamixel_controllers.srv import SetCompliancePunch @@ -75,6 +76,7 @@ def __init__(self, dxl_io, controller_namespace, port_namespace): self.speed_service = rospy.Service(self.controller_namespace + '/set_speed', SetSpeed, self.process_set_speed) self.torque_service = rospy.Service(self.controller_namespace + '/torque_enable', TorqueEnable, self.process_torque_enable) + self.enable_service = rospy.Service(self.controller_namespace + '/enable', Enable, self.process_enable) self.compliance_slope_service = rospy.Service(self.controller_namespace + '/set_compliance_slope', SetComplianceSlope, self.process_set_compliance_slope) self.compliance_marigin_service = rospy.Service(self.controller_namespace + '/set_compliance_margin', SetComplianceMargin, self.process_set_compliance_margin) self.compliance_punch_service = rospy.Service(self.controller_namespace + '/set_compliance_punch', SetCompliancePunch, self.process_set_compliance_punch) @@ -120,6 +122,9 @@ def stop(self): def set_torque_enable(self, torque_enable): raise NotImplementedError + + def set_enable(self, enable): + raise NotImplementedError def set_speed(self, speed): raise NotImplementedError @@ -143,6 +148,10 @@ def process_set_speed(self, req): def process_torque_enable(self, req): self.set_torque_enable(req.torque_enable) return [] + + def process_enable(self, req): + self.set_enable(req.enable) + return [] def process_set_compliance_slope(self, req): self.set_compliance_slope(req.slope) From 3321510a84c8ecceaa4e8cb57cf9c8fd3131d858 Mon Sep 17 00:00:00 2001 From: Antons Rebguns Date: Wed, 12 Feb 2014 12:51:32 -0600 Subject: [PATCH 4/6] Revert "Allow negative speed values." This reverts commit 6d3616e0f895809a7aa63ccad1fc58146c82b4ff. --- .../joint_torque_controller_dual_motor.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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..0466cb4 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 @@ -122,8 +122,8 @@ def set_speed(self, speed): elif speed > self.joint_max_speed: speed = self.joint_max_speed self.last_commanded_torque = speed speed_raw = int(round(speed / self.VELOCITY_PER_TICK)) - mcv_master = (self.master_id, speed_raw) - mcv_slave = (self.slave_id, -mcv_master[1]) + mcv_master = (self.master_id, speed_raw if speed_raw > 0 else 1) + mcv_slave = (self.slave_id, mcv_master[1]) self.dxl_io.set_multi_speed([mcv_master, mcv_slave]) From c6ce7273cf292eb34b65879833b42c2323d2c309 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 6 Mar 2014 16:28:35 -0800 Subject: [PATCH 5/6] Merge branches 'master' and 'test-enable' From 95ee296d354294a49dfccae5cc6bb78dc94c0625 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 6 Mar 2014 18:23:52 -0800 Subject: [PATCH 6/6] Add Enable/Disable support to the JointActionTrajectory controller --- .../joint_trajectory_action_controller.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_trajectory_action_controller.py b/dynamixel_controllers/src/dynamixel_controllers/joint_trajectory_action_controller.py index c9d83eb..fc5c759 100755 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_trajectory_action_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_trajectory_action_controller.py @@ -158,6 +158,14 @@ def process_trajectory(self, traj): rospy.logerr(msg) self.action_server.set_aborted(text=msg) return + + # make sure none of the required servos is disabled + for joint in self.joint_names: + if not self.joint_to_controller[joint].enabled: + msg = 'At least one of the required servos is currently disabled' + rospy.logerr(msg) + self.action_server.set_aborted(text=msg) + return # correlate the joints we're commanding to the joints in the message # map from an index of joint in the controller to an index in the trajectory