dynamixel_controllers: joint_position_controller_dual

Go to the documentation of this file.
00001 # -*- coding: utf-8 -*-
00002 #
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2010-2011, Antons Rebguns.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of University of Arizona nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
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         # verify that the expected motor is connected and responding
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     # turn off motor torque
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