18 #ifndef KINEMATICS_INTERFACE_KDL__KINEMATICS_INTERFACE_KDL_HPP_
19 #define KINEMATICS_INTERFACE_KDL__KINEMATICS_INTERFACE_KDL_HPP_
23 #include <unordered_map>
26 #include "eigen3/Eigen/Core"
27 #include "eigen3/Eigen/LU"
28 #include "kdl/chainfksolverpos_recursive.hpp"
29 #include "kdl/chainfksolvervel_recursive.hpp"
30 #include "kdl/chainjnttojacsolver.hpp"
31 #include "kdl/treejnttojacsolver.hpp"
32 #include "kdl_parser/kdl_parser.hpp"
33 #include "kinematics_interface/kinematics_interface.hpp"
34 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
35 #include "tf2_eigen_kdl/tf2_eigen_kdl.hpp"
43 std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
44 const std::string & end_effector_name)
override;
47 const Eigen::VectorXd & joint_pos,
const Eigen::Matrix<double, 6, 1> & delta_x,
48 const std::string & link_name, Eigen::VectorXd & delta_theta)
override;
51 const Eigen::VectorXd & joint_pos,
const Eigen::VectorXd & delta_theta,
52 const std::string & link_name, Eigen::Matrix<double, 6, 1> & delta_x)
override;
55 const Eigen::VectorXd & joint_pos,
const std::string & link_name,
56 Eigen::Isometry3d & transform)
override;
59 const Eigen::VectorXd & joint_pos,
const std::string & link_name,
60 Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian)
override;
64 bool verify_initialized();
65 bool verify_link_name(
const std::string & link_name);
66 bool verify_joint_vector(
const Eigen::VectorXd & joint_vector);
67 bool verify_jacobian(
const Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
69 bool initialized =
false;
70 std::string root_name_;
73 std::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_;
76 std::shared_ptr<KDL::Jacobian> jacobian_;
77 std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
78 std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface_;
79 std::unordered_map<std::string, int> link_name_map_;
Definition: kinematics_interface.hpp:34
Definition: kinematics_interface_kdl.hpp:40
bool convert_joint_deltas_to_cartesian_deltas(const Eigen::VectorXd &joint_pos, const Eigen::VectorXd &delta_theta, const std::string &link_name, Eigen::Matrix< double, 6, 1 > &delta_x) override
Convert joint delta-theta to Cartesian delta-x.
Definition: kinematics_interface_kdl.cpp:86
bool calculate_link_transform(const Eigen::VectorXd &joint_pos, const std::string &link_name, Eigen::Isometry3d &transform) override
Calculates the joint transform for a specified link using provided joint positions.
Definition: kinematics_interface_kdl.cpp:159
bool initialize(std::shared_ptr< rclcpp::node_interfaces::NodeParametersInterface > parameters_interface, const std::string &end_effector_name) override
Initialize plugin. This method must be called before any other.
Definition: kinematics_interface_kdl.cpp:23
bool convert_cartesian_deltas_to_joint_deltas(const Eigen::VectorXd &joint_pos, const Eigen::Matrix< double, 6, 1 > &delta_x, const std::string &link_name, Eigen::VectorXd &delta_theta) override
Convert Cartesian delta-x to joint delta-theta, using the Jacobian.
Definition: kinematics_interface_kdl.cpp:109
bool calculate_jacobian(const Eigen::VectorXd &joint_pos, const std::string &link_name, Eigen::Matrix< double, 6, Eigen::Dynamic > &jacobian) override
Calculates the jacobian for a specified link using provided joint positions.
Definition: kinematics_interface_kdl.cpp:137
Definition: kinematics_interface_kdl.hpp:38