From 7bdf6a2e10a103f1c8414ac877ff8a416657b3cd Mon Sep 17 00:00:00 2001 From: Prashanth Ramadoss Date: Wed, 31 Jan 2018 14:40:38 +0100 Subject: [PATCH 1/3] [contact switching] Added setPrimaryFoot method --- .../iDynTree/Estimation/BipedFootContactClassifier.h | 6 ++++++ src/estimation/src/BipedFootContactClassifier.cpp | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h b/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h index a2d503b8f41..9059f65a0eb 100644 --- a/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h +++ b/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h @@ -116,6 +116,12 @@ namespace iDynTree * @param pattern switching pattern */ void setContactSwitchingPattern(SwitchingPattern pattern) { m_pattern = pattern; } + + /** + * set primary foot + * @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..a2ae90ddc44 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) From 74f3a4c17f626a5cf5aad5fb1f609caf1e39ae3e Mon Sep 17 00:00:00 2001 From: Prashanth Ramadoss Date: Thu, 1 Feb 2018 11:19:11 +0100 Subject: [PATCH 2/3] [contact switching] changed alternate switching logic considering contact makes rather tan contacct breaks --- src/estimation/src/BipedFootContactClassifier.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/estimation/src/BipedFootContactClassifier.cpp b/src/estimation/src/BipedFootContactClassifier.cpp index a2ae90ddc44..a0b187fb36e 100644 --- a/src/estimation/src/BipedFootContactClassifier.cpp +++ b/src/estimation/src/BipedFootContactClassifier.cpp @@ -35,8 +35,9 @@ 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 ((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 +56,9 @@ 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 ((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; } From d113240f1455a6906f7e7637b17612ac5f8ca0fb Mon Sep 17 00:00:00 2001 From: Prashanth Ramadoss Date: Fri, 2 Feb 2018 11:28:37 +0100 Subject: [PATCH 3/3] [contact switching] Adding documentation for setPrimaryFoot --- .../include/iDynTree/Estimation/BipedFootContactClassifier.h | 4 ++++ src/estimation/src/BipedFootContactClassifier.cpp | 4 ---- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h b/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h index 9059f65a0eb..dc39dc47939 100644 --- a/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h +++ b/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h @@ -119,6 +119,10 @@ namespace iDynTree /** * 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; } diff --git a/src/estimation/src/BipedFootContactClassifier.cpp b/src/estimation/src/BipedFootContactClassifier.cpp index a0b187fb36e..74109eaaa13 100644 --- a/src/estimation/src/BipedFootContactClassifier.cpp +++ b/src/estimation/src/BipedFootContactClassifier.cpp @@ -35,8 +35,6 @@ 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; @@ -56,8 +54,6 @@ 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;