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_driver.dynamixel_ros_commands import *
00054 from dynamixel_controllers.joint_controller import JointController
00055
00056 from dynamixel_msgs.msg import JointState
00057
00058 class JointPositionController(JointController):
00059 def __init__(self, out_cb, param_path, port_name):
00060 JointController.__init__(self, out_cb, param_path, port_name)
00061
00062 self.motor_id = rospy.get_param(self.topic_name + '/motor/id')
00063 self.initial_position_raw = rospy.get_param(self.topic_name + '/motor/init')
00064 self.min_angle_raw = rospy.get_param(self.topic_name + '/motor/min')
00065 self.max_angle_raw = rospy.get_param(self.topic_name + '/motor/max')
00066
00067 self.flipped = self.min_angle_raw > self.max_angle_raw
00068
00069 self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
00070
00071 def initialize(self):
00072
00073 available_ids = rospy.get_param('dynamixel/%s/connected_ids' % self.port_namespace, [])
00074 if not self.motor_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 id: %d' % self.motor_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.motor_id))
00081 self.ENCODER_TICKS_PER_RADIAN = rospy.get_param('dynamixel/%s/%d/encoder_ticks_per_radian' % (self.port_namespace, self.motor_id))
00082
00083 if self.flipped:
00084 self.min_angle = (self.initial_position_raw - self.min_angle_raw) * self.RADIANS_PER_ENCODER_TICK
00085 self.max_angle = (self.initial_position_raw - self.max_angle_raw) * self.RADIANS_PER_ENCODER_TICK
00086 else:
00087 self.min_angle = (self.min_angle_raw - self.initial_position_raw) * self.RADIANS_PER_ENCODER_TICK
00088 self.max_angle = (self.max_angle_raw - self.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.motor_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.motor_id))
00093 self.MAX_VELOCITY = rospy.get_param('dynamixel/%s/%d/max_velocity' % (self.port_namespace, self.motor_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.topic_name + '/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(self.joint_speed)
00110
00111 return True
00112
00113 def set_torque_enable(self, torque_enable):
00114 mcv = (self.motor_id, torque_enable)
00115 self.send_packet_callback((DXL_SET_TORQUE_ENABLE, [mcv]))
00116
00117 def set_speed(self, speed):
00118 if speed < self.MIN_VELOCITY: speed = self.MIN_VELOCITY
00119 elif speed > self.joint_max_speed: speed = self.joint_max_speed
00120 speed_raw = int(round(speed / self.VELOCITY_PER_TICK))
00121 mcv = (self.motor_id, speed_raw)
00122 self.send_packet_callback((DXL_SET_GOAL_SPEED, [mcv]))
00123
00124 def set_compliance_slope(self, slope):
00125 if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
00126 elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE
00127 slope2 = (slope << 8) + slope
00128 mcv = (self.motor_id, slope2)
00129 self.send_packet_callback((DXL_SET_COMPLIANCE_SLOPES, [mcv]))
00130
00131 def set_compliance_margin(self, margin):
00132 if margin < DXL_MIN_COMPLIANCE_MARGIN: margin = DXL_MIN_COMPLIANCE_MARGIN
00133 elif margin > DXL_MAX_COMPLIANCE_MARGIN: margin = DXL_MAX_COMPLIANCE_MARGIN
00134 else: margin = int(margin)
00135 margin2 = (margin << 8) + margin
00136 mcv = (self.motor_id, margin2)
00137 self.send_packet_callback((DXL_SET_COMPLIANCE_MARGINS, [mcv]))
00138
00139 def set_compliance_punch(self, punch):
00140 if punch < DXL_MIN_PUNCH: punch = DXL_MIN_PUNCH
00141 elif punch > DXL_MAX_PUNCH: punch = DXL_MAX_PUNCH
00142 else: punch = int(punch)
00143 mcv = (self.motor_id, punch)
00144 self.send_packet_callback((DXL_SET_PUNCH, [mcv]))
00145
00146 def set_torque_limit(self, max_torque):
00147 if max_torque > 1: max_torque = 1.0
00148 elif max_torque < 0: max_torque = 0.0
00149 raw_torque_val = int(DXL_MAX_TORQUE_TICK * max_torque)
00150 mcv = (self.motor_id, raw_torque_val)
00151 self.send_packet_callback((DXL_SET_TORQUE_LIMIT, [mcv]))
00152
00153 def process_motor_states(self, state_list):
00154 if self.running:
00155 state = filter(lambda state: state.id == self.motor_id, state_list.motor_states)
00156 if state:
00157 state = state[0]
00158 self.joint_state.motor_temps = [state.temperature]
00159 self.joint_state.goal_pos = self.raw_to_rad(state.goal, self.initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
00160 self.joint_state.current_pos = self.raw_to_rad(state.position, self.initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
00161 self.joint_state.error = state.error * self.RADIANS_PER_ENCODER_TICK
00162 self.joint_state.velocity = (state.speed / DXL_MAX_SPEED_TICK) * self.MAX_VELOCITY
00163 self.joint_state.load = state.load
00164 self.joint_state.is_moving = state.moving
00165 self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)
00166
00167 self.joint_state_pub.publish(self.joint_state)
00168
00169 def process_command(self, msg):
00170 angle = msg.data
00171 if angle < self.min_angle: angle = self.min_angle
00172 elif angle > self.max_angle: angle = self.max_angle
00173 mcv = (self.motor_id, self.rad_to_raw(angle, self.initial_position_raw, self.flipped, self.ENCODER_TICKS_PER_RADIAN))
00174 self.send_packet_callback((DXL_SET_GOAL_POSITION, [mcv]))
00175