diff --git a/source/state_representation/src/space/cartesian/CartesianState.cpp b/source/state_representation/src/space/cartesian/CartesianState.cpp index b818d8cf1..c44a4e5aa 100644 --- a/source/state_representation/src/space/cartesian/CartesianState.cpp +++ b/source/state_representation/src/space/cartesian/CartesianState.cpp @@ -702,6 +702,9 @@ CartesianState& CartesianState::operator*=(double lambda) { this->set_position(lambda * this->get_position()); // calculate the scaled rotation as a displacement from identity auto q = math_tools::exp(math_tools::log(this->get_orientation()), lambda); + if (this->get_orientation().w() * q.w() < 0) { + q = Eigen::Quaterniond(-q.coeffs()); + } this->set_orientation(q); // calculate the other vectors normally this->set_twist(lambda * this->get_twist()); diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp index efbee5809..4c0260b85 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp @@ -765,15 +765,26 @@ TEST(CartesianStateTest, ScalarDivision) { TEST(CartesianStateTest, OrientationScaling) { auto cs = CartesianState::Random("A"); - auto scale = static_cast(rand()) / RAND_MAX; - auto cscaled = scale * cs; - auto qscaled = Eigen::Quaterniond::Identity().slerp(scale, cs.get_orientation()); - EXPECT_TRUE(cscaled.get_orientation().coeffs().isApprox(qscaled.coeffs())); - scale = 1 + static_cast(rand()) / RAND_MAX; - cscaled = scale * cs; - qscaled = cs.get_orientation() * Eigen::Quaterniond::Identity().slerp(scale - 1, cs.get_orientation()); - EXPECT_TRUE(cscaled.get_orientation().coeffs().isApprox(qscaled.coeffs())); + for (int i = 0; i < 5; ++i) { + double scale = static_cast(rand()) / RAND_MAX + i; + + auto qscaled = Eigen::Quaterniond::Identity(); + auto cscaled = scale * cs; + for (int j = 0; j < i; ++j) { + qscaled = qscaled * cs.get_orientation(); + } + qscaled = qscaled * Eigen::Quaterniond::Identity().slerp(scale - i, cs.get_orientation()); + EXPECT_LT(cscaled.get_orientation().angularDistance(qscaled), 1e-3); + + qscaled = Eigen::Quaterniond::Identity(); + cscaled = - scale * cs; + for (int j = 0; j < i; ++j) { + qscaled = qscaled * cs.get_orientation(); + } + qscaled = qscaled * Eigen::Quaterniond::Identity().slerp(scale - i, cs.get_orientation()); + EXPECT_LT(cscaled.get_orientation().angularDistance(qscaled.conjugate()), 1e-3); + } } TEST(CartesianStateTest, Multiplication) {