sr_friction_compensation.cpp
Go to the documentation of this file.
00001 
00026 #include "sr_mechanism_controllers/sr_friction_compensation.hpp"
00027 
00028 #include <sstream>
00029 #include <math.h>
00030 #include "sr_utilities/sr_math_utils.hpp"
00031 #include "sr_utilities/sr_deadband.hpp"
00032 
00033 namespace sr_friction_compensation
00034 {
00035   const double SrFrictionCompensator::velocity_for_static_friction = 0.01;
00036 
00037   SrFrictionCompensator::SrFrictionCompensator(std::string joint_name)
00038   {
00039     joint_name_ = joint_name;
00040 
00041     std::pair<std::vector<joint_calibration::Point>, std::vector<joint_calibration::Point> > both_maps = read_friction_map();
00042     friction_interpoler_forward = boost::shared_ptr<shadow_robot::JointCalibration>( new shadow_robot::JointCalibration( both_maps.first ) );
00043     friction_interpoler_backward = boost::shared_ptr<shadow_robot::JointCalibration>( new shadow_robot::JointCalibration( both_maps.second ) );
00044   }
00045 
00046   SrFrictionCompensator::~SrFrictionCompensator()
00047   {}
00048 
00049   double SrFrictionCompensator::friction_compensation( double position, double velocity, int force_demand, int deadband )
00050   {
00051     double compensation = 0.0;
00052 
00053     if( force_demand > 0.0 )
00054       compensation = friction_interpoler_forward->compute( position );
00055     else
00056       compensation = friction_interpoler_backward->compute( position );
00057 
00058     //A value by which we'll multiply the compensation computed by the
00059     // friction map.
00060     double mult = 1.0;
00061     //we're out of the "finger is stopped" zone ->
00062     //   progressively decrease the amount of compensation
00063     if(fabs(velocity) > velocity_for_static_friction)
00064     {
00065       if( velocity < 0.0 )
00066         mult = exp( -fabs(velocity + velocity_for_static_friction)*20 );
00067       else
00068         mult = exp( -fabs(velocity - velocity_for_static_friction)*20 );
00069     }
00070 
00071     //decrease the compensation around the force sign change
00072     // to have less discontinuity. Use the deadband for this.
00073     if( abs(force_demand) < deadband )
00074     {
00075       //we want the multiplier to be 1 if force_demand = deadband
00076       // and 0 if force_demand = 0
00077       mult *= sr_math_utils::linear_interpolate_(static_cast<double>(abs(force_demand)),
00078                                                 0.0, 0.0,
00079                                                 static_cast<double>(deadband), 1.0);
00080     }
00081 
00082     //amend the compensation value
00083     compensation *= mult;
00084 
00085     return compensation;
00086   }
00087 
00088   std::pair<std::vector<joint_calibration::Point>, std::vector<joint_calibration::Point> > SrFrictionCompensator::read_friction_map()
00089   {
00090     std::vector<joint_calibration::Point> friction_map_forward, friction_map_backward;
00091     std::string param_name = "/sr_friction_map";
00092 
00093     bool joint_not_found_forward = true;
00094     bool joint_not_found_backward = true;
00095 
00096     XmlRpc::XmlRpcValue calib;
00097     if( node_.hasParam(param_name) )
00098     {
00099       node_.getParam(param_name, calib);
00100 
00101       ROS_DEBUG_STREAM("  Reading friction for: " <<  joint_name_);
00102       ROS_DEBUG_STREAM(" value: " << calib);
00103 
00104       ROS_ASSERT(calib.getType() == XmlRpc::XmlRpcValue::TypeArray);
00105       //iterate on all the joints
00106       for(int32_t index_cal = 0; index_cal < calib.size(); ++index_cal)
00107       {
00108         //check the calibration is well formatted:
00109         // first joint name, then calibration table
00110         ROS_ASSERT(calib[index_cal][0].getType() == XmlRpc::XmlRpcValue::TypeString);
00111         ROS_ASSERT(calib[index_cal][1].getType() == XmlRpc::XmlRpcValue::TypeInt);
00112         ROS_ASSERT(calib[index_cal][2].getType() == XmlRpc::XmlRpcValue::TypeArray);
00113 
00114         std::string joint_name_tmp = static_cast<std::string> (calib[index_cal][0]);
00115 
00116         ROS_DEBUG_STREAM("  Checking joint name: "<< joint_name_tmp << " / " << joint_name_);
00117         if(  joint_name_tmp.compare( joint_name_ ) != 0 )
00118           continue;
00119 
00120         //reading the forward map:
00121         if( static_cast<int>(calib[index_cal][1]) == 1 )
00122         {
00123           joint_not_found_forward = false;
00124 
00125           friction_map_forward = read_one_way_map(calib[index_cal][2]);
00126         }
00127         else
00128         {
00129           joint_not_found_backward = false;
00130 
00131           friction_map_backward = read_one_way_map(calib[index_cal][2]);
00132         }
00133       }
00134     }
00135 
00136     if( joint_not_found_forward )
00137     {
00138       ROS_INFO_STREAM("  No forward friction compensation map for: " << joint_name_ );
00139 
00140       friction_map_forward = generate_flat_map();
00141     }
00142 
00143     if( joint_not_found_backward )
00144     {
00145       ROS_INFO_STREAM("  No backward friction compensation map for: " << joint_name_ );
00146 
00147       friction_map_backward = generate_flat_map();
00148     }
00149 
00150     /*
00151       ROS_INFO_STREAM(" Friction map forward: [" << joint_name_ << "]");
00152       for( unsigned int i=0; i<friction_map_forward.size(); ++i )
00153       ROS_INFO_STREAM("    -> position=" << friction_map_forward[i].raw_value << " compensation: " << friction_map_forward[i].calibrated_value);
00154       ROS_INFO_STREAM(" Friction map backward: [" << joint_name_ << "]");
00155       for( unsigned int i=0; i<friction_map_backward.size(); ++i )
00156       ROS_INFO_STREAM("    -> position=" << friction_map_backward[i].raw_value << " compensation: " << friction_map_backward[i].calibrated_value);
00157     */
00158 
00159     std::pair<std::vector<joint_calibration::Point>, std::vector<joint_calibration::Point> > both_maps;
00160     both_maps.first = friction_map_forward;
00161     both_maps.second = friction_map_backward;
00162 
00163     return both_maps;
00164   } //end read_friction_map
00165 
00166   std::vector<joint_calibration::Point> SrFrictionCompensator::read_one_way_map(XmlRpc::XmlRpcValue raw_map)
00167   {
00168     std::vector<joint_calibration::Point> friction_map;
00169 
00170     //now iterates on the calibration table for the current joint
00171     for(int32_t index_table=0; index_table < raw_map.size(); ++index_table)
00172     {
00173       ROS_ASSERT(raw_map[index_table].getType() == XmlRpc::XmlRpcValue::TypeArray);
00174       //only 2 values per calibration point: raw and calibrated (doubles)
00175       ROS_ASSERT(raw_map[index_table].size() == 2);
00176       ROS_ASSERT(raw_map[index_table][0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00177       ROS_ASSERT(raw_map[index_table][1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00178 
00179 
00180       joint_calibration::Point point_tmp;
00181       point_tmp.raw_value = sr_math_utils::to_rad(static_cast<double> (raw_map[index_table][0]));
00182       point_tmp.calibrated_value = static_cast<double> (raw_map[index_table][1]);
00183       friction_map.push_back(point_tmp);
00184     }
00185 
00186     return friction_map;
00187   }
00188 
00189   std::vector<joint_calibration::Point> SrFrictionCompensator::generate_flat_map()
00190   {
00191     std::vector<joint_calibration::Point> friction_map;
00192 
00193     joint_calibration::Point point_tmp;
00194     point_tmp.raw_value = 0.0;
00195     point_tmp.calibrated_value = 0.0;
00196     friction_map.push_back(point_tmp);
00197     point_tmp.raw_value = 1.0;
00198     friction_map.push_back(point_tmp);
00199 
00200     return friction_map;
00201   }
00202 }
00203 
00204 
00205 
00206 /* For the emacs weenies in the crowd.
00207    Local Variables:
00208    c-basic-offset: 2
00209    End:
00210 */
00211 
00212 


sr_mechanism_controllers
Author(s): Ugo Cupcic / ugo@shadowrobot.com , contact@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:54:39