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 = 3000
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 PST
00319     store_1_PST = [ 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 = 60),
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 PST
00340     store_2_PST = [ joint(joint_name = "THJ1", joint_target = 50),
00341                     joint(joint_name = "THJ2", joint_target = 12),
00342                     joint(joint_name = "THJ3", joint_target = 0),
00343                     joint(joint_name = "THJ4", joint_target = 60),
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 1 BioTac
00361     store_1_BioTac = [ 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 = 30),
00365                     joint(joint_name = "THJ5", joint_target = 0),
00366                     joint(joint_name = "FFJ0", joint_target = 180),
00367                     joint(joint_name = "FFJ3", joint_target = 90),
00368                     joint(joint_name = "FFJ4", joint_target = 0),
00369                     joint(joint_name = "MFJ0", joint_target = 180),
00370                     joint(joint_name = "MFJ3", joint_target = 90),
00371                     joint(joint_name = "MFJ4", joint_target = 0),
00372                     joint(joint_name = "RFJ0", joint_target = 180),
00373                     joint(joint_name = "RFJ3", joint_target = 90),
00374                     joint(joint_name = "RFJ4", joint_target = 0),
00375                     joint(joint_name = "LFJ0", joint_target = 180),
00376                     joint(joint_name = "LFJ3", joint_target = 90),
00377                     joint(joint_name = "LFJ4", joint_target = 0),
00378                     joint(joint_name = "LFJ5", joint_target = 0),
00379                     joint(joint_name = "WRJ1", joint_target = 0),
00380                     joint(joint_name = "WRJ2", joint_target = 0) ]
00381     # store step 2 BioTac
00382     store_2_BioTac = [ joint(joint_name = "THJ1", joint_target = 20),
00383                     joint(joint_name = "THJ2", joint_target = 36),
00384                     joint(joint_name = "THJ3", joint_target = 0),
00385                     joint(joint_name = "THJ4", joint_target = 30),
00386                     joint(joint_name = "THJ5", joint_target = -3),
00387                     joint(joint_name = "FFJ0", joint_target = 180),
00388                     joint(joint_name = "FFJ3", joint_target = 90),
00389                     joint(joint_name = "FFJ4", joint_target = 0),
00390                     joint(joint_name = "MFJ0", joint_target = 180),
00391                     joint(joint_name = "MFJ3", joint_target = 90),
00392                     joint(joint_name = "MFJ4", joint_target = 0),
00393                     joint(joint_name = "RFJ0", joint_target = 180),
00394                     joint(joint_name = "RFJ3", joint_target = 90),
00395                     joint(joint_name = "RFJ4", joint_target = 0),
00396                     joint(joint_name = "LFJ0", joint_target = 180),
00397                     joint(joint_name = "LFJ3", joint_target = 90),
00398                     joint(joint_name = "LFJ4", joint_target = 0),
00399                     joint(joint_name = "LFJ5", joint_target = 0),
00400                     joint(joint_name = "WRJ1", joint_target = 0),
00401                     joint(joint_name = "WRJ2", joint_target = 0) ]
00402     # business card pre-zero position
00403     bc_pre_zero = [ joint(joint_name = "FFJ0", joint_target = 13.6),
00404                         joint(joint_name = "FFJ3", joint_target = 7),
00405                         joint(joint_name = "FFJ4", joint_target = -0.4),
00406                         joint(joint_name = "MFJ0", joint_target = 51.1),
00407                         joint(joint_name = "MFJ3", joint_target = 33),
00408                         joint(joint_name = "MFJ4", joint_target = 21),
00409                         joint(joint_name = "RFJ0", joint_target = 50),
00410                         joint(joint_name = "RFJ3", joint_target = 18),
00411                         joint(joint_name = "RFJ4", joint_target = -21),
00412                         joint(joint_name = "LFJ0", joint_target = 30),
00413                         joint(joint_name = "LFJ3", joint_target = 0),
00414                         joint(joint_name = "LFJ4", joint_target = -24),
00415                         joint(joint_name = "LFJ5", joint_target = 7),         
00416                         joint(joint_name = "THJ1", joint_target = 15.2),
00417                         joint(joint_name = "THJ2", joint_target = 7.4),
00418                         joint(joint_name = "THJ3", joint_target = -4),
00419                         joint(joint_name = "THJ4", joint_target = 50),
00420                         joint(joint_name = "THJ5", joint_target = -16.8) ]
00421     # business card zero position
00422     bc_zero = [ joint(joint_name = "FFJ0", joint_target = 13.6),
00423                     joint(joint_name = "FFJ3", joint_target = 7),
00424                     joint(joint_name = "FFJ4", joint_target = -0.4),
00425                     joint(joint_name = "MFJ0", joint_target = 51.1),
00426                     joint(joint_name = "MFJ3", joint_target = 32),
00427                     joint(joint_name = "MFJ4", joint_target = 21),
00428                     joint(joint_name = "RFJ0", joint_target = 50),
00429                     joint(joint_name = "RFJ3", joint_target = 18),
00430                     joint(joint_name = "RFJ4", joint_target = -21),
00431                     joint(joint_name = "LFJ0", joint_target = 30),
00432                     joint(joint_name = "LFJ3", joint_target = 0),
00433                     joint(joint_name = "LFJ4", joint_target = -24),
00434                     joint(joint_name = "LFJ5", joint_target = 7),             
00435                     joint(joint_name = "THJ1", joint_target = 17.2),
00436                     joint(joint_name = "THJ2", joint_target = 10.4),
00437                     joint(joint_name = "THJ3", joint_target = -4),
00438                     joint(joint_name = "THJ4", joint_target = 50),
00439                     joint(joint_name = "THJ5", joint_target = -13.6) ]
00440     # business card position 1
00441     bc_1 = [ joint(joint_name = "FFJ0", joint_target = 137),
00442                  joint(joint_name = "FFJ3", joint_target = 7) ]
00443     # business card position 2 #UNUSED
00444     bc_2 = [ joint(joint_name = "FFJ0", joint_target = 137),
00445                  joint(joint_name = "FFJ3", joint_target = 31) ]
00446     # business card position 3
00447     bc_3 = [ joint(joint_name = "FFJ0", joint_target = 137),
00448                  joint(joint_name = "FFJ3", joint_target = 58) ]
00449     # business card position 4
00450     bc_4 = [ joint(joint_name = "FFJ0", joint_target = 66),
00451                  joint(joint_name = "FFJ3", joint_target = 58) ]
00452     # business card position 5
00453     bc_5 = [ joint(joint_name = "FFJ0", joint_target = 180),
00454                  joint(joint_name = "FFJ3", joint_target = 58) ]
00455     # business card position 6
00456     bc_6 = [ joint(joint_name = "FFJ0", joint_target = 180),
00457                  joint(joint_name = "FFJ3", joint_target = 0) ]
00458     # business card position 7
00459     bc_7 = [ joint(joint_name = "FFJ0", joint_target = 0),
00460                  joint(joint_name = "FFJ3", joint_target = 0) ]
00461     # business card position 8
00462     bc_8 = [ joint(joint_name = "FFJ0", joint_target = 137),
00463                  joint(joint_name = "FFJ3", joint_target = 15) ]
00464     # business card position 9 #UNUSED
00465     bc_9 = [ joint(joint_name = "FFJ0", joint_target = 137),
00466                  joint(joint_name = "FFJ3", joint_target = 30) ]
00467     # business card position 10 #UNUSED
00468     bc_10 = [ joint(joint_name = "FFJ0", joint_target = 137),
00469                   joint(joint_name = "FFJ3", joint_target = 60) ]
00470     # business card position 11 #UNUSED
00471     bc_11 = [ joint(joint_name = "FFJ0", joint_target = 137),
00472                   joint(joint_name = "FFJ3", joint_target = 31) ]
00473     # business card position 12
00474     bc_12 = [ joint(joint_name = "FFJ0", joint_target = 137),
00475                   joint(joint_name = "FFJ3", joint_target = 58) ]
00476     # business card position 13
00477     bc_13 = [ joint(joint_name = "FFJ0", joint_target = 66),
00478                   joint(joint_name = "FFJ3", joint_target = 58) ]
00479     # business card position 14
00480     bc_14 = [ joint(joint_name = "MFJ3", joint_target = 64),
00481                   joint(joint_name = "FFJ4", joint_target = 20) ]
00482     # business card position 15
00483     bc_15 = [ joint(joint_name = "FFJ0", joint_target = 81 ),
00484                   joint(joint_name = "FFJ4", joint_target = 20),
00485                   joint(joint_name = "FFJ3", joint_target = 50),
00486               joint(joint_name = "THJ4", joint_target = 55),
00487                   joint(joint_name = "THJ5", joint_target = 20) ]
00488     # business card position 16
00489     bc_16 = [ joint(joint_name = "MFJ0", joint_target = 20),
00490                   joint(joint_name = "MFJ3", joint_target = 10),
00491                   joint(joint_name = "MFJ4", joint_target = 0) ]
00492 
00493     #A boolean used in this demo: set to true while an action is running
00494     # just so we don't do 2 actions at once
00495     action_running = mutex.mutex()
00496 
00497     def __init__(self):
00498         #A vector containing the different callbacks, in the same order
00499         # as the tactiles.
00500         self.fingers_pressed_functions = [self.ff_pressed, self.mf_pressed, self.rf_pressed,
00501                                           self.lf_pressed, self.th_pressed]
00502 
00503         #The hand publishers:
00504         # we use a dictionnary of publishers, because on the etherCAT hand
00505         # you have one publisher per controller.
00506         self.hand_publishers = self.create_hand_publishers()
00507 
00508         #send the start position to the hand
00509         #self.hand_publish(self.start_pos_hand)
00510 
00511         #wait for the node to be initialized and then go to the starting position
00512         time.sleep(1)
00513         rospy.loginfo("OK, ready for the demo")
00514 
00515         # We subscribe to the data being published by the biotac sensors.
00516         # self.sub_biotacs = rospy.Subscriber("/rh/tactile", BiotacAll, self.callback_biotacs, queue_size=1)
00517         # self.sub_psts   = rospy.Subscriber("/rh/tactile", ShadowPST, self.callback_psts, queue_size=1)
00518 
00519     def create_hand_publishers(self):
00520         """
00521         Creates a dictionnary of publishers to send the targets to the controllers
00522         on /sh_??j?_mixed_position_velocity_controller/command
00523         """
00524         hand_pub = {}
00525 
00526         for joint in ["FFJ0", "FFJ3", "FFJ4",
00527                       "MFJ0", "MFJ3", "MFJ4",
00528                       "RFJ0", "RFJ3", "RFJ4",
00529                       "LFJ0", "LFJ3", "LFJ4", "LFJ5",
00530                       "THJ1", "THJ2", "THJ3", "THJ4", "THJ5",
00531                       "WRJ1", "WRJ2" ]:
00532             hand_pub[joint] = rospy.Publisher('sh_'+joint.lower()+'_position_controller/command', Float64, latch=True)
00533 
00534         return hand_pub
00535 
00536     def hand_publish(self, pose):
00537         """
00538         Publishes the given pose to the correct controllers for the hand.
00539         The targets are converted in radians.
00540         """
00541         for joint in pose:
00542             self.hand_publishers[joint.joint_name].publish( math.radians(joint.joint_target) )
00543 
00544     def callback_biotacs(self, msg):
00545         """
00546         The callback function for the biotacs. Checks if one of the fingers
00547         was pressed (filter the noise). If it is the case, call the
00548         corresponding function.
00549 
00550         @msg is the message containing the biotac data
00551         """
00552         #loop through the five tactiles
00553         for index,tactile in enumerate(msg.tactiles):
00554             #here we're just checking pdc (the pressure)
00555             # to see if a finger has been pressed, but you have
00556             # access to the whole data from the sensor
00557             # (look at sr_robot_msgs/msg/Biotac.msg)
00558             if tactile.pdc >= PDC_THRESHOLD:
00559                 # the tactile has been pressed, call the
00560                 # corresponding function
00561                 self.fingers_pressed_functions[index](tactile.pdc)
00562 
00563     def callback_psts(self, msg):
00564         """
00565         The callback function for the PSTs. Checks if one of the fingers
00566         was pressed (filter the noise). If it is the case, call the
00567         corresponding function.
00568 
00569         @msg is the message containing the biotac data
00570         """
00571         #loop through the five tactiles
00572         for index,tactile in enumerate(msg.pressure):
00573             #here we're just checking the pressure
00574             # to see if a finger has been pressed
00575             # 18456 is the value the PST takes when the sensor is not plugged in
00576             if tactile >= PST_THRESHOLD and tactile != 18456:
00577                 # the tactile has been pressed, call the
00578                 # corresponding function
00579                 self.fingers_pressed_functions[index](tactile)
00580 
00581     def ff_pressed(self,data):
00582         """
00583         The first finger was pressed.
00584         
00585         @param data: the pressure value (pdc)
00586         """
00587         #if we're already performing an action, don't do anything
00588         if not self.action_running.testandset():
00589             return
00590         
00591         #ok the finger sensor was pressed
00592         #p = subprocess.Popen('beep')
00593         
00594         rospy.loginfo("FF touched, running basic demo ")
00595         
00596         #send the start position to the hand
00597         self.hand_publish( self.start_pos_hand )
00598         time.sleep(1)
00599         self.hand_publish( self.flex_ff )
00600         time.sleep(1)
00601         self.hand_publish( self.ext_ff )
00602         time.sleep(1)
00603         self.hand_publish( self.flex_mf )
00604         time.sleep(1)
00605         self.hand_publish( self.ext_mf )
00606         time.sleep(1)
00607         self.hand_publish( self.flex_rf )
00608         time.sleep(1)
00609         self.hand_publish( self.ext_rf )
00610         time.sleep(1)
00611         self.hand_publish( self.flex_lf )
00612         time.sleep(1)
00613         self.hand_publish( self.ext_lf )
00614         time.sleep(1)
00615         self.hand_publish( self.flex_th_1 )
00616         time.sleep(1)
00617         self.hand_publish( self.flex_th_2 )
00618         time.sleep(1)
00619         self.hand_publish( self.ext_th_1 )
00620         time.sleep(1)
00621         self.hand_publish( self.ext_th_2 )
00622         time.sleep(1)
00623         self.hand_publish( self.n_wr )
00624         time.sleep(1)
00625         self.hand_publish( self.s_wr )
00626         time.sleep(1)
00627         self.hand_publish( self.zero_wr )
00628         time.sleep(1)
00629         self.hand_publish( self.e_wr )
00630         time.sleep(1)
00631         self.hand_publish( self.w_wr )
00632         time.sleep(1)
00633         self.hand_publish( self.zero_wr )
00634         time.sleep(1)
00635         self.hand_publish( self.pre_ff_ok )
00636         time.sleep(0.3)
00637         self.hand_publish( self.ff_ok )
00638         time.sleep(1)
00639         self.hand_publish( self.ff2mf_ok )
00640         time.sleep(1)
00641         self.hand_publish( self.mf_ok )
00642         time.sleep(1)
00643         self.hand_publish( self.mf2rf_ok )
00644         time.sleep(1)
00645         self.hand_publish( self.rf_ok )
00646         time.sleep(1)
00647         self.hand_publish( self.rf2lf_ok )
00648         time.sleep(1)
00649         self.hand_publish( self.lf_ok )
00650         time.sleep(1)
00651         self.hand_publish( self.start_pos_hand )
00652         time.sleep(1)
00653         self.hand_publish( self.flex_ff )
00654         time.sleep(0.2)
00655         self.hand_publish( self.flex_mf )
00656         time.sleep(0.2)
00657         self.hand_publish( self.flex_rf )
00658         time.sleep(0.2)
00659         self.hand_publish( self.flex_lf )
00660         time.sleep(0.2)
00661         self.hand_publish( self.ext_ff )
00662         time.sleep(0.2)
00663         self.hand_publish( self.ext_mf )
00664         time.sleep(0.2)
00665         self.hand_publish( self.ext_rf )
00666         time.sleep(0.2)
00667         self.hand_publish( self.ext_lf )
00668         time.sleep(0.2)
00669         self.hand_publish( self.flex_ff )
00670         time.sleep(0.2)
00671         self.hand_publish( self.flex_mf )
00672         time.sleep(0.2)
00673         self.hand_publish( self.flex_rf )
00674         time.sleep(0.2)
00675         self.hand_publish( self.flex_lf )
00676         time.sleep(0.2)
00677         self.hand_publish( self.ext_ff )
00678         time.sleep(0.2)
00679         self.hand_publish( self.ext_mf )
00680         time.sleep(0.2)
00681         self.hand_publish( self.ext_rf )
00682         time.sleep(0.2)
00683         self.hand_publish( self.ext_lf )
00684         time.sleep(0.2)
00685         self.hand_publish( self.flex_ff )
00686         time.sleep(0.2)
00687         self.hand_publish( self.flex_mf )
00688         time.sleep(0.2)
00689         self.hand_publish( self.flex_rf )
00690         time.sleep(0.2)
00691         self.hand_publish( self.flex_lf )
00692         time.sleep(0.2)
00693         self.hand_publish( self.ext_ff )
00694         time.sleep(0.2)
00695         self.hand_publish( self.ext_mf )
00696         time.sleep(0.2)
00697         self.hand_publish( self.ext_rf )
00698         time.sleep(0.2)
00699         self.hand_publish( self.ext_lf )
00700         time.sleep(1.0)
00701         self.hand_publish( self.pre_ff_ok )
00702         time.sleep(0.3)
00703         self.hand_publish( self.ff_ok )
00704         time.sleep(1)
00705         self.hand_publish( self.ne_wr )
00706         time.sleep(1.2)
00707         self.hand_publish( self.nw_wr )
00708         time.sleep(1.2)
00709         self.hand_publish( self.sw_wr )
00710         time.sleep(1.2)
00711         self.hand_publish( self.se_wr )
00712         time.sleep(1.2)
00713         self.hand_publish( self.ne_wr )
00714         time.sleep(0.4)
00715         self.hand_publish( self.nw_wr )
00716         time.sleep(0.4)
00717         self.hand_publish( self.sw_wr )
00718         time.sleep(0.4)
00719         self.hand_publish( self.se_wr )
00720         time.sleep(0.4)
00721         self.hand_publish( self.zero_wr )
00722         time.sleep(1)
00723         self.hand_publish( self.start_pos_hand )
00724         time.sleep(1)
00725         
00726         #wait before next possible action
00727         time.sleep(.2)
00728         self.action_running.unlock()
00729 
00730     def mf_pressed(self, data):
00731         """
00732         The middle finger was pressed.
00733         
00734         @param data: the pressure value (pdc)
00735         """
00736         #if we're already performing an action, don't do anything
00737         if not self.action_running.testandset():
00738             return
00739         
00740         #ok finger was pressed
00741         #p = subprocess.Popen('beep')
00742         
00743         rospy.loginfo("MF touched, running business card demo ")
00744         
00745         #wait 1s for the user to release the sensor
00746         time.sleep(.2)
00747         
00748         #send the start position to the hand
00749         self.hand_publish( self.start_pos_hand )
00750         time.sleep(1)
00751         self.hand_publish( self.bc_pre_zero )
00752         time.sleep(2)
00753         self.hand_publish( self.bc_zero )
00754         time.sleep(3)
00755         self.hand_publish( self.bc_1 )
00756         time.sleep(1)
00757         #       self.hand_publish( self.bc_2 )
00758         #       time.sleep(1)
00759         self.hand_publish( self.bc_3 )
00760         time.sleep(1)
00761         self.hand_publish( self.bc_4 )
00762         time.sleep(1)
00763         self.hand_publish( self.bc_5 )
00764         time.sleep(1)
00765         self.hand_publish( self.bc_6 )
00766         time.sleep(1)
00767         self.hand_publish( self.bc_7 )
00768         time.sleep(1)
00769         self.hand_publish( self.bc_8 )
00770         time.sleep(1)
00771         #       self.hand_publish( self.bc_9 )
00772         #       time.sleep(1)
00773         #       self.hand_publish( self.bc_10 )
00774         #       time.sleep(1)
00775         #       self.hand_publish( self.bc_11 )
00776         #       time.sleep(1)
00777         self.hand_publish( self.bc_12 )
00778         time.sleep(1)
00779         self.hand_publish( self.bc_13 )
00780         time.sleep(1)
00781         #       self.hand_publish( self.bc_14 )
00782         #       time.sleep(1)
00783         self.hand_publish( self.bc_15 )
00784         time.sleep(1)
00785         self.hand_publish( self.bc_16 )
00786         time.sleep(3)
00787         self.hand_publish( self.start_pos_hand )
00788         
00789         #wait before next possible action
00790         time.sleep(3)
00791         self.action_running.unlock()
00792 
00793 
00794     def rf_pressed(self, data):
00795         """
00796         The ring finger was pressed.
00797         
00798         @param data: the pressure value (pdc)
00799         """
00800         #if we're already performing an action, don't do anything
00801         if not self.action_running.testandset():
00802             return
00803         
00804         #ok finger was pressed
00805         #p = subprocess.Popen('beep')
00806         
00807         rospy.loginfo("RF touched, running shaking hands demo.")
00808         
00809         #wait 1s for the user to release the sensor
00810         time.sleep(.2)
00811         
00812         #send the start position to the hand
00813         self.hand_publish( self.shake_grasp_1 )
00814         time.sleep(3)
00815         self.hand_publish( self.shake_grasp_2 )
00816         time.sleep(1)
00817         self.hand_publish( self.e_wr )
00818         time.sleep(0.33)
00819         self.hand_publish( self.w_wr )
00820         time.sleep(0.33)
00821         self.hand_publish( self.zero_wr )
00822         time.sleep(0.66)
00823         self.hand_publish( self.shake_grasp_1 )
00824         time.sleep(3)
00825         self.hand_publish( self.start_pos_hand )
00826         time.sleep(2)
00827         
00828         #wait before next possible action
00829         time.sleep(.2)
00830         self.action_running.unlock()
00831 
00832     def lf_pressed(self, data):
00833         """
00834         The little finger was pressed.
00835         
00836         @param data: the pressure value (pdc)
00837         """
00838         #if we're already performing an action, don't do anything
00839         if not self.action_running.testandset():
00840             return
00841         
00842         #ok finger pressed
00843         #p = subprocess.Popen('beep')
00844         
00845         rospy.loginfo("LF touched, going to store position.")
00846         
00847         #wait 1s for the user to release the sensor
00848         time.sleep(.2)
00849         
00850         #send the start position to the hand
00851         self.hand_publish( self.start_pos_hand )
00852         time.sleep(1)
00853         self.hand_publish( self.flex_lf )
00854         time.sleep(0.2)
00855         self.hand_publish( self.flex_rf )
00856         time.sleep(0.2)
00857         self.hand_publish( self.flex_mf )
00858         time.sleep(0.2)
00859         self.hand_publish( self.flex_ff )
00860         time.sleep(0.2)
00861         self.hand_publish( self.ext_lf )
00862         time.sleep(0.2)
00863         self.hand_publish( self.ext_rf )
00864         time.sleep(0.2)
00865         self.hand_publish( self.ext_mf )
00866         time.sleep(0.2)
00867         self.hand_publish( self.ext_ff )
00868         time.sleep(0.2)
00869         self.hand_publish( self.flex_lf )
00870         time.sleep(0.2)
00871         self.hand_publish( self.flex_rf )
00872         time.sleep(0.2)
00873         self.hand_publish( self.flex_mf )
00874         time.sleep(0.2)
00875         self.hand_publish( self.flex_ff )
00876         time.sleep(1)
00877         #self.hand_publish( self.store_1_PST )
00878         self.hand_publish( self.store_1_BioTac )
00879         time.sleep(1)
00880         #self.hand_publish( self.store_2_PST )
00881         self.hand_publish( self.store_2_BioTac )
00882         time.sleep(1)
00883         
00884         #wait before next possible action
00885         time.sleep(.2)
00886         self.action_running.unlock()
00887 
00888     def th_pressed(self, data):
00889         """
00890         The thumb was pressed.
00891         
00892         @param data: the pressure value (pdc)
00893         """
00894         #if we're already performing an action, don't do anything
00895         if not self.action_running.testandset():
00896             return
00897         
00898         #ok the finger was pressed
00899         #p = subprocess.Popen('beep')
00900         
00901         rospy.loginfo("TH touched, going to start position.")
00902         
00903         #wait 1s for the user to release the sensor
00904         time.sleep(.2)
00905         
00906         #send the thumb_up_position to the hand
00907         self.hand_publish( self.start_pos_hand )
00908         
00909         #wait before next possible action
00910         time.sleep(.2)
00911         self.action_running.unlock()
00912 
00913 def main():
00914     """
00915     The main function
00916     """
00917     # init the ros node
00918     rospy.init_node('fancy_touch_demo', anonymous=True)
00919 
00920     fancy_demo = FancyDemo()
00921 
00922     fancy_demo.lf_pressed(0)
00923 
00924     # subscribe until interrupted
00925     # rospy.spin()
00926 
00927 
00928 if __name__ == '__main__':
00929     main()
00930