sr_friction_compensation.hpp

Go to the documentation of this file.

/*
* Copyright 2011 Shadow Robot Company Ltd.
*
* This program is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
* Software Foundation version 2 of the License.
*
* This program is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef _SR_FRICTION_COMPENSATION_HPP_
#define _SR_FRICTION_COMPENSATION_HPP_

#include <ros/node_handle.h>

#include <boost/scoped_ptr.hpp>

#include <utility>
#include <string>
#include <vector>

#include <control_toolbox/pid.h>
#include <boost/thread/condition.hpp>
#include <realtime_tools/realtime_publisher.h>
#include <std_msgs/Float64.h>


#include <sr_robot_msgs/SetMixedPositionVelocityPidGains.h>

#include <sr_utilities/calibration.hpp>

namespace sr_friction_compensation
{
class SrFrictionCompensator
{
public:
  explicit SrFrictionCompensator(const std::string &joint_name);

  ~SrFrictionCompensator();

  double friction_compensation(double position, double velocity, int force_demand, int deadband);

private:
  ros::NodeHandle node_;

  std::pair<std::vector<joint_calibration::Point>, std::vector<joint_calibration::Point> > read_friction_map();

  std::vector<joint_calibration::Point> read_one_way_map(XmlRpc::XmlRpcValue &raw_map);

  std::vector<joint_calibration::Point> generate_flat_map();

  boost::scoped_ptr<shadow_robot::JointCalibration> friction_interpoler_forward;
  boost::scoped_ptr<shadow_robot::JointCalibration> friction_interpoler_backward;

  // the joint name
  std::string joint_name_;

  // the threshold under which we use the static friction map
  static const double velocity_for_static_friction;
};
}  // namespace sr_friction_compensation

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/


#endif