kdl_inv_kin_chain_nr.h

Go to the documentation of this file.

 
#ifndef TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_H
#define TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_H
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <kdl/tree.hpp>
#include <kdl/chain.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <unordered_map>
#include <console_bridge/console.h>
#include <mutex>
 
#include <tesseract_scene_graph/graph.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_kinematics/core/inverse_kinematics.h>
#include <tesseract_kinematics/kdl/kdl_utils.h>
 
namespace tesseract_kinematics
{
static const std::string KDL_INV_KIN_CHAIN_NR_SOLVER_NAME = "KDLInvKinChainNR";
 
class KDLInvKinChainNR : public InverseKinematics
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  using Ptr = std::shared_ptr<KDLInvKinChainNR>;
  using ConstPtr = std::shared_ptr<const KDLInvKinChainNR>;
  using UPtr = std::unique_ptr<KDLInvKinChainNR>;
  using ConstUPtr = std::unique_ptr<const KDLInvKinChainNR>;
 
  ~KDLInvKinChainNR() override = default;
  KDLInvKinChainNR(const KDLInvKinChainNR& other);
  KDLInvKinChainNR& operator=(const KDLInvKinChainNR& other);
  KDLInvKinChainNR(KDLInvKinChainNR&&) = delete;
  KDLInvKinChainNR& operator=(KDLInvKinChainNR&&) = delete;
 
  KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph,
                   const std::string& base_link,
                   const std::string& tip_link,
                   std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME);
 
  KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph,
                   const std::vector<std::pair<std::string, std::string> >& chains,
                   std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME);
 
  IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses,
                         const Eigen::Ref<const Eigen::VectorXd>& seed) const override final;
 
  std::vector<std::string> getJointNames() const override final;
  Eigen::Index numJoints() const override final;
  std::string getBaseLinkName() const override final;
  std::string getWorkingFrame() const override final;
  std::vector<std::string> getTipLinkNames() const override final;
  std::string getSolverName() const override final;
  InverseKinematics::UPtr clone() const override final;
 
private:
  KDLChainData kdl_data_;                                       
  std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_;  
  std::unique_ptr<KDL::ChainIkSolverVel_pinv> ik_vel_solver_;   
  std::unique_ptr<KDL::ChainIkSolverPos_NR> ik_solver_;         
  std::string solver_name_{ KDL_INV_KIN_CHAIN_NR_SOLVER_NAME }; 
  mutable std::mutex mutex_; 
  IKSolutions calcInvKinHelper(const Eigen::Isometry3d& pose,
                               const Eigen::Ref<const Eigen::VectorXd>& seed,
                               int segment_num = -1) const;
};
 
}  // namespace tesseract_kinematics
#endif  // TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_H