Add PI-ADHERENT framework to ML (#889)

CHANGELOG.md

@@ -9,6 +9,8 @@ All notable changes to this project are documented in this file.
 - Add `blf-motor-current-tracking.py` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/894)
 - Add the possibility to initialize the base position and the feet pose in the `unicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/887)
 - Add software joint position limits  to the `blf-motor-current-tracking.py` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/901)
+- Implement `velMANN` class to perform inference on MANN model with velocity-based features in `ML` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/889)
+- Implement `velMANNAutoregressive`, `velMANNAutoregressiveInputBuilder`, and `velMANNTrajectoryGenerator` to generate trajectories using MANN model with velocity-based features in `ML` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/889)
 
 ### Changed
 
@@ -55,6 +57,7 @@ All notable changes to this project are documented in this file.
 - Refactor the `Planners::UnicyclePlanner` to mimic the functionalitites of the planner deployed in [walking-controllers](https://github.com/robotology/walking-controllers) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/844)
 
 ### Fixed
+- Fix the Python binding for the `change_fixed_frame` function to use `Eigen::Vector4d` for the input quaternion instead of `Eigen::Quaterniond`, which isn't supported by `pybind11`(https://github.com/ami-iit/bipedal-locomotion-framework/pull/889)
 - Fix the barrier logic for threads synchronization (https://github.com/ami-iit/bipedal-locomotion-framework/pull/811)
 - InstallBasicPackageFiles: Fix bug of OVERRIDE_MODULE_PATH that corrupt `CMAKE_MODULE_PATH` values set by blf transitive dependencies (https://github.com/ami-iit/bipedal-locomotion-framework/pull/827)
 - InstallBasicPackageFiles: Fix compatibility with CMake 3.29.1 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/835)

bindings/python/FloatingBaseEstimators/src/LeggedOdometry.cpp

@@ -89,10 +89,16 @@ void CreateLeggedOdometry(pybind11::module& module)
         .def("change_fixed_frame",
              py::overload_cast<const std::ptrdiff_t&>(&LeggedOdometry::changeFixedFrame),
              py::arg("frame_index"))
-        .def("change_fixed_frame",
-             py::overload_cast<const std::ptrdiff_t&,
-                               const Eigen::Quaterniond&,
-                               const Eigen::Vector3d&>(&LeggedOdometry::changeFixedFrame),
+        .def("change_fixed_frame", [](LeggedOdometry& impl,
+               const std::ptrdiff_t& frameIndex,
+               const Eigen::Vector4d& frameOrientationInWorld,
+               const Eigen::Vector3d& framePositionInWorld) -> bool {
+                    Eigen::Quaterniond frameOrientationInWorldQ = Eigen::Quaterniond(frameOrientationInWorld[0],
+                                                                                     frameOrientationInWorld[1],
+                                                                                     frameOrientationInWorld[2],
+                                                                                     frameOrientationInWorld[3]);
+             return impl.changeFixedFrame(frameIndex, frameOrientationInWorldQ, framePositionInWorld);
+             },
              py::arg("frame_index"),
              py::arg("frame_orientation_in_world"),
              py::arg("frame_position_in_world"))

bindings/python/ML/CMakeLists.txt

@@ -8,10 +8,14 @@ if(TARGET BipedalLocomotion::ML)
 
   add_bipedal_locomotion_python_module(
     NAME MLBindings
-    SOURCES src/Module.cpp src/MANN.cpp src/MANNAutoregressive.cpp
-            src/MANNTrajectoryGenerator.cpp src/MANNAutoregressiveInputBuilder.cpp
-    HEADERS ${H_PREFIX}/Module.h ${H_PREFIX}/MANN.h ${H_PREFIX}/MANNAutoregressive.h
-            ${H_PREFIX}/MANNTrajectoryGenerator.h ${H_PREFIX}/MANNAutoregressiveInputBuilder.h
+    SOURCES src/Module.cpp src/MANN.cpp src/VelMANN.cpp
+            src/MANNAutoregressive.cpp src/VelMANNAutoregressive.cpp
+            src/MANNTrajectoryGenerator.cpp src/VelMANNTrajectoryGenerator.cpp
+            src/MANNAutoregressiveInputBuilder.cpp src/VelMANNAutoregressiveInputBuilder.cpp
+    HEADERS ${H_PREFIX}/Module.h ${H_PREFIX}/MANN.h ${H_PREFIX}/VelMANN.h
+            ${H_PREFIX}/MANNAutoregressive.h ${H_PREFIX}/VelMANNAutoregressive.h
+            ${H_PREFIX}/MANNTrajectoryGenerator.h ${H_PREFIX}/VelMANNTrajectoryGenerator.h
+            ${H_PREFIX}/MANNAutoregressiveInputBuilder.h ${H_PREFIX}/VelMANNAutoregressiveInputBuilder.h
     LINK_LIBRARIES BipedalLocomotion::ML
     )
 

bindings/python/ML/include/BipedalLocomotion/bindings/ML/VelMANN.h

@@ -0,0 +1,26 @@
+/**
+ * @file VelMANN.h
+ * @authors Evelyn D'Elia
+ * @copyright 2024 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_ML_VEL_MANN_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_ML_VEL_MANN_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ML
+{
+
+void CreateVelMANN(pybind11::module& module);
+
+} // namespace ML
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_ML_VEL_MANN_H

bindings/python/ML/include/BipedalLocomotion/bindings/ML/VelMANNAutoregressive.h

@@ -0,0 +1,26 @@
+/**
+ * @file VelMANNAutoregressive.h
+ * @authors Evelyn D'Elia
+ * @copyright 2024 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_ML_VEL_MANN_TRAJECTORY_GENERATION_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_ML_VEL_MANN_TRAJECTORY_GENERATION_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ML
+{
+
+void CreateVelMANNAutoregressive(pybind11::module& module);
+
+} // namespace ML
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_ML_VEL_MANN_TRAJECTORY_GENERATION_H

bindings/python/ML/include/BipedalLocomotion/bindings/ML/VelMANNAutoregressiveInputBuilder.h

@@ -0,0 +1,26 @@
+/*
+ * @file VelMANNAutoregressiveInputBuilder.h
+ * @authors Evelyn D'Elia
+ * @copyright 2024 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_ML_VEL_MANN_AUTOREGRESSIVE_INPUT_BUILDER_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_ML_VEL_MANN_AUTOREGRESSIVE_INPUT_BUILDER_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ML
+{
+
+void CreateVelMANNAutoregressiveInputBuilder(pybind11::module& module);
+
+} // namespace ML
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_ML_VEL_MANN_AUTOREGRESSIVE_INPUT_BUILDER_H

bindings/python/ML/include/BipedalLocomotion/bindings/ML/VelMANNTrajectoryGenerator.h

@@ -0,0 +1,26 @@
+/*
+ * @file VelMANNTrajectoryGenerator.h
+ * @authors Evelyn D'Elia
+ * @copyright 2024 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_ML_VEL_MANN_TRAJECTORY_GENERATOR_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_ML_VEL_MANN_TRAJECTORY_GENERATOR_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ML
+{
+
+void CreateVelMANNTrajectoryGenerator(pybind11::module& module);
+
+} // namespace ML
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_ML_VEL_MANN_TRAJECTORY_GENERATOR_H

bindings/python/ML/src/Module.cpp

@@ -11,6 +11,10 @@
 #include <BipedalLocomotion/bindings/ML/MANNAutoregressive.h>
 #include <BipedalLocomotion/bindings/ML/MANNTrajectoryGenerator.h>
 #include <BipedalLocomotion/bindings/ML/MANNAutoregressiveInputBuilder.h>
+#include <BipedalLocomotion/bindings/ML/VelMANN.h>
+#include <BipedalLocomotion/bindings/ML/VelMANNAutoregressive.h>
+#include <BipedalLocomotion/bindings/ML/VelMANNTrajectoryGenerator.h>
+#include <BipedalLocomotion/bindings/ML/VelMANNAutoregressiveInputBuilder.h>
 
 
 namespace BipedalLocomotion
@@ -27,6 +31,10 @@ void CreateModule(pybind11::module& module)
     CreateMANNAutoregressive(module);
     CreateMANNTrajectoryGenerator(module);
     CreateMANNAutoregressiveInputBuilder(module);
+    CreateVelMANN(module);
+    CreateVelMANNAutoregressive(module);
+    CreateVelMANNTrajectoryGenerator(module);
+    CreateVelMANNAutoregressiveInputBuilder(module);
 }
 } // namespace ML
 } // namespace bindings

bindings/python/ML/src/VelMANN.cpp

@@ -0,0 +1,53 @@
+/**
+ * @file VelMANN.cpp
+ * @authors Evelyn D'Elia
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/ML/VelMANN.h>
+#include <BipedalLocomotion/bindings/ML/VelMANN.h>
+#include <BipedalLocomotion/bindings/System/Advanceable.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ML
+{
+
+void CreateVelMANN(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    namespace ML = BipedalLocomotion::ML;
+    namespace System = BipedalLocomotion::System;
+
+    py::class_<ML::VelMANNInput>(module, "VelMANNInput")
+        .def(py::init())
+        .def_readwrite("base_linear_velocity_trajectory", &ML::VelMANNInput::baseLinearVelocityTrajectory)
+        .def_readwrite("base_angular_velocity_trajectory", &ML::VelMANNInput::baseAngularVelocityTrajectory)
+        .def_readwrite("joint_positions", &ML::VelMANNInput::jointPositions)
+        .def_readwrite("joint_velocities", &ML::VelMANNInput::jointVelocities);
+
+    py::class_<ML::VelMANNOutput>(module, "VelMANNOutput")
+        .def(py::init())
+        .def_readwrite("future_base_linear_velocity_trajectory",
+                       &ML::VelMANNOutput::futureBaseLinearVelocityTrajectory)
+        .def_readwrite("future_base_angular_velocity_trajectory",
+                       &ML::VelMANNOutput::futureBaseAngularVelocityTrajectory)
+        .def_readwrite("joint_positions", &ML::VelMANNOutput::jointPositions)
+        .def_readwrite("joint_velocities", &ML::VelMANNOutput::jointVelocities);
+
+    BipedalLocomotion::bindings::System::CreateAdvanceable<ML::VelMANNInput, //
+                                                           ML::VelMANNOutput>(module, "VelMANN");
+    py::class_<ML::VelMANN, System::Advanceable<ML::VelMANNInput, ML::VelMANNOutput>>(module, "VelMANN")
+        .def(py::init());
+}
+
+} // namespace ML
+} // namespace bindings
+} // namespace BipedalLocomotion

bindings/python/ML/src/VelMANNAutoregressive.cpp

@@ -0,0 +1,81 @@
+/**
+ * @file VelMANNAutoregressive.cpp
+ * @authors Evelyn D'Elia
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <iDynTree/Model/Model.h>
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/ML/VelMANNAutoregressive.h>
+#include <BipedalLocomotion/bindings/ML/VelMANNAutoregressive.h>
+#include <BipedalLocomotion/bindings/System/Advanceable.h>
+#include <BipedalLocomotion/bindings/type_caster/swig.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ML
+{
+
+void CreateVelMANNAutoregressive(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    namespace ML = BipedalLocomotion::ML;
+    namespace System = BipedalLocomotion::System;
+
+    py::class_<ML::VelMANNAutoregressiveInput>(module, "VelMANNAutoregressiveInput")
+        .def(py::init())
+        .def_readwrite("desired_future_base_trajectory",
+                       &ML::VelMANNAutoregressiveInput::desiredFutureBaseTrajectory)
+        .def_readwrite("desired_future_base_directions",
+                       &ML::VelMANNAutoregressiveInput::desiredFutureBaseDirections)
+        .def_readwrite("desired_future_base_velocities",
+                       &ML::VelMANNAutoregressiveInput::desiredFutureBaseVelocities)
+        .def_readwrite("desired_future_base_angular_velocities",
+                       &ML::VelMANNAutoregressiveInput::desiredFutureBaseAngVelocities);
+
+    py::class_<ML::VelMANNAutoregressiveOutput>(module, "VelMANNAutoregressiveOutput")
+        .def(py::init())
+        .def_readwrite("joint_positions", &ML::VelMANNAutoregressiveOutput::jointsPosition)
+        .def_readwrite("joint_velocities", &ML::VelMANNAutoregressiveOutput::jointsVelocity)
+        .def_readwrite("base_pose", &ML::VelMANNAutoregressiveOutput::basePose)
+        .def_readwrite("base_velocity", &ML::VelMANNAutoregressiveOutput::baseVelocity)
+        .def_readwrite("left_foot_pose", &ML::VelMANNAutoregressiveOutput::leftFootPose)
+        .def_readwrite("right_foot_pose", &ML::VelMANNAutoregressiveOutput::rightFootPose)
+        .def_readwrite("left_foot_velocity", &ML::VelMANNAutoregressiveOutput::leftFootVelocity)
+        .def_readwrite("right_foot_velocity", &ML::VelMANNAutoregressiveOutput::rightFootVelocity)
+        .def_readwrite("left_foot", &ML::VelMANNAutoregressiveOutput::leftFoot)
+        .def_readwrite("right_foot", &ML::VelMANNAutoregressiveOutput::rightFoot);
+
+    BipedalLocomotion::bindings::System::CreateAdvanceable<ML::VelMANNAutoregressiveInput, //
+                                                           ML::VelMANNAutoregressiveOutput> //
+        (module, "VelMANNAutoregressive");
+    py::class_<ML::VelMANNAutoregressive,
+               System::Advanceable<ML::VelMANNAutoregressiveInput, //
+                                   ML::VelMANNAutoregressiveOutput>>(module, "VelMANNAutoregressive")
+        .def(py::init())
+        .def("reset",
+             py::overload_cast<Eigen::Ref<const Eigen::VectorXd>, const manif::SE3d&>(
+                 &ML::VelMANNAutoregressive::reset),
+             py::arg("joint_positions"),
+             py::arg("base_pose"))
+        .def("set_robot_model", [](ML::VelMANNAutoregressive& impl, ::pybind11::object& obj) {
+            iDynTree::Model* cls = py::detail::swig_wrapped_pointer_to_pybind<iDynTree::Model>(obj);
+
+            if (cls == nullptr)
+            {
+                throw ::pybind11::value_error("Invalid input for the function. Please provide "
+                                              "an iDynTree::Model object.");
+            }
+            return impl.setRobotModel(*cls);
+        });
+}
+
+} // namespace ML
+} // namespace bindings
+} // namespace BipedalLocomotion

bindings/python/ML/src/VelMANNAutoregressiveInputBuilder.cpp

@@ -0,0 +1,46 @@
+/**
+ * @file VelMANNAutoregressiveInputBuilder.cpp
+ * @authors Evelyn D'Elia
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/ML/VelMANNAutoregressiveInputBuilder.h>
+#include <BipedalLocomotion/bindings/ML/VelMANNAutoregressiveInputBuilder.h>
+#include <BipedalLocomotion/bindings/System/Advanceable.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace ML
+{
+
+void CreateVelMANNAutoregressiveInputBuilder(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    namespace ML = BipedalLocomotion::ML;
+    namespace System = BipedalLocomotion::System;
+
+    py::class_<ML::VelMANNDirectionalInput>(module, "VelMANNDirectionalInput")
+        .def(py::init())
+        .def_readwrite("motion_direction", &ML::VelMANNDirectionalInput::motionDirection)
+        .def_readwrite("base_direction", &ML::VelMANNDirectionalInput::baseDirection);
+
+    BipedalLocomotion::bindings::System::CreateAdvanceable<ML::VelMANNDirectionalInput, //
+                                                           ML::VelMANNAutoregressiveInput> //
+        (module, "VelMANNAutoregressiveInputBuilder");
+    py::class_<ML::VelMANNAutoregressiveInputBuilder,
+               System::Advanceable<ML::VelMANNDirectionalInput, //
+                                   ML::VelMANNAutoregressiveInput>>(module,
+                                                                 "VelMANNAutoregressiveInputBuilder")
+        .def(py::init());
+}
+
+} // namespace ML
+} // namespace bindings
+} // namespace BipedalLocomotion