sr_ethercat_hand_config: demo

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2011 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 import roslib; roslib.load_manifest('sr_hand')
00020 import rospy
00021 
00022 import time, mutex, subprocess, math
00023 
00024 from sr_robot_msgs.msg import sendupdate, joint, Biotac, BiotacAll, ShadowPST
00025 from sensor_msgs.msg import *
00026 from std_msgs.msg import Float64
00027 
00028 #the threshold for pdc above which the tactile is considered "pressed"
00029 PDC_THRESHOLD = 2000
00030 #the threshold for the PSTs above which the tactile is considered "pressed"
00031 PST_THRESHOLD = 420
00032 
00033 class FancyDemo(object):
00034     # starting position for the hand (DON't use until reviewed. Should be executed in two movement sequences)
00035     start_pos_hand = [ joint(joint_name = "THJ1", joint_target = 0),
00036                        joint(joint_name = "THJ2", joint_target = 0),
00037                        joint(joint_name = "THJ3", joint_target = 0),
00038                        joint(joint_name = "THJ4", joint_target = 0),
00039                        joint(joint_name = "THJ5", joint_target = 0),
00040                        joint(joint_name = "FFJ0", joint_target = 0),
00041                        joint(joint_name = "FFJ3", joint_target = 0),
00042                        joint(joint_name = "FFJ4", joint_target = 0),
00043                        joint(joint_name = "MFJ0", joint_target = 0),
00044                        joint(joint_name = "MFJ3", joint_target = 0),
00045                        joint(joint_name = "MFJ4", joint_target = 0),
00046                        joint(joint_name = "RFJ0", joint_target = 0),
00047                        joint(joint_name = "RFJ3", joint_target = 0),
00048                        joint(joint_name = "RFJ4", joint_target = 0),
00049                        joint(joint_name = "LFJ0", joint_target = 0),
00050                        joint(joint_name = "LFJ3", joint_target = 0),
00051                        joint(joint_name = "LFJ4", joint_target = 0),
00052                        joint(joint_name = "LFJ5", joint_target = 0),
00053                        joint(joint_name = "WRJ1", joint_target = 0),
00054                        joint(joint_name = "WRJ2", joint_target = 0) ]
00055     # flex first finger
00056     flex_ff = [ joint(joint_name = "FFJ0", joint_target = 180),
00057                         joint(joint_name = "FFJ3", joint_target = 90),
00058                         joint(joint_name = "FFJ4", joint_target = 0) ]
00059     # extend first finger
00060     ext_ff = [ joint(joint_name = "FFJ0", joint_target = 0),
00061                    joint(joint_name = "FFJ3", joint_target = 0),
00062                    joint(joint_name = "FFJ4", joint_target = 0) ]
00063     # flex middle finger
00064     flex_mf = [ joint(joint_name = "MFJ0", joint_target = 180),
00065                         joint(joint_name = "MFJ3", joint_target = 90),
00066                         joint(joint_name = "MFJ4", joint_target = 0) ]
00067     # extend middle finger
00068     ext_mf = [ joint(joint_name = "MFJ0", joint_target = 0),
00069                    joint(joint_name = "MFJ3", joint_target = 0),
00070                    joint(joint_name = "MFJ4", joint_target = 0) ]
00071     # flex ring finger
00072     flex_rf = [ joint(joint_name = "RFJ0", joint_target = 180),
00073                         joint(joint_name = "RFJ3", joint_target = 90),
00074                         joint(joint_name = "RFJ4", joint_target = 0) ]
00075     # extend ring finger
00076     ext_rf = [ joint(joint_name = "RFJ0", joint_target = 0),
00077                    joint(joint_name = "RFJ3", joint_target = 0),
00078                    joint(joint_name = "RFJ4", joint_target = 0) ]
00079     # flex little finger
00080     flex_lf = [ joint(joint_name = "LFJ0", joint_target = 180),
00081                         joint(joint_name = "LFJ3", joint_target = 90),
00082                         joint(joint_name = "LFJ4", joint_target = 0) ]
00083     # extend little finger
00084     ext_lf = [ joint(joint_name = "LFJ0", joint_target = 0),
00085                    joint(joint_name = "LFJ3", joint_target = 0),
00086                    joint(joint_name = "LFJ4", joint_target = 0) ]
00087     # flex thumb step 1
00088     flex_th_1 = [ joint(joint_name = "THJ1", joint_target = 0),
00089                           joint(joint_name = "THJ2", joint_target = 0),
00090                           joint(joint_name = "THJ3", joint_target = 0),
00091                           joint(joint_name = "THJ4", joint_target = 70),
00092                           joint(joint_name = "THJ5", joint_target = 0) ]
00093     # flex thumb step 2
00094     flex_th_2 = [ joint(joint_name = "THJ1", joint_target = 70),
00095                           joint(joint_name = "THJ2", joint_target = 26),
00096                           joint(joint_name = "THJ3", joint_target = 0),
00097                           joint(joint_name = "THJ4", joint_target = 70),
00098                           joint(joint_name = "THJ5", joint_target = 50) ]
00099     # extend thumb step 1
00100     ext_th_1 = [ joint(joint_name = "THJ1", joint_target = 70),
00101                      joint(joint_name = "THJ2", joint_target = 15),
00102                      joint(joint_name = "THJ3", joint_target = 0),
00103                      joint(joint_name = "THJ4", joint_target = 70),
00104                      joint(joint_name = "THJ5", joint_target = 0) ]
00105     # extend thumb step 2
00106     ext_th_2 = [ joint(joint_name = "THJ1", joint_target = 0),
00107                      joint(joint_name = "THJ2", joint_target = 0),
00108                      joint(joint_name = "THJ3", joint_target = -12),
00109                      joint(joint_name = "THJ4", joint_target = 0),
00110                      joint(joint_name = "THJ5", joint_target = -50) ]
00111     # zero thumb
00112     zero_th = [ joint(joint_name = "THJ1", joint_target = 0),
00113                         joint(joint_name = "THJ2", joint_target = 0),
00114                         joint(joint_name = "THJ3", joint_target = 0),
00115                         joint(joint_name = "THJ4", joint_target = 0),
00116                         joint(joint_name = "THJ5", joint_target = 0) ]
00117 
00118     # Pre O.K. with first finger
00119     pre_ff_ok = [ joint(joint_name = "THJ4", joint_target = 50) ]
00120     # O.K. with first finger
00121     ff_ok = [ joint(joint_name = "FFJ0", joint_target = 93),
00122                   joint(joint_name = "FFJ3", joint_target = 37),
00123                   joint(joint_name = "FFJ4", joint_target = -0.2),
00124                   joint(joint_name = "MFJ0", joint_target = 42),
00125                   joint(joint_name = "MFJ3", joint_target = 33),
00126                   joint(joint_name = "MFJ4", joint_target = -3),
00127                   joint(joint_name = "RFJ0", joint_target = 50),
00128                   joint(joint_name = "RFJ3", joint_target = 18),
00129                   joint(joint_name = "RFJ4", joint_target = 0.5),
00130                   joint(joint_name = "LFJ0", joint_target = 30),
00131                   joint(joint_name = "LFJ3", joint_target = 0),
00132                   joint(joint_name = "LFJ4", joint_target = -6),
00133                   joint(joint_name = "LFJ5", joint_target = 7),       
00134                   joint(joint_name = "THJ1", joint_target = 40),
00135                   joint(joint_name = "THJ2", joint_target = 12),
00136                   joint(joint_name = "THJ3", joint_target = -10),
00137                   joint(joint_name = "THJ4", joint_target = 50),
00138                   joint(joint_name = "THJ5", joint_target = 11) ]
00139     # O.K. transition from first finger to middle finger
00140     ff2mf_ok = [ joint(joint_name = "FFJ0", joint_target = 13.6),
00141                      joint(joint_name = "FFJ3", joint_target = 7),
00142                      joint(joint_name = "FFJ4", joint_target = -0.4),
00143                      joint(joint_name = "MFJ0", joint_target = 42),
00144                      joint(joint_name = "MFJ3", joint_target = 33),
00145                      joint(joint_name = "MFJ4", joint_target = -3),
00146                      joint(joint_name = "RFJ0", joint_target = 50),
00147                      joint(joint_name = "RFJ3", joint_target = 18),
00148                      joint(joint_name = "RFJ4", joint_target = 0.5),
00149                      joint(joint_name = "LFJ0", joint_target = 30),
00150                      joint(joint_name = "LFJ3", joint_target = 0),
00151                      joint(joint_name = "LFJ4", joint_target = -6),
00152                      joint(joint_name = "LFJ5", joint_target = 7),            
00153                      joint(joint_name = "THJ1", joint_target = 40),
00154                      joint(joint_name = "THJ2", joint_target = 12),
00155                      joint(joint_name = "THJ3", joint_target = -10),
00156                      joint(joint_name = "THJ4", joint_target = 50),
00157                      joint(joint_name = "THJ5", joint_target = 2) ]
00158     # O.K. with middle finger
00159     mf_ok = [ joint(joint_name = "FFJ0", joint_target = 13.6),
00160                   joint(joint_name = "FFJ3", joint_target = 7),
00161                   joint(joint_name = "FFJ4", joint_target = -0.4),
00162                   joint(joint_name = "MFJ0", joint_target = 89),
00163                   joint(joint_name = "MFJ3", joint_target = 45),
00164                   joint(joint_name = "MFJ4", joint_target = 8),
00165                   joint(joint_name = "RFJ0", joint_target = 50),
00166                   joint(joint_name = "RFJ3", joint_target = 18),
00167                   joint(joint_name = "RFJ4", joint_target = 0.5),
00168                   joint(joint_name = "LFJ0", joint_target = 30),
00169                   joint(joint_name = "LFJ3", joint_target = 0),
00170                   joint(joint_name = "LFJ4", joint_target = -6),
00171                   joint(joint_name = "LFJ5", joint_target = 7),       
00172                   joint(joint_name = "THJ1", joint_target = 45),
00173                   joint(joint_name = "THJ2", joint_target = 7),
00174                   joint(joint_name = "THJ3", joint_target = -10),
00175                   joint(joint_name = "THJ4", joint_target = 66),
00176                   joint(joint_name = "THJ5", joint_target = 23) ]
00177     # O.K. transition from middle finger to ring finger
00178     mf2rf_ok = [ joint(joint_name = "FFJ0", joint_target = 13.6),
00179                  joint(joint_name = "FFJ3", joint_target = 7),
00180                      joint(joint_name = "FFJ4", joint_target = -0.4),
00181                      joint(joint_name = "MFJ0", joint_target = 45),
00182                      joint(joint_name = "MFJ3", joint_target = 3.7),
00183                  joint(joint_name = "MFJ4", joint_target = -1),
00184                      joint(joint_name = "RFJ0", joint_target = 50),
00185                      joint(joint_name = "RFJ3", joint_target = 18),
00186                  joint(joint_name = "RFJ4", joint_target = 0.5),
00187                  joint(joint_name = "LFJ0", joint_target = 30),
00188                      joint(joint_name = "LFJ3", joint_target = 0),
00189                      joint(joint_name = "LFJ4", joint_target = -6),
00190                  joint(joint_name = "LFJ5", joint_target = 7),        
00191                      joint(joint_name = "THJ1", joint_target = 45),
00192                      joint(joint_name = "THJ2", joint_target = 8),
00193                      joint(joint_name = "THJ3", joint_target = -9.2),
00194                      joint(joint_name = "THJ4", joint_target = 64),
00195                      joint(joint_name = "THJ5", joint_target = 13.2) ]
00196     # O.K. with ring finger
00197     rf_ok = [ joint(joint_name = "FFJ0", joint_target = 13.6),
00198               joint(joint_name = "FFJ3", joint_target = 7),
00199                   joint(joint_name = "FFJ4", joint_target = -0.4),
00200                   joint(joint_name = "MFJ0", joint_target = 45),
00201                   joint(joint_name = "MFJ3", joint_target = 3.7),
00202                   joint(joint_name = "MFJ4", joint_target = -1),
00203                   joint(joint_name = "RFJ0", joint_target = 90),
00204                   joint(joint_name = "RFJ3", joint_target = 48),
00205                   joint(joint_name = "RFJ4", joint_target = -20),
00206                   joint(joint_name = "LFJ0", joint_target = 30),
00207                   joint(joint_name = "LFJ3", joint_target = 0),
00208                   joint(joint_name = "LFJ4", joint_target = -6),
00209                   joint(joint_name = "LFJ5", joint_target = 7),       
00210                   joint(joint_name = "THJ1", joint_target = 44),
00211                   joint(joint_name = "THJ2", joint_target = 8),
00212                   joint(joint_name = "THJ3", joint_target = 15),
00213                   joint(joint_name = "THJ4", joint_target = 70),
00214                   joint(joint_name = "THJ5", joint_target = 27) ]
00215     # O.K. transition from ring finger to little finger
00216     rf2lf_ok = [ joint(joint_name = "FFJ0", joint_target = 13.6),
00217                  joint(joint_name = "FFJ3", joint_target = 7),
00218                      joint(joint_name = "FFJ4", joint_target = -0.4),
00219                      joint(joint_name = "MFJ0", joint_target = 45),
00220                      joint(joint_name = "MFJ3", joint_target = 3.7),
00221                      joint(joint_name = "MFJ4", joint_target = -1),
00222                      joint(joint_name = "RFJ0", joint_target = 74),
00223                      joint(joint_name = "RFJ3", joint_target = 6.5),
00224                      joint(joint_name = "RFJ4", joint_target = 0.5),
00225                      joint(joint_name = "LFJ0", joint_target = 30),
00226                      joint(joint_name = "LFJ3", joint_target = 0),
00227                      joint(joint_name = "LFJ4", joint_target = -6),
00228                      joint(joint_name = "LFJ5", joint_target = 7),            
00229                      joint(joint_name = "THJ1", joint_target = 42),
00230                      joint(joint_name = "THJ2", joint_target = 4.5),
00231                      joint(joint_name = "THJ3", joint_target = 7.7),
00232                      joint(joint_name = "THJ4", joint_target = 73),
00233                      joint(joint_name = "THJ5", joint_target = 21) ]
00234     # O.K. with little finger
00235     lf_ok = [ joint(joint_name = "FFJ0", joint_target = 13.6),
00236               joint(joint_name = "FFJ3", joint_target = 7),
00237                   joint(joint_name = "FFJ4", joint_target = -0.4),
00238                   joint(joint_name = "MFJ0", joint_target = 15),
00239                   joint(joint_name = "MFJ3", joint_target = 3.7),
00240                   joint(joint_name = "MFJ4", joint_target = -1),
00241                   joint(joint_name = "RFJ0", joint_target = 74),
00242                   joint(joint_name = "RFJ3", joint_target = 6.5),
00243                   joint(joint_name = "RFJ4", joint_target = 0.5),
00244                   joint(joint_name = "LFJ0", joint_target = 100),
00245                   joint(joint_name = "LFJ3", joint_target = 9),
00246                   joint(joint_name = "LFJ4", joint_target = -7.6),
00247                   joint(joint_name = "LFJ5", joint_target = 37),              
00248                   joint(joint_name = "THJ1", joint_target = 40),
00249                   joint(joint_name = "THJ2", joint_target = 10),
00250                   joint(joint_name = "THJ3", joint_target = 10),
00251                   joint(joint_name = "THJ4", joint_target = 68),
00252                   joint(joint_name = "THJ5", joint_target = 25) ]
00253     # zero wrist
00254     zero_wr = [ joint(joint_name = "WRJ1", joint_target = 0),
00255                         joint(joint_name = "WRJ2", joint_target = 0) ]
00256     # north wrist
00257     n_wr = [ joint(joint_name = "WRJ1", joint_target = 15),
00258                  joint(joint_name = "WRJ2", joint_target = 0) ]
00259     # south wrist
00260     s_wr = [ joint(joint_name = "WRJ1", joint_target = -20),
00261                  joint(joint_name = "WRJ2", joint_target = 0) ]
00262     # east wrist
00263     e_wr = [ joint(joint_name = "WRJ1", joint_target = 0),
00264              joint(joint_name = "WRJ2", joint_target = 8) ]
00265     # west wrist
00266     w_wr = [ joint(joint_name = "WRJ1", joint_target = 0),
00267              joint(joint_name = "WRJ2", joint_target = -14) ]
00268     # northeast wrist
00269     ne_wr = [ joint(joint_name = "WRJ1", joint_target = 15),
00270                   joint(joint_name = "WRJ2", joint_target = 8) ]
00271     # northwest wrist
00272     nw_wr = [ joint(joint_name = "WRJ1", joint_target = 15),
00273               joint(joint_name = "WRJ2", joint_target = -14) ]
00274     # southweast wrist
00275     sw_wr = [ joint(joint_name = "WRJ1", joint_target = -20),
00276                   joint(joint_name = "WRJ2", joint_target = -14) ]
00277     # southeast wrist
00278     se_wr = [ joint(joint_name = "WRJ1", joint_target = -20),
00279                   joint(joint_name = "WRJ2", joint_target = 8) ]
00280     # grasp for shaking hands step 1
00281     shake_grasp_1 = [ joint(joint_name = "THJ1", joint_target = 0),
00282                               joint(joint_name = "THJ2", joint_target = 6),
00283                               joint(joint_name = "THJ3", joint_target = 10),
00284                               joint(joint_name = "THJ4", joint_target = 37),
00285                               joint(joint_name = "THJ5", joint_target = 9),
00286                               joint(joint_name = "FFJ0", joint_target = 21),
00287                               joint(joint_name = "FFJ3", joint_target = 26),
00288                               joint(joint_name = "FFJ4", joint_target = 0),
00289                               joint(joint_name = "MFJ0", joint_target = 18),
00290                               joint(joint_name = "MFJ3", joint_target = 24),
00291                               joint(joint_name = "MFJ4", joint_target = 0),
00292                               joint(joint_name = "RFJ0", joint_target = 30),
00293                               joint(joint_name = "RFJ3", joint_target = 16),
00294                               joint(joint_name = "RFJ4", joint_target = 0),
00295                               joint(joint_name = "LFJ0", joint_target = 30),
00296                               joint(joint_name = "LFJ3", joint_target = 0),
00297                               joint(joint_name = "LFJ4", joint_target = -10),
00298                               joint(joint_name = "LFJ5", joint_target = 10) ]
00299     # grasp for shaking hands step 2
00300     shake_grasp_2 = [ joint(joint_name = "THJ1", joint_target = 21),
00301                               joint(joint_name = "THJ2", joint_target = 12),
00302                       joint(joint_name = "THJ3", joint_target = 10),
00303                       joint(joint_name = "THJ4", joint_target = 42),
00304                               joint(joint_name = "THJ5", joint_target = 21),
00305                       joint(joint_name = "FFJ0", joint_target = 75),
00306                               joint(joint_name = "FFJ3", joint_target = 29),
00307                       joint(joint_name = "FFJ4", joint_target = 0),
00308                               joint(joint_name = "MFJ0", joint_target = 85),
00309                       joint(joint_name = "MFJ3", joint_target = 29),
00310                               joint(joint_name = "MFJ4", joint_target = 0),
00311                               joint(joint_name = "RFJ0", joint_target = 75),
00312                               joint(joint_name = "RFJ3", joint_target = 41),
00313                               joint(joint_name = "RFJ4", joint_target = 0),
00314                               joint(joint_name = "LFJ0", joint_target = 100),
00315                               joint(joint_name = "LFJ3", joint_target = 41),
00316                               joint(joint_name = "LFJ4", joint_target = 0),
00317                               joint(joint_name = "LFJ5", joint_target = 0) ]
00318     # store step 1
00319     store_1 = [ joint(joint_name = "THJ1", joint_target = 0),
00320                     joint(joint_name = "THJ2", joint_target = 0),
00321                 joint(joint_name = "THJ3", joint_target = 0),
00322                     joint(joint_name = "THJ4", joint_target = 65),
00323                     joint(joint_name = "THJ5", joint_target = 0),
00324                     joint(joint_name = "FFJ0", joint_target = 180),
00325                     joint(joint_name = "FFJ3", joint_target = 90),
00326                     joint(joint_name = "FFJ4", joint_target = 0),
00327                     joint(joint_name = "MFJ0", joint_target = 180),
00328                     joint(joint_name = "MFJ3", joint_target = 90),
00329                     joint(joint_name = "MFJ4", joint_target = 0),
00330                     joint(joint_name = "RFJ0", joint_target = 180),
00331                     joint(joint_name = "RFJ3", joint_target = 90),
00332                     joint(joint_name = "RFJ4", joint_target = 0),
00333                     joint(joint_name = "LFJ0", joint_target = 180),
00334                     joint(joint_name = "LFJ3", joint_target = 90),
00335                     joint(joint_name = "LFJ4", joint_target = 0),
00336                     joint(joint_name = "LFJ5", joint_target = 0),
00337                     joint(joint_name = "WRJ1", joint_target = 0),
00338                     joint(joint_name = "WRJ2", joint_target = 0) ]
00339     # store step 2
00340     store_2 = [ joint(joint_name = "THJ1", joint_target = 50),
00341                     joint(joint_name = "THJ2", joint_target = 13),
00342                     joint(joint_name = "THJ3", joint_target = 11),
00343                     joint(joint_name = "THJ4", joint_target = 65),
00344                     joint(joint_name = "THJ5", joint_target = 27),
00345                     joint(joint_name = "FFJ0", joint_target = 180),
00346                     joint(joint_name = "FFJ3", joint_target = 90),
00347                     joint(joint_name = "FFJ4", joint_target = 0),
00348                     joint(joint_name = "MFJ0", joint_target = 180),
00349                     joint(joint_name = "MFJ3", joint_target = 90),
00350                     joint(joint_name = "MFJ4", joint_target = 0),
00351                     joint(joint_name = "RFJ0", joint_target = 180),
00352                     joint(joint_name = "RFJ3", joint_target = 90),
00353                     joint(joint_name = "RFJ4", joint_target = 0),
00354                     joint(joint_name = "LFJ0", joint_target = 180),
00355                     joint(joint_name = "LFJ3", joint_target = 90),
00356                     joint(joint_name = "LFJ4", joint_target = 0),
00357                     joint(joint_name = "LFJ5", joint_target = 0),
00358                     joint(joint_name = "WRJ1", joint_target = 0),
00359                     joint(joint_name = "WRJ2", joint_target = 0) ]
00360     # store step 3
00361     store_3 = [ joint(joint_name = "THJ1", joint_target = 0),
00362                     joint(joint_name = "THJ2", joint_target = 0),
00363                     joint(joint_name = "THJ3", joint_target = 0),
00364                     joint(joint_name = "THJ4", joint_target = 65),
00365                     joint(joint_name = "THJ5", joint_target = 0)]
00366     # business card pre-zero position
00367     bc_pre_zero = [ joint(joint_name = "FFJ0", joint_target = 13.6),
00368                         joint(joint_name = "FFJ3", joint_target = 7),
00369                         joint(joint_name = "FFJ4", joint_target = -0.4),
00370                         joint(joint_name = "MFJ0", joint_target = 51.1),
00371                         joint(joint_name = "MFJ3", joint_target = 33),
00372                         joint(joint_name = "MFJ4", joint_target = 21),
00373                         joint(joint_name = "RFJ0", joint_target = 50),
00374                         joint(joint_name = "RFJ3", joint_target = 18),
00375                         joint(joint_name = "RFJ4", joint_target = -21),
00376                         joint(joint_name = "LFJ0", joint_target = 30),
00377                         joint(joint_name = "LFJ3", joint_target = 0),
00378                         joint(joint_name = "LFJ4", joint_target = -24),
00379                         joint(joint_name = "LFJ5", joint_target = 7),         
00380                         joint(joint_name = "THJ1", joint_target = 15.2),
00381                         joint(joint_name = "THJ2", joint_target = 7.4),
00382                         joint(joint_name = "THJ3", joint_target = -4),
00383                         joint(joint_name = "THJ4", joint_target = 50),
00384                         joint(joint_name = "THJ5", joint_target = -16.8) ]
00385     # business card zero position
00386     bc_zero = [ joint(joint_name = "FFJ0", joint_target = 13.6),
00387                     joint(joint_name = "FFJ3", joint_target = 7),
00388                     joint(joint_name = "FFJ4", joint_target = -0.4),
00389                     joint(joint_name = "MFJ0", joint_target = 51.1),
00390                     joint(joint_name = "MFJ3", joint_target = 32),
00391                     joint(joint_name = "MFJ4", joint_target = 21),
00392                     joint(joint_name = "RFJ0", joint_target = 50),
00393                     joint(joint_name = "RFJ3", joint_target = 18),
00394                     joint(joint_name = "RFJ4", joint_target = -21),
00395                     joint(joint_name = "LFJ0", joint_target = 30),
00396                     joint(joint_name = "LFJ3", joint_target = 0),
00397                     joint(joint_name = "LFJ4", joint_target = -24),
00398                     joint(joint_name = "LFJ5", joint_target = 7),             
00399                     joint(joint_name = "THJ1", joint_target = 17.2),
00400                     joint(joint_name = "THJ2", joint_target = 10.4),
00401                     joint(joint_name = "THJ3", joint_target = -4),
00402                     joint(joint_name = "THJ4", joint_target = 50),
00403                     joint(joint_name = "THJ5", joint_target = -13.6) ]
00404     # business card position 1
00405     bc_1 = [ joint(joint_name = "FFJ0", joint_target = 137),
00406                  joint(joint_name = "FFJ3", joint_target = 7) ]
00407     # business card position 2 #UNUSED
00408     bc_2 = [ joint(joint_name = "FFJ0", joint_target = 137),
00409                  joint(joint_name = "FFJ3", joint_target = 31) ]
00410     # business card position 3
00411     bc_3 = [ joint(joint_name = "FFJ0", joint_target = 137),
00412                  joint(joint_name = "FFJ3", joint_target = 58) ]
00413     # business card position 4
00414     bc_4 = [ joint(joint_name = "FFJ0", joint_target = 66),
00415                  joint(joint_name = "FFJ3", joint_target = 58) ]
00416     # business card position 5
00417     bc_5 = [ joint(joint_name = "FFJ0", joint_target = 180),
00418                  joint(joint_name = "FFJ3", joint_target = 58) ]
00419     # business card position 6
00420     bc_6 = [ joint(joint_name = "FFJ0", joint_target = 180),
00421                  joint(joint_name = "FFJ3", joint_target = 0) ]
00422     # business card position 7
00423     bc_7 = [ joint(joint_name = "FFJ0", joint_target = 0),
00424                  joint(joint_name = "FFJ3", joint_target = 0) ]
00425     # business card position 8
00426     bc_8 = [ joint(joint_name = "FFJ0", joint_target = 137),
00427                  joint(joint_name = "FFJ3", joint_target = 15) ]
00428     # business card position 9 #UNUSED
00429     bc_9 = [ joint(joint_name = "FFJ0", joint_target = 137),
00430                  joint(joint_name = "FFJ3", joint_target = 30) ]
00431     # business card position 10 #UNUSED
00432     bc_10 = [ joint(joint_name = "FFJ0", joint_target = 137),
00433                   joint(joint_name = "FFJ3", joint_target = 60) ]
00434     # business card position 11 #UNUSED
00435     bc_11 = [ joint(joint_name = "FFJ0", joint_target = 137),
00436                   joint(joint_name = "FFJ3", joint_target = 31) ]
00437     # business card position 12
00438     bc_12 = [ joint(joint_name = "FFJ0", joint_target = 137),
00439                   joint(joint_name = "FFJ3", joint_target = 58) ]
00440     # business card position 13
00441     bc_13 = [ joint(joint_name = "FFJ0", joint_target = 66),
00442                   joint(joint_name = "FFJ3", joint_target = 58) ]
00443     # business card position 14
00444     bc_14 = [ joint(joint_name = "MFJ3", joint_target = 64),
00445                   joint(joint_name = "FFJ4", joint_target = 20) ]
00446     # business card position 15
00447     bc_15 = [ joint(joint_name = "FFJ0", joint_target = 81 ),
00448                   joint(joint_name = "FFJ4", joint_target = 20),
00449                   joint(joint_name = "FFJ3", joint_target = 50),
00450               joint(joint_name = "THJ4", joint_target = 55),
00451                   joint(joint_name = "THJ5", joint_target = 20) ]
00452     # business card position 16
00453     bc_16 = [ joint(joint_name = "MFJ0", joint_target = 20),
00454                   joint(joint_name = "MFJ3", joint_target = 10),
00455                   joint(joint_name = "MFJ4", joint_target = 0) ]
00456 
00457     #A boolean used in this demo: set to true while an action is running
00458     # just so we don't do 2 actions at once
00459     action_running = mutex.mutex()
00460 
00461     def __init__(self):
00462         #A vector containing the different callbacks, in the same order
00463         # as the tactiles.
00464         self.fingers_pressed_functions = [self.ff_pressed, self.mf_pressed, self.rf_pressed,
00465                                           self.lf_pressed, self.th_pressed]
00466 
00467         #The hand publishers:
00468         # we use a dictionnary of publishers, because on the etherCAT hand
00469         # you have one publisher per controller.
00470         self.hand_publishers = self.create_hand_publishers()
00471 
00472         #send the start position to the hand
00473         #self.hand_publish(self.start_pos_hand)
00474 
00475         #wait for the node to be initialized and then go to the starting position
00476         time.sleep(1)
00477         rospy.loginfo("OK, ready for the demo")
00478 
00479         # We subscribe to the data being published by the biotac sensors.
00480         #self.sub_biotacs = rospy.Subscriber("/tactiles", BiotacAll, self.callback_biotacs, queue_size=1)
00481         #self.sub_psts    = rospy.Subscriber("/tactile", ShadowPST, self.callback_psts, queue_size=1)
00482 
00483     def create_hand_publishers(self):
00484         """
00485         Creates a dictionnary of publishers to send the targets to the controllers
00486         on /sh_??j?_mixed_position_velocity_controller/command
00487         """
00488         hand_pub = {}
00489 
00490         for joint in ["FFJ0", "FFJ3", "FFJ4",
00491                       "MFJ0", "MFJ3", "MFJ4",
00492                       "RFJ0", "RFJ3", "RFJ4",
00493                       "LFJ0", "LFJ3", "LFJ4", "LFJ5",
00494                       "THJ1", "THJ2", "THJ3", "THJ4", "THJ5",
00495                       "WRJ1", "WRJ2" ]:
00496             hand_pub[joint] = rospy.Publisher('sh_'+joint.lower()+'_position_controller/command', Float64, latch=True)
00497 
00498         return hand_pub
00499 
00500     def hand_publish(self, pose):
00501         """
00502         Publishes the given pose to the correct controllers for the hand.
00503         The targets are converted in radians.
00504         """
00505         for joint in pose:
00506             self.hand_publishers[joint.joint_name].publish( math.radians(joint.joint_target) )
00507 
00508     def callback_biotacs(self, msg):
00509         """
00510         The callback function for the biotacs. Checks if one of the fingers
00511         was pressed (filter the noise). If it is the case, call the
00512         corresponding function.
00513 
00514         @msg is the message containing the biotac data
00515         """
00516         #loop through the five tactiles
00517         for index,tactile in enumerate(msg.tactiles):
00518             #here we're just checking pdc (the pressure)
00519             # to see if a finger has been pressed, but you have
00520             # access to the whole data from the sensor
00521             # (look at sr_robot_msgs/msg/Biotac.msg)
00522             if tactile.pdc >= PDC_THRESHOLD:
00523                 # the tactile has been pressed, call the
00524                 # corresponding function
00525                 self.fingers_pressed_functions[index](tactile.pdc)
00526 
00527     def callback_psts(self, msg):
00528         """
00529         The callback function for the PSTs. Checks if one of the fingers
00530         was pressed (filter the noise). If it is the case, call the
00531         corresponding function.
00532 
00533         @msg is the message containing the biotac data
00534         """
00535         #loop through the five tactiles
00536         for index,tactile in enumerate(msg.pressure):
00537             #here we're just checking the pressure
00538             # to see if a finger has been pressed
00539             # 18456 is the value the PST takes when the sensor is not plugged in
00540             if tactile >= PST_THRESHOLD and tactile != 18456:
00541                 # the tactile has been pressed, call the
00542                 # corresponding function
00543                 self.fingers_pressed_functions[index](tactile)
00544 
00545     def ff_pressed(self,data):
00546         """
00547         The first finger was pressed.
00548         
00549         @param data: the pressure value (pdc)
00550         """
00551         #if we're already performing an action, don't do anything
00552         if not self.action_running.testandset():
00553             return
00554         
00555         #ok the finger sensor was pressed
00556         #p = subprocess.Popen('beep')
00557         
00558         rospy.loginfo("FF touched, running basic demo ")
00559         
00560         #send the start position to the hand
00561         self.hand_publish( self.start_pos_hand )
00562         time.sleep(1)
00563         self.hand_publish( self.flex_ff )
00564         time.sleep(1)
00565         self.hand_publish( self.ext_ff )
00566         time.sleep(1)
00567         self.hand_publish( self.flex_mf )
00568         time.sleep(1)
00569         self.hand_publish( self.ext_mf )
00570         time.sleep(1)
00571         self.hand_publish( self.flex_rf )
00572         time.sleep(1)
00573         self.hand_publish( self.ext_rf )
00574         time.sleep(1)
00575         self.hand_publish( self.flex_lf )
00576         time.sleep(1)
00577         self.hand_publish( self.ext_lf )
00578         time.sleep(1)
00579         self.hand_publish( self.flex_th_1 )
00580         time.sleep(1)
00581         self.hand_publish( self.flex_th_2 )
00582         time.sleep(1)
00583         self.hand_publish( self.ext_th_1 )
00584         time.sleep(1)
00585         self.hand_publish( self.ext_th_2 )
00586         time.sleep(1)
00587         self.hand_publish( self.n_wr )
00588         time.sleep(1)
00589         self.hand_publish( self.s_wr )
00590         time.sleep(1)
00591         self.hand_publish( self.zero_wr )
00592         time.sleep(1)
00593         self.hand_publish( self.e_wr )
00594         time.sleep(1)
00595         self.hand_publish( self.w_wr )
00596         time.sleep(1)
00597         self.hand_publish( self.zero_wr )
00598         time.sleep(1)
00599         self.hand_publish( self.pre_ff_ok )
00600         time.sleep(0.3)
00601         self.hand_publish( self.ff_ok )
00602         time.sleep(1)
00603         self.hand_publish( self.ff2mf_ok )
00604         time.sleep(1)
00605         self.hand_publish( self.mf_ok )
00606         time.sleep(1)
00607         self.hand_publish( self.mf2rf_ok )
00608         time.sleep(1)
00609         self.hand_publish( self.rf_ok )
00610         time.sleep(1)
00611         self.hand_publish( self.rf2lf_ok )
00612         time.sleep(1)
00613         self.hand_publish( self.lf_ok )
00614         time.sleep(1)
00615         self.hand_publish( self.start_pos_hand )
00616         time.sleep(1)
00617         self.hand_publish( self.flex_ff )
00618         time.sleep(0.2)
00619         self.hand_publish( self.flex_mf )
00620         time.sleep(0.2)
00621         self.hand_publish( self.flex_rf )
00622         time.sleep(0.2)
00623         self.hand_publish( self.flex_lf )
00624         time.sleep(0.2)
00625         self.hand_publish( self.ext_ff )
00626         time.sleep(0.2)
00627         self.hand_publish( self.ext_mf )
00628         time.sleep(0.2)
00629         self.hand_publish( self.ext_rf )
00630         time.sleep(0.2)
00631         self.hand_publish( self.ext_lf )
00632         time.sleep(0.2)
00633         self.hand_publish( self.flex_ff )
00634         time.sleep(0.2)
00635         self.hand_publish( self.flex_mf )
00636         time.sleep(0.2)
00637         self.hand_publish( self.flex_rf )
00638         time.sleep(0.2)
00639         self.hand_publish( self.flex_lf )
00640         time.sleep(0.2)
00641         self.hand_publish( self.ext_ff )
00642         time.sleep(0.2)
00643         self.hand_publish( self.ext_mf )
00644         time.sleep(0.2)
00645         self.hand_publish( self.ext_rf )
00646         time.sleep(0.2)
00647         self.hand_publish( self.ext_lf )
00648         time.sleep(0.2)
00649         self.hand_publish( self.flex_ff )
00650         time.sleep(0.2)
00651         self.hand_publish( self.flex_mf )
00652         time.sleep(0.2)
00653         self.hand_publish( self.flex_rf )
00654         time.sleep(0.2)
00655         self.hand_publish( self.flex_lf )
00656         time.sleep(0.2)
00657         self.hand_publish( self.ext_ff )
00658         time.sleep(0.2)
00659         self.hand_publish( self.ext_mf )
00660         time.sleep(0.2)
00661         self.hand_publish( self.ext_rf )
00662         time.sleep(0.2)
00663         self.hand_publish( self.ext_lf )
00664         time.sleep(1.0)
00665         self.hand_publish( self.pre_ff_ok )
00666         time.sleep(0.3)
00667         self.hand_publish( self.ff_ok )
00668         time.sleep(1)
00669         self.hand_publish( self.ne_wr )
00670         time.sleep(1.2)
00671         self.hand_publish( self.nw_wr )
00672         time.sleep(1.2)
00673         self.hand_publish( self.sw_wr )
00674         time.sleep(1.2)
00675         self.hand_publish( self.se_wr )
00676         time.sleep(1.2)
00677         self.hand_publish( self.ne_wr )
00678         time.sleep(0.4)
00679         self.hand_publish( self.nw_wr )
00680         time.sleep(0.4)
00681         self.hand_publish( self.sw_wr )
00682         time.sleep(0.4)
00683         self.hand_publish( self.se_wr )
00684         time.sleep(0.4)
00685         self.hand_publish( self.zero_wr )
00686         time.sleep(1)
00687         self.hand_publish( self.start_pos_hand )
00688         time.sleep(1)
00689         
00690         #wait before next possible action
00691         time.sleep(.2)
00692         self.action_running.unlock()
00693 
00694     def mf_pressed(self, data):
00695         """
00696         The middle finger was pressed.
00697         
00698         @param data: the pressure value (pdc)
00699         """
00700         #if we're already performing an action, don't do anything
00701         if not self.action_running.testandset():
00702             return
00703         
00704         #ok finger was pressed
00705         #p = subprocess.Popen('beep')
00706         
00707         rospy.loginfo("MF touched, running business card demo ")
00708         
00709         #wait 1s for the user to release the sensor
00710         time.sleep(.2)
00711         
00712         #send the start position to the hand
00713         self.hand_publish( self.store_3 )
00714         time.sleep(1)
00715         self.hand_publish( self.start_pos_hand )
00716         time.sleep(1)
00717         self.hand_publish( self.bc_pre_zero )
00718         time.sleep(2)
00719         self.hand_publish( self.bc_zero )
00720         time.sleep(3)
00721         self.hand_publish( self.bc_1 )
00722         time.sleep(1)
00723         #       self.hand_publish( self.bc_2 )
00724         #       time.sleep(1)
00725         self.hand_publish( self.bc_3 )
00726         time.sleep(1)
00727         self.hand_publish( self.bc_4 )
00728         time.sleep(1)
00729         self.hand_publish( self.bc_5 )
00730         time.sleep(1)
00731         self.hand_publish( self.bc_6 )
00732         time.sleep(1)
00733         self.hand_publish( self.bc_7 )
00734         time.sleep(1)
00735         self.hand_publish( self.bc_8 )
00736         time.sleep(1)
00737         #       self.hand_publish( self.bc_9 )
00738         #       time.sleep(1)
00739         #       self.hand_publish( self.bc_10 )
00740         #       time.sleep(1)
00741         #       self.hand_publish( self.bc_11 )
00742         #       time.sleep(1)
00743         self.hand_publish( self.bc_12 )
00744         time.sleep(1)
00745         self.hand_publish( self.bc_13 )
00746         time.sleep(1)
00747         #       self.hand_publish( self.bc_14 )
00748         #       time.sleep(1)
00749         self.hand_publish( self.bc_15 )
00750         time.sleep(1)
00751         self.hand_publish( self.bc_16 )
00752         time.sleep(3)
00753         self.hand_publish( self.start_pos_hand )
00754         
00755         #wait before next possible action
00756         time.sleep(3)
00757         self.action_running.unlock()
00758 
00759 
00760     def rf_pressed(self, data):
00761         """
00762         The ring finger was pressed.
00763         
00764         @param data: the pressure value (pdc)
00765         """
00766         #if we're already performing an action, don't do anything
00767         if not self.action_running.testandset():
00768             return
00769         
00770         #ok finger was pressed
00771         #p = subprocess.Popen('beep')
00772         
00773         rospy.loginfo("RF touched, running shaking hands demo.")
00774         
00775         #wait 1s for the user to release the sensor
00776         time.sleep(.2)
00777         
00778         #send the start position to the hand
00779         self.hand_publish( self.shake_grasp_1 )
00780         time.sleep(3)
00781         self.hand_publish( self.shake_grasp_2 )
00782         time.sleep(1)
00783         self.hand_publish( self.e_wr )
00784         time.sleep(0.33)
00785         self.hand_publish( self.w_wr )
00786         time.sleep(0.33)
00787         self.hand_publish( self.zero_wr )
00788         time.sleep(0.66)
00789         self.hand_publish( self.shake_grasp_1 )
00790         time.sleep(3)
00791         self.hand_publish( self.start_pos_hand )
00792         time.sleep(2)
00793         
00794         #wait before next possible action
00795         time.sleep(.2)
00796         self.action_running.unlock()
00797 
00798     def lf_pressed(self, data):
00799         """
00800         The little finger was pressed.
00801         
00802         @param data: the pressure value (pdc)
00803         """
00804         #if we're already performing an action, don't do anything
00805         if not self.action_running.testandset():
00806             return
00807         
00808         #ok finger pressed
00809         #p = subprocess.Popen('beep')
00810         
00811         rospy.loginfo("LF touched, going to store position.")
00812         
00813         #wait 1s for the user to release the sensor
00814         time.sleep(.2)
00815         
00816         #send the start position to the hand
00817         self.hand_publish( self.start_pos_hand )
00818         time.sleep(1)
00819         self.hand_publish( self.flex_lf )
00820         time.sleep(0.2)
00821         self.hand_publish( self.flex_rf )
00822         time.sleep(0.2)
00823         self.hand_publish( self.flex_mf )
00824         time.sleep(0.2)
00825         self.hand_publish( self.flex_ff )
00826         time.sleep(0.2)
00827         self.hand_publish( self.ext_lf )
00828         time.sleep(0.2)
00829         self.hand_publish( self.ext_rf )
00830         time.sleep(0.2)
00831         self.hand_publish( self.ext_mf )
00832         time.sleep(0.2)
00833         self.hand_publish( self.ext_ff )
00834         time.sleep(0.2)
00835         self.hand_publish( self.flex_lf )
00836         time.sleep(0.2)
00837         self.hand_publish( self.flex_rf )
00838         time.sleep(0.2)
00839         self.hand_publish( self.flex_mf )
00840         time.sleep(0.2)
00841         self.hand_publish( self.flex_ff )
00842         time.sleep(1)
00843         self.hand_publish( self.store_1 )
00844         time.sleep(1)
00845         self.hand_publish( self.store_2 )
00846         time.sleep(1)
00847         
00848         #wait before next possible action
00849         time.sleep(.2)
00850         self.action_running.unlock()
00851 
00852     def th_pressed(self, data):
00853         """
00854         The thumb was pressed.
00855 
00856         @param data: the pressure value (pdc)
00857         """
00858         #if we're already performing an action, don't do anything
00859         if not self.action_running.testandset():
00860             return
00861 
00862         #ok the finger was pressed
00863         #p = subprocess.Popen('beep')
00864 
00865         rospy.loginfo("TH touched, going to start position.")
00866 
00867         #wait 1s for the user to release the sensor
00868         time.sleep(.2)
00869 
00870         #send the thumb_up_position to the hand
00871         self.hand_publish( self.start_pos_hand )
00872 
00873         #wait before next possible action
00874         time.sleep(.2)
00875         self.action_running.unlock()
00876 
00877 def main():
00878     """
00879     The main function
00880     """
00881     # init the ros node
00882     rospy.init_node('fancy_touch_demo', anonymous=True)
00883 
00884     fancy_demo = FancyDemo()
00885 
00886     fancy_demo.mf_pressed(0)
00887 
00888     # subscribe until interrupted
00889     # rospy.spin()
00890 
00891 
00892 if __name__ == '__main__':
00893     main()
00894