35 from __future__
import division
38 __author__ =
'Antons Rebguns' 39 __copyright__ =
'Copyright (c) 2010-2011 Antons Rebguns' 42 __maintainer__ =
'Antons Rebguns' 43 __email__ =
'anton@email.arizona.edu' 51 from dynamixel_msgs.msg
import JointState
54 def __init__(self, dxl_io, controller_namespace, port_namespace):
55 JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
72 available_ids = rospy.get_param(
'dynamixel/%s/connected_ids' % self.
port_namespace, [])
73 if not (self.
master_id in available_ids
and 75 rospy.logwarn(
'The specified motor id is not connected and responding.')
76 rospy.logwarn(
'Available ids: %s' % str(available_ids))
115 mcv_master = (self.
master_id, torque_enable)
116 mcv_slave = (self.
slave_id, torque_enable)
117 self.dxl_io.set_multi_torque_enabled([mcv_master, mcv_slave])
126 mcv_slave = (self.
slave_id, -mcv_master[1])
127 self.dxl_io.set_multi_speed([mcv_master, mcv_slave])
131 if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
132 elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE
133 mcv_master = (self.
master_id, slope, slope)
134 mcv_slave = (self.
slave_id, slope, slope)
135 self.dxl_io.set_multi_compliance_slopes([mcv_master, mcv_slave])
139 if margin < DXL_MIN_COMPLIANCE_MARGIN: margin = DXL_MIN_COMPLIANCE_MARGIN
140 elif margin > DXL_MAX_COMPLIANCE_MARGIN: margin = DXL_MAX_COMPLIANCE_MARGIN
141 else: margin = int(margin)
142 mcv_master = (self.
master_id, margin, margin)
143 mcv_slave = (self.
slave_id, margin, margin)
144 self.dxl_io.set_multi_compliance_margins([mcv_master, mcv_slave])
148 if punch < DXL_MIN_PUNCH: punch = DXL_MIN_PUNCH
149 elif punch > DXL_MAX_PUNCH: punch = DXL_MAX_PUNCH
150 else: punch = int(punch)
153 self.dxl_io.set_multi_punch([mcv_master, mcv_slave])
157 if max_torque > 1: max_torque = 1.0
158 elif max_torque < 0: max_torque = 0.0
159 raw_torque_val = int(DXL_MAX_TORQUE_TICK * max_torque)
160 mcv_master = (self.
master_id, raw_torque_val)
161 mcv_slave = (self.
slave_id, raw_torque_val)
162 self.dxl_io.set_multi_torque_limit([mcv_master, mcv_slave])
169 for state
in state_list.motor_states:
174 self.joint_state.motor_temps = [state.temperature, states[self.
slave_id].temperature]
177 self.joint_state.error = 0.0
179 self.joint_state.load = state.load
180 self.joint_state.is_moving = state.moving
181 self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)
def raw_to_rad(self, raw, initial_position_raw, flipped, radians_per_encoder_tick)
def set_compliance_slope(self, slope)
def __init__(self, dxl_io, controller_namespace, port_namespace)
def set_compliance_margin(self, margin)
def set_speed(self, speed)
def process_command(self, msg)
def set_torque_limit(self, max_torque)
def set_compliance_margin(self, margin)
master_initial_position_raw
def set_compliance_punch(self, punch)
def set_torque_enable(self, torque_enable)
def set_speed(self, speed)
def set_compliance_punch(self, punch)
def process_motor_states(self, state_list)
def set_torque_limit(self, max_torque)
def set_compliance_slope(self, slope)