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 <pr2_controller_interface/controller.h> 00038 #include <control_toolbox/pid.h> 00039 #include <boost/thread/condition.hpp> 00040 #include <realtime_tools/realtime_publisher.h> 00041 #include <std_msgs/Float64.h> 00042 #include <pr2_controllers_msgs/JointControllerState.h> 00043 00044 00045 #include <sr_robot_msgs/SetMixedPositionVelocityPidGains.h> 00046 00047 #include <sr_utilities/calibration.hpp> 00048 00049 namespace sr_friction_compensation 00050 { 00051 00052 class SrFrictionCompensator 00053 { 00054 public: 00055 00056 SrFrictionCompensator(std::string joint_name); 00057 ~SrFrictionCompensator(); 00058 00080 double friction_compensation( double position, double velocity, int force_demand, int deadband ); 00081 00082 private: 00084 ros::NodeHandle node_; 00085 00094 std::pair<std::vector<joint_calibration::Point>, std::vector<joint_calibration::Point> > read_friction_map(); 00095 00104 std::vector<joint_calibration::Point> read_one_way_map(XmlRpc::XmlRpcValue raw_map); 00105 00112 std::vector<joint_calibration::Point> generate_flat_map(); 00113 00115 boost::shared_ptr<shadow_robot::JointCalibration> friction_interpoler_forward; 00117 boost::shared_ptr<shadow_robot::JointCalibration> friction_interpoler_backward; 00118 00120 std::string joint_name_; 00121 00123 static const double velocity_for_static_friction; 00124 }; 00125 } // namespace 00126 00127 /* For the emacs weenies in the crowd. 00128 Local Variables: 00129 c-basic-offset: 2 00130 End: 00131 */ 00132 00133 00134 #endif