Add methods for computing frame differences

Author: francescodonofrio

kinematics_interface/include/kinematics_interface/kinematics_interface.hpp

@@ -95,6 +95,18 @@ class KinematicsInterface
     const Eigen::VectorXd & joint_pos, const std::string & link_name,
     Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian) = 0;
 
+  /**
+   * \brief Calculates the difference between two Cartesian frames
+   * \param[in] x_a first Cartesian frame (x, y, z, wx, wy, wz, ww)
+   * \param[in] x_b second Cartesian frame (x, y, z, wx, wy, wz, ww)
+   * \param[in] dt time interval over which the numerical differentiation takes place
+   * \param[out] delta_x  Cartesian deltas (x, y, z, wx, wy, wz)
+   * \return true if successful
+   */
+  virtual 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) = 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);
@@ -110,6 +122,10 @@ class KinematicsInterface
   bool calculate_jacobian(
     const std::vector<double> & joint_pos_vec, const std::string & link_name,
     Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
+
+  bool calculate_frame_difference(
+    std::vector<double> & x_a_vec, std::vector<double> & x_b_vec, double dt,
+    std::vector<double> & delta_x_vec);
 };
 
 }  // namespace kinematics_interface

kinematics_interface/src/kinematics_interface.cpp

@@ -80,4 +80,37 @@ bool KinematicsInterface::calculate_jacobian(
   return calculate_jacobian(joint_pos, link_name, jacobian);
 }
 
+bool KinematicsInterface::calculate_frame_difference(
+  std::vector<double> & x_a_vec, std::vector<double> & x_b_vec, double dt,
+  std::vector<double> & delta_x_vec)
+{
+  if (x_a_vec.size() != 7)
+  {
+    RCLCPP_ERROR(
+      LOGGER, "The length of the first cartesian vector (%zu) must be 7.", x_a_vec.size());
+    return false;
+  }
+  Eigen::Matrix<double, 7, 1> x_a(x_a_vec.data());
+  if (x_b_vec.size() != 7)
+  {
+    RCLCPP_ERROR(
+      LOGGER, "The length of the second cartesian vector (%zu) must be 7.", x_b_vec.size());
+    return false;
+  }
+  Eigen::Matrix<double, 7, 1> x_b(x_b_vec.data());
+  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 = calculate_frame_difference(x_a, x_b, dt, delta_x);
+  for (auto i = 0ul; i < delta_x_vec.size(); i++)
+  {
+    delta_x_vec[i] = delta_x[i];
+  }
+  return ret;
+}
+
 }  // namespace kinematics_interface