Add doTransform function for twists or wrenches

tf2_eigen/include/tf2_eigen/tf2_eigen.hpp

@@ -127,6 +127,51 @@ void doTransform(
   t_out = Eigen::Vector3d(transformToEigen(transform) * t_in);
 }
 
+/** \brief Apply a geometry_msgs TransformStamped to a 6-long Eigen vector.
+ * Useful for transforming wrenches or twists.
+ * Wrench: (force-x, force-y, force-z, torque-x, torque-y, torque-z)
+ * Twist: (trans. vel. x, trans. vel. y, trans. vel. z, ang. vel. x, ang. vel. y, ang. vel. z)
+ * This function is a specialization of the doTransform template defined in tf2/convert.h,
+ * although it can not be used in tf2_ros::BufferInterface::transform because this
+ * functions rely on the existence of a time stamp and a frame id in the type which should
+ * get transformed.
+ * \param t_in The vector to transform. Must be 6-long.
+ * \param t_out The transformed vector. Will be 6-long.
+ * \param transform The timestamped transform to apply, as a TransformStamped message.
+ */
+template<>
+inline
+void doTransform(
+  const Eigen::VectorXd & t_in,
+  Eigen::VectorXd & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  // References:
+  // https://core.ac.uk/download/pdf/154240607.pdf, https://www.seas.upenn.edu/~meam520/notes02/Forces8.pdf
+
+  Eigen::Isometry3d affine_transform = tf2::transformToEigen(transform);
+
+  // Build the 6x6 transformation matrix
+  Eigen::MatrixXd twist_transform(6, 6);
+  // upper left 3x3 block is the rotation part
+  twist_transform.block(0, 0, 3, 3) = affine_transform.rotation();
+  // upper right 3x3 block is all zeros
+  twist_transform.block(0, 3, 3, 3) = Eigen::MatrixXd::Zero(3, 3);
+  // lower left 3x3 block is tricky. See https://core.ac.uk/download/pdf/154240607.pdf
+  Eigen::MatrixXd pos_vector_3x3(3, 3);
+  // Disable formatting checks so the matrix remains human-readable
+  /* *INDENT-OFF* */
+  pos_vector_3x3 << 0, -affine_transform.translation().z(), affine_transform.translation().y(),
+                    affine_transform.translation().z(), 0, -affine_transform.translation().x(),
+                    -affine_transform.translation().y(), affine_transform.translation().x(), 0;
+  /* *INDENT-ON* */
+  twist_transform.block(3, 0, 3, 3) = pos_vector_3x3 * affine_transform.rotation();
+  // lower right 3x3 block is the rotation part
+  twist_transform.block(3, 3, 3, 3) = affine_transform.rotation();
+
+  t_out = twist_transform * t_in;
+}
+
 /** \brief Convert a Eigen Vector3d type to a Point message.
  * This function is a specialization of the toMsg template defined in tf2/convert.h.
  * \param in The timestamped Eigen Vector3d to convert.

tf2_eigen/test/tf2_eigen-test.cpp

@@ -239,6 +239,34 @@ TEST_F(EigenBufferTransform, Vector)
   EXPECT_NEAR(v_advanced.z(), 27, EPS);
 }
 
+// Test transformation of a 6-long wrench or twist
+TEST_F(EigenBufferTransform, WrenchTransform)
+{
+  // Transform the wrench (due to gravity) of a point mass to a different frame
+
+  double mass = 1.0;
+  double gravity = -9.81;
+  // Negative y force, no moment
+  Eigen::VectorXd wrench_in(6);
+  wrench_in << 0., mass * gravity, 0., 0., 0., 0.;
+
+  // The new frame is not rotated at all but it is offset along x-axis
+  double x_offset = -0.1;
+  geometry_msgs::msg::TransformStamped tf_to_new_frame;
+  tf_to_new_frame.transform.translation.x = x_offset;
+  tf_to_new_frame.transform.rotation.w = 1.0;
+
+  Eigen::VectorXd wrench_out(6);
+  tf2::doTransform(wrench_in, wrench_out, tf_to_new_frame);
+
+  EXPECT_NEAR(wrench_out(0), 0., EPS);
+  EXPECT_NEAR(wrench_out(1), mass * gravity, EPS);
+  EXPECT_NEAR(wrench_out(2), 0., EPS);
+  EXPECT_NEAR(wrench_out(3), 0., EPS);
+  EXPECT_NEAR(wrench_out(4), 0., EPS);
+  EXPECT_NEAR(wrench_out(5), mass * gravity * x_offset, EPS);
+}
+
 // helper method for Quaternion tests
 ::testing::AssertionResult EigenBufferTransform::doTestEigenQuaternion(
   const Eigen::Quaterniond & parameter, const Eigen::Quaterniond & expected)