Skip to content

Commit

Permalink
Update BaseEstimatorFromFootIMU.h
Browse files Browse the repository at this point in the history 
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
  • Loading branch information
@G-Cervettini
G-Cervettini authored Dec 16, 2024
1 parent 2d2eb06 commit 56d3c4f
Showing 1 changed file with 85 additions and 43 deletions.
128 changes: 85 additions & 43 deletions src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
#include <manif/SE3.h>
#include <manif/SO3.h>

Expand All @@ -30,33 +30,52 @@ struct BaseEstimatorFromFootIMUState
{
manif::SE3d basePose; /**< final output of the estimator - pose of the robot
root link. */
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;
std::vector<Eigen::Vector3d> stanceFootShadowCorners;
std::vector<Eigen::Vector3d> stanceFootCorners;
int supportCornerIndex;
};

/**
* BaseEstimatorFromFootIMUInput contains the input of the BaseEstimatorFromFootIMU class.
*/
struct BaseEstimatorFromFootIMUInput
{
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 */
manif::SO3d measuredRotation_R; /**< actual orientation of the right foot measured by
on-board IMU */
manif::SO3Tangentd measuredAngularVelocity_L; /**< actual angular velocity of the left foot
measured by on-board IMU */
manif::SO3Tangentd measuredAngularVelocity_R; /**< actual angular velocity of the right foot
measured by on-board IMU */
};

/**
* BaseEstimatorFromFootIMU implements the propagation of the foot pose to the root link through the
* kinematic chain given by the leg joints positions.
*
* This class assumes that the foot has a rectangular shape as shown in the following schematics
* +
* p2 __|__ p1 __
* | | | |
* ___|__|__|___+ | FOOT
* | | | | LENGTH
* |__|__| __|
* p3 | p4
*
* FRONT
* +X
* p1 __|__ p0 __
* | | | |
* +Y___|__|__|___ | FOOT
* | | | | LENGTH
* |__|__| __|
* p2 | p3
*
* HIND
*
* |_____|
* FOOT
Expand All @@ -77,7 +96,7 @@ class BaseEstimatorFromFootIMU
// clang-format off
/**
* Initialize the BaseEstimatorFromFootIMU block.
* The setModel and setStanceFootRF methods must be called before initialization.
* The setModel method must be called before initialization.
* @param handler pointer to the parameter handler.
* @note The following parameters are required
* | Parameter Name | Type | Description | Mandatory |
Expand Down Expand Up @@ -120,6 +139,8 @@ class BaseEstimatorFromFootIMU
*/
const BaseEstimatorFromFootIMUState& getOutput() const override;

void resetBaseEstimatorFromFootIMU();

/**
* Set the state of the BaseEstimatorFromFootIMU.
* @param state of the BaseEstimatorFromFootIMU
Expand All @@ -139,50 +160,71 @@ class BaseEstimatorFromFootIMU
/**
* Define the 4 foot vertices in World reference frame:
*
* +
* m_p2 __|__ m_p1 __
* | | | |
* ___|__|__|___+ | FOOT
* | | | | LENGTH
* |__|__| __|
* m_p3 | m_p4
*
* |_____|
* FOOT
* WIDTH
* +x FRONT
* m_p1 __|__ m_p0 __
* | | | |
* +y___|__|__|___ | FOOT
* | | | | LENGTH
* |__|__| __|
* m_p2 | m_p3
* HIND
* |_____|
* FOOT
* WIDTH
*
*/
std::vector<Eigen::Vector3d> m_cornersInLocalFrame; /**< this implementation is considering
std::vector<Eigen::Vector3d> m_cornersInInertialFrame; /**< this implementation is considering
rectangular feet (4 corners) */
std::vector<Eigen::Vector3d> m_transformedFootCorners; /**< vector of the foot corners
std::vector<Eigen::Vector3d> m_tiltedFootCorners; /**< vector of the foot corners
transformed into the inertial frame */
Eigen::Vector3d m_desiredTranslation; /**< the position vector extracted from the
`desiredFootPose` */
Eigen::Matrix3d m_desiredRotation; /**< the rotation matrix extracted from the `desiredFootPose`
*/
Eigen::Vector3d m_desiredRPY; /**< the rotation matrix extracted from the `desiredFootPose`
Eigen::Vector3d m_offsetTranslation; /**< the position vector extracted from the
`offsetFootPose` */
Eigen::Matrix3d m_offsetRotation; /**< the rotation matrix extracted from the `offsetFootPose`
*/
Eigen::Vector3d m_offsetRPY; /**< the rotation matrix extracted from the `offsetFootPose`
converted into Euler angles */
Eigen::Matrix3d m_measuredRotation; /**< the measured foot orientation casted manif::SO3d -->
Eigen::Matrix3d */
manif::SO3Tangentd m_measuredAngularVelocity; /**< the measured stance foot angular velocity */
Eigen::Vector3d m_measuredRPY; /**< the measured foot orientation converted into Euler angles */
manif::SO3d m_measuredRotationCorrected; /**< rotation matrix that employs: measured Roll,
measured Pitch, desired Yaw */
measured Pitch, offset Yaw */
manif::SO3d m_offsetRotationCasted;
manif::SO3d m_measuredTilt;
manif::SE3d m_measuredFootPose; /**< the final foot pose matrix obtained through measured and
desired quantities */
manif::SE3d m_frame_H_link; /**< coordinate change matrix from foot link frame to foot sole frame
*/
offset quantities */
manif::SE3d m_T_walk;
manif::SE3d m_T_yawDrift;
double m_yawOld;
manif::SE3d m_footFrame_H_link_L; /**< coordinate change matrix from left foot link frame to
* left foot sole frame
*/
manif::SE3d m_footFrame_H_link_R; /**< coordinate change matrix from right foot link frame to
* right foot sole frame
*/
Eigen::Vector3d m_gravity;
const Eigen::Vector3d m_noTras{0.0, 0.0, 0.0};
iDynTree::KinDynComputations m_kinDyn;
iDynTree::Model m_model;
iDynTree::LinkIndex m_linkIndex;
int m_frameIndex;
std::string m_frameName;
int m_baseFrame; /**< Index of the frame whose pose needs to be estimated */
std::string m_footFrameName; /**< reference frames of the possible stance feet (whose
orientations are measured)*/
bool m_isOutputValid{false};
bool m_isModelSet{false};
iDynTree::LinkIndex m_stanceLinkIndex;

std::string m_baseFrameName; /**< Base link of the robot (whose pose must be estimated) */
int m_baseFrameIndex; /**< Index of the frame whose pose needs to be estimated */

std::string m_footFrameName_L; /**< reference frame of the left stance foot (whose
orientation is measured)*/
int m_footFrameIndex_L; /**< Index of the left foot frame */

std::string m_footFrameName_R; /**< reference frame of the right stance foot (whose
orientation is measured)*/
int m_footFrameIndex_R; /**< Index of the right foot frame */

bool m_isLastStanceFoot_L{false}; /**< true if the last stance foot was the left one */
bool m_isLastStanceFoot_R{false}; /**< true if the last stance foot was the right one */

bool m_isInitialized{false};
bool m_isInputSet{false};
bool m_isOutputValid{false};
};

} // namespace Estimators
Expand Down

0 comments on commit 56d3c4f

Please sign in to comment.