From a54f510df4723ad6f4348dd8c2201f38f2146e7b Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 13 Jan 2025 11:47:43 +0100 Subject: [PATCH] Attempt to fix BaseEstimatorFromFootIMU test --- .../BaseEstimatorFromFootIMUTest.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/Estimators/tests/FloatingBaseEstimators/BaseEstimatorFromFootIMUTest.cpp b/src/Estimators/tests/FloatingBaseEstimators/BaseEstimatorFromFootIMUTest.cpp index 1d106bbb68..026aa5e2da 100644 --- a/src/Estimators/tests/FloatingBaseEstimators/BaseEstimatorFromFootIMUTest.cpp +++ b/src/Estimators/tests/FloatingBaseEstimators/BaseEstimatorFromFootIMUTest.cpp @@ -88,9 +88,15 @@ 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; + 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 +111,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)); }