Merge pull request #213 from prashanthr05/pybind/contactDetectors

Add python bindings for SchmittTriggerDetector class
ami-iit · Feb 25, 2021 · aaa03cd · aaa03cd
2 parents cc2fd01 + 74a85fd
commit aaa03cd
Show file tree

Hide file tree

Showing 7 changed files with 191 additions and 0 deletions.
diff --git 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)

diff --git 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
@@ -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 <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#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_<Advanceable<EstimatedContactList>>(module, "EstimatedContactListAdvanceable");
+    py::class_<ContactDetector, Advanceable<EstimatedContactList>>(module, "ContactDetector");
+}
+
+void BipedalLocomotion::bindings::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 BipedalLocomotion::bindings::CreateSchmittTriggerDetector(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    using namespace BipedalLocomotion::Contacts;
+    using namespace BipedalLocomotion::System;
+    using namespace BipedalLocomotion::ParametersHandler;
+
+    py::class_<SchmittTriggerDetector, ContactDetector>(module, "SchmittTriggerDetector")
+        .def(py::init())
+        .def(
+            "initialize",
+            [](SchmittTriggerDetector& impl, std::shared_ptr<IParametersHandler> 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<const std::string&>(&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<const std::string&, const bool&, const SchmittTriggerParams&>(&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"), 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
@@ -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
@@ -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
@@ -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
@@ -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)