From 95a29b4089a34aa294a2107f867ac1db20b78059 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 17 Mar 2023 23:59:23 +0100 Subject: [PATCH] Update ContactDetectors after the restructure of Contacts --- .../ContactDetectors/FixedFootDetector.h | 17 ++++--- src/Contacts/src/ContactDetector.cpp | 3 +- src/Contacts/src/FixedFootDetector.cpp | 6 ++- src/Contacts/src/SchmittTriggerDetector.cpp | 13 +++-- .../FixedFootDetectorUnitTest.cpp | 50 ++++++++++--------- .../SchmittTriggerDetectorUnitTest.cpp | 49 ++++++++++-------- 6 files changed, 79 insertions(+), 59 deletions(-) diff --git a/src/Contacts/include/BipedalLocomotion/ContactDetectors/FixedFootDetector.h b/src/Contacts/include/BipedalLocomotion/ContactDetectors/FixedFootDetector.h index 99e2b990fd..7c0d2dbf97 100644 --- a/src/Contacts/include/BipedalLocomotion/ContactDetectors/FixedFootDetector.h +++ b/src/Contacts/include/BipedalLocomotion/ContactDetectors/FixedFootDetector.h @@ -13,6 +13,7 @@ #include #include +#include #include namespace BipedalLocomotion @@ -67,8 +68,10 @@ namespace Contacts class FixedFootDetector : public ContactDetector { ContactPhaseList m_contactPhaselist; /**< List of the contacts */ - double m_currentTime{0}; /**< Current time in seconds */ - double m_dT{0}; /**< Fixed sampling time in seconds */ + std::chrono::nanoseconds m_currentTime{std::chrono::nanoseconds::zero()}; /**< Current time in + seconds */ + std::chrono::nanoseconds m_dT{std::chrono::nanoseconds::zero()}; /**< Fixed sampling time in + seconds */ EstimatedContact m_dummyContact; /**< A dummy esitmated contact */ /** @@ -83,9 +86,9 @@ class FixedFootDetector : public ContactDetector * Initialize the detector. * @param handler pointer to the parameter handler. * @note the following parameters are required by the class - * | Parameter Name | Type | Description | Mandatory | - * |:-----------------:|:----------:|:------------------------------------------:|:---------:| - * | `sampling_time` | `double` | Sampling time of the detector is seconds | Yes | + * | Parameter Name | Type | Description | Mandatory | + * |:-----------------:|:---------------------:|:------------------------------------------:|:---------:| + * | `sampling_time` | `chrono::nanoseconds` | Sampling time of the detector is seconds | Yes | * @return true in case of success/false otherwise. */ bool initialize(std::weak_ptr handler) override; @@ -104,9 +107,9 @@ class FixedFootDetector : public ContactDetector /** * Reset the time - * @param time the time in seconds + * @param time time */ - void resetTime(const double& time); + void resetTime(const std::chrono::nanoseconds& time); /** * Get the fixed foot diff --git a/src/Contacts/src/ContactDetector.cpp b/src/Contacts/src/ContactDetector.cpp index fbcf9b48eb..12adacb185 100644 --- a/src/Contacts/src/ContactDetector.cpp +++ b/src/Contacts/src/ContactDetector.cpp @@ -7,6 +7,7 @@ #include #include +#include using namespace BipedalLocomotion::ParametersHandler; using namespace BipedalLocomotion::Contacts; @@ -15,7 +16,7 @@ bool ContactDetector::resetContacts() { for (auto& [name, contact] : m_contactStates) { - contact.switchTime = 0.0; + contact.switchTime = std::chrono::nanoseconds::zero(); contact.isActive = false; } return true; diff --git a/src/Contacts/src/FixedFootDetector.cpp b/src/Contacts/src/FixedFootDetector.cpp index 8ba1ff77fc..30ff8847a0 100644 --- a/src/Contacts/src/FixedFootDetector.cpp +++ b/src/Contacts/src/FixedFootDetector.cpp @@ -10,6 +10,7 @@ #include #include +#include #include #include @@ -39,7 +40,8 @@ bool FixedFootDetector::initialize(std::weak_ptr handl return false; } - if (m_dT <= 0) + // This should never happen + if (m_dT <= std::chrono::nanoseconds::zero()) { log()->error("{} The parameter 'sampling_time' must be a strictly positive number.", logPrefix); @@ -243,7 +245,7 @@ void FixedFootDetector::setContactPhaseList(const ContactPhaseList& phaseList) } } -void FixedFootDetector::resetTime(const double &time) +void FixedFootDetector::resetTime(const std::chrono::nanoseconds &time) { m_currentTime = time; } diff --git a/src/Contacts/src/SchmittTriggerDetector.cpp b/src/Contacts/src/SchmittTriggerDetector.cpp index 8997ec99bc..1c58ddde5b 100644 --- a/src/Contacts/src/SchmittTriggerDetector.cpp +++ b/src/Contacts/src/SchmittTriggerDetector.cpp @@ -10,6 +10,7 @@ #include #include #include +#include namespace blf = BipedalLocomotion; using namespace BipedalLocomotion::Contacts; @@ -73,10 +74,10 @@ bool SchmittTriggerDetector::initialize(std::weak_ptr std::vector offThreshold; ok = ok && setupParam("contact_break_thresholds", offThreshold); - std::vector switchOnAfter; + std::vector switchOnAfter; ok = ok && setupParam("contact_make_switch_times", switchOnAfter); - std::vector switchOffAfter; + std::vector switchOffAfter; ok = ok && setupParam("contact_break_switch_times", switchOffAfter); if (!ok) @@ -101,7 +102,9 @@ bool SchmittTriggerDetector::initialize(std::weak_ptr params.offThreshold = offThreshold[idx]; // set the initial state for the trigger - constexpr blf::Math::SchmittTriggerState initialState{false, 0, 0}; + constexpr blf::Math::SchmittTriggerState initialState{false, + std::chrono::nanoseconds::zero(), + std::chrono::nanoseconds::zero()}; if (!this->addContact(contacts[idx], initialState, params)) { log()->error("{} Could not add Schmitt Trigger unit for specified contact.", logPrefix); @@ -262,7 +265,7 @@ bool SchmittTriggerDetector::resetContact(const std::string& contactName, triggerState.state = state; trigger.setState(triggerState); m_contactStates.at(contactName).isActive = state; - m_contactStates.at(contactName).switchTime = 0.0; + m_contactStates.at(contactName).switchTime = std::chrono::nanoseconds::zero(); return true; } @@ -280,7 +283,7 @@ bool SchmittTriggerDetector::resetState(const std::string& contactName, const bo triggerState.state = state; m_pimpl->manager.at(contactName).setState(triggerState); m_contactStates.at(contactName).isActive = state; - m_contactStates.at(contactName).switchTime = 0.0; + m_contactStates.at(contactName).switchTime = std::chrono::nanoseconds::zero(); return true; } diff --git a/src/Contacts/tests/ContactDetectors/FixedFootDetectorUnitTest.cpp b/src/Contacts/tests/ContactDetectors/FixedFootDetectorUnitTest.cpp index 822c668be2..961d1ddba4 100644 --- a/src/Contacts/tests/ContactDetectors/FixedFootDetectorUnitTest.cpp +++ b/src/Contacts/tests/ContactDetectors/FixedFootDetectorUnitTest.cpp @@ -12,6 +12,7 @@ #include #include #include +#include using namespace BipedalLocomotion::ParametersHandler; using namespace BipedalLocomotion::Contacts; @@ -22,46 +23,48 @@ struct FixedFootState EstimatedContact rightFoot; }; -FixedFootState getFixedFootState(double t, const ContactListMap& listMap) +FixedFootState getFixedFootState(const std::chrono::nanoseconds& t, const ContactListMap& listMap) { // t 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 // L |+++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++| // R |+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++| // stance foot |LLL|RRR|RRR|LLL|LLL|RRR|RRR|LLL|LLL|RRR|RRR|LLL|LLL|RRR|RRR|LLL|LLL| + using namespace std::chrono_literals; + FixedFootState state; state.leftFoot.pose = listMap.find("left_foot")->second.getPresentContact(t)->pose; state.rightFoot.pose = listMap.find("right_foot")->second.getPresentContact(t)->pose; - if (t < 1) + if (t < 1s) { state.leftFoot.isActive = true; state.rightFoot.isActive = false; - } else if (t < 3) + } else if (t < 3s) { state.leftFoot.isActive = false; state.rightFoot.isActive = true; - } else if (t < 5) + } else if (t < 5s) { state.leftFoot.isActive = true; state.rightFoot.isActive = false; - } else if (t < 7) + } else if (t < 7s) { state.leftFoot.isActive = false; state.rightFoot.isActive = true; - } else if (t < 9) + } else if (t < 9s) { state.leftFoot.isActive = true; state.rightFoot.isActive = false; - } else if (t < 11) + } else if (t < 11s) { state.leftFoot.isActive = false; state.rightFoot.isActive = true; - } else if (t < 13) + } else if (t < 13s) { state.leftFoot.isActive = true; state.rightFoot.isActive = false; - } else if (t < 15) + } else if (t < 15s) { state.leftFoot.isActive = false; state.rightFoot.isActive = true; @@ -82,49 +85,50 @@ ContactPhaseList createContactList() // stance foot |LLL|RRR|RRR|LLL|LLL|RRR|RRR|LLL|LLL|RRR|RRR|LLL|LLL|RRR|RRR|LLL|LLL| ContactListMap contactListMap; + using namespace std::chrono_literals; Eigen::Vector3d leftPosition = Eigen::Vector3d::Zero(); manif::SE3d leftTransform(leftPosition, manif::SO3d::Identity()); - contactListMap["left_foot"].addContact(leftTransform, 0.0, 1.0); + contactListMap["left_foot"].addContact(leftTransform, 0s, 1s); leftPosition(0) += 0.05; leftTransform.translation(leftPosition); - contactListMap["left_foot"].addContact(leftTransform, 2.0, 5.0); + contactListMap["left_foot"].addContact(leftTransform, 2s, 5s); leftPosition(0) += 0.1; leftTransform.translation(leftPosition); - contactListMap["left_foot"].addContact(leftTransform, 6.0, 9.0); + contactListMap["left_foot"].addContact(leftTransform, 6s, 9s); leftPosition(0) += 0.1; leftTransform.translation(leftPosition); - contactListMap["left_foot"].addContact(leftTransform, 10.0, 13.0); + contactListMap["left_foot"].addContact(leftTransform, 10s, 13s); leftPosition(0) += 0.1; leftTransform.translation(leftPosition); - contactListMap["left_foot"].addContact(leftTransform, 14.0, 17.0); + contactListMap["left_foot"].addContact(leftTransform, 14s, 17s); // right foot // first footstep Eigen::Vector3d rightPosition = Eigen::Vector3d::Zero(); manif::SE3d rightTransform(rightPosition, manif::SO3d::Identity()); - contactListMap["right_foot"].addContact(rightTransform, 0.0, 3.0); + contactListMap["right_foot"].addContact(rightTransform, 0s, 3s); rightPosition(0) += 0.1; rightTransform.translation(rightPosition); - contactListMap["right_foot"].addContact(rightTransform, 4.0, 7.0); + contactListMap["right_foot"].addContact(rightTransform, 4s, 7s); rightPosition(0) += 0.1; rightTransform.translation(rightPosition); - contactListMap["right_foot"].addContact(rightTransform, 8.0, 11.0); + contactListMap["right_foot"].addContact(rightTransform, 8s, 11s); rightPosition(0) += 0.1; rightTransform.translation(rightPosition); - contactListMap["right_foot"].addContact(rightTransform, 12.0, 15.0); + contactListMap["right_foot"].addContact(rightTransform, 12s, 15s); rightPosition(0) += 0.05; rightTransform.translation(rightPosition); - contactListMap["right_foot"].addContact(rightTransform, 16.0, 17.0); + contactListMap["right_foot"].addContact(rightTransform, 16s, 17s); ContactPhaseList phaseList; phaseList.setLists(contactListMap); @@ -133,8 +137,9 @@ ContactPhaseList createContactList() TEST_CASE("Fixed Foot Detector") { - constexpr auto dT = 0.01; - constexpr auto horizon = 20.0; + using namespace std::chrono_literals; + constexpr std::chrono::nanoseconds dT = 10ms; + constexpr std::chrono::nanoseconds horizon = 20s; FixedFootDetector detector; auto handler = std::make_shared(); handler->setParameter("sampling_time", dT); @@ -143,12 +148,11 @@ TEST_CASE("Fixed Foot Detector") const auto phaseList = createContactList(); detector.setContactPhaseList(phaseList); - for (int i = 0; i < horizon / dT; i++) + for (std::chrono::nanoseconds currentTime = 0s; currentTime < horizon; currentTime += dT) { // advance is used to advance the time stored in the detector and to evaluate the outputs REQUIRE(detector.advance()); - const double currentTime = phaseList.firstPhase()->beginTime + i * dT; auto state = getFixedFootState(currentTime, phaseList.lists()); REQUIRE(detector.getOutput().find("right_foot")->second.isActive == state.rightFoot.isActive); diff --git a/src/Contacts/tests/ContactDetectors/SchmittTriggerDetectorUnitTest.cpp b/src/Contacts/tests/ContactDetectors/SchmittTriggerDetectorUnitTest.cpp index 9058da4fa9..b4ba08eec1 100644 --- a/src/Contacts/tests/ContactDetectors/SchmittTriggerDetectorUnitTest.cpp +++ b/src/Contacts/tests/ContactDetectors/SchmittTriggerDetectorUnitTest.cpp @@ -5,6 +5,8 @@ * distributed under the terms of the BSD-3-Clause license. */ +#include + #include #include @@ -20,85 +22,90 @@ TEST_CASE("Schmitt Trigger Detector") { SchmittTriggerDetector detector; + using namespace std::chrono_literals; + std::shared_ptr originalHandler = std::make_shared(); IParametersHandler::shared_ptr parameterHandler = originalHandler; parameterHandler->setParameter("contacts", std::vector{"right"}); parameterHandler->setParameter("contact_make_thresholds", std::vector{100}); parameterHandler->setParameter("contact_break_thresholds", std::vector{10}); - parameterHandler->setParameter("contact_make_switch_times", std::vector{0.2}); - parameterHandler->setParameter("contact_break_switch_times", std::vector{0.2, 0.2}); + parameterHandler->setParameter("contact_make_switch_times", + std::vector{200ms}); + parameterHandler->setParameter("contact_break_switch_times", + std::vector{200ms, 200ms}); // mismatch in parameter size shoulc cause initialization failure REQUIRE_FALSE(detector.initialize(parameterHandler)); // initialization should pass - parameterHandler->setParameter("contact_break_switch_times", std::vector{0.2}); + parameterHandler->setParameter("contact_break_switch_times", + std::vector{200ms}); REQUIRE(detector.initialize(parameterHandler)); // set all contacts to false REQUIRE(detector.resetContacts()); // rise signal - detector.setTimedTriggerInput("right", {0.1 ,120}); + detector.setTimedTriggerInput("right", {100ms ,120}); detector.advance(); - detector.setTimedTriggerInput("right", {0.2, 120}); + detector.setTimedTriggerInput("right", {200ms, 120}); detector.advance(); - detector.setTimedTriggerInput("right", {0.3, 120}); + detector.setTimedTriggerInput("right", {300ms, 120}); detector.advance(); // contact state should turn true EstimatedContact rightContact; REQUIRE(detector.get("right", rightContact)); REQUIRE(rightContact.isActive); - REQUIRE(rightContact.switchTime == 0.3); + REQUIRE(rightContact.switchTime == 300ms); // fall signal - detector.setTimedTriggerInput("right", {0.4, 7}); + detector.setTimedTriggerInput("right", {400ms, 7}); detector.advance(); - detector.setTimedTriggerInput("right", {0.5, 7}); + detector.setTimedTriggerInput("right", {500ms, 7}); detector.advance(); - detector.setTimedTriggerInput("right", {0.6, 7}); + detector.setTimedTriggerInput("right", {600ms, 7}); detector.advance(); // contact state should turn false REQUIRE(detector.get("right", rightContact)); REQUIRE(!rightContact.isActive); - REQUIRE(rightContact.switchTime == 0.6); + REQUIRE(rightContact.switchTime == 600ms); // add a new contact EstimatedContact newContact; SchmittTrigger::Params params; params.offThreshold = 10; params.onThreshold = 100; - params.switchOffAfter = 0.2; - params.switchOnAfter = 0.2; + params.switchOffAfter = 200ms; + params.switchOnAfter = 200ms; - detector.addContact("left", {false, 0.6, 0.6}, params); + detector.addContact("left", {false, 600ms, 600ms}, params); auto contacts = detector.getOutput(); REQUIRE(contacts.size() == 2); // test multiple measurement updates std::unordered_map timedForces; - timedForces["right"] = {0.7, 120}; - timedForces["left"] = {0.7, 120}; + timedForces["right"] = {700ms, 120}; + timedForces["left"] = {700ms, 120}; detector.setTimedTriggerInputs(timedForces); detector.advance(); - timedForces["right"] = {0.8, 120}; - timedForces["left"] = {0.8, 120}; + timedForces["right"] = {800ms, 120}; + timedForces["left"] = {800ms, 120}; detector.setTimedTriggerInputs(timedForces); detector.advance(); - timedForces["right"] = {0.9, 120}; - timedForces["left"] = {0.9, 120}; + timedForces["right"] = {900ms, 120}; + timedForces["left"] = {900ms, 120}; detector.setTimedTriggerInputs(timedForces); detector.advance(); contacts = detector.getOutput(); REQUIRE(contacts["right"].isActive); REQUIRE(contacts["left"].isActive); - REQUIRE(contacts["right"].switchTime == 0.9); + REQUIRE(contacts["right"].switchTime == 900ms); // Test removing contact REQUIRE(detector.removeContact("left"));