Convert interface to use Eigen

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
 )
 

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 <memory>
+#include <string>
+#include <vector>
+
+#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<rclcpp::node_interfaces::NodeParametersInterface> 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<double, 6, 1> & 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<double, 6, 1> & 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<double, 6, Eigen::Dynamic> & jacobian) = 0;
+
+  bool convert_cartesian_deltas_to_joint_deltas(
+    std::vector<double> & joint_pos_vec, const std::vector<double> & delta_x_vec,
+    const std::string & link_name, std::vector<double> & delta_theta_vec)
+  {
+    auto joint_pos = Eigen::Map<const Eigen::VectorXd>(joint_pos_vec.data(), joint_pos_vec.size());
+    auto delta_x = Eigen::Map<const Eigen::VectorXd>(delta_x_vec.data(), delta_x_vec.size());
+    // TODO(anyone): heap allocation should be removed for realtime use
+    Eigen::VectorXd delta_theta =
+      Eigen::Map<Eigen::VectorXd>(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<double> & joint_pos_vec, const std::vector<double> & delta_theta_vec,
+    const std::string & link_name, std::vector<double> & delta_x_vec)
+  {
+    auto joint_pos = Eigen::Map<const Eigen::VectorXd>(joint_pos_vec.data(), joint_pos_vec.size());
+    Eigen::VectorXd delta_theta =
+      Eigen::Map<const Eigen::VectorXd>(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<double, 6, 1> 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<double> & joint_pos_vec, const std::string & link_name,
+    Eigen::Isometry3d & transform)
+  {
+    auto joint_pos = Eigen::Map<const Eigen::VectorXd>(joint_pos_vec.data(), joint_pos_vec.size());
+
+    return calculate_link_transform(joint_pos, link_name, transform);
+  }
+
+  bool calculate_jacobian(
+    const std::vector<double> & joint_pos_vec, const std::string & link_name,
+    Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian)
+  {
+    auto joint_pos = Eigen::Map<const Eigen::VectorXd>(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_

kinematics_interface/package.xml

@@ -12,6 +12,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>eigen</depend>
   <depend>rclcpp_lifecycle</depend>
 
   <export>

kinematics_interface_kdl/CMakeLists.txt

@@ -10,6 +10,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   kdl_parser
   kinematics_interface
   pluginlib
+  tf2_eigen_kdl
 )
 
 # find dependencies

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 <memory>
 #include <string>
 #include <unordered_map>
@@ -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<rclcpp_lifecycle::LifecycleNode> node,
+    std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> 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<double> & joint_pos, const std::vector<double> & delta_x_vec,
-    const std::string & link_name, std::vector<double> & delta_theta_vec) override;
+    const Eigen::VectorXd & joint_pos, const Eigen::Matrix<double, 6, 1> & 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<double> & joint_pos, const std::vector<double> & delta_theta_vec,
-    const std::string & link_name, std::vector<double> & delta_x_vec) override;
+    const Eigen::VectorXd & joint_pos, const Eigen::VectorXd & delta_theta,
+    const std::string & link_name, Eigen::Matrix<double, 6, 1> & 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<double> & joint_pos, const std::string & link_name,
-    std::vector<double> & 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<double> & joint_pos, const std::string & link_name,
-    std::vector<double> & jacobian) override;
+    const Eigen::VectorXd & joint_pos, const std::string & link_name,
+    Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian) override;
 
 private:
-  bool update_joint_array(const std::vector<double> & joint_pos);
-
   // verification methods
   bool verify_initialized();
   bool verify_link_name(const std::string & link_name);
-  bool verify_transform_vector(const std::vector<double> & transform);
-  bool verify_cartesian_vector(const std::vector<double> & cartesian_vector);
-  bool verify_joint_vector(const std::vector<double> & joint_vector);
-  bool verify_jacobian(const std::vector<double> & jacobian_vector);
+  bool verify_joint_vector(const Eigen::VectorXd & joint_vector);
+  bool verify_jacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
 
   bool initialized = false;
   std::string root_name_;
@@ -112,11 +75,9 @@ class KDLKinematics : public kinematics_interface::KinematicsBaseClass
   KDL::Frame frame_;
   std::shared_ptr<KDL::Jacobian> jacobian_;
   std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
-  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
+  std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface_;
   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;
 };
 

kinematics_interface_kdl/kinematics_interface_kdl.xml

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