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 00081 double friction_compensation( double position, double velocity, int force_demand, int deadband ); 00082 00083 private: 00085 ros::NodeHandle node_; 00086 00095 std::pair<std::vector<joint_calibration::Point>, std::vector<joint_calibration::Point> > read_friction_map(); 00096 00105 std::vector<joint_calibration::Point> read_one_way_map(XmlRpc::XmlRpcValue raw_map); 00106 00113 std::vector<joint_calibration::Point> generate_flat_map(); 00114 00116 boost::shared_ptr<shadow_robot::JointCalibration> friction_interpoler_forward; 00118 boost::shared_ptr<shadow_robot::JointCalibration> friction_interpoler_backward; 00119 00121 std::string joint_name_; 00122 00124 static const double velocity_for_static_friction; 00125 }; 00126 } // namespace 00127 00128 /* For the emacs weenies in the crowd. 00129 Local Variables: 00130 c-basic-offset: 2 00131 End: 00132 */ 00133 00134 00135 #endif