Add KDL implementation for computing frame differences

kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp

@@ -60,20 +60,26 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface
     const Eigen::VectorXd & joint_pos, const std::string & link_name,
     Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian) override;
 
+  bool calculate_frame_difference(
+    Eigen::Matrix<double, 7, 1> & x_a, Eigen::Matrix<double, 7, 1> & x_b, double dt,
+    Eigen::Matrix<double, 6, 1> & delta_x) override;
+
 private:
   // verification methods
   bool verify_initialized();
   bool verify_link_name(const std::string & link_name);
   bool verify_joint_vector(const Eigen::VectorXd & joint_vector);
   bool verify_jacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
+  bool verify_period(const double dt);
 
   bool initialized = false;
   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_;
+  Eigen::Matrix<KDL::Frame, 2, 1> frames_;
+  KDL::Twist delta_x_;
   std::shared_ptr<KDL::Jacobian> jacobian_;
   std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
   std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface_;

kinematics_interface_kdl/src/kinematics_interface_kdl.cpp

@@ -205,8 +205,34 @@ bool KinematicsInterfaceKDL::calculate_link_transform(
   }
 
   // create forward kinematics solver
-  fk_pos_solver_->JntToCart(q_, frame_, link_name_map_[link_name]);
-  tf2::transformKDLToEigen(frame_, transform);
+  fk_pos_solver_->JntToCart(q_, frames_(0), link_name_map_[link_name]);
+  tf2::transformKDLToEigen(frames_(0), transform);
+  return true;
+}
+
+bool KinematicsInterfaceKDL::calculate_frame_difference(
+  Eigen::Matrix<double, 7, 1> & x_a, Eigen::Matrix<double, 7, 1> & x_b, double dt,
+  Eigen::Matrix<double, 6, 1> & delta_x)
+{
+  // verify inputs
+  if (!verify_initialized() || !verify_period(dt))
+  {
+    return false;
+  }
+
+  // get frames
+  frames_(0) = KDL::Frame(
+    KDL::Rotation::Quaternion(x_a(3), x_a(4), x_a(5), x_a(6)), KDL::Vector(x_a(0), x_a(1), x_a(2)));
+  frames_(1) = KDL::Frame(
+    KDL::Rotation::Quaternion(x_b(3), x_b(4), x_b(5), x_b(6)), KDL::Vector(x_b(0), x_b(1), x_b(2)));
+
+  // compute the difference
+  delta_x_ = KDL::diff(frames_(0), frames_(1), dt);
+  for (size_t i = 0; i < 6; ++i)
+  {
+    delta_x(i) = delta_x_[i];
+  }
+
   return true;
 }
 
@@ -269,6 +295,16 @@ bool KinematicsInterfaceKDL::verify_jacobian(
   return true;
 }
 
+bool KinematicsInterfaceKDL::verify_period(const double dt)
+{
+  if (dt < 0)
+  {
+    RCLCPP_ERROR(LOGGER, "The period (%f) must be a non-negative number", dt);
+    return false;
+  }
+  return true;
+}
+
 }  // namespace kinematics_interface_kdl
 
 #include "pluginlib/class_list_macros.hpp"

kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp

@@ -108,6 +108,21 @@ TEST_F(TestKDLPlugin, KDL_plugin_function)
   {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
+
+  // compute the difference between two cartesian frames
+  Eigen::Matrix<double, 7, 1> x_a, x_b;
+  x_a << 0, 1, 0, 0, 0, 0, 1;
+  x_b << 2, 3, 0, 0, 1, 0, 0;
+  double dt = 1.0;
+  delta_x = Eigen::Matrix<double, 6, 1>::Zero();
+  delta_x_est << 2, 2, 0, 0, 3.14, 0;
+  ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x));
+
+  // ensure that difference math is correct
+  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
+  {
+    ASSERT_NEAR(delta_x(i), delta_x_est(i), 0.02);
+  }
 }
 
 TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
@@ -142,6 +157,21 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
   {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
+
+  // compute the difference between two cartesian frames
+  std::vector<double> x_a(7), x_b(7);
+  x_a = {0, 1, 0, 0, 0, 0, 1};
+  x_b = {2, 3, 0, 0, 1, 0, 0};
+  double dt = 1.0;
+  delta_x = {0, 0, 0, 0, 0, 0};
+  delta_x_est = {2, 2, 0, 0, 3.14, 0};
+  ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x));
+
+  // ensure that difference math is correct
+  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
+  {
+    ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
+  }
 }
 
 TEST_F(TestKDLPlugin, incorrect_input_sizes)
@@ -161,10 +191,14 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
   delta_x[2] = 1;
   Eigen::Matrix<double, Eigen::Dynamic, 1> delta_theta = Eigen::Matrix<double, 2, 1>::Zero();
   Eigen::Matrix<double, 6, 1> delta_x_est;
+  Eigen::Matrix<double, 7, 1> x_a, x_b;
 
   // wrong size input vector
   Eigen::Matrix<double, Eigen::Dynamic, 1> vec_5 = Eigen::Matrix<double, 5, 1>::Zero();
 
+  // wrong value for period
+  double dt = -0.1;
+
   // 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));
@@ -183,6 +217,9 @@ 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));
+
+  // compute the difference between two cartesian frames
+  ASSERT_FALSE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x));
 }
 
 TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description)