00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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 rospy
00047
00048 from dynamixel_driver.dynamixel_const import *
00049 from dynamixel_controllers.joint_controller import JointController
00050
00051 from dynamixel_msgs.msg import JointState
00052
00053 class JointTorqueControllerDualMotor(JointController):
00054 def __init__(self, dxl_io, controller_namespace, port_namespace):
00055 JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
00056
00057 self.master_id = rospy.get_param(self.controller_namespace + '/motor_master/id')
00058 self.master_initial_position_raw = rospy.get_param(self.controller_namespace + '/motor_master/init')
00059 self.master_min_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/min')
00060 self.master_max_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/max')
00061
00062 self.slave_id = rospy.get_param(self.controller_namespace + '/motor_slave/id')
00063
00064 self.flipped = self.master_min_angle_raw > self.master_max_angle_raw
00065 self.last_commanded_torque = 0.0
00066
00067 self.joint_state = JointState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
00068
00069
00070 def initialize(self):
00071
00072 available_ids = rospy.get_param('dynamixel/%s/connected_ids' % self.port_namespace, [])
00073 if not (self.master_id in available_ids and
00074 self.slave_id in available_ids):
00075 rospy.logwarn('The specified motor id is not connected and responding.')
00076 rospy.logwarn('Available ids: %s' % str(available_ids))
00077 rospy.logwarn('Specified ids: %d %d' % (self.master_id, self.slave_id))
00078 return False
00079
00080 self.RADIANS_PER_ENCODER_TICK = rospy.get_param('dynamixel/%s/%d/radians_per_encoder_tick' % (self.port_namespace, self.master_id))
00081 self.ENCODER_TICKS_PER_RADIAN = rospy.get_param('dynamixel/%s/%d/encoder_ticks_per_radian' % (self.port_namespace, self.master_id))
00082
00083 if self.flipped:
00084 self.master_min_angle = (self.master_initial_position_raw - self.master_min_angle_raw) * self.RADIANS_PER_ENCODER_TICK
00085 self.master_max_angle = (self.master_initial_position_raw - self.master_max_angle_raw) * self.RADIANS_PER_ENCODER_TICK
00086 else:
00087 self.master_min_angle = (self.master_min_angle_raw - self.master_initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
00088 self.master_max_angle = (self.master_max_angle_raw - self.master_initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
00089
00090 self.ENCODER_RESOLUTION = rospy.get_param('dynamixel/%s/%d/encoder_resolution' % (self.port_namespace, self.master_id))
00091 self.MAX_POSITION = self.ENCODER_RESOLUTION - 1
00092 self.VELOCITY_PER_TICK = rospy.get_param('dynamixel/%s/%d/radians_second_per_encoder_tick' % (self.port_namespace, self.master_id))
00093 self.MAX_VELOCITY = rospy.get_param('dynamixel/%s/%d/max_velocity' % (self.port_namespace, self.master_id))
00094 self.MIN_VELOCITY = self.VELOCITY_PER_TICK
00095
00096 if self.compliance_slope is not None: self.set_compliance_slope(self.compliance_slope)
00097 if self.compliance_margin is not None: self.set_compliance_margin(self.compliance_margin)
00098 if self.compliance_punch is not None: self.set_compliance_punch(self.compliance_punch)
00099 if self.torque_limit is not None: self.set_torque_limit(self.torque_limit)
00100
00101 self.joint_max_speed = rospy.get_param(self.controller_namespace + '/joint_max_speed', self.MAX_VELOCITY)
00102
00103 if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY
00104 elif self.joint_max_speed > self.MAX_VELOCITY: self.joint_max_speed = self.MAX_VELOCITY
00105
00106 if self.joint_speed < self.MIN_VELOCITY: self.joint_speed = self.MIN_VELOCITY
00107 elif self.joint_speed > self.joint_max_speed: self.joint_speed = self.joint_max_speed
00108
00109 self.set_speed(0.0)
00110
00111 return True
00112
00113
00114 def set_torque_enable(self, torque_enable):
00115 mcv_master = (self.master_id, torque_enable)
00116 mcv_slave = (self.slave_id, torque_enable)
00117 self.dxl_io.set_multi_torque_enabled([mcv_master, mcv_slave])
00118
00119
00120 def set_speed(self, speed):
00121 if speed < -self.joint_max_speed: speed = -self.joint_max_speed
00122 elif speed > self.joint_max_speed: speed = self.joint_max_speed
00123 self.last_commanded_torque = speed
00124 speed_raw = int(round(speed / self.VELOCITY_PER_TICK))
00125 mcv_master = (self.master_id, speed_raw)
00126 mcv_slave = (self.slave_id, -mcv_master[1])
00127 self.dxl_io.set_multi_speed([mcv_master, mcv_slave])
00128
00129
00130 def set_compliance_slope(self, slope):
00131 if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
00132 elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE
00133 mcv_master = (self.master_id, slope, slope)
00134 mcv_slave = (self.slave_id, slope, slope)
00135 self.dxl_io.set_multi_compliance_slopes([mcv_master, mcv_slave])
00136
00137
00138 def set_compliance_margin(self, margin):
00139 if margin < DXL_MIN_COMPLIANCE_MARGIN: margin = DXL_MIN_COMPLIANCE_MARGIN
00140 elif margin > DXL_MAX_COMPLIANCE_MARGIN: margin = DXL_MAX_COMPLIANCE_MARGIN
00141 else: margin = int(margin)
00142 mcv_master = (self.master_id, margin, margin)
00143 mcv_slave = (self.slave_id, margin, margin)
00144 self.dxl_io.set_multi_compliance_margins([mcv_master, mcv_slave])
00145
00146
00147 def set_compliance_punch(self, punch):
00148 if punch < DXL_MIN_PUNCH: punch = DXL_MIN_PUNCH
00149 elif punch > DXL_MAX_PUNCH: punch = DXL_MAX_PUNCH
00150 else: punch = int(punch)
00151 mcv_master = (self.master_id, punch)
00152 mcv_slave = (self.slave_id, punch)
00153 self.dxl_io.set_multi_punch([mcv_master, mcv_slave])
00154
00155
00156 def set_torque_limit(self, max_torque):
00157 if max_torque > 1: max_torque = 1.0
00158 elif max_torque < 0: max_torque = 0.0
00159 raw_torque_val = int(DXL_MAX_TORQUE_TICK * max_torque)
00160 mcv_master = (self.master_id, raw_torque_val)
00161 mcv_slave = (self.slave_id, raw_torque_val)
00162 self.dxl_io.set_multi_torque_limit([mcv_master, mcv_slave])
00163
00164
00165 def process_motor_states(self, state_list):
00166 if self.running:
00167 states = {}
00168
00169 for state in state_list.motor_states:
00170 if state.id in [self.master_id, self.slave_id]: states[state.id] = state
00171
00172 if states:
00173 state = states[self.master_id]
00174 self.joint_state.motor_temps = [state.temperature, states[self.slave_id].temperature]
00175 self.joint_state.goal_pos = self.last_commanded_torque
00176 self.joint_state.current_pos = self.raw_to_rad(state.position, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
00177 self.joint_state.error = 0.0
00178 self.joint_state.velocity = state.speed * self.VELOCITY_PER_TICK
00179 self.joint_state.load = state.load
00180 self.joint_state.is_moving = state.moving
00181 self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)
00182
00183 self.joint_state_pub.publish(self.joint_state)
00184
00185
00186 def process_command(self, msg):
00187 self.set_speed(msg.data)
00188