pressure_observer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  \author Joe Romano
00036  *********************************************************************/
00037 //@author  Joe Romano
00038 //@email   joeromano@gmail.com
00039 //@brief   pressure_observer.cpp
00040 
00041 #include <pr2_gripper_sensor_controller/pressure_observer.h>
00042 
00043 // method to be called each 1ms timestep in realtime to ensure data collection and manipulation happens properly
00044 void pressureObserver::spin()
00045 {
00046   // update the sensor readings and perform actions if new data was aquired.
00047   // this inner loop shuld run (become true) at the update rate of the pressure sensors (24.9 Hz)
00048   if(updatePressureState())
00049   {
00050     // update our time values
00051     time_prev = time_cur;
00052     time_cur = ros::Time::now().toSec();
00053 
00054     zero();                        // zero out our data
00055     bias_estimator();              // calculate filtered (bp,hp) force values
00056     updateTotalLoading();          // update our total loading estimates
00057     updateXYCentroids();
00058     updateBrokenSensorStatus();    // update our flag to indicate a broken sensor
00059 
00060     updateContactState();
00061   }
00062     
00063 }
00064 
00065 
00066 void pressureObserver::updateBrokenSensorStatus()
00067 {
00068   if(getPadForce(pressure_now.pressure_left) == 0 || getPadForce(pressure_now.pressure_left) == 0)
00069     broken_sensor = true;
00070   else
00071     broken_sensor = false;
00072 
00073 }
00074 
00075 
00076 // NOTE: remove bias and do for all? remove completely? try remove completely first.
00077 // remove the mean for near-zero data at low frequencies
00078 void pressureObserver::bias_estimator()
00079 {
00080   // position high-pass filter cutoff frequency                               
00081   double poshp_fc = 5.0;   // Hz                                               
00082   double poshp_lambda = 2*3.14159265*poshp_fc;
00083   double dt = .041;
00084  
00085   // for all cells (only if we are not currently zeroing things out)
00086   for( int i = 0; (i < 22); i ++) 
00087   {
00088     pressure_prev_bias.pressure_left[i] = pressure_cur_bias.pressure_left[i];
00089     pressure_prev_bias.pressure_right[i] = pressure_cur_bias.pressure_right[i];
00090                 
00091     pressure_cur_bias.pressure_left[i] = ((1/(1+dt*poshp_lambda))*pressure_prev_bias.pressure_left[i]) + ((1/(1+dt*poshp_lambda))*(pressure_current_zerod.pressure_left[i]-pressure_prev_zerod.pressure_left[i]));
00092     pressure_cur_bias.pressure_right[i] = ((1/(1+dt*poshp_lambda))*pressure_prev_bias.pressure_right[i]) + ((1/(1+dt*poshp_lambda))*(pressure_current_zerod.pressure_right[i]-pressure_prev_zerod.pressure_right[i]));
00093 
00094     // calculate our band-passed data
00095     pressure_bp.pressure_left[i] = pressureLFFilt_left[i]->getNextFilteredValue(pressure_current_zerod.pressure_left[i]);
00096     pressure_bp.pressure_right[i] = pressureLFFilt_right[i]->getNextFilteredValue(pressure_current_zerod.pressure_right[i]);
00097 
00098     pressure_cur_bias.rostime = ros::Time::now().toSec();
00099   }  
00100 }
00101 
00102 
00103 bool pressureObserver::checkPlaceContact(double dF)
00104 {
00105   if( checkSlip(dF,0.2)  || placeContact )
00106     placeContact = true;
00107   else
00108     placeContact = false;
00109   return placeContact;
00110 }
00111 
00112 
00113 int pressureObserver::checkSideImpact(double dF)
00114 {
00115   for(int i = 0; i < 7; i++)
00116   {
00117     // should really be 600 and 400 depending on sensor. heh.
00118     if( (pressure_cur_bias.pressure_left[i]/500.0) > dF )
00119       return i+1;
00120     if( (pressure_cur_bias.pressure_right[i]/500.0) > dF )
00121       return i+10;
00122   }
00123   return -1;
00124 }
00125 
00126 
00127 // method to watch pressure sensor transience during grasp and return true
00128 // when both fingers saw a significant force transience
00129 bool pressureObserver::graspContact(int contactsDesired)
00130 {
00131 
00132   // if we are looking for both fingers to contact 
00133   if( contactsDesired == 0   && left_contact && right_contact)
00134      return true;
00135   // if looking for a single finger contact
00136   else if( (contactsDesired == 1 && left_contact) || (contactsDesired == 2 && right_contact))
00137      return true;
00138   // if we are looking for any contact
00139   else if( (contactsDesired ==3) && (left_contact || right_contact) )
00140     return true;
00141  
00142   return false;
00143 }                                              
00144 
00145 
00146 // method to update contact state of fingerpads
00147 void pressureObserver::updateContactState()
00148 {
00149   //float totalLoading_contactThresh = 3000.0/UNITS_PER_N;  // was 3000 before unit change to N. Probably should be on param server
00150  
00151   if(fabs(padForce_left_cur) > hpForceTrigger || padForce_left_cur_nonbiased > forceLightest-.25 )
00152     left_contact = true;
00153   else
00154     left_contact = false;
00155 
00156   if(fabs(padForce_right_cur) > hpForceTrigger || padForce_right_cur_nonbiased > forceLightest-.25 )
00157     right_contact = true;
00158   else
00159     right_contact = false;
00160 }
00161 
00162 
00163 // method to update the toal loading of each finger PAD totalLoad_left _right
00164 void pressureObserver::updateTotalLoading()                                    
00165 {
00166   // store our previous forces on the fingerpads
00167   padForce_left_prev = padForce_left_cur;
00168   padForce_right_prev = padForce_right_cur;
00169 
00170   // get our new fingerpad forces
00171   padForce_left_cur = getPadForce(pressure_cur_bias.pressure_left);
00172   padForce_right_cur = getPadForce(pressure_cur_bias.pressure_right);
00173   padForce_left_cur_nonbiased = getPadForce(pressure_current_zerod.pressure_left);
00174   padForce_right_cur_nonbiased = getPadForce(pressure_current_zerod.pressure_right);
00175   padForce_cur = (padForce_left_cur + padForce_right_cur)/2.0;
00176   padForce_cur_nonbiased = (padForce_left_cur_nonbiased + padForce_right_cur_nonbiased)/2.0;
00177 
00178   // take the discrete derivative and absolute value to get the new total loading
00179   totalLoading_left = fabs((padForce_left_cur-padForce_left_prev)/(dt));
00180   totalLoading_right = fabs((padForce_right_cur-padForce_right_prev)/(dt));
00181   totalLoading_prev = totalLoading;
00182   totalLoading = (totalLoading_left+totalLoading_right)/2.0;
00183 
00184   lfForce_left = getPadForce(pressure_bp.pressure_left);
00185   lfForce_right = getPadForce(pressure_bp.pressure_right);
00186 }
00187 
00188 
00189 // method to take in a fingertip pressure vector and return the cumultive sum on the fingerpad (just the pad)
00190 double pressureObserver::getPadForce(boost::array<double,22> pressureArray)
00191 {
00192   double tempLoading = 0.0;
00193 
00194   // sum the fingerpads to get the new total loading
00195   for( int i = 7; i < 22; i ++)
00196   {
00197     tempLoading += pressureArray[i];
00198   }
00199 
00200   return tempLoading/UNITS_PER_N;
00201 
00202 }
00203 
00204 
00205 // method to update our zero value for each cell based on a low-pass filtered version of the no-load pressure signal
00206 void pressureObserver::updateZeros2()
00207 {
00208   double 
00209   zero_weight = 0.6;
00210   for( int i = 0; i < 22; i ++) 
00211   {
00212     zero_offset.pressure_left[i] = (  ((1.0-zero_weight)*zero_offset.pressure_left[i]) + (zero_weight*pressure_current.pressure_left[i]) ); 
00213     zero_offset.pressure_right[i] = ( ((1.0-zero_weight)*zero_offset.pressure_right[i]) + (zero_weight*pressure_current.pressure_right[i]) ); 
00214   }
00215 }
00216 
00217 
00218 // method to zero out our data by modifying pressureState_current
00219 void pressureObserver::zero()
00220 {
00221     // update our zero vector
00222     for( int i = 0; i < 22; i ++)
00223     {
00224       pressure_prev_zerod.pressure_left[i] = pressure_current_zerod.pressure_left[i];
00225       pressure_prev_zerod.pressure_right[i] = pressure_current_zerod.pressure_right[i];
00226 
00227       pressure_current_zerod.pressure_left[i] = pressure_current.pressure_left[i] - zero_offset.pressure_left[i];
00228       pressure_current_zerod.pressure_right[i] = pressure_current.pressure_right[i] - zero_offset.pressure_right[i];
00229     }  
00230 }
00231 
00232 
00233 // method to acquire new data from both fingers and apply appropriate subsampling
00234 // @return true if the data was updated (we took a subsample), false if no data update occured (repetitive data)
00235 bool pressureObserver::updatePressureState()
00236 {
00237 
00238   bool newData = false;
00239   bool updated = false;
00240 
00241    // grab new data from the left and right sensors
00242   std::vector< uint16_t > pressureVector_left = left_finger->state_.data_;                                                                                            
00243   std::vector< uint16_t > pressureVector_right = right_finger->state_.data_;                                                                                          
00244   for( int i = 0; i < 22; i ++)                                        
00245   {                                                                                                                                                                                     
00246       pressure_now.pressure_left[i] = (double)pressureVector_left[i]; 
00247       pressure_now.pressure_right[i] = (double)pressureVector_right[i]; 
00248 
00249       // check if any values have changed, and if so set a flag
00250       if(pressure_now.pressure_left[i]  != pressure_current.pressure_left[i] || pressure_now.pressure_right[i] != pressure_current.pressure_right[i])
00251       {
00252         newData = true;
00253       }
00254   }
00255 
00256   // if we went over our 41st sample (the rate at which samples are updated) then update our stored values
00257   // subSample_cnt >= 41 if too many samples happened
00258   if( newData || subSample_cnt >= subSample_steps)
00259   {
00260 
00261      for( int i = 0; i < 22; i ++)                
00262      {
00263         // store the current value as previous
00264         pressure_prev.pressure_left[i] = pressure_current.pressure_left[i];
00265         pressure_prev.pressure_right[i] = pressure_current.pressure_right[i];
00266 
00267         // store the new value as current
00268         pressure_current.pressure_left[i] = pressure_now.pressure_left[i];
00269         pressure_current.pressure_right[i] = pressure_now.pressure_right[i];
00270      }
00271      // reset our sample counter to zero
00272      subSample_cnt = 0;
00273 
00274      updated = true;
00275   }
00276    
00277   // increment the subsample counter
00278   subSample_cnt++;
00279 
00280   return updated;
00281 }
00282 
00283 
00284 void pressureObserver::getPadCentroid(boost::array<double,22> pressureArray, double *x, double *y)
00285 {
00286   // weights to multiply our cells on the fingerpad by to determine the x-y centroid
00287   double y_weights[15] = {-1, 0, 1, -1, 0, 1, -1, 0, 1, -1, 0, 1, -1, 0, 1}; 
00288   double x_weights[15] = {-1, -1, -1, -0.5, -0.5, -0.5, 0, 0, 0, 0.5, 0.5, 0.5, 1, 1, 1};
00289   
00290   *y = 0;
00291   *x = 0;
00292 
00293   for(int i = 7; i < 22; i++)
00294   {
00295     *y += pressureArray[i]*y_weights[i-7];
00296     *x += pressureArray[i]*x_weights[i-7];
00297   }
00298 
00299   // divide the x and y by the sum of the pad
00300   double pressureSum = getPadForce(pressureArray);
00301   *y = *y/pressureSum;
00302   *x = *x/pressureSum;
00303 }
00304 
00305 void pressureObserver::updateXYCentroids()
00306 {
00307   double x_centroid_left, y_centroid_left, x_centroid_right, y_centroid_right;
00308   getPadCentroid(pressure_current_zerod.pressure_left,&x_centroid_left,&y_centroid_left);
00309   getPadCentroid(pressure_current_zerod.pressure_right,&x_centroid_right,&y_centroid_right);
00310 
00311 }
00312 
00313 
00314 bool pressureObserver::checkSlip(double slip_motion_limit, double slip_force_limit)
00315 {
00316   double disturbance_left =  std::min(std::max(slip_motion_limit*fabs(padForce_left_cur_nonbiased), .048),0.2);
00317   double disturbance_right = std::min(std::max(slip_motion_limit*fabs(padForce_right_cur_nonbiased), .048),0.2);
00318 
00319   if( ( fabs(padForce_left_cur) > disturbance_left) && (fabs(lfForce_left) < slip_force_limit))
00320     return true;
00321   else if( ( fabs(padForce_right_cur) > disturbance_right) && (fabs(lfForce_right) < slip_force_limit))
00322     return true;
00323   else
00324     return false;
00325 }
00326 
00327 
00328 // constructor
00329 pressureObserver::pressureObserver(pr2_hardware_interface::PressureSensor* left,pr2_hardware_interface::PressureSensor* right)
00330 {
00331   // store our pressure sensor state handles
00332   left_finger = left;
00333   right_finger = right;
00334 
00335   // initialize our subsample counter to 1
00336   subSample_cnt = 1;
00337 
00338   // set our subsampling up to pick every 41st sample (since we sample at 1ms but the pressure array updates every 41ms)
00339   subSample_steps = 41;
00340 
00341   // initialize our loading values to zero
00342   padForce_left_cur = 0.0;
00343   padForce_right_cur = 0.0;
00344   padForce_left_prev = 0.0;
00345   padForce_right_prev = 0.0;
00346 
00347   // initialize our time values
00348   time_prev = ros::Time::now().toSec();
00349   time_cur = ros::Time::now().toSec();
00350   dt = 0.041;
00351 
00352   // intialize the right/left contact states
00353   left_contact = false;
00354   right_contact = false;
00355 
00356   // initialize our placement contact state
00357   placeContact = false;
00358 
00359   // get some fresh data from the sensors
00360   updatePressureState();
00361 
00362   // set all our zero_offset pressures to the first seen value
00363   for( int i = 0; i < 22; i ++) 
00364   {
00365      zero_offset.pressure_left[i] = pressure_now.pressure_left[i]; 
00366      zero_offset.pressure_right[i] = pressure_now.pressure_right[i]; 
00367   }
00368 
00369   // create our filter for low frequency pressure changes
00370   // instatiate our digital filter
00371   // 1st order chebychev. 0.8 to 5 hz bandpass. 0.5 r. (3 terms for chebychev?)
00372   float b_vfilt[] = { 0.6323, 0, -0.6323};
00373   float a_vfilt[] = {1.0, -0.6294, -0.2647};
00374   for(int i=0; i < 22; i++)
00375   {
00376     pressureLFFilt_left[i] = new digitalFilter(1+1, true,b_vfilt,a_vfilt);
00377     pressureLFFilt_right[i] = new digitalFilter(1+1, true,b_vfilt,a_vfilt);
00378   }
00379 }
00380 
00381 
00382 // destructor
00383 pressureObserver::~pressureObserver()
00384 {
00385   delete[] pressureLFFilt_left;
00386   delete[] pressureLFFilt_right;
00387 }


pr2_gripper_sensor_controller
Author(s): Joe Romano
autogenerated on Thu Jun 6 2019 18:02:06