$search
00001 # -*- coding: utf-8 -*- 00002 # 00003 # Software License Agreement (BSD License) 00004 # 00005 # Copyright (c) 2010-2011, Antons Rebguns. 00006 # All rights reserved. 00007 # 00008 # Redistribution and use in source and binary forms, with or without 00009 # modification, are permitted provided that the following conditions 00010 # are met: 00011 # 00012 # * Redistributions of source code must retain the above copyright 00013 # notice, this list of conditions and the following disclaimer. 00014 # * Redistributions in binary form must reproduce the above 00015 # copyright notice, this list of conditions and the following 00016 # disclaimer in the documentation and/or other materials provided 00017 # with the distribution. 00018 # * Neither the name of University of Arizona nor the names of its 00019 # contributors may be used to endorse or promote products derived 00020 # from this software without specific prior written permission. 00021 # 00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 # POSSIBILITY OF SUCH DAMAGE. 00034 00035 from __future__ import division 00036 00037 00038 __author__ = 'Antons Rebguns' 00039 __copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns' 00040 00041 __license__ = 'BSD' 00042 __maintainer__ = 'Antons Rebguns' 00043 __email__ = 'anton@email.arizona.edu' 00044 00045 00046 import roslib 00047 roslib.load_manifest('dynamixel_controllers') 00048 00049 import rospy 00050 00051 from dynamixel_driver.dynamixel_const import * 00052 from dynamixel_controllers.joint_controller import JointController 00053 00054 from dynamixel_msgs.msg import JointState 00055 00056 class JointTorqueControllerDualMotor(JointController): 00057 def __init__(self, dxl_io, controller_namespace, port_namespace): 00058 JointController.__init__(self, dxl_io, controller_namespace, port_namespace) 00059 00060 self.master_id = rospy.get_param(self.controller_namespace + '/motor_master/id') 00061 self.master_initial_position_raw = rospy.get_param(self.controller_namespace + '/motor_master/init') 00062 self.master_min_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/min') 00063 self.master_max_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/max') 00064 00065 self.slave_id = rospy.get_param(self.controller_namespace + '/motor_slave/id') 00066 00067 self.flipped = self.master_min_angle_raw > self.master_max_angle_raw 00068 self.last_commanded_torque = 0.0 00069 00070 self.joint_state = JointState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id]) 00071 00072 00073 def initialize(self): 00074 # verify that the expected motor is connected and responding 00075 available_ids = rospy.get_param('dynamixel/%s/connected_ids' % self.port_namespace, []) 00076 if not (self.master_id in available_ids and 00077 self.slave_id in available_ids): 00078 rospy.logwarn('The specified motor id is not connected and responding.') 00079 rospy.logwarn('Available ids: %s' % str(available_ids)) 00080 rospy.logwarn('Specified ids: %d %d' % (self.master_id, self.slave_id)) 00081 return False 00082 00083 self.RADIANS_PER_ENCODER_TICK = rospy.get_param('dynamixel/%s/%d/radians_per_encoder_tick' % (self.port_namespace, self.master_id)) 00084 self.ENCODER_TICKS_PER_RADIAN = rospy.get_param('dynamixel/%s/%d/encoder_ticks_per_radian' % (self.port_namespace, self.master_id)) 00085 00086 if self.flipped: 00087 self.master_min_angle = (self.master_initial_position_raw - self.master_min_angle_raw) * self.RADIANS_PER_ENCODER_TICK 00088 self.master_max_angle = (self.master_initial_position_raw - self.master_max_angle_raw) * self.RADIANS_PER_ENCODER_TICK 00089 else: 00090 self.master_min_angle = (self.master_min_angle_raw - self.master_initial_position_raw) * self.RADIANS_PER_ENCODER_TICK 00091 self.master_max_angle = (self.master_max_angle_raw - self.master_initial_position_raw) * self.RADIANS_PER_ENCODER_TICK 00092 00093 self.ENCODER_RESOLUTION = rospy.get_param('dynamixel/%s/%d/encoder_resolution' % (self.port_namespace, self.master_id)) 00094 self.MAX_POSITION = self.ENCODER_RESOLUTION - 1 00095 self.VELOCITY_PER_TICK = rospy.get_param('dynamixel/%s/%d/radians_second_per_encoder_tick' % (self.port_namespace, self.master_id)) 00096 self.MAX_VELOCITY = rospy.get_param('dynamixel/%s/%d/max_velocity' % (self.port_namespace, self.master_id)) 00097 self.MIN_VELOCITY = self.VELOCITY_PER_TICK 00098 00099 if self.compliance_slope is not None: self.set_compliance_slope(self.compliance_slope) 00100 if self.compliance_margin is not None: self.set_compliance_margin(self.compliance_margin) 00101 if self.compliance_punch is not None: self.set_compliance_punch(self.compliance_punch) 00102 if self.torque_limit is not None: self.set_torque_limit(self.torque_limit) 00103 00104 self.joint_max_speed = rospy.get_param(self.controller_namespace + '/joint_max_speed', self.MAX_VELOCITY) 00105 00106 if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY 00107 elif self.joint_max_speed > self.MAX_VELOCITY: self.joint_max_speed = self.MAX_VELOCITY 00108 00109 if self.joint_speed < self.MIN_VELOCITY: self.joint_speed = self.MIN_VELOCITY 00110 elif self.joint_speed > self.joint_max_speed: self.joint_speed = self.joint_max_speed 00111 00112 self.set_speed(0.0) 00113 00114 return True 00115 00116 00117 def set_torque_enable(self, torque_enable): 00118 mcv_master = (self.master_id, torque_enable) 00119 mcv_slave = (self.slave_id, torque_enable) 00120 self.dxl_io.set_multi_torque_enabled([mcv_master, mcv_slave]) 00121 00122 00123 def set_speed(self, speed): 00124 if speed < -self.joint_max_speed: speed = -self.joint_max_speed 00125 elif speed > self.joint_max_speed: speed = self.joint_max_speed 00126 self.last_commanded_torque = speed 00127 speed_raw = int(round(speed / self.VELOCITY_PER_TICK)) 00128 mcv_master = (self.master_id, speed_raw if speed_raw > 0 else 1) 00129 mcv_slave = (self.slave_id, mcv_master[1]) 00130 self.dxl_io.set_multi_speed([mcv_master, mcv_slave]) 00131 00132 00133 def set_compliance_slope(self, slope): 00134 if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE 00135 elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE 00136 mcv_master = (self.master_id, slope, slope) 00137 mcv_slave = (self.slave_id, slope, slope) 00138 self.dxl_io.set_multi_compliance_slopes([mcv_master, mcv_slave]) 00139 00140 00141 def set_compliance_margin(self, margin): 00142 if margin < DXL_MIN_COMPLIANCE_MARGIN: margin = DXL_MIN_COMPLIANCE_MARGIN 00143 elif margin > DXL_MAX_COMPLIANCE_MARGIN: margin = DXL_MAX_COMPLIANCE_MARGIN 00144 else: margin = int(margin) 00145 mcv_master = (self.master_id, margin, margin) 00146 mcv_slave = (self.slave_id, margin, margin) 00147 self.dxl_io.set_multi_compliance_margins([mcv_master, mcv_slave]) 00148 00149 00150 def set_compliance_punch(self, punch): 00151 if punch < DXL_MIN_PUNCH: punch = DXL_MIN_PUNCH 00152 elif punch > DXL_MAX_PUNCH: punch = DXL_MAX_PUNCH 00153 else: punch = int(punch) 00154 mcv_master = (self.master_id, punch) 00155 mcv_slave = (self.slave_id, punch) 00156 self.dxl_io.set_multi_punch([mcv_master, mcv_slave]) 00157 00158 00159 def set_torque_limit(self, max_torque): 00160 if max_torque > 1: max_torque = 1.0 00161 elif max_torque < 0: max_torque = 0.0 # turn off motor torque 00162 raw_torque_val = int(DXL_MAX_TORQUE_TICK * max_torque) 00163 mcv_master = (self.master_id, raw_torque_val) 00164 mcv_slave = (self.slave_id, raw_torque_val) 00165 self.dxl_io.set_multi_torque_limit([mcv_master, mcv_slave]) 00166 00167 00168 def process_motor_states(self, state_list): 00169 if self.running: 00170 states = {} 00171 00172 for state in state_list.motor_states: 00173 if state.id in [self.master_id, self.slave_id]: states[state.id] = state 00174 00175 if states: 00176 state = states[self.master_id] 00177 self.joint_state.motor_temps = [state.temperature, states[self.slave_id].temperature] 00178 self.joint_state.goal_pos = self.last_commanded_torque 00179 self.joint_state.current_pos = self.raw_to_rad(state.position, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK) 00180 self.joint_state.error = 0.0 00181 self.joint_state.velocity = (state.speed / DXL_MAX_SPEED_TICK) * self.MAX_VELOCITY 00182 self.joint_state.load = state.load 00183 self.joint_state.is_moving = state.moving 00184 self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp) 00185 00186 self.joint_state_pub.publish(self.joint_state) 00187 00188 00189 def process_command(self, msg): 00190 self.set_speed(msg.data) 00191