Updated imu base estimation. by G-Cervettini · Pull Request #917 · gbionics/bipedal-locomotion-framework

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

 #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

     */
    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

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

Conversation

G-Cervettini commented Dec 16, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

GiulioRomualdi left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

G-Cervettini commented Dec 16, 2024 •

edited

Loading