gripper_controller.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   gripperController.h - class to read data from gripper sensor
00040 //         data helper classes and make the gripper take action on it
00041 //         through a variety of different control modes.
00042 
00043 #include <pr2_hardware_interface/hardware_interface.h>
00044 #include <pr2_controller_interface/controller.h>
00045 #include <pr2_mechanism_model/joint.h>
00046 #include <pr2_gripper_sensor_controller/pressure_observer.h>
00047 #include <pr2_gripper_sensor_controller/acceleration_observer.h>
00048 #include <pr2_gripper_sensor_controller/digitalFilter.h>
00049 
00050 #ifndef _GRIPPER_CONTROLLER
00051 #define _GRIPPER_CONTROLLER
00052 
00053 
00054 class gripperController{
00055   
00056   public:
00057     // functions
00058   gripperController(pr2_mechanism_model::JointState* joint_state_, pressureObserver* pressure_observer_, accelerationObserver* acceleration_observer_);    // ctor
00059     ~gripperController();                                             // default dtor
00060     bool velocityServo(double desiredVel);
00061     bool positionServo(double desiredPos, double desiredVel);     // servo to a desired position
00062     void updateData();                                            // update gripper state info (should run at 1khz)
00063     bool initializeGripper();                                     // 
00064     bool forceServo2(double desired_Force);    // servo gripper on pressure sensor force
00065     bool grabObject(double close_speed, int contactsDesired);      // look for object contacts on each finger
00066     bool slipServo2();                                            // servo gripper on slip information from psensors
00067     bool forceRampTo(double force, double duration);              // ramp the current force to a specific value over time
00068     bool place(int placeConditions,double acc_trigger, double slip_trigger);                                                 // method to look for contact and return true when detected
00069 
00070 
00071     // externally modified variables
00072     double max_effort;
00073     double fingertip_force_limit;
00074     double positionMarker_limit;
00075     double positionMarker;
00076     
00077     // externally accessed variables
00078     bool slip_flag;
00079     bool deformation_limit_flag;
00080     bool force_limit_flag;
00081     bool dropped_flag;
00082     
00083     //variables
00084     //PerformanceCounter counter;
00085     double vel_integral;                                          // velocity integral for force control
00086     double vel_integral_vcontrol;
00087     double lpVelocity;                                            // a low-passsed version of our velocity
00088     double accThresh;
00089     digitalFilter *velocityLPFilt;
00090     double positionOpen;                                          // the 'open' position of gripper
00091     double positionClosed;                                        // the 'closed' position of gripper
00092     double positionCurrent;                                       // current gripper position
00093     double positionDropped;                                       // the position below which we consider things dropped
00094     double servoForce;                                            // the force we are trying to servo to
00095     double objectForce;                                           // the force found to be best to hold the current object                                             
00096     double positionContact;                                       // the last position we saw contact at
00097     double forceContact;                                          // the force seen at contact (average)
00098     double forceContact_l, forceContact_r;                        // the force seen on l/r fingers at contact
00099     double force_servo_force_tolerance;
00100 
00101     // storage of our gripper state information
00102     double gripper_state_now_measured_effort, gripper_state_now_position, gripper_state_now_velocity;
00103     double gripper_state_prev_measured_effort, gripper_state_prev_position, gripper_state_prev_velocity;
00104 
00105     double kP;                                                    // proportional gain of position controller
00106     double kD;                                                    // derivative gain of position controller
00107     double dt;                                                    // time expired this current timestep
00108     double coulomb;                                               // coulomb term for position controller
00109     
00110     pr2_mechanism_model::JointState* jointState;                  // a copy of our joint handle
00111     pressureObserver* myPressureObserver;                         // a copy of our pres. obsrv. handle
00112     accelerationObserver* myAccelerationObserver;                         // a copy of our acc. obsrv. handle
00113 
00114     bool firstRamp;                                               // flag to indicate the first iteration of forceRampTo
00115     double ramp_start_time;                                       // time that forceRampTo started
00116     double ramp_start_force;                                      // force that forceRampTo started on
00117 
00118   private:
00119     double initialPosition;                                       // the starting position when this class was called
00120     double time_prev, time_cur;                                   // the prev/current time of our control loop
00121 };
00122 
00123 
00124 
00125 void gripperController::updateData()
00126 {
00127   // update our timing information
00128   time_prev = time_cur;
00129   //counter.StopCounter();
00130   time_cur = ros::Time::now().toSec();//counter.GetElapsedTime();
00131 
00132 
00133   // move old variables to gripperState_prev
00134   gripper_state_prev_measured_effort = gripper_state_now_measured_effort;
00135   gripper_state_prev_position = gripper_state_now_position;
00136   gripper_state_prev_velocity = gripper_state_now_velocity;
00137 
00138   // get new variables and store them in _now
00139   gripper_state_now_measured_effort = jointState->measured_effort_;                         
00140   gripper_state_now_position = jointState->position_; 
00141   gripper_state_now_velocity = jointState->velocity_;   
00142   lpVelocity = velocityLPFilt->getNextFilteredValue(gripper_state_now_velocity);
00143 
00144   // store a convenient double for our current position
00145   positionCurrent = gripper_state_now_position;
00146 
00147   // call our pressure sensor and accelerometer update algorithm
00148   myPressureObserver->spin();
00149   myAccelerationObserver->spin();
00150 }
00151 
00152 
00153 // method to close the hand and stop when dual contacts are encountered
00154 bool gripperController::grabObject(double close_speed, int contactsDesired)
00155 {
00156   // if the pressure sensors have not yet detected dual impacts
00157   if( !(myPressureObserver->graspContact(contactsDesired)) )
00158   {
00159     velocityServo(close_speed);  // keep closing
00160     return false;
00161   }
00162   // if we have detected dual impacts
00163   else
00164   {
00165     // store the contact position and force
00166     positionContact = positionCurrent;
00167     if(-myPressureObserver->padForce_left_cur_nonbiased < forceContact_l)
00168       forceContact_l = -myPressureObserver->padForce_left_cur_nonbiased;
00169     if(-myPressureObserver->padForce_right_cur_nonbiased < forceContact_r)
00170       forceContact_r = -myPressureObserver->padForce_right_cur_nonbiased;
00171     if(-myPressureObserver->padForce_cur_nonbiased < forceContact)
00172       forceContact = -myPressureObserver->padForce_cur_nonbiased;
00173     return true;
00174   }
00175 }
00176 
00177 
00178 // method to open the hand when gripper transience is encountered
00179 bool gripperController::place(int placeConditions, double acc_trigger, double slip_trigger)
00180 {
00181   bool accContact = myAccelerationObserver->checkPlaceContact(acc_trigger);
00182   bool slipContact = myPressureObserver->checkPlaceContact(slip_trigger);  
00183   int  sideImpact = myPressureObserver->checkSideImpact(0.2);
00184   bool returnValue = false;
00185   
00186   if(placeConditions == 0 )
00187     returnValue = accContact || (sideImpact != -1);
00188   else if(placeConditions == 1 )
00189     returnValue = accContact && slipContact;
00190   else if(placeConditions == 2 )
00191     returnValue = accContact || slipContact || (sideImpact != -1);
00192   else if(placeConditions == 3)
00193     returnValue = slipContact;
00194   else if(placeConditions == 4)
00195     returnValue = accContact;
00196 
00197 
00198   return returnValue;
00199 }
00200 
00201 
00202 // servo slip
00203 bool gripperController::slipServo2()
00204 {
00205   // note: with higher forceservo gains .0005 seems good.
00206   double slipIncrementPercent = 0.002; // .001 worked nicely. .005 seemed a little agressive   // actually percent/100. Since we are "oversampling" the real percentage we are increasing is this *41        
00207 
00208   if( myPressureObserver->checkSlip())
00209   {
00210     servoForce = servoForce + servoForce*slipIncrementPercent;
00211     slip_flag = true;
00212   }
00213   else
00214     slip_flag = false;
00215 
00216   // check for our force limit
00217   if(( servoForce <= fingertip_force_limit ) && (fingertip_force_limit <= 0.0))
00218   {
00219     force_limit_flag = true;
00220     servoForce = fingertip_force_limit;
00221   }
00222   else
00223     force_limit_flag = false;
00224 
00225   forceServo2(servoForce);
00226  
00227   // if the pressure ever drops to less than 0.1N less than our lightest applicable force
00228   // or the gripper ends up closed, consider the object f
00229   if( (-myPressureObserver->padForce_cur_nonbiased > -((myPressureObserver->forceLightest)-0.25)) || positionCurrent <= positionDropped)
00230   {  
00231     dropped_flag = true;
00232     positionMarker = positionCurrent;    // reset our position marker since the object was dropped
00233     jointState->commanded_effort_ = -100.0;   // close as hard as possible
00234   }
00235   else
00236     dropped_flag = false;
00237 
00238  
00239 
00240   return true;
00241 }
00242 
00243 
00244 
00245 
00246 // method to servo the robot to a desired finger force value
00247 bool gripperController::forceServo2(double desired_Force)
00248 {
00249   // do not allow the force controller to apply anything weaker than forceLightest
00250   if(desired_Force > -(myPressureObserver->forceLightest))
00251     desired_Force = -(myPressureObserver->forceLightest);
00252 
00253   // store the force we are currently trying to servo to
00254   servoForce = desired_Force;
00255 
00256   // figure out which finger is seeing less force, and use that value
00257   double minFingerForce = -std::min(myPressureObserver->padForce_left_cur_nonbiased,myPressureObserver->padForce_right_cur_nonbiased);
00258 
00259   // our gain for the force controller
00260   double kP_f =  0.0008;//was 0.0000003; // was .0006
00261   // if we are trying to close more use a strong gain
00262   if( desired_Force-minFingerForce < 0)
00263     kP_f = 0.0016; // .008 breaks eggs (spikes too hard). .003 seems good
00264 
00265   // calculate our force components
00266   double v_force = -kP_f * (minFingerForce - desired_Force);
00267 
00268   // limit our velocity values to limit extremely high velocities on large force descrepancies
00269   double v_bound = 0.50;  // set a limit on the speed (m/s?)  .005 worked  .015 worked  .035 worked  .075 looked good
00270   if(v_force < -v_bound)
00271     v_force = -v_bound;
00272   else if(v_force > v_bound)
00273     v_force = v_bound;
00274 
00275 
00276   // limit our integral (position) term
00277   double p_bound = 0.03;
00278   if( (((vel_integral-positionCurrent) > p_bound) && v_force*dt > 0) ||
00279       (((vel_integral-positionCurrent) < -p_bound) && v_force*dt < 0) )
00280     {}
00281   else
00282   {
00283     // integrate the velocity over time
00284     vel_integral += v_force * dt;
00285   }
00286 
00287   // assign our position to be the integrated velocity value
00288   double p_force = vel_integral;
00289 
00290   // zero closes gripper    
00291   positionServo(p_force,v_force);
00292 
00293   // return true if force achieved within some tolerance
00294   if(fabs(minFingerForce-desired_Force) < force_servo_force_tolerance)
00295     return true;
00296   else
00297     return false;
00298 }
00299 
00300 
00301 // method to ramp the current force to a specific value over a defined timeperiod
00302 bool gripperController::forceRampTo(double force, double duration)
00303 {
00304   // if it is the first time entering this forceRampTo function
00305   if(firstRamp)                                
00306   {
00307     // store the time and force we started at
00308     ramp_start_time = time_cur;      
00309     ramp_start_force = -std::max(myPressureObserver->padForce_left_cur_nonbiased,myPressureObserver->padForce_right_cur_nonbiased); 
00310     firstRamp = false;
00311     return false;
00312   }
00313 
00314   // if the time of the ramp has not expired ramp down linearly
00315   else if(time_cur-ramp_start_time < duration)
00316   {
00317     forceServo2( ((force-ramp_start_force)*((time_cur - ramp_start_time)/duration)) + ramp_start_force);
00318     return false;
00319   }
00320 
00321   return true;
00322 
00323 }
00324 
00325 
00326 bool gripperController::velocityServo(double desiredVel)
00327 { 
00328   // integrate the velocity over time
00329   vel_integral_vcontrol += desiredVel * dt;
00330   return positionServo(vel_integral_vcontrol, desiredVel);
00331 }
00332 
00333 
00334 // method to position servo the robot to a desired position. non-blocking. requires continuous polling to accurately update torque information
00335 // @input desiredPos - the desired position to move to. 
00336 // @input desireVel - the desiredVelocity to try to move at
00337 // @output - true if the desired position has been achieved (within some threshold), false if it has not yet been achieved
00338 bool gripperController::positionServo(double desiredPos, double desiredVel)
00339 {
00340 
00341   // NOTE: This is coded so that it only stops the robot from closing too much, not opening
00342   // check if we are using a deformation limit
00343   // and if we are violating that limit, and trying to violate it more
00344   if( (positionMarker_limit >= 0.0) && (positionCurrent < positionMarker-positionMarker_limit) && (desiredPos < positionCurrent) )
00345     desiredPos = positionMarker-positionMarker_limit;
00346   if( (positionMarker_limit >= 0.0) && (positionCurrent < positionMarker-positionMarker_limit) )
00347     deformation_limit_flag = true;
00348   else
00349     deformation_limit_flag = false;
00350     
00351   
00352   // calculate our force components
00353   double pos_force = -kP * (gripper_state_now_position - desiredPos);
00354   double vel_force = -kD * (gripper_state_now_velocity - desiredVel);
00355 
00356   // deal with coulomb friction in the gripper
00357   if      (desiredVel > 0.0)  vel_force += coulomb;
00358   else if (desiredVel < 0.0)  vel_force -= coulomb;
00359 
00360   double fbk_force = pos_force + vel_force;
00361 
00362   // ensure we do not exceed the max_effort value
00363   if (max_effort >= 0.0)
00364     fbk_force = std::max(-max_effort, std::min(fbk_force, max_effort)); 
00365   
00366   // ensure that we do not exceed our deformation limit
00367   jointState->commanded_effort_ =  fbk_force;
00368 
00369   return false;
00370 }
00371 
00372 
00373 // method returns true when initialization complete
00374 bool gripperController::initializeGripper()
00375 {
00376     return true;
00377 }
00378 
00379 // ctor
00380 gripperController::gripperController(pr2_mechanism_model::JointState* joint_state_, pressureObserver* pressure_observer_, accelerationObserver* acceleration_observer_)
00381 {
00382   // store our gripper joint state handle
00383   jointState = joint_state_;
00384 
00385   // store our pressure observer
00386   myPressureObserver = pressure_observer_;
00387 
00388   // store our acceleration observer
00389   myAccelerationObserver = acceleration_observer_;
00390 
00391   // setup our position controller gain values
00392   kP = 20000; // 20000
00393   kD = 5000;  // 5000
00394   coulomb = 7;
00395 
00396   // some default's for our variables
00397   positionOpen = 0.1;         // param server stuff?
00398   positionClosed = 0.0;       // param server stuff?
00399   vel_integral = 0.0;
00400   positionDropped = 0.003;    // 3mm
00401   servoForce = 0.0;
00402   accThresh = 4.0;
00403 
00404   // instatiate our digital filter
00405   // 1st order butterworth, 5 hz lp
00406   float b_vfilt[] = { 0.0155, 0.0155};
00407   float a_vfilt[] = {1.0, -0.9691};
00408   velocityLPFilt = new digitalFilter(1, true,b_vfilt,a_vfilt);
00409   
00410   // our timestep time we expect
00411   dt = 0.001;  // 1 ms
00412   
00413   // store the initial position                                                                                                                                   
00414   initialPosition = jointState->position_;
00415 
00416   // set the firstInitialize state to true
00417   firstRamp = true;
00418 
00419 }
00420 
00421 
00422 // destructor
00423 gripperController::~gripperController()
00424 {}
00425 
00426 
00427 #endif // _GRIPPER_CONTROLLER


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