kdl_utils.h

Go to the documentation of this file.

 
#ifndef TESSERACT_KINEMATICS_KDL_UTILS_H
#define TESSERACT_KINEMATICS_KDL_UTILS_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/tree.hpp>
#include <kdl/chain.hpp>
#include <Eigen/Geometry>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_scene_graph/fwd.h>
 
namespace tesseract_kinematics
{
void KDLToEigen(const KDL::Frame& frame, Eigen::Isometry3d& transform);
 
void EigenToKDL(const Eigen::Isometry3d& transform, KDL::Frame& frame);
 
void KDLToEigen(const KDL::Jacobian& jacobian, Eigen::Ref<Eigen::MatrixXd> matrix);
 
void KDLToEigen(const KDL::Jacobian& jacobian, const std::vector<int>& q_nrs, Eigen::Ref<Eigen::MatrixXd> matrix);
 
void EigenToKDL(const Eigen::Ref<const Eigen::VectorXd>& vec, KDL::JntArray& joints);
 
void KDLToEigen(const KDL::JntArray& joints, Eigen::Ref<Eigen::VectorXd> vec);
 
struct KDLChainData
{
  KDL::Chain robot_chain;                   
  KDL::Tree kdl_tree;                       
  std::vector<std::string> joint_names;     
  std::string base_link_name;               
  std::string tip_link_name;                
  std::map<std::string, int> segment_index; 
  std::vector<std::pair<std::string, std::string>> chains; 
  KDL::JntArray q_min;                                     
  KDL::JntArray q_max;                                     
};
 
bool parseSceneGraph(KDLChainData& results,
                     const tesseract_scene_graph::SceneGraph& scene_graph,
                     const std::vector<std::pair<std::string, std::string>>& chains);
 
bool parseSceneGraph(KDLChainData& results,
                     const tesseract_scene_graph::SceneGraph& scene_graph,
                     const std::string& base_name,
                     const std::string& tip_name);
}  // namespace tesseract_kinematics
#endif  // TESSERACT_KINEMATICS_KDL_UTILS_H