Updated imu base estimation. #917
Open
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
G-Cervettini
requested review from
GiulioRomualdi and
S-Dafarra
as code owners
G-Cervettini
changed the title
S-Dafarra
reviewed
Dec 16, 2024
| @@ -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> | |||
There was a problem hiding this comment.
I think you should revert this change, as this way of including iDynTree headers has been deprecated since version 10.0.0
Comment on lines
+47
to
+54
| 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 */ |
There was a problem hiding this comment.
Consider using a single notation. Either:
isStance_LandmeasuredRotation_L
or
isLeftStanceandmeasuredLeftRotation
Comment on lines
+14
to
+19
| 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 |
There was a problem hiding this comment.
How comes that you changed the order of the angles and the computations remain the same? Was it bugged before?
| 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"), |
There was a problem hiding this comment.
Consider storing the output of baseEstimatorPtr->getGroup("MODEL_INFO") since you are using it multiple times below
Comment on lines
+254
to
+258
| // 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; |
There was a problem hiding this comment.
Leftover? There is also some other commented code below
| } | ||
| } | ||
|
|
||
| m_stanceLinkIndex = m_model.getFrameLink(stanceFootFrameIndex); |
There was a problem hiding this comment.
What if both isLeftStance and isRightStance are false? Can it happen? Maybe it is worth printing an error in this case?
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
This PR involves the first stable release of the
BaseEstimatorFromFootIMU.As agreed with @GiulioRomualdi this PR replaces #792 to solve its conflicts.