Updated imu base estimation. #917

G-Cervettini · 2024-12-16T14:14:05Z

This PR involves the first stable release of the BaseEstimatorFromFootIMU.
As agreed with @GiulioRomualdi this PR replaces #792 to solve its conflicts.

Copied BaseEstimatorFromFootIMU.cpp from: https://github.com/G-Cervettini/bipedal-locomotion-framework/blob/fix_imu_base_estimation_rebased/src/Estimators/src/BaseEstimatorFromFootIMU.cpp

Copied BaseEstimatorFromFootIMU.h from: https://github.com/G-Cervettini/bipedal-locomotion-framework/blob/fix_imu_base_estimation_rebased/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h

S-Dafarra · 2024-12-16T14:54:50Z

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h

@@ -12,7 +12,7 @@
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/System/Advanceable.h>
 #include <iDynTree/KinDynComputations.h>
-#include <iDynTree/Model.h>
+#include <iDynTree/Model/Model.h>


I think you should revert this change, as this way of including iDynTree headers has been deprecated since version 10.0.0

S-Dafarra · 2024-12-16T14:57:21Z

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h

+    bool isLeftStance; /**< true if the left foot is in contact with the ground */
+    bool isRightStance; /**< true if the right foot is in contact with the ground */
    Eigen::VectorXd jointPositions; /**< vector of the robot joint positions */
    Eigen::VectorXd jointVelocities; /**< vector of the robot joint velocities */
-    manif::SE3d desiredFootPose; /**< desired orientation and position of the foot
-                                  as per footstep planner output */
-    manif::SO3d measuredRotation; /**< actual orientation of the foot measured by
-                                   on-board IMU */
+    manif::SE3d offsetStanceFootPose; /**< Optional offset orientation and position of the foot.
+                                      E.g. as per footstep planner output */
+    manif::SO3d measuredRotation_L; /**< actual orientation of the left foot measured by
+                                    on-board IMU */


Consider using a single notation. Either:

isStance_L and measuredRotation_L

or

isLeftStance and measuredLeftRotation

I prefer this:

isLeftStance and measuredLeftRotation

S-Dafarra · 2024-12-16T15:02:43Z

src/Estimators/src/BaseEstimatorFromFootIMU.cpp

+Eigen::Vector3d toXYZ(const Eigen::Matrix3d& r)
 {
    Eigen::Vector3d output;
-    double& thetaZ = output[0]; // Roll
+    double& thetaX = output[0]; // Roll
    double& thetaY = output[1]; // Pitch
-    double& thetaX = output[2]; // Yaw
+    double& thetaZ = output[2]; // Yaw


How comes that you changed the order of the angles and the computations remain the same? Was it bugged before?

S-Dafarra · 2024-12-16T15:08:56Z

src/Estimators/src/BaseEstimatorFromFootIMU.cpp

-    m_cornersInLocalFrame.emplace_back(-m_footWidth / 2, -m_footLength / 2, 0);
-    m_cornersInLocalFrame.emplace_back(+m_footWidth / 2, -m_footLength / 2, 0);
+    // Frame associated to the base of the robot (whose pose is estimated)
+    bool ok = populateParameter(baseEstimatorPtr->getGroup("MODEL_INFO"),


Consider storing the output of baseEstimatorPtr->getGroup("MODEL_INFO") since you are using it multiple times below

S-Dafarra · 2024-12-16T15:09:38Z

src/Estimators/src/BaseEstimatorFromFootIMU.cpp

+            // Eigen::Matrix3d stepRot = m_state.footPose_R.rotation().inverse() * m_state.footPose_L.rotation();
+            // Eigen::Vector3d stepRotRPY = toXYZ(stepRot);
+            // manif::SO3d stepRotManif = manif::SO3d(stepRotRPY(0), stepRotRPY(1), stepRotRPY(2));
+            // manif::SE3d T_walkRot(m_noTras, stepRotManif);
+            // manif::SE3d temp = T_walkTras * T_walkRot * m_T_walk * m_T_yawDrift;


Leftover? There is also some other commented code below

S-Dafarra · 2024-12-16T15:11:28Z

src/Estimators/src/BaseEstimatorFromFootIMU.cpp

+        }
+    }
+
+    m_stanceLinkIndex = m_model.getFrameLink(stanceFootFrameIndex);


