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 __credits__ = 'Cara Slutter'
00041
00042 __license__ = 'BSD'
00043 __maintainer__ = 'Antons Rebguns'
00044 __email__ = 'anton@email.arizona.edu'
00045
00046
00047 import roslib
00048 roslib.load_manifest('dynamixel_controllers')
00049
00050 import rospy
00051
00052 from dynamixel_driver.dynamixel_const import *
00053 from dynamixel_controllers.joint_controller import JointController
00054
00055 from dynamixel_msgs.msg import JointState
00056
00057 class JointPositionControllerDual(JointController):
00058 def __init__(self, dxl_io, controller_namespace, port_namespace):
00059 JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
00060
00061 self.master_id = rospy.get_param(self.controller_namespace + '/motor_master/id')
00062 self.master_initial_position_raw = rospy.get_param(self.controller_namespace + '/motor_master/init')
00063 self.master_min_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/min')
00064 self.master_max_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/max')
00065
00066 self.slave_id = rospy.get_param(self.controller_namespace + '/motor_slave/id')
00067 self.slave_offset = rospy.get_param(self.controller_namespace + '/motor_slave/calibration_offset', 0)
00068
00069 self.flipped = self.master_min_angle_raw > self.master_max_angle_raw
00070
00071 self.joint_state = JointState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
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(self.joint_speed)
00113
00114 return True
00115
00116 def set_torque_enable(self, torque_enable):
00117 mcv_master = (self.master_id, torque_enable)
00118 mcv_slave = (self.slave_id, torque_enable)
00119 self.dxl_io.set_multi_torque_enabled([mcv_master, mcv_slave])
00120
00121 def set_speed(self, speed):
00122 if speed < self.MIN_VELOCITY: speed = self.MIN_VELOCITY
00123 elif speed > self.joint_max_speed: speed = self.joint_max_speed
00124 speed_raw = int(round(speed / self.VELOCITY_PER_TICK))
00125 mcv_master = (self.master_id, speed_raw if speed_raw > 0 else 1)
00126 mcv_slave = (self.slave_id, mcv_master[1])
00127 self.dxl_io.set_multi_speed([mcv_master, mcv_slave])
00128
00129 def set_compliance_slope(self, slope):
00130 if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
00131 elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE
00132 mcv_master = (self.master_id, slope, slope)
00133 mcv_slave = (self.slave_id, slope, slope)
00134 self.dxl_io.set_multi_compliance_slopes([mcv_master, mcv_slave])
00135
00136 def set_compliance_margin(self, margin):
00137 if margin < DXL_MIN_COMPLIANCE_MARGIN: margin = DXL_MIN_COMPLIANCE_MARGIN
00138 elif margin > DXL_MAX_COMPLIANCE_MARGIN: margin = DXL_MAX_COMPLIANCE_MARGIN
00139 else: margin = int(margin)
00140 mcv_master = (self.master_id, margin, margin)
00141 mcv_slave = (self.slave_id, margin, margin)
00142 self.dxl_io.set_multi_compliance_margins([mcv_master, mcv_slave])
00143
00144 def set_compliance_punch(self, punch):
00145 if punch < DXL_MIN_PUNCH: punch = DXL_MIN_PUNCH
00146 elif punch > DXL_MAX_PUNCH: punch = DXL_MAX_PUNCH
00147 else: punch = int(punch)
00148 mcv_master = (self.master_id, punch)
00149 mcv_slave = (self.slave_id, punch)
00150 self.dxl_io.set_multi_punch([mcv_master, mcv_slave])
00151
00152 def set_torque_limit(self, max_torque):
00153 if max_torque > 1: max_torque = 1.0
00154 elif max_torque < 0: max_torque = 0.0
00155 raw_torque_val = int(DXL_MAX_TORQUE_TICK * max_torque)
00156 mcv_master = (self.master_id, raw_torque_val)
00157 mcv_slave = (self.slave_id, raw_torque_val)
00158 self.dxl_io.set_multi_torque_limit([mcv_master, mcv_slave])
00159
00160 def process_motor_states(self, state_list):
00161 if self.running:
00162 states = {}
00163
00164 for state in state_list.motor_states:
00165 if state.id in [self.master_id, self.slave_id]: states[state.id] = state
00166
00167 if states:
00168 state = states[self.master_id]
00169 self.joint_state.motor_temps = [state.temperature, states[self.slave_id].temperature]
00170 self.joint_state.goal_pos = self.raw_to_rad(state.goal, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
00171 self.joint_state.current_pos = self.raw_to_rad(state.position, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
00172 self.joint_state.error = state.error * self.RADIANS_PER_ENCODER_TICK
00173 self.joint_state.velocity = (state.speed / DXL_MAX_SPEED_TICK) * self.MAX_VELOCITY
00174 self.joint_state.load = state.load
00175 self.joint_state.is_moving = state.moving
00176 self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)
00177
00178 self.joint_state_pub.publish(self.joint_state)
00179
00180 def process_command(self, msg):
00181 angle = msg.data
00182 if angle < self.master_min_angle: angle = self.master_min_angle
00183 elif angle > self.master_max_angle: angle = self.master_max_angle
00184 mcv_master = (self.master_id, self.rad_to_raw(angle, self.master_initial_position_raw, self.flipped, self.ENCODER_TICKS_PER_RADIAN))
00185 mcv_slave = [self.slave_id, self.MAX_POSITION - mcv_master[1] + self.slave_offset]
00186 if mcv_slave[1] < 0: mcv_slave[1] = 0
00187 elif mcv_slave[1] > self.MAX_POSITION: mcv_slave[1] = self.MAX_POSITION
00188 self.dxl_io.set_multi_position([mcv_master, mcv_slave])
00189