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