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_driver.dynamixel_ros_commands import *
00053 from dynamixel_controllers.joint_controller import JointController
00054
00055 from dynamixel_msgs.msg import JointState
00056
00057 class JointTorqueControllerDualMotor(JointController):
00058 def __init__(self, out_cb, param_path, port_name):
00059 JointController.__init__(self, out_cb, param_path, port_name)
00060
00061 self.master_id = rospy.get_param(self.topic_name + '/motor_master/id')
00062 self.master_initial_position_raw = rospy.get_param(self.topic_name + '/motor_master/init')
00063 self.master_min_angle_raw = rospy.get_param(self.topic_name + '/motor_master/min')
00064 self.master_max_angle_raw = rospy.get_param(self.topic_name + '/motor_master/max')
00065
00066 self.slave_id = rospy.get_param(self.topic_name + '/motor_slave/id')
00067 self.slave_offset = rospy.get_param(self.topic_name + '/motor_slave/calibration_offset', 0)
00068
00069 self.flipped = self.master_min_angle_raw > self.master_max_angle_raw
00070 self.last_commanded_torque = 0.0
00071
00072 self.joint_state = JointState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
00073
00074
00075 def initialize(self):
00076
00077 available_ids = rospy.get_param('dynamixel/%s/connected_ids' % self.port_namespace, [])
00078 if not (self.master_id in available_ids and
00079 self.slave_id in available_ids):
00080 rospy.logwarn('The specified motor id is not connected and responding.')
00081 rospy.logwarn('Available ids: %s' % str(available_ids))
00082 rospy.logwarn('Specified ids: %d %d' % (self.master_id, self.slave_id))
00083 return False
00084
00085 self.RADIANS_PER_ENCODER_TICK = rospy.get_param('dynamixel/%s/%d/radians_per_encoder_tick' % (self.port_namespace, self.master_id))
00086 self.ENCODER_TICKS_PER_RADIAN = rospy.get_param('dynamixel/%s/%d/encoder_ticks_per_radian' % (self.port_namespace, self.master_id))
00087
00088 if self.flipped:
00089 self.master_min_angle = (self.master_initial_position_raw - self.master_min_angle_raw) * self.RADIANS_PER_ENCODER_TICK
00090 self.master_max_angle = (self.master_initial_position_raw - self.master_max_angle_raw) * self.RADIANS_PER_ENCODER_TICK
00091 else:
00092 self.master_min_angle = (self.master_min_angle_raw - self.master_initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
00093 self.master_max_angle = (self.master_max_angle_raw - self.master_initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
00094
00095 self.ENCODER_RESOLUTION = rospy.get_param('dynamixel/%s/%d/encoder_resolution' % (self.port_namespace, self.master_id))
00096 self.MAX_POSITION = self.ENCODER_RESOLUTION - 1
00097 self.VELOCITY_PER_TICK = rospy.get_param('dynamixel/%s/%d/radians_second_per_encoder_tick' % (self.port_namespace, self.master_id))
00098 self.MAX_VELOCITY = rospy.get_param('dynamixel/%s/%d/max_velocity' % (self.port_namespace, self.master_id))
00099 self.MIN_VELOCITY = self.VELOCITY_PER_TICK
00100
00101 if self.compliance_slope is not None: self.set_compliance_slope(self.compliance_slope)
00102 if self.compliance_margin is not None: self.set_compliance_margin(self.compliance_margin)
00103 if self.compliance_punch is not None: self.set_compliance_punch(self.compliance_punch)
00104 if self.torque_limit is not None: self.set_torque_limit(self.torque_limit)
00105
00106 self.joint_max_speed = rospy.get_param(self.topic_name + '/joint_max_speed', self.MAX_VELOCITY)
00107
00108 if self.joint_max_speed < self.MIN_VELOCITY: self.joint_max_speed = self.MIN_VELOCITY
00109 elif self.joint_max_speed > self.MAX_VELOCITY: self.joint_max_speed = self.MAX_VELOCITY
00110
00111 if self.joint_speed < self.MIN_VELOCITY: self.joint_speed = self.MIN_VELOCITY
00112 elif self.joint_speed > self.joint_max_speed: self.joint_speed = self.joint_max_speed
00113
00114 self.set_speed(0.0)
00115
00116 return True
00117
00118
00119 def set_torque_enable(self, torque_enable):
00120 mcv_master = (self.master_id, torque_enable)
00121 mcv_slave = (self.slave_id, torque_enable)
00122 self.send_packet_callback((DXL_SET_TORQUE_ENABLE, [mcv_master, mcv_slave]))
00123
00124
00125 def set_speed(self, speed):
00126 if speed < -self.joint_max_speed: speed = -self.joint_max_speed
00127 elif speed > self.joint_max_speed: speed = self.joint_max_speed
00128 self.last_commanded_torque = speed
00129 speed_raw = int(round(speed / self.VELOCITY_PER_TICK))
00130 mcv_master = (self.master_id, speed_raw if speed_raw > 0 else 1)
00131 mcv_slave = (self.slave_id, mcv_master[1])
00132 self.send_packet_callback((DXL_SET_GOAL_SPEED, [mcv_master, mcv_slave]))
00133
00134
00135 def set_compliance_slope(self, slope):
00136 if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
00137 elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE
00138 slope2 = (slope << 8) + slope
00139 mcv_master = (self.master_id, slope2)
00140 mcv_slave = (self.slave_id, slope2)
00141 self.send_packet_callback((DXL_SET_COMPLIANCE_SLOPES, [mcv_master, mcv_slave]))
00142
00143
00144 def set_compliance_margin(self, margin):
00145 if margin < DXL_MIN_COMPLIANCE_MARGIN: margin = DXL_MIN_COMPLIANCE_MARGIN
00146 elif margin > DXL_MAX_COMPLIANCE_MARGIN: margin = DXL_MAX_COMPLIANCE_MARGIN
00147 else: margin = int(margin)
00148 margin2 = (margin << 8) + margin
00149 mcv_master = (self.master_id, margin2)
00150 mcv_slave = (self.slave_id, margin2)
00151 self.send_packet_callback((DXL_SET_COMPLIANCE_MARGINS, [mcv_master, mcv_slave]))
00152
00153
00154 def set_compliance_punch(self, punch):
00155 if punch < DXL_MIN_PUNCH: punch = DXL_MIN_PUNCH
00156 elif punch > DXL_MAX_PUNCH: punch = DXL_MAX_PUNCH
00157 else: punch = int(punch)
00158 mcv_master = (self.master_id, punch)
00159 mcv_slave = (self.slave_id, punch)
00160 self.send_packet_callback((DXL_SET_PUNCH, [mcv_master, mcv_slave]))
00161
00162
00163 def set_torque_limit(self, max_torque):
00164 if max_torque > 1: max_torque = 1.0
00165 elif max_torque < 0: max_torque = 0.0
00166 raw_torque_val = int(DXL_MAX_TORQUE_TICK * max_torque)
00167 mcv_master = (self.master_id, raw_torque_val)
00168 mcv_slave = (self.slave_id, raw_torque_val)
00169 self.send_packet_callback((DXL_SET_TORQUE_LIMIT, [mcv_master, mcv_slave]))
00170
00171
00172 def process_motor_states(self, state_list):
00173 if self.running:
00174 states = {}
00175
00176 for state in state_list.motor_states:
00177 if state.id in [self.master_id, self.slave_id]: states[state.id] = state
00178
00179 if states:
00180 state = states[self.master_id]
00181 self.joint_state.motor_temps = [state.temperature, states[self.slave_id].temperature]
00182 self.joint_state.goal_pos = self.last_commanded_torque
00183 self.joint_state.current_pos = self.raw_to_rad(state.position, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
00184 self.joint_state.error = 0.0
00185 self.joint_state.velocity = (state.speed / DXL_MAX_SPEED_TICK) * self.MAX_VELOCITY
00186 self.joint_state.load = state.load
00187 self.joint_state.is_moving = state.moving
00188 self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)
00189
00190 self.joint_state_pub.publish(self.joint_state)
00191
00192
00193 def process_command(self, msg):
00194 self.set_speed(msg.data)
00195