diff --git a/.github/workflows/conda-forge-ci.yml b/.github/workflows/conda-forge-ci.yml index 6a5d621af7..80006db49b 100644 --- a/.github/workflows/conda-forge-ci.yml +++ b/.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') diff --git a/CHANGELOG.md b/CHANGELOG.md index eeea4fe089..7fe7ba8f65 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index 08e6a4c15f..84d132fd1f 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/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) diff --git a/cmake/Findonnxruntime.cmake b/cmake/Findonnxruntime.cmake new file mode 100644 index 0000000000..aea10a04d3 --- /dev/null +++ b/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() diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index bdab3a8213..74a048689a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -20,3 +20,4 @@ add_subdirectory(TSID) add_subdirectory(Perception) add_subdirectory(IK) add_subdirectory(SimplifiedModelControllers) +add_subdirectory(ML) diff --git a/src/ML/CMakeLists.txt b/src/ML/CMakeLists.txt new file mode 100644 index 0000000000..2b5e66118e --- /dev/null +++ b/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() diff --git a/src/ML/include/BipedalLocomotion/ML/MANN.h b/src/ML/include/BipedalLocomotion/ML/MANN.h new file mode 100644 index 0000000000..879e49900e --- /dev/null +++ b/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 +#include + +#include + +#include +#include +#include + +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 +{ +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 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 m_pimpl; +}; + +} // namespace ML +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ML_MANN_H diff --git a/src/ML/src/MANN.cpp b/src/ML/src/MANN.cpp new file mode 100644 index 0000000000..c203242518 --- /dev/null +++ b/src/ML/src/MANN.cpp @@ -0,0 +1,338 @@ +/** + * @file MANN.cpp + * @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. + */ + +#include +#include + +#include + +// onnxruntime +#include + +#include +#include +#include +#include + +using namespace BipedalLocomotion::ML; +using namespace BipedalLocomotion; + +struct MANN::Impl +{ + enum class FSM + { + NotInitialized, Initialized, Running, + }; + + struct DataStructured + { + BipedalLocomotion::System::VariablesHandler handler; + Eigen::VectorXf rawData; + + Ort::Value tensor{nullptr}; + std::array shape; + }; + + DataStructured structuredInput; + DataStructured structuredOutput; + + MANNOutput output; + + Ort::MemoryInfo memoryInfo; + + Ort::Env env; + std::unique_ptr session; + + Impl(); + bool populateInput(const MANNInput& input); + + FSM state{FSM::NotInitialized}; +}; + +MANN::Impl::Impl() + : memoryInfo(::Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU)) +{ +} + +bool MANN::Impl::populateInput(const MANNInput& input) +{ + constexpr auto logPrefix = "[MANN::Impl::populateInput]"; + + auto populateVectorData + = [&input, this, logPrefix](const std::string& variableName, + Eigen::Ref data) -> bool { + const auto& variable = this->structuredInput.handler.getVariable(variableName); + if (data.size() != variable.size) + { + log()->error("{} Invalid size of the variable named '{}'. Expected size: {}, " + "vector size: {}.", + logPrefix, + variableName, + variable.size, + data.size()); + return false; + } + + this->structuredInput.rawData.segment(variable.offset, variable.size) = data.cast(); + return true; + }; + + auto populateProjectedData + = [&input, + this, + logPrefix](const std::string& variableName, + Eigen::Ref data) -> bool { + const auto& variable = this->structuredInput.handler.getVariable(variableName); + if (data.size() != variable.size) + { + log()->error("{} Invalid size of the variable named '{}'. Expected size: {}, " + "Passed matrix size: {} = {} x {}.", + logPrefix, + variableName, + variable.size, + data.size(), + data.cols(), + data.rows()); + return false; + } + + // Eigen considers the matrix stored in column wise form + // [x1, x2, ..., xn; + // y1, y2, ..., yn] + // becomes [x1, y1, x2, y2, ...., xn, yn] + this->structuredInput.rawData.segment(variable.offset, variable.size) + = Eigen::Map(data.data(), data.cols() * data.rows()) + .cast(); + + return true; + }; + + // [x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12 + // y1, y2, ......................................y12] + Eigen::Ref tmp + = input.basePositionTrajectory.rightCols(input.basePositionTrajectory.cols() / 2); + const double trajectoryLength + = (tmp.rightCols(tmp.cols() - 1) - tmp.leftCols(tmp.cols() - 1)).colwise().norm().sum(); + + bool ok = populateVectorData("joint_velocities", input.jointVelocities); + ok = ok && populateVectorData("joint_positions", input.jointPositions); + ok = ok && populateProjectedData("base_positions", input.basePositionTrajectory); + ok = ok && populateProjectedData("base_velocities", input.baseVelocitiesTrajectory); + ok = ok && populateProjectedData("facing_directions", input.facingDirectionTrajectory); + ok = ok + && populateVectorData("trajectory_length", + Eigen::Matrix::Constant(trajectoryLength)); + + return ok; +} + +MANN::MANN() +{ + m_pimpl = std::make_unique(); +} + +MANN::~MANN() = default; + +bool MANN::initialize( + std::weak_ptr handler) +{ + constexpr auto logPrefix = "[MANN::initialize]"; + auto ptr = handler.lock(); + + if (ptr == nullptr) + { + log()->error("{} Invalid parameter handler.", logPrefix); + return false; + } + + auto loadParam = [ptr, logPrefix](const std::string& paramName, auto& param) -> bool { + if (!ptr->getParameter(paramName, param)) + { + log()->error("{} Unable to get the parameter named '{}'.", logPrefix, paramName); + return false; + } + return true; + }; + + int numberOfJoints, projectedBaseHorizon; + std::string networkModelPath; + bool ok = loadParam("number_of_joints", numberOfJoints); + ok = ok && loadParam("projected_base_horizon", projectedBaseHorizon); + ok = ok && loadParam("onnx_model_path", networkModelPath); + + if (projectedBaseHorizon % 2 != 0) + { + log()->error("{} Project base horizon must be an even number. This is required by mann.", + logPrefix); + return false; + } + + if (!ok) + { + return false; + } + + m_pimpl->session = std::make_unique(m_pimpl->env, + networkModelPath.c_str(), + Ort::SessionOptions{nullptr}); + + if (m_pimpl->session == nullptr) + { + log()->error("{} Unable to instantiate the model in: '{}'.", logPrefix, networkModelPath); + return false; + } + + // the input of the network is composed by + const std::size_t inputSize = 2 * projectedBaseHorizon // position of the base on x and y + // coordinate in the horizon + + 2 * projectedBaseHorizon // facing direction on x and y + // coordinate in the horizon (equal to + // projectedBaseHorizon) + + 2 * projectedBaseHorizon // velocity of the base on x and y + // coordinate in the horizon + + 1 // length of the trajectory of the based projected + + numberOfJoints // joints positions + + numberOfJoints; // joints velocities + + // resize the input + m_pimpl->structuredInput.rawData.resize(inputSize); + m_pimpl->structuredInput.shape[0] = 1; // batch + m_pimpl->structuredInput.shape[1] = inputSize; + + // create tensor required by onnx + m_pimpl->structuredInput.tensor + = Ort::Value::CreateTensor(m_pimpl->memoryInfo, + m_pimpl->structuredInput.rawData.data(), + m_pimpl->structuredInput.rawData.size(), + m_pimpl->structuredInput.shape.data(), + m_pimpl->structuredInput.shape.size()); + + // populate variable handler related to the input + // the serialization matters + m_pimpl->structuredInput.handler.addVariable("base_positions", 2 * projectedBaseHorizon); + m_pimpl->structuredInput.handler.addVariable("facing_directions", 2 * projectedBaseHorizon); + m_pimpl->structuredInput.handler.addVariable("base_velocities", 2 * projectedBaseHorizon); + m_pimpl->structuredInput.handler.addVariable("trajectory_length", 1); + m_pimpl->structuredInput.handler.addVariable("joint_positions", numberOfJoints); + m_pimpl->structuredInput.handler.addVariable("joint_velocities", numberOfJoints); + + // populate the output + const std::size_t outputSize = 2 * projectedBaseHorizon / 2 // position of the base on x and y + // coordinate in the future horizon + + 2 * projectedBaseHorizon / 2 // facing direction on x and y + // coordinate in the future + // horizon (equal to + // projectedBaseHorizon) + + 2 * projectedBaseHorizon / 2 // velocity of the base on x and y + // coordinate in the future + // horizon + + numberOfJoints // joints positions + + numberOfJoints // joints velocities + + 3; // x y and omega component of the velocity + + // resize the output + m_pimpl->structuredOutput.rawData.resize(outputSize); + m_pimpl->structuredOutput.shape[0] = 1; // batch + m_pimpl->structuredOutput.shape[1] = outputSize; + + // create tensor required by onnx + m_pimpl->structuredOutput.tensor + = Ort::Value::CreateTensor(m_pimpl->memoryInfo, + m_pimpl->structuredOutput.rawData.data(), + m_pimpl->structuredOutput.rawData.size(), + m_pimpl->structuredOutput.shape.data(), + m_pimpl->structuredOutput.shape.size()); + // populate variable handler related to the output + // the serialization matters + m_pimpl->structuredOutput.handler.addVariable("future_base_positions", + 2 * projectedBaseHorizon / 2); + m_pimpl->structuredOutput.handler.addVariable("future_facing_directions", + 2 * projectedBaseHorizon / 2); + m_pimpl->structuredOutput.handler.addVariable("future_base_velocities", + 2 * projectedBaseHorizon / 2); + m_pimpl->structuredOutput.handler.addVariable("joint_positions", numberOfJoints); + m_pimpl->structuredOutput.handler.addVariable("joint_velocities", numberOfJoints); + m_pimpl->structuredOutput.handler.addVariable("base_velocity", 3); + + // resize the output + m_pimpl->output.futureBasePositionTrajectory.resize(2, projectedBaseHorizon / 2); + m_pimpl->output.futureFacingDirectionTrajectory.resize(2, projectedBaseHorizon / 2); + m_pimpl->output.futureBaseVelocitiesTrajectory.resize(2, projectedBaseHorizon / 2); + m_pimpl->output.jointPositions.resize(numberOfJoints); + m_pimpl->output.jointVelocities.resize(numberOfJoints); + m_pimpl->output.projectedBaseVelocity = manif::SE2Tangentd::Zero(); + + m_pimpl->state = Impl::FSM::Initialized; + + return true; +} + +bool MANN::setInput(const MANNInput& input) +{ + if (m_pimpl->state != Impl::FSM::Initialized && m_pimpl->state != Impl::FSM::Running) + { + log()->error("[MANN::setInput] The network is not initialized, please call initialize()"); + return false; + } + return m_pimpl->populateInput(input); +} + +bool MANN::advance() +{ + auto unpackMatrix + = [this](const std::string& variableName, Eigen::Ref matrix) { + const auto& variable = m_pimpl->structuredOutput.handler.getVariable(variableName); + assert(variable.isValid()); + + // the matrix has been already allocated + const std::size_t rows = matrix.rows(); + const std::size_t cols = matrix.cols(); + + matrix = Eigen::Map( + m_pimpl->structuredOutput.rawData.segment(variable.offset, variable.size).data(), + rows, + cols).cast(); + + return; + }; + + const char* inputNames[] = {"input"}; + const char* outputNames[] = {"output"}; + + m_pimpl->session->Run(Ort::RunOptions(), + inputNames, + &(m_pimpl->structuredInput.tensor), + 1, + outputNames, + &(m_pimpl->structuredOutput.tensor), + 1); + + unpackMatrix("future_base_positions", m_pimpl->output.futureBasePositionTrajectory); + unpackMatrix("future_facing_directions", m_pimpl->output.futureFacingDirectionTrajectory); + unpackMatrix("future_base_velocities", m_pimpl->output.futureBaseVelocitiesTrajectory); + unpackMatrix("joint_positions", m_pimpl->output.jointPositions); + unpackMatrix("joint_velocities", m_pimpl->output.jointVelocities); + + Eigen::Vector3d tempVector; + unpackMatrix("base_velocity", tempVector); + m_pimpl->output.projectedBaseVelocity + = manif::SE2Tangentd(tempVector(0), tempVector(1), tempVector(2)); + + m_pimpl->state = Impl::FSM::Running; + + return true; +} + +bool MANN::isOutputValid() const +{ + return m_pimpl->state == Impl::FSM::Running; +} + +const MANNOutput& MANN::getOutput() const +{ + return m_pimpl->output; +} diff --git a/src/ML/tests/CMakeLists.txt b/src/ML/tests/CMakeLists.txt new file mode 100644 index 0000000000..c57c7ec898 --- /dev/null +++ b/src/ML/tests/CMakeLists.txt @@ -0,0 +1,27 @@ +# 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. + +include_directories(${CMAKE_CURRENT_BINARY_DIR}) +configure_file("${CMAKE_CURRENT_SOURCE_DIR}/FolderPath.h.in" "${CMAKE_CURRENT_BINARY_DIR}/MANNModelFolderPath.h" @ONLY) + +set(MANN_ONNX_EXPECTED_MD5 de9080f1b909368ffae044615d7646af) + +if (EXISTS "${CMAKE_CURRENT_BINARY_DIR}/model.onnx") + file(MD5 "${CMAKE_CURRENT_BINARY_DIR}/model.onnx" ONNX_MODEL_CHECKSUM_VARIABLE) + string(COMPARE EQUAL ${ONNX_MODEL_CHECKSUM_VARIABLE} ${MANN_ONNX_EXPECTED_MD5} MANN_ONNX_UPDATED) +else() + set(MANN_ONNX_UPDATED FALSE) +endif() + +if(NOT ${MANN_ONNX_UPDATED}) + message(STATUS "Fetching MANN onnx model from ami-iit/mann-pytorch...") + file(DOWNLOAD https://raw.githubusercontent.com/ami-iit/mann-pytorch/7386952b61440c19459bbe680b818faf7f461ede/models/storage_20220909-131438/models/model_49.onnx + ${CMAKE_CURRENT_BINARY_DIR}/model.onnx + EXPECTED_MD5 ${MANN_ONNX_EXPECTED_MD5}) +endif() + +add_bipedal_test( + NAME MANN + SOURCES MANNTest.cpp + LINKS BipedalLocomotion::ML) diff --git a/src/ML/tests/FolderPath.h.in b/src/ML/tests/FolderPath.h.in new file mode 100644 index 0000000000..0534dc5411 --- /dev/null +++ b/src/ML/tests/FolderPath.h.in @@ -0,0 +1,18 @@ +/** + * @file FolderPath.h(.in) + * @authors Stefano Dafarra + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef CONFIG_FOLDERPATH_H_IN +#define CONFIG_FOLDERPATH_H_IN + +#define SOURCE_CONFIG_DIR "@CMAKE_CURRENT_BINARY_DIR@" + +inline std::string getMANNModelPath() +{ + return std::string(SOURCE_CONFIG_DIR) + "/model.onnx"; +} + +#endif // CONFIG_FOLDERPATH_H_IN diff --git a/src/ML/tests/MANNTest.cpp b/src/ML/tests/MANNTest.cpp new file mode 100644 index 0000000000..5ae9f9c308 --- /dev/null +++ b/src/ML/tests/MANNTest.cpp @@ -0,0 +1,116 @@ +/** + * @file MANNTest.cpp + * @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. + */ + +#include + +// Catch2 +#include + +#include +#include + +#include + +using namespace BipedalLocomotion::ML; +using namespace BipedalLocomotion::ParametersHandler; + +TEST_CASE("MANN") +{ + MANN mann; + + auto handler = std::make_shared(); + handler->setParameter("number_of_joints", 32); + handler->setParameter("projected_base_horizon", 12); + handler->setParameter("onnx_model_path", getMANNModelPath()); + + REQUIRE(mann.initialize(handler)); + + // set the input. The values are taken from a python test + MANNInput input; + input.basePositionTrajectory.resize(2, 12); + input.facingDirectionTrajectory.resize(2, 12); + input.baseVelocitiesTrajectory.resize(2, 12); + input.jointPositions.resize(32); + input.jointVelocities.resize(32); + + std::array basePositionTemp{0.020465626137500123, -0.028526278638462008, + 0.0204657739768999, -0.02852627656493037, + 0.020465822166082566, -0.028526210469724575, + 0.020465802925311055, -0.028526161526930927, + 0.020465772830464538, -0.02852614179540976, + 0.0, 0.0, + -0.009585547395551581, -0.02175600333393604, + -0.017817201285188853, -0.0332971138031881, + -0.02033611982326177, -0.030715595478538575, + -0.016440085949668892, -0.01989833515449035, + -0.008254458531998493, -0.008547476348220699, + 0.0, 0.0}; + + input.basePositionTrajectory = Eigen::Map(basePositionTemp.data(), 2, 12); + + std::array facingDirectionTemp{0.9998046173508225, 0.019766818762118485, + 0.9998046180476308, 0.019766783517618902, + 0.9998046178527036, 0.01976679337697099, + 0.9998046175664108, 0.01976680785767637, + 0.9998046173199947, 0.019766820321406378, + 1.0, 0.0, + 1.000195227589745, -0.0019179446719683577, + 1.0010723856647088, -0.002920366320472865, + 1.0019448535892073, -0.006219539528750589, + 1.00220806134242, -0.005959674409629769, + 1.0015735374358077, -0.0019812837541796284, + 1.0, 0.0}; + input.facingDirectionTrajectory + = Eigen::Map(facingDirectionTemp.data(), 2, 12); + + std::array baseVelocitiesTemp{0.02856285193696938, 0.019224769398957234, + 0.028562823489490678, 0.019225725173215375, + 0.02856269486540722, 0.01922613391891199, + 0.028562619171457148, 0.01922618174545223, + 0.02856256157025901, 0.019226074516058275, + -0.03709010698840217, -0.08405826644833612, + -0.03298855398555943, -0.09948148489499548, + -0.022790401966434587, -0.03630620503764005, + -0.006813425765586863, -0.005878922358647386, + 0.00036041265075565515, 0.011310742547698205, + 0.007901453422695888, 0.0018729868979242416, + 0.0, 1.3552527156068805e-20}; + input.baseVelocitiesTrajectory = Eigen::Map(baseVelocitiesTemp.data(), 2, 12); + + input.jointPositions << -0.08229444762971491, 0.1377980352398303, 0.014799786094367792, + -0.17823363484560167, -0.18445155789555484, -0.13905690533655496, -0.006777036784569264, + 0.14304762524836648, 0.031516238814005136, -0.1426330272419353, -0.06098283408307653, + -0.1341771055787397, 0.19911560020803554, 0.0328098631623236, 0.024781975683781033, + 0.01324623650231824, 0.03895569208112638, -0.5063181281080337, -0.15188637555678797, + -0.09926268052516171, -0.35648960192960166, 0.8564996250726968, 1.3064277172088623, + -0.05746928859305393, 0.16694140434265137, -0.1773331496644323, -0.09618105207864167, + -0.3637387390826281, -0.12775099875607665, 0.3775743246078491, -0.06556004402010714, + -0.9403902888298035; + + input.jointVelocities << 0.0982293093461868, 0.12588027548389802, 0.018234994616183622, + -0.26306079352524886, -0.17989701726927035, -0.13636938658177092, -0.018404592896727315, + -0.04892431424018461, 0.011816611105509, -0.030103574006318207, -0.054470913637429216, + 0.04110477832027632, 0.006149116530581617, 0.0022679536303376294, -0.01772658684486485, + 0.0018849445068382577, 0.0027670107680974943, 0.003149718526371251, 0.009372946811675064, + -0.019232750841138426, 0.00124201362405774, 0.0019440759412122286, -0.08404139429330826, + 2.529428034093097e-05, 0.16792771220207214, -0.02287937013013993, -0.002927608219628366, + 0.018047571904430328, -0.01034426546939903, -0.023162882775068283, 2.750370144352529e-05, + 0.0011318349279463291; + + REQUIRE(mann.setInput(input)); + REQUIRE(mann.advance()); + REQUIRE(mann.isOutputValid()); + + std::cerr << "base position" << std::endl; + std::cerr << mann.getOutput().futureBasePositionTrajectory << std::endl; + + std::cerr << "joint positions" << std::endl; + std::cerr << mann.getOutput().jointPositions.transpose() << std::endl; + + std::cerr << "joint velocities" << std::endl; + std::cerr << mann.getOutput().jointVelocities.transpose() << std::endl; +}