pressure_observer.h
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.h
00040 
00041 #include <pr2_hardware_interface/hardware_interface.h>
00042 #include <pr2_gripper_sensor_msgs/PR2GripperPressureData.h>
00043 #include <pr2_gripper_sensor_controller/digitalFilter.h>
00044 
00045 #ifndef _PRESSURE_OBSERVER
00046 #define _PRESSURE_OBSERVER
00047 
00048 #define UNITS_PER_N 1600
00049 
00050 class pressureObserver{
00051   
00052   public:
00053     // functions
00054     pressureObserver(pr2_hardware_interface::PressureSensor* left,pr2_hardware_interface::PressureSensor* right);    // constructor
00055     ~pressureObserver();                                // default destructor
00056     void spin();                                        // method to update sensor data. should run at 1khz
00057     bool graspContact(int contactsDesired);             // detects finger impacts
00058     void updateZeros2();                                // method to update our zero value offsets (zero_offset)
00059     bool checkPlaceContact(double dF);                  // method to look for impact slip and return true
00060     bool checkSlip(double slip_motion_limit = 0.007, double slip_force_limit = 0.18);
00061     int  checkSideImpact(double dF);                    // method to check for side impacts on finger sensors
00062     void updateBrokenSensorStatus();
00063 
00064 
00065     double padForce_cur;                                // fingerpad force average between both fingers
00066     double padForce_cur_nonbiased;                      // force average from the non-biased pressures
00067     double padForce_left_cur, padForce_left_prev;       // current cumulative force on our left/rigth fingerpads
00068     double padForce_left_cur_nonbiased;
00069     double padForce_right_cur, padForce_right_prev;     // previous cumulative force on our left/right fingerpads
00070     double padForce_right_cur_nonbiased;
00071     double totalLoading_left, totalLoading_right;       // total dynamic laoding on our left/right pads
00072     double totalLoading,totalLoading_prev;              // left and right loading average
00073     double cellLoading_left, cellLoading_right;         // cellular loading on the left/right fingers
00074     double forceLightest;
00075     double hpForceTrigger;
00076     double lfForce_left, lfForce_right;
00077     bool broken_sensor;                                 // flag to indicate we may have a broken sensor
00078     bool left_contact, right_contact;                   // flags to indicate whether a left or right contact happened
00079     bool placeContact;
00080     
00081     // storage of our pressure sensor state information. _now is the most recent raw data. _prev is the last subsampled value
00082     // _current is the current subsampled value
00083     pr2_gripper_sensor_msgs::PR2GripperPressureData  pressure_now, pressure_prev, pressure_current;
00084     pr2_gripper_sensor_msgs::PR2GripperPressureData  pressure_thresh_now, pressure_thresh_prev, pressure_thresh_current;
00085     pr2_gripper_sensor_msgs::PR2GripperPressureData  pressure_prev_zerod, pressure_current_zerod;
00086     pr2_gripper_sensor_msgs::PR2GripperPressureData  pressure_prev_bias, pressure_cur_bias;
00087     pr2_gripper_sensor_msgs::PR2GripperPressureData  pressure_bp;
00088 
00089     digitalFilter *pressureLFFilt_left[22];
00090     digitalFilter *pressureLFFilt_right[22];
00091 
00092 
00093   private:
00094     void getPadCentroid(boost::array<double,22> pressureArray, double *x, double *y);
00095     void updateXYCentroids();
00096     bool updatePressureState();                         // method to acquire new data from both fingers and apply appropriate subsampling 
00097     void updateTotalLoading();                          // method to update the toal loading of each finger totalLoad_left _right
00098     void zero();                                        // method to zero out data
00099     void bias_estimator();                              // remove mean bias for near zero data
00100     double getPadForce(boost::array<double,22> pressureArray);  // translate fingertip pressure vector to cumultive sum on the fingerpad (just the pad)
00101     void updateContactState();                          // update the state of the robots contact sensors
00102 
00103 
00104     pr2_hardware_interface::PressureSensor* left_finger;// a copy of our pressure sensor handle
00105     pr2_hardware_interface::PressureSensor* right_finger;// a copy of our pressure sensor handle
00106 
00107     int subSample_cnt;                                  // a counter to keep track of how many samples we ignored during subsampling
00108     int subSample_steps;                                // 1+ the number of values to skip during subsampling
00109     int zero_samples;                                   // the number of samples to use when trying to evaluate our zero offset
00110     int zero_cnt;                                       // counter to keep track of how many samples we've stored for zero estimation
00111     pr2_gripper_sensor_msgs::PR2GripperPressureData zero_offset;// a storage container for our zero offset values
00112 
00113     double time_prev, time_cur;                         // stroage containers for the previous and current pressure update times
00114     double dt;                                          // the time expired between pressure update iterations
00115 
00116 };
00117 
00118 
00119 
00120 #endif // _PRESSURE_OBSERVER


pr2_gripper_sensor_controller
Author(s): Joe Romano
autogenerated on Fri Jan 3 2014 11:54:05