Add Wrench conversion

Author: gleichdick

test_tf2/test/test_convert.cpp

@@ -41,6 +41,8 @@
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+
 TEST(tf2Convert, kdlToBullet)
 {
   double epsilon = 1e-9;
@@ -234,6 +236,34 @@ TEST(TfEigenKdl, TestFrameAffine3d)
   EXPECT_EQ(kdl_v, kdl_v1);
 }
 
+TEST(TfEigenKdl, TestTwistMatrix)
+{
+  const auto kdl_v = KDL::Twist(KDL::Vector(1, 2, 3), KDL::Vector(4, 5, 6));
+  Vector6d eigen_v;
+  tf2::convert(kdl_v, eigen_v);
+  KDL::Twist kdl_v1;
+  tf2::convert(eigen_v, kdl_v1);
+  EXPECT_EQ(kdl_v, kdl_v1);
+}
+
+
+TEST(TfEigenKdl, TestMatrixWrench)
+{
+  Vector6d eigen_v;
+  eigen_v << 1, 2, 3, 3, 2, 1;
+  KDL::Wrench kdl_v;
+  tf2::convert(eigen_v, kdl_v);
+  std::array<tf2::Vector3, 2> tf2_v;
+  tf2::convert(kdl_v, tf2_v);
+  Vector6d eigen_v1;
+  tf2::convert(tf2_v, eigen_v1);
+  std::array<tf2::Vector3, 2> tf2_v1;
+  tf2::convert(eigen_v1, tf2_v1);
+  Vector6d eigen_v2;
+  tf2::convert(tf2_v1, eigen_v2);
+  EXPECT_EQ(eigen_v, eigen_v2);
+}
+
 
 TEST(TfEigenKdl, TestVectorVector3d)
 {

tf2_eigen/include/tf2_eigen/tf2_eigen.h

@@ -39,6 +39,7 @@
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/quaternion_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/wrench.hpp>
 
 namespace tf2
 {
@@ -388,6 +389,35 @@ struct defaultMessage<Eigen::Matrix<double, 6, 1>>
   using type = geometry_msgs::msg::Twist;
 };
 
+template <>
+struct ImplDetails<Eigen::Matrix<double, 6, 1>, geometry_msgs::msg::Wrench>
+{
+  static void toMsg(const Eigen::Matrix<double, 6, 1> & in, geometry_msgs::msg::Wrench & msg)
+  {
+    msg.force.x = in[0];
+    msg.force.y = in[1];
+    msg.force.z = in[2];
+    msg.torque.x = in[3];
+    msg.torque.y = in[4];
+    msg.torque.z = in[5];
+  }
+
+  /** \brief Convert a Twist message transform type to a Eigen 6x1 Matrix.
+   * This function is a specialization of the toMsg template defined in tf2/convert.h.
+   * \param msg The Twist message to convert.
+   * \param out The twist converted to a Eigen 6x1 Matrix.
+   */
+  static void fromMsg(const geometry_msgs::msg::Wrench & msg, Eigen::Matrix<double, 6, 1> & out)
+  {
+    out[0] = msg.force.x;
+    out[1] = msg.force.y;
+    out[2] = msg.force.z;
+    out[3] = msg.torque.x;
+    out[4] = msg.torque.y;
+    out[5] = msg.torque.z;
+  }
+};
+
 }  // namespace impl
 
 /** \brief Convert a Eigen::Vector3d type to a geometry_msgs::msg::Vector3.

tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h

@@ -48,9 +48,14 @@
 #include <geometry_msgs/msg/wrench.hpp>
 #include <kdl/frames.hpp>
 
-namespace tf2
+namespace Eigen
 {
+template <typename T, int, int, int, int, int>
+class Matrix;
+}
 
+namespace tf2
+{
 /** \brief Convert a TransformStamped message to a KDL frame.
  * \param t TransformStamped message to convert.
  * \return The converted KDL Frame.
@@ -446,7 +451,21 @@ struct ImplDetails<std::array<tf2::Vector3, 2>, geometry_msgs::msg::Wrench>
   }
 };
 
+template <>
+struct defaultMessage<std::array<tf2::Vector3, 2> >
+{
+  using type = geometry_msgs::msg::Wrench;
+};
 }  // namespace impl
+
+template <int options, int row, int cols>
+void convert(
+  const Eigen::Matrix<double, 6, 1, options, row, cols> & in, std::array<tf2::Vector3, 2> & out)
+{
+  out[0] = tf2::Vector3{in(0), in(1), in(2)};
+  out[1] = tf2::Vector3{in(3), in(4), in(5)};
+}
+
 }  // namespace tf2
 
 #endif  // TF2_GEOMETRY_MSGS_H

tf2_kdl/include/tf2_kdl/tf2_kdl.h

@@ -43,6 +43,12 @@
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <kdl/frames.hpp>
 
+namespace Eigen
+{
+template <typename T, int, int, int, int, int>
+class Matrix;
+}
+
 namespace tf2
 {
 /** \brief Convert a timestamped transform to the equivalent KDL data type.
@@ -245,6 +251,14 @@ struct defaultMessage<KDL::Wrench>
 };
 }  // namespace impl
 
+template <int options, int rows, int cols>
+void convert(Eigen::Matrix<double, 6, 1, options, rows, cols> const & in, KDL::Wrench & out)
+{
+  const auto msg =
+    toMsg<Eigen::Matrix<double, 6, 1, options, rows, cols>, geometry_msgs::msg::Wrench>(in);
+  fromMsg<>(msg, out);
+}
+
 // ---------------------
 // Frame
 // ---------------------