diff --git a/README.md b/README.md index bae44df..437fda3 100644 --- a/README.md +++ b/README.md @@ -1 +1,2 @@ # kinematics_interface +This is a ROS 2 package for using C++ kinematics frameworks in the context of ROS 2 control. A kinematics interface is designed to allow ROS 2 controllers to control robots in Cartesian space. This package also contains a basic implementation of the interface using KDL. \ No newline at end of file diff --git a/kinematics_interface/CMakeLists.txt b/kinematics_interface/CMakeLists.txt new file mode 100644 index 0000000..baffede --- /dev/null +++ b/kinematics_interface/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.5) +project(kinematics_interface) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +# Create interface library for kinematics base class +add_library(kinematics_interface + INTERFACE + include/kinematics_interface/kinematics_interface_base.hpp + ) + +target_include_directories( + kinematics_interface + INTERFACE + include +) + +ament_target_dependencies( + kinematics_interface + INTERFACE + rclcpp_lifecycle +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(kinematics_interface INTERFACE "KINEMATICS_INTERFACE_BUILDING_DLL") + +install(DIRECTORY include/ + DESTINATION include + ) + +ament_export_dependencies( + rclcpp_lifecycle +) + +ament_export_include_directories( + include +) + +ament_package() \ No newline at end of file diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface_base.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface_base.hpp new file mode 100644 index 0000000..813f968 --- /dev/null +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface_base.hpp @@ -0,0 +1,88 @@ +// 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 IK_PLUGIN_BASE__IK_PLUGIN_BASE_HPP_ +#define IK_PLUGIN_BASE__IK_PLUGIN_BASE_HPP_ + +#include + +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 radians + * \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 // IK_PLUGIN_BASE__IK_PLUGIN_BASE_HPP_ diff --git a/kinematics_interface/include/kinematics_interface/visibility_control.h b/kinematics_interface/include/kinematics_interface/visibility_control.h new file mode 100644 index 0000000..a29e085 --- /dev/null +++ b/kinematics_interface/include/kinematics_interface/visibility_control.h @@ -0,0 +1,49 @@ +// 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. + +#ifndef IK_PLUGIN_BASE__VISIBILITY_CONTROL_H_ +#define IK_PLUGIN_BASE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define IK_PLUGIN_BASE_EXPORT __attribute__((dllexport)) +#define IK_PLUGIN_BASE_IMPORT __attribute__((dllimport)) +#else +#define IK_PLUGIN_BASE_EXPORT __declspec(dllexport) +#define IK_PLUGIN_BASE_IMPORT __declspec(dllimport) +#endif +#ifdef IK_PLUGIN_BASE_BUILDING_DLL +#define IK_PLUGIN_BASE_PUBLIC IK_PLUGIN_BASE_EXPORT +#else +#define IK_PLUGIN_BASE_PUBLIC IK_PLUGIN_BASE_IMPORT +#endif +#define IK_PLUGIN_BASE_PUBLIC_TYPE IK_PLUGIN_BASE_PUBLIC +#define IK_PLUGIN_BASE_LOCAL +#else +#define IK_PLUGIN_BASE_EXPORT __attribute__((visibility("default"))) +#define IK_PLUGIN_BASE_IMPORT +#if __GNUC__ >= 4 +#define IK_PLUGIN_BASE_PUBLIC __attribute__((visibility("default"))) +#define IK_PLUGIN_BASE_LOCAL __attribute__((visibility("hidden"))) +#else +#define IK_PLUGIN_BASE_PUBLIC +#define IK_PLUGIN_BASE_LOCAL +#endif +#define IK_PLUGIN_BASE_PUBLIC_TYPE +#endif + +#endif // IK_PLUGIN_BASE__VISIBILITY_CONTROL_H_ diff --git a/kinematics_interface/package.xml b/kinematics_interface/package.xml new file mode 100644 index 0000000..43aa603 --- /dev/null +++ b/kinematics_interface/package.xml @@ -0,0 +1,22 @@ + + + + kinematics_interface + 0.0.0 + Kinematics interface for ROS 2 control + Denis Štogl + Bence Magyar + Andy Zelenak + Paul Gesel + Apache License 2.0 + + ament_cmake + + rclcpp_lifecycle + + ament_cmake_gmock + + + ament_cmake + + diff --git a/kinematics_interface_kdl/CMakeLists.txt b/kinematics_interface_kdl/CMakeLists.txt new file mode 100644 index 0000000..4e70949 --- /dev/null +++ b/kinematics_interface_kdl/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.8) +project(kinematics_interface_kdl) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(pluginlib REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(kinematics_interface REQUIRED) + +add_library(kinematics_interface_kdl SHARED + src/kinematics_interface_kdl.cpp + ) + +ament_target_dependencies(kinematics_interface_kdl + kdl_parser + pluginlib + Eigen3 + kinematics_interface + ) + +target_include_directories( + kinematics_interface_kdl + PRIVATE + include +) + +pluginlib_export_plugin_description_file(kinematics_interface_kdl kinematics_interface_kdl.xml) + +install( + TARGETS kinematics_interface_kdl + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock( + test_kinematics_interface_kdl + test/test_kinematics_interface_kdl.cpp + ) + target_include_directories(test_kinematics_interface_kdl PRIVATE include) + ament_target_dependencies(test_kinematics_interface_kdl + kinematics_interface + ros2_control_test_assets + pluginlib + ros2_control_test_assets + ) + +endif() + +ament_package() 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 new file mode 100644 index 0000000..85721cd --- /dev/null +++ b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp @@ -0,0 +1,116 @@ +// 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: KDL plugin for kinematics interface + +#pragma once + +#include "kinematics_interface/kinematics_interface_base.hpp" +#include +#include "kdl_parser/kdl_parser.hpp" +#include "kdl/chainfksolverpos_recursive.hpp" +#include "kdl/chainfksolvervel_recursive.hpp" +#include "kdl/chainjnttojacsolver.hpp" +#include "kdl/treejnttojacsolver.hpp" +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" + + +namespace kinematics_interface_kdl +{ + + class KDLKinematics : public kinematics_interface::KinematicsBaseClass + { + public: + /** + * \brief KDL implementation of ros2_control kinematics interface + */ + virtual bool + initialize(std::shared_ptr node, const std::string &end_effector_name); + + /** + * \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 + */ + 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); + + /** + * \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 + */ + 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); + + /** + * \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); + + /** + * \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); + + + private: + bool update_joint_array(const std::vector& joint_pos); + bool verify_link_name(const std::string& link_name); + + bool initialized = false; + std::string end_effector_name_; + std::string root_name_; + size_t num_joints_; + KDL::Chain chain_; + std::shared_ptr fk_pos_solver_; + KDL::JntArray q_; + KDL::Frame frame_; + std::shared_ptr jacobian_; + std::shared_ptr jac_solver_; + std::shared_ptr node_; + std::unordered_map link_name_map_; + double alpha; // damping term for Jacobian inverse + Eigen::Matrix delta_x; + Eigen::VectorXd delta_theta; + Eigen::MatrixXd I; + }; + +} // namespace kinematics_interface_kdl diff --git a/kinematics_interface_kdl/kinematics_interface_kdl.xml b/kinematics_interface_kdl/kinematics_interface_kdl.xml new file mode 100644 index 0000000..cb1c546 --- /dev/null +++ b/kinematics_interface_kdl/kinematics_interface_kdl.xml @@ -0,0 +1,9 @@ + + + + KDL plugin for ros2_control kinematics interface + + + diff --git a/kinematics_interface_kdl/package.xml b/kinematics_interface_kdl/package.xml new file mode 100644 index 0000000..960d8d2 --- /dev/null +++ b/kinematics_interface_kdl/package.xml @@ -0,0 +1,20 @@ + + + + kinematics_interface_kdl + 0.0.0 + KDL implementation of ros2_control kinematics interface + paul + Apache License 2.0 + + ament_cmake + + kinematics_interface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp new file mode 100644 index 0000000..efc58ed --- /dev/null +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -0,0 +1,210 @@ +// 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 + +#include "kinematics_interface_kdl/kinematics_interface_kdl.hpp" + +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) { + initialized = true; + end_effector_name_ = end_effector_name; + // get robot description + auto robot_param = rclcpp::Parameter(); + if (!node->get_parameter("robot_description", robot_param)) { + RCLCPP_ERROR(LOGGER, "parameter robot_description not set"); + return false; + } + auto robot_description = robot_param.as_string(); + // get alpha damping term + auto alpha_param = rclcpp::Parameter("alpha", 0.000005); + if (node->has_parameter("alpha")) { + node->get_parameter("alpha", alpha_param); + } + alpha = alpha_param.as_double(); + // create kinematic chain + KDL::Tree robot_tree; + kdl_parser::treeFromString(robot_description, robot_tree); + root_name_ = robot_tree.getRootSegment()->first; + if (!robot_tree.getChain(root_name_, end_effector_name, chain_)) { + RCLCPP_ERROR(LOGGER, "failed to find chain from robot root %s to end effector %s", + root_name_.c_str(), end_effector_name.c_str()); + return false; + } + //create map from link names to their index + for (auto i = 0u; i < chain_.getNrOfSegments(); i++) { + link_name_map_[chain_.getSegment(i).getName()] = i + 1; + } + // 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 + fk_pos_solver_ = std::make_shared(chain_); + jac_solver_ = std::make_shared(chain_); + jacobian_ = std::make_shared(num_joints_); + + 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) { + // get joint array and check dimensions + if (!update_joint_array(joint_pos)) { + return false; + } + if (!verify_link_name(link_name)){ + return false; + } + // 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) { + // get joint array and check dimensions + if (!update_joint_array(joint_pos)) { + return false; + } + if (!verify_link_name(link_name)){ + return false; + } + // 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 this dynamic allocation needs to be replaced + Eigen::Matrix J = jacobian_->data; + // damped inverse + 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) { + // get joint array and check dimensions + if (!update_joint_array(joint_pos)) { + return false; + } + if (!verify_link_name(link_name)){ + return false; + } + if (jacobian.size() != 6*num_joints_) { + RCLCPP_ERROR(LOGGER, "The size of the jacobian argument (%zu) does not match the required size of (%zu)", + jacobian.size(), 6*num_joints_); + return false; + } + + // calculate Jacobian + jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); + + memcpy(jacobian.data(), jacobian_->data.data(), 6*num_joints_*sizeof(double)); + + return true; + } + + bool KDLKinematics::calculate_link_transform(const std::vector &joint_pos, + const std::string &link_name, + std::vector &transform_vec + ) { + // get joint array and check dimensions + if (!update_joint_array(joint_pos)) { + return false; + } + if (!verify_link_name(link_name)){ + return false; + } + + // reset transform_vec + memset(transform_vec.data(), 0, 16*sizeof(double)); + + // 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; + } + 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; + return true; + } + + bool KDLKinematics::update_joint_array(const std::vector &joint_pos) { + // check if interface is initialized + if (!initialized) { + RCLCPP_ERROR(LOGGER, "The KDL kinematics plugin was not initialized. Ensure you called the initialize method."); + return false; + } + // check that joint_pos_ is the correct size + if (joint_pos.size() != num_joints_) { + RCLCPP_ERROR(LOGGER, "The size of joint_pos_ (%zu) does not match that of the robot model (%zu)", + joint_pos.size(), num_joints_); + return false; + } + memcpy(q_.data.data(), joint_pos.data(), joint_pos.size()*sizeof(double)); + return true; + } + + bool KDLKinematics::verify_link_name(const std::string &link_name) { + if (link_name == root_name_) { + return true; + } + if (link_name_map_.find(link_name) == link_name_map_.end()) { + std::string links; + for (auto i = 0u; i < chain_.getNrOfSegments(); i++) { + links += "\n" + chain_.getSegment(i).getName(); + } + RCLCPP_ERROR(LOGGER, "The link %s was not found in the robot chain. Available links are: %s", + link_name.c_str(), links.c_str()); + return false; + } + return true; + } +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(kinematics_interface_kdl::KDLKinematics, + kinematics_interface::KinematicsBaseClass) \ No newline at end of file diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp new file mode 100644 index 0000000..734d6fb --- /dev/null +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -0,0 +1,88 @@ +// 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: Paul Gesel + +#include +#include +#include "kinematics_interface/kinematics_interface_base.hpp" +#include "pluginlib/class_loader.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 node; + std::string end_effector = "link2"; + + void SetUp() { + // init ros + rclcpp::init(0, nullptr); + node = std::make_shared("test_node"); + + std::string plugin_name = "kinematics_interface_kdl/KDLKinematics"; + ik_loader_ = std::make_shared>( + "kinematics_interface_kdl", "kinematics_interface::KinematicsBaseClass"); + ik_ = std::unique_ptr( + ik_loader_->createUnmanagedInstance(plugin_name)); + } + + void TearDown() { + // shutdown ros + rclcpp::shutdown(); + } + + void loadURDFParameter() { + auto urdf = std::string(ros2_control_test_assets::urdf_head) + std::string(ros2_control_test_assets::urdf_tail); + rclcpp::Parameter param("robot_description", urdf); + node->declare_parameter("robot_description", ""); + node->set_parameter(param); + } + + void loadAlphaParameter() { + rclcpp::Parameter param("alpha", 0.005); + node->declare_parameter("alpha", 0.005); + node->set_parameter(param); + } +}; + +TEST_F(TestKDLPlugin, KDL_plugin_function) { + std::vector pos = {0, 0}; + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + ASSERT_TRUE(ik_->initialize(node, end_effector)); + + // calculate end effector transform + std::vector end_effector_transform(3 + 9); + ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector, end_effector_transform)); + + // convert cartesian delta to joint delta + std::vector delta_x = {0, 0, 1, 0, 0, 0}; + std::vector delta_theta(6); + ASSERT_TRUE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector, delta_theta)); + + // convert joint delta to cartesian delta + std::vector delta_x_est(6); + ASSERT_TRUE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector, delta_x_est)); +} + +TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description) { + + loadAlphaParameter(); + ASSERT_FALSE(ik_->initialize(node, end_effector)); +} \ No newline at end of file