diff --git a/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h b/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h index a2d503b8f41..dc39dc47939 100644 --- a/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h +++ b/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h @@ -116,6 +116,16 @@ namespace iDynTree * @param pattern switching pattern */ void setContactSwitchingPattern(SwitchingPattern pattern) { m_pattern = pattern; } + + /** + * set primary foot + * This method was mainly intended to be called by an external process + * to set the primary foot in the initial setting, before any contact is broken. + * In case it is set to UNKNOWN_FOOT, it waits for the foot normal force measurements (checks left first and then right), + * and activates corresponding foot, handled in the detectTransitions() method. + * @param foot primary foot + */ + void setPrimaryFoot(contactFoot foot) { m_primaryFoot = foot; } // unique pointer to contact state machine for left foot std::unique_ptr m_leftFootContactClassifier; diff --git a/src/estimation/src/BipedFootContactClassifier.cpp b/src/estimation/src/BipedFootContactClassifier.cpp index c78ec9348f7..74109eaaa13 100644 --- a/src/estimation/src/BipedFootContactClassifier.cpp +++ b/src/estimation/src/BipedFootContactClassifier.cpp @@ -4,7 +4,7 @@ namespace iDynTree { BipedFootContactClassifier::BipedFootContactClassifier(const SchmittParams& leftFootSchmittParams, - const SchmittParams& rightFootSchmittParams) : m_primaryFoot(LEFT_FOOT), + const SchmittParams& rightFootSchmittParams) : m_primaryFoot(RIGHT_FOOT), m_leftFootContactState(true), m_rightFootContactState(true), m_pattern(ALTERNATE_CONTACT) @@ -35,8 +35,7 @@ void BipedFootContactClassifier::detectFeetTransition() case SwitchingPattern::ALTERNATE_CONTACT: if (m_primaryFoot == LEFT_FOOT) { - if ((leftFootTransition == ContactStateMachine::CONTACT_BREAK && rightFootTransition == ContactStateMachine::CONTACT_MAKE) - || (leftFootTransition == ContactStateMachine::CONTACT_BREAK && rightFootTransition == ContactStateMachine::STABLE_ONCONTACT)) + if ( rightFootTransition == ContactStateMachine::CONTACT_MAKE && m_leftFootContactState == true) { m_primaryFoot = RIGHT_FOOT; } @@ -55,8 +54,7 @@ void BipedFootContactClassifier::detectFeetTransition() } else if (m_primaryFoot == RIGHT_FOOT) { - if ((rightFootTransition == ContactStateMachine::CONTACT_BREAK && leftFootTransition == ContactStateMachine::CONTACT_MAKE) - || (rightFootTransition == ContactStateMachine::CONTACT_BREAK && leftFootTransition == ContactStateMachine::STABLE_ONCONTACT)) + if ( leftFootTransition == ContactStateMachine::CONTACT_MAKE && m_rightFootContactState == true) { m_primaryFoot = LEFT_FOOT; }