fix: tests

aica-technology · Nov 6, 2023 · 4a44a9e · 4a44a9e
1 parent bec2e87
commit 4a44a9e
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 8 deletions.
diff --git 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
@@ -765,15 +765,26 @@ TEST(CartesianStateTest, ScalarDivision) {
 
 TEST(CartesianStateTest, OrientationScaling) {
   auto cs = CartesianState::Random("A");
-  auto scale = static_cast<double>(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<double>(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<double>(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) {