Python bindings for : Centroidal MPC, Centroidal Dynamics and Contacts (

#650)

CHANGELOG.md

@@ -22,6 +22,7 @@ All notable changes to this project are documented in this file.
 - Implement `MANNAutoregressiveInputBuilder` in `ML` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/689)
 - Implement the `CentroidalMPC` in `ReducedModelControllers`component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/645)
 - Implement the `BaseEstimatorFromFootIMU` in the `Estimators` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/641)
+- Add bindings for `CentroidalMPC`, `CentroidalDynamics`, `ContactPhaseList`, `DiscreteGeometryContact` and `Corner` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/650)
 
 ### Changed
 - Restructure application folders of `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/670)

bindings/python/CMakeLists.txt

@@ -16,6 +16,7 @@ add_subdirectory(TextLogging)
 add_subdirectory(Conversions)
 add_subdirectory(YarpUtilities)
 add_subdirectory(ContinuousDynamicalSystem)
+add_subdirectory(ReducedModelControllers)
 add_subdirectory(ML)
 
 include(ConfigureFileWithCMakeIf)

bindings/python/Contacts/src/Contacts.cpp

@@ -10,8 +10,8 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
-#include <iomanip>
 #include <chrono>
+#include <iomanip>
 
 #include <BipedalLocomotion/Contacts/Contact.h>
 #include <BipedalLocomotion/Contacts/ContactList.h>
@@ -86,6 +86,13 @@ void CreateContact(pybind11::module& module)
     py::class_<ContactWrench, ContactBase>(module, "ContactWrench")
         .def(py::init())
         .def_readwrite("wrench", &ContactWrench::wrench);
+    py::class_<DiscreteGeometryContact>(module, "DiscreteGeometryContact")
+        .def(py::init())
+        .def_readwrite("corners", &DiscreteGeometryContact::corners);
+    py::class_<Corner>(module, "Corner")
+        .def(py::init())
+        .def_readwrite("position", &Corner::position)
+        .def_readwrite("force", &Corner::force);
 }
 
 void CreateContactList(pybind11::module& module)
@@ -130,7 +137,8 @@ void CreateContactList(pybind11::module& module)
         .def("size", &ContactList::size)
         .def("__len__", &ContactList::size)
         .def("edit_contact", &ContactList::editContact, py::arg("element"), py::arg("new_contact"))
-        .def("get_present_contact",
+        .def(
+            "get_present_contact",
             [](const ContactList& l, const std::chrono::nanoseconds& time) -> PlannedContact {
                 return *l.getPresentContact(time);
             },
@@ -187,10 +195,29 @@ void CreateContactPhaseList(pybind11::module& module)
     using namespace BipedalLocomotion::Contacts;
     py::class_<ContactPhaseList>(module, "ContactPhaseList")
         .def(py::init())
+        .def("size", &ContactPhaseList::size)
+        .def("__getitem__", &ContactPhaseList::operator[])
+        .def(
+            "last_phase",
+            [](const ContactPhaseList& impl) -> const ContactPhase& { return *impl.lastPhase(); },
+            py::return_value_policy::reference_internal)
+        .def(
+            "first_phase",
+            [](const ContactPhaseList& impl) -> const ContactPhase& { return *impl.firstPhase(); },
+            py::return_value_policy::reference_internal)
         .def("set_lists",
              py::overload_cast<const ContactListMap&>(&ContactPhaseList::setLists),
              py::arg("contact_lists"))
-        .def("lists", &ContactPhaseList::lists);
+        .def("lists", &ContactPhaseList::lists)
+        .def(
+            "get_present_phase",
+            [](const ContactPhaseList& l, const std::chrono::nanoseconds& time)
+                -> const ContactPhase& { return *l.getPresentPhase(time); },
+            py::arg("time"),
+            py::return_value_policy::reference_internal)
+        .def("__iter__",
+             [](const ContactPhaseList& l) { return py::make_iterator(l.cbegin(), l.cend()); });
+    ;
 }
 
 void CreateContactListJsonParser(pybind11::module& module)

bindings/python/Contacts/tests/test_contact.py

@@ -1,5 +1,5 @@
 import pytest
-pytestmark = pytest.mark.planners
+pytestmark = pytest.mark.contact
 
 import bipedal_locomotion_framework.bindings as blf
 import manifpy as manif
@@ -125,3 +125,58 @@ def test_contact_phase():
     list2.set_default_name(default_name="List2")
 
     # TODO: the active_contacts is a read_only attribute
+
+def test_contact_phase_list():
+
+    contact_list_left_foot = blf.contacts.ContactList()
+    contact = blf.contacts.PlannedContact()
+    leftPosition = np.zeros(3)
+    quaternion = [0.0, 0.0, 0.0, 1.0]
+    contact.pose = manif.SE3(position=leftPosition, quaternion=quaternion)
+    contact.activation_time = timedelta(seconds=0.0)
+    contact.deactivation_time = timedelta(seconds=1.0)
+    contact.name = "contactLeft1"
+    contact_list_left_foot.add_contact(contact)
+    contact.activation_time = timedelta(seconds=2.0)
+    contact.deactivation_time = timedelta(seconds=4.0)
+    contact_list_left_foot.add_contact(contact)
+
+    # Right Foot
+    contact_list_right_foot = blf.contacts.ContactList()
+    contact = blf.contacts.PlannedContact()
+    rightPosition = np.zeros(3)
+    contact.pose = manif.SE3(position = rightPosition, quaternion = quaternion)
+    contact.activation_time = timedelta(seconds=0.0)
+    contact.deactivation_time = timedelta(seconds=3.0)
+    contact.name = "contactRight1"
+    contact_list_right_foot.add_contact(contact)
+    contact.activation_time = timedelta(seconds=4.0)
+    contact.deactivation_time = timedelta(seconds=6.0)
+    contact_list_right_foot.add_contact(contact)
+
+    contact_list_map ={}
+    contact_list_map.update({"left_foot":contact_list_left_foot})
+    contact_list_map.update({"right_foot":contact_list_right_foot})
+    contact_phase_list = blf.contacts.ContactPhaseList()
+
+    contact_phase_list.set_lists(contact_list_map)
+
+    c_1 = contact_phase_list.get_present_phase(timedelta(seconds=50.0))
+    c_2 = contact_phase_list.last_phase()
+    assert c_1 == c_2
+
+    c_3 = contact_phase_list.get_present_phase(timedelta(seconds=0.5))
+    c_4 = contact_phase_list.first_phase()
+
+    assert (c_3 == c_4)
+
+    assert contact_phase_list.size() == 5
+    i = 0
+    for item in contact_phase_list:
+        i  = i + 1
+        if(i == 1):
+            assert item == contact_phase_list.first_phase()
+        if( i == contact_phase_list.size()):
+            assert item == contact_phase_list.last_phase()
+
+

bindings/python/ContinuousDynamicalSystem/CMakeLists.txt

@@ -6,11 +6,11 @@ if(FRAMEWORK_COMPILE_ContinuousDynamicalSystem)
 
   add_bipedal_locomotion_python_module(
     NAME ContinuousDynamicalSystemBindings
-    SOURCES src/MultiStateWeightProvider.cpp src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/Module.cpp
+    SOURCES src/MultiStateWeightProvider.cpp src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/CentroidalDynamics.cpp src/Module.cpp
     HEADERS ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/LinearTimeInvariantSystem.h ${H_PREFIX}/FloatingBaseSystemKinematics.h ${H_PREFIX}/Integrator.h
-            ${H_PREFIX}/MultiStateWeightProvider.h
+            ${H_PREFIX}/MultiStateWeightProvider.h ${H_PREFIX}/CentroidalDynamics.h
             ${H_PREFIX}/Module.h
     LINK_LIBRARIES BipedalLocomotion::ContinuousDynamicalSystem
-    TESTS tests/test_linear_system.py tests/test_floating_base_system_kinematics.py
+    TESTS tests/test_linear_system.py tests/test_floating_base_system_kinematics.py tests/test_centroidal_dynamics.py
     )
 endif()

bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/CentroidalDynamics.h

@@ -0,0 +1,28 @@
+/**
+ * @file CentroidalDynamics.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_CONTINUOUS_DYNAMICAL_SYSTEM_CENTROIDAL_DYNAMICS_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_CONTINUOUS_DYNAMICAL_SYSTEM_CENTROIDAL_DYNAMICS_H
+
+#include <memory>
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ContinuousDynamicalSystem
+{
+
+void CreateCentroidalDynamics(pybind11::module& module);
+
+} // namespace ContinuousDynamicalSystem
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_CONTINUOUS_DYNAMICAL_SYSTEM_CENTROIDAL_DYNAMICS_H

bindings/python/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp

@@ -0,0 +1,46 @@
+/**
+ * @file CentroidalDynamics.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 <BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h>
+
+#include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/DynamicalSystem.h>
+#include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/Integrator.h>
+#include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h>
+#include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/CentroidalDynamics.h>
+
+#include <pybind11/eigen.h>
+#include <pybind11/stl.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ContinuousDynamicalSystem
+{
+
+void CreateCentroidalDynamics(pybind11::module& module)
+{
+    using namespace BipedalLocomotion::ContinuousDynamicalSystem;
+    namespace py = ::pybind11;
+
+    constexpr auto name = "CentroidalDynamics";
+
+    CreateDynamicalSystem<DynamicalSystem<CentroidalDynamics>,
+                          std::shared_ptr<DynamicalSystem<CentroidalDynamics>>> //
+        (module, name);
+
+    py::class_<CentroidalDynamics,
+               DynamicalSystem<CentroidalDynamics>,
+               std::shared_ptr<CentroidalDynamics>>(module, name)
+        .def(py::init());
+
+    CreateForwardEulerIntegrator<CentroidalDynamics>(module, name);
+}
+} // namespace ContinuousDynamicalSystem
+} // namespace bindings
+} // namespace BipedalLocomotion

bindings/python/ContinuousDynamicalSystem/src/Module.cpp

@@ -7,6 +7,7 @@
 
 #include <pybind11/pybind11.h>
 
+#include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/CentroidalDynamics.h>
 #include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h>
 #include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h>
 #include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/Module.h>
@@ -26,6 +27,7 @@ void CreateModule(pybind11::module& module)
     CreateLinearTimeInvariantSystem(module);
     CreateFloatingBaseSystemKinematics(module);
     CreateMultiStateWeightProvider(module);
+    CreateCentroidalDynamics(module);
 }
 } // namespace ContinuousDynamicalSystem
 } // namespace bindings

bindings/python/ContinuousDynamicalSystem/tests/test_centroidal_dynamics.py

@@ -0,0 +1,20 @@
+import pytest
+pytestmark = pytest.mark.centroidalDynamics
+
+import bipedal_locomotion_framework.bindings as blf
+import numpy as np
+import manifpy as manif
+
+from datetime import timedelta
+
+
+def test_centroidal_dynamics():
+
+    centroidal_integrator = blf.continuous_dynamical_system.CentroidalDynamicsForwardEulerIntegrator()
+    centroidal_dynamics = blf.continuous_dynamical_system.CentroidalDynamics()
+    dT = timedelta(milliseconds=100)
+    assert centroidal_integrator.set_dynamical_system(centroidal_dynamics)
+    assert centroidal_dynamics.set_state((np.zeros(3),np.zeros(3),np.zeros(3)))
+    assert centroidal_integrator.set_integration_step(timedelta(milliseconds=100))
+
+    centroidal_integrator.integrate(timedelta(0),dT)

bindings/python/ReducedModelControllers/CMakeLists.txt

@@ -0,0 +1,16 @@
+# Copyright (C) 2023 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license.
+
+if(FRAMEWORK_COMPILE_ReducedModelControllers)
+
+  set(H_PREFIX include/BipedalLocomotion/bindings/ReducedModelControllers)
+
+  add_bipedal_locomotion_python_module(
+    NAME ReducedModelControllersBindings
+    SOURCES src/CentroidalMPC.cpp src/Module.cpp
+    HEADERS ${H_PREFIX}/CentroidalMPC.h ${H_PREFIX}/Module.h
+    LINK_LIBRARIES BipedalLocomotion::ReducedModelControllers
+    TESTS tests/test_centroidal_mpc.py)
+
+endif()

bindings/python/ReducedModelControllers/include/BipedalLocomotion/bindings/ReducedModelControllers/CentroidalMPC.h

@@ -0,0 +1,26 @@
+/**
+ * @file CentroidalMPC.h
+ * @authors Carlotta Sartore
+ * @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_REDUCED_MODEL_CONTROLLERS_CENTROIDAL_MPC_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_REDUCED_MODEL_CONTROLLERS_CENTROIDAL_MPC_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ReducedModelControllers
+{
+
+void CreateCentroidalMPC(pybind11::module& module);
+
+} // namespace ReducedModelControllers
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_REDUCED_MODEL_CONTROLLERS_CENTROIDAL_MPC_H

bindings/python/ReducedModelControllers/include/BipedalLocomotion/bindings/ReducedModelControllers/Module.h

@@ -0,0 +1,26 @@
+/**
+ * @file Module.h
+ * @authors Carlotta Sartore
+ * @copyright 2021 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_REDUCED_MODEL_CONTROLLERS_MODULE_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_REDUCED_MODEL_CONTROLLERS_MODULE_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ReducedModelControllers
+{
+
+void CreateModule(pybind11::module& module);
+
+} // namespace Contacts
+} // namespace bindings
+} // namespace ReducedModelControllers
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_REDUCED_MODEL_CONTROLLERS_MODULE_H