18 #ifndef KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_HPP_
19 #define KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_HPP_
25 #include "eigen3/Eigen/Core"
26 #include "eigen3/Eigen/Geometry"
27 #include "eigen3/Eigen/LU"
28 #include "rclcpp/logging.hpp"
29 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
44 std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
45 const std::string & end_effector_name) = 0;
56 const Eigen::VectorXd & joint_pos,
const Eigen::Matrix<double, 6, 1> & delta_x,
57 const std::string & link_name, Eigen::VectorXd & delta_theta) = 0;
68 const Eigen::VectorXd & joint_pos,
const Eigen::VectorXd & delta_theta,
69 const std::string & link_name, Eigen::Matrix<double, 6, 1> & delta_x) = 0;
79 const Eigen::VectorXd & joint_pos,
const std::string & link_name,
80 Eigen::Isometry3d & transform) = 0;
90 const Eigen::VectorXd & joint_pos,
const std::string & link_name,
91 Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian) = 0;
94 std::vector<double> & joint_pos_vec,
const std::vector<double> & delta_x_vec,
95 const std::string & link_name, std::vector<double> & delta_theta_vec);
98 const std::vector<double> & joint_pos_vec,
const std::vector<double> & delta_theta_vec,
99 const std::string & link_name, std::vector<double> & delta_x_vec);
102 const std::vector<double> & joint_pos_vec,
const std::string & link_name,
103 Eigen::Isometry3d & transform);
106 const std::vector<double> & joint_pos_vec,
const std::string & link_name,
107 Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
Definition: kinematics_interface.hpp:34
virtual bool initialize(std::shared_ptr< rclcpp::node_interfaces::NodeParametersInterface > parameters_interface, const std::string &end_effector_name)=0
Initialize plugin. This method must be called before any other.
virtual 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)=0
Convert joint delta-theta to Cartesian delta-x.
virtual 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)=0
Convert Cartesian delta-x to joint delta-theta, using the Jacobian.
virtual bool calculate_jacobian(const Eigen::VectorXd &joint_pos, const std::string &link_name, Eigen::Matrix< double, 6, Eigen::Dynamic > &jacobian)=0
Calculates the jacobian for a specified link using provided joint positions.
virtual bool calculate_link_transform(const Eigen::VectorXd &joint_pos, const std::string &link_name, Eigen::Isometry3d &transform)=0
Calculates the joint transform for a specified link using provided joint positions.
Definition: kinematics_interface.hpp:32