What if both isLeftStance and isRightStance are false? Can it happen? Maybe it is worth printing an error in this case?

GiulioRomualdi

Other comments

GiulioRomualdi · 2025-01-07T13:42:51Z

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h

+    manif::SE3d::Tangent baseVelocity; /**< velocity of the robot  root link. */
+    manif::SE3d footPose_L; /**< pose of the left foot */
+    manif::SE3d footPose_R; /**< pose of the right foot */
+    Eigen::Vector3d centerOfMassPosition;


I would avoid propagating the CoM position. In the end the base estimator should provide the base state

GiulioRomualdi · 2025-01-07T13:43:07Z

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h

+    std::vector<Eigen::Vector3d> stanceFootShadowCorners;
+    std::vector<Eigen::Vector3d> stanceFootCorners;
+    int supportCornerIndex;


Could you please add comments for this?

GiulioRomualdi · 2025-01-07T13:43:28Z

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h

+    bool isLeftStance; /**< true if the left foot is in contact with the ground */
+    bool isRightStance; /**< true if the right foot is in contact with the ground */
    Eigen::VectorXd jointPositions; /**< vector of the robot joint positions */
    Eigen::VectorXd jointVelocities; /**< vector of the robot joint velocities */
-    manif::SE3d desiredFootPose; /**< desired orientation and position of the foot
-                                  as per footstep planner output */
-    manif::SO3d measuredRotation; /**< actual orientation of the foot measured by
-                                   on-board IMU */
+    manif::SE3d offsetStanceFootPose; /**< Optional offset orientation and position of the foot.
+                                      E.g. as per footstep planner output */
+    manif::SO3d measuredRotation_L; /**< actual orientation of the left foot measured by
+                                    on-board IMU */


I prefer this:

isLeftStance and measuredLeftRotation

GiulioRomualdi · 2025-01-07T13:43:51Z

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h

@@ -120,6 +139,8 @@ class BaseEstimatorFromFootIMU
     */
    const BaseEstimatorFromFootIMUState& getOutput() const override;

+    void resetBaseEstimatorFromFootIMU();


Could you please add the documentation to this function?

GiulioRomualdi · 2025-01-07T13:44:11Z

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h

+    manif::SE3d m_T_walk;
+    manif::SE3d m_T_yawDrift;
+    double m_yawOld;


Could you please comment this quantities?

GiulioRomualdi · 2025-01-07T13:45:36Z

src/Estimators/src/BaseEstimatorFromFootIMU.cpp

+    // MANUAL CORRECTION: measured Roll, Pitch and Yaw.
+    // double temp_roll = -m_measuredRPY(1);
+    // double temp_pitch = -m_measuredRPY(0);
+    // double temp_yaw = -m_measuredRPY(2);
+    // m_measuredRPY(0) = temp_roll;
+    // m_measuredRPY(1) = temp_pitch;
+    // m_measuredRPY(2) = temp_yaw;


I would remove this code if not required

GiulioRomualdi · 2025-01-07T13:47:01Z

src/Estimators/src/BaseEstimatorFromFootIMU.cpp

-    // desired position.
-    switch (supportCornerIndex)
+    // checking that the index of the lowest corner is within the range [0, 3].
+    if (!(0 <= m_state.supportCornerIndex <= 3))


I would simplify the if state as

Suggested change

if (!(0 <= m_state.supportCornerIndex <= 3))

if (m_state.supportCornerIndex > 3 || m_state.supportCornerIndex < 0)

GiulioRomualdi · 2025-01-07T13:47:15Z

src/Estimators/src/BaseEstimatorFromFootIMU.cpp

+    // std::cerr << "L FOOT ROTATION IN: " << toXYZ(m_input.measuredRotation_L.rotation()).transpose() << std::endl;
+    // std::cerr << "L FOOT ROTATION OUT: " << toXYZ(m_state.footPose_L.rotation()).transpose() << std::endl;
+    // std::cerr << "R FOOT ROTATION IN: " << toXYZ(m_input.measuredRotation_R.rotation()).transpose() << std::endl;
+    // std::cerr << "R FOOT ROTATION OUT: " << toXYZ(m_state.footPose_R.rotation()).transpose() << std::endl;


Suggested change

// std::cerr << "L FOOT ROTATION IN: " << toXYZ(m_input.measuredRotation_L.rotation()).transpose() << std::endl;

// std::cerr << "L FOOT ROTATION OUT: " << toXYZ(m_state.footPose_L.rotation()).transpose() << std::endl;

// std::cerr << "R FOOT ROTATION IN: " << toXYZ(m_input.measuredRotation_R.rotation()).transpose() << std::endl;

// std::cerr << "R FOOT ROTATION OUT: " << toXYZ(m_state.footPose_R.rotation()).transpose() << std::endl;

G-Cervettini added 2 commits December 16, 2024 15:08

Update BaseEstimatorFromFootIMU.cpp

2d2eb06

Copied BaseEstimatorFromFootIMU.cpp from: https://github.com/G-Cervettini/bipedal-locomotion-framework/blob/fix_imu_base_estimation_rebased/src/Estimators/src/BaseEstimatorFromFootIMU.cpp

Update BaseEstimatorFromFootIMU.h

56d3c4f

Copied BaseEstimatorFromFootIMU.h from: https://github.com/G-Cervettini/bipedal-locomotion-framework/blob/fix_imu_base_estimation_rebased/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h

G-Cervettini requested review from GiulioRomualdi and S-Dafarra as code owners December 16, 2024 14:14

github-actions bot added the 🔨 component: Estimators label Dec 16, 2024

G-Cervettini changed the title ~~Updated imu base estimation. Replaces: https://github.com/ami-iit/bipedal-locomotion-framework/pull/792~~ Updated imu base estimation. Dec 16, 2024

S-Dafarra reviewed Dec 16, 2024

View reviewed changes

GiulioRomualdi assigned G-Cervettini Dec 17, 2024

GiulioRomualdi requested changes Jan 7, 2025

View reviewed changes

G-Cervettini mentioned this pull request Jan 20, 2025

Stable BaseEstimatorFromFootIMU. #792

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Updated imu base estimation. #917

Updated imu base estimation. #917

G-Cervettini commented Dec 16, 2024 •

edited

Loading

S-Dafarra Dec 16, 2024

S-Dafarra Dec 16, 2024

GiulioRomualdi Jan 7, 2025

S-Dafarra Dec 16, 2024

S-Dafarra Dec 16, 2024

S-Dafarra Dec 16, 2024

S-Dafarra Dec 16, 2024

GiulioRomualdi left a comment

GiulioRomualdi Jan 7, 2025

GiulioRomualdi Jan 7, 2025

GiulioRomualdi Jan 7, 2025

GiulioRomualdi Jan 7, 2025

GiulioRomualdi Jan 7, 2025

GiulioRomualdi Jan 7, 2025

GiulioRomualdi Jan 7, 2025

GiulioRomualdi Jan 7, 2025

	if (!(0 <= m_state.supportCornerIndex <= 3))
	if (m_state.supportCornerIndex > 3 \|\| m_state.supportCornerIndex < 0)

Updated imu base estimation. #917

Are you sure you want to change the base?

Updated imu base estimation. #917

Conversation

G-Cervettini commented Dec 16, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

GiulioRomualdi left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

G-Cervettini commented Dec 16, 2024 •

edited

Loading