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