joint_position_integrator.hpp

Go to the documentation of this file.

#ifndef __VELOCITY_TO_POSITION__
#define __VELOCITY_TO_POSITION__

#include <ros/ros.h>
#include <kdl/jntarray.hpp>

namespace generic_control_toolbox
{
class JointPositionIntegrator
{
 public:
  JointPositionIntegrator(ros::NodeHandle &nh);
  ~JointPositionIntegrator() {}

  KDL::JntArray update(const KDL::JntArray &q_dot, const KDL::JntArray &q,
                       float dt);
  Eigen::VectorXd update(const Eigen::VectorXd &q_dot, const Eigen::VectorXd &q,
                         float dt);

  void reset();

 private:
  ros::NodeHandle nh_;
  KDL::JntArray virt_q_;
  float max_joint_error_;
  bool got_state_;

  bool init();
};
}  // namespace generic_control_toolbox
#endif