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);