Kinematics interface with KDL implementation

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.   

kdl_plugin/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()

kdl_plugin/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 <rclcpp_lifecycle/lifecycle_node.hpp>
+#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<rclcpp_lifecycle::LifecycleNode> 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<double> &joint_pos,
+                                               const std::vector<double> &delta_x_vec,
+                                               const std::string &link_name,
+                                               std::vector<double> &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<double> &joint_pos,
+                                               const std::vector<double> &delta_theta_vec,
+                                               const std::string &link_name,
+                                               std::vector<double> &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<double> &joint_pos, const std::string &link_name,
+                               std::vector<double> &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<double> &joint_pos, const std::string &link_name,
+                         std::vector<double> &jacobian);
+
+
+    private:
+        bool update_joint_array(const std::vector<double>& 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<KDL::ChainFkSolverPos_recursive> fk_pos_solver_;
+        KDL::JntArray q_;
+        KDL::Frame frame_;
+        std::shared_ptr<KDL::Jacobian> jacobian_;
+        std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
+        std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
+        std::unordered_map<std::string, int> link_name_map_;
+        double alpha; // damping term for Jacobian inverse
+        Eigen::Matrix<double,6,1> delta_x;
+        Eigen::VectorXd delta_theta;
+        Eigen::MatrixXd I;
+    };
+
+}  // namespace kinematics_interface_kdl

kdl_plugin/kinematics_interface_kdl.xml

@@ -0,0 +1,9 @@
+<library path="kinematics_interface_kdl">
+  <class name="kinematics_interface_kdl/KDLKinematics"
+         type="kinematics_interface_kdl::KDLKinematics"
+         base_class_type="kinematics_interface::KinematicsBaseClass">
+    <description>
+      KDL plugin for ros2_control kinematics interface
+    </description>
+  </class>
+</library>

kdl_plugin/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>kinematics_interface_kdl</name>
+  <version>0.0.0</version>
+  <description>KDL implementation of ros2_control kinematics interface</description>
+  <maintainer email="[email protected]">paul</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>kinematics_interface</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>