kdl_fwd_kin_chain.h

Go to the documentation of this file.

 
#ifndef TESSERACT_KINEMATICS_KDL_FWD_KINEMATIC_CHAIN_H
#define TESSERACT_KINEMATICS_KDL_FWD_KINEMATIC_CHAIN_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <kdl/tree.hpp>
#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainjnttojacsolver.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/forward_kinematics.h>
#include <tesseract_kinematics/kdl/kdl_utils.h>
 
namespace tesseract_kinematics
{
static const std::string KDL_FWD_KIN_CHAIN_SOLVER_NAME = "KDLFwdKinChain";
 
class KDLFwdKinChain : public ForwardKinematics
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  using Ptr = std::shared_ptr<KDLFwdKinChain>;
  using ConstPtr = std::shared_ptr<const KDLFwdKinChain>;
  using UPtr = std::unique_ptr<KDLFwdKinChain>;
  using ConstUPtr = std::unique_ptr<const KDLFwdKinChain>;
 
  ~KDLFwdKinChain() override = default;
  KDLFwdKinChain(const KDLFwdKinChain& other);
  KDLFwdKinChain& operator=(const KDLFwdKinChain& other);
  KDLFwdKinChain(KDLFwdKinChain&&) = delete;
  KDLFwdKinChain& operator=(KDLFwdKinChain&&) = delete;
 
  KDLFwdKinChain(const tesseract_scene_graph::SceneGraph& scene_graph,
                 const std::string& base_link,
                 const std::string& tip_link,
                 std::string solver_name = KDL_FWD_KIN_CHAIN_SOLVER_NAME);
 
  KDLFwdKinChain(const tesseract_scene_graph::SceneGraph& scene_graph,
                 const std::vector<std::pair<std::string, std::string> >& chains,
                 std::string solver_name = KDL_FWD_KIN_CHAIN_SOLVER_NAME);
 
  tesseract_common::TransformMap calcFwdKin(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const override final;
 
  Eigen::MatrixXd calcJacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
                               const std::string& joint_link_name) const override final;
 
  std::string getBaseLinkName() const override final;
  std::vector<std::string> getJointNames() const override final;
  std::vector<std::string> getTipLinkNames() const override final;
  Eigen::Index numJoints() const override final;
  std::string getSolverName() const override final;
  ForwardKinematics::UPtr clone() const override final;
 
private:
  KDLChainData kdl_data_;                                      
  std::string name_;                                           
  std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_; 
  std::unique_ptr<KDL::ChainJntToJacSolver> jac_solver_;       
  std::string solver_name_{ KDL_FWD_KIN_CHAIN_SOLVER_NAME };   
  mutable std::mutex mutex_; 
  tesseract_common::TransformMap calcFwdKinHelperAll(const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const;
 
  bool calcJacobianHelper(KDL::Jacobian& jacobian,
                          const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
                          int segment_num = -1) const;
 
};  // class KDLKinematicChain
 
}  // namespace tesseract_kinematics
#endif  // TESSERACT_KDL_KINEMATIC_CHAIN_H