Attempt to fix BaseEstimatorFromFootIMU test

ami-iit · Jan 13, 2025 · 9a354c8 · 9a354c8
1 parent a9df589
commit 9a354c8
Showing 1 changed file with 8 additions and 3 deletions.
diff --git a/src/Estimators/tests/FloatingBaseEstimators/BaseEstimatorFromFootIMUTest.cpp b/src/Estimators/tests/FloatingBaseEstimators/BaseEstimatorFromFootIMUTest.cpp
@@ -88,9 +88,14 @@ TEST_CASE("BaseEstimatorFromFootIMU")
     baseVelocity.setZero();
     Eigen::Vector3d gravity;
     gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation;
-    manif::SE3d I = manif::SE3d::Identity();
+    manif::SE3d basePose = manif::SE3d::Identity();
+    Eigen::Matrix3d alignedRoot << -1.0,  0.0,  0.0,
+                                    0.0, -1.0,  0.0,
+                                    0.0,  0.0,  1.0;
+    Eigen::Quaterniond alignedRootQuaternion(alignedRoot);
+    basePose.quat(alignedRootQuaternion);
 
-    REQUIRE(kinDyn->setRobotState(I.transform(), encoders, baseVelocity, encoder_speeds, gravity));
+    REQUIRE(kinDyn->setRobotState(basePose.transform(), encoders, baseVelocity, encoder_speeds, gravity));
 
     BaseEstimatorFromFootIMUInput input;
     input.jointPositions = encoders;
@@ -105,5 +110,5 @@ TEST_CASE("BaseEstimatorFromFootIMU")
     REQUIRE(estimator.isOutputValid());
 
     constexpr double tolerance = 1e-3;
-    REQUIRE(estimator.getOutput().basePose.coeffs().isApprox(I.coeffs(), tolerance));
+    REQUIRE(estimator.getOutput().basePose.coeffs().isApprox(basePose.coeffs(), tolerance));
 }