bhand_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2014, Robotnik Automation SLL
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Robotnik Automation SSL nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 import rospy 
00035 from pyHand_api import *
00036 
00037 from std_msgs.msg import String
00038 from bhand_controller.msg import State, TactileArray, Service
00039 from bhand_controller.srv import Actions, SetControlMode
00040 from sensor_msgs.msg import JointState
00041 
00042 import time, threading
00043 import math
00044 
00045 DEFAULT_FREQ = 100.0
00046 MAX_FREQ = 250.0
00047 
00048 
00049 CONTROL_MODE_POSITION = "POSITION"
00050 CONTROL_MODE_VELOCITY = "VELOCITY"
00051 
00052 WATCHDOG_VELOCITY = 0.1 # Max time (seconds) between velocity commands before stopping the velocity
00053 
00054 # class to save the info of each joint
00055 class JointHand:
00056         
00057         def __init__(self, id, name):
00058                 self.id = id
00059                 self.joint_name = name
00060                 self.desired_position = 0.0
00061                 self.desired_velocity = 0.0
00062                 self.real_position = 0.0
00063                 self.real_velocity = 0.0
00064                 
00065         
00066 # Class control the Barrett Hand
00067 class BHand:
00068         
00069         def __init__(self, args):
00070                 
00071                 self.port = args['port']
00072                 self.topic_state_name = args['topic_state']
00073                 self.desired_freq = args['desired_freq'] 
00074                 if self.desired_freq <= 0.0 or self.desired_freq > MAX_FREQ:
00075                         rospy.loginfo('%s::init: Desired freq (%f) is not possible. Setting desired_freq to %f'%(rospy.get_name(), self.desired_freq, DEFAULT_FREQ))
00076                         self.desired_freq = DEFAULT_FREQ
00077                 self.tactile_sensors = args['tactile_sensors'] 
00078                 self.real_freq = 0.0
00079                 
00080                 joint_ids_arg = args['joint_ids']
00081                 joint_names_arg = args['joint_names']
00082                 # Control mode
00083                 self.control_mode = args['control_mode']
00084                 if self.control_mode != CONTROL_MODE_POSITION and self.control_mode != CONTROL_MODE_VELOCITY:
00085                         rospy.loginfo('%s: init: setting control mode by default to %s'%(rospy.get_name(), CONTROL_MODE_POSITION))
00086                         self.control_mode = CONTROL_MODE_POSITION
00087                 
00088                 self.state = State.INIT_STATE
00089                 # flag to control the initialization of CAN device 
00090                 self.can_initialized = False
00091                 # flag to control the initialization of the hand
00092                 self.hand_initialized = False
00093                 # flag to control the initialization of ROS stuff
00094                 self.ros_initialized = False
00095                 # flag to control that the control loop is running
00096                 self.running = False
00097                 
00098                 self.time_sleep = 1.0
00099                 # BHand library object
00100                 self.hand = pyHand(self.port)
00101                 # Current state of the joints
00102                 self.joint_state = JointState()
00103                 # Msg to publish the tactile array
00104                 self.msg_tact_array = TactileArray()
00105                 # State msg to publish
00106                 self.msg_state = State()
00107                 # Timer to publish state
00108                 self.publish_state_timer = 1
00109                 # Flag active after receiving a new command
00110                 self.new_command = False
00111                 # Flag active for reading temperature
00112                 self.temp_command = False
00113                 # Used to read one temperature sensor every time
00114                 self.temp_current_sensor = 0
00115                 # Total number of temperature sensors 
00116                 self.temp_sensor_number = 8
00117                 # Frequency to read temperature
00118                 self.temp_read_timer = 5# seconds
00119                 # Counts the number of CAN errors before switching to FAILURE
00120                 self.can_errors = 0
00121                 # Max. number of CAN errors 
00122                 self.max_can_errors = 5
00123                 # Flag to perform a failure recover
00124                 self.failure_recover = False
00125                 # Timer to try the FAILURE RECOVER
00126                 self.failure_recover_timer = 5  
00127                 # Timer to save the last command 
00128                 self.timer_command = time.time()
00129                 self.watchdog_command = float(WATCHDOG_VELOCITY)
00130                 
00131                 # List where saving the required actions
00132                 self.actions_list = []
00133                 # Mode of the hand to perform the grasp
00134                 self.grasp_mode = Service.SET_GRASP_1
00135                 
00136                 #print 'Control mode = %s'%self.control_mode
00137                 
00138                 # Data structure to link assigned joint names with every real joint
00139                 # {'ID': ['joint_name', index], ...}
00140                 #self.joint_names = {'F1': ['j12_joint',  0], 'F1_TIP': ['j13_joint', 1] , 'F2': ['j22_joint', 2], 'F2_TIP': ['j23_joint', 3],  'F3': ['j32_joint', 4], 'F3_TIP': ['j33_joint', 5], 'SPREAD_1': ['j11_joint', 6], 'SPREAD_2': ['j21_joint', 7]}
00141                 # {'ID': ['joint_name'], ...}
00142                 #'j33_joint' -> F3-Fingertip
00143                 #'j32_joint' -> F3-Base
00144                 #
00145                 #'j11_joint' -> F1-Spread
00146                 #'j12_joint' -> F1-Base
00147                 #'j13_joint' -> F1-Fingertip
00148                 #
00149                 #'j21_joint' -> F2-Spread
00150                 #'j22_joint' -> F2-Base
00151                 #'j23_joint' -> F2-Fingertip
00152                 
00153                 self.joint_names = {}
00154                 
00155                 for i in range(len(joint_ids_arg)):
00156                         self.joint_names[joint_ids_arg[i]] = [joint_names_arg[i], i]
00157                 
00158                 print   self.joint_names
00159                 
00160                 # {'ID': [Puck_temp, motor_temp], ..}
00161                 self.temperatures = {'F1': [0.0,  0.0], 'F2': [0.0,  0.0], 'F3': [0.0,  0.0], 'SPREAD': [0.0,  0.0]}
00162                 
00163                 # Inits joint state (FOR PUBLISHING)
00164                 k = 0
00165                 for i in self.joint_names:
00166                         self.joint_state.name.append(self.joint_names[i][0])
00167                         self.joint_names[i][1] = k # Updates the index
00168                         k = k + 1
00169                         #self.joint_state.name = ['j33_joint', 'j32_joint', 'j21_joint', 'j22_joint', 'j23_joint', 'j11_joint', 'j12_joint', 'j13_joint']
00170                         self.joint_state.position.append(0.0)
00171                         self.joint_state.velocity.append(0.0)
00172                         self.joint_state.effort.append(0.0)
00173                 
00174                 #  Saves the desired position of the joints {'joint_name': value, ...}
00175                 self.desired_joints_position = {self.joint_names['F1'][0]: 0.0, self.joint_names['F2'][0]: 0.0, self.joint_names['F3'][0]: 0.0, self.joint_names['SPREAD_1'][0]: 0.0, self.joint_names['SPREAD_2'][0]: 0.0}
00176                 self.desired_joints_velocity = {self.joint_names['F1'][0]: 0.0, self.joint_names['F2'][0]: 0.0, self.joint_names['F3'][0]: 0.0, self.joint_names['SPREAD_1'][0]: 0.0, self.joint_names['SPREAD_2'][0]: 0.0}
00177                 # Saves the last sent velocity
00178                 self.sent_joints_velocity = {self.joint_names['F1'][0]: 0.0, self.joint_names['F2'][0]: 0.0, self.joint_names['F3'][0]: 0.0, self.joint_names['SPREAD_1'][0]: 0.0, self.joint_names['SPREAD_2'][0]: 0.0}
00179                 
00180                 self.t_publish_state = threading.Timer(self.publish_state_timer, self.publishROSstate)
00181                 self.t_read_temp = threading.Timer(self.temp_read_timer, self.readTemp)
00182                 
00183                 rospy.loginfo('%s: port %s, freq = %d, topic = %s, tactile_sensors = %s'%(rospy.get_name() , self.port, self.desired_freq, self.topic_state_name, self.tactile_sensors))
00184         
00185         
00186         def setup(self):
00187                 '''
00188                         Initializes de hand
00189                         @return: True if OK, False otherwise
00190                 '''
00191                 if self.can_initialized or self.hand.initialize() == True:
00192                         self.can_initialized = True
00193                         return 0
00194                 else:
00195                         self.hand.can_uninit()
00196                         rospy.logerr('%s: Error initializing the hand'%rospy.get_name() )
00197                         return -1
00198                 
00199                 
00200         def rosSetup(self):
00201                 '''
00202                         Creates and inits ROS components
00203                 '''
00204                 if self.ros_initialized:
00205                         return 0
00206                         
00207                 #self._enable_disable_service = rospy.Service('%s/enable_disable'%rospy.get_name(), enable_disable, self.enableDisable)
00208                 # PUBLISHERS
00209                 self._state_publisher = rospy.Publisher('%s/state'%rospy.get_name(), State, queue_size=5)
00210                 self._joints_publisher = rospy.Publisher('/joint_states', JointState, queue_size=5)
00211                 self._tact_array_publisher = rospy.Publisher('%s/tact_array'%rospy.get_name(), TactileArray, queue_size=5)
00212                 # SUBSCRIBERS
00213                 self._joints_subscriber = rospy.Subscriber('%s/command'%rospy.get_name(), JointState, self.commandCallback)
00214                 
00215                 # SERVICES
00216                 self._hand_service = rospy.Service('%s/actions'%rospy.get_name(), Actions, self.handActions)
00217                 self._set_control_mode_service = rospy.Service('%s/set_control_mode'%rospy.get_name(), SetControlMode, self.setControlModeServiceCb)
00218         
00219                 self.ros_initialized = True
00220                 
00221                 self.publishROSstate()
00222                 
00223                 return 0
00224                 
00225                 
00226         def shutdown(self):
00227                 '''
00228                         Shutdowns device
00229                         @return: 0 if it's performed successfully, -1 if there's any problem or the component is running
00230                 '''
00231                 if self.running or not self.can_initialized:
00232                         return -1
00233                 rospy.loginfo('%s::shutdown'%rospy.get_name())
00234                 self.setControlMode('POSITION')
00235                 # Performs Hand shutdown
00236                 self.hand.can_uninit()
00237                 # Cancels current timers
00238                 self.t_publish_state.cancel()
00239                 self.t_read_temp.cancel()
00240                 
00241                 
00242                 self.can_initialized = False
00243                 self.hand_initialized = False
00244                 
00245                 return 0
00246         
00247         
00248         def rosShutdown(self):
00249                 '''
00250                         Shutdows all ROS components
00251                         @return: 0 if it's performed successfully, -1 if there's any problem or the component is running
00252                 '''
00253                 if self.running or not self.ros_initialized:
00254                         return -1
00255                 
00256                 # Performs ROS topics & services shutdown
00257                 self._state_publisher.unregister()
00258                 self._joints_publisher.unregister()
00259                 self._tact_array_publisher.unregister()
00260                 self._joints_subscriber.unregister()
00261                 self._set_control_mode_service.shutdown()
00262                 self._hand_service.shutdown()
00263                 
00264                 self.ros_initialized = False
00265                 
00266                 return 0
00267                         
00268         
00269         def stop(self):
00270                 '''
00271                         Creates and inits ROS components
00272                 '''
00273                 self.running = False
00274                 
00275                 return 0
00276         
00277         
00278         def start(self):
00279                 '''
00280                         Runs ROS configuration and the main control loop
00281                         @return: 0 if OK
00282                 '''
00283                 self.rosSetup()
00284                 
00285                 if self.running:
00286                         return 0
00287                         
00288                 self.running = True
00289                 
00290                 self.controlLoop()
00291                 
00292                 return 0
00293         
00294         
00295         def controlLoop(self):
00296                 '''
00297                         Main loop of the component
00298                         Manages actions by state
00299                 '''
00300                 
00301                 while self.running and not rospy.is_shutdown():
00302                         t1 = time.time()
00303                         
00304                         if self.state == State.INIT_STATE:
00305                                 self.initState()
00306                                 
00307                         elif self.state == State.STANDBY_STATE:
00308                                 self.standbyState()
00309                                 
00310                         elif self.state == State.READY_STATE:
00311                                 self.readyState()
00312                                 
00313                         elif self.state == State.EMERGENCY_STATE:
00314                                 self.emergencyState()
00315                                 
00316                         elif self.state == State.FAILURE_STATE:
00317                                 self.failureState()
00318                                 
00319                         elif self.state == State.SHUTDOWN_STATE:
00320                                 self.shutdownState()
00321                                 
00322                         self.allState()
00323                         
00324                         t2 = time.time()
00325                         tdiff = (t2 - t1)
00326                         
00327                         
00328                         t_sleep = self.time_sleep - tdiff
00329                         #print 't_sleep = %f secs'%(t_sleep)
00330                         if t_sleep > 0.0:
00331                                 #print 't_sleep = %f secs'%(self.time_sleep)
00332                                 rospy.sleep(t_sleep)
00333                         
00334                         t3= time.time()
00335                         self.real_freq = 1.0/(t3 - t1)
00336                 
00337                 self.running = False
00338                 # Performs component shutdown
00339                 self.shutdownState()
00340                 # Performs ROS shutdown
00341                 self.rosShutdown()
00342                 rospy.loginfo('%s::controlLoop: exit control loop'%rospy.get_name())
00343                 
00344                 return 0
00345                 
00346         
00347         
00348         def rosPublish(self):
00349                 '''
00350                         Publish topics at standard frequency
00351                 '''
00352                 #self._state_publisher.publish(self.getState())
00353                 
00354                 
00355                 t = rospy.Time.now()
00356                 
00357                 self.joint_state.header.stamp = t
00358                 self._joints_publisher.publish(self.joint_state)
00359                 
00360                 if self.tactile_sensors:
00361                         self.msg_tact_array.header.stamp = t
00362                         self._tact_array_publisher.publish(self.msg_tact_array)
00363                         
00364                 return 0
00365                 
00366         
00367         
00368         def initState(self):
00369                 '''
00370                         Actions performed in init state
00371                 '''
00372                 error = 0
00373                 if not self.can_initialized:
00374                         ret = self.setup()
00375                         if ret == -1:
00376                                 error = 1
00377                                 
00378                 if self.hand_initialized and self.can_initialized:
00379                         self.switchToState(State.STANDBY_STATE)
00380                 
00381                 self.canError(error)
00382                 
00383                 return
00384         
00385         def standbyState(self):
00386                 '''
00387                         Actions performed in standby state
00388                 '''
00389                 #self.hand.close_grasp()
00390                 self.switchToState(State.READY_STATE)
00391                 
00392                 return
00393         
00394         
00395         def readyState(self):
00396                 '''
00397                         Actions performed in ready state
00398                 '''
00399                 f1_index = self.joint_names['F1'][1]
00400                 f2_index = self.joint_names['F2'][1]
00401                 f3_index = self.joint_names['F3'][1]
00402                 f1_tip_index = self.joint_names['F1_TIP'][1]
00403                 f2_tip_index = self.joint_names['F2_TIP'][1]
00404                 f3_tip_index = self.joint_names['F3_TIP'][1]
00405                 spread1_index = self.joint_names['SPREAD_1'][1]
00406                 spread2_index = self.joint_names['SPREAD_2'][1]
00407                 errors = 0
00408                 
00409                 
00410                 try:
00411                         # Reads position
00412                         self.hand.read_packed_position(SPREAD)
00413                         self.hand.read_packed_position(FINGER1)
00414                         self.hand.read_packed_position(FINGER2)
00415                         self.hand.read_packed_position(FINGER3)
00416                         
00417                         # Reads strain
00418                         self.hand.read_strain(FINGER1)
00419                         self.hand.read_strain(FINGER2)
00420                         self.hand.read_strain(FINGER3)
00421                         
00422                         # Reads temperature
00423                         if self.temp_command:
00424                                 self.hand.read_temp(FINGER1)
00425                                 self.hand.read_temp(FINGER2)
00426                                 self.hand.read_temp(FINGER3)
00427                                 self.hand.read_temp(SPREAD)
00428                                 self.hand.read_therm(FINGER1)
00429                                 self.hand.read_therm(FINGER2)
00430                                 self.hand.read_therm(FINGER3)
00431                                 self.hand.read_therm(SPREAD)
00432                                 self.initReadTempTimer()
00433                         
00434                         # Reads tactile sensor cells
00435                         if self.tactile_sensors:
00436                                 self.hand.read_full_tact(SPREAD)
00437                                 self.hand.read_full_tact(FINGER1)
00438                                 self.hand.read_full_tact(FINGER2)
00439                                 self.hand.read_full_tact(FINGER3)
00440                                 
00441                                 
00442                 except Exception, e:
00443                         rospy.logerr('%s::readyState: error getting info: %s'%(rospy.get_name(), e))
00444                         errors = errors + 1
00445                 
00446                 # Predefined actions
00447                 if len(self.actions_list) > 0:
00448                         action = self.actions_list[0]
00449                         # Removes action from list 
00450                         self.actions_list.remove(action)
00451                         
00452                         # Actions performed in CONTROL POSITION
00453                         if self.control_mode == CONTROL_MODE_VELOCITY:
00454                                 self.setControlMode(CONTROL_MODE_POSITION)
00455                         
00456                         if action == Service.CLOSE_GRASP:
00457                                 self.closeFingers(3.14)
00458                         
00459                         if action == Service.CLOSE_HALF_GRASP:
00460                                 self.closeFingers(1.57)
00461                                 
00462                         elif action == Service.OPEN_GRASP:
00463                                 self.openFingers()
00464                                 
00465                         elif action == Service.SET_GRASP_1:
00466                                 if self.joint_state.position[f1_index] > 0.1 or self.joint_state.position[f2_index] > 0.1 or self.joint_state.position[f3_index] > 0.1:
00467                                         #rospy.logerr('BHand::ReadyState: Service SET_GRASP 1 cannot be performed. Rest of fingers have to be on zero position')
00468                                         self.openFingers()
00469                                         time.sleep(2.0)
00470                                 
00471                                 self.desired_joints_position['SPREAD_1'] = 0.0
00472                                 self.desired_joints_position['SPREAD_2'] = 0.0
00473                                 self.hand.move_to(SPREAD, self.hand.rad_to_enc(self.desired_joints_position['SPREAD_1'], BASE_TYPE), False)
00474                                 self.grasp_mode = action
00475                                         
00476                         elif action == Service.SET_GRASP_2:
00477                                 if self.joint_state.position[f1_index] > 0.1 or self.joint_state.position[f2_index] > 0.1 or self.joint_state.position[f3_index] > 0.1:
00478                                         #rospy.logerr('BHand::ReadyState: Service SET_GRASP 2 cannot be performed. Rest of fingers have to be on zero position')
00479                                         self.openFingers()
00480                                         time.sleep(2.0)
00481                                 #else:
00482                                 self.desired_joints_position['SPREAD_1'] = 3.14
00483                                 self.desired_joints_position['SPREAD_2'] = 3.14
00484                                 self.hand.move_to(SPREAD, self.hand.rad_to_enc(self.desired_joints_position['SPREAD_1'], BASE_TYPE), False)
00485                                 self.grasp_mode = action
00486                         
00487                 # NO pre-defined actions                        
00488                 else: 
00489                         if self.control_mode == CONTROL_MODE_POSITION:
00490                         
00491                                 # Moves joints to desired pos
00492                                 if self.new_command:
00493                                         try:
00494                                                 f1_joint = self.joint_names['F1'][0]            
00495                                                 if self.desired_joints_position[f1_joint] != self.joint_state.position[f1_index]:
00496                                                         #print 'moving joint %s to position  %f'%(self.joint_state.name[0], self.desired_joints_position['j12_joint'])
00497                                                         self.hand.move_to(FINGER1, self.hand.rad_to_enc(self.desired_joints_position[f1_joint], BASE_TYPE), False)
00498                                                 
00499                                                 f2_joint = self.joint_names['F2'][0]
00500                                                 if self.desired_joints_position[f2_joint] != self.joint_state.position[f2_index]:
00501                                                         #print 'moving joint %s to position  %f'%(self.joint_state.name[2], self.desired_joints_position['j22_joint'])
00502                                                         self.hand.move_to(FINGER2, self.hand.rad_to_enc(self.desired_joints_position[f2_joint], BASE_TYPE), False)
00503                                                         
00504                                                 f3_joint = self.joint_names['F3'][0]
00505                                                 if self.desired_joints_position[f3_joint] != self.joint_state.position[f3_index]:
00506                                                         #print 'moving joint %s to position  %f'%(self.joint_state.name[4], self.desired_joints_position['j32_joint'])
00507                                                         self.hand.move_to(FINGER3, self.hand.rad_to_enc(self.desired_joints_position[f3_joint], BASE_TYPE), False)
00508                                                         
00509                                                 spread1_joint = self.joint_names['SPREAD_1'][0]
00510                                                 spread2_joint = self.joint_names['SPREAD_2'][0]
00511                                                 if self.desired_joints_position[spread1_joint] != self.joint_state.position[spread1_index]:
00512                                                         #print 'moving joint %s to position  %f'%(self.joint_state.name[6], self.desired_joints_position['jspread_joint'])
00513                                                         self.hand.move_to(SPREAD, self.hand.rad_to_enc(self.desired_joints_position[spread1_joint], SPREAD_TYPE), False)
00514                                                 elif self.desired_joints_position[spread2_joint] != self.joint_state.position[spread2_index]:
00515                                                         #print 'moving joint %s to position  %f'%(self.joint_state.name[6], self.desired_joints_position['jspread_joint'])
00516                                                         self.hand.move_to(SPREAD, self.hand.rad_to_enc(self.desired_joints_position[spread2_joint], SPREAD_TYPE), False)
00517                                                 
00518                                         except Exception, e:
00519                                                 rospy.logerr('%s::readyState: error sending command: %s'%(rospy.get_name(), e))
00520                                                 errors = errors + 1
00521                                         
00522                                         self.new_command = False
00523                         
00524                         else:
00525                                 # VELOCITY CONTROL
00526                                 if ((time.time() - self.timer_command) >= self.watchdog_command):
00527                                         try:
00528                                                 #rospy.loginfo('BHand::readyState: Watchdog velocity')
00529                                                 self.desired_joints_velocity[self.joint_names['F1'][0]] = 0.0
00530                                                 self.desired_joints_velocity[self.joint_names['F2'][0]] = 0.0
00531                                                 self.desired_joints_velocity[self.joint_names['F3'][0]] = 0.0
00532                                                 self.desired_joints_velocity[self.joint_names['SPREAD_1'][0]] = 0.0
00533                                                 self.desired_joints_velocity[self.joint_names['SPREAD_2'][0]] = 0.0
00534                                                 self.setJointVelocity('F1')
00535                                                 self.setJointVelocity('F2')
00536                                                 self.setJointVelocity('F3')
00537                                                 self.setJointVelocity('SPREAD_1')
00538                                         except Exception, e:
00539                                                 rospy.logerr('%s::readyState: error sending command: %s'%(rospy.get_name(), e))
00540                                                 errors = errors + 1
00541                                 # Moves joints to desired pos
00542                                 if self.new_command:
00543                         
00544                                         try:
00545                                                 self.setJointVelocity('F1')
00546                                                 self.setJointVelocity('F2')
00547                                                 self.setJointVelocity('F3')
00548                                                 self.setJointVelocity('SPREAD_1')
00549                                                 
00550                                                 
00551                                         except Exception, e:
00552                                                 rospy.logerr('%s::readyState: error sending command: %s'%(rospy.get_name(), e))
00553                                                 errors = errors + 1
00554                                         
00555                                         self.new_command = False
00556                         
00557                 time.sleep(0.002)
00558                 
00559                 # Reads messages from CAN bus
00560                 if self.hand.process_can_messages() != 0:
00561                         # If it doesn't read any msg, counts as error
00562                         self.canError(1)
00563                 
00564                 # Updating variables
00565                 # Position
00566                 self.joint_state.position[f1_index], self.joint_state.position[f1_tip_index] = self.hand.get_packed_position(FINGER1)
00567                 self.joint_state.position[f2_index], self.joint_state.position[f2_tip_index] = self.hand.get_packed_position(FINGER2)
00568                 self.joint_state.position[f3_index], self.joint_state.position[f3_tip_index] = self.hand.get_packed_position(FINGER3)
00569                 self.joint_state.position[spread1_index], e = self.hand.get_packed_position(SPREAD)
00570                 self.joint_state.position[spread2_index] = self.joint_state.position[spread1_index]
00571                 
00572                 # Strain
00573                 self.joint_state.effort[f1_index] = self.joint_state.effort[f1_tip_index] = self.strain_to_nm(self.hand.get_strain(FINGER1))
00574                 self.joint_state.effort[f2_index] = self.joint_state.effort[f2_tip_index] = self.strain_to_nm(self.hand.get_strain(FINGER2))
00575                 self.joint_state.effort[f3_index] = self.joint_state.effort[f3_tip_index] = self.strain_to_nm(self.hand.get_strain(FINGER3))
00576                 
00577                 # Temperature
00578                 self.msg_state.temp_f1[1] = self.hand.get_temp(FINGER1)
00579                 self.msg_state.temp_f2[1] = self.hand.get_temp(FINGER2)
00580                 self.msg_state.temp_f3[1] = self.hand.get_temp(FINGER3)
00581                 self.msg_state.temp_spread[1] = self.hand.get_temp(SPREAD)
00582                 self.msg_state.temp_f1[0] = self.hand.get_therm(FINGER1)
00583                 self.msg_state.temp_f2[0] = self.hand.get_therm(FINGER2)
00584                 self.msg_state.temp_f3[0] = self.hand.get_therm(FINGER3)
00585                 self.msg_state.temp_spread[0] = self.hand.get_therm(SPREAD)
00586                 
00587                 # Tactile Sensors
00588                 self.msg_tact_array.finger1 = self.hand.get_full_tact(FINGER1)
00589                 self.msg_tact_array.finger2 = self.hand.get_full_tact(FINGER2)
00590                 self.msg_tact_array.finger3 = self.hand.get_full_tact(FINGER3)
00591                 self.msg_tact_array.palm = self.hand.get_full_tact(SPREAD)
00592                 
00593                 '''     e21, e22 = self.hand.get_packed_position(FINGER2, 1)
00594                         e31, e32 = self.hand.get_packed_position(FINGER3, 1)
00595                         espread, e_ = self.hand.get_packed_position(SPREAD, 1)
00596                         
00597                         self.joint_state.position[f2_index] = self.hand.enc_to_rad(e21, BASE_TYPE)
00598                         self.joint_state.position[f2_tip_index] = self.hand.enc_to_rad(e22, TIP_TYPE)
00599                         self.joint_state.position[f3_index] = self.hand.enc_to_rad(e31, BASE_TYPE)
00600                         self.joint_state.position[f3_tip_index] = self.hand.enc_to_rad(e32, TIP_TYPE)
00601                         self.joint_state.position[spread1_index] = self.joint_state.position[spread2_index] = self.hand.enc_to_rad(espread, SPREAD_TYPE)
00602                 '''
00603                 #print 'End loop'
00604                 # Check the CAN bus status
00605                 self.canError(errors)
00606                 
00607                 return
00608         
00609         
00610         def setJointVelocity(self, finger):
00611                 '''
00612                         Sets the joint velocity of the desired joint
00613                         Takes the velocity value from the attribute desired_joints_velocity
00614                         @param joint: finger of the hand (F1, F2, F3, SPREAD)
00615                         @type joint: string
00616                 '''
00617                 if self.joint_names.has_key(finger):
00618                         joint = self.joint_names[finger][0]     
00619                                                 
00620                         if self.desired_joints_velocity[joint] != self.sent_joints_velocity[joint]:
00621                                         
00622                                 value = self.hand.rad_to_enc(self.desired_joints_velocity[joint], BASE_TYPE) / 1000
00623                                 self.sent_joints_velocity[joint] = self.desired_joints_velocity[joint]  # Saves the last sent ref 
00624                                 #print 'Moving Joint %s at %f rad/s (%d conts/sec)'%(joint, self.desired_joints_velocity[joint], value)
00625                                 
00626                                 if finger == 'F1':
00627                                         self.hand.set_velocity(FINGER1, value)
00628                                 elif finger == 'F2':
00629                                         self.hand.set_velocity(FINGER2, value)
00630                                 elif finger == 'F3':
00631                                         self.hand.set_velocity(FINGER3, value)
00632                                 elif finger == 'SPREAD_1' or finger == 'SPREAD_2':
00633                                         self.hand.set_velocity(SPREAD, value)
00634                                                 
00635         
00636         def shutdownState(self):
00637                 '''
00638                         Actions performed in shutdown state 
00639                 '''
00640                 print 'shutdownState'
00641                 if self.shutdown() == 0:
00642                         self.switchToState(State.INIT_STATE)
00643                 
00644                 return
00645         
00646         def emergencyState(self):
00647                 '''
00648                         Actions performed in emergency state
00649                 '''
00650                 
00651                 return
00652         
00653         def failureState(self):
00654                 '''
00655                         Actions performed in failure state
00656                 '''
00657                 if self.failure_recover:
00658                         # Performs Hand shutdown
00659                         self.hand.can_uninit()
00660                         self.can_initialized = False
00661                         self.switchToState(State.INIT_STATE)
00662                         self.failure_recover = False
00663                         self.can_errors = 0
00664                         
00665                 return
00666         
00667         def switchToState(self, new_state):
00668                 '''
00669                         Performs the change of state
00670                 '''
00671                 if self.state != new_state:
00672                         self.state = new_state
00673                         rospy.loginfo('BHand::switchToState: %s', self.stateToString(self.state))
00674                         
00675                         if self.state == State.READY_STATE:
00676                                 # On ready state it sets the desired freq.
00677                                 self.time_sleep = 1.0 / self.desired_freq
00678                                 self.initReadTempTimer()
00679                         elif self.state == State.FAILURE_STATE:
00680                                 # On other states it set 1 HZ frequency
00681                                 self.time_sleep = 1.0
00682                                 self.initFailureRecoverTimer(self.failure_recover_timer)
00683                         else:
00684                                 # On other states it set 1 HZ frequency
00685                                 self.time_sleep = 1.0
00686                 
00687                 return
00688                 
00689         def allState(self):
00690                 '''
00691                         Actions performed in all states
00692                 '''
00693                 self.rosPublish()
00694                 
00695                 return
00696         
00697         
00698         def stateToString(self, state):
00699                 '''
00700                         @param state: state to convert
00701                         @type state: bhand_controller.msg.State
00702                         @returns the equivalent string of the state
00703                 '''
00704                 if state == State.INIT_STATE:
00705                         return 'INIT_STATE'
00706                                 
00707                 elif state == State.STANDBY_STATE:
00708                         return 'STANDBY_STATE'
00709                         
00710                 elif state == State.READY_STATE:
00711                         return 'READY_STATE'
00712                         
00713                 elif state == State.EMERGENCY_STATE:
00714                         return 'EMERGENCY_STATE'
00715                         
00716                 elif state == State.FAILURE_STATE:
00717                         return 'FAILURE_STATE'
00718                         
00719                 elif state == State.SHUTDOWN_STATE:
00720                         return 'SHUTDOWN_STATE'
00721                 else:
00722                         return 'UNKNOWN_STATE'
00723         
00724         
00725         def actionsToString(self, action):
00726                 '''
00727                         @param action: action to convert
00728                         @type state: bhand_controller.msg.Service
00729                         @returns the equivalent string of the action
00730                 '''
00731                 if action == Service.INIT_HAND:
00732                         return 'INIT_HAND'
00733                                 
00734                 elif action == Service.CLOSE_GRASP:
00735                         return 'CLOSE_GRASP'
00736                         
00737                 elif action == Service.OPEN_GRASP:
00738                         return 'OPEN_GRASP'
00739                         
00740                 elif action == Service.SET_GRASP_1:
00741                         return 'SET_GRASP_1'
00742                         
00743                 elif action == Service.SET_GRASP_2:
00744                         return 'SET_GRASP_2'
00745                 
00746                 elif action == Service.CLOSE_HALF_GRASP:
00747                         return 'CLOSE_HALF_GRASP'
00748                         
00749                 else:
00750                         return 'UNKNOWN_ACTION'
00751         
00752         
00753         def commandCallback(self, data):
00754                 '''
00755                         Function called when receive a new value
00756                         @param data: state to convert
00757                         @type data: sensor_msgs.JointState
00758                 '''
00759                 bingo = False
00760                 
00761                 for i in range(len(data.name)):
00762                         if self.desired_joints_position.has_key(data.name[i]):
00763                                 self.desired_joints_position[data.name[i]] = data.position[i]
00764                                 self.desired_joints_velocity[data.name[i]] = data.velocity[i]
00765                                 bingo = True
00766                                 
00767                 
00768                 if bingo:
00769                         self.new_command = True
00770                         self.timer_command = time.time()
00771                 #print 'commandCallback'
00772         
00773         
00774         def readTemp(self):
00775                 '''
00776                         Function periodically to enable the read of temperature
00777                 '''
00778                 
00779                 self.temp_current_sensor = self.temp_current_sensor + 1
00780                 
00781                 if self.temp_current_sensor > self.temp_sensor_number:
00782                         self.temp_current_sensor = 1
00783                 
00784                 self.temp_command = True
00785                 
00786                 
00787         def initReadTempTimer(self):
00788                 '''
00789                         Function to start the timer to read the temperature
00790                 '''
00791                 self.temp_command = False
00792                 
00793                 self.t_read_temp = threading.Timer(self.temp_read_timer, self.readTemp)
00794                 self.t_read_temp.start()
00795         
00796                 
00797         def publishROSstate(self):
00798                 '''
00799                         Publish the State of the component at the desired frequency
00800                 '''
00801                 self.msg_state.state = self.state
00802                 self.msg_state.state_description = self.stateToString(self.state)
00803                 self.msg_state.desired_freq = self.desired_freq
00804                 self.msg_state.real_freq = self.real_freq
00805                 self.msg_state.hand_initialized = self.hand_initialized 
00806                 self.msg_state.control_mode = self.control_mode 
00807                 self._state_publisher.publish(self.msg_state)
00808                 
00809                 self.t_publish_state = threading.Timer(self.publish_state_timer, self.publishROSstate)
00810                 self.t_publish_state.start()
00811         
00812         
00813         def strain_to_nm(self, x):
00814                 '''
00815                 Converts from raw encoder unit reading to Newton-meters.
00816 
00817                 @param x: value to convert
00818 
00819                 @returns nm: the torque value in Newton-meters. 
00820                 '''
00821                 p1 = 2.754e-10
00822                 p2 = -1.708e-06
00823                 p3 = 0.003764
00824                 p4 = -2.85
00825                 nm= p1*x**3 + p2*x**2 + p3*x + p4
00826                 
00827                 return nm
00828                 
00829                 
00830         def handActions(self, req):
00831                 '''
00832                 Handles the callback to Actions ROS service. Allows a set of predefined actions like init_hand, close_grasp, ...
00833 
00834                 @param req: action id to perform
00835                 @type req: bhand_controller.srv.Actions
00836 
00837                 @returns: True or false depending on the result
00838                 '''
00839                 if req.action == Service.INIT_HAND:
00840                         if self.state != State.INIT_STATE:
00841                                 rospy.logerr('%s::handActions: action INIT_HAND not allowed in state %s'%(rospy.get_name() ,self.stateToString(self.state)))
00842                                 
00843                                 return False
00844                         else:
00845                                 ret = self.hand.init_hand()
00846                                 if ret:
00847                                         self.hand_initialized = True
00848                                         rospy.loginfo('%s::handActions: INIT HAND service OK'%rospy.get_name() )
00849                                         return True
00850                                 else:
00851                                         rospy.logerr('%s::handActions: error on INIT_HAND service'%rospy.get_name() )
00852                                         # Set the max number of errors to perform the CAN reinitialization
00853                                         self.canError(self.max_can_errors + 1)
00854                                         return False
00855                         
00856                 if self.state != State.READY_STATE:
00857                         rospy.logerr('%s::handActions: action not allowed in state %s'%(rospy.get_name(), self.stateToString(self.state)))
00858                         return False
00859                 else:
00860                         rospy.loginfo('%s::handActions: Received new action %s'%(rospy.get_name(), self.actionsToString(req.action)))
00861                         self.actions_list.append(req.action)
00862                         
00863                 return True
00864 
00865 
00866         def canError(self, n):
00867                 '''
00868                 Manages the CAN errors ocurred
00869 
00870                 @param n: number of produced
00871                 @type n: int
00872 
00873                 @returns: True or false depending on the result
00874                 '''
00875                 
00876                 self.can_errors = self.can_errors + n   
00877                 
00878                 #rospy.loginfo('%s::canError: %d errors on CAN bus'%(rospy.get_name(),self.can_errors)) 
00879                 
00880                 if self.can_errors > self.max_can_errors:
00881                         rospy.logerr('%s::canError: Errors on CAN bus'%rospy.get_name())
00882                         self.switchToState(State.FAILURE_STATE)
00883         
00884         
00885         def initFailureRecoverTimer(self, timer = 5):
00886                 '''
00887                         Function to start the timer to recover from FAILURE STATE
00888                 '''
00889                 t = threading.Timer(timer, self.failureRecover)
00890                 t.start()
00891         
00892                 
00893         def failureRecover(self):               
00894                 '''
00895                         Sets the recovery flag TRUE. Function called after timer
00896                 '''
00897                 self.failure_recover = True
00898         
00899         
00900         def setControlModeServiceCb(self, req):
00901                 '''
00902                         Sets the hand control mode
00903                         @param req: Requested service
00904                         @type req: bhand_controller.srv.SetControlMode
00905                 '''
00906                 if self.state != State.READY_STATE:
00907                         rospy.logerr('%s:setControlModeServiceCb: bhand is not in a valid state to change the mode'%rospy.get_name() )
00908                         return False
00909                         
00910                 if req.mode != CONTROL_MODE_POSITION and req.mode != CONTROL_MODE_VELOCITY:
00911                         rospy.logerr('%s:setControlModeServiceCb: invalid control mode %s'%(rospy.get_name(), req.mode))
00912                         return False
00913                         
00914                 else:
00915                         if self.setControlMode(req.mode):
00916                                 rospy.loginfo('%s:setControlModeServiceCb: control mode %s set successfully'%(rospy.get_name(), req.mode))
00917                                 return True
00918                                 
00919                         else:
00920                                 return False
00921         
00922         
00923         def setControlMode(self, mode):         
00924                 '''
00925                         Configures the hand to work under the desired control mode
00926                         @param mode: new mode
00927                         @type mode: string
00928                 '''
00929                 if self.control_mode == mode:
00930                         return True
00931                                 
00932                 if mode == CONTROL_MODE_POSITION:
00933                         self.hand.set_mode(HAND_GROUP, 'IDLE')
00934                         
00935                         
00936                 elif mode == CONTROL_MODE_VELOCITY:
00937                         self.hand.set_mode(HAND_GROUP, 'VEL')
00938                         
00939                 self.control_mode = mode
00940         
00941         
00942         def openFingers(self):
00943                 '''
00944                         Opens all the fingers
00945                 '''             
00946                 self.desired_joints_position['F1'] = 0.0
00947                 self.desired_joints_position['F2'] = 0.0
00948                 self.desired_joints_position['F3'] = 0.0
00949                 self.hand.move_to(FINGER1, self.hand.rad_to_enc(self.desired_joints_position['F1'], BASE_TYPE), False)
00950                 self.hand.move_to(FINGER2, self.hand.rad_to_enc(self.desired_joints_position['F2'], BASE_TYPE), False)
00951                 self.hand.move_to(FINGER3, self.hand.rad_to_enc(self.desired_joints_position['F3'], BASE_TYPE), False)
00952                                 
00953         
00954         def closeFingers(self, value):
00955                 '''
00956                         Closes all the fingers
00957                 '''             
00958                 self.desired_joints_position['F1'] = value
00959                 self.desired_joints_position['F2'] = value
00960                 self.desired_joints_position['F3'] = value
00961                 self.hand.move_to(FINGER1, self.hand.rad_to_enc(self.desired_joints_position['F1'], BASE_TYPE), False)
00962                 self.hand.move_to(FINGER2, self.hand.rad_to_enc(self.desired_joints_position['F2'], BASE_TYPE), False)
00963                 self.hand.move_to(FINGER3, self.hand.rad_to_enc(self.desired_joints_position['F3'], BASE_TYPE), False)
00964                 
00965                 
00966                 
00967 def main():
00968 
00969         rospy.init_node("bhand_node")
00970         
00971         
00972         _name = rospy.get_name()
00973         
00974         arg_defaults = {
00975           'port':  '/dev/pcan32',
00976           'topic_state': 'state',
00977           'desired_freq': DEFAULT_FREQ,
00978           'tactile_sensors': True,
00979           'control_mode': 'POSITION',
00980           'joint_ids': [ 'F1', 'F1_TIP', 'F2', 'F2_TIP', 'F3', 'F3_TIP', 'SPREAD_1', 'SPREAD_2'],
00981           'joint_names': ['bh_j12_joint', 'bh_j13_joint', 'bh_j22_joint', 'bh_j23_joint', 'bh_j32_joint', 'bh_j31_joint', 'bh_j11_joint', 'bh_j21_joint']
00982         }
00983         
00984         args = {}
00985         
00986         for name in arg_defaults:
00987                 try:
00988                         if rospy.search_param(name): 
00989                                 args[name] = rospy.get_param('%s/%s'%(_name, name)) # Adding the name of the node, because the para has the namespace of the node
00990                         else:
00991                                 args[name] = arg_defaults[name]
00992                         #print name
00993                 except rospy.ROSException, e:
00994                         rospy.logerror('bhand_node: %s'%e)
00995                         
00996         #print args
00997         bhand_node = BHand(args)
00998         
00999         rospy.loginfo('bhand_node: starting')
01000 
01001         bhand_node.start()
01002 
01003 
01004 if __name__ == "__main__":
01005         main()


bhand_controller
Author(s): Román Navarro , Jorge Ariño
autogenerated on Sat Jun 8 2019 20:56:12