kdl_inv_kin_chain_lma.h

Go to the documentation of this file.

 
#ifndef TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_LMA_H
#define TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_LMA_H
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <kdl/chainiksolverpos_lma.hpp>
#include <array>
#include <mutex>
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_LMA_SOLVER_NAME = "KDLInvKinChainLMA";
 
class KDLInvKinChainLMA : public InverseKinematics
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  using Ptr = std::shared_ptr<KDLInvKinChainLMA>;
  using ConstPtr = std::shared_ptr<const KDLInvKinChainLMA>;
  using UPtr = std::unique_ptr<KDLInvKinChainLMA>;
  using ConstUPtr = std::unique_ptr<const KDLInvKinChainLMA>;
 
  struct Config
  {
    std::array<double, 6> task_weights{ 1.0, 1.0, 1.0, 0.1, 0.1, 0.1 };
    double eps{ 1E-5 };
    int max_iterations{ 500 };
    double eps_joints{ 1E-15 };
  };
 
  ~KDLInvKinChainLMA() override = default;
  KDLInvKinChainLMA(const KDLInvKinChainLMA& other);
  KDLInvKinChainLMA& operator=(const KDLInvKinChainLMA& other);
  KDLInvKinChainLMA(KDLInvKinChainLMA&&) = delete;
  KDLInvKinChainLMA& operator=(KDLInvKinChainLMA&&) = delete;
 
  KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph,
                    const std::string& base_link,
                    const std::string& tip_link,
                    Config kdl_config,
                    std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME);
 
  KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph,
                    const std::vector<std::pair<std::string, std::string> >& chains,
                    Config kdl_config,
                    std::string solver_name = KDL_INV_KIN_CHAIN_LMA_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_;                                        
  Config kdl_config_;                                            
  std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver_;         
  std::string solver_name_{ KDL_INV_KIN_CHAIN_LMA_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_LMA_H