43 const std::string & robot_description,
44 std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
45 const std::string & param_namespace)
override;
48 const Eigen::VectorXd & joint_pos,
const Eigen::Matrix<double, 6, 1> & delta_x,
49 const std::string & link_name, Eigen::VectorXd & delta_theta)
override;
52 const Eigen::VectorXd & joint_pos,
const Eigen::VectorXd & delta_theta,
53 const std::string & link_name, Eigen::Matrix<double, 6, 1> & delta_x)
override;
56 const Eigen::VectorXd & joint_pos,
const std::string & link_name,
57 Eigen::Isometry3d & transform)
override;
60 const Eigen::VectorXd & joint_pos,
const std::string & link_name,
61 Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian)
override;
64 const Eigen::VectorXd & joint_pos,
const std::string & link_name,
65 Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse)
override;
68 Eigen::Matrix<double, 7, 1> & x_a, Eigen::Matrix<double, 7, 1> & x_b,
double dt,
69 Eigen::Matrix<double, 6, 1> & delta_x)
override;
73 bool verify_initialized();
74 bool verify_link_name(
const std::string & link_name);
75 bool verify_joint_vector(
const Eigen::VectorXd & joint_vector);
76 bool verify_jacobian(
const Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
77 bool verify_jacobian_inverse(
const Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian);
78 bool verify_period(
const double dt);
80 bool initialized =
false;
81 std::string root_name_;
84 std::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_;
86 Eigen::Matrix<KDL::Frame, 2, 1> frames_;
88 std::shared_ptr<KDL::Jacobian> jacobian_;
89 std::shared_ptr<Eigen::Matrix<double, Eigen::Dynamic, 6>> jacobian_inverse_;
90 std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
91 std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface_;
92 std::unordered_map<std::string, int> link_name_map_;