Skip to content

Commit

Permalink
Update ContactDetectors after the restructure of Contacts
Browse files Browse the repository at this point in the history
  • Loading branch information
@GiulioRomualdi
GiulioRomualdi committed Mar 21, 2023
1 parent 41f4bda commit 95a29b4
Show file tree
Hide file tree
Showing 6 changed files with 79 additions and 59 deletions.
17 changes: 10 additions & 7 deletions src/Contacts/include/BipedalLocomotion/ContactDetectors/FixedFootDetector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
#include <BipedalLocomotion/Contacts/Contact.h>
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>

#include <chrono>
#include <memory>

namespace BipedalLocomotion
Expand Down Expand Up @@ -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 */

/**
Expand All @@ -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<const ParametersHandler::IParametersHandler> handler) override;
Expand All @@ -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
Expand Down
3 changes: 2 additions & 1 deletion src/Contacts/src/ContactDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@

#include <BipedalLocomotion/ContactDetectors/ContactDetector.h>
#include <BipedalLocomotion/TextLogging/Logger.h>
#include <chrono>

using namespace BipedalLocomotion::ParametersHandler;
using namespace BipedalLocomotion::Contacts;
Expand All @@ -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;
Expand Down
6 changes: 4 additions & 2 deletions src/Contacts/src/FixedFootDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
#include <BipedalLocomotion/TextLogging/Logger.h>

#include <chrono>
#include <limits>
#include <memory>

Expand Down Expand Up @@ -39,7 +40,8 @@ bool FixedFootDetector::initialize(std::weak_ptr<const IParametersHandler> 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);
Expand Down Expand Up @@ -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;
}
Expand Down
13 changes: 8 additions & 5 deletions src/Contacts/src/SchmittTriggerDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <BipedalLocomotion/Math/SchmittTrigger.h>
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
#include <BipedalLocomotion/TextLogging/Logger.h>
#include <chrono>

namespace blf = BipedalLocomotion;
using namespace BipedalLocomotion::Contacts;
Expand Down Expand Up @@ -73,10 +74,10 @@ bool SchmittTriggerDetector::initialize(std::weak_ptr<const IParametersHandler>
std::vector<double> offThreshold;
ok = ok && setupParam("contact_break_thresholds", offThreshold);

std::vector<double> switchOnAfter;
std::vector<std::chrono::nanoseconds> switchOnAfter;
ok = ok && setupParam("contact_make_switch_times", switchOnAfter);

std::vector<double> switchOffAfter;
std::vector<std::chrono::nanoseconds> switchOffAfter;
ok = ok && setupParam("contact_break_switch_times", switchOffAfter);

if (!ok)
Expand All @@ -101,7 +102,9 @@ bool SchmittTriggerDetector::initialize(std::weak_ptr<const IParametersHandler>
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);
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
}
50 changes: 27 additions & 23 deletions src/Contacts/tests/ContactDetectors/FixedFootDetectorUnitTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include <BipedalLocomotion/ContactDetectors/FixedFootDetector.h>
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
#include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
#include <chrono>

using namespace BipedalLocomotion::ParametersHandler;
using namespace BipedalLocomotion::Contacts;
Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -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<StdImplementation>();
handler->setParameter("sampling_time", dT);
Expand All @@ -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);
Expand Down
Loading

0 comments on commit 95a29b4

Please sign in to comment.