diff --git a/kinematics_interface/CMakeLists.txt b/kinematics_interface/CMakeLists.txt index b97aeeb..5ecab38 100644 --- a/kinematics_interface/CMakeLists.txt +++ b/kinematics_interface/CMakeLists.txt @@ -10,6 +10,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + Eigen3 + rclcpp rclcpp_lifecycle ) diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp new file mode 100644 index 0000000..742bdb6 --- /dev/null +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -0,0 +1,156 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Andy Zelenak, Paul Gesel +/// \description: Base class for kinematics interface + +#ifndef KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_HPP_ +#define KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_HPP_ + +#include +#include +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Geometry" +#include "eigen3/Eigen/LU" +#include "rclcpp/logging.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" + +namespace kinematics_interface +{ +rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface"); + +class KinematicsInterface +{ +public: + KinematicsInterface() = default; + + virtual ~KinematicsInterface() = default; + + /** + * \brief Initialize plugin. This method must be called before any other. + */ + virtual bool initialize( + std::shared_ptr parameters_interface, + const std::string & end_effector_name) = 0; + + /** + * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. + * \param[in] joint_pos joint positions of the robot in radians + * \param[in] delta_x input Cartesian deltas (x, y, z, wx, wy, wz) + * \param[in] link_name the link name at which delta_x is applied + * \param[out] delta_theta outputs joint deltas + * \return true if successful + */ + virtual bool convert_cartesian_deltas_to_joint_deltas( + const Eigen::VectorXd & joint_pos, const Eigen::Matrix & delta_x, + const std::string & link_name, Eigen::VectorXd & delta_theta) = 0; + + /** + * \brief Convert joint delta-theta to Cartesian delta-x. + * \param joint_pos joint positions of the robot in radians + * \param[in] delta_theta joint deltas + * \param[in] link_name the link name at which delta_x is calculated + * \param[out] delta_x Cartesian deltas (x, y, z, wx, wy, wz) + * \return true if successful + */ + 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 & delta_x) = 0; + + /** + * \brief Calculates the joint transform for a specified link using provided joint positions. + * \param[in] joint_pos joint positions of the robot in radians + * \param[in] link_name the name of the link to find the transform for + * \param[out] transform transformation matrix of the specified link + * \return true if successful + */ + virtual bool calculate_link_transform( + const Eigen::VectorXd & joint_pos, const std::string & link_name, + Eigen::Isometry3d & transform) = 0; + + /** + * \brief Calculates the jacobian for a specified link using provided joint positions. + * \param[in] joint_pos joint positions of the robot in radians + * \param[in] link_name the name of the link to find the transform for + * \param[out] jacobian Jacobian matrix of the specified link in column major format. + * \return true if successful + */ + virtual bool calculate_jacobian( + const Eigen::VectorXd & joint_pos, const std::string & link_name, + Eigen::Matrix & jacobian) = 0; + + bool convert_cartesian_deltas_to_joint_deltas( + std::vector & joint_pos_vec, const std::vector & delta_x_vec, + const std::string & link_name, std::vector & delta_theta_vec) + { + auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); + auto delta_x = Eigen::Map(delta_x_vec.data(), delta_x_vec.size()); + // TODO(anyone): heap allocation should be removed for realtime use + Eigen::VectorXd delta_theta = + Eigen::Map(delta_theta_vec.data(), delta_theta_vec.size()); + + bool ret = convert_cartesian_deltas_to_joint_deltas(joint_pos, delta_x, link_name, delta_theta); + for (auto i = 0ul; i < delta_theta_vec.size(); i++) + { + delta_theta_vec[i] = delta_theta[i]; + } + return ret; + } + + bool convert_joint_deltas_to_cartesian_deltas( + const std::vector & joint_pos_vec, const std::vector & delta_theta_vec, + const std::string & link_name, std::vector & delta_x_vec) + { + auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); + Eigen::VectorXd delta_theta = + Eigen::Map(delta_theta_vec.data(), delta_theta_vec.size()); + if (delta_x_vec.size() != 6) + { + RCLCPP_ERROR( + LOGGER, "The length of the cartesian delta vector (%zu) must be 6.", delta_x_vec.size()); + return false; + } + Eigen::Matrix delta_x(delta_x_vec.data()); + bool ret = convert_joint_deltas_to_cartesian_deltas(joint_pos, delta_theta, link_name, delta_x); + for (auto i = 0ul; i < delta_x_vec.size(); i++) + { + delta_x_vec[i] = delta_x[i]; + } + return ret; + } + + bool calculate_link_transform( + const std::vector & joint_pos_vec, const std::string & link_name, + Eigen::Isometry3d & transform) + { + auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); + + return calculate_link_transform(joint_pos, link_name, transform); + } + + bool calculate_jacobian( + const std::vector & joint_pos_vec, const std::string & link_name, + Eigen::Matrix & jacobian) + { + auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); + + return calculate_jacobian(joint_pos, link_name, jacobian); + } +}; + +} // namespace kinematics_interface + +#endif // KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_HPP_ diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface_base.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface_base.hpp deleted file mode 100644 index 4c6eed1..0000000 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface_base.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright (c) 2022, PickNik, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -/// \author: Andy Zelenak, Paul Gesel -/// \description: Base class for kinematics interface - -#ifndef KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_BASE_HPP_ -#define KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_BASE_HPP_ - -#include -#include -#include - -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -namespace kinematics_interface -{ -class KinematicsBaseClass -{ -public: - KinematicsBaseClass() = default; - - virtual ~KinematicsBaseClass() = default; - - /** - * \brief Create an interface object which takes calculate forward and inverse kinematics - */ - virtual bool initialize( - std::shared_ptr node, - const std::string & end_effector_name) = 0; - - /** - * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. - * \param[in] joint_pos joint positions of the robot in radians - * \param[in] delta_x_vec input Cartesian deltas (x, y, z, wx, wy, wz) - * \param[out] delta_theta_vec output vector with joint states - * \return true if successful - */ - virtual bool convert_cartesian_deltas_to_joint_deltas( - const std::vector & joint_pos, const std::vector & delta_x_vec, - const std::string & link_name, std::vector & delta_theta_vec) = 0; - - /** - * \brief Convert joint delta-theta to Cartesian delta-x. - * \param joint_pos joint positions of the robot in radiansgit checkout - * \param[in] delta_theta_vec vector with joint states - * \param[out] delta_x_vec Cartesian deltas (x, y, z, wx, wy, wz) - * \return true if successful - */ - virtual bool convert_joint_deltas_to_cartesian_deltas( - const std::vector & joint_pos, const std::vector & delta_theta_vec, - const std::string & link_name, std::vector & delta_x_vec) = 0; - - /** - * \brief Calculates the joint transform for a specified link using provided joint positions. - * \param[in] joint_pos joint positions of the robot in radians - * \param[in] link_name the name of the link to find the transform for - * \param[out] transform_vec transformation matrix of the specified link in column major format. - * \return true if successful - */ - virtual bool calculate_link_transform( - const std::vector & joint_pos, const std::string & link_name, - std::vector & transform_vec) = 0; - - /** - * \brief Calculates the joint transform for a specified link using provided joint positions. - * \param[in] joint_pos joint positions of the robot in radians - * \param[in] link_name the name of the link to find the transform for - * \param[out] jacobian Jacobian matrix of the specified link in column major format. - * \return true if successful - */ - virtual bool calculate_jacobian( - const std::vector & joint_pos, const std::string & link_name, - std::vector & jacobian) = 0; -}; - -} // namespace kinematics_interface - -#endif // KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_BASE_HPP_ diff --git a/kinematics_interface/package.xml b/kinematics_interface/package.xml index c853f66..2ca89ee 100644 --- a/kinematics_interface/package.xml +++ b/kinematics_interface/package.xml @@ -12,6 +12,7 @@ ament_cmake + eigen rclcpp_lifecycle diff --git a/kinematics_interface_kdl/CMakeLists.txt b/kinematics_interface_kdl/CMakeLists.txt index 72c8183..2681114 100644 --- a/kinematics_interface_kdl/CMakeLists.txt +++ b/kinematics_interface_kdl/CMakeLists.txt @@ -10,6 +10,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS kdl_parser kinematics_interface pluginlib + tf2_eigen_kdl ) # find dependencies diff --git a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp index 0289c4e..9ed343d 100644 --- a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp +++ b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp @@ -18,8 +18,6 @@ #ifndef KINEMATICS_INTERFACE_KDL__KINEMATICS_INTERFACE_KDL_HPP_ #define KINEMATICS_INTERFACE_KDL__KINEMATICS_INTERFACE_KDL_HPP_ -#include "kinematics_interface/kinematics_interface_base.hpp" - #include #include #include @@ -32,76 +30,41 @@ #include "kdl/chainjnttojacsolver.hpp" #include "kdl/treejnttojacsolver.hpp" #include "kdl_parser/kdl_parser.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "kinematics_interface/kinematics_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "tf2_eigen_kdl/tf2_eigen_kdl.hpp" namespace kinematics_interface_kdl { -class KDLKinematics : public kinematics_interface::KinematicsBaseClass +class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface { public: - /** - * \brief KDL implementation of ros2_control kinematics interface - */ bool initialize( - std::shared_ptr node, + std::shared_ptr parameters_interface, const std::string & end_effector_name) override; - /** - * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. - * \param[in] joint_pos joint positions of the robot in radians - * \param[in] delta_x_vec input Cartesian deltas (x, y, z, wx, wy, wz) - * \param[in] link_name the name of the link that the Jacobian is calculated with - * \param[out] delta_theta_vec output vector with joint states - * \return true if successful - */ bool convert_cartesian_deltas_to_joint_deltas( - const std::vector & joint_pos, const std::vector & delta_x_vec, - const std::string & link_name, std::vector & delta_theta_vec) override; + const Eigen::VectorXd & joint_pos, const Eigen::Matrix & delta_x, + const std::string & link_name, Eigen::VectorXd & delta_theta) override; - /** - * \brief Convert joint delta-theta to Cartesian delta-x. - * \param joint_pos joint positions of the robot in radians - * \param[in] delta_theta_vec vector with joint states - * \param[in] link_name the name of the link that the Jacobian is calculated with - * \param[out] delta_x_vec Cartesian deltas (x, y, z, wx, wy, wz) - * \return true if successful - */ bool convert_joint_deltas_to_cartesian_deltas( - const std::vector & joint_pos, const std::vector & delta_theta_vec, - const std::string & link_name, std::vector & delta_x_vec) override; + const Eigen::VectorXd & joint_pos, const Eigen::VectorXd & delta_theta, + const std::string & link_name, Eigen::Matrix & delta_x) override; - /** - * \brief Calculates the joint transform for a specified link using provided joint positions. - * \param[in] joint_pos joint positions of the robot in radians - * \param[in] link_name the name of the link to find the transform for - * \param[out] transform_vec transformation matrix of the specified link in column major format. - * \return true if successful - */ bool calculate_link_transform( - const std::vector & joint_pos, const std::string & link_name, - std::vector & transform_vec) override; + const Eigen::VectorXd & joint_pos, const std::string & link_name, + Eigen::Isometry3d & transform) override; - /** - * \brief Calculates the joint transform for a specified link using provided joint positions. - * \param[in] joint_pos joint positions of the robot in radians - * \param[in] link_name the name of the link to find the transform for - * \param[out] jacobian Jacobian matrix of the specified link in column major format. - * \return true if successful - */ bool calculate_jacobian( - const std::vector & joint_pos, const std::string & link_name, - std::vector & jacobian) override; + const Eigen::VectorXd & joint_pos, const std::string & link_name, + Eigen::Matrix & jacobian) override; private: - bool update_joint_array(const std::vector & joint_pos); - // verification methods bool verify_initialized(); bool verify_link_name(const std::string & link_name); - bool verify_transform_vector(const std::vector & transform); - bool verify_cartesian_vector(const std::vector & cartesian_vector); - bool verify_joint_vector(const std::vector & joint_vector); - bool verify_jacobian(const std::vector & jacobian_vector); + bool verify_joint_vector(const Eigen::VectorXd & joint_vector); + bool verify_jacobian(const Eigen::Matrix & jacobian); bool initialized = false; std::string root_name_; @@ -112,11 +75,9 @@ class KDLKinematics : public kinematics_interface::KinematicsBaseClass KDL::Frame frame_; std::shared_ptr jacobian_; std::shared_ptr jac_solver_; - std::shared_ptr node_; + std::shared_ptr parameters_interface_; std::unordered_map link_name_map_; double alpha; // damping term for Jacobian inverse - Eigen::Matrix delta_x; - Eigen::VectorXd delta_theta; Eigen::MatrixXd I; }; diff --git a/kinematics_interface_kdl/kinematics_interface_kdl.xml b/kinematics_interface_kdl/kinematics_interface_kdl.xml index cb1c546..9ceb33a 100644 --- a/kinematics_interface_kdl/kinematics_interface_kdl.xml +++ b/kinematics_interface_kdl/kinematics_interface_kdl.xml @@ -1,7 +1,7 @@ - + KDL plugin for ros2_control kinematics interface diff --git a/kinematics_interface_kdl/package.xml b/kinematics_interface_kdl/package.xml index ef9878e..ed2b545 100644 --- a/kinematics_interface_kdl/package.xml +++ b/kinematics_interface_kdl/package.xml @@ -13,11 +13,11 @@ ament_cmake eigen3_cmake_module - eigen - + eigen kdl_parser kinematics_interface pluginlib + tf2_eigen_kdl ament_cmake_gmock ros2_control_test_assets diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index c2c95da..464cdc7 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -20,15 +20,16 @@ namespace kinematics_interface_kdl { rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface_kdl"); -bool KDLKinematics::initialize( - std::shared_ptr node, const std::string & end_effector_name) +bool KinematicsInterfaceKDL::initialize( + std::shared_ptr parameters_interface, + const std::string & end_effector_name) { // track initialization plugin initialized = true; // get robot description auto robot_param = rclcpp::Parameter(); - if (!node->get_parameter("robot_description", robot_param)) + if (!parameters_interface->get_parameter("robot_description", robot_param)) { RCLCPP_ERROR(LOGGER, "parameter robot_description not set"); return false; @@ -36,9 +37,9 @@ bool KDLKinematics::initialize( auto robot_description = robot_param.as_string(); // get alpha damping term auto alpha_param = rclcpp::Parameter("alpha", 0.000005); - if (node->has_parameter("alpha")) + if (parameters_interface->has_parameter("alpha")) { - node->get_parameter("alpha", alpha_param); + parameters_interface->get_parameter("alpha", alpha_param); } alpha = alpha_param.as_double(); // create kinematic chain @@ -60,7 +61,6 @@ bool KDLKinematics::initialize( // allocate dynamics memory num_joints_ = chain_.getNrOfJoints(); q_ = KDL::JntArray(num_joints_); - delta_theta = Eigen::VectorXd(num_joints_); I = Eigen::MatrixXd(num_joints_, num_joints_); I.setIdentity(); // create KDL solvers @@ -71,48 +71,45 @@ bool KDLKinematics::initialize( return true; } -bool KDLKinematics::convert_joint_deltas_to_cartesian_deltas( - const std::vector & joint_pos, const std::vector & delta_theta_vec, - const std::string & link_name, std::vector & delta_x_vec) +bool KinematicsInterfaceKDL::convert_joint_deltas_to_cartesian_deltas( + const Eigen::Matrix & joint_pos, + const Eigen::Matrix & delta_theta, const std::string & link_name, + Eigen::Matrix & delta_x) { // verify inputs if ( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || - !verify_cartesian_vector(delta_x_vec) || !verify_joint_vector(delta_theta_vec)) + !verify_joint_vector(delta_theta)) { return false; } // get joint array - update_joint_array(joint_pos); + q_.data = joint_pos; - // copy vector to eigen type - memcpy(delta_theta.data(), delta_theta_vec.data(), delta_theta_vec.size() * sizeof(double)); // calculate Jacobian jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); delta_x = jacobian_->data * delta_theta; - // copy eigen type to vector - memcpy(delta_x_vec.data(), delta_x.data(), 6 * sizeof(double)); + return true; } -bool KDLKinematics::convert_cartesian_deltas_to_joint_deltas( - const std::vector & joint_pos, const std::vector & delta_x_vec, - const std::string & link_name, std::vector & delta_theta_vec) +bool KinematicsInterfaceKDL::convert_cartesian_deltas_to_joint_deltas( + const Eigen::Matrix & joint_pos, + const Eigen::Matrix & delta_x, const std::string & link_name, + Eigen::Matrix & delta_theta) { // verify inputs if ( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || - !verify_cartesian_vector(delta_x_vec) || !verify_joint_vector(delta_theta_vec)) + !verify_joint_vector(delta_theta)) { return false; } // get joint array - update_joint_array(joint_pos); + q_.data = joint_pos; - // copy vector to eigen type - memcpy(delta_x.data(), delta_x_vec.data(), delta_x_vec.size() * sizeof(double)); // calculate Jacobian jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); // TODO(anyone): this dynamic allocation needs to be replaced @@ -121,86 +118,61 @@ bool KDLKinematics::convert_cartesian_deltas_to_joint_deltas( Eigen::Matrix J_inverse = (J.transpose() * J + alpha * I).inverse() * J.transpose(); delta_theta = J_inverse * delta_x; - // copy eigen type to vector - memcpy(delta_theta_vec.data(), delta_theta.data(), num_joints_ * sizeof(double)); + return true; } -bool KDLKinematics::calculate_jacobian( - const std::vector & joint_pos, const std::string & link_name, - std::vector & jacobian_vector) +bool KinematicsInterfaceKDL::calculate_jacobian( + const Eigen::Matrix & joint_pos, const std::string & link_name, + Eigen::Matrix & jacobian) { // verify inputs if ( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || - !verify_jacobian(jacobian_vector)) + !verify_jacobian(jacobian)) { return false; } // get joint array - update_joint_array(joint_pos); + q_.data = joint_pos; // calculate Jacobian jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); - - memcpy(jacobian_vector.data(), jacobian_->data.data(), 6 * num_joints_ * sizeof(double)); + jacobian = jacobian_->data; return true; } -bool KDLKinematics::calculate_link_transform( - const std::vector & joint_pos, const std::string & link_name, - std::vector & transform_vec) +bool KinematicsInterfaceKDL::calculate_link_transform( + const Eigen::Matrix & joint_pos, const std::string & link_name, + Eigen::Isometry3d & transform) { // verify inputs - if ( - !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || - !verify_transform_vector(transform_vec)) + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name)) { return false; } // get joint array - update_joint_array(joint_pos); + q_.data = joint_pos; // reset transform_vec - memset(transform_vec.data(), 0, 16 * sizeof(double)); + transform.setIdentity(); // special case: since the root is not in the robot tree, need to return identity transform if (link_name == root_name_) { - transform_vec[0] = 1.0; - transform_vec[5] = 1.0; - transform_vec[10] = 1.0; - transform_vec[15] = 1.0; return true; } // create forward kinematics solver fk_pos_solver_->JntToCart(q_, frame_, link_name_map_[link_name]); - double tmp[] = {frame_.p.x(), frame_.p.y(), frame_.p.z()}; - // KDL::Rotation stores data in row-major format. - // e.g Xx, Yx, Zx, Xy... = data index at 0, 1, 2, 3, 4... - for (int r = 0; r < 3; r++) - { - for (int c = 0; c < 3; c++) - { - transform_vec[r + 4 * c] = frame_.M.data[3 * r + c]; - } - transform_vec[4 * 3 + r] = tmp[r]; - } - transform_vec[15] = 1.0; + tf2::transformKDLToEigen(frame_, transform); return true; } -bool KDLKinematics::update_joint_array(const std::vector & joint_pos) -{ - memcpy(q_.data.data(), joint_pos.data(), joint_pos.size() * sizeof(double)); - return true; -} - -bool KDLKinematics::verify_link_name(const std::string & link_name) +bool KinematicsInterfaceKDL::verify_link_name(const std::string & link_name) { if (link_name == root_name_) { @@ -209,7 +181,7 @@ bool KDLKinematics::verify_link_name(const std::string & link_name) if (link_name_map_.find(link_name) == link_name_map_.end()) { std::string links; - for (auto i = 0u; i < chain_.getNrOfSegments(); i++) + for (size_t i = 0; i < chain_.getNrOfSegments(); ++i) { links += "\n" + chain_.getSegment(i).getName(); } @@ -221,43 +193,19 @@ bool KDLKinematics::verify_link_name(const std::string & link_name) return true; } -bool KDLKinematics::verify_transform_vector(const std::vector & transform) -{ - if (transform.size() != 16) - { - RCLCPP_ERROR( - LOGGER, "Invalid size (%zu) for the transformation vector. Expected size is 16.", - transform.size()); - return false; - } - return true; -} - -bool KDLKinematics::verify_cartesian_vector(const std::vector & cartesian_vector) -{ - if (cartesian_vector.size() != jacobian_->rows()) - { - RCLCPP_ERROR( - LOGGER, "Invalid size (%zu) for the cartesian vector. Expected size is %d.", - cartesian_vector.size(), jacobian_->rows()); - return false; - } - return true; -} - -bool KDLKinematics::verify_joint_vector(const std::vector & joint_vector) +bool KinematicsInterfaceKDL::verify_joint_vector(const Eigen::VectorXd & joint_vector) { if (joint_vector.size() != num_joints_) { RCLCPP_ERROR( - LOGGER, "Invalid size (%zu) for the joint vector. Expected size is %d.", joint_vector.size(), + LOGGER, "Invalid joint vector size (%zu). Expected size is %zu.", joint_vector.size(), num_joints_); return false; } return true; } -bool KDLKinematics::verify_initialized() +bool KinematicsInterfaceKDL::verify_initialized() { // check if interface is initialized if (!initialized) @@ -270,13 +218,14 @@ bool KDLKinematics::verify_initialized() return true; } -bool KDLKinematics::verify_jacobian(const std::vector & jacobian_vector) +bool KinematicsInterfaceKDL::verify_jacobian( + const Eigen::Matrix & jacobian) { - if (jacobian_vector.size() != jacobian_->rows() * jacobian_->columns()) + if (jacobian.rows() != jacobian_->rows() || jacobian.cols() != jacobian_->columns()) { RCLCPP_ERROR( - LOGGER, "The size of the jacobian argument (%zu) does not match the required size of (%zu)", - jacobian_vector.size(), jacobian_->rows() * jacobian_->columns()); + LOGGER, "The size of the jacobian (%zu, %zu) does not match the required size of (%u, %u)", + jacobian.rows(), jacobian.cols(), jacobian_->rows(), jacobian_->columns()); return false; } return true; @@ -287,4 +236,4 @@ bool KDLKinematics::verify_jacobian(const std::vector & jacobian_vector) #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - kinematics_interface_kdl::KDLKinematics, kinematics_interface::KinematicsBaseClass) + kinematics_interface_kdl::KinematicsInterfaceKDL, kinematics_interface::KinematicsInterface) diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index b58de8f..8b4dea5 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -16,15 +16,16 @@ #include #include -#include "kinematics_interface/kinematics_interface_base.hpp" +#include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "ros2_control_test_assets/descriptions.hpp" class TestKDLPlugin : public ::testing::Test { public: - std::shared_ptr> ik_loader_; - std::shared_ptr ik_; + std::shared_ptr> ik_loader_; + std::shared_ptr ik_; std::shared_ptr node_; std::string end_effector_ = "link2"; @@ -33,11 +34,11 @@ class TestKDLPlugin : public ::testing::Test // init ros rclcpp::init(0, nullptr); node_ = std::make_shared("test_node"); - std::string plugin_name = "kinematics_interface_kdl/KDLKinematics"; + std::string plugin_name = "kinematics_interface_kdl/KinematicsInterfaceKDL"; ik_loader_ = - std::make_shared>( - "kinematics_interface", "kinematics_interface::KinematicsBaseClass"); - ik_ = std::unique_ptr( + std::make_shared>( + "kinematics_interface", "kinematics_interface::KinematicsInterface"); + ik_ = std::unique_ptr( ik_loader_->createUnmanagedInstance(plugin_name)); } @@ -71,16 +72,50 @@ TEST_F(TestKDLPlugin, KDL_plugin_function) loadAlphaParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_, end_effector_)); + ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + + // calculate end effector transform + Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + ASSERT_TRUE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); + + // convert joint delta to cartesian delta + Eigen::Matrix delta_x_est; + ASSERT_TRUE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); + + // Ensure kinematics math is correct + for (auto i = 0l; i < delta_x.size(); i++) + { + ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); + } +} + +TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector) +{ + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); // calculate end effector transform std::vector pos = {0, 0}; - std::vector end_effector_transform(16); + Eigen::Isometry3d end_effector_transform; ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); // convert cartesian delta to joint delta - std::vector delta_x = {0, 0, .5, 0, 0, 0}; - std::vector delta_theta(2); + std::vector delta_x = {0, 0, 0, 0, 0, 0}; + delta_x[2] = 1; + std::vector delta_theta = {0, 0}; ASSERT_TRUE( ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); @@ -90,9 +125,9 @@ TEST_F(TestKDLPlugin, KDL_plugin_function) ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); // Ensure kinematics math is correct - for (auto i = 0ul; i < delta_x.size(); i++) + for (auto i = 0l; i < delta_x.size(); i++) { - ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.01); + ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } } @@ -103,28 +138,26 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes) loadAlphaParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_, end_effector_)); + ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); // define correct values - std::vector pos = {0, 0}; - std::vector end_effector_transform(16); - std::vector delta_x = {0, 0, 1, 0, 0, 0}; - std::vector delta_theta(2); - std::vector delta_x_est(6); + Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Isometry3d end_effector_transform; + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + Eigen::Matrix delta_x_est; // wrong size input vector - std::vector vec_5 = {1.0, 2.0, 3.0, 4.0, 5.0}; + Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); // calculate transform ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform)); ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform)); - ASSERT_FALSE(ik_->calculate_link_transform(pos, end_effector_, vec_5)); // convert cartesian delta to joint delta ASSERT_FALSE( ik_->convert_cartesian_deltas_to_joint_deltas(vec_5, delta_x, end_effector_, delta_theta)); - ASSERT_FALSE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, vec_5, end_effector_, delta_theta)); ASSERT_FALSE( ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, "link_not_in_model", delta_theta)); ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, vec_5)); @@ -136,13 +169,11 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes) ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est)); ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas( pos, delta_theta, "link_not_in_model", delta_x_est)); - ASSERT_FALSE( - ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, vec_5)); } TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description) { // load alpha to parameter server loadAlphaParameter(); - ASSERT_FALSE(ik_->initialize(node_, end_effector_)); + ASSERT_FALSE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); }