From 6d1aa9bdab854fac6d285e220c3bfe6091ab84b6 Mon Sep 17 00:00:00 2001 From: Prashanth Date: Thu, 25 Feb 2021 13:33:49 +0100 Subject: [PATCH 1/2] Add python bindings for contact detectors --- bindings/python/CMakeLists.txt | 2 + bindings/python/ContactDetectors.cpp | 97 +++++++++++++++++++ .../python/bipedal_locomotion_framework.cpp | 5 + .../python/bipedal_locomotion_framework.h | 5 + bindings/python/tests/pytest.ini | 1 + .../tests/test_schmitt_trigger_detector.py | 80 +++++++++++++++ 6 files changed, 190 insertions(+) create mode 100644 bindings/python/ContactDetectors.cpp create mode 100644 bindings/python/tests/test_schmitt_trigger_detector.py diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 541f4cc4ba..93260ec751 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -16,6 +16,7 @@ pybind11_add_module(pybind11_blf MODULE TimeVaryingDCMPlanner.cpp RobotInterface.cpp Constants.cpp + ContactDetectors.cpp ) target_link_libraries(pybind11_blf PUBLIC @@ -25,6 +26,7 @@ target_link_libraries(pybind11_blf PUBLIC BipedalLocomotion::RobotInterfaceYarpImplementation BipedalLocomotion::Contacts BipedalLocomotion::Math + BipedalLocomotion::ContactDetectors ) target_include_directories(pybind11_blf PRIVATE diff --git a/bindings/python/ContactDetectors.cpp b/bindings/python/ContactDetectors.cpp new file mode 100644 index 0000000000..fb5035964e --- /dev/null +++ b/bindings/python/ContactDetectors.cpp @@ -0,0 +1,97 @@ +/** + * @file ContactDetectors.cpp + * @authors Prashanth Ramadoss + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include +#include +#include + +#include "BipedalLocomotion/ContactDetectors/ContactDetector.h" +#include "BipedalLocomotion/ContactDetectors/SchmittTriggerDetector.h" +#include "bipedal_locomotion_framework.h" + + +void BipedalLocomotion::bindings::CreateContactDetector(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::Contacts; + using namespace BipedalLocomotion::System; + using namespace BipedalLocomotion::ParametersHandler; + + py::class_>(module, "EstimatedContactListAdvanceable"); + py::class_>(module, "ContactDetector"); +} + +void BipedalLocomotion::bindings::CreateSchmittTriggerUnit(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::Contacts; + + py::class_(module, "SchmittTriggerInput") + .def(py::init()) + .def_readwrite("time", &SchmittTriggerInput::time) + .def_readwrite("value", &SchmittTriggerInput::value); + + py::class_(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_(module, "SchmittTriggerUnit") + .def(py::init()) + .def("set_state", py::overload_cast(&SchmittTriggerUnit::setState), py::arg("state")) + .def("set_state", py::overload_cast(&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 BipedalLocomotion::bindings::CreateSchmittTriggerDetector(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::Contacts; + using namespace BipedalLocomotion::System; + using namespace BipedalLocomotion::ParametersHandler; + + py::class_(module, "SchmittTriggerDetector") + .def(py::init()) + .def( + "initialize", + [](SchmittTriggerDetector& impl, std::shared_ptr handler) -> bool { + return impl.initialize(handler); + }, + py::arg("handler")) + .def("advance", &SchmittTriggerDetector::advance) + .def("reset_contacts", &SchmittTriggerDetector::resetContacts) + .def("get", py::overload_cast<>(&SchmittTriggerDetector::get, py::const_)) + .def("get", py::overload_cast(&SchmittTriggerDetector::get, py::const_), + py::arg("contact_name")) + .def("is_valid", &SchmittTriggerDetector::isValid) + .def("set_timed_trigger_input", &SchmittTriggerDetector::setTimedTriggerInput, + py::arg("contact_name"), py::arg("time"), py::arg("trigger_input")) + .def("set_timed_trigger_inputs", &SchmittTriggerDetector::setTimedTriggerInputs, + py::arg("timed_inputs")) + .def("add_contact", py::overload_cast(&SchmittTriggerDetector::addContact), + py::arg("contact_name"), py::arg("initial_state"), py::arg("params")) + .def("add_contact", py::overload_cast(&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"), py::arg("state")) + .def("reset_contact", &SchmittTriggerDetector::resetContact, + py::arg("contact_name"), py::arg("state"), py::arg("params")) + .def("remove_contact", &SchmittTriggerDetector::removeContact, + py::arg("contact_name")); +} diff --git a/bindings/python/bipedal_locomotion_framework.cpp b/bindings/python/bipedal_locomotion_framework.cpp index f85e382305..39b8381c66 100644 --- a/bindings/python/bipedal_locomotion_framework.cpp +++ b/bindings/python/bipedal_locomotion_framework.cpp @@ -50,6 +50,11 @@ PYBIND11_MODULE(bindings, m) // Constants.cpp bindings::CreateConstants(m); + + // ContactDetectors.cpp + bindings::CreateContactDetector(m); + bindings::CreateSchmittTriggerUnit(m); + bindings::CreateSchmittTriggerDetector(m); } std::string bindings::ToString(const manif::SE3d& se3) diff --git a/bindings/python/bipedal_locomotion_framework.h b/bindings/python/bipedal_locomotion_framework.h index 2b625ad696..2ee87dec82 100644 --- a/bindings/python/bipedal_locomotion_framework.h +++ b/bindings/python/bipedal_locomotion_framework.h @@ -61,4 +61,9 @@ void CreateYarpSensorBridge(pybind11::module& module); // Constants.cpp void CreateConstants(pybind11::module& module); +// ContactDetectors.cpp +void CreateContactDetector(pybind11::module& module); +void CreateSchmittTriggerUnit(pybind11::module& module); +void CreateSchmittTriggerDetector(pybind11::module& module); + } // namespace BipedalLocomotion::bindings diff --git a/bindings/python/tests/pytest.ini b/bindings/python/tests/pytest.ini index 377e2bbac6..e3b41f23fc 100644 --- a/bindings/python/tests/pytest.ini +++ b/bindings/python/tests/pytest.ini @@ -3,3 +3,4 @@ addopts = -rsxX -v markers = planners: select the tests of BipedalLocomotion::Planners parameters_handler: select the tests of BipedalLocomotion::ParametersHandler + contact_detectors: select the tests of BipedalLocomotion::ContactDetectors diff --git a/bindings/python/tests/test_schmitt_trigger_detector.py b/bindings/python/tests/test_schmitt_trigger_detector.py new file mode 100644 index 0000000000..8eeb08b930 --- /dev/null +++ b/bindings/python/tests/test_schmitt_trigger_detector.py @@ -0,0 +1,80 @@ +import pytest +pytestmark = pytest.mark.contact_detectors + +import bipedal_locomotion_framework.bindings as blf + + +def test_schmitt_trigger_detector(): + + parameters_handler = blf.StdParametersHandler() + parameters_handler.set_parameter_vector_string("contacts", ["right"]) + parameters_handler.set_parameter_vector_float("contact_make_thresholds", [100.0]) + parameters_handler.set_parameter_vector_float("contact_break_thresholds", [10.0]) + parameters_handler.set_parameter_vector_float("contact_make_switch_times", [0.2]) + parameters_handler.set_parameter_vector_float("contact_break_switch_times", [0.2, 0.2]) + + detector = blf.SchmittTriggerDetector() + assert(detector.initialize(parameters_handler) == False) + + parameters_handler.set_parameter_vector_float("contact_break_switch_times", [0.2]) + assert(detector.initialize(parameters_handler)) + + assert(detector.reset_contacts()) + + # rise signal + assert(detector.set_timed_trigger_input("right", 0.1, 120.0)) + assert(detector.advance()) + assert(detector.set_timed_trigger_input("right", 0.2, 120.0)) + assert(detector.advance()) + assert(detector.set_timed_trigger_input("right", 0.3, 120.0)) + assert(detector.advance()) + + # contact state should turn true + right_contact = detector.get("right") + assert(right_contact.is_active) + assert(right_contact.switch_time == 0.3) + + # fall signal + assert(detector.set_timed_trigger_input("right", 0.4, 7.0)) + assert(detector.advance()) + assert(detector.set_timed_trigger_input("right", 0.5, 7.0)) + assert(detector.advance()) + assert(detector.set_timed_trigger_input("right", 0.6, 7.0)) + assert(detector.advance()) + + # contact state should turn false + right_contact = detector.get("right") + assert(right_contact.is_active == False) + assert(right_contact.switch_time == 0.6) + + # add a new contact + params = blf.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) + contacts = detector.get() + assert(len(contacts) == 2) + assert(contacts["right"].is_active == False) + + # test multiple measurement updates + right_input = blf.SchmittTriggerInput() + right_input.time = 0.7 + right_input.value = 120 + left_input = blf.SchmittTriggerInput() + left_input.time = 0.7 + left_input.value = 120 + timed_inputs = {"right":right_input, "left":left_input} + assert(detector.set_timed_trigger_inputs(timed_inputs)) + + + # test removing a contact + assert(detector.remove_contact("left")) + contacts = detector.get() + assert(len(contacts) == 1) + + # test resetting a contact + assert(detector.reset_contact("right", True, params)) + right_contact = detector.get("right") + assert(right_contact.is_active == True) From 74a85fd08c826b14f5b3f128a03aba11572934c1 Mon Sep 17 00:00:00 2001 From: Prashanth Date: Thu, 25 Feb 2021 15:22:27 +0100 Subject: [PATCH 2/2] Update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 938138cb0a..eaa496919f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,7 @@ All notable changes to this project are documented in this file. - Implement ISensorBridge python bindings (https://github.com/dic-iit/bipedal-locomotion-framework/pull/203) - Implement `LeggedOdometry` class as a part of `FloatingBaseEstimators` library and handle arbitrary contacts in `FloatingBaseEstimator`. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/151) - Implement the possibility to set a desired reference trajectory in the TimeVaryingDCMPlanner. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/208) +- Implement SchmittTriggerDetector python bindings (https://github.com/dic-iit/bipedal-locomotion-framework/pull/213) ### Changed - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204)