kdl_manager.hpp

Go to the documentation of this file.

#ifndef __KDL_MANAGER__
#define __KDL_MANAGER__

#include <eigen_conversions/eigen_kdl.h>
#include <eigen_conversions/eigen_msg.h>
#include <generic_control_toolbox/ArmInfo.h>
#include <geometry_msgs/WrenchStamped.h>
#include <kdl_conversions/kdl_msg.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_listener.h>
#include <urdf/model.h>
#include <generic_control_toolbox/manager_base.hpp>
#include <generic_control_toolbox/matrix_parser.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainiksolvervel_pinv_nso.hpp>
#include <kdl/chainiksolvervel_wdls.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/frames.hpp>
#include <kdl/kdl.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <stdexcept>

const std::string WDLS_SOLVER("wdls"), NSO_SOLVER("nso");

namespace generic_control_toolbox
{
class KDLManager : public ManagerBase
{
 public:
  KDLManager(const std::string &chain_base_link,
             ros::NodeHandle nh = ros::NodeHandle("~"));
  ~KDLManager();

  bool initializeArm(const std::string &end_effector_link);

  bool isInitialized(const std::string &end_effector_link) const;

  bool checkStateMessage(const std::string &end_effector_link,
                         const sensor_msgs::JointState &state) const;
  bool setGrippingPoint(const std::string &end_effector_link,
                        const std::string &gripping_point_frame);

  bool setSensorPoint(const std::string &end_effector_link,
                      const std::string &sensor_point_frame);

  bool createJointState(const std::string &end_effector_link,
                        const Eigen::VectorXd &q, const Eigen::VectorXd &qdot,
                        sensor_msgs::JointState &state) const;
  bool getJointState(const std::string &end_effector_link,
                     const Eigen::VectorXd &qdot,
                     sensor_msgs::JointState &state) const;

  bool getJointState(const std::string &end_effector_link,
                     const Eigen::VectorXd &q, const Eigen::VectorXd &qdot,
                     sensor_msgs::JointState &state) const;

  bool getJointState(const std::string &end_effector_link,
                     const Eigen::VectorXd &q, const Eigen::VectorXd &qdot,
                     const Eigen::VectorXd &effort,
                     sensor_msgs::JointState &state) const;

  bool getGrippingPoint(const std::string &end_effector_link,
                        const sensor_msgs::JointState &state,
                        KDL::Frame &out) const;

  bool getSensorPoint(const std::string &end_effector_link,
                      const sensor_msgs::JointState &state,
                      KDL::Frame &out) const;

  bool getGrippingTwist(const std::string &end_effector_link,
                        const sensor_msgs::JointState &state,
                        KDL::Twist &out) const;

  bool verifyPose(const std::string &end_effector_link,
                  const KDL::Frame &in) const;

  bool getPoseIK(const std::string &end_effector_link,
                 const sensor_msgs::JointState &state, const KDL::Frame &in,
                 KDL::JntArray &out) const;

  bool getGrippingPoseIK(const std::string &end_effector_link,
                         const sensor_msgs::JointState &state,
                         const KDL::Frame &in, KDL::JntArray &out) const;

  bool getPoseFK(const std::string &end_effector_link,
                 const sensor_msgs::JointState &state, const KDL::JntArray &in,
                 KDL::Frame &out) const;

  bool getGrippingVelIK(const std::string &end_effector_link,
                        const sensor_msgs::JointState &state,
                        const KDL::Twist &in, KDL::JntArray &out) const;

  bool grippingTwistToEef(const std::string &end_effector_link,
                          const sensor_msgs::JointState &state,
                          const KDL::Twist &in, KDL::Twist &out) const;

  bool getVelIK(const std::string &end_effector_link,
                const sensor_msgs::JointState &state, const KDL::Twist &in,
                KDL::JntArray &out) const;

  bool getJacobian(const std::string &end_effector_link,
                   const sensor_msgs::JointState &state,
                   KDL::Jacobian &out) const;

  bool getEefPose(const std::string &end_effector_link,
                  const sensor_msgs::JointState &state, KDL::Frame &out) const;

  bool getEefTwist(const std::string &end_effector_link,
                   const sensor_msgs::JointState &state,
                   KDL::FrameVel &out) const;

  bool getJointLimits(const std::string &end_effector_link,
                      KDL::JntArray &q_min, KDL::JntArray &q_max,
                      KDL::JntArray &q_vel_lim) const;

  bool getJointPositions(const std::string &end_effector_link,
                         const sensor_msgs::JointState &state,
                         KDL::JntArray &q) const;
  bool getJointPositions(const std::string &end_effector_link,
                         const sensor_msgs::JointState &state,
                         Eigen::VectorXd &q) const;

  bool getJointVelocities(const std::string &end_effector_link,
                          const sensor_msgs::JointState &state,
                          KDL::JntArray &q_dot) const;

  bool getInertia(const std::string &end_effector_link,
                  const sensor_msgs::JointState &state, Eigen::MatrixXd &H);

  bool getGravity(const std::string &end_effector_link,
                  const sensor_msgs::JointState &state, Eigen::MatrixXd &g);

  bool getCoriolis(const std::string &end_effector_link,
                   const sensor_msgs::JointState &state,
                   Eigen::MatrixXd &coriolis);

  bool getNumJoints(const std::string &end_effector_link,
                    unsigned int &num_joints) const;

  bool addSegment(const std::string &end_effector_link,
                  KDL::Segment &new_segment);

 private:
  typedef KDL::ChainIkSolverVel IkSolverVel;
  typedef KDL::ChainIkSolverPos_LMA IkSolverPos;
  typedef KDL::ChainFkSolverPos_recursive FkSolverPos;
  typedef KDL::ChainFkSolverVel_recursive FkSolverVel;
  typedef KDL::ChainJntToJacSolver JacSolver;

  typedef std::shared_ptr<IkSolverVel> IkSolverVelPtr;
  typedef std::shared_ptr<IkSolverPos> IkSolverPosPtr;
  typedef std::shared_ptr<FkSolverPos> FkSolverPosPtr;
  typedef std::shared_ptr<FkSolverVel> FkSolverVelPtr;
  typedef std::shared_ptr<JacSolver> JacSolverPtr;
  typedef std::shared_ptr<KDL::ChainDynParam> ChainDynParamPtr;

  std::map<std::string, IkSolverVelPtr> ikvel_;
  std::map<std::string, IkSolverPosPtr> ikpos_;
  std::map<std::string, FkSolverPosPtr> fkpos_;
  std::map<std::string, FkSolverVelPtr> fkvel_;
  std::map<std::string, JacSolverPtr> jac_solver_;
  std::map<std::string, KDL::Frame> eef_to_gripping_point_;
  std::map<std::string, KDL::Frame> eef_to_sensor_point_;
  std::map<std::string, KDL::Chain> chain_;
  std::map<std::string, ChainDynParamPtr> dynamic_chain_;

  urdf::Model model_;
  ros::NodeHandle nh_;
  tf::TransformListener listener_;
  KDL::Vector gravity_in_chain_base_link_;
  std::map<std::string, std::vector<std::string> >
      actuated_joint_names_;  
  std::string chain_base_link_, ikvel_solver_;
  double eps_, max_tf_attempts_, nso_weight_, ik_pos_tolerance_,
      ik_angle_tolerance_;

  bool getParam();

  bool initializeArmCommon(const std::string &end_effector_link);

  bool getRigidTransform(const std::string &base_frame,
                         const std::string &target_frame,
                         KDL::Frame &out) const;

  bool getChainJointState(const sensor_msgs::JointState &current_state,
                          const std::string &end_effector_link,
                          KDL::JntArray &positions,
                          KDL::JntArrayVel &velocities) const;

  bool hasJoint(const KDL::Chain &chain, const std::string &joint_name) const;

  void updateSolvers();
};

bool setKDLManager(const ArmInfo &arm_info,
                   std::shared_ptr<KDLManager> manager);

bool setKDLManagerNso(const ArmInfo &arm_info,
                      std::shared_ptr<KDLManager> manager);
}  // namespace generic_control_toolbox
#endif