Implement a class to perform inference with the MANN network

.github/workflows/conda-forge-ci.yml

@@ -48,8 +48,17 @@ jobs:
       if: contains(matrix.os, 'ubuntu')
       shell: bash -l {0}
       run: |
-        # See https://github.com/robotology/robotology-superbuild/issues/477
-        mamba install expat-cos6-x86_64 libselinux-cos6-x86_64 libxau-cos6-x86_64 libxcb-cos6-x86_64 libxdamage-cos6-x86_64 libxext-cos6-x86_64 libxfixes-cos6-x86_64 libxxf86vm-cos6-x86_64 mesalib mesa-libgl-cos6-x86_64 mesa-libgl-devel-cos6-x86_64
+        mamba install expat-cos6-x86_64 libselinux-cos6-x86_64 libxau-cos6-x86_64 libxcb-cos6-x86_64 \
+                      libxdamage-cos6-x86_64 libxext-cos6-x86_64 libxfixes-cos6-x86_64 \
+                      libxxf86vm-cos6-x86_64 mesalib mesa-libgl-cos6-x86_64 \
+                      mesa-libgl-devel-cos6-x86_64 onnxruntime
+
+    - name: maxOS-only Dependencies [macOS]
+      if: contains(matrix.os, 'macos')
+      shell: bash -l {0}
+      run: |
+        mamba install onnxruntime
+
 
     - name: Windows-only Dependencies [Windows]
       if: contains(matrix.os, 'windows')

CHANGELOG.md

@@ -3,6 +3,7 @@ All notable changes to this project are documented in this file.
 
 ## [unreleased]
 ### Added
+- Implement a class to perform inference with the [MANN network](https://github.com/ami-iit/mann-pytorch) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/652)
 
 ### Changed
 

cmake/BipedalLocomotionFrameworkDependencies.cmake

@@ -105,6 +105,9 @@ checkandset_dependency(VALGRIND)
 find_package(UnicyclePlanner QUIET)
 checkandset_dependency(UnicyclePlanner)
 
+find_package(onnxruntime QUIET)
+checkandset_dependency(onnxruntime)
+
 ##########################      Components       ##############################
 framework_dependent_option(FRAMEWORK_RUN_Valgrind_tests
   "Run Valgrind tests?" OFF
@@ -190,6 +193,10 @@ framework_dependent_option(FRAMEWORK_COMPILE_IK
   "Compile IK library?" ON
   "FRAMEWORK_COMPILE_System;FRAMEWORK_USE_LieGroupControllers;FRAMEWORK_COMPILE_ManifConversions;FRAMEWORK_USE_manif;FRAMEWORK_USE_OsqpEigen" OFF)
 
+framework_dependent_option(FRAMEWORK_COMPILE_ML
+  "Compile machine learning libraries?" ON
+  "FRAMEWORK_USE_onnxruntime;FRAMEWORK_USE_manif" OFF)
+
 framework_dependent_option(FRAMEWORK_COMPILE_SimplifiedModelControllers
   "Compile SimplifiedModelControllers library?" ON
   "FRAMEWORK_USE_manif" OFF)

cmake/Findonnxruntime.cmake

@@ -0,0 +1,34 @@
+# SPDX-FileCopyrightText: 2023 Giulio Romualdi
+# SPDX-License-Identifier: BSD-3-Clause
+
+#[=======================================================================[.rst:
+Findonnxruntime
+-----------
+
+The following imported targets are created:
+
+onnxruntime::onnxruntime
+
+#]=======================================================================]
+
+include(FindPackageHandleStandardArgs)
+
+find_path(onnxruntime_INCLUDE_DIR
+  NAMES onnxruntime_c_api.h)
+mark_as_advanced(onnxruntime_INCLUDE_DIR)
+find_library(onnxruntime_LIBRARY
+  NAMES onnxruntime)
+mark_as_advanced(onnxruntime_LIBRARY)
+mark_as_advanced(onnxruntime_LIBRARY)
+
+find_package_handle_standard_args(onnxruntime DEFAULT_MSG onnxruntime_INCLUDE_DIR onnxruntime_LIBRARY)
+
+if(onnxruntime_FOUND)
+  if(NOT TARGET onnxruntime::onnxruntime)
+    add_library(onnxruntime::onnxruntime UNKNOWN IMPORTED)
+    set_target_properties(onnxruntime::onnxruntime PROPERTIES
+      INTERFACE_INCLUDE_DIRECTORIES "${onnxruntime_INCLUDE_DIR}")
+    set_property(TARGET onnxruntime::onnxruntime PROPERTY
+      IMPORTED_LOCATION "${onnxruntime_LIBRARY}")
+  endif()
+endif()

src/CMakeLists.txt

@@ -20,3 +20,4 @@ add_subdirectory(TSID)
 add_subdirectory(Perception)
 add_subdirectory(IK)
 add_subdirectory(SimplifiedModelControllers)
+add_subdirectory(ML)

src/ML/CMakeLists.txt

@@ -0,0 +1,19 @@
+# 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_ML)
+
+  set(H_PREFIX include/BipedalLocomotion/ML)
+
+  add_bipedal_locomotion_library(
+    NAME ML
+    PUBLIC_HEADERS ${H_PREFIX}/MANN.h
+    SOURCES        src/MANN.cpp
+    PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System
+    PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging onnxruntime::onnxruntime
+    INSTALLATION_FOLDER ML)
+
+  add_subdirectory(tests)
+
+endif()

src/ML/include/BipedalLocomotion/ML/MANN.h

@@ -0,0 +1,147 @@
+/**
+ * @file MANN.h
+ * @authors Paolo Maria Viceconte, 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_ML_MANN_H
+#define BIPEDAL_LOCOMOTION_ML_MANN_H
+
+#include <Eigen/Dense>
+#include <manif/SE2.h>
+
+#include <memory>
+
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <BipedalLocomotion/System/Advanceable.h>
+#include <BipedalLocomotion/System/VariablesHandler.h>
+
+namespace BipedalLocomotion
+{
+namespace ML
+{
+
+/**
+ * MANNInput contains the input to the MANN network.
+ * The base position trajectory, facing direction trajectory and base velocity trajectories are
+ * written in a bidimensional local reference frame L in which we assume all the quantities related
+ * to the ground-projected base trajectory in xi and yi to be expressed. At each step ti,
+ * L is defined to have its origin in the current ground-projected robot base position and orientation defined
+ * by the current facing direction (along with its orthogonal vector).
+ */
+struct MANNInput
+{
+    /** Matrix containing the base position trajectory. The rows contain the x and y position
+     * projected into the ground while the columns the position at each time instant. */
+    Eigen::Matrix2Xd basePositionTrajectory;
+    /** Matrix containing the facing direction trajectory. The rows contain the x and y direction
+     * projected into the ground while the columns the direction at each time instant. */
+    Eigen::Matrix2Xd facingDirectionTrajectory;
+    /** Matrix containing the base velocity trajectory. The rows contain the x and y velocity
+     *  projected into the ground while the columns the position  at each time instant. */
+    Eigen::Matrix2Xd baseVelocitiesTrajectory;
+
+    Eigen::VectorXd jointPositions; /**< Vector containing the actual joint position in radians. */
+    Eigen::VectorXd jointVelocities; /**< Vector containing the actual joint velocity in radians per
+                                        seconds. */
+};
+
+/**
+ * MANNOutput contains the output to the MANN network.
+ * The base position trajectory, facing direction trajectory and base velocity trajectories are
+ * written in a bidimensional local reference frame L in which we assume all the quantities related
+ * to the ground-projected base trajectory in xi and yi to be expressed. At each step ti,
+ * L is defined to have its origin in the current ground-projected robot base position and
+ * orientation defined by the current facing direction (along with its orthogonal vector).
+ */
+struct MANNOutput
+{
+    /** Matrix containing the future base position trajectory. The rows contains the x and y
+     * position projected into the ground while the columns the position at each time
+     * instant. */
+    Eigen::Matrix2Xd futureBasePositionTrajectory;
+
+    /** Matrix containing the future base facing direction. The rows contains the x and y direction
+     * projected into the ground while the columns the position at each time instant. */
+    Eigen::Matrix2Xd futureFacingDirectionTrajectory;
+
+    /** Matrix containing the future base velocity trajectory. The rows contains the x and y
+     * velocity projected into the ground while the columns the position at each time instant. */
+    Eigen::Matrix2Xd futureBaseVelocitiesTrajectory;
+    Eigen::VectorXd jointPositions; /**< Vector containing the next joint position in radians */
+    Eigen::VectorXd jointVelocities; /**< Vector containing the next joint velocity in radians per
+                                        seconds */
+    manif::SE2Tangentd projectedBaseVelocity; /**< Velocity of the base projected on the ground */
+};
+
+/**
+ * MANN is a class that allows to perform the inference of an `onnx` implementing Mode-Adaptive
+ * Neural Networks (MANN). The network was originally proposed in [H. Zhang, S. Starke, T. Komura,
+ * and J. Saito, “Mode-adaptive neural networks for quadruped motion control,” ACM Trans. Graph.,
+ * vol. 37, no. 4, pp. 1–11, 2018.](https://doi.org/10.1145/3197517.3201366) This class uses
+ * `onnxruntime` to perform inference and it has been tested with an `onnx` model generated from
+ * https://github.com/ami-iit/mann-pytorch
+ */
+class MANN : public BipedalLocomotion::System::Advanceable<MANNInput, MANNOutput>
+{
+public:
+    /**
+     * Constructor
+     */
+    MANN();
+
+    /**
+     * Destructor. It is required by the pimpl implementation.
+     */
+    ~MANN();
+
+    /**
+     * Initialize the network.
+     * @param paramHandler pointer to the parameters handler.
+     * @note the following parameters are required by the class
+     * |      Parameter Name      |       Type       |                          Description                                | Mandatory |
+     * |:------------------------:|:----------------:|:-------------------------------------------------------------------:|:---------:|
+     * |    `onnx_model_path`     |      `string`    |  Path to the `onnx` model that will be loaded to perform inference  |    Yes    |
+     * |   `number_of_joints`     |       `int`      |         Number of robot joints considered in the model              |    Yes    |
+     * | `projected_base_horizon` |       `int`      |    Number of samples of the base horizon considered in the model    |    Yes    |
+     * @return True in case of success, false otherwise.
+     */
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+
+    /**
+     * Set the input of the network
+     * @param input the struct containing all the inputs of the network.
+     * @return true if case of success, false otherwise.
+     */
+    bool setInput(const MANNInput& input) override;
+
+    /**
+     * Perform one step of the inference given the input set.
+     * @return true if case of success, false otherwise.
+     */
+    bool advance() override;
+
+    /**
+     * Get the output of the network once advance has been called.
+     * @return the output of the network.
+     */
+    const MANNOutput& getOutput() const override;
+
+    /**
+     * Check if the output of the network is valid.
+     * @return true if it is valid, false otherwise.
+     */
+    bool isOutputValid() const override;
+
+private:
+
+    /** Private implementation */
+    struct Impl;
+    std::unique_ptr<Impl> m_pimpl;
+};
+
+} // namespace ML
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_ML_MANN_H