Changed the order of multiplication when calculating quaternion based…

cartesian_trajectory_interpolation/include/cartesian_trajectory_interpolation/cartesian_trajectory_segment.h

@@ -142,7 +142,8 @@ std::ostream& operator<<(std::ostream& os, const CartesianTrajectorySegment::Spl
  *
  * The computation of quaternion velocities and accelerations from
  * Cartesian angular velocities and accelerations is based on
- * <a href="https://math.stackexchange.com/questions/1792826">this blog post</a>.
+ * <a href="https://math.stackexchange.com/questions/1792826">this blog post</a> and on <a
+ * href="https://arxiv.org/abs/0811.2889v1">this article</a>.
  * \note SplineState has the velocities and accelerations
  * given in the body-local reference frame that is implicitly defined
  * by \b state's pose.
@@ -165,7 +166,8 @@ CartesianTrajectorySegment::SplineState convert(const CartesianState& state);
  *
  * The computation of Cartesian angular velocities and accelerations from
  * quaternion velocities and accelerations is based on
- * <a href="https://math.stackexchange.com/questions/1792826">this blog post</a>.
+ * <a href="https://math.stackexchange.com/questions/1792826">this blog post</a>. and on <a
+ * href="https://arxiv.org/abs/0811.2889v1">this article</a>.
  *
  * @param state The SplineState to convert
  *

cartesian_trajectory_interpolation/src/cartesian_trajectory_segment.cpp

@@ -70,6 +70,7 @@ SplineState convert(const CartesianState& state)
 
   // Note: The pre-multiplication of velocity and acceleration terms with
   // `state.q.inverse()` transforms them into the body-local reference frame.
+  // The calculation performed is `state.q.inverse() * w * state.q()`
   // This is required for computing quaternion-based velocities and
   // accelerations below.
 
@@ -97,7 +98,8 @@ SplineState convert(const CartesianState& state)
   Eigen::Quaterniond q_dot;
   Eigen::Vector3d tmp = state.q.inverse() * state.w;
   Eigen::Quaterniond omega(0, tmp.x(), tmp.y(), tmp.z());
-  q_dot.coeffs() = 0.5 * (omega * state.q).coeffs();
+  // Calculate quaternion based velocity
+  q_dot.coeffs() = 0.5 * (state.q * omega).coeffs();
 
   fill(spline_state.velocity, state.q.inverse() * state.v, q_dot);
 
@@ -110,7 +112,8 @@ SplineState convert(const CartesianState& state)
   Eigen::Quaterniond q_ddot;
   tmp = state.q.inverse() * state.w_dot;
   Eigen::Quaterniond omega_dot(0, tmp.x(), tmp.y(), tmp.z());
-  q_ddot.coeffs() = 0.5 * (omega_dot * state.q).coeffs() + 0.5 * (omega * q_dot).coeffs();
+  // Calculate quaternion based acceleration
+  q_ddot.coeffs() = 0.5 * (state.q * omega_dot).coeffs() + 0.5 * (q_dot * omega).coeffs();
 
   fill(spline_state.acceleration, state.q.inverse() * state.v_dot, q_ddot);
 
@@ -119,6 +122,10 @@ SplineState convert(const CartesianState& state)
 
 CartesianState convert(const SplineState& s)
 {
+  // Note: The pre-multiplication of velocity and acceleration terms with
+  // `state.q.()` transforms them back into correct reference frame.
+  // The calculation performed is `state.q() * w * state.q.inverse()`
+
   CartesianState state;
 
   // Cartesian positions
@@ -136,22 +143,27 @@ CartesianState convert(const SplineState& s)
   }
   Eigen::Quaterniond q_dot(s.velocity[3], s.velocity[4], s.velocity[5], s.velocity[6]);
 
+  // Calculate vel from quaternion based velocity
   Eigen::Quaterniond omega;
-  omega.coeffs() = 2.0 * (q_dot * state.q.inverse()).coeffs();
+  omega.coeffs() = 2.0 * (state.q.inverse() * q_dot).coeffs();
 
   state.v = Eigen::Vector3d(s.velocity[0], s.velocity[1], s.velocity[2]);
   state.w = Eigen::Vector3d(omega.x(), omega.y(), omega.z());
 
   // Cartesian accelerations
   if (s.acceleration.empty())
   {
+    // Re-transform vel to the correct reference frame.
+    state.v = state.q * state.v;
+    state.w = state.q * state.w;
     return state;  // with accelerations zero initialized
   }
   Eigen::Quaterniond q_ddot(s.acceleration[3], s.acceleration[4], s.acceleration[5], s.acceleration[6]);
 
+  // Calculate acc from quaternion based acceleration
   Eigen::Quaterniond omega_dot;
-  omega_dot.coeffs() = 2.0 * ((q_ddot * state.q.inverse()).coeffs() -
-                              ((q_dot * state.q.inverse()) * (q_dot * state.q.inverse())).coeffs());
+  omega_dot.coeffs() = 2.0 * ((state.q.inverse() * q_ddot).coeffs() -
+                              ((state.q.inverse() * q_dot) * (state.q.inverse() * q_dot)).coeffs());
 
   state.v_dot = Eigen::Vector3d(s.acceleration[0], s.acceleration[1], s.acceleration[2]);
   state.w_dot = Eigen::Vector3d(omega_dot.x(), omega_dot.y(), omega_dot.z());

cartesian_trajectory_interpolation/test/cartesian_trajectory_segment_test.cpp

@@ -142,6 +142,227 @@ TEST(TestCartesianTrajectorySegment, NansYieldEmptySplineVelocities)
   EXPECT_EQ(expected.str(), actual.str());
 }
 
+TEST(TestCartesianTrajectorySegment, ConvertToSplineState)
+{
+  CartesianState cartesian_state;
+  // Position
+  cartesian_state.p.x() = 0.1;
+  cartesian_state.p.y() = 0.2;
+  cartesian_state.p.z() = 0.3;
+  cartesian_state.q.w() = 0.0;
+  cartesian_state.q.x() = -0.707107;
+  cartesian_state.q.y() = -0.707107;
+  cartesian_state.q.z() = 0.0;
+  // Velocity
+  cartesian_state.v[0] = 0.1;
+  cartesian_state.v[1] = 0.1;
+  cartesian_state.v[2] = 0.1;
+  cartesian_state.w[0] = 0.1;
+  cartesian_state.w[1] = 0.1;
+  cartesian_state.w[2] = 0.1;
+  // Acceleration
+  cartesian_state.v_dot[0] = 0.1;
+  cartesian_state.v_dot[1] = 0.1;
+  cartesian_state.v_dot[2] = 0.1;
+  cartesian_state.w_dot[0] = 0.1;
+  cartesian_state.w_dot[1] = 0.1;
+  cartesian_state.w_dot[2] = 0.1;
+
+  CartesianTrajectorySegment::SplineState expected_spline_state;
+  // Position
+  expected_spline_state.position.push_back(0.1);
+  expected_spline_state.position.push_back(0.2);
+  expected_spline_state.position.push_back(0.3);
+  expected_spline_state.position.push_back(0.0);
+  expected_spline_state.position.push_back(-0.707107);
+  expected_spline_state.position.push_back(-0.707107);
+  expected_spline_state.position.push_back(0.0);
+  // Velocity
+  expected_spline_state.velocity.push_back(0.1);
+  expected_spline_state.velocity.push_back(0.1);
+  expected_spline_state.velocity.push_back(-0.0999999);
+  expected_spline_state.velocity.push_back(0.0707107);
+  expected_spline_state.velocity.push_back(0.0353553);
+  expected_spline_state.velocity.push_back(-0.0353553);
+  expected_spline_state.velocity.push_back(0.0);
+  // Acceleration
+  expected_spline_state.acceleration.push_back(0.1);
+  expected_spline_state.acceleration.push_back(0.1);
+  expected_spline_state.acceleration.push_back(-0.0999999);
+  expected_spline_state.acceleration.push_back(0.0707107);
+  expected_spline_state.acceleration.push_back(0.0406586);
+  expected_spline_state.acceleration.push_back(-0.030052);
+  expected_spline_state.acceleration.push_back(0.0);
+
+  std::stringstream expected;
+  expected << expected_spline_state;
+
+  CartesianTrajectorySegment::SplineState actual_spline_state = convert(cartesian_state);
+  std::stringstream actual;
+  actual << actual_spline_state;
+
+  EXPECT_EQ(expected.str(), actual.str());
+}
+
+TEST(TestCartesianTrajectorySegment, ConvertToCartesianState)
+{
+  CartesianTrajectorySegment::SplineState spline_state;
+  // Position
+  spline_state.position.push_back(0.1);
+  spline_state.position.push_back(0.2);
+  spline_state.position.push_back(0.3);
+  spline_state.position.push_back(0.0);
+  spline_state.position.push_back(-0.707107);
+  spline_state.position.push_back(-0.707107);
+  spline_state.position.push_back(0.0);
+  // Velocity
+  spline_state.velocity.push_back(0.1);
+  spline_state.velocity.push_back(0.1);
+  spline_state.velocity.push_back(0.1);
+  spline_state.velocity.push_back(0);
+  spline_state.velocity.push_back(0.05);
+  spline_state.velocity.push_back(0.05);
+  spline_state.velocity.push_back(0.05);
+  // Acceleration
+  spline_state.acceleration.push_back(0.1);
+  spline_state.acceleration.push_back(0.1);
+  spline_state.acceleration.push_back(0.1);
+  spline_state.acceleration.push_back(-0.0075);
+  spline_state.acceleration.push_back(0.05);
+  spline_state.acceleration.push_back(0.05);
+  spline_state.acceleration.push_back(0.05);
+
+  CartesianState expected_cartesian_state;
+  // Position
+  expected_cartesian_state.p.x() = 0.1;
+  expected_cartesian_state.p.y() = 0.2;
+  expected_cartesian_state.p.z() = 0.3;
+  expected_cartesian_state.q.w() = 0;
+  expected_cartesian_state.q.x() = -0.707107;
+  expected_cartesian_state.q.y() = -0.707107;
+  expected_cartesian_state.q.z() = 0.0;
+  // Velocity
+  expected_cartesian_state.v[0] = 0.1;
+  expected_cartesian_state.v[1] = 0.1;
+  expected_cartesian_state.v[2] = -0.1;
+  expected_cartesian_state.w[0] = -0.0707107;
+  expected_cartesian_state.w[1] = 0.0707107;
+  expected_cartesian_state.w[2] = 0.0;
+  // Acceleration
+  expected_cartesian_state.v_dot[0] = 0.1;
+  expected_cartesian_state.v_dot[1] = 0.1;
+  expected_cartesian_state.v_dot[2] = -0.1;
+  expected_cartesian_state.w_dot[0] = -0.0913173;
+  expected_cartesian_state.w_dot[1] = 0.0701041;
+  expected_cartesian_state.w_dot[2] = 0.0;
+
+  std::stringstream expected;
+  expected << expected_cartesian_state;
+
+  CartesianState actual_cartesian_state = convert(spline_state);
+  std::stringstream actual;
+  actual << actual_cartesian_state;
+
+  EXPECT_EQ(expected.str(), actual.str());
+}
+
+TEST(TestCartesianTrajectorySegment, interpolationOfOrientation)
+{
+  // This will test that the orientation is interpolated correctly. The trajectory is based on a sine wave, such that
+  // the first and second derivatives are known. The sine wave will produce an angle that is rotated around the z-axis
+  // of an "artificial" robot's TCP. This orientation is then rotated to the base of the "artificial" robot. This will
+  // make sure that we catch that the interpolation of the orientation is done correctly.
+
+  // Convenience method
+  auto compute_cartesian_state = [](auto& cartesian_state, const auto& time) {
+    double omega = 0.2;
+    double angle = sin(omega * time);
+
+    // Rotation around the "artificial" robot's TCP z-axis
+    Eigen::Matrix3d rot_z = Eigen::Matrix3d::Zero();
+    rot_z(0, 0) = cos(angle);
+    rot_z(0, 1) = -sin(angle);
+    rot_z(1, 0) = sin(angle);
+    rot_z(1, 1) = cos(angle);
+    rot_z(2, 2) = 1;
+
+    // Rotate the z-axis rotation to the base of the "artificial" robot
+    Eigen::Matrix3d rot_base = Eigen::Matrix3d::Zero();
+    rot_base(0, 1) = 1.0;
+    rot_base(1, 0) = 1.0;
+    rot_base(2, 2) = -1.0;
+    rot_z = rot_base * rot_z;
+
+    // Create cartesian state
+    Eigen::Quaterniond q(rot_z);
+
+    cartesian_state.p.x() = 0.0;
+    cartesian_state.p.y() = 0.0;
+    cartesian_state.p.z() = 0.0;
+
+    cartesian_state.q.w() = q.w();
+    cartesian_state.q.x() = q.x();
+    cartesian_state.q.y() = q.y();
+    cartesian_state.q.z() = q.z();
+
+    cartesian_state.v[0] = 0.0;
+    cartesian_state.v[1] = 0.0;
+    cartesian_state.v[2] = 0.0;
+
+    cartesian_state.w[0] = 0.0;
+    cartesian_state.w[1] = 0.0;
+    cartesian_state.w[2] = omega * cos(omega * time);
+    // Rotate the velocity to the base of the "artificial" robot
+    cartesian_state.w = rot_base * cartesian_state.w;
+
+    cartesian_state.v_dot[0] = 0.0;
+    cartesian_state.v_dot[0] = 0.0;
+    cartesian_state.v_dot[0] = 0.0;
+
+    cartesian_state.w_dot[0] = 0.0;
+    cartesian_state.w_dot[1] = 0.0;
+    cartesian_state.w_dot[2] = (-omega * omega) * sin(omega * time);
+    // Rotate the acceleration to the base of the "artificial" robot
+    cartesian_state.w_dot = rot_base * cartesian_state.w_dot;
+  };
+
+  double start_time = 0.0;
+  double end_time = 1.0;
+
+  // Setup states
+  CartesianState start_state;
+  compute_cartesian_state(start_state, start_time);
+
+  CartesianState end_state;
+  compute_cartesian_state(end_state, end_time);
+
+  // Create segment
+  CartesianTrajectorySegment segment{ start_time, start_state, end_time, end_state };
+
+  CartesianState sampled_state;
+  segment.sample(0.3, sampled_state);
+
+  CartesianState expected_state;
+  compute_cartesian_state(expected_state, 0.3);
+
+  // Orientation
+  double eps = 1e-3;
+  EXPECT_NEAR(expected_state.q.x(), sampled_state.q.x(), eps);
+  EXPECT_NEAR(expected_state.q.y(), sampled_state.q.y(), eps);
+  EXPECT_NEAR(expected_state.q.z(), sampled_state.q.z(), eps);
+  EXPECT_NEAR(expected_state.q.w(), sampled_state.q.w(), eps);
+
+  // Velocity
+  EXPECT_NEAR(expected_state.w[0], sampled_state.w[0], eps);
+  EXPECT_NEAR(expected_state.w[1], sampled_state.w[1], eps);
+  EXPECT_NEAR(expected_state.w[2], sampled_state.w[2], eps);
+
+  // Acceleration
+  EXPECT_NEAR(expected_state.w_dot[0], sampled_state.w_dot[0], eps);
+  EXPECT_NEAR(expected_state.w_dot[1], sampled_state.w_dot[1], eps);
+  EXPECT_NEAR(expected_state.w_dot[2], sampled_state.w_dot[2], eps);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);