Skip to content

Commit

Permalink
Merge pull request #624 from ami-iit/contact_detector
Browse files Browse the repository at this point in the history
Restructure ContactDetector machinery
  • Loading branch information
GiulioRomualdi authored Mar 20, 2023
2 parents 11a4633 + 747fb92 commit 066cdb6
Show file tree
Hide file tree
Showing 25 changed files with 940 additions and 664 deletions.
5 changes: 5 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -4,10 +4,15 @@ All notable changes to this project are documented in this file.
## [unreleased]
### Added
- Implement the `DiscreteGeometryContact` in Contacts component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/626)
- Implement the `SchmittTrigger` in component `Math` and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/624)

### Changed
- Update the `IK tutorial` to use `QPInverseKinematics::build` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/621)
- Handle case where no FT sensors are specified to split the model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/625)
- General restructure of the `ContactDetector`and the derived classes (`SchmittTriggerDetector` and `FixedFootDetector`) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/624)
Thanks to this refactory the `FixedFootDetector` usage becomes similar to the others `advanceable`.
Indeed now `FixedFootDetector::advace()` considers the input set by the user and provides the corresponding output.
⚠️ Even if this modification do not break the API the user may notice some strange behavior if `advance` was called after getting the output of the detector.

### Fixed
- Return an error if an invalid `KinDynComputations` object is passed to `QPInverseKinematics::build()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/622)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
/**
* @file ContactDetectors.h
* @authors Giulio Romualdi
* @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* @copyright 2021-2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

Expand All @@ -18,7 +18,6 @@ namespace Contacts
{

void CreateContactDetector(pybind11::module& module);
void CreateSchmittTriggerUnit(pybind11::module& module);
void CreateSchmittTriggerDetector(pybind11::module& module);
void CreateFixedFootDetector(pybind11::module& module);

Expand Down
72 changes: 12 additions & 60 deletions bindings/python/Contacts/src/ContactDetectors.cpp
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
/**
* @file ContactDetectors.cpp
* @authors Prashanth Ramadoss
* @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* @copyright 2021-2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

Expand All @@ -12,6 +12,7 @@
#include <BipedalLocomotion/ContactDetectors/ContactDetector.h>
#include <BipedalLocomotion/ContactDetectors/FixedFootDetector.h>
#include <BipedalLocomotion/ContactDetectors/SchmittTriggerDetector.h>
#include <BipedalLocomotion/Math/SchmittTrigger.h>

#include <BipedalLocomotion/bindings/Contacts/ContactDetectors.h>
#include <BipedalLocomotion/bindings/System/Advanceable.h>
Expand All @@ -35,51 +36,11 @@ void CreateContactDetector(pybind11::module& module)
py::class_<ContactDetector, Source<EstimatedContactList>>(module, "ContactDetector");
}

void CreateSchmittTriggerUnit(pybind11::module& module)
{
namespace py = ::pybind11;
using namespace BipedalLocomotion::Contacts;

py::class_<SchmittTriggerInput>(module, "SchmittTriggerInput")
.def(py::init())
.def_readwrite("time", &SchmittTriggerInput::time)
.def_readwrite("value", &SchmittTriggerInput::value);

py::class_<SchmittTriggerParams>(module, "SchmittTriggerParams")
.def(py::init())
.def_readwrite("on_threshold", &SchmittTriggerParams::onThreshold)
.def_readwrite("off_threshold", &SchmittTriggerParams::offThreshold)
.def_readwrite("switch_on_after", &SchmittTriggerParams::switchOnAfter)
.def_readwrite("switch_off_after", &SchmittTriggerParams::switchOffAfter);

py::class_<SchmittTriggerUnit>(module, "SchmittTriggerUnit")
.def(py::init())
.def("set_state",
py::overload_cast<const bool&>(&SchmittTriggerUnit::setState),
py::arg("state"))
.def("set_state",
py::overload_cast<const bool&, const double&>(&SchmittTriggerUnit::setState),
py::arg("state"),
py::arg("time_now"))
.def("set_params", &SchmittTriggerUnit::setParams, py::arg("params"))
.def("update", &SchmittTriggerUnit::update, py::arg("current_time"), py::arg("raw_value"))
.def("reset", &SchmittTriggerUnit::reset)
.def("get_state", py::overload_cast<>(&SchmittTriggerUnit::getState))
.def("get_state_and_switch_time",
[](SchmittTriggerUnit& impl) {
double switchTime;
bool ok = impl.getState(switchTime);
return std::make_tuple(ok, switchTime);
})
.def("get_params", &SchmittTriggerUnit::getParams);
}

void CreateSchmittTriggerDetector(pybind11::module& module)
{
namespace py = ::pybind11;
using namespace BipedalLocomotion::Contacts;
using namespace BipedalLocomotion::System;
using namespace BipedalLocomotion::ParametersHandler;
using namespace BipedalLocomotion::Math;

py::class_<SchmittTriggerDetector, ContactDetector>(module, "SchmittTriggerDetector")
.def(py::init())
Expand All @@ -88,28 +49,20 @@ void CreateSchmittTriggerDetector(pybind11::module& module)
py::overload_cast<const std::string&>(&SchmittTriggerDetector::get, py::const_),
py::arg("contact_name"))
.def("set_timed_trigger_input",
&SchmittTriggerDetector::setTimedTriggerInput,
py::overload_cast<const std::string&, const SchmittTriggerInput&>(
&SchmittTriggerDetector::setTimedTriggerInput),
py::arg("contact_name"),
py::arg("time"),
py::arg("trigger_input"))
py::arg("input"))
.def("set_timed_trigger_inputs",
&SchmittTriggerDetector::setTimedTriggerInputs,
py::overload_cast<const std::unordered_map<std::string, //
Math::SchmittTriggerInput>&>(
&SchmittTriggerDetector::setTimedTriggerInputs),
py::arg("timed_inputs"))
.def("add_contact",
py::overload_cast<const std::string&, const bool&, const SchmittTriggerParams&>(
&SchmittTriggerDetector::addContact),
&SchmittTriggerDetector::addContact,
py::arg("contact_name"),
py::arg("initial_state"),
py::arg("params"))
.def("add_contact",
py::overload_cast<const std::string&,
const bool&,
const SchmittTriggerParams&,
const double&>(&SchmittTriggerDetector::addContact),
py::arg("contact_name"),
py::arg("initial_state"),
py::arg("params"),
py::arg("time_now"))
.def("reset_state",
&SchmittTriggerDetector::resetState,
py::arg("contact_name"),
Expand All @@ -126,15 +79,14 @@ void CreateFixedFootDetector(pybind11::module& module)
{
namespace py = ::pybind11;
using namespace BipedalLocomotion::Contacts;
using namespace BipedalLocomotion::System;
using namespace BipedalLocomotion::ParametersHandler;

py::class_<FixedFootDetector, ContactDetector>(module, "FixedFootDetector")
.def(py::init())
.def("get_fixed_foot", &FixedFootDetector::getFixedFoot)
.def("set_contact_phase_list",
&FixedFootDetector::setContactPhaseList,
py::arg("phase_list"));
py::arg("phase_list"))
.def("reset_time", &FixedFootDetector::resetTime, py::arg("time"));
}

} // namespace Contacts
Expand Down
1 change: 0 additions & 1 deletion bindings/python/Contacts/src/Module.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@ void CreateModule(pybind11::module& module)
CreateContactListJsonParser(module);

CreateContactDetector(module);
CreateSchmittTriggerUnit(module);
CreateSchmittTriggerDetector(module);
CreateFixedFootDetector(module);
}
Expand Down
37 changes: 21 additions & 16 deletions bindings/python/Contacts/tests/test_schmitt_trigger_detector.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,14 @@ def test_schmitt_trigger_detector():
assert(detector.reset_contacts())

# rise signal
assert(detector.set_timed_trigger_input("right", 0.1, 120.0))
assert(detector.set_timed_trigger_input("right",
blf.math.SchmittTriggerInput(time=0.1, raw_value=120.0)))
assert(detector.advance())
assert(detector.set_timed_trigger_input("right", 0.2, 120.0))
assert(detector.set_timed_trigger_input("right",
blf.math.SchmittTriggerInput(time=0.2, raw_value=120.0)))
assert(detector.advance())
assert(detector.set_timed_trigger_input("right", 0.3, 120.0))
assert(detector.set_timed_trigger_input("right",
blf.math.SchmittTriggerInput(time=0.3, raw_value=120.0)))
assert(detector.advance())

# contact state should turn true
Expand All @@ -35,11 +38,14 @@ def test_schmitt_trigger_detector():
assert(right_contact.switch_time == 0.3)

# fall signal
assert(detector.set_timed_trigger_input("right", 0.4, 7.0))
assert(detector.set_timed_trigger_input("right",
blf.math.SchmittTriggerInput(time=0.4, raw_value=7.0)))
assert(detector.advance())
assert(detector.set_timed_trigger_input("right", 0.5, 7.0))
assert(detector.set_timed_trigger_input("right",
blf.math.SchmittTriggerInput(time=0.5, raw_value=7.0)))
assert(detector.advance())
assert(detector.set_timed_trigger_input("right", 0.6, 7.0))
assert(detector.set_timed_trigger_input("right",
blf.math.SchmittTriggerInput(time=0.6, raw_value=7.0)))
assert(detector.advance())

# contact state should turn false
Expand All @@ -48,23 +54,22 @@ def test_schmitt_trigger_detector():
assert(right_contact.switch_time == 0.6)

# add a new contact
params = blf.contacts.SchmittTriggerParams()
params.off_threshold = 10
params.on_threshold = 100
params.switch_off_after = 0.2
params.switch_on_after = 0.2
detector.add_contact("left", False, params, 0.6)
params = blf.math.SchmittTrigger.Params(off_threshold=10, on_threshold=100,
switch_off_after=0.2, switch_on_after=0.2)
detector.add_contact("left",
blf.math.SchmittTriggerState(state=False, switch_time=0.6, edge_time=0.6),
params)
contacts = detector.get_output()
assert(len(contacts) == 2)
assert(contacts["right"].is_active == False)

# test multiple measurement updates
right_input = blf.contacts.SchmittTriggerInput()
right_input = blf.math.SchmittTriggerInput()
right_input.time = 0.7
right_input.value = 120
left_input = blf.contacts.SchmittTriggerInput()
right_input.raw_value = 120
left_input = blf.math.SchmittTriggerInput()
left_input.time = 0.7
left_input.value = 120
left_input.raw_value = 120
timed_inputs = {"right":right_input, "left":left_input}
assert(detector.set_timed_trigger_inputs(timed_inputs))

Expand Down
4 changes: 2 additions & 2 deletions bindings/python/Math/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ set(H_PREFIX include/BipedalLocomotion/bindings/Math)

add_bipedal_locomotion_python_module(
NAME MathBindings
SOURCES src/Constants.cpp src/Module.cpp
HEADERS ${H_PREFIX}/Constants.h ${H_PREFIX}/Module.h
SOURCES src/Constants.cpp src/SchmittTrigger.cpp src/Module.cpp
HEADERS ${H_PREFIX}/Constants.h ${H_PREFIX}/SchmittTrigger.h ${H_PREFIX}/Module.h
LINK_LIBRARIES BipedalLocomotion::Math
)
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
/**
* @file SchmittTrigger.h
* @authors Giulio Romualdi
* @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

#ifndef BIPEDAL_LOCOMOTION_BINDINGS_MATH_SCHMITT_TRIGGER_H
#define BIPEDAL_LOCOMOTION_BINDINGS_MATH_SCHMITT_TRIGGER_H

#include <pybind11/pybind11.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace Math
{

void CreateSchmittTrigger(pybind11::module& module);

} // namespace Math
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_BINDINGS_MATH_SCHMITT_TRIGGER_H
7 changes: 4 additions & 3 deletions bindings/python/Math/src/Module.cpp
Original file line number Diff line number Diff line change
@@ -1,15 +1,15 @@
/**
* @file Module.cpp
* @authors Giulio Romualdi
* @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* @copyright 2021-2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

#include <pybind11/pybind11.h>

#include <BipedalLocomotion/bindings/Math/Module.h>

#include <BipedalLocomotion/bindings/Math/Constants.h>
#include <BipedalLocomotion/bindings/Math/Module.h>
#include <BipedalLocomotion/bindings/Math/SchmittTrigger.h>

namespace BipedalLocomotion
{
Expand All @@ -22,6 +22,7 @@ void CreateModule(pybind11::module& module)
module.doc() = "Math module contains the bindings for BipedalLocomotion::Math";

CreateConstants(module);
CreateSchmittTrigger(module);
}
} // namespace Math
} // namespace bindings
Expand Down
92 changes: 92 additions & 0 deletions bindings/python/Math/src/SchmittTrigger.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
/**
* @file SchmittTrigger.cpp
* @authors Giulio Romualdi
* @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

#include <pybind11/pybind11.h>

#include <BipedalLocomotion/Math/SchmittTrigger.h>
#include <BipedalLocomotion/System/Advanceable.h>
#include <BipedalLocomotion/bindings/Math/SchmittTrigger.h>
#include <BipedalLocomotion/bindings/System/Advanceable.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace Math
{

void CreateSchmittTrigger(pybind11::module& module)
{
using namespace BipedalLocomotion::Math;
namespace py = ::pybind11;

py::class_<SchmittTriggerState>(module, "SchmittTriggerState")
.def(py::init([](bool state, double switchTime, double edgeTime) -> SchmittTriggerState {
return SchmittTriggerState{std::move(state),
std::move(switchTime),
std::move(edgeTime)};
}),
py::arg("state") = false,
py::arg("switch_time") = 0.0,
py::arg("edge_time") = 0.0)
.def_readwrite("state", &SchmittTriggerState::state)
.def_readwrite("switch_time", &SchmittTriggerState::switchTime)
.def_readwrite("edge_time", &SchmittTriggerState::edgeTime);

py::class_<SchmittTriggerInput>(module, "SchmittTriggerInput")
.def(py::init([](double time, double rawValue) -> SchmittTriggerInput {
return SchmittTriggerInput{std::move(time), std::move(rawValue)};
}),
py::arg("time") = 0.0,
py::arg("raw_value") = 0.0)
.def_readwrite("time", &SchmittTriggerInput::time)
.def_readwrite("raw_value", &SchmittTriggerInput::rawValue);

BipedalLocomotion::bindings::System::CreateAdvanceable<
BipedalLocomotion::Math::SchmittTriggerInput,
BipedalLocomotion::Math::SchmittTriggerState>(module, "SchmittTrigger");

py::class_<SchmittTrigger,
::BipedalLocomotion::System::Advanceable<SchmittTriggerInput, //
SchmittTriggerState>>
schmittTrigger(module, "SchmittTrigger");

py::class_<SchmittTrigger::Params>(schmittTrigger, "Params")
.def(py::init([](double onThreshold,
double offThreshold,
double switchOnAfter,
double switchOffAfter,
double timeComparisonThreshold) -> SchmittTrigger::Params {
SchmittTrigger::Params params;
params.onThreshold = std::move(onThreshold);
params.offThreshold = std::move(offThreshold);
params.switchOnAfter = std::move(switchOnAfter);
params.switchOffAfter = std::move(switchOffAfter);
params.timeComparisonThreshold = std::move(timeComparisonThreshold);
return params;
}),
py::arg("on_threshold") = 0.0,
py::arg("off_threshold") = 0.0,
py::arg("switch_on_after") = 0.0,
py::arg("switch_off_after") = 0.0,
py::arg("time_comparison_threshold") = std::numeric_limits<double>::epsilon())
.def_readwrite("on_threshold", &SchmittTrigger::Params::onThreshold)
.def_readwrite("off_threshold", &SchmittTrigger::Params::offThreshold)
.def_readwrite("switch_on_after", &SchmittTrigger::Params::switchOnAfter)
.def_readwrite("switch_off_after", &SchmittTrigger::Params::switchOffAfter)
.def_readwrite("time_comparison_threshold",
&SchmittTrigger::Params::timeComparisonThreshold);

schmittTrigger.def(py::init())
.def("set_state", &SchmittTrigger::setState, py::arg("state"))
.def("initialize",
py::overload_cast<const SchmittTrigger::Params&>(&SchmittTrigger::initialize),
py::arg("parameters"));
}
} // namespace Math
} // namespace bindings
} // namespace BipedalLocomotion
Loading

0 comments on commit 066cdb6

Please sign in to comment.