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