00001 00026 #ifndef _SR_FRICTION_COMPENSATION_HPP_ 00027 #define _SR_FRICTION_COMPENSATION_HPP_ 00028 00029 #include <ros/node_handle.h> 00030 00031 #include <boost/scoped_ptr.hpp> 00032 00033 #include <utility> 00034 00035 00036 00037 #include <control_toolbox/pid.h> 00038 #include <boost/thread/condition.hpp> 00039 #include <realtime_tools/realtime_publisher.h> 00040 #include <std_msgs/Float64.h> 00041 00042 00043 #include <sr_robot_msgs/SetMixedPositionVelocityPidGains.h> 00044 00045 #include <sr_utilities/calibration.hpp> 00046 00047 namespace sr_friction_compensation 00048 { 00049 00050 class SrFrictionCompensator 00051 { 00052 public: 00053 00054 SrFrictionCompensator(const std::string &joint_name); 00055 ~SrFrictionCompensator(); 00056 00079 double friction_compensation( double position, double velocity, int force_demand, int deadband ); 00080 00081 private: 00083 ros::NodeHandle node_; 00084 00093 std::pair<std::vector<joint_calibration::Point>, std::vector<joint_calibration::Point> > read_friction_map(); 00094 00103 std::vector<joint_calibration::Point> read_one_way_map(XmlRpc::XmlRpcValue &raw_map); 00104 00111 std::vector<joint_calibration::Point> generate_flat_map(); 00112 00114 boost::scoped_ptr<shadow_robot::JointCalibration> friction_interpoler_forward; 00116 boost::scoped_ptr<shadow_robot::JointCalibration> friction_interpoler_backward; 00117 00119 std::string joint_name_; 00120 00122 static const double velocity_for_static_friction; 00123 }; 00124 } // namespace 00125 00126 /* For the emacs weenies in the crowd. 00127 Local Variables: 00128 c-basic-offset: 2 00129 End: 00130 */ 00131 00132 00133 #endif