From 5e06e5a1d193fc24e789b6550a55ffa4fb40409e Mon Sep 17 00:00:00 2001
From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com>
Date: Wed, 12 Jun 2024 10:22:54 +0200
Subject: [PATCH] =?UTF-8?q?Changed=20the=20order=20of=20multiplication=20w?=
=?UTF-8?q?hen=20calculating=20quaternion=20based=E2=80=A6=20(#15)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
* Changed the order of multiplication when calculating quaternion based vel and acc
This is done since the velocity and acceleration is given in the body-local reference frame.
* Added new tests for cartesian trajectory segment
This adds test for
* For conversion between spline state and cartesian state
* Interpolation of orientation
* Trim trailing white space
---
.../cartesian_trajectory_segment.h | 6 +-
.../src/cartesian_trajectory_segment.cpp | 22 +-
.../cartesian_trajectory_segment_test.cpp | 221 ++++++++++++++++++
3 files changed, 242 insertions(+), 7 deletions(-)
diff --git a/cartesian_trajectory_interpolation/include/cartesian_trajectory_interpolation/cartesian_trajectory_segment.h b/cartesian_trajectory_interpolation/include/cartesian_trajectory_interpolation/cartesian_trajectory_segment.h
index c50100e..0f22d94 100644
--- a/cartesian_trajectory_interpolation/include/cartesian_trajectory_interpolation/cartesian_trajectory_segment.h
+++ b/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
- * this blog post.
+ * this blog post and on this article.
* \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
- * this blog post.
+ * this blog post. and on this article.
*
* @param state The SplineState to convert
*
diff --git a/cartesian_trajectory_interpolation/src/cartesian_trajectory_segment.cpp b/cartesian_trajectory_interpolation/src/cartesian_trajectory_segment.cpp
index 272e066..6c59390 100644
--- a/cartesian_trajectory_interpolation/src/cartesian_trajectory_segment.cpp
+++ b/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,8 +143,9 @@ 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());
@@ -145,13 +153,17 @@ CartesianState convert(const SplineState& s)
// 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());
diff --git a/cartesian_trajectory_interpolation/test/cartesian_trajectory_segment_test.cpp b/cartesian_trajectory_interpolation/test/cartesian_trajectory_segment_test.cpp
index f87db6d..6fef925 100644
--- a/cartesian_trajectory_interpolation/test/cartesian_trajectory_segment_test.cpp
+++ b/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);