sr_example: fancy_touch_demo.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2014 Shadow Robot Company Ltd.
00004 #
00005 # This program is free software: you can redistribute it and/or modify it
00006 # under the terms of the GNU General Public License as published by the Free
00007 # Software Foundation, either version 2 of the License, or (at your option)
00008 # any later version.
00009 #
00010 # This program is distributed in the hope that it will be useful, but WITHOUT
00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013 # more details.
00014 #
00015 # You should have received a copy of the GNU General Public License along
00016 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00017 #
00018 
00019 
00020 import rospy
00021 import random
00022 import time
00023 from sr_hand.shadowhand_commander import Commander
00024 rospy.init_node('receiver_example')
00025 c = Commander()
00026 
00027 ##########
00028 # RANGES #
00029 ##########
00030 
00031 # Minimum alllowed range for the joints in this particular script
00032 min_range = {"THJ2": -40, "THJ3": -12, "THJ4": 0, "THJ5": -55,
00033              "FFJ0": 20, "FFJ3": 0, "FFJ4": -20,
00034              "MFJ0": 20, "MFJ3": 0, "MFJ4": -10,
00035              "RFJ0": 20, "RFJ3": 0, "RFJ4": -10,
00036              "LFJ0": 20, "LFJ3": 0, "LFJ4": -20, "LFJ5": 0,
00037              "WRJ1": -20, "WRJ2": -10,
00038              "interpolation_time": 0.0}
00039 
00040 # Maximum alllowed range for the joints in this particular script
00041 max_range = {"THJ2": 20, "THJ3": 12, "THJ4": 70, "THJ5": 0,
00042              "FFJ0": 110, "FFJ3": 90, "FFJ4": 0,
00043              "MFJ0": 110, "MFJ3": 90, "MFJ4": 0,
00044              "RFJ0": 110, "RFJ3": 90, "RFJ4": 0,
00045              "LFJ0": 110, "LFJ3": 90, "LFJ4": 0, "LFJ5": 1,
00046              "WRJ1": 10, "WRJ2": 5,
00047              "interpolation_time": 4.0}
00048 
00049 
00050 
00051 ####################
00052 # POSE DEFINITIONS #
00053 ####################
00054 
00055 # starting position for the hand 
00056 start_pos = {"THJ1": 0, "THJ2": 0, "THJ3": 0, "THJ4": 0, "THJ5": 0,
00057                   "FFJ0": 0, "FFJ3": 0, "FFJ4": 0,
00058                   "MFJ0": 0, "MFJ3": 0, "MFJ4": 0,
00059                   "RFJ0": 0, "RFJ3": 0, "RFJ4": 0,
00060                   "LFJ0": 0, "LFJ3": 0, "LFJ4": 0, "LFJ5": 0,
00061                   "WRJ1": 0, "WRJ2": 0 }
00062 # Start position for the Hand
00063 pregrasp_pos = {"THJ2": 12, "THJ3": 15, "THJ4": 69, "THJ5": -23,
00064                 "FFJ0": 40, "FFJ3": 21, "FFJ4": -15,
00065                 "MFJ0": 40, "MFJ3": 21, "MFJ4": 0,
00066                 "RFJ0": 40, "RFJ3": 21, "RFJ4": -7,
00067                 "LFJ0": 40, "LFJ3": 21, "LFJ4": -10, "LFJ5": 0,
00068                 "WRJ1": 0, "WRJ2": 0}
00069 # Close position for the Hand
00070 grasp_pos = {"THJ2": 30, "THJ3": 15, "THJ4": 69, "THJ5": -3,
00071              "FFJ0": 77, "FFJ3": 67, "FFJ4": -19,
00072              "MFJ0": 82, "MFJ3": 62, "MFJ4": 0,
00073              "RFJ0": 89, "RFJ3": 64, "RFJ4": -18,
00074              "LFJ0": 97, "LFJ3": 64, "LFJ4": -19, "LFJ5": 0,
00075              "WRJ1": 0, "WRJ2": 0,
00076              "interpolation_time": 10.0}
00077 # Random position for the Hand (initialied at 0)
00078 rand_pos = {"THJ2": 0, "THJ3": 0, "THJ4": 0, "THJ5": 0,
00079             "FFJ0": 0, "FFJ3": 0, "FFJ4": 0,
00080             "MFJ0": 0, "MFJ3": 0, "MFJ4": 0,
00081             "RFJ0": 0, "RFJ3": 0, "RFJ4": 0,
00082             "LFJ0": 0, "LFJ3": 0, "LFJ4": 0, "LFJ5": 0,
00083             "WRJ1": 0, "WRJ2": 0,
00084             "interpolation_time": 0.0 }
00085 # flex first finger
00086 flex_ff = {"FFJ0": 180, "FFJ3": 90, "FFJ4": 0 }
00087 # extend first finger
00088 ext_ff = {"FFJ0": 0, "FFJ3": 0, "FFJ4": 0 }
00089 # flex middle finger
00090 flex_mf = {"MFJ0": 180, "MFJ3": 90, "MFJ4": 0 }
00091 # extend middle finger
00092 ext_mf = {"MFJ0": 0, "MFJ3": 0, "MFJ4": 0 }
00093 # flex ring finger
00094 flex_rf = {"RFJ0": 180, "RFJ3": 90, "RFJ4": 0 }
00095 # extend ring finger
00096 ext_rf = {"RFJ0": 0, "RFJ3": 0, "RFJ4": 0 }
00097 # flex little finger
00098 flex_lf = {"LFJ0": 180, "LFJ3": 90, "LFJ4": 0 }
00099 # extend middle finger
00100 ext_lf = {"LFJ0": 0, "LFJ3": 0, "LFJ4": 0 }
00101 # flex thumb step 1
00102 flex_th_1 = {"THJ1": 0, "THJ2": 0, "THJ3": 0, "THJ4": 70, "THJ5": 0 }
00103 # flex thumb step 2
00104 flex_th_2 = {"THJ1": 20, "THJ2": 40, "THJ3": 10, "THJ4": 70, "THJ5": 58 }
00105 # extend thumb step 1
00106 ext_th_1 = {"THJ1": 10, "THJ2": 20, "THJ3": 5, "THJ4": 35, "THJ5": 25 }
00107 # extend thumb step 2
00108 ext_th_2 = {"THJ1": 0, "THJ2": 0, "THJ3": 0, "THJ4": 0, "THJ5": 0 }
00109 # zero thumb
00110 zero_th = {"THJ1": 0, "THJ2": 0, "THJ3": 0, "THJ4": 0, "THJ5": 0 }
00111 # Pre O.K. with first finger
00112 pre_ff_ok = {"THJ4": 70 }
00113 # O.K. with first finger
00114 ff_ok = {"THJ1": 15, "THJ2": 20, "THJ3": 0, "THJ4": 56, "THJ5": 11,
00115          "FFJ0": 75, "FFJ3": 65, "FFJ4": -0.2,
00116          "MFJ0": 42, "MFJ3": 33, "MFJ4": -3,
00117          "RFJ0": 50, "RFJ3": 18, "RFJ4": 0.5,
00118          "LFJ0": 30, "LFJ3": 0, "LFJ4": -6, "LFJ5": 7 }
00119 # O.K. transition from first finger to middle finger
00120 ff2mf_ok = {"THJ1": 5, "THJ2": 12, "THJ3": -10, "THJ4": 60, "THJ5": 2,
00121             "FFJ0": 14, "FFJ3": 7, "FFJ4": -0.4,
00122             "MFJ0": 42, "MFJ3": 33, "MFJ4": -3,
00123             "RFJ0": 50, "RFJ3": 18, "RFJ4": 0.5,
00124             "LFJ0": 30, "LFJ3": 0, "LFJ4": -6, "LFJ5": 7 }
00125 # O.K. with middle finger
00126 mf_ok = {"THJ1": 15, "THJ2": 18, "THJ3": 7, "THJ4": 66, "THJ5": 23,
00127          "FFJ0": 14, "FFJ3": 7, "FFJ4": -0.4,
00128          "MFJ0": 88, "MFJ3": 63, "MFJ4": 11,
00129          "RFJ0": 50, "RFJ3": 18, "RFJ4": -10,
00130          "LFJ0": 30, "LFJ3": 0, "LFJ4": -6, "LFJ5": 7 }
00131 # O.K. transition from middle finger to ring finger
00132 mf2rf_ok = {"THJ1": 5, "THJ2": -5, "THJ3": 15, "THJ4": 70, "THJ5": 19,
00133             "FFJ0": 14, "FFJ3": 7, "FFJ4": -0.4,
00134             "MFJ0": 45, "MFJ3": 4, "MFJ4": -1,
00135             "RFJ0": 50, "RFJ3": 18, "RFJ4": -19,
00136             "LFJ0": 30, "LFJ3": 0, "LFJ4": -12, "LFJ5": 7 }
00137 # O.K. with ring finger
00138 rf_ok = {"THJ1": 15, "THJ2": 5, "THJ3": 15, "THJ4": 70, "THJ5": 42,
00139          "FFJ0": 14, "FFJ3": 7, "FFJ4": -0.4,
00140          "MFJ0": 45, "MFJ3": 4, "MFJ4": -1,
00141          "RFJ0": 103, "RFJ3": 52, "RFJ4": -19,
00142          "LFJ0": 30, "LFJ3": 0, "LFJ4": -12, "LFJ5": 7 }
00143 # O.K. transition from ring finger to little finger
00144 rf2lf_ok = {"THJ1": 5, "THJ2": 4.5, "THJ3": 8, "THJ4": 60, "THJ5": 21,
00145             "FFJ0": 14, "FFJ3": 7, "FFJ4": -0.4,
00146             "MFJ0": 45, "MFJ3": 4, "MFJ4": -1,
00147             "RFJ0": 30, "RFJ3": 6, "RFJ4": 0.5,
00148             "LFJ0": 30, "LFJ3": 0, "LFJ4": -10, "LFJ5": 7 }
00149 # O.K. with little finger
00150 lf_ok = {"THJ1": 30, "THJ2": 8, "THJ3": 10, "THJ4": 69, "THJ5": 26,
00151          "FFJ0": 14, "FFJ3": 7, "FFJ4": -0.4,
00152          "MFJ0": 15, "MFJ3": 4, "MFJ4": -1,
00153          "RFJ0": 15, "RFJ3": 6, "RFJ4": 0.5,
00154          "LFJ0": 96, "LFJ3": 19, "LFJ4": -7, "LFJ5": 45 }
00155 # zero wrist
00156 zero_wr = {"WRJ1": 0, "WRJ2": 0 }
00157 # north wrist
00158 n_wr = {"WRJ1": 15, "WRJ2": 0 }
00159 # south wrist
00160 s_wr = {"WRJ1": -20, "WRJ2": 0 }
00161 # east wrist
00162 e_wr = {"WRJ1": 0, "WRJ2": 8 }
00163 # west wrist
00164 w_wr = {"WRJ1": 0, "WRJ2": -14 }
00165 # northeast wrist
00166 ne_wr = {"WRJ1": 15, "WRJ2": 8 }
00167 # northwest wrist
00168 nw_wr = {"WRJ1": 15, "WRJ2": -14 }
00169 # southweast wrist
00170 sw_wr = {"WRJ1": -20, "WRJ2": -14 }
00171 # southeast wrist
00172 se_wr = {"WRJ1": -20, "WRJ2": 8 }
00173 # lateral lf ext side
00174 l_ext_lf = {"LFJ4": -15 }
00175 # lateral rf ext side
00176 l_ext_rf = {"RFJ4": -15 }
00177 # lateral mf ext side
00178 l_ext_mf = {"MFJ4": 15 }
00179 # lateral ff ext side
00180 l_ext_ff = {"FFJ4": 15 }
00181 # lateral all int side
00182 l_int_all = {"FFJ4": -15, "MFJ4": -15, "RFJ4": 15, "LFJ4": 15 }
00183 # lateral all ext side
00184 l_ext_all = {"FFJ4": 15, "MFJ4": 15, "RFJ4": -15, "LFJ4": -15 }
00185 # lateral ff int side
00186 l_int_ff = {"FFJ4": -15 }
00187 # lateral mf int side
00188 l_int_mf = {"MFJ4": -15 }
00189 # lateral rf int side
00190 l_int_rf = {"RFJ4": 15 }
00191 # lateral lf int side
00192 l_int_lf = {"LFJ4": 15 }
00193 # all zero
00194 l_zero_all = {"FFJ4": 0, "MFJ4": 0, "RFJ4": 0, "LFJ4": 0 }
00195 # spock
00196 l_spock = {"FFJ4": -20, "MFJ4": -20, "RFJ4": -20, "LFJ4": -20 }
00197 # grasp for shaking hands step 1
00198 shake_grasp_1 = {"THJ1": 0, "THJ2": 6, "THJ3": 10, "THJ4": 37, "THJ5": 9,
00199                  "FFJ0": 21, "FFJ3": 26, "FFJ4": 0,
00200                  "MFJ0": 18, "MFJ3": 24, "MFJ4": 0,
00201                  "RFJ0": 30, "RFJ3": 16, "RFJ4": 0,
00202                  "LFJ0": 30, "LFJ3": 0, "LFJ4": -10, "LFJ5": 10 }
00203 # grasp for shaking hands step 2
00204 shake_grasp_2 = {"THJ1": 21, "THJ2": 21, "THJ3": 10, "THJ4": 42, "THJ5": 21,
00205                  "FFJ0": 75, "FFJ3": 29, "FFJ4": 0,
00206                  "MFJ0": 75, "MFJ3": 41, "MFJ4": 0,
00207                  "RFJ0": 75, "RFJ3": 41, "RFJ4": 0,
00208                  "LFJ0": 100, "LFJ3": 41, "LFJ4": 0, "LFJ5": 0 }
00209 # store step 1 PST
00210 store_1_PST = {"THJ1": 0, "THJ2": 0, "THJ3": 0, "THJ4": 60, "THJ5": 0,
00211                "FFJ0": 180, "FFJ3": 90, "FFJ4": 0,
00212                "MFJ0": 180, "MFJ3": 90, "MFJ4": 0,
00213                "RFJ0": 180, "RFJ3": 90, "RFJ4": 0,
00214                "LFJ0": 180, "LFJ3": 90, "LFJ4": 0, "LFJ5": 0,
00215                "WRJ1": 0, "WRJ2": 0 }
00216 # store step 2 PST
00217 store_2_PST = {"THJ1": 50, "THJ2": 12, "THJ3": 0, "THJ4": 60, "THJ5": 27,
00218                "FFJ0": 180, "FFJ3": 90, "FFJ4": 0,
00219                "MFJ0": 180, "MFJ3": 90, "MFJ4": 0,
00220                "RFJ0": 180, "RFJ3": 90, "RFJ4": 0,
00221                "LFJ0": 180, "LFJ3": 90, "LFJ4": 0, "LFJ5": 0,
00222                "WRJ1": 0, "WRJ2": 0 }
00223 # store step 1 Bio_Tac
00224 store_1_BioTac = {"THJ1": 0, "THJ2": 0, "THJ3": 0, "THJ4": 30, "THJ5": 0,
00225                   "FFJ0": 180, "FFJ3": 90, "FFJ4": 0,
00226                   "MFJ0": 180, "MFJ3": 90, "MFJ4": 0,
00227                   "RFJ0": 180, "RFJ3": 90, "RFJ4": 0,
00228                   "LFJ0": 180, "LFJ3": 90, "LFJ4": 0, "LFJ5": 0,
00229                   "WRJ1": 0, "WRJ2": 0 }
00230 # store step 2 Bio_Tac
00231 store_2_BioTac = {"THJ1": 20, "THJ2": 36, "THJ3": 0, "THJ4": 30, "THJ5": -3,
00232                   "FFJ0": 180, "FFJ3": 90, "FFJ4": 0,
00233                   "MFJ0": 180, "MFJ3": 90, "MFJ4": 0,
00234                   "RFJ0": 180, "RFJ3": 90, "RFJ4": 0,
00235                   "LFJ0": 180, "LFJ3": 90, "LFJ4": 0, "LFJ5": 0,
00236                   "WRJ1": 0, "WRJ2": 0 }
00237 # store step 3
00238 store_3 = {"THJ1": 0, "THJ2": 0, "THJ3": 0, "THJ4": 65, "THJ5": 0 }
00239 
00240 
00241 ########################
00242 # FUNCTION DEFINITIONS #
00243 ########################
00244 
00245 def secuence_ff():   
00246    # Start secuence 1
00247    rospy.sleep(1)
00248    c.move_hand(store_3)
00249    rospy.sleep(1)
00250    c.move_hand(start_pos)
00251    rospy.sleep(1.5)
00252    c.move_hand(flex_ff)
00253    rospy.sleep(1.0)
00254    c.move_hand(ext_ff)
00255    rospy.sleep(1.0)
00256    c.move_hand(flex_mf)
00257    rospy.sleep(1.0)
00258    c.move_hand(ext_mf)
00259    rospy.sleep(1.0)
00260    c.move_hand(flex_rf)
00261    rospy.sleep(1.0)
00262    c.move_hand(ext_rf)
00263    rospy.sleep(1.0)
00264    c.move_hand(flex_lf)
00265    rospy.sleep(1.0)
00266    c.move_hand(ext_lf)
00267    rospy.sleep(1.0)
00268    c.move_hand(flex_th_1)
00269    rospy.sleep(0.7)
00270    tmp = flex_th_2.copy()
00271    tmp.update({'interpolation_time': 2.0})
00272    c.move_hand(tmp)
00273    
00274    while True:
00275       # Check  the state of the tactile senors
00276       read_tactile_values()
00277       # Record current joint positions
00278       hand_pos = c.get_hand_position()
00279       # If the tacticle sensor is triggered stop movement
00280       if ( tactile_values['TH'] > force_zero['TH'] ):
00281          c.move_hand(hand_pos)
00282          print 'Thumb contact'
00283          break
00284 
00285    rospy.sleep(1)
00286    c.move_hand(ext_th_2)
00287    rospy.sleep(0.5)
00288    c.move_hand(l_ext_lf)
00289    rospy.sleep(0.5)
00290    c.move_hand(l_ext_rf)
00291    rospy.sleep(0.5)
00292    c.move_hand(l_ext_mf)
00293    rospy.sleep(0.5)
00294    c.move_hand(l_ext_ff)
00295    rospy.sleep(0.5)
00296    c.move_hand(l_int_all)
00297    rospy.sleep(0.5)
00298    c.move_hand(l_ext_all)
00299    rospy.sleep(0.5)
00300    c.move_hand(l_int_ff)
00301    rospy.sleep(0.5)
00302    c.move_hand(l_int_mf)
00303    rospy.sleep(0.5)
00304    c.move_hand(l_int_rf)
00305    rospy.sleep(0.5)
00306    c.move_hand(l_int_lf)
00307    rospy.sleep(0.5)
00308    c.move_hand(l_zero_all)
00309    rospy.sleep(0.5)
00310    c.move_hand(l_spock)
00311    rospy.sleep(0.5)
00312    c.move_hand(l_zero_all)
00313    rospy.sleep(0.5)     
00314    c.move_hand(pre_ff_ok)
00315    rospy.sleep(1)
00316    tmp = ff_ok.copy()
00317    tmp.update({'interpolation_time': 2.0})
00318    c.move_hand(tmp)
00319    
00320    while True:
00321       # Check  the state of the tactile senors
00322       read_tactile_values()
00323       # Record current joint positions
00324       hand_pos = c.get_hand_position()
00325       # If the tacticle sensor is triggered stop movement
00326       if ( tactile_values['TH'] > force_zero['TH'] or tactile_values['FF'] > force_zero['FF'] ):
00327          c.move_hand(hand_pos)
00328          print 'First finger contact'
00329          break
00330 
00331    rospy.sleep(1)
00332    c.move_hand(ff2mf_ok)
00333    rospy.sleep(0.8)
00334    tmp = mf_ok.copy()
00335    tmp.update({'interpolation_time': 2.0})
00336    c.move_hand(tmp)
00337 
00338    while True:
00339       # Check  the state of the tactile senors
00340       read_tactile_values()
00341       # Record current joint positions
00342       hand_pos = c.get_hand_position()
00343       # If the tacticle sensor is triggered stop movement
00344       if ( tactile_values['TH'] > force_zero['TH'] or tactile_values['MF'] > force_zero['MF'] ):
00345          c.move_hand(hand_pos)
00346          print 'Middle finger contact'   
00347          break
00348 
00349    rospy.sleep(1)
00350    c.move_hand(mf2rf_ok)
00351    rospy.sleep(0.8)
00352    tmp = rf_ok.copy()
00353    tmp.update({'interpolation_time': 2.0})
00354    c.move_hand(tmp)
00355 
00356    while True:
00357       # Check  the state of the tactile senors
00358       read_tactile_values()
00359       # Record current joint positions
00360       hand_pos = c.get_hand_position()
00361       # If the tacticle sensor is triggered stop movement
00362       if ( tactile_values['TH'] > force_zero['TH'] or tactile_values['RF'] > force_zero['RF'] ):
00363          c.move_hand(hand_pos)
00364          print 'Ring finger contact'
00365          break
00366    
00367    rospy.sleep(1)
00368    c.move_hand(rf2lf_ok)
00369    rospy.sleep(0.8)
00370    tmp = lf_ok.copy()
00371    tmp.update({'interpolation_time': 2.0})
00372    c.move_hand(tmp)
00373    
00374    while True:
00375       # Check  the state of the tactile senors
00376       read_tactile_values()
00377       # Record current joint positions
00378       hand_pos = c.get_hand_position()
00379       # If the tacticle sensor is triggered stop movement
00380       if ( tactile_values['TH'] > force_zero['TH'] or tactile_values['LF'] > force_zero['LF'] ):
00381          c.move_hand(hand_pos)
00382          print 'Little finger contact'
00383          break
00384 
00385    rospy.sleep(1)
00386    c.move_hand(start_pos)
00387    rospy.sleep(1.5)
00388    c.move_hand(flex_ff)
00389    rospy.sleep(0.4)
00390    c.move_hand(flex_mf)
00391    rospy.sleep(0.4)
00392    c.move_hand(flex_rf)
00393    rospy.sleep(0.4)
00394    c.move_hand(flex_lf)
00395    rospy.sleep(0.4)
00396    c.move_hand(ext_ff)
00397    rospy.sleep(0.4)
00398    c.move_hand(ext_mf)
00399    rospy.sleep(0.4)
00400    c.move_hand(ext_rf)
00401    rospy.sleep(0.4)
00402    c.move_hand(ext_lf)
00403    rospy.sleep(0.4)
00404    c.move_hand(flex_ff)
00405    rospy.sleep(0.4)
00406    c.move_hand(flex_mf)
00407    rospy.sleep(0.4)
00408    c.move_hand(flex_rf)
00409    rospy.sleep(0.4)
00410    c.move_hand(flex_lf)
00411    rospy.sleep(0.4)
00412    c.move_hand(ext_ff)
00413    rospy.sleep(0.4)
00414    c.move_hand(ext_mf)
00415    rospy.sleep(0.4)
00416    c.move_hand(ext_rf)
00417    rospy.sleep(0.4)
00418    c.move_hand(ext_lf)
00419    rospy.sleep(0.4)
00420    c.move_hand(flex_ff)
00421    rospy.sleep(0.4)
00422    c.move_hand(flex_mf)
00423    rospy.sleep(0.4)
00424    c.move_hand(flex_rf)
00425    rospy.sleep(0.4)
00426    c.move_hand(flex_lf)
00427    rospy.sleep(0.4)
00428    c.move_hand(ext_ff)
00429    rospy.sleep(0.4)
00430    c.move_hand(ext_mf)
00431    rospy.sleep(0.4)
00432    c.move_hand(ext_rf)
00433    rospy.sleep(0.4)
00434    c.move_hand(ext_lf)
00435    rospy.sleep(1.5)
00436    c.move_hand(pre_ff_ok)
00437    rospy.sleep(1)
00438    tmp = ff_ok.copy()
00439    tmp.update({'interpolation_time': 3.0})
00440    c.move_hand(tmp)
00441    
00442    while True:
00443       # Check  the state of the tactile senors
00444       read_tactile_values()
00445       # Record current joint positions
00446       hand_pos = c.get_hand_position()
00447       # If the tacticle sensor is triggered stop movement
00448       if ( tactile_values['TH'] > force_zero['TH'] or tactile_values['FF'] > force_zero['FF'] ):
00449          c.move_hand(hand_pos)
00450          print 'First finger contact'
00451          break
00452 
00453    rospy.sleep(1.5)
00454    c.move_hand(ne_wr)
00455    rospy.sleep(1.4)
00456    c.move_hand(nw_wr)
00457    rospy.sleep(1.4)
00458    c.move_hand(sw_wr)
00459    rospy.sleep(1.4)
00460    c.move_hand(se_wr)
00461    rospy.sleep(1.4)
00462    c.move_hand(ne_wr)
00463    rospy.sleep(0.7)
00464    c.move_hand(nw_wr)
00465    rospy.sleep(0.7)
00466    c.move_hand(sw_wr)
00467    rospy.sleep(0.7)
00468    c.move_hand(se_wr)
00469    rospy.sleep(0.7)
00470    c.move_hand(zero_wr)
00471    rospy.sleep(0.4)
00472    c.move_hand(start_pos)
00473    rospy.sleep(1.5)    
00474    return
00475 
00476 def secuence_mf():
00477    # Start the secuence 2
00478    rospy.sleep(2.0)    
00479    # Initialize wake time
00480    wake_time = time.time()
00481 
00482    while True:
00483       # Check if any of the tactile senors have been triggered
00484       # If so, send the Hand to its start position
00485       read_tactile_values()
00486       if ( tactile_values['FF'] > force_zero['FF'] or
00487            tactile_values['MF'] > force_zero['MF'] or
00488            tactile_values['RF'] > force_zero['RF'] or
00489            tactile_values['LF'] > force_zero['LF'] or
00490            tactile_values['TH'] > force_zero['TH'] ):
00491 
00492          c.move_hand(start_pos)
00493          print 'HAND TOUCHED!'
00494          rospy.sleep(2.0)
00495 
00496          if ( tactile_values['TH'] > force_zero['TH'] ):
00497             break
00498  
00499       # If the tactile sensors have not been triggered and the Hand
00500       # is not in the middle of a movement, generate a random position
00501       # and interpolation time
00502       else:
00503          if time.time() > wake_time:
00504             for i in rand_pos:
00505                rand_pos[i] = random.randrange(min_range[i],max_range[i])
00506 
00507             rand_pos['FFJ4'] = random.randrange(min_range['FFJ4'],rand_pos['MFJ4'])
00508             rand_pos['LFJ4'] = random.randrange(min_range['LFJ4'],rand_pos['RFJ4'])
00509             rand_pos['interpolation_time'] = max_range['interpolation_time'] * random.random()
00510 
00511             c.move_hand(rand_pos)
00512             wake_time = time.time() + rand_pos['interpolation_time']*0.9
00513    return
00514 
00515 def secuence_rf():
00516    # Start the secuence 3
00517    rospy.sleep(0.5)
00518    c.move_hand(start_pos)
00519    rospy.sleep(1.5)
00520    c.move_hand(shake_grasp_1)
00521    rospy.sleep(2.5)
00522    c.move_hand(shake_grasp_2)
00523    rospy.sleep(1)
00524    c.move_hand(e_wr)
00525    rospy.sleep(0.4)
00526    c.move_hand(w_wr)
00527    rospy.sleep(0.4)
00528    c.move_hand(zero_wr)
00529    rospy.sleep(0.8)
00530    c.move_hand(shake_grasp_1)
00531    rospy.sleep(1.5)
00532    c.move_hand(start_pos)
00533    rospy.sleep(1.5)   
00534    return
00535 
00536 def secuence_lf():
00537    # Start the secuence 4
00538    # Trigger flag array
00539    trigger = [0,0,0,0,0]
00540 
00541    # Move Hand to zero position
00542    c.move_hand(start_pos)
00543    rospy.sleep(2.0)
00544 
00545    # Move Hand to starting position
00546    c.move_hand(pregrasp_pos)
00547    rospy.sleep(2.0)
00548 
00549    # Move Hand to close position
00550    c.move_hand(grasp_pos)
00551    offset1 = 3
00552 
00553    # Initialize end time
00554    end_time = time.time() + 11
00555 
00556    while True:
00557       # Check  the state of the tactile senors
00558       read_tactile_values()
00559 
00560       # Record current joint positions
00561       hand_pos = c.get_hand_position()
00562 
00563       # If any tacticle sensor has been triggered, send
00564       # the corresponding digit to its current position
00565       if ( tactile_values['FF'] > force_zero['FF'] and trigger[0] == 0 ):
00566          c.move_hand({"FFJ0": hand_pos['FFJ0'] + offset1, "FFJ3": hand_pos['FFJ3'] + offset1})
00567          print 'First finger contact'
00568          trigger[0] = 1
00569 
00570       if ( tactile_values['MF'] > force_zero['MF'] and trigger[1] == 0 ):
00571          c.move_hand({"MFJ0": hand_pos['MFJ0'] + offset1, "MFJ3": hand_pos['MFJ3'] + offset1})
00572          print 'Middle finger contact'
00573          trigger[1] = 1
00574 
00575       if ( tactile_values['RF'] > force_zero['RF'] and trigger[2] == 0 ):
00576          c.move_hand({"RFJ0": hand_pos['RFJ0'] + offset1, "RFJ3": hand_pos['RFJ3'] + offset1})
00577          print 'Ring finger contact'
00578          trigger[2] = 1
00579 
00580       if ( tactile_values['LF'] > force_zero['LF'] and trigger[3] == 0 ):
00581          c.move_hand({"LFJ0": hand_pos['LFJ0'] + offset1, "LFJ3": hand_pos['LFJ3'] + offset1})
00582          print 'Little finger contact'
00583          trigger[3] = 1
00584 
00585       if ( tactile_values['TH'] > force_zero['TH'] and trigger[4] == 0 ):
00586          c.move_hand({"THJ2": hand_pos['THJ2'] + offset1, "THJ5": hand_pos['THJ5'] + offset1 })
00587          print 'Thumb contact'
00588          trigger[4] = 1
00589 
00590       if ( trigger[0] == 1 and
00591            trigger[1] == 1 and
00592            trigger[2] == 1 and
00593            trigger[3] == 1 and
00594            trigger[4] == 1 ):
00595          break
00596 
00597       if time.time() > end_time:
00598          break
00599 
00600    # Send all joints to current position to compensate
00601    # for minor offsets created in the previous loop
00602    hand_pos = c.get_hand_position()
00603    c.move_hand(hand_pos)
00604    rospy.sleep(2.0)
00605 
00606    # Generate new values to squeeze object slightly
00607    offset2 = 3
00608    squeeze = hand_pos.copy()
00609    squeeze.update({"THJ5": hand_pos['THJ5'] + offset2, "THJ2": hand_pos['THJ2'] + offset2,
00610                    "FFJ3": hand_pos['FFJ3'] + offset2, "FFJ0": hand_pos['FFJ0'] + offset2,
00611                    "MFJ3": hand_pos['MFJ3'] + offset2, "MFJ0": hand_pos['MFJ0'] + offset2,
00612                    "RFJ3": hand_pos['RFJ3'] + offset2, "RFJ0": hand_pos['RFJ0'] + offset2,
00613                    "LFJ3": hand_pos['LFJ3'] + offset2, "LFJ0": hand_pos['LFJ0'] + offset2})
00614 
00615    # Squeeze object gently
00616    c.move_hand(squeeze)
00617    rospy.sleep(0.5)
00618    c.move_hand(hand_pos)
00619    rospy.sleep(0.5)
00620    c.move_hand(squeeze)
00621    rospy.sleep(0.5)
00622    c.move_hand(hand_pos)
00623    rospy.sleep(2.0)
00624    c.move_hand(pregrasp_pos)
00625    rospy.sleep(2.0)
00626    c.move_hand(start_pos)
00627    rospy.sleep(2.0)
00628 
00629    return
00630 
00631 def secuence_th():
00632    # Start the secuence 5
00633    rospy.sleep(0.5)
00634    c.move_hand(start_pos)
00635    rospy.sleep(1.5)
00636    return
00637 
00638 def zero_tactile_sensors():
00639    # Move Hand to zero position
00640    rospy.sleep(0.5)
00641    c.move_hand(start_pos)
00642 
00643    print '\n\nPLEASE ENSURE THAT THE TACTILE SENSORS ARE NOT PRESSED\n'
00644    #raw_input ('Press ENTER to continue')
00645    rospy.sleep(1.0)
00646 
00647    for x in xrange (1,1000):
00648       # Read current state of tactile sensors to zero them
00649       read_tactile_values()
00650 
00651       if tactile_values['FF'] > force_zero['FF']:
00652          force_zero['FF'] = tactile_values['FF']
00653       if tactile_values['MF'] > force_zero['MF']:
00654          force_zero['MF'] = tactile_values['MF']
00655       if tactile_values['RF'] > force_zero['RF']:
00656          force_zero['RF'] = tactile_values['RF']
00657       if tactile_values['LF'] > force_zero['LF']:
00658          force_zero['LF'] = tactile_values['LF']
00659       if tactile_values['TH'] > force_zero['TH']:
00660          force_zero['TH'] = tactile_values['TH']
00661 
00662    force_zero['FF'] = force_zero['FF'] + 3
00663    force_zero['MF'] = force_zero['MF'] + 3
00664    force_zero['RF'] = force_zero['RF'] + 3
00665    force_zero['LF'] = force_zero['LF'] + 3
00666    force_zero['TH'] = force_zero['TH'] + 3
00667 
00668    print 'Force Zero', force_zero
00669 
00670    rospy.loginfo("\n\nOK, ready for the demo")
00671    
00672    print "\nPRESS ONE OF THE TACTILES TO START A DEMO"
00673    print "   FF: Standard Demo"
00674    print "   MF: Shy Hand Demo"
00675    print "   RF: Handshake Demo"
00676    print "   LF: Grasp Demo"
00677    print "   TH: Open Hand"
00678 
00679    return
00680 
00681 def read_tactile_values():
00682    # Read tactile type
00683    tactile_type = c.get_tactile_type()
00684    # Read current state of tactile sensors
00685    tactile_state = c.get_tactile_state()
00686 
00687    if tactile_type == "biotac":
00688       tactile_values['FF'] = tactile_state.tactiles[0].pdc
00689       tactile_values['MF'] = tactile_state.tactiles[1].pdc
00690       tactile_values['RF'] = tactile_state.tactiles[2].pdc
00691       tactile_values['LF'] = tactile_state.tactiles[3].pdc
00692       tactile_values['TH'] = tactile_state.tactiles[4].pdc
00693 
00694    elif tactile_type == "PST":
00695       tactile_values['FF'] = tactile_state.pressure[0]
00696       tactile_values['MF'] = tactile_state.pressure[1]
00697       tactile_values['RF'] = tactile_state.pressure[2]
00698       tactile_values['LF'] = tactile_state.pressure[3]
00699       tactile_values['TH'] = tactile_state.pressure[4]
00700 
00701    elif tactile_type == None:
00702       print "You don't have tactile sensors. Talk to your Shadow representative to purchase some"
00703 
00704    return
00705 
00706 ########
00707 # MAIN #
00708 ########
00709 
00710 # Zero values in dictionary for tactile sensors (initialized at 0)
00711 force_zero = {"FF": 0,"MF": 0,"RF": 0,"LF": 0,"TH": 0}
00712 # Initialize values for current tactile values
00713 tactile_values = {"FF": 0,"MF": 0,"RF": 0,"LF": 0,"TH": 0}
00714 # Zero tactile sensors
00715 zero_tactile_sensors()
00716 
00717 while not rospy.is_shutdown():
00718    # Check the state of the tactile senors
00719    read_tactile_values()
00720    
00721    # If the tactile is touched, trigger the corresponding function
00722    if (tactile_values['FF'] > force_zero['FF']):
00723       print 'First finger contact'
00724       secuence_ff()
00725       print 'FF demo completed'
00726       zero_tactile_sensors()
00727    if (tactile_values['MF'] > force_zero['MF']):
00728       print 'Middle finger contact'
00729       secuence_mf()
00730       print 'MF demo completed'
00731       zero_tactile_sensors()
00732    if (tactile_values['RF'] > force_zero['RF']):
00733       print 'Ring finger contact'
00734       secuence_rf()
00735       print 'RF demo completed'
00736       zero_tactile_sensors()
00737    if (tactile_values['LF'] > force_zero['LF']):
00738       print 'Little finger contact'
00739       secuence_lf()
00740       print 'LF demo completed'
00741       zero_tactile_sensors()
00742    if (tactile_values['TH'] > force_zero['TH']):
00743       print 'Thumb finger contact'
00744       secuence_th()
00745       print 'TH demo completed'
00746       zero_tactile_sensors()