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 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
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
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