Add Momentum Based Torque Controller Code

.gitignore

@@ -0,0 +1,91 @@
+# Created by https://www.toptal.com/developers/gitignore/api/linux,c++,visualstudiocode,cmake
+# Edit at https://www.toptal.com/developers/gitignore?templates=linux,c++,visualstudiocode,cmake
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### CMake ###
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+_deps
+build/*
+
+### CMake Patch ###
+# External projects
+*-prefix/
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+.cache/*
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+
+### VisualStudioCode Patch ###
+# Ignore all local history of files
+.history
+.ionide
+
+# End of https://www.toptal.com/developers/gitignore/api/linux,c++,visualstudiocode,cmake

CMakeLists.txt

@@ -0,0 +1,104 @@
+# Copyright (C) 2013-2018 Fondazione Istituto Italiano di Tecnologia
+#
+# Licensed under either the GNU Lesser General Public License v3.0 :
+# https://www.gnu.org/licenses/lgpl-3.0.html
+# or the GNU Lesser General Public License v2.1 :
+# https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+# at your option.
+
+cmake_minimum_required(VERSION 3.16.0)
+
+project(MomentumBasedTorqueControl CXX)
+
+include(GNUInstallDirs)
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}")
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
+set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
+
+set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+
+if(MSVC)
+    set(CMAKE_DEBUG_POSTFIX "d")
+endif()
+
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+set(CMAKE_C_EXTENSIONS OFF)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
+
+option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON)
+
+# Enable RPATH support for installed binaries and libraries
+option(ENABLE_RPATH "Enable RPATH for this library" ON)
+mark_as_advanced(ENABLE_RPATH)
+include(AddInstallRPATHSupport)
+add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}"
+  LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}"
+  INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}"
+  DEPENDS ENABLE_RPATH
+  USE_LINK_PATH)
+
+# Encourage user to specify a build type (e.g. Release, Debug, etc.), otherwise set it to Release.
+if(NOT CMAKE_CONFIGURATION_TYPES)
+    if(NOT CMAKE_BUILD_TYPE)
+        message(STATUS "Setting build type to 'Release' as none was specified.")
+        set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release")
+    endif()
+endif()
+
+
+################################################################################
+########################## Mandatory dependencies ##############################
+
+find_package(iDynTree 12.3.1 REQUIRED)
+
+find_package(BipedalLocomotionFramework 0.19.0 REQUIRED)
+
+find_package(Eigen3 3.2.92 REQUIRED)
+
+find_package(OsqpEigen REQUIRED)
+
+find_package(QpSolversEigen REQUIRED)
+
+########################## Optional dependencies ##############################
+
+find_package(YARP QUIET)
+
+find_package(matioCpp QUIET)
+
+find_package(BipedalLocomotionFramework COMPONENTS VectorsCollection REQUIRED)
+
+set(CONTROLLER_HEADERS include/MomentumBasedTorqueControl/Controller.h include/MomentumBasedTorqueControl/QPproblem.h include/MomentumBasedTorqueControl/ReferenceGenerator.h)
+
+set(CONTROLLER_SOURCES src/Controller.cpp src/QPproblem.cpp src/ReferenceGenerator.cpp src/main.cpp)
+
+add_executable(${PROJECT_NAME} ${CONTROLLER_SOURCES} ${CONTROLLER_HEADERS})
+
+target_include_directories(${PROJECT_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+target_include_directories(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+
+target_link_libraries(${PROJECT_NAME} PRIVATE YARP::YARP_os
+    BipedalLocomotion::ParametersHandlerYarpImplementation
+    BipedalLocomotion::RobotInterfaceYarpImplementation
+    BipedalLocomotion::TextLogging
+    BipedalLocomotion::FloatingBaseEstimators
+    BipedalLocomotion::SystemYarpImplementation
+    BipedalLocomotion::ContinuousDynamicalSystem
+    BipedalLocomotion::YarpUtilities
+    BipedalLocomotion::System
+    iDynTree::idyntree-high-level
+    BipedalLocomotion::VectorsCollection
+    QpSolversEigen::QpSolversEigen)
+
+add_subdirectory(config)
+
+install(TARGETS ${PROJECT_NAME} DESTINATION bin)
+
+include(AddUninstallTarget)
+
+
+
+
+

LICENSE

@@ -0,0 +1,28 @@
+BSD 3-Clause License
+
+Copyright (c) 2024, Artificial and Mechanical Intelligence 
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -1 +1,103 @@
-# paper_sorrentino_ral2024_balancing_torque
+# paper_sorrentino_ral2024_balancing_torque
+
+<p align="center">
+  <img src="https://github.com/user-attachments/assets/65f5d3d6-583b-4c6f-9c37-e4b026beec6e" alt="paperVideoBalacingController">
+</p>
+
+## Installation
+
+For all the following steps, open a terminal and move to a directory where to install the software.
+All the following steps are meant to be executed sequentially in this terminal.
+
+
+Clone this repo:
+
+```
+git clone https://github.com/ami-iit/paper_sorrentino_ral2024_balancing_torque
+```
+
+Create a `conda` environment:
+
+```
+conda env create -f environment.yaml
+```
+
+Once created, activate the `conda` environment:
+
+```
+conda activate momentumbasedtorquecontrolenv
+```
+
+Clone the [`bipedal-locomotion-framework`](https://github.com/ami-iit/bipedal-locomotion-framework) and checkout the `restoreUFK` branch of [this fork](https://github.com/isorrentino/bipedal-locomotion-framework):
+
+```
+git clone https://github.com/ami-iit/bipedal-locomotion-framework
+cd bipedal-locomotion-framework
+git remote add fork https://github.com/isorrentino/bipedal-locomotion-framework
+git fetch fork
+git checkout restoreUKF
+```
+
+Configure the `bipedal-locomotion-framework`
+
+```
+cmake -S . -B build/ \
+  -DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX \
+  -DFRAMEWORK_COMPILE_PYTHON_BINDINGS:BOOL=OFF
+```
+
+Compile and install it:
+
+```
+cmake --build build/ --target install
+```
+
+It might take a while.
+
+Configure the `MomentumBasedTorqueController`:
+
+```
+cd ../paper_sorrentino_ral2024_balancing_torque
+cmake -S . -B build/ \
+  -DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX 
+```
+
+Compile and install it:
+
+```
+cmake --build build/ --target install
+```
+
+## Simulation
+
+Open a terminal and run:
+
+```
+conda activate momentumbasedtorquecontrolenv 
+export GZ_SIM_RESOURCE_PATH=<user_directory>/paper_sorrentino_ral2024_balancing_torque/config/robots/ergoCubGazeboV1_1/:$GZ_SIM_RESOURCE_PATH
+export YARP_ROBOT_NAME=ergoCubGazeboV1_1
+```
+
+replacing `<user_directory>` with yours.
+
+Moreover, consider to add this command to your `.bashrc` to avoid running it everytime.
+
+Then, run:
+
+```
+yarpserver --write
+```
+
+In another terminal, launch `gz-sim`:
+
+```
+cd ./paper_sorrentino_ral2024_balancing_torque/config/robots/ergoCubGazeboV1_1/world
+YARP_CLOCK=/clock gz sim world.sdf -r
+```
+
+In another terminal, run the controller:
+
+```
+cd ./paper_sorrentino_ral2024_balancing_torque/config/robots/ergoCubGazeboV1_1
+YARP_CLOCK=/clock MomentumBasedTorqueControl
+